JP2007322138A - 移動装置及び移動装置の自己位置推定方法 - Google Patents

Abstract

【課題】自律移動ロボット等の移動装置において自己位置推定を行う際の推定精度を向上させる。
【解決手段】ロボット１は、カメラ部１０１によって作業環境内に存在する物体を撮像した画像データを入力し、レーザレンジファインダ１０７によって作業環境内に存在する物体との距離を計測して得たレンジデータを入力する。また、i）再投影誤差算出部１０３が、画像データから検出したランドマークの画像上での位置と、記憶部１０６に格納された地図情報に含まれる作業空間におけるランドマークの位置を前記画像データに再投影した位置との再投影誤差を算出し、ii）位置誤差算出部１０８が、レンジデータと地図情報に含まれる形状データとの位置合わせ誤差を算出する。さらに、最適化部１０９が、これら再投影誤差及び位置合わせ誤差を含む目的関数をロボット１の周囲環境に応じて決定し、決定した目的関数を最適化してロボット１の自己位置を算出する。
【選択図】図１

Description

本発明は、作業環境を自律的に移動する移動ロボット等の移動装置の自己位置推定方法に関する。

作業環境内を自律的に移動する移動ロボットの自己位置推定の方法として、外界観測による自己位置推定法が知られている（例えば特許文献１を参照）。具体的には、作業空間内に存在するランドマークの位置を地図情報として予め移動ロボットに記憶させておき、作業環境中において位置が既知であるランドマークを観測することにより、観測したランドマークとの相対的な位置関係から自己位置の算出を行うものである。ランドマークとしては、作業環境に元々存在している壁面などの自然特徴のほか、人工的に作業環境に配置した標識が用いられる。

また、ランドマークを観測する手法としては、レーザレンジファインダ等のアクティブ距離センサを用いる手法や、カメラで撮像した画像データを利用する手法が一般的である。アクティブ距離センサの代表例であるレーザレンジファインダを用いる手法は、レーザ光を周囲に照射し、その反射光を観測することによって得たレンジデータを地図情報として予め記憶された三次元形状データと照合することによって自己位置を推定する。一方、カメラで撮像した画像データを利用する手法は、ＣＣＤイメージセンサ又はＣＭＯＳイメージセンサ等の撮像素子を備えたカメラによって周囲環境を撮像し、得られた画像データに含まれるランドマークを基準点として抽出し、幾何的な演算によってランドマークと撮像位置との距離を算出して自己位置を推定する。
特開２００５−２４２４０９号公報

上述したようにランドマークを観測する方法には、レーザレンジファインダ等のアクティブ距離センサを用いる方法、カメラを用いる方法等がある。しかしながら、これらの方法はあらゆる作業環境において十分な観測精度が得られるとは限らない。例えば、レーザレンジファインダは、障害物が多い環境や、作業環境内に反射率の高い物体や黒色物体が多い場合などには三次元形状データとの照合精度が低下するため、このような環境下では自己位置推定の精度が低下する。また、カメラで撮像した画像データを用いる方法では、照明が暗くコントラストの高い濃淡画像が得られない場合などには、画像データから基準点となるランドマークを正確に抽出することとが困難であるため、このような環境下では自己位置推定の精度の低下を招く。つまり、１つの観測手法に依存して自己位置推定を行ったのでは、様々な環境下において自己位置推定の精度を高めることは困難である。

本発明は、上述した事情を考慮してなされたものであり、自律移動ロボットに代表される移動装置において自己位置推定を行う際の推定精度の向上を目的とする。

本発明にかかる移動装置は、作業環境を自律移動する移動装置であって、前記作業環境の地図情報を格納した記憶部と、前記作業環境内に存在する物体との距離を計測してレンジデータを生成する距離センサと、前記作業環境内に存在する物体を撮像した画像データを生成するカメラ部と、前記レンジデータを前記地図情報に含まれる形状データと照合することによって、当該移動装置の自己位置を推定する第１の推定手段と、前記画像データからランドマークを検出し、検出したランドマークの画像上での位置情報及び前記地図情報に含まれる前記作業空間におけるランドマークの位置情報を用いた演算によって、当該移動装置の自己位置を推定する第２の推定手段と、当該移動装置の周囲環境に応じて、前記第１の推定手段及び前記第２の推定手段の信頼度を決定する信頼度決定手段と、前記第１の推定手段による推定結果と前記第２の推定手段による推定結果とを、前記信頼度決定手段によって決定された信頼度に応じた重み付けを行って統合することにより、当該移動装置の自己位置を決定する自己位置決定手段とを備える。

