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

Abstract

移動物体位置姿勢推定装置は、撮像部２と、対比画像取得部と、尤度設定部１２と、移動物体位置姿勢推定部１３とを備える。撮像部２は、移動物体周辺を撮像して、撮像画像を取得する。対比画像取得部は、予め定めた位置及び姿勢角から見た時の対比画像を取得する。尤度設定部１２は、撮像画像と対比画像とを比較し、撮像画像内の遠方位置画素と対比画像内の遠方位置画素とが一致した場合には対比画像の姿勢角尤度を高く設定し、撮像画像内の近傍位置画素と対比画像内の近傍位置画素とが一致した場合には対比画像の位置尤度を高く設定する。移動物体位置姿勢推定部１３は、姿勢角尤度が高く設定された対比画像の姿勢角に基づいて移動物体の姿勢角を推定し、位置尤度が高く設定された対比画像の位置に基づいて移動物体の位置を推定する。

Description

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

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

特開2009-199572号公報

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

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

本発明の第１態様に係わる移動物体位置姿勢推定装置は、移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定装置であって、撮像部と、対比画像取得部と、尤度設定部と、移動物体位置姿勢推定部とを備える。撮像部は、移動物体周辺を撮像して、撮像画像を取得する。対比画像取得部は、予め定めた位置及び姿勢角から見た時の対比画像を取得する。尤度設定部は、撮像部により取得された撮像画像と対比画像取得部により取得された対比画像とを比較し、撮像画像内の遠方位置画素と対比画像内の遠方位置画素とが一致した場合には対比画像の姿勢角尤度を高く設定し、撮像画像内の近傍位置画素と対比画像内の近傍位置画素とが一致した場合には対比画像の位置尤度を高く設定する。移動物体位置姿勢推定部は、尤度設定部により姿勢角尤度が高く設定された対比画像の姿勢角に基づいて移動物体の姿勢角を推定し、尤度設定部により位置尤度が高く設定された対比画像の位置に基づいて移動物体の位置を推定する。

本発明の第２態様に係わる移動物体位置姿勢推定方法は、移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定方法である。移動物体位置姿勢推定方法では、移動物体周辺を撮像した撮像画像と予め定めた位置及び姿勢角から見た時の対比画像とを比較する。撮像画像内の遠方位置画素と対比画像内の遠方位置画素とが一致した場合には対比画像の姿勢角尤度を高く設定する。撮像画像内の近傍位置画素と対比画像内の近傍位置画素とが一致した場合には対比画像の位置尤度を高く設定する。姿勢角尤度が高く設定された対比画像の姿勢角に基づいて移動物体の姿勢角を推定する。位置尤度が高く設定された対比画像の位置に基づいて移動物体の位置を推定する。

図１は、本発明の第１実施形態に係わる移動物体位置姿勢推定装置の構成を示すブロック図である。 図２（ａ）は撮像部により取得された撮像画像を示し、図２（ｂ）は図２（ａ）の撮像画像の中からエッジを抽出したエッジ画像を示し、図２（ｃ）は仮想画像取得部により取得された仮想画像、図２（ｄ）は図２（ｃ）における仮想位置が右方向にずれた場合の仮想画像を示す平面図である。 図３は、本発明の第１実施形態に係わる移動物体位置姿勢推定装置の動作手順の一例を示すフローチャートである。 図４は、パーティクル（候補点）を移動させる動作を説明するための平面図である。 図５は、車両からの距離と位置尤度like_pとの関係の一例を示すグラフである。 図６は、車両からの距離と姿勢角尤度like_aとの関係の一例を示すグラフである。 図７は、本発明の第２実施形態に係わる移動物体位置姿勢推定装置の動作手順の一例を示すフローチャートである。

以下、本発明の実施の形態について図面を参照して説明する。
（第１実施形態）
図１を参照して、本発明の第１実施形態に係わる移動物体位置姿勢推定装置の構成を説明する。第１実施形態に係わる移動物体位置姿勢推定装置は、ＥＣＵ（Engine Control Unit）１と、カメラ（撮像部の一例）２と、３次元地図データベース３と、車両センサ群４とを備える。車両センサ群４には、ＧＰＳ受信機４１、アクセルセンサ４２、ステアリングセンサ４３、ブレーキセンサ４４、車速センサ４５、加速度センサ４６、車輪速センサ４７、及び、ヨーレートセンサ等のその他センサ４８が含まれる。なお、ＥＣＵ１は、実際にはＲＯＭ、ＲＡＭ、演算回路等にて構成されている。ＥＣＵ１は、ＲＯＭに格納された移動物体位置姿勢推定用のコンピュータプログラムに従って処理を実行することによって、仮想画像取得部１１（対比画像取得部の一例）、尤度設定部１２、移動物体位置姿勢推定部１３として機能する。

カメラ２は、例えばＣＣＤ等の固体撮像素子を用いたものである。以下、移動物体が車両である場合について説明する。カメラ２は、例えば車両のフロント部分（位置）に、車両前方を撮像可能な方向（姿勢角）で設置される。カメラ２は、所定時間毎に車両周辺を撮像して撮像画像を取得する。取得された撮像画像は、ＥＣＵ１に供給される。

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

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

ＥＣＵ１は、カメラ２により撮像された撮像画像と３次元地図データベース３に記憶された３次元位置情報とを用いて車両の位置及び姿勢角の推定を行う電子制御ユニットである。なお、ＥＣＵ１は、他の制御に用いるＥＣＵと兼用しても良い。

