狭窄紧凑环境下智能小车SLAM导航实验

王 铎，杜 峰，关志伟，2，赵 彪，刘云鹏

（1.天津职业技术师范大学汽车与交通学院，天津 300222；2.天津中德应用技术大学汽车与轨道交通学院，天津 300350）

同步定位与建图（simultaneous localization and mapping，SLAM）是指搭载传感器的主体，从陌生环境的任意地点出发，通过传感器完成与环境的信息交互来确定自身位置，构建增量地图，从而实现同步定位与地图构建的功能[1]。该技术作为人工智能技术不可或缺的一环，已大量应用于全方位移动平台、仓储机器人、物流机器人等领域，而这些领域有一个共同特征——工作环境狭窄而紧凑，智能移动平台的活动空间较小[2]。传统的阿克曼转向方式因转向时占用空间较大而不适用于狭窄紧凑的工况[3]，故迫切需要一种更高效的转向方式来应对该工作环境。麦克纳姆轮的出现完美地解决了这个问题。麦克纳姆轮具有灵活的运动控制方式、良好的机动性，能够在任意角度实现平移或原地旋转等优势，这种移动方式在空间狭窄，活动空间有限的环境里尤为合适[4]。如Airtrax开发的Sidewinder叉车、Vetex公司研发生产的RoboMate全向移动平台等产品，均针对狭小、多变的工作环境而设计。Sarmento等[5]将无线远程遥控技术应用到装载了麦克纳姆轮技术的智能移动平台中，为麦克纳姆轮技术在智能移动平台的进一步普及做出了贡献。随着麦克纳姆轮技术日趋成熟，国内外学者对其在智能移动平台的自主导航问题进行了更深入的研究，徐守江[6]针对未知全局静态环境智能移动平台的路径规划问题，提出了一种新颖的基于粒子群算法的滚动规划算法。李维维等[7]为了提高智能移动平台在栅格环境下的路径规划质量和规划稳定性，提出了启发式信息素交流异构双种群蚁群算法的规划方法。针对农业机器人的自主导航问题，Nehme等[8]提出了一种稳健的基于激光雷达的藤行结构跟踪解决方案。在机器人自主导航的领域，国内外的研究已经有了很大的进展，但针对狭窄紧凑环境下的机器人自主导航研究还稍显薄弱。本文选取装载ROS系统的麦克纳姆轮智能小车平台作为研究对象，对狭窄环境下的智能小车膨胀半径参数进行实验优化研究。

1 智能小车系统框架

1.1 硬件架构

智能小车的硬件结构包括：里程计与单线激光雷达等传感器、麦克纳姆轮底盘、树莓派（配有ROS系统）主机、集成了IMU的驱动面板（STM32）、编码电动机。硬件架构如图1所示，小车硬件架构如图1（a）所示，实物图如图1（b）所示，车身长30 cm，轮间距31 cm。

图1 智能小车硬件架构

借助智能小车平台发出的Wi-Fi信号，PC端通过ROS的分布式通讯远端登入树莓派，实现对树莓派的远程管理。单线激光雷达经USB接口把采集的点云数据发送给树莓派，树莓派向驱动板传达控制指令。当小车移动后，里程计与IMU也会反馈信息至树莓派，用以确定小车的位姿信息。

1.2 软件架构

ROS[9]是一种集成使用了多种通信协议、工具库的智能机器人控制系统，提供了各种通信功能包，使用TCP/UDP网络，可以进行各种通信和机制之间的实时数据交互传输。因此，搭载ROS系统的树莓派平台可实现对上层工作环境变化的实时感知和对下层决策过程的管理。智能小车系统的软件框架结构如图2所示。

图2 智能小车软件架构

智能小车的通信交互机制主要可简单概括归纳为以下3个核心模块：感知层、决策层、控制层。感知层为传感器数据，导入决策层即为ROS主机通过算法进行分析并下达命令传送至小车驱动板，即控制层，支配小车运动[10]。

2 运动学模型

2.1 麦克纳姆轮简介

麦克纳姆轮[11]的概念最早由瑞典Mecanum公司的工程师提出，麦克纳姆轮是一种全新的移动方式，装载麦克纳姆轮的智能小车在不改变自身位姿的状态下可以实现全方位移动。麦克纳姆轮如图3所示。

图3 麦克纳姆轮

麦克纳姆轮由轮毂和辊子两部分组成，轮毂轴和辊子旋转传导轴之间处于45°夹角，如图3（b）所示，麦克纳姆轮在运动过程中，辊子和地面之间的摩擦会产生一定的摩擦力，其产生的方向与辊子轴相反[12]。麦克纳姆轮通常4个1组进行使用，其中2个轮为左旋，2个轮为右旋，以手征对称的方式2个1组装配在小车上[13]。

2.2 运动学模型

智能小车运动学分析示意图如图4所示。

图4 智能小车运动学分析示意图

以模型质心O点为原点，建立全局坐标系，该模型具有3个自由度，中心点速度vx、vy、ωz，设右前轮的中心为O2，转动的角速度为ω2，辊子的速度为vg2，设2轮在全局坐标系下沿x轴方向的分速度为v2x，沿y轴方向的分速度为v2y，分速度与辊子的速度及轮的转速有如下关系[14]

