机器人导航local planner深度解析：DWA与TEB选型及调参实战

我这几年做机器人导航项目，从建图、定位到全局规划一路趟过来，说实话前面几步的成就感都来得比较“虚”：地图画出来了，路径规划出来了，看起来头头是道，可真让机器人跑起来才发现，真正决定它能不能活着走到目标点的，其实是藏在最后那几百毫秒里的local planner。这玩意儿在ROS导航栈里一直不怎么起眼，但实车一跑就原形毕露——全局规划给了一条康庄大道，local planner要是拉胯，机器人照样原地转圈、贴墙摩擦、被一把椅子堵到死机。

这个系列写到这里已经是第四篇，前面聊过gmapping建图、AMCL定位、global planner选路，今天这篇就是冲着local planner来的。不管你是在做机器人导航仿真，还是已经上实物车准备做抓取、配送、巡检，只要你发现机器人“有路不走、见障就懵”，那问题大概率出在这里。

1. 先搞清楚：为什么有了全局规划，还要一个local planner

1.1 全局规划给出的只是一条“大方向”

很多人第一次接触ROS导航栈，最容易误解的就是全局路径规划。以为地图上画出来的那条红色连线，就是机器人要走的实际轨迹。这种理解在仿真里跑直线还好，一放到真实环境立刻出问题。

全局规划器拿到的是静态地图，它在这张地图上用Dijkstra或者A*算法算出一条从起点到目标点的路径。这个过程本身没有问题，问题在于它赖以决策的世界是“静止”的。地图上标着这里是墙、那里是桌子，机器人就沿着它们之间的空隙规划路径。可真实世界里充满了不在图上的东西：一个人走过来了，一把椅子被挪了位置，一扇门被打开了。这些东西在全局地图上完全不存在，全局规划器自然也不可能绕过它们。

换句话说，全局规划干的活是从地图层面找路，它的粒度是“米”级的，只负责保证在大尺度上方向正确。至于这中间怎么躲开突然冒出来的障碍物，怎么在狭窄通道里调整姿态，怎么平滑地从一个路径点过渡到下一个路径点，全局规划一概不管。

这里可以用一个生活化的例子来理解：全局规划相当于高德地图给你规划的路线，它告诉你“前方第三个路口右转”；而local planner相当于你握方向盘的实际操作——路上突然有个坑，你得绕一下；前车慢得要死，你得并线超车。高德不会帮你避让路上的临时情况，但它保证了大方向没错。在机器人导航里，local planner干的就是“握方向盘”这个活。

1.2 local planner到底在谁的指挥链里干活

把local planner放在整个导航系统的指挥链里看，它的位置就非常清晰了。在ROS的move_base框架里，整个导航决策流水线大概是这个顺序：传感器数据进来，先喂给costmap生成障碍物代价地图；全局规划器基于全局costmap计算出宏观路径；local planner拿到这条全局路径的一小段，结合本地costmap的实时障碍物信息，计算出机器人接下来几百毫秒到几秒内应该执行的速度指令；最后这个速度指令通过cmd_vel话题发给底盘驱动。

关键就在这里：local planner其实从来没有拿到过完整路径，它从全局规划器那里得到的只是一段“局部视野”。move_base会不断把全局路径上距离机器人当前位置最近的一段切出来，转发给local planner，让它在短距离内规划一条可执行的真实轨迹。这样做的目的是实时性——局部规划的视野只需要覆盖机器人的制动距离和动态避障范围，处理起来速度很快，能跟得上传感器数据的更新节奏。

这种“一大一小”的规划架构几乎是所有移动机器人导航系统的标准设计。全局规划保证最优性和可达性，局部规划保证安全性和实时性。两者互相配合，缺一不可。很多人调导航时只看全局路径走得漂不漂亮，忽略了local planner才是真正拿着方向盘的手，这是根子上的误区。

1.3 costmap：local planner的“眼睛”和“触觉”

