从零搭建自动驾驶中间件（五）：状态机、诊断与运维——让系统“可观测、可控制“

这是系列第五篇。前四篇聊了通信、调度、回灌这些"运行时"能力，这篇聊三个让系统从"能跑"变成"可靠"的关键能力：状态机、诊断、运维。

1. 为什么需要这三个能力？

自动驾驶系统不是跑一次就结束的批处理任务，它是一个7×24 运行的嵌入式实时系统。这意味着：

没有这三个能力，你的系统在实验室里跑得再好，一上车也是"裸奔"。

2. 状态机：给系统一个"大脑"

2.1 自动驾驶系统为什么需要状态机？

自动驾驶系统的行为因状态而异：

INIT 状态：
– 所有模块初始化
– 传感器自检
– 等待 EOL（End of Line）信号

STANDBY 状态：
– 传感器正常工作
– 算法模块就绪
– 等待驾驶员触发"启动自动驾驶"

AUTONOMOUS 状态：
– 完全自动驾驶模式
– 执行感知→预测→规划→控制全链路

EMERGENCY 状态：
– 最小风险策略（靠边停车）
– 降级运行
– 等待人工接管

没有状态机的系统就像一个不知道自己在干嘛的机器人——它不知道什么时候该干什么，也无法在异常发生时做出正确的响应。

2.2 HyperFlow 的层次化状态机

HyperFlow 实现了一个层次化有限状态机（HFSM），以 Module 插件形式集成。

层次化意味着一个状态可以包含一个子状态机：

RootStateMachine
├─ INIT ──── InitSubMachine
│ ├─ SELF_TEST
│ └─ WAIT_EOL
├─ STANDBY
├─ AUTONOMOUS ──── DrivingSubMachine
│ ├─ CRUISE
│ ├─ LANE_CHANGE
│ └─ INTERSECTION
└─ EMERGENCY

子状态机让复杂的状态转换变得清晰——根状态机只关心大状态之间的跳转，子状态机负责细粒度的内部状态管理。

2.3 三种转换触发方式

方式一：事件触发

// 业务代码中触发事件
state_machine_module->Fire(EVENT_EMERGENCY);

方式二：超时自动转换

{
"name": "init_timeout",
"event_id": 2,
"to_state": 5,
"timeout": 5000,
"condition": "power_on == 1"
}

INIT 状态停留超过 5 秒且 power_on == 1，自动跳转到 NORMAL 状态。

方式三：条件表达式守卫

{
"name": "eol_event",
"event_id": 1,
"to_state": 2,
"condition": "speed == 0 && power_on == 1"
}

只有当 speed == 0 且 power_on == 1 时，事件 1 才能触发从 INIT 到 EOL 的跳转。

条件表达式基于 muParser 引擎，支持数学运算和逻辑表达式：

speed == 0 && power_on == 1
speed > 60
gear == 4 && brake_pressure < 10

2.4 状态机与框架的集成

状态机不仅仅是个独立的状态管理器，它与 HyperFlow 的通信和触发机制深度集成：

┌────────────────────────────────────────────────────────┐
│ StateMachineModule │
│ │
│ 状态变更时： │
│ ├── Write("sys.state", current_state_id) → 共享内存 │
│ │ 其他模块: Read("sys.state") → 获取当前状态 │
│ │ │
│ └── Notify("sys.state_changed") → Trigger │
│ 其他模块: Wait("sys.state_changed") → 被唤醒 │
│ │
│ 表达式变量更新： │
│ ├── SetValue("speed", 60.0) │
│ └── 下次 OnGuard() 自动使用新值 │
└────────────────────────────────────────────────────────┘

这意味着：

• 感知模块可以通过 Read("sys.state") 获取当前系统状态，根据状态调整算法参数
• 规划模块可以通过 Wait("sys.state_changed") 等待状态变更，切换规划策略
• 状态机的表达式变量（如 speed）可以从共享内存中的车辆数据实时更新

