机器人贴着走廊中线跑得好好的，前方忽然横着一张椅子，全局路径从椅子边缘擦过去——这种场景几乎每个做移动机器人导航的人都撞见过。DWA 在这种时候经常给出两条都不太体面的选项：要么提前减速到几乎停住，绕不出去；要么一把甩出去，把切角切到墙里。换成 TEB（Timed Elastic Band，时间弹性带）之后，同一个场景的表现会明显不一样：轨迹会在椅子外侧鼓出一个平滑的小弧，速度不掉太多，车头也不会来回摆。TEB 是 ROS 生态里用得最多、被吐槽也最多的局部规划器之一，它把"轨迹"当成一根可以拉伸、弯曲、有时间属性的橡皮筋，用图优化一次性解出位姿序列和时间间隔。它的能力上限很高，参数表也长得劝退，weight_ 开头的参数密密麻麻一屏装不下。这篇东西想做的事很简单：把 TEB 从数学模型到参数调优这条线捋直，让刚接手导航栈的人知道每个参数在拧哪颗螺丝，也让正在被"原地打转""贴墙切角""优化失败"折磨的人有几条能立刻验证的排查路径。
1. 为什么值得花时间啃 TEB：从"弹性带"这个名字说起
1.1 TEB 到底在优化什么
先把这个名字拆开。"Band"（带子）指的是轨迹本身——一串带时间戳的机器人位姿，可以想象成一根穿在若干珠子上的橡皮筋；"Elastic"（弹性）指的是这根带子允许被拉伸和弯曲，代价由各种约束的惩罚项决定；"Timed"（时间）则是最关键的那个词：TEB 优化的不只是路径形状，还有每个位姿之间花多少时间。
这一点把它和绝大多数"先规划一条几何路径、再沿路径做速度规划"的两段式方案彻底分开了。几何路径规划器（A*、Dijkstra、全局规划）输出的是"走哪里"，速度是事后沿着弧长分配的一个附属品；TEB 把位姿序列和一个时间间隔序列 ΔT 放进同一个优化问题里联合求解。结果就是：机器人为了让某个急弯过得稳，可以自动在弯前多花一点时间；为了在直道上冲起来，可以把 ΔT 压小。时间不是被分配出来的，是被"优化"出来的。
好处很实在。第一，轨迹天然满足时间和空间的耦合约束，速度、加速度、转弯半径能不能实现，是在优化里一次性考虑的，不会出现"路径规划得很漂亮但车根本拐不过来"的尴尬。第二，它输出的是一条完整的、带时间戳的局部轨迹，任意时刻都能从中查询未来若干秒的期望位姿和速度，天生适合做前馈控制。第三，它的约束是可叠加的，加减速限制、非完整约束、最小转弯半径、障碍物避让、跟随全局路径，全都可以作为"边"塞进同一个图里。
代价也很明确：这是一个非凸优化问题。非凸意味着初值极其重要，意味着它可能收敛到局部最优，意味着同样的输入在不同初值下可能给出完全不同的轨迹。理解了这一点，后面很多"玄学现象"就不玄学了——不是参数坏了，是初值或者代价地形本身就长那样。
1.2 与 DWA 的分水岭：采样法 vs 优化法
把 TEB 和 DWA 放在一起对比，不是为了分高下，而是为了搞清楚什么时候该选谁。它们的哲学完全不同：DWA 是在速度空间 (v, ω) 里做离散采样，对每个采样点做前向仿真打分，选分数最高的一个执行一个控制周期；TEB 是在位姿空间里做连续优化，解出一条完整的轨迹再提取当前时刻的控制量。
| 对比维度 | DWA（速度空间采样） | TEB（图优化） |
|---|---|---|
| 搜索空间 | (v, ω) 离散采样 | 位姿序列 + 时间间隔，连续 |
| 约束表达 | 硬编码在采样范围里 | 软约束，作为代价项参与优化 |
| 非完整约束 | 天然满足（前向仿真） | 显式建模为边，可加权 |
| 多障碍复杂场景 | 容易出现评分断层 | 平滑性通常更好 |
| 计算开销 | 低且稳定 | 波动大，随位姿数增长 |
| 参数数量 | 十几个 | 六七十个 |
| 局部最优风险 | 采样分辨率不足会漏解 | 非凸优化，可能收敛到坏解 |
| 调参难度 | 中等 | 高 |
实用判断：如果你的场景是开阔场地、障碍物稀疏、机器人速度不高，DWA 调好之后足够稳，调参成本还低。如果你的场景是狭窄、障碍物密集、需要过弯平滑、机器人有阿克曼或差速但转弯半径受限，那 TEB 的优势会明显得多——它能给出那种"贴着障碍物外侧鼓出来一个小弧线"的轨迹，采样法很难靠离散点凑出这种精度。
注意：TEB 不是万能的。它严重依赖局部代价地图的质量和障碍物点的表达方式，如果 costmap 里全是膨胀得很大的栅格、障碍物轮廓一团糊，TEB 的优化会在一个失真的地形上求最优解，出来的轨迹照样难看。上线前先把 costmap 调对，再谈 TEB 参数。
2. TEB 的骨架：超图、顶点与边
2.1 顶点：位姿序列与时间间隔
TEB 内部用的求解器是 g2o，一个通用的图优化框架，原本为 SLAM 的位姿图优化而生。TEB 借它来解自己的超图（hyper-graph），所谓"超图"是因为这里存在连接两个以上顶点的边——时间间隔顶点同时连着前后两个位姿顶点，这类边在普通图里表达不了。
顶点分两类，非常清晰：
- 位姿顶点
PoseSE2，存的是 (x, y, θ) 三个量。一条中间轨迹至少有 2 个位姿，实际跑起来常见的是 10 到 30 个，取决于dt_ref和规划时域。 - 时间间隔顶点
TimeDiff，存的是 ΔT，即第 i 个位姿和第 i+1 个位姿之间的时间。数量是位姿数减一。
位姿数不是固定的，这是 TEB 一个很容易被忽略的机制：优化过程中会根据dt_ref（参考时间间隔）自动增删顶点。如果某一段的实际 ΔT 大于dt_ref + dt_hysteresis，就在这段中间插入一个新位姿，把时间跨度拆细；反之如果 ΔT 小于dt_ref - dt_hysteresis，就删掉一个位姿。dt_hysteresis的作用是防抖，避免在阈值附近反复增删导致轨迹长度振荡。默认dt_ref = 0.3、dt_hysteresis = 0.1，意思是 ΔT 超过 0.4 秒就插点，低于 0.2 秒就删点。
这个机制决定了 TEB 的自适应能力，也埋了一个坑：位姿数会增长。机器人速度越快、规划时域越长，需要的位姿就越多，单次优化耗时线性上升。我在一个差速底盘上把max_vel_x从 0.5 提到 1.2 之后，优化耗时从 8 ms 涨到了 26 ms，几乎全是因为位姿数翻了一倍多。所以后面讲参数时，dt_ref不是个可以随手调的小数字，它直接决定计算预算。
2.2 边：把工程需求翻译成数学项
TEB 真正的精华在边上。每一条边就是一个"我要求这件事尽量成立"的声明，写成误差函数，优化器负责让所有误差的加权平方和最小。理解了每条边在惩罚什么，参数表就不难读了。
障碍物边EdgeObstacle：对每个位姿顶点，找它附近最近的若干个障碍物点，如果距离小于min_obstacle_dist，误差就按差值惩罚。这里的"附近若干个"由obstacle_poses_affected控制，默认 30，含义是一个障碍物点会影响它周围 30 个位姿顶点。这个数字看起来无所谓，实际上很要命——设小了，障碍物只对离它最近的那个位姿起作用，轨迹会像针一样扎过去；设大了，计算量飙升而且相邻位姿被过度耦合，轨迹会变得又硬又钝。
膨胀边EdgeInflatedObstacle：在min_obstacle_dist之外再加一层inflation_dist，形成软边界。距离在min_obstacle_dist以内是强惩罚，介于两者之间是较弱的惩罚。这一层的价值在于给优化器一个"提前量"，让轨迹在被硬怼之前就开始拐，而不是等到已经踩线才剧烈修正。
跟随边EdgeViaPoint：惩罚位姿顶点偏离全局路径上的关键点（via point）。全局路径不是整条都拿来跟随，而是按global_plan_viapoint_sep抽稀成若干关键点，每个位姿顶点只关联离它最近的那一个。这个设计很关键——如果整条全局路径都参与约束，TEB 会被"钉死"在路径上，绕障能力就没了。
运动学边EdgeKinematicsDiffDrive/EdgeKinematicsCarlike：前者约束相邻位姿之间的位移方向与车头朝向一致（禁止横移），后者额外加最小转弯半径的约束。这是 TEB 相对采样法的一个明显优势：非完整约束是显式写进图里的，不是靠仿真间接满足的，所以优化器"知道"自己不能侧着走。
速度与加速度边：EdgeVelocity、EdgeAcceleration分别对最大速度、最大加速度做惩罚，还有EdgeVelocityStart、EdgeAccelerationStart、EdgeAccelerationGoal处理起点和终点的边界情况。起点速度必须和当前实际速度接上，终点可以在free_goal_vel打开时自由处理，否则要求到点速度为零。
时间最优边EdgeTimeOptimal：这一条很有意思，它的作用是在满足其他约束的前提下尽量压缩总时间。数学上它来自一个马尔可夫假设——每个时间间隔是独立的指数分布，取负对数后得到的就是 ΔT 本身。所以这条边不是"惩罚某个物理量"，而是把"快"这个目标本身编码进去了。
最短路径边EdgeShortestPath：权重weight_shortest_path默认是 0，也就是关闭。打开之后 TEB 会倾向于选择几何上更短的路径，好处是绕大圈的情况会减少，坏处是它和避障权重打架，容易把轨迹往障碍物上压。我的经验是默认别开，除非你发现机器人总是绕出很夸张的大弧。
2.3 权重归一化与量纲问题
所有边的误差项量纲根本不一样：距离是米，速度是米每秒，加速度是米每二次方秒，时间是秒，角度是弧度。把它们直接相加是没有物理意义的。TEB 的做法是对每个误差项做归一化——除以对应的阈值或上限，变成无量纲量，再乘以权重。
举个例子，速度边的误差大致是max(0, v - v_max) / v_max，这样超速 10% 和超速 100% 在数值上有可比性。加速度边同理，用acc_lim归一化。障碍物边用min_obstacle_dist归一化。
这件事的实际意义是：weight_obstacle = 50和weight_optimaltime = 1之间不是"50 倍重要"的关系，因为两个误差项的量纲已经被各自归一化过了，权重之比反映的是在归一化空间里的相对优先级。理解这一点之后，调参时就不会陷入"我该把这个权重除以 10 还是乘以 10"的茫然——你其实是在调两个目标的相对优先级，而不是在调绝对数值。
提示：
weight_adapt_factor这个参数很多人没注意。它控制的是内外层迭代时障碍物权重的放大倍数，默认 2.0。每次外层迭代如果发现轨迹仍然碰撞，就把障碍物权重乘上这个系数再来一轮，最多no_outer_iterations（默认 4）轮。这相当于一个自适应的"越来越严厉"的机制，权重不够时它能兜底，但也可能因为反复放大导致轨迹剧烈变形。
3. 从全局路径到可执行速度指令的完整链路
3.1 全局路径预处理与 via point 抽取
TEB 拿到的是全局规划器给的nav_msgs/Path，但它不会直接照着这条路跑。第一步是路径裁剪：从当前机器人位置往前截取一段长度（由max_global_plan_lookahead_dist控制），太远的路径对局部规划没有意义，反而增加计算负担。
第二步是抽稀成 via point。按global_plan_viapoint_sep指定的弧长间隔采样，如果设成 -1 就退化成"每个位姿都跟随最近的一个路径点"，等于全路径都参与约束。这一步的设计意图很明确：给优化器留出自由度。全局路径只提供"大方向"，具体怎么走由局部避障决定。
这里有个常见误区：把global_plan_viapoint_sep设得太小（比如 0.05），以为"跟得越紧越好"。实际上密集的 via point 会把轨迹牢牢钉在全局路径上，避障权重再大也拉不开，机器人就会贴着全局路径硬顶障碍物。我一般在 0.3 到 0.8 之间取值，视全局路径的平滑程度而定。
3.2 初始轨迹生成
非凸优化对初值敏感，TEB 的初始化做得相对朴素：把裁剪后的全局路径按dt_ref的间距重采样，得到初始位姿序列，每个 ΔT 初始化为dt_ref。如果当前机器人不在路径起点上，会用一段平滑过渡把当前位置接进去。
初始化阶段有几个可调开关值得知道：
allow_init_with_backwards_motion：允许初始化时倒车。差速底盘在一些狭窄场景里倒着接上路径更顺，但打开之后可能产生你意想不到的倒车轨迹。global_plan_overwrite_orientation：是否用全局路径的方向覆盖位姿的朝向。差速底盘通常保持 true，因为全局路径的方向就是合理的车头朝向；但如果全局路径本身有锯齿，覆盖之后初值会抖。prefer_rotdir/weight_kinematics_forward_drive：偏好转向方向和前向偏好。前向权重默认 1.0，倒车时这个权重决定 TEB 有多不情愿倒。
初始化质量差会直接导致两类现象：一是轨迹在第一个控制周期跳变明显，二是优化陷入局部最优解出"绕远路"或者"贴着障碍物边"的怪轨迹。如果遇到这类问题，先别急着调权重，先确认全局路径本身是不是平滑的、起点是不是和机器人实际位姿对得上。
3.3 内层外层迭代与可行性检查
TEB 的优化是双层结构。内层迭代（no_inner_iterations，默认 5）解一次当前图的优化；外层迭代（no_outer_iterations，默认 4）负责检查可行性、调整图结构（增删时间间隔顶点）、必要时放大障碍物权重。
外层迭代里最关键的动作是可行性检查：feasibility_check_no_poses指定从轨迹末端往前检查多少个位姿是否碰撞。默认 4，意思是只看最后 4 个位姿。如果这几个位姿全都不碰，就认为整个规划时域可行，提前退出外层迭代；如果碰了，就加大障碍物权重再来一轮。
这个设计的逻辑是：轨迹末端离机器人最远，那里可行不代表中间一定可行，但工程上没必要全查，只查末端是精度和耗时之间的折中。反过来，如果你的场景里障碍物经常出现在轨迹中段而末端是空的，feasibility_check_no_poses设小了就会漏检，机器人跑到中途才发现要撞。密集障碍环境里我会把它提到 6 到 8，代价是优化耗时涨一点。
3.4 控制量提取
优化完得到一条位姿序列，最后一步是从中提取当前时刻该发的geometry_msgs/Twist。TEB 的做法是找第一个时间间隔大于控制周期的位姿，用起始位姿和它的差除以 ΔT，得到 v 和 ω。control_look_ahead_poses可以往前多看几个位姿做平滑，默认 1，调大之后速度指令更平滑但响应变慢。
差速底盘还有一种特殊情况：cmd_angle_instead_rotvel打开后，角度通过Twist.angular.z传的是车头朝向而不是角速度。这个参数默认 false，很多人不知道它的存在，遇到"底盘收到的指令和自己的期望对不上"时可以检查一下这里。
整个链路跑通之后，实测一个典型差速底盘的耗时分布大致是：路径预处理和 via point 抽取 1~2 ms，优化主体 5~20 ms（随位姿数波动），控制提取不到 1 ms。控制频率设 10 Hz 时余量充足，设 20 Hz 就要留意大场景下的尖峰了。
4. 拓扑多路径：TEB 最容易被忽略的一半
4.1 同伦类的直观理解
走廊里横着一张椅子，左边能过右边也能过，这是两条在拓扑上不等价的路线——无论怎么平滑形变，都不能让左边的路线连续变成右边的路线而不穿过椅子。这就是同伦类（homotopy class）的直观含义。
普通的局部规划器只会解出其中一条，具体是哪条取决于初值和代价地形，往往还带着随机性。TEB 的做法是同时维护多条候选轨迹，每条属于不同的同伦类，然后按代价选出最好的那条。这个特性叫enable_homotopy_class_planning，默认是关的，但它才是 TEB 真正区别于一众局部规划器的地方。
打开之后你会看到 RViz 里同时出现好几条彩色候选轨迹（绿色是当前选中的，其余是备选），机器人会在障碍物移动、道路被堵死的情况下自动切换到另一条候选，而不是傻等在原地震荡。这个能力在动态环境里价值极大。
4.2 概率路线图采样与等价类判定
候选轨迹怎么生成？TEB 用的是一套"采样 + 等价类聚类"的流程：
- 在局部区域的矩形范围内用概率路线图（PRG）随机撒点，范围由
roadmap_graph_area_width、roadmap_graph_area_length_scale和roadmap_graph_no_samples控制。 - 每个采样点向内连接若干近邻形成路线图，从起点搜到终点得到大量候选路径。
- 对每条候选路径计算 H 签名（H-signature），一种用于判定路径同伦类的数学不变量。签名相近的认为属于同一类，只保留一条。
- 每类路径分别初始化一个 TEB 优化器，并行（或串行）优化。
- 按代价选出最优的一条。
H 签名和近似的精度由h_signature_prescaler和h_signature_threshold控制，前者是离散化精度，后者是判定为同一类的阈值。候选类数量上限是max_number_classes，默认 5，这个数字直接决定计算量——5 类就是 5 份优化开销，在低算力平台上必须往下压，通常 2 到 3 是实际可用的甜点。
4.3 什么场景该开、什么场景必须关
开了拓扑规划的代价很直白：CPU 占用可能是关闭时的 3 到 5 倍。所以判断标准应该基于场景复杂度而不是"功能看起来更高级"。
| 场景特征 | 建议 | 理由 |
|---|---|---|
| 室内走廊、办公室、多岔口 | 开启，max_number_classes取 3 | 多个同伦类频繁出现，切换能力有价值 |
| 仓储货架通道、单向狭窄 | 开启，取 2 | 两侧对称，备选路径有意义 |
| 开阔厂房、稀疏障碍 | 关闭 | 只有一个合理同伦类，白烧 CPU |
| 低算力嵌入式平台 | 关闭或取 2 | 优化耗时不稳定会影响控制周期 |
| 动态行人环境 | 开启，取 3~4 | 路径被封死时能立刻切换 |
| 需要严格确定性复现 | 关闭 | 采样带随机性，轨迹不完全可复现 |
还有一个细节：selection_alternative_time_cost打开后，选优时代价里会包含时间成本而不只是几何成本，这会让选择更偏向"绕路但更快"的方案。默认 false，在长距离走廊里打开效果不错，但在狭窄环境里容易选出激进的绕行。
注意：
roadsgraph那组采样参数和全局规划的采样半径一旦配得太小，候选路径会退化到只剩一条，"多路径"变成摆设。排查时把 RViz 的 marker 打开，看候选轨迹是不是真的有多条颜色不同的曲线，没有就说明采样范围不够。
5. 参数调优实战：一份可抄的配置清单
5.1 机器人本体约束参数
这一组是硬约束，必须和底盘规格严格对齐，抄错一个数全盘皆输。它们决定了哪些轨迹在物理上"根本不可能实现"。
TebLocalPlannerROS: max_vel_x: 0.8 # 前进最大速度，实测能稳住的上限，不是电机标称 max_vel_x_backwards: 0.3 # 倒车速度，通常取前进的 1/3 到 1/2 max_vel_theta: 1.2 # 最大角速度，rad/s acc_lim_x: 0.6 # 线加速度上限，实测值 acc_lim_theta: 1.5 # 角加速度上限 min_turning_radius: 0.0 # 差速底盘填 0；阿克曼必须填真实值 wheelbase: 0.0 # 仅阿克曼需要，轴距 cmd_angle_instead_rotvel: falsemax_vel_x的取值原则：填底盘在带载、地面略有摩擦的条件下真正能跑到的速度，不是空载标称速度。填大了 TEB 会规划出根本跑不到的轨迹，底盘跟不上，误差累积导致轨迹抖；填小了白白牺牲效率。acc_lim_x同理，填实测值而不是电机峰值。min_turning_radius对差速底盘填 0，一旦手滑填了非零值，会触发阿克曼约束，差速车的行为会变得莫名其妙。
5.2 障碍物相关参数
这组是 TEB 里最需要反复试的部分，因为它和 costmap 的分辨率、膨胀层、障碍物提取方式强耦合。
min_obstacle_dist: 0.35 # 硬约束距离，小于这个值强惩罚 inflation_dist: 0.55 # 软约束距离，提前量 include_costmap_obstacles: true costmap_obstacles_behind_robot_dist: 1.0 obstacle_poses_affected: 25 costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" costmap_converter_rate: 5min_obstacle_dist要大于底盘外接圆半径加一点安全余量。车宽 0.5 米、长 0.7 米的底盘，外接半径约 0.43，min_obstacle_dist至少 0.45 才能保证不刮蹭。很多人把这数设得比车体半径还小，然后在 RViz 里看着轨迹贴边过得去，实车直接擦墙——因为优化是在质点模型上做的，车体的实际轮廓要靠这个参数来补。
inflation_dist和min_obstacle_dist的差值决定"提前量"的宽度，我一般取 0.2 左右。差太小，轨迹在红线附近走钢丝，抖；差太大（比如 3 倍），轨迹会过度保守，窄通道直接过不去。
obstacle_poses_affected这个参数特别值得单独说。它的物理含义是"一个障碍物点影响多少个位姿"。设成 5，障碍物只对最近几个位姿起作用，轨迹会从障碍物边缘穿针；设成 100，一个障碍物点的影响范围扩散到整条轨迹，局部细节全被抹平，车会绕得很远。25 到 40 是常用区间，取决于dt_ref和平均位姿数——大致经验是取位姿总数的 1 到 1.5 倍。
5.3 时间与权重参数
dt_ref: 0.3 dt_hysteresis: 0.1 global_plan_viapoint_sep: 0.5 weight_max_vel_x: 2.0 weight_max_vel_theta: 1.0 weight_acc_lim_x: 1.0 weight_acc_lim_theta: 1.0 weight_kinematics_nh: 1000.0 weight_kinematics_forward_drive: 1.0 weight_optimaltime: 1.0 weight_obstacle: 50.0 weight_inflation: 0.2 weight_viapoint: 1.0 weight_shortest_path: 0.0 weight_adapt_factor: 2.0权重里唯一"必须很大"的是weight_kinematics_nh，默认 1000 是有道理的：非完整约束是物理硬约束，差速车就是不能横移，这个惩罚必须强到优化器不敢违反。有人调小它想换来更灵活的运动，结果就是轨迹里出现横向位移，底盘执行时完全是另一回事。
其余权重的相对关系大致可以这样理解：障碍物权重 50 是"绝对不能让"的级别，viapoint 权重 1 是"尽量跟随"的级别，时间最优权重 1 是"能快就快"的级别。如果发现机器人绕障绕得太远，优先降weight_obstacle（比如到 30）或者降inflation_dist，而不是去加weight_optimaltime；后者会让整体速度变激进，副作用更大。
5.4 我实际用的调参顺序
参数太多的时候，乱调一定翻车。我自己的顺序是这样的，每一步只动一组，跑同一个场景录 rosbag 对比：
- 先对齐物理约束：
max_vel_x、acc_lim_x、max_vel_theta、acc_lim_theta，全部填实测值。这步错了后面全白搭。 - 再定安全距离：
min_obstacle_dist按车体外接半径加余量定，inflation_dist先设成min_obstacle_dist + 0.2。 - 调跟随强度：
global_plan_viapoint_sep从 0.5 起试，观察机器人是否被钉死在全局路径上。 - 处理轨迹形态：轨迹抖动调
obstacle_poses_affected和weight_obstacle；绕太远反过来调。 - 调时间行为：
dt_ref决定计算量和轨迹分辨率，weight_optimaltime决定激进程度，最后动。 - 最后才考虑拓扑：基础参数稳定之后再开
enable_homotopy_class_planning，看是否有收益。
每一步都建议录 bag，用rosbag play复现，不然同一场景的两次运行差异根本分不清是参数变了还是障碍物位置变了。
6. 常见问题与排查实录
6.1 机器人原地打转或来回摆动
这是最高频的问题，成因有几种，得按顺序排查。
第一种：目标点在机器人背后或者侧面很近的位置。差速车此时理论上需要转大半个圈，但如果weight_kinematics_forward_drive很大，它又不愿意倒车，就会在"转"和"不倒"之间来回拉锯。解法是把weight_kinematics_forward_drive降到 0.1 到 0.3，或者允许倒车（max_vel_x_backwards > 0）。
第二种：xy_goal_tolerance设得比实际控制精度还小。比如设成 0.02 米，而定位噪声就有 0.03 米，机器人永远到不了，就会在目标点附近绕圈。这个值要大于定位精度加控制误差，一般 0.1 到 0.2 比较合理。
第三种：min_obstacle_dist太大，把车自己"围"住了。如果机器人当前位置离某个障碍物的距离已经小于min_obstacle_dist，优化器会认为初始状态就不可行，拼命想把轨迹推出去，表现出来就是抖动。排查方法是在 RViz 里看机器人周围的膨胀障碍物是不是已经覆盖了车体中心。
第四种：拓扑多路径在两条等价路径间反复切换。现象是候选轨迹颜色来回变。解法是调大selection_cost_hysteresis（默认 1.0，可以提到 1.5），让切换需要更大的代价优势。
6.2 贴障碍物、切角、擦墙
轨迹看起来"擦着"障碍物过去，实车撞了，这类问题几乎全部指向距离参数和障碍物表达。
先检查min_obstacle_dist和车体半径的关系，这是第一嫌疑。然后检查costmap_converter_plugin：如果用的是CostmapToLinesDBSRANSAC，障碍物被拟合成线段，墙角会被简化掉，TEB 认为那里是空的；换成CostmapToPolygonsDBSMCCH会保留多边形轮廓，更保守但也更准。还有一种情况是 costmap 的inflation_radius和cost_scaling_factor配得太激进，障碍物在 costmap 层面就已经糊成一大坨，TEB 看到的地形本身失真。
一个容易被忽略的点：obstacle_poses_affected太小会让障碍物只对单个位姿起作用，那个位姿被推开了，相邻位姿没被推到，轨迹就在障碍物边缘形成尖锐的折角，底盘执行时因为转弯半径限制切出去，正好撞上。这个现象很有辨识度——轨迹在障碍物旁边有个明显的尖点。
6.3 优化失败与轨迹跳变
日志里出现teb failed或者轨迹突然消失，通常是这两种情况。
初值不可行。机器人已经贴住障碍物了，初始的全局路径直接穿过膨胀区域，优化器找不到下降方向。这时候可以打开allow_init_with_backwards_motion，让它有更多自由度退出来。
位姿数爆炸。dt_ref太小或者max_global_plan_lookahead_dist太大，位姿数超过几十个，单次优化超过控制周期，规划器开始丢弃结果。实测下来，位姿数控制在 30 以内比较安全，超过 50 就要考虑缩小时域或者加大dt_ref。
还有一种隐蔽的情况：全局路径本身不连续，有跳跃或者回环，TEB 抽出的 via point 顺序是乱的，优化结果自然一团糟。排查方法是把全局路径在 RViz 里单独显示出来看，很多时候问题根本不在 TEB。
6.4 CPU 占用过高
TEB 的 CPU 占用不是线性的，它在某些场景下会突然飙高。常见的三个原因：一是拓扑规划开着且max_number_classes大于 3；二是obstacle_poses_affected设得过大，每条障碍物边的计算量随它线性增长；三是costmap_converter_rate设得太高，相当于在后台频繁做障碍物聚类。
我的建议是把enable_multithreading打开（默认 true，但要确认真的生效），然后把costmap_converter_rate降到 5 Hz 左右——障碍物分布变化没那么快，5 Hz 足够。另外确认feasibility_check_no_poses不要设得太大，它是每次外层迭代都要跑一遍碰撞检测的。
6.5 问题速查表
| 现象 | 第一嫌疑参数 | 调整方向 |
|---|---|---|
| 原地打转 | weight_kinematics_forward_drive | 降到 0.1~0.3 |
| 到点绕圈 | xy_goal_tolerance | 提到 0.1~0.2 |
| 贴墙擦碰 | min_obstacle_dist | 提到大于车体外接半径 |
| 轨迹尖锐折角 | obstacle_poses_affected | 从 25 往上试到 40 |
| 绕障绕太远 | inflation_dist/weight_obstacle | 各降 20%~30% |
| 被全局路径钉死 | global_plan_viapoint_sep | 加大到 0.6~0.8 |
| 轨迹跳变 | dt_ref/ 位姿数 | 加大dt_ref或缩小时域 |
| CPU 飙高 | max_number_classes/costmap_converter_rate | 各降到 3 和 5 |
| 窄通道过不去 | inflation_dist | 降到min_obstacle_dist + 0.1 |
| 频繁切换候选 | selection_cost_hysteresis | 提到 1.5 |
7. 踩过这些坑之后我的体会
TEB 最容易让人挫败的地方，是它把很多本来分离的决策塞进了一个优化问题里，于是任何一个参数动一下，整体行为都可能跟着变。这也是为什么我不建议一上来就照着别人的参数表全盘复制——每台车的质量分布、轮子打滑程度、定位噪声、costmap 参数都不一样，同一份配置在两台车上表现可以差出很远。
我自己的做法是维护一份"最小可用配置"，只保留必须调的十几个参数，其余全走默认。等基础跑稳了，再针对具体场景加参数。参数表越长，出问题时越难定位。
另外强烈建议把 RViz 的 teb marker 打开，把pose sequence、obstacle associations、via points都显示出来。很多看起来"玄学"的轨迹问题，看一眼 marker 就明白了——比如你会发现轨迹其实一直在正确的位置上，抖的是底盘的速度环；或者你会发现机器人根本没看到那个障碍物，问题在 costmap 而不是在 TEB。可视化省下的时间，比读十遍参数文档都多。
最后一点，如果条件允许，给 TEB 单独留一个跟主控制频率对齐的计时统计，记录每次computeVelocityCommands的耗时分布（不只是平均值，要看 P95、P99）。TEB 的耗时尖峰经常出现在位姿数突变的那一帧，平均耗时看起来很正常，但偶尔一次的 100 ms 抖动就足以让整个控制链条出问题。
