Paper (pdf file)

2 本指ロボットハンドによる指先の転がりを考慮した対象物の把握・操り
—拘束の Nonholonomic 性を利用した対象物と接触点の制御—
名古屋大学 ° 中島 明
長瀬 賢二
早川 義一
Grasping and Manipuation of an Object by Two-fingered Robot Hand
with Rolling Contact at the Fingertip
—Control of the Object and Contact Points by Using the Nonholonomy of the Constraints—
° Akira Nakashima, Kenji Nagase and Yoshikazu Hayakawa, Nagoya University
Abstract: In this paper, we discuss grasping and manipulation of an object by two-fingered robot hand, each of
which has six degree of freedom. The contact motion between the object and each fingertips is assumed to be the pure
rolling. First, we provide a general formulation of a linearizing compensator for the motion of the object, the grasping
force and some of the contact points. Second, we propose a control method which regulates of all contact points by
utilizing a closed path of the controlled contact points. Numerical examples are shown to prove the efficiency.
1
object
はじめに
finger1
近年，多指ハンドロボットによる物体の把握・操りにお
いて盛んな研究がなされている 1) ．ハンドシステムは指
先の滑り・転がりを利用することで接触点の移動が可能
であり，把持点の変更などができる．特に転がり接触で
は拘束の nonholonomic 性 2) を利用して接触点の移動を
行なうため，滑りに比べて安定した把握・操りの制御が可
能である．転がりを伴う把握・操りにおける従来の研究で
は，接触点の運動は考慮されているものの，対象物の操り
による接触点の変化を受動的に考慮し，拘束式に反映さ
れているだけのものが多い 3) ．また，文献 4) においては，
接触点の一部の制御は行なわれているが，nonholonomic
性を積極的に利用していないため全ての接触点の制御は
成されておらず，内力の制御もされていないため制御則
は完全な記述とはなっていない．
本研究では，6 関節 2 本指ロボットハンドによる転がり
接触での把握・操り系において，対象物の位置・姿勢，把
握力，及び全て接触点の制御を考える．はじめに，対象
物の位置・姿勢，把握力，及び接触点の一部に対する線
形化補償器を導出する．次に，接触点の制御においては，
上記の補償器により接触点の一部の軌道追従がなされて
いるものとして，その軌道を用いたレギュレーションを行
なうことを考える．指先が球状，対象物が直方体ならば，
接触点の運動は平面を転がる球の運動となり，Chained
form のような正準形に変換できないので一般にその制御
は難しい 5) ．この問題に対して本研究では，接触点の一
部が単一の閉軌道を繰り返すことにより，残りの接触点
の全てを目標点へと到達させる手法を提案する．提案す
る閉軌道により，目標点に近づけるような閉軌道を決定
するパラメータが必ず存在することを示す．また，本手
法の有効性を数値シミュレーションにより確認する．
2
2.1
モデル化
対象モデル
Fig.1 のような 6 関節 2 本指による対象物の把握・操
り系を抽象化した，中心に並進力・モーメントの 6 成分
6
τ i ∈ R （i = 1, 2）が入力可能な指先による対象物の把
f1
p
o
poco
1
o
f1 cf
1
ΣO
ith contact point
poco
(xfi )
2
finger2
f2
Σ F1
po , φ o
pf1 , φf1
pf2 c
f2
Σ F2
finger i
xfi
ΣLf (t)
i
= ΣCf
i
+ψi
x oi
object
zfi
ΣLo (t)
i
z oi
= ΣCo
i
y oi
yfi
pf2 , φf2
ΣB
Fig.1
An object grasped by two fingers.
握・操り系を考える．この把握・操り系について以下の
ことを仮定する．
仮定 1： 指先・対象物の表面形状は滑らかであり，局所
的に 2 次曲面である．
仮定 2： 指先力は soft-finger タイプとし，並進力 3 成分
に加えて接触点法線回りのモーメントの計 4 成分を
持つ 5) ．ただし，指先のへこみは十分に小さく無視
出来るものとし，指と対象物は点接触とする．また
摩擦はクーロン摩擦のみである．
Fig.1 左図 において，ΣB は基準座標系，ΣFi ，ΣO はそ
れぞれ指 i と対象物の中心に固定された座標系である．右
図における ΣCfi , ΣCoi はそれぞれ接触点 i における指・
対象物側の接触点座標系，また，ΣLfi ，ΣLoi は指・対象
物表面に固定された局所座標系であり，これらはそれぞれ
時刻 t において ΣCfi , ΣCoi に一致するものである．Rab
は ΣB から ΣA への回転行列とする．以後文中で現れる
変数の左肩添え字は表現される座標系を表し，特に表記
のない場合は ΣB で表されているものとする．
T
指 1，2 と対象物の位置・姿勢を xf := [ xT
f1 xf2 ] ∈
R12 ，xfi := [ pTfi φTfi ]T ∈ R6 ，xo := [ pTo φTo ]T ∈ R6
（p は位置，φ は姿勢）と表し，一般化座標 q を q :=
18
T T
[ xT
とする．接触点の位置は指・対象物
f xo ] ∈ R
の座標系から接触点までのベクトル
3
fi
3
pfi cf (αfi ) ∈ R ，
i
poco (αoi ) ∈ R で表す．αfi := [ ufi vfi ]T ，αoi :=
i
[ uoi voi ]T は指・対象物表面の 2 次曲面上の座標であ
o
−emτ
る．接触状態はこの αfi ，αoi と，ΣCfi と ΣCoi の相対
姿勢 ψi（Fig.1 右図）で表すことができるため，contact
5
T
T
coordinates と呼ばれる η i := [ αT
fi αoi ψi ] ∈ R で
T T
表され，これらをまとめて η := [ η T
1 η2 ] ∈
おく．
2.2
R10 と
V
=
i
·
¸
¤ ẋfi
Glfi (xfi , η i ) − Gloi (xo , η i )
(1)
ẋo
η̇ i
0
=
=
Gηi (η i )V blo lf
i i
vzi
Fig.2
e1z
m(2)
Internal moment F cN
A(q, η)q̇ = 0, A(q, η) ∈ R
(3)
上式の vxi = 0, vyi = 0, ωzi = 0 を (2) 式の第 1 式に代入
して整理すると，
·
¸
ωxi
5×2
η̇ i = A⊥
(η
)
, A⊥
(4)
i
ηi
ηi ∈ R
ωyi
のような転がり運動の式が得られ，この式から ωxi ，ωyi
を消去すれば，η i に関する拘束式
3×5
(5)
を得る．ここで Aηi A⊥
ηi = 0 である．この (5) 式は指と
対象物の表面形状が反射的でない限り η i に関する完全
nonholonomic 拘束である 7) ．接触状態にある指 i と対象
物の相対的な位置・姿勢の集合を gofi とすると，指と対象
物の形状が特異でない限り，gofi と η i の間において局所的
に滑らかな全単射が存在する 8) ので，η i が nonholonomic
拘束 (5) 式を受けるならば gofi も nonholonomic 拘束を
受けることが分かる．したがって，(1) 式と (2) 式の第 1
式により得られる η i と一般化座標 xfi ，xo の関係
£
η̇ i = Gηi Glfi
− Gloi
·
¸
¤ ẋfi
ẋo
8×18
(7)
を得る．ただし A := Āη Ḡ であり，
i
Aηi (η i )η̇ i = 0, Aηi ∈ R
emτ
拘束と 1 本の holonomic 拘束である．これらを接触点 1，
2 についてまとめれば拘束式
·
Āη :=
6×6
vxi = 0, vyi = 0, vzi = 0, ωzi = 0
C2
e2z
pc12
Internal linear force and internal moment.
(2)
である．(1) 式の Glfi ，Gloi ∈ R
はそれぞれ ΣFi か
ら ΣLfi まで，ΣO から ΣLfi までの変換行列であり，(2)
式は Montana 6) により定式化されたものである．なお，
5×6
(2) 式において，Gηi ∈ R
の 3 列目は 0 ベクトルで
あり vzi は η̇ i には寄与しないこと，また 0 = vzi は接触
条件であることに注意されたい．
さて，転がり接触を純粋転がりとすれば拘束条件は
V blo lf を用いて以下のように表される 5, 6) ．
i
21
(1)
いる 5) ．一般化座標 xfi ，xo ならびに η i との関係は
£
C1
Internal linear force F cN
接触状態における指と対象物の相対運動を特徴づける
ものとして，ΣLoi に対する ΣLfi の速度を ΣLfi で表現
したもの V blo lf := [ vxi vyi vzi ωxi ωyi ωzi ]T を用
b
l oi l f i
cf
2e
cf
1 e12
拘束式の導出
i
C2
pc12
C1
(6)
·
¸
¤ ẋfi
を (5) 式に代入した Aηi Gηi Glfi − Gloi
=0
ẋo
は一般化座標に関する nonholonomic 拘束である．また
vzi = 0 を (1) 式に代入したものは holonomic 拘束となり，
したがって接触点 i における拘束は 3 本の nonholonomic
Āη1
04×6
·
Gη1
 e16