local planner本身不直接处理激光雷达或深度相机数据，它读的是costmap。costmap本质上是一张栅格地图，每个栅格上存着一个“代价值”，表示机器人走到这个位置的“危险程度”。这个值从0到255，0是完全安全，254是接近障碍物，255是直接撞墙。

costmap分为全局costmap和局部costmap。全局costmap主要服务于全局规划，虽然也实时更新，但更新频率低、范围覆盖整个地图，主要用于宏观路径选择。局部costmap则是local planner的主场，它只围在机器人周围一小片区域内（比如3米乘3米），以高频率刷新，实时反映周围几米内的障碍物分布。

在costmap里，除了原始障碍物栅格，还会生成一层“膨胀区域”（inflation layer）。这层膨胀区域的意义在于：机器人是有体积的，不能像质点一样贴着墙根走。膨胀半径会把障碍物向外“撑大”一圈，撑出来的区域代价值随距离递减。local planner在做轨迹评价时，会优先考虑让机器人远离高代价区域，但又不至于完全避开所有非零区域——毕竟现实中走廊就那么宽，一点代价都不敢碰的话，机器人寸步难行。

注意：costmap膨胀半径调大，机器人会显得“很怂”，离障碍物老远就开始绕；调小了，小车贴着墙走，看着勇猛，但稍微有点定位误差就蹭上了。对大部分人来说，这个参数值得花很多时间反复实验。

2. 主流local planner算法盘点与选型思路

2.1 DWA：ROS里用得最多的“窗口扫描法”

DWA（Dynamic Window Approach，动态窗口法）是ROS导航栈里最经典、久经考验、也是默认使用的local planner算法。它的核心思路可以用一句话概括：在每个控制周期内，根据机器人当前速度，计算出所有“下一时刻可能达到的速度组合”，然后对每个速度组合模拟出一段轨迹，用评价函数打分，选分数最高的那个速度发出去。

为什么叫“动态窗口”？因为它不是盲目的在整个速度空间里采样，而是只在一个受限的速度窗口内采样。这个窗口的大小由机器人当前速度、加速度极限和控制周期决定。比如机器人当前速度为0.5m/s，加速度极限为0.5m/s²，控制周期为0.1s，那下一个时刻机器人能达到的速度就只在0.45到0.55m/s之间。这个范围就是一个动态变化的“窗口”，每时每刻都在随当前状态移动。

DWA的优势是算法简单、计算量小、容易调试，对差分驱动机器人来说基本够用了。它不显式规划一条未来的路径，而是通过“采样-模拟-评价”三步循环实现局部避障，控制频率可以做到很高，非常适合对实时性要求苛刻的场景。这也是它多年来一直霸占ROS默认local planner位置的原因。

不过DWA的短板也明显：它只考虑当前这一小段时间内的轨迹，没有全局视野，容易被局部环境“绕进去”，比如在U形障碍物前面来回摆动。另外它对机器人的运动模型做了较大简化，如果是阿克曼转向的车型或者全向移动平台，DWA的表现就会差一些，需要针对运动模型做定制修改。

2.2 TEB：把轨迹和速度一起当作优化对象

TEB（Timed Elastic Band，时间弹性带）是另一款主流local planner，它的名字听起来玄乎，实际思路却很优雅。它把机器人从当前位置到局部目标点之间规划出的轨迹想象成一条“弹性带子”，这条带子上的每个点都连接着机器人的位姿、时间戳。然后通过构建一个多目标优化问题，让这条“带子”在满足避障、运动学约束的前提下，尽可能短、尽可能快、尽可能平滑地到达目标。

TEB的“弹性”体现在它的优化机制上：轨迹上的点之间像是有弹簧在拉扯，太远就往回拉，太近就往外推；遇到障碍物会形成一个排斥力，让轨迹尽量绕开；目标点和时间约束又会让轨迹整体往前进方向拖动。最终通过图优化求解器（比如g2o）迭代求解，让所有“力”达到平衡，输出一串最优位姿序列，再从中提取出当前控制周期的速度指令。

