JP4760732B2

JP4760732B2 - 経路作成装置

Info

Publication number: JP4760732B2
Application number: JP2007039438A
Authority: JP
Inventors: 真太郎吉澤
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2007-02-20
Filing date: 2007-02-20
Publication date: 2011-08-31
Anticipated expiration: 2027-02-20
Also published as: JP2008204161A

Description

本発明は、ロボットが障害物を回避する経路を作成する経路作成装置に関する。

近年、各種産業用ロボットや人型ロボットなどの様々なロボットが開発されている。例えば、ロボットには、多数の関節を有し、関節間がリンクで連結され、各関節の動作によって多数の自由度を持つロボットがある。このようなロボットを動作させる場合、初期位置姿勢と目標位置姿勢が与えられ、初期位置姿勢から目標位置姿勢に至る経路を作成し、その経路に基づいてロボットの各関節を動作させる。さらに、このようなロボット全体を２次元平面内などで移動させるものもある。

このようなロボットを動作や移動させる場合、ロボットの周辺に障害物が存在すると、この障害物を回避しながらロボットを動作や移動させるための経路の作成は非常に難しくなる。例えば、周囲の障害物で形成される狭い空間を通り抜けるような経路を作成するような場合がある。このような経路の作成方法としては、一定の経路ステップサイズを設定し、その探索ステップサイズ毎に障害物と干渉しない経路を探索し、その探索した経路ステップサイズ毎の経路を接続しながら全経路を作成する（特許文献１参照）。

しかし、経路ステップサイズが一定値の場合、ロボットの大きさ、形状、位置と障害物の大きさ、形状、位置との相対関係（例えば、ロボットの大きさや形状と上記した周囲の障害物による狭い空間との相対関係）により、経路ステップサイズが大き過ぎて障害物を回避できる経路を作成できない場合がある。そこで、経路の作成方法として、障害物を回避できる経路を作成可能な経路ステップサイズに自動的に更新し、更新した経路ステップサイズ毎に経路を作成するものがある（非特許文献１参照）。この経路作成方法では、経路を作成しようとする領域内に複数の点を生成し、障害物と生成された点との境界の情報を取得し、経路を作成しようとする領域を複数の円で近似して経路を作成する。特に、この経路作成方法では、その領域を各点との距離によって分割したボロノイ領域を設定し、障害物との境界に位置する点を中心とする円によってボロノイ領域を近似し、この円の半径を障害物との干渉回数によって一定割合αで徐々に増減する。
特開平７−６４６３３号公報Ｌ．ＪａｉｌｌｅｔＡ．ＹｅｒｓｈｏｖａＳ．ＬａＶａｌｌｅ，Ｔ．Ｓｉｍｅｏｎ，ＡｄａｐｔｉｖｅＴｕｎｉｎｇｏｆｔｈｅＳａｍｐｌｉｎｇＤｏｍａｉｎｆｏｒＤｙｎａｍｉｃ−ＤｏｍａｉｎＲＲＴｓ，ＩＥＥＥＩｎｔｅｒｎａｔｉｏｎａｌＣｏｆｅｒｅｎｃｅｏｎＩｎｔｅｌｌｉｇｅｎｔＲｏｂｏｔｓａｎｄＳｙｓｔｅｍｓ２００５

ロボットなどを実用化するには、リアルタイム性が要求される。しかし、上記経路作成方法の場合、経路を作成する領域の次元が高くなったり、ロボットの自由度が大きくなったりあるいは障害物の形状が複雑になると、経路ステップサイズを求める処理負荷が大きくなる。これは、一様乱数を生成すべき領域の形状を、随時、保持しながら一様乱数を生成することは非常に複雑な処理を要するからである。そのため、処理負荷が増大し、リアルタイムでの処理ができなくなり、実用化に適さない。

そこで、本発明は、処理負荷を軽減しつつ経路ステップサイズを自動的に求めることができる経路作成装置を提供することを課題とする。

