(57)【特許請求の範囲】
【請求項１】
移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定装置であって、
移動物体の前方を撮像して前方撮像画像を取得する前方撮像手段と、
移動物体の右側又は左側を撮像して側方撮像画像を取得する側方撮像手段と、
３次元地図データを前記前方撮像手段により仮想的に撮像した画像に変換して仮想前方画像を取得すると共に、３次元地図データを前記側方撮像手段により仮想的に撮像した画像に変換して仮想側方画像を取得する仮想画像取得手段と、
撮像方向が遮蔽されている状態を表す遮蔽状態を検出する遮蔽状態検出手段と、
前記遮蔽状態検出手段により検出された遮蔽状態に基づいて、前記前方撮像手段により取得された前方撮像画像及び前記側方撮像手段により取得された側方撮像画像より、移動物体の位置を推定するために適した撮像画像及び移動物体の姿勢角を推定するために適した撮像画像を選択し、
前記移動物体の位置を推定するために適した撮像画像と当該撮像画像と同じ方向に投影された仮想画像との一致度に基づいて、実際の移動物体の位置を推定すると共に、前記移動物体の姿勢角を推定するために適した撮像画像と当該撮像画像と同じ方向に投影された仮想画像との一致度に基づいて、実際の移動物体の姿勢角を推定する移動物体位置姿勢推定手段と
を有することを特徴とする移動物体位置姿勢推定装置。
【請求項２】
前記移動物体位置姿勢推定手段は、
前記遮蔽状態検出手段により検出された遮蔽状態に基づいて、
前記前方撮像手段により取得された前方撮像画像及び前記側方撮像手段により取得された側方撮像画像のそれぞれについて、移動物体の位置を推定するための画像として適している度合いを表す位置推定用画像尤度と、移動物体の姿勢角を推定するための画像として適している度合いを表す姿勢角推定用画像尤度とを設定するものであり、前記前方撮像画像の姿勢角推定用画像尤度を高め、前記側方撮像画像の位置推定用画像尤度を高める画像尤度設定手段を有し、
前記移動物体位置姿勢推定手段は、前記前方撮像画像及び前記側方撮像画像のうち、前記画像尤度設定手段により設定された位置推定用画像尤度が所定値以上の撮像画像を移動物体の位置を推定するために適した撮像画像として選択し、
前記前方撮像画像及び側方撮像画像のうち、前記画像尤度設定手段により設定された姿勢角推定用画像尤度が所定値以上の撮像画像を前記移動物体の姿勢角を推定するために適した撮像画像として選択すること
を特徴とする請求項１に記載の移動物体位置姿勢推定装置。
【請求項３】
前記遮蔽状態検出手段は、移動物体が進行方向の左側を走行している場合には左側側方画像が遮蔽された状態であり、移動物体が進行方向の右側を走行している場合には右側側方画像が遮蔽された状態であることを検出することを特徴とする請求項１又は請求項２に記載の移動物体位置姿勢推定装置。
【請求項４】
前記遮蔽状態検出手段は、前記３次元地図データを参照して移動物体の周囲の勾配を求める勾配検出手段を含み、移動物体から上方に勾配がある方向よりも遠い範囲の撮像画像が遮蔽された状態であることを検出することを特徴とする請求項１乃至請求項３の何れか一項に記載の移動物体位置姿勢推定装置。
【請求項５】
前記遮蔽状態検出手段は、前記３次元地図データを参照して前記移動物体の周囲に存在する構造物の位置及び形状を求める構造物検出手段を含み、移動物体に対する構造物の位置及び形状によって前記撮像画像の遮蔽状態を検出することを特徴とする請求項１乃至請求項４の何れか一項に記載の移動物体位置姿勢推定装置。
【請求項６】
前記遮蔽状態検出手段は、降雨又は降雪があることを検出する降雨降雪検出手段を含み、降雨又は降雪がある場合には、降雨又は降雪がない場合と比べて前方撮像画像が遮蔽されていることを検出することを特徴とする請求項１乃至請求項５の何れか一項に記載の移動物体位置姿勢推定装置。
【請求項７】
前記遮蔽状態検出手段は、現在時刻を検出して太陽の方位角を算出する太陽位置算出手段を含み、前記移動物体位置姿勢推定手段により推定された移動物体の姿勢角と、前記太陽の方位角とから、太陽光によって遮蔽される撮像画像を検出することを特徴とする請求項１乃至請求項６の何れか一項に記載の移動物体位置姿勢推定装置。
【請求項８】
前記遮蔽状態検出手段は、現在時刻を検出して現在の日照量を推定する日照量推定手段を含み、前記日照量が少ない場合には、前記移動物体に搭載された照明機器によって照射されていない方向の撮像画像が、前記照明機器により照射されている方向の撮像画像に比べて遮蔽されていると検出することを特徴とする請求項１乃至請求項７の何れか一項に記載の移動物体位置姿勢推定装置。
【請求項９】
前記遮蔽状態検出手段は、前記移動物体の周辺に存在する障害物を検出する障害物検出手段を含み、前記検出された障害物に基づいて、当該障害物によって前記前方撮像手段又は前記側方撮像手段の撮像方向が遮られているかを判定することを特徴とする請求項１乃至請求項８の何れか一項に記載の移動物体位置姿勢推定装置。
【請求項１０】
移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定方法であって、
前記移動物体の前方を撮像して前方撮像画像を取得し、右側又は左側を撮像して側方撮像画像を取得するステップと、
３次元地図データから仮想前方画像、及び仮想側方画像を取得するステップと、
撮像方向が遮蔽されている状態を表す遮蔽状態を検出するステップと、
前記遮蔽状態に基づいて、前記前方撮像画像及び前記側方撮像画像より、前記移動物体の位置を推定するために適した撮像画像及び移動物体の姿勢角を推定するために適した撮像画像を選択し、
前記移動物体の位置を推定するために適した撮像画像と当該撮像画像と同じ方向に投影された仮想画像との一致度に基づいて、実際の移動物体の位置を推定すると共に、前記移動物体の姿勢角を推定するために適した撮像画像と当該撮像画像と同じ方向に投影された仮想画像との一致度に基づいて、実際の移動物体の姿勢角を推定するステップと
を有することを特徴とする移動物体位置姿勢推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動物体の位置を推定する移動物体位置姿勢推定装置及び方法に関する。
【背景技術】
【０００２】
３次元地図とカメラの撮像画像を比較することによって移動物体の位置を算出する技術として、例えば下記の特許文献１に記載された技術が知られている。この特許文献１には、車両に備えられた車載カメラにより得られる現実の映像から抽出したエッジ画像と、周囲環境のエッジの位置や形状を３次元で記録した３次元地図を車載カメラの位置姿勢に投影したときの仮想映像とが一致するように車載カメラの位置と姿勢角を調整する。これによって、車載カメラの３次元空間での位置と姿勢角を推定している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開2009-199572号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１において、現実の映像と仮想映像が一致していても、この一致した場所が車載カメラから遠い場合、車載カメラの位置の誤差が大きい可能性がある。逆に、一致した位置が車載カメラから近い場合、車載カメラの姿勢角の誤差が大きい可能性がある。
【０００５】
そこで、本発明は、上述した実情に鑑みて提案されたものであり、精度良く移動物体の位置及び姿勢角を推定することができる移動物体位置姿勢推定装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明は、前方撮像画像の姿勢角推定用画像尤度を高め、側方撮像画像の位置推定用画像尤度を高めて、位置推定用画像尤度が所定値以上の撮像画像と仮想画像との一致度に基づいて位置を推定すると共に、姿勢角推定用画像尤度が所定値以上の撮像画像と仮想画像との一致度に基づいて、実際の移動物体の姿勢角を推定する。
【発明の効果】
【０００７】
本発明によれば、自車両の姿勢角を前方撮像画像を用いて推定でき、自車両の位置を側方撮像画像を用いて推定できるので、精度良く移動物体の位置及び姿勢角を推定することができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の実施形態として示す移動物体位置姿勢推定装置の構成を示すブロック図である。
【図２】自車両の方向を示す上面図である。
【図３】カメラの画角及び撮像方向を示す図である。
【図４】（ａ）は前方カメラ及び左側方カメラが搭載された自車両の上面図、（ｂ）は前方カメラ及び右側方カメラが搭載された自車両の上面図である。
【図５】（ａ）は前方カメラ、左側方カメラ及び後方カメラが搭載された自車両の上面図、（ｂ）は前方カメラ、右側方カメラ及び後方カメラが搭載された自車両の上面図である。
【図６】前方カメラ、左側方カメラ及び右側方カメラが搭載された自車両の上面図である。
【図７】（ａ）は前方カメラ、左側方カメラ、右側方カメラ及び後方カメラが搭載された自車両の上面図、（ｂ）は全方位カメラが搭載された自車両の上面図である。
【図８】（ａ）は撮像画像、（ｂ）は（ａ）のエッジ画像、（ｃ）は仮想画像、（ｄ）は（ｃ）の仮想位置が右方向にずれた場合の仮想画像、である。
【図９】本発明の実施形態として示す移動物体位置姿勢推定装置の動作手順を示すフローチャートである。
【図１０】本発明の実施形態として示す移動物体位置姿勢推定装置において、車速と前後方向カメラの画角との関係を示す図である。
【図１１】本発明の実施形態として示す移動物体位置姿勢推定装置において、車速と左右横方向カメラの画角との関係を示す図である。
【発明を実施するための形態】
【０００９】
以下、本発明の実施の形態について図面を参照して説明する。
【００１０】
本発明の実施形態として示す移動物体位置姿勢推定装置は、例えば図１に示すように構成される。この移動物体位置姿勢推定装置は、ＥＣＵ１、カメラ群２、３次元地図データベース３、及び、車両センサ群４を含む。車両センサ群４は、ＧＰＳ受信機４１、アクセルセンサ４２、ステアリングセンサ４３、ブレーキセンサ４４、車速センサ４５、加速度センサ４６、車輪速センサ４７、及び、ヨーレートセンサ等のその他センサ４８を含む。なお、ＥＣＵ１は、実際にはＲＯＭ、ＲＡＭ、演算回路等にて構成されている。ＥＣＵ１がＲＯＭに格納された移動物体位置姿勢推定用のプログラムに従って処理をすることによって、後述する機能（仮想画像取得手段、画像尤度設定手段、移動物体位置姿勢推定手段）を実現する。
【００１１】
カメラ群２は、複数の撮像手段（カメラ）を含む。カメラ群２の各カメラは例えばＣＣＤ等の固体撮像素子を用いたものである。カメラ群２は、少なくとも車両の前方カメラ（前方撮像手段）と、左側又は左側の何れか一方を撮像する側方カメラ（側方撮像手段）とを含む。
【００１２】
ここで、図２に示すように、自車両Ｖの前方向とは自車両Ｖの主たる進行方向である。自車両Ｖの後方向とは前方向とは逆の方向である。自車両Ｖの右方向とは前方向を見た場合の右手方向である。自車両Ｖの左方向とは前方向を見た場合の左手方向である。また、カメラ群２に含まれるカメラは、図３に示すように、所定の撮像画角θであり、カメラの撮像方向を中心とした撮像範囲２１となっている。
【００１３】
カメラ群２の車載例を図３に示す。図４（ａ）、（ｂ）は本実施形態における最小構成を示す。図４（ａ）のカメラ群２は自車両Ｖの前方が撮像範囲２１ｆとされた前方カメラ２ｆと自車両Ｖの左方向が撮像範囲２１ｌとされた左側方カメラ２ｌとを含む。図４（ｂ）のカメラ群２は自車両Ｖの前方が撮像範囲２１ｆとされた前方カメラ２ｆと自車両Ｖの右方向が撮像範囲２１ｒとされた右側方カメラ２ｒとを含む。
【００１４】
また、カメラ群２は、図５乃至図７の何れかの構成であってもよい。図５（ａ）、（ｂ）に示したカメラ群２は、図４（ａ）、（ｂ）の構成に加えて、自車両Ｖの後方向が撮像範囲２１ｂとされた後方カメラ２ｂを含む。図６に示したカメラ群２は、前方カメラ２ｆと、自車両Ｖの右方向及び左方向の双方を撮像可能なように左側方カメラ２ｌ及び右側方カメラ２ｒの双方を含む。図７（ａ）、（ｂ）に示したカメラ群２は、自車両Ｖの全方向を撮像範囲とするものである。図７（ａ）に示したカメラ群２は、自車両Ｖの前方向、右方向、左方向、後方向のすべてが撮像可能なように前方カメラ２ｆ、左側方カメラ２ｌ、右側方カメラ２ｒ、及び、後方カメラ２ｂを含む。図７（ｂ）に示すカメラ群２は、自車両Ｖの全方向を切れ目なく撮像範囲２１ａとされた全方位カメラ２ａのみを含む。なお、図４乃至図７に示したカメラ群２ではなく、自車両Ｖの前方や側方等を撮像できるような画角がカバーできていれば、例えば自車両Ｖに対して斜めにカメラを設置してもよい。
【００１５】
カメラ群２に含まれるカメラは、所定時間毎に撮像して撮像画像を取得し、ＥＣＵ１に供給する。
【００１６】
３次元地図データベース３は、例えば路面表示を含む周囲環境のエッジ等の３次元位置情報が記憶されている。本実施形態において、３次元地図データベース３には、白線、停止線、横断歩道、路面マーク等の路面表示の他に、縁石、建物等の構造物のエッジ情報も含まれる。白線などの各地図情報は、エッジの集合体で定義される。エッジが長い直線の場合には、例えば１ｍ毎に区切られるため、極端に長いエッジは存在しない。直線の場合には、各エッジは、直線の両端点を示す３次元位置情報を持っている。曲線の場合には、各エッジは、曲線の両端点と中央点を示す３次元位置情報を持っている。
【００１７】
車両センサ群（走行状態検出手段）４は、ＥＣＵ１に接続される。車両センサ群４は、各センサ４１〜４８により検出した各種のセンサ値をＥＣＵ１に供給する。ＥＣＵ１は、車両センサ群４の出力値を用いることで、自車両Ｖの概位置の算出、単位時間に自車両Ｖが進んだ移動量を示すオドメトリを算出する。特に、車両センサ群４は、自車両Ｖに搭載されたカメラ群２により取得する撮像画像が遮蔽される遮蔽状態を検出する（遮蔽状態検出手段）。
【００１８】
また、ＥＣＵ１は、カメラ群２により撮像された撮像画像と３次元地図データベース３に記憶された３次元位置情報とを用いて自車両Ｖの位置及び姿勢角の推定を行う電子制御ユニットである。特に、ＥＣＵ１は、車両センサ群４により検出された撮像画像の遮蔽状態に基づいて、自車両Ｖの位置及び姿勢角の推定を行う。なお、ＥＣＵ１は、他の制御に用いるＥＣＵと兼用しても良い。
特に、この移動物体位置姿勢推定装置は、カメラ群２により撮像した撮像画像と、３次元地図データを仮想位置及び仮想姿勢角から撮像した画像に変換した仮想画像とを比較して、自車両Ｖの位置及び姿勢角を推定する。ここで、図８（ａ）に示すような撮像画像が得られ、図８（ｂ）のようなエッジ画像が得られたとする。一方、３次元位置情報をカメラ群２の位置及び姿勢角に投影した仮想画像が図８（ｃ）のようになったとする。図８（ｂ）の撮像画像と図８（ｃ）の仮想画像とを比較すると、遠方位置（Ａ）及び近傍位置（Ｂ）の双方で一致しているため、仮想画像を生成した仮想位置及び仮想姿勢角が、自車両Ｖの位置及び姿勢角に相当すると推定できる。しかし、仮想位置が右方向に位置ずれした場合には、仮想画像は、図８（ｄ）に示すようになる。この場合、図８（ａ）の撮像画像と図８（ｄ）の撮像画像とを比較すると、遠方位置（Ａ）は一致しているが、近傍位置（Ｂ）は大きくずれてしまう。逆に、図８（ｃ）の仮想画像の仮想姿勢角をずらし（図示せず）、図８（ａ）の撮像画像と比較すると、近傍位置（Ｂ）は一致するが、遠方位置（Ａ）は大きくずれる。
【００１９】
このような現象に着目して、移動物体位置姿勢推定装置は、撮像画像内の近傍位置画素と仮想画像内の近傍位置画素とが一致した場合には仮想画像の仮想位置が尤もらしいと判断する。逆に、移動物体位置姿勢推定装置は、撮像画像内の遠方位置画素と仮想画像内の遠方位置画素とが一致した場合には仮想画像の仮想姿勢角が尤もらしいと判断する。
【００２０】
以下、この移動物体位置姿勢推定装置の動作について、図９の位置姿勢推定アルゴリズムを参照して説明する。なお、本実施形態は、自車両Ｖの６自由度の位置（前後方向，横方向，上下方向）及び姿勢角（ロール，ピッチ，ヨー）を推定するものとする。また、この図９の位置姿勢推定アルゴリズムは、ＥＣＵ１によって、例えば１００ｍｓｅｃ程度の間隔で連続的に行われる。
【００２１】
ステップＳ１において、ＥＣＵ１は、車両センサ群４から得られるセンサ値の情報から車速Ｖ（単位：ｍ／ｓ）とヨーレートγ（単位：ｒａｄ／ｓ）を検出する。ＥＣＵ１は、旋回半径ρ（単位：ｍ）を、ρ＝Ｖ／γなる演算によって取得する。更に、１ループ前の位置姿勢推定アルゴリズムにおけるステップＳ１を実行した時から自車両Ｖの移動量をオドメトリとして算出する。なお、位置姿勢推定アルゴリズムを開始して最初のループの場合は、オドメトリをゼロとして算出する。また、ヨーレートγが十分に小さい場合には、旋回半径ρは例えば１０００［ｍ］に設定して、車速を略ゼロで除算することを回避する。
【００２２】
このオドメトリ算出方法としては、例えば、車両運動を平面上に限定した上で、各車輪の車輪速とヨーレートセンサから、単位時間での移動量と回転量を算出すれば良い。また、ＥＣＵ１は、車輪速を車速やＧＰＳ受信機４１の測位値の差分で代用してもよく、ヨーレートセンサを操舵角で代用してもよい。なお、オドメトリの算出方法は、様々な算出手法が考えられるが、オドメトリが算出できればどの手法を用いても良い。
【００２３】
次のステップＳ２において、ＥＣＵ１は、ステップＳ１にて検出した車速Ｖに基づいて、自車両Ｖの前後方向の撮像画像の取得範囲を設定する。本実施形態では、撮像範囲の取得範囲を自車両Ｖの前後方向を撮像する前方カメラ２ｆ又は後方カメラ２ｂ（前後方向カメラ）の水平画角αf（単位：deg）とする。この前後方向カメラの画角は、図１０に示すように、車速Ｖに応じて設定される。車速Ｖが高いほど前後方向カメラの画角は狭くなる。なお、本実施形態は、自車両Ｖに車載されて前後方向を撮像する前方カメラ２ｆ又は後方カメラ２ｂは、水平画角の最大値が９０［ｄｅｇ］以上のものであることが望ましい。
【００２４】
なお、自車両Ｖの前後左右方向の撮像画像のうち、前後方向の撮像画像については車速Ｖが高くなるほど撮像範囲が狭くなる画像とする。これは、前後方向の撮像画像は、自車両Ｖの速度が低くなるほど横方向の運動の影響を受けやすくなるため、より広い範囲で撮像して３次元地図データと一致するように調整する必要があるためである。
【００２５】
次のステップＳ３において、ＥＣＵ１は、カメラ群２によって撮像された撮像画像のうち、ステップＳ２にて設定した前後方向の撮像画像の取得範囲に亘って、前方向の画像である前方撮像画像を抽出する。また、ＥＣＵ１は、撮像画像のうち、ステップＳ２にて設定した前後方向の撮像画像の取得範囲に亘って、後方撮像画像を抽出する。これにより、ＥＣＵ１は、前後撮像画像群を抽出する。ＥＣＵ１は、後述するように位置推定用画像又は姿勢角推定用画像として使用する画像の撮像方向に応じて、前後一方の撮像画像を抽出してもよく、双方の撮像画像を抽出してもよい。後述する処理において後方撮像画像を使用しない場合には、後方撮像画像を抽出する必要はない。なお、抽出する画像は、図３のように、自車両Ｖの前方方向から左右にそれぞれ水平画角αf／２の角度θの範囲内で撮像された部分である。
【００２６】
ステップＳ４において、ＥＣＵ１は、ステップＳ１で算出した旋回半径ρに基づいて左方向及び右方向の画像の取得範囲を設定する。本実施形態において、左方向及び右方向の画像取得範囲を自車両Ｖの左右横方向を撮像する左側方カメラ２ｌ又は右側方カメラ２ｒの水平画角αs（単位：deg）とする。この左方向及び右方向カメラの画角は、図１１に示すように、旋回半径ρを参照して設定される。旋回半径ρが大きいほど左方向及び右方向カメラの画角は狭くなる。なお、左側方カメラ２ｌ及び右側方カメラ２ｒは、例えば水平画角の最大値が９０［ｄｅｇ］以上のものであることが望ましい。
【００２７】
次のステップＳ５において、ＥＣＵ１は、カメラ群２によって撮像された撮像画像のうち、ステップＳ４にて設定した左方向及び右方向の撮像画像の取得範囲に亘って、左方向又は右方向の何れかの画像（側方撮像画像）を抽出する。ＥＣＵ１は、後述するように位置推定用画像又は姿勢角推定用画像として選択する画像の撮像方向に応じて、左右一方の側方撮像画像を抽出してもよく、双方の側方撮像画像を抽出してもよい。これにより、ＥＣＵ１は、側方撮像画像群を抽出する。なお、抽出する画像は、図３のように、自車両Ｖの左右横方向から左右にそれぞれ水平画像αs／２の角度θの範囲内で撮像された部分である。
【００２８】
ステップＳ６において、ＥＣＵ１は、車両センサ群４により検出された撮像画像の遮蔽状態に基づいて、ステップＳ３及びステップＳ５にて取得された撮像画像のそれぞれについて、位置推定用画像尤度及び姿勢角推定用画像尤度を設定する（画像尤度設定手段）。位置推定用画像尤度は、自車両Ｖの位置を推定するための画像として適している度合いを表す。姿勢角推定用画像尤度は、自車両Ｖの姿勢角を推定するための画像として適している度合いを表す。この位置推定用画像尤度及び姿勢角推定用画像尤度は、後述するように位置推定用尤度と姿勢角推定用尤度を求める際に参照される。
【００２９】
ＥＣＵ１は、初期設定において、前後撮像画像群のうち前方撮像画像についての姿勢角推定用画像尤度を高めている。また、ＥＣＵ１は、初期設定において、側方撮像画像群についての位置推定用画像尤度を高めている。ここで、初期設定とは、前方撮像画像の遮蔽状態があまりない状態を想定している。また、初期設定において、側方撮像画像は、自車両Ｖの前方よりも側方の方が壁等の構造物で遮蔽されていることを想定している。
【００３０】
自車両Ｖの前方は横方向に比べて見通しが良いために、前方撮像画像は自車両Ｖから遠くを撮像していることが多い。一方、側方撮像画像は自車両Ｖから近くを撮像していることが多い。したがって、移動物体位置姿勢推定装置は、上述した図８を参照して説明したように、自車両Ｖから遠くを撮像している前方撮像画像を姿勢角推定用画像として使用するために姿勢角推定用画像尤度を高める。一方、移動物体位置姿勢推定装置は、自車両Ｖから近くを撮像している側方撮像画像を位置推定用画像として使用するために位置推定用画像尤度を高める。
【００３１】
また、ステップＳ６において、ＥＣＵ１は、自車両Ｖが走行する走行車線が規制されている場合には、当該規制された車線に応じて姿勢角推定用画像尤度を設定してもよい。ＥＣＵ１は、車両センサ群４からの出力信号に基づいて、自車両Ｖが進行方向の左側を走行している場合には左側側方画像が遮蔽された状態であることを検出する。また、ＥＣＵ１は、自車両Ｖが進行方向の右側を走行している場合には右側側方画像が遮蔽された状態であることを検出する。
【００３２】
例えば、左側通行に規制されている場合には自車両Ｖの左側が遠くまで見通せない、遮蔽された状態にあると判断する。ＥＣＵ１は、遮蔽された側方を撮像した側方撮像画像の姿勢角推定用画像尤度を低く設定する。これにより、ＥＣＵ１は、左方向の画像を姿勢角を推定するために選択しにくくする。一方、ＥＣＵ１は、遮蔽された側方を撮像した側方撮像画像の位置推定用画像尤度を高く設定する。これにより、ＥＣＵ１は、左方向の画像を位置を推定するために選択しやすくする。また、ＥＣＵ１は、遮蔽されていない前方を撮像した前方撮像画像の位置推定用画像尤度を低く設定する。これにより、ＥＣＵ１は、前方の画像を位置を推定するために選択しにくくする。一方、ＥＣＵ１は、遮蔽されていない前方を撮像した前方撮像画像の姿勢角推定用画像尤度を高く設定する。これにより、ＥＣＵ１は、前方の画像を姿勢角を推定するために選択しやすくする。このように姿勢角推定用画像尤度及び位置推定用画像尤度を設定する場合、カメラ群２は、最低限でも図４（ａ）に示した前方カメラ２ｆと左側方カメラ２ｌを含む構成であればよい。
【００３３】
例えば、ＥＣＵ１は、側方撮像画像の位置推定用画像尤度を「１」に設定する。また、ＥＣＵ１は、前方撮像画像の姿勢角推定用画像尤度を「１」に設定する。すなわち、尤度が最も高い場合には値を「１」に設定し、尤度が低いほど値を高くする。例えば図７（ａ）のように４方向を撮像するカメラ群２の場合、側方撮像画像以外の位置推定用画像尤度を「２」に設定し、前方撮像画像以外の姿勢角推定用画像尤度を「２」に設定する。
【００３４】
逆に、右側通行に規制されている場合には自車両Ｖの右側が遠くまで見通せない、遮蔽された状態にあると判断する。これにより、ＥＣＵ１は、右方向の画像を姿勢角を推定するために選択しにくくする。一方、ＥＣＵ１は、右方向の画像を位置を推定するために選択しやすくする。このように姿勢角推定用画像尤度及び位置推定用画像尤度を設定する場合、カメラ群２は、最低限でも図４（ｂ）に示した前方カメラ２ｆと右側方カメラ２ｒを含む構成であればよい。
【００３５】
ＥＣＵ１は、ステップＳ３及びステップＳ５にて抽出した撮像画像それぞれについて、その全画素の輝度平均を求める。ＥＣＵ１は、輝度平均値が閾値ＴＨｍａｘ（単位：なし）以上、又は、閾値ＴＨｍｉｎ（単位：なし）以下であれば、当該撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を下げる。ＥＣＵ１は、例えば、姿勢角推定用画像尤度及び位置推定用画像尤度を「４」に設定する。すなわち、撮像画像の輝度平均が極端に大きい場合は、カメラ群２によって太陽光を直接撮像しているといった明るすぎる撮像画像となってしまう。逆に、撮像画像の輝度平均が極端に小さい場合には、カメラ群２によって暗がりを撮像している所謂遮蔽された状態にある撮像画像となる。このような撮像画像は、後述するステップＳ７で行うエッジ抽出などの画像の特徴点を抽出し難い画像であり、投影画像と一致するよう調整することが難しい。このため、自車両Ｖの位置や姿勢角の推定が不安定化する恐れがあるので、姿勢角推定用画像尤度及び位置推定用画像尤度を低下させる。例えば、各撮像画像をＥＣＵ１に記録するときの輝度数値は、０〜２５５（０：最も暗い，２５５：最も明るい）の範囲内であり、閾値ＴＨｍａｘには２４０、閾値ＴＨｍｉｎには１５が設定される。
【００３６】
更に、ステップＳ６において、ＥＣＵ１は、自車両Ｖから上方に勾配がある方向よりも遠い範囲の撮像画像が遮蔽された状態であることを検出してもよい。このとき、ＥＣＵ１は、３次元地図データベース３を参照して自車両Ｖの周囲の勾配を求める（勾配検出手段）。
【００３７】
具体的には、ＥＣＵ１は、３次元地図データベース３と前回の位置姿勢推定アルゴリズムにおいて推定した自車両Ｖの位置及び姿勢角から、自車両Ｖの周囲において道路又は地平面の勾配が上方に凸となっている方向を抽出する。ＥＣＵ１は、抽出した方向において、当該凸となっている場所以遠は遮蔽されていると判断する。なお、勾配が上方に凸となっていると判断する条件は、自車両Ｖから離れる方向に単位長さ（１ｍ）辺りの勾配変化量が２［ｄｅｇ］以上の場合とする。また、上方に凸となっている場所が、カメラ群２の各カメラから３０［ｍ］以内の距離であれば遠方が見渡せないとして、姿勢角推定用画像尤度を低下させる。例えば、ＥＣＵ１は、姿勢角推定用画像尤度を「５」に設定する。更に、上方に凸となっている場所が、各カメラから５［ｍ］以内の距離であれば近場が撮像できないので、更に姿勢角推定用画像尤度を低下させる。例えば、ＥＣＵ１は、姿勢角推定用画像尤度「７」、位置推定用画像尤度を「５」に設定する。なお、ＥＣＵ１は、自車両Ｖの近傍の勾配が撮像できる場合には、当該撮像画像の位置推定用画像尤度を高めてもよい。
【００３８】
更に、ステップＳ６において、ＥＣＵ１は、自車両Ｖに対する構造物の位置及び形状によって撮像画像の遮蔽状態を検出してもよい。このとき、ＥＣＵ１は、３次元地図データベース３を参照して自車両Ｖの周囲に存在する構造物の位置及び形状を求める（構造物検出手段）。
【００３９】
具体的には、ＥＣＵ１は、自車両Ｖの周囲にある建物や塀などの構造物の位置及び形状を抽出する。ＥＣＵ１は、当該構造物が存在する方向の撮像画像が、当該構造物以遠で遮蔽されているか否かを判断して、姿勢角推定用画像尤度及び位置推定用画像尤度を設定する。例えば、ＥＣＵ１は、構造物の位置及び形状と、前回の位置姿勢推定アルゴリズムにおいて推定した自車両Ｖの位置及び姿勢角から、ステップＳ３及びステップＳ５で抽出した各撮像画像の画素のうち、どの程度の割合で構造物が映りこんでいるかを推定する。例えば、撮像画像のうち５０％以上構造物が映りこんでいると推定された撮像画像は、遠くが見渡せないので、姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「３」に設定する。更に、最も近い構造物までの距離が２０［ｍ］以内の距離であれば、更に姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「５」に設定する。更に、最も近い構造物までの距離が２［ｍ］以内の距離であれば、更に姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「７」に設定し、位置推定用画像尤度を「５」に設定する。なお、ＥＣＵ１は、自車両Ｖの近傍の構造物が撮像できる場合には、当該撮像画像の位置推定用画像尤度を高めてもよい。
【００４０】
更に、ステップＳ６において、ＥＣＵ１は、降雨又は降雪がない場合と比べて前方撮像画像が遮蔽されていることを検出してもよい。このとき、ＥＣＵ１は、降雨又は降雪があることを検出する（降雨降雪検出手段）。
【００４１】
具体的には、ＥＣＵ１は、車両センサ群４から、降雨や降雪があることをワイパーの動作状態、又はインターネット等の天気予報を取得する。ＥＣＵ１は、当該車両センサ群４からの情報に基づいて各撮像画像の遮蔽状態を判断して、姿勢角推定用画像尤度及び位置推定用画像尤度を設定する。
【００４２】
例えば、降雨や降雪が時間当たり１［ｍｍ］を超えた場合には、自車両Ｖの進行方向から３０［ｄｅｇ］以内を撮像している部分を含む撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低くする。例えば、姿勢角推定用画像尤度及び位置推定用画像尤度を「４」に設定する。一方、それ以外の撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を高め、例えば、「２」に設定する。更に、降雨や降雪が時間当たり３［ｍｍ］を超えた場合には、自車両Ｖの進行方向から３０［ｄｅｇ］以内を撮像している部分を含む撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低くし、例えば「６」に設定する。一方、それ以外の撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を「４」に低下させる。更に、自車両Ｖの車速Ｖが１０［ｍ／ｓ］以上となった場合には、自車両Ｖの進行方向から３０［ｄｅｇ］以内を撮像している部分を含む撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を更に低くして「８」に設定する。
【００４３】
更に、ステップＳ６において、ＥＣＵ１は、太陽光によって遮蔽される撮像画像を検出してもよい。このとき、ＥＣＵ１は、現在時刻を検出して太陽の方位角を算出する（太陽位置算出手段）。ＥＣＵ１は、前回の位置姿勢推定アルゴリズムにより推定された自車両Ｖの姿勢角と太陽の方位角とから、太陽光によって遮蔽される撮像画像を検出する。具体的には、ＥＣＵ１は、ステップＳ３及びステップＳ５にて抽出した各撮像画像を撮像した方向内に太陽が存在するか判定する。ＥＣＵ１は、太陽が存在する方向の撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低下させ、例えば「６」に設定する。なお、ＥＣＵ１は、現在時刻だけでなく月日も検出できる時計を用いて太陽の高度を検出し、前回の位置姿勢推定アルゴリズムにて推定した自車両Ｖの位置及び姿勢角を参照して、各撮像画像を撮像した方向内に太陽が存在するか判定するとなお良い。
【００４４】
なお、ＥＣＵ１は、例えばインターネット等の天気情報を参照し、天候が雨や曇りの場合には、当該太陽光に応じた姿勢角推定用画像尤度及び位置推定用画像尤度の設定をキャンセルすることが望ましい。また、ＥＣＵ１は、上述したように自車両Ｖの周辺の構造物を検出して、当該構造物によって太陽光が直接カメラ群２に届かない、すなわち遮蔽されていることを検出してもよい。この場合には、太陽光に応じた姿勢角推定用画像尤度及び位置推定用画像尤度の設定をキャンセルするとなお良い。
【００４５】
更に、ステップＳ６において、ＥＣＵ１は、日照量が少ない場合には、自車両Ｖに搭載された照明機器によって照射されていない方向の撮像画像が、照明機器により照射されている方向の撮像画像に比べて遮蔽されていると検出してもよい。このとき、ＥＣＵ１は、現在時刻を検出して現在の日照量を推定する（日照量推定手段）。
【００４６】
具体的には、ＥＣＵ１は、８時〜１６時までは日照量が十分であり、姿勢角推定用画像尤度及び位置推定用画像尤度を再設定せず、高い値に維持する。一方、７時〜８時及び１６時〜１７時は、それぞれ夜明けと夕方なので日照量が不足する。したがって、ＥＣＵ１は、各撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低下させる。このとき、ＥＣＵ１は、自車両Ｖのヘッドライトによる照射が可能な前方撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度の低下度合いを抑制して、例えば「３」に設定する。一方、ヘッドライトによる照射が不可能な側方撮像画像及び後方撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度は大きく低下させ、例えば「６」に設定する。更に、１７時〜７時の夜間においては、ヘッドライトによる照射が可能な前方撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低下させて、例えば「４」に設定する。一方、ヘッドライトによる照射が不可能な側方撮像画像及び後方撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を大きく低下させて、例えば「９」に設定する。
【００４７】
なお、３次元地図データベース３に現在走行している道路に設置された街灯等の照明機器による明るさ情報をあらかじめ記録しておき、ＥＣＵ１は、当該３次元地図データベース３を参照して自車両Ｖ周囲の明るさに応じて姿勢角推定用画像尤度及び位置推定用画像尤度を設定するとなお良い。
【００４８】
更に、ステップＳ６において、ＥＣＵ１は、自車両Ｖ周辺の障害物に基づいて、当該障害物によって前方又は側方の撮像方向が遮られているかを判定してもよい。このために、ＥＣＵ１は、自車両Ｖの周辺に存在する障害物を検出する（障害物検出手段）。例えば、カメラ群２を用いたモーションステレオ又は自車両Ｖに別途レーザレンジファインダ等を搭載することによって、自車両Ｖの周囲にある他車両や歩行者等の障害物の位置及び形状を抽出する。ＥＣＵ１は、当該障害物が存在する方向の撮像画像が、当該障害物以遠で遮蔽されているか否かを判断して、姿勢角推定用画像尤度及び位置推定用画像尤度を設定する。
【００４９】
具体的には、自車両Ｖ周辺の障害物の位置及び形状と、前回の位置姿勢推定アルゴリズムにおいて推定した自車両Ｖの位置及び姿勢角から、ステップＳ３及びステップＳ５で抽出した各撮像画像の画素のうち、どの程度の割合で障害物が映りこんでいるかを推定する。例えば、撮像画像のうち５０％以上障害物が映りこんでいると推定された撮像画像は、遠くが見渡せないので、姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「３」に設定する。更に、最も近い障害物までの距離が２０［ｍ］以内の距離であれば、更に姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「５」に設定する。更に、最も近い障害物までの距離が２［ｍ］以内の距離であれば、更に姿勢角推定用画像尤度を低下させる。例えば、姿勢角推定用画像尤度を「７」に設定し、位置推定用画像尤度を「５」に設定する。
【００５０】
次のステップＳ７において、ＥＣＵ１は、ステップＳ６にて位置推定用画像尤度及び姿勢角推定用画像尤度が設定された撮像画像のうち、所定値を超える（又は所定値以上）位置推定用画像尤度又は姿勢角推定用画像尤度の撮像画像からエッジ画像を作成する。この所定値は、適切な撮像画像を使用して位置及び姿勢角を推定するための任意に設定した値であってもよい。これにより、ＥＣＵ１は、姿勢角推定用エッジ画像Ｅａ及び位置推定用エッジ画像Ｅｐを作成できる。
【００５１】
例えば初期設定においては、前方撮像画像についての姿勢角推定用画像尤度を高めており、側方撮像画像群についての位置推定用画像尤度を高めているので、前方撮像画像から姿勢推定用エッジ画像Ｅａを作成し、側方撮像画像から位置推定用エッジ画像Ｅｐを作成する。他の例を挙げると、自車両Ｖが左側通行の道路を走行している場合には、左側方撮像画像の位置推定用画像尤度が高く設定され、前方撮像画像の姿勢角推定用画像尤度が高く設定されている。この場合、ＥＣＵ１は、左側方撮像画像から位置推定用エッジ画像Ｅｐを作成し、前方撮像画像から姿勢推定用エッジ画像Ｅａを作成する。
【００５２】
なお、このステップＳ７においては、上述した勾配、構造物、降雨又は降雪、又は、太陽光の有無や、日射量、障害物の有無に応じて設定された姿勢角推定用画像尤度及び位置推定用画像尤度に応じて、位置及び姿勢角の推定に適切な方向を撮像した撮像画像からエッジ画像を作成することが望ましい。
【００５３】
本実施形態におけるエッジとは、画素の輝度が鋭敏に変化している箇所を指す。エッジ検出方法としては、例えばCanny法を用いることができる。これに限らず、エッジ検出手法は、他にも微分エッジ検出など様々な手法を使用してもよい。また、ＥＣＵ１は、カメラ群２の撮像画像から、エッジの輝度変化の方向やエッジ近辺のカラーなど抽出することが望ましい。これにより、後述するステップＳ５及びステップＳ６において、３次元地図データベース３にも記録しておいた、これらエッジ以外の情報も用いて位置推定用尤度、姿勢角推定用尤度を設定して、自車両の位置及び姿勢角を推定してもよい。
【００５４】
ステップＳ１の次のステップＳ８において、ＥＣＵ１は、１ループ前のステップＳ６にて推定された車両位置から、今回のステップＳ１で算出したオドメトリ分だけ移動させる。ＥＣＵ１は、移動させた車両位置の近傍で、複数の仮想（予測）位置及び仮想（予測）姿勢角の候補を算出する。ただし、位置姿勢推定アルゴリズムを開始して初めてのループの場合には、ＥＣＵ１は前回の車両位置情報を持っていない。このため、ＥＣＵ１は、車両センサ群４に含まれるＧＰＳ受信機４１からのデータを初期位置情報とする。又は、ＥＣＵ１は、前回に停車時に最後に算出した車両位置及び姿勢角を記憶しておき、初期位置及び姿勢角情報にしてもよい。
【００５５】
ＥＣＵ１は、車両センサ群４の測定誤差や通信遅れによって生じるオドメトリの誤差や、オドメトリで考慮できない車両の動特性を考慮に入れ、車両の位置や姿勢角の真値が取り得る可能性がある複数の仮想位置及び仮想姿勢角の候補を複数個生成する。この仮想位置及び仮想姿勢角の候補は、位置及び姿勢角の６自由度のパラメータに対してそれぞれ誤差の上下限を設定し、この誤差の上下限の範囲内で乱数表等を用いてランダムに設定する。
【００５６】
なお、本実施形態では、仮想位置及び仮想姿勢角の候補を５００個作成する。また、位置及び姿勢角の６自由度のパラメータに対する誤差の上下限は、前後方向，横方向，上下方向、ロール，ピッチ，ヨーの順に±0.05[m]，±0.05[m]，±0.05[m]，±0.5[deg] ，±0.5[deg]，±0.5[deg]とする。この仮想位置及び仮想姿勢角の候補を作成する数や、位置及び姿勢角の６自由度のパラメータに対する誤差の上下限は、自車両Ｖの運転状態や路面の状況を検出又は推定して、適宜変更することが望ましい。例えば、急旋回やスリップなどが発生している場合には、平面方向（前後方向，横方向，ヨー）の誤差が大きくなる可能性が高いので、この３つのパラメータの誤差の上下限を大きくし、且つ仮想位置及び仮想姿勢角の候補の作成数を増やすことが望ましい。
【００５７】
次のステップＳ９において、ＥＣＵ１は、ステップＳ８で作成した全ての仮想位置及び仮想姿勢角の候補のそれぞれについて仮想（投影）画像を作成する。このとき、ＥＣＵ１は、例えば３次元地図データベース３に記憶されたエッジ等の３次元位置情報を、仮想位置及び仮想姿勢角から撮像した撮像画像となるよう投影変換して、仮想画像を作成する。この仮想画像は、各仮想位置及び仮想姿勢角の候補が実際の自車両Ｖの位置及び姿勢角と合致しているかを評価する評価用画像である。投影変換では、カメラ群２の位置を示す外部パラメータと、カメラ２の内部パラメータが必要となる。外部パラメータは、車両位置（例えば中心位置）からカメラ群２までの相対位置を予め計測しておくことで、仮想位置及び仮想姿勢角の候補から算出すればよい。また内部パラメータは、予めキャリブレーションをしておけばよい。
【００５８】
ＥＣＵ１は、各仮想位置及び仮想姿勢角の候補について、自車両Ｖに搭載されたカメラ群２から撮像した撮像画像となるような仮想画像を作成する。例えば図４（ａ）に示すようにカメラ群２が前方カメラ２ｆ及び左側方カメラ２ｌを含む場合、各仮想位置及び仮想姿勢角の候補について仮想前方画像及び仮想左側方画像を作成する。また、図７（ａ）に示すようにカメラ群２が前方カメラ２ｆ、左側方カメラ２ｌ、右側方カメラ２ｒ及び後方カメラ２ｂを含む場合、各仮想位置及び仮想姿勢角の候補について仮想前方画像、仮想右側方画像、仮想左側方画像及び仮想後方画像を作成する。これにより、ＥＣＵ１は、姿勢推定用エッジ画像Ｅａの撮像方向と同じ方向に投影した姿勢推定用仮想画像Ａａを作成し、位置推定用エッジ画像Ｅｐの撮像方向と同じ方向に投影した位置推定用仮想画像Ａｐを作成する。なお、この仮想画像は、ステップＳ２及びステップＳ４で設定されたαf、αsの画角の範囲のみ算出すればよい。
【００５９】
なお、ステップＳ１においてカメラ２により撮像した撮像画像からエッジの輝度変化の方向や、エッジ近辺のカラーなどについても撮像画像から抽出できる場合には、それらについても３次元位置情報を３次元地図データベース３に記録しておき、仮想画像を作成するとなお良い。
【００６０】
ステップＳ７及びステップＳ９の処理の後に、ステップＳ１０を行う。ステップＳ１０において、ＥＣＵ１は、ステップＳ８にて設定した全ての仮想位置及び仮想姿勢角の候補それぞれについて、ステップＳ７にて作成したエッジ画像Ｅａ，Ｅｐと、エッジ画像の撮像方向と同じステップＳ９にて作成した仮想画像Ａａ，Ａｐとをそれぞれ比較する。ＥＣＵ１は、姿勢角推定用エッジ画像Ｅａと姿勢推定用仮想画像Ａａとの一致度に応じた値の姿勢角推定用尤度（単位：なし）を設定する。また、ＥＣＵ１は、位置推定用エッジ画像Ｅｐと位置推定用仮想画像Ａｐとの一致度に応じた値の位置推定用尤度（単位：なし）を設定する。これにより、ＥＣＵ１は、各仮想位置及び仮想姿勢角の候補点について、姿勢角推定用尤度及び位置推定用尤度を設定できる。
【００６１】
具体的には、仮想画像内の画素位置に、姿勢角推定用エッジ画像Ｅａ及び位置推定用エッジ画像Ｅｐの画素がある場合には、姿勢角推定用尤度及び位置推定用尤度を加算する。逆に、仮想画像内の画素位置に、姿勢角推定用エッジ画像Ｅａ及び位置推定用エッジ画像Ｅｐの画素が無い場合には、姿勢角推定用尤度及び位置推定用尤度を加算しない。これにより、各仮想位置及び仮想姿勢角の候補点の撮像方向ごとに、姿勢角推定用尤度及び位置推定用尤度を設定できる。
【００６２】
また、姿勢推定用エッジ画像Ｅａと姿勢推定用仮想画像Ａａとを比較して、同じ位置に一致した画素がある場合には、姿勢推定用尤度に１．０を加算し、位置推定用尤度に０．２を加算してもよい。一方、位置推定用エッジ画像Ｅｐと位置推定用仮想画像Ａｐとを比較して、同じ位置に一致した画素がある場合には、姿勢推定用尤度に０．２を加算し、位置推定用尤度に１．０を加算してもよい。
【００６３】
次のステップＳ１１において、ＥＣＵ１は、ステップＳ１０にて位置推定用尤度及び姿勢角推定用尤度が求められた複数の仮想位置及び仮想姿勢角の候補を用いて、最終的な自車両Ｖの位置及び姿勢角を算出する。ＥＣＵ１は、仮想画像の姿勢角推定用尤度に基づいて当該仮想画像の仮想姿勢角に対する実際の自車両Ｖの姿勢角を推定する（移動物体位置姿勢推定手段）。ＥＣＵ１は、仮想画像の位置推定用尤度に基づいて当該仮想画像の仮想位置に対する実際の自車両Ｖの位置を推定する（移動物体位置姿勢推定手段）。
【００６４】
このとき、ＥＣＵ１は、例えば、位置推定用尤度が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Ｖの実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ１は、姿勢角推定用尤度が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Ｖの実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ１は、位置推定用尤度と姿勢角推定用尤度との和が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Ｖの実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ１は、複数の仮想位置及び仮想姿勢角の候補に対して、各仮想画像の位置推定用尤度に応じた重み付き平均を取って自車両Ｖの実際の位置及び姿勢角を算出してもよい。又は、ＥＣＵ１は、複数の仮想位置及び仮想姿勢角の候補に対して、各仮想画像の姿勢角推定用尤度に応じた重み付き平均を取って自車両Ｖの実際の位置及び姿勢角を算出してもよい。又は、ＥＣＵ１は、各仮想画像の位置推定用尤度と姿勢角推定用尤度の和に応じた重み付き平均を取って自車両Ｖの実際の位置及び姿勢角を算出してもよい。
【００６５】
ＥＣＵ１は、以上のようなステップＳ１乃至ステップＳ１１を繰り返して行うことによって、逐次、自車両Ｖの位置と姿勢角を算出できる。
【００６６】
以上詳細に説明したように、本実施形態として示した移動物体位置姿勢推定装置によれば、撮像方向が遮蔽されている遮蔽状態に応じて、姿勢角推定用画像尤度及び位置推定用画像尤度を設定する。これにより、移動物体位置姿勢推定装置は、遮蔽状態に基づいて最適な撮像画像と当該撮像方向と同じ方向に仮想的に投影した仮想画像とを比較して、実際の自車両Ｖの位置及び姿勢角を推定できる。したがって、移動物体位置姿勢推定装置は、精度良く自車両Ｖの位置及び姿勢角を推定することができる。
【００６７】
特に、移動物体位置姿勢推定装置は、前方撮像画像の姿勢角推定用画像尤度を高めるので、側方に比べて遮蔽されずに遠くまで撮像した前方撮像画像を用いて自車両Ｖの姿勢を推定できる。また、移動物体位置姿勢推定装置は、側方撮像画像の位置推定用画像尤度を高めるので、前方に比べて遮蔽されて近くを撮像した側方撮像画像を用いて自車両Ｖの位置を推定できる。
【００６８】
２つの映像が一致した場所が自車両Ｖから遠い場合には位置の誤差が大きい場合があり、逆に、２つの映像が一致した位置が自車両Ｖから近い場合には姿勢角の誤差が大きい場合があるが、この移動物体位置姿勢推定装置によれば、精度良く移動物体の位置及び姿勢角を推定することができる。
【００６９】
また、この移動物体位置姿勢推定装置によれば、自車両Ｖの位置及び姿勢角を求めるために不要な撮像画像については仮想画像と一致する処理を省くことができるため、演算負荷の低減が実現できる。
【００７０】
また、この移動物体位置姿勢推定装置によれば、自車両Ｖが進行方向の左側を走行している場合には左側側方画像が遮蔽された状態であり、自車両Ｖが進行方向の右側を走行している場合には右側側方画像が遮蔽された状態であることを検出できる。これにより、この移動物体位置姿勢推定装置によれば、遮蔽された左側方又は右側方の撮像画像を姿勢角を推定するのに適切な撮像画像とは選択しないようにすることができる。また、移動物体位置姿勢推定装置は、自車両Ｖの前方に比べて側方は近くを撮像できることを利用して位置推定用画像尤度を高めることもできる。これにより、移動物体位置姿勢推定装置は、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置及び姿勢角の推定精度を向上させることができる。  
更に、この移動物体位置姿勢推定装置によれば、自車両Ｖから上方に勾配がある方向よりも遠い範囲の撮像画像が遮蔽された状態であることを検出できる。これにより、例えば坂道の頂上手前では前方が遮蔽され、頂上を越えたら後方が遮蔽された状態にあることが判断できる。これにより、ＥＣＵ１は、遮蔽された方向の撮像画像を使用して自車両Ｖの位置及び姿勢角を推定しないよう姿勢角推定用画像尤度及び位置推定用画像尤度を低下できる。又は、ＥＣＵ１は、遮蔽された状態によっては構造物が撮像できる状況であり、この場合には、自車両Ｖの位置を推定しやすくするので位置推定用画像尤度を高めることができる。これにより、移動物体位置姿勢推定装置は、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置及び姿勢角の推定精度を向上させることができる。
【００７１】
更に、この移動物体位置姿勢推定装置によれば、自車両Ｖに対する構造物の位置及び形状によって撮像画像の遮蔽状態を検出できる。この移動物体位置姿勢推定装置によれば、例えばトンネルや建物などの構造物情報を参照して遮蔽状態を判断するので、構造物が遠くあり遮蔽されていない場合には、当該方向の撮像画像の姿勢角推定用画像尤度を高めて、当該撮像画像を姿勢角の推定に使用しやすくすることができる。また、構造物が近くにあって遮蔽されている場合には、当該方向の撮像画像を位置及び姿勢角の推定に使用しないよう位置推定用画像尤度及び姿勢角推定用画像尤度を低下させることができる。更に、自車両Ｖの近くの構造物と投影画像が一致させることができる場合には、当該方向の撮像画像により位置の推定ができるよう位置推定用画像尤度を高めることができる。
【００７２】
更に、この移動物体位置姿勢推定装置によれば、降雨又は降雪がある場合には、降雨又は降雪がない場合と比べて前方撮像画像が遮蔽されていることを検出できる。この移動物体位置姿勢推定装置によれば、降雨や降雪によって遮蔽されている場合には、当該降雨や降雪による視界悪化が著しい移動方向を撮像した撮像画像の姿勢角推定用画像尤度及び位置推定用画像尤度を低下させる。これにより、移動物体位置姿勢推定装置は、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置と姿勢角の推定精度を向上させることができる。
【００７３】
更に、この移動物体位置姿勢推定装置によれば、自車両Ｖの姿勢角と、太陽の方位角とから、太陽光によって遮蔽される撮像画像を検出できる。この移動物体位置姿勢推定装置によれば、カメラ群２のうちの特定のカメラが太陽を含む範囲を撮像している場合には太陽光の影響によって撮像し難いので、当該方向の撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低くする。これにより、移動物体位置姿勢推定装置は、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置と姿勢角の推定精度を向上させることができる。  
更に、この移動物体位置姿勢推定装置によれば、日照量が少ない場合には照明機器によって照射されていない方向の撮像画像が、照明機器により照射されている方向の撮像画像に比べて遮蔽されていると検出できる。この移動物体位置姿勢推定装置によれば、日照量が高い場合には姿勢角推定用画像尤度及び位置推定用画像尤度を高めることができる。また、日照量が少ない場合には撮像画像が遮蔽されており、姿勢角推定用画像尤度及び位置推定用画像尤度を低下させることができる。また、日照量が少ないがヘッドライト等で照射される前方撮像画像が遮蔽されていないとして適切な姿勢角推定用画像尤度を設定することができる。これにより移動物体位置姿勢推定装置によれば、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置と姿勢角の推定精度を向上させることができる。
【００７４】
更に、この移動物体位置姿勢推定装置によれば、自車両Ｖの周囲の障害物を検出して、当該障害物が存在する方向の撮像画像が当該障害物以遠が遮蔽されていると判定できる。例えば、カメラ群２の視界は、大きな障害物であっても自車両Ｖから遠くにあれば遮蔽されないが、小さな障害物であっても自車両Ｖの間近にあれば遮蔽されてしまう。したがって、この移動物体位置姿勢推定装置は、障害物が遠くにあって撮像画像が遮蔽されていない場合には当該撮像画像についての姿勢角推定用画像尤度を高めることができる。また、移動物体位置姿勢推定装置は、障害物が近くにあって撮像画像が遮蔽されている場合には当該撮像画像についての姿勢角推定用画像尤度及び位置推定用画像尤度を低下させることができる。これにより移動物体位置姿勢推定装置によれば、適切な姿勢角推定用画像尤度及び位置推定用画像尤度を設定して、位置と姿勢角の推定精度を向上させることができる。
【００７５】
なお、上述の実施の形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施の形態以外であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計等に応じて種々の変更が可能であることは勿論である。
【００７６】
上述した実施形態では車両を例にしたが、少なくとも１台以上のカメラを搭載した移動体であれば、航空機や船舶などにも適用可能である。
【００７７】
また、上述した実施形態では車両の６自由度の位置（前後方向，横方向，上下方向）及び姿勢角（ロール，ピッチ，ヨー）を求めているが、これに限らない。例えば、サスペンション等を有さない、工場などで用いられる無人搬送車などのように３自由度での位置（前後方向，横方向）及び姿勢角（ヨー）を推定する場合にも、本実施形態が適用可能である。具体的には、このような車両であれば、上下方向の位置と、ロールおよびピッチといった姿勢角は固定であるので、予めこれらのパラメータを計測しておいたり、３次元地図データベース３を参照して求めるようにすればよい。
【符号の説明】
【００７８】
１    ＥＣＵ
２    カメラ群
２ａ  全方位カメラ
２ｂ  後方カメラ
２ｆ  前方カメラ
２ｌ  左側方カメラ
２ｒ  右側方カメラ
３    ３次元地図データベース
４    車両センサ群
２１  撮像範囲
４１  ＧＰＳ受信機
４２  アクセルセンサ
４３  ステアリングセンサ
４４  ブレーキセンサ
４５  車速センサ
４６  加速度センサ
４７  車輪速センサ
４８  その他センサ
【図１】
0005910180.tif 000002
【図２】
0005910180.tif 000003
【図３】
0005910180.tif 000004
【図４】
0005910180.tif 000005
【図５】
0005910180.tif 000006
【図６】
0005910180.tif 000007
【図７】
0005910180.tif 000008
【図８】
0005910180.tif 000009
【図９】
0005910180.tif 000010
【図１０】
0005910180.tif 000011
【図１１】
0005910180.tif 000012