JP6410231B2 - 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム - Google Patents

Description

本発明は、互いに異なる方向に向けられた車載の複数のカメラにより撮影された画像上の特徴点同士を対応付ける位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラムに関する。

近年、運転支援などの目的で、車両には、１台以上のカメラが取り付けられることがある。このような場合、カメラにより得られた画像を有効に利用するために、異なるカメラにより生成された複数の画像間で、あるいは、異なる位置で同一のカメラにより生成された複数の画像間で、実空間上の同一の点に対応する画像上の特徴点同士を対応付けることが提案されている（例えば、特許文献１及び非特許文献１、２を参照）。

例えば、特許文献１には、路面上にある、マンホールの蓋の中心、白線の端部などの特徴点を、移動体の直進時における移動前後の移動体に取り付けられたカメラにより撮影された画像間で、物体追跡処理を用いて対応付けることが提案されている。

また、非特許文献１には、車両に搭載された前方を写すカメラと左側または右側を写すカメラ間で、連続する二つのフレームについてSURF特徴を抽出し、照合することが記載されている。さらに、非特許文献２には、一方の画像上の特徴点周囲の領域をワーピングにより変形させて、対応付けを行う他方の画像上での対応点を検出することが提案されている。

特開２０１４−１０１０７５号公報

Gim Hee Lee他、「Motion Estimation for Self-Driving Cars With a Generalized Camera」、Computer Vision and Pattern Recognition (CVPR)、2013 IEEE Conference on. IEEE、2013年 Simon Baker他、「Lucas-Kanade 20 Years On: A Unifying Framework: Part 1」、International journal of computer vision 56.3、pp.221-255、2004年

特許文献１に開示された技術では、二つの画像間での対応付けが行われるのは、路面上の点に限られている。しかしながら、用途によっては、路面以外の点に相当する特徴点同士を画像間で対応付けることが要求されることもある。

また、非特許文献１または２に開示された技術では、実空間の同じ位置を撮影した二つの画像のそれぞれについて、その位置を見る視点が異なっているために、画像上でもその位置の見え方が異なっている。そのため、特徴点の対応付けに関して、十分な精度が得られないおそれがあった。

そこで、本発明は、車両に搭載された複数のカメラのそれぞれにより生成された画像間で、実空間上の同じ点に対応する画像上の特徴点同士の対応付けの精度を向上できる位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラムを提供することを目的とする。

請求項１の記載によれば、本発明の一つの形態として、位置合わせ装置が提供される。係る位置合わせ装置は、車両（１０）に対して第１の方向を向くように取り付けられた第１の撮像部（２−２）により第１の時刻において生成された第１の画像から、少なくとも一つの第１の特徴点を抽出する特徴点抽出部（２１）と、第１の画像上の第１の特徴点を含むマッチング範囲に対応する、実空間上の第１の領域を、車両（１０）と車両（１０）の周囲の構造物の位置関係を規定する所定の条件に従って特定し、第１の領域を、車両（１０）に対して第１の方向と異なる第２の方向を向くように取り付けられた第２の撮像部（２−１）により、第１の時刻と異なる第２の時刻において生成された第２の画像上に投影することで、マッチング範囲に対応する第２の画像上の範囲を特定する投影部（２２）と、第２の画像上の所定の探索範囲内で、マッチング範囲に対応する第２の画像上の範囲の相対的な位置を変えながらマッチング範囲と第２の画像間でブロックマッチングを行うことで、マッチング範囲と最も類似する領域を特定し、その最も類似する領域内の所定点を第１の特徴点に対応する第２の特徴点とする対応付け部（２３）とを有する。
本発明に係る位置合わせ装置は、上記の構成を有することにより、車両に搭載された複数のカメラのそれぞれにより生成された画像間で、実空間上の同じ点に対応する画像上の特徴点同士の対応付けの精度を向上できる。

また請求項２の記載によれば、所定の条件は、マッチング範囲に対応する第１の領域が車両（１０）の直進方向と平行でかつ路面に垂直な面上、または路面上に有るとする条件であることが好ましい。
これにより、位置合わせ装置は、マッチング範囲に対応する第２の画像上の範囲を適切に決定できる。

あるいは、請求項３の記載によれば、所定の条件は、マッチング範囲に対応する第１の領域が、第１の時刻における車両（１０）の位置と第２の時刻における車両（１０）の位置間にける、車両（１０）が走行する道路の何れかの位置での道路と平行でかつ路面に垂直な面上にあるとする条件であることが好ましい。
これにより、位置合わせ装置は、第１の時刻と第２の時刻の間で車両が道路に沿って曲線状に移動していても、マッチング範囲に対応する第２の画像上の範囲を適切に決定できる。

さらに、請求項４の記載によれば、投影部（２２）は、車両（１０）に搭載された車両（１０）の移動量または速度を検知するセンサから取得したセンサ情報に基づいて求められた、第１の時刻と第２の時刻間の車両（１０）の移動量に基づいて、第１の撮像部を基準とする座標系における第１の領域の座標を第２の撮像部を基準とする第１の領域の座標に変換し、変換された第１の領域の座標を第２の画像上へ投影することで、マッチング範囲に対応する第２の画像上の範囲を特定することが好ましい。
これにより、位置合わせ装置は、二つの画像の生成時刻間で車両が移動していても、マッチング範囲に対応する第２の画像上の範囲を適切に特定できる。

さらに、請求項５の記載によれば、投影部（２２）は、第２の撮像部（２−１）により互いに異なる時刻で生成された複数の画像のうち、その画像について求められたマッチング範囲に対応する範囲がその画像内に含まれる場合、その画像を第２の画像とすることが好ましい。
これにより、位置合わせ装置は、特徴点同士の対応付けを行う第２の画像を適切に決定できる。

