1. TEB算法概述机器人路径规划的核心引擎在移动机器人导航领域路径规划算法直接决定了机器人在动态环境中的运动表现。Timed Elastic BandTEB算法作为一种基于优化的局部路径规划方法通过将机器人的运动轨迹建模为带有时间信息的弹性带实现了对复杂约束条件的综合处理。我首次接触TEB算法是在一个仓储AGV项目中当时需要解决叉车在狭窄通道中同时满足速度限制和动态避障的需求。TEB算法的核心思想是将路径表示为一系列带时间戳的位姿序列称为橡皮筋通过优化这些位姿的时空分布来满足多种约束条件。与传统方法相比它具有三个显著优势一是显式考虑时间维度便于直接施加速度/加速度约束二是支持动态障碍物处理三是优化框架灵活可扩展各种自定义约束。在ROS导航栈中teb_local_planner已成为替代dwa_local_planner的主流选择。2. TEB算法核心原理拆解2.1 弹性带模型与优化框架TEB将路径表示为位姿序列$P_i(x_i,y_i,\theta_i)^T$与对应时间间隔$\Delta T_i$的集合。这个弹性带需要满足几何连续性相邻位姿间的距离符合运动学约束时间合理性时间间隔与速度/加速度限制匹配避障安全所有位姿与障碍物保持最小距离优化问题可表述为 $$ \min_{P,\Delta T} \sum_{k}w_kf_k(P,\Delta T) $$ 其中$f_k$表示各类目标函数如路径长度、时间、与障碍物距离等$w_k$为对应权重。通过高斯-牛顿法或Levenberg-Marquardt等数值优化方法求解。2.2 关键约束条件实现速度约束处理// teb_local_planner/src/optimal_planner.cpp for (int i0; i teb_.sizePoses()-1; i) { Eigen::Vector2d delta_dist teb_.Pose(i1).position() - teb_.Pose(i).position(); double delta_orient g2o::normalize_theta(teb_.Pose(i1).theta() - teb_.Pose(i).theta()); double delta_time teb_.TimeDiff(i); // 线速度约束 if (delta_time 0) { double vel delta_dist.norm() / delta_time; cost penaltyBoundFromBelow(vel, 0, cfg_-robot.max_vel_x); } // 角速度约束 if (delta_time 0 delta_dist.norm() 0.1) { // 忽略微小位移时的旋转 double omega delta_orient / delta_time; cost penaltyBoundFromBelow(omega, -cfg_-robot.max_vel_theta, cfg_-robot.max_vel_theta); } }避障策略实现计算位姿到障碍物的最小距离将障碍物膨胀为机器人轮廓的Minkowski和在优化目标中添加指数型惩罚项 $$ f_{obs} \sum_i \exp\left(-\frac{d_i}{\sigma}\right) $$ 其中$d_i$为位姿i到最近障碍物的距离$\sigma$控制惩罚强度3. 工程实现关键点3.1 ROS中的参数配置实践在teb_local_planner_params.yaml中关键参数组包括参数类别关键参数典型值范围调节建议轨迹优化weight_optimaltime0.1-5.0值越大时间优化越强weight_obstacle10-100动态环境下建议较高值机器人约束max_vel_x0.5-2.0 m/s根据电机性能设置acc_lim_theta0.5-3.0 rad/s²影响转弯流畅度障碍物处理min_obstacle_dist0.2-0.5 m需大于机器人物理半径inflation_dist0.1-0.3 m动态障碍物建议较大值调试经验建议先固定障碍物权重从较低的速度限制开始测试逐步提高速度参数直至出现轨迹震荡然后回退10%作为安全阈值。3.2 自定义约束扩展方法在工业场景中经常需要添加特殊约束例如// 添加通过特定路径点的约束 for (const auto wp : waypoints) { auto edge boost::make_sharedEdgeViaPoint(); edge-setVertex(0, teb_.vertex(i)); // 关联到最近的轨迹点 edge-setInformation(Eigen::Matrix2d::Identity()); edge-setParameters(wp.position(), cfg_-optim.weight_viapoint); optimizer_-addEdge(edge); } // 添加朝向约束如叉车需要保持与货架平行 auto edge_ori boost::make_sharedEdgeOrientation(); edge_ori-setVertex(0, teb_.vertex(i)); edge_ori-setMeasurement(desired_theta); edge_ori-setInformation(Eigen::Matrixdouble,1,1::Identity()); optimizer_-addEdge(edge_ori);4. 典型问题排查指南4.1 轨迹震荡问题现象机器人频繁左右摆动前进排查步骤检查障碍物权重是否过高weight_obstacle 200易导致过激避障降低优化频率no_inner_iterations参数控制在3-5次增加轨迹点间隔dt_ref建议0.3-0.5秒确认最小转弯半径是否合理min_turning_radius应大于机器人物理极限4.2 局部最优陷阱现象机器人在U型障碍前停滞解决方案启用备选轨迹enable_homotopy_class_planningtrue增加轨迹采样数max_number_classes5-7添加人工势场辅助可通过自定义障碍物代价函数实现4.3 实时性优化技巧限制优化迭代次数max_iterations3-5使用稀疏障碍物采样obstacle_poses_affected25在RViz中关闭不必要的可视化visualize_with_time_as_colorfalse对静态地图做预膨胀处理避免在线计算Minkowski和5. 进阶应用场景5.1 多机协同路径规划通过修改障碍物代价函数实现机间避碰// 将其他机器人的预测轨迹作为动态障碍物 for (const auto robot_traj : other_robots_trajs) { auto edge boost::make_sharedEdgeDynamicObstacle(); edge-setVertex(0, teb_.vertex(i)); edge-setTrajectory(robot_traj); edge-setInformation(Eigen::Matrix2d::Identity()); optimizer_-addEdge(edge); }5.2 非完整约束增强针对拖车等复杂机构需扩展运动学模型class EdgeKinematicCarLike : public g2o::BaseBinaryEdge3, Eigen::Vector3d, VertexPose, VertexPose { virtual void computeError() { // 实现汽车模型约束|Δy|/Δx ≈ tan(θ)*Δx // 详细实现参考ompl::base::ReedsSheppStateSpace } };在实际项目中TEB算法参数需要结合具体机器人动力学特性进行精细调节。建议通过rosbag记录典型场景下的运行数据使用rqt_teb_planner插件进行离线分析。对于仓储AGV场景通常需要重点优化侧向位移惩罚weight_kinematics_nh以提升轨迹直线度而在服务机器人场景中则需要加强对人腿等细长障碍物的检测处理。