JP2019191133A - 位置推定装置 - Google Patents

Abstract

【課題】高精度の位置推定を行うことができる位置推定装置を提供する。【解決手段】本発明の位置推定装置１は、撮像装置１２ａ、１２ｂ、・・・１２ｎを搭載した移動体１００の現在位置を推定する。位置推定装置１は、移動体１００の現在位置を推定し、その現在位置に基づいて複数の仮想位置を作成し、複数の仮想位置での仮想画像をそれぞれ作成し、複数の仮想画像を実際画像と比較して比較誤差を算出し、撮像装置により取得された情報と、移動体の現在位置誤差の情報の少なくとも一つに基づいて重みを算出し、重みを用いて比較誤差に重み付けを行い、重み付けされた比較誤差に基づいて現在位置を補正する。【選択図】図１

Description

本発明は，ロボットや自動車などの移動体の位置を推定する技術に関する。

ロボット及び自動車などの移動体がその周囲の情報を収集し，移動体の現在位置及び走行状態を推定し，移動体の走行を制御する自律走行技術及び運転支援技術が開発されている。移動体の周囲の情報検出のためには様々なセンサが用いられる。一般に，周囲の情報を計測するためのセンサとしては，カメラなどの撮像装置，レーザセンサ，ミリ波レーダなどがある。移動体の位置を測定するためのセンサとしては，ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）またはＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）が用いられる。

自律走行制御では，移動体に搭載された制御装置において，例えばＩＭＵで算出した移動体の速度または角速度を積算したり，ＧＰＳ測位を用いたりして，移動体自身の位置（自己位置）を推定する。また，地図情報とランドマークがなく，ＧＰＳも使えない場合は，移動体の周囲に存在する物体との相対位置を推定しながら走行中環境の地図を作成するＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）法が用いられる。ＳＬＡＭで推定した相対位置の誤差は，蓄積するため，位置修正が必須となる。位置修正は，例えば，制御装置は，レーザセンサあるいはカメラなどで周囲の情報を収集し，位置推定の際の基準となる立体物の位置や形状を検出する。そして，検出された立体物の位置を地図情報と比較（マップマッチング）することにより，移動体の現在位置を修正する。従って，検出された立体物の位置誤差が大きい場合，移動体の位置を修正できない。また，修正前は，すでに位置誤差が大きい場合は，マップマッチングを行っても，現在位置を修正できない。

ここで，例えば，非特許文献１は，正確な現在位置を推定するために，複数の仮想の現在位置を作成し，それぞれの仮想位置，地図情報，センサの内部と外部パラメータを用い，仮想の強度画像と視差画像を作成する。それぞれの作成した仮想の強度画像と視差画像を実際に取得した強度画像と視差画像とマッチングし，それぞれの強度画像と視差画像のマッチング結果に固定の重みを付けてマッチング誤差を算出する。最もマッチング誤差の小さい仮想位置を現在位置とする。

Yuquan Xu, et al. 3D Point Cloud Map Based Vehicle Localization Using Stereo Camera. 2017 IEEE Intelligent Vehicles Symposium. USA.

しかし，非特許文献１のように，それぞれの仮想位置からマッチング誤差を算出する際，強度画像のマッチング結果と視差画像のマッチング結果に固定の重みを付けるため，走行環境の明るさや撮像装置のキャリブレーション誤差による撮像装置が取得した情報の信頼度が低くなり，マッチングで推定した現在位置精度が低くなる。

本発明は、上記の点に鑑みてなされたものであり、その目的とするところは、高精度の位置推定を行うことができる位置推定装置を提供することである。

上記課題を解決するために，本発明の位置推定装置は，移動体に搭載した撮像装置であって，
撮像装置を搭載した移動体の現在位置を推定する位置推定装置であって、
前記移動体の現在位置を推定する現在位置推定手段と、
該現在位置推定手段により推定された現在位置に基づいて複数の仮想位置を作成する仮想位置作成手段と、
前記撮像装置により前記複数の仮想位置で撮像したと仮定した場合の複数の仮想画像をそれぞれ作成する仮想画像作成手段と、
前記複数の仮想画像を前記撮像装置により前記現在位置で撮像された画像と比較してそれぞれの比較誤差を算出する画像マッチング手段と、
前記撮像装置により取得された情報と、前記現在位置推定手段で得られた前記移動体の現在位置誤差の情報との少なくとも一つに基づいて重みを算出し、該重みを用いて前記それぞれの比較誤差に重み付けを行う重み付け手段と、
前記現在位置推定手段で推定されている現在位置を前記重み付けされた比較誤差に基づいて補正する位置補正手段と、
を有することを特徴とする。

本発明によれば、雨天や日差しが強い条件（逆光、照り返し、反射）や撮像装置のキャリブレーション誤差が大きい場合などのように画像によるマッチングが難しい状況でも，走行状況に応じて高精度にマッチングができる。

本発明に関連する更なる特徴は、本明細書の記述、添付図面から明らかになるものである。また、上記した以外の、課題、構成及び効果は、以下の実施形態の説明により明らかにされる。

本発明の一実施形態に係る移動体の位置推定装置の構成図。 画像処理部が行う画像処理手順を示すフローチャート。 仮想位置作成方法の一例を説明する図。 仮想画像作成方法の一例を説明する図。 キャリブレーション誤差による重みを算出する方法の一例を説明する図。 視差画像誤差による重みを算出する方法の一例を説明する図。 画像強度による重みを算出する方法を説明する図。 画像解像度による重みを算出する方法を説明する図。 特徴点による重みを算出する方法を説明する図。 画像マッチング方法の一例を説明する図。 本発明の実施例を説明する図。 本発明の実施例を説明する図。 異常な場合の説明。 障害物の影響と対策の説明。

以下，本発明の実施形態に係る移動体の位置推定装置について，図面を用いて説明する。

図１は，本発明の一実施形態に係る位置推定装置１の構成図である。
位置推定装置１は，撮像装置を搭載した移動体の現在位置を推定するものであり、自動車またはロボットなどの移動体１００に搭載されている。位置推定装置１は，一台以上の撮像装置１２（１２ａ，１２ｂ，・・・１２ｎ）と，情報処理装置１３を有する。撮像装置１２は，例えばスチルカメラまたはビデオカメラである。また，撮像装置１２は単眼カメラまたは複眼カメラでもよい。

情報処理装置１３は，撮像装置１２で撮像された画像を処理して移動体１００の位置または移動量を算出する。情報処理装置１３は，算出された位置または移動量に応じた表示を行ってもよく，または移動体１００の制御に関する信号を出力してもよい。

情報処理装置１３は，例えば一般的なコンピュータであって，撮像装置１２によって撮像された画像を処理する画像処理部１４と，画像処理結果に基づく処理を行う制御部（ＣＰＵ）１５と，メモリ１６と，ディスプレイなどの表示部１７と，これら構成要素を相互に接続するバス１８とを有する。情報処理装置１３は，画像処理部１４及び制御部１５が所定のコンピュータプログラムを実行することにより，以下の処理を行う。

