多项式轨迹表示
路径规划可以得到一系列路径点，而轨迹则是时间的函数，可以使用n阶多项式表示：
\[
\begin{aligned}
f(t) &=p_{0}+p_{1} t+p_{2} t^{2}+\cdots+p_{n} t^{n} \\
&=\sum_{i=0}^{n} p_{i} t^{i}
\end{aligned}
\]
写成向量形式：
\[
f(t)=\left[\begin{array}{llll}
1 & t & \cdots & t^{n}
\end{array}\right]\left[\begin{array}{c}
p_{0} \\
p_{1} \\
\vdots \\
p_{n}
\end{array}\right]
\]
对轨迹函数求导，可以得到速度、加速度、jerk等参数随时间变化的函数：
\[
\begin{aligned}
\operatorname{vel}(t) &=f^{(1)}(t)=\left[\begin{array}{llllll}
0 & 1 & 2 t & 3 t^{2} & \cdots & \frac{n !}{(n-1) !} t^{n-1}
\end{array}\right] \cdot p \\
\operatorname{acc}(t) &=f^{(2)}(t)=\left[\begin{array}{llllll}
0 & 0 & 2 & 6 t & \cdots & \frac{n !}{(n-2) !} t^{n-2}
\end{array}\right] \cdot p \\
\operatorname{jerk}(t) &=f^{(3)}(t)=\left[\begin{array}{llllll}
0 & 0 & 0 & 6 & \cdots & \frac{n !}{(n-3) !} t^{n-3}
\end{array}\right] \cdot p \\
p &= \left[ \begin{array}{} p_0 & p_1 \cdots p_n \end{array} \right]^T
\end{aligned}
\]
概括得到轨迹的k阶通用导数：
\[
f^{(k)}(t)=\left[\begin{array}{}
\overbrace{ \begin{array}{}0 &{0} &{\cdots} &{0} \end{array} }^{k} & \overbrace{ \begin{array}{} \frac{(k+0) !}{0 !} t^{0} &{\frac{(k+1) !}{1 !} t^{1}} &{\frac{(k+2) !}{2 !} t^{2}} &{\cdots} &{\frac{n !}{(n-k) !} t^{n-k}} \end{array} }^{n-k+1}
\end{array}\right] \cdot p
\]
Minimum-jerk
Minimum-jerk，顾名思义就是求解每段轨迹的系数p使得总的jerk最小，同时还要满足约束条件，所以这是一个带约束的最优化问题。
jerk是轨迹\(f(t)\)的3阶导数，所以令\(k=3\),Minimum-jerk会直接对首尾的位置、速度和加速度进行约束，这就有6个等式约束了，所以优化参数必须提供6个以上的自由度，而5阶多项式有6个系数，所以符合要求的多项式的最小阶次为\(n = order = 5\).因此可以选择五阶多项式表示轨迹：
\[
f(t)=\left\{\begin{array}{cc}
p_{1,0}+p_{1,1} t+p_{1,2} t^{2}+p_{1,3} t^{3}+p_{1,4} t^{4}+p_{1,5} t^{5} & T_{0} \leq t \leq T_{1} \\
p_{2,0}+p_{2,1} t+p_{2,2} t^{2}+p_{2,3} t^{3}+p_{2,4} t^{4}+p_{2,5} t^{5} & T_{1} \leq t \leq T_{2} \\
\vdots & \\
p_{M, 0}+p_{M, 1} t+p_{M, 2} t^{2}+p_{M, 3} t^{3}+p_{M, 4} t^{4}+p_{M, 5} t^{5} & T_{M-1} \leq t<T_{M}
\end{array}\right.
\]
包含M段多项式，每段有\(n + 1 = 6\)个参数。
目标函数
Minimum-jerk使整个轨迹jerk最小即是是jerk积分最小，选择jerk 2范数最小：
\[
\min _{p} f^{(3)}(t)=\min _{p} \sum_{i=1}^{M} \int_{T_{i-1}}^{T_{i}}\left(f^{(3)}(t)\right)^{2} d t
\]
定义目标函数：
\[
J(p) = \sum_{i=1}^{M} \int_{T_{i-1}}^{T_{i}}\left(f^{(3)}(t)\right)^{2} d t
\]
令\(a = \left[\begin{array}{}0 & 0 & 0 & 6 & \cdots & \frac{n !}{(n-3) !} t^{n-3}\end{array}\right]^T\). 对\(f^{(3)}(t)\)求平方：
\[
\begin{aligned}
\left(f^{(3)}(t)\right)^{2} &\left.=\left(\begin{array}{llllll}
0 & 0 & 0 & 6 & 12 t & 20 t^{2}
\end{array}\right] \cdot p\right)^{T}\left(\left[\begin{array}{llllll}
0 & 0 & 0 & 6 & 12 t & 20 t^{2}
\end{array}\right] \cdot p\right) \\
&=\left(a^{T} p\right)^{T}\left(a^{T} p\right) \\
&=p^{T} a a^{T} p
\end{aligned}
\]
令\(A(t) = aa^T\):
\[
A=a a^{T}=\left[\begin{array}{c}
0 \\
0 \\
0 \\
6 \\
12 t \\
20 t^{2}
\end{array}\right]\left[\begin{array}{llllll}
0 & 0 & 0 & 6 & 12 t & 20 t^{2}
\end{array}\right]=\left[\begin{array}{cccccc}
0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 36 & 72 t & 120 t^{2} \\
0 & 0 & 0 & 72 t & 144 t^{2} & 240 t^{3} \\
0 & 0 & 0 & 120 t^{2} & 240 t^{3} & 400 t^{4}
\end{array}\right]
\]
因此积分：
\[
\begin{aligned}
\int_{T_{i-1}}^{T_{i}}\left(f^{(3)}(t)\right)^{2} d t &=\int_{T_{i-1}}^{T_{i}} p^{T} A(t) p d t \\
&=p^{T} \cdot \int_{T_{i-1}}^{T_{i}} A(t) d t \cdot p
\end{aligned}
\]
令\(A(t)\)的积分矩阵Q：
\[
Q=\int_{T_{i-1}}^{T_{i}} A(t) d t=\left[\begin{array}{cccccc}
0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 36 t & 36 t^{2} & 40 t^{3} \\
0 & 0 & 0 & 36 t^{2} & 48 t^{3} & 60 t^{4} \\
0 & 0 & 0 & 40 t^{3} & 60 t^{4} & 80 t^{5}
\end{array}\right]_{T_{i-1}}^{T_i}
\]
因此目标函数：
\[
\begin{aligned}
J(p) &=\sum_{i=1}^{M} \int_{T_{i-1}}^{T_{i}}\left(f^{(3)}(t)\right)^{2} \\
&=\sum_{i=1}^{M} p_{i}^{T} Q_{i} p_{i} \\
&=\left[\begin{array}{llll}
p_{1}^{T} & p_{2}^{T} & \cdots & p_{M}^{T}
\end{array}\right]\left[\begin{array}{llll}
Q_{1} & & & & \\
& Q_{2} & & \\ & & \ddots & \\
& & & Q_{M}
\end{array}\right]\left[\begin{array}{c}
p_{1} \\
p_{2} \\
\vdots \\
p_{M}
\end{array}\right] \\
&=p^{T} Q p \\
p &= \left[\begin{array}{llll}
p_{1}^{T} & p_{2}^{T} & \cdots & p_{M}^{T}
\end{array}\right]^{T}=\left[\begin{array}{lllllll}
p_{1,0} & p_{1,1} & \cdots & p_{1, K} & p_{2,0} & \cdots & p_{M, K}
\end{array}\right]^{T}
\end{aligned}
\]
可以看到\(J(p)\)是一个二次型，所以基于Minimum-jerk的轨迹优化问题可以转换为一个二次规划问题。
约束
如果不考虑障碍物，主要有两类约束，一类是导数约束(Derivative Constraint)，它约束了轨迹的初始状态和终止状态，以及每一段轨迹的开始/结束位置，也就是用路径规划得到的路径点对轨迹进行约束；另一类约束是连续性约束(Continuity Constraint)，它可以使相邻轨迹平滑过渡。
导数约束
-
初始状态和终止状态约束，如位置、速度、加速度等状态
\[
\left\{\begin{array}{l}
f^{(k)}\left(T_{0}\right)=x_{0}^{(k)} \\
f^{(k)}\left(T_{M}\right)=x_{M}^{(k)}
\end{array}, k=0,1, \cdots, K-1\right.
\]
-
每一段轨迹的初始位置由路径点给出,\(x_0, x_1, \dots x_M\)。
\[
f_i(T_{i-1}) = x_{i - 1}
\]
连续性约束
在两段轨迹的连接点处，我们希望这个点处的轨迹是平滑的，可以通过施加连续性约束来实现这个要求，也就是使相邻两段轨迹在连接点k-1阶导数分别相等
\[
\begin{gathered}
f_{i}\left(T_{i}\right)=f_{i+1}\left(T_{i}\right) \\
f_{i}^{(1)}\left(T_{i}\right)=f_{i+1}^{(1)}\left(T_{i}\right) \\
\vdots \\
f_{i}^{(k)}\left(T_{i}\right)=f_{i+1}^{(k)}\left(T_{i}\right)
\end{gathered}
\]
其中\(i = 1, 2, \dots M-1\)表示第i段轨迹。对于M+1个路径点，一共有M段轨迹，连续性约束一共有\(k * (M-1)\)。
对于Minimum-jerk，要求每一段轨迹连接处的位置、速度、加速度一致。
References
[1] Richter C, Bry A, Roy N. Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments[M]//Robotics Research. Springer International Publishing, 2016: 649-666.
[2] Mellinger D, Kumar V. Minimum snap trajectory generation and control for quadrotors[C]//Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011: 2520-2525.