自动驾驶中的有限状态机（FSM）：从理论到实践，手把手教你设计一个高速公路巡航系统

自动驾驶决策核心：用有限状态机（FSM）构建一个健壮的高速公路巡航系统

在自动驾驶系统的决策层，工程师们常常面临一个核心挑战：如何将复杂、连续的现实世界驾驶行为，转化为计算机能够稳定、可靠执行的离散逻辑。这不仅仅是算法问题，更是一个关乎系统安全与可维护性的工程哲学。有限状态机（Finite State Machine, FSM），这个在计算机科学中历史悠久的概念，恰恰为这一挑战提供了一个优雅而坚实的起点。它并非最前沿的AI模型，但在处理规则明确、安全至上的驾驶任务时，其逻辑的清晰性、行为的可预测性以及实现的简洁性，使其成为许多量产自动驾驶系统中不可或缺的基石。

尤其在高速公路巡航这类场景中，驾驶任务相对结构化，状态边界清晰，FSM的优势得以充分发挥。本文将从一个实践者的视角出发，抛开教科书式的理论罗列，深入探讨如何从零开始，设计并实现一个面向真实工程环境的高速公路巡航FSM。我们将重点关注如何定义状态、设计健壮的转移条件、处理边界情况，并最终将其融入一个完整的软件模块中。我们的目标不是构建一个“玩具”Demo，而是打造一个具备工程化思维、能够应对现实复杂性的决策核心。

1. 理解FSM：超越五元组的工程视角

在开始编码之前，我们必须对FSM有一个超越数学定义的工程化理解。一个五元组 (S, E, A, δ, s0) 定义了它的骨架，但血肉在于如何将这些抽象元素映射到瞬息万变的驾驶环境中。

状态（State） 不应仅仅是“巡航”、“跟车”这样的标签。在工程实现中，一个状态是一个完整的、自洽的行为策略包。它至少包含：

• 目标生成器：根据当前状态，计算车辆的短期目标（如目标速度、目标车道中心线）。
• 控制器接口：决定向底层控制模块（如纵向的ACC、横向的LKA/LCC）发送何种指令。
• 内部计时器与计数器：用于处理超时、状态保持最小时间等逻辑，防止状态在边界条件附近高频振荡。
• 专属的安全检查：在该状态下需要持续监控的特殊安全条件。

例如，跟车状态不仅仅是“跟着前车”。它的行为策略包可能包括：基于前车速度和安全时距计算目标速度的算法、监控前车刹车灯的逻辑、以及判断前车是否已驶离本车道的条件。

事件（Event） 是状态转移的触发器，但它们很少是简单的布尔信号。一个健壮的系统需要将原始传感器数据（如毫米波雷达的距离、摄像头识别的车辆类型）融合并升华为具有语义意义的事件。这通常由一个独立的“事件检测器”模块完成。

注意：事件检测的延迟和误报/漏报会直接影响FSM的决策质量。在实践中，我们常对事件信号进行滤波（如持续一段时间才确认）和置信度加权，以提高鲁棒性。

动作（Action） 在自动驾驶上下文中，通常不是FSM直接执行的，而是FSM通过选择不同的状态，间接决定了由哪个底层控制器或规划器来执行动作。因此，FSM的“动作”更准确地说是行为策略的选择。

下面的表格对比了理论FSM与工程化FSM的核心要素差异：

要素	理论定义	工程化实践
状态 (S)	离散的符号标识	包含策略、目标、内部逻辑的“行为模式”对象
事件 (E)	抽象的输入符号	经过多传感器融合、滤波和语义理解后的稳定信号
转移函数 (δ)	数学映射关系	包含优先级、互斥锁和条件检查的决策逻辑表或函数
动作 (A)	输出函数	对底层控制器（速度/路径规划）的模式选择与参数设定

这种视角的转变至关重要。它意味着我们的FSM设计从一开始就要考虑模块化、可测试性和与系统其他部分的接口。

