ROS2学习笔记_01——泛读论文《基于ROS2平台的室内移动机器人导航系统设计与研究》

论文链接：https://kns.cnki.net/kcms2/article/abstract?v=FqYZq-Q0wRQ9PDUlUWwfn9pxvQNda6HlGlCAHfZLc4JjUnWZFgUQcvAQZRAyY63QhUFianpKrYUfWW5NwuPNB2OZWHm95_mbQy4PzlWoMqw37uEdR-VbHmg2EWq_FXH08XX1kNB0qC8E900TH7lkhdtusx9hWVp_0n9sAEV4VRR2e8sSYqsaJikedows8XKmJQnf35Wmm-c=&uniplatform=NZKPT&language=CHS

移动机器人导航技术的核心是机器人实现即时自主定位、地图构建、路径规划三个方面。

1、研究现状

1.1 SLAM研究现状

1.1.1 传感器的发展

（1）视觉SLAM：

单目相机：提供没有深度信息的RGB图像，用于收集高分辨率的地理标记或全景图像，无法生成高精度的3D位置。

双目相机：使用两台捕获同步立体图像的相机构成，成像原理类似于人眼。

深度相机（RGB-D）：使用结构光法或飞行时间法(ToF)采集物体的深度信息。深度相机可以提供RGB图像和深度图像，而视觉相机对于光照的依赖度高，不适用于高曝光或者低亮度的环境。

（2）激光SLAM：相较于视觉SLAM，测距精度高、噪声小、不易受光照条件影响。

机械式：使用旋转镜面，体积较笨重，系统惯性较大。

半固体：微机电系统（MEMS）

谐振式：使用简单的控制算法在高频下实现更大的扫描角度，缺点是扫描轨迹为正弦形，扫描速度不稳定。

非谐振式：使用相对复杂的控制算法，拥有更多自由度。

固体：

光学相控阵（OPA）：采用相位调制器来控制激光光束，通过激光发射器阵列中应用不同的相位来实现不同方向光的发射，接收器以同样的方式控制接收光。不需要任何移动部件，相较于机械式可以实现更快的扫描频率。

Flash：在短时间内发射覆盖整个环境的激光，用光电探测器收集反射光，可以快速地捕获数据，而且不易受到光失真影响。

1.1.2 SLAM算法

依据后端处理阶段采用的不同优化方法，可将激光SLAM分为基于滤波的方法和基于图优化的方法（基于图增加了回环检测过程）。

1.2路径规划研究现状

解决的问题：①使机器人能够从起点到达目标点；②使机器人避开障碍物；③在实现前两条的基础上，尽可能优化路径。

根据机器人对周围环境的感知程度和规划时所考虑的时间和空间范围不同，路径规划可分为全局路径规划和局部路径规划。

（1）全局路径规划算法：在静态地图上规划一条可行路径，以使移动机器人能够以最短时间、最安全的方式到达目标点，不考虑未知因素对生成路径的影响。

Dijkstra算法

A*算法

D*算法

采样路径规划算法：RRT算法等。

智能仿生算法：神经网络算法、蚁群算法、遗传算法等。

（2）局部路径规划算法：在未知、动态环境下规划最优路径。局部规划将环境中的不确定的、动态因素加入到路径规划范围内，使移动机器人安全到达目标点。

人工势场法（APF）

动态窗口法（DWA）

时间弹性带（TEB）

2、建图与定位

创新点：对Cartographer算法进行改进，提出了一种基于Fast_LIO2和ICP的纯定位方法。在现有的激光SLAM技术中，系统框架主要包括五个功能模块，分别为前端传感器数据的读取和预处理、里程计、后端优化和地图构建五个过程。常用的2DSLAM算法有Gmapping和Cartographer等。

2.1 2D激光SLAM算法

2.1.1 Gmapping算法

基于粒子滤波。首次将SLAM问题中的定位与建图两部分分开。适合于小场景中，拥有较高的精度，对于雷达扫描频率要求较低。

2.1.2 Cartographer算法

基于图优化。支持2D和3D，适合激光雷达、IMU、里程计、GPS等传感器配置，是当今应用最为广泛的SLAM方法之一。引入子图的概念，具有回环检测的功能。

2.1.3 改进Cartographer算法

