TEB（Time Elastic Band）局部路径规划算法详解及代码实现

1、算法原理的理解

TEB全称Time Elastic Band（时间弹性带），该方法通对全局轨迹进行修正，从而优化机器人的局部运动轨迹，属于局部路径规划。在轨迹优化过程中，该算法拥有多种优化目标，包括但不限于：整体路径长度、轨迹运行时间、与障碍物的距离、通过中间路径点以及机器人动力学、运动学以及几何约束的符合性。

它和MPC同属于优化方法，只不过TEB计算最优的轨迹，再通过计算得到最优控制量，而MPC可以理解为直接计算最优控制量；MPC使用OSPQ优化器求解，TEB使用g2o来求解。

不重复造轮子，先直接推荐几篇比较好的博客

TEB概念理解：https://www.leiphone.com/category/transportation/0TCtaBOIcFOIBN69.html

TEB参数理解：https://blog.csdn.net/weixin_44917390/article/details/107568507

TEB论文翻译：http://t.csdnimg.cn/FJIww

TEB算法理解：https://blog.csdn.net/xiekaikaibing/article/details/83417223、 https://blog.csdn.net/flztiii/article/details/121545662

TEB源码地址：https://github.com/rst-tu-dortmund/teb_local_planner

附赠自动驾驶最全的学习资料和量产经验：链接

2、源码解读

1、进行初始化：teb参数，障碍物，机器人模型，全局路径点

TebOptimalPlanner::TebOptimalPlanner(const TebConfig& cfg, ObstContainer* obstacles, RobotFootprintModelPtr robot_model, TebVisualizationPtr visual, const ViaPointContainer* via_points)
{    
  initialize(cfg, obstacles, robot_model, visual, via_points);
}

2、initOptimizer函数进行优化器构造，在函数中，首先是注册自定义的顶点和边，之后构造了一个线性方程求解器 linear_solver，求解器使用的是g2o::LinearSolverCSparse，在此求解器基础上，定义了类型为g2o::BlockSolver的 block_solver。在此基础上，又定义了LM算法的优化器g2o::OptimizationAlgorithmLevenberg，最终得到稀疏优化器 g2o::SparseOptimizer。并且定义优化器可以以多线程形式进行。

void TebOptimalPlanner::initialize(const TebConfig& cfg, ObstContainer* obstacles,
                                   RobotFootprintModelPtr robot_model,
                                   const ViaPointContainer* via_points) {
    // init optimizer (set solver and block ordering settings)
    optimizer_ = initOptimizer();

    cfg_ = &cfg;
    obstacles_ = obstacles;
    robot_model_ = robot_model;
    via_points_ = via_points;