第4章ロボットアームの運動学

第 4 章ロボットアームの運動学
4.1
はじめに
前章で示したように、ロボットの作業はロボットハンドの同次座標変換行列を用いて簡便に記述で
きる。しかし第２章で見たように、ロボットアームは関節（回転関節あるいは並進関節）を駆動して
作業を行う。それ故ロボットを用いて作業するためには、関節変数値（回転角度あるいは並進変位）
からロボットハンドの現在の配置を計算したり、与えられた同次座標変換行列の配置にロボットハン
ドを移動する関節変数値を求めることが必要となる。ロボットの関節変数値からロボットハンドの配
置を計算する問題は順運動学問題と言われ、与えられたロボットハンドの配置を達成する関節変数値
を求める問題は逆運動学問題といわれる。
より具体的に考えるために Fig. 4.1(a) を考える。図よりロボットハンドの配置は
BASE
HAND = BASE ROBOT ROBOT ARM ARM HAND
(4.1)
で与えられる。ハンド座標系は一般に次のように定義される（ Fig. 4.1(b) 参照）
。
n s a p
BASE
HAND =
0 0 0 1
(4.2)
ここで、a はハンドの接近方向ベクトル (approach vector) 、s は開閉方向ベクトル (open or slide
vector)（初期には o が用いられたが零と紛らわしいので改められた）、n は法線方向ベクトル (normal
vector) といわれる。この中で、一般に BASE ROBOT と ARM HAND は定数行列である。ROBOT ARM
は関節変数の値によって定まる行列であり、与えられた関節角に対してこの行列を計算する問題は順
運動学問題といわれる。
次に物体 BOX を GRASP の配置で把握するためには、
BASE
ROBOT ROBOT ARM ARM HAND = BASE TABLE TABLE BOX BOX GRASP
(4.3)
を解かなければならない。テーブルと物体が静止しているならば BASE TABLE 、TABLE BOX 、BOX GRASP
も既知の定数行列となり、ROBOT ARM のみが未知の行列となる。上式を
ROBOT
ARM について解
くと
ROBOT
ARM = ROBOT BASE BASE TABLE TABLE BOX BOX GRASP HAND ARM
(4.4)
となり、この物体を把握するためのロボットハンドの目標配置を求めることができる。この与えられ
た目標配置を達成する関節変数値を求める問題が逆運動学問題である。逆運動学問題は順運動学問
題の解を用いて効率的に解くことができるので、以下ではまず順運動学問題を考察し、次いで逆運動
学問題を解く。その際、関節変数としては回転関節に対しては回転角度であり並進関節に対しては並
進変位となる。そこで関節変数一般を表す記号としては qi を用いる事とする。
45
46
6
HAND
ROBOT
GRASP
@
@ ARM
""
"
@
"
6
?
?
BASE
BOX 6
- *
6* " "
?
HAND
TABLE
(a)
6
n
- a
o
S
S
S
s
S
6
p S
S
BASE
(b)
図 4.1: 作業の記述とハンド座標系
4.2
順運動学
順運動学問題は各関節に関節座標系を設定することによって解かれる。まず平面ロボットアームで
考え方を示した後、３次元ロボットアームに対する順運動学問題を考察する。
4.2.1
平面ロボットアームの順運動学
ここでは Fig. 4.2 の n 自由度回転関節平面アームを考える。次の手順で関節座標系を設定する。
1. 基盤側から手先に向かって関節に番号 i = 1, 2, . . . , n を付ける。
2. 各関節 i に原点 Oi をとる。
3. 基盤をリンク 0 とし、Oi と Oi+1 の間のリンクをリンク i とする。リンク n は存在しない。
4. i = 1, 2, . . . , n − 1 に対して原点 Oi から Oi+1 方向に xi 軸をとる。
5. i = 1, 2, . . . , n − 1 に対して右手系を成すように yi 軸をり、それを関節座標系 Σi とする。
6. Σ0 座標系を θ1 = 0◦ とした時の Σ1 座標系と等しく取る。
7. θn = 0◦ の時方向が xn−1 軸の方向と等しくなるように原点 On に xn 軸をとる。右手系を成す
ように yn 軸を定め Σn 座標系とする。
リンクパラメータを次のように定義する。
リンク長さ ai
：
原点 Oi から Oi+1 までの長さ
θi
：
xi−1 軸と xi 軸のなす角
関節角この時 Σi−1 から Σi へは次の２つの移動で移ることができる。
1. Σi−1 の xi−1 軸に沿って ai−1 進む。それを Σai−1 とする。
2. Σai−1 の原点周りに θi 回転する。
ここで、ai−1 は定数であり θi は関節角度を表す変数で関節変数（ qi = θi ) となる。以上の操作は同
次座標変換行列によって、それぞれ Trans(ai−1 , 0) と Rot(θi ) と表されるので
i−1
Ti
= Trans(ai−1 , 0)Rot(θi )
(4.5)
Intelligent Machine Lab. Meiji univ.
6
47
xn xn−1
e
e On−1
C
On
C
C
Ce O
3
y0
x2
6
x1 e O
2
1e
O0 , O1 x0
0i+1
e
xi θi
ai−1
e
:
e Oi
xi−1
Oi−1
(a)
(b)
図 4.2: 平面アームと関節座標系

