5阶多项式轨迹

前提条件

• 为了获得一条速度连续的轨迹，则该轨迹中每段轨迹需要满足位置和速度约束（4个边界条件），即前一段轨迹$J_{i-1}$的终点位置和速度要和后一段轨迹$J_i$的位置速度相同，因此使用三阶多项式来表示每段轨迹。
• 为了获得一条加速度连续的轨迹，则该轨迹中每段轨迹需要满足位置、速度和加速度约束（6个边界条件），即前一段轨迹$J_{i-1}$的终点位置、速度和加速度要和后一段轨迹$J_i$的位置、速度和加速度相同，因此使用五阶多项式来表示每段轨迹。

正文

  对于一条轨迹而言，通常可以将一条连续轨迹分段成多段多项式轨迹，这里考虑每段轨迹使用5阶多项式来表示。其中每段轨迹对应的时间均从$0$开始到$T_i$结束，即对于第$i$段轨迹$J_i$而言，其开始时间为$0$，结束时间为$T_i$，起始位置、速度、加速度分别为$p_i$、$v_i$和$a_i$。根据多项式轨迹的定义
$$\begin{array}{rl}p& =c_0+c_{1}t+c_2{t}^2+c_3{t}^3+c_4{t}^4+c_5{t}^5\\ v& =c_1+2c_{2}t+3c_3{t}^2+4c_4{t}^3+5c_5{t}^4\\ a& =2c_2+6c_{3}t+12c_4{t}^2+20c_5{t}^3\\ jerk& =6c_3+24c_{4}t+60c_5{t}^2\\ snap& =24c_4+120c_{5}t.（公式1）\end{array}$$
以第$1$段轨迹为例，将$t=0$带入（公式1）可以得第一段轨迹开始时：
$$\begin{array}{rl}{p}_1(0)& =c_{10},\\ v_1(0)& =c_{11},\\ a_1(0)& =2c_{12}.\end{array}$$
将$t=T_1$带入（公式1）可以得到初始时第一段轨迹结束时：
$$\begin{array}{rl}{p}_1(T_1)& =c_{10}+c_{11}(T_1-0)+c_{12}(T_1-0{)}^2+c_{13}(T_1-0{)}^3+c_{14}(T_1-0{)}^4+c_{15}(T_1-0{)}^5\\ & =c_{10}+c_{11}{T}_1+c_{12}{T}_1^2+c_{13}{T}_1^3+c_{14}{T}_1^4+c_{15}{T}_1^5,\\ v_1(T_1)& =c_{11}+2c_{12}(T_1-0)+3c_{13}(T_1-0{)}^2+4c_{14}(T_1-0{)}^3+5c_{15}(T_1-0{)}^4\\ & =c_{11}+2c_{12}(T_1)+3c_{13}(T_1{)}^2+4c_{14}(T_1{)}^3+5c_{15}(T_1{)}^4,\\ a_1(T_1)& =2c_{12}+6c_{13}(T_1-0)+12c_{14}(T_1-0{)}^2+20c_{15}(T_1-0{)}^3\\ & =2c_{12}+6c_{13}(T_1)+12c_{14}(T_1{)}^2+20c_{15}(T_1{)}^3.\end{array}$$
同理可得第二段轨迹开始时：
$$\begin{array}{rl}{p}_2(0)& =c_{20},\\ v_2(0)& =c_{21},\\ a_2(0)& =2c_{22}.\end{array}$$
由于一条轨迹是由多段多项式轨迹组成，因此需要满足：
$$\{\begin{array}{rl}& c_{10}=p_1(0):起始位置边界条件\\ & c_{11}=v_1(0):起始速度边界条件\\ & 2c_{12}=a_1(0):起始加速度边界条件\\ & 前一段轨迹终止jerk与后一段轨迹起始snap相同，无jerk大小约束，即jer{k}_1(T_1)-jer{k}_2(0)=0\\ & 6c_{13}+24c_{14}{T}_1+60c_{15}{T}_1^2-6c_{23}=0\\ & 前一段轨迹终止snap与后一段轨迹起始snap相同，无snap大小约束，即sna{p}_1(T_1)-sna{p}_2(0)=0\\ & 24c_{14}+120T_1-24c_{24}=0\\ & 前一段轨迹终止位置与后一段轨迹起始位置相同，即p_1(T_1)=p_2(0):\\ & c_{10}+c_{11}{T}_1+c_{12}{T}_1^2+c_{13}{T}_1^3+c_{14}{T}_1^4+c_{15}{T}_1^5=p_2(0)\\ & c_{10}+c_{11}{T}_1+c_{12}{T}_1^2+c_{13}{T}_1^3+c_{14}{T}_1^4+c_{15}{T}_1^5-c_{20}=0\\ & 前一段轨迹终止位置与后一段轨迹起始速度相同，即v_1(T_1)-v_2(0)=0\\ & c_{11}+2c_{12}{T}_1+3c_{13}{T}_1^2+4c_{14}{T}_1^3+5c_{15}{T}_1^4-c_{21}=0\\ & 前一段轨迹终止位置与后一段轨迹起始加速度相同，即a_1(T_1)-a_2(0)=0\\ & 2c_{12}+6c_{13}{T}_1+12c_{14}{T}_1^2+20c_{15}{T}_1^3-2c_{22}=0\end{array}$$

将上述约束转换成矩阵Ax=b形式，可得：
$$x=\left[\begin{array}{c}{c}_{10}\\ c_{11}\\ c_{12}\\ c_{13}\\ c_{14}\\ c_{15}\\ c_{20}\\ c_{21}\\ c_{22}\\ c_{23}\\ c_{24}\\ c_{25}\end{array}\right]$$

$$b=\left[\begin{array}{c}{p}_1(0)\\ v_1(0)\\ a_1(0)\\ 0\\ 0\\ p_2(0)\\ 0\\ 0\\ 0\\ \end{array}\right]$$

构建A矩阵如下：

	0	1	2	3	4	5	6	7	8	9	10	11
0	$1$
1		$1$
2			$2$
3				$6$	$24T_1$	$60T_1^2$				$-6$
4					$24$	$120T_1$					$-24$
5	$1$	$T_1$	$T_1^2$	$T_1^3$	$T_1^4$	$T_1^5$
6	$1$	$T_1$	$T_1^2$	$T_1^3$	$T_1^4$	$T_1^5$	$-1$
7		$1$	$2T_1$	$3T_1^2$	$4T_1^3$	$5T_1^4$		$-1$
8			$2$	$6T$	$12T_1^2$	$20T_1^3$			$-2$
9					…
10							$1$	$T_2$	$T_2^2$	$T_2^3$	$T_2^4$	$T_2^5$
11								$1$	$2T_2$	$3T_2^2$	$4T_2^3$	$5T_2^4$
12									$2$	$6T_2$	$12T_2^2$	$20T_2^3$

注：红色是行/列标号。

inline void generate(const Eigen::MatrixXd &inPs,
const Eigen::VectorXd &ts)
{
    
	T1 = ts;
	T2 = T1.cwiseProduct(T1);
	T3 = T2.cwiseProduct(T1);
	T4 = T2.cwiseProduct(T2);
	T5 = T4.cwiseProduct(T1);
	A.reset();
	b.setZero();
	A(0, 0) = 1.0;
	A(1, 1) = 1.0;
	A(2, 2) = 2.0;
	b.row(0) = headPVA.col(0).transpose();
	b.row(1) = headPVA.col(1).transpose();
	b.row(2) = headPVA.col(2).transpose();
	for (int i = 0; i < N - 1; i++)
	{
    
		A(6 * i + 3, 6 * i + 3) = 6.0;
		A(6 * i + 3, 6 * i + 4) = 24.0 * T1(i);
		A(6 * i + 3, 6 * i + 5) = 60.0 * T2(i);
		A(6 * i + 3, 6 * i + 9) = -6.0;
		A(6 * i + 4, 6 * i + 4) = 24.0;
		A(6 * i + 4, 6 * i + 5) = 120.0 * T1(i);
		A(6 * i + 4, 6 * i + 10) = -24.0;
		A(6 * i + 5, 6 * i) = 1.0;
		A(6 * i + 5, 6 * i + 1) = T1(i);
		A(6 * i + 5, 6 * i + 2) = T2(i);
		A(6 * i + 5, 6 * i + 3) = T3(i);
		A(6 * i + 5, 6 * i + 4) = T4(i);
		A(6 * i + 5, 6 * i + 5) = T5(i);
		A(6 * i + 6, 6 * i) = 1.0;
		A(6 * i + 6, 6 * i + 1) = T1(i);
		A(6 * i + 6, 6 * i + 2) = T2(i);
		A(6 * i + 6, 6 * i + 3) = T3(i);
		A(6 * i + 6, 6 * i + 4) = T4(i);
		A(6 * i + 6, 6 * i + 5) = T5(i);
		A(6 * i + 6, 6 * i + 6) = -1.0;
		A(6 * i + 7, 6 * i + 1) = 1.0;
		A(6 * i + 7, 6 * i + 2) = 2 * T1(i);
		A(6 * i + 7, 6 * i + 3) = 3 * T2(i);
		A(6 * i + 7, 6 * i + 4) = 4 * T3(i);
		A(6 * i + 7, 6 * i + 5) = 5 * T4(i);
		A(6 * i + 7, 6 * i + 7) = -1.0;
		A(6 * i + 8, 6 * i + 2) = 2.0;
		A(6 * i + 8, 6 * i + 3) = 6 * T1(i);
		A(6 * i + 8, 6 * i + 4) = 12 * T2(i);
		A(6 * i + 8, 6 * i + 5) = 20 * T3(i);
		A(6 * i + 8, 6 * i + 8) = -2.0;
		b.row(6 * i + 5) = inPs.col(i).transpose();
	}
	A(6 * N - 3, 6 * N - 6) = 1.0;
	A(6 * N - 3, 6 * N - 5) = T1(N - 1);
	A(6 * N - 3, 6 * N - 4) = T2(N - 1);
	A(6 * N - 3, 6 * N - 3) = T3(N - 1);
	A(6 * N - 3, 6 * N - 2) = T4(N - 1);
	A(6 * N - 3, 6 * N - 1) = T5(N - 1);
	A(6 * N - 2, 6 * N - 5) = 1.0;
	A(6 * N - 2, 6 * N - 4) = 2 * T1(N - 1);
	A(6 * N - 2, 6 * N - 3) = 3 * T2(N - 1);
	A(6 * N - 2, 6 * N - 2) = 4 * T3(N - 1);
	A(6 * N - 2, 6 * N - 1) = 5 * T4(N - 1);
	A(6 * N - 1, 6 * N - 4) = 2;
	A(6 * N - 1, 6 * N - 3) = 6 * T1(N - 1);
	A(6 * N - 1, 6 * N - 2) = 12 * T2(N - 1);
	A(6 * N - 1, 6 * N - 1) = 20 * T3(N - 1);
	b.row(6 * N - 3) = tailPVA.col(0).transpose();
	b.row(6 * N - 2) = tailPVA.col(1).transpose();
	b.row(6 * N - 1) = tailPVA.col(2).transpose();
	A.factorizeLU();
	A.solve(b);
	return;