对小车，有

将式（1）、（2）联立消去vg2可得

同理，其他3轮通过推导，也可得出式（3）。

将所有车轮转动角速度公式联立转化为矩阵形式为

式中：R为车轮半径；α为电机轴与辊子夹角；l1为前轮中心线与车身中心线的距离；l2为前轮中心点水平线与车身质心水平线的距离。

依据该矩阵可给定小车一个速度状态，进而控制小车运动。

3 同步定位与地图构建

3.1 Gmapping算法

Gmapping算法在构建场地较小面积的地图时，相比于其他SLAM算法具有计算量更小、鲁棒性更强、且更适用于单线激光雷达等优势，因此也更适合于本文研究的智能小车平台及狭窄紧凑的实验环境，故本文采用Gmapping算法构建高精度地图。Gmapping实时计算技术[15]以RBPF SLAM为基础，通过激光雷达和里程计所采集到的实时周围环境和位姿信息，完善了粒子分布计算和数据导入的自适应重采样计算机制，极大改善了实时计算效率，适用于对数据实时化处理精度要求相对较高的实际应用场合。该算法的流程如下：

（1）输入t－1时刻的里程计消息Ut-1，粒子集合St-1，t时刻的观测值Zt。

（3）观察更新，以运动更新后的粒子位置和上一时刻粒子权值为依据，计算当下粒子权值。

（5）更新粒子集。

（6）进行St重采样。

RBPF SLAM中的后验概率p（x1：t|z1：t，u1：t）的近似值被修改为p（x1：t|m（i）t-1，x（i）t-1，z1：t，u1：t-1），将上一次的里程计数据也考虑在内，极大减少了粒子扩散的问题。通过传感器与里程计数据可以推测出机器人的位姿与轨迹信息，从而计算地图的概率分布，最终得到精确地图。

通过与智能小车平台发出的无线Wi-Fi信号相连接，在PC端使用键盘控制小车移动探索区域建图，遍历实验室，通过Gmapping算法构建出的地图效果如图5所示。

图5 基于Gmapping算法的建图

3.2 智能小车自主导航

3.2.1 智能小车定位

在SLAM中智能小车根据已构建好的地图进行自主导航的前提是要确定自身的位姿与当下在地图中所处的位置。智能小车定位问题可分为局部定位与全局定位，其判断依据为是否确定小车初始位姿。在初始位姿已知的情况下，智能小车准确追踪自身位姿即为局部定位，也称位姿跟踪。智能小车初始位姿状态不确定，此种情况下追踪自身位姿被称为全局定位。然而，也存在另一种情况，采用SLAM创建环境地图后，小车遭遇“绑架问题”，即智能小车突然被移置其他位置，无法识别自身在地图中所处的位姿状态。针对此种问题，本文采用自适应蒙特卡洛定位算法（adaptive Monte Carlo localization，AMCL）。该算法将智能小车运动中获取的观测数据更新粒子进行重采样处理，并在其过程中加入随机粒子以完成更精确的定位，添加的粒子数目可通过平均测量概率决定。粒子的权值为观测模型的随即估计，将粒子的权值与总体观测概率进行比对，可得出随机粒子数量的近似值

式中：z为系统观测值；u为控制输入；m为地图。

粒子的分布可以根据姿态空间上的均匀分布绘制粒子，并利用当前观测模型对粒子进行加权，该算法适用于大范围的局部定位[16]。

3.2.2 全局路径规划

全局路径规划是指智能小车在地图中从当前位置移动到目标位置时，所选择的一条最佳移动路线，该路线在绝大多数情况下是最短的路线，在特殊情况下也可能由其他需求决定，如功率或速度。全局路径规划主要采用Dijkstra或A*算法[17]，本文采用A*算法。A*算法与Dijkstra算法的不同之处在于，其在路径搜索中添加了影响因子，使用启发式函数h（n），将启发信息融入估计函数中，使得当前路径的选择能快速收敛至目标点附近，这样使得在寻找最短路径时无需遍历所有节点，节省搜索时间的同时，也减少了计算过程。其估计函数可表示为

式中：g（n）为从初始节点到当前节点n的真实代价值；h（n）为当前节点n到目标点的估计代价；函数f（n）为初始点到当前节点n的总估价函数。

3.2.3 局部路径规划

局部路径规划主要通过控制智能小车的角速度和线速度，引导智能小车尽可能沿着己经规划好的最优线路移动，使得小车尽可能符合整体最优线路的规划。当周围环境中出现地图中不存在的障碍物时，智能小车将依据传感器实时监测到的信息，判定与障碍物之间的距离并重新规划路径进行避障。局部路径规划通常使用动态窗口法（dynamical window approach，DWA）[18]。DWA算法是在一定时间内，通过外部传感器采样智能小车线速度和角速度，利用智能小车运动学模型与相关障碍物信息对速度空间进行一定范围限制（称之窗口），然后对采样的速度空间进行数值模拟，规划小车可行走的多条路径，再通过评价函数对多条路径进行评估，选取最优的路径作为小车行驶路线。本文采用DWA算法用以进行动态避障。

