(57)【要約】【課題】障害物と干渉するノードを含まない非干渉経路の算出確率を高めること。【解決手段】軌道計画装置１００は、エッジを介したノード間の接続により構成される経路情報を修正する。軌道計画装置１００は、環境情報に基づいて経路情報に含まれるノードの干渉性を判定する干渉判定部４２と、干渉判定部４２により干渉有りと判定されたノードの周囲に追加ノードを生成する追加ノード生成部７０と、干渉判定部４２による追加ノードの干渉性の判定結果に応じて、干渉有りと判定されたノードを追加ノードに置き換えるノード置換実行部４４と、を備える。【選択図】図１
2012190405.tif 000002
【特許請求の範囲】
【請求項１】
エッジを介したノード間の接続により構成される経路情報を修正する経路情報修正装置であって、
環境情報に基づいて前記経路情報に含まれる前記ノードの干渉性を判定する干渉判定部と、
前記干渉判定部により干渉有りと判定された前記ノードの周囲に追加ノードを生成する追加ノード生成部と、
前記干渉判定部による前記追加ノードの干渉性の判定結果に応じて、干渉有りと判定された前記ノードを前記追加ノードに置き換えるノード置換実行部と、
を備える、経路情報修正装置。
【請求項２】
前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定し、
前記追加ノード生成部は、連続しない態様にて干渉している前記ノードの周囲に前記追加ノードを生成することを特徴とする請求項１に記載の経路情報修正装置。
【請求項３】
前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定する、請求項１又は２に記載の経路情報修正装置であって、
当該経路情報修正装置は、前記干渉判定部により、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定された場合、連続的に干渉している各ノード及び当該ノードらに接続されたエッジの削除を指示する。
【請求項４】
前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定する、請求項１乃至３のいずれか一項に記載の経路情報修正装置であって、
当該経路情報修正装置は、前記干渉判定部により連続しない態様にて干渉していると判定された前記ノードに至るエッジのコストの増加を指示する。
【請求項５】
前記経路情報同士の一致性を判定する比較判定部を更に備え、
前記追加ノード生成部は、前記比較判定部によって時間的に連続して探索された前記経路情報同士が一致すると判定されたとき、前記干渉判定部により干渉有りと判定された前記ノードに基づいて前記追加ノードを生成することを特徴とする請求項１乃至４のいずれか一項に記載の経路情報修正装置。
【請求項６】
前記経路情報同士の一致性を判定する比較判定部を更に備え、
前記追加ノード生成部は、前記比較判定部によって時間的に連続して探索された前記経路情報同士が一致すると判定されたとき、前記干渉判定部により干渉有りと判定された前記ノードに基づいて前記追加ノードを生成する請求項１乃至５のいずれか一項に記載の経路情報修正装置であって、
前記追加ノード生成部は、所定数の前記追加ノードを生成し、
前記干渉判定部は、所定数の前記追加ノードに含まれる前記追加ノードの干渉性を順に判定する。
【請求項７】
前記追加ノード生成部は、所定関数の実行に基づいて、複数の前記追加ノードを生成することを特徴とする請求項１乃至６のいずれか一項に記載の経路情報修正装置。
【請求項８】
前記ノードは、複数の値から構成されることを特徴とする請求項１乃至７のいずれか一項に記載の経路情報修正装置。
【請求項９】
前記ノード置換実行部によって処理された前記経路情報に対して経路補間処理を実行する補間処理部を更に備え、
前記補間処理部は、前記経路情報に含まれる経路に対して所定間隔で離散点を設け、
前記干渉判定部は、前記環境情報に基づいて前記離散点の干渉性を判定することを特徴とする請求項１乃至８のいずれか一項に記載の経路情報修正装置。
【請求項１０】
コスト設定されたエッジを介して複数のノードが接続されたロードマップ情報を記憶するロードマップ記憶部と、
前記ロードマップ情報から得られるコスト情報の処理に基づいて、スタート及びゴールノード間の経路を探索する経路探索部と、
前記経路探索部から供給される前記経路情報を修正する請求項１乃至９のいずれか一項に記載の経路情報修正装置と、
を備える軌道計画装置。
【請求項１１】
前記環境情報を取得する環境情報取得部を更に備えることを特徴とする請求項１０に記載の軌道計画装置。
【請求項１２】
請求項１１に記載の軌道計画装置を内蔵するロボット。
【請求項１３】
エッジを介したノード間の接続により構成される経路情報を修正する経路情報修正装置の動作方法であって、
環境情報に基づいて前記経路情報に含まれる前記ノードの干渉性を判定し、
前記判定により干渉有りと判定された前記ノードの周囲に追加ノードを生成し、
前記判定による前記追加ノードの干渉性の判定結果に応じて、干渉有りと判定された前記ノードを前記追加ノードに置き換える、経路情報修正装置の動作方法。
【請求項１４】
請求項１３に記載の動作方法をコンピュータに実行させるプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、経路情報修正装置、軌道計画装置、及びロボットに関する。
【背景技術】
【０００２】
２次元／３次元空間におけるロボットの移動経路を算出する技術の進展が著しい。
【０００３】
特許文献１には、同文献の段落００１７に記載のように、地図生成部１２で生成された地図情報に対し、コーナー経路情報生成部１６が出力する情報を付加してグラフ表現の地図情報を修正する点が開示されている。なお、地図情報とは、同文献の段落００１５に記載のように、ノード、エッジからなるグラフ表現である。また、同文献の段落００１５には、軌道計画部１３は、地図生成部１２が生成した地図情報及びユーザより与えられる移動開始地点及び目的地の位置情報に基づいて、目的地までの経路情報を探索して経路列を生成する、と記載されている。同文献の段落００２０には、ノード情報には、図３に定義されるコーナー環境情報が含まれる点が記載されている。同文献の段落００２１には、同文献の図４乃至６を参照して算出処理する点が開示されている。同文献の段落００２８には、ステップ８までで求めた、通過領域、円弧経路中心位置および円弧経路終了点における移動ロボットの姿勢をノード情報に登録する、と記載されている。これらによって、より最適な経路を求めることが可能になる点が開示されている。
【０００４】
特許文献２には、ロボットの移動経路の算出に関する一例が開示されている。特許文献２の図４に示すように、現在位置と目標位置間の間には、ＲＲＴ（Rapidly-exploring Random Tree）により算出された複数のノードが設けられている。特許文献２の図４に示すように、ノード間は、バネ及びダンパにより接続され、ノード間に引力が作用し、軌跡が滑らかになる点が開示されている（例えば、特許文献１の段落００４４参照）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００７−３２８４８６号公報
【特許文献２】特開２００９−２１１５７１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
ノード間がエッジにより接続されたグラフ構造のロードマップを探索して経路を求める場合、予め用意されるノードが有限であるため、ロボットの周囲の障害物の存在状態によっては、障害物と干渉するノードを含まない非干渉経路を求めることが容易ではない場合があり得る。このように、障害物と干渉するノードを含まない非干渉経路の算出確率を高めることが強く望まれている。
【課題を解決するための手段】
【０００７】
本発明に係る経路情報修正装置は、エッジを介したノード間の接続により構成される経路情報を修正する経路情報修正装置であって、環境情報に基づいて前記経路情報に含まれる前記ノードの干渉性を判定する干渉判定部と、前記干渉判定部により干渉有りと判定された前記ノードの周囲に追加ノードを生成する追加ノード生成部と、前記干渉判定部による前記追加ノードの干渉性の判定結果に応じて、干渉有りと判定された前記ノードを前記追加ノードに置き換えるノード置換実行部と、を備える。
【０００８】
前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定し、前記追加ノード生成部は、連続しない態様にて干渉している前記ノードの周囲に前記追加ノードを生成する、と良い。
【０００９】
前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定する、上記の経路情報修正装置であって、当該経路情報修正装置は、前記干渉判定部により、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定された場合、連続的に干渉している各ノード及び当該ノードらに接続されたエッジの削除を指示する。
【００１０】
前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定する、上記いずれかの経路情報修正装置であって、当該経路情報修正装置は、前記干渉判定部により連続しない態様にて干渉していると判定された前記ノードに至るエッジのコストの増加を指示する。
【００１１】
前記経路情報同士の一致性を判定する比較判定部を更に備え、前記追加ノード生成部は、前記比較判定部によって時間的に連続して探索された前記経路情報同士が一致すると判定されたとき、前記干渉判定部により干渉有りと判定された前記ノードに基づいて前記追加ノードを生成する、と良い。
【００１２】
前記経路情報同士の一致性を判定する比較判定部を更に備え、前記追加ノード生成部は、前記比較判定部によって時間的に連続する前記経路情報同士が一致すると判定されたとき、前記干渉判定部により干渉有りと判定された前記ノードに基づいて前記追加ノードを生成する上記いずれかに記載の経路情報修正装置であって、前記追加ノード生成部は、所定数の前記追加ノードを生成し、前記干渉判定部は、所定数の前記追加ノードに含まれる前記追加ノードの干渉性を順に判定する、と良い。
【００１３】
前記追加ノード生成部は、所定関数の実行に基づいて、複数の前記追加ノードを生成する、と良い。前記ノードは、複数の値から構成される、と良い。前記ノード置換実行部によって処理された前記経路情報に対して経路補間処理を実行する補間処理部を更に備え、前記補間処理部は、前記経路情報に含まれる経路に対して所定間隔で離散点を設け、前記干渉判定部は、前記環境情報に基づいて前記離散点の干渉性を判定する、と良い。
【００１４】
本発明に係る軌道計画装置は、コスト設定されたエッジを介して複数のノードが接続されたロードマップ情報を記憶するロードマップ記憶部と、前記ロードマップ情報から得られるコスト情報の処理に基づいて、スタート及びゴールノード間の経路を探索する経路探索部と、前記経路探索部から供給される前記経路情報を修正する上記いずれかに記載の経路情報修正装置と、を備える。前記環境情報を取得する環境情報取得部を更に備える、と良い。
【００１５】
本発明に係るロボットは、上記した軌道計画装置を内蔵する。
【００１６】
本発明に係る経路情報修正装置は、エッジを介したノード間の接続により構成される経路情報を修正する経路情報修正装置の動作方法であって、環境情報に基づいて前記経路情報に含まれる前記ノードの干渉性を判定し、前記判定により干渉有りと判定された前記ノードの周囲に追加ノードを生成し、前記判定による前記追加ノードの干渉性の判定結果に応じて、干渉有りと判定された前記ノードを前記追加ノードに置き換える。
【００１７】
本発明に係るプログラムは、上記の動作方法をコンピュータに実行させる。
【発明の効果】
【００１８】
本発明によれば、障害物と干渉するノードを含まない非干渉経路の算出確率を高めることができる。
【図面の簡単な説明】
【００１９】
【図１】実施の形態１にかかる軌道計画装置の概略的なブロック図である。
【図２】実施の形態１にかかる軌道計画装置の動作を示す概略的なフローチャートである。
【図３】実施の形態１にかかる軌道計画装置の動作を示す概略的なフローチャートである。
【図４】実施の形態１にかかるロードマップにおいて探索された経路を示す模式図である。
【図５】実施の形態１にかかるロードマップにおいて探索された経路を示す模式図である。
【図６】実施の形態１にかかるロードマップにおいて探索された経路を示す模式図である。
【図７】実施の形態１にかかるロードマップにおいて探索された経路及び経路データを示す模式図である。
【図８】実施の形態１にかかるエッジに付加されたコストの増加処理を示す模式図である。
【図９】実施の形態１にかかる追加ノードの生成／追加のノードの干渉確認処理を示す模式図である。
【図１０】実施の形態１にかかる干渉ノードを追加ノードに置き換える処理を示す模式図である。
【図１１】実施の形態１にかかる補間処理を示す模式図である。
【図１２】実施の形態２にかかるロボットの模式図である。
【図１３】実施の形態２にかかるロボットの概略的な構成を示すブロック図である。
【図１４】実施の形態２にかかるロボットの動作を説明するための説明図である。
【発明を実施するための形態】
【００２０】
以下、図面を参照して実施の形態について説明する。なお、各実施の形態は、個々に独立したものではなく、互いに組み合わせ可能であり、また、その組み合わせに基づく効果も主張可能なものとする。図面は、説明のために用いられており、適宜、簡略化されている。
【００２１】
実施の形態１
本実施形態に係る軌道計画装置１００（図１参照）は、エッジを介したノード間の接続により構成される経路データを修正するものであり、環境情報に基づいて経路データに含まれるノードの干渉性を判定し、干渉有りと判定されたノード（以下、単に干渉ノードと呼ぶ場合がある）の周囲に追加ノードを生成し、追加ノードの干渉性の判定結果に応じて、元の干渉ノードを非干渉の追加ノードに置き換える（図１０参照：干渉ノードｑ３が非干渉の追加ノードｑｎ５に置き換えられている）。干渉ノードが非干渉の追加ノードに置きかえられて代替経路が提供されることによって、経路中に干渉ノードを含まない非干渉経路の算出確率を効果的に高めることができる。干渉ノードの周囲に追加ノードを設定することによって、多数のノードを事前に用意する必要を回避できる。ロボットの周囲環境が任意／随時に変化し、かつ、ロボットの移動態様の自由度が大きい場合であっても、その場の環境に応じて、適宜、ノード配置状態を修正することができ、非干渉経路の算出確率を効果的に高めることができる。なお、非干渉のノードとは、補足説明すれば、障害物に対してノードが干渉していないノードである。障害物に対してノードが干渉するか否かの判断手法／判断基準は任意であり、それは、軌道計画装置１００が組み込まれる装置に応じて適宜調整される。
【００２２】
なお、膨大な数のノードを事前に用意することでも非干渉経路の算出確率を高めることができる。しかしながら、この場合、ノード数及びエッジ数が増加するため、経路探索処理に要する計算コストが増大してしまうおそれがあり、全体としてのパフォーマンスを向上させることは難しくなるおそれがある。なお、計算コストとは、経路探索において解を求めることに要する計算処理の程度を示す。計算コストが大きい場合には、多量の計算処理が必要になる。計算コストが小さい場合には、少量の計算処理で済む。
【００２３】
以下、図面を参照しつつ具体的に説明する。なお、以下では、多関節ロボットアームの軌道を計画することを念頭において説明するが、本願発明は、これに限らず、他の軌道計画（例えば、ロボットが空間移動する際の軌道計画）に適用しても良い。算出される軌道は、２次元／３次元空間のいずれに適用され得る。軌道上のノードにより指標される情報は、任意であり、軌道が適用される装置等に応じて適宜設定される。以下では、情報とデータとを区別することなく説明する場合がある。
【００２４】
図１は、軌道計画装置の概略的なブロック図である。図２及び図３は、軌道計画装置の動作を示す概略的なフローチャートである。図４乃至図６は、探索された経路を示す模式図である。図７は、探索された経路及び経路データを示す模式図である。図８は、エッジに付加されるコストの増加処理を示す模式図である。図９は、追加ノードの生成／追加のノードの干渉確認処理を示す模式図である。図１０は、干渉ノードを追加ノードに置き換える処理を示す模式図である。図１１は、補間処理を示す模式図である。
【００２５】
図１に示すように、軌道計画装置１００は、ロードマップ記憶部１０、経路供給部２０、環境情報取得部３０、経路修正部４０、軌道補間部５０、経路一致判定部６０、および追加ノード生成部７０を含む。経路供給部２０は、ロードマップ一時記憶部２２、ノード探索部２４、および経路探索部２６を含む。経路修正部４０は、干渉判定部４２、及びノード置換実行部４４を含む。経路一致判定部６０は、経路一時記憶部６２、及び経路比較部６４を含む。ロードマップ一時記憶部２２は、ロードマップを記憶するという意味においてロードマップ記憶部１０と同等であるため、ロードマップ記憶部と単に把握する場合もある。
【００２６】
軌道計画装置１００は、ロボットに組み込まれている。より具体的には、ロードマップ記憶部１０を除いた計算機部分８０と環境情報取得部３０がロボットに組み込まれている。ロードマップ記憶部１０は、当該ロボットに対して、ネットワーク接続されている。適当なタイミングで所要のロードマップデータがネットワークを介してロボットのロードマップ一時記憶部２２にダウンロードされる。
【００２７】
軌道計画装置１００は、コンピュータにより構成され、ＣＰＵ(Central Processing Unit)等の演算処理器がメモリ等の記憶装置に記憶されたプログラムを実行することによって所定の機能を発揮する。プログラムの実行過程では、ＲＡＭ(Random Access Memory)等に様々なデータが書き込まれ、ＣＰＵによる演算処理に用いられたりする。ＣＰＵとＲＡＭ間は、バス等を介して接続され、当該バスには、ＲＡＭ等のメモリアクセスを制御するメモリコントローラも接続される。なお、コンピュータの具体的なアーキテクチャは任意である。軌道計画装置１００を構成するコンピュータに対してインストールされるプログラムは、適宜、ハードディスク、光学記憶媒体（ＣＤ、ＤＶＤ、ＢＤ等）等の任意の記憶手段に記憶される。
【００２８】
ロードマップ記憶部１０は、グラフ構造のロードマップを記憶する記憶措置である。グラフ構造のロードマップは、エッジ（枝と呼ぶ場合もある）を介して多数のノード（頂点と呼ぶ場合もある）が接続されたデータである。グラフ構造のロードマップは、図４に示されている。但し、図４では、ロードマップ記憶部１０からロードマップ一時記憶部２２にダウンロードされた部分的なロードマップである。ロードマップ記憶部１０に記憶されたロードマップは、ダウンロードされたロードマップよりもノード及びエッジ数が比較にならないほど多い。
【００２９】
ノードは、ロボットアームの姿勢を示す。ロボットアームのモーター軸数がＮのとき、ノードｑｃは、次式から明らかなように、関節角ベクトルｑｃ１〜ｑｃＮによって表現される。
ｑｃ＝（ｑｃ１，ｑｃ２，・・・，ｑｃＮ）  ・・・式（１）
【００３０】
エッジは、ノード間を接続し、ノード間の接続関係を示す。ノード間のノルム（距離と呼ぶ場合がある）が閾値以下／閾値未満のとき、それらのノード間にはエッジが接続される。エッジには、コストが付与されている。経路探索は、エッジに付与されたコストの演算処理によって実行される。例えば、コストの加算値が最少の経路が経路候補として算出される。コストは、例えば、姿勢変化に要求されるエネルギー量、時間等に応じて付与される。
【００３１】
ロードマップ記憶部１０に記憶されるロードマップの生成方法は任意である。例えば、ノードの表現式、エッジの生成条件等の各種条件をコンピュータに入力することによってコンピュータによって自動生成される。
【００３２】
経路供給部２０は、初期姿勢及び最終姿勢に基づいて、それらの間を接続する経路を決定して供給する。なお、初期姿勢及び最終姿勢は、任意の手段によって算出されて、経路供給部２０に供給されるものとする。初期姿勢及び最終姿勢は、例えば、ノードと同様に、式（１）によって表現される。
【００３３】
ロードマップ一時記憶部２２には、ロードマップ記憶部１０に記憶されたロードマップが部分的にダウンロードされたデータを記憶する。ロードマップ一時記憶部２２に記憶されたロードマップは、ノード探索部２４と経路探索部２６へ供給される。後述の説明から明らかなように、ロードマップ一時記憶部２２にロードマップを一時的に記憶することによって、ロードマップ一時記憶部２２に記憶されたロードマップ自体を修正することが可能になる。例えば、不要なノードの削除、不要なエッジの削除が可能になる。これによって、異なる条件下において、経路探索部２６によって再度経路探索することが可能となり、代替的な経路の迅速提供等が可能となる。
【００３４】
ノード探索部２４は、初期姿勢に対応するノードをロードマップから探索する。同様に、最終姿勢に対応するノードをロードマップから探索する。探索処理後、ノード探索部２４は、初期姿勢に対応するスタートノードと最終姿勢に対応するゴールノードとを経路探索部２６へ出力する。
【００３５】
経路探索部２６は、スタートノードとゴールノード間を接続する経路をロードマップから探索する。上述のように、ロードマップに含まれる各エッジにはコストが設定されている。経路探索部２６は、エッジに付与されたコストの加算値が最小となる経路を求め、この経路を出力する。
【００３６】
環境情報取得部３０は、ロボットアームの周囲環境の３次元計測を実行して、３次元データを取得するセンシング機能部である。３次元計測の具体的な手段は任意である。環境情報取得部３０に含まれるセンサは、例えば、ステレオカメラ、レーザーレンジファインダ、又はデプスメジャーである。ＴＯＦ(Time Of Flight)の原理に基づいて、前方の３次元状態を検出しても良い。なお、ＴＯＦに基づく測距方法は、検査光の発光タイミングから反射光の受光タイミング間の時間間隔を検知することによって、検査光の出射方向の奥行きを計測する方法である。ステレオカメラのように、複数の撮像系で取得した複数の画像の処理に基づいて奥行き情報を得ても良い。環境情報取得部３０は、センサ出力により取得される３次元情報をモデル化し、モデル化したデータを経路修正部４０に対して供給する。なお、モデル化は、例えば、検知された３次元形状に対して幾何モデルを適応することによって行われる。
【００３７】
経路修正部４０は、後述の説明から明らかなように、経路修正処理に加えて、その他様々な処理を実行する。例えば、経路修正部４０は、環境情報取得部３０から供給されたデータに基づいて、経路供給部２０から供給された経路に干渉ノードが含まれるか否かを判定する。経路修正部４０は、ロードマップ一時記憶部２２に対してノード及びエッジの削除を指示する。経路修正部４０は、軌道補間部５０に対して経路データを提供する。経路修正部４０は、経路一時記憶部６２に対して経路データを供給し、その書き込みを指示する。経路修正部４０は、経路比較部６４に対して、経路データを供給し、これと経路一時記憶部６２に記憶された経路データとの比較を指示する。経路修正部４０は、追加ノード生成部７０から供給される経路データに含まれる追加ノードの干渉性を判断する。経路修正部４０は、干渉ノードを非干渉の追加ノード置き換える処理も実行する。なお、経路修正部４０の動作の詳細は、後述の説明から明らかになる。
【００３８】
干渉判定部４２は、経路供給部２０から供給された経路に含まれるノードが、環境情報取得部３０から供給されるデータから検知される障害物に対して干渉するか否かを判定する。干渉判定部４２は、追加ノード生成部７０から供給される経路に含まれる追加ノードが、環境情報取得部３０から供給されるデータから検知される障害物に対して干渉するか否かを判定する。
【００３９】
ノード置換実行部４４は、後述の説明から明らかなように、追加ノードの生成対象となった干渉ノードを非干渉の追加ノードに置き換える処理を実行する。ノード置換実行部４４の動作の詳細は、後述の説明から明らかになる。
【００４０】
軌道補間部５０は、経路を円滑化する処理をする。経路一致判定部６０は、経路供給部２０から連続して供給される経路が一致するか否かを判定する。経路一時記憶部６２は、前回供給された経路を一時的に記憶する。経路比較部６４は、今回供給された経路と前回供給された経路とを比較し、両者が一致するか否かを判定する。経路比較部６４は、両者が一致するとき、その経路データを追加ノード生成部７０へ出力し、両者が異なるとき、不一致信号を経路修正部４０へ出力する。追加ノード生成部７０は、対象となる干渉ノードの周囲に追加ノードを生成し、追加ノードが追加された経路を経路修正部４０へ供給する。なお、軌道補間部５０〜追加ノード生成部７０の動作の詳細は、後述の説明から明らかになる。  
【００４１】
図２及び図３を参照して、軌道計画装置１００の動作手順について説明する。適宜、図４乃至図１１を併せて参照して説明する。図４乃至図１０では、実線が探索された経路を示し、破線が探索されなかった経路を示す。一点鎖線は、環境情報取得部３０の出力から検知される障害物が存在する領域を示す。
【００４２】
まず、環境情報を取得する（Ｓ１００）。具体的には、環境情報取得部３０は、センサ系を駆動して環境情報を取得し、取得データを情報処理して出力する。これによって、ロボットアームが可動する空間の障害物位置が検知される。
【００４３】
次に、軌道計画する（Ｓ１０１）。具体的には、経路供給部２０は、初期及び最終姿勢間を接続する経路を探索する。ノード探索部２４は、まず、初期姿勢に対応するノードを探索し、最終姿勢に対応するノードを探索し、そして、これらの探索により得られたスタート及びゴールノードを経路探索部２６に供給する。経路探索部２６は、コスト計算に基づいて、スタート及びゴールノード間を接続する経路を探索する。経路探索部２６は、コストが最小となる経路を求め、これを経路修正部４０へ出力する。なお、経路探索の具体的な手法は任意であり、コスト計算処理に基づく手法に限られるべきものではない。
【００４４】
軌道計画が成功したとき（Ｓ１０２）、干渉チェックをする（Ｓ１０３）。
【００４５】
軌道計画が成功しているか否かは、障害物に対するノードの干渉性を考慮することなく行われる。例えば、図４に模式的に示すように経路が探索された場合であっても、軌道計画が成功したものと判定される。なお、経路探索部２６が経路の出力の有無によって、軌道計画が成功したか否かが決定される。
【００４６】
経路探索部２６から経路が供給されると、干渉判定部４２は、その経路の構成ノードの干渉性を判定する（Ｓ１０３）。
【００４７】
今回探索された経路の構成ノードが障害物と干渉していない場合、補間処理（Ｓ１０４）が行われる。具体的には、軌道補間部５０は、経路修正部４０から供給される経路に対して一定間隔Δｑで離散点を設ける（図１１（ａ）参照）。
【００４８】
次に、最終確認をする（Ｓ１０５）。具体的には、干渉判定部４２は、軌道補間部５０から供給された経路上のノード及び離散点が障害物と干渉しているか否かを判定する（図１１（ｂ）の点参照）。離散点が障害物と干渉しているかを判定することによって、ロボットアームが障害物と接触することをより確実に回避することができる。また、経路修正部４０は、ロボットアームが許容する条件（例えば、ロボットアームの関節の可動角度）を満足するか否かを判定する。ロボットアームの関節の可動角度以外の場合、ロボットアームは、その経路に沿って動き得ないため、このような無意味な経路を排除する。
【００４９】
最終確認がＯＫの場合、経路修正部４０は、最終的な経路を軌道データとして出力する。最終確認がＮＧの場合、不要なエッジを削除する（Ｓ１０６）。不要なエッジは、例えば、障害物と干渉しているノードに至るエッジ、障害物と干渉している離散点が含まれるエッジ等である。経路修正部４０は、ロードマップ一時記憶部２２に対して、上述の不要なエッジを削除することを指示する。ロードマップ一時記憶部２２は、経路修正部４０からの指示によって、当該指示により特定されるエッジを削除する。一時的に記憶されたロードマップ自体を修正／変更することによって、軌道計画（Ｓ１０１）によって再び探索される経路がより的確なものとなる可能性を効果的に高めることができる。なお、スタートノードとゴールノード間を接続する経路が存在しない場合（Ｓ１０２）、軌道計画が失敗となる。
【００５０】
障害物に対して干渉しているノードが経路に含まれるとき（Ｓ１０３）、追加処理（Ｓ１０７）へ移行する。この追加処理（Ｓ１０７）を経て、最終的な経路が生成され、又はＳ１０１へ戻ることになる。追加処理（Ｓ１０７）の詳細を図３に示す。
【００５１】
図３に示すように、干渉チェックがＮＧのとき（Ｓ１０３）、ノードが連続して干渉しているか否かを判定する（Ｓ２００）。具体的には、干渉判定部４２は、エッジを介して連続するノード夫々が障害物に干渉しているか否かを判定する。
【００５２】
例えば、図４に示す経路が供給されると、干渉判定部４２は、ノードｑ１が障害物に干渉していることを検知し（Ｓ１０３）、次に、ノードｑ３が障害物に干渉していることを検知する（Ｓ２００）。
【００５３】
ノードが連続して干渉しているとき、干渉ノード／関連エッジを削除する（Ｓ２１１）。具体的には、経路修正部４０は、干渉ノード及びこれに接続されたエッジを除去することをロードマップ一時記憶部２２に対して指示する。ロードマップ一時記憶部２２は、経路修正部４０からの指示に応じて、この指示により特定されるノード及びエッジを除去する。次サイクルのＳ１０１では、干渉ノード及びこれに至るエッジが存在しない条件で経路探索される。
【００５４】
例えば、図４に示す場合、経路修正部４０は、ノードｑ１、ｑ３及びこれらに接続されたエッジｅ１０〜ｅ１５の除去をロードマップ一時記憶部２２に対して指示する。ロードマップ一時記憶部２２は、経路修正部４０からの指示に応じて、指定されたノード及びエッジを除去する。
【００５５】
ノードが連続干渉していないとき、保存データ有りか否かを判定する（Ｓ２０１）。具体的には、経路修正部４０は、経路一時記憶部６２に一時的に記憶された経路データがあるか否か判断する。なお、初期状態のとき、経路一時記憶部６２には、何ら経路データが記憶されていない。従って、ノードが連続して干渉していないことが初めて検知された場合、ステップ２０９へ移行することになる。
【００５６】
経路が保存されていない場合、今回探索された経路を一時保存する（Ｓ２０９）。具体的には、経路修正部４０は、経路探索部２６から供給された経路データを経路一時記憶部６２に保存する。
【００５７】
例えば、図５及び図６に示す場合、ノードが連続して干渉していない。従って、このような経路が初めて探索された場合、経路修正部４０は、経路一時記憶部６２に一時的にこの経路データを保存させる。
【００５８】
なお、経路データの保存態様は任意であるが、経路に含まれるノード列を記憶することが簡便である。図７（ａ）に示すように、スタートノード、ノードｑ２、ノードｑ３、ノードｑ６、ゴールノードの順で経路が形成されたとき、図７（ｂ）に示すように、その順番に並べられたノード名を記憶すればよい。
【００５９】
次に、移動コストを増加する（Ｓ２１０）。具体的には、経路修正部４０は、干渉ノードに接続されたエッジに付与されたコストの増加をロードマップ一時記憶部２２に対して指示する。これを受けて、ロードマップ一時記憶部２２は、その指示により特定されたエッジのコストを増加する。これによって、今回探索された経路以外の他の経路が探索される可能性が高められる。
【００６０】
例えば、図８に示すように、ノードｑ３が干渉ノードのとき、ノードｑ２とノードｑ３間のエッジのコストを増加させる。具体的には、ノードｑ３とノードｑ２間のエッジに付与されたコストを倍増する。図８（ａ）と図８（ｂ）の対比から明らかなように、ノードｑ２とノードｑ３間のエッジに付与されたコストが７０から１４０になっている。このように干渉ノードに至るエッジのコストを増加することによって、当該干渉ノードが経路に含まれない確率を高めることができ、他の代替経路が探索される可能性を高めることができる。
【００６１】
保存経路がある場合（Ｓ２０１）、今回探索された経路と前回の経路とを比較し、両者が一致しているか否かを判断する（Ｓ２０２）。具体的には、経路比較部６４には、経路一時記憶部６２に記憶された前回探索された経路と、今回探索された経路とを比較する。なお、経路の一致性の具体的な判断手法は任意である。例えば、経路に含まれるノード総数及びノード順番に基づいて経路の一致性は判断される。今回探索された経路は、経路修正部４０から経路比較部６４へ供給され、前回探索された経路は、経路一時記憶部６２から経路比較部６４へ供給される。
【００６２】
比較対象とした経路が一致していないとき、今回探索された経路を一時保存する（Ｓ２０９）。具体的には、経路修正部４０は、今回探索された経路を経路一時記憶部６２に記憶する。これによって、経路一時記憶部６２には、最新の経路が記憶されることになる。なお、経路修正部４０は、経路比較部６４から供給される不一致信号に応じて、経路一時記憶部６２に対して経路データの更新を指示する。
【００６３】
比較対象の経路が一致しているとき、追加ノードを生成する（Ｓ２０３）。具体的には、経路比較部６４は、一致判定された経路データを追加ノード生成部７０に対して供給し、追加ノード生成部７０は、干渉と判定されたノードの周囲に所定数の追加ノードを追加する。なお、追加ノード生成部７０により追加される追加ノード数は任意であり、複数に限らず、単数であっても良い。なお、追加ノードの生成数を所定数に限定することによって、計算コストの極端な増加を回避することができる。
【００６４】
追加ノードの生成方法は任意である。例えば、式（２）により規定される追加ノードは、干渉ノードの構成値を式（２）に代入することによって算出される。干渉ノードがｑｃ＝（ｑｃ１，ｑｃ２，・・・，ｑｃＮ）で表現されるものとする。ｑｃ１を式（３）に代入することで、ｑｉ１が算出される。ｑｃ２を式（３）に代入することで、ｑｉ２が算出される。これをｑｃＮまで繰り返すことでｑｉＮが算出され、追加ノードの値が定まる。なお、式（３）に含まれる関数ｒａｎｄ（）は、−１＜ｒａｎｄ（）＜１の条件を満足する値をランダムに出力する関数である。
ｑｉ＝（ｑｉ１，ｑｉ２，・・・，ｑｉＮ）  ・・・式（２）
ｑｉｊ＝ｑｃｊ（１＋σ×ｒａｎｄ（））  ・・・式（３）
【００６５】
ステップＳ２００〜２０２を経てステップ２０３に至る場合は、干渉ノードが連続しない態様にて経路上に存在し、かつ、干渉ノードに至るエッジに付与されたコストを増加しても代替的な経路が探索されない場合である。このような場合、干渉ノードの位置を適切に修正して当該ノードを非干渉状態とすることで経路全体が有効化できる可能性がある。本実施形態では、この点に鑑みて、干渉ノードの周囲に追加ノードを配置し、この追加ノードの干渉性を判断し、干渉ノードを非干渉の追加ノードで置き換える。これによって、干渉ノードを経路中に含まない非干渉経路の算出確率を効果的に高めることができる。
【００６６】
追加ノード生成後、非干渉の追加ノードの有無を判断する（Ｓ２０４）。具体的には、干渉判定部４２は、追加ノード生成部７０により生成された追加ノードが、環境情報取得部３０から供給されるデータから検知される障害物に対して干渉するか否かを判定する。
【００６７】
例えば、図９に示す場合、追加ノード生成部７０は、ノードｑ３の周囲に追加ノードｑｎ１〜ｑｎＮを生成する。干渉判定部４２は、ノードｑｎ１から干渉性を順に判定し、ノードｑｎ５が非干渉ノードであることを検知する。なお、図９（ａ）は、ノードｑ３の周囲に追加ノードが生成されたことを模式的に示し、図９（ｂ）は、ノードｑ３の周囲のｑｎ５以外は干渉ノードであることを模式的に示す。
【００６８】
非干渉の追加ノードがない場合は、追加ノードの生成をもってしても障害物回避できない場合に相当する。この場合、追加ノードの生成対象となった干渉ノード及びこれに対して接続されたエッジを削除する（ステップＳ２１１）。ノード及びエッジの削除は、経路修正部４０により指示され、ロードマップ一時記憶部２２により実行される。その後、ステップＳ１０１へ帰還する。
【００６９】
非干渉追加ノードがある場合、置換処理を実行する（Ｓ２０５）。具体的には、図１０に模式的に示すように、経路修正部４０は、追加ノードの生成対象となった干渉ノードを非干渉の追加ノードへ置き換える。まず、ノード置換実行部４４は、図１０（ａ）に示すように、干渉ノードｑ３及びこれに接続されたエッジを削除する。次に、ノード置換実行部４４は、図１０（ｂ）に示すように、追加ノードｑｎ５に対してエッジを生成する。具体的には、ノードｑ２と追加ノードｑｎ５をエッジにより接続し、追加ノードｑｎ５とノードｑ６間をエッジにより接続する。このようにして、ノードｑ３が追加ノードｑｎ５に置き換えられる。経路修正部４０は、修正後の経路を軌道補間部５０へ供給する。
【００７０】
次に、補間処理する（Ｓ２０６）。具体的には、軌道補間部５０は、経路修正部４０から供給された経路に対して所定間隔Δｑで離散点を設ける（図１１（ａ）参照）。
【００７１】
次に、最終確認をする（Ｓ２０７）。具体的には、干渉判定部４２は、軌道補間部５０から供給された経路上のノード及び離散点が障害物と干渉しているか否かを判定する（図１１（ｂ）参照）。離散点が障害物と干渉しているかを判定することによって、ロボットアームが障害物と接触することをより確実に回避することができる。また、経路修正部４０は、ロボットアームが許容する条件（例えば、ロボットアームの関節の可動角度）を満足するか否かを判定する。ロボットアームの関節の可動角度以外の場合、ロボットアームは、その経路に沿って動き得ないため、このような無意味な経路を排除する。
【００７２】
最終確認ＮＧの場合、干渉ノードが置き換えられた非干渉の追加ノードを削除する（Ｓ２０８）。具体的には、経路修正部４０は、ノード置換実行部４４により導入された追加ノードを削除する。次に、Ｓ２０４へ移行し、その余の追加ノードに非干渉の追加ノードが存在するか判定される。その後の処理は、上述の説明と同様である。
【００７３】
上述の説明から明らかなように、本実施形態では、ノードが連続して干渉しているか否かを判断基準として経路を選別する。ノードが連続して干渉している場合、干渉ノードの位置を調整したとしても障害物を回避できる可能性は高くないと推定できる。従って、この場合、干渉ノード自体をロードマップから削除し、他の代替的なルートを探索することを可能とする。これにより、相対的な短い時間で経路探索することが可能になる場合が見込まれる。
【００７４】
更に、本実施形態では、連続して探索される経路の一致性を判断し、一致しているときに初めて追加ノードに基づく経路修正ステップ（端的には、Ｓ２０３）へ移行する。連続して探索される経路が一致しない場合には、エッジに付与されたコストの値を調整し、再度の経路探索を実行することで、他の経路が探索されることが見込まれる。これにより、相対的な短い時間で代替的な経路を探索することが可能になる場合が見込まれる。
【００７５】
また、本実施形態では、干渉ノードの構成値を所定関数に代入することにより追加ノードを算出する。これによって、複雑な計算を伴うことなく、追加ノードを算出することができる。追加ノード生成の対象となる干渉ノードの周囲に複数の追加ノードを輪状に生成することによって、障害物回避の可能性を高めることができる。なお、追加ノードの生成態様は任意である。例えば、対象となる干渉ノードを中心として球状に追加ノードを生成しても良い。追加ノードの生成数を所定数に限定することによって、計算コストの極端な増加も回避することかできる。
【００７６】
実施の形態２
図１２乃至図１４を参照して実施の形態２について説明する。図１２は、ロボットの模式図である。図１３は、ロボットの概略的な構成を示すブロック図である。図１４は、ロボットの動作を説明するための説明図である。
【００７７】
実施の形態１に示した軌道計画装置１００は、例えば、図１２に示すロボット３００に内蔵される。図１２に示すロボット３００は、頭部及び車輪部が設けられた胴部３０１、胴部３０１に対して設けられたアーム部３０２、およびアーム部３０２の先端に設けられたハンド部３０３を有する。アーム部３０２の関節には電気モーター等の駆動部が設けられており、アーム部３０２は関節運動可能に構成されている。なお、アーム部３０２の運動は、胴部３０１に内蔵するコンピュータにより制御される。
【００７８】
図１２に示すロボット３００は、図１３に示すシステム構成を含む。図１３に示すように、図１に示した軌道計測装置１００に対応する軌道計画部１００の出力は、駆動制御部２００へ供給され、駆動制御部２００の出力は、駆動部２１０へ供給される。
【００７９】
駆動制御部２００は、軌道計画部１０から供給される経路データに応じて、駆動部２１０を制御する。図１１に示した経路データが供給されると、駆動制御部２００は、順次、離散点／ノードにより指定される姿勢となるように駆動部２１０を制御する。駆動部２１０は、駆動制御部２００により制御され、これにより、ロボット３００のアーム部３０２の運動が実現される。図１４に模式的に示すように、ロボット３００のハンド部３０３は、経路上の進行し、ノードｑｎ５、ｑ６を介して、把持対象物を把持することができるゴールノードにより指定される位置まで移動する。本実施形態でも、実施の形態１で説明したものと同様の効果を得ることができる。
【００８０】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。ノードにより指定される対象は、３次元的な姿勢に限られるべきではなく、２次元空間上の位置であっても良い。ロボット以外の様々な機械／装置に適用し得る。
【符号の説明】
【００８１】
１００    軌道計画装置
１０    ロードマップ記憶部
２０    経路供給部
２２    ロードマップ一時記憶部
２４    接続ノード探索部
２６    経路探索部
３０    環境情報取得部
４０    経路修正部
４２    干渉判定部
４４    ノード置換実行部
５０    軌道補間部
６０    経路一致判定部
６２    経路一時記憶部
６４    経路比較部
７０    追加ノード生成部
８０    計算機部分
２００    駆動制御部
２１０    駆動部
３００    ロボット
３０１    胴部
３０２    アーム部
３０３    ハンド部
【図１】
2012190405.tif 000003
【図２】
2012190405.tif 000004
【図３】
2012190405.tif 000005
【図４】
2012190405.tif 000006
【図５】
2012190405.tif 000007
【図６】
2012190405.tif 000008
【図７】
2012190405.tif 000009
【図８】
2012190405.tif 000010
【図９】
2012190405.tif 000011
【図１０】
2012190405.tif 000012
【図１１】
2012190405.tif 000013
【図１２】
2012190405.tif 000014
【図１３】
2012190405.tif 000015
【図１４】
2012190405.tif 000016