JP6044084B2 - 移動物体位置姿勢推定装置及び方法 - Google Patents

Description

本発明は、移動物体の位置を推定する移動物体位置姿勢推定装置及び方法に関する。

３次元地図とカメラの撮像画像を比較することによって移動物体の位置を算出する技術として、例えば下記の特許文献１に記載された技術が知られている。この特許文献１には、車両に備えられた車載カメラにより得られる現実の映像から抽出したエッジ画像と、周囲環境のエッジの位置や形状を３次元で記録した３次元地図を車載カメラの位置姿勢に投影したときの仮想映像とが一致するように車載カメラの位置と姿勢角を調整する。これによって、車載カメラの３次元空間での位置と姿勢角を推定している。

特開2009-199572号公報

しかしながら、特許文献１において、現実の映像と仮想映像が一致していても、この一致した場所が車載カメラから遠い場合、車載カメラの位置の誤差が大きい可能性がある。逆に、一致した位置が車載カメラから近い場合、車載カメラの姿勢角の誤差が大きい可能性がある。

そこで、本発明は、上述した実情に鑑みて提案されたものであり、精度良く移動物体の位置及び姿勢角を推定することができる移動物体位置姿勢推定装置及び方法を提供することを目的とする。

本発明は、走行状態が加速又は一定速である場合には、前方撮像画像と当該前方撮像画像と同じ方向に投影された仮想前方画像との一致度に基づいて、実際の移動物体の姿勢角を推定する。また、側方撮像画像と当該側方撮像画像と同じ方向に投影された仮想側方画像との一致度に基づいて、実際の移動物体の位置を推定する。

本発明によれば、自車両の姿勢角を前方撮像画像を用いて推定でき、自車両の位置を側方撮像画像を用いて推定できるので、精度良く移動物体の位置及び姿勢角を推定することができる。

本発明の実施形態として示す移動物体位置姿勢推定装置の構成を示すブロック図である。 自車両の方向を示す上面図である。 カメラの画角及び撮像方向を示す図である。 （ａ）は前方カメラ及び左側方カメラが搭載された自車両の上面図、（ｂ）は前方カメラ及び右側方カメラが搭載された自車両の上面図である。 （ａ）は前方カメラ、左側方カメラ及び後方カメラが搭載された自車両の上面図、（ｂ）は前方カメラ、右側方カメラ及び後方カメラが搭載された自車両の上面図である。 前方カメラ、左側方カメラ及び右側方カメラが搭載された自車両の上面図である。 （ａ）は前方カメラ、左側方カメラ、右側方カメラ及び後方カメラが搭載された自車両の上面図、（ｂ）は全方位カメラが搭載された自車両の上面図である。 （ａ）は撮像画像、（ｂ）は（ａ）のエッジ画像、（ｃ）は仮想画像、（ｄ）は（ｃ）の仮想位置が右方向にずれた場合の仮想画像、である。 本発明の実施形態として示す移動物体位置姿勢推定装置の動作手順を示すフローチャートである。 本発明の実施形態として示す移動物体位置姿勢推定装置において、車速と前後方向カメラの画角との関係を示す図である。 本発明の実施形態として示す移動物体位置姿勢推定装置において、車速と左右横方向カメラの画角との関係を示す図である。 本発明の実施形態として示す移動物体位置姿勢推定装置において、走行状態に対する前方撮像画像及び左側方撮像画像の姿勢推定用画像尤度を示す図である。 本発明の実施形態として示す移動物体位置姿勢推定装置において、走行状態に対する前方撮像画像及び左側方撮像画像の位置推定用画像尤度を示す図である。 本発明の実施形態として示す移動物体位置姿勢推定装置において、走行状態に対する前方撮像画像、後方撮像画像、左側方撮像画像及び右側方撮像画像の姿勢角推定用画像尤度を示す図である。 本発明の実施形態として示す移動物体位置姿勢推定装置において、走行状態に対する前方撮像画像、後方撮像画像、左側方撮像画像及び右側方撮像画像の位置推定用画像尤度を示す図である。

以下、本発明の実施の形態について図面を参照して説明する。

本発明の実施形態として示す移動物体位置姿勢推定装置は、例えば図１に示すように構成される。この移動物体位置姿勢推定装置は、ＥＣＵ１、カメラ群２、３次元地図データベース３、及び、車両センサ群４を含む。車両センサ群４は、ＧＰＳ受信機４１、アクセルセンサ４２、ステアリングセンサ４３、ブレーキセンサ４４、車速センサ４５、加速度センサ４６、車輪速センサ４７、及び、ヨーレートセンサ等のその他センサ４８を含む。なお、ＥＣＵ１は、実際にはＲＯＭ、ＲＡＭ、演算回路等にて構成されている。ＥＣＵ１がＲＯＭに格納された移動物体位置姿勢推定用のプログラムに従って処理をすることによって、後述する機能（仮想画像取得手段、画像尤度設定手段、移動物体位置姿勢推定手段）を実現する。

カメラ群２は、複数の撮像手段（カメラ）を含む。カメラ群２の各カメラは例えばＣＣＤ等の固体撮像素子を用いたものである。カメラ群２は、少なくとも車両の前方カメラ（前方撮像手段）と、左側又は左側の何れか一方を撮像する側方カメラ（側方撮像手段）とを含む。

ここで、図２に示すように、自車両Ｖの前方向とは自車両Ｖの主たる進行方向である。自車両Ｖの後方向とは前方向とは逆の方向である。自車両Ｖの右方向とは前方向を見た場合の右手方向である。自車両Ｖの左方向とは前方向を見た場合の左手方向である。また、カメラ群２に含まれるカメラは、図３に示すように、所定の撮像画角θであり、カメラの撮像方向を中心とした撮像範囲２１となっている。

