3次スプライン補間で軌跡生成：連続的な軌跡を生成する（その２）

3次スプライン補間を用いることで、離散的な指令値から連続的で滑らかな軌跡の生成を実現することが出来ます。

前回の記事では、3次スプライン補間に用いる3次多項式のパラメータの内$a_i$と$c_i$を求める方法を紹介しました。

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

今回は、残りのパラメータ$b_i$と$d_i$を算出して、3次多項式を用いた3次スプライン補間による軌跡生成を実現していきます。

Contents

前回までの復習
３次多項式のパラメータを算出
- パラメータ$d_i$を求める
- パラメータ$b_i$を求める
算出したパラメータを用いて3次スプライン補間を行う
まとめ

前回までの復習

3次スプライン補間に用いる3次多項式のパラメータを算出するにあたり、必要となる情報のおさらいをしていきます。

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

離散点$x_i$と$x_{i+1}$の間を補間する3次多項式$S_i$

$$ S_i(x) = a_i + b_i (x-x_i) + c_i (x-x_i)^2 + d_i (x-x_i)^3 $$

について、3次スプライン補間の条件

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

を導くことが出来ます。

詳細はこちらの記事を参考にしてください。

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

3次多項式のパラメータの関係式

3次スプライン補間の条件より、3次多項式のパラメータ（$a_i, b_i, c_i, d_i$）について

$$ a_i = y_i $$

$$ 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) $$

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

$$ d_i = \frac{c_{i+1}-c_i}{3 h_i} $$

の関係が成り立ちます。

詳しくはこちらの記事を参考にしてください。

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

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

取り扱う離散的な指令点

3次スプライン補間の条件より導かれた各パラメータの関係式を用いて、実際にX-Y平面上に表された離散的な指令点を連続的に補間していきます。

今回の記事で補間する離散的な指令点は下記のように定義します。

$$ \begin{eqnarray} \begin{array}{c|c} X-axis & Y-axis \\ \hline 0 & 0 \\ 1 & 3 \\ 4 & 4 \\ 5 & 1 \\ 8 & 2 \\ \end{array} \end{eqnarray} $$

この離散的に与えられた指令点について、入力をX軸、出力をY軸として3次スプライン補間を用いて連続的で滑らかな経路を生成します。

３次多項式のパラメータを算出

前回の記事では、3次多項式のパラメータ$a_i$と$c_i$を算出しました。

$$ \begin{eqnarray} \begin{array}{c|cccc} i & a_i & b_i & c_i & d_i \\ \hline 0 & 0 & & 0 & \\ 1 & 3 & & -0.5417 & \\ 2 & 4 & & -1.2222 & \\ 3 & 1 & & 1.4028 & \\ \end{array} \end{eqnarray} $$

今回の記事では、残りのパラメータ$b_i$と$d_i$を算出していきます。

パラメータ$d_i$を求める

算出したパラメータ$c_i$を用いて、パラメータ$d_i$を求めていきます。

3次スプライン補間の条件より求めた関係式

$$ d_i = \frac{c_{i+1}-c_i}{3 h_i} $$

より、パラメータ$d_i$は$c_i$を用いて算出することが出来ます。

よって、3次多項式のパラメータ$d_i$は、

$$ d_0 = \frac{\frac{-13}{24}-0}{3 \cdot 1} = \frac{-13}{72} = -0.1806$$

$$ d_1 = \frac{\frac{-11}{9}-\frac{-13}{24}}{3 \cdot 3} = \frac{-49}{648} = -0.0756 $$

$$ d_2 = \frac{\frac{101}{72}-\frac{-11}{9}}{3 \cdot 1} = \frac{7}{8} = 0.875 $$

$$ d_3 = \frac{0-\frac{101}{72}}{3 \cdot 3} = \frac{-101}{648} = -0.1559 $$

と算出することが出来ました。

パラメータ$b_i$を求める

これまでに求めたパラメータ$a_i$と$c_i$より、パラメータ$b_i$を求めていきます。

他のパラメータの場合と同様に、3次スプライン補間の条件より求めた関係式

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

より、パラメータ$b_i$をパラメータ$a_i$と$c_i$を用いて算出することが出来ます。

よって、3次多項式のパラメータ$b_i$は、

$$ b_0 = \frac{3-0}{1} – \frac{1\cdot(\frac{-13}{24}+2 \cdot 0)}{3} $$

$$ = 3 + \frac{13}{72} = \frac{229}{72} = 3.1806 $$

$$ b_1 = \frac{4-3}{3} – \frac{3\cdot(\frac{-11}{9}+2 \cdot \frac{-13}{24})}{3} $$

$$ = \frac{1}{3} + \frac{83}{36} = \frac{95}{36} = 2.6389 $$

$$ b_2 = \frac{1-4}{1} – \frac{1\cdot(\frac{101}{72}+2 \cdot \frac{-11}{9})}{3} $$

$$ = -3 + \frac{25}{72} = \frac{-191}{72} = -2.6528 $$

$$ b_3 = \frac{2-1}{3} – \frac{3\cdot(0+2 \cdot \frac{101}{72})}{3} $$

$$ = \frac{1}{3} – \frac{202}{72} = \frac{-89}{36} = -2.4722 $$

と算出することが出来ました。

算出したパラメータを用いて3次スプライン補間を行う

これまでに算出した３次多項式のパラメータより、実際に3次スプライン補間を用いて連続的な軌跡を作成していきます。

与えられた離散点に対する３次多項式のパラメータは下記の通り求められました。

$$ \begin{eqnarray} \begin{array}{c|cccc} i & a_i & b_i & c_i & d_i \\ \hline 0 & 0 & 3.1806 & 0 & -0.1806 \\ 1 & 3 & 2.6389 & -0.5417 & -0.0756 \\ 2 & 4 & -2.6528 & -1.2222 & 0.875 \\ 3 & 1 & -2.4722 & 1.4028 & -0.1559 \\ \end{array} \end{eqnarray} $$

この算出したパラメータ（$a_i$、$b_i$、$c_i$、$d_i$）を用いて各区間（$x_i$と$x_{i+1}$）において3次多項式$S_i$を作成し、全体の軌跡を生成していきます。