当一个扫地机器人第一次来到你家时，它对家里的环境一无所知，所以第一次启动时，它的主要工作是探索这个未知环境，使用的技术就是SLAM。
地图建立完成后，就要正式开始干活了，接下来很多问题摆在机器人面前：如何完整走过家里每一个地方？如何躲避地图中已知的墙壁、衣柜等障碍物？静态的还好说，如果有“熊孩子”或者宠物，还有地上不时出现的各种杂物，机器人又该如何一一躲避？
这些问题就需要一套智能化的自主导航算法来解决。
机器人自主导航的流程并不复杂，和我们日常使用地图App的导航功能非常相似。
首先，选择一个导航的目标点，如图1所示的Goal，可以在地图App里直接输入，也可以在机器人中人为给定，目的是明确机器人“去哪里”。
接下来，在进行路径规划前，机器人还得知道自己“在哪里”，地图App可以通过手机中的GPS获知定位，机器人在室外也可以用类似的方法。如果在室内，GPS的精度不够，那么可以使用SLAM技术进行定位，也可以使用后面将要介绍的AMCL——一种全局定位的算法进行定位。
图1 移动机器人的自主导航流程
回想一下地图App中的操作，接下来App会画出一条连接起点和终点的最优路径，这就是路径规划的过程。规划这条最优路径的模块被称为全局规划器，也就是站在全局地图的视角，分析如何让机器人以最优的路径抵达目的地。
规划出路径后，机器人就开始移动了，在理想状态下，机器人需要尽量沿着全局路径运动，这个过程中难免会遇到临时增加的障碍物等问题，需要机器人动态决策。此时，机器人会偏离全局路径，动态躲避障碍物，这个过程就需要机器人搭载一个局部规划器。
局部规划器除了会实时规划避障路径，还会努力让机器人沿着全局路径运动，也就是规划机器人每时每刻的运动速度，这个速度就是之前频繁用到的cmd_vel话题。将速度指令传输给机器人底盘，底盘中的驱动就会控制机器人的电机按照某一速度运动，从而带动机器人向目标前进。
先来了解一下Nav2自主导航框架的系统架构，如图2所示，该框架需要和外部节点交互的信息较多。
首先是路点（waypoints），也就是一系列导航的目标位置，路点可以有一个或多个，例如从A到B，从B到C，再从C到D，这里的B、C、D就是路径点的目标信息。
图2 Nav2自主导航框架的系统架构
为了实现导航，Nav2还需要一些辅助信息帮助它明确位置和导航信息。例如tf坐标转换信息，它表示机器人和里程计之间的关系，可以帮助机器人确定自身在地图中的位置。对于机器人导航而言，只有知道自己在哪里、目标在哪里，才能进行路径规划。另一个重要信息是地图，SLAM构建的地图可以提供环境中的静态障碍物信息，例如墙壁和桌子等。
此外，机器人还需要通过雷达动态检测环境中的动态障碍物，例如突然出现在前方的人，并实时更新定位信息和避障策略。
有了以上信息，接下来实现Nav2框架。框架上方有一个行为树导航服务器，它是一个组织和管理导航算法的机制。行为树通过参数化配置，设置了导航过程中使用的插件和功能，用于组织和管理多个有具体功能的服务器。
类似导航App中的路径规划功能，规划器服务器负责规划全局路径。根据全局代价地图（Global Costmap），服务器规划器计算从A点到B点的最佳路径，从而绕过障碍物。其中可以选配的规划算法种类较多，常见的如A*、Dijkstra算法等。
在全局路径规划完成后，控制器服务器提供路径跟随（Follow Path）服务，让机器人尽量沿着预定的路径移动。同时，它根据局部代价地图（Local Costmap）和实时传感器数据，动态调整机器人的路径，确保机器人能够避开动态障碍物并沿着全局路径行进。
在规划器服务器和控制器服务器输出路径后，平滑器服务器会获取路径，并通过一些处理使之更加平滑，减少大幅度的转角或者加减速变化，确保机器人的运动过程更加顺畅。
这三个服务器通过行为树进行组织和管理。行为树决定了各个功能模块的执行顺序和条件，确保导航过程的正确性和有效性。
行为树就像一位组织者，协调各个模块的工作。
Nav2框架完成轨迹的规划控制后生成速度控制指令——cmd_vel，包括线速度和角速度。为了提高机器人底盘控制的平稳度，速度平滑器还会对算法输出的速度进行平稳的加减速，然后检测机器人按照该速度运动是否会发生碰撞，最后将结果传递给机器人的底盘控制器，驱动机器人沿着规划的路径移动，完成整个导航过程。
全局导航是由规划器服务器负责的，它的主要任务是根据全局代价地图计算从起点到目标点的最佳路径。
那么全局代价地图是什么呢？
Nav2中的代价（Cost）指的是机器人通过某个区域的难易程度。例如，空旷的区域代价低，机器人可以轻松通过；靠近障碍物的区域代价高，机器人需要小心避让。全局代价地图指的就是通过SLAM生成的静态地图和传感器提供的动态数据构建的一个详细的环境模型。
所以全局导航的关键在于利用地图信息和传感器数据，确保路径规划的准确性和有效性。其中，地图信息通常通过SLAM技术生成，地图中包含环境中的静态障碍物，而传感器数据则实时提供障碍物信息。综合这些信息，规划服务器就能规划出一条安全且高效的路径。
在Nav2中，全局规划算法以插件的形式设置于行为树XML文件中，常见的算法有A*和Dijkstra，两种算法的效果对比如图3所示。
图3 Dijkstra与A*算法
Dijkstra算法。Dijkstra可以看作一种广度优先算法，搜索过程会从起点开始一层一层辐射出去，直到发现目标点，由于搜索的空间大，往往可以找到全局最优解作为全局路径，不过消耗的时间和内存资源相对较多，适合小范围的路径规划，例如室内或者园区内的导航。
A*算法。由于加入了一个启发函数，在搜索过程中会有一个搜索的方向，缩小了搜索的空间。但是启发函数存在一定的随机性，最终得到的全局路径不一定是全局最优解。不过这种算法效率高，占用资源少，适合范围较大的应用场景。
考虑到移动机器人的大部分应用场景范围有限，而且计算资源丰富，所以在ROS 2导航中，还是以Dijkstra算法为主。
在Nav2框架中，局部导航由控制服务器（Controller Server）负责，它的主要任务是确保机器人在全局路径规划的基础上能够实时地沿着规划好的路径移动，并利用局部代价地图动态避开环境中的障碍物。局部导航类似于手机导航App中的实时导航功能，当我们行驶在路上时，App会不断调整路线，确保不会偏离预定路径。
局部导航的关键在于实时性和灵活性。虽然全局路径规划提供了从起点到目标点的最优路径，但在实际行进过程中，环境可能发生变化，例如突然出现的行人或移动的障碍物。控制器服务器通过不断接收传感器数据（如激光扫描和点云数据），实时更新机器人的位置和周围环境信息，确保机器人能够灵活应对这些变化。
局部代价地图与全局代价地图类似，但它专注于机器人周围的局部环境，更新频率通常高于全局代价地图。
局部路径规划算法与全局路径规划算的原理不同，但是它的设置方式也在行为树的XML文件中，Nav2框架中常见的算法有DWA、TEB算法等。
DWA算法。DWA（Dynamic Window Approaches）算法的输入是全局路径和本地代价地图的参考信息，输出是整个导航框架的最终目的——传输给机器人底盘的速度指令。这中间的处理过程是什么样的呢？如图4右侧所示。
图4 DWA算法框架
DWA首先将机器人的控制空间离散化，也就是根据机器人当前的运行状态，采样多组速度，然后使用这些速度模拟机器人在一定时间内的运动轨迹。
得到多条轨迹后，再通过一个评价函数对这些轨迹打分，打分标准包括轨迹是否会导致机器人碰撞、是否在向全局路径靠拢等，综合评分最高的轨迹速度，就是传输给机器人的速度指令。
DWA算法的实现流程简单，计算效率也比较高，但是不太适用于环境频繁发生变化的场景。
TEB算法。TEB算法的全称是Time Elastic Band，其中Elastic Band的中文意思是橡皮筋，可见这种算法也具备橡皮筋的特性：连接起点和目标点，路径可以变形，变形的条件就是各种路径约束，类似于给橡皮筋施加了一个外力。
如图5所示，在TEB的算法框架中，机器人位于当前位置，目标点是全局路径上的一个点，这两个点类似橡皮筋的两端，是固定的。接下来，TEB算法会在两点之间插入一些机器人的姿态点控制橡皮筋的形变，为了显示轨迹的运动学信息，还得定义点和点之间的运动时间，也就是这里Time的含义。
图5 TEB算法框架
接下来这些离散的位姿就组成了一个优化问题：让这些离散位姿组成的轨迹能实现时间最短、距离最短、远离障碍物等目标，同时限制速度与加速度，符合机器人的运动学。
最终，满足这些约束的机器人状态，就作为局部导航输入机器人底盘的速度指令。
在Nav2框架中，定位功能是机器人自主导航的基础，主要任务是确定机器人在环境中的准确位置和姿态（位置和方向）。Nav2通常使用AMCL算法帮助机器人进行定位。
什么是AMCL算法呢？
AMCL功能包封装了一套针对二维环境的蒙特卡罗定位方法，如图6所示，算法的核心是粒子滤波器，它使用一系列粒子来表示机器人可能的状态。
图6 AMCL算法框架
每个粒子包含了机器人的位置和方向的估计值。在机器人移动时，这些粒子也会根据机器人的运动模型进行更新。同时，通过将机器人的传感器数据（如激光雷达数据）与地图进行比较，算法会评估每个粒子的权重，即该粒子代表的位置估计与实际观测数据匹配的程度。在每次更新中，权重较高的粒子将有更大的机会被保留下来，而权重较低的粒子则可能被淘汰。这个过程被称为重采样。通过这种方式，粒子群逐渐聚焦于最可能代表机器人实际位置的区域，从而实现高精度的定位。
如图7所示，我们可以形象地描述AMCL算法：AMCL算法会在机器人的初始位姿周围随机撒很多粒子，每个粒子都可以看作机器人的分身，由于这些粒子是随机撒下的，所以这些分身的姿态并不一致。
图7 AMCL算法示例
接下来机器人开始运动，例如，机器人以1m/s的速度前进，那么这些粒子分身也会按照同样的速度运动，由于姿态不同，每个粒子的运动方向不一致，也就会和机器人渐行渐远，如何判断这些粒子偏航了呢？这就要结合地图信息了。
例如，机器人向前走了1m，这时通过传感器我们可以发现机器人距离前方的障碍物从原来的10m变为9m，这个信息也会传给所有粒子，那些和机器人渐行渐远的粒子会被算法删除，和机器人状态一致的粒子则被保留，同时派生出一个同样状态的粒子，以避免最后所有粒子都被删除了。
按照这样的思路，以某个固定的频率不断对粒子进行筛选，基本一致的留下，不一致的删除，最终这些粒子就会逐渐向机器人的真实位姿靠拢，聚集度最高的地方，就被看作机器人的当前位姿，也就是定位的结果。
以上就是AMCL算法的主要流程，更多相关内容可参阅《ROS 2智能机器人开发实践》一书。👇
↑限时优惠↑
本书特色
本书采用最新稳定版本ROS 2系统和全新一代的Gazebo机器人仿真平台，绝大部分功能和源码可以在单独的计算机和Gazebo仿真平台上运行。
同时，本书介绍实物机器人的搭建方法，并且在实物机器人上实现相应的功能。
配套源码都加入了中文注释，同时针对核心内容提供C++和Python两个版本，方便读者理解。
限时优惠中，快快抢购吧！
活动方式：关注微信公众号“一起学嵌入式”，点赞、转发本文，并在评论区留言互动，届时留言点赞前三的的小伙伴各赠送《ROS 2智能机器人开发实践》一本！
活动时间：截至2024年12月31日12点。
快快拉上你的小伙伴参与进来吧~~
关注【一起学嵌入式】，回复“加群”进技术交流群。
觉得文章不错，点击“分享”、“赞”、“在看” 呗！
