䣔䣕䣌䢴䢲䢲䢻䣃䣅䢵䣃䢴䢯䢲䢳

楕円形状を有する 2 次元物体把持の数値シミュレータ構築法○吉田守夫 (理研) 有本卓 (立命大, 理研) 田原健二 (九大, 理研)

Numerical Simulator Design for pinching a 2D Object with Ellipsoidal Shape*Morio YOSHIDA (RIKEN), Suguru ARIMOTO (Ritsumeikan Univ./RIKEN),
Kenji TAHARA (Kyushu Univ./RIKEN)
Abstract— A construction method of numerical simulators for pinching a 2D object with ellipsoidal shapeby a robot ﬁnger is proposed. Since a shape function possibly expressed by arclength parameter is quite
restrictive to describe arbitrary shapes, we propose an extended method to treat a more general class of shapes
including ellipsoidal shapes which are not directly expressed by arclength parameter. In order to verify theeﬀectiveness of our proposed method and the stability of motion of the overall ﬁnger-object system, numericalsimulations are carried out.
Key Words: Multibody Dynamics, Arbitrary Shape, Rolling Constraint, Holonomic Constraint, Grasping,
Robot Finger

1.

はじめに人とロボットとの共生を目指したロボットの開発・研究は注目を集めている．そのようなヒューマンフレン
ドリーなロボットを実現する重大な要素として，環境とロボットとの相互作用の実現が挙げられる．例えば，ロボットと人とが共同して何かを実現するためには，ロボットと人，または，ロボットと物体との間に相互作用が生じる．実際，そのような視点によるロボットの研究は，数多くなされきた [1]. しかし，センサー技術やロボットの製作・制御技術が進歩したにも関わらず，そのようなタスクを安定して実現するのは難しい．その最たる理由として，ロボットが我々の生活する環境において活躍するために必要不可欠なその相互作用の関係を，数理の視点から十分に明らかにされていなことが挙げられる．
ロボットと環境との相互作用の研究として，ロボットハンドによるグラスピングの研究が挙げられる [2]．多くの研究は，相互作用が生じる物体間の力の受け渡しがどうなっているのかが重要であるにも関わらず，そのような視点についてあまり関心がなかった．また，相互作用する物体間の形状を直線や球といった簡素化したモデルに終始し，我々の日常生活にある複雑な形状をどのようにモデル化し，数値シミュレータに組み込み，その結果を可視化したらよいかについて注目してこなかった．
ごく最近になり，有本らにより 2 次平面に限定されるが，微分幾何学の視点から，ロボットの指先と物体表面の形状を任意形状とする動的ピンチングの数理モデ
ルを指ロボットと物体との間の力関係を明示した形で提案された [3]．また，この数理モデルを基にした数値シミュレータは，吉田らにより提案された [4]．しかしながらこの方法は，孤長パラメータを用いた形状関数に特化していたため，表現できる形状に制限があった．
そこで本報告では，孤長パラメータを用いた形状関数では表現できない形状，たとえば楕円形状を有する物体の数値シミュレータ構築法を提案する．つまり，孤長パラメータであるための制限を受け無くなるので，任➨䢴䢹ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢻ᖺ䢻᭶䢳䢷᪥ࠥ䢳䢹᪥凛

q1xOyθOmX
θ1Normal Vector

q2

r1q3

Y

Tangent Vector

Fig.1 Robot ﬁnger pinches an object with ellipsoidal
shape, which is ﬁxed at Om

意の形状を近似によって表すことが可能になることを意味する．文献 [3] により提案された制御入力を用いて，数値シミュレーションの視点から物体が安定化可能であることを示し，提案する数値シミュレータの妥当性を検証する．

2.

数値シミュレータ構築

Fig.1 に提案するモデルの全体図を示す．3 自由度を有する 2 次元ロボット指が，点 Om に固定された物体を安定化させる問題を考える．この際，ロボット指の指先形状は半球とし，把持する物体の左側面の形状を楕円とする．ロボットのリンク，指先および安定化する物体は全て剛体とする．ロボット指と物体の全ての動きは 2 次元平面に限定されているとし，重力の影響は無視する．物体は，点 Om により固定されているので Om 点回りの回転運動しかすることができない．ロボットの指先は，安定化する物体と点接触をし，滑ることなく物体表面を転がるとする．この場合の動力学䣔䣕䣌䢴䢲䢲䢻䣃䣅䢵䣃䢴䢯䢲䢳0.03となるので0.02

