３．１１．２１PR運動の別解法　　　　　　　 　　　　　　　　　　　　　　　　2023-1-17

PRManip.h PRMotion1( ), PRSphMotion24( ), PRCylMotion24( );

・円筒指がワークと接触してPR運動する解法を考える．

・「詳説ロボットの運動学 9.4.1 指の腹による物体操りの接触運動特性」の(a) PR接触運動条件は

　ワークと指野原の接触点Cにおける速度と回転速度をvwc[ ], vfc[ ], ωwc[ ], ωfc[ ]とすると

vfc[ ] = vwc[ ], (ωfc[ ], nc[ ]) = (ωwc[ ], nc[ ])

であり，この4条件を満足する指の4関節の変位速度ν[ ]が確定する．

　微小時間dt後の関節角は

dq[ ] = ν[ ]*dt, q2[ ] = q1[ ] + dq[ ]

この関節角から指のポーズが決まり，新しい接触点が決まる．

・この計算はC点の移動がdt時間の直線運動であり，dtを十分に小さい値にしなければ誤差が生じ，長い時間でその誤差が累積して正しい運動と異なってしまう．（非ホロノーム系の特徴）

　指とワークのPR接触運動では上記dt後に接触がわずかに離れる．

第2PR運動

・PR運動を以下のように変形する．dt後の状態

(1) 指とワークは接触する．or 指面とワーク面の最短距離D = 0

(2) 指とワークの転がり量は等しい．Sf = Sw

(3) dt後の転がり軌跡sf[ ], sw[ ]の接触平面内の方向ベクトルは等しい．

(4) 指とワークの接触点における回転速度の法線方向成分は等しい. ωfcn = ωwcn

これらを数式で表現すると以下のようになる．

・指の初期関節角q1[ ], 接触指（第4指）の関節位置J41[ ], 接触点位置rfc1[ ], 接触点までのリンク長Lf1, 指円筒回りの偏角φf1

rfc1[ ] = J41[ ] + Lf1*efz1[ ] + af*(exf1[ ]*cos(φf1) + eyf1[ ]*sin(φf1) )

・ワークの初期位置rw1[ ], ワークの接触点位置rwc1[ ]

球面の場合：接触点のparameter αw1, βw1

