5分钟看懂ROS的Navigation导航系统

    Navigation导航系统是ROS中最常用的子系统，毫不夸张的说，超过一半的机器人开发者都是因为Navigation才开始接触ROS系统的。Navigation这个导航系统的架构设计比较复杂，但是却非常经典，以至于后来出现的其他机器人系统，在导航部分都基本借鉴了Navigation这个经典架构。

   刚接触Navigation的同学，很容易被这个复杂的架构图吓到，下面我们就花5分钟，将它分拆来理解。最后你会发现，它只是看起来复杂而已。

    一、Navigation如何使用

   我们都知道Navigation是用来导航的，但是它到底如何帮助我们进行导航呢？只要找出它的输入量和输出量就明白了。我们将Navigation的架构图简化到只保留输入量和输出部分，并把它和外界的对接关系搞清楚，就能理解它是如何实现导航的。

   从上图可以看出，Navigation的输入是开发者设置的导航目标点坐标，输出是机器人的运动控制指令。这就简单了，我们只需要让机器人的主控Node订阅主题“cmd_vel”里的速度指令，然后向导航服务“move_base_simple/goal”提交导航目标点就完事了。Navigation会自动向主题“cmd_vel”发送速度值，驱动机器人到达导航目标。我们上一节实验里，在Gazebo中加载了一个启智ROS机器人的虚拟核心节点（真实世界中对应wpb_home_core节点），由这个核心节点来获取Navigation输出速度并驱动机器人移动。而我们编写的节点demo_simple_goal就是简单发送了一个导航目标点，就可以等待最终结果了，是不是感觉很简单？

   二、导航的路线怎么生成

   要进行导航，我们肯定需要有个地图，这个地图可以手绘也可以用SLAM方法来创建（参看上一节实验）。但是原始的地图并不能直接进行导航，通常需要先将其转换成“代价地图”（cost_map）。

   什么是“代价地图”，就是说机器人在地图里移动是需要付出“代价”的，这个“代价”有显性的也有隐性的。显性的，比如行走的距离，绕远路费电啊，这是最明显的“代价”。隐性的，比如靠近障碍物，万一机器人一哆嗦就磕着脑袋碰着胳臂了，这是隐性的“代价”。还有一些机器人体型比较长（无人驾驶大卡车有木有），转个弯掉个头巨费劲，对它来说，路线转弯太多也是“代价”，所以导航的路线越顺滑代价就越小。

   那么在Navigation系统里，“代价地图”是如何生成的呢？注意架构图的右上角。

   可以看到，全局代价地图是通过map_server提供的全局地图（通常是SLAM建好的）和激光雷达侦测到的当前机器人周围的障碍物分布融合后生成的。map_server提供的全局地图代表的是以前记录的地图，现在可能会有点变化了。会有哪些变化呢？这种变化有很多，或许只是路上多了个行人，也可能曾经开着的门关上了。远处的变化机器人看不到先不考虑，近处的变化可以用激光雷达侦测扫描，这就是为何全局代价地图需要融合两者的信息来生成。Navigation生成的全局代价地图可以在Rviz里查看，比如下图：

   可以看到全局代价地图里，在障碍物的边缘会膨胀出一层淡蓝色的渐变区域，这个代表的就是机器人可能与障碍物发生碰撞的隐性“代价”。越靠近障碍物，与障碍物碰撞的风险越大，于是颜色越深，隐性“代价”越大。移动距离产生的显性代价，通常都是在路径规划算法内部才会实现，在Rviz里一般不会显示。

   有了代价地图，如何得到导航的路线？在Navigation系统里，这个通常由全局规划器（global_planner）来生成。我们继续沿着上面的架构图顺藤摸瓜：

    从图中可以看出，全局规划器的任务就是从外部获得导航的目标点，然后在全局代价地图里找出“代价最小”的那条路线，这条路线就是我们苦苦寻找的导航路线。

   三、局部规划器

   现在路线已经有了，按理说，直接循线走就完了。无奈这个世界实在变化太快，过去的坦途大道此刻是否已经沧海桑田，我们一无所知。这需要一个机灵点的老司机带我们去应对这一路上可能出现的突发情况，这就是局部规划器（local_planner）。局部规划器的工作就是从全局规划器获得导航路线，根据这个路线向机器人发送速度，驱动机器人“尽可能”按照路线去行走。

    路线已经拿到，方向盘也在手里，咱是不是可以出发了？老司机（局部规划器）掏出老花眼镜，调整调整后视镜，嘬了一口保温杯里的枸杞水：“年轻人，莫着急，待老朽把眼前的情况看清楚”。是的，作为一个饱经风霜的资深驾驶员，深知开车的首要准则就是千万不要完全相信导航仪给出的路线……于是，局部规划器利用激光雷达获得的当前障碍物数据，又做了一个“代价地图”，叫做局部代价地图（local_costmap）。

    为什么有了全局代价地图，还要再做这么一个局部的代价地图呢？我们知道，在真实的环境里，并不是只有我们的机器人在移动，在商场里有行人，在马路上有汽车，在工业环境中也有其他正在移动作业的AGV。这些不停运动着的交通参与者，在全局地图中很多是看不到的（毕竟激光雷达的扫描范围有限）。所以我们需要一个小范围（至少在激光雷达探测距离内）的局部代价地图，并在机器人移动过程中，让这个局部代价地图跟着走，始终围绕在机器人周围，以弥补了全局代价地图里没有体现移动障碍的缺点。

    由此，我们就看得出来Navigation的这一设计并非多此一举，它的策略还是非常合理的：远处瞅个大概，近处瞄得仔细，既有大局观，又注重局部视野，再加上各种细节走位，一看这段位就低不了。

    四、AMCL

     导航路线、带头司机、局部视野都凑齐了，是否可以开始发车了？还不行，我们还缺一样：GPS定位器。没有定位器，机器人连自己在哪都不知道，就是老司机也带不动啊。

    在Navigation中，作为定位器功能的是一个叫AMCL的东东。这个东东有个超有逼格的中文名：“蒙特卡罗粒子滤波定位算法”，既神秘又科幻，一下就把人看懵了……好吧，怎么理解这个“粒子滤波”呢？复杂的概念咱就不展开了，为了帮助同学们快速理解，我举个不太贴切的例子：漩涡鸣人的“多重影分身之术”。

    在我们的机器人开始移动之前，AMCL会在机器人周围“嘭”的一声分裂出一堆的分身，这些分身随机的分布在地图里。在Rviz窗口里，我们会看到机器人周围有很多绿色的小箭头，这些小箭头就是AMCL变出来的“分身”。

    当机器人移动的时候，所有分身会用统一的步伐行进，这个统一步伐来自底盘电机码盘（odom里程计），当电机码盘里程计提示机器人往前直行的时候，所有分身都会同时往前直行；当电机码盘里程计提示机器人往左转的时候，所有分身也同时往左转。机器人的分身在移动的过程中，会不停的用激光雷达扫描到的身边障碍物和地图进行比对，以判断自己是不是那个正确的位置。比如有的分身走着走着，发现面前一堵墙，再查查地图里自己的位置，这地方没有墙啊……完了，自己的位置八成是错的，于是“嘭”的一声，这个分身就消失了。随着机器人的移动，这些定位错误的分身一个接一个的消失，最后剩下的，就是最有可能是机器人位置的真身。对应的，在Rviz里，我们会看到导航中的机器人，身边的绿色箭头会逐渐收拢，最终聚合到机器人脚下，和机器人真实的位置合为一体。

    在Navigation架构图中，AMCL的部分在最左侧：

    原图不太容易看懂，我们把它调整一下：

    这样是不是就容易理解多了。

    五、最终输出

    加上AMCL定位器之后，我们的局部规划器（local_planner）终于可以发车了：

    所以可以看到，Navigation的结构虽然复杂，但是其目标还是很统一的。所有的一切，最终的目的就是让局部规划器（local_planner）能够正常发车。在整个Navigation系统里，局部规划器（local_planner）堪称C位中的C位。如果把机器人比作一辆车，那么Navigation里的各部分模块所扮演的角色就像下图这样：

    在实际的操作当中，对局部规划器（local_planner）的调教也是最曲折最费时的部分。因为机器人的底盘类型千差万别，有的只能前后移动和原地转向（差动底盘），有的可以360°随意移动（全向底盘），有的底盘是履带双足四足甚至多足，除非撞墙否则都不带避障的（Big Dog和Atlas有木有）。底盘的多样性导致很难用一个统一的局部规划器去适配所有的机器人。这也使得在ROS中，局部规划器的类型是最多最繁琐的，比较主流的什么base_local_planner、DWA、TEB、Eband等等等等，非主流的更是数不胜数。这些规划器还动不动带个几十上百个参数，能把人调得死去活来，不亦乐乎……当然，在众多的机器人生产厂家中，总有那么一些对用户体验有极致追求的异类（比如，六部工坊），为了避免用户在开发中遇到这些苦恼，索性为每个产品单独开发一个专用局部规划器（比如，wpbh_local_planner），所有参数调到最优，开箱即用，落地就跑，足见其用心程度和技术实力！（此处为广告）

    六、遇到障碍物不停转

    诚然，并不是所有的ROS开发者都能用上六部工坊的机器人。在众多的导航调试历劫当中，最常见的问题莫过于机器人遇到新出现的障碍物时，会不停的原地打转，而且一旦转起来就没完了……要分析其中原因，就不能不提到一个叫做“recovery_behavior”的机制，在Navigation架构图中，它是这个样子的：

    这个样子还不太好理解，我们将它分拆开：

    1、 “recovery_behavior”被激活，通常是出现在机器人面前被大体积的障碍物挡住导航路线的时候。一些局部规划器（local_planner）如果参数设置不合理，就会导致规划不出能绕过障碍的局部路线，于是不得不向“recovery_behaviors”求助。

    2、在很多版本的ROS中，“recovery_behaviors”里默认行为通常会设置为“rotate_recovery”，也就是让机器人原地旋转，用激光雷达去完全的扫描附近的障碍物。这就有疑问了，为何原地旋转就能完全扫描附近的障碍物？因为这是考虑到，大部分的机器人，在激光雷达的周围，通常都会有一些支撑结构会挡住它的扫描视野，也就导致机器人的激光雷达在视野上有一些盲区。“recovery_behavior”觉得也许找不到路线只是因为局部规划器（local_planner）的视野被挡住了，能够绕过障碍物的路线搞不好就藏在激光雷达的视野盲区里，说不定转个一两圈就能解决问题了。毕竟让全局规划器（global_planner）重新规划全局路径太耗时了，如果可能，还是尽量不要惊动那位爷……

    3、按照“recovery_behavior”设计的机制，当转圈也解决不了问题的时候，最终还是得全局规划器（global_planner）出马，按照现在的囧境，重新规划一条新的全局导航路线。

    4、所以，事情到这就结束了吗？很遗憾还没有……我们以为“recovery_behavior”也就是转两个圈就完事了，严重低估它为了我们能够“尝试绕过障碍物”又多操了多少心。完整的“recovery_behaviors”状态链如下：

    这么多状态，一不小心就在某个状态就“clear”到“Navigating”，然后又“stuck”卡回到“recovery_behaviors”状态链。于是，这几种状态在不停的循环跳不出来，这个为了让我们能够恢复导航状态的“recovery_behaviors”机制，让我们再也无法恢复到导航状态。

    所以，对于不熟悉局部规划器（local_planner）参数的同学，建议在launch文件里直接把move_base的         recovery_behavior_enabled参数设置为false，直接禁掉好了。条条大路通罗马，何必单恋一枝花，打不过就换线，战略撤退不丢人……当然最省心的方案，还是尽量使用六部工坊的机器人产品。

   七、结束

   不知不觉，5分钟过去了。现在再回到ROS的Navigation架构图，看着里面一个个熟悉的名字，是不是感觉也没有那么复杂了？

 

更多产品了解

欢迎扫码加入云巴巴企业数字化交流服务群

产品交流、问题咨询、专业测评

都在这里！