JPWO2014091877A1 - 移動物***置姿勢角推定装置及び移動物***置姿勢角推定方法 - Google Patents

Abstract

本発明の移動物***置姿勢角推定装置１０は、移動物体の周囲環境を魚眼レンズで撮像した魚眼レンズ画像を取得し、魚眼レンズ画像の所定領域について歪みの小さいピンホール画像を取得する。そして、移動物体の進行方向における走行路の屈曲度を検出し、検出された屈曲度が所定値未満の場合にはピンホール画像を選択し、屈曲度が所定値以上の場合には魚眼レンズ画像を選択する。その後、選択された画像のエッジ情報と三次元地図データとのマッチングを行って移動物体の位置及び姿勢角を推定する。

Description

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

従来、三次元地図情報と魚眼レンズカメラの撮像画像とをマッチングすることによって移動物体の自己位置を算出する技術が開発されており、特許文献１が開示されている。

この技術では、車両に設置された魚眼レンズカメラで信号機を撮像し、撮像した信号機画像から車両に対する仰角と水平角を算出した上で、信号機の発光位置座標と高さとから車両の現在位置を特定している。

特開２００６−１０３２８号公報

しかしながら、上述した従来の技術では、車両の自己位置を推定するためには信号機が必要であり、信号機のない交差点や単路等では自己位置を推定することができないという問題点があった。

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

上述した課題を解決するために、本発明は、移動物体の周囲環境を魚眼レンズで撮像した魚眼レンズ画像を取得し、魚眼レンズ画像の所定領域について歪みの小さいピンホール画像を取得する。そして、移動物体の進行方向における走行路の屈曲度を検出し、検出された屈曲度が所定値未満の場合にはピンホール画像を選択し、屈曲度が所定値以上の場合には魚眼レンズ画像を選択する。こうして画像が選択されると、選択された画像のエッジ情報と三次元地図データから抽出された仮想画像のエッジ情報とのマッチングを行って移動物体の位置及び姿勢角を推定する。

図１は、本発明を適用した第１実施形態に係る移動物***置姿勢角推定装置を備えた移動物***置姿勢角推定システムの構成を示すブロック図である。 図２は、本発明を適用した第１実施形態に係る移動物***置姿勢角推定装置による移動物***置姿勢角推定処理の処理手順を示すフローチャートである。 図３は、魚眼レンズカメラの画角とピンホール画像の画角とを説明するための図である。 図４は、本発明を適用した第１実施形態に係る移動物***置姿勢角推定装置による移動物***置姿勢角算出処理の処理手順を示すフローチャートである。 図５は、本発明を適用した第１実施形態に係る移動物***置姿勢角推定装置による移動物***置姿勢角算出処理の処理手順を示すフローチャートである。 図６は、本発明を適用した第２実施形態に係る移動物***置姿勢角推定装置による移動物***置姿勢角推定処理の処理手順を示すフローチャートである。 図７は、本発明を適用した第３実施形態に係る移動物***置姿勢角推定装置による移動物***置姿勢角推定処理の処理手順を示すフローチャートである。 図８は、本発明を適用した第４実施形態に係る移動物***置姿勢角推定装置による移動物***置姿勢角推定処理の処理手順を示すフローチャートである。 図９は、本発明を適用した第５実施形態に係る移動物***置姿勢角推定装置による移動物***置姿勢角推定処理の処理手順を示すフローチャートである。 図１０は、本発明を適用した第６実施形態に係る移動物***置姿勢角推定装置による移動物***置姿勢角推定処理の処理手順を示すフローチャートである。 図１１は、本発明を適用した第７実施形態に係る移動物***置姿勢角推定装置を備えた移動物***置姿勢角推定システムの構成を示すブロック図である。

以下、本発明を適用した第１〜第７実施形態について図面を参照して説明する。

［第１実施形態］
［移動物***置姿勢角推定装置の構成］
図１は、本実施形態に係る移動物***置姿勢角推定装置を搭載した移動物***置姿勢角推定システムの構成を示すブロック図である。図１に示すように、本実施形態に係る移動物***置姿勢角推定システムは、ＥＣＵ１と、魚眼レンズカメラ２と、三次元地図データベース３と、車両センサ群４と、ナビゲーションシステム５とを備えている。尚、本実施形態では、移動物***置姿勢角推定装置を車両に適用した場合について説明する。

ここで、ＥＣＵ１は、ＲＯＭ、ＲＡＭ、演算回路等によって構成された電子制御ユニットであり、本実施形態に係る移動物***置姿勢角推定装置１０を備えている。尚、ＥＣＵ１は他の制御に用いるＥＣＵと兼用してもよい。

魚眼レンズカメラ２は、例えばＣＣＤ等の固体撮像素子を用いたものであり、本実施形態では、車両のフロントバンパーに光軸が水平で、尚且つ車両前方が撮像可能となるように設置されている。撮像された魚眼レンズ画像は、ＥＣＵ１へ送信される。また、魚眼レンズは画角が約１７０[deg]で等距離射影方式の一般的なものでよい。

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

車両センサ群４は、ＧＰＳ受信機４１と、アクセルセンサ４２と、ステアリングセンサ４３と、ブレーキセンサ４４と、車速センサ４５と、加速度センサ４６と、車輪速センサ４７と、三軸ジャイロセンサ４８とを備えている。車両センサ群４は、ＥＣＵ１に接続され、各センサ４１〜４８によって検出された各種の検出値をＥＣＵ１に供給する。ＥＣＵ１は、車両センサ群４の出力値を用いることによって、車両の概位置を算出したり、単位時間に車両が進んだ移動量を示すオドメトリを算出する。

ナビゲーションシステム５は、一般の車両に搭載されているものと同様のシステムでよく、リンクとノードで記録された道路地図情報を格納したデータベースを備えている。そして、車両の運転者の操作によって設定された目的地に対して、運転時間や距離等を最適化したルートを算出し、算出したルートは運転者に報知され、ＥＣＵ１に送信される。

移動物***置姿勢角推定装置１０は、移動物体の周囲環境を撮像した画像と三次元地図データとのマッチングを行って移動物体の位置及び姿勢角を推定するものである。移動物***置姿勢角推定装置１０は、ＲＯＭに格納された移動物***置姿勢角推定用のプログラムを実行することにより、魚眼レンズ画像取得部１２、ピンホール画像取得部１４、画像選択部１６及び位置姿勢角推定部１８として動作する。

魚眼レンズ画像取得部１２は、移動物体の周囲環境を魚眼レンズで撮像した魚眼レンズ画像を魚眼レンズカメラ２から取得する。魚眼レンズ画像には移動物体の周囲環境が撮像されており、自車両の位置及び姿勢角を推定するために必要な路面情報として、少なくとも道路端部を示す区画線や縁石が撮像されている。また、この他に白線、停止線、横断歩道、路面マーク等の路面表示が撮像されていてもよい。

