局部运动规划器TEBTimed Elastic Band在ROS里是个很特殊的存在它不是靠采样、打分、挑最优那种试出来的规划方式而是把一条由全局规划器给出的粗路径连同时间信息一起当成一根有弹性的橡皮筋用图优化一次性揉成一条满足运动学、避障和时间要求的可执行轨迹。很多人第一次接触它是被它优雅的建模思路吸引真正用起来以后又会被那一长串权重参数和偶尔冒出来的原地抖贴墙走规划不出来折磨得够呛。我对TEB的理解是在几个不同地盘和底盘上反复调、反复翻车之后慢慢拼起来的这篇就按我自己的理解顺序拆开讲从它到底在算什么到参数之间的拉扯关系再到几类典型症状的排查链路。适合已经能把TEB跑起来、但还不清楚它为什么这么动的人看也适合刚接手一个别人调好的参数文件、不敢乱改的人看。1. TEB不是在找路而是在把一条粗线揉成可执行的轨迹先说清楚一件事TEB不负责发现新路线。它的输入是一条全局路径通常是A*或Dijkstra在一张静态地图上算出来的折线它的输出是一条位姿序列加上相邻位姿之间的时间间隔。它做的工作是在这条折线附近做局部变形让结果满足机器人自身的运动学限制、满足与障碍物的最小距离、同时尽量短、尽量快。这个定位搞明白了后面所有参数你都能知道它在影响哪一部分。1.1 弹性带的数学形态位姿序列加时间间隔TEB把轨迹描述成两串东西一串二维位姿每个位姿是x、y、yaw以及一串时间间隔ΔT。位姿序列决定走哪儿时间间隔决定走多快。这两串量被放进同一个优化图里一起求解这就是它和普通路径规划最大的区别——它从一开始就是时空联合优化的不是先算路径再算速度。把时间间隔也当变量好处很直接在障碍物密集的地方优化器会自动把对应的ΔT拉长减速在空旷直道上把ΔT压短加速不需要你手写一个速度规划器来插值。代价是优化维度变高了而且时间变量没有上界约束的时候容易往奇怪的方向跑所以后面才会有一堆跟速度、加速度相关的硬约束边。我记得第一次看它的可视化输出看到轨迹点上标着每个点的速度大小还以为是后处理算出来的。实际上那些速度就是从相邻两个位姿的距离除以ΔT得到的是优化结果的一部分。1.2 优化器里到底放了哪些边理解TEB最有效的方式是把它的超图hyper-graph在脑子里过一遍。图里的节点是位姿和时间间隔边就是各个代价项和约束项。我按自己的理解分三类类别典型的边作用属于软约束代价障碍物距离、膨胀层、路径长度、总时间、途经点、动态障碍物优先生成更好的轨迹可调权重硬约束不等式最大速度、最大角速度、最大线加速度、最大角加速度、非完整约束、转向半径不许越界权重极大或直接判不可行结构约束相邻位姿之间的连接、首尾位姿固定保证轨迹连续性不可调软约束之间是可以互相让步的比如路径长度想短、障碍物想远两者冲突时谁权重高听谁的。硬约束是要么满足要么这条轨迹废掉这就是为什么你的acc_lim_x设得太小、而max_vel_x设得太大的时候规划器会直接告诉你找不到可行解——不是它不想是这个组合在数学上就是矛盾的。1.3 为什么选图优化而不是采样DWA那类方法是在速度空间里采样一堆v, ω组合往前推演一段打分选最好的。它简单、实时性好但缺点是短视只看一两秒后的结果容易掉进局部极小值走那种贴着障碍物扭来扭去的路。TEB的做法是把整条局部轨迹一起优化所以它看得更远——理论上它能为了绕开后面一个障碍物在前面就提前偏出去。这也是它在地盘窄、障碍物密集的场景里表现明显优于DWA的原因。但代价也很实在图优化的计算量跟轨迹点数成正比跟迭代次数成正比如果局部costmap开很大、全局路径截得很长单次规划耗时会从几毫秒涨到几十毫秒。我见过一次局部规划占了控制器周期一大半的情况查下来就是max_samples设太大加上同伦类规划开着等于每次都在算四五条路径。2. 五个代价项的真实分工与相互拉扯参数文件里那一排weight_*不是各自独立的旋钮它们是一组跷跷板。你动了一个另外几个的效果就会变。我按实际影响从大到小讲。2.1 weight_obstacle 和 weight_inflation 是两层结构不要当一回事很多人以为weight_obstacle越大越安全于是直接拉到几百。结果机器人开始在通道里左右横跳因为它对每一条边上的障碍物都过度敏感一点点不对称就让它觉得这边更险于是往另一边躲躲过去又觉得那边险。要理解这两个参数得先理解min_obstacle_dist和inflation_dist这两个距离阈值构成的双层结构在min_obstacle_dist以内代价是惩罚函数越近涨得越快在min_obstacle_dist到inflation_dist之间是一个软性的尽量别靠太近的缓冲区超出inflation_dist基本不管。weight_obstacle控制的是第一层的陡峭程度weight_inflation控制第二层的软推力。我的经验值是min_obstacle_dist按机器人实际半宽加个5到10厘米的安全余量设inflation_dist比它大0.1到0.2米weight_obstacle保持在几十的量级weight_inflation保持很小零点几甚至更小。真正让机器人不贴墙的应该是costmap的膨胀层加上min_obstacle_dist的实际值而不是把权重怼上去让它自己害怕。2.2 weight_optimaltime 与速度约束的隐性冲突weight_optimaltime控制总时间这一项的权重。调大它轨迹会更倾向于走直线、走近路、加速通过。听起来挺好但它的效果会被速度约束卡住你把时间压得很狠可max_vel_x就那么点优化器只能把每个ΔT压到极小值这时候轨迹点之间的位移相对于时间就超速了速度约束边就会大量报错最后要么退化成不可行要么解出来的速度曲线在限幅后跟轨迹形状对不上表现出来就是轨迹是一条平滑曲线机器人实际走的是折线。我一般的做法是weight_optimaltime保持默认的1左右想快就老老实实调max_vel_x和acc_lim_x让物理约束自己决定速度上限。想让它更抄近路我更倾向动路径长度那项的权重而不是时间项。2.3 weight_kinematics_forward_drive 决定机器人愿不愿意倒车这个参数很有意思。非完整约束weight_kinematics_nh保证机器人只能沿着车头方向走但它不区分往前还是往后——倒着走同样满足非完整约束。weight_kinematics_forward_drive就是用来惩罚倒着走的。设得太小机器人在狭窄处会疯狂倒车微调看起来像在倒库视觉上很不自然而且倒车时后方的传感器盲区风险高。设得太大机器人在死胡同里就完全不愿意后退宁可原地画圈。我通常把它设到一个中等偏上的值让倒车成为不得已才用的手段同时在狭窄通道场景里适当降低一点允许它小幅度倒车调整姿态。提示这个参数和prefer_rotdir偏好旋转方向是两个东西前者管平动方向偏好后者管原地转向时更喜欢左转还是右转。调参时别混。3. 参数表里没写清楚的那几个数dt_ref、迭代次数与采样上限这几个参数在很多教程里被一句保持默认带过但它们其实是决定TEB性格的关键。默认值我按手上的版本大概记一下具体以你装的包里的参数声明为准不同分支会有出入。3.1 dt_ref 与 dt_hysteresis 决定的是轨迹的分辨率dt_ref是期望的时间间隔默认大概0.3秒。它的真正作用不是让机器人每0.3秒走一步而是控制轨迹点的疏密优化过程中如果某个ΔT超过dt_ref dt_hysteresis就在中间插一个新位姿如果短于dt_ref - dt_hysteresis就删掉一个。这是一个自适应的重采样机制。这意味着dt_ref直接影响优化变量个数进而影响计算量。设得小比如0.1轨迹点变密轨迹更平滑、避障更精细但计算量翻倍设得大比如0.5点变稀计算快但在窄通道里可能因为采样太粗而看不见障碍物之间的缝隙直接判定不可行。我的经验是小车、低速1 m/s以下、场地窄dt_ref往0.2到0.25调大车、高速、场地开阔保持0.3甚至0.4。dt_hysteresis别设得太小否则点数会频繁增减优化结果抖动也别设太大否则自适应失去意义。0.05到0.1之间比较稳妥。3.2 no_inner_iterations 与 no_outer_iterations 是在拿精度换时间TEB用的是g2o的稀疏求解器每次规划做若干轮外层迭代每轮里做若干次内层迭代。默认大概是内外各4次这个组合在大多数场景下够用。什么时候需要加当你的机器人走出来的实际轨迹明显比可视化里那条更折或者规划器经常报优化未收敛时适当加内层迭代会有改善。什么时候需要减当你的控制周期很紧比如10ms单次规划耗时吃掉了大半预算时把外层迭代降到2通常轨迹质量下降不明显但耗时能砍掉不少。这里有个容易被忽略的细节TEB对初始解很敏感。如果上一個周期的轨迹被成功复用它会把上一周期的解作为这一周期的初值收敛会快很多。如果全局路径频繁大幅变化或者局部costmap里障碍物位置一直在跳初始解质量差迭代次数不够就会解出很奇怪的形状。所以遇到偶尔抽风的情况先看是不是上游给的东西在抖。3.3 max_samples 是硬上限不是目标值max_samples限制单条轨迹最多多少个位姿默认500。它是个保险丝防止dt_ref设太小导致变量爆炸。正常情况下轨迹点数远小于这个值除非你的局部规划范围特别长。真正要警惕的是局部costmap的尺寸和全局路径截取长度。TEB优化的轨迹长度大致就是局部costmap覆盖的那一段。把局部costmap开到10米×10米dt_ref又设0.2那就是50个位姿点起步再加上同伦类规划要算好几条计算量直接上天。4. 障碍物关联TEB最容易崩掉的一环如果说TEB有哪个部分最容易出问题我认为不是权重是障碍物关联。优化器本身很健壮但把costmap里的栅格变成优化图里的障碍物边这个转换过程坑特别多。4.1 legacy 关联与 costmap_converter 的区别不装costmap_converter的时候TEB走的是legacy模式直接把局部costmap里的障碍物栅格点按一定规则关联到最近的轨迹点上。这种方式简单粗暴问题是栅格点数量可能很大而且同一个障碍物会被拆成几十个点分别关联优化的时候相当于被罚了几十次导致机器人对单个障碍物反应过度。装了costmap_converter之后栅格会被聚类成多边形或点集关联时以障碍物为单位而不是以栅格为单位。结果是对障碍物的反应更整体不会因为栅格边缘的毛刺而误判。我的建议是只要条件允许就装上尤其是场地里有柱子、桌腿这种离散小障碍物的时候差别非常明显。有一个参数值得单独提costmap_obstacles_behind_robot_dist默认大概1米意思是机器人身后1米内的障碍物也参与优化。设太大机器人会因为身后的东西而不敢前进设太小倒车时容易撞。如果你发现机器人在空旷地方也犹犹豫豫可以先查这个。4.2 footprint 模型选错前面所有调参都白搭TEB里有个参数叫footprint模型可选点模型、圆模型、线段模型、双圆模型、多边形模型。默认往往是点模型这是最省计算但也最危险的优化器认为机器人是一个质点那么它算出来的最小障碍物距离跟机器人的实际外形完全对不上。我见过最典型的翻车是差速小车用了点模型min_obstacle_dist设0.3结果车头两侧的角在转弯时蹭到了门框。查了半天以为是权重问题最后发现是footprint模型的问题。模型适用代价备注点快速验证、极小机器人最低不安全别上实车圆圆形底盘、全向底盘低min_obstacle_dist要包含半径线段窄长形底盘中适合差速小车双圆有明确前后轴的底盘中比圆模型贴合度好多边形非规则外形最高最准但计算量大换成多边形以后注意min_obstacle_dist的含义变了——它变成了多边形边界到障碍物的距离不再是中心点距离。这时候原来的数值要重新调通常会调小。4.3 动态障碍物的关联超时与剔除开启动态障碍物支持以后TEB会把上一帧关联到的障碍物保留一段时间如果这一帧没关联上就逐渐衰减剔除。这块在人多、物体移动快的场景里很容易出问题障碍物突然出现关联不上机器人反应慢半拍障碍物已经走了关联还留着机器人绕着一个不存在的东西走。我一般的处理是把动态障碍物相关的权重设得比静态障碍物低一些让它起辅助避让的作用而不是主导同时确认上游的感知频率跟得上控制频率。如果感知只有5Hz而控制器跑20Hz那动态障碍物这块基本没有意义不如关掉靠costmap的实时更新来间接处理。5. 多拓扑路径homotopy class 到底值不值得开同伦类规划是TEB比较有特色的功能它不只优化一条轨迹而是在机器人周围生成一张简化的路线图找出几条拓扑上不同的绕行方案比如从障碍物左边绕、从右边绕每条都优化一遍最后挑一个代价最低的。5.1 h-signature 是怎么区分左右绕行的它给每个障碍物一个标记然后计算轨迹环绕这些障碍物的签名。从障碍物左边绕和从右边绕签名的符号不同于是被判定为不同的同伦类。这样即使两条轨迹的几何代价很接近也能被区分开不会因为数值噪声在两者之间反复横跳。这个机制解决的是一个很实际的问题当机器人正对一根柱子时从左绕和从右绕在局部看代价几乎一样普通优化器会在这个对称点附近震荡一会儿想左一会儿想右。有了同伦类筛选它会明确选一条并保持一段时间行为上稳定得多。5.2 开着它CPU翻倍什么时候该关代价就是计算量。默认会尝试好几个类每个类都要独立优化一遍参数里那个最大类别数就是上限。我实测下来在中低算力的板子上开着同伦类规划会让单次规划耗时增加一倍以上如果控制频率高很容易拖垮整个周期。我的取舍原则场地里有大量离散柱状障碍物、且通道宽度允许左右两种走法时开着它收益明显场地是长廊、走廊、只有一条路可走时开了也是白开直接关掉省算力。另外那个类别选择迟滞参数值得调一下调大一点可以让选择更稳定减少在两条路径之间来回切换。5.3 via_points 的替代用法有时候你不想开同伦类规划但又想让机器人从障碍物的某一侧绕。这时候可以用途经点从全局路径上按一定间隔采样出若干点作为软约束让局部轨迹尽量贴合全局路径。TEB里有个参数控制这个采样间隔设成正值就启用了。这个做法的好处是计算量小坏处是它只是软吸引遇到强障碍物还是会偏出去而且如果采样点落在障碍物里面会跟避障项打架。我的用法是把它当成轻度引导间隔设在半米到一米之间权重不要太高。真正需要强制走某条路的时候还是得在全局规划层面解决靠局部规划器拧不过来。6. 现场排障五类典型症状的排查链路下面这些是我真实遇到过的按症状分类把我当时的排查顺序写出来。这个顺序很重要——先说结论没用你得知道怎么一步步缩小范围。6.1 原地摆头与低速抖动现象机器人在直道上正常一进狭窄处就开始左右小幅摆动或者原地反复转向。第一步关掉同伦类规划看还抖不抖。如果关了就好说明是多个候选路径在争抢去调类别选择的迟滞参数。第二步如果还抖把weight_kinematics_forward_drive往上加同时确认非完整约束的权重足够大。有时候是机器人被允许侧着走非完整约束太弱优化器就找到了这种不合物理的捷径。第三步检查dt_ref是不是太小、dt_hysteresis是不是也太小导致点数频繁增减。我遇到过一次就是dt_hysteresis设成了0.01优化结果每个周期点数都不一样轨迹自然抖。第四步看上游全局路径是不是在抖。全局规划器如果每次重规划给出略微不同的折线局部规划器就得跟着改表现出来也是抖。这种情况要去治全局不是调TEB。6.2 贴墙走、蹭膨胀层现象明明通道很宽机器人偏偏贴着一边走。先确认footprint模型点模型是最常见的元凶。然后看min_obstacle_dist是不是设得太小——有人为了通过窄通道把它调到接近0结果在所有地方都贴着走。再往下查就是costmap的膨胀层半径和TEB的inflation_dist是否匹配。如果costmap的膨胀层只有0.1米而TEB的inflation_dist是0.6那TEB认为的缓冲区大部分落在costmap认为的自由空间里没有实际障碍物支撑推力就失效了。最后一个容易被忽略的点weight_optimaltime过大也会导致贴墙。因为贴墙往往是最短路径时间项权重高优化器宁可贴着障碍物走直线也不愿意绕开。6.3 轨迹穿过障碍物现象可视化里轨迹明明穿过了障碍物但规划器不报错。这个一定优先查关联不要怀疑优化器。常见原因有三个一是costmap里那个位置确实没有障碍物传感器没看到、或者被过滤掉了TEB只是忠实地反映了它看到的世界二是障碍物关联的距离阈值设得太小远处的障碍物根本没被关联进来三是局部costmap的更新频率太低用的是过期数据。我遇到过一次很典型的机器人贴着玻璃走激光直接穿透costmap上一片空白TEB当然认为可以走。这种情况不是规划器的问题是感知的问题得在传感器配置或者地图层面解决。6.4 规划器报no feasible trajectory或直接停住这个错误信息的信息量其实很大它说的是在给定的约束下找不到可行解通常意味着约束之间互相矛盾。排查顺序先看速度约束和加速度约束是否自洽max_vel_x大而acc_lim_x小到根本达不到某些情况下会出问题再看min_obstacle_dist是不是大于通道实际半宽如果通道只有0.6米宽而min_obstacle_dist是0.4那机器人认为自己过不去这是正确的判断然后看可行性检查的位姿数参数它控制往前检查多少个点设太大容易误判设太小可能漏判。还有一个隐蔽的原因机器人当前位姿本身就在膨胀区里。比如它被人推着靠墙了或者地图和实际有偏差这时候所有轨迹的起点都违反约束自然找不到解。这种情况需要允许它先走出来有些实现里会有相应的恢复机制。6.5 规划周期跟不上控制周期现象控制器频率设的20Hz但实际执行频率只有8Hz机器人动作一顿一顿的。先测单次规划耗时。如果确实是TEB太慢按这个顺序砍关同伦类规划、把max_samples降下来、把局部costmap尺寸减小、把dt_ref适当调大、减少迭代次数。这几个手段的收益是递减的一般砍到前两个就够用了。另外一个常被忽略的点可视化。开着大量的Marker发布在低算力平台上会显著拖慢周期。调试的时候开跑起来的时候关掉。7. 与DWA的分工边界和工程落地细节最后说说选型。TEB不是万能的我在一些场景里会主动换回更简单的方案。7.1 什么场景应该换回DWA第一算力极其受限的嵌入式平台。TEB的图优化对CPU的要求比采样类方法高一个档次如果板子本来就紧张DWA更稳妥。第二场地极其开阔、几乎没有障碍物。这种场景下TEB的建模优势发挥不出来反而白白付出计算代价。第三机器人模型特别特殊比如某些非标的全向底盘TEB的运动学模型未必能准确描述硬套反而出问题不如用能自定义运动模型的采样法。反过来窄通道、密集障碍物、需要精细避障、需要平滑轨迹的场景TEB的优势非常明显尤其是它输出的轨迹天然带时间信息对下游的速度控制很友好。7.2 局部costmap尺寸与全局路径的配合这个是我踩过最久的一个坑。局部costmap设得太大规划慢、容易陷入局部最优因为它看到太多东西反而不知道该往哪走设得太小遇到大障碍物时会因为看不到绕行空间而卡死。我的经验是局部costmap的边长取机器人能看到至少一个完整绕行方案的最小值。走廊场景2到3米够开阔车间可能要到5米以上。另外rolling window的更新频率要跟得上不然机器人是在一张过期的地图上做优化。全局路径的截取也有讲究。如果只取全局路径最前面的一小段喂给TEB它没有足够的前瞻来做平滑如果取太长计算量又上去了。通常按局部costmap的对角线长度来截比较合适。7.3 ROS1与ROS2移植版的功能差这一点必须提醒。TEB在ROS2下的移植版本功能完整度和ROS1的原版并不一致有些高级特性比如部分动态障碍物处理、部分可视化调试工具在移植版里可能没有实现或者实现方式不同。参数文件的组织方式也变了从.yaml嵌套结构变成了通过controller插件加载。我一般的做法是拿到一个新环境的TEB包先去翻它的参数声明文件看哪些参数是真正被读取的。别照着ROS1的老教程一顿配配完发现一半参数根本没生效然后在错误的方向上查半天。提示不同分支的默认值差异不小尤其是权重类的默认值同一个参数名在ROS1和ROS2版本里可能差一个数量级。调参前先把自己环境里的默认值打印出来作为基准。我个人在实际操作中的体会是TEB这类优化型规划器最忌讳的就是头痛医头式调参。它内部各个项是耦合的你在障碍物项上加一点可能就在时间项上减了一点表现出来是另一个症状。真正有效的做法是先确认输入是干净的costmap准确、全局路径合理、footprint模型正确、上游频率稳定再去动权重。我见过的绝大多数TEB不好用的案例追到最后都不是权重问题而是输入数据的问题。