TEB算法解析：移动机器人动态路径规划实战指南

1. 移动机器人导航中的路径规划挑战

在移动机器人自主导航领域，路径规划算法直接决定了机器人的运动能力和场景适应性。传统全局规划器虽然能给出起点到终点的最优路径，但在动态环境中往往会遇到突发障碍物、行人穿行等复杂情况。这时就需要局部路径规划器实时调整轨迹，而Timed Elastic Band（TEB）算法正是为解决这类问题而生的经典方案。

我第一次接触TEB是在一个仓储AGV项目中，当时机器人经常在货架窄道中发生"卡死"现象。全局规划生成的路径虽然理论最优，但遇到临时堆放的货箱时，机器人要么急停等待人工干预，要么尝试绕行却导致轨迹震荡。引入TEB后，系统能够平滑地调整原路径的时空特性，像橡皮筋一样弹性地绕过障碍物，同时保持运动效率和安全性。

2. TEB算法核心原理拆解

2.1 弹性带模型与时空优化

TEB的核心思想是将机器人运动轨迹建模为一条由连续位姿点组成的弹性带（Elastic Band）。每个位姿点包含位置(x,y)和朝向θ信息，相邻点之间通过时间间隔Δt连接。这种表示方法巧妙地将路径规划问题转化为带时间约束的位姿序列优化问题。

与传统方法不同，TEB在优化目标中同时考虑：

• 路径长度最短（效率性）
• 与障碍物距离最大（安全性）
• 速度/加速度平滑（舒适性）
• 满足机器人运动学约束（可行性）

通过调节各目标的权重系数，可以实现不同场景下的优化侧重。例如在人群密集区域调高安全性权重，在空旷区域则侧重运动效率。

2.2 基于图优化的求解框架

TEB采用图优化（g2o）库进行数值求解，将每个位姿点及其约束转化为图中的顶点和边。具体实现时：

1. 顶点类型：

   • 位姿顶点：存储(x,y,θ)
   • 时间顶点：存储Δt

2. 边约束类型：

   • 动力学约束：连接连续位姿点，确保符合最大速度/加速度
   • 障碍物约束：计算位姿点与最近障碍物的距离
   • 路径平滑约束：最小化连续位姿间的变化率
   • 目标吸引约束：引导轨迹趋向终点

优化过程通过不断调整顶点位置，使所有边的误差项总和最小化。这种基于图优化的方法计算效率高，适合实时应用。

3. TEB参数配置实战指南

3.1 关键性能参数解析

在ROS的teb_local_planner包中，以下参数对性能影响最大：

yaml复制# 轨迹生成
max_vel_x: 0.4       # 最大线速度(m/s)
max_vel_theta: 0.3   # 最大角速度(rad/s)
acc_lim_x: 0.2       # 线加速度限制(m/s²)
acc_lim_theta: 0.1   # 角加速度限制(rad/s²)

# 优化权重
weight_kinematics: 500  # 运动学约束权重
weight_obstacle: 100    # 障碍物避让权重 
weight_velocity: 50     # 速度平滑权重

# 弹性带配置
dt_ref: 0.3          # 期望时间间隔(s)
dt_hysteresis: 0.1   # 时间间隔允许波动范围
min_samples: 3       # 最小位姿点数

实际调试时建议采用"增量测试法"：先设置保守参数确保安全，再逐步提高性能限制。例如先设定max_vel_x为理论值的50%，确认无碰撞后再以10%步长递增。

3.2 典型场景参数模板

场景1：狭窄通道导航

yaml复制weight_obstacle: 200  # 提高避障优先级
min_obstacle_dist: 0.15 # 缩小最小允许距离
max_vel_x: 0.3        # 降低最大速度

场景2：动态避障

yaml复制obstacle_poses_affected: 20 # 增加考虑的障碍物点数
penalty_epsilon: 0.05      # 减小障碍物惩罚起始距离

场景3：高速巡航

yaml复制weight_velocity: 100      # 提高速度平滑权重
max_vel_x: 1.0           # 提高速度上限
dt_ref: 0.2              # 减小时间间隔提升控制频率

4. 工程实践中的问题排查

4.1 常见故障模式与解决方案

4.2 实时调试技巧

1. 可视化诊断：启用RViz的TEB插件，观察：

   • 绿色箭头：当前优化轨迹
   • 红色区域：障碍物影响范围
   • 蓝色连线：速度方向指示

2. 性能监控：

   bash复制rostopic echo /move_base/TebLocalPlannerROS/teb_markers
   

   查看优化迭代次数和计算耗时，正常应小于50ms

3. 动态调参工具：

   bash复制rosrun rqt_reconfigure rqt_reconfigure
   

   实时修改参数并观察响应，特别适合测试加速度参数

5. 进阶优化方向

5.1 多目标点协同优化

对于需要连续通过多个航点的任务（如巡逻路线），标准TEB可能产生不必要的停顿。可通过以下改进实现流畅过渡：

1. 将中间航点设为"软约束"，允许一定距离偏差
2. 采用预测控制机制，提前3-5个位姿点开始减速
3. 添加航点方向约束，确保以正确朝向通过

5.2 非完整约束增强

差速驱动机器人存在最小转弯半径限制，原始TEB对此处理较简单。改进方法包括：

1. 在优化目标中添加曲率约束项：

   cpp复制addEdgeCurvature(const Eigen::Vector2d& meas, double weight)
   

2. 采用圆弧段连接连续位姿点
3. 预筛选不可行位姿组合，减少优化计算量

5.3 动态障碍物预测

标准TEB将动态障碍物视为静态处理，可通过以下方式提升：

1. 集成Kalman滤波器预测障碍物运动轨迹
2. 在优化目标中添加时间维度避让约束
3. 建立速度障碍物模型（Velocity Obstacle）

在实际AGV项目中，我们通过融合TEB与DWA（Dynamic Window Approach），使机器人能够预判行人运动趋势，提前调整轨迹。这种混合策略将碰撞率降低了62%，同时保持90%以上的任务完成率。