特に、移動物体位置姿勢推定装置は、カメラ２により撮像した撮像画像と、予め定めた位置及び姿勢角から見た時の対比画像とを比較して、車両の位置及び姿勢角を推定する。なお、実施形態では「予め定めた位置及び姿勢角から見た時の対比画像」の一例として、「三次元地図データを仮想位置及び仮想姿勢角から撮像した画像に変換した仮想画像」を用いる。ここで、図２（ａ）に示すような撮像画像が得られ、図２（ｂ）のようなエッジ画像が得られたとする。一方、３次元位置情報をカメラ２の位置及び姿勢角に投影した仮想画像が図２（ｃ）のようになったとする。図２（ｂ）の撮像画像と図２（ｃ）の仮想画像とを比較すると、遠方位置（Ａ）及び近傍位置（Ｂ）の双方で一致しているため、仮想画像を生成した仮想位置及び仮想姿勢角が、自車両の位置及び姿勢角に相当すると推定できる。しかし、仮想位置が右方向に位置ずれした場合には、仮想画像は、図２（ｄ）に示すようになる。この場合、図２（ａ）の撮像画像と図２（ｄ）の撮像画像とを比較すると、遠方位置（Ａ）は一致しているが、近傍位置（Ｂ）は大きくずれてしまう。逆に、図２（ｃ）の仮想画像の仮想姿勢角をずらし（図示せず）、図２（ａ）の撮像画像と比較すると、近傍位置（Ｂ）は一致するが、遠方位置（Ａ）は大きくずれる。

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

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

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

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

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

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

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

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

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

ステップＳ３において、仮想画像取得部１１は、所謂パーティクルフィルタを用いて複数の仮想位置及び仮想姿勢角の候補を設定してもよい。このとき、仮想画像取得部１１は、１ループ前のステップＳ６で生成された複数の仮想位置及び仮想姿勢角の候補である各パーティクル（候補点）の位置及び姿勢角を、オドメトリ分だけ移動させる。具体的には、図４に示すように、１ループ前に推定されていた車両Ｖ（ｔ１）の位置及び姿勢角のパーティクルＰと周囲のパーティクルＰ１〜Ｐ５を、オドメトリ分だけ移動させる。この結果、仮想画像取得部１１は、新たな車両Ｖ（ｔ２）の位置及び姿勢角を推定するためのパーティクルＰ１０〜１５を設定する。これにより、仮想画像取得部１１は、今回における複数の仮想位置及び仮想姿勢角の候補を算出する。すなわち、各パーティクルの位置及び姿勢角を、複数の仮想位置及び仮想姿勢角の候補とする。なお、車両センサ群４の測定誤差や通信遅れによって生じるオドメトリの誤差や、オドメトリで考慮できない車両の動特性を考慮に入れて、各パーティクルの位置情報及び姿勢角情報をオドメトリ分だけ移動させ、その後に、上記のように位置及び姿勢角の６自由度のパラメータに対する誤差の上下限の範囲内で乱数表等を用いてランダムに変化させることが更に好ましい。

ただし、位置姿勢推定アルゴリズムを開始して初めてのループの場合には、各パーティクルは位置情報及び姿勢角情報を持っていない。このため、車両センサ群４に含まれるＧＰＳ受信機４１の検出データを初期位置情報としてもよい。又は、前回停車時に最後に推定した車両位置から、各パーティクルの位置情報及び姿勢角情報を設定してもよい。本実施形態では、初めてのループの場合、前回停車時に最後に推定した車両位置から、位置及び姿勢角の６自由度のパラメータに対する誤差の上下限を設定する。そして、この誤差の上下限の範囲内で乱数表等を用いてランダムに各パーティクルの位置情報及び姿勢角を設定する。本実施形態では、初めてのループの場合はパーティクルを５００個作成する。また、各パーティクルの６自由度のパラメータに対する誤差の上下限は、前後方向，横方向，上下方向、ロール，ピッチ，ヨーの順に±0.05[m]，±0.05[m]，±0.05[m]，±0.5[deg] ，±0.5[deg]，±0.5[deg]とする。

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

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

ステップＳ５において、ＥＣＵ１（尤度設定部１２）は、ステップＳ３で設定した複数の仮想位置及び仮想姿勢角の候補それぞれにおいて、ステップＳ１で作成したエッジ画像と、ステップＳ４で作成した仮想画像とを比較する。尤度設定部１２は、比較した結果に基づいて、仮想位置及び仮想姿勢角の候補ごとに、位置尤度及び姿勢角尤度を設定する。位置尤度とは、仮想位置の候補がどれぐらい実際の車両の位置に対して尤もらしいかを示す指標である。姿勢角尤度とは、仮想姿勢角の候補がどれぐらい実際の車両の姿勢角に対して尤もらしいかを示す指標である。尤度設定部１２は、仮想画像とエッジ画像との一致度が高いほど、位置尤度或いは姿勢角尤度を高く設定する。なお、位置尤度或いは姿勢角尤度の求め方については後述する。

尤度設定部１２は、撮像画像と仮想画像とを比較し、撮像画像内の遠方位置画素と仮想画像内の遠方位置画素とが一致した場合には仮想画像の姿勢角尤度を高く設定する。尤度設定部１２は、撮像画像と仮想画像とを比較し、撮像画像内の近傍位置画素と仮想画像内の近傍位置画素とが一致した場合には仮想画像の位置尤度を高く設定する。遠方位置画素及び近傍位置画素は、例えば、撮像画像及び仮想画像の位置によって設定してもよい。例えば、撮像画像及び仮想画像内の縦方向における中間位置より所定幅だけ下方向の範囲を、遠方位置画素が存在する画像領域に設定してもよい。また、撮像画像及び仮想画像内の縦方向における底面位置より所定幅だけ上方向の範囲を、近傍位置画素が存在する画像領域に設定してもよい。この距離は、三次元地図をカメラ２に投影したときの仮想画像を求める際、カメラ２の位置を仮定しているため求めることが可能である。

