こんにちはinukaiです!
“Cで実装する「実践ロボット制御」“の連載第4回目です。
前回までは、順運動学問題および逆運動学問題を解くことで、作業座標系において任意の手先位置を実現するための関節角度を求める方法を紹介しました。ただ、前回の例では位置制御モードのサーボモータに関節角度を指令するだけであったため、それぞれの関節のサーボモータがばらばらに目標位置へと動いていました。
第4~5回は、「実践ロボット制御」軌道の計画方法についての復習をしながら紹介します。あらかじめ軌道を計画しておくことで、手先の位置を望みの軌道にしたり、各関節のサーボモータの速度を指定するなどして滑らかにロボットを動かす方法を紹介します。
第4回は2回の記事に分けて、前半の4.1では軌道計画の概要について紹介し、後半の4.2では実際にCRANE-X7を動かすプログラムを作成していきます。
作業座標系での軌道計画と関節空間での軌道計画
目標軌道は、作業座標系で計画するか、関節空間で計画するかの2つの方法があります。垂直3自由度のロボットアームを考えて、それぞれの空間で直線的に軌道計画する具体例を見ていきましょう。
作業空間での軌道計画
作業座標系で直線的に軌道計画したときのロボットの描く軌跡は下図のようになります。
もちろんですが、作業空間上で直線的になるように軌道計画をしているため、手先はまっすぐに目標座標へと動きます。この手先の作業座標を\( \boldsymbol{p_d} = \begin{bmatrix}
p_{xd} \\
p_{yd} \\
p_{zd}
\end{bmatrix} \)とおいて数式で表すと、手先軌道の初期位置を\( \boldsymbol{p_0} \)、目標位置を\( \boldsymbol{p_f} \)として、
\begin{align}
\boldsymbol{p_d} = \boldsymbol{p_0} (1 – s) + \boldsymbol{p_f} s
\end{align}
で表されます(式(3.12))。
(「実践ロボット制御」中の式を参照しているものは式番号を赤字で表記しています)
ここで\( s \)は、初期位置と目標位置を内分するパラメータで、初期位置で\( s =0 \)、目標位置で\( s =1 \)という値を取ります。ここで式(3.12)は、作業座標系でどこを通るかを表していて、いつ通るかといった時間の情報は含まれていません。このように空間的にどこを通るかといった情報のみを表すものを軌跡といい、軌跡に時間的な情報を追加していつどこを通るかといった情報を含むものを軌道と呼ぶこととします。軌跡に時間的な情報を追加するには、パラメータ\( s \)を時間\( t \)の関数として表せばよく、例えば\( s \)を時間的に直線の関数とすると、
\begin{align}
s = \frac{t}{T}
\end{align}
となります(式(3.13))。ここで\( T \)は\( \boldsymbol{p_0} \)から\( \boldsymbol{p_f} \)まで移動する時間です。よって\( t=0 \)のとき\( s =0 \)、\( t=T \)のとき\( s =1 \)となります。すると軌道の式は、
\begin{align}
\boldsymbol{p_d} = \boldsymbol{p_0} (1 – \frac{t}{T}) + \boldsymbol{p_f} \frac{t}{T}
\end{align}
と表せます(式(3.14))。この式から得られる\( \boldsymbol{p_d} \)を、前回解いた逆運動学の式に代入して関節角度に変換したものを、ロボットアームの指令値として与えることで、軌道を実現することができます。実際には\( s \)を時間的に直線となる軌道をロボットに指令することは一般的に行われないため、\( s \)を時間的どう変化させるか( \( s \)にどういった関数を用いるか)については、本記事の後半で代表的なものをいくつか解説します。このように空間上のどこを通るかといった軌跡の式(作業座標\( \boldsymbol{p} \)と\( s \)の関係)と時間的にどう変化するか(時間\( t \)と\( s \)の関係)をそれぞれ独立に設計することで、空間的な情報と時間的な情報を分けて考えることができます。軌跡と時間軌道を分けて考えると,プログラムを実装するときに,軌跡はそのままで速度だけを変えるなど,汎用性の高いモジュールを作ることができるはずです。
ここで、作業座標で軌道計画する手順をまとめると、
- まず、現在の関節角度をロボットから取得し、順運動学問題を解くことで、作業座標系の初期位置\( \boldsymbol{p_0} \)を求める。
- 次に、初期位置\( \boldsymbol{p_0} \)から目標位置\( \boldsymbol{p_f} \)への軌跡と軌道を、作業座標系の座標\( \boldsymbol{p_d} \)として設計する。
- 設計した\( \boldsymbol{p_d} \)を、各時刻ごとに逆運動学を解いて関節角度に変換してロボットに指令値として与える。
といった流れになります。作業座標で軌道計画する場合は、あらかじめ作業空間上のどこを通るか設計できますが、一方で、逆運動学問題を各時刻ごとに解く必要があることや、各関節の可動範囲や最大速度をあらかじめ考慮することが難しくなるといった問題もあります。
関節空間での軌道計画
次に関節空間で軌道計画する方法について説明します。関節空間で直線的に軌道計画をすると、ロボットアームの手先は下の図のように動きます。
図の例では、初期位置から目標位置に手先を動かすときに、主に根元の第一関節が動くため、手先は弧を描くような軌跡になります。この関節角度の軌跡を\( \boldsymbol{\theta_d} = \begin{bmatrix}
\theta_1 \\
\theta_2 \\
\theta_3
\end{bmatrix} \)として数式で表すと、初期の関節角度\( \boldsymbol{\theta_0} \)、目標の関節角度\( \boldsymbol{\theta_f} \)として、
\begin{align}
\boldsymbol{\theta_d} = \boldsymbol{\theta_0} + \boldsymbol{\theta_f} s
\end{align}
で表されます(式(3.38))。\( s \)は先ほどと同様に、初期の関節角度で\( s =0 \)、目標の関節角で\( s =1 \)となるパラメータです。先ほどの作業空間の例と同様に、\( s \)を時間的に直線の関数として、軌跡に時間的な情報を追加すると、
\begin{align}
\boldsymbol{\theta_d} = \boldsymbol{\theta_0} (1 – \frac{t}{T}) + \boldsymbol{\theta_f} \frac{t}{T}
\end{align}
となります。得られた\( \boldsymbol{\theta_d} \)をロボットに角度指令値として与えることで、軌道を実現することができます。
関節空間で軌道計画する手順についてまとめると、
- まず、初期の関節角度\( \boldsymbol{\theta_0} \)をロボットから取得する。
- 次に、目標位置の座標\( \boldsymbol{p_f} \)から逆運動学問題を解いて、目標の関節角度\( \boldsymbol{\theta_f} \)に変換する。
- 初期の関節角度\( \boldsymbol{\theta_0} \)から目標の関節角度\( \boldsymbol{\theta_f} \)への軌跡と軌道を、関節空間で\( \boldsymbol{\theta_d} \)として設計し、各時刻ごとにロボットに指令値として与える。
といった流れになります。この場合は、目標位置に対して1回だけ逆運動学を解けばよいです。また、関節空間で軌跡・軌道を設計するため、各関節の可動範囲や最大速度を考慮することが容易です。一方で、結果的に手先が作業空間上のどこを通るかあらかじめ考えることが難しくなります。
軌道の時間関数の設計
目標軌道の計画方法について、作業空間で軌道計画する方法と関節空間での軌道計画する方法のそれぞれの流れや特徴を解説してきました。また、軌道計画問題について軌跡(どこを通るか)と軌道(時間的にどう変化するか)を分けて考えられることを紹介しました。軌道を考える際には、パラメータ\( s \)について説明を簡単にするために\( s \)を時間的に直線となる1次式\( s = \frac{t}{T} \)を用いていました。しかし、下図のようにパラメータ\( s \)のグラフを見ると、その時間微分である\( \dot{s} \)(速度の軌道)は、\( t=0 \)と\( t=T \)で不連続となり、この軌道でロボットを動かすと\( t=0 \)と\( t=T \)で急激な加減速によってロボットが振動してしまいます。そのため、\( t=0 \)と\( t=T \)で速度が不連続にならないような軌道について考えていきます。
3次の多項式に基づく軌道生成
時間関数が直線である1次式は、時刻\( t=0 \)で\( s=0 \)、\( t=T \)と\( s=1 \)となる最小次数の多項式です。さらに条件として、初期位置と目標位置で速度が0となるように、\( t=0 \)で\( \dot{s}=0 \)、\( t=T \)と\( \dot{s}=0 \)という条件を追加します。これを満たす最小次数の多項式は3次式、
\begin{align}
\dot{s} = a_3 { \left( \frac{t}{T} \right)}^3 + a_2 { \left( \frac{t}{T} \right)}^2 + a_1 \left( \frac{t}{T} \right) + a_0
\end{align}
です(式(3.17))。この式の各係数\( a_0 \)~\( a_3 \)は、時刻\( t=0 \)、\( t=T \)の時の\( s \)および\( \dot{s} \)の4つの条件から求めることができます。これらの条件を満たす3次の多項式をグラフで表すと下図になります。
このように、時間関数に3次の多項式を使うと、位置だけでなく速度も連続な軌道を設計することができます。しかし、この方法でも加速度は時刻\( t=0 \)、\( t=T \)で不連続になっているため、動き始めや止まるときに振動が生じます。1次式比べて、速度が連続、加速度が有限の値に収まっているため、3次式の方が振動は小さくなります。
5次の多項式に基づく軌道生成
次に、加速度も連続になる軌道を考えます。時刻\( t=0 \)、\( t=T \)の条件に、\( t=0 \)で\( \ddot{s}=0 \)、\( t=T \)と\( \ddot{s}=0 \)を追加します。これを満たす最小次数の多項式は5次式、
\begin{align}
\dot{s} = a_5 { \left( \frac{t}{T} \right)}^5 +a_4 { \left( \frac{t}{T} \right)}^4 +a_3 { \left( \frac{t}{T} \right)}^3 + a_2 { \left( \frac{t}{T} \right)}^2 + a_1 \left( \frac{t}{T} \right) + a_0
\end{align}
です(式(3.21))。この式の各係数\( a_0 \)~\( a_5 \)は、時刻\( t=0 \)、\( t=T \)の時の\( s \)、\( \dot{s} \)および\( \ddot{s} \)の6つの条件から求めることができます。これらの条件を満たす5次の多項式をグラフで表すと、
となり、加速度まで連続となり振動の生じにくい軌道を作ることができます。
時間の多項式に基づく軌道生成方法を紹介しましたが、3次の多項式と5次の多項式の共通の欠点として、最大速度となる点が時刻\( t=\frac{T}{2} \)の1点であり、関節のモータの性能を生かしきれないという点があります。次に、最大速度の区間を生かすために、定速度領域を指定できる軌道について紹介します。
速度台形則に基づく軌道生成
この方法は、その名前の通り、下図のように速度が台形となるような軌道になります。
グラフを見るとわかるように、加速度の加速区間、等速区間、減速区間に分割されます。等速区間では、速度のグラフが最大速度で一定となるため、ロボットのモータの定格速度や最大速度を考慮しながら軌道生成することが容易になります。また、モータの最大トルクなどにより、加速度の最大値が決まっている場合に、速度、加速度の最大値の条件から移動時間が最短となる軌道になります。ただし、この方法は加速度の波形が、時刻\( t=0 \)、\( t=T \)と、加速、等速、減速の切り替わる点の4点で不連続となります。速度台形則の数式等については、次回プログラムを作成していく際に紹介したいと思います。
ここまで、軌道の時間関数を設計する方法を3つ紹介しました。これらの3つ使い分けとしては、例えば、精密な動作のためなど振動が少ない軌道を使用したい場合は5次の多項式、多少の振動があってもさほど問題にならなく、モータの最大速度・最大加速度を生かしてできるだけ短い時間で動かしたい場合は速度台形則、その中間として3次の多項式を使用するなどが考えられます。
まとめ
第4回の前半では、ロボットアームの軌道生成の方法として、作業座標系で軌道計画をする方法と関節空間で軌道生成する2通りの方法があることを紹介しました。また、軌道計画には軌跡を表す式と、その時間変化含めた軌道の時間関数を別々に考えることができ、軌跡は直線の例、時間の関数については、時間の多項式および速度台形則に基づく軌道生成の方法を紹介しました。
次回以降の第4回の後半と第5回は、CRANE-X7を紹介した方法をもとに動かすプログラムを作成していきます。具体的には第4回の後半は関節空間での軌道計画、第5回は作業座標での軌道計画についてそれぞれ速度台形則を用いて動かしてみます。
それではまた次回!