3次スプライン補間で軌跡生成：３次多項式のパラメータを求める（その２）

離散的な位置指令から3次スプライン補間を用いて連続的な軌跡を生成する方法を紹介します。

前回の記事では、3次スプライン補間の条件から３次多項式の各パラメータを求める方法を紹介しました。

3次スプライン補間で軌跡生成：３次多項式のパラメータを求める（その１） 3次スプライン補間を用いた離散点から連続的な軌跡を生成する方法を紹介します。前回の記事では、3次スプライン補間に...

今回は、前回から引き続き３次多項式の各パラメータを求める方法を紹介していきます。

Contents

3次スプライン補間の条件
条件３よりパラメータ$c_{i,i+1,i+2}$と$a_{i,i+1,i+2}$の関係を求める
パラメータ$c_{0,\cdots,n}$を$a_{0,\cdots,n}$から求める
まとめ

3次スプライン補間の条件

3次スプライン補間に用いる３次多項式のパラメータを求めるための条件をおさらいします。

条件１：各区間の始点
条件２：位置の連続性
条件３：速度の連続性
条件４：加速度の連続性
条件５：軌跡の始点と終点

3次スプライン補間で軌跡生成：３次多項式と補間条件ロボットなどを動かす経路を指定する際に、通過してほしい点を離散的な指令点で指示することが多くあります。この様な離...

これらの条件を用いて、３次多項式のパラメータを求めていきます。

条件３よりパラメータ$c_{i,i+1,i+2}$と$a_{i,i+1,i+2}$の関係を求める

これまでの条件２と４からの算出により、パラメータ$b_i$と$d_i$をパラメータ$a_{i,i+1}$と$c_{i,i+1}$を用いて表すことが出来ました。

また、パラメータ$a_i$については条件１より求めることが出来ました。

よって、パラメータ$c_i$をパラメータ$a_i$のみを用いて表すことが出来れば、すべてのパラメータを算出することが出来ます。

条件３で示される速度の連続性を用いて、パラメータ$c_i$を表していきます。

各区間の３次多項式$S_i$と$S_{i+1}$が境界点$x_{i+1}$において、速度が連続するためには、

$$ S_i'(x_{i+1}) = S_{i+1}'(x_{i+1}) $$

$$ \Rightarrow b_i + 2 c_i (x_{i+1}-x_i) + 3 d_i (x_{i+1}-x_i)^2 $$

$$ =b_{i+1} + 2 c_{i+1} (x_{i+1}-x_{i+1}) + 3 d_{i+1} (x_{i+1}-x_{i+1})^2 $$

$$ \Rightarrow b_i + 2 c_i h_i + 3 d_i {h_i}^2 =b_{i+1} $$

という関係式が導かれます。

この関係式について、先ほどまでに求めたパラメータ$b_{i,i+1}$と$d_i$についてパラメータ$a_{i,i+1,i+2}$と$c_{i,i+1,i+2}$で表した式を代入して整理すると、

$$ \frac{a_{i+1}-a_i}{h_i} – \frac{h_i(c_{i+1}+2 c_i)}{3} + 2 c_i h_i + 3 \frac{c_{i+1}-c_i}{3 h_i} {h_i}^2 $$

$$ =\frac{a_{i+2}-a_{i+1}}{h_{i+1}} – \frac{h_{i+1}(c_{i+2}+2 c_{i+1})}{3} $$

$$ \Rightarrow – h_i (c_{i+1}+2 c_i) + 6 c_i h_i + 3 h_i (c_{i+1}-c_i) + h_{i+1}(c_{i+2}+2 c_{i+1}) $$

$$ =\frac{3}{h_{i+1}}(a_{i+2}-a_{i+1}) – \frac{3}{h_i}(a_{i+1}-a_i) $$

$$ \Rightarrow h_{i+1} c_{i+2} + 2 (h_{i+1} + h_i) c_{i+1}+ c_i h_i = \frac{3}{h_{i+1}}(a_{i+2}-a_{i+1}) – \frac{3}{h_i}(a_{i+1}-a_i) $$

とパラメータ$c_{i,i+1,i+2}$と$a_{i,i+1,i+2}$の関係式を導くことが出来ました。

パラメータ$c_{0,\cdots,n}$を$a_{0,\cdots,n}$から求める

先ほど求めたパラメータ$c_{i,i+1,i+2}$と$a_{i,i+1,i+2}$の関係式からパラメータ$c_{0,\cdots,n}$を$a_{0,\cdots,n}$から求める方法を紹介します。

先程のパラメータ$c_{i,i+1,i+2}$と$a_{i,i+1,i+2}$の関係式

$$ h_{i+1} c_{i+2} + 2 (h_{i+1} + h_i) c_{i+1}+ c_i h_i = \frac{3}{h_{i+1}}(a_{i+2}-a_{i+1}) – \frac{3}{h_i}(a_{i+1}-a_i) $$

に加えて、条件５より補間された軌跡の始点と終点での加速度は0とすることから、

$$ c_0 = 0 $$

$$ c_{end} = 0 $$

が導かれます。

これらの式より、以下の行列を用いた関係式を導くことが出来ます。

$$ A = \begin{bmatrix} 1 & 0 & \cdots & \cdots & \cdots & \cdots & \cdots & 0 \\ h_0 & 2 (h_0 + h_1) & h_1 & 0 & \cdots & \cdots & \cdots & 0 \\ 0 & h_1 & 2 (h_1 + h_2) & h_2 & 0 & \cdots & \cdots & 0 \\ \vdots & & \ddots & \ddots & \ddots & & & \vdots \\ \vdots & & & \ddots & \ddots & \ddots & & \vdots \\ \vdots & & & & \ddots & \ddots & \ddots & 0 \\ \vdots & & & & & h_{n-2}& 2 (h_{n-2} + h_{n-1}) & h_{n-1} \\ 0 & \cdots & \cdots & \cdots & \cdots & \cdots & 0 & 1 \end{bmatrix} $$

$$ c = \begin{bmatrix} c_0 \\ c_1 \\ c_2 \\ \vdots \\ \vdots \\ \vdots \\ c_{n-1} \\ c_n \end{bmatrix} $$

$$ b = \begin{bmatrix} 0 \\ \frac{3}{h_1}(a_2-a_1) – \frac{3}{h_0}(a_1-a_0) \\ \frac{3}{h_2}(a_3-a_2) – \frac{3}{h_1}(a_2-a_1) \\ \vdots \\ \vdots \\ \vdots \\ \frac{3}{h_{n-1}}(a_n-a_{n-1}) – \frac{3}{h_{n-2}}(a_{n-1}-a_{n-2}) \\ 0 \end{bmatrix} $$

$$ \Rightarrow A c = b $$

この行列式を連立方程式として解くことで、パラメータ$c=c_{0,\cdots,n}$を求めることが出来ます。