张江桥，范平清，陈 勇

(上海工程技术大学 机械与汽车工程学院，上海 201620)

随着移动机器人相关技术的研究发展，自动导引运输车（Automated Guided Vehicle，AGV）在各个领域的应用也越来越广泛，移动机器人的定位、避障、导航成为目前研究的热点，其中定位技术是根据机器人自身感知周围环境以及执行动作获得相对位置.AGV 在感知外部环境的过程中，由于环境的动态变化会产生一些噪音，传感器的精度差异会造成定位上的偏差，这就需要合适的滤波融合模型获得机器人的精确位姿.

目前移动机器人主要的定位方式分为相对定位和绝对定位两种.相对定位使用的是里程计航位推算法[1]和陀螺仪的惯性导航法.但由于相对定位的误差会随着距离的增大而增大，所以需要引入绝对定位纠偏，而绝对定位主要用概率定位，如马尔科夫定位和卡尔曼滤波定位等.本文采用绝对定位和相对定位相结合的方式进行定位.Li 等[2]提出利用粒子滤波器实现机器人定位，并通过在估计过程中调整样本集大小的方式提高粒子滤波器的效率.尚文等[3]将马尔科夫（Markov）方法和扩展卡尔曼（Extended Kalman Filter，EKF）方法组合应用，将声纳传感器和视觉传感器信息融合，提高了定位的精度.但EKF 会造成系统线性化误差且计算繁琐，从而造成冗余传感器的数据对定位精度的影响.

高端阳等[4]提出了改进的鲁棒无迹卡尔曼滤波法(Unscented Kalman Filter，UKF)进行室内定位，该方法避免了EKF 中计算雅克比矩阵的繁琐步骤，简化了算法.Yao 等[5]提出了将图像、里程计和惯性传感器（InertialMeasurement Unit，IMU）3 种传感器融合的方法，将里程计和陀螺仪的预积分和图像的信息对齐，再进行局部和全局的优化.Li 等[6]改进了ICP 算法，提出了一种基于分段的扫描匹配框架，用于六自由度姿态估计和映射.结合6D 图优化算法，采用基于图像的地面点提取方法滤除噪声和地面点.罗荣华等[7]提出利用Hough 变换将图像与声呐传感器进行特征级别的融合.但上述方法只适用图像与其他传感器的融合，且图像需要预处理，步骤较为复杂，从而影响算法的实时性.

由于图像传感器的实时性欠佳，唐恒博等[8]设计了一种里程计融合定位针路标的轨道机器人全局定位系统，但定位针的安放并不适用于室内情况.柳长安等[9]提出了将惯性测量单元与激光雷达扫描匹配融合的算法，并通过置信度动态选择机器人位姿.但是该算法结构复杂，运算强度大，实时性不高.文献[10-11]分别提出了优化箱粒子滤波算法和最近邻匹配与粒子滤波协同定位算法以获得更精确的定位，但是粒子滤波法求解系统后验概率的好坏程度取决于样本的数量，在样本数量少时定位精度较差.

基于上述研究基础，本文结合卡尔曼滤波模型，提出了一种基于主观贝叶斯网络的多传感器数据融合方法.文中将激光雷达与IMU 和里程计相融合，计算信息增益大小，并根据信息增益自主地选择不同传感器进行融合.该方法具有定位精度高，鲁棒性好，计算速度快的优点.

1 主观贝叶斯网络融合模型

主观贝叶斯首先在不确定状态下，对部分未知的状态用主观概率估计，然后用贝叶斯公式对发生概率进行修正，最后利用期望值和修正概率做出最优决策.选择主观贝叶斯网络的方式将传感器定位数据融合，是因为在室内情况下，EKF 需要对系统的非线性方程进行线性化处理，会造成系统的线性化误差，算法较为繁琐.贝叶斯的主动融合可以避开EKF 中雅克比矩阵的计算，且对相对较多的传感器融合时，算法更为简单，实时性更强.动态贝叶斯网络（Dynamic Bayesian Network，DBN）发现了所有变量之间的依赖关系，可以对事件的不确定性进行建模，并处理定位过程中不确定的位置信息.不同于隐马尔可夫模型[12]（Hidden Markov Model，HMM）中位置信息节点只与单个传感器节点有关，贝叶斯网络可以融合多传感器节点，从而获取多传感器定位数据，得到更精确的AGV 位姿信息.

