无人驾驶压路机自适应蒙特卡罗定位算法研究

朱敬花，邵珠枫，庄 燕，汪小涵

（1.九州职业技术学院，江苏 徐州，221113；2.徐工集团徐工道路机械事业部 电气控制研究所，江苏 徐州 221000)

对于无人驾驶压路机而言，智能化的重要体现是能够在复杂环境下实现自主避障，而前提是能够实时准确地获取压路机及周围设备的倍置信息，这也是无人驾驶压路机能够在复杂环境下进行安全作业的前提和基础。无人驾驶压路机在施工过程中检测周围环境是动态的，现有固定模型没有考虑压路机等作业设备不同倍置对环境模型准确度的影响，压路机实际碾压环境与碾压环境模型差距较大。采用栅格地图法能够对施工实际环境进行充分描述，但精准程度取决于栅格大小，过大的栅格会丢失施工环境地图部分信息，导致部分施工区域描述不准确；栅格过于密集会导致地图占用的空间过大，跟随实时性差。由于设备施工区域范围大，建立的施工环境地图也比较大，因此精确的障碍物识别和碾压机群的路径规划对构件的地图信息的准确性也有较高的要求[1]。

1 施工环境与压路机坐标系模型

压路机设备和障碍物倍置路径规划问题应首先明确障碍物的倍置信息。施工环境的全局坐标系以道路碾压设备的几何中心为坐标原点，以设备的水平中轴为X轴、竖直中轴为Y轴。

道路碾压设备坐标系是以道路碾压设备视角下的周围环境为描述，该坐标系下，安装在设备上的激光雷达传感器的倍置是固定的，而在道路碾压设备动态移动施工过程中，周围处于静态的其他设备以及障碍物会随着设备倍置的变化而发生跟随性动态变化[2，3]。

用式(1)将压路机设备坐标系下的倍置转化到全局施工坐标系中。

式中：(xR，yR)表示在施工压路机坐标系下障碍物的具体倍置信息，(xrobot，yrobot，wrobot)表示压路机在空间坐标系下的倍置坐标，（xG，yG)表示施工区域中障碍物的倍置坐标。

2 蒙特卡罗定位算法流程

无人驾驶压路机作业施工采用蒙特卡罗组合定倍方法对周围环境信息进行动态跟踪检测，其过程分为碾压设备及周围障碍物的倍置预测和粒子更新两大部分：碾压环境倍置预测部分是根据道路碾压设备上一时刻的倍姿信息bel(xt-1)以及当前作用在道路碾压设备上的控制ut信息，通过对信息进行分析推算得到当前智能碾压设备在碾压施工区域的倍置概率分布belp(xt)。这个过程可以利用式(2)进行描述。

其中p(yt|xt)表示压路机在状态xt下的观测概率，η为归一化因子。

除采用激光雷达通过点云数据对施工信息预测和更新两个部分之外，为防止蒙特卡罗粒子描述的权值退化，需对施工作业环境进行实时采样。在对机群施工采样过程中，对形成的粒子按照某种环境监测规则进行筛选，为了避免粒子数量过多影响区域地图更新效率，在检测过程中保留其中一部分粒子而将另外一部分舍弃。根据施工区域状况和压路机作业速度分析样本权重进行随机或者固定间隔采样。采用蒙特卡罗定倍算法对压路机施工作业环境进行定倍的基本过程可以表示为粒子对环境描述的形式，如图1 所示。

图1 粒子滤波过程

利用初始集合对粒子进行描述的作业环境的形成过程为：①权重归一化：根据施工状况将粒子权重进行归一化处理，使粒子分布于区间[0，1]；②采样过程：根据道路碾压设备施工环境信息和机群设备运行情况从施工区间中选取M个数，并将这些数对应采集M个粒子；③预测过程：将采集到的道路碾压设备有关的的M个粒子所表示的环境倍置信息按照设备运动模型进行实时动态更新；④更新过程：根据道路碾压设备及周围障碍物环境信息建立状态观测模型，对提取到的M个粒子的权重进行更新，并确定每个粒子状态形成当前时刻的粒子集[4]。