Ci

=  Si
0
−Si
Ci
0

ai−1

0 
1
(4.6)
となる。なお、0 T 1 に対してはその設定法から、a0 = 0 となる。i−1 T i は第 i − 1 関節座標系に関す
る第 i 関節座標系の配置を与える。また、連続する自然数 i, i + 1, i + 2, . . . , j − 1, j に対して
i
T j = i T i+1 i+1 T i+2 · · · j−1 T j
(4.7)
の記法を用いる。この記法に従うと ROBOT ARM = 0 T n となる。
例題 4.2.1 (平面ロボットアームの順運動学) Fig.4.3 の３関節平面ロボットを考えると、a0 = 0, a1 =
L1 , a2 = L2 となる。よって、

0
2
T1
T3
=
=
となるので、
C1

 S1
0

C3

 S3
0
−S1
C1
0
−S3
C3
0
0

0 ,
1

L2

0 
1

ROBOT

C123

ARM = 0 T 3 =  S123
0

1
C2

T 2 =  S2
0
−S2
C2
0
L1


0 ,
1
(4.8)
−S123
C123
0
L1 C1 + L2 C12


L1 S1 + L2 S12 
1
(4.9)
を得る。ここで、C123 = cos(θ1 + θ2 + θ3 ) であり、他も同様である。さらに、
BASE
ROBOT = Trans(L0 , 0), ARM
Hand = Trans(L3 , 0)
(4.10)
48
図 4.3: ３関節平面アーム
とすると、
BASE
HAND =
=
BASE

ROBOT 0 T 3
C123

 S123
0
ARM
Hand −S123
L0 + L1 C1 + L2 C12 + L3 C123
C123
0
L1 S1 + L2 S12 + L3 S123
1



(4.11)
となる。//
逆運動学を考えるときには 2 T 3 = Trans(a2 , 0)Rot(θ3 ) より 0 T 3 を以下のように分割しておくのが
便利である。
0
T 3 = 0 T̄ 2 2 T̄ 3
(4.12)
ここで、

0
2
T̄ 2
T̄ 3
=
=
C12

0
T 2 Trans(a2 , 0) =  S12
0

C3 −S3 0

Rot(θ3 ) =  S3 C3 0
0
0
−S12
C12

0

L1 C1 + L2 C12

L1 S1 + L2 S12 
1


