計測自動制御学会東北支部 第 215 回研究集会 (2004.5.27)
資料番号 215-15

マニピュレータ逆運動学問題の補間解法に関する研究
Solving Inverse Kinematics Problem Using Interpolation Method○吉原佑器，小野幸彦，高橋隆行，中野栄二○ Yuki Yoshihara, Ono Yukihiko Takayuki Takahashi, Eiji Nakano
東北大学

Tohoku University
キーワード :

マニピュレータ (manipulator), 逆運動学 (inverse kinematics)

補間 (interpolation), セル (cell), クラスタリング (clustering)

連絡先 : 〒 980-8579 仙台市青葉区荒巻字青葉 01 東北大学大学院情報科学研究科中野研究室 (青葉山キャンパス
機械系) 吉原 佑器，Tel.: (022)217-7025， Fax.: (022)217-7023， E-mail: yoshihara@robotics.is.tohoku.ac.jp

1.

はじめに数値解法としてはヤコビアンの疑似逆行列を利マニピュレータの制御上，極めて重要な計算のひとつに逆運動学がある．これは次式のようにマ用して，式(1) を満たす解を繰返しにより導出するヤコビ法がある．これは一般的な構造のマニピュニピュレータの手先座標変換行列 H を実現する関レータに適用可能であり，汎用性問題を解決して節角 θ = (θ1 , · · · , θF ) (F ：自由度) を求める計算をいるが，局所的最小解に陥りやすく解一意性問題いい，マニピュレータの手先をある位置に，所定の姿勢で移動させる場合に欠かすことのできないがあるマニピュレータにおいてもただひとつの解しか求まらない．加えて演算時間を要するのでリアルタイムの制御には適さないという欠点がある．

制御上の重要な要素である．

Buchberger アルゴリズムは Grobner 基底を利用θ = f (H)(1)

逆運動学は通常マニピュレータの幾何学的な構造を利用して解析的に導出する場合が多い (解析法)．しかし解析法はマニピュレータの構造に依存しており，マニピュレータによって解き方が異なる上(汎用性問題)，マニピュレータの構造によっては解が存在しない，もしくは一意に定まらない場合がある (解一意性問題)．解析法に付帯するこれらの問題は逆運動学問題と呼ばれ，過去，解析法することで，どのマニピュレータに対しても同じ手順で解析解を導出することのできる方法とされており 1) ，汎用性問題を解決しているが，解一意性問題の解は与えていない．特に解が無数に存在するようなマニピュレータでは演算時間およびメモリ消費量が増大することが知られている．これ
らの方法は場合によっては強力であるが，固有の問題を抱えており，上記に挙げた2 つの問題を同時に解決しているとはいいがたい．によらない逆運動学解法が数多く研究されてきた．

–1–

そこで本研究では，上記 2 点の問題を解決する

逆運動学の構築を目標とする．具体的には，手先H-θ データセットを補間し，各セル毎に補間座標変換行列 H が与えられると，それに対応する式 θ (d) = f(d) (H)(d = 1, . . . , W ) を求める．

関節角θ を複数求めることができ，しかもマニピュレータの構造に依存しない汎用性の高い逆運動学

2.2

演算システム (Fig.1) の構築を目指す．

• selector
指定された座標変換行列 H が所属するセル

Manipulator
structureの番号 d を選択する．

θ (1)HCalculator

TargetSystem• calculator
各セルにおいて θ (d) = f(d) (H) を用い，関節θ (s)Fig. 1角 θ (d) を得る．

The schematic diagram of target system

2.3

セル分割補間法

2.

セル分割補間法の特徴

セル分割補間法は次の点で従来の逆運動学演算逆運動学の汎用性，解の一意性問題を同時に解決システムより優れた性能を示すと期待できる．

する手法としてセル分割補間法 (CDI:Cell DivisionInterpolation) を提案する．セル分割補間法とは，順運動学とクラスタリング，補間を組み合わせた

1) 順運動学のみを使用するため，順運動学が簡単に求まるほとんどの構造のマニピュレータへ適用可能 (汎用性問題の解決)．

逆運動学演算システムであり，オフライン演算部の Generator とオンライン演算部の Calculator から

2) 指定手先座標変換行列H が所属するセルが複成っている．それぞれの概念図をFig.2，Fig.3 に示数ある場合は，selector の出力が d1 , . . . , ds とし，またその各演算要素の役割を次に要約する．

複数得られ，その結果関節角θも，θ(d1 ) , . . . , θ(ds )のように有限の複数解として求めることがで

2.1

きる (解一意性問題の解決)．

Generator• datamaker
関節角θ をθ(1) , .., θ(M) のように変化させ，対応する手先座標変換行列 H を順運動学により H (1) , .., H (M) と求め，これらの組である

H-θ データセットを作成する．
• celldivider
H-θデータセットの集合を手先位置(dx , dy , dz )，手先姿勢(r, p, y) および逆運動学の解の個数 sにより分類し，小領域であるセルをW 個作成する．これをCELL(d) (d = 1, . . . , W )とよぶ．