撮像装置１２ａは，例えば，移動体１００の前方に設置されている。撮像装置１２ａのレンズは移動体１００の前方に向けられている。撮像装置１２ａは，例えば，移動体１００の前方の遠景を撮像する。他の撮像装置１２ｂ，・・・撮像装置１２ｎは，撮像装置１２ａと異なる位置に設置され，撮像装置１２ａと異なる撮像方向または領域を撮像する。撮像装置１２ｂは，例えば，移動体１００の後方で下方に向けて設置されていてもよい。撮像装置１２ｂは，移動体１００後方の近景を撮像するものでよい。

撮像装置１２が単眼カメラの場合，路面が平らであれば，画像上のピクセル位置と実際の地上位置関係（ｘ，ｙ）が一定になるため，撮像装置１２から特徴点までの距離を幾何学的に計算できる。撮像装置１２がステレオカメラの場合，画像上の特徴点までの距離をより正確に計測できる。以下の説明では，単眼の標準レンズを有するカメラを採用した事例について説明するが，これ以外のカメラ（広角レンズを有するカメラまたはステレオカメラなど）でもよい。

また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎが，ある時刻で撮像する対象物は，それぞれ互いに異なるものでよい。例えば，撮像装置１２ａは，移動体１００の前方の遠景を撮像するものでよい。この場合，遠景を撮像した画像からは，立体物，または位置推定のためのランドマークなどの特徴点が抽出されるようにしてもよい。撮像装置１２ｂは，移動体１００周辺の路面などの近景を撮像するようにしてもよい。この場合，近景を撮像した画像からは，移動体１００の周囲の白線，または路面ペイントなどが検出されるようにしてもよい。

また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎは，同時に雨や日差しなどの環境外乱の影響を受けないような条件で移動体１００に設置されてもよい。例えば，撮像装置１２ａは移動体１００の前方で前向きに設置されるのに対して，撮像装置１２ｂは移動体１００の後方で後ろ向きまたは下向きに設置されてもよい。これにより，例えば，降雨時に撮像装置１２ａのレンズに雨滴が付着した場合でも，進行方向の逆向きまたは下向きの撮像装置１２ｂのレンズには雨滴が付着しにくい。このため，撮像装置１２ａが撮像した画像が雨滴の影響で不鮮明であっても，撮像装置１２ｂが撮像した画像は雨滴の影響を受けにくい。あるいは，日差しの影響で撮像装置１２ａの画像が不鮮明であっても，撮像装置１２ｂが撮像した画像は鮮明である可能性がある。

また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎは，互いに異なる撮像条件（絞り値，ホワイトバランス，等）で撮影してもよい。例えば，明るい場所用にパラメータを調整した撮像装置と，暗い場所用にパラメータを調整した撮像装置とを搭載することで，環境の明暗によらず撮像可能としてもよい。

撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎは，制御部１５から撮影開始の指令を受けたとき，または一定の時間間隔で画像を撮像する。撮像された画像のデータ及び撮像時刻は，メモリ１６に格納される。メモリ１６は，情報処理装置１３の主記憶装置（メインメモリ）およびストレージなどの補助記憶装置を含む。

画像処理部１４がメモリ１６に格納された画像データ及び撮像時刻に基づいて，様々な画像処理を行う。この画像処理では，例えば，中間画像が作成されてメモリ１６に保存される。中間画像は，画像処理部１４による処理の他，制御部１５などの判断や処理に利用されてもよい。

バス１８は，ＩＥＢＵＳ（Ｉｎｔｅｒ Ｅｑｕｉｐｍｅｎｔ Ｂｕｓ）やＬＩＮ（Ｌｏｃａｌ Ｉｎｔｅｒｃｏｎｎｅｃｔ Ｎｅｔｗｏｒｋ）やＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ Ａｒｅａ Ｎｅｔｗｏｒｋ）などで構成できる。

地図１９は，移動体１００が走行する環境の情報を有する（環境情報記憶手段）。地図１９は，例えば，走行環境にある静止物（木，建物，道路，車線，信号，標識，路面ペイント，路端など）の形状や位置の情報を記憶している。地図１９のそれぞれの情報を数式で表してもよい。例えば，線情報を複数点で構成せず，線の傾きと切片のみでよい。また，地図１９の情報を区別せずに，点群で表してもよい。点群は３Ｄ（ｘ，ｙ，ｚ），４Ｄ（ｘ，ｙ，ｚ，色）などで表してもよい。最終的に，移動体１００の現在位置から走行環境を検出し，マップマッチングができれば，地図１９の情報をどんな形にしてもよい。

画像処理部１４は，地図１９とメモリ１６に格納された移動体１００の現在位置と撮像装置１２の内部パラメータに基づいて，撮像装置１２で撮像された画像に地図情報を投影する。画像処理部１４は，撮像装置１２で撮像された画像に基づいて移動体１００の複数の位置候補を特定し，その複数の位置候補と移動体１００の移動速度とに基づいて移動体１００の位置を推定する。

画像処理部１４は，例えば，移動体１００の走行中に撮像装置１２が撮像した画像を処理して，移動体１００の位置を推定する。例えば，画像処理部１４は，撮像装置１２が撮像したビデオ画像で移動体１００の移動量を算出し，過去の位置に移動量を加算して現在位置を推定する。画像処理部１４は，ビデオ画像の各フレーム画像で特徴点を抽出してもよい。画像処理部１４は，さらに，次以降のフレーム画像で同じ特徴点を抽出する。そして，画像処理部１４は，特徴点のトラッキングにより移動体１００の移動量を算出する。

制御部１５は，画像処理部１４の画像処理の結果に基づいて，移動体１００に対して移動速度に関する指令を出力してもよい。例えば，制御部１５は，画像内の立体物の画素数，画像内の特徴点のうちの外れ値の数または画像処理の種類等に応じて，移動体１００の移動速度を増加させる指令，減少させる指令または維持させる指令を出力してもよい。

図２は，画像処理部１４が行う画像処理手順を示すフローチャートである。
現在位置推定手段２０１は，移動体１００の現在位置を推定する手段である。現在位置推定手段２０１は，例えば，絶対位置を推定するＧＰＳで構成する。また，現在位置推定手段２０１は，ある基準点から相対位置を推定するオドメトリで構成してもよい。前述のＧＰＳやオドメトリに基づく位置推定手法には，位置誤差が発生するが，後述する位置補正手段２１１で補正する。

環境情報抽出手段２０２は，移動体１００に搭載されている撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した移動体１００の周囲の環境情報を抽出する手段である。環境情報抽出手段２０２では，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像された移動体１００の周囲のグレースケール画像やカラー画像を取得する。また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像から作成された視差画像を取得してもよい。そして，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像された画像上の特徴点の情報を取得してもよい。また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像上の線情報を取得してもよい。