(4.13)
(4.14)
1
であって、0 T̄ 2 は腕部の同次座標変換行列を表し、2 T̄ 3 は手首部のそれを表していると考えることが
できる。なお、0 T̄ 2 は θ3 = 0 としたときの 0 T 3 と等しい。
4.2.2
３次元ロボットアームの順運動学
３次元 n 関節ロボットアームにおける順運動学問題の解法も平面ロボットアームのそれとほぼ同
様である。以下にその手順を示す。まず関節座標系の設定を行う（図 4.4 参照）
。
1. 基盤側から手先に向かって関節に番号 i = 1, 2, . . . , n をつける。
2. 基盤をリンク 0 とし第 i 関節と第 i + 1 関節の間のリンクをリンク i とする。リンク n は存在
しない。
Intelligent Machine Lab. Meiji univ.
49
3. 第 i 関節が回転関節のときは回転軸に、並進関節のときは並進軸に zi 軸をとる。
4. zi 軸と zi+1 軸の共通垂線の zi 軸上の足を原点 Oi とする。zi 軸と zi+1 軸が平行の時は適当な
位置に定める。
5. i = 1, 2, . . . , n − 1 に対して原点 Oi から共通垂線に沿って zi+1 軸方向に xi 軸をとる。
6. i = 1, 2, . . . , n − 1 に対して右手系を成すように yi 軸をとり、それを関節座標系 Σi とする。
7. Σ0 座標系を第１関節の関節変位が零の時の Σ1 座標系にとる。
8. On を zn−1 軸と zn 軸の共通垂線の zn 軸側の足にとり、xn 軸の方向を第 n 関節の関節変位が
零の時の xn−1 軸の方向と等しくなるようにとる。右手系を成すように yn 軸を定め関節座標系
Σn とする。
リンクパラメータを次のように定める（図 4.5 参照）
。
ai−1
αi−1
リンク長さ
リンクねじれ角
di
θi
関節オフセット
関節角
：
：
：
：
zi−1 軸と zi 軸の共通垂線の長さ（距離）
xi−1 軸まわりに測った zi−1 軸と zi 軸のなす角（ねじれ角）
xi−1 軸と xi 軸の共通垂線の長さ（距離）
zi 軸まわりに測った xi−1 軸と xi 軸のなす角（ねじれ角）
ここで、ai−1 と αi−1 はリンクの定数パラメータであり、a0 = 0 、α0 = 0 となる。関節パラメータ
di と θi については、もし関節 i が回転関節であれば di は定数となり、θi が関節変数（ qi = θi ）とな
る。並進関節の時は di が関節変数（ qi = di ）となり θi は定数となる。この時、Σi−1 から Σi までは
次のような４つの連続する操作で移動できる（図 4.6 参照）
。
1. Σi−1 の xi−1 軸に沿って ai−1 だけ並進する。それを Σai−1 とする。
2. Σai−1 の xai−1 軸回りに αi−1 だけ回転する。それを Σbi−1 とする。
b
3. Σbi−1 の zi−1
軸に沿って di だけ並進する。それを Σci−1 とする。
c
4. Σci−1 の zi−1
軸回りに θi だけ回転する。
第１の操作は Trans(ai−1 , 0, 0) で表され、２、３、４の操作はそれぞれ、Rot(x, αi−1 ) 、Trans(0, 0, di ) 、
Rot(z, θi ) で表せる。それ故、Σi−1 座標系に関する Σi 座標系の配置を表す同次座標変換行列 i−1 T i
は次のように与えられる。
i−1
Ti
=
=
Trans(ai−1 , 0, 0)Rot(x, αi−1 )Trans(0, 0, di )Rot(z, θi )


Ci
−Si
0
ai−1


 Cα Si Cα Ci −Sα −Sα di 


 S S S C
Cα
Cα di 
α i

 α i
0
0
0
1
(4.15)
ここで、Si = sin θi 、Ci = cos θi 、Sα = sin αi−1 、Cα = cos αi−1 である。以上の記法をＤ-Ｈ
(Denabit-Hartenberg) 記法と呼ぶ。
4.2.3
Puma 型ロボットアームの順運動学
Puma 型ロボットアームは関節型アームと Euler 型手首機構を組み合わせて構成した６自由度ロ
ボットアームである。関節座標系を Fig. 4.7 の様に定めると、リンクパラメータは Table 4.1 の様に
なる。なお、θi の欄の値はこの姿勢に対応する関節角度を表している。関節は全て回転型であるの
50
図 4.4: 関節座標系の設定
図 4.5: リンクパラメータの設定
図 4.6: 同次座標変換行列
Intelligent Machine Lab. Meiji univ.
51
で関節変数として qi = θi をとると、各 i−1 T i 行列は次のようになる。



C1 −S1 0 0
C2 −S2 0






S1 C1 0 0 
0
0
1
0
, 1T 2 = 
T1 = 

 0

0
1 0 

 −S2 −C2 0

2
4
T3
T5
0
C3

 S3
= 
 0

0

C5

 0
= 
 S
 5
0
0
0 1
−S3
C3

0 Lc

0 0 
, 1 0 

0 1

0 0

−1 0 
, 0 0 

0 1
0
0
−S5
0
C5
0
よって、ROBOT ARM 同次座標変換行列は
ROBOT

0
0
C4 −S4


0
0
3
T4 = 
 −S −C
4
4

0
0

C6 −S6


0
0
5
T6 = 
 −S −C
6
6

0
0
0
Lb − Ld
0
0



 

1
0
1
0
0
0
1
0
0

Le

Lf 
 0 

1

0

0 

0 

1
(4.16)
ARM = 0 T 6 = 0 T 1 1 T 2 · · · 5 T 6 で計算できるが、
順運動学を解く際には、
3
T 4 = Trans(a3 , 0, 0)Rot(x, α3 )Trans(0, 0, d4 ) · Rot(z, θ4 )
(4.17)
に着目して、次のように表しておくのが便利である。
0
T 6 = 0 T̄ 3 · 3 T̄ 6
(4.18)
ここで、
0
T̄ 3
=
=
3
0
T 3 Trans(a3 , 0, 0)Rot(x, α3 )Trans(0, 0, d4 )


