スプライン曲線のパラメータを行列式で簡単に算出する方法

スプライン曲線を用いることで離散点を補間することが出来ます。

このスプライン曲線を書くためには、式に含まれる各係数のパラメータを求める必要があります。

以前の記事では、連立方程式を用いてパラメータを求める方法を紹介しました。

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

しかし、この方法では必要な計算量が多く、またスプライン曲線の次数が上がると計算がとても複雑になり、連立方程式を解くことが難しくなるという問題があります。

今回の記事では、行列式を用いることでスプライン曲線のパラメータを比較的簡単に算出する方法を紹介します。

Contents

スプライン曲線とパラメータ
- 5次スプライン曲線の基本式
- パラメータの式
行列式によるパラメータの算出
- 制約条件と行列式
- パラメータの算出
まとめ

スプライン曲線とパラメータ

スプライン曲線を用いることで、離散的な指令点を滑らかな曲線で補間することが出来ます。

取り扱うスプライン曲線の次数により、生成される軌跡の滑らかさが変わります。

今回の記事では、スプライン曲線の例として5次のスプライン曲線を取り扱います。

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

2つの離散的な点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 $$

となります。

ここで、基本式の変数uは

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

という2点P_iとP_i+1間の距離l_i

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

を用いた範囲で定義されています。

この基本式に含まれるパラメータ（A_i、B_i、C_i、D_i、E_i、F_i）を求めることで、与えられた離散点P_iとP_i+1を滑らかに補間することが出来ます。

パラメータの式

5次スプライン曲線の基本式に含まれるパラメータ（A_i、B_i、C_i、D_i、E_i、F_i）を、各離散点での制約条件から求めていきます。

5次スプライン曲線では、6つのパラメータを求めるために

$$ \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つ値を制約条件として取り扱います。

この6つの制約条件から得られた式を連立方程式として表すと

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

となります。

ここで、uは

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

です。

この連立方程式について各パラメータについて解くことで、

$$ \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つの関係式を連立方程式として解くことでパラメータを求めることが出来ます。

しかし、スプライン曲線の次数が上がると解くべき連立方程式の数が増え、解くことが難しくなります。

例えば、7次のスプライン曲線の場合は制約条件は各離散点での位置、速度、加速度、そして躍度と8個になり、解くべき連立方程式も8つになります。

そこで、スプライン曲線の次数が上がってもパラメータを求めることが出来るように、この連立方程式を用いた手法ではなく、行列式を用いてパラメータを求める方法を紹介します。

制約条件と行列式

先に紹介した連立方程式を用いた場合と同様に、各離散点での制約条件（位置、速度、加速度）から得られるパラメータの関係式は

となります。

ここで、5次スプラインのパラメータについて

$$ \boldsymbol{X} = \begin{bmatrix} \boldsymbol{A}_i \\ \boldsymbol{B}_i \\ \boldsymbol{C}_i \\ \boldsymbol{D}_i \\ \boldsymbol{E}_i \\ \boldsymbol{F}_i \end{bmatrix} $$

のようにベクトルXの形で表します。

また、各離散点での位置と速度、加速度に対する制約条件についても

$$ \boldsymbol{V} = \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} $$

のようにベクトルVの形で表します。

このパラメータと制約条件についてのベクトルを用いて、先の関係式を行列式の形で表すと

$$ \begin{eqnarray} \boldsymbol{M} \boldsymbol{X} &=& \boldsymbol{V} \\ \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} \end{eqnarray} $$

のように、変数uによる行列M

$$ \boldsymbol{M} = \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} $$

を用いて表すことが出来ます。

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

パラメータの算出

先ほどのパラメータによるベクトルXを含む行列式

$$ \boldsymbol{M} \boldsymbol{X} = \boldsymbol{V} $$

について、左辺の行列Mを右辺に移動させることで、ベクトルXについての式に変更します。

つまり、行列Mの逆行列を利用して

$$ \begin{eqnarray} \boldsymbol{M} \boldsymbol{X} = \boldsymbol{V} \\ \boldsymbol{M}^{-1} \boldsymbol{M} \boldsymbol{X} = \boldsymbol{M}^{-1} \boldsymbol{V} \\ \boldsymbol{X} = \boldsymbol{M}^{-1} \boldsymbol{V} \end{eqnarray} $$

のように式を変更します。

この逆行列の算出は行列Mの大きさが増えると難しくなりますが、MATLABなどを用いることで簡単に求めることができます。

このような各離散点による制約条件から得られる行列式を用いたパラメータ算出方法により、スプライン曲線の次数が上がっても簡単に解くことが出来ます。

また、プログラムを作成する際にも容易になるという利点があります。