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参数，障碍物，机器人模型，全局路径点

cpp 复制代码

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。并且定义优化器可以以多线程形式进行。

cpp 复制代码

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;
}

cpp 复制代码

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(block_solver));

    optimizer->setAlgorithm(solver);

    optimizer->initMultiThreading(); // required for >Eigen 3.1

    return optimizer;
}

3、registerG2OTypes函数中注册顶点和边

cpp 复制代码

void TebOptimalPlanner::registerG2OTypes() {
    g2o::Factory* factory = g2o::Factory::instance();
    factory->registerType("VERTEX_POSE",
                          std::make_shared<g2o::HyperGraphElementCreator<VertexPose>>());
    factory->registerType("VERTEX_TIMEDIFF",
                          std::make_shared<g2o::HyperGraphElementCreator<VertexTimeDiff>>());

    factory->registerType("EDGE_TIME_OPTIMAL",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeTimeOptimal>>());
    factory->registerType("EDGE_SHORTEST_PATH",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeShortestPath>>());
    factory->registerType("EDGE_VELOCITY",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeVelocity>>());
    factory->registerType("EDGE_VELOCITY_HOLONOMIC",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeVelocityHolonomic>>());
    factory->registerType("EDGE_ACCELERATION",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeAcceleration>>());
    factory->registerType("EDGE_ACCELERATION_START",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationStart>>());
    factory->registerType("EDGE_ACCELERATION_GOAL",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationGoal>>());
    factory->registerType(
        "EDGE_ACCELERATION_HOLONOMIC",
        std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomic>>());
    factory->registerType(
        "EDGE_ACCELERATION_HOLONOMIC_START",
        std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicStart>>());
    factory->registerType(
        "EDGE_ACCELERATION_HOLONOMIC_GOAL",
        std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicGoal>>());
    factory->registerType(
        "EDGE_KINEMATICS_DIFF_DRIVE",
        std::make_shared<g2o::HyperGraphElementCreator<EdgeKinematicsDiffDrive>>());
    factory->registerType("EDGE_KINEMATICS_CARLIKE",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeKinematicsCarlike>>());
    factory->registerType("EDGE_OBSTACLE",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeObstacle>>());
    factory->registerType("EDGE_INFLATED_OBSTACLE",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeInflatedObstacle>>());
    factory->registerType("EDGE_DYNAMIC_OBSTACLE",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeDynamicObstacle>>());
    factory->registerType("EDGE_VIA_POINT",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgeViaPoint>>());
    factory->registerType("EDGE_PREFER_ROTDIR",
                          std::make_shared<g2o::HyperGraphElementCreator<EdgePreferRotDir>>());
}

4、规划函数plan。先判断teb是否初始化完成，如果没有初始化，则进行初始化， initTrajectoryToGoal功能是生成从起点到终点的连线，并在连线上进行采样，填充teb中的顶点。如果已经初始化过了，则判断teb_中顶点数量是否为0，如果不为0且终点没有发生巨大变化，则使用updateAndPruneTEB函数添加顶点，反之则重新初始化顶点。最后调用优化函数optimizeTEB进行优化。

cpp 复制代码

bool TebOptimalPlanner::plan(const std::vector<PoseStamped>& initial_plan, const Twist* start_vel,
                             bool free_goal_vel) {
    if (!teb_.isInit()) {
        teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
                                  cfg_->trajectory.global_plan_overwrite_orientation,
                                  cfg_->trajectory.min_samples,
                                  cfg_->trajectory.allow_init_with_backwards_motion);
    } else // warm start
    {
        PoseSE2 start_(initial_plan.front().pose);
        PoseSE2 goal_(initial_plan.back().pose);
        if (teb_.sizePoses() > 0 &&
            (goal_.position() - teb_.BackPose().position()).norm() <
                cfg_->trajectory.force_reinit_new_goal_dist &&
            fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) <
                cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start!
            teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB
        else // goal too far away -> reinit
        {
            printf(
                "New goal: distance to existing goal is higher than the specified threshold. "
                "Reinitalizing trajectories.");
            teb_.clearTimedElasticBand();
            teb_.initTrajectoryToGoal(
                initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
                cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples,
                cfg_->trajectory.allow_init_with_backwards_motion);
        }
    }
    if (start_vel) setVelocityStart(*start_vel);
    if (free_goal_vel)
        setVelocityGoalFree();
    else
        vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be
                                // zero if nothing was modified)

    // now optimize
    return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
}

