特許 7337617 推定装置、推定方法及びプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-08-25

(45)【発行日】2023-09-04

(54)【発明の名称】推定装置、推定方法及びプログラム

(51)【国際特許分類】

   G01C 21/30 20060101AFI20230828BHJP

   G01C 21/26 20060101ALI20230828BHJP

   G08G 1/0969 20060101ALI20230828BHJP

   G09B 29/10 20060101ALN20230828BHJP

【ＦＩ】

G01C21/30

G01C21/26 B

G08G1/0969

G09B29/10 A

【請求項の数】 9

(21)【出願番号】P 2019168061

(22)【出願日】2019-09-17

(65)【公開番号】P2021047024

(43)【公開日】2021-03-25

【審査請求日】2022-03-14

(73)【特許権者】

【識別番号】000003078

【氏名又は名称】株式会社東芝

(73)【特許権者】

【識別番号】317011920

【氏名又は名称】東芝デバイス＆ストレージ株式会社

(74)【代理人】

【識別番号】110002147

【氏名又は名称】弁理士法人酒井国際特許事務所

(72)【発明者】

【氏名】杉浦 貴行

(72)【発明者】

【氏名】渡辺 友樹

(72)【発明者】

【氏名】関 晃仁

【審査官】宮本 礼子

(56)【参考文献】

【文献】特開２００７－３２２１３８（ＪＰ，Ａ）

【文献】特開２０１５－０３６８４０（ＪＰ，Ａ）

【文献】特表２０１１－５１１２８１（ＪＰ，Ａ）

【文献】特開２０１８－１１６０１４（ＪＰ，Ａ）

【文献】特開２０１８－１８５２３９（ＪＰ，Ａ）

【文献】特開平０５－０６６７１３（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｃ ２１／００－２５／００

Ｇ０８Ｇ １／００－９９／００

Ｇ０９Ｂ ２９／１０

(57)【特許請求の範囲】

【請求項1】

第１のランドマークの地図中の位置及び姿勢の少なくとも一方を示す第１の位置情報を記憶する記憶部と、
移動体の周辺の第２のランドマークの位置及び姿勢の少なくとも一方を示す第２の位置情報と、前記移動体の時系列の相対位置及び姿勢の少なくとも一方を示す相対位置情報と、を取得する取得部と、
前記第１の位置情報と前記第２の位置情報とに基づいて、前記移動体の地図中の位置及び姿勢の少なくとも一方を示す第３の位置情報、及び、前記第１のランドマークと前記第２のランドマークとの対応付けを推定する推定部と、を備え、
前記推定部は、前記第３の位置情報により特定される位置又は姿勢の少なくとも一方における前記第２のランドマークの観測誤差、前記第２のランドマークに対応付けられる前記第１のランドマークと前記対応付けの誤差とを示す対応付け誤差、及び、時系列の前記第３の位置情報により特定される前記移動体の相対位置及び姿勢の少なくとも一方と前記相対位置情報に含まれる相対位置及び姿勢の少なくとも一方との誤差を示す相対位置誤差がより小さくなるようにして、前記第３の位置情報と前記対応付けとを推定する、
推定装置。

【請求項2】

前記第２のランドマーク毎に、前記第２のランドマークの特徴に基づいて、対応付けの対象とする前記第１のランドマークを限定する限定部を更に備え、
前記推定部は、前記限定部によって限定された第１のランドマークと、前記第２のランドマークとの対応付けを推定する、
請求項１に記載の推定装置。

【請求項3】

前記限定部は、前記第２のランドマークの位置、姿勢、形状、種類及び状態の少なくとも１つに基づいて、対応付けの対象とする前記第１のランドマークを限定する、
請求項２に記載の推定装置。

【請求項4】

前記記憶部は、前記第１の位置情報を含む地図データを複数、記憶し、
前記限定部は、前記地図データ毎に、対応付けの対象とする前記第１のランドマークを限定し、
前記推定部は、前記限定部によって限定された第１のランドマークと前記第２のランドマークとの対応付け、及び、前記第３の位置情報を、前記地図データ毎に推定する、
請求項２に記載の推定装置。

【請求項5】

前記相対位置情報のスケールは、前記第１の位置情報を含む地図データのスケールとは異なり、
前記推定部は、前記相対位置情報のスケールを、前記地図データのスケールに変化させてから、前記相対位置誤差を算出する、
請求項１に記載の推定装置。

【請求項6】

前記推定部は、前記相対位置誤差を算出するときのスケールを、前記移動体の各時刻の位置及び姿勢の少なくとも一方毎に算出する、
請求項５に記載の推定装置。

【請求項7】

前記推定部は、前記第２のランドマークと、前記第１のランドマークとの対応付けを１対１にして、前記対応付けを推定する、
請求項１に記載の推定装置。

【請求項8】

第１のランドマークの地図中の位置及び姿勢の少なくとも一方を示す第１の位置情報を記憶するステップと、
移動体の周辺の第２のランドマークの位置及び姿勢の少なくとも一方を示す第２の位置情報と、前記移動体の時系列の相対位置及び姿勢の少なくとも一方を示す相対位置情報と、を取得するステップと、
前記第１の位置情報と前記第２の位置情報とに基づいて、前記移動体の地図中の位置及び姿勢の少なくとも一方を示す第３の位置情報、及び、前記第１のランドマークと前記第２のランドマークとの対応付けを推定するステップと、を含み、
前記推定するステップは、前記第３の位置情報により特定される位置又は姿勢の少なくとも一方における前記第２のランドマークの観測誤差、及び、前記第２のランドマークに対応付けられる前記第１のランドマークと前記対応付けの誤差とを示す対応付け誤差、及び、時系列の前記第３の位置情報により特定される前記移動体の相対位置及び姿勢の少なくとも一方と前記相対位置情報に含まれる相対位置及び姿勢の少なくとも一方との誤差を示す相対位置誤差がより小さくなるようにして、前記第３の位置情報と前記対応付けとを推定する、
推定方法。

【請求項9】

コンピュータを、
第１のランドマークの地図中の位置及び姿勢の少なくとも一方を示す第１の位置情報を記憶する記憶部と、
移動体の周辺の第２のランドマークの位置及び姿勢の少なくとも一方を示す第２の位置情報と、前記移動体の時系列の相対位置及び姿勢の少なくとも一方を示す相対位置情報と、を取得する取得部と、
前記第１の位置情報と前記第２の位置情報とに基づいて、前記移動体の地図中の位置及び姿勢の少なくとも一方を示す第３の位置情報、及び、前記第１のランドマークと前記第２のランドマークとの対応付けを推定する推定部として機能させ、
前記推定部は、前記第３の位置情報により特定される位置又は姿勢の少なくとも一方における前記第２のランドマークの観測誤差、及び、前記第２のランドマークに対応付けられる前記第１のランドマークと前記対応付けの誤差とを示す対応付け誤差、及び、時系列の前記第３の位置情報により特定される前記移動体の相対位置及び姿勢の少なくとも一方と前記相対位置情報に含まれる相対位置及び姿勢の少なくとも一方との誤差を示す相対位置誤差がより小さくなるようにして、前記第３の位置情報と前記対応付けとを推定する、
プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明の実施形態は推定装置、推定方法及びプログラムに関する。