カメラ群２の車載例を図３に示す。図４（ａ）、（ｂ）は本実施形態における最小構成を示す。図４（ａ）のカメラ群２は自車両Ｖの前方が撮像範囲２１ｆとされた前方カメラ２ｆと自車両Ｖの左方向が撮像範囲２１ｌとされた左側方カメラ２ｌとを含む。図４（ｂ）のカメラ群２は自車両Ｖの前方が撮像範囲２１ｆとされた前方カメラ２ｆと自車両Ｖの右方向が撮像範囲２１ｒとされた右側方カメラ２ｒとを含む。

また、カメラ群２は、図５乃至図７の何れかの構成であってもよい。図５（ａ）、（ｂ）に示したカメラ群２は、図４（ａ）、（ｂ）の構成に加えて、自車両Ｖの後方向が撮像範囲２１ｂとされた後方カメラ２ｂを含む。図６に示したカメラ群２は、前方カメラ２ｆと、自車両Ｖの右方向及び左方向の双方を撮像可能なように左側方カメラ２ｌ及び右側方カメラ２ｒの双方を含む。図７（ａ）、（ｂ）に示したカメラ群２は、自車両Ｖの全方向を撮像範囲とするものである。図７（ａ）に示したカメラ群２は、自車両Ｖの前方向、右方向、左方向、後方向のすべてが撮像可能なように前方カメラ２ｆ、左側方カメラ２ｌ、右側方カメラ２ｒ、及び、後方カメラ２ｂを含む。図７（ｂ）に示すカメラ群２は、自車両Ｖの全方向を切れ目なく撮像範囲２１ａとされた全方位カメラ２ａのみを含む。なお、図４乃至図７に示したカメラ群２ではなく、自車両Ｖの前方や側方等を撮像できるような画角がカバーできていれば、例えば自車両Ｖに対して斜めにカメラを設置してもよい。

カメラ群２に含まれるカメラは、所定時間毎に撮像して撮像画像を取得し、ＥＣＵ１に供給する。

３次元地図データベース３は、例えば路面表示を含む周囲環境のエッジ等の３次元位置情報が記憶されている。本実施形態において、３次元地図データベース３には、白線、停止線、横断歩道、路面マーク等の路面表示の他に、縁石、建物等の構造物のエッジ情報も含まれる。白線などの各地図情報は、エッジの集合体で定義される。エッジが長い直線の場合には、例えば１ｍ毎に区切られるため、極端に長いエッジは存在しない。直線の場合には、各エッジは、直線の両端点を示す３次元位置情報を持っている。曲線の場合には、各エッジは、曲線の両端点と中央点を示す３次元位置情報を持っている。

車両センサ群（走行状態検出手段）４は、ＥＣＵ１に接続される。車両センサ群４は、各センサ４１〜４８により検出した各種のセンサ値をＥＣＵ１に供給する。ＥＣＵ１は、車両センサ群４の出力値を用いることで、自車両Ｖの概位置の算出、単位時間に自車両Ｖが進んだ移動量を示すオドメトリを算出する。特に、車両センサ群４は、自車両Ｖの走行状態を検出する。

また、ＥＣＵ１は、カメラ群２により撮像された撮像画像と３次元地図データベース３に記憶された３次元位置情報とを用いて自車両Ｖの位置及び姿勢角の推定を行う電子制御ユニットである。特に、ＥＣＵ１は、車両センサ群４により検出された自車両Ｖの走行状態に基づいて、自車両Ｖの位置及び姿勢角の推定を行う。なお、ＥＣＵ１は、他の制御に用いるＥＣＵと兼用しても良い。
特に、この移動物体位置姿勢推定装置は、カメラ群２により撮像した撮像画像と、３次元地図データを仮想位置及び仮想姿勢角から撮像した画像に変換した仮想画像とを比較して、自車両Ｖの位置及び姿勢角を推定する。ここで、図８（ａ）に示すような撮像画像が得られ、図８（ｂ）のようなエッジ画像が得られたとする。一方、３次元位置情報をカメラ群２の位置及び姿勢角に投影した仮想画像が図８（ｃ）のようになったとする。図８（ｂ）の撮像画像と図８（ｃ）の仮想画像とを比較すると、遠方位置（Ａ）及び近傍位置（Ｂ）の双方で一致しているため、仮想画像を生成した仮想位置及び仮想姿勢角が、自車両Ｖの位置及び姿勢角に相当すると推定できる。しかし、仮想位置が右方向に位置ずれした場合には、仮想画像は、図８（ｄ）に示すようになる。この場合、図８（ａ）の撮像画像と図８（ｄ）の撮像画像とを比較すると、遠方位置（Ａ）は一致しているが、近傍位置（Ｂ）は大きくずれてしまう。逆に、図８（ｃ）の仮想画像の仮想姿勢角をずらし（図示せず）、図８（ａ）の撮像画像と比較すると、近傍位置（Ｂ）は一致するが、遠方位置（Ａ）は大きくずれる。

このような現象に着目して、移動物体位置姿勢推定装置は、撮像画像内の近傍位置画素と仮想画像内の近傍位置画素とが一致した場合には仮想画像の仮想位置が尤もらしいと判断する。逆に、移動物体位置姿勢推定装置は、撮像画像内の遠方位置画素と仮想画像内の遠方位置画素とが一致した場合には仮想画像の仮想姿勢角が尤もらしいと判断する。

以下、この移動物体位置姿勢推定装置の動作について、図９の位置姿勢推定アルゴリズムを参照して説明する。なお、本実施形態は、自車両Ｖの６自由度の位置（前後方向，横方向，上下方向）及び姿勢角（ロール，ピッチ，ヨー）を推定するものとする。また、この図９の位置姿勢推定アルゴリズムは、ＥＣＵ１によって、例えば１００ｍｓｅｃ程度の間隔で連続的に行われる。

