3次スプライン補間で軌跡生成:連続的な軌跡を生成する(その2)

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

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

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

今回は、残りのパラメータ\(b_i\)と\(d_i\)を算出して、3次多項式を用いた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次スプライン補間の条件

  1. 条件1:各区間の始点
  2. 条件2:位置の連続性
  3. 条件3:速度の連続性
  4. 条件4:加速度の連続性
  5. 条件5:軌跡の始点と終点

を導くことが出来ます。

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

ロボットなどを動かす経路を指定する際に、通過してほしい点を離散的な指令点で指示することが多くあります。 この様な離散的...

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次スプライン補間の条件より導かれた各パラメータの関係式を用いて、実際に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次多項式のパラメータを算出

前回の記事では、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次多項式のパラメータより、実際に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\)を作成し、全体の軌跡を生成していきます。

上図からも分かるように、黒バツの指令値として与えられた離散点が、赤マルの通りに滑らかで連続的な軌跡となっています。

まとめ

本記事では前回に引き続き、3次スプライン補間に用いる3次多項式の残りのパラメータ\(b_i\)と\(d_i\)を算出して、実際に3次多項式を用いた3次スプライン補間による軌跡生成を実現しました。

結果からも分かるように、3次スプライン補間を用いることで離散的な指令値を連続的な軌跡に変換することができます。

よって、この滑らかで連続的に変換された軌跡を用いることで、ロボット等の機械をスムーズに制御することが可能になります。

スポンサーリンク
レクタングル(大)広告
レクタングル(大)広告

シェアする

  • このエントリーをはてなブックマークに追加

フォローする

関連コンテンツ