2. 高速公路巡航FSM的详细设计与状态拆解

让我们聚焦于高速公路巡航这一具体场景。一个看似简单的“从A点开到B点”的任务，可以被分解为多个相互协作又互斥的子行为。我们的目标是设计一个状态集合，既能覆盖所有关键场景，又尽可能保持简洁，避免“状态爆炸”。

2.1 核心状态定义及其行为策略

我们首先定义五个核心状态，这构成了系统的主干：

1. CRUISE (主动巡航)：这是系统的默认和理想状态。车辆在无前车干扰或前车距离很远时，按照设定的速度（可能是道路限速或用户设定值）行驶。其行为策略是跟踪一条光滑的参考路径（车道中心线），并维持一个恒定的速度。

   # 伪代码示例：CRUISE状态的行为策略
   class CruiseState:
       def execute(self, perception_data):
           target_speed = min(self.user_set_speed, self.road_speed_limit)
           target_path = self.lane_center_trajectory
           # 发送给控制器的指令
           return ControlCommand(speed=target_speed, path=target_path)
   

2. FOLLOW (自适应跟车)：当雷达检测到同车道存在前车，且其速度低于我车目标速度，距离进入安全跟车范围时触发。行为策略切换为自适应巡航控制（ACC），目标速度调整为前车速度，并保持一个动态的安全距离（通常与时距相关，如2秒）。

   • 关键细节：从CRUISE到FOLLOW的转移需要滞后设计，即“切入”跟车的距离阈值要略大于“切出”跟车的距离阈值，防止在两车速度接近时状态频繁跳动。

3. LANE_CHANGE_PREPARE (变道准备)：这不是一个执行变道动作的状态，而是一个决策与确认状态。当系统根据导航路线或超车需求决定需要变道时，进入此状态。它的行为是：

   • 持续监测目标车道的可行性（是否有车、距离、相对速度）。
   • 可能控制本车进行微小的横向偏移或速度调整，以创造更好的变道窗口。
   • 一旦条件满足并持续稳定一段时间（如1秒），则触发变道执行。
   • 如果超时或条件变差，则放弃变道，退回CRUISE或FOLLOW。

4. LANE_CHANGE_EXECUTE (变道执行)：这是真正的动作执行状态。它接收来自准备阶段规划好的变道轨迹（通常是一条平滑的曲线），并控制车辆严格跟踪该轨迹。此状态必须有一个明确的成功或失败终止条件：

   • 成功：车辆中心线越过目标车道线一定距离，且横摆角与车道对齐。
   • 失败：目标车道突然出现近距离切入车辆，或驾驶员干预。

   提示：变道执行是一个开环或短时闭环的过程，一旦开始，除非安全条件被违反，应尽量完成，避免在车道线上犹豫不决，这对其他交通参与者是危险的。

5. MINIMAL_RISK_CONDITION (最小风险状态)：这是整个FSM的安全基石，一个必须存在的“安全港”状态。当系统遇到无法处理的严重故障（如主要传感器失效、通信中断、或规划器报错），或检测到极端危险（如前方突然出现静止障碍物且无法避让）时，立即无条件切换到此状态。其行为策略是：在保证车辆稳定性的前提下，执行最保守的操作——打开双闪、平稳减速至停车，或靠边停车。

2.2 状态转移事件与条件设计

事件是状态的开关。设计时，我们需要明确定义每个事件的检测逻辑、信号来源和滤波方式。

