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

一、题外话

经济的持续低迷让一线打工者们情绪焦虑、对未来丧失信心，导致保守消费；企业端也是想着降本增效，裁员收缩。而在主流经济界有两种拉动经济的方式，第一是通过生产拉动经济、第二是通过消费拉动经济，毫无疑问我国的现状更偏向于前者，汽车行业的卷就是最好的印证，但无论如何，提升国民信心才是当务之急。

提升个人信心的最好方式就是 终身学习，提升自己的眼界，锻炼极强的适应能力。

先看效果：前面白色的线条就是需要优化的局部全局路径

TEB演示demo

二、源码解读

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;
    cost_ = HUGE_VAL;
    prefer_rotdir_ = RotType::none;

    vel_start_.first = true;
    vel_start_.second.linear.x() = 0;
    vel_start_.second.linear.y() = 0;
    vel_start_.second.angular.z() = 0;

    vel_goal_.first = true;
    vel_goal_.second.linear.x() = 0;
    vel_goal_.second.linear.y() = 0;
    vel_goal_.second.angular.z() = 0;
    initialized_ = true;
}

boost::shared_ptr<g2o::SparseOptimizer> TebOptimalPlanner::initOptimizer() {
   
    // Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)
    static boost::once_flag flag = BOOST_ONCE_INIT;
    boost::call_once(&registerG2OTypes, flag);

    // allocating the optimizer
    boost::shared_ptr<g2o::SparseOptimizer> optimizer = boost::make_shared<g2o::SparseOptimizer>();
    std::unique_ptr<TEBLinearSolver> linear_solver(
        new TEBLinearSolver()); // see typedef in optimization.h
    linear_solver->setBlockOrdering(true);
    std::unique_ptr<TEBBlockSolver> block_solver(new TEBBlockSolver(std::move(linear_solver)));
    g2o::OptimizationAlgorithmLevenberg* solver =
        new g2o::OptimizationAlgorithmLevenberg(std::move