基于激光SLAM实现AGV自主导航技术的仿真

张开乐，陈兴文

(大连民族大学信息与通信工程学院,辽宁 大连 116600)

随着智能领域的快速发展，激光SLAM(Simultaneous localization and mapping)技术具有稳定、可靠等性能优势，把该技术融入到AGV小车中进行自主导航[1]，可以大大提升AGV小车工作的灵活性和配送效率。可见自主导航技术是提高AGV小车智能化程度的核心技术之一，其中的同步定位与地图构建是解决其自主导航的关键问题。目前激光SLAM可分为两大类：基于滤波器和基于图优化的SLAM算法[2]。其中滤波器算法是用粒子滤波来估算机器人的位姿，并根据估算的位姿构建地图。但是该类算法存在两个问题，一是在大场景下，若里程计累计误差较大，则需要用较多的粒子来表示机器人位姿的后验概率分布，严重消耗内存；二是随着重采样次数的增多，粒子多样性散失，粒子耗散问题会严重影响地图的构建。而基于图优化的算法依靠前端回环检测和后端优化过程，优化所有的位姿能够减小里程计带来的累计误差，使定位和建图的精度较高。因此采用基于图优化的Cartographer算法[3]，融入里程计信息和激光雷达数据实现小车的定位和环境地图的构建，可以更好的实现自主导航功能。

1 基于图优化的Cartographer算法原理

Cartographer是Google推出的一套基于图优化的SLAM算法，主要由前端和后端两部分组成。Cartographer算法流程图如图1所示。

图1 Cartographer算法流程图

首先，传感器的数据获取，主要利用激光雷达数据、底盘里程计数据以及IMU数据，其中激光雷达数据是必须项。对获取后的激光雷达数据使用体素滤波器筛选出指定范围的点，并通过自适应计算获得适合的采样间隔，以减少点云数据量；滤波之后的激光雷达数据被加入到前端里面进行扫描匹配；同时对输入的里程计和IMU数据加入到位姿插值器中并融入上一次前端匹配完成的位姿，以估算出下一次前端匹配的初始位姿，并给到扫描匹配中作为其初值。

其次，前端主要负责构建一系列的栅格子图。子图的构建就是一段时间内对一帧一帧激光扫描匹配对齐和子图坐标迭代的过程。其思想就是通过变换矩阵Tξ将每一帧雷达扫描点观测数据获得位姿的平移量(ξx,ξy)和旋转量ξθ，从当前雷达观测值(Scan)的坐标系转换到子图坐标系，具体公式如(1)所示：

.

(1)

一帧Scan插入到子图之前，要通过扫描匹配完成点云位姿的最优化，确保Scan中的点最大概率地匹配到子图上。通过相关性扫描匹配(realtimeCSM)对激光帧数据在子图的栅格地图上进行一次粗匹配，产生一个比较好的位姿值，之后将该位姿再一次与子图进行优化匹配进行精细的搜索。在这里主要采用自带的Ceres非线性优化库通过最小二乘法进行最优化求解。具体公式如(2)所示：

.

(2)

式中变换矩阵Tξ能把所有的Scan点hk转换到子图坐标系下，通过M函数用双三次样条插值使雷达点变成在子图中概率值的平滑形式，式中M取概率值为0到1之间的数。最后根据运动滤波器检测此次姿态运动的大小，当小于某阈值时进行数据去除不加入到子图中，然后根据优化好的位姿将点云帧插入到子图中进行叠加，实现子图的构建。子图采用分辨率为5 cm的离散栅格地图表示，每个2 d的小格子都有自己的概率值P位于Pmin和Pmax空间当中，并且每一帧的激光点都会生成一组称为命中和未命中的栅格点，每一次Scan插入到子图上，都要更新命中集和非命中集。当激光点打在一个小格子上时，被击中的格子更新命中集(就是激光点打在障碍物上的概率),对每个命中的点与激光原点连线上的点更新其未命中集，而没有进行命中或未命中的点为其自定义分配一个概率值，逐一对地图中每个栅格进行概率更新。当子图创建完成时，栅格的概率值小于Pmin时，表示这个点无障碍物，中间为不确定，大于Pmax表示有障碍物。

最后，后端目的是通过在大场景下创建小的子图来抑制误差的累计，短时间内连续的激光数据组成的子图误差累计很小，但是当大量的子图被创建完成时，就会有误差累计，这时引入后端回环检测，优化所有子图的位姿。位姿图的构建其实就是一个回环检测匹配的过程。把所有创建完成的子图和当前的Scan数据用作回环检测的初值，在其附近一定范围内的搜索窗口中搜索最优匹配帧，如果检测到当前的扫描帧Scan和已创建的子图中的某个Scan的位姿在距离上足够近，则将子图和扫描帧进行匹配，若最优的匹配帧符合要求，则认为是一个回环。为了提高在子图中的搜索效率减少计算量，通过分支定界的优化方法进行优化搜索，计算各个Scan和子图之间的粗略的初始相对位姿，如果找到好的匹配，则调用Ceres的非线性优化对粗匹配的结果进行精细匹配调整，从而构建位姿图。