本発明に係る経路作成装置は、ノードを繋ぐ線分を順次接続することによりロボットが障害物を回避する経路を作成する経路作成装置であって、経路を探索する際に経路を作成可能なノード間の線分長である探索ステップサイズを計算するためのノードの個数を設定する個数設定手段と、障害物とは干渉しないサンプル点を生成するサンプル点生成手段と、個数設定手段で設定された個数分のノードの分散情報に基づいて、個数分のノードの分散が大きいほど探索ステップサイズとして大きい値を計算する探索ステップサイズ計算手段と、探索ステップサイズ計算手段で計算された探索ステップサイズに応じてノードを探索する探索手段と、探索手段で探索されたノードを繋ぐ線分を接続し、経路を作成する線分接続手段とを備え、探索ステップサイズ計算手段は、サンプル点生成手段で生成されたサンプル点ｑ _{ｒａｎｄｏｍ} から近いノードを個数設定手段で設定された個数分抽出し、抽出された個数分のノードの中からサンプル点ｑ _{ｒａｎｄｏｍ} に最も近いノードＮ _ｎｅａｒを選択し、抽出された個数分のノード間の線分の中から最も短い線分長ｄ _ｍｉｎを選択し、抽出された個数分のノードの共分散行列Σを計算し、線分長ｄ _ｍｉｎと共分散行列Σとを用いたマハラノビス距離によって、ノードＮ _ｎｅａｒから新たなノードＮ _ｎｅｗを求め、新たなノードＮ _ｎｅｗのユークリッド距離での大きさを探索ステップサイズとすることを特徴とする。

この経路作成装置では、個数設定手段により、経路を探索していくための探索ステップサイズを計算するためのノードの個数を設定する。そして、経路作成装置では、探索ステップサイズ計算手段により、その設定された個数分のノード（探索済みのノード）の分散情報に基づいて探索ステップサイズを計算する。さらに、経路作成装置では、探索手段により探索ステップサイズに応じてノードを探索し、線分接続手段により探索されたノードを繋ぐ線分を接続しながら経路を作成する。ノードが密集しているほど、ロボットと障害物との相対的関係によってロボットが動作や移動し難い状況であり、探索ステップサイズを小さくする必要がある。一方、ノードが分散しているほど、ロボットと障害物との相対的関係においてロボットが動作や移動を自由にできる状況であり、探索ステップサイズを大きくできる。このように、経路作成装置では、ノードの分散状況に応じて探索ステップサイズを自動的に求めることができる。さらに、経路作成装置では、所定の個数分のノードの分散情報から探索ステップサイズを計算するだけなので、処理が容易であり、処理負荷を軽減できる。その結果、この経路作成装置では、リアルタイム処理も可能となる。また、経路作成装置では、自動的に探索ステップサイズを更新することができるので、障害物を回避しながら目標位置姿勢に至る経路を作成することができる。

なお、ロボットには、関節などによって動作する一般的なロボット以外にも、移動体（自動車など）も含むものとする。したがって、経路には、移動するだけの経路も含むものとする。

本発明の上記経路作成装置では、個数設定手段は、探索ステップサイズ計算手段で計算された探索ステップサイズが所定の範囲外の場合に個数を変更する構成としてもよい。

この経路作成装置では、個数設定手段により、計算された探索ステップサイズが所定の範囲外の場合に個数を変更する。これによって、ノードが密集してきた場合でも、個数を多くすることによって探索ステップサイズを大きくことが可能であり、多くの個数のノードにより分散を大きくできる。その結果、ノードが密集し過ぎることによってロボットが動作や移動できないような経路の作成を防止できる。

本発明の上記経路作成装置では、ノードとノードを繋ぐ線分からなるツリー構造を記憶する記憶手段を備え、探索ステップサイズ計算手段は、記憶手段のツリー構造から個数設定手段で設定された個数分のノードを抽出し、当該抽出したノードの分散情報に基づいて探索ステップサイズを計算する構成としてもよい。

