自动驾驶决策算法实战：从状态机到POMDP的工程取舍

1. 从感知到决策：自动驾驶最容易被低估的一环

聊自动驾驶，大部分人第一反应是激光雷达、摄像头、BEV感知、端到端大模型这些热词。感知确实重要，它决定了车“看得见”还是“看不见”。但真正开过项目、跑过实车的人心里都清楚：感知决定下限，决策决定上限。一辆车能不能在拥堵路口果断汇入、能不能在加塞时既不怂也不莽、能不能在鬼探头前一秒做出合理避让，靠的不是感知有多准，而是决策算法在那一瞬间怎么权衡。

我接触决策算法这几年，最大的感受是：它不像感知那样有明确的benchmark可以刷分，也不像控制那样有清晰的数学推导。决策处在中间地带，上接感知的不确定性，下接控制的执行约束，还要面对人类驾驶行为的随机性。说白了，决策算法要解决的核心问题是：在信息不完备、未来不可完全预测的情况下，选出一条既安全又合理、既符合交规又贴近人类预期的行为序列。

这篇文章适合谁看？如果你刚入行自动驾驶，正在搞清楚各模块的分工，那这篇能帮你建立决策模块的整体认知；如果你已经在做规划控制，但对决策层的设计取舍还有困惑，那这篇里关于行为树、状态机、POMDP、学习类方法的对比和踩坑经验，应该能给你一些参考。我不打算写成教科书，而是按一个实际做过项目的人的角度，把决策算法里那些“文档里不会写、但上手就会遇到”的东西摊开来讲。

2. 决策算法的核心定位与设计思路拆解

2.1 决策到底在系统里干什么

先把分工说清楚。感知输出的是障碍物的位置、速度、类别、朝向，以及道路结构、车道线、交通标志。预测模块给出其他交通参与者未来几秒的可能轨迹。决策模块拿到的就是这些信息，然后要回答几个问题：我现在该保持车道还是变道？遇到前车慢行是跟车还是超车？路口右转时要不要让行直行电动车？如果让，让到什么程度可以走？

决策的输出通常不是具体的油门刹车曲线，而是行为层面的指令，比如“跟车”“变道”“让行”“停车等待”，再交给下游的规划模块去做轨迹优化，最后控制模块执行。有些架构里决策和规划是合并的，叫“行为规划”或“运动规划”，但逻辑上决策负责“做什么”，规划负责“怎么做”。

这里有个常见的误解：很多人以为决策就是写一堆if-else。早期确实是这样，但现在的场景复杂度早就超出了手写规则能覆盖的范围。一个城市路口，同时有行人、非机动车、对向车、同向车、信号灯、待转区，组合爆炸。所以决策算法的演进，本质上是在可解释性和泛化能力之间找平衡。

2.2 为什么选有限状态机作为起点

如果你去看大多数量产项目的决策模块，底层大概率还是有限状态机（FSM）或者行为树（BT）。为什么？因为可解释、可调试、可验证。状态机的每个状态和转移条件都是显式定义的，出了问题能定位到具体哪条转移逻辑写错了。这在功能安全要求极高的场景下，是刚需。

我参与过一个项目，决策层最初用纯规则状态机，状态包括：车道保持、跟车、变道准备、变道执行、避让、紧急停车。转移条件基于TTC（碰撞时间）、车距、相对速度、车道线类型等。这套东西在高速场景跑得很稳，因为高速的交互模式相对固定。但一进城区就露馅了：加塞、鬼探头、非机动车乱穿，状态机的转移条件越加越多，最后变成几百条规则，维护成本爆炸，而且规则之间开始互相冲突。

所以我的经验是：状态机适合作为决策的骨架，但不能作为全部。骨架负责管理行为模式的大切换，比如巡航、跟车、变道、泊车。而具体到某个模式内部的微决策，比如跟车时前车速度波动怎么处理，可以交给更细粒度的优化器或学习模型。

2.3 从规则到学习：决策算法的光谱

把决策算法排个谱，从左到右大概是：纯规则状态机 → 行为树 → 基于采样的方法（如MCTS）→ 基于优化的方法（如MPC）→ 部分可观测马尔可夫决策过程（POMDP）→ 模仿学习/强化学习。

左边可解释性强、安全性容易验证，但泛化差；右边泛化潜力大，但黑盒、难验证、长尾场景不可控。实际项目里很少纯用某一种，大多是混合架构。比如用状态机做高层行为管理，用MPC做局部轨迹优化，用学习模型做人类意图预测或交互决策。

