マニピュレータの軌道追従制御
M2013SC006 今村智貴
指導教員：大石泰章

1

はじめにmili
Giri
θiIi
E

リンク i の質量 [kg]
リンク i の長さ [m]リンク i の重心の座標 (xi , yi )第 i 関節から Gi までの長さ [m]リンク i の関節角 [rad]リンク i の慣性モーメント [kg·m2 ]エンドエフェクタの座標 (xe , ye )現在マニピュレータの制御においては様々な手法が提案されており，計算トルク法はその中でも基盤となる手法である．計算トルク法は運動方程式を使って，与えられた目標軌道を実現するのに必要な関節駆動トルクを計算し，それを制御する方法であり，角度と角速度の偏差にかかるゲインを正に選ぶだけで追従誤差を零に収束させることができる [1]．
表 1 ロボットアームのパラメータ (i = 1, 2)
しかし, この手法は「すべての物理パラメータが既知」という仮定を必要とする．なぜなら，計算トルク法ではこのマニピュレータをラグランジュの運動方程式を用マニピュレータの運動方程式の中の非線形な項を打ち消す設計をおこなっているため，制御対象の物理パラメー いて定式化する．
運動エネルギーの総和 K はタの推定値と真値に誤差がある場合，打ち消すことができなくなるためである．
K = K1 + K2 ,そのため本研究ではパラメータ誤差が存在していても111
計算トルク法によるマニピュレータの軌道追従制御を可 K1 = m1 (−r1 θ̇1 sin θ1 )2 + m1 (−r1 θ̇1 cos θ1 )2 + I1 θ˙12222能とするための手法を提案する．具体的には，パラメー
12˙
2= (m1 r1 + I1 )θ1 ,
タ誤差を外乱として考え，それを抑制するような追加入
2∞
力を H 制御を使って設計する．
12
シミュレーションでは提案する方法を用いて，2 リンク K2 = 2 m2 (−l1 θ̇1 sin θ1 − r2 (θ̇1 + θ̇2 ) sin(θ1 + θ2 ))1平面マニピュレータの制御を行う．特に 1 つのリンクの+ m2 (l1 θ̇1 cos θ1 + r2 (θ̇1 + θ̇2 ) cos(θ1 + θ2 ))22
質量が推定した値よりも大きい場合にも目標軌道にマニ1ピュレータが追従することを示す．
+ I2 (θ̇1 + θ̇2 )2
2
12 2 リンク平面マニピュレータのモデル= (m2 l12 θ̇1 + 2m2 l1 r2 θ̇1 (θ̇1 + θ̇2 ) cos θ22
本研究では 2 リンク平面マニピュレータの制御を行う．
+(m2 r22 + I2 )(θ̇1 + θ̇2 ))考える 2 リンク平面マニピュレータは図 1 のようなものであり，各パラメータは表 1 の通りである．ただしリ となる．またポテンシャルエネルギーの総和 U はンク 1 の一端は原点で固定されており，x 軸正方向からの角度を θ1 とする．リンク 1 のもう一端はリンク 2 に接U = U1 + U2 ,続されており，その間の角度を θ2 とする．角度 θ1 と θ2
U1 = m1 gr1 sin θ1 ,はトルク τ1 ，τ2 を加えることで変化させることができる．
U2 = m2 g(l1 sin θ1 + r2 sin(θ1 + θ2 ))となる．これらを用いて, ラグランジュの運動方程式を求め, 以下の式のように整理する:

M (θ)θ̈ + V (θ, θ̇) + G(θ) = τ.
ここで M (θ) は n × n の慣性行列，V (θ, θ̇) は n × 1 の遠心力やコリオリ力を示す項，G(θ) は n × 1 の重力を示す項である．ただし，入力 τ と関節変数 θ を[][]
τ1
θ1τ=,θ =τ2
θ2

図 1 2 リンク平面マニピュレータとする．今回の研究では平面マニピュレータを対象としているので, G(θ) の重力項は無視でき，M (θ)θ̈ + V (θ, θ̇) = τ(1)

としてよい．ここで各行列は
[][]
M1 M2
V1M=,V =M3 M4V2となる．よって， 式 (4)–式 (8) より目標軌道θd (t) =θs +6(θe − θs )t2t2e8
3− 3 (θe − θs )t3 + 4 (θe − θs )t4
teteとするとき，

M1 = m1 r12 + m2 l12 + m2 r2 + I1 + I2 + 2m2 l1 r2 cos θ2 ,を得る [2, pp. 132–135]．

M2 = m2 r22 + I2 + m2 l1 r2 cos θ2 ,M3 = m2 r22 + I2 + m2 l1 r2 cos θ2 ,4

計算トルク法2 リンク平面マニピュレータの運動方程式 (1)M4 = m2 r22 + I2であり，M (θ)θ̈ + V (θ, θ̇) = τ(10)τ = M (θ)τ ′ + V (θ, θ̇)

(11)

に対して，

2V1 = −m2 l1 r2 (2θ˙1 θ˙2 + θ˙2 ) sin θ2 ,2V2 = −m2 l1 r2 θ˙1 sin θ2という非線形状態フィードバック補償を考える．このとき

