4、ADAS决策规划算法：行为预测、路径规划（A*/RRT）、运动控制（PID/MPC）

好，咱们进入第四章。这一章可以说是ADAS系统的“大脑”核心——决策规划。说白了，就是让车知道“接下来该干嘛”。我做了这么多年系统，发现很多团队在感知上砸了大钱，结果决策规划写得一塌糊涂，车开起来像个新手司机。嗯，这里咱们得好好捋一捋。

4.1 行为预测：猜猜别人想干嘛

自动驾驶最难的其实不是自己怎么开，而是猜别人怎么开。你想想看，你正常开着，旁边车道一辆车突然不打灯就挤进来，你怎么办？

行为预测就是干这个的。它要回答三个问题：

• 这个目标会动吗？——是静止障碍物还是动态车辆
• 它打算往哪去？——直行、变道、还是转弯
• 它什么时候行动？——立即执行还是等待时机

我个人习惯把预测方法分成两类：

4.2 路径规划：A*与RRT的相爱相杀

路径规划，说白了就是找一条从A到B的路。但这条路不是随便画的，它得满足：安全、舒适、可执行。

我最早接触路径规划是在一个园区物流项目上。当时用的是A*算法，效果还行，但遇到动态障碍物就抓瞎。后来我换成了RRT，嗯，又是另一个故事。

4.2.1 A*算法：有地图就稳

A*是经典的图搜索算法。它的核心公式很简单：

f(n) = g(n) + h(n)
// g(n): 从起点到当前点的实际代价
// h(n): 从当前点到终点的估计代价（启发式函数）

我建议你在结构化道路上优先用A*。比如高速公路、城市主干道，这些场景的障碍物位置相对固定，A*能快速找到最优路径。

4.2.2 RRT算法：无路也能走

RRT（快速随机扩展树）就不一样了。它不依赖地图，而是通过随机采样来探索空间。说白了就是“瞎蒙，但蒙得有策略”。

RRT的流程大致是：

1. 在空间中随机采样一个点
2. 找到树上离它最近的点
3. 从最近点向采样点延伸一段距离
4. 检查这段路径是否碰撞
5. 没碰撞就加入树，重复直到到达终点

我记得有一次做泊车场景，车位窄得连A*都搜不出路径。换成RRT后，虽然路径有点绕，但至少能停进去。后来我加了个路径平滑后处理，效果就很好了。

4.3 运动控制：PID与MPC的博弈

路径规划好了，接下来就是让车按路径走。这就是运动控制的事。控制不好，规划得再好也是白搭。

4.3.1 PID控制：简单但够用

PID（比例-积分-微分）控制，我估计你在大学就学过。它的公式很简单：

u(t) = Kp * e(t) + Ki * ∫e(t)dt + Kd * de(t)/dt
// e(t): 当前误差（比如横向偏差）
// Kp: 比例系数
// Ki: 积分系数
// Kd: 微分系数

为什么PID在ADAS中还能用？因为它快。一个PID控制器在嵌入式平台上跑，延迟可以控制在1ms以内。对于车道保持、自适应巡航这些场景，完全够用。

4.3.2 MPC控制：高级但吃算力

MPC（模型预测控制）就高级多了。它不是看当前误差，而是预测未来一段时间的状态，然后优化控制量。

MPC的核心思想是：

1. 建立车辆运动模型
2. 预测未来N步的状态
3. 求解一个优化问题，找到最优控制序列
4. 只执行第一步，然后重复

我建议你在以下场景用MPC：

• 高速变道：需要精确控制横向和纵向
• 紧急避障：需要考虑车辆动力学极限
• 轨迹跟踪：要求高精度、低抖动

4.4 决策规划的整体架构

最后，我给大家画个整体架构图（用文字描述）：

感知层 → 行为预测 → 路径规划 → 运动控制 → 执行器
  ↑          ↑           ↑           ↑
  |          |           |           |
  |     (预测其他车)  (找路径)   (算控制量)
  |
  (检测障碍物、车道线等)

这个架构看起来简单，但实际工程中每个环节都有坑。我建议你从最简单的开始：先用PID做车道保持，再加A*做路径规划，最后上MPC做精确控制。一步一个脚印，别想一口吃成胖子。

好了，这一章就到这里。下一章咱们聊聊系统集成和测试验证，那才是真正考验工程能力的地方。