多传感器信息融合在四足机器人避障中的应用*

张 天，杨晨曦，朱 颖，宋明辉，张 地

(1.北京理工大学 光电学院，北京100081;2.北京理工大学 信息与电子学院，北京100081;3.北京理工大学 机械与车辆学院，北京100081;4 郑州大学 国际学院，河南 郑州450001)

0 引 言

多传感器信息融合技术综合了概率统计、信号处理、人工智能、控制理论等多个学科的最新科研成果，为机器人精确、全面、实时地感知各种复杂的、动态的、不确定的未知环境提供了一种先进的技术手段。在研究基于多传感器融合的移动机器人避障技术的相关文献中，为获取机器人本体与障碍物的距离信息，经常使用的传感器有超声测距传感器、红外测距传感器、里程计、GPS、激光传感器等，文献［1］中使用里程计和激光传感器进行信息融合;文献［2］中使用激光雷达和超声测距传感器进行信息融合;文献［3］中使用超声测距传感器和红外测距传感器进行信息融合，这些传感器均为测距类传感器，传感器之间的冗余信息量大，互补信息量少［4］，在使用的过程中必须提供先验知识，对于动态的、复杂的应用场景，其效果并不理想。文献［5，6］中对单目视觉传感器和超声测距传感器进行了信息融合，传感器间的互补信息量变大，融合结果提高了系统的鲁棒性，但单目视觉只有在特定的环境下才能得到距离信息，依然不能满足动态的应用场景。

双目视觉传感器对动态环境适应性强，超声测距传感器的测量精度高，为满足动态的应用场景，本文将进行这两种传感器的融合研究，需要指出，由于干扰信号的存在，在进行融合之前，先使用卡尔曼滤波算法对两种传感器获取的距离信息进行滤波处理。

1 卡尔曼滤波算法与STF 融合算法

由于受杂波等干扰信号的影响，传感器获取的距离信息具有统计信号［7］的特征，为保证测量精度，需要进行参数估计，根据具体的情况，解决参数估计问题的常用方法有卡尔曼滤波、α－β 滤波、α－β－γ 滤波等。卡尔曼滤波算法主要有两条主线，一条是基于自协方差矩阵的运算，另一条是基于滤波值和预测值的运算，两者通过增益矩阵联系起来。卡尔曼滤波算法的公式推导参见文献［8］，这里不再赘述。

多传感器信息融合方法大致可以分为三类，即，概率统计方法、逻辑推理方法和学习方法［9］。文献［5，10，11］分别使用模糊推理、D－S 证据理论和产生式规则的方法进行信息融合，这些方法都属于逻辑推理的范畴;文献［3，4，6］使用神经网络的方法进行信息融合，该方法属于学习方法的范畴，文献中依据这些融合算法，均达到了预期效果。本文中使用的STF 融合算法则是概率统计方法的一种。

假设仿生四足机器人上的双目视觉传感器和超声测距传感器获取的状态向量的估计值分别为和，协方差矩阵分别为P1和P2，互协方差矩阵。当P12=时，为了得到状态向量和协方差矩阵的最佳估计值，可以使用STF 融合算法。

系统状态向量和协方差矩阵的最佳估计分别为

2 匀速直线运动模型

匀速直线运动(constant velocity，CV)模型［12］。CV 直线运动模型的一般描述为:目标做CV 直线运动，位移为x(t)，速度为˙x(t)，加速度a=¨x(t)=0。实际情况中，速度在有随机扰动的情况下会发生轻微变化，假设这个随机扰动是均值为零的高斯白噪声。在此条件下，经离散处理后，卡尔曼滤波的基本公式可表示如下

其中

式中 T 为采样周期，σw为过程噪声的标准差，σv为量测噪声的标准差。

3 参数确定

基于双目视觉传感器和超声测距传感器，在CV 模型下应用卡尔曼滤波算法，可以得到两组状态向量的估计值和，以及相应的协方差矩阵P1和P2，由于以上两组数据来自两个不同的传感器系统，故满足这一条件，可以使用STF 融合算法得到整个系统的状态向量和协方差矩阵的最佳估计值^X 和P。为此，需要确定以下参数，系统的过程噪声标准差σw，双目视觉传感器的量测噪声标准差σv1，超声测距传感器的量测噪声标准差σv2和卡尔曼滤波算法的初始值。下面结合仿生四足机器人的实际情况，确定以上参数。

3.1 确定系统的过程噪声标准差

由于在机械结构设计和装配过程中产生的误差，使得仿生四足机器人在Walk 步态下行走时，并不是理论上以0.4 m/s的速度做匀速直线运动，而是在做变速直线运动，系统的过程噪声标准差是机器人在Walk 步态下行走时的加速度值。下面介绍获取该加速度值的方法。

在Adams 仿真软件中，建立仿生四足机器人的运动学模型，如图1 所示。在仿生四足机器人机体的质心处建立一个前进方向的加速度测量，运行仿真，打开Adams 仿真软件的后处理器，对获得的加速度曲线进行巴特沃斯滤波，然后计算加速度的平均值，将其作为该次仿真的加速度值。重复进行50 次，得到50 个加速度值，求出标准差，即为系统的过程噪声标准差。图2 为某次仿真中的加速度曲线。最终求出系统的过程噪声标准差为0.012 m/s2，即σw=0.012 m/s2。

图1 Adams 中机器人的运动学模型Fig 1 Kinetic model of robot in Adams

图2 加速度曲线Fig 2 Curve of acceleration

3.2 确定传感器的量测噪声标准差

对于传感器的量测噪声标准差，在仿真情况下，可以由其测量误差来反映。

