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

Abstract

【課題】高い精度で移動物体の位置及び姿勢角を推定できる移動物体位置姿勢推定装置及び方法を提供する。
【解決手段】仮想位置及び仮想姿勢角が設定された候補点を複数含む候補点群を設定し（Ｓ３）、候補点ごとに三次元地図データを仮想位置及び仮想姿勢角から撮像した画像に変換し（Ｓ４）、撮像画像と仮想画像との一致度に基づいて、実際の移動物体の姿勢角及び位置を推定し（Ｓ５、Ｓ６）、複数の候補点を再設定する（Ｓ７）。互いに平行する構造物に沿った平行画素群が複数存在し（Ｓ８）、撮像画像と仮想画像との一致度が高い平行画素群と、撮像画像と仮想画像との一致度が低い平行画素群とがある場合（Ｓ１１）、一致度が低い平行画素群から一致度の高い平行画素群に向かう方向に、候補点群を移動させる（Ｓ１２）。
【選択図】図２

Description

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

従来より、パーティクルフィルタを用いて移動体の位置と姿勢角を算出する技術として、下記の特許文献１に記載されたものが知られている。特許文献１に記載された移動体の位置推定方法は、移動体に備えられたエンコーダの測定値を積分して得られた移動体の位置と姿勢角が、レーザセンサによる周囲の障害物やランドマークの観測結果をもとにパーティクルフィルタを用いて算出された移動体の位置と姿勢角からどれだけ補正されたのか算出し、この補正量に応じてパーティクルを散布させる範囲と数を動的に設定している。

特開2010−224755号公報

しかしながら、移動物体の挙動が急激に変化すると、パーティクルを散布する範囲から移動物体がずれてしまう場面が考えられる。例えば、移動物体が自動車である場合、タイヤのスリップ等によって挙動が急変することが考えられる。この状況では、自車両Ｖの位置及び姿勢角が正確に算出できない可能性がある。

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

本発明は、仮想位置及び仮想姿勢角が設定された候補点を複数含む候補点群を設定し、候補点ごとに三次元地図データを仮想位置及び仮想姿勢角から撮像した画像に変換し、撮像画像と仮想画像との一致度に基づいて、実際の移動物体の姿勢角及び位置を推定し、複数の候補点を再設定する。このような本発明は、互いに平行する構造物に沿った平行画素群が複数存在し、撮像画像と仮想画像との一致度が高い平行画素群と、撮像画像と仮想画像との一致度が低い平行画素群とがある場合、一致度が低い平行画素群から一致度の高い平行画素群に向かう方向に、候補点群を移動させる。

本発明によれば、一致度が低い平行画素群から一致度の高い平行画素群に向かう方向に候補点群を移動させるので、喩え移動物体の挙動が急変して候補点群が移動物体の位置及び姿勢角からずれても、候補点群を修正して、高い精度で移動物体の位置及び姿勢角を推定できる。

本発明の実施形態として示す移動物体位置姿勢推定装置の構成を示すブロック図である。 本発明の実施形態として示す位置姿勢推定アルゴリズムの手順を示すフローチャートである。 パーティクルの散布範囲を示す図であり、（ａ）は自車両の位置及び姿勢角の真値がパーティクルの散布範囲に含まれている様子であり、（ｂ）は自車両の位置及び姿勢角の真値がパーティクルの散布範囲に含まれていない様子である。 （ａ）は自車両が左車線を走行しており自車両の位置がパーティクルの散布範囲に含まれている様子を示し、（ｂ）は（ａ）の状況における撮像画像と投影画像とを重ねた状態を示す。 自車両の位置及び姿勢角の真値がパーティクルの散布範囲に含まれていない様子を示す図である。 （ａ）は自車両が左車線を走行しており自車両の位置がパーティクルの散布範囲に含まれていない様子を示し、（ｂ）は（ａ）の状況における撮像画像と投影画像とを重ねた状態を示す。

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

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

カメラ２は、例えばＣＣＤ等の固体撮像素子を用いたものである。カメラ２は、例えば車両のフロント部分に設置され、車両前方を撮像可能な方向に設置される。カメラ２は、所定時間毎に周辺を撮像して撮像画像を取得し、ＥＣＵ１に供給する。

