5次多項式の軌跡

前提条件

連続速度の軌道を取得するには、軌道内の各軌道が位置と速度の制約 (4 つの境界条件)、つまり前の軌道 $J_{i-1}を満たす必要があります。$ 速度 $J$ の位置と速度は同じであるため、各軌道を表すために 3 次多項式が使用されます。
連続的な加速度を伴う軌道を取得するには、軌道内の各軌道が位置、速度、加速度の制約 (6 つの境界条件)、つまり前の軌道 J i − 1 J_{i-1} を満たす必要があります $J_{i -}$ $J_{i}$ と同じでなければなりません $J$ の位置、速度、加速度は同じであるため、各軌道を表すために 5 次の多項式が使用されます。

文章

軌跡の場合、通常は連続軌跡を複数の多項式軌跡に分割することができ、各軌跡は５次の多項式で表現されると考える。各軌跡に対応する時間は $0$ $T_i$ まで $T$ 終わり、つまり $i$ セグメント軌道 $J_i$ たとえば、開始時刻は $0$ 、終了時刻は $T_i$ 、開始位置、速度、加速度はそれぞれ $p_i$ 、 $v_i$ 和 $a_i$ 多段式パスの設定によると、
$\begin{aligned} p&=c_{0}+c_{1}t+c_{2}t^2+c_{3}t^3+c_{4}t^4+c_{5 }t^5 \\ v&=\quad\quad c_{1}+2c_{2}t+3c_{3}t^2+4c_{4}t^3+5c_{5}t^4 \\ a&= \quad\quad\quad\quad2c_{2}+6c_{3}t+12c_{4}t^2+20c_{5}t^3 \\ ジャーク&=\quad\quad\quad\quad\quad\quad \ quad6c_{3}+24c_{4}t+60c_{5}t^2 \\ スナップ&=\quad\quad\quad\quad\quad\quad \quad\quad \quad\quad 24c_{4}+120c_{5 }t. \quad\quad（公式1）\\ \end{整列}$
テイクNo.1 $軌跡の1$ セグメントを例として、 $t = (式 1) に0$ を代入すると、最初の軌道の開始点を取得できます:
$p_1 (0)&=c_{10}, \\ v_1(0)&=c_{11},\\ a_1(0)&=2c_{12}. \end{整列}$
t = T 1とする $t=T_1$ (式 1) を導入すると、軌道の最初のセグメントの最初の端を取得できます。
$Ｔ１）３．\begin{整列} 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{整列}$
同様に、2 番目の軌道の開始点を取得できます:
$\begin{aligned} p_2( 0)&= c_{20}、\\ v_2(0)&=c_{21}、\\ a_2(0)&=2c_{22}. \end{aligned}$
軌道は複数の多項式軌道で構成されるため、次の条件を満たす必要があります。
$\left\{\begin {aligned} &c_{10}=p_1(0): 開始位置境界条件\\ &c_{11}=v_1(0): 開始速度境界条件\\ &2c_ {12}=a_1(0): 開始加速度の境界条件\\ &前の軌道の終了ジャークは、次の軌道の開始スナップと同じです。ジャークサイズの制約はありません。つまり、ジャーク_1(T_1)-ジャーク_2 (0)=0\\ &6c_{13}+24c_{14}T_1+60c_ {15}T_1^2-6c_{23}=0\\ &前の軌道の終了スナップは、前の軌道の開始スナップと同じです。スナップサイズの制約はありません。つまり、snap_1(T_1)-snap_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{整列}\右。$

上記の制約を行列 Ax=b 形式に変換すると、次のようになります:
$x=\begin{bmatrix} 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{bmatrix}$

$b=\begin{bmatrix} {p_1(0)}\\ {v_1(0)} \\ {a_1(0)}\\ 0\\ 0\\ p_2(0)\\ 0\\ 0\\ 0\\ \\ \end{bmatrix}$

次のように 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;

前提条件

文章

おすすめ