事件代号	事件描述	检测逻辑（示例）	信号来源
E_LEAD_CAR_APPEAR	同车道出现有效前车	前车距离 < 120米，且持续5个周期（100ms）	前向雷达/视觉融合
E_LEAD_CAR_GONE	同车道前车消失或距离过远	前车距离 > 150米，或目标丢失持续1秒	前向雷达/视觉融合
E_LC_REQUEST	导航或超车逻辑请求变道	路径规划模块发出变道指令	决策上层模块
E_LC_CONDITION_MET	变道条件满足	目标车道前后安全空间充足，相对速度差小，持续稳定1秒	周视雷达/视觉
E_LC_COMPLETE	变道动作完成	车辆横向位置越过目标车道线，且航向角对齐	定位与车道线感知
E_LC_ABORT	变道需中止	目标车道车辆加速逼近，或驾驶员反向打方向盘	周视传感器/方向盘扭矩
E_FAULT_DETECTED	系统严重故障	关键传感器健康状态为无效，或控制器报错	系统监控模块
E_EMERGENCY_BRAKE	紧急制动需求	碰撞时间(TTC) < 紧急阈值，且制动距离不足	前向融合感知

将这些事件与之前定义的状态结合，我们可以绘制出核心的状态转移图。但更重要的是，在代码中实现一个清晰、易于维护的转移表。我们可以用一个字典结构来实现：

# 状态转移表的数据结构示例
transition_table = {
    State.CRUISE: {
        Event.E_LEAD_CAR_APPEAR: (State.FOLLOW, self._action_to_follow),
        Event.E_LC_REQUEST: (State.LANE_CHANGE_PREPARE, self._action_prepare_lc),
        Event.E_FAULT_DETECTED: (State.MINIMAL_RISK_CONDITION, self._action_enter_mrc),
    },
    State.FOLLOW: {
        Event.E_LEAD_CAR_GONE: (State.CRUISE, self._action_to_cruise),
        Event.E_LC_REQUEST: (State.LANE_CHANGE_PREPARE, self._action_prepare_lc),
        Event.E_EMERGENCY_BRAKE: (State.MINIMAL_RISK_CONDITION, self._action_emergency_brake),
    },
    State.LANE_CHANGE_PREPARE: {
        Event.E_LC_CONDITION_MET: (State.LANE_CHANGE_EXECUTE, self._action_execute_lc),
        Event.E_LC_REQUEST_CANCEL: (State.CRUISE, self._action_abort_lc_prep), # 取消请求
        Event.E_LEAD_CAR_BRAKE_HARD: (State.FOLLOW, self._action_abort_lc_prep), # 前车急刹，放弃变道优先跟车
    },
    # ... 其他状态的转移规则
}

这种结构化的定义方式，使得整个状态机的逻辑一目了然，添加或修改规则也非常方便。

3. 实现模式：从简单Switch到可维护的框架

有了清晰的设计，接下来就是如何用代码实现它。实现模式的选择直接影响代码的可读性、可扩展性和可测试性。

3.1 模式一：面向过程的Switch-Case（快速原型）

对于验证概念或极其简单的FSM，这是一种直接的方式。

// C++ 简单示例
enum VehicleState { CRUISE, FOLLOW, PREPARE_LC, EXECUTE_LC, MRC };
VehicleState current_state = CRUISE;

void updateFSM(const PerceptionData& data, const Event& event) {
    switch (current_state) {
        case CRUISE:
            if (event == E_LEAD_CAR_APPEAR) {
                current_state = FOLLOW;
                engageACC(); // 执行跟车动作
            } else if (event == E_FAULT) {
                current_state = MRC;
                activateMinimalRiskMode();
            }
            // ... 其他条件
            break;
        case FOLLOW:
            // ... 处理FOLLOW状态下的转移
            break;
        // ... 其他状态
    }
}

缺点：逻辑分散在各个case中，状态和动作耦合紧，添加新状态或修改转移规则时代码改动会像“霰弹枪修改”一样分散在各处，难以维护。

3.2 模式二：状态模式（State Pattern）—— 面向对象的最佳实践

这是工业级项目更青睐的方式。它将每个状态封装成一个独立的类，符合开闭原则。

# Python 状态模式示例
from abc import ABC, abstractmethod