(3)

2 AGV小车仿真与环境建模

2.1 ROS和Gazebo简介

ROS是机器人操作系统的简称，提供了一套用于描述和建立小车模型的工具，并可进行数据记录、分析等。ROS采用松散耦合的机制，运行时由多个松耦合的进程组成，每个进程称之为节点，所有节点可以运行在一个处理器上，也可以分布式运行在多个处理器上。Gazebo是一款功能强大的三维物理仿真平台，与ROS有较好的兼容性，可以加载具有传感器数据的小车模型和三维可视化环境的仿真；可以使用Gazebo提供的命令行工具在终端实现仿真控制。

2.2 AGV小车模型建立

在ROS中通过URDF(United Robotics Description Format)对小车模型建立，并可以实现模型在Gazebo仿真环境中的建图与分析。AGV小车模型的构建是通过多个连杆(link)和关节(joint)实现的，每个link之间通过joint连接，joint则通过parent-child关系把上下link联系起来，同时添加link部分的外观参数，实现小车外形的构建。小车的外观形状完成后，需要定义link所在的位置，由于各个link通过joint进行连接，那么link的位置就由joint确定，所以使用子标签origin在joint内定义位置。在定义URDF建模时存在一些问题，会出现模型冗长重复内容过多，没有参数计算功能等，这时可用Xacro文件来优化URDF模型，精简模型代码，并调用Xacro文件解析器，解析成小车模型。URDF模型如图2所示。

图2 URDF模型

2.3 仿真环境下小车SLAM的实现

在Gazebo中完成仿真环境的搭建，并加载小车模型，然后用Cartographer_ros功能包进行环境地图的构建。该功能包是基于ROS的通信机制并获取各种传感器的数据将其转换成Cartographer算法中定义的格式传递给Cartographer进行处理，经过处理后又以消息的形式发布出去，便于调试和可视化。配置其中的Lua文件和Launch文件，通过roslaunch命令启动键盘控制节点mbot_teleop控制小车运动进行建图，并通过Rviz观察地图的建立，把已建好的地图保存在Navigation功能包下为自主导航提供一张地图。仿真地图如图3(a)所示，栅格地图如图3(b)所示。

图3 仿真地图和栅格地图

3 已知地图下的定位与导航的仿真

在Navigation功能包下的Launch文件中运行地图服务器map_server节点将上面已经构建好的栅格地图加载进来，启动AMCL节点估算小车在地图坐标系下的位姿信息，并利用move_base功能包完成全局和局部的路径规划，实现最终的导航功能[4]。

AMCL(即自适应蒙特卡洛定位)算法的实现主要是在已知环境地图下获取激光雷达数据、里程计数据，利用粒子滤波方法得到机器人在地图中的位姿。首先在地图空间上很均匀的撒一把粒子，并初始化所有粒子的方向和位置；其次，根据机器人的运动参数(里程计，速度等)对粒子的状态进行更新；再次，根据传感器的观测数据，计算出每一个粒子的权重并更新其状态；之后判断是否进行随机采样，随机采样公式如(4)所示。

.

(4)

其中参数ωslow和ωfast分别是长期似然平均估计权重和短期似然平均估计权重。如果短期似然优于长期似然，则不增加随机采样，粒子群进行重采样操作，否则按两者之比的比例增加随机采样，以这种方式可以消除瞬时传感器噪声带来的定位误差；最后对粒子群进行重采样，去除权重小的粒子，保留权重大的粒子，进行不断迭代，粒子群会慢慢收敛到一起并得到机器人的真实位姿，完成机器人定位。AMCL定位流程图如图4所示。

利用ROS中move_base导航功能包配置其中的yaml文件实现全局和局部的路径规划，通过订阅激光雷达数据、栅格地图以及AMCL定位数据，利用Dijkstra算法规划出全局路径和DWA算法规划出局部路径，再将路径转化为小车的速度信息，把控制命令Twist发布到速度控制cmd_vel主题上，其中Twist消息包括小车前进速度和角速度，最终实现对小车左右轮速度的控制。导航过程仿真如图5所示。

图5 导航过程仿真

4 结语

本文对Cartographer SLAM算法的构图和定位原理进行分析，从软件仿真的角度基本实现了AGV小车的地图构建、定位以及自主导航功能的仿真设计，并对导航系统的避障功能进行仿真测试与分析。但是对于Cartographer算法在优化过程中的定位、建图精度以及鲁棒性上还需要进一步提升性能，以确保复杂工作场景如颠簸路面、斜坡、长距离、等宽度、少特征巷道下的定位、建图与导航的精度与适应性。