ロボット工学の基礎 ―マニピュレータの運動学・動 …第2章...
TRANSCRIPT
ロボット工学の基礎
―マニピュレータの運動学・動力学―
岡山大学 自然科学研究科
見浪 護
平成 29 年 1 月 30 日
目 次
章 1章 ラグランジュ法による動力学解法 3
1.1 ラグランジュの方程式 . . . . . . . . . . . . . . . . . . . . . . . . 3
章 2章 2リンクマニピュレータのラグランジュ解法 5
2.1 ２リンクマニピュレータの運動方程式 . . . . . . . . . . . . . . . 5
章 3章 運動学 9
3.1 順・逆運動学関係とは？ . . . . . . . . . . . . . . . . . . . . . . . 9
3.2 リンク座標系 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.3 座標変換行列 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.4 座標変換行列の性質 . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.5 ハンド位置の記述 . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.6 同次変換行列 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.7 同次変換行列の性質 . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.8 逆運動学の解 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
章 4章 3リンクマニピュレータのラグランジュ解法 34
4.1 運動エネルギー . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.1.1 並進運動エネルギー . . . . . . . . . . . . . . . . . . . . . 34
4.1.2 重心の速度ベクトル . . . . . . . . . . . . . . . . . . . . . 35
4.1.3 リンク回転角速度ベクトル . . . . . . . . . . . . . . . . . . 36
4.1.4 リンク速度ベクトル . . . . . . . . . . . . . . . . . . . . . 38
4.2 回転運動エネルギー . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.3 位置エネルギーと損失エネルギー . . . . . . . . . . . . . . . . . . 49
4.3.1 運動方程式の導出 . . . . . . . . . . . . . . . . . . . . . . . 50
章 5章 2リンクマニピュレータのニュートンオイラー解法 54
5.1 ニュートン・オイラー法 . . . . . . . . . . . . . . . . . . . . . . . 54
5.2 ２リンクの運動方程式 . . . . . . . . . . . . . . . . . . . . . . . . 54
5.2.1 準備 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
5.2.2 運動方程式の導出 . . . . . . . . . . . . . . . . . . . . . . . 55
章 6章 平面 3リンクマニピュレーターのニュートンオイラー解法 60
6.1 緒言 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
6.2 3リンクマニピュレーターの運動方程式導出 . . . . . . . . . . . . 60
6.2.1 準備 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
6.2.2 運動方程式導出 . . . . . . . . . . . . . . . . . . . . . . . . 62
1
章 7章 垂直多関節 3リンクマニピュレータのニュートンオイラー解法 66
7.1 準備 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
7.2 導出，正順繰り返し計算 . . . . . . . . . . . . . . . . . . . . . . . 67
7.2.1 i=1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
7.2.2 i=2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
7.2.3 i=3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
7.3 導出，逆順繰り返し計算 . . . . . . . . . . . . . . . . . . . . . . . 70
7.3.1 i=3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
7.3.2 i=2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
7.3.3 i=1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
章 8章 数値積分法 (ルンゲクッタ法)の導出 73
章 9章 付録 81
2
第 1章
ラグランジュ法による動力学解法
1.1 ラグランジュの方程式ある一つの剛体の運動エネルギーを T，ポテンシャルエネルギーをU，損失エ
ネルギーをDとする．また一般化座標を q，一般化速度を q，一般化入力を u と
すると，この物体のラグランジュ方程式は，
d
dt(∂T
∂q) − ∂T
∂q+
∂D
∂q+
∂U
∂q= u (1.1)
と与えられる．T，D，U に関する偏微分と時間微分を計算すると上式は，
mq + cq + g = u (1.2)
と表わすことができ，この式を運動方程式という．
ロボットは一般的に多リンクで構成されているので，q，q，u，T，U，Dは
それぞれ，q = [q1, q2, · · · , qn]T，q = [q1, q2, · · · , qn]T，u = [u1, u2, · · · , un]T，
T (q, q)，U(q)，D(q)，と表わされる．ここで“T”は転置を表わす．よって，q =
[q1, q2, · · · , qn]Tはqが縦ベクトルであることを意味している．q，q，uはそれぞれ
ベクトルであるが，T (q, q)，U(q)，D(q)，はそれぞれq，qを変数とするスカラ関
数であることに注意しておく．i = 1, 2, · · · , nの第 iリンクのT，U，Dをそれぞれ
Ti，Ui，Diとするとき，Ti，Ui，Diは，qi = [q1, q2, · · · , qi]T，ui = [u1, u2, · · · , ui]
T
の関数となり，ロボットの運動エネルギーT (q, q)，ポテンシャルエネルギーU(q)，
損失エネルギーD(q)は，
T (q, q) =n∑
i=1
Ti(qi, qi) (1.3)
U(q) =n∑
i=1
Ui(qi) (1.4)
D(q) =n∑
i=1
Di(qi) (1.5)
と表わされる．第 iリンクの運動エネルギー Tiが qi = [q1, q2, · · · , qn]T ,
qi = [q1, q2, · · · , qn]T の関数 Ti(qi, qi)と表わされる．Ti(qn, qn)と表わさない理由
は，第 iリンクの角度や速度は q1～qi，q1～qiの関数であり，第 i + 1～n間のリ
3
ンクの角度や角速度には無関係であるからである．
ロボットのラグランジュの方程式は上の T，D，U を用いて，
d
dt(∂T (q, q)
∂qi
) − ∂T (q, q)
∂qi
+∂D(q)
∂qi
+∂U(q)
∂qi
= ui (i = 1, 2, · · · , n) (1.6)
と求めることができる．上式の微分を計算すると，リンク多関節ロボットの運動
方程式
M(q)q + c(q, q) + g(q) = u (1.7)
が得られる．
以上が運動方程式を求める概要であるが，これだけでは T，D，U をどの様に
して得るのか，式 (1.6)の偏微分の具体的な式はどうなっているのかなど不明で
ある．以下では，運動方程式 (1.7)を得る手段について垂直多関節マニピュレー
タを例にとり，何も省略することなく具体的に述べる．
運動エネルギーには 2種類あり，並進の運動エネルギーを Tt，回転の運動エネ
ルギーを Trと呼ぶことにする．さらに第 iリンクのそれぞれを Tti，Triと呼ぶと
第 iリンクの運動エネルギー Tiは，
Ti = Tti + Tri (1.8)
と表わされる．ここでTti，Triはそれぞれ第 iリンクの重心の並進運動エネルギー
と重心回りの回転エネルギーを表わすことに注意しておく．
4
第 2章
2リンクマニピュレータの
ラグランジュ解法
2.1 ２リンクマニピュレータの運動方程式
y
x
l1
l2
Olg1
lg2
q1
q2
G1
G2
g
図 2.1: 2links manipulator
図 2.1で示した平面２リンクマニピュレータの運動方程式について述べる．運
動エネルギーと位置エネルギーを求めるには各リンクの重心の位置と並進速度を
記述する必要がある．
まず，第１リンクの重心G1の位置座標 [x1 y1]T は以下となる．
[x1
y1
]=
[lg1 cos q1
lg1 sin q1
](2.1)
式（2.1）の両辺を時間 tで微分して次式を得る．[
x1
y1
]=
[−lg1q1 sin q1
lg1q1 cos q1
](2.2)
したがって，第１リンクの慣性モーメントを I1，運動エネルギーを T1，位置エネ
ルギーをU1，関節の粘性摩擦係数を d1，損失エネルギーをD1とすると，以下の
5
ようになる．
T1 =1
2m1(x
21 + y2
1) +1
2I1q
21
=1
2m1
{(−lg1q1 sin q1)
2 + (lg1q1 cos q1)2}
+1
2I1q
21
=1
2m1lg1
2q21
{(− sin q1)
2 + (cos q1)2}
+1
2I1q
21
=1
2(m1lg1
2q21 + I1q
21) (2.3)
U1 = m1gy1 = m1glg1 sin q1 (2.4)
D1 =1
2d1q
21 (2.5)
次に，第２リンクの重心G2の位置座標 [x2 y2]T は以下となる．
[x2
y2
]=
[l1 cos q1 + lg2 cos (q1 + q2)
l1 sin q1 + lg2 sin (q1 + q2)
](2.6)
式（2.6）の両辺を時間 tで微分して次式を得る．[
x2
y2
]=
[−l1q1 sin q1 − lg2(q1 + q2) sin (q1 + q2)
l1q1 cos q1 + lg2(q1 + q2) cos (q1 + q2)
](2.7)
したがって，第２リンクの慣性モーメントを I2，運動エネルギーを T2，位置エネ
ルギーをU2，関節の粘性摩擦係数を d1，損失エネルギーをD2とすると，以下の
ようになる．
T2 =1
2m2(x
22 + y2
2) +1
2I2(q1 + q2)
2
=1
2m2
[{−l1q1 sin q1 − lg2(q1 + q2) sin (q1 + q2)
}2
+{l1q1 cos q1 + lg2(q1 + q2) cos (q1 + q2)
}2]
+1
2I2(q1 + q2)
2
=1
2m2
{l21q
21 sin2 q1 + 2l1lg2q1(q1 + q2) sin q1 sin (q1 + q2) + l2g2(q1 + q2)
2 sin2 (q1 + q2)
+l21q21 cos2 q1 + 2l1lg2q1(q1 + q2) cos q1 cos (q1 + q2) + l2g2(q1 + q2)
2 cos2 (q1 + q2)}
=1
2m2
[l21q
21(sin
2 q1 + cos2 q1) + 2l1lg2q1(q1 + q2){cos q1 cos (q1 + q2) + sin q1 sin (q1 + q2)
}
+l2g2(q21 + q2
2){sin2 (q1 + q2) + cos2 (q1 + q2)
}]+
1
2I2(q1 + q2)
2
=1
2
[m2
{l21q
21 + l2g2(q1 + q2)
2 + 2l1lg2q1(q1 + q2) cos q2
}
+I2(q1 + q2)2]
(2.8)
ここで加法定理より，cos q1 cos (q1 + q2) + sin q1 sin (q1 + q2) = cos q2である．
U2 = m2gy2 = m2g{l1 sin q1 + lg2 sin (q1 + q2)
}(2.9)
D2 =1
2d2q
22 (2.10)
6
したがって，総運動エネルギー T = T1 + T2，総位置エネルギーU = U1 + U2，総
損失エネルギーD = D1 + D2は次のようになる．
T = T1 + T2
=1
2(m1l
2g1q
21 + I1q
21)
+1
2
[m2
{l21q
21 + l2g2(q1 + q2)
2 + 2l1lg2q1(q1 + q2) cos q2
}+ I2(q1 + q2)
2](2.11)
U = U1 + U2 = m1glg1 sin q1 + m2g{l1 sin q1 + lg2 sin (q1 + q2)
}(2.12)
D = D1 + D2 =1
2(d1q
21 + d2q
22) (2.13)
これらより，∂T/∂q1および d(∂T/∂q1)/dtについて,d(q1 cos q2)/dt = q1 cos q2 −q1q2 sin q2, d(q2 cos q2)/dt = q2 cos q2 − q2
2 sin q2に注意すると，
∂T
∂q1
= m1l2g1q1 + I1q1 +
1
2
[m2
{(2l21q1 + 2l2g2(q1 + q2)
+2l1lg2(q1 + q2) cos q2 + 2l1lg2q1 cos q2
}+ 2I2(q1 + q2)
]
= m1l2g1q1 + I1q1 + m2
{l21q1 + l2g2(q1 + q2) + 2l1lg2q1 cos q2
+l1lg2q2 cos q2
}+ I2(q1 + q2) (2.14)
d
dt(∂T
∂q1
) =d
dt
[m1l
2g1q1 + I1q1 + m2
{l21q1 + l2g2(q1 + q2) + 2l1lg2q1 cos q2
+l1lg2q2 cos q2
}+ I2(q1 + q2)
]
={I1 + m1l
2g1 + I2 + m2(l
21 + l2g2 + 2l1lg2 cos q2)
}q1
+{I2 + m2(l
2g2 + l1lg2 cos q2)
}q2 − m2l1lg2(2q1q2 + q2
2) sin q2 (2.15)
である．さらに，
∂T
∂q1
= 0 (2.16)
∂U
∂q1
= m1glg1 cos q1 + m2g{l1 cos q1 + lg2 cos (q1 + q2)
}(2.17)
∂D
∂q1
= d1q1 (2.18)
d
dt(∂T
∂q2
) =d
dt
{m2l
2g2(q1 + q2) + m2l1lg2(q1 cos q2) + I2(q1 + q2)
}
={I2 + m2(l
2g2 + l1lg2 cos q2)
}q1 + (I2 + m2l
2g2)q2
−m2l1lg2q1q2 sin q2 (2.19)
7
∂T
∂q2
= −m2l1lg2q1(q1 + q2) sin q2 (2.20)
∂U
∂q2
= m2glg2 cos (q1 + q2) (2.21)
∂D
∂q2
= d2q2 (2.22)
が成り立つ．式 (2.15)～(2.22)に対するラグランジュの運動方程式より，下線部
の計算に注意すると２組の駆動トルク τ1，τ2に関する運動方程式を得る．
τ1 =d
dt(∂T
∂q1
) − ∂T
∂q1
+∂U
∂q1
+∂D
∂q1
={I1 + m1l
2g1 + I2 + m2(l
21 + l2g2 + 2l1lg2 cos q2)
}q1
+{I2 + m2(l
2g2 + l1lg2 cos q2)
}q2 − m2l1lg2(2q1q2 + q2
2) sin q2
+m1glg1 cos q1 + m2g{l1 cos q1 + lg2 cos (q1 + q2)
}+ d1q1 (2.23)
τ2 =d
dt(∂T
∂q2
) − ∂T
∂q2
+∂U
∂q2
+∂D
∂q2
={I2 + m2(l
2g2 + l1lg2 cos q2)
}q1 + (I2 + m2l
2g2)q2−m2l1lg2q1q2 sin q2
+m2l1lg2q1(q1+q2)sin q2 + m2glg2 cos (q1 + q2) + d2q2
={I2 + m2(l
2g2 + l1lg2 cos q2)
}q1 + (I2 + m2l
2g2)q2 + m2l1lg2q
21 sin q2
+m2glg2 cos (q1 + q2) + d2q2 (2.24)
式（2.23），（2.24）の運動方程式系は，連立非線形微分方程式となっており複雑
に見えるが，次のように整理することができる．[
τ1
τ2
]=
[I1 + m1l
2g1 + I2 + m2(l
21 + l2g2 + 2l1lg2 cos q2) I2 + m2(l
2g2 + l1lg2 cos q2)
I2 + m2(l2g2 + l1lg2 cos q2) I2 + m2l
2g2
][q1
q2
]
+
[−m2l1lg2(2q1q2 + q2
2) sin q2
m2l1lg2q21 sin q2
]+
[m1glg1 cos q1 + m2g
{l1 cos q1 + lg2 cos (q1 + q2)
}
m2glg2 cos (q1 + q2)
]
+
[d1 0
0 d2
][q1
q2
](2.25)
式（5.18）を簡単にすると，
τ = M(q)q + h(q, q) + g(q) + Dq (2.26)
となる．
式（2.26）のM(q)qは慣性項であり，h(q, q)は遠心力とコリオリ力からなる
項，g(q)は重力項，Dは関節の粘性摩擦係数を表す対角行列である．特に行列
M(q)は慣性行列（inertia matrix）と呼ばれており，ロボットアームの動力学特
性を特徴づける重要な行列である．
8
第 3章
運動学
3.1 順・逆運動学関係とは？ロボット運動学とは，基本的にはハンドの位置・姿勢とそれを実現する関節
角度の関係のことを表す．ハンドの速度と関節角速度・角加速度との関係も含め
て，運動学と呼ぶが，ここでは上の基本的関係について述べる．床に固定され
た直交座標系をΣW と呼ぶことにする．ΣW で表されたハンドの位置ベクトルをW r = [W rx,
W ry,W rz]
T と表し，図 3.1に示す．W r は q = [q1, q2, q3]T で表される
関節角ベクトルの関数として，
W r =
W rx
W ry
W rz
=
fx(q1, q2, q3)
fy(q1, q2, q3)
fz(q1, q2, q3)
= f(q) (3.1)
と表すことができる．この関係を運動学関係といい，
q = f−1(W r) (3.2)
を逆運動学関係という．
W
rWP
図 3.1:
3.2 リンク座標系２リンクで構成されるロボットの順逆運動学関係は，幾何学的に求めることも
容易である．しかし３リンク以上になると直感的に求めることは難しくなるため，
解析的に式 (3.1)，(3.2)を求める方法が必要になる．準備として，構成リンク全
てにリンク座標系と呼ばれる動座標系を設定する．この設定方法は一意でなく，
好みに応じて決定することができるが，設定に依存して以下の運動学解析が変化
する．しかし，最終的な結果である式 (3.1)，(3.2)には影響しない．だからといっ
9
x
z
Pi
i リンクi-1 リンク
図 3.2:
[
]
[d \ ]
\
�
;�8Ð=�<]�D�
[
]
\ d ] [
\�
;�8Ð=�<[�E�
[
]
] d [ \
\�
;�8Ð=�<\�F�
x y
z
図 3.3:
て座標系の設定に任意性を残したままにして運動方程式の導出を進めると，共通
の議論の妨げになるので，ここでは，広く用いられるようになったCraigの方法
を述べる．実用化されているほとんどのロボットは回転関節のみで構成されてお
り，ここでは，このようなロボットについて考える．通常，回転関節軸方向はリ
ンク中心軸に対して直交または平行であることを考慮すると，リンク座標系は，
・ z軸を回転軸方向に合わせる (図 3.2に示す)．
・ x軸をリンク中心線を通り，リンク先端方向を正とする．
・ y軸は，x,z軸に対し，右手形を構成するように設定する．右手系とは図 3.3
に示すように，x×y = z (×は外積を表わす)が成り立つ座標系のことで
ある．
ただし，Σiは第 iリンクに固定されている.回転軸方向とリンク中心線が一致
する場合は，リンク中心線に z軸を一致させ x,y軸は任意とする．以上の方法に
よってもリンク座標系の設定は完全には定まらないが，回転軸を z軸で表すこと
と定めることで，後に述べるメリットが生じる．初期状態の PUMA型３リンク
ロボットにリンク座標系Σ1，Σ2，Σ3及びハンド座標系ΣH を取り付けた状態を
図 3.4に示す．ΣW（0 − xW yW zW）は作業座標系を表し，ΣH の原点が対象物を
把持する点を意味する．
10
W 1
zW1zw
xW1xw
yW1yw
2
xW2
zW2
yW2
3
xW3
zW3
yW3
H
xWH
yWH
zWH
O
図 3.4:
3.3 座標変換行列３次元空間内を運動する剛体を記述しようとするとき，まず，その物体の姿勢
を行列で表現するすることが必要となる．ここでは，この行列を座標変換行列と
呼ぶことにする．座標変換行列を用いることで，式 (3.1)で表される順動力学関
係を解析的に求めることが容易になる．図 3.5に示すリンクAには，ΣAが固定
W
ÂË�X
ÂË�A
ÂË�B
xA
yA
zA
OA
rWA
rAB
zB
yB
xB
図 3.5:
されており，ΣW の原点からΣAの原点への位置ベクトルをW rAと表す．またΣA
の原点からΣBの原点へのベクトルをΣW で表したベクトルをW rA,Bと表すこと
にすると，ΣW の原点からΣBの原点へのベクトルW rBは，
W rB =W rA +W rA,B (3.3)
と表される．W rA，W rA,BともΣW で表現したベクトルであるため，図 3.6に示
すように加算することができる．しかし，W rA,Bを動座標系ΣAで表現したもの
11
: U%
: U$: U$�%
図 3.6:
ArA,BとW rAとは，基準とする座標が異なるため加減算を行うことができない．
ここで，ArA,BとW rA,Bとの関係であるが，ΣAからΣW への座標変換行列
W RA
を用いると，
W rA,B = W RAArA,B (3.4)
と表すことができる．式 (3.4)を式 (3.3)に代入すると，
W rB = W rA + W RAArA,B (3.5)
が得られる．なぜ式 (3.4)を用いて，W rA,BをW RAと
ArA,Bとで表したのかとい
うと，回転関節の場合ΣAはリンクＡに固定されているためArA,Bは定数ベクト
ルとなり，リンクに対するリンクＡの回転運動は，W RAの中に閉じこめて考え
ることができるからである．
W RAの中身について考えてみよう．W RAは，
W RA =[
W xAW yA
W zA
](3.6)
と定義され，W xA，W yA，
W zAはΣAの各座標軸ベクトルをΣW で表したもので
ある．従って，それぞれは単位ベクトルであり，これは，内積を用いて
(W xA)T W xA = 1
(W yA)T W yA = 1
(W zA)T W zA = 1
(3.7)
と表わされる．また互いに直交することから，
(W xA)T (W yA) = 0
(W yA)T (W zA) = 0
(W zA)T (W xA) = 0
(3.8)
である．W RAの定義より明らかなことであるが，W RAは正規直交行列である．
例３-１
ΣW の z軸回りに θ回転した座標系をΣAとすると，
W xA =[
cos θ, sin θ, 0]T
, W yA =[− sin θ, cos θ, 0
]T
, W zA =[
0, 0, 1]T
であるから,W RAは
W RA(θ) =
cos θ − sin θ 0
sin θ cos θ 0
0 0 1
(3.9)
12
: [
:\
:[$
:\$
h:h$ t�
�
��
S��
S��
: U ���
� �S� ��
��� $U
���S� �� ��
���
図 3.7:
となる．図 3.7に示す θ = 30◦のときを考えると，
W RA|θ=30◦ =
√3/2 −1/2 0
1/2√
3/2 0
0 0 1
(3.10)
であり,Ar =[ √
3/2, 1/2, 0]T
について考えると，
W RA|θ=30◦Ar =
√3/2 −1/2 0
1/2√
3/2 0
0 0 1
√3/2
1/2
0
=
1/2√3/2
0
= W r (3.11)
と変換され，ArがW RAArによりΣW で表わされた
W r =[
1/2,√
3/2, 0]T
に変換されていることが分かる．
以上より，座標変換行列 W RAは，以下の二つの特徴，
(1) ΣAの各軸をΣW で表わしたベクトルを列ベクトルとして持つ 3 × 3行列で
ある．
(2) W RAはΣAで表わされたベクトルをΣW で表わしたベクトルに変換する．
を持つことが分かった．
式 (3.9)は z軸回りの回転を意味するが，これを明確に表わすためここでは，
Rot(z, θ) =
cos θ − sin θ 0
sin θ cos θ 0
0 0 1
(3.12)
と表わす．例 3-1と同様な考察により，x軸，y軸回りの回転については，
Rot(x, θ) =
1 0 0
0 cos θ − sin θ
0 sin θ cos θ
(3.13)
Rot(y, θ) =
cos θ 0 sin θ
0 1 0
− sin θ 0 cos θ
(3.14)
となる．
13
3.4 座標変換行列の性質
式 (3.7)，(3.8)で表わされた W RAの性質より
(W RA)T (W RA) =[
W xA, W yA, W zA
]T [W xA, W yA, W zA
]
=
W xTA
W yTA
W zTA
[W xA, W yA, W zA
]
=
W xTA
W xAW xT
AW yA
W xTA
W zA
W yTA
W xAW yT
AW yA
W yTA
W zA
W zTA
W xAW zT
AW yA
W zTA
W zA
=
1 0 0
0 1 0
0 0 1
(3.15)
となることから，
(W RA)T = W RA−1
(3.16)
となることがわかる．また，
W r = W RAAr (3.17)
が成り立つとき，逆の関係
Ar = ARWW r (3.18)
も成り立つから，両式より
W r = W RAARW
W r (3.19)
が任意の W rについて，成り立つことから，
W RAARW = I3 (3.20)
である．よって，W RA = (ARW )−1であり，式 (3.16)とあわせると，
ARW = (W RA)T = W RA−1
(3.21)
が成り立つ．
次に，第３の座標系ΣBを考えると，
W r = W RBBr
Ar = ARBBr (3.22)
が成り立つから，式 (3.22)を式 (3.17)に代入すると
W RBBr =W RA
ARBBr (3.23)
が得られる．任意の Brについて成り立つことから，
W RB =W RAARB (3.24)
14
さらに，座標系ΣC も考えるとき，
W RC =W RAARC (3.25)
ARC =A RBBRC (3.26)
が成り立つことから，
W RC =W RAARB
BRC (3.27)
が得られる．これは，ΣW とΣC との関係W RC が中間座標系となったΣAとΣB
を用いて，W RA，ARBと
BRC の積で表わされ，しかもW RC はΣAとΣBの設
置方法に依存しないことを表わす．また式 (3.27)の逆の関係は
W R−1C = (W RA
ARBBRC)−1
= BR−1C
AR−1B
W R−1A
= CRBBRA
ARW
= CRW (3.28)
となるから
W R−1C =C RW (3.29)
この関係を用いると，ΣW と第 iリンクのリンク座標系Σiとの関係は，
W Ri =W R11R2 · · ·i−1 Ri (3.30)
と表わされることが分かる．図 3.8に示すΣW とΣH との関係は，
W RH(q) =W R1(q1)1R2(q2) · · ·n−1 Rn(qn) nRH (3.31)
と表わされることから，ハンドの姿勢ΣHは，リンク１からリンクｎまでの姿勢の
変化を積算することで求められることが分かる．ここで，q =[
q1, q2, · · · , qn
]T
である．また，回転関節で構成されるマニピュレータの場合，W R1は q1のみの関数
であり，1R2は q2のみの関数であることから，W R1(q1),
1 R2(q2), · · · ,n−1 Rn(qn)
と表わされることも分かる．また，ΣnとΣHとの関係は変化しない，つまり駆動
軸を持たないので nRH は定数行列となる．
W R−1H = (W R1 . . .n RH)−1
= nR−1H . . .W R−1
1
= HRnnRn−1 . . .1 RW
= HRW (3.32)
例３-２３リンクマニピュレータのリンク座標系を図 3.9に示す．このマニピュレータ
の座標変換行列 W Ri(i = 1, 2, 3, H)を求めてみよう．まず W R1についてはΣ1の
15
h:
h+T�T�íö²1
íö²2
íö²n
図 3.8:
xW
zW
yW
z1 y1
x1
x2
x3
xH
y2
y3
yH
z2
z3
zH
q1
q2q3
図 3.9:
各座標軸W x1,W y1,
W z1は，図 3.4に示すとおり，q1 = 0◦のときxW , yW , zW と
一致しているので，初期状態で，
[W x1]q1=0◦ =
1
0
0
, [W y1]q1=0◦ =
0
1
0
, [W z1]q1=0◦ =
0
0
1
(3.33)
である．よって．ΣW とΣ1の初期状態での変換行列W R1は,
W R1 =
1 0 0
0 1 0
0 0 1
(3.34)
と表される．この状態から，W z1 軸回りに q1 回転したときの座標変換行列を
Rot(z, q1)と表すと,W R1(q1)は，例 3-1より，
W R1(q1) = W R1Rot(z, q1)
=
1 0 0
0 1 0
0 0 1
C1 −S1 0
S1 C1 0
0 0 1
=
C1 −S1 0
S1 C1 0
0 0 1
(3.35)
16
と求めることができる．ここでC1 = cos q1，S1 = sin q1である．
次に 1R2を求める．初期状態での関係1R2は，図 3.4の初期状態の関係より，
W x2がW z1と，
W y2がW x1と，
W z2がW y1とそれぞれ一致していることから，
1R2 =
0 1 0
0 0 1
1 0 0
(3.36)
であり，この初期状態より，W z2軸回りに q2回転するので，Σ1とΣ2の関係は，
1R2 = 1R2Rot(z, q2)
=
0 1 0
0 0 1
1 0 0
C2 −S2 0
S2 C2 0
0 0 1
=
S2 C2 0
0 0 1
C2 −S2 0
(3.37)
となる．2R3については，
2R3 = I3であることと，Σ3は Σ2に対して z軸回りに q3回
転するから，次式で表される．
2R3 = 2R3Rot(z, q3)
=
C3 −S3 0
S3 C3 0
0 0 1
(3.38)
3RHについては，Σ3とΣHとの関係は不変であり，図 3.2に示すとおり，W xHがW y3と，
W yH がW z3と，
W zH がW x3とそれぞれ一致していることから，
3RH = 3RH
=
0 0 1
1 0 0
0 1 0
(3.39)
と表される．以上の順準より，
W R1(q1) =
C1 −S1 0
S1 C1 0
0 0 1
(3.40)
W R2(q1, q2) = W R1(q1)1R2(q2)
=
C1 −S1 0
S1 C1 0
0 0 1
S2 C2 0
0 0 1
C2 −S2 0
=
C1S2 C1C2 −S1
S1S2 S1C2 C1
C2 −S2 0
(3.41)
17
W R3(q1, q2, q3) = W R2(q1, q2)2R3(q3)
=
C1S2 C1C2 −S1
S1S2 S1C2 C1
C2 −S2 0
C3 −S3 0
S3 C3 0
0 0 1
=
C1S2C3 + C1C2S3 −C1S2S3 + C1C2C3 −S1
S1S2C3 + S1C2S3 −S1S2S3 + S1C2C3 C1
C2C3 − S2S3 −C2S3 − S2C3 0
=
C1S23 C1C23 −S1
S1S23 S1C23 C1
C23 −S23 0
(3.42)
W RH(q1, q2, q3) = W R3(q1, q2, q3)3RH
=
C1S23 C1C23 −S1
S1S23 S1C23 C1
C23 −S23 0
0 0 1
1 0 0
0 1 0
=
C1C23 −S1 C1S23
S1C23 C1 S1S23
−S23 0 C23
(3.43)
と求めることができる．ただし，C23 = cos(q2 + q3)，S23 = sin(q2 + q3)である．
3.5 ハンド位置の記述
ΣW で表わされたハンドの位置ベクトルW rH は，qの関数であり，
W rH(q) = W rW,1 +W r1,2(q1) + · · ·+W rn−1,n(q1, q2, · · · , qn−1) +W rn,H(q1, q2, · · · , qn) (3.44)
と表わされる．ΣW の原点からΣ1の原点へのベクトルW rW,1は q1に無関係であ
り,常に定数となる. 注意すべきことは，ベクトルの加減算は，それを表わしてい
る座標系が同じであるとき意味を持つことである．上式では，すべてのベクトル
がΣW を規準に表わされている．ここで，姿勢変換行列W Ri(q1, q2, · · · , qi)を用
いて W rW,1,W r1,2(q1), · · · を求めてみる．
W rW,1 = W rW,1 （一定）
W r1,2(q1) = W R1(q1)1r1,2
W r2,3(q1, q2) = W R2(q1, q2)2r2,3 = W R1(q1)
1R2(q2)2r2,3
· · ·W rn,H(q1, q2, · · · , qn) = W Rn(q1, q2, · · · , qn) nrn,H
= W R1(q1)1R2(q2) · · · n−1Rn(qn) nrn,H (3.45)
18
以上を用いると，式 (3.44)は，
W rH(q) = W rW,1 + W R1(q1)1r1,2 +W R2(q1, q2)
2r2,3
+ · · · + W Rn(q1, q2, · · · , qn) nrn,H
= W rW,1 + W R1(q1)1r1,2 + W R1(q1)
1R2(q2)2r2,3
+ · · · + W R1(q1)1R2(q2) · · · n−1Rn(qn) nrn,H (3.46)
と表わされる．
例３-３図 3.9に示す 3リンクマニピュレータのハンドの位置ベクトル W rH(q1, q2, q3)
を求めてみよう．ΣW とΣ1の原点間の距離は零であるから，
W rW,1 = [0, 0, 0]T (3.47)
リンク 1の長さは l1であるから1r1,2 = [0, 0, l1]
T であり，これより
W r1,2(q1) =
C1 −S1 0
S1 C1 0
0 0 1
0
0
l1
=
0
0
l1
(3.48)
となり，W r1,2は q1の関数とならず定数ベクトルとなる．これはΣ2の原点がリ
ンク 1の回転ベクトル W z1の延長線上にあることから当然である．
次に，W r2,3(q1, q2)を求めてみよう．2r2,3 = [l2, 0, 0]T であることから，
W r2,3(q1, q2) = W R2(q1, q2)2r2,3
=
C1S2 C1C2 −S1
S1S2 S1C2 C1
C2 −S2 0
l20
0
=
l2C1S2
l2S1S2
l2C2
(3.49)
と表わすことができる．W r3,H については，
W r3,H(q1, q2, q3) = W R3(q1, q2, q3)3r3,H
=
C1S23 C1C23 −S1
S1S23 S1C23 C1
C23 −S23 0
l30
0
=
l3C1S23
l3S1S23
l3C23
(3.50)
となる．
以上をまとめると，
W rH(q) =
0
0
l1
+
l2C1S2
l2S1S2
l2C2
+
l3C1S23
l3S1S23
l3C23
=
l2C1S2 + l3C1S23
l2S1S2 + l3S1S23
l1 + l2C2 + l3C23
(3.51)
が得られる．
19
3.6 同次変換行列
ハンドに取り付けた座標系ΣH の姿勢W RH(q)は，式 (3.31)で表され，ハン
ドの位置ベクトル W rH(q)は，式 (3.46)で表わされることがわかった．ここで，
位置と姿勢を同時に表わすことを考える．リンク 1の姿勢 W R1(q1)とリンク１
の座標つまりΣ1の原点の位置ベクトルW r1をまとめて，
W T 1(q1) =
[W R1(q1)
W r1
0 0 0 1
](3.52)
と表わす．この座標変換行列と位置ベクトルを並べた上の行列は同次変換行列と
呼ばれている．リンク 2について，W R2とW r2は，
W R2 = W R1(q1)1R2(q2)
W r2 = W rw,1 + W R1(q1)1r1,2
}(3.53)
と 2つの式を用いて表わしていたが，これを同次変換行列で表わせば，
W T 2(q1, q2) =
[W R2(q1, q2)
W r2
0 0 0 1
]
=
[W R1(q1)
1R2(q2)W rW,1 + W R1(q1)
1r1,2
0 0 0 1
]
=
[W R1(q1)
W rW,1
0 0 0 1
][1R2(q2)
1r1,2
0 0 0 1
]
= W T 1(q1)1T 2(q2) (3.54)
と分解することができる．3リンク目について考えてみよう．
W R3 = W R1(q1)1R2(q2)
2R3(q3)W r3 = W rW,1 + W R1(q1)
1r1,2 + W R1(q1)1R2(q2)
2r2,3
}(3.55)
は，
W T 3(q1, q2, q3) =
[W R3(q1, q2, q3)
W r3
0 0 0 1
]
=
[W R1(q1)
1R2(q2)2R3(q3)
W r1 + W R1(q1)1r1,2 + W R1(q1)
1R2(q2)2r2,3
0 0 0 1
]
=
[W R1(q1)
W rW,1
0 0 0 1
][1R2(q2)
1r1,2
0 0 0 1
][2R3(q3)
2r2,3
0 0 0 1
]
= W T 1(q1)1T 2(q2)
2T 3(q3) (3.56)
と表わされる．W T 3としてまとめられたリンクの位置・姿勢がW T 1,
1T 2,2T 3
の積として簡潔にまとめられていることがわかる．つまり同次変換行列は，位置
と姿勢の変化を同時に表現することができる便利な表現方法なのである．この手
法のメリットは，3リンクマニピュレータではあまり意識できないが，6リンク
以上のマニピュレータの運動学計算を行うときは，その単純な表現は，運動学関
係の整理に役立つ．
20
例３-４例 3-2と例 3-3の結果を同次変換行列を用いて同時に求めてみよう．
W T H(q1, q2, q3) = W T 1(q1)1T 2(q2)
2T 3(q3)3T H
=
C1 −S1 0 0
S1 C1 0 0
0 0 1 0
0 0 0 1
S2 C2 0 0
0 0 1 0
C2 −S2 0 l10 0 0 1
C3 −S3 0 l2S3 C3 0 0
0 0 1 0
0 0 0 1
0 0 1 l31 0 0 0
0 1 0 0
0 0 0 1
=
C1C23 −S1 C1S23 l2C1S2 + l3C1S23
S1C23 C1 S1S23 l2S1S2 + l3S1S23
−S23 0 C23 l1 + l2C2 + l3C23
0 0 0 1
(3.57)
以上の結果が式 (3.43)の W RH と式 (3.51)の W rH を含んでいることから，ハ
ンドの姿勢と位置が同時に求められていることがわかる．
3.7 同次変換行列の性質
第一の性質同次変換行列の性質として第一に位置ベクトルの座標変換についてのべる．式
(3.56)より直ちに[
W R3(q1, q2, q3)W r3
0 0 0 1
]= W T 1(q1)
1T 2(q2)
[2R3(q3)
2r2,3
0 0 0 1
](3.58)
を得る．さらに上式より第 4列のみの関係を抜きを出すと，[
W r3
1
]= W T 1(q1)
1T 2(q2)
[2r2,3
1
]
= W T 2(q1, q2)
[2r2,3
1
](3.59)
が成り立っている．上式はΣW からΣ2までの位置・姿勢関係をW T 2で表わし，
さらにΣ2を規準にベクトル2r2,3が伸びたときの先端の位置ベクトルをΣW で
表現したものとなっている．同様に例 3-4より，[
W rH
1
]= W T 3(q1, q2, q3)
[3r3,H
1
](3.60)
が成り立つ．以上より同次変換行列は，座標変換と位置ベクトルの加算を同時に
行っていることがわかる．
21
第二の性質第二に同次変換行列の部分行列である座標変換行列は，式 (3.21)で表わしたよ
うに常に正則であり逆行列は常に存在する．また，同次変換行列の第 4列は，(4,4)
要素に 1があることにより，1～3列に対して常に独立であるから，同次変換行列
も常に正則である．この特徴は後に述べる逆運動学の解を得るときに役に立つ．
同次変換行列 T を
T =
[x y z r
0 0 0 1
]=
[R r
0 0 0 1
](3.61)
と定義すると，T−1は，
T−1 =
xT −rT x
yT −rT y
zT −rT z
0 0 0 1
=
−rT x
RT −rT y
−rT z
0 0 0 1
(3.62)
と表わさせる．確認のために，
T−1T =
−rT x
RT −rT y
−rT z
0 0 0 1
[R r
0 0 0 1
](3.63)
を計算してみる．ここで
RT r =
xT
yT
zT
r =
xT r
yT r
zT r
=
rT x
rT y
rT z
(3.64)
となることを考えて，式 (3.7)，(3.8)と式 (3.64)より
T−1T =
xT r − rT x
RT R yT r − rT y
zT r − rT z
0 0 0 1
= I4 (3.65)
であることがわかる．
また, TT−1 = I4の確認については, rT x, rT y, rT zがスカラーであり，xT r = rT x
であることを用いている.
22
TT−1 =
[R r
0 0 0 1
]
−rT x
RT −rT y
−rT z
0 0 0 1
=
RRT R
−rT x
−rT y
−rT z
+ r
0 0 0 1
=
I3 R
−xT r
−yT r
−zT r
+ r
0 0 0 1
=
I3 R
−xT
−yT
−zT
r + r
0 0 0 1
=
[I3 −RRT r + r
0 0 0 1
]
=
0
I3 0
0
0 0 0 1
= I4 (3.66)
第三の性質第三に同次変換行列の逆行列の意味について考えるために，同次変換行列につ
いてもう少し考えてみる．ここで，ΣW で規定された 3次元空間内に物体があり，
その物体にΣAが固定されている状態について考える．
ΣW の原点よりΣAの原点への位置ベクトルをΣW で表現したものをW rW,Aと
表わす．ΣW をW rW,Aだけ平行移動すると，その原点はΣAの原点と一致する．
この同次変換行列を Trans( W rW,A)と表記することにすると、これは，
Trans( W rW,A) =
[I3
W rW,A
0 0 0 1
](3.67)
と表わされる．
次に，移動したΣW の z軸方向に φ回転し，回転後の座標系の y軸方向回りに
θ，さらに新たな z軸回りに ψ回転したときの各座標軸がΣAと完全に一致した
場合を考える．この様子を図 3.10に示す．これらの 3つの回転を，3× 3行列行列
で表わした回転変換は，式 (3.12)～(3.14)で与えられるが，ここでは，これらを
改めて同次変換行列で定義し，それぞれ TRot(z, φ)，TRot(y, θ)，TRot(z, ψ)
23
h:
h$: U:�$h: �
図 3.10:
x
z
y�軸回りに-90度回転ー �軸回りに-90度回転
z
yx
Wz2
Wy2
Wx2
Wx1
Wz1
Wy1
��だけ並進
図1.4に記述
されている∑ と� ∑ の�関係と一致
P1
Wx1
Wz1
Wy1P1
ー
図 3.11:
と表わす．すなわち，
TRot(z, φ) =
Cφ −Sφ 0 0
Sφ Cφ 0 0
0 0 1 0
0 0 0 1
(3.68)
TRot(y, θ) =
Cθ 0 Sθ 0
0 1 0 0
−Sθ 0 Cθ 0
0 0 0 1
(3.69)
TRot(z, ψ) =
Cψ −Sψ 0 0
Sψ Cψ 0 0
0 0 1 0
0 0 0 1
(3.70)
と表される．ΣW からΣAへの位置と姿勢の変化を表現する同次変換行列をW T A
で表すと，それはW rW,A, φ, θ, ψの関数となっているからW T A(W rW,A, φ, θ, ψ)と
表わされ，
W T A(W rW,A, φ, θ, ψ) = Trans( W rW,A)TRot(z, φ)TRot(y, θ)TRot(z, ψ)(3.71)
と表される．つまり，W T Aを求めるには，ΣW からΣAへの座標系の移動を個別
に表現し，それぞれを移動の順序に従って積算していけば求められるのである．
例３-５
24
x
z
y
�軸回りに+90度回転+ �軸回りに+90度回転
z
y
x
WzH
WyH
WxHWx3
Wz3
Wy3
��だけ並進
図1.4に記述
されている∑ と� ∑ の�関係と一致
P3
Wx3
Wz3Wy3
P3
+
図 3.12:
例 3-4で求めた W T H(q1, q2, q3)を上の考え方でもう一度考えてみる．図 3.9より
W T 1(q1) = TRot(z, q1) =
C1 −S1 0 0
S1 C1 0 0
0 0 1 0
0 0 0 1
(3.72)
である．1T 2(q2)は，q2 = 0のとき図 3.11に示す順序でΣ1がΣ2に一致するから
式 (3.68)～(3.70)を用いると，
1T 2(q2) = Trans(z, l1) TRot(x,−90◦) TRot(z,−90◦) TRot(z, q2)
=
1 0 0 0
0 1 0 0
0 0 1 l10 0 0 1
1 0 0 0
0 C−90 −S−90 0
0 S−90 C−90 0
0 0 0 1
C−90 −S−90 0 0
S−90 C−90 0 0
0 0 1 0
0 0 0 1
C2 −S2 0 0
S2 C2 0 0
0 0 1 0
0 0 0 1
=
1 0 0 0
0 1 0 0
0 0 1 l10 0 0 1
1 0 0 0
0 0 1 0
0 −1 0 0
0 0 0 1
0 1 0 0
−1 0 0 0
0 0 1 0
0 0 0 1
C2 −S2 0 0
S2 C2 0 0
0 0 1 0
0 0 0 1
=
S2 C2 0 0
0 0 1 0
C2 −S2 0 l10 0 0 1
(3.73)
となり例 3-4の中に記述されている 1T 2(q2)と一致する．次に図 3.9より z2の原
点をその x軸方向に l2移動すると，Σ2とΣ3の座標軸の初期状態は一致すること
25
から，
2T 3(q3) = Trans(x, l2) TRot(z, q3)
=
1 0 0 l20 1 0 0
0 0 1 0
0 0 0 1
C3 −S3 0 0
S3 C3 0 0
0 0 1 0
0 0 0 1
=
C3 −S3 0 l2S3 C3 0 0
0 0 1 0
0 0 0 1
(3.74)
となり，これも例 3-4の中に記述されている 2T 3(q3)と一致する．
最後に 3T H については，図 3.12に示すとおり，
3T H = Trans(x, l3) TRot(y, 90◦) TRot(z, 90◦)
=
1 0 0 l30 1 0 0
0 0 1 0
0 0 0 1
C90 0 S90 0
0 1 0 0
−S90 0 C90 0
0 0 0 1
C90 −S90 0 0
S90 C90 0 0
0 0 1 0
0 0 0 1
=
1 0 0 l30 1 0 0
0 0 1 0
0 0 0 1
0 0 1 0
0 1 0 0
−1 0 0 0
0 0 0 1
0 −1 0 0
1 0 0 0
0 0 1 0
0 0 0 1
=
0 0 1 l31 0 0 0
0 1 0 0
0 0 0 1
(3.75)
となり，これも一致する．従ってW T H(q1, q2, q3)も一致する．以上の確認により，W T H が座標系の並進移動と回転移動を順次積算していくことで求められること
がわかる．
次にW T A(W rW,A, φ, θ, ψ)の逆行列の意味について考える．W T Aは常に正則で
あることから，
W T−1A (W rW,A, φ, θ, ψ)
=[
Trans(W rW,A) TRot(z, φ) TRot(y, θ) TRot(z, ψ)]−1
= TRot−1(z, ψ)TRot−1(y, θ)TRot−1(z, φ)Trans−1(W rW,A) (3.76)
となる．ここで，TRot−1(z, ψ)について考えてみる．式 (3.62)，(3.72)より
TRot−1(z, ψ) =
Cψ Sψ 0 0
−Sψ Cψ 0 0
0 0 1 0
0 0 0 1
=
C−ψ −S−ψ 0 0
S−ψ C−ψ 0 0
0 0 1 0
0 0 0 1
= TRot(z,−ψ) (3.77)
26
となり，回転に関する同次変換行列の逆行列は，回転方向を逆にした変換である
ことがわかる．同様に
TRot−1(y, θ) = TRot(y,−θ) (3.78)
TRot−1(z, φ) = TRot(z,−φ) (3.79)
となる．次に Trans−1(W rW,A)については，式 (3.62)より
Trans−1(W rW,A) =
[I3
W rW,A
0 0 0 1
]−1
=
1 0 0 −W rTW,A[ 1 0 0]T
0 1 0 −W rTW,A[ 0 1 0]T
0 0 1 −W rTW,A[ 0 0 1]T
0 0 0 1
=
[I3 −W rW,A
0 0 0 1
]
= Trans(−W rW,A) (3.80)
となることから，並進についての逆行列も逆方向の並進を意味することがわかる．
以上より式 (3.76)は，
W T−1A (W rW,A, φ, θ, ψ)
= TRot(z,−ψ)TRot(y,−θ)TRot(z,−φ)Trans(−W rW,A) (3.81)
と表わされ，同次変換行列が座標の移動を意味するということから，上の式 (3.81)
は∑
Aから∑
W への逆の変換を意味している．以上のことから
W T−1A = AT W (3.82)
が成り立つことがわかる．さらに上式より
W T AAT W = I4 (3.83)
となることがわかる．
3.8 逆運動学の解
3リンクマニピュレータの順運動学関係は，式 (3.51)より
W rH(q) =
l2C1S2 + l3C1C23
l2S1S2 + l3S1S23
l1 − l2C2 − l3S23
(3.84)
と求められている．逆運動学はW rHとしてある目標位置W rHd = [W xHd,
W yHd,W zHd]
T
が指定されたとき，これを実現する q = [q1, q2, q3]T を求めることである．このと
き上式より直接逆関数を求めることは見通しがつきにくいという意味で得策では
なく，次のような同次変換行列を用いた方法がより解を得る目処を立てやすい．
27
上式の問題は，式 (3.60)より[
W rHd
1
]= W T 1(q1)
1T 2(q2)2T 3(q3)
[3r3,H
1
](3.85)
と表わすことができる．ここで W rHd，3rH は定数ベクトルである．同次変換行
列は正則であることより上式は
W T−11 (q1)
[W rHd
1
]= 1T 2(q2)
2T 3(q3)
[3r3,H
1
](3.86)
であり，式 (3.62)より
W T−11 (q1) =
C1 S1 0 0
−S1 C1 0 0
0 0 1 0
0 0 0 1
(3.87)
が求められ，さらに式 (3.73)，(3.74)より
1T 2(q2)2T 3(q3) =
S2 C2 0 0
0 0 1 0
C2 −S2 0 l10 0 0 1
C3 −S3 0 l2S3 C3 0 0
0 0 1 0
0 0 0 1
=
S2C3 + C2S3 −S2S3 + C2C3 0 l2S2
0 0 1 0
C2C3 − S2S3 −C2S3 − S2C3 0 l1 + l2C2
0 0 0 1
=
S23 C23 0 l2S2
0 0 1 0
C23 −S23 0 l1 + l2C2
0 0 0 1
(3.88)
と求めることができる．さらに 3r3,H =[
l3 0 0]T
であることより，式 (3.86)
は，
C1 S1 0 0
−S1 C1 0 0
0 0 1 0
0 0 0 1
W xHd
W yHd
W zHd
1
=
S23 C23 0 l2S2
0 0 1 0
C23 −S23 0 l1 + l2C2
0 0 0 1
l30
0
1
(3.89)
となる．上式は４つの方程式を意味するが，行列の第４行とベクトルの積は 1 = 1
となり意味を持たない．その他の関係は，
W xHdC1 + W yHdS1 = l3S23 + l2S2 (3.90)
−W xHdS1 + W yHdC1 = 0 (3.91)
28
D � �� E ! � D ! �� E ! �
D ! �� E � �D � �� E � �
DUJ�E� MD�D
E
図 3.13:
W zHd = l1 + l2C2 + l3C23 (3.92)
となる．まず q1の解を求めよう．式 (3.91)より
tan q1 =W yHd
W xHd
(3.93)
となり，
q1 = tan−1
(W yHd
W xHd
)(3.94)
ここで，y = tan θという関数について考える．tan θは周期 πの関数であるため，
逆関数
θ = tan−1 y (3.95)
は，−π ≤ θ < πの間で解を２つ持ち一意に定まらない．従って式 (3.95)の解も
一意に定まらず都合が悪い．この点を解決するために tan−1を一意に与える関数
atan2が考えられた．atan2は，
atan2(b, a) = arg(a + jb) (3.96)
と定義され，jは虚数単位，argは複素数の偏角を表す．この関数のアイデアは，
a, bそれぞれの符号を残したまま逆関数を作ることで図 3.13に示すように解が存
在する領域を第１象限から第４象限の中で決定し，逆関数の任意性を取り除いた
ことである．ただし，a = b = 0となるときは tan−1の値は一意に定まらず，この
場合のみ複数の解が存在することになる．ここで注意したいことは，atan2(b, a)
の bは y座標を表し，aは x座標を表すことである．通常座標を，(x, y)と表記す
る慣習とは異なっている．関数 atan2は，プログラミングでよく使われるC言語
でもサポートされている．
さて，atan2を用いて q1の解を一意に解くために，式 (3.93)を
tan q1 =+ W yHd
+ W xHd
(3.97)
29
D ! �
E � �D ! �
E ! �
E � �D � �
E ! �D � �
{
{�b{b{
�t
WDQt
図 3.14:
[: U[+G
: U\+G
h:図 3.15:
と表現する．atan2を用いると q1は，
q1 = atan2(+ W yHd, +W xHd) (3.98)
と求められる．
次に，q1が決定されたので，式 (3.90)左辺はある定数となり
W xHdC1 + W yHdS1 = k1 (3.99)
とおく．式 (3.90)の両辺を 2乗したものと，式 (3.92)の右辺の l1を左辺に移動し
たときの両辺を 2乗して，それぞれ加えると，
k12 + ( W zHd − l1)
2 = l22 + l3
2 + 2l2l3C3 (3.100)
が求められる．ここで，
k12 + ( W zHd − l1)
2 = k2 (3.101)
とおくと，
2l2l3C3 = k2 − l22 − l3
2 (3.102)
30
T�N� b O�� b O��
T�O��O�� b �N� b O�� b O����
�O�O�
図 3.16:
となる．さらにピタゴラスの定理を用いる．
2l2l3S3 =√
4l22l23 − (k2 − l22 − l23)
2 (3.103)
が得られることから，
q3 = atan2(
√4l2
2l32 − (k2 − l2
2 − l32), k2 − l2
2 − l32) (3.104)
より q3が定まる．
q2については，式 (3.90)～(3.92)では見通しが悪いので，式 (3.85)より
W T−12 (q1, q2)
[W rHd
1
]= 2T 3(q3)
[3rH
1
](3.105)
を求めてみる．式 (3.57)より，
W T 2(q1, q2) =
C1S2 C1C2 −S1 0
S1S2 S1C2 C1 0
C2 −S2 0 l10 0 0 1
(3.106)
W T 2(q1, q2)を求め，式 (3.86)を用いることにより，式 (3.105)は，
C1S2 S1S2 C2 −l1C2
C1C2 S1C2 −S2 l1S2
−S1 C1 0 0
0 0 0 1
W xHd
W yHd
W zHd
1
=
C3 −S3 0 l2S3 C3 0 0
0 0 1 0
0 0 0 1
l30
0
1
(3.107)
これより，
W xHdC1S2 +W yHdS1S2 +W zHdC2 − l1C2 = l3C3 + l2 (3.108)
W xHdC1C2 +W yHdS1C2 −W zHdS2 + l1S2 = l3S3 (3.109)
−W xHdS1 +W yHdC1 = 0 (3.110)
が求められる．式 (3.110)は，式 (3.91)と一致しているので新たな情報ではない．
式 (3.108)，(3.109)は，式 (3.99)の k1を用いることで，
k1S2 − (l1 −W zHd)C2 = l2 + l3C3 (3.111)
k1C2 + (l1 −W zHd)S2 = l3S3 (3.112)
31
h:
\
[
h:[
]
O
OO
T�
T�
T�
T�
S�O図 3.17:
これをまとめると，[
k1 −(l1 −W zHd)
l1 −W zHd k1
][S2
C2
]=
[l2 + l3C3
l3S3
]
(3.113)
となり，よって，[
S2
C2
]=
1
k21 + (l1 −W zHd)2
[k1 −(l1 −W zHd)
(l1 −W zHd) k1
][l2 + l3C3
l3S3
](3.114)
となる，S2，C2の解は，
{k21 + (l1 −W zHd)
2}S2 = k1(l2 + l3C3) − (l1 −W zHd)l3S3 (3.115)
{k21 + (l1 −W zHd)
2}C2 = k1l3S3 + (l1 −W zHd)(l2 + l3C3) (3.116)
と求められる．これより
q2 = atan2(k1(l2 + l3C3) − (l1 −W zHd)l3S3,
k1l3S3 + (l1 −W zHd)(l2 + l3C3)) (3.117)
となる．
例３-６逆運動学の解，式 (3.98),(3.104),(3.117)が正しく解かれていることを確認しよ
う．ここで，−π ≤ q1, q2, q3 ≤ πと l1 = l2 = l3 = lを仮定し W rHdを図 3.17に示
すように W rHd = [√
2l, 0, l] と与える．このときの解は q = [0, π/4, π/2] または，
[0, 3π/4,−π/2] となることは容易に解かる．
32
まず式 (3.98)より q1を求めよう．
q1 = atan2(0,√
2l)
= 0,−π (3.118)
２つの解が存在するが−πは W rHdを実現するとき無駄な動き，つまり図 3.4に
おいての方向をロボットの正面と考えるとき，反転して背を向けた状態からハン
ドを降ろす形で，ここでは除いて考える．よって
q1 = 0 (3.119)
と求める．次に q3を求める．式 (3.99)より k1 =W xHd =√
2l，式 (3.101)より
k2 = k21 = 2l2と求められるから，式 (3.104)より q2は
q3 = atan2(2l2, 0)
= ±π
2(3.120)
と求まる．この２つの解は，図 3.17より上腕姿勢と下腕姿勢の２つの解に対応し
ており，２つとも意味を持つ．
q2については q3 = π/2のとき，式 (3.95)より
q2 = atan2(√
2l2,√
2l2) =π
4(3.121)
また，q3 = −π/2のとき，
q2 = atan2(−√
2l2,√
2l2) =3π
4(3.122)
となり，図 3.17の上腕，下腕姿勢に対応していることがわかる.
33
第 4章
3リンクマニピュレータの
ラグランジュ解法
4.1 運動エネルギー
4.1.1 並進運動エネルギー図 4.1に示すように，質量miのリンク iの重心が速度 vgi(g:重心)で運動してい
る状態を考える．このときの並進運動エネルギーは
Tti =1
2mivgi
2 (4.1)
と表わされる (t:並進 translation)．ところで，ロボットの場合，vgiの方向と大き
さはロボットの運動により時々刻々変化する．したがって，vgiをロボットの運動
と関連付けて記述する必要がある．まず，ΣW で表わされたベクトル量として，
W vgi = [W vgxi,W vgyi,
W vgzi]T (4.2)
を定義すると
∣∣ W vgi
∣∣ = vgi =
√W vgxi
2 + W vgyi2 + W vgzi
2 (4.3)
と表わされる．図 4.2にW vgiをΣW で表わしたときのW vgiの要素の意味を示す．
vgi2 = W vgxi
2+ W vgyi
2+ W vgzi
2であることと，vgi
2は vgi2 = W vgi
T W vgi とベク
トルの内積によって表わされることから，式 (4.1)は，
Tti =1
2mi
W vgiT W vgi (4.4)
と表わすことができる．さらに，リンク iの重心の位置ベクトルをW rgi = [W xgi,
W ygi,W zgi]
Tとすると，図4.3に示すようにW vgi =d
dtW rgi =W rgi = [W xgi,
W ygi,W zgi]
T と表わされることから，式 (4.4)は
Tti =1
2mi
W rgiT W rgi (4.5)
と書き改められる．以上により，第 iリンクの並進運動エネルギー Ttiが，ΣW を
規準にしたリンク iの重心の速度ベクトルW rgiを用いて求められることが分かっ
たが，W rgiをどのようにして求めるかについて次に述べる．
34
P
�æ�ï�«LY !
図 4.1:
h
[
\
]
Y !"
Y #"
Y $"Y
"
!"
�æ�ï�«
図 4.2:
4.1.2 重心の速度ベクトル第 iリンクにはリンク座標系Σiが固定されている．Σiを規準にしたリンク iの
重心の位置ベクトルを iri,giと表わす．重心はΣi上では動かないのでiri,giは定数
ベクトルとなる．
Σiの原点の位置ベクトルをW riと表わすとき，
W rgiは式 (3.5)より，
W rgi =W ri +W Riiri,gi (4.6)
と表わされる．この関係を図 4.4に示す．上式を時間で微分すると
W rgi =W ri +W Riiri,gi +W Ri
iri,gi (4.7)
となる．iri,giが定数ベクトルであることにより，iri,gi = 0となることから
W rgi =W ri +W Riiri,gi (4.8)
と簡単になる．右辺の W riはリンク iの原点の並進運動を表わし，W RiはΣiの
回転運動を表わす．ここで，W Riについて考えてみる．W Riは式 (3.6)で表わさ
れるから，
W Ri = [W xi,W yi,
W zi] (4.9)
となる．リンク iの回転角速度ベクトルを W ωiと表わすとき，W ωiによって発
生するΣiの各座標軸単位ベクトルの先端に発生する速度すなわちW xi,
W yi,W zi
は，
W xi = W ωi×W xi (4.10)
W yi = W ωi×W yi (4.11)
W zi = W ωi×W zi (4.12)
35
h
U!"
Y!"
BU!"
�æ�ï�«
図 4.3:
h
[!
]!
\!
h
�æ�ï�«
Oœ
U!
U!"
"
U"#!"
図 4.4:
と表わされるから，式 (4.7)は，
W rgi =W ri + [W ωi×W xi,
W ωi×W yi,
W ωi×W zi]
iri,gi (4.13)
となる．付録１の説明より，上式は，
W rgi = W ri +W ωi× [W xi,W yi,
W zi]iri,gi
= W ri +W ωi×W Ri
iri,gi (4.14)
と変形され，iri,gi はリンク毎に測定することができるから既知である．したがっ
て，リンク iの原点の速度ベクトルW riとリンク iの回転角速度ベクトルW ωiが
明らかになれば，W rgiが求まる．
4.1.3 リンク回転角速度ベクトルリンク iの回転角速度ベクトルW ωiの算出方法について述べる．Σi(i = 1, 2, · · · , n)
の回転運動はすべてΣiの z軸回りとなるようにΣiを取りつけた．このメリットは，
Σiを規準にした回転方向単位ベクトルがW ziで表されることである．1, 2, · · · , i
リンクは W zi方向にそれぞれの角速度 q1, q2, · · · , qnで回転するから，W ωiは，
W ωi =W z1q1 +W z2q2 + · · · +W ziqi (4.15)
と表わされる．Σiを規準にW ziの方向を表わすと，iに関わらず [0, 0, 1]Tである．こ
の単純さを得るためにΣiのz軸を回転軸と一致させて設定したことを思い出して頂
きたい．[0, 0, qi]T = [0, 0, 1]T qi = iqiであり，
W zi qi = [ W xi,W yi,
W zi][0, 0, 1]T qi
36
W
i
�iW
x
y
zW W
W
i i
i
zWi
yWi
xWi
図 4.5:
であるから，上式は，
W ωi =[
W x1,W y1,
W z1
]
0
0
1
q1
+[
W x2,W y2,
W z2
]
0
0
1
q2
+ · · · +[
W xi,W yi,
W zi
]
0
0
1
qi
= W R11q1 +W R2
2q2 + · · · +W Ri−1i−1qi−1 +W Ri
iqi (4.16)
と表わされる．同じ考え方で，
W ωi−1 = W R11q1 + W R2
2q2 + · · · + W Ri−1i−1qi−1 (4.17)
と表わされるから，式 (4.16)，(4.17)より，W ωiが
W ωi =W ωi−1 +W Riiqi (4.18)
と漸化式で表わされることがわかる．
例４-１図 3.9に示す３リンクマニピュレータについて，W ω1,
W ω2,W ω3を求めてみ
よう．式 (4.18)で i = 1とすると，
W ω1 = W ω0 + W R11q1 (4.19)
となる．W ω0はW ωW を意味するので
W ω0 = 0であるから，式 (4.18)より
W ω1 =
C1 −S1 0
S1 C1 0
0 0 1
0
0
q1
=
0
0
q1
(4.20)
37
W
i-1
i
zWi
xWi
yWi
zWi-1
xWi-1
yWi-1W
i-1
W
i
ÂË�i
ÂË�i-1
r
r
図 4.6:
と求まる．i = 2のとき，式 (4.18)より
W ω2 = W ω1 + W R22q2
=
0
0
q1
+
C1S2 C1C2 −S1
S1S2 S1C2 C1
C2 −S2 0
0
0
q2
=
−S1q2
C1q2
q1
(4.21)
i = 3のとき，式 (4.18)より
W ω3 =
−S1q2
C1q2
q1
+
C1S23 C1C23 −S1
S1S23 S1C23 C1
C23 −S23 0
0
0
q3
=
−S1q2 − S1q3
C1q2 + C1q3
q1
(4.22)
と求めることができる．
4.1.4 リンク速度ベクトル図 4.6に示す，Σiの位置ベクトル
W riは式 (3.5)より，
W ri =W ri−1 +W Ri−1i−1ri−1,i (4.23)
と表わされる．ここで i−1ri−1,iはΣi−1を規準に表わしたΣi−1の原点からΣiの原
点までの位置ベクトルであり，定数ベクトルとなる．
このため式 (4.23)の時間微分は，
W ri = W ri−1 + W Ri−1i−1ri−1,i
= W ri−1 + W ωi−1 × {W Ri−1i−1ri−1,i} (4.24)
38
となり，漸化式で表わされる．W ωi−1は式 (4.18)の漸化式により求められるか
ら，i=1のときの右辺 W r0が定まれば，順にW riを求めることができる．ここ
で W r0はW rW を意味し，ΣW はΣW に対して動かないことより
W r0 = 0とな
る．
例４-２例 4-1と同様に，W r1,
W r2,W r3を求めてみよう．
i = 1のとき，式 (4.24)より，
W r1 = W r0 + W ω0 × {W R00r0,1} (4.25)
ここで，添え字の 0は ΣW を意味するから，W rW = 0, W ωW = 0, W RW =
I3,W rW,1 = 0 (ΣWとΣ1の原点が一致しているから)となり
W r1 = 0 (4.26)
となる．i = 2のとき
W r2 = W r1 + W ω1 × {W R11r1,2}
=
0
0
q1
×
C1 −S1 0
S1 C1 0
0 0 1
0
0
l1
=
0
0
q1
×
0
0
l1
=
0
0
0
(4.27)
となる．Σ2の原点が動かないことは図 4.7よりΣ2の原点がリンク１の駆動軸上
にあることから理解される．
i = 3のとき
W r3 = W r2 + W ω2 × {W R22r2,3}
=
−S1q2
C1q2
q1
×
C1S2 C1C2 −S1
S1S2 S1C2 C1
C2 −S2 0
l20
0
=
−S1q2
C1q2
q1
×
C1S2l2S1S2l2C2l2
=
C1C2l2q2 − S1S2l2q1
C1S2l2q1 + S1C2l2q2
−S2l2q2
(4.28)
と求められる．
以上により，W ri,W ωi,
W Riが定まるから式 (4.24)により W riが求められ，式
(4.14)より W rgiが求められ，式 (4.5)により第 iリンクの並進運動エネルギー Tti
が求められる．
39
g1 g1
g1
g1
zW
xW
yWg1l
yW
xW
zW�|lÂË�Í
H�
図 4.7:
例４-３Tti(i = 1, 2, 3)と Ttを求めてみよう．
W rg1は，式 (4.8)より
W rg1 = W r1 + W ω1 × {W R11r1,g1} (4.29)
であり，図 4.7より 1r1,g1 = [0, 0, lg1]T と，式 (4.26)，(4.20)より，
W rg1 =
0
0
q1
×
C1 −S1 0
S1 C1 0
0 0 1
0
0
lg1
= 0 (4.30)
次に， 2r2,g2 = [lg2, 0, 0]T と，式 (4.21)，(4.27)より
W rg2 = W r2 + W ω2 × {W R22r2,g2}
=
−S1q2
C1q2
q1
×
C1S2 C1C2 −S1
S1S2 S1C2 C1
C2 −S2 0
lg2
0
0
=
−S1q2
C1q2
q1
×
C1S2lg2
S1S2lg2
C2lg2
=
C1C2lg2q2 − S1S2lg2q1
C1S2lg2q1 + S1C2lg2q2
−S2lg2q2
(4.31)
ここで W r2,g2はΣW で表したΣ2の原点からリンク 2の重心までの位置ベクトル
40
である．また， 3r3,g3 = [lg3, 0, 0]T と，式 (4.22)，(4.28)より,
W rg3 = W r3 + W ω3 × {W R33r3,g3}
=
C1C2l2q2 − S1S2l2q1
C1S2l2q1 + S1C2l2q2
−S2l2q2
+
−(q2 + q3)S1
(q2 + q3)C1
q1
×
C1S23 C1C23 −S1
S1S23 S1C23 C1
C23 −S23 0
lg3
0
0
=
C1C2l2q2 − S1S2l2q1
C1S2l2q1 + S1C2l2q2
−S2l2q2
+
−(q2 + q3)S1
(q2 + q3)C1
q1
×
C1S23lg3
S1S23lg3
C23lg3
=
C1C2l2q2 − S1S2l2q1
C1S2l2q1 + S1C2l2q2
−S2l2q2
+
(q2 + q3)C1C23lg3 − q1S1S23lg3
q1C1S23lg3 + (q2 + q3)S1C23lg3
−(q2 + q3)S21S23lg3 − (q2 + q3)C
21S23lg3
=
−S1S2l2q1 − S1S23lg3q1 + C1C2l2q2 + (q2 + q3)C1C23lg3
C1S2l2q1 + C1S23lg3q1 + S1C2l2q2 + (q2 + q3)S1C23lg3
−S2l2q2 − (q2 + q3)S23lg3
(4.32)
と求まる．
Tt1, Tt2, Tt3はそれぞれ
Tt1 =1
2m1
W rTg1
W rg1 = 0 (4.33)
Tt2 =1
2m2
W rTg2
W rg2
=1
2m2((C1C2lg2q2 − S1S2lg2q1)
2 + (C1S2lg2q1 + S1C2lg2q2)2 + S2
2 l2g2q
22)
=1
2m2(S
22 l
2g2q
21 + C2
2 l2g2q
22 + S2
2 l2g2q
22)
=1
2m2(S
22 l
2g2q
21 + l2g2q
22) (4.34)
Tt3 =1
2m3
W rTg3
W rg3 (4.35)
の計算については，式が長くなるので，W r3 = [W xg3,W yg3,
W zg3]T を用いて，ま
ず式 (4.34)の x，y，z成分毎に計算する．
W x2g3 = {−S1S2l2q1 − S1S23lg3q1 + C1C2l2q2 + (q2 + q3)C1C23lg3}2
= S21S
22 l
22q
21 + S2
1S223l
2g3q
21 + C2
1C22 l
22q
22 + (q2 + q3)
2C21C
223l
2g3
+2S21S2S23l2lg3q
21 − 2S1C1C2S23lg3l2q1q2 + 2C2
1C2C23lg3l2q2(q2 + q3)
−2S1C1S2C23l2lg3q1(q2 + q3) − 2S1C1S2C2l22q1q2
−2S1C1S23C23l2g3q1(q2 + q3) (4.36)
41
W y2g3 = {C1S2l2q1 + C1S23lg3q1 + S1C2l2q2 + (q2 + q3)S1C23lg3}2
= C21S
22 l
22q
21 + C2
1S223l
2g3q
21 + S2
1C22 l
22q
22 + (q2 + q3)
2S21C
223l
2g3
+2C21S2S23l2lg3q
21 + 2S1C1C2S23l2lg3q1q2 + 2S2
1C2C23l2lg3q2(q2 + q3)
+2S1C1S2C23l2lg3q1(q2 + q3) + 2S1C1S2C2l22q1q2
+2S1C1S23C23l2g3q1(q2 + q3) (4.37)
W z2g3 = {−S2l2q2 − (q2 + q3)S23lg3}2
= S22 l
22q
22 + (q2 + q3)
2S223l
2g3 + 2S2S23l2lg3q2(q2 + q3) (4.38)
以上より
W rTg3
W rg3 = S22 l
22q
21 + S2
23l2g3q
21 + C2
2 l22q
22 + C2
23l2g3(q2 + q3)
2 + 2S2S23l2lg3q21
+2C2C23l2lg3q2(q2 + q3) + S22 l
22q
22 + S2
23l2g3(q2 + q3)
2
+2S2S23l2lg3q2(q2 + q3)
= (S22 l
22 + 2S2S23l2lg3 + S2
23l2g3)q
21 + l22q
22 + l2g3(q2 + q3)
2
+2(C2C23 + S2S23)l2lg3q2(q2 + q3)
= (S2l2 + S23lg3)2q2
1 + l22q22 + l2g3(q2 + q3)
2
+2(C2C23 + S2S23)l2lg3q2(q2 + q3)
= (S2l2 + S23lg3)2q2
1 + l22q22 + l2g3(q2 + q3)
2 + 2C3l2lg3q2(q2 + q3) (4.39)
と求まるから，
T t3 =1
2m3
W rTg3
W rg3
=1
2m3{(S2l2 + S23lg3)
2q21 + l22q
22 + l2g3(q2 + q3)
2 + 2C3l2lg3q2(q2 + q3)} (4.40)
となる．最後に Ttは，
Tt =3∑
i=1
Tti
=1
2m2(S
22 l
2g2q
21 + l2g2q
22) +
1
2m3{(S2l2 + S23lg3)
2q21
+l22q22 + l2g3(q2 + q3)
2 + 2C3l2lg3q2(q2 + q3)} (4.41)
となる．
4.2 回転運動エネルギー
並進運動エネルギーの計算でΣW を規準座標系を規準にした各リンクの速度ベ
クトル W rgiを用いて計算したように，回転運動エネルギーについても，その角
速度ベクトル及び慣性テンソルをΣW を用いて表わす．第 iリンクの回転エネル
ギ Triは次式により与えられる．
Tri =1
2W ωT
iW I i
W ωi (4.42)
42
è
X
XXj
ÂË�j
図 4.8:
W wiについては，式 (4.18)より求められることが明らかになっているが，W I i
については次のような問題がある．まず，W I iの定義から話を始める．ある時刻
のリンク iが図 4.8に示す姿勢であったと考える．このリンクの重心へ ΣW を平
行移動した時，次式で与えられる行列がΣW を規準にした慣性テンソルという．
W I i =
W IxxiW Ixyi
W Ixzi
W IxyiW Iyyi
W Iyzi
W IxziW Iyzi
W Izzi
(4.43)
であり，行列の各要素は，
W Ixxi =
∫∫∫( W r2
yi + W r2zi)ρdxdydz (4.44)
W Iyyi =
∫∫∫( W r2
zi + W r2xi)ρdxdydz (4.45)
W Izzi =
∫∫∫( W r2
xi + W r2yi)ρdxdydz (4.46)
W Ixyi = −∫∫∫
W rxiW ryiρdxdydz (4.47)
W Iyzi = −∫∫∫
W ryiW rziρdxdydz (4.48)
W Ixzi = −∫∫∫
W rxiW rziρdxdydz (4.49)
で与えられる．W Ixxi,W Iyyi,
W Izziを慣性モーメント，W Ixyi,
W Iyzi,W Ixziの符号
を反転したものつまり式 (4.47)∼(4.49)の右辺の重積の結果を慣性乗積という．上
式の右辺の重積は，ΣW を規準に表わした第 iリンクの剛体内部の積分を意味す
る．ここで問題となるのは，図 4.9に示すように，時刻 tと t′とでは，リンク iの
位置・姿勢が変化するため，式 (4.44)∼(4.49)のΣW で表わされた三重積分の範囲
が変化してしまい，その結果慣性テンソルの各要素の値も変化してしまうことで
ある．従って，式 (4.43)∼(4.49)の定義に従ってW I iを求め，式 (4.42)により Tri
43
.cu�
.cu
ÂË�j
図 4.9:
を求めることは困難である．結論から言うと，W I iは式 (4.15)で与えられる座標
変換行列 W Riを用いて
W I i = W RiiI i
W RTi (4.50)
と求めることができる．この理由については付録 2に示している．ここで iI iは∑iと同じ方向を持ち，リンク iの重心に原点を持つ座標系
∑giを規準に現れた
慣性テンソルである．∑
giはリンク iの運動とともに動くから，式 (4.44)∼(4.49)
の積分範囲に変化はなく，iI iは定数行列になる．リンク iの運動によるW I iの変
化は，W Riにより変換されるので，∑
W を規準に直接W I iを求める必要がなく，
上の問題は解決される．特に∑
giがリンク iの慣性主軸と一致する時は，iI iは
対角行列となり，式 (4.47)∼(4.49)の計算は必要ない．
例４-４Tri(i = 1, 2, 3, )を求めた後，Tr =
∑3i=1 Triを求めてみよう．まず，
∑g1,
∑g2,
∑g3
とも各リンクの慣性主軸と一致していることを仮定し，
1I1 =
1Ixx 0 0
0 1Iyy 0
0 0 1Izz
,2 I2 =
2Ixx 0 0
0 2Iyy 0
0 0 2Izz
3I3 =
3Ixx 0 0
0 3Iyy 0
0 0 3Izz
(4.51)
とおく．
Triについて求める．
44
W I1 = W R11I1
W RT1
=
C1 −S1 0
S1 C1 0
0 0 1
1Ixx 0 0
0 1Iyy 0
0 0 1Izz
C1 −S1 0
S1 C1 0
0 0 1
=
C11Ixx −S1
1Iyy 0
S11Ixx C1
1Iyy 0
0 0 1Izz
C1 S1 0
−S1 C1 0
0 0 1
=
C21
1Ixx + S21
1Iyy C1 S11Ixx − S1 C1
1Iyy 0
C1 S11Ixx − C1 S1
1Iyy S21
1Ixx + C21
1Iyy 0
0 0 1Izz
(4.52)
より
Tr1 =1
2W ωT
1W I1
W ω1
=1
2[ 0, 0, q1]
W I1
0
0
q1
=1
21Izz q2
1 (4.53)
次に，
W I2 = W R22I2
W RT2
=
C1S2 C1C2 −S1
S1S2 S1C2 C1
C2 −S2 0
2Ixx 0 0
0 2Iyy 0
0 0 2Izz
C1S2 S1S2 C2
C1C2 S1C2 −S2
−S1 C1 0
=
C1S22Ixx C1C2
2Iyy −S12Izz
S1S22Ixx S1C2
2Iyy C12Izz
C22Ixx −S2
2Iyy 0
C1S2 S1S2 C2
C1C2 S1C2 −S2
−S1 C1 0
=
C21S
22
2Ixx + C21C
22
2Iyy + S21
2Izz
S1C1S22
2Ixx + S1C1C22
2Iyy − S1C12Izz
C1C2S22
2Ixx − C1S2C22Iyy
S1C1S22
2Ixx + S1C1C22
2Iyy − S1C12Izz
S21S
22
2Ixx + S21C
22
2Iyy + C21
2Izz
S1S2C22Ixx − S1S2C2
2Iyy
C1S2C22Ixx − C1S2C2
2Iyy
S1S2C22Ixx − S1S2C2
2Iyy
C22
2Ixx + S22
2Iyy
4
=
I2(1,1) I2(1,2) I2(1,3)
I2(2,1) I2(2,2) I2(2,3)
I2(3,1) I2(3,2) I2(3,3)
(4.54)
と求まる．こんなやっかいな例題を始めてしまったことを後悔しているがもう遅
い．実は，上のW I2を計算しなくても2I2をそのまま使って Triを求める方法も
45
あるのだが・・・．本書ではよりわかりやすくなるようにすべてのベクトル，行列
を基準座標系ΣW で表現する努力を試みている．しかし上式のW I2の計算はやっ
かいであり，次のW I3を考えると，もうやめたいくらいである． 気を取り直し
て，Tr2を求める．式 (4.21)，(4.54)より
Tr2 =1
2W wT
2W I2
W w2
=1
2
[−S1 q2, C1 q2, q1
]
I2(1,1) I2(1,2) I2(1,3)
I2(2,1) I2(2,2) I2(2,3)
I2(3,1) I2(3,2) I2(3,3)
−S1 q2
C1 q2
q1
=1
2
[−S1 q2, C1 q2, q1
]
−I2(1,1) S1 q2 + I2(1,2) C1 q2 + I2(1,3) q1
−I2(2,1) S1 q2 + I2(2,2) C1 q2 + I2(2,3) q1
−I2(3,1) S1 q2 + I2(3,2) C1 q2 + I2(3,3) q1
=1
2{ I2(1,1) S2
1 q22 − I2(1,2) S1 C1 q2
2 − I2(2,3) S1 q1 q2
−I2(2,1) S1 C1 q22 + I2(2,2) C2
1 q22 + I2(2, 3) C1 q1 q2
−I2(3,1) S1 q1 q2 + I2(3,2) C1 q1 q2 + I2(3,3) q21}
=1
2{ (C2
1 S22
2Ixx + C21 C2
22Iyy + S2
12Izz)S
21 q2
2
−(S1 C1 S22
2Ixx + S1 C1 C22
2Iyy − S1 C21 Izz)S1 C1 q2
2
−(C1 S2 C22Ixx − C1 S2 C2
2Iyy)S1 q1 q2
−(S1 C1 S22
2Ixx + S1 C1 C22
2Iyy − S1 C12Izz)S1 C1, q
22
+(S21 S2
22Ixx + S12 C2
22Iyy + C2
12Izz)C
21 q2
2
+(S1 S2 C22Ixx − S1 S2 C2
2Iyy)C1 q1 q2
−(C1 C2 S22Ixx − C1 S2 C2
2Iyy)S1 q1 q2
+(C22
2Ixx + S22
2Iyy)q21}
=1
2{ 2Izz S2
1 q22 +2 Izz C2
1 q2 + (C21
2Ixx + S22
2Iyy)q21}
=1
2{ 2Izz q2
2 + (C22
2Ixx + S22
2Iyy)q21} (4.55)
と求めることができる．
式 (4.42)に基づく方法では，Tr2の計算でも複雑になることがわかる．したがっ
て３リンク以上のマニピュレータに対しては，式 (4.42)の方法は不可能に近い．
以上より 12
W ωT3
W I3W ω3の計算は手計算で無理であると思われるので以下に
別の方法で行うことを考える．W Riを用いることにより，W ωiとΣiを規準に表
した同じベクトル iωiの関係は
W ωi = W Riiωi (4.56)
と表すことができる．また，付録 2の式 (9.12)より,
W I i = W RiiI i
W RTi (4.57)
46
が成り立つことから，式 (4.42)に式 (4.56)，(4.57)を代入すると，
Tri =1
2W ωT
iW I i
W ωi
=1
2iωT
iW R
T
iW Ri
iI iW R
T
iW Ri
iωi
=1
2iωT
iiI i
iωi (4.58)
と求めることができる．式 (4.62)は iI iが対角行列であることからより計算しや
すい．
例４-５例 4-4と同様の計算を式 (4.58)を用いて行う．
1ω1 = W RT
1W ω1
=
C1 −S1 0
S1 C1 0
0 0 1
0
0
q1
=
0
0
q1
(4.59)
より
Tr1 =1
2
[0 0 q1
]
1Ixx 0 0
0 1Iyy 0
0 0 1Izz
0
0
q1
=
1
21Izz q
21 (4.60)
なり，式 (4.53)と一致する．
2ω2 = W RT
2W ω2
=
C1S2 S1S2 C2
C1C2 S1C2 −S2
−S1 C1 0
−S1q2
C1q2
q1
=
−S1C1S2q2 + S1C1S2q2 + C2q1
−S1C1C2q2 + S1C1C2q2 − S2q1
S21 q2 + C2
1 q2
=
C2q1
−S2q1
q2
(4.61)
より
Tr2 =1
2
[C2q1, −S2q1, q2
]
2Ixx 0 0
0 2Iyy 0
0 0 2Izz
C2q1
−S2q1
q2
=1
2
[c2q1, −S2q1, q2
]
C22Ixx q1
−S22Iyy q1
2Izz q2
=1
2(C2
22Ixx q2
1 + S22
2Iyy q21 + 2Izz q
22)
=1
2{(S2
22Iyy + C2
22Ixx) q2
1 + 2Izz q22} (4.62)
47
が得られる．これは，式 (4.55)と一致していること，また式 (4.55)の計算量に比
べてより簡単に求まることが分かる．
次に Tr3を求める．3ω3は，式 (4.22)より
3ω3 = W RiT W ω3
=
C1S23 S1S23 C23
C1C23 S1C23 −S23
−S1 C1 0
−(q2 + q3)S1
(q2 + q3)C1
q1
=
−(q2 + q3)S1C1S23 + (q2 + q3)S1C1S23 + C23q1
−(q2 + q3)S1C1S23 + (q2 + q3)S1C1S23 + S23q1
(q2 + q3)S21 + (q2 + q3)C
21
=
C23q1
S23q
q2 + q3
(4.63)
従って Tr3は，
Tr3 =1
23ω3
T 3I33ω3
=1
2
[C23q1, −S23q1, q2 + q3
]
3Ixx 0 0
0 3Iyy 0
0 0 3Izz
C23q1
S23q1
q2 + q3
=1
2
[C23q1, S23q1, q2 + q3
]
3Ixx C23 q1
3Iyy S23q1
3Izz (q2 + q3)
=1
2{3Ixx C2
23 q21 + 3Iyy S2
23 q21 + 3Izz(q2 + q3)
2} (4.64)
と求まる．従って Trは，
Tr =3∑
i=1
Tri
=1
21Izz q2
1 +1
2{(2IxxC
22 + 2IyyS
22)q
21 + 2Izz q
22}
+1
2{(3IxxC
223 + 3IyyS
223)q
21 + 3Izz(q2 + q3)
2} (4.65)
となる．
最後に運動エネルギー T は，式 (4.41)と (4.65)より
T = Tt + Tr
=1
2m2(S
22 l2g2 q2
1 + l2g2 q22) +
1
2m3{(S2l2 + S23lg3)
2 q21 + l22q
22 + l2g3(q2 + q3)
2
+2C3l2lg3q2(q2 + q3)} +1
21Izz q
21 +
1
2{(2IxxC
22
+2IyyS22) q2
1 +2 Izz q22} +
1
2{(3IxxC
223 + 3IyyS
223)q
21 +3 Izz(q2 + q3)
2} (4.66)
と求められる．
48
4.3.1 運動方程式の導出
最後に運動方程式を求める．リンク 1の運動については，式 (4.66)より
∂T
∂q1
= m2S22 l
2g2q1 + m3(S2l2 + S23lg3)
2q1 + 1Izz q1
+( 2IxxC22 + 2IyyS
22)q1 + ( 3IxxC
223 + 3IyyS
223)q1 (4.73)
d
dt
(∂T
∂q1
)= {m2S
22 l
2g2 + m3(S2l2 + S23lg3)
2
+ 1Izz + 2IxxC22 + 2IyyS
22 + 3IxxC
223 + 3IyyS
223}q1
+2m2S2C2l2g2q1q2
+2m3(S2l2 + S23lg3){C2l2q2 + C23lg3(q2 + q3)}q1 − 2C2S22Ixxq1q2
+2S2C22Iyy q1q2 − 2C23S23
3Ixx(q2 + q3)q1 + 2S23C233Iyy(q2 + q3)q1
= {m2S22 l
2g2 + m3(S2l2 + S23lg3)
2 + 1Izz + 2IxxC22 + 2IyyS
22
+ 3IxxC223 + 3IyyS
223}q1
+{2m2S2C2l2g2 + 2m3(S2l2 + S23lg3)(C2l2 + C23lg3)
−2C2S22Ixx + 2S2C2
2Iyy − 2S23C233Ixx + 2S23C23
3Iyy}q1q2
+{2m3(S2l2 + S23lg3)C23lg3 − 2S23C233Ixx + 2S23C23
3Iyy}q1q3(4.74)
− ∂T
∂q1
= 0 (4.75)
∂U
∂q1
= 0 (4.76)
∂D
∂q1
= D1 q1 (4.77)
以上式 (4.74) ∼ (4.77)を式 (1.6)に代入すると，リンク 1の運動方程式
{m2S22 l
2g2 + m3(S2l2 + S23lg3)
2 +1 Izz
+2IxxC22 +2 IyyS
22 +3 IxxC
223 +3 IyyS
223}q1
+{2m2S2C2l2g2 + 2m3(S2l2 + S23lg3)(C2l2 + C23lg3)
+2S2C2(2Iyy −2 Ixx) + 2S23C23(
3Iyy −3 Ixx)}q1q2
+{2m3(S2l2 + S23lg3)C23lg3 + 2S23C23(3Iyy −3 Ixx)}q1q3
+D1q1 = u1 (4.78)
が求まる．
リンク 2の運動については，
∂T
∂q2
= m2 l2g2 q2 + m3 l22 q2 + m3 l2g3(q2 + q3)
+m3 C3 l2 lg3(q2 + q3) + m3 C3 l2 lg3 q2
+2Izz q2 +3 Izz(q2 + q3) (4.79)
50
d
dt(∂T
∂q2
) = m2 l2g2 q2 + m3 l22 q2 + m3 l2g3(q2 + q3)
+m3 C3 l2 lg3(q2 + q3) − m3 S3 l2 lg3 (q2 + q3)q3
+m3 C3 l2 lg3 q2 − m3 S3 l2 lg3 q2 q3
+2Izz q2 +3 Izz(q2 + q3)
= (m2 l2g2 + m3 l22 + m3 l2g3 + 2 m3 C3 l2 lg3 +2 Izz +3 Izz)q2
+(m3 lg3 + m3 C3 l2 lg3 +3 Izz)q3
−2 m3 S3 l2 lg3 q2 q3 − m3 S3 l2 lg3 q23 (4.80)
− ∂T
∂q2
= −m2S2C2l2g2q
21 − m3(S2l2 + S23lg3)(C2l2 − C23lg3)q
21
−S2C2(2Iyy −2 Ixx)q
21 − S23C23(
3Iyy −3 Ixx)q21 (4.81)
∂U
∂q2
= m2g(−lg2S2) + m3g(−l2S2 − lg3S23) (4.82)
∂D
∂q2
= D2q2 (4.83)
以上の式 (4.80)∼(4.82)を式 (1.6)に代入するとリンク 2の運動方程式を求めるこ
とができる．
(m2 l2g2 + m3 l22 + m3 l2g3 + 2m3 C3 l2 lg3 + 2Izz + 3Izz) q2
+(m3 lg3 + m3 C3 l2 lg3 + 3Izz) q3 − 2m3 S3 l2 lg3 q2 q3 − m3 S3 l2 lg3 q23
−m2 S2 C2 l2g2 q21 − m3 (S2 l2 + S23 lg3) (C2 l2 + C23 lg3) q2
1
+S2 C2 (2Ixx − 2Iyy) q21 + S23 C23 (3Ixx − 3Iyy) q2
1 − m2 g lg2 S2
−m3 g (l2 S2 + lg3 S23) + D2q2 = u2 (4.84)
リンク 3の運動については，
∂T
∂q3
= m3l2g3(q2 + q3) + m3C3l2lg3q2 +3 Izz(q2 + q3) (4.85)
d
dt(∂T
∂q3
) = m3l2g3(q2 + q3) + m3C3l2lg3q2 − m3S3l2lg3q2q3 +3 Izz(q2 + q3)
= (m3l2g3 + m3C3l2lg3 +3 Izz)q2 + (m3l
2g3 +3 Izz)q3 − m3S3l2lg3q2q3(4.86)
− ∂T
∂q3
= −m3(S2l2 + S23lg3)C23lg3q12 + m3S3l2lg3g2(q2 + q3)
+S23C23(3Ixx − 3Iyy)q1
2 (4.87)
∂U
∂q3
= −m3glg3S23 (4.88)
∂D
∂q3
= D3q3 (4.89)
51
以上，式 (4.86)～(4.89)を式 (1.6)に代入するとリンク 3の運動方程式
(m3l2g3 + m3C3l2lg3 + 3Izz)q3 + (m3l
2g3 + 3Izz)q3
−m3(S2l2 + S23lg3g12)C23lg3q1
2
+m3S3l2lg3q22
+S23C23(3Ixx − 3Iyy)q1
2
−m3glg3S23 + D3q3 = u3 (4.90)
が得られる．
第 1，2，3リンクの運動方程式，式 (4.78)，(4.84)，(4.90)をまとめると次式と
なる．
m2S22 l
2g2 + m3(S2l2 + S23lg3)
2
+ 1Izz + 2IxxC22 + 2IyyS
22 0
+ 3IxxC223 + 3IyyS
223
0 m2l2g2 + m3(l
22 + l2g3)
+2m3C3l2lg3 + 2Izz + 3Izz
0 m3l2g3 + m3C3l2lg3 + 3Izz
0
m3l2g3 + m3C3l2lg3 +3 Izz
m3l2g3 + 3Izz
q1
q2
q3
+
0 0 0
−m2S2C2l2g2
−m3(S2l2 + S23lg3)(C2l2 + C23lg3) 0 −m3S3l2lg3
+S2C2(2Ixx − 2Iyy) + S23C23(
3Ixx − 3Iyy)
−m3(S2l2 + S23lg3)C23lg3 m3S3l2lg3 0
+S23C23(3Ixx − 3Iyy)g1
2
q1
q2
q3
+
2m2S2C2l2g2 2m3(S2l2 + S23lg3)C23lg3
+2m3(S2l2 + S23lg3)(C2l2 + C23lg3) 2S23C23(3Iyy − 3Ixx)
+2S2C2(2Iyy − 2Ixx) + 2S23C23(
3Iyy − 3Ixx)
0 0
0 0
0
−2m3S3l2lg3
0
q1q2
q1q3
q2q3
+
0
−m2glg2S2 − m3g(l2S2 + lg3S23)
−m3glg3S23
=
u1
u2
u3
(4.91)
52
上式を行列式とベクトルを用いた微分方程式で表わすと，q = [q1, q2, q3]T， ˙q =
[q1q2, q2q3, q2q3]T，q = [q1, q2, q3]
T を用いて，
M(q)q + N (q) ˙q + H(q)q + g(q) = u (4.92)
と表わすことができる．ただし，M(q)，N (q)，H(q)は式 (4.91)の左辺の各行
列を表わす．式 (4.92)の左辺第 1項は慣性力を表わし，M(q)は慣性行列と呼ば
れる．また第 2，3，4項は，それぞれ遠心力，コリオリ力，重力を表わす．
通常の教科書では，第 2第 3項をまとめて
N (q) ˙q + H(q)q = c(q, q) (4.93)
と表わす習慣となっているため，式 (4.92)は，
M(q)q + c(q, q) + g(q) = u (4.94)
と表わすことが多い．
53
第 5章
2リンクマニピュレータの
ニュートンオイラー解法
5.1 ニュートン・オイラー法
各関節の現在の角度qiと角速度 qi，ある目標加速度 qiが与えられたとする．こ
の時，まずアームの台座部から手先のリンクに向かって順に，各リンクの基準座
標系から見た回転角速度ωi，角加速度 ωi，並進速度 pi，並進加速度 piを計算す
る．次に各リンクがそのような運動をするために外部から質量中心に加えられな
ければならない並進力 f i及び回転モーメント niをニュートンの運動方程式とオ
イラーの運動方程式によって求める．さらに，手先に加わる力が与えられたもの
として手先から台座に向かって順に各関節において加えられるべき力とモーメン
トを算出しこれらをもとに各関節において加えられるべき駆動力の大きさ τ iを
求める．
5.2 ２リンクの運動方程式
l1lg1
m1; I1q1
q2lg2l2
m2; I2
Z
Y
X
g
図 5.1: 2link
リンクパラメーターは図 5.1に従う．
54
5.2.1 準備
0R1 =
cos q1 − sin q1 0
sin q1 cos q1 0
0 0 1
, 1R2 =
cos q2 − sin q2 0
sin q2 cos q2 0
0 0 1
, g =
0
−g
0
0p1 =
0
0
0
, 1p2 =
l10
0
, 2p3 =
l20
0
, 1s1 =
lg1
0
0
, 2s2 =
lg2
0
0
1I1 =
1Ix1 0 0
0 1Iy1 0
0 0 1Iz1
, 2I2 =
2Ix2 0 0
0 2Iy2 0
0 0 2Iz2
終端条件
3f 3 = 0, 3n3 = 0
初期条件
0p0 = −g, 0ω0 = 0, 0ω0 = 0
5.2.2 運動方程式の導出
リンク 1，リンク 2の角速度 1ω1，2ω2は
1ω1 =
0
0
q1
, 2ω2 =
0
0
q1 + q2
(5.1)
となる．これを，時間で微分して角加速度を求めると，
1ω1 =
0
0
q1
, 2ω2 =
0
0
q1 + q2
(5.2)
次にリンク 1，リンク 2の加速度を求める．
55
1p1 = 0RT1 [0p0 + 0ω0 × 0p1 + 0ω0 × (0ω0
0p1)]
=
cos q1 sin q1 0
− sin q1 cos q1 0
0 0 1
0
g
0
+
0
0
0
×
0
0
0
+
0
0
0
×
0
0
0
×
0
0
0
=
cos q1 sin q1 0
− sin q1 cos q1 0
0 0 1
0
g
0
=
sin q1g
cos q1g
0
(5.3)
2p2 = 1RT2 [1p1 + 1ω1 × 1p2 + 1ω1 × (1ω1
1p2)]
=
cos q2 sin q2 0
− sin q2 cos q2 0
0 0 1
sin q1g
cos q1g
0
+
0
0
q1
×
l10
0
+
0
0
q1
×
0
0
q1
×
l10
0
=
cos q1 sin q1 0
− sin q1 cos q1 0
0 0 1
sin q1g − l1q21
cos q1g + l1q1
0
=
(cos q2 sin q1 + cos q1 sin q2)g − l1 cos q2q21 + l1 sin q2q1
(− sin q2 sin q1 + cos q2 cos q1)g + l1 sin q2q21 + l1 cos q2q1
0
=
sin(q1 + q2)g − l1(cos q2q21 − sin q2q1)
cos(q1 + q2)g + l1(sin q2q21 + cos q2q1)
0
(5.4)
リンク 1の重心位置（質量中心）の加速度を計算する．
56
1s1 = 1p1 + 1ω1 × 1s1 + 1ω1 × (1ω1 × 1s1)
=
sin q1g
cos q1g
0
+
0
0
q1
×
lg1
0
0
+
0
0
q1
×
0
0
q1
×
lg1
0
0
=
sin q1g − lg1q21
cos q1g + lg1q1
0
(5.5)
2s2 = 2p2 + 2ω2 × 2s2 + 2ω2 × (2ω2 × 2s2)
=
sin(q1 + q2)g − l1(cos q2q21 − sin q2q1)
cos(q1 + q2)g + l1(sin q2q21 + cos q2q1)
0
+
0
0
q1 + q2
×
lg2
0
0
+
0
0
q1 + q2
×
0
0
q1 + q2
×
lg2
0
0
=
sin(q1 + q2)g − l1(cos q2q21 − sin q2q1) − lg2(q1 + q2)
2
cos(q1 + q2)g + l1(sin q2q21 + cos q2q1) + lg2(q1 + q2)
0
(5.6)
リンク 1とリンク 2に加わる外力を求める．
1f 1 = m11s1 (5.7)
2f 2 = m22s2 (5.8)
1n1 = 1I11ω1 + 1ω1 × (1I1
1ω1)
=
1Ix1 0 0
0 1Iy1 0
0 0 1Iz1
0
0
q1
+
0
0
q1
×
1Ix1 0 0
0 1Iy1 0
0 0 1Iz1
0
0
q1
=
0
01Iz1q1
(5.9)
2n2 = 2I22ω2 + 2ω2 × (2I2
2ω2)
=
2Ix2 0 0
0 2Iy2 0
0 0 2Iz2
0
0
q1 + q2
+
0
0
q1 + q2
×
2Ix2 0 0
0 2Iy2 0
0 0 2Iz2
0
0
q1 + q2
=
0
02Iz2(q1 + q2)
(5.10)
リンク 1，リンク 2に加えられる力とトルクは
57
2f 2 = m22s2 + 2R3
3f 3 (5.11)
= m22s2
= m2
sin(q1 + q2)g − l1(cos q2q21 − sin q2q1) − lg2(q1 + q2)
2
cos(q1 + q2)g + l1(sin q2q21 + cos q2q1) + lg2(q1 + q2)
0
(5.12)
1f 1 = 1R2m22s2 + m1
1s1
=
cos q2 − sin q2 0
sin q2 cos q2 0
0 0 1
m2
sin(q1 + q2)g − l1(cos q2q21 − sin q2q1) − lg2(q1 + q2)
2
cos(q1 + q2)g + l1(sin q2q21 + cos q2q1) + lg2(q1 + q2)
0
+m1
sin q1g − lg1q21
cos q1g + lg1q1
0
= m2
sin q1g − l1q21 − lg2 cos q2(q1 + q2)
2 − lg2 sin q2(q1 + q2)
cos q1g + l1q1 − lg2 sin q2(q1 + q2)2 + lg2 cos q2(q1 + q2)
0
+ m1
sin q1g − lg1q21
cos q1g + lg1q1
0
(5.13)
2n2 = 2R33n3 + 2n2 + 2s2 × 2f 2 + 2p3 × (2R3
3f 3)
= 2n2 + 2s2 × 2f 2
=
0
02Iz2(q1 + q2)
+
lg2
0
0
× m2
sin(q1 + q2)g − l1(cos q2q21 − sin q2q1) − lg2(q1 + q2)
2
cos(q1 + q2)g + l1(sin q2q21 + cos q2q1) + lg2(q1 + q2)
0
=
0
02Iz2(q1 + q2) + lg2[cos(q1 + q2)g + l1(sin q2q
21 + cos q2q1) + lg2(q1 + q2)]m2
(5.14)
1n1 = 1R22n2 + 1n1 + 1s1 × 1f 1 + 1p2 × (1R2
2f 2)
=
cos q2 − sin q2 0
sin q2 cos q2 0
0 0 1
0
02Iz2(q1 + q2) + lg2[cos(q1 + q2)g + l1(sin q2q
21 + cos q2q1) + lg2(q1 + q2)]m2
+
0
01Iz1q1
+
lg1
0
0
× m1
sin q1g − lg1q21
cos q1g + lg1q1
0
+
l10
0
×
cos q2 − sin q2 0
sin q2 cos q2 0
0 0 1
m2
sin(q1 + q2)g − l1(cos q2q21 − sin q2q1) − lg2(q1 + q2)
2
cos(q1 + q2)g + l1(sin q2q21 + cos q2q1) + lg2(q1 + q2)
0
=
0
02Iz2(q1 + q2) + lg2[cos(q1 + q2)g + l1(sin q2q
21 + cos q2q1) + lg2(q1 + q2)]m2
+
0
01Iz1q1
+
0
0
m1lg1(cos q1g + lg1q1)
+
0
0
m2l1[cos q1g + l1q1 − lg2 sin q2(q1 + q2)2 + lg2 cos q2(q1 + q2)]
=
0
02Iz2(q1 + q2) + lg2[cos(q1 + q2)g + l1(sin q2q
21 + cos q2q1) + lg2(q1 + q2)]m2 + 1Iz1q1
+m1lg1(cos q1g + lg1q1) + m2l1[cos q1g + l1q1 − lg2 sin q2(q1 + q2)2 + lg2 cos q2(q1 + q2)]
(5.15)
58
ここで，リンク 1，リンク 2は共に回転関節であるから，各関節の粘性摩擦係数
を d1，d2と置くと，
τ1 = [0 0 1]1n1 + d1q1
= 2Iz2(q1 + q2) + lg2[cos(q1 + q2)g + l1(sin q2q21 + cos q2q1) + lg2(q1 + q2)]m2 + 1Iz1q1
+m1lg1(cos q1g + lg1q1) + m2l1[cos q1g + l1q1 − lg2 sin q2(q1 + q2)2
+lg2 cos q2(q1 + q2)] + d1q1 (5.16)
τ2 = [0 0 1]2n2 + d2q2
= 2Iz2(q1 + q2) + lg2[cos(q1 + q2)g + l1(sin q2q21 + cos q2q1)
+lg2(q1 + q2)]m2 + d2q2 (5.17)
を得る．
ここで式 (5.16)，(5.17)を整理すると，[
τ1
τ2
]=
[1Iz1 + m1l
2g2 + 2Iz2 + m2(l
21 + l2g2 + 2l1lg2 cos q2)
2Iz2 + m2(l2g2 + l1lg2 cos q2)
2Iz2 + m2(l2g2 + l1lg2 cos q2)
2Iz2 + m2l2g2
][q1
q2
]
+
[−m2l1lg2(2q1q2 + q2
2) sin q2
m2l1lg2q21 sin q2
]+
[m1glg1 cos q1 + m2g(l1 cos q1 + lg2 cos (q1 + q2))
m2glg2 cos (q1 + q2)
]
+
[d1 0
0 d2
][q1
q2
](5.18)
となる．ここで，1Iz1 = I1，2Iz2 = I2とすると，ラグランジュ法とニュートン・
オイラー法で求めた運動方程式が一致する．
59
第 6章
平面3リンクマニピュレーターの
ニュートンオイラー解法
6.1 緒言
順動力学問題とは，ロボットの初期状態と入力トルク τ が与えられたときにロ
ボットがどのような運動を行うか，というものを運動方程式をもとに，求める問
題である．多自由度のロボットに対する逆動力学問題を閉じた形で解こうとする
と非常に複雑になり，効率も悪くなる．そのため高速解法であるニュートン・オイ
ラー法した，順動力学問題の解放が多く行われている．順動力学によって導かれ
た角加速度は，ルンゲクッタ法により数値積分され，角速度および角度が導出さ
れる．これらの計算を繰り返し行うことにより，各時間における運動方程式導出
をプログラム上で行っている．シミュレーションの精度を検証する場合，ニュー
トンオイラー法によって導出される時刻 t = 0での角加速度を求めることによ
り確認することができる．なぜならば，その時の角加速度が正しければ代数方
程式は正しいことが分かるからである．そのため、今回３リンクマニピュレータ
を用いて時刻 t = 0の各加速度が手計算によって導かれた値と正しいか検証を行う．
また，ヒューマノイドロボットは現在多く研究・開発が行われている．しかし,ASIMO
などの二足歩行ロボットは特異姿勢などの原因で膝を曲げたまま歩行を行ってい
るのが現状である.人間の行う膝を伸ばしながらの歩行には，消費エネルギーが削
減したりするなど利点が存在している．そのため，我々は，膝拘束を用いてヒュー
マノイドロボットのシミュレーションを作成し，消費エネルギーが実際に削減さ
れているか検証した．
6.2 3リンクマニピュレーターの運動方程式導出
6.2.1 準備
リンク 1,2,3の角速度 1w1,2 w2,
3 w3
1w1 =
0
0
θ1
, 2w2 =
0
0
θ1 + θ2
, 3w3 =
0
0
θ1 + θ2 + θ3
60
X
Y
Z
1θ
2θ
3θ
1l
2l1gl
2gl
3l
3gl
1m
2m
3m
図 6.1: 3-link manipulator
角速度は同様にして
1w1 =
0
0
θ1
, 2w2 =
0
0
θ1 + θ2
, 3w3 =
0
0
θ1 + θ2 + θ3
次に各回転行列を計算する．
0R1 =
cos θ1 − sin θ1 0
sin θ1 cos θ1 0
0 0 1
, 1R2 =
cos θ2 − sin θ2 0
sin θ2 cos θ2 0
0 0 1
2R3 =
cos θ3 − sin θ3 0
sin θ3 cos θ3 0
0 0 1
0P 1 =
0
0
0
, 1P 2 =
l10
0
, 2P 3 =
l20
0
1P G1 =
lg1
0
0
, 2P G2 =
lg2
0
0
, 3P G3 =
lg3
0
0
1I1 =
1Ix1 0 0
0 1Iy1 0
0 0 1Iz1
, 2I2 =
2Ix2 0 0
0 2Iy2 0
0 0 2Iz2
3I3 =
3Ix3 0 0
0 3Iy3 0
0 0 1Iz3
61
6.2.2 運動方程式導出
リンク 1,2,3の加速度を導出する.一般式では
AP C =A P B +A RBBP BC (6.1)
ここで 2階微分すると
AP C = AP B +A RBd2
dt2(BP BC) + 2AωB × [ARB
d
dt(BP BC)]
+ AωB × (ARBBP BC) +A ωB × [AωB × (ARB
BP BC)
BP BC は定数ベクトルより,微分すると消えるため
AP C = AP B +A ωB × (ARBBP BC)
+ AωB × [AωB × (ARBBP BC) (6.2)
したがって,
1P 1 = 0RT1 [0P 0 +0 ω0 ×0 P 1 +0 ω0 × (0ω0 ×0 P 1)]
=
cos θ1 sin θ1 0
− sin θ1 cos θ1 0
0 0 1
0
g
0
=
g sin θ1
g cos θ1
0
(6.3)
2P 2 = 1RT2 [1P 1 +1 ω1 ×1 P 2 +1 ω1 × (1ω1 ×1 P 2)]
=
g sin(θ1 + θ2) − l1(θ12cos θ2 − θ1 sin θ2)
g cos(θ1 + θ2) + l1(θ12sin θ2 + θ1 cos θ2)
0
(6.4)
62
3P 3 = 2RT3 [2P 2 +2 ω2 ×2 P 3 +2 ω2 × (2ω2 ×2 P 3)]
=
g sin(θ1 + θ2 + θ3) + l1[(−θ12cos(θ2 + θ3)
+θ1 sin(θ2 + θ3)] + l2[(θ1 + θ2) sin θ3
−(θ1 + θ2)2 cos θ3]
g cos(θ1 + θ2 + θ3) + l1[(−θ12sin(θ2 + θ3)
+θ1 cos(θ2 + θ3)] + l2[(θ1 + θ2) cos θ3
+(θ1 + θ2)2 sin θ3]
0
=
a
b
0
(6.5)
1P G1 = 1P 1 +1 ω1 ×1 P G1 +1 ω1 × (1ω1 ×1 P G1] (6.6)
=
g sin θ1 − θ12lg1
g cos θ1 + θ1lg1
0
(6.7)
2P G2 = 2P 2 +2 ω2 ×2 P G2 +2 ω2 × (2ω2 ×2 P G2]
=
g sin(θ1 + θ2) − l1(θ12cos θ2 − θ1 sin θ2)
−lg2(θ1 + θ2)2
g cos(θ1 + θ2) + l1(θ12sin θ2 + θ1 cos θ2)
+lg2(θ1 + θ2)
0
(6.8)
3P G3 = 3P 3 +3 ω3 ×3 P G3 +3 ω3 × (3ω3 ×3 P G3]
=
a − lg3(θ1 + θ2 + θ3)2
b + lg3(θ1 + θ2 + θ3)
0
(6.9)
リンク 1,2,3に加わる外力は
1f 1 = m11P G1 (6.10)
2f 2 = m22P G2 (6.11)
3f 3 = m33P G3 (6.12)
1n1 = 1I11ω1 +1 ω1 × (1I1
1ω1) =
0
01Iz1θ1
(6.13)
2n2 = 2I22ω2 +2 ω2 × (2I2
2ω2) =
0
02Iz2(θ1 + θ2)
(6.14)
3n3 = 3I33ω3 +3 ω3 × (3I3
2ω3) =
0
03Iz3(θ1 + θ2 + θ3)
(6.15)
63
したがって、リンク 1,2,3に加えられる力とトルクの式は
3f 3 = 3f 3 +3 f 4 =3 f 3 (6.16)2f 2 = 2f 2 +2 f 3 = m2
2P G2 +2 R3m33P G3 (6.17)
2f 2 = 1f 1 +1 f 2 = m11P G1 +1 R2m2
2P G2 (6.18)
ここで以下の行列をおく,
1P G1 =
α1
β1
0
, 2P G2 =
α2
β2
0
, 3P G3 =
α3
β3
0
3n3 = 3R44n4 +3 n3 +3 P G3 ×3 f 3 +3 P 4 × (3R4
4f 4)
=
0
03Iz3(θ1 + θ2 + θ3) + m3lg3[g cos(θ1 + θ2 + θ3)
+l1(θ2) sin(θ2 + θ3) + l2((θ1 + θ2) cos θ3 + (θ1
+θ2)2 sin θ3) + lg3(θ1 + θ2 + θ3)]
(6.19)
2n2 = 2R33n3 +2 n2 +2 P G2 ×2 f 2 +2 P 3 × (2R3
3f 3)
=
0
03Iz3(θ1 + θ2 + θ3) + m3lg3β3 +2 Iz2(θ1 + θ2)
+m2lg2[g cos(θ1 + θ2) + l1(θ12+ θ1 cos θ2)]
+lg2(θ1 + θ2) + m3l2(α3 sin θ3 + β3 cos θ3)
(6.20)
1n1 = 1R22n2 +1 n1 +1 P G1 ×1 f 1 +1 P 2 × (1R2
2f 2)
=
0
02nz2 +1 Iz1θ1 + m1lg1β2 + l1[m2(α2cos θ2 − β2 sin θ2)
+m3(2α3cos θ1sin θ2 + β3(cos θ22 − sin θ2
2))]
(6.21)
ここで，リンク1，リンク２，リンク３はともに回転行列より，式 (6.19),(6.20),(6.21)
を用いて
τ3 =[0 0 1
]3n3
= 3Iz3(θ1 + θ2 + θ3) + m3lg3[g cos(θ1 + θ2 + θ3)
+ l1(θ2) sin(θ2 + θ3) + l2((θ1 + θ2) cos θ3 + (θ1
+ θ2)2 sin θ3) + lg3(θ1 + θ2 + θ3)] (6.22)
64
τ2 =[0 0 1
]2n2
= τ3 + 3Iz3(θ1 + θ2) + m2lg2[g cos(θ1 + θ2) + l1(θ12sin θ2
+ θ1 cos θ2) + lg2(θ1 + θ2)] + l2[g cos(θ1 + θ2)
+ l1(θ1 cos θ2 + θ12sin θ2) + 2l2(θ1 + θ2)
− lg3 sin(θ1 + θ2 + θ3)2 + lg3 cos(θ1 + θ2 + θ3)] (6.23)
τ1 =[0 0 1
]1n1
= τ2 +1 Iz1θ1 + m1lg1(g cos θ1 + θ1lg1) + l1[m2(g cos θ1
+ l1θ1 − lg2(θ1 + θ2)2 sin θ2 + lg2(θ1 + θ2) cos θ2)
+ m3(g cos θ1 + l1θ1 − l2(θ1 + θ2)2 sin θ2 + 2l2(θ1 + θ2) cos θ2
− lg3(θ1 + θ2 + θ3)2 sin(θ2 + θ3)
+ lg3(θ1 + θ2 + θ3) cos(θ2 + θ3)] (6.24)
65
第 7章
垂直多関節3リンクマニピュレータ
のニュートンオイラー解法
3リンクマニピュレータの運動方程式をニュートンオイラー法を用いて導出
する．
7.1 準備
回転行列
0R1 =
C1 −S1 0
S1 C1 0
0 0 1
(7.1)
1R2 =
0 1 0
0 0 1
1 0 0
C2 −S2 0
S2 C2 0
0 0 1
=
S2 C2 0
0 0 1
C2 −S2 0
(7.2)
2R3 =
C3 −S3 0
S3 C3 0
0 0 1
(7.3)
3RH =
0 0 1
−1 0 0
0 −1 0
(7.4)
リンク長さ
0p0 =
0
0
0
, 1p2 =
0
0
l1
, 2p3 =
l20
0
, 3pH =
l30
0
(7.5)
リンク重心
0s0 =
0
0
0
, 1s2 =
0
0
lg1
, 2s3 =
lg2
0
0
, 3sH =
lg3
0
0
(7.6)
66
慣性モーメント
1I1 =
1Ixx 0 0
0 1Iyy 0
0 0 1Izz
, 2I2 =
2Ixx 0 0
0 2Iyy 0
0 0 2Izz
3I3 =
3Ixx 0 0
0 3Iyy 0
0 0 3Izz
(7.7)
初期条件
0p0 =
0
0
g
, 0ω0 = 0 , 0ω0 = 0 (7.8)
終端条件
3f 3 = 0 , 3n3 = 0 (7.9)
7.2 導出，正順繰り返し計算
7.2.1 i=1
リンク 1角速度
1ω1 =
0
0
q1
(7.10)
リンク 1角加速度
2ω2 =
0
0
q1
(7.11)
リンク 1並進速度
1p1 = 0RT1 [0p0 + 0ω0 × 0p1 + 0ω0 × (0ω0
0p1)]
=
C1 −S1 0
S1 C1 0
0 0 1
T
0
0
g
=
0
0
g
(7.12)
リンク 1重心並進速度
1s1 = 1p1 + 1ω1 + 1s1 + 1ω1 × (1ω1 × 1s1)
=
0
0
g
+
0
0
q1
+
0
0
lg1
+
0
0
q1
×
0
0
q1
×
0
0
lg1
=
0
0
g
(7.13)
67
7.2.2 i=2
リンク 2角速度
2ω2 = 1RT2
1ω1 + ez q2
=
S2 C2 0
0 0 1
C2 −S2 0
T
0
0
q1
+
0
0
q2
=
C2q1
−S2q1
q2
(7.14)
リンク 2角加速度
2ω2 = 1RT2
1ω1 + ez q2 + (1RT2
1ω1) × ez q2
=
S2 C2 0
0 0 1
C2 −S2 0
T
0
0
q1
+
0
0
q2
+
S2 C2 0
0 0 1
C2 −S2 0
T
0
0
q1
×
0
0
q2
=
C2q1 − S2q1q2
−S2q1 − C2q1q2
q2
(7.15)
リンク 2並進速度
2p2 = 1RT2 [1p1 + 1ω1 × 1p2 + 1ω1 × (1ω1
1p2)]
=
C1 −S1 0
S1 C1 0
0 0 1
T
0
0
g
+
0
0
q1
×
0
0
l1
+
0
0
q1
×
0
0
q1
×
0
0
l1
=
C2g
−S2g
0
(7.16)
リンク 2重心並進速度
2s2 = 2p2 + 2ω2 + 2s2 + 2ω2 × 2ω2 × (2ω2 × 2s2)
=
C2g
−S2g
0
+
C2q1 − S2q1q2
−S2q1 − C2q1q2
q2
×
lg2
0
0
+
C2q1
−S2q1
q2
×
C2q1
−S2q1
q2
×
lg2
0
0
=
C2g − lg2S22 q
21 − lg2q
22
−S2g + lg2q2 − lg2C2S2q21
lg2(S2q1 + C2q1q2) + lg2C2q2q2
(7.17)
7.2.3 i=3
リンク 3角速度
3ω3 = 2RT3
2ω2 + ez q3
=
C3 −S3 0
S3 C3 0
0 0 1
T
C2q1
−S2q1
q2
+
0
0
q3
=
C23q1
−S23q1
q2 + q3
(7.18)
68
リンク 3角加速度
3ω3 = 2RT3
2ω2 + ez q3 + (2RT3
2ω2) × ez q3
=
C3 −S3 0
S3 C3 0
0 0 1
T
C2q1 − S2q1q2
−S2q1 − C2q1q2
q2
+
0
0
q3
+
C3 −S3 0
S3 C3 0
0 0 1
T
C2q1
−S2q1
q2
×
0
0
q3
=
C23q1 − S23q1q2 − S23q1q3
−S23q1 + C23q1q2 − C23q1q3
q2 + q3
(7.19)
リンク 3並進速度
3p3 = 2R3T [2p2 +2 ω2 ×2 p3 + 2ω2 × (2ω2
2p3)]
=
C3 −S3 0
S3 C3 0
0 0 1
T
C2g
−S2g
0
+
C2q1 − S2q1q2
−S2q1 − C2q1q2
q2
×
l20
0
+
C2q1
−S2q1
q2
×
C2q1
−S2q1
q2
×
l20
0
=
C23g − l2S2S23q21 − l2C3q
22 + l2S3q2
−S23g − l2S2C23q21 + l2S3q
22 + l2C3q2
l2S2q1 + 2l2C2q1q2
(7.20)
リンク 3重心並進速度
3s3 = 3p3 + 3ω3 + 3s3 + 3ω3 × 3ω3 × (3ω3 × 3s3)
=
C23g − l2S2S23q21 − l2C3q
22 + l2S3q2
−S23g − l2S2C23q21 + l2S3q
22 + l2C3q2
l2S2q1 + 2l2C2q1q2
+
C23q1 − S23q1q2 − S23q1q3
−S23q1 + C23q1q2 − C23q1q3
q2 + q3
×
lg3
0
0
+
C23q1
−S23q1
q2 + q3
×
C23q1
−S23q1
q2 + q3
×
lg3
0
0
=
C23g − S23q21(l2S2 + lg3S23) − l2C3q
22 + l2S3q2 − lg3(q2 + q3)
2
−S23g − C23q21(l2S2 + lg3S23) + l2S3q
22 + l2C3q2 + lg3(q2 + q3)
q1(l2S2 + lg3S23) + 2q1q2(l2C3 + lg3C23) + 2lg3C23q1q3
(7.21)
69
7.3 導出，逆順繰り返し計算
7.3.1 i=3
リンク 3外力
3f 3 = m33s3 + 3RH
HfH
= m33s3
= m3
C23g − S23q21(l2S2 + lg3S23) − l2C3q
22 + l2S3q2 − lg3(q2 + q3)
2
−S23g − C23q21(l2S2 + lg3S23) + l2S3q
22 + l2C3q2 + lg3(q2 + q3)
q1(l2S2 + lg3S23) + 2q1q2(l2C3 + lg3C23) + 2lg3C23q1q3
(7.22)
リンク 3モーメント
3n3 = 3I33ω3 + 3ω3 × (3I3
3ω3) + 3s3 × (m33s3)
=
3Ixx 0 0
0 3Iyy 0
0 0 3Izz
C23q1 − S23q1q2 − S23q1q3
−S23q1 + C23q1q2 − C23q1q3
q2 + q3
+
C23q1
−S23q1
q2 + q3
×
3Ixx 0 0
0 3Iyy 0
0 0 3Izz
C23q1
−S23q1
q2 + q3
+
lg3
0
0
× m3
C23g − S23q21(l2S2 + lg3S23) − l2C3q
22 + l2S3q2 − lg3(q2 + q3)
2
−S23g − C23q21(l2S2 + lg3S23) + l2S3q
22 + l2C3q2 + lg3(q2 + q3)
q1(l2S2 + lg3S23) + 2q1q2(l2C3 + lg3C23) + 2lg3C23q1q3
=
0
−m3lg3(l2S2 + lg3S23)q1 − 2m3lg3(l2C3 + lg3C23)q1q2 − 2m3l2g3C23q1q3
−m3lg3S23g − m3lg3(l2S3 + lg3S23)q21 + m3lg3(l2S3q2 + l2C3q2) + m3l
2g3(q2 + q3)
(7.23)
関節トルク
τ 3 =[
0 0 1]
3n3
= −m3lg3S23g − m3lg3(l2S3 + lg3S23)q21 + m3lg3(l2S3q2 + l2C3q2) + m3l
2g3(q2 + q3)
= (m3l2g3 + m3C3l2lg3 + 3Izz)g3 + (m3l
2g3 + 3Izz)q3
− m3(S2l2 + S23lg3g12)C23lg3q1
2
+ m3S3l2lg3q22
+ S23C23(3Ixx − 3Iyy)q1
2
− m3glg3S23 + D3q3 = τ3 (7.24)
70
7.3.2 i=2
リンク 2外力
2f 2 = m22s2 + 2R3
3f 3
= m2
C2g − lg2S22 q
21 − lg2q
22
−S2g + lg2q2 − lg2C2S2q21
lg2(S2q1 + C2q1q2) + lg2C2q2q2
+
C3 −S3 0
S3 C3 0
0 0 1
3f 3 (7.25)
リンク 2モーメント
2n2 = 2R33n3 + 2I2
2ω2 + 2ω2 × (2I22ω2) + 2s2 × (m2
2s2) + 2p2 × 2R33f 3
=
C3 −S3 0
S3 C3 0
0 0 1
0
−m3lg3(l2S2 + lg3S23)q1 − 2m3lg3(l2C3 + lg3C23)q1q2 − 2m3l2g3C23q1q3
−m3lg3S23g − m3lg3(l2S3 + lg3S23)q21 + m3lg3(l2S3q2 + l2C3q2) + m3l
2g3(q2 + q3)
+
2Ixx 0 0
0 2Iyy 0
0 0 2Izz
C2q1 − S2q1q2
−S2q1 − C2q1q2
q2
+
C2q1
−S2q1
q2
×
2Ixx 0 0
0 2Iyy 0
0 0 2Izz
C2q1
−S2q1
q2
+
lg2
0
0
× m2
C2g − lg2S22 q
21 − lg2q
22
−S2g + lg2q2 − lg2C2S2q21
lg2(S2q1 + C2q1q2) + lg2C2q2q2
+
l20
0
× m3
C3 −S3 0
S3 C3 0
0 0 1
C23g − S23q21(l2S2 + lg3S23) − l2C3q
22 + l2S3q2 − lg3(q2 + q3)
2
−S23g − C23q21(l2S2 + lg3S23) + l2S3q
22 + l2C3q2 + lg3(q2 + q3)
q1(l2S2 + lg3S23) + 2q1q2(l2C3 + lg3C23) + 2lg3C23q1q3
=
C3nx3 − S3ny3 + Ix2C2q1 − Ix2S2q1q2 − Iz2S2q1q2 + Iy2S2q1q2
S3nx3 + C3ny3 − Iy2S2q1 − Iy2C2q1q2 + IX2C2q1q2 − Iz2C2q1q2 − m2l2g2(S2q1 + C2q1q2) − m2l
2g2C2q1q2
nz3 + Iz2q2 − Iy2S2C2q21 + Ix2S2C2q
21 − m2l
2g2S2g + m2l
2g2q2 − m2l
2g2C2S2q1 + l2(S3fx3 + C3fy3)
(7.26)
関節トルク
τ 2 =[
0 0 1]
2n2 (7.27)
71
(m2 l2g2 + m3 l23 + m3 l2g3 + 2m3 C3 l2 lg3 + 2Izz + 3Izz) q2
+(m3 lg3 + m3 C3 l2 lg3 + 3Izz) q3 − 2m3 S3 l2 lg3 q2 q3 − m3 S3 l2 lg3 q23
−m2 S2 C2 l2g2 q21 − m3 (S2 l2 + S23 lg3) (C2 l2 + C23 lg3) q2
1
+S2 C2 (2Ixx − 2Iyy) q21 + S23 C23 (3Ixx − 3Iyy) q2
1 − m2 g lg2 S2
−m3 g (l2 S2 + lg3 S23) + D2q2 = τ2 (7.28)
7.3.3 i=1
リンク 1外力
1f 1 = m11s1 + 1R2
2f 2 (7.29)
リンク 1モーメント
1n1 = 1R22n2 + 1I1
1ω1 + 1ω1 × (1I11ω1) + 1s1 × (m1
1s1) + 1p1 × 1R22f 2
=
S2 C2 0
0 0 1
C2 −S2 0
C3nx3 − S3ny3 + Ix2C2q1 − Ix2S2q1q2 − Iz2S2q1q2 + Iy2S2q1q2
S3nx3 + C3ny3 − Iy2S2q1 − Iy2C2q1q2 + IX2C2q1q2 − Iz2C2q1q2 − m2l2g2(S2q1 + C2q1q2) − m2l
2g2C2q1q2
nz3 + Iz2q2 − Iy2S2C2q21 + Ix2S2C2q
21 − m2l
2g2S2g + m2l
2g2q2 − m2l
2g2C2S2q1 + l2(S3fx3 + C3fy3)
+
1Ixx 0 0
0 1Iyy 0
0 0 1Izz
0
0
q1
+
0
0
q1
×
1Ixx 0 0
0 1Iyy 0
0 0 1Izz
0
0
q1
+
0
0
lg1
× m1
0
0
g
+
0
0
lg1
×
S2 C2 0
0 0 1
C2 −S2 0
fx2
fy2
fz2
=
S2nx2 + C2ny2 − l1fz2
nz2 + l1S2fx2 + l1fy2
C2nx2S2ny2 + Iz1q1
(7.30)
関節トルク
τ 1 =[
0 0 1]
1n1 (7.31)
{m2S22 l
2g2 + m3(S2l2 + S23lg3)
2 +1 Izz
+2IxxC22 +2 IyyS
22 +3 IxxC
223 +3 IyyS
223}q1
+{2m2S2C2l2g2 + 2m3(S2l2 + S23lg3)(C2l2 − C23lg3)
+2S2C2(2Iyy −2 Ixx) + 2S23C23(
3Iyy −3 Ixx)}q1q2
+{2m3(S2l2 + S23lg3)C23lg3 + 2S23C23(3Iyy −3 Ixx)}q1q3
+D1q1 = τ1 (7.32)
72
第 8章
数値積分法(ルンゲクッタ法)の導出
次式で表わされる非線形微分方程式の解 q(t)を数値積分により解く方法につい
て考える．
dq(t)
dt= f(q(t)) (8.1)
解 q(t)のある時刻 t0より微小時間 h経過した値 q(t + h)は，テーラー展開より
q(t0 + h) =
[q(t) + h
dq
dt+
h2
2!
d2q
dt2+ · · · + hi
i!
diq
dti+ · · ·
]
t0
(8.2)
と表わされる．ここで，上式は式 (3.1)を満たす解と考えると，上式に式 (3.1)を
代入することで
q(t0 + h) =
[q(t) + hf(q(t)) +
h2
2!
d
dtf + · · · + hi
i!
di−1q
dti−1f + · · ·
]
t0
(8.3)
となることがわかる．[]t0 は，t = t0の時刻の値であることを意味することとす
ると，[df
dt
]
t0
=
[df
dq
dq
dt
]
t0
=
[dq
dt
d
dqf
]
t0
=
[f
d
dqf
]
t0
(8.4)
であり，
D = fd
dq(8.5)
とおくことで上式は，[df
dt
]
t0
=
[Df
]
t0
(8.6)
と表わされる．さらに
df2
dt2=
(f
d
dq
)(f
d
dqf
)= f 2d2f
dq2+ f
(df
dq
)2
(8.7)
であるから時刻 t0において，[d2f
dt2
]
t0
=
[D2f + Df
df
dq
]
t0
(8.8)
73
となる．さらに [d3f/dt3]t0を計算すると，式 (3.7)より[d3f
dt3
]
t0
=
[(f
d
dq
)3
f −(
fd
dq
){f 2d2f
dq2+ f
(df
dq
)2
}]
t0
=
[f 3d3f
dq3+ f
(d
dqf 2
)d2f
dq2+ f
df
dq
(df
dq
)2
+ f 2 d
dq
(df
dq
)2]
t0
=
[f 3d3f
dq3+ 2f 2 df
dq
d2f
dq2+ f
(df
dq
)3
+ 2f 2
(df
dq
)d2f
dq2
]
t0
=
[f 3d3f
dq3+ 2
(f
d
dq
)(df
dq
)(f
df
dq
)(df
dq
)2(f
d
dq
)f
+
(f
d
dq
)(df
dq
)(f
df
dq
)+
(f
d
dq
)(f
df
dq
)(df
dq
)]
t0
=
[D3f + D2f
df
dq+ Df
(df
dq
)2
+ 3DfDdf
dq
]
t0
(8.9)
ただし，ここでは
f 2
(df
dq
)(d2f
dq2
)=
(f
d
dq
)(f
df
dq
)(df
dq
)= D2f
df
dq(8.10)
f 2
(df
dq
)(d2f
dq2
)=
(f
d
dq
)(f
df
dq
)(df
dq
)= Df
df
dqDf (8.11)
を用いた．kを，
k = q(t0 + h) − q(t0) (8.12)
と定義する．式 (3.3)の hの 4次項までで，kを表わすと，
k =
[hf(q(t)) +
1
2h2df
dt+
1
6h3d2f
dt2+
1
24h4d3f
dt3
]
t0
(8.13)
となり，式 (3.6)，(3.8)，(3.9)を代入すると，
k =
[hf +
1
2h2Df +
1
6h3
(D2f + Df
df
dq
)
+1
24h4{D3f + D2f
df
dq+ Df
(df
dq
)2
+ 3DfDdf
dq}]
t0
(8.14)
と表わされる．上式は，hの 4次項までで q(t0)より q(t0 +h)を数値的に求めるこ
とができ，式 (3.1)の数値解を与えるが，Dif(i = 0, 1, 2, 3, )つまり f の 3次微分
までの微係数を求める必要がある．f に sin，cosなどが複雑に入っているロボッ
トの運動方程式の場合，Dif を求めることは一般的に困難である．
そこでDifを求めることなく，式 (3.13)を算出する方法を別に考える必要があ
る．平均値の定理を用いて
k =
∫ t0+h
t0
f(q(t))dt = hf(q(t0) + ah) ( 0 ≤ a ≤ 1のある値 ) (8.15)
より kを決定する方法について考える．しかし aの値は関数 f に依存するためこ
れも容易ではない．
74
そこで t0 < t < t0 + hの時間区間のいろいろな時刻の f の値を用いて kを決定
する方法について考える. 時間区間の中の時刻をどのように設定するかについて
は，式 (3.14)の４次項までの近似が正しく求まるように設定する方法である．例
えば
k1 = hf(q(t0)) = hf0
k2 = hf(q(t0) + βk1)
k3 = hf(q(t0) + β1k1 + γ1k2)
k4 = hf(q(t0) + β2k1 + γ2k2 + δ2k3)
(8.16)
k = R1k1 + R2k2 + R3k3 + R4k4 (Riは重み) (8.17)
ここでRi, β, βi, γi, δiは未知数であり以下のようにして決定する．
式 (3.1)の f(q(t))をテーラ展開すると，
f(q(t) + p) = f(q(t)) + pd
dqf + (p
d
dq)2f + (p
d
dq)3f + · · · (8.18)
そこで，上式を用いてまず式 (3.15)の k2を求めてみると，
k2 = hf(q(t0)) + βk1)
= h[f(q(t) + βk1d
dqf +
1
2(βk1
d
dq)2f + · · · ]t0 (8.19)
となり
hD1 = βk1d
dq(8.20)
とおくと，
k2 = h[f + hD1f +1
2h2D2
1f + · · · ]t0 (8.21)
と表わされる．次に，
D2 = (β1 + γ1)f0d
dq(8.22)
とおけば，式 (3.15)の k1 = hf0より
(β1k1 + γ1k2)d
dq= (β1hf0 + γ1k2)
d
dq
= h(β1 + γ1)f0d
dq+ (γ1k2 − hγ1f0)
d
dq
= hD2 + γ1(k2 − hf0)d
dq
= hD2 + γ1[h(f + hD1f +1
2h2D2
1f + · · · ) − hf0]t0d
dq
= hD2 + γ1h2[D1f +
1
2hD2
1f + · · · ]t0d
dq(8.23)
75
となり，これから
k3 = hf(q(t0) + β1k1 + γ1k2)
= h[f + (β1k1 + γ1k2)d
dqf +
1
2(β1k1 + γ1k2)
2 d2
dq2f + · · · ]t0
= h[f0 + (hD2 + γ1h2[D1f +
1
2hD2
1f + · · · ]t0d
dq)f
+1
2(hD2 + γ1h
2[D1f +1
2hD2
1f + · · · ]t0d
dq)2f + · · ·
= h[{f + hD2f +1
2h2D2
2f +1
6h3D3
2f + · · · }t0
+γ1h2{D1f
df
dq+
1
2hD2
1fdf
dq+ hD1fD2
df
dq+ · · · }t0 ] (8.24)
をあらわすことが出来る.よって最後に，
D3 = (β2 + γ2 + δ2)f0d
dq(8.25)
とおいて
(β2k1 + γ2k2 + δ2k3)d
dq
= {β2hf0 + γ2h[f + hD1f +1
2h2D2
1f + · · · ]t0
+δ2h[{f + hD2f +1
2h2D2
2f + · · · }t0
+γ1h2{D1f
df
dq+
1
2hD2
1fdf
dq+ hD1fD2
df
dq+ · · · }t0 ]}
d
dq
= h(β2 + γ2 + δ2)f0d
dq
+[h2γ2{D1f +1
2hD2
1f + · · · }t0
+h2δ2{D2f +1
2hD2
2f + · · · }t0
+γ1δ2h3{D1f
df
dq+
1
2hD2
1fdf
dq+ hD1fD2
df
dq+ · · · }t0 ]
d
dq
= hD3 + h2[γ2{D1f +1
2hD2
1f + · · · }t0
+δ2{D2f +1
2h(D2
2f + 2γ1D1fdf
dq) + · · · }t0 ]
d
dq(8.26)
76
より，
k4 = hf(q(t0) + β2k1 + γ2k2 + δ2k3)
= h[f + (β2k1 + γ2k2 + δ2k3)d
dqf +
1
2(β2k1 + γ2k2 + δ2k3)
2 d2
dq2f
= h[f + {hD3f + h2[γ2{D1f +1
2hD2
1f + · · · }t0
+δ2{D2f +1
2h(D2
2f + 2γ1D1fdf
dq) + · · · }t0 ]
df
dq}
+1
2{hD3 + h2[γ2{D1f +
1
2hD2
1f + · · · }t0
+δ2{D2f +1
2h(D2
2f + 2γ1D1fdf
dq) + · · · }t0 ]
d
dq}2f + · · · ]
= h[f + hD3f +1
2h2D2
3f +1
6h3D3
3f + · · ·
+h2(γ2D1f + δ2D2f)df
dq+ h3γ2D1fD3
df
dq+ h3δ2D2fD3
df
dq
+1
2h3γ2D
21f
df
dq+
1
2h3δ2D
22f
df
dq+ 2γ1δ2D1f(
df
dy)2 + · · · ]t0
= h[f + hD3f +1
2h2D2
3f +1
6h3D3
3f + · · ·
+h2(γ2D1f + δ2D2f)df
dq+ h3(γ2D1f + δ2D2f)D3
df
dq
+1
2h3(γ2D
21f + δ2D
22f + 2γ1δ2D1f
df
dy)df
dy+ · · · ]t0 (8.27)
が得られる. 以上の k2～k4の計算式 (3.20),(3.23),(3.26)と式 (3.15)の k1より式
(3.10)の kを求めると次式となる．
k = R1k1 + R2k2 + R3k3 + R4k4
= R1k[f ]t0
+R2h[f + hD1f +1
2h2D2
1f +1
6h3D3
1f + · · · ]t0
+R3h[f + hD2f +1
2h2D2
2f +1
6h3D3
2f + · · ·
+ γ1h2(D1f
df
dq+
1
2hD2
1fdf
dq+ hD1fD2
df
dq]t0
= R4h[f + hD3f +1
2h2D2
3f +1
6h3D3
3f + · · ·
+ h2(γ2D1f + δ2D2f)df
dq+ h3(γ2D1f + δ2D2f)D3
df
dq
+1
2h3(γ2D
21f + δ2D
22f + 2γ1δ2D1f
df
dq)df
dq+ · · · ]t0 (8.28)
上式が式 (3.14)の kと hの４次項まで一致する条件を求めると，
77
１次の項 : R1 + R2 + R3 + R4 = 1 (8.29)
２次の項 : R2D1f + R3D2f + R4D3f =1
2Df (8.30)
３次の項 : f に関して1
2R2D
21f +
1
2R3D
22f +
1
2R4D
23f =
1
6D2f (8.31)
:df
dqに関して
R3γ1D1f + R4(γ2D1f + δ2D2f) =1
6Df (8.32)
４次の項 : f に関して1
6R2D
31f +
1
6R3D
32f +
1
6R4D
33f =
1
24D3f (8.33)
:df
dqに関して
1
2R3γ1D
21f +
1
2R4(γ2D
21f + δ2D
22f) =
1
24D2f (8.34)
: (df
dq)2に関して
R4γ1δ2D1f =1
24Df (8.35)
: Ddf
dqに関して
R3γ1D1fD2df
dq+ R4(γ2D1f + δ2D2f)D3
df
dq=
3
24DfD
df
dq(8.36)
となり，式 (3.29)～(3.36)が成り立つとき，hの４次項までで k = kが成り立
つ．式 (3.15),(3.19)よりD1 = βf0ddqであり，この結果と式 (3.5)より
D1 = βD4= α1D (8.37)
さらに，式 (3.21),(3.24)より
D2 = (β1 + γ1)D = a2D (8.38)
D3 = (β2 + γ2 + δ2)D = a3D (8.39)
(8.40)
とおくことにする．上式より式 (3.28)～(3.35)は
R1 + R2 + R3 + R4 = 1
R2α1 + R3α2 + R4α3 = 12
R2α21 + R3α
22 + R4α
23 = 1
3
R3γ1α1 + R4(γ2α1 + δ2α2) = 16
R2α31 + R3α
32 + R4α
33 = 1
4
R3γ1α21 + R4(γ2α
21 + δ2α
22) = 1
12
R4γ1δ2α1 = 124
R3γ1α1α2 + R4(γ2α1 + δ2α2)α3 = 18
(8.41)
78
式 (3.40)は，R1～R4,α1～α3,γ1,γ2,δ2の１０個の未知数を決定するのに対し，８
個の式が制約を与えていることを意味するから，２つの未知数を任意にとれるこ
とになる．このため，α1 = α2 = 12と定めることにする．このとき式 (3.41)は
R1 + R2 + R3 + R4 = 1 (8.42)1
2R2 +
1
2R3 + R4α3 =
1
2(8.43)
1
4R2 +
1
4R3 + R4α
23 =
1
3(8.44)
1
2R3γ1 + R4(
1
2γ2 +
1
2δ2) =
1
6(8.45)
1
8R2 +
1
8R3 + R4α
33 =
1
4(8.46)
1
4R3γ1 +
1
4R4(γ2 + δ2) =
1
12(8.47)
1
2R4γ1δ2 =
1
24(8.48)
1
4R3γ1 +
1
2R4(γ2 + δ2)α3 =
1
8(8.49)
となる。式 (3.43),(3.44),(3.46)よりα3 = 1が求まり，これを用いると，式 (3.43),(3.46)
よりR4 = 1/6となる．さらに式 (3.42),(3.43)よりR1 = 1/6となる．以上の結果
より式 (3.43),(3.44),(3.46)の意味が無くなる。残りの方程式は
R2 + R3 =2
3(8.50)
6R3γ1 + γ2 + δ2 = 2 (8.51)
2γ1δ2 = 1 (8.52)
6R3γ1 + 2(γ2 + δ2) = 3 (8.53)
となる．式 (3.51),(3.53)により γ2 + δ2 = 1となりこれを式 (3.51)に代入すると，
r1 = 1/6R3となり，式 (3.52)により δ2 = 1/2γ1 = 3R3となる γ2は γ2 = 1− δ2 =
1 − 3R3となる．以上よりR3に任意性が残っていることになる．R3 = 1/3と定
めるときの数値積分法がルンゲ-クッタの式であり，このとき１０個の未知数は，
R1 = 16, R2 = 1
3, R3 = 1
3, R4 = 1
6α1 = 1
2, α2 = 1
2, α3 = 1
γ1 = 12, γ2 = 0, δ2 = 1
}(8.54)
と定まる．さらに式 (3.37)～(3.39)より β1, β2, β3が定まる．
β =1
2, β1 = 0, β2 = 0 (8.55)
このとき式 (3.14)で与えられる kと式 (3.17)で与えられる kは一致する．
以上より式 (3.1)を満足する解軌道 q(t)の，時刻 t0より hの時間幅進んだとき
の値 q(t0 + h)は，
q(t0 + h) = q(t0) + k
= q(t0) +1
6(k1 + 2k2 + 2k3 + k4) (8.56)
79
と求められ，k1 ∼ k4は，式 (3.16)より
k1 = hf(q(t0))
k2 = hf(q(t0) + 12k1)
k3 = hf(q(t0) + 12k2)
k4 = hf(q(t0) + k3)
(8.57)
と与えられることがわかる．上式の t0を i∆t，hを∆tとおくと，∆t後の qの値は，
q((i + 1)∆t) = q(i∆t) + 16(k1 + 2k2 + 3k3 + k4)
k1 = ∆tf(q(i∆t))
k2 = ∆tf(q(i∆t) + 12k1)
k3 = ∆tf(q(i∆t) + 12k2)
k4 = ∆tf(q(i∆t) + k3)
(8.58)
と与えられ，上式を∆t毎に繰り返し計算することでq(t0), q(t0+∆t), q(t0+2∆t), · · ·と q(t)の解軌道が得られる．
２階常微分方程式の解法については，次のような２つの連立１階常微分方程式
に分解して解くことができる．
dq1
dt= f1(q1, q2) ,
dq2
dt= f2(q1, q2) (8.59)
が与えられるとき，
q1((i + 1)∆t) = q1(i∆t) + 16(k11 + 2k12 + 2k13 + k14)
q2((i + 1)∆t) = q2(i∆t) + 16(k21 + 2k22 + 2k23 + k24)
}(8.60)
k11 = ∆tf1(q1(i∆t), q2(i∆t))
k21 = ∆tf2(q1(i∆t), q2(i∆t))
k12 = ∆tf1(q1(i∆t) + 12k11, q2(i∆t) + 1
2k21)
k22 = ∆tf2(q1(i∆t) + 12k11, q2(i∆t) + 1
2k21)
k13 = ∆tf1(q1(i∆t) + 12k12, q2(i∆t) + 1
2k22)
k23 = ∆tf2(q1(i∆t) + 12k12, q2(i∆t) + 1
2k22)
k14 = ∆tf1(q1(i∆t) + k13, q2(i∆t) + k23)
k24 = ∆tf2(q1(i∆t) + k13, q2(i∆t) + k23)
(8.61)
と求めることができる．プログラムを作成するときは，式 (3.60)に先だって式
(3.61)を計算しておく．
80
第 9章
付録
付録 1ω × {[n, o, a]l} = [ω × n, ω × o, ω × a, ]l の証明
ω =
ωx
ωy
ωz
, n =
nx
ny
nz
, o =
ox
oy
oz
, a =
ax
ay
az
, l =
lxlylz
(9.1)
とするとき
左辺 = ω × {[n, o, a]l}
=
ωx
ωy
ωz
nxlx + oxly + axlznylx + oyly + aylznzlx + ozly + azlz
=
ωy(nzlx + ozly + azlz) − ωz(nylx + oyly + aylz)
ωz(nxlx + oxly + axlz) − ωx(nzlx + ozly + aalz)
ωx(nxlx + oxly + axlz) − ωy(nxlx + oxly + axlz)
=
ωynz − ωzny ωyoz − ωzoy ωyaz − ωzay
ωznx − ωxnz ωzox − ωxoz ωzax − ωxaz
ωxny − ωynx ωxoy − ωyox ωxay − ωyax
lxlylz
= [ω × n, ω × o, ω × a, ]l (9.2)
よって，証明された．
付録 2式 (2.58)の証明は，剛体の角運動量Lの定義より，慣性テンソルが規定される
ことから考える必要がある．Lは，
L =
∫∫∫( W r × W r)ρdxdydz (9.3)
と定義され，W rは剛体の回転角速度W ωにより生じる速度である．ここで×はベクトルの外積を表わす．したがって，
W r = W ω × W r
81
である．よって，Lは，Eを単位行列とすると，
L =
∫∫∫( W r × ( W ω × W r))ρdxdydz
=
∫∫∫( W rT W r W ω − W r W rT W ω)ρdxdydz
=
∫∫∫( W rT W rE − W r W rT )ρdxdydz · ω
= W Iω (9.4)
と表わされる．つまり，W Iの定義は，
W I =
∫∫∫( W rT W rE − W r W rT )ρdxdydz (9.5)
である．上式より式 (2.51)～(2.57)が得られる．ここで上の変形
W r × ( W ω × W r) = ( W rT W rE − W r W rT )ω (9.6)
を証明しておく．W r = [x, y, z]T，W ω = [ωx, ωy, ωz]T とすると，
W r × ( W ω × W r) =
∣∣∣∣∣∣∣
i j k
x y z
ωyz − ωzy ωzx − ωxz ωxy − ωyx
∣∣∣∣∣∣∣= [ωx(y
2 + z2) − ωyxy − ωzzx]i
+[−ωxxy + ωy(z2 + x2) − ωzyz]j
+[−ωxzx − ωyyz + ωz(x2 + y2)]k
=
ωx(y2 + z2) − ωyxy − ωzzx
−ωxxy + ωy(z2 + x2) − ωzyz
−ωxzx − ωyyz + ωz(x2 + y2)
=
y2 + z2 −xy −zx
−xy z2 + x2 yz
−zx −yz x2 + y2
ωx
ωy
ωz
=
x2 + y2 + z2 0 0
0 x2 + y2 + z2 0
0 0 x2 + y2 + z2
−
x2 xy zx
xy y2 yz
zx yz z2
ωx
ωy
ωz
= W rT W rE − W r W rT (9.7)
以上で式 (4.6)が確かめられた．
さて，W Riは，式 (1.4)で示すように座標系を変換する．ΣgiとΣW との変換
は，ΣgiとΣiが同じ姿勢を持つことから，
W Riで行うことができる．Σgiで表わ
されたベクトルを irと表わすと，これをΣW に基づく表現W rへの変換は，
W r =W Riir (9.8)
82
と表わされ，これを式 (4.5)に代入してみる．
W I =
∫∫∫( irT W RT
iW Ri
irE − W Riir irT W RT
i )ρdxdydz
=
∫∫∫( irT irE − W Ri
ir irT W RTi )ρdxdydz)
=
∫∫∫( W Ri
W RTi
irT ir W RiW RT
i − W Riir irT W RT
i )ρdxdydz
重積は irに関する積分でありW Riは定数として取り扱うことができるから，上
式は
W I = W Ri
∫∫∫( W RT
iirT
iiri
W Ri − ir irT )ρdxdydz W RTi (9.9)
ここで，W RTi
irTi
iriW Ri − ir irT は，irT ir = ix2 + iy2 + iz2であるから，
W RTi
irTi
iriW Ri − ir irT
= ( ix2 + iy2 + iz2) W RTi
W Ri − ir irT
= ( ix2 + iy2 + iz2)E −
ix2 ix iy ix iziy ix iy2 iy iziz ix iy ix iz2
=
iy2 + iz2 − ix iy − ix iz
− iy ix ix2 + iy2 iy iz
− ix iz − iy iz ix2 + iy2
(9.10)
以上の結果より (4.9)の重積は，
iI i =
∫∫∫( W RT
iirT ir W Ri − iri irT )ρdxdydz
=
iIxxiIxy
iIxz
iIxyiIyy
iIyz
iIzxiIzy
iIzz
(9.11)
iIxx =
∫∫∫( iy2 + iz2)ρdxdydz
iIyy =
∫∫∫( ix2 + iz2)ρdxdydz
iIzz =
∫∫∫( ix2 + iy2)ρdxdydz
iIxy = −∫∫∫
ix iyρdxdydz
iIxz = −∫∫∫
ix izρdxdydz
iIyz = −∫∫∫
iy izρdxdydz
とΣiを規準にした表現となる．この定義を用いると，式 (4.9)は，
W I i = W RiiI i
W RTi (9.12)
と表わされる．
83
付録 3運動学方程式（再掲）
W r =
W rx
W ry
W rz
=
fx(q1, q2, q3)
fy(q1, q2, q3)
fz(q1, q2, q3)
= f(q) (9.13)
式 3.1を一階微分すると
W r =d
dtf(q) =
∂f(q)
∂qT
dq
dt
=
∂
fx(q1, q2, q3)
fy(q1, q2, q3)
fz(q1, q2, q3)
∂
q1
q2
q3
dq1
dtdq2
dtdq3
dt
=
∂fx
∂q1
∂fx
∂q2
∂fx
∂q3∂fy
∂q1
∂fy
∂q2
∂fy
∂q3∂fz
∂q1
∂fz
∂q2
∂fz
∂q3
q1
q2
q3
= J(q)q (9.14)
となる．J(q)はヤコビ行列．これは手先速度と関節角速度の関係を表す．
_rd
移動可
移動不可jJ(q)j=0JÄ1
となりが存在しない
特異姿勢
※疑似逆行列 を用いて表現できる場合もある
J#
図 9.1:
付録 4W RAは，
W RA =[
W xAW yA
W zA
](9.15)
W xAはΣW から見たΣAの x軸単位ベクトル．W yAはΣW から見たΣAの y軸単位ベクトル．W zAはΣW から見たΣAの z軸単位ベクトル．
84
xW
zW
yW
PW
xAzA
yA
PA
90°
y軸を中心に90度回転
2640
0
1
375
2640
1
0
375
2641
0
0
375
2641
0
0
375
2640
0
Ä1
375
2640
1
0
375
WRA=
2640 0 1
0 1 0
Ä1 0 0
375
図 9.2:
付録 53章式（3.52）について
W R3 = W R1(q1)1R2(q2)
2R3(q3)W r3 = W rW,1 + W R1(q1)
1r1,2 + W R1(q1)1R2(q2)
2r2,3
}
ここで
W R1(q1)1r1,2 =W r1,2
W R1(q1)1R2(q2)
2r2,3 =W r2,3
3章式 (3.61)
RT r =
xT
yT
zT
r =
xT r
yT r
zT r
=
rT x
rT y
rT z
において内積の計算の確認．
(a, b) = (b, a) (9.16)
aT b = a1b1 + a2b2 + a3b3 (9.17)
bT a = b1a1 + b2a2 + b3a3 (9.18)
となり，等しいことが明らか．同様に考えて 3章式 (3.61)のように変形できる．
85
付録 6
���������
���������
×
+-
��� ��
=
����� − ���������� − ���������� −�����
�� � =
�� × ��
外積：
(4.10)
x
zy
!
0
0
1
W!i =
rv= r!
1
0
0
0
1
0
v=r×! !
r
v
・・・・
・i-thリンク
Wxi
Wzi Wyi W _xi
�� �：∑ の� �軸先端の速度
�� �=
図 9.3:
W _rg1
W _rg2
W _rg3
W _r3W _r2
W _r1=0
W!1
W!2
W!3
W _rH
Wr2;2gPW
P1
P2
P3
∑ で� 表した∑ の� 原点からリンク2の重心までの位置ベクトル
図 9.4:
86
付録 7
x
z y
x
z y
PW
W!
x
z y
PW
y
i-thリンク
質量要素dm
�
�������
�= 定数 �� = 変数
�� = ���(� ��� +� ��� )��非対角項が影響を及ぼさない
l2=W r2zi+W r2xi
図 9.5:
�
��
� = �� +���
m
図 9.6:
87