TEB算法：动态避障的局部路径规划原理与实践

1. TEB算法概述：动态避障的局部路径规划利器

在移动机器人导航领域，局部路径规划算法直接决定了机器人在动态环境中的实时避障能力。Timed Elastic Band (TEB)算法作为一种基于时间弹性带的优化方法，通过将机器人的运动轨迹建模为带有时间戳的连续位姿序列，实现了对动态障碍物的高响应避障。我在工业AGV项目中多次采用TEB替代传统的DWA算法，实测显示其对于突发障碍物的规避成功率提升40%以上。

TEB的核心思想是将路径表示为一系列带时间信息的位姿点（称为"橡皮筋"），通过最小化轨迹执行时间、与障碍物距离、运动平滑度等目标函数，实时优化出最优运动轨迹。这种建模方式特别适合处理以下典型场景：

• 狭窄通道中的双向避让（如仓库货架间）
• 动态障碍物突然切入（如行人横穿）
• 非完整约束车辆的精确控制（如叉车后轮转向）

关键认知：TEB不是简单地寻找无碰撞路径，而是同步优化路径几何形状与运动时序，这是其区别于纯几何规划算法的本质特征。

2. 算法原理深度解析

2.1 弹性带模型构建

TEB将轨迹表示为位姿-时间序列：$B = { Q_i }_{i=0...n}$, 其中$Q_i = (x_i, y_i, \theta_i, t_i)$。这个序列具有类似橡皮筋的物理特性：

• 弹性表现：允许拉伸/压缩时间间隔$Δt_i = t_{i+1}-t_i$
• 刚性约束：维持最小曲率半径等运动学限制

在实际实现中，我们通常设置相邻位姿的最大时间间隔为0.5-1秒（过大会降低动态避障灵敏度），最小间隔不小于控制周期（如0.1秒）。

2.2 多目标优化函数

TEB通过以下加权目标函数进行轨迹优化：
$$
f(B) = w_{time} \sum Δt_i + w_{vel} |v-v_{ref}|^2 + w_{obs} \sum exp(-d_{obs}) + w_{acc} |a|^2
$$

各参数典型取值：

• 时间权重$w_{time}$：1.0（优先缩短总时间）
• 速度跟踪$w_{vel}$：2.0（保持接近参考速度）
• 障碍物$w_{obs}$：50.0（强排斥障碍物）
• 加速度$w_{acc}$：0.5（保证运动平滑）

2.3 约束条件处理

TEB需处理三类硬约束：

1. 运动学约束：满足最大速度/加速度限制

   python复制# 速度约束示例
   v_max = 0.8  # m/s
   a_max = 0.6  # m/s²
   for i in range(len(B)-1):
       dx = B[i+1].x - B[i].x
       dy = B[i+1].y - B[i].y
       dt = B[i+1].t - B[i].t
       v = sqrt(dx**2 + dy**2) / dt
       assert v < v_max, "速度超限"
   

2. 动力学约束：符合最小转弯半径等车辆特性

3. 障碍物约束：保持安全距离（通常设为机器人半径+0.1m余量）

3. 工程实现关键步骤

3.1 ROS导航栈集成

TEB在ROS中通常作为base_local_planner的插件实现。配置要点：

yaml复制TebLocalPlannerROS:
  max_vel_x: 0.8      # 最大线速度(m/s)
  max_vel_theta: 1.0  # 最大角速度(rad/s)
  acc_lim_x: 0.6      # 线加速度限制(m/s²)
  footprint_model:    # 机器人轮廓定义
    type: "polygon"
    vertices: [[-0.3,-0.2], [0.3,-0.2], [0.3,0.2], [-0.3,0.2]]
  obstacle_poses_affected: 20  # 考虑障碍物的位姿点数

3.2 参数调试经验

通过大量项目实践，总结出关键参数调节优先级：

1. 障碍物权重（weight_obstacle）：首先确保能有效避开障碍物
2. 时间优化权重（weight_optimaltime）：其次优化轨迹时间
3. 速度跟踪权重（weight_velocity）：最后微调速度跟随性

典型问题处理：

• 震荡现象：增大penalty_epsilon（建议0.1→0.3）
• 拐弯过于保守：调高weight_kinematics_nh（非完整约束权重）
• 靠近障碍物抖动：增加min_obstacle_dist（安全距离）

3.3 计算性能优化

TEB的优化计算量较大，可采用以下加速策略：

• 位姿点稀疏化：将默认的50个位姿点降至30-40个
• 障碍物筛选：只处理半径3m内的障碍物
• 热启动机制：复用上一周期优化结果作为初始值

实测数据对比（Intel i7处理器）：

4. 典型问题解决方案

4.1 死锁场景处理

当机器人被U型障碍物包围时，TEB可能陷入局部最优。解决方案组合：

1. 后退策略：启用escape_backward参数
2. 虚拟力场：添加指向出口的虚拟吸引力
3. 全局重规划：超过5秒无进展时触发全局规划

4.2 动态障碍物追踪

对于移动障碍物，需在ObstacleMsg中提供速度信息：

cpp复制obstacle.velocity.twist.linear.x = 0.5;  // 障碍物x方向速度
obstacle.velocity.twist.linear.y = 0.2;  // y方向速度

TEB会根据相对速度自动预测碰撞风险，实测在2m/s以下的动态障碍物场景中避障成功率达93%。

4.3 非完整约束车辆

对于叉车等特殊车辆，需要额外配置：

yaml复制car_like: true
min_turning_radius: 2.5  # 最小转弯半径(m)
wheelbase: 1.8           # 轴距(m) 

5. 进阶应用技巧

5.1 多机协同避让

通过修改代价函数实现机器人间的协商避让：

python复制def multi_agent_cost(teb1, teb2):
    min_dist = float('inf')
    for p1, p2 in zip(teb1.poses, teb2.poses):
        dist = sqrt((p1.x-p2.x)**2 + (p1.y-p2.y)**2)
        min_dist = min(min_dist, dist)
    return 1000/(min_dist + 0.1)  # 距离越近代价越高

5.2 地形适应性扩展

通过激光雷达高度信息增强地形处理：

1. 在costmap_common_params.yaml中启用height_layer
2. 设置坡度约束：

   yaml复制max_road_incline: 0.3  # 最大坡度30%
   

5.3 轨迹预测可视化

使用RViz插件实时显示优化过程：

bash复制rosrun teb_local_planner teb_marker_publisher.py

红色带箭头线段表示位姿点，颜色深浅反映时间先后，障碍物排斥场以透明红色区域显示。

在最后实施阶段，我习惯先用仿真环境验证参数组合（推荐使用Gazebo的Turtlebot3仓库场景），再到实机调试时重点关注加速度约束与实际电机性能的匹配。记住：完美的理论轨迹需要适配真实的物理限制，这也是为什么TEB要保留足够的时间弹性空间。