さらに、請求項６の記載によれば、投影部（２２）は、第２の撮像部（２−１）により互いに異なる時刻で生成された複数の画像のうち、その画像が生成された時刻と第１の時刻間の車両（１０）の移動量が所定範囲となる画像を第２の画像とすることが好ましい。
これにより、位置合わせ装置は、特徴点同士の対応付けを行う第２の画像を、簡単な演算で適切に決定できる。

さらに、請求項７の記載によれば、位置合わせ装置は、第１の画像上で路面が写っている領域を検出する路面検出部をさらに有し、投影部（２２）は、第１の特徴点が路面が写っている領域に含まれる場合、マッチング範囲に対応する第１の領域が路面上に有るとして特定することが好ましい。
これにより、位置合わせ装置は、マッチング範囲に対応する実空間上の領域をより適切に設定できる。

また請求項８の記載によれば、本発明の他の形態として、位置合わせ方法が提供される。係る位置合わせ方法は、車両（１０）に対して第１の方向を向くように取り付けられた第１の撮像部（２−２）により第１の時刻において生成された第１の画像から、少なくとも一つの第１の特徴点を抽出するステップと、第１の画像上の第１の特徴点を含むマッチング範囲に対応する、実空間上の第１の領域を、車両（１０）と車両（１０）の周囲の構造物の位置関係を規定する所定の条件に従って特定し、第１の領域を、車両（１０）に対して第１の方向と異なる第２の方向を向くように取り付けられた第２の撮像部（２−１）により、第１の時刻と異なる第２の時刻において生成された第２の画像上に投影することで、マッチング範囲に対応する第２の画像上の範囲を特定するステップと、第２の画像上の所定の探索範囲内で、マッチング範囲に対応する第２の画像上の範囲の相対的な位置を変えながらマッチング範囲と第２の画像間でブロックマッチングを行うことで、マッチング範囲と最も類似する領域を特定し、その最も類似する領域内の所定点を第１の特徴点に対応する第２の特徴点とするステップと、を含む。
本発明に係る位置合わせ方法は、上記のステップを有することにより、車両に搭載された複数のカメラのそれぞれにより生成された画像間で、実空間上の同じ点に対応する画像上の特徴点同士の対応付けの精度を向上できる。

また請求項８の記載によれば、本発明の他の形態として、位置合わせ用コンピュータプログラムが提供される。係る位置合わせ用コンピュータプログラムは、車両（１０）に対して第１の方向を向くように取り付けられた第１の撮像部（２−２）により第１の時刻において生成された第１の画像から、少なくとも一つの第１の特徴点を抽出するステップと、第１の画像上の第１の特徴点を含むマッチング範囲に対応する、実空間上の第１の領域を、車両（１０）と車両（１０）の周囲の構造物の位置関係を規定する所定の条件に従って特定し、第１の領域を、車両（１０）に対して第１の方向と異なる第２の方向を向くように取り付けられた第２の撮像部（２−１）により、第１の時刻と異なる第２の時刻において生成された第２の画像上に投影することで、マッチング範囲に対応する第２の画像上の範囲を特定するステップと、第２の画像上の所定の探索範囲内で、マッチング範囲に対応する第２の画像上の範囲の相対的な位置を変えながらマッチング範囲と第２の画像間でブロックマッチングを行うことで、マッチング範囲と最も類似する領域を特定し、その最も類似する領域内の所定点を第１の特徴点に対応する第２の特徴点とするステップと、をコンピュータに実行させるための命令を含む。
本発明に係る位置合わせ用コンピュータプログラムは、上記の命令を有することにより、車両に搭載された複数のカメラのそれぞれにより生成された画像間で、実空間上の同じ点に対応する画像上の特徴点同士の対応付けの精度を向上できる。

上記各部に付した括弧内の符号は、後述する実施形態に記載の具体的手段との対応関係を示す一例である。

本発明の一つの実施形態に係る位置合わせ装置の概略構成図である。 本発明の一つの実施形態に係る位置合わせ装置の制御部の機能ブロック図である。 二つのカメラのそれぞれについてのカメラ座標系と車両座標系の関係を示す図である。 Manhattan-World仮説に基づく、実空間上の点と車両の位置関係を示す図である。 位置合わせ処理の動作フローチャートである。 変形例における、パッチに対応する実空間上の領域の例を示す図である。

以下、図を参照しつつ、一つの実施形態による、位置合わせ装置について説明する。
この位置合わせ装置は、車両に搭載され、異なる方向に向けられた二つのカメラのそれぞれにより、互いに異なるタイミングで生成された二つの画像間で、実空間上の同一の点に対応する画像上の特徴点同士を対応付ける。その際、この位置合わせ装置は、一方のカメラにより生成された画像上の特徴点の周囲に設定される、ブロックマッチング用の領域であるパッチに対応する実空間上の領域を、Manhattan-World仮説に基づいて設定し、その実空間上の領域を他方のカメラにより生成された画像上に投影して他方の画像上でのブロックマッチングの対象領域を特定することで、特徴点の対応付けの精度を向上するとともに、演算量の削減を図る。

図１は、一つの実施形態による位置合わせ装置の概略構成図である。図１に示すように、位置合わせ装置１は、車両１０に搭載され、カメラ２−１及び２−２、慣性計測装置(IMU)３、車輪速センサ４、及び電子制御ユニット(ECU)５と、コントロールエリアネットワーク（以下、ＣＡＮという）６を介して互いに接続されている。なお、図１では、説明の都合のため、位置合わせ装置１など、車両１０に搭載されている各構成要素及び車両１０の形状、サイズ及び配置は、実際のものとは異なっている。