このように本発明にかかる移動装置は、カメラ部によって撮像した画像データ及び距離センサによって取得したレンジデータの双方を用いて自己位置の推定を行うものである。さらに当該移動装置は、周囲環境を応じて画像データによる自己位置推定及びレンジデータによる自己位置推定の信頼度を決定し、決定した信頼度を反映して２つの自己位置推定手段の結果を統合して最終的な自己位置を決定するものである。このため、１つの観測手法に依存して自己位置推定を行う場合に比べて、様々な環境下において自己位置推定の精度を高めることができる。

また、上述した本発明にかかる移動装置において、前記自己位置決定手段は、前記第１の推定手段による自己位置推定の誤差と前記第２の推定手段による自己位置推定の誤差とを含む目的関数を、前記第１の推定手段及び前記第２の推定手段の信頼度を反映して決定し、決定した目的関数を最適化して得られる前記作業空間内での位置を当該移動装置の自己位置としてもよい。

ここで、前記第１の推定手段による自己位置推定の誤差は、前記レンジデータと前記形状データとの位置合わせ誤差とし、前記第２の推定手段による自己位置推定の誤差は、前記画像データへのランドマークの再投影誤差とすることができる。

また、前記信頼度決定手段は、前記作業環境の明るさに基づいて、前記第１の推定手段及び前記第２の推定手段の信頼度を決定してもよい。これにより、例えば、照明が暗いために画像データのコントラストが不十分である等のために、画像データによる自己位置推定の信頼度を低下させること等が可能となる。

また、前記カメラ部は、複数の異なる位置から撮像した複数の画像データを生成するものとしてもよい。さらに、前記第２の推定手段は、前記画像データに含まれるランドマークの数及び前記画像データに含まれるランドマークの画像上での距離の少なくとも一方に基づいて、前記複数の画像データから自己位置の推定に用いる画像データを選択してもよい。これにより、自己位置推定に適した画像データを選択して、自己位置推定を行うことができる。

さらに、前記第２の推定手段は、自己位置の推定に用いる画像データを選択した上で、複数の画像データを用いる場合と１の画像データを用いる場合とで、異なる演算によって自己位置を算出してもよい。

一方、本発明にかかる自己位置推定方法は、作業環境の地図情報を格納した記憶部を有し、作業環境を自律的に移動する移動装置が行う自己位置推定方法である。具体的には、距離センサによって取得したレンジデータを、前記地図情報に含まれる形状データと照合することによって第１の自己位置推定値を算出する。また、カメラによって撮像した画像データからランドマークを検出し、検出したランドマークの画像上での位置情報及び前記地図情報に含まれるランドマークの前記作業空間における位置情報を用いた演算によって第２の自己位置推定値を算出する。さらに、前記移動装置の周囲環境に応じて、前記第１の自己位置推定値及び前記第２の自己位置推定値の信頼度を決定し、前記第１の自己位置推定値及び前記第２の自己位置推定値を、決定した信頼度に応じた重み付けにより統合した値を自己位置とする。

このように、本発明にかかる自己位置推定方法は、周囲環境を応じて画像データによる自己位置推定及びレンジデータによる自己位置推定の信頼度を決定し、決定した信頼度を反映して２つの自己位置推定手段の結果を統合して最終的な自己位置を決定するものである。このため、１つの観測手法に依存して自己位置推定を行う場合に比べて、様々な環境下において自己位置推定の精度を高めることができる。

また、本発明にかかる他の自己位置推定方法は、作業環境の地図情報を格納した記憶部を有し、作業環境を自律的に移動する移動装置が行う自己位置推定方法であって、まず、距離センサによって前記作業環境内に存在する物体との距離を計測したレンジデータ、及び、カメラによって前記作業環境内に存在する物体を撮像した画像データを入力する。次に、i）前記レンジデータを前記地図情報に含まれる形状データと照合することにより推定した自己位置の第１の誤差と、ii）前記画像データから検出したランドマークの画像上での位置情報及び前記地図情報に含まれるランドマークの前記作業空間における位置情報を用いた演算により推定した自己位置の第２の誤差とを含む目的関数を、当該移動装置の周囲環境に応じて決定し、決定した目的関数を最適化して得られる前記作業空間内での位置を当該移動装置の自己位置とする。

このように、画像データ及びレンジデータの信頼度を反映した目的関数を最適化することにより自己位置を推定することにより、１つの観測手法に依存して自己位置推定を行う場合に比べて、様々な環境下において自己位置推定の精度を高めることができる。