仮想位置作成手段２０３は，現在位置推定手段２０１で推定した現在位置に基づいて仮想位置を作成する手段である。仮想位置作成手段２０３で作成する仮想位置の個数は１個以上であり、固定値にせず，走行環境に応じて変更する。仮想位置作成手段２０３の詳細は後述する。

地図参照手段２０４は，地図１９から情報を取得する手段である。制御部１５の指令に基づいて，地図１９から情報を取得する。制御部１５の指令で，地図１９にある情報を全て取得してもよい。また，移動体１００の周囲の情報のみを取得してもよい。

仮想画像作成手段２０５は，地図参照手段２０４で取得した地図情報と仮想位置作成手段２０３で作成したそれぞれの仮想位置を用い，それぞれの仮想画像を作成する。仮想画像は、撮像装置で仮想位置から撮像したと仮定した場合の画像である。例えば，地図１９ が３Ｄ（ｘ，ｙ，ｚ）の点群で構成される場合，それぞれの点をピクセル（ｕ，ｖ）に変換する。また，仮想画像作成手段２０５は，地図１９の情報に合わせて画像を作成する。例えば，地図１９ が３Ｄ（ｘ，ｙ，ｚ）の点群で構成される場合，それぞれの点をピクセル（ｕ，ｖ）に変換し，ピクセル（ｕ，ｖ）に距離情報を追加する。従って，（ｕ，ｖ，距離）で構成した仮想画像とする。

また，地図１９ が４Ｄ（ｘ，ｙ，ｚ，色）の点群で構成される場合，それぞれの点をピクセル（ｕ，ｖ）に色情報を追加し，（ｕ，ｖ，色）で構成した仮想画像にしてもよい。また，地図１９ が３Ｄ（ｘ，ｙ，ｚ）の点群で構成される場合，それぞれの点をピクセル（ｕ，ｖ）に深さ情報を追加し，（ｕ，ｖ，深さ）で構成した仮想視差画像にしてもよい。色情報は、ピクセルの強度である。画像はカラーかグレースケールで表示され、グレースケールの場合、画像のそれぞれのピクセルの強度は０から２５５（黒から白）となり、カラーの場合、Ｒ（０〜２５５）＋Ｇ（０〜２５５）＋Ｂ（０〜２５５）の３チャネルとなる。そして、深さ情報は、撮像装置などのセンサの検出方向であるＺ軸方向の長さである。

画像マッチング手段２０６は，環境情報抽出手段２０２で撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎにより実際に撮像した画像を、仮想画像作成手段２０５で作成した仮想画像とマッチングする（比較する）手段である。画像マッチング手段２０６については後述する。

条件分岐２０９は，仮想位置作成手段２０３で最後の仮想位置が作成されたかをチェックする。最後の仮想位置が作成されなかった場合，仮想位置作成手段２０３に進み，最後の仮想位置が作成された場合，重み付け手段２１０に進む。

重み付け手段２１０は，画像マッチング手段２０６で得られたそれぞれのマッチング結果（比較誤差）に重みを付ける手段である。重み付け手段２１０は，走行環境やセンサの状態に応じた重みを算出する。重み付け手段２１０は，撮像装置により取得された情報と、現在位置推定手段で得られた移動体１００の現在位置誤差の情報の少なくとも一つに基づいて重みを算出する。撮像装置により取得された情報には、撮像装置のキャリブレーション誤差の情報と、撮像装置により撮像された画像から抽出された画像上の特徴点の情報と、撮像装置により撮像された画像から取得された視差画像誤差の情報と、撮像装置により撮像された画像から取得された画像強度の情報と、撮像装置により撮像された画像から取得された画像解像度の情報の少なくとも一つが含まれる。重み付け手段２１０は，撮像装置キャリブレーション誤差推定手段２１０ａ，特徴点抽出手段２１０ｂ，視差画像誤差推定手段２１０ｃ，現在位置誤差推定手段２１０ｄ，画像強度取得手段２１０ｅ，画像解像度取得手段２１０ｆなどで構成される。

重み付け手段２１０の撮像装置キャリブレーション誤差推定手段２１０ａは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのそれぞれのキャリブレーション誤差を推定する。前述のキャリブレーションは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのそれぞれの歪みを補正するためのステップであり，既知のパターンを用いて撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像する。一方，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像の枚数，又は，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した時の距離，位置，角度などによって，キャリブレーション結果が異なるため，誤差が発生する。撮像装置キャリブレーション誤差推定手段２１０ａが前述の誤差を求めて，重み付け手段２１０に結果を出力する。

重み付け手段２１０の特徴点抽出手段２１０ｂは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像上で特徴点を抽出する。前述の特徴点は，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像上でのコーナー，エッジ，最大値，最小値などである。特徴点抽出手段２１０ｂで抽出した特徴点を重み付け手段２１０に出力する。

重み付け手段２１０の視差画像誤差推定手段２１０ｃは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像された画像を用いて視差画像を作成する際の誤差を推定する。前述の視差画像誤差は，例えば，視差画像を作成するためのキャリブレーションによる誤差である。視差画像を作成する場合，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのお互いの相対位置を正確に推定する必要があり，誤差が発生する場合，出力の視差画像の誤差が大きくなる。また，視差画像を作成する場合，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎに写る情報をそれぞれの撮像装置の画像で探索し，検索する必要があるため，探索誤差が発生するときは，出力の視差画像の誤差が大きくなる。

重み付け手段２１０の現在位置誤差推定手段２１０ｄは，現在位置推定手段２０１で推定した位置の誤差を推定する。例えば，現在位置推定手段２０１でＧＰＳにより移動体１００の現在位置を推定する場合，ＧＰＳが出力する誤差を現在位置誤差とする。また，ある基準から相対位置を推定する手法において現在位置推定手段２０１で移動体１００の現在位置を推定する場合，走行距離に比例した誤差を現在位置誤差としてもよい。

重み付け手段２１０の画像強度取得手段２１０ｅは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した画像のそれぞれのピクセル強度（画像強度）を取得する手段である。撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した画像のそれぞれのピクセル強度を取得した後，下限値以下の暗すぎるピクセルと上限値以上の明るすぎるピクセルを探索する。

重み付け手段２１０の画像解像度取得手段２１０ｆは，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した画像のそれぞれの解像度を取得する手段である。重み付け手段２１０の詳細は後述する。

位置補正手段２１１は，重み付け手段２１０を用い，現在位置推定手段２０１で推定した現在位置を補正する手段である。重み付け手段２１０で重み付けたそれぞれの仮想画像のマッチング誤差（比較誤差）の中で，最小誤差を探索する。そして、最小誤差を有した仮想位置を現在位置にする。

図３は、仮想位置作成方法の一例を説明する図である。この図３を用いて，仮想位置作成手段２０３の詳細を説明する。座標系３０１は移動体１００の現在位置推定手段２０１で推定した現在位置を原点とする。仮想位置（３０２Ａ，３０２Ｂ，・・・３０２Ｎ）は座標系３０１上の位置であり，それぞれの仮想位置の座標を３０３Ａ，３０３Ｂ，・・・３０３Ｎとする。

