䣔䣕䣌䢴䢲䢳䢲䣃䣅䢳䣑䢳䢯䢶

２本指ロボットによる任意形状を持つ物体把持の数値シミュレータ構築法○吉田守夫 (理研) 有本卓 (立命大, 理研) 田原健二 (九大, 理研)

Dynamic Simulation of Pinching a 2-D Object with Arbitrary Geometryby Two Robot Fingers*Morio YOSHIDA (RIKEN), Suguru ARIMOTO (Ritsumeikan Univ./RIKEN),
Kenji TAHARA (Kyushu Univ./RIKEN)
Abstract— A method for realizing numerical simulation of pinching a 2-D object with arbitrary geometry
under rolling contacts by a pair of robot ﬁngers with arbitrary geometry is presented. To reconstruct smoothﬁtting curves suﬃciently diﬀerentiable from the discrete data points, smoothing cubic splines are used. Acollision detection method is proposed in order to determine an initial pose of the overall system without
mutual penetration. Finally, the results of numerical simulations show the eﬀectiveness of our proposedmethods.
Key Words: Grasping, Robot Finger, Rolling Contact, Numerical Simulation, Penetration Problem

1.

はじめに高齢者の介護などの生活を支援するロボットの実用化が期待されている．そのようなロボットを安心して使用するためには，人とロボットとが接触する際に生じる相互作用の関係を明らかにする必要がある．そこ
で本稿ではロボット指と物体との間に生じる転がりの関係を考慮した，人差し指と拇指の輪郭を模した形状を指先に有する２本指ロボット (Fig.1) による任意形状を有する物体の動的把持の数値シミュレータ構築方法を提案する．その際，Arimoto [1] により提案された数学モデルを基にし，指ロボットが物体表面を滑らずに転がる拘束と，その接触点が離れない拘束を仮定する．
ロボットの指先を人の人差し指と親指を模した形状で近似し，把持する物体形状も実在する物体を近似することで再現する．そのために，３次スプライン関数を用いる代わりに３次平滑化スプライン関数を用いた．その理由として，３次スプライン関数は，全ての与えら
xOq21q11yp2
Omq12q22

Xp1q13Y

Fig.1 Two robot ﬁngers with arbitrary shape pinch
an object with arbitrary shape➨䢴䢺ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢳䢲ᖺ䢻᭶䢴䢴᪥ࠥ䢴䢶᪥凛

Fig.2 Picture of human hand

れたデータ点を通過するので，細かな凹凸まで再現し，その凹凸が振動を引き起こす可能性が挙げられる．本稿で提案するモデルは形状パラメターを用いていることから，数値シミュレーションを開始するためのロボッ
トの初期値を Penetration を引き起こすことなく求めるために，逆運動学などの解析手法が用いれないため，指ロボットと物体との間の貫通条件を調べ，その衝突問題を解くことで導出する必要がある．安定把持を実現する制御入力として，Arimoto らが従来から提案してきた “blind grasping” と呼ばれる制御手法を，指先形状および物体形状が複雑になっているにも関わらず適用可能であることを示す．最後に，数値シミュレーションを行うことにより提案する手法の妥当性を検証する．

2.

指ロボットと物体の形状関数

人差し指と拇指，把持される物体の滑らかな輪郭線を数値シミュレータに実装するたの再構築法を提案す
る．Fig.2 に示す人の手の写真を撮り，人差し指に局所座標系 O1 -X1 Y1 を張り付ける (Fig.3)．その座標に基づき人差し指の輪郭線上の 10 点 (xi , yi )(i = 1, · · · , 10)を計測する． 3 次スプライン関数は 2 階微分が連続なので，その点における曲率が導出可能である．また，第䣔䣕䣌䢴䢲䢳䢲䣃䣅䢳䣑䢳䢯䢶-0.01

O1Y1Y2O2

X1O2
X2[m]

Exterior

X2

