Loading [MathJax]/jax/element/mml/optable/GeneralPunctuation.js
制御工学

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

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

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

 

今回と次回の記事では、実際の離散的な指令値を用いて、3次スプライン補間で連続的な軌跡を作成していきます。

 

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

離散点x_ix_{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次多項式のパラメータ(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次スプライン補間の条件より導かれた各パラメータの関係式を用いて、実際に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次スプライン補間を用いて連続的で滑らかな経路を生成します。

 

パラメータa_iを求める

3次多項式に含まれるパラメータa_iを求めていきます。

 

前回求めたパラメータa_iに関する関係式より、パラメータa_iは出力y_iと同値になります。

 

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

a_0 = y_0 = 0

a_1 = y_1= 3

a_2 = y_2= 4

a_3 = y_3= 1

a_4 = y_4= 2

と求めることが出来ます。

 

各区間の変化量h_iを求める

ここで、他のパラメータ(b_ic_id_i)を求める際に必要となる、各区間における入力xの変化量h_iを求めておきます。

 

離散点x_ix_{i+1}の間の区間における入力xの変化量h_i

h_i = x_{i+1} – x_i

で求めることが出来ます。

 

よって、各区間における入力の変化量は

h_0 = 1-0=1

h_1 = 4-1=3

h_2 = 5-4=1

h_3 = 8-5=3

となります。

 

パラメータc_iを求める

先程求めたパラメータa_iと変化量h_iから3次多項式のパラメータc_iを求めていきます。

 

前回求めたパラメータc_ia_iの関係式より、以下の行列式を作成します。

A \cdot c = b

ここで、行列Aとベクトルcb

A = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 1 & 8 & 3 & 0 & 0 \\ 0 & 3 & 8 & 1 & 0 \\ 0 & 0 & 1 & 8 & 3 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix}

c = \begin{bmatrix} c_0 \\ c_1 \\ c_2 \\ c_3 \\ c_4 \end{bmatrix}

b = \begin{bmatrix} 0 \\ \frac{3}{3}(4-3) – \frac{3}{1}(3-0) \\ \frac{3}{1}(1-4) – \frac{3}{3}(4-3) \\ \frac{3}{3}(2-1) – \frac{3}{1}(1-4) \\ 0 \end{bmatrix} = \begin{bmatrix} 0 \\ -8 \\ -10 \\ 10 \\ 0 \end{bmatrix}

とパラメータa_iと変化量h_iから求めることが出来ます。

 

また、3次スプライン補間の条件より、

c_0 = 0

c_4 = 0

が導かれます。

 

この行列式より以下の連立方程式を導くことが出来ます。

8 c_1 + 3 c_2 = -8

3 c_1 + 8 c_2 + c_3 = -10

c_2 + 8 c_3 = 10

 

この連立方程式を解くことで、3次多項式のパラメータc_iを求めます。

 

1つ目の式から、パラメータc_1c_2を用いて

8 c_1 + 3 c_2 = -8

\Rightarrow c_1 = \frac{-3 c_2 – 8}{8}

と表すことが出来ます。

 

同様に3つ目の式から、パラメータc_3c_2を用いて

c_2 + 8 c_3 = 10

\Rightarrow c_3 = \frac{-c_2 + 10}{8}

と表されます。

 

このパラメータc_1c_3について2式を2つ目の式に代入することで、パラメータc_2を求めていきます。

3 c_1 + 8 c_2 + c_3 = -10

\Rightarrow 3 \frac{-3 c_2 – 8}{8} + 8 c_2 + \frac{-c_2 + 10}{8} = -10

\Rightarrow -9 c_2 – 24 + 64 c_2 – c_2 + 10 = -80

\Rightarrow 54 c_2 = -66

\Rightarrow c_2  = \frac{-11}{9} = -1.2222

 

求めたパラメータc_2を用いて、パラメータc_1c_3を先に求めた式にc_2の値を代入して求めていきます。

c_1 = \frac{-3 c_2 – 8}{8}

\Rightarrow c_1 = \frac{-3 \frac{-11}{9} – 8}{8} = \frac{-13}{24} = -0.5417

c_3 = \frac{-c_2 + 10}{8}

\Rightarrow c_3 = \frac{-\frac{-11}{9} + 10}{8} = \frac{101}{72} = 1.4028

 

以上のように、3次多項式のパラメータc_iを求めることが出来ました。

 

逆行列を用いてパラメータc_iを求める

3次多項式のパラメータc_iについて、パラメータa_iとの関係式から導いた行列式から、逆行列を用いて算出することも出来ます。

 

パラメータc_iとパラメータa_iとの関係式より求められる行列式

A c = b

について、両辺の左側から行列Aの逆行列A^{-1}を掛けることで

A c = b

\Rightarrow A^{-1}A c = A^{-1} b

\Rightarrow c = A^{-1} b

と、パラメータc_iを示すベクトルを行列Aの逆行列A^{-1}と行列bから求めることが出来ます。

 

実際に先程の逆行列を用いた行列式からパラメータc_iを求めると

c_0 = 0

c_1 = -0.5417

c_2 = -1.2222

c_3 = 1.4028

c_4 = 0

となります。

 

これらパラメータc_iの値は、連立方程式を用いて算出された値と同じになります。

 

まとめ

今回は、実際の離散的な指令値を用いて、3次スプライン補間による連続的で滑らかな軌跡を作成する方法を紹介しました。

 

3次スプライン補間の条件より導かれた3次多項式のパラメータの関係式より、本記事ではパラメータa_ic_iを算出しました。

 

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

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

COMMENT

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です