5次スプライン曲線で離散点を補間してロボットの軌跡を生成する（２）

5次スプライン曲線を用いて滑らかな軌跡を生成することで、ロボットや機械を滑らかに制御することが出来ます。

前回の記事では、5次スプライン曲線の基本式とパラメータについて紹介しました。

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

今回の記事では、前回紹介した5次スプライン曲線の基本式について、与えられた制約条件からパラメータを求める方法を紹介します。

Contents

5次スプライン曲線補間とは
- 5次スプライン曲線の基本式
- パラメータの制約条件
5次スプライン曲線のパラメータ
- 連立方程式を求める
- 連立方程式を解く
まとめ

5次スプライン曲線補間とは

前回の記事のおさらいです。

5次スプライン曲線補間とは、その名の通り5次のスプラインによる曲線を用いて離散点を補間する方法のことです。

5次のスプライン曲線を用いることで、各指令点での位置だけでなく速度と加速度を連続的に補間することが可能になります。

そのため、ロボットや機械を高速で正確に制御することが出来ます。

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

2つの離散点（指令点）P_iとP_i+1を補間するための5次スプライン曲線を考えます。

この2つの離散点間（P_iとP_i+1）の距離l_iは

$$ l_i = \vert \boldsymbol{P}_{i+1} – \boldsymbol{P}_i \vert $$

のように求められます。

これより、5次スプライン曲線の変数uは

$$ u \in [ 0 , l_i ] $$

と、0からl_iまでの変数となります。

この変数uを用いて、離散点P_iとP_i+1を補間するため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）を求めることで、離散点間を補間する曲線を求めることが出来ます。

パラメータの制約条件

5次スプライン曲線の基本式に含まれる6つのパラメータ（A_i、B_i、C_i、D_i、E_i、F_i）を算出するために、位置と速度、加速度の制約条件を求めます。

補間したい離散点P_iとP_i+1について、点P_iおける速度と加速度をdP_i/duとd²P_i/du²、点P_i+1における速度と加速度をdP_i+1/duとd²P_i+1/du²と与えられた場合を考えます。

これら各離散点での位置、速度、加速度の値を制約条件とすると、

$$ \begin{eqnarray} \left\{ \begin{array}{l} \boldsymbol{S}_i (u=0) = \boldsymbol{P}_i \\ \boldsymbol{S}_i (u=l_i) = \boldsymbol{P}_{i+1} \\ \frac{d \boldsymbol{S}_i (u=0)}{d u} = \frac{d \boldsymbol{P}_i}{d u} \\ \frac{d \boldsymbol{S}_i (u=l_i)}{d u} = \frac{d \boldsymbol{P}_{i+1}}{d u} \\ \frac{d^2 \boldsymbol{S}_i (u=0)}{d u^2} = \frac{d^2 \boldsymbol{P}_i}{d u^2} \\ \frac{d^2 \boldsymbol{S}_i (u=l_i)}{d u^2} = \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} \end{array} \right. \end{eqnarray} $$

という、6つの式からなる制約条件が得られます。

これら制約条件より得られる連立方程式を解くことで、5次スプライン曲線で離散点を補間するために必要となる6つのパラメータ（A_i、B_i、C_i、D_i、E_i、F_i）を求めることが出来ます。

5次スプライン曲線のパラメータ

先ほどの位置と速度、加速度の各制約条件から得られた6つの関係式から連立方程式を作成し、5次スプライン曲線のパラメータを算出します。

連立方程式を求める

5次スプライン曲線の制約条件を示す6つ式について、曲線の始点での変数uをu₀、終点での変数uをu₁とすると

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

という連立方程式で表されます。

ここで、補間したい2つの離散点（P_iとP_i+1）間の距離l_iより変数uは

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

となります。

よって、制約条件から得られた5次スプライン曲線のパラメータを算出するための連立方程式に変数uの値を代入することで

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

と連立方程式を変換することが出来ます。

この6つの式からなる連立方程式を解くことで、5次スプライン曲線のパラメータを算出していきます。

連立方程式を解く

まず、連立方程式に含まれる6つの式の内、簡単な3つの式（1,3,5番目の式）を用いてパラメータを算出します。

まず、連立方程式の1番目の式より、

$$ \boldsymbol{F}_i = \boldsymbol{P}_i $$

のように、パラメータF_iは点P_iでの位置で表されることが分かります。

同様に、連立方程式の3番目の式より、

$$ \boldsymbol{E}_i = \frac{d \boldsymbol{P}_i}{d u} $$