具体的には、尤度設定部１２は、仮想画像上とエッジ画像上のエッジが一致するか否か、すなわち、仮想画像上のエッジが存在する画素座標位置に、エッジ画像上のエッジが存在しているかを判断する。尤度設定部１２は、仮想画像上とエッジ画像上のエッジが一致する場合には、その一致したエッジについて３次元地図データベース３を参照して、その一致したエッジの三次元空間上での位置を求める。そして、位置尤度を求めている仮想位置及び仮想姿勢角の候補の位置情報と、その一致したエッジ部との距離Ｌ（単位：ｍ）を求め、その距離Ｌの逆数を位置尤度like_p（単位：なし）とする。なお、尤度設定部１２は、仮想画像上とエッジ画像上のエッジが一致しない場合には位置尤度like_pに０を設定する。

尤度設定部１２は、この処理を仮想画像上の全画素に対して実施する。尤度設定部１２は、車両から近い部分でエッジ画像と仮想画像とが一致していた場合、当該画素に大きい位置尤度like_pを設定する。ＥＣＵ１は、逆に、車両から遠い部分でエッジ画像と仮想画像とが一致していた場合、当該画素に小さい位置尤度like_pを設定する。尤度設定部１２は、全画素の位置尤度like_pの総和を、仮想画像についての位置尤度LIKE_P（単位：なし）とする。

尤度設定部１２は、位置尤度like_pを求めるときに、図５に示すように、車両からの距離に応じて上限値を設けてもよい。尤度設定部１２は、所定距離Ｌthp以上の近傍でエッジ画像（撮像画像）の画素と仮想画像の画素が一致した場合には、所定の上限値lthpとなるように位置尤度like_pを設定する。又は、尤度設定部１２は、車両からの距離が近くなるほど、尤度like_pの増加幅を少なくしてもよい。なお、本実施形態では距離Ｌthpは１．０［ｍ］が設定される。

具体的には、尤度設定部１２は、予め車両からの距離と位置尤度like_pとの関係を記述した位置尤度設定マップを用意しておく。ＥＣＵ１は、位置尤度設定マップを参照して、エッジ画像と仮想画像とが一致した車両からの距離に応じて位置尤度like_pを設定する。

これにより、極端に車両（カメラ２）からの距離が近い部分に対して抽出したエッジにノイズや誤差があっても、その影響を抑制することができ、位置推定誤差を小さくすることができる。

ＥＣＵ１は、姿勢角尤度も求める。尤度設定部１２は、仮想画像上とエッジ画像上のエッジ部が一致する場合には、位置尤度LIKE_Pを求めた時と同様に、一致した画素の車両からの距離Ｌ（単位：ｍ）を求める。尤度設定部１２は、車両からの距離Ｌを１０で除した値を姿勢角尤度like_a（単位：なし）とする。なお、仮想画像上とエッジ画像上のエッジ部が一致しない場合には姿勢角尤度like_aに０を設定する。

尤度設定部１２は、姿勢角尤度like_aを設定する処理を仮想画像上の全画素に対して実施する。尤度設定部１２は、車両から遠い部分でエッジ画像と仮想画像とが一致していた場合、当該画素に大きい姿勢角尤度like_aを設定する。尤度設定部１２は、逆に、車両から近い部分でエッジ画像と仮想画像とが一致していた場合、当該画素に小さい姿勢角尤度like_aを設定する。尤度設定部１２は、全画素の姿勢角尤度like_aの総和を、仮想画像についての姿勢角尤度LIKE_A（単位：なし）とする。

尤度設定部１２は、姿勢角尤度like_aを求めるときに、図６に示すように、車両からの距離に応じて上限値を設けてもよい。ＥＣＵ１は、所定距離Ｌtha以上の遠方でエッジ画像（撮像画像）の画素と仮想画像の画素が一致した場合には、所定の上限値lthaとなるように姿勢角尤度like_aを設定する。又は、ＥＣＵ１は、車両からの距離が遠くなるほど、姿勢角尤度like_aの増加幅を少なくしてもよい。なお、本実施形態では距離Ｌthaは３０．０［ｍ］が設定される。

具体的には、尤度設定部１２は、予め車両からの距離と姿勢角尤度like_aとの関係を記述した姿勢角尤度設定マップを用意しておく。ＥＣＵ１は、姿勢角尤度設定マップを参照して、エッジ画像と仮想画像とが一致した車両からの距離に応じて姿勢角尤度like_aを設定する。

これにより、極端に車両（カメラ２）からの距離が遠い部分に対して抽出したエッジにノイズや誤差があっても、その影響を抑制することができ、姿勢角推定誤差を小さくすることができる。

尤度設定部１２は、各仮想画像について位置尤度LIKE_P及び姿勢角尤度LIKE_Aを求める。すなわち、仮想位置及び仮想姿勢角の各候補について位置尤度LIKE_P及び姿勢角尤度LIKE_Aを算出する。尤度設定部１２は、複数の仮想位置及び仮想姿勢角の候補についての全結果を用いて、位置尤度LIKE_P、姿勢角尤度LIKE_Aのそれぞれの合計値が１になるよう正規化する。

次のステップＳ６において、移動物体位置姿勢推定部１３は、ステップＳ５にて位置尤度LIKE_P及び姿勢角尤度LIKE_Aが求められた複数の仮想位置及び仮想姿勢角の候補を用いて、最終的な車両の位置及び姿勢角を算出する。移動物体位置姿勢推定部１３は、姿勢角尤度LIKE_Aが高く設定された仮想画像の姿勢角に基づいて実際の車両の姿勢角を推定する。移動物体位置姿勢推定部１３は、位置尤度LIKE_Pが高く設定された仮想画像の位置に基づいて実際の車両の位置を推定する。