我个人的判断是：在L2到L3的量产阶段，规则+优化的混合方案仍然是主流，因为功能安全认证这一关绕不过去。学习类方法更多用在仿真、数据挖掘、或者作为决策的“建议器”，最终输出还要经过安全校验层。完全端到端的决策，目前更多是在研究阶段，离量产还有距离。

3. 核心细节解析与实操要点

3.1 状态机设计：状态划分的粒度怎么定

状态划分太粗，比如只有“行驶”和“停车”两个状态，那决策层等于没干活；太细，比如把“跟车”拆成“跟车加速”“跟车匀速”“跟车减速”，状态数量爆炸，转移条件也难写。

我的经验是：按行为意图划分，而不是按控制量划分。行为意图是“我要变道”“我要让行”“我要跟车”，控制量是“加速度多少”。前者是决策该管的，后者是规划控制该管的。一个实用的状态集合大概是：

• 车道保持巡航
• 跟车行驶
• 主动变道（又分准备、执行、取消）
• 避让（静态障碍、动态障碍）
• 路口通行（直行、左转、右转、掉头）
• 停车等待
• 紧急制动

每个状态内部，再根据场景参数做细分。比如跟车状态里，根据前车距离和相对速度，决定是匀速跟、减速跟还是准备切入。这些细分可以用查表或简单规则实现，不必都做成状态。

注意：状态转移条件一定要加迟滞，否则会在边界反复横跳。比如变道条件里，目标车道后车距离阈值，进入变道用80米，退出变道用100米，避免在80米附近抖动。

3.2 行为树：比状态机更灵活的组织方式

行为树在游戏AI里用得多，这几年在自动驾驶决策里也常见了。它的好处是模块化、可复用、层次清晰。根节点是选择器，下面挂各种行为子树，每个子树有前置条件。执行时从根往下遍历，找到第一个条件满足的分支执行。

举个例子，根选择器下面挂：紧急避让、路口通行、变道、跟车、巡航。紧急避让条件最严格，TTC低于阈值才触发；路口通行条件是有路口地图；变道条件是满足变道安全条件；跟车条件是有前车；巡航是兜底。这样组织，逻辑比状态机清晰，加新行为也方便。

但行为树也有坑：条件节点的评估顺序和频率很关键。如果每帧都从头遍历，计算量大；如果缓存条件结果，又可能错过状态变化。我的做法是，高频条件（如碰撞风险）每帧评估，低频条件（如地图信息）按事件触发更新。

3.3 交互决策：怎么处理他车意图的不确定性

决策最难的不是自己怎么开，而是别人怎么开。前车会不会突然刹车？旁边车道的车会不会加塞？路口电动车会不会闯红灯？这些不确定性，用确定性规则很难覆盖。

一种实用做法是基于预测的滚动决策。预测模块给出他车未来3-5秒的多条可能轨迹及概率，决策层对每条轨迹评估本车的行为代价，选期望代价最小的行为。代价函数通常包括：安全性（碰撞概率、TTC）、舒适性（加速度、 jerk）、效率（通行时间）、合规性（是否压线、是否让行）。

另一种是博弈论方法，把交互建模成博弈，求纳什均衡。但实际用起来，博弈论对假设太敏感，他车的效用函数很难准确建模，所以更多用在特定场景，比如无保护左转、匝道汇入。

我试过在仿真里用POMDP做交互决策，效果确实比规则好，尤其是在拥堵加塞场景。但POMDP的计算复杂度高，状态空间和观测空间离散化后仍然很大，实时性是个问题。后来改成分层POMDP，高层用离散状态做行为选择，低层用连续优化做轨迹生成，才勉强满足实时性。

3.4 安全校验层：决策的最后一道闸

不管决策算法多聪明，输出之前一定要过安全校验。这一层通常是独立的、基于规则的、可形式化验证的模块。它的职责很简单：检查决策输出是否会导致不可接受的风险。比如：

• 预测轨迹是否与任何障碍物有碰撞可能
• 是否违反交通规则（闯红灯、压实线、逆行）
• 是否超出车辆动力学约束
• 是否满足舒适性底线（加速度、jerk限值）

如果校验不通过，就触发降级策略：要么回退到保守行为（如减速停车），要么交给冗余系统。这一层不能省，也不能和学习模型混在一起，必须独立可验证。

提示：安全校验层的阈值设定要留足余量。比如TTC阈值，仿真里设1.5秒能过，实车建议设2.0秒以上，因为感知和预测都有延迟和误差。

4. 实操过程与核心环节实现

4.1 场景建模：把道路和交通参与者结构化