である．

3(9)

θ̈ = τ ′軌道生成

この章では初期位置から目標位置まで設定した時間内に到達させるための軌道を生成する．
初期角度を θs ，目標角度を θe ，時刻 t のときの角度をθd (t) と書く．また初期時刻を 0 とし，目標角度に到達する最終時刻を te とする．このとき，

θd (0) = θs ,θd (te ) = θe(12)という関節変数 θ に関する線形かつ非干渉な系を得る．もしモデル化誤差や外乱が存在しない場合，目標軌道 θd の角加速度 θ̈d を τ ′ として与えれば，θ = θd となり，目標軌道への追従が達成される．
しかし，モデル化誤差や外乱は避けることができない
ため，その影響を式 (12) に対するサーボ補償器を加えることで抑える．これをブロック線図で表すと図 2 のようになる．すなわち，である．
また，初期時刻と最終時刻でマニピュレータが静止するとしθ̇d (0) = 0,θ̈d (0) = 0,θ̇d (te ) = 0,θ̈d (te ) = 0と考える．
これら 6 つの拘束条件を満たすには

θd (t) = a0 + a1 t + a2 t2 + a3 t3 + a4 t4 .

図 2 計算トルク法(2)とすればよい．
この関数を t で微分すると

θ̇d (t) = a1 + 2a2 t + 3a3 t2 + 4a4 t3 .

τ ′ = θ̈d + kv (θ̇d − θ̇) + kp (θd − θ)(3)となる．このとき式 (2)，式 (3) を用いて係数を定めると

a0 = θs ,(4)a1 = 0,6
a2 = 2 (θe − θs ),te8a3 = − 3 (θe − θs ),te3a4 = 4 (θe − θs )te(5)(6)というサーボ補償器を設け，目標角度と現在の角度の追従誤差 θd − θ を e と置く．式 (12)，式 (13) からë + kv ė + kp e = 0(8)(14)となる．このときこのとき，kv と kp を正にとることで誤差 e を零に収束させることができる [3, pp. 172–174].

5(7)

(13)

H ∞ 制御を用いたモデル化誤差の抑制

前章にて計算トルク法を用いた 2 リンクマニピュレータの制御を紹介した. しかし, 入力 τ を計算するには M (θ)，
V (θ, θ̇) を算出しなくてはならない．M (θ) や V (θ, θ̇) の中

には物理パラメータが含まれているので，「すべての物理パラメータが既知」であることが必要である． 物理パラメータが正確にわからない場合に対応するための手法を考える．すなわちパラメータ誤差の影響を外乱と見なし,この外乱を抑制するような追加の入力を H ∞ 制御を用いて作成する方法である．
2 リンクマニピュレータの運動方程式 (1)M (θ)θ̈ + V (θ, θ̇) = τが得られるすると以下のような一般化制御対象を求めることができる [4, pp. 103–108]：ẋ(t) = A + B1 d(t) + B2 u(t),e(t) = C1 x(t),x(t) = C2 x(t).
ただし[OA=−kp Iにおいて, 入力 τ を以下のようにする：

τ = M̂ (θ)(θ̈d + kv ė + kp e) + V̂ (θ, θ̇) + u(t).

(15)

ただし M̂ (θ), V̂ (θ, θ̇) は慣性行列, 遠心力・コリオリ力の項において必ずしも正しくないパラメータ値を使った場合の値である．また，u(t) はパラメータ誤差を抑制するための入力である．式 (15) において

θ̈d = θ̈ + (θ̈d − θ̈)][][]OOI, B1 =, B2 =−kv IM̂ −1 (θ)−M̂ −1 (θ)[C1 = Iẋ(t) = (A + B2 K)x(t) + B1 d(t),+V̂ (θ, θ̇) + u(t)e(t) = C1 x(t).

= M̂ (θ)θ̈ + M̂ (θ)(ë + kv ė + kp e)+V̂ (θ, θ̇) + u(t).

与えられた γ >0 に対して, 閉ループ系を安定化し, かつH ∞ ノルムを γ より小さくする状態フィードバックゲイン K を設計する. 参考文献 [5, pp. 152–153] より以下のような LMI 条件を得る：

右辺の M̂ (θ)θ̈, V̂ (θ, θ̇)，u(t) を左辺に移項すると，M̂ (θ)(ë + kv ė + kp e) = (M (θ) − M̂ (θ))θ̈+(V (θ, θ̇) − V̂ (θ, θ̇))) − u(t)[(θ) を両辺にかけると，ë + kv ė + kp e = M̂(θ)[(M (θ) − M̂ (θ))θ̈[となる．このとき, 右辺 [・] の中の信号を d(t) と置くと以下のようになる.

I−kv I][ ] [Oe+ėM̂ −1 (θ)

O−M̂ −1 (θ)][d(t)u(t)−(A + B2 K)X − X(A + B2 K)T − B1 B1T−C1 X

−XC1T
γ2I]

このとき, 追従誤差 e を評価出力とし, 観測出力として[]e(t)
x(t) =ė(t)−(AX + B2 Y ) − (XAT + Y T B2T ) − B1 B1T−C1 X

−XC1T
γ2I]
≻ 0.
]
≻0となる. この LMI を満足する X ，Y を求めることで状態フィードバックゲイン K を(17)この d(t) を外乱と見なし, 抑制するための入力 u を H ∞制御を用いて設計する.
式 (17) を書き換えると以下のようになる：[ ] [d eO=−kp I
dt ė

X ≻ 0,Y = KX とすると+(V (θ, θ̇) − V̂ (θ, θ̇))] − M̂ −1 (θ)u(t)ë + kv ė + kp e = M̂ −1 (θ)d(t) − M̂ −1 (θ)u(t).