このとき、移動物体位置姿勢推定部１３は、例えば、位置尤度LIKE_Pが最も高く設定された仮想画像の仮想位置及び仮想姿勢角を、車両の実際の位置及び姿勢角として算出してもよい。又は、移動物体位置姿勢推定部１３は、姿勢角尤度LIKE_Aが最も高く設定された仮想画像の仮想位置及び仮想姿勢角を、車両の実際の位置及び姿勢角として算出してもよい。又は、移動物体位置姿勢推定部１３は、位置尤度LIKE_Pと姿勢角尤度LIKE_Aとの和が最も高い仮想画像の仮想位置及び仮想姿勢角を、車両の実際の位置及び姿勢角として算出してもよい。又は、移動物体位置姿勢推定部１３は、仮想位置及び仮想姿勢角の各候補に対して、各仮想画像の位置尤度LIKE_Pに応じた重み付けを行い、重みづけされた仮想位置及び仮想姿勢角の平均値を、車両の実際の位置及び姿勢角として算出してもよい。又は、移動物体位置姿勢推定部１３は、仮想位置及び仮想姿勢角の各候補に対して、各仮想画像の姿勢角尤度LIKE_Aに応じた重み付けを行い、重みづけされた仮想位置及び仮想姿勢角の平均値を、車両の実際の位置及び姿勢角として算出してもよい。又は、移動物体位置姿勢推定部１３は、仮想位置及び仮想姿勢角の各候補に対して、各仮想画像の位置尤度LIKE_Pと姿勢角尤度LIKE_Aの和に応じた重み付けを行い、重み付けされた仮想位置及び仮想姿勢角の平均値を車両の実際の位置及び姿勢角として算出してもよい。

ステップＳ３においてパーティクルフィルタを用いた場合、先ず、各パーティクルに対して、位置尤度LIKE_P及び姿勢角尤度LIKE_Aの各々に応じた重みを設定する。そして、移動物体位置姿勢推定部１３は、位置尤度LIKE_Pが最も高いパーティクルの仮想位置を、実際の車両の位置とする。又は、移動物体位置姿勢推定部１３は、複数のパーティクルの仮想位置に対して位置尤度LIKE_Pに応じた重み付けを行い、重みづけされた複数のパーティクルの仮想位置の平均値を車両の実際の位置として算出してもよい。又は、移動物体位置姿勢推定部１３は、姿勢角尤度LIKE_Aが最も高いパーティクルの仮想姿勢角を、車両の実際の姿勢角として算出してもよい。又は、移動物体位置姿勢推定部１３は、複数のパーティクルの仮想姿勢角に対して姿勢角尤度LIKE_Aに応じた重み付けを行い、重みづけされた複数のパーティクルの仮想姿勢角の平均値を、車両の実際の姿勢角として算出してもよい。

更に、仮想画像取得部１１は、各パーティクルのリサンプリングを、位置尤度LIKE_Pと姿勢角尤度LIKE_Aに基づいて行う。すなわち、複数の仮想画像の姿勢角尤度及び複数の仮想画像の位置尤度に基づいて、複数の仮想位置及び仮想姿勢角を再設定する。

具体的には、仮想画像取得部１１は、位置尤度LIKE_Pと姿勢角尤度LIKE_Aの和が最も高いパーティクルを中心として、各パーティクルのリサンプリングを行う。又は、仮想画像取得部１１は、各パーティクルの位置情報と姿勢角情報とを一旦分離し、位置情報のみのパーティクルについては位置尤度LIKE_Pに基づいてリサンプリングを行い、姿勢角情報のみのパーティクルについては姿勢角尤度LIKE_Aに基づいてリサンプリングを行う。その後、仮想画像取得部１１は、ランダムに、位置情報のみのパーティクルと姿勢角情報のみのパーティクルの位置情報と姿勢角情報を組み合わせて、新たに位置情報と姿勢角情報を持ったパーティクルを再構成してもよい。

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

以上詳細に説明したように、本実施形態として示した移動物体位置姿勢推定装置によれば、撮像画像と仮想画像とを比較し、撮像画像内の遠方位置画素と仮想画像内の遠方位置画素とが一致した場合には仮想画像の姿勢角尤度を高く設定する。一方、撮像画像内の近傍位置画素と仮想画像内の近傍位置画素とが一致した場合には仮想画像の位置尤度を高く設定する。そして、移動物体位置姿勢推定装置は、姿勢角尤度を高く設定した仮想画像の仮想姿勢角に基づいて実際の移動物体の姿勢角を推定する。一方、位置尤度を高く設定した仮想画像の仮想位置に基づいて実際の移動物体の位置を推定する。

移動物体位置姿勢推定装置によれば、位置と姿勢角に対して別々の尤度を設定し、現実の映像と仮想映像とが一致した場所までの距離に応じて、位置と姿勢角を別々に調整して推定することができる。具体的には、カメラ２から遠い場所で撮像画像と仮想画像が一致していた場合には、仮想姿勢角は真値に近いが仮想位置の誤差が大きい可能性がある。このため、姿勢角尤度をより大きく設定できるが、位置尤度はあまり大きく設定しない。一方、カメラ２から近い場所で撮像画像と仮想画像が一致していた場合には、位置尤度をより大きく設定し、逆に、姿勢角尤度はあまり大きく設定しない。

