多传感器信息融合

多传感器信息融合

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 α=，δ为超声波传感器的波带开放角。根据超声波传感器