跟DWA相比，TEB有几个明显优势。第一，它生成的轨迹更平滑，转弯轨迹是连续过渡的，不像DWA那样有生硬的折线感。第二，它能自然处理机器人的运动学约束，比如最小转弯半径、加速度限制，对阿克曼车型和差速车的适配性都很好。第三，TEB是可以“倒车”的，在狭窄空间里会主动设计倒车动作来调整姿态，这是DWA做不到的。

但TEB也有代价。它的计算量比DWA大得多，优化问题每一帧都要迭代求解，对机载电脑算力有一定要求。而且TEB的超参数特别多，每一条权重都要调，调不好容易出现轨迹抖动、速度忽快忽慢、掉头过于频繁等问题。新手直接上手TEB，很容易被它的一堆weight参数搞到心态爆炸。

2.3 其他方案与选型对照表

除了DWA和TEB，ROS生态里还有几个local planner方案，虽然用的人相对少，但各自有不可替代的场景。MPC（Model Predictive Control，模型预测控制）是这几年学术界和工业界都很热的方案，它把预测控制的思想引入局部规划，在每个控制周期内滚动求解一个有限时域最优控制问题，能显式处理各种复杂约束，控制效果最稳，但计算开销最大。还有基于纯跟踪（Pure Pursuit）的算法，算法极简、只跟踪前方一个目标点，适合做一些简单巡线任务，碰到复杂障碍物就抓瞎。

实际项目里怎么选，我给一张对照表供参考：

2.4 我的选择心得与换用场景

单从仿真demo来看，DWA和TEB好像差别不大，反正都能跑到目标点。但一旦上了实物车，差异就非常明显。我做过的几个项目里，室内小差速底盘用DWA比较多，原因很实在：拿一块树莓派或者Jetson Nano做机载电脑，DWA跑起来非常轻松，控制频率能拉到20Hz以上，反应灵敏；而且DWA的行为模式比较“耿直”，基本只往前走，不容易出现TEB那种为了调整姿态反复倒车、原地扭来扭去的“迷惑行为”。

后来做一款楼宇巡检机器人，底盘是差速转向但体积大、速度要求快，通道经常有门框和临时摆放的杂物，DWA就顶不住了。小车总是到了门框附近才开始慌慌张张地调整方向，几次差点蹭到门框。换到TEB之后，它在几米外就开始规划平滑的进门轨迹，配合倒车动作，在狭窄区域的表现明显上了一个台阶。

所以选local planner不是越高级越好，而是看你的底盘约束、环境复杂度、算力盈余这三者的平衡。仿真阶段务必把两种方案都跑一遍，至少了解它们的脾性，免得项目做到一半才换，那才叫一个痛苦。

3. DWA核心原理与参数调优实操

3.1 DWA的“动态窗口”到底是什么

DWA的运行流程可以拆成四个步骤。第一步是生成速度采样空间，第二步是筛掉不可行速度，第三步是对剩余速度组合模拟轨迹，第四步是轨迹评分。

先看速度空间。对差速驱动机器人来说，速度组合就是线速度v和角速度ω的二元组(v, ω)。理论上这个空间是无穷大的，但真正可能执行的速度范围要受到三重约束。第一重约束来自机器人硬件本身的速度极限，线速度不能超过max_vel_x，角速度不能超过max_vel_theta。第二重约束来自动力学，也就是加速度极限——假设当前车速是0.3m/s，加速度上限是0.3m/s²，采样周期是0.1s，那下一拍车速最高只能到0.33m/s，最低只能到0.27m/s。第三重约束来自安全制动，机器人必须保证在当前速度下、按照最大减速度刹停，轨迹不能撞到障碍物。

只要有一个采样速度组合过不了这三重约束中的任何一重，就直接排除。剩下的速度组合构成一个多边形区域，这个区域随着机器人当前状态每小时在动态移动，这就是“动态窗口”名字的由来。

这里有个很微妙的地方：障碍物检测是放在采样之前还是采样之后的顺序，直接决定了DWA的安全性预期。正常的DWA会在采样后模拟一段轨迹，然后检测这条轨迹上所有点离最近障碍物的距离，如果距离小于机器人半径，这个速度组合就会被丢弃。所以DWA本质上是“先验后验”：先采样，再验证。这个过程每100毫秒左右重复一次，够快到让机器人躲开动态障碍物。