撮像画像と仮想画像が一致した場所がカメラ２から遠い場合には、仮想位置の誤差が大きい場合がある。逆に、撮像画像と仮想画像が一致した場所がカメラ２から近い場合には仮想姿勢角の誤差が大きい場合がある。この移動物体位置姿勢推定装置によれば、精度良く移動物体の位置及び姿勢角を推定することができる。

また、所定距離以上の遠方で撮像画像の画素と仮想画像の画素が一致した場合には、姿勢角尤度の増加幅を少なく又は所定の上限値となるように仮想画像の姿勢角尤度を設定する。これにより、移動物体位置姿勢推定装置によれば、一致させる映像の中に極端にカメラ２からの距離が遠い部分が含まれていた時に、この遠方部分での一致度合いによって姿勢角尤度が決定されないようにできる。このため、極端にカメラ２からの距離が遠い部分から抽出したエッジにノイズや誤差があっても、その影響を抑制することができ、姿勢角の推定誤差を小さくすることができる。

更に、所定距離以上の近傍で撮像画像の画素と仮想画像の画素が一致した場合には、位置尤度の増加幅を少なく又は所定の上限値となるように仮想画像の位置尤度を設定する。これにより、カメラ２から一定距離以内で２つの映像が一致していた場合に位置尤度を大きくしすぎないようにする。したがって、移動物体位置姿勢推定装置によれば、極端にカメラ２からの距離が近い部分から抽出したエッジにノイズや誤差があっても、その影響を抑制することができ、位置の推定誤差を小さくすることができる。

更に、移動物体位置姿勢推定装置によれば、複数のパーティクル（候補点）を設定して各パーティクルについての位置尤度LIKE_P及び姿勢角尤度LIKE_Aを設定する。各パーティクルについての位置尤度LIKE_P及び姿勢角尤度LIKE_Aに基づいて、車両の位置及び姿勢角を求める。そして、位置尤度LIKE_P及び姿勢角尤度LIKE_Aに基づいてパーティクルをリサンプリングできる。

ここで、パーティクルフィルタにおいて、推定する次数がｎとした場合、推定精度をａ倍とするためには、散布するパーティクルの数も原理的にａのｎ乗倍に増やす必要がある（確率ロボティクス・第4章3節（（著）Sebastian Thrun／Wolfram Burgard／Dieter Fox，（訳）上田隆一，（発行）(株)毎日コミュニケーションズ））。既存技術では、例えば三次元での位置情報と姿勢角情報を同時に推定する場合、その次数は６次となる。よって、推定精度を２倍に上げようとすると演算時間は６４倍となり、推定精度を３倍に上げようとすると演算時間は７２９倍に増加してしまう。

これに対し、本実施形態に係わる移動物体位置姿勢推定装置によれば、位置情報と姿勢角情報とを別々に扱うことができる。したがって、原理的に推定精度を２倍に上げようとした場合には演算時間は２３×２=１６倍となり、推定精度を３倍に上げようとした場合で演算時間は３３×２=５４倍となるため、演算負荷を大きく低減できる。
（第２実施形態）
本発明の第２実施形態では、車両の位置及び姿勢角の各々を異なる仮想画像を用いて推定する移動物体位置姿勢推定装置及び移動物体位置姿勢推定方法を説明する。

具体的に、第１実施形態では、仮想位置及び仮想姿勢角の組合せからなる候補点（パーティクル）を複数設定し、各候補点について取得された仮想画像に基づいて、車両の位置及び姿勢角を推定した。これに対して、第２実施形態では、複数の仮想位置に基づく車両近傍領域の仮想画像を取得し、車両近傍領域の仮想画像を用いて車両の位置を推定する。そして、複数の仮想姿勢角に基づく車両遠方領域の仮想画像を取得し、車両遠方領域の仮想画像を用いて車両の姿勢角を推定する。つまり、車両の位置及び姿勢角の推定に、異なる仮想画像を用いている点が相違する。

なお、第２実施形態に係わる移動物体位置姿勢推定装置のハードウェア構成は、図１に示した構成と同じであり、説明を省略する。ただし、ＥＣＵ１のソフトウェア構成、すなわち、仮想画像取得部１１、尤度設定部１２、移動物体位置姿勢推定部１３は、以下に示すように相違する。

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

ステップＳ１〜Ｓ２の処理は、図３を参照して説明したステップＳ１〜Ｓ２と同じであり、説明を省略する。

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

ステップＳ１４に進み、仮想画像取得部１１は、ステップＳ１３で算出された初期予測位置及びその近傍において、複数の仮想位置を設定する。このとき、仮想画像取得部１１は、車両センサ群４の測定誤差や通信遅れによって生じるオドメトリの誤差や、オドメトリで考慮できない車両の動特性を考慮に入れ、車両の位置の真値が取り得る可能性がある複数の仮想位置を生成する。仮想位置は、位置に含まれる３自由度のパラメータに対してそれぞれ誤差の上下限を設定し、この誤差の上下限の範囲内で、所定間隔で設定する。本実施形態では、位置に含まれる３自由度のパラメータに対する誤差の上下限は、前後方向，横方向，上下方向の順に±0.2[m]，±0.1[m]，±0.05[m]とし、それぞれ0.01[m]の間隔で仮想位置を設定する。したがて、40×20×10＝8000の仮想位置が作成される。誤差の上下限及び所定間隔は、車両の運転状態や路面の状況を検出或いは推定して、適宜変更することが望ましい。例えば、急旋回やスリップなどが発生している場合には、平面方向（前後方向，横方向）の誤差が大きくなる可能性が高い。よって、３つのパラメータの誤差の上下限を大きくすることが好ましい。

