4. 局部路径规划：DWA动态窗口法、TEB时间弹性带、MPC模型预测控制
各位同事，今天我们聊局部路径规划。全局规划给你一条大方向，但真飞起来，风一吹、树一挡、突然冒出个电线杆，你得自己躲。这就是局部规划干的事——实时、在线、反应快。
我这些年飞过的地形，从城市峡谷到深山密林，试过不少方法。今天重点讲三个：DWA、TEB、MPC。它们各有脾气，选对了是利器，选错了……嗯，我吃过亏。
核心区别一句话：DWA 快但短视，TEB 灵活但计算重，MPC 最优但调参烦。没有银弹，只有适不适合你的场景。
4.1 DWA 动态窗口法：快刀斩乱麻
DWA 的思路很直白：在当前时刻，我能飞多快、多慢、多快转弯？把这些可能的 (v, ω) 组合列出来，形成一个「动态窗口」。然后每个组合往前模拟一小段，看哪个最安全、最靠近目标、速度最快。选最好的那个，执行一步，下一帧再来。
说白了，就是「枚举 + 评价」。我最早在四旋翼上实现 DWA 时，觉得这方法太粗暴了。但后来在密集树林里试飞，发现它反应是真快——遇到突然横出来的树枝，DWA 能瞬间刹车转向，比人反应还快。
我的经验：DWA 的评价函数权重很关键。安全权重设太低，飞机会贴障碍物太近；速度权重设太高，容易急转弯翻机。我一般从 1:1:1 开始调，安全权重略高一点。
DWA 的步骤可以拆成三步：
- 生成速度窗口——根据当前速度、最大加减速、最大角速度，算出 (v, ω) 的可行范围。
- 模拟轨迹——对每个 (v, ω)，用运动模型前推几秒，得到一条预测轨迹。
- 评价打分——用目标距离、障碍物距离、速度大小三个指标加权求和，选最高分。
代码实现其实不复杂，核心就几十行。我贴一个简化版：
def dwa_control(x, goal, obstacles, config):
# 1. 生成动态窗口
v_min = max(config.v_min, x.v - config.a_max * dt)
v_max = min(config.v_max, x.v + config.a_max * dt)
w_min = max(config.w_min, x.w - config.alpha_max * dt)
w_max = min(config.w_max, x.w + config.alpha_max * dt)
best_score = -float('inf')
best_u = (0, 0)
# 2. 遍历速度空间
for v in np.arange(v_min, v_max, config.v_res):
for w in np.arange(w_min, w_max, config.w_res):
# 模拟轨迹
traj = simulate_trajectory(x, v, w, config.predict_time)
# 3. 评价
score = evaluate_trajectory(traj, goal, obstacles, config)
if score > best_score:
best_score = score
best_u = (v, w)
return best_u注意：DWA 没有记忆。它只看当前窗口，不考虑未来长远路径。在复杂地形里，容易陷入局部最优——比如被 U 型谷困住。我曾在山谷里遇到过，DWA 在谷底来回转，就是出不去。后来加了全局引导项才解决。
4.2 TEB 时间弹性带：走窄路的高手
TEB 的思路跟 DWA 完全不同。它把整条路径看作一根「弹性带」，带子上每个点都有时间戳。然后通过优化，让带子既靠近全局路径，又避开障碍物，同时满足动力学约束。
你想想看，这就像拉一根橡皮筋——你用手在中间捏一下（障碍物），橡皮筋会自动绕开。TEB 就是数学版的橡皮筋。
我记得第一次在狭窄的峡谷里用 TEB，效果惊艳。无人机贴着岩壁飞过去，离障碍物只有半米，但轨迹非常平滑。换成 DWA，早就抖成筛子了。
TEB 的优化目标包含几个部分：
- 路径跟随——让带子贴近全局参考路径
- 障碍物避让——与障碍物保持安全距离
- 时间最优——让总时间尽量短
- 动力学可行——速度、加速度、角速度不超限
- 平滑性——轨迹曲率变化小
这些目标加权求和，用非线性优化器求解。常用的求解器是 g2o 或 Ceres。
关键参数：TEB 的「弹性」由权重控制。障碍物权重设太大，路径会绕很远；时间权重设太大，飞机会猛冲。我建议在仿真里先跑几圈，观察轨迹形态再调。
TEB 的缺点也很明显——计算量大。优化问题每帧都要解，在嵌入式平台上容易掉帧。我曾在 Pixhawk 上试过，优化一次要 30-50ms，飞快了根本跟不上。
优化技巧：可以降低优化频率，比如每 5 帧优化一次，中间帧用插值。或者减少带子上的控制点数量，从 30 个减到 15 个，计算量能降一半。
4.3 MPC 模型预测控制：运筹帷幄
MPC 是这三兄弟里最「聪明」的。它不只考虑当前，而是往前看一个时间窗口（预测时域），在这个窗口里求解一个带约束的最优控制问题。然后只执行第一步，下一帧重新算。
为什么叫「模型预测」？因为它需要知道系统的运动模型——你给油门，飞机会怎么动？模型越准，预测越靠谱。
我做过一个对比实验：同样在风场中飞行，DWA 被风吹偏了才纠正，MPC 能提前预测风的影响，提前调整姿态。结果 MPC 的轨迹误差只有 DWA 的三分之一。
MPC 的数学形式可以写成：
min Σ (x_k - x_ref)^T Q (x_k - x_ref) + u_k^T R u_k
s.t. x_{k+1} = f(x_k, u_k) # 运动模型
x_min ≤ x_k ≤ x_max # 状态约束
u_min ≤ u_k ≤ u_max # 控制约束这里 Q 和 R 是权重矩阵，Q 大表示更看重跟踪精度，R 大表示更看重控制平顺。
调参警告：MPC 的参数比 DWA 和 TEB 多得多。预测时域、控制时域、Q、R、约束边界……我曾经调了一个星期才找到一组能飞的参数。建议从短预测时域（10 步）开始，先让飞机动起来，再慢慢加长。
三种方法的对比，我整理了一张表：
| 特性 | DWA | TEB | MPC |
|---|---|---|---|
| 计算速度 | 快（<5ms） | 中（20-50ms） | 慢（50-200ms） |
| 轨迹质量 | 一般，有抖动 | 平滑，适合窄通道 | 最优，适合动态环境 |
| 约束处理 | 弱（靠评价函数） | 中（优化目标） | 强（显式约束） |
| 调参难度 | 低（3个权重） | 中（5-8个参数） | 高（10+个参数） |
| 适用场景 | 快速避障、低空飞行 | 狭窄通道、结构化环境 | 高精度跟踪、强风环境 |
4.4 选型建议：别纠结，先试 DWA
如果你刚开始做局部规划，我建议先从 DWA 入手。它简单、直观、容易调通。等你把 DWA 跑顺了，再根据场景需求升级到 TEB 或 MPC。
我个人习惯的选型流程：
- 开阔地、快速避障 → DWA
- 狭窄通道、需要平滑轨迹 → TEB
- 高精度、强风、动态障碍物多 → MPC
- 计算资源有限（如 Pixhawk） → DWA 或轻量 TEB
最后说一句：局部规划再强，也救不了全局规划的错误。我见过太多人把精力全花在调局部规划上，结果全局路径给了一条死路——局部规划再聪明也飞不过去。先保证全局路径合理，再优化局部细节。