3.2 轨迹评价公式与权重理解

速度空间筛完，剩下的合法速度组合依然有很多个，DWA靠一个评价函数来打分排序。ROS里经典的评分公式分三个分量——方位角代价（heading）、障碍物距离代价（dist）和速度代价（vel）。

方位角代价衡量的是：如果机器人以这个速度组合运动，模拟轨迹末端机器人朝向与目标方向之间的夹角差。夹角差越小，说明这个速度组合越能帮助机器人对准目标，评分越高。障碍物距离代价衡量的则是模拟轨迹上离障碍物最近的距离。最近距离越大，说明轨迹越安全，评分越高。速度代价最简单，就是鼓励机器人尽量跑得快，速度越高评分越高，防止机器人为了安全而长期龟速爬行。

最终的综合评分是这三者的加权和。这就引出了调参的核心：三个权重的相对大小决定了机器人的“性格”。如果path_distance_bias（障碍物距离权重）设得过大，机器人会变得极度保守，前面有一点代价区域就急刹车，甚至卡住不动。如果goal_distance_bias（目标方向权重）设得过大，机器人会猛打方向直扑目标，结果往往一头撞上中间的障碍物。如果速度权重过大，机器人会为了跑快而牺牲避障的安全边际，走出来的轨迹比较“浪”。

在代码层面，这些权重在teb_local_planner和base_local_planner的参数文件里的名字略有不同，写作时要留意当前ROS版本。

实操心得：DWA调参不要上来就动权重。先把速度极限、加速度极限、采样周期这些基础参数调到一个合理范围，再动权重。基础的动态约束没调好，权重怎么调都是在给错误系统打补丁。

3.3 一个简单实现框架（伪代码级）

理解了DWA的原理，写一个简化版的实现就顺理成章了。下面这个伪代码框架可以帮你快速对照ROS源码里的base_local_planner，逻辑是基本一致的：

每个控制周期： 读取当前速度(v0, w0)、当前位置和姿态、 局部代价地图、当前局部目标点 生成候选速度列表： for dv in velocity_increment: for dw in angular_increment: v = clamp(v0 + dv, min_vel_x, max_vel_x) w = clamp(w0 + dw, min_vel_theta, max_vel_theta) 模拟轨迹 = 以(v, w)按sim_time时长正向积分 轨迹上均匀取点 if 轨迹上任意点<a的栅格代价值大于阈值: 丢弃该速度组合 else: 计算heading_score = 轨迹末端朝向与目标方向夹角 计算dist_score = 轨迹离最近障碍物距离 计算vel_score = v 总评分 = α*heading + β*dist + γ*vel 记录到候选队列 选择总评分最高的速度组合，发布到cmd_vel

这里值得留意的是轨迹模拟这一步。DWA不是直接算出一条理论轨迹，而是用运动学模型做“正向积分”——从机器人当前位姿出发，按照一辆车在(v, w)下的运动规律，一步一步往前推sim_time秒，得到一条表现“如果我这样做，我会走到哪里”的轨迹。sim_time一般设为1到3秒，太短看不到完整避障效果，太长计算量上去了还容易把误差放大。

3.4 参数调试的顺序与踩坑记录

DWA参数多，但调试顺序有讲究。我先调的是速度极限：max_vel_x直接决定了机器人最快跑多快，配合acc_lim_x一起定。室内小底盘的典型起步参数是max_vel_x=0.3m/s、max_vel_theta=1.0rad/s、acc_lim_x=0.5m/s²、acc_lim_theta=1.5rad/s²。这个范围比较保守，宁可先慢一点把流程跑通，再加速度。

然后调sim_time和sim_granularity。sim_time决定模拟轨迹的时长，sim_granularity决定轨迹上的采样点间隔。采样点间隔越密，障碍物检测越准，但计算量越大。我一般先设置sim_time=2.0s、sim_granularity=0.05m，如果机器人显得“近视”——到了障碍物跟前才突然转向，就把sim_time调大到2.5s或3.0s；如果机器人反应迟钝、转向犹豫，就适当减小sim_time。

