EGO-Planner文章解读(一)——论文原理和算法实现

VIPEGO-Planner文章解读(一)——论文原理和算法实现
详细解读egoplanner论文和代码
开通可读全文 ·3.9k 人阅读
最低0.47元/天 解锁文章

        在fastplanner中,ESDF 对于评估梯度大小和方向至关重要。然而轨迹优化过程只覆盖了 ESDF 更新范围的非常有限的子空间,计算全图的ESDF是非常冗余的。除此之外,在建图的时候只能看见障碍物的表面,看不见障碍物的后面/内部,在优化的过程中会使用ESDF去产生一个排斥轨迹远离障碍物,在下图所示的例子中,上面的障碍物将轨迹往下推,下面的障碍物将轨迹往上推,会出现轨迹卡在障碍物中的情况。

        EGO-Planner中将碰撞项从fastplanner中依据ESDF图进行改进。仅当轨迹撞到新的障碍物时,生成的障碍物信息才会被存储,这使得规划器只提取必要的障碍物信息。然后,如果违反了动力学可行性,我们就会延长时间分配,引入各向异性曲线拟合算法,在保持原始形状的同时调整轨迹的高阶导数。

一、不使用ESDF场产生轨迹推力

        在egoplanner这篇论文中,决策变量是控制点Qi。主要思想是,轨迹在优化的过程中,如果轨迹与障碍物发生碰撞,我们就基于这个碰撞对轨迹产生一个反方向的力来把轨迹推出来,远离刚刚碰到的障碍物。

        我们对轨迹段进行检查,找到在障碍物内部的控制点Qi,以及与该控制点左右临近的两个不在障碍物内部的控制点。根据这两个不在障碍物内部的控制点,利用A*生成一条远离障碍物的路径。取该控制点的切向量,以该向量为法向量生成一个平面。该平面与障碍物相交,在之前根据左右两个控制点生成的轨迹一侧与障碍物表面的交点为pij点(i代表控制点的索引,j代表障碍物的索引),从Qi到该p点的方向向量为向量v。这样就组成了一个{p,v}对,一个{p,v}对只属于一个控制点Qi。

        这里定义第i个控制点Qi到第j个障碍物的距离为dij

d_{ij}=\left ( Q_{i}-p_{ij} \right )\cdot \mathbf{v_{ij}}

        为了避免轨迹从当前障碍物中逃脱之前生成重复的 {p, v} 对,只有当Qi对于所有的j,dij>0都满足,我们才认为当前障碍物是新发现的。同时,该方法使得只有对最终轨迹有贡献的障碍物才在优化项中进行优化,因此,计算时间显着减少。

二、前端搜索

Fast Planner的前端使用的是Hybrid A * 算法,而Ego Planner并不需要一个与障碍物不发生碰撞的初值轨迹,所以它的前端不需要Hybrid A * 这种比较复杂的算法,它只需要一条不考虑障碍物,满足给定起点和目标点状态的轨迹即可,然后在后端优化中再把轨迹优化成无碰撞的。

前端的路径搜索没有在论文中说明,我们在代码中去解读,对应代码如下:

/*** STEP 1: INIT ***/
    double ts = (start_pt - local_target_pt).norm() > 0.1 ? pp_.ctrl_pt_dist / pp_.max_vel_ * 1.2 : pp_.ctrl_pt_dist / pp_.max_vel_ * 5; // pp_.ctrl_pt_dist / pp_.max_vel_ is too tense, and will surely exceed the acc/vel limits
    //ts是时间间隔,这里是判断起点距终点是否接近,距离目标点较远时,初始轨迹的时间间隔稍长;距离目标点较近时,时间间隔较短,生成更紧密的控制点
    vector<Eigen::Vector3d> point_set, start_end_derivatives;
    static bool flag_first_call = true, flag_force_polynomial = false;
    bool flag_regenerate = false;
    do
    {
      point_set.clear();
      start_end_derivatives.clear();
      flag_regenerate = false;

      if (flag_first_call || flag_polyInit || flag_force_polynomial /*|| ( start_pt - local_target_pt ).norm() < 1.0*/) // Initial path generated from a min-snap traj by order.
      //三个标志位,首次使用,强制使用多项式,
      {
        flag_first_call = false;
        flag_force_polynomial = false;

        PolynomialTraj gl_traj;

        double dist = (start_pt - local_target_pt).norm();//轨迹距离
        double time = pow(pp_.max_vel_, 2) / pp_.max_acc_ > dist ? sqrt(dist / pp_.max_acc_) : (dist - pow(pp_.max_vel_, 2) / pp_.max_acc_) / pp_.max_vel_ + 2 * pp_.max_vel_ / pp_.max_acc_;
        //轨迹时间,如果起点和目标点距离小于速度-加速度约束的临界值,用加速度时间,如果距离较远,采用加速-匀速-减速时间,time为总时间
        if (!flag_randomPolyTraj)//不需要随机中间点
        {
          gl_traj = PolynomialTraj::one_segment_traj_gen(start_pt, start_vel, start_acc, local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), time);
        //直接使用起点和终点的mininum snap轨迹
        }
        else//需要随机中间点
        {
          Eigen::Vector3d horizen_dir = ((start_pt - local_target_pt).cross(Eigen::Vector3d(0, 0, 1))).normalized();
          Eigen::Vector3d vertical_dir = ((start_pt - local_target_pt).cross(horizen_dir)).normalized();
          Eigen::Vector3d random_inserted_pt = (start_pt + local_target_pt) / 2 +//随机生成中间点
                                               (((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * horizen_dir * 0.8 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989) +
                                               (((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * vertical_dir * 0.4 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989);
          Eigen::MatrixXd pos(3, 3);//轨迹点
          pos.col(0) = start_pt;
          pos.col(1) = random_inserted_pt;
          pos.col(2) = local_target_pt;
          Eigen::VectorXd t(2);
          t(0) = t(1) = time / 2;
          gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, local_target_vel, start_acc, Eigen::Vector3d::Zero(), t);//使用mininum snap生成轨迹
        }

        double t;
        bool flag_too_far;
        ts *= 1.5; // ts will be divided by 1.5 in the next//将时间变为之前的1.5倍,放宽一点,
        do
        {
          ts /= 1.5;//在每次循环中缩小时间间隔
          point_set.clear();
          flag_too_far = false;
          Eigen::Vector3d last_pt = gl_traj.evaluate(0);
          for (t = 0; t < time; t += ts)
          {
            Eigen::Vector3d pt = gl_traj.evaluate(t);//每隔ts采样一个点pt
            if ((last_pt - pt).norm() > pp_.ctrl_pt_dist * 1.5)//计算pt与上一个点的距离,如果超过了pp_.ctrl_pt_dist * 1.5,说明轨迹不够平滑,终止循环
            {
              flag_too_far = true;
              break;
            }
            last_pt = pt;
            point_set.push_back(pt);//将pt加入点集中
          }
        } while (flag_too_far || point_set.size() < 7); // To make sure the initial path has enough points.//采样多项式轨迹点,直到满足控制点密度约束
        //存在两个轨迹点的间距超过阈值,点的数量太少,会重新进行采样
        t -= ts;//t是采样终止的时间点,将时间t修正到轨迹的实际终点
        start_end_derivatives.push_back(gl_traj.evaluateVel(0));//起点速度
        start_end_derivatives.push_back(local_target_vel);//终点速度
        start_end_derivatives.push_back(gl_traj.evaluateAcc(0));//起点加速度
        start_end_derivatives.push_back(gl_traj.evaluateAcc(t));//终点加速度
      }
      else // Initial path generated from previous trajectory.//不是第一次生成轨迹,从上一次生成的轨迹提取点集,用作当前规划的初始路径
      {

        double t;
        double t_cur = (ros::Time::now() - local_data_.start_time_).toSec();//计算起始时间,当前时间相对于轨迹开始时间移动了多少秒,此时开始为起点时间

        vector<double> pseudo_arc_length;//记录轨迹点之间的累积弧长,用于插值采样
        vector<Eigen::Vector3d> segment_point;//提取的轨迹点集合
        pseudo_arc_length.push_back(0.0);//起始点弧长为0
        for (t = t_cur; t < local_data_.duration_ + 1e-3; t += ts)//以固定步长ts来遍历时间
        {
          segment_point.push_back(local_data_.position_traj_.evaluateDeBoorT(t));//调用 evaluateDeBoorT(t) 函数,从B样条轨迹中计算时间 t 对应的轨迹点位置
          if (t > t_cur)//跳过起点,只有当当前时间大于起点时间时才计算弧长
          {
            pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back());//(segment_point.back() - segment_point[segment_point.size() - 2]).norm()当前位置减去前一个点的位置的弧长,加入到pseudo_arc_length累积弧长中
          }
        }
        t -= ts;//由于 t 在最后一次循环中被多加了一个时间步长 ts,需要减去 ts,回退到最后一个有效采样时间点

        double poly_time = (local_data_.position_traj_.evaluateDeBoorT(t) - local_target_pt).norm() / pp_.max_vel_ * 2;//估计从当前轨迹末点到目标点的时间
        if (poly_time > ts)//时间大于时间间隔ts,当前点到目标点的轨迹较长,且需要根据该时间来生成新的轨迹,否则就不需要生成新的轨迹
        {
          PolynomialTraj gl_traj = PolynomialTraj::one_segment_traj_gen(local_data_.position_traj_.evaluateDeBoorT(t),
                                                                        local_data_.velocity_traj_.evaluateDeBoorT(t),
                                                                        local_data_.acceleration_traj_.evaluateDeBoorT(t),
                                                                        local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), poly_time);//根据当前轨迹的末端状态(位置、速度、加速度)和目标点的状态(位置、速度)生成一个单段的多项式轨迹

          for (t = ts; t < poly_time; t += ts)//使用时间步长 ts 对生成的多项式轨迹 gl_traj 进行采样
          {
            if (!pseudo_arc_length.empty())
       
VIP 无人机路径规划实战:Fast-Planner和Ego-Planner怎么选?附场景对比表 本文深入对比了无人机路径规划中Fast-Planner和Ego-Planner两大主流算法。Fast-Planner侧重全局最优,适合已知环境的离线或低频规划;Ego-Planner则强调局部实时反应,在动态复杂环境中表现出色。文章通过核心哲学、性能维度和典型场景的实战分析,提供了清晰的选型指南和调参建议,帮助开发者根据具体项目需求做出高效决策。 开通可读全文·607 阅读·2 点赞·1 收藏 阅读全文

相关推荐

EGO-Planner深度解读:无ESDF的无人机局部轨迹规划与实现

在无人机自主导航中,路径规划与避障是核心难题。传统梯度优化方法依赖欧几里得符号距离场(ESDF),但ESDF的构建与更新开销巨大,且参数耦合严重。EGO-Planner通过“查询代替构建”的思路,利用B样条表示轨迹,并推导出碰撞项的闭合梯度,实现了真正的无ESDF局部轨迹优化,大幅降低计算与内存消耗。该算法在动态环境重规划中响应快,尤其适合计算资源受限的小型无人机。围绕EGO-Planner,从原理到实现,拆解其目标函数设计、碰撞项梯度推导、初始化与时间重分配的关键环节,并结合复现实验分享调参经验与常见坑点

weixin_33708633的博客 55

路径规划——曲线拟合详解(二):贝塞尔曲线、B样条曲线与QP优化( Fast-Planner算法核心部分)

曲线优化收官之战(看懂B样条+Fast-Planner)

weixin_43793717的博客 1万+

51c自动驾驶~合集57

多目标多头蒸馏模块:我们看到轨迹模仿学习之后的轨迹还过了其他的MLP,这就是其他头,它的目标也是不一样的,第一个是跟碰撞相关的,第二个是跟行驶区域相关的,第三个是跟舒适度相关的,也就是说不同的评判指标都有一个teacher,之前的模仿学习就是人类的teacher,那么这些teacher是怎么来的呢?本文作者提出的新的范式,就是规划模块是多模的输出,同时,目标也是多样性的,即不仅是GT的轨迹也同时引入了更多的正样本,由不同的专家给出的。还是那句话,小米汽车的成功,是小米价值观,小米模式,和小米方法论的成功。

whaosoft~aiotの开发板商城 2368

Ego Planner规划算法(上)——Ego Planner流程介绍

本系列文章用于学习记录Ego-Planner规划算法的相关内容,本系列文章是在在Fast Planner规划算法系列文章的基础上,展开对Ego Planner的相关介绍,主要包括Ego Planner算法流程和程序框架两部分,主要学习资料是深蓝学院的移动机器人运动规划课程

慕羽★的博客 1万+

Ego Planner规划算法(下)——Ego Planner程序框架

本系列文章用于学习记录Ego-Planner规划算法的相关内容,本系列文章是在在Fast Planner规划算法系列文章的基础上,展开对Ego Planner的相关介绍,主要包括Ego Planner算法流程和程序框架两部分,主要学习资料是深蓝学院的移动机器人运动规划课程

慕羽★的博客 7265

为什么需要知识图谱?什么是知识图谱?——KG的前世今生

我的知乎专栏,对知识图谱感兴趣的朋友可以关注。标题的命名顺序可能让有的读者不太习惯。通常在介绍一个陌生事物的应用前,我们先给出其定义。之所以换个顺序,是为了不让读者一开始就接触比较冰冷生硬的概念刻板描述(后面我尽量用更具体、准确的例子来表达),另一方面也是为了通过现实生活中的例子自然的引入知识图谱的概念。希望通过这种方式加深读者的印象和理解。为了减轻读者理解的负担,我尽可能地避免引入过多的概念和技术

SimmerChan的专栏 8万+

TIMER-XL:突破长上下文限制的Transformer时序预测新范式

本文深入解析了清华大学提出的TIMER-XL模型,这是一种突破长上下文限制的Transformer时序预测新范式。它采用Decoder-only架构与补丁级生成技术,并创新性地引入TimeAttention机制,有效解决了传统模型在处理海量历史数据时面临的“上下文瓶颈”问题,显著提升了长序列预测的精度与稳定性。

weixin_28340315的博客 447

SenseVoice 实测,阿里开源语音大模型,识别效果和效率优于 Whisper,居然还能检测掌声、笑声!5分钟带你部署体验

阿里开源语音大模型:语音识别效果和性能强于 Whisper,还能检测掌声、笑声、咳嗽等

u010522887的专栏 4万+

KEGG数据库与KAAS工具:基因功能注释的实用指南

本文提供了使用KEGG数据库与KAAS工具进行基因功能注释的完整实用指南。详细阐述了如何从准备基因或蛋白序列开始,通过KAAS服务器获取KEGG Orthology(KO)编号,并解读注释结果。文章进一步介绍了如何利用KOBAS工具进行通路富集分析,将定性观察转化为定量洞察,并分享了提高注释准确率、处理大数据集及使用API等避坑技巧与高阶方法,为基因组学、转录组学研究者奠定坚实的数据分析基础。

weixin_29292535的博客 398

MuJoCo新手必看:5分钟搞定你的第一个机器人仿真(附Python代码)

本文为零基础新手提供了一份详尽的MuJoCo物理仿真引擎入门指南。从环境搭建、MJCF模型文件解析,到编写Python控制代码,手把手教你5分钟内创建并运行第一个机器人仿真。内容涵盖双摆模型控制与自定义四轮小车构建,帮助你快速掌握这一高性能开源工具,迈出机器人学与强化学习仿真的第一步。

weixin_29032489的博客 658

GLM-4本地部署的实战教程

本文主要介绍了GLM-4-9B本地部署的实战教程,希望对学习和使用大模型的同学们有所帮助。 文章目录 1. 前言 2. 配置环境 2.1 安装虚拟环境 2.2 安装依赖库 2.3 下载模型文件 3. 运行glm-4-9b-chat

herosunly的博客 25万+

2026数学建模国赛D题:时频冲突检测与消解 论文解析

最终无需撤销任何计划,共调整110个计划,其中频移43个、时移48个、间隔调整19个,总调整幅度528。附件模板只要求填写新增计划的频段区间和首次时间区间,没有要求填写宽度、间隔和次数,因此本文按附件中C类装备的统一需求特征处理:每个新增计划占用3个连续频段,持续2个时间单位,间隔8,使用12次。本文针对有限频带内大量周期性用频计划的时频冲突检测、单参数消解、无冲突条件下新增C类计划装载,以及允许调整间隔时长后的再次消解问题,建立了由占用展开、冲突图、组合动作集、精确约束满足和二维资源装箱组成的统一模型。

YOLO 192

传感器校准与标定:传感器的灵敏度校准_(11).常见传感器的灵敏度校准方法

线性校准:适用于传感器输出与输入之间存在线性关系的情况。通过收集数据并使用最小二乘法拟合线性方程,可以确定斜率和截距,从而实现校准。非线性校准:适用于传感器输出与输入之间存在非线性关系的情况。通过收集数据并使用非线性拟合方法(如最小二乘法),可以确定多项式或其他非线性方程的参数,从而实现校准。温度补偿校准:适用于传感器在不同温度下输出特性变化的情况。通过收集不同温度下的数据并使用温度补偿方程,可以提高传感器在不同温度下的测量精度。其他校准方法。

2401_87715305的博客 923

Camera2Demo

在Camera2 的基础上进行封装,使得Camera2一堆初始化逻辑与Activity分离,降低耦合度,完成预览拍照并保存的操作

vb 6.0并口开发操作实例

vb6并口开发,简体中文,试例内有inpout32.dll动态链接库

jre9下载 jdk9

(Java SE Runtime Environment 9.0) 9.0u175 官方最新版 【x86|x64】

上一篇: FastPlanner论文解读(三)——后端时间优化及代码流程
fegxg
博客等级 码龄5年 282粉丝 23原创
评论 1
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付:10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值
⌂ 换网址