EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors（论文学习）

EGO规划器：一种基于ESDF自由梯度的四转子局部规划器

摘要

ESDF地图被广泛运用在局部地图的梯度方向和大小估计之中，但是由于我们在进行轨迹优化的过程中，只用到了ESDF地图中很小的一部分，这造成了极大的计算冗余。在本文中，我们提出了EGO, 其主要改进是通过比较碰撞轨迹和未碰撞轨迹来建立起惩罚项，并将必要的障碍物信息存储起来。如果整条轨迹不满足动力学可行约束，就分配更多的时间。同时，提出了一种各向异性曲线拟合算法，在保持原轨迹形状的同时调整轨迹的高阶导数。(存疑)
本文的贡献如下：
(1). 提出了一种新颖且具有鲁棒性的基于梯度的无人机局部规划方法，实现了实时的估计和计算梯度信息
(2). 提出了一种有效的轻量化的轨迹优化算法，生成了更加光滑的轨迹。
(3). 将该算法整合进入了无人机系统.

避碰力估算

首先，我们生成一个只满足起始终端点约束的B样条曲线,不管他是否满足碰撞约束，然后，对这条轨迹开始逐片段(B样条曲线一般由多条曲线拼接而成)检查是否满足碰撞检测。对于每一条碰撞片段中的控制点Qi, 我们都将为其分配对应的一些{p, v}对,与此同时也生成一条无碰撞的路径.如下图所示:


同时，为了避免在一个障碍物的前后两侧都生成{p, v}对, 计算

我们采用了只要一个控制点满足他的所有合法dij > 0,那这样这个控制点就处在一个新的障碍物中.与此同时，这样的判断操作使得那些只有对最终的轨迹优化有效用的障碍物会被记录，极大的减少了操作时间。
基于ESDF地图的局部规划器由于ESDF地图不全或者错误的信息，导致陷入局部极小值，最终生成不合格的路径，如下图所示:

而上文提出的方法明确的设计了对对应障碍物的排斥力，且初始化了一个无碰撞路径，因此可以有效处理上述方法的缺陷，同时本文提出的方法不需要对起始点进行无碰撞初始化。

基于梯度的轨迹优化

A. 问题构建

因为我们生成的轨迹是B样条曲线，关于B样条曲线的性质不多赘述，我们可以利用他的凸包特性，确保控制点的凸空间中来保证整条轨迹在凸空间中，我们构造代价函数为:

Js:轨迹的平滑惩罚
Jc:碰撞惩罚
Jd:可行性惩罚
(1) Js：本文中结合了加速度和加加速度的累计和

(2) Jc: 当任意控制点的dij < sf(安全距离)时存在，具体如下:

我们将所有的控制点Qi对应的dij对应的碰撞惩罚累加


在本文中，我们直接对Qi进行求导来获得梯度值.

(3) Jd: 可行性的保证是通过限制高阶微分的轨迹曲线来得到实现的，由于本文的轨迹是B样条，所以可以利用B样条曲线的递归性来实现对高阶微分轨迹的计算。(也就是说，可以通过限制控制点的微分来限制整条曲线).

B. 数值优化

我们构建的损失函数方程具有两个特点,一是损失函数总是动态的随着环境的变化而变化，因此需要我们快速的再启动规划器,二是再损失函数中，二次项占了绝大多数，使得整个损失函数具有类二次性质，因此我们可以通过使用海森矩阵的方式加速他的收敛来获得极值,但是获得海森矩阵的逆矩阵需要极大的计算量，因此我们采用似牛顿法来计算。

时间再分配与轨迹优化

在生成避障轨迹时，规划器对最终轨迹的信息一无所知，因此，分配定额时间来进行规划这一方案是不合理的，也不利于保证轨迹的动态可行性，对轨迹中的各段的时间分配问题，我们通过计算对应段中最大的速度，加速度和加加速度和其对应的极限的比值的，对应比例的分配各段的时间。



而对轨迹的最后优化（好吧这段是真的没看懂）

一些细节

本文采用A方法进行搜索并生成无碰撞路径，使用A的好处是搜索得到的路径会天然的靠近障碍物。对于

中的Ri向量，其计算公式为:

Ri是控制点Qi处的切向量,绿色平面是垂直于Ri,pij是绿色平面和障碍物相交得到的障碍物表面点，v是的含义个人理解感觉就是Qi - pij,只不过利用,利用方向的是否相同来获得唯一的{p, v}点对。

结论

本文探讨了梯度规划器使用ESDF地图的必要性并提出了EGO算法，并获得了极好的性能.

个人理解

本文没有使用并构建ESDF地图(前几次读的时候以为只是构建了必要的ESDF地图),首先初始化一条只满足终端约束的B样条曲线和A*算法搜索得到的无碰撞路径图,因为B样条曲线是具有凸包的特性,所以只要保证控制点空间不与障碍物相撞就可以保证整条曲线的安全性,所以计算得到控制点和障碍物的距离信息并保存(dij的虽然是一个平方数，但是他的模值仍然是距离)，在保证了唯一性的同时，也保证了信息的准确性。然后就是构建损失函数，因为B样条曲线的递归性，可以利用低阶的B样条曲线满足高阶微分的曲线的需求。梯度值的计算不使用过微分的方式而是通过低阶控制点相减的方式得到的，之后就是对曲线的细节优化部分(好吧这部分没具体看懂)。