衝突回避のための関節角速度計算法

逆運動学計算における目標タスクと衝突回避の統合は リンク間最短距離を用いたblending係数により行われる. これにより,衝突回避の必要のないときは目標タスクを厳密に満し 衝突回避の必要性があらわれたときに目標タスクを あきらめて衝突回避の行われる関節角速度計算を行うことが可能になる. 最終的な関節角速度の関係式はEquation  で得られる. 以下では$ca$の添字は衝突回避計算のための成分を表し, $task$の部分は衝突回避計算以外のタスク目標を表すことにする.

$$\mbox{\boldmath {$\dot{\theta}$}} = f(d) \mbox{\boldmath {$\dot{\theta}$}} _{ca} + \left(1-f(d)\right) \mbox{\boldmath {$\dot{\theta}$}} _{task}$$ (24)

blending係数$f(d)$は, リンク間距離$d$と閾値$d_a$・$d_b$の関数として計算される (Equation  ).

$$f(d) = \left\{ \begin{array}{l l} (d-d_a)/(d_b-d_a) & if d<d_a\\ 0 & otherwise \end{array}\right.$$ (25)

$d_a$は衝突回避計算を行い始める値 (yellow zone@xdefthefnmarkfootnotemark)であり, $d_b$は目標タスクを阻害しても衝突回避を行う閾値 (orange zone@xdefthefnmarkfootnotemark)である.

衝突計算をする2リンク間の最短距離・最近傍点が計算できた場合の 衝突を回避するための動作戦略は 2リンク間に作用する仮想的な反力ポテンシャルから導出される.

2リンク間の最近傍点同士をつなぐベクトル $p$を用いた 2リンク間反力から導出される速度計算を Equation   に記す.

$$\mbox{\boldmath {$\delta x$}} = \left\{ \begin{array}{l l} 0 & if \left\vert\mbox{\boldmath... ...}\right\vert-1)\mbox{\boldmath {$p$}} & else \end{array}\right.$$ (26)

これを用いた関節角速度計算はEquation   となる.

$$\dot{\mbox{\boldmath {$\theta$}}}_{ca} = \mbox{\boldmath {$J$}}_{... ..._{task}) \mbox{\boldmath {$J$}}_{ca}^{T} k_{null} \mbox{\boldmath {$\delta x$}}$$ (27)

$k_{joint}$・$k_{null}$はそれぞれ反力ポテンシャルを 目標タスクのNullSpaceに分配するかそうでないかを制御する係数である.