4. 局部规划器性能调优：DWB、MPC、TEB算法参数优化与计算资源分配

局部规划器，说白了就是机器人的“脚”。全局规划告诉你往哪走，局部规划决定每一步怎么迈。我见过太多项目，全局路径规划得漂漂亮亮，结果机器人一上路就原地抽搐——十有八九是局部规划器没调好。

这一章，咱们就聊聊Nav2里三个主流的局部规划器：DWB、MPC和TEB。我会结合自己的踩坑经历，讲讲怎么调参、怎么分配计算资源。

核心观点：没有最好的规划器，只有最适合场景的规划器。选对算法，调好参数，比盲目追求“最新”更重要。

4.1 三大规划器核心差异

先看一张对比图，帮你快速建立整体认知：

4.2 DWB参数调优实战

DWB是Nav2默认的局部规划器，也是我最早接触的。说实话，刚开始觉得它挺“笨”的——只会往前看一小段。但后来发现，只要参数调对了，它其实很可靠。

4.2.1 核心参数解读

我个人习惯把DWB参数分成三组来调：

参数组	关键参数	作用	我的经验值
速度空间	min_vel_x, max_vel_x min_vel_theta, max_vel_theta	限制采样范围	max_vel_x = 0.5m/s (室内) max_vel_theta = 1.0 rad/s
采样密度	vx_samples, vtheta_samples	决定轨迹数量	vx_samples=20, vtheta_samples=40 再高就卡顿了
评价权重	path_distance_bias goal_distance_bias occdist_scale	平衡跟踪与避障	path:goal:occ = 4:2:3 （走廊场景）

调参技巧：先固定速度限制，再调采样密度，最后调权重。别一上来就动所有参数——我吃过这个亏，调了一整天发现是某个权重设反了。

4.2.2 典型场景配置

我在项目中遇到过最头疼的场景是窄走廊。机器人总是贴着墙走，稍微偏一点就撞上。后来发现是occdist_scale设得太低了。

# 窄走廊场景推荐配置
DWB:
  min_vel_x: 0.0
  max_vel_x: 0.3          # 慢一点，安全第一
  min_vel_theta: -0.5
  max_vel_theta: 0.5
  vx_samples: 15
  vtheta_samples: 30
  path_distance_bias: 3.0  # 跟踪路径
  goal_distance_bias: 1.5  # 朝向目标
  occdist_scale: 4.0       # 避障权重拉高
  sim_time: 1.5            # 预测时间缩短
  sim_granularity: 0.025   # 步长精细些

注意：sim_time不是越大越好。设大了，机器人反应迟钝；设小了，又容易来回摆动。我一般从1.5秒开始调，根据实际响应微调。

4.3 MPC参数调优实战

MPC是我个人最喜欢的规划器。它“看得远”，能提前规划好几步。但代价是计算量大——你想想看，每秒钟要解几十次优化问题。

4.3.1 关键参数与计算资源

MPC的核心是预测时域（N）和控制时域（M）。这两个参数直接决定了计算量：

# MPC参数配置示例
MPC:
  # 时域参数
  prediction_horizon: 20   # 预测步数
  control_horizon: 10      # 控制步数
  
  # 权重矩阵
  Q: [1.0, 1.0, 0.5]       # 状态误差权重 [x, y, theta]
  R: [0.1, 0.1]            # 控制输入权重 [v, w]
  
  # 约束
  max_velocity: 0.5
  max_acceleration: 0.3
  max_angular_velocity: 0.8
  
  # 求解器配置
  solver: "osqp"           # 推荐使用OSQP
  max_iterations: 100
  tolerance: 1e-4

我记得有一次在Jetson Nano上跑MPC，CPU直接飙到90%。后来把prediction_horizon从30降到20，control_horizon从15降到10，CPU占用降到了40%，效果差别不大。

经验公式：计算量 ≈ O(N × M²)。N和M每增加1，计算量可能翻倍。建议从N=15, M=8开始，逐步增加直到效果满意。

4.3.2 权重调优策略

MPC的权重矩阵Q和R，说白了就是告诉机器人“什么更重要”。

Q矩阵大：机器人更“听话”，严格跟踪路径
R矩阵大：机器人更“省力”，动作平缓
Q/R比值：决定了跟踪精度vs平滑度的trade-off