3.2.4 代价地图

代价地图用来储存周围环境障碍信息，全局路径代价地图（global_costmap）储存全局路径规划信息，局部路径代价地图（local_costmap）储存本地路径规划信息。因此，还需要创建通用配置文件来为这2种代价地图储存一些通用或单独的配置文件，该文件用来存储一些参数，其中inflation_radius参数（以下简称膨胀半径参数）用来设置障碍物的膨胀参数，即小车与障碍物之间应保持的最小安全距离。本车默认值为1.2，初步判断，如果该参数初始值设置过大，将导致小车的通过性与安全性较差，进而影响小车在狭窄环境下的导航效果。因此，该参数的优化是本研究的重点。

3.3 move_base功能包

当控制智能小车利用Gmapping算法建好地图后，通过ROS系统中的Rviz可视化界面为智能小车在已构建好的地图中设定目标位置，智能小车便会调用move_base功能包躲避障碍物并移动到给定的目标位置。智能小车运动时，move_base功能包会进行全面的路径规划。在进行路径规划时，根据传感器实时数据、已构建好的地图、配置文件中给定的速度，实时调整小车的角速度、线速度和加速度。该功能包必须具备的话题包括LaserScan（激光雷达信息）、Odom（里程计信息）、TF（各个坐标系的转换关系）、Path（规划路径）及Map（构建的地图信息）。该功能包节点流程如图6所示。move_base功能包主要实现全局路径规划（global_planner）与局部路径规划（local_planner）两大功能。基于move_base功能包的小车导航如图7所示。

图6 move_base节点流程图

从图7（a）可以看出，在构建好的地图中给定目标点，智能小车根据move_base功能包所规划的最优路径（全局路径规划）进行自主导航。当在路径上放置动态障碍物时，如图7（b）所示，此时小车开始利用局部路径规划的DWA算法重新规划局部路径完成实时避障，如图7（c）所示。

图7 基于Move_base功能包的小车导航

4 实验与结果分析

实验研究发现，代价地图中的膨胀半径参数可能是在狭窄坏境下影响小车通过性能的最关键因素。因此，需要利用图5所建立的实际狭窄环境地图进行验证和确认，以达到进一步提高智能小车避障与通过性的要求。

使用Gmapping算法构建地图，在地图上设置一个目标点，同时在目标点前设置建图时不存在的障碍物，使智能小车可通过的空间长度仅有35 cm（小车轮距为31 cm），且只能从此通道通过（如图7（b）所示）。在不同的膨胀半径参数下，使用PC端控制小车到达目标点，观察智能小车的全局路径规划，以及是否能够通过局部路径规划实时规避障碍物到达目标点。全局路径规划如图8所示。

图8 不同参数的膨胀半径全局路径规划路线

从图8可以看出，随着膨胀半径参数的递减，周围环境的自由空间逐渐增大，智能小车的全局路径规划策略也随之变化；膨胀半径参数越小，全局路径规划越简洁，智能小车的空间利用效率越高。当膨胀半径为0.1时，智能小车的路径规划最简单，但却与周围环境发生碰撞，如图8（e）、（f）所示。究其原因是由于膨胀半径参数选取过小，智能小车与周围环境最小安全距离过小。代价地图通用配置文件中检测障碍物最小高度的参数（min_obstacle_height）为0.05 m，如果障碍物低于0.05 m，智能小车无法识别。膨胀半径过小，智能小车就会因规划的路径与障碍物过近而相碰。因此，智能小车或机器人的结构不同，选取的膨胀半径参数亦不相同。智能小车具体导航结果如表1所示。

表1 智能小车导航结果

从表1可以看出，智能小车在膨胀半径参数分别为1.2、1.0、0.8时，虽然不能绕过障碍物通过狭窄空间，但也不会与障碍物发生碰撞。这是因为膨胀半径参数选取较大，智能小车判定通过不了狭窄空间，所以终止了命令。而膨胀半径参数为0.6和0.4时，小车没有通过狭窄空间，同时与障碍物发生了碰撞，是因为此时智能小车判断可以通过障碍物，但由于选取的膨胀半径参数不合适，与实际场景产生偏差，导致小车与障碍物相碰。综上所述，膨胀半径参数为0.2是最适合本智能小车在当前环境下工作的参数，既可以在全局路径规划时与周围的环境保持足够的安全距离，同时对空间的利用也相对高效，而且进行局部路径规划时，也能保证智能小车绕过障碍物通过足够狭小的空间。

5 结语

本文针对智能小车自主导航时在狭窄、紧凑的工况下通过性较差的问题，提出了智能小车代价地图通用配置文件的膨胀半径参数是决定智能小车在狭窄环境下通过性能的决定性参数的假定，并通过实验方法予以验证，找到了最适合所研究环境的膨胀半径参数，同时也证实了不同的智能小车或机器人在不同环境下的参数设置不能简单的统一处理。在未来的物流领域发展中，单线激光雷达SLAM将会占据越来越重要的地位。因此，本实验研究对于智能小车在狭窄紧凑环境下的导航问题具有一定的指导意义。