Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight (论文学习)

快速自主飞行中鲁棒高效的四旋翼轨迹生成

摘要

在本文中，我们提出了一种有效且鲁棒的3D环境中的四旋翼运功规划系统.我们在离散化的控制空间中，采用动态路径搜索方法寻找安全，动态可行，时间最短的初始轨迹.利用B样条函数的凸包特性，结合欧氏距离场（EDF）的梯度信息和动态约束，通过B样条函数优化轨迹，提高了轨迹的平滑度和间隙。最后，将最终轨迹表示为非均匀B样条曲线，采用迭代时间调整方法来保证轨迹的动态可行性和非保守性。与其他的方法相比，本文提出的方法能够以更高的成功率在窄小的环境中生成高质量的轨迹路径。
本文的贡献如下:
(1) 提出了一种鲁棒有效的系统方法，将动态路径搜索、B样条优化和时间调整相结合，自下而上建立安全性、动态可行性和敏捷性。
(2) 本文提出了一种基于B样条凸包性质的优化公式，它巧妙地引入了梯度信息和动态约束
(3) 研究了导数控制点与非均匀B样条时间分配的关系。为了保证运动的可行性和非保守性，采用了基于关系的时间调整方法。

动力学路径搜索

A. 生成基元

无人机的性质(四转子系统的微分平坦度)允许我们可以用含t的一元多项式函数来表示轨迹,

我们设状态变量:

控制变量:

整个状态空间模型为:

对上述状态空间方程得到的闭式解为:

由上式可知，我们只需要给出给定的轨迹初始状态和控制变量，就可以得到未来的轨迹.
因此我们可以通过离散化控制空间(即枚举不同给定控制变量区间里的值)，来获得不同的未来轨迹
其中控制量的取值集合为：

下图为r = 1, μ = 2是的运动基元的示意图

B. 实际代价和启发式代价

轨迹的实际代价gc我们定义为:

该式表示在每个运动基元上输入的控制量的平方*持续时间的累计和
同时，我们通过庞特里亚金最小化原理来设计启发式函数(具体讲解请参照高飞的深蓝学院的路径规划课程OBVP部分)
说说我个人理解为什么要用庞特里亚金最小化原理来设计启发式函数，我们设计启发式函数，是为了能够在最小化代价的时候，引导整条规划轨迹前往目标点。关于庞法的通式如下:

我们可以看到这个式子和我们设计的损失函数非常的相似,前者对应启发式函数，后者对应实际代价函数

而汉密尔顿方程和协方差向量如下所示:

庞法的大致思想是，只要我们知道了当前的状态和我们想要达到的状态，这样最后我们得到的损失函数J就是一个关于t的单变量函数，然后通过极值化操作就可以最小化我们设定的损失函数.
最后庞法得到的结果是:.

大致流程就是,我们可以通过汉密尔顿方程算出协方差向量，然后通过协方差向量算出我们可以得到的最优控制信号，然后因为控制信号与其他变量的关系，我们可以通过一次或者多次积分来获得其余的状态变量，然后通过带入当前和最终的状态变量，可以将所有的状态变量函数化为只和t相关的单变量函数,然后，再将状态变量带入代价函数中，就可以找到一个最佳的时间T*从而获得一个最小的代价.

位置p的表达函数

最终的代价表达式，未带入α, β

C. 分析扩展点

弹出一个节点，如果在当前的节点我们就可以获得比较不错的代价约束，我们就不再去迭代的更新搜索路径，直接获得与终点的链接,这一操作极大的提升了在空旷地区的搜索效率.

上图中紫色的路径就是跳过了运动基元生成后获得的路径，增加了在空旷地区的搜索效率

D. 最优性和完备性

实际中取得了不错的效果

B样条轨迹优化

由于我们在搜寻轨迹的时候忽略了地图信息，导致我们获得的路径有时会十分靠近障碍物,因此，我们结合EDF的梯度信息和动态约束，并利用B样条的凸包特性，从而生成一条光滑的，安全的，且符合动力学约束的轨迹。

A. 均匀B样条曲线

在我们通多kindodynamical的路径搜索方法获得了从起点到终点的路径之后，因为我们希望能够利用b样条的性质来优化这一条搜索到的曲线路径，因此，我们通过对这条kindo_path进行均匀的采样，获得采样点，我们新生成的b样条曲线需要穿过这些采样点，而b样条的计算公式如下:

然后通过求解方程我们可以得到我们所需要的b样条的控制点.

B. 凸包性质

因为B样条轨迹一定存在于B样条控制点构成的凸包多边形,所以保证这个多边形和障碍物没有碰撞即可.因此，我们需要对控制点Q进行约束控制

dh代表的任何一个处在凸包多边形中的控制点, 满足三角原则:

这样就可以保证轨迹无碰撞

C. 问题构建

定义损失函数为:

其各项分别为:

fs:光滑项

fc:碰撞项(斥力)


d(Qi)表示的是Qi和最近的障碍物的距离, dthr代表阈值，安全距离

fv, fa: 动力学可行项

时间调整

为了进一步确保轨迹的动力学可行性，我们采用了一种基于导数控制点与非均匀B样条时间分配（节点跨度）关系的时间调整方法。

A. 非均匀B样条

非均匀B样条的速度轨迹和加速度轨迹的控制点分别为：

对于轨迹的可行性，我们只需要保证一阶和二阶轨迹曲线的可行性就可以了.

B. 节点跨距调整

假设速度曲线的控制点超过了可行性的约束，我们发现只需要控制最初控制点之间的节点向量的间隔，就可以把超过速度阈值的部分减少。


也即，我们为非均匀B样条的控制点乘上一个系数，即:

同理类似于对加速度轨迹的控制点.

C. 迭代的调节时间分配

对生成的轨迹进行遍历，发现如果有速度和加速度的控制点不符合可行性约束的，就对应的调整B样条曲线所对应的节点间距.流程如下:

实施细节

A. 采用滚动视角的局部规划

(1) 路线的规划只存在于已知环境中，位置环境的规划只会浪费时间
(2) 规划触发机制: 当所处路径上出现了新的障碍物或者在固定的时间间隔会被调用

B.EDF地图

本文使用了EDF地图来获得所需要的地图信息和梯度信息,同时为了解决建立ESDF地图过于费时的缺点，我们只增量式的更新局部的视野内的EDF地图

结论

本文将路径规划的问题分解成为了前端:利用kindodynamic的方法搜索一条安全，符合动力学约束且耗时最小的一条路径.后端:利用b样条曲线的梯度信息去优化轨迹的平滑度，间隙.这两个部分.极好地利用了b样条曲线的凸包性质，与前人的工作相比,更具效率和鲁棒性.

整体流程理解

算法流程如上图所示，总体来说，是在每一个点处计算运动基元，然后再迭代的在运动基元中的点去计算运动基元，在这个过程中同时使用A算法去计算并搜索路径。然后利用b样条曲线来对已经获得的A路径进行优化，