ピンホール画像取得部１４は、魚眼レンズ画像の所定領域について歪みの小さいピンホール画像を取得する。特に、本実施形態では、魚眼レンズカメラ２で撮像された魚眼レンズ画像の所定領域に対して歪み補正を行うことによって、歪みの小さいピンホール画像を取得する。ピンホール画像には魚眼レンズ画像と同様に移動物体の周囲環境が撮像されており、自車両の位置及び姿勢角を推定するために必要な路面情報として、少なくとも道路端部を示す区画線や縁石が撮像されている。また、この他に白線、停止線、横断歩道、路面マーク等の路面表示が撮像されていてもよい。

画像選択部１６は、移動物体の進行方向における走行路の屈曲度を検出し、検出された屈曲度が所定値未満の場合にはピンホール画像を選択し、屈曲度が所定値以上の場合には魚眼レンズ画像を選択する。ここで、走行路の屈曲度は、走行路がどの程度曲がっているかを示す指標であり、走行路が大きく曲がっていれば屈曲度の値は大きくなり、走行路が曲がっていなければ屈曲度の値は小さくなる。

特に、本実施形態では、移動物体の進行方向における旋回半径を検出し、検出された旋回半径が所定値以上の場合には屈曲度が所定値未満であると判定してピンホール画像を選択する。一方、旋回半径が所定値未満の場合には屈曲度が所定値以上であると判定して魚眼レンズ画像を選択する。

位置姿勢角推定部１８は、画像選択部１６で選択された画像に基づいて三次元地図データとのマッチングを行って移動物体の位置及び姿勢角を推定する。特に、選択された画像のエッジ情報と、三次元地図データから抽出された仮想画像のエッジ情報とのマッチングを行って移動物体の位置及び姿勢角を推定する。このとき仮想画像には、推定される移動物体の仮想位置と仮想姿勢角が含まれている。また、三次元地図データとのマッチングでは、選択された画像に撮像されている周囲環境のうち、特に道路端部を示す路面情報に基づいてマッチングする。

尚、本実施形態では車両に適用した場合について説明するが、移動物体であれば航空機や船舶等にも適用可能である。航空機や船舶に適用する場合には、周囲環境として道路に関する路面情報の代わりに地形や建造物とマッチングすることによって移動物体の位置と姿勢角を推定すればよい。

［移動物体の位置姿勢角推定処理の手順］
次に、本実施形態に係る移動物体の位置姿勢角推定処理の手順を図２のフローチャートを参照して説明する。尚、本実施形態では、車両の位置情報として前後方向、横方向、上下方向、姿勢角情報としてロール、ピッチ、ヨーの合計６自由度の位置と姿勢角を推定する。また、以下で説明する移動物体の位置姿勢角推定処理は、例えば１００ｍｓｅｃ程度の間隔で連続的に行われる。

図２に示すように、まずステップＳ１０において、魚眼レンズカメラ２で撮像された映像がＥＣＵ１に送信され、魚眼レンズ画像取得部１２が魚眼レンズ画像を取得する。

ステップＳ２０では、ピンホール画像取得部１４が、魚眼レンズカメラ２で撮像された映像の所定領域に対して歪み補正を行うことによって歪みの小さいピンホール画像を算出して取得する。魚眼レンズカメラ２の魚眼レンズは等距離射影方式なので、本実施形態では魚眼レンズの仮想球面モデルで考え、仮想球面と光軸との交点で接する平面スクリーン像を再現するように歪みを補正してピンホール画像を算出する。また、このピンホール画像は、図３に示すように魚眼レンズカメラ２の画角１７０[deg]のうち所定領域として画角４０[deg]の領域を歪み補正して算出する。このようにして算出されたピンホール画像は、障害物などによって遮蔽されていなければ、ほとんどの走行シーンにおいて自車両前方の左右の道路端部が撮像されている。

次に、ステップＳ３０において、画像選択部１６は、移動物体である自車両の進行方向における旋回半径Ｒ[ｍ]を検出する。このとき旋回半径が変化する場合には、最も小さい旋回半径を旋回半径Ｒとして検出する。そして、検出された旋回半径Ｒが所定値Ｒｔｈ[ｍ]以上の場合には屈曲度が所定値未満であると判定してステップＳ５０に進んでピンホール画像を選択する。一方、旋回半径Ｒが所定値Ｒｔｈ[ｍ]未満の場合には屈曲度が所定値以上であると判定してステップＳ４０に進んで魚眼レンズ画像を選択する。また、ターンシグナルのＯＮ、ＯＦＦを用いて画像を選択してもよい。尚、本実施形態では所定値Ｒｔｈとして３０ｍを設定する。

ここで、旋回半径の検出方法としては、以下の方法を用いて行えばよい。例えば、自車両の運転者のステアリング操作を車両センサ群４のステアリングセンサ４３で検出し、自車両のステアリング機構のステアリングギア比を乗じて前輪の転舵角を求める。そして、アッカーマンステアリングジオメトリや線形二輪モデル（「自動車の運動と制御」第３章，安部正人，山海堂）等の車両モデルを用いることによって、旋回半径を算出することができる。また、ナビゲーションシステム５で算出した自車両の走行ルートから三次元地図データベース３を参照し、走行ルート上での旋回半径を予め算出してＥＣＵ１に記録する。そして、１ループ前に算出された自車両の位置と姿勢角から現在の旋回半径Ｒを求めるようにしてもよい。

一方、ステップＳ３０では、旋回半径に応じて画像を選択するだけでなく、自車両の進行方向における左右の道路端部がピンホール画像に撮像できないと判定した場合にのみステップＳ４０に進んで魚眼レンズ画像を選択するようにしてもよい。

この場合には、まず走行ルート上の左右の道路端部の間隔、即ち道路の幅員を、三次元地図データベース３を参照して求める。そして、この幅員と、旋回半径Ｒと、魚眼レンズカメラ２の取り付け位置と、１ループ前の処理で算出した自車両の位置と姿勢角とを参照する。これらの情報から、画像選択部１６は、現在の自車位置から自車両の進行方向における左右の道路端部がピンホール画像上に撮像できるか否かを判定し、撮像できないと判定した場合には、ステップＳ４０に進んで魚眼レンズ画像を選択する。これにより、道路端部が撮像できる場合にはピンホール画像が選択されるので、ピンホール画像を選択する場合が増加し、移動物体の位置及び姿勢角の推定をよりロバストに、尚且つ高速に行うことができる。

このようにしてステップＳ３０において旋回半径が判定されると、ステップＳ４０では、画像選択部１６が、後述するステップＳ６０の処理で用いる画像として魚眼レンズ画像を選択してステップＳ６０に進む。旋回半径が小さい場合、すなわち急カーブや交差点での右左折のような屈曲度の大きいシーンでは、自車両の位置と姿勢角を算出するために必要な区画線や縁石などの道路端部はピンホール画像には撮像されていない可能性が高い。そこで、視野角の広い魚眼レンズ画像を選択して確実に道路端部を検出し、移動物体の位置及び姿勢角を推定できるようにする。

