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