ロボットアームの制御について

　ロボットアームの制御をする場合、足回りと同じように、目標の座標から目標の角度を逆運動学で解析し、現在の角度との誤差に対してPID制御などのフィードバック制御を適用する しかし、これを行うには以下のような問題がある

• 三自由度以下のロボットアームでなければならない ¹
• ロボットアームは基本、非線形なため、静止動作含め制御するのが難しい

分解加速度制御は、この2つ目の非線形なモデルを制御する方法であるといえる 簡潔に説明すると、ロボットアームの運動方程式に基づいてトルクを計算することで、非線形状態フィードバック制御を行うような制御方法である よって、運動方程式の計算にラグランジュの運動方程式を使用するため、これから説明する

※7月19日追記。最近、twitterで聞かれたので、これを機会に書き直してみることにする。 ※2022年12月8日変更。ちゃんと書き直した。

ラグランジュの運動方程式

ラグランジュの運動方程式とか一般人は知らないので説明する まず、以下の式が公式である

$$\frac{d}{dt}\left(\frac{\partial \boldsymbol{L}}{\partial \dot{x}}\right)-\frac{\partial \boldsymbol{L}}{\partial x}=0$$

• tは時間
• xは位置
• Lはラグランジアン

を意味している

ラグランジアンは以下を示すものである L = T ー U Tは運動エネルギー Uはポテンシャルエネルギー を意味している

ポテンシャルエネルギーは、位置エネルギーを含むものなので、とりあえず位置エネルギーだと思っておけばよい

導出

ニュートンの運動方程式

ここから、ラグランジュの運動方程式がニュートンの運動方程式と同じになることを説明する 大学物理ではニュートンの運動方程式は以下のように示す

$$m\ddot{x}=\boldsymbol{F}$$

• mは質量
• Fは力

を意味している ここでＦはポテンシャルエネルギーから受ける力であるため、以下のようにあらわせる

$$\boldsymbol{F}=-\frac{\partial \boldsymbol{U}}{\partial x}$$

ラグランジュの運動方程式

大学物理では運動エネルギーは以下のように示す

$$\boldsymbol{T}=\frac{1}{2}m\dot{x}^2$$

よって、運動エネルギーはXの一階微分(速度)の関数といえるため、ラグランジアンは

$$\boldsymbol{L}=\boldsymbol{T}(\dot{x})-U(x)$$

と表現できる。 これを偏微分すると以下のようになる

$$\frac{\partial \boldsymbol{L}}{\partial \dot{x}}=\frac{\partial \boldsymbol{T}}{\partial \dot{x}}=m\dot{x},\;\frac{\partial \boldsymbol{L}}{\partial x}=-\frac{\partial U}{\partial x}$$

右の式はニュートンの運動方程式の右辺と同じである。 また、左の式を t で微分すると以下のようになる。

$$\frac{d}{dt}\left(\frac{\partial \boldsymbol{L}}{\partial \dot{x}}\right)=m\ddot{x}$$

これは、ニュートンの運動方程式の左辺と同じである ここで、ラグランジュの運動方程式は、以下であったため、

$$\frac{d}{dt}\left(\frac{\partial \boldsymbol{L}}{\partial \dot{x}}\right)-\frac{\partial \boldsymbol{L}}{\partial x}=0$$

移項すれば、ラグランジュの運動方程式とニュートンの運動方程式は同じであることがわかる。

分解加速度制御 ロボットアームの運動方程式と制御

運動方程式の導出

ロボットアームの運動方程式は、ラグランジュの運動方程式を使用して計算する

※今回は簡単のために、１関節のロボットアームを考えることにする。 ※上の説明で x (位置) を示していたものは、ロボットアームで扱うのは角度であるため、θで表現する

ラグランジュの運動方程式は、以下の式になる

$$\frac{d}{dt}\left(\frac{\partial \boldsymbol{L}}{\partial \dot{\theta}}\right)-\frac{\partial \boldsymbol{L}}{\partial \theta}=\tau$$

• θはアーム関節の角度
• τ(タウ)は加えられる力 (目標トルクとなる)

を意味している

また、ラグランジアンは以下のようになる (関節数が増えると複雑になる)

$$\boldsymbol{L}=\frac{1}{2}ml^2\dot{\theta}^2+\frac{1}{2}I\dot{\theta}^2-mglcos\theta$$

