安防机器人的自主导航方法设计与实现

张嘉诚，刘文汇，袁吴越，王泽坤，巢 渊

（江苏理工学院 机械工程学院，江苏 常州 213001）

室内安防机器人通常工作在半结构化环境中，其环境布局多为长条形、回字形走廊，由平整且垂直于地面墙壁组成的空间、允许人通过的宽敞通道、桌椅支撑腿组成的狭窄低矮通道以及连接楼层之间的台阶等构成。为适应该环境下机器人的移动，其底盘发展出了如轮式、履带式、腿足式等多种形式。其中，麦克纳姆轮（Mecanum）由若干个与轮毂轴线存在45°偏置角的辊子围绕中心轮组成，辊子与地面产生与辊子轴方向相反的摩擦力，该摩擦力可分解为沿底盘前进方向和垂直于底盘前进方向的两个分力，通过调整分力大小可以实现底盘全向移动。

移动安防机器人在未知室内环境下工作时，需要自主决策运动路径、巡视整个空间范围并构建完整环境地图，以实现无人干预的自主导航。而上述环境通常具有面积小、结构化、动态情况少等特点，因而更侧重于依靠机器人自身传感器实现静态环境的地图构建与导航。近年来，同步定位与建图（simultaneous localization and mapping，SLAM）技术广受关注，它有效推进了对复杂场景的描述和理解，使导航更加趋于智能化。由于实现准确导航与避障的前提是获取精确的室内二维栅格地图；因此，本文应用基于SLAM方法的定位与导航技术，实现精确定位与室内地图的构建。常用SLAM包含视觉与激光雷达两个方向：经典视觉SLAM算法有ORB-SLAM[1]、SVOSLAM[2]等，其实现方式主要有基于点云数据、深度相机方法与位姿变化比较方法的距离计算，以及基于单目、双目或鱼眼相机方法[3]等；激光雷达SLAM技术目前较为成熟，经典方法有GMapping[4]、Hector[5]、Karto[6]和Cartographer[7]等，能够实现2D和3D建图[8]。

安防机器人在对室内环境建图过程中，需通过算法规划全局路径与局部路径。常用全局路径规划算法包括Dijkstra算法、RRT算法与A*算法等。Dijkstra算法通过集合间的判断与遍历，以广度优先寻找最短路径，由于遍历结果较多导致效率较低；RRT算法（Rapidly Exploring Random Tree）路径规划具有快速性与随机性，但随机点的生成过程易陷入局部最优；A*算法采用启发式搜索方法，在使搜索方向靠向目标点的同时，兼顾了优先搜索路径代价较小的点，省去无意义节点的计算，从而能够有效提高算法效率。全局路径规划能够获取完整路径轨迹，但其不适用于快速避障，且路径规划计算量较大，若因局部路径变化而反复调整全局路径还将降低导航效率。局部路径规划可使用全局地图的一部分来计算最佳运行轨迹，从而减小计算量，提高机器人的反应能力。应用动态窗口算法可实现局部路径规划，提高导航效率。

1 安防机器人实验平台

本文设计的安防机器人实验平台，由四个直流减速编码电机驱动麦克纳姆轮实现全向移动，配备12 000 mAH电池向机器人系统供电。以树莓派作为核心数据处理器；TF卡用于存储树莓派镜像和地图等信息；OpenCRP4是实验平台控制器；用RPLidar A1型激光雷达进行环境扫描与信息采集，并将结果传输给树莓派。由安装了ubuntu 18.04虚拟机的上位机（计算机端）通过WiFi模块实现与树莓派的通讯，再由树莓派将采集到的数据信息传给计算机端，计算机将相应的控制指令传送给机器人。

2 安防机器人定位与建图方法设计

2.1 定位方法设计

自适应蒙特卡洛定位算法（Adaptive Monte Carlo Localization，AMCL）是基于粒子滤波的一种不受环境限制的2D概率定位算法，它是对仅基于已知环境地图的估计机器人位姿的蒙特卡洛算法的一种改进。在ROS系统中使用AMCL算法来跟踪估计任意2D环境下机器人的位姿。AMCL算法通过一系列加权粒子来表示机器人估计位姿，当机器人移动到下一个位置时，新粒子根据上一时刻粒子的权重重新生成，且权重越高生成概率越大，在舍弃低权重粒子的同时能够随机生成新的粒子。粒子的权重在机器人运行过程中通过运动模型、传感器模型和实时数据不断改变。通过粒子的不断迭代就能使其逐渐收敛到一个区域，从而获得机器人最大概率的位姿。通过重采样和库尔贝克-莱布勒散度方法交替进行采样[10-11]。

2.2 建图方法设计

GMapping算法的实现基于滤波SLAM框架，在获知传感器与里程计数据的情况下推算机器人位姿和地图信息，结合条件联合概率分布与贝叶斯公式[12]，可表示为以下概率形式：

其中，算法参数描述为：M-地图，U-里程计信息，X-机器人位姿，Z-传感器数据。