最后才是调三个权重。一个很常见的错误是阈值卡得太敏感，结果机器人动不动就“我全都要”式地紧急避让，速度掉到接近零然后原地打转。我遇到过的情况是，障碍物距离权重从0.1改成0.2之后，小车在走廊里就开始画龙，走两步停一下，回头检查代价地图才发现，膨胀层把整个通道染成了半红状态，DWA根本找不到一条“足够安全”的轨迹。

注意：如果你在RViz里看到局部代价地图周围一圈全是刺眼的红色，别急着骂local planner——先检查激光雷达安装高度、里程计漂移、tf树是否正常。local planner只是在它看到的地图上做决策，地图本身脏了，它再聪明也白搭。

4. TEB原理与工程化要点

4.1 时间弹性带：轨迹被“弹性拉直”

TEB跟DWA的思路完全不同，它不扫速度窗口，而是直接对整段路径做优化。它的名字里的“Timed”，表示每个路径点都携带一个时间信息，不只是地位移，还记录“什么时候到达这里”；“Elastic Band”表示整条路径被想象成一条有弹性的带子，可以在外力作用下拉伸、弯曲、变形。

具体来说，TEB把局部轨迹表示成一串离散位姿点，相邻点之间有固定或可变的时间间隔Δt。这个序列不是随便生成的，而是基于当前全局路径的局部截取段初始化而来的。在优化过程中，算法会在这些点之间施加若干“虚拟力”：

——目标吸引力：让轨迹末端正好指向局部目标点； ——障碍物排斥力：让轨迹点尽量远离障碍物和代价地图里的高价值区域； ——时间最优力：希望总时间尽量短，也就是速度尽量快； ——运动学约束力：保证相邻位姿之间的运动符合机器人转弯半径、加速度等物理极限； ——平滑性约束力：让轨迹点之间不要剧烈拐弯、不要抖动。

这些“力”最终被统一形式化为一个多目标最小化问题，通过图优化求解器迭代求解。每迭代一轮，轨迹就会往“受力平衡”的状态靠近一步。这个过程持续到收敛或者达到最大迭代次数，然后TEB从优化后的轨迹中提取当前需要执行的速度指令。

这里有个有趣的特性：优化过程中整条轨迹是不断“蠕动”的。你以为它最终会稳定在一条固定轨迹上，其实每个控制周期它都在微调，因为局部代价地图在更新、障碍物在移动、目标点也在切换。这种持续调整正是TEB能处理动态环境的原因，但也意味着如果优化问题参数设置不当，轨迹可能一直“扭来扭去”停不下来。

4.2 目标函数与图优化求解

我在这里把目标函数稍微展开一点，但不会把公式写成大段数学推导，尽量用“是什么意思”来翻译。TEB把优化问题写成几个目标项的和，每项都带一个权重：

• 障碍物代价项：每个位姿点到周围障碍物距离的惩罚函数。距离越近代价越高，低于安全阈值时代价趋近无穷。这个项直接决定机器人与障碍物保持的“心理距离”。
• 时间最优项：所有相邻的时间间隔之和，追求总时间最短。权重越大，机器人越倾向于走快路，哪怕稍微绕远一点。
• 轨迹跟踪项：轨迹偏离参考路径（初始全局路径）的代价。权重越大，轨迹越贴合全局规划给出的路径。
• 动力学约束项：对速度、加速度、转弯速率的惩罚。保证优化结果不是数学可行但物理上跑不出来的轨迹。

这个优化问题怎么解？TEB用的是基于g2o（通用图优化库）的求解框架。整个问题被抽象成一个图结构：位姿点作为图的顶点，顶点之间通过“约束边”连接。约束边代表运动学关系、时间关系、障碍物距离关系等。图优化求解器通过不断调整顶点（即位姿）的值，让所有边的残差最小化。

