多项式(Polynomial)轨迹
在机器人轨迹规划中，我们常用n阶多项式来表示机器人的规划轨迹
p(t)=p_0+p_1t+p_2t^2\ldots+p_nt^n=\sum_{i=0}^np_it^i
这其中的p_0,p_1,\ldots,p_n 为多项式轨迹的各项系数，如果我们设一个参数向量p=[p_0,p_1,\ldots,p_n]^T ，则多项式轨迹可以写成向量形式，
p(t)=[1,t,t^2,\ldots,t^n]\cdot p
对于任意时刻t ，可以根据已经确定了各项系数的多项式计算出轨迹的位置(Position) ,速度(Velocity) ,加速度(Acceleration) ,jerk ,snap 等(通过求不同阶次的导数获得 )
\begin{gathered}
v(t)=p'(t)=[0,1,2t,3t^2,4t^3,\ldots,nt^{n-1}]\cdot p \\
a(t)=p^{\prime\prime}(t)=[0,0,2,6t,12t^2,\ldots,n(n-1)t^{n-2}]\cdot p \\
jerk(t)=p^{(3)}(t)=[0,0,0,6,24t,\ldots,\frac{n!}{(n-3!)}t^{n-3}]\cdot p \\
snap(t)=p^{(4)}(t)=[0,0,0,0,24,\dots,\frac{n!}{(n-4!)}t^{n-4}]\cdot p
\end{gathered}
通常情况下，我们要设置多个**航点(Waypoint)**用于让机器人通过很多个固定的点，对于这种相对复杂的轨迹，是很难用一个多项式来表示的，所以我们将每两个航点之间用一条多项式进行表示， 然后再将它们连接起来，放在一起表达就是：
\begin{equation}
p(t) =
\begin{cases}
[1, t, t^2, \ldots, t^n] \cdot p_1 & t_0 \leq t < t_1 \\
[1, t, t^2, \ldots, t^n] \cdot p_2 & t_1 \leq t < t_2 \\
\dots \\
[1, t, t^2, \ldots, t^n] \cdot p_k & t_{k-1} \leq t < t_k \\
\end{cases}
\end{equation}
这其中：
k 为轨迹的段数p_1,p_2,...,p_k 对应的就是p_i=[p_{i_0},p_{i_1},\ldots,p_{i_n}]^T ，也就是每一段轨迹的参数向量
在实际的问题中，例如无人车轨迹问题，是一个二维问题，我们就需要对x和y进行分别描述后合并在一起
Minimum-jerk
Minimum-jerk的含义就是求解每一段路径的参数向量，使得在整个路径上总的jerk最小，同时还需要满足约束条件，所以是一个带约束的最优化问题
多项式的阶次选择
在最小化jerk的过程中，我们需要对路径首尾的位置(x_0,x_1 )，速度(v_0,v_1 )，加速度(a_0,a_1 ),这6个独立的约束条件也就对应了我们有6个独立方程分别为:
p(t_0) = [1, t_0, t_0^2, \ldots, t_0^n] \cdot p = x_0 \\
p'(t_0) = [0, 1, 2t_0, 3t_0^2, \ldots, nt_0^{n-1}] \cdot p = v_0 \\
p''(t_0) = [0, 0, 2, 6t_0, 12t_0^2, \ldots, n(n-1)t_0^{n-2}] \cdot p = a_0 \\
p(t_1) = [1, t_1, t_1^2, \ldots, t_1^n] \cdot p = x_1 \\
p'(t_1) = [0, 1, 2t_1, 3t_1^2, \ldots, nt_1^{n-1}] \cdot p = v_1 \\
p''(t_1) = [0, 0, 2, 6t_1, 12t_1^2, \ldots, n(n-1)t_1^{n-2}] \cdot p = a_1 \\
由于这个地方我们有6个独立的方程，所以我们需要6个系数来满足这六个方程，因为如果我们需要求解6个方程组成的方程组，那我们也需要最少6个系数(自由度) ，所以我们这个地方符合要求的多项式的最小阶次为5 ，所以对于最小化jerk的优化问题，我们选择五次多项式来表达每一段轨迹：
\left.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.\right.
其中每一个多项式就代表了一段轨迹
构造目标函数
我们的目标是让总的jerk最小，所以我们选择可以表示jerk的表达式，作为我们的目标函数：
\begin{aligned}jerk(t)=f^{(3)}(t)&=6p_3 +24tp_4 +60t^2p_5\\&=\begin{bmatrix}0&0&0&6&24t&60t^2\end{bmatrix}\cdot p\end{aligned}
为了要使其最小，我们需要jerk(t) 在整段时间上的积分最小，在这里我们可以选择使其的L_2 范数最小，因为保证轨迹的平滑性、优化过程的便利性以及其在物理和能量方面(具体说明放在后面 )，所以在minimum-jerk中选择使jerk(t) 的二范数最小即:
min f^{(3)}(t)=min \sum_{i=1}^M \int_{T_{i-1}}^{T_i} (f^{(3)}(t))^2dt
所以目标函数J(p) 定义为：
J(p)=\sum_{i=1}^M\int_{T_{i-1}}^{T_i}(f^{(3)}(t))^2dt
令a=\begin{bmatrix}0&0&0&6&24t&60t^2\end{bmatrix}^T ，然后对f^{(3)}(t). 求平方可以得到:
\begin{aligned}
(f^{(3)}(t))^2& =(\begin{bmatrix}0&0&0&6&24t&60t^2\end{bmatrix}\cdot p)^T(\begin{bmatrix}0&0&0&6&24t&60t^2\end{bmatrix}\cdot p) \\
&=(a^Tp)^T(a^Tp) \\
&=p^Taa^Tp
\end{aligned}
其中，对于(a^Tp)^T(a^Tp)=p^Taa^Tp 这一步中，具体的，(a^Tp)^T = (p^Ta) 这一步是因为(a^Tp)^T 中的a^T 是一个1x6的矩阵，而p 是一个6x1的矩阵，这两个矩阵相乘所得到的就是一个标量，因为标量的转置还是标量本身，所以它只能等于(p^Ta) ，这又是为什么前面令a 时需要加上一个转置的原因。
此时，我们令A(t)=aa^T ,得到:
A=aa^T=\begin{bmatrix}0\\0\\0\\6\\24t\\60t^2\end{bmatrix}\begin{bmatrix}0&0&0&6&24t&60t^2\end{bmatrix}=\begin{bmatrix}0&0&0&0&0&0\\0&0&0&0&0&0&\\0&0&0&0&0&0&\\0&0&0&36&144t&360t^2\\0&0&0&144t&576t^2&1440t^2\\0&0&0&360t^2&1440t^3&3600t^2\end{bmatrix}
令A(t) 的积分为矩阵Q ，则可以得到
Q=\int_{T_{i-1}}^{T_i}A(t)dt=\begin{bmatrix}0&0&0&0&0&0\\0&0&0&0&0&0\\0&0&0&0&0&0\\0&0&0&36t&72t^2&120t^3\\0&0&0&72t^2&192t^3&360t^4\\0&0&0&120t^3&360t^4&720t^5\end{bmatrix}_{T_i-1}^{T_i}
因此，得到最终的目标函数:
\begin{aligned}
J(p)& =\sum_{i=1}^N\int_{T_{i-1}}^{T_i}(f^{(3)}(t))^2 \\
&=\sum_{i=1}^Np_i^TQ_ip_i \\
&=\begin{bmatrix}p_1^T&&p_2^T&\cdots&p_N^T\end{bmatrix}\begin{bmatrix}Q_1&&&\\&&Q_2&&\\&&&\ddots&\\&&&&Q_N\end{bmatrix}\begin{bmatrix}p_1\\p_2\\\vdots\\p_N\end{bmatrix} \\
&=p^TQp
\end{aligned}
其中:
p=\begin{bmatrix}p_1^T&p_2^T&\cdots&p_N^T\end{bmatrix}^T 代表的就是优化变量，也就是每一段多项式轨迹的系数的列组合，其维数为N(多项式次数 + 1) x 1Q 是一个维数为N x N的实对称矩阵
很明显，J(p) 是一个二次型，所以这个问题也就很自然的成为了一个二次规划问题，二次规划问题定义与求解请看这里：二次规划问题与拉格朗日乘数求解法 p 就是二次规划问题中的代表变量的向量c，而Q 矩阵也就是二次规划问题中的对称矩阵Q 。
添加约束
根据上面内容我们已经知道了我们的目标函数是一个二次型，也就是说，我们的问题时一个二次规划问题，在上一节中我们已经得到了二次规划问题的c向量和Q矩阵，接下来我们将要加上代表约束的矩阵A_c 和向量b
目前我们先不考虑障碍物约束，因此我们可以加上的约束主要有两类约束
初始状态与终止状态约束，如位置、速度、加速度等状态，也就是为了保证通过每一个航点，以及通过航点时的状态(导数约束 )：
\begin{align*} f_0^{(0)}\left(T_0\right) &= x_0 & f_0^{(0)}\left(T_1\right) &= x_1 & \dots \\ f_0^{(1)}\left(T_0\right) &= v_0 & f_0^{(1)}\left(T_1\right) &= v_1 & \dots \\ f_0^{(2)}\left(T_0\right) &= a_0 & f_0^{(2)}\left(T_1\right) &= a_1 & \dots \\ \vdots && \vdots \end{align*}
更具体的，拿第一段约束举例，我们可以得到约束矩阵为:
A_{c1}=\begin{bmatrix}1&t_0&t_0^2&t_0^3&t_0^4&t_0^5\\0&1&2t_0&3t_0^2&4t_0^3&5t_0^4\\0&0&2&6t_0&12t_0^2&20t_0^3\\1&t_1&t_1^2&t_1^3&t_1^4&t_1^5\\0&1&2t_1&3t_1^2&4t_1^3&5t_1^4\\0&0&2&6t_1&12t_1^2&20t_1^3\end{bmatrix}\cdot p_1^T,\quad b_1=\begin{bmatrix}x_0\\v_0\\a_0\\x_1\\v_1\\a_1\end{bmatrix}
在两段轨迹的连接处，为了保证整个轨迹的平滑性，通过让各个航点之间过渡位置的速度与加速度是连续的，具体来说(连续性约束 )：
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)
更具体的，拿第一段到第二段的过渡作为举例，我们可以得到约束矩阵为:
A_{c1} = \begin{bmatrix}
1 & t_1 & t_1^2 & t_1^3 & t_1^4 & t_1^5 & -1 & -t_1 & -t_1^2 & -t_1^3 & -t_1^4 & -t_1^5 \\
0 & 1 & 2t_1 & 3t_1^2 & 4t_1^3 & 5t_1^4 & 0 & -1 & -2t_1 & -3t_1^2 & -4t_1^3 & -5t_1^4 \\
0 & 0 & 2 & 6t_1 & 12t_1^2 & 20t_1^3 & 0 & 0 & -2 & -6t_1 & -12t_1^2 & -20t_1^3
\end{bmatrix} \cdot \begin{bmatrix} p_{1,0} \\ p_{1,1} \\ p_{1,2} \\ p_{1,3} \\ p_{1,4} \\ p_{1,5} \\ p_{2,0} \\ p_{2,1} \\ p_{2,2} \\ p_{2,3} \\ p_{2,4} \\ p_{2,5} \end{bmatrix}, \quad b_1 = \begin{bmatrix}
0 \\
0 \\
0
\end{bmatrix}
L-2范数性质
选用L_2 范数进行优化会带来平滑性，这是因为L_2 范数的定义和它对误差的处理方式。具体原因如下：
误差的平方和:
L_2 范数的定义是通过对函数值的平方求和再取平方根。对于函数f(t) ，其L_2 范数定义为：
\|f(t)\|_2=\left(\int_a^b|f(t)|^2 dt\right)^{1/2}
在最小化轨迹的 jerk 时，我们考虑的是三阶导数f^{(3)}(t) 的L_2 范数：
\min_p\int_{T_{i-1}}^{T_i}\left(f^{(3)}(t)\right)^2 dt
这种平方和的形式会惩罚大的误差，因此会倾向于分散误差，使得误差在整个区间内尽可能均匀地分布，避免局部的大变化。 (这也就是我们想要的平滑)
凸优化 ：
L_2 范数的平方形式是一个凸函数(正是因为这个性质，我们才能在minimum-jerk中引入二次型，从而使使用二次规划 )。凸函数的优化问题具有一个重要性质，即其局部最小值也是全局最小值。这意味着通过最小化 L_2 范数，我们能够找到一个全局最优解，避免出现局部不平滑的情况。
光滑性约束 ：
轨迹规划中，jerk（即三阶导数）的平方和最小化会自然地限制轨迹的变化速率。
这意味着轨迹的变化是渐进的，而不是突然的，从而使得轨迹更加平滑。
参考
https://blog.csdn.net/qq_42688495/article/details/121879551
https://blog.csdn.net/q597967420/article/details/76099491