【背景技術】

【0002】

自動運転や運転者支援では、自車両の地図中の自己位置を推定することで、地図に紐付いた情報を活用した高度な判断や、自車両の運動推定の高精度化が期待されている。ロボット分野においても地図中の自己位置はあらかじめ設定した移動経路に対する追従走行に利用することが出来る。移動体に搭載されたセンサで観測したランドマークと、地図データベースにあらかじめ登録されている位置が既知のランドマークとを照合し、ランドマークの観測に矛盾が少ないセンサの位置情報を利用して、地図中の自己位置を推定する技術が従来から知られている。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１８－１８５２３９号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

しかしながら、従来の技術では、ランドマークの照合に誤対応が起こり得る状況では、推定処理の精度を向上させることが難しかった。

【課題を解決するための手段】

【0005】

実施形態の推定装置は、記憶部と取得部と推定部とを備える。記憶部は、第１のランドマークの地図中の位置及び姿勢の少なくとも一方を示す第１の位置情報を記憶する。取得部は、移動体の周辺の第２のランドマークの位置及び姿勢の少なくとも一方を示す第２の位置情報と、前記移動体の時系列の相対位置及び姿勢の少なくとも一方を示す相対位置情報と、を取得する。推定部は、前記第１の位置情報と前記第２の位置情報とに基づいて、前記移動体の地図中の位置及び姿勢の少なくとも一方を示す第３の位置情報、及び、前記第１のランドマークと前記第２のランドマークとの対応付けを推定する。前記推定部は、前記第３の位置情報により特定される位置又は姿勢の少なくとも一方における前記第２のランドマークの観測誤差、及び、前記第２のランドマークに対応付けられる前記第１のランドマークと前記対応付けの誤差とを示す対応付け誤差、及び、時系列の前記第３の位置情報により特定される前記移動体の相対位置及び姿勢の少なくとも一方と前記相対位置情報に含まれる相対位置及び姿勢の少なくとも一方との誤差を示す相対位置誤差がより小さくなるようにして、前記第３の位置情報と前記対応付けとを推定する。

【図面の簡単な説明】

【0006】

【図1】第１実施形態の移動体の例を示す図。

【図2】第１実施形態の移動体の機能構成の例を示す図。

【図3】第１実施形態の第１の位置情報の例を示す図。

【図4A】第１実施形態の推定部の動作例を説明するための図。

【図4B】図４Ａの対応付けに対応する重み行列の例を示す図。

【図5A】第１実施形態の推定部の推定結果を説明するための図。

【図5B】図５Ａの推定結果に対応する重み行列の例を示す図。

【図6】第１実施形態の推定方法の例を示すフローチャート。

【図7】第２実施形態の移動体の機能構成の例を示す図。

【図8】第２実施形態の第１の位置情報の例を示す図。

【図9A】第２実施形態の限定部により限定された第１のランドマークの例１を示す図。

【図9B】第２実施形態の限定部により限定された第１のランドマークの例２を示す図。

【図9C】第２実施形態の限定部により限定された第１のランドマークの例３を示す図。

【図10A】図９Ａ乃至９Ｃの対応付けに対応する重み行列の例を示す図。

【図10B】第２実施形態の推定部の推定結果に対応する重み行列の例を示す図。

【図11】第２実施形態の推定方法の例を示すフローチャート。

【図12】第３実施形態の推定方法の例を示すフローチャート。

【図13】第１乃至第３実施形態の推定装置のハードウェア構成の例を示す図。

【発明を実施するための形態】

【0007】

以下、添付図面を参照して実施形態の推定装置、推定方法及びプログラムについて説明する。

【0008】

（第１実施形態）
第１実施形態の推定装置は、例えば移動体に搭載される。

【0009】

［移動体の例］
図１は第１実施形態の移動体１０の例を示す図である。

【0010】

移動体１０は、推定装置２０、出力部１０Ａ、カメラ１０Ｂ、センサ１０Ｃ、動力制御部１０Ｇ及び動力部１０Ｈを備える。

【0011】

移動体１０は任意でよい。移動体１０は、例えば車両、台車及び移動ロボット等である。車両は、例えば自動二輪車、自動四輪車及び自転車等である。また、移動体１０は、例えば人による運転操作を介して走行する移動体でもよいし、人による運転操作を介さずに自動的に走行（自律走行）可能な移動体でもよい。

【0012】

推定装置２０は、例えば専用又は汎用のコンピュータハードウェアにより実現される。推定装置２０は、移動体１０の位置及び姿勢の少なくとも一方を示す自己位置を推定する。具体的には、推定装置２０は、移動体１０の自己位置を時系列に推定するときに同時に、カメラ１０Ｂで観測されたランドマーク（第２のランドマーク）と、地図データに登録されているランドマーク（第１のランドマーク）との対応付けを照合する。これによりランドマークの対応付けにロバストな自己位置推定を可能とする。

【0013】

なお、推定装置２０は、移動体１０に搭載された形態に限定されない。推定装置２０は、静止物に搭載されていてもよい。静止物は、例えば地面に固定された物等の移動不可能な物である。地面に固定された静止物は、例えばガードレール、ポール、駐車車両及び道路標識等である。また例えば、静止物は、地面に対して静止した状態の物である。また、推定装置２０は、クラウドシステム上で処理を実行するクラウドサーバに搭載されていてもよい。

【0014】

動力部１０Ｈは、移動体１０に搭載された駆動デバイスである。動力部１０Ｈは、例えば、エンジン、モータ及び車輪等である。

【0015】

動力制御部１０Ｇは、動力部１０Ｈを制御する。動力制御部１０Ｇの制御によって動力部１０Ｈが駆動する。

【0016】

出力部１０Ａは情報を出力する。第１実施形態では、出力部１０Ａは、推定装置２０により推定されたカメラ１０Ｂの運動の推定結果を示す推定結果情報を出力する。

【0017】

出力部１０Ａは、例えば、推定結果情報を送信する通信機能、推定結果情報を表示する表示機能、及び、推定結果情報を示す音を出力する音出力機能等を備える。出力部１０Ａは、例えば通信部１０Ｄ、ディスプレイ１０Ｅ及びスピーカ１０Ｆの少なくとも１つを備える。なお、第１実施形態では、出力部１０Ａは、通信部１０Ｄ、ディスプレイ１０Ｅ及びスピーカ１０Ｆを備えた構成を例にして説明する。