• interpolator–2–

datamaker

H (1)H-θ data set

3.

H (k)
θ (1)

セル分割補間法の実装

セル分割補間法を試験的に実装した．そのブロッ
H (M )

ク図を Fig.4，Fig.5 に，また Generator の設計指針

θ (k)θ (M )を以下にまとめる．なお対象とするマニピュレー

use forward kinematics to obtainhand homogeneous transform matrix
H and the joint angle θ.

タは，簡単のためシリアルリンクで，かつ解一意性の問題をもたないよう可動範囲を限定したマニ(r, p, y)ピュレータとする．

(dx, dy , dz ).......

• datamaker.......

関節角の可動範囲(θ min , θmax )をδ 等分し，∆θ =divide H-θ data set∈ 7with the hand position (dx, dy , dz )and rotation (r, p, y)yO(θ max −θmin )/δ づつ変化させながらH-θデータセットを作成する．ただし必ずしも等分とxz
CELL(d)celldivider

する必要はなく，ここでは簡単のため等分割

interpolatorで実装した．(Fig.6)．

• celldivider

CELL(d).......

シリアルリンクマニピュレータの場合，データセットは比較的マニピュレータ第1関節(Fig.7

interpolate the H-θ data set
in each CELL to obtain,
θ (d) = f (d)(H)

if the uniformization problem exists,
divide H-θ data set morewith the number of solutions
to generate ”CELLS”.の O 点) 近傍で密になることが多いが，補間精度の観点からセル毎のデータセット量に大Fig. 2

The conceptual scheme of Generator

きなばらつきがあるのは好ましくない．そこで第 1 関節近傍で小さく，遠方で大きくなるselectorCell(d1)ようなセルを作成する．これを満たす簡単

Cell(ds)

input Hな形状として，マニピュレータ第 1 関節を中.......

心としデータセットを内包する中空球を，半径・経度・緯度方向にそれぞれ∆分割した微小Ox

ﬁnd the number of celld1, d2, ..., ds which
the input H belongs片を採用する．なおデータセットの分類は，手先位置 (dx , dy , dz ) のみに関して行い，簡単zcalculator

CELL(d1)のため手先姿勢 (r, p, y) の分割は行わないもCELL(ds)のとする．また本実装において対象とするマ.......

θ (d1)

ニピュレータは解一意性の問題をもたないので解の個数 s については考えなくてよい．

θ (ds)• interpolatoruse θ (di) = f (di)(H) in each cellto obtain the joint angle θ(d1), ..., θ (ds)Fig. 3

補間式は次式に示すN 次多項式とし，最小自乗法により係数 Cij を定める 2) ．この方法に

The conceptual scheme of Calculator

よれば補間次数N の調整によりデータセット–3–の情報量圧縮が期待できる．

Generatorcalc

θi (H) =Q

Cij fj (H) (1 <=i<= F)

Joint lower andupper limit

θ min, θ max(2)datamaker

dataset finessangle changer
parameter δj=1

θ (k)parameter setting file"config.dat"

ここで，N Delta delta
th1min th1max...
thFmin thFmax

kkkfj (H) = r11j1 r12j2 r13j3 dkxj4
kkkGenerator
forward kinematics
write

H (k)
θ (1), H (1)kk···k· r21j5 r22j6 r23j7 dkyj8 r31j9 r32j10 r33j11 dkz j12(3)θ (M) , H (M)cell division parameter∆cell dividerClass......

r11


 r21
H=
 r
 31
0

Q=

N
dx



r12

r13

r22
r32

r23
r33

dy dz 00

1

H-theta data set"H_theta.dat"

Function......

"H_theta.dat.1""H_theta.dat.d""H_theta.dat.W"

coefficientscoefficientscoefficients"C.dat.1""C.dat.d""C.dat.W"

interpolation
order

N

Memory(4)Filecoefficients
data file"C.dat"12+k−1 Ck =12+N CN

(5)

k=0

Fig. 512
kjs <=NGenerator Class(6)s=1

H-θ data setyH-θ data set
CellGenerator(dx, dy , dz )Calculator
Constructor

coefficients
data file"Cdat"

θ minx∆θwriteOθ max

C[1],...C[d],...C[W]∆θ =θ max − θ minδreadend effector
homogeneous matrix

H

calcClassFig. 6

datamaker

Fig. 7

celldivider

selector
FunctionC[d]calculator

4.
MemoryFile
joint angleθ

Fig. 4

Calculator Class

セル分割補間法の精度評価

xy 平面運動をする 3 自由度マニピュレータにセル分割補間法を適用して逆運動学を解き，その精度評価を行った．

4.1

計算条件

マニピュレータの座標系を Fig.8 に，諸元および計算条件を Table.1 に示す．本評価では簡単のためマニピュレータの手先を第 3 関節 θ3 上におく．また第 2 関節 θ2 の可動範囲を (0,90)[deg] と限定することで解一意性問題を排除する．