ステップＳ１５に進み、仮想画像取得部１１は、評価点投影方法を用いて、ステップＳ１４で設定した仮想位置ごとに、車両に近い領域の仮想画像を作成する。ただし、ステップＳ１５における仮想画像の作成は、車両１の位置を推定することを目的としている。よって、３次元地図データベース３に記憶されたエッジ等の三次元位置情報のうち車両１の近傍にある情報のみを、評価点投影手段を用いて仮想位置及び仮想姿勢角から撮像した仮想画像に変換する。本実施形態では、各仮想位置から距離３ｍ以内にあるエッジ等の三次元位置情報のみを投影して仮想画像を作成する。なお、ステップＳ１５において用いる仮想姿勢角としては、１ループ前のステップＳ１９で推定された車両の姿勢角、或いはステップＳ１３で求めた初期予測姿勢角を用いればよい。

ステップＳ１６に進み、尤度設定部１２は、ステップＳ１５で作成した車両に近い領域の各仮想画像と撮像画像とを比較する。具体的には、ステップＳ１５で作成された仮想画像の各々について、仮想画像に含まれるエッジと撮像画像に含まれるエッジからなるエッジ画像を比較し、仮想画像内のエッジとエッジ画像内のエッジとの一致度合いを算出する。例えば、車両近傍の仮想画像とエッジ画像の間でエッジが一致する画素数、即ち、エッジが存在する仮想画像上の画素座標位置に、エッジ画像上のエッジが存在している画素数を一致度合いとして計数する。そして、尤度設定部１２は、仮想画像内のエッジと撮像画像内のエッジとの一致度合いが高いほど、位置尤度を高く設定する。

続けて、移動物体位置姿勢推定部１３は、尤度設定部１２により位置尤度が高く設定された仮想画像の仮想位置に基づいて車両の位置を推定する。例えば、移動物体位置姿勢推定部１３は、ステップＳ１５で作成した複数の仮想画像のうち、一致する画素の数が最も多かった仮想画像の仮想位置を車両１の実際の位置として算出する。又は、移動物体位置姿勢推定部１３は、各仮想位置に対して、各仮想画像の位置尤度に応じた重み付けを行い、重みづけされた仮想位置の平均値を、車両の実際の位置として算出してもよい。

このように、ステップＳ１４〜Ｓ１６の処理によって、複数の仮想位置に基づく車両近傍領域の仮想画像を取得し、車両近傍領域の仮想画像を用いて車両の位置を推定することができる。

次に、ステップＳ１７に進み、仮想画像取得部１１は、ステップＳ１３で算出された初期予測姿勢角を参照して、複数の仮想姿勢角を設定する。このとき、仮想画像取得部１１は、車両センサ群４の測定誤差や通信遅れによって生じるオドメトリの誤差や、オドメトリで考慮できない車両の動特性を考慮に入れ、車両の姿勢角の真値が取り得る可能性がある複数の仮想姿勢角を生成する。仮想姿勢角は、姿勢角に含まれる３自由度のパラメータに対してそれぞれ誤差の上下限を設定し、この誤差の上下限の範囲内で、所定間隔で設定する。本実施形態では、姿勢角に含まれる３自由度のパラメータに対する誤差の上下限は、ロール，ピッチ，ヨーの順に±0.5[deg] ，±0.5[deg]，±0.5[deg]とし、それぞれ0.05[deg]の間隔で仮想姿勢角を設定する。したがて、20×20×20＝8000の仮想位置が作成される。ステップＳ１４の位置の場合と同様に、誤差の上下限及び所定間隔は、車両の運転状態や路面の状況を検出或いは推定して、適宜変更することが望ましい。例えば、急旋回やスリップなどが発生している場合には、ヨー角の誤差が大きくなる可能性が高い。よって、ヨー角の誤差の上下限を大きくする、或いは段差乗り越えのような場合にはピッチ角の誤差の上下限を大きくすることが好ましい。

ステップＳ１８に進み、仮想画像取得部１１は、評価点投影方法を用いて、ステップＳ１７で設定した仮想姿勢角ごとに、車両から遠い領域の仮想画像を作成する。ただし、ステップＳ１８における仮想画像の作成は、車両１の姿勢角を推定することを目的としている。よって、３次元地図データベース３に記憶されたエッジ等の三次元位置情報のうち車両１から遠方にある情報のみを、評価点投影手段を用いて仮想位置及び仮想姿勢角から撮像した仮想画像に変換する。本実施形態では、ステップＳ１３で算出した初期予測位置或いはステップＳ１６で算出した車両１の位置から距離２０ｍ以遠にあるエッジ等の三次元位置情報のみを投影して仮想画像を作成する。なお、ステップＳ１８において用いる仮想位置としては、１ループ前のステップＳ１９で推定された車両の位置、或いはステップＳ１３で求めた初期予測位置を用いればよい。なお、極端に遠方まで投影しても投影の処理に時間がかかる上、カメラ２の解像度ではエッジが一致している否かを区別できない。このため、本実施形態では距離５０ｍ以遠のエッジ等の三次元位置情報は投影しない。

ステップＳ１９に進み、尤度設定部１２は、ステップＳ１８で作成した車両から遠い領域の各仮想画像と撮像画像とを比較する。具体的には、ステップＳ１８で作成された仮想画像の各々について、仮想画像に含まれるエッジと撮像画像に含まれるエッジからなるエッジ画像とを比較し、仮想画像内のエッジとエッジ画像内のエッジとの一致度合いを算出する。例えば、車両遠方の仮想画像とエッジ画像の間でエッジが一致する画素数、即ち、エッジが存在する仮想画像上の画素座標位置に、エッジ画像上のエッジが存在している画素数を一致度合いとして計数する。そして、尤度設定部１２は、仮想画像内のエッジと撮像画像内のエッジとの一致度合いが高いほど、姿勢角尤度を高く設定する。

