TEB算法核心：从超图构建到轨迹优化的源码解析
1. TEB算法基础概念与核心思想
TEB（Time 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的配置与初始化
g2o（General Graph Optimization）是TEB算法使用的优化框架，它能够高效地求解超图优化问题。在TEB中，g2o的初始化包括以下几个关键步骤：
选择线性方程求解器：TEB通常使用CSparse求解器，因为它对稀疏矩阵有很好的处理能力。
配置BlockSolver：这决定了如何分解和求解优化问题的各个块。
选择迭代策略：TEB使用Levenberg-Marquardt（LM）算法，因为它在非线性优化问题中表现稳定。
// 创建g2o优化器 auto optimizer = std::make_unique<g2o::SparseOptimizer>(); // 设置线性求解器 auto linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>>(); // 设置BlockSolver auto blockSolver = g2o::make_unique<g2o::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对动态障碍物的处理能力有限。可以考虑降低规划频率，或者结合其他传感器数据进行障碍物运动预测。