式（1）将机器人的轨迹估计转化为一个增量估计的问题。其中：p(Xt|Xt-1,Ut)为基于前一时刻下机器人的位姿状态和这一时刻下里程计信息估计当前时刻机器人位姿状态；p(X1:t-1|U1:t-1,Z1:t-1)使用上一时刻的粒子群表示前一时刻下机器人的位姿状态；每个粒子都使用p(Xt|Xt-1,Ut)作为运动学模型进行状态传播，来得到粒子对应的预测轨迹；对每一个传播之后的粒子，使用p(Zt|Xt)作为观测模型进行权重计算，并做归一化计算；p(M|X1:t,Z1:t)表示使用当前时刻的机器人位姿和当前时刻的传感器数据进行地图构建。至此，可完成基于GMapping的地图构建过程。

3 安防机器人路径规划方案设计

3.1 全局路径规划方案设计

A*算法通过节点化的地图，应用启发式算法得到到达终点的最少代价的路径。在该地图中，每个栅格被标记为可通过状态和不可通过的节点。通过定位运算获取自身在全局地图中的位置，并指定全局地图中的某一点作为导航终点。计算过程中取水平方向和竖直方向之间相邻节点的距离为1，对角线方向的距离为1.4。定义节点A为起点，B为终点。定义数组open_set和close_set分别用于存储待处理节点和已处理节点。定义评价函数：

其中：dist_abs表示不考虑障碍物所占节点情况下两点之间的距离；dist_estimate为考虑障碍物时的距离。Cost函数值越小表示机器人移动到目标点的代价越小。在计算Cost函数时，仅需考虑从点到点之间的移动代价，因此，选择曼哈顿距离作为计算距离有利于提高路径计算效率。

基于上述理论，A*算法的实现流程如图1所示。

图1 A＊算法实现流程图

3.2 局部路径规划方案设计

动态窗口算法是在ROS中常用的局部路径规划方法，其原理为：在速度空间(v,ω)中采样多组速度，并模拟在这些速度下一定时间内的轨迹；在得到多组轨迹后，对这些轨迹进行评价，选取最优轨迹对应的速度来驱动机器人运动。算法突出动态窗口的特点，依据移动机器人加减速性能，限定速度采样空间在一个以当前速度为中心、以诸多限制下能达到的最大速度为左右区间长度的动态窗口范围内。使用全向移动小车进行基于动态窗口算法的局部路径规划，其流程如图2所示。

4 实验与分析

4.1 定位方法验证

用于测试机器人实验平台的真实家庭住宅场景如图3所示。ACML算法在该场景的实现效果如图4所示，红色箭头表示粒子对机器人位姿的估计信息。可以看出，实验平台能够快速准确地定位到机器人的全局位置，图中粒子聚集在一小块区域即为理想定位情况。

4.2 建图方法性能对比与分析

本文基于slam_mapping ROSpackage[13]实现SLAM功能，通过可视化软件实时监测机器人建图过程。应用Cartographer算法与GMapping算法针对图3场景进行仿真测试，结果分别如图5、图6和表1所示。

图2 动态窗口算法实现流程图

图3 用于测试实验平台的真实场景

图4 ACML算法可视化模型

图5 GMapping建图效果

图6 Cartographer建图效果

表1 室内环境下建图算法比较

实验结果表明：Cartographer算法计算量较大，建图耗费时间较长，不宜在室内安防场景中使用；而GMapping算法适合小范围且地面平坦环境，能以较低设备成本获得精度较高的地图，但算法依赖里程计，且缺少回环检测，因此更适用于地面平坦的室内环境[14]；使用GMapping算法进行建图，在没有回环检测的情况下能够清晰地检测到建图空间的边界；而Cartographer算法建图需要通过一系列子图对比回环检测，在实际操作过程中要对单一区域重复检测才能使得某一块区域的环境显示清晰，因而影响了建图效率。

4.3 路径规划方法验证

综合考虑搜索效率和路径有效性，本文基于navigation_melodic_devel导航框架功能包[15]，采用A*算法实现机器人在如图3场景的全局路径规划与导航，结果如图7、图8所示。图7中红色路径表示全局路径，绿色路径表示局部规划路径。图8表示实验平台能够根据上位机端发布的导航点，自主规划出一条通往该点的路径，从而验证了本文路径规划方法的有效性。

5 结论

图7 路径规划结果

图8 到达目标点结果

为了实现室内安防机器人的建图与导航，本文设计了一种针对室内场景的通用移动安防机器人平台。通过机器人麦克纳姆轮底盘与实验平台、定位、建图与路径规划方案等的设计，采用自适应蒙特卡洛定位算法、基于GMapping算法的定位与建图方法、基于A*算法的全局路径规划方法，较好地实现了定位、建图与全局路径规划功能，并在导航过程中通过动态窗口算法实现避障。实验结果验证了所提出方法的有效性，显示出本文方法在建图与导航效率方面优于对比算法，具有良好的应用前景。