この経路作成装置では、記憶手段に経路を作成するためのノードとノード間の線分からなるツリー構造を保持する。そして、経路作成装置では、探索ステップサイズ計算手段により、記憶手段に保持されているツリー構造から設定された個数分のノードを抽出し、その抽出したノードの分散情報に基づいて探索ステップサイズを計算する。これによって、経路の途中に対応する部分のツリーにおけるノード情報を記憶手段から常時取得することができ、そのノード情報から分散情報を求めることができる。

本発明の上記経路作成装置では、探索ステップサイズ計算手段は、サンプル点生成手段で生成されたサンプル点ｑ_{ｒａｎｄｏｍ}から近いノードを個数設定手段で設定された個数分抽出し、抽出された個数分のノードの中からサンプル点ｑ_{ｒａｎｄｏｍ}に最も近いノードＮ_ｎｅａｒを選択し、抽出された個数分のノード間の線分の中から最も短い線分長ｄ_ｍｉｎを選択し、抽出された個数分のノードの共分散行列Σを計算し、共分散行列Σと最も近いノードＮ_ｎｅａｒと新たなノードＮ_ｎｅｗを用いてマハラノビス距離を最も短い線分長ｄ_ｍｉｎとした式（４）を構築し、新たなノードＮ_ｎｅｗを計算するための式（５）を構築し、式（４）と式（５）から新たなノードＮ_ｎｅｗを計算し、新たなノードＮ_ｎｅｗのユークリッド距離での大きさを探索ステップサイズとする構成としてもよい。
（Ｎ_ｎｅｗ−Ｎ_ｎｅａｒ）Σ^−１（Ｎ_ｎｅｗ−Ｎ_ｎｅａｒ）^Ｔ＝ｄ_ｍｉｎ ^２…（４）
Ｎ_ｎｅｗ＝ｒ（ｑ_{ｒａｎｄｏｍ}−Ｎ_ｎｅａｒ）…（５）

この経路作成装置では、探索ステップサイズ計算手段により、設定された個数分のノードの共分散行列を計算し、その共分散行列を用いたマハラノビス距離により探索ステップサイズを計算する。このように、分散情報として共分散行列によるマハラノビス距離を適用することにより、簡単に探索ステップサイズを求めることができる。さらに、マハラノビス距離を用いることにより、ノードが密集している場合には小さい探索ステップサイズを計算でき、ノードが分散している場合には大きい探索ステップサイズを計算できる。

本発明は、所定の個数のノードの分散情報を用いて探索ステップサイズを計算することにより、処理負荷を軽減しつつ経路ステップサイズを自動的に求めることができる。

以下、図面を参照して、本発明に係る経路作成装置の実施の形態を説明する。

本実施の形態では、本発明に係る経路作成装置を、多自由度リンク系の２次元平面を移動可能なロボットの経路を作成する経路作成装置に適用する。本実施の形態に係る経路作成装置は、固定の障害物を回避しつつ、ロボットが所定の位置から所定の位置までの動作（移動）するための経路を作成する。

図１〜図８を参照して、本実施の形態に係る経路作成装置１について説明する。図１は、本実施の形態に係る経路作成装置の構成図である。図２は、本実施の形態で適用されるロボットの一例である。図３は、本実施の形態で適用されるロボットの他の例である。図４は、コンフィグレーション空間（作業空間）における障害物とロボットのスタートノード及びゴール条件の一例である。図５は、図１のサンプル点生成部での処理の説明図であり、（ａ）がスタートノードから１つ目のノードを生成した場合であり、（ｂ）がスタートノードから２つ目のノードを生成した場合である。図６は、図１の個数設定部で設定した個数分のノード群の一例である。図７は、図１の線分長計算部での処理の説明図である。図８は、図１の線分接続部での処理の説明図である。

