室内移动机器人自主导航系统设计

林义忠，马 凯

（广西大学 机械工程学院，南宁530004）

移动机器人是机器人领域的重要分支，随着计算机技术的进步和硬件性能的提升，机器人正逐渐为人类分担起更多的任务。在日常生活中，移动机器人可以自主完成地面清洁、商场导购及酒店迎宾等任务；在工业生产中，移动机器人可胜任无人仓储货运和无人送货等岗位，伴随着无人汽车的出现，移动机器人的发展极大的丰富了人们的生活体验，给人们的生活带来了巨大的变化。

要想实现智能移动机器人在环境中的自主导航，首要就是要带领机器人认识环境，即环境示教，并将环境信息存储下来，这一过程称为地图构建。常见的地图构建方法有通过贴磁条等方式形成拓扑地图，基于几何特征关联的激光扫描匹配方法得到的几何地图，以及基于视觉的地图构建方法得到点云图或栅格图[1-4]。其中磁条等方式需经常维护，激光价格昂贵且需要通过布置反射板以提高精度，而通过视觉带领机器人感知环境不仅是最贴近人类认识世界的方式，同时，视觉可携带的信息更为丰富，可以为移动机器人带来更为广阔的应用空间。

本文主要从移动机器人的硬件、软件架构两方面展开研究，在ROS（robot operating system）框架下设计并实现移动机器人在室内复杂环境下的地图构建，并在此基础上实现移动机器人的自主导航。

1 传感器模型

1.1 相机几何模型

视觉传感器有多种类型，常用于移动机器人的视觉传感器有单目摄像头、双目摄像头、RGB-D 相机等类型[5]。单目相机结构简单，成本较低，但只能采集二维数据，无法直接确定物体的真实大小，具有尺度不确定性[6]，双目摄像头通过视差法计算出特征点在双目相机左相机坐标系下的坐标，并且左右相机间距越大时可测得的距离越远，但是通过视差法计算非常占用计算资源[7]。RGB-D 相机可同时提供彩色和深度图像信息，与其它相机相比，不仅避免了在获取深度信息时不必要的计算，而且RGB-D 相机在拍摄时连续性好，环境信息更加丰富，适合用于移动机器人进行环境感知[8]。

RGB-D 相机的几何模型可用来描述将三维坐标映射到二维平面的过程，通过相机标定过程得到相机的内外参数，进一步解算出像素坐标系下的点与世界坐标系中点的坐标转化关系，如图1所示。

图1 相机成像模型Fig.1 Camera imaging model

式中：为相机内参；M=［RT］为相机外参。

1.2 里程计模型

里程计是一种常用于轮式机器人相对定位的传感器[9]，可以通过安装在左右电机上的光电编码器来获取里程数据，然后根据机器人的轮子半径和两轮间距进而计算出机器人的实时速度和位姿信息，其运动模型如图2所示。

图2 里程计测量模型Fig.2 Odometer measurement model

记移动机器人在某一时刻的移动速度与水平方向的夹角为α；速度为ν；角速度为ω，则任意时刻机器人的位姿可表示为

2 移动机器人自主导航系统

2.1 基于Gmapping 的二维栅格地图构建

Gmapping 是基于RBPF 粒子滤波器构建算法（rao-blackwellized particle filter，RBPF）的改进[10]。根据贝叶斯法则，RBPF 将定位和建图分离，实现先定位再建图，大大降低了SLAM 模型的复杂度，此过程如式（3）所示：

式中：P表示需要描述的分量；x代表移动机器人位姿；M代表环境地图信息；Z代表观测信息的集合；U代表运动控制输入的集合。

在针对RBPF 算法需要大量粒子拟合目标分布且多次重采样容易导致粒子耗散的问题上，Gmapping 在RBPF 的基础上提出了改进提议分布和选择性重采样，从而减少粒子个数和防止粒子退化[11]。在减少粒子个数方面，考虑到通常激光测得的数据比里程计精确，观测模型的分布小于里程计估计分布，在仅以里程计运动信息作为先验位姿估计的基础上，将激光信息也融入到提议分布中以便更接近目标分布，改进后的提议分布函数为

在防止粒子退化方面，通过计算粒子权重，并设定阈值，将粒子权重与阈值比较，进行选择性重采样，既保证了粒子权重，也减少了粒子重复采样的次数，从而避免粒子退化。粒子权重计算方法为

Gmapping 算法以激光数据和里程计数据作为输入，基于运动模型和观测模型实现同步定位与地图构建。其核心在于将机器人置于环境中的任意起始位姿后，通过驱动机器人移动获取环境的先验信息，并通过观测模型将获取到的观测信息与先验信息进行匹配，实现对里程计由于打滑等原因造成数据误差的实时修正，完成对机器人的定位，然后基于机器人的实时位姿构建二维栅格地图。

ROS 中通过Gmapping 构建地图的数据流程如图3所示，里程计发布的数据传入到STM32 控制开发板得到底盘移动的距离和转动的角度，通过USB通信的方式将数据传到上位机中，完成里程计信息的输入。RGB-D 相机需要先将相机获得的深度图转化为伪激光扫描信息，其核心原理为将深度图中的像素经过坐标转换得到世界坐标系下的投影点到相机光心之间的距离[12]。以获得的里程计信息和激光扫描信息作为Gmapping 的输入参数，经过复杂的运算后得到地图信息，最终将地图信息显示在上位机的Rviz 中。