–4–

yynd

θ3ErdyEpL2p

L1
θ1xO

Spacial 3 axes manipulator
Fig. 9

Table 1xOdx

Fig. 8npd

θ2Positional error Ep and orientational Er

Conditions for CDI method

z=0[mm](-90,90)[deg](θ2min , θ2max )(θ3min , θ3max )(0,90)[deg](-90,90)[deg]L1

0.45[m]400

L20.48[m]200Interpolation order

N2Cell division param.

∆20−200

Dataset finess param.

δ80−400

Length8000.126000.1y[mm]Joint limit

Ep[mm]

1000(θ1min , θ1max )

0.08

00.06

0.04

−6000.02−8004.2

評価式−1000−5000500

1000

x[mm]

補間法の精度は次式で定義される手先位置誤差

Fig. 10Distribution of positional error Ep

Ep ，手先姿勢誤差 Er で評価する (Fig.9)．

Er = cos−1(n nd )T

z=0[mm](7)1000

Er[deg]Ep = ||p − pd ||800(8)

0.350.3

600400

ここで，pd ： 指定手先位置ベクトル0.25

p： CDI により実現される手先位置ベクトル

ndn： 指定手先姿勢ベクトルy[mm]2000.200.15−200： CDI により実現される手先姿勢ベクトル−4000.1−6000.05−8004.3計算結果−1000−5000500

1000x[mm]

作業領域内に約 20000 点の計算点をとり，各点においてEp , Er を求めた．手先位置誤差分布をFig.10に，姿勢誤差分布を Fig.11 に示す．

–5–Fig. 11

Distribution of orientational error Er

4.4考察関節目標軌道計算

5.2

Fig.10 によれば手先位置誤差は最大で 0.12[mm]程度であり，これはマニピュレータ全長930[mm]に対して十分小さな値といえる．また手先姿勢誤差

Er は Fig.11 より最大で 0.35[deg] 程度と十分小さな

位置目標軌道にとった代表点を解析法とセル分割補間法によって関節目標軌道に変換した．セル分割補間法による軌道をFig.13に，セル分割補間法と解析法による軌道の差を Fig.14 にそれぞれ示す．

値が得られた．しかしながら，セルの境界部分で80
th1CDI
th2CDI
th3CDI誤差が大きくなる特性が認められた．このことか
60

らセル分割補間法を実際の軌道計算に適用した場

40

th1,th2,th3[deg]合，マニピュレータ手先が境界部をまたぐ際の関節軌道が不連続になると推測できる．

200-205. セル分割補間法による軌道計算-40-60

この節ではマニピュレータに直線の位置目標軌

-80
00.5

11.5

22.5

time[s]

道を与え，セル分割補間法により実際に関節目標Fig. 13

軌道を計算し，解析法による関節目標軌道と比較

Angular desired path by CDI method

する．
2th1ana-th1CDI
th2ana-th2CDI
th3ana-th3CDI

5.1

1.5

位置目標軌道の設定

1

th1,th2,th3[deg]位置目標軌道を xy 平面内に Fig.12 のように設定する．なおマニピュレータの速度は等速時0.3[m/s]の台形曲線で与え，時間間隔 2[ms] の代表点を位置0.50-0.5-1

目標軌道上に設定する．

-1.5

1000-200.5

800

11.5

22.5

time[s]

600Fig. 14

Diﬀerence of analytical and CDI path

400y[mm]2005.30

考察−200

軌道差 Fig.14 を見ると，セル分割補間法による−400

θ1 , θ2 の軌道が，解析法とほぼ同じであることが分−600かる．一方，θ3 の軌道には不連続性が認められる．

−800

これは celldivider が手先の位置 (dx , dy ) のみについ−1000

Fig. 120200

400
600x[mm]

800

1000

Positional desired pathてH-θ データセットを分類しているので，セルの境界部で特に大きな誤差を生じるためと考えられる．

–6–

6.

おわりにマニピュレータの逆運動学問題(汎用性問題，解一意性問題) を解決するアルゴリズムとしてセル分割補間法(CDI)を提案した．簡単なマニピュレータについて試験的に実装し，精度の評価を行った．
この結果マニピュレータ全長に対し約 0.001[%] 以下の実用上ほぼ問題ない精度が得られることが確認できた．また直線軌道を追従する関節目標軌道を計算して解析法と比較を行い，解析法に対しほぼ遜色ない軌道が得られることを確認した．今後の課題としては以下が挙げられる．

• セル境界部での軌道不連続への対応．
• 複数解のあるマニピュレータへの演算システ
ムの拡張．

• 多種のマニピュレータへの適用，検証．

参考文献
1) T.Shimizu and H. Kawasaki：An Analysis for Inverse Kinematics of Robot Manipulators using
Grobner basis, Jour. of Robotics and Mechatronics, 9-5, 324/331(1997)2) 新井：多関節マニピュレータにおける演算処理手法，計測自動制御学会論文集，18-1，92/99(1982)–7–