本発明により、自己位置推定の精度を向上させた移動装置及び移動装置の自己位置推定方法を提供することができる。

以下では、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略する。なお、以下に示す実施の形態は、自律移動ロボットに対して本発明を適用したものである。

発明の実施の形態１．
本実施の形態にかかるロボット１の要部構成を図１に示す。ロボット１は、カメラ部１０１及びレーザレンジファインダ１０７の２つのセンサによって外界の情報を入力し、これら２つのセンサによって取得した画像データ及びレンジデータの夫々に基づいて自己位置推定を行うことができる。さらに、外部環境に応じて２つの推定結果の信頼度を決定し、決定した信頼度を反映した重み付けを行って２つの推定結果を統合することにより、最終的な自己位置推定の結果を得るものである。以下では、図１を参照してロボット１が有する各機能ブロックについて説明する。

カメラ部１０１は、２台のカメラ１０１ａ及び１０１ｂを有する。カメラ１０１ａ及び１０１ｂはそれぞれ、ＣＣＤイメージセンサ又はＣＭＯＳイメージセンサ等の撮像素子を備えており、外界を撮像して得たデジタル画像データをランドマーク検出部１０２に出力する。

ランドマーク検出部１０２は、カメラ部１０１から画像データを入力し、画像中に含まれる複数のランドマークを検出する。上述したように、ランドマークとは、自律移動ロボットが移動する環境に固定された基準点であり、作業環境に元々存在している壁面などの自然特徴のほか、人工的に作業環境に配置した標識を用いてもよい。作業空間内に存在するランドマークの位置及びランドマークの形状を示す情報は、地図情報として予め記憶部１０６に格納されている。具体例を示すと、ランドマークの様々な見え方を示すテンプレート画像を記憶部１０６に格納しておき、これらのテンプレート画像と撮像された画像データとの相関検出、パターンマッチング等によってランドマークを検出することができる。

三次元位置算出部１０３は、ランドマーク検出部１０２において検出した画像データ上でのランドマーク位置を用いて、カメラ座標系でのランドマークの三次元位置を算出する。ここで、カメラ座標系とは、カメラ部１０１に固定された座標系である。なお、カメラ座標系でのランドマークの三次元位置の算出は、例えば、２つのカメラ１０１ａ及び１０１ｂによって撮影された２つの画像のステレオ視によって、ランドマークまでの距離を獲得することにより算出できる。

自己位置推定部１０４は、三次元位置算出部１０３において算出した２以上のランドマークのカメラ座標系での位置、及び作業区間に固定された座標系（以下、世界座標系と呼ぶ）でのランドマーク位置を用いて、世界座標系でのロボット位置を算出する。なお、ランドマークの世界座標系での位置は、地図情報として予め記憶部１０６に格納された情報である。

再投影誤差算出部１０５は、三次元位置算出部１０３において算出したランドマークの三次元位置を画像上に再投影した場合の再投影誤差を算出する。ここで、再投影誤差は、地図情報として記憶されたランドマークの三次元位置を画像上に再投影して得られる座標と、ランドマーク検出部１０２において検出したランドマークの画像上での座標との距離又は二乗距離によって定義される。

記憶部１０６は、作業空間内に存在するランドマークの位置及びランドマークの形状を示す情報を格納している。なお、ランドマークの形状を示す情報としては、上述したテンプレート画像のほか、後述するレンジデータによる位置推定に用いるための三次元形状データを格納している。

レーザレンジファインダ１０７は、レーザ光を周囲に照射し、その反射光を観測することによって得たレンジデータを位置合わせ誤差算出部１０８に出力する。

位置合わせ誤差算出部１０８は、レーザレンジファインダ１０７によって得たレンジデータを記憶部１０６に格納されている三次元形状データと照合して位置合わせ誤差を算出する。具体的には、自己位置推定部１０４において算出したロボット位置から見た三次元形状とレンジデータとの照合を行う。ここで、位置合わせ誤差は、レンジデータと三次元形状データとを照合した際のレンジデータの対応点と三次元形状データの対応点との距離又は二乗距離により定義される。

最適化部１０９は、再投影誤差算出部１０５で算出される再投影誤差と位置合わせ誤差算出部１０８で算出される位置合わせ誤差とを入力し、再投影誤差と位置合わせ誤差を含む目的関数が最小となるように最適化を実行することにより、目的関数に最適値を与える位置をロボット１の自己位置に決定する。

