白 创,闫 昱,陈 立

(长沙理工大学物理与电子科学学院,湖南 长沙 410114)

0 引言

为了确保室内机器人的人机交互的安全性和稳定性，更精确的移动机器人导航系统必不可少[1]，室内机器人导航系统可以分为同步定位与建图(simultaneous localization and mapping,SLAM)[2]、重定位和路径规划3大模块。黄通交等[3]设计了一种移动机器人系统，使用Cartographer算法[4]进行室内建图，但是在Long-Corridor场景下建图精度较差；Xu等[5]为了提高在Long-Corridor场景下的建图精度，在Cartographer算法中增加延时判断模块，过滤错误的回环点的方法，但是提升的精度不高；Wang等[6]提出了一种基于机器人操作系统(robot operating system,ROS)[7]的自主跨层导航系统，使用自适应蒙特卡罗定位(adaptive Monte Carlo localization,AMCL)算法[8]进行定位，但是定位的耗时较长；Lim等[9]提出了一种利用声纳数据对移动机器人进行重定位的方法，定位耗时短，但是需要对环境中的每一种物体进行物理建模，否则会出现定位错误；张明岳[10]对室内自主移动与导航机器人展开研究，在路径规划采用A*算法[11]，但是具有冗余拐点较多、搜索时间较长的问题;Szazaerba等[12]提出了一种稀疏 A*算法，在基本 A*算法中添加了搜索约束条件，提高了算法的搜索效率，但是没有对冗余拐点进行优化。

因此，本文基于机器人操作系统，对3大模块同时进行优化，实现一种高性能室内移动机器人导航系统。在SLAM模块中，选择雷达、里程计和IMU这3种数据融合的方式，极大提高了Long-Corridor场景下建图精度；在重定位模块中，采用一种基于扫描匹配的方法，减少了重定位耗时；在路径规划模块中，提出一种改进A*全局规划算法，缩短了算法搜索耗时并去除了路径冗余拐点。

1 多传感器融合的Cartographer算法

在室内Long-Corridor场景下，先用扩展卡尔曼滤波(extended Kalman filter,EKF)算法[13]融合里程计数据和惯性测量单元(inertial measurement unit,IMU)数据，再将融合后的数据与激光数据经过EKF算法再次融合进行位姿估计，即可得到较为精准的定位，从而提高Cartographer算法在Long-Corridor场景下建图精度。下面以EKF算法融合里程计数据与IMU数据为例说明。

室内机器人采用二轮差分小车，其运动方程可表示为

(1)

将运动方程扩展到EKF下对应的系统状态转移方程可表示为

(2)

里程计与IMU的观测方程可以分别表示为：

(3)

(4)

Hot为里程计的观测矩阵；Hit为IMU的观测矩阵；Qot为里程计数据的观测误差；Qit为IMU数据的观测误差；I6为单位6阶观测矩阵。

将IMU和里程计的观测方程整合到一起，即可得到总体的观测方程，即

(5)

Ht为总体观测矩阵；Qt为总体观测误差。

式(5)展开后可表示为

(6)

通过将状态转移方程、观测方程和误差方程代入到EKF算法中，可得到里程计与IMU这2种传感器的融合数据，然后用同样的方法将此数据与雷达数据进行融合，即可得到最终的最优位姿估计值。

2 基于扫描匹配的重定位

重定位属于SLAM与路径规划的中间一环， 它主要用于解决机器人遭遇“绑架”和路径规划起点与建图起点不一致时，导致的路径规划失败问题。传统的重定位方法是采用AMCL，它是一种概率模型定位系统，通过将粒子分布在全图，然后在每个粒子的位置上模拟传感器，将传感器信息与观测到的信息进行比较，由于粒子数过多，收敛需要非常长的时间。

Cartographer在建图时会构建许多由多个Scans组成的Submap，扫描匹配是通过将当前帧的雷达数据与Submap进行匹配，来定位当前的位姿，从而达到重定位的目的。此问题可以转化成优化问题来求解，优化问题可以看成一个非线性最小二乘问题，表达式为

(7)

ξ*为机器人的真实位姿；Tζ为变换矩阵，能把所有的Scan中的点hk从Scan坐标系下变换到Submap坐标系下；M函数为双三次插值函数，它把每个Scan中的雷达点变成了是否在Submap中的概率值[14]，每个雷达点的概率值通过函数M变换后如果都能趋近于1，说明这帧Scan很好地匹配了某个Submap，此时便得到机器人的当前位姿。

3 基于改进A*算法的全局规划

A*算法是常见的全局路径规划算法，是一种基于Dijkstra算法[15]的改进算法，但是存在着搜索时间过长以及拐点较多等缺点。A*算法分别考虑了移动机器人出发点到当前节点的真实代价和当前节点到目标点的估计代价，算法的代价计算公式为

f(n)=g(n)+h(n)

(8)

n为机器人的当前节点；f(n)为全局估计代价；g(n)为出发点到节点n的真实代价；h(n)为从节点n到目标点的估计代价。

A*算法的优化主要分为2方面：对搜索时间的优化；对路径的多余拐角进行优化，减少不必要的转弯。为了优化算法搜索时间，为h(n)增加权重参数q(n)，因此全局评估代价计算公式为

f(n)=g(n)+h(n)·q(n)

(9)