カメラ２−１、２−２は、それぞれ、撮像部の一例である。本実施形態では、カメラ２−１、２−２は、それぞれ、CCDあるいはC-MOSなど、可視光に感度を有する光電変換素子のアレイで構成された２次元検出器と、その２次元検出器上に撮影対象となる領域の像を結像する結像光学系を有する。そしてカメラ２−１は、結像光学系の光軸が地面に対して略平行となり、かつ車両１０の前方を向くように、例えば、車両１０の車室内に取り付けられる。そしてカメラ２−１は、所定の撮影周期（例えば1/30秒）ごとに車両１０の前方領域を撮影し、その前方領域が写った画像を生成する。一方、カメラ２−２は、結像光学系の光軸が地面に対して略平行となり、かつ車両１０の後方を向くように、車両１０の後端部に取り付けられる。そしてカメラ２−２は、所定の撮影周期ごとに車両１０の後方領域を撮影し、その後方領域が写った画像を生成する。カメラ２−１、２−２により得られた画像は、カラー画像であってもよく、あるいは、グレー画像であってもよい。

カメラ２−１、２−２は、それぞれ、画像を生成する度に、その生成した画像を位置合わせ装置１へ出力する。

位置合わせ装置１は、記憶部１１と、通信部１２と、制御部１３とを有する。
記憶部１１は、例えば、電気的に書き換え可能な不揮発性メモリ及び揮発性メモリなどの半導体メモリを有する。そして記憶部１１は、位置合わせ装置１を制御するための各種プログラム、及び、位置合わせ処理で利用される各種の情報、カメラ２−１またはカメラ２−２により生成された画像、及び制御部１３による一時的な演算結果などを記憶する。

通信部１２は、カメラ２−１、２−２、IMU３、車輪速センサ４、及びECU５などとＣＡＮ６を通じて通信する通信インターフェース及びその制御回路を有する。そして通信部１２は、カメラ２−１、２-２から画像を受け取り、その画像を制御部１３へ渡す。また通信部１２は、位置合わせ処理を実行する周期ごとに、IMU３から、車両１０の速度及び移動量などを表すオドメトリ情報を取得したり、あるいは、車輪速センサ４から車輪速を取得して、制御部１３へ渡す。

制御部１３は、１個もしくは複数個の図示してないプロセッサ及びその周辺回路を有する。そして制御部１３は、位置合わせ装置１全体を制御する。
図２に、制御部１３の機能ブロック図を示す。図２に示すように、制御部１３は、特徴点抽出部２１と、投影部２２と、対応付け部２３とを有する。制御部１３が有するこれらの各部は、例えば、制御部１３が有するプロセッサ上で実行されるコンピュータプログラムによって実現される機能モジュールとして実装される。

本実施形態では、制御部１３は、ある時刻n（ここでは、フレーム間隔を単位とする時刻を表す。秒、分などを単位とする時刻については、実時刻と呼ぶ）において車両１０の後方を撮影するカメラ２−２により生成された画像上の特徴点を、その時刻よりもkフレーム前の時刻(n-k)において車両１０の前方を撮影するカメラ２−１により生成された画像上の特徴点と対応付ける。これは、車両１０が前進している場合、車両１０の後方を撮影するカメラ２−２により時刻nで撮影された範囲は、それより前の時刻(n-k)にて車両１０の前方を撮影するカメラ２−１により撮影された範囲と重なる部分があると想定されるためである。
なお、以下では、説明の便宜上、カメラ２-１により生成された画像を前方画像と呼び、カメラ２−２により生成された画像を後方画像と呼ぶ。

特徴点抽出部２１は、カメラ２−２から制御部１３が後方画像を受け取る度に、その後方画像から、カメラ２−２の撮影領域内にある、路面、あるいは構造物などの特徴点を抽出する。そのために、特徴点抽出部２１は、例えば、コーナー検出アルゴリズムなどを用いて後方画像上のエッジのコーナーを特徴点として検出する。特徴点抽出部２１は、コーナー検出アルゴリズムとして、例えば、J.Shi他、"Good Features to Track"、 IEEE CVPR、 1994年、pp.593-600に開示されたアルゴリズムを利用できる。あるいは、特徴点抽出部２１は、特徴点として、画像からSIFT（Scale Invariant Feature Transform）特徴点を抽出してもよい。SIFT特徴点及びその抽出手法の詳細については、例えば、David G.Lowe、 "Distinctive Image Features from Scale-Invariant Keypoints"、 Journal of Computer Vision、 2004年、vol.60、 No.2、 pp.91-110に開示されている。
特徴点抽出部２１は、後方画像ごとに、その後方画像の取得時刻とその後方画像から抽出した特徴点の後方画像上の座標を投影部２２へ渡す。

同様に、特徴点抽出部２１は、カメラ２−１から制御部１３が前方画像を受け取る度に、その前方画像から特徴点を抽出する。そして特徴点抽出部２１は、抽出した特徴点の座標を、前方画像の取得時刻とともに記憶部１１に記憶する。

投影部２２は、後方画像上の特徴点ごとに、その特徴点を中心とするブロックマッチング用のパッチを設定し、実空間上でそのパッチに相当する領域を特定する。そして投影部２２は、実空間上でのパッチの対応領域を、後方画像よりもkフレーム前に取得された前方画像上に投影する。
この投影処理には、画像上の座標と、実空間上での座標間の変換が必要となる。そこで先ず、その変換に利用される座標系について説明する。

図３は、二つのカメラのそれぞれについてのカメラ座標系と車両座標系の関係を示す図である。カメラ２-１を基準とする実空間の座標系であるカメラ座標系（以下、便宜上、前方カメラ座標系と呼ぶ）C1は、カメラ２−１の像側焦点を原点とし、車両１０の直進方向をz軸、路面と平行かつz軸と直交する方向をx軸、路面に垂直な方向をy軸とする座標系である。そしてz軸について、車両１０の前方へ向かう方向を正とし、x軸の正方向は右手座標系により規定される。また、カメラ２-２を基準とする実空間の座標系であるカメラ座標系（以下、便宜上、後方カメラ座標系と呼ぶ）C2は、カメラ２−２の像側焦点を原点とし、車両１０の直進方向をz軸、路面と平行かつz軸と直交する方向をx軸、路面に垂直な方向をy軸とする座標系である。そしてz軸について、車両１０の後方へ向かう方向を正とし、x軸の正方向は右手座標系により規定される。

