如何设计自动驾驶系统的状态机

状态机模块在自动驾驶系统中扮演着关键的角色，它负责管理和控制各个功能的状态转换和行为执行。今天我们来聊聊如何设计自动驾驶系统的状态机 。

0.闲谈

作为自动驾驶系统工程师，从参与项目开始，就必不可少的与状态管理模块打交道，因为状态机在系统运行的全功能周期内起管理作用。状态机这个模块，从技术实现角度来说，并没有什么难度，在网上有很多关于FSM(Finite-state machine）的介绍文章，有兴趣可以自行了解。但如何设计得巧妙、周到、精致，却很考验设计者的底蕴与对系统的理解。

大部分的ADAS功能都需要状态机进行状态管理，笔者手中就有不下十几份状态机的设计文档，包括FCW/LDW/AEB/ACC/LKA/NOP/APA/AVP等等，设计大相径庭，但细细想来内核却大同小异。其中NOP功能的状态跳转还是比较复杂的，涉及横向、纵向控制与功能降级等逻辑，需要长期的雕琢与迭代才能设计出符合项目要求的效果。

笔者最近也在负责APA功能的状态机设计，虽然比较简单，但还是想借此机会对状态机模块做一点总结，也是对以往工作的回顾。

1.模块概述

状态机模块的主要作用是跟踪系统的当前状态，并根据特定的事件和条件进行状态转换。它可以根据传感器数据、车辆状态和系统输入来判断当前功能的可用性和执行条件。状态机模块还能够监控系统的运行情况，及时响应来自驾驶决策或用户的指令，并根据需要触发相应的功能执行。

状态机模块通过定义和维护一组状态，以及状态之间的转换条件和行为，确保系统在不同的场景和条件下正确地执行相应的功能。例如，当检测到前方车辆与本车距离过近时，FCW功能会被触发，状态机模块会根据预设的逻辑条件和行为来切换到相应的预警状态，并触发声音或振动等警示措施。

状态机模块的设计需要考虑各个功能之间的优先级、依赖关系和冲突情况。它需要具备灵活性和可扩展性，以应对不同的道路情况和交通场景。比如cat-in、cat-out情景、自动变道时的变道空间判断等等。同时，状态机模块还需要具备高效的算法和实时性能，以保证系统的快速响应和可靠性。

总之，状态机模块在自动驾驶系统中扮演着决策和控制的关键角色。通过有效地管理和控制各个功能的状态转换和行为执行，状态机模块能够确保系统的稳定性、安全性和可靠性，它是实现ADAS功能的基础模块之一。

2.设计原理

状态机的设计原理是基于状态、事件和转换的概念。状态表示系统或功能在某一时刻的特定状态，事件表示触发状态转换的条件，而转换则表示状态之间的切换过程。

首先，需要定义系统或功能的各个状态。状态可以是具体的行为状态，也可以是抽象的控制状态。每个状态都代表了系统的一个特定方面或功能，例如“Standby”、“OFF”、“Parking”等。

接下来，定义触发状态转换的事件。事件可以是传感器的触发信号、用户的输入指令或系统内部的条件判断。通过检测事件的发生，状态机能够判断是否需要进行状态转换。

然后，定义状态之间的转换条件和行为。转换条件是判断是否可以进行状态转换的逻辑条件，例如满足一定的时间限制、特定的传感器数据或用户指令。转换行为是在状态转换时执行的操作，例如触发警报、调整车速或切换到下一个状态。

为了更好地设计和管理状态机，可以使用状态表和状态转换图。状态表是一个表格，列出了系统的各个状态和相应的转换条件。状态转换图则是通过节点和箭头表示各个状态和转换，直观地展示了状态之间的关系和转换规则。

EA示例 状态转换图

EA示例 状态表

在设计状态机时，需要确保其特性，包括确定性、完备性和可达性。

确定性表示每个状态都有明确的转换规则，不会出现歧义或冲突。

完备性表示系统的所有可能状态都被考虑到，并定义了相应的转换规则。

可达性表示系统能够从任意初始状态达到目标状态，确保状态机的可靠性和稳定性。

通过使用状态机的设计原理，可以清晰地定义系统的各个状态和转换规则，确保系统在不同的条件下正确地执行相应的行为和功能。同时，状态机的设计原理也为系统的扩展和维护提供了便利，使得系统能够适应不断变化的需求和环境。

4.定义状态

状态定义：明确定义系统中的状态，确保每个状态都是清晰且互斥的。每个状态应该具有明确的含义和行为。

不同的自动驾驶功能可能具有不同的状态机，状态的定义是由功能决定的，系统需要在不同的条件下正确地执行相应的行为和功能。同时，状态机的设计原理也应该为系统的扩展和维护提供了便利，使得系统能够适应不断变化的需求和环境；比如在NOP功能设计时，应考虑到如何与LCC/ACC等功能进行切换。

L0的ADAS功能，比如FCW/LDW/BSD等等基本不涉及车辆控制，仅起到预警作用。其状态定义也较为简单，可以分为以下几个状态：

OFF:系统关闭，未进行工作。

Standby：系统满足运行条件，正在待机。

Warning：符合预警触发条件，正在报警。

Inhibit：预警被用户抑制。

Error：系统故障。

根据国标法规要求，Warning状态中可能还包括Pre-Warning状态。

L1的AEB/LCC等功能仅涉及横向或纵向的单一控制，比如AEB主要负责紧急制动功能，会在FCW Warning后增加Brake的状态。而ACC根据功能定义，会存在Speed Control、Distance Control、Override、Temp Stop等状态。LCC则会存在Lane Keeping等状态。由此可以看出，状态机主要是为功能服务，围绕核心功能而分解为不同状态。

State Flow 逻辑系统建模教材 图来源于笔者

由于L2级别的NOP功能需要同时对车辆进行横向、纵向控制，主状态中的横向控制和纵向控制状态会同时在运行，受限于ODD范围还存在降级的要求，比如Safe Stop等状态，逻辑相对复杂，对兼容性要求较高。

而泊车功能状态机，当然会存在Searching（搜索车位）、Parking（泊车过程）、Suspend（泊车中断）、Completed（泊车完成）等状态。

5.事件定义

事件定义：识别系统中可能发生的事件，并为每个事件定义清晰的触发条件。事件可以是传感器输入、用户操作或其他系统内外的变化。

举例说明，在很多系统中，OFF->Standby的触发事件为“车辆的为IGN ON状态”即车辆已经上电，这个事件可以被认为是车辆状态输入。Stadnby->Active的触发事件可能为“用户点击中控屏幕上的功能开启按键”，这个事件可以被认为是用户操作，等等。

如果系统中存在多个事件，并且它们可以同时发生，那么需要考虑事件的优先级。事件优先级决定了在多个事件同时触发时，哪个事件会被优先处理。例如，紧急制动事件可能具有比前方障碍物检测预警事件具备更高的优先级。

6.转换规则

转换规则：确定状态之间的转换规则，即在特定事件发生时如何从一个状态转换到另一个状态。转换规则应该考虑到系统的逻辑和约束条件。

当我们考虑转换规则时，应当尽量全面而细致，确保状态机的完备性，即确保状态之间的所有可能转换都已经定义和覆盖。缺乏完备性可能导致系统出现未定义的行为或状态异常，从而使状态机跳入死循环无法跳出bug。通过仔细分析系统需求和设计，期望确保所有可能的状态转换都被考虑和定义。

7.动作和行为

动作和行为：为每个状态和状态转换定义相应的动作和行为。这些动作可以是执行特定的功能、发送控制命令、更新状态变量等。

当我们考虑状态时，可以辅以该状态下的工作描述。比如在APA自动泊车系统中，当我们进入Parking状态后，状态机应当负责与车辆控制系统进行握手，握手成功后由Control模块进行控车。（此处仅为举例，握手也可以由其他模块完成）

8.错误处理

错误处理：考虑系统可能发生的错误和异常情况，并为其设计相应的处理机制。包括错误状态的定义、错误处理流程以及恢复机制，一般统一由Error状态进行处理和管理。

9.代码生成与测试

状态机是一个比较有历史的东西，很多厂家为了它做了相关的开发，方便我们使用和测试。现在有很多成熟的能够自动生成状态机代码的工具，笔者以前也参与过相关项目，公司使用的是MATLAB/Simulink中的一个工具库State Flow，还为此斥巨资30大洋买过一本书，简单聊聊。

Stateflow是MATLAB/Simulink中的一个工具，用于建模和设计复杂的状态机。它提供了一个图形化界面，可以轻松地创建、编辑和调试状态机模型，并自动生成相应的代码。

Stateflow的主要功能包括以下几个方面：

状态机建模：Stateflow提供了丰富的建模元素，如状态、转换、事件、条件、动作等，可以直观地描述系统的状态和状态之间的转换关系。通过拖拽和连接这些建模元素，可以轻松地构建状态机模型，并定义状态之间的转换条件和相应的动作。

事件驱动：Stateflow基于事件驱动的模型执行方式。系统的状态转换是通过触发事件来驱动的。可以定义各种类型的事件，如输入信号、定时器、条件判断等。当满足转换条件时，Stateflow会相应地执行转换并触发相应的动作。

动作执行：在状态转换过程中，可以指定特定的动作或行为。这些动作可以是简单的赋值操作、函数调用，或者是复杂的算法逻辑。Stateflow提供了丰富的动作语言和函数库，可以灵活地定义状态转换过程中需要执行的动作。

状态检测：Stateflow提供了灵活的状态检测机制，可以检测系统处于的当前状态，以及特定条件下的状态转换。这些检测可以用于系统的监控、故障检测和安全保护等方面。

代码生成：一旦完成状态机的建模和设计，Stateflow可以自动生成相应的代码，以便集成到实际的系统中。生成的代码可以是C/C++代码、MATLAB函数或Simulink模块，可以与其他系统组件进行无缝集成。

State Flow开发界面 图来源于网络

开发界面如上图所示，总的来说该软件还是比较好用的，上手简单，对于代码能力要求较低，只要跑通流程后，后续开发和维护的难度较小。在调试方面，该软件也非常出色，能够拉出每个信号的值直接进行仿真验证，有助于快速定位和解决问题。

刚毕业那阵几个同届的小姑娘在软件组就是负责开发状态机的，后来都做得有声有色以至于现在都去了各大供应商卷了，毕竟这种技能一般还是在Tier 1零部件供应商那边比较有用....

10.结语

以上是笔者总结的一些状态机设计的要点，根据具体的应用场景和系统需求，可能会有其他特定的要求和注意事项。在设计状态机时，重点是要充分理解系统的功能和目标，并与团队成员进行密切合作，确保设计的状态机能够满足系统的要求和预期效果。

在开发阶段，设计也可以由简到繁，先思考清楚有哪几个状态，确认每个状态的跳转条件应该包含哪些方面，然后根据项目进度不断去迭代设计。笔者在项目中的设计状态机时，在初版需要思考清楚系统边界，但每个状态的跳转条件可能只有一个激活信号，或者车速信号；后续在开发不断成熟的过程中，再逐步丰富设计，完善子状态与跳转条件，这样能够使开发既敏捷又稳重。