C1 C23 S1 −C1 S23 px


 S1 C23 −C1 −S1 S23 py 


 −S
0
−C23
pz 
23


0
0
0
1
(4.19)
T̄ 6
=
Rot(z, θ4 )4 T 6 = Euler(θ4 , −θ5 , θ6 )
(4.20)
px
=
C1 (Lc C2 + Le C23 − Lf S23 ) − (Lb − Ld )S1
(4.21)
py
=
S1 (Lc C2 + Le C23 − Lf S23 ) + (Lb − Ld )C1
(4.22)
pz
=
−Lc S2 − Le S23 − Lf C23
(4.23)
である（ Rot(x, 90◦ )Rot(z, θ5 )Rot(x, −90◦ ) = Rot(y, −θ5 ) であることに注意せよ）。なお、0 T̄ 3 は
θ4 = 0 のときの 0 T 4 と同一である。//
4.3
4.3.1
逆運動学問題
はじめに
第１節で示したように、ロボットの操作は例えば
BASE
TROBOT 0 T n ARM HAND = BASE TABLE TABLE BOX BOX GRASP
(4.24)
52
図 4.7: Puma 型ロボットアームの関節座標系
Intelligent Machine Lab. Meiji univ.
i
ai−1
53
αi−1
◦
di
θi
0
θ1 + 0 ◦
1
0
0
2
0
−90◦
Lb − Ld
θ2 + 0 ◦
3
Lc
0◦
0
θ3 + 0 ◦
4
Le
−90◦
Lf
θ4 + 0 ◦
5
0
90◦
0
θ5 + 0 ◦
0
θ6 + 0 ◦
6
0
−90
◦
表 4.1: Puma 型ロボットアームのパラメータ
となるように 0 T n を定めることに等しい。前節で述べたように、0 T n は q1 , q2 , . . . , qn（ qi = θi または
qi = di ）の関数となる。ロボットは実際には関節変数を制御して駆動するので、0 T n から q1 , q2 , . . . , qn
を求めることが必要となる。
順運動学では q1 , q2 , . . . , qn からただ一つの 0 T n が定まったが、逆運動学では 0 T n に対して複数個
の解が存在するのが普通である。例えば、後で見るように３自由度の平面ロボットアームでは２組の
解が存在し、６自由度の３次元回転関節ロボットアームでは８組の解が存在する。また、逆運動学問
題の場合、解が常に存在するとは限らないし、もし存在したとしても解析的に求められるとは限らな
い。６自由度の回転関節ロボットアームに関しては、解析的な解が存在するための十分条件として以
下のものが知られている。すなわち、以下の条件のうち少なくともいずれか一方が成立するすれば解
析的な解が存在する。
1. ひと続きの３つの関節軸が一点で交わる。
2. ひと続きの３つの関節軸が平行である。
(1) の成立するアームとしては Puma 型アームがあり、(2) の成立するアームのしては T 3 型アーム
がある。
さて、逆運動学問題の解法としては、
1. 幾何学的解法
2. 代数学的解法
3. 繰り返し手法による解法
がある。幾何学的解法は図式的に解を求めるもので、解の存在範囲や解とそれに対するロボットアー
ムの形状の対応が明確であるが、発見手法的に解を求めるため幾分の経験を要し、また注意して全て
の解を漏れなく求める必要がある。それに対して、代数学的解法は系統的に全ての解を求めることが
できるが、得られた解の存在条件やその解の対応するロボット姿勢との直感的な対応付けが困難とな
る。繰り返し手法は通常初期値に近い単一の解のみを求めることができる。以下では平面アームに対
する幾何学的解法と３次元アームに対する代数学的解法を示す。
4.3.2
準備
ここで以下でしばしば利用する関数 atan2(a, b) について説明しておく。通常の逆正接関数 arctan(c)
は tan(θ) = c となる −90◦ ≤ θ < 90◦ の値を返すが、atan2(a, b) は複素数 b + aj の偏角 −180◦ ≤
54
θ < 180◦ を返す。よって、
atan2(sin θ, cos θ)
atan2(−a, b)
atan2(a, −b)
= θ
(4.25)
= −atan2(a, b)
◦
= ±180 − atan2(a, b)
atan2(−a, −b) = atan2(a, b) ± 180◦
(4.26)
(4.27)
(4.28)
であり、さらに任意の正の数 α に対して
atan2(αa, αb) = atan2(a, b)
(4.29)
が成立する。また、(b + aj)(d + cj) = (bd − ac) + (bc + ad)j および Eq.(4.27) より、
atan2(a, b) + atan2(c, d) =
atan2(bc + ad, bd − ac)
(4.30)
atan2(a, b) − atan2(c, d) =
atan2(−bc + ad, bd + ac)
(4.31)
となる。
また、次の三角関数の計算を多用する。
A sin θ + B cos θ = C sin(θ + ϕ) = C cos(θ − ψ)
(4.32)
ここで、
A2 + B 2
(4.33)
ϕ =
atan2(B, A)
(4.34)
ψ
atan2(A, B)
(4.35)
C
=
=
である。
4.3.3
平面ロボットアームの逆運動学（幾何学的解法）
例題 4.2.1 においてハンドの目標配置が Ĥ で与えられてるとする。BASE ROBOT 0 T 3 ARM HAND =
Ĥ とする θ1 , θ2 , θ3 を求める問題を考えてみよう。これは、上で述べたように、
0
T 3 (θ1 , θ2 , θ3 ) = ROBOT BASE Ĥ HAND ARM ≡ 0 T̂ 3
(4.36)
となる θ1 , θ2 , θ3 を求める問題へ変換される。 0 T̂ 3 を改めて

