EgoPlanner后端轨迹优化算法—如何在2D地面机器人项目中将其工程落地？文中附修改后源代码GitHub仓库

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

序

作者兴趣点在于机器人如何在复杂的动态环境中快速避障，通过查找资料，注意到无人机领域的EgoPlanner后端优化算法不错，于是想研究研究，寻求些新突破，像TEB（Time elastic band）和FastPlanner后端局部轨迹优化技术之前已经做了储备。

需要说明的是Ego Planner是在Fast Planner上进一步优化的，不依赖于ESDF欧式距离场。

之前写过的FastPlanner后端轨迹优化算法：

1.FastPlanner后端轨迹优化算法—如何在2D地面机器人项目中将其工程落地？文中附视频代码讲解以及源代码GitHub仓库

2.快速局部轨迹规划算法Faster Planner-深入理解B样条

3.快速运动规划算法—基于欧几里得有符号距离场（ESDF）的局部地图算法

4.机器人规划算法——42分钟弄懂3D环境下的ESDF地图算法

5.Fast Planner后端B样条轨迹优化算法代码分析(一)—B样条的导数

6.Fast Planner后端B样条轨迹优化算法代码分析(二)—B样条的参数化

7.轨迹优化算法—FastPlanner轨迹优化算法详解

考虑到，未来能应用的项目平台不支持ROS，必须将其改写，去掉ROS相关逻辑，封装成可调用的纯C++文件，这样就可以兼容非ROS平台上。同时，我已经将其C++接口封装好了，可以根据自己的需求部署到项目里，也可以作为仿真器去debug代码，一步一步去分析学习底层算法原理，我觉得这是很有意义的一件事。

本文不会重点讨论EgoPlanner后端基于B样条轨迹优化的数学原理，我将重点站在一线算法工程师的角度上，如何能将适用于三维的无人机轨迹优化修改适配到二维的移动机器人轨迹优化上，以及如何让快速使用到自己的工程项目里。

首先，开源的EgoPlanner是三维度的，不适用于移动机器人路径规划，作者在仿真的时候，它在优化绕障轨迹的时候，出现了从Z轴方向避开障碍物，这显然不适用于地面的移动机器人。因此对于低矮的障碍物无人机是可以从上方飞越的，而对于地面机器人来说其只能绕过障碍物，因此需要对原始的地图进行修改。具体操作方法就是在处理点云时候，将这些点云沿着Z轴方向进行扩展即可。这样就会使最终轨迹规划就能约束不会从障碍物上方越过，而是实现绕行。

其次，我修改的工程base是浙江大学高飞老师课题组https://github.com/ZJU-FAST-Lab/ego-planner.git开源代码。

然后，由于考虑到ego-planner开源代码比较系统，包含了前端的路径搜索还有飞行走廊等比较多的模块，对于我只关注于后端基于B样条的轨迹优化的算法来说，太多了，我需要一个“纯净版”，对原代码进行了一定的删减和修改封装，这样我可以直接和TEB轨迹优化算法进行对标，并用C++ matplotlib进行可视化观察规轨迹的效果。

上图就是我修改后的效果，蓝色的直线是给定的全局轨迹，绕障安全距离设置0.5米，圆点是设置的障碍物，绿色曲线就是EgoPlanner的后端基于B样条的轨迹优化的结果。

写这篇文章原因是，我在研究过程中，搜了网上没有EgoPlanner后端如何适配到2D维度的移动机器人项目上的开源代码，我修改后的工程代码请参见https://github.com/JackJu-HIT/EgoPlanner.git，如果你觉得还不错，请帮忙给github点个星星！

因个人水平有限，错误在所难免，如有错误请帮忙指出。

2025年8月2日 21:52:00

Jack Ju于沈阳

1EgoPlanner 基本原理

1.为什么不像FastPlanner使用ESDF？

Fast Planner轨迹陷入局部最小值，这是非常常见的，因为摄像头看不到障碍物的背面。

1.Ego-Planner 优势

（1）计算效率极高（0.81 ms），适合实时应用（如无人机避障）。

（2）放弃 ESDF提升速度。

2.ESDF 的局限性

（1）计算成本较高（柱状图中 ESDF 相关操作耗时 1.39–4.01 ms）。