３次元地図データベース３は、例えば路面表示を含む周囲環境のエッジ等の三次元位置情報が記憶されている。各地図情報は、エッジの集合体で定義される。エッジが長い直線の場合には、例えば１ｍ毎に区切られるため、極端に長いエッジは存在しない。直線の場合には、各エッジは、直線の両端点を示す３次元位置情報を持っている。曲線の場合には、各エッジは、曲線の両端点と中央点を示す３次元位置情報を持っている。

特に、３次元地図データベース３には、白線、停止線、横断歩道、路面マーク等の道路標示の他に、道路端を示す縁石や中央分離帯などの段差や、建物等の構造物のエッジ情報も含まれる。３次元地図データベース３は、地図情報のうち互いに略平行となる構造物を表すエッジ情報（平行画素群）の組合せを記憶しておく。なお、この略平行となる構造物の組合せは、カメラ２の画角や画素数から撮影できる範囲を求め、道路の走行車線上からこの範囲内にあるエッジ情報の組合せのみを記憶しておくとなお良い。

本実施形態において、略平行の構造物とは、当該構造物のエッジ同士の姿勢角の差が３軸（ロール，ピッチ，ヨー）共に１［ｄｅｇ］以下のものを指す。また、特にその長さが長い、区画線や道路端を表す縁石等については、その長さを１０［ｍ］で区切って平行であるか否かを判別し、組合せを記憶しておく。

また、３次元地図データベース３には、互いに平行する構造物が道路標示であってもよい。この道路標示は、道路上に描かれた区画線、停止線、ゼブラゾーン、横断歩道が挙げられる。３次元地図データベース３は、互いに平行となる道路標示の組み合わせを記憶しておく。

更に、３次元地図データベース３は、道路端を示す略平行に設置された縁石や中央分離帯の段差のエッジ情報（平行画素群）の組合せを記憶しておくことが望ましい。

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

ＥＣＵ１は、カメラ２により撮像された撮像画像と３次元地図データベース３に記憶された３次元位置情報とを用いて自車両の位置及び姿勢角の推定を行う電子制御ユニットである。なお、ＥＣＵ１は、他の制御に用いるＥＣＵと兼用しても良い。
以下、この移動物体位置姿勢推定装置の動作について、図２の位置姿勢推定アルゴリズムを参照して説明する。なお、本実施形態は、車両の６自由度の位置（前後方向，横方向，上下方向）及び姿勢角（ロール，ピッチ，ヨー）を推定するものとする。また、この図２の位置姿勢推定アルゴリズムは、ＥＣＵ１によって、例えば１００ｍｓｅｃ程度の間隔で連続的に行われる。

先ずステップＳ１において、ＥＣＵ１は、カメラ２により撮像された映像を取得し、当該映像に含まれる撮像画像から、エッジ画像を算出する。本実施形態におけるエッジとは、画素の輝度が鋭敏に変化している箇所を指す。エッジ検出方法としては、例えばCanny法を用いることができる。これに限らず、エッジ検出手法は、他にも微分エッジ検出など様々な手法を使用してもよい。

また、ＥＣＵ１は、カメラ２の撮像画像から、エッジの輝度変化の方向やエッジ近辺のカラーなど抽出することが望ましい。これにより、後述するステップＳ５及びステップＳ６において、３次元地図データベース３にも記録しておいた、これらエッジ以外の情報も用いて尤度を設定して、自車両の位置及び姿勢角を推定してもよい。

次のステップＳ２において、ＥＣＵ１は、車両センサ群４から得られるセンサ値から、１ループ前の位置姿勢推定アルゴリズムのステップＳ２にて算出した時からの車両の移動量であるオドメトリを算出する（挙動検出手段）。なお、位置姿勢推定アルゴリズムを開始して最初のループの場合は、オドメトリをゼロとして算出する。

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

次のステップＳ３において、ＥＣＵ１は、１ループ前に生成された複数のパーティクルの位置及び姿勢角を、ステップＳ２にて算出したオドメトリだけ移動させる。例えば、図３（ａ）に示すように、ＥＣＵ１は、自車両Ｖの位置がＶ１からＶ２まで移動するオドメトリが算出された場合、パーティクルＰ１０〜Ｐ１５をパーティクルＰ２０〜Ｐ２５に移動させる。

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

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

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

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

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