続けて、移動物体位置姿勢推定部１３は、尤度設定部１２により姿勢角尤度が高く設定された仮想画像の仮想姿勢角に基づいて車両の姿勢角を推定する。例えば、移動物体位置姿勢推定部１３は、ステップＳ１８で作成した複数の仮想画像のうち、一致する画素の数が最も多かった仮想画像の仮想姿勢角を車両１の実際の姿勢角として算出する。又は、移動物体位置姿勢推定部１３は、各仮想姿勢角に対して、各仮想画像の姿勢角尤度に応じた重み付けを行い、重みづけされた仮想姿勢角の平均値を、車両の実際の姿勢角として算出してもよい。

このように、ステップＳ１７〜Ｓ１９の処理によって、複数の仮想姿勢角に基づく車両遠方領域の仮想画像を取得し、車両遠方領域の仮想画像を用いて車両の姿勢角を推定することができる。

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

以上説明したように、本実施形態では、車両の位置及び姿勢角の各々を異なる仮想画像を用いて推定する。具体的には、複数の仮想位置に基づく車両近傍領域の仮想画像を取得し、車両近傍領域の仮想画像を用いて車両の位置を推定する。そして、複数の仮想姿勢角に基づく車両遠方領域の仮想画像を取得し、車両遠方領域の仮想画像を用いて車両の姿勢角を推定する。これにより、位置と姿勢角に対して別々の尤度を設定し、現実の映像と仮想映像とが一致した場所までの距離に応じて、位置と姿勢角を別々に調整して推定することができる。よって、精度良く移動物体の位置及び姿勢角を推定することができる。

また、所定距離以上の遠方で撮像画像の画素と仮想画像の画素が一致した場合には、姿勢角尤度の増加幅を少なく又は所定の上限値となるように仮想画像の姿勢角尤度を設定してもよい。これにより、移動物体位置姿勢推定装置によれば、一致させる映像の中に極端にカメラ２からの距離が遠い部分が含まれていた時に、この遠方部分での一致度合いによって姿勢角尤度が決定されないようにできる。このため、極端にカメラ２からの距離が遠い部分から抽出したエッジにノイズや誤差があっても、その影響を抑制することができ、姿勢角の推定誤差を小さくすることができる。

更に、所定距離以上の近傍で撮像画像の画素と仮想画像の画素が一致した場合には、位置尤度の増加幅を少なく又は所定の上限値となるように仮想画像の位置尤度を設定してもよい。これにより、カメラ２から一定距離以内で２つの映像が一致していた場合に位置尤度を大きくしすぎないようにする。したがって、移動物体位置姿勢推定装置によれば、極端にカメラ２からの距離が近い部分から抽出したエッジにノイズや誤差があっても、その影響を抑制することができ、位置の推定誤差を小さくすることができる。

以上、実施形態に沿って本発明の内容を説明したが、本発明はこれらの記載に限定されるものではなく、種々の変形及び改良が可能であることは、当業者には自明である。

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

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

なお、第１及び第２実施形態では、「予め定めた位置及び姿勢角から見た時の対比画像を取得する対比画像取得部」として、「三次元地図データを仮想位置及び仮想姿勢角から撮像した画像に変換して、仮想画像を取得する仮想画像取得部１１」を例示した。しかし、「対比画像取得部」はこれに限定されない。例えば、対比画像取得部は、カメラ２が過去に撮像した撮像画像を対比画像として取得してもよい。この場合、移動物体位置姿勢推定部１３は、尤度設定部１２により姿勢角尤度が高く設定された対比画像を撮像した時の車両の姿勢角に基づいて車両の姿勢角を推定し、尤度設定部１２により位置尤度が高く設定された対比画像を撮像した時の車両の位置に基づいて車両の位置を推定すればよい。これにより、位置と姿勢角に対して別々の尤度を設定し、現在の撮像映像と過去の撮像画像（対比画像）とが一致した場所までの距離に応じて、位置と姿勢角を別々に調整して推定することができる。よって、精度良く移動物体の位置及び姿勢角を推定することができる。

特願２０１２−０４９３７２号（出願日：２０１２年３月６日）の全内容は、ここに援用される。

本実施形態に係わる移動物体位置姿勢推定装置及び移動物体位置姿勢推定方法によれば、遠方位置で撮像画像と仮想画像とが一致した場合には姿勢角尤度を高くし、近傍位置で撮像画像と仮想画像とが一致した場合には位置尤度を高くするので、位置誤差が大きい遠方での一致により姿勢角尤度を設定し、姿勢角誤差が大きい近傍での一致により位置尤度を設定できる。これにより、精度良く移動物体の位置及び姿勢角を推定することができる。よって、本発明は、産業上の利用可能性を有する。

１ ＥＣＵ
２ カメラ（撮像部）
３ ３次元地図データベース
１１ 仮想画像取得部（対比画像取得部）
１２ 尤度設定部
１３ 移動物体位置姿勢推定部

Claims (8)

1. 移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定装置であって、
   前記移動物体周辺を撮像して、撮像画像を取得する撮像部と、
   予め定めた位置及び姿勢角から見た時の対比画像を取得する対比画像取得部と、
   前記撮像部により取得された撮像画像と前記対比画像取得部により取得された対比画像とを比較し、前記撮像画像内の遠方位置画素と前記対比画像内の遠方位置画素とが一致した場合には前記対比画像の姿勢角尤度を高く設定し、前記撮像画像内の近傍位置画素と前記対比画像内の近傍位置画素とが一致した場合には前記対比画像の位置尤度を高く設定する尤度設定部と、
   前記尤度設定部により前記姿勢角尤度が高く設定された前記対比画像の姿勢角に基づいて前記移動物体の姿勢角を推定し、前記尤度設定部により前記位置尤度が高く設定された前記対比画像の位置に基づいて前記移動物体の位置を推定する移動物体位置姿勢推定部と
   を有することを特徴とする移動物体位置姿勢推定装置。