Fig.3 The side of index ﬁnger attached on the local
coordinate O1 -X1 Y1Interior

00.01

Fig.6 A
human
thumb expressedwith an attachedlocal coordinates

Y2[m]

0.02
-0.02

-0.01

0Fig.7 A smoothing
cubic spline-0.01

Interior-0.01

Interior
O1

X1[m]0Exterior0.01

Exterior

Y1[m]

Y1[m]

0.01
0.02
-0.01

0.02
-0.0100.01

X1[m]

O10Om
00.01

X0.020.02

Fig.4 A cubic spline

Fig.5 A smoothing
cubic splineY

3 章で述べられるように，曲率の値は孤長パラメターの更新則に使われる．従って，3 次スプライン関数で補間することは，本論文で提案する数値シミュレータ構築法に適用可能である．しかしながら，計測されたデータは常に計測誤差を含み，3 次スプライン関数は全ての与えられた点を通過するように補間するので，そのスプライン関数は，Fig.4 に示すように細かな凹凸まで再現してしまう．そのため 3 次スプライン関数により補間される曲線は，特に曲率が急に変化する場所で振動を引き起こ可能性がある．このような現象を避けるために，3 次平滑化スプライン関数 [2] を導入し，その結果を Fig.5 に示す．3 次平滑化スプラインを導入するために，区間 [a, b] を分割した [xi , xi+1 ](i = 0, 1, · · · , m− 1)
上において下記の 3 次多項式を考える．
(i)Fig.8 Picture of an object

Fig.9 The contour ofan object

Exterior
0.02

XOm0Interior-0.02

Y-0.02

00.02

Fig.10 Shape function expresses the left side of anobject by a smoothing cubic spline(i)

Si (x) = a0 + a1 (x − xi )
(i)

(i)

+a2 (x − xi )2 + a3 (x − xi )3(i)

(i)

(i)

(1)

(i)

ここで，a0 ,a1 ,a2 ,a3 はそれぞれ区間 i の係数とする．3 次平滑化スプライン S(x)(= Si (x) if x ∈[xi , xi+1 ]) は，全ての区間で 2 階微分連続，かつ，下記の関数を最小にするように決定される．
 bm

122J(S) =(S  (x)) dx +(S(xi ) − yi ) (2)ρiai=0

ここで，ρi > 0 とし，こちら側が与える重み係数とする．式 (2) の右辺第一項は，曲率を減らす方向へ働くことを意味する．もし，この関数の曲率が最小のゼロならば，このスプライン関数は直線になることを意味する．式 (2) の右辺第二項は，重み係数 ρi を増やせば，その関数は与えられたデータ点より，さらに遠ざかることを意味する．我々は ρ0 = 0,ρ9 = 0 とする
ことで点 x1 と点 x9 を通過するようにし，残りの値を
ρi (i = 1, · · · , 8) = 5.0×10−9 と設定した．Fig.4 と Fig.5➨䢴䢺ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢳䢲ᖺ䢻᭶䢴䢴᪥ࠥ䢴䢶᪥凛を比較すると，平滑化スプライン関数によりフィッティングした Fig.5 のほうが 3 次スプライン関数により補間した Fig.4 よりも凹凸が減り滑らかになっていることが確認できる．そこで，本論文にて提案する数値シミュレータに Fig.5 の曲線を左指ロボットの形状関数として採用する．同様にして，拇指の写真を撮り，局所座
標 O2 -X2 Y2 を張り付けた (Fig.6)．その拇指の輪郭線から 17 点を計測し，Fig.7 に示すように平滑化スプラインによりフィッティングした．その際，ρ0 = 0,ρ16 = 0とし，残りの値を ρi (i = 1, · · · , 15) = 5.0 × 10−9 とした．Fig.7 の曲線を提案する数値シミュレータの右指のロボットの形状関数として採用する．さらに，把持する物体の写真 (Fig.8) を撮り，局所座標系 Om -XY を張り付けた (Fig.9)．その物体の左側の輪郭線から 21点を計測し，Fig.10 に示すように平滑化スプラインによりフィッティングした．その際，ρ0 = 0,ρ20 = 0 とし，残りの値を ρi (i = 1, · · · , 19) = 1.0 × 10−8 とした．