一方、ステップＳ５０では、画像選択部１６が、後述するステップＳ６０の処理で用いる画像としてピンホール画像を選択してステップＳ６０に進む。旋回半径が大きい場合、すなわち直線路や緩いカーブ、交差点を直進するような屈曲度が小さい場合では、視野角の狭いピンホール画像でも道路端部が撮像されている可能性が高い。そこで、ピンホール画像を用いても移動物体の位置及び姿勢角を推定することは可能であり、またピンホール画像は歪みが小さいので、高速かつ高精度に移動物体の位置及び姿勢角を推定することができる。

次に、ステップＳ６０では、位置姿勢角推定部１８が、ステップＳ４０またはステップＳ５０で選択された画像を用いて三次元地図データとのマッチングを行って自車両の位置と姿勢角を算出して推定する。具体的には、ステップＳ４０で魚眼レンズ画像が選択された場合には図４のフローチャートを用いて移動物体の位置と姿勢角を算出し、ステップＳ５０でピンホール画像が選択された場合には図５のフローチャートを用いて移動物体の位置と姿勢角を算出する。こうして移動物体の位置と姿勢角が推定されると、本実施形態に係る移動物体の位置姿勢角推定処理を終了する。

以下、ステップＳ６０における移動物体の位置姿勢角算出処理の手順を図４、５のフローチャートを参照して説明する。

［魚眼レンズ画像が選択された場合の移動物体の位置姿勢角算出処理の手順］
まず、図４のフローチャートを参照して、ステップＳ４０で魚眼レンズ画像が選択された場合の移動物体の位置姿勢角算出処理の手順を説明する。

ステップＳ１１０において、位置姿勢角推定部１８は、魚眼レンズ画像からエッジ画像を算出する。本実施形態におけるエッジとは、画素の輝度が鋭敏に変化している箇所を指している。エッジの検出方法としては、例えば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]とする。

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

なお、ステップＳ１１０において魚眼レンズ画像からエッジの輝度変化の方向やエッジ近辺のカラー等について抽出している場合には、それらを用いて投影画像を作成することが望ましい。

次に、ステップＳ１５０において、位置姿勢角推定部１８は、ステップＳ１３０で設定した複数の仮想位置及び仮想姿勢角の候補のそれぞれにおいて、ステップＳ１１０で作成したエッジ画像と、ステップＳ１４０で作成した投影画像のエッジ情報とを比較する。そして、比較した結果に基づいて、各仮想位置及び仮想姿勢角の候補ごとに尤度を求める。この尤度とは、仮想位置及び仮想姿勢角の候補が、どれぐらい実際の車両の位置及び姿勢角に対して尤もらしいかを示す指標である。位置姿勢角推定部１８は、投影画像とエッジ画像との一致度が高いほど、尤度が高くなるように設定する。この尤度の求め方の一例を以下で説明する。

まず、投影画像上において画素位置を特定し、この画素位置にエッジが存在するか否かを判定する。そして、投影画像と同じ画素位置となるエッジ画像上の位置にエッジが存在するか否かを判定し、エッジの有無が一致する場合には尤度likelihood（単位：なし）として１を設定し、一致しない場合には０を設定する。このような処理を投影画像上の全画素に対して行い、すべての画素位置に設定された尤度を加算することによって、最終的にその仮想位置及び仮想姿勢角の候補の尤度とする。そして、全ての仮想位置及び仮想姿勢角の候補について尤度を算出したら、それぞれの尤度の合計値が１になるよう正規化する。

次に、ステップＳ１６０において、位置姿勢角推定部１８は、ステップＳ１５０で算出した尤度情報を有する複数の仮想位置及び仮想姿勢角の候補を用いて、最終的な自車両の位置及び姿勢角を算出する。例えば、位置姿勢角推定部１８は、尤度が最も高い仮想位置及び仮想姿勢角の候補を、車両の実際の位置及び姿勢角として算出する。また、各仮想位置及び仮想姿勢角の候補の尤度を用いて、仮想位置及び仮想姿勢角の重み付き平均を求め、その値を最終的な自車両の位置及び姿勢角としてもよい。また、ステップＳ１６０では、各パーティクルのリサンプリングを、各仮想位置及び仮想姿勢角の候補の尤度に基づいて行う。こうして自車両の位置と姿勢角が算出されると、本実施形態に係る移動物体の位置姿勢角算出処理を終了する。そして、算出された自車両の位置と姿勢角は、図２のステップＳ６０における推定結果として出力される。

［ピンホール画像が選択された場合の移動物体の位置姿勢角算出処理の手順］
次に、図２のステップＳ５０でピンホール画像が選択された場合の移動物体の位置姿勢角算出処理の手順について図５のフローチャートを参照して説明する。

ステップＳ２１０において、位置姿勢角推定部１８は、上述した図４のステップＳ１１０と同様の方法でピンホール画像からエッジ画像を算出する。

次に、ステップＳ２２０において、位置姿勢角推定部１８は、ステップＳ２１０で算出したエッジ画像にハフ（Ｈｏｕｇｈ）変換などを用いて、道路端部や区画線の線分がエッジ画像上のどこに存在するかを特定する。尚、三次元地図データベース３に、停止線、横断歩道、路面マークなどの路面表示や建物等の構造物のエッジ情報が含まれている場合には、これらの線分についてもエッジ画像上のどこに存在するかを特定する。

次に、ステップＳ２３０において、位置姿勢角推定部１８は、ステップＳ２２０で特定した道路端部等のエッジ画像上の位置に、三次元地図ＤＢ３に記録された道路端部の三次元位置及び姿勢角情報がマッチングするように調整して自車両の位置と姿勢角を算出する。

具体的には、ピンホール画像のカメラモデルを通じて三次元地図データベース３に記録された三次元地図データを投影し、投影された道路端部の三次元位置及び姿勢角が、エッジ画像上で特定された道路端部とマッチングするように調整する。そして、エッジ画像上で特定された道路端部と一致する三次元地図データの道路端部があれば、その道路端部の三次元位置と姿勢角を自車両の位置及び姿勢角として算出する。

尚、ステップＳ２３０では、例えば車両センサ群４のＧＰＳ受信機４１からの情報を用いて自車位置を絞り込んでから、エッジ画像上で特定された道路端部と、投影された三次元地図データの道路端部とをマッチングするように調整すればなお良い。

また、自車位置を絞り込む方法として、１ループ前の図２のステップＳ６０で算出された自車両の位置及び姿勢角を、図４のステップＳ１３０の方法で算出したオドメトリの分だけ移動させた近傍に、現在の自車両の位置及び姿勢角の真値があるとしてもよい。

こうして自車両の位置と姿勢角が算出されると、本実施形態に係る移動物体の位置姿勢角算出処理を終了する。そして、算出された自車両の位置と姿勢角は、図２のステップＳ６０における推定結果として出力される。