ステップＳ１において、ＥＣＵ１は、車両センサ群４から得られるセンサ値の情報から車速Ｖ（単位：ｍ／ｓ）とヨーレートγ（単位：ｒａｄ／ｓ）を検出する。ＥＣＵ１は、旋回半径ρ（単位：ｍ）を、ρ＝Ｖ／γなる演算によって取得する。更に、１ループ前の位置姿勢推定アルゴリズムにおけるステップＳ１を実行した時から自車両Ｖの移動量をオドメトリとして算出する。なお、位置姿勢推定アルゴリズムを開始して最初のループの場合は、オドメトリをゼロとして算出する。また、ヨーレートγが十分に小さい場合には、旋回半径ρは例えば１０００［ｍ］に設定して、車速を略ゼロで除算することを回避する。

このオドメトリ算出方法としては、例えば、車両運動を平面上に限定した上で、各車輪の車輪速とヨーレートセンサから、単位時間での移動量と回転量を算出すれば良い。また、ＥＣＵ１は、車輪速を車速やＧＰＳ受信機４１の測位値の差分で代用してもよく、ヨーレートセンサを操舵角で代用してもよい。なお、オドメトリの算出方法は、様々な算出手法が考えられるが、オドメトリが算出できればどの手法を用いても良い。

次のステップＳ２において、ＥＣＵ１は、ステップＳ１にて検出した車速Ｖに基づいて、自車両Ｖの前後方向の撮像画像の取得範囲を設定する。本実施形態では、撮像範囲の取得範囲を自車両Ｖの前後方向を撮像する前方カメラ２ｆ又は後方カメラ２ｂ（前後方向カメラ）の水平画角αf（単位：deg）とする。この前後方向カメラの画角は、図１０に示すように、車速Ｖに応じて設定される。車速Ｖが高いほど前後方向カメラの画角は狭くなる。なお、本実施形態は、自車両Ｖに車載されて前後方向を撮像する前方カメラ２ｆ又は後方カメラ２ｂは、水平画角の最大値が９０［ｄｅｇ］以上のものであることが望ましい。

なお、自車両Ｖの前後左右方向の撮像画像のうち、前後方向の撮像画像については車速Ｖが高くなるほど撮像範囲が狭くなる画像とする。これは、前後方向の撮像画像は、自車両Ｖの速度が低くなるほど横方向の運動の影響を受けやすくなるため、より広い範囲で撮像して３次元地図データと一致するように調整する必要があるためである。

次のステップＳ３において、ＥＣＵ１は、カメラ群２によって撮像された撮像画像のうち、ステップＳ２にて設定した前後方向の撮像画像の取得範囲に亘って、前方向の画像である前方撮像画像を抽出する。また、ＥＣＵ１は、撮像画像のうち、ステップＳ２にて設定した前後方向の撮像画像の取得範囲に亘って、後方撮像画像を抽出する。これにより、ＥＣＵ１は、前後撮像画像群を抽出する。ＥＣＵ１は、後述するように位置推定用画像又は姿勢角推定用画像として選択する画像の撮像方向に応じて、前後一方の撮像画像を抽出してもよく、双方の撮像画像を抽出してもよい。後述する処理において後方撮像画像を使用しない場合には、後方撮像画像を抽出する必要はない。なお、抽出する画像は、図３のように、自車両Ｖの前方方向から左右にそれぞれ水平画角αf／２の角度θの範囲内で撮像された部分である。

ステップＳ４において、ＥＣＵ１は、ステップＳ１で算出した旋回半径ρに基づいて左方向及び右方向の画像の取得範囲を設定する。本実施形態において、左方向及び右方向の画像取得範囲を自車両Ｖの左右横方向を撮像する左側方カメラ２ｌ又は右側方カメラ２ｒの水平画角αs（単位：deg）とする。この左方向及び右方向カメラの画角は、図１１に示すように、旋回半径ρを参照して設定される。旋回半径ρが増大するほど左方向及び右方向カメラの画角は狭くなる。なお、左側方カメラ２ｌ及び右側方カメラ２ｒは、例えば水平画角の最大値が９０［ｄｅｇ］以上のものであることが望ましい。

なお、自車両Ｖの前後左右方向の撮像画像のうち、左右横方向の撮像画像については旋回半径が増大するほど撮像範囲が狭くなる画像とする。これは、左右横方向の撮像画像は、旋回半径が減少する、すなわち急カーブであるほど当該カーブの状態をより広い範囲で撮像して、３次元地図データと一致するように調整する必要があるためである。

次のステップＳ５において、ＥＣＵ１は、カメラ群２によって撮像された撮像画像のうち、ステップＳ４にて設定した左方向及び右方向の撮像画像の取得範囲に亘って、左方向又は右方向の何れかの画像（側方撮像画像）を抽出する。ＥＣＵ１は、後述するように位置推定用画像又は姿勢角推定用画像として使用する画像の撮像方向に応じて、左右一方の側方撮像画像を抽出してもよく、双方の側方撮像画像を抽出してもよい。これにより、ＥＣＵ１は、側方撮像画像群を抽出する。なお、抽出する画像は、図３のように、自車両Ｖの左右横方向から左右にそれぞれ水平画像αs／２の角度θの範囲内で撮像された部分である。

ステップＳ６において、ＥＣＵ１は、車両センサ群４により検出された走行状態に基づいて、ステップＳ３及びステップＳ５にて取得された撮像画像のそれぞれについて、位置推定用画像尤度及び姿勢角推定用画像尤度を設定する（画像尤度設定手段）。位置推定用画像尤度は、自車両Ｖの位置を推定するための画像として適している度合いを表す。姿勢角推定用画像尤度は、自車両Ｖの姿勢角を推定するための画像として適している度合いを表す。この位置推定用画像尤度及び姿勢角推定用画像尤度は、後述するように位置推定用尤度と姿勢角推定用尤度を求める際に参照される。

ＥＣＵ１は、通常の走行状態である場合には、前後撮像画像群のうち前方撮像画像についての姿勢角推定用画像尤度を高める。また、ＥＣＵ１は、通常の走行状態である場合には、側方撮像画像群についての位置推定用画像尤度を高める。ここで、通常の走行状態とは、自車両Ｖが加速又は一定速となっている状況である。