rwc1[ ] = rw1[ ] + aw*(cos(αw1)*sin((βw1) + sin(αw1)*sin(βw1) + cos(βw1) )

円筒面の場合：接触点のparameter Lf1, φf1

rwc1[ ] = rw1[ ] + Lw1*ezw1[ ] + aw*(exw1[ ]*cos(φw1) + eyw1[ ]*sin(φw1) )

・ワークの代表点の速度vw[ ], 代表点回りの回転速度ωw[ ]

・dt後のワークの位置rw2[ ], 接触点の移動先rwc3[ ]

rw2[ ] = rw1[ ] * vw[ ]*dt

rwc3[ ] = rw2[ ] + ωw[ ]×(rwc1[ ] – rw1[ ])

・dt後の指の関節変位の増分dq[ ], 関節角q2[ ], 折衝\句点の移動先rfc3[ ]

　q2[ ]を与えると指の諸元が決まる．

q2[ ] = q1[ ] + dq[ ]

rfc3[ ] = J42[ ] + Lf1*ezf2[ ] * af*(exf2[ ]*cos(φf1) + eyf2[ ]*sin(φf1))

・dt後の指-ワーク間の最近点Cf2, Cw2位置rfc2[ ], rwc2[ ], そのときの指parameter Lf2, φf2

rfc2[ ] = J42[ ] + Lf2*ezf2[ ] + af*(exf2[ ]*cos(φf2) + eyf2[ ]*sin(φf2))

球面の場合のparameter αw2, βw2

rwc2[ ] = rw2[ ] + aw*(cos(αw2)*sin((βw2) + sin(αw2)*sin(βw2) + cos(βw2) )

円筒面の場合のparameter Lw2, φw2

rwc2[ ] = rw2[ ] + Lw2*ezw2[ ] + aw*(exw2[ ]*cos(φw2) + eyw2[ ]*sin(φw2) )

・第2PR運動の誤差は上記の諸元から求められる．

(1) 指-ワーク間の最短距離D

D = abs(rfc2[ ] – rwc2[ ])

(2) 指・ワークの転がり量の差 dS

sf[ ] = rfc2[ ] – rfc3[ ], Sf = abs(sf[ ])

sw[ ] = rwc2[ ] – rwc3[ ] Sw = abs(sw[ ])

dS = Sf – Sw

(3) 指・ワークの接平面内の転がりベクトルesf[ ], ews[ ]のなす角dθ

接平面内のベクトルexs[ ], eys[ ]：exs[ ] = ezf2[ ], eys[ ] = exf2[ ]とする．

efs[x] = (sf[ ], exs[ ]), sfs[y] = (sf[ ], eys[ ]), θf = atan2(efs[y], efs[x])

ews[x] = (sw[ ], exs[ ]), sws[y] = (sw[ ], eys[ ]), θw = atan2(ews[y], ews[x])

dθ = θf – θw

(4) 接触点C2における指とワークの回転速度の法線方向nc2[ ]成分ωfn, ωwn

関節速度 ν[ ] = dq[ ] / dt，関節軸ベクトル AX[ ]

ωf[ ] = AX[1]*ν[1] + AX[2]*ν[2] + AX[3]*ν[3] + AX[4]*ν[4]

ωfn = (ωf[ ], nc2[ ])

AXN[ ] = (AX[ ],nc2[ ])と置くと

ωfn = AXN[1]*ν[1] + AXN[2]*ν[2] + AXN[3]*ν[3] + AXN[4]*ν[4]

ωwn = (ωw[ ], nc2[ ])

dωn = ωfn – ωwn

・誤差 ERR[ ] = (D, dS, dθ, dωn)を0とするdq[ ]をNewton法によって求める．

　　 ERR[ ] = JB4[ ][ ] * dq[ ]

dq[ ] = IJB4[ ][ ]*ERR[ ]

・PR運動で速度条件を満足し（第1PR運動），ワークが静止し指だけがワーク上を転がる第2のPR運動を付け加える．

・第2PR運動の条件：

　第1PR運動によるdt後のC点の移動先を指・ワーク上でCf3, Cw3とする．

　dt後に静止ワーク上を指が転がり接触する点をC2とすると転がり条件は

　 転がり線 sf = C2 – Cf3, sw = C2 – Cw3

　　C2点の接平面の座標軸 (exc, eyc)

(1) 指・ワークの間隔 D = 0

dq[ ] = ν[ ]*dt, q2[ ] = q1[ ] + dq[ ]

D = |rfc2[ ] – rwc2[ ]|

rfc2[ ], rwc2[ ] : dt後の指上の最近点

(2) 新しい接触点C2に到る転がり量は指上とワーク上で等しい．

sf[ ] = rfc2[ ]- rfc3[ ], sw[ ] = rwc3[ ] – rwc[ ]

Sf = |sf[ ]|, Sw = |sw[ ]|, Sf = Sw

rfc3[ ] : 転がり後の指上C点の移動先C3

rfc2[ ] : 転がり後の指上の新しい接点C2

(3) 転がり線のベクトルのなす角 　dv=inpro(sf, exc) – inpro(sw, exc), dth = asin(dv) = 0

(4) C2点の法線方向の成分は(ωf2, nc[ ]) = 0

この4条件を満足する関節速度ν2[ ], またはdq2[ ] = ν2[ ]*dtを求めることができる．

・PR運動を定式化すると以下のようになる．

・第1PR運動

ワークのC点の速度：

vwc[ ] = vw[ ] + ωw[ ]×(aw*nc[ ])

nc[ ] : C点の法線ベクトル

　指上C点の速度：

vfc1[ ] = ΣLC[k][ ]*ν1[k] = vwc[ ] (1-1)

指の回転速度：

　 ωf1[ ] = ΣAX[k][ ]*ν1[k] (1-2)

(ωf1[ ], nc[ ]) = ΣAXN[k]*ν1[k] = (ωw[ ], nc[ ]) (1-3)

　 AX[k]：k関節軸の方向ベクトル

　　　　LC[k] = AX[k][ ]×LCJ[k][ ]：k関節から見たC点の位置ベクトル

この4条件からν1[1,2,3,4]が得られる．

dt後の指上Cf1点位置：rfc11[ ] = rfc1[ ] + vfc[ ]*dt

dt後のワーク上Cw1点位置：rwc11[ ] = rwc[ ] + vwc[ ]*dt

dt後の関節角：dq1[ ] = ν1[ ]*dt, q11[ ] = q1[ ] + dq1[ ] (1-4)

条件(1-1), (1-3)を満たすν1[ ], dq1[ ]は線形演算で求められる．

・第2PR運動

指の相対回転速度:：ωf2[ ]

ωf2[ ] = ΣAX[k][ ]*ν2[k]

(ωf2[ ], nc[ ]) = 0 → ΣAXN[k]*ν2[ ]=0 → ΣAXN[k]*dq2[ ] = 0 (2-1)

dt後の指-ワーク最近点, ,間隔：

dq2[ ] = ν2[ ]*dt, q2[ ] = q11[ ] + dq2[ ] (2-2)

q2[ ]のときの指-ワーク間の最近点と間隔

rfc2[ ] = Ffr(q2[ ]), rwc2[ ] = Fwr(q2[ ])

(1) D = abs(rfc2[ ] – rwc2[ ]) → 0 (2-3)

指上とワーク上の転がり量

q2[ ]のときの指上, ワーク上C点の移動先C3:

rfc3[ ] = Ffc(q2[ ]), rwc3[ ] = rwc11[ ]

指上，ワーク上の転がり

sf[ ] = rfc2[ ] – rfc3[ ], Sf = abs(sf[ ])

sw[ ] = rwc2[ ] – rwc3[ ], Sw = abs(sw[ ])

移動量の差

(2) dS = Sf – Sw → 0 (2-4)

追加PR運動の3条件(2-1), (2-3), (2-4)を満たすdq2[1,2,3,4]は１つの任意性がある．