特許第6044084号(P6044084)IP Force 特許公報掲載プロジェクト 2015.5.11 β版

知財求人 - 知財ポータルサイト「IP Force」

▶ 日産自動車株式会社の特許一覧
<>
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000002
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000003
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000004
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000005
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000006
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000007
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000008
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000009
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000010
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000011
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000012
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000013
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000014
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000015
  • 特許6044084-移動物体位置姿勢推定装置及び方法 図000016
< >
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】6044084
(24)【登録日】2016年11月25日
(45)【発行日】2016年12月14日
(54)【発明の名称】移動物体位置姿勢推定装置及び方法
(51)【国際特許分類】
   G08G 1/16 20060101AFI20161206BHJP
   B60R 21/00 20060101ALI20161206BHJP
   G06T 1/00 20060101ALI20161206BHJP
【FI】
   G08G1/16 C
   B60R21/00 628Z
   G06T1/00 330B
【請求項の数】6
【全頁数】18
(21)【出願番号】特願2012-51623(P2012-51623)
(22)【出願日】2012年3月8日
(65)【公開番号】特開2013-186718(P2013-186718A)
(43)【公開日】2013年9月19日
【審査請求日】2015年2月3日
(73)【特許権者】
【識別番号】000003997
【氏名又は名称】日産自動車株式会社
(74)【代理人】
【識別番号】100083806
【弁理士】
【氏名又は名称】三好 秀和
(74)【代理人】
【識別番号】100100712
【弁理士】
【氏名又は名称】岩▲崎▼ 幸邦
(74)【代理人】
【識別番号】100095500
【弁理士】
【氏名又は名称】伊藤 正和
(74)【代理人】
【識別番号】100101247
【弁理士】
【氏名又は名称】高橋 俊一
(74)【代理人】
【識別番号】100098327
【弁理士】
【氏名又は名称】高松 俊雄
(72)【発明者】
【氏名】山口 一郎
【審査官】 相羽 昌孝
(56)【参考文献】
【文献】 特開2010−175423(JP,A)
【文献】 特開2009−199572(JP,A)
【文献】 特開2007−183432(JP,A)
【文献】 米国特許出願公開第2007/0285217(US,A1)
(58)【調査した分野】(Int.Cl.,DB名)
G08G 1/00−99/00
G01C 21/00−21/36
G01C 23/00−25/00
B60R 21/00−21/13
B60R 21/34−21/38
G06T 1/00− 1/40
G06T 3/00− 9/40
(57)【特許請求の範囲】
【請求項1】
移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定装置であって、
移動物体の前方を撮像して前方撮像画像を取得する前方撮像手段と、
移動物体の右側又は左側を撮像して側方撮像画像を取得する側方撮像手段と、
次元地図データを前記前方撮像手段により仮想的に撮像した画像に変換して仮想前方画像を取得すると共に、次元地図データを前記側方撮像手段により仮想的に撮像した画像に変換して仮想側方画像を取得する仮想画像取得手段と、
移動物体の走行状態を検出する走行状態検出手段と、
前記走行状態検出手段により検出された移動物体の前記走行状態が加速又は一定速である場合には、前記前方撮像画像と当該前方撮像画像と同じ方向に投影された前記仮想前方画像との一致度に基づいて、実際の移動物体の姿勢角を推定すると共に、前記側方撮像画像と当該側方撮像画像と同じ方向に投影された前記仮想側方画像との一致度に基づいて、実際の移動物体の位置を推定する移動物体位置姿勢推定手段と
を有することを特徴とする移動物体位置姿勢推定装置。
【請求項2】
移動物体の後方を撮像して後方撮像画像を取得する後方撮像手段を更に有し、
前記仮想画像取得手段は、3次元地図データを前記後方撮像手段により仮想的に撮像した画像に変換して仮想後方画像を取得し、
前記移動物体位置姿勢推定手段は、前記走行状態検出手段により移動物体が減速していることが検出された場合に、前記後方撮像画像と当該後方撮像画像と同じ方向に投影された仮想後方画像との一致度に基づいて、実際の移動物体の姿勢角を推定すること
を特徴とする請求項1に記載の移動物体位置姿勢推定装置。
【請求項3】
前記移動物体位置姿勢推定手段は、前記走行状態検出手段により移動物体の旋回半径が減少することが検出された場合には前記後方撮像画像と前記仮想後方画像との一致度に基づいて、実際の移動物体の姿勢角を推定し、前記走行状態検出手段により移動物体の旋回半径が増大することが検出された場合には前記前方撮像画像と前記仮想前方画像との一致度に基づいて、実際の移動物体の姿勢角を推定することを特徴とする請求項2に記載の移動物体位置姿勢推定装置。
【請求項4】
前記側方撮像手段は、移動物体の右側及び左側の双方を撮像して右側撮像画像及び左側撮像画像を取得し、
前記移動物体位置姿勢推定手段は、前記走行状態検出手段により検出された移動物体の旋回方向の内側方向を撮像する前記右側撮像画像又は前記左側撮像画像を用いて実際の移動物体の位置を推定することを特徴とする請求項1乃至請求項3の何れか一項に記載の移動物体位置姿勢推定装置。
【請求項5】
前記側方撮像手段は、移動物体の右側及び左側の双方を撮像して右側撮像画像及び左側撮像画像を取得し、
前記移動物体位置姿勢推定手段は、前記走行状態検出手段により検出された移動物体の横方向への移動方向を撮像する前記右側撮像画像又は前記左側撮像画像を用いて実際の移動物体の位置を推定することを特徴とする請求項1乃至請求項4の何れか一項に記載の移動物体位置姿勢推定装置。
【請求項6】
前方撮像手段と、側方撮像手段と、3次元地図データと、移動物体の走行状態を検出する走行状態検出手段と、を備え、移動物体の位置及び姿勢角を推定する移動物体位置姿勢推定装置の移動物体位置姿勢推定方法であって、
前記前方撮像手段が移動物体の前方を撮像して前方撮像画像を取得するステップと、
前記側方撮像手段が移動物体の右側及び左側を撮像して側方撮像画像を取得するステップと、
前記3次元地図データを前記前方撮像手段により仮想的に撮像した画像に変換して仮想前方画像を取得すると共に、前記3次元地図データを前記側方撮像手段により仮想的に撮像した画像に変換して仮想側方画像を取得するステップと、
移動物体の走行状態を検出するステップと、
前記走行状態検出手段により検出された移動物体の前記走行状態が加速又は一定速である場合には、前記前方撮像画像と当該前方撮像画像と同じ方向に投影された前記仮想前方画像との一致度に基づいて、実際の移動物体の姿勢角を推定すると共に、前記側方撮像画像と当該側方撮像画像と同じ方向に投影された前記仮想側方画像との一致度に基づいて、実際の移動物体の位置を推定するステップと
を有することを特徴とする移動物体位置姿勢推定装置の移動物体位置姿勢推定方法。

