(57)【要約】【課題】処理負荷を軽減しつつ経路ステップサイズを自動的に求めることができる経路作成装置を提供することを課題とする。【解決手段】ノードを繋ぐ線分を順次接続することによりロボットが障害物を回避する経路を作成する経路作成装置１であって、探索ステップサイズを計算するためのノードの個数を設定する個数設定手段５と、設定された個数分のノードの分散情報に基づいて探索ステップサイズを計算する探索ステップサイズ計算手段６と、計算された探索ステップサイズに応じてノードを探索する探索手段４，６と、探索されたノードを繋ぐ線分を接続し、経路を作成する線分接続手段７とを備えることを特徴とする。【選択図】図１
2008204161.tif 000002
【特許請求の範囲】
【請求項１】
ノードを繋ぐ線分を順次接続することによりロボットが障害物を回避する経路を作成する経路作成装置であって、
探索ステップサイズを計算するためのノードの個数を設定する個数設定手段と、
前記個数設定手段で設定された個数分のノードの分散情報に基づいて探索ステップサイズを計算する探索ステップサイズ計算手段と、
前記探索ステップサイズ計算手段で計算された探索ステップサイズに応じてノードを探索する探索手段と、
前記探索手段で探索されたノードを繋ぐ線分を接続し、経路を作成する線分接続手段と
を備えることを特徴とする経路作成装置。
【請求項２】
前記個数設定手段は、前記探索ステップサイズ計算手段で計算された探索ステップサイズが所定の範囲外の場合に個数を変更することを特徴とする請求項１に記載する経路作成装置。
【請求項３】
ノードとノードを繋ぐ線分からなるツリー構造を記憶する記憶手段を備え、
前記探索ステップサイズ計算手段は、前記記憶手段のツリー構造から前記個数設定手段で設定された個数分のノードを抽出し、当該抽出したノードの分散情報に基づいて探索ステップサイズを計算することを特徴とする請求項１又は請求項２に記載する経路作成装置。
【請求項４】
前記探索ステップサイズ計算手段は、前記個数設定手段で設定された個数分のノードの共分散行列を計算し、当該共分散行列を用いたマハラノビス距離により探索ステップサイズを計算することを特徴とする請求項１〜請求項３に記載する経路作成装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットが障害物を回避する経路を作成する経路作成装置に関する。
【背景技術】
【０００２】
近年、各種産業用ロボットや人型ロボットなどの様々なロボットが開発されている。例えば、ロボットには、多数の関節を有し、関節間がリンクで連結され、各関節の動作によって多数の自由度を持つロボットがある。このようなロボットを動作させる場合、初期位置姿勢と目標位置姿勢が与えられ、初期位置姿勢から目標位置姿勢に至る経路を作成し、その経路に基づいてロボットの各関節を動作させる。さらに、このようなロボット全体を２次元平面内などで移動させるものもある。
【０００３】
このようなロボットを動作や移動させる場合、ロボットの周辺に障害物が存在すると、この障害物を回避しながらロボットを動作や移動させるための経路の作成は非常に難しくなる。例えば、周囲の障害物で形成される狭い空間を通り抜けるような経路を作成するような場合がある。このような経路の作成方法としては、一定の経路ステップサイズを設定し、その探索ステップサイズ毎に障害物と干渉しない経路を探索し、その探索した経路ステップサイズ毎の経路を接続しながら全経路を作成する（特許文献１参照）。
【０００４】
しかし、経路ステップサイズが一定値の場合、ロボットの大きさ、形状、位置と障害物の大きさ、形状、位置との相対関係（例えば、ロボットの大きさや形状と上記した周囲の障害物による狭い空間との相対関係）により、経路ステップサイズが大き過ぎて障害物を回避できる経路を作成できない場合がある。そこで、経路の作成方法として、障害物を回避できる経路を作成可能な経路ステップサイズに自動的に更新し、更新した経路ステップサイズ毎に経路を作成するものがある（非特許文献１参照）。この経路作成方法では、経路を作成しようとする領域内に複数の点を生成し、障害物と生成された点との境界の情報を取得し、経路を作成しようとする領域を複数の円で近似して経路を作成する。特に、この経路作成方法では、その領域を各点との距離によって分割したボロノイ領域を設定し、障害物との境界に位置する点を中心とする円によってボロノイ領域を近似し、この円の半径を障害物との干渉回数によって一定割合αで徐々に増減する。
【特許文献１】特開平７−６４６３３号公報
【非特許文献１】Ｌ．Ｊａｉｌｌｅｔ  Ａ．Ｙｅｒｓｈｏｖａ  Ｓ．ＬａＶａｌｌｅ，Ｔ．Ｓｉｍｅｏｎ，Ａｄａｐｔｉｖｅ  Ｔｕｎｉｎｇ  ｏｆ  ｔｈｅ  Ｓａｍｐｌｉｎｇ  Ｄｏｍａｉｎ  ｆｏｒ  Ｄｙｎａｍｉｃ−Ｄｏｍａｉｎ  ＲＲＴｓ，ＩＥＥＥ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｆｅｒｅｎｃｅ  ｏｎ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ｒｏｂｏｔｓ  ａｎｄ  Ｓｙｓｔｅｍｓ  ２００５
【発明の開示】
【発明が解決しようとする課題】
【０００５】
ロボットなどを実用化するには、リアルタイム性が要求される。しかし、上記経路作成方法の場合、経路を作成する領域の次元が高くなったり、ロボットの自由度が大きくなったりあるいは障害物の形状が複雑になると、経路ステップサイズを求める処理負荷が大きくなる。これは、一様乱数を生成すべき領域の形状を、随時、保持しながら一様乱数を生成することは非常に複雑な処理を要するからである。そのため、処理負荷が増大し、リアルタイムでの処理ができなくなり、実用化に適さない。
【０００６】
そこで、本発明は、処理負荷を軽減しつつ経路ステップサイズを自動的に求めることができる経路作成装置を提供することを課題とする。
【課題を解決するための手段】
【０００７】
本発明に係る経路作成装置は、ノードを繋ぐ線分を順次接続することによりロボットが障害物を回避する経路を作成する経路作成装置であって、探索ステップサイズを計算するためのノードの個数を設定する個数設定手段と、個数設定手段で設定された個数分のノードの分散情報に基づいて探索ステップサイズを計算する探索ステップサイズ計算手段と、探索ステップサイズ計算手段で計算された探索ステップサイズに応じてノードを探索する探索手段と、探索手段で探索されたノードを繋ぐ線分を接続し、経路を作成する線分接続手段とを備えることを特徴とする。
【０００８】
この経路作成装置では、個数設定手段により、経路を探索していくための探索ステップサイズを計算するためのノードの個数を設定する。そして、経路作成装置では、探索ステップサイズ計算手段により、その設定された個数分のノード（探索済みのノード）の分散情報に基づいて探索ステップサイズを計算する。さらに、経路作成装置では、探索手段により探索ステップサイズに応じてノードを探索し、線分接続手段により探索されたノードを繋ぐ線分を接続しながら経路を作成する。ノードが密集しているほど、ロボットと障害物との相対的関係によってロボットが動作や移動し難い状況であり、探索ステップサイズを小さくする必要がある。一方、ノードが分散しているほど、ロボットと障害物との相対的関係においてロボットが動作や移動を自由にできる状況であり、探索ステップサイズを大きくできる。このように、経路作成装置では、ノードの分散状況に応じて探索ステップサイズを自動的に求めることができる。さらに、経路作成装置では、所定の個数分のノードの分散情報から探索ステップサイズを計算するだけなので、処理が容易であり、処理負荷を軽減できる。その結果、この経路作成装置では、リアルタイム処理も可能となる。また、経路作成装置では、自動的に探索ステップサイズを更新することができるので、障害物を回避しながら目標位置姿勢に至る経路を作成することができる。
【０００９】
なお、ロボットには、関節などによって動作する一般的なロボット以外にも、移動体（自動車など）も含むものとする。したがって、経路には、移動するだけの経路も含むものとする。
【００１０】
本発明の上記経路作成装置では、個数設定手段は、探索ステップサイズ計算手段で計算された探索ステップサイズが所定の範囲外の場合に個数を変更する構成としてもよい。
【００１１】
この経路作成装置では、個数設定手段により、計算された探索ステップサイズが所定の範囲外の場合に個数を変更する。これによって、ノードが密集してきた場合でも、個数を多くすることによって探索ステップサイズを大きくことが可能であり、多くの個数のノードにより分散を大きくできる。その結果、ノードが密集し過ぎることによってロボットが動作や移動できないような経路の作成を防止できる。
【００１２】
本発明の上記経路作成装置では、ノードとノードを繋ぐ線分からなるツリー構造を記憶する記憶手段を備え、探索ステップサイズ計算手段は、記憶手段のツリー構造から個数設定手段で設定された個数分のノードを抽出し、当該抽出したノードの分散情報に基づいて探索ステップサイズを計算する構成としてもよい。
【００１３】
この経路作成装置では、記憶手段に経路を作成するためのノードとノード間の線分からなるツリー構造を保持する。そして、経路作成装置では、探索ステップサイズ計算手段により、記憶手段に保持されているツリー構造から設定された個数分のノードを抽出し、その抽出したノードの分散情報に基づいて探索ステップサイズを計算する。これによって、経路の途中に対応する部分のツリーにおけるノード情報を記憶手段から常時取得することができ、そのノード情報から分散情報を求めることができる。
【００１４】
本発明の上記経路作成装置では、探索ステップサイズ計算手段は、個数設定手段で設定された個数分のノードの共分散行列を計算し、当該共分散行列を用いたマハラノビス距離により探索ステップサイズを計算する構成としてもよい。
【００１５】
この経路作成装置では、探索ステップサイズ計算手段により、設定された個数分のノードの共分散行列を計算し、その共分散行列を用いたマハラノビス距離により探索ステップサイズを計算する。このように、分散情報として共分散行列によるマハラノビス距離を適用することにより、簡単に探索ステップサイズを求めることができる。さらに、マハラノビス距離を用いることにより、ノードが密集している場合には小さい探索ステップサイズを計算でき、ノードが分散している場合には大きい探索ステップサイズを計算できる。
【発明の効果】
【００１６】
本発明は、所定の個数のノードの分散情報を用いて探索ステップサイズを計算することにより、処理負荷を軽減しつつ経路ステップサイズを自動的に求めることができる。
【発明を実施するための最良の形態】
【００１７】
以下、図面を参照して、本発明に係る経路作成装置の実施の形態を説明する。
【００１８】
本実施の形態では、本発明に係る経路作成装置を、多自由度リンク系の２次元平面を移動可能なロボットの経路を作成する経路作成装置に適用する。本実施の形態に係る経路作成装置は、固定の障害物を回避しつつ、ロボットが所定の位置から所定の位置までの動作（移動）するための経路を作成する。
【００１９】
図１〜図８を参照して、本実施の形態に係る経路作成装置１について説明する。図１は、本実施の形態に係る経路作成装置の構成図である。図２は、本実施の形態で適用されるロボットの一例である。図３は、本実施の形態で適用されるロボットの他の例である。図４は、コンフィグレーション空間（作業空間）における障害物とロボットのスタートノード及びゴール条件の一例である。図５は、図１のサンプル点生成部での処理の説明図であり、（ａ）がスタートノードから１つ目のノードを生成した場合であり、（ｂ）がスタートノードから２つ目のノードを生成した場合である。図６は、図１の個数設定部で設定した個数分のノード群の一例である。図７は、図１の線分長計算部での処理の説明図である。図８は、図１の線分接続部での処理の説明図である。
【００２０】
経路作成装置１は、経路を作成可能な線分長（探索ステップサイズ）を自動的に計算し、線分長に応じたノードと線分を生成し、その線分を接続していくことによってロボットが最終位置姿勢に至るまでの経路を作成する。特に、経路作成装置１は、線分長を計算する際の処理負荷を軽減するために、数え点の個数分のノードの共分散行列を計算し、共分散行列を用いたマハラノビス距離により線分長を計算する。そのために、経路作成装置１は、入力部２、データベース３、サンプル点生成部４、個数設定部５、線分長計算部６、線分接続部７、経路出力部８を備えている。経路作成装置１の主要部はコンピュータ上あるいはロボット内の電子制御ユニットなどに構成され、特に、サンプル点生成部４、個数設定部５、線分長計算部６、線分接続部７はハードディスクあるいはＲＯＭ内に格納された各アプリケーションプログラムをＲＡＭにロードし、ＣＰＵで実行することによって構成される。
【００２１】
なお、本実施の形態では、データベース３が特許請求の範囲に記載する記憶手段に相当し、サンプル点生成部４及び線分長計算部６が特許請求の範囲に記載する探索手段に相当し、個数設定部５が特許請求の範囲に記載する個数設定手段に相当し、線分長計算部６が特許請求の範囲に記載する探索ステップサイズ計算手段に相当し、線分接続部７が特許請求の範囲に記載する線分接続手段に相当する。
【００２２】
まず、本実施の形態に適用されるロボットについて説明しておく。図２にはロボットの一例を示している。ロボットＲ１は、多数の関節Ｊ３，・・・，Ｊｎを備えており、関節間がリンクＬ３，・・・，Ｌｎ＋１で接続されている。また、ロボットＲ１は、末端のリンクＬ３の一端が台車Ｃに取り付けられ、先端のリンクＬｎ＋１の一端にハンドＨが取り付けられている。各関節Ｊ３，・・・，Ｊｎは、アクチュエータが内蔵されており、回転動作をそれぞれ行い、接続される２本のリンク間の角度Ｘ３，・・・，Ｘｎをそれぞれ変更する。台車Ｃは、駆動用モータと操舵用モータが設けられており、２次元平面上を移動し、ロボットＲ１の位置（Ｘ１，Ｘ２）を変更する。
【００２３】
このように、ロボットＲ１は、ｎ個の自由度を持つ。この自由度は、ｎ−２個の角度に対して座標軸及び２個の位置に対して座標軸を持つｎ次元座標空間（コンフィグレーション空間）における一点（Ｘ１，・・・，Ｘｎ）で表される。ロボットＲ１の先端部Ｔ（リンクＬｎ＋１とハンドＨとの取付部）の座標系（Ｙ１，Ｙ２，Ｙ３）は、コンフィグレーション空間から実際の３次元空間への非線形写像の値として、Ｙ１＝ｆ１（Ｘ１，・・・，Ｘｎ）、Ｙ２＝ｆ２（Ｘ１，・・・，Ｘｎ）、Ｙ３＝ｆ３（Ｘ１，・・・，Ｘｎ）で定義される。したがって、コンフィグレーション空間の（Ｘ１，・・・，Ｘｎ）を規定すると、（Ｙ１，Ｙ２，Ｙ３）が決まる。なお、ロボットＲ１や障害物が実際に存在する空間は、２次元平面や３次元空間であり、作業空間と呼ぶ。
【００２４】
また、図３にはロボットの他の例を示している。ロボットＲ２は、人型のロボットであり、左右一対のアーム部Ａ１，Ａ２とハンドＨ１，Ｈ２を有している。ロボットＲ２は、台車Ｃで２次元平面上を移動可能であり、１０個の関節Ｊ３，・・・，Ｊ１２を備えており、１２個の自由度を持つ。ロボットＲ２は、自由度がコンフィグレーション空間における座標系（Ｘ１，・・・，Ｘ１２）で表される。ロボットＲ２の場合、コンフィグレーション空間の（Ｘ１，・・・，Ｘ１２）を規定すると、各先端部Ｔ１，Ｔ２の座標系（Ｙ１１，Ｙ１２，Ｙ１３）、（Ｙ２１，Ｙ２２，Ｙ２３）が決まる。
【００２５】
本実施の形態では、説明を容易にするために、コンフィグレーション空間と作業空間とが一致する場合について説明する。具体的には、ロボットが２次元平面を移動する場合で説明する。図４に示す例では、２次元の四角形状範囲ＳＡ内をロボットが移動可能であり、その四角形状範囲ＳＡ内に障害物ＯＢが存在する。障害物ＯＢは、位置、形状、大きさなどの情報があり、図４に示す例では位置が固定の壁状であり、一箇所に狭い出口ＥＸを有する略円形状である。ロボットの初期位置姿勢は、ロボットの位置や姿勢などで定義され、図４の例の場合にはスタートノードＮｓとして障害物ＯＢの内部領域に設定される。ロボットの目標位置姿勢は、ロボットの位置や姿勢などによって定義され、図４の例の場合にはロボットのゴール条件としてロボットが障害物ＯＢの出口ＥＸから出ることである。以下の説明では、図４に示す例を用いて説明する。
【００２６】
それでは、経路作成装置１の各部について説明する。入力部２は、ロボット情報、障害物情報、スタートノード、ゴール条件、数え点の個数、初期線分長ｄ０を入力する手段である。ロボット情報としては、ロボット全体や各部の質量、ロボット全体や各部の形状及び大きさ、ロボットの重心、関節の回転角度範囲、リンク長及び位置、姿勢などがある。ロボットの質量、形状、大きさ、重心、関節情報については、オペレータによる入力である。ロボットの位置や姿勢については、ＧＰＳ、エンコーダ、カメラによる画像などで取得する。障害物情報としては、障害物の位置、形状、大きさなどである。これらの障害物情報については、ミリ波センサ、超音波センサ、レーザセンサ、レンジファインダ、カメラによる画像などで取得するが、図４の例に示すような固定物の場合にはオペレータによって予め入力するようにしてもよい。例えば、図３に示す人型ロボットの場合、顔部の目に相当する部分にカメラなどが取り付けされる。スタートノードについては、オペレータによる入力である。数え点の個数（基準値と補正値）は、線分長を計算するためのノードの個数であり、オペレータによる入力である。初期線分長ｄ０は、線分長の初期値であり、オペレータによる入力である。
【００２７】
データベース３は、ハードディスクあるいはＲＡＭの所定の領域に構成される。データベース３には、ロボット情報、障害物情報、スタートノード、ゴール条件、数え点の個数、初期線分長ｄ０及びサンプル点生成部４や線分長計算部６で生成するノードの位置情報、ノード間を繋ぐ線分の長さ情報、線分を接続したツリー構造が記憶される。
【００２８】
サンプル点生成部４は、サンプル点ｑrandomを生成するとともに、初期線分長ｄ０でのノードＮ及び線分Ｓを生成する手段である。まず、サンプル点生成部４では、一様乱数などにより、コンフィグレーション空間内に任意のサンプル点ｑrandomを生成する。サンプル点ｑrandomは、障害物とは干渉しない点であり、ロボットが移動や動作可能な範囲内に設定される。そして、サンプル点生成部４では、そのサンプル点ｑrandomとスタートノードＮｓを直線で結び、その直線上にスタートノードＮｓからユークリッド距離で初期線分長ｄ０分に相当する位置にノードＮ１を設定し、スタートノードＮｓとノードＮ１とを繋ぐ線分Ｓ１を生成する（図５（ａ）参照）。
【００２９】
続いて、サンプル点生成部４では、コンフィグレーション空間内に新たな任意のサンプル点ｑrandomを生成する。そして、サンプル点生成部４では、そのサンプル点ｑrandomとスタートノードＮｓ間のユークリッド距離とサンプル点ｑrandomとノードＮ１間のユークリッド距離とを比較し、短い方のユークリッド距離（図５（ｂ）の例では、サンプル点ｑrandomとノードＮ１間の距離）の間を直線で結び、その直線上にノードＮ１からユークリッド距離で初期線分長ｄ０分に相当する位置にノードＮ２を設定し、ノードＮ１とノードＮ２とを繋ぐ線分Ｓ２を生成する（図５（ｂ）参照）。
【００３０】
続いて、サンプル点生成部４では、コンフィグレーション空間内に新たな任意のサンプル点ｑrandomを生成する。そして、サンプル点生成部４では、そのサンプル点ｑrandomとスタートノードＮｓ間のユークリッド距離とサンプル点ｑrandomとノードＮ１間のユークリッド距離とサンプル点ｑrandomとノードＮ２間のユークリッド距離を比較し、最も短いユークリッド距離の間を直線で結び、その直線上にノードＮ２からユークリッド距離で初期線分長ｄ０分に相当する位置にノードＮ３を設定し、ノードＮ２とノードＮ３とを繋ぐ線分Ｓ３を生成する（図６参照）。さらに、サンプル点生成部４では、上記と同様の処理によりノードＮ４を生成し、スタートノードＮｓを含めて数え点の個数分（５つ分）のノードＮｓ，Ｎ１，Ｎ２，Ｎ３，Ｎ４を設定する（図６参照）。このようにスタートノードＮｓを含めて５つのノードが生成されるまで線分長ｄは初期線分長ｄ０で固定値であり、それ以降、ノードＮnewが生成される毎に、任意の長さの線分長ｄnewが線分長計算部６で自動的に計算される。
【００３１】
そして、サンプル点生成部４では、ノードＮnewが生成される毎に、コンフィグレーション空間内に新たな任意のサンプル点ｑrandomを生成する。
【００３２】
サンプル点生成部４では、ノードＮや線分Ｓを生成する毎に、そのノードＮの情報（位置情報など）と線分Ｓの情報（長さ情報、接続される２つのノード）及びノードＮや線分Ｓからなるツリー情報をデータベース３に記憶させる。
【００３３】
個数設定部５は、線分長を求めるためのノードの数え点の個数を設定する手段である。個数設定部５では、通常、数え点の個数としてデータベース３に記憶されている数え点の個数（基準値）を設定する。この基準値は、ロボットの周辺環境(障害物)などを考慮して予め設定され、例えば、５である。さらに、個数設定部５では、線分長計算部６で新たに線分長ｄnewを計算する毎に、その線分長ｄnewが下限線分長ｄｌより小さいか否かを判定し、線分長ｄnewが下限線分長ｄｌより小さい場合には数え点の個数としてデータベース３に記憶されている数え点の個数（補正値）を設定する。下限線分長ｄｌは、ロボットの周辺環境(障害物)などを考慮して予め設定される。数え点の個数（補正値）は、数え点の個数（基準値）より所定数分多い個数であり、ロボットの周辺環境(障害物)などを考慮して予め設定される。
【００３４】
数え点の個数分のノードが密集しているほど、新たな線分長ｄnewが小さい値になる。線分長ｄnewが小さくなり過ぎると（ノードが密集し過ぎると）、ロボットが動けない状態となり、ゴールに到達する経路を作成できない。そこで、数え点の個数を基準値より多くし、線分長ｄnewの計算に用いるノードの個数を増やして、その多数個のノードによる分散を大きくする。これによって、線分長ｄnewが長くなり、ノードの密集を抑制でき、ロボットが動けるようになる。
【００３５】
線分長計算部６は、新たな線分長ｄnewを計算し、新たなノードＮnewと線分Ｓnewを生成する手段である。線分長計算部６では、データベース３に記憶されているノードの数が数え点の個数（例えば、５つ）以上になると、データベース３に記憶されているノード群の中からサンプル点生成部４で生成したサンプル点ｑrandomから近い（ひいては、新たに生成するノードＮnewの近傍の）ノードＮａ，Ｎｂ，Ｎｃ，Ｎｄ，Ｎｅを数え点の個数分選択する。図７の例では、スタートノードＮｓを含めて数え点の個数分のノードＮｓ，Ｎ１，Ｎ２，Ｎ３，Ｎ４が選択される。さらに、線分長計算部６では、そのサンプル点ｑrandomと数え点の個数分のノードＮａ，Ｎｂ，Ｎｃ，Ｎｄ，Ｎｅ間の各ユークリッド距離を比較し、最も短い距離となるノードＮnearを選択する。図７の例では、ノードＮnearはノードＮ１である。
【００３６】
そして、線分長計算部６では、式（１）により、この数え点の個数分のノードＮａ，Ｎｂ，Ｎｃ，Ｎｄ，Ｎｅの共分散行列Σを計算する。但し、Ｎａ＝（ｘａ，ｙａ）、Ｎｂ＝（ｘｂ，ｙｂ）、Ｎｃ＝（ｘｃ，ｙｃ）、Ｎｄ＝（ｘｄ，ｙｄ）、Ｎｅ＝（ｘｅ，ｙｅ）とし、Ｎｉ’の値は式（２）から計算し、Ｎａｖｅの値は式（３）から計算する。Ｎｉ’Ｔは、Ｎｉ’の転置行列である。
【００３７】
【数１】
2008204161.tif 000003
【００３８】
そして、線分長計算部６では、共分散行列Σを用いて、マハラノビス距離をｄｍｉｎとした方程式（式（４））を構築する。ここで、ｄｍｉｎは、ノードＮａ，Ｎｂ，Ｎｃ，Ｎｄ，Ｎｅの間を結ぶ４つの線分のうちの最小のユークリッド距離での線分長であり、図７の例では、線分Ｓ１，Ｓ２，Ｓ３，Ｓ４の長さは全て初期線分長ｄ０となっているので、ｄｍｉｎ＝ｄ０である。また、線分長計算部６では、新たなノードＮnewを求めるための式（５）を構築する。ｒは、正のスカラーである。
【００３９】
【数２】
2008204161.tif 000004
【００４０】
そして、線分長計算部６では、式（５）を式（４）の方程式に代入してｒに関する二次方程式を解き、その求めたｒの値を式（５）に代入し、新たなノードＮnewを計算し、ノードＮnewのユークリッド距離での大きさをｄとし、ｄの値を新たな線分長ｄnewとする。さらに、線分長計算部６では、ノードＮnearとノードＮnewとを結ぶ新たな線分Ｓnewを求める。線分長計算部６では、ゴール条件が満たされるまで、以上の処理を繰り返し行う。なお、線分長計算部６では、線分長ｄnewだけを計算し、新たなノードＮnewや線分Ｓnewの生成についてはサンプル点生成部４で行うようにしてもよい。
【００４１】
数え点の個数分のノードＮａ，Ｎｂ，Ｎｃ，Ｎｄ，Ｎｅが密集している場合（分散が小さくなる場合）、ロボットの形状や位置と障害物の形状や位置との相対的な関係でロボットが移動（動作）し難くなっている。この場合、マハラノビス距離が小さくなり、新たな線分長ｄnewとして小さい値が計算され、ロボットをより微小移動させることができ、ロボットの移動が可能となる。一方、数え点の個数分のノードＮａ，Ｎｂ，Ｎｃ，Ｎｄ，Ｎｅが密集していない場合（分散が大きくなる場合）、ロボットと障害物との相対的な関係でロボットが移動（動作）し易くなっている。この場合、マハラノビス距離が大きくなり、新たな線分長ｄnewとして大きい値が計算され、ロボットをより大幅に移動させることができる。
【００４２】
線分長計算部６では、ノードＮnewや線分Ｓnewを生成する毎に、そのノードＮnewの情報と線分Ｓnewの情報及びノードＮnewと線分Ｓnewからなるツリー情報をデータベース３に記憶させる。
【００４３】
線分接続部７は、ノード間を繋ぐ線分を接続して経路を作成する手段である。線分接続部７では、新たなノードＮnewが生成される毎に、新たなノードＮnewの位置が障害物ＯＢの出口ＥＸから出た位置か否かを判定する（すなわち、ゴール条件を満たしたか否かを判定する）。新たなノードＮnewの位置が障害物ＯＢの出口ＥＸから出た場合、線分接続部７では、サンプル点生成部４、個数設定部５、線分長計算部６での処理を終了させ、データベース３からスタートノードＮｓから出口ＥＸから出たノードＮnewまでを接続する線分を抽出し、その線分を接続して経路を作成する（図８参照）。
【００４４】
経路出力部８は、線分接続部７で作成した経路を出力する手段である。経路出力部８は、例えば、モニタ、プリンタ、ロボットを動作させる制御部との通信を行う通信装置である。また、経路出力部８は、ロボットを動作させる制御部としての機能を有する場合、動作経路における各位置姿勢のコンフィグレーション空間の座標系（台車の位置や各関節の角度）に従ってロボットの各関節のアクチュエータを駆動制御する。
【００４５】
図１を参照して、経路作成装置１の動作を説明する。経路作成装置１では、オペレータによる入力によって、データベース３に、ロボット情報、スタートノードＮｓ、ゴール条件、数え点の個数、初期線分長ｄ０が予め記憶される。また、経路作成装置１では、入力部２の各種センサによって、一定時間毎に、ロボットの位置情報や各種障害物情報を取得する。なお、図４の例のように、障害物が固定物の場合、オペレータによって予め入力されてもよい。
【００４６】
まず、個数設定部５では、数え点の個数として基準値を設定する。サンプル点生成部４では、コンフィグレーション空間内に任意のサンプル点ｑrandomを作成し、そのサンプル点ｑrandomとスタートノードＮｓとの直線上に初期線分長ｄ０に応じた新たなノードＮ１と線分Ｓ１を生成する。続いて、サンプル点生成部４では、コンフィグレーション空間内に任意のサンプル点ｑrandomを作成し、そのサンプル点ｑrandomとスタートノードＮｓ間のユークリッド距離とサンプル点ｑrandomとノードＮ１間のユークリッド距離とを比較し、短い方のユークリッド距離の直線上に初期線分長ｄ０に応じた新たなノードＮ２と線分Ｓ２を生成する。サンプル点生成部４では、上記と同様の処理を繰り返し行い、数え点の個数分のノードＮｓ，Ｎ１，Ｎ２，Ｎ３，Ｎ４と（数え点の個数−１）分の線分Ｓ１，Ｓ２，Ｓ３，Ｓ４を生成する。サンプル点生成部４では、ノードＮや線分Ｓを生成する毎に、そのノードＮの情報と線分Ｓの情報及びこれらの情報からなるツリー情報をデータベース３に記憶させる。
【００４７】
続いて、個数設定部５では、数え点の個数として基準値を設定する。そして、サンプル点生成部４では、コンフィグレーション空間内に新たな任意のサンプル点ｑrandomを生成する。線分長計算部６では、データベース３からサンプル点生成部４で生成したサンプル点ｑrandomからユークリッド距離で近いノードＮａ，Ｎｂ，Ｎｃ，Ｎｄ，Ｎｅを数え点の個数分抽出するとともにそのノード間を繋ぐ線分を抽出する。そして、線分長計算部６では、その数え点の個数分のノードＮａ，Ｎｂ，Ｎｃ，Ｎｄ，Ｎｅの中からそのサンプル点ｑrandomにユークリッド距離で最も近いノードＮnearを選択する。また、線分長計算部６では、その（数え点の個数−１）分の線分の中からユークリッド距離で最も短い線分長をｄｍｉｎに設定する。
【００４８】
線分長計算部６では、この数え点の個数分のノードＮａ，Ｎｂ，Ｎｃ，Ｎｄ，Ｎｅの座標を用いて、式（１）により、ノードＮａ，Ｎｂ，Ｎｃ，Ｎｄ，Ｎｅの共分散行列Σを計算する。そして、線分長計算部６では、共分散行列Σ、ノードＮnew、ノードＮnear用いて、マハラノビス距離をｄｍｉｎとした方程式（式（４））を構築する。さらに、線分長計算部６では、ノードＮnewを求めるための式（５）を式（４）の方程式に代入してｒに関する二次方程式を解き、その求めたｒの値を式（５）に代入し、新たなノードＮnewを計算し、ノードＮnewのユークリッド距離での大きさをｄとし、ｄの値を新たな線分長ｄnewとする。そして、線分長計算部６では、ノードＮnearとノードＮnewとを結ぶ新たな線分Ｓnewを求める。そして、線分長計算部６では、そのノードＮnewの情報と線分Ｓnewの情報及びそれらの情報からなるツリー情報をデータベース３に記憶させる。新たなノードＮnewが求められると、線分接続部７では、新たなノードＮnewがゴール条件を満たしたか否かを判定する。
【００４９】
ゴール条件を満たさなかった場合、個数設定部５では、新たな線分長ｄnewが下限線分長ｄｌより小さいか否かを判定する。そして、個数設定部５では、線分長ｄnewが下限線分長ｄｌより小さい場合には数え点の個数として基準値より多い補正値を設定し、線分長ｄnewが下限線分長ｄｌ以上の場合には数え点の個数として基準値を設定する。そして、サンプル点生成部４で上記と同様に新たなサンプル点ｑrandomを作成し、線分長計算部６では上記と同様に新たな線分長ｄnewを計算し、新たなノードＮnewと線分Ｓnewを生成し、線分接続部７では上記と同様に新たなノードＮnewがゴール条件を満たしたか否かを判定する。
【００５０】
ゴール条件を満たした場合、線分接続部７では、サンプル点生成部４、個数設定部５、線分長計算部６での処理を終了させる。そして、線分接続部７では、データベース３からスタートノードＮｓから出口ＥＸから出たノードＮnewまでを接続する線分を抽出し、その線分を接続して経路を作成する。経路出力部８は、線分接続部７で作成された経路を出力する。
【００５１】
この経路作成装置１によれば、ノードの分散状況に従って線分長を計算し、線分長を自動的に更新してゆくことができる。特に、経路作成装置１では、数え点の個数分のノードの分散情報から線分長を計算するだけなので、処理が容易であり、処理負荷を軽減できる。その結果、経路作成装置１では、リアルタイム処理が可能となる。また、経路作成装置１では、自動的に線分長を更新することができるので、障害物を回避しながらゴールまで到達可能な経路を作成することができる。
【００５２】
また、経路作成装置１によれば、線分長が下限線分長より小さくなった場合には数え点の個数を多くすることにより、ノードが密集し過ぎてロボットが移動できないような経路の作成を防止できる。また、経路作成装置によれば、データベース３から経路の途中に対応する部分のツリー構造におけるノード情報を常時取得することができ、そのノード情報から分散情報を求めることができる。また、経路作成装置１によれば、分散情報として共分散行列を用いたマハラノビス距離を適用することにより、簡単に線分長を計算することができり、処理負荷を軽減できる。さらに、マハラノビス距離を用いることにより、ノードが密集している場合には小さい線分長を計算でき、ノードが分散している場合には大きい線分長を計算できる。
【００５３】
以上、本発明に係る実施の形態について説明したが、本発明は上記実施の形態に限定されることなく様々な形態で実施される。
【００５４】
例えば、本実施の形態では多数の関節を有し、関節間がリンクで連結され、各関節の回転動作によって多数の自由度を持ちかつ２次元平面を移動するロボットに適用したが、関節が伸縮動作などの他の動作を行うものでも適用可能であり、１次元的に移動するロボットや３次元空間を移動するロボットでも適用可能であり、ロボット全体が移動しないものでも適用可能であり、あるいは、関節を持たない単純な移動体（自動車など）にも適用可能である。
【００５５】
また、本実施の形態では固定の障害物が存在し、その障害物を回避してスタートからゴール条件を満たした場合の経路作成について説明したが、移動する障害物の場合にも適用可能であり、あるいは、ロボットが３次元空間における所定の位置や姿勢になるまでの経路作成にも適用可能である。
【００５６】
また、本実施の形態では線分長が下限線分長より小さくなった場合に数え点の個数を多くする構成としたが、線分長が上限線分長より大きくなった場合に数え点の個数を少なくするなどの他の数え点の個数の変更方法としてもよいし、あるいは、数え点の個数を固定値としてもよい。さらに、直接的に線分長を上限線分長に設定するようにしてもよい。固定値とする場合、ノードが密集し過ぎないように、比較的多めの個数を設定するとよい。
【００５７】
また、本実施の形態ではノードを繋ぐ線分を接続したツリー構造をデータベースに保持する構成としたが、このようなツリー構造をデータベースに保持しなくても実現可能である。
【００５８】
また、本実施の形態では分散情報として共分散行列を用いたマハラノビス距離を適用したが、分散情報として他の様々なものを適用可能である。
【００５９】
また、本実施の形態では線分長が下限線分長より小さい場合には数え点の個数として補正値を設定する構成としたが、線分長が下限線分長より小さくなる毎に所定個数分を加算して徐々に数え点の個数を増加してゆくようにしてもよい。
【図面の簡単な説明】
【００６０】
【図１】本実施の形態に係る経路作成装置の構成図である。
【図２】本実施の形態で適用されるロボットの一例である。
【図３】本実施の形態で適用されるロボットの他の例である。
【図４】コンフィグレーション空間（作業空間）における障害物とロボットのスタートノード及びゴール条件の一例である。
【図５】図１のサンプル点生成部での処理の説明図であり、（ａ）がスタートノードから１つ目のノードを生成した場合であり、（ｂ）がスタートノードから２つ目のノードを生成した場合である。
【図６】図１の個数設定部で設定した個数分のノード群の一例である。
【図７】図１の線分長計算部での処理の説明図である。
【図８】図１の線分接続部での処理の説明図である。
【符号の説明】
【００６１】
１…経路作成装置、２…入力部、３…データベース、４…サンプル点生成部、５…個数設定部、６…線分長計算部、７…線分接続部、８…経路出力部
【図１】
2008204161.tif 000005
【図２】
2008204161.tif 000006
【図３】
2008204161.tif 000007
【図４】
2008204161.tif 000008
【図５】
2008204161.tif 000009
【図６】
2008204161.tif 000010
【図７】
2008204161.tif 000011
【図８】
2008204161.tif 000012