また、車両１０を基準とする実空間の座標系である車両座標系Vは、車両１０の任意の点、例えば、前側の車輪間の中点、後ろ側の車輪間の中点、あるいは前側の車輪間の中点と後ろ側の車輪間の中点との中点を原点とし、車両１０の直進方向をz軸、路面と平行かつz軸と直交する方向をx軸、路面に垂直な方向をy軸とする座標系である。そしてz軸について、車両１０の前方へ向かう方向を正とし、x軸の正方向は右手座標系により規定される。また、これらの座標系において、xz平面における、z軸とのなす角はθで表される。

本実施形態では、ブロックマッチングを行うマッチング範囲であるパッチはu_pは、例えば、次式のように定義される。

ここで、(u,v)は、後方画像上での特徴点の座標を表し、α、βは、それぞれ、後方画像上でのパッチu_pの水平方向及び垂直方向の範囲を表す。例えば、α=[-20,20](pixel)、β=[-20,20](pixel)である。

また、投影部２２は、後方画像上の特徴点に基づいて設定されるパッチu_pから、車両１０と車両１０の周囲の構造物の位置関係を規定する所定の条件に従って実空間上での対応領域を特定する。そして投影部２２は、その対応領域について、後方カメラ座標系→時刻nにおける車両座標系→実空間の絶対座標である世界座標系→時刻(n-k)における車両座標系→前方カメラ座標系→前方画像上の順に投影する。
そこで先ず、パッチu_pに対応する実空間上での領域についてのカメラ座標系での座標の導出について説明する。

後方画像あるいは前方画像上の座標（２次元座標）をカメラ座標系の座標（３次元座標）に変換する際、本実施形態では、ピンホールカメラモデルが適用される。この場合、後方画像上でのパッチu_p内の各画素の座標は、次式に従って正規化座標系上の座標U_nに変換される。

ここで、(c_u,c_v)は後方画像の中心の座標、すなわち、カメラ２−２の光軸上の位置に相当する後方画像上の位置の座標を表す。またf_u、f_vは、それぞれ、後方画像上の画素の水平方向及び垂直方向のサイズを考慮した、水平方向及び垂直方向に相当するカメラ２−２の焦点距離を表す。そして(u_n、v_n)は、特徴点(u,v)に対応する正規化座標であり、U_n=(u_n+α_n,v_n+β_n)は、正規化座標系でのパッチu_p内の各画素の座標である。そして(x,y,z)は、それぞれ、特徴点(u,v)に対応するカメラ座標系上の点の座標である。

本実施形態では、車両１０と車両１０の周囲の構造物の位置関係を規定する所定の条件として、Manhattan-World仮説を導入する。そしてManhattan-World仮説に基づいて、各特徴点についてのパッチに対応する実空間上の領域を特定する。Manhattan-World仮説では、特徴点に対応する実空間上の点は、以下の３種類の何れかに位置する。
（１）車両１０の直進方向と平行な建物の側壁上の点
（２）車両１０の直進方向と直交する建物の側壁上の点
（３）路面上の点
また、特徴点に対応する実空間上の点の周囲は平面になっていると仮定される。

図４は、本実施形態による、Manhattan-World仮説に基づく、実空間上の点と車両の位置関係を示す図である。本実施形態では、カメラ２−１は車両１０の前方を向いており、一方、カメラ２−２は、車両１０の後方を向いているので、上記の（２）に相当する点については、両方のカメラから見えることはなく、一方の画像上の特徴点が他方の画像上の特徴点と対応付けられることはない。したがって、（１）に相当する、車両１０の直進方向と平行な側壁上の点４０１に対応する特徴点、あるいは、（３）に相当する、路面上の点４０２に対応する特徴点が、前方画像と後方画像の間で対応付けられる。

点４０１にように、特徴点に対応する実空間上の点が車両１０の直進方向と平行な建物４００の側壁にある場合、その特徴点のパッチに対応する実空間上の領域に含まれる各点について、カメラ座標系ではx=dとなる。なお、dは、車両１０と特徴点に対応する実空間上での点までの距離である。したがって、上記の（１）の条件が仮定される場合、パッチu_pに対応する実空間上の領域内の各点についてのカメラ座標系での座標P_pは次式で表される。

一方、点４０２にように、特徴点に対応する実空間上の点が路面上にある場合、その特徴点のパッチに対応する実空間上の領域に含まれる各点について、カメラ座標系ではy=hとなる。なお、hは路面からカメラ２−２までの高さであり、予め記憶部１１に記憶される。したがって、上記の（３）の条件が仮定される場合、パッチu_pに対応する実空間上の領域内の各点についてのカメラ座標系での座標P_pは次式で表される。

投影部２２は、（２）式と（３）式、または（２）式と（４）式にしたがって、後方画像上の各特徴点についてのパッチu_pに対応する実空間上の領域内の各点のカメラ座標系上の座標P_pを求める。なお、dについては、任意の所定区間、例えば、[1,20](m)の区間で、1m単位で変化させる。すなわち、各特徴点について、dの値ごとに（２）式と（３）式に基づいて求められる20個の領域の座標P_pと、（２）式と（４）式に基づいて求められる1個の領域の座標P_pが求められる。

なお、投影部２２は、バンドル調整によりdの区間を上記の所定区間よりも狭く設定してもよい。この場合には、投影部２２は、例えば、特徴点に対応する、実空間上の点を、カメラ２−２により異なる時間での撮影により得られた二つの後方画像間で特徴点の対応付けを行うことで特定する。そして投影部２２は、その二つの後方画像の生成時に対応する二つの車両の位置を利用した三角測量によって車両１０からその特徴点に対応する実空間上の点までの距離を測定し、その測定された距離を含むようにdの区間を設定すればよい。

投影部２２は、後方画像上の各特徴点のパッチについて、そのパッチに対応する実空間上の領域のそれぞれを前方画像上に投影するwarping関数は、次式で表される。