ステップＳ５において、ＥＣＵ１は、ステップＳ３で設定した複数のパーティクルそれぞれにおいて、ステップＳ１で作成したエッジ画像と、ステップＳ４で作成した仮想画像とを比較する。ＥＣＵ１は、比較した結果に基づいて、各パーティクルについて、尤度LIKE（単位：なし）を設定する。この尤度LIKEとは、それぞれのパーティクルに設定された仮想位置及び仮想姿勢角が、どれぐらい実際の自車両Ｖの位置及び姿勢角に対して尤もらしいかを示す指標である。ＥＣＵ１は、仮想画像とエッジ画像との一致度が高いほど、尤度LIKEが高くなるように設定する。

具体的には、仮想画像上のエッジが存在する画素座標位置に、エッジ画像上のエッジが存在しており、仮想画像内の画素とエッジ画像上のエッジとが一致する場合には、このパーティクルの尤度LIKEに１を加算する。ＥＣＵ１は、仮想画像に含まれる全画素（評価点）について尤度LIKEに１を加算するか否かを判定する。この判定処理を仮想画像の全画素について行って得られた総和（一致した評価点の数）が、パーティクルの尤度LIKEとなる。なお、尤度LIKEの算出方法は、その他の方法であってもよく、本実施形態は上記の算出方法に限定するものではない。ＥＣＵ１は、すべてのパーティクルについて尤度LIKEを算出した後に、全結果を用いて尤度LIKEの合計値が１になるよう尤度LIKEを正規化し、ステップＳ６に処理を進める。

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

このとき、ＥＣＵ１は、例えば、尤度LIKEが最も高いパーティクルを用いて、当該パーティクルの仮想位置及び仮想姿勢角を、自車両Ｖの実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ１は、複数のパーティクルに対して、複数のパーティクルの尤度LIKEの重み付き平均を取って自車両Ｖの実際の位置及び姿勢角を算出してもよい。

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

次のステップＳ７において、ＥＣＵ１は、各パーティクルのリサンプリングを、尤度LIKEに基づいて行う。すなわち、複数のパーティクル（候補点）についての複数の仮想画像の一致度（尤度LIKE）に基づいて、複数のパーティクルを再設定する（候補点再設定手段）。

具体的には、ＥＣＵ１は、総パーティクル数の500個に各パーティクルの尤度LIKEを乗じた値から小数点以下を切り捨てた値から、１を引いた値だけ、各パーティクルの複製を作成する。すなわち、尤度LIKEの高いパーティクルについては多数のパーティクルの複製を作成し、尤度LIKEの低いパーティクルについては少ないパーティクルの複製しか作成しない又はパーティクルを消滅させる。なお、小数点以下を切り捨てて、各パーティクルの複製を行っているため、パーティクルの総数が図２の位置姿勢推定アルゴリズムを繰り返すたびに減ってしまう。このため、ＥＣＵ１は、総パーティクル数が500個に満たない場合は、ステップＳ６において算出した仮想位置及び仮想姿勢角が設定されたパーティクルを追加して総パーティクル数が500個になるようにする。

また、ステップＳ７において、ＥＣＵ１は、既存技術（例えば特開2010−224755の技術）を用いてリサンプリングを行ってもよい。この場合、ＥＣＵ１は、レーザセンサによる周囲の障害物やランドマークの観測結果に基づいてパーティクルフィルタを用いて算出した移動体の位置と姿勢角を、ステップＳ６において算出した自車両Ｖの位置及び姿勢角に置き換えて、パーティクルをリサンプリングすればよい。すなわち、移動体に備えられたエンコーダの測定値を積分して得られた移動体の位置及び姿勢角が、ステップＳ６において算出した自車両Ｖの位置及び姿勢角によってどれだけ補正されたのか算出する。この補正量に応じて、ＥＣＵ１は、パーティクルをリサンプリングさせる範囲と数を動的に設定する。

次のステップＳ８において、ＥＣＵ１は、３次元地図データベース３を参照して、ステップＳ６において算出した自車両Ｖの位置及び姿勢角から、カメラ２の撮像範囲内に略平行に存在するエッジの組合せが含まれているか否かを判定する。すなわち、撮像画像内に互いに平行する構造物に沿った平行画素群が複数存在するか否かを判定する。撮像範囲内に略平行に存在するエッジの組合せが含まれている場合にはステップＳ９に処理を進め、含まれていない場合には処理を終了する。例えば図４（ｂ）に示すように、区画線についてのエッジが複数存在する場合、ＥＣＵ１は、撮像範囲内に略平行に存在するエッジの組合せが含まれていると判定する。