残差这个概念对机器人工程师来说很关键。所谓残差，就是约束边当前值与期望值的差，比如障碍物约束要求“轨迹点离障碍物至少0.3m”，当前值是0.1m，残差就是0.2m。优化求解器就是不断迭代，把这些残差压下去。如果你发现TEB规划的轨迹跟障碍物贴得太近，往往是权重不足，而不是求解器失效。

4.3 TEB调参重点与典型陷阱

TEB调参是个深坑，我吃过不少亏，把几个最关键的经验整理出来。

第一个关注的是dt_ref，这是TEB中两个相邻位姿之间参考时间间隔。dt_ref越小，轨迹上的点越稠密，优化出的轨迹更细腻，但计算量也更大；dt_ref太大，轨迹显得“稀疏”，在紧急避障时可能漏掉细节。我的经验值是在0.3到0.5s之间起步，再看CPU占用和规划的细腻程度调整。

第二个重点是一堆weight参数。weight_obstacle控制避障的优先级，weight_time控制“多快到达”的优先级，weight_shortest_path控制轨迹长度优先级。这三个参数在调的时候要特别小心，因为它们是相互拉扯的。weight_obstacle设高了，机器人见障碍物就绕，一条直线被它绕成S形；weight_time设高了，机器人会在障碍物边缘强行加速通过，看着都悬；weight_shortest_path设高了，它又会主动从障碍物边上“贴”过去，因为这样路径最短。

第三个是min_obstacle_dist。这是TEB规划轨迹时必须保持的最小障碍物距离，单位是米。这个值比costmap的膨胀半径要独立得多，设太小直接从视觉上就没有安全余量。我的建议是这个值不要小于机器人的底盘半径+5cm，还要考虑传感器噪声——激光雷达有系统偏差时，标称20cm的实际距离可能只有15cm。

再提醒一个很典型的TEB陷阱：开启倒车。TEB默认是允许倒车的，这在某些狭窄场景里很实用，但代价地图和传感器覆盖范围如果不能覆盖车尾区域，倒车就会变成“盲倒”。我一度遇到小车在走廊里疯狂来回倒车，一查才发现车尾方向激光雷达有盲区，TEB检测不到后面的障碍物，只能靠位姿估计硬倒。解决办法是限制min_vel_x不允许为负，或者确保车尾传感器覆盖充分。

4.4 DWA与TEB在同一台小车上的对比实验

为了给选型提供真实依据，我曾在同一台差速底盘上、同一个工作环境里分别跑了DWA和TEB。环境是一条大约12米长的走廊，中间摆了几把椅子当障碍，走廊尽头放了一个目标点。激光雷达布置在底盘中央，机载电脑是一块Jetson Nano。

DWA跑下来的结果是：平均速度0.28m/s，全程无碰撞，但在椅子附近会有明显的减速和小幅绕行动作，轨迹看起来有一点“犹豫”。从开始到抵达目标点用了大约50秒。TEB跑同一路段时，平均速度提到0.35m/s，轨迹平滑得多，在椅子之间走出了一个非常流畅的S形，中间几乎没有停顿，抵达目标点用了约38秒。代价也很明显：TEB在Jetson Nano上的CPU占用比DWA高出一大截，优化过程偶尔会出现1到2帧的延迟，不过还没到影响控制稳定的程度。

这个对比实验给了我一个很实际的选型参考：算力足够、环境复杂、追求效率，TEB是更好的选择；算力紧张、行为保守、追求稳定，DWA更香。没有绝对的好坏，只有匹配不匹配。

5. 仿真验证实战：从导航到抓取的一整套流程

5.1 仿真环境准备与地图构建

聊了这么多理论，实际操作才是检验真理的唯一标准。我在仿真环境里把local planner的完整调试流程走了一遍，这里拿Gazebo配合TurtleBot3模型来做演示。这套组合的好处是开源免费、社区资料多、复现成本低，用来理解导航栈的机制特别合适。

