点補間法を用いた任意形状輪郭モデルの物体把持
数値シミュレーション○吉田 守夫 (理研)

有本卓 (立命大/理研)

田原 健二 (九大/理研)

Numerical Simulation of Pinching an Object
with Arbitrary Shape Expressed by Points Interpolation*Morio YOSHIDA(RIKEN), Suguru ARIMOTO(Ritsumeikan Univ./RIKEN),
and Kenji Tahara(Kyushu Univ./RIKEN)
Abstract— A numerical-simulation method for pinching a 2D object with arbitrary shape by a robot ﬁnger
is proposed. The shape of a robot ﬁnger’s tip, which is similar to the side shape of one’s index ﬁnger, isexpressed by the points interpolation method based on cubic spline. Since second derivative of a cubic spline
function is continuous, it is possible to derive the curvature of the function at every point. By virtue of
this characteristic of cubic spline, it is shown that the cubic spline interpolation method is embedded in ourproposed numerical simulator. In order to verify the eﬀectiveness of our proposed method and the stabilityof motion of the overall ﬁnger-object system, numerical simulations are carried out.

1.

x

はじめに重い荷物を持ち所望の場所に運んだり，様々な形の食器を所定の棚に置いたりする仕事は，人に代わって疲れを知らないロボットがすることを期待されている．
そのような仕事をロボットが実現するためには，荷物や食器などをロボットが掴む際に生じるロボットと物体とのインタラクションを無視することができない．実際，そのようなインタラクションの対象となる物体情報を計測するために必要なセンサー技術の発展には目を見張るものがある．しかしながら，インタラクションを伴う仕事をロボットに実現させるためには，センサーからの情報のみでは不十分である．なぜなら，センサーは既に生じた現象を計測することはできるが，まだ生じていない現象に関しては計測しようが無いからである．そこで，ロボットの目の前にある物体をロボットが持てるかどうかを判断するためにはロボット内部のコンピュータにロボットと対象物体の数学モデルを作成し，それを仮想的にシミュレートする必要がある．
そのような数学モデルに基づくロボット研究は数多くなされてきが 1) ，その際の数学モデルの多くは形状を円柱や直方体といった基本的な形に近似したものを利用していた．
ごく最近になり，有本により 2 次元ではあるが指先の形状と安定化する物体との形状を任意とした場合の動力学が導出された 2) ．またその論文 2) において，ロボット指と物体とは，1) 貫通することなく常に接触する，2) 接触点において滑らずに転がる，という 2 種類の拘束条件を微分幾何学とニュートン力学に基づき厳密に取り扱っていた．そのダイナミクスを用いた数値シ
ミュレーション法は，吉田らによって提案された 3)4) ．
特に文献 4) では，弧長パラメータでは直接表現できな
い楕円形状を有する物体の安定化問題を取り扱っていた．しかし，実際に我々の生活に存在する物体をデジタイジングしてコンピュータに取り込む際，物体は離散点として与えられるので，そのような場合にどのよ

q1
yq2

X
θ

Omq3

Y

Fig.1 One robot ﬁnger pinches an object pivoted at
a ﬁxed point Om

うな方法を用いて取り扱うかという重要な問題を見逃してきた．
そこで本稿では，Fig.1 に示す人差し指の腹を模した形状関数を指先として有する１本のロボット指を用いて，点 Om をピンでさした物体 (従って，並進運動はできないが，その点回りの回転は可能) を安定化させる問題を取り扱う．ここでロボットの指先の形状は，人の人差し指の腹の写真から離散点として 10 点を計測し，その点同士を 3 次スプライン関数を用いて補間することにより作成する．3 次スプラインを用いる理由は，2階微分まで形状関数が連続であるので，その形状関数の曲率を常に導出することが可能だからである．また
文献 2) と同様に，ロボット指と物体とは常に接触しており，滑らずに転がると仮定する．指ロボットと物体とは全て剛体でできているとし，重力の影響は考慮し第10回システムインテグレーション部門講演会(SI2009)（2009年12月24日～26日・東京）-1367-O

SY0013/09/0000 - 1367 © 2009 SICE

