4、决策规划入门：行为决策与路径速度规划
好，咱们今天聊聊决策规划。说实话，这是自动驾驶里最“像人”的一个模块。你想想看，一个司机开车，他得先判断“我要不要变道”，然后想“怎么变过去”，最后还得琢磨“以多快的速度变”。这就是行为决策、路径规划和速度规划三件事。
我个人习惯把这三层拆开来看。行为决策是“做什么”，路径规划是“怎么做”，速度规划是“多快做”。咱们一个一个说。
4.1 行为决策：换道、跟车、超车
行为决策，说白了就是给车一个“意图”。我见过不少新手，一上来就搞复杂的强化学习，结果车在路上像个无头苍蝇。其实，工业界最常用的还是有限状态机（FSM）。
举个例子，一个简单的驾驶状态机：
状态：跟车 → 判断前车速度
如果前车太慢 → 检查左车道是否安全
安全 → 状态切换为“换道”
不安全 → 保持“跟车”
如果前车加速 → 保持“跟车”
嗯，这里要注意。状态机虽然简单，但状态多了会爆炸。我在项目中遇到过，定义了30多个状态，最后自己都理不清。我的建议是：核心状态控制在5-7个，比如：
- 巡航：前方没车，按设定速度跑
- 跟车：前方有车，保持安全距离
- 换道：准备变道，需要路径规划配合
- 超车：换道+加速的组合动作
- 停车：遇到红灯或障碍物
我曾经犯过一个错，把“换道”和“超车”混在一起。结果车在超车时犹豫不决，一会想变道一会想加速。后来我学乖了，超车就是“换道+巡航”，拆成两个子状态，逻辑清晰多了。
4.2 路径规划：A* 与 Dijkstra
路径规划，就是回答“怎么走”。这里有两个经典算法，A* 和 Dijkstra。你可能会问，都2025年了还用这些？其实，底层逻辑没变过。
Dijkstra 是个老实人。它从起点开始，一圈一圈往外扩，直到找到终点。优点是保证找到最短路径，缺点是慢。你想想看，如果地图有10000个节点，它得把9000个没用的都算一遍。
A* 就聪明多了。它加了一个“启发式函数”，说白了就是“猜一下终点在哪边”。比如：
f(n) = g(n) + h(n)
g(n)：从起点到当前点的实际代价
h(n)：从当前点到终点的估计代价（比如直线距离）
A* 会优先扩展 f(n) 最小的节点。所以它总是朝着终点方向搜，而不是像 Dijkstra 那样无脑扩散。
我记得有一次做园区物流车，地图不大，但要求实时性极高。我用 Dijkstra，每次规划要200ms，车都停下来了。换成 A* 后，30ms 搞定。为什么？因为园区道路是网格状的，启发式函数特别准。
| 算法 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| Dijkstra | 保证最优解 | 搜索范围大，慢 | 小地图、静态环境 |
| A* | 速度快，有方向性 | 启发式函数设计不好会变差 | 大部分场景，尤其是网格地图 |
4.3 速度规划：让车跑得舒服
路径规划好了，但车怎么跑？是匀速、加速还是减速？这就是速度规划的事。
速度规划的核心是“约束”。你想想看，车不能无限加速，有物理极限。也不能突然急刹，乘客会骂娘。常见的约束有：
- 最大速度：道路限速、车辆性能
- 最大加速度：舒适性要求，一般 2-3 m/s²
- 最大减速度：紧急制动可以大，但正常行驶要小
- 曲率约束：转弯时速度要降，否则会侧翻
我常用的方法是“梯形速度规划”。说白了，就是先加速到目标速度，匀速跑一段，再减速停下来。像这样：
输入：路径长度 L，目标速度 V_max，加速度 a
输出：每个点的速度
1. 计算加速段距离：d_acc = V_max² / (2 * a)
2. 计算减速段距离：d_dec = V_max² / (2 * a)
3. 如果 d_acc + d_dec > L：
// 到不了目标速度，得重新算
V_actual = sqrt(2 * a * L / 2)
4. 否则：
匀速段距离 = L - d_acc - d_dec
嗯，这里要注意。梯形规划虽然简单，但加速度突变会让乘客不舒服。我建议用“S形速度规划”，加一个加加速度（jerk）约束。说白了，就是让加速度也平滑变化。
我的经验：速度规划的输出，一定要和路径规划耦合。比如，路径上有个急弯，速度规划必须提前减速。我见过一个方案，路径和速度分开算，结果车到了弯道口才发现速度太快，一把方向打过去，差点翻车。
最后说一句，决策规划这三层，不是孤立的。行为决策告诉路径规划“我要换道”，路径规划算出一条曲线，速度规划再决定“以多快的速度沿着曲线走”。三者配合好了，车才能开得像老司机。
我个人习惯，先搭一个简单的状态机+梯形速度规划，跑通后再慢慢优化。别一上来就上强化学习、MPC，那玩意儿调试起来，够你喝一壶的。