なお、上述したように、カメラ部１０１が取得する画像データ及びレーザレンジファインダ１０７が取得するレンジデータの信頼度は、照明の明るさや障害物の多さなどのロボット１が置かれた外部環境に依存して変動する。このため、最適化部１０９は、環境情報入力部１１０が取得した外部環境情報を入力し、外部環境に応じて最適化の対象とする目的関数を変更する。具体的には、画像データ及びレンジデータのうち、信頼度が相対的に低いほうのデータから得られた自己位置推定の誤差、つまり再投影誤差又は位置合わせ誤差の目的関数への寄与が小さくなるように、再投影誤差若しくは位置合わせ誤差又はこれら両方の誤差を目的関数に取り込む際の重み付けを変更する。

環境情報入力部１１０は、ロボット１の外部環境の状況が反映された環境情報を取得する。環境情報としては様々な情報を収集することができる。例えば、カメラ部１０１が取得する画像データの信頼度を示す情報として、照明の明るさ、画像データのコントラスト、画像データに重畳したノイズ量の他、特徴的なテクスチャの有無などが挙げられる。また、レーザレンジファインダ１０７が取得するレンジデータの信頼度を示す情報として、障害物の存在、特徴的な形状を備えた物体の存在、反射率の高い物体や黒色物体の存在、レーザレンジファインダ１０７が検出する反射光の強度、移動目標地点までの残余距離、レンジデータ地図の解像度などが挙げられる。

最後に、制御部１１１は、最適化部１０９が出力する自己位置推定結果を入力して経路計画を行い、ロボット１の移動機構（不図示）を駆動する。

続いて以下では、図２を参照しながら、ロボット１が行う自己位置推定処理の詳細を説明する。なお、ここでは、説明を簡単にするため、二次元における自己位置を算出する場合について説明する。図２は、ロボット１が行う自己位置推定の手順を示すフローチャートである。ステップＳ１０１では、カメラ部１０１によって撮像した画像データを用いて自己位置を算出する。

ここで、ランドマークを撮影した画像データからロボット１の自己位置を算出する具体例を説明する。図３に示すように、２つのランドマーク３1１及び３1２の世界座標系Σ_{ＷＯＲＬＤ}での位置（Ｘ_１，Ｙ_１）及び（Ｘ_２，Ｙ_２）と、カメラ座標系Σ_ＣＡＭでの位置（ｘ_１，ｙ_１）及び（ｘ_２，ｙ_２）が既知であるとき、世界座標系Σ_{ＷＯＲＬＤ}でのロボット１の位置（Ｘ_Ｃ，Ｙ_Ｃ）及び姿勢θは、以下の（１）〜（３）式によって算出することができる。なお、（１）式及び（２）式中のＤは、以下の（４）式で表される。

ステップＳ１０２では、再投影誤差Ｅ１を算出する。再投影誤差Ｅ１は、三次元位置算出部１０３において算出したランドマークの三次元位置を画像上に再投影して得られる座標と、ランドマーク検出部１０２において検出したランドマークの画像上での座標との二乗距離として定義される。このとき、再投影誤差Ｅ１は以下の（５）式により算出できる。

ここで、Ｋは画像枚数、Ｎはランドマーク数である。ｍ_iは、撮影した画像データから検出した画像座標系におけるランドマーク位置である。Ｍ_ｉは、ランドマークの世界座標系における位置である。また、変換ｆ_ｊ（Ｍ_ｉ）は、世界座標系の座標Ｍ_ｉを画像データに再投影する変換を表す。世界座標系Σ_{ＷＯＲＬＤ}の座標Ｍ_ｉ（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）をｊ番目の画像座標系Σ_ＩＭＧの点（ｕ_ｉ，ｖ_ｉ）に射影する変換ｆ_ｊ（Ｍ_ｉ）は、射影行列Ｐ_ｊを用いて以下の（６）式により表すことができる。ここで、射影行列Ｐ_ｊは、ロボット１の位置（Ｘ_Ｃ，Ｙ_Ｃ）及び姿勢θの関数であり、ステップＳ１０１で算出した自己位置を用いて算出する。

（６）式を用いて変換後の座標（ｕ_ｉ，ｖ_ｉ）を算出すれば、（５）式に示した再投影誤差Ｅ１は、以下の（７）式により求めることができる。なお、ｍ_ｉｕ及びｍ_ｉｖは、画像データから検出した画像座標系におけるランドマークの座標（ｍ_ｉｕ，ｍ_ｉｖ）である。