我曾经做过一个仓储机器人项目，要求路径跟踪误差小于5cm。一开始Q设得很大，结果机器人走起来一抖一抖的。后来把R适当调大，平滑度上去了，误差反而更小了——因为减少了不必要的修正动作。

4.4 TEB参数调优实战

TEB是个“中间派”——比DWB聪明，比MPC轻量。但它有个毛病：对时间参数特别敏感。

4.4.1 时间参数的艺术

TEB的核心是时间弹性带，说白了就是“在时间轴上拉橡皮筋”。

参数	含义	典型值	调大效果
dt_ref	参考时间步长	0.3~0.5s	轨迹更稀疏，计算更快
dt_hysteresis	时间迟滞	0.1~0.2s	减少轨迹点增减抖动
max_global_plan_lookahead_dist	前瞻距离	2.0~5.0m	看得更远，但可能忽略近处障碍

避坑指南：我曾经把dt_ref设成0.1s，想着“时间步长越小越精细”。结果机器人走两步就卡住了——轨迹点太多，优化求解超时。记住：TEB不是越精细越好，够用就行。

4.4.2 优化求解器配置

TEB使用g2o进行图优化。这里有个小技巧：

# TEB优化器配置
TebLocalPlannerROS:
  # 优化参数
  no_inner_iterations: 5      # 内层迭代次数
  no_outer_iterations: 4      # 外层迭代次数
  optimization_activate: true
  optimization_verbose: false
  
  # 权重
  weight_kinematics_forward_drive: 100.0  # 前向运动约束
  weight_kinematics_turning_radius: 50.0  # 转弯半径约束
  weight_optimaltime: 10.0                # 时间最优权重
  weight_obstacle: 50.0                   # 避障权重
  
  # 计算资源控制
  enable_homotopy_class_planning: false   # 关闭同伦类规划，节省计算

我建议把enable_homotopy_class_planning关掉——除非你的场景特别复杂。这个功能会生成多条候选轨迹，计算量直接翻倍。在大多数场景下，单条轨迹就够了。

4.5 计算资源分配策略

调完参数，还得管好计算资源。不然机器人跑着跑着就“死机”了。

4.5.1 CPU资源分配

我一般这样分配CPU资源：

局部规划器：分配1个核（绑定到特定CPU核心）
代价地图：分配1个核
传感器处理：分配1个核
其他任务：共享剩余核心

# 在launch文件中设置CPU亲和性
<node pkg="nav2_controller" exec="controller_server" name="controller_server">
  <param name="controller_frequency" value="20.0"/>
  <!-- 设置线程优先级 -->
  <env name="RCUTILS_CONSOLE_OUTPUT_FORMAT" value="[{severity}][{time}]: {message}"/>
</node>

性能监控：用htop或perf监控CPU使用率。如果局部规划器CPU占用超过50%，就该考虑降采样或换算法了。

4.5.2 频率与延迟的平衡

规划频率不是越高越好。我见过有人把controller_frequency设到50Hz，结果CPU跑满，实际控制周期反而更不稳定。

规划器	推荐频率	最大频率	说明
DWB	20~30 Hz	50 Hz	计算量小，频率可以高
MPC	10~20 Hz	30 Hz	计算量大，频率要控制
TEB	15~25 Hz	40 Hz	适中，看优化迭代次数

嗯，这里要注意：频率设得再高，如果规划器算不出来，实际控制周期反而会抖动。我建议用ros2 topic hz /cmd_vel看看实际输出频率，别光看配置值。

4.6 实战调优流程

最后，分享一个我总结的调优流程：

选算法：根据机器人类型和场景选DWB/MPC/TEB
设限制：先设好速度、加速度等硬约束
调采样：调整采样密度或时域参数
配权重：平衡跟踪、避障、平滑度
看资源：监控CPU和内存，必要时降配
实车验证：在真实场景中跑几圈，微调参数

记住：调参是个迭代过程。每次只改一个参数，记录效果。我习惯用ros2 param dump保存每次的配置，方便回溯。

局部规划器调优，说白了就是找到“快、准、稳”的平衡点。没有万能参数，只有最适合你场景的配置。多试几次，你就能找到感觉了。