経路作成装置１は、経路を作成可能な線分長（探索ステップサイズ）を自動的に計算し、線分長に応じたノードと線分を生成し、その線分を接続していくことによってロボットが最終位置姿勢に至るまでの経路を作成する。特に、経路作成装置１は、線分長を計算する際の処理負荷を軽減するために、数え点の個数分のノードの共分散行列を計算し、共分散行列を用いたマハラノビス距離により線分長を計算する。そのために、経路作成装置１は、入力部２、データベース３、サンプル点生成部４、個数設定部５、線分長計算部６、線分接続部７、経路出力部８を備えている。経路作成装置１の主要部はコンピュータ上あるいはロボット内の電子制御ユニットなどに構成され、特に、サンプル点生成部４、個数設定部５、線分長計算部６、線分接続部７はハードディスクあるいはＲＯＭ内に格納された各アプリケーションプログラムをＲＡＭにロードし、ＣＰＵで実行することによって構成される。

なお、本実施の形態では、データベース３が特許請求の範囲に記載する記憶手段に相当し、サンプル点生成部４及び線分長計算部６が特許請求の範囲に記載する探索手段に相当し、個数設定部５が特許請求の範囲に記載する個数設定手段に相当し、線分長計算部６が特許請求の範囲に記載する探索ステップサイズ計算手段に相当し、線分接続部７が特許請求の範囲に記載する線分接続手段に相当する。

まず、本実施の形態に適用されるロボットについて説明しておく。図２にはロボットの一例を示している。ロボットＲ１は、多数の関節Ｊ３，・・・，Ｊｎを備えており、関節間がリンクＬ３，・・・，Ｌｎ＋１で接続されている。また、ロボットＲ１は、末端のリンクＬ３の一端が台車Ｃに取り付けられ、先端のリンクＬｎ＋１の一端にハンドＨが取り付けられている。各関節Ｊ３，・・・，Ｊｎは、アクチュエータが内蔵されており、回転動作をそれぞれ行い、接続される２本のリンク間の角度Ｘ３，・・・，Ｘｎをそれぞれ変更する。台車Ｃは、駆動用モータと操舵用モータが設けられており、２次元平面上を移動し、ロボットＲ１の位置（Ｘ１，Ｘ２）を変更する。

このように、ロボットＲ１は、ｎ個の自由度を持つ。この自由度は、ｎ−２個の角度に対して座標軸及び２個の位置に対して座標軸を持つｎ次元座標空間（コンフィグレーション空間）における一点（Ｘ１，・・・，Ｘｎ）で表される。ロボットＲ１の先端部Ｔ（リンクＬｎ＋１とハンドＨとの取付部）の座標系（Ｙ１，Ｙ２，Ｙ３）は、コンフィグレーション空間から実際の３次元空間への非線形写像の値として、Ｙ１＝ｆ１（Ｘ１，・・・，Ｘｎ）、Ｙ２＝ｆ２（Ｘ１，・・・，Ｘｎ）、Ｙ３＝ｆ３（Ｘ１，・・・，Ｘｎ）で定義される。したがって、コンフィグレーション空間の（Ｘ１，・・・，Ｘｎ）を規定すると、（Ｙ１，Ｙ２，Ｙ３）が決まる。なお、ロボットＲ１や障害物が実際に存在する空間は、２次元平面や３次元空間であり、作業空間と呼ぶ。

また、図３にはロボットの他の例を示している。ロボットＲ２は、人型のロボットであり、左右一対のアーム部Ａ１，Ａ２とハンドＨ１，Ｈ２を有している。ロボットＲ２は、台車Ｃで２次元平面上を移動可能であり、１０個の関節Ｊ３，・・・，Ｊ１２を備えており、１２個の自由度を持つ。ロボットＲ２は、自由度がコンフィグレーション空間における座標系（Ｘ１，・・・，Ｘ１２）で表される。ロボットＲ２の場合、コンフィグレーション空間の（Ｘ１，・・・，Ｘ１２）を規定すると、各先端部Ｔ１，Ｔ２の座標系（Ｙ１１，Ｙ１２，Ｙ１３）、（Ｙ２１，Ｙ２２，Ｙ２３）が決まる。