ステップＳ１０３では、レーザレンジファインダ１０７から入力したレンジデータと、地図情報として有する三次元データとの位置合わせ誤差Ｅ２を算出する。位置合わせ誤差Ｅ２は、以下の（８）式により算出できる。

（８）式において、ｐ_ｊは地図情報として保持する三次元データの参照点を意味し、ｑ_ｊは参照点ｐ_ｊに対応するレンジデータ上の参照点である。また、Ｒは参照点ｐ_ｊに対する回転行列であり、ｔは参照点ｐ_ｊに対する平行移動行列である。

ステップＳ１０４では、環境情報入力部１１０が収集した環境情報を最適化部１０９に入力する。ステップＳ１０５では、最適化部１０９が、入力された環境情報に応じて目的関数を決定する。最適化の対象となる目的関数の一例を（９）式に示す。

（９）式に含まれるλが環境情報に応じて変更されるパラメタであり、最適化部１０９は、環境情報に応じてλを決定する。パラメタλが１に近いほど、レンジデータより画像データを信頼して自己位置推定を行うことを意味し、逆にパラメタλが０に近いほど、画像データよりレンジデータを信頼して自己位置推定を行うことを意味する。例えば、移動目標物体に近づいて自己位置推定の精度が要求される場合や、障害物の存在等のために、レンジデータの信頼度が画像データの信頼度に比べて相対的に低いと判定した場合には、パラメタλを１に近づければ良い。また、カメラ部１０１が撮像した画像からランドマークを検出できない場合や、照明が暗いために画像データのコントラストが不十分である等のために、画像データの信頼度がレンジデータの信頼度に比べて相対的に低いと判定した場合には、パラメタλを０に近づければ良い。

ステップＳ１０６では、ステップＳ１０５で決定した目的関数の最適化を行い、目的関数が最小となる世界座標系でのロボット１の位置（Ｘ_Ｃ，Ｙ_Ｃ）及び姿勢θを決定する。目的関数の最適化は、ニュートン法、Levenberg-Marquart 法等の降下法のほか、公知の最適化方法を適用して行えば良い。

ステップＳ１０７では、最適化部１０９が決定した自己位置を制御部１１１に出力し、自己位置推定処理を終了する。

上述したように、本実施の形態にかかるロボット１は、カメラ部１０１によって撮像した画像データ及びレーザレンジファインダ１０７によって取得したレンジデータの双方を用いて自己位置の推定を行うものである。さらにロボット１は、周囲環境の環境情報を入力し、入力した環境情報を規準として、画像データ及びレンジデータの相対的な信頼度を決定し、決定した信頼度を反映した目的関数を最適化することにより自己位置を推定するものである。このため、１つの観測手法に依存して自己位置推定を行う場合に比べて、様々な環境下において自己位置推定の精度を高めることができる。

またさらに、ロボット１は、周囲環境に応じて画像データ及びレンジデータの相対的な信頼度を動的に変更して、自己位置を推定することができる。このため、障害物の侵入などの急激な周囲環境の変動による自己位置推定精度の悪化を抑制することができ、信頼性の高い自己位置推定を行うことができる。

なお、自律移動を行うロボットにおいて、レーザレンジファインダ等の距離センサによって取得したレンジデータを用いて高精度な自己位置推定を行おうとすると、高精細な三次元形状データを地図情報として記憶しておく必要がある。このため、地図情報を記憶しておくメモリ量が大きくなることや、自己位置推定に要する計算量が大きくなること等の問題がある。

これに対して、本実施の形態のロボット１であれば、例えば物体把持を行う場合などに把持対象物体から遠い場合にはレンジデータを用いて比較的粗く自己位置推定を行い、把持対象物体との距離が所定位置を下回った場合に、画像データのみ又は画像データとレンジデータの併用によって精度の高い自己位置推定を行うことが可能である。具体的には、特定の物体等の移動目標をランドマークとして記憶しておき、移動目標までの距離を環境情報として入力すればよい。これにより、レンジデータとの照合を行うための３次元形状データの解像度を下げることができるため、三次元形状データの記憶容量の低減、レンジデータを用いた自己位置推定の計算量の低減を図ることができる。

なお、上述した本実施の形態にかかるロボット１のうちカメラ部１０１及びレーザレンジファインダ１０７を除く他の構成要素は、プログラムを実行するＣＰＵ（Central Processing Unit）を有するコンピュータシステムにより構成することが可能である。具体的には、ＣＰＵが内部に備える又はＣＰＵの外部に接続されるＲＯＭ又はフラッシュメモリ等の記憶部に、ランドマーク検出部１０２、三次元位置算出部１０３、自己位置推定部１０４、再投影誤差算出部１０５、位置合わせ誤差算出部１０８、及び最適化部１０９が行う処理をコンピュータシステムに実行させるための１又は複数のプログラムを格納しておき、当該プログラムをＣＰＵで実行することとすればよい。また、記憶部１０６は、コンピュータシステムが備えるハードディスクやフラッシュメモリ等の不揮発性の記憶部とすればよい。