自車両Ｖの前方は横方向に比べて見通しが良いために、前方撮像画像は自車両Ｖから遠くを撮像していることが多い。一方、側方撮像画像は自車両Ｖから近くを撮像していることが多い。したがって、移動物体位置姿勢推定装置は、上述した図８を参照して説明したように、自車両Ｖから遠くを撮像している前方撮像画像を姿勢角推定用画像として使用するために姿勢角推定用画像尤度を高める。一方、移動物体位置姿勢推定装置は、自車両Ｖから近くを撮像している側方撮像画像を位置推定用画像として使用するために位置推定用画像尤度を高める。

この処理のため、ＥＣＵ１は、例えば図１２及び図１３に示すようなテーブルデータを参照してもよい。なお、図１２及び図１３は、図４（ａ）のように、前方カメラ２ｆ及び左側方カメラ２ｌのみを搭載した自車両Ｖについて設定したものである。また、このテーブルデータは、位置推定用画像尤度及び姿勢角推定用画像尤度を０〜１０の範囲で設定したものであり、この数値には限定されないことは勿論である。

図１２に示すテーブルデータは、走行状態に応じて姿勢角推定用画像尤度を設定するためのデータである。図１２によれば、ＥＣＵ１は、どのような走行状態であっても、概ね、側方撮像画像（左側画像）の姿勢角推定用画像尤度よりも、前方撮像画像の姿勢角推定用画像尤度を高めることができる。図１３に示すテーブルデータは、走行状態に応じて位置推定用画像尤度を設定するためのデータである。図１３によれば、ＥＣＵ１は、どのような走行状態であっても、概ね、前方撮像画像の位置推定用画像尤度よりも、側方撮像画像（左側画像）の位置推定用画像尤度を高めることができる。

また、ＥＣＵ１は、自車両Ｖが減速している、すなわち車速Ｖが継続して減少している場合には、前方撮像画像も位置の推定に適しているために位置推定用画像尤度を高めてもよい。図１３に示すように、自車両Ｖが減速しており、且つ、直進方向に走行している場合には、他の走行状態と比較して前方撮像画像の位置推定用画像尤度を高めている。これは、自車両Ｖが減速する場合には、前方に減速する対象である、停止線、交差点、カーブがあり、前方撮像画像によって近傍の構造物が撮像可能であることが多いためである。

更に、ＥＣＵ１は、自車両Ｖが減速している、すなわち車速Ｖが継続して減少している場合には、後方撮像画像についての姿勢角推定用画像尤度を高めることが望ましい。自車両Ｖが減速するということは前方が見渡せない状態になる可能性が高いため、前方よりも後方の方が姿勢角推定に適しているからである。この処理のため、ＥＣＵ１は、例えば図１４及び図１５に示すようなテーブルデータを参照してもよい。図１４及び図１５は、図７（ａ）のように、前方カメラ２ｆ、右側方カメラ２ｒ、左側方カメラ２ｌ及び後方カメラ２ｂを搭載した自車両Ｖについて設定したものである。図１４に示すテーブルデータは、走行状態に応じて姿勢角推定用画像尤度を設定するためのデータである。図１５に示すテーブルデータは、走行状態に応じて位置推定用画像尤度を設定するためのデータである。図１４によれば、自車両Ｖが減速しており、且つ、直進方向に走行している場合には、前方撮像画像の姿勢角推定用画像尤度を「３」とし、後方撮像画像の姿勢角推定用画像尤度を「７」に設定する。

更に、ＥＣＵ１は、自車両Ｖの旋回半径ρが減少する場合には、後方撮像画像についての姿勢角推定用画像尤度を高めることが望ましい。また、ＥＣＵ１は、自車両Ｖの旋回半径ρが増大する場合には、前方撮像画像についての姿勢角推定用画像尤度を高めることが望ましい。これは、自車両Ｖが旋回する最初は旋回半径ρが減少し自車両Ｖの後方が遠くまで見渡すことが可能であり、自車両Ｖが旋回する最後は旋回半径ρが増大し前方が遠くまで見渡せる可能性が高いことによる。例えば図１４において、自車両Ｖが前進走行している場合であって旋回半径ρが減少している場合には前方撮像画像の姿勢角推定用画像尤度を「３」とし、後方撮像画像の姿勢角推定用画像尤度を「６」に設定する。逆に、旋回半径ρが増大している場合には前方撮像画像の姿勢角推定用画像尤度を「６」とし、後方撮像画像の姿勢角推定用画像尤度を「３」に設定する。ＥＣＵ１は、前回半径ρの大小を判定するための閾値を設定しておくことが望ましい。この閾値は、自車両Ｖの旋回半径ρに応じた後方撮像画像及び前方撮像画像の撮像範囲によって設定される。

更に、ＥＣＵ１は、自車両Ｖの旋回半径がある閾値ＴＨ（単位：ｍ）を超えて減少した場合には、旋回内側方向の側方撮像画像についての位置推定用画像尤度を高めることが望ましい。ＥＣＵ１は、ステップＳ１にて検出したヨーレートセンサからの出力値の符号に基づいて自車両Ｖの旋回方向を判定する。例えば閾値ＴＨを１００［ｍ］とする。自車両Ｖの旋回内側には道路端等の撮像画像と仮想画像とが照合できる構造物が連続して近い状態にあり、旋回内側の側方撮像画像が位置の推定に適しているからである。例えば図１５において、自車両Ｖが前進走行している場合であって左旋回している場合には左側撮像画像の位置推定用画像尤度を「７」とし、右側撮像画像の位置推定用画像尤度を「２」に設定する。逆に、右旋回している場合には右側撮像画像の位置推定用画像尤度を「６」とし、左側撮像画像の位置推定用画像尤度を「３」に設定する。