推定した位置誤差３０４は，現在位置誤差推定手段２１０ｄで推定した移動体１００の位置誤差である。位置誤差３０４の範囲は、現在位置推定手段２０１で推定された移動体１００の現在位置を中心とした範囲であり、その大きさは、現在位置誤差推定手段２１０ｄで推定した位置誤差に応じて変化する。

範囲Ｒ３０５は，仮想位置が作成される範囲である。仮想位置作成手段２０３は、現在位置推定手段２０１で推定された移動体１００の現在位置に基づいて複数の仮想位置を作成する。仮想位置作成手段２０３が作成する仮想位置の個数Ｎは，移動体１００の走行環境や状況に依存するため，現在位置誤差推定手段２１０ｄで推定した位置誤差３０４に基づいて，範囲Ｒ３０５を設定する。仮想位置は、移動体の推定された現在位置を中心とした所定範囲内に設定される位置と向きである。

仮想位置の互いの間隔ｋ３０６は、移動体１００の現在位置推定手段２０１で推定した現在位置３０１から仮想位置範囲Ｒ３０５までの間を所定数で分割した間隔である。間隔ｋ３０６は求めた位置精度や撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎの精度で調整する。位置推定手段２１１で推定した位置精度がＲの場合，予め設定した間隔ｋを用いて仮想位置を作成するが、走行中環境に必要な位置推定精度に基づいてｋを調整してもよい。例えば、位置推定手段２１１で指定した位置精度がＲの場合、間隔ｋを用いて位置補正手段２１１で補正する位置精度がｋとなる。一方、走行中環境に必要な精度が０．５Ｒの場合，間隔ｋから間隔０．５Ｒとし、位置補正する。簡単のため，図３をＸ−Ｙ座標で示しているが，Ｚ，ロール，ピッチ，ヨー（６ＤＯＦ）で仮想位置を作成してもよい。

一方，次元が多い場合，処理時間が増えるため，それぞれの次元の誤差に応じて仮想位置を作成する。例えば，移動体１００が走行する一車線の道路であり，白線検出機能が移動体１００に搭載された場合，移動体１００のＹ方向の誤差は低くなるため，仮想位置作成手段２０３でＹ方向の仮想位置を作成しなくてもよい。また，Ｘ方向とＹ方向の異なった間隔ｋ３０６を設定し，それぞれの方向の仮想位置の個数を調整してもよい。また，移動体１００が走行する環境に凹凸が少ない場合，Ｚ方向，ピッチ，ロールの仮想位置を作成せず，短い処理時間でＸ，Ｙ，ヨーのみ精度よく推定する。なお、仮想位置における車両の向きとして、ある範囲αとそれを分割した間隔Ｋを用いて設定してもよい。間隔Ｋは、仮想位置を作成したときと同じｋ（ｋ＝Ｋ）を設定するが、現在位置誤差推定手段２１０ｄで推定した現在位置誤差に基づいて、Ｋを調整（Ｋ＞ｋ or Ｋ＜ｋ）してもよい。

図４は、仮想画像作成方法の一例を説明する図である。この図４を用いて，仮想画像作成手段２０５の説明をする。
地図情報４０１，４０２，４０３は地図１９に記載された情報である。仮想画像４０４Ａは仮想位置３０２Ａから撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎにより撮像したと仮定した場合の画像であり，仮想画像４０４Ｎは仮想位置３０２Ｎから撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像したと仮定した場合の画像である。この時，地図１９に対する座標３０３Ａと座標３０３Ｎの位置と撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎの内部パラメータは既知であるため，地図情報４０１，４０２，４０３は仮想画像４０４Ａ上で情報４０１Ａ，４０２Ａ，４０３Ａとして映し出され，仮想画像４０４Ｎ上で情報４０１Ｎ，４０２Ｎ，４０３Ｎとして映し出される。

地図情報１９に走行環境の位置（ｘ，ｙ，ｚ）情報のみが記載されていれば，地図１９の情報は３次元（ｘ，ｙ，ｚ）となるため，仮想画像４０４Ａと仮想画像４０４Ｎを（ｕ，ｖ，距離）又は（ｕ，ｖ，深さ）で表せる。また，地図情報１９に走行環境の位置（ｘ，ｙ，ｚ）と色情報が記載されていれば，地図１９の情報は４次元（ｘ，ｙ，ｚ，色）となるため，仮想画像４０４Ａと仮想画像４０４Ｎを（ｕ，ｖ，色）で表せる。

地図１９に移動体１００の現在位置の近傍情報と遠方情報が含まれているため，全ての情報を用いて仮想画像を作成する場合，処理時間が長くなる。従って，移動体１００の現在位置から予め定められた閾値内（高さ，距離など）の情報のみを画像に変換してもよい。また，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎの撮像範囲が決まっているため，その撮像範囲内の情報を用いて，仮想画像を作成してもよい。

図５Ａから図５Ｅを用いて重み付け手段２１０の詳細を説明する。
図５Ａは、キャリブレーション誤差による重みを算出する方法の一例を説明する図であり、撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのキャリブレーションパラメータを用いた重み付けを表す。撮像装置キャリブレーション誤差推定手段２１０ａで、撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのそれぞれのキャリブレーション誤差を推定する。

パターン５００は撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎが撮像するパターンである。画像５０１は撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎがキャリブレーションされる前に取得した画像である。キャリブレーション前のため，取得した画像が歪んでいて，パターン５００を撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像する場合，誤って撮像する。

一方，画像５０２は従来のキャリブレーション技術で補正した画像を表す。この場合，歪みが補正されたため，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像５０２は実際のパターン５００と同じとなる。

キャリブレーションを行っても，完全に補正できない場合がある。キャリブレーションを行う際，パターン５００のコーナー位置が等間隔に配置されているため，補正後のコーナーの位置推定が可能であり，推定したコーナーの位置をピクセル５０３とする。一方，コーナーの画像５０２上での実際の位置がピクセル５０４の場合，推定したピクセル５０３からの誤差Ｅ５０５が発生する。従って，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのピクセル毎に推定したピクセル位置と実際の位置の誤差を算出することで，誤差Ｅ１，Ｅ２，・・・ＥＴを含めた誤差マップ５０６（キャリブレーション誤差の情報）が得られる。ピクセル数がＵ×Ｖの場合，Ｔ＝Ｕ×Ｖの誤差マップとなる。

式（１）と前述の誤差マップ５０６から重みλｃを付ける。誤差マップ５０６のそれぞれの算出した誤差Ｅ１，Ｅ２，・・・ＥＴがある閾値Ｅｔｈｒｅｓｈｏｌｄより低い場合，パラメータＩが増える。パラメータＩの全ピクセル（Ｕ×Ｖ）に対する割合を重みλｃとする。