5、optimizeTEB函数中构建图，具体过程可阅读源码

cpp 复制代码

bool TebOptimalPlanner::buildGraph(double weight_multiplier) {
    if (!optimizer_->edges().empty() || !optimizer_->vertices().empty()) {
        // ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!");
        return false;
    }

    // add TEB vertices
    AddTEBVertices();

    // add Edges (local cost functions)
    if (cfg_->obstacles.legacy_obstacle_association)
        AddEdgesObstaclesLegacy(weight_multiplier);
    else
        AddEdgesObstacles(weight_multiplier);

    if (cfg_->obstacles.include_dynamic_obstacles) AddEdgesDynamicObstacles();

    AddEdgesViaPoints();

    AddEdgesVelocity();

    AddEdgesAcceleration();

    AddEdgesTimeOptimal();

    AddEdgesShortestPath();

    if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
        AddEdgesKinematicsDiffDrive(); // we have a differential drive robot
    else
        AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded
                                     // from below.

    AddEdgesPreferRotDir();

    return true;
}

6、图的构建完成后就对图进行优化，函数 optimizeGraph

先设置优化器属性，然后进行迭代

cpp 复制代码

bool TebOptimalPlanner::optimizeGraph(int no_iterations, bool clear_after) {
    if (cfg_->robot.max_vel_x < 0.01) {
        // ROS_WARN("optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. Optimizing
        // aborted...");
        if (clear_after) clearGraph();
        return false;
    }

    if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples) {
        // ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping
        // optimization.");
        if (clear_after) clearGraph();
        return false;
    }

    optimizer_->setVerbose(cfg_->optim.optimization_verbose);
    optimizer_->initializeOptimization();

    int iter = optimizer_->optimize(no_iterations);

    // Save Hessian for visualization
    //  g2o::OptimizationAlgorithmLevenberg* lm = dynamic_cast<g2o::OptimizationAlgorithmLevenberg*>
    //  (optimizer_->solver()); lm->solver()->saveHessian("~/MasterThesis/Matlab/Hessian.txt");

    if (!iter) {
        // ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter);
        return false;
    }

    if (clear_after) clearGraph();

    return true;
}

7、优化成功后，更新目标函数权重。组后优化完成的轨迹保存在变量teb_中。

cpp 复制代码

        success = optimizeGraph(iterations_innerloop, false);
        if (!success) {
            clearGraph();
            return false;
        }
        optimized_ = true;

        if (compute_cost_afterwards &&
            i == iterations_outerloop - 1) // compute cost vec only in the last iteration
            computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
        clearGraph();
        weight_multiplier *= cfg_->optim.weight_adapt_factor;

8、调用getVelocityCommand函数获取控制指令

cpp 复制代码

bool TebOptimalPlanner::getVelocityCommand(float& vx, float& vy, float& omega,
                                           int look_ahead_poses) const {
    if (teb_.sizePoses() < 2) {
        // ROS_ERROR("TebOptimalPlanner::getVelocityCommand(): The trajectory contains less than 2
        // poses. Make sure to init and optimize/plan the trajectory fist.");
        vx = 0;
        vy = 0;
        omega = 0;
        return false;
    }
    look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1));
    double dt = 0.0;
    for (int counter = 0; counter < look_ahead_poses; ++counter) {
        dt += teb_.TimeDiff(counter);
        if (dt >= cfg_->trajectory.dt_ref *
                      look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory?
        {
            look_ahead_poses = counter + 1;
            break;
        }
    }
    if (dt <= 0) {
        // ROS_ERROR("TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid!");
        vx = 0;
        vy = 0;
        omega = 0;
        return false;
    }
    // Get velocity from the first two configurations
    extractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega);
    return true;
}

至此，结束。

三、总结

1、优化的局部轨迹越长，TEB耗时越长，如果太短，容易陷入局部最优。全局规划器和局部规划器的配合才是完美的解决途径。

2、相比于DWA，TEB的优势是优化轨迹，机器人运动更加丝滑，这都是以庞大的计算作为代价的。

3、TEB的参数很多，实际工程应用需要对每个参数有深刻的理解。

4、花了很多的精力在源码的阅读及剥离ROS上。

5、对于优化器的理解还不透彻，底层是数学问题。