ここで、u_n'は、座標P_pに対応する、前方画像上の領域の座標である。また関数π_vc1()は、車両座標系からカメラ２-１のカメラ座標系への変換関数であり、関数π_vc2()は、車両座標系からカメラ２-２のカメラ座標系への変換関数である。なお、これらの逆関数は、それぞれ、対応するカメラ座標系から車両座標系への変換関数となる。なお、車両座標系とカメラ座標系間の変換は、それぞれの座標系の原点の差に相当する並進行列と、車両１０の直進方向とカメラの光軸間の傾きに相当する回転行列により表される。

また、関数π_ci()は、カメラ座標系から画像上の座標への変換関数であり、（２）式のカメラ座標(x,y,z)から画像上の座標(u_n,v_n)への変換に相当する。

また、関数πⁿ _wv()は、時刻nにおける世界座標系から車両座標系への変換関数である。一方、その逆関数πⁿ _wv ^-1()は、時刻nにおける車両座標系から世界座標系への変換関数である。

以下に、関数πⁿ _wv()の詳細について説明する。X_n=^t(x_n,z_n,θ_n)を、世界座標系での時刻nにおける車両１０の位置及び向きを表す位置ベクトルとする。この場合、X_nは、時刻(n-k)における車両１０の位置X_n-k=^t(x_n-k,z_n-k,θ_n-k)との間に、次式で表される関係が成立する。

ここで、u_nは、オドメトリ情報であり、v_n,right、v_n,leftは、それぞれ、時刻nにおける車両１０の右側後輪の車輪速、及び左側後輪の車輪速を表す。そしてω_n、v_nは、それぞれ、時刻ｎにおける車両１０の角速度及び速度を表す。またd_aは、車両１０の左右の後輪間の間隔である。さらに、Δtは、時刻(n-k)と時刻n間の実時刻の差であり、Δt=k×fで表される。なお、fは、カメラ２−１、２−２のフレーム周期（撮影間隔）である。またε_xは、移動量の誤差を確率分布で表す誤差モデルである。本実施形態では、誤差モデルε_xは、ガウス分布で表されるものとし、その共分散Σ_xは、x軸方向、z軸方向及びθに関して、以下のように定義される。

ここで、σ_x、σ_z及びσ_θは、例えば、オドメトリ情報あるいは車輪速の誤差に基づいて設定され、例えば、σ_x=σ_z=0.1[m]であり、σ_θ=2π/360[rad]である。

時刻nにおける、世界座標系から車両座標系への変換を表す回転行列R^t及び並進行列T^tは、時刻nにおける車両１０の位置ベクトルX_nを用いて次式で表される。

したがって、関数πⁿ _wv()は、次式で表される。

ここで、P_wは、世界座標系での着目点の座標を表し、P_vは、P_wに対応する点の車両座標系での座標を表す。なお、（５）式における、π^n-k _wv()では、x_n=x_n-k、z_n=z_n-k、θ_n=θ_n-kとすればよい。

上記のように、パッチu_pに対応する、実空間上の領域を前方画像上へ投影することで、パッチu_p内の各画素に対応する前方画像上の位置、すなわち、パッチu_pを前方画像上に投影した領域が求められる。
投影部２２は、後方画像上の各特徴点についてのパッチu_p内の各画素に対応する、kフレーム前の前方画像上での画素の座標を対応付け部２３へ出力する。

対応付け部２３は、後方画像上の各特徴点についてのパッチu_pに対応する、kフレーム前の前方画像上での領域と、前方画像との間でブロックマッチングを行って互いに類似する領域を特定することで、後方画像上の各特徴点に前方画像上の特徴点を対応付ける。

本実施形態では、対応付け部２３は、後方画像上の各特徴点について、対応する前方画像上の特徴点の探索を行う探索範囲を誤差モデルに基づく誤差楕円として設定する。その際、誤差楕円は、次式により算出される共分散行列Σ_Fにより定められる。

なお、wは、（５）式の右辺を表す。

対応付け部２３は、共分散行列Σ_Fの固有値λ₁、λ₂と、固有ベクトルv₁、v₂を算出する。ただし、大きい方の固有値をλ₁とする。このとき、誤差楕円の長径a及び短径bは、次式で表される。

ここで、χ²は、χ二乗分布であり、誤差楕円の信用区間を表す。また、前方画像上の水平方向に対して誤差楕円の長径がなす角θ_εは、次式で表される。

対応付け部２３は、後方画像上の各特徴点について、kフレーム前の前方画像上の探索範囲である誤差楕円内にある特徴点を、後方画像上の特徴点に対応付ける候補として検出する。なお、対応付け部２３は、次式を満たす前方画像上の特徴点を、探索範囲に含まれると判定する。

ここで、(u'_c,v'_c)は、後方画像上の着目する特徴点に対応する前方画像上での画素の座標を表す。そして(u',v')は、前方画像上で検出された特徴点の座標である。

対応付け部２３は、後方画像上の各特徴点について、kフレーム前の前方画像上の探索範囲内にある特徴点のそれぞれについて、ブロックマッチングを行って正規化相互相関値を算出する。その際、対応付け部２３は、パッチが投影された前方画像上の領域の中心、すなわち、後方画像上の特徴点が前方画像上に投影された位置と、ブロックマッチングの対象となる前方画像上の特徴点間の位置の差を打ち消すように、前方画像上に投影されたパッチ内の各画素の位置を平行移動させることで、後方画像上のパッチ内の各画素に対応する前方画像上の画素を特定すればよい。そして対応付け部２３は、後方画像上のパッチ内の各画素の輝度値と、対応する前方画像上の画素の輝度値に基づいて正規化相互相関値を算出できる。対応付け部２３は、その正規化相互相関値が最大となる前方画像上の特徴点を、後方画像上の着目する特徴点に対応付ける。なお、対応付け部２３は、正規化相互相関値の最大値が所定の閾値（例えば、0.6〜0.7）以上である場合に限り、その二つの特徴点を対応付けてもよい。

