台形速度の軌跡生成をしてロボットを制御する方法

ロボットをスムーズに制御するために、台形速度プロファイルを用いた手法があります。

以前の記事では、この加速度を制限した台形速度手法について、利点と計算式について紹介しました。

ロボットの軌跡生成：加速度制限を用いて台形速度プロファイルを作成する ロボットの軌跡を生成する際に重要な項目として、 モータの出力できる限界を使って 短い時間で移動する という...

今回の記事では、実際に具体的な値を用いて計算を行うことで、より台形速度制御を行うための軌跡生成について理解を深めていきたいと思います。

加速度制限台形速度制御とは

加速度制限台形速度制御とは、軌跡生成に用いる加速度の値を制限することで、スムーズな台形の速度プロファイルを作成し制御する方法です。

台形速度プロファイル

加速度制限台形速度制御で用いられる台形速度プロファイルは、下の図のように表されます。

台形速度プロファイルは、全部で3つの期間

• 加速期間（T₁）
• 巡航期間（T₂）
• 減速期間（T₃）

で生成されています。

この台形速度プロファイルを作成するため、必要となるパラメータ（T₁、T₂、T₃）を求めます。

パラメータの算出

ロボットの初速V_Sから終速V_Eまでを結ぶスムーズな速度プロファイルを作成ためのパラメータ（T₁、T₂、T₃）を算出方法を紹介します。

ロボットの巡航速度をF[m/sec]とし、加速度をA[m/sec²]としたときの加速期間T₁と減速期間T₃は

$$ \begin{eqnarray} T_1 &=& \frac{F – V_S}{A} \\ T_3 &=& \frac{F – V_E}{A} \end{eqnarray} $$

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

この加速期間T₁と減速期間T₃で移動する距離（L₁とL₃）は

$$ \begin{eqnarray} L_1 &=& \frac{1}{2}(V_S+F) T_1 \\ L_3 &=& \frac{1}{2}(F+V_E) {T_3} \end{eqnarray}$$

と求められるため、ロボットの移動距離をLとすると巡航期間T₂は

$$ T_2 = \frac{L – L_1 – L_3}{F} $$

で算出することが出来ます。

詳しくは、こちらの記事を参考にしてください。

ロボットの軌跡生成：加速度制限を用いて台形速度プロファイルを作成する ロボットの軌跡を生成する際に重要な項目として、 モータの出力できる限界を使って 短い時間で移動する という...

実際に台形速度制御を行う

最大速度と最大加速度が5[ｍ/sec]と4[ｍ/sec²]の時に、停止状態（速度0[ｍ/sec]）のロボットが10[m]進んだ後に速度2[ｍ/sec]の状態になるような軌跡生成を行います。

よって、今回の記事で求める台形速度プロファイルの条件は