［第１実施形態の効果］
以上詳細に説明したように、本実施形態に係る移動物***置姿勢角推定装置では、移動物体の周囲環境を魚眼レンズで撮像した魚眼レンズ画像か、または魚眼レンズ画像の所定領域を撮像したピンホール画像に基づいて移動物体の位置及び姿勢角を推定する。これにより、信号機のような特定の装置がなくても移動物体の周囲環境から移動物体の位置及び姿勢角を推定することができる。

また、本実施形態に係る移動物***置姿勢角推定装置では、移動物体の進行方向における旋回半径を検出し、検出された旋回半径が所定値以上の場合には屈曲度が所定値未満であると判定してピンホール画像を選択する。一方、旋回半径が所定値未満の場合には屈曲度が所定値以上であると判定して魚眼レンズ画像を選択する。これにより、急カーブや交差点での右左折のようなシーンでも視野角の広い魚眼レンズ画像を用いることによって移動物体の位置及び姿勢角を推定することが可能となる。一方、直線路や緩いカーブ、交差点でも直進するような場合にはピンホール画像を用いることによって高速かつ高精度に移動物体の位置及び姿勢角を推定することができる。

また、本実施形態に係る移動物***置姿勢角推定装置では、魚眼レンズ画像の所定領域に対して歪み補正を行うことによって歪みの小さいピンホール画像を取得する。これにより、ピンホールカメラを設置することなくピンホール画像を取得できるので、コストを大幅に削減することができる。

さらに、本実施形態に係る移動物***置姿勢角推定装置では、移動物体の進行方向における左右の道路端部をピンホール画像に撮像できないと判定した場合にのみ魚眼レンズ画像を選択する。これにより、道路端部が撮像できる場合にはピンホール画像を選択できるので、ピンホール画像が選択される場合が増加し、移動物体の位置及び姿勢角の推定をよりロバストに、尚且つ高速に行うことができる。

［第２実施形態］
次に、本発明の第２実施形態に係る移動物***置姿勢角推定装置について図面を参照して説明する。尚、本実施形態に係る移動物***置姿勢角推定装置の構成は第１実施形態と同一なので、詳細な説明は省略する。

［移動物体の位置姿勢角推定処理の手順］
本実施形態に係る移動物体の位置姿勢角推定処理の手順を図６のフローチャートを参照して説明する。本実施形態では、自車両の車速が所定値未満であるか否かを判定するステップＳ２５を追加したことが、第１実施形態と相違している。

ステップＳ２５では、画像選択部１６が、車両センサ群４の車速センサ４５で取得した車速情報に基づいて、自車両の移動速度が所定値未満であるか否かを判定する。本実施形態では所定値として車速が３０[km/h]未満であるか否かを判定する。そして、車速が３０[km/h]未満である場合には、自車両の進行方向における旋回半径に関係なくステップＳ４０に進んで魚眼レンズ画像を選択する。すなわち、ステップＳ３０の処理を行わずにステップＳ４０に進む。一方、車速が３０[km/h]以上である場合には、ステップＳ３０に進む。

本実施形態では、自車両の移動速度が低く、高速な周期で移動物体の位置及び姿勢角を推定する必要性が低い場合には、魚眼レンズ画像を用いて移動物体の位置及び姿勢角を推定する。

［第２実施形態の効果］
以上詳細に説明したように、本実施形態に係る移動物***置姿勢角推定装置では、移動物体の移動速度が所定値未満である場合には魚眼レンズ画像を選択する。一般に低速で走行する渋滞中や交差点などではピンホール画像のように視野角が狭いと、障害物によって遮蔽されて道路端部が撮像できない可能性が高い。そこで、移動速度が所定値未満となる場合には道路端部が撮像されている可能性の高い魚眼レンズ画像を用いることにより、ロバストに移動物体の位置及び姿勢角を推定することが可能となる。

［第３実施形態］
次に、本発明の第３実施形態に係る移動物***置姿勢角推定装置について図面を参照して説明する。尚、本実施形態に係る移動物***置姿勢角推定装置の構成は第１実施形態と同一なので、詳細な説明は省略する。また、本実施形態は、第１実施形態だけでなく、第２実施形態にも適用可能である。

［移動物体の位置姿勢角推定処理の手順］
本実施形態に係る移動物体の位置姿勢角推定処理の手順を図７のフローチャートを参照して説明する。本実施形態では、自車両のピッチレートまたはロールレートが所定値以上であるか否かを判定するステップＳ２８を追加したことが、第１実施形態と相違している。

ステップＳ２８では、画像選択部１６が、車両センサ群４の三軸ジャイロセンサ４８で取得したピッチレートまたはロールレートが所定値以上であるか否かを判定する。本実施形態では所定値としてピッチレートまたはロールレートが３[deg/s]以上であるか否かを判定する。そして、ピッチレートまたはロールレートが３[deg/s]以上である場合には、自車両の進行方向における旋回半径に関係なくステップＳ５０に進んでピンホール画像を選択する。すなわち、ステップＳ３０の処理を行わずにステップＳ５０に進む。一方、ピッチレートまたはロールレートが３[deg/s]未満である場合には、ステップＳ３０に進む。

本実施形態では、自車両のピッチ角やロール角の時間変化率が大きい場合、例えば凹凸のある路面を走行しているような場合には、ピンホール画像を用いて自車両の位置及び姿勢角を推定する。ピッチ角やロール角の時間変化率が大きい場合には、カメラの視点が振動的になるので、レンズの歪みが大きい魚眼レンズ画像に撮像されている道路端部は、振動的に移動すると共にその形状も常に変化し続けてしまう。そのため、三次元地図データとのマッチングが難しくなり、演算負荷が大きくなる。そこで、このようなシーンでは、ピンホール画像を用いて移動物体の位置及び姿勢角を推定する。

［第３実施形態の効果］
以上詳細に説明したように、本実施形態に係る移動物***置姿勢角推定装置では、移動物体のピッチ角の時間変化率が所定値以上である場合にはピンホール画像を選択する。これにより、路面の凹凸や段差等によってカメラが振動的になったとしても、歪みの小さいピンホール画像を用いることができるので、よりロバストかつ高速に移動物体の位置及び姿勢角を推定することができる。

また、本実施形態に係る移動物***置姿勢角推定装置では、移動物体のロール角の時間変化率が所定値以上である場合にはピンホール画像を選択する。これにより、路面の凹凸や段差等によってカメラが振動的になったとしても、歪みの小さいピンホール画像を用いることができるので、よりロバストかつ高速に移動物体の位置及び姿勢角を推定することができる。

［第４実施形態］
次に、本発明の第４実施形態に係る移動物***置姿勢角推定装置について図面を参照して説明する。尚、本実施形態に係る移動物***置姿勢角推定装置の構成は第１実施形態と同一なので、詳細な説明は省略する。

ただし、本実施形態の画像選択部１６では、ピンホール画像取得部１４で取得したピンホール画像に基づいて移動物体の周囲環境が遮蔽されているか否かを判定し、遮蔽されている場合には屈曲度が所定値以上であると判定して魚眼レンズ画像を選択する。一方、遮蔽されていない場合には屈曲度が所定値未満であると判定してピンホール画像を選択する。