なお、対応付け部２３は、演算量を軽減するために、探索範囲を、後方画像上の特徴点に対応する前方画像上の座標を中心とする、予め設定された所定の範囲に設定してもよい。この場合、所定の範囲は、例えば、水平方向について、パッチの水平方向サイズの2〜3倍とし、垂直方向について、パッチの垂直方向サイズの2〜3倍とすることができる。

また、変形例によれば、対応付け部２３は、後方画像上の各特徴点について、対応する探索範囲内の各画素をそれぞれパッチの中心に対応させるよう、パッチを平行移動させてそれぞれ正規化相互相関値を算出し、その正規化相互相関値が最大となる画素を、後方画像上の着目する特徴点に対応する前方画像上の特徴点としてもよい。この場合には、特徴点抽出部２１は、前方画像から特徴点を抽出しなくてもよい。

対応付け部２３は、後方画像上の特徴点と対応付けられた前方画像上の特徴点の座標の組、及び、対応する後方画像の取得時刻と前方画像の取得時刻の組とを関連付けて、記憶部１１に記憶する。

図５に、制御部１３により制御される、位置合わせ処理の動作フローチャートを示す。なお、制御部１３は、後方画像ごとに、この動作フローチャートにしたがって位置合わせ処理を実行する。

特徴点抽出部２１は、後方画像及び前方画像のそれぞれから特徴点を抽出する（ステップＳ１０１）。そして特徴点抽出部２１は、前方画像から抽出した各特徴点の座標を記憶部１１に記憶する。また特徴点抽出部２１は、後方画像から抽出した各特徴点の座標を投影部２２へ渡す。

投影部２２は、後方画像から抽出した各特徴点について、その特徴点を中心とするパッチを設定し、そのパッチに対応する実空間上の１以上の領域をManhattan-World仮説に従って設定する（ステップＳ１０２）。そして投影部２２は、後方画像から抽出した各特徴点について、パッチに対応する実空間上の各領域を、後方画像の取得時よりもkフレーム前に取得された前方画像上へ投影する（ステップＳ１０３）。

対応付け部２３は、後方画像から抽出した各特徴点について、パッチに対応する実空間上の領域の中心の前方画像上の投影位置を中心として探索範囲を設定する（ステップＳ１０４）。そして対応付け部２３は、探索範囲内にある前方画像上の特徴点の周囲の領域と、前方画像上でのパッチに対応する実空間上の領域の投影範囲との間でのブロックマッチングで最も一致する特徴点を、後方画像上の特徴点と対応付ける（ステップＳ１０５）。そして制御部１３は、位置合わせ処理を終了する。

以上説明してきたように、位置合わせ装置は、異なるカメラにより、異なるタイミングで生成された二つの画像間で、実空間上の同一の点に対応する特徴点同士を対応付ける際、Manhattan-World仮説に基づいて、一方の画像上の特徴点の周囲のパッチに相当する実空間上の領域を特定することで、そのパッチを他方の画像へ投影する際の精度を向上する。これにより、この位置合わせ装置は、特徴点の対応付けの精度を向上できる。またこの位置合わせ装置は、パッチに相当する実空間上の領域の数を減らせるので、演算量を削減できる。

以上、本発明の好適な実施形態について説明してきたが、本発明はこれらの実施形態に限定されるものではない。例えば、制御部１３は、一つの後方画像上の特徴点との対応付けを行う前方画像を複数選択し、その複数の前方画像に対して投影部２２及び対応付け部２３の処理を行って、特徴点同士の対応付けを行ってもよい。その際、投影部２２は、異なる時刻に生成された複数の前方画像のうち、後方画像上の着目する特徴点について設定されたパッチの投影範囲が含まれる前方画像のみを、その着目する特徴点についての対応付けを行う前方画像として選択してもよい。あるいは、投影部２２は、異なる時刻に生成された複数の前方画像のうち、後方画像の取得時とその前方画像の取得時との間の車両１０の移動量が、二つのカメラの撮影範囲が重なると想定される所定の移動距離範囲内に含まれる前方画像のみを、特徴点の対応付けを行う前方画像として選択してもよい。なお、車両１０の移動量は、後方画像の取得時と前方画像の取得時間のオドメトリ情報または車輪速に基づいて算出される。さらに、制御部１３は、オドメトリ情報などに基づいて、後方画像の取得時と、対応付けを行う前方画像の取得時との間で、車両１０が直進していると判断される場合にのみ、後方画像上の特徴点と前方画像上の特徴点の対応付けを行ってもよい。

他の変形例によれば、制御部１３は、後方画像上で路面を検出する路面検出部をさらに有していてもよい。この場合、投影部２２は、後方画像上で抽出された特徴点のうち、検出された路面の範囲内にある特徴点については、（２）式と（４）式にしたがって、パッチに対応する実空間上の領域を設定すればよい。一方、投影部２２は、後方画像上で抽出された特徴点のうち、検出された路面の範囲外にある特徴点については、（２）式と（３）式にしたがって、パッチに対応する実空間上の領域を設定すればよい。

なお、路面検出部は、後方画像から路面を検出するために、画像上で路面が写っている領域を検出する様々な手法を利用できる。例えば、路面検出部は、自車両の両側の白線などの車線を表す線を、例えば、テンプレートマッチングなどにより検出して、その両側の車線を表す線で挟まれた領域を路面とする手法を利用できる。あるいは、路面検出部は、撮影範囲が重なる二つのカメラ（この場合には、カメラ２−２の他に、車両１０の後方を撮影する別のカメラが必要とされる）によりそれぞれ生成された画像、あるいは、一つのカメラで異なる時間に撮影された二つの画像を利用した３次元計測により、路面を検出する手法（例えば、奥富他、「ステレオ動画像を用いた視覚誘導のための平坦部の連続推定」、情報処理学会論文誌、Vol.43、No.4、pp.1061-1069、2002年を参照）を利用してもよい。この場合には、位置合わせ装置は、パッチに対応する実空間上の領域をさらに限定できるので、特徴点間の対応付けの精度をより向上できる。

