深入理解轨迹优化器TEB算法—从零开始使用C++实现轨迹优化器【附Github仓库代码链接】

机器人规划与控制研究所 ——机器人/自动驾驶规划与控制方向综合、全面、专业的平台。4万人订阅的微信大号。点击标题下蓝字“机器人规划与控制研究所”关注，我们将为您提供有价值、有深度的延伸阅读。

作者序

今天是周六，现在是北京时间23:19:00，经过7个小时编码和调试，刚将代码上传到我的Github仓库，链接：

https://github.com/JackJu-HIT/TrajectoryOptimizeMethod。

上周，我写了第一个深入理解轨迹优化器TEB算法：深入解析轨迹优化（TEB）算法中非线性求解器的 C++ 框架的实现。这篇文章，主要是介绍了TEB算法的g2o框架是如何编写的，如果没有读过的朋友，可以先去读这篇文章。写这篇文章的原因是，我觉得上一篇文章的内容不够透彻，读完后依然觉得理解不深刻。

考虑于此，于是我今天在上一篇文章的基础上，又写了一个demo，这个demo主要是实现跟踪轨迹点和绕障碍物的曲线生成，即写了 EdgeViaPointConstraint约束和EdgeObstacleConstraint这两个约束，熟悉TEB调参的朋友应该知道，EdgeViaPointConstraint约束是保证生成的轨迹尽可能逼近参考轨迹，而障碍物EdgeObstacleConstraint约束是尽快可能让生成的轨迹绕开障碍物。这两个权重在调参中比较重要，有些场景需要严格跟线，而有些场景需要绕开障碍物，这就需要调整这两个权重了。

我想，如果结合本文以及我github仓库开源TrajectoryOptimizeMethod的代码，读懂了以后，你应该对TEB算法有一个深刻的理解，后续可以在这个TEB框架上进行改进优化，抑或参考TEB算法自己再写一个轨迹优化器算法，或者数学水平不错的话，还可以做些算法创新。

好了，说了这么多废话，大半夜码字，总是性情中人！

喝杯酒，再点一根烟，抖音循环播放着好听的背景音乐，便是我的此时此刻！

对了，我写的这个demo有个缺陷，目前生成的轨迹没有考虑平滑约束，也没有考虑转弯半径，所以出来的曲线不会平滑，大家如若有兴趣可以在我的开源代码基础上加入时间间隔，把TEB的里面其它约束写进来。

下面是仿真结果图：

上图，是考虑EdgeViaPointConstraint和EdgeObstacleConstraint这两个约束的生成的曲线，其中ref_traj模拟的是全局规划器发过来的轨迹，planTraj是写的轨迹优化器TrajectoryOptimizeMethod优化后的结果，那两个黑点是模拟的障碍物信息，障碍物最小安全距离设置为1米。

另外，如果你想传入多边形障碍物抑或是线型的障碍物，这个接口没写，其实就是算个距离，可以参考TEB里的处理方式。

本文主要讲解https://github.com/JackJu-HIT/TrajectoryOptimizeMethod的代码思路。

下面我们开始正文吧。

因个人水平有限，如有错误，欢迎指出。

2025年9月14日 01:00:00

Jack Ju 于沈阳

1 代码框架

代码的目录层级如下：

其中：

（1）edge_obstacle_edge.h：这个文件主要是实现EdgeObstacleConstraint障碍物约束，保证优化后的轨迹，可以绕开障碍物。

（2）edge_via_point.h：这个文件主要是实现EdgeViaPointConstraint跟随参考轨迹点的约束，保证优化后的轨迹逼近参考轨迹。

（3）planner_manager.h：这个文件是轨迹优化器的管理类。

（4）main.cpp：这个文件是模拟外部调用。

2 代码分析

1.主函数