次のステップＳ９において、ＥＣＵ１は、ステップＳ６にて算出した自車両Ｖの位置及び姿勢角から撮像した投影画像を作成する。なお、ＥＣＵ１は、ステップＳ４と同じ処理を行う。

次のステップＳ１０において、ＥＣＵ１は、撮像画像内に略平行に存在するエッジ（平行画素群）の全てについて、ステップＳ１で作成したエッジ画像と、ステップＳ９で作成した投影画像との一致度を算出する。具体的には、３次元地図データベース３において略平行に存在する投影画像の平行画素群と、エッジ画像に含まれる平行画素群とを対比し、どの程度の割合で、双方の平行画素群が一致するかを算出する。これにより、ＥＣＵ１は、ステップＳ８において存在すると判定された平行画素群ごとに、投影画像と一致した割合（一致割合）LIKE_Pを算出する。なお、この割合LIKE_Pの最大値は１となる。例えば図４（ｂ）に示すように、４本の区画線Ｌ１〜Ｌ４についてのエッジのそれぞれと、４本の区画線Ｌ１〜Ｌ４に対応した投影画像内の平行画素群Ｐ１〜Ｐ４とが一致する割合LIKE_Pを算出する。

次のステップＳ１１において、ＥＣＵ１は、略平行に存在する平行画素群のうち、一致割合LIKE_Pが他の平行画素群の一致割合LIKE_Pと比して低い平行画素群があるか否かを判定する。具体的には、複数の平行画素群のうち、例えば最も高い一致割合LIKE_Pの３０％以下の一致割合LIKE_Pしかない平行画素群がある場合、一致割合LIKE_Pが低い平行画素群があるものと判定する。また、ステップＳ１で作成したエッジ画像にノイズがある場合を考慮して、過去に求めた一致割合LIKE_Pとの加重平均を用いた値と、ステップＳ１０にて算出した一致割合LIKE_Pとを比較してもよい。他と比して低い一致割合LIKE_Pの平行画素群がある場合にはステップＳ１２に処理を進め、無い場合には処理を終了する。

例えば図６（ａ）に示すように、撮像画像内の区画線Ｌ１〜Ｌ４の平行画素群に対して、
投影画像の投影点群Ｐ１err〜Ｐ４errが位置している場合には、ステップＳ１１の判定は「ＮＯ」となる。

一方、図６（ｂ）に示すように、撮像画像内の区画線Ｌ１〜Ｌ４の平行画素群に対して、投影画像内に区画線Ｌ１〜Ｌ４に対応しない投影点群Ｐ１errが位置している場合ある。この場合、区画線Ｌ１〜Ｌ３と投影点群Ｐ２err〜Ｐ４errとの一致割合LIKE_Pは高くなるが、投影点群Ｐ１errはエッジ画像に対して低い一致割合LIKE_Pとなる。したがって、ＥＣＵ１は、図６（ｂ）のような場面では、ステップＳ１１の判定が「ＹＥＳ」となる。

ステップＳ１１において、ＥＣＵ１は、一致割合LIKE_Pが低い平行画素群と自車両Ｖとの間に障害物があるかを検出し（障害物検出手段）、障害物がある場合には、処理を終了することが望ましい。これにより、ステップＳ１２の複数のパーティクルを移動させることを禁止する。

このとき、ＥＣＵ１は、ステップＳ１１にて撮像されたカメラ２の撮影画像を用いて、モーションステレオ法などを用いて、他の車両や自動二輪車などの障害物の位置を算出する。ＥＣＵ１は、算出した障害物の位置が、自車両Ｖと、一致割合LIKE_Pの低い平行画素群との間であるかを判定する。障害物が自車両Ｖと一致割合LIKE_Pの低い平行画素群との間にある場合、障害物によって平行画素群の一致割合LIKE_Pが低くなったものとして、位置姿勢推定アルゴリズムを終了する。

なお、例えばレーザレンジファインダを搭載した自車両Ｖであれば、これを用いて障害物を検出してもよい。又は、障害物の大きさや形状も検出して、この一致割合LIKE_Pが低い平行画素群がこの障害物によってどの程度見えにくくなっているか判断し、この障害物によって一致割合LIKE_Pが低くなっているか判断することが望ましい。