$$ \begin{eqnarray} \left\{ \begin{array}{l} L = 10 \ [m] \\ F = 5 \ [m/sec] \\ A = 4 \ [m/{sec}^2] \end{array} \right. \end{eqnarray} $$

と

$$ \begin{eqnarray} \left\{ \begin{array}{l} V_S = 0 \ [m/sec] \\ V_E = 2 \ [m/sec] \end{array} \right. \end{eqnarray} $$

になります。

これらの条件値を用いて、台形速度プロファイルの作成に必要となるパラメータの算出をしていきます。

加速期間T₁の算出

まずはじめに、加速期間T₁を求めていきます。

ロボットの巡航速度と加速度がF[m/sec]とA[m/sec²]、初速がV_S[m/sec]のときの加速期間T₁は

$$ T_1 = \frac{F – V_S}{A} $$

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

よって、この式に今回の条件

$$ \begin{eqnarray} \left\{ \begin{array}{l} F = 5 \ [m/sec] \\ A = 4 \ [m/{sec}^2] \\ V_S = 0 \end{array} \right. \end{eqnarray} $$

を代入すると、軌跡生成に必要となる加速期間T₁は

$$ \begin{eqnarray} T_1 &=& \frac{F – V_S}{A} \\ &=& \frac{5 – 0}{4} \\ &=& 1.25 \ [sec] \end{eqnarray} $$

と算出することが出来ます。

減速期間T₃の算出

加速期間T₁の場合と同様に減速期間T₃を求めていきます。

ロボットの巡航速度と加速度がF[m/sec]とA[m/sec²]、終速がV_E[m/sec]のときの減速期間T₃は

$$ T_3 = \frac{F – V_E}{A} $$

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

よって、この式に今回の条件

$$ \begin{eqnarray} \left\{ \begin{array}{l} F = 5 \ [m/sec] \\ A = 4 \ [m/{sec}^2] \\ V_E = 2 \end{array} \right. \end{eqnarray} $$

を代入すると、軌跡生成に必要となる加速期間T₁は

$$ \begin{eqnarray} T_1 &=& \frac{F – V_E}{A} \\ &=& \frac{5 – 2}{4} \\ &=& 0.75 \ [sec] \end{eqnarray} $$

と算出することが出来ます。

巡航期間T₂の算出

最後に、巡航期間T₂を求めていきます。

先に求めた加速期間T₁と減速期間T₃で移動する距離L₁とL₃は、

$$ \begin{eqnarray} L_1 &=& \frac{1}{2}(V_S+F) T_1 \\ L_3 &=& \frac{1}{2}(F+V_E) {T_3} \end{eqnarray}$$

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

よって、この式に今回の条件

$$ \begin{eqnarray} \left\{ \begin{array}{l} F = 5 \ [m/sec] \\ A = 4 \ [m/{sec}^2] \\ V_S = 0　\\ V_E = 2 \end{array} \right. \end{eqnarray} $$

を代入すると、加速期間T₁と減速期間T₃での移動距離L₁とL₃はそれぞれ

$$ \begin{eqnarray} L_1 &=& \frac{1}{2}(V_S+F) T_1 \\ &=& \frac{1}{2}(0+5) 1.25 \\ &=& 3.125 \ [m] \end{eqnarray} $$

と

$$ \begin{eqnarray} L_3 &=& \frac{1}{2}(F+V_E) T_3 \\ &=& \frac{1}{2}(5+2) 0.75 \\ &=& 2.625 \ [m] \end{eqnarray} $$

のように求めることが出来ます。

巡航期間T₂は、ロボットの移動距離をL、加速期間T₁と減速期間T₃での移動距離をそれぞれL₁とL₃とすると

$$ T_2 = \frac{L – L_1 – L_3}{F} $$

で算出することが出来るため、この式に今回の条件（FとL）と求めた値（L₁とL₃）

$$ \begin{eqnarray} \left\{ \begin{array}{l} F = 5 \ [m/sec] \\ L = 10 \ [m] \\ L_1 = 3.125 \ [m] \\ L_3 = 2.625 \ [m] \end{array} \right. \end{eqnarray} $$

を代入することで、

$$ \begin{eqnarray} T_2 &=& \frac{L – L_1 – L_3}{F} \\ &=& \frac{10 – 3.125 – 2.625}{5} \\ &=& 0.85 \ [sec] \end{eqnarray} $$

と算出することが出来ました。

台形速度制御を行った結果

算出したパラメータ

$$ \begin{eqnarray} \left\{ \begin{array}{l} T_1 = 1.25 \ [sec] \\ T_2 = 0.85 \ [sec] \\ T_3 = 0.75 \ [sec] \end{array} \right. \end{eqnarray} $$

を基に、台形速度プロファイルを生成した結果は下の図の通りです。

生成した台形速度制御の軌跡を見ると分かるように、台形速度プロファイルの3つの期間はそれぞれ算出したパラメータ（T₁、T₂、T₃）の値になっています。

そして、加速期間と減速期間の加速度は、条件で与えられた最大加速度の4[m/sec²]と-4[m/sec²]を用いています。

さらに巡航期間の速度も、条件で与えられた最大速度の5[m/sec]を用いています。

これより、今回紹介した軌跡生成手法は最大速度および最大加速度を用いていることから、時間最適（Time-optimal）のスムーズな台形速度プロファイルを生成できることが分かりました。

まとめ

今回は、ロボットをスムーズに動作させるために用いられている台形速度制御について、実際に具体的な値を用いて速度プロファイルを求める流れを紹介しました。

次回は、記事内で注意点として紹介した各期間の値が正の値でない場合（0や負の値の場合）niについて、加速度制限を用いて台形速度制御を行うための軌跡生成手法を紹介したいと思います。

加速度制限台形速度プロファイルによる軌跡生成の注意点：巡航期間が無い場合 加速度制限による台形速度プロファイルを用いることで、ロボットを滑らかに制御することが可能になります。 前回までの記...