/* * @Function:using Trajectory Optimize Method  * @Create by:juchunyu@qq.com * @Date:2025-09-13 16:10:01 */#include "planner_manager.h"#include "matplotlib-cpp/matplotlibcpp.h"
namespace plt = matplotlibcpp;//可视化
int main(){    std::vector<double> glob_x;    std::vector<double> glob_y;
    std::vector<double> plan_x;    std::vector<double> plan_y;
    std::vector<double> obs_x;    std::vector<double> obs_y;
    std::vector<double> color;    // 1. 配置初始化    teb_local_planner::TebConfig cfg;    cfg.no_inner_iterations = 5;    cfg.no_outer_iterations = 4;    cfg.min_obstacle_dist = 1.0;    cfg.penalty_epsilon = 0.05;    cfg.obstacle_weight = 10;                        cfg.optimization_verbose = true;    cfg.weight_viapoint  = 1;
    // 2. 创建规划器并运行优化    // teb_local_planner::plannerManager planner(cfg);    std::shared_ptr<:plannermanager> planner = std::make_shared<:plannermanager>(cfg);
    std::vector<:pathinfo> gloablPlan;    std::vector<:obstacleinfo> obstacles;    std::cout  << std::endl;    for(int


    
 i = 0;i 10;i++)    {        tools::pathInfo temp;        temp.x = i;        temp.y = i;        gloablPlan.push_back(temp);        std::cout  << temp.x          glob_x.push_back(temp.x);        glob_y.push_back(temp.y);    }    std::cout  << std::endl;

    // add obstacles 1    tools::obstacleInfo obstemp;    obstemp.x = 3;    obstemp.y = 3.5;    obs_x.push_back(obstemp.x);    obs_y.push_back(obstemp.y);    color.push_back(1.0);    obstacles.push_back(obstemp);
    // add obstacles 2    obstemp.x = 7;    obstemp.y = 7.6;    obs_x.push_back(obstemp.x);    obs_y.push_back(obstemp.y);
    color.push_back(1.0);
    obstacles.push_back(obstemp);
    planner->setpathInfo(gloablPlan);    planner->setObstacleInfo(obstacles);
    planner->runOptimization();
    std::vector<:pathinfo> planResult;

    // 3. 获取规划结果    planner->getPlannerResults(planResult);
    // 4. 可视化    for(int i = 0;i < planResult.size();i++)    {        plan_x.push_back(planResult[i].x);        plan_y.push_back(planResult[i].y);    }
    std::map<:string std::string> keywords1;    keywords1.insert(std::pair<:string std::string>("label", "ref_traj") );    keywords1.insert(std::pair<:string std::string>("linewidth", "3.5") );    plt::plot(glob_x,glob_y,keywords1);

    std::map<:string std::string> keywords4;    keywords4.insert(std::pair<:string std::string>("label", "planTraj") );    keywords4.insert(std::pair<:string std::string>("linewidth", "3.5") );    plt::plot(plan_x,plan_y,keywords4);
    double point_size = 100.0;    plt::scatter_colored(obs_x, obs_y,color,point_size,{{"cmap", "viridis"}});
    plt::legend();    plt::title("Trajectory Optimize Method ");    plt::show();
    return 0;}

主函数主要做了如下几个事情：

（1）配置的初始化

 // 1. 配置初始化    teb_local_planner::TebConfig cfg;    cfg.no_inner_iterations = 5;     cfg.no_outer_iterations = 4;    cfg.min_obstacle_dist = 1.0;    cfg.penalty_epsilon = 0.05;    cfg.obstacle_weight = 10;                        cfg.optimization_verbose = true;    cfg.weight_viapoint  = 1;

设置优化器的迭代次数no_inner_iterations 和no_outer_iterations ，最小障碍物距离min_obstacle_dist，障碍物权重obstacle_weight，跟线的权重weight_viapoint 。