次のステップＳ１２において、ＥＣＵ１は、一致割合LIKE_Pの低い平行画素群から一致割合LIKE_Pの高い平行画素群に向かう方向に、複数のパーティクルを移動させる（候補点群移動手段）。すなわち、ＥＣＵ１は、複数のパーティクルの存在分布位置を、一致割合LIKE_Pが低い平行画素群から一致割合LIKE_Pが高い平行画素群に向かう方向に移動させる。

具体的には、ステップＳ７においてリサンプリングした各パーティクルを、ステップＳ１１で一致割合LIKE_Pが低いと判断された平行画素群と略平行に存在し、且つ、当該平行画素群に最も近い位置にあって一致割合LIKE_Pが低くない平行画素群の方向に、全パーティクルを移動させる。

例えば、ＥＣＵ１は、一致割合LIKE_Pが高いと判断された平行画素群の方向に、例えば０．５ｍだけ平行移動させる。例えば図６（ｂ）に示すように、各パーティクルの存在分布位置を、一致割合LIKE_Pの低い平行画素群Ｐ１errから一致割合LIKE_Pの高い平行画素群Ｐ２errに向かう方向に移動させる。

上記のように所定距離ずつ複数のパーティクルを移動させる場合、位置姿勢推定アルゴリズムを１回実行しても、自車両Ｖの位置及び姿勢角の真値がパーティクルの散布範囲に含まれない可能性がある。しかし、図２の位置姿勢推定アルゴリズムを繰り返し実行することによって、複数のパーティクルの散布範囲が自車両Ｖの位置及び姿勢角の真値に近づく。複数回に亘って位置姿勢推定アルゴリズムを繰り返すと、複数のパーティクルの散布範囲が、自車両Ｖの位置及び姿勢角の真値に含まれる。これによって、ＥＣＵ１は、自車両Ｖの位置及び姿勢角の真値が推定できる状態になる。

ここで、平行画素群の組合せの中に、一致割合LIKE_Pの低い平行画素群が複数あった場合、当該低い一致割合LIKE_Pの平行画素群のうち最も一致割合LIKE_Pが低い平行画素群を選択する。そして、選択した平行画素群から最も近い位置にある一致割合LIKE_Pが低くない平行画素群の方向に移動させる。

また、平行画素群の組合せが複数あり、一致割合LIKE_Pの低い平行画素群がある場合、所定の優先順位に従ってパーティクルの散布範囲を移動させてもよい。例えば、道路端を示す段差を表す平行画素群の組合せ、区画線を表す平行画素群の組合せ、その他の平行画素群の組合せといった優先順位を設定しておく。ＥＣＵ１は、優先順位の最も高い平行画素群の組み合わせを選択して、一致割合LIKE_Pの低い平行画素群から一致割合LIKE_Pの高い平行画素群へパーティクルの散布範囲を移動させる。

一方で、パーティクルの散布範囲を複数の方向に移動させてもよい。互いに略直行して存在する平行画素群の組合せがある場合、例えば停止線を表す複数の平行画素群と、区画線を表す複数の平行画素群がある場合、当該複数の平行画素群同士は略直行する位置関係となる。この場合、ＥＣＵ１は、それぞれの平行画素群の組み合わせに基づいて判定された方向に、パーティクルの散布範囲を移動させることができる。

ＥＣＵ１は、ステップＳ１２にてパーティクルの散布範囲を移動させる距離を演算することが望ましい。

ＥＣＵ１は、一致割合LIKE_Pが低い平行画素群から最も近い位置に存在する一致割合LIKE_Pの高い平行画素群までの距離だけ、パーティクルの散布範囲を移動させてもよい。すなわい、各パーティクルを移動させる距離を、一致割合LIKE_Pが低いと判断された平行画素群と略平行に存在し、且つ最も近い位置にある一致割合LIKE_Pが低くない平行画素群と、一致割合LIKE_Pが低い平行画素群との距離だけ移動させる。この距離は、例えば３次元地図データベース３に含まれる投影画像に含まれた投影点の位置情報を参照することによって演算することができる。

ＥＣＵ１は、自車両Ｖの挙動の履歴に基づいてパーティクルの散布範囲を移動させる方向及び距離を推定してもよい。このために、ＥＣＵ１は、一致割合LIKE_Pが低い平行画素群がないと判断された時点が、位置姿勢推定アルゴリズムを何回繰り返す前までだったのかを記憶しておく。最後に一致割合LIKE_Pが低い平行画素群がないと判断された時から現在までの間に、想定される通信遅れや車両センサ群４の測定精度からオドメトリに積算されると想定される誤差を求めて、パーティクルの散布範囲を移動させる距離を推定する。また、この時、自車両Ｖの各車輪がスリップやロックしていたことを検出していた場合には、パーティクルの散布範囲の移動距離を更に長めに設定するとなお良い。