决策算法的输入不是原始点云或图像，而是结构化的场景表示。这一步通常在感知和预测之后，由专门的场景管理模块完成。场景表示包括：

• 静态层：车道线、路沿、停止线、交通标志、信号灯状态
• 动态层：车辆、行人、非机动车的当前位置、速度、朝向、预测轨迹
• 语义层：车道连接关系、路口拓扑、通行优先级

我习惯用车道坐标系而不是笛卡尔坐标系来做决策。因为决策关心的是“我在哪条车道、离路口多远、目标车道是哪条”，这些在车道坐标系下是自然表达。转换关系由地图模块提供。

场景建模的难点在于不一致性处理。感知可能漏检，预测可能不准，地图可能过期。决策层要能容忍这些不确定性。我的做法是给每个输入加置信度，低置信度的信息在代价函数里降权，或者触发保守行为。

4.2 行为选择：用代价函数做多选项评估

行为选择的核心是生成候选行为，评估代价，选最优。候选行为可以从状态机或行为树生成，也可以用采样方法生成。评估代价的典型项：

权重需要标定，而且不同场景权重不同。高速上效率权重高，城区里合规和安全权重高。我通常用分层权重：安全类代价是硬约束，不满足直接淘汰；舒适和效率是软代价，加权求和。

4.3 轨迹生成与优化：从行为到具体路径

行为选定后，要生成具体轨迹。常用方法有：

• 采样法：在Frenet坐标系下采样横向偏移和纵向速度，组合成候选轨迹，用代价函数筛选。优点是简单、并行性好；缺点是采样密度和计算量矛盾。
• 优化法：用MPC或二次规划，在约束下优化轨迹。优点是精确、能处理复杂约束；缺点是求解时间不确定，可能无解。
• 混合法：先采样生成粗轨迹，再用优化做平滑。兼顾效率和精度。

我实际项目里用得最多的是Frenet坐标系下的采样+优化。先在横向采样多个偏移（如-1.5m到1.5m，间隔0.5m），纵向采样多个速度曲线（如匀速、加速、减速），组合成候选。然后用二次规划对最优候选做平滑，满足动力学约束。

参数选择上，采样间隔要匹配控制周期。控制周期50ms的话，轨迹点间隔建议10-20ms，保证控制平滑。横向采样范围要覆盖车道宽度，但不要超出太多，否则候选太多影响实时性。

4.4 仿真验证：决策算法的试炼场

决策算法不能直接上实车调，必须先在仿真里跑够里程。仿真环境要能复现真实场景的复杂性：交通流、随机行为、感知噪声、通信延迟。

我常用的仿真流程是：

1. 场景库构建：从实车数据里提取典型场景，包括切入、加塞、鬼探头、无保护左转、拥堵跟车等。
2. 参数化：把场景参数化，比如他车速度、距离、切入时机，随机采样生成大量变体。
3. 批量运行：决策算法在仿真里跑，记录碰撞、违规、舒适性指标。
4. 回归测试：每次算法更新，跑一遍场景库，确保没有退化。

仿真里最容易忽略的是感知噪声。真实感知有漏检、误检、延迟，仿真里如果给完美感知，决策算法会过拟合。我的做法是在仿真里加噪声模型，模拟感知的不确定性。

注意：仿真里跑得好不代表实车没问题。仿真和现实的差距（sim-to-real gap）在决策层主要体现在他车行为的随机性和感知误差的分布上。实车调试时，一定要留足安全冗余，从简单场景开始，逐步增加复杂度。

5. 常见问题与排查技巧实录

5.1 决策抖动：为什么车会来回摆

决策抖动是新手最常遇到的问题。表现是车辆在车道内左右摆，或者变道决策反复横跳。原因通常有几个：

• 状态转移条件没有迟滞：在阈值附近反复触发。解决办法是加迟滞区间，进入和退出用不同阈值。
• 代价函数权重不合理：效率权重太高，导致车辆不断尝试超车；舒适权重太低，导致轨迹不平滑。解决办法是重新标定权重，做敏感性分析。
• 预测抖动：他车预测轨迹每帧变化大，导致决策跟着变。解决办法是对预测结果做时序滤波，或者用预测的不确定性加权。
• 控制延迟：决策输出到执行有延迟，导致过冲。解决办法是在决策里补偿延迟，或者降低决策频率。

我踩过最坑的一次是，变道决策里用了瞬时TTC，结果旁边车稍微加速就触发变道取消，稍微减速又触发变道，来回横跳。后来改成用预测TTC，取未来3秒的最小值，并且加0.5秒的决策保持时间，才稳定下来。

