从零开始使用C++实现轨迹优化器【附Github仓库代码链接】

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

作者序

本文将介绍如何从零开始编写轨迹优化算法，参考的开源项目是TEB算法。目前的代码主要在之前的基础上做了角速度和速度约束以及差动轮运动学约束，开源的代码链接：

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

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

有交流群的同行有部署问题，我在这里说一下，我自己亲自使用另一台电脑走了下部署流程，我使用的是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)。

优化后结果图如下：

本文会写的比较简单，主要侧重于新添加的约束代码解析，详细的可以看下面的视频。

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

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.1154


    
x = 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算法的理解和从零开始编码写一个轨迹规划器，到这就终结了。

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