［移動物体の位置姿勢角推定処理の手順］
本実施形態に係る移動物体の位置姿勢角推定処理の手順を図８のフローチャートを参照して説明する。本実施形態では、ステップＳ３３０において自車両の周囲環境が遮蔽されているか否かを判定するようにしたことが、第１実施形態と相違している。

図８に示すように、ステップＳ３１０、３２０において、図２に示すステップＳ１０、２０と同一の処理を実行してピンホール画像を取得する。

ステップＳ３３０では、画像選択部１６が、ステップＳ３２０で取得したピンホール画像に基づいて移動物体である自車両の周囲環境が遮蔽されているか否かを判定する。遮蔽されているか否かの判定方法としては、以下の方法を用いて行う。

まず、ピンホール画像の全画素の輝度平均を求め、その輝度平均値が閾値ＴＨｍａｘ（単位：なし）以上、或いは閾値ＴＨｍｉｎ（単位：なし）以下である場合には遮蔽されていると判定する。すなわち、輝度平均が極端に大きい画像は太陽光を直接撮像しているような明るすぎる画像であり、逆に輝度平均が極端に小さい画像は暗がりを撮像しているような遮蔽された状態にある画像であると考えられるためである。このような画像は、後述する移動物体の位置及び姿勢角を推定する際に行うエッジ抽出で画像の特徴点を抽出し難い画像である。そのため、三次元地図データと一致するように調整することが難しいので、視野角を広げる必要があり、ピンホール画像ではなく魚眼レンズ画像に切り替える必要がある。尚、本実施形態では抽出した画像をＥＣＵ１に記録するときの輝度数値は、０〜２５５（０：最も暗い、２５５：最も明るい）の範囲内であり、ＴＨｍａｘには２４０、ＴＨｍｉｎには１５を設定する。

また、別の判定方法としては、自車両の周囲に存在する車両や歩行者などの障害物の位置と形状を検出し、その障害物が存在する方向の画像において、その障害物の遠方が遮蔽されているか否かを判定してもよい。障害物の検出方法としては、カメラ画像を用いたモーションステレオや自車両に別途レーザレンジファインダ等を搭載することによって障害物を検出すればよい。本実施形態では、障害物の位置と形状と、前ループにおいて推定した車両の位置と姿勢角から、ピンホール画像の画素のうちの何％に障害物が映りこんでいるかを推定する。その上で、ピンホール画像の中で５０％以上障害物が映りこんでいると推定された場合には、遠くを見渡せないと判断して遮蔽されていると判定する。

さらに、別の判定方法としては、三次元地図と前ループにおいて推定した車両の位置と姿勢角とから車両の前方において道路或いは地平面の勾配が上方に凸となっている場所を抽出することによって遮蔽の有無を判定してもよい。本実施形態では、勾配が上方に凸となっていると判断する基準は、車両から離れる方向に単位長さ（１ｍ）当り勾配変化量が２[deg]以上の場合とする。また、上方に凸となっている場所が、魚眼レンズカメラ２から１０[ｍ]以内の距離である場合も遮蔽されていると判断する。

また、上述の判定方法を行う際に、ピンホール画像において、移動物体の進行方向における左右の道路端部が遮蔽されていない場合には、周囲環境は遮蔽されていないと判定してもよい。すなわち、たとえピンホール画像の大部分が遮蔽されていたとしても区画線や縁石などの道路端部さえ検出できれば遮蔽されていないと判定する。例えば、ピンホール画像の上半分だけ、すなわち主に路面ではない領域だけが遮蔽されている場合には遮蔽されていないと判定する。また、ピンホール画像に対して二値化処理等を行って白線部分を検出し、白線が検出できた場合には遮蔽されていないと判定してもよい。

逆に、たとえ遮蔽されている部分が一部であっても区画線や縁石などの道路端部が遮蔽されている場合には、周囲環境は遮蔽されていると判定してもよい。例えば、ピンホール画像の下半分、すなわち主に路面を撮像した領域が遮蔽されている場合には、遮蔽されていると判定する。また、ピンホール画像に対して二値化処理等を行って白線部分を検出する処理を行い、白線が検出できない場合には遮蔽されていると判定してもよい。

上述した判定方法によって、画像選択部１６は、自車両の周囲環境が遮蔽されているか否かを判定し、遮蔽されている場合には屈曲度が所定値以上であると判定してステップＳ３４０に進む。一方、遮蔽されていない場合には屈曲度が所定値未満であると判定してステップＳ３５０に進む。

ステップＳ３４０では、画像選択部１６が、後述するステップＳ３６０の処理で用いる画像として魚眼レンズ画像を選択してステップＳ３６０に進む。通常、区画線や縁石などの道路端部は、走行路の屈曲度が大きい場合のほかに他車両等の障害物による影など光環境等によって遮蔽されやすいが、魚眼レンズ画像を用いれば視野角が広いので、道路端部が撮像されている可能性が高い。そこで、周囲環境が遮蔽されていると判定された場合には、魚眼レンズ画像を選択して確実に道路端部を検出し、移動物体の位置及び姿勢角を推定する。

ステップＳ３５０では、画像選択部１６が、後述するステップＳ３６０の処理で用いる画像としてピンホール画像を選択してステップＳ６０に進む。ピンホール画像は歪みが小さいので、ピンホール画像を用いれば、移動物体の位置及び姿勢角の推定をより高速かつ高精度に行うことができる。そこで、周囲環境が遮蔽されていないと判定された場合には、ピンホール画像を用いる。

こうして画像選択部１６によって魚眼レンズ画像かピンホール画像のいずれかが選択されると、ステップＳ３６０において図２のステップＳ６０と同一の処理が実行されて、本実施形態に係る移動物体の位置姿勢角推定処理を終了する。

［第４実施形態の効果］
以上詳細に説明したように、本実施形態に係る移動物***置姿勢角推定装置では、移動物体の周囲環境が遮蔽されているか否かを判定し、遮蔽されている場合には屈曲度が所定値以上であると判定して魚眼レンズ画像を選択する。一方、遮蔽されていない場合には屈曲度が所定値未満であると判定してピンホール画像を選択する。これにより、遮蔽されている場合でも視野角の広い魚眼レンズ画像を用いることによって移動物体の位置及び姿勢角を推定することが可能となる。一方、遮蔽されていない場合にはピンホール画像を用いることによって高速かつ高精度に移動物体の位置及び姿勢角を推定することができる。

また、本実施形態に係る移動物***置姿勢角推定装置では、魚眼レンズ画像の所定領域に対して歪み補正を行うことによって歪みの小さいピンホール画像を取得する。これにより、ピンホールカメラを設置することなくピンホール画像を取得できるので、コストを大幅に削減することができる。

