移动机器人平台低成本自主导航系统设计与实现

1 引 言
地面移动机器人具有运动稳定性高、承载能力强、机械结构简单等特点，能够在复杂恶劣环境中代替人类执行特定任务，已经成为当前的研究热点[1-2]。移动机器人在楼宇巡逻、地面清洁、仓储货运等场景中都具有巨大的应用价值，可以极大丰富人们的生活体验。自主导航、自主规划以及地图构建共同构成了移动机器人系统的核心，是移动机器人实现智能化、自主化的前提。

常见的移动机器人通常采用多传感器组合导航的方式，Zheng等[3]采用激光雷达、惯性测量单元以及 GPS 多传感器融合的方法解决轮式机器人的导航问题。陈智伟等[4]利用激光雷达、相机和惯性测量单元（Inertial Measurement Unit，IMU）组成车载多传感器系统获取环境信息，通过训练 RBF 神经网络对环境进行建模。李中道等[5]提出一种基于超宽带与激光雷达的融合定位算法，算法基于粒子滤波对两个传感器的数据进行互补融合解算，以提高定位精度。上述方法使用多传感器进行组合导航虽然有助于提高系统定位精度，但同时也增加了系统的复杂性，对机载计算机的运算能力提出更高的要求。

根据使用传感器的类型，常用的定位方法大致可以分为两类，基于激光雷达的定位方法和基于视觉的定位方法[6]。Gmapping[7]、hector slam[8]等采用单线激光雷达作为输入，是一种二维SLAM 算法。 LOAM[9] 、 LeGO-LOAM[10] 、LIO-SAM[11]、LOAM_livox[12]等主要针对多线激光雷达的三维 SLAM 算法，可以输出传感器在三维空间中的位姿。VINS-mono[13]、Open-VINS[14]等融合视觉信息与 IMU 信息实现定位。ORB-SLAM2[15]是一种基于 ORB 特征点的几何SLAM 框架，支持单目、双目以及 RGB-D 相机。

在移动机器人的路径规划中，A算法具有广泛应用。许多学者基于 A算法提出了不同的改进方法，Ju 等[16]提出了一种基于人工势场法和 A算法的融合算法，提高了 A算法的动态避障能力。张志文等[17]使用 JPS 算法提升 A算法节点扩展效率，利用贝塞尔曲线对规划路径进行平滑处理，提高了 A算法的规划能力。杨瑶等[18]将车辆的运动学模型融入 A*算法的搜索过程，采用自由边界三次插值平滑路径，生成更加符合车辆运动特性的轨迹。上述方法都是在二维平面进行规划，没有充分利用三维环境信息。

本文提出了一种基于单一传感器的低成本方案解决移动机器人在室内未知环境中自主规划与地图构建问题。该方案选取廉价的 RGB-D 相机作为环境感知传感器，利用 RGB-D 相机提供的环境深度信息和 RGB 图像信息实现自主定位，实时解算移动机器人的位置和姿态，无需额外的里程计(车轮编码器)或者 IMU 等传感器。此外，利用 RGB-D 相机采集的点云数据，可以构建未知环境的三维地图，完成对未知环境的探测。为提高移动机器人在未知环境中运动轨迹的安全性和平稳性，本文基于 A算法提出了一种适用于二维平面运动物体的三维规划方法。首先，根据RGB-D 相机提供的点云数据构建局部 3D 占据栅格地图，在三维空间中进行碰撞检测，提高规划路径的安全性；其次，改进 A算法节点扩展方式，将节点的搜索过程转移到状态空间，提高规划路径的平滑性；最后，基于 B 样条曲线构建优化方程，进一步提升初始路径的安全性和平滑性，保障移动机器人在未知的环境中平稳安全移动。

