1. TEB算法基础概念与核心思想TEBTime Elastic Band算法是ROS导航栈中广泛使用的局部路径规划器它的核心思想是将机器人的运动轨迹建模为一条带有时间信息的弹性带。这条弹性带由一系列离散的位姿点组成每个位姿点不仅包含空间坐标信息还包含时间间隔信息。这种建模方式使得算法能够同时优化轨迹的空间属性和时间属性。在实际应用中TEB算法会接收全局规划器生成的初始路径然后在其基础上进行局部优化。优化的目标包括但不限于最小化轨迹执行时间、保持与障碍物的安全距离、满足机器人运动学约束等。整个过程可以形象地理解为对一条橡皮筋进行拉伸和变形使其在满足各种约束条件下达到最优状态。与传统的路径规划算法相比TEB最大的特点是考虑了时间维度。这使得它特别适合处理动态环境中的路径规划问题因为时间信息的引入让算法能够预测障碍物的运动轨迹从而提前规划避让路径。在ROS的teb_local_planner包中这一算法被实现为两种不同的规划器基础的TebOptimalPlanner和更高级的HomotopyClassPlanner。2. 超图构建从理论到代码实现2.1 超图的基本结构TEB算法将路径规划问题转化为超图优化问题。在这个超图中顶点Vertex主要包括两类机器人的位姿顶点VertexPose和时间差顶点VertexTimeDiff。位姿顶点存储机器人在某个时刻的空间位置和朝向时间差顶点则存储相邻两个位姿之间的时间间隔。在代码层面这些顶点通过特定的数据结构进行管理。以teb_local_planner中的实现为例位姿序列被存储在PoseSequence中这是一个由VertexPose指针组成的向量时间差序列则存储在TimeDiffSequence中由VertexTimeDiff指针组成。这种设计使得算法能够高效地访问和修改各个顶点。2.2 buildGraph()函数解析buildGraph()函数是TEB算法构建超图的核心函数它的主要任务是将所有顶点和约束边添加到优化问题中。具体来说这个函数会完成以下几项工作添加位姿顶点遍历PoseSequence中的所有位姿将它们作为顶点添加到g2o优化器中。起点和终点的位姿通常会被固定不允许被优化。添加时间差顶点将TimeDiffSequence中的时间间隔作为顶点添加到优化问题中。这些顶点决定了轨迹的时间属性。添加各种约束边根据不同的优化目标添加相应的约束边。常见的约束边包括避障约束边确保轨迹与障碍物保持安全距离速度约束边限制机器人的最大线速度和角速度加速度约束边限制机器人的线加速度和角加速度运动学约束边确保轨迹符合机器人的运动学特性void TebOptimalPlanner::buildGraph() { // 添加位姿顶点 for(auto pose : teb_.pose_vec_) optimizer_-addVertex(pose); // 添加时间差顶点 for(auto timediff : teb_.timediff_vec_) optimizer_-addVertex(timediff); // 添加各种约束边 addObstacleEdges(); addVelocityEdges(); addAccelerationEdges(); addKinematicsEdges(); // ...其他约束边 }3. 约束边的具体实现与优化3.1 避障约束的实现细节避障约束是TEB算法中最重要的约束之一它确保机器人的轨迹与障碍物保持安全距离。在代码实现上避障约束通过计算轨迹上每个位姿到最近障碍物的距离来实现。具体来说算法会为每个位姿顶点添加一条避障约束边。这条边的残差函数通常设计为机器人与障碍物之间的实际距离减去安全距离。如果实际距离大于安全距离残差为零否则残差为两者之差。这种设计使得优化器会尽量调整位姿的位置使得残差最小化从而保证安全距离。在实际应用中为了提高计算效率teb_local_planner会对障碍物进行预处理。例如使用costmap_converter插件将障碍物从网格地图转换为几何图元点、线、多边形等这样可以大大减少距离计算的开销。3.2 动力学约束的代码实现动力学约束主要包括速度约束和加速度约束它们确保轨迹满足机器人的动力学特性。在代码中这些约束通过相邻位姿之间的关系来实现。以速度约束为例算法会计算相邻两个位姿之间的平均速度double vel (pose2-position() - pose1-position()).norm() / timediff-dt();然后将这个速度与机器人的最大允许速度进行比较构建相应的约束边。如果计算速度超过最大速度优化器会调整位姿位置或时间间隔使得速度降低到允许范围内。加速度约束的实现类似但需要考虑三个连续的位姿和两个时间间隔。算法会计算位姿变化的二阶导数来估计加速度然后与最大允许加速度进行比较。4. g2o求解器在TEB中的应用4.1 g2o的配置与初始化g2oGeneral Graph Optimization是TEB算法使用的优化框架它能够高效地求解超图优化问题。在TEB中g2o的初始化包括以下几个关键步骤选择线性方程求解器TEB通常使用CSparse求解器因为它对稀疏矩阵有很好的处理能力。配置BlockSolver这决定了如何分解和求解优化问题的各个块。选择迭代策略TEB使用Levenberg-MarquardtLM算法因为它在非线性优化问题中表现稳定。// 创建g2o优化器 auto optimizer std::make_uniqueg2o::SparseOptimizer(); // 设置线性求解器 auto linearSolver g2o::make_uniqueg2o::LinearSolverCSparseg2o::BlockSolverX::PoseMatrixType(); // 设置BlockSolver auto blockSolver g2o::make_uniqueg2o::BlockSolverX(std::move(linearSolver)); // 设置优化算法 auto algorithm new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver)); // 将算法设置给优化器 optimizer-setAlgorithm(algorithm);4.2 优化过程与结果解析当超图构建完成后g2o求解器会开始迭代优化过程。每次迭代中求解器会计算当前所有约束边的残差即目标函数值根据残差计算梯度方向和步长调整各个顶点的值使得总残差减小检查收敛条件决定是否继续迭代优化完成后最优的位姿和时间间隔就存储在原来的PoseSequence和TimeDiffSequence中。这些结果可以直接用于生成机器人的运动命令。值得注意的是TEB的优化问题是非凸的这意味着可能存在多个局部最优解。为了应对这个问题teb_local_planner中的HomotopyClassPlanner会同时优化多条不同拓扑结构的轨迹然后选择其中最优的一条作为最终结果。5. 轨迹优化与结果应用5.1 轨迹的自动调整策略在优化过程中TEB算法会根据需要自动调整轨迹的密度。这一功能主要由autoResize()函数实现它会检查相邻位姿之间的距离和时间间隔决定是否需要插入新的位姿或删除多余的位姿。具体来说当两个位姿之间的空间距离过大时算法会插入新的位姿以确保轨迹足够平滑当时间间隔过长时也会进行分割以更精细地控制速度变化。这种动态调整策略使得TEB能够适应各种复杂的运动场景。5.2 优化结果的提取与使用优化完成后算法需要从优化器中提取结果并应用到实际控制中。这一过程主要涉及以下几个步骤从g2o优化器中获取更新后的顶点值将这些值写回到PoseSequence和TimeDiffSequence中计算轨迹的各种性能指标如总时间、总长度、最小障碍物距离等根据前几个位姿和时间间隔计算机器人的当前运动命令// 从优化器提取结果 optimizer_-updateEstimate(); // 计算轨迹成本 computeCurrentCost(); // 计算机器人速度 double v (teb_.Pose(1).position() - teb_.Pose(0).position()).norm() / teb_.TimeDiff(0); double omega (teb_.Pose(1).theta() - teb_.Pose(0).theta()) / teb_.TimeDiff(0);在实际应用中这些运动命令会被发送到机器人的底层控制器驱动机器人沿着优化后的轨迹运动。同时优化后的轨迹也会被可视化工具如RViz显示出来方便开发者调试和验证。6. 实际应用中的注意事项6.1 参数调优经验分享TEB算法的性能很大程度上取决于各种参数的设置。根据实际项目经验有几个关键参数需要特别注意障碍物距离参数min_obstacle_dist这个值设置得太小可能导致机器人与障碍物碰撞设置得太大则可能使机器人在狭窄空间无法找到可行路径。通常建议从机器人半径的1.2倍开始调整。速度加速度限制这些参数应该根据机器人的实际性能设置过于保守会降低运动效率过于激进则可能导致控制不稳定。轨迹分辨率dt_ref这个参数影响轨迹的精细程度。值越小轨迹越平滑但计算量也越大。通常设置在0.1-0.3秒之间比较合适。优化迭代次数no_inner_iterations/no_outer_iterations增加迭代次数可以提高优化质量但也会增加计算时间。需要根据实际硬件性能进行权衡。6.2 常见问题与解决方案在实际使用TEB算法时可能会遇到一些典型问题轨迹震荡问题表现为机器人在运动过程中来回摆动。这通常是由于障碍物权重设置过高或优化迭代次数不足导致的。可以尝试降低障碍物权重或增加迭代次数。狭窄通道通过困难当通道宽度接近机器人直径时TEB可能无法找到可行路径。这时可以适当减小min_obstacle_dist或者启用HomotopyClassPlanner尝试多条路径。计算耗时过长如果优化过程耗时太多可以尝试减小轨迹长度通过max_global_plan_lookahead_dist参数、降低轨迹分辨率或减少优化迭代次数。动态障碍物处理不佳TEB对动态障碍物的处理能力有限。可以考虑降低规划频率或者结合其他传感器数据进行障碍物运动预测。