（2）创建规划器并运行优化

     // 2. 创建规划器并运行优化    std::shared_ptr<:plannermanager> planner = std::make_shared<:plannermanager>(cfg);    std::vector<:pathinfo> gloablPlan;    std::vector<:obstacleinfo> obstacles;    std::cout  << std::endl;    for(int i = 0;i 10;i++)    {        tools::pathInfo temp;        temp.x = i;        temp.y = i;        gloablPlan.push_back(temp);        std::cout  << temp.x          glob_x.push_back(temp.x);        glob_y.push_back(temp.y);    }    std::cout  << std::endl;    // add obstacles 1    tools::obstacleInfo obstemp;    obstemp.x = 3;    obstemp.y = 3.5;    obs_x.push_back(obstemp.x);    obs_y.push_back(obstemp.y);    color.push_back(1.0);    obstacles.push_back(obstemp);    // add obstacles 2    obstemp.x = 7;    obstemp.y = 7.6;    obs_x.push_back(obstemp.x);    obs_y.push_back(obstemp.y);    color.push_back(1.0);        obstacles.push_back(obstemp);       planner->setpathInfo(gloablPlan);    planner->setObstacleInfo(obstacles);    planner->runOptimization();

首先实例化plannerManager类，然后模拟产生参考轨迹。

//参考轨迹  for(int i = 0;i 10;i++) {        tools::pathInfo temp;        temp.x = i;        temp.y = i;        gloablPlan.push_back(temp);        std::cout  << temp.x          glob_x.push_back(temp.x);        glob_y.push_back(temp.y); }

模拟产生障碍物点云：产生两个障碍物(3,3.5)和(7,7.6)。

// add obstacles 1    tools::obstacleInfo obstemp;    obstemp.x = 3;    obstemp.y = 3.5;    obs_x.push_back(obstemp.x);    obs_y.push_back(obstemp.y);    color.push_back(1.0);    obstacles.push_back(obstemp);


    
    // add obstacles 2    obstemp.x = 7;    obstemp.y = 7.6;    obs_x.push_back(obstemp.x);    obs_y.push_back(obstemp.y);    color.push_back(1.0);        obstacles.push_back(obstemp);

最后调用plannerManager类成员将参考轨迹和障碍物信息传递到轨迹优化器，并开始启动优化。

planner->setpathInfo(gloablPlan);planner->setObstacleInfo(obstacles);planner->runOptimization();

（3）获取轨迹规划器结果

 // 3. 获取规划结果 planner->getPlannerResults(planResult);

（4）可视化结果

    std::map<:string std::string> keywords1;    keywords1.insert(std::pair<:string std::string>("label", "ref_traj") );    keywords1.insert(std::pair<:string std::string>("linewidth", "3.5") );    plt::plot(glob_x,glob_y,keywords1);    std::map<:string std::string> keywords4;    keywords4.insert(std::pair<:string std::string>("label", "planTraj") );    keywords4.insert(std::pair<:string std::string>("linewidth", "3.5") );    plt::plot(plan_x,plan_y,keywords4);    double point_size = 100.0;    plt::scatter_colored(obs_x, obs_y,color,point_size,{{"cmap", "viridis"}});    plt::legend();    plt::title("Trajectory Optimize Method ");    plt::show();

2.plannerManager优化器管理类

/* * @Function:Trajectory Optimize Method Manager * @Create by:juchunyu@qq.com * @Date:2025-09-13 16:10:01 */#pragma once #include "base_teb_edges.h"#include  "vertexPoint.h"#include "edge_obstacle_edge.h"#include "edge_via_point.h"


    
#include "tools.h"// -----------------------------------------------------------------------------// 优化器管理类// -----------------------------------------------------------------------------namespace teb_local_planner{class plannerManager{public:    // 构造函数：初始化配置、优化器、顶点容器    plannerManager(const TebConfig& cfg)        : cfg_(cfg)    {        optimizer_ = initOptimizer();        pose_vertices_.clear();    }    // 析构函数：释放顶点内存    ~plannerManager()    {       clearGraph();    }    void setpathInfo(std::vector<:pathinfo>& path);    void setObstacleInfo(std::vector<:obstacleinfo>& obs);    void getPlannerResults(std::vector<:pathinfo>& path);    // 核心：运行优化    void runOptimization();private:    // 初始化优化器（注册类型、配置求解器）    boost::shared_ptr<:sparseoptimizer> initOptimizer();    // 注册g2o类型（顶点+边）    static void registerG2OTypes();    // 添加顶点（TEB风格：容器管理顶点）    void AddVertices();    // 添加边（TEB风格：new边+setTebConfig+关联顶点）    void AddObstacleEdges();    void AddViaPointEdges();    int findClosestTrajectoryPose(tools::pathInfo &ref_point,int& idx);    // 执行优化    bool optimizeGraph();    // 清空图（保留顶点，仅删除边）    void clearGraph()    {        if (optimizer_)        {            optimizer_->edges().clear();            optimizer_->clear();        }    }    // 打印优化结果    void printResult();private:    const TebConfig& cfg_;  // 全局配置（只读）    boost::shared_ptr<:sparseoptimizer> optimizer_;  // 优化器    std::vector pose_vertices_;          // 顶点容器（TEB风格管理）    std::vector<:pathinfo> pathPointArr_;    std::vector<:obstacleinfo> obstaclePointInfo_;};}  // namespace teb_local_planner

