3.10.34 ワーク(RP関節の先の関節)の目標方向を干渉しない方向に修正 2012-5-25
・経路の逆生成において,ワークの目標方向を決めリストのRP関節によってその方向へ一気に運動するということを行う.
・この目標方向は,壁の外側では壁の外向きの法線方向,内側では垂直方向であるが,その目標方向ではワークが壁とぶつかったり,リストを曲げ過ぎて自身のアームとぶつかることがある.
・それらの干渉があれば干渉しない方向に目標方向を修正する計算を述べる.
・RPRリスト(利用する関節は第1,2関節RP)はどの方向へ持っていくこともできる.
・以下の計算はワーク及びRP関節を持つアームの干渉回避運動にも適用できる.
・説明はワークの干渉回避運動を代表して行う.RP関節の場合はそのアームの先の関節をワークに置き換えて考えればよい.
1.ワークの目標方向とその修正
・ロボットの最終ポーズは,ワークが床上に位置決めされた状態にある.
・経路逆生成の始めは,リストによりワークをリストより壁外向きの法線方向を目標として一気に持ち上げる.自己干渉する可能性がある場合はリストアーム(リストを先端とするアーム)から遠い方向へと目標を修正する.
・ワークの現在の向きから目標向きに持っていくとき,リストリンクとぶつからないよう旋回方向を決める.リスト関節はワークをどの向きにもできるので,ぶつからない経路を生成することができる.
・ワークを持ち上げて穴通り抜け運動中はワークを壁面法線方向を保つ.床干渉や自己干渉があれば同様に方向を修正する.
・穴通り抜け後はワークの目標方向を垂直とする,壁干渉や自己干渉があれば修正する.
2.壁外側におけるワークの目標方向とその床干渉回避のための方向修正
・リストによりワークを持ち上げて壁面法線ehy方向へ持っていく.すなわちワークの目標方向は
これにより後の穴通り抜けが容易になる.
・これが基本であるが,リストが低いとこの目標方向ではワークが床とぶつかる(床干渉).
・そのときはワークを床干渉しない高さHGまで持ち上げる.
・リストからの高さをとし,リスト−ワーク間(ワークリンク)のリンク長を
とする.
・また壁法線ベクトルを,リンクの修正した目標方向ベクトルを
とする.
・ワークはリストを中心とする半径の球面上を動けるから高さHとなりさえすれば水平面のどの方向へ動かしてもよいが,ここでは壁法線方向からそのまま上へ動かすとする.
・目標方向ベクトルは図3.4のように
面内で高さHだけ床垂直方向へ変えたものである.すなわち
・図のように角度を表すと,目標方向は以下のようになる.
(2-7)
・右辺は1以下でなければならない.Hが大き過ぎるとその高さまでワークを持ち上げることができない.
・目標方向は
(2-8)
・これは絶対座標表示であるから,この方向へ向けるリスト角の計算にはこのをリスト座標系に変換しなければならない.リスト座標系の(X, Y, Z)軸は
であるから
のリスト座標系各成分は
3.壁内側(自由空間)におけるワークの目標方向とその壁干渉回避のための方向修正
・リストによりワークを持ち上げて上方方向へ持っていく.すなわちワークの目標方向は
これによりワークが通り抜けた後の自由空間におけるロボットの初期ポーズまでの経路逆生成が容易になる.
・これが基本であるが,リストが壁に近いと,目標方向ではワークが壁とぶつかる(壁干渉).
・この干渉を避けるためにワークを壁から干渉しない距離YGまで引き離す方向へ目標方向を修正する..
・以下の説明で,壁法線方向を「横」と表現する.
・ワークのリストからの横距離を(>0)とし,リスト−ワーク間(ワークリンク)のリンク長を
とする.
・また壁法線ベクトルを,リンクの修正した目標方向ベクトルを
とする.
・ワークはリストを中心とする半径の球面上を動けるから横距離Yとなりさえすれば水平面のどの方向へ動かしてもよいが,ここでは壁法線方向からそのまま横へ動かすとする.
・修正目標方向ベクトルは図4のようにRWZ面内で距離Yだけ横方向へ変えたものである.すなわち
ただし,床面法線方向は(0, 0, 1)であるからαhは決まらない.このときはリストを含む壁面法線を含む面を考える.
・図のように角度を表すと,修正目標方向は以下のようになる.
(2-7)
・右辺は1以下でなければならない.Yが大き過ぎるとその距離まででワークを横に持っていくことができない.
・目標方向は
(2-8)
・これは絶対座標表示であるから,この方向へ向けるリスト角の計算にはこのをリスト座標系に変換しなければならない.その計算は2.壁外でのリスト角計算と同じである.
・リスト座標系の(X,
Y, Z)軸はであるから
のリスト座標系各成分は
4.自己干渉(ワークとリストリンクとの衝突)を避けるワーク目標方向
・リスト前のリンクが目標方向
に近いとき(リストリンクが水平に近いとき),ワークがリンク
とぶつかる(自己干渉)可能性がある.
・この場合はから自己干渉しない限界角度
以上離れた方向へ旧
を新しい目標方向
へ向ける必要がある.
・自己干渉しない方向は図5のを軸とする円錐角
の限界円錐面内にあればどこでもよいが,ここではリストから旧目標
と同じ高さとなるか,またはその高さではワークと床が干渉する場合には干渉しないだけの高さとなる方向を円錐面上の新しい目標方向
とする.
・すなわち
旧目標方向:
新目標方向:
ワークが床と干渉しない高さ:Hc (ワークの大きさを考慮した高さ)
新目標のときのワークの位置:
新目標のときのワークの高さ:
とすると,目標方向のz成分が決まる.
・旧目標方向でワーク高さが十分のときは
高さ不十分のときは(リスト−ワーク間アーム長をとして)
リストが低すぎるとは存在しない.これはリストが床面以下にあることを意味し,ワークの方向計算する前にリストの干渉回避をするので,通常は考えなくてよい.
・この方向の限界円錐面上で2通りあるが,現在のワークの方向に近い値を採用する.これによって初期方向から新しい目標へ円錐面との干渉の心配なしに方向ベクトルの移動経路を生成することができる.
・リストから見たC点のベクトルは
・円錐面上の単位ベクトルが描く円の半径rは
・回転面の座標系を
,
とする.
・新しい目標方向を回転円上で
からの角度θで表すと
・新しい目標を旧目標と高さ等しい方向とする.
・のz成分を上で設定した
と等しいと置いて
ただし ,
・書き直して
・と置くと
∴
・リストから見た新しいワークの目標方向は
・α,βは目標が円錐内にあれば必ず存在する.
・解は2通りあり,円錐の最低点から見て初期のワークの方向と
が同じ側にある値を選択する.
・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・
[補]
・円錐の最低点は
のときである.
・初期ワークの方向を座標系で表してθWの方向にあるとすると
・φWはと
がなす角であり
・φW=0のときは初期方向がJ[M-1]アーム軸と一致していることを示し,θ1はのどちらでも構わない.
・φW=0でないとき,上記等式を成り立たせるθWは,(X, Y, Z)どれかの成分の左右辺を等値して得られるが,ここでは2成分の等値から求める.
,
,
,
・円錐最低点Lから見て初期ベクトルと新しい目標ベクトルが円錐面の同じ側にあるということは,
であり,これを満足するθ1を選択する.
・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・