軌跡生成

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

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

 

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

 

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

 

今回からの記事では、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 ] $$

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

 

左辺のSiは、変数uが距離0から距離liまで変化させた時の各軸の位置を表しており、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に対する位置Siを求めるためのパラメータ(AiBiCiDiEiFi)についても

$$ \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つのパラメータ(AiBiCiDiEiFi)を用いて、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つのパラメータ(AiBiCiDiEiFi)を求める方法を紹介していきます。

 

 

パラメータ算出の制約条件

5次スプライン曲線の基本式に含まれる6つのパラメータ(AiBiCiDiEiFi)を求める方法を紹介します。

 

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

 

この時、この2つの離散点間の距離li

$$ 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からliまでの範囲を取る変数となります。

 

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

 

位置条件

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

 

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

 

よって、5次スプライン曲線の始点はPiで終点はPi+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} $$

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

 

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

 

速度条件

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

 

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

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

 

この速度を表す式について、離散点Piでの速度をdPi/duとし、離散点Pi+1での速度をdPi+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式について、dSi(0)/duは始点での速度を表し、dSi(li)/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 $$

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

 

この加速度を表す式において、離散点PiPi+1での加速度がd2Pi/du2d2Pi+1/du2と与えられたとき

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

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

 

ここで、d2Si(0)/du2は始点での加速度を表し、d2Si(li)/du2は終点での加速度を表しています。

 

小まとめ

このように、位置と速度、加速度の制約条件から得られた6つの式より、6つの連立方程式を得ることが出来ます。

 

これら制約条件より得られた連立方程式として解くことで、5次スプライン曲線で補間するために必要な6つのパラメータ(AiBiCiDiEiFi)を求めることが出来ます。

 

 

まとめ

今回の記事では、離散的な点で与えられた指令を滑らかに制御するために用いられる、5次スプライン曲線について紹介しました。

 

5次スプライン曲線を用いることで、位置だけでなく速度と加速度を連続的に補間することが出来ます。

 

次回の記事では、今回紹介した各制約条件による連立方程式を解くことで、5次スプライン曲線のパラメータを求める方法を紹介したいと思います。

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

COMMENT

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です