Ḡ := 

05×6
¸
·
04×6
Aηi
, Āηi :=
Āη2
01×5
¸
03×1
1
¸
(8)

·
05×6  ·
¸  Glf1
Gη2  06×6
e16
06×6
Glf2
−Glo1
−Glo2
¸
e16 := [ 0 0 1 0 0 0 ]
(9)
である．よって，拘束式 (7) 式の 8 本の拘束は，6 本の
nonholonomic 拘束と 2 本の holonomic 拘束である．な
お以降では，場合により A = [ Ah − Ao ]，Ah ∈
R8×12 ，Ao ∈ R8×6 と分割した表現を用い，rank Ah =
8，rank Ao = 6 であるとする．これらは指と対象物が特
異な関係でなければ成立するものである．
2.3
内力の表現
8
仮定 2 より，指先力 c F c ∈ R を以下のようにおく．
c
F c := [ c1 F T
c1
c2
T
FT
c2 ] ,
ci
F ci := [ ci f T
ci
4
ci
τci ]T (10)
3
F ci ∈ R は接触点 i での指先力，その成分 ci f ci ∈ R
は並進力，ci τci は接触点における接平面法線回りのモー
メントである．拘束は仕事をしないとすると仮想仕事の原
6
c
b
理より b F o = AT
o F c が成り立つ．ただし F o ∈ R は
指先力により対象物に加えられる合力である．指先力を対
象物の運動に寄与する操り力 c F cm と寄与しない把握力
6×8
c
F cg に分解する．AT
であるから，c F cg は 2 自
o ∈R
由度を有しており，その表現に一意性はないが，物理的に
対象物の把持として意味のある表現が必要である．ここ
では，後述する内並進力と内モーメントにとり，それぞれ
の大きさをあらわす把握力パラメータ hg := [ hgf hgτ ]T
を用いて
ci
£
c
F c := c F cm + c F cg
(11)
ただし
c
+b
F cm := (AT
F o,
o)
B g := [
c
(1)
F cN
c
c
(2)
F cN
F cg := B g hg
(12)
]
(13)

