ロボットの移動方向決定方法及びロボットの制御装置
【課題】ユーザがロボットをリアルタイムで動作させる場合でも、ロボットを動作領域の限界まで動作させる。
【解決手段】ロボットの動作領域空間をキューブの集合として捉え、各キューブの外面を規定する３次元座標値を決定し、それらを安全空間と危険空間との何れかに評価した空間評価データを用い、手先部分を覆うガード領域Ｇを設定する。ユーザが、ジョイスティックで指示した方向に手先を移動させ続けるとガード領域Ｇと危険空間が干渉する場合で両者が面で接触すれば、その面と平行な方向に手先を移動可能であれば単位距離だけ移動させた後（Ｓ１４）、指示方向への移動を再試行する。両者が面で接触しない場合，又はガード領域Ｇが干渉する場合は、指示方向を３つの方向ベクトルに分解して第１方向，第２方向を定め（Ｓ５）、手先を第１方向に移動可能であれば単位距離だけ移動させた後（Ｓ１５）指示方向への移動を再試行し、干渉する場合は第１方向を４５度回転させた方向を次に移動させる候補にする（Ｓ８）。
【解決手段】ロボットの動作領域空間をキューブの集合として捉え、各キューブの外面を規定する３次元座標値を決定し、それらを安全空間と危険空間との何れかに評価した空間評価データを用い、手先部分を覆うガード領域Ｇを設定する。ユーザが、ジョイスティックで指示した方向に手先を移動させ続けるとガード領域Ｇと危険空間が干渉する場合で両者が面で接触すれば、その面と平行な方向に手先を移動可能であれば単位距離だけ移動させた後（Ｓ１４）、指示方向への移動を再試行する。両者が面で接触しない場合，又はガード領域Ｇが干渉する場合は、指示方向を３つの方向ベクトルに分解して第１方向，第２方向を定め（Ｓ５）、手先を第１方向に移動可能であれば単位距離だけ移動させた後（Ｓ１５）指示方向への移動を再試行し、干渉する場合は第１方向を４５度回転させた方向を次に移動させる候補にする（Ｓ８）。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ユーザが、操作手段を介してロボットの手先の移動方向を指示する操作入力を行った場合、実際に手先を移動させる方向を決定する方法，及びロボットの制御装置に関する。
【背景技術】
【０００２】
ロボットを動作させる場合には、ロボットが周辺の設備に衝突することを回避するため、動作領域の限界を定める、いわゆるバーチャルフェンス，或いはソフトウェアケージと称する仮想的なフェンスやケージを設定することが行われている。例えば特許文献１には、指定された目標点に向かうためロボットの動作軌跡を生成する場合に、その軌跡の途中に障害物が存在すると、目標点を前記障害物の直前の位置に変更する技術が開示されている。また、特許文献２には、特許文献１と同様に目標点に向かうためロボットの動作軌跡を生成する場合に、動作領域について既に得ている情報から、安全な経路を選択するように検索を行う技術が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平７−１２９２１４号公報
【特許文献２】特開２００３−２８０７１０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ロボットを制御プログラムに従い動作させる場合（変数移動）であれば、特許文献１，２に開示されている技術を適用することで、手先を障害物との境界に沿って動かすことが可能となり、動作領域の限界まで動作させることができる。しかしながら、ユーザが、例えばジョイステイック等の操作手段を用いて、移動方向をリアルタイムで指示しながらロボットを動作させる場合には、移動の最終目標座標が定まらないため特許文献１，２のような技術が適用できず、ロボットを動作領域の限界まで動作させることができないという問題がある。
【０００５】
本発明は上記事情に鑑みてなされたものであり、その目的は、ユーザがロボットをリアルタイムで動作させる場合でも、ロボットを動作領域の限界まで動作させることが可能となるロボットの移動方向決定方法，及びロボットの制御装置を提供することにある。
【課題を解決するための手段】
【０００６】
請求項１記載のロボットの移動方向決定方法によれば、ロボットが動作する動作領域空間を立方体状の単位空間の集合として捉え、各単位空間の外面を規定する３次元座標値を決定し、それらの各単位空間を、ロボットが通過可能な安全空間と、ロボットが通過不能な危険空間との何れかに評価した空間評価データを用意する。また、単位空間よりもサイズが大きな立方体状の空間によって、ロボットの手先部分を覆うガード領域を設定する。
【０００７】
そして、ユーザが、操作手段を介してロボットの手先の移動方向を指示する操作入力を行い、手先を操作入力に応じた指示方向に移動させ続けるとガード領域が危険空間に進入することが予測される場合には、ガード領域と危険空間とが接する状態で移動を一旦停止させる。この時、両者が面で接触する場合は、その面に平行となる方向を第１候補として、手先を前記方向に移動させるとガード領域が安全空間に進入する場合は、前記方向に前記単位空間の１辺の距離に相当する単位距離だけ移動させた後、指示方向への移動を再試行する。
【０００８】
最初にガード領域と危険空間とが面で接触しない場合，又は第１候補の方向に移動させるとガード領域が危険空間に進入する場は、指示方向をロボットの基準座標系の原点に立つ単位ベクトルで表わし、単位ベクトルの方向を３次元座標上で３つの方向ベクトルに成分分解する。そして、３つの方向ベクトルの長さが最長のベクトルの方向を第１方向，それ以外の２つのベクトルを合成した方向を第２方向に定め、第１方向を次に移動させる第２候補として設定する。ここで、３つの方向ベクトルのうち、何れか２つの長さが等しい場合には、そのうちの何れか一方を選択して第１及び第２方向ベクトルを定める。また、３つの方向ベクトルの、全ての長さが等しい場合は、何れか２つの方向を選択して第１及び第２方向ベクトルを定める。
【０００９】
すなわち、動作領域空間は単位空間の集合であるため、ガード領域と危険空間とが接している状態から手先が移動可能な方向は、±９０度，±１３５度，１８０度の何れかとなる。そこで、手先を第１方向に移動させると安全空間に進入する場合は、そのまま第１方向に単位距離だけ移動させた後、指示方向への移動を再試行する。一方、第１方向に移動させるとガード領域が危険空間に進入する場合は、第２方向を基準として第１方向を４５度回転させた第１回転方向（第２方向基準で１３５度回転させた方向）を、次に移動させる第３候補として設定する。
【００１０】
次に、手先を第１回転方向に移動させるとガード領域が安全空間に進入する場合は、第１回転方向に単位距離の２１／２倍だけ移動させた後、指示方向への移動を再試行し、ガード領域が危険空間に進入する場合は、第２方向を基準として第１回転方向を更に４５度回転させた第２回転方向（第２方向基準で１８０度回転させた方向）を、次に移動させる第４候補として設定する。更に、手先を第２回転方向に移動させるとガード領域が安全空間に進入する場合は、第２回転方向に単位距離だけ移動させた後指示方向への移動を再試行し、ガード領域が危険空間に進入する場合は、その時点で移動を停止させる。
【００１１】
このように移動方向を決定すれば、ユーザがリアルタイムで、操作手段を介してロボットの手先の移動方向を指示する操作入力を行うことで手先が危険空間に近付いた場合に、危険空間が連続する境界に沿わせながら、ユーザが指示した方向に極力近づくように手先を移動させることができる。したがって、ユーザがリアルタイムで移動方向を指示する場合でも、ロボットの手先を、安全空間として設定されている領域は全て移動させることが可能となり、空間を限界まで使用することができる。また、ガード領域が危険空間に接した場合は、ユーザが操作手段を介して指示した方向を基準として、手先が危険空間との境界を滑るようにして移動するので、ユーザがイメージした手先の動作に近い動きになり、予想を逸脱するような動きにならず、危険な動作状態が生じることを抑制できる。
【００１２】
請求項２記載のロボットの移動方向決定方法によれば、ガード領域のサイズを変更可能とし、安全空間内において、手先を移動させる速度が上昇するのに応じてガード領域のサイズを段階的に拡大させ、安全空間内において、手先を移動させる速度が下降するのに応じてガード領域のサイズを段階的に縮小させる。これにより、手先の移動速度が速ければガード領域のサイズが大きくなるため狭い安全空間には進入し難くなり、結果としてガード領域；手先はより広く設定されている安全空間に沿って移動することになる。すなわち、手先の移動速度が速い場合とは、作業者が単位時間当たりの手先移動距離を長くすることを所望している状態であり、目標位置の近傍において移動を精密に制御する状態とは異なっている。したがって、手先をより広い安全空間に沿って移動させることで、移動距離をより長く伸ばすことが可能となる。
【００１３】
請求項３記載のロボットの移動方向決定方法によれば、ガード領域のサイズの拡大及び縮小を、前記手先の移動速度の速さに応じて複数段階で行うので、ガード領域のサイズをより広いレンジで変化させることができる。
【００１４】
請求項４記載のロボットの移動方向決定方法によれば、手先を、現在位置が存在する単位空間からｎ個先にある単位空間内にある目標座標まで移動させる際に、ｎ個先の単位空間が安全空間であることを条件に、手先の移動速度により１サンプリングタイムで進む単位空間の数をｎｓとすると、ガード領域のサイズをｎ／ｎｓの値の大きさを基準として決定する。これにより、ガード領域のサイズを手先の移動速度の速さとのバランスに応じて最適に決定することができる。
【００１５】
請求項５記載のロボットの移動方向決定方法によれば、ｎ個先の単位空間が危険空間であれば、ガード領域のサイズを１段階だけ拡大させる。すなわち、手先を危険空間との境界に沿って移動させている状態とは、作業者が単位時間当たりの手先移動距離を長くすることを所望している状態であり、目標位置の近傍において移動を精密に制御する状態とは異なっている。したがって、危険空間との干渉を回避しながら、請求項２と同様に移動距離をより長く伸ばすことが可能となる。
【図面の簡単な説明】
【００１６】
【図１】第１実施例であり、手先の移動方向を決定する処理を示すフローチャート
【図２】手先の動きを２次元的に示した場合の処理手順をイメージ的に示す図
【図３】指示方向を３軸方向ベクトル成分に分解する状態を示す図
【図４】ガード領域Ｇが障害物ＯＢに接近した場合の３次元イメージを示す図
【図５】ロボット本体の手先にガード領域Ｇを設定した状態を示す図
【図６】動作領域空間にキューブを設定した状態を示す図
【図７】垂直多関節型ロボットのシステム構成を示す図
【図８】機能ブロック図
【図９】第２実施例を示す図１の一部相当図
【図１０】ガード領域Ｇのサイズが変化するイメージを示す図
【図１１】第３実施例を示す図１の一部相当図
【図１２】手先の移動とガード領域Ｇの拡大との関係を説明する図
【発明を実施するための形態】
【００１７】
（第１実施例）
以下、第１実施例について図１ないし図８を参照して説明する。図７は、垂直多関節型（６軸）ロボットのシステム構成を示す。このロボット本体１は、ベース（回転軸）２上に、この場合６軸のアームを有している。前記ベース２上には、第１関節Ｊ１を介してショルダ部３が回転可能に連結されている。このショルダ部３には、第２関節Ｊ２を介して上方に延びる下アーム４の下端部が回転可能に連結され、さらに、この下アーム４の先端部には、第３関節Ｊ３を介して第１上アーム５が回転可能に連結されている。
【００１８】
この第１上アーム５の先端には第４関節Ｊ４を介して第２上アーム６が回転可能に連結され、この第２上アーム６の先端には第５関節Ｊ５を介して手首７が回転可能に連結され、この手首７には第６関節Ｊ６を介して第６のフランジ８が回転可能に連結されている。なお、各関節Ｊ１〜Ｊ６においては、サーボモータ２６（図８参照）によりショルダ部３〜フランジ８を回転駆動する。
【００１９】
ロボット本体１と制御装置（コントローラ，ガード領域設定手段，方向決定手段）１１との間は、接続ケーブル１２によって接続されている。これにより、ロボット本体１の各軸を駆動するサーボモータは、制御装置１１により制御される。制御装置１１には、パーソナルコンピュータ（パソコン）１３がケーブル１４を介して接続され、パソコン１３との間で、ロボット本体１の手先の現在位置の送信や動作コマンドの受信など、高速通信を実行するようになっている。パソコン１３には、メモリやハードディスクなどの記憶装置が内蔵されている。
【００２０】
ジョイスティック（操作手段）１５は、ケーブル１６を介してパソコン１３に接続されている。ユーザは、ジョイスティック１５を操作して倒した方向により、ロボット本体１の手先（フランジ８の先端，ＴＣＰ：Tool Center Point）の移動方向を指示する。その指示方向は、パソコン１３を介して制御装置１１に伝達され、制御装置１１は、ロボット本体１の手先をＸＹ平面上で指示された方向に移動させるように制御する。
【００２１】
制御装置１１は、図８に示すように、ＣＰＵ２１と、駆動回路２２と、位置検出回路２３とを備えて構成されている。ＣＰＵ２２には、システムプログラムや動作プログラムを作成するためのロボット言語などを記憶するＲＯＭ２４及び動作プログラムなどを記憶するＲＡＭ２５が接続されていると共に、図示しない通信インターフェイスを介してパソコン１３が接続されている。
【００２２】
位置検出回路２３は、ショルダ部３〜フランジ８の位置を検出するためのもので、各軸の駆動源であるサーボモータ２６に設けられているロータリエンコーダ２７が接続されている。位置検出回路２３は、ロータリエンコーダ２７から入力される検出信号に基づいて各軸の回転角度を検出し、それら検出した位置検出情報をＣＰＵ２１に出力する。そして、ＣＰＵ２１は、動作プログラムに基づいてショルダ部３〜フランジ８を動作させる際に、位置検出回路２３から入力する位置検出情報をフィードバック信号として、各軸のサーボモータ２６を駆動する。
【００２３】
次に、本実施例の作用について図１ないし図６を参照して説明する。本実施例では、図６に示すように、ロボット本体１が動作する領域を、立方体状の単位空間（以下、キューブと称す）を３次元的に積み上げたものとして設定しており、各キューブを構成する６面，或いは１２本の直線を規定する３次元座標値が決定されている。すなわち、制御装置１１のソフトウェア上で、そのように仮想されている。そして、各キューブについては、ロボット本体１が動作した場合に、周辺の障害物等に接触する可能性があるものが「危険」と評価され（危険空間）、接触する可能性がないものが「安全」と評価されている（安全空間）。そして、その情報が空間評価データベースとして、例えばＲＡＭ（記憶手段）２５に予め記憶されている。
【００２４】
一方、ロボット本体１については、図５に示すように、手先を覆うようにガード領域Ｇが設定されている（キューブと同様にソフトウェア上で仮想されている）。例えばガード領域Ｇの中心は手先に設定されている。この場合、ガード領域Ｇの体積（サイズ）は、キューブの体積よりも大きく設定されており、例えば１０倍程度となっている。そして、ロボット本体１を動作させることで、手先であるフランジ８の位置が変化すると、その位置の変化に応じてガード領域Ｇの位置もソフトウェア上で変化する。
【００２５】
図１は、制御装置２が手先の移動方向を決定する処理を示すフローチャートであり、図２は、簡単のため、手先の動きを２次元的に示した場合の処理手順をイメージ的に示す図である。ユーザが、例えばジョイスティック１５を操作して、ロボット本体１の手先の移動方向を指示した場合、その指示方向に手先を移動させると、ガード領域Ｇが危険空間と干渉するか否かを判断する（ステップＳ１）。ここで「干渉」とは、ガード領域Ｇが危険空間の一部に進入する（両者が交錯する）状態を言う。両者が干渉しなければ（ＮＯ）、指示方向に手先を移動させて（ステップＳ１３）ステップＳ１に戻る。また、以降では、上記のように干渉するか否かの判断を「干渉チェック」と称する。
【００２６】
一方、ガード領域Ｇが危険空間と干渉する場合は（ステップＳ１：ＹＥＳ）、干渉が発生する直前の状態で手先を停止させ、指示方向を、図３に示すように、ロボット本体１の基準座標上で原点に立つ単位ベクトルＤと見た場合に、そのベクトルＤを、Ｘ，Ｙ，Ｚ方向成分である各軸方向ベクトルＤｘ，Ｄｙ，Ｄｚに分解する。そして、それらのうち、危険空間のＸＹ平面，ＹＺ平面，ＺＸ平面の何れかと干渉するベクトルを除いた２つの合成ベクトルの方向（上記干渉する面に対して平行となる方向）を、次に手先を移動させる方向の候補（第１候補）とする（ステップＳ２，図２（ａ）（２）参照）。
【００２７】
尚、図２では、ユーザがジョイスティック１５を操作することで、ジョグ動作として指示した方向を破線矢印で示しており、移動候補となる方向を実線矢印で示している。そして、前記の平行な方向に移動させるとした場合について干渉チェックを行い（ステップＳ３）、干渉しなければ（ＮＯ）手先を当該方向に、キューブの１辺の長さである単位距離だけ移動させて（ステップＳ１４）ステップＳ１に戻る。また、両者が干渉する場合は（ＹＥＳ）ステップＳ４に移行する。
【００２８】
ステップＳ４では、指示方向を、ステップＳ２と同様に、ロボット本体１の基準座標上で原点に立つ単位ベクトルＤと見た場合に、そのベクトルＤを、Ｘ，Ｙ，Ｚ方向成分である各軸方向ベクトルＤｘ，Ｄｙ，Ｄｚに分解する。それから、各方向ベクトルの長さを評価して、長さが最も長いベクトルの方向を第１方向，それ以外の２つのベクトルを合成した方向を第２方向とする（ステップＳ５）。図３は、ここでの処理を３次元的なイメージで示す。但し、３つの方向ベクトルＤｘ，Ｄｙ，Ｄｚのうち、何れか２つの長さが等しい場合には、そのうちの何れか一方を選択して第１及び第２方向ベクトルを定める。
【００２９】
また、３つの方向ベクトルＤｘ，Ｄｙ，Ｄｚの、全ての長さが等しい場合は、何れか２つの方向を選択して第１及び第２方向ベクトルを定める。すなわち、上記方向ベクトルの長さはジョイスティック１５の移動方向の近似度を示しているので、長さが等しいということは、その何れの方向に移動させてもジョイスティック１５によって指示された方向に対する近似度は等しくなるからである。
【００３０】
第１及び第２方向を定めると、第１方向が示す方向（第１方向）を、手先を次に移動させる候補として（第２候補，ステップＳ６，図２（ａ）（３）参照）、第１方向に移動させるとした場合の干渉チェックを行う（ステップＳ７）。そして干渉しなければ（ＮＯ）手先を第１方向に単位距離だけ移動させてから（ステップＳ１５）ステップＳ１に戻る。一方、第１方向に移動させると干渉する場合は（ＹＥＳ，図２（ａ）（４）参照）、第１方向を、第２方向を基準として４５度回転させて（第１回転方向，ステップＳ８，図２（ａ）（５）参照）、その第１回転方向について干渉チェックを行う（ステップＳ９）。
【００３１】
図３では、方向ベクトルＤｘが第１方向となり、方向ベクトルＤｙ，Ｄｚの方向を合成したベクトルＤyzの方向が第２方向となる。そして、方向ベクトルＤｘを、合成ベクトルＤyzを基準として４５度回転させた（或いは、ベクトル（−Ｄyz）に向けて４５度回転させた）ベクトルＤｒの方向が、第１回転方向となっている（第２方向から＋１３５度）。ステップＳ９におけるチェックの結果、干渉しない場合は（ＮＯ）手先を第１回転方向に、単位距離の２１／２倍だけ移動させてから（ステップＳ１６）ステップＳ１に戻る。
【００３２】
一方、第１回転方向に移動させると干渉する場合は（ステップＳ９：ＹＥＳ，図２（ａ）（６）参照）、その第１回転方向を更に４５度回転させる（第２回転方向，ステップＳ１０，図２（ａ）（７）参照）。そして、第２回転方向（第２方向から＋１８０度）について干渉チェックを行い（ステップＳ１１）、干渉しない場合は（ＮＯ，図２（ａ）（８）参照）手先を第２回転方向に単位距離だけ移動させてから（ステップＳ１７，図２（ａ）（９）参照）ステップＳ１に戻る。第２回転方向に移動させると干渉する場合は（ステップＳ１１：ＹＥＳ）、この時点で指示方向に基づく移動を停止させてエラー出力を行う（ステップＳ１２）。例えば、指示方向の変更を促す旨のメッセージを、パソコン１３のディスプレイ画面に表示させるなどする。
【００３３】
図２（ａ）（１０）は、（１）と同様に、（９）の状態から再び指示方向への移動を試みた場合を示している。この場合も干渉が発生するので、（３）と同様に方向成分の分解を行う（１１）。そして、第１方向に移動させても干渉が発生しないので、第１方向に単位距離だけ移動させる（１２）。
図２（ｂ）は、危険空間をマクロ的に見た場合に安全空間との境界が斜面となっており、手先を破線矢印方向に移動させるように指示すると、上記斜面にガード領域Ｇが接して干渉する場合である。このような状態で本実施例の処理を適用すると、手先は斜面に沿って移動するようになり、危険空間が存在しなくなった位置で指示方向に移動するようになる。
【００３４】
また、図４は、３次元イメージで示しており、ロボット本体１の動作領域空間に円柱状の障害物ＯＢが存在する場合、その障害物ＯＢは、制御装置２では危険空間であるキューブの集合として把握されている。そして、ロボット本体１の手先の移動方向が図中の矢印方向で指示されている場合に、ガード領域Ｇが障害物ＯＢに接近すると、ガード領域Ｇは、障害物ＯＢの表面に沿って、すなわち危険空間との境界に沿って移動するようになり、障害物ＯＢとの干渉が回避される。
【００３５】
以上のように本実施例によれば、ロボット本体１が動作する動作領域空間をキューブの集合として捉え、各キューブの外面を規定する３次元座標値を決定し、それらの各キューブを、ロボットが通過可能な安全空間と通過不能な危険空間との何れかに評価した空間評価データを用い、キューブよりもサイズが大きな立方体状の空間によって、ロボット本体１の手先部分を覆うガード領域Ｇを設定する。そして、ユーザが、ジョイスティック１５を介して指示した方向に手先を移動させ続けると、ガード領域Ｇと危険空間とが干渉すると予測される場合は両者が接する状態で移動を一旦停止させ、両者が面で接触する場合はその面に平行となる方向を第１候補として、手先を前記方向に移動可能であれば当該方向に単位距離だけ移動させた後指示方向への移動を再試行する。
【００３６】
また、ガード領域Ｇと危険空間とが面で接触しない場合，或いは第１候補の方向に移動させるとガード領域Ｇが干渉する場合は、指示方向を３つの方向ベクトルに成分分解して第１方向，第２方向を定め、第１方向を次に移動させる第２候補とする。そして、手先を第１方向に移動可能であれば、そのまま第１方向に単位距離だけ移動させた後指示方向への移動を再試行し、干渉する場合は、第１方向を４５度回転させた第１回転方向を、次に移動させる第３候補とする。
次に、手先を第１回転方向に移動可能であれば、第１回転方向に単位距離の２１／２倍だけ移動させた後指示方向への移動を再試行し、干渉する場合は、第１回転方向を更に４５度回転させた第２回転方向を次に移動させる第４候補とする。更に、手先を第２回転方向に移動可能であれば、第２回転方向に単位距離だけ移動させた後指示方向への移動を再試行し、干渉する場合はその時点で移動を停止させるようにした。
【００３７】
このように移動方向を決定すれば、ユーザがリアルタイムで、ジョイスティック１５を介してロボット本体１の手先の移動方向を指示する操作入力を行うことで手先が危険空間に近付いた場合に、危険空間が連続する境界に沿わせながら、ユーザが指示した方向に極力近づくように手先を移動させることができる。
したがって、ユーザがリアルタイムで移動方向を指示する場合でも、ロボット本体１の手先を、安全空間として設定されている領域は全て移動させることが可能となり、動作領域空間を限界まで使用することができる。また、ガード領域Ｇが危険空間に接した場合は、指示方向を基準として、手先が危険空間との境界を滑るようにして移動するので、ユーザがイメージした手先の動作に近い動きになり、予想を逸脱するような動きにならず、危険な動作状態が生じることを抑制できる。
【００３８】
（第２実施例）
図９及び図１０は第２実施例であり、第１実施例と同一部分には同一符号を付して説明を省略し、以下異なる部分について説明する。図９は図１の一部相当図であり、ステップＳ１で「ＮＯ」と判断してからステップＳ１３に移行する間に、ステップＳ２１〜Ｓ２５の処理を実行するようになっている。先ず、ステップＳ２１では、ユーザにより設定されたロボットの手先の移動速度をしきい値（１）と比較する。手先の移動速度は、例えばジョイスティック１５を傾けた速さなどで設定される。そして、移動速度がしきい値（１）以下であれば（ＮＯ）、ガード領域Ｇのサイズを設定値（１）とする（ステップＳ２３）。設定値（１）は、デフォルトの最小サイズであり、例えば１辺の長さがキューブ２，３個分程度とする。
【００３９】
ステップＳ２１において、移動速度がしきい値（１）を超えていると（ＹＥＳ）、次に移動速度を、しきい値（２）と比較する（ステップＳ２２）。尚、しきい値（２）は、しきい値（１）よりも高い値に設定されている。そして、移動速度がしきい値（２）以下であれば（ＮＯ）、ガード領域Ｇのサイズを設定値（２）とする（ステップＳ２４）。設定値（２）は、設定値（１）に対して、１辺の長さを例えばキューブ１，２個分程度増加させたサイズとする。例えば、設定値（１）における１辺のサイズがキューブ３個分（３Ｃｂとする）であるとすると、そこから１辺のサイズを４Ｃｂにすればガード領域Ｇの体積増加分は、６４／２７≒２．７３倍となる。
【００４０】
一方、ステップＳ２２において、移動速度がしきい値（２）を超えていると（ＹＥＳ）、ガード領域Ｇのサイズを設定値（３）とする（ステップＳ２５）。設定値（３）は、設定値（２）に対して、更に１辺の長さを例えばキューブ１，２個分程度増加させたサイズとする。尚、ユーザがロボット本体１を直接操作する場合の移動速度は、一般に、自動運転時の１０％〜２０％程度に設定されている。その範囲における最大速度の範囲内でしきい値（１），（２）を設定する。
【００４１】
すなわち、図９に示す処理を実行する毎に、手先の移動速度がしきい値（１），（２）と比較され、その比較結果に応じてガード領域Ｇのサイズが拡大したり縮小したり変化することになる。図１０は、ガード領域Ｇのサイズが変化するイメージを示している。（ａ）は手先の移動速度が低速の場合であり（例えばしきい値（１）以下）、ガード領域Ｇのサイズが小さく設定されるので、より精密な位置制御が可能であり、安全空間の開口部にも進入可能となっている。一方、（ｂ）は手先の移動速度が高速の場合であり（例えばしきい値（２）を上回る）、ガード領域Ｇのサイズが大きく設定されているため、安全空間の開口部には進入不能となっている（図中に破線で示すように、開口部に向かう以外の方向には移動可能である）。
【００４２】
以上のように第２実施例によれば、ガード領域Ｇのサイズを変更可能とし、安全空間内において、手先を移動させる速度が上昇するのに応じてガード領域Ｇのサイズを段階的に拡大させ、安全空間内において、手先を移動させる速度が下降するのに応じてガード領域Ｇのサイズを段階的に縮小させるようにした。これにより、手先の移動速度が速ければガード領域Ｇのサイズが大きくなるため狭い安全空間には進入し難くなり、結果としてガード領域Ｇ；手先はより広く設定されている安全空間に沿って移動することになる。
【００４３】
すなわち、手先の移動速度が速い場合とは、作業者が単位時間当たりの手先移動距離を長くすることを所望している状態であり、目標位置の近傍において移動を精密に制御する状態とは異なっている。したがって、手先をより広い安全空間に沿って移動させることで、移動距離をより長く伸ばすことが可能となる。また、ガード領域Ｇのサイズの拡大及び縮小を、手先の移動速度の速さに応じて複数段階で行うので、ガード領域Ｇのサイズをより広いレンジで変化させることができる。
【００４４】
（第３実施例）
図１１及び図１２は第３実施例であり、第１又は第２実施例と異なる部分のみ説明する。第３実施例では、第２実施例と同様にガード領域Ｇのサイズを手先の移動速度に応じて変化させるが、その手法が異なっている。図１１において処理を開始すると、ステップＳ３０〜Ｓ３５を実行し、その後、ステップＳ１，Ｓ２に替えて、ステップＳ４１〜４４を実行する。この場合、手先の移動目標位置がユーザ（作業者）により予め決定されるので、それに伴い、目標位置の座標が含まれているキューブが現在位置から何個目になるのか（ｎ）を決定する（ステップＳ３０）。続いて「ｎ＝１」か否かを判断し（ステップＳ３１）、「ｎ＝１」であれば（ＹＥＳ）ステップＳ４１に移行する。この場合、ガード領域Ｇのサイズをデフォルトから拡大させる余地はない。また、「ｎ≧２」であれば（ＮＯ）ステップＳ３２に移行する。
【００４５】
ステップＳ３２では、設定されている移動速度が最小値（ＭＩＮ）か否かを判断し、最小値（ＭＩＮ）であれば（ＹＥＳ）ステップＳ４１に移行し（上記と同様に理由による）、最小値でなければ（ＮＯ）ステップＳ３３に移行する。そして、上記移動速度によって、制御装置１１によるロボット本体１の制御周期である１サンプリングタイム（単位時間）Ｔｓにおいて、手先が進むキューブの数ｎｓを計算し、その数ｎｓがステップＳ３０で求められたｎ以上か否かを判断する。（ｎｓ≧ｎ）であれば（ＹＥＳ）、移動速度を最小値に設定してからステップＳ４１に移行する。すなわち（ｎｓ≧ｎ）の場合も、ガード領域Ｇのサイズを拡大させる余地はないからである。
【００４６】
一方、ステップＳ３３において、（ｎｓ＜ｎ）であれば（ＮＯ）ｎ／ｎｓを計算し、その商の値に応じてガード領域Ｇの大きさを設定してから（ステップＳ３５）、ステップＳ４１に移行する。すなわち、ガード領域Ｇの大きさをどのように設定すべきかは、その時点の現在位置から目標位置までの移動距離と、移動速度の速さとに応じて相対的に変化するため、ｎ／ｎｓを指標として用い、最適な大きさを設定する。
【００４７】
例えば、図１２に示すように、ｎ＝２５であり、移動距離並びに移動速度に応じて決まるキューブ数ｎｓ＝５の場合はｎ／ｎｓ＝５となる。手先ＫＴＣＰがｎ＝０の位置にあれば、ｎ個先のキューブに干渉する直前まで４Ｔｓ（４制御周期）の時間があることになる（但し、ガード領域Ｇの一片の大きさが１０Ｃｂ以下であることを条件とする）。そして、ガード領域Ｇの１辺の大きさを拡大させれば上記の到達時間はより短くなるが、初期値が１０Ｃｂ以下であるとすれば増加数を３０Ｃｂ以下にすれば、次の制御周期における移動でガード領域Ｇが危険領域に干渉することは無い。したがって、ｎ／ｎｓを指標として用いることで最適な大きさを設定することができる。
【００４８】
ステップＳ４１では、指示方向にキューブｎ個分移動するか否かを判断し、干渉しなければ（ＮＯ）ステップＳ１３に移行する。また、干渉する場合は（ＹＥＳ）、ガード領域Ｇが拡大可能か否かを判断する（ステップＳ４２）。ここでの判断は、上述したステップＳ３５の場合と同様であり、例えばｎ／ｎｓを指標として用いることで判断すれば良い。そして、拡大可能であれば（ＹＥＳ）１段階だけ拡大させてから（ステップＳ４３）ステップＳ２に替わるステップＳ４４に移行する。また、拡大不能であれば（ＮＯ）そのままステップＳ４４に移行する。ステップＳ４４では、ステップ２と同様の処理を、キューブｎ個分移動した位置について行う。
【００４９】
すなわち、手先を危険空間との境界に沿って移動させている状態は、ユーザがサンプリングタイム当たりの手先移動距離を長くすることを所望している状態であり、目標位置の近傍において移動を精密に制御する状態とは異なっている。したがって、ガード領域Ｇを１段階だけ拡大させて危険空間との干渉を回避しつつ移動距離をより長く伸ばすようにする。
【００５０】
以上のように第３実施例によれば、ロボット本体１の手先を、現在位置が存在するキューブからｎ個先にあるキューブ内にある目標座標まで移動させる際に、ｎ個先のキューブが安全空間であることを条件に、手先の移動速度により１サンプリングタイムで進むキューブ数をｎｓとすると、ガード領域Ｇのサイズをｎ／ｎｓの値の大きさを基準として決定するようにした。これにより、ガード領域Ｇのサイズを手先の移動速度の速さとのバランスに応じて最適に決定できる。また、ｎ個先のキューブが危険空間であれば、ガード領域Ｇのサイズを１段階だけ拡大させるので、危険空間との干渉を回避しつつ移動距離をより長く伸ばすことができる。
【００５１】
本発明は上記し、又は図面に記載した実施例にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
操作手段は、ジョイスティック１５に限ることなく、少なくとも方向を２次元で指示する操作入力が可能な手段であれば良い。
キューブとガード領域とのサイズについては、両者の大小関係を維持する範囲で適宜変更して良い。
ガード領域の拡大，縮小は、少なくとも１段階行えば良く、また、３段階以上に変化させても良い。
【符号の説明】
【００５２】
図面中、１はロボット本体、１１は制御装置（ガード領域設定手段，方向決定手段，領域サイズ変更手段）、１５はジョイスティック（操作手段）、２５はＲＡＭ（記憶手段）を示す。
【技術分野】
【０００１】
本発明は、ユーザが、操作手段を介してロボットの手先の移動方向を指示する操作入力を行った場合、実際に手先を移動させる方向を決定する方法，及びロボットの制御装置に関する。
【背景技術】
【０００２】
ロボットを動作させる場合には、ロボットが周辺の設備に衝突することを回避するため、動作領域の限界を定める、いわゆるバーチャルフェンス，或いはソフトウェアケージと称する仮想的なフェンスやケージを設定することが行われている。例えば特許文献１には、指定された目標点に向かうためロボットの動作軌跡を生成する場合に、その軌跡の途中に障害物が存在すると、目標点を前記障害物の直前の位置に変更する技術が開示されている。また、特許文献２には、特許文献１と同様に目標点に向かうためロボットの動作軌跡を生成する場合に、動作領域について既に得ている情報から、安全な経路を選択するように検索を行う技術が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平７−１２９２１４号公報
【特許文献２】特開２００３−２８０７１０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ロボットを制御プログラムに従い動作させる場合（変数移動）であれば、特許文献１，２に開示されている技術を適用することで、手先を障害物との境界に沿って動かすことが可能となり、動作領域の限界まで動作させることができる。しかしながら、ユーザが、例えばジョイステイック等の操作手段を用いて、移動方向をリアルタイムで指示しながらロボットを動作させる場合には、移動の最終目標座標が定まらないため特許文献１，２のような技術が適用できず、ロボットを動作領域の限界まで動作させることができないという問題がある。
【０００５】
本発明は上記事情に鑑みてなされたものであり、その目的は、ユーザがロボットをリアルタイムで動作させる場合でも、ロボットを動作領域の限界まで動作させることが可能となるロボットの移動方向決定方法，及びロボットの制御装置を提供することにある。
【課題を解決するための手段】
【０００６】
請求項１記載のロボットの移動方向決定方法によれば、ロボットが動作する動作領域空間を立方体状の単位空間の集合として捉え、各単位空間の外面を規定する３次元座標値を決定し、それらの各単位空間を、ロボットが通過可能な安全空間と、ロボットが通過不能な危険空間との何れかに評価した空間評価データを用意する。また、単位空間よりもサイズが大きな立方体状の空間によって、ロボットの手先部分を覆うガード領域を設定する。
【０００７】
そして、ユーザが、操作手段を介してロボットの手先の移動方向を指示する操作入力を行い、手先を操作入力に応じた指示方向に移動させ続けるとガード領域が危険空間に進入することが予測される場合には、ガード領域と危険空間とが接する状態で移動を一旦停止させる。この時、両者が面で接触する場合は、その面に平行となる方向を第１候補として、手先を前記方向に移動させるとガード領域が安全空間に進入する場合は、前記方向に前記単位空間の１辺の距離に相当する単位距離だけ移動させた後、指示方向への移動を再試行する。
【０００８】
最初にガード領域と危険空間とが面で接触しない場合，又は第１候補の方向に移動させるとガード領域が危険空間に進入する場は、指示方向をロボットの基準座標系の原点に立つ単位ベクトルで表わし、単位ベクトルの方向を３次元座標上で３つの方向ベクトルに成分分解する。そして、３つの方向ベクトルの長さが最長のベクトルの方向を第１方向，それ以外の２つのベクトルを合成した方向を第２方向に定め、第１方向を次に移動させる第２候補として設定する。ここで、３つの方向ベクトルのうち、何れか２つの長さが等しい場合には、そのうちの何れか一方を選択して第１及び第２方向ベクトルを定める。また、３つの方向ベクトルの、全ての長さが等しい場合は、何れか２つの方向を選択して第１及び第２方向ベクトルを定める。
【０００９】
すなわち、動作領域空間は単位空間の集合であるため、ガード領域と危険空間とが接している状態から手先が移動可能な方向は、±９０度，±１３５度，１８０度の何れかとなる。そこで、手先を第１方向に移動させると安全空間に進入する場合は、そのまま第１方向に単位距離だけ移動させた後、指示方向への移動を再試行する。一方、第１方向に移動させるとガード領域が危険空間に進入する場合は、第２方向を基準として第１方向を４５度回転させた第１回転方向（第２方向基準で１３５度回転させた方向）を、次に移動させる第３候補として設定する。
【００１０】
次に、手先を第１回転方向に移動させるとガード領域が安全空間に進入する場合は、第１回転方向に単位距離の２１／２倍だけ移動させた後、指示方向への移動を再試行し、ガード領域が危険空間に進入する場合は、第２方向を基準として第１回転方向を更に４５度回転させた第２回転方向（第２方向基準で１８０度回転させた方向）を、次に移動させる第４候補として設定する。更に、手先を第２回転方向に移動させるとガード領域が安全空間に進入する場合は、第２回転方向に単位距離だけ移動させた後指示方向への移動を再試行し、ガード領域が危険空間に進入する場合は、その時点で移動を停止させる。
【００１１】
このように移動方向を決定すれば、ユーザがリアルタイムで、操作手段を介してロボットの手先の移動方向を指示する操作入力を行うことで手先が危険空間に近付いた場合に、危険空間が連続する境界に沿わせながら、ユーザが指示した方向に極力近づくように手先を移動させることができる。したがって、ユーザがリアルタイムで移動方向を指示する場合でも、ロボットの手先を、安全空間として設定されている領域は全て移動させることが可能となり、空間を限界まで使用することができる。また、ガード領域が危険空間に接した場合は、ユーザが操作手段を介して指示した方向を基準として、手先が危険空間との境界を滑るようにして移動するので、ユーザがイメージした手先の動作に近い動きになり、予想を逸脱するような動きにならず、危険な動作状態が生じることを抑制できる。
【００１２】
請求項２記載のロボットの移動方向決定方法によれば、ガード領域のサイズを変更可能とし、安全空間内において、手先を移動させる速度が上昇するのに応じてガード領域のサイズを段階的に拡大させ、安全空間内において、手先を移動させる速度が下降するのに応じてガード領域のサイズを段階的に縮小させる。これにより、手先の移動速度が速ければガード領域のサイズが大きくなるため狭い安全空間には進入し難くなり、結果としてガード領域；手先はより広く設定されている安全空間に沿って移動することになる。すなわち、手先の移動速度が速い場合とは、作業者が単位時間当たりの手先移動距離を長くすることを所望している状態であり、目標位置の近傍において移動を精密に制御する状態とは異なっている。したがって、手先をより広い安全空間に沿って移動させることで、移動距離をより長く伸ばすことが可能となる。
【００１３】
請求項３記載のロボットの移動方向決定方法によれば、ガード領域のサイズの拡大及び縮小を、前記手先の移動速度の速さに応じて複数段階で行うので、ガード領域のサイズをより広いレンジで変化させることができる。
【００１４】
請求項４記載のロボットの移動方向決定方法によれば、手先を、現在位置が存在する単位空間からｎ個先にある単位空間内にある目標座標まで移動させる際に、ｎ個先の単位空間が安全空間であることを条件に、手先の移動速度により１サンプリングタイムで進む単位空間の数をｎｓとすると、ガード領域のサイズをｎ／ｎｓの値の大きさを基準として決定する。これにより、ガード領域のサイズを手先の移動速度の速さとのバランスに応じて最適に決定することができる。
【００１５】
請求項５記載のロボットの移動方向決定方法によれば、ｎ個先の単位空間が危険空間であれば、ガード領域のサイズを１段階だけ拡大させる。すなわち、手先を危険空間との境界に沿って移動させている状態とは、作業者が単位時間当たりの手先移動距離を長くすることを所望している状態であり、目標位置の近傍において移動を精密に制御する状態とは異なっている。したがって、危険空間との干渉を回避しながら、請求項２と同様に移動距離をより長く伸ばすことが可能となる。
【図面の簡単な説明】
【００１６】
【図１】第１実施例であり、手先の移動方向を決定する処理を示すフローチャート
【図２】手先の動きを２次元的に示した場合の処理手順をイメージ的に示す図
【図３】指示方向を３軸方向ベクトル成分に分解する状態を示す図
【図４】ガード領域Ｇが障害物ＯＢに接近した場合の３次元イメージを示す図
【図５】ロボット本体の手先にガード領域Ｇを設定した状態を示す図
【図６】動作領域空間にキューブを設定した状態を示す図
【図７】垂直多関節型ロボットのシステム構成を示す図
【図８】機能ブロック図
【図９】第２実施例を示す図１の一部相当図
【図１０】ガード領域Ｇのサイズが変化するイメージを示す図
【図１１】第３実施例を示す図１の一部相当図
【図１２】手先の移動とガード領域Ｇの拡大との関係を説明する図
【発明を実施するための形態】
【００１７】
（第１実施例）
以下、第１実施例について図１ないし図８を参照して説明する。図７は、垂直多関節型（６軸）ロボットのシステム構成を示す。このロボット本体１は、ベース（回転軸）２上に、この場合６軸のアームを有している。前記ベース２上には、第１関節Ｊ１を介してショルダ部３が回転可能に連結されている。このショルダ部３には、第２関節Ｊ２を介して上方に延びる下アーム４の下端部が回転可能に連結され、さらに、この下アーム４の先端部には、第３関節Ｊ３を介して第１上アーム５が回転可能に連結されている。
【００１８】
この第１上アーム５の先端には第４関節Ｊ４を介して第２上アーム６が回転可能に連結され、この第２上アーム６の先端には第５関節Ｊ５を介して手首７が回転可能に連結され、この手首７には第６関節Ｊ６を介して第６のフランジ８が回転可能に連結されている。なお、各関節Ｊ１〜Ｊ６においては、サーボモータ２６（図８参照）によりショルダ部３〜フランジ８を回転駆動する。
【００１９】
ロボット本体１と制御装置（コントローラ，ガード領域設定手段，方向決定手段）１１との間は、接続ケーブル１２によって接続されている。これにより、ロボット本体１の各軸を駆動するサーボモータは、制御装置１１により制御される。制御装置１１には、パーソナルコンピュータ（パソコン）１３がケーブル１４を介して接続され、パソコン１３との間で、ロボット本体１の手先の現在位置の送信や動作コマンドの受信など、高速通信を実行するようになっている。パソコン１３には、メモリやハードディスクなどの記憶装置が内蔵されている。
【００２０】
ジョイスティック（操作手段）１５は、ケーブル１６を介してパソコン１３に接続されている。ユーザは、ジョイスティック１５を操作して倒した方向により、ロボット本体１の手先（フランジ８の先端，ＴＣＰ：Tool Center Point）の移動方向を指示する。その指示方向は、パソコン１３を介して制御装置１１に伝達され、制御装置１１は、ロボット本体１の手先をＸＹ平面上で指示された方向に移動させるように制御する。
【００２１】
制御装置１１は、図８に示すように、ＣＰＵ２１と、駆動回路２２と、位置検出回路２３とを備えて構成されている。ＣＰＵ２２には、システムプログラムや動作プログラムを作成するためのロボット言語などを記憶するＲＯＭ２４及び動作プログラムなどを記憶するＲＡＭ２５が接続されていると共に、図示しない通信インターフェイスを介してパソコン１３が接続されている。
【００２２】
位置検出回路２３は、ショルダ部３〜フランジ８の位置を検出するためのもので、各軸の駆動源であるサーボモータ２６に設けられているロータリエンコーダ２７が接続されている。位置検出回路２３は、ロータリエンコーダ２７から入力される検出信号に基づいて各軸の回転角度を検出し、それら検出した位置検出情報をＣＰＵ２１に出力する。そして、ＣＰＵ２１は、動作プログラムに基づいてショルダ部３〜フランジ８を動作させる際に、位置検出回路２３から入力する位置検出情報をフィードバック信号として、各軸のサーボモータ２６を駆動する。
【００２３】
次に、本実施例の作用について図１ないし図６を参照して説明する。本実施例では、図６に示すように、ロボット本体１が動作する領域を、立方体状の単位空間（以下、キューブと称す）を３次元的に積み上げたものとして設定しており、各キューブを構成する６面，或いは１２本の直線を規定する３次元座標値が決定されている。すなわち、制御装置１１のソフトウェア上で、そのように仮想されている。そして、各キューブについては、ロボット本体１が動作した場合に、周辺の障害物等に接触する可能性があるものが「危険」と評価され（危険空間）、接触する可能性がないものが「安全」と評価されている（安全空間）。そして、その情報が空間評価データベースとして、例えばＲＡＭ（記憶手段）２５に予め記憶されている。
【００２４】
一方、ロボット本体１については、図５に示すように、手先を覆うようにガード領域Ｇが設定されている（キューブと同様にソフトウェア上で仮想されている）。例えばガード領域Ｇの中心は手先に設定されている。この場合、ガード領域Ｇの体積（サイズ）は、キューブの体積よりも大きく設定されており、例えば１０倍程度となっている。そして、ロボット本体１を動作させることで、手先であるフランジ８の位置が変化すると、その位置の変化に応じてガード領域Ｇの位置もソフトウェア上で変化する。
【００２５】
図１は、制御装置２が手先の移動方向を決定する処理を示すフローチャートであり、図２は、簡単のため、手先の動きを２次元的に示した場合の処理手順をイメージ的に示す図である。ユーザが、例えばジョイスティック１５を操作して、ロボット本体１の手先の移動方向を指示した場合、その指示方向に手先を移動させると、ガード領域Ｇが危険空間と干渉するか否かを判断する（ステップＳ１）。ここで「干渉」とは、ガード領域Ｇが危険空間の一部に進入する（両者が交錯する）状態を言う。両者が干渉しなければ（ＮＯ）、指示方向に手先を移動させて（ステップＳ１３）ステップＳ１に戻る。また、以降では、上記のように干渉するか否かの判断を「干渉チェック」と称する。
【００２６】
一方、ガード領域Ｇが危険空間と干渉する場合は（ステップＳ１：ＹＥＳ）、干渉が発生する直前の状態で手先を停止させ、指示方向を、図３に示すように、ロボット本体１の基準座標上で原点に立つ単位ベクトルＤと見た場合に、そのベクトルＤを、Ｘ，Ｙ，Ｚ方向成分である各軸方向ベクトルＤｘ，Ｄｙ，Ｄｚに分解する。そして、それらのうち、危険空間のＸＹ平面，ＹＺ平面，ＺＸ平面の何れかと干渉するベクトルを除いた２つの合成ベクトルの方向（上記干渉する面に対して平行となる方向）を、次に手先を移動させる方向の候補（第１候補）とする（ステップＳ２，図２（ａ）（２）参照）。
【００２７】
尚、図２では、ユーザがジョイスティック１５を操作することで、ジョグ動作として指示した方向を破線矢印で示しており、移動候補となる方向を実線矢印で示している。そして、前記の平行な方向に移動させるとした場合について干渉チェックを行い（ステップＳ３）、干渉しなければ（ＮＯ）手先を当該方向に、キューブの１辺の長さである単位距離だけ移動させて（ステップＳ１４）ステップＳ１に戻る。また、両者が干渉する場合は（ＹＥＳ）ステップＳ４に移行する。
【００２８】
ステップＳ４では、指示方向を、ステップＳ２と同様に、ロボット本体１の基準座標上で原点に立つ単位ベクトルＤと見た場合に、そのベクトルＤを、Ｘ，Ｙ，Ｚ方向成分である各軸方向ベクトルＤｘ，Ｄｙ，Ｄｚに分解する。それから、各方向ベクトルの長さを評価して、長さが最も長いベクトルの方向を第１方向，それ以外の２つのベクトルを合成した方向を第２方向とする（ステップＳ５）。図３は、ここでの処理を３次元的なイメージで示す。但し、３つの方向ベクトルＤｘ，Ｄｙ，Ｄｚのうち、何れか２つの長さが等しい場合には、そのうちの何れか一方を選択して第１及び第２方向ベクトルを定める。
【００２９】
また、３つの方向ベクトルＤｘ，Ｄｙ，Ｄｚの、全ての長さが等しい場合は、何れか２つの方向を選択して第１及び第２方向ベクトルを定める。すなわち、上記方向ベクトルの長さはジョイスティック１５の移動方向の近似度を示しているので、長さが等しいということは、その何れの方向に移動させてもジョイスティック１５によって指示された方向に対する近似度は等しくなるからである。
【００３０】
第１及び第２方向を定めると、第１方向が示す方向（第１方向）を、手先を次に移動させる候補として（第２候補，ステップＳ６，図２（ａ）（３）参照）、第１方向に移動させるとした場合の干渉チェックを行う（ステップＳ７）。そして干渉しなければ（ＮＯ）手先を第１方向に単位距離だけ移動させてから（ステップＳ１５）ステップＳ１に戻る。一方、第１方向に移動させると干渉する場合は（ＹＥＳ，図２（ａ）（４）参照）、第１方向を、第２方向を基準として４５度回転させて（第１回転方向，ステップＳ８，図２（ａ）（５）参照）、その第１回転方向について干渉チェックを行う（ステップＳ９）。
【００３１】
図３では、方向ベクトルＤｘが第１方向となり、方向ベクトルＤｙ，Ｄｚの方向を合成したベクトルＤyzの方向が第２方向となる。そして、方向ベクトルＤｘを、合成ベクトルＤyzを基準として４５度回転させた（或いは、ベクトル（−Ｄyz）に向けて４５度回転させた）ベクトルＤｒの方向が、第１回転方向となっている（第２方向から＋１３５度）。ステップＳ９におけるチェックの結果、干渉しない場合は（ＮＯ）手先を第１回転方向に、単位距離の２１／２倍だけ移動させてから（ステップＳ１６）ステップＳ１に戻る。
【００３２】
一方、第１回転方向に移動させると干渉する場合は（ステップＳ９：ＹＥＳ，図２（ａ）（６）参照）、その第１回転方向を更に４５度回転させる（第２回転方向，ステップＳ１０，図２（ａ）（７）参照）。そして、第２回転方向（第２方向から＋１８０度）について干渉チェックを行い（ステップＳ１１）、干渉しない場合は（ＮＯ，図２（ａ）（８）参照）手先を第２回転方向に単位距離だけ移動させてから（ステップＳ１７，図２（ａ）（９）参照）ステップＳ１に戻る。第２回転方向に移動させると干渉する場合は（ステップＳ１１：ＹＥＳ）、この時点で指示方向に基づく移動を停止させてエラー出力を行う（ステップＳ１２）。例えば、指示方向の変更を促す旨のメッセージを、パソコン１３のディスプレイ画面に表示させるなどする。
【００３３】
図２（ａ）（１０）は、（１）と同様に、（９）の状態から再び指示方向への移動を試みた場合を示している。この場合も干渉が発生するので、（３）と同様に方向成分の分解を行う（１１）。そして、第１方向に移動させても干渉が発生しないので、第１方向に単位距離だけ移動させる（１２）。
図２（ｂ）は、危険空間をマクロ的に見た場合に安全空間との境界が斜面となっており、手先を破線矢印方向に移動させるように指示すると、上記斜面にガード領域Ｇが接して干渉する場合である。このような状態で本実施例の処理を適用すると、手先は斜面に沿って移動するようになり、危険空間が存在しなくなった位置で指示方向に移動するようになる。
【００３４】
また、図４は、３次元イメージで示しており、ロボット本体１の動作領域空間に円柱状の障害物ＯＢが存在する場合、その障害物ＯＢは、制御装置２では危険空間であるキューブの集合として把握されている。そして、ロボット本体１の手先の移動方向が図中の矢印方向で指示されている場合に、ガード領域Ｇが障害物ＯＢに接近すると、ガード領域Ｇは、障害物ＯＢの表面に沿って、すなわち危険空間との境界に沿って移動するようになり、障害物ＯＢとの干渉が回避される。
【００３５】
以上のように本実施例によれば、ロボット本体１が動作する動作領域空間をキューブの集合として捉え、各キューブの外面を規定する３次元座標値を決定し、それらの各キューブを、ロボットが通過可能な安全空間と通過不能な危険空間との何れかに評価した空間評価データを用い、キューブよりもサイズが大きな立方体状の空間によって、ロボット本体１の手先部分を覆うガード領域Ｇを設定する。そして、ユーザが、ジョイスティック１５を介して指示した方向に手先を移動させ続けると、ガード領域Ｇと危険空間とが干渉すると予測される場合は両者が接する状態で移動を一旦停止させ、両者が面で接触する場合はその面に平行となる方向を第１候補として、手先を前記方向に移動可能であれば当該方向に単位距離だけ移動させた後指示方向への移動を再試行する。
【００３６】
また、ガード領域Ｇと危険空間とが面で接触しない場合，或いは第１候補の方向に移動させるとガード領域Ｇが干渉する場合は、指示方向を３つの方向ベクトルに成分分解して第１方向，第２方向を定め、第１方向を次に移動させる第２候補とする。そして、手先を第１方向に移動可能であれば、そのまま第１方向に単位距離だけ移動させた後指示方向への移動を再試行し、干渉する場合は、第１方向を４５度回転させた第１回転方向を、次に移動させる第３候補とする。
次に、手先を第１回転方向に移動可能であれば、第１回転方向に単位距離の２１／２倍だけ移動させた後指示方向への移動を再試行し、干渉する場合は、第１回転方向を更に４５度回転させた第２回転方向を次に移動させる第４候補とする。更に、手先を第２回転方向に移動可能であれば、第２回転方向に単位距離だけ移動させた後指示方向への移動を再試行し、干渉する場合はその時点で移動を停止させるようにした。
【００３７】
このように移動方向を決定すれば、ユーザがリアルタイムで、ジョイスティック１５を介してロボット本体１の手先の移動方向を指示する操作入力を行うことで手先が危険空間に近付いた場合に、危険空間が連続する境界に沿わせながら、ユーザが指示した方向に極力近づくように手先を移動させることができる。
したがって、ユーザがリアルタイムで移動方向を指示する場合でも、ロボット本体１の手先を、安全空間として設定されている領域は全て移動させることが可能となり、動作領域空間を限界まで使用することができる。また、ガード領域Ｇが危険空間に接した場合は、指示方向を基準として、手先が危険空間との境界を滑るようにして移動するので、ユーザがイメージした手先の動作に近い動きになり、予想を逸脱するような動きにならず、危険な動作状態が生じることを抑制できる。
【００３８】
（第２実施例）
図９及び図１０は第２実施例であり、第１実施例と同一部分には同一符号を付して説明を省略し、以下異なる部分について説明する。図９は図１の一部相当図であり、ステップＳ１で「ＮＯ」と判断してからステップＳ１３に移行する間に、ステップＳ２１〜Ｓ２５の処理を実行するようになっている。先ず、ステップＳ２１では、ユーザにより設定されたロボットの手先の移動速度をしきい値（１）と比較する。手先の移動速度は、例えばジョイスティック１５を傾けた速さなどで設定される。そして、移動速度がしきい値（１）以下であれば（ＮＯ）、ガード領域Ｇのサイズを設定値（１）とする（ステップＳ２３）。設定値（１）は、デフォルトの最小サイズであり、例えば１辺の長さがキューブ２，３個分程度とする。
【００３９】
ステップＳ２１において、移動速度がしきい値（１）を超えていると（ＹＥＳ）、次に移動速度を、しきい値（２）と比較する（ステップＳ２２）。尚、しきい値（２）は、しきい値（１）よりも高い値に設定されている。そして、移動速度がしきい値（２）以下であれば（ＮＯ）、ガード領域Ｇのサイズを設定値（２）とする（ステップＳ２４）。設定値（２）は、設定値（１）に対して、１辺の長さを例えばキューブ１，２個分程度増加させたサイズとする。例えば、設定値（１）における１辺のサイズがキューブ３個分（３Ｃｂとする）であるとすると、そこから１辺のサイズを４Ｃｂにすればガード領域Ｇの体積増加分は、６４／２７≒２．７３倍となる。
【００４０】
一方、ステップＳ２２において、移動速度がしきい値（２）を超えていると（ＹＥＳ）、ガード領域Ｇのサイズを設定値（３）とする（ステップＳ２５）。設定値（３）は、設定値（２）に対して、更に１辺の長さを例えばキューブ１，２個分程度増加させたサイズとする。尚、ユーザがロボット本体１を直接操作する場合の移動速度は、一般に、自動運転時の１０％〜２０％程度に設定されている。その範囲における最大速度の範囲内でしきい値（１），（２）を設定する。
【００４１】
すなわち、図９に示す処理を実行する毎に、手先の移動速度がしきい値（１），（２）と比較され、その比較結果に応じてガード領域Ｇのサイズが拡大したり縮小したり変化することになる。図１０は、ガード領域Ｇのサイズが変化するイメージを示している。（ａ）は手先の移動速度が低速の場合であり（例えばしきい値（１）以下）、ガード領域Ｇのサイズが小さく設定されるので、より精密な位置制御が可能であり、安全空間の開口部にも進入可能となっている。一方、（ｂ）は手先の移動速度が高速の場合であり（例えばしきい値（２）を上回る）、ガード領域Ｇのサイズが大きく設定されているため、安全空間の開口部には進入不能となっている（図中に破線で示すように、開口部に向かう以外の方向には移動可能である）。
【００４２】
以上のように第２実施例によれば、ガード領域Ｇのサイズを変更可能とし、安全空間内において、手先を移動させる速度が上昇するのに応じてガード領域Ｇのサイズを段階的に拡大させ、安全空間内において、手先を移動させる速度が下降するのに応じてガード領域Ｇのサイズを段階的に縮小させるようにした。これにより、手先の移動速度が速ければガード領域Ｇのサイズが大きくなるため狭い安全空間には進入し難くなり、結果としてガード領域Ｇ；手先はより広く設定されている安全空間に沿って移動することになる。
【００４３】
すなわち、手先の移動速度が速い場合とは、作業者が単位時間当たりの手先移動距離を長くすることを所望している状態であり、目標位置の近傍において移動を精密に制御する状態とは異なっている。したがって、手先をより広い安全空間に沿って移動させることで、移動距離をより長く伸ばすことが可能となる。また、ガード領域Ｇのサイズの拡大及び縮小を、手先の移動速度の速さに応じて複数段階で行うので、ガード領域Ｇのサイズをより広いレンジで変化させることができる。
【００４４】
（第３実施例）
図１１及び図１２は第３実施例であり、第１又は第２実施例と異なる部分のみ説明する。第３実施例では、第２実施例と同様にガード領域Ｇのサイズを手先の移動速度に応じて変化させるが、その手法が異なっている。図１１において処理を開始すると、ステップＳ３０〜Ｓ３５を実行し、その後、ステップＳ１，Ｓ２に替えて、ステップＳ４１〜４４を実行する。この場合、手先の移動目標位置がユーザ（作業者）により予め決定されるので、それに伴い、目標位置の座標が含まれているキューブが現在位置から何個目になるのか（ｎ）を決定する（ステップＳ３０）。続いて「ｎ＝１」か否かを判断し（ステップＳ３１）、「ｎ＝１」であれば（ＹＥＳ）ステップＳ４１に移行する。この場合、ガード領域Ｇのサイズをデフォルトから拡大させる余地はない。また、「ｎ≧２」であれば（ＮＯ）ステップＳ３２に移行する。
【００４５】
ステップＳ３２では、設定されている移動速度が最小値（ＭＩＮ）か否かを判断し、最小値（ＭＩＮ）であれば（ＹＥＳ）ステップＳ４１に移行し（上記と同様に理由による）、最小値でなければ（ＮＯ）ステップＳ３３に移行する。そして、上記移動速度によって、制御装置１１によるロボット本体１の制御周期である１サンプリングタイム（単位時間）Ｔｓにおいて、手先が進むキューブの数ｎｓを計算し、その数ｎｓがステップＳ３０で求められたｎ以上か否かを判断する。（ｎｓ≧ｎ）であれば（ＹＥＳ）、移動速度を最小値に設定してからステップＳ４１に移行する。すなわち（ｎｓ≧ｎ）の場合も、ガード領域Ｇのサイズを拡大させる余地はないからである。
【００４６】
一方、ステップＳ３３において、（ｎｓ＜ｎ）であれば（ＮＯ）ｎ／ｎｓを計算し、その商の値に応じてガード領域Ｇの大きさを設定してから（ステップＳ３５）、ステップＳ４１に移行する。すなわち、ガード領域Ｇの大きさをどのように設定すべきかは、その時点の現在位置から目標位置までの移動距離と、移動速度の速さとに応じて相対的に変化するため、ｎ／ｎｓを指標として用い、最適な大きさを設定する。
【００４７】
例えば、図１２に示すように、ｎ＝２５であり、移動距離並びに移動速度に応じて決まるキューブ数ｎｓ＝５の場合はｎ／ｎｓ＝５となる。手先ＫＴＣＰがｎ＝０の位置にあれば、ｎ個先のキューブに干渉する直前まで４Ｔｓ（４制御周期）の時間があることになる（但し、ガード領域Ｇの一片の大きさが１０Ｃｂ以下であることを条件とする）。そして、ガード領域Ｇの１辺の大きさを拡大させれば上記の到達時間はより短くなるが、初期値が１０Ｃｂ以下であるとすれば増加数を３０Ｃｂ以下にすれば、次の制御周期における移動でガード領域Ｇが危険領域に干渉することは無い。したがって、ｎ／ｎｓを指標として用いることで最適な大きさを設定することができる。
【００４８】
ステップＳ４１では、指示方向にキューブｎ個分移動するか否かを判断し、干渉しなければ（ＮＯ）ステップＳ１３に移行する。また、干渉する場合は（ＹＥＳ）、ガード領域Ｇが拡大可能か否かを判断する（ステップＳ４２）。ここでの判断は、上述したステップＳ３５の場合と同様であり、例えばｎ／ｎｓを指標として用いることで判断すれば良い。そして、拡大可能であれば（ＹＥＳ）１段階だけ拡大させてから（ステップＳ４３）ステップＳ２に替わるステップＳ４４に移行する。また、拡大不能であれば（ＮＯ）そのままステップＳ４４に移行する。ステップＳ４４では、ステップ２と同様の処理を、キューブｎ個分移動した位置について行う。
【００４９】
すなわち、手先を危険空間との境界に沿って移動させている状態は、ユーザがサンプリングタイム当たりの手先移動距離を長くすることを所望している状態であり、目標位置の近傍において移動を精密に制御する状態とは異なっている。したがって、ガード領域Ｇを１段階だけ拡大させて危険空間との干渉を回避しつつ移動距離をより長く伸ばすようにする。
【００５０】
以上のように第３実施例によれば、ロボット本体１の手先を、現在位置が存在するキューブからｎ個先にあるキューブ内にある目標座標まで移動させる際に、ｎ個先のキューブが安全空間であることを条件に、手先の移動速度により１サンプリングタイムで進むキューブ数をｎｓとすると、ガード領域Ｇのサイズをｎ／ｎｓの値の大きさを基準として決定するようにした。これにより、ガード領域Ｇのサイズを手先の移動速度の速さとのバランスに応じて最適に決定できる。また、ｎ個先のキューブが危険空間であれば、ガード領域Ｇのサイズを１段階だけ拡大させるので、危険空間との干渉を回避しつつ移動距離をより長く伸ばすことができる。
【００５１】
本発明は上記し、又は図面に記載した実施例にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
操作手段は、ジョイスティック１５に限ることなく、少なくとも方向を２次元で指示する操作入力が可能な手段であれば良い。
キューブとガード領域とのサイズについては、両者の大小関係を維持する範囲で適宜変更して良い。
ガード領域の拡大，縮小は、少なくとも１段階行えば良く、また、３段階以上に変化させても良い。
【符号の説明】
【００５２】
図面中、１はロボット本体、１１は制御装置（ガード領域設定手段，方向決定手段，領域サイズ変更手段）、１５はジョイスティック（操作手段）、２５はＲＡＭ（記憶手段）を示す。
【特許請求の範囲】
【請求項１】
多関節型のロボットが動作する動作領域空間を、立方体状の単位空間の集合として捉え、各単位空間の外面を規定する３次元座標値を決定し、前記各単位空間を、前記ロボットが通過可能な安全空間と、前記ロボットが通過不能な危険空間との何れかに評価した空間評価データを有し、
前記単位空間よりもサイズが大きな立方体状の空間によって、前記ロボットの手先部分を覆うガード領域を設定し、
ユーザが、操作手段を介して前記ロボットの手先の移動方向を指示する操作入力を行い、前記手先を前記操作入力に応じた指示方向に移動させ続けると、前記ガード領域が危険空間に進入すると予測される場合は、前記ガード領域と前記危険空間とが接する状態で移動を一旦停止させ、
前記ガード領域と前記危険空間とが面で接触する場合は、前記接触する面に平行となる方向を第１候補として、前記手先を前記方向に移動させると前記ガード領域が安全空間に進入する場合は、前記方向に前記単位空間の１辺の距離に相当する単位距離だけ移動させた後、前記指示方向への移動を再試行し、
前記ガード領域と前記危険空間とが面で接触しない場合，又は前記第１候補の方向に移動させると前記ガード領域が危険空間に進入する場合は、前記指示方向を、前記ロボットの基準座標系の原点に立つ単位ベクトルで表わして、前記単位ベクトルの方向を３次元座標上で３つの方向ベクトルに成分分解し、前記３つの方向ベクトルの長さを評価して、長さが最長のベクトルの方向を第１方向，それ以外の２つのベクトルを合成した方向を第２方向すると、前記第１方向を、前記手先を次に移動させる第２候補として設定し、
前記手先を前記第１方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第１方向に前記単位空間の１辺の距離に相当する単位距離だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、前記第２方向を基準として前記第１方向を４５度回転させた第１回転方向を、前記手先を次に移動させる第３候補として設定し、
前記手先を前記第１回転方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第１回転方向に前記単位距離の２１／２倍だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、前記第２方向を基準として前記第１回転方向を更に４５度回転させた第２回転方向を、前記手先を次に移動させる第４候補として設定し、
前記手先を前記第２回転方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第２回転方向に前記単位距離だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、その時点で移動を停止させることを特徴とするロボットの移動方向決定方法。
【請求項２】
前記ガード領域のサイズを変更可能として、
前記安全空間内において、前記手先を移動させる速度が上昇するのに応じて前記ガード領域のサイズを段階的に拡大させ、
前記安全空間内において、前記手先を移動させる速度が下降するのに応じて前記ガード領域のサイズを段階的に縮小させることを特徴とする請求項１記載のロボットの移動方向決定方法。
【請求項３】
前記ガード領域のサイズの拡大及び縮小を、前記手先の移動速度の速さに応じて複数段階で行うことを特徴とする請求項２記載のロボットの移動方向決定方法。
【請求項４】
前記手先を、現在位置が存在する単位空間からｎ（ｎは自然数）個先にある単位空間内にある目標座標まで移動させる際に、前記ｎ個先までの単位空間が安全空間であることを条件として、設定されている前記手先の移動速度により１サンプリングタイムで進む前記単位空間の数をｎｓ（ｎｓは自然数）とすると、前記ガード領域のサイズをｎ／ｎｓの値の大きさを基準として決定することを特徴とする請求項２又は３記載のロボットの移動方向決定方法。
【請求項５】
前記ｎ個先の単位空間が危険空間であれば、前記ガード領域のサイズを１段階だけ拡大させることを特徴とする請求項４記載のロボットの移動方向決定方法。
【請求項６】
多関節型のロボットが動作する動作領域空間を、立方体状の単位空間の集合として捉え、各単位空間の外面を規定する３次元座標値を決定し、前記各単位空間を、前記ロボットが通過可能な安全空間と、前記ロボットが通過不能な危険空間との何れかに評価した空間評価データが記憶される記憶手段と、
前記単位空間よりもサイズが大きな立方体状の空間によって、前記ロボットの手先部分を覆うガード領域を設定するガード領域設定手段と、
ユーザが、操作手段を介して前記ロボットの手先の移動方向を指示する操作入力を行い、前記手先を前記操作入力に応じた指示方向に移動させ続けると、前記ガード領域が危険空間に進入すると予測される場合は、前記ガード領域と前記危険空間とが接する状態で移動を一旦停止させ、
前記ガード領域と前記危険空間とが面で接触する場合は、前記接触する面に平行となる方向を第１候補として、前記手先を前記方向に移動させると前記ガード領域が安全空間に進入する場合は、前記方向に前記単位空間の１辺の距離に相当する単位距離だけ移動させた後、前記指示方向への移動を再試行し、
前記ガード領域と前記危険空間とが面で接触しない場合，又は前記第１候補の方向に移動させると前記ガード領域が危険空間に進入する場合は、前記指示方向を、前記ロボットの基準座標系の原点に立つ単位ベクトルで表わして、前記単位ベクトルの方向を３次元座標上で３つの方向ベクトルに成分分解し、前記３つの方向ベクトルの長さを評価して、長さが最長のベクトルの方向を第１方向，それ以外の２つのベクトルを合成した方向を第２方向すると、前記第１方向を、前記手先を次に移動させる第２候補として設定し、
前記手先を前記第１方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第１方向に前記単位空間の１辺の距離に相当する単位距離だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、前記第２方向を基準として前記第１方向を４５度回転させた第１回転方向を、前記手先を次に移動させる第３候補として設定し、
前記手先を前記第１回転方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第１回転方向に前記単位距離の２１／２倍だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、前記第２方向を基準として前記第１回転方向を更に４５度回転させた第２回転方向を、前記手先を次に移動させる第４候補として設定し、
前記手先を前記第２回転方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第２回転方向に前記単位距離だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、その時点で移動を停止させる方向決定手段とを備えたことを特徴とするロボットの制御装置。
【請求項７】
前記ガード領域設定手段は、前記ガード領域のサイズを変更可能に構成され、
前記安全空間内において、前記手先を移動させる速度が上昇するのに応じて前記ガード領域のサイズを段階的に拡大させ、
前記安全空間内において、前記手先を移動させる速度が下降するのに応じて前記ガード領域のサイズを段階的に縮小させることを特徴とする請求項６記載のロボットの制御装置。
【請求項８】
前記ガード領域設定手段は、前記ガード領域のサイズの拡大及び縮小を、前記手先の移動速度の速さに応じて複数段階で行うことを特徴とする請求項７記載のロボットの制御装置。
【請求項９】
前記手先を、現在位置が存在する単位空間からｎ（ｎは自然数）個先にある単位空間内にある目標座標まで移動させる際に、
前記ガード領域設定手段は、前記ｎ個先までの単位空間が安全空間であることを条件として、設定されている前記手先の移動速度により１サンプリングタイムで進む前記単位空間の数をｎｓ（ｎｓは自然数）とすると、前記ガード領域のサイズをｎ／ｎｓの値の大きさを基準として決定することを特徴とする請求項７又は８記載のロボットの制御装置。
【請求項１０】
前記ガード領域設定手段は、前記ｎ個先の単位空間が危険空間であれば、前記ガード領域のサイズを１段階だけ拡大させることを特徴とする請求項９記載のロボットの制御装置。
【請求項１】
多関節型のロボットが動作する動作領域空間を、立方体状の単位空間の集合として捉え、各単位空間の外面を規定する３次元座標値を決定し、前記各単位空間を、前記ロボットが通過可能な安全空間と、前記ロボットが通過不能な危険空間との何れかに評価した空間評価データを有し、
前記単位空間よりもサイズが大きな立方体状の空間によって、前記ロボットの手先部分を覆うガード領域を設定し、
ユーザが、操作手段を介して前記ロボットの手先の移動方向を指示する操作入力を行い、前記手先を前記操作入力に応じた指示方向に移動させ続けると、前記ガード領域が危険空間に進入すると予測される場合は、前記ガード領域と前記危険空間とが接する状態で移動を一旦停止させ、
前記ガード領域と前記危険空間とが面で接触する場合は、前記接触する面に平行となる方向を第１候補として、前記手先を前記方向に移動させると前記ガード領域が安全空間に進入する場合は、前記方向に前記単位空間の１辺の距離に相当する単位距離だけ移動させた後、前記指示方向への移動を再試行し、
前記ガード領域と前記危険空間とが面で接触しない場合，又は前記第１候補の方向に移動させると前記ガード領域が危険空間に進入する場合は、前記指示方向を、前記ロボットの基準座標系の原点に立つ単位ベクトルで表わして、前記単位ベクトルの方向を３次元座標上で３つの方向ベクトルに成分分解し、前記３つの方向ベクトルの長さを評価して、長さが最長のベクトルの方向を第１方向，それ以外の２つのベクトルを合成した方向を第２方向すると、前記第１方向を、前記手先を次に移動させる第２候補として設定し、
前記手先を前記第１方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第１方向に前記単位空間の１辺の距離に相当する単位距離だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、前記第２方向を基準として前記第１方向を４５度回転させた第１回転方向を、前記手先を次に移動させる第３候補として設定し、
前記手先を前記第１回転方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第１回転方向に前記単位距離の２１／２倍だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、前記第２方向を基準として前記第１回転方向を更に４５度回転させた第２回転方向を、前記手先を次に移動させる第４候補として設定し、
前記手先を前記第２回転方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第２回転方向に前記単位距離だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、その時点で移動を停止させることを特徴とするロボットの移動方向決定方法。
【請求項２】
前記ガード領域のサイズを変更可能として、
前記安全空間内において、前記手先を移動させる速度が上昇するのに応じて前記ガード領域のサイズを段階的に拡大させ、
前記安全空間内において、前記手先を移動させる速度が下降するのに応じて前記ガード領域のサイズを段階的に縮小させることを特徴とする請求項１記載のロボットの移動方向決定方法。
【請求項３】
前記ガード領域のサイズの拡大及び縮小を、前記手先の移動速度の速さに応じて複数段階で行うことを特徴とする請求項２記載のロボットの移動方向決定方法。
【請求項４】
前記手先を、現在位置が存在する単位空間からｎ（ｎは自然数）個先にある単位空間内にある目標座標まで移動させる際に、前記ｎ個先までの単位空間が安全空間であることを条件として、設定されている前記手先の移動速度により１サンプリングタイムで進む前記単位空間の数をｎｓ（ｎｓは自然数）とすると、前記ガード領域のサイズをｎ／ｎｓの値の大きさを基準として決定することを特徴とする請求項２又は３記載のロボットの移動方向決定方法。
【請求項５】
前記ｎ個先の単位空間が危険空間であれば、前記ガード領域のサイズを１段階だけ拡大させることを特徴とする請求項４記載のロボットの移動方向決定方法。
【請求項６】
多関節型のロボットが動作する動作領域空間を、立方体状の単位空間の集合として捉え、各単位空間の外面を規定する３次元座標値を決定し、前記各単位空間を、前記ロボットが通過可能な安全空間と、前記ロボットが通過不能な危険空間との何れかに評価した空間評価データが記憶される記憶手段と、
前記単位空間よりもサイズが大きな立方体状の空間によって、前記ロボットの手先部分を覆うガード領域を設定するガード領域設定手段と、
ユーザが、操作手段を介して前記ロボットの手先の移動方向を指示する操作入力を行い、前記手先を前記操作入力に応じた指示方向に移動させ続けると、前記ガード領域が危険空間に進入すると予測される場合は、前記ガード領域と前記危険空間とが接する状態で移動を一旦停止させ、
前記ガード領域と前記危険空間とが面で接触する場合は、前記接触する面に平行となる方向を第１候補として、前記手先を前記方向に移動させると前記ガード領域が安全空間に進入する場合は、前記方向に前記単位空間の１辺の距離に相当する単位距離だけ移動させた後、前記指示方向への移動を再試行し、
前記ガード領域と前記危険空間とが面で接触しない場合，又は前記第１候補の方向に移動させると前記ガード領域が危険空間に進入する場合は、前記指示方向を、前記ロボットの基準座標系の原点に立つ単位ベクトルで表わして、前記単位ベクトルの方向を３次元座標上で３つの方向ベクトルに成分分解し、前記３つの方向ベクトルの長さを評価して、長さが最長のベクトルの方向を第１方向，それ以外の２つのベクトルを合成した方向を第２方向すると、前記第１方向を、前記手先を次に移動させる第２候補として設定し、
前記手先を前記第１方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第１方向に前記単位空間の１辺の距離に相当する単位距離だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、前記第２方向を基準として前記第１方向を４５度回転させた第１回転方向を、前記手先を次に移動させる第３候補として設定し、
前記手先を前記第１回転方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第１回転方向に前記単位距離の２１／２倍だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、前記第２方向を基準として前記第１回転方向を更に４５度回転させた第２回転方向を、前記手先を次に移動させる第４候補として設定し、
前記手先を前記第２回転方向に移動させると前記ガード領域が前記安全空間に進入する場合は、前記第２回転方向に前記単位距離だけ移動させた後、前記指示方向への移動を再試行し、前記ガード領域が危険空間に進入する場合は、その時点で移動を停止させる方向決定手段とを備えたことを特徴とするロボットの制御装置。
【請求項７】
前記ガード領域設定手段は、前記ガード領域のサイズを変更可能に構成され、
前記安全空間内において、前記手先を移動させる速度が上昇するのに応じて前記ガード領域のサイズを段階的に拡大させ、
前記安全空間内において、前記手先を移動させる速度が下降するのに応じて前記ガード領域のサイズを段階的に縮小させることを特徴とする請求項６記載のロボットの制御装置。
【請求項８】
前記ガード領域設定手段は、前記ガード領域のサイズの拡大及び縮小を、前記手先の移動速度の速さに応じて複数段階で行うことを特徴とする請求項７記載のロボットの制御装置。
【請求項９】
前記手先を、現在位置が存在する単位空間からｎ（ｎは自然数）個先にある単位空間内にある目標座標まで移動させる際に、
前記ガード領域設定手段は、前記ｎ個先までの単位空間が安全空間であることを条件として、設定されている前記手先の移動速度により１サンプリングタイムで進む前記単位空間の数をｎｓ（ｎｓは自然数）とすると、前記ガード領域のサイズをｎ／ｎｓの値の大きさを基準として決定することを特徴とする請求項７又は８記載のロボットの制御装置。
【請求項１０】
前記ガード領域設定手段は、前記ｎ個先の単位空間が危険空間であれば、前記ガード領域のサイズを１段階だけ拡大させることを特徴とする請求項９記載のロボットの制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【公開番号】特開２０１２−８１５７７（Ｐ２０１２−８１５７７Ａ）
【公開日】平成２４年４月２６日（２０１２．４．２６）
【国際特許分類】
【出願番号】特願２０１１−６０６２５（Ｐ２０１１−６０６２５）
【出願日】平成２３年３月１８日（２０１１．３．１８）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
【公開日】平成２４年４月２６日（２０１２．４．２６）
【国際特許分類】
【出願日】平成２３年３月１８日（２０１１．３．１８）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
[ Back to top ]