这个C++类主要做下面几个事

（1）G2O的初始化

 boost::shared_ptr<: class="code-snippet__title">SparseOptimizer> plannerManager::initOptimizer() {        // 线程安全注册自定义类型        static boost::once_flag flag = BOOST_ONCE_INIT;        boost::call_once(&registerG2OTypes, flag);        // 配置求解器（二维顶点，动态残差维度）        typedef g2o::BlockSolver<:blocksolvertraits>2, -1>> BlockSolverType;        typedef g2o::LinearSolverDense<:posematrixtype> LinearSolverType;        // 创建求解器        auto linear_solver = std::make_unique<LinearSolverType>();        auto block_solver = std::make_unique<BlockSolverType>(std::move(linear_solver));        auto solver = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));        // 初始化优化器        auto optimizer = boost::make_shared<: class="code-snippet__title">SparseOptimizer>();        optimizer->setAlgorithm(solver);        optimizer->setVerbose(cfg_.optimization_verbose);  // 从配置读取verbose        return optimizer;}

其中registerG2OTypes需要将我们创建的EdgeViaPointConstraint和EdgeObstacleConstraint这两个约束进行注册，还有待优化的顶点进行注册。

 // 注册g2o类型（顶点+边） void plannerManager::registerG2OTypes() {     g2o::Factory* factory = g2o::Factory::instance();     factory->registerType("VERTEX_POINT2D", new g2o::HyperGraphElementCreator);     factory->registerType("EDGE_obstacle_CONSTRAINT", new g2o::HyperGraphElementCreator);     factory->registerType("EDGE_via_point_CONSTRAINT", new g2o::HyperGraphElementCreator);}

（2）开始执行优化

void plannerManager::runOptimization(){    std::cout  << std::endl;    // 1. 创建并添加顶点    AddVertices();    // 2. 创建并添加边    AddObstacleEdges();    AddViaPointEdges();    // 3. 执行优化    for(int i = 0; i < cfg_.no_outer_iterations;i++)    {        optimizeGraph();     }     // 4. 清空图（保留顶点）     clearGraph();}

添加顶点 AddVertices()，添加障碍约束AddObstacleEdges()，添加AddViaPointEdges() 添加逼近参考路线的约束。

3 约束的代码理解？

1.障碍约束AddObstacleEdges

首先看下，这个障碍物约束如何实现。


/* * @Function: Obstacle Contraint Edge Class * @Create by:juchunyu@qq.com * @Date:2025-09-13 16:10:01 */
#pragma once #include "base_teb_edges.h"#include "vertexPoint.h"#include "tools.h"
// 距离约束边namespace teb_local_planner{//public BaseTebUnaryEdge<1, teb_local_planner::obstacleInfo, VertexPoint2D>//第一个参数 1：表示误差向量的维度（这里是 1 维误差）。//第二个参数 double：表示边所关联的测量值（observation）的数据类型。//第三个参数 VertexPoint2D：表示该边所连接的顶点类型（这里连接的是一个 2D 点顶点）。class EdgeObstacleConstraint : public BaseTebUnaryEdge<1, tools::obstacleInfo, VertexPoint2D>{public:    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    // 核心：误差计算    virtual void computeError() override    {        if (!cfg_)        {            std::cerr  << std::endl;            _error[0] = 0.0;            return;        }
        // 获取顶点        const VertexPoint2D* v1 = static_cast<const VertexPoint2D*>(_vertices[0]);        Eigen::Vector2d point = v1->estimate();
        double dist = sqrt(pow(_measurement.x - point[0],2) + pow(_measurement.y - point[1],2));        _error[0] = tools::penaltyBoundFromBelow(dist, cfg_->min_obstacle_dist, cfg_->penalty_epsilon);        // std::cout <<  "point" << point <<  "误差值: " << _error[0] << std::endl;  // 距离小于阈值时应>0    }
    void setObstcele(tools::obstacleInfo &obs,const TebConfig* cfg)    {        _measurement = obs;


    
        cfg_         = cfg;    }};}  // namespace teb_local_planner