本実施の形態では、説明を容易にするために、コンフィグレーション空間と作業空間とが一致する場合について説明する。具体的には、ロボットが２次元平面を移動する場合で説明する。図４に示す例では、２次元の四角形状範囲ＳＡ内をロボットが移動可能であり、その四角形状範囲ＳＡ内に障害物ＯＢが存在する。障害物ＯＢは、位置、形状、大きさなどの情報があり、図４に示す例では位置が固定の壁状であり、一箇所に狭い出口ＥＸを有する略円形状である。ロボットの初期位置姿勢は、ロボットの位置や姿勢などで定義され、図４の例の場合にはスタートノードＮ_ｓとして障害物ＯＢの内部領域に設定される。ロボットの目標位置姿勢は、ロボットの位置や姿勢などによって定義され、図４の例の場合にはロボットのゴール条件としてロボットが障害物ＯＢの出口ＥＸから出ることである。以下の説明では、図４に示す例を用いて説明する。

それでは、経路作成装置１の各部について説明する。入力部２は、ロボット情報、障害物情報、スタートノード、ゴール条件、数え点の個数、初期線分長ｄ_０を入力する手段である。ロボット情報としては、ロボット全体や各部の質量、ロボット全体や各部の形状及び大きさ、ロボットの重心、関節の回転角度範囲、リンク長及び位置、姿勢などがある。ロボットの質量、形状、大きさ、重心、関節情報については、オペレータによる入力である。ロボットの位置や姿勢については、ＧＰＳ、エンコーダ、カメラによる画像などで取得する。障害物情報としては、障害物の位置、形状、大きさなどである。これらの障害物情報については、ミリ波センサ、超音波センサ、レーザセンサ、レンジファインダ、カメラによる画像などで取得するが、図４の例に示すような固定物の場合にはオペレータによって予め入力するようにしてもよい。例えば、図３に示す人型ロボットの場合、顔部の目に相当する部分にカメラなどが取り付けされる。スタートノードについては、オペレータによる入力である。数え点の個数（基準値と補正値）は、線分長を計算するためのノードの個数であり、オペレータによる入力である。初期線分長ｄ_０は、線分長の初期値であり、オペレータによる入力である。

データベース３は、ハードディスクあるいはＲＡＭの所定の領域に構成される。データベース３には、ロボット情報、障害物情報、スタートノード、ゴール条件、数え点の個数、初期線分長ｄ_０及びサンプル点生成部４や線分長計算部６で生成するノードの位置情報、ノード間を繋ぐ線分の長さ情報、線分を接続したツリー構造が記憶される。

サンプル点生成部４は、サンプル点ｑ_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はノードＮ_１である。

そして、線分長計算部６では、式（１）により、この数え点の個数分のノードＮ_ａ，Ｎ_ｂ，Ｎ_ｃ，Ｎ_ｄ，Ｎ_ｅの共分散行列Σを計算する。但し、Ｎ_ａ＝（ｘ_ａ，ｙ_ａ）、Ｎ_ｂ＝（ｘ_ｂ，ｙ_ｂ）、Ｎ_ｃ＝（ｘ_ｃ，ｙ_ｃ）、Ｎ_ｄ＝（ｘ_ｄ，ｙ_ｄ）、Ｎ_ｅ＝（ｘ_ｅ，ｙ_ｅ）とし、Ｎ_ｉ’の値は式（２）から計算し、Ｎ_ａｖｅの値は式（３）から計算する。Ｎ_ｉ’^Ｔは、Ｎ_ｉ’の転置行列である。

そして、線分長計算部６では、共分散行列Σを用いて、マハラノビス距離をｄ_ｍｉｎとした方程式（式（４））を構築する。ここで、ｄ_ｍｉｎは、ノードＮ_ａ，Ｎ_ｂ，Ｎ_ｃ，Ｎ_ｄ，Ｎ_ｅの間を結ぶ４つの線分のうちの最小のユークリッド距離での線分長であり、図７の例では、線分Ｓ_１，Ｓ_２，Ｓ_３，Ｓ_４の長さは全て初期線分長ｄ_０となっているので、ｄ_ｍｉｎ＝ｄ_０である。また、線分長計算部６では、新たなノードＮ_newを求めるための式（５）を構築する。ｒは、正のスカラーである。