图3 地图构建系统主要的数据流程Fig.3 Main data flow chart of the map building system

2.2 路径规划

移动机器人的路径规划指的是基于对环境信息的掌握情况，结合移动机器人自身的定位信息，搜索出一条从起点到目标点的无碰撞并尽量最优的路径。路径规划通常有全局路径规划和局部路径规划[13]。常用的路径规划算法包括人工势场法、蚁群算法、A*算法及DWA 算法等[14]。其中全局路径规划是基于静态环境下的路径规划，即在先验地图模型的基础上，给机器人规划出一条到达最终目标点且满足一定要求的全局路径。A*是一种典型的全局路径规划算法，通过引入启发函数使整体性能相比于Dijkstra 有了较大改善，其数学描述可表示为

式中：f（n）指从起始点经过中间节点n到达最终目标点所经过的这条最优路径的估值函数；g（n）表示从初始点到某一时刻的节点n的实际代价函数；h（n）是从节点n到目标点的代价估值函数，又称启发函数。

局部路径规划侧重于实时避障，对于环境中出现的动态障碍物或者当环境特征出现较小的改动时，通过机器人上搭载的视觉传感器对环境的实时感知，在基于静态先验地图模型的基础上，添加障碍物信息，形成障碍物层以实现环境信息的动态更新，局部路径规划基于障碍物图层，规划出实时路径避开障碍物。DWA 算法是通过添加约束条件并取其交集，将移动速度限定在一个小范围内，如式（7）所示：

然后通过预估一定时间内相应的轨迹，结合轨迹评价函数G（ν，ω）确定最终的规划路线。

式中：h（ν，ω）为方位角评价函数，用来评估若移动机器人按照当前采样的速度移动，在到达预测轨迹末端时的朝向与目标方向的一致性；d（ν，ω）为间隙评价函数，评估移动机器人按照预测的轨迹运动到目标点时与障碍物间的最小距离；v（ν，ω）为速度评价函数，评估当前轨迹的速度大小。

本文结合A* 算法和DWA 来进行移动机器人的路径规划，通过A* 算法搜索出从起点到目标点的路线信息，结合DWA 实现动态避障，使移动机器人具备自主导航的能力。

3 系统构建与实验结果

3.1 硬件平台和系统架构

本文设计的自主导航系统主要由机器人底盘、RGB-D 相机和上位机组成，其中机器人底盘采用Yujin 公司开发的Kobuki 移动机器人，该款机器人底盘采用两轮差速驱动，内部配备了STM32 控制开发板，并配有外接扩展口，可通过USB 通信的方式与上位机进行通信；RGB-D 相机选用华硕的Xtion Pro Live，其有效视距为0.8~3.5 m；上位机为一台Intel Core i5-3210M，8 GB 内存的笔记本电脑，并预装有ROS 操作系统，主要承担着算法运算和指令发布等工作，搭建的移动机器人实验平台如图4所示，自主导航系统框架如图5所示。

图4 移动机器人实验平台Fig.4 Mobile robot experimental platform

图5 移动机器人自主导航系统框架Fig.5 Framework diagram of mobile robot autonomous navigation system

3.2 移动机器人地图构建

首先在ROS 系统中驱动Kobuki 机器人底盘节点和键盘控制终端，建立PC 与底盘之间的控制关系，然后加载Gmapping 算法并通过键盘终端发布机器人移动的速度话题，底盘节点订阅速度话题并按照指令在环境中移动，伴随着底盘的移动，在移动机器人经过的地方，可在Rviz 中观察增量式地图构建的过程，其中白色区域为无障碍区，黑色区域表示有障碍物，其它区域表示未知区域。当移动机器人遍历整个环境后，便可得到整个环境的二维栅格地图信息。实际环境与栅格地图如图6所示，最终所建立的栅格地图如图6（b）图所示。

图6 实际环境与栅格地图Fig.6 Actual environment and grid map

为了定量的描述栅格地图的精度，在栅格地图和真实环境的横向和纵向2 个方向上分别取3 个不同的点对进行测量，每个点对在栅格地图中和真实环境中都测量5 次，取其平均值并记录于表1中，最后将栅格地图中测量平均值与真实环境中测量结果的平均值进行对比，可以从横向和纵向两个维度衡量出栅格地图的整体精度。

表1 地图点坐标记录Tab.1 Map point coordinate record

3.3 移动机器人自主导航

完成环境地图的构建后，在上位机中加载构建的二维栅格地图，通过基于粒子滤波的自适应蒙特卡洛算法对机器人进行定位，然后给定目标点坐标信息，便可规划出一条从起点到目标点的路线，如图7所示。

图7 全局路径规划Fig.7 Global path planning

当环境发生小的变化或有障碍物突然出现在所规划的路线前方时，通过RGB-D 相机识别障碍物的距离信息，及时地更新障碍物图层，移动机器人将根据新的障碍物图层重新规划局部路线以绕开障碍物并继续向着目标点运动，避障对比过程如图8所示。

图8 局部路径规划Fig.8 Local path planning

4 结语

本文首先对SLAM 技术进行了研究，然后完成了移动机器人软硬件平台的搭建，并驱动移动机器人在真实环境中完成地图构建，最后结合A* 算法和动态窗口法对移动机器人进行路径规划。实验结果表明，该自主导航系统在真实场景下建立的二维栅格地图基本能够满足实际需求，并且基于该栅格地图可以实现移动机器人的自主导航，提高了移动机器人在未知环境中的自主性。