0
nx

T̂ 3 =  ny
0
sx
sy
0
px


py 
1
(4.37)
と書くことにする。
図 4.8 をもとに考える。図より解の存在条件は
|L1 − L2 | ≤ p2x + p2y ≤ L1 + L2
(4.38)
Intelligent Machine Lab. Meiji univ.
55
図 4.8: 右手型と左手型
となる。図に示すように解には右手型（実線）と左手型（破線）の２種類がある。
まず右手型の解を求めてみよう。余弦定理によって
p2x + p2y = L21 + L22 − 2L1 L2 cos φ
(4.39)
より
L21 + L22 − p2x − p2y
≡a
(4.40)
2L1 L2
√
√
となる。このとき、0 ≤ φ ≤ 180◦ より、sin φ = 1 − a2 となりよって、φ = atan2( 1 − a2 , a) を得
る。これより、
θ2 = 180◦ − atan2( 1 − a2 , a)
(4.41)
cos φ =
となる。同様にして左手型の解は
θ2 = 180◦ + atan2(
1 − a2 , a)
となる。よって、形状パラメータ κ を導入することによって、右手型と左手型の解を
θ2 = 180◦ − κ atan2( 1 − a2 , a)
(4.42)
(4.43)
と表すことができる。ここで、κ = 1 のとき右手型の形状を与え、κ = −1 のとき左手型の形状を与
える。
次に、θ1 は p2x + p2y = 0 のときには
L2 sin θ2
L1 + L2 cos θ2
sin ψ = , cos ψ = p2x + p2y
p2x + p2y
(4.44)
θ1 = atan2(py , px ) − atan2(L2 sin θ2 , L1 + L2 cos θ2 )
(4.45)
より、
となる。p2x + p2y = 0 のときは L1 = L2 の時のみ解が存在し、θ1 は任意で θ2 = ±180◦ である。最後
に、θ3 は
θ3 = atan2(ny − sx , nx + sy ) − θ1 − θ2
(4.46)
で与えられる。このように幾何学的解法を用いると、解の存在条件が明確であり ( Eq.(4.38)) 、アー
ムの形状をパラメータによって指定できる ( Eq. (4.43)) が、解法が発見手法的であることがわかる。
56
4.3.4
３次元ロボットアームの逆運動学（代数学的解法）
ここでは、代数学的解法を用いて３次元ロボットアームの逆運動学問題を解く。そのために、まず
Euler 角や RPY 角の逆運動学問題を解き、その後、Puma 型ロボットアーム（ひと続きの３つの関
節軸が一点で交わる）の逆運動学問題を解く。
(a) Euler 角の逆運動学
第３章において Euler 角による姿勢表示は次のように与えられている。
Euler(φ, θ, ψ) = Rot(z, φ)Rot(y, θ)Rot(z, ψ)
そこで、

nx

 ny
Euler(φ, θ, ψ) = 
 n
 z
0
sx
ax
0
sy
sz
ay
az
0
0
0
0
1
(4.47)






