"エッジを介したノード間の接続により構成される経路情報を修正する経路情報修正装置であって、環境情報に基づいて前記経路情報に含まれる前記ノードの干渉性を判定する干渉判定部と、前記干渉判定部により干渉有りと判定された前記ノードの周囲に追加ノードを生成する追加ノード生成部と、前記干渉判定部による前記追加ノードの干渉性の判定結果に応じて、干渉有りと判定された前記ノードを前記追加ノードに置き換えるノード置換実行部と、を備える、経路情報修正装置。前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定し、前記追加ノード生成部は、連続しない態様にて干渉している前記ノードの周囲に前記追加ノードを生成することを特徴とする請求項1に記載の経路情報修正装置。前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定する、請求項1又は2に記載の経路情報修正装置であって、当該経路情報修正装置は、前記干渉判定部により、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定された場合、連続的に干渉している各ノード及び当該ノードらに接続されたエッジの削除を指示する。前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定する、請求項1乃至3のいずれか一項に記載の経路情報修正装置であって、当該経路情報修正装置は、前記干渉判定部により連続しない態様にて干渉していると判定された前記ノードに至るエッジのコストの増加を指示する。前記経路情報同士の一致性を判定する比較判定部を更に備え、前記追加ノード生成部は、前記比較判定部によって時間的に連続して探索された前記経路情報同士が一致すると判定されたとき、前記干渉判定部により干渉有りと判定された前記ノードに基づいて前記追加ノードを生成することを特徴とする請求項1乃至4のいずれか一項に記載の経路情報修正装置。前記経路情報同士の一致性を判定する比較判定部を更に備え、前記追加ノード生成部は、前記比較判定部によって時間的に連続して探索された前記経路情報同士が一致すると判定されたとき、前記干渉判定部により干渉有りと判定された前記ノードに基づいて前記追加ノードを生成する請求項1乃至5のいずれか一項に記載の経路情報修正装置であって、前記追加ノード生成部は、所定数の前記追加ノードを生成し、前記干渉判定部は、所定数の前記追加ノードに含まれる前記追加ノードの干渉性を順に判定する。前記追加ノード生成部は、所定関数の実行に基づいて、複数の前記追加ノードを生成することを特徴とする請求項1乃至6のいずれか一項に記載の経路情報修正装置。前記ノードは、複数の値から構成されることを特徴とする請求項1乃至7のいずれか一項に記載の経路情報修正装置。前記ノード置換実行部によって処理された前記経路情報に対して経路補間処理を実行する補間処理部を更に備え、前記補間処理部は、前記経路情報に含まれる経路に対して所定間隔で離散点を設け、前記干渉判定部は、前記環境情報に基づいて前記離散点の干渉性を判定することを特徴とする請求項1乃至8のいずれか一項に記載の経路情報修正装置。コスト設定されたエッジを介して複数のノードが接続されたロードマップ情報を記憶するロードマップ記憶部と、前記ロードマップ情報から得られるコスト情報の処理に基づいて、スタート及びゴールノード間の経路を探索する経路探索部と、前記経路探索部から供給される前記経路情報を修正する請求項1乃至9のいずれか一項に記載の経路情報修正装置と、を備える軌道計画装置。前記環境情報を取得する環境情報取得部を更に備えることを特徴とする請求項10に記載の軌道計画装置。請求項11に記載の軌道計画装置を内蔵するロボット。エッジを介したノード間の接続により構成される経路情報を修正する経路情報修正装置の動作方法であって、環境情報に基づいて前記経路情報に含まれる前記ノードの干渉性を判定し、前記判定により干渉有りと判定された前記ノードの周囲に追加ノードを生成し、前記判定による前記追加ノードの干渉性の判定結果に応じて、干渉有りと判定された前記ノードを前記追加ノードに置き換える、経路情報修正装置の動作方法。請求項13に記載の動作方法をコンピュータに実行させるプログラム。ノード間がエッジにより接続されたグラフ構造のロードマップを探索して経路を求める場合、予め用意されるノードが有限であるため、ロボットの周囲の障害物の存在状態によっては、障害物と干渉するノードを含まない非干渉経路を求めることが容易ではない場合があり得る。このように、障害物と干渉するノードを含まない非干渉経路の算出確率を高めることが強く望まれている。本発明に係る経路情報修正装置は、エッジを介したノード間の接続により構成される経路情報を修正する経路情報修正装置であって、環境情報に基づいて前記経路情報に含まれる前記ノードの干渉性を判定する干渉判定部と、前記干渉判定部により干渉有りと判定された前記ノードの周囲に追加ノードを生成する追加ノード生成部と、前記干渉判定部による前記追加ノードの干渉性の判定結果に応じて、干渉有りと判定された前記ノードを前記追加ノードに置き換えるノード置換実行部と、を備える。前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定し、前記追加ノード生成部は、連続しない態様にて干渉している前記ノードの周囲に前記追加ノードを生成する、と良い。前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定する、上記の経路情報修正装置であって、当該経路情報修正装置は、前記干渉判定部により、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定された場合、連続的に干渉している各ノード及び当該ノードらに接続されたエッジの削除を指示する。前記干渉判定部は、前記干渉有りと判定された前記ノードにエッジを介して隣接するノードも干渉有りと判定されたか否かを判定する、上記いずれかの経路情報修正装置であって、当該経路情報修正装置は、前記干渉判定部により連続しない態様にて干渉していると判定された前記ノードに至るエッジのコストの増加を指示する。前記経路情報同士の一致性を判定する比較判定部を更に備え、前記追加ノード生成部は、前記比較判定部によって時間的に連続して探索された前記経路情報同士が一致すると判定されたとき、前記干渉判定部により干渉有りと判定された前記ノードに基づいて前記追加ノードを生成する、と良い。前記経路情報同士の一致性を判定する比較判定部を更に備え、前記追加ノード生成部は、前記比較判定部によって時間的に連続する前記経路情報同士が一致すると判定されたとき、前記干渉判定部により干渉有りと判定された前記ノードに基づいて前記追加ノードを生成する上記いずれかに記載の経路情報修正装置であって、前記追加ノード生成部は、所定数の前記追加ノードを生成し、前記干渉判定部は、所定数の前記追加ノードに含まれる前記追加ノードの干渉性を順に判定する、と良い。前記追加ノード生成部は、所定関数の実行に基づいて、複数の前記追加ノードを生成する、と良い。前記ノードは、複数の値から構成される、と良い。前記ノード置換実行部によって処理された前記経路情報に対して経路補間処理を実行する補間処理部を更に備え、前記補間処理部は、前記経路情報に含まれる経路に対して所定間隔で離散点を設け、前記干渉判定部は、前記環境情報に基づいて前記離散点の干渉性を判定する、と良い。本発明に係る軌道計画装置は、コスト設定されたエッジを介して複数のノードが接続されたロードマップ情報を記憶するロードマップ記憶部と、前記ロードマップ情報から得られるコスト情報の処理に基づいて、スタート及びゴールノード間の経路を探索する経路探索部と、前記経路探索部から供給される前記経路情報を修正する上記いずれかに記載の経路情報修正装置と、を備える。前記環境情報を取得する環境情報取得部を更に備える、と良い。本発明に係るロボットは、上記した軌道計画装置を内蔵する。本発明に係る経路情報修正装置は、エッジを介したノード間の接続により構成される経路情報を修正する経路情報修正装置の動作方法であって、環境情報に基づいて前記経路情報に含まれる前記ノードの干渉性を判定し、前記判定により干渉有りと判定された前記ノードの周囲に追加ノードを生成し、前記判定による前記追加ノードの干渉性の判定結果に応じて、干渉有りと判定された前記ノードを前記追加ノードに置き換える。本発明に係るプログラムは、上記の動作方法をコンピュータに実行させる。本発明によれば、障害物と干渉するノードを含まない非干渉経路の算出確率を高めることができる。以下、図面を参照して実施の形態について説明する。なお、各実施の形態は、個々に独立したものではなく、互いに組み合わせ可能であり、また、その組み合わせに基づく効果も主張可能なものとする。図面は、説明のために用いられており、適宜、簡略化されている。実施の形態1本実施形態に係る軌道計画装置100図1参照は、エッジを介したノード間の接続により構成される経路データを修正するものであり、環境情報に基づいて経路データに含まれるノードの干渉性を判定し、干渉有りと判定されたノード以下、単に干渉ノードと呼ぶ場合があるの周囲に追加ノードを生成し、追加ノードの干渉性の判定結果に応じて、元の干渉ノードを非干渉の追加ノードに置き換える図10参照:干渉ノードq3が非干渉の追加ノードqn5に置き換えられている。干渉ノードが非干渉の追加ノードに置きかえられて代替経路が提供されることによって、経路中に干渉ノードを含まない非干渉経路の算出確率を効果的に高めることができる。干渉ノードの周囲に追加ノードを設定することによって、多数のノードを事前に用意する必要を回避できる。ロボットの周囲環境が任意/随時に変化し、かつ、ロボットの移動態様の自由度が大きい場合であっても、その場の環境に応じて、適宜、ノード配置状態を修正することができ、非干渉経路の算出確率を効果的に高めることができる。なお、非干渉のノードとは、補足説明すれば、障害物に対してノードが干渉していないノードである。障害物に対してノードが干渉するか否かの判断手法/判断基準は任意であり、それは、軌道計画装置100が組み込まれる装置に応じて適宜調整される。なお、膨大な数のノードを事前に用意することでも非干渉経路の算出確率を高めることができる。しかしながら、この場合、ノード数及びエッジ数が増加するため、経路探索処理に要する計算コストが増大してしまうおそれがあり、全体としてのパフォーマンスを向上させることは難しくなるおそれがある。なお、計算コストとは、経路探索において解を求めることに要する計算処理の程度を示す。計算コストが大きい場合には、多量の計算処理が必要になる。計算コストが小さい場合には、少量の計算処理で済む。以下、図面を参照しつつ具体的に説明する。なお、以下では、多関節ロボットアームの軌道を計画することを念頭において説明するが、本願発明は、これに限らず、他の軌道計画例えば、ロボットが空間移動する際の軌道計画に適用しても良い。算出される軌道は、2次元/3次元空間のいずれに適用され得る。軌道上のノードにより指標される情報は、任意であり、軌道が適用される装置等に応じて適宜設定される。以下では、情報とデータとを区別することなく説明する場合がある。図1は、軌道計画装置の概略的なブロック図である。図2及び図3は、軌道計画装置の動作を示す概略的なフローチャートである。図4乃至図6は、探索された経路を示す模式図である。図7は、探索された経路及び経路データを示す模式図である。図8は、エッジに付加されるコストの増加処理を示す模式図である。図9は、追加ノードの生成/追加のノードの干渉確認処理を示す模式図である。図10は、干渉ノードを追加ノードに置き換える処理を示す模式図である。図11は、補間処理を示す模式図である。図1に示すように、軌道計画装置100は、ロードマップ記憶部10、経路供給部20、環境情報取得部30、経路修正部40、軌道補間部50、経路一致判定部60、および追加ノード生成部70を含む。経路供給部20は、ロードマップ一時記憶部22、ノード探索部24、および経路探索部26を含む。経路修正部40は、干渉判定部42、及びノード置換実行部44を含む。経路一致判定部60は、経路一時記憶部62、及び経路比較部64を含む。ロードマップ一時記憶部22は、ロードマップを記憶するという意味においてロードマップ記憶部10と同等であるため、ロードマップ記憶部と単に把握する場合もある。軌道計画装置100は、ロボットに組み込まれている。より具体的には、ロードマップ記憶部10を除いた計算機部分80と環境情報取得部30がロボットに組み込まれている。ロードマップ記憶部10は、当該ロボットに対して、ネットワーク接続されている。適当なタイミングで所要のロードマップデータがネットワークを介してロボットのロードマップ一時記憶部22にダウンロードされる。軌道計画装置100は、コンピュータにより構成され、CPU等の演算処理器がメモリ等の記憶装置に記憶されたプログラムを実行することによって所定の機能を発揮する。プログラムの実行過程では、RAM等に様々なデータが書き込まれ、CPUによる演算処理に用いられたりする。CPUとRAM間は、バス等を介して接続され、当該バスには、RAM等のメモリアクセスを制御するメモリコントローラも接続される。なお、コンピュータの具体的なアーキテクチャは任意である。軌道計画装置100を構成するコンピュータに対してインストールされるプログラムは、適宜、ハードディスク、光学記憶媒体CD、DVD、BD等等の任意の記憶手段に記憶される。ロードマップ記憶部10は、グラフ構造のロードマップを記憶する記憶措置である。グラフ構造のロードマップは、エッジ枝と呼ぶ場合もあるを介して多数のノード頂点と呼ぶ場合もあるが接続されたデータである。グラフ構造のロードマップは、図4に示されている。但し、図4では、ロードマップ記憶部10からロードマップ一時記憶部22にダウンロードされた部分的なロードマップである。ロードマップ記憶部10に記憶されたロードマップは、ダウンロードされたロードマップよりもノード及びエッジ数が比較にならないほど多い。ノードは、ロボットアームの姿勢を示す。ロボットアームのモーター軸数がNのとき、ノードqcは、次式から明らかなように、関節角ベクトルqc1~qcNによって表現される。qc=qc1,qc2,,qcN式1エッジは、ノード間を接続し、ノード間の接続関係を示す。ノード間のノルム距離と呼ぶ場合があるが閾値以下/閾値未満のとき、それらのノード間にはエッジが接続される。エッジには、コストが付与されている。経路探索は、エッジに付与されたコストの演算処理によって実行される。例えば、コストの加算値が最少の経路が経路候補として算出される。コストは、例えば、姿勢変化に要求されるエネルギー量、時間等に応じて付与される。ロードマップ記憶部10に記憶されるロードマップの生成方法は任意である。例えば、ノードの表現式、エッジの生成条件等の各種条件をコンピュータに入力することによってコンピュータによって自動生成される。経路供給部20は、初期姿勢及び最終姿勢に基づいて、それらの間を接続する経路を決定して供給する。なお、初期姿勢及び最終姿勢は、任意の手段によって算出されて、経路供給部20に供給されるものとする。初期姿勢及び最終姿勢は、例えば、ノードと同様に、式1によって表現される。ロードマップ一時記憶部22には、ロードマップ記憶部10に記憶されたロードマップが部分的にダウンロードされたデータを記憶する。ロードマップ一時記憶部22に記憶されたロードマップは、ノード探索部24と経路探索部26へ供給される。後述の説明から明らかなように、ロードマップ一時記憶部22にロードマップを一時的に記憶することによって、ロードマップ一時記憶部22に記憶されたロードマップ自体を修正することが可能になる。例えば、不要なノードの削除、不要なエッジの削除が可能になる。これによって、異なる条件下において、経路探索部26によって再度経路探索することが可能となり、代替的な経路の迅速提供等が可能となる。ノード探索部24は、初期姿勢に対応するノードをロードマップから探索する。同様に、最終姿勢に対応するノードをロードマップから探索する。探索処理後、ノード探索部24は、初期姿勢に対応するスタートノードと最終姿勢に対応するゴールノードとを経路探索部26へ出力する。経路探索部26は、スタートノードとゴールノード間を接続する経路をロードマップから探索する。上述のように、ロードマップに含まれる各エッジにはコストが設定されている。経路探索部26は、エッジに付与されたコストの加算値が最小となる経路を求め、この経路を出力する。環境情報取得部30は、ロボットアームの周囲環境の3次元計測を実行して、3次元データを取得するセンシング機能部である。3次元計測の具体的な手段は任意である。環境情報取得部30に含まれるセンサは、例えば、ステレオカメラ、レーザーレンジファインダ、又はデプスメジャーである。TOFの原理に基づいて、前方の3次元状態を検出しても良い。なお、TOFに基づく測距方法は、検査光の発光タイミングから反射光の受光タイミング間の時間間隔を検知することによって、検査光の出射方向の奥行きを計測する方法である。ステレオカメラのように、複数の撮像系で取得した複数の画像の処理に基づいて奥行き情報を得ても良い。環境情報取得部30は、センサ出力により取得される3次元情報をモデル化し、モデル化したデータを経路修正部40に対して供給する。なお、モデル化は、例えば、検知された3次元形状に対して幾何モデルを適応することによって行われる。経路修正部40は、後述の説明から明らかなように、経路修正処理に加えて、その他様々な処理を実行する。例えば、経路修正部40は、環境情報取得部30から供給されたデータに基づいて、経路供給部20から供給された経路に干渉ノードが含まれるか否かを判定する。経路修正部40は、ロードマップ一時記憶部22に対してノード及びエッジの削除を指示する。経路修正部40は、軌道補間部50に対して経路データを提供する。経路修正部40は、経路一時記憶部62に対して経路データを供給し、その書き込みを指示する。経路修正部40は、経路比較部64に対して、経路データを供給し、これと経路一時記憶部62に記憶された経路データとの比較を指示する。経路修正部40は、追加ノード生成部70から供給される経路データに含まれる追加ノードの干渉性を判断する。経路修正部40は、干渉ノードを非干渉の追加ノード置き換える処理も実行する。なお、経路修正部40の動作の詳細は、後述の説明から明らかになる。干渉判定部42は、経路供給部20から供給された経路に含まれるノードが、環境情報取得部30から供給されるデータから検知される障害物に対して干渉するか否かを判定する。干渉判定部42は、追加ノード生成部70から供給される経路に含まれる追加ノードが、環境情報取得部30から供給されるデータから検知される障害物に対して干渉するか否かを判定する。ノード置換実行部44は、後述の説明から明らかなように、追加ノードの生成対象となった干渉ノードを非干渉の追加ノードに置き換える処理を実行する。ノード置換実行部44の動作の詳細は、後述の説明から明らかになる。軌道補間部50は、経路を円滑化する処理をする。経路一致判定部60は、経路供給部20から連続して供給される経路が一致するか否かを判定する。経路一時記憶部62は、前回供給された経路を一時的に記憶する。経路比較部64は、今回供給された経路と前回供給された経路とを比較し、両者が一致するか否かを判定する。経路比較部64は、両者が一致するとき、その経路データを追加ノード生成部70へ出力し、両者が異なるとき、不一致信号を経路修正部40へ出力する。追加ノード生成部70は、対象となる干渉ノードの周囲に追加ノードを生成し、追加ノードが追加された経路を経路修正部40へ供給する。なお、軌道補間部50~追加ノード生成部70の動作の詳細は、後述の説明から明らかになる。図2及び図3を参照して、軌道計画装置100の動作手順について説明する。適宜、図4乃至図11を併せて参照して説明する。図4乃至図10では、実線が探索された経路を示し、破線が探索されなかった経路を示す。一点鎖線は、環境情報取得部30の出力から検知される障害物が存在する領域を示す。まず、環境情報を取得するS100。具体的には、環境情報取得部30は、センサ系を駆動して環境情報を取得し、取得データを情報処理して出力する。これによって、ロボットアームが可動する空間の障害物位置が検知される。次に、軌道計画するS101。具体的には、経路供給部20は、初期及び最終姿勢間を接続する経路を探索する。ノード探索部24は、まず、初期姿勢に対応するノードを探索し、最終姿勢に対応するノードを探索し、そして、これらの探索により得られたスタート及びゴールノードを経路探索部26に供給する。経路探索部26は、コスト計算に基づいて、スタート及びゴールノード間を接続する経路を探索する。経路探索部26は、コストが最小となる経路を求め、これを経路修正部40へ出力する。なお、経路探索の具体的な手法は任意であり、コスト計算処理に基づく手法に限られるべきものではない。軌道計画が成功したときS102、干渉チェックをするS103。軌道計画が成功しているか否かは、障害物に対するノードの干渉性を考慮することなく行われる。例えば、図4に模式的に示すように経路が探索された場合であっても、軌道計画が成功したものと判定される。なお、経路探索部26が経路の出力の有無によって、軌道計画が成功したか否かが決定される。経路探索部26から経路が供給されると、干渉判定部42は、その経路の構成ノードの干渉性を判定するS103。今回探索された経路の構成ノードが障害物と干渉していない場合、補間処理S104が行われる。具体的には、軌道補間部50は、経路修正部40から供給される経路に対して一定間隔Δqで離散点を設ける図11a参照。次に、最終確認をするS105。具体的には、干渉判定部42は、軌道補間部50から供給された経路上のノード及び離散点が障害物と干渉しているか否かを判定する図11bの点参照。離散点が障害物と干渉しているかを判定することによって、ロボットアームが障害物と接触することをより確実に回避することができる。また、経路修正部40は、ロボットアームが許容する条件例えば、ロボットアームの関節の可動角度を満足するか否かを判定する。ロボットアームの関節の可動角度以外の場合、ロボットアームは、その経路に沿って動き得ないため、このような無意味な経路を排除する。最終確認がOKの場合、経路修正部40は、最終的な経路を軌道データとして出力する。最終確認がNGの場合、不要なエッジを削除するS106。不要なエッジは、例えば、障害物と干渉しているノードに至るエッジ、障害物と干渉している離散点が含まれるエッジ等である。経路修正部40は、ロードマップ一時記憶部22に対して、上述の不要なエッジを削除することを指示する。ロードマップ一時記憶部22は、経路修正部40からの指示によって、当該指示により特定されるエッジを削除する。一時的に記憶されたロードマップ自体を修正/変更することによって、軌道計画S101によって再び探索される経路がより的確なものとなる可能性を効果的に高めることができる。なお、スタートノードとゴールノード間を接続する経路が存在しない場合S102、軌道計画が失敗となる。障害物に対して干渉しているノードが経路に含まれるときS103、追加処理S107へ移行する。この追加処理S107を経て、最終的な経路が生成され、又はS101へ戻ることになる。追加処理S107の詳細を図3に示す。図3に示すように、干渉チェックがNGのときS103、ノードが連続して干渉しているか否かを判定するS200。具体的には、干渉判定部42は、エッジを介して連続するノード夫々が障害物に干渉しているか否かを判定する。例えば、図4に示す経路が供給されると、干渉判定部42は、ノードq1が障害物に干渉していることを検知しS103、次に、ノードq3が障害物に干渉していることを検知するS200。ノードが連続して干渉しているとき、干渉ノード/関連エッジを削除するS211。具体的には、経路修正部40は、干渉ノード及びこれに接続されたエッジを除去することをロードマップ一時記憶部22に対して指示する。ロードマップ一時記憶部22は、経路修正部40からの指示に応じて、この指示により特定されるノード及びエッジを除去する。次サイクルのS101では、干渉ノード及びこれに至るエッジが存在しない条件で経路探索される。例えば、図4に示す場合、経路修正部40は、ノードq1、q3及びこれらに接続されたエッジe10~e15の除去をロードマップ一時記憶部22に対して指示する。ロードマップ一時記憶部22は、経路修正部40からの指示に応じて、指定されたノード及びエッジを除去する。ノードが連続干渉していないとき、保存データ有りか否かを判定するS201。具体的には、経路修正部40は、経路一時記憶部62に一時的に記憶された経路データがあるか否か判断する。なお、初期状態のとき、経路一時記憶部62には、何ら経路データが記憶されていない。従って、ノードが連続して干渉していないことが初めて検知された場合、ステップ209へ移行することになる。経路が保存されていない場合、今回探索された経路を一時保存するS209。具体的には、経路修正部40は、経路探索部26から供給された経路データを経路一時記憶部62に保存する。例えば、図5及び図6に示す場合、ノードが連続して干渉していない。従って、このような経路が初めて探索された場合、経路修正部40は、経路一時記憶部62に一時的にこの経路データを保存させる。なお、経路データの保存態様は任意であるが、経路に含まれるノード列を記憶することが簡便である。図7aに示すように、スタートノード、ノードq2、ノードq3、ノードq6、ゴールノードの順で経路が形成されたとき、図7bに示すように、その順番に並べられたノード名を記憶すればよい。次に、移動コストを増加するS210。具体的には、経路修正部40は、干渉ノードに接続されたエッジに付与されたコストの増加をロードマップ一時記憶部22に対して指示する。これを受けて、ロードマップ一時記憶部22は、その指示により特定されたエッジのコストを増加する。これによって、今回探索された経路以外の他の経路が探索される可能性が高められる。例えば、図8に示すように、ノードq3が干渉ノードのとき、ノードq2とノードq3間のエッジのコストを増加させる。具体的には、ノードq3とノードq2間のエッジに付与されたコストを倍増する。図8aと図8bの対比から明らかなように、ノードq2とノードq3間のエッジに付与されたコストが70から140になっている。このように干渉ノードに至るエッジのコストを増加することによって、当該干渉ノードが経路に含まれない確率を高めることができ、他の代替経路が探索される可能性を高めることができる。保存経路がある場合S201、今回探索された経路と前回の経路とを比較し、両者が一致しているか否かを判断するS202。具体的には、経路比較部64には、経路一時記憶部62に記憶された前回探索された経路と、今回探索された経路とを比較する。なお、経路の一致性の具体的な判断手法は任意である。例えば、経路に含まれるノード総数及びノード順番に基づいて経路の一致性は判断される。今回探索された経路は、経路修正部40から経路比較部64へ供給され、前回探索された経路は、経路一時記憶部62から経路比較部64へ供給される。比較対象とした経路が一致していないとき、今回探索された経路を一時保存するS209。具体的には、経路修正部40は、今回探索された経路を経路一時記憶部62に記憶する。これによって、経路一時記憶部62には、最新の経路が記憶されることになる。なお、経路修正部40は、経路比較部64から供給される不一致信号に応じて、経路一時記憶部62に対して経路データの更新を指示する。比較対象の経路が一致しているとき、追加ノードを生成するS203。具体的には、経路比較部64は、一致判定された経路データを追加ノード生成部70に対して供給し、追加ノード生成部70は、干渉と判定されたノードの周囲に所定数の追加ノードを追加する。なお、追加ノード生成部70により追加される追加ノード数は任意であり、複数に限らず、単数であっても良い。なお、追加ノードの生成数を所定数に限定することによって、計算コストの極端な増加を回避することができる。追加ノードの生成方法は任意である。例えば、式2により規定される追加ノードは、干渉ノードの構成値を式2に代入することによって算出される。干渉ノードがqc=qc1,qc2,,qcNで表現されるものとする。qc1を式3に代入することで、qi1が算出される。qc2を式3に代入することで、qi2が算出される。これをqcNまで繰り返すことでqiNが算出され、追加ノードの値が定まる。なお、式3に含まれる関数randは、-1<rand<1の条件を満足する値をランダムに出力する関数である。qi=qi1,qi2,,qiN式2qij=qcj1σ×rand式3ステップS200~202を経てステップ203に至る場合は、干渉ノードが連続しない態様にて経路上に存在し、かつ、干渉ノードに至るエッジに付与されたコストを増加しても代替的な経路が探索されない場合である。このような場合、干渉ノードの位置を適切に修正して当該ノードを非干渉状態とすることで経路全体が有効化できる可能性がある。本実施形態では、この点に鑑みて、干渉ノードの周囲に追加ノードを配置し、この追加ノードの干渉性を判断し、干渉ノードを非干渉の追加ノードで置き換える。これによって、干渉ノードを経路中に含まない非干渉経路の算出確率を効果的に高めることができる。追加ノード生成後、非干渉の追加ノードの有無を判断するS204。具体的には、干渉判定部42は、追加ノード生成部70により生成された追加ノードが、環境情報取得部30から供給されるデータから検知される障害物に対して干渉するか否かを判定する。例えば、図9に示す場合、追加ノード生成部70は、ノードq3の周囲に追加ノードqn1~qnNを生成する。干渉判定部42は、ノードqn1から干渉性を順に判定し、ノードqn5が非干渉ノードであることを検知する。なお、図9aは、ノードq3の周囲に追加ノードが生成されたことを模式的に示し、図9bは、ノードq3の周囲のqn5以外は干渉ノードであることを模式的に示す。非干渉の追加ノードがない場合は、追加ノードの生成をもってしても障害物回避できない場合に相当する。この場合、追加ノードの生成対象となった干渉ノード及びこれに対して接続されたエッジを削除するステップS211。ノード及びエッジの削除は、経路修正部40により指示され、ロードマップ一時記憶部22により実行される。その後、ステップS101へ帰還する。非干渉追加ノードがある場合、置換処理を実行するS205。具体的には、図10に模式的に示すように、経路修正部40は、追加ノードの生成対象となった干渉ノードを非干渉の追加ノードへ置き換える。まず、ノード置換実行部44は、図10aに示すように、干渉ノードq3及びこれに接続されたエッジを削除する。次に、ノード置換実行部44は、図10bに示すように、追加ノードqn5に対してエッジを生成する。具体的には、ノードq2と追加ノードqn5をエッジにより接続し、追加ノードqn5とノードq6間をエッジにより接続する。このようにして、ノードq3が追加ノードqn5に置き換えられる。経路修正部40は、修正後の経路を軌道補間部50へ供給する。次に、補間処理するS206。具体的には、軌道補間部50は、経路修正部40から供給された経路に対して所定間隔Δqで離散点を設ける図11a参照。次に、最終確認をするS207。具体的には、干渉判定部42は、軌道補間部50から供給された経路上のノード及び離散点が障害物と干渉しているか否かを判定する図11b参照。離散点が障害物と干渉しているかを判定することによって、ロボットアームが障害物と接触することをより確実に回避することができる。また、経路修正部40は、ロボットアームが許容する条件例えば、ロボットアームの関節の可動角度を満足するか否かを判定する。ロボットアームの関節の可動角度以外の場合、ロボットアームは、その経路に沿って動き得ないため、このような無意味な経路を排除する。最終確認NGの場合、干渉ノードが置き換えられた非干渉の追加ノードを削除するS208。具体的には、経路修正部40は、ノード置換実行部44により導入された追加ノードを削除する。次に、S204へ移行し、その余の追加ノードに非干渉の追加ノードが存在するか判定される。その後の処理は、上述の説明と同様である。上述の説明から明らかなように、本実施形態では、ノードが連続して干渉しているか否かを判断基準として経路を選別する。ノードが連続して干渉している場合、干渉ノードの位置を調整したとしても障害物を回避できる可能性は高くないと推定できる。従って、この場合、干渉ノード自体をロードマップから削除し、他の代替的なルートを探索することを可能とする。これにより、相対的な短い時間で経路探索することが可能になる場合が見込まれる。更に、本実施形態では、連続して探索される経路の一致性を判断し、一致しているときに初めて追加ノードに基づく経路修正ステップ端的には、S203へ移行する。連続して探索される経路が一致しない場合には、エッジに付与されたコストの値を調整し、再度の経路探索を実行することで、他の経路が探索されることが見込まれる。これにより、相対的な短い時間で代替的な経路を探索することが可能になる場合が見込まれる。また、本実施形態では、干渉ノードの構成値を所定関数に代入することにより追加ノードを算出する。これによって、複雑な計算を伴うことなく、追加ノードを算出することができる。追加ノード生成の対象となる干渉ノードの周囲に複数の追加ノードを輪状に生成することによって、障害物回避の可能性を高めることができる。なお、追加ノードの生成態様は任意である。例えば、対象となる干渉ノードを中心として球状に追加ノードを生成しても良い。追加ノードの生成数を所定数に限定することによって、計算コストの極端な増加も回避することかできる。実施の形態2図12乃至図14を参照して実施の形態2について説明する。図12は、ロボットの模式図である。図13は、ロボットの概略的な構成を示すブロック図である。図14は、ロボットの動作を説明するための説明図である。実施の形態1に示した軌道計画装置100は、例えば、図12に示すロボット300に内蔵される。図12に示すロボット300は、頭部及び車輪部が設けられた胴部301、胴部301に対して設けられたアーム部302、およびアーム部302の先端に設けられたハンド部303を有する。アーム部302の関節には電気モーター等の駆動部が設けられており、アーム部302は関節運動可能に構成されている。なお、アーム部302の運動は、胴部301に内蔵するコンピュータにより制御される。図12に示すロボット300は、図13に示すシステム構成を含む。図13に示すように、図1に示した軌道計測装置100に対応する軌道計画部100の出力は、駆動制御部200へ供給され、駆動制御部200の出力は、駆動部210へ供給される。駆動制御部200は、軌道計画部10から供給される経路データに応じて、駆動部210を制御する。図11に示した経路データが供給されると、駆動制御部200は、順次、離散点/ノードにより指定される姿勢となるように駆動部210を制御する。駆動部210は、駆動制御部200により制御され、これにより、ロボット300のアーム部302の運動が実現される。図14に模式的に示すように、ロボット300のハンド部303は、経路上の進行し、ノードqn5、q6を介して、把持対象物を把持することができるゴールノードにより指定される位置まで移動する。本実施形態でも、実施の形態1で説明したものと同様の効果を得ることができる。なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。ノードにより指定される対象は、3次元的な姿勢に限られるべきではなく、2次元空間上の位置であっても良い。ロボット以外の様々な機械/装置に適用し得る。"