自动驾驶决策算法 —— 有限状态机 FSM

自动驾驶决策算法 —— 有限状态机 FSM

一、FSM 是什么？

1.1 定义与核心思想

有限状态机（FSM, Finite State Machine） 是一种离散事件系统模型，用于描述系统在有限个状态（State）**之间的切换规律。
在自动驾驶中，FSM 是**决策层常用的行为组织方式，用来确定车辆在不同驾驶情境下应执行的策略。

它的核心思想可以用一句话概括：

在不同状态下，执行不同动作，并根据事件触发进行状态切换。

数学上，一个 FSM 可以用五元组表示：

$\text{FSM}=(S,E,A,\delta ,s_0)$

其中：

• $S$：有限状态集合（如巡航、跟车、变道）
• $E$：事件/触发条件集合（如前方有车、障碍物出现）
• $A$：动作集合（如加速、减速、转向）
• $\delta :S\times E\to S$：状态转移函数
• $s_0$：初始状态

---

1.2 FSM 与其他决策方法的区别

特性	FSM（有限状态机）	行为树（BT）	规则引擎（Rule-based）
结构	状态-事件-转移	节点层次执行	规则集合
可读性	高	中	中
灵活性	低（状态爆炸风险）	高	高
扩展性	一般	好	好
学习能力	无	可结合学习	可结合学习
适用场景	规则明确、状态有限的任务	复杂多任务决策	条件推理、多领域融合

自动驾驶中，FSM 常用于：

• 高速公路巡航与变道
• 路口通行控制
• 停车/出库
• 避障行为切换

而在更复杂的任务（如城市混合交通环境），FSM 往往和行为树、深度强化学习等结合使用。

---

1.3 自动驾驶中 FSM 的典型应用场景

1.3.1 高速公路巡航与变道决策

• 状态：巡航、跟车、变道、避障、减速
• 事件：前车减速、目标车道可用、障碍物出现
• 动作：加速、保持速度、转向换道

1.3.2 城市道路红绿灯与路口通行控制

• 状态：等红灯、起步、穿越路口
• 事件：红灯→绿灯、车辆完全停稳
• 动作：刹车、加速、匀速通过

1.3.3 交互式避障

• 状态：正常行驶、避障、返回原车道
• 事件：检测到障碍物 / 障碍物消失
• 动作：转向避让、加速回归

1.3.4 停车与出库

• 状态：巡航、进入停车模式、调整位置、停止
• 事件：检测到停车位、完成位置调整
• 动作：低速前进、转向、停车

---

图示示例

下面给出一个高速公路巡航+变道的 FSM 状态转移图示例：

在这个例子中：

• 状态是矩形框（巡航、跟车、变道、避障）
• 事件是箭头上的条件
• 箭头方向表示状态转移方向

二、FSM 的原理

---

2.1 核心构成要素

在自动驾驶场景中，有限状态机（FSM）的核心要素包括：

2.1.1 状态（State）

• 表示车辆当前所处的驾驶模式或行为阶段
• 例如：巡航（Cruise）、跟车（Follow）、变道（Lane Change）、避障（Avoidance）、停车（Stop）
• 每个状态内部会对应一套具体的控制策略（如速度设定、转向角设定）

2.1.2 事件 / 触发条件（Event / Trigger）

• 由感知与环境判断结果触发
• 例如：
  • 前方车辆距离小于安全阈值
  • 检测到障碍物
  • 红灯亮起
  • 变道完成

2.1.3 动作（Action）

• 车辆在该状态下执行的具体控制指令
• 例如：
  • 加速、减速、匀速
  • 向左/右转向
  • 刹车、停车

2.1.4 状态转移函数（State Transition Function）

• 数学表示：

  $s_{t+1}=\delta (s_t,e_t)$

  其中：

  • $s_t$：当前状态
  • $e_t$：当前触发事件
  • $s_{t+1}$：下一状态
  • $\delta$：状态转移规则

---

2.2 FSM 的运行机制

2.2.1 初始状态设定

• 系统启动时，设定为一个默认安全状态（例如：低速巡航）
• 确保在缺乏环境信息时车辆不会采取危险动作

2.2.2 根据事件触发状态切换

• 实时监测感知数据
• 检查触发条件是否满足
• 若满足，执行状态转移

2.2.3 在当前状态执行对应动作

• 调用与状态绑定的控制策略
• 持续执行，直到有事件触发新的状态转移

---

2.3 状态转移图与转移表

2.3.1 状态转移图

直观地展示各状态之间的切换关系：

2.3.2 状态转移表

用表格形式列出所有可能的状态、事件和转移结果：