仿真第一步是构建地图。TurtleBot3仿真里已经带了gazebo世界文件和地图文件，直接用现成的就行。但如果你模仿的是真实场景，请在仿真里尽量还原实际环境——门框宽度、走廊长度、桌椅摆放位置，这些几何参数直接影响local planner的行为。我在仿真的摆放了跟办公室一致的桌椅和柜子，这样后面跑了有效果才能指导实物部署。

地图建好后，先启动AMCL定位，再用RViz设定目标点，做一个快速验证：机器人能不能从A点平滑走到B点。这个阶段不要急着上抓取，先把导航的“基本功”验证到位。

5.2 local planner的launch配置与启动

在仿真里切换local planner，关键在move_base的launch文件。你要在params目录下的local_planner参数文件里指定算法插件。DWA用的是base_local_planner的DWAPlannerROS插件，TEB用的是teb_local_planner的TebLocalPlannerROS插件。把参数文件里base_local_planner这个键的值换成对应的插件名，再启动move_base，就完成了切换。

参数文件的格式大同小异，通常包含了机器人的速度上限、加速度上限、目标容差、代价地图配置等。以下是我在仿真中使用的局部规划参数核心片段，能直接参考：

base_local_planner: teb_local_planner/TebLocalPlannerROS TebLocalPlannerROS: odom_topic: odom map_frame: map # 运动学约束（以TurtleBot3为例） max_vel_x: 0.25 max_vel_x_backwards: 0.15 max_vel_theta: 1.0 acc_lim_x: 0.3 acc_lim_theta: 1.0 # 目标容差 xy_goal_tolerance: 0.15 yaw_goal_tolerance: 0.1 # 障碍物 min_obstacle_dist: 0.2 penalty_epsilon: 0.1 weight_obstacle: 50.0 weight_time: 5.0 weight_shortest_path: 8.0

启动之后，在RViz里查看局部路径那条彩色带子。如果机器人走得很顺，彩色带子会稳定地贴在全局路径附近，速度指令平滑；如果出现轨迹扭曲、频繁掉头，先看权重，再看运动学约束有没有设太小。

5.3 机械臂抓取与local planner的衔接

仿真中做“导航+抓取”任务时，local planner的角色不止是把机器人开到目标点，还要保证最终停位姿态符合抓取需求。这一点经常被忽略——机器人到达“目标点”不等于到达“可抓取位姿”。

我在仿真里布置了一个抓取任务：机器人需要从起点出发，导航到桌子前的目标点，然后机械臂把桌上的方块夹起来。难点在于，目标点不能离桌子太近，否则机械臂伸展范围不够；也不能太远，否则够不着。local planner的xy_goal_tolerance参数这时候就起作用了，它决定了机器人停在多远的范围算到达。如果这个容差设得太大，机器人可能在距离目标点半米外就认为“我到了”，机械臂往下伸，直接抓空。

正确的做法是把导航目标点设置成“机械臂基准坐标系能达到的理想位置”，然后在local planner的容差范围内要求尽可能精确地停位。我在仿真里把xy_goal_tolerance从默认的0.2m收紧到0.1m，yaw_goal_tolerance从0.1收紧到0.05rad，这样机器人停下后机械臂的抓取成功率明显提升。

实操心得：做导航+机械臂联合任务，一定不要等到机器人“到了”再去伸机械臂。提前在仿真里标定好，机械臂的抓取工作空间跟底盘停位偏差之间的映射关系，然后反推local planner的容差要求。我见过太多项目卡在“导航到了但抓不到”这种问题上，根子就是停位精度没跟抓取需求对齐。

5.4 实测记录与数据复盘

我在仿真里反复跑了十次完整的导航+抓取流程，记录了每次的导航时间、停位误差、抓取成功率。DWA版本的数据是：平均导航时间45秒，停位误差在x方向7cm、y方向4cm，yaw方向偏差约0.06rad，抓取成功率七成左右，失败的几次基本都是停位时车头朝向偏了。TEB版本的数据明显更好：平均导航时间36秒，停位误差x方向4cm、y方向2cm，yaw方向偏差0.03rad，抓取成功率九成。