【0018】

通信部１０Ｄは、推定結果情報を他の装置へ送信する。例えば、通信部１０Ｄは、通信回線を介して推定結果情報を他の装置へ送信する。ディスプレイ１０Ｅは、推定結果に関する情報を表示する。ディスプレイ１０Ｅは、例えばＬＣＤ（Ｌｉｑｕｉｄ Ｃｒｙｓｔａｌ Ｄｉｓｐｌａｙ）、投影装置及びライト等である。スピーカ１０Ｆは、推定結果に関する情報を示す音を出力する。

【0019】

カメラ１０Ｂは、例えば単眼カメラ、ステレオカメラ、魚眼カメラ及び赤外線カメラ等である。カメラ１０Ｂの数は任意でよい。また、撮像される画像はＲＧＢの３チャネルで構成されたカラー画像であってもよく、グレースケールで表現された１チャネルのモノクロ画像であってもよい。カメラ１０Ｂは、移動体１０周辺の時系列の画像を撮像する。カメラ１０Ｂは、例えば移動体１０の周辺を時系列に撮像することにより、時系列の画像を撮像する。移動体１０の周辺は、例えば当該移動体１０から予め定められた範囲内の領域である。この範囲は、例えばカメラ１０Ｂの撮像可能な範囲である。

【0020】

第１実施形態では、カメラ１０Ｂが、移動体１０の前方を撮影方向として含むように設置されている場合を例にして説明する。すなわち、第１実施形態では、カメラ１０Ｂは、移動体１０の前方を時系列に撮像する。

【0021】

センサ１０Ｃは、測定情報を測定するセンサである。測定情報は、例えば移動体１０の速度、並びに、移動体１０のハンドルの舵角を含む。センサ１０Ｃは、例えば慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）、速度センサ及び舵角センサ等である。ＩＭＵは、移動体１０の三軸加速度及び三軸角速度を含む測定情報を測定する。速度センサは、タイヤの回転量から速度を測定する。舵角センサは、移動体１０のハンドルの舵角を測定する。また例えば、センサ１０Ｃは、ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ、Ｌａｓｅｒ Ｉｍａｇｉｎｇ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）のように物体までの距離を計測する奥行距離センサである。

【0022】

次に、第１実施形態の移動体１０の機能構成の例について詳細に説明する。

【0023】

［機能構成の例］
図２は第１実施形態の移動体１０の機能構成の例を示す図である。

【0024】

移動体１０は、推定装置２０、出力部１０Ａ、カメラ１０Ｂ、センサ１０Ｃ、動力制御部１０Ｇ及び動力部１０Ｈを備える。推定装置２０は、処理部２０Ａ及び記憶部２０Ｂを備える。出力部１０Ａは、通信部１０Ｄ、ディスプレイ１０Ｅ及びスピーカ１０Ｆを備える。

【0025】

処理部２０Ａ、記憶部２０Ｂ、出力部１０Ａ、カメラ１０Ｂ、センサ１０Ｃ及び動力制御部１０Ｇは、バス１０Ｉを介して接続されている。動力部１０Ｈは、動力制御部１０Ｇに接続されている。

【0026】

なお、出力部１０Ａ（通信部１０Ｄ、ディスプレイ１０Ｅ及びスピーカ１０Ｆ）、カメラ１０Ｂ、センサ１０Ｃ、動力制御部１０Ｇ及び記憶部２０Ｂは、ネットワークを介して接続されていてもよい。接続に使用されるネットワークの通信方式は、有線方式であっても無線方式であってもよい。また、接続に使用されるネットワークは、有線方式と無線方式とを組み合わせることにより実現されていてもよい。

【0027】

記憶部２０Ｂは、例えば半導体メモリ素子、ハードディスク及び光ディスク等である。半導体メモリ素子は、例えばＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）及びフラッシュメモリ等である。なお、記憶部２０Ｂは、推定装置２０の外部に設けられた記憶装置であってもよい。また、記憶部２０Ｂは、記憶媒体であってもよい。具体的には、記憶媒体は、プログラムや各種情報を、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）やインターネットなどを介してダウンロードして記憶または一時記憶したものであってもよい。また、記憶部２０Ｂを、複数の記憶媒体から構成してもよい。

【0028】

記憶部２０Ｂは、例えば第１のランドマークの地図中の位置及び姿勢の少なくとも一方を示す第１の位置情報を記憶する。第１の位置情報は、例えば地図中の位置を示す地図データベースによって記憶される。第１のランドマークの位置は、地図中の点で表してもよいし、複数点の集合、矩形、及び、既知の形状モデル等の任意の形態で表してもよい。

【0029】

図３は第１実施形態の第１の位置情報の例を示す図である。図３の第１の位置情報の例は、第１のランドマーク１０１ａ及び１０１ｂの地図中の位置を示す。以下、第１のランドマーク１０１ａ及び１０１ｂを区別しない場合、単に第１のランドマーク１０１という。

【0030】

第１のランドマーク１０１は、地図データベースの座標系において位置を登録することが可能であり、カメラ１０Ｂ及びセンサ１０Ｃで観測される際にも、移動せず同じ位置に存在する対象であることが好ましい。第１のランドマーク１０１は、例えば道路信号、交通標識、道路面標示及びポール等の道路構造物、店舗の看板のような路面に対して固定されている対象が好ましい。また、本来は移動可能な対象であっても、地図データベース生成時と観測時との間には移動していないことが事前に保証されている、あるいは移動していないことが検証可能であれば第１のランドマーク１０１として用いてもよい。また、ユーザが環境に設置することが可能であれば特定のマーカを、第１のランドマーク１０１として用いてもよい。

【0031】

第１の位置情報は、緯度経度により絶対座標系で定義されていてもよいし、特定の位置を座標の基準とした任意の座標系で定義されていてもよい。

【0032】

図２に戻り、処理部２０Ａは、取得部２１及び推定部２２を備える。

【0033】

取得部２１は、移動体１０の周辺の第２のランドマークの位置及び姿勢の少なくとも一方を示す第２の位置情報を取得する。取得部２１は、例えばカメラ１０Ｂによって撮像された画像から第２の位置情報を取得する。また例えば、取得部２１は、センサ１０Ｃによって取得されたデータから第２の位置情報を取得する。

【0034】

推定部２２は、取得部２１から第２の位置情報を受け付け、記憶部２０Ｂから第１の位置情報を読み出す。そして、推定部２２は、第１の位置情報と第２の位置情報とに基づいて、移動体１０の地図中の位置及び姿勢の少なくとも一方を示す第３の位置情報、及び、第１のランドマークと第２のランドマークとの対応付けを推定する。推定部２２の処理の詳細は後述する。