b ∧ −
RT
bcf1 ( pc12 ) emτ


1




bp
be
·


1z
c12
:= 

T
b
∧
−
 −Rbcf ( pc12 ) emτ 
2




−1
bp
b
c12 · e2z

c
c (1)
F cN

f1
e12
 0  c (2)

:= 
 cf2 e21  , F cN
0
と表す．行列右肩の − は一般化逆行列，ベクトル右肩
8×2
の ∧ は外積に等しい歪対称行列を表す．B g ∈ R
は
AT
B
=
0
を満たす把握力の方向を特徴付けるものであ
g
o
(1)
(2)
り，c F cN ，c F cN はそれぞれ内並進力，内モーメントの
(1)
方向を表す．c F cN は，Fig.2 のように接触点 1 から 2
へと向かうベクトル b pc12 の向きで互いに押しつけ合う
ものであり，cf1 e12 ，cf2 e21 はその線分上の単位ベクトル
(2)
で b e21 = −b e12 である．c F cN は，Fig.2 にあるように
b
pc12 に直交する平面と接触点 1・2 における接平面の単
位法線ベクトル b e1z ，b e2z により張られる平面の交線方
向 emτ 回りに拮抗するモーメントである．
(1)
(2)
このように c F cN ，c F cN の方向で hg を調整すること
により，対象物を把握する力を設定できる．なお，これ
(1)
(2)
らの方向は c F cN · c F cN = 0 であるので，互いに独立に
与えられることに注意する．以降では rank B g = 2 であ
るとする．
2.4
運動方程式
以上から把握・操り系は以下で与えられる．
−T
c
M f ẍf + C f ẋf + N f = −AT
h Fc + Tf τ
c
M o ẍo + C o ẋo + N o = AT
o Fc
(14)
[ Ah (xf , η) − Ao (xo , η) ]q̇ = 0
ただし η と q の関係は (6) 式を接触点 1，2 についてま
とめた以下で与えられる．
·
G :=
Gη1
05×6
η̇ = G(q, η)q̇
¸·
Glf1 06×6
05×6
Gη2
06×6 Glf2
12×12
6×6
(15)
−Glo1
−Glo2
¸
(16)
12×12
Mf ∈ R
，M o ∈ R
，C f ∈ R
，C o ∈
R6×6 ，N f ∈ R12 ，N o ∈ R6 は指・対象物の慣性行列，
6
遠心コリオリ力項，重力項である．
·
¸τ i ∈ R は指 i への
T f1 06×6
入力である．T f :=
であり，T fi は指 i
06×6 T f2
の位置・姿勢の速度から位置の速度・角速度への変換行
列である．
3
3.1
制御系設計
制御系設計の方針
(14) 式で与えられるシステムは，これまでの議論から
分かるように速度 q̇ に関する 8 本，位置 q に関する 2 本
の拘束を受けるので，システムが本質的に持つ自由度は
12
q̇ : 10，q : 16 である．一方，入力 τ ∈ R は把握
力の制御に用いる 2 入力を除いた 10 入力を運動の制御に
用いることができるので，この 10 入力により速度の自由
6
度 10 は直接制御可能である．速度の自由度を ẋo ∈ R ，
4
v ω := [ ωx1 ωy1 ωx2 ωy2 ]T ∈ R にとり，これらの軌
道追従が成されているとすれば，η は完全 nonholonomic
拘束 (5) 式を受けるので，この拘束を満たす (4) 式に従
い， v ω に適当な軌道を与えることによりレギュレーショ
ンが可能である．よって制御系設計の方針としては，【1】
hg ，ẋo ，v ω に対する線形化補償器を設計して，【2】 v ω
を用いた η の制御則を設計する，ということになる．
3.2
線形化補償器の設計
T T
線形化によりサーボを行なう変数を v := [ ẋT
o vω ] ∈
10
R とおき，この v を q̇ から取り出すことを考える．q̇
は (7) 式の拘束 A(q, η)q̇ = 0 を受けるので，以下のよう
にとる．
q̇ = S(q, η)v, A(q, η)S(q, η) = 0
(17)
18×10
この S(q, η) ∈ R
を 2.2 節の拘束式 A(q, η)q̇ = 0
の導出の過程を利用することにより導く．(7)∼(9) 式を
T
T
T
中間変数 η̄˙ := [ η̄˙ 1 η̄˙ 2 ]T ，η̄˙ i := [ η̇ T
i vzi ] を用いて
以下のように改めて表現する．
8×12
Āη η̄˙ = 0, Āη ∈ R
12×18
η̄˙ = Ḡq̇, Ḡ ∈ R
(18)
(19)
まず，(18) 式を満たす η̄˙ の一般解うち，v ω を自由度に
とる解は (4)，(5) 式より
· ⊥ ¸

