自己位置推定装置、自己位置推定方法およびプログラム
【課題】自己位置を正確に推定することが可能な自己位置推定装置、自己位置推定方法およびプログラムを提供する。
【解決手段】自己位置推定装置は、撮影部、距離算出部、ランドマーク位置取得部、自己位置推定部、移動位置予測部、観測尤度計算部、視野内ランドマーク抽出部、撮影角度変更部、撮影制御部を有している。ランドマーク位置取得部は、各ランドマークの位置を取得する。自己位置推定部は、取得された画像、算出された距離、および上記位置に基づき自己位置を推定する。移動位置予測部は、推定された自己位置に基づき予測位置を算出する。撮影角度変更部は、視野内ランドマーク抽出部が抽出した視野内ランドマークについて観測尤度計算部が計算した観測尤度と、視野内ランドマークの観測角度とに応じて撮影角度を変更する。撮影制御部は、変更後の撮影角度で撮影した画像に基づき自己位置を更新する。
【解決手段】自己位置推定装置は、撮影部、距離算出部、ランドマーク位置取得部、自己位置推定部、移動位置予測部、観測尤度計算部、視野内ランドマーク抽出部、撮影角度変更部、撮影制御部を有している。ランドマーク位置取得部は、各ランドマークの位置を取得する。自己位置推定部は、取得された画像、算出された距離、および上記位置に基づき自己位置を推定する。移動位置予測部は、推定された自己位置に基づき予測位置を算出する。撮影角度変更部は、視野内ランドマーク抽出部が抽出した視野内ランドマークについて観測尤度計算部が計算した観測尤度と、視野内ランドマークの観測角度とに応じて撮影角度を変更する。撮影制御部は、変更後の撮影角度で撮影した画像に基づき自己位置を更新する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自己位置推定装置、自己位置推定方法およびプログラムに関する。
【背景技術】
【０００２】
移動ロボットの自律移動を制御する方式として、ランドマーク（目印）となる局所的な画像を用いる手法が従来から用いられている。例えば、予め走行経路上で撮影した画像を記憶し、撮影した画像の方向が一定の方向になるようにロボットの指令値を制御して走行を制御する方式が知られている。この方式では、レーザ等の他のセンサデータを用いた軌道制御を備え、画像による制御と適宜切り替えるようにする場合がある。
【０００３】
ランドマークの位置を記したランドマーク地図を利用して自己位置を推定する方法も知られている。ランドマーク地図の生成には、Ｓｃａｌｅ Ｉｎｖａｒiａｎｔ Ｆｅａｔｕｒｅ Ｔｒａｎｓｆｏｒｍ（ＳＩＦＴ）特徴量を用いたビジョンベースＳiｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌiｚａｔiｏｎ Ａｎｄ Ｍａｐ Ｂｕiｌｄiｎｇ（ＳＬＡＭＢ）アルゴリズムに基づく例がある。このとき、ロボットは、生成したランドマーク地図を参照するとともに、撮影した画像においてランドマークを抽出して、自己位置を推定することができる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１０−１４０２４６号公報
【非特許文献】
【０００５】
【非特許文献１】陳彬、中尾学、深貝卓也、沢崎直之 著「ビジュアルランドマークとレイアウト地図を用いた移動地図を用いた移動知能用ナビゲーションシステムの開発」
【非特許文献２】S. Se, D. Lowe, and J. Little.著“Local and global localization for mobile robots using visual landmarks.” In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 414-420, Maui, Hawaii, October 2001.
【発明の概要】
【発明が解決しようとする課題】
【０００６】
ところで、上記のようにランドマークを用いた自己位置推定においては、複数のランドマークが観測可能でも、取得した画像において互いに近傍に観測される場合には、自己位置推定の精度が低下してしまう。
【０００７】
また、例えば、移動経路上に障害物がある場合には、障害物を回避するために、ロボットが撮影した画像の方向が一定の方向になる経路とは、異なる経路を移動する必要がある。このようにロボットが経路を外れた場合には、既に撮影された画像とは異なる方向にカメラが向き、そのランドマークが見える角度のずれ量が大きくなる場合、或いはランドマークとのパターンマッチングの一致度が大きく低下する場合がある。さらには、ランドマークを検出できない場合が発生する。
【０００８】
このようなランドマークが互いに近傍に観測される場合、ランドマークが見える角度のずれ量が大きい場合や、検出できない場合では、走行部のエンコーダを元にしたオドメトリ情報のみで自己位置を推定することになる。このように、ランドマークが見えない期間が長くなるとオドメトリの誤差が積算されて自己位置推定精度が悪くなり、自己位置を見失ってしてしまう可能性がある。
【０００９】
上記課題に鑑み、ランドマークを見失うことを防止することにより、自己の位置を正確に推定することが可能な自己位置推定装置、自己位置推定方式およびプログラム提供する。
【課題を解決するための手段】
【００１０】
ひとつの態様である自己位置推定装置は、撮影部、距離算出部、ランドマーク位置取得部、自己位置推定部、移動位置予測部、観測尤度計算部、視野内ランドマーク抽出部、撮影角度変更部、撮影制御部を有することを特徴としている。撮影部は、少なくとも一つのランドマークの画像を含む画像を撮影する。距離算出部は、自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出する。ランドマーク位置取得部は、少なくとも一つのランドマークの位置を取得する。自己位置推定部は、前記画像、前記距離、および前記少なくとも一つのランドマークの位置に基づき前記自己位置推定装置の自己位置を推定する。移動位置予測部は、前記自己位置推定部により推定された自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出する。観測尤度計算部は、前記予測位置における前記少なくとも一つのランドマークが観測される観測尤度を算出する。視野内ランドマーク抽出部は、前記予測位置において前記画像が撮影される際の撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出する。撮影角度変更部は、抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更する。撮影制御部は、変更後の前記撮影角度で前記画像が撮影されるように撮影を制御する。前記自己位置推定部は、変更後の前記撮影角度で撮影された画像とランドマーク位置取得部により取得される前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新する。
【００１１】
別の態様である自己位置推定方法においては、少なくとも一つのランドマークの画像を含む画像を取得し、前記自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出し、前記少なくとも一つのランドマークの位置を取得する。前記取得された画像、前記算出された距離、および前記記憶された前記少なくとも一つのランドマークの位置に基づき自己位置を推定し、前記推定された自己位置に基づき移動時の将来の予測位置を算出する。前記予測位置において前記少なくとも一つのランドマークが観測される確率である観測尤度を算出する。前記予測位置において画像が撮影される撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出する。また、抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更し、変更後の前記撮影角度で前記画像が撮影されるように撮影を制御する。さらに、変更後の前記撮影角度で撮影された画像と取得された前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新することを特徴としている。
【００１２】
なお、上述した本発明に係る方法をコンピュータに行わせるためのプログラムであっても、このプログラムを当該コンピュータによって実行させることにより、上述した本発明に係る方法と同様の作用・効果を奏するので、前述した課題が解決される。
【発明の効果】
【００１３】
上述した態様の自己位置推定装置、自己位置推定方法およびプログラムによれば、自己位置を正確に推定することが可能になる。
【図面の簡単な説明】
【００１４】
【図１】第１の実施の形態によるロボットの構成を示すブロック図である。
【図２】第１の実施の形態によるロボットの機能を示すブロック図である。
【図３】第１の実施の形態によるランドマーク地図生成の動作を示すフローチャートである。
【図４】第１の実施の形態によるランドマーク地図の一例を示す図である。
【図５】第１の実施の形態による観測方向ベクトルと観測距離の一例を示す図である。
【図６】第１の実施の形態によるランドマークの位置と推定される自己位置の関係を示す図であり、（ａ）はランドマーク１個の場合、（ｂ）は、ランドマーク２個が近接して観測された場合、（ｃ）は、ランドマーク２個が所定距離離れて観測された場合、（ｄ）は、ランドマーク３個が近接して観測された場合である。
【図７】第１の実施の形態によるロボットによる自己位置推定処理を示すフローチャートである。
【図８】第１の実施の形態によるパンチルト角度リストの一例を示す図である。
【図９】第１の実施の形態によるリストΩの一例を示す図である。
【図１０】第１の実施の形態によるロボットおよびランドマークの位置関係の一例を示す図である。
【図１１】第１の実施の形態による評価値の算出例を示す図である。
【図１２】第２の実施の形態によるロボットの機能を示すブロック図である。
【図１３】第２の実施の形態による自己位置推定処理を示すフローチャートである。
【図１４】標準的なコンピュータの構成を示す図である。
【発明を実施するための形態】
【００１５】
（第１の実施の形態）
以下、図面を参照しながら第１の実施の形態によるロボット１について説明する。図１は、第１の実施の形態によるロボット１の構成を示すブロック図、図２は、第１の実施の形態によるロボット１の機能を示すブロック図である。
【００１６】
図１に示すように、ロボット１は、ステレオカメラ５、パンチルトモータ７、パンチルト制御部９、車輪制御部１１、車輪モータ１３、記憶部１５を有し、演算処理装置３により制御されている。
【００１７】
図２に示すように、ステレオカメラ５は、互いに平行な方向に撮影方向を設定され、所定間隔を隔てて配置された２つのカメラ５Ａ、５Ｂを有している。所定間隔は、２つのカメラ５Ａ、５Ｂで撮影した画像により、撮影対象までの距離が所定の精度で算出できる間隔であることが好ましい。カメラ５Ａ、５Ｂは、少なくとも静止画像を撮影し、電気的信号に変換して出力する撮影装置である。なお、カメラ５Ａ、５Ｂにより撮影された２つの画像をまとめてステレオ画像ともいう。
【００１８】
パンチルトモータ７は、ステレオカメラ５の撮影方向を変化させるための駆動部である。パンチルト制御部９は、演算処理装置３による指示によりパンチルトモータ７の動作を制御する制御部である。車輪モータ１３は、図２に示す車輪１９を回転させてロボット１を移動させるための駆動部である。車輪制御部１１は、演算処理装置３による指示により車輪モータ１３を制御する制御部である。
【００１９】
記憶部１５は、例えばＲｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ（ＲＯＭ）や、Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ（ＲＡＭ）、またはハードディスク装置等の記憶装置である。記憶部１５は、ロボット１の動作を制御するプログラムを予め記憶したり、プログラムを実行する際に必要に応じて作業領域として使用したりするための記憶装置である。また、記憶部１５は、図２に示すランドマーク地図５５を格納している。演算処理装置３が、記憶部１５のプログラムを読み込み実行することにより、ロボット１の各構成要素の動作制御が可能になる。
【００２０】
図２に示すように、ロボット１は、車輪１９が備えられた本体１７を有している。本体１７は、直方体形状であり、車輪１９は、本体１７の下部に備えられている。本体１７は、車輪１９が車輪制御部１１および車輪モータ１３を介して制御されることにより移動可能に構成されている。
【００２１】
本体１７の上面１７Ａには、ステレオカメラ５が載置されている。ステレオカメラ５は、パンチルトモータ７、パンチルト制御部９により制御されることにより撮影方向が可変に構成されている。なお、本実施の形態においては、ロボット１の上下方向を図２に示すｚ軸に平行な方向とするものとし、水平方向は、図２に示すｙ軸に平行な方向、前後方向は、ｘ軸に平行な方向とする。また、第１の実施の形態においては、パンチルトモータ７は、ステレオカメラ５をｚ軸に垂直な面内、すなわち水平方向に回転させるものとする。
【００２２】
ロボット１の演算処理装置３は、ランドマーク抽出部３１、地図生成部３３、自己位置推定部３５、移動位置予測部３７、移動経路計画部３９、方向距離計算部４１、観測尤度計算部４３、視野内ランドマーク抽出部、観測評価部４７、カメラ制御部４９の機能を有する。
【００２３】
ランドマーク抽出部３１は、ステレオカメラ５のカメラ５Ａ、５Ｂが撮影した２つの画像から、目印となる局所的な画像領域であるランドマークを抽出する。また、ランドマーク抽出部３１は、ステレオカメラ５のカメラ５Ａ、５Ｂが撮影した２つの画像に基づいて抽出したランドマークまでの距離を算出することにより、３次元位置を特定する。
【００２４】
地図生成部３３は、抽出したランドマークと特定した３次元位置とを関連付けた情報の集合であるランドマーク地図を生成し、ランドマーク地図５５に記憶させる。このとき、地図生成部３３は、ランドマーク地図５５に記憶させたランドマークの３次元位置を特定したときに用いた画像を撮影した際の、ロボット１の位置および向き（以下、ロボット１の位置姿勢という）、ステレオカメラ５の角度を記臆させておくことが好ましい。また、地図生成部３３は、各ランドマークを撮影可能なロボット１の位置に対応した位置姿勢を、各ランドマークに関連する代表位置として例えばランドマーク地図５５に記憶させておく。代表位置は、例えば、各ランドマークを撮影したときのロボット１の位置の平均とすることが好ましい。
【００２５】
自己位置推定部３５は、ランドマーク抽出部３１が抽出したランドマークの３次元位置と、ランドマーク地図５５を参照することにより、ロボット１の自己位置を推定する。移動位置予測部３７は、自己位置推定部３５で推定された自己位置と、移動経路計画部３９で計画される経路により、ロボット１の所定時間後の移動予測位置を算出する。移動経路計画部３９は、ロボット１を移動させる経路を計画する。
【００２６】
方向距離計算部４１は、各ランドマークについて、ランドマーク地図５５に記憶されている情報を参照し、各ランドマークから対応するそれぞれの代表位置への方向ベクトルを算出する。また、方向距離計算部４１は、移動位置予測部３７が予測した、ロボット１の将来の予測位置において観測可能な視野内の各ランドマークから、対応する予測位置への方向ベクトルを算出する。さらに方向距離計算部４１は、算出した２つの方向ベクトルに基づき、２つの方向ベクトルの角度差と距離差とを算出する。なお、角度差と距離差の詳細については後述する。
【００２７】
観測尤度計算部４３は、方向距離計算部４１が算出した角度差および距離差に基づき、各ランドマークに関する観測尤度を算出する。視野内ランドマーク抽出部４５は、ステレオカメラ５の角度をパンチルト制御部９により変化させたときに撮影される画像内に含まれると判断される視野内ランドマークを、ステレオカメラ５のそれぞれの角度に応じて抽出する。また、このときのステレオカメラ５から見たランドマークＬＭの観測角度を算出する。なお、観測尤度および観測角度の詳細については後述する。
【００２８】
観測評価部４７は、移動位置予測部３７が予測した予測位置においてステレオカメラ５の角度を変化させた際の各視野内ランドマークについて、観測尤度および観測角度に基づく評価値を算出する。観測評価部４７は、算出した評価値に基づき、予測位置における最適なステレオカメラ５の角度を判別する。なお、評価値の詳細については後述する。カメラ制御部４９は、観測評価部４７で算出された最適な角度にステレオカメラ５を制御するように、パンチルト制御部９に制御のための信号を出力する。
【００２９】
以下、図３から図１３を参照しながら、第１の実施の形態によるロボット１の動作について説明する。第１の実施の形態によるロボット１の自己位置推定においては、まず、ランドマーク地図５５を作成する。このようなロボット１では、ステレオカメラ５は、移動の際には姿勢をロボット１を正面方向に固定しているのが一般的である。よって、本実施の形態においても、自律移動を行なう前に、演算処理装置３は、パンチルト制御部９によりステレオカメラ５の姿勢をロボット１の正面方向に設定する。そしてその状態で、演算処理装置３は、車輪制御部１１により車輪モータ１３を介して車輪１９を制御して、実運用での自律移動で使用する経路に沿って予めロボット１を動かし、ランドマークの３次元位置を記したランドマーク地図５５を生成する。
【００３０】
図３は、ランドマーク地図生成の動作を示すフローチャートである。ランドマーク地図５５の作成時には、演算処理装置３は、車輪制御部１１を介してロボット１を、使用する経路のスタート地点に配置し、時刻ｊ＝０とする（Ｓ１０１）。ここで、演算処理装置３は、ステレオカメラ５にステレオ画像を撮影させ、ランドマーク抽出部３１は、撮影された画像においてランドマークを検出する（Ｓ１０２）。ランドマークの検出は、例えばパターマッチング法や特徴量抽出法により行なうことができる。
【００３１】
演算処理装置３は、地図生成部３３によりランドマークを検出したときのロボット１の位置姿勢と、ランドマークの位置とを同時に推定し、ランドマーク地図５５を更新する（Ｓ１０３）。
【００３２】
ここで、時刻ｊにおけるロボット１の位置姿勢は、位置姿勢（ｘｒ（ｊ）、ｙｒ（ｊ）、θｒ（ｊ））と表す。ここで、座標ｘｒ（ｊ）、ｙｒ（ｊ）は、時刻ｊにおけるロボット１の、図２に示したｘｙｚ座標系におけるｘｙ平面上の座標（ｘｒ（ｊ）、ｙｒ（ｊ））であり、角度θｒ（ｊ）は、ロボット１の正面方向がｘ軸となす角度である。
【００３３】
図２のｘｙｚ座標系は、ロボットが移動する空間において定める。例えば、ｚ軸を鉛直方向とし、ｘ、ｙ軸を、ｚ軸に垂直な平面内で互いに直交する軸として定める。このとき原点（０、０、０）は、例えば、ランドマーク地図５５生成時のロボット１のスタート地点とすることができる。なお、撮影した画像を用いてロボット１の位置姿勢とランドマークの位置とを同時に推定する方法は、例えば非特許文献１の方法など、公知の方法を用いることができる。
【００３４】
また、ランドマークの３次元位置は、例えば識別情報ＩＤ＝i（i＝１〜Ｉ：Ｉはランドマークの数）のランドマークをランドマークＬＭiとして、３次元位置（ｘｌ（ｉ）、ｙｌ（ｉ）、ｚｌ（ｉ））と表す。ここで、座標ｘｌ（ｉ）、ｙｌ（ｉ）、ｚｌ（ｉ）は、図２のｘｙｚ座標系における座標である。
【００３５】
図４は、ランドマーク地図５５の一例を示す図である。図４に示すように、例１２０においては、例えば、識別情報ＩＤ＝１では、３次元位置（ｘｌ（１）、ｙｌ（１）、ｚｌ（１））＝（４．５、１．２、１．０）である。また、識別情報ＩＤ＝２に対しては、３次元位置（ｘｌ（２）、ｙｌ（２）、ｚｌ（２））＝（５．０、−０．４、１．０）である。以下同様である。
【００３６】
図３に戻って、演算処理装置３の自己位置推定部３５は、時刻ｊにおける推定したロボット１の位置姿勢（ｘｒ（ｊ）、ｙｒ（ｊ）、θｒ（ｊ））と、検出したランドマークＬＭｉの識別情報ＩＤのリストを、互いに関連付けて例えばランドマーク地図５５に保存する（Ｓ１０４）。演算処理装置３は、ｊ＝ｊ＋１とし（Ｓ１０５）、経路の終点であるゴールへ付いたか否か判別する（Ｓ１０６）。ゴールへつかないと判別された場合には（Ｓ１０６：ＮＯ）、Ｓ１０２の処理に戻る。ゴールへついたと判別された場合には（Ｓ１０６：ＹＥＳ）、演算処理装置３は処理をＳ１０７に進める。
【００３７】
Ｓ１０７では、演算処理装置３の地図生成部３３は、まず、ランドマークＬＭ（ランドマークＬＭｉの総称）の識別情報ＩＤ＝１とする。Ｓ１０８の処理では、地図生成部３３は、識別情報ＩＤ＝i（i＝１〜Ｉ）のランドマークＬＭiを観測した全ての時刻ｊにおけるロボット１の位置姿勢（ｘｒ（ｊ）、ｙｒ（ｊ）、θｒ（ｊ））の平均を求める。求めた平均を、ランドマークＬＭiを観測した際の、ロボット１の代表位置姿勢（ｘｒ（ｉ）、ｙｒ（ｉ）、θｒ（ｉ））として決定する（Ｓ１０８）。さらに、地図生成部３３は、識別情報ＩＤ＝iの地図生成時における、ランドマークＬＭiから、対応する代表位置への観測方向ベクトル（ｄｘ（ｉ）、ｄｙ（ｉ）、ｄｚ（ｉ））と観測距離ｄ（ｉ）とを計算する（Ｓ１０９）。なお、代表位置とは、代表位置位置姿勢（ｘｒｊ、ｙｒｊ、θｒｊ）におけるロボット１の位置（ｘｒｊ、ｙｒｊ）である。
【００３８】
図５は、観測方向ベクトル（ｄｘ（ｉ）、ｄｙ（ｉ）、ｄｚ（ｉ））と、観測距離ｄ（ｉ）の一例を示す図である。図５に示す例では、ランドマークＬＭiを観測した場合のロボット１の代表位置が、代表位置姿勢（ｘｒ（ｉ）、ｙｒ（ｉ）、θｒ（ｉ））である。このとき、観測方向ベクトル（ｄｘ（ｉ）、ｄｙ（ｉ）、ｄｚ（ｉ））は、図５に示すベクトル６２であり、ベクトル６２の長さが、観測距離ｄ（ｉ）となる。
【００３９】
演算処理装置３は、i＝i＋１とし（Ｓ１１０）、i＞Ｉであるか否か、すなわち全てのランドマークＬＭiについてＳ１０８、１０９の処理が終了したか否か判別する（Ｓ１１１）。i≦Ｉである場合には（Ｓ１１１：ＮＯ）、演算処理装置３は、Ｓ１０８に戻って処理を繰り返し、i＞Ｉの場合には（Ｓ１１１：ＹＥＳ）、ランドマーク地図生成処理を終了する。
【００４０】
以上の処理により、ランドマーク地図５５として、それぞれのランドマークＬＭの３次元位置（ｘｌ（ｉ）、ｙｌ（ｉ）、ｚｌ（ｉ））が、ランドマークＬＭの識別情報ＩＤと関連付けて記憶される。また、ランドマーク地図５５には、各ランドマークＬＭに対応するロボット１の代表位置における代表位置姿勢（ｘｒ（ｉ）、ｙｒ（ｉ）、θｒ（ｉ））が、識別情報ＩＤと関連付けて記録されることが好ましい。また、ランドマーク地図５５には、各ランドマークＬＭiに対応する観測方向ベクトル（ｄｘ（ｉ）、ｄｙ（ｉ）、ｄｚ（ｉ））、観測距離ｄ（ｉ）を記録することが好ましい。
【００４１】
次に、図６から図９を参照しながら、ロボット１による自己位置推定の処理について説明する。自律走行による自己位置推定は、ランドマークＬＭの検出数が多く、さらにいろいろ方向にあるランドマークＬＭを検出できた方が推定精度は高くなる。例えば、図６を参照しながら、以下の３つのケースの場合を考える。図６は、ランドマークの位置と推定される自己位置の関係を示す図である。ここで、（ａ）はランドマーク１個の場合、（ｂ）は、ランドマーク２個が近接して観測された場合、（ｃ）は、ランドマーク２個が所定距離離れて観測された場合、（ｄ）は、ランドマーク３個が近接して観測された場合である。
【００４２】
第１の場合として、図６（ａ）に示すように、ランドマーク６１を１個、距離ｄ、誤差±ｅで観測した場合には、ロボットの位置はランドマーク６１を中心とした半径ｄ−ｅと半径ｄ＋ｅの円の間の円環内６３にあると位置を推定され、推定結果の広がりが大きいことになる。
【００４３】
第２の場合として、図６（ｂ）に示すように、近接したランドマーク６５、６７の２個をそれぞれ距離ｄ、誤差±ｅで観測した場合には、ロボットの位置はそれぞれのランドマーク６５、６７を中心とした円環同士が重なった部分６９として推定される。しかし、円環同士の重なりが大きいため、推定結果の広がりが大きい。
【００４４】
第３の場合として、図６（ｃ）に示すように、離れた位置にあるランドマーク７１、７３の２個をそれぞれ距離ｄ、誤差±ｅで観測した場合には、ロボットの位置はそれぞれのランドマークを中心とした円環同士が重なった部分７４として推定される。この場合には、円環同士の重なりが小さいため、推定結果の広がりが小さい。
【００４５】
第４の場合として、図６（ｄ）に示すように、３個のランドマーク７５、７７、７９をそれぞれ距離ｄ、誤差±ｅで観測した場合でも、互いに近接して観測されると、円環同士の重なった部分８１が大きく、推定結果の広がりが大きい。
【００４６】
このように自己位置推定は、ランドマークの検出数が多く、さらにいろいろな方向にあるランドマークを検出できた方が推定精度は高くなる。そこで、自律移動のロバスト性を高めるためには、ランドマークが見えない期間が長くなって位置をロストすることがないように、ランドマークが多く、多方向から観測できるように注視方向を決定して、自己位置を大きく見失うことがない状態を持続させるようにする。
【００４７】
図７は、ロボット１による自己位置推定処理を示すフローチャートである。ここでは、ロボット１は、予め定められた経路に従い、自律走行を行うものとする。図７に示すように、まず、演算処理装置３の自己位置推定部３５は、撮影された画像とランドマーク地図５５とを参照してロボット１の自己位置を推定する（Ｓ１５０）。このとき、ステレオカメラ５の撮影方向は、初期状態においてはロボット１の正面方向に設定することが好ましい。このとき、ランドマーク地図５５を参照しながら行なうＳ１５１における自己位置の推定は、例えば非特許文献２における方法等、公知の方法を用いて行う。
【００４８】
演算処理装置３は、自己位置推定部３５により推定された自己位置によりゴールに到着したか否かを判別し（Ｓ１５１）、ゴールに到着した場合には（Ｓ１５１：ＹＥＳ）、処理を終了する。ゴールに到着していない場合には（Ｓ１５１：ＮＯ）、演算処理装置３は処理をＳ１５２に進める。
【００４９】
移動経路計画部３９は、自己位置推定部３５が推定した自己位置に基づき、移動経路を計画する。移動位置予測部３７は、自己位置推定部３５が推定した自己位置と、移動経路計画部３９で計画された移動経路とから、一定時間後（例えば１００ミリ秒後）のロボット１の予測位置を算出する（Ｓ１５２）。
【００５０】
方向距離計算部４１は、ランドマークＬＭの識別情報ＩＤ＝１とする（Ｓ１５３）。方向距離計算部４１は、Ｓ１５２で計算された予測位置へのランドマークＬＭｉからの観測方向ベクトル（ｄｘａ（ｉ）、ｄｙａ（ｉ）、ｄｚａ（ｉ））を算出する。方向距離計算部４１は、ランドマークＬＭｉから代表位置への観測方向ベクトル（ｄｘ（ｉ）、ｄｙ（ｉ）、ｄｚ（ｉ））と、予測位置への観測方向ベクトル（ｄｘａ（ｉ）、ｄｙａ（ｉ）、ｄｚａ（ｉ））とから、角度差φ（ｉ）を計算する（Ｓ１５４）。角度差φ（ｉ）は、以下の式で算出する。
φ（ｉ）＝ａｒｃｃｏｓ（ｄｘ（ｉ）・ｄｘａ（ｉ）＋ｄｙ（ｉ）・ｄｙａ（ｉ）＋ｄｚ（ｉ）・ｄｚａ（ｉ））
・・・式２０１
また、方向距離計算部４１は、ランドマークＬＭｉの代表位置（ｘｒ（ｉ）、ｙｒ（ｉ））からの観測距離ｄ（ｉ）と予測位置からの観測距離ｄａ（ｉ）とから距離差δ（ｉ）を計算する（Ｓ１５５）。
【００５１】
観測尤度計算部４３は、Ｓ１５４、Ｓ１５５で算出された角度差φ（ｉ）と距離差δ（ｉ）とから、観測尤度ρ（ｉ）を決定する。観測尤度ρ（ｉ）は、例えば以下の式２０２のようにφ（ｉ）とδ（ｉ）とが小さいほど尤度が大きくなる観測モデルを用いる。以下の式で算出する。ここで、σ１、σ２は係数である。
【００５２】
【数１】
【００５３】
観測尤度計算部４３は、ｉ＝ｉ＋１とし（Ｓ１５７）、ｉ＞Ｉとなるまで、Ｓ１５４からＳ１５７の処理を繰り返す（Ｓ１５８：ＮＯ）。ｉ＞Ｉとなると（Ｓ１５８）、演算処理装置３は、処理をＳ１５９に進める。Ｓ１５９では、視野内ランドマーク抽出部４５は、ｍ＝１とし、記憶部１５においてパンチルト角度リストを参照する（Ｓ１５９）。
【００５４】
図８は、パンチルト角度リストの一例を示す図である。上述したように、第１の実施の形態によるロボット１においては、ステレオカメラ５は、ｚ軸に垂直な平面の方向にのみ角度を変化させており（パン角度）、ｙ軸に垂直な平面の方向のチルト角度はゼロに設定している。図８の例では、ｍ＝１〜５に対応してパン角度＝−０．５２〜０．５２（ｒａｄ）の５段階の角度が設定されている。
【００５５】
視野内ランドマーク抽出部４５は、各ｍに対応してパン角度を変化させ、それぞれのパン角度で視野内に入るランドマークＬＭ（以下、視野内ランドマークという）を抽出し、ランドマークＩＤリストΩを生成する（Ｓ１６０）。また、視野内ランドマークが観察される観測角度を算出する。観測角度とは、予測位置へのランドマークＬＭからの観測方向ベクトルのｘｙ平面への投影成分が、ロボット１の正面方向となす鋭角である。
【００５６】
図９は、リストΩの一例を示す図である。図９に示すように、リストΩの一例１８４は、例えばｍ＝１のときには、ランドマークＬＭ２、ランドマークＬＭ３が観測されていることを示している。ｍ＝２のときには、ランドマークＬＭ１、ランドマークＬＭ２が観測され、ｍ＝３、４のときには、ランドマークＬＭ１が観測されている。ｍ＝５のときには、ランドマークＬＭ４、ランドマークＬＭ５が観測されている。
【００５７】
観測評価部４７は、それぞれのパン角度ｐにおける視野内ランドマークについて、それぞれ上述の観測尤度ρ（ｉ）の総和と、観測角度の標準偏差を算出する。観測評価部４７は、算出された観測尤度ρ（ｉ）の総和と、観測角度の標準偏差から、評価値Ｐ（ｐ、ｔ）を算出する。
【００５８】
例えば評価値としては、α、βを係数として以下の式が適用可能である。
評価値Ｐ（ｐ、ｔ）
＝α（係数）×（抽出されたランドマークＬＭの観測尤度ρ（i）の総和）
＋β（係数）×（抽出されたランドマークＬＭの観測角度の標準偏差）
・・・式２０３
【００５９】
続いて観測評価部４７は、ｍ＝ｍ＋１と置き換え（Ｓ１６２）、ｍ＞Ｍ（Ｍは、パンチルト角度を変化させる種類の数）となるまで（Ｓ１６３：ＮＯ）、Ｓ１６０からＳ１６２の処理を繰り返す。ｍ＞Ｍとなると（Ｓ１６３：ＹＥＳ）、観測評価部４７は、評価値Ｐ（ｐ、ｔ）が最大となるパンチルト角度ｐ、ｔ（パン角度ｐおよびチルト角度ｔの総称であり、本実施の形態においては、ｔ＝０）を求める。
【００６０】
なお、抽出されたランドマークＬＭの観測尤度ρ（ｉ）の総和が高いほど評価値Ｐ（ｐ、ｔ）が高く、抽出されたランドマークＬＭの観測角度の標準偏差が大きいほど評価値Ｐ（ｐ、ｔ）が高い。
【００６１】
このように、評価値Ｐ（ｐ、ｔ）が最大となるパン角度ｐとチルト角度ｔの組み合わせを求めることでステレオカメラ５のパン角度ｐとチルト角度ｔを決定する。カメラ制御部４９は、求めた角度にパンチルト制御部９がステレオカメラ５の角度を制御するように信号を出力する。
【００６２】
以下、図１０、１１を参照しながら、具体的な評価値Ｐ（ｐ、ｔ）の算出例について説明する。図１０は、ロボット１およびランドマークＬＭの位置関係の一例を示す図であり、図１１は、評価値Ｐ（ｐ、ｔ）の算出例を示す図である。
【００６３】
図１０に示すように、ロボット１の現在位置は、例えばｘｙｚ座標系における位置（０．０、０．０、１．０）であるとする。このとき、ランドマークＬＭ１（４．５、１．２、１．０）、ランドマークＬＭ２（５．０、−０．６）、ランドマークＬＭ３（３．５、−１．０、１．０）、ランドマークＬＭ４（４．０、３．９、１．０）、ランドマークＬＭ５（３．５、３．６、１．０）が存在しているとする。
【００６４】
図１１に示すように、移動位置予測部３７は、図１０の状態から所定時間後のロボット１の予測位置を位置（１．０、１．０、１．０）であると算出したとする。この位置で、方向距離計算部４１は、それぞれのランドマークＬＭｉ（ｉ＝１〜５）について、上述のように角度差φ（ｉ）および距離差δ（ｉ）を算出する。また、方向距離計算部４１は、それぞれのランドマークの観測角度を算出する。
【００６５】
観測尤度計算部４３は、算出した角度差φ（ｉ）および距離差δ（ｉ）に基づき、それぞれのランドマークＬＭｉ（ｉ＝１〜５）について、上述のように観測尤度ρ（ｉ）を算出する。
【００６６】
一例として、式２０２のパラメータを、σ１＝１．０、σ２＝０．２、式２０４のパラメータを、α＝β＝０．５とする。このとき、式２０１および式２０２から、ランドマークＬＭ１〜ランドマークＬＭ５の観測尤度および観測角度は以下のように算出される。
ランドマークＬＭ１：観測尤度 ０．２４４、観測角度＝０．０５７（ｒａｄ）
ランドマークＬＭ２：観測尤度 ０．２６０、観測角度=−０．３８１（ｒａｄ）
ランドマークＬＭ３：観測尤度 ０．１０１、観測角度=−０．６７５（ｒａｄ）
ランドマークＬＭ４：観測尤度 ０．２９３、観測角度=０．７６８（ｒａｄ）
ランドマークＬＭ５：観測尤度 ０．２９３、観測角度=０．８０５（ｒａｄ）
このとき、上記各ランドマークの観測尤度、観測角度に基づき式２０３を用いて、評価値Ｐ（ｐ、ｔ）を算出する。
【００６７】
視野内ランドマーク抽出部４５は、図８に示すｍ＝１〜５に対応するパンチルト角度にステレオカメラ５を設定した場合について、視野内ランドマークを検出する。例えば図１１は、ｍ＝１（ｐ＝０．５２（ｒａｄ）、ｔ＝０．００（ｒａｄ））とした場合を示している。このとき、視野内ランドマークは、ランドマークＬＭ２、ランドマークＬＭ３である。以下、視野内ランドマーク抽出部４５は、ｍ＝２〜５について、同様の処理を繰り返す。観測評価部４７は、図８に示すｍ＝１〜５に対応する評価値Ｐ（ｐ、ｔ）を算出する。
【００６８】
１）ｍ＝１のとき（ｐ＝０．５２（ｒａｄ）、ｔ＝０．００（ｒａｄ））
観測されるランドマーク：ランドマークＬＭ２、ＬＭ３
このとき、評価値Ｐ（０．５２、０）
＝α×（ρ（２）＋ρ（３））+β×標準偏差（−０．３８１、−０．６７５））
=０．５×０．３６１＋０．５×０．１４７
＝０．２５４
【００６９】
２）ｍ＝２のとき（ｐ＝０．２６（ｒａｄ）、ｔ＝０．００（ｒａｄ））
観測されるランドマーク：ランドマークＬＭ１、ＬＭ２
評価値Ｐ（０．２６、０）＝０．３６２
【００７０】
３）ｍ＝３のとき（ｐ＝０．０（ｒａｄ）、ｔ＝０．００（ｒａｄ））
観測されるランドマーク：ランドマークＬＭ１
評価値Ｐ（０．０、０．０）＝０．１２２
【００７１】
４）ｍ＝４のとき（ｐ＝−０．２６（ｒａｄ）、ｔ＝０．００（ｒａｄ））
観測されるランドマーク：ランドマークＬＭ１
評価値Ｐ（−０．２６、０）＝０．１２２
【００７２】
５）ｍ＝５のとき（ｐ＝−０．５２（ｒａｄ）、ｔ＝０．００（ｒａｄ））
観測されるランドマーク：ランドマークＬＭ４、ＬＭ５
評価値Ｐ（−０．５２、０）＝０．３０２
【００７３】
以上のように、観測評価部４７は、ｍ＝１〜５についてそれぞれ算出した評価値Ｐ（ｐ、ｔ）が最大となるパンチルト角度を判別する。上記の例では、ｍ＝２のときが最大である。よって求めるパンチルト角は、ｐ＝０.２６（ｒａｄ）、ｔ＝０．００（ｒａｄ）となる。カメラ制御部４９は、ステレオカメラ５を、求めたパンチルト角になるように制御するための信号をパンチルト制御部９に出力する。パンチルト制御部９は、求めたパンチルト角になるようにステレオカメラ５を設定して自律移動を継続する。
【００７４】
以上詳細に説明したように、第１の実施の形態によるロボット１の地図生成部３３は、利用する経路のランドマーク地図５５を生成する。このとき、ランドマークＬＭの識別情報ＩＤと、その３次元位置（ｘｌ（i）、ｙｌ（i）、ｚｌ（i））、および、ランドマークＬＭが観測された際のロボット１の代表位置姿勢（ｘｒ（ｊ）、ｙｒ（ｊ）、θｒ（ｊ））が算出される。
【００７５】
ロボット１の自律移動時には、移動位置予測部３７は所定時間後の自己位置を予測して予測位置を算出する。方向距離計算部４１は、各ランドマークＬＭｉから予測位置への方向ベクトルと、ランドマークＬＭｉから代表位置への方向ベクトルとの角度差φ（ｉ）、距離差δ（ｉ）を算出する。観測尤度計算部４３は、各ランドマークＬＭの観測尤度ρ（ｉ）を、角度差φ（ｉ）、距離差δ（ｉ）に基づいて算出する。視野内ランドマーク抽出部４５は、視野内のランドマークＬＭを抽出し、それぞれのランドマークＬＭの観測角度を抽出する。観測評価部４７は、視野内のランドマークＬＭの観測尤度ρ（ｉ）および観測角度の分散に基づき評価値Ｐ（ｐ、ｔ）を算出する。算出した評価値Ｐ（ｐ、ｔ）が最も大きいパン角度を、ステレオカメラ５の設定角度として設定し、その状態で撮影されるステレオ画像およびランドマーク地図５５に基づき自己位置を推定する。
【００７６】
以上のように、第１の実施の形態によるロボット１によれば、自律移動の際には、生成されたランドマーク地図５５と自律移動時に検出したランドマークＬＭとを照合して自己位置の推定を行うことができる。また、ランドマーク地図５５に登録した時のロボット１の代表位置姿勢と自律移動中の現在のロボット１の位置姿勢情報とから、個々のランドマークが観測できるかを示す観測尤度ρ（ｉ）とランドマークの観測角度の標準偏差（分散）との２つの指標を決定する。そして、これら組み合わせて評価値Ｐにより評価することで、いろいろな方向にあるランドマークをなるべく多く検出できるように、ステレオカメラ５の方向を決定することができる。よって、より正確に自己位置を推定可能なロボット１が提供される。
【００７７】
また、検出されるランドマークＬＭの数が多くても、ランドマークＬＭが固まって存在する場合には、自己位置推定精度は良くない。しかし、上記のように、第１の実施の形態においては、評価値Ｐ（ｐ、ｔ）として個々のランドマークＬＭが観測できるか否かを示す観測尤度ρ（ｉ）と、ランドマークＬＭの観測角度の標準偏差を組み合わせた指標を用いている。そして、近接したランドマークＬＭのみである場合には評価値Ｐ（ｐ、ｔ）を小さくようにすることにより、近接したランドマークＬＭが多いか否かの要因を評価することが可能となる。よって、ランドマークが近接して存在するケースを、自己位置推定精度が悪くなり、観測には適していないケースであると判断することが可能となる効果を生み出している。このように、多くのランドマークＬＭを観測可能とすることにより、ランドマークＬＭを見る位置が代表位置とは異なる場合にも、正確に自己位置推定を行うことが可能になる。
【００７８】
（第２の実施の形態）
以下、図１２および図１３を参照しながら、第２の実施の形態によるロボット３００について説明する。第２の実施の形態において、第１の実施の形態と同様の構成および動作については、同一の符号を付し、重複説明は省略する。
【００７９】
第２の実施の形態は、ランドマークＬＭが人等の障害物により遮蔽された場合の対応を含む例である。第２の実施の形態においては、検出した障害物がランドマークを遮蔽するか否かを評価することで、障害物による遮蔽を考慮してステレオカメラ５の方向を決定する。
【００８０】
図１２は、第２の実施の形態によるロボット３００の機能を示すブロック図である。ロボット３００は、ロボット１に加え、人障害物検出部５１および遮蔽評価部５３を備えている。図１２に示すように、人障害物検出部５１は、ステレオカメラ５の視野内にある障害物を検出し、検出した障害物を円柱モデルに近似し、位置を計算する。遮蔽評価部５３は、検出された障害物が、ランドマークＬＭを遮蔽しているか否か評価する。
【００８１】
以下、図１３を参照しながらロボット３００の処理を説明するが、図１３のフローチャートのＳ１５０〜Ｓ１６４は、第１の実施の形態による図７のフローチャートと同様であるので、説明を省略する。第２の実施の形態においては、ステレオカメラ５で撮影されたステレオ画像において、人障害物検出部５１が、人障害物の位置と速度をパターン識別と３次元計測とを用いて検出して、円柱モデルに近似する（Ｓ１７０）。
【００８２】
さらに、人障害物検出部５１は、現在から所定時間後のロボット３００の予測位置から見た人障害物の位置を、Ｓ１７０で検出した位置と速度から計算する（Ｓ１７１）。視野内ランドマーク抽出部４５がパンチルト角度を確定すると、遮蔽評価部５３は、検出した人障害物を近似した３次元モデルを、ステレオカメラ５の画像に投影する（Ｓ１７２）。
【００８３】
視野内ランドマーク抽出部４５が、ステレオカメラ５の視野内に入るランドマークＬＭのリストΩを作成すると、遮蔽評価部５３は、各ランドマークＬＭをステレオカメラ５の画像面に投影して遮蔽を判定し、遮蔽されていると判定されると、リストΩから削除する（Ｓ１７３）。
【００８４】
ロボット３００は、遮蔽されたランドマークＬＭが削除されたリストΩに基づき、評価値Ｐ（ｐ、ｔ）を算出することにより、最適なパンチルト角度ｐ、ｔを求め、ステレオカメラ５を制御する。
【００８５】
以上説明したように、第２の実施の形態によるロボット３００は、自律移動時に、ステレオカメラ５によるステレオ画像を用いてパターン認識と３次元計測を行って人障害物の位置とその速度を検出し、人障害物を円柱モデルで近似する。人障害物検出部５１は、現在から所定時間後の予測したロボット３００の位置における、ロボット３００から見た障害物の位置を、人障害物の位置と速度から計算する。
【００８６】
遮蔽評価部５３は、リストΩにある各パン角度とチルト角度との組み合わせに対して、人障害物を画像上に投影し、遮蔽されるランドマークＬＭを視野内ランドマークのリストΩから削除する。観測評価部４７は、遮蔽されたランドマークＬＭが削除されたリストΩに基づいて最適なパンチルト角を求め、カメラ制御部４９は、求めたパンチルト角にステレオカメラ５を制御するための信号を出力する。
【００８７】
以上説明したように、第２の実施の形態によるロボット３００によれば、第１の実施の形態によるロボット１による効果に加え、障害物によりランドマークが遮蔽されている場合にも、視野内にランドマークを多く観測できる注視方向の決定が可能となる。この結果、ランドマークが見えずに自己位置推定の精度が悪化するのを防止することで、自律移動のロバスト性が高まる。よって、障害物がある場合にも、正確な自己位置推定が可能となる。
【００８８】
上記第１および第２の実施の形態において、ロボット１は、自己位置推定装置の一例であり、ステレオカメラ５は、撮影部の一例であり、ランドマーク抽出部３１は、距離算出部、および検出部の一例である。地図生成部３３は、代表算出部の一例であり、方向距離計算部４１は、ランドマーク位置取得部の一例であり、観測評価部４７は、撮影角度変更部の一例であり、カメラ制御部４９は、撮影制御部の一例である。また、人障害物位置検出部５１は、障害物位置検出部の一例である。
【００８９】
なお、本発明は、以上に述べた実施の形態に限定されるものではなく、本発明の要旨を逸脱しない範囲内で種々の構成または実施形態を採ることができる。
例えば、上記第１または第２の実施の形態においては、ステレオカメラ５は、２つのカメラ５Ａ、５Ｂを有した構成としたが、１台のカメラと、距離計を備えるようにしてもよい。このとき、距離計としては、例えば２次元的にスキャンが可能なレーザによる距離計を用いることができる。上記第１および第２の実施の形態においては、チルト角度ｔはゼロとしたが、任意の角度に設定するようにしてもよい。また、ランドマークＬＭの数、パン角度ｐの種類は上記に限定されない。
【００９０】
上記第１および第２の実施の形態において、観測尤度と観測角度の分散とを組み合わせて評価する評価値の算出方法は一例であり、他の方法による評価でもよい。観測尤度は、予測位置におけるランドマークの観測確率を反映した値であればよく、算出方法は上記に限定されない。さらに、観測角度を評価するために標準偏差を用いたが、分散が評価できるものであればこれに限定されない。
【００９１】
ここで、上記第１および第２の実施の形態による自己位置推定処理の動作をコンピュータに行わせるために共通に適用されるコンピュータの例について説明する。図１４は、標準的なコンピュータのハードウエア構成の一例を示すブロック図である。図１４に示すように、コンピュータ４００は、Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓiｎｇ Ｕｎiｔ（ＣＰＵ）４０２、メモリ４０４、入力装置４０６、出力装置４０８、外部記憶装置４１２、媒体駆動装置４１４、ネットワーク接続装置等がバス４１０を介して接続されている。
【００９２】
ＣＰＵ４０２は、コンピュータ４００全体の動作を制御する演算処理装置である。メモリ４０４は、コンピュータ４００の動作を制御するプログラムを予め記憶したり、プログラムを実行する際に必要に応じて作業領域として使用したりするための記憶部である。メモリ４０４は、例えばＲａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ（ＲＡＭ）、Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ（ＲＯＭ）等である。入力装置４０６は、コンピュータの使用者により操作されると、その操作内容に対応付けられている使用者からの各種情報の入力を取得し、取得した入力情報をＣＰＵ４０２に送付する装置であり、例えばキーボード装置、マウス装置などである。出力装置４０８は、コンピュータ４００による処理結果を出力する装置であり、表示装置などが含まれる。例えば表示装置は、ＣＰＵ４０２により送付される表示データに応じてテキストや画像を表示する。
【００９３】
外部記憶装置４１２は、例えば、ハードディスクなどの記憶装置であり、ＣＰＵ４０２により実行される各種制御プログラムや、取得したデータ等を記憶しておく装置である。媒体駆動装置４１４は、可搬記録媒体４１６に書き込みおよび読み出しを行うための装置である。ＣＰＵ４０２は、可搬型記録媒体４１６に記録されている所定の制御プログラムを、記録媒体駆動装置４１４を介して読み出して実行することによって、各種の制御処理を行うようにすることもできる。可搬記録媒体４１６は、例えばＣｏｎｐａｃｔ Ｄiｓｃ（ＣＤ）−ＲＯＭ、Ｄiｇiｔａｌ Ｖｅｒｓａｔiｌｅ Ｄiｓｃ（ＤＶＤ）、Ｕｎiｖｅｒｓａｌ Ｓｅｒiａｌ Ｂｕｓ（ＵＳＢ）メモリ等である。
【００９４】
上記第１および第２の実施の形態による自己位置推定方法をコンピュータに実行させるプログラムは、例えば外部記憶装置４１２に記憶させる。ＣＰＵ４０２は、外部記憶装置４１２からプログラムを読み出し、コンピュータ４００に自己位置推定の動作を行なわせる。このとき、まず、自己位置推定の処理をＣＰＵ４０２に行わせるための制御プログラムを作成して外部記憶装置４１２に記憶させておく。そして、入力装置４０６から所定の指示をＣＰＵ４０２に与えて、この制御プログラムを外部記憶装置４１２から読み出させて実行させるようにする。また、このプログラムは、可搬記録媒体４１６に記憶するようにしてもよい。
【００９５】
以上の実施形態に関し、さらに以下の付記を開示する。
（付記１）
自己位置を推定する自己位置推定装置であって、
複数のランドマークの内の少なくとも一つのランドマークの画像を含む画像を撮影する画像取得部と、
前記自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出する距離算出部と、
前記少なくとも一つのランドマークの３次元位置を取得するランドマーク位置取得部と、
前記画像、前記距離、および前記少なくとも一つのランドマークの位置に基づき前記自己位置推定装置の自己位置を推定する自己位置推定部と、
前記自己位置推定部により推定された自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出する移動位置予測部と、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出する観測尤度計算部と、
前記予測位置において前記画像が撮影される際の撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出する視野内ランドマーク抽出部と、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影部の撮影角度を変更する撮影角度変更部と、
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御する撮影制御部と、
を有し、
前記自己位置推定部は、変更後の前記撮影角度で撮影された画像と前記ランドマーク位置取得部により取得される前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新することを特徴とする自己位置推定装置。
（付記２）
前記ランドマーク位置取得部が取得した前記位置に前記ランドマークが観測された際の前記自己位置推定装置の位置を代表して対応付けられた代表位置を取得し、
前記位置および前記代表位置に基づき、前記少なくとも一つのランドマークからそれぞれの前記代表位置への第１の観測方向と第１の観測距離とを計算するとともに、
推定された前記自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出し、それぞれの前記少なくとも一つのランドマークから前記予測位置への第２の観測方向と第２の観測距離とを計算する方向距離計算部、
をさらに有し、
前記観測尤度計算部は、前記第１の観測方向と前記第２の観測方向との角度差と、前記第１の観測距離と前記第２の観測距離との距離差とに基づき、前記視野内ランドマークの各々に関して観測尤度を算出することを特徴とする付記１に記載の自己位置推定装置。
（付記３）
取得された前記画像および算出された前記距離に基づき、前記少なくとも一つのランドマークの位置を検出する検出部と、
前記少なくとも一つのランドマークを検出した際の前記自己位置推定装置の位置を代表する代表位置を前記少なくともひとつのランドマークのそれぞれについて算出する代表算出部と、
をさらに有することを特徴とする付記２に記載の自己位置推定装置。
（付記４）
前記撮影角度変更部は、前記撮影角度において撮影された画像おける前記視野内ランドマークの前記観測尤度の総和と、前記観測角度の分散とに基づき前記評価値を算出することを特徴とする付記１から付記３のいずれかに記載の自己位置推定装置。
（付記５）
障害物の位置を検出する障害物位置検出部と、
前記予測位置において前記障害物が前記少なくとも一つのランドマークを遮蔽するか否かを判別する遮蔽評価部と、
前記遮蔽評価部が、前記障害物が前記少なくとも一つのランドマークを遮蔽すると判別すると、前記抽出部は、抽出した前記視野内ランドマークから遮蔽される前記ランドマークを削除することを特徴とする付記１から付記４のいずれかに記載の自己位置推定装置。
（付記６）
前記遮蔽評価部は、前記障害物をモデル化し、前記撮影角度で撮影された画像において、前記モデル化された障害物が前記少なくとも一つのランドマークを遮蔽するか否かを判別することを特徴とする付記４に記載の自己位置推定装置。
（付記７）
複数のランドマーク内の少なくとも一つのランドマークの画像を含む画像を取得し、
自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出し、
前記少なくとも一つのランドマークの位置を取得し、
前記取得された画像、前記算出された距離、および前記取得された前記少なくとも一つのランドマークの位置に基づき自己位置を推定し、
前記推定された自己位置に基づき移動時の将来の予測位置を算出し、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出し、
前記予測位置において画像が撮影される撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出し、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更し、
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御し、
変更後の前記撮影角度で撮影された画像と取得された前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新することを特徴とする自己位置推定方法。
（付記８）
前記少なくとも一つのランドマークが観測された際の前記自己位置推定装置の代表位置を取得し、
前記位置および前記代表位置に基づき、前記少なくとも一つのランドマークからそれぞれの前記代表位置への第１の観測方向と第１の観測距離とを計算するとともに、
推定された前記自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出し、それぞれの前記少なくとも一つのランドマークから前記予測位置への第２の観測方向と第２の観測距離とを計算し、
前記第１の観測方向と前記第２の観測方向との角度差と、前記第１の観測距離と前記第２の観測距離との距離差とに基づき、前記視野内ランドマークの各々に関して観測尤度を算出することを特徴とする付記７に記載の自己位置推定方法。
（付記９）
取得された前記画像および算出された前記距離に基づき、前記少なくとも一つのランドマークの位置を検出し、
前記少なくとも一つのランドマークを検出した際の前記自己位置推定装置の位置を代表する代表位置を前記少なくともひとつのランドマークのそれぞれについて算出する、
ことを特徴とする付記８に記載の自己位置推定方法。
（付記１０）
前記評価値は、前記視野内ランドマークの前記観測尤度の総和と、前記観測角度の分散とに基づき算出されることを特徴とする付記７から付記９のいずれかに記載の自己位置推定方法。
（付記１１）
障害物の位置を検出し、
前記予測位置において前記障害物が前記少なくとも一つのランドマークを遮蔽するか否かを判別し、
前記障害物が前記少なくとも一つのランドマークを遮蔽すると判別されると、抽出した前記視野内ランドマークから遮蔽される前記ランドマークを削除することを特徴とする付記６から付記９のいずれかに記載の自己位置推定方法。
（付記１２）
複数のランドマーク内の少なくとも一つのランドマークの画像を含む画像を取得し、
自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出し、
前記少なくとも一つのランドマークの位置を取得し、
前記取得された画像、前記算出された距離、および前記取得された前記少なくとも一つのランドマークの位置に基づき自己位置を推定し、
前記推定された自己位置に基づき移動時の将来の予測位置を算出し、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出し、
前記予測位置において画像が撮影される撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出し、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更し、
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御し、
変更後の前記撮影角度で撮影された画像と取得された前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新する処理をコンピュータに実行させることを特徴とするプログラム。
（付記１３）
前記少なくとも一つのランドマークが観測された際の前記自己位置推定装置の代表位置を取得し、
前記位置および前記代表位置に基づき、前記少なくとも一つのランドマークからそれぞれの前記代表位置への第１の観測方向と第１の観測距離とを計算するとともに、
推定された前記自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出し、それぞれの前記少なくとも一つのランドマークから前記予測位置への第２の観測方向と第２の観測距離とを計算し、
前記第１の観測方向と前記第２の観測方向との角度差と、前記第１の観測距離と前記第２の観測距離との距離差とに基づき、前記視野内ランドマークの各々に関して観測尤度を算出することを特徴とする付記１２に記載のプログラム。
（付記１４）
取得された前記画像および算出された前記距離に基づき、前記少なくとも一つのランドマークの位置を検出し、
前記少なくとも一つのランドマークを検出した際の前記自己位置推定装置の位置を代表する代表位置を前記少なくともひとつのランドマークのそれぞれについて算出することを特徴とする付記１３に記載のプログラム。
（付記１５）
前記評価値は、前記視野内ランドマークの前記観測尤度の総和と、前記観測角度の分散とに基づき算出されることを特徴とする付記１２から付記１４のいずれかに記載のプログラム。
【符号の説明】
【００９６】
１ ロボット
３ 演算処理装置
５ ステレオカメラ
７ パンチルトモータ
９ パンチルト制御部
１１ 車輪制御部
１３ 車輪モータ
１５ 記憶部
１７ 本体
１９ 車輪
３１ ランドマーク抽出部
３３ 地図生成部
３５ 自己位置推定部
３７ 移動位置予測部
３９ 移動経路計画部
４１ 方向距離計算部
４３ 観測尤度計算部
４５ 視野内ランドマーク抽出部
４７ 観測評価部
４９ カメラ制御部
５１ 人障害物検出部
５３ 遮蔽評価部
５５ ランドマーク地図
【技術分野】
【０００１】
本発明は、自己位置推定装置、自己位置推定方法およびプログラムに関する。
【背景技術】
【０００２】
移動ロボットの自律移動を制御する方式として、ランドマーク（目印）となる局所的な画像を用いる手法が従来から用いられている。例えば、予め走行経路上で撮影した画像を記憶し、撮影した画像の方向が一定の方向になるようにロボットの指令値を制御して走行を制御する方式が知られている。この方式では、レーザ等の他のセンサデータを用いた軌道制御を備え、画像による制御と適宜切り替えるようにする場合がある。
【０００３】
ランドマークの位置を記したランドマーク地図を利用して自己位置を推定する方法も知られている。ランドマーク地図の生成には、Ｓｃａｌｅ Ｉｎｖａｒiａｎｔ Ｆｅａｔｕｒｅ Ｔｒａｎｓｆｏｒｍ（ＳＩＦＴ）特徴量を用いたビジョンベースＳiｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌiｚａｔiｏｎ Ａｎｄ Ｍａｐ Ｂｕiｌｄiｎｇ（ＳＬＡＭＢ）アルゴリズムに基づく例がある。このとき、ロボットは、生成したランドマーク地図を参照するとともに、撮影した画像においてランドマークを抽出して、自己位置を推定することができる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１０−１４０２４６号公報
【非特許文献】
【０００５】
【非特許文献１】陳彬、中尾学、深貝卓也、沢崎直之 著「ビジュアルランドマークとレイアウト地図を用いた移動地図を用いた移動知能用ナビゲーションシステムの開発」
【非特許文献２】S. Se, D. Lowe, and J. Little.著“Local and global localization for mobile robots using visual landmarks.” In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 414-420, Maui, Hawaii, October 2001.
【発明の概要】
【発明が解決しようとする課題】
【０００６】
ところで、上記のようにランドマークを用いた自己位置推定においては、複数のランドマークが観測可能でも、取得した画像において互いに近傍に観測される場合には、自己位置推定の精度が低下してしまう。
【０００７】
また、例えば、移動経路上に障害物がある場合には、障害物を回避するために、ロボットが撮影した画像の方向が一定の方向になる経路とは、異なる経路を移動する必要がある。このようにロボットが経路を外れた場合には、既に撮影された画像とは異なる方向にカメラが向き、そのランドマークが見える角度のずれ量が大きくなる場合、或いはランドマークとのパターンマッチングの一致度が大きく低下する場合がある。さらには、ランドマークを検出できない場合が発生する。
【０００８】
このようなランドマークが互いに近傍に観測される場合、ランドマークが見える角度のずれ量が大きい場合や、検出できない場合では、走行部のエンコーダを元にしたオドメトリ情報のみで自己位置を推定することになる。このように、ランドマークが見えない期間が長くなるとオドメトリの誤差が積算されて自己位置推定精度が悪くなり、自己位置を見失ってしてしまう可能性がある。
【０００９】
上記課題に鑑み、ランドマークを見失うことを防止することにより、自己の位置を正確に推定することが可能な自己位置推定装置、自己位置推定方式およびプログラム提供する。
【課題を解決するための手段】
【００１０】
ひとつの態様である自己位置推定装置は、撮影部、距離算出部、ランドマーク位置取得部、自己位置推定部、移動位置予測部、観測尤度計算部、視野内ランドマーク抽出部、撮影角度変更部、撮影制御部を有することを特徴としている。撮影部は、少なくとも一つのランドマークの画像を含む画像を撮影する。距離算出部は、自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出する。ランドマーク位置取得部は、少なくとも一つのランドマークの位置を取得する。自己位置推定部は、前記画像、前記距離、および前記少なくとも一つのランドマークの位置に基づき前記自己位置推定装置の自己位置を推定する。移動位置予測部は、前記自己位置推定部により推定された自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出する。観測尤度計算部は、前記予測位置における前記少なくとも一つのランドマークが観測される観測尤度を算出する。視野内ランドマーク抽出部は、前記予測位置において前記画像が撮影される際の撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出する。撮影角度変更部は、抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更する。撮影制御部は、変更後の前記撮影角度で前記画像が撮影されるように撮影を制御する。前記自己位置推定部は、変更後の前記撮影角度で撮影された画像とランドマーク位置取得部により取得される前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新する。
【００１１】
別の態様である自己位置推定方法においては、少なくとも一つのランドマークの画像を含む画像を取得し、前記自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出し、前記少なくとも一つのランドマークの位置を取得する。前記取得された画像、前記算出された距離、および前記記憶された前記少なくとも一つのランドマークの位置に基づき自己位置を推定し、前記推定された自己位置に基づき移動時の将来の予測位置を算出する。前記予測位置において前記少なくとも一つのランドマークが観測される確率である観測尤度を算出する。前記予測位置において画像が撮影される撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出する。また、抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更し、変更後の前記撮影角度で前記画像が撮影されるように撮影を制御する。さらに、変更後の前記撮影角度で撮影された画像と取得された前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新することを特徴としている。
【００１２】
なお、上述した本発明に係る方法をコンピュータに行わせるためのプログラムであっても、このプログラムを当該コンピュータによって実行させることにより、上述した本発明に係る方法と同様の作用・効果を奏するので、前述した課題が解決される。
【発明の効果】
【００１３】
上述した態様の自己位置推定装置、自己位置推定方法およびプログラムによれば、自己位置を正確に推定することが可能になる。
【図面の簡単な説明】
【００１４】
【図１】第１の実施の形態によるロボットの構成を示すブロック図である。
【図２】第１の実施の形態によるロボットの機能を示すブロック図である。
【図３】第１の実施の形態によるランドマーク地図生成の動作を示すフローチャートである。
【図４】第１の実施の形態によるランドマーク地図の一例を示す図である。
【図５】第１の実施の形態による観測方向ベクトルと観測距離の一例を示す図である。
【図６】第１の実施の形態によるランドマークの位置と推定される自己位置の関係を示す図であり、（ａ）はランドマーク１個の場合、（ｂ）は、ランドマーク２個が近接して観測された場合、（ｃ）は、ランドマーク２個が所定距離離れて観測された場合、（ｄ）は、ランドマーク３個が近接して観測された場合である。
【図７】第１の実施の形態によるロボットによる自己位置推定処理を示すフローチャートである。
【図８】第１の実施の形態によるパンチルト角度リストの一例を示す図である。
【図９】第１の実施の形態によるリストΩの一例を示す図である。
【図１０】第１の実施の形態によるロボットおよびランドマークの位置関係の一例を示す図である。
【図１１】第１の実施の形態による評価値の算出例を示す図である。
【図１２】第２の実施の形態によるロボットの機能を示すブロック図である。
【図１３】第２の実施の形態による自己位置推定処理を示すフローチャートである。
【図１４】標準的なコンピュータの構成を示す図である。
【発明を実施するための形態】
【００１５】
（第１の実施の形態）
以下、図面を参照しながら第１の実施の形態によるロボット１について説明する。図１は、第１の実施の形態によるロボット１の構成を示すブロック図、図２は、第１の実施の形態によるロボット１の機能を示すブロック図である。
【００１６】
図１に示すように、ロボット１は、ステレオカメラ５、パンチルトモータ７、パンチルト制御部９、車輪制御部１１、車輪モータ１３、記憶部１５を有し、演算処理装置３により制御されている。
【００１７】
図２に示すように、ステレオカメラ５は、互いに平行な方向に撮影方向を設定され、所定間隔を隔てて配置された２つのカメラ５Ａ、５Ｂを有している。所定間隔は、２つのカメラ５Ａ、５Ｂで撮影した画像により、撮影対象までの距離が所定の精度で算出できる間隔であることが好ましい。カメラ５Ａ、５Ｂは、少なくとも静止画像を撮影し、電気的信号に変換して出力する撮影装置である。なお、カメラ５Ａ、５Ｂにより撮影された２つの画像をまとめてステレオ画像ともいう。
【００１８】
パンチルトモータ７は、ステレオカメラ５の撮影方向を変化させるための駆動部である。パンチルト制御部９は、演算処理装置３による指示によりパンチルトモータ７の動作を制御する制御部である。車輪モータ１３は、図２に示す車輪１９を回転させてロボット１を移動させるための駆動部である。車輪制御部１１は、演算処理装置３による指示により車輪モータ１３を制御する制御部である。
【００１９】
記憶部１５は、例えばＲｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ（ＲＯＭ）や、Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ（ＲＡＭ）、またはハードディスク装置等の記憶装置である。記憶部１５は、ロボット１の動作を制御するプログラムを予め記憶したり、プログラムを実行する際に必要に応じて作業領域として使用したりするための記憶装置である。また、記憶部１５は、図２に示すランドマーク地図５５を格納している。演算処理装置３が、記憶部１５のプログラムを読み込み実行することにより、ロボット１の各構成要素の動作制御が可能になる。
【００２０】
図２に示すように、ロボット１は、車輪１９が備えられた本体１７を有している。本体１７は、直方体形状であり、車輪１９は、本体１７の下部に備えられている。本体１７は、車輪１９が車輪制御部１１および車輪モータ１３を介して制御されることにより移動可能に構成されている。
【００２１】
本体１７の上面１７Ａには、ステレオカメラ５が載置されている。ステレオカメラ５は、パンチルトモータ７、パンチルト制御部９により制御されることにより撮影方向が可変に構成されている。なお、本実施の形態においては、ロボット１の上下方向を図２に示すｚ軸に平行な方向とするものとし、水平方向は、図２に示すｙ軸に平行な方向、前後方向は、ｘ軸に平行な方向とする。また、第１の実施の形態においては、パンチルトモータ７は、ステレオカメラ５をｚ軸に垂直な面内、すなわち水平方向に回転させるものとする。
【００２２】
ロボット１の演算処理装置３は、ランドマーク抽出部３１、地図生成部３３、自己位置推定部３５、移動位置予測部３７、移動経路計画部３９、方向距離計算部４１、観測尤度計算部４３、視野内ランドマーク抽出部、観測評価部４７、カメラ制御部４９の機能を有する。
【００２３】
ランドマーク抽出部３１は、ステレオカメラ５のカメラ５Ａ、５Ｂが撮影した２つの画像から、目印となる局所的な画像領域であるランドマークを抽出する。また、ランドマーク抽出部３１は、ステレオカメラ５のカメラ５Ａ、５Ｂが撮影した２つの画像に基づいて抽出したランドマークまでの距離を算出することにより、３次元位置を特定する。
【００２４】
地図生成部３３は、抽出したランドマークと特定した３次元位置とを関連付けた情報の集合であるランドマーク地図を生成し、ランドマーク地図５５に記憶させる。このとき、地図生成部３３は、ランドマーク地図５５に記憶させたランドマークの３次元位置を特定したときに用いた画像を撮影した際の、ロボット１の位置および向き（以下、ロボット１の位置姿勢という）、ステレオカメラ５の角度を記臆させておくことが好ましい。また、地図生成部３３は、各ランドマークを撮影可能なロボット１の位置に対応した位置姿勢を、各ランドマークに関連する代表位置として例えばランドマーク地図５５に記憶させておく。代表位置は、例えば、各ランドマークを撮影したときのロボット１の位置の平均とすることが好ましい。
【００２５】
自己位置推定部３５は、ランドマーク抽出部３１が抽出したランドマークの３次元位置と、ランドマーク地図５５を参照することにより、ロボット１の自己位置を推定する。移動位置予測部３７は、自己位置推定部３５で推定された自己位置と、移動経路計画部３９で計画される経路により、ロボット１の所定時間後の移動予測位置を算出する。移動経路計画部３９は、ロボット１を移動させる経路を計画する。
【００２６】
方向距離計算部４１は、各ランドマークについて、ランドマーク地図５５に記憶されている情報を参照し、各ランドマークから対応するそれぞれの代表位置への方向ベクトルを算出する。また、方向距離計算部４１は、移動位置予測部３７が予測した、ロボット１の将来の予測位置において観測可能な視野内の各ランドマークから、対応する予測位置への方向ベクトルを算出する。さらに方向距離計算部４１は、算出した２つの方向ベクトルに基づき、２つの方向ベクトルの角度差と距離差とを算出する。なお、角度差と距離差の詳細については後述する。
【００２７】
観測尤度計算部４３は、方向距離計算部４１が算出した角度差および距離差に基づき、各ランドマークに関する観測尤度を算出する。視野内ランドマーク抽出部４５は、ステレオカメラ５の角度をパンチルト制御部９により変化させたときに撮影される画像内に含まれると判断される視野内ランドマークを、ステレオカメラ５のそれぞれの角度に応じて抽出する。また、このときのステレオカメラ５から見たランドマークＬＭの観測角度を算出する。なお、観測尤度および観測角度の詳細については後述する。
【００２８】
観測評価部４７は、移動位置予測部３７が予測した予測位置においてステレオカメラ５の角度を変化させた際の各視野内ランドマークについて、観測尤度および観測角度に基づく評価値を算出する。観測評価部４７は、算出した評価値に基づき、予測位置における最適なステレオカメラ５の角度を判別する。なお、評価値の詳細については後述する。カメラ制御部４９は、観測評価部４７で算出された最適な角度にステレオカメラ５を制御するように、パンチルト制御部９に制御のための信号を出力する。
【００２９】
以下、図３から図１３を参照しながら、第１の実施の形態によるロボット１の動作について説明する。第１の実施の形態によるロボット１の自己位置推定においては、まず、ランドマーク地図５５を作成する。このようなロボット１では、ステレオカメラ５は、移動の際には姿勢をロボット１を正面方向に固定しているのが一般的である。よって、本実施の形態においても、自律移動を行なう前に、演算処理装置３は、パンチルト制御部９によりステレオカメラ５の姿勢をロボット１の正面方向に設定する。そしてその状態で、演算処理装置３は、車輪制御部１１により車輪モータ１３を介して車輪１９を制御して、実運用での自律移動で使用する経路に沿って予めロボット１を動かし、ランドマークの３次元位置を記したランドマーク地図５５を生成する。
【００３０】
図３は、ランドマーク地図生成の動作を示すフローチャートである。ランドマーク地図５５の作成時には、演算処理装置３は、車輪制御部１１を介してロボット１を、使用する経路のスタート地点に配置し、時刻ｊ＝０とする（Ｓ１０１）。ここで、演算処理装置３は、ステレオカメラ５にステレオ画像を撮影させ、ランドマーク抽出部３１は、撮影された画像においてランドマークを検出する（Ｓ１０２）。ランドマークの検出は、例えばパターマッチング法や特徴量抽出法により行なうことができる。
【００３１】
演算処理装置３は、地図生成部３３によりランドマークを検出したときのロボット１の位置姿勢と、ランドマークの位置とを同時に推定し、ランドマーク地図５５を更新する（Ｓ１０３）。
【００３２】
ここで、時刻ｊにおけるロボット１の位置姿勢は、位置姿勢（ｘｒ（ｊ）、ｙｒ（ｊ）、θｒ（ｊ））と表す。ここで、座標ｘｒ（ｊ）、ｙｒ（ｊ）は、時刻ｊにおけるロボット１の、図２に示したｘｙｚ座標系におけるｘｙ平面上の座標（ｘｒ（ｊ）、ｙｒ（ｊ））であり、角度θｒ（ｊ）は、ロボット１の正面方向がｘ軸となす角度である。
【００３３】
図２のｘｙｚ座標系は、ロボットが移動する空間において定める。例えば、ｚ軸を鉛直方向とし、ｘ、ｙ軸を、ｚ軸に垂直な平面内で互いに直交する軸として定める。このとき原点（０、０、０）は、例えば、ランドマーク地図５５生成時のロボット１のスタート地点とすることができる。なお、撮影した画像を用いてロボット１の位置姿勢とランドマークの位置とを同時に推定する方法は、例えば非特許文献１の方法など、公知の方法を用いることができる。
【００３４】
また、ランドマークの３次元位置は、例えば識別情報ＩＤ＝i（i＝１〜Ｉ：Ｉはランドマークの数）のランドマークをランドマークＬＭiとして、３次元位置（ｘｌ（ｉ）、ｙｌ（ｉ）、ｚｌ（ｉ））と表す。ここで、座標ｘｌ（ｉ）、ｙｌ（ｉ）、ｚｌ（ｉ）は、図２のｘｙｚ座標系における座標である。
【００３５】
図４は、ランドマーク地図５５の一例を示す図である。図４に示すように、例１２０においては、例えば、識別情報ＩＤ＝１では、３次元位置（ｘｌ（１）、ｙｌ（１）、ｚｌ（１））＝（４．５、１．２、１．０）である。また、識別情報ＩＤ＝２に対しては、３次元位置（ｘｌ（２）、ｙｌ（２）、ｚｌ（２））＝（５．０、−０．４、１．０）である。以下同様である。
【００３６】
図３に戻って、演算処理装置３の自己位置推定部３５は、時刻ｊにおける推定したロボット１の位置姿勢（ｘｒ（ｊ）、ｙｒ（ｊ）、θｒ（ｊ））と、検出したランドマークＬＭｉの識別情報ＩＤのリストを、互いに関連付けて例えばランドマーク地図５５に保存する（Ｓ１０４）。演算処理装置３は、ｊ＝ｊ＋１とし（Ｓ１０５）、経路の終点であるゴールへ付いたか否か判別する（Ｓ１０６）。ゴールへつかないと判別された場合には（Ｓ１０６：ＮＯ）、Ｓ１０２の処理に戻る。ゴールへついたと判別された場合には（Ｓ１０６：ＹＥＳ）、演算処理装置３は処理をＳ１０７に進める。
【００３７】
Ｓ１０７では、演算処理装置３の地図生成部３３は、まず、ランドマークＬＭ（ランドマークＬＭｉの総称）の識別情報ＩＤ＝１とする。Ｓ１０８の処理では、地図生成部３３は、識別情報ＩＤ＝i（i＝１〜Ｉ）のランドマークＬＭiを観測した全ての時刻ｊにおけるロボット１の位置姿勢（ｘｒ（ｊ）、ｙｒ（ｊ）、θｒ（ｊ））の平均を求める。求めた平均を、ランドマークＬＭiを観測した際の、ロボット１の代表位置姿勢（ｘｒ（ｉ）、ｙｒ（ｉ）、θｒ（ｉ））として決定する（Ｓ１０８）。さらに、地図生成部３３は、識別情報ＩＤ＝iの地図生成時における、ランドマークＬＭiから、対応する代表位置への観測方向ベクトル（ｄｘ（ｉ）、ｄｙ（ｉ）、ｄｚ（ｉ））と観測距離ｄ（ｉ）とを計算する（Ｓ１０９）。なお、代表位置とは、代表位置位置姿勢（ｘｒｊ、ｙｒｊ、θｒｊ）におけるロボット１の位置（ｘｒｊ、ｙｒｊ）である。
【００３８】
図５は、観測方向ベクトル（ｄｘ（ｉ）、ｄｙ（ｉ）、ｄｚ（ｉ））と、観測距離ｄ（ｉ）の一例を示す図である。図５に示す例では、ランドマークＬＭiを観測した場合のロボット１の代表位置が、代表位置姿勢（ｘｒ（ｉ）、ｙｒ（ｉ）、θｒ（ｉ））である。このとき、観測方向ベクトル（ｄｘ（ｉ）、ｄｙ（ｉ）、ｄｚ（ｉ））は、図５に示すベクトル６２であり、ベクトル６２の長さが、観測距離ｄ（ｉ）となる。
【００３９】
演算処理装置３は、i＝i＋１とし（Ｓ１１０）、i＞Ｉであるか否か、すなわち全てのランドマークＬＭiについてＳ１０８、１０９の処理が終了したか否か判別する（Ｓ１１１）。i≦Ｉである場合には（Ｓ１１１：ＮＯ）、演算処理装置３は、Ｓ１０８に戻って処理を繰り返し、i＞Ｉの場合には（Ｓ１１１：ＹＥＳ）、ランドマーク地図生成処理を終了する。
【００４０】
以上の処理により、ランドマーク地図５５として、それぞれのランドマークＬＭの３次元位置（ｘｌ（ｉ）、ｙｌ（ｉ）、ｚｌ（ｉ））が、ランドマークＬＭの識別情報ＩＤと関連付けて記憶される。また、ランドマーク地図５５には、各ランドマークＬＭに対応するロボット１の代表位置における代表位置姿勢（ｘｒ（ｉ）、ｙｒ（ｉ）、θｒ（ｉ））が、識別情報ＩＤと関連付けて記録されることが好ましい。また、ランドマーク地図５５には、各ランドマークＬＭiに対応する観測方向ベクトル（ｄｘ（ｉ）、ｄｙ（ｉ）、ｄｚ（ｉ））、観測距離ｄ（ｉ）を記録することが好ましい。
【００４１】
次に、図６から図９を参照しながら、ロボット１による自己位置推定の処理について説明する。自律走行による自己位置推定は、ランドマークＬＭの検出数が多く、さらにいろいろ方向にあるランドマークＬＭを検出できた方が推定精度は高くなる。例えば、図６を参照しながら、以下の３つのケースの場合を考える。図６は、ランドマークの位置と推定される自己位置の関係を示す図である。ここで、（ａ）はランドマーク１個の場合、（ｂ）は、ランドマーク２個が近接して観測された場合、（ｃ）は、ランドマーク２個が所定距離離れて観測された場合、（ｄ）は、ランドマーク３個が近接して観測された場合である。
【００４２】
第１の場合として、図６（ａ）に示すように、ランドマーク６１を１個、距離ｄ、誤差±ｅで観測した場合には、ロボットの位置はランドマーク６１を中心とした半径ｄ−ｅと半径ｄ＋ｅの円の間の円環内６３にあると位置を推定され、推定結果の広がりが大きいことになる。
【００４３】
第２の場合として、図６（ｂ）に示すように、近接したランドマーク６５、６７の２個をそれぞれ距離ｄ、誤差±ｅで観測した場合には、ロボットの位置はそれぞれのランドマーク６５、６７を中心とした円環同士が重なった部分６９として推定される。しかし、円環同士の重なりが大きいため、推定結果の広がりが大きい。
【００４４】
第３の場合として、図６（ｃ）に示すように、離れた位置にあるランドマーク７１、７３の２個をそれぞれ距離ｄ、誤差±ｅで観測した場合には、ロボットの位置はそれぞれのランドマークを中心とした円環同士が重なった部分７４として推定される。この場合には、円環同士の重なりが小さいため、推定結果の広がりが小さい。
【００４５】
第４の場合として、図６（ｄ）に示すように、３個のランドマーク７５、７７、７９をそれぞれ距離ｄ、誤差±ｅで観測した場合でも、互いに近接して観測されると、円環同士の重なった部分８１が大きく、推定結果の広がりが大きい。
【００４６】
このように自己位置推定は、ランドマークの検出数が多く、さらにいろいろな方向にあるランドマークを検出できた方が推定精度は高くなる。そこで、自律移動のロバスト性を高めるためには、ランドマークが見えない期間が長くなって位置をロストすることがないように、ランドマークが多く、多方向から観測できるように注視方向を決定して、自己位置を大きく見失うことがない状態を持続させるようにする。
【００４７】
図７は、ロボット１による自己位置推定処理を示すフローチャートである。ここでは、ロボット１は、予め定められた経路に従い、自律走行を行うものとする。図７に示すように、まず、演算処理装置３の自己位置推定部３５は、撮影された画像とランドマーク地図５５とを参照してロボット１の自己位置を推定する（Ｓ１５０）。このとき、ステレオカメラ５の撮影方向は、初期状態においてはロボット１の正面方向に設定することが好ましい。このとき、ランドマーク地図５５を参照しながら行なうＳ１５１における自己位置の推定は、例えば非特許文献２における方法等、公知の方法を用いて行う。
【００４８】
演算処理装置３は、自己位置推定部３５により推定された自己位置によりゴールに到着したか否かを判別し（Ｓ１５１）、ゴールに到着した場合には（Ｓ１５１：ＹＥＳ）、処理を終了する。ゴールに到着していない場合には（Ｓ１５１：ＮＯ）、演算処理装置３は処理をＳ１５２に進める。
【００４９】
移動経路計画部３９は、自己位置推定部３５が推定した自己位置に基づき、移動経路を計画する。移動位置予測部３７は、自己位置推定部３５が推定した自己位置と、移動経路計画部３９で計画された移動経路とから、一定時間後（例えば１００ミリ秒後）のロボット１の予測位置を算出する（Ｓ１５２）。
【００５０】
方向距離計算部４１は、ランドマークＬＭの識別情報ＩＤ＝１とする（Ｓ１５３）。方向距離計算部４１は、Ｓ１５２で計算された予測位置へのランドマークＬＭｉからの観測方向ベクトル（ｄｘａ（ｉ）、ｄｙａ（ｉ）、ｄｚａ（ｉ））を算出する。方向距離計算部４１は、ランドマークＬＭｉから代表位置への観測方向ベクトル（ｄｘ（ｉ）、ｄｙ（ｉ）、ｄｚ（ｉ））と、予測位置への観測方向ベクトル（ｄｘａ（ｉ）、ｄｙａ（ｉ）、ｄｚａ（ｉ））とから、角度差φ（ｉ）を計算する（Ｓ１５４）。角度差φ（ｉ）は、以下の式で算出する。
φ（ｉ）＝ａｒｃｃｏｓ（ｄｘ（ｉ）・ｄｘａ（ｉ）＋ｄｙ（ｉ）・ｄｙａ（ｉ）＋ｄｚ（ｉ）・ｄｚａ（ｉ））
・・・式２０１
また、方向距離計算部４１は、ランドマークＬＭｉの代表位置（ｘｒ（ｉ）、ｙｒ（ｉ））からの観測距離ｄ（ｉ）と予測位置からの観測距離ｄａ（ｉ）とから距離差δ（ｉ）を計算する（Ｓ１５５）。
【００５１】
観測尤度計算部４３は、Ｓ１５４、Ｓ１５５で算出された角度差φ（ｉ）と距離差δ（ｉ）とから、観測尤度ρ（ｉ）を決定する。観測尤度ρ（ｉ）は、例えば以下の式２０２のようにφ（ｉ）とδ（ｉ）とが小さいほど尤度が大きくなる観測モデルを用いる。以下の式で算出する。ここで、σ１、σ２は係数である。
【００５２】
【数１】
【００５３】
観測尤度計算部４３は、ｉ＝ｉ＋１とし（Ｓ１５７）、ｉ＞Ｉとなるまで、Ｓ１５４からＳ１５７の処理を繰り返す（Ｓ１５８：ＮＯ）。ｉ＞Ｉとなると（Ｓ１５８）、演算処理装置３は、処理をＳ１５９に進める。Ｓ１５９では、視野内ランドマーク抽出部４５は、ｍ＝１とし、記憶部１５においてパンチルト角度リストを参照する（Ｓ１５９）。
【００５４】
図８は、パンチルト角度リストの一例を示す図である。上述したように、第１の実施の形態によるロボット１においては、ステレオカメラ５は、ｚ軸に垂直な平面の方向にのみ角度を変化させており（パン角度）、ｙ軸に垂直な平面の方向のチルト角度はゼロに設定している。図８の例では、ｍ＝１〜５に対応してパン角度＝−０．５２〜０．５２（ｒａｄ）の５段階の角度が設定されている。
【００５５】
視野内ランドマーク抽出部４５は、各ｍに対応してパン角度を変化させ、それぞれのパン角度で視野内に入るランドマークＬＭ（以下、視野内ランドマークという）を抽出し、ランドマークＩＤリストΩを生成する（Ｓ１６０）。また、視野内ランドマークが観察される観測角度を算出する。観測角度とは、予測位置へのランドマークＬＭからの観測方向ベクトルのｘｙ平面への投影成分が、ロボット１の正面方向となす鋭角である。
【００５６】
図９は、リストΩの一例を示す図である。図９に示すように、リストΩの一例１８４は、例えばｍ＝１のときには、ランドマークＬＭ２、ランドマークＬＭ３が観測されていることを示している。ｍ＝２のときには、ランドマークＬＭ１、ランドマークＬＭ２が観測され、ｍ＝３、４のときには、ランドマークＬＭ１が観測されている。ｍ＝５のときには、ランドマークＬＭ４、ランドマークＬＭ５が観測されている。
【００５７】
観測評価部４７は、それぞれのパン角度ｐにおける視野内ランドマークについて、それぞれ上述の観測尤度ρ（ｉ）の総和と、観測角度の標準偏差を算出する。観測評価部４７は、算出された観測尤度ρ（ｉ）の総和と、観測角度の標準偏差から、評価値Ｐ（ｐ、ｔ）を算出する。
【００５８】
例えば評価値としては、α、βを係数として以下の式が適用可能である。
評価値Ｐ（ｐ、ｔ）
＝α（係数）×（抽出されたランドマークＬＭの観測尤度ρ（i）の総和）
＋β（係数）×（抽出されたランドマークＬＭの観測角度の標準偏差）
・・・式２０３
【００５９】
続いて観測評価部４７は、ｍ＝ｍ＋１と置き換え（Ｓ１６２）、ｍ＞Ｍ（Ｍは、パンチルト角度を変化させる種類の数）となるまで（Ｓ１６３：ＮＯ）、Ｓ１６０からＳ１６２の処理を繰り返す。ｍ＞Ｍとなると（Ｓ１６３：ＹＥＳ）、観測評価部４７は、評価値Ｐ（ｐ、ｔ）が最大となるパンチルト角度ｐ、ｔ（パン角度ｐおよびチルト角度ｔの総称であり、本実施の形態においては、ｔ＝０）を求める。
【００６０】
なお、抽出されたランドマークＬＭの観測尤度ρ（ｉ）の総和が高いほど評価値Ｐ（ｐ、ｔ）が高く、抽出されたランドマークＬＭの観測角度の標準偏差が大きいほど評価値Ｐ（ｐ、ｔ）が高い。
【００６１】
このように、評価値Ｐ（ｐ、ｔ）が最大となるパン角度ｐとチルト角度ｔの組み合わせを求めることでステレオカメラ５のパン角度ｐとチルト角度ｔを決定する。カメラ制御部４９は、求めた角度にパンチルト制御部９がステレオカメラ５の角度を制御するように信号を出力する。
【００６２】
以下、図１０、１１を参照しながら、具体的な評価値Ｐ（ｐ、ｔ）の算出例について説明する。図１０は、ロボット１およびランドマークＬＭの位置関係の一例を示す図であり、図１１は、評価値Ｐ（ｐ、ｔ）の算出例を示す図である。
【００６３】
図１０に示すように、ロボット１の現在位置は、例えばｘｙｚ座標系における位置（０．０、０．０、１．０）であるとする。このとき、ランドマークＬＭ１（４．５、１．２、１．０）、ランドマークＬＭ２（５．０、−０．６）、ランドマークＬＭ３（３．５、−１．０、１．０）、ランドマークＬＭ４（４．０、３．９、１．０）、ランドマークＬＭ５（３．５、３．６、１．０）が存在しているとする。
【００６４】
図１１に示すように、移動位置予測部３７は、図１０の状態から所定時間後のロボット１の予測位置を位置（１．０、１．０、１．０）であると算出したとする。この位置で、方向距離計算部４１は、それぞれのランドマークＬＭｉ（ｉ＝１〜５）について、上述のように角度差φ（ｉ）および距離差δ（ｉ）を算出する。また、方向距離計算部４１は、それぞれのランドマークの観測角度を算出する。
【００６５】
観測尤度計算部４３は、算出した角度差φ（ｉ）および距離差δ（ｉ）に基づき、それぞれのランドマークＬＭｉ（ｉ＝１〜５）について、上述のように観測尤度ρ（ｉ）を算出する。
【００６６】
一例として、式２０２のパラメータを、σ１＝１．０、σ２＝０．２、式２０４のパラメータを、α＝β＝０．５とする。このとき、式２０１および式２０２から、ランドマークＬＭ１〜ランドマークＬＭ５の観測尤度および観測角度は以下のように算出される。
ランドマークＬＭ１：観測尤度 ０．２４４、観測角度＝０．０５７（ｒａｄ）
ランドマークＬＭ２：観測尤度 ０．２６０、観測角度=−０．３８１（ｒａｄ）
ランドマークＬＭ３：観測尤度 ０．１０１、観測角度=−０．６７５（ｒａｄ）
ランドマークＬＭ４：観測尤度 ０．２９３、観測角度=０．７６８（ｒａｄ）
ランドマークＬＭ５：観測尤度 ０．２９３、観測角度=０．８０５（ｒａｄ）
このとき、上記各ランドマークの観測尤度、観測角度に基づき式２０３を用いて、評価値Ｐ（ｐ、ｔ）を算出する。
【００６７】
視野内ランドマーク抽出部４５は、図８に示すｍ＝１〜５に対応するパンチルト角度にステレオカメラ５を設定した場合について、視野内ランドマークを検出する。例えば図１１は、ｍ＝１（ｐ＝０．５２（ｒａｄ）、ｔ＝０．００（ｒａｄ））とした場合を示している。このとき、視野内ランドマークは、ランドマークＬＭ２、ランドマークＬＭ３である。以下、視野内ランドマーク抽出部４５は、ｍ＝２〜５について、同様の処理を繰り返す。観測評価部４７は、図８に示すｍ＝１〜５に対応する評価値Ｐ（ｐ、ｔ）を算出する。
【００６８】
１）ｍ＝１のとき（ｐ＝０．５２（ｒａｄ）、ｔ＝０．００（ｒａｄ））
観測されるランドマーク：ランドマークＬＭ２、ＬＭ３
このとき、評価値Ｐ（０．５２、０）
＝α×（ρ（２）＋ρ（３））+β×標準偏差（−０．３８１、−０．６７５））
=０．５×０．３６１＋０．５×０．１４７
＝０．２５４
【００６９】
２）ｍ＝２のとき（ｐ＝０．２６（ｒａｄ）、ｔ＝０．００（ｒａｄ））
観測されるランドマーク：ランドマークＬＭ１、ＬＭ２
評価値Ｐ（０．２６、０）＝０．３６２
【００７０】
３）ｍ＝３のとき（ｐ＝０．０（ｒａｄ）、ｔ＝０．００（ｒａｄ））
観測されるランドマーク：ランドマークＬＭ１
評価値Ｐ（０．０、０．０）＝０．１２２
【００７１】
４）ｍ＝４のとき（ｐ＝−０．２６（ｒａｄ）、ｔ＝０．００（ｒａｄ））
観測されるランドマーク：ランドマークＬＭ１
評価値Ｐ（−０．２６、０）＝０．１２２
【００７２】
５）ｍ＝５のとき（ｐ＝−０．５２（ｒａｄ）、ｔ＝０．００（ｒａｄ））
観測されるランドマーク：ランドマークＬＭ４、ＬＭ５
評価値Ｐ（−０．５２、０）＝０．３０２
【００７３】
以上のように、観測評価部４７は、ｍ＝１〜５についてそれぞれ算出した評価値Ｐ（ｐ、ｔ）が最大となるパンチルト角度を判別する。上記の例では、ｍ＝２のときが最大である。よって求めるパンチルト角は、ｐ＝０.２６（ｒａｄ）、ｔ＝０．００（ｒａｄ）となる。カメラ制御部４９は、ステレオカメラ５を、求めたパンチルト角になるように制御するための信号をパンチルト制御部９に出力する。パンチルト制御部９は、求めたパンチルト角になるようにステレオカメラ５を設定して自律移動を継続する。
【００７４】
以上詳細に説明したように、第１の実施の形態によるロボット１の地図生成部３３は、利用する経路のランドマーク地図５５を生成する。このとき、ランドマークＬＭの識別情報ＩＤと、その３次元位置（ｘｌ（i）、ｙｌ（i）、ｚｌ（i））、および、ランドマークＬＭが観測された際のロボット１の代表位置姿勢（ｘｒ（ｊ）、ｙｒ（ｊ）、θｒ（ｊ））が算出される。
【００７５】
ロボット１の自律移動時には、移動位置予測部３７は所定時間後の自己位置を予測して予測位置を算出する。方向距離計算部４１は、各ランドマークＬＭｉから予測位置への方向ベクトルと、ランドマークＬＭｉから代表位置への方向ベクトルとの角度差φ（ｉ）、距離差δ（ｉ）を算出する。観測尤度計算部４３は、各ランドマークＬＭの観測尤度ρ（ｉ）を、角度差φ（ｉ）、距離差δ（ｉ）に基づいて算出する。視野内ランドマーク抽出部４５は、視野内のランドマークＬＭを抽出し、それぞれのランドマークＬＭの観測角度を抽出する。観測評価部４７は、視野内のランドマークＬＭの観測尤度ρ（ｉ）および観測角度の分散に基づき評価値Ｐ（ｐ、ｔ）を算出する。算出した評価値Ｐ（ｐ、ｔ）が最も大きいパン角度を、ステレオカメラ５の設定角度として設定し、その状態で撮影されるステレオ画像およびランドマーク地図５５に基づき自己位置を推定する。
【００７６】
以上のように、第１の実施の形態によるロボット１によれば、自律移動の際には、生成されたランドマーク地図５５と自律移動時に検出したランドマークＬＭとを照合して自己位置の推定を行うことができる。また、ランドマーク地図５５に登録した時のロボット１の代表位置姿勢と自律移動中の現在のロボット１の位置姿勢情報とから、個々のランドマークが観測できるかを示す観測尤度ρ（ｉ）とランドマークの観測角度の標準偏差（分散）との２つの指標を決定する。そして、これら組み合わせて評価値Ｐにより評価することで、いろいろな方向にあるランドマークをなるべく多く検出できるように、ステレオカメラ５の方向を決定することができる。よって、より正確に自己位置を推定可能なロボット１が提供される。
【００７７】
また、検出されるランドマークＬＭの数が多くても、ランドマークＬＭが固まって存在する場合には、自己位置推定精度は良くない。しかし、上記のように、第１の実施の形態においては、評価値Ｐ（ｐ、ｔ）として個々のランドマークＬＭが観測できるか否かを示す観測尤度ρ（ｉ）と、ランドマークＬＭの観測角度の標準偏差を組み合わせた指標を用いている。そして、近接したランドマークＬＭのみである場合には評価値Ｐ（ｐ、ｔ）を小さくようにすることにより、近接したランドマークＬＭが多いか否かの要因を評価することが可能となる。よって、ランドマークが近接して存在するケースを、自己位置推定精度が悪くなり、観測には適していないケースであると判断することが可能となる効果を生み出している。このように、多くのランドマークＬＭを観測可能とすることにより、ランドマークＬＭを見る位置が代表位置とは異なる場合にも、正確に自己位置推定を行うことが可能になる。
【００７８】
（第２の実施の形態）
以下、図１２および図１３を参照しながら、第２の実施の形態によるロボット３００について説明する。第２の実施の形態において、第１の実施の形態と同様の構成および動作については、同一の符号を付し、重複説明は省略する。
【００７９】
第２の実施の形態は、ランドマークＬＭが人等の障害物により遮蔽された場合の対応を含む例である。第２の実施の形態においては、検出した障害物がランドマークを遮蔽するか否かを評価することで、障害物による遮蔽を考慮してステレオカメラ５の方向を決定する。
【００８０】
図１２は、第２の実施の形態によるロボット３００の機能を示すブロック図である。ロボット３００は、ロボット１に加え、人障害物検出部５１および遮蔽評価部５３を備えている。図１２に示すように、人障害物検出部５１は、ステレオカメラ５の視野内にある障害物を検出し、検出した障害物を円柱モデルに近似し、位置を計算する。遮蔽評価部５３は、検出された障害物が、ランドマークＬＭを遮蔽しているか否か評価する。
【００８１】
以下、図１３を参照しながらロボット３００の処理を説明するが、図１３のフローチャートのＳ１５０〜Ｓ１６４は、第１の実施の形態による図７のフローチャートと同様であるので、説明を省略する。第２の実施の形態においては、ステレオカメラ５で撮影されたステレオ画像において、人障害物検出部５１が、人障害物の位置と速度をパターン識別と３次元計測とを用いて検出して、円柱モデルに近似する（Ｓ１７０）。
【００８２】
さらに、人障害物検出部５１は、現在から所定時間後のロボット３００の予測位置から見た人障害物の位置を、Ｓ１７０で検出した位置と速度から計算する（Ｓ１７１）。視野内ランドマーク抽出部４５がパンチルト角度を確定すると、遮蔽評価部５３は、検出した人障害物を近似した３次元モデルを、ステレオカメラ５の画像に投影する（Ｓ１７２）。
【００８３】
視野内ランドマーク抽出部４５が、ステレオカメラ５の視野内に入るランドマークＬＭのリストΩを作成すると、遮蔽評価部５３は、各ランドマークＬＭをステレオカメラ５の画像面に投影して遮蔽を判定し、遮蔽されていると判定されると、リストΩから削除する（Ｓ１７３）。
【００８４】
ロボット３００は、遮蔽されたランドマークＬＭが削除されたリストΩに基づき、評価値Ｐ（ｐ、ｔ）を算出することにより、最適なパンチルト角度ｐ、ｔを求め、ステレオカメラ５を制御する。
【００８５】
以上説明したように、第２の実施の形態によるロボット３００は、自律移動時に、ステレオカメラ５によるステレオ画像を用いてパターン認識と３次元計測を行って人障害物の位置とその速度を検出し、人障害物を円柱モデルで近似する。人障害物検出部５１は、現在から所定時間後の予測したロボット３００の位置における、ロボット３００から見た障害物の位置を、人障害物の位置と速度から計算する。
【００８６】
遮蔽評価部５３は、リストΩにある各パン角度とチルト角度との組み合わせに対して、人障害物を画像上に投影し、遮蔽されるランドマークＬＭを視野内ランドマークのリストΩから削除する。観測評価部４７は、遮蔽されたランドマークＬＭが削除されたリストΩに基づいて最適なパンチルト角を求め、カメラ制御部４９は、求めたパンチルト角にステレオカメラ５を制御するための信号を出力する。
【００８７】
以上説明したように、第２の実施の形態によるロボット３００によれば、第１の実施の形態によるロボット１による効果に加え、障害物によりランドマークが遮蔽されている場合にも、視野内にランドマークを多く観測できる注視方向の決定が可能となる。この結果、ランドマークが見えずに自己位置推定の精度が悪化するのを防止することで、自律移動のロバスト性が高まる。よって、障害物がある場合にも、正確な自己位置推定が可能となる。
【００８８】
上記第１および第２の実施の形態において、ロボット１は、自己位置推定装置の一例であり、ステレオカメラ５は、撮影部の一例であり、ランドマーク抽出部３１は、距離算出部、および検出部の一例である。地図生成部３３は、代表算出部の一例であり、方向距離計算部４１は、ランドマーク位置取得部の一例であり、観測評価部４７は、撮影角度変更部の一例であり、カメラ制御部４９は、撮影制御部の一例である。また、人障害物位置検出部５１は、障害物位置検出部の一例である。
【００８９】
なお、本発明は、以上に述べた実施の形態に限定されるものではなく、本発明の要旨を逸脱しない範囲内で種々の構成または実施形態を採ることができる。
例えば、上記第１または第２の実施の形態においては、ステレオカメラ５は、２つのカメラ５Ａ、５Ｂを有した構成としたが、１台のカメラと、距離計を備えるようにしてもよい。このとき、距離計としては、例えば２次元的にスキャンが可能なレーザによる距離計を用いることができる。上記第１および第２の実施の形態においては、チルト角度ｔはゼロとしたが、任意の角度に設定するようにしてもよい。また、ランドマークＬＭの数、パン角度ｐの種類は上記に限定されない。
【００９０】
上記第１および第２の実施の形態において、観測尤度と観測角度の分散とを組み合わせて評価する評価値の算出方法は一例であり、他の方法による評価でもよい。観測尤度は、予測位置におけるランドマークの観測確率を反映した値であればよく、算出方法は上記に限定されない。さらに、観測角度を評価するために標準偏差を用いたが、分散が評価できるものであればこれに限定されない。
【００９１】
ここで、上記第１および第２の実施の形態による自己位置推定処理の動作をコンピュータに行わせるために共通に適用されるコンピュータの例について説明する。図１４は、標準的なコンピュータのハードウエア構成の一例を示すブロック図である。図１４に示すように、コンピュータ４００は、Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓiｎｇ Ｕｎiｔ（ＣＰＵ）４０２、メモリ４０４、入力装置４０６、出力装置４０８、外部記憶装置４１２、媒体駆動装置４１４、ネットワーク接続装置等がバス４１０を介して接続されている。
【００９２】
ＣＰＵ４０２は、コンピュータ４００全体の動作を制御する演算処理装置である。メモリ４０４は、コンピュータ４００の動作を制御するプログラムを予め記憶したり、プログラムを実行する際に必要に応じて作業領域として使用したりするための記憶部である。メモリ４０４は、例えばＲａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ（ＲＡＭ）、Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ（ＲＯＭ）等である。入力装置４０６は、コンピュータの使用者により操作されると、その操作内容に対応付けられている使用者からの各種情報の入力を取得し、取得した入力情報をＣＰＵ４０２に送付する装置であり、例えばキーボード装置、マウス装置などである。出力装置４０８は、コンピュータ４００による処理結果を出力する装置であり、表示装置などが含まれる。例えば表示装置は、ＣＰＵ４０２により送付される表示データに応じてテキストや画像を表示する。
【００９３】
外部記憶装置４１２は、例えば、ハードディスクなどの記憶装置であり、ＣＰＵ４０２により実行される各種制御プログラムや、取得したデータ等を記憶しておく装置である。媒体駆動装置４１４は、可搬記録媒体４１６に書き込みおよび読み出しを行うための装置である。ＣＰＵ４０２は、可搬型記録媒体４１６に記録されている所定の制御プログラムを、記録媒体駆動装置４１４を介して読み出して実行することによって、各種の制御処理を行うようにすることもできる。可搬記録媒体４１６は、例えばＣｏｎｐａｃｔ Ｄiｓｃ（ＣＤ）−ＲＯＭ、Ｄiｇiｔａｌ Ｖｅｒｓａｔiｌｅ Ｄiｓｃ（ＤＶＤ）、Ｕｎiｖｅｒｓａｌ Ｓｅｒiａｌ Ｂｕｓ（ＵＳＢ）メモリ等である。
【００９４】
上記第１および第２の実施の形態による自己位置推定方法をコンピュータに実行させるプログラムは、例えば外部記憶装置４１２に記憶させる。ＣＰＵ４０２は、外部記憶装置４１２からプログラムを読み出し、コンピュータ４００に自己位置推定の動作を行なわせる。このとき、まず、自己位置推定の処理をＣＰＵ４０２に行わせるための制御プログラムを作成して外部記憶装置４１２に記憶させておく。そして、入力装置４０６から所定の指示をＣＰＵ４０２に与えて、この制御プログラムを外部記憶装置４１２から読み出させて実行させるようにする。また、このプログラムは、可搬記録媒体４１６に記憶するようにしてもよい。
【００９５】
以上の実施形態に関し、さらに以下の付記を開示する。
（付記１）
自己位置を推定する自己位置推定装置であって、
複数のランドマークの内の少なくとも一つのランドマークの画像を含む画像を撮影する画像取得部と、
前記自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出する距離算出部と、
前記少なくとも一つのランドマークの３次元位置を取得するランドマーク位置取得部と、
前記画像、前記距離、および前記少なくとも一つのランドマークの位置に基づき前記自己位置推定装置の自己位置を推定する自己位置推定部と、
前記自己位置推定部により推定された自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出する移動位置予測部と、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出する観測尤度計算部と、
前記予測位置において前記画像が撮影される際の撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出する視野内ランドマーク抽出部と、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影部の撮影角度を変更する撮影角度変更部と、
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御する撮影制御部と、
を有し、
前記自己位置推定部は、変更後の前記撮影角度で撮影された画像と前記ランドマーク位置取得部により取得される前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新することを特徴とする自己位置推定装置。
（付記２）
前記ランドマーク位置取得部が取得した前記位置に前記ランドマークが観測された際の前記自己位置推定装置の位置を代表して対応付けられた代表位置を取得し、
前記位置および前記代表位置に基づき、前記少なくとも一つのランドマークからそれぞれの前記代表位置への第１の観測方向と第１の観測距離とを計算するとともに、
推定された前記自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出し、それぞれの前記少なくとも一つのランドマークから前記予測位置への第２の観測方向と第２の観測距離とを計算する方向距離計算部、
をさらに有し、
前記観測尤度計算部は、前記第１の観測方向と前記第２の観測方向との角度差と、前記第１の観測距離と前記第２の観測距離との距離差とに基づき、前記視野内ランドマークの各々に関して観測尤度を算出することを特徴とする付記１に記載の自己位置推定装置。
（付記３）
取得された前記画像および算出された前記距離に基づき、前記少なくとも一つのランドマークの位置を検出する検出部と、
前記少なくとも一つのランドマークを検出した際の前記自己位置推定装置の位置を代表する代表位置を前記少なくともひとつのランドマークのそれぞれについて算出する代表算出部と、
をさらに有することを特徴とする付記２に記載の自己位置推定装置。
（付記４）
前記撮影角度変更部は、前記撮影角度において撮影された画像おける前記視野内ランドマークの前記観測尤度の総和と、前記観測角度の分散とに基づき前記評価値を算出することを特徴とする付記１から付記３のいずれかに記載の自己位置推定装置。
（付記５）
障害物の位置を検出する障害物位置検出部と、
前記予測位置において前記障害物が前記少なくとも一つのランドマークを遮蔽するか否かを判別する遮蔽評価部と、
前記遮蔽評価部が、前記障害物が前記少なくとも一つのランドマークを遮蔽すると判別すると、前記抽出部は、抽出した前記視野内ランドマークから遮蔽される前記ランドマークを削除することを特徴とする付記１から付記４のいずれかに記載の自己位置推定装置。
（付記６）
前記遮蔽評価部は、前記障害物をモデル化し、前記撮影角度で撮影された画像において、前記モデル化された障害物が前記少なくとも一つのランドマークを遮蔽するか否かを判別することを特徴とする付記４に記載の自己位置推定装置。
（付記７）
複数のランドマーク内の少なくとも一つのランドマークの画像を含む画像を取得し、
自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出し、
前記少なくとも一つのランドマークの位置を取得し、
前記取得された画像、前記算出された距離、および前記取得された前記少なくとも一つのランドマークの位置に基づき自己位置を推定し、
前記推定された自己位置に基づき移動時の将来の予測位置を算出し、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出し、
前記予測位置において画像が撮影される撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出し、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更し、
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御し、
変更後の前記撮影角度で撮影された画像と取得された前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新することを特徴とする自己位置推定方法。
（付記８）
前記少なくとも一つのランドマークが観測された際の前記自己位置推定装置の代表位置を取得し、
前記位置および前記代表位置に基づき、前記少なくとも一つのランドマークからそれぞれの前記代表位置への第１の観測方向と第１の観測距離とを計算するとともに、
推定された前記自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出し、それぞれの前記少なくとも一つのランドマークから前記予測位置への第２の観測方向と第２の観測距離とを計算し、
前記第１の観測方向と前記第２の観測方向との角度差と、前記第１の観測距離と前記第２の観測距離との距離差とに基づき、前記視野内ランドマークの各々に関して観測尤度を算出することを特徴とする付記７に記載の自己位置推定方法。
（付記９）
取得された前記画像および算出された前記距離に基づき、前記少なくとも一つのランドマークの位置を検出し、
前記少なくとも一つのランドマークを検出した際の前記自己位置推定装置の位置を代表する代表位置を前記少なくともひとつのランドマークのそれぞれについて算出する、
ことを特徴とする付記８に記載の自己位置推定方法。
（付記１０）
前記評価値は、前記視野内ランドマークの前記観測尤度の総和と、前記観測角度の分散とに基づき算出されることを特徴とする付記７から付記９のいずれかに記載の自己位置推定方法。
（付記１１）
障害物の位置を検出し、
前記予測位置において前記障害物が前記少なくとも一つのランドマークを遮蔽するか否かを判別し、
前記障害物が前記少なくとも一つのランドマークを遮蔽すると判別されると、抽出した前記視野内ランドマークから遮蔽される前記ランドマークを削除することを特徴とする付記６から付記９のいずれかに記載の自己位置推定方法。
（付記１２）
複数のランドマーク内の少なくとも一つのランドマークの画像を含む画像を取得し、
自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出し、
前記少なくとも一つのランドマークの位置を取得し、
前記取得された画像、前記算出された距離、および前記取得された前記少なくとも一つのランドマークの位置に基づき自己位置を推定し、
前記推定された自己位置に基づき移動時の将来の予測位置を算出し、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出し、
前記予測位置において画像が撮影される撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出し、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更し、
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御し、
変更後の前記撮影角度で撮影された画像と取得された前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新する処理をコンピュータに実行させることを特徴とするプログラム。
（付記１３）
前記少なくとも一つのランドマークが観測された際の前記自己位置推定装置の代表位置を取得し、
前記位置および前記代表位置に基づき、前記少なくとも一つのランドマークからそれぞれの前記代表位置への第１の観測方向と第１の観測距離とを計算するとともに、
推定された前記自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出し、それぞれの前記少なくとも一つのランドマークから前記予測位置への第２の観測方向と第２の観測距離とを計算し、
前記第１の観測方向と前記第２の観測方向との角度差と、前記第１の観測距離と前記第２の観測距離との距離差とに基づき、前記視野内ランドマークの各々に関して観測尤度を算出することを特徴とする付記１２に記載のプログラム。
（付記１４）
取得された前記画像および算出された前記距離に基づき、前記少なくとも一つのランドマークの位置を検出し、
前記少なくとも一つのランドマークを検出した際の前記自己位置推定装置の位置を代表する代表位置を前記少なくともひとつのランドマークのそれぞれについて算出することを特徴とする付記１３に記載のプログラム。
（付記１５）
前記評価値は、前記視野内ランドマークの前記観測尤度の総和と、前記観測角度の分散とに基づき算出されることを特徴とする付記１２から付記１４のいずれかに記載のプログラム。
【符号の説明】
【００９６】
１ ロボット
３ 演算処理装置
５ ステレオカメラ
７ パンチルトモータ
９ パンチルト制御部
１１ 車輪制御部
１３ 車輪モータ
１５ 記憶部
１７ 本体
１９ 車輪
３１ ランドマーク抽出部
３３ 地図生成部
３５ 自己位置推定部
３７ 移動位置予測部
３９ 移動経路計画部
４１ 方向距離計算部
４３ 観測尤度計算部
４５ 視野内ランドマーク抽出部
４７ 観測評価部
４９ カメラ制御部
５１ 人障害物検出部
５３ 遮蔽評価部
５５ ランドマーク地図
【特許請求の範囲】
【請求項１】
自己位置を推定する自己位置推定装置であって、
複数のランドマークの内の少なくとも一つのランドマークの画像を含む画像を撮影する撮影部と、
前記自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出する距離算出部と、
前記少なくとも一つのランドマークの位置を取得するランドマーク位置取得部と、
前記画像、前記距離、および前記少なくとも一つのランドマークの位置に基づき前記自己位置推定装置の自己位置を推定する自己位置推定部と、
前記自己位置推定部により推定された自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出する移動位置予測部と、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出する観測尤度計算部と、
前記予測位置において前記画像が撮影される際の撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出する視野内ランドマーク抽出部と、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影部の撮影角度を変更する撮影角度変更部と、
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御する撮影制御部と、
を有し、
前記自己位置推定部は、変更後の前記撮影角度で撮影された画像と前記ランドマーク位置取得部により取得される前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新することを特徴とする自己位置推定装置。
【請求項２】
前記ランドマーク位置取得部が取得した前記位置に前記ランドマークが観測された際の前記自己位置推定装置の位置を代表して対応付けられた代表位置を取得し、
前記位置および前記代表位置に基づき、前記少なくとも一つのランドマークからそれぞれの前記代表位置への第１の観測方向と第１の観測距離とを計算するとともに、
推定された前記自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出し、それぞれの前記少なくとも一つのランドマークから前記予測位置への第２の観測方向と第２の観測距離とを計算する方向距離計算部、
をさらに有し、
前記観測尤度計算部は、前記第１の観測方向と前記第２の観測方向との角度差と、前記第１の観測距離と前記第２の観測距離との距離差とに基づき、前記視野内ランドマークの各々に関して観測尤度を算出することを特徴とする請求項１に記載の自己位置推定装置。
【請求項３】
取得された前記画像および算出された前記距離に基づき、前記少なくとも一つのランドマークの位置を検出する検出部と、
前記少なくとも一つのランドマークを検出した際の前記自己位置推定装置の位置を代表する代表位置を前記少なくともひとつのランドマークのそれぞれについて算出する代表算出部と、
をさらに有することを特徴とする請求項２に記載の自己位置推定装置。
【請求項４】
前記撮影角度変更部は、前記撮影角度において撮影された画像おける前記視野内ランドマークの前記観測尤度の総和と、前記観測角度の分散とに基づき前記評価値を算出することを特徴とする請求項１から請求項３のいずれかに記載の自己位置推定装置。
【請求項５】
障害物の位置を検出する障害物位置検出部と、
前記予測位置において前記障害物が前記少なくとも一つのランドマークを遮蔽するか否かを判別する遮蔽評価部と、
前記遮蔽評価部が、前記障害物が前記少なくとも一つのランドマークを遮蔽すると判別すると、前記抽出部は、抽出した前記視野内ランドマークから遮蔽される前記ランドマークを削除することを特徴とする請求項１から請求項４のいずれかに記載の自己位置推定装置。
【請求項６】
前記遮蔽評価部は、前記障害物をモデル化し、前記撮影角度で撮影された画像において、前記モデル化された障害物が前記少なくとも一つのランドマークを遮蔽するか否かを判別することを特徴とする請求項４に記載の自己位置推定装置。
【請求項７】
複数のランドマーク内の少なくとも一つのランドマークの画像を含む画像を取得し、
自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出し、
前記少なくとも一つのランドマークの位置を取得し、
前記取得された画像、前記算出された距離、および前記取得された前記少なくとも一つのランドマークの位置に基づき自己位置を推定し、
前記推定された自己位置に基づき移動時の将来の予測位置を算出し、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出し、
前記予測位置において画像が撮影される撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出し、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更し
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御し、
変更後の前記撮影角度で撮影された画像と取得された前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新することを特徴とする自己位置推定方法。
【請求項８】
複数のランドマーク内の少なくとも一つのランドマークの画像を含む画像を取得し、
自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出し、
前記少なくとも一つのランドマークの位置を取得し、
前記取得された画像、前記算出された距離、および前記取得された前記少なくとも一つのランドマークの位置に基づき自己位置を推定し、
前記推定された自己位置に基づき移動時の将来の予測位置を算出し、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出し、
前記予測位置において画像が撮影される撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出し、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更し
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御し、
変更後の前記撮影角度で撮影された画像と取得された前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新する処理をコンピュータに実行させることを特徴とするプログラム。
【請求項１】
自己位置を推定する自己位置推定装置であって、
複数のランドマークの内の少なくとも一つのランドマークの画像を含む画像を撮影する撮影部と、
前記自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出する距離算出部と、
前記少なくとも一つのランドマークの位置を取得するランドマーク位置取得部と、
前記画像、前記距離、および前記少なくとも一つのランドマークの位置に基づき前記自己位置推定装置の自己位置を推定する自己位置推定部と、
前記自己位置推定部により推定された自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出する移動位置予測部と、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出する観測尤度計算部と、
前記予測位置において前記画像が撮影される際の撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出する視野内ランドマーク抽出部と、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影部の撮影角度を変更する撮影角度変更部と、
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御する撮影制御部と、
を有し、
前記自己位置推定部は、変更後の前記撮影角度で撮影された画像と前記ランドマーク位置取得部により取得される前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新することを特徴とする自己位置推定装置。
【請求項２】
前記ランドマーク位置取得部が取得した前記位置に前記ランドマークが観測された際の前記自己位置推定装置の位置を代表して対応付けられた代表位置を取得し、
前記位置および前記代表位置に基づき、前記少なくとも一つのランドマークからそれぞれの前記代表位置への第１の観測方向と第１の観測距離とを計算するとともに、
推定された前記自己位置に基づき前記自己位置推定装置の移動時の将来の予測位置を算出し、それぞれの前記少なくとも一つのランドマークから前記予測位置への第２の観測方向と第２の観測距離とを計算する方向距離計算部、
をさらに有し、
前記観測尤度計算部は、前記第１の観測方向と前記第２の観測方向との角度差と、前記第１の観測距離と前記第２の観測距離との距離差とに基づき、前記視野内ランドマークの各々に関して観測尤度を算出することを特徴とする請求項１に記載の自己位置推定装置。
【請求項３】
取得された前記画像および算出された前記距離に基づき、前記少なくとも一つのランドマークの位置を検出する検出部と、
前記少なくとも一つのランドマークを検出した際の前記自己位置推定装置の位置を代表する代表位置を前記少なくともひとつのランドマークのそれぞれについて算出する代表算出部と、
をさらに有することを特徴とする請求項２に記載の自己位置推定装置。
【請求項４】
前記撮影角度変更部は、前記撮影角度において撮影された画像おける前記視野内ランドマークの前記観測尤度の総和と、前記観測角度の分散とに基づき前記評価値を算出することを特徴とする請求項１から請求項３のいずれかに記載の自己位置推定装置。
【請求項５】
障害物の位置を検出する障害物位置検出部と、
前記予測位置において前記障害物が前記少なくとも一つのランドマークを遮蔽するか否かを判別する遮蔽評価部と、
前記遮蔽評価部が、前記障害物が前記少なくとも一つのランドマークを遮蔽すると判別すると、前記抽出部は、抽出した前記視野内ランドマークから遮蔽される前記ランドマークを削除することを特徴とする請求項１から請求項４のいずれかに記載の自己位置推定装置。
【請求項６】
前記遮蔽評価部は、前記障害物をモデル化し、前記撮影角度で撮影された画像において、前記モデル化された障害物が前記少なくとも一つのランドマークを遮蔽するか否かを判別することを特徴とする請求項４に記載の自己位置推定装置。
【請求項７】
複数のランドマーク内の少なくとも一つのランドマークの画像を含む画像を取得し、
自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出し、
前記少なくとも一つのランドマークの位置を取得し、
前記取得された画像、前記算出された距離、および前記取得された前記少なくとも一つのランドマークの位置に基づき自己位置を推定し、
前記推定された自己位置に基づき移動時の将来の予測位置を算出し、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出し、
前記予測位置において画像が撮影される撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出し、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更し
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御し、
変更後の前記撮影角度で撮影された画像と取得された前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新することを特徴とする自己位置推定方法。
【請求項８】
複数のランドマーク内の少なくとも一つのランドマークの画像を含む画像を取得し、
自己位置推定装置から前記少なくとも一つのランドマークまでの距離を算出し、
前記少なくとも一つのランドマークの位置を取得し、
前記取得された画像、前記算出された距離、および前記取得された前記少なくとも一つのランドマークの位置に基づき自己位置を推定し、
前記推定された自己位置に基づき移動時の将来の予測位置を算出し、
前記予測位置において前記少なくとも一つのランドマークが観測される観測尤度を算出し、
前記予測位置において画像が撮影される撮影角度に応じて観測される前記少なくとも一つのランドマークのうちの視野内ランドマークを抽出するとともに、それぞれの前記視野内ランドマークの観測角度を算出し、
抽出された前記視野内ランドマークの前記観測尤度と前記観測角度とに応じて前記撮影角度を変更し
変更後の前記撮影角度で前記画像が撮影されるように撮影を制御し、
変更後の前記撮影角度で撮影された画像と取得された前記少なくとも一つのランドマークの位置とに基づき前記自己位置推定装置の現在位置を更新する処理をコンピュータに実行させることを特徴とするプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１３−２５４０１（Ｐ２０１３−２５４０１Ａ）
【公開日】平成２５年２月４日（２０１３．２．４）
【国際特許分類】
【出願番号】特願２０１１−１５７０９９（Ｐ２０１１−１５７０９９）
【出願日】平成２３年７月１５日（２０１１．７．１５）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２３年度、独立行政法人新エネルギー・産業技術総合開発機構、「次世代ロボット知能化技術開発プロジェクト 移動知能（サービス産業分野）の開発 動的視覚認識に基づく移動知能モジュール群の研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
【公開日】平成２５年２月４日（２０１３．２．４）
【国際特許分類】
【出願日】平成２３年７月１５日（２０１１．７．１５）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２３年度、独立行政法人新エネルギー・産業技術総合開発機構、「次世代ロボット知能化技術開発プロジェクト 移動知能（サービス産業分野）の開発 動的視覚認識に基づく移動知能モジュール群の研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
[ Back to top ]