さらに、本実施形態に係る移動物***置姿勢角推定装置では、移動物体の進行方向における左右の道路端部が遮蔽されていない場合には屈曲度が所定値未満であると判定してピンホール画像を選択する。これにより、道路端部さえ遮蔽されていなければピンホール画像を選択できるので、ピンホール画像が選択される場合が増加し、移動物体の位置及び姿勢角の推定をよりロバストに、尚且つ高速に行うことができる。

［第５実施形態］
次に、本発明の第５実施形態に係る移動物***置姿勢角推定装置について図面を参照して説明する。尚、本実施形態に係る移動物***置姿勢角推定装置の構成は第４実施形態と同一なので、詳細な説明は省略する。

［移動物体の位置姿勢角推定処理の手順］
本実施形態に係る移動物体の位置姿勢角推定処理の手順を図９のフローチャートを参照して説明する。本実施形態では、自車両の車速が所定値未満であるか否かを判定するステップＳ３２５を追加したことが、第４実施形態と相違している。

ステップＳ３２５では、画像選択部１６が、車両センサ群４の車速センサ４５で取得した車速情報に基づいて、自車両の移動速度が所定値未満であるか否かを判定する。本実施形態では所定値として車速が３０[km/h]未満であるか否かを判定する。そして、車速が３０[km/h]未満である場合には、ステップＳ３２０で取得したピンホール画像が遮蔽されているか否かにかかわらずステップＳ３４０に進む。すなわち、ステップＳ３３０の処理を行わずにステップＳ３４０に進む。一方、車速が３０[km/h]以上である場合には、ステップＳ３３０に進む。

本実施形態では、自車両の移動速度が低く、高速な周期で移動物体の位置及び姿勢角を推定する必要性が低い場合には、魚眼レンズ画像を用いて移動物体の位置及び姿勢角を推定する。

［第５実施形態の効果］
以上詳細に説明したように、本実施形態に係る移動物***置姿勢角推定装置では、移動物体の移動速度が所定値未満である場合には魚眼レンズ画像を選択する。これにより、一般に低速で走行する渋滞中や交差点など障害物によって遮蔽が多く発生するシーンでも道路端部が撮像された魚眼レンズ画像を用いることができるので、よりロバストに移動物体の位置及び姿勢角を推定することができる。

［第６実施形態］
次に、本発明の第６実施形態に係る移動物***置姿勢角推定装置について図面を参照して説明する。尚、本実施形態に係る移動物***置姿勢角推定装置の構成は第４実施形態と同一なので、詳細な説明は省略する。また、本実施形態は、第４実施形態だけでなく、第５実施形態にも適用可能である。

［移動物体の位置姿勢角推定処理の手順］
本実施形態に係る移動物体の位置姿勢角推定処理の手順を図１０のフローチャートを参照して説明する。本実施形態では、自車両のピッチレートまたはロールレートが所定値以上であるか否かを判定するステップＳ３２８を追加したことが、第４実施形態と相違している。

ステップＳ３２８では、画像選択部１６が、車両センサ群４の三軸ジャイロセンサ４８で取得したピッチレートまたはロールレートが所定値以上であるか否かを判定する。本実施形態では所定値としてピッチレートまたはロールレートが３[deg/s]以上であるか否かを判定する。そして、ピッチレートまたはロールレートが３[deg/s]以上である場合には、ステップＳ３２０で取得したピンホール画像が遮蔽されているか否かにかかわらずステップＳ３５０に進む。すなわち、ステップＳ３３０の処理を行わずにステップＳ３５０に進む。一方、ピッチレートまたはロールレートが３[deg/s]未満である場合には、ステップＳ３３０に進む。

本実施形態では、自車両のピッチ角やロール角の時間変化率が大きい場合、例えば凹凸のある路面を走行しているような場合には、ピンホール画像を用いて自車両の位置及び姿勢角を推定する。移動物体のピッチ角やロール角の時間変化率が大きい場合には、カメラの視点が振動的になるので、レンズの歪みが大きい魚眼レンズ画像に撮像されている道路端部は、振動的に移動すると共にその形状も常に変化し続けてしまう。そのため、三次元地図データとのマッチングが難しくなり、演算負荷が大きくなる。そこで、このようなシーンでは、ピンホール画像を用いて移動物体の位置及び姿勢角を推定する。

ただし、ピンホール画像上において左右の道路端部がほぼ完全に遮蔽されていることが検出された場合にはステップＳ３４０に進んで魚眼レンズ画像を用いるようにしてもよい。また、このような場合には、ステップＳ３３０以下の処理を中止して、図４のステップＳ１２０で示したオドメトリを算出して、移動物体の位置及び姿勢角を推定してもよい。すなわち、カメラの視点が振動的で、尚且つカメラが遮蔽されているような場合には、カメラの画像と三次元地図データとのマッチングを一時的に中断し、オドメトリから移動物体の位置及び姿勢角を推定するようにしてもよい。

［第６実施形態の効果］
以上詳細に説明したように、本実施形態に係る移動物***置姿勢角推定装置では、移動物体のピッチ角の時間変化率が所定値以上である場合にはピンホール画像を選択する。これにより、路面の凹凸や段差等によってカメラが振動的になったとしても、歪みの小さいピンホール画像を用いることができるので、よりロバストかつ高速に移動物体の位置及び姿勢角を推定することができる。

また、本実施形態に係る移動物***置姿勢角推定装置では、移動物体のロール角の時間変化率が所定値以上である場合にはピンホール画像を選択する。これにより、路面の凹凸や段差等によってカメラが振動的になったとしても、歪みの小さいピンホール画像を用いることができるので、よりロバストかつ高速に移動物体の位置及び姿勢角を推定することができる。

［第７実施形態］
次に、本発明の第７実施形態に係る移動物***置姿勢角推定装置について図面を参照して説明する。尚、第１〜第６実施形態と同一の構成要素には同一の番号を付して詳細な説明は省略する。また、本実施形態は、第１〜第６実施形態に適用可能である。

［移動物***置姿勢角推定装置の構成］
図１１は、本実施形態に係る移動物***置姿勢角推定装置を搭載した移動物***置姿勢角推定システムの構成を示すブロック図である。図１１に示すように、本実施形態に係る移動物***置姿勢角推定システムでは、ピンホールカメラ８１をさらに備えたことが第１〜第６実施形態と相違している。

ピンホールカメラ８１は、例えばＣＣＤ等の固体撮像素子を用いた狭角レンズカメラであり、本実施形態では、車両のフロントバンパーに光軸が水平で、尚且つ車両前方が撮像可能となるように設置されている。また、ピンホールカメラ８１の光軸は魚眼レンズカメラ２の光軸と一致しており、画角は約４０[deg]である。

ピンホールカメラ８１で撮像されたピンホール画像は、魚眼レンズ画像の所定領域である画角４０[deg]の範囲を撮像した画像であり、歪みの小さい画像である。ピンホール画像は撮像されると、ピンホールカメラ８１からＥＣＵ１へ送信され、ピンホール画像取得部１４が取得する。

したがって、本実施形態では、ピンホール画像取得部１４は、魚眼レンズ画像の所定領域に対して歪み補正を行わずに、ピンホールカメラ８１からピンホール画像を取得する。