• Iは慣性モーメント
• l(エル)はアームの長さ

を意味している

偏微分すると、以下のようになる

$$\frac{\partial \boldsymbol{L}}{\partial \dot{\theta}}=ml^2\dot{\theta}+I\dot{\theta},\;\frac{\partial \boldsymbol{L}}{\partial \theta}=mglsin\theta$$

また、左の式は t で微分しないといけないので、

$$\frac{d}{dt}\left(\frac{\partial \boldsymbol{L}}{\partial \dot{\theta}}\right)=ml^2\ddot{\theta}+I\ddot{\theta}$$

よって、このロボットアームの運動方程式は、

$$ml^2\ddot{\theta}+I\ddot{\theta}-mglsin\theta=\tau$$

となる

よく見ると、左辺は慣性力と重力を表していることに気が付くだろう これは、一般的に以下のような式で表現される

$$J(\theta)\ddot{\theta} + G(\theta) = \tau$$

• Jは慣性モーメントとかの行列
• Gは重力

を意味している

２関節以上になると遠心力・コリオリ力が出てくる さらに、摩擦力を考慮してラグランジアンを立てれば同じように計算できる

角加速度の計算

θの二階微分以外は、慣性モーメントと重力なので、アーム角度から容易に計算することができる つまり、θの二階微分つまり角加速度を計算すれば目標トルクを求めることが可能となる よって、以下のような非線形状態フィードバック補償を考える

$$J(\theta)\tau^{\prime}+ G(\theta) = \tau$$

(θの二階微分がτプライムに置き換わっている)

これにより線形な系まで落とし込むことができた しかし、運動方程式とのモデル誤差は避けられないため、これを補償するために、サーボ補償器というものを定義する

サーボ補償器は以下のようなものである

$$\tau^{\prime} = \ddot{\theta_{d}} + K_{d}(\dot{\theta_{d} }- \dot{\theta}) + K_{p}(\theta_{d} - \theta)$$

• θdは目標角度
• KdはDゲイン
• KpはPゲイン

を意味している

θdーθ=eと置くと、ゲインを適切に求めることで、eを0に収束させることができることがわかる つまりPD制御ってこと

座標系の変換（上平のオリジナル）

この状態で制御をするには、すべてθベース、つまりアームの関節角度ベースで制御しなければならない 低能な僕では、ゲインをどのくらいにすればよいのか見当もつかないし、 経路生成的問題でも、普通直交座標系の経路を生成するはずなので、 直交座標系でPD制御を行いたい

よって、以下のようにサーボ補償器を定義し直す

$$\tau^{\prime\prime}=\ddot{x}_{d}+K_{d}(\dot{x}_{d}-\dot{x})+K_{p}(x_{d}-x)$$

しかし、これでは座標系が違うので、これを座標変換することを考える ３ステップあるため、順に説明する まず、角速度を速度に変換する式は、

$$\dot{x}(t) = J(t)\dot{\theta}(t)$$

• J(t)はヤコビ行列

を意味している

つぎに、両辺をtで微分すると

$$\ddot{x}(t) = \dot{J}\dot{\theta}(t) + J\ddot{\theta}(t)$$

これを移項とかすると

$$\tau^{\prime} = \ddot{\theta}(t) = J^{-1}(\ddot{x}(t) - \dot{J}\dot{\theta}(t))$$

になる よって、上のサーボ補償器で計算したxの二階微分をこれに代入して、目標角加速度を計算する

これで一通り終わりだが、実際にはモデル化誤差が問題になってくる。そのため、式に積分項を追加することで誤差を吸収する方法が考えられる。よって、フィートバックパラメータ$K_{i}$を用いて再定義する。

$$\tau^{\prime\prime} = \ddot{x_{d}} + K_{d}(\dot{x_{d}} - \dot{x}) + K_{p}(x_{d} - x) +K_{i}\int(x_{d} - x)dt$$

僕はこれでいけたと思っていますが、本当はあっているかわからないので、もし間違っているところに気付いたら教えてください。お願いします。

この計算方法は、目標トルクを計算するため、計算トルク法と呼ばれる。また、加速度を用いて計算するため分解加速度制御とも呼ばれる。 ちなみに、運動方程式を $G(\theta) = \tau$ の形にすると重力の分しか出力をしていないので重力のみを補償する形になり、静止することができる。

モータを制御する

