系列文章目录
提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理
文章目录
前言
认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对**【机器人/无人驾驶的行为决策】分层有限状态机**做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章
提示：以下是本篇文章正文内容
一、机器人/无人驾驶的行为决策的功能
主要是依据来自上层（任务规划层）的任务类型（巡航、换道等任务），根据当前的交通场景和环境感知的信息的理解，来确定自身当前驾驶状态，在交通规则的约束和驾驶经验的指导下规划出合理的驾驶行为。
.
.
二、机器人/无人驾驶的行为决策系统设计准则–根据现实场景和要求抽象出来设计
行为规划部分几乎直接关系到无人车的可靠性和安全性，要设计出完全符合人类驾驶员习惯和交通规则的行为规划系统在目前来看仍然是一大挑战，目前而言实现行为规划模块的方法很多，但是其设计理念大致可以总结为两点
1.合理性
无人车驾驶的合理性建立在两个基础之上——交通法规和驾驶经验，其中交通法规的优先级又要高于驾驶经验，交通法规需要考虑的内容包括：靠右侧车道行驶，不能超速，换到超车时应提前开启转向灯，对于感知到的交通信号灯和交通标志，应按照其只是内容行驶，出现任何危险情况，应当能够果断地执行紧急制动等等。驾驶经验需要考虑的内容主要包括：尽量保持在原车道，不应随意变道；城市路段行驶时，不应随意加速，确保驾驶的平顺性；对于前车行驶缓慢而条件运行的情况应当果断超车等等。
.
2.实时性
任何无人车系统中的行为规划都是实时的，行为规划应当能够处理复杂的动态交通场景，并且能够根据环境的变化快速的调整驾驶行为以避免危险的发生。
.
.
三、机器人/无人驾驶的行为决策系统设计工具
1.有限状态机FSM
有限状态机就是一个非常简单的抽象反应系统，它之所以非常简单是因为它只对特定的外界输入产生数量有限的响应，在有限状态机中，我们只能构造有限数量的状态，外界的输入只能让状态机在有限的状态中从一个状态跳到另一个状态。
（1）有限状态机FSM的概念
一个有限状态机通常包含如下几部分：
- 输入集合：通常也叫刺激集合，包含我们考虑到的状态机可能收到的所有输入。通常我们使用符号 Σ 表示这个集合。一个简单的例子，假设我们的无人车上有启动，停止两个按钮（我们以a，b代替，不能同时被按下），那么以这两个按钮为输入的FSM的输入集合 Σ={a,b}。
- 输出集合：即FSM能够作出的响应的集合，这个集合也是有限的，我们通常使用符号 Λ 来表示输出集合，很多情况下FSM并不一定有输出，即 Λ 为空集。
- 状态集合：我们通常使用有向图来描述FSM内部的状态和转移逻辑，我们使用符号 S 来表示有向图中状态的集合。
- FSM通常有一个固定的初始状态（不需要任何输入，状态机默认处于的状态），我们使用符号 s0 表示。
- 结束状态集合，是状态机 S 的子集，也有可能为空集（即整个状态机没有结束状态），通常使用符号 F 表示。
- 转移逻辑：即状态机从一个状态转移到另一个状态的条件（通常是当前状态和输入的共同作用），比如说我们要从上图的 Python 状态转移到 Error 状态，需要的条件是： 1）状态机处于 Python 状态；2）输入不是 “is”。我们通常使用状态转移函数来描述转移逻辑：δ:S×Σ→S
接收器（Acceptors）和变换器(Transducers):根据是否有输出可以将感知机分为两类：接收器和变换器，其中接收器是指没有输出但是有结束状态，而变换器则有输出集合。
FSM可进一步区分为确定型（Deterministic）和非确定型（Non-Deterministic）自动机。在确定型自动机中，每个状态对每个可能输入只有精确的一个转移。在非确定型自动机中，给定状态对给定可能输入可以没有或有多于一个转移。
.
（1）有限状态机FSM的代码实现
这个可以参考我另外的一篇博客
https://blog.csdn.net/qq_35635374/article/details/121626001
.
.
2.分层有限状态机HFSM
（1）分层有限状态机HFSM的概念
目前在无人车行为规划上并没有一个“最佳解决方案”，目前普遍认可和采用的方法是分层有限状态机（Hierarchical Finite-State Machine ，HFSM），分层有限状态机也是早期DARPA挑战赛中被许多队伍采用的行为规划方法。而有限状态机是分层有限状态机的基础。
当状态很多的时候，有限状态机就有可能变得非常庞大，假设有限转态机有N中状态，那么其可能的状态转换就有 N×N 种，当状态的数量很大的时候，状态机的结构也会变得更加复杂，此外，有限状态机还存在如下几个问题：
1）可维护性差：当新增或者删除一个状态的时候，需要改变所有与之相关联的状态，所以对状态机的大幅度的修改很容易出错；
2）可扩展性差：当FSM包含大量状态时，有向图可读性很差，不方便扩展；
3）复用性差：几乎不可能在多个项目中使用相同的FSM
这时候就可以使用分层状态机(Hierarchical Finite-State Machine)了
（2）分层有限状态机HFSM的核心思想
把那些同一类型的状态机做为一个状态机，然后再做一个大的状态机，来维护这些子状态机，可以用C++的基类和子类去理解，基类是一个状态机的模板，使用基类派生出多个小子类和一个管理子类，子类就是一个层次，子类之间下相互调用，同时子类又被管理子类维护
相比于FSM，HFSM新增了一个超级状态（Super-state）：本质上也就是将性质同一类型的一组状态何为一个集合（即上图中的大方框），超级状态之间也有转移逻辑。这也就意味着HFSM不需要为每一个状态和其他所有状态建立转移逻辑，由于状态被归类，类和类之间存在转移逻辑，那么类和类之间的状态转移可以通过继承这个转移逻辑来实现，这里的转换继承就像面向对象编程中通过多态性让子类继承超类一样。
（2）分层有限状态机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大多数缺点，例如有限的可重用性。
.
.
3.其他方法
参考我另外一篇博客
https://blog.csdn.net/qq_35635374/article/details/120592768
.
.