と、パラメータE_iは点P_iで速度dP_i/duで算出することが出来ます。

そして、連立方程式の5番目の式から、

$$ \begin{eqnarray} 2 \boldsymbol{D}_i &=& \frac{d^2 \boldsymbol{P}_i}{d u^2} \\ \boldsymbol{D}_i &=& \frac{1}{2} \frac{d^2 \boldsymbol{P}_i}{d u^2} \end{eqnarray} $$

のように、パラメータD_iを点P_iでの加速度d²P_i/du²を用いて求めることが出来ます。

ここまでで、連立方程式の1、3、５番目の式からパラメータD_i、E_i、F_iを求めました。

残りの2、4、6番目で連立方程式を作成すると

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

となります。

この連立方程式を解くことで、残りのパラメータA_i、B_i、C_iを求めていきます。

連立方程式に含まれるパラメータD_i、E_i、F_iについて、先に求めた値

$$ \begin{eqnarray} \left\{ \begin{array}{l} \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} $$

を代入すると

$$ \begin{eqnarray} \left\{ \begin{array}{l} {l_i}^5 \boldsymbol{A}_i + {l_i}^4 \boldsymbol{B}_i + {l_i}^3 \boldsymbol{C}_i + {l_i}^2 \frac{1}{2} \frac{d^2 \boldsymbol{P}_i}{d u^2} + {l_i} \frac{d \boldsymbol{P}_i}{d u} + \boldsymbol{P}_i &=& \boldsymbol{P}_{i+1} \\ 5 {l_i}^4 \boldsymbol{A}_i + 4 {l_i}^3 \boldsymbol{B}_i + 3 {l_i}^2 \boldsymbol{C}_i + 2 {l_i} \frac{1}{2} \frac{d^2 \boldsymbol{P}_i}{d u^2} + \frac{d \boldsymbol{P}_i}{d u} &=& \frac{d \boldsymbol{P}_{i+1}}{d u} \\ 20 {l_i}^3 \boldsymbol{A}_i + 12 {l_i}^2 \boldsymbol{B}_i + 6 {l_i} \boldsymbol{C}_i + 2 \frac{1}{2} \frac{d^2 \boldsymbol{P}_i}{d u^2} &=& \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} \end{array} \right. \end{eqnarray} $$

となります。

この連立方程式をパラメータA_i、B_i、C_iについて整理すると

$$ \begin{eqnarray} \left\{ \begin{array}{l} {l_i}^5 \boldsymbol{A}_i + {l_i}^4 \boldsymbol{B}_i + {l_i}^3 \boldsymbol{C}_i &=& \boldsymbol{P}_{i+1} – \boldsymbol{P}_i – {l_i} \frac{d \boldsymbol{P}_i}{d u} – \frac{1}{2} {l_i}^2 \frac{d^2 \boldsymbol{P}_i}{d u^2} \\ 5 {l_i}^4 \boldsymbol{A}_i + 4 {l_i}^3 \boldsymbol{B}_i + 3 {l_i}^2 \boldsymbol{C}_i &=& \frac{d \boldsymbol{P}_{i+1}}{d u} – \frac{d \boldsymbol{P}_i}{d u} – {l_i} \frac{d^2 \boldsymbol{P}_i}{d u^2} \\ 20 {l_i}^3 \boldsymbol{A}_i + 12 {l_i}^2 \boldsymbol{B}_i + 6 {l_i} \boldsymbol{C}_i &=& \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} – \frac{d^2 \boldsymbol{P}_i}{d u^2} \end{array} \right. \end{eqnarray} $$

と表されます。

このパラメータA_i、B_i、C_iについての連立方程式を解くために、1番目の式の3倍から2番目の式のl_i倍を引いてC_iを除くと

$$ – 2 {l_i}^5 \boldsymbol{A}_i – {l_i}^4 \boldsymbol{B}_i = 3 \boldsymbol{P}_{i+1} – 3 \boldsymbol{P}_i – l_i \frac{d \boldsymbol{P}_{i+1}}{d u} – 2 {l_i} \frac{d \boldsymbol{P}_i}{d u} – \frac{1}{2}{l_i}^2 \frac{d^2 \boldsymbol{P}_i}{d u^2} $$

となります。

同様に、1番目の式の6倍から3番目の式のl_i²倍を引いてC_iを除くと

