ロボティクス基礎
担当：平田 健太郎
第
1
学期 木Ⅲ・Ⅳ限 11
：00-13
：10 1
号館 大講義室5/16
第5
回 運動学・動力学2
4/11
第1
回 序論4/16 *
第２回 運動方程式 （4/25
休講分）4/18
第３回 ラグランジュ法5/9
第４回 座標変換5/16
第５回 運動学・動力学5/21 *
第６回 線形制御との関わり （5/23
休講分）5/30
第７回 サーボ系6/6
第８回 まとめ/
期末試験講義日程（予定）
Basic Robotics
*
補講前回のおさらい
ロボットの姿勢変化を座標変換として 表現し , これをもとに各リンクの物理量
を求める . （ → ラグランジュの運動方程式）
回転変換
Basic Robotics
4
例題：
𝑥𝑥
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 軸まわりに−𝜋𝜋
回転Basic Robotics
6
例題：
𝑥𝑥
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
Basic Robotics
8
座標系の逐次変換回転行列を
𝐴𝐴 𝜃𝜃
𝑖𝑖 で表すと𝑥𝑥
𝑖𝑖, 𝑦𝑦
𝑖𝑖, 𝑧𝑧
𝑖𝑖= 𝑥𝑥
𝑖𝑖−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 のとき, 各リンク座標系の方向と 姿勢は基準座標系と一致 しているものとする.
Basic Robotics
10
例題：
𝑥𝑥
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
と略記Basic Robotics
12
𝑥𝑥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 の座標を求めよ.
Basic Robotics
14
運動学・動力学
座標変換による物理量の漸化式とラグランジュ法を用 いれば
, (3
次元空間内の)
ロボットの運動方程式が導出 できるので,
ロボットのモデリングに関しての議論は一 段落.
次にロボットの制御を考えるにあたって,
運動学・動力学の概念を整理しておく
.
Basic Robotics
16
運動学
(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)
Basic Robotics
18
-
順運動学•
各関節の角度・角速度→
ロボットの動きの関係-
逆運動学(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𝑥𝑥
を解析的に求めるのは困難.
各動作点近傍で 線形近似して求める.Basic Robotics
20
𝑦𝑦 = 𝑓𝑓(𝑥𝑥)
を𝑥𝑥 = 𝑎𝑎
のまわりで多項式によって近似することをいう.
関数のテーラー展開
𝑦𝑦 = 𝑓𝑓 𝑎𝑎 + 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次までの テーラー展開をとる
高次項は無視する
Δ𝜃𝜃𝑖𝑖 の係数を整理
Basic Robotics
22
同様にして, Δ�𝑦𝑦
𝑒𝑒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
次導関数の値（微係数）Basic Robotics
24
例題： 1
̅𝜃𝜃 = ̅𝜃𝜃
2= ̅𝜃𝜃
3= 𝜋𝜋/6
のときのヤコビアンを求めよ.
例題： 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) ̅𝐽𝐽
𝑖𝑖𝑖𝑖= 𝜕𝜕𝑓𝑓
𝑖𝑖𝜕𝜕𝜃𝜃
𝑖𝑖̅𝐽𝐽
11= 𝜕𝜕𝑓𝑓
1𝜕𝜕𝜃𝜃
1= −2𝑠𝑠𝜃𝜃
1𝑠𝑠𝜃𝜃
2− 2𝑠𝑠𝜃𝜃
1𝑠𝑠 𝜃𝜃
2+ 𝜃𝜃
3→ −2 1 2
1
2 − 2 1 2
3
2 = − 1
2 − 3 2
̅𝐽𝐽
12= 𝜕𝜕𝑓𝑓
1𝜕𝜕𝜃𝜃
2= 2𝑐𝑐𝜃𝜃
1𝑐𝑐𝜃𝜃
2+ 2𝑐𝑐𝜃𝜃
1𝑐𝑐 𝜃𝜃
2+ 𝜃𝜃
3→ 3 2 +
3 2
̅𝐽𝐽
13= 𝜕𝜕𝑓𝑓
1𝜕𝜕𝜃𝜃
3= 2𝑐𝑐𝜃𝜃
1𝑐𝑐(𝜃𝜃
2+ 𝜃𝜃
3) → 3 2
𝜋𝜋/6 3
2 1
Basic Robotics
26 𝑓𝑓 𝑞𝑞 = 𝑓𝑓
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) ̅𝐽𝐽
𝑖𝑖𝑖𝑖= 𝜕𝜕𝑓𝑓
𝑖𝑖𝜕𝜕𝜃𝜃
𝑖𝑖̅𝐽𝐽
21= 𝜕𝜕𝑓𝑓
2𝜕𝜕𝜃𝜃
1= 2𝑐𝑐𝜃𝜃
1𝑠𝑠𝜃𝜃
2+ 2𝑐𝑐𝜃𝜃
1𝑠𝑠 𝜃𝜃
2+ 𝜃𝜃
3→ 3 2 +
3 2
̅𝐽𝐽
22= 𝜕𝜕𝑓𝑓
2𝜕𝜕𝜃𝜃
2= 2𝑠𝑠𝜃𝜃
1𝑐𝑐𝜃𝜃
2+ 2𝑠𝑠𝜃𝜃
1𝑐𝑐(𝜃𝜃
2+ 𝜃𝜃
3) → 3 2 +
1 2
̅𝐽𝐽
23= 𝜕𝜕𝑓𝑓
2𝜕𝜕𝜃𝜃
3= 2𝑠𝑠𝜃𝜃
1𝑐𝑐(𝜃𝜃
2+ 𝜃𝜃
3) → 1 2
𝜋𝜋/6 3
2 1
𝑓𝑓 𝑞𝑞 = 𝑓𝑓
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) ̅𝐽𝐽
𝑖𝑖𝑖𝑖= 𝜕𝜕𝑓𝑓
𝑖𝑖𝜕𝜕𝜃𝜃
𝑖𝑖̅𝐽𝐽
31= 𝜕𝜕𝑓𝑓
3𝜕𝜕𝜃𝜃
1= 0 → 0
̅𝐽𝐽
32= 𝜕𝜕𝑓𝑓
3𝜕𝜕𝜃𝜃
2= −2𝑠𝑠𝜃𝜃
2− 2𝑠𝑠 𝜃𝜃
2+ 𝜃𝜃
3→ −1 − 3
̅𝐽𝐽
33= 𝜕𝜕𝑓𝑓
3𝜕𝜕𝜃𝜃
3= −2𝑠𝑠 𝜃𝜃
2+ 𝜃𝜃
3→ − 3
𝜋𝜋/6 3
2 1
Basic Robotics
28
̅𝐽𝐽 =
− 1
2 − 3 2
3 2 +
3 2
3 3 2
2 + 3 2
3 2 +
1 2
1 0 −1 − 3 − 2 3
= 1 2
−1 − 3 3 + 3 3
3 + 3 1 + 3 1
0 −2 − 2 3 −2 3
=: 𝑀𝑀 2
̅𝐽𝐽
−1
= 2𝑀𝑀
−1Δ𝑥𝑥 = ̅𝐽𝐽Δ𝑞𝑞 .
これよりΔ𝑞𝑞 =
−1̅𝐽𝐽 Δ𝑥𝑥
とすれば,
現在の手先位置の微小変化に対応する関節角度の操作量が分かる
.
𝑀𝑀 = −1 − 3 −2 3 1 + 3 + 2 + 2 3 − 3 + 3 −2 3 3 + 3 + 3 2 + 2 3
= 16(1 + 3)
𝑀𝑀
−1= 1
𝑀𝑀 𝑎𝑎𝑑𝑑𝑎𝑎 (𝑀𝑀)
正方行列から第
𝑖𝑖
行と第𝑎𝑎
列を取り除いた小行列の行列に符号数−1
𝑖𝑖+𝑖𝑖 をかけたものを𝑖𝑖, 𝑎𝑎
要素の余因子という.行列
𝑀𝑀
に対して, その𝑖𝑖, 𝑎𝑎
要素が𝑀𝑀
の𝑎𝑎, 𝑖𝑖
要素の余因子となって いる行列を𝑀𝑀
の余因子行列といい, 𝑎𝑎𝑑𝑑𝑎𝑎(𝑀𝑀)
で表す.
𝑀𝑀
の行列式を𝑀𝑀
で表すとき, 𝑀𝑀
の逆行列は次式で与えられる:
あとは
(
粛々と) 𝑀𝑀
の余因子行列を計算すれば −1̅𝐽𝐽
が求まる.
̅𝐽𝐽
−1
= 2 −0.0915 0.1585 0
0.3750 0.2165 0.2500
−0.5915 −0.3415 −0.6830
30
Basic Robotics
𝐽𝐽
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)
•
ロボットの姿勢 （各関節の角度の時間関数）→
各関節に 作用しているトルクの関係通常, フィードバック制御で考えるのは順動力学
Basic Robotics
32
-
順動力学与えたトルクに対するロボットの動きをシミュレーションする際に用いられる
.
数値計算法： オイラー法
̇𝑥𝑥 = 𝑓𝑓 𝑡𝑡, 𝑥𝑥 , 𝑥𝑥 𝑡𝑡
0= 𝑥𝑥
0に対して𝑥𝑥 𝑡𝑡
0+ 𝛥𝛥𝑡𝑡 = 𝑥𝑥 𝑡𝑡
0+ �
𝑡𝑡0
𝑡𝑡0+𝛥𝛥𝑡𝑡
̇𝑥𝑥 𝜏𝜏 𝑑𝑑𝜏𝜏 = 𝑥𝑥 𝑡𝑡
0+ �
𝑡𝑡0
𝑡𝑡0+ 𝛥𝛥𝑡𝑡
𝑓𝑓 𝑡𝑡, 𝑥𝑥 𝑑𝑑𝜏𝜏
∫
𝑡𝑡𝑡𝑡00+𝛥𝛥𝑡𝑡𝑓𝑓 𝑡𝑡, 𝑥𝑥 𝑑𝑑𝜏𝜏 ≃ Δ𝑡𝑡𝑓𝑓 𝑡𝑡
0, 𝑥𝑥 𝑡𝑡
0 なる近似を用いる𝑡𝑡 𝑥𝑥
𝑡𝑡
0𝛥𝛥𝑡𝑡
̇𝑥𝑥
𝐽𝐽 𝜃𝜃 ̈𝜃𝜃 + 𝐶𝐶 ̇𝜃𝜃, 𝜃𝜃 + 𝐷𝐷 ̇𝜃𝜃 + 𝑃𝑃 𝜃𝜃 = 𝜏𝜏
一般のロボットの運動方程式：
𝑑𝑑
𝑑𝑑𝑡𝑡 ̇𝜃𝜃 = ̈𝜃𝜃 = 𝐽𝐽 𝜃𝜃
−1𝜏𝜏 − 𝐶𝐶 ̇𝜃𝜃, 𝜃𝜃 − 𝐷𝐷 ̇𝜃𝜃 − 𝑃𝑃 𝜃𝜃 𝑑𝑑
𝑑𝑑𝑡𝑡 𝜃𝜃 = ̇𝜃𝜃
𝑥𝑥 = 𝜃𝜃
̇𝜃𝜃 , 𝑓𝑓 𝑥𝑥, 𝑡𝑡 = ̇𝜃𝜃
𝐽𝐽 𝜃𝜃
−1𝜏𝜏 − 𝐶𝐶 ̇𝜃𝜃, 𝜃𝜃 − 𝐷𝐷 ̇𝜃𝜃 − 𝑃𝑃 𝜃𝜃
とおくと̇𝑥𝑥 = 𝑓𝑓 𝑡𝑡, 𝑥𝑥
であるので,
初期値𝜃𝜃 0 , ̇𝜃𝜃 0
と𝜏𝜏 𝑡𝑡
が与えられれば解軌道
𝜃𝜃 𝑡𝑡 , ̇𝜃𝜃 𝑡𝑡
を計算できる.
Basic Robotics
34
-
逆動力学(Inverse Dynamics)
望ましいロボットの姿勢 （各関節角度）を時間関数として与え
,
各関節に 加えるべきトルクを計算𝐽𝐽 𝜃𝜃 ̈𝜃𝜃 + 𝐶𝐶 ̇𝜃𝜃, 𝜃𝜃 + 𝐷𝐷 ̇𝜃𝜃 + 𝑃𝑃 𝜃𝜃 = 𝜏𝜏
一般のロボットの運動方程式：
𝜃𝜃(𝑡𝑡)
が与えられているとき, ̇𝜃𝜃 𝑡𝑡 , ̈𝜃𝜃 𝑡𝑡
が計算できる.
これらを左辺に代入すれば
,
加えるべきトルク𝜏𝜏 𝑡𝑡
が求められる.
正確なモデルに依存したフィードフォワード制御なので, 通常これだけで
,
うまくいくとは考えにくい.
(
動力学でなく)
運動学に基づいてロボットを制御する場合も 多い.
•
ロボットの運動は複雑なので,
静的な姿勢の問題,
あるいは準静的 な速度の問題として考える.
•
各関節毎に（モデルレスの）ハイゲインフィードバックを施せば,
角度（角速度）の指令値に実際の角度（角速度）が追従するとみなしてよ い場合がある
.
モータ モータ
制御系 手先方向のリンクを
含む負荷
電流 トルク 角度
角速度 指令値
（角度・角速度）
（運動方程式）