そして、線分長計算部６では、式（５）を式（４）の方程式に代入してｒに関する二次方程式を解き、その求めたｒの値を式（５）に代入し、新たなノードＮ_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までを接続する線分を抽出し、その線分を接続して経路を作成する。経路出力部８は、線分接続部７で作成された経路を出力する。

この経路作成装置１によれば、ノードの分散状況に従って線分長を計算し、線分長を自動的に更新してゆくことができる。特に、経路作成装置１では、数え点の個数分のノードの分散情報から線分長を計算するだけなので、処理が容易であり、処理負荷を軽減できる。その結果、経路作成装置１では、リアルタイム処理が可能となる。また、経路作成装置１では、自動的に線分長を更新することができるので、障害物を回避しながらゴールまで到達可能な経路を作成することができる。

また、経路作成装置１によれば、線分長が下限線分長より小さくなった場合には数え点の個数を多くすることにより、ノードが密集し過ぎてロボットが移動できないような経路の作成を防止できる。また、経路作成装置によれば、データベース３から経路の途中に対応する部分のツリー構造におけるノード情報を常時取得することができ、そのノード情報から分散情報を求めることができる。また、経路作成装置１によれば、分散情報として共分散行列を用いたマハラノビス距離を適用することにより、簡単に線分長を計算することができり、処理負荷を軽減できる。さらに、マハラノビス距離を用いることにより、ノードが密集している場合には小さい線分長を計算でき、ノードが分散している場合には大きい線分長を計算できる。

以上、本発明に係る実施の形態について説明したが、本発明は上記実施の形態に限定されることなく様々な形態で実施される。

例えば、本実施の形態では多数の関節を有し、関節間がリンクで連結され、各関節の回転動作によって多数の自由度を持ちかつ２次元平面を移動するロボットに適用したが、関節が伸縮動作などの他の動作を行うものでも適用可能であり、１次元的に移動するロボットや３次元空間を移動するロボットでも適用可能であり、ロボット全体が移動しないものでも適用可能であり、あるいは、関節を持たない単純な移動体（自動車など）にも適用可能である。

また、本実施の形態では固定の障害物が存在し、その障害物を回避してスタートからゴール条件を満たした場合の経路作成について説明したが、移動する障害物の場合にも適用可能であり、あるいは、ロボットが３次元空間における所定の位置や姿勢になるまでの経路作成にも適用可能である。

また、本実施の形態では線分長が下限線分長より小さくなった場合に数え点の個数を多くする構成としたが、線分長が上限線分長より大きくなった場合に数え点の個数を少なくするなどの他の数え点の個数の変更方法としてもよいし、あるいは、数え点の個数を固定値としてもよい。さらに、直接的に線分長を上限線分長に設定するようにしてもよい。固定値とする場合、ノードが密集し過ぎないように、比較的多めの個数を設定するとよい。

また、本実施の形態ではノードを繋ぐ線分を接続したツリー構造をデータベースに保持する構成としたが、このようなツリー構造をデータベースに保持しなくても実現可能である。

また、本実施の形態では分散情報として共分散行列を用いたマハラノビス距離を適用したが、分散情報として他の様々なものを適用可能である。

また、本実施の形態では線分長が下限線分長より小さい場合には数え点の個数として補正値を設定する構成としたが、線分長が下限線分長より小さくなる毎に所定個数分を加算して徐々に数え点の個数を増加してゆくようにしてもよい。

本実施の形態に係る経路作成装置の構成図である。本実施の形態で適用されるロボットの一例である。本実施の形態で適用されるロボットの他の例である。コンフィグレーション空間（作業空間）における障害物とロボットのスタートノード及びゴール条件の一例である。図１のサンプル点生成部での処理の説明図であり、（ａ）がスタートノードから１つ目のノードを生成した場合であり、（ｂ）がスタートノードから２つ目のノードを生成した場合である。図１の個数設定部で設定した個数分のノード群の一例である。図１の線分長計算部での処理の説明図である。図１の線分接続部での処理の説明図である。