【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動物体の位置を推定する移動物体位置姿勢推定装置及び方法に関する。
【背景技術】
【0002】
次元地図とカメラの撮像画像を比較することによって移動物体の位置を算出する技術として、例えば下記の特許文献1に記載された技術が知られている。この特許文献1には、車両に備えられた車載カメラにより得られる現実の映像から抽出したエッジ画像と、周囲環境のエッジの位置や形状を次元で記録した次元地図を車載カメラの位置姿勢に投影したときの仮想映像とが一致するように車載カメラの位置と姿勢角を調整する。これによって、車載カメラの次元空間での位置と姿勢角を推定している。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開2009-199572号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、特許文献1において、現実の映像と仮想映像が一致していても、この一致した場所が車載カメラから遠い場合、車載カメラの位置の誤差が大きい可能性がある。逆に、一致した位置が車載カメラから近い場合、車載カメラの姿勢角の誤差が大きい可能性がある。
【0005】
そこで、本発明は、上述した実情に鑑みて提案されたものであり、精度良く移動物体の位置及び姿勢角を推定することができる移動物体位置姿勢推定装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【0006】
本発明は、走行状態が加速又は一定速である場合には、前方撮像画像と当該前方撮像画像と同じ方向に投影された仮想前方画像との一致度に基づいて、実際の移動物体の姿勢角を推定する。また、側方撮像画像と当該側方撮像画像と同じ方向に投影された仮想側方画像との一致度に基づいて、実際の移動物体の位置を推定する。
【発明の効果】
【0007】
本発明によれば、自車両の姿勢角を前方撮像画像を用いて推定でき、自車両の位置を側方撮像画像を用いて推定できるので、精度良く移動物体の位置及び姿勢角を推定することができる。
【図面の簡単な説明】
【0008】
図1】本発明の実施形態として示す移動物体位置姿勢推定装置の構成を示すブロック図である。
図2】自車両の方向を示す上面図である。
図3】カメラの画角及び撮像方向を示す図である。
図4】(a)は前方カメラ及び左側方カメラが搭載された自車両の上面図、(b)は前方カメラ及び右側方カメラが搭載された自車両の上面図である。
図5】(a)は前方カメラ、左側方カメラ及び後方カメラが搭載された自車両の上面図、(b)は前方カメラ、右側方カメラ及び後方カメラが搭載された自車両の上面図である。
図6】前方カメラ、左側方カメラ及び右側方カメラが搭載された自車両の上面図である。
図7】(a)は前方カメラ、左側方カメラ、右側方カメラ及び後方カメラが搭載された自車両の上面図、(b)は全方位カメラが搭載された自車両の上面図である。
図8】(a)は撮像画像、(b)は(a)のエッジ画像、(c)は仮想画像、(d)は(c)の仮想位置が右方向にずれた場合の仮想画像、である。
図9】本発明の実施形態として示す移動物体位置姿勢推定装置の動作手順を示すフローチャートである。
図10】本発明の実施形態として示す移動物体位置姿勢推定装置において、車速と前後方向カメラの画角との関係を示す図である。
図11】本発明の実施形態として示す移動物体位置姿勢推定装置において、車速と左右横方向カメラの画角との関係を示す図である。
図12】本発明の実施形態として示す移動物体位置姿勢推定装置において、走行状態に対する前方撮像画像及び左側方撮像画像の姿勢推定用画像尤度を示す図である。
図13】本発明の実施形態として示す移動物体位置姿勢推定装置において、走行状態に対する前方撮像画像及び左側方撮像画像の位置推定用画像尤度を示す図である。
図14】本発明の実施形態として示す移動物体位置姿勢推定装置において、走行状態に対する前方撮像画像、後方撮像画像、左側方撮像画像及び右側方撮像画像の姿勢角推定用画像尤度を示す図である。
図15】本発明の実施形態として示す移動物体位置姿勢推定装置において、走行状態に対する前方撮像画像、後方撮像画像、左側方撮像画像及び右側方撮像画像の位置推定用画像尤度を示す図である。
【発明を実施するための形態】
【0009】
以下、本発明の実施の形態について図面を参照して説明する。
【0010】
本発明の実施形態として示す移動物体位置姿勢推定装置は、例えば図1に示すように構成される。この移動物体位置姿勢推定装置は、ECU1、カメラ群2、3次元地図データベース3、及び、車両センサ群4を含む。車両センサ群4は、GPS受信機41、アクセルセンサ42、ステアリングセンサ43、ブレーキセンサ44、車速センサ45、加速度センサ46、車輪速センサ47、及び、ヨーレートセンサ等のその他センサ48を含む。なお、ECU1は、実際にはROM、RAM、演算回路等にて構成されている。ECU1がROMに格納された移動物体位置姿勢推定用のプログラムに従って処理をすることによって、後述する機能(仮想画像取得手段、画像尤度設定手段、移動物体位置姿勢推定手段)を実現する。
【0011】
カメラ群2は、複数の撮像手段(カメラ)を含む。カメラ群2の各カメラは例えばCCD等の固体撮像素子を用いたものである。カメラ群2は、少なくとも車両の前方カメラ(前方撮像手段)と、左側又は左側の何れか一方を撮像する側方カメラ(側方撮像手段)とを含む。
【0012】
ここで、図2に示すように、自車両Vの前方向とは自車両Vの主たる進行方向である。自車両Vの後方向とは前方向とは逆の方向である。自車両Vの右方向とは前方向を見た場合の右手方向である。自車両Vの左方向とは前方向を見た場合の左手方向である。また、カメラ群2に含まれるカメラは、図3に示すように、所定の撮像画角θであり、カメラの撮像方向を中心とした撮像範囲21となっている。
【0013】
カメラ群2の車載例を図3に示す。図4(a)、(b)は本実施形態における最小構成を示す。図4(a)のカメラ群2は自車両Vの前方が撮像範囲21fとされた前方カメラ2fと自車両Vの左方向が撮像範囲21lとされた左側方カメラ2lとを含む。図4(b)のカメラ群2は自車両Vの前方が撮像範囲21fとされた前方カメラ2fと自車両Vの右方向が撮像範囲21rとされた右側方カメラ2rとを含む。
【0014】
また、カメラ群2は、図5乃至図7の何れかの構成であってもよい。図5(a)、(b)に示したカメラ群2は、図4(a)、(b)の構成に加えて、自車両Vの後方向が撮像範囲21bとされた後方カメラ2bを含む。図6に示したカメラ群2は、前方カメラ2fと、自車両Vの右方向及び左方向の双方を撮像可能なように左側方カメラ2l及び右側方カメラ2rの双方を含む。図7(a)、(b)に示したカメラ群2は、自車両Vの全方向を撮像範囲とするものである。図7(a)に示したカメラ群2は、自車両Vの前方向、右方向、左方向、後方向のすべてが撮像可能なように前方カメラ2f、左側方カメラ2l、右側方カメラ2r、及び、後方カメラ2bを含む。図7(b)に示すカメラ群2は、自車両Vの全方向を切れ目なく撮像範囲21aとされた全方位カメラ2aのみを含む。なお、図4乃至図7に示したカメラ群2ではなく、自車両Vの前方や側方等を撮像できるような画角がカバーできていれば、例えば自車両Vに対して斜めにカメラを設置してもよい。
【0015】
カメラ群2に含まれるカメラは、所定時間毎に撮像して撮像画像を取得し、ECU1に供給する。
【0016】
3次元地図データベース3は、例えば路面表示を含む周囲環境のエッジ等の次元位置情報が記憶されている。本実施形態において、3次元地図データベース3には、白線、停止線、横断歩道、路面マーク等の路面表示の他に、縁石、建物等の構造物のエッジ情報も含まれる。白線などの各地図情報は、エッジの集合体で定義される。エッジが長い直線の場合には、例えば1m毎に区切られるため、極端に長いエッジは存在しない。直線の場合には、各エッジは、直線の両端点を示す3次元位置情報を持っている。曲線の場合には、各エッジは、曲線の両端点と中央点を示す3次元位置情報を持っている。
【0017】
車両センサ群(走行状態検出手段)4は、ECU1に接続される。車両センサ群4は、各センサ41〜48により検出した各種のセンサ値をECU1に供給する。ECU1は、車両センサ群4の出力値を用いることで、自車両Vの概位置の算出、単位時間に自車両Vが進んだ移動量を示すオドメトリを算出する。特に、車両センサ群4は、自車両Vの走行状態を検出する。
【0018】
また、ECU1は、カメラ群2により撮像された撮像画像と3次元地図データベース3に記憶された3次元位置情報とを用いて自車両Vの位置及び姿勢角の推定を行う電子制御ユニットである。特に、ECU1は、車両センサ群4により検出された自車両Vの走行状態に基づいて、自車両Vの位置及び姿勢角の推定を行う。なお、ECU1は、他の制御に用いるECUと兼用しても良い。
特に、この移動物体位置姿勢推定装置は、カメラ群2により撮像した撮像画像と、次元地図データを仮想位置及び仮想姿勢角から撮像した画像に変換した仮想画像とを比較して、自車両Vの位置及び姿勢角を推定する。ここで、図8(a)に示すような撮像画像が得られ、図8(b)のようなエッジ画像が得られたとする。一方、3次元位置情報をカメラ群2の位置及び姿勢角に投影した仮想画像が図8(c)のようになったとする。図8(b)の撮像画像と図8(c)の仮想画像とを比較すると、遠方位置(A)及び近傍位置(B)の双方で一致しているため、仮想画像を生成した仮想位置及び仮想姿勢角が、自車両Vの位置及び姿勢角に相当すると推定できる。しかし、仮想位置が右方向に位置ずれした場合には、仮想画像は、図8(d)に示すようになる。この場合、図8(a)の撮像画像と図8(d)の撮像画像とを比較すると、遠方位置(A)は一致しているが、近傍位置(B)は大きくずれてしまう。逆に、図8(c)の仮想画像の仮想姿勢角をずらし(図示せず)、図8(a)の撮像画像と比較すると、近傍位置(B)は一致するが、遠方位置(A)は大きくずれる。
【0019】
このような現象に着目して、移動物体位置姿勢推定装置は、撮像画像内の近傍位置画素と仮想画像内の近傍位置画素とが一致した場合には仮想画像の仮想位置が尤もらしいと判断する。逆に、移動物体位置姿勢推定装置は、撮像画像内の遠方位置画素と仮想画像内の遠方位置画素とが一致した場合には仮想画像の仮想姿勢角が尤もらしいと判断する。
【0020】
以下、この移動物体位置姿勢推定装置の動作について、図9の位置姿勢推定アルゴリズムを参照して説明する。なお、本実施形態は、自車両Vの6自由度の位置(前後方向,横方向,上下方向)及び姿勢角(ロール,ピッチ,ヨー)を推定するものとする。また、この図9の位置姿勢推定アルゴリズムは、ECU1によって、例えば100msec程度の間隔で連続的に行われる。
【0021】
ステップS1において、ECU1は、車両センサ群4から得られるセンサ値の情報から車速V(単位:m/s)とヨーレートγ(単位:rad/s)を検出する。ECU1は、旋回半径ρ(単位:m)を、ρ=V/γなる演算によって取得する。更に、1ループ前の位置姿勢推定アルゴリズムにおけるステップS1を実行した時から自車両Vの移動量をオドメトリとして算出する。なお、位置姿勢推定アルゴリズムを開始して最初のループの場合は、オドメトリをゼロとして算出する。また、ヨーレートγが十分に小さい場合には、旋回半径ρは例えば1000[m]に設定して、車速を略ゼロで除算することを回避する。
【0022】
このオドメトリ算出方法としては、例えば、車両運動を平面上に限定した上で、各車輪の車輪速とヨーレートセンサから、単位時間での移動量と回転量を算出すれば良い。また、ECU1は、車輪速を車速やGPS受信機41の測位値の差分で代用してもよく、ヨーレートセンサを操舵角で代用してもよい。なお、オドメトリの算出方法は、様々な算出手法が考えられるが、オドメトリが算出できればどの手法を用いても良い。
【0023】
次のステップS2において、ECU1は、ステップS1にて検出した車速Vに基づいて、自車両Vの前後方向の撮像画像の取得範囲を設定する。本実施形態では、撮像範囲の取得範囲を自車両Vの前後方向を撮像する前方カメラ2f又は後方カメラ2b(前後方向カメラ)の水平画角αf(単位:deg)とする。この前後方向カメラの画角は、図10に示すように、車速Vに応じて設定される。車速Vが高いほど前後方向カメラの画角は狭くなる。なお、本実施形態は、自車両Vに車載されて前後方向を撮像する前方カメラ2f又は後方カメラ2bは、水平画角の最大値が90[deg]以上のものであることが望ましい。
【0024】
なお、自車両Vの前後左右方向の撮像画像のうち、前後方向の撮像画像については車速Vが高くなるほど撮像範囲が狭くなる画像とする。これは、前後方向の撮像画像は、自車両Vの速度が低くなるほど横方向の運動の影響を受けやすくなるため、より広い範囲で撮像して3次元地図データと一致するように調整する必要があるためである。
【0025】
次のステップS3において、ECU1は、カメラ群2によって撮像された撮像画像のうち、ステップS2にて設定した前後方向の撮像画像の取得範囲に亘って、前方向の画像である前方撮像画像を抽出する。また、ECU1は、撮像画像のうち、ステップS2にて設定した前後方向の撮像画像の取得範囲に亘って、後方撮像画像を抽出する。これにより、ECU1は、前後撮像画像群を抽出する。ECU1は、後述するように位置推定用画像又は姿勢角推定用画像として選択する画像の撮像方向に応じて、前後一方の撮像画像を抽出してもよく、双方の撮像画像を抽出してもよい。後述する処理において後方撮像画像を使用しない場合には、後方撮像画像を抽出する必要はない。なお、抽出する画像は、図3のように、自車両Vの前方方向から左右にそれぞれ水平画角αf/2の角度θの範囲内で撮像された部分である。
【0026】
ステップS4において、ECU1は、ステップS1で算出した旋回半径ρに基づいて左方向及び右方向の画像の取得範囲を設定する。本実施形態において、左方向及び右方向の画像取得範囲を自車両Vの左右横方向を撮像する左側方カメラ2l又は右側方カメラ2rの水平画角αs(単位:deg)とする。この左方向及び右方向カメラの画角は、図11に示すように、旋回半径ρを参照して設定される。旋回半径ρが増大するほど左方向及び右方向カメラの画角は狭くなる。なお、左側方カメラ2l及び右側方カメラ2rは、例えば水平画角の最大値が90[deg]以上のものであることが望ましい。
【0027】
なお、自車両Vの前後左右方向の撮像画像のうち、左右横方向の撮像画像については旋回半径が増大するほど撮像範囲が狭くなる画像とする。これは、左右横方向の撮像画像は、旋回半径が減少する、すなわち急カーブであるほど当該カーブの状態をより広い範囲で撮像して、3次元地図データと一致するように調整する必要があるためである。
【0028】
次のステップS5において、ECU1は、カメラ群2によって撮像された撮像画像のうち、ステップS4にて設定した左方向及び右方向の撮像画像の取得範囲に亘って、左方向又は右方向の何れかの画像(側方撮像画像)を抽出する。ECU1は、後述するように位置推定用画像又は姿勢角推定用画像として使用する画像の撮像方向に応じて、左右一方の側方撮像画像を抽出してもよく、双方の側方撮像画像を抽出してもよい。これにより、ECU1は、側方撮像画像群を抽出する。なお、抽出する画像は、図3のように、自車両Vの左右横方向から左右にそれぞれ水平画像αs/2の角度θの範囲内で撮像された部分である。
【0029】
ステップS6において、ECU1は、車両センサ群4により検出された走行状態に基づいて、ステップS3及びステップS5にて取得された撮像画像のそれぞれについて、位置推定用画像尤度及び姿勢角推定用画像尤度を設定する(画像尤度設定手段)。位置推定用画像尤度は、自車両Vの位置を推定するための画像として適している度合いを表す。姿勢角推定用画像尤度は、自車両Vの姿勢角を推定するための画像として適している度合いを表す。この位置推定用画像尤度及び姿勢角推定用画像尤度は、後述するように位置推定用尤度と姿勢角推定用尤度を求める際に参照される。
【0030】
ECU1は、通常の走行状態である場合には、前後撮像画像群のうち前方撮像画像についての姿勢角推定用画像尤度を高める。また、ECU1は、通常の走行状態である場合には、側方撮像画像群についての位置推定用画像尤度を高める。ここで、通常の走行状態とは、自車両Vが加速又は一定速となっている状況である。
【0031】
自車両Vの前方は横方向に比べて見通しが良いために、前方撮像画像は自車両Vから遠くを撮像していることが多い。一方、側方撮像画像は自車両Vから近くを撮像していることが多い。したがって、移動物体位置姿勢推定装置は、上述した図8を参照して説明したように、自車両Vから遠くを撮像している前方撮像画像を姿勢角推定用画像として使用するために姿勢角推定用画像尤度を高める。一方、移動物体位置姿勢推定装置は、自車両Vから近くを撮像している側方撮像画像を位置推定用画像として使用するために位置推定用画像尤度を高める。
【0032】
この処理のため、ECU1は、例えば図12及び図13に示すようなテーブルデータを参照してもよい。なお、図12及び図13は、図4(a)のように、前方カメラ2f及び左側方カメラ2lのみを搭載した自車両Vについて設定したものである。また、このテーブルデータは、位置推定用画像尤度及び姿勢角推定用画像尤度を0〜10の範囲で設定したものであり、この数値には限定されないことは勿論である。
【0033】
図12に示すテーブルデータは、走行状態に応じて姿勢角推定用画像尤度を設定するためのデータである。図12によれば、ECU1は、どのような走行状態であっても、概ね、側方撮像画像(左側画像)の姿勢角推定用画像尤度よりも、前方撮像画像の姿勢角推定用画像尤度を高めることができる。図13に示すテーブルデータは、走行状態に応じて位置推定用画像尤度を設定するためのデータである。図13によれば、ECU1は、どのような走行状態であっても、概ね、前方撮像画像の位置推定用画像尤度よりも、側方撮像画像(左側画像)の位置推定用画像尤度を高めることができる。
【0034】
また、ECU1は、自車両Vが減速している、すなわち車速Vが継続して減少している場合には、前方撮像画像も位置の推定に適しているために位置推定用画像尤度を高めてもよい。図13に示すように、自車両Vが減速しており、且つ、直進方向に走行している場合には、他の走行状態と比較して前方撮像画像の位置推定用画像尤度を高めている。これは、自車両Vが減速する場合には、前方に減速する対象である、停止線、交差点、カーブがあり、前方撮像画像によって近傍の構造物が撮像可能であることが多いためである。
【0035】
更に、ECU1は、自車両Vが減速している、すなわち車速Vが継続して減少している場合には、後方撮像画像についての姿勢角推定用画像尤度を高めることが望ましい。自車両Vが減速するということは前方が見渡せない状態になる可能性が高いため、前方よりも後方の方が姿勢角推定に適しているからである。この処理のため、ECU1は、例えば図14及び図15に示すようなテーブルデータを参照してもよい。図14及び図15は、図7(a)のように、前方カメラ2f、右側方カメラ2r、左側方カメラ2l及び後方カメラ2bを搭載した自車両Vについて設定したものである。図14に示すテーブルデータは、走行状態に応じて姿勢角推定用画像尤度を設定するためのデータである。図15に示すテーブルデータは、走行状態に応じて位置推定用画像尤度を設定するためのデータである。図14によれば、自車両Vが減速しており、且つ、直進方向に走行している場合には、前方撮像画像の姿勢角推定用画像尤度を「3」とし、後方撮像画像の姿勢角推定用画像尤度を「7」に設定する。
【0036】
更に、ECU1は、自車両Vの旋回半径ρが減少する場合には、後方撮像画像についての姿勢角推定用画像尤度を高めることが望ましい。また、ECU1は、自車両Vの旋回半径ρが増大する場合には、前方撮像画像についての姿勢角推定用画像尤度を高めることが望ましい。これは、自車両Vが旋回する最初は旋回半径ρが減少し自車両Vの後方が遠くまで見渡すことが可能であり、自車両Vが旋回する最後は旋回半径ρが増大し前方が遠くまで見渡せる可能性が高いことによる。例えば図14において、自車両Vが前進走行している場合であって旋回半径ρが減少している場合には前方撮像画像の姿勢角推定用画像尤度を「3」とし、後方撮像画像の姿勢角推定用画像尤度を「6」に設定する。逆に、旋回半径ρが増大している場合には前方撮像画像の姿勢角推定用画像尤度を「6」とし、後方撮像画像の姿勢角推定用画像尤度を「3」に設定する。ECU1は、前回半径ρの大小を判定するための閾値を設定しておくことが望ましい。この閾値は、自車両Vの旋回半径ρに応じた後方撮像画像及び前方撮像画像の撮像範囲によって設定される。
【0037】
更に、ECU1は、自車両Vの旋回半径がある閾値TH(単位:m)を超えて減少した場合には、旋回内側方向の側方撮像画像についての位置推定用画像尤度を高めることが望ましい。ECU1は、ステップS1にて検出したヨーレートセンサからの出力値の符号に基づいて自車両Vの旋回方向を判定する。例えば閾値THを100[m]とする。自車両Vの旋回内側には道路端等の撮像画像と仮想画像とが照合できる構造物が連続して近い状態にあり、旋回内側の側方撮像画像が位置の推定に適しているからである。例えば図15において、自車両Vが前進走行している場合であって左旋回している場合には左側撮像画像の位置推定用画像尤度を「7」とし、右側撮像画像の位置推定用画像尤度を「2」に設定する。逆に、右旋回している場合には右側撮像画像の位置推定用画像尤度を「6」とし、左側撮像画像の位置推定用画像尤度を「3」に設定する。
【0038】
更に、ECU1は、自車両Vの横方向への移動方向を撮像する右側撮像画像又は左側撮像画像についての位置推定用画像尤度を高めることが望ましい。左右横方向への移動速度がある閾値TH(単位:m/s)を超えて大きくなった場合には、移動している方向の横画像が位置推定に適しているとする。例えば閾値THを0.5[m/s]とする。例えば車線変更時には車線変更方向に撮像画像と仮想画像とが照合できる構造物や道路標示がある可能性が高く、移動側の側方撮像画像が位置の推定に適しているからである。例えば図15において、自車両Vが前進走行している場合であって左方向に車線変更する場合には左側撮像画像の位置推定用画像尤度を「7」とし、右側撮像画像の位置推定用画像尤度を「2」に設定する。逆に、右方向に車線変更している場合には右側撮像画像の位置推定用画像尤度を「7」とし、左側撮像画像の位置推定用画像尤度を「2」に設定する。
【0039】
次のステップS7において、ECU1は、ステップS6にて位置推定用画像尤度及び姿勢角推定用画像尤度が設定された撮像画像のうち、所定値以上の位置推定用画像尤度又は姿勢角推定用画像尤度の撮像画像からエッジ画像を作成する。この所定値は、0より大きい値であってもよく、任意に設定した値であってもよい。これにより、ECU1は、姿勢角推定用エッジ画像Ea及び位置推定用エッジ画像Epを作成できる。
【0040】
例えば図4(a)に示すように前方カメラ2fと左側方カメラ2lを搭載した自車両Vが一定速で前進している場合、図12に示すように、前方撮像画像のエッジ画像が姿勢角推定用エッジ画像Eaとなる。また、図13に示すように、左側方撮像画像が位置推定用エッジ画像Epとなる。他の例では、図7(a)に示すように前方カメラ2f、左側方カメラ2l、右側方カメラ2r、後方カメラ2bを備えた自車両Vが一定速で前進している場合、図14に示すように、前方撮像画像のエッジ画像と後方撮像画像のエッジ画像とがそれぞれ姿勢角推定用エッジ画像Eaとなる。また、自車両Vが一定速で前進している場合、図15に示すように、左側方撮像画像のエッジ画像と右側方撮像画像のエッジ画像とがそれぞれ位置推定用エッジ画像Epとなる。
【0041】
本実施形態におけるエッジとは、画素の輝度が鋭敏に変化している箇所を指す。エッジ検出方法としては、例えばCanny法を用いることができる。これに限らず、エッジ検出手法は、他にも微分エッジ検出など様々な手法を使用してもよい。また、ECU1は、カメラ群2の撮像画像から、エッジの輝度変化の方向やエッジ近辺のカラーなど抽出することが望ましい。これにより、後述するステップS5及びステップS6において、3次元地図データベース3にも記録しておいた、これらエッジ以外の情報も用いて位置推定用尤度、姿勢角推定用尤度を設定して、自車両の位置及び姿勢角を推定してもよい。
【0042】
ステップS1の次のステップS8において、ECU1は、1ループ前のステップS6にて推定された車両位置から、今回のステップS1で算出したオドメトリ分だけ移動させる。ECU1は、移動させた車両位置の近傍で、複数の仮想(予測)位置及び仮想(予測)姿勢角の候補を算出する。ただし、位置姿勢推定アルゴリズムを開始して初めてのループの場合には、ECU1は前回の車両位置情報を持っていない。このため、ECU1は、車両センサ群4に含まれるGPS受信機41からのデータを初期位置情報とする。又は、ECU1は、前回に停車時に最後に算出した車両位置及び姿勢角を記憶しておき、初期位置及び姿勢角情報にしてもよい。
【0043】
ECU1は、車両センサ群4の測定誤差や通信遅れによって生じるオドメトリの誤差や、オドメトリで考慮できない車両の動特性を考慮に入れ、車両の位置や姿勢角の真値が取り得る可能性がある複数の仮想位置及び仮想姿勢角の候補を複数個生成する。この仮想位置及び仮想姿勢角の候補は、位置及び姿勢角の6自由度のパラメータに対してそれぞれ誤差の上下限を設定し、この誤差の上下限の範囲内で乱数表等を用いてランダムに設定する。
【0044】
なお、本実施形態では、仮想位置及び仮想姿勢角の候補を500個作成する。また、位置及び姿勢角の6自由度のパラメータに対する誤差の上下限は、前後方向,横方向,上下方向、ロール,ピッチ,ヨーの順に±0.05[m],±0.05[m],±0.05[m],±0.5[deg] ,±0.5[deg],±0.5[deg]とする。この仮想位置及び仮想姿勢角の候補を作成する数や、位置及び姿勢角の6自由度のパラメータに対する誤差の上下限は、車両の運転状態や路面の状況を検出或いは推定して、適宜変更することが望ましい。例えば、急旋回やスリップなどが発生している場合には、平面方向(前後方向,横方向,ヨー)の誤差が大きくなる可能性が高いので、この3つのパラメータの誤差の上下限を大きくし、且つ仮想位置及び仮想姿勢角の候補の作成数を増やすことが望ましい。
【0045】
次のステップS9において、ECU1は、ステップS8で作成した全ての仮想位置及び仮想姿勢角の候補のそれぞれについて仮想(投影)画像を作成する。このとき、ECU1は、例えば3次元地図データベース3に記憶されたエッジ等の次元位置情報を、仮想位置及び仮想姿勢角から撮像した撮像画像となるよう投影変換して、仮想画像を作成する。この仮想画像は、各仮想位置及び仮想姿勢角の候補が実際の自車両の位置及び姿勢角と合致しているかを評価する評価用画像である。投影変換では、カメラ群2の位置を示す外部パラメータと、カメラ群2の内部パラメータが必要となる。外部パラメータは、車両位置(例えば中心位置)からカメラ群2までの相対位置を予め計測しておくことで、仮想位置及び仮想姿勢角の候補から算出すればよい。また内部パラメータは、予めキャリブレーションをしておけばよい。
【0046】
ECU1は、各仮想位置及び仮想姿勢角の候補について、自車両Vに搭載されたカメラ群2から撮像した撮像画像となるような仮想画像を作成する。例えば図4(a)に示すようにカメラ群2が前方カメラ2f及び左側方カメラ2lを含む場合、各仮想位置及び仮想姿勢角の候補について仮想前方画像及び仮想左側方画像を作成する。また、図7(a)に示すようにカメラ群2が前方カメラ2f、左側方カメラ2l、右側方カメラ2r及び後方カメラ2bを含む場合、各仮想位置及び仮想姿勢角の候補について仮想前方画像、仮想右側方画像、仮想左側方画像及び仮想後方画像を作成する。これにより、ECU1は、姿勢推定用エッジ画像Eaの撮像方向と同じ方向に投影した姿勢推定用仮想画像Aaを作成し、位置推定用エッジ画像Epの撮像方向と同じ方向に投影した位置推定用仮想画像Apを作成する。なお、この仮想画像は、ステップS2及びステップS4で設定されたαf、αsの画角の範囲のみ算出すればよい。
【0047】
なお、ステップS1においてカメラ2により撮像した撮像画像からエッジの輝度変化の方向や、エッジ近辺のカラーなどについても撮像画像から抽出できる場合には、それらについても次元位置情報を3次元地図データベース3に記録しておき、仮想画像を作成するとなお良い。
【0048】
ステップS7及びステップS9の処理の後に、ステップS10を行う。ステップS10において、ECU1は、ステップS8にて設定した全ての仮想位置及び仮想姿勢角の候補それぞれについて、ステップS7にて作成したエッジ画像Ea,Epと、エッジ画像の撮像方向と同じステップS9にて作成した仮想画像Aa,Apとをそれぞれ比較する。ECU1は、姿勢角推定用エッジ画像Eaと姿勢推定用仮想画像Aaとの一致度に応じた値の姿勢角推定用尤度(単位:なし)を設定する。また、ECU1は、位置推定用エッジ画像Epと位置推定用仮想画像Apとの一致度に応じた値の位置推定用尤度(単位:なし)を設定する。これにより、ECU1は、各仮想位置及び仮想姿勢角の候補点について、姿勢角推定用尤度及び位置推定用尤度を設定できる。
【0049】
具体的には、仮想画像内の画素位置に、姿勢角推定用エッジ画像Ea及び位置推定用エッジ画像Epの画素がある場合には、姿勢角推定用尤度及び位置推定用尤度を加算する。逆に、仮想画像内の画素位置に、姿勢角推定用エッジ画像Ea及び位置推定用エッジ画像Epの画素が無い場合には、姿勢角推定用尤度及び位置推定用尤度を加算しない。これにより、各仮想位置及び仮想姿勢角の候補点の撮像方向ごとに、姿勢角推定用尤度及び位置推定用尤度を設定できる。
【0050】
また、姿勢推定用エッジ画像Eaと姿勢推定用仮想画像Aaとを比較して、同じ位置に一致した画素がある場合には、姿勢推定用尤度に1.0を加算し、位置推定用尤度に0.2を加算してもよい。一方、位置推定用エッジ画像Epと位置推定用仮想画像Apとを比較して、同じ位置に一致した画素がある場合には、姿勢推定用尤度に0.2を加算し、位置推定用尤度に1.0を加算してもよい。
【0051】
また、更に比較に用いた画像の、位置推定用画像尤度と姿勢角推定用尤度を、位置推定用尤度と姿勢角推定用尤度にそれぞれ乗じてもよい。
【0052】
例えば図4(a)に示すように前方カメラ2fと左側方カメラ2lを搭載した自車両Vが一定速で前進している場合、前方撮像画像から得た姿勢角推定用エッジ画像Eaと前方に投影した仮想画像との一致度に応じた姿勢角推定用尤度を設定する。また、左側方撮像画像から得た位置推定用エッジ画像Epと左側方に投影した仮想画像との一致度に応じた位置推定用尤度を設定する。
【0053】
他の例では、図7(a)に示すように前方カメラ2f、左側方カメラ2l、右側方カメラ2r、後方カメラ2bを備えた自車両Vが一定速で前進している場合、前方撮像画像から得た姿勢角推定用エッジ画像Eaと前方に投影した仮想画像との一致度に応じた姿勢角推定用尤度を設定する。更に、後方撮像画像から得た姿勢角推定用エッジ画像Eaと後方に投影した仮想画像との一致度に応じた姿勢角推定用尤度を設定する。また、左側方撮像画像から得た位置推定用エッジ画像Epと左側方に投影した仮想画像との一致度に応じた姿勢角推定用尤度を設定する。更に右側方撮像画像から得た位置推定用エッジ画像Epと右側方に投影した仮想画像との一致度に応じた姿勢角推定用尤度を設定する。なお、ECU1は、1つの仮想位置及び仮想姿勢角について2つの姿勢角推定用エッジ画像Eaから得た2つの姿勢角推定用尤度を平均してもよく、姿勢角推定用画像尤度の値に応じて重み付け平均してもよい。同様に、ECU1は、1つの仮想位置及び仮想姿勢角について2つの位置推定用エッジ画像Epから得た2つの位置推定用尤度を平均してもよく、位置推定用画像尤度の値に応じて重み付け平均してもよい。
【0054】
次のステップS11において、ECU1は、ステップS10にて位置推定用尤度及び姿勢角推定用尤度が求められた複数の仮想位置及び仮想姿勢角の候補を用いて、最終的な自車両Vの位置及び姿勢角を算出する。ECU1は、仮想画像の姿勢角推定用尤度に基づいて当該仮想画像の仮想姿勢角に対する実際の自車両Vの姿勢角を推定する(移動物体位置姿勢推定手段)。ECU1は、仮想画像の位置推定用尤度に基づいて当該仮想画像の仮想位置に対する実際の自車両Vの位置を推定する(移動物体位置姿勢推定手段)。
【0055】
このとき、ECU1は、例えば、位置推定用尤度が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Vの実際の位置及び姿勢角として算出してもよい。又は、ECU1は、姿勢角推定用尤度が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Vの実際の位置及び姿勢角として算出してもよい。又は、ECU1は、位置推定用尤度と姿勢角推定用尤度との和が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、自車両Vの実際の位置及び姿勢角として算出してもよい。又は、ECU1は、複数の仮想位置及び仮想姿勢角の候補に対して、各仮想画像の位置推定用尤度に応じた重み付き平均を取って自車両Vの実際の位置及び姿勢角を算出してもよい。又は、ECU1は、複数の仮想位置及び仮想姿勢角の候補に対して、各仮想画像の姿勢角推定用尤度に応じた重み付き平均を取って自車両Vの実際の位置及び姿勢角を算出してもよい。又は、ECU1は、各仮想画像の位置推定用尤度と姿勢角推定用尤度の和に応じた重み付き平均を取って自車両Vの実際の位置及び姿勢角を算出してもよい。
【0056】
ECU1は、以上のようなステップS1乃至ステップS11を繰り返して行うことによって、逐次、自車両Vの位置と姿勢角を算出できる。
【0057】
以上詳細に説明したように、本実施形態として示した移動物体位置姿勢推定装置によれば、走行状態が加速又は一定速の通常状態である場合には、前方撮像画像についての姿勢角推定用画像尤度を高め、側方撮像画像についての位置推定用画像尤度を高める。これにより、移動物体位置姿勢推定装置は、自車両Vの姿勢角を前方撮像画像を用いて推定でき、自車両Vの位置を側方撮像画像を用いて推定できる。したがって、この移動物体位置姿勢推定装置によれば、位置と姿勢角に対して別々の尤度を考え、位置と姿勢角を別々に調整して推定することができる。具体的には、自車両Vから遠い場所でこの2つの映像が一致していた場合には、仮想画像の姿勢角は真値に近いが仮想画像の位置情報の誤差が大きい可能性があるので、姿勢角推定用尤度をより大きく設定できる。逆に、位置推定用尤度はあまり大きく設定しない。一方、自車両Vから近い場所でこの2つの映像が一致していた場合には、位置推定用尤度をより大きく設定し、逆に、姿勢角推定用尤度はあまり大きく設定しない。
【0058】
2つの映像が一致した場所が自車両Vから遠い場合には位置の誤差が大きい場合があり、逆に、2つの映像が一致した位置が自車両Vから近い場合には姿勢角の誤差が大きい場合があるが、この移動物体位置姿勢推定装置によれば、精度良く移動物体の位置及び姿勢角を推定することができる。
【0059】
また、この移動物体位置姿勢推定装置によれば、自車両Vの位置及び姿勢角を求めるために不要な撮像画像については仮想画像と一致する処理を省くことができるため、演算負荷の低減が実現できる。
【0060】
更に、この移動物体位置姿勢推定装置によれば、自車両Vが減速している場合には、後方撮像画像についての姿勢角推定用画像尤度を高めるので、自車両Vの前方に障害物があり広く見渡せる後方撮像画像を用いて自車両Vの姿勢角を推定できる。これにより、この移動物体位置姿勢推定装置によれば、見通しの良い後方撮像画像を用いて自車両Vの姿勢角を推定できるので、姿勢角の推定精度を向上させることができる。
【0061】
更にまた、この移動物体位置姿勢推定装置によれば、移動物体の旋回半径が減少する場合には後方撮像画像についての姿勢角推定用画像尤度を高め、移動物体の旋回半径が増大する場合には前方撮像画像の姿勢角推定用画像尤度を高める。これより、この移動物体位置姿勢推定装置によれば、旋回を開始して旋回半径が減少する時には見通しの良い後方撮像画像を用いて自車両Vの姿勢角を推定でき、旋回の終了する旋回半径の増大する時には見通しの良い前方撮像画像を用いて自車両Vの姿勢角を推定できる。したがって、自車両Vの旋回半径が変化しても姿勢角の推定精度を向上させることができる。
【0062】
更にまた、この移動物体位置姿勢推定装置によれば、自車両Vが旋回する場合に、旋回方向の内側方向を撮像する右側撮像画像又は左側撮像画像についての姿勢角推定用画像尤度を高める。これにより、移動物体位置姿勢推定装置は、旋回内側の近距離に存在することが多い道路端や区画線などが連続して撮像可能な旋回内側方向の撮像画像を用いて姿勢角を推定できる。したがって、自車両Vが旋回中でも姿勢角の推定精度を向上させることができる。
【0063】
更にまた、この移動物体位置姿勢推定装置によれば、自車両Vが横方向に移動する場合に、横方向への移動方向を撮像する右側撮像画像又は左側撮像画像についての位置推定用画像尤度を高める。これにより、移動物体位置姿勢推定装置は、例えば自車両Vが車線変更するときに近距離に存在することが多い道路端や区画線など撮像可能な横移動方向の撮像画像を用いて位置を推定できる。したがって、自車両Vが横方向に移動中でも位置の推定精度を向上させることができる。
【0064】
なお、上述の実施の形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施の形態以外であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計等に応じて種々の変更が可能であることは勿論である。
【0065】
上述した実施形態では車両を例にしたが、少なくとも1台以上のカメラを搭載した移動体であれば、航空機や船舶などにも適用可能である。
【0066】
また、上述した実施形態では車両の6自由度の位置(前後方向,横方向,上下方向)及び姿勢角(ロール,ピッチ,ヨー)を求めているが、これに限らない。例えば、サスペンション等を有さない、工場などで用いられる無人搬送車などのように3自由度での位置(前後方向,横方向)及び姿勢角(ヨー)を推定する場合にも、本実施形態が適用可能である。具体的には、このような車両であれば、上下方向の位置と、ロールおよびピッチといった姿勢角は固定であるので、予めこれらのパラメータを計測しておいたり、3次元地図データベース3を参照して求めるようにすればよい。
【符号の説明】
【0067】
1 ECU
2 カメラ群
2a 全方位カメラ
2b 後方カメラ
2f 前方カメラ
2l 左側方カメラ
2r 右側方カメラ
次元地図データベース
4 車両センサ群
21 撮像範囲
41 GPS受信機
42 アクセルセンサ
43 ステアリングセンサ
44 ブレーキセンサ
45 車速センサ
46 加速度センサ
47 車輪速センサ
48 その他センサ
図1
図2
図3
図4
図5
図6
図7
図8
図9
図10
図11
図12
図13
図14
図15