［第７実施形態の効果］
以上詳細に説明したように、本実施形態に係る移動物***置姿勢角推定装置によれば、魚眼レンズ画像の所定領域を撮像するピンホールカメラからピンホール画像を取得するので、魚眼レンズ画像に対して歪み補正を行う必要がなく、処理負荷を軽減することができる。

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

本出願は、２０１２年１２月１２日に出願された日本国特許願第２０１２−２７０９７９号及び日本国特許願第２０１２−２７０９８０号に基づく優先権を主張しており、この出願の内容が参照により本発明の明細書に組み込まれる。

本発明の一態様に係る移動物***置姿勢角推定装置及び移動物***置姿勢角推定方法によれば、信号機のような特定の装置がなくても周囲環境から移動物体の位置及び姿勢角を推定することができる。したがって、本発明の一態様に係る移動物***置姿勢角推定装置及び移動物***置姿勢角推定方法は、産業上利用可能である。

１ ＥＣＵ
２ 魚眼レンズカメラ
３ 三次元地図データベース
４ 車両センサ群
５ ナビゲーションシステム
１０ 移動物***置姿勢角推定装置
１２ 魚眼レンズ画像取得部
１４ ピンホール画像取得部
１６ 画像選択部
１８ 位置姿勢角推定部
４１ ＧＰＳ受信機
４２ アクセルセンサ
４３ ステアリングセンサ
４４ ブレーキセンサ
４５ 車速センサ
４６ 加速度センサ
４７ 車輪速センサ
４８ 三軸ジャイロセンサ
８１ ピンホールカメラ

【０００５】
勢角推定部１８として動作する。
［００１７］
魚眼レンズ画像取得部１２は、移動物体の周囲環境を魚眼レンズで撮像した魚眼レンズ画像を魚眼レンズカメラ２から取得する。魚眼レンズ画像には移動物体の周囲環境が撮像されており、自車両の位置及び姿勢角を推定するために必要な路面情報として、少なくとも道路端部を示す区画線や縁石が撮像されている。また、この他に白線、停止線、横断歩道、路面マーク等の路面表示が撮像されていてもよい。
［００１８］
ピンホール画像取得部１４は、魚眼レンズ画像の所定領域について歪みの小さいピンホール画像を取得する。特に、本実施形態では、魚眼レンズカメラ２で撮像された魚眼レンズ画像の所定領域に対して歪み補正を行うことによって、歪みの小さいピンホール画像を取得する。ピンホール画像には魚眼レンズ画像と同様に移動物体の周囲環境が撮像されており、自車両の位置及び姿勢角を推定するために必要な路面情報として、少なくとも道路端部を示す区画線や縁石が撮像されている。また、この他に白線、停止線、横断歩道、路面マーク等の路面表示が撮像されていてもよい。
［００１９］
画像選択部１６は、移動物体の進行方向における走行路の屈曲度を検出し、検出された屈曲度が所定値未満の場合にはピンホール画像を選択し、屈曲度が所定値以上の場合には魚眼レンズ画像を選択する。ここで、走行路の屈曲度は、走行路がどの程度曲がっているかを示す指標であり、走行路が大きく曲がっていれば屈曲度の値は大きくなり、走行路が曲がっていなければ屈曲度の値は小さくなる。
［００２０］
特に、本実施形態では、移動物体の進行方向における旋回半径を検出し、検出された旋回半径が所定値より大きい場合には屈曲度が所定値未満であると判定してピンホール画像を選択する。一方、旋回半径が所定値以下の場合には屈曲度が所定値以上であると判定して魚眼レンズ画像を選択する。
［００２１］
位置姿勢角推定部１８は、画像選択部１６で選択された画像に基づいて三次元地図データとのマッチングを行って移動物体の位置及び姿勢角を推定する。特に、選択された画像のエッジ情報と、三次元地図データから抽出され

【０００７】
行シーンにおいて自車両前方の左右の道路端部が撮像されている。
［００２６］
次に、ステップＳ３０において、画像選択部１６は、移動物体である自車両の進行方向における旋回半径Ｒ［ｍ］を検出する。このとき旋回半径が変化する場合には、最も小さい旋回半径を旋回半径Ｒとして検出する。そして、検出された旋回半径Ｒが所定値Ｒｔｈ［ｍ］より大きい場合には屈曲度が所定値未満であると判定してステップＳ５０に進んでピンホール画像を選択する。一方、旋回半径Ｒが所定値Ｒｔｈ［ｍ］以下の場合には屈曲度が所定値以上であると判定してステップＳ４０に進んで魚眼レンズ画像を選択する。また、ターンシグナルのＯＮ、ＯＦＦを用いて画像を選択してもよい。尚、本実施形態では所定値Ｒｔｈとして３０ｍを設定する。
［００２７］
ここで、旋回半径の検出方法としては、以下の方法を用いて行えばよい。例えば、自車両の運転者のステアリング操作を車両センサ群４のステアリングセンサ４３で検出し、自車両のステアリング機構のステアリングギア比を乗じて前輪の転舵角を求める。そして、アッカーマンステアリングジオメトリや線形二輪モデル（「自動車の運動と制御」第３章，安部正人，山海堂）等の車両モデルを用いることによって、旋回半径を算出することができる。また、ナビゲーションシステム５で算出した自車両の走行ルートから三次元地図データベース３を参照し、走行ルート上での旋回半径を予め算出してＥＣＵ１に記録する。そして、１ループ前に算出された自車両の位置と姿勢角から現在の旋回半径Ｒを求めるようにしてもよい。
［００２８］
一方、ステップＳ３０では、旋回半径に応じて画像を選択するだけでなく、自車両の進行方向における左右の道路端部がピンホール画像に撮像できないと判定した場合にのみステップＳ４０に進んで魚眼レンズ画像を選択するようにしてもよい。
［００２９］
この場合には、まず走行ルート上の左右の道路端部の間隔、即ち道路の幅員を、三次元地図データベース３を参照して求める。そして、この幅員と、旋回半径Ｒと、魚眼レンズカメラ２の取り付け位置と、１ループ前の処理で算出した自車両の位置と姿勢角とを参照する。これらの情報から、画像選択