当前状态	触发事件	下一状态	动作
巡航	前方车减速	跟车	降低车速
跟车	距离不足	变道	向左打方向
变道	变道完成	巡航	保持新车道速度

---

2.4 自动驾驶场景下 FSM 的运行流程图

下面给出 FSM 在自动驾驶决策层的运行机制示意图：

在代码实现中，这个循环会不断执行，例如每隔 50ms 检查一次环境状态，并决定是否切换状态和执行新动作。

---

2.5 运行机制总结

• FSM 将复杂的驾驶任务分解成有限个离散状态
• 每个状态有清晰的触发条件和执行动作
• 这种设计让自动驾驶系统的决策逻辑可读性强、易于调试
• 但也存在状态数量爆炸和适应性差的问题，需要在后续设计中进行优化

三、FSM 的设计方法

---

3.1 场景需求分析

在构建 FSM 之前，需要明确自动驾驶任务场景和决策目标。

• 明确环境类型：高速公路、城市道路、停车场等
• 识别典型驾驶任务：巡航、变道、避障、停车等
• 确定决策层的输入信息：感知模块输出（车辆位置、障碍物距离、交通信号状态）

示例（高速公路场景）：

• 环境特征：车速较高、车道数量有限、障碍物较少
• 典型任务：巡航、跟车、超车、变道
• 关键输入：前车速度与距离、目标车道空闲情况、道路限速

---

3.2 状态集合的定义

• 将驾驶任务抽象为一组离散状态 $S$
• 状态划分需满足互斥性（同一时刻只能处于一个状态）和完整性（覆盖全部可能场景）

示例状态集合（高速公路）：

S={巡航,跟车,变道,避障,停车}S = \{ \text{巡航}, \text{跟车}, \text{变道}, \text{避障}, \text{停车} \}S={巡航,跟车,变道,避障,停车}

---

3.3 事件与条件的提取

• 从感知与预测模块输出中提取事件触发条件
• 事件应具有可检测性（传感器可测）、唯一性（不会和其他事件冲突）

示例事件：

• $e_1$：前车距离小于安全距离
• $e_2$：目标车道无障碍
• $e_3$：检测到静态障碍物
• $e_4$：停车信号触发

---

3.4 状态转移规则设计

状态转移规则由当前状态 + 事件 → 下一状态决定
数学形式：

$\delta (s,e)=s^{\prime }$

转移设计需遵循：

1. 安全优先：避免高风险切换（如高速直接停车）
2. 逻辑一致：不存在循环死锁（死循环转移）
3. 实时可达：紧急情况下快速进入安全状态

---

3.5 动作函数的实现

每个状态都对应一个动作函数 $a(s)$，调用底层控制器执行。
例如：

• 巡航：调用速度保持控制器
• 跟车：调用自适应巡航（ACC）
• 变道：调用路径规划 + 转向控制器
• 避障：调用轨迹重规划模块

---

3.6 防止状态爆炸的设计策略

由于自动驾驶环境复杂，FSM 容易出现状态数量随场景复杂度指数增长的“状态爆炸”问题。常用优化方法有：

3.6.1 层次化 FSM（Hierarchical FSM, HFSM）

• 将 FSM 分为高层任务状态与低层子状态
• 高层：决策任务（巡航、超车、停车）
• 低层：具体动作（加速、减速、转向）
• 优点：减少全局状态数，模块化维护

3.6.2 模块化 FSM（Modular FSM）

• 将不同功能（如避障、车道保持、速度控制）分离为独立 FSM
• 各 FSM 独立运行，通过调度模块协调

3.6.3 条件聚合与抽象

• 合并相似事件（如“前车距离小”与“前方障碍物接近”可归为“前方危险接近”）
• 抽象低频场景，减少冗余状态

---

图示示例：层次化 FSM 架构

四、自动驾驶中的 FSM 应用实例

---

4.1 高速公路驾驶 FSM

4.1.1 状态定义

在高速公路驾驶场景中，可以定义以下主要状态：

1. 巡航（Cruise）：在当前车道保持恒定速度行驶
2. 跟车（Follow）：当前方车辆速度低于限速且影响本车时，保持安全车距行驶
3. 变道（Lane Change）：在目标车道空闲时变更车道
4. 避障（Avoidance）：检测到障碍物时进行绕行
5. 减速停车（Stop）：在紧急情况下或出口处减速直至停车

---

4.1.2 事件条件

• E1：前方车距 < 安全阈值
• E2：目标车道无障碍且可变道
• E3：检测到静态障碍物
• E4：出口到达或接收到停车指令
• E5：变道完成
• E6：障碍物已清除

---

4.1.3 状态转移表