Aη1
06×2 
 01×2
⊥
⊥
· ⊥ ¸
η̄˙ = Āη v ω , Āη := 
(20)

Aη2 
06×2
01×2
と記述できる．また，(19) 式の一般解は
18
+
+
q̇ = Ḡ η̄˙ + (I 18 − Ḡ Ḡ)z, z ∈ R
(21)
+
とかける．(I 18 − Ḡ Ḡ)z が本質的に 6 自由度を持つの
+
+
で，Ḡ ，(I 18 − Ḡ Ḡ)z を
"
#
·
¸
+
K1
Ḡ1
+
+
Ḡ :=
(22)
+ , (I 18 − Ḡ Ḡ) :=
K2
Ḡ2
+
12×12
+
6×12
12×18
のように Ḡ1 ∈ R
，Ḡ2 ∈ R
，K 1 ∈ R
，
6×18
K2 ∈ R
に分割し K 2 が行フルランクとすれば，
6
18×6
z := Dz̄ （z̄ ∈ R , D ∈ R
）において
+
z̄ = (K 2 D)−1 (−Ḡ2 η̄˙ + ẋo )
(23)
と選ぶことで，z̄ の 6 自由度を ẋo にとることができる．
以上より，(21) 式に (20)，(22)，(23) 式を代入して v に
ついてまとめれば以下の S を得る．
·
¸
S1
12×10
6×10
S :=
, S1 ∈ R
S2 ∈ R
(24)
S2
ただし，
£
+
+
⊥ ¤
S 1 := K 1 D(K 2 D)−1 {Ḡ1 − K 1 D(K 2 D)−1 Ḡ2 }Āη
£
¤
S 2 := I 6 06×4
z0
以上から得られた S を用いて v に対する線形化補償
器を導出する．把握・操り系 (14) 式を制御変数である v ，
hg についてまとめる．(14) 式の運動方程式と指先力に関
する式 (11)，(12) 式から b F o を消去し，q̇ = Sv を代入
すれば
−T
M̄ v̇ + AT
(25)
h B g hg + V̄ = T f τ
T +
M̄ := M f S 1 + AT
h (Ao ) M o S 2
Cf
θ1
Bf
0
z0
6
M̄ (v̇ − u) + AT
h B g (hg − ug ) = 0
4
(29)
T T
が得られる．この両辺に左から S T
1 をかけて，S 1 Ah =
T T
T
T
S 2 Ao と Ao B g = 0 に注意すれば，S 1 M̄ (v̇−u) = 0 と
T
T
なる．(26) 式より S T
1 M̄ = S 1 M f S 1 +S 2 M o S 2 である
T
から S 1 M̄ は正定であるので，v̇ −u = 0 が成り立つ．ま
た，v̇−u = 0 を (29) 式に代入すれば AT
h B g (hg −ug ) = 0
が得られ，AT
，
B
が列フルランクであれば，
hg −ug = 0
g
h
が成り立つ．
最後に，η̇ と制御変数 v の関係を導く．Ḡ := [ Ḡ1 Ḡ2 ]，
12×12
12×6
Ḡ1 ∈ R
，Ḡ2 ∈ R
とおけば，(22) 式より
+
+
Ḡ1 Ḡ1 + Ḡ2 Ḡ2 = I 12 ，Ḡ1 K 1 + Ḡ2 K 2 = 0 である
⊥
ので，これらを用いて ḠS = [ 012×6 Āη ] が得られる．
(9)，(16) 式より G は Ḡ の 6，12 行目を除いたものであ
るから，
GS = [ 010×6 A⊥
(30)
η]
が得られる．したがって (15)，(24) 式より
η̇ = GSv = A⊥
η vω
Eo
∆α0o
ρθ1
u0o
Ao
vo
z
vo0
ẍo = uo , v̇ ω = uω , hg = ug
y0
π−ψ+ϕ
x, x0
ϕ
y
(32)
(33)
(32) 式が対象物の位置姿勢 xo ，把握力パラメータ hg と
接触点における転がり速度 v ω の制御系であり，これら
は単純な PID コントローラなどにより制御可能である．
また，v ω の軌道を適当に与えれば，(33) 式に従い η が
制御される．
閉軌道の繰り返しによる接触点のレギュ
レーション
ここでは，指先の形状を球，対象物を直方体に限定して
考える．これにより (33) 式は平面を転がる球のモデルに
∆αo
uo
u0o
The Closed path on the sphere generates the
motion in the plane (uo , vo ) and the angle of contact
ψ.
Fig.3
なる．なお，接触点 1 と 2 は同じモデルになるので，添
え字 i は省略する．球（指）上の接触点 αf = [ uf vf ]T
を uf が東経，vf が北緯の向きになるようにとる．この
とき η̇ と α̇f の関係 (33) 式は以下のようになる．
  