以上詳細に説明したように、本実施形態として示した移動物体位置姿勢推定装置によれば、互いに平行する構造物に沿った平行画素群が複数存在し、一致割合LIKE_Pの低い平行画素群と一致割合LIKE_Pの高い平行画素群がある場合に、一致割合LIKE_Pの低い平行画素群から一致割合LIKE_Pの高い平行画素群に向かう方向にパーティクルの散布範囲を移動できる。したがって、この移動物体位置姿勢推定装置によれば、自車両Ｖの挙動が急変して例えば図６（ａ）、（ｂ）のようにパーティクルの散布範囲が実際の自車両Ｖからずれても、パーティクルの散布範囲を修正して、図４（ａ）、（ｂ）のような状態に復帰することができる。これにより、移動物体位置姿勢推定装置によれば、高い精度で移動物体の位置及び姿勢角を推定できる。

道路環境によってはタイヤのスリップ等で車両挙動が急変して、喩え自車両Ｖの位置や姿勢角がパーティクルの散布範囲から外れてしまった場合でも、平行に存在するランドマークのうちの一部はカメラ２で認識できている可能性が高い。投影画像と撮像画像とを比較して自車両Ｖの位置と姿勢角を算出するＥＣＵ１は、当該算出処理を利用して、パーティクルの散布範囲を移動させることができる。

本実施形態によって奏する効果を具体的に説明する。

通常状態で直進走行している場合、図３（ａ）のように自車両位置がＶ１からＶ２に変化すると、自車両位置がＶ１の近傍で散布されていた複数のパーティクルＰ１０〜Ｐ１５は、自車両位置Ｖ２を略中心とした範囲ＰにパーティクルＰ２０〜Ｐ２５を散布できる。この状態では、車両のオドメトリ等によって自車両の挙動が予測できるため、自車両の位置及び姿勢角の真値がパーティクルの散布範囲Ｐに含まれている。なお、図３（ａ）は、パーティクルフィルタにおける、予測ステップを模式的に表している（確率ロボティクス・第4章3節（（著）Sebastian Thrun／Wolfram Burgard／Dieter Fox，（訳）上田隆一，（発行）(株)毎日コミュニケーションズ））。

しかし、自車両の挙動が急変して、図３（ｂ）のような自車両位置Ｖ２となった場合には、自車両位置Ｖ２に対して誤った範囲ＰerrにパーティクルＰ２０〜Ｐ２５を散布してしまう。このような状態になる理由は、自車両の挙動が予測できないことによる。この状態では、自車両の位置及び姿勢角の真値がパーティクルの散布範囲Ｐerrに含まれなくなってしまう。したがって、この状態では、パーティクルフィルタによる車両の位置や姿勢角の推定が困難になるという問題がある。

例えば図４（ａ）に示すように、区画線Ｌ１〜Ｌ４によって区分された片側３車線を含む道路を自車両Ｖが走行している場面では、自車両Ｖの位置がパーティクルの散布範囲Ｐに含まれる。この状況においては、カメラ映像から区画線を抽出して、道路の区画線の３次元位置が記録された三次元地図と一致するように自車両Ｖの位置と姿勢角を算出する。このとき、図４（ｂ）に示すように、仮想画像内の仮想点群Ｐ１〜Ｐ４は、撮像画像内の区画線Ｌ１〜Ｌ４に沿ったものである。