（2）依赖全局环境信息，对 未知障碍物（Unseen Obstacle） 无效。

2.Ego-Planner原理

1. 初始化轨迹生成

输入：一条朴素的初始 B 样条曲线（A Naïve Initial B-spline Curve）
特点：仅满足起点/终点约束（Terminal Constraints），忽略碰撞检测（Regardless of Collision）。

2. 轨迹优化循环

(1)计算成本与梯度（Calculate cost and gradient）

1) 成本函数：包含平滑性、动力学可行性、终端约束等。

2) 梯度计算：指导轨迹向更低成本方向调整。

(2) 梯度下降（Gradient Descent）

1) 沿梯度反方向更新轨迹参数，逐步降低总成本。

(3)更新曲线（Update Curve）

1) 生成新的 B 样条轨迹。

(4) 碰撞检测（Check Collision）

检测新轨迹是否与环境障碍物碰撞：

1)无碰撞 → 进入时间重分配和轨迹微调（Time Re-allocation）。

2)有碰撞 → 触发避障力估计（Collision Avoidance Force Estimation）。

3. 避障力驱动重新优化

(1)避障力估计（Force Estimation） ：

1)在碰撞点附近生成虚拟斥力（类似物理模型的排斥力），将轨迹推离障碍物。

(2) 重构优化问题（Formulate new problem）：

1)将避障力作为新约束加入成本函数，重新计算梯度和优化轨迹。

2)循环迭代：直至轨迹无碰撞。

4. 最终处理：时间重分配与微调

（1）时间重分配（Time Re-allocation）：调整轨迹的时间参数，确保速度/加速度符合动力学约束（如无人机最大加速度）。

（2）轨迹微调（Trajectory Refinement）：对无碰撞轨迹做最后平滑处理，提升执行稳定性。

上述图片就是构建的优化问题。

本文不重点分析基本原理以及对应的代码，后续作者会陆续更新从理论到代码的文章。

2 代码分析

我将以我修改后的工程代码https://github.com/JackJu-HIT/EgoPlanner进行分析。

首先介绍下代码层级结构：

（1）plan_env：这个文件夹下主要是localmap。

（2）bspline_opt：这个文件夹下是本文要讨论的FastPlanner的后端基于B样条的轨迹优化的部分。

（3）path_searching：这个文件夹下是A*路径搜索。

（4）plan_manage：这个文件夹下是规划器的接口。

1.主程序入口

main.cpp/* * @Function:Using Ego Planner to Generate Trajectory * @Create by:juchunyu@qq.com * @Date:2025-08-02 18:58:01 */#include  #include     #include      
#include "planner_interface.h"#include "./planner/matplotlib-cpp/matplotlibcpp.h"
using namespace ego_planner;
namespace plt = matplotlibcpp;

int main(){    std::vector<double> global_x;    std::vector<double> global_y;
    std::vector<double> local_plan_x;    std::vector<double> local_plan_y;
    std::vector<double> obs_x;    std::vector<double> obs_y;    std::vector<double


    
> color;
    std::shared_ptr  plan = std::make_shared();
    //初始化控制参数    double max_vel = 1.0;    double max_acc = 1.0;    double max_jerk = 1.0;    plan->initParam(max_vel,max_acc,max_jerk);
    //初始化ESDF地图    double resolution = 0.1;    double x_size = 10.0;    double y_size = 10.0;    double z_size = 10.0;    Eigen::Vector3d origin(0.0, 0.0, 0.0);    double inflateValue = 0.0;    plan->initEsdfMap(x_size,y_size,z_size,resolution,origin,inflateValue);
    //添加障碍物点云    std::vector obstacle;    ObstacleInfo Obstemp;    Obstemp.x = 6;    Obstemp.y = 6;    obstacle.push_back(Obstemp);    Obstemp.x = 3.0;    Obstemp.y = 3.0;    obstacle.push_back(Obstemp);
    plan->setObstacles(obstacle);
    //添加全局路径点    std::vector global_plan_traj;
    global_x.clear();    global_y.clear();
    float j = 0;
    while(j 10)    {        PathPoint tempPoint;        tempPoint.x = j;        tempPoint.y = j;
        global_plan_traj.push_back(tempPoint);
        j+= 0.1;        global_x.push_back(j);        global_y.push_back(j);    }

    plan->setPathPoint(global_plan_traj);

    //开始规划    plan->makePlan();
    //获取规划结果    std::vector plan_traj_res;    plan->getLocalPlanTrajResults(plan_traj_res);
    local_plan_x.clear();    local_plan_y.clear();
    for(int i = 0; i < plan_traj_res.size(); i++)    {        local_plan_x.push_back(plan_traj_res[i].x);        local_plan_y.push_back(plan_traj_res[i].y);    }
    //show    for(int i = 0; i < obstacle.size();i++)    {        obs_x.push_back(obstacle[i].x);        obs_y.push_back(obstacle[i].y);        color.push_back(1.0);    }
    std::map<:string std::string> keywords1;    keywords1.insert(std::pair<:string std::string>("label", "globaltraj"));    keywords1.insert(std::pair<:string std::string>("linewidth", "3.5"));    plt::plot(global_x, global_y, keywords1);
    std::map<:string std::string> keywords2;    keywords2.insert(std::pair<:string std::string>("label", "localtraj"));    keywords2.insert(std::pair<:string std::string>("linewidth", "3.5"));    plt::plot(local_plan_x, local_plan_y, keywords2);

    double point_size = 100.0;


    
    plt::scatter_colored(obs_x, obs_y,color,point_size,{{"cmap", "viridis"}});    plt::legend();    plt::title("Ego Planner Results!");    plt::show();

    return 0;}

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