2.5 一个自动驾驶状态机的配置实例

{
"name": "system_state",
"type": "sys.StateMachineModule",
"interval": 100,
"expr_values": {
"speed": 0,
"power_on": 0,
"emergency_flag": 0
},
"state_machines": [
{
"name": "root",
"is_root": true,
"init_state": 1,
"states": [
{
"id": 1,
"name": "INIT",
"transitions": [
{
"name": "eol",
"event_id": 1,
"to_state": 2,
"condition": "speed == 0 && power_on == 1"
},
{
"name": "init_timeout",
"event_id": 2,
"to_state": 4,
"timeout": 30000,
"condition": "power_on == 0"
}
]
},
{
"id": 2,
"name": "STANDBY",
"transitions": [
{
"name": "engage",
"event_id": 3,
"to_state": 3,
"condition": "speed > 0 && emergency_flag == 0"
}
]
},
{
"id": 3,
"name": "AUTONOMOUS",
"transitions": [
{
"name": "disengage",
"event_id": 4,
"to_state": 2
},
{
"name": "emergency",
"event_id": 5,
"to_state": 4,
"condition": "emergency_flag == 1"
}
]
},
{
"id": 4,
"name": "EMERGENCY",
"transitions": [
{
"name": "recovered",
"event_id": 6,
"to_state": 2,
"condition": "emergency_flag == 0 && speed == 0"
}
]
}
]
}
],
"notify": ["sys.state_changed"],
"output": ["sys.state"]
}

2.6 运行时调试

通过 Telnet 可以实时操控状态机：

$ telnet localhost 8800
> cd modules/system_state/state_machine

# 查看当前状态
> state
1

# 查看所有表达式变量
> value
speed = 0
power_on = 0
emergency_flag = 0

# 设置变量值
> set power_on 1
done
> set speed 0
done

# 触发事件
> fire 1
switched

# 确认状态已切换
> state
2

这在实车调试时非常实用——不需要修改代码、不需要重启进程，一个 Telnet 命令就能切换系统状态。

3. 诊断系统：让故障无处遁形

3.1 为什么需要诊断？

自动驾驶系统中，故障是常态：

• 激光雷达被遮挡
• 摄像头帧率下降
• GPS 信号丢失
• CAN 总线超时
• 算法输出异常

问题不在于故障是否发生，而在于能否及时发现并处理。

3.2 AUTOSAR 风格的诊断

HyperFlow 的诊断系统参考了 AUTOSAR 的 DTC（Diagnostic Trouble Code）管理：

┌──────────────────────────────────────────────────────────┐
│ DiagModule (集中式诊断管理) │
│ │
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │
│ │ DiagInfo 1 │ │ DiagInfo 2 │ │ DiagInfo 3 │ │
│ │ 传感器检查 │ │ 通信超时 │ │ 算法异常 │ │
│ │ stable: 3次 │ │ stable: 5次 │ │ stable: 2次 │ │
│ │ priority:HIGH│ │ priority:MED │ │ priority:LOW │ │
│ └──────────────┘ └──────────────┘ └──────────────┘ │
│ │
│ Step(): 周期检查所有 DiagInfo │
│ ├── 状态变更 → 更新 DTC 位图 │
│ ├── 故障上报 → ReportFail(event_id, priority) │
│ ├── 恢复上报 → ReportResume(event_id, priority) │
│ └── DTC 更新 → SendDtcData() → MCU │
└──────────────────────────────────────────────────────────┘

3.3 稳定性判断

诊断系统最重要的概念是稳定性判断：一次异常不能直接报故障，必须连续多次异常才确认。

时间轴: t1 t2 t3 t4 t5
传感器: OK FAIL FAIL FAIL OK
↑
连续 3 次 FAIL → 确认故障
stable_count = 3