線に座標系 O01 -X0 Y0 を貼り付け，人差し指の腹の座標を 10 点計測した．その 10 点を 3 次スプライン関数を用いて補間することにより Fig.4 に示す指の形状関数を獲得した．この際，Fig.4 に示される形状関数のスケールは，おおよそ Fig.2 に示される実際の指の大きさと同じとなるように変換した．ここで，この形状関数を O01 -X0 Y0 を用いて

X0 (t1 ) = f (t1 ),3.

X0I θ̈ + f lb1 (s) − λln1 (s) = 0(2)

 T

1
G1 (q 1 )q̈1 +Ġ1 + S1 q 1 + f J1 (q 1 )n1 + lb0 (s)e2 T+λ J1 (q 1 ) − ln0 (s)e = u(3)Fig.3 The side of index ﬁnger attached on the local
coordinate O01 -X0 Y0と導出されている．ここで，I は物体の慣性モーメント，G1 は指ロボットの慣性行列，f と λ はラグランジュ乗数を表し，q 1 は q 1 =(q1 , q2 , q3 )T とする指ロボットの関節角度ベクトル，θ は物体の回転角度，e=(1, 1, 1)Tを表す．ここで s は，ロボットの指先と物体との形状を表す孤長パラメータを表しており，その更新則は文献 2) より
ds = q̇1 + q̇2 + q̇3 − θ̇{κ0 (s) + κ1 (s)}
(4)
dt

-0.01

Interior
O 01

X0[m]

Exterior
Y0[m]0.01となる．本報告にて取り扱う指先形状は孤長パラメター
では無く，パラメータ t1 であるので，文献 2) を参考にして更新則は
 dtdt1(5){κ0 (t1 ) + κ1 (s)} 1 = q̇ T1 e − θ̇dtds

0.02
-0.01

00.010.02

Fig.4 Shape function expresses the side of index ﬁnger by cubic spline

ないとする．このとき，指ロボットは Fig.1 より 3 自由度有しており，物体は 1 自由度を有している．この指-物体からなるモデルを安定化するために，物体情報を全く用いない制御入力を用いる．最後に，このモデルに基づく数値シミュレータに提案する制御入力を用いて安定化可能であることを示す．

2.

数値シミュレータ構築法

前節で述べた形状関数を用いた人差し指を模したロボット指を用いて，物体を安定化させるための数値シミュレータ構築法について述べる．指先と対象物体との輪郭線を任意形状とした場合のダイナミクスは，有本により 2)O010(1)と定義する．t1 が形状関数を表すパラメータ，f () を上述した 3 次スプライン関数とする．Fig.4 をよく観察す
ると，凹凸があることに気がつく．このような凹凸は，現実の世界にある対象物をコンピュータに取り込む場
合には，コンピュータを用いて作成した理想的なモデルでは無いので，割けることはできない．

Fig.2 Picture of human hand

Y0Y0 (t1 ) = t1

指先の形状関数

人差し指を模倣した指の形状関数の作成法を以下に述べる．手の写真を撮影し (Fig.2)，その画像から人差し指の輪郭線を抽出する (Fig.3 の青い線)．その輪郭-1368-と表わされる．ここで，dt1 /ds は2
dt1dX(t1 )dY (t1 )
= 1/+
ds
dt1
dt12となる．特に今回用いるスプライン関数の場合
2
dt1df (t1 )= 1/ 1.0 +dsdt1(6)(7)となる．また，κ0 と κ1 は指先と物体の形状関数の曲率を表しておりそれぞれ

κ0 (t1 ) =−X0 (t1 )Y0 (t1 ) + X0 (t1 )Y0 (t1 )3/2(X0 (t1 )2 + Y0 (t1 )2 )
κ1 (s) = X (s)Y  (s) − X  (s)Y  (s)

(8)
(9)

となる．
数値シミュレーションを行うための初期値を導出する
際に，指先の形状を表すパラメータ t1 を孤長パラメー
タ s0 に変換した値と物体形状を表す孤長パラメータ sとが同じ値になるようにしなければならないため (文献2)参考)，パラメータ t1 から孤長パラメータ s0 へ変換するのに Simpson による数値積分法を用いた．つまり
22
t1dX(t1 )dY (t1 )s0 =+dt1 (10)dt1dt10とした．実際には，指先形状と物体形状とが一点で接していることを確かめる干渉チェックを行う必要があるが今回は使用していない．