䣔䣕䣌䢴䢲䢳䢲䣃䣅䢳䣑䢳䢯䢶Fig.10 の曲線を提案する数値シミュレータの左側物体側面として採用する．物体の右側面は，Fig.8 に示される把持される物体の対称性から，Fig.10 の X 軸の値の正負を反転させ，Y 軸の値をそのままにした曲線を採用する．

3.

ダイナミクス

本稿では，Fig.1 に示す指ロボットと物体からなるシステムを取り扱う．指ロボット，把持される物体ともに剛体と仮定し，左指ロボット (i = 1) は 3 自由度から，右指ロボット (i = 2) は 2 自由度から構成されている．
左指ロボットの関節角度を q 1 = (q11 , q12 , q13 )T と定義
し，右指ロボットの関節角度を q 2 = (q21 , q22 )T と定義する．把持される物体の位置 Om を x = (x, y)T とし，回転角度を θ とする．全ての角度の値は，反時計回りを正とする．局所座標系 Oi -Xi Yi (i = 1, 2) に基づく指ロボットの輪郭曲線を γ i (ti )，局所座標系 Om -XY に基づく物体の輪郭曲線を γ 0i (t0i ) と下記のように定義する．

γ i = (Xi (ti ), Yi (ti ))T(3)T

γ 0i = (X0i (t0i ), Y0i (t0i ))(4)

ここで，ti (i = 1, 2) をロボットの指先の輪郭線を描くための形状パラメターとし，t0i (i = 1, 2) を物体の輪郭線を描くための形状パラメターとする．ロボットと物体とは常に接触し，滑ることなく転がる拘束を仮定する．これらの仮定の下で，Fig.1 と Fig.11 に示されるシステム全体のダイナミクスは文献 [1] よりM ẍ − f1 n01 − f2 n02 − λ1 b01 − λ2 b02 = 0(5)TI θ̈ − f1 bT01 γ01 + f2 b02 γ02


 T

+λ1 nT(6)01 γ01 − λ1 n02 γ02 = 0
1
Gi (q i )q̈ i +Ġi (q i ) + Si (q i , q̇ i ) q̇ i2+fi JiT (q i )n0i − (−1)i bT
γi i ei+λi JiT b0i − (−1)i nTi γi ei = ui , i = 1, 2 (7)と導かれる．ここで，e1 = (1, 1, 1)T ,e2 = (1, 1)T を表し，I は物体の慣性モーメント，Gi は指ロボット i の慣

Omq11+ q+q1312X1O1

γ1 P1γ01
n01

n1b1 Y1

b01
b1

Xxq21+ q22γ02θ
yP2 γ2 O2n02n2Yb2b2b02 Y2

Fig.11 Deﬁnitions of the tangent and normal vectors
at the contact points➨䢴䢺ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢳䢲ᖺ䢻᭶䢴䢴᪥ࠥ䢴䢶᪥凛

X2性行列，Si は歪対称行列を表す．ni ,bi (i = 1, 2) は，それぞれロボット指の局所座標系 Oi -Xi Yi から見た法線ベクトルと接ベクトルを表す．n0i ,b0i (i = 1, 2) は，それぞれ局所座標系 Om -XY から見た法線ベクトルと接ベクトルを表す．n0i ,b0i は，それぞれ慣性座標系 O-xyから見た法線ベクトルと接ベクトルを表す．形状を表現する孤長パラメター ti の更新則は，文献 [1] を参考にすることにより


 dt