建图中需要注意，机器人的运动速度不能过大，否则容易造成建图漂移和畸变。其中快速旋转使Cartographer算法建图更加难以相互匹配。

为了准确估计机器人的位置，本文通过IMU数据对里程计数据进行修正。使用的IMU型号为GY95T，该模块是一个低成本的数字九轴传感器，具有高性价比、高精度、可输出四元数等特点。通过虚拟串口获取IMU中的加速度和速度信息，利用扩展卡尔曼滤波将IMU数据与车轮里程计数据融合，生成新的机器人实际观测里程计数据，并以/odom发布。

改进后的Cartographer算法对于小物品、墙角和墙面的建图效果要优于改进前，边界更加清晰平整，减少了模糊区域的存在。

2.2 3D激光SLAM

2.2.1 相关算法

3D激光SLAM技术通过三维激光雷达捕捉环境的三维空间信息，生成点云数据，并利用连续帧的点云匹配计算来估计激光雷达的运动位姿，进而构建详细的三维点云地图。该技术可划分为基于滤波器和图优化两类方法，其中Cartographer算法作为图优化方法的代表，其原理与2D激光SLAM相似。然而，3D激光雷达点云的稀疏性和多视角扫描特性增加了匹配难度，且高数据生成率要求高效算法和强大计算能力。

激光里程计通过点云匹配算法估计当前帧的全局位姿，但受点云稀疏性和运动干扰影响，精度受限。为提升精度，研究者结合IMU数据，形成激光惯性里程计，通过IMU状态数据修正激光里程计预测，以缓解匹配难题。

FAST_LIO2作为一种先进的激光惯性里程计框架，采用高效紧耦合迭代卡尔曼滤波器，并创新性地引入增量kd树（ikd-Tree）数据结构，有效处理大规模点云数据，支持增量映射更新，以低计算成本实现高频里程计和点云地图构建，展现了在资源受限平台上的高性能潜力。

2.2.2 地面分割提取算法

在移动机器人应用中，鉴于其计算资源有限及3D激光雷达点云数据量大、无序且不规则的特性，直接进行点云扫描匹配面临高算力需求挑战。为此，本文提出一种优化策略，即通过特征值筛选与分割处理，剔除对匹配影响较小的数据点，特别是室内环境中特征值小、主要由平整地面构成的点云部分。这一过程称为地面提取分割，旨在从点云数据中精确识别并分离出地面点，以简化后续环境建模、导航及定位等任务的数据处理复杂度，提升算法效率与准确性。地面提取分割算法的核心在于有效区分地面点与非地面障碍物点，为机器人提供更为精简且信息丰富的环境表示。

由于该算法只利用了点云数据的低维特征进行分割，计算效率较高，适用于实时性要求较高的应用场景。但也存在一定的局限性，例如当地面不是平坦水平面时，该算法的分割效果可能会受到影响，需要根据具体情况进行调整和优化

2.3 机器人定位方法

2.3.1 自适应蒙特卡洛算法(AMCL)

是一种基于粒子滤波的定位算法，被广泛用于机器人、无人车等自主导航系统中。其核心思想是利用随机采样的粒子来表示机器人可能的位姿，然后根据传感器测量数据对这些粒子进行更新和优化，从而得到机器人在环境中的准确位置和方向。AMCL算法主要包括预测、修正和重采样三个关键步骤。

2.3.2 ICP纯定位

ICP定位就是利用迭代最近点(IterativeClosestPoint，ICP)算法在给定的地图中定位激光雷达传感器的位姿，继而解算出机器人的位姿。ICP算法是一种迭代式最近点匹配算法，用于将两个物体的点云数据进行对齐。ICP算法的核心思想是通过迭代计算找到两个点云数据之间的最小欧式距离，算法的目的是配准两帧激光点云信息，解算出上一时刻和当前时刻机器人的未知关系，其中误差函数为最小二乘法。

2.4 仿真实验与分析

2.4.1 建图算法仿真

在相同的配置环境下，改进Cartographer算法的性能较好，消耗内存相对Gmapping算法略多。在相同环境下，改进Cartographer算法的建图速度要慢一些，但地图质量更优。综合来看，改进Cartographer算法更适合室内移动机器人环境建图。

2.4.2 定位算法仿真