本文结合卡尔曼滤波模型，首先读取AGV 上所搭载的激光雷达、里程计、IMU 传感器数据，并对数据进行降噪，从而得到更为稳定的多传感器数据.然后通过主观贝叶斯网络融合模型进行传感器数据融合，再判断融合后的结果是否合理，若合理，则输出数据；反之，则重新选择传感器进行融合.最后根据融合后的传感器信息进行AGV 位置状态更新，以获得更精确的AGV 位姿信息.上述定位流程如图1 所示.

图1 融合贝叶斯定位流程Fig.1 Positioning process of fusion Bayesian

文中贝叶斯融合定位过程通过3 种不同的传感器来测量环境，在t时刻观测的系统状态记为Ot.首先根据系统状态Ot计算传感器的信息增益，选择增益大的传感器并获取所选传感器的数据，从而确定观测系统t+1 时刻的状态Ot+1；再将传感器数据按照信息增益的比重进行融合；最后利用动态贝叶斯推理算法计算后验概率，以验证方案的合理性.

综上所述，构建的动态贝叶斯融合模型如图2所示[13].

图2 动态贝叶斯网络模型Fig.2 Dynamic Bayesian network model

图2 中，传感器S1、S2、S3对应激光雷达、里程计和IMU.在AGV 定位过程中，由于传感器噪声等原因，造成传感器数据的不确定性.为了处理这种不确定性，本文在传感器和中间变量之间增加了信息变量.传感器与信息变量之间的条件概率能够量化传感器测量的不确定性.因此，通过信息变量大小选择传感器数据，再由DBN 推理进行融合，完成AGV 的精确定位.

2 动态传感器数据融合

基于上述定位过程和主观动态贝叶斯网络融合模型的建立，计算卡尔曼滤波和信息增益，根据得到的信息增益进行传感器定位数据融合，从而得到更精确的AGV 位姿信息，实现AGV 的精确定位.

2.1 卡尔曼滤波为了降低传感器噪声对AGV定位精度的影响，本文将卡尔曼预测与传感器测量数据结合以减小传感器定位数据误差，根据k-1 时刻的AGV 位置预测值，利用状态转换矩阵计算k时刻的位置状态值，从而获得下一时刻的机器人位姿信息.k时刻传感器数据的均值和方差分别用xk/k,pk/k表示，状态的一步预测方程[14]：

式中：得到k时刻的状态为(k|k-1)，A为状态转换矩阵，B为控制输入矩阵,u(k) 为控制输入.

式中：Q为过程噪声的协方差.

为了获得AGV 位置状态更新的增益值，计算预测误差的方差Sk：

式中：R为测量噪声的协方差，H为转换矩阵.

得到误测误差的方差后，即可计算得卡尔曼增益值K(k)：

为使下一时刻的融合精度更高，误差在可控范围内，计算预测新息ek：

式中：z(k)为k时刻的测量值.

利用预测信息和卡尔曼增益更新预测方程式(1)以及预测误差式(2)并进行不断迭代，获得新的k时刻AGV 位置状态信息x(k|k)和协方差p(k|k)，从而获得更准确的传感器数据：

2.2 信息增益传感器数据滤波完成之后，本文引入信息增益[15]I(X,Y)衡量传感器数据的不确定性，选择确定性较高的传感器进行融合以获取更为精确的AGV 位姿信息.信息增益的计算如下所示：

式中：I(X,Y)即为所求信息增益.

对任何信息增益的定量分析都需要考虑传感器的激活成本，所以本文加入效用函数.效用函数由信息增益u1和激活传感器的成本C(S)两个部分组成.用u2=1-C(S)将其转化为成节约的成本，则新加入的效用函数为：

式中：h1、h2为自定义权重参数，h1+h2=1.

2.3 贝叶斯融合完成卡尔曼滤波及信息增益的计算后，将滤波后的传感器数据按照增益比重进行融合.首先，当k=0 时刻，求得传感器集合S：

式中：当U(I(x,y),C(s))函数取得最大值时，求得对应的传感器集合并获取传感器的数据xi，然后按信息增益比例融合多传感器的数据，融合公式为：

而后通过最大后验概率p(y|xi)的大小判断融合的数据是否合理，贝叶斯后验概率[16]表达式为：

若融合数据不合理，则执行公式(12)重新选择传感器，融合所选传感器数据直到后验概率最大；反之，输出传感器数据，从而获得机器人的位姿信息，实现AGV 的精确定位.

3 移动机器人定位实验

为验证上述贝叶斯多传感器融合算法的效果，搭建实验平台与实验环境进行验证.

3.1 实验平台与实验环境实验平台为自行研制的移动机器人，移动机器人的控制器为自主研制嵌入式控制器.在运动学上，机器人采用伺服电机进行左右双驱，差速控制.该移动机器人搭载1 个16线激光雷达，1 个惯性传感器和里程计.其中，激光雷达安装在车辆投影的几何中心.如图3 所示.实验场地为长23 m、宽20 m 的室内场地，场地内无特殊障碍物，如图4 所示.