dti
1i=
θ̇ − q̇ Ti eidt
(κ0i + κi )dsi(8)と導出できる．κi は指ロボットの指先の曲率を表し，κ0i は把持される物体の曲率を表す．同様に t0i の更新則は


 dt
dt0i
10i=
θ̇ − q̇ Teiidt(κ0i + κi )dsi(9)と導出できる．

4.

制御入力

本稿にて使用する制御入力も “blind grasping” の考えに基づく制御入力を下記のように提案する．この制御入力の利点として，ロボットの指先形状と把持される物体形状を制御する側が知る必要が無いことが挙げられる．

ui = −ci q̇ i + (−1)i βJiT (q i ) (r1 − r2 ) − αi N̂ii = 1, 2(10)
ここでN̂i = pi (t) − pi (0), i = 1, 2(11)とし，ci , αi (i = 1, 2),β は正の定数とする．r i (i = 1, 2)は，Fig.11 の点 Oi (i = 1, 2) を示す．式 (22) 右辺の第一項目はダンピング項を表し，第二項目は拇指対向性に基づく安定把持を実現する項を表している．第三項
目は，指ロボットが初期姿勢からなるべく離れないようにする項を表す．

5.

初期値の導出

数値シミュレーターを構築する際，解決しなければならない問題の一つに，どのようにロボットと物体からなるシステム全体の初期値を決めるかという問題がある．本稿にて取り扱うモデルは，ロボットの指先と物体の輪郭線を形状パラメターを用いて表現しているので，逆運動学といった解析手法を用いて初期値を設定することが出来ない．そこで，左指ロボットの関節角度，右指ロボットの関節角度の一部，物体の姿勢などはこちら側から与え，右指ロボットの関節角度 q21 と形状パラメター t2 ,t02 を決めるために，指先と把持する物体とが貫通しているかどうかを調べる．始めに左指ロボットの関節角度 q11 ,q12 ,q13 , 右指ロボットの関節角度
q22 , 物体の姿勢 θ をこちらから与える．次に左指ロボットの形状パラメター t1 を与え，γ 1 を決めることにより左指ロボットと物体との接触点を決める．この接触点での慣性座標系 O-xy から見た指側の接ベクトル b1が t1 が与えられたことにより導出され，b1 と物体側の䣔䣕䣌䢴䢲䢳䢲䣃䣅䢳䣑䢳䢯䢶xO
q21
yPoi (xoi,yoi)
Ppj (xpj,ypj)

q22

Fig.15 After 1 seconds

Fig.14 Initial pose

q11
q12
q13
The value of zero

. .

q21
q22
The value of zero

10

500

0.2

0.4

0.6

0.8

5.

.
.

0

θ
The value of zero10

θ [rad/s]

5q21,q22[rad/s]

.
.
.

. .

q11,q12,q13[deg/s]

10.

.

Fig.12 The discrete points (xoi , yoi ) for i = 1, · · · , mand (xpj , ypj ) for j = 1, · · · , n are deﬁned

01time[s]yoiExteriorPpj (xpj ,ypj)
yoi+1

Poi+1 (xoi+1 ,yoi+1)

0.2

0.2

0.1

0.1

y[m/s]

ppj.

0.

-0.1

100.2

0.2

0.4

0.6

0.8

time[s]Fig.19 ẋ

10.6

0.8

1Fig.18 θ̇

302010θ.

x
The value of zero

-0.2

0.4

time[s]-0.10➨䢴䢺ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢳䢲ᖺ䢻᭶䢴䢴᪥ࠥ䢴䢶᪥凛

0.8

0.

Fig.13 Deﬁnitions of the vector oi and the vector pj

