リンク平面シリアルリンクマニピュレータの数式モ...
TRANSCRIPT
Kiyotaka Izumi 2008/06/13 #1/7
2リンク平面シリアルリンクマニピュレータの数式モデル
1. 順運動学手先位置を (x, y) [m]とし，各リンクの関節角を θ1 および θ2 [rad]とする．マニピュレータの幾何学パラメー
タである各リンク長を `1 および `2 [m]とすると
x = `1 cos θ1 + `2 cos (θ1 + θ2) (1)
y = `1 sin θ1 + `2 sin (θ1 + θ2) (2)
という順運動学を得る．時間微分すると
vx =dx
dt= −`1θ1 sin θ1 − `2
(θ1 + θ2
)sin (θ1 + θ2)
vy =dy
dt= `1θ1 cos θ1 + `2
(θ1 + θ2
)cos (θ1 + θ2)
となる．行列・ベクトル表現すると[vx
vy
]= J
[θ1
θ2
]J =
[−`1 sin θ1 − `2 sin (θ1 + θ2) −`2 sin (θ1 + θ2)`1 cos θ1 + `2 cos (θ1 + θ2) `2 cos (θ1 + θ2)
]となる．ここで，行列 J をヤコビ行列という．
2. 逆運動学手先の位置ベクトルが与えられた場合にそれを実現する関節角を求める問題を逆運動学問題という．ここで，逆
運動学の計算に用いる関数 atan2(·, ·)がある．この関数は，2つのスカラ変数 a，bに対して
atan2(a, b) = arg(b + ja)
θ1
θ2
X
Y
Fig. 1 2-link manipulator
Kiyotaka Izumi 2008/06/13 #2/7
Re
Im
0
a
b
arg(b + ja)
Fig. 2 Argument
X
Y
0
a
b
φ
c
θ
Fig. 3 Geometric configuration
で定義されるスカラ関数である．ここで，j は虚数単位，argは複素数の偏角を表す．この関数を用いて
a sin θ + b cos θ = c
の解 θ を求める．ただし，a，bおよび cは既知のスカラ定数で a2 + b2 ≥ c2 を満足するものとする．この問題は幾何学的に Fig. 3に示すように，(b, a)の点からの垂線の足と原点間の距離が cであるような直線の x軸からの角度 θ を求めよというものである．まず
φ = atan2(a, b)
とおくとc =
√a2 + b2 cos(θ − φ)
であるから √a2 + b2 sin(θ − φ) = ±
√a2 + b2 − c2
となる．ここで，atan2(·, ·)の性質
θ = atan2(k sin θ, k cos θ) k > 0
atan2(±a, b) = ±atan2(a, b)
を用いるとθ − φ = ±atan2
(√a2 + b2 − c2, c
)を得る．したがって，解は
θ = φ ± atan2(√
a2 + b2 − c2, c)
= atan2(a, b) ± atan2(√
a2 + b2 − c2, c)
となる．逆運動学を解く．`22 = `22 cos2 (θ1 + θ2) + `22 sin2 (θ1 + θ2) が成立するので，この式に式 (1)と式 (2)を代入す
ると
`22 = (x − `1 cos θ1)2 + (y − `1 sin θ1)
2
y sin θ1 + x cos θ1 =x2 + y2 + `21 − `22
2`1
Kiyotaka Izumi 2008/06/13 #3/7
θ1
θ2
X
Y
φ
Right-hand
θ1
θ2
Left-hand
Fig. 4 The posture of a 2-link manipulator
となる．よって
θ1 = atan2 (y, x) ± atan2(√
x2 + y2 − d21, d1
)(3)
d1 =x2 + y2 + `21 − `22
2`1
を得る．ここで，θ1 に関して 2つの解が求められており，これは Fig. 4に示すように逆運動学の解として 2つの姿勢が存在することを意味しており，atan2(·, ·)の符号が正のとき左手形となり，負のとき右手形となる．同様にして，`21 = `21 cos2 θ1 + `21 sin2 θ1 の関係より
`21 = {x − `2 cos (θ1 + θ2)}2 + {y − `2 sin (θ1 + θ2)}2
y sin (θ1 + θ2) + x cos (θ1 + θ2) =x2 + y2 − `21 + `22
2`2
となる．よって
θ1 + θ2 = atan2 (y, x) ± atan2(√
x2 + y2 − d22, d2
)d2 =
x2 + y2 − `21 + `222`2
となる．ただし，atan2(·, ·) の符号が正のとき右手形となり，負のとき左手形となるので，式 (3) の符号に合わせて
θ2 = ∓atan2(√
x2 + y2 − d21, d1
)∓ atan2
(√x2 + y2 − d2
2, d2
)となる．例えば，Fig. 1に示す右手形での逆運動学は
θ1 = atan2 (y, x) − atan2(√
x2 + y2 − d21, d1
)θ2 = atan2
(√x2 + y2 − d2
1, d1
)+ atan2
(√x2 + y2 − d2
2, d2
)である．
Kiyotaka Izumi 2008/06/13 #4/7
3. 動力学3.1 ラグランジュの運動方程式系内の運動エネルギーの総和をK，ポテンシャルエネルギーの総和を U とするとき，ラグランジュ関数 Lを
L = K − U
とする．ラグランジュの運動方程式は，Lを用いて
d
dt
(∂L
∂qi
)− ∂L
∂qi= ui (i = 1, 2, . . . , n)
である．ここで，nは運動の自由度であり，qi はこの自由度に対する変位や回転角を意味する一般化座標であり，qi は qi の時間微分値を意味する一般化速度であり，ui は qi 方向に働くポテンシャル力以外の外力や制御入力を意味する一般化力である．3.2 マニピュレータの運動方程式
Fig. 1に示すマニピュレータは，質点として各リンクの重心位置を考え，その個数が 2つであるので，自由度 n
は 2である．運動エネルギーの総和K は
K = K1 + K2
K1 =12m1x
2g1 +
12m1y
2g1 +
12I1θ
21
K2 =12m2x
2g2 +
12m2y
2g2 +
12I2
(θ1 + θ2
)2
である．ここで，mi は第 iリンクの質量 [kg]であり，Ii は第 iリンクの重心中心の慣性モーメント [kgm2]である．(xgi, ygi)は，第 i リンクの重心位置 [m]であり，第 i関節から (xgi, ygi)までの距離 [m]を `gi とすると
xg1 = `g1 cos θ1
yg1 = `g1 sin θ1
xg2 = `1 cos θ1 + `g2 cos (θ1 + θ2)
yg2 = `1 sin θ1 + `g2 sin (θ1 + θ2)
である．θ1 と θ2 は時間変化するので，これらを時間微分すれば
xg1 = −`g1θ1 sin θ1
yg1 = `g1θ1 cos θ1
xg2 = −`1θ1 sin θ1 − `g2
(θ1 + θ2
)sin (θ1 + θ2)
yg2 = `1θ1 cos θ1 + `g2
(θ1 + θ2
)cos (θ1 + θ2)
Kiyotaka Izumi 2008/06/13 #5/7
となる．よって，各リンクの運動エネルギーKi は
K1 =12m1
(−`g1θ1 sin θ1
)2
+12m1
(`g1θ1 cos θ1
)2
+12I1θ
21
=12
(m1`
2g1 + I1
)θ21
K2 =12m2
{−`1θ1 sin θ1 − `g2
(θ1 + θ2
)sin (θ1 + θ2)
}2
+12m2
{`1θ1 cos θ1 + `g2
(θ1 + θ2
)cos (θ1 + θ2)
}2
+12I2
(θ1 + θ2
)2
=12
{m2`
21θ
21 + 2m2`1`g2θ1
(θ1 + θ2
)cos θ2 +
(m2`
2g2 + I2
) (θ1 + θ2
)2}
となる．ポテンシャルエネルギーの総和 U は，重力加速度を g [m/s2]とすると重心位置の高さから
U = U1 + U2
U1 = m1g`g1 sin θ1
U2 = m2g {`1 sin θ1 + `g2 sin (θ1 + θ2)}
となる．算出した K および U は，θ1 および θ2 とそれらの時間微分値 θ1 および θ2 の関数となることが分かる．したがって，このマニピュレータの一般化座標ベクトル q を
q =[q1
q2
]=
[θ1
θ2
]と選択すればよいことが分かる．アクチュエータにより各関節に駆動トルク τi [Nm]を印加できると考えて一般化力ベクトル uは
u =[u1
u2
]=
[τ1
τ2
]とする．まず，第 1リンクについての運動方程式を求める．Ki を θ1 で偏微分すると
∂K1
∂θ1
=(m1`
2g1 + I1
)θ1
∂K2
∂θ1
= m2`21θ1 + m2`1`g2
(θ1 + θ2
)cos θ2 + m2`1`g2θ1 cos θ2 +
(m2`
2g2 + I2
) (θ1 + θ2
)= m2`
21θ1 + m2`1`g2
(2θ1 + θ2
)cos θ2 +
(m2`
2g2 + I2
) (θ1 + θ2
)を得る．これらを時間微分すると
d
dt
(∂K1
∂θ1
)=
(m1`
2g1 + I1
)θ1
d
dt
(∂K2
∂θ1
)= m2`
21θ1 + m2`1`g2
(2θ1 + θ2
)cos θ2 − m2`1`g2
(2θ1 + θ2
)θ2 sin θ2 +
(m2`
2g2 + I2
) (θ1 + θ2
)=
(m2`
21 + m2`
2g2 + I2 + 2m2`1`g2 cos θ2
)θ1 +
(m2`
2g2 + I2 + m2`1`g2 cos θ2
)θ2
− m2`1`g2
(2θ1 + θ2
)θ2 sin θ2
を得る．U は，陽に θ1 を含まないので∂U
∂θ1
= 0
d
dt
(∂U
∂θ1
)= 0
Kiyotaka Izumi 2008/06/13 #6/7
である．したがって
d
dt
(∂L
∂θ1
)=
d
dt
(∂K
∂θ1
)− d
dt
(∂U
∂θ1
)=
d
dt
(∂K1
∂θ1
)+
d
dt
(∂K2
∂θ1
)=
(m1`
2g1 + m2`
21 + m2`
2g2 + I1 + I2 + 2m2`1`g2 cos θ2
)θ1 +
(m2`
2g2 + I2 + m2`1`g2 cos θ2
)θ2
− m2`1`g2
(2θ1 + θ2
)θ2 sin θ2
を得る．Ki は，陽に θ1 を含まないので∂K
∂θ1= 0
である．Ui を θ1 で偏微分すると∂U1
∂θ1= m1g`g1 cos θ1
∂U2
∂θ1= m2g {`1 cos θ1 + `g2 cos (θ1 + θ2)}
を得る．したがって∂L
∂θ1=
∂K
∂θ1− ∂U
∂θ1
= −∂U1
∂θ1− ∂U2
∂θ1
= − (m1g`g1 + m2g`1) cos θ1 − m2g`g2 cos (θ1 + θ2)
を得る．ゆえに，第 1リンクの運動方程式は(m1`
2g1 + m2`
21 + m2`
2g2 + I1 + I2 + 2m2`1`g2 cos θ2
)θ1 +
(m2`
2g2 + I2 + m2`1`g2 cos θ2
)θ2
−m2`1`g2
(2θ1 + θ2
)θ2 sin θ2 + (m1g`g1 + m2g`1) cos θ1 + m2g`g2 cos (θ1 + θ2) = τ1 (4)
となる．第 2リンクに対しても同様にして，運動方程式は(m2`
2g2 + I2 + m2`1`g2 cos θ2
)θ1 +
(m2`
2g2 + I2
)θ2 + m2`1`g2θ
21 sin θ2 + m2g`g2 cos (θ1 + θ2) = τ2 (5)
となる．ロボットの一般式
M (θ) θ + h(θ, θ
)+ g (θ) = τ
のように，式 (4)と式 (5)を整理すると
M (θ) =[m1`
2g1 + m2`
21 + m2`
2g2 + I1 + I2 + 2m2`1`g2 cos θ2 m2`
2g2 + I2 + m2`1`g2 cos θ2
m2`2g2 + I2 + m2`1`g2 cos θ2 m2`
2g2 + I2
]h
(θ, θ
)=
[−m2`1`g2
(2θ1 + θ2
)θ2 sin θ2
m2`1`g2θ21 sin θ2
]
g (θ) =[(m1g`g1 + m2g`1) cos θ1 + m2g`g2 cos (θ1 + θ2)
m2g`g2 cos (θ1 + θ2)
]となる．ただし，θ = [θ1 θ2]T であり，その時間微分値を θ とし，さらに時間微分した値を θ とし，τ = [τ1 τ2]T
である．
Kiyotaka Izumi 2008/06/13 #7/7
参考文献[1] 吉川恒夫，“ロボット制御基礎論”，コロナ社 (1988)
[2] 川崎晴久，“ロボット工学の基礎”，森北出版 (1991)
[3] 古田勝久，川路茂保，美多勉，原辰次，“メカニカルシステム制御”，オーム社 (1984)