発明の実施の形態２．
発明の実施の形態１にかかるロボット１は、再投影誤差Ｅ１及び位置合わせ誤差Ｅ２を反映した目的関数の最適化により、精度の高い自己位置推定を行うこととした。しかしながら、画像データを用いて推定した自己位置（第１の候補値と呼ぶ）と、レンジデータを用いて推定した自己位置（第２の候補値と呼ぶ）を、環境情報を反映した重み付けを行って統合することにより、簡便な自己位置推定を行うこととしてもよい。このような簡便な自己位置推定を行うロボット２の構成を図４に示す。

自己位置推定部２０１は、レンジデータと地図情報として記憶部に格納された三次元形状データとを照合することにより第２の候補値を算出する。統合部２０２は、自己位置推定部１０４が画像データを用いて算出した第１の候補値と、自己位置推定部２０１がレンジデータを用いて算出した第２の候補値とを、環境情報に応じて決定した重み付けを行って統合することで自己位置を算出する。統合部２０２による自己位置の算出は、例えば、以下の（１０）式により行うことができる。（１０）式において、（Ｘ_Ｃ１，Ｙ_Ｃ１，θ_１）は第１の候補、（Ｘ_Ｃ２，Ｙ_Ｃ２，θ_２）は第２の候補、（Ｘ_Ｃ，Ｙ_Ｃ，θ）は統合部２０２が算出する自己位置である。また、λは環境情報に応じて決定されるパラメタであり、第１の候補値及び第２の候補値の重みに対応する。

発明の実施の形態３．
発明の実施の形態１では、カメラ部１０１が備える２つのカメラ１０１ａ及び１０１ｂによるステレオ視によって、撮像されたランドマークのカメラ座標系における位置、つまりロボットから見たランドマークの相対位置を算出することにより、自己位置を求める場合を例示した。しかしながら、２つのカメラ１０１ａ及び１０１ｂによって撮影した画像データが必ずしもステレオ視に適したものとは限らない。例えば、一方の画像データには、ランドマークが１つしか写っていない場合や、撮像された複数のランドマークが近接している場合には、ステレオ視に適さない画像となる。このように、ステレオ視に適した画像データが得られない状況では、１枚の画像データを用いた自己位置推定を行う構成とすることにより、自己位置推定の信頼性を向上させることができる。なお、以下では、発明の実施の形態１で説明した２つの画像データによるランドマーク認識を"複眼によるランドマーク認識"と呼ぶ。また、１つの画像データによるランドマーク認識を"単眼によるランドマーク認識"と呼ぶ。

カメラ部１０１で撮像した画像データに応じて、"複眼によるランドマーク認識"と"単眼によるランドマーク認識"を切り替えて自己位置推定を行う手順を図５のフローチャートを用いて説明する。なお、図５のフローチャートが示す処理は、図２に示したフローチャートのステップＳ１０１に対応するものである。

ステップＳ２０１では、カメラ１０１ａ及び１０１ｂによって撮像した２つの画像データのそれぞれから、２以上のランドマークが検出できたか否かを判定する。２つの画像データのうちの一方からしか２以上のランドマークが検出できなった場合は、ステップＳ２０４に進み、単眼によるランドマーク認識によって自己位置推定を行う。

２つの画像データから共に、２以上のランドマークが検出された場合は、ステップＳ２０２において、２つのランドマーク間の画像上での距離が予め定めた所定値以上であるか否かを判定する。所定値以下である場合は、ステレオ視に適さない画像であるため、ステップＳ２０４にて単眼によるランドマーク認識によって自己位置推定を行う。２つのランドマーク間の画像上での距離が、２つの画像共に所定値より大きい場合は、ステップＳ２０３に進み、複眼によるランドマーク認識によって自己位置推定を行う。

単眼によるランドマーク認識は、世界座標系における三次元位置が既知である２以上ランドマークが撮像された単一画像を用いて、そのような画像を撮影可能なカメラ位置及びロボット位置を特定するものである。単眼によるランドマーク認識によって自己位置を算出する具体例を以下に説明する。