（1）初始化控制参数：设置最大速度和最大加速度。

plan->initParam(max_vel,max_acc);

（2）初始化ESDF地图：包括地图的尺寸，分辨率，原点和地图膨胀半径。

plan->initEsdfMap(x_size,y_size,z_size,resolution,origin,inflateValue);

（3）添加障碍物体点云：将障碍物点云添加到ESDF中。

plan->setObstacles(obstacle);

（4）添加全局路径点：这个是优化的初始轨迹。

plan->setPathPoint(global_plan_traj);

（5）开启规划器：基于B样条轨迹优化器开始求解轨迹

plan->makePlan();

（6）获取轨迹优化的结果

plan->getLocalPlanTrajResults(plan_traj_res);

（7）可视化：剩下部分就是可视化障碍物和原始轨迹以及规划轨迹。

std::map<:string std::string> keywords1;keywords1.insert(std::pair<:string std::string>("label", "globaltraj"));keywords1.insert(std::pair<:string std::string>("linewidth", "3.5"));plt::plot(global_x, global_y, keywords1);
std::map<:string std::string> keywords2;keywords2.insert(std::pair<:string std::string>("label", "localtraj"));keywords2.insert(std::pair<:string std::string>("linewidth", "3.5"));plt::plot(local_plan_x, local_plan_y, keywords2);

 double point_size = 100.0; plt::scatter_colored(obs_x, obs_y,color,point_size,{{"cmap", "viridis"}}); plt::legend(); plt::show();

2.接口PlannerInterface类

这个接口，就是最终调用B样条轨迹优化器部分。

/* * @Function:Ego Planner:Trajectory Optimize Base Bspline * @Create by:juchunyu@qq.com * @Date:2025-08-02 17:40:01 */



    
#ifndef _PLANNER_INTERFACE_H_#define _PLANNER_INTERFACE_H_
#include 
#include #include #include #include #include "Bspline.h"#include 

namespace  ego_planner{    struct PathPoint    {        float x;        float y;        float z;        float v;    };
    struct ObstacleInfo    {        float x;        float y;    };
    class PlannerInterface    {
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
            bool reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,                                                Eigen::Vector3d start_acc, Eigen::Vector3d local_target_pt,                                                Eigen::Vector3d local_target_vel,vector<:vector3d> point_set);            PlanParameters pp_;            LocalTrajData local_data_;            shared_ptr grid_map_;
        private:
            BsplineOptimizer::Ptr bspline_optimizer_rebound_;
            int continous_failures_count_{0};
            void updateTrajInfo(const UniformBspline &position_traj);
            void reparamBspline(UniformBspline &bspline, vector<:vector3d> &start_end_derivative, double ratio, Eigen::MatrixXd &ctrl_pts, double &dt,                            double &time_inc);
            bool refineTrajAlgo(UniformBspline &traj, vector<:vector3d> &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points);

