4.2. End effector 制御

4.2.1. 接触の運動学モデル

物体座標系 \(C-\xi\eta\zeta\) ,空間座標系 \(O-xyz\) を設定する.空間座標系で表した点 \(C\) の位置ベクトルを \(\mathbf{x}_o\) ,指 \(i\) の接触点 \(P_i\) の位置ベクトルを \(\mathbf{x}_i\) とした場合,

\[\dot{\mathbf{x}_i} = \dot{\mathbf{x}}_o + \mathbf{\omega}\times(\mathbf{x}_i-\mathbf{x}_0)\]

となる,ここで, \(\mathbf{\omega}\) は物体の角速度ベクトルである.指が物体と干渉しないためには,

\[\mathbf{n}_i\cdot\dot{\mathbf{x}}_i \leq 0\]

でなくれはならない.ここで, \(\mathbf{n}_i\) は点 \(P_i\) における物体表面の外向き法線ベクトルである.この関係より,

\[\mathbf{n}_i^\top\cdot\dot{\mathbf{x}}_o + \mathbf{n}_i^\top\cdot(\mathbf{\omega}\times(\mathbf{x}_i-\mathbf{x}_o)) \leq 0\]

となり,スカラー三重積の公式を用いて,

\[\begin{split}\begin{bmatrix} \mathbf{n}_i\\ (\mathbf{x}_i-\mathbf{x}_o)\times\mathbf{n}_i \end{bmatrix}^\top \begin{bmatrix} \dot{\mathbf{x}}_o \\ \mathbf{\omega} \end{bmatrix} \leq 0\end{split}\]

を得る.ここで,wrench および twist

\[\begin{split}\mathbf{w}_i \equiv \begin{bmatrix} \mathbf{n}_i\\ (\mathbf{x}_i-\mathbf{x}_o)\times\mathbf{n}_i \end{bmatrix} ,\quad \dot{\mathbf{x}} \equiv \begin{bmatrix} \dot{\mathbf{x}}_o \\ \mathbf{\omega} \end{bmatrix}\end{split}\]

を導入し,

\[\mathbf{w}_i^\top\dot{\mathbf{x}} \leq 0\]

とも表される.これは,物体の運動に対する制約である.物体に \(n\) 本の指が接触しているとき,物体の速度と角速度は連立一次方程式

\[\forall i\in\{1,2,\ldots,n\}, \mathbf{w}_i^\top\dot{\mathbf{x}} \leq 0\]

を満たす必要がある.よって,物体がとることが可能な速度と角速度の集合は,

\[V = \{\dot{\mathbf{x}}\in\mathbb{R}^6 \mid \forall i\in\{1,2,\ldots,n\}, \mathbf{w}_i^\top\dot{\mathbf{x}} \leq 0\}\]

である.この集合 \(V\) を許容速度集合と呼ぶ.face形式で,

\[V = \text{face} \{\mathbf{w}_1, \mathbf{w}_2, \ldots, \mathbf{w}_n\}\]

と書くこともある.

4.2.2. 接触の静力学モデル

\(i\) が物体と点接触しているとき,垂直抗力は,

\[\mathbf{f}_i = R_i(-\mathbf{n}_i)\]

である.ここで, \(R_i\geq 0\) は垂直抗力の大きさ, \(n_i\) は法線ベクトルである.摩擦がないと仮定すると,

\[\begin{split}\begin{bmatrix} \mathbf{f}_i\\ (\mathbf{x}_i-\mathbf{x}_o)\times\mathbf{f}_i \end{bmatrix} =-R_i\mathbf{w}_i\end{split}\]

を得る.外力と外モーメントをまとめて \(\mathbf{p}\) とすると,力とモーメントの釣り合いより,

\[\mathbf{p}+\sum_{i=1}^n(-R_i\mathbf{w}_i)=\mathbf{0}\]

となる.釣り合いが保たれる外力と外モーメントの集合は,

\[P=\left\{ \sum_{i=1}^n R_i\mathbf{w}_i\in\mathbb{R}^6 \mid \forall i\in\{1,2,\ldots,n\}, R_i \geq 0 \right\} .\]

この集合は \(P\) を許容力集合と呼ぶ.span形式で,

\[P=\text{span}\{\mathbf{w}_1,\mathbf{w}_2,\cdots,\mathbf{w}_n\}\]

と書くこともある.摩擦がある場合は,摩擦係数が一定であるとし,合力の集合は摩擦円錐で与えられる.指 \(i\) に対する摩擦円錐を \(m\) 個の稜線をもつ凸多面推で近似すると,稜線に沿うベクトル \(\mathbf{d}_{i,j}\) を用いて,

\[\mathbf{f}_i = \sum_{j=1}^m R_{i,j}(-\mathbf{d}_{i,j}),\quad R_{i,j}\geq 0\]

となる.ここで,

\[\begin{split}\mathbf{w}_{i,j} \equiv \begin{bmatrix} \mathbf{d}_{i,j}\\ (\mathbf{x}_i-\mathbf{o})\times \mathbf{d}_{i,j} \end{bmatrix}\end{split}\]

とし,許容力集合

\[ \begin{align}\begin{aligned}P = \text{span} \{ \mathbf{w}_{1,1},\mathbf{w}_{1,2},\cdots,\mathbf{w}_{1,m},\\\cdots,\mathbf{w}_{n,1},\mathbf{w}_{n,2},\cdots,\mathbf{w}_{n,m} \}\end{aligned}\end{align} \]

を得る.face形式とspan形式は互い双対であり,凸多面錐 \(C\) の双対凸多面体錐を \(C^*\) で表す.摩擦がない場合は特に,

\[P=V^*, \quad V=P^*\]

が成り立つ.

4.2.3. form closure, force closure

物体がどのような運動も実現できない状態であるとき,そのような把持をform closureといい,許容速度集合が,

\[V=\{\mathbf{0}\}\]

が成り立つときである.一方,物体にどのような外力や外モーメントを与えても,力とモーメントの釣り合いが成り立つとき,そのような把持をforce closureといい,許容力集合が,

\[P=\mathbb{R}^6\]

が成り立つときである.