当q(n)取较小的值时，A*算法接近于迪杰斯特拉算法(Dijkstra)算法，会导致搜索速率过慢；当q(n)取较大的值时，A*算法接近于深度优先搜索(depth first search,DFS)算法，虽然搜索速度快，但无法找到最优路径。q(n)取合适的值，可以在不增加路径长度的同时提升搜索速度。为了得到最佳的q(n)取值，在MATLAB下进行仿真，分别选取5组不同的起点和终点，选择不同的参数进行测试，测试结果如图1所示。

从图1可知，当q(n)取1.2时，可以在不增加路径长度的前提下，使搜索时间减少22.79%。当q(n)取值大于1.2时，虽然搜索时间会进一步减少，但是路径长度增加，从而无法得到最优路径。

图1 q(n)取值对A*算法的影响

传统A*算法计算的路径会产生多余的拐角，消除多余拐角可以提高机器人行进效率，图2为了优化路径多余拐角，增加拐角优化函数，方法流程如下：

a.从开集中按序列找到总代价最小的点。

b.得到这个点对应的父节点的索引值，进而得到父节点储存的方向信息。

c.根据方向信息得到期望的点的方向是其反方向。

d.判定这个期望点的总代价是否跟原来那个点相同。

e.如果是则替换原来的点，否则跳过这个点；回到步骤a。

在MATLAB下进行仿真，A*算法拐角优化前后对比结果如图2所示。可以看出,1号和2号位置处的拐角优化后由3个减少为1个，这样规划出来的路线更加符合实际环境下机器人运动轨迹。

图2 A*算法拐角优化前后对比

4 实验测试和分析

将文中算法部署到室内机器人上进行实验分析。图3是基于ROS的分布式系统结构，采取分布式配置是因为机器人硬件性能有限。上位机选择联想笔记本R7000，其CPU主频为3 GHz，硬件上选择树莓派4b作为工控板，STM32作为底层驱动板，传感器上选择里程计、IMU和激光雷达。STM32采集传感器数据后传递给树莓派，树莓派再通过ROS通信传递给上位机，上位机再做出决策来控制机器人运动。实验分为建图、重定位和路径规划，在长走廊场景下进行建图测试，在实验室场景下进行重定位和路径规划测试。

图3 基于ROS的分布式系统结构

4.1 建图测试

在Long-Corridor场景下进行建图测试，开启建图节点后，通过Rviz 软件得到可视化的2D 栅格地图，如图4所示。图4a是只含激光雷达数据的地图，其中矩形框可以看出明显的错误，矩形框中的地图部分与实际环境的差异较大，这是因为Long-Corridor场景下的局部环境基本重复，特征点较少，激光雷达无法准确感知到环境的改变。图4b是含有激光雷达、里程计和IMU这3种数据的地图，可以看出方框部分的地图得到明显的改善。由实验可知，机器人在Long-Corridor场景下，比较依赖于IMU和里程计传感器数据，这是因为IMU 数据能提供比较精确的角度增量，而里程计数据则能得到比较精确的线速度，和激光雷达数据融合后得到的地图更符合真实场景。

图4 不同传感器的2D栅格地图

4.2 重定位测试

图5是实验室环境2D地图，在ABCD这4处位置分别基于扫描定位方法和AMCL的方法进行重定位实验，其中Z位置是建图的起点，也是小车重定位默认初始位置。

图5 实验室环境地图

以B点为例说明重定位过程，将小车放置在B点，开启重定位功能，此时机器人默认在Z点，控制机器人前后左右移动一段距离，机器人便会重定位至B点。如图6a所示，开启AMCL节点后，粒子分布于全图，机器人位置初始化为地图中间。如图6b所示，当粒子基本收敛到机器人周围时，说明重定位成功。图7a是机器人开启扫描匹配节点后，会不断将当前帧Scan与Submap进行匹配，此时可以看出局部地图未与全局地图重合。图7b所示，当局部地图与全局地图重合时便说明重定位成功。每处位置进行5次实验，然后取其耗时平均值，结果如表1所示。

图6 基于AMCL重定位过程

图7 基于扫描匹配重定位过程

表1 重定位实验平均耗时对比

由表1可知，扫描匹配平均耗时比AMCL减少80.43%，大幅度减少了重定位所花费的时间。因此，在使用重定位功能时，采用基于扫描匹配的方法，定位效果好，并且速度更快。

4.3 路径规划实验

路径规划实验分为静态环境实验和动态环境实验，静态环境是为了验证机器人在无外界干扰情况下的导航能力，动态环境测试是为了验证机器人在室内环境下的紧急避障能力，不同环境下的实验过程分别如图8和图9所示。

图8 静态环境实验过程

图9 动态环境实验过程

图8a中的路线是机器人调用改进后的A*算法规划出来的全局路径，图8b是机器人检测到静态障碍物后，调用ROS中自带的动态窗口法(dynamic window approach,DWA)[16]进行避障。图9a是机器人正在按全局路径行进，图9b是当机器人检测到突然放置的障碍物时，调用DWA算法进行的紧急避障，图8c和图9c是实际场景。2种实验环境下机器人都成功完成路径规划，证明了机器人的避障能力可满足室内基本要求。

5 结束语

基于树莓派4b和单片机STM32设计了一款室内移动机器人导航系统，该导航系统具有低成本、高性能、可移植性高等特点。