更に、ＥＣＵ１は、自車両Ｖの横方向への移動方向を撮像する右側撮像画像又は左側撮像画像についての位置推定用画像尤度を高めることが望ましい。左右横方向への移動速度がある閾値ＴＨ（単位：m/s）を超えて大きくなった場合には、移動している方向の横画像が位置推定に適しているとする。例えば閾値ＴＨを０．５［ｍ／ｓ］とする。例えば車線変更時には車線変更方向に撮像画像と仮想画像とが照合できる構造物や道路標示がある可能性が高く、移動側の側方撮像画像が位置の推定に適しているからである。例えば図１５において、自車両Ｖが前進走行している場合であって左方向に車線変更する場合には左側撮像画像の位置推定用画像尤度を「７」とし、右側撮像画像の位置推定用画像尤度を「２」に設定する。逆に、右方向に車線変更している場合には右側撮像画像の位置推定用画像尤度を「７」とし、左側撮像画像の位置推定用画像尤度を「２」に設定する。

次のステップＳ７において、ＥＣＵ１は、ステップＳ６にて位置推定用画像尤度及び姿勢角推定用画像尤度が設定された撮像画像のうち、所定値以上の位置推定用画像尤度又は姿勢角推定用画像尤度の撮像画像からエッジ画像を作成する。この所定値は、０より大きい値であってもよく、任意に設定した値であってもよい。これにより、ＥＣＵ１は、姿勢角推定用エッジ画像Ｅａ及び位置推定用エッジ画像Ｅｐを作成できる。

例えば図４（ａ）に示すように前方カメラ２ｆと左側方カメラ２ｌを搭載した自車両Ｖが一定速で前進している場合、図１２に示すように、前方撮像画像のエッジ画像が姿勢角推定用エッジ画像Ｅａとなる。また、図１３に示すように、左側方撮像画像が位置推定用エッジ画像Ｅｐとなる。他の例では、図７（ａ）に示すように前方カメラ２ｆ、左側方カメラ２ｌ、右側方カメラ２ｒ、後方カメラ２ｂを備えた自車両Ｖが一定速で前進している場合、図１４に示すように、前方撮像画像のエッジ画像と後方撮像画像のエッジ画像とがそれぞれ姿勢角推定用エッジ画像Ｅａとなる。また、自車両Ｖが一定速で前進している場合、図１５に示すように、左側方撮像画像のエッジ画像と右側方撮像画像のエッジ画像とがそれぞれ位置推定用エッジ画像Ｅｐとなる。

本実施形態におけるエッジとは、画素の輝度が鋭敏に変化している箇所を指す。エッジ検出方法としては、例えばCanny法を用いることができる。これに限らず、エッジ検出手法は、他にも微分エッジ検出など様々な手法を使用してもよい。また、ＥＣＵ１は、カメラ群２の撮像画像から、エッジの輝度変化の方向やエッジ近辺のカラーなど抽出することが望ましい。これにより、後述するステップＳ５及びステップＳ６において、３次元地図データベース３にも記録しておいた、これらエッジ以外の情報も用いて位置推定用尤度、姿勢角推定用尤度を設定して、自車両の位置及び姿勢角を推定してもよい。

ステップＳ１の次のステップＳ８において、ＥＣＵ１は、１ループ前のステップＳ６にて推定された車両位置から、今回のステップＳ１で算出したオドメトリ分だけ移動させる。ＥＣＵ１は、移動させた車両位置の近傍で、複数の仮想（予測）位置及び仮想（予測）姿勢角の候補を算出する。ただし、位置姿勢推定アルゴリズムを開始して初めてのループの場合には、ＥＣＵ１は前回の車両位置情報を持っていない。このため、ＥＣＵ１は、車両センサ群４に含まれるＧＰＳ受信機４１からのデータを初期位置情報とする。又は、ＥＣＵ１は、前回に停車時に最後に算出した車両位置及び姿勢角を記憶しておき、初期位置及び姿勢角情報にしてもよい。

ＥＣＵ１は、車両センサ群４の測定誤差や通信遅れによって生じるオドメトリの誤差や、オドメトリで考慮できない車両の動特性を考慮に入れ、車両の位置や姿勢角の真値が取り得る可能性がある複数の仮想位置及び仮想姿勢角の候補を複数個生成する。この仮想位置及び仮想姿勢角の候補は、位置及び姿勢角の６自由度のパラメータに対してそれぞれ誤差の上下限を設定し、この誤差の上下限の範囲内で乱数表等を用いてランダムに設定する。

なお、本実施形態では、仮想位置及び仮想姿勢角の候補を５００個作成する。また、位置及び姿勢角の６自由度のパラメータに対する誤差の上下限は、前後方向，横方向，上下方向、ロール，ピッチ，ヨーの順に±0.05[m]，±0.05[m]，±0.05[m]，±0.5[deg] ，±0.5[deg]，±0.5[deg]とする。この仮想位置及び仮想姿勢角の候補を作成する数や、位置及び姿勢角の６自由度のパラメータに対する誤差の上下限は、車両の運転状態や路面の状況を検出或いは推定して、適宜変更することが望ましい。例えば、急旋回やスリップなどが発生している場合には、平面方向（前後方向，横方向，ヨー）の誤差が大きくなる可能性が高いので、この３つのパラメータの誤差の上下限を大きくし、且つ仮想位置及び仮想姿勢角の候補の作成数を増やすことが望ましい。

