张伟民 户肖剑 贾群喜
摘 要：借助ROS机器人操作系统，通过实验分析并比较三种建图算法，使用Gmapping算法并结合RPLIDAR生成二维地图效果最佳。在原有AMCL算法的基础上，添加了动态参数，使其粒子群收敛速度根据车体速度的改变而改变，对于无人驾驶车的实时定位会更加精确。创新性的使用路径曲率控制方案，相较与纯路径追踪控制，方便且更易编程实现。最后使用Move_base功能包实时规划出最优的全局与局部路径，使无人驾驶车实现自主导航与避障的功能。
关键词：Gmapping算法;AMCL算法;Move_base功能包;自主导航
中图分类号：TP277 文献标识码：A 文章编号：2096-4706（2020）23-0080-04
Research on Algorithm Optimization of Autonomous Vehicle Based on ROS
ZHANG Weimin，HU Xiaojian，JIA Qunxi
（Luoyang Institute of Science and Technology，Luoyang 471023，China）
Abstract：With the help of the ROS robot operating system，the three mapping algorithms are analyzed and compared through experiments，and the Gmapping algorithm combined with RPLIDAR to generate a two-dimensional map has the best effect. On the basis of the original AMCL algorithm，the dynamic parameters are added to make the convergence speed of particle swarm change according to the change of vehicle speed，so that the real-time positioning of the autonomous vehicle will be more accurate. The innovative path curvature control scheme is more convenient and easier to program than pure path tracking control. Finally，the Move_base function package is used to plan the optimal global and local paths in real time，so that the autonomous vehicle can realize the function of autonomous navigation and obstacle avoidance.
Keywords：Gmapping algorithm;AMCL algorithm;Move_base function pack;autonomous navigation
0 引 言
无人驾驶汽车的出现，带来了新的经济增长点，而且更加的贴合共享经济，使残疾人也能拥有私家车。但是在无人驾驶汽车迅速发展的同时，也存在着很多问题：如很多地区定位不精确，就无法正常导航;而且，部分道路临时维护时，也不会即时更新到地图;部分道路路况复杂，无人驾驶汽车可能不会顺利通过;最重要的是相关交通事故法规，也未完全完善。因此作者以参加全国大学生智能车大赛和机器人竞赛的经验为基础，自主搭建了无人驾驶车车体，利用ROS中开源的功能包，并不断地创新优化算法，使无人驾驶汽车能够自主行驶。
1 ROS无人驾驶车各模块算法研究
1.1 ROS无人驾驶车建图算法研究
二维环境地图是ROS无人驾驶车通过传感器对周围环境进行采集分析后形成的数据信息，是确保导航定位的标准。ROS无人驾驶车将激光雷达、姿态传感器和里程计信息相互融合，搭建完善的二维栅格地图，为无人驾驶提供导航定位相关的数据消息。
1.1.1 建图算法比较
二维地图的建立算法基本上都是基于概率估计的，比如Gmapping、Hector-SLAM、谷歌Cartographer算法等。Gmapping算法是研究最普遍的SLAM算法，因此，该算法目前有非常高的成熟度。而且，该算法建图效果精准，地图也无漏隙。Hector-SLAM建图算法则对激光雷达的扫描频率要求特别高，使用此算法必须借助价格昂贵的雷达。而且Hector-SLAM算法在ROS无人驾驶车快速转向的时候容易发生错误匹配，导致搭建的地图发生错位。谷歌Cartographer算法结合RPLIDAR数据、底盘odom数据、IMU数据，再通过相应的建图优化算法，构造出一个几乎没有缺陷的二维地图。
结合以上分析，Gmapping建图算法原理相较于其他算法比较简单，而且使用该算法构建出的地图也非常精确，因而本文研究采用该算法构建地图。
1.1.2 Gmapping建图算法分析
Gmapping建图其实使用的是基于2D激光雷达的RBPF算法完成二维栅格地图构建的SLAM算法。可以使用一个条件联合概率分布公式来描述：
p（x1：t，mu1：t，z1：t） （1）
其中，p为概率，z1：t为ROS无人驾驶车从初始时刻到时间t时刻通过激光雷达和IMU传感器获得的数据。u1：t为ROS无人驾驶车从初始时刻到时间t时刻所走过的位移，m为地图中的栅格点，x1：t为当前ROS无人驾驶车的轨迹状态。
利用通用公式分布律：
p（A，BZ）=p（AZ）p（BA，Z） （2）
其中，A，B，Z为3种不同的随机变量，对式（2）进行化简得：
p（x1：t，mu1：t，z1：t）=p（x1：tu1：t，z1：t）p（mx1：t，z1：t）
（3）
则进一步可以使用p（x1：tu1：t，z1：t）表示ROS智能机器人的路径状态概率，使用p（mx1：t，z1：t）表示在已知路径状态和传感器数据时，计算地图构建概率点。
由于Gmapping建图算法使用粒子滤波来估计当前ROS无人驾驶车位置，同时将粒子集合一起组建成栅格地图，所以每一个粒子就包含了位置姿态信息和地图信息，利用贝叶斯准则对p（x1：tu1：t，z1：t）进行化简。
先进行转换：
p（x1：tu1：t，z1：t）=p（x1：tx1：tzt，u1：t，z1：t-1） （4）
再将zt消掉，并利用贝叶斯公式化简，最后得到：
（5）
其中，η为归一化变量，接着就可以使用p（x1：t-1u1：t-1，z1：t-1）表示上一时刻的粒子群。每一个粒子都用运动学模型p（xtxt-1，ut）进行状态传播，这样就得到每个粒子对应的预测轨迹。对于每一个传播之后的粒子，用观测模型p（ztxt）进行权重计算归一化处理，这样就可以得到该时刻的机器人轨迹，之后根据估计的轨迹以及观测数据进行二维地图构建。图1为利用激光雷达并借助Gmapping算法建立的地图。
1.2 AMCL定位算法研究
AMCL对MCL算法进行了优化，借助RPLIDAR传感器的360度扫描数据，实时获取无人驾驶车的位姿，并用粒子不断地进行概率估计，且可视化成箭头标识。当无人驾驶车行驶时，AMCL位姿粒子的估计度会逐渐达到饱和，且粒子也将会逐渐汇聚到一块，实时跟随无人驾驶车移动。
在二维栅格地图中，粒子的分布度由初始化条件决定，初始化条件即在调用功能包时需要配置的初始参数，包括雷达的扫描频率、最大扫描范围，及粒子的初始化大小、覆盖范围等。
假设有N个观测点，以z表示：
z={z1，z2，…，zN} （6）
估计在某定点位置的概率为：
（7）
观测点z为激光雷达和IMU获得的数据，它是一个数学上的集合，含有N个观测数据。m为使用Gmapping建图算法构建的地图信息。x为当前ROS机器人在二维地图上的坐标。P为估计在某定点位置的概率值，n为从1开始的整数。
我们又在该算法的基础上增加一个动态参数kv，将式（7）改为：
（8）
此时，粒子收敛的概率将会根据车体速度的改变而改变。车体速度快时，粒子收敛速度将会略微降低，以免出现快速下的误估计;车体速度快慢时，则相反。将此算法应用在ROS无人驾驶车上，可以发现粒子在初始时刻聚集度就较高，成功地实现了算法的优化，如图2中聚集群所示。
1.4 轮式里程计算法研究
轮式里程计与简单的左右差速驱动方法不同，该算法通过实时测量的左右轮转速信息推演机器人的轮式里程计信息。
在无人驾驶车的底盘上，我们增加编码器装置测得车轮实时的线速度vt，根据线速度与角速度关系，可以得到角速度 ，R为无人驾驶车的转向半径。
继而我们建立以下运动学模型，去实时推演当前ROS无人驾驶车的位置与姿态：
（9）
θt为前轮转向角，dt为极短的时间段，则可以使用以上模型去实时获取车体在横纵两个方向的位移xt+1与yt+1，继而给无人驾驶车提供精确的位姿信息，使其更稳定的行驶。
2 ROS无人驾驶车控制算法
2.1 路径曲率控制
我们采用外接圆半径公式来控制速度，假设在△ABC中，已知三边a，b，c那么它的外接圆的半径R的公式为：
（10）
其中，。
三角形外接圆的圆心被称作三角形的外心，即三角形三边垂直平分线的交点，而三角形的三个顶点就在此外接圆上。我们可以得知关于三角形外接圆外心的性质：锐角三角形的外心在三角形内;直角三角形的外心在斜边上，与斜边中点重合;钝角三角形的外心在三角形外;等边三角形外心与内心为同一点。
我们以最典型的弯道分析该算法，可以看到，在图3中，采集的三个点A、B、C，组成了一个钝角三角形，在图4中的三个点组成了一个锐角三角形。而经过以上的性质可知，钝角三角形的外接圆半径较锐角三角形的外接圆半径长。除此之外，可以看到，图4中的弯道曲率明显比图3中的弯道曲率大。
综合以上分析，就可以根据采集到的3个点组成的三角形的外接圆的半径的大小，来控制速度的大小。即当外接圆的半径值越大，给定的速度值越大，外接圆的半径值越小，给定的速度值越小。
当已知线速度，同时就可以根据小车的实际机械结构，测量出小车的最小转向半径R，继而根据公式v=ωR，可以得出角速度ω。则角速度ω将可以被用来控制小车的转向。
2.2 纯路径追踪控制
对于阿克曼车体，因为其速度控制仅依赖于一个电机，转向依赖于一个舵机，因此为了简化控制模型，可以将此阿克曼车体等效为一个自行车模型，如图5所示。
则关于运动学自行车模型中的各个状态量的更新公式为：
X轴方向的位移xt+1：
xt+1=xt+vtcos（ψt+β）·dt （11）
Y轴方向的位移yt+1：
yt+1=yt+vtsin（ψt+β）·dt （12）
两轮中心距与Y轴的夹角ψt+1：
（13）
速度变化△v：
△v=vt+1-vt-a·dt （14）
结合式（11）、式（12）与式（13），车辆的转向角β则可以求得 ：
（15）
a为车体当前加速度，lf为自行车模型重心点到前轮的距离，lr为自行车模型重心点到前轮的距离。
上述纯路径追踪方案相较于路径曲率控制，在编程实现时，需要调节的参数过多，而且算法实现要最理想的效果也很困难。所以，我们选择路径曲率方案来实现ROS无人驾驶车得控制。
3 ROS无人驾驶车的功能实现
3.1 ROS无人驾驶车自主导航
利用Gmapping建图算法，得到一张二维地图，提供给ROS无人驾驶车运行的环境。然后，结合AMCL定位算法，使其知道自身在地图中的位置，即X轴、Y轴坐标。同时，轮式里程计的使用，将能使其得知自身离起点的位移，使其定位更为精确。
ROS是一个分布式的进程框架，这些进程被封装在易于被分享和发布的程序包和功能包中。从而我们就可以将我们要使用的功能包联系起来。
对于ROS无人驾驶车来说，主要使用3大功能包，如图6所示。
Gmapping功能包主要利用上述Gmapping建图算法，在提供里程计信息后，再结合激光雷达建立一个二维栅格地图。此二维栅格地图可利用rviz插件可视化。
AMCL功能包利用蒙特卡罗概率统计定位方法，针对已有的二维地图，并使用粒子滤波器跟踪ROS无人驾驶车的姿态。
Move_base包主要利用ROS中的话题机制，并规划出两条路径，一条是由起点到终点的最优路径，另一条则是根据当前车体姿态规划出的局部路径。将这三者联系在一起，即可实现无人驾驶车的自主导航。
如图7所示，二维地图中的虚线线条即是Move_base功能包规划出的全局路径。
3.2 ROS无人驾驶车避障
避障对于ROS无人驾驶车是一个主要的功能，主要分为静态避障，动态避障：
（1）静态避障：指智能机器人根据构建的二维栅格地图，并借助各个传感器数据，当障碍物出现在地图中，能够实时检测到，并顺利的规避过去。
（2）动态避障：对于智能机器人检测范围内正在运动着的物体或者行人，也能够进行有效避障。即能够实时规划出安全快捷的路径，且能够实时跟随。
对于ROS无人驾驶车，当基于Gmapping算法建立的二维地图实际环境中存在障碍物，比如锥桶、行人等，无人驾驶智能车首先通过雷达传感器进行探测，探测到障碍物时，将数据传送到车体上的处理器中，然后，再利用ROS之间的话题机制，发送到Move_base功能包中，实时规划出最优避障全局路径与局部路径，并发出速度与转向话题，使无人驾驶车有效避开障碍物，如图8所示。
4 结 论
本文利用ROS机器人操作系统，结合Gmapping建图功能包、AMCL定位功能包和Move_base路径规划功能包，实现了无人驾驶车的自主导航与避障功能。并分析了两种控制方案，本文最终了编程易实现的方案，实现了直道加速与弯道减速的控制。
参考文献：
[1] 李业谦，陈春苗.基于ROS和激光雷达的移动机器人自动导航系统设计 [J].现代电子技术， 2020，43（10）：176-178+183.
[2] 杜边境，李富强，刘勇，等.基于激光SLAM的移动机器人实验室巡检 [J].电子测试，2020（22）：52-53.
[3] 许晓梅.汽车转向系统典型模型的对比研究 [D].长春：吉林大学，2012.
[4] 任春华，陈灿，王满喜，等.一种惯性测量与距离组合的定位修正方法研究 [J].仪器仪表学报，2013，34（11）：2513-2519.
[5] 李博.GP-SLAM：基于激光雷达的新型同时定位与建图算法 [D].杭州：浙江大学，2020.
作者简介：张伟民（1978.12-），男，汉族，河南洛阳人，讲师，硕士研究生，研究方向：自动化。