【0035】

処理部２０Ａは、例えばＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等のプロセッサにプログラムを実行させること、すなわちソフトウェアにより実現されてもよい。また例えば、処理部２０Ａは、専用のＩＣ（Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等の１又は複数のプロセッサ、すなわちハードウェアにより実現されてもよい。また例えば、処理部２０Ａは、ソフトウェア及びハードウェアを併用することにより実現されてもよい。

【0036】

なお、実施形態において用いられる「プロセッサ」の文言は、例えば、ＣＰＵ、ＧＰＵ（Ｇｒａｐｈｉｃａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、特定用途向け集積回路（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ：ＡＳＩＣ）、及び、プログラマブル論理デバイスを含む。プログラマブル論理デバイスは、例えば単純プログラマブル論理デバイス（Ｓｉｍｐｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ：ＳＰＬＤ）、複合プログラマブル論理デバイス（Ｃｏｍｐｌｅｘ Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ：ＣＰＬＤ）、及び、フィールドプログラマブルゲートアレイ（Ｆｉｅｌｄ Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ：ＦＰＧＡ）等を含む。

【0037】

プロセッサは、記憶部２０Ｂに保存されたプログラムを読み出し実行することで、処理部２０Ａを実現する。なお、記憶部２０Ｂにプログラムを保存する代わりに、プロセッサの回路内にプログラムを直接組み込むよう構成してもよい。この場合、プロセッサは回路内に組み込まれたプログラムを読み出し実行することで、処理部２０Ａを実現する。

【0038】

なお、図１０に示す移動体１０の機能の一部は、他の装置に備えられていてもよい。例えば、カメラ１０Ｂ及びセンサ１０Ｃ等が移動体１０に搭載され、推定装置２０は移動体１０の外部に設置されたサーバ装置として動作させてもよい。この場合、通信部１０Ｄが、カメラ１０Ｂ及びセンサ１０Ｃ等によって観測されたデータをサーバ装置に送信する。

【0039】

［推定部の動作例］
次に、推定部２１の動作の詳細について説明する。

【0040】

図４Ａは第１実施形態の推定部２１の動作例を説明するための図である。図４Ａの例では、第１のランドマーク１０１ａ及び１０１ｂと、第２のランドマーク１０２ａ及び１０２ｂとがある場合について説明する。第２のランドマーク１０２ａ及び１０２ｂが、観測位置２０１ａ及び２０１ｂで観測された場合を例にして説明する。

【0041】

まず、移動体１０に搭載されたカメラ１０Ｂが、観測位置２０１ａ及び２０１ｂで、第２のランドマーク１０２ａ及び１０２ｂを観測する。第２のランドマーク１０２ａ及び１０２ｂの第２の位置情報は、移動体１０に定義された局所的な座標系（画像座標）で表される。以下、第２のランドマーク１０２ａ及び１０２ｂを区別しない場合は、単に第２のランドマーク１０２という。

【0042】

ここでセンサとして、地図データベースに第１のランドマーク１０１として保持されている対象を、第２のランドマーク１０２として観測できれば、任意のセンサが用いられてよい。上述のようにカメラ１０Ｂを用いた画像を取得してもよいし、ＬｉＤＡＲのように物体までの距離を計測するセンサ１０Ｃを用いてもよい。

【0043】

センサデータから第２のランドマーク１０２を検出する方法は任意でよい。センサデータが画像である場合は、例えば特徴点検出方法や、ＣＮＮ（Ｃｏｎｖｏｌｕｔｉｏｎａｌ Ｎｅｕｒａｌ Ｎｅｔｗｏｒｋ）による特定物体認識を用いればよい。

【0044】

第２のランドマーク１０２の位置関係は、例えば画像におけるピクセル位置で表現される。また例えば、第２のランドマーク１０２の位置関係は、奥行距離センサにおける方向と距離のように、移動体１０の位置姿勢に対する第２のランドマーク１０２の観測方向で表現される。

【0045】

また、第１実施形態では、センサデータに定義された座標の点によって第２のランドマーク１０２を観測するが、第２のランドマーク１０２を複数点の集合、矩形、及び、既知の形状モデル等により観測してもよい。第１の位置情報と照合できる形態であり、後述する観測誤差Ｅ_ｏｂｓ、及び、対応付け誤差Ｅ_{ｍａｔｃｈ}を算出できれば、第２のランドマーク１０２を表す形態は任意でよい。

【0046】

推定部２２は、第１のランドマーク１０１の第１の位置情報と、第２のランドマーク１０２の第２の位置情報とに基づいて、移動体１０の地図中の位置及び姿勢の少なくとも一方を示す第３の位置情報、並びに、第１のランドマーク１０１及び第２のランドマーク１０２の対応付けを推定する。

【0047】

具体的には、推定部２２は、例えば下記式（１）を用いて、第２のランドマーク１０２の地図中の位置を示す３次元点Ｌに関して、移動体１０の地図中での位置姿勢Ｐを用いた第２のランドマーク１０２の観測情報Ｖとの観測誤差Ｅ_ｏｂｓと、第１のランドマーク１０１を含む地図データＭとの対応付け誤差Ｅ_{ｍａｔｃｈ}の和Ｅをより小さくする。

【0048】

Ｅ（Ｐ，Ｌ，Ｗ│Ｍ，Ｖ）＝Ｅ_ｏｂｓ（Ｐ，Ｌ│Ｖ）
＋Ｅ_{ｍａｔｃｈ}（Ｌ，Ｗ｜Ｍ）・・・（１）

【0049】

ここで、観測情報Ｖは、カメラ１０Ｂにより観測された画像の座標等によって表された第２の位置情報である。Ｗは第１のランドマーク１０１と第２のランドマーク１０２との対応付けを示す変数である。

【0050】

観測誤差Ｅ_ｏｂｓは、推定された移動体１０の位置姿勢において、推定された地図中の位置に基づいて第２のランドマーク１０２を再度観測したときの観測情報ｖと、カメラ１０Ｂによって実際に観測された観測情報Ｖとの差を表す。例えば、推定部２２は、第２のランドマーク１０２の地図中における３次元点Ｌを、カメラモデルを用いて２次元画像面に投影した場合の投影位置と、画像から検出された第２のランドマーク１０２の検出位置（第２の位置情報）との誤差である再投影誤差によって観測誤差Ｅ_ｏｂｓを評価し、当該再投影誤差をより小さくする（最適化する）。

【0051】