次のステップＳ９において、ＥＣＵ１は、ステップＳ８で作成した全ての仮想位置及び仮想姿勢角の候補のそれぞれについて仮想（投影）画像を作成する。このとき、ＥＣＵ１は、例えば３次元地図データベース３に記憶されたエッジ等の３次元位置情報を、仮想位置及び仮想姿勢角から撮像した撮像画像となるよう投影変換して、仮想画像を作成する。この仮想画像は、各仮想位置及び仮想姿勢角の候補が実際の自車両の位置及び姿勢角と合致しているかを評価する評価用画像である。投影変換では、カメラ群２の位置を示す外部パラメータと、カメラ群２の内部パラメータが必要となる。外部パラメータは、車両位置（例えば中心位置）からカメラ群２までの相対位置を予め計測しておくことで、仮想位置及び仮想姿勢角の候補から算出すればよい。また内部パラメータは、予めキャリブレーションをしておけばよい。

ＥＣＵ１は、各仮想位置及び仮想姿勢角の候補について、自車両Ｖに搭載されたカメラ群２から撮像した撮像画像となるような仮想画像を作成する。例えば図４（ａ）に示すようにカメラ群２が前方カメラ２ｆ及び左側方カメラ２ｌを含む場合、各仮想位置及び仮想姿勢角の候補について仮想前方画像及び仮想左側方画像を作成する。また、図７（ａ）に示すようにカメラ群２が前方カメラ２ｆ、左側方カメラ２ｌ、右側方カメラ２ｒ及び後方カメラ２ｂを含む場合、各仮想位置及び仮想姿勢角の候補について仮想前方画像、仮想右側方画像、仮想左側方画像及び仮想後方画像を作成する。これにより、ＥＣＵ１は、姿勢角推定用エッジ画像Ｅａの撮像方向と同じ方向に投影した姿勢推定用仮想画像Ａａを作成し、位置推定用エッジ画像Ｅｐの撮像方向と同じ方向に投影した位置推定用仮想画像Ａｐを作成する。なお、この仮想画像は、ステップＳ２及びステップＳ４で設定されたαｆ、αｓの画角の範囲のみ算出すればよい。

なお、ステップＳ１においてカメラ２により撮像した撮像画像からエッジの輝度変化の方向や、エッジ近辺のカラーなどについても撮像画像から抽出できる場合には、それらについても３次元位置情報を３次元地図データベース３に記録しておき、仮想画像を作成するとなお良い。

ステップＳ７及びステップＳ９の処理の後に、ステップＳ１０を行う。ステップＳ１０において、ＥＣＵ１は、ステップＳ８にて設定した全ての仮想位置及び仮想姿勢角の候補それぞれについて、ステップＳ７にて作成したエッジ画像Ｅａ，Ｅｐと、エッジ画像の撮像方向と同じステップＳ９にて作成した仮想画像Ａａ，Ａｐとをそれぞれ比較する。ＥＣＵ１は、姿勢角推定用エッジ画像Ｅａと姿勢推定用仮想画像Ａａとの一致度に応じた値の姿勢角推定用尤度（単位：なし）を設定する。また、ＥＣＵ１は、位置推定用エッジ画像Ｅｐと位置推定用仮想画像Ａｐとの一致度に応じた値の位置推定用尤度（単位：なし）を設定する。これにより、ＥＣＵ１は、各仮想位置及び仮想姿勢角の候補点について、姿勢角推定用尤度及び位置推定用尤度を設定できる。

具体的には、仮想画像内の画素位置に、姿勢角推定用エッジ画像Ｅａ及び位置推定用エッジ画像Ｅｐの画素がある場合には、姿勢角推定用尤度及び位置推定用尤度を加算する。逆に、仮想画像内の画素位置に、姿勢角推定用エッジ画像Ｅａ及び位置推定用エッジ画像Ｅｐの画素が無い場合には、姿勢角推定用尤度及び位置推定用尤度を加算しない。これにより、各仮想位置及び仮想姿勢角の候補点の撮像方向ごとに、姿勢角推定用尤度及び位置推定用尤度を設定できる。

また、姿勢角推定用エッジ画像Ｅａと姿勢推定用仮想画像Ａａとを比較して、同じ位置に一致した画素がある場合には、姿勢推定用尤度に１．０を加算し、位置推定用尤度に０．２を加算してもよい。一方、位置推定用エッジ画像Ｅｐと位置推定用仮想画像Ａｐとを比較して、同じ位置に一致した画素がある場合には、姿勢推定用尤度に０．２を加算し、位置推定用尤度に１．０を加算してもよい。

また、更に比較に用いた画像の、位置推定用画像尤度と姿勢角推定用尤度を、位置推定用尤度と姿勢角推定用尤度にそれぞれ乗じてもよい。

例えば図４（ａ）に示すように前方カメラ２ｆと左側方カメラ２ｌを搭載した自車両Ｖが一定速で前進している場合、前方撮像画像から得た姿勢角推定用エッジ画像Ｅａと前方に投影した仮想画像との一致度に応じた姿勢角推定用尤度を設定する。また、左側方撮像画像から得た位置推定用エッジ画像Ｅｐと左側方に投影した仮想画像との一致度に応じた位置推定用尤度を設定する。

他の例では、図７（ａ）に示すように前方カメラ２ｆ、左側方カメラ２ｌ、右側方カメラ２ｒ、後方カメラ２ｂを備えた自車両Ｖが一定速で前進している場合、前方撮像画像から得た姿勢角推定用エッジ画像Ｅａと前方に投影した仮想画像との一致度に応じた姿勢角推定用尤度を設定する。更に、後方撮像画像から得た姿勢角推定用エッジ画像Ｅａと後方に投影した仮想画像との一致度に応じた姿勢角推定用尤度を設定する。また、左側方撮像画像から得た位置推定用エッジ画像Ｅｐと左側方に投影した仮想画像との一致度に応じた姿勢角推定用尤度を設定する。更に右側方撮像画像から得た位置推定用エッジ画像Ｅｐと右側方に投影した仮想画像との一致度に応じた姿勢角推定用尤度を設定する。なお、ＥＣＵ１は、１つの仮想位置及び仮想姿勢角について２つの姿勢角推定用エッジ画像Ｅａから得た２つの姿勢角推定用尤度を平均してもよく、姿勢角推定用画像尤度の値に応じて重み付け平均してもよい。同様に、ＥＣＵ１は、１つの仮想位置及び仮想姿勢角について２つの位置推定用エッジ画像Ｅｐから得た２つの位置推定用尤度を平均してもよく、位置推定用画像尤度の値に応じて重み付け平均してもよい。

