多传感器信息融合

多传感器信息融合0前言移动机器人的定位问题是提高移动机器人自主能力的关键问题之一。

具体来说，定位是利用先验环境地图信息、机器人位姿的当前估计及传感器的观测值等输入信息，经过一定的处理和变换，产生更加准确地对机器人当前位姿的估计。

机器人的定位方式有很多种，如，基于光电寻线的定位、基于声纳的机器人自主定位、基于全景视觉的定位及基于激光测距的定位等。

可以看出：机器人的定位方式取决于所采用的传感器。

目前，在移动机器人上使用较多的传感器有视觉传感器、里程计和惯导系统、超声传感器、激光测距仪、GPS 定位系统等。

其中，视觉传感器具有信息量大、感应时间短的优点，但往往获得的数据噪声大、信息处理时间长；激光传感器在测距范围和方向上具有较高的精度，但价格昂贵；超声波传感器虽然角度分辨力较低，但它处理信息简单、成本低、速度快，因此，在自主移动机器人上得到了广泛的应用；里程计是一种相对定位传感器，它通过累计计算得到定位信息，缺点是存在累计误差问题，因此，可结合绝对定位传感器，如超声传感器等，提供较准确的定位。

各传感器都有它自己的局限性，因此，移动机器人往往同时装备多种传感器，各自提供关于机器人定位的消息。

目前的趋势是：根据传感器的可靠性。

使用不同类型的传感器来测量相关数据。

本文采用扩展卡尔曼滤波( EKF) 技术，将里程计和超声波传感器所提供的数据进行融合定位。

1 机器人运动模型的建立由于移动机器人机构复杂，为了便于构造运动学模型与规划控制机器人的位姿，本文选择两轮驱动小车作为运动平台。

将整个机器人本体看作一个刚体，车轮视为刚性轮，并在运动不是太快而转弯半径较大时，不考虑车轮与地面侧向滑动的情况，其简化运动学模型如图1 所示。

图 1 两轮驱动机器人运动学模型为了确定机器人在平面中的位置，建立平面全局参考坐标系O X Y 和机器人局部参考坐标系O R X R Y R ， 把O R X R Y R 坐标系原点建立在 2个驱动轮轴心连线的中点0R 上，并将该点作为机器人的位置参考点。

相交于点OR 的 2个轴分别定义为XR 和YR 轴。

在 O X Y 坐标系下， O R 的位置由坐标x 和y 确定， 坐标系 O X Y 和O R X R Y R 之间的角度差(也是机器人线速度的方向)由θ给定。

可以将机器人的姿态描述为具有这3个元素的向量：[,,]T x y θ。

图1中，l ω，τω分别为左右两驱动轮的转动角速度，r 为两轮半径，b 为两驱动轮之间的距离，OR 点速度(也就是机器人的线速度) 为()/2l v r τωω=+， 分别投影到 O X Y 坐标系上得.cos cos ()/2l x v r τθθωω==*+ ，.sin sin ()/2l y v r τθθωω==*+。

移动机器人的角速度.()/r l r b θωω=-。

于是，机器人的运动方程为 ...cos cos 22sin sin 22r r x r r y r r b b θθθθθ⎛⎫⎛⎫ ⎪ ⎪ ⎪ ⎪ ⎪= ⎪ ⎪ ⎪ ⎪ ⎪ ⎪-⎝⎭ ⎪⎝⎭ （1）将方程(1)进行离散化，并加上模型噪声，可得机器人的离散随机状态空间表达式(1)(())()X k f X k w k +=+ （2）式中()[(),(),()]T X k x k y k k θ=；()w k 为模型误差，是零均值的高斯白噪声；()k θ为方差，T 为采样时间()cos ()/2(())()sin ()/2()cos ()/2l r l r r l x k Tr f X k y k Tr k Tr θωωθωωθθωω++⎛⎫ ⎪=++ ⎪ ⎪+-⎝⎭ （3）式(3)就是移动机器人的运动模型，也是系统的状态方程。

2 传感器观测模型的建立2．1 里程计里程计的工作原理是根据安装在2个驱动轮电机上的光电编码器来检测车轮在一定时间内转过的弧度，进而推算机器人相对位姿的变化。

设车轮半径为r ，光电码盘为P 线／转，t 时间内光码盘输出的脉冲数为N ，则该车轮移动距离s ∆为2*N s r p π∆= （4）假设由光电码盘检测出机器人左右轮的移动距离分别为l s ∆和r s ∆，且两轮的间距为b ，机器人从位姿()[(),(),()]T X k x k y k k θ=运动到(1)[(1),(1),(1)]T X k x k y k k θ+=+++。

则机器人移动的距离()/2l r s s s ∆=∆+∆， 机器人转过的角度()/l r s s b θ∆=∆-∆。

因此，在已知初始位置的情况下，即可求出前轮转过的距离，若采样时间取得足够短，通过计算出在时间内机器人位置的横、纵坐标和方向的变化量，进行累加， 可推出机器人在全局坐标中的位置坐标和方向角，从而获得自定位信息。

2．2 超声波传感器超声波传感器的基本原理是发送(超声)压力波包，一般为40-45kHz ，当波包遇到物体后，就会被反弹回，通过测量该波包反射和回到接收器所占用的时间，引起反射的物体距离 d 可以根据声音传播速度 C 和飞越时间 t 进行计算2ct d =（5）其模型可简化为在一个固定的波带开放角方位之内，传感器到莫一物体的最短距离。