dt1= 1/ds1

Y (t'1)[m]

0.01

Om

0-0.02

-0.03

-0.02

-0.01

00.01κ1 (t1 ) =は，文献 [3] に従うと，I θ̈ − f1 lb1 (s1 ) − λ1 ln1 (s1 ) = 0(1)
1G1 (q 1 )q̈1 +Ġ1 + S1 q 1 + f1 J1T (q 1 )n1 (θ)
2+λ1 J1T (q 1 ) − r1 e1 = u1(2)となることが知られている．ここで，式 (1) は安定化する物体，式 (2) はロボット指のダイナミクスを表すことに注意する．また，q 1 = (q11 , q12 , q13 )T はロボットの関節角度のベクトル，θ は物体の回転角度を表す．s1は孤長パラメータを表し，この更新式は文献 [3] より
ds1r1=
ṗ1 − θ̇dt
1 + r1 κ1 (s1 )(3)となることが知られている．この数理モデルに基づく数値シミュレータ構築法は文献 [4] に述べられているが，この場合の物体座標 Om -XY に基づき物体の輪郭を表す

座標を (X(s1 ), Y (s1 )) として X  (s1 )2 + Y  (s1 )2 = 1を満たす形状関数のみを取り扱っていたため，一般の任意形状を表すには窮屈であった．そこで，本報告でs1 の代わりにパラメータ t1 を導入
は孤長パラメータすることにより， X  (s1 )2 + Y  (s1 )2 = 1 を満たさない楕円の形状関数を取り扱う．
本報告にて取り扱う物体の輪郭を表す楕円形状関数(X(t1 ), (Y (t1 )) をX(t1 ) = 50 − 0.016 − 50 cos(t1 ) [m](4)Y (t1 ) = sin(t1 ) [m](5)と決める (Fig.2 参照)．実際に X  (t1 )2 + Y  (t1 )2 = 1となるので，t1 が孤長パラメータとなり得ないことがすぐにわかる．ここで，孤長 s1 の長さは0dX(t1 )dt12dY (t1 )dt12+2+dY (t1 )dt1

dt1(6)により導出される．従って

ds1=
dt1dY (t1 )dt12(8)dX(t1 )dt1(9)となる．ここで κ1 (t1 ) は曲率を表しておりFig.2 Shape function expresses the contour of theobject with ellipsoidal shape t1+ dt
dt1r11=ṗ1 − θ̇dt
1 + r1 κ1 (t1 )
ds1

0.02

X (t'1)[m]s1 =2となる．従って，パラメータ t1 の更新式は式 (3) と式(8) により

-0.01

-0.03
-0.04

dX(t1 )dt1X  (t1 )Y  (t1 ) − X  (t1 )Y  (t1 )(X  (t1 )2 + Y  (t1 )2 )3/2(10)となる．この式 (9) の更新式を用いる限りにおいて各時刻 t では，X(s1 ) = X(t1 ), Y (s1 ) = Y (t1 ) となる．
また，Fig.1 の θ1 は

θ1 (t1 ) = tan−1dX dY/dt1 dt1(11)と表せることも容易に確認できる．従って上述の部分に注意することにより，文献 [4] の方法を用いて数値シミュレータを構築すればよいが，転がり拘束について注意しなければならない点がある．文献 [3] により，転がり拘束に孤長パラメータ s1 の値が直接必要になる部分があることから，パラメータ t1 の値と同時に s1 の値も必要になることがわかる．その際，s1 の初期値をs1 (0) = t1 (0) を満たすように解かなければならないよ
うに一見思えるが，s1 (t) − s1 (0) の差分が解ればよいことから，その必要性が無いことがわかる．

3.

制御入力と閉ループダイナミクス

ロボット指により物体の回転を静止させるための制御入力は，文献 [3] の類推により

u1 =−c1 q̇ 1 −fd T
J (q )
r1 1 1x01 − x
y01 − y− r1 N̂1 e1(12)とする．ここでN̂1 (t) = γ1−1 r1 (p1 (t) − p1 (0))(13)とし，fd ，c1 および γ1 はそれぞれ正の値とする．また，e1 = (1, 1, 1)T とし p1 = q1 + q2 + q3 とする．
制御入力 (式 (12)) を指と物体からなるダイナミクス(式 (1) と (2)) に代入することにより閉ループダイナミクスI θ̈ − ∆f1 lb1 (t1 ) − ∆λ1 ln1 (t1 ) = 0(14)
1G1 (q 1 )q̈1 +Ġ1 + S1 q 1 + ∆f1 J1T (q 1 )n1 (θ)
2+∆λ1 J1T (q 1 ) − r1 e1 + r1 ∆N1 e1 + c1 q 1 = 0(15)を得る．ここで，2➨䢴䢹ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢻ᖺ䢻᭶䢳䢷᪥ࠥ䢳䢹᪥凛(7)∆f1 = f1 −fd(r1 + ln1 (t1 ))r1(16)

䣔䣕䣌䢴䢲䢲䢻䣃䣅䢵䣃䢴䢯䢲䢳fdlb1 (t1 )(17)
r1fd(18)∆N1 = N̂1 + lb1 (t1 )r1fdfdSN = − (r1 + ln1 (t1 )) lb1 (t1 ) + lb1 ln1 (19)r1
r1
∆λ1 = λ1 +とする．

4.

数値シミュレーション

提案する数値シミュレータに制御入力 (式 (12)) を用いることにより，数値シミュレーションを行った．その際に用いたロボットの物理パラメターと制御パラメータを Table 1 と Table 2 に示す．また，物体の形状パラメターとして式 (4) と (5) を用いた．その際の数値シミュレーションの 0.04 秒後と 5 秒後の姿勢を Fig.3と 4 に示す．初期姿勢を図示しなかった理由は，指ロボットが物体に加える力をゼロにしていたため (力がゼロなのでもちろん不安定であるが)，不安定であることを示す図例としては 0.04 秒後の図のほうが適していたからである．
t → ∞ において，安定に物体の回転を止めることができるなら，θ̈,θ̇,q̈ 1 ,q̇ 1 → 0 となるので閉ループダイナ
ミクス (式 (14),(15)) はFig.3 Motion of pinching a 2-D object with ellipsoidal shape by one robot ﬁnger(After 0.04 seconds)−∆f1 lb1 (t1 ) − ∆λ1 ln1 (t1 ) = 0(20) T
T∆f1J1 (q 1)n1(θ)∆λ1 J1 (q 1)−r1 e1 + r1 ∆N1 e1 = 0 (21)となり，これがロボット指と物体との安定状態を表す平衡点になる．数値シミュレーション結果のグラフ(Figs.(5)∼(8)) より ∆f1 ,∆λ1 ,∆N1 ,SN → 0 なので，平衡点を表す式 (20) と (21) を満足していることを確認できる．Fig.3 は，数値シミュレーションの 0.04 秒後の様子を表している．この状態において，法線力 f1 と接線力 λ1 の合力が物体を固定している点 Om を貫通していないため，物体に回転モーメントが生じている．
一方，5 秒後のロボット指が物体に加える力の様子を表す Fig.4 では，合力が物体を固定している点 Om を貫通していることを示しており，物体に回転モーメント

Table 1 Physical parameters of the ﬁnger and object.

l11

length

0.065 [m]l12length

0.039 [m]l13length0.026 [m]m11

weight0.045 [kg]m12

weight0.025 [kg]m13

weight0.015 [kg]r1radius

0.010 [m]Mobject weight0.040 [kg]Table 2 Parameters of control signals

fdinternal force

1.000 [N]

cdamping coeﬃcient

0.003 [Nms]γ1regressor gain

0.001 [s2 /kg]➨䢴䢹ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢻ᖺ䢻᭶䢳䢷᪥ࠥ䢳䢹᪥凛

Fig.4 Motion of pinching a 2-D object with ellipsoidal shape by one robot ﬁnger(After 5 senconds)が生じていないことがわかる．従って，数値シミュレーションの観点から安定に物体の回転を止めていることを確認できた．その際のパラメータ t1 と孤長パラメー
タの初期値からの変動値 (s1 (t) − s1 (0)) を Figs.9 と 10に示す．また数値シミュレーションを行う際，ロボットの指先と物体との間の転がりを表すホロノミック拘束R̃1 (文献 [3] [4] 参照) とロボットの指先と物体との接触を表すホロノミック拘束 Q1 (文献 [3] [4] 参照) を維持するために “Constraint Stabilization Method”(CSM 法)を用いている．ここで，転がり拘束 R̃1 = 0 は 2 つの物体の間に滑りが全く生じないことを示している．一方，接触拘束 Q1 = 0 は 2 つの物体が常に接触していることを表している．Figs.11 と 12 から数値シミュレーションを行っている間，Q1 が 10−18 ，R̃1 が 10−12 のオーダーでありゼロに非常に近いことから，CSM 法が十分効いており，2 種類の拘束条件 Q1 と R̃1 が十分拘束されていることが確認できる．以上から，提案する数値シミュレータの妥当性が確認できた．

0.10.100∆λ 1[N]∆ f1[N]䣔䣕䣌䢴䢲䢲䢻䣃䣅䢵䣃䢴䢯䢲䢳-0.1る．
謝辞 本研究は，日本学術振興会科学研究費補助金基盤研究(B)(課題番号 20360117) の交付により行われたものである．

-0.1

-0.2

-0.2

∆ f1
The value of zero

-0.3
01234

∆λ 1
The value of zero

-0.3
05123

45time[s]time[s]Fig.6 ∆λ1

Fig.5 ∆f1
0.1

0SN[Nm]∆ N1[N]0-0.0005-0.1-0.2∆ N1
The value of zero

-0.3
01234SNThe value of zero

-0.001

05123

4

5

time[s]time[s]Fig.8 SN

Fig.7 ∆N1
t'1

0s1(t)-s1(0)t'1

s1(t)-s1(0)[m]

0.01610.01605-0.002-0.004

0.016
01234(×10-18)

05time[s]Fig.9 t1123time[s]45Fig.10 s1 (t) − s1 (0)(×10-12)00-1-1~
R1[m]

Q1[m]

-2-3-3-4

Q1
The value of zero

-50

1

23time[s]Fig.11 Q1

5.

-2

45~
R1
The value of zero

-40

1

23time[s]45Fig.12 R̃1まとめ

安定化する物体の輪郭を楕円とした場合の取り扱い方を提案し，その方法の妥当性を数値シミュレーションの結果から確認した．また，簡便な制御入力を用いることで，ロボット指一本により物体を安定化することが可能であることを，数値シミュレーションの結果が閉ループダイナミクスより導出した平衡点を満足することと，数値シミュレーションの結果を可視化した図により確認した．本報告にて取り扱った形状関数は楕円であったが，本提案法を用いることで，スプライン関数などの近似関数により，我々の生活環境にある，より複雑な形状をモデル化できる．
最後に，本報告にて提案した数値シミュレータ構築法が有益であることは，他の方法とは異なり，有本らにより導出された数学モデル [3] が任意形状間の転がり拘束を非ホロノミックとしてではなく，ホロノミック拘束として導出することに成功したことにより，非ホロノミック拘束の一種である Pfaﬃan 拘束として転がり拘束を表した場合よりも転がりの拘束条件を，CSM法を用いて非常に精度よく維持できることからもいえ➨䢴䢹ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢻ᖺ䢻᭶䢳䢷᪥ࠥ䢳䢹᪥凛[1] B. Sicilliano and O. Khatib, Eds., Springer Handbookof Robotics, Springer, New York, NY, USA, 2008.
[2] R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press,
Boca Raton,FL, USA, 1994.
[3] 有本卓, “ロボットの腕や手の巧みさと制御:リーマン幾何学に基づくアプローチ”,IEICE Fundamentals Review,Vol.2, No.4,pp.1-11, 2009.
[4] M. Yoshida, S. Arimoto, and K. Tahara, “Modelingand Control of Pinching 2D Object with Arbitrary
Shape by a Pair of Robot Fingers under Rolling Constraints” ,in Proceedings of Multibody Dynamics 2009,
ECCOMAS Thematic Conference, Warsaw, Poland,
2009.