さらに他の変形例によれば、対応付け部２３は、パッチと前方画像上の対応領域間のブロックマッチングについて、正規化相互相関値を算出する代わりに、対応画素間の輝度値の差の絶対値の総和などを評価値として算出してもよい。この場合には、対応付け部２３は、評価値が最小となる前方画像上の特徴点を、後方画像上の特徴点と対応付けてもよい。

さらにまた、他の変形例によれば、投影部２２は、後方画像上の各特徴点について設定されるパッチに対応する実空間上の領域が、後方画像の生成時における車両１０の位置と前方画像の生成時における車両１０の位置との間にける、車両１０が走行する道路の何れかの位置での道路と平行でかつ路面に垂直な面上にあるとしてもよい。
図６は、この変形例における、パッチに対応する実空間上の領域の例を示す図である。この場合、後方画像上の特徴点の座標により、その特徴点に対応する実空間上の点へ向かう方向とその後方画像生成時におけるカメラ２−２の光軸とのなす角が分かるので、その後方画像生成時における、車両１０からその特徴点に対応する実空間上の点へ向かう方向が分かる。そこで投影部２２は、例えば、記憶部１１に記憶された、オドメトリ情報から求められる車両１０の軌跡情報を参照して、後方画像生成時における車両１０の位置からその特徴点に対応する実空間上の点へ向かう方向の直線６０１から、車両１０が走行してきた軌跡６０２へ下した垂線６０３の長さが、車両１０と特徴点に対応する実空間上での点までの距離dの探索範囲（例えば、上記の実施形態と同様、1m〜20mとすることができる）に含まれる、線６０１上の位置を特定する。そして投影部２２は、特徴点に対応する実空間上の点６０４は、その特定された位置から車両１０の軌跡６０２に下した垂線６０３の足におけるその軌跡６０２の接線方向と平行でかつ路面に垂直な面６０５上にあると仮定する。なお、車両１０は、道路と平行な方向に走行していると仮定できるので、その特定された位置から車両１０の軌跡に下した垂線の足におけるその軌跡の接線方向は、道路と平行であるみなせる。この場合、その特徴点について設定されるパッチに対応する実空間上の領域は、後方画像生成時の道路の延伸方向とその特定された位置での道路の延伸方向間の角度だけ、車両１０の直進方向から傾き、かつ、路面に垂直な面内にあると仮定することで、そのパッチ内の各画素に対応する実空間上の領域内の各点の座標を求めればよい。

さらにまた、他の変形例によれば、位置合わせ装置は、前方画像から抽出された各特徴点についてのパッチに相当する実空間上の領域をManhattan-World仮説に基づいて設定することで、上記と同様の処理を行って、後方画像上の特徴点との対応付けを行ってもよい。あるいは、特徴点の対応付けを行う画像を生成する二つのカメラは、車両の前方と後方を向いているものに限られず、互いに異なる方向を向き、かつ、車両の走行に伴って一方のカメラの撮影範囲に含まれていた領域が他方のカメラの撮影範囲に含まれるように取り付けられていればよい。

例えば、一方のカメラが車両１０の前方を向くように車両１０に取り付けられ、他方のカメラが車両１０の側方を向くように取り付けられているとする。この場合には、車両１０の進行方向に対して直交し、かつ、路面に対して垂直な面も、その二つのカメラの撮影範囲に含まれることになる。そこで、このような場合には、上記の（２）で示された、車両１０の直進方向と直交する建物の側壁上の点（例えば、図４に示される点４０３）に対応する特徴点が、各カメラにより得られるそれぞれの画像において検出可能となる。

そこで、投影部２２は、（２）式と（３）式、または（２）式と（４）式にしたがって、一方のカメラにより得られる画像上の各特徴点についてのパッチu_pに対応する実空間上の領域内の各点のカメラ座標系上の座標P_pを求めるだけでなく、各特徴点が、上記の（２）に相当する点である場合のパッチu_pに対応する実空間上の領域内の各点のカメラ座標系上の座標P_pも求める。

特徴点に対応する実空間上の点が車両１０の直進方向と直交する建物の側壁にある場合、その特徴点のパッチに対応する実空間上の領域に含まれる各点について、カメラ座標系では、車両１０の進行方向におけるカメラからの距離d'は等しいので、z=d'となる。したがって、上記の（１）の条件が仮定される場合、パッチu_pに対応する実空間上の領域内の各点についてのカメラ座標系での座標P_pは次式で表される。

そこで、投影部２２は、（２）式と（１４）式にしたがって、特徴点に対応する実空間上の点が車両１０の直進方向と直交する建物の側壁にある場合における、一方のカメラにより得られる画像上の各特徴点についてのパッチu_pに対応する実空間上の領域内の各点のカメラ座標系上の座標P_pを求めればよい。なお、d'については、任意の所定区間、例えば、[1,20](m)の区間で、1m単位で変化させればよい。

以上の実施形態または変形例によって異なる画像間で対応付けられた特徴点の組は、例えば、車両１０の自己位置推定及び環境地図の作成(Simultaneous Localization and Mapping,SLAM)などで利用可能である。この場合、その特徴点の組に対応する二つの画像が生成されたときのカメラの位置間の距離、すなわち、基線長が長くなるので、その特徴点の組に対応する実空間上の点を特定する際の精度が向上する。
以上のように、当業者は、本発明の範囲内で、実施される形態に合わせて様々な変更を行うことができる。

１ 位置合わせ装置
２−１、２−２ カメラ
３ ＩＭＵ
４ 車輪速センサ
５ ＥＣＵ
６ ＣＡＮ
１１ 記憶部
１２ 通信部
１３ 制御部
２１ 特徴点抽出部
２２ 投影部
２３ 対応付け部