次のステップＳ１１において、ＥＣＵ１は、ステップＳ１０にて位置推定用尤度及び姿勢角推定用尤度が求められた複数の仮想位置及び仮想姿勢角の候補を用いて、最終的な自車両Ｖの位置及び姿勢角を算出する。ＥＣＵ１は、仮想画像の姿勢角推定用尤度に基づいて当該仮想画像の仮想姿勢角に対する実際の自車両Ｖの姿勢角を推定する（移動物体位置姿勢推定手段）。ＥＣＵ１は、仮想画像の位置推定用尤度に基づいて当該仮想画像の仮想位置に対する実際の自車両Ｖの位置を推定する（移動物体位置姿勢推定手段）。

このとき、ＥＣＵ１は、例えば、位置推定用尤度が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Ｖの実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ１は、姿勢角推定用尤度が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Ｖの実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ１は、位置推定用尤度と姿勢角推定用尤度との和が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Ｖの実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ１は、複数の仮想位置及び仮想姿勢角の候補に対して、各仮想画像の位置推定用尤度に応じた重み付き平均を取って自車両Ｖの実際の位置及び姿勢角を算出してもよい。又は、ＥＣＵ１は、複数の仮想位置及び仮想姿勢角の候補に対して、各仮想画像の姿勢角推定用尤度に応じた重み付き平均を取って自車両Ｖの実際の位置及び姿勢角を算出してもよい。又は、ＥＣＵ１は、各仮想画像の位置推定用尤度と姿勢角推定用尤度の和に応じた重み付き平均を取って自車両Ｖの実際の位置及び姿勢角を算出してもよい。

ＥＣＵ１は、以上のようなステップＳ１乃至ステップＳ１１を繰り返して行うことによって、逐次、自車両Ｖの位置と姿勢角を算出できる。

以上詳細に説明したように、本実施形態として示した移動物体位置姿勢推定装置によれば、走行状態が加速又は一定速の通常状態である場合には、前方撮像画像についての姿勢角推定用画像尤度を高め、側方撮像画像についての位置推定用画像尤度を高める。これにより、移動物体位置姿勢推定装置は、自車両Ｖの姿勢角を前方撮像画像を用いて推定でき、自車両Ｖの位置を側方撮像画像を用いて推定できる。したがって、この移動物体位置姿勢推定装置によれば、位置と姿勢角に対して別々の尤度を考え、位置と姿勢角を別々に調整して推定することができる。具体的には、自車両Ｖから遠い場所でこの２つの映像が一致していた場合には、仮想画像の姿勢角は真値に近いが仮想画像の位置情報の誤差が大きい可能性があるので、姿勢角推定用尤度をより大きく設定できる。逆に、位置推定用尤度はあまり大きく設定しない。一方、自車両Ｖから近い場所でこの２つの映像が一致していた場合には、位置推定用尤度をより大きく設定し、逆に、姿勢角推定用尤度はあまり大きく設定しない。

２つの映像が一致した場所が自車両Ｖから遠い場合には位置の誤差が大きい場合があり、逆に、２つの映像が一致した位置が自車両Ｖから近い場合には姿勢角の誤差が大きい場合があるが、この移動物体位置姿勢推定装置によれば、精度良く移動物体の位置及び姿勢角を推定することができる。

また、この移動物体位置姿勢推定装置によれば、自車両Ｖの位置及び姿勢角を求めるために不要な撮像画像については仮想画像と一致する処理を省くことができるため、演算負荷の低減が実現できる。

更に、この移動物体位置姿勢推定装置によれば、自車両Ｖが減速している場合には、後方撮像画像についての姿勢角推定用画像尤度を高めるので、自車両Ｖの前方に障害物があり広く見渡せる後方撮像画像を用いて自車両Ｖの姿勢角を推定できる。これにより、この移動物体位置姿勢推定装置によれば、見通しの良い後方撮像画像を用いて自車両Ｖの姿勢角を推定できるので、姿勢角の推定精度を向上させることができる。

更にまた、この移動物体位置姿勢推定装置によれば、移動物体の旋回半径が減少する場合には後方撮像画像についての姿勢角推定用画像尤度を高め、移動物体の旋回半径が増大する場合には前方撮像画像の姿勢角推定用画像尤度を高める。これより、この移動物体位置姿勢推定装置によれば、旋回を開始して旋回半径が減少する時には見通しの良い後方撮像画像を用いて自車両Ｖの姿勢角を推定でき、旋回の終了する旋回半径の増大する時には見通しの良い前方撮像画像を用いて自車両Ｖの姿勢角を推定できる。したがって、自車両Ｖの旋回半径が変化しても姿勢角の推定精度を向上させることができる。

更にまた、この移動物体位置姿勢推定装置によれば、自車両Ｖが旋回する場合に、旋回方向の内側方向を撮像する右側撮像画像又は左側撮像画像についての姿勢角推定用画像尤度を高める。これにより、移動物体位置姿勢推定装置は、旋回内側の近距離に存在することが多い道路端や区画線などが連続して撮像可能な旋回内側方向の撮像画像を用いて姿勢角を推定できる。したがって、自車両Ｖが旋回中でも姿勢角の推定精度を向上させることができる。

更にまた、この移動物体位置姿勢推定装置によれば、自車両Ｖが横方向に移動する場合に、横方向への移動方向を撮像する右側撮像画像又は左側撮像画像についての位置推定用画像尤度を高める。これにより、移動物体位置姿勢推定装置は、例えば自車両Ｖが車線変更するときに近距離に存在することが多い道路端や区画線など撮像可能な横移動方向の撮像画像を用いて位置を推定できる。したがって、自車両Ｖが横方向に移動中でも位置の推定精度を向上させることができる。