【００１５】
なくても移動物体の周囲環境から移動物体の位置及び姿勢角を推定することができる。
［００５８］
また、本実施形態に係る移動物***置姿勢角推定装置では、移動物体の進行方向における旋回半径を検出し、検出された旋回半径が所定値より大きい場合には屈曲度が所定値未満であると判定してピンホール画像を選択する。一方、旋回半径が所定値以下の場合には屈曲度が所定値以上であると判定して魚眼レンズ画像を選択する。これにより、急カーブや交差点での右左折のようなシーンでも視野角の広い魚眼レンズ画像を用いることによって移動物体の位置及び姿勢角を推定することが可能となる。一方、直線路や緩いカーブ、交差点でも直進するような場合にはピンホール画像を用いることによって高速かつ高精度に移動物体の位置及び姿勢角を推定することができる。
［００５９］
また、本実施形態に係る移動物***置姿勢角推定装置では、魚眼レンズ画像の所定領域に対して歪み補正を行うことによって歪みの小さいピンホール画像を取得する。これにより、ピンホールカメラを設置することなくピンホール画像を取得できるので、コストを大幅に削減することができる。
［００６０］
さらに、本実施形態に係る移動物***置姿勢角推定装置では、移動物体の進行方向における左右の道路端部をピンホール画像に撮像できないと判定した場合にのみ魚眼レンズ画像を選択する。これにより、道路端部が撮像できる場合にはピンホール画像を選択できるので、ピンホール画像が選択される場合が増加し、移動物体の位置及び姿勢角の推定をよりロバストに、尚且つ高速に行うことができる。
［００６１］
［第２実施形態］
次に、本発明の第２実施形態に係る移動物***置姿勢角推定装置について図面を参照して説明する。尚、本実施形態に係る移動物***置姿勢角推定装置の構成は第１実施形態と同一なので、詳細な説明は省略する。
［００６２］
［移動物体の位置姿勢角推定処理の手順］
本実施形態に係る移動物体の位置姿勢角推定処理の手順を図６のフローチャートを参照して説明する。本実施形態では、自車両の車速が所定値未満で

ステップＳ３５０では、画像選択部１６が、後述するステップＳ３６０の処理で用いる画像としてピンホール画像を選択してステップＳ３６０に進む。ピンホール画像は歪みが小さいので、ピンホール画像を用いれば、移動物体の位置及び姿勢角の推定をより高速かつ高精度に行うことができる。そこで、周囲環境が遮蔽されていないと判定された場合には、ピンホール画像を用いる。

Claims (12)

1. 移動物体の位置及び姿勢角を推定する移動物***置姿勢角推定装置であって、
   前記移動物体の周囲環境を魚眼レンズで撮像した魚眼レンズ画像を取得する魚眼レンズ画像取得部と、
   前記魚眼レンズ画像の所定領域について歪みの小さいピンホール画像を取得するピンホール画像取得部と、
   前記移動物体の進行方向における走行路の屈曲度を検出し、前記屈曲度が所定値未満の場合には前記ピンホール画像を選択し、前記屈曲度が所定値以上の場合には前記魚眼レンズ画像を選択する画像選択部と、
   前記画像選択部で選択された画像のエッジ情報と、三次元地図データから抽出された仮想画像のエッジ情報とのマッチングを行って前記移動物体の位置及び姿勢角を推定する位置姿勢角推定部と
   を備えたことを特徴とする移動物***置姿勢角推定装置。
2. 前記画像選択部は、前記移動物体の進行方向における旋回半径を検出し、前記旋回半径が所定値以上の場合には前記屈曲度が所定値未満であると判定して前記ピンホール画像を選択し、前記旋回半径が所定値未満の場合には前記屈曲度が所定値以上であると判定して前記魚眼レンズ画像を選択することを特徴とする請求項１に記載の移動物***置姿勢角推定装置。
3. 前記画像選択部は、前記移動物体の進行方向における左右の道路端部を前記ピンホール画像に撮像できないと判定した場合にのみ前記魚眼レンズ画像を選択することを特徴とする請求項２に記載の移動物***置姿勢角推定装置。
4. 前記画像選択部は、前記ピンホール画像取得部で取得したピンホール画像に基づいて前記移動物体の周囲環境が遮蔽されているか否かを判定し、遮蔽されている場合には前記屈曲度が所定値以上であると判定して前記魚眼レンズ画像を選択し、遮蔽されていない場合には前記屈曲度が所定値未満であると判定して前記ピンホール画像を選択することを特徴とする請求項１に記載の移動物***置姿勢角推定装置。
5. 前記画像選択部は、前記移動物体の進行方向における左右の道路端部が遮蔽されていない場合には、前記屈曲度が所定値未満であると判定して前記ピンホール画像を選択することを特徴とする請求項４に記載の移動物***置姿勢角推定装置。
6. 前記ピンホール画像取得部は、前記魚眼レンズ画像取得部によって取得した魚眼レンズ画像の所定領域に対して歪み補正を行うことによって歪みの小さいピンホール画像を取得することを特徴とする請求項１〜５のいずれか１項に記載の移動物***置姿勢角推定装置。
7. 前記ピンホール画像取得部は、前記魚眼レンズ画像の所定領域を撮像するピンホールカメラから前記ピンホール画像を取得することを特徴とする請求項１〜５のいずれか１項に記載の移動物***置姿勢角推定装置。
8. 前記画像選択部は、前記移動物体の移動速度が所定値未満である場合には、前記魚眼レンズ画像を選択することを特徴とする請求項１〜７のいずれか１項に記載の移動物***置姿勢角推定装置。
9. 前記画像選択部は、前記移動物体のピッチ角の時間変化率が所定値以上である場合には、前記ピンホール画像を選択することを特徴とする請求項１〜８のいずれか１項に記載の移動物***置姿勢角推定装置。
10. 前記画像選択部は、前記移動物体のロール角の時間変化率が所定値以上である場合には、前記ピンホール画像を選択することを特徴とする請求項１〜９のいずれか１項に記載の移動物***置姿勢角推定装置。
11. 移動物体の位置及び姿勢角を推定する移動物***置姿勢角推定装置における移動物***置姿勢角推定方法であって、
    前記移動物体の周囲環境を魚眼レンズで撮像した魚眼レンズ画像を取得し、
    前記魚眼レンズ画像の所定領域について歪みの小さいピンホール画像を取得し、
    前記移動物体の進行方向における走行路の屈曲度を検出し、前記屈曲度が所定値未満の場合には前記ピンホール画像を選択し、前記屈曲度が所定値以上の場合には前記魚眼レンズ画像を選択し、
    前記選択された画像のエッジ情報と、三次元地図データから抽出された仮想画像のエッジ情報とのマッチングを行って前記移動物体の位置及び姿勢角を推定する
    ことを特徴とする移動物***置姿勢角推定方法。
12. 移動物体の位置及び姿勢角を推定する移動物***置姿勢角推定装置であって、
    前記移動物体の周囲環境を魚眼レンズで撮像した魚眼レンズ画像を取得する魚眼レンズ画像取得手段と、
    前記魚眼レンズ画像の所定領域について歪みの小さいピンホール画像を取得するピンホール画像取得手段と、
    前記移動物体の進行方向における走行路の屈曲度を検出し、前記屈曲度が所定値未満の場合には前記ピンホール画像を選択し、前記屈曲度が所定値以上の場合には前記魚眼レンズ画像を選択する画像選択手段と、
    前記画像選択手段で選択された画像のエッジ情報と、三次元地図データから抽出された仮想画像のエッジ情報とのマッチングを行って前記移動物体の位置及び姿勢角を推定する位置姿勢角推定手段と
    を備えたことを特徴とする移動物***置姿勢角推定装置。

Images