ｉｆ Ｅ ＞ Ｅｔｈｒｅｓｈｏｌｄ， Ｉ＋＋
λｃ ＝ Ｉ／（Ｕ×Ｖ）・・・（１）

キャリブレーションは、例えば移動体１００が走行しながら定期的に実施してもよく、キャリブレーションするごとに重みを算出してもよい。

図５Ｂは、視差画像誤差による重みを算出する方法の一例を説明する図である。この図５Ｂを用いて、撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで視差画像を作成した時のパラメータを用いた重み付けの説明をする。画像５０７Ｌは視差画像を作成するための左画像であり，画像５０７Ｒは視差画像を作成するための右画像である。視差画像を作成する時，画像５０７Ｌのそれぞれのピクセル情報と同じ情報を持つ画像５０７Ｒのピクセルで探索する。従来の視差画像作成技術はピクセル強度で探索を行い，強度の最も近いピクセルとマッチングする。

例えば，ピクセル５０８Ｌを画像５０７Ｌのピクセルとし，画像５０７Ｒでそれぞれのピクセル強度と比較（探索）すると，グラフ５０９が得られる。グラフ５０９の縦軸Ｃは、左右の画像のピクセル同士を比較した誤差量である。最も誤差の小さい（Ｃｍｉｎ５１０）ピクセルを５０８Ｒとする。式（２）に示すとおり，画像５０７Ｌのそれぞれのピクセルで得られた最小誤差量Ｃｍｉｎが予め定められた閾値Ｃｔｈｒｅｓｈｏｌｄより小さければ，パラメータＪが増える。パラメータＪの全ピクセル（Ｕ×Ｖ）に対する割合を重みλｓとする。視差画像誤差が小さくなるほど重みλｓが高くなる。

ｉｆ Ｃｍｉｎ ＜ Ｃｔｈｒｅｓｈｏｌｄ， Ｊ＋＋
λｓ ＝ Ｊ／（Ｕ×Ｖ）・・・（２）

図５Ｃは、画像強度による重みを算出する方法を説明する図である。この図５Ｃを用いてピクセル強度による重み付けの説明をする。
画像５１１は現在位置から撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎが取得した画像である。

路面５１２は、移動体１００が走行している道路の路面である。
この時，静止物５１３の影５１４の影響で，画像５１１の一部が暗くなった（強度が０になった）。この時の画像５１１は、暗いピクセルが多いため，画像５１１で位置推定を行う場合，誤差が大きくなる。従って，明るいピクセル（強度＝１００％）と暗いピクセル（強度＝０％）の個数をパラメータＫで表す。明るいピクセル（強度＝１００％），又は暗いピクセル（強度＝０％）が増えると，信頼度が低くなるため，ピクセル強度による重みλＩは、式（３）を用いて算出される。

ｉｆ 強度＝０％ ＯＲ 強度＝１００％， Ｋ＋＋
λＩ ＝ １−Ｋ／（Ｕ×Ｖ）・・・（３）

簡単のため，暗いピクセルで説明したが，ピクセルの強度が高すぎる（明るすぎる）場合でも，信頼度が低くなるため，式（３）で重みλＩが低くなる。上記した式(３)により、例えば雨天等のピクセルの強度が弱すぎる場合及び逆光などのピクセル強度が強すぎる場合には重みλＩが低くなる。

図５Ｄは、画像解像度による重みを算出する方法を説明する図である。この図５ｄを用いて，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像の解像度による重み付けの説明をする。

画像５１５は撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像できる最大の解像度であり，合計Ｕ×Ｖピクセルで構成される。解像度の高い画像で位置推定する場合，信頼度が高くなる。一方，画像５１６は画像５１５より解像度の低い画像である（Ｖ’＜Ｖ，Ｕ’＜Ｕ）。また，画像５１７は画像５１６より解像度の低い画像である（Ｖ’’＜Ｖ’，Ｕ’’＜Ｕ’）。従って，画像５１６で位置推定する場合，画像５１５の位置推定より信頼度が低くなり，画像５１７で位置推定する場合の信頼度がさらに低くなる。ここで，式（４）のとおり撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで撮像した画像の画像解像度によって重み付ける。画像解像度が大きくなるに応じて重みも高くなる。

λｒ ＝ （画像解像度）／（Ｕ×Ｖ）・・・（４）

図５Ｅは、特徴点による重みを算出する方法を説明する図である。この図５Ｅを用いて，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎ上での抽出した特徴点数による重み付けの説明をする。

画像５２０は撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した画像である。静止物５２１は，画像５２０に写っている静止物である。特徴点５２２は，画像５２０上で静止物５２１から抽出した特徴点である。一方，画像５２３は撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで画像５２０を取得した時とは違う環境の画像である。静止物５２４は画像５２３に写っている静止物である。特徴点５２５は，画像５２３上で静止物５２４から抽出した特徴点である。

画像５２０の静止物５２１から抽出した特徴点数は画像５２３の静止物５２４から抽出した特徴点数より多いため，画像５２０のマッチング結果の信頼度が高い。従って，式（５）のとおり，特徴点数を用いて重みを算出する。特徴点数が多くなるに応じて重みも高くなる。

λＴ ＝ （特徴点数）／（Ｕ×Ｖ）・・・（５）

図６は、画像マッチング方法の一例を説明する図である。この図６を用いて，画像マッチング手段２０６の説明をする。本実施例では、仮想画像と実際に撮像した画像とのマッチングと、仮想距離画像と実際に撮像した画像から作成した距離画像とのマッチングを行い、仮想画像の比較誤差と仮想距離画像の比較誤差とを算出し、仮想画像の比較誤差と仮想距離画像の比較誤差とを組み合わせて平均した比較誤差を算出する。

仮想画像６０１Ａ，６０１Ｂ，・・・，６０１Ｎは，仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎからのそれぞれの作成した仮想画像（ｕ，ｖ，色）である。仮想距離画像６０２Ａ，６０２Ｂ，・・・，６０２Ｎは，仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎからのそれぞれの作成した仮想距離画像（ｕ，ｖ，距離）である。画像６０３は，移動体１００の現在位置から撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで実際に撮像した画像（ｕ，ｖ，色）である。画像６０４は，移動体１００の現在位置から撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで実際に撮像した距離画像（ｕ，ｖ，距離）である。

それぞれの仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎから作成した仮想画像６０１Ａ，６０１Ｂ，・・・，６０１Ｎを実際に撮像した画像６０３と比較し，それぞれの比較誤差をＥｉＡ，ＥｉＢ，・・・，ＥｉＮとする。それぞれの仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎから作成した仮想画像６０１Ａ，６０１Ｂ，・・・，６０１Ｎのピクセル（ｕ，ｖ）の色を色Ａ，色Ｂ，・・・，色Ｎとし，実際に撮像した画像６０３のピクセル（ｕ，ｖ）の色を色ｒとする。従って，前述の比較は，例えば，式（６）に示す通り，ピクセル（ｕ，ｖ）毎の差である。それぞれのピクセルで得られたＥｉＡ，ＥｉＢ，・・・，ＥｉＮの平均を式（６’）の通り，ＥｉＡ’，ＥｉＢ’，・・・，ＥｉＮ’とする。