        private:

            std::vector _global_plan_traj_;            std::vector _plan_traj_results_;

        public:            PlannerInterface();
            ~PlannerInterface();
            void initParam(double max_vel,double


    
 max_acc,double max_jerk);
            void initEsdfMap(double x_size,double y_size,double z_size,double resolution, Eigen::Vector3d org,double inflate_values);
            void setPathPoint(std::vector &plan_traj);
            void setObstacles(std::vector &obstacle);
            void makePlan();
            void getLocalPlanTrajResults(std::vector &plan_traj_results);  
            void getTraj();
    };

}

#endif

主要分为如下几个部分：

（1）外部接口：用于外部传入数据以及返回数据。

   void initParam(double max_vel,double max_acc,double max_jerk);   void initEsdfMap(double x_size,double y_size,double z_size,double resolution_, Eigen::Vector3d org,double inflate_values);   void setPathPoint(std::vector &plan_traj);   void setObstacles(std::vector &obstacle);   void makePlan();   void getLocalPlanTrajResults(std::vector &plan_traj_results);

这几个类函数在上面介绍过，main函数调用的就是它们。

（2）内部处理接口

bool reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,                                                Eigen::Vector3d start_acc, Eigen::Vector3d local_target_pt,                                                Eigen::Vector3d local_target_vel,vector<Eigen::Vector3d> point_set);         
void updateTrajInfo(const UniformBspline &position_traj);void reparamBspline(UniformBspline &bspline, vector<Eigen::Vector3d> &start_end_derivative, double ratio, Eigen::MatrixXd &ctrl_pts, double &dt,                            double &time_inc);bool refineTrajAlgo(UniformBspline &traj, vector<Eigen::Vector3d> &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points);

（3）makePlan()函数:这个最终要计算的主程序了，细节不讲了。

void PlannerInterface::makePlan(){            Eigen::Vector3d start_pt;        Eigen::Vector3d start_vel;        Eigen::Vector3d start_acc;        Eigen::Vector3d local_target_pt;        Eigen::Vector3d local_target_vel;        vector<Eigen::Vector3d> point_set;        vector<Eigen::Vector3d> traj_pts;        for(int i = 0; i< _global_plan_traj_.size();i++)        {            Eigen::Vector3d plan_pt(_global_plan_traj_[i].x,_global_plan_traj_[i].y,0.2);            point_set.push_back(plan_pt);        }        start_pt[0] = _global_plan_traj_[0].x;        start_pt[1] = _global_plan_traj_[0].y;        start_pt[2] = 0.0;                local_target_pt[0] = _global_plan_traj_[_global_plan_traj_.size()-1].x;        local_target_pt[1] = _global_plan_traj_[_global_plan_traj_.size()-1].y;        local_target_pt[2] = 0;        start_vel[0] = 0;  //根据实际需求修改接入        start_vel[1] = 0;        start_vel[2] = 0;        local_target_vel[0] = 0;//根据实际需求修改接入        local_target_vel[1] = 0;        local_target_vel[2] = 0;        start_acc[0] = 0; //根据实际需求修改接入        start_acc[1] = 0;        start_acc[2] = 0;        auto start = std::chrono::system_clock::now();        bool plan_success = reboundReplan(start_pt,start_vel, start_acc,local_target_pt,local_target_vel, point_set);        if (plan_success)           getTraj();        auto end = std::chrono::system_clock::now();        auto elapsed = std::chrono::duration_cast<:chrono::milliseconds>(end - start);        printf("MotionPlanner Total Running Time: %d  ms \n", elapsed.count());    }

3 讨论

在这个部分，我将讨论几个重点的参数在哪里调整。

1.参数调整

（1）lamda1_：控制轨迹平滑性（smoothness）的权重;

（2）lamda2_：控制避障强度（collision avoidance）的权重;

（3）lamda3 _：强制轨迹满足动力学约束（dynamic feasibility）的权重;

（4）lamda4_：控制轨迹与全局参考路径贴合度的权重;

（5）安全距离阈值 (dist0_):定义最小安全避障距离

如果你想改变绕障时距离障碍物的距离，可以调整dist0_。

2.程序编译执行

cd EgoPlannermkdir buildcd build


    
cmake ..make ./EgoPlanCore