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

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

作者序

之前关于深入理解轨迹优化器TEB算法的文章：

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

今天的文章会在深入理解轨迹优化器TEB算法—从零开始使用C++实现轨迹优化器【附Github仓库代码链接】基础上更完善一些。

主要在之前的基础上做了角速度和速度约束以及差动轮运动学约束，开源的代码链接：

https://github.com/JackJu-HIT/Trajectory_Optimize_Method_Release

在这个基本算是比较完善的版本了，完全可以在这个版本上添加TEB剩下的约束了。

上次我发布了深入理解轨迹优化器TEB算法—从零开始使用C++实现轨迹优化器【附Github仓库代码链接】文章，有交流群的同行有部署问题，我在这里说一下，我自己亲自使用另一台电脑走了下部署流程，我使用的是ubuntu 24.04，g2o使用的是我仓库上传的：

https://github.com/JackJu-HIT/G2O_2020_04

直接源码编译，然后安装，Eigen库系统有，然后解决下找不到头文件的问题，就能编译通过。

另外，我在写速度约束的时候，发现有一个深刻的事情，就是，g2o在构造边edge的时候，会分为一元边/二元边/更加通用的多元边，之前文章的demo里只说明了一元边和二元边。简单介绍下更加通用的多元边如何是使用

class EdgeVelocityConstraint : public BaseTebMultiEdge<2,Eigen::Vector2d>// 2维残差，类型Eigen::Vector2d{public:    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    /* @brief 构造函数：指定连接的顶点数量（3个）     */    EdgeVelocityConstraint()    {        resize(3);  // 关键：连接3个顶点（v1, v2, v3）    }    // 核心：误差计算（使用基类的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]);        const VertexPoint2D* v2 = static_cast<const VertexPoint2D*>(_vertices[1]);        const vertexTimeDiff* v3 = static_cast<const vertexTimeDiff*>(_vertices[2]);        Eigen::Vector3d point1 = v1->estimate();        Eigen::Vector3d point2 = v2->estimate();        double dist =  sqrt(pow(point1[0] - point2[0],2) + pow(point1[1] - point2[1],2));        double deltaAngle = fabs(point1[2] - point2[2]);        double vel = dist / v3->estimate();        double w   = tools::normalize_theta(deltaAngle) / v3->estimate();        std::cout  << w         _error[0] =  tools::penaltyBoundToInterval(vel, -cfg_->max_vel_x_backwards, cfg_->max_vel,cfg_->penalty_epsilon);        _error[1] =  tools::penaltyBoundToInterval(w, cfg_->max_vel_theta,cfg_->penalty_epsilon);        std::cout  <<  _error[0] 1] << std::endl;    }    void setcfg(const TebConfig* cfg)    {        cfg_         = cfg;    }};

BaseTebMultiEdge<2,Eigen::Vector2d> 表示元边，2维残差，以及残差类型Eigen::Vector2d。