なお、上述の実施の形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施の形態以外であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計等に応じて種々の変更が可能であることは勿論である。

上述した実施形態では車両を例にしたが、少なくとも１台以上のカメラを搭載した移動体であれば、航空機や船舶などにも適用可能である。

また、上述した実施形態では車両の６自由度の位置（前後方向，横方向，上下方向）及び姿勢角（ロール，ピッチ，ヨー）を求めているが、これに限らない。例えば、サスペンション等を有さない、工場などで用いられる無人搬送車などのように３自由度での位置（前後方向，横方向）及び姿勢角（ヨー）を推定する場合にも、本実施形態が適用可能である。具体的には、このような車両であれば、上下方向の位置と、ロールおよびピッチといった姿勢角は固定であるので、予めこれらのパラメータを計測しておいたり、３次元地図データベース３を参照して求めるようにすればよい。

１ ＥＣＵ
２ カメラ群
２ａ 全方位カメラ
２ｂ 後方カメラ
２ｆ 前方カメラ
２ｌ 左側方カメラ
２ｒ 右側方カメラ
３ ３次元地図データベース
４ 車両センサ群
２１ 撮像範囲
４１ ＧＰＳ受信機
４２ アクセルセンサ
４３ ステアリングセンサ
４４ ブレーキセンサ
４５ 車速センサ
４６ 加速度センサ
４７ 車輪速センサ
４８ その他センサ

Claims (6)

1. 移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定装置であって、
   移動物体の前方を撮像して前方撮像画像を取得する前方撮像手段と、
   移動物体の右側又は左側を撮像して側方撮像画像を取得する側方撮像手段と、
   ３次元地図データを前記前方撮像手段により仮想的に撮像した画像に変換して仮想前方画像を取得すると共に、３次元地図データを前記側方撮像手段により仮想的に撮像した画像に変換して仮想側方画像を取得する仮想画像取得手段と、
   移動物体の走行状態を検出する走行状態検出手段と、
   前記走行状態検出手段により検出された移動物体の前記走行状態が加速又は一定速である場合には、前記前方撮像画像と当該前方撮像画像と同じ方向に投影された前記仮想前方画像との一致度に基づいて、実際の移動物体の姿勢角を推定すると共に、前記側方撮像画像と当該側方撮像画像と同じ方向に投影された前記仮想側方画像との一致度に基づいて、実際の移動物体の位置を推定する移動物体位置姿勢推定手段と
   を有することを特徴とする移動物体位置姿勢推定装置。
2. 移動物体の後方を撮像して後方撮像画像を取得する後方撮像手段を更に有し、
   前記仮想画像取得手段は、３次元地図データを前記後方撮像手段により仮想的に撮像した画像に変換して仮想後方画像を取得し、
   前記移動物体位置姿勢推定手段は、前記走行状態検出手段により移動物体が減速していることが検出された場合に、前記後方撮像画像と当該後方撮像画像と同じ方向に投影された仮想後方画像との一致度に基づいて、実際の移動物体の姿勢角を推定すること
   を特徴とする請求項１に記載の移動物体位置姿勢推定装置。
3. 前記移動物体位置姿勢推定手段は、前記走行状態検出手段により移動物体の旋回半径が減少することが検出された場合には、前記後方撮像画像と前記仮想後方画像との一致度に基づいて、実際の移動物体の姿勢角を推定し、前記走行状態検出手段により移動物体の旋回半径が増大することが検出された場合には、前記前方撮像画像と前記仮想前方画像との一致度に基づいて、実際の移動物体の姿勢角を推定することを特徴とする請求項２に記載の移動物体位置姿勢推定装置。
4. 前記側方撮像手段は、移動物体の右側及び左側の双方を撮像して右側撮像画像及び左側撮像画像を取得し、
   前記移動物体位置姿勢推定手段は、前記走行状態検出手段により検出された移動物体の旋回方向の内側方向を撮像する前記右側撮像画像又は前記左側撮像画像を用いて実際の移動物体の位置を推定することを特徴とする請求項１乃至請求項３の何れか一項に記載の移動物体位置姿勢推定装置。
5. 前記側方撮像手段は、移動物体の右側及び左側の双方を撮像して右側撮像画像及び左側撮像画像を取得し、
   前記移動物体位置姿勢推定手段は、前記走行状態検出手段により検出された移動物体の横方向への移動方向を撮像する前記右側撮像画像又は前記左側撮像画像を用いて実際の移動物体の位置を推定することを特徴とする請求項１乃至請求項４の何れか一項に記載の移動物体位置姿勢推定装置。
6. 前方撮像手段と、側方撮像手段と、３次元地図データと、移動物体の走行状態を検出する走行状態検出手段と、を備え、移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定装置の移動物体位置姿勢推定方法であって、
   前記前方撮像手段が移動物体の前方を撮像して前方撮像画像を取得するステップと、
   前記側方撮像手段が移動物体の右側及び左側を撮像して側方撮像画像を取得するステップと、
   前記３次元地図データを前記前方撮像手段により仮想的に撮像した画像に変換して仮想前方画像を取得すると共に、前記３次元地図データを前記側方撮像手段により仮想的に撮像した画像に変換して仮想側方画像を取得するステップと、
   移動物体の走行状態を検出するステップと、
   前記走行状態検出手段により検出された移動物体の前記走行状態が加速又は一定速である場合には、前記前方撮像画像と当該前方撮像画像と同じ方向に投影された前記仮想前方画像との一致度に基づいて、実際の移動物体の姿勢角を推定すると共に、前記側方撮像画像と当該側方撮像画像と同じ方向に投影された前記仮想側方画像との一致度に基づいて、実際の移動物体の位置を推定するステップと
   を有することを特徴とする移動物体位置姿勢推定装置の移動物体位置姿勢推定方法。
   
   

Images