ＥｉＡ（ｕ，ｖ） ＝ ｜色ｒ−色Ａ｜・・・（６）
ＥｉＢ（ｕ，ｖ） ＝ ｜色ｒ−色Ｂ｜
・
・
・
ＥｉＮ（ｕ，ｖ） ＝ ｜色ｒ−色Ｎ｜

ＥｉＡ’＝［ＥｉＡ（１，０）＋・・・＋ＥｉＡ（Ｕ，Ｖ）］／（Ｕ×Ｖ）・・・（６’）
ＥｉＢ’＝［ＥｉＢ（１，０）＋・・・＋ＥｉＢ（Ｕ，Ｖ）］／（Ｕ×Ｖ）
・
・
・
ＥｉＮ’＝［ＥｉＮ（１，０）＋・・・＋ＥｉＮ（Ｕ，Ｖ）］／（Ｕ×Ｖ）

それぞれの仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎから作成した仮想距離画像６０２Ａ，６０２Ｂ，・・・，６０２Ｎを、撮像装置で実際に撮像した画像から作成した距離画像６０４と比較し，それぞれの比較誤差をＥｓＡ，ＥｓＢ，・・・，ＥｓＮとする。それぞれの仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎから作成した仮想画像６０１Ａ，６０１Ｂ，・・・，６０１Ｎのピクセル（ｕ，ｖ）の距離を距離Ａ，距離Ｂ，・・・，距離Ｎとし，実際に撮像した距離画像６０４のピクセル（ｕ，ｖ）の距離を距離ｒとする。従って，前述の比較は，例えば，式（７）に示す通り，ピクセル（ｕ，ｖ）毎の差である。それぞれのピクセルで得られたＥｓＡ，ＥｓＢ，・・・，ＥｓＮの平均を式（７’）のとおり，ＥｓＡ’，ＥｓＢ’，・・・，ＥｓＮ’とする。

ＥｓＡ（ｕ，ｖ） ＝ ｜距離ｒ−距離Ａ｜・・・（７）
ＥｓＢ（ｕ，ｖ） ＝ ｜距離ｒ−距離Ｂ｜
・
・
・
ＥｓＮ（ｕ，ｖ） ＝ ｜距離ｒ−距離Ｎ｜

ＥｓＡ’＝［ＥｓＡ（１，０）＋・・・＋ＥｓＡ（Ｕ，Ｖ）］／（Ｕ×Ｖ）・・・（７’）
ＥｓＢ’＝［ＥｓＢ（１，０）＋・・・＋ＥｓＢ（Ｕ，Ｖ）］／（Ｕ×Ｖ）
・
・
・
ＥｓＮ’＝［ＥｓＮ（１，０）＋・・・＋ＥｓＮ（Ｕ，Ｖ）］／（Ｕ×Ｖ）

この時，移動体１００の現在位置から実際に撮像した画像６０３の重みＷｉは，図５で説明した重みを用いて式（８）に示す通り算出する。

Ｗｉ ＝ λｃ・λＩ・λｒ・λＴ・・・（８）

また，移動体１００の現在位置から実際に撮像した距離画像６０４の重みＷｓは，前述の図５で説明した重みを用いて式（９）に示す通り算出する。

Ｗｓ ＝ λｃ・λｓ・λｒ・λＴ・・・（９）

従って，それぞれの仮想位置３０２Ａ，３０２Ｂ，・・・，３０２Ｎから作成した仮想画像６０１Ａ，６０１Ｂ，・・・，６０１Ｎと仮想距離画像６０２Ａ，６０２Ｂ，・・・，６０２Ｎで得られた平均誤差ＥｉＡ’，ＥｉＢ’，・・・，ＥｉＮ’とＥｓＡ’，ＥｓＢ’，・・・，ＥｓＮ’の組み合わせを比較誤差ＥｆＡ，ＥｆＢ，・・・，ＥｆＮとする。式（１０）で比較誤差ＥｆＡ，ＥｆＢ，・・・，ＥｆＮの詳細を表す。

ＥｆＡ ＝ （Ｗｉ・ＥｉＡ’＋Ｗｓ・ＥｓＡ’）／（Ｗｉ＋Ｗｓ）・・・（１０）
ＥｆＢ ＝ （Ｗｉ・ＥｉＢ’＋Ｗｓ・ＥｓＢ’）／（Ｗｉ＋Ｗｓ）
・
・
・
ＥｆＮ ＝ （Ｗｉ・ＥｉＮ’＋Ｗｓ・ＥｓＮ’）／（Ｗｉ＋Ｗｓ）

位置補正手段２１１では、式（１０）で得られた比較誤差ＥｆＡ，ＥｆＢ，・・・，ＥｆＮの中で最小誤差を求め，最小誤差の仮想位置を現在位置とする。例えば，位置補正手段２１１で求めた最小誤差がＥｆＡの場合，現在位置推定手段２０１で推定した現在位置３０１（Ｘ，Ｙ，Ｚ）を式（１１）に示す通り，仮想位置３０２Ａ（ＸＡ，ＹＡ，ＺＡ）とする。

Ｘ＝ＸＡ・・・（１１）
Ｙ＝ＹＡ
Ｚ＝ＺＡ

また，式（１０）で得られた誤差ＥｆＡ，ＥｆＢ，・・・，ＥｆＮに基づいて，位置補正手段２１１で位置を補正してもよい。例えば，式（１２）の通り，現在位置３０１を算出する。

Ｘ＝（ＸＡ／ＥｆＡ＋・・・＋ＸＮ／ＥｆＮ）／（１／ＥｆＡ＋・・・＋１／ＥｆＮ）
Ｙ＝（ＹＡ／ＥｆＡ＋・・・＋ＹＮ／ＥｆＮ）／（１／ＥｆＡ＋・・・＋１／ＥｆＮ）
Ｚ＝（ＺＡ／ＥｆＡ＋・・・＋ＺＮ／ＥｆＮ）／（１／ＥｆＡ＋・・・＋１／ＥｆＮ）
・・・（１２）

図７Ａと図７Ｂを用いて本発明の実施例について説明する。
図７Ａの道路７００ａは移動体１００が走行中の道路である。誤差７０１ａは，現在位置誤差推定手段２１０ｄで得られた現在位置誤差である。影７０２は，移動体１００が走行している道路７００の上にある影である。実施例の説明のため，以下の条件（ａ１）〜（ｆ１）とする。
（ａ１）撮像装置キャリブレーション誤差推定手段２１０ａで得られた誤差が大きい
（ｂ１）特徴点抽出手段２１０ｂで抽出した特徴点数が多い
（ｃ１）視差画像誤差推定手段２１０ｃで得られた視差画像誤差が小さい
（ｄ１）現在位置誤差推定手段２１０ｄで推定した現在位置誤差７０１ａが小さい
（ｅ１）影７０２のため，画像強度取得手段２１０ｅで得られた強度の低いピクセルの数が多い
（ｆ１）画像解像度取得手段２１０ｆで取得した画像解像度が高い

