おべんきょうwiki

TSR

最終更新：2014年08月06日 20:13

yahirohumpty

- view

管理者のみ編集可

TSR: Task Space Region

エンドエフェクタの拘束条件を記述する．

TSR

3つのパートからなる．

$^o T _w$ : 世界座標系oからTSR座標系wへの座標変換
$^w T _e$ : TSR座標系wからエンドエフェクタeへの座標変換
$^w B$ : TSR座標系における拘束条件

$^w B = \left[ \begin{array}{cc} x_{min} & x_{max} \\ y_{min} & y_{max} \\ z_{min} & z_{max} \\ \psi_{min} & \psi_{max} \\ \theta_{min} & \theta_{max} \\ \phi_{min} & \phi_{max} \\ \end{array} \right]$

回転はRPYで表される．
(この後の検算の結果，おそらく元論文ではRPY = $RotZ(\phi) RotY(\theta) RotX(\psi)$ と思われる．)

TSRに対する距離の定義

コンフィグレーション空間 $q_s$ に対応するエンドエフェクタの座標を $^o T _s$ とする．
このときのTSR座標系を $^o T _{s'}$ とすると，次のように逆算できる．

${^o T _{s'}} = {^o T _s} ( {^w T _e} ) ^ {-1}$

初期のTSR座標系との差分は以下のように計算される．

${^w T _{s'}} = ( { ^o T _w } )^{-1} { ^o T _{s'} }$

これを拘束 $^w B$ に合わせた形になおすと，

$^w d = \left[ \begin{array}{c} ^w t _{s'} \\ \arctan ( {^w R _{s'}}(3, 2) / {^w R _{s'}}(3, 3) ) \\ -\arcsin {^w R _{s'}}(3, 1) \\ \arctan ( {^w R _{s'}}(2, 1) / {^w R _{s'}}(1, 1) ) \\ \end{array} \right]$

拘束に対する距離に直すと，各要素は以下のようになる．

$\Delta x _i = \left\{ \begin{array}{cc} {^w d _i} - {^w B}(i, 1) & \mbox{if}({^w d _i} < {^w B}(i, 1)) \\ {^w d _i} - {^w B}(i, 2) & \mbox{if}({^w d _i} > {^w B}(i, 2)) \\ 0 & \mbox{otherwise} \end{array} \right.$

すなわちTSRの距離は $|| \Delta x ||$ となる．

回転角度の検算

$RotZ(\phi) RotY(\theta) RotX(\psi) = \left[ \begin{array}{ccc} C_{\phi} C_{\theta} & -S_{\phi} C_{\psi} + C_{\phi} S_{\theta} S_{\psi} & S_{\phi} S_{\psi} + C_{\phi} S_{\theta} C_{\psi} \\ S_{\phi} C_{\theta} & C_{\phi} C_{\psi} + S_{\phi} S_{\theta} S_{\psi} & -C_{\phi} S_{\psi} + S_{\phi} S_{\theta} C_{\psi} \\ -S_{\theta} & C_{\theta} S_{\psi} & C_{\theta} C_{\psi}\end{array} \right]$

ここから，

$\psi = \arctan (C_{\theta} S_{\psi}) / (C_{\theta} C_{\psi}) = \arctan ( {^w R _{s'}}(3, 2) / {^w R _{s'}}(3, 3) )$
$\theta = -\arcsin (-S_{\theta}) = -\arcsin {^w R _{s'}}(3, 1)$
$\phi = \arctan (S_{\phi} C_{\theta}) / (C_{\phi} C_{\theta}) = \arctan ( {^w R _{s'}}(2, 1) / {^w R _{s'}}(1, 1) )$

拘束多様体への投影

距離を定義できたので，距離を零とするような $q$ の修正を考える．
ヤコビアンの擬似逆行列を用いて反復的に求めることができる．

$\Delta q _{error} = J^T (J J^t) ^{-1} \Delta x$
$q \leftarrow (q - \Delta q_{error})$

TSR Chain

その名の通りTSRを連結したもの．
TSRにおけるエンドエフェクタ座標が次のTSR座標に，
二番目以降のTSR座標がそれ以前のTSRを連結した結果になる．

ここで，仮想関節 $^w T _{sample}$ をTSRに付加することを考える．
$C_i = [{^o T _w}, {^w T _e}, {^w B}, {^w T _{sample}}]$
という4つの構成要素から成り立つものをi番目のTSRとすると，
${^o T _w}$ はそれ以前のすべてのTSRの $^w T _{sample}$ の影響を受ける．

つまり，
$C_i . {^o T _w} = (C_{i-1} . {^o T _w}) (C_{i-1} . {^w T _{sample}}) (C_{i-1} . {^w T _e})$

各TSRの仮想関節 $^w T _{sample}$ を決定するような仮想関節角度列cをTSR Chainに持たせる．

TSR Chain に対する距離の定義

コンフィグレーション空間 $q_s$ に対応するエンドエフェクタの座標を $^o T _s$ とする．
TSR Chain自体をひとつの仮想マニピュレータとして捉え，終端の座標を $^o T _s$ を目標としてIKを使って解く．
その時の両座標の差分が距離として定義される．

CBiRRT2: Constrained Bidirectional RRT

TSRによる拘束条件を用いて計算可能な拡張型RRT．

 CBiRRT2(q_start, q_goal) {
   tree_a.Init(q_init);  // 初期値q_initをツリーのルートに登録する
   tree_b.Init(q_goal);  // 目標値q_goalを別のツリーのルートに登録する
   while (TimeRemaining()) {  // 規定の時間内に計算が終わらなければ失敗
     tree_goal = GetBackwardTree(tree_a, tree_b);  // ゴールから伸びている方の木を選ぶ
     if (tree_goal.empty() ||  // ゴールから伸びている方の木が空か，
         random(0, 1) < P_SAMPLE) {  // 確率P_SAMPLEでTSRから直接サンプリング
       AddRoot(tree_goal);
     } else {  // 通常の探索を行う
       q_rand = RandomConfig();  // ランダムに目標値を入れる
       // ツリーAの探索
       q_a_near = NearestNeighbor(tree_a, q_rand);
       q_a_rearch = ConstrainedExtend(tree_a, q_a_near, q_rand);
       // ツリーBの探索
       q_b_near = NearestNeighbor(tree_b, q_rand);
       q_b_rearch = ConstrainedExtend(tree_b, q_b_near, q_rand);
       if (q_a_reach == q_b_reach) {  // 十分近いところに到達したならば
         path = ExtractPath(tree_a, q_a_reach, tree_b, q_b_reach);  // 2つの木を接続
         return ShortenPath(path);  // パスを最適化したものが解
       } else {
         Swap(tree_a, tree_b);  // 2つのツリーを入れ替える
       }
     }
   }
   return NULL;
 }
 
 ConstrainedExtend(tree, q_near, q_target) {
   q_s = q_near;
   q_s_old = q_near;
   while (true) {
     if (q_target == q_s) {  // 目標に到達している場合
       return q_s;
     } else if (norm(q_target - q_s) > norm(q_s_old - q_target)) {  // 発散する方向に動いている場合
       return q_s_old;
     }
     q_s_old = q_s;
     q_s = q_s +
       min(DELTA_Q, norm(q_target - q_s) * (q_target - q_s) / norm(q_target - q_s)  // ターゲット方向に動く
     c = GetConstraintValues(tree, q_s_old);  // q_s_oldのときの仮想関節角度列c．同時に格納されているので取り出すのみで良い．
     (q_s, c) = ConstrainConfig(q_s_old, q_s, c, NULL);  // コンフィグ空間から拘束多様体への投影を行う
     if (!q_s.empty()) {
       tree.AddVertex(q_s, c);
       tree.AddEdge(q_s_old, q_s);
     } else {
       return q_s_old;
     }
   }
 }

ここまでは細かい差異はあるが基本的にBiRRTと同じ．

直接サンプリングを行う場合

 AddRoot(tree) {
   for (i = 1 to m) {  // mはTSR Chainの数
     cc = GetTSRChainsForManipulator(i);  // ccはTSR Chain
     (t_o_to_targ, c) = SampleFromTSRChains(cc);  // 直接サンプリング
     targets.AddTarget(t_o_to_targ, i);
   }
   (q_s, c) = GetInitialGuess();  // 初期値，元論文ではロボットの初期姿勢と零ベクトルと記述されている
   (q_s, c) = ConstrainConfig(NULL, q_s, c, targets);  // コンフィグ空間から拘束多様体への投影を行う
   if (!q_s.empty()) {
     tree.AddVertex(q_s, c);
   }
 }

コンフィグレーションが拘束条件を満たすように毎度修正を行う．

 // @brief コンフィグレーションが拘束条件を満たすように，物理拘束を考慮し拘束多様体へ投影する
 // @param q_s_old 一手前の関節角度列
 // @param q_s 関節角度列
 // @param c TSR Chainの仮想関節角度列
 // @param targets 目標座標のリスト
 ConstrainConfig(q_s_old, q_s, c, targets) {
   check_dist = false;
   if (targets.empty()) {  // ConstrainedExtendから呼ばれた場合
     check_dist = true;
     for (i = 1 to m) {  // mはTSR Chainの数
       cc = GetTSRChainsForManipulator(i);  // ccはTSR Chain
       t_o_to_s = GetEndEffectorTransform(q_s, i);  // q_sのときのエンドエフェクタの姿勢
       (t_o_to_targ, c) = GetClosestTransform(cc, t_o_to_s, c);  // 目標座標をTSR Chainから修正する
       targets.AddTarget(t_o_to_targ, i);
     }
   }
   q_s = UpdatePhysicalConstraintDOF(q_s, c);  // 物理拘束によりq_sを修正する
   q_s = ProjectConfig(q_s, targets);  // 多様体への投影を行う
   if (q_s.empty() ||  // 拘束多様体への投影が出来なかった場合か，
       (check_dist &&  // ConstrainedExtendから呼ばれ，
        norm(q_s - q_s_old) > 2 * DELTA_Q) {  // かつ一手前の値から2*DELTA_Q以上離れた時
     return NULL;  // 失敗とする．
   }
   return (q_s, c);
 }

うまくつながったら最後はパスを最適化して返す．

 ShortenPath(path) {
   while (TimeRemaining()) {  // 規定の時間内粘る
     tree_shortcut = {};
     i = (int)(random(1, path.size() - 1));
     j = (int)(random(i, path.size()));
     q_reach = ConstrainedExtend(tree_shortcut, path[i], path[j]);  // iからjまで直通可能か調べる
     if (q_reach == path[j] &&  // jまで到達できて
         tree_shortcut.size() < (j - i)) {  // かつ元々のパスよりも短ければ
       path = {path[0 ... i], tree_shortcut, path[j + 1 ...]};  // i-j間を直通したものに置き換える
     }
   }
   return path;
 }

参考文献

Dmitry Berenson, Siddhartha Srinivasa, James Kuffner.
"Task Space Regions: A Framework for Pose-Constrained Manipulation Planning",
International Journal of Robotics Research (IJRR), Vol. 30, No. 12, October, 2011, pp. 1435 - 1460.
http://www.ri.cmu.edu/pub_files/2011/10/dmitry_ijrr10-1.pdf

タグ：