那么待估计的顶点数量如何确认？

   EdgeVelocityConstraint()    {        resize(3);  // 关键：连接3个顶点（v1, v2, v3）    }

调用上述的resize(3)就行，你需要传入3个顶点。

还有就是，本篇文章引入了相邻轨迹点之间的时间间隔timeDiff，需要注意的是，它和待优化的轨迹位姿点一样都是我们待估计的变量。

同时待估计的位姿顶点也从二维(x,y)变成三维(x,y,theta)。

优化后结果图如下：

本文会写的比较简单，主要侧重于新添加的约束代码解析，所以如果没看过之前的文章，可以先去看之前的两篇文章，这样更好理解一些。

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

2025年9月21日 14:15:00

Jack Ju于沈阳

1 速度约束构建

velocityEdge.h 文件实现了线速度和角速度约束。

class EdgeVelocityConstraint : public BaseTebMultiEdge<2,Eigen::Vector2d>// 2维残差，类型Eigen::Vector2d{public:    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    /* @brief 构造函数：指定连接的顶点数量（3个）     */    EdgeVelocityConstraint()    {        resize(3);  // 关键：连接3个顶点（v1, v2, v3）    }    // 核心：误差计算（使用基类的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]);        const VertexPoint2D* v2 = static_cast<const VertexPoint2D*>(_vertices[1]);        const vertexTimeDiff* v3 = static_cast<const vertexTimeDiff*>(_vertices[2]);        Eigen::Vector3d point1 = v1->estimate();        Eigen::Vector3d point2 = v2->estimate();        double dist =  sqrt(pow(point1[0] - point2[0],2) + pow(point1[1] - point2[1],2));        double deltaAngle = fabs(point1[2] - point2[2]);        double vel = dist / v3->estimate();        double w   = tools::normalize_theta(deltaAngle) / v3->estimate();        std::cout  << w         _error[0] =  tools::penaltyBoundToInterval(vel, -cfg_->max_vel_x_backwards, cfg_->max_vel,cfg_->penalty_epsilon);        _error[1] =  tools::penaltyBoundToInterval(w, cfg_->max_vel_theta,cfg_->penalty_epsilon);        std::cout  <<  _error[0] 1] << std::endl;    }    void setcfg(const TebConfig* cfg)    {        cfg_         = cfg;    }};

下述是分别估计出速度vel和角速度w的数值：

  double deltaAngle = fabs(point1[2] - point2[2


    
]);  double vel = dist / v3->estimate();  double w   = tools::normalize_theta(deltaAngle) / v3->estimate();

然后再分别对上述速度和角速度进行构造代价函数。

 _error[0] =  tools::penaltyBoundToInterval(vel, -cfg_->max_vel_x_backwards, cfg_->max_vel,cfg_->penalty_epsilon); _error[1] =  tools::penaltyBoundToInterval(w, cfg_->max_vel_theta,cfg_->penalty_epsilon);

2 差动模型运动学约束构建

参考TEB里的构建方式即可：

/* * @Function: edgeKineticConstraint Contraint Edge Class * @Create by:juchunyu@qq.com * @Date:2025-09-21 12:47:01 */#pragma once #include "base_teb_edges.h"#include  "vertexPoint.h"#include "tools.h"#include "vertexTImeDiff.h" ///距离约束边（继承TEB二元边基类）namespace teb_local_planner{class EdgeKineticConstraint : public BaseTebMultiEdge<2,Eigen::Vector2d>// 2维残差，类型Eigen::Vector2d{public:    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    /* @brief 构造函数：指定连接的顶点数量（3个）     */    EdgeKineticConstraint()    {        resize(2);  // 关键：连接3个顶点（v1, v2, v3）    }    // 核心：误差计算（使用基类的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]);        const VertexPoint2D* v2 = static_cast<const VertexPoint2D*>(_vertices[1]);              Eigen::Vector3d point1 = v1->estimate();        Eigen::Vector3d point2 = v2->estimate();                Eigen::Vector2d p1(point1[0],point1[1]);        Eigen::Vector2d p2(point2[0],point2[1]);                Eigen::Vector2d  deltaS = p2 - p1;        // non holonomic constraint        _error[0] = fabs( ( cos(point1[2])+cos(point2[2]) ) * deltaS[1] - ( sin(point1[2])+sin(point2[2]) ) * deltaS[0] );        // positive-drive-direction constraint        Eigen::Vector2d angle_vec ( cos(point1[2]), sin(point1[2]) );              _error[1] = tools::penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0);        std::cout  << _error[0] << std::endl;    }    void setcfg(const TebConfig* cfg)    {        cfg_         = cfg;    }};}  // namespace teb_local_planner

3运行分析

我们做了如下的参数配置：

    // 1. 配置初始化    teb_local_planner::TebConfig cfg;    cfg.no_inner_iterations = 5;    cfg.no_outer_iterations = 4;    cfg.min_obstacle_dist = 2.0;    cfg.penalty_epsilon = 0.05;


    
    cfg.obstacle_weight = 10;                        cfg.optimization_verbose = true;    cfg.weight_viapoint  = 1;    cfg.max_vel           = 1.0;    cfg.max_vel_theta     = 1.0;    cfg.max_vel_x_backwards = 0.3;    cfg.weight_max_vel_x    = 10;    cfg.weight_max_vel_theta = 10;    cfg.weight_kinematics_nh  = 1000;    cfg.weight_kinematics_forward_drive = 1;

编译运行后：

黑点是障碍物，ref_traj 是模拟全局规划的轨迹，plan_traj是这个轨迹优化器输出的轨迹。最小障碍物距离为2米。

我们再看下优化后的theta是否变化，初始全局轨迹的theta设置为PI/4 ；

x后面分别是x ，y ，theta，我们注意的输出的航向角theta在变化，分析趋势是正确的。

x = 1.0003, 0.9997, 0.7845)dt = 0.1489x = 1.1004, 1.0996, 0.7846)dt = 0.1489x = 1.2004, 1.1996, 0.7871)dt = 0.1489x = 1.3001, 1.2999, 0.7897)dt = 0.1494x = 1.3995, 1.4005, 0.7921)dt = 0.1537x = 1.4989, 1.5011, 0.7919)dt = 0.1523x = 1.5985, 1.6016, 0.7876)dt = 0.1490x = 1.6991, 1.7011, 0.7715)dt = 0.1486x = 1.8028, 1.7970, 0.7212)


    
dt = 0.1285x = 1.8713, 1.8534, 0.6586)dt = 0.1189x = 1.9311, 1.8973, 0.6067)dt = 0.1160x = 1.9945, 1.9389, 0.5547)dt = 0.1154x = 2.0616, 1.9782, 0.5042)dt = 0.1159x = 2.1330, 2.0152, 0.4527)dt = 0.1169x = 2.2095, 2.0499, 0.3986)dt = 0.1186x = 2.2926, 2.0822, 0.3419)dt = 0.1213x = 2.3848, 2.1122, 0.2873)dt = 0.1259x = 2.4904, 2.1398, 0.2246)dt = 0.1365x = 2.6176, 2.1650, 0.1670)dt = 0.1731x = 2.7802, 2.1902, 0.1402)dt = 0.2397x = 2.9992, 2.2256, 0.1799)dt = 0.3301x = 3.2986, 2.3050, 0.3389)dt = 0.4392x = 3.6685, 2.4970, 0.6185)dt = 0.5139x = 4.0030, 2.8315, 0.9523)dt = 0.4392x = 4.1950, 3.2014, 1.2319)dt = 0.3298x = 4.2744, 3.5008, 1.3909)dt = 0.2397x = 4.3098, 3.7198, 1.4306)dt = 0.1731x = 4.3350, 3.8824, 1


    
.4039)dt = 0.1365x = 4.3602, 4.0096, 1.3462)dt = 0.1259x = 4.3878, 4.1152, 1.2835)dt = 0.1213x = 4.4178, 4.2074, 1.2290)dt = 0.1186x = 4.4501, 4.2906, 1.1723)dt = 0.1169x = 4.4847, 4.3671, 1.1182)dt = 0.1159x = 4.5217, 4.4384, 1.0667)dt = 0.1154x = 4.5610, 4.5055, 1.0158)dt = 0.1160x = 4.6027, 4.5689, 0.9633)dt = 0.1189x = 4.6466, 4.6287, 0.9113)dt = 0.1286x = 4.7031, 4.6971, 0.8494)dt = 0.1486x = 4.7990, 4.8008, 0.7991)dt = 0.1490x = 4.8986, 4.9014, 0.7823)dt = 0.1546x = 4.9991, 5.0009, 0.7774)dt = 0.1489x = 5.0999, 5.1001, 0.7780)dt = 0.1488x = 5.2004, 5.1996, 0.7825)dt = 0.1489x = 5.3005, 5.2995, 0.7870)dt = 0.1489x = 5.4001, 5.3999, 0.7912)dt = 0.1489x = 5.4994, 5.5006, 0.7940)dt = 0.1557x = 5.5986, 


    
5.6014, 0.7927)dt = 0.1490x = 5.6985, 5.7017, 0.7822)dt = 0.1491x = 5.8006, 5.7996, 0.7467)dt = 0.1399x = 5.8985, 5.8845, 0.6822)dt = 0.1226x = 5.9579, 5.9295, 0.6141)dt = 0.1175x = 6.0217, 5.9717, 0.5548)dt = 0.1163x = 6.0899, 6.0114, 0.4998)dt = 0.1166x = 6.1627, 6.0488, 0.4477)dt = 0.1176x = 6.2409, 6.0839, 0.3962)dt = 0.1193x = 6.3259, 6.1169, 0.3448)dt = 0.1220x = 6.4196, 6.1480, 0.2969)dt = 0.1262x = 6.5259, 6.1775, 0.2445)dt = 0.1351x = 6.6511, 6.2058, 0.2003)dt = 0.1659x = 6.8046, 6.2358, 0.1849)dt = 0.2140x = 6.9996, 6.2753, 0.2149)dt = 0.2764x = 7.2510, 6.3447, 0.3237)dt = 0.3549x = 7.5585, 6.4830, 0.5221)dt = 0.4227x = 7.8747, 6.7253, 0.7854)dt = 0.4227x = 8.1170, 7.0415, 1.0487)dt = 0.3549x


    
 = 8.2553, 7.3489, 1.2471)dt = 0.2772x = 8.3247, 7.6004, 1.3564)dt = 0.2142x = 8.3642, 7.7955, 1.3859)dt = 0.1652x = 8.3943, 7.9487, 1.3668)dt = 0.1352x = 8.4226, 8.0739, 1.3304)dt = 0.1164x = 8.4518, 8.1806, 1.2768)dt = 0.1220x = 8.4830, 8.2742, 1.2213)dt = 0.1193x = 8.5162, 8.3590, 1.1742)dt = 0.1176x = 8.5513, 8.4372, 1.1230)dt = 0.1166x = 8.5887, 8.5100, 1.0710)dt = 0.1163x = 8.6284, 8.5782, 1.0164)dt = 0.1175x = 8.6705, 8.6421, 0.9583)dt = 0.1225x = 8.7154, 8.7015, 0.8886)dt = 0.1364x = 8.8004, 8.7993, 0.8234)dt = 0.1492x = 8.8983, 8.9015, 0.7897)dt = 0.1491x = 8.9984, 9.0015, 0.7800)dt = 0.1531x = 9.0991, 9.1010, 0.7788)dt = 0.1519x = 9.1996, 9.2004, 0.7803)dt = 0.1493x = 9.3000, 9.3000, 0.7827)dt = 0.


    
1489x = 9.4002, 9.3998, 0.7842)dt = 0.1489x = 9.5003, 9.4997, 0.7849)dt = 0.1489x = 9.6003, 9.5997, 0.7858)dt = 0.1489x = 9.7002, 9.6998, 0.7867)dt = 0.1489x = 9.8001, 9.7999, 0.7869)dt = 0.1488x = 9.9000, 9.9000, 0.7861)

另外时间间隔一开始也是固定的，因为我轨迹给的均匀，看上述的dt是优化后的时间间隔，这个会影响最终计算的线速度和角速度。

提取速度那块没写，还有几个约束也没写，后面只要参考TEB的就能写出来了。

关于TEB算法的理解和从零开始编码写一个轨迹规划器，到这就终结了。

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