多传感器信息融合.docx

0前言
移动机器人的定位问题是提高移动机器人自主能力的关键问题之一。具体来说，定位是利用先验环境地图信息、机器人位姿的当前估计及传感器的观测值等输入信息，经过一定的处理和变换，产生更加准确地对机器人当前位姿的估计。机器人的定位方式有很多种，如，基于光电寻线的定位、基于声纳的机器人自主定位、基于全景视觉的定位及基于激光测距的定位等。可以看出：机器人的定位方式取决于所采用的传感器。目前，在移动机器人上使用较多的传感器有视觉传感器、里程计和惯导系统、超声传感器、激光测距仪、GPS定位系统等。其中，视觉传感器具有信息量大、感应时间短的优点，但往往获得的数据噪声大、信息处理时间长；激光传感器在测距范围和方向上具有较高的精度，但价格昂贵；超声波传感器虽然角度分辨力较低，但它处理信息简单、成本低、速度快，因此，在自主移动机器人上得到了广泛的应用；里程计是一种相对定位传感器，它通过累计计算得到定位信息，缺点是存在累计误差问题，因此，可结合绝对定位传感器，如超声传感器等，提供较准确的定位。各传感器都有它自己的局限性，因此，移动机器人往往同时装备多种传感器，各自提供关于机器人定位的消息。目前的趋势是：根据传感器的可靠性。使用不同类型的传感器来测量相关数据。本文采用扩展卡尔曼滤波(EKF)技术，将里程计和超声波传感器所提供的数据进行融合定位。

1机器人运动模型的建立
由于移动机器人机构复杂，为了便于构造运动学模型与规划控制机器人的位姿，本文选择两轮驱动小车作为运动平台。将整个机器人本体看作一个刚体，车轮视为刚性轮，并在运动不是太快而转弯半径较大时，不考虑车轮与地面侧向滑动的情况，其简化运动学模型如图1所示。

图1两轮驱动机器人运动学模型

为了确定机器人在平面中的位置，建立平面全局参考坐标系OXY和机器人局部参考坐标系ORXRYR，把ORXRYR坐标系原点建立在2个驱动轮轴心连线的中点0R上，并将该点作为机器人的位置参考点。相交于点OR的2个轴分别定义为XR和YR轴。在OXY坐标系下，OR的位置由坐标x和y确定，坐标系OXY和ORXRYR之间的角度差(也是机器人线速度的方向)由θ给定。可以将机器人的姿态描述为具有这3个元素的向量：。图1中，，分别为左右两驱动轮的转动角速度，r为两轮半径，b为两驱动轮之间的距离，OR点速度(也就是机器人的线速度)为，分别投影到OXY坐标系上得，。移动机器人的角速度。于是，机器人的运动方程为
（1）
将方程(1)进行离散化，并加上模型噪声，可得机器人的离散随机状态空间表达式 （2）
式中；为模型误差，是零均值的高斯白噪声；为方差，T为采样时间
（3）
式(3)就是移动机器人的运动模型，也是系统的状态方程。
2传感器观测模型的建立
2．1里程计
里程计的工作原理是根据安装在2个驱动轮电机上的光电编码器来检测车轮在一定时间内转过的弧度，进而推算机器人相对位姿的变化。设车轮半径为r，光电码盘为P线／转，t时间内光码盘输出的脉冲数为N，则该车轮移动距离为
（4）
假设由光电码盘检测出机器人左右轮的移动距离分别为和，且两轮的间距为b，机器人从位姿运动到。则机器人移动的距离，机器人转过的角度。
因此，在已知初始位置的情况下，即可求出前轮转过的距离，若采样时间取得足够短，通过计算出在时间内机器人位置的横、纵坐标和方向的变化量，进行累加，可推出机器人在全局坐标中的位置坐标和方向角，从而获得自定位信息。
2．2超声波传感器
超声波传感器的基本原理是发送(超声)压力波包，一般为40-45kHz，当波包遇到物体后，就会被反弹回，通过测量该波包反射和回到接收器所占用的时间，引起反射的物体距离d可以根据声音传播速度C和飞越时间t进行计算
（5）
其模型可简化为在一个固定的波带开放角方位之内，传感器到莫一物体的最短距离。其读数与机器人所在的环境和传感器的安装位置有关。
设第i只超声波传感器在ORXRYR坐标系中的坐标为，其方向（与轴的夹角）为。假定在地k个采样时刻，机器人的位置为，则经过一个旋转平移的坐标变换，可将第i只超声波传感器在ORXRYR坐标系中的坐标为转换到OXY坐标系中的坐标，写成齐次坐标的形式为
（6）
同时，将超声波传感器的方向转换为与OXY坐标系的X轴的夹角
（7）
机器人移动所在的环境中的反射墙面与障碍等可用OXY平面上的直线来表示（j=1,2,3，……为环境中的墙与障碍物得个数）。超声波的测量原理如图2所示

图2超声波传感器的测量示意图
设，为超声波传感器的波带开放角。根据超声波传感器的测量原理，当第i只超声波传感器和第j个物体之间满足时，超声波传感器i测得的到第j个物体的距离为；当不满足上述关系时