当前状态	事件条件	下一状态	动作
巡航	E1	跟车	降低速度，保持安全距离
巡航	E3	避障	规划绕行路径
巡航	E4	停车	减速至停止
跟车	E2	变道	打转向灯并执行变道轨迹
跟车	前方车速恢复	巡航	加速至巡航速度
变道	E5	巡航	保持新车道巡航
避障	E6	巡航	返回原车道并恢复速度

---

4.2 城市道路驾驶 FSM

4.2.1 状态定义

1. 起步（Start）：等待绿灯或启动信号后加速
2. 巡航（Cruise）：在城市道路上正常行驶
3. 等红灯（Wait at Red Light）：在路口红灯时停车等待
4. 通过路口（Intersection Pass）：在绿灯或允许通行条件下通过路口
5. 避障（Avoidance）：遇到行人、自行车或其他障碍物时绕行

---

4.2.2 事件条件

• C1：红灯亮起
• C2：绿灯亮起或允许通行信号
• C3：检测到障碍物
• C4：障碍物清除
• C5：路口通过完成

---

4.2.3 状态转移表

当前状态	事件条件	下一状态	动作
起步	C1	等红灯	刹车至停止
起步	C2	巡航	平稳加速
巡航	C1	等红灯	减速至停止
巡航	C3	避障	绕行或减速避让
等红灯	C2	起步	起步加速
避障	C4	巡航	恢复原路线速度
通过路口	C5	巡航	回归巡航模式

---

4.3 应用实例总结

通过这两个案例可以看到：

• 高速公路 FSM 更偏向连续高速运行的车道与车距管理
• 城市道路 FSM 更侧重交通信号与障碍物避让
• 两者都体现了 FSM 结构简单、逻辑清晰的特点，但在复杂环境下可能需要层次化 FSM 或混合决策方法提升适应性

五、FSM 的优缺点与改进

---

5.1 优点

1. 逻辑清晰
   • 每个状态都有明确的行为逻辑
   • 状态切换条件直观易懂，便于调试与验证
2. 实时性强
   • 状态切换与事件检测通常是 O(1) 的复杂度，适合毫秒级决策
3. 开发门槛低
   • 工程实现简单，可直接用 if-else 或 switch-case 完成原型设计
4. 安全可控
   • 在安全关键系统中，FSM 提供了可预测的状态与动作，不会出现不可解释的行为

---

5.2 缺点

1. 状态爆炸（State Explosion）
   • 场景复杂时，状态数量随条件组合指数增长
   • 例如：高速 + 城市 + 停车场全覆盖时可能需要上百个状态
2. 缺乏灵活性
   • 固定的状态转移规则无法应对未定义的环境变化
   • 容易在非预期场景下失效
3. 维护困难
   • 状态之间依赖多时，修改一个条件可能影响多个转移逻辑
4. 缺少学习能力
   • 无法自动适应新情况，需要人工不断添加状态与规则

---

5.3 改进方向

5.3.1 层次化 FSM（Hierarchical FSM, HFSM）

• 将 FSM 分为多层次：
  • 高层任务 FSM（巡航 / 超车 / 停车）
  • 低层动作 FSM（加速 / 转向 / 制动）
• 优点：减少全局状态数量，降低状态爆炸风险

---

5.3.2 模块化 FSM（Modular FSM）

• 将不同功能（如速度控制、车道保持、避障）拆分为独立的 FSM
• 各 FSM 独立运行，通过调度模块进行协调
• 有利于多团队并行开发与测试

---

5.3.3 FSM + 行为树（Behavior Tree, BT）混合

• 用 FSM 管理高层任务切换
• 用行为树执行具体任务逻辑（条件判断、任务分解）
• 提高系统的灵活性与可扩展性

---

5.3.4 FSM + 数据驱动方法

• 在状态转移条件判断中引入机器学习模型（如深度强化学习、神经网络预测器）
• 使得某些条件可自适应学习，而不是人工硬编码

---

5.4 改进示意图

下面是 混合式决策架构（FSM + BT + RL） 示例：

这种方式的好处是：

• FSM 管控整体流程，保持可预测性
• 行为树增强灵活性
• 机器学习提供自适应能力

六、FSM 工作流程总结

---

6.1 FSM 运行时序概述

在自动驾驶决策层，FSM 的运行是一个周期性循环：

1. 获取输入：实时读取车辆状态与环境感知信息
2. 检测事件：判断是否有事件触发条件满足
3. 状态转移：根据转移规则切换到新状态（若条件满足）
4. 执行动作：调用当前状态绑定的控制模块
5. 返回循环：进入下一周期，持续运行

---

6.2 工作流程步骤

