2リンク平面シリアルリンクマニピュレータの数式モデル

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

Download Report