EGO-Planner详解(1)之论文解读_ego-planner避障算法

本系列文章主要解析浙大高飞老师FAST-Lab团队的文章EGO-Planner，该算法作为现在无人机的主流轨迹规划算法值得详细解读，笔者利用自己有限的理解力做一个笔记摘录，若有问题请各位大佬评论区指正。本文主要解析论文部分。
原代码和论文

1. Introduction

基于梯度的运动规划是UAV局部轨迹生成的主流方法，将无人机局部轨迹生成问题归结为无约束非线性优化问题。许多轨迹规划算法直接利用ESDF地图中丰富的梯度信息优化轨迹。
本文争对传统的基于梯度优化的轨迹，是先建立ESDF地图然后再利用ESDF地图中的梯度信息对轨迹进行优化，但是在EWOK一文中给出了整个一个轨迹规划流程中的时间分布如下图，建图占用时间过大。本文旨在只在必要时计算梯度，避免在与局部轨迹无关的区域计算ESDF。

主要贡献有下：

1. 一种新的基于梯度的四旋翼局部规划方法，直接从障碍物中评估和投影梯度信息，而不是预先构建的方法。
2. 轻量级的轨迹细化算法，产生更平滑的轨迹，制定各向异性的错误惩罚的轨迹拟合问题。

2. 避障估计

避障轨迹的形成过程：
第一步，生成一条只满足终端约束不考虑避障的B样条曲线Φ；
第二步，迭代检测每个碰撞段，用图搜索算法(论文结尾提到用的A*)生成一条无碰撞路径$\Gamma$(图a)；
第三步，轨迹Φ上控制点$Q_i$的切向量定义为$R_i$，过控制点$Q_i$切向量$R_i$(直接用$(Q_{i+1}-Q_{i-1})/2\Delta t$计算)的垂直平面$\Psi$与$\Gamma$相交形成线l，l与障碍物j的表面相交于点$p_{ij}$，同时获得向量$v_{ij}$，v是Q指向p的单位向量，形成(p,v)对(图b)；
第四步，根据(p,v)对定义距离场$d_{ij}=(Q_i-p_{ij})\cdot v_{ij}$（论文中没有讲清楚这个定义，此时的$Q_i$不是原本的控制点，而是想要寻找的避障的新控制点，而v固定不再改变），$d_{ij}$大于0表示不在该障碍物j内。

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

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

轨迹优化主要用的也是B样条轨迹进行平滑度和最小损失的优化，可以参考FAST-Planner的笔记，根据四旋翼的微分平坦性，优化问题被定义为：
$$\underset{Q}{\min }J={\lambda }_s{J}_s+{\lambda }_c{J}_c+{\lambda }_d{J}_d$$

与FAST-Planner一样$J_s$为平滑项惩罚，$J_c$为碰撞项惩罚，$J_d$为动力学可行性惩罚，$\lambda$为惩罚项的权重。

3.1 平滑度惩罚

根据B样条的两个性质：1.凸包；2.pb阶B样条的k次导数为pb-k阶B样条。只要最小化B样条路径轨迹的二阶和三阶控制点的平方和就能有效减少加速度和加加速度的平方和(这里可参考minimum snap)：
$$J_s=\sum _{i=1}^{N_c-2}||A_i|{|}_2^2+\sum _{i=1}^{N_c-3}||J_i|{|}_2^2$$

3.2 碰撞项惩罚

碰撞惩罚使控制点远离障碍物，通过采用安全间隙和惩罚控制点$d_{ij}<s_f$来实现，下式是该论文设计的一个二次连续可微惩罚函数，随着$d_{ij}$的减小而抑制其斜率，这里的$d_{ij}$是第二节的控制点距离场，$s_f$为给定的安全间隙

对所有控制点的惩罚求和得到总的碰撞项惩罚：
$$J_c=\sum _{i=1}^{N_c}{j}_c(Q_i)$$

相比于传统用三线性插值的方法求碰撞项的梯度，本文直接用惩罚函数对控制点求导获得梯度：

3.3 可行性惩罚

也可称为动力学惩罚，受限于物理的速度、加速度、加加速度的极限，在计算轨迹时，要设定可行性惩罚，通过限制轨迹在每一维上的高阶导数来保证其可行性。根据B样条曲线的性质，对控制点的导数进行约束足以约束整个B样条。

3.4 数值优化

本优化目标函数是
$$\underset{Q}{\min }J={\lambda }_s{J}_s+{\lambda }_c{J}_c+{\lambda }_d{J}_d$$

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

4. 时间重分配以及轨迹进一步优化

• 与FAST-Planner类似，计算极限超标率(超过限制的最大比例)
  r e = max ⁡ { ∣ v i , r / v m ∣ , ∣ A j , r / a m ∣ , J k , r / j m 3 , 1 } r_e=\max\{|v_{i,r}/v_m|,\sqrt{|A_{j,r}/a_m|},\sqrt[3]{J_{k,r}/j_m},1\} re​=max{∣vi,r​/vm​∣,∣Aj,r​/am​∣ ​,3Jk,r​/jm​ ​,1}
• 得到新的时间间隔$\Delta t^{\prime }=r_e\Delta t$
• 通过求解一个如下的闭式的最小二乘问题，在速度及其导数的约束下初始生成时间跨度为$\Delta t^{\prime }$的轨迹${\Phi }_f$，同时保持与${\Phi }_s$相同的形状和控制点数，然后重新计算光滑度惩罚和可行性惩罚得到新的目标函数：
  $$\underset{Q}{\min }{J}^{\prime }={\lambda }_s{J}_s+{\lambda }_d{J}_d+{\lambda }_f{J}_f$$
• ${\lambda }_f$是适应度惩罚权重，$J_f$被定义为从${\Phi }_f(\alpha T’)$到${\Phi }_s(\alpha T)$各向异性位移的积分($\alpha \in [0,1]$)，其中T和T’为轨迹${\Phi }_s$和${\Phi }_f$的持续时间。
• ${\Phi }_s$是第3节中优化获得的无碰撞曲线(安全轨迹)，利用低权重的轴向位移$d_a$来放宽光滑度调整，用高权重的径向位移$d_r$来防止碰撞。
  
• 优化${\Phi }_f$轨迹来拟合轨迹${\Phi }_s$同时调整平滑度和可行度。椭圆球上的$J_f$惩罚相同。
  
  $$J_f={\int }_0^1[\frac{d_a(\alpha T^{\prime }{)}^2}{a^2}+\frac{d_r(\alpha T^{\prime }{)}^2}{b^2}]d\alpha$$
  
  参考博客