多传感器信息融合

吉林大学吉林大学#离为的测量原理，当第i只超声波传感器和第J个物体之间满足兀/2-®引q伙）-b/2,G伙）+b/2］时，超声波传感器i测得的到第J个物体的距离为:当不满足上述关系时，得到的数据要去除。如果超声波传感器测出的数据超出了其测量范围，则超出范围的数据也要去除。因此，在每个采样时刻，不是所有的超声波传感器的读数都被使用，而是结合环境地图来决定应该使用哪些读数。(8)可以得到该多传感器系统的随机观测模型为(8)Z(k)=h(X(k))+v(k)其中，卩伙）为观测误差，是零均值的高斯白噪声，方差为R（k）。Z伙）包括里程计的输出与超声波传感器的读数，所以，Z伙）的维数为里程计输出向量个数加上超声传感器的只数。里程计的输出为2伙）=/伙）+岭伙）•。伙）伙）+“2伙） （9）①伙）=&伙）+冬伙）超声波传感器的输出为S伙）=〃/+也伙） （10）定位算法的结构如图3所示。图3定位算法框图3EKF算法EKF假设系统状态的概率分布是高斯分布，噪声是相互独立的。式(2)与式(8)分别为机器人的运动模型和传感器模型。初始化已冻结i气人的状态向量X。和误差协方差矩阵仇。预测机器人位置：根据机器人在k时刻的位置预测时间k+1时刻的位置左伙+1/灯=/(X(l/k)) (11)P伙+1/k)=F伙)P(1/k)F伙)"+Q{k) (12)其中，P为X的误差协方差矩阵，F为f的Jacobian矩阵，可得3）观测传感器的预测值为z（^+i）=ft（x（^i/fc））,用来对预测状态进行校正的量为实际的测量值与预测值之差，即信息为曲+1）曲（21）-旅+1）,信息的协方差矩阵为S（k+1）=H（A）PM+1/約H（*）T+RM）,H为测量方程中/7（X伙））的Jacobian矩阵TOC\o"1-5"\h\zP ° 0 〕° 1 0H伙）=▽〃（《）=0 0 j〜 5 人 AO+/?2 亦心sin（&伙）+勺）+儿cos（&伙）+勺）（14）式中心，儿为第i只超声波传感器的参数；勺，◎为外部环境中第J个物体的参数；H（k）的行数是不固定的，与各个采样时间实际所使用的超声波传感器的只数有关。4）对于每一次测量，由于测量的不确定性，需要对每次测量进行校验来决定是否匹配。若测量结果满足下式，则测量值匹配。否则，不匹配并舍弃y（k+1）S伙+\yxY（k+1）7<G2 （15）式中G，为正确性检验门。5）纠正EKF的增益矩阵W更新为W伙+1)=P伙+1/k)HTS~｛伙+1)(16)机器人的状态纠正为左伙+\/k+\)=X伙+1/£)+w(k+1)了伙+1)(17)协方差矩阵更新为P仇+1/k+1)=(/—W(k+l)H(k))P(k+\/k)(18)6）返回步骤（2）递归执行上述步骤（2—5）。

4实验实验环境设为四周是垂直的墙壁，移动机器人逆时针沿着墙面做矩形轨线运动，用里程计和超声波传感器进行自定位。图4曲线分别表示系统的观测真值z（k）,观测估计2伙），状态真值x（k）,状态先验估计攵伙+1/幻，状态后验估计丘伙+1/£+1）。20 40 60 80 1000 20 40 60 80 100时间/s观测貞•实20 40 60 80 1000 20 40 60 80 100时间/s观测貞•实（X）估计（0）状态貞实（八）先峻估计（:）膚验估计（・）图4系统观测向量与状态向量曲线图5为仅使用里程计的位置估计，图6为融合里程计和超声波传感器数据信息后的位置估计。机器人的移动速度为0.25m/s,采样时间T=1S,估计的初始位置为（0,0）,实际的初始位置为（0.15,0）,P0=1051,模型噪声的方差取为0・1,测量噪声方差取为0.8。60000042oo-r/cO40

60000042oo-r/cO40图5实际位置与里程计定位信息600400£虽20000 200 400 600rt/cm图6实际位置与传感器融合定位信息从实验结果中可以看出：由于里程计的测量噪声不断地被累加，其估计过程中估量值与实际量之间在转角处偏差大，在运动不到2圈时，位姿估计信息就已经完全丢失，估计效果较差。釆用里程计和超声波传感器融合后的定位算法,基本消除了里程计的累计误差，提髙了定位的精度。5结论采用多传感器信息融合算法将里程计和超声波传感器采集的信息融合后进行机器人的自定位，该方法基于EKF算法，通过对机器人和传感器的建模，在路径规划相对简单、轨迹相对较短的情况下，可以取得满意的效果。而实际中，特别是在机器人行走轨迹复杂或者环境条件动态未知的情况下，关于噪声的先验