class FSMState(ABC):
    """状态基类"""
    def __init__(self, context):
        self.context = context  # 持有FSM上下文，用于访问数据和切换状态

    @abstractmethod
    def enter(self):
        """进入该状态时执行一次"""
        pass

    @abstractmethod
    def execute(self, perception_data):
        """在该状态下周期性执行的动作"""
        pass

    @abstractmethod
    def handle_event(self, event):
        """处理事件，可能触发状态转移"""
        pass

    def exit(self):
        """离开该状态时执行一次"""
        pass

class FollowState(FSMState):
    def enter(self):
        self.context.logger.info("进入跟车状态")
        self.context.controller.switch_to_acc_mode() # 切换控制器模式
        self.time_entered = self.context.current_time

    def execute(self, perception_data):
        # 计算跟车目标速度和安全距离
        target_speed = self._calculate_target_speed(perception_data.lead_vehicle)
        self.context.controller.set_target_speed(target_speed)
        # 持续监控前车状态...

    def handle_event(self, event):
        if event == Event.E_LEAD_CAR_GONE:
            # 触发状态转移
            self.context.change_state(CruiseState(self.context))
        elif event == Event.E_EMERGENCY_BRAKE:
            self.context.change_state(MinimalRiskState(self.context))
        # 处理其他事件...

class FSMContext:
    """FSM上下文，管理当前状态和提供全局接口"""
    def __init__(self):
        self.current_state = None
        self.controller = VehicleController()
        self.logger = Logger()

    def change_state(self, new_state):
        if self.current_state:
            self.current_state.exit()
        self.current_state = new_state
        self.current_state.enter()

    def run_cycle(self, perception_data, events):
        """每个周期调用"""
        # 1. 处理事件
        for event in events:
            self.current_state.handle_event(event)
        # 2. 执行当前状态动作
        self.current_state.execute(perception_data)

优势：

• 高内聚：每个状态的行为和转移逻辑都封装在自己的类里。
• 易扩展：添加新状态只需新建一个类，无需修改其他状态类的代码。
• 易测试：每个状态类可以独立进行单元测试。

3.3 关键实现细节与“坑”

在实际编码中，有几个细节处理不好就会导致系统行为异常：

• 运行周期与实时性：FSM的run_cycle函数通常以固定频率（如10Hz或20Hz）被调用。必须确保每个周期的处理时间远小于周期时长，否则会丢失事件或导致控制延迟。
• 事件队列与优先级：一个周期内可能检测到多个事件。需要设计一个事件队列，并定义事件的优先级。例如，E_EMERGENCY_BRAKE事件的优先级必须高于E_LC_REQUEST。FSM在每个周期处理事件时，应优先处理高优先级事件。
• 状态转移的原子性：状态转移（change_state）和对应的exit()、enter()调用应该在一个周期内原子性地完成，避免在状态“半切换”时执行execute。
• 日志与调试：必须在每个状态的enter、exit和关键事件处理处打上详细的日志。这对于在线调试和事后分析FSM的决策流程至关重要。可以记录[时间戳] [状态] [事件] -> [新状态]这样的信息。

4. 进阶话题：应对复杂性与提升鲁棒性

一个基础的FSM可以工作，但一个工业级的FSM必须考虑更多。

4.1 层次化状态机（HFSM）化解“状态爆炸”

当我们的巡航系统需要增加更多功能，比如处理“施工区”、“大雨天气”、“汇入匝道”等场景时，基础状态数量会急剧增长。HFSM通过引入层级来解决这个问题。

设想一个高层FSM，负责管理如NORMAL_DRIVING、CONSTRUCTION_ZONE、TAKE_EXIT等宏观任务状态。而在NORMAL_DRIVING这个状态内部，又嵌套着一个我们前面设计的负责具体驾驶动作的底层FSM（包含CRUISE， FOLLOW等）。高层FSM的切换会重置或影响底层FSM的运行参数（例如，进入CONSTRUCTION_ZONE状态后，底层FSM的跟车时距会被拉大，目标速度会降低）。

