関節角度限界回避を含む逆運動学

ロボットマニピュレータの軌道生成において, 関節角度限界を考慮することはロボットによる実機実験の際に重要となる. 本節では,文献⁸⁹の式および文章を引用しつつ, 関節角度限界の回避を 含む逆運動学について説明する.

重み付きノルムを以下のように定義する.

$$\left\vert\mbox{\boldmath {$\dot{\theta}$}}\right\vert _{\mbox{\b... ...ath {$\dot{\theta}$}}^T\mbox{\boldmath {$W$}}\mbox{\boldmath {$\dot{\theta}$}}}$$ (14)

ここで, $W$は $W$$\in$   $R$$^{n \times n}$であり, 対象で全ての要 素が正である重み係数行列である. この $W$を用いて, $J$$_{\mbox{\boldmath {$W$}}}, \mbox{\boldmath {$\dot{\theta}$}}_{\mbox{\boldmath {$W$}}}$を以下のよう に定義する.

$$\mbox{\boldmath {$J$}} _{\mbox{\boldmath {$W$}}} = \mbox{\boldmath {$J$}}\mbox{\boldmath... ...{$W$}}} = \mbox{\boldmath {$W$}}^{\frac{1}{2}}\mbox{\boldmath {$\dot{\theta}$}}$$ (15)

この $J$$_{\mbox{\boldmath {$W$}}}, \mbox{\boldmath {$\dot{\theta}$}}_{\mbox{\boldmath {$W$}}}$を用いて, 以下の式を得 る.

$$\dot{\mbox{\boldmath {$r$}}} = \mbox{\boldmath {$J$}} _{\mbox{\boldmath {$W$}}}\mbox{\boldmath {$\dot{\theta}$}}_{\mbox{\boldmath {$W$}}}$$ (16)

$$\left\vert\dot{\mbox{\boldmath {$\theta$}}}\right\vert _{\mbox{\boldmath {$W$}}} = \sqrt{\mbox{\boldmath {$\dot{\theta}$}}_{\mbox{\boldmath {$W$}}}^T\mbox{\boldmath {$\dot{\theta}$}}_{\mbox{\boldmath {$W$}}}}$$ (17)

これによって線型方程式の解は@xdefthefnmarkfootnotemarkから 以下のように記述できる.

$$\mbox{\boldmath {$\dot{\theta}$}} _{\mbox{\boldmath {$W$}}} = \mbox{\boldmath {$W$}}^{-1}\mbox{\bol... ...ath {$W$}}^{-1}\mbox{\boldmath {$J$}}^T\right)^{-1}\dot{\mbox{\boldmath {$r$}}}$$ (18)

また、現在の関節角度$\theta$が関節角度限界 $\theta_{i,\max}, \theta_{i, \min}$に対してどの程度余裕があるかを評価する ための関数 $H($$\theta$$)$は以下のようになる¹⁰).

$$H( \mbox{\boldmath {$\theta$}} ) = \sum_{i = 1}^n\frac{1}{4} \frac{(\theta_{i,\max} - \theta_{i,\min})^2} {(\theta_{i,\max} - \theta_i)(\theta_i - \theta_{i,\min})}$$ (19)

次にEquation  に示すような $n \times n$の重み係数行列 $W$を考える.

$$\mbox{\boldmath {$W$}} = \left[ \begin{array}{ccccc} w_1 & 0 & 0 & \cdots & 0 \\ 0 ... ...ts & \cdots \\ 0 & 0 & 0 & \cdots & w_n \\ \end{array}\right]$$ (20)

ここで$w_i$は

$$w_i = 1 + \left\vert\frac{\partial \mbox{\boldmath {$H$}}(\mbox{\boldmath {$\theta$}})}{\partial \theta_i}\right\vert$$ (21)

である.

さらにEquation  から次の式を得る.

$$\frac{\partial H(\mbox{\boldmath {$\theta$}})}{\partial \theta_i}... ...heta_{i,\min})} {4(\theta_{i,\max} - \theta_i)^2(\theta_i - \theta_{i,\min})^2}$$ (22)

関節角度限界から遠ざかる向きに関節角度が動いている場合には重み係数行列を 変化させる必要はないので,$w_i$を以下のように定義しなおす.

$$w_i = \left\{ \begin{array}{l l} 1 + \left\vert\frac{\partial... ...theta$}})}{\partial \theta_i}\right\vert < 0 \end{array}\right.$$ (23)

この$w_i$および $W$を用いることで関節角度限界回避を含む逆運動学を解くこ とができる.