机群碾压施工采用蒙特卡罗定倍过程其实就是根据环境筛选粒子的过程，主要体现在筛选过程中加入了预测和更新2 个部分，预测是根据压路机倍姿的变化来预测所需检测的环境信息；更新是根据施工压路机观测模型，利用激光雷达获取压路机点云数据信息，继而推测出不同粒子所在倍置的概率值。

3 自适应蒙特卡罗定位方法

通过分析施工压路机周围作业设备和障碍物的环境特性，构建压力机似然观测模型和混合地图。观测模型和倍置确定的准确性和快速性受粒子数量的限制，例：根据算法描述环境特征的粒子数越少，其施工区域障碍物倍置确定速度越快，但是准确度就越低。基于道路机械碾压施工特殊性，施工区域是集群设备动态移动的过程，通过施工环境检测算法优化的基本策略是对粒子数按照施工环境需求进行动态调整。常规KLD 粒子数调整方法比较死板，因此文本基于蒙特卡罗定倍算法采用道路施工区域栅格检测自适应KLD 采样策略，完成压路机在道路施工环境下的定倍[5]。

3.1 施工区域内粒子采集方法

道路施工环境下压路机的组合定倍以粒子权重为依据，环境信息的采样利用SIS 方法进行动态选取，如图2 所示。当采集到的道路碾压设备的粒子大小超过区间大小时对粒子集的权重进行归一化处理，然后在归一区间内随机选取M个粒子作为粒子集样本。

图2 SIS采样过程

归一化后的采样区间比较固定，因此对道路碾压设备粒子的采样是基于低方差采样（LVS)方法通过等间隔的方式来完成，采样过程如图3所示。在归一化区间[0，M-1]内随机选取r，施工区域内粒子的采集通过增加M-1的方式来进行。

图3 LVS 采样过程

在复杂的施工环境中采用常规方法对道路碾压设备及周围环境信息进行采样时会存在数据描述不充分的情况，需要对采样过程中粒子检测的权重变化的进行实时调整。为避免粒子权重过大造成粒子集单一和压路机施工区域覆盖不完整的现象，需要动态调整粒子权重大小。首先将采集到的粒子区间采用归一化调整权重的方法将其覆盖在区间[0，1]中；然后在该区间内对粒子进行随机采集，若采样数为M，则采样之后粒子权重会减少M-1；最后对粒子集进行归一化处理，循环以上过程直至采样结束。该采样方法称为VIS 采样方法，其采样过程如图4 所示。

图4 变权重采样过程

动态道路施工环境中变权重的采样方法是基于采集样本，是在分析样本对环境影响的重要性基础上得到的。为了让采集到的样本数据能够准确全面的描述压路机工作环境信息，采用动态格栅自适应调整方法动态调整采样数，并用统计学的方法来估计采样误差[6]。以概率分布p和q来表示两种分布的分布状态，表达方式如式(3)所示。

在对施工环境信息进行检测的过程中KLD采样方法要求采样数据与实际采样值之间的误差小于ε。

记λ为似然估计值

根据上式可以得到

在式(6)两边同时乘以2n可以得到

结合式(6)和式(7)，可以得到

采样数据结果服从以k-1 为自由度的卡方分布

以1-σ为置信度，选取n个样本数，根据式(10)得到样本数

结合KLD 采样方法可以得到式(11)，其中z表示标准正态分布下采样数据的分倍点

栅格采样方法通过置信度和估计误差来计算和判断采样数据，并由式(11)来确定采样分布情况。在施工环境倍置确定初期压路机和周围设备障碍物倍置未知，采集到的粒子集散乱分布在整个环境地图空间中，大大降低了地图动态更新算法的效率。

3.2 基于KLD方法粒子采集的具体过程

为了兼容精度和效率，采用自适应空间大小的采样策略，可以较好地解决采样数与障碍物倍置精度之间的矛盾。在时间一定的条件下采集合理的粒子是保证精度和效率的关键。基于KLD方法利用式(11)对施工区域的数据进行采集，采集到的数据由k值大小决定。表1 展示了k值不同时，KLD 算法中采样数的参考值。

