无人机轨迹优化——Minimum Snap

一、背景意义

  无人机在执行一段飞行任务或者在一段简单的飞行中应用到轨迹规划器，所飞行的轨迹应该是平滑的、安全的、动态可行的。然而，无人机通过路径规划器得到的轨迹是一段段折线轨迹，而且有时路径规划的算法（如A*、RRT、RRT*等）未考虑到机体本身的大小、执行轨迹的能力和环境的因素，使得轨迹非常靠近障碍物，实际飞行中可能会碰撞障碍物。这时，需要对得到的轨迹进行二次优化，使得轨迹能够适合无人机的自主移动，不会产生速度、高阶动力学状态突变，同时也可以节约飞行时的能源。

二、前提

  轨迹一般用n阶多项式进行表示，即：

其中有p0到pn共（n+1）个轨迹参数，设参数向量p=[p0,p1...,pn]可以将其写成向量的形式：

接下来对于任意时刻t，可以根据参数计算出轨迹的位置P，速度v，加速度a，加加速度jerk，加加加速度snap等。

接着对轨迹按时间进行分段，这样更好表达一段复杂的多项式，每段各用一条多项式曲线表示：

说明，在实际问题中轨迹往往在二维、三维甚至高维，这时需要对每个维度进行单独求解。

三、Minimum Snap Optimization

 在进行轨迹优化时往往需要轨迹满足一系列约束，比如：得到轨迹需要满足相邻连接处平滑（位置、速度、加速度等连续），轨迹需要满足经历某些路径节点，轨迹需要满足在规定的走廊范围内等。然而，满足约束的轨迹有无数条，实际问题中，只需要一条最优的轨迹，因此需要构建一个最优函数，将问题建模成一个约束优化问题，形如：

其中f(p)为需要优化的函数，s.t.为等式约束和不等式约束。这样可以通过最优求解得到轨迹参数p。Minimum Snap顾名思义就是最小化加加加速度，即

在无人机中Snap表示差动推力，也就是在给定的时间内平滑地通过关键帧，同时保持在安全走廊内。

四、基本步骤

1.将轨迹按照时间进行分段

将无人机的状态用m个时间间隔的n阶分段多项式函数表达：

需注意，时间应该怎样分配才能获得更加的轨迹，常用的方法有匀速分配或梯形分配，假设每段轨迹内速度满足匀速或梯形速度变化，根据每段的距离将总时间T分配到每段。另外，对于初始分配的轨迹和时间，在迭代算法中，如果不满足安全走廊约束和可行性约束，可以通过插值或者增大某一段时间等进行重新分段和分配时间。

  2.构建需要优化的函数

四旋翼无人机的动力学是差分平坦的，也就是说其状态和输入可以写成四个精心（x，y，z，Ψ）选择的平坦输出及其导数的代数函数。这有利于轨迹的自动生成，因为在平坦输出空间中的任何平滑轨迹(具有合理有界导数)都可以跟随欠驱动四旋翼飞行器。因此，无人机最小化函数通过最小化位置平方的kr阶导数和偏航角平方的kΨ阶导数：

其中μr和μΨ为常数，这里σT=[XT,YT,ZT,ΨT]的装置，σi=[Xi,Yi,Zi,Ψi]的装置

然后将其二次规划QP问题：

进行一个简单的推导来解释转化为QP问题：

3. 添加约束条件

①设定某点的位置、速度、加速度等为一个特定指，比如起点和终点的p=某值、v=0、a=0，这样可以构成等式约束。其中p、v、a由第二节前提的公式提供。

②相邻两端之间的连续性约束，可以表达为在端点处，其函数的导数等于0。例如第i、i+1段的位置连续构成的等式约束为：

③安全走廊约束:把ti定义为从ri到ri+1段的单位向量。与段i的垂直距离向量di(t)被定义为：

定义走廊的宽度为：

通过引入nc个中间点，可以将这个约束融入到QP中

 ④其他特定的约束，比如无人机在执行某个任务时，需要多大的距离来执行，飞行中需要避开某个轨迹节点等等。

4. 求解

  将优化函数、不等式约束、等式约束和某些特定约束利用QP求解器进行求解得到相应的轨迹。对轨迹还需一个初步判断，是否符合要求，是否能满足无人机控制需求等。

 

五、参考文献

[1] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom., Shanghai, China, May 2011, pp. 2520–2525.