しかし、自車両Ｖの急激な挙動変化によって、例えば図５のように自車両Ｖの位置とは異なる位置にパーティクルの散布範囲Ｐerrが設定されたとする。この場合には、図６（ａ）に示すように、実際の自車両Ｖの位置とは異なるパーティクルの散布範囲Ｐerr内に自車両位置Ｖerrが存在すると判断される。この状態で撮像画像と仮想画像とを比較して自車両Ｖの位置及び姿勢角を算出しようとすると、自車両Ｖの前方向に左車線が存在し右側に中央車線、右車線が存在する撮像画像に対して、仮想画像は、自車両Ｖの前方向に中央車線が存在し左側に左車線、右側に右車線が存在するものとなってしまう。すなわち、仮想画像は、図６（ｂ）に示すように、実際の撮像画像とは誤っている段差Ｇ及び区画線Ｌ１〜Ｌ３上に仮想点群Ｐ１err〜Ｐ４errが現れる。この状態では、仮想画像内の仮想点群Ｐ２err〜Ｐ４errは高い一致度で撮像画像と一致するが、仮想画像内の仮想点群Ｐ１errは撮像画像とは一致しないという所謂局所解となってしまう。この状況では、自車両Ｖの位置及び姿勢角が正確に算出できない可能性がある。しかし、本実施形態の移動物体位置姿勢位置姿勢推定装置は、一致割合LIKE_Pの低い平行画素群から一致割合LIKE_Pの高い平行画素群に向かう方向にパーティクルの散布範囲を移動できる。したがって、この移動物体位置姿勢推定装置によれば、自車両Ｖの挙動が急変して例えば図６（ａ）、（ｂ）のようにパーティクルの散布範囲が実際の自車両Ｖからずれても、パーティクルの散布範囲を修正して、図４（ａ）、（ｂ）のような状態に復帰することができる。これにより、移動物体位置姿勢推定装置によれば、高い精度で移動物体の位置及び姿勢角を推定できる。

また、この移動物体位置姿勢推定装置によれば、一致割合LIKE_Pが低い平行画素群から最も近い位置に存在する一致割合LIKE_Pの高い平行画素群までの距離だけ、パーティクルの散布範囲を移動させることができる。例えば図６（ｂ）のような場合には、一致割合LIKE_Pが低い平行画素群Ｐ１errを、一致割合LIKE_Pが高い平行画素群Ｐ２errまでの距離だけ移動させることができる。移動物体位置姿勢推定装置は、移動距離を適切にすることができるので、より速やかに自車両Ｖの位置及び姿勢角の真値がパーティクルの散布領域に含まれるようにできる。

なお、例えば図４乃至図６のようなシーンにおいてパーティクルの散布範囲が１車線ではなく、２車線ずれるような場合が考えられる。このような場合でも、上記のように移動距離を演算することによって、位置姿勢推定アルゴリズムを１回適用する毎に、自車両Ｖの位置及び姿勢角の真値が含まれる方向に、パーティクルの散布範囲を１車線ずつ近づけていくことができる。従って、位置姿勢推定アルゴリズムを２回行えば、自車両Ｖの位置及び姿勢角の真値がパーティクルの散布範囲に含まれるようにできる。

更にまた、この移動物体位置姿勢推定装置は、自車両Ｖの挙動を検出し、自車両Ｖの挙動の履歴に基づいてパーティクルの散布範囲を移動させる方向及び距離を推定できる。例えば車速やヨーレート等の車両挙動の過去の履歴から、パーティクルの散布範囲を移動させる距離を算出できる。これにより、移動物体位置姿勢推定装置によれば、より速やかに自車両Ｖの位置及び姿勢角の真値がパーティクルの散布範囲に含まれるようにできる。

更にまた、この移動物体位置姿勢推定装置によれば、互いに平行する構造物として三次元地図データに含まれた道路標示とし、道路標示に沿って設定されたパーティクルの散布範囲を移動させることができる。また、この移動物体位置姿勢推定装置によれば、互いに平行する構造物として三次元地図データに含まれた道路標示とし、道路標示に沿って設定されたパーティクルの散布範囲を移動させることができる。ここで、道路環境上に存在する平行なランドマークは多々存在するが、その全てについて演算を行うと演算負荷が高くなってしまう。そこで、平行に存在するランドマークとして、多くの道路環境に存在する道路標示又は道路端の段差の何れか、又は、両方のみについて位置姿勢推定アルゴリズムを行う。これにより、この移動物体位置姿勢推定装置によれば、演算負荷が過大とならず多くの道路環境において位置姿勢推定アルゴリズムを実施できる。

更にまた、この移動物体位置姿勢推定装置によれば、一致割合LIKE_Pが低い平行画素群と自車両Ｖとの間に障害物が検出された場合に、パーティクルの散布範囲を移動させることを禁止する。これにより、この移動物体位置姿勢推定装置によれば、障害物によって平行に存在するランドマークの組合せの一部で一致割合LIKE_Pが低下していることを識別して、誤ってパーティクルの散布範囲を間違った方向に移動させないようにできる。

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

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