{
"diag_infos": [
{
"enable": true,
"enable_report": true,
"stable_count": 3,
"stable_time": 1000,
"check_time": 100
}
]
}

• stable_count：连续异常多少次确认故障
• stable_time：异常持续多久确认故障（ms）
• check_time：检查间隔（ms）

3.4 DiagComponent：模块内诊断

算法模块通过 DiagComponent 上报故障和恢复：

class PerceptionModule : public Module {
DiagComponent diag_;

void Step() override {
const auto* cloud = nullptr;
auto ec = Get("lidar_points", cloud);

if (ec || cloud == nullptr) {
diag_.ReportFail(EVENT_LIDAR_FAILURE, Priority::HIGH);
} else {
diag_.ReportResume(EVENT_LIDAR_FAILURE, Priority::HIGH);
}
}
};

ReportFail/ReportResume 的信息会通过共享内存传递给 DiagModule，由 DiagModule 统一管理 DTC 状态。

3.5 DTC 故障码

DTC 是车载诊断的标准格式，用于与 MCU 通信：

struct DtcData {
uint32_t dtc; // DTC 类型
uint32_t bits[32]; // 故障位图

bool CheckDtcType(uint32_t type);
bool CheckDtcBit(uint32_t bit);
};

DiagModule 维护一个 DTC 位图，每当诊断状态变更时更新位图，并通过 SendDtcData() 虚函数发送到 MCU：

class MyDiagModule : public DiagModule {
void SendDtcData() override {
// 通过 CAN/SOME-IP 发送 DTC 到 MCU
can_bus_->SendDtc(current_dtc_);
}
};

4. 进程管理：让系统自愈

4.1 为什么需要进程管理？

自动驾驶系统通常由多个进程组成：

Process: perception (CPU: 4核, 内存: 4GB)
Process: planning (CPU: 2核, 内存: 2GB)
Process: control (CPU: 1核, 内存: 512MB)
Process: lidar_driver (CPU: 1核, 内存: 256MB)

如果 perception 进程因为一个 segfault 崩溃了：

• 没有进程管理：整个系统挂掉，车失控
• 有进程管理：自动重启 perception，其他进程不受影响

4.2 ProcessManager

HyperFlow 的 ProcessManager 负责子进程的生命周期管理：

struct ProcessInfo {
string name;
string exe; // 可执行文件路径
string work_dir; // 工作目录
string args; // 启动参数
string envs; // 环境变量
string cgroup; // cgroup 资源限制

pid_t pid;
ProcessStatus status; // INIT / RUNNING / STOP / QUIT / ERROR
float cpu_usage;
uint64_t vm_rss_kb;
uint64_t vm_size_kb;
uint32_t wait_time; // 重启等待时间
bool enable;
};

核心能力：

4.3 cgroup 资源限制

在嵌入式平台上，必须防止某个进程吃光所有资源：

// process_manager.json
{
"cgroup": {
"enable": true,
"config": "config/cgconfig.conf"
}
}

// cgconfig.conf
group perception {
cpu {
cpu.shares = 512;
cpu.cfs_quota_us = 400000; // 4核 × 100ms
cpu.cfs_period_us = 100000;
}
memory {
memory.limit_in_bytes = 4G;
memory.swappiness = 10;
}
}

group control {
cpu {
cpu.shares = 256;
cpu.cfs_quota_us = 100000; // 1核 × 100ms
cpu.cfs_period_us = 100000;
}
memory {
memory.limit_in_bytes = 512M;
memory.swappiness = 0;
}
}

为什么给 control 更高的优先级？

控制模块虽然 CPU 占用低，但对实时性要求最高。通过 cpu.shares 保证它能在需要时优先获得 CPU 时间。

4.4 ProcessMonitor

ProcessMonitor 是一个 Module，周期检查子进程状态：

