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

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

しかし、通常ロボットに与えられる指令は離散的な点で与えられることが多いです。

よって、この離散的な点を連続的で滑らかな曲線で補間することが必要になります。

今回からの記事では、5次スプライン曲線を用いて離散的な点を補間する方法を紹介します。

Contents

5次スプライン曲線補間とは
パラメータ算出の制約条件
まとめ

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

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

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

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

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

離散的な指令点を滑らかに補間するために用いる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 ] $$

のように、補間したい離散点間の距離l_iで定義される範囲で表される変数です。

左辺のS_iは、変数uが距離0から距離l_iまで変化させた時の各軸の位置を表しており、3次元空間（x軸、y軸、z軸）の場合は

$$ \boldsymbol{S}_i (u) = \begin{bmatrix} S_{x,i}(u) \\ S_{y,i}(u) \\ S_{z,i}(u) \end{bmatrix} $$

のように、3次元のベクトルで定義されます。

そして、この変数uに対する位置S_iを求めるためのパラメータ（A_i、B_i、C_i、D_i、E_i、F_i）についても

$$ \begin{eqnarray} \left\{ \begin{array}{l} \boldsymbol{A}_i &=& \begin{bmatrix} A_{x,i} & A_{y,i} & A_{z,i} \end{bmatrix}^T \\ \boldsymbol{B}_i &=& \begin{bmatrix} B_{x,i} & B_{y,i} & B_{z,i} \end{bmatrix}^T \\ \boldsymbol{C}_i &=& \begin{bmatrix} C_{x,i} & C_{y,i} & C_{z,i} \end{bmatrix}^T \\ \boldsymbol{D}_i &=& \begin{bmatrix} D_{x,i} & D_{y,i} & D_{z,i} \end{bmatrix}^T \\ \boldsymbol{E}_i &=& \begin{bmatrix} E_{x,i} & E_{y,i} & E_{z,i} \end{bmatrix}^T \\ \boldsymbol{F}_i &=& \begin{bmatrix} F_{x,i} & F_{y,i} & F_{z,i} \end{bmatrix}^T \end{array} \right. \end{eqnarray} $$

のように、各軸に対するベクトルで定義します。

この基本式からも分かるように、変数（u）に対する6つのパラメータ（A_i、B_i、C_i、D_i、E_i、F_i）を用いて、5次のスプライン曲線による離散的な2点の補間を行います。

5次スプライン曲線の速度と加速度の式

5次スプライン曲線の基本式から、5次スプライン曲線の速度式と加速度式を求めることが出来ます。

まず、5次スプライン曲線の基本式を変数uについて微分することで、

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

のように5次スプライン曲線の速度を表す式を求めることが出来ます。

そして、この速度の式を変数uについて微分する（つまり基本式を変数uについて2階微分する）ことで、

$$ \frac{d^2 \boldsymbol{S}_i (u)}{d u^2} = 20 \boldsymbol{A}_i u^3 + 12 \boldsymbol{B}_i u^2 + 6 \boldsymbol{C}_i u + 2 \boldsymbol{D}_i $$

と5次スプライン曲線の加速度を表す式が求められます。

小まとめ

紹介したように、離散点を補間する際の速度と加速度についても位置を表す5次スプライン曲線の基本式に用いられているパラメータを用いて表すことが出来ます。

では、この5次スプライン曲線を描くために必要となる6つのパラメータ（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）を求める方法を紹介します。

5次のスプライン曲線を用いて補間したい2つの離散点（指令点）をP_iとP_i+1とします。

この時、この2つの離散点間の距離l_iは

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

のように求められます。

よって、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 ] $$

と0からl_iまでの範囲を取る変数となります。

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

位置条件

まず初めに、位置による制約条件を求めます。

いま、与えられた2つの離散点P_iとP_i+1を補間する5次スプライン曲線を考えています。

よって、5次スプライン曲線の始点はP_iで終点はP_i+1である必要があります。

この条件を式で表すと

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

という、位置による制約条件になります。

ここで、S_i(u)がS_i(0)のときは始点での位置を表し、S_i(u)がS_i(l_i)のときは終点での位置を表しています。

速度条件

同様に、速度による制約条件を求めます。

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について微分することで速度を表す式

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

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

この速度を表す式について、離散点P_iでの速度をdP_i/duとし、離散点P_i+1での速度をdP_i+1/duとすると、

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

のように、速度による制約条件を求めることが出来ます。

この2式について、dS_i(0)/duは始点での速度を表し、dS_i(l_i)/duは終点での速度を表しています。

加速度条件

最後に、加速度による制約条件を求めていきます。

5次スプライン曲線の基本式を変数uについて2階微分することで加速度を表す式

$$ \frac{d^2 \boldsymbol{S}_i (u)}{d u^2} = 20 \boldsymbol{A}_i u^3 + 12 \boldsymbol{B}_i u^2 + 6 \boldsymbol{C}_i u + 2 \boldsymbol{D}_i $$

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

この加速度を表す式において、離散点P_iとP_i+1での加速度がd²P_i/du²とd²P_i+1/du²と与えられたとき

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

のように、加速度による制約条件を表す式を求めることが出来ます。

ここで、d²S_i(0)/du²は始点での加速度を表し、d²S_i(l_i)/du²は終点での加速度を表しています。