3次スプライン補間で軌跡生成:3次多項式のパラメータを求める(その2)

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

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

3次スプライン補間を用いた離散点から連続的な軌跡を生成する方法を紹介します。 前回の記事では、3次スプライン補間に用い...

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

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

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

  1. 条件1:各区間の始点
  2. 条件2:位置の連続性
  3. 条件3:速度の連続性
  4. 条件4:加速度の連続性
  5. 条件5:軌跡の始点と終点
ロボットなどを動かす経路を指定する際に、通過してほしい点を離散的な指令点で指示することが多くあります。 この様な離散的...

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

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

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

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

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

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

各区間の3次多項式\(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) $$

に加えて、条件5より補間された軌跡の始点と終点での加速度は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}\)を求めることが出来ます。

まとめ

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

今回の方法を用いることで、3次多項式のパラメータ\(c_{0,\cdots,n}\)を前回求めたパラメータ\(a_{0,\cdots,n}\)を用いて求めることが出来ました。

また、今回求めたパラメータ\(c_{0,\cdots,n}\)を用いて、前回の記事での情報を用いて他のパラメータ\(b_i\)と\(d_i\)を求めることが出来ます。

次回は、前回を今回の記事で紹介した方法を用いて、実際に3次多項式のパラメータを求めて離散点を連続的に補間していきたいと思います。

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

シェアする

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

フォローする