$$ -14 {l_i}^5 \boldsymbol{A}_i – 6 {l_i}^4 \boldsymbol{B}_i = 6 \boldsymbol{P}_{i+1} – 6 \boldsymbol{P}_i – 6 {l_i} \frac{d \boldsymbol{P}_i}{d u} – {l_i}^2 \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} – 2 {l_i}^2 \frac{d^2 \boldsymbol{P}_i}{d u^2} $$

となります。

よって、この算出した2つの式の式は

$$ \begin{eqnarray} \left\{ \begin{array}{l} – 2 {l_i}^5 \boldsymbol{A}_i – {l_i}^4 \boldsymbol{B}_i &=& 3 \boldsymbol{P}_{i+1} – 3 \boldsymbol{P}_i – l_i \frac{d \boldsymbol{P}_{i+1}}{d u} – 2 {l_i} \frac{d \boldsymbol{P}_i}{d u} – \frac{1}{2}{l_i}^2 \frac{d^2 \boldsymbol{P}_i}{d u^2} \\ -14 {l_i}^5 \boldsymbol{A}_i – 6 {l_i}^4 \boldsymbol{B}_i &=& 6 \boldsymbol{P}_{i+1} – 6 \boldsymbol{P}_i – 6 {l_i} \frac{d \boldsymbol{P}_i}{d u} – {l_i}^2 \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} – 2 {l_i}^2 \frac{d^2 \boldsymbol{P}_i}{d u^2} \end{array} \right. \end{eqnarray} $$

と、パラメータA_iとB_iによる連立方程式で表すことが出来ます。

この連立方程式の2式について、1つ目の式の6倍から2つ目の式を引くことで

$$ 2 {l_i}^5 \boldsymbol{A}_i =12 \boldsymbol{P}_{i+1} – 12 \boldsymbol{P}_i – 6 l_i \frac{d \boldsymbol{P}_{i+1}}{d u} – 6 {l_i} \frac{d \boldsymbol{P}_i}{d u} + {l_i}^2 \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} – {l_i}^2 \frac{d^2 \boldsymbol{P}_i}{d u^2} $$

のようにパラメータB_iを除くことができ、パラメータA_iを

$$ \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] $$

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

同様に、1つ目の式の7倍から2つ目の式を引くことで

$$ – {l_i}^4 \boldsymbol{B}_i =15 \boldsymbol{P}_{i+1} – 15 \boldsymbol{P}_i – 7 l_i \frac{d \boldsymbol{P}_{i+1}}{d u} – 8 {l_i} \frac{d \boldsymbol{P}_i}{d u} + {l_i}^2 \frac{d^2 \boldsymbol{P}_{i+1}}{d u^2} – \frac{3}{2} {l_i}^2 \frac{d^2 \boldsymbol{P}_i}{d u^2} $$

のようにパラメータA_iを除くことができ、パラメータB_iを

$$ \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] $$

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

ここで、パラメータA_i、B_i、C_iについての連立方程式の1つ目の式

$$ {l_i}^5 \boldsymbol{A}_i + {l_i}^4 \boldsymbol{B}_i + {l_i}^3 \boldsymbol{C}_i = \boldsymbol{P}_{i+1} – \boldsymbol{P}_i – {l_i} \frac{d \boldsymbol{P}_i}{d u} – \frac{1}{2} {l_i}^2 \frac{d^2 \boldsymbol{P}_i}{d u^2} $$

についてパラメータC_iについて解くと

$$ \boldsymbol{C}_i = \frac{1}{{l_i}^3} \left[ \boldsymbol{P}_{i+1} – \boldsymbol{P}_i – {l_i} \frac{d \boldsymbol{P}_i}{d u} – \frac{1}{2} {l_i}^2 \frac{d^2 \boldsymbol{P}_i}{d u^2} – {l_i}^5 \boldsymbol{A}_i – {l_i}^4 \boldsymbol{B}_i \right] $$

となります。

このパラメータC_iについての式に求めたパラメータA_iとB_iを代入すると

$$ \begin{eqnarray} \boldsymbol{C}_i &=& \frac{1}{{l_i}^3} \left[ \boldsymbol{P}_{i+1} – \boldsymbol{P}_i – {l_i} \frac{d \boldsymbol{P}_i}{d u} – \frac{1}{2} {l_i}^2 \frac{d^2 \boldsymbol{P}_i}{d u^2} \\ – \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] \\ – \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] \right] \end{eqnarray} $$

となり、この式を整理すると

$$ \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] $$

のように、パラメータC_iを求めることが出来ました。

これらより、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} $$

と算出できることが分かりました。