符号の説明

１…経路作成装置、２…入力部、３…データベース、４…サンプル点生成部、５…個数設定部、６…線分長計算部、７…線分接続部、８…経路出力部

Claims

ノードを繋ぐ線分を順次接続することによりロボットが障害物を回避する経路を作成する経路作成装置であって、
経路を探索する際に経路を作成可能なノード間の線分長である探索ステップサイズを計算するためのノードの個数を設定する個数設定手段と、
障害物とは干渉しないサンプル点を生成するサンプル点生成手段と、
前記個数設定手段で設定された個数分のノードの分散情報に基づいて、前記個数分のノードの分散が大きいほど探索ステップサイズとして大きい値を計算する探索ステップサイズ計算手段と、
前記探索ステップサイズ計算手段で計算された探索ステップサイズに応じてノードを探索する探索手段と、
前記探索手段で探索されたノードを繋ぐ線分を接続し、経路を作成する線分接続手段と
を備え、
前記探索ステップサイズ計算手段は、前記サンプル点生成手段で生成されたサンプル点ｑ _{ｒａｎｄｏｍ} から近いノードを前記個数設定手段で設定された個数分抽出し、前記抽出された個数分のノードの中から前記サンプル点ｑ _{ｒａｎｄｏｍ} に最も近いノードＮ _ｎｅａｒを選択し、前記抽出された個数分のノード間の線分の中から最も短い線分長ｄ _ｍｉｎを選択し、前記抽出された個数分のノードの共分散行列Σを計算し、前記線分長ｄ _ｍｉｎと前記共分散行列Σとを用いたマハラノビス距離によって、前記ノードＮ _ｎｅａｒから新たなノードＮ _ｎｅｗを求め、新たなノードＮ _ｎｅｗのユークリッド距離での大きさを探索ステップサイズとすることを特徴とする経路作成装置。
前記個数設定手段は、前記探索ステップサイズ計算手段で計算された探索ステップサイズが所定の範囲外の場合に個数を変更することを特徴とする請求項１に記載する経路作成装置。
ノードとノードを繋ぐ線分からなるツリー構造を記憶する記憶手段を備え、
前記探索ステップサイズ計算手段は、前記記憶手段のツリー構造から前記個数設定手段で設定された個数分のノードを抽出し、当該抽出したノードの分散情報に基づいて探索ステップサイズを計算することを特徴とする請求項１又は請求項２に記載する経路作成装置。
前記探索ステップサイズ計算手段は、前記サンプル点生成手段で生成されたサンプル点ｑ_{ｒａｎｄｏｍ}から近いノードを前記個数設定手段で設定された個数分抽出し、前記抽出された個数分のノードの中から前記サンプル点ｑ_{ｒａｎｄｏｍ}に最も近いノードＮ_ｎｅａｒを選択し、前記抽出された個数分のノード間の線分の中から最も短い線分長ｄ_ｍｉｎを選択し、前記抽出された個数分のノードの共分散行列Σを計算し、前記共分散行列Σと前記最も近いノードＮ_ｎｅａｒと新たなノードＮ_ｎｅｗを用いてマハラノビス距離を前記最も短い線分長ｄ_ｍｉｎとした式（４）を構築し、新たなノードＮ_ｎｅｗを計算するための式（５）を構築し、式（４）と式（５）から新たなノードＮ_ｎｅｗを計算し、新たなノードＮ_ｎｅｗのユークリッド距離での大きさを探索ステップサイズとすることを特徴とする請求項１〜請求項３のいずれか１項に記載する経路作成装置。
（Ｎ_ｎｅｗ−Ｎ_ｎｅａｒ）Σ^−１（Ｎ_ｎｅｗ−Ｎ_ｎｅａｒ）^Ｔ＝ｄ_ｍｉｎ ^２…（４）
Ｎ_ｎｅｗ＝ｒ（ｑ_{ｒａｎｄｏｍ}−Ｎ_ｎｅａｒ）…（５）