なお、観測誤差Ｅ_ｏｂｓは、再投影誤差に限らず、推定された移動体１０の位置姿勢と第２のランドマーク１０２の位置とに基づいて算出された観測情報ｖと、実際の観測情報Ｖとの誤差が評価できればよい。例えば観測誤差Ｅ_ｏｂｓは、第２のランドマーク１０２の方向を示すベクトルのなす角度で評価されてもよいし，投影した形状モデルの重なり具合に基づく一致度で評価されてもよい。

【0052】

対応付け誤差Ｅ_{ｍａｔｃｈ}は、第２のランドマーク１０２各々に、どの第１のランドマーク１０１を対応付けるか、及び対応付けられた第１のランドマーク１０１との差を表す。例えば、推定部２２は、対応付けＷを、第２のランドマークの各々について、地図データベースに含まれる第１のランドマーク１０１への対応付けの重みで表現される重み行列とする。

【0053】

図４Ｂは、図４Ａの対応付けに対応する重み行列の例を示す図である。図４Ｂの例では、全ての組み合わせで同じ重み０．５が使用されている。推定部２２は、地図データベースに登録されている第１のランドマーク１０１の地図中の位置を重み行列で合成した線形和による位置と、第２のランドマーク１０２の位置間の距離を対応付け誤差Ｅ_{ｍａｔｃｈ}として評価し、当該距離をより小さくする（最適化する）。

【0054】

ここで距離を評価する位置とは、地図データベースに登録されている位置から算出できればよく、３次元点、特定の平面に投影した２次元点、または経緯度表現でもよい。誤差を求める位置を、第１のランドマーク１０１の位置の線形和で算出したが、第２のランドマーク１０２に対応付けた第１のランドマーク１０１の位置が参照できればよい。例えばＳｏｆｔｍａｘ等で特定の第１のランドマーク１０１に重みを付けてもよいし、非線形な関数を用いて第１のランドマーク１０１に重みを付けてもよい。

【0055】

第１実施形態では、対応付けの重みによっては、第２のランドマーク１０２が複数の第１のランドマーク１０１に対応付く場合もあるが、１つの第２のランドマーク１０２に対して１つの第１のランドマーク１０１を対応付けるように拘束を加える構成であってもよい。すなわち、推定部２２が、第２のランドマーク１０２と、第１のランドマーク１０１との対応付けを１対１にして、対応付けを推定してもよい。

【0056】

また、誤差には二つの位置間の距離を用いたが、形状モデルがある場合はモデル同士の一致度を用いてもよいし、対応付けられた第１のランドマーク１０１と第２のランドマーク１０２との一致度を評価できれば任意の形式で表された誤差を用いてよい。

【0057】

また、対応付けの変数Ｗを重み行列として表現する形態を示したが、第１のランドマーク１０１及び第２のランドマーク１０２同士の対応付けが表現できれば、対応付けの変数Ｗは重み行列以外の形態でもよい。例えば、対応付けられた第１のランドマーク１０１と第２のランドマーク１０２のペアを、重複を許して保持する形態でもよい。誤差の最適化方法については、例えばＬｅｖｅｎｂｅｒｇ－Ｍａｒｑｕａｒｄｔ法のような方法を用いればよい。

【0058】

図５Ａは第１実施形態の推定部２２の推定結果を説明するための図である。図５Ａでは、第２のランドマーク１０２をカメラ１０Ｂに再投影したときの観測誤差Ｅ_ｏｂｓ、及び、第１のランドマーク１０１と第２のランドマーク１０２との対応付け誤差Ｅ_{ｍａｔｃｈ}をより小さくすることで得られた推定結果を表す。

【0059】

図５Ｂは、図５Ａの推定結果に対応する重み行列の例を示す図である。図５Ｂの例では、第１のランドマーク１０１ａと第２のランドマーク１０２ａとが、対応付けの重み１によって対応付けられる。また、第１のランドマーク１０１ｂと第２のランドマーク１０２ｂとが、対応付けの重み１によって対応付けられる。第１のランドマーク１０１ａと第２のランドマーク１０２ｂとの対応付け、及び、第１のランドマーク１０１ｂと第２のランドマーク１０２ａとの対応付けは、Ｅ_ｏｂｓを合わせて小さくすることで観測との位置関係により棄却される（重み０）。

【0060】

図４Ａ及び５Ａの説明では、簡単のため平面上における２次元での動作の例を示したが、３次元空間で最適化を行ってもよい。また、地図データは高さ一定の平面で、位置姿勢は３次元のように各座標系で最適化を行ってもよい。

【0061】

また、１台のカメラ１０Ｂによる複数箇所での計測を用いることにより、第２のランドマーク１０２の３次元位置を推定する方法を説明したが、複数台のカメラ１０Ｂが移動体１０に搭載されていてもよい。また、１つのカメラ１０Ｂによって撮像された画像から直接奥行を推定してもよいし、奥行距離センサ等のセンサ１０Ｃにより、奥行きの計測をしてもよい。推定部２２は、奥行を含む観測情報Ｖを使用して、観測誤差Ｅ_ｏｂｓを求める方式を使用してもよい。この場合は観測位置２０１が少なくとも１か所あればよい。

【0062】

［推定方法の例］
図６は第１実施形態の推定方法の例を示すフローチャートである。はじめに、カメラ１０Ｂが、第２のランドマーク１０２を含む画像を撮像することによって、第２のランドマーク１０２を観測する（ステップＳ１）。次に、取得部２１が、ステップＳ１の処理によって撮像された画像から、上述の第２の位置情報を取得する（ステップＳ２）。次に、推定部２２が、記憶部２０Ｂから、第１のランドマーク１０１の第１の位置情報を読み出す（ステップＳ３）。なお、図６の例は一例であり、ステップＳ３の処理は、例えばステップＳ１及びステップＳ２の処理と並列で実施されてもよい。

【0063】

次に、推定部２２が、上述の式（１）によって、観測誤差Ｅ_ｏｂｓ及び対応付け誤差Ｅ_{ｍａｔｃｈ}をより小さくすることによって、上述の第３の位置情報と、第１のランドマーク１０１及び第２のランドマーク１０２の対応付けとを推定する（ステップＳ４）。具体的には、推定部２２は、第３の位置情報により特定される位置又は姿勢の少なくとも一方における第２のランドマーク１０２の観測誤差Ｅ_ｏｂｓと、対応付け誤差Ｅ_{ｍａｔｃｈ}とがより小さくなるようにして、第３の位置情報と、第１のランドマーク１０１及び第２のランドマーク１０２の対応付けとを推定する。

【0064】

