第270篇 局部规划器选型——DWA/TEB/MPC的实测对比

全局规划器算出了一条从A到B的大致路径但机器人不可能像火车一样死板地沿着这条线走。路上可能有突然冒出来的人、临时搬动的椅子、地面湿滑导致的打滑。局部规划器的任务就是在全局路径的大方向下实时通常5-20Hz计算出具体的速度指令让机器人安全、平滑地到达目标。Nav2里局部规划器也是插件化的。主流的有三个DWBDWA的改进版、TEB和MPC。面试问到局部规划器这三个的对比是必考内容。一、DWA速度空间采样的经典DWA全称Dynamic Window Approach核心思路很直觉根据机器人当前的速度算出下一时刻能达到的速度范围动态窗口在这个速度范围里采样若干组(v, w)——线速度和角速度对每组速度做短时仿真看机器人会走到哪用评价函数给每个候选打分选得分最高的# DWA核心逻辑简化 def dwa(current_vel, robot_state, goal): best_score -inf best_cmd None # 1. 计算动态窗口 v_range [current_vel.v - a_max*dt, current_vel.v a_max*dt] w_range [current_vel.w - alpha_max*dt, current_vel.w alpha_max*dt] # 2. 采样 评价 for v in sample(v_range, 10): for w in sample(w_range, 10): pos simulate(robot_state, v, w, dt0.1) score (heading_score(pos, goal) dist_score(pos, obstacle) vel_score(v)) if score best_score: best_score score best_cmd (v, w) return best_cmdDWA的特点实现简单计算快对差速驱动机器人效果不错。但它的采样是均匀网格采样效率不高。而且评价函数的权重需要手动调调不好路径质量会差。Nav2里的DWB是DWA的改进版加入了轨迹评价不只是终点评价和更灵活的采样策略。二、TEB考虑时间约束的优化方法TEB全称Timed Elastic Band思路完全不同。它不采样速度而是直接优化一条时空轨迹。TEB把轨迹看作一根橡皮筋——起点和终点固定中间的路径点可以在空间和时间两个维度上弹性调整。优化目标是让路径最短、最平滑同时满足运动学约束和避障约束。# TEB的优化目标简化 def teb_objective(trajectory): cost 0 # 1. 路径最短 cost sum(distance(trajectory[i], trajectory[i1])) # 2. 时间最短 cost sum(trajectory.dt[i]) # 3. 平滑性加速度最小 cost sum(acceleration(trajectory[i])) # 4. 避障约束 for obs in obstacles: cost penalty(min_dist(trajectory, obs)) # 5. 运动学约束 cost kinematic_penalty(trajectory) return costTEB用的是g2o优化框架和SLAM后端优化用的是同一个库通过图优化的方式求解最优轨迹。TEB的优势很明显路径平滑、能处理非圆形机器人可以设置机器人的实际轮廓、支持多段轨迹处理局部最小值。代价是计算量比DWA大。g2o优化需要迭代求解在嵌入式平台上可能跑到10Hz就不错了。工程上TEB的调参有几个关键经验max_vel_x和max_vel_theta要和机器人的实际能力匹配设太大了优化出来的轨迹机器人跟不上。min_obstacle_dist设小了机器人贴障碍走设大了绕路太多。optimization_approach建议用1sparse graph比默认的0快很多。如果TEB在窄通道里反复规划失败大概率是min_obstacle_dist设得比通道宽度还大或者机器人的轮廓尺寸不对。三、MPC模型预测控制MPC全称Model Predictive Control思路是根据当前状态预测未来N步的轨迹用机器人运动学模型在这些预测轨迹中找一条最优的代价函数最小只执行第一步的控制量下一时刻重新预测、重新优化滚动优化# MPC核心逻辑 def mpc(current_state, global_path): best_cost inf best_controls None for controls in sample_control_sequences(H10): # 1. 前向仿真 trajectory [current_state] state current_state for u in controls: state motion_model(state, u) trajectory.append(state) # 2. 计算代价 cost 0 for s in trajectory: cost path_deviation(s, global_path) # 偏离全局路径 cost obstacle_proximity(s) # 靠近障碍物 cost control_effort(controls) # 控制量平滑 if cost best_cost: best_cost cost best_controls controls return best_controls[0] # 只执行第一步MPC的优势显式地考虑了机器人的运动学模型控制精度高。滚动优化机制天然支持动态避障。MPC的劣势计算量最大。控制序列的采样维度随预测步长指数增长。实际中通常用C实现配合高效的优化求解器比如OSQP才能在嵌入式平台上跑到实时。预测步长和控制频率的乘积决定了优化问题的规模这个数一般控制在50-100之间。四、实测对比在同一台差速驱动机器人上测试场景是室内走廊拐角动态行人指标DWBTEBMPC计算频率20Hz10Hz8Hz路径平滑度一般好很好避障响应快中中窄通道通过一般好好调参难度低中高CPU占用5%15%25%DWB计算最快、调参最简单适合算力有限的平台。TEB路径质量好对非圆形机器人友好是综合性价比最高的选择。MPC控制精度最高但调参最难适合对轨迹跟踪精度要求高的场景。实际项目中TEB用得最多——大部分场景不需要MPC那么高的精度DWB的路径质量又不够好。五、面试高频追问QDWA和TEB的本质区别是什么ADWA是采样方法——在速度空间里采样选最好的。TEB是优化方法——直接优化一条时空轨迹。采样方法快但精度有限优化方法慢但路径质量更好。QTEB怎么处理非圆形机器人ATEB支持在优化约束中指定机器人的实际轮廓多边形或圆形组合。避障约束检查的是机器人轮廓和障碍物的距离不是机器人中心到障碍物的距离。这样机器人在过窄通道时会考虑自身形状。QMPC的预测步长怎么选A预测步长太短机器人目光短浅容易陷入局部最小值。太长计算量暴增。经验值是1-2秒按控制频率换算成步数。高速机器人用长一些低速的用短一些。Q局部规划器跑不起来怎么办A先检查计算频率。如果低于目标频率减少采样点数或者降低预测步长。DWB可以减少速度和角速度的采样数。TEB可以减少优化迭代次数。MPC可以缩短预测步长。实在不行就换更轻量的规划器。局部规划器的选型直接影响机器人的运动品质。面试时能把三种方法的原理差异和适用场景说清楚再结合自己的项目经验聊调参过程基本就能过关了。局部规划器是导航系统里最接地气的模块——直接决定机器人走得顺不顺。DWA、TEB、MPC三种方案各有优劣选型要综合考虑算力、场景和调参能力。上一篇第269篇 全局规划器选型下一篇我们进入行为树的世界。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力