Step 1：获取车辆与环境状态

• 从感知模块获取位置、速度、加速度、道路信息、交通信号、障碍物信息等
• 从预测模块获取目标物体未来运动趋势

Step 2：匹配当前状态的触发条件

• 检查当前状态对应的事件集合
• 如果某一条件被满足，记录触发事件 ID

Step 3：执行状态转移

• 根据状态转移函数 $\delta (s_t,e_t)$ 查找下一状态
• 如果无事件触发，保持当前状态

Step 4：调用状态动作函数

• 执行当前状态对应的控制策略
• 向路径规划与底层控制模块发送加速度、转向角等指令

Step 5：进入下一控制周期

• 等待下一个控制周期（如 50ms 或 100ms）
• 重复步骤 1～4

---

6.3 FSM 运行时序图

---

6.4 优势与适用性总结

• FSM 通过有限状态集合和明确的事件条件，将复杂的驾驶行为拆解为可管理的决策单元
• 适合场景：
  • 高速巡航与车道管理
  • 城市道路交通信号控制
  • 停车与出库
• 不适合场景：
  • 状态变化频繁且不规则的高度动态环境
  • 对未知环境需要自适应学习的任务

---

6.5 与自动驾驶全栈系统的关系

在典型的自动驾驶系统中，FSM 处于决策规划层：

感知层 (Perception)↓
预测层 (Prediction)↓
决策层 (Decision) —— FSM 决策管理↓
路径规划 (Planning)↓
控制层 (Control)

FSM 的职责是根据感知与预测的结果，选择合适的任务模式，并将任务指令传递给路径规划与控制模块执行。

七、FSM 与其他决策方法的对比

---

7.1 FSM vs 行为树（Behavior Tree, BT）

对比维度	FSM（有限状态机）	BT（行为树）
结构形式	状态节点 + 事件触发条件	树形节点结构（选择器、序列器、条件节点）
执行逻辑	当前状态唯一，事件触发才切换	按优先级或顺序遍历节点，实时选择可执行任务
可扩展性	扩展状态需增加转移规则，易状态爆炸	增加新行为只需添加节点
调试难度	逻辑简单，容易验证	节点组合复杂时调试难度增加
灵活性	低（固定逻辑）	高（可组合复杂行为）
典型应用	高速巡航、信号灯控制、停车流程	复杂交互、多任务并行决策

结论：

• FSM 更适合高层任务模式切换（如巡航 ↔ 跟车 ↔ 停车）
• BT 更适合复杂任务分解与执行（如多种避障策略、任务优先级管理）

---

7.2 FSM vs 规则引擎（Rule-based System）

对比维度	FSM	规则引擎
逻辑形式	状态 + 转移	规则集（IF 条件 THEN 动作）
灵活性	低	高
执行顺序	受限于状态转移表	可按优先级或触发条件动态执行
适用场景	状态有限、模式稳定的任务	条件多变、组合多样的任务
维护成本	增加新功能需调整状态机结构	仅需添加/修改规则

结论：

• FSM 在状态数有限且转移逻辑固定的情况下性能优越
• 规则引擎更适合规则频繁变化、且需要快速迭代的应用（例如交通法规更新）

---

7.3 FSM vs 数据驱动方法（强化学习 RL、深度学习）

对比维度	FSM	数据驱动方法
可解释性	高（逻辑完全可见）	低（黑箱模型）
适应性	差（需人工调整）	高（可通过训练自适应新环境）
实时性	高	中（需模型推理时间）
安全性	高（可预测）	低（需额外安全约束）
开发成本	低	高（需数据收集与训练）

结论：

• FSM 在安全关键场景中仍是首选
• 数据驱动方法适合用于改进 FSM 的转移条件或替代部分状态逻辑

---

7.4 混合决策趋势

现代自动驾驶系统中，纯 FSM 越来越少单独使用，而是与其他方法结合：

1. FSM + BT：FSM 负责高层任务切换，BT 负责任务内细节执行
2. FSM + 规则引擎：FSM 管理主流程，规则引擎动态调整局部策略
3. FSM + RL/ML：FSM 保证安全，RL 提供自适应能力

---

7.5 对比总结表

决策方法	优点	缺点	适用场景
FSM	简单可控，实时性强，可解释性高	灵活性差，状态爆炸	高速巡航、信号灯控制
行为树	灵活、可组合复杂任务	调试复杂，执行效率略低	多任务、多优先级管理
规则引擎	易扩展、条件可动态调整	逻辑不易整体把控	条件频繁变化的任务
数据驱动	自适应能力强，可处理复杂模式	可解释性差，需大量数据	不确定性高的动态环境