自动驾驶决策算法 —— 有限状态机 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 常用于：

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

而在更复杂的任务（如城市混合交通环境），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 状态转移表

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 状态转移表

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 更适合复杂任务分解与执行（如多种避障策略、任务优先级管理）

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

结论：

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

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

结论：

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

7.4 混合决策趋势

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

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

7.5 对比总结表