ロボット１の外観構成の具体例として、図６に示すモデルを考える。始めに、画像座標系Σ_ＩＭＧからカメラ座標系Σ_ＣＡＭへの座標変換は、上述した（６）式の逆変換として（１１）式により表すことができる。なお、（１１）式における３×３行列Ｑ及び３×１行列（Ｐ_１４，Ｐ_２４，Ｐ_３４）^Ｔは、（１２）式により定義される射影行列Ｐの成分である。

さらに、カメラ座標系Σ_ＣＡＭからロボット座標系Σ_{ＲＯＢＯＴ}への座標変換、及びロボット座標系Σ_{ＲＯＢＯＴ}から世界座標系Σ_{ＷＯＲＬＤ}への座標変換は、それぞれ（１３）式及び（１４）式により表すことができる。なお、αは、ここではカメラのピッチ角を表す。

上述した３つの座標変換式を用いることにより、世界座標系におけるロボット位置（Ｘ_Ｃ，Ｙ_Ｃ）及び姿勢θは、以下の（１５）〜（１７）式によって表される。つまり、予めカメラキャリブレーションを行って射影行列Ｐを決定しておけば、２つのランドマーク３1１及び３1２の世界座標系Σ_{ＷＯＲＬＤ}での位置（Ｘ_１，Ｙ_１，Ｚ_１）及び（Ｘ_２，Ｙ_２，Ｚ_２）と、画像座標系での位置（ｕ_１，ｖ_１）及び（ｕ_２，ｖ_２）とを用いて、世界座標系におけるロボット位置（Ｘ_Ｃ，Ｙ_Ｃ）及び姿勢θを算出することができる。

なお、（１５）〜（１７）式におけるｃ_１、ｃ_２、ｃ_３、ｄ_１、ｄ_２、ｄ_３、及びｓは、以下の（１８）〜（２１）式で表される。

このような単眼によるランドマーク認識は、２枚の画像データを必要としないため、撮影した２枚の画像データの一方が自己位置推定に適さないものであっても再度の撮影を行うことなく自己位置推定を行えるという利点がある。一方、発明の実施の形態１において詳述した複眼によるランドマーク認識は、ステレオ視によりカメラ座標系でのランドマーク位置が算出できるため、射影行列の演算が不要であり、単眼によるランドマーク認識に比べて演算量が少なくて済むという利点がある。このため、複眼によるランドマーク認識と単眼によるランドマーク認識を切り替え可能とすることにより、自己位置推定に要する演算処理の高速化が可能となる。

その他の実施の形態．
発明の実施の形態１にかかるロボット１は、始めに画像データを用いて自己位置を推定することとした。しかしながら、レーザレンジファインダ１０７によって取得したレンジデータと三次元形状データとのマッチングによる自己位置推定を行う推定部をロボット１にさらに備えることとし、始めにレンジデータを用いた自己位置推定を行ってもよい。また、ロボット１の移動履歴を示すオドメトリ情報によって、レンジデータと三次元形状データとのマッチングを行う際の自己位置の初期値を決定しても良い。

また、上述した発明の実施の形態では、画像データを用いた自己位置推定の誤差として再投影誤差を使用し、レンジデータを用いた自己位置推定の誤差として位置合わせ誤差を使用することとし、これらの誤差を含む目的関数を環境情報に応じて決定し、最適化を行う場合を説明した。しかしながら、画像データを用いた自己位置推定及びレンジデータを用いた自己位置推定の誤差には、再投影誤差及び位置合わせ誤差に代えて他のパラメタを用いても構わない。

また、上述した発明の実施の形態では、レーザレンジファインダによってレンジデータを得ることとしたが、他の距離センサによってレンジデータを得てもよい。

さらに、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

本発明にかかるロボットの主要構成を示すブロック図である。 本発明にかかるロボットにおける自己位置推定の手順を示すフローチャートである。 本発明にかかるロボットが行う自己位置推定を説明するための参考図である。 本発明にかかるロボットの主要構成を示すブロック図である。 本発明にかかるロボットにおける画像データを用いた自己位置推定の手順を示すフローチャートである。 単眼によるランドマーク認識を説明するための図である。

符号の説明

１、２ ロボット
１０１ カメラ部
１０２ ランドマーク検出部
１０３ 三次元位置算出部
１０４ 自己位置推定部
１０５ 再投影誤差算出部
１０６ 記憶部
１０７ レーザレンジファインダ
１０８ 位置合わせ誤差算出部
１０９ 最適化部
１１０ 環境情報入力部
１１１ 制御部
２０１ 自己位置推定部
２０２ 統合部

