Polynomial locus of order 5

Prerequisite

• In order to get a track with continuous speed , Then each track in the track needs to meet the position and speed constraints （4 A boundary condition ）, That is, the previous track $J_{i-1}$ The position and speed of the end point should be the same as that of the next track $J_i$ The position and speed are the same , Therefore, the third-order polynomial is used to represent each trajectory .
• In order to obtain a continuous acceleration trajectory , Then each track in the track needs to meet the position 、 Velocity and acceleration constraints （6 A boundary condition ）, That is, the previous track $J_{i-1}$ The end position of 、 The speed and acceleration should be consistent with the later trajectory $J_i$ The location of 、 The speed and acceleration are the same , Therefore, the fifth order polynomial is used to represent each trajectory .

Text

   about One In terms of trajectory , Usually, a continuous trajectory can be segmented into multi segment polynomial trajectory , Here, consider using 5 Polynomial of order . The corresponding time of each track is from $0$ Start to $T_i$ end , That is to say, for the $i$ Segment trajectory $J_i$ for , Its start time is $0$, The end time is $T_i$, The starting position 、 Speed 、 The accelerations are $p_i$、$v_i$ and $a_i$. According to the definition of polynomial trajectory
$$\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.（Maletype1）\end{array}$$
By the end of $1$ Take segment trajectory as an example , take $t=0$ Into the （ The formula 1） You can get the beginning of the first track ：
$$\begin{array}{rl}{p}_1(0)& =c_{10},\\ v_1(0)& =c_{11},\\ a_1(0)& =2c_{12}.\end{array}$$
take $t=T_1$ Into the （ The formula 1） You can get the initial end of the first track ：
$$\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}$$
Similarly, when the second track starts ：
$$\begin{array}{rl}{p}_2(0)& =c_{20},\\ v_2(0)& =c_{21},\\ a_2(0)& =2c_{22}.\end{array}$$
Because a trajectory is composed of multi segment polynomial trajectory , So it needs to meet ：
$$\{\begin{array}{rl}& c_{10}=p_1(0):risebeginningpositionSet\; upedgeworldstripPieces\; of\\ & c_{11}=v_1(0):risebeginningspeeddegreeedgeworldstripPieces\; of\\ & 2c_{12}=a_1(0):risebeginningAddspeeddegreeedgeworldstripPieces\; of\\ & frontOneparagraphrailtraceendstopjerkAndafterOneparagraphrailtracerisebeginningsnapphaseSame\; as,nothingjerkBigSmallaboutbeam,namelyjer{k}_1(T_1)-jer{k}_2(0)=0\\ & 6c_{13}+24c_{14}{T}_1+60c_{15}{T}_1^2-6c_{23}=0\\ & frontOneparagraphrailtraceendstopsnapAndafterOneparagraphrailtracerisebeginningsnapphaseSame\; as,nothingsnapBigSmallaboutbeam,namelysna{p}_1(T_1)-sna{p}_2(0)=0\\ & 24c_{14}+120T_1-24c_{24}=0\\ & frontOneparagraphrailtraceendstoppositionSet\; upAndafterOneparagraphrailtracerisebeginningpositionSet\; upphaseSame\; as,namely{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\\ & frontOneparagraphrailtraceendstoppositionSet\; upAndafterOneparagraphrailtracerisebeginningspeeddegreephaseSame\; as,namely{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\\ & frontOneparagraphrailtraceendstoppositionSet\; upAndafterOneparagraphrailtracerisebeginningAddspeeddegreephaseSame\; as,namely{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}$$

Convert the above constraints into a matrix Ax=b form , Available ：
$$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]$$

structure A The matrix is as follows ：

	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$

notes ： Red is OK / Column label .

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;