其读数与机器人所在的环境和传感器的安装位置有关。

设第i 只超声波传感器在OR XR YR 坐标系中的坐标为(,)Ri Ri x y ，其方向（与R X 轴的夹角）为Ri θ。

假定在地k 个采样时刻，机器人的位置为()[(),(),()]T X k x k y k k θ=，则经过一个旋转平移的坐标变换，可将第i 只超声波传感器在O R X R Y R 坐标系中的坐标为(,)Ri Ri x y 转换到OXY 坐标系中的坐标((),())i i x k y k ，写成齐次坐标的形式为()sin ()cos ()()()cos ()sin ()()00111i Ri i Ri x k x k k x k y k k k y k y θθθθ⎛⎫⎛⎫⎛⎫ ⎪ ⎪ ⎪=- ⎪ ⎪ ⎪ ⎪ ⎪ ⎪⎝⎭⎝⎭⎝⎭ （6）同时，将超声波传感器的方向Ri θ转换为与OXY 坐标系的X 轴的夹角()i k θ ()()i Ri k k θθθ=+ （7）机器人移动所在的环境中的反射墙面与障碍等可用OXY 平面上的直线0j j j a x b y c ++=来表示（j=1,2,3，……为环境中的墙与障碍物得个数）。

超声波的测量原理如图2所示图 2 超 声波传感器的测量示意图设arctan(/)j j j a b α=，δ为超声波传感器的波带开放角。

根据超声波传感器的测量原理， 当第i 只超声波传感器和第j 个物体之间满足/2[()/2,()/2]j i i a k k πθσθσ-∈-+时，超声波传感器i 测得的到第j 个物体的距离为j i a x b y cd ++=传感器测出的数据超出了其测量范围，则超出范围的数据也要去除。

因此，在每个采样时刻，不是所有的超声波传感器的读数都被使用，而是结合环境地图来决定应该使用哪些读数。

可以得到该多传感器系统的随机观测模型为()(())()Z k h X k v k =+ （8）其中，()v k 为观测误差，是零均值的高斯白噪声，方差为R(k)。

()Z k 包括里程计的输出与超声波传感器的读数，所以，()Z k 的维数为里程计输 出向量个数加上超声传感器的只数。

里程计的输出为112233()()()()()()()()()z k x k v k z k y k v k z k k v k θ=+⎧⎪=+⎨⎪=+⎩ （9）超声波传感器的输出为33()()j i i i z k d v k ++=+ （10） 定位算法的结构如图3所示。

图3定位算法框图3 EKF 算法EKF 假设系统状态的概率分布是高斯分布，噪声是相互独立的。

式(2)与式(8)分别为机器人的运动模型和传感器模型。

1）初始化已冻结i 气人的状态向量0X ∧和误差协方差矩阵0P 。

2）预测机器人位置：根据机器人在k 时刻的位置预测时间k+1时刻的位置 (1/)((1/))X k k f X k ∧∧+= （11）(1/)()(1/)()()T P k k F k P k F k Q k +=+ （12） 其中，P 为X 的误差协方差矩阵，F 为f 的Jacobian 矩阵，可得3）观测 传感器的预测值为，用来对预测状态进行校正的量为实际的测量值与预测值之差，即信息为，信息的协方差矩阵为，H 为测量方程中(())h X k 的Jacobian 矩阵22221001000()()1sin(())cos(())ri j ri j j j j j H k h k a b x k y k a b a b θαθα∧∧⎛⎫ ⎪ ⎪ ⎪=∇= ⎪⎪+++⎪++⎭（14）式中ri x ，ri y 为第i 只超声波传感器的参数；j a ，j b 为外部环境中第j 个物体的参数；H （k ）的行数是不固定的，与各个采样时间实际所使用的超声波传感器的只数有关。

4）对于每一次测量，由于测量的不确定性，需要对每次测量进行校验来决定是否匹配。

若测量结果满足下式，则测量值匹配。

否则，不匹配并舍弃 12(1)(1)(1)T k S k k G γγ-+++≤ (15)式中2G 为正确性检验门。

5）纠正EKF 的增益矩阵W 更新为1(1)(1/)(1)T W k P k k H S k -+=++ （16）机器人的状态纠正为 (1/1)(1/)(1)(1)X k k X k k w k k γ∧∧++=++++ （17）协方差矩阵更新为 (1/1)((1)())(1/)P k k I W k H k P k k ++=-++ （18） 6）返回步骤（2）递归执行上述步骤（2—5）。

4 实验实验环境设为四周是垂直的墙壁，移动机器人逆时针沿着墙面做矩形轨线运动，用里程计和超声波传感器进行自定位。

图4曲线分别表示系统的观测真值 Z( k )，观测估计()Z k ∧，状态真值X( k )，状态先验估计(1/)X k k ∧+，状态后验估计(1/1)X k k ∧++。

图4系统观测向量与状态向量曲线图5为仅使用里程计的位置估计，图6为融合里程计和超声波传感器数据信息后的位置估计。

机器人的移动速度为0.25m ／s ， 采样时间T=lS ，估计的初始位置为（0,0），实际的初始位置为（0.15,0），P 0 =105I ， 模型噪声的方差取为0.1，测量噪声方差取为0.8。

图 5 实际位置与里程计定位信息图 6 实际位置与传感器融合定位信息从实验结果中可以看出：由于里程计的测量噪声不断地被累加，其估计过程中估量值与实际量之间在转角处偏差大，在运动不到2圈时，位姿估计信息就已经完全丢失，估计效果较差。