１ ＥＣＵ
２ カメラ
３ ３次元地図データベース
４ 車両センサ群
４１ ＧＰＳ受信機
４２ アクセルセンサ
４３ ステアリングセンサ
４４ ブレーキセンサ
４５ 車速センサ
４６ 加速度センサ
４７ 車輪速センサ
４８ 他センサ

Claims (7)

1. 移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定装置であって、
   前記移動物体周辺を撮像して、撮像画像を取得する撮像手段と、
   仮想位置及び仮想姿勢角が設定された候補点を複数含む候補点群を設定し、前記候補点ごとに三次元地図データを仮想位置及び仮想姿勢角から撮像した画像に変換して仮想画像を取得する仮想画像取得手段と、
   前記撮像手段により取得された撮像画像と前記仮想画像取得手段により取得された仮想画像とを比較して、前記撮像画像と前記仮想画像との一致度に基づいて、当該仮想画像の仮想姿勢角に対する実際の移動物体の姿勢角を推定すると共に、当該仮想画像の仮想位置に対する実際の移動物体の位置を推定する移動物体位置姿勢推定手段と、
   複数の仮想画像の一致度に基づいて、前記複数の候補点を再設定する候補点再設定手段と、
   前記撮像画像内に互いに平行する構造物に沿った平行画素群が複数存在し、前記撮像画像と前記仮想画像との一致度が高い平行画素群と、前記撮像画像と前記仮想画像との一致度が低い平行画素群とがある場合、前記一致度が低い平行画素群から前記一致度の高い平行画素群に向かう方向に、前記候補点群を移動させる候補点群移動手段と
   を備えることを特徴とする移動物体位置姿勢推定装置。
2. 前記候補点群移動手段は、前記一致度が低い平行画素群から最も近い位置に存在する前記一致度の高い平行画素群までの距離だけ、前記候補点群を移動させることを特徴とする請求項１に記載の移動物体位置姿勢推定装置。
3. 移動物体の挙動を検出する挙動検出手段を更に備え、
   前記候補点群移動手段は、移動物体の挙動の履歴に基づいて前記候補点群を移動させる方向及び距離を推定することを特徴とする請求項２に記載の移動物体位置姿勢推定装置。
4. 互いに平行する構造物が、前記三次元地図データに含まれた道路標示であり、
   前記候補点群移動手段は、前記道路標示に沿って設定された候補点群を移動させること
   を特徴とする請求項１乃至請求項３の何れか一項に記載の移動物体位置姿勢推定装置。
5. 互いに平行する構造物が、前記三次元地図データに含まれた道路端を示す略平行に設置された段差であり、
   前記候補点群移動手段は、前記段差に沿って設定された候補点群を移動させること
   を特徴とする請求項１乃至請求項３の何れか一項に記載の移動物体位置姿勢推定装置。
6. 移動物体周辺の障害物を検出する障害物検出手段を更に備え、
   前記候補点群移動手段は、前記障害物検出手段により前記一致度が低い平行画素群と移動物体との間に障害物が検出された場合に、前記候補点群を移動させることを禁止することを特徴とする請求項１乃至請求項５の何れか一項に記載の移動物体位置姿勢推定装置。
7. 移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定方法であって、
   仮想位置及び仮想姿勢角が設定された候補点を複数含む候補点群を設定するステップと、
   移動物体周辺を撮像した撮像画像と前記候補点ごとに三次元地図データを仮想位置及び仮想姿勢角から撮像した画像に変換仮想画像とを比較するステップと、
   前記撮像画像と前記仮想画像との一致度に基づいて、当該仮想画像の仮想姿勢角に対する実際の移動物体の姿勢角を推定すると共に、当該仮想画像の仮想位置に対する実際の移動物体の位置を推定するステップと、
   複数の仮想画像の一致度に基づいて、前記複数の候補点を再設定するステップと、
   前記撮像画像内に互いに平行する構造物に沿った平行画素群が複数存在し、前記撮像画像と前記仮想画像との一致度が高い平行画素群と、前記撮像画像と前記仮想画像との一致度が低い平行画素群とがある場合、前記一致度が低い平行画素群から前記一致度の高い平行画素群に向かう方向に、前記候補点群を移動させるステップと
   を有することを特徴とする移動物体位置姿勢角推定方法。

Images