4.

制御入力

物体の回転を静止させるためにロボット指に加える制御入力を文献 2) より

u=−cq̇1 −βJ1T (q 1 )x01 − xy01 − y(11)とする．c,β はある正の定数とする．ここで，右辺第 1項は粘性項を表しており，第 2 項は物体の回転中心と指ロボットの点 O01 との距離を最短にすることにより，物体の回転を止める項である．どちらの項も物体情報が入っていないことに注意する．

5.

数値シミュレーション

第 3 章にて説明した数値シミュレータと第 4 章で提案した制御入力，さらに 2 種類の拘束 (転がり，接触拘束) を数値シミュレーションの間維持するための CSM
法 3) を用いて，数値シミュレーションを行った．その際に，物理パラメータとして Table.1 の値を，制御パラメータとして Table.2 の値を用いた．この場合の物
Table 1 Physical parameters of the ﬁnger and object.

l1

length0.065 [m]l2length

0.039 [m]l3length0.026 [m]m11

weight0.045 [kg]m12

weight0.025 [kg]m13

weight0.015 [kg]Mobject weight0.040 [kg]control gain

20.0 [N/m]cdamping coeﬃcient

0.006 [Nms]

Y [m]

Om

0-0.01-0.03-0.02-0.01

00.01

X [m]

Fig.5 Shape function expresses the contour of the objectを用いた．式 (12) と (13) により描かれる形状関数をFig.5 に示す．数値シミュレーションの妥当性を確認するために，提案する制御入力 (式 (11)) を指-物体からなるダイナミクス (式 (2) と (3)) に加え，下記の閉ループダイナミクスを導出した．

I θ̈ + Δf lb1 (s) − Δλln1 (s) + βN1 = 0
1G1 (q 1 )q̈1 +Ġ1 + S1 q 12
 T
+Δf J1 (q 1 )n1 + lb0 (s)e+Δλ J1T (q 1 ) − ln0 (s)e1 − βN1 e1 = 0

体の形状関数として

X(s)=−0.03

1 + 4 × 502 × (s − 2.472 × 10−4 )2(12)+2 × 50Asinh(2 × 50 × (s − 2.472 × 10−4 ))(13)
Y (s)=
2 × 50-1369-(14)(15)ここで，Δf = f − β (ln0 (s) + ln1 (s))(16)Δλ = λ + β (lb0 (s) + lb1 )(17)

N1 = −lb0 ln1 + ln0 lb1(18)とする．ここで，指-物体からなるシステム全体が安定であるときロボット指と物体の各加速度，および速度がゼロであるから，つまり，q̈1 ,q̈2 ,q̈3 ,θ̈,q̇1 ,q̇2 ,q̇3 ,θ̇→ 0(t →∞) であるから，指-物体からなるシステム全体の平衡状態は式 (14) と (15) よりΔf lb1 (s) − Δλln1 (s) + βN1 = 0
Δf J1T (q 1 )n1 + lb0 (s)e+Δλ J1T (q 1 ) − ln0 (s)e1 − βN1 e1 = 0Table 2 Parameters of control signals

β0.01(19)(20)と表わせる．
数値シミュレーションの様子を Fig.6∼9 に示す．Fig.6と 7(Fig.6 の拡大図) に示す初期姿勢では，接線力と法線力の合力が物体の回転中心 (Fixed Point) を貫通していないが，20 秒後には，合力が回転中心を貫通していることが Fig.8 と 9(Fig.8 の拡大図) より確認できる．
Fig.10 ∼ 20 に数値シミュレーションから得られた主な結果を示す．各ロボット指の関節角速度 q̇1 ,q̇2 ,q̇3 および物体の回転速度 θ̇ がゼロに収束していることが Fig.13∼ 16 より確認できる．また，式 (19) と (20) に示される
Δf ,Δλ,N1 がゼロに収束していることを Figs.10,11,12より確認できる．従って，t → ∞ に物体を安定化できたことを数値シミュレーションの観点から示した．この数値シミュレーションにおける孤長パラメター s とパラメター t1 の値を Fig.17 と 18 に示すことにより，

1200

Δλ[N]

-1-2Δf
The value of zero

-2051015N1[m2]

Δ f[N]

0-0.001ΔλThe value of zero-4

2005time[s].

1015-4-205time[s]Fig.13 q̇115-42005101520time[s]Fig.14 q̇2Fig.15 q̇3
s0.01

t1 '

0.01

0.008

t1'[m]

s[m]

.

0.008

-50.006.
θ
The value of zero
051015200.006

051015200.004

0time[s]time[s]5101520time[s]Fig.18 t1Fig.17 s

Fig.16 θ̇0.0001

05e-05

R1[m]

Q1[m]

-0.0002

0R1
The value of zero
51015time[s]Fig.19 R1

Fiexd Point

20Q1
The value of zero
-5e-0505101520time[s]Fig.20 Q1ンを行なうと転がりと接触の拘束条件の維持が難しくなる現象が生じた．今後はこのような現象を起りにくくするために，与えられたデータ点列を全て通過する方法で補間するのではなく，滑らかな曲線に近似する方法を使用し，数値シミュレーションの妥当性を検証する必要がある．

Contact Point
Tangent Force

Fig.6 Initial pose10.
q3
The value of zerotime[s]0200.
q2
The value of zero2015.

-2510Fig.12 N10.
q1
The value of zero
05time[s].

-20

Total Force020

q3[rad/sec]

0-0.0004

Normal Force

15

Fig.11 Δλq2[rad/sec]

q1[rad/sec]

Fig.10 Δf-410

N1
The value of zerotime[s]θ [rad/sec]

数値シミュレーションの間にロボットと物体との接触点は約 4.4cm 動いたことがわかる．また，R1 は指先と物体とが滑ることなく接することを表すホロノミック
拘束を示し，Q1 は指先と物体とが常に接触するホロノミック拘束を示している 2) ．これらの拘束が維持されている場合，理論的には R1 = 0,Q1 = 0 であるが，数値シミュレーションの間この条件を維持するためには様々な方法が提案されており，今回は CSM 法を用いた．
今回行った数値シミュレーションにおける R1 と Q1 の値を Fig.19 と 20 に示す．これらのグラフから，数値シミュレーションを始めてすぐに R1 の値が −0.4mm，Q1 の値が 0.1mm の値を取っていることから，拘束条件をうまく維持できていなことがわかる．その理由として，物体側面が指先にめり込んでいる可能性がある．
これは作成した形状関数が，文献 2) に示される 2 種類の拘束に関する仮定を満足するのに必要な接触点近傍に他の接触点が存在することなく，その点において常に共通接線が引ける条件を満たしていたいためだと考える．この現象を回避する方法として，与えられたデータ点列を通過するように補間曲線を作成するのではな
く，なるべく振動が少なくなるように滑らかな曲線を作成する必要がある．その視点から考えると，3 次ス
プライン関数はデータ点列を全て通過するように曲線を作成するため，提案した数値シミュレータに組み込むには適切では無いと考える．

Fig.7 Zoom of initial pose

謝辞 本研究は，日本学術振興会科学研究費補助金基盤研究(B)(課題番号 20360117) の交付により行われたものである．

参考文献
1) B. Sicilliano and O. Khatib, Eds., Springer Handbookof Robotics, Springer, New York, NY, USA, 2008.
2) 有本卓, “ロボットの腕や手の巧みさと制御:リーマン幾何学に基づくアプローチ”,IEICE Fundamentals Review,Vol.2, No.4,pp.1-11, 2009.
3) 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.
4) 吉田守夫，有本卓，田原健二，“楕円形状を有する 2 次元物体把持の数値シミュレータ構築法”，第 27 回日本ロボット学会学術講演会，RSJ2009AC3A2-01，2009 年 9月，横浜国立大学

Normal Force

Fixed Point

Total ForceContact Point

Tangent Force

Fig.8 After 20 seconds

6.

Fig.9 Zoom of 20
seconds’ pose

まとめ

人の人差し指の腹を模した指先形状を有するロボッ
ト指１本を用いて任意形状の輪郭を持つ物体を安定化させる問題を取り扱った．ロボットの指先形状は，人の人差し指の腹を模した形状を 3 次スプライン関数を用いることにより描いた．その指先形状は細かな凹凸を有しており，この形状を用いて数値シミュレーショ

-1370-