これで、アームの目標トルクは計算することができた。 このトルクを制御する方法として、以下の２つが考えられえる

• 電流制御によるトルク制御を行い、モーターを制御する
• PWMによって制御するために、モーターをモデル化し、トルクをPWM値へと変換する

電流制御器は簡単に設計できるため、目標トルクを電流に変換して、適当にPIDとかで電流制御やってればいける モーターのモデル化は次に示す

モーターのモデル化

　まず、モーターのデータシートより、モーターのトルク定数、端子間抵抗、誘導起電力定数を読みるか、実測する。そして、計算トルク法により計算した目標トルクをトルク定数で割り、目標の電流を計算する。これと端子間抵抗をかけたものを目標電圧１とする。また、誘導起電力定数とモーターの現在の角速度をかけたものを目標電圧２とする。目標電圧1と目標電圧2を足して、電源の電圧で割ると、目標のPWM値が計算できる。これをモータードライバの出力にすることで、ロボットアームを制御することができる。ちなみに、トルク定数を$K_{\tau}$、端子間抵抗を$R$、誘導起電力定数を$K_{E}$、電源電圧を$V_{0}$、角速度を$\omega$、与えるトルクを$\tau^{ref}$とすると、

$$\Large PWM = \frac{R\frac{\tau^{ref}}{K_{\tau}} + K_{E}\omega}{V_{0}}$$

こんな感じになる。

　この制御方法はロボットアームとモーターの両方のモデル化が必要になるため、モデルをきちんと計算することが重要になってくる。しかし、(4)の式はPIDになっているため、このパラメータしだいで意外と綺麗に動かすことができるため、おすすめの制御方法である。

軌道生成

　軌道生成には三次以上のスプライン曲線を用いることで加速度まで連続性が保たれた、滑らかな軌道をつくる。加速度でも滑らかな曲線をつくるために五次スプライン曲線を使うことも考えられるが、今回は三次スプライン曲線を用いた。 　そもそも、スプライン曲線とはある多項式によって描かれる、複数の点を通るなめらかな曲線のことで、$i$を点の順番とすると、三次のスプライン曲線の公式は、

$$S_{i}(x) = a_{i} + b_{i}(x - x_{i}) + c_{i}(x - x_{i})^2 + d_{i}(x - x_{i})^3$$

この$a,b,c,d$を求めるだけで軌道が生成できるため、三次スプライン曲線はよく使われる。$a,b,c,d$は、三次スプライン曲線の条件式5つを連立させて解く。しかし、この公式の$x$はロボコンではおそらく時間を示す$t$を用いて使う(自分はそうした)ため、これからは$t$で置き換えることにし、$x$は座標の$x,y,z$とかの$x$として扱うことにすると、公式はこう置き換えることができる…と思う。

$$S_{i}(t) = a_{i} + b_{i}(t - t_{i}) + c_{i}(t - t_{i})^2 + d_{i}(t - t_{i})^3$$

また、点の数を$n$とすると、条件は以下になる。

条件1

$$S_{i}(t_{i}) = x_{i}$$

条件2

$$S_{i}(t_{i+1}) = S_{i+1}(t_{i}) = x_{i+1}$$

条件3

$$S^{\prime}_{i}(t_{i+1}) = S^{\prime}_{i+1}(t_{i+1})$$

条件4

$$S^{\prime\prime}_{i}(t_{i+1}) = S^{\prime\prime}_{i+1}(t_{i+1})$$

条件5

$$S^{\prime\prime}_{0}(0) = S^{\prime\prime}_{n-1}(x_{n}) = 0$$

$a,b,c,d$を点と点の間の数だけ解くことでスプライン曲線を生成することができる。また、上の公式を使うと目標の座標(位置)が求められるが、あとは、公式を時間$t$で微分すれば速度の公式になって、もう一回微分すれば加速度の式になるだけだが、一応実装用に書いておく。$t$は点と点の間を一秒としたときの時間である。

$$x(t) = a + b*t + c*t^2 + d*t^3 \\ x^{\prime}(t) = b + 2c*t + 3d*t^2 \\ x^{\prime\prime}(t) = 2c + 6d*t$$

$(y, zも同様)$

ちなみに、ロボコンでバスタオルと取った時の軌道は こんな感じでした

注釈

Footnotes

1. 三自由度までしか逆運動学が求められず、解析的に求めることができないため、ニュートン・ラフソン法などで近似するしかない ↩