2. 前記対比画像取得部は、三次元地図データを仮想位置及び仮想姿勢角から撮像した画像に変換して、仮想画像を取得する仮想画像取得部であり、
   前記尤度設定部は、前記撮像部により取得された撮像画像と前記仮想画像取得部により取得された仮想画像とを比較し、前記撮像画像内の遠方位置画素と前記仮想画像内の遠方位置画素とが一致した場合には前記仮想画像の姿勢角尤度を高く設定し、前記撮像画像内の近傍位置画素と前記仮想画像内の近傍位置画素とが一致した場合には前記仮想画像の位置尤度を高く設定し、
   前記移動物体位置姿勢推定部は、前記尤度設定部により前記姿勢角尤度が高く設定された前記仮想画像の仮想姿勢角に基づいて前記移動物体の姿勢角を推定し、前記尤度設定部により前記位置尤度が高く設定された前記仮想画像の仮想位置に基づいて前記移動物体の位置を推定する
   ことを特徴とする請求項１に記載の移動物体位置姿勢推定装置。
3. 前記対比画像取得部は、前記撮像部が過去に撮像した前記撮像画像を前記対比画像として取得し、
   前記移動物体位置姿勢推定部は、前記尤度設定部により前記姿勢角尤度が高く設定された前記対比画像を撮像した時の前記移動体の姿勢角に基づいて前記移動物体の姿勢角を推定し、前記尤度設定部により前記位置尤度が高く設定された前記対比画像を撮像した時の前記移動体の位置に基づいて前記移動物体の位置を推定する
   ことを特徴とする請求項１に記載の移動物体位置姿勢推定装置。
4. 前記尤度設定部は、前記撮像画像及び前記対比画像内の遠方位置画素のうち、所定距離以上の遠方で前記撮像画像の画素と前記対比画像の画素が一致した場合には、尤度の増加幅を少なく又は所定の上限値となるように前記対比画像の姿勢角尤度を設定することを特徴とする請求項１乃至請求項３の何れか一項に記載の移動物体位置姿勢推定装置。
5. 前記尤度設定部は、前記撮像画像及び前記対比画像内の近傍位置画素のうち、所定距離以上の近傍で前記撮像画像の画素と前記対比画像の画素が一致した場合には、尤度の増加幅を少なく又は所定の上限値となるように前記対比画像の位置尤度を設定することを特徴とする請求項１乃至請求項４の何れか一項に記載の移動物体位置姿勢推定装置。
6. 前記仮想画像取得部は、仮想位置及び仮想姿勢角が設定された候補点を複数設定して、前記候補点ごとに仮想画像を取得し、前記尤度設定部は、各仮想画像と撮像画像とを比較して姿勢角尤度及び位置尤度を設定し、前記移動物体位置姿勢推定部は、複数の仮想画像の姿勢角尤度に基づいて前記移動物体の姿勢角を推定し、複数の仮想画像の位置尤度に基づいて前記移動物体の位置を推定し、
   前記仮想画像取得部は、複数の仮想画像の姿勢角尤度及び複数の仮想画像の位置尤度に基づいて、前記複数の候補点を再設定すること
   を特徴とする請求項２、４及び５の何れか一項に記載の移動物体位置姿勢推定装置。
7. 前記対比画像取得部は、複数の位置を設定し、前記位置ごとに前記移動体に近い領域の前記対比画像を取得し、
   前記尤度設定部は、前記移動体に近い領域の各対比画像と前記撮像画像とを比較して、前記対比画像内のエッジと前記撮像画像内のエッジとの一致度合いが高いほど、前記位置尤度を高く設定し、
   前記移動物体位置姿勢推定部は、前記尤度設定部により前記位置尤度が高く設定された前記対比画像の位置に基づいて前記移動物体の位置を推定し、
   前記対比画像取得部は、複数の姿勢角を設定し、前記姿勢角ごとに前記移動体から遠い領域の前記対比画像を取得し、
   前記尤度設定部は、前記移動体から遠い領域の各対比画像と前記撮像画像とを比較して、前記対比画像内のエッジと前記撮像画像内のエッジとの一致度合いが高いほど、前記姿勢角尤度を高く設定し、
   前記移動物体位置姿勢推定部は、前記尤度設定部により前記姿勢角尤度が高く設定された前記対比画像の姿勢角に基づいて前記移動物体の姿勢角を推定する
   ことを特徴とする請求項１に記載の移動物体位置姿勢推定装置。
8. 移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定方法であって、
   前記移動物体周辺を撮像した撮像画像と、予め定めた位置及び姿勢角から見た時の対比画像とを比較し、
   前記撮像画像内の遠方位置画素と前記対比画像内の遠方位置画素とが一致した場合には前記対比画像の姿勢角尤度を高く設定し、前記撮像画像内の近傍位置画素と前記対比画像内の近傍位置画素とが一致した場合には前記対比画像の位置尤度を高く設定し、
   前記姿勢角尤度が高く設定された前記対比画像の姿勢角に基づいて前記移動物体の姿勢角を推定し、前記位置尤度が高く設定された前記対比画像の位置に基づいて前記移動物体の位置を推定する
   を有することを特徴とする移動物体位置姿勢推定方法。

Images