上記条件（ａ１）〜（ｆ１）によれば，現在位置誤差推定手段２１０ｄで推定した誤差７０１ａが小さいため，仮想位置作成手段２０３で仮想位置を作成せず，現在位置推定手段２０１で推定した現在位置のみから環境情報抽出手段２０２，地図参照２０４，仮想画像作成２０５，画像マッチング２０６を実施する。式（６）と式（７）で平均誤差を算出し，式（８）と式（９）で仮想画像と仮想距離画像のそれぞれの重みを算出する。ここで，撮像装置キャリブレーション誤差推定手段２１０ａで得られた撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのキャリブレーション誤差が大きいため，λｃの重みが低くなる。影７０２の影響で画像強度取得手段２１０ｅで取得した撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎでの暗いピクセルが多いため，重みλＩが低くなる。しかし，画像解像度取得手段２１０ｆで得られた撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎの解像度が高いため，重みλｒが高くなり，視差画像誤差推定手段２１０ｃで得られた誤差が小さかったため，重みλｓが高くなった。従って，式（８）と式（９）で仮想画像の重みＷｉと仮想距離画像の重みＷｓを算出する場合，Ｗｉ＜Ｗｓとなる。

一方，図７Ｂの場合，移動体が走行中の道路７００ｂに横断歩道７０３があり，現在位置推定手段２０１で推定した現在位置誤差７０１ｂが大きい。そして、横断歩道７０３を用いて撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのキャリブレーション補正ができたため，
（ａ２）撮像装置キャリブレーション誤差推定手段２１０ａで得られたキャリブレーション誤差が小さい
（ｂ２）特徴点抽出手段２１０ｂで抽出した特徴点数が多い
（ｃ２）視差画像誤差推定手段２１０ｃで得られた視差画像誤差が大きい
（ｄ２）現在位置誤差推定手段２１０ｄで推定した現在位置誤差７０１ｂが大きい
（ｅ２）明るすぎるピクセルと暗すぎるピクセルはないため，画像強度取得手段２１０ｅで得られた強度の低いピクセルか強度の高いピクセルが少ない
（ｆ２）画像解像度取得手段２１０ｆで取得した画像解像度が高い

上記条件（ａ２）〜（ｆ２）によれば，図７Ｂの場合，現在位置誤差推定手段２１０ｄで推定した誤差７０１ｂが大きいため，仮想位置作成手段２０３で仮想位置を複数作成し，それぞれの仮想位置から地図参照手段２０４で地図情報を得て，それぞれの仮想位置から仮想画像作成手段２０５で仮想画像を作成する。そして、それぞれの仮想位置から作成した仮想画像を撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで実際に取得した画像とマッチングし，誤差を算出する。ここで，式（６）と式（７）で平均誤差を算出し，式（８）と式（９）で仮想画像と仮想距離画像のそれぞれの重みを算出する。

図７Ｂの場合，横断歩道７０３を用いて撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎのキャリブレーション補正ができるため，撮像装置キャリブレーション誤差推定手段２１０ａで得られたキャリブレーション誤差が小さくなり，λｃの重みが高くなる。そして、画像強度取得手段２１０ｅで得られた強度の低いピクセルか強度の高いピクセルが少ないため，重みλＩが高くなる。また、画像解像度取得手段２１０ｆで取得した画像解像度が高いため，重みλｒが高くなる。一方，距離画像を作成した時の視差画像誤差推定手段２１０ｃで得られた誤差が大きかったため，重みλｓが低くなった。従って，式（８）と式（９）で算出する仮想画像の重みＷｉが仮想距離画像の重みＷｓより高い（Ｗｉ＞Ｗｓ）。

図８は異常な場合を説明する図である。図８は、普段であれば地図情報のとおりに制限なく道路８００を走行できるが、事故などの影響で一時的に車線が制限された状況を示している。
道路８００は移動体１００が走行している道路である。走行可能な領域８０１ａは道路８００の走行可能な領域である。走行禁止領域８０１ｂは，道路８００の一時的に走行できない領域である。障害物８０２は，走行禁止領域８０１を区別するための障害物である。経路８０３は，障害物８０２を避けるために作成された経路である。移動体８０４は走行可能な領域８０１ａの先行車である。推定した誤差８０５は移動体１００の現在位置誤差推定手段２１０ｄで推定した誤差である。

移動体１００が道路８００を走行している時に，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎを用いて障害物８０２を検出する。この時，移動体１００の現在位置誤差推定手段２１０ｄで推定した誤差８０５は大きくないが，障害物８０２のため，走行可能な領域８０１ａは道路８００より狭くなっている。従って，普段道路８００を走行する時より，正確に位置推定を行いながら走行することが必要となる。このため，仮想位置作成手段２０３で作成する仮想位置の個数と仮想画像作成手段２０５で作成する仮想画像の個数を増やす。そして、移動体１００が経路８０３の通り、走行可能な領域８０１ａを走行した後は，道路８００の幅が元に戻り、走行可能な領域８０１ａよりも幅広くなる。したがって，通常どおり現在位置誤差推定手段２１０ｄで推定した誤差８０５に基づいて仮想位置を作成する。

簡単のため，障害物８０２を撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで認識したとしたが，障害物８０２の情報を地図１９に記載してもよい。例えば，移動体８０４が障害物８０２を認識してすぐに地図１９に記載し，他車と共有してもよい。また，地図１９に記載せず，障害物８０２情報をＶ２Ｖ（Ｖｅｈｉｃｌｅ ｔｏ Ｖｅｈｉｃｌｅ）技術を用いて領域８０１ａを走行済みの移動体８０４から、これから走行する移動体１００へ直接送信してもよい。また，Ｖ２Ｖ技術で障害物８０２の情報を送信せず，仮想位置を増やすための指令を移動体８０４から移動体１００に送信してもよい。

図９は障害物の影響と対策を説明する。
道路９００は，移動体１００が走行中の道路である。位置誤差９０１は，重み付け手段２１０の現在位置誤差推定手段２１０ｄで推定した移動体１００の現在位置誤差である。画像９０２は，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで取得した画像である。

静止物９０３は道路９００にある静止物であり，画像９０２上で静止物９０３ａとして写っている。障害物９０４は道路９００にある障害物であり，画像９０２上で障害物９０４ａとして写っている。静止物９０５は道路９００にある静止物であり，画像９０２上で静止物９０５ａとして写っている。