AMCL算法使用粒子滤波进行定位，意味着在初始定位时需要大量的计算资源和时间来收敛到准确的机器人位置。当机器人突然移动或者环境发生剧烈变化时，AMCL算法可能无法快速适应，导致定位失效，需要耗费大量时间重新定位，影响了机器人的实时性和稳定性。相对而言，ICP算法虽然需要大量计算资源，但却能够快速且准确地实现机器人的定位与跟踪。通过三维点云地图的匹配，ICP算法可以复杂环境中准确地跟踪机器人位置，即使遇到突发情况也能快速恢复定位，使得ICP算法在某些对定位准确性和实时性要求较高的应用场景中表现优异，例如自动驾驶、无人机导航等领域。ICP算法在自定位方面的精确度优于AMCL算法。本文使用ICP算法。

3、移动机器人路径规划方法

3.1 全局路径规划

3.1.1 Dijkstra算法

又称为广度优先搜索算法，是一种通过广度优先搜索寻找最短路径的图遍历方法。Dijkstra算法需要遍历地图中所有节点，当环境地图扩大时，Dijkstra算法的计算复杂度将增加，导致算法的运行速度显著降低，并且算法需要更多的资源来处理数据。因此，在面对复杂导航环境时，Dijkstra算法的运行效率较低，同时占用较多的内存空间。

3.1.2 A*算法

A*算法是求解静态环境中最短有效路径常用的路径规划算法之一，结合了Dijkstra算法和广度优先搜索(BreadthFirstSearch，BFS)的优势，引入了启发式方法来引导搜索方向和提升效率。该算法通过评价函数确定搜索方向，利用启发信息削减无用搜索，有效提高了搜索效率。

3.2 局部路径规划

局路径规划是在静态地图中求解出起点到目标点的最佳路径，但现实环境常常变化，可能会有新增的静态或动态障碍物，导致全局路径规划无法有效应对。因此，机器人需要根据传感器信息实时进行局部路径规划，以适应环境变化。该规划针对机器人当前所处的局部环境进行，确保机器人在环境发生变化时能够有效避免障碍物。

3.2.1 DWA算法

DWA（DynamicWindowApproach）算法是局部路径规划中的一项关键技术，它实时地根据移动机器人的当前位置与速度状态，在受限的可行速度空间(v,ω)内采样，生成一系列候选速度组合。随后，算法模拟这些速度组合下机器人未来轨迹的演变，并通过一个综合评估函数，考量目标趋近性、环境安全性及运动平滑度等关键指标，对候选轨迹进行评分。最终，DWA选择评价最优的轨迹所对应的速度配置，作为机器人下一时刻的执行指令。DWA算法包括三个主要环节：速度空间采样、轨迹预测、轨迹评价函数。

3.2.2 TEB算法

时间弹性带算法基于最优参数控制，把局部路径规划看作多目标优化问题，通过建立多目标优化函数，考虑了可行路径、障碍点约束、速度、加速度约束以及非线性运动学约束。针对实际情况对多目标优化函数进行加权并进行最优处理。

3.3 仿真实验与分析

3.3.1 全局路径规划仿真

Dijkstra算法能够找到最短路径，但搜索范围和计算资源呈平方级增长，规划时间也随之增加。虽然环境复杂度对规划时间影响不大，Dijkstra算法适用于小范围且复杂环境的场景。相比之下，A星算法通过引入评估函数减小搜索范围，从而缩短规划时间，但并不保证得到最短路径。随着搜索范围和计算资源的增长，Dijkstra算法能够稳健求解最短路径问题，而A星算法在规划时长方面具有一定优势，但不能保证最优解。A*算法适合大多数环境尤其是大范围、要求实时性高且环境简单的场景。通过上面的分析，A星算法作为机器人全局路径规划算法，更适合本文的室内移动机器人的要求。

3.3.2 定位算法仿真实验

使用TEB算法时，机器人能够更好的处理环境中的拐角，所规划的路径更加平滑和具有连续性，但是计算路径的时长要比DWA算法花费更多，并且在运动过程中会出现较大的速度与角度波动。虽然DWA算法可以较快的完成路径规划，但是处理动态障碍物时的避障效果不理想。因此，相较于DWA算法，TEB算法更适合于机器人的避障问题的解决。本文将采用TEB算法作为局部路径规划算法。

4、实体机器人验证