図６のフローチャートの処理により得られた移動体１０の地図中の位置（第３の位置情報）は、例えば地図に紐付いた情報を活用した高度な判断に利用することが出来る。具体的には、例えば移動体１０からは遮蔽や遠方にあるために直接、カメラ１０Ｂ及びセンサ１０Ｃ等で観測できない信号や標識の情報を、地図データから取得することであらかじめ安全な動作を行うことが出来る。自動運転やロボットにおいては、あらかじめ地図に定義された参照経路をたどることで、一度走行済みの経路あるいは目的地へ向かう経路を追従走行することが出来る。なお、第３の位置情報の利用方法はこれらに限られない。

【0065】

以上、説明したように、第１実施形態の推定装置２０では、記憶部２０Ｂは、第１のランドマーク１０１の地図中の位置及び姿勢の少なくとも一方を示す第１の位置情報を記憶する。取得部２１は、移動体１０の周辺の第２のランドマーク１０２の位置及び姿勢の少なくとも一方を示す第２の位置情報を取得する。推定部２２は、第１の位置情報と第２の位置情報とに基づいて、移動体１０の地図中の位置及び姿勢の少なくとも一方を示す第３の位置情報、及び、第１のランドマーク１０１と第２のランドマーク１０２との対応付けを推定する。推定部２２は、第３の位置情報により特定される位置又は姿勢の少なくとも一方における第２のランドマーク１０２の観測誤差Ｅ_ｏｂｓと、対応付けの誤差を示す対応付け誤差Ｅ_{ｍａｔｃｈ}とがより小さくなるようにして、第３の位置情報と対応付けとを推定する。

【0066】

これにより第１実施形態の推定装置２０によれば、地図データのランドマーク（第１のランドマーク１０１）と、観測されたランドマーク（第２のランドマーク１０２）との照合に誤対応が起こり得る状況でも、推定処理の精度を向上させることができる。具体的には、推定部２２が、第２のランドマーク１０２の地図中の位置に関して、推定された移動体１０の位置姿勢を用いた観測誤差Ｅ_ｏｂｓと、対応付けられた第１のランドマーク１０１との対応付け誤差Ｅ_{ｍａｔｃｈ}をより小さくする（最適化する）。これにより地図データと観測データとを対応付けることで移動体１０の地図中の自己位置を推定する際に、地図データのランドマーク（第１のランドマーク１０１）と、観測されたランドマーク（第２のランドマーク１０２）との対応付け（ランドマークの照合）を同時に推定できる。すなわち、ランドマークの照合を、移動体１０の地図中の自己位置を推定処理の前に、あらかじめしておく必要がない。第１実施形態の推定装置２０によれば、カメラ１０Ｂ及びセンサ１０Ｃによる第２のランドマーク１０２の観測と、観測された第２のランドマーク１０２と地図データの第１のランドマーク１０１との位置関係により、矛盾のある対応付けを棄却できるため、単体の照合ではランドマークの誤対応が起こりやすい場合でもロバストな自己位置推定が可能になる。

【0067】

（第２実施形態）
次に第２実施形態について説明する。第２実施形態の説明では、第１実施形態と同様の説明については省略し、第１実施形態と異なる箇所について説明する。

【0068】

［機能構成の例］
図７は第２実施形態の移動体１０の機能構成の例を示す図である。

【0069】

移動体１０は、推定装置２０－２、出力部１０Ａ、カメラ１０Ｂ、センサ１０Ｃ、動力制御部１０Ｇ及び動力部１０Ｈを備える。推定装置２０－２は、処理部２０Ａ及び記憶部２０Ｂを備える。

【0070】

処理部２０Ａは、取得部２１、推定部２２及び限定部２３を備える。第２実施形態では、限定部２３が更に追加されている。

【0071】

限定部２３は、第２のランドマーク１０２毎に、対応付けの対象とする第１のランドマーク１０１を限定する。限定部２３は、例えば第２のランドマーク１０２の位置、姿勢、形状、種類及び状態の少なくとも１つに基づいて、対応付けの対象とする第１のランドマーク１０１を限定する。

【0072】

推定部２２は、限定部２３によって限定された第１のランドマーク１０１と、第２のランドマーク１０２との対応付けを推定する。

【0073】

図８は第２実施形態の第１の位置情報の例を示す図である。図８の第１の位置情報の例は、第１のランドマーク１０１ｃ乃至１０１ｇの地図中の位置を示す。第１のランドマーク１０１ｃ及び１０１ｄは、信号機である。第１のランドマーク１０１ｅ及び１０１ｆは、転回禁止を示す道路標識である。第１のランドマーク１０１ｇは、駐車禁止を示す道路標識である。このように図８の例では、３種類の第１のランドマーク１０１が含まれている。上述の第１実施形態では、第１のランドマーク１０１の種類が１種類だった。

【0074】

図９Ａ乃至Ｃは、第２実施形態の限定部２３により限定された第１のランドマーク１０１の例を示す図である。限定部２３は、例えばカメラ１０Ｂによって撮像された画像から、第２のランドマーク１０２の種類を特定できる場合、記憶部２０Ｂに含まれる第１のランドマーク１０１の種類を参照し、種類が一致していない第１のランドマーク１０１を削除する。推定部２２は、対応付けの対象を同一種類の第１のランドマーク１０１に限定して、対応付けを推定する。

【0075】

図９Ａの例は、第１のランドマーク１０１の種類が信号機に限定された場合を示す。この場合、第２のランドマーク１０２ｃ及び１０２ｄと対応付けられる第１のランドマーク１０１は、第１のランドマーク１０１ｃ及び１０１ｄに限定される。

【0076】

図９Ｂの例は、第１のランドマーク１０１の種類が、転回禁止を示す道路標識に限定された場合を示す。この場合、第２のランドマーク１０２ｅと対応付けられる第１のランドマーク１０１は、第１のランドマーク１０１ｅ及び１０１ｆに限定される。

【0077】

図９Ｃの例は、第１のランドマーク１０１の種類が、信号機以外に限定された場合を示す。種類を１つに限定できない場合は、明らかに棄却できる種類を除去する形で図９Ｃのように限定してもよい。この場合、第２のランドマーク１０２ｆと対応付けられる第１のランドマーク１０１は、第１のランドマーク１０１ｅ乃至１０１ｇに限定される。

【0078】

図１０Ａは、図９Ａ乃至９Ｃの対応付けに対応する重み行列の例を示す図である。限定部２３は、取得部２１から第２のランドマーク１０２の情報を受け取り、記憶部２２Ｂから第１のランドマーク１０１の情報を受け取る。限定部２３は、第２のランドマーク１０２ｃ乃至１０２ｆについて、照合する可能性のない第１のランドマーク１０１を削除することで、地図データベースの一部を抽出する。限定部２３は、第２のランドマーク１０２ｃ乃至１０２ｆ毎に、残った第１のランドマーク１０１を含む地図データベースの一部（図９Ａ乃至９Ｃ参照）を推定部２２に入力する。推定部２２は、第２のランドマーク１０２ｃ乃至１０２ｆごとに、一部に限定された地図データベース中の第１のランドマーク１０１に基づく重み行列を用いて、上述の第３の位置情報と、対応付けとを推定する。