这个使用的是一元边，其中外部障碍物点云是_measurement ，待优化的顶点_vertices，是我们要计算待优化的顶点（_vertices）与障碍物的距离，将这个距离与最小障碍物距离进行比较，并设置成error误差(cost)。

那么plannerManager如何调用EdgeObstacleConstraint 类的？

void plannerManager::AddObstacleEdges(){        Eigen::Matrix<double,1,1> information;        information.fill(cfg_.obstacle_weight);//mat.fill(n)　将 mat 的所有元素均赋值为 n        std::cout  << information << std::endl;        int edge_id = 0;  // 全局递增ID        for (int i = 0; i < pose_vertices_.size(); ++i)        {            // const VertexPoint2D* v1 = static_cast(pose_vertices_[i]);            // //    double act_dist = (v1->estimate() - v2->estimate()).norm();            // Eigen::Vector2d point = v1->estimate();            // std::cout << "point =" << point << std::endl;            // continue;            for(int j = 0; j < obstaclePointInfo_.size();j++)            {                EdgeObstacleConstraint* e_soft = new EdgeObstacleConstraint();                 e_soft->setId(edge_id++);                // std::cout<< " vertex:" << *pose_vertices_[i] << std::endl;                std::cout << obstaclePointInfo_[j].x endl;                e_soft->setVertex(0, pose_vertices_[i]);  // 关联顶点1                e_soft->setTebConfig(cfg_);              // 传递配置（基类接口）                e_soft->setObstcele(obstaclePointInfo_[j],&cfg_);                // e_soft->setInformation(information);                e_soft->setInformation(Eigen::Matrix<double,1,1>::Identity());
                optimizer_->addEdge(e_soft);            }        }}

上述需要注意的是， e_soft->setId(edge_id++)这个的id一定要唯一。pose_vertices_是待优化的顶点，它最终赋值给_vertices ，obstaclePointInfo_是外部障碍物点云，它最终赋值给_measurement 。information是信息矩阵，cfg_.obstacle_weight的权重设置。

2.AddViaPointEdges逼近参考路线的约束

首先看下，这个约束如何实现的，


/* * @Function: Follw Path Contraint Edge Class * @Create by:juchunyu@qq.com * @Date:2025-09-13 16:10:01 */
#pragma once #include "base_teb_edges.h"#include  "vertexPoint.h"#include "tools.h"
///距离约束边（继承TEB二元边基类）namespace teb_local_planner{//public BaseTebUnaryEdge<1, teb_local_planner::obstacleInfo, VertexPoint2D>//第一个参数 1：表示误差向量的维度（这里是 1 维误差）。//第二个参数 double：表示边所关联的测量值（observation）的数据类型。//第三个参数 VertexPoint2D：表示该边所连接的顶点类型（这里连接的是一个 2D 点顶点）。class EdgeViaPointConstraint : public BaseTebUnaryEdge<1, tools::pathInfo, VertexPoint2D>{public:    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    // 核心：误差计算（使用基类的cfg_获取配置）    virtual void computeError() override    {        if (!cfg_)        {            std::cerr  << std::endl;            _error[0] = 0.0;            return;        }        // 获取顶点        const VertexPoint2D* v1 = static_cast<const VertexPoint2D*>(_vertices[0]);        Eigen::Vector2d point = v1->estimate();        _error[0] = sqrt(pow(_measurement.x - point[0],2) + pow(_measurement.y - point[1],2));        // std::cout <<  "point" << point <<  "误差值: " << _error[0] << std::endl;  // 距离小于阈值时应>0    }
    void setPathPoint(tools::pathInfo &path,const TebConfig* cfg)    {        _measurement = path;        cfg_         = cfg;    }};}  // namespace teb_local_planner

