5th order polynomial trajectory

Prerequisites

• In order to obtain a trajectory with continuous velocity, each trajectory in the trajectory needs to satisfy position and velocity constraints (4 boundary conditions), that is, the previous trajectory $J_{i-1}$The end position and speed of$J_i$The position and velocity of are the same, so a third-order polynomial is used to represent each trajectory.
• In order to obtain a trajectory with continuous acceleration, each trajectory in the trajectory needs to satisfy position, velocity and acceleration constraints (6 boundary conditions), that is, the previous trajectory $J_{i-1}$The end position, speed and acceleration must be the same as those of the next trajectory $J_i$The position, velocity and acceleration of are the same, so a fifth-order polynomial is used to represent each trajectory.

text

  For a trajectory, a continuous trajectory can usually be segmented into multi-segment polynomial trajectories. Here, each trajectory is considered to be represented by a fifth-order polynomial. The time corresponding to each trajectory is from $Starting\; from\; 0$ to$T_i$end, that is, for the $i$ segment trajectory$J_i$, its start time is $0$ , the end time is$T_i$, the starting position, speed and acceleration are respectively $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.(Official1\end{array}$$
Take No. $Taking\; 1$ segment of trajectory as an example, set$t=By\; bringing\; 0$ into (Formula 1), we can get the beginning of the first trajectory:
$$\begin{array}{rl}{p}_1(0)& =c_{10},\\ v_1\left(0\right)& =c_{11},\\ a_1(0)& =2c_{12}.\end{array}$$
Let $t=T_1$Bringing in (Formula 1) we can get the initial end of the first segment of the trajectory:
$$\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\left(T_1\right)& =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}\left(T_1\right)+3c{\_}_{13}(T_1{)}^2+4c_{14}(T_1{)}^3+5c_{15}(T_1{)}^4,\\ a_1\left(T_1\right)& =2c_{12}+6c{\_}_{13}(T_1-0)+12c_{14}(T_1-0{)}^2+20c_{15}(T_1-0{)}^3\\ & =2c_{12}+6c{\_}_{13}\left(T_1\right)+12c_{14}(T_1{)}^2+20c_{15}(T_1{)}^3.\end{array}$$
In the same way, we can get the beginning of the second trajectory:
$$\begin{array}{rl}{p}_2(0)& =c_{20},\\ v_2\left(0\right)& =c_{21},\\ a_2(0)& =2c_{22}.\end{array}$$
Since a trajectory is composed of multiple polynomial trajectories, it needs to satisfy:
$$\{\begin{array}{rl}& c_{10}=p_1(0):Startingpositionboundaryconditions\\ & c_{11}=v_1\left(0\right):Startingvelocityboundarycondition\\ & 2c_{12}=a_1\left(0\right):Startingaccelerationboundarycondition\\ & The\; endjerkof\; the\; previoussegmentof\; the\; trajectoryisthe\; same\; asthestartingsnapof\; the\; nextsegmentof\; the\; trajectory,\; andthere\; is\; nojerksizeconstraint,that\; is,jerk{}_1(T_1)-jer{k}_2(0)=0\\ & 6c{\_}_{13}+24c_{14}{T}_1+60c\_{}_{15}{T}_1^2-6c{\_}_{23}=0\\ & The\; endingsnapofthe\; previoussegmentof\; the\; trajectoryis\; the\; same\; asthe\; startingsnapof\; the\; nextsegmentof\; the\; trajectory,\; andthere\; is\; nosnapsizeconstraint,that\; is,snap{}_1(T_1)-sna{p}_2(0)=0\\ & 24c_{14}+120T_1-24c_{24}=0\\ & The\; endpositionoftheprevioustrajectoryis\; the\; sameasthe\; startingpositionof\; thenexttrajectory,that\; is\; ,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\left(0\right)\\ & 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\\ & The\; endpositionof\; theprevioustrajectoryis\; the\; same\; asthestartingspeedof\; thenexttrajectory,that\; is\; ,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\\ & The\; endpositionof\; theprevioustrajectoryis\; the\; same\; asthestartingaccelerationofthenexttrajectory,that\; is\; ,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 matrix Ax=b form, we can get:
$$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\left(0\right)\\ a_1\left(0\right)\\ 0\\ 0\\ p_2\left(0\right)\\ 0\\ 0\\ 0\\ \end{array}\right]$$

Construct the A matrix 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$

Note : Red is the row/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;