【0079】

図１０Ｂは第２実施形態の推定部２２の推定結果に対応する重み行列の例を示す図である。図１０Ｂの例では、推定処理の結果、対応付けられた組み合わせの重みは１となり、対応付けが棄却された重みは０になっている。

【0080】

上述の例では、第１のランドマーク１０１の種類を用いて、地図データベースを限定する場合を説明したが、種類以外にも、地図データベースに登録されている情報であれば、限定に用いられる情報は任意でよい。例えば、第２のランドマーク１０２の大よその位置を用いる方法でもよく、第２のランドマーク１０１との距離が所定の距離よりも遠い第１のランドマーク１０１を、地図データベースから削除してもよい。また例えば、限定部２３は、平面の向きや路面からの高さといった設置の情報を用いてもよいし、点滅のような状態の時間変化を用いて限定してもよい。また例えば、限定部２３は、第１のランドマーク１０１の形状が利用できる場合は、形状の近さを基準としてもよいし、第２のランドマーク１０２の一部パーツが観測できた場合に、パーツを含むか否かを基準としてもよい。また例えば、限定部２３は、第１のランドマーク１０１の色やテクスチャを用いてもよいし、第１のランドマーク１０１に記載されている文字による限定を行ってもよい。

【0081】

また、ここまで第２のランドマーク１０２毎に、地図データベースの一部を抽出する形態について述べたが、１つの第２のランドマーク１０２に対して、複数の地図データベースを割り当ててもよい。この場合、地図データベースごとに個別に推定された自己位置姿勢を、後段の処理で合成する形態でもよいし、推定部２２が適宜、地図データベースを切り替えながら推定する形態でもよい。

【0082】

例えば、記憶部２０Ｂが、第１の位置情報を含む地図データを複数、記憶してもよい。この場合、限定部２３が、地図データ毎に、対応付けの対象とする第１のランドマーク１０１を限定する。そして、推定部２２が、限定部２３によって限定された第１のランドマーク１０１と第２のランドマーク１０２との対応付け、及び、第３の位置情報を、地図データ毎に推定してもよい。

【0083】

［推定方法の例］
図１１は第２実施形態の推定方法の例を示すフローチャートである。ステップＳ１１～ステップＳ１３の説明は、第１実施形態のステップＳ１～ステップＳ３の説明と同じなので省略する。なお、図１１の例は一例であり、ステップＳ１３の処理は、例えばステップＳ１１及びステップＳ１２の処理と並列で実施されてもよい。

【0084】

限定部２３は、第２のランドマーク１０２の位置、姿勢、形状、種類及び状態の少なくとも１つに基づいて、ステップＳ１３の処理により読み出された第１の位置情報に対応する第１のランドマーク１０１の中から、対応付けの対象とする第１のランドマーク１０１を限定する（ステップＳ１４）。

【0085】

次に、推定部２２が、上述の第３の位置情報、及び、ステップＳ１４の処理によって限定された第１のランドマーク１０１と第２のランドマーク１０２との対応付けを推定する（ステップＳ１５）。

【0086】

以上、説明したように、第２実施形態によれば、第１実施形態とは異なり第２のランドマーク１０２毎に限定された地図データベースの一部を用いて、移動体１０の自己位置姿勢、及び、第１のランドマーク１０１と第２のランドマーク１０２との対応付けを推定できる。これにより、明らかに対応付かない第１のランドマーク１０１をあらかじめ取り除くことができるので、誤対応が起こりやすい場合でもロバストな自己位置推定が可能になる。

【0087】

（第３実施形態）
次に第３実施形態について説明する。第３実施形態の説明では、第１実施形態と同様の説明については省略し、第１実施形態と異なる箇所について説明する。

【0088】

［機能構成の例］
第３実施形態の移動体１０の機能構成は、第１実施形態と同様（図２参照）であるが、取得部２１が更に移動体１０の時系列の相対位置を取得し、推定部２２が当該相対位置に更に基づいて推定処理をする点が第１実施形態とは異なる。具体的には、第３実施形態では、移動体１０が刻々と移動しており，移動する都度、カメラ１０Ｂ及びセンサ１０Ｃ等が観測を行い、時系列の観測情報Ｖを取得する。推定部２２は、観測位置２０１の相対的な位置関係を更に考慮して、移動体１０の位置姿勢を推定する。

【0089】

第３実施形態の取得部２１は、移動体１０の時系列の相対位置及び姿勢の少なくとも一方を示す相対位置情報を更に取得する。相対位置情報は、例えば移動体１０の回転を含めた位置姿勢を示す。

【0090】

相対位置情報は、上述の観測情報Ｖを用いて、例えば時系列カメラ画像から検出された特徴点によるＳｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎを用いて推定してもよいし、車輪オドメトリのようにカメラ１０Ｂとは異なるセンサ１０Ｃを用いて取得されてもよい。

【0091】

推定部２２は、上述の観測誤差Ｅ_ｏｂｓと、上述の対応付け誤差Ｅ_{ｍａｔｃｈ}と、時系列の上述の第３の位置情報により特定される移動体１０の相対位置及び姿勢の少なくとも一方と相対位置情報に含まれる相対位置及び姿勢の少なくとも一方との誤差を示す相対位置誤差と、がより小さくなるようにして、第３の位置情報と対応付けとを推定する。

【0092】

具体的には、推定部２２は、第１実施形態と同様に、観測誤差Ｅ_ｏｂｓ及び対応付け誤差Ｅ_{ｍａｔｃｈ}をより小さくしつつ、時系列の第３の位置情報から推定された移動体１０の相対位置と、取得部２１により取得された相対位置との相対位置誤差がより小さくなるように拘束を加える。第１実施形態では同一の第２のランドマーク１０２を複数の観測位置２０１で観測している場合に間接的に拘束が加えられる一方で、第３実施形態では相対位置情報を取得した観測位置２０１間に直接拘束が加えられる。第３実施形態では、第２のランドマーク１０２が無いあるいは少ない範囲において、時系列の運動を拘束として用いることでより安定して滑らかな位置を推定できる。これにより、第２のランドマーク１０２が少ないことで対応付けが一意に決まらない場合においても、移動体１０の時系列の移動の拘束を明に加えることになるため、第２のランドマーク１０２と第１のランドマーク１０１との誤対応による影響を減らすことができる。

【0093】

