5次スプライン曲線で離散点を滑らかに補間する方法

滑らかで連続的な指令を用いることで、ロボットや機械をスムーズに制御することが出来ます。

しかし、ロボットや機械を制御するための指令は、離散的で不連続な点で与えられることが多いです。

そこで、スプライン曲線を用いて離散的で不連続な指令点から、滑らかで連続的な軌跡を生成する方法があります。

5次スプライン曲線で離散点を補間してロボットの軌跡を生成する滑らかな指令の軌跡をロボットや機械に与えることで、ロボットや機械を滑らかに制御することが出来ます。しかし、通常ロ...

今回の記事では、5次スプライン曲線による離散点の補間方法を、実際に具体的な値を用いながら紹介していきたいと思います。

Contents

シミュレーション条件
パラメータ算出
- 行列式から求める
- 連立方程式の解から求める
シミュレーション結果
まとめ

シミュレーション条件

今回の記事では、5次スプライン曲線を利用した離散点補間の簡単な例として1軸のみを移動する場合を考えます。

5次スプライン曲線を用いた軌跡生成の基本については、こちらの記事を参考にしてください。

補間したい2つの離散点P₁とP₂について、それぞれ

$$ \begin{eqnarray} \left\{ \begin{array}{l} P_1 &=& 2 \ [m] \\ P_2 &=& 8 \ [m] \end{array} \right. \end{eqnarray} $$

と与えられた場合を考えます。

つまり2mの位置から8mの位置まで移動するロボットの軌跡を考えます。

そして、離散点P₁での速度と加速度をそれぞれV₁とA₁、離散点P₂での速度と加速度をV₂とA₂とすると

$$ \begin{eqnarray} \left\{ \begin{array}{l} V_1 &=& 1 \ [m/sec] \\ A_1 &=& 1 \ [m/{sec}^2] \end{array} \right. \\ \left\{ \begin{array}{l} V_2 &=& 3 \ [m/sec] \\ A_2 &=& 2 \ [m/{sec}^2] \end{array} \right. \end{eqnarray} $$

と与えられた場合を考えます。

このような2つの離散点での位置と速度、加速度が与えられた場合について、実際に5次スプライン曲線を用いて軌跡を生成する方法を紹介していきます。

パラメータ算出

与えられた補間したい2つの離散点の条件

$$ \begin{eqnarray} \left\{ \begin{array}{l} P_1 &=& 2 \ [m] \\ V_1 &=& 1 \ [m/sec] \\ A_1 &=& 1 \ [m/{sec}^2] \end{array} \right. \quad \left\{ \begin{array}{l} P_2 &=& 8 \ [m] \\ V_2 &=& 3 \ [m/sec] \\ A_2 &=& 2 \ [m/{sec}^2] \end{array} \right. \end{eqnarray} $$

について、5次スプライン曲線のパラメータを算出していきます。

行列式から求める

まず、与えられた2つの離散点P₁とP₂の距離l₁は

$$ l_1 = | P_2 – P_1 | = | 8 – 2 | = 6 [m] $$

と求められます。

ここで、5次スプライン曲線の基本式

$$ \boldsymbol{S}_i (u) = \boldsymbol{A}_i u^5 + \boldsymbol{B}_i u^4 + \boldsymbol{C}_i u^3 + \boldsymbol{D}_i u^2 + \boldsymbol{E}_i u + \boldsymbol{F}_i $$

のパラメータ（A_i、B_i、C_i、D_i、E_i、F_i）と各点での制約条件（位置、速度、加速度）についての関係式は

$$ \begin{bmatrix} {u_0}^5 & {u_0}^4 & {u_0}^3 & {u_0}^2 & u_0 & 1 \\ {u_1}^5 & {u_1}^4 & {u_1}^3 & {u_1}^2 & u_1 & 1 \\ 5 {u_0}^4 & 4 {u_0}^3 & 3 {u_0}^2 & 2 u_0 & 1 & 0 \\ 5 {u_1}^4 & 4 {u_1}^3 & 3 {u_1}^2 & 2 u_1 & 1 & 0 \\ 20 {u_0}^3 & 12 {u_0}^2 & 6 u_0 & 2 & 0 & 0 \\ 20 {u_1}^3 & 12 {u_1}^2 & 6 u_1 & 2 & 0 & 0 \end{bmatrix} \begin{bmatrix} \boldsymbol{A}_i \\ \boldsymbol{B}_i \\ \boldsymbol{C}_i \\ \boldsymbol{D}_i \\ \boldsymbol{E}_i \\ \boldsymbol{F}_i \end{bmatrix} = \begin{bmatrix} \boldsymbol{P}_i \\ \boldsymbol{P}_{i+1} \\ \frac{d \boldsymbol{P}_i}{d u} \\ \frac{d \boldsymbol{P}_{i+1}}{d u} \\ \frac{d^2 \boldsymbol{P}_i}{d u^2} \\ \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} \end{bmatrix} $$

という行列式の形で表すことが出来ます。

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

スプライン曲線のパラメータを行列式で簡単に算出する方法スプライン曲線を用いることで離散点を補間することが出来ます。このスプライン曲線を書くためには、式に含まれる各係数...

この行列式に含まれる変数u₀とu₁は

$$ \begin{eqnarray} \left\{ \begin{array}{l} u_0 &=& 0 \\ u_1 &=& l_i \end{array} \right. \end{eqnarray} $$

で定義されます。

この変数u₀とu₁はを行列式に代入すると

$$ \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 1 \\ {l_i}^5 & {l_i}^4 & {l_i}^3 & {l_i}^2 & l_i & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 5 {l_i}^4 & 4 {l_i}^3 & 3 {l_i}^2 & 2 l_i & 1 & 0 \\ 0 & 0 & 0 & 2 & 0 & 0 \\ 20 {l_i}^3 & 12 {l_i}^2 & 6 l_i & 2 & 0 & 0 \end{bmatrix} \begin{bmatrix} \boldsymbol{A}_i \\ \boldsymbol{B}_i \\ \boldsymbol{C}_i \\ \boldsymbol{D}_i \\ \boldsymbol{E}_i \\ \boldsymbol{F}_i \end{bmatrix} = \begin{bmatrix} \boldsymbol{P}_i \\ \boldsymbol{P}_{i+1} \\ \frac{d \boldsymbol{P}_i}{d u} \\ \frac{d \boldsymbol{P}_{i+1}}{d u} \\ \frac{d^2 \boldsymbol{P}_i}{d u^2} \\ \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} \end{bmatrix} $$

という式が得られます。

この行列式に距離l₁や各離散点での制約条件を代入すると、今回の記事で取り扱っているシミュレーション条件での5次スプライン曲線の各パラメータは

$$ \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 1 \\ 7776 & 1296 & 216 & 36 & 6 & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 6480 & 864 & 108 & 12 & 1 & 0 \\ 0 & 0 & 0 & 2 & 0 & 0 \\ 4320 & 432 & 36 & 2 & 0 & 0 \end{bmatrix} \begin{bmatrix} A_i \\ B_i \\ C_i \\ D_i \\ E_i \\ F_i \end{bmatrix} = \begin{bmatrix} 2 \\ 8 \\ 1 \\ 3 \\ 1 \\ 2 \end{bmatrix} $$

という行列式で表されることが分かりました。

この行列式をパラメータのベクトルについて解くことで、5次スプライン曲線のパラメータを求めることが出来ます。

よって、5次スプライン曲線の各パラメータ（A_i、B_i、C_i、D_i、E_i、F_i）は

$$ \begin{eqnarray} \begin{bmatrix} A_i \\ B_i \\ C_i \\ D_i \\ E_i \\ F_i \end{bmatrix} &=& \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 1 \\ 7776 & 1296 & 216 & 36 & 6 & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 6480 & 864 & 108 & 12 & 1 & 0 \\ 0 & 0 & 0 & 2 & 0 & 0 \\ 4320 & 432 & 36 & 2 & 0 & 0 \end{bmatrix}^{-1} \begin{bmatrix} 2 \\ 8 \\ 1 \\ 3 \\ 1 \\ 2 \end{bmatrix} \\ &=& \begin{bmatrix} -0.0008 & 0.0008 & -0.0023 & -0.0023 & -0.0023 & 0.0023 \\ 0.0116 & -0.0116 & 0.0370 & 0.0324 & 0.0417 & -0.0278 \\ -0.0463 & 0.0463 & -0.1667 & -0.1111 & -0.2500 & 0.0833 \\ 0 & 0 & 0 & 0 & 0.5 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 1 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} 2 \\ 8 \\ 1 \\ 3 \\ 1 \\ 2 \end{bmatrix} \\ &=& \begin{bmatrix} -0.0023 \\ 0.0509 \\ -0.3056 \\ 0.5 \\ 1 \\ 2 \end{bmatrix} \end{eqnarray} $$

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

連立方程式の解から求める

ちなみに、こちらの記事で紹介した方法により算出した連立方程式の解からでも、もちろん各パラメータを求めることが出来ます。

制約条件より得られた連立方程式を解くことで得られる、5次スプラインを用いて離散点を補間するために必要となるパラメータは

$$ \begin{eqnarray} \left\{ \begin{array}{l} \boldsymbol{A}_i &=& \frac{1}{{l_i}^5} \left[ 6 \left( \boldsymbol{P}_{i+1} – \boldsymbol{P}_i \right) – 3 l_i \left( \frac{d \boldsymbol{P}_{i+1}}{d u} + \frac{d \boldsymbol{P}_i}{d u} \right) + \frac{1}{2} {l_i}^2 \left( \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} – \frac{d^2 \boldsymbol{P}_i}{d u^2} \right) \right] \\ \boldsymbol{B}_i &=& \frac{1}{{l_i}^4} \left[ -15 \left( \boldsymbol{P}_{i+1} – \boldsymbol{P}_i \right) + l_i \left( 7 \frac{d \boldsymbol{P}_{i+1}}{d u} + 8 \frac{d \boldsymbol{P}_i}{d u} \right) – {l_i}^2 \left( \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} – \frac{3}{2} \frac{d^2 \boldsymbol{P}_i}{d u^2} \right) \right] \\ \boldsymbol{C}_i &=& \frac{1}{{l_i}^3} \left[ 10 \left( \boldsymbol{P}_{i+1} – \boldsymbol{P}_i \right) – l_i \left( 4 \frac{d \boldsymbol{P}_{i+1}}{d u} + 6 \frac{d \boldsymbol{P}_i}{d u} \right) + {l_i}^2 \left( \frac{1}{2} \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} – \frac{3}{2} \frac{d^2 \boldsymbol{P}_i}{d u^2} \right) \right] \\ \boldsymbol{D}_i &=& \frac{1}{2} \frac{d^2 \boldsymbol{P}_i}{d u^2} \\ \boldsymbol{E}_i &=& \frac{d \boldsymbol{P}_i}{d u} \\ \boldsymbol{F}_i &=& \boldsymbol{P}_i \end{array} \right. \end{eqnarray} $$

という関係になります。

詳しい算出方法は、こちらの記事を参考にしてください。

5次スプライン曲線で離散点を補間してロボットの軌跡を生成する（２） 5次スプライン曲線を用いて滑らかな軌跡を生成することで、ロボットや機械を滑らかに制御することが出来ます。前回の記...

この6つの式にそれぞれ値を代入することで、

$$ \begin{eqnarray} \left\{ \begin{array}{l} \boldsymbol{A}_i &=& \frac{1}{{l_i}^5} \left[ 6 \left( \boldsymbol{P}_{i+1} – \boldsymbol{P}_i \right) – 3 l_i \left( \frac{d \boldsymbol{P}_{i+1}}{d u} + \frac{d \boldsymbol{P}_i}{d u} \right) + \frac{1}{2} {l_i}^2 \left( \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} – \frac{d^2 \boldsymbol{P}_i}{d u^2} \right) \right] \\ &=& \frac{1}{6^5} \left[ 6 \left( 8 – 2 \right) – 3 \cdot 6 \left( 3 + 1 \right) + \frac{1}{2} 6^2 \left( 2 – 1 \right) \right] \\ &=& -0.0023 \\ \boldsymbol{B}_i &=& \frac{1}{{l_i}^4} \left[ -15 \left( \boldsymbol{P}_{i+1} – \boldsymbol{P}_i \right) + l_i \left( 7 \frac{d \boldsymbol{P}_{i+1}}{d u} + 8 \frac{d \boldsymbol{P}_i}{d u} \right) – {l_i}^2 \left( \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} – \frac{3}{2} \frac{d^2 \boldsymbol{P}_i}{d u^2} \right) \right] \\ &=& \frac{1}{6^4} \left[ -15 \left( 8 – 2 \right) + 6 \left( 7 \cdot 3 + 8 \cdot 1 \right) – 6^2 \left( 2 – \frac{3}{2} \cdot 1 \right) \right] \\ &=& 0.0509 \\ \boldsymbol{C}_i &=& \frac{1}{{l_i}^3} \left[ 10 \left( \boldsymbol{P}_{i+1} – \boldsymbol{P}_i \right) – l_i \left( 4 \frac{d \boldsymbol{P}_{i+1}}{d u} + 6 \frac{d \boldsymbol{P}_i}{d u} \right) + {l_i}^2 \left( \frac{1}{2} \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} – \frac{3}{2} \frac{d^2 \boldsymbol{P}_i}{d u^2} \right) \right] \\ &=& \frac{1}{6^3} \left[ 10 \left( 8 – 2 \right) – 6 \left( 4 \cdot 3 + 6 \cdot 1 \right) + 6^2 \left( \frac{1}{2} \cdot 2 – \frac{3}{2} \cdot 1 \right) \right] \\ &=& -0.3056 \\ \boldsymbol{D}_i &=& \frac{1}{2} \frac{d^2 \boldsymbol{P}_i}{d u^2} = 0.5 \\ \boldsymbol{E}_i &=& \frac{d \boldsymbol{P}_i }{d u} = 1 \\ \boldsymbol{F}_i &=& \boldsymbol{P}_i = 2 \end{array} \right. \end{eqnarray} $$

のように、5次スプラインのパラメータを求めることも出来ます。