4.1 室内导航系统

4.1.1 系统设计

本文的室内移动机器人导航系统首先利用三维激光雷达采集原始三维点云数据，选择Fast_LIO2算法进行点云地图构建，并应用地面提取分割方法提取出地面。接着，通过PointCloudtoLidar算法将点云数据转化为雷达扫描数据。利用二维构图算法将雷达扫描数据构建成二维栅格地图，并通过ICP匹配算法实现机器人的纯定位。在二维地图上，利用A*算法进行全局路径规划，并结合TEB算法进行局部路径规划。

本文的移动机器人自主导航系统由三个模块组成： ① 地图构建模块。本模块主要功能是利用三维激光雷达获取环境位置信息，通过Fast_LIO2算法生成三维点云地图。利用地面提取分割算法、PoinCloudtoLidar算法以及改进Cartographer算法构建二维栅格地图； ② 机器人定位模块。使用ICP点云配准方法将实时点云数据与点云地图进行匹配，获得机器人实时位姿； ③ 路径规划模块。路径规划模块依赖于ROS2中的Navigation2导航框架。在栅格地图上利用A*算法进行全局路径规划，并使用TEB进行局部路径规划，输出速度命令。

4.1.2 软件平台

移动机器人自主导航系统主要包括以下几个功能包： ① 激光雷达驱动包：将览沃Mid-360激光雷达采集得到的点云数据发布到话题/scan上，此外，激光雷达发布/imu话题，用于表达激光雷达的位姿信息，这两个话题用于激光SLAM建图； ② Fast_LIO2功能包：订阅/scan话题和接收激光雷达获得的点云信息，构建三维点云地图，并提供保存地图服务/map_save； ③ 地面分割提取功能包：订阅/map话题识别地面并进行分割；④ICP功能包：订阅/scan话题和/imu话题，输出机器人激光雷达的位姿，该功能包在机器人进行导航时提供定位功能； ⑤ Navigation2功能包：该功能包是ROS2机器人操作系统用于机器人导航的默认框架，其中包含A*和TEB路径规划算法，需要订阅/map话题和机器人模型位姿信息，发布机器人速度控制/cmd_vel话题； ⑥ Robot_description功能包：用于发布机器人模型信息；⑦Robot_bringup功能包：用于与主控单元通信，订阅导航速度控制话题

4.1.3 评价指标

评估算法的优劣需要一套可行的标准进行评判。在机器人导航中，算法评估指标一般分为精度指标和性能指标。

(1) 精度指标 SLAM算法需要输出机器人的定位信息和地图信息，定位的精确度通常使用相对位姿误差来进行衡量，相对位姿误差就是一定时间内计算估计位姿变化量和真实位姿变化量之间的差距。相对位姿误差可使用欧式距离和平移模量进行度量， 而地图信息的结果和机器人位姿信息是相辅相成的，也可通过建图效果的精度来衡量SLAM算法的优良。 (2) 性能指标 性能指标包括算法的处理速度和鲁棒性。自主导航算法要及时根据激光雷达的距离信息计算模拟路径，所以算法处理时间是衡量算法的重要指标。机器人在运动过程中会出现传感器的抖动和漂移，会影响算法在不同程度上的失真，因此算法要合理处理这些变动，使结果更加可靠。

4.2 PC端与机器人端通讯

实体机器人导航过程中，局部路径规划器所模拟的速度需要将其传输到机器人的主控中，才能驱动机器人的电机进行转动，并带动机器人进行运动。此外， 机器人的运动状态信息也要通过主控传输到上位机中，为机器人导航程序提供反馈信息，因此主控与上位机之间的通讯工作凸显的尤为重要。 本文机器人上位机软件与单片机之间的数据通讯使用虚拟串口进行，分别在 上位机和单片机上模拟物理串口，实现真正串口通信。虚拟串口具有灵活性高、易使用、低成本等优点，大大简化串口通信的开发和应用过程。 PC端的通讯程序需要订阅局部路径规划器发布的/cmd_vel话题，并发布机器人速度话题。本文借助C++标准库Boost库中asio库作为通信串口的依赖库，该串口通信库高效稳定、配置简单，需要在上位机和主控端分别设置同样的通信协议。在程序中需要配置串口，包括串口名称、波特率、数据位、起始位、停止位和校验位等。