图3 自研移动机器人模块组成Fig.3 Module composition of self-developed mobile robot

图4 室内实验场地Fig.4 Indoor experimental venue

3.2 实验结果实验对卡尔曼滤波的降噪效果进行了验证，列出降噪前后的传感器实验采集数据，并进行了对比分析，如表1 所示.

表1 滤波前后均方差Tab.1 Mean square error before and after filtering

表1 中列出了3 种传感器降噪前后的均方差，反映了数据的离散程度.从表1 可得，雷达、里程计、陀螺仪的均方差分别降低了27.1%、23.0%和34.5%，降噪后传感器的数据更加稳定，为多传感器的融合提供了稳定的数据.

完成降噪后，将降噪后的数值进行融合.根据式(12)指定里程计，IMU 和激光雷达的成本C分别为0.4、0.3、0.3 s，将这3 种传感器数据按照式(13)进行融合，融合后定位结果如图5 所示.

图5 两种算法定位实验结果比较Fig.5 Comparison of experimental positioning results for two algorithms

图5 中，绿色路径为主观贝叶斯网络融合算法得出的定位结果，另采用主流RUKF 算法进行对比，红色即为RUKF 定位算法所得定位结果[4].可以明显看出，在AGV 转向和移动时，两种不同算法之间的路径会出现误差.为了能够量化主观贝叶斯网络融合算法的精度，作X轴方向与Y轴方向定位结果图，如图6、图7 所示.

图6 两种算法在 X 轴方向定位结果Fig.6 The positioning results of two algorithms in X -axis

图7 两种算法在 Y 轴方向定位结果Fig.7 The positioning results of two algorithms in Y-axis

图6 中，RUKF 算法最大定位为1.38 m，主观贝叶斯网络融合算法最大定位为1.42 m.图7 中，RUKF 算法最大定位为10.83 m，主观贝叶斯网络融合算法最大定位为10.85 m.为了便于比较两种算法相比于真实数据的误差值，给出整体定位误差如图8 所示.

图8 两种算法整体定位误差比较Fig.8 Comparison of global positioning error for two algorithms

图8 中，主观贝叶斯多传感器融合算法的最大定位误差为0.031 m，RUKF 算法最大误差为0.065 m，定位精度提高了52.3%.

定位过程中，不仅要考虑到X轴、Y轴的定位精度，在机器人转向时，同时也要考虑到机器人的转向角度，如图9 所示.

图9 两种算法旋转角实验结果比较Fig.9 Comparison of experimental results of rotation angle for two algorithms

图9 给出了两种不同算法与真实测量值之间的旋转角对比，为了更加直观地出两种算法所得旋转角与真实旋转角之间的误差，给出旋转角误差图，如图10 所示.

图10 两种算法旋转角测量误差比较Fig.10 Comparison of rotation angle error for two algorithms

由主观贝叶斯网络多传感器融合算法所得出的旋转角最大误差为1.19°，RUKF 算法所得旋转角最大误差为2.97°，精度提高了59.9%，从而验证了贝叶斯算法的精度更高.

根据实验数据，还得出两种定位算法的均方根误差，计算时间以及均方差对比表，如表2 所示.

表2 两种算法的误差和时间Tab.2 Error and time of two algorithms

表2 中，均方根误差反映了融合值与真实值的平均误差大小.贝叶斯网络算法的均方根误差为0.17，RUKF 算法的均方根误差为0.25，贝叶斯算法将定位精度提高了43.6%，贝叶斯网络算法的计算时间为1.47 s，RUKF 算法的时间为1.54 s，时间上缩短了0.071 s，效率提高了4.5%，数据稳定性也提高了47.8%.在定位精度、鲁棒性、效率都优于RUKF 定位算法.

4 结论

本文针对室内AGV 定位问题，提出了一种基于主观贝叶斯网络多传感器数据融合的方法.该方法结合卡尔曼滤波，并通过信息增益自主地选择传感器数据进行融合，根据融合后的数据获取AGV的位姿信息.通过实验证明，该方法相对于RUKF算法，均方根误差缩小到了0.17 m，定位精度提高了43.6%，定位时间缩短了0.071 s，效率提高了4.5%，数据稳定性也提高了47.8%.证明了主观贝叶斯多传感器数据融合算法能够使多传感器的数据互补，减小了定位数值的均方差，缩短了计算时间且提高了移动机器人的定位精度.