接ベクトル b01 が同じになることを利用し，NewtonRaphson 法を用いて数値的に解を導くことにより，物体の形状パラメター t01 を導出する．t01 と物体の角度
θ が解っていることから物体の Om = (x, y)T を得ることが出来る．この時点において，未知変数は，物体の形状パラメター t02 , 右指ロボットの形状パラメター t2 ,右指ロボットの関節角度 q21 である． Fig.12 に示すよ
うに，物体の輪郭線を慣性座標系 O-xy に基づき１万点(xoi , yoi )(i = 1, · · · , 104 ) に離散化し，各点に各点でのt02 の値を付加する．右指ロボットの輪郭線も慣性座標系 O-xy に基づき１万点 (xpj , ypj )(j = 1, · · · , 104 ) に離散化し，各点に各点での t2 の値を付加する．右指ロボットの関節角度 q21 の値を適当に増減することにより，右指ロボットの指先と物体との距離が十分短くなった点を選び出し初期値として記録する．具体的には，ロボットの指先の点 Ppj (= (xpj , ypj )) が物体の外部にあるのか内部にあるのかを下記のように調べることにより物体と右指ロボットとの距離を求める．ロボットの指先の離散点が 1 点でも物体の内部にある場合には，物体の内部にあるロボットの指先の離散点と物体の離散点とのペアの中から最も物体の輪郭線から離れたペアを選び出し，このペアの距離を物体とロボットとの距離とする．
一方，ロボットの指先の離散点が物体の内部に１点も無い場合には，ロボットの指先の離散点と物体の輪郭線の離散点とのペアの中から，最も短い距離を持つペアを選び出し，この距離をロボットの指先と物体との距離とする．Fig.13 に示すように，物体の輪郭線の離散点
Poi (= (xoi , yoi )) と Poi+1 (= (xoi+1 , yoi+1 )) を結ぶベク−−−−−→
トル Poi Poi+1 の単位ベクトルを poi とする．Fig.13 に示すように，ロボットの指先の点 Ppj と物体の輪郭線−−−−−−→の離散点 Poi+1 を結ぶベクトル Ppj Poi+1 の単位ベクト

0.6

Fig.17 q̇21 ,q̇22x[m/s]

ppj

0.4

θ [deg]poi

0.2

time[s]Fig.16q̇11 ,q̇12 ,q̇13

Poi (xoi ,yoi)InteriorPpj (xpj ,ypj)

0yThe value of zero

-0.2
00.2

0.4

0.6

0.8

time[s]Fig.20 ẏ

1000.2

0.4

0.6

0.8

1time[s]Fig.21 θルを ppj とする．もし，yo1 > ypj または yo104 ≤ ypj
ならば，ロボットの指先は物体の外部にあるとする．もし，yoi ≤ ypj < yoi+1 ，かつ，外積 poi × ppj の z 軸成分が負ならば内部とし，正ならば外部とする．上述した方法を用いてシステム全体の初期値を導出する．

6.

数値シミュレーション提案する手法の妥当性を確認するために数値シミュレーションを行った．Fig.14 にシステム全体の初期姿勢を示し，Fig.15 に一秒後の姿勢を示す．Figs.16∼20
より，状態変数の全ての速度がゼロに収束していることが確認できることから，数値シミュレーションの観点から安定把持が実現できたことを確認した．

7.

まとめ

本稿では，人差し指と拇指の形状を近似した 2 本指ロボットハンドを用いて，実在する物体の形状を近似した物体を転がり拘束下で把持するための動的数値シミュレータを構築する方法を提案した．最後に数値シミュレーションを実行し，その結果を検証することで本論文にて提案する手法の妥当性を確認した．
謝辞 本研究は，日本学術振興会科学研究費補助金基盤研究(B)(課題番号 20360117) の交付により行われたものである．

[1] S. Arimoto and M. Yoshida, “Modeling and Control of 2-D Grasping under Rolling Contact Constraints between Arbitrary Shapes:A RiemannianGeometry Approach”, J. of Robotics vol.2010, Article
ID 926579, 2010.
[2] E.V.Shikin and A.I.Plis, Handbook on splines for theuser, CRC Press, Boca Raton,FL, USA and Tokyo,
Japan, 1995.