多项式轨迹

对于空间中的两个点，可以使用直线进行拟合，对于三个点可以使用二次函数进行拟合。

对于一段一维轨迹，并且以一个五次多项式去拟合它：

P(t)=P0*t0+P1*t1+...+P5*t5

上式中有6个未知的系数P0~P5，如果要求解出它，至少需要6个方程。一个很明显的思路是可以取6个位置点进行拟合。多数情况下不光关心机器人的位置，对于它的速度和加速度同样很关心，因此如果选取轨迹头和尾的位置以及速度和加速度，那么也可以唯一确定这个多项式，而且机器人还能以想要的速度和加速度开始和结束。

对于一个经过多个中间航迹点的轨迹，分别使用多个轨迹段进行拟合，如T 0 − T 1 − T 2 − T 3 这条轨迹，将其分为三段分别进行轨迹拟合，分别拟合轨迹段T 0 − T 1 , T 1 − T 2 , T 2 − T 3并且每一段轨迹的拟合沿用一段轨迹拟合的思路。

但是与一段轨迹拟合有一些区别，多数情况下，很难去给定机器人在经过中间航迹点的速度、加速度、Jerk（加速度的导数，表示加速度变化快慢一般用于表示这条轨迹的舒适度）等信息，于是就认为中间点的速度、加速度等信息是一个可以自由变化的状态，那么对于这种中间状态不确定的问题，可以构建一个优化问题，使得中间状态取得某一个值时，制定的代价函数达到最小值，那么一方面就不去设计机器人以什么样的状态通过中间点，另一方面还可以通过优化代价函数得到一个最优的轨迹。

Minimum snap

代价函数

类似于上图的轨迹，它是一个一维的轨迹，横轴是时间t ，纵轴是位置P m ( t ) ，以N 次多项式来拟合每一个分段轨迹，那么对于第m 段轨迹:

那么，这个多项式就有N + 1个未知系数，对其分别进行一次求导、二次求导、三次求导、四次求导可分别得到速度函数、加速度函数、jerk函数、Snap函数：

对于最优的轨迹生成问题，构建一个如下的代价含数J m，对应的是一段多项式轨迹的四阶导数(Snap)平方的积分。

具体公式推导原文有，不予过多赘述，通过化简可以将其可以写成二次规划 PMTQMPM的形式。

其中PM=[p0,p2,....pn]为(n+1)*1的矩阵，Qm为(n+1)*(n+1)矩阵。

假设有M段轨迹，那么每一段轨迹都是一个高阶的多项式函数，写出代价函数和如下：

其中Pi（大写的P）表示在第i段轨迹中每个参数的集合，[P0,P1,....PM]T为(M+1)*(n+1)的矩阵，Qi表示第i段轨迹中的Q矩阵。

到此以多项式为基础推导出了代价函数的具体形式，如果直接对上式进行优化求解得到其最小值，看似确实完成了的目标，但是其中忽略了一个问题，那就是可能优化出来的结果是，航迹点之间的速度或者加速度不连续，显然这是可能的，因为没有限制航迹点在前后两端轨迹中的状态是否一致，也就没有限制要连续。但是机器人想要平滑的运动，前后状态一定是要连续的，因此需要加一些额外的约束去限制连续性。

连续性约束

首先，需要限制中间的航迹点在两段轨迹的接合处左右两边状态连续。如果要最小化的目标是Jerk，那么必须保证接合处三阶导数是连续的。因为要对Jerk进行积分，那么多项式在求导3次之后必须要连续，所以要确保0阶、1阶、2阶导数都是光滑的，也就是保证求导到加速度函数依然是光滑的，这一点就体现了多项式轨迹的优势，它天然就是一个高阶光滑的函数。

即轨迹段Pm 和Pm + 1在Tm 时刻具有相同的k 阶导数。

微分约束

连续性约束保证了轨迹的光滑性。虽然限制了轨迹的光滑性，但是并没有限制轨迹必须通过某一些点，或者以一个确定的状态通过某一些中间的航迹点。也就是上述的这些多项式系数中，有一些是已知的。但是大多数情况下，都是对轨迹的位置有一些限制的，并不能随意生成，所以还需要给中间的航迹点加一些限制。包括位置、速度、加速度、Jerk、Snap等等的限制。

首先考虑很实际的问题，机器人的起点和终点肯定事先是知道的，并且机器人以什么样的速度或者加速度开始，也是可以设置的，机器人的终点速度和加速度(都为0)也是可以事先给定的。机器人中间航迹点的位置也是需要事先给定的(往往都是上一步搜索算法离散出来的点，例如A star，RRT satr)，但是机器人中间点的速度和加速度等信息，没法去约束它，因为很难去确定机器人到达某一个点的速度或者加速度，所以对于中间点的速度和加速度，或者更高阶的Jerk甚至Snap等，不对其进行微分限制，让它们只具有连续性约束，然后让优化器给它们分配最优的状态，同时还需要限制其值，避免越界。

起始点和终点的位置、速度、加速度等使用确定值限制，中间航迹点使用位置限制，写成数学等式：

将连续性约束和微分约束进行合并，可以得到一个带约束的QP优化问题，将其带入第三方求解库就可以得到一个全局最优解。

闭式求解

提出了将系数通过一个映射矩阵转换成轨迹端点的导数，也就是说不再优化轨迹的系数，而是对轨迹在端点处的位置、速度、加速度、Jerk等进行优化。

以最小化Jerk为例，那么根据前面的结论，需要构建次数为5次的多项式，那么它将有6个未知的系数，需要提供轨迹两端的位置、速度、加速度约束：

只需要将分段轨迹两端的时间都带入上式，就可以找到多项式系数到运动微分的映射关系：

对于上式提到的Min PMTQPM,将Pm=AM-1dm代入可得，Min dTA-TQA-1d

上式替换，实际上已经微分约束给带入到了代价函数中，但是连续性约束还没有引入到代价函数中。引入一个permutation matrix (置换矩阵)C来将连续性约束进行引入到代价函数中。C 矩阵本身是一个只包含0和1的矩阵，它的作用就是给所有[ d 0 d 1 ⋯ d M ] 进行一个组合，将固定的变量放在头部，将需要优化的变量方位尾部，并且对于连续性约束的变量，由于他们的值是相等的，因此选择其中一个来表达连续性约束。

以最小化Jerk为例，假设有如下一条轨迹，它有两个分段轨迹，它的C矩阵如下：

Dki,j，这里分清楚k、i、j三个下标的含义。k是导数的阶数，0是位置，1是速度，2是加速度;i是时间点，0和T分别代表起点和终点;j是分段轨迹的编号，0和1对应第一段和第二段。

将化简的代入代价函数，则最终可以得到：

对CA-TQA-1CT=R根据dF和dP的尺寸进行分块，则得到如下变换：

其中Q是对称矩阵，CA-TQA-1CT=（CA-TQA-1CT）T为对称矩阵，则RPF=RTFP

将J进行化简得：J=dTF​RFF​dF​+2dTF​RFP​dP​+dTP​RPP​dP​

对于上式要求的变量是dP，因此将其对dP 求导，并令导数为0时取极值点的值对应的就是最优的dP ∗

求解出来了dP ∗之后，只需要将它带入C和A就能求出最终的各个多项式的系数：

当然优化过程存在避障问题，minimumsnap函数并没有考虑触碰到障碍物的情况，下一次会结合硬约束贝塞尔曲线优化来进行避障优化。

原博客：

【附源码和详细的公式推导】Minimum Snap轨迹生成，闭式求解Minimum Snap问题，机器人轨迹优化，多项式轨迹路径生成与优化-CSDN博客