无人驾驶汽车系统入门（十九）——分层有限状态机和无人车行为规划

分层有限状态机和无人车行为规划

行为规划又称为行为决策，是无人车规划模块三层（任务，行为，动作）的中间层，本文将介绍行为规划的基本概念，设计核心，同时介绍一种具体的无人车行为规划方法——分层有限状态机，

行为规划（Behavior Planning）是无人车规划模块的一层，位于全局任务规划和底层的动作规划层之间，驾驶行为规划也被称为驾驶行为决策，这一层的作用主要是依据来自上层（任务规划层）的全局最优行驶路线信息，根据当前的交通场景和环境感知的信息的理解，来确定自身当前驾驶状态，在交通规则的约束和驾驶经验的指导下规划出合理的驾驶行为。下图是无人车行为决策层的信息流：

无人车决策规划系统设计准则

行为规划部分几乎直接关系到无人车的可靠性和安全性，要设计出完全符合人类驾驶员习惯和交通规则的行为规划系统在目前来看仍然是一大挑战，目前而言实现行为规划模块的方法很多，但是其设计理念大致可以总结为两点：

合理性：无人车驾驶的合理性建立在两个基础之上——交通法规和驾驶经验，其中交通法规的优先级又要高于驾驶经验，交通法规需要考虑的内容包括：靠右侧车道行驶，不能超速，换到超车时应提前开启转向灯，对于感知到的交通信号灯和交通标志，应按照其只是内容行驶，出现任何危险情况，应当能够果断地执行紧急制动等等。驾驶经验需要考虑的内容主要包括：尽量保持在原车道，不应随意变道；城市路段行驶时，不应随意加速，确保驾驶的平顺性；对于前车行驶缓慢而条件运行的情况应当果断超车等等。
实时性：任何无人车系统中的行为规划都是实时的，行为规划应当能够处理复杂的动态交通场景，并且能够根据环境的变化快速的调整驾驶行为以避免危险的发生。

有限状态机

目前在无人车行为规划上并没有一个“最佳解决方案”，目前普遍认可和采用的方法是分层有限状态机（Hierarchical Finite-State Machine ，HFSM），分层有限状态机也是早期DARPA挑战赛中被许多队伍采用的行为规划方法。而有限状态机是分层有限状态机的基础。

相信很多读者都认为计算机（Computer）就是我们的笔记本，PC或者是服务器，实际上我们生活中的所有计算设备都是计算机，包括我们手里的手机，这些计算设备可以被统称为反应系统（Reactive System），因为它们都是根据来自外部世界的输入信号工作的。

而有限状态机就是一个非常简单的抽象反应系统，它之所以非常简单是因为它只对特定的外界输入产生数量有限的响应，在有限状态机中，我们只能构造有限数量的状态，外界的输入只能让状态机在有限的状态中从一个状态跳到另一个状态。下图是一个简单的FSM：

一个有限状态机通常包含如下几部分：
1. 输入集合：通常也叫刺激集合，包含我们考虑到的状态机可能收到的所有输入。通常我们使用符号 ΣΣ\Sigma 表示这个集合。一个简单的例子，假设我们的无人车上有启动，停止两个按钮（我们以a，b代替，不能同时被按下），那么以这两个按钮为输入的FSM的输入集合 Σ={a,b}Σ={a,b}\Sigma = \{ a, b\}。
2. 输出集合：即FSM能够作出的响应的集合，这个集合也是有限的，我们通常使用符号 ΛΛ\Lambda 来表示输出集合，很多情况下FSM并不一定有输出，即 ΛΛ\Lambda 为空集。
3. 我们通常使用有向图来描述FSM内部的状态和转移逻辑，我们使用符号 SSS 来表示有向图中状态的集合。
4. FSM通常有一个固定的初始状态（不需要任何输入，状态机默认处于的状态），我们使用符号 s0" role="presentation" style="position: relative;">s0s0s_0 表示。
5. 结束状态集合，是状态机 SSS 的子集，也有可能为空集（即整个状态机没有结束状态），通常使用符号 F" role="presentation" style="position: relative;">FFF 表示。
6. 转移逻辑：即状态机从一个状态转移到另一个状态的条件（通常是当前状态和输入的共同作用），比如说我们要从上图的 Python 状态转移到 Error 状态，需要的条件是： 1）状态机处于 Python 状态；2）输入不是 “is”。我们通常使用状态转移函数来描述转移逻辑：δ:S×Σ→Sδ:S×Σ→S\delta: S \times \Sigma \rightarrow S

接收器（Acceptors）和变换器(Transducers):根据是否有输出可以将感知机分为两类：接收器和变换器，其中接收器是指没有输出但是有结束状态，而变换器则有输出集合。

FSM可进一步区分为确定型（Deterministic）和非确定型（Non-Deterministic）自动机。在确定型自动机中，每个状态对每个可能输入只有精确的一个转移。在非确定型自动机中，给定状态对给定可能输入可以没有或有多于一个转移。

分层有限状态机