(4.48)
となる φ, θ, ψ を求めてみよう。そのために、Eq. (4.47) に注意して、Eq. (4.48 ）の左から Rot−1 (z, φ)
を乗じると

Cθ Cψ

 Sψ

 −S C
θ ψ

0
−Cθ Sψ
Cψ
Sθ
0
Sθ Sψ
0
Cθ
0
 
fx (n)
0
 

0 
 =  fy (n)

0  
 fz (n)
1
0
fx (s)
fy (s)
fx (a)
fy (a)
fz (s)
0
fz (a)
0

0

0 

0 

1
(4.49)
となる。ここで、
fx (ξ)
=
ξx Cφ + ξy Sφ
(4.50)
fy (ξ)
=
−ξx Sφ + ξy Cφ
(4.51)
fz (ξ)
=
ξz , ξ = n, s, a
(4.52)
である。
a2x + a2y = 0 の時には、第 (2,3) 要素に着目して、
−ax Sφ + ay Cφ = 0 =⇒ tan φ =
ay
ax
or
−ay
−ax
(4.53)
を得る。よって、
φ = arctan(ay /ax ) または φ = arctan(ay /ax ) + 180◦
(4.54)
である。さらに、第 (1,3) 、(3,3) 要素、および、第 (2,1) 、(2,2) 要素より、
となる。
θ
= atan2(ax Cφ + ay Sφ , az )
(4.55)
ψ
= atan2(−nx Sφ + ny Cφ , −sx Sφ + sy Cφ )
(4.56)
Intelligent Machine Lab. Meiji univ.
57
a2x + a2y = 0 の時には az = ±1 であるから、それに従って
φ
= 任意
(4.57)
θ
= 90◦ − 90◦ × az
(4.58)
ψ
= atan2(−nx Sφ + ny Cφ , −sx Sφ + sy Cφ )
(4.59)
となる。
(b) RPY 角の逆運動学
第３章において RPY 角による姿勢表示は次のように与えられている。
RPY(α, β, γ) = Rot(z, α)Rot(y, β)Rot(x, γ)
ここでも同様に、

nx

 ny
RPY(α, β, γ) = 
 n
 z
0
sx
sy
ax
ay
sz
0
az
0

0

0 

0 

1
(4.60)
(4.61)
とする。Euler 角の場合と全く同様に左から Rot−1 (z, α) を乗じることによって、

 

Cβ Sβ Sγ Sβ Cγ 0
fx (n) fx (s) fx (a) 0

 

 0


Cγ
−Sγ 0 

 =  fy (n) fy (s) fy (a) 0 
 −S


Cβ Sγ Cβ Cγ 0   fz (n) fz (s) fz (a) 0 
β


0
0
0
1
0
0
0
1
(4.62)
となる。ここで、
fx (ξ)
=
ξx Cα + ξy Sα
(4.63)
fy (ξ)
=
−ξx Sα + ξy Cα
(4.64)
fz (ξ)
=
ξz , ξ = n, s, a
(4.65)
である。
n2x + n2y = 0 の時には、第 (2,1) 要素に着目して、
−nx Sα + ny Cα = 0 =⇒ tan α =
ny
nx
or
−ny
−nx
(4.66)
を得る。よって、
α = arctan(ny /nx ) または α = arctan(ny /nx ) + 180◦
(4.67)
である。さらに、第 (1,1) 、(3,1) 要素、および、第 (2,2) 、(2,3) 要素より、
となる。
β
= atan2(−nz , nx Cα + ny Sα )
(4.68)
γ
= atan2(ax Sα − ay Cα , −sx Sα + sy Cα )
(4.69)
58
n2x + n2y = 0 の時には nz = ±1 であるから、それに従って
α
= 任意
(4.70)
◦
β
= −90 × nz
(4.71)
γ
= atan2(ax Sα − ay Cα , −sx Sα + sy Cα )
(4.72)
となる。
(c) Puma 型アームの逆運動学
Puma 型アームに対しては、例題 4.2.3 で示したように
0
と表される。ここで、
0
3
T̄ 3
T̄ 6
=
T 6 = 0 T̄ 3 · 3 T̄ 6
R1
0
p1
1
T6 =
(4.74)
= Euler(θ4 , −θ5 , θ6 ) =
である。よって、平面ロボットアームの場合と同様に、

0
(4.73)
R1 R2
0
nx


p1
ny
= 0 T̂ 6 ≡ 

1
 nz
0
R2
0
0
1
sx
sy
ax
ay
sz
0
az
0
(4.75)

px

py 

pz 

1
(4.76)
となる。
T
T
0 −1
T
これより、(pT1 , 1)T = (px , py , pz , 1)T 、さらに、0 T −1
1 (p1 , 1) = T 1 (px , py , pz , 1) が成立する
ことが分かる。よって次式が成立する。

 

Le C23 − Lf S23 + Lc C2
px C1 + py S1

 

Lb − Ld

 =  −px S1 + py C1 
−Le S23 − Lf C23 − Lc S2
(4.77)
pz
これより、p2x + p2y = 0 のとき −px S1 + py C1 = Lb − Ld より φ = atan2(py , px ) とすると、
Lb − Ld
sin(φ − θ1 ) = ≡a
p2x + p2y
となる。よって解が存在するためには
Lb − Ld ≤ 1 または |Lb − Ld | ≤ p2x + p2y
|a| = p2x + p2y √
が必要であることが分かる。この時、cos(φ − θ1 ) = ± 1 − a2 より
θ1 = atan2(py , px ) − atan2(a, ± 1 − a2 )
(4.78)
(4.79)
(4.80)
Intelligent Machine Lab. Meiji univ.
59
となる。p2x + p2y = 0 のときは Lb = Ld の時のみ解が存在して、θ1 は任意になる。
次に Eq.(4.77) において b = px C1 + py S1 とすると
Lc C2
= b − Le C23 + Lf S23
(4.81)
L c S2
= −pz − Le S23 − Lf C23
(4.82)
より、(4.81)2 +(4.82)2 から
L2c = b2 + p2z + L2e + L2f + 2(Le pz + bLf )S23 + 2(Lf pz − bLe )C23
(4.83)
を得る。これより p2x + p2y + p2z = 0 のとき
(Le pz + bLf )S23 + (Lf pz − bLe )C23 =
L2c − b2 − p2z − L2e − L2f
2
(4.84)
となるので、
η
c
= atan2(Lf pz − bLe , Le pz + bLf )
L2c − b2 − p2z − L2e − L2f
=
2 (Le pz + bLf )2 + (Lf pz − bLe )2
(4.85)
とすると、
sin(θ2 + θ3 + η) = c
(4.86)
L2c − b2 − p2z − L2e − L2f
|c| = ≤1
2 (Le pz + bLf )2 + (Lf pz − bLe )2 となるので、
のとき解が存在して、
ζ = θ2 + θ3 = atan2(c, ± 1 − c2 ) − η
(4.87)
(4.88)
となる。よって、Eqs. (4.81), (4.82), (4.88) から
θ2
=
atan2(−pz − Le Sζ − Lf Cζ , b − Le Cζ + Lf Sζ )
(4.89)
θ3
=
ζ − θ2
(4.90)
となる。p2x + p2y + p2z = 0 の時は L2c = L2e + L2f の時のみ解が存在する。その時 ζ = θ2 + θ3 は任意
となり、
θ2
=
atan2(−Le Sζ − Lf Cζ , −Le Cζ + Lf Sζ )
(4.91)
θ3
=
ζ − θ2
(4.92)
となる。
次に手首の３関節を求める。Eq. (4.76) より
3
0
T̄ 6 = T̄ 3
−1 0
T̂ 6 =
RT1 0 R̂6
0
0
1
(4.93)
となる。右辺の行列は既知であるので、3 T̄ 6 = Euler(θ4 , −θ5 , θ6 ) であることに注意して、上式を
Eq. (4.48) と対比することによって、Eqs. (4.54) -(4.59) から２組の解 θ4 , θ5 , θ6 を得る。よって、
Eqs.(4.80) と (4.88) の複号を考慮すると Puma 型ロボットアームには合計８組の解が存在すること
となる。
以上のように、Puma 型ロボットアームに対して８組の解が得られた。これらの解は
60
腕形状
：右手型 (Right Arm) 、
左手型 (Left Arm)
肘形状
：上肘型 (Above Elbow) 、
下肘型 (Below Elbow)
手首形状
：下向き型 (Wrist Down) 、上向き型 (Wrist Up)
に対応することが知られているがその対応関係は明確ではない（図 4.9 参照）
。
4.4
おわりに
本章ではロボットアームの順運動学と逆運動学について論じた。ここでは、それらに対する２、３
のコメントを述べる。
(a) D-H 記法について
まず順運動学においては、関節に座標系を導入し、その相互関係を表す同次座標変換行列を D-H
記法に基づいて計算することで解くことができた。このように D-H 記法は便利な方法であるが欠点
もある。第１に関節座標系の場所が必ずしも物理的な関節と一致しないことである。それに関して本
文では触れなかったが、D-H 記法では、例えば第 i 関節が並進関節の時には、zi 軸はその並進方向
に平行であれさえすればどこにとってもよい。この時、リンクパラメータ ai−1 あるいは ai を零にな
るようにとることも可能であるが、パラメータの決定や動力学問題を考えるようなときには、質量が
関節部分に集中することが多いので、物理的な関節軸とできるだけ一致させておく方が便利である。
二つの関節軸が平行な場合にもその共通垂線を任意にとれるが、その時も物理的なリンクとできる
だけ一致させておく方がよい。第２の欠点はその基準姿勢である。D-H 記法では全ての関節変数値
が零の時、全ての関節座標系の x 軸が同一方向を向くことになるが、この姿勢が一般に使用者には
不自然な姿勢となることである。それは、例題 4.2.3 の図 4.7 から分かる。それ故、ロボット言語な
どを作る際には、使用者にとってわかりやすい関節変数と D-H 記法による関節変数との間の変換が
必要となることがある。
D-H 記法には本書で紹介した物と実はもう一種類ある。本書で紹介した D-H 記法はリンク i の根
元に関節 i があり、これは Craig で紹介された記法である。もう一つは Paul の導入した物でリンク
i の先端に関節 i が存在する。このとき、i−1 Ti の関節変数が qi−1 となる、リンク i の重心の位置が
負の方向にある等、直感とのずれが生じるのでしだいに使用されなくなった。ただ、Paul の D-H 記
法を用いると、例題 4.2.3 で行ったような 0 T̄3 の様な行列をわざわざ導入する必要がないという利点
がある。
(b) 逆運動学問題の解の選択について
上で示したように、同一目標配置に対して逆運動学の解は複数個あるのが普通である（図 4.9 参
照）
。現在の形状では障害物と衝突する可能性がある、あるいは、後述するようにロボットアームの
最も作業しやすい形状を選択するため等の考慮から他の解を用いるような事もあるが、その様な場
合には特別の命令を用意し、通常は現在の関節角度に最も近い解を選ぶのが普通である。
その際、一般にロボットの関節変数には取り得る範囲に限界があること、三角関数が 360◦ の周期
関数であることに注意する必要がある。例えば、現在の関節角度が 350◦ の時 185◦ と 5◦ の解が与え
られたとする。この時、もし関節が 360◦ を越えて動けるならば、逆運動学問題の解は 365◦ となる
Intelligent Machine Lab. Meiji univ.
61
図 4.9: PUMA 型アームの形状
べきである。そうでなければ、どちらの解を選んでもロボットアームが急激な動作をすることが予測
されるので、この場合の正解は 185◦ を出力し何らかの警告を返すことである。
4.5
問題
1. 図 4.10 の平面歩行ロボットにおいて
BASE
3
T0
T BODY
=
=

1 0

Trans(x, 0) =  0 1
0 0

x

0 ,
1

0
1

Trans(L3 , 0)Rot(−90◦ ) =  −1 0
0 0
L3


0 
1
と与えられている。
(a) 同次座標変換行列 i−1 T i , i = 1, 2, . . . , 5 を求めよ。なおリンクの長さは膝からかかとまで
が L1 、大腿部が L2 とする。
(b) BASE 座標系に対する BODY 座標系の配置および遊脚（接地していない脚）のつま先
TIPTOE の座標を求めよ。
2. 図 4.11 の BBR 型手首機構を持つ６軸ロボットアームについて以下の問いに答えよ。
(a) 各軸に関節座標系を設定せよ。
(b) 各リンクパラメータと関節パラメータを求めよ。
(c) 各 i−1 T i , i = 1, 2, . . . , 6 を求めよ。また PUMA 型アームにならって 0 T̄ 3 と 3 T̄ 6 を求めよ。
62
図 4.10: 平面歩行ロボット
3. 問 4.1 の平面歩行ロボットにおいて L1 = L2 = L3 = 1



1 0 10
1



BASE
BASE
T̂ 0 =  0 1 0  ,
T̂ BODY =  0
0
1
0
0
0 0
1

10
√ 
1+ 3 
1
とする。
(a)
BASE
T̂ BODY = BASE T̂ 0 0 T̂ 3 3 T BODY から 0 T̂ 3 を求めよ。
(b) 0 T 3 (θ1 , θ2 , θ3 ) = 0 T̂ 3 とする θ1 , θ2 , θ3 を求めよ。
√
√ √
(c) 遊脚のつま先の BASE 座標系に関する座標を (x, y) = (9− 2/2, 3 − 2/2) とする θ4 , θ5
を求めよ。
Intelligent Machine Lab. Meiji univ.
図 4.11: ６軸ロボットアーム
63

Download Report