在实际应用中，利用两个CCD 摄像机获取视差信息，再根据三角测量原理恢复出场景的深度信息，如此即可测量出障碍物与机器人之间的距离信息，然而，由于CCD 摄像机所拍摄的图像是以像元大小为单位的一组离散的数据，故在用双目视觉进行测量时存在最小分辨率误差，仿生四足机器人上搭载的双目视觉传感器的测量误差约为6.8 cm，即σv1=0.068 m。

超声测距传感器的发射头发出超声波信号，此信号被障碍物反射后，由接收头接收，根据发射和接收到信号的时间差和声速，即可得到障碍物的距离信息。当探测范围内有目标物体之外的物体存在时，会产生测量误差。仿生四足机器人上搭载的超声测距传感器的测量误差为1 cm，即σv2=0.01 m。

3.3 确定卡尔曼滤波算法的初始值

卡尔曼滤波算法作为一个迭代过程，需要赋予其初值，初值的选择至关重要，如果初值选择不合适，就不能满足收敛性的要求。在CV 模型中，P(0|0)的确定方法已经由模型给出，这里只需给出X(0|0)的取值，本文中取X(0|0)=［10，－0.4］＇。

4 仿真实验与结果分析

在完成上述准备工作后，笔者在Matlab 软件中进行仿真实验，仿真实验流程如图3 所示。

图3 仿真实验流程图Fig 3 Flow chart of simulation experiment

根据仿真实验流程图，在Matlab 中先对模拟出的目标位置信息进行卡尔曼滤波处理，如图4 和图5 所示，这里的目标指的是所测障碍物。首先，从图4 和图5 可以看出:经卡尔曼滤波处理后的目标位置的估计值在前2 s 偏离真实值较远，从第4 s 以后，无论观测值如何波动，估计值曲线均能很好地跟踪真实值曲线，说明卡尔曼滤波算法起到了良好的滤波效果。

将融合处理前后，目标位置的估计值曲线和目标位置估计值的方差曲线分别置于同一幅图中，如图6 所示，通过对比反映STF 融合算法的优点。从图6(b)中可以看出:融合处理后，目标位置估计值的方差变小，说明融合处理后对目标位置的估计更加准确。从图6 中可以发现，融合曲线介于双目视觉传感器的估计值曲线和超声测距传感器的估计值曲线之间，且更加靠近准确度高的超声测距传感器的估计值曲线。

在本文所引文献中，实验验证环节均在具体的应用场景下进行，实验结果是移动机器人能够进行无碍行走，文中均未给出具体的测量精度。本文仿真实验的结果表明:融合处理后，测量精度可达4.6 cm，满足了仿生四足机器人对测距的精度要求。

图4 双目视觉传感器系统的卡尔曼滤波Fig 4 Kalman filtering of binocular sensor

图5 超声测距传感器系统的卡尔曼滤波Fig 5 Kalman filtering of ultrasonic ranging sensor system

5 结 论

为提高仿生四足机器人在复杂、动态环境下对障碍物位置信息的感知能力，本文针对仿生四足机器人在结构化路面上以Walk 步态行走的情况，将双目视觉传感器和超声测距传感器获取的障碍物距离信息进行融合研究。仿真实验结果表明:滤波后的距离信息的估计值曲线很好地跟踪了真实值曲线，说明卡尔曼滤波算法发挥了出色的滤波作用;与融合前相比，融合处理后的距离信息的估计值的方差明显减小，说明融合处理后获得的障碍物的位置信息更加准确，且测量精度为4.6 cm，满足了机器人的应用要求。

图6 融合前后目标位置估计值曲线和方差曲线的对比Fig 6 Comparison of curve of estimation value of target position before and after fusion and covariance curve

［1］ Bao Bizhen，Yang Ping，Cao Huwen.A method for modeling and simulation of differential drive mobile robot localization based on EKF multisensor fusion［J］.Advanced Materials Research，2011，383－390:2339－2345.

［2］ 周 霞，何建忠，王宝磊.多传感器信息融合伺服的移动机器人快速跟随［J］.计算机工程与设计，2013，34(6):2247－2251.

［3］ 姜志兵.模糊神经网络信息融合在移动机器人导航中的应用［J］.机床与液压，2009，37(7):154－156.

［4］ 王耀南，李树涛.多传感器信息融合及其应用综述［J］.控制与决策，2001，16(5):518－522.

［5］ Nouara Dechemi.Multisensor data fusion based on fuzzy logic［C］∥International Conference on Automation，Robotics and Control System，Orlando，FL，USA，2010:121－127.

［6］ 丁承君，张明路，段 萍.模糊神经网络在移动机器人信息融合中的应用［J］.控制理论与应用，2004，21(1):59－62.

［7］ Steven M.Kay fundamentals of statistical signal processing［M］.Beijing:Publishing House of Electronics Industry，2003:14－20.

［8］ 何 友，王国宏，陆大金，等.多传感器信息融合及应用［M］.北京:电子工业出版社，2007:13－15.

［9］ 王 军，苏剑波，席裕庚.多传感器融合综述［J］.数据采集与处理，2004，19(1):72－77.

［10］余 华，吴文全，刘 忠.D－S 证据理论在移动机器人信息融合中的应用［J］.计算机与数字工程，2009，37(11):125－129.

［11］许 芬，咸宝金，李正熙.基于产生式规则多传感器数据融合方法的移动机器人避障［J］.电子测量与仪器学报，2009，23(10):73－79.

［12］姚惠元.TWS 雷达目标航迹跟踪及预测算法研究［D］.哈尔滨:哈尔滨工业大学，2011:6－8.