ロボティクス基礎
担当：平田 健太郎
第
1
学期 木Ⅲ・Ⅳ限 11
：00-13
：10 1
号館 大講義室5/16
第5
回 運動学・動力学例題：
𝑥𝑥
0𝑦𝑦
0𝑧𝑧
0𝑎𝑎 = 3,0,0
𝑇𝑇𝑎𝑎′
𝑎𝑎′′
𝑎𝑎
を𝑧𝑧
0 軸まわりに𝜋𝜋/2
回転し, 𝑥𝑥
0 軸まわりに𝜋𝜋
回転したときの( 𝑥𝑥
0, 𝑦𝑦
0, 𝑧𝑧
0 座標系における)
座標 を求める.𝑥𝑥
0𝑦𝑦
0𝑧𝑧
0𝑎𝑎 = 3𝑥𝑥
0𝑥𝑥
1𝑦𝑦
1𝑧𝑧
1𝑥𝑥
2𝑦𝑦
2𝑧𝑧
2𝑎𝑎′ = 3𝑥𝑥
1𝑎𝑎′′ = 3𝑥𝑥
2𝑧𝑧
0 軸まわりに𝜋𝜋/2
回転𝑥𝑥
0 軸まわりに𝜋𝜋
回転𝑦𝑦
1 軸まわりに−𝜋𝜋
回転例題：
𝑥𝑥
0𝑦𝑦
0𝑧𝑧
0𝑎𝑎 = 3,0,0
𝑇𝑇𝑎𝑎′
𝑎𝑎′′
𝑎𝑎
を𝑧𝑧
0 軸まわりに𝜋𝜋/2
回転し, 𝑥𝑥
0 軸まわりに𝜋𝜋
回転したときの( 𝑥𝑥
0, 𝑦𝑦
0, 𝑧𝑧
0 座標系における)
座標 を求める.それぞれの回転後の座標系を
𝑥𝑥
1, 𝑦𝑦
1, 𝑧𝑧
1, 𝑥𝑥
2, 𝑦𝑦
2, 𝑧𝑧
2 とすると𝑥𝑥
1, 𝑦𝑦
1, 𝑧𝑧
1= 𝑥𝑥
0, 𝑦𝑦
0, 𝑧𝑧
0𝑅𝑅 𝑥𝑥
0, 𝜋𝜋
2
𝑥𝑥
2, 𝑦𝑦
2, 𝑧𝑧
2= 𝑥𝑥
1, 𝑦𝑦
1, 𝑧𝑧
1𝑅𝑅 𝑦𝑦
1, −𝜋𝜋
𝑥𝑥
0𝑦𝑦
0𝑧𝑧
0𝑎𝑎 = 3,0,0
𝑇𝑇𝑎𝑎′
𝑎𝑎′′
ベクトル
𝑎𝑎
は座標系ごと回転しているので𝑥𝑥
2, 𝑦𝑦
2, 𝑧𝑧
2 座標系でも3,0,0
𝑇𝑇 と表される.𝑥𝑥1,𝑦𝑦1,𝑧𝑧1 = 𝑥𝑥0,𝑦𝑦0,𝑧𝑧0 𝑅𝑅 𝑧𝑧0,𝜋𝜋 2 𝑥𝑥2,𝑦𝑦2,𝑧𝑧2 = 𝑥𝑥1,𝑦𝑦1,𝑧𝑧1 𝑅𝑅 𝑦𝑦1,−𝜋𝜋 𝑥𝑥0,𝑦𝑦0,𝑧𝑧0 𝑎𝑎′′ = 𝑥𝑥2,𝑦𝑦2,𝑧𝑧2 3
00 𝑎𝑎′′ = 𝑥𝑥0,𝑦𝑦0,𝑧𝑧0 −1 𝑥𝑥2,𝑦𝑦2,𝑧𝑧2 3
00 = 𝑥𝑥0,𝑦𝑦0,𝑧𝑧0 −1 𝑥𝑥1,𝑦𝑦1,𝑧𝑧1 𝑅𝑅 𝑦𝑦1,−𝜋𝜋 3 00
= 𝑥𝑥0,𝑦𝑦0,𝑧𝑧0 −1 𝑥𝑥0,𝑦𝑦0,𝑧𝑧0 𝑅𝑅 𝑥𝑥0,𝜋𝜋
2 𝑅𝑅 𝑦𝑦1,−𝜋𝜋 3 00
=
cos𝜋𝜋
2 −sin𝜋𝜋 2 0 sin𝜋𝜋
2 cos𝜋𝜋 0 02 01
cos(−𝜋𝜋) 0 sin(−𝜋𝜋)
0 1 0
−sin(−𝜋𝜋) 0 cos(−𝜋𝜋) 30
0 = 0 −1 0
1 0 0
0 0 1
−1 0 0
0 1 0
0 0 −1 30 0
= 0 −1 0
−1 0 0
0 0 −1
30
0 = 0
−30
座標系の逐次変換回転行列を
𝐴𝐴 𝜃𝜃
𝑖𝑖 で表すと𝑥𝑥
𝑖𝑖, 𝑦𝑦
𝑖𝑖, 𝑧𝑧
𝑖𝑖= 𝑥𝑥
𝑖𝑖−1, 𝑦𝑦
𝑖𝑖−1, 𝑧𝑧
𝑖𝑖−1𝐴𝐴 𝜃𝜃
𝑖𝑖= 𝑥𝑥
𝑖𝑖−2, 𝑦𝑦
𝑖𝑖−2, 𝑧𝑧
𝑖𝑖−2𝐴𝐴 𝜃𝜃
𝑖𝑖−1𝐴𝐴 𝜃𝜃
𝑖𝑖= 𝑥𝑥
0, 𝑦𝑦
0, 𝑧𝑧
0𝐴𝐴 𝜃𝜃
1𝐴𝐴 𝜃𝜃
2⋯ 𝐴𝐴 𝜃𝜃
𝑖𝑖−1𝐴𝐴 𝜃𝜃
𝑖𝑖⋯ (𝑎𝑎. 2)
𝑥𝑥
0, 𝑦𝑦
0, 𝑧𝑧
0= 𝐼𝐼
R-P-P
ロボットの例𝑥𝑥0
Rotation-Pivot-Pivot
型の3
自由度 ロボットアーム𝑧𝑧0
𝑦𝑦0
𝑥𝑥1
𝑧𝑧1 𝑦𝑦1
𝜃𝜃1
𝑥𝑥2 𝑧𝑧2
𝑦𝑦2 𝑥𝑥3
𝑧𝑧3 𝑦𝑦3
𝜃𝜃2
𝜃𝜃3
𝜃𝜃1 = 𝜃𝜃2 = 𝜃𝜃3 = 0 のとき, 各リンク座標系の方向と 姿勢は基準座標系と一致 しているものとする.
例題：
𝑥𝑥
1, 𝑦𝑦
1, 𝑧𝑧
1, 𝑥𝑥
2, 𝑦𝑦
2, 𝑧𝑧
2, 𝑥𝑥
3, 𝑦𝑦
3, 𝑧𝑧
3 を求めよ.
𝑥𝑥1
𝑧𝑧1 𝑦𝑦1
𝜃𝜃1
𝑥𝑥2 𝑧𝑧2
𝑦𝑦2 𝑥𝑥3
𝑧𝑧3 𝑦𝑦3
𝜃𝜃2
𝜃𝜃3
回転軸
: 𝑞𝑞
1= 𝑧𝑧
0, 𝑞𝑞
2= 𝑦𝑦
1, 𝑞𝑞
3= 𝑦𝑦
2𝐴𝐴(𝜃𝜃1) = 𝑅𝑅(𝑧𝑧0,𝜃𝜃1) 𝐴𝐴(𝜃𝜃2) = 𝑅𝑅(𝑦𝑦1,𝜃𝜃2) 𝐴𝐴(𝜃𝜃3) = 𝑅𝑅(𝑦𝑦2,𝜃𝜃3)
𝑥𝑥
1, 𝑦𝑦
1, 𝑧𝑧
1= 𝑥𝑥
0, 𝑦𝑦
0, 𝑧𝑧
0𝐴𝐴 𝜃𝜃
1= 𝐴𝐴 𝜃𝜃
1= cos 𝜃𝜃
1−sin 𝜃𝜃
10
sin 𝜃𝜃
1cos 𝜃𝜃
10
0 0 1 ⋯ (𝑎𝑎. 3)
𝑥𝑥
2, 𝑦𝑦
2, 𝑧𝑧
2= 𝐴𝐴 𝜃𝜃
1𝐴𝐴 𝜃𝜃
2= cos 𝜃𝜃
1−sin 𝜃𝜃
10 sin 𝜃𝜃
1cos 𝜃𝜃
10
0 0 1
cos 𝜃𝜃
20 sin 𝜃𝜃
20 1 0
−sin 𝜃𝜃
20 cos 𝜃𝜃
2= c 𝜃𝜃
1c 𝜃𝜃
2−s 𝜃𝜃
1c 𝜃𝜃
1s 𝜃𝜃
2s 𝜃𝜃
1c 𝜃𝜃
2c 𝜃𝜃
1s 𝜃𝜃
1s 𝜃𝜃
2−s 𝜃𝜃
20 c 𝜃𝜃
2⋯ (𝑎𝑎. 4)
cos sin → → c s
と略記𝑥𝑥1
𝑧𝑧1 𝑦𝑦1
𝜃𝜃1 𝑥𝑥2 𝑧𝑧2
𝑦𝑦2 𝑥𝑥3
𝑧𝑧3 𝑦𝑦3
𝜃𝜃2
𝜃𝜃3
𝑥𝑥
3, 𝑦𝑦
3, 𝑧𝑧
3= 𝐴𝐴 𝜃𝜃
1𝐴𝐴 𝜃𝜃
2𝐴𝐴 𝜃𝜃
3= c 𝜃𝜃
1c 𝜃𝜃
2+ 𝜃𝜃
3−s 𝜃𝜃
1c 𝜃𝜃
1s 𝜃𝜃
2+ 𝜃𝜃
3s 𝜃𝜃
1c 𝜃𝜃
2+ 𝜃𝜃
3c 𝜃𝜃
1s 𝜃𝜃
1s 𝜃𝜃
2+ 𝜃𝜃
3−s 𝜃𝜃
2+ 𝜃𝜃
30 c 𝜃𝜃
2+ 𝜃𝜃
3⋯ (𝑎𝑎. 5)
cos sin → → s c
と略記𝐴𝐴 𝜃𝜃
2𝐴𝐴 𝜃𝜃
3= 𝑅𝑅(𝑦𝑦
1, 𝜃𝜃
2+ 𝜃𝜃
3)
に注意例題： 第
𝑖𝑖
リンク先端位置を𝑝𝑝
𝑒𝑒𝑖𝑖 と表す. 𝑝𝑝
𝑒𝑒1= 𝑙𝑙
1𝑧𝑧
1, 𝑝𝑝
𝑒𝑒2= 𝑙𝑙
1𝑧𝑧
1+ 𝑙𝑙
2𝑧𝑧
2 より 手先位置𝑝𝑝
𝑒𝑒3= 𝑙𝑙
1𝑧𝑧
1+ 𝑙𝑙
2𝑧𝑧
2+ 𝑙𝑙
3𝑧𝑧
3 となる. 𝑝𝑝
𝑒𝑒3 の座標を求めよ.
運動学・動力学
座標変換による物理量の漸化式とラグランジュ法を用 いれば
, (3
次元空間内の)
ロボットの運動方程式が導出 できるので,
ロボットのモデリングに関しての議論は一 段落.
次にロボットの制御を考えるにあたって,
運動学・動力学の概念を整理しておく
.
運動学
(Kinematics)
•
ロボットの手先の位置・速度と各関節の角度・角速度の 間の幾何学的関係動力学
(Dynamics)
•
ロボットに加える力（トルク）とその結果生じる動きの間の 関係.
常微分方程式（運動方程式）によって記述される.
通常
,
（ロボティクス以外の）制御で考えるのはこちら𝑥𝑥1
𝑧𝑧1 𝑦𝑦1
𝜃𝜃1 𝑥𝑥2 𝑧𝑧2
𝑦𝑦2 𝑥𝑥3
𝑧𝑧3 𝑦𝑦3
𝜃𝜃2
𝜃𝜃3
𝑙𝑙
1= 𝑙𝑙
2= 𝑙𝑙
3= 2
とするとき,
先の例題（
R-P-P
マニピュレータ）のロボットの手先位置は次のように求められる.
𝑎𝑎1 𝑙𝑙1
𝑚𝑚1 𝐺𝐺1 𝑎𝑎2
𝑙𝑙2
𝑚𝑚2 𝐺𝐺2
𝑎𝑎3 𝑙𝑙3
𝑚𝑚3 𝐺𝐺3
𝑝𝑝
𝑒𝑒3= 𝑥𝑥
𝑒𝑒3𝑦𝑦
𝑒𝑒3𝑧𝑧
𝑒𝑒3= 2 c 𝜃𝜃
1s 𝜃𝜃
2+ 2c 𝜃𝜃
1s(𝜃𝜃
2+ 𝜃𝜃
3) 2 s 𝜃𝜃
1s 𝜃𝜃
2+ 2s 𝜃𝜃
1s(𝜃𝜃
2+ 𝜃𝜃
3)
2 + 2 c 𝜃𝜃
2+2c(𝜃𝜃
2+ 𝜃𝜃
3)
-
順運動学•
各関節の角度・角速度→
ロボットの動きの関係-
逆運動学(Inverse Kinematics)
•
手先位置・速度→
各関節の角度・角速度の関係𝑥𝑥
𝑒𝑒3𝑦𝑦
𝑒𝑒3𝑧𝑧
𝑒𝑒3= 2 c 𝜃𝜃
1s 𝜃𝜃
2+ 2c 𝜃𝜃
1s(𝜃𝜃
2+ 𝜃𝜃
3) 2 s 𝜃𝜃
1s 𝜃𝜃
2+ 2s 𝜃𝜃
1s(𝜃𝜃
2+ 𝜃𝜃
3)
2 + 2 c 𝜃𝜃
2+2c(𝜃𝜃
2+ 𝜃𝜃
3)
𝜃𝜃
1, 𝜃𝜃
2, 𝜃𝜃
3 を与えると𝑥𝑥
𝑒𝑒3, 𝑦𝑦
𝑒𝑒3, 𝑧𝑧
𝑒𝑒3 が定まる.
順運動学𝑥𝑥 ≔ 𝑥𝑥
𝑒𝑒3𝑦𝑦
𝑒𝑒3𝑧𝑧
𝑒𝑒3, 𝑞𝑞 ≔ 𝜃𝜃
1𝜃𝜃
2𝜃𝜃
3, 𝑥𝑥 = ̅𝑥𝑥 + Δ𝑥𝑥, 𝑞𝑞 = �𝑞𝑞 + Δ𝑞𝑞
とすると 逆運動学𝑥𝑥 ≔ 𝑥𝑥
𝑒𝑒3𝑦𝑦
𝑒𝑒3𝑧𝑧
𝑒𝑒3, 𝑞𝑞 ≔ 𝜃𝜃
1𝜃𝜃
2𝜃𝜃
3とおくと
, 𝑥𝑥 = 𝑓𝑓 𝑞𝑞 .
𝑞𝑞 = 𝑓𝑓
−1𝑥𝑥
を解析的に求めるのは困難.
各動作点近傍で 線形近似して求める.𝑦𝑦 = 𝑓𝑓(𝑥𝑥)
を𝑥𝑥 = 𝑎𝑎
のまわりで多項式によって近似することをいう.
関数のテーラー展開𝑦𝑦 = 𝑓𝑓 𝑎𝑎 + 1
1! 𝑓𝑓
′𝑎𝑎 𝑥𝑥 − 𝑎𝑎 + 1
2! 𝑓𝑓
′′𝑎𝑎 𝑥𝑥 − 𝑎𝑎
2+ ⋯
= �
𝑘𝑘=0
∞
𝑓𝑓
𝑘𝑘𝑎𝑎
𝑘𝑘! 𝑥𝑥 − 𝑎𝑎
𝑘𝑘𝑥𝑥
𝑒𝑒3𝑦𝑦
𝑒𝑒3𝑧𝑧
𝑒𝑒3= 2 c 𝜃𝜃
1s 𝜃𝜃
2+ 2c 𝜃𝜃
1s(𝜃𝜃
2+ 𝜃𝜃
3) 2 s 𝜃𝜃
1s 𝜃𝜃
2+ 2s 𝜃𝜃
1s(𝜃𝜃
2+ 𝜃𝜃
3)
2 + 2 c 𝜃𝜃
2+2c(𝜃𝜃
2+ 𝜃𝜃
3)
̅𝑥𝑥
𝑒𝑒3+ Δ𝑥𝑥
𝑒𝑒3= 2 c(
1̅𝜃𝜃 +Δ𝜃𝜃
1) s( ̅𝜃𝜃
2+Δ𝜃𝜃
2) + 2c( ̅𝜃𝜃
1+Δ𝜃𝜃
1) s( ̅𝜃𝜃
2+Δ𝜃𝜃
2+ ̅𝜃𝜃
3+Δ𝜃𝜃
3)
≃ 2 c ̅𝜃𝜃
1− s ̅𝜃𝜃
1Δ𝜃𝜃
1s ̅𝜃𝜃
2+ c ̅𝜃𝜃
2Δ𝜃𝜃
2+2 c
1̅𝜃𝜃 − s
1̅𝜃𝜃 Δ𝜃𝜃
1s ̅𝜃𝜃
2+ ̅𝜃𝜃
3+c
2̅𝜃𝜃 +
3̅𝜃𝜃 Δ𝜃𝜃
2+Δ𝜃𝜃
3≃ 2 c
1̅𝜃𝜃 s ̅𝜃𝜃
2− 2s
1̅𝜃𝜃 s ̅𝜃𝜃
2Δ𝜃𝜃
1+ 2 c
1̅𝜃𝜃 c
2̅𝜃𝜃 Δ𝜃𝜃
2+2 c ̅𝜃𝜃
1s ̅𝜃𝜃
2+ ̅𝜃𝜃
3+ 2s
1̅𝜃𝜃 s ̅𝜃𝜃
2+ ̅𝜃𝜃
3Δ𝜃𝜃
1+ 2 c
1̅𝜃𝜃 c ̅𝜃𝜃
2+ ̅𝜃𝜃
3Δ𝜃𝜃
2+Δ𝜃𝜃
3= ̅𝑥𝑥
𝑒𝑒3+ 2 s
1̅𝜃𝜃 s ̅𝜃𝜃
2+ ̅𝜃𝜃
3− s ̅𝜃𝜃
1s ̅𝜃𝜃
2Δ𝜃𝜃
1+2 c
1̅𝜃𝜃 c
2̅𝜃𝜃 + c
1̅𝜃𝜃 c ̅𝜃𝜃
2+ ̅𝜃𝜃
3Δ𝜃𝜃
2+ 2 c
1̅𝜃𝜃 c ̅𝜃𝜃
2+ ̅𝜃𝜃
3Δ𝜃𝜃
3=: ̅𝑥𝑥
𝑒𝑒3+ 𝑎𝑎
11Δ𝜃𝜃
1+ 𝑎𝑎
12Δ𝜃𝜃
2+ 𝑎𝑎
13Δ𝜃𝜃
3各三角関数の1次までの テーラー展開をとる
高次項は無視する
Δ𝜃𝜃𝑖𝑖 の係数を整理
同様にして
, Δ�𝑦𝑦
𝑒𝑒3= 𝑎𝑎
21Δ𝜃𝜃
1+ 𝑎𝑎
22Δ𝜃𝜃
2+ 𝑎𝑎
23Δ𝜃𝜃
3,
Δ ̅𝑧𝑧
𝑒𝑒3= 𝑎𝑎
31Δ𝜃𝜃
1+ 𝑎𝑎
32Δ𝜃𝜃
2+ 𝑎𝑎
33Δ𝜃𝜃
3 を求める.
̅𝐽𝐽 = 𝑎𝑎
11𝑎𝑎
12𝑎𝑎
13𝑎𝑎
21𝑎𝑎
22𝑎𝑎
23𝑎𝑎
31𝑎𝑎
32𝑎𝑎
33 とおくと, Δ𝑥𝑥 = ̅𝐽𝐽Δ𝑞𝑞 .
これよりΔ𝑞𝑞 =
−1̅𝐽𝐽 Δ𝑥𝑥
とすれば
,
現在の手先位置の微小変化に対応する関節角度の操作量が 分かる.速度の対応は
Δ𝑞𝑞/Δ𝑡𝑡 =
−1̅𝐽𝐽 Δ𝑥𝑥/Δ𝑡𝑡
𝜕𝜕𝑓𝑓
𝜕𝜕𝑞𝑞 =: 𝐽𝐽 𝑞𝑞
ヤコビアン𝐽𝐽 𝑞𝑞 = 𝜕𝜕𝑓𝑓
𝜕𝜕𝑞𝑞 = Δ𝑥𝑥 Δ𝑞𝑞 = ̅𝐽𝐽 𝑑𝑑𝑞𝑞
𝑑𝑑𝑡𝑡 = 𝐽𝐽
−1𝑞𝑞 𝑑𝑑𝑥𝑥 𝑑𝑑𝑡𝑡 𝑑𝑑𝑞𝑞
𝑑𝑑𝑡𝑡 =
−1̅𝐽𝐽 𝑑𝑑𝑥𝑥
現在の動作点近傍では𝑑𝑑𝑡𝑡
一般論
𝑥𝑥 = 𝑥𝑥
𝑒𝑒3𝑦𝑦
𝑒𝑒3𝑧𝑧
𝑒𝑒3, 𝑞𝑞 = 𝜃𝜃
1𝜃𝜃
2𝜃𝜃
3別の言い方をすれば, これはベクトル変数に対してベクトル値を返す 関数
𝑓𝑓
のテーラー展開𝑥𝑥 = 𝑓𝑓 𝑞𝑞 = 𝑓𝑓
1(𝑞𝑞) 𝑓𝑓
2(𝑞𝑞) 𝑓𝑓
3(𝑞𝑞)
̅𝑥𝑥 = 𝑓𝑓 �𝑞𝑞 ̅𝑥𝑥 + Δ𝑥𝑥 = 𝑓𝑓 �𝑞𝑞 + Δ𝑞𝑞 ≃ 𝑓𝑓 �𝑞𝑞 + ̅𝐽𝐽Δ𝑞𝑞
̅𝐽𝐽 𝑖𝑖𝑖𝑖 = 𝜕𝜕𝑓𝑓 𝑖𝑖
𝜕𝜕𝜃𝜃 𝑖𝑖
定数項
1
次項1
次導関数の値（微係数）例題： 1
̅𝜃𝜃 = ̅𝜃𝜃
2= ̅𝜃𝜃
3= 𝜋𝜋/6
のときのヤコビアンを求めよ.
𝑓𝑓 𝑞𝑞 = 𝑓𝑓
1(𝑞𝑞) 𝑓𝑓
2(𝑞𝑞)
𝑓𝑓
3(𝑞𝑞) = 2 c 𝜃𝜃
1s 𝜃𝜃
2+ 2c 𝜃𝜃
1s(𝜃𝜃
2+ 𝜃𝜃
3) 2 s 𝜃𝜃
1s 𝜃𝜃
2+ 2s 𝜃𝜃
1s(𝜃𝜃
2+ 𝜃𝜃
3)
2 + 2 c 𝜃𝜃
2+2c(𝜃𝜃
2+ 𝜃𝜃
3) ̅𝐽𝐽
𝑖𝑖𝑖𝑖= 𝜕𝜕𝑓𝑓
𝑖𝑖𝜕𝜕𝜃𝜃
𝑖𝑖Δ𝑥𝑥 = ̅𝐽𝐽Δ𝑞𝑞 .
これよりΔ𝑞𝑞 =
−1̅𝐽𝐽 Δ𝑥𝑥
とすれば,
現在の手先位置の微小変化に対応 する関節角度の操作量が分かる.
𝑀𝑀
−1= 1
𝑀𝑀 𝑎𝑎𝑑𝑑𝑎𝑎 (𝑀𝑀)
正方行列から第
𝑖𝑖
行と第𝑎𝑎
列を取り除いた小行列の行列に符号数−1
𝑖𝑖+𝑖𝑖 をかけたものを𝑖𝑖, 𝑎𝑎
要素の余因子という.
行列
𝑀𝑀
に対して,
その𝑖𝑖, 𝑎𝑎
要素が𝑀𝑀
の𝑎𝑎, 𝑖𝑖
要素の余因子となって いる行列を𝑀𝑀
の余因子行列といい,𝑎𝑎𝑑𝑑𝑎𝑎(𝑀𝑀)
で表す.𝑀𝑀
の行列式を𝑀𝑀
で表すとき, 𝑀𝑀
の逆行列は次式で与えられる:
あとは
(
粛々と) 𝑀𝑀
の余因子行列を計算すれば −1̅𝐽𝐽
が求まる.
𝐽𝐽
1𝛽𝛽 cos(𝜃𝜃
2− 𝜃𝜃
1)
𝛽𝛽 cos(𝜃𝜃
1− 𝜃𝜃
2) 𝐽𝐽
2̈𝜃𝜃
1̈𝜃𝜃
2+ 0 𝛽𝛽 sin(𝜃𝜃
1− 𝜃𝜃
2)
𝛽𝛽 sin(𝜃𝜃
2− 𝜃𝜃
1) 0 ̇𝜃𝜃
12̇𝜃𝜃
22+ 𝐷𝐷
1+ 𝐷𝐷
2−𝐷𝐷
2−𝐷𝐷
2𝐷𝐷
2̇𝜃𝜃
1̇𝜃𝜃
2− 𝑚𝑚
1+ 2𝑚𝑚
2𝑔𝑔ℓ
1sin 𝜃𝜃
1𝑚𝑚
2𝑔𝑔ℓ
2sin 𝜃𝜃
2= 𝜏𝜏
1− 𝜏𝜏
2𝜏𝜏
2（前出）2リンクアームの運動方程式：
一般のロボットの運動方程式：
𝜃𝜃 :
関節角ベクトル𝐽𝐽 𝜃𝜃 ̈𝜃𝜃 + 𝐶𝐶 ̇𝜃𝜃, 𝜃𝜃 + 𝐷𝐷 ̇𝜃𝜃 + 𝑃𝑃 𝜃𝜃 = 𝜏𝜏
慣性力項 粘性摩擦力項 駆動トルク
-
順動力学•
各関節に作用するトルク→
ロボットの動きの関係-
逆動力学(Inverse Dynamics)
•
ロボットの姿勢 （各関節の角度の時間関数）→
各関節に 作用しているトルクの関係通常, フィードバック制御で考えるのは順動力学
-
順動力学与えたトルクに対するロボットの動きをシミュレーションする際に用いられる
.
数値計算法： オイラー法
̇𝑥𝑥 = 𝑓𝑓 𝑡𝑡, 𝑥𝑥 , 𝑥𝑥 𝑡𝑡
0= 𝑥𝑥
0に対して𝑥𝑥 𝑡𝑡
0+ 𝛥𝛥𝑡𝑡 = 𝑥𝑥 𝑡𝑡
0+ �
𝑡𝑡0
𝑡𝑡0+𝛥𝛥𝑡𝑡
̇𝑥𝑥 𝜏𝜏 𝑑𝑑𝜏𝜏 = 𝑥𝑥 𝑡𝑡
0+ �
𝑡𝑡0
𝑡𝑡0+ 𝛥𝛥𝑡𝑡
𝑓𝑓 𝑡𝑡, 𝑥𝑥 𝑑𝑑𝜏𝜏
∫
𝑡𝑡𝑡𝑡00+𝛥𝛥𝑡𝑡𝑓𝑓 𝑡𝑡, 𝑥𝑥 𝑑𝑑𝜏𝜏 ≃ Δ𝑡𝑡𝑓𝑓 𝑡𝑡
0, 𝑥𝑥 𝑡𝑡
0 なる近似を用いる𝑥𝑥
̇𝑥𝑥
𝐽𝐽 𝜃𝜃 ̈𝜃𝜃 + 𝐶𝐶 ̇𝜃𝜃, 𝜃𝜃 + 𝐷𝐷 ̇𝜃𝜃 + 𝑃𝑃 𝜃𝜃 = 𝜏𝜏
一般のロボットの運動方程式：
𝑑𝑑
𝑑𝑑𝑡𝑡 ̇𝜃𝜃 = ̈𝜃𝜃 = 𝐽𝐽 𝜃𝜃
−1𝜏𝜏 − 𝐶𝐶 ̇𝜃𝜃, 𝜃𝜃 − 𝐷𝐷 ̇𝜃𝜃 − 𝑃𝑃 𝜃𝜃 𝑑𝑑
𝑑𝑑𝑡𝑡 𝜃𝜃 = ̇𝜃𝜃
𝑥𝑥 = 𝜃𝜃
̇𝜃𝜃 , 𝑓𝑓 𝑥𝑥, 𝑡𝑡 = ̇𝜃𝜃
𝐽𝐽 𝜃𝜃
−1𝜏𝜏 − 𝐶𝐶 ̇𝜃𝜃, 𝜃𝜃 − 𝐷𝐷 ̇𝜃𝜃 − 𝑃𝑃 𝜃𝜃
とおくと̇𝑥𝑥 = 𝑓𝑓 𝑡𝑡, 𝑥𝑥
であるので,
初期値𝜃𝜃 0 , ̇𝜃𝜃 0
と𝜏𝜏 𝑡𝑡
が与えられれば解軌道
𝜃𝜃 𝑡𝑡 , ̇𝜃𝜃 𝑡𝑡
を計算できる.
-
逆動力学(Inverse Dynamics)
望ましいロボットの姿勢 （各関節角度）を時間関数として与え
,
各関節に 加えるべきトルクを計算𝐽𝐽 𝜃𝜃 ̈𝜃𝜃 + 𝐶𝐶 ̇𝜃𝜃, 𝜃𝜃 + 𝐷𝐷 ̇𝜃𝜃 + 𝑃𝑃 𝜃𝜃 = 𝜏𝜏
一般のロボットの運動方程式：
𝜃𝜃(𝑡𝑡)
が与えられているとき, ̇𝜃𝜃 𝑡𝑡 , ̈𝜃𝜃 𝑡𝑡
が計算できる.
これらを左辺に代入すれば
,
加えるべきトルク𝜏𝜏 𝑡𝑡
が求められる.
正確なモデルに依存したフィードフォワード制御なので, 通常これだけで
