EGO-Planner详解：一种创新的无人机轨迹规划算法

创作时间:

作者:

@小白创作中心

EGO-Planner详解：一种创新的无人机轨迹规划算法

引用

CSDN

https://m.blog.csdn.net/weixin_45534376/article/details/137781651

EGO-Planner是浙江大学高飞老师FAST-Lab团队提出的一种基于梯度的无人机局部轨迹规划算法。该算法通过直接从障碍物中评估和投影梯度信息，避免了传统方法中预先构建ESDF地图的高计算成本。本文将详细介绍EGO-Planner的核心技术原理和创新点。

1. Introduction

基于梯度的运动规划是UAV局部轨迹生成的主流方法，将无人机局部轨迹生成问题归结为无约束非线性优化问题。许多轨迹规划算法直接利用ESDF地图中丰富的梯度信息优化轨迹。

然而，传统的基于梯度优化的轨迹规划方法存在一个显著问题：需要先建立ESDF地图，然后再利用ESDF地图中的梯度信息对轨迹进行优化。这种做法在EWOK一文中被指出存在建图时间过长的问题。

EGO-Planner的主要贡献包括：

提出了一种新的基于梯度的四旋翼局部规划方法，直接从障碍物中评估和投影梯度信息，避免了预先构建ESDF地图的需要。
开发了一个轻量级的轨迹细化算法，能够产生更平滑的轨迹，通过制定各向异性的错误惩罚来优化轨迹拟合。

2. 避障估计

避障轨迹的形成过程可以分为四个步骤：

第一步：生成一条只满足终端约束而不考虑避障的B样条曲线Φ。
第二步：迭代检测每个碰撞段，使用图搜索算法（论文中提到使用A*算法）生成一条无碰撞路径Γ。
第三步：在轨迹Φ的每个控制点Qi处，计算切向量Ri（使用(Qi+1 - Qi-1) / 2Δt计算），并构造一个垂直平面Ψ。该平面与无碰撞路径Γ相交形成线l，l与障碍物j的表面相交于点pij，同时获得向量vij（Qi指向pij的单位向量），形成(p,v)对。
第四步：根据(p,v)对定义距离场dij = (Qi - pij) ⋅ vij。dij大于0表示不在该障碍物j内。

为了减少迭代过程中产生的重复{p,v}对，算法还引入了一个判断标准：如果控制点Qi处于障碍物中时，并且对于当前得到的所有障碍物j满足dij > 0，则认为障碍物为新发现的障碍物，从而只计算影响轨迹的障碍物信息，减少运行时间。

3. 基于梯度的轨迹优化器

轨迹优化主要采用B样条轨迹进行平滑度和最小损失的优化。根据四旋翼的微分平坦性，优化问题被定义为：

minQ J = λsJs + λcJc + λdJd

其中，Js为平滑项惩罚，Jc为碰撞项惩罚，Jd为动力学可行性惩罚，λ为惩罚项的权重。

3.1 平滑度惩罚

根据B样条的两个性质（凸包和导数性质），通过最小化B样条路径轨迹的二阶和三阶控制点的平方和来减少加速度和加加速度的平方和：

Js = ∑i=1Nc-2 ||Ai||22 + ∑i=1Nc-3 ||Ji||22

3.2 碰撞项惩罚

碰撞惩罚使控制点远离障碍物，通过采用安全间隙和惩罚控制点dij < sf来实现。论文设计了一个二次连续可微惩罚函数，随着dij的减小而抑制其斜率：

Jc = ∑i=1Nc jc(Qi)

与传统方法不同，本文直接用惩罚函数对控制点求导获得梯度。

3.3 可行性惩罚

可行性惩罚（也称为动力学惩罚）通过限制轨迹在每一维上的高阶导数来保证其可行性。根据B样条曲线的性质，对控制点的导数进行约束足以约束整个B样条。

3.4 数值优化

优化目标函数为：

minQ J = λsJs + λcJc + λdJd

由于轨迹执行过程中会随着新障碍物的加入不断改变目标函数，因此要求求解器能够快速重启并求解。考虑到目标函数主要由二次项组成，Hessian信息能够加快收敛速度。但是精确的Hessian矩阵求解会消耗大量计算资源，因此采用拟牛顿法（quasi-Newton methods）从梯度信息来近似计算Hessian。