# 简化的HFSM概念代码
class HighLevelState(ABC):
    def __init__(self):
        self.low_level_fsm = LowLevelFSM() # 底层FSM实例

    def activate(self):
        """激活此高层状态，并配置底层FSM"""
        self.configure_low_level_fsm()
        self.low_level_fsm.start()

class NormalDrivingState(HighLevelState):
    def configure_low_level_fsm(self):
        self.low_level_fsm.set_params(default_follow_time_gap=2.0, max_speed=120)

class ConstructionZoneState(HighLevelState):
    def configure_low_level_fsm(self):
        self.low_level_fsm.set_params(default_follow_time_gap=3.0, max_speed=80, lane_change_allowed=False)

4.2 与行为树（BT）的混合架构

对于变道准备（LANE_CHANGE_PREPARE）这种需要持续评估多种条件、可能涉及一系列子动作（如微调车速、等待窗口）的状态，纯FSM会显得笨拙。此时，可以在此状态内部嵌入一个行为树。

行为树擅长处理条件判断和任务序列。在变道准备状态中，行为树可以这样工作：

1. 选择器节点：持续检查变道条件。
2. 序列节点：如果条件满足，则执行“打开转向灯” -> “微调纵向位置” -> “确认最终窗口”等一系列子任务。
3. 只有当行为树成功执行完序列，才触发E_LC_CONDITION_MET事件，让FSM切换到LANE_CHANGE_EXECUTE状态。

这种混合模式让FSM继续负责宏观、互斥的高层行为模式管理，而将复杂的、有条件序列的逻辑交给更灵活的行为树处理。

4.3 数据驱动优化转移逻辑

FSM的转移条件（事件）最初都是人工设定的规则（如“距离<50米”）。我们可以利用机器学习模型来优化这些阈值，甚至预测事件的发生。例如：

• 使用一个轻量级模型，根据历史跟车数据，动态预测更舒适、更安全的最佳跟车时距，而非固定值。
• 使用分类模型，综合目标车道前后车的速度、加速度、行为预测，来更准确地判断E_LC_CONDITION_MET事件是否成立，减少误判。

这相当于用数据“软化”了FSM的硬规则边界，使其行为更拟人、更适应不同交通流风格，而FSM的核心框架依然保证了决策的可解释性和安全性底线。

4.4 全面的测试策略

FSM的测试不能只靠路测。必须建立多层次的测试体系：

• 单元测试：针对每个State类的handle_event和execute方法，模拟各种输入，验证输出和状态转移是否正确。
• 集成测试：在仿真环境中，构建完整的感知-决策-控制闭环，用海量的场景库（包括 corner cases）测试整个FSM的决策链。
• 硬件在环测试：将FSM代码部署到真实的域控制器中，与虚拟的车辆动力学模型和传感器模型对接，测试其实时性和资源占用。
• 影子模式：在实车上运行FSM但不执行控制，将其决策与人类驾驶员的决策进行对比，收集差异数据，用于分析和优化。

设计一个用于高速公路巡航的FSM，是一个在“简洁”与“完备”、“确定”与“灵活”之间寻找精妙平衡的过程。它没有深度学习模型那样的黑箱魅力，但其每一行代码、每一个状态转移都闪烁着工程师对安全与责任的深刻理解。从清晰的状态定义开始，用健壮的事件检测驱动，通过可维护的模式实现，再辅以层次化、混合架构等进阶手段来应对复杂性，这样一个FSM就能成为你自动驾驶系统决策层最可靠的中坚力量。在实际项目中，我最大的体会是，FSM的设计文档和代码本身一样重要，它必须是活的，随着对场景理解的深入而不断迭代。最后，永远为那个MINIMAL_RISK_CONDITION状态留好退路，这是智能驾驶系统对安全最基本的敬畏。