当状态很多的时候，有限状态机就有可能变得非常庞大，假设有限转态机有N中状态，那么其可能的状态转换就有 N×NN×NN \times N 种，当 NN<script type="math/tex" id="MathJax-Element-11">N</script> 的数量很大的时候，状态机的结构也会变得更加复杂，此外，有限状态机还存在如下几个问题：

可维护性差：当新增或者删除一个状态的时候，需要改变所有与之相关联的状态，所以对状态机的大幅度的修改很容易出错；
可扩展性差：当FSM包含大量状态时，有向图可读性很差，不方便扩展；
复用性差：几乎不可能在多个项目中使用相同的FSM

这时候就可以使用分层状态机(Hierarchical Finite-State Machine)了，把那些同一类型的状态机做为一个状态机，然后再做一个大的状态机，来维护这些子状态机，如下图所示：

相比于FSM，HFSM新增了一个超级状态（Super-state）：本质上也就是将性质同一类型的一组状态何为一个集合（即上图中的大方框），超级状态之间也有转移逻辑。这也就意味着HFSM不需要为每一个状态和其他所有状态建立转移逻辑，由于状态被归类，类和类之间存在转移逻辑，那么类和类之间的状态转移可以通过继承这个转移逻辑来实现，这里的转换继承就像面向对象编程中通过多态性让子类继承超类一样。

感知机在行为规划中的使用

那么我们为什么在无人车行为规划层使用感知机呢？无人车的行为规划层从某种程度上来说也是一种反应系统——无人车的决策（也就是无人车一个时间段以后的状态）是基于无人车当前所处的状态以及来自感知模块的信息（输入）共同决定的。我们以一个实际的应用案例来分析：

“Junior”是斯坦福大学在2007年参加DARPA城市挑战赛时的无人车，它是第二名完成该比赛的无人车，Junior的行为规划系统就是通过分层有限状态机实现的，他们将顶层的驾驶行为分成了13个超级状态（也就是我们上图中的大方框），每个驾驶行为又对应一些子状态来完成这一行为。顶层行为由一个FSM管理，如下图所示：

我们简单的分析一下这个状态机：

LOCATE_VEHICLE: 这是Junior的初始状态，即在无人车出发之前确定其在地图中的位置。
FORWARD_DRIVE: 这个超级状态实际上包含了直行，车道保持和障碍物规避，当不是在停车场（即无道路开放区域）时，这是状态机首选的状态。
STOP_SIGN_WAIT: 当无人车在停车标志处等待时，进入此状态。（停车标志是美国十字路口的常见标志）
CROSS_INTERSECTION：在这个状态下无人车处理十字路口通过这一场景，无人车会等待直到确认能够安全通过。
UTURN_DRIVE: 在U型掉头时调用的状态
UTURN_STOP: 在U型掉头前的停车状态
CROSS_DIVIDER: 跨过黄线行驶
PARKING_NAVIGATE：停车场内的普通驾驶模式
TRAFFIC_JAM和ESCAPE：处理交通阻塞时的两个状态
BAD_RNDF: 如果当前道路和预先做的路网图不同的时候，即进入该状态，在这个状态下，无人车会采用混合A*算法完成车辆的路径规划。
MISSION_COMPLETE: 当挑战赛（DARPA）结束，无人车进入该状态，即整个状态机的结束状态。

在无人车正常行驶中这个状态机几乎处在普通驾驶模式（即FORWARD_DRIVE和PARKING_NAVIGATE这两个状态），系统通过stuckness detectors（胶着检测器）来确定是否从普通驾驶状态转移至底层的其他状态，在完成了相应的动作以后行为模块又会回到原来的普通驾驶模式。这样的状态机能够让无人车处理一下复杂情况：

当当前车道阻塞的时候，车辆会考虑驶入对面车道（即CROSS_DIVIDER）, 如果对面车道也被阻塞了，则会启动U形转弯（UTURN_STOP/UTURN_DRIVE），此时内部RNDF（Route Network Definition File，即全局路网图）也会相应修改，并执行dynamic programming以重新生成RNDF值函数。
对于十字路口的交通阻塞问题，在等待时间结束以后，会调用混合A*算法找出最近出口制动车辆离开阻塞区域。
在单向通道阻塞的时候，如果规划失败也会调用混合A*算法规划至下一个GPS路点。
某些路点在循环多次以后仍然无法到达，那么跳过这个路点（这是为了防止车辆为了抵达规划中某个路点而进入死循环）
如果无人车长时间没有取得任何的进展（指比赛的进展），车辆将会调用混合A*规划出通往附近的GPS航点的路径，这个规划是无视交通规则的。

虽然Junior的策略有些是针对DRAPA挑战赛设计的（即为了尽可能赢得比赛），但是其设计的理念仍然具有参考价值。在实际的无人驾驶应用中，需要实现的状态机将更为复杂。虽然HFSM比传统的FSM更为模块化，但它仍然继承了FSM大多数缺点，例如有限的可重用性。在后面的文章中，我们将介绍行为树（Behavior Tree），行为树这种方法将各种状态模块化，以便逻辑的不同部分能够更轻松地重用它们。