多传感器信息融合

#式中X(k)二[x(k),y(k),6(k)]T；w(k)为模型误差，是零均值的高斯白噪声;0(k)为方差，T为采样时间3)TOC\o"1-5"\h\z'x(k)+Trcos0(①)/2'3)lrf(X(k))=y(k)+Trsin0f(X(k))=lr、0(k)+Trcos0(①一①)/2丿rl式(3)就是移动机器人的运动模型，也是系统的状态方程。2传感器观测模型的建立2．1里程计里程计的工作原理是根据安装在2个驱动轮电机上的光电编码器来检测车轮在一定时间内转过的弧度，进而推算机器人相对位姿的变化。设车轮半径为r,光电码盘为P线/转，t时间内光码盘输出的脉冲数为N,则该车轮移动距离止为4)假设由光电码盘检测出机器人左右轮的移动距离分别为As和As，且两轮lr的间距为b，机器人从位姿X(k)=[x(k),y(k),0(k)]T运动到X(k+1)=[x(k+1),y(k+1),0(k+1)]t。则机器人移动的距离As=(As+As)/2,机lr器人转过的角度A0=(As-As)/b。lr因此，在已知初始位置的情况下，即可求出前轮转过的距离，若采样时间取得足够短，通过计算出在时间内机器人位置的横、纵坐标和方向的变化量，进行累加，可推出机器人在全局坐标中的位置坐标和方向角，从而获得自定位信息。2．2超声波传感器超声波传感器的基本原理是发送(超声)压力波包，一般为40-45kHz,当波包遇到物体后，就会被反弹回，通过测量该波包反射和回到接收器所占用的时间，引起反射的物体距离d可以根据声音传播速度C和飞越时间t进行计算d=ct(5)

其模型可简化为在一个固定的波带开放角方位之内，传感器到莫一物体的最短距离。其读数与机器人所在的环境和传感器的安装位置有关。设第i只超声波传感器在ORXRYR坐标系中的坐标为(x,y)，其方向(与RiRiX轴的夹角)为0。假定在地k个采样时刻，机器人的位置为RRiX(k)二[x(k),y(k),0(k)]t，则经过一个旋转平移的坐标变换，可将第i只超声波传感器在ORXrYr坐标系中的坐标为(x,y)转换到OXY坐标系中的坐标RiRi(x(k),y(k))，写成齐次坐标的形式为ii'x(k)iy(k)i'x(k)iy(k)iI1'sin0(k)-cos0(k)<0cos0(k)sin0(k)0(x)Riy(k)yRi11丿6)同时，将超声波传感器的方向0转换为与OXY坐标系的X轴的夹角0(k)Rii0(k)=0(0(k)=0(k)+0iRi机器人移动所在的环境中的反射墙面与障碍等可用OXY平面上的直线ax+by+c二0来表示(j=1,23……为环境中的墙与障碍物得个数)。超声波jjj的测量原理如图2所示图2超声波传感器的测量示意图设«=arctan(a/b)，§为超声波传感器的波带开放角。根据超声波传感器jjj

的测量原理，当第i只超声波传感器和第j个物体之间满足兀/2-ag[0(k)-a/2,0(k)+a/2]时，超声波传感器i测得的到第j个物体的距jii离为dj=j*空*°;当不满足上述关系时，得到的数据要去除。如果超声波1Ja2+b2传感器测出的数据超出了其测量范围，则超出范围的数据也要去除。因此，在每个采样时刻，不是所有的超声波传感器的读数都被使用，而是结合环境地图来决定应该使用哪些读数。可以得到该多传感器系统的随机观测模型为(8)Z(k)=h(X(k))+v(k)其中，v(k)为观测误差，是零均值的高斯白噪声，方差为R(k)。Z(k)包括里程计的输出与超声波传感器的读数，所以，Z(k)的维数为里程计输出向量个数加上超声传感器的只数。里程计的输出为9)10)”z(k)=x(k)+v(k)9)10)11<z(k)=y(k)+v(k)22z(k)=0(k)+v(k)33超声波传感器的输出为z(k)=dj*v(k)3*ii3*i定位算法的结构如图3所示。图3定位算法框图3EKF算法EKF假设系统状态的概率分布是高斯分布，噪声是相互独立的。式(2)与式(8)分别为机器人的运动模型和传感器模型。初始化已冻结i气人的状态向量X和误差协方差矩阵p。0011)12)预测机器人位置：根据机器人在k时刻的位置预测时间11)12)X(k+1/k)=f(X(f/k))P(k+1/k)二F(k)P(1/k)F(k)t+Q(k)其中，P为X的误差协方差矩阵，F为f的Jacobian矩阵，可得

观测传感器的预测值为施+1；=hNIL5:.:.，用来对预测状态进行校正的量为实际的测量值与预测值之差，即信息为丁*」-必飞7丈11：,信息的协方差矩阵为X-〃*；，h为测量方程中h(X(k))的Jacobian矩阵H(k)=VH(k)=Vh(k)=xsin©(k)+a)+yrijricos©(k)+a)j丿14)式中x，y为第i只超声波传感器的参数；a，b为外部环境中第j个物ririjj体的参数；H(k)的行数是不固定的，与各个采样时间实际所使用的超声波传感器的只数有关。对于每一次测量，由于测量的不确定性，需要对每次测量进行校验来决定是否匹配。若测量结果满足下式，则测量值匹配。否则，不匹配并舍弃Y(k+1)S(k+1)-1Y(k+1)t<G2(15)式中G2为正确性检验门。纠正16)17)EKF的增益矩阵16)17)W(k+1)二P(k+1/k)HtS-1(k+1)机器人的状态纠正为X(k+1/k+1)=X(k+1/k)+w(k+1)Y(k+1)协方差矩阵更新为P(k+1/k+1)=(I-W(k+1)H(k))P(k+1/k)返回步骤(2)递归执行上述步骤(2—5)。

4实验实验环境设为四周是垂直的墙壁，移动机器人逆时针沿着墙面做矩形轨线运动，用里程计和超声波传感器进行自定位。图4曲线分别表示系统的观测真值Z(k)，观测估计Z(k)，状态真值X(k)，状态先验估计X(k+1/k)，状态后验估计X(k+1/k+1)。煽测農实(©估计⑷狀态貞实(--)先验估计⑴肩姻折to图4系统观测向量与状态向量曲线图5为仅使用里程计的位置估计，图6为融合里程计和超声波传感器数据信息后的位置估计。机器人的移动速度为0.25m/s,采样时间T=lS,估计的初始位置为(0,0)，实际的初始位置为(0.15,0)，P0=105I，模型噪声的方差取为0.1，测量噪声方差取为0.8。0200400600Jr/Cm从实验结果中可以看出：由于里程计的测量噪声不断地被累加，其估计过程中估量值与实际量之间在转角处偏差大，在运动不到2圈时，位姿估计信息就已经完全丢失，估计效果较差。采用里程计和超声波传感器融合后的定位算法，基本消除了里程计的累计误差，提高了定位的精度。5结论采用多传感器信息融合算法将里程计和超声波传感器采集的信息融合后进行机器人的自定位，该方法基于EKF算法，通过对机器人和传感器的建模，在路径规划相对简单、轨迹相对较短的情况下，可以取