点９０６は画像９０２に投影した道路９００上での地図情報である。点９０７は画像９０２に投影した静止物９０３上での地図情報である。点９０８は画像９０２に投影した静止物９０５上での地図情報である。この時，障害物９０４ａは静止物９０５ａの前に写っている。地図１９の情報を（ｘ，ｙ，ｚ）の形とした時，点９０８のピクセル情報は（ｕ，ｖ，距離Ａ）となるが，撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで検出した静止物９０５への実際の距離は距離Ａではなく，静止物９０５の前にある障害物９０４への距離Ｂとなってしまう。従って，位置補正手段２１１による位置誤差が大きくなる。

前述の障害物による位置誤差を避けるための対策を説明する。移動体１００が走行中環境の地図情報を区別して，マッチングを行う。例えば，道路９００上には他車や歩行者が存在する確率が高いため，道路９００上での地図情報９０６を位置補正手段２１１で使わない。また，静止物９０５上での地図情報が周囲の障害物に影響される可能性が高いため，静止物９０５上での地図情報９０８も使わない。従って，移動体１００の現在位置推定手段２０１で推定した位置により，ある高さ以上の地図情報のみを用いて、位置補正手段２１１で位置推定を行う。すると，障害物の影響を受ける確率の低い点９０７のみで位置補正手段２１１で位置推定を行うため，移動体１００が走行中環境の障害物の影響を削減できる。

また，移動体１００の位置推定だけでなく，前述の対策で障害物の位置推定や検出もできる。例えば，点９０７のみで位置補正手段２１１で高精度に位置推定を行った後，位置補正手段２１１で使わなかった点９０７と点９０８への距離を撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎで計測する。ここで，例えば，地図１９の情報による静止物９０５へ距離は距離Ｂであるが，実際に計測した距離は距離Ａとなってしまうため，画像９０２上での点９０８は外れ値，又は，障害物であることが明確になる。従って，点９０７のみで位置補正手段２１１で推定した位置と，点９０８を用いて検出した障害物９０４の位置の正確さが高くなる。

簡単のため，点９０８を一点にして説明したが，複数点であってもよい。複数点の場合，それぞれの点の距離と点９０７のみで位置補正手段２１１で推定した位置から計測した距離を比較し，障害物検出を行ってもよい。また，画像９０２を様々な領域に分けて，ある領域内の点９０８をひとつの障害物としてもよい。また，信頼度を向上させるため，図９の時の地図情報１９と撮像装置１２ａ，撮像装置１２ｂ，・・・撮像装置１２ｎの情報とは限らず，時系列に観測し，統計的に障害物検出を行ってもよい。また，移動体１００の現在位置誤差９０１に基づいて障害物検出を行う。例えば，移動体１００の現在位置誤差９０１が低い場合，位置補正手段２１１で位置推定し，障害物かどうか判断する。一方，移動体１００の現在位置誤差９０１が高い場合，位置補正手段２１１で位置推定を何回か行って，信頼度が高くなった後に障害物検出を行う。

上述の実施形態では、現在位置推定手段で得られた移動体１００の現在位置誤差の情報と、撮像装置により取得された情報である撮像装置のキャリブレーション誤差の情報と、撮像装置により撮像された画像から抽出された画像上の特徴点の情報と、撮像装置により撮像された画像から取得された視差画像誤差の情報と、撮像装置により撮像された画像から取得された画像強度の情報と、撮像装置により撮像された画像から取得された画像解像度の情報を用いて重みを算出しているが、これらの情報の少なくとも一つに基づいて算出してもよい。

上述の実施形態によれば、撮像装置１２により取得された情報と、現在位置推定手段で得られた移動体１００の現在位置誤差の情報に基づいて重みを算出し、複数の仮想画像と実際に撮像した画像とをそれぞれ比較して算出した比較誤差に重み付けを行い、複数の仮想位置の中から、重み付けされた比較誤差が最も小さい仮想位置を移動体１００の現在位置とする処理を行っている。重みは、従来のように固定ではなく、状況に応じた値が算出されるので、情報の信頼性が高くなり、高精度の位置推定を行うことができる。したがって、例えば雨天や日差しが強い条件（逆光、照り返し、反射）や撮像装置のキャリブレーション誤差が大きい場合などのように、画像によるマッチングが難しい状況でも，走行状況に応じて高精度にマッチングができる。

以上、本発明の実施形態について詳述したが、本発明は、前記の実施形態に限定されるものではなく、特許請求の範囲に記載された本発明の精神を逸脱しない範囲で、種々の設計変更を行うことができるものである。例えば、前記した実施の形態は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。また、ある実施形態の構成の一部を他の実施形態の構成に置き換えることが可能であり、また、ある実施形態の構成に他の実施形態の構成を加えることも可能である。さらに、各実施形態の構成の一部について、他の構成の追加・削除・置換をすることが可能である。

１ 位置推定装置
１２ 撮像装置
１３ 情報処理装置
１４ 画像処理部
１５ 制御部
１６ メモリ
１７ 表示部
１００ 移動体

Claims (5)

1. 撮像装置を搭載した移動体の現在位置を推定する位置推定装置であって、
   前記移動体の現在位置を推定する現在位置推定手段と、
   該現在位置推定手段により推定された現在位置に基づいて複数の仮想位置を作成する仮想位置作成手段と、
   前記撮像装置により前記複数の仮想位置で撮像したと仮定した場合の複数の仮想画像をそれぞれ作成する仮想画像作成手段と、
   前記複数の仮想画像を前記撮像装置により前記現在位置で撮像された画像と比較してそれぞれの比較誤差を算出する画像マッチング手段と、
   前記撮像装置により取得された情報と、前記現在位置推定手段で得られた前記移動体の現在位置誤差の情報との少なくとも一つに基づいて重みを算出し、該重みを用いて前記それぞれの比較誤差に重み付けを行う重み付け手段と、
   前記現在位置推定手段で推定されている現在位置を前記重み付けされた比較誤差に基づいて補正する位置補正手段と、
   を有することを特徴とする位置推定装置。
2. 前記位置補正手段は、前記複数の仮想位置の中から前記重み付けされた比較誤差が最も小さい仮想位置を前記移動体の現在位置とすることを特徴とする請求項１に記載の位置推定装置。
3. 前記移動体の現在位置誤差の情報に基づいて、前記仮想位置の個数と現在位置からの間隔を調整することを特徴とする請求項１または２に記載の位置推定装置。
4. 前記移動体の現在位置誤差の情報に基づいて、前記撮像装置の画像解像度を変更することを特徴とする請求項１から請求項３のいずれか一項に記載の位置推定装置。
5. 前記撮像装置により取得された情報には、
   前記撮像装置のキャリブレーション誤差の情報と、
   前記撮像装置により撮像された画像上の特徴点の情報と、
   前記撮像装置により撮像された画像から視差画像を作成する際の視差画像誤差の情報と、
   前記撮像装置により撮像された画像から取得された画像強度の情報と、
   前記撮像装置により撮像された画像から取得された画像解像度の情報と、
   の少なくとも一つが含まれることを特徴とする請求項１から請求項4のいずれか一項に記載の位置推定装置。

Images