なお、取得部２１により取得される相対位置情報と、地図データベースの座標系とが同じスケールで定義されていない場合がある。すなわち、地図データベースの座標系のスケールと、相対位置情報の座標系のスケールが異なる場合がある。

【0094】

例えば地図データベースは実空間のスケールであるのに対して、相対位置情報に含まれる相対位置姿勢は、最初の２フレームの距離を１とした時の空間に定義されている場合等がある。この場合、推定部２２が、二つの座標系におけるスケール変化も推定（算出）し、一方の相対位置を他方の相対位置のスケールに変化させてから相対位置誤差を小さくする。

【0095】

例えば推定部２２は、相対位置情報のスケールを、地図データベースのスケールに変化させてから、相対位置誤差を算出する。これにより、取得部２１が、地図データベースのスケールと異なるスケールの相対位置情報を取得した場合でも、地図データベースにおけるスケールに対する拘束を得ることが出来る。ここでスケールの推定は、シーン全体で１つのスケールを用いてもよいし、一部あるいは１つの観測位置２０１ごとにスケールを推定してもよい。また例えば、推定部２２は、相対位置誤差を算出するときのスケールを、移動体１０の各時刻の位置及び姿勢の少なくとも一方毎に算出してもよい。

【0096】

ここまで、第３実施形態における推定装置２０が、第３の位置情報と、相対位置情報とを時系列に取得する形態を示したが、相対位置関係が取得できれば、使用されるデータが時系列である必要はない。例えば、あらかじめ相対姿勢を校正済みの複数のカメラ１０Ｂ及びセンサ１０Ｃ等の間のデータを用いてもよいし、複数の移動体１０で観測したセンサデータを用いてもよい。

【0097】

［推定方法の例］
図１２は第３実施形態の推定方法の例を示すフローチャートである。ステップＳ３１～ステップＳ３２の説明は、第１実施形態のステップＳ１～ステップＳ２の説明と同じなので省略する。

【0098】

取得部２１は、移動体１０の上述の相対位置情報を取得する（ステップＳ３３）。

【0099】

次に、推定部２２が、記憶部２０Ｂから、第１のランドマーク１０１の第１の位置情報を読み出す（ステップＳ３４）。

【0100】

次に、推定部２２が、上述の観測誤差Ｅ_ｏｂｓと、上述の対応付け誤差Ｅ_{ｍａｔｃｈ}と、上述の相対位置誤差と、がより小さくなるようにして、上述の第３の位置情報、及び、第１のランドマーク１０１と第２のランドマーク１０２との対応付けを推定する（ステップＳ３５）。

【0101】

以上、説明したように、第３実施形態の推定装置２０では、第１実施形態とは異なり移動体１０の観測位置２０１の相対的な位置関係を、推定部２２が直接拘束として用いる。これにより、矛盾なく観測できるランドマーク対応付けの拘束が増えるため、誤対応が起こりやすい場合でもロバストな自己位置推定が可能になる。

【0102】

最後に、第１乃至第３実施形態の推定装置２０のハードウェア構成の例について説明する。

【0103】

［ハードウェア構成の例］
図１３は第１乃至第３実施形態の推定装置２０（２０－２）のハードウェア構成の例を示す図である。推定装置２０は、制御装置３０１、主記憶装置３０２、補助記憶装置３０３、表示装置３０４、入力装置３０５及び通信装置３０６を備える。制御装置３０１、主記憶装置３０２、補助記憶装置３０３、表示装置３０４、入力装置３０５及び通信装置３０６は、バス３１０を介して接続されている。

【0104】

なお、表示装置３０４、入力装置３０５及び通信装置３０６は備えていなくてもよい。例えば、推定装置２０が他の装置と接続される場合、当該他の装置の表示機能、入力機能及び通信機能を利用してもよい。

【0105】

制御装置３０１は補助記憶装置３０３から主記憶装置３０２に読み出されたプログラムを実行する。制御装置３０１は、例えばＣＰＵ等の１以上のプロセッサである。主記憶装置３０２はＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及び、ＲＡＭ等のメモリである。補助記憶装置３０３はメモリカード、及び、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等である。

【0106】

表示装置３０４は情報を表示する。表示装置３０４は、例えば液晶ディスプレイである。入力装置３０５は、情報の入力を受け付ける。入力装置３０５は、例えばハードウェアキー等である。なお表示装置３０４及び入力装置３０５は、表示機能と入力機能とを兼ねる液晶タッチパネル等でもよい。通信装置３０６は他の装置と通信する。

【0107】

推定装置２０で実行されるプログラムは、インストール可能な形式又は実行可能な形式のファイルでＣＤ－ＲＯＭ、メモリカード、ＣＤ－Ｒ、及び、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）等のコンピュータで読み取り可能な記憶媒体に記憶されてコンピュータ・プログラム・プロダクトとして提供される。

【0108】

また推定装置２０で実行されるプログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するように構成してもよい。また推定装置２０が実行するプログラムを、ダウンロードさせずにインターネット等のネットワーク経由で提供するように構成してもよい。

【0109】

また推定装置２０で実行されるプログラムを、ＲＯＭ等に予め組み込んで提供するように構成してもよい。

【0110】

推定装置２０で実行されるプログラムは、推定装置２０の機能のうち、プログラムにより実現可能な機能を含むモジュール構成となっている。

【0111】

プログラムにより実現される機能は、制御装置３０１が補助記憶装置３０３等の記憶媒体からプログラムを読み出して実行することにより、主記憶装置３０２にロードされる。すなわちプログラムにより実現される機能は、主記憶装置３０２上に生成される。

【0112】

なお推定装置２０の機能の一部を、ＩＣ等のハードウェアにより実現してもよい。ＩＣは、例えば専用の処理を実行するプロセッサである。

【0113】

また複数のプロセッサを用いて各機能を実現する場合、各プロセッサは、各機能のうち１つを実現してもよいし、各機能のうち２つ以上を実現してもよい。

【0114】

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

【符号の説明】

【0115】

１０ 移動体
１０Ａ 出力部
１０Ｂ カメラ
１０Ｃ センサ
１０Ｄ 通信部
１０Ｅ ディスプレイ
１０Ｆ スピーカ
１０Ｇ 動力制御部
１０Ｈ 動力部
１０Ｉ バス
２０ 推定装置
２１ 取得部
２２ 推定部
２３ 限定部
３０１ 制御装置
３０２ 主記憶装置
３０３ 補助記憶装置
３０４ 表示装置
３０５ 入力装置
３０６ 通信装置
３１０ バス

【図1】

【図2】

【図3】

【図4A】

【図4B】

【図5A】

【図5B】

【図6】

【図7】

【図8】

【図9A】

【図9B】

【図9C】

【図10A】

【図10B】

【図11】

【図12】

【図13】