这个数据很有说服力。差异主要来自TEB在接近目标点时会主动做姿态调整，在路径末尾规划出一段平缓的“对齐轨迹”；而DWA是到了目标点附近才开始匆忙转方向，姿态调整的余地有限。如果你的项目涉及机械臂抓取、充电对接、停靠取货这类对接动作，建议优先考虑TEB，或者至少在目标点附近增加一个“姿态调整阶段”。

仿真跑完后再上实物，顺序一定要保持：先在仿真里把参数调到行为和实物预期一致，再上实物车验证。因为仿真是理想化环境，很多实物上的噪声和摩擦效应它模拟不出来。仿真阶段把行为调顺了，上实物后只需要微调传感器话题和里程计参数，不需要从头再来。

6. 常见问题与排查技巧实录

6.1 典型故障现象与原因速查表

做local planner调试的过程中，我积累了不少“症状-病因”对照经验。整理成速查表放在这里，遇到问题可以先把下面的原因排除一圈，大概率能解决一半以上：

6.2 排查思路与日志分析技巧

排查local planner问题时，不要上来就猛改参数。我习惯遵循三步走：先看数据流，再看代价地图，最后才动参数。

第一步看数据流。启动导航后，在终端里用rostopic echo /cmd_vel观察速度指令是否规律；再监听odom话题，确认底盘反馈的里程计数据是否是合理的平滑变化。如果cmd_vel里速度指令跳变剧烈，很可能是local planner在“纠结”；如果cmd_vel一直为零但规划在跑，问题可能出在目标点设置或代价地图上。

第二步看代价地图。打开RViz，把Map显示切到/local_costmap/costmap，把代价值显示范围调成可见。仔细观察机器人周围一圈的代价分布：如果障碍物周围的高代价区域过宽，机器人当然不敢过；如果高代价区域比实际障碍物偏了，问题在传感器或tf上，不在local planner上。这一步能筛掉大量假故障，很多人卡在local planner上半天，最后发现是激光雷达装歪了。

第三步才动参数。每次只改一个参数，改完跑一遍同一条路线对比效果。一次改多个参数的做法看起来省事，实际上根本分不清是哪个改动起了作用。我个人的习惯是每调一个参数就记录当时的现象和结论，整个调参过程下来，你手上的参数档案就是这个项目最值钱的沉淀。

6.3 调参先调什么的经验法则

最后送一个我摸索出来的调参顺序，基本可以套用在任何local planner上，按照这个顺序走一遍，能避免大多数“调了半天发现搞反了”的情况。

从硬件约束开始：速度上限、加速度上限、角速度上限。这些参数必须跟底盘的实际驱动能力匹配。把上限设小一点不丢人，跑得稳比跑得快重要得多。再从传感器噪声出发：如果激光雷达有抖动或者里程计有漂移，先把这些数据喂进系统时做滤波，别指望local planner能靠算法兜住脏数据。

然后是运动学与安全约束：最小障碍物距离、膨胀半径、安全制动距离。这几个参数决定机器人的“保命底线”。底线设定合理后，机器人至少不会撞墙，再谈效率。

接下来才是行为权重：目标方向权重、障碍物权重、速度权重，这些参数决定机器人的“性格偏好”。最后是控制频率和优化细节：sim_time、dt_ref等，在行为表现基本满意后再收尾微调。

熟悉这套流程以后，你会发现local planner的调参不是玄学，而是一个有明确因果关系的工程问题。每一步都有据可依，每一个现象都能对应到某一层的参数，排查效率能翻好几倍。

——说回我自己，从最初不知道local planner和全局规划有什么区别，到后来能在仿真和实物上熟练切换DWA和TEB，花了挺长时间踩坑。这个过程中最有价值的领悟就是：导航系统是一个整体，local planner只是最后一环，前面任何一环出错都会在它身上体现出“症状”。所以遇到问题别急着骂local planner，先把你自己的数据链路检查一遍。等哪一天你的机器人能在障碍物中间走出漂亮平滑的曲线、稳稳停在抓取点前，那种成就感，真的比看一百遍理论文章都爽。