5.2 交互僵局：谁先走的问题

在无保护左转、窄路会车、匝道汇入这些场景，容易出现双方都让行、谁也不走的僵局。规则决策很容易陷入这种死锁。

解决办法有几种：

• 优先级规则：定义明确的通行优先级，比如直行优先、右侧优先、先到优先。但规则太死，可能不符合人类预期。
• 博弈求解：建模成博弈，求均衡。但计算复杂，且需要他车模型。
• 主动试探：通过缓慢移动（creep）释放意图，观察他车反应。这是人类驾驶员常用的策略，决策算法也可以模拟。
• 随机打破：在僵局持续一定时间后，随机选择一方先行。简单但有效，实际项目里常用。

我的经验是，优先级规则+主动试探组合最实用。先用规则定优先级，如果僵局超过2秒，本车缓慢前移，同时监控他车是否减速让行。如果他车让行，本车通过；如果他车也前移，本车退回等待。

5.3 长尾场景：规则覆盖不到怎么办

长尾场景是决策算法的终极挑战。比如：路上有个掉落的沙发、前方有动物横穿、施工区域临时改道、交警手势指挥。这些场景规则很难穷举。

目前的实用做法是：

• 降级策略：遇到未知场景，降级到保守行为，减速、靠边、停车，请求接管。
• 数据驱动：从实车数据里挖掘长尾场景，补充到场景库，针对性开发规则或训练模型。
• 仿真生成：用生成模型合成极端场景，做压力测试。
• 远程协助：L3以上可以请求远程操作员协助，但依赖通信和法规。

我个人认为，长尾场景不可能靠规则完全覆盖，必须接受“降级+接管”作为兜底。决策算法的目标不是解决所有场景，而是在能力范围内做到安全、合理，超出能力时能安全退出。

5.4 常见问题速查表

6. 决策算法的演进方向与个人实践体会

6.1 学习类方法在决策中的定位

这几年学习类方法很热，尤其是模仿学习和强化学习。模仿学习直接从人类驾驶数据里学决策策略，理论上能学到人类的交互技巧。强化学习在仿真里试错，能探索出规则没想到的策略。

但实际项目里，学习类方法更多用在特定子问题上，比如：

• 他车意图预测：用序列模型预测他车未来行为
• 交互决策：用图神经网络建模多车交互
• 轨迹生成：用生成模型直接输出轨迹

完全用学习模型替代决策层，目前还面临几个硬伤：可解释性差、安全验证难、长尾不可控、数据偏差。我的看法是，学习类方法会逐步渗透到决策的各个环节，但短期内不会完全取代规则和优化。更现实的路径是混合架构：规则管安全底线，学习管交互和泛化，优化管轨迹平滑。

6.2 数据闭环：决策算法迭代的关键

决策算法的迭代，离不开数据闭环。实车跑出来的数据，尤其是接管、急刹、违规这些异常片段，是改进决策的宝贵素材。我参与的项目里，数据闭环流程大概是：

1. 车端触发：检测到异常事件（接管、急刹、碰撞风险），自动上传数据片段。
2. 云端回放：在仿真里回放场景，复现问题。
3. 根因分析：定位是感知、预测还是决策的问题。
4. 算法更新：修改决策逻辑或参数，跑回归测试。
5. 灰度发布：小范围实车验证，确认无退化后全量推送。

这个闭环跑得越快，算法迭代越高效。我见过做得好的团队，从数据上传到算法更新只要一周；做得差的，一个月都跑不完一轮。

6.3 个人体会：决策算法的“艺术”成分

做了这几年决策算法，我最大的体会是：决策不只是技术，还有“艺术”成分。同样的场景，不同的决策风格，乘客的体验完全不同。有的算法很保守，安全但效率低，乘客觉得“太肉”；有的算法很激进，效率高但乘客紧张。怎么在安全和效率之间找到平衡，怎么让决策行为符合人类预期，这些没有标准答案，需要反复调、反复坐、反复感受。

我个人的偏好是：安全底线要硬，交互风格要柔。安全校验层严格把关，但决策层的交互行为尽量平滑、可预测，不要突然变道、突然刹车。乘客坐得舒服，才愿意用。

最后分享一个小技巧：决策算法的调试，一定要坐车感受，不能只看数据。数据上加速度、jerk都达标，但坐起来可能还是不舒服。因为人的感受是非线性的，有些细微的抖动数据上看不出来，但身体能感觉到。所以实车调试时，我通常会跟车跑几趟，记录主观感受，再回去调参数。这个环节，再好的仿真也替代不了。