スポンサーリンク

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

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

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

滑らかな指令の軌跡をロボットや機械に与えることで、ロボットや機械を滑らかに制御することが出来ます。 しかし、通常ロボッ...

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

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

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

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

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

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

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

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

この2つの離散点間(PiPi+1)の距離li

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

のように求められます。

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

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

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

この変数uを用いて、離散点PiPi+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 $$

と表すことが出来ます。

この基本式の中に含まれるパラメータ(AiBiCiDiEiFi)を求めることで、離散点間を補間する曲線を求めることが出来ます。

パラメータの制約条件

5次スプライン曲線の基本式に含まれる6つのパラメータ(AiBiCiDiEiFi)を算出するために、位置と速度、加速度の制約条件を求めます。

補間したい離散点PiPi+1について、点Piおける速度と加速度をdPi/dud2Pi/du2、点Pi+1における速度と加速度をdPi+1/dud2Pi+1/du2と与えられた場合を考えます。

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

$$ \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つのパラメータ(AiBiCiDiEiFi)を求めることが出来ます。

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

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

連立方程式を求める

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

$$ \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つの離散点(PiPi+1)間の距離liより変数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 $$

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

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

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

と、パラメータEiは点Piで速度dPi/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} $$

のように、パラメータDiを点Piでの加速度d2Pi/du2を用いて求めることが出来ます。

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

残りの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} $$

となります。

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

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

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

となります。

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

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

と表されます。

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

$$ – 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番目の式のli2倍を引いてCiを除くと

$$ -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} $$

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

この連立方程式の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} $$

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

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

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

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

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

ここで、パラメータAiBiCiについての連立方程式の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} $$

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

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

となります。

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

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

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

これらより、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次スプライン曲線の基本式について、与えられた制約条件からパラメータを求める方法を紹介しました。

今回紹介した方法でパラメータを算出することで、補間したい2つの離散点PiPi+1における位置と速度、そして加速度の情報をもとに、5次スプライン曲線で滑らかに補間することが可能になります。

次回の記事では、今回紹介した連立方程式ではなく行列式を用いてパラメータを算出する方法を紹介します。

スプライン曲線を用いることで離散点を補間することが出来ます。 このスプライン曲線を書くためには、式に含まれる各係数のパ...
スポンサーリンク
レクタングル(大)広告
レクタングル(大)広告

シェアする

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

フォローする

関連コンテンツ