本文设计并实现了一种基于单一传感器的自主系统，该系统能够感知非结构化的环境，并且提供足够精度的自定位信息。系统采用体积小、重量轻、低功耗的 Intel NUC 作为主计算单元，易于部署到小型自主移动平台。此外，NUC 的运算能力支持扩展更多的功能，如物体的检测与识别等计算机视觉任务。系统的软件框架基于机器人操作系统（Robot Operating System, ROS）搭建，具有较强的灵活性，可以兼容不同的硬件设备，使得系统支持不同的硬件解决方案成为可能。此外，本文提出的软件算法求解速度快，不需要消耗太多的计算资源，易于部署到不同的运算平台。本文提供了包括软件部分和硬件部分在内的完整系统框架，以及该系统在现实环境中的应用效果。

与现有的工作相比，本文提出的系统采用单一传感器作为输入，使用低成本的相机实现移动机器人自主系统的构建。为解决无人系统在未知环境的自主避障问题，改进了传统的 A*算法，提高了系统的环境适应能力。此外，本文还提出了一些规划策略，以提高系统的实时性。本文的贡献总结如下。
（1）本文提出了一种可以在未知环境中实现自主规划与地图构建的低成本解决方案。其核心部件只有一个感知环境信息的 RGB-D 相机以及一台提供计算服务的机载计算机。
（2）本文提出了一种适用于二维平面运动物体的三维规划方法，充分利用三维环境地图，实现三维避障。
（3）本文提供了一套基于开源软件的系统框架，包含软件架构和硬件架构，便于快速部署。

2 系统概况

系统的软件框架基于 ROS 搭建，整套系统采用模块化的设计理念，按照功能划分为不同的模块单独进行开发，再按照相应的需求将不同的功能模块进行组合，完成整套系统开发，如图 1 所示。基于 ROS 的特性，将每个功能模块设计为一个节点，节点的运行是由 ROS Master 统一进行调度和分配，节点之间的通信采用 ROS 的话题机制，发布者将消息发布到指定的话题中，接收方订阅相同的话题，双方即可建立通信链路，完成信息传输。

系统使用了部分基于 C++编写的开源代码包，驱动包 realsense-ros[19]用于处理 RealsenseD455 RGB-D 相机的数据，将其转化为 ROS 标准信息类型，通过 ROS Topic 的形式发布。定位模块基于 ORB-SLAM2 算法设计，定位模块订阅ROS 相关的话题，获取 RGB 图像和深度图像，计算相机当前的位置和姿态，输出位姿矩阵Tcw ∈SE(3) 。Tcw 包含旋转矩阵和平移向量，位姿转换模块将矩阵Tcw 转换为 ROS 标准格式的里程计信息（ nav_msgs/Odometry ）和位姿信息(geometry_msgs/ PoseStamped)供系统其他模块使用。局部建图模块和轨迹规划模块基于Fast-Planner[20]框架搭建，局部建图模块接收RGB-D 相机的深度图像和里程计信息，生成局部3D 占据栅格地图用于路径搜索，同时生成 ESDF地图用于轨迹优化。由于传感器的感知范围有限，因此采用局部规划的策略，系统只在局部地图中进行轨迹规划，到达局部地图的边缘则停止规划，系统间隔固定的时间周期调用规划器以更新轨迹，随着机器人的移动逐步规划到目标点。运动控制模块结合期望轨迹信息和当前里程计信息计算误差量，采用 PD 控制器计算期望速度和角速度，实现轨迹跟踪，并将速度指令转换为 Twist消息类型（geometry_msgs/Twist），以 ROS Topic的方式发布，使得系统可以快速地移植到任意一种支持 Twist 控制指令的移动平台上。

3 系统关键算法分析
3.1 路径规划算法

路径规划是指移动机器人按照某一性能指标（如距离、时间等）搜索一条从起始状态到目标状态的最优或次优路径。常用的搜索算法有Dijkstra、A、RRT、RRT等，但是利用这些算法得到的路径往往是非光滑的。此外，传统的规划算法仅在传感器扫描得到二维平面上搜索，因此只能避开搜索平面内的障碍物，对于平面上方或者下方的障碍物难以有效规避。本系统采用的环境感知传感器为 RGB-D 相机，可以建立机器人周围环境的三维地图。因此，本文对传统的 A*算法进行改进，优化节点扩展策略，使得搜索的路径更加平滑，同时在三维空间中判断路径是否发生碰撞，实现三维避障。