| 编辑推荐: |
本文主要介绍了移动机器人导航技术、行为树及导航过程生命周期（Life
Cycle）管理等，希望对你的学习有帮助。
本文来自知乎，由火龙果软件Linda编辑、推荐。 |
|
随着ROS2第四个正式版本Dashing的发布，ROS2的成熟度已经得到了很大的发展。机器人相关的一些核心部件在ROS2系统里都有了实现，而且已经陆续有一些机器人公司将ROS2成功地跑在了他们的商业机器人平台上。越来越多的业界人士已经意识到，在很多的机器人应用场景中，ROS2才是独一无二的首选系统。因此，ROS2导航模块（Navigation
Stack）也渐入人们的视野，现在正是需要尽快了解这个模块的大好时机。 近期天马实验室对ROS2 Navigation
Stack进行了一轮深入的评测，现分享部分报告，以飨读者。
当今语境下的机器人技术是一门跨学科、跨领域、需要许多先验知识的庞杂的学问。机器人以及其他的一些智能系统，被市场以及人们的直觉赋予了许多智能化的需求。
对于移动机器人来说，最基本的自然是它的移动能力，我们这里所讲的导航技术，也是专门针对这类机器人展开。
移动机器人导航技术
有人把机器人在从简单的机械特性向复杂的智能过渡的过程，用下面这幅图来描述。
机器人领域交叉与技术衍生
机器人获得一定的智能的过程，就是在自主运动、智能操纵和人机交互3个方面寻求交叉式突破。这3个方向的单方向发展已经有相当的念头了，也都取得了很多成就，而让他们发生交叉并融为一体，也就诞生了我们设想的智能机器人。
而深度交叉的一个结果就是对于机器人所处场景的理解以及数字化重建，这就是所谓的世界模型的建设问题。
机器人的世界模型与世界观
事实上，包括人在内的所有的智能体都需要对自己所处的世界树立确定的世界观跟世界模型。确定的世界模型明确了你如何理解世界，如果同外界沟通，以及如何解决问题。对于我们将要讲的导航模块，针对的是移动机器人，那么就需要从移动机器人的角度来建立世界。移动机器人通过各种传感器来感知世界，而通过一些控制器来控制运动，同时将内部的感知、理解、推理、决策等过程隐藏起来形成一个整体。
那么，机器人的导航模块（Navigation Stack）是用来解决什么问题的呢？
机器人导航的现实问题
顾名思义，它解决的自然是在一个已知的、动态的环境里，实现让机器人本体从A地到B地的自主运动。在这个过程中，实现对物理世界的建模、路径规划、避障、移动等等各种功能。
为这样的一个命题来建模，我们需要假定这样的一些条件：
一个几何化了的对世界的描述，这里对应的是地图
一个几何化了的对机器人的抽象，这里对应的是机器人模型
一个是可以求得的机器人的起始位置
一个是给定的需要到达的目的地
导航模块就是在给定这些条件之后，对这个几何问题获得动力学方面的解的过程。
导航模块的逻辑抽象
到此为止事实上我们已经导出了导航（Navigation）的基本定义了。导航就是解决一个有先决条件、有动力学限制的几何问题的一系列过程。Wikipedia上也是这么定义的。大体上分为了解世界、响应需求、了解自我、实现目标这四个步骤，像极了一个有为青年的奋斗史。
而从ROS机器人操作系统这样的环境里，又有不同的步骤提炼。
机器人导航模块的数据与逻辑处理流程
对于一个特定的导航需求，首先有传感技术与SLAM模块共同决定一个世界模型，然后通过路径规划算法获得加权之后的最优路径，再次导入机器人模型做避障规划，最后实现对机器人硬件的移动控制。
机器人导航模块的数据与逻辑处理流程
在过去的10多年时间里，ROS Navigation取得了很大的成就跟普及，在ROS的生态里，一直导航模块的代表者，无人出其右。然而，ROS版本的Navigation自然有时代所固有或形势所确定的一些问题。
到目前它已经支持了很多不同形状的机器人，不过在原生设计上，它是只支持圆形机器人的，这对于对世界模型的建立、解题思路上具备了一些先决的限制。
在设计之初，它也是只支持LiDAR sensor输入的2D地图。
另外，在软件设计上，ROS Navigation也没有引入一些新的编程技术，模块化不好，导致运行性能不好，难以维护、debug。
同时，从2017年开始，OSRF加大了对ROS2的开发投入，也明确并加快了ROS2开发、发布的节奏。确定了每半年做一次正式版本发布（ROS1是每一年有一个正式发布），以TSC（技术领导委员会）领导的不同特定技术的Working
Group（工作小组）的模式来开发关键模块。
正是在这样的历史背景下，Intel在2017年底开始酝酿创建一个完全针对于ROS2系统的新的导航项目。该项目在2018年8月份正式创建。Intel是ROS2
TSC的主要成员之一，领导了Navigation Working Group。到目前为止，ROS2
Navigation模块已经持续发布了多个版本，在最近发布的ROS2 Dashing版本，也已经集成了ROS2
Navigation。当然，ROS2 Navigation也被集成到了Intel发布的机器人开发套件中，该开发套件涵盖了Intel在机器人方面一些尝试与能力。
基于目前最新的版本（tag：0.2.2 ），ROS2 Navigation的主要的着力点在于如下几个方面：
引入了行为树（Behavior Tree）和任务管理
实现了更好的接口隔离，对世界模型跟机器人模型有了更完善的抽象
重新设计了导航过程的主模块
移植并重构了全局规划模块（NavFn Global Planner） 和局部控制模块（DWB Local
Planner），以及其他关键模块
重新模块化设计了Recovery模块
ROS2导航模块与外部模块的接口隔离
对于这些在ROS2 Navigation模块中主要的改动，这里做一个逐一讲解。
行为树（Behavior Tree）
行为树（Behavior Tree）是最近比较流行的一种有限元状态机（FSM），但是它跟传统的FSM实现有一些差别。
从定义上来说，它的主体是一些模块化设计的任务(Task)或者也可以成为行为（Behavior），这些任务称为Node。Node分为不同的种类：根节点（Root），控制节点（Control），执行节点（Execution）。
BT关注的是行为，而传统FSM关注的是状态。
BT的执行有几个特点：
一是一个BT有且仅有一个Root，执行必须从Root开始。
二是可以以设定的频率循环。
三是所有的Node采取从左到右的优先级执行。
每个Node被执行都会返回3个状态之一：
Success
Running
Failure
Smash是被引入ROS最早的一个FSM，它属于传统的类型，服务于Python程序，面向于State。功能也很完备，也能应付机器人领域的各种状态管理。
然而如果把BT跟Smash做一个比较，就可以看出BT具有更多的一些优势。
首先BT能够将复杂的任务轻易地分解成符合模块化设计的子任务，每个子任务可以很简单。
其次BT面向的是任务管理，因此BT更像是函数调用过程，而传统的FSM就相当于GOTO调用。
BT将对状态的管理集中到特定的Node之中。由于规则简单确定，容易上手，这对开发过程或以后的产品维护、升级，都具有很大的好处。
由此引申开来，BT更符合人类的思考习惯，封装了具体的实现，分离了状态转换过程，使状态控制清晰明确，减少了错误的引入。
另外，BT已经在游戏领域获得了验证跟推广，而就我所知，现在在许多的机器人开发公司，也是流行使用BT。
当然，BT也并不是十全十美的，它采取的是平铺式任务管理，必然会导致在某些情况下，一个任务在BT不同的位置被调度多次，这可能会导致函数重入或者线程调度竞争等复杂问题。
Selector Node类似于逻辑或的操作，在顺序执行子Node的过程中，只要有一个子Node返回Success/Running，Selector就返回Success/Running。
只要子Node返回Failure，就顺序执行下一个子Node，一旦不是，就立马返回该状态。
Sequence Node类似于逻辑与的操作，只有所有的子Node都返回Success，才会返回Success。
Parallel Node同时启动子Node的执行，当返回Success的子Node个数超过了设定的个数下限的时候，返回success。如果没有超过，返回Running。
Parallel不会返回Failure状态。
Decorator Node是对执行条件做一些限制，一旦设定的条件被满足才会执行子Node。最好的一个例子是RateDecoratorNode，限定子Node被执行的频率。
Action Node是真正实现任务的node，实现具体的业务逻辑，并负责在每个tick的执行过程中，及时返回状态（Failure，Running，Success）。
Condition Node决定某个给定条件然后返回状态（Failure/Success），它没有Running状态。
最常用的情况，是Condition Node跟Action Node一起组合为一个Sequence。Condition
Node检测某个条件，一旦条件满足，则会执行Action Node中实现的特定任务。
Recovery Node在Navigation中很重要，它提供了让整个BT在遇到异常之后，自我修复的能力。
一个Recovery Node只能有2个子Node，期望状态下第一个Node总是返回Success或Running。如果第一个Node发生异常，就会启动Recovery
Node的修复过程，也就是第二个Node被执行。
在第二个Node执行成功之后，第一个Node又会被再次执行。
Recovery Node还可以设定这个循环的执行次数，超过该次数之后，Recovery Node返回Failure。
BT还有一个很重要的概念，叫黑板Blackboard。这个概念的引出是基于在不同Node之间共享数据的考虑。
任何一个系统，都少不了游离于流程之外的全局数据，这些数据需要得到某种封装跟格式化。
BT规则规定，每个BT只能有一个blackboard，BT提供一系列对blackboard以及其中的数据的访问接口。
当然，当前版本的BT对于这些数据的封装还不足够，每个Node都可以一引用+指针的方式访问这些共享数据。这些数据以全局数据（Global
Data）暴露于整个BT系统，引入了数据的不一致性风险，也破坏了模块化设计。
按照计划，BT模块很快会有Version 3的release，在这个新的版本中，将会优化对数据共享的实现，很可能会采取FIFO（有名管道）的方式来重新实现。ROS2
Navigation也会在第一时间做版本升级。
那么ROS2 Navigation是如何应用BT并扩展了BT的呢？
正如上图所示：
实现了ComputePathToPose任务，这个是Global Planner的统一接口。如果在ROS2
Navigation支持新的Global Planner，该任务需要被实例化。
扩展了Decorator Node，实现了Rate Controller。RateController主要被应用于设计Global
Planner被执行的频率。正如前文所讲，计算路径是一个低频率需求，这是处于一个业务考虑，也是出于对系统资源合理分配的考虑。
实现了FollowPath任务，这是Local Controller的同意接口。
实现了Condition Node GoalReached。
如前文所讲，实现了Recovery Node。
所有的这些实现，依赖于在Blackboard中共享的两个数据：导航目的地Goal以及ComputePathToPose模块计算得出的全局路径path。
总结来说，BT采用了平铺式行为逻辑设计，分离了任务实现与逻辑控制，避免了逻辑交叉，降低了模块的多入多出导致的编码复杂度，能够轻松地实现复杂逻辑。当然，对于多个状态联系比较密切的情况，行为树的表达就会比较复杂。
然而，牺牲一点复杂度是非常有必要的，因为BT会由此带来一个好处，就是BT会细粒度析构了复杂逻辑。
举个例子，为了实现下面的这个状态机：
可以设计成这样（Design A）：
也可以设计成这样（Design B）：
无疑，Design B方案是更优的，它将Recovery逻辑在不同的模块中作了析构，使每个Recovery
任务的实现不受其他模块的影响，也不需要自行判断系统的当前状态，对系统的模块化设计跟性能都是有帮助的。这也是ROS2
Navigation接下来将要实现的方向。
Global Planner
全局规划模块（Global Planner）是在给定的世界模型下，对给定世界做路径规划的过程。世界模型决定了环境描述、运动学以及机构的限制。
同ROS Navigation一样，ROS2 Navigation的路径规划采纳了2个假设。
一个是非精确单元格分解（Approximate Cell Decomposition），
另一个是组合规划算法（Combinatorial planning）。
组合规划算法的代表作是Dijkstra以及A*，在ROS/ROS2 Navigation模块中实现的也是这个模块。跟它对应是基于采样的规划算法。组合规划算法需要以明确的地图作为先决条件，然后换算精确的路径。采样算法并不明确依赖地图，也不以获得精确路径作为目标。这两种算法的一大差别是对上帝视角的依赖程度。Dijkstra是完全从上帝视角出发的，A*是在上帝视角的基础上加入启发式权重。而采样算法不依赖上帝视角，而是以一种“盲人摸象”的方式，在构建路径的同时了解世界。
在ROS领域，D、A*算法的引入需要将世界表达为2D占用地图，地图的每个格子都有确定的有限多个值，来对应实际空间的状态。
如果对A*和Dijkstra算法做一个比较，Dijkstra是对全局做分析，它总是能得到最短路径，A*是在全局分析跟目标导向两个方面做了一个启发式权重分配，不能总是保证找到最短路径，但是性能会比Dijkstra好很多。
在机器人导航的实践过程中，这两个算法有一个通病，那就是他们都一获得最可能短路径作为目标，势必导致他们紧贴障碍物规划路径。解决这个问题的思路在于对于每个格子的描述：从非黑即白的障碍物表述，引入一些中间状态，用以表征机器人处于当前格子时发生碰撞的风险高低。
在ROS领域，这些格子都被定义为固定大小，跟世界坐标系对齐的。
Costmap是对这些定义的归纳与实现。通过分层来实现不同功能的融合。使用Static层作为对世界的静态描述，Inflation层附加表达机器人在不同位置的安全指数，obstacle层反映实时的动态的障碍物情景。当然还可以加入更多的层，实现对更多输入数据的数字化抽象跟融合。
导航过程生命周期（Life Cycle）管理
另一方面，ROS2 Navigation引入了Mission的概念。Mission概念超越了Navigation这一个单一的功能，它试图解决的是Navigation功能如何被应用到实际的客户需求当中。真正的客户需求，不会直接告诉机器人需要移动到某个坐标点，也不会明确navigation的哪个子模块在什么时间需要被运行。Mission是客户需求的动作分解的实现者。
大体上，Mission过程可以分为规划、执行、调用Navigation系统以及机器人运动控制几个部分。这里展示了Path
Planning的例子，展示了Mission是如何一步步细化任务并最终跟Navigation的某些具体实现挂上钩的。
从严格意义上来说，Mission不应该是Navigation的一个子集，而应该反过来，Mission是一个机器人系统中任务调度的一种实现。但是这很好地展示了ROS2
Navigation在设计上的独到见解，navigation可以跟这种复杂的任务调度实现无缝连接。
ROS2 Navigation的另一个独到之处是ROS Node的生命周期管理（Lifecycle
Management）。生命周期管理是ROS2系统的一大特色，有别于ROS1系统。除了标准的Node，ROS2还有一种生命周期管理相关的Node。大家知道ROS/ROS2中Node概念，是跟进程、线程不能等同又有一定的相似度，因此就可以应用一些进程或线程相关的管理办法来管理Node。
在Node的生命周期管理当中，常用的状态有四个：初始状态（unconfigured）、未激活（inactive）、激活（inactive）、完备（Finalized）。
而状态转换有7种命令模式：创建、配置、清空、激活、反激活、关闭、destroy。
ROS2 Navigation 引入了生命周期管理，实例化了lifecycle_manager Node，可以通过参数来管理其它Node。这带来的最大的好处，在于ROS2
Navigation可以动态监测系统状态，并自动纠正系统错误，甚至可以自动启动自身进程，而不需要客户或其它进程的干预。
Local Planner
在ROS/ROS2领域，DWA是知名的Local Planning的算法。Local planner是追求精确的路径规划，需要包含细致的机器人行程计算以及无误的避障措施。
对于一个Local Planner的过程，可以归纳为两个步骤。
先是通过已知的地图跟路径，在选定的窗口范围内，找出需要到达的临时目标点，设定可搜索区域，并规划出实时路径。这一步包含对环境的实时感知。
再是在规划出的实时路径中，按照机器人模型的不同，选择最适合的一条路径，并把路径转换为对机器人的运动控制。这一步包含对机器人的控制。
因此local Planner要求需要性能高，足够实时，并达到对机器人的有效控制。
下图是一个示例。
DWB是一个DWA算法的模块化实现。在ROS1 Navigation中，已经有一个dwa_local_planner的实现了。然而，在ROS1中，对于trajectory
scoring的算法提炼却很少，而且是hardcode的，只能从是否会发生碰撞和是否达到目的地来评分，这不能反映实际导航中遇到的现实问题，譬如说路径的长短，机器人运动速度的稳定性，机器人朝向的稳定性等等。
在DWB中，则是优化了这方面的评分标准，也在编码方法上做了很大改进，可以使用户定制的一些得分机制加入其中。
在具体的ROS2 Navigation的实现当中，由于ROS2 Navigation使用的BT的task管理，ROS1
Navigation当中的move_base 跟nav_core相关的接口都被移除了，代之以行为树相关的接口实现。同时，完整地保留了DWB项目中关于Plugin的所有功能，可以启用系统自带的或者用户自己定制的plugin。这样的设计，也避免了ROS1
Navigation中一些代码的耦合太重的坏设计。 | 