Claims (9)

1. 作業環境内を自律移動する移動装置であって、
   前記作業環境の地図情報を格納した記憶部と、
   前記作業環境内に存在する物体との距離を計測してレンジデータを生成する距離センサと、
   前記作業環境内に存在する物体を撮像した画像データを生成するカメラ部と、
   前記レンジデータを前記地図情報に含まれる形状データと照合することによって、当該移動装置の自己位置を推定する第１の推定手段と、
   前記画像データからランドマークを検出し、検出したランドマークの画像上での位置情報及び前記地図情報に含まれる前記作業空間におけるランドマークの位置情報を用いた演算によって、当該移動装置の自己位置を推定する第２の推定手段と、
   当該移動装置の周囲環境に応じて、前記第１の推定手段及び前記第２の推定手段の信頼度を決定する信頼度決定手段と、
   前記第１の推定手段による推定結果と前記第２の推定手段による推定結果とを、前記信頼度決定手段によって決定された信頼度に応じた重み付けを行って統合することにより、当該移動装置の自己位置を決定する自己位置決定手段と、
   を備える移動装置。
2. 前記自己位置決定手段は、
   前記第１の推定手段による自己位置推定の誤差と前記第２の推定手段による自己位置推定の誤差とを含む目的関数を、前記第１の推定手段及び前記第２の推定手段の信頼度を反映して決定し、
   決定した目的関数を最適化して得られる前記作業空間内での位置を当該移動装置の自己位置とする請求項１に記載の移動装置。
3. 前記第１の推定手段による自己位置推定の誤差は、前記レンジデータと前記形状データとの位置合わせ誤差であり、
   前記第２の推定手段による自己位置推定の誤差は、前記画像データへのランドマークの再投影誤差である請求項２に記載の移動装置。
4. 前記信頼度決定手段は、前記作業環境の明るさに基づいて、前記第１の推定手段及び前記第２の推定手段の信頼度を決定する請求項１又は２に記載の移動装置。
5. 前記カメラ部は、複数の異なる位置から撮像した複数の画像データを生成し、
   前記第２の推定手段は、前記複数の画像データそれぞれに含まれるランドマークの数及び画像上でのランドマーク間距離の少なくとも一方に基づいて、前記複数の画像データから自己位置の推定に用いる画像データを選択する請求項１に記載の移動装置。
6. 前記第２の推定手段は、複数の画像データを用いる場合と、１の画像データを用いる場合とで、異なる演算によって自己位置を算出する請求項５に記載の移動装置。
7. 作業環境の地図情報を格納した記憶部を有し、作業環境内を自律的に移動する移動装置が行う自己位置推定方法であって、
   距離センサによって取得したレンジデータを、前記地図情報に含まれる形状データと照合することによって第１の自己位置推定値を算出し、
   カメラによって撮像した画像データからランドマークを検出し、検出したランドマークの画像上での位置情報及び前記地図情報に含まれるランドマークの前記作業空間における位置情報を用いた演算によって第２の自己位置推定値を算出し、
   前記移動装置の周囲環境に応じて、前記第１の自己位置推定値及び前記第２の自己位置推定値の信頼度を決定し、
   前記第１の自己位置推定値及び前記第２の自己位置推定値を、決定した信頼度に応じた重み付けにより統合した値を自己位置とする自己位置推定方法。
8. 作業環境の地図情報を格納した記憶部を有し、作業環境を自律的に移動する移動装置が行う自己位置推定方法であって、
   距離センサによって前記作業環境内に存在する物体との距離を計測したレンジデータ、及び、カメラによって前記作業環境内に存在する物体を撮像した画像データを入力し、
   i）前記レンジデータを前記地図情報に含まれる形状データと照合することにより推定した自己位置の第１の誤差と、ii）前記画像データから検出したランドマークの画像上での位置情報及び前記地図情報に含まれるランドマークの前記作業空間における位置情報を用いた演算により推定した自己位置の第２の誤差とを含む目的関数を、当該移動装置の周囲環境に応じて決定し、
   決定した目的関数を最適化して得られる前記作業空間内での位置を当該移動装置の自己位置とする自己位置推定方法。
9. 前記第１の誤差は、前記レンジデータと前記地図情報に含まれる形状データとの位置合わせ誤差であり、
   前記第２の誤差は、前記画像データから検出したランドマークの画像上での位置と、前記作業空間におけるランドマークの三次元位置を前記画像データに再投影した位置との再投影誤差である請求項８に記載の自己位置推定方法。