経路生成装置および経路生成方法
【課題】Ｃ空間における過剰な計算を排除して、動作経路計画工数を低減する。
【解決手段】初期位置記憶手段と、目標位置記憶手段と、現在位置検出手段と、経路記憶手段と、現在位置隣接コンフィギュレーション点を求める第１候補判断手段と、現在位置隣接コンフィギュレーション点のうち障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求める第２候補判断手段と、現在位置隣接コンフィギュレーション点が干渉点であるかどうかを判断する干渉チェック手段と、現在位置隣接コンフィギュレーション点のうち現在位置から目標コンフィギュレーション点に近づくようにコンフィギュレーション点を選定する距離候補判断手段と、を備え、現在位置において前記距離候補判断手段により選定された距離候補コンフィギュレーション点が干渉点でない場合には、直進モード経路を生成し、干渉点である場合には境界追従モード経路を生成する。
【解決手段】初期位置記憶手段と、目標位置記憶手段と、現在位置検出手段と、経路記憶手段と、現在位置隣接コンフィギュレーション点を求める第１候補判断手段と、現在位置隣接コンフィギュレーション点のうち障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求める第２候補判断手段と、現在位置隣接コンフィギュレーション点が干渉点であるかどうかを判断する干渉チェック手段と、現在位置隣接コンフィギュレーション点のうち現在位置から目標コンフィギュレーション点に近づくようにコンフィギュレーション点を選定する距離候補判断手段と、を備え、現在位置において前記距離候補判断手段により選定された距離候補コンフィギュレーション点が干渉点でない場合には、直進モード経路を生成し、干渉点である場合には境界追従モード経路を生成する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はロボットアームの経路生成装置および経路生成方法に係り、特にコンフィギュレーション空間を用いて障害物を回避する動作計画を行うロボットアームの経路生成装置および経路生成方法に関する。
【背景技術】
【０００２】
一般的に、原子炉など、人の進入が困難な場所において作業を行う場合、ＣＣＤカメラ、距離センサ、接触センサなどを組み込んで遠隔制御を行い、炉内の点検、形状の計測、補修作業などを行うといったマスタ・スレーブ方式が採用されるケースが多い。
特に、原子炉内において、動作経路選択を自動制御することのできる多関節ロボットを用いた作業が行われており、そうした自動制御および経路生成を実現する手法として、コンフィギュレーション空間を用いた動作経路生成法、迷路探索法、ＲＲＴ（Ｒａｐｉｄｌｙ−ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）法などの確率的手法を用いた動作経路生成法が開示されている（特許文献１〜３）。
【０００３】
コンフィギュレーション空間（Ｃスペース）は、マニュピレータ（ロボットアーム）の自由度の数と同数の次元数を持つ仮想空間であり、このＣスペース上では、ロボットアームのコンフィギュレーション（姿勢）は、ロボットアームの自由度を表すパラメータを座標軸の値とした場合、点（コンフィギュレーション点）で表すことができる。
例えば、ロボットアームのすべてのコンフィギュレーションについて障害物との干渉の有無を調査してロボットアームが障害物と干渉しない空間（自由空間）をＣスペース上に構築し、その中でロボットアームの動作経路を生成する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平４−２３５６０６号公報
【特許文献２】特開２００８−１０５１３２号公報
【特許文献３】特開２０１０−３２３２６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、Ｃスペースを用いた経路生成方法は、ロボットアームの自由度が増えると、計算量や必要な記憶容量が膨大となり、多自由度ロボットへの適用は非現実的であるといった課題があった。
【０００６】
さらに、上記Ｃスペースを用いた経路生成方法に対する解決策として、Ｃスペース上の初期コンフィギュレーション点と目標コンフィギュレーション点から波面を広げるようにＣスペースを構築することで計算量を削減し、２つの波面を重ねた自由空間で動作生成する方法も開示されているが、波面を全方向に一様に膨張させるため、余分な自由空間の構築を行うといった課題があった（特許文献１）。
【０００７】
また、迷路探索法を用いた経路生成方法は、Ｃスペース上の初期コンフィギュレーション点と目標コンフィギュレーション点を直線で結び、その直線およびその直線と交わるＣスペース上の障害物の境界をたどることによって経路を生成するが、２次元迷路を前提としており、３次元以上のＣスペースにおける障害物の境界は面や立体で現れるため、経路決定ができないといった課題があった。
【０００８】
また、ＲＲＴ法を用いた経路生成方法は、ランダムに選んだコンフィギュレーション点に向けて一定距離だけ進んだコンフィギュレーション点を経路探索木に取り入れて探索木を成長させ、探索木に取り入れたコンフィギュレーション点をたどることによって経路を生成するが、乱数を用いて自由空間を構築するため、計算時間の推定が困難であるといった課題があった。
【０００９】
本発明は、このような背景に鑑みてなされたものであり、コンフィギュレーション空間における過剰な計算を排除して、ロボットアームの動作計画工数を低減することができる経路生成装置および経路生成方法を提供することを課題とする。
【課題を解決するための手段】
【００１０】
本発明の請求項１に係る発明は、演算処理装置および記憶装置からなる制御装置を有し、初期コンフィギュレーション点から目標コンフィギュレーション点まで障害物を回避するように新コンフィギュレーション点を採択してロボットアームの動作経路を生成するコンフィギュレーション空間を用いた経路生成装置であって、前記制御装置は、前記初期コンフィギュレーション点を格納する初期位置記憶手段と、前記目標コンフィギュレーション点を格納する目標位置記憶手段と、前記ロボットアームの現在位置を検出する現在位置検出手段と、前記初期コンフィギュレーション点から現在位置までの経路を構成する各コンフィギュレーション点を格納する経路記憶手段と、前記動作経路における現在位置に隣接する現在位置隣接コンフィギュレーション点を求める第１候補判断手段と、前記現在位置隣接コンフィギュレーション点のうち前記障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求める第２候補判断手段と、前記現在位置隣接コンフィギュレーション点が前記干渉点であるかどうかを判断する干渉チェック手段と、前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点に近づくように距離候補コンフィギュレーション点を選定する距離候補判断手段と、を備え、前記動作経路における現在位置において前記距離候補判断手段により選定された距離候補コンフィギュレーション点が前記干渉チェック手段により前記干渉点でないと判断された場合には、当該距離候補コンフィギュレーション点を前記新コンフィギュレーション点として採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む直進モード経路を生成し、前記動作経路における現在位置において前記距離候補判断手段により選定された距離候補コンフィギュレーション点が前記干渉チェック手段により前記干渉点であると判断された場合には、前記第２候補判断手段により求めた前記干渉点隣接コンフィギュレーション点のうち、前記距離候補判断手段により現在位置から前記目標コンフィギュレーション点に近づくように前記新コンフィギュレーション点を採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む境界追従モード経路を生成し、このようにして、前記直進モード経路および前記境界追従モード経路を逐次生成しながら前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することを特徴とする。
【００１１】
本発明は、第１候補判断手段および第２候補判断手段により、現在位置隣接コンフィギュレーション点のうち前記障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求めることで、ロボットアームの自由度に対応したすべてのコンフィギュレーション点について干渉の有無を判断するのではなく、コンフィギュレーション空間（Ｃスペース）の構築をロボットアームの現在位置および干渉点の周囲に限定して部分的なものに留めるので、Ｃスペースにおける過剰な計算を排除することができる。
このようにして、本発明は、確率的手法によらずにＣスペースの構築を部分的なものに限定することで、動作計画工数を効果的に低減することができる。
【００１２】
そして、前記動作経路における現在位置において距離候補コンフィギュレーション点が前記干渉点でないと判断された場合には直進モード経路を生成することで、干渉点に隣接するまでは目標コンフィギュレーション点に迅速に到達することができるため、無駄な動作を低減してロボットアームの効率的な動作経路を生成することができる。
【００１３】
また、距離候補コンフィギュレーション点が前記干渉点であると判断された場合には、境界追従モード経路を生成することで、障害物の境界を辿る方向に進む経路を採択するため、狭いスペースでも障害物との干渉領域を確実に回避しながら動作経路を生成することが可能である。
【００１４】
以上より、本発明に係る経路生成装置は、前記直進モード経路および前記境界追従モード経路を逐次生成しながら、前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することで、障害物との干渉領域を確実に回避しながら、Ｃスペースにおける過剰な計算を排除して、ロボットアームの動作計画工数を低減することができる。
【００１５】
このため、本発明に係る経路生成装置は、自由度が高い多関節ロボットであっても好適に適用することができる。例えば、安全性が重視される原子力関係の機器等におけるシュラウドや複数の配管が並存するような狭あいな作業空間の中で多関節ロボットアームの動作経路を生成するような場合には、干渉領域を確実に回避することができる動作経路を迅速にシミュレーションすることが可能であるため、経路生成ツールとしての高い利用価値が期待される。
【００１６】
請求項２に係る発明は、請求項１に記載の経路生成装置であって、前記距離候補判断手段は、前記仮想直線経路に対して最も近づくようにコンフィギュレーション点を選定することを特徴とする。
【００１７】
かかる構成によれば、前記距離候補判断手段は、前記仮想直線経路に対して最も近づくようにコンフィギュレーション点を選定することで、より狭いスペースでも障害物との干渉領域を確実に回避しながら動作経路を生成することが可能となる。
【００１８】
請求項３に係る発明は、請求項１または請求項２に記載の経路生成装置であって、前記現在位置隣接コンフィギュレーション点のうち現在位置までに通過した経路を構成する前記経路記憶手段に格納された経路構成コンフィギュレーション点を参照して前記新コンフィギュレーション点として採択しないように除外するデッドロック回避手段をさらに備えたことを特徴とする。
【００１９】
かかる構成によれば、現在位置までに遭遇したコンフィギュレーション点を除外して、常に新しいコンフィギュレーション点について経路探索が行われるため、経路を逆戻りして同じ経路を往復したり、同じ経路を周回したりしてデッドロックに陥ることを回避することができる。
【００２０】
請求項４に係る発明は、初期コンフィギュレーション点から目標コンフィギュレーション点まで障害物を回避するように新コンフィギュレーション点を採択してロボットアームの動作経路を生成するコンフィギュレーション空間を用いた迷路探索法に基づく経路生成方法であって、演算処理装置および記憶装置からなる制御装置を有する経路生成装置を使用して、前記演算処理装置を構成する第１候補判断手段により、前記動作経路における現在位置に隣接する現在位置隣接コンフィギュレーション点を求めるＡステップと、前記演算処理装置を構成する第２候補判断手段により、前記現在位置隣接コンフィギュレーション点のうち前記障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求めるＢステップと、前記動作経路における現在位置において、前記演算処理装置を構成する距離候補判断手段により前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点までの仮想直線経路に対して近づくように選定された距離候補コンフィギュレーション点が、前記演算処理装置を構成する干渉チェック手段により前記障害物の干渉点でないと判断された場合には、当該距離候補コンフィギュレーション点を前記新コンフィギュレーション点として採択して経路記憶手段に格納し当該新コンフィギュレーション点まで進む直進モード経路を生成するＣステップと、前記動作経路における現在位置において、前記演算処理装置を構成する距離候補判断手段により前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点までの仮想直線経路に対して近づくように選定された距離候補コンフィギュレーション点が、前記干渉チェック手段により前記障害物の干渉点であると判断された場合には、前記第２候補判断手段により求めた前記干渉点隣接コンフィギュレーション点のうち、前記距離候補判断手段により現在位置から前記目標コンフィギュレーション点に近づくように前記新コンフィギュレーション点を採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む境界追従モード経路を生成するＤステップと、を含み、前記Ｃステップにおける直進モード経路および前記Ｄステップにおける境界追従モード経路を逐次生成しながら前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することを特徴とする。
【００２１】
かかる構成によれば、前記直進モード経路および前記境界追従モード経路を逐次生成しながら前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することで、Ｃスペースにおける過剰な計算を回避して、多関節ロボットにおける動作計画工数を低減することができる効率的な動作経路を生成することが可能となる。
【００２２】
請求項５に係る発明は、請求項４に記載の経路生成方法であって、前記距離候補判断手段は、前記仮想直線経路に対して最も近づくようにコンフィギュレーション点を選定することを特徴とする。
【００２３】
かかる構成によれば、前記距離候補判断手段は、前記仮想直線経路に対して最も近づくようにコンフィギュレーション点を選定することで、より狭いスペースでも障害物との干渉点を確実に回避しながら動作経路を生成することが可能となる。
【発明の効果】
【００２４】
本発明によれば、コンフィギュレーション空間における過剰な計算を排除して、ロボットアームにおける動作計画工数を低減することができる経路生成装置および経路生成方法を提供することができる。したがって、本発明は、自由度が高い多関節ロボットであっても円滑に適用できることから、原子力分野等における狭あいな作業空間の中で複雑な動作をさせたい場合等において特に好適である。
【図面の簡単な説明】
【００２５】
【図１】ロボットアームにおける６軸構成を示す斜視図である。
【図２】２軸のロボットアームにおけるロボットアームの姿勢と障害物との関係を示す図であり、（ａ）は実空間における関係、（ｂ）はコンフィギュレーション空間における関係を示す。
【図３】本発明の実施形態に係る経路生成装置の構成を示すブロック図である。
【図４】Ｃスペースにおける凸形状の干渉領域を想定した場合における経路生成の具体的な手法を説明するための図であり、（ａ）は直進モード経路、（ｂ）は境界追従モード経路、（ｃ）は直進モード経路である。
【図５】Ｃスペースにおける凹形状の障害物を想定した場合におけるデッドロック回避手段の作用を説明するための図である。
【図６】本発明の実施形態に係る経路生成装置の動作を説明するための流れ図である。
【図７】ロボットアームの初期位置から目標位置までの動作のイメージを示す斜視図であり、（ａ）は初期位置、（ｂ）は目標位置、（ｃ）は初期位置から目標位置までの動作のイメージを示す。
【発明を実施するための形態】
【００２６】
本発明の実施形態に係る経路生成装置１（図３参照）について、適宜図を参照しながら詳細に説明する。経路生成装置１は、コンフィギュレーション空間（Ｃスペース）を用いて、６軸を有するロボットアーム１００（図１参照）の動作経路を生成する場合を例として説明する。なお、ロボットアーム１００は、一般的にはロボットアーム自体が移動するような移動体として広く観念されるものであればよく、特に限定して解釈されるものではない。
【００２７】
ロボットアーム１００は、図１に示すように、６つの関節（６つの自由度）からなる６軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６を備えている。
ロボットアームの自由度を表すパラメータを関節角度として、ロボットアーム１００の６軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６の関節角度をそれぞれθ１，θ２，θ３，θ４，θ５，θ６とすれば、ロボットアーム１００のコンフィギュレーション（姿勢）はθ１，θ２，θ３，θ４，θ５，θ６で表すことができる。
【００２８】
このように、ロボットアーム１００の自由度を表すパラメータ（例えば、関節角度や伸縮長さ）を座標軸の値とした場合、ロボットアーム１００のコンフィギュレーション点はＣスペース上の点として表現することができる。
【００２９】
例えば、図２に示すように、簡略化のために例えば、実空間において関節角度をθｘとθｙで表すことができる２軸（θｘ，θｙ）のロボットアーム１００′（図２（ａ）参照）について、横軸にθｘをとり縦軸にθｙをとれば、Ｃスペース上において、ロボットアーム１００′のコンフィギュレーション（姿勢）をコンフィギュレーション点（θｘ，θｙ）して表すことができる。
【００３０】
一方、ロボットアーム１００′が作業空間中の障害物Ｏｂ（Ｏｂｓｔａｃｌｅ）と干渉（衝突）するコンフィギュレーション点を干渉点と定義する。干渉点が単独で存在することはまれであり、一般に連続した点の集合である干渉領域ＣＡ（Ｃｏｌｌｉｓｉｏｎ Ａｒｅａ）としてＣスペース上に表すことができる（図２（ｂ）参照）。
このようにして、図２（ｂ）に示すように、Ｃスペース上において、ロボットアーム１００′のコンフィギュレーションと干渉領域ＣＡを表すことで、動作経路におけるロボットアームと障害物との干渉をチェックすることができる。
【００３１】
なお、図２では２軸（θｘ，θｙ）について説明したが、計算量が異なるとしても同じ考え方でロボットアーム１００（図１）のような６軸（θ１，θ２，θ３，θ４，θ５，θ６）に発展させて処理することも可能である。このため、図４や図５に示すように、２軸のみを平面的に示す方が理解しやすいので、便宜上簡略化したものである。
【００３２】
経路生成装置１は、図３と図４に示すように、初期コンフィギュレーション点ｑｉから目標コンフィギュレーション点ｑｇまで干渉領域ＣＡを回避するように逐次新コンフィギュレーション点ｑｎ（例えば、図５（ａ）参照）を採択してロボットアーム１００の動作経路を生成する装置である（図４、図７参照）。
ここで、Ｃスペースを用いた動作計画とは、図４に示すように、Ｃスペース上において干渉領域ＣＡを避けて初期コンフィギュレーション点ｑｉ（ｉｎｉｔｉａｌ）と目標コンフィギュレーション点ｑｇ（ｇｏａｌ）を接続する経路を生成し、実空間における経路に置き換えて動作経路を探索する手法である。
【００３３】
経路生成装置１は、図３に示すように、作業者（不図示）がデータ入力等を行う操作盤２と、図示しないＣＰＵ等からなる演算処理装置３と、種々のデータを格納する記憶装置４と、を備えている。
操作盤２には、ディスプレイ２１が配設され、ロボットアーム１００や作業空間を３Ｄ（３次元）グラフィックス表示して、作業者（不図示）が生成された動作経路を視認できるようになっている。
【００３４】
演算処理装置３は、図３と図４に示すように、現在位置検出手段３１と、現在位置ｑｐに隣接する現在位置隣接コンフィギュレーション点ｑｃを求める第１候補判断手段３２と、干渉点に隣接する干渉点隣接コンフィギュレーション点ｑｃａを求める第２候補判断手段３３と、干渉点であるかどうかを判断する干渉チェック手段３４と、現在位置ｑｐから目標コンフィギュレーション点ｑｇに近づくように距離候補コンフィギュレーション点を選定する距離候補判断手段３５と、現在位置ｑｐまでに通過したコンフィギュレーション点を除外するデッドロック回避手段３６と、を備えている。
【００３５】
記憶装置４は、初期位置記憶手段４１と、目標位置記憶手段４２と、ロボットアーム１００（図１）の形状データを格納するロボット形状記憶手段４３と、作業空間の形状データを格納する作業空間記憶手段４４と、現在位置ｑｐまでの経路を格納する経路記憶手段４５と、発見した干渉点を格納する干渉点記憶手段４６と、を備えている。
【００３６】
以下、経路生成装置１を構成する具体的手段、およびその作用効果について、図３と図４を参照しながら経路生成装置１の動作の順に沿って説明する。
参照する図４において、細線で表示された図の縦方向のグリッド線をａ〜ｌまで、横方向のグリッド線を１〜１１まで符号をつけて示し、それらのグリッド線の交点を格子点といい（ａ，１）〜（ｌ，１１）のように表示する。ただし、グリッド線の間隔等の表示は模式的なものであり縦横の比率等は簡略化したものである。
【００３７】
格子点は、Ｃスペースを細かく分割して、離散化されたコンフィギュレーション点の集合として扱うための概念である。このように、Ｃスペースを連続した空間として認識するのではなく、格子点を基準として離散化（量子化）し、この量子化された格子点を通るように新コンフィギュレーション点を採択しながら動作経路を生成することで、Ｃスペース上の演算処理を簡略化することができる。
例えば、ロボットアーム１００の根元部分の関節角度θ１〜θ３を１度刻みにして、手先部分の関節角度θ４〜θ６を５度刻みにしてＣスペースにおける格子点を設定し、この格子点を基準にして離散化されたＣスペースを構築することができる。
【００３８】
初期位置記憶手段４１は、初期コンフィギュレーション点ｑｉ（図４参照）であるロボットアーム１００の初期姿勢（実空間では図７（ａ）参照）を各関節の関節角度等により記憶する装置であり、操作盤２から作業者（不図示）が入力したり、演算処理装置３により初期位置を角度センサ等の検出器で検出して初期コンフィギュレーション点ｑｉとして設定したりすることができる。
例えば、初期コンフィギュレーション点ｑｉは、ｑｉ＝（θ１，θ２，θ３，θ４，θ５，θ６）＝（−１７０，９０，０，２５、−８５，７５）のように関節角度をパラメータとして表すことができる。
【００３９】
目標位置記憶手段４２は、目標コンフィギュレーション点ｑｇであるロボットアーム１００の目標姿勢（実空間では図７（ｂ）参照）を各関節の関節角度等により記憶する手段であり、操作盤２から作業者（不図示）が入力したり、演算処理装置３により目標位置を角度センサ等の検出器で検出して目標コンフィギュレーション点ｑｇとして設定したりすることができる。
例えば、目標コンフィギュレーション点ｑｇは、ｑｇ＝（θ１，θ２，θ３，θ４，θ５，θ６）＝（−７１，−７５，３８，８０，−１０，２５）のように関節角度をパラメータとして表すことができる。
【００４０】
ロボット形状記憶手段４３は、ＣＡＤファイル等から作成したロボットアーム１００の形状データを格納するが、データ処理を簡略化するため、例えば、ロボットアーム１００の形状データをポリゴンと呼ばれる多角形等の最小構成単位（例えば三角形）の集合で表して、この多角形の集合をＣスペース上の形状として表現して記憶することができる。
かかる構成により、実空間におけるロボットアーム１００の形状データを三角形等のポリゴンの集合として表して、Ｃスペース上の形状として認識することで、Ｃスペース上におけるロボットアーム１００の形状データを簡略化（離散化）して演算処理を短縮化することができる。
【００４１】
作業空間記憶手段４４は、障害物Ｏｂ（図７参照）が配置された作業空間の形状データを格納するが、データ処理を簡略化するため、例えば、障害物Ｏｂの形状データをポリゴン（例えば三角形等の多角形）の集合で表して、この三角形等の集合をＣスペース上の形状として表現して記憶することができる。
かかる構成により、Ｃスペース上における作業空間の形状データを簡略化（離散化）して演算処理を短縮化することができる。
【００４２】
現在位置検出手段３１は、例えば、ロボットアーム１００（図１参照）の６軸の関節角度（θ１，θ２，θ３，θ４，θ５，θ６）を角度センサ等により計測して、Ｃスペース上の探索経路における現在位置ｑｐを検出する装置である。
現在位置ｑｐは、経路探索中は現在位置ｑｐから新たに採択された新コンフィギュレーション点ｑｎ（例えば、図５（ａ）参照）を生成するごとに更新されるが、現在位置ｑｐの位置データは経路記憶手段４５に格納されている。
【００４３】
経路記憶手段４５は、初期コンフィギュレーション点ｑｉから現在位置ｑｐまでの経路を構成する経路構成コンフィギュレーション点を格納する記憶装置である。
例えば、経路記憶手段４５は、図４（ｂ）に示す現在位置ｑｐ（ｇ，４）では、現在位置ｑｐ（ｇ，４）までの経路を構成する経路構成コンフィギュレーション点として（ｆ，３），（ｇ，４）を格納している。
【００４４】
第１候補判断手段３２は、図３と図４に示すように、動作経路における現在位置ｑｐに隣接する現在位置隣接コンフィギュレーション点ｑｃを求める演算装置である。例えば、現在位置ｑｐのコンフィギュレーション点に対して、実空間におけるロボットアーム１００の関節角度（θ１，θ２，θ３，θ４，θ５，θ６）がそれぞれ離散化された回転角度だけ変化した場合のＣスペース上における隣接するコンフィギュレーション点を演算して求めることができる。
具体的には、第１候補判断手段３２は、図４（ａ）における初期コンフィギュレーション点ｑｉ（ｆ，２）が現在位置ｑｐである場合には、コンフィギュレーション点（ｅ，１），（ｅ，２），（ｅ，３），（ｆ，１），（ｆ，３），（ｇ，１），（ｇ，２），および（ｇ，３）を現在位置隣接コンフィギュレーション点ｑｃとして求める。
【００４５】
第２候補判断手段３３は、図４（ｂ）に示すように、現在位置ｑｐに対する現在位置隣接コンフィギュレーション点ｑｃのうち干渉領域ＣＡとの干渉点に隣接する干渉点隣接コンフィギュレーション点ｑｃａを求める演算装置である。
例えば、２次元平面で表したＣスペース上では、図４（ｂ）における現在位置ｑｐ（ｇ，４）では、コンフィギュレーション点（ｆ，３），（ｆ，４），（ｆ，５），（ｇ，３），（ｇ，５），（ｈ，３），（ｈ，４），および（ｈ，５）が現在位置隣接コンフィギュレーション点ｑｃであるから、第２候補判断手段３３は、これらの現在位置隣接コンフィギュレーション点ｑｃのうちの（ｆ，４），および（ｈ，４）を干渉点に隣接する干渉点隣接コンフィギュレーション点ｑｃａとして求める。
【００４６】
このようにして本発明の実施形態に係る経路生成装置１は、第１候補判断手段３２および第２候補判断手段３３により、現在位置隣接コンフィギュレーション点ｑｃのうち障害物Ｏｂとの干渉点に隣接する干渉点隣接コンフィギュレーション点ｑｃａを求めることで、ロボットアーム１００の自由度に対応したすべてのコンフィギュレーション点について干渉の有無を判断するのではなく、Ｃスペースの構築をロボットアーム１００の現在位置ｑｐおよび干渉点の周囲に限定して部分的なものに留めるので、コンフィギュレーション空間における過剰な計算を排除することができる。
【００４７】
したがって、経路生成装置１は、確率的手法によらずにＣスペースの構築を部分的なものに限定することで、動作計画工数を効果的に低減することができ、自由度が高い６軸のロボットアーム１００であっても好適に適用することができる。
【００４８】
干渉チェック手段３４は、図３と図４に示すように、現在位置隣接コンフィギュレーション点が干渉点であるかどうかを判断する演算装置である。
つまり、干渉チェック手段３４は、対象となる現在位置隣接コンフィギュレーション点におけるロボットアーム１００（図１参照）の関節角度（θ１，θ２，θ３，θ４，θ５，θ６）から、ロボット形状記憶手段４３に格納されたロボットアーム１００の形状データを基にして対象となる現在位置隣接コンフィギュレーション点におけるロボットアーム１００の形状（姿勢）を求めて、作業空間記憶手段４４に格納された障害物Ｏｂ（干渉領域ＣＡ）の形状データと干渉するかどうかをチェックする。
【００４９】
例えば、干渉チェック手段３４は、ロボットアーム１００と障害物ＯｂのＣＡＤデータをポリゴン（例えば三角形等の多角形）の集合として表した場合には、対象となる現在位置隣接コンフィギュレーション点におけるロボットアーム１００のポリゴンの集合と障害物Ｏｂのポリゴンの集合が幾何学的に接触しているかどうかを判定して判断する。
【００５０】
具体的には、干渉チェック手段３４は、Ｃスペース上において、対象となる現在位置隣接コンフィギュレーション点ｑｃにおけるロボットアーム１００の形状を構成する１つの多角形（ポリゴン）と障害物Ｏｂの形状を構成する１つの多角形（ポリゴン）との接触を判定し、それぞれすべての多角形（ポリゴン）の集合について同様に判定して、ロボットアーム１００と障害物Ｏｂが干渉するか否かを判断して、ディスプレイ２１（図３）に表示させることができる。
【００５１】
なお、本実施形態においては、Ｃスペース上において、ポリゴンの集合が接触しているかどうかで判断したが、これに限定されるものではなく、ロボットアーム１００の自由度数（Ｎ次元）に応じて、このＮ次元形状の干渉領域ＣＡを想定してロボットアーム１００と干渉領域ＣＡとの干渉をチェックしてもよい。
また、ポリゴン（多角形状）ではなく、ロボットアーム１００と障害物Ｏｂを点や線ないし面の集合として表して、このような点と面との干渉チェック、または線と面との干渉チェックでもよい。
さらに、Ｃスペース上における干渉チェックに限定されるものではなく、ＣＡＤ空間や実空間内で点や線ないしポリゴンを利用した干渉チェックを採用することもできる。
【００５２】
干渉点記憶手段４６は、干渉チェック手段３４により動作経路を探索中に遭遇して発見した干渉点を格納する記憶装置である。
例えば、干渉点記憶手段４６は、図４（ｂ）における現在位置ｑｐ（ｇ，４）において、干渉チェック手段３４により発見された干渉点としてコンフィギュレーション点（ｆ，５），（ｇ，５），および（ｈ，５）を格納する。
【００５３】
距離候補判断手段３５は、図３と図４に示すように、現在位置隣接コンフィギュレーション点ｑｃのうち現在位置ｑｐから目標コンフィギュレーション点ｑｇまでの仮想直線経路に対して最も近づくように距離候補コンフィギュレーション点を選定する演算装置である。
例えば、距離候補判断手段は、図４（ａ）に示すように、初期コンフィギュレーション点ｑｉ（ｆ，２）が現在位置ｑｐである場合には、現在位置（ｆ，２）から目標コンフィギュレーション点ｑｇまでの仮想直線経路（破線で示す直線）に対して最も近づくように距離候補コンフィギュレーション点（ｆ，３）を選定する。
【００５４】
なお、本実施形態においては、現在位置ｑｐから目標コンフィギュレーション点ｑｇまでの仮想直線経路に対して最も近づくように距離候補コンフィギュレーション点を選定したが、これに限定されるものではなく、仮想直線経路に対して近づくように距離候補コンフィギュレーション点（ｇ，３）を選定することもできる。
【００５５】
デッドロック回避手段３６は、図３と図４に示すように、現在位置隣接コンフィギュレーション点ｑｃのうち現在位置ｑｐまでに通過した経路を構成する経路記憶手段４５に格納された経路構成コンフィギュレーション点を参照して新コンフィギュレーション点ｑｎとして採択しないように除外する演算装置である。
【００５６】
続いて、本発明の実施形態に係る経路生成装置１における動作について図４と図５、および適宜図６のフローチャートを参照しながら説明する。
図４と図５は、初期コンフィギュレーション点ｑｉから目標コンフィギュレーション点ｑｇまで障害物を回避するように新コンフィギュレーション点ｑｎを採択してロボットアーム１００の動作経路を生成する様子を説明するための模式図であり、図４は干渉領域ＣＡが凸形状をなした例を示し、図５は干渉領域ＣＡが凹形状をなしデッドロック回避手段３６の動作を説明するための模式図である。
【００５７】
［データの格納］
作業者（不図示）がロボットアーム１００（図１）の初期位置データ、目標位置データ、ロボットアーム１００の形状データ、および障害物Ｏｂ（図２（ａ）、図７）の形状データを操作盤２から入力すると、経路生成装置１は、図６に示すように、Ｃスペース上において、ロボットアーム１００の初期位置データを初期コンフィギュレーション点ｑｉとして初期位置記憶手段４１に格納し、目標位置データを目標コンフィギュレーション点ｑｇとして目標位置記憶手段４２に格納し、ロボットアーム１００（図１参照）の形状データをロボット形状記憶手段４３に格納し、障害物Ｏｂ（図２（ａ）、図７）の形状データを作業空間記憶手段４４に格納する（図６のステップＳ１参照）。
【００５８】
そして、経路生成装置１は、現在位置検出手段により、Ｃスペース上における現在位置ｑｐを検出し（図６のステップＳ２）、干渉チェック手段３４により、現在位置隣接コンフィギュレーション点が干渉点であるかどうかを判断する（図６のステップＳ３）。
【００５９】
経路生成装置１は、図３と図４に示すように、初期コンフィギュレーション点ｑｉ、および現在位置ｑｐから目標コンフィギュレーション点ｑｇに向かって直線的に経路を採択する直進モード経路（図４（ａ），（ｃ）参照）と、干渉領域ＣＡの境界に沿って経路を採択する境界追従モード経路（図４（ｂ））と、を繰り返すことで経路を生成する（図６のステップＳ４とＳ５）。
【００６０】
［直進モード経路］
直進モード経路は、図４（ａ），（ｃ）に示すように、動作経路における現在位置ｑｐにおいて距離候補判断手段３５により選定された距離候補コンフィギュレーション点（ｆ，３）が干渉チェック手段３４により干渉点でないと判断された場合には（図６のステップＳ３のＹｅｓ）、この距離候補コンフィギュレーション点（ｆ，３）を新コンフィギュレーション点ｑｎとして採択して（図６のステップＳ９）、経路記憶手段４５に格納し新コンフィギュレーション点ｑｎまで進む経路である（図６のステップＳ４〜Ｓ９）。
【００６１】
具体的には、経路生成装置１は、図４（ａ）に示すように、初期コンフィギュレーション点ｑｉ（ｆ，２）が現在位置ｑｐである場合には、第１候補判断手段３２により、コンフィギュレーション点（ｅ，１），（ｅ，２），（ｅ，３），（ｆ，１），（ｆ，３），（ｇ，１），（ｇ，２），および（ｇ，３）を現在位置隣接コンフィギュレーション点ｑｃとして求める。
【００６２】
そして、経路生成装置１は、距離候補判断手段３５により、これらの現在位置隣接コンフィギュレーション点ｑｃのうち現在位置ｑｐから目標コンフィギュレーション点ｑｇに最も近づくようにコンフィギュレーション点（ｆ，３）を選定する。
【００６３】
そして、経路生成装置１は、干渉チェック手段３４により、コンフィギュレーション点（ｆ，３）が干渉点であるかどうかを判断して、干渉点ではないので、コンフィギュレーション点（ｆ，３）が経路構成コンフィギュレーション点でないこと（図６のステップＳ６）、および、目標コンフィギュレーション点でないことを確認して（図６のステップＳ７）、このコンフィギュレーション点（ｆ，３）を新コンフィギュレーション点ｐｎとして採択する（図６のステップＳ９）。
【００６４】
同様にして、図４（ａ）に示すように、経路生成装置１は、コンフィギュレーション点（ｆ，３）からさらに直進モード経路を採択し、コンフィギュレーション点（ｇ，４）まで進む経路を生成する。
【００６５】
かかる構成により、経路生成装置１は、このようにして直進モード経路を生成することで、干渉点に隣接するまでは目標コンフィギュレーション点に迅速に到達することができるため、無駄な動作を低減してロボットアーム１００の効率的な動作経路を生成することができる。
【００６６】
［境界追従モード経路］
境界追従モード経路は、図４（ｂ）に示すように、動作経路における現在位置ｑｐにおいて距離候補判断手段３５により選定された距離候補コンフィギュレーション点が干渉チェック手段３４により干渉点であると判断された場合には（図６のステップＳ３のＮｏ）、第２候補判断手段３３により求めた干渉点隣接コンフィギュレーション点ｑｃａのうち、距離候補判断手段３５により現在位置ｑｐから目標コンフィギュレーション点ｑｇに近づくように新コンフィギュレーション点ｑｎを採択して経路記憶手段４５に格納し新コンフィギュレーション点ｑｎまで進む経路である（図６のステップＳ５〜Ｓ９）。
【００６７】
具体的には、経路生成装置１は、図４（ｂ）に示すように、現在位置ｑｐがコンフィギュレーション点（ｇ，４）である場合には、第１候補判断手段３２により、コンフィギュレーション点（ｆ，３），（ｆ，４），（ｆ，５），（ｇ，３），（ｇ，５），（ｈ，３），（ｈ，４），および（ｈ，５）を現在位置隣接コンフィギュレーション点ｑｃとして求める。
【００６８】
そして、経路生成装置１は、距離候補判断手段３５により、これらの現在位置隣接コンフィギュレーション点ｑｃのうち現在位置ｑｐから目標コンフィギュレーション点ｑｇに最も近づくように、例えばコンフィギュレーション点（ｇ，５）を選定する。
【００６９】
そして、経路生成装置１は、干渉チェック手段３４により、コンフィギュレーション点（ｇ，５）が干渉点であるかどうかを判断して、この場合にはコンフィギュレーション点（ｇ，５）が干渉点であるので、第２候補判断手段３３により、干渉点隣接コンフィギュレーション点ｑｃａ（ｆ，４），（ｈ，４）のうち、距離候補判断手段３５により、現在位置ｑｐから目標コンフィギュレーション点ｑｇに近づくように新コンフィギュレーション点ｑｎ（ｈ，４）を採択して、経路記憶手段４５に格納し、この新コンフィギュレーション点（ｈ，４）まで進む経路を生成する（図６のステップＳ６，Ｓ７，Ｓ９）。
【００７０】
このとき、経路生成装置１は、経路探索中に干渉チェック手段３４が発見した干渉点であるコンフィギュレーション点（ｇ，５）を干渉点記憶手段４６に格納して記憶する。
このように発見した干渉点を記憶しておくことで、繰り返しの演算を回避して処理を迅速化することができる。
【００７１】
同様にして、図４（ｂ）に示すように、経路生成装置１は、コンフィギュレーション点（ｈ，４）からコンフィギュレーション点（ｋ，８）まで境界追従モード経路を採択して経路を生成する。
【００７２】
かかる構成により、経路生成装置１は、距離候補コンフィギュレーション点が干渉点であると判断された場合には、境界追従モード経路を生成することで、障害物Ｏｂの境界を辿る方向に進む経路を採択するため、狭いスペースでも障害物Ｏｂとの干渉領域ＣＡを確実に回避しながら動作経路を生成することが可能である。
【００７３】
経路生成装置１は、図４（ｃ）に示すように、図４（ａ）に示す直進モード経路と同様にして、コンフィギュレーション点（ｋ，８）から目標コンフィギュレーション点ｑｇ（ｋ，８）まで直進モード経路を生成する。
このようにして、経路生成装置１は、直進モード経路（図４（ａ），（ｃ））および境界追従モード経路（図４（ｂ））を逐次生成しながら初期コンフィギュレーション点ｑｉから目標コンフィギュレーション点ｑｇまでの動作経路を生成する。
【００７４】
続いて、図５を参照しながら干渉領域ＣＡが凹形状の場合におけるデッドロック回避手段３６（図３）の動作について説明する。なお、直進モード経路および境界追従モード経路については図４の場合と同様であるので詳細な説明は省略する。
図５（ａ）に示すように、経路生成装置１は、初期コンフィギュレーション点ｑｉ（ｂ，５）からコンフィギュレーション点（ｇ，８）までは直進モード経路を生成して進行し、コンフィギュレーション点（ｇ，８）からコンフィギュレーション点（ｊ，８）までは境界追従モードを生成して進行する。
【００７５】
ここで、現在位置ｑｐがコンフィギュレーション点（ｊ，８）である場合には、デッドロック回避手段３６により、現在位置ｑｐ（ｊ，８）までに通過した経路であるコンフィギュレーション点（ｉ，８）を採択しないので（図６のステップＳ６〜Ｓ８）、経路を逆戻りして同じ経路を往復するようなデッドロックに陥ることを回避することができる。
【００７６】
したがって、経路生成装置１は、常に新しいコンフィギュレーション点を採択するため、コンフィギュレーション点（ｊ，８）では、新コンフィギュレーション点ｑｎとして（ｊ，７）を採択する（図６のステップＳ９）。このようにして、経路生成装置１は、コンフィギュレーション点（ｊ，６）からコンフィギュレーション点（ｉ，６）まで、境界追従モードを生成して進行する（図６のステップＳ５）。
【００７７】
現在位置ｑｐがコンフィギュレーション点（ｉ，６）である場合には、図５（ｂ）に示すように、コンフィギュレーション点（ｊ，７）と（ｈ，６）が干渉点隣接コンフィギュレーション点ｑｃａであるが、コンフィギュレーション点（ｊ，７）は、現在位置ｑｐまでに通過した経路構成コンフィギュレーション点であるから、デッドロック回避手段３６により新コンフィギュレーション点ｑｎとして採択しないように除外される（図６のステップＳ６〜Ｓ８）。
【００７８】
このため、経路生成装置１は、コンフィギュレーション点（ｊ，７）を新コンフィギュレーション点として採択しないように除外することで、コンフィギュレーション点（ｊ，７）から（ｊ，６）、（ｉ，６）のように同じ経路を周回するようなデッドロックに陥ることを回避することができる。
このようにして、経路生成装置１は、コンフィギュレーション点（ｉ，６）から新コンフィギュレーション点ｑｎとして（ｈ，６）を採択し、同様にして、図５（ｃ）に示すように、コンフィギュレーション点（ｈ，６）からさらに境界追従モード経路および直進モード経路を生成しながら目標コンフィギュレーション点ｑｇに到達する。
【００７９】
以上、本発明の実施形態について説明したが、本発明は前記した実施形態に限定されず、適宜変更して実施することが可能である。
例えば、本実施形態においては、干渉点記憶手段４６を備えて構成したが、これに限定されるものではなく、干渉点記憶手段４６を設けずにその都度干渉チェック手段３４により、干渉点であるかどうかを判断してもよい。
【００８０】
本実施形態においては、コンフィギュレーション空間を用いた経路生成装置について説明したが、実空間における干渉チェック等の手法を排除する趣旨ではなく、本発明における経路生成の後、実際のロボットアーム１００の動作を実空間において確認し検証することもできる。
【符号の説明】
【００８１】
１ 経路生成装置
２ 操作盤
３ 演算処理装置
４ 記憶装置
２１ ディスプレイ
３１ 現在位置検出手段
３２ 第１候補判断手段
３３ 第２候補判断手段
３４ 干渉チェック手段
３５ 距離候補判断手段
３６ デッドロック回避手段
４１ 初期位置記憶手段
４２ 目標位置記憶手段
４３ ロボット形状記憶手段
４４ 作業空間記憶手段
４５ 経路記憶手段
４６ 干渉点記憶手段
１００，１００′ ロボットアーム
ＣＡ 干渉領域
Ｏｂ 障害物
ｑｉ 初期コンフィギュレーション点
ｑｇ 目標コンフィギュレーション点
ｑｎ 新コンフィギュレーション点
ｑｐ 現在位置
ｑｃ 現在位置隣接コンフィギュレーション点
ｑｃａ 干渉点隣接コンフィギュレーション点
【技術分野】
【０００１】
本発明はロボットアームの経路生成装置および経路生成方法に係り、特にコンフィギュレーション空間を用いて障害物を回避する動作計画を行うロボットアームの経路生成装置および経路生成方法に関する。
【背景技術】
【０００２】
一般的に、原子炉など、人の進入が困難な場所において作業を行う場合、ＣＣＤカメラ、距離センサ、接触センサなどを組み込んで遠隔制御を行い、炉内の点検、形状の計測、補修作業などを行うといったマスタ・スレーブ方式が採用されるケースが多い。
特に、原子炉内において、動作経路選択を自動制御することのできる多関節ロボットを用いた作業が行われており、そうした自動制御および経路生成を実現する手法として、コンフィギュレーション空間を用いた動作経路生成法、迷路探索法、ＲＲＴ（Ｒａｐｉｄｌｙ−ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）法などの確率的手法を用いた動作経路生成法が開示されている（特許文献１〜３）。
【０００３】
コンフィギュレーション空間（Ｃスペース）は、マニュピレータ（ロボットアーム）の自由度の数と同数の次元数を持つ仮想空間であり、このＣスペース上では、ロボットアームのコンフィギュレーション（姿勢）は、ロボットアームの自由度を表すパラメータを座標軸の値とした場合、点（コンフィギュレーション点）で表すことができる。
例えば、ロボットアームのすべてのコンフィギュレーションについて障害物との干渉の有無を調査してロボットアームが障害物と干渉しない空間（自由空間）をＣスペース上に構築し、その中でロボットアームの動作経路を生成する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平４−２３５６０６号公報
【特許文献２】特開２００８−１０５１３２号公報
【特許文献３】特開２０１０−３２３２６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、Ｃスペースを用いた経路生成方法は、ロボットアームの自由度が増えると、計算量や必要な記憶容量が膨大となり、多自由度ロボットへの適用は非現実的であるといった課題があった。
【０００６】
さらに、上記Ｃスペースを用いた経路生成方法に対する解決策として、Ｃスペース上の初期コンフィギュレーション点と目標コンフィギュレーション点から波面を広げるようにＣスペースを構築することで計算量を削減し、２つの波面を重ねた自由空間で動作生成する方法も開示されているが、波面を全方向に一様に膨張させるため、余分な自由空間の構築を行うといった課題があった（特許文献１）。
【０００７】
また、迷路探索法を用いた経路生成方法は、Ｃスペース上の初期コンフィギュレーション点と目標コンフィギュレーション点を直線で結び、その直線およびその直線と交わるＣスペース上の障害物の境界をたどることによって経路を生成するが、２次元迷路を前提としており、３次元以上のＣスペースにおける障害物の境界は面や立体で現れるため、経路決定ができないといった課題があった。
【０００８】
また、ＲＲＴ法を用いた経路生成方法は、ランダムに選んだコンフィギュレーション点に向けて一定距離だけ進んだコンフィギュレーション点を経路探索木に取り入れて探索木を成長させ、探索木に取り入れたコンフィギュレーション点をたどることによって経路を生成するが、乱数を用いて自由空間を構築するため、計算時間の推定が困難であるといった課題があった。
【０００９】
本発明は、このような背景に鑑みてなされたものであり、コンフィギュレーション空間における過剰な計算を排除して、ロボットアームの動作計画工数を低減することができる経路生成装置および経路生成方法を提供することを課題とする。
【課題を解決するための手段】
【００１０】
本発明の請求項１に係る発明は、演算処理装置および記憶装置からなる制御装置を有し、初期コンフィギュレーション点から目標コンフィギュレーション点まで障害物を回避するように新コンフィギュレーション点を採択してロボットアームの動作経路を生成するコンフィギュレーション空間を用いた経路生成装置であって、前記制御装置は、前記初期コンフィギュレーション点を格納する初期位置記憶手段と、前記目標コンフィギュレーション点を格納する目標位置記憶手段と、前記ロボットアームの現在位置を検出する現在位置検出手段と、前記初期コンフィギュレーション点から現在位置までの経路を構成する各コンフィギュレーション点を格納する経路記憶手段と、前記動作経路における現在位置に隣接する現在位置隣接コンフィギュレーション点を求める第１候補判断手段と、前記現在位置隣接コンフィギュレーション点のうち前記障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求める第２候補判断手段と、前記現在位置隣接コンフィギュレーション点が前記干渉点であるかどうかを判断する干渉チェック手段と、前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点に近づくように距離候補コンフィギュレーション点を選定する距離候補判断手段と、を備え、前記動作経路における現在位置において前記距離候補判断手段により選定された距離候補コンフィギュレーション点が前記干渉チェック手段により前記干渉点でないと判断された場合には、当該距離候補コンフィギュレーション点を前記新コンフィギュレーション点として採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む直進モード経路を生成し、前記動作経路における現在位置において前記距離候補判断手段により選定された距離候補コンフィギュレーション点が前記干渉チェック手段により前記干渉点であると判断された場合には、前記第２候補判断手段により求めた前記干渉点隣接コンフィギュレーション点のうち、前記距離候補判断手段により現在位置から前記目標コンフィギュレーション点に近づくように前記新コンフィギュレーション点を採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む境界追従モード経路を生成し、このようにして、前記直進モード経路および前記境界追従モード経路を逐次生成しながら前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することを特徴とする。
【００１１】
本発明は、第１候補判断手段および第２候補判断手段により、現在位置隣接コンフィギュレーション点のうち前記障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求めることで、ロボットアームの自由度に対応したすべてのコンフィギュレーション点について干渉の有無を判断するのではなく、コンフィギュレーション空間（Ｃスペース）の構築をロボットアームの現在位置および干渉点の周囲に限定して部分的なものに留めるので、Ｃスペースにおける過剰な計算を排除することができる。
このようにして、本発明は、確率的手法によらずにＣスペースの構築を部分的なものに限定することで、動作計画工数を効果的に低減することができる。
【００１２】
そして、前記動作経路における現在位置において距離候補コンフィギュレーション点が前記干渉点でないと判断された場合には直進モード経路を生成することで、干渉点に隣接するまでは目標コンフィギュレーション点に迅速に到達することができるため、無駄な動作を低減してロボットアームの効率的な動作経路を生成することができる。
【００１３】
また、距離候補コンフィギュレーション点が前記干渉点であると判断された場合には、境界追従モード経路を生成することで、障害物の境界を辿る方向に進む経路を採択するため、狭いスペースでも障害物との干渉領域を確実に回避しながら動作経路を生成することが可能である。
【００１４】
以上より、本発明に係る経路生成装置は、前記直進モード経路および前記境界追従モード経路を逐次生成しながら、前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することで、障害物との干渉領域を確実に回避しながら、Ｃスペースにおける過剰な計算を排除して、ロボットアームの動作計画工数を低減することができる。
【００１５】
このため、本発明に係る経路生成装置は、自由度が高い多関節ロボットであっても好適に適用することができる。例えば、安全性が重視される原子力関係の機器等におけるシュラウドや複数の配管が並存するような狭あいな作業空間の中で多関節ロボットアームの動作経路を生成するような場合には、干渉領域を確実に回避することができる動作経路を迅速にシミュレーションすることが可能であるため、経路生成ツールとしての高い利用価値が期待される。
【００１６】
請求項２に係る発明は、請求項１に記載の経路生成装置であって、前記距離候補判断手段は、前記仮想直線経路に対して最も近づくようにコンフィギュレーション点を選定することを特徴とする。
【００１７】
かかる構成によれば、前記距離候補判断手段は、前記仮想直線経路に対して最も近づくようにコンフィギュレーション点を選定することで、より狭いスペースでも障害物との干渉領域を確実に回避しながら動作経路を生成することが可能となる。
【００１８】
請求項３に係る発明は、請求項１または請求項２に記載の経路生成装置であって、前記現在位置隣接コンフィギュレーション点のうち現在位置までに通過した経路を構成する前記経路記憶手段に格納された経路構成コンフィギュレーション点を参照して前記新コンフィギュレーション点として採択しないように除外するデッドロック回避手段をさらに備えたことを特徴とする。
【００１９】
かかる構成によれば、現在位置までに遭遇したコンフィギュレーション点を除外して、常に新しいコンフィギュレーション点について経路探索が行われるため、経路を逆戻りして同じ経路を往復したり、同じ経路を周回したりしてデッドロックに陥ることを回避することができる。
【００２０】
請求項４に係る発明は、初期コンフィギュレーション点から目標コンフィギュレーション点まで障害物を回避するように新コンフィギュレーション点を採択してロボットアームの動作経路を生成するコンフィギュレーション空間を用いた迷路探索法に基づく経路生成方法であって、演算処理装置および記憶装置からなる制御装置を有する経路生成装置を使用して、前記演算処理装置を構成する第１候補判断手段により、前記動作経路における現在位置に隣接する現在位置隣接コンフィギュレーション点を求めるＡステップと、前記演算処理装置を構成する第２候補判断手段により、前記現在位置隣接コンフィギュレーション点のうち前記障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求めるＢステップと、前記動作経路における現在位置において、前記演算処理装置を構成する距離候補判断手段により前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点までの仮想直線経路に対して近づくように選定された距離候補コンフィギュレーション点が、前記演算処理装置を構成する干渉チェック手段により前記障害物の干渉点でないと判断された場合には、当該距離候補コンフィギュレーション点を前記新コンフィギュレーション点として採択して経路記憶手段に格納し当該新コンフィギュレーション点まで進む直進モード経路を生成するＣステップと、前記動作経路における現在位置において、前記演算処理装置を構成する距離候補判断手段により前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点までの仮想直線経路に対して近づくように選定された距離候補コンフィギュレーション点が、前記干渉チェック手段により前記障害物の干渉点であると判断された場合には、前記第２候補判断手段により求めた前記干渉点隣接コンフィギュレーション点のうち、前記距離候補判断手段により現在位置から前記目標コンフィギュレーション点に近づくように前記新コンフィギュレーション点を採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む境界追従モード経路を生成するＤステップと、を含み、前記Ｃステップにおける直進モード経路および前記Ｄステップにおける境界追従モード経路を逐次生成しながら前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することを特徴とする。
【００２１】
かかる構成によれば、前記直進モード経路および前記境界追従モード経路を逐次生成しながら前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することで、Ｃスペースにおける過剰な計算を回避して、多関節ロボットにおける動作計画工数を低減することができる効率的な動作経路を生成することが可能となる。
【００２２】
請求項５に係る発明は、請求項４に記載の経路生成方法であって、前記距離候補判断手段は、前記仮想直線経路に対して最も近づくようにコンフィギュレーション点を選定することを特徴とする。
【００２３】
かかる構成によれば、前記距離候補判断手段は、前記仮想直線経路に対して最も近づくようにコンフィギュレーション点を選定することで、より狭いスペースでも障害物との干渉点を確実に回避しながら動作経路を生成することが可能となる。
【発明の効果】
【００２４】
本発明によれば、コンフィギュレーション空間における過剰な計算を排除して、ロボットアームにおける動作計画工数を低減することができる経路生成装置および経路生成方法を提供することができる。したがって、本発明は、自由度が高い多関節ロボットであっても円滑に適用できることから、原子力分野等における狭あいな作業空間の中で複雑な動作をさせたい場合等において特に好適である。
【図面の簡単な説明】
【００２５】
【図１】ロボットアームにおける６軸構成を示す斜視図である。
【図２】２軸のロボットアームにおけるロボットアームの姿勢と障害物との関係を示す図であり、（ａ）は実空間における関係、（ｂ）はコンフィギュレーション空間における関係を示す。
【図３】本発明の実施形態に係る経路生成装置の構成を示すブロック図である。
【図４】Ｃスペースにおける凸形状の干渉領域を想定した場合における経路生成の具体的な手法を説明するための図であり、（ａ）は直進モード経路、（ｂ）は境界追従モード経路、（ｃ）は直進モード経路である。
【図５】Ｃスペースにおける凹形状の障害物を想定した場合におけるデッドロック回避手段の作用を説明するための図である。
【図６】本発明の実施形態に係る経路生成装置の動作を説明するための流れ図である。
【図７】ロボットアームの初期位置から目標位置までの動作のイメージを示す斜視図であり、（ａ）は初期位置、（ｂ）は目標位置、（ｃ）は初期位置から目標位置までの動作のイメージを示す。
【発明を実施するための形態】
【００２６】
本発明の実施形態に係る経路生成装置１（図３参照）について、適宜図を参照しながら詳細に説明する。経路生成装置１は、コンフィギュレーション空間（Ｃスペース）を用いて、６軸を有するロボットアーム１００（図１参照）の動作経路を生成する場合を例として説明する。なお、ロボットアーム１００は、一般的にはロボットアーム自体が移動するような移動体として広く観念されるものであればよく、特に限定して解釈されるものではない。
【００２７】
ロボットアーム１００は、図１に示すように、６つの関節（６つの自由度）からなる６軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６を備えている。
ロボットアームの自由度を表すパラメータを関節角度として、ロボットアーム１００の６軸Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６の関節角度をそれぞれθ１，θ２，θ３，θ４，θ５，θ６とすれば、ロボットアーム１００のコンフィギュレーション（姿勢）はθ１，θ２，θ３，θ４，θ５，θ６で表すことができる。
【００２８】
このように、ロボットアーム１００の自由度を表すパラメータ（例えば、関節角度や伸縮長さ）を座標軸の値とした場合、ロボットアーム１００のコンフィギュレーション点はＣスペース上の点として表現することができる。
【００２９】
例えば、図２に示すように、簡略化のために例えば、実空間において関節角度をθｘとθｙで表すことができる２軸（θｘ，θｙ）のロボットアーム１００′（図２（ａ）参照）について、横軸にθｘをとり縦軸にθｙをとれば、Ｃスペース上において、ロボットアーム１００′のコンフィギュレーション（姿勢）をコンフィギュレーション点（θｘ，θｙ）して表すことができる。
【００３０】
一方、ロボットアーム１００′が作業空間中の障害物Ｏｂ（Ｏｂｓｔａｃｌｅ）と干渉（衝突）するコンフィギュレーション点を干渉点と定義する。干渉点が単独で存在することはまれであり、一般に連続した点の集合である干渉領域ＣＡ（Ｃｏｌｌｉｓｉｏｎ Ａｒｅａ）としてＣスペース上に表すことができる（図２（ｂ）参照）。
このようにして、図２（ｂ）に示すように、Ｃスペース上において、ロボットアーム１００′のコンフィギュレーションと干渉領域ＣＡを表すことで、動作経路におけるロボットアームと障害物との干渉をチェックすることができる。
【００３１】
なお、図２では２軸（θｘ，θｙ）について説明したが、計算量が異なるとしても同じ考え方でロボットアーム１００（図１）のような６軸（θ１，θ２，θ３，θ４，θ５，θ６）に発展させて処理することも可能である。このため、図４や図５に示すように、２軸のみを平面的に示す方が理解しやすいので、便宜上簡略化したものである。
【００３２】
経路生成装置１は、図３と図４に示すように、初期コンフィギュレーション点ｑｉから目標コンフィギュレーション点ｑｇまで干渉領域ＣＡを回避するように逐次新コンフィギュレーション点ｑｎ（例えば、図５（ａ）参照）を採択してロボットアーム１００の動作経路を生成する装置である（図４、図７参照）。
ここで、Ｃスペースを用いた動作計画とは、図４に示すように、Ｃスペース上において干渉領域ＣＡを避けて初期コンフィギュレーション点ｑｉ（ｉｎｉｔｉａｌ）と目標コンフィギュレーション点ｑｇ（ｇｏａｌ）を接続する経路を生成し、実空間における経路に置き換えて動作経路を探索する手法である。
【００３３】
経路生成装置１は、図３に示すように、作業者（不図示）がデータ入力等を行う操作盤２と、図示しないＣＰＵ等からなる演算処理装置３と、種々のデータを格納する記憶装置４と、を備えている。
操作盤２には、ディスプレイ２１が配設され、ロボットアーム１００や作業空間を３Ｄ（３次元）グラフィックス表示して、作業者（不図示）が生成された動作経路を視認できるようになっている。
【００３４】
演算処理装置３は、図３と図４に示すように、現在位置検出手段３１と、現在位置ｑｐに隣接する現在位置隣接コンフィギュレーション点ｑｃを求める第１候補判断手段３２と、干渉点に隣接する干渉点隣接コンフィギュレーション点ｑｃａを求める第２候補判断手段３３と、干渉点であるかどうかを判断する干渉チェック手段３４と、現在位置ｑｐから目標コンフィギュレーション点ｑｇに近づくように距離候補コンフィギュレーション点を選定する距離候補判断手段３５と、現在位置ｑｐまでに通過したコンフィギュレーション点を除外するデッドロック回避手段３６と、を備えている。
【００３５】
記憶装置４は、初期位置記憶手段４１と、目標位置記憶手段４２と、ロボットアーム１００（図１）の形状データを格納するロボット形状記憶手段４３と、作業空間の形状データを格納する作業空間記憶手段４４と、現在位置ｑｐまでの経路を格納する経路記憶手段４５と、発見した干渉点を格納する干渉点記憶手段４６と、を備えている。
【００３６】
以下、経路生成装置１を構成する具体的手段、およびその作用効果について、図３と図４を参照しながら経路生成装置１の動作の順に沿って説明する。
参照する図４において、細線で表示された図の縦方向のグリッド線をａ〜ｌまで、横方向のグリッド線を１〜１１まで符号をつけて示し、それらのグリッド線の交点を格子点といい（ａ，１）〜（ｌ，１１）のように表示する。ただし、グリッド線の間隔等の表示は模式的なものであり縦横の比率等は簡略化したものである。
【００３７】
格子点は、Ｃスペースを細かく分割して、離散化されたコンフィギュレーション点の集合として扱うための概念である。このように、Ｃスペースを連続した空間として認識するのではなく、格子点を基準として離散化（量子化）し、この量子化された格子点を通るように新コンフィギュレーション点を採択しながら動作経路を生成することで、Ｃスペース上の演算処理を簡略化することができる。
例えば、ロボットアーム１００の根元部分の関節角度θ１〜θ３を１度刻みにして、手先部分の関節角度θ４〜θ６を５度刻みにしてＣスペースにおける格子点を設定し、この格子点を基準にして離散化されたＣスペースを構築することができる。
【００３８】
初期位置記憶手段４１は、初期コンフィギュレーション点ｑｉ（図４参照）であるロボットアーム１００の初期姿勢（実空間では図７（ａ）参照）を各関節の関節角度等により記憶する装置であり、操作盤２から作業者（不図示）が入力したり、演算処理装置３により初期位置を角度センサ等の検出器で検出して初期コンフィギュレーション点ｑｉとして設定したりすることができる。
例えば、初期コンフィギュレーション点ｑｉは、ｑｉ＝（θ１，θ２，θ３，θ４，θ５，θ６）＝（−１７０，９０，０，２５、−８５，７５）のように関節角度をパラメータとして表すことができる。
【００３９】
目標位置記憶手段４２は、目標コンフィギュレーション点ｑｇであるロボットアーム１００の目標姿勢（実空間では図７（ｂ）参照）を各関節の関節角度等により記憶する手段であり、操作盤２から作業者（不図示）が入力したり、演算処理装置３により目標位置を角度センサ等の検出器で検出して目標コンフィギュレーション点ｑｇとして設定したりすることができる。
例えば、目標コンフィギュレーション点ｑｇは、ｑｇ＝（θ１，θ２，θ３，θ４，θ５，θ６）＝（−７１，−７５，３８，８０，−１０，２５）のように関節角度をパラメータとして表すことができる。
【００４０】
ロボット形状記憶手段４３は、ＣＡＤファイル等から作成したロボットアーム１００の形状データを格納するが、データ処理を簡略化するため、例えば、ロボットアーム１００の形状データをポリゴンと呼ばれる多角形等の最小構成単位（例えば三角形）の集合で表して、この多角形の集合をＣスペース上の形状として表現して記憶することができる。
かかる構成により、実空間におけるロボットアーム１００の形状データを三角形等のポリゴンの集合として表して、Ｃスペース上の形状として認識することで、Ｃスペース上におけるロボットアーム１００の形状データを簡略化（離散化）して演算処理を短縮化することができる。
【００４１】
作業空間記憶手段４４は、障害物Ｏｂ（図７参照）が配置された作業空間の形状データを格納するが、データ処理を簡略化するため、例えば、障害物Ｏｂの形状データをポリゴン（例えば三角形等の多角形）の集合で表して、この三角形等の集合をＣスペース上の形状として表現して記憶することができる。
かかる構成により、Ｃスペース上における作業空間の形状データを簡略化（離散化）して演算処理を短縮化することができる。
【００４２】
現在位置検出手段３１は、例えば、ロボットアーム１００（図１参照）の６軸の関節角度（θ１，θ２，θ３，θ４，θ５，θ６）を角度センサ等により計測して、Ｃスペース上の探索経路における現在位置ｑｐを検出する装置である。
現在位置ｑｐは、経路探索中は現在位置ｑｐから新たに採択された新コンフィギュレーション点ｑｎ（例えば、図５（ａ）参照）を生成するごとに更新されるが、現在位置ｑｐの位置データは経路記憶手段４５に格納されている。
【００４３】
経路記憶手段４５は、初期コンフィギュレーション点ｑｉから現在位置ｑｐまでの経路を構成する経路構成コンフィギュレーション点を格納する記憶装置である。
例えば、経路記憶手段４５は、図４（ｂ）に示す現在位置ｑｐ（ｇ，４）では、現在位置ｑｐ（ｇ，４）までの経路を構成する経路構成コンフィギュレーション点として（ｆ，３），（ｇ，４）を格納している。
【００４４】
第１候補判断手段３２は、図３と図４に示すように、動作経路における現在位置ｑｐに隣接する現在位置隣接コンフィギュレーション点ｑｃを求める演算装置である。例えば、現在位置ｑｐのコンフィギュレーション点に対して、実空間におけるロボットアーム１００の関節角度（θ１，θ２，θ３，θ４，θ５，θ６）がそれぞれ離散化された回転角度だけ変化した場合のＣスペース上における隣接するコンフィギュレーション点を演算して求めることができる。
具体的には、第１候補判断手段３２は、図４（ａ）における初期コンフィギュレーション点ｑｉ（ｆ，２）が現在位置ｑｐである場合には、コンフィギュレーション点（ｅ，１），（ｅ，２），（ｅ，３），（ｆ，１），（ｆ，３），（ｇ，１），（ｇ，２），および（ｇ，３）を現在位置隣接コンフィギュレーション点ｑｃとして求める。
【００４５】
第２候補判断手段３３は、図４（ｂ）に示すように、現在位置ｑｐに対する現在位置隣接コンフィギュレーション点ｑｃのうち干渉領域ＣＡとの干渉点に隣接する干渉点隣接コンフィギュレーション点ｑｃａを求める演算装置である。
例えば、２次元平面で表したＣスペース上では、図４（ｂ）における現在位置ｑｐ（ｇ，４）では、コンフィギュレーション点（ｆ，３），（ｆ，４），（ｆ，５），（ｇ，３），（ｇ，５），（ｈ，３），（ｈ，４），および（ｈ，５）が現在位置隣接コンフィギュレーション点ｑｃであるから、第２候補判断手段３３は、これらの現在位置隣接コンフィギュレーション点ｑｃのうちの（ｆ，４），および（ｈ，４）を干渉点に隣接する干渉点隣接コンフィギュレーション点ｑｃａとして求める。
【００４６】
このようにして本発明の実施形態に係る経路生成装置１は、第１候補判断手段３２および第２候補判断手段３３により、現在位置隣接コンフィギュレーション点ｑｃのうち障害物Ｏｂとの干渉点に隣接する干渉点隣接コンフィギュレーション点ｑｃａを求めることで、ロボットアーム１００の自由度に対応したすべてのコンフィギュレーション点について干渉の有無を判断するのではなく、Ｃスペースの構築をロボットアーム１００の現在位置ｑｐおよび干渉点の周囲に限定して部分的なものに留めるので、コンフィギュレーション空間における過剰な計算を排除することができる。
【００４７】
したがって、経路生成装置１は、確率的手法によらずにＣスペースの構築を部分的なものに限定することで、動作計画工数を効果的に低減することができ、自由度が高い６軸のロボットアーム１００であっても好適に適用することができる。
【００４８】
干渉チェック手段３４は、図３と図４に示すように、現在位置隣接コンフィギュレーション点が干渉点であるかどうかを判断する演算装置である。
つまり、干渉チェック手段３４は、対象となる現在位置隣接コンフィギュレーション点におけるロボットアーム１００（図１参照）の関節角度（θ１，θ２，θ３，θ４，θ５，θ６）から、ロボット形状記憶手段４３に格納されたロボットアーム１００の形状データを基にして対象となる現在位置隣接コンフィギュレーション点におけるロボットアーム１００の形状（姿勢）を求めて、作業空間記憶手段４４に格納された障害物Ｏｂ（干渉領域ＣＡ）の形状データと干渉するかどうかをチェックする。
【００４９】
例えば、干渉チェック手段３４は、ロボットアーム１００と障害物ＯｂのＣＡＤデータをポリゴン（例えば三角形等の多角形）の集合として表した場合には、対象となる現在位置隣接コンフィギュレーション点におけるロボットアーム１００のポリゴンの集合と障害物Ｏｂのポリゴンの集合が幾何学的に接触しているかどうかを判定して判断する。
【００５０】
具体的には、干渉チェック手段３４は、Ｃスペース上において、対象となる現在位置隣接コンフィギュレーション点ｑｃにおけるロボットアーム１００の形状を構成する１つの多角形（ポリゴン）と障害物Ｏｂの形状を構成する１つの多角形（ポリゴン）との接触を判定し、それぞれすべての多角形（ポリゴン）の集合について同様に判定して、ロボットアーム１００と障害物Ｏｂが干渉するか否かを判断して、ディスプレイ２１（図３）に表示させることができる。
【００５１】
なお、本実施形態においては、Ｃスペース上において、ポリゴンの集合が接触しているかどうかで判断したが、これに限定されるものではなく、ロボットアーム１００の自由度数（Ｎ次元）に応じて、このＮ次元形状の干渉領域ＣＡを想定してロボットアーム１００と干渉領域ＣＡとの干渉をチェックしてもよい。
また、ポリゴン（多角形状）ではなく、ロボットアーム１００と障害物Ｏｂを点や線ないし面の集合として表して、このような点と面との干渉チェック、または線と面との干渉チェックでもよい。
さらに、Ｃスペース上における干渉チェックに限定されるものではなく、ＣＡＤ空間や実空間内で点や線ないしポリゴンを利用した干渉チェックを採用することもできる。
【００５２】
干渉点記憶手段４６は、干渉チェック手段３４により動作経路を探索中に遭遇して発見した干渉点を格納する記憶装置である。
例えば、干渉点記憶手段４６は、図４（ｂ）における現在位置ｑｐ（ｇ，４）において、干渉チェック手段３４により発見された干渉点としてコンフィギュレーション点（ｆ，５），（ｇ，５），および（ｈ，５）を格納する。
【００５３】
距離候補判断手段３５は、図３と図４に示すように、現在位置隣接コンフィギュレーション点ｑｃのうち現在位置ｑｐから目標コンフィギュレーション点ｑｇまでの仮想直線経路に対して最も近づくように距離候補コンフィギュレーション点を選定する演算装置である。
例えば、距離候補判断手段は、図４（ａ）に示すように、初期コンフィギュレーション点ｑｉ（ｆ，２）が現在位置ｑｐである場合には、現在位置（ｆ，２）から目標コンフィギュレーション点ｑｇまでの仮想直線経路（破線で示す直線）に対して最も近づくように距離候補コンフィギュレーション点（ｆ，３）を選定する。
【００５４】
なお、本実施形態においては、現在位置ｑｐから目標コンフィギュレーション点ｑｇまでの仮想直線経路に対して最も近づくように距離候補コンフィギュレーション点を選定したが、これに限定されるものではなく、仮想直線経路に対して近づくように距離候補コンフィギュレーション点（ｇ，３）を選定することもできる。
【００５５】
デッドロック回避手段３６は、図３と図４に示すように、現在位置隣接コンフィギュレーション点ｑｃのうち現在位置ｑｐまでに通過した経路を構成する経路記憶手段４５に格納された経路構成コンフィギュレーション点を参照して新コンフィギュレーション点ｑｎとして採択しないように除外する演算装置である。
【００５６】
続いて、本発明の実施形態に係る経路生成装置１における動作について図４と図５、および適宜図６のフローチャートを参照しながら説明する。
図４と図５は、初期コンフィギュレーション点ｑｉから目標コンフィギュレーション点ｑｇまで障害物を回避するように新コンフィギュレーション点ｑｎを採択してロボットアーム１００の動作経路を生成する様子を説明するための模式図であり、図４は干渉領域ＣＡが凸形状をなした例を示し、図５は干渉領域ＣＡが凹形状をなしデッドロック回避手段３６の動作を説明するための模式図である。
【００５７】
［データの格納］
作業者（不図示）がロボットアーム１００（図１）の初期位置データ、目標位置データ、ロボットアーム１００の形状データ、および障害物Ｏｂ（図２（ａ）、図７）の形状データを操作盤２から入力すると、経路生成装置１は、図６に示すように、Ｃスペース上において、ロボットアーム１００の初期位置データを初期コンフィギュレーション点ｑｉとして初期位置記憶手段４１に格納し、目標位置データを目標コンフィギュレーション点ｑｇとして目標位置記憶手段４２に格納し、ロボットアーム１００（図１参照）の形状データをロボット形状記憶手段４３に格納し、障害物Ｏｂ（図２（ａ）、図７）の形状データを作業空間記憶手段４４に格納する（図６のステップＳ１参照）。
【００５８】
そして、経路生成装置１は、現在位置検出手段により、Ｃスペース上における現在位置ｑｐを検出し（図６のステップＳ２）、干渉チェック手段３４により、現在位置隣接コンフィギュレーション点が干渉点であるかどうかを判断する（図６のステップＳ３）。
【００５９】
経路生成装置１は、図３と図４に示すように、初期コンフィギュレーション点ｑｉ、および現在位置ｑｐから目標コンフィギュレーション点ｑｇに向かって直線的に経路を採択する直進モード経路（図４（ａ），（ｃ）参照）と、干渉領域ＣＡの境界に沿って経路を採択する境界追従モード経路（図４（ｂ））と、を繰り返すことで経路を生成する（図６のステップＳ４とＳ５）。
【００６０】
［直進モード経路］
直進モード経路は、図４（ａ），（ｃ）に示すように、動作経路における現在位置ｑｐにおいて距離候補判断手段３５により選定された距離候補コンフィギュレーション点（ｆ，３）が干渉チェック手段３４により干渉点でないと判断された場合には（図６のステップＳ３のＹｅｓ）、この距離候補コンフィギュレーション点（ｆ，３）を新コンフィギュレーション点ｑｎとして採択して（図６のステップＳ９）、経路記憶手段４５に格納し新コンフィギュレーション点ｑｎまで進む経路である（図６のステップＳ４〜Ｓ９）。
【００６１】
具体的には、経路生成装置１は、図４（ａ）に示すように、初期コンフィギュレーション点ｑｉ（ｆ，２）が現在位置ｑｐである場合には、第１候補判断手段３２により、コンフィギュレーション点（ｅ，１），（ｅ，２），（ｅ，３），（ｆ，１），（ｆ，３），（ｇ，１），（ｇ，２），および（ｇ，３）を現在位置隣接コンフィギュレーション点ｑｃとして求める。
【００６２】
そして、経路生成装置１は、距離候補判断手段３５により、これらの現在位置隣接コンフィギュレーション点ｑｃのうち現在位置ｑｐから目標コンフィギュレーション点ｑｇに最も近づくようにコンフィギュレーション点（ｆ，３）を選定する。
【００６３】
そして、経路生成装置１は、干渉チェック手段３４により、コンフィギュレーション点（ｆ，３）が干渉点であるかどうかを判断して、干渉点ではないので、コンフィギュレーション点（ｆ，３）が経路構成コンフィギュレーション点でないこと（図６のステップＳ６）、および、目標コンフィギュレーション点でないことを確認して（図６のステップＳ７）、このコンフィギュレーション点（ｆ，３）を新コンフィギュレーション点ｐｎとして採択する（図６のステップＳ９）。
【００６４】
同様にして、図４（ａ）に示すように、経路生成装置１は、コンフィギュレーション点（ｆ，３）からさらに直進モード経路を採択し、コンフィギュレーション点（ｇ，４）まで進む経路を生成する。
【００６５】
かかる構成により、経路生成装置１は、このようにして直進モード経路を生成することで、干渉点に隣接するまでは目標コンフィギュレーション点に迅速に到達することができるため、無駄な動作を低減してロボットアーム１００の効率的な動作経路を生成することができる。
【００６６】
［境界追従モード経路］
境界追従モード経路は、図４（ｂ）に示すように、動作経路における現在位置ｑｐにおいて距離候補判断手段３５により選定された距離候補コンフィギュレーション点が干渉チェック手段３４により干渉点であると判断された場合には（図６のステップＳ３のＮｏ）、第２候補判断手段３３により求めた干渉点隣接コンフィギュレーション点ｑｃａのうち、距離候補判断手段３５により現在位置ｑｐから目標コンフィギュレーション点ｑｇに近づくように新コンフィギュレーション点ｑｎを採択して経路記憶手段４５に格納し新コンフィギュレーション点ｑｎまで進む経路である（図６のステップＳ５〜Ｓ９）。
【００６７】
具体的には、経路生成装置１は、図４（ｂ）に示すように、現在位置ｑｐがコンフィギュレーション点（ｇ，４）である場合には、第１候補判断手段３２により、コンフィギュレーション点（ｆ，３），（ｆ，４），（ｆ，５），（ｇ，３），（ｇ，５），（ｈ，３），（ｈ，４），および（ｈ，５）を現在位置隣接コンフィギュレーション点ｑｃとして求める。
【００６８】
そして、経路生成装置１は、距離候補判断手段３５により、これらの現在位置隣接コンフィギュレーション点ｑｃのうち現在位置ｑｐから目標コンフィギュレーション点ｑｇに最も近づくように、例えばコンフィギュレーション点（ｇ，５）を選定する。
【００６９】
そして、経路生成装置１は、干渉チェック手段３４により、コンフィギュレーション点（ｇ，５）が干渉点であるかどうかを判断して、この場合にはコンフィギュレーション点（ｇ，５）が干渉点であるので、第２候補判断手段３３により、干渉点隣接コンフィギュレーション点ｑｃａ（ｆ，４），（ｈ，４）のうち、距離候補判断手段３５により、現在位置ｑｐから目標コンフィギュレーション点ｑｇに近づくように新コンフィギュレーション点ｑｎ（ｈ，４）を採択して、経路記憶手段４５に格納し、この新コンフィギュレーション点（ｈ，４）まで進む経路を生成する（図６のステップＳ６，Ｓ７，Ｓ９）。
【００７０】
このとき、経路生成装置１は、経路探索中に干渉チェック手段３４が発見した干渉点であるコンフィギュレーション点（ｇ，５）を干渉点記憶手段４６に格納して記憶する。
このように発見した干渉点を記憶しておくことで、繰り返しの演算を回避して処理を迅速化することができる。
【００７１】
同様にして、図４（ｂ）に示すように、経路生成装置１は、コンフィギュレーション点（ｈ，４）からコンフィギュレーション点（ｋ，８）まで境界追従モード経路を採択して経路を生成する。
【００７２】
かかる構成により、経路生成装置１は、距離候補コンフィギュレーション点が干渉点であると判断された場合には、境界追従モード経路を生成することで、障害物Ｏｂの境界を辿る方向に進む経路を採択するため、狭いスペースでも障害物Ｏｂとの干渉領域ＣＡを確実に回避しながら動作経路を生成することが可能である。
【００７３】
経路生成装置１は、図４（ｃ）に示すように、図４（ａ）に示す直進モード経路と同様にして、コンフィギュレーション点（ｋ，８）から目標コンフィギュレーション点ｑｇ（ｋ，８）まで直進モード経路を生成する。
このようにして、経路生成装置１は、直進モード経路（図４（ａ），（ｃ））および境界追従モード経路（図４（ｂ））を逐次生成しながら初期コンフィギュレーション点ｑｉから目標コンフィギュレーション点ｑｇまでの動作経路を生成する。
【００７４】
続いて、図５を参照しながら干渉領域ＣＡが凹形状の場合におけるデッドロック回避手段３６（図３）の動作について説明する。なお、直進モード経路および境界追従モード経路については図４の場合と同様であるので詳細な説明は省略する。
図５（ａ）に示すように、経路生成装置１は、初期コンフィギュレーション点ｑｉ（ｂ，５）からコンフィギュレーション点（ｇ，８）までは直進モード経路を生成して進行し、コンフィギュレーション点（ｇ，８）からコンフィギュレーション点（ｊ，８）までは境界追従モードを生成して進行する。
【００７５】
ここで、現在位置ｑｐがコンフィギュレーション点（ｊ，８）である場合には、デッドロック回避手段３６により、現在位置ｑｐ（ｊ，８）までに通過した経路であるコンフィギュレーション点（ｉ，８）を採択しないので（図６のステップＳ６〜Ｓ８）、経路を逆戻りして同じ経路を往復するようなデッドロックに陥ることを回避することができる。
【００７６】
したがって、経路生成装置１は、常に新しいコンフィギュレーション点を採択するため、コンフィギュレーション点（ｊ，８）では、新コンフィギュレーション点ｑｎとして（ｊ，７）を採択する（図６のステップＳ９）。このようにして、経路生成装置１は、コンフィギュレーション点（ｊ，６）からコンフィギュレーション点（ｉ，６）まで、境界追従モードを生成して進行する（図６のステップＳ５）。
【００７７】
現在位置ｑｐがコンフィギュレーション点（ｉ，６）である場合には、図５（ｂ）に示すように、コンフィギュレーション点（ｊ，７）と（ｈ，６）が干渉点隣接コンフィギュレーション点ｑｃａであるが、コンフィギュレーション点（ｊ，７）は、現在位置ｑｐまでに通過した経路構成コンフィギュレーション点であるから、デッドロック回避手段３６により新コンフィギュレーション点ｑｎとして採択しないように除外される（図６のステップＳ６〜Ｓ８）。
【００７８】
このため、経路生成装置１は、コンフィギュレーション点（ｊ，７）を新コンフィギュレーション点として採択しないように除外することで、コンフィギュレーション点（ｊ，７）から（ｊ，６）、（ｉ，６）のように同じ経路を周回するようなデッドロックに陥ることを回避することができる。
このようにして、経路生成装置１は、コンフィギュレーション点（ｉ，６）から新コンフィギュレーション点ｑｎとして（ｈ，６）を採択し、同様にして、図５（ｃ）に示すように、コンフィギュレーション点（ｈ，６）からさらに境界追従モード経路および直進モード経路を生成しながら目標コンフィギュレーション点ｑｇに到達する。
【００７９】
以上、本発明の実施形態について説明したが、本発明は前記した実施形態に限定されず、適宜変更して実施することが可能である。
例えば、本実施形態においては、干渉点記憶手段４６を備えて構成したが、これに限定されるものではなく、干渉点記憶手段４６を設けずにその都度干渉チェック手段３４により、干渉点であるかどうかを判断してもよい。
【００８０】
本実施形態においては、コンフィギュレーション空間を用いた経路生成装置について説明したが、実空間における干渉チェック等の手法を排除する趣旨ではなく、本発明における経路生成の後、実際のロボットアーム１００の動作を実空間において確認し検証することもできる。
【符号の説明】
【００８１】
１ 経路生成装置
２ 操作盤
３ 演算処理装置
４ 記憶装置
２１ ディスプレイ
３１ 現在位置検出手段
３２ 第１候補判断手段
３３ 第２候補判断手段
３４ 干渉チェック手段
３５ 距離候補判断手段
３６ デッドロック回避手段
４１ 初期位置記憶手段
４２ 目標位置記憶手段
４３ ロボット形状記憶手段
４４ 作業空間記憶手段
４５ 経路記憶手段
４６ 干渉点記憶手段
１００，１００′ ロボットアーム
ＣＡ 干渉領域
Ｏｂ 障害物
ｑｉ 初期コンフィギュレーション点
ｑｇ 目標コンフィギュレーション点
ｑｎ 新コンフィギュレーション点
ｑｐ 現在位置
ｑｃ 現在位置隣接コンフィギュレーション点
ｑｃａ 干渉点隣接コンフィギュレーション点
【特許請求の範囲】
【請求項１】
演算処理装置および記憶装置からなる制御装置を有し、
初期コンフィギュレーション点から目標コンフィギュレーション点まで障害物を回避するように新コンフィギュレーション点を採択してロボットアームの動作経路を生成するコンフィギュレーション空間を用いた経路生成装置であって、
前記制御装置は、
前記初期コンフィギュレーション点を格納する初期位置記憶手段と、
前記目標コンフィギュレーション点を格納する目標位置記憶手段と、
前記ロボットアームの現在位置を検出する現在位置検出手段と、
前記初期コンフィギュレーション点から現在位置までの経路を構成する各コンフィギュレーション点を格納する経路記憶手段と、
前記動作経路における現在位置に隣接する現在位置隣接コンフィギュレーション点を求める第１候補判断手段と、
前記現在位置隣接コンフィギュレーション点のうち前記障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求める第２候補判断手段と、
前記現在位置隣接コンフィギュレーション点が前記干渉点であるかどうかを判断する干渉チェック手段と、
前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点に近づくようにコンフィギュレーション点を選定する距離候補判断手段と、を備え、
前記動作経路における現在位置において前記距離候補判断手段により選定された距離候補コンフィギュレーション点が前記干渉チェック手段により前記干渉点でないと判断された場合には、当該距離候補コンフィギュレーション点を前記新コンフィギュレーション点として採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む直進モード経路を生成し、
前記動作経路における現在位置において前記距離候補判断手段により選定された距離候補コンフィギュレーション点が前記干渉チェック手段により前記干渉点であると判断された場合には、前記第２候補判断手段により求めた前記干渉点隣接コンフィギュレーション点のうち、前記距離候補判断手段により現在位置から前記目標コンフィギュレーション点に近づくように前記新コンフィギュレーション点を採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む境界追従モード経路を生成し、
このようにして、前記直進モード経路および前記境界追従モード経路を逐次生成しながら前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することを特徴とする経路生成装置。
【請求項２】
前記距離候補判断手段は、前記目標コンフィギュレーション点に最も近づくようにコンフィギュレーション点を選定することを特徴とする請求項１に記載の経路生成装置。
【請求項３】
前記現在位置隣接コンフィギュレーション点のうち現在位置までに通過した経路を構成する前記経路記憶手段に格納された経路構成コンフィギュレーション点を参照して前記新コンフィギュレーション点として採択しないように除外するデッドロック回避手段をさらに備えたことを特徴とする請求項１または請求項２に記載の経路生成装置。
【請求項４】
初期コンフィギュレーション点から目標コンフィギュレーション点まで障害物を回避するように新コンフィギュレーション点を採択してロボットアームの動作経路を生成するコンフィギュレーション空間を用いた迷路探索法に基づく経路生成方法であって、
演算処理装置および記憶装置からなる制御装置を有する経路生成装置を使用して、
前記演算処理装置を構成する第１候補判断手段により、前記動作経路における現在位置に隣接する現在位置隣接コンフィギュレーション点を求めるＡステップと、
前記演算処理装置を構成する第２候補判断手段により、前記現在位置隣接コンフィギュレーション点のうち前記障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求めるＢステップと、
前記動作経路における現在位置において、前記演算処理装置を構成する距離候補判断手段により前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点に近づくように選定された距離候補コンフィギュレーション点が、前記演算処理装置を構成する干渉チェック手段により前記障害物の干渉点でないと判断された場合には、当該距離候補コンフィギュレーション点を前記新コンフィギュレーション点として採択して前記記憶装置を構成する経路記憶手段に格納し当該新コンフィギュレーション点まで進む直進モード経路を生成するＣステップと、
前記動作経路における現在位置において、前記演算処理装置を構成する距離候補判断手段により前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点までの仮想直線経路に対して近づくように選定された距離候補コンフィギュレーション点が、干渉チェック手段により前記障害物の干渉点であると判断された場合には、前記第２候補判断手段により求めた前記干渉点隣接コンフィギュレーション点のうち、前記距離候補判断手段により現在位置から前記目標コンフィギュレーション点に近づくように前記新コンフィギュレーション点を採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む境界追従モード経路を生成するＤステップと、を含み、
前記Ｃステップにおける直進モード経路および前記Ｄステップにおける境界追従モード経路を逐次生成しながら前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することを特徴とする経路生成方法。
【請求項５】
前記距離候補判断手段は、前記仮想直線経路に対して最も近づくようにコンフィギュレーション点を選定することを特徴とする請求項４に記載の経路生成方法。
【請求項１】
演算処理装置および記憶装置からなる制御装置を有し、
初期コンフィギュレーション点から目標コンフィギュレーション点まで障害物を回避するように新コンフィギュレーション点を採択してロボットアームの動作経路を生成するコンフィギュレーション空間を用いた経路生成装置であって、
前記制御装置は、
前記初期コンフィギュレーション点を格納する初期位置記憶手段と、
前記目標コンフィギュレーション点を格納する目標位置記憶手段と、
前記ロボットアームの現在位置を検出する現在位置検出手段と、
前記初期コンフィギュレーション点から現在位置までの経路を構成する各コンフィギュレーション点を格納する経路記憶手段と、
前記動作経路における現在位置に隣接する現在位置隣接コンフィギュレーション点を求める第１候補判断手段と、
前記現在位置隣接コンフィギュレーション点のうち前記障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求める第２候補判断手段と、
前記現在位置隣接コンフィギュレーション点が前記干渉点であるかどうかを判断する干渉チェック手段と、
前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点に近づくようにコンフィギュレーション点を選定する距離候補判断手段と、を備え、
前記動作経路における現在位置において前記距離候補判断手段により選定された距離候補コンフィギュレーション点が前記干渉チェック手段により前記干渉点でないと判断された場合には、当該距離候補コンフィギュレーション点を前記新コンフィギュレーション点として採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む直進モード経路を生成し、
前記動作経路における現在位置において前記距離候補判断手段により選定された距離候補コンフィギュレーション点が前記干渉チェック手段により前記干渉点であると判断された場合には、前記第２候補判断手段により求めた前記干渉点隣接コンフィギュレーション点のうち、前記距離候補判断手段により現在位置から前記目標コンフィギュレーション点に近づくように前記新コンフィギュレーション点を採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む境界追従モード経路を生成し、
このようにして、前記直進モード経路および前記境界追従モード経路を逐次生成しながら前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することを特徴とする経路生成装置。
【請求項２】
前記距離候補判断手段は、前記目標コンフィギュレーション点に最も近づくようにコンフィギュレーション点を選定することを特徴とする請求項１に記載の経路生成装置。
【請求項３】
前記現在位置隣接コンフィギュレーション点のうち現在位置までに通過した経路を構成する前記経路記憶手段に格納された経路構成コンフィギュレーション点を参照して前記新コンフィギュレーション点として採択しないように除外するデッドロック回避手段をさらに備えたことを特徴とする請求項１または請求項２に記載の経路生成装置。
【請求項４】
初期コンフィギュレーション点から目標コンフィギュレーション点まで障害物を回避するように新コンフィギュレーション点を採択してロボットアームの動作経路を生成するコンフィギュレーション空間を用いた迷路探索法に基づく経路生成方法であって、
演算処理装置および記憶装置からなる制御装置を有する経路生成装置を使用して、
前記演算処理装置を構成する第１候補判断手段により、前記動作経路における現在位置に隣接する現在位置隣接コンフィギュレーション点を求めるＡステップと、
前記演算処理装置を構成する第２候補判断手段により、前記現在位置隣接コンフィギュレーション点のうち前記障害物との干渉点に隣接する干渉点隣接コンフィギュレーション点を求めるＢステップと、
前記動作経路における現在位置において、前記演算処理装置を構成する距離候補判断手段により前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点に近づくように選定された距離候補コンフィギュレーション点が、前記演算処理装置を構成する干渉チェック手段により前記障害物の干渉点でないと判断された場合には、当該距離候補コンフィギュレーション点を前記新コンフィギュレーション点として採択して前記記憶装置を構成する経路記憶手段に格納し当該新コンフィギュレーション点まで進む直進モード経路を生成するＣステップと、
前記動作経路における現在位置において、前記演算処理装置を構成する距離候補判断手段により前記現在位置隣接コンフィギュレーション点のうち現在位置から前記目標コンフィギュレーション点までの仮想直線経路に対して近づくように選定された距離候補コンフィギュレーション点が、干渉チェック手段により前記障害物の干渉点であると判断された場合には、前記第２候補判断手段により求めた前記干渉点隣接コンフィギュレーション点のうち、前記距離候補判断手段により現在位置から前記目標コンフィギュレーション点に近づくように前記新コンフィギュレーション点を採択して前記経路記憶手段に格納し当該新コンフィギュレーション点まで進む境界追従モード経路を生成するＤステップと、を含み、
前記Ｃステップにおける直進モード経路および前記Ｄステップにおける境界追従モード経路を逐次生成しながら前記初期コンフィギュレーション点から目標コンフィギュレーション点までの動作経路を生成することを特徴とする経路生成方法。
【請求項５】
前記距離候補判断手段は、前記仮想直線経路に対して最も近づくようにコンフィギュレーション点を選定することを特徴とする請求項４に記載の経路生成方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２０１２−５６０６４（Ｐ２０１２−５６０６４Ａ）
【公開日】平成２４年３月２２日（２０１２．３．２２）
【国際特許分類】
【出願番号】特願２０１０−２０４０９６（Ｐ２０１０−２０４０９６）
【出願日】平成２２年９月１３日（２０１０．９．１３）
【出願人】（０００１３２１６１）株式会社スギノマシン (144)
【Ｆターム（参考）】
【公開日】平成２４年３月２２日（２０１２．３．２２）
【国際特許分類】
【出願日】平成２２年９月１３日（２０１０．９．１３）
【出願人】（０００１３２１６１）株式会社スギノマシン (144)
【Ｆターム（参考）】
[ Back to top ]