OIcos θ2 ∼=1= M̂ (θ)(θ̈ + (θ̈d − θ̈) + kv ė + kp e)−1I
O , C2 =Oと仮定する．
この一般化制御対象に入力 u(t) = Kx(t) を施すとき，閉ループ系の状態方程式は以下のようになる：M (θ)θ̈ + V (θ, θ̇) = M̂ (θ)(θ̈d + kv ė + kp e) + V̂ (θ, θ̇) + u(t)M̂][とする．このとき B1 , B2 内の M̂ −1 (θ) に含まれる cos θ2を θ2 ∼= 0 と仮定し，(16)を代入し, 書き換えると以下のようになる：−1]

K = Y X −1(18)として求めることができる．

6

シミュレーション前章で提案した手法を用いて作成した追加入力 u(t) を使用してシミュレーションを行う. 2 リンク平面マニピュ
レータの各パラメータの推定値は表 2 のようにする．ただし，m2 と I2 の推定値は不正確であり，真のパラメー
タ値は m2 = 0.25, I2 = 0.0025 であるとする.
このとき，前章で求めた LMI を解くと追加入力 u のフィードバックゲイン K は[]0.0264 0.0072 0.0138 0.0010
K=0.0104 0.0049 0.0056 0.0014

m1m2
l1
l2
r1
r2
I2
I2

リンク 1 の質量
リンク 2 の質量
リンク 1 の質量
リンク 2 の質量
第 1 関節から G1 までの長さ
第 2 関節から G2 までの長さ
リンク 1 の慣性モーメント
リンク 2 の慣性モーメント0.2[kg]0.2[kg]0.1[m]
0.1[m]
0.05[m]0.05[m]0.001[kg·m2 ]0.001[kg·m2 ]表 2 シミュレーションに用いる各パラメータの値となる．また目標位置に到達する最終時刻を te = 3[s] とした場合の目標軌道の式は以下のようになり, 図 3 のような軌道をとる：

2θd (t) =θs + (θe − θs )t2
38
1− (θe − θs )t3 + (θe − θs )t4 .
2727

図 4 追加入力 u(t) を加えない場合のシミュレーション(19)

これらを用いて, 初期位置 (xs , ys ) = (0.05, 0.15) から目

図 5 追加入力 u(t) を加える場合のシミュレーション

図 3 目標軌道
標位置 (xe , ye ) = (0.18, 0.02) にエンドエフェクタを動かしたときの結果が図 4, 図 5 となる．各関節が円で表されているものが目標軌道であり，四角形で表されているものが実現される軌道である．図 4 は追加入力 u(t) を加えずにおこなった場合のシミュレーション, 図 5 は追加入力u(t) を加えておこなった場合のシミュレーションである.
図 4 では実現される軌道が目標軌道から逸れてしまっていることがわかる. しかし，図 5 では実現される軌道が目標軌道とほぼ重なっている．以上により，提案の方法によってパラメータ誤差の影響を抑え，目標軌道へと追従させることができたと言える.

7

おわりにH ∞ 制御によって設計した追加入力 u(t) によって，モデルのパラメータ誤差の影響を抑えて，計算トルク法を用いた 2 リンク平面マニピュレータの軌道追従制御が可能であることが示せた. 今後の課題としては本研究で提案した方法を用いて実機実験を行い，有効であることを確認することである．また今回は M̂ (θ) の中の cos θ2 を 1と近似して LMI を解いた．しかし，変化が大きくなる可能性がある cos θ2 を定数とするのは少々乱暴である．そこで二乗和多項式を用いることで変数のまま LMI を解き，フィードバックゲイン K を求めることができる [6]．今後はこの手法を用いた研究も進めていきたい．

参考文献[1] 吉川恒夫 :計算トルク法のディジタル制御，ロボット学会誌，Vol. 7, No. 3，1989．
[2] 吉川恒夫 :ロボット制御基礎論，コロナ社，1988．
[3] J. J. Craig 著，三浦宏文・下山勇訳 ：ロボティクス：機構・力学・制御，共立出版株式会社，1991．
[4] 野波健蔵 編著，西村秀和・平田光男 共著 ：MATLABによる制御系設計，東京電機大学出版局，1991．
[5] 蛯原義雄 :LMI によるシステム制御，森北出版，1991.
[6] 大石泰章 :2 乗和多項式とその非線形制御への応用 :システム/制御/応用, Vol. 58, No. 1, 2014.