Claims (9)

1. 車両（１０）に対して第１の方向を向くように取り付けられた第１の撮像部（２−２）により第１の時刻において生成された第１の画像から、少なくとも一つの第１の特徴点を抽出する特徴点抽出部（２１）と、
   前記第１の画像上の前記第１の特徴点を含むマッチング範囲に対応する、実空間上の第１の領域を、前記車両（１０）と前記車両（１０）の周囲の構造物の位置関係を規定する所定の条件に従って特定し、前記第１の領域を、前記車両（１０）に対して前記第１の方向と異なる第２の方向を向くように取り付けられた第２の撮像部（２−１）により、前記第１の時刻と異なる第２の時刻において生成された第２の画像上に投影することで、前記マッチング範囲に対応する前記第２の画像上の範囲を特定する投影部（２２）と、
   前記第２の画像上の所定の探索範囲内で、前記マッチング範囲に対応する前記第２の画像上の範囲の相対的な位置を変えながら前記マッチング範囲と前記第２の画像間でブロックマッチングを行うことで、前記マッチング範囲と最も類似する領域を特定し、該最も類似する領域内の所定点を前記第１の特徴点に対応する第２の特徴点とする対応付け部（２３）と、
   を有する位置合わせ装置。
2. 前記所定の条件は、前記マッチング範囲に対応する前記第１の領域が前記車両（１０）の直進方向と平行でかつ路面に垂直な面上、または路面上に有るとする条件である、請求項１に記載の位置合わせ装置。
3. 前記所定の条件は、前記マッチング範囲に対応する前記第１の領域が、前記第１の時刻における前記車両（１０）の位置と前記第２の時刻における前記車両（１０）の位置間にける、前記車両（１０）が走行する道路の何れかの位置での前記道路と平行でかつ路面に垂直な面上にあるとする条件である、請求項１に記載の位置合わせ装置。
4. 前記投影部（２２）は、前記車両（１０）に搭載された前記車両（１０）の移動量または速度を検知するセンサから取得したセンサ情報に基づいて求められた、前記第１の時刻と前記第２の時刻間の前記車両（１０）の移動量に基づいて、前記第１の撮像部を基準とする座標系における前記第１の領域の座標を前記第２の撮像部を基準とする前記第１の領域の座標に変換し、当該変換された前記第１の領域の座標を前記第２の画像上へ投影することで、前記マッチング範囲に対応する前記第２の画像上の範囲を特定する、請求項１〜３の何れか一項に記載の位置合わせ装置。
5. 前記投影部（２２）は、前記第２の撮像部（２−１）により互いに異なる時刻で生成された複数の画像のうち、当該画像について求められた前記マッチング範囲に対応する範囲が当該画像内に含まれる場合、当該画像を前記第２の画像とする、請求項１〜４の何れか一項に記載の位置合わせ装置。
6. 前記投影部（２２）は、前記第２の撮像部（２−１）により互いに異なる時刻で生成された複数の画像のうち、当該画像が生成された時刻と前記第１の時刻間の前記車両（１０）の移動量が所定範囲となる画像を前記第２の画像とする、請求項１〜４の何れか一項に記載の位置合わせ装置。
7. 前記第１の画像上で路面が写っている領域を検出する路面検出部をさらに有し、
   前記投影部（２２）は、前記第１の特徴点が前記路面が写っている領域に含まれる場合、前記マッチング範囲に対応する前記第１の領域が路面上に有るとして特定する、請求項１〜６の何れか一項に記載の位置合わせ装置。
8. 車両（１０）に対して第１の方向を向くように取り付けられた第１の撮像部（２−２）により第１の時刻において生成された第１の画像から、少なくとも一つの第１の特徴点を抽出するステップと、
   前記第１の画像上の前記第１の特徴点を含むマッチング範囲に対応する、実空間上の第１の領域を、前記車両（１０）と前記車両（１０）の周囲の構造物の位置関係を規定する所定の条件に従って特定し、前記第１の領域を、前記車両（１０）に対して前記第１の方向と異なる第２の方向を向くように取り付けられた第２の撮像部（２−１）により、前記第１の時刻と異なる第２の時刻において生成された第２の画像上に投影することで、前記マッチング範囲に対応する前記第２の画像上の範囲を特定するステップと、
   前記第２の画像上の所定の探索範囲内で、前記マッチング範囲に対応する前記第２の画像上の範囲の相対的な位置を変えながら前記マッチング範囲と前記第２の画像間でブロックマッチングを行うことで、前記マッチング範囲と最も類似する領域を特定し、該最も類似する領域内の所定点を前記第１の特徴点に対応する第２の特徴点とするステップと、
   を含む位置合わせ方法。
9. 車両（１０）に対して第１の方向を向くように取り付けられた第１の撮像部（２−２）により第１の時刻において生成された第１の画像から、少なくとも一つの第１の特徴点を抽出するステップと、
   前記第１の画像上の前記第１の特徴点を含むマッチング範囲に対応する、実空間上の第１の領域を、前記車両（１０）と前記車両（１０）の周囲の構造物の位置関係を規定する所定の条件に従って特定し、前記第１の領域を、前記車両（１０）に対して前記第１の方向と異なる第２の方向を向くように取り付けられた第２の撮像部（２−１）により、前記第１の時刻と異なる第２の時刻において生成された第２の画像上に投影することで、前記マッチング範囲に対応する前記第２の画像上の範囲を特定するステップと、
   前記第２の画像上の所定の探索範囲内で、前記マッチング範囲に対応する前記第２の画像上の範囲の相対的な位置を変えながら前記マッチング範囲と前記第２の画像間でブロックマッチングを行うことで、前記マッチング範囲と最も類似する領域を特定し、該最も類似する領域内の所定点を前記第１の特徴点に対応する第２の特徴点とするステップと、
   をコンピュータに実行させるための位置合わせ用コンピュータプログラム。

Images