void Step() override {
check_running(); // 检查进程是否存活
check_stop_timeout(); // 检查停止超时
stat_process(); // 统计进程 CPU/内存
stat_system_resources(); // 统计系统级资源
process_command(); // 处理远程控制命令
}

check_running() 的实现：

void check_running() {
for (auto& [name, info] : processes_) {
if (!info.enable) continue;
if (info.status != ProcessStatus::RUNNING) continue;

int status;
pid_t ret = waitpid(info.pid, &status, WNOHANG);
if (ret == 0) continue; // 进程正常运行

// 进程已退出
if (WIFEXITED(status)) {
LOG(WARNING) << "Process " << name << " exited with code " << WEXITSTATUS(status);
} else if (WIFSIGNALED(status)) {
LOG(WARNING) << "Process " << name << " killed by signal " << WTERMSIG(status);
}

// 等待一段时间后重启
info.status = ProcessStatus::QUIT;
info.wait_time = 5; // 5秒后重启
}
}

4.5 远程进程控制

通过 ProcessControl 模块可以远程启停子进程：

// 在任何模块中
ProcessComponent proc;
proc.Init(this);
proc.StopProcess("perception"); // 停止感知进程
proc.StartProcess("perception"); // 重新启动
proc.GetProcessStatus("perception", &status); // 查询状态

结合 Terminal，可以在运行时远程控制进程：

$ telnet localhost 8800
> cd process
> ls
– status
– start
– stop
> status
perception: RUNNING, CPU=45%, MEM=2.1GB
planning: RUNNING, CPU=12%, MEM=512MB
control: RUNNING, CPU=3%, MEM=128MB
> stop perception
done
> start perception
done

5. 三者的协同：状态驱动的故障响应

状态机、诊断、进程管理不是孤立的，它们协同工作构成了完整的"可观测、可控制"体系：

┌──────────────────────────────────────────────────────────────┐
│ 故障响应流程 │
│ │
│ 1. DiagModule 检测到激光雷达连续 3 帧数据异常 │
│ └── ReportFail(EVENT_LIDAR_FAILURE, HIGH) │
│ │
│ 2. DiagModule 更新 DTC 位图 │
│ └── DTC[lidar_failure] = 1 │
│ │
│ 3. 状态机模块检测到故障条件 │
│ └── expr_values: emergency_flag = 1 │
│ └── State: AUTONOMOUS → EMERGENCY │
│ └── Notify("sys.state_changed") │
│ │
│ 4. 规划模块被唤醒，切换到最小风险策略 │
│ └── Read("sys.state") → EMERGENCY │
│ └── 执行靠边停车 │
│ │
│ 5. ProcessMonitor 记录事件日志 │
│ └── 日志: 故障时间、DTC码、状态转换记录 │
│ │
│ 6. 驾驶员接管后，故障恢复 │
│ └── ReportResume(EVENT_LIDAR_FAILURE, HIGH) │
│ └── emergency_flag = 0 │
│ └── State: EMERGENCY → STANDBY │
└──────────────────────────────────────────────────────────────┘

6. 与 ROS2 / CyberRT 的运维能力对比

HyperFlow 在运维能力上的核心优势：将状态机、诊断、进程管理深度集成，形成完整的故障检测→状态切换→恢复链路。

7. 踩坑总结

8. 小结

状态机、诊断、运维是让自动驾驶系统从"能跑"变成"可靠"的三个关键能力：

• 状态机：给系统一个"大脑"，让它在正确的时机做正确的事
• 诊断：给系统一双"眼睛"，让故障无处遁形
• 运维：给系统一双"手"，让它能自我修复

HyperFlow 将三者深度集成，形成了完整的"检测→切换→恢复"链路。这是 ROS2 和 CyberRT 都不具备的能力。

下一篇，也是最后一篇，我们将聊一个常被忽视但极其重要的功能——交互式终端。

下期预告：《从零搭建自动驾驶中间件（六）：交互式终端——为嵌入式系统注入灵魂》