(57)【要約】【課題】自律移動体が自己位置を推定する際に移動障害物の影響を受け難くするとともに、平らな壁面以外の障害物の計測情報も自己位置推定に利用できる自律移動体およびその自己位置推定方法を提供する。【解決手段】自律移動体１００は、移動環境計測用の視覚センサ２と、視覚センサ２による計測結果を用いて移動環境内に存在する障害物の３次元の位置データ群を含む環境データを生成する３次元環境データ生成部４と、環境データを用いて、移動環境内に存在する平らな壁面を検出する壁面検出部６と、環境データに含まれる位置データ群のうち第１の高さ範囲に属する位置データによって特定される障害物の位置情報及び壁面の位置情報を、移動環境内に固定的に存在する固定障害物の位置を示す参照マップと照合することで、自己の位置を推定する自己位置推定部８と、を備える。【選択図】図１
2011008320.tif 000002
【特許請求の範囲】
【請求項１】
移動環境を計測する視覚センサと、
前記視覚センサによる計測結果を用いて、前記移動環境内に存在する障害物に関する３次元の位置データ群を含む環境データを生成する３次元環境データ生成部と、
前記環境データを用いて、前記移動環境内に存在する平らな壁面を検出する壁面検出部と、
前記環境データに含まれる前記位置データ群のうち第１の高さ範囲に属する位置データによって特定される障害物の位置情報、および前記壁面の位置情報を、前記移動環境内に固定的に存在する固定障害物の位置を示す参照マップと照合することによって、自己の位置を推定する自己位置推定部と、
を備える自律移動体。
【請求項２】
前記第１の高さ範囲は、前記位置データ群の高さ分布に基づいて決定される請求項１に記載の自律移動体。
【請求項３】
前記第１の高さ範囲は、前記位置データ群のうち最も高さが高いデータを含むように決定される請求項１または２に記載の自律移動体。
【請求項４】
前記位置データ群は、前記環境データに含まれる前記位置データ群から前記壁面に対応する位置データ群を除いた残りのデータ群である請求項２または３に記載の自律移動体。
【請求項５】
前記位置データに信頼度を付与する信頼度設定部をさらに備え、
前記信頼度設定部は、前記位置データ群のうち第１の高さ範囲に属する位置データによって特定される障害物の位置情報に設定する信頼度よりも、前記壁面の位置情報に設定する信頼度を高くし、
前記自己位置推定部は、前記信頼度の高い位置情報を参照マップとの照合に優先的に用いる、請求項１〜４のいずれか一項に記載の自律移動体。
【請求項６】
前記信頼度設定部は、前記環境データに含まれる前記位置データ群のうち第１の高さ範囲に属する位置データによって特定される障害物の位置情報および前記壁面の位置情報をグリッドデータに変換し、
前記位置データの夫々が対応する各グリッドに、前記位置データの数に基づいて信頼度を設定する請求項５に記載の自律移動体。
【請求項７】
前記自己位置推定部は、前記壁面の法線ベクトルを用いて、前記参照マップによって特定される前記固定障害物の配置に前記壁面を重ね合わせるための回転角度を算出する請求項１〜４のいずれか一項に記載の自律移動体。
【請求項８】
移動環境内に存在する障害物に関する３次元の位置データ群を含む環境データに含まれる前記位置データ群のうち第１の高さ範囲に属する位置データによって特定される障害物の位置情報と、前記環境データを用いて検出された平らな壁面の位置情報を、前記移動環境内に固定的に存在する固定障害物の位置を示す参照マップと照合することによって、自己の位置を推定する自己位置推定装置。
【請求項９】
移動環境内に存在する障害物に関する３次元の位置データ群を含む環境データに含まれる前記位置データ群のうち第１の高さ範囲に属する位置データによって特定される障害物の位置情報と、前記環境データを用いて検出された平らな壁面の位置情報を、前記移動環境内に固定的に存在する固定障害物の位置を示す参照マップと照合することによって、自己の位置を推定する自己位置推定ステップを備える、
自己位置推定処理をコンピュータに実行させるための自己位置推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自律的に移動する自律移動体および自律移動体の自己位置推定方法に関する。
【背景技術】
【０００２】
近年、ロボットや車両などが自ら目的を達するために考えて、自律移動をする自律移動体が開発されている。自律移動体は、自己が有する参照マップを参照することによって移動経路を計画し、当該移動経路に従って移動を行う。自律移動体において、自身の周囲の環境を認識して経路計画を立て、それに応じて移動する能力は重要である。
【０００３】
特許文献１では自律移動ロボットが、所定環境内において、自己の位置と壁面との距離を計測して現在位置を推定して目的地に向かう技術が開示されている。該文献では、自己位置の計測ができなくなった場合の回復手段の開示もされている。
また、特許文献２には床面に段差がある領域での移動においても、移動可能か否か判定するための環境マップを作成することができる装置が開示されている。
このように、近年では移動体が適切な自律移動をするための手法について開発されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００８−５９２１８号公報
【特許文献２】特開２００５−９２８２０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１の自律移動体は、自己の周囲を計測して、自己の位置と周囲の壁面との距離を測る。すなわち、所定環境内の壁面を基準として自己の位置を推定、または修正している。
しかし、前記所定環境内に固定的に存在する障害物は平らな壁面（以下、壁面）だけに限られない。例えば、円弧を描く壁面（以下、曲壁面）、円柱状の柱および所定環境内の床面と高さの異なる段差などが存在している場合もある。
【０００６】
段差は、単なる段差である場合もあるし、移動障害物である場合もある。移動障害物は人間、台車または、他の移動体などが挙げられる。また、柱もまた移動障害物である可能性がある。自律移動体は、固定障害物を基準に自己位置を推定するため、移動障害物を誤って認識した場合には、自己位置を誤って推定する可能性がある。
また、曲壁面および円柱状の柱などは平面検出手段などで検出することはできない。
【０００７】
本発明は、上述の問題を解決するためになされたものであり、自律移動体が自己位置を推定する際に移動障害物の影響を受け難くするとともに、壁面以外の障害物の計測情報も自己位置推定に利用することができる自律移動体および自己位置推定プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明に係る自律移動体は、移動環境を計測する視覚センサと、前記視覚センサによる計測結果を用いて、前記移動環境内に存在する障害物に関する３次元の位置データ群を含む環境データを生成する３次元環境データ生成部と、前記環境データを用いて、前記移動環境内に存在する平らな壁面を検出する壁面検出部と、前記環境データに含まれる前記位置データ群のうち第１の高さ範囲に属する位置データによって特定される障害物の位置情報、および前記壁面の位置情報を、前記移動環境内に固定的に存在する固定障害物の位置を示す参照マップと照合することによって、自己の位置を推定する自己位置推定部と、を備える自律移動体である。
人間や台車または他の移動体のような移動障害物は、壁や柱などの固定障害物に比べて、高さが低く、３次元空間内の一部に存在する。したがって、特定の高さよりも高い位置データを有する位置情報を基に参照マップと照合することによって、自律移動体が自己位置を推定する際に移動障害物の影響を受け難くするとともに、壁面以外の障害物の計測情報も自己位置推定に利用することが可能となり、正確な自己位置推定が可能となる。
【０００９】
また、前記第１の高さ範囲は、前記位置データ群の高さ分布に基づいて決定されてもよい。前述のように、障害物の高さによって、固定障害物か移動障害物かの傾向がわかる。本構成により、移動障害物を効率よく省いて、前記参照マップとの照合が可能となる。
【００１０】
さらに、前記第１の高さ範囲は、前記位置データ群のうち最も高さが高いデータを含むように決定されることが好ましい。前述のように、移動障害物などは壁面と比較すると高さが低いため、自己位置推定には不要な移動障害物を抽出せずに固定障害物である曲壁面および円柱状の柱などだけを抽出することが可能となる。
【００１１】
さらにまた、前記位置データ群は、前記環境データに含まれる前記位置データ群から前記壁面に対応する位置データ群を除いた残りのデータ群であってもよい。
【００１２】
ここで、前記位置データに信頼度を付与する信頼度設定部をさらに備えてもよく、前記信頼度設定部は、前記位置データ群のうち第１の高さ範囲に属する位置データによって特定される障害物の位置情報に設定する信頼度よりも、前記壁面の位置情報に設定する信頼度を高くし、前記自己位置推定部は、前記信頼度の高い位置情報を参照マップとの照合に優先的に用いる。この場合、壁や柱などの高さが高い物体と、段差などの高さが低い物体の信頼度を分けることが可能であり、固定障害物と移動障害物の区別が可能となる。なお、本発明でいう信頼度とは、３次元環境データから抽出された壁、柱、段差、その他の障害物が、移動障害物であるか否かを示す度合であり、信頼度が低いほど移動障害物として判断され、その情報を利用することなく自己位置推定することができる。
【００１３】
また、前記信頼度設定部は、前記環境データに含まれる前記位置データ群のうち第１の高さ範囲に属する位置データによって特定される障害物の位置情報および前記壁面の位置情報をグリッドデータに変換し、前記位置データの夫々が対応する各グリッドに、前記位置データの数に基づいて信頼度を設定してもよい。
【００１４】
また、前記自己位置推定部は、前記壁面の法線ベクトルを用いて、前記参照マップによって特定される前記固定障害物の配置に前記壁面を重ね合わせるための回転角度を算出することが好ましい。夫々の壁面の法線ベクトルが一致する回転角度を求めることで、夫々の点と点をマッチングして自己位置推定するよりも、正確な自己位置推定が可能となる。
【００１５】
他方、本発明に係る自己位置推定装置は、移動環境内に存在する障害物に関する３次元の位置データ群を含む環境データに含まれる前記位置データ群のうち第１の高さ範囲に属する位置データによって特定される障害物の位置情報と、前記環境データを用いて検出された平らな壁面の位置情報を、前記移動環境内に固定的に存在する固定障害物の位置を示す参照マップと照合することによって、自己の位置を推定することを特徴とする。
【００１６】
また、本発明に係る自己位置推定プログラムは、移動環境内に存在する障害物に関する３次元の位置データ群を含む環境データに含まれる前記位置データ群のうち第１の高さ範囲に属する位置データによって特定される障害物の位置情報と、前記環境データを用いて検出された平らな壁面の位置情報を、前記移動環境内に固定的に存在する固定障害物の位置を示す参照マップと照合することによって、自己の位置を推定する自己位置推定ステップを備える、自己位置推定処理をコンピュータに実行させるためのプログラムである。
【発明の効果】
【００１７】
本発明により、自律移動体が自己位置を推定する際に移動障害物の影響を受け難くするとともに、壁面以外の障害物の計測情報も自己位置推定に利用することが可能となり、正確な自己位置を推定することができる自律移動体およびその自己位置推定プログラムを提供することができる。
【図面の簡単な説明】
【００１８】
【図１】実施の形態１に係る移動体の制御系を示すブロック図。
【図２】自己位置推定部を説明するブロック図。
【図３】実施の形態１の所定環境の斜視図。
【図４】環境データから取得したデータを２次元に図示する説明図。
【図５】高さ範囲情報を取得する際の説明図。
【図６】実施の形態２のマッチング部のブロック図。
【図７】ＩＣＰアルゴリズムの説明図。
【図８】実施の形態１の自己位置推定プロセスのフローチャート。
【図９】実施の形態２の自己位置推定部のブロック図。
【図１０】２次元グリッドマップへ環境データを変換する説明図。
【図１１】実施の形態３のマッチング部のブロック図。
【発明を実施するための形態】
【００１９】
以下では、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略される。
【００２０】
<発明の実施の形態１>
図１に実施の形態１に係る自律移動体（以下、移動体）１００の構成を示す。移動体１００は視覚センサ２、３次元環境データ生成部４、壁面検出部６および自己位置推定部８など備えている。
【００２１】
視覚センサ２は、レーザレンジファインダ等のアクティブ距離センサを有している。そして、視覚センサ２は、所定環境の距離画像データを取得する。視覚センサ２が計測を行う範囲は、視覚センサ２の性能によって規定される。
【００２２】
なお、視覚センサ２としては、ＣＣＤ（Ｃｈａｒｇｅ  Ｃｏｕｐｌｅｄ  Ｄｅｖｉｃｅ）イメージセンサまたはＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ  Ｍｅｔａｌ  Ｏｘｉｄｅ  Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）イメージセンサ等の撮像素子を備えた複数のカメラを備えてもよい。そして、視覚センサ２は、これら複数のカメラによって撮影した画像データを用いて距離画像データを生成してもよい。具体的には、視覚センサ２は、複数のカメラによって撮影した画像データから対応点を検出する。次いで、視覚センサ２は、ステレオ視によって対応点の３次元位置を復元すればよい。ここで、複数の撮影画像における対応点の探索は、複数の撮影画像に対する自空間微分の拘束式を用いた勾配法や相関法等の公知の手法を適用して行えばよい。
【００２３】
３次元環境データ生成部４では、距離画像データの座標変換を行って環境データを生成する。なお、環境データとは、画像データに含まれる多数の計測点の位置ベクトルを３次元直交座標系で表したデータの集合である。ここで、環境データは位置データ群を含み、前記位置データ群の座標は、移動体１００に固定された装置座標系で表示されていてもよいし、環境に固定されたグローバル座標系で表示されていてもよい。
【００２４】
壁面検出部６は、環境データから平面を検出し、検出された平面のなかで床面に平行な法線ベクトルを持つ平面を壁面として検出する。また、壁面検出部６は、検出した平面に平面ＩＤを付与する。
本実施の形態における平面検出には、プレーンセグメントファインダ（Ｐｌａｎｅ  Ｓｅｇｍｅｎｔ  Ｆｉｎｄｅｒ（ＰＬＳ））と呼ばれる手法を用いればよい。プレーンセグメントファインダでは、まず、３次元の点群から２点をランダムにサンプリングする。次いで、当該２点のベクトルをクラスタリングすることにより、平面の位置姿勢を求めるアルゴリズムである。
具体的には、プレーンセグメントファインダでは、まず、環境データに対してランダマイズド３次元ハフ変換を行って、最大平面の傾きを検出する。次に、再び、ランダマイズド３次元ハフ変換を用いて、当該最大平面と原点との距離を検出する。そして、当該最大平面に属する点を抽出することにより、平面領域（プレーンセグメント）を検出する。
【００２５】
より具体的には、プレーンセグメントファインダにおいて、平面は、以下の数式（１）により表わされる。
ρ＝（ｘ０ｃｏｓ（θ）＋ｙ０ｓｉｎ（θ））ｃｏｓ（φ）＋ｚ０ｓｉｎ（φ）
・・・・・・（１）
数式（１）において、ρは平面から原点へ下ろした垂線の長さ、θは当該垂線をｘｙ平面に投影した線とｘ軸とのなす角、φは当該垂線とｘｙ平面とのなす角である。
そして、環境データ内の各測定点（ｘ０,ｙ０,ｚ０）を（ρ,θ,φ）空間に投影し、ピークを検出することにより、平面を検出する。プレーンセグメントファインダでは、複数に分割された平面であっても検出することができる。また、環境データの所定部分の測定点のみを（ρ,θ,φ）空間の部分空間に投影して、検出範囲を狭めることができる。これにより、計算コストを大幅に低減することも可能である。
【００２６】
さらに、多数の計測点（環境データ）から平面の方程式を表す平面パラメータ（例えば、法線ベクトル及び法線ベクトルの座標系原点からの距離）を検出するために、ハフ変換法やランダムサンプリング法が従来から用いられている。本実施の形態における平面検出に、従来から知られているこれらの手法を適用してもよい。これにより、前記平面の法線ベクトルを得ることができ、床面に平行な法線ベクトルを有する平面を壁面として認識できる。
【００２７】
本実施の形態における自己位置推定部８では、発明の理解を容易にするため、図２のように、壁面位置情報取得部１０、高さ範囲情報取得部１２およびマッチング部１４に分けて説明する。
【００２８】
壁面位置情報取得部１０は、壁面検出部６で検出された壁面に属する位置情報を取得する。また、高さ範囲情報取得部１２は、環境データから所定の基準で決められた高さ範囲に属する位置データによって特定される障害物の位置情報を取得する。本実施の形態では、図３のように壁面４１〜４３、段差４４および円柱４５で構成されている所定環境における３次元空間における環境データの取得について説明する。
【００２９】
取得する前記高さ範囲は、前記位置データ群の高さ分布に基づいて決定される。また、環境データ内に存在する位置データであって、最も高さの高い位置データを含むように決定されることが好ましい。これにより、背の低い移動障害物を容易に除外でき、より正確な自己位置推定が可能となる。また、前記高さ範囲に属する位置データは、環境データ内に存在する位置データであって、前記壁面検出部６で検出された壁面に対応する位置データを除いた環境データから決定されても良い。
【００３０】
壁面位置情報取得部１０および高さ範囲情報取得部１２で取得された位置情報を図４のような２次元の環境マップに図示して取得方法を説明する。
まず、壁面位置情報取得部１０は、前記壁面検出部６で検出された壁面４１〜４３の３次元の環境データに含まれる位置データ群を取得する。取得した壁面４１〜４３の位置データ群から２次元の位置情報を取得し、前記位置情報を２次元の環境マップに直線で図示した。
次に、高さ範囲情報取得部１２は、図５のように、環境データ中の前記高さ範囲に設定されたデータ取得空間４６に含まれる位置データを取得する。本実施の形態ではデータ取得空間４６に含まれる円柱４５の位置データ群から２次元の位置情報を取得し、前記位置情報を２次元環境マップの円で図示した。前記データ取得空間４６は高さに幅を持たせてもよく、環境データに含まれる位置データ数などによって高さの幅を任意に決めることが可能である。
これにより、柱などの背の高い固定障害物をマップに出力でき、より正確なマッチングが可能となる。
【００３１】
自己位置推定が適切にできない場合には、位置データ群を取得する高さ範囲を低く変更してもよい。これは自己位置の推定が正常に終了するまで繰り返し変更されてもよい。
【００３２】
マッチング部１４は、前記自己の周囲の参照マップと前記各情報取得部で取得したデータをマッチングし、移動体１００の自己位置を推定する。マッチングについては図６のマッチング部１４のブロック図を用いて説明する。本実施の形態ではＩＣＰ（Ｉｎｔｅｒａｃｔｉｖｅ  Ｃｌｏｓｅｓｔ  Ｐｏｉｎｔ）アルゴリズムを用いたマッチングについて記述する。
【００３３】
初期位置設定部１６は、前記各情報取得部１０、１２で取得したデータおよび参照マップの初期位置をデッドレコニングによる自己位置推定方法によって、ＩＣＰアルゴリズムを開始する以前に決めている。
【００３４】
対応壁面検出部１８は、ＩＣＰアルゴリズムを用いて、参照マップに含まれる壁面と対応する前記壁面位置情報取得部１０および前記高さ範囲情報取得部１２で取得した位置情報に含まれる壁面を検出する。
図７を用いて、ＩＣＰアルゴリズムによる処理を説明する。ＩＣＰアルゴリズムは、２つの異なる座標系のデータを合わせるために用いる。本実施の形態では、参照マップに含まれる位置情報と前記壁面位置情報取得部１０および高さ範囲情報取得部１２で検出された壁面の位置情報を前記２つの異なる座標系のデータとして処理をする。一方のデータに含まれる点（図７において白抜きの丸で示す）から最も近傍にある他方のデータに含まれる点（図７においてハッチングが付された丸で示す）に着目し、当該他方のデータに含まれる点が属する平面の平面ＩＤを検出する。そして、対応壁面検出部１８では一方のデータに含まれる平面に属するすべての点から最も近傍にある他方のデータに含まれる点が属する平面ＩＤとして最も多く検出された平面ＩＤを有する平面を、対応する壁面として検出する。
【００３５】
剛体変換パラメータ算出部２０は、前記対応壁面検出部１８において検出された壁面および参照マップならびに高さ範囲情報取得部１２で取得した柱などを用いて、２つのデータが一致するような回転Ｒおよび並進Ｔを求める。
この座標変換を簡単にするため、一方の形状の座標系を固定し、もう一方の形状を座標変換する。データ全体の点に対してこれらの対応点間で定義されるユークリッド距離が最小化される回転Ｒおよび並進Ｔを算出する。
なお、本実施の形態におけるマッチングは、ＩＣＰアルゴリズムを用いた手法を紹介したが、各データのマッチングが可能であればどのような手法であってもよい。また、ＩＣＰアルゴリズムの代わりにＷｅｉｓｓのマッチング手法またはパーティクルフィルタを用いたマッチング手法などを利用してもよい。パーティクルフィルタなどを利用することによって、３次元環境内に壁面が存在せず、柱や段差などが存在する場合であっても、対応壁面検出部１８の処理を飛ばして回転Ｒおよび並進Ｔを算出することも可能である。
【００３６】
図８は実施の形態１に係る移動体１００の自己位置推定手法を示すフローチャートである。３次元計測ステップ（Ｓ１０１）は、移動体１００が周囲の環境情報を視覚センサなどの情報取得装置を用いて計測するステップである。３次元環境データ生成ステップ（Ｓ１０２）は、Ｓ１０１で取得した周囲の環境情報を基に３次元の位置データ群を含む環境データに変換する。壁面検出ステップ（Ｓ１０３）は、環境データから平面検出手法を用いて壁面を検出する。壁面位置情報取得ステップ（Ｓ１０４）は、Ｓ１０３で検出された壁面に属する壁面位置情報を取得する。高さ範囲情報取得ステップ（Ｓ１０５）は、環境データ中の所定の基準で決められた高さ範囲に含まれる位置データ群から位置情報を取得する。マッチングステップ（Ｓ１０６）では、移動体１００が備える自己の周囲の参照マップと各位置情報取得ステップで取得された位置情報とをマッチングして、移動体１００は自己位置を推定する。
壁面検出ステップにおいて、壁面の検出がされなかった場合には、高さ範囲情報取得ステップに移行する。また、マッチングが適切に行われなかった場合には、再度高さ範囲情報取得ステップに移行し、位置データ群を取得する高さ範囲を変更したうえで、位置情報を取得し、マッチングを再度行う。該マッチングは適切に行われるまで、あるいは、全ての高さ範囲情報を取得するまで繰り返されてもよい。
【００３７】
上述した自己位置推定部８による自己位置推定手法は、図８のフローチャートに示した処理手順が記述された自己位置推定プログラムをマイクロプロセッサ等のコンピュータに実行させることによって実現してもよい。
【００３８】
自己位置推定プログラムは、様々な種類の記憶媒体に格納することが可能であり、また、通信媒体を介して伝達されることが可能である。ここで、記憶媒体には、例えば、フレキシブルディスク、ハードディスク、磁気ディスク、光磁気ディスク、ＣＤ−ＲＯＭ，ＤＶＤ、ＲＯＭカートリッジ、不揮発性ＲＡＭカートリッジ等が含まれる。また、通信媒体には、電話回線等の有線通信媒体、マイクロ波回線等の無線通信媒体等が含まれ、インターネットも含まれる。
【００３９】
<発明の実施の形態２>
実施の形態２について図９を用いて説明する。移動体１００の自己位置推定部８は、信頼度設定部２２をさらに有していてもよい。信頼度設定部２２は、前記壁面位置情報取得部１０および前記高さ範囲情報取得部１２で取得された位置情報に信頼度を設定する。前記のように壁面は平面検出手法を用いて検出されるため、床面に垂直な平面を確実に検出することができるが、弧を描く壁面および円柱状の柱などは平面検出手段では検出することができない。また、柱や段差などは移動障害物の可能性がある。そこで、曲壁面、柱または段差などの障害物の信頼度を低く設定することで、より正確な自己位置推定が可能である。なお、信頼度を設定するタイミングは、壁面位置情報取得部１０および高さ範囲情報取得部１２で位置情報を取得する前でもよく、その場合は、環境データに対して信頼度を設定してもよいし、位置データに対して信頼度を設定してもよい。
【００４０】
ここで、信頼度設定手法は、壁面の信頼度を１とし、曲壁面、段差または柱の信頼度については数式（２）で設定してもよい。
（段差の信頼度）＝（段差の高さ）／（壁面の高さ）    ・・・・・・（２）
【００４１】
また、信頼度設定部２２は、図１０のように、前記壁面位置情報取得部１０および前記高さ範囲情報取得部１２で取得した位置情報を２次元グリッドデータに変換してもよく、この場合には各グリッドへ投影された位置データの数に応じて、各グリッドの信頼度を決定してもよい。床面に平行な法線ベクトルを有する平面であっても壁面ではなく、段差の場合もあり得る。段差の場合には、壁面に比べて位置データ数が少ないため、グリッドの信頼度は減少する。これにより、壁面と段差の区別が可能となる。
なお、視覚センサ２が、移動環境内に存在する障害物に関する３次元の位置データ群を含む環境データを生成した際に、傾斜したときは、信頼度を低く設定してもよい。正確なマッチングができないおそれがあるからである。
【００４２】
自己位置推定部８では、設定された信頼度を参照し、特定の閾値を満たす信頼度の位置情報を用いてマッチングする。但し、移動体１００が備える参照マップとのマッチングが適切に行われかった場合は、前記閾値を変動させることも可能である。
【００４３】
<発明の実施の形態３>
次に、前記のマッチングとは異なるマッチング手法について述べる。実施の形態３では、環境データ中の壁面データが有する法線ベクトルに着目してマッチングをする。なお、移動体１００が備える自己の周囲の参照マップは、あらかじめその壁面の法線ベクトル情報を保持している。
【００４４】
図１１は実施の形態３のマッチング部１４のブロック図である。
なお、本実施の形態３では、初期位置の設定については、実施の形態１と同様に、初期位置設定部１６においてデッドレコニングによる自己位置推定方法によって設定されている。また、対応する壁面の検出についても、実施の形態１と同様に、対応壁面検出部１８において行われる。よって、実施の形態１と共通しているこれらの処理に関する説明を省略する。
【００４５】
回転Ｒ算出部２４は、前記対応壁面検出部１８で検出された対応する壁面が有する各法線ベクトルの差が最小になる回転Ｒを算出する。壁面検出部６によって検出された壁面は、法線ベクトルとともに検出されており、壁面位置情報取得部１０によって取得された壁面位置情報もまた法線ベクトルを備えている。式（３）を利用して回転Ｒを求めることができる。ｎｏｒｍａｌ（）は法線ベクトルを表している。
Ｒ＝Ｍｉｎ（Ｒ・ｎｏｒｍａｌ（ａ）−ｎｏｒｍａｌ（Ａ）＋Ｒ・ｎｏｒｍａｌ（ｂ）−ｎｏｒｍａｌ（Ｂ）＋・・・）            ・・・・・・（３）
【００４６】
並進Ｔ算出部２６は、壁面位置情報取得部１０が取得した位置情報または自己の周囲の参照マップに含まれる壁面の位置情報をＲ回転させ、再度マッチング処理をして並進Ｔを算出する。環境データまたは取得した位置情報に含まれる各点を用いて回転Ｒおよび並進Ｔを求めるよりも、各壁面が有する法線ベクトルを用いて回転Ｒを求めた後に、再度マッチング処理をして並進Ｔを求めることにより、点と点の誤対応状態でのマッチングを防ぎ、より精度の高いマッチングが可能となる。再度のマッチングではＩＣＰアルゴリズム、Ｗｅｉｓｓのマッチング手法またはパーティクルフィルタを用いたマッチング手法などを利用することができる。
【符号の説明】
【００４７】
１００    ・・・自律移動体
２        ・・・視覚センサ
４        ・・・３次元環境データ生成部
４１      ・・・壁面
４２      ・・・壁面
４３      ・・・壁面
４４      ・・・段差
４５      ・・・円柱
４６      ・・・データ取得空間
６        ・・・壁面検出部
８        ・・・自己位置推定部
１０      ・・・壁面位置情報取得部
１２      ・・・高さ範囲情報取得部
１４      ・・・マッチング部
１６      ・・・初期位置設定部
１８      ・・・対応壁面検出部
２０      ・・・剛体変換パラメータ算出部
２２      ・・・信頼度設定部
２４      ・・・回転Ｒ算出部
２６      ・・・並進Ｔ算出部
【図１】
2011008320.tif 000003
【図２】
2011008320.tif 000004
【図３】
2011008320.tif 000005
【図４】
2011008320.tif 000006
【図５】
2011008320.tif 000007
【図６】
2011008320.tif 000008
【図７】
2011008320.tif 000009
【図８】
2011008320.tif 000010
【図９】
2011008320.tif 000011
【図１０】
2011008320.tif 000012
【図１１】
2011008320.tif 000013