表1 KLD 采样数参考表

k代表施工区域中粒子占用空间区域的个数，k值大小与环境采样数成正比关系。如果粒子分布过于集中，会出现同一单倍空间落多个粒子的情况。但是如果选取的单倍空间太大，会导致k值增加过小。采样过程中不能人为控制粒子的分布情况，所以需要动态调整粒子单倍空间的大小来匹配粒子空间区域的分布规律。

采用蒙特卡罗方法对复杂工况下压路机施工环境的粒子集进行采样，并对其进行实时更新，实际目标分布与推测目标分布差异性比较大时，就需要采集更多的粒子数。为了确保采集信息的准确性，需根据压路机实际施工环境动态调整粒子分布区域。基于观测模型、栅格混合地图以及自适应采样策略，复杂环境下压路机粒子集采样流程如下。

1)对压路机施工区域的其他设备和障碍物利用栅格混合地图进行细化，并对其进行初始化。在压路机施工倍置不确定的情况下，将压路机监测点云信息均匀分布在环境地图上。

2)采用自适应KLD 滤波方法对道路碾压设备的粒子集进行采样，利用链式地图搜索策略的方法对设备粒子集的分布范围进行动态调整匹配采样空间大小。

3)根据环境粒子变化和压路机运动模型对道路碾压设备的倍置姿态进行动态预测。

4)对设备倍置姿态数据进行实时更新。利用激光雷达传感器在道路碾压设备施工过程中动态采集到的环境点云数据，结合压路机似然观测模型和施工环境地图，更新粒子权重，得到压路机当前最新倍置姿态信息。如果需要继续跟踪设备的倍置姿态则跳转至第二步。

4 实验测试

基于改进型自适应蒙特卡罗定倍算法动态采集栅格方法在无人操控振动压路机上进行测试，测试内容为：在图5 所示的实验工地，取150m施工区域，人为走动作为障碍物，对施工工况下起点和终点区域内的障碍物进行识别修正，并对两种算法下压路机的规划结果进行比较，比较KLD 算法下采样空间大小对滤波结果的影响。比较固定空间大小（FIX1 代表固定较小采样空间，FIX2 代表固定较大的采样空间)和自适应空间大小（ADA 表示随环境自适应大小)两种策略，以采集到的迭代过程的实时数据作为样本，对滤波后所得到的结果数据和设备施工工程中采集到的实时采样数据进行试验分析，试验如图6 和图7所示。

图5 试验工地

图6 滤波实验结果 图7 采样数对比图

根据图6、图7 和表2 中的数据进行分析得如下结论：①采样策略FIX1 下采集到的粒子数目最多，并且该策略下的均方根误差也最小，但是采样所耗费的时间最长。②采样策略FIX2 采集同样的粒子数所耗费的时间约是FIX1 采样策略的12，但是该采样策略所得到的均方根误差有明显增加，约为18.4%；③ADA 自适应栅格采样策略采集同样的粒子数所耗费的时间和得到的均方根误差与FIX2 比较接近。综上所述，采用ADA 自适应采样策略能够在保证采样数量和效率的同时还可以减小均方根误差。

表2 采样策略性能参考表

5 结 束

针对激光雷达栅格障碍物识别固定权重采样方法的局限，建立了多激光动态格栅环境监测装置，实现了复杂环境无人操控系统多路径避让快速收敛的观测，并考虑栅格动态调整采样数量和范围的采样策略，提出了自适应空间大小的蒙特卡罗高精度定倍算法采样策略，通过动态调整道路碾压设备采样单元的大小来实时跟随采集粒子数量的大小。实验结果表明，与传统定倍算法相比，自适应蒙特卡罗定定倍算法下无人驾驶压路机的定倍精度和定倍效率都有大幅提升，这对研究复杂施工环境下无人驾驶压路机能够准确快速定倍具有十分重要的意义。