上述也是一元边，其中外部的参考轨迹点是_measurement，_vertices[0]代表待优化的顶点，需要指出的是，这个待优化的顶点是 _measurement在待优化的顶点容器上的投影，投影过程如下：

 int plannerManager::findClosestTrajectoryPose(tools::pathInfo &ref_point,int& idx) {        int n = pose_vertices_.size();        if (idx 0 || idx >= n)            return -1;
        double min_dist_sq = std::numeric_limits<double>::max();        int min_idx = -1;
        for (int i = idx; i < n; i++)        {            VertexPoint2D* v        =  static_cast(pose_vertices_[i]);            Eigen::Vector2d point   =  v->estimate();
            double dist_sq = sqrt(pow(ref_point.x - point[0],2) + pow(ref_point.y - point[1],2));            if (dist_sq < min_dist_sq)            {                min_dist_sq = dist_sq;                min_idx = i;            }        }
        idx = min_idx;
        return min_idx;}

现在分析，plannerManager类如何调用EdgeViaPointConstraint类？

    void plannerManager::AddViaPointEdges()    {        int start_index = 0;        int edge_id = 0;        for(int i = 0; i < pathPointArr_.size();i++)        {            int index = findClosestTrajectoryPose(pathPointArr_[i],start_index);
            Eigen::Matrix<double,1,1> information;            information.fill(cfg_.weight_viapoint);
            std::cout  << index << std::endl;
            EdgeViaPointConstraint* edge_viapoint = new EdgeViaPointConstraint;            edge_viapoint->setId(edge_id++);            edge_viapoint->setVertex(0,pose_vertices_[index]);            edge_viapoint->setInformation(information);            edge_viapoint->setPathPoint(pathPointArr_[i],&cfg_);            optimizer_->addEdge(edge_viapoint);        } }

这个也是，跟上述差不多。

如果不太明白，请阅读这篇文章：深入解析轨迹优化（TEB）算法中非线性求解器的 C++ 框架的实现。

4 代码运行调参

1.执行方法：

g2o版本建议使用2020年4月份版本，你去官方仓库找源码，并切换到2020年4月份的递交commit，然后对g2o进行源码编译安装。

mkdir buildcd buildcmake ..make./main

2.参数调整：

（1）如果你想让EdgeObstacleConstraint障碍物约束起的权重大，那就使obstacle_weight更大些。

设置如下：

    // 1. 配置初始化    teb_local_planner::TebConfig cfg;    cfg.no_inner_iterations = 5;    cfg.no_outer_iterations = 4;    cfg.min_obstacle_dist = 1.0;    cfg.penalty_epsilon = 0.05;    cfg.obstacle_weight = 10;                        cfg.optimization_verbose = true;    cfg.weight_viapoint  = 1;

曲线图：

会注意到，轨迹优化器输出的planTraj轨迹确实躲闪障碍物了。

（2）如果你想让EdgeViaPointConstraint逼近参考轨迹约束起的权重大，那就使weight_viapoint 更大些。

设置如下：

    // 1. 配置初始化    teb_local_planner::TebConfig cfg;    cfg.no_inner_iterations = 5;    cfg.no_outer_iterations = 4;    cfg.min_obstacle_dist = 1.0;    cfg.penalty_epsilon = 0.05;    cfg.obstacle_weight = 1;                        cfg.optimization_verbose = true;    cfg.weight_viapoint  = 10;

会注意到，轨迹优化器输出的planTraj轨迹确实跟随参考轨迹ref_traj了。

电脑好卡...

公众号平台编辑界面老崩溃...

可算写完了...

我建了一个轨迹优化与运动控制方向技术交流群，欢迎各位同行专家加入交流讨论！扫描下方二维码，添加作者微信，然后拉进群。