u̇f
0
1
 v̇f  

1
0
  

 u̇o  =  ρ cos vf cos ψ
 α̇f
−ρ
sin
ψ
(34)
  

 v̇o   −ρ cos vf sin ψ −ρ cos ψ 
0
sin vf
ψ̇
ρ は球の半径である．ただし v ω と α̇f は
·
¸
0 1
vω =
(K gf + K̃ go )M gf α̇f
−1 0
2×2
(31)
が得られる．
以上から，把握・操り系 (14) 式及び接触点の運動の式
(15) 式は以下のシステムとなる．
3.3
Co
ρθ2
(27)
T T
ただし u := [ uT
o uω ] ∈ R ，uo ∈ R ，uω ∈ R は
2
v に対する入力であり，ug ∈ R は hg に対する入力で
ある．システム (25) 式にコントローラ (28) 式を適用する
と，以下の閉ループ系
η̇ =
Do
ρθ2
Bo
である．したがって線形化補償器は以下のように与えら
れる．
T T
τ = TT
(28)
f (M̄ u + V̄ ) + T f Ah B g ug
A⊥
η vω
y0
Af , E f
T +
V̄ := (M f Ṡ 1 + C f S 1 + AT
h (Ao ) C o S 2 )v
10
ρ cot θ2
θ2
(26)
T +
+N f + AT
h (Ao ) N o
θ1 sin θ2
Df
x
が得られる．ただし
vo0
(35)
2×2
の関係を持ち，K gf ∈ R
，K̃ go ∈ R
，M gf ∈
2×2
R
は指，対象物の表面形状により決まる行列である
5)
．本節では，指上の接触点 αf の閉軌道を繰り返すこ
とにより目標点へのレギュレーションを行なう．
閉軌道としては，Li ら 9) により提案された閉軌道の拡
張として，3 つの自由度を持ち，システム (34) 式の特異点
vf = π2 を通らないものを提案する．すなわち，Fig.3 上段
のように球上において Af → Bf → Cf → Df → Ef (Af )
とたどる経路を，Fig.3 下段のように赤道から π + ψ − ϕ
だけ傾いて行なうような閉軌道を考える．このときの αo ，
ψ の変化量 ∆αo ，∆ψ は (34) 式を積分して求めれば
·
¸
cos ϕ sin ϕ
∆α0o (θ1 , θ2 )(36)
∆αo (θ1 , θ2 , ϕ) =
− sin ϕ cos ϕ
{z
}
|
Rϕ (ϕ)
∆ψ(θ1 , θ2 ) = −θ1 sin θ2
(37)
ただし
·
¸
−ρθ1 + ρ(cot θ2 + θ2 ) sin(θ1 sin θ2 )
∆α0o :=
(38)
ρ(cot θ2 + θ2 )(1 − cos(θ1 sin θ2 ))
となる．なお，∆α0o は Fig.3 上段の (u0o , vo0 ) 平面上の
変化である．各パラメータの範囲は −π < θ1 < π, − π2 <
θ2 < π2 , −π < ϕ < π である．閉軌道を決定するパラメー
タは θ1 , θ2 , ϕ の 3 つであるので，目標点に合わせてパラ
メータを調整することにより αf , ψ を目標点へと持って
いくことが期待できる．実際，以下の定理を示すことが
できる．
T
定理 1 η̃ := [ αT
o ψ ] とし，(36)∼(37) 式により与え
T
られる閉軌道による変化を ∆η̃ := [ ∆αT
o ∆ψ ] とお
く．一般性を失うことなく目標点を原点とし，η̃ から原
点へと移動することを考える．このとき，以下のことが
成り立つ．
∀
η̃,
∃
∆η̃
s.t. kη̃ + ∆η̃k < kη̃k
(39)
証明は付録を参照されたい．この定理は，(39) 式から分
かるように，任意の η̃ から原点に近づく閉軌道が必ず存
在することを示している．
目標点を原点としたときの制御アルゴリズムを以下に
示す．
Step.1： 指上の接触点 αf を原点へと持っていく．
Step.2： αf の閉軌道の繰り返しにより αo ，ψ を原点へ
と持っていく．k 回目の閉軌道を決定するパラメー
タ θ1 [k], θ2 [k], ϕ[k] は以下の最小化問題
(θ1 , θ2 , ϕ) = arg min kη̃[k] + ∆η̃k
θ1 ,θ2 ,ϕ
(40)
を解くことにより求める．
4
シミュレーション
指（球）の半径は指 1，2 ともに 1.0 × 10−2 [m]，対象物
は一辺が 0.10[m] の立方体とする．制御目的は，転がり
接触において対象物を落とさないような把握力を加えな
がら，対象物の位置・姿勢，接触点を目標値へと持って
いくことである．把握力パラメータの目標値は，文献 10)
のように指先力から動的に摩擦円錐内に入るように決定
する．
シミュレーション結果を Fig.4，Fig.5 に示す．Fig.4
は線形化補償器による軌道追従の様子を表しており，左
から xo ，αf1 ，αf2 ，hg を表す．図では重なっているが，
破線が目標軌道，実線が実際の軌道であり，全ての変数
が目標軌道へと追従している事がわかる．なお αf1 の軌
道の目標軌道は (40) 式により決定している．次に，Fig.5
は αf1 の 閉軌道による η 1 のレギュレーションを表して
おり，左図の上 2 つは左から対象物上の接触点 1 の座標
uo1 ，vo1 ，左下は相対姿勢 ψ1 を表す．右図はそれを平面
(uo1 , vo1 ) 上に描いたものであり，矢印により最初と最後
の姿勢 ψ1 が示してある．これらより提案した閉軌道を
描いて αo1 ，ψ1 が目標値（原点）へと到達していること
が分かる．
5
おわりに
本研究では，6 関節 2 本指による把握・操り系において，
対象物の位置・姿勢，把握力，接触点の一部に対する線
形化補償器を導出した．また，平面と球のモデルに対し
て，球上の接触点（接触点の一部）が単一の閉軌道を繰
り返すことにより，全接触点を目標点へ到達させる手法
を提案し，目標点に近づけるような閉軌道を決定するパ
ラメータが必ず存在することを示した．これらの手法の
の有効性を数値例にて確認した．
参考文献
[1] 吉川 恒夫, “器用なメカニカルハンド”, 日本ロボッ
ト学会誌, Vol.18, No.6, pp.763–766, 2000.
[2] 中村 仁彦, “非ホロノミックロボットシステム（第
1-5 回）”, 日本ロボット学会誌, Vol.11, Nos.4, 5, 6,
7, 1993, Vol.12, No.2, 1994.
[3] A.B.A.Cole, J.E.Hauser and S.S.Sastry, “Kinematics and Control of Multifingered Hands with
Rolling Contact,” IEEE Trans. Automat. Contr.,
Vol.34, No.4, pp.398–404, April 1989.
[4] Nilanjan Sarkar and Xiaoping Yun, “Dynamic
Control of 3–D Rolling Contacts in Two–Arm Manipulation,” IEEE Trans. Robot. Automat., Vol.13,
No.3, pp.364–376, June 1997.
[5] Richard M.Murray, Zexiang Li, S.Shankar Sastry,
A Mathmatical Introduction to ROBOTIC MANIPULATION, CRC Press, 1994.
[6] David J. Montana, “The Kinematics of Contact
and Grasp”, International Journal Conference on
Robotics Research, 7(3):17–32, 1988.
[7] A.Marigo, A.Bicchi, “Rolling Bodies with Regular Surface: Controllabillity Theory and Applications”, IEEE Trans. Automatic Control, Vol.45,
No.9, pp.1586–1599, September 2000.
[8] R.M.Murray and S.S.Sastry, “Grasping and manipulation using multifingered robot hands”, In
R.W.Brockett, editor, Robotics: Proceedings of
Symposia in Applied Mathematics, Vol.41, pp91–
128, American Mathmatical Society, 1990.
[9] Z.Li, J.Canny, “Motion of two rigid bodies with
rolling costraint”, IEEE Trans. Robotics and Automation, Vol.6, No.1, pp.62–72, February 1990.
[10] 前川 仁, 谷江和雄, 小森谷 清, “多指ハンド把握に
おける触覚フィードバックを用いた動的握力制御”,
計測自動制御学会論文集, Vol.32, No.11, pp.1526–
1534, 1996.
A
定理 1 の証明
∆η̃ = [ (Rϕ ∆α0o )T ∆ψ ]T を用いて (39) 式の両辺の
2 乗の差を計算すると
kη̃ 0 + ∆η̃k2 − kη̃ 0 k2
0 T
0
= [ 2(RT
ϕ αo ) + ∆αo ] ∆αo + (2ψ + ∆ψ)∆ψ
となる．このとき，Rϕ が回転行列であることから ϕ を
∆α0o
用いて RT
ϕ αo = −kαo k k∆α0 k とできるので，(39) 式は
o
∀
αo ,³ψ,
s.t.
∃
θ1 , θ2
kαo k
−2 k∆α0 k
o
´
+ 1 k∆α0o k2 + (2ψ + ∆ψ)∆ψ < 0
(41)
【The trajectry of the object】
p
0.1
p
oxd
0
1
2
0
1
0
0.5
1.5
2
1
0
0.5
1
u
f
u
0
0
0.5
1
1.5
v
f
v
2
h gτ
h
f 2d
1
0
2
0.1
gτd
1
-1
-1
0.5
2
t [s]
1.5
2
f 2d
2
1
1.5
t [s]
3
ozd
2
1.5
t [s]
[rad]
0
-1
1
2
1
h gf [a.u]
f 1
v
-1
-1.5
φoz
φ
2
0
-3
2
3
oyd
oy
0
-1
gfd
2.5
-0.5
-2
t[s]
φoz [rad]
1
[rad]
[rad]
0
φoy
φ
2
φ [rad]
φox [rad]
3
oxd
0
-1
t[s]
φox
φ
2
0.1
0.05
t[s]
3
h
gf
h
3
1d
0.05
h g τ [a.u]
2
3.5
vf
0.5
[rad]
1
1
uf
0
【The grasping force】
f 1
0.5
0
f 2
-0.1
v
1
v
-0.05
1d
1
0
-0.1
1.5
f 1
uf
2
0.15
-0.05
u
0.2
p oz [m]
p
ox
0
3
oz
p ozd
oyd
0.05
p oy [m]
[m]
0.05
oy
【The contact point of finger1,2】
p
0.25
uf
p
ox
p
0.1
-0.5
0
-0.05
-2
-2
-2
-2
-1
-3
-3
-3
-3
-1.5
0
1
2
0
1
t[s]
2
0
1
t[s]
0
2
0.5
1.5
2
0
0.5
1
1.5
-0.1
2
0
0.5
t [s]
1
1.5
2
t [s]
The controlled v by the linearization compensator.
Fig.4
【The motion of αo1 ,ψ1 】
0.05
1
t [s]
t[s]
【The motion of αo1 ,ψ1 on αo1 plane】
0.05
( u
o 1
,v
o 1
) plane
0.015
[m]
[m]
initial orientation ψ
0
1
initial point
0
u
vo
o 1
0
1
0.01
0.005
0
0.5
1
1.5
-0.05
2
0.5
1
1.5
2
t [s]
0
final orientation
ψ
vo
1
t [s]
final point
0
[m]
-0.05
3
1
f
-0.005
[rad]
2
-0.01
1
ψ
1
0
-0.015
-1
-2
-0.02
-0.005
-3
0
0.5
1
1.5
2
0
0.005
Fig.5
θ1 → 0 または θ2 → 0 ⇐⇒ ∆ψ → 0
θ1 → 0 または θ2 → 0 =⇒ ∆α0o → 0
(42)
(43)
の性質を持つことに注意する．(42) 式は (37) 式より明ら
かであり，(43) 式は (38) 式より θ1 = 0 のときは明らか，
また θ2 → 0 ときは，極限をロピタルの定理を用いて計算
すれば得られる．以降では，(i) kαo k 6= 0，ψ 6= 0，(ii)
kαo k 6= 0，ψ = 0，(iii) kαo k = 0，ψ 6= 0 の場合に分け
て考える．
【(i) kαo k 6= 0，ψ 6= 0．
】(42)，(43) 式より
−2
kαo k
+1<0
k∆α0o k
(44)
を満たす十分小さな θ1 ，θ2 が存在するので (41) 式が成
り立つ．
【(ii) kαo k 6= 0，ψ = 0．
】このとき (41) 式は
µ
1+
∆ψ 2
kαo k
−2
k∆α0o k2
k∆α0o k
0.015
1
0.02
0.025
0.03
[m]
The motion of αo1 ,ψ1 by the closed path of αf1 .
となる．ここで ∆α0o ，∆ψ は連続関数であり，
ψ∆ψ < 0, |2ψ| > |∆ψ|,
0.01
uo
t [s]
¶
k∆α0o k2 < 0
(45)
となる．(45) 式の括弧内において正の項と負の項の比
µ
¶,
µ
¶
∆ψ 2
∆ψ 2
kαo k
1
0
1+
2
=
k∆α
k
+
o
k∆α0o k2
k∆α0o k
2kαo k
k∆α0o k
は θ1 → 0 のとき極限 0 であることがロピタルの定理を
用いることで確認できるので，少なくとも θ1 を十分小さ
くとれば (45) 式が成り立つ．
【(iii) kαo k = 0，ψ 6= 0．
】このとき (41) 式は
µ
¶
k∆α0o k2
+ ∆ψ + 2ψ ∆ψ < 0
(46)
∆ψ
となる．(46) 式の括弧内の第 1 項，第 2 項は θ1 → 0 の
とき極限値 0 を取ることがロピタルの定理を用いて示せ
る．したがって θ1 を十分小さくとれば，
¯
¯
¯
¯ k∆α0o k2
¯
+ ∆ψ ¯¯
(47)
ψ∆ψ < 0, |2ψ| > ¯
∆ψ
とできるので，(46) 式が成立する．