距離測定装置、距離測定方法、およびプログラム

本発明は、距離測定装置、距離測定方法、およびプログラムに関する。

車両などの移動体に撮影装置を備え、走行しながら異なる時刻に撮影した映像を用いて、周囲物の三次元位置を計測する技術が知られている。三次元位置を計測する装置の一例においては、撮影時刻の異なる2枚の画像中で共通に表示されていると判断される特徴点が多数抽出され、それらが互いに対応付けられて、両画像間の対応点が推定される。また、車両の仮の自車運動量が演算され、仮の基礎行列が演算推定され、仮の基礎行列から、多数抽出された各対応点について、エピポール誤差が演算される。これにより、多数抽出された対応点から、大きな誤差値を有する対応点が誤対応点として除去される。さらに、誤対応点が除去された多数抽出された対応点に基づいて、基礎行列が推定され、当該推定された基礎行列から、車両の自車運動量が演算推定される。そして、推定された車両の自車運動量及び誤対応点が除去された対応点を用いて、特徴点の三次元座標が推定される(例えば、特許文献1参照)。

特開2007−256029号公報

測定対象が静止物の場合、上記のような従来の三次元位置を計測する装置のように、エピポーラ拘束が成立することを用いてエピポール誤差が大きい点を除去することで、二時刻間の特徴点照合を行うことができる。しかし、測距対象が移動物の場合、エピポーラ拘束が成立しない場合があり、この場合、異なる時刻に撮影された画像における特徴点の画像位置は、エピポーラ線上に存在しない。その結果、特徴点照合に失敗し三次元位置は出力されない。

しかし、移動物の移動方向によっては、移動後に見える特徴点画像位置がエピポーラ線上に存在して特徴点照合が取れてしまい、しかも、実際の距離と異なる位置が算出されてしまう特殊な場合があり、誤検出の原因となるという問題がある。

ひとつの側面によれば、本発明は、周囲の物体の誤った距離を出力することを防止することが可能な距離測定装置、距離測定方法、およびプログラムである。

ひとつの態様である距離測定装置は、車両に設けられる。三次元位置算出部は、車両に設けられた撮影装置により前記車両の位置が第1の位置であるときに撮影された第1の画像、および前記第1の位置とは異なる第2の位置であるときに撮影された第2の画像を受付ける。三次元位置算出部は、前記車両に設けられた移動量検出装置から、前記第1の位置と前記第2の位置との間の前記車両の移動量を受付け、前記第1の画像、前記第2の画像、および前記移動量に基づき、物体の三次元位置を算出する。判別部は、算出された三次元位置に基づき設定された第3の位置を、前記第1の画像および前記第2の画像のうち一方の画像に投影した第4の位置と、前記一方の画像における前記物体の位置とのずれが所定値以上の場合に、算出された三次元位置が誤りであると判別する。出力部は、算出された前記三次元位置が、誤りであると判別されなかった場合には距離測定対象とし、誤りであると判別された場合に距離測定対象から除外して前記距離測定対象の前記三次元位置を出力する。

別の態様によれば、演算処理装置が、車両に設けられた撮影装置により前記車両の位置が第1の位置であるときに撮影された第1の画像、および前記第1の位置とは異なる第2の位置であるときに撮影された第2の画像を受付ける。演算処理装置は、前記車両に設けられた移動量検出装置から、前記第1の位置と前記第2の位置との間の前記車両の移動量を受付け、前記第1の画像、前記第2の画像、および前記移動量に基づき、物体の三次元位置を算出する。さらに、演算処理装置は、算出された三次元位置に基づき設定された第3の位置を、前記第1の画像および前記第2の画像のうち一方の画像に投影した第4の位置と、前記一方の画像における前記物体の位置とのずれと所定値とを比較する。ずれが所定値以上の場合に、演算処理装置は、算出された三次元位置が誤りであると判別し、算出された前記三次元位置が、誤りであると判別されなかった場合には距離測定対象とし、誤りであると判別された場合に距離測定対象から除外する。

なお、上述した態様に係る方法をコンピュータに行わせるためのプログラムであっても、このプログラムを当該コンピュータによって実行させることにより、上述した態様に係る方法と同様の作用効果を奏するので、前述した課題が解決される。

上述した態様の距離測定装置、距離測定方法、およびプログラムによれば、周囲の物体の誤った距離を出力することを防止することができる。

一実施の形態による距離測定装置の機能的な構成の一例を示すブロック図である。

一実施の形態による距離測定装置のハードウエア構成の一例を示すブロック図である。

移動体として車両が路面上を走行している場合の座標系の関係を示す図である。

車両が移動中の撮影状況を概念的に示す図である。

カメラ位置と特徴点との位置関係の一例を示す図である。

エピポーラ拘束がない場合の測定例を概念的に示す図である。

対象物体の移動方向が車両の移動方向と平行で同方向の場合を概念的に示す図である。

対象物体の移動方向が車両の移動方向と平行で逆方向の場合を概念的に示す図である。

距離測定装置による静止物と移動物の距離測定を概念的に示す図である。

三次元位置算出例を撮影画像に再投影した場合を概念的に示す図である。

移動物の判定方法を概念的に説明する図である。

エッジリストの一例を示す図である。

登録済みリストの一例を示す図である。

物体リストの一例を示す図である。

接続先リストの一例を示す図である。

最終接続先リストの一例を示す図である。

個数リストの一例を示す図である。

探索リストの一例を示す図である。

含有リストの一例を示す図である。

動静リストを示す図である。

移動物判定の一例を説明する図である。

距離測定装置の主要な動作を示すフローチャートである。

特徴点抽出処理の詳細を示すフローチャートである。

特徴点照合処理の詳細を示すフローチャートである。

特徴点更新処理の詳細を示すフローチャートである。

三次元位置算出処理の詳細を示すフローチャートである。

物体検出処理を示すフローチャートである。

初期番号付与処理の詳細を示すフローチャートである。

初期番号付与処理の詳細を示すフローチャートである。

物体番号の決定処理の詳細を示すフローチャートである。

移動物判定処理の一例を示すフローチャートである。

撮影画像への再投影処理の詳細を示すフローチャートである。

探索範囲算出処理の詳細を示すフローチャートである。

移動物判定処理の詳細を説明するフローチャートである。

結果出力処理の詳細を示すフローチャートである。

変形例による駐車支援システムの構成例を示す図である。

標準的なコンピュータのハードウエア構成の一例を示すブロック図である。

以下、図面を参照しながら、本実施の形態による距離測定装置1について説明する。距離測定装置1は、例えば車両などの移動体に備えられ、異なる位置から周囲の画像を撮影し、物体までの距離を計測する移動ステレオ法により、所定距離以内に存在する物体に関する情報を出力することにより駐車時の支援を行う装置である。

図1は、一実施の形態による距離測定装置1の機能的な構成の一例を示すブロック図である。図2は、距離測定装置1のハードウエア構成の一例を示すブロック図である。図1に示すように、距離測定装置1は、特徴点抽出部3、特徴点照合部5、特徴点更新部7、三次元位置算出部9、物体検出部11、移動物判定部13、出力部15、特徴点DataBase(DB)17の機能を備えている。

図1に示した距離測定装置1の機能は、例えば、図2に示したハードウエア構成により実現される。距離測定装置1は、演算処理装置20、カメラ21、車速センサ23、舵角センサ24、記憶装置25、ディスプレイ27、スピーカ28などを有している。図1に示した距離測定装置1の各機能は、例えば記憶装置25に記憶されたプログラムを読み込み、演算処理装置20により実行することにより実現される。また、距離測定装置1の各機能は、その例えば一部を、半導体を用いた集積回路などにより実現するようにしてもよい。

演算処理装置20は、距離測定装置1の動作制御などの演算処理を行う演算処理装置である。カメラ21は、例えば車両に固定された単眼の撮影装置である。車速センサ23は、車両の速度を検出する検出装置である。舵角センサ24は、車両の例えばハンドル操作角を検出することにより、進行方向の変化を検出する検出装置である。記憶装置25は、随時読出し書込み可能な記憶装置等である。記憶装置25には、例えば距離測定装置1の動作を制御するプログラムや、特徴点DB17が記憶されるようにしてもよい。また、距離測定装置1において行われる処理において、一時的に情報を記憶させるようにしてもよい。

ディスプレイ27は、例えば液晶表示装置などの表示装置であり、周囲を撮影した画像、算出された三次元距離に基づき処理された画像等を表示する。スピーカ28は、例えば、算出された三次元距離に基づき音声を発生する音声出力装置である。

図1に戻って、特徴点抽出部3は、カメラ21で撮影された画像から特徴点を抽出する。特徴点照合部5は、カメラ21において第1の時刻と、車両が移動した後の第2の時刻において互いに異なる位置から撮影された画像に共通して存在すると判断される特徴点を照合する。特徴点DB17は、例えば、現在の画像の一つ前の画像に関する情報が記憶されたデータベースである。特徴点DB17は、過去のいくつかの画像に関する情報を有するようにしてもよい。

特徴点更新部7は、特徴点DB17を例えば、最新の画像の情報に更新する。なお、特徴点DB17がいくつかの画像に関する情報を有する場合には、特徴点更新部7は、最も古い画像の情報を削除して最新の情報を追加するようにしてもよい。三次元位置算出部9は、特徴点照合部5において照合された共通する特徴点と、例えば車速センサ23、舵角センサ24から得られる第1の時刻から第2の時刻までの車両の移動量とに基づき、共通する特徴点に対応する物体の三次元位置を算出する。

物体検出部11は、三次元位置算出部9により算出された三次元位置が互いに所定距離よりも小さい距離範囲にあるものを同一の物体の三次元位置として判別することにより、物体を検出する。移動物判定部13は、検出された同一の物体の三次元位置を例えば路面上に投影するなどの処理を行い、カメラ21で撮影された画像上に再投影し、画像上に撮影された物体との位置ずれが所定以内か否かを判別する。移動物判定部13は、位置ずれが所定以内の場合には対象となる物体は静止物であり、誤検出ではないと判別し、所定以上の場合には誤検出であると判別する。出力部15は、誤検出であると判別された三次元位置を算出結果から削除し、誤検出ではないと判別された三次元位置のみを出力する。

なお、上記構成および動作の詳細については後述する。ここではまず、本実施の形態による距離測定装置1における距離測定方法について、全体的な説明を行う。図3は、移動体として車両50が路面54上を走行している場合の座標系の関係を示す図である。

図3に示すように、本実施の形態による距離測定方法では、車両50にカメラ21が固定されており、車両50は、路面54上を走行する場合を例にして説明する。このとき、車両50のある一点から路面54への垂線の足を原点Oとし、路面54に垂直な方向にZ軸、車両50の進行方向に平行な方向にY軸をとる直交座標系O−XYZを、移動体座標系という。この車両50のある一点は、例えば、車両50の重心、路面54に平行な断面における中心などとするようにしてもよい。

一方、車両50に備えられたカメラ21のカメラ位置(例えば、カメラ21の対物レンズの中心点:以下、単にカメラ位置という。)を原点oとし、光軸をz軸とする直交座標系o−xyzをカメラ座標系という。このとき、カメラ座標系において、z軸に垂直で、カメラ位置から矢印58のようにカメラの焦点距離f離れた面を、撮像面52とする。矢印56は、移動体座標系とカメラ座標系との変換を示しているが、詳細は後述する。なお、上記各座標系の定義は一例であり、これに限られない。

図4は、車両50が移動中の撮影状況を概念的に示す図である。図4に示すように、時刻t1におけるカメラ位置を原点o1とする第1のカメラ座標系は、座標系o1−xyzであり、時刻t2におけるカメラ位置を原点o2とする第2のカメラ座標系は、座標系o2−xyzであるとする。矢印66は、時刻t1と時刻t2との間の車両50の移動、および第1のカメラ座標系と第2のカメラ座標系との変換を示しているが、詳細は後述する。また、撮像面62は、時刻t1における撮像面であり、撮像面64は、時刻t2における撮像面である。

図4に示すように、時刻t1においては、第1のカメラ座標系において、特徴点P1(px1、py1、f)が抽出されている。また、時刻t2においては、第2のカメラ座標系において、特徴点P2(px2、py2、f)が抽出されている。図4は、これらの特徴点は、後述する照合処理により同一の物体の点P(X,Y,Z)(移動体座標系)を撮影したものであることを概念的に示している。

図5は、カメラ位置と特徴点との位置関係の一例を示す図である。図5に示すように、移動体座標系における時刻t1のカメラ21の三次元位置と時刻t2のカメラ21の三次元位置とが分かっており、測定対象の物体(以下、対象物体という)が静止物であるとする。この場合、時刻t2での物体特徴点の撮像面64における位置は、エピポーラ線77上に存在する。エピポーラ線77とは、時刻t1、時刻t2のカメラ位置(原点o1,o2)と、原点o1と特徴点P1とを結ぶ直線73とを含む平面(エピポーラ面)と、時刻t2の撮像面64との交線である。ここでは、エピポーラ面上のエピポーラ点79、エピポーラ点81が示されている。

直線73上に存在する物体特徴点は、時刻t2の撮像面64では、エピポーラ線77上のどこかに存在することが知られている。この性質をエピポーラ拘束という。本実施の形態において、三次元位置を算出する移動ステレオ法で静止物の三次元位置を算出する場合は、エピポーラ拘束が成立する。

図6は、エピポーラ拘束がない場合の測定例を概念的に示す図である。図6に示すように、時刻t1において点Pにあった対象物体が、矢印83のように、例えば、上記エピポーラ面上でない点P’に移動している。このとき、時刻t2で撮影される撮像面64では、対象物体は点P2’の位置となり、エピポーラ線77上には存在しない。このような性質を利用して、2時刻の特徴点照合時はエピポーラ線上の特徴点を探索することで、精度よくかつ効率よく特徴点照合がおこなわれる。

しかし、対象物体の移動方向によっては、時刻t2に見える特徴点の画像位置がエピポーラ線上に存在し、特徴点照合がとれてしまう特殊な場合がある。以下、その特殊な場合の一例として、移動方向が、車両50の進行方向と平行の場合について説明する。なお、上記特殊な場合は、対象物体が移動物である場合の特殊な例であるため、以下、便宜的に特殊な場合を「移動物」、正確な三次元位置算出が可能な場合を「静止物」として説明する。

図7は、対象物体の移動方向が車両50の移動方向と平行で同方向の場合を概念的に示す図である。図7に示すように、カメラ位置90が車両移動方向92の方向に移動し、物体位置94が変わらない場合には、対象物体は、直線98で示した方向に観察される。一方、対象物体が移動方向96に移動している場合は、対象物体は、直線100により示した観察方向に観察される。よって、対象物体の位置は算出位置102と算出され、矢印104に示すように、実際の位置より水平方向に遠方にあると算出される。同様に、矢印106に示すように、算出位置102は、上下方向には下側の算出位置102にあると算出される。

図7の例のように、車両50と対象物体とが同方向に移動している場合は、幾何学的な位置関係としては実際の位置より遠方に存在している静止物と考えても矛盾しない。ただし、高さは実際の位置より下側(路面下)になるため、算出された三次元位置は、移動物によるものとして除去することができる。

図8は、対象物体の移動方向が車両50の移動方向と平行で逆方向の場合を概念的に示す図である。図8に示すように、カメラ位置90が車両移動方向92の方向に移動し、物体位置108が変わらない場合には、対象物体は、直線98で示した方向に観察される。対象物体が移動方向110に移動している場合は、対象物体は、直線112により示した観察方向に観察される。よって、対象物体の位置は算出位置114と算出され、矢印116に示すように、車両50の位置に対して、実際の位置よりも水平方向に手前にあると算出される。同様に、矢印108に示すように、算出位置114は、上下方向には上側の算出位置114にあると算出される。図8の例のように、車両50と対象物体とが逆方向に移動している場合は、幾何学的な位置関係としては実際の位置より近方に浮いている静止物と考えても矛盾しないことになる。

図9は、距離測定装置1による静止物と移動物の距離測定を概念的に示す図である。図9に示すように、時刻t1における撮影状況130において、静止物120と、車両の移動方向とは逆の矢印110の方向に移動する移動物109とが車両50の周囲に存在しているとする。時刻t2における撮影状況132においては、静止物120と矢印110の方向に移動する移動物109とが存在しているとする。それぞれの時刻に撮影された画像に基づき本実施の形態による三次元位置算出部9が三次元位置を算出する。このとき、算出例134に示すように、静止物120については、物体の下端の三次元位置を路面に投影した位置は、例えば、路面位置122となり、静止物120と一致する。しかし、移動物109については、例えば、物体の下端の三次元位置を路面に投影した位置として例えば路面位置124が算出される。すなわち、実際の移動物109より車両50に近い位置が算出され、実際の移動物109の位置と一致しない。

図10は、算出例134を撮影画像に再投影した場合を概念的に示す図である。図10に示すように、算出例134を例えば、時刻t2において撮影された画像140に再投影すると、静止物120については、路面位置122は、再投影位置123に投影され、静止物120の下端位置と一致する。しかし、移動物109については、路面位置124は再投影位置125に投影され、実際の移動物109とは一致しない。

図11は、移動物の判定方法を概念的に説明する図である。図11に示すように、画像140において、再投影位置123は、静止物120の三次元位置に対応して算出された位置であり、探索範囲127は、再投影位置123に基づき算出される範囲である。同様に、移動物109では、再投影位置125、および探索範囲128が示されている。探索範囲127、128の算出例については後述する。

静止物120では、画像140上の静止物120の下端は、再投影位置123に基づき算出された探索範囲127内に存在している。一方、移動物109では、画像140上の移動物109の下端126は、算出された探索範囲128を外れている。

このように、算出された三次元位置と、実際の三次元位置との差を、路面54に投影された物体の下端位置を画像140に再投影した位置と、画像140における物体の下端位置との差により判別して、誤った三次元位置が出力されることを防止する。

以下、本実施の形態における距離測定装置1について、さらに詳細に説明する。まず、図12を参照しながら、特徴点抽出部3について説明する。図12は、エッジリスト154の一例を示す図である。エッジリスト154は、特徴点抽出部3による処理により生成される情報である。以下、例えばエッジリスト154、登録済みリスト156など各情報を示すリストは、特に断らない限り、各リストの一要素を代表して図示する。各リストは、図示した要素の集合とすることができる。

特徴点抽出部3は、車両50に取り付けたカメラ21によって撮影された画像から、特徴点(エッジ点)群を検出する。なお、必要に応じてカラー映像をモノクロ濃淡画像に変換したり、撮影画像に対して歪み補正を行ったりする。特徴点とは、画像において、明るさが急激に変化している箇所であり、特徴点の抽出には、Sobelフィルタなど、一般的なフィルタ処理を用いることができる。特徴点抽出部3は、各点(画素)のフィルタ適用後の値(エッジ強度)が閾値以上であれば、特徴点として抽出する。

特徴点抽出部3は、対象となる画像の各画素のエッジ強度を算出し、エッジ強度が閾値以上になった箇所がある場合は、エッジリスト154に、後述する現在特徴点位置、周囲パターン情報を登録する。

図12に示すように、エッジリスト154は、ある時刻に取得された画像において抽出された特徴点e(s1)に関する情報である。ここで、変数s1は1〜Neの整数である。要素数Neは、取得された画像において抽出された特徴点数を示す整数であり、特徴点の要素数ともいう。エッジリスト154は、現在特徴点位置、過去特徴点位置、パターン情報、照合処理フラグを有している。現在特徴点位置は、例えば、時刻t1に撮影された画像から抽出された特徴点の位置であり、画像上に定義された座標(nx、ny)で表される。過去特徴点位置は、後述する照合処理で照合された場合に記憶される、例えば、一つ前に撮影された画像における対応する特徴点の座標(rx、ry)である。パターン情報vn(b・a)は、特徴点(nx、ny)を中心としたy軸方向にb画素分、x軸方向にa画素分の画素値を表す。画素値は、例えば輝度、エッジ強度等である。照合処理フラグは、後述する特徴点照合部5により記憶される情報であり、対応する特徴点が抽出されたか否かを示す。

なお、図12のエッジリスト154では、一つの特徴点e(s1)に対応する情報のみを示しているが、複数の特徴点のおのおのに応じて、同様の情報を有することができる。また、エッジリスト154は、特徴点抽出処理の前に初期化(要素数Ne=0)する。

次に、図13を参照しながら、特徴点照合部5の詳細について説明する。図13は、登録済みリスト156の一例を示す図である。登録済みリスト156は、過去に撮影された画像においてにすでに抽出された特徴点に関する情報であり、例えば特徴点DB17に記憶される。

例えば、エッジリスト154が時刻t2に撮影された画像に関する情報であるとすると、登録済みリスト156は、時刻t2より前の時刻t1に撮影された画像において特徴点抽出部3により抽出された特徴点r(s2)に関する情報とすることができる。ここで、変数s2は、1〜Nrの整数である。要素数Nrは、対象となる画像、例えば前フレームにおいて抽出された特徴点の数である。

カメラ21により撮影される画像が動画像である場合には、登録済みリスト156は、現在の一つ前のフレームの画像であるとすることができる。以下、エッジリスト154に対応する画像を現フレーム、登録済みリスト156に対応する画像を前フレームという。

登録済みリスト156は、特徴点位置とパターン情報を有している。なお、登録済みリスト156では、一つの特徴点r(s2)に対応する情報のみを示しているが、複数の特徴点のおのおのに応じて、同様の情報を有することができる。登録済みリスト156は、時系列に使用するため、初期化はしない。

特徴点照合部5は、特徴点抽出部3で抽出した現フレームの特徴点群と、特徴点DB17に記録された例えば前フレームの特徴点群について照合を行う。照合の際には、特徴点照合部5は、例えば、特徴点を中心として一定の大きさの領域(例えば、特徴点を含むa×b個の画素)を設定する。特徴点照合部5は、特徴点同士の上記一定の大きさの領域内の画素値に基づく一般的な映像間の照合処理を適用できる。一般的な照合処理とは、例えば、一定の大きさの領域の輝度値の差の絶対値和(Sum of Absoluted Difference:SAD)や、輝度値の差の自乗和(Sum of Squared Difference:SSD)等を用いた処理である。また、照合条件に特徴点間の距離が閾値以内であることを加えてもよい。

特徴点照合部5が、エッジリスト154と登録済みリスト156との間で照合に成功した場合には、登録済みリスト156の要素r(s2)の特徴点位置(rx,ry)をエッジリストの要素e(s1)の特徴点位置(rx,ry)に登録する。また、特徴点照合部5は、要素e(s1)のフラグ(flag)を例えば「1」にする。

次に特徴点更新部7についてさらに説明する。特徴点更新部7は、登録済みリスト156を初期化し、特徴点抽出部3で抽出した特徴点群のエッジリスト154の情報を、登録済みリスト156に登録することにより、登録済みリスト156を更新する。特徴点更新部7は、更新された登録済みリスト156を、特徴点DB17に記録する。

次に、三次元位置算出部9についてさらに説明する。三次元位置算出部9は、特徴点照合部5で照合に成功した2時刻の特徴点位置と車両50の挙動(カメラ21の移動量)から、特徴点に対応する三次元位置を算出する。車両50の挙動は車速センサ23、舵角センサ24等から取得する。以下に、三次元位置の算出方法について述べる。

座標系については、上述したように、移動体座標系と、カメラ座標系を図3のように定義する。図3の矢印56に示すように、移動体座標系とカメラ座標系との変換は、移動体座標系において、回転行列Rw、並進ベクトルTwにより表されるとする。回転行列Rwとは、例えば、移動体座標系の各軸に対するカメラ座標系の各軸のなす角を用いて表される3行3列の行列である。並進ベクトルTwとは、例えば、移動体座標系の原点に対するとカメラ座標系の原点の位置を表す3行1列の行列である。

回転行列Rm、並進ベクトルTmで表される車両50の移動量(Rm、Tm)は、移動体座標系で記述されたものである。また、時刻t1での移動体座標系で表記した三次元位置をベクトルXw1、時刻t2での移動体の座標系で表記した三次元位置をベクトルXw2とし、同じ点を、カメラ座標系で表記した三次元位置をベクトルxh1、xh2とする。ここで、時刻t1から時刻t2での移動体の移動量を回転行列Rm、並進ベクトルTmで表すとする。このとき、移動体の座標系Xw(X,Y,Z)からカメラ座標系xh(x,y,z)への変換は、回転行列Rw、並進ベクトルTwを用いて、以下のように表されるものとする。なお、「*」は乗算を表す。 xh=Rw*(Xw−Tw)・・・(式1) よって、時刻t1では、 xh1=Rw*(Xw1−Tw)・・・(式2) 時刻t2では、 xh2=Rw*(Xw2−Tw)・・・(式3) となる。ここで、時刻t1と時刻t2との間でのベクトルXw1とXw2の関係は、 Xw2=Rm*Xw1+Tm・・・(式4) と表されるとすると、式3より以下のように表される。 Rw^T*xh2+Tw=Rm*(Rw^T*xh1+Tw)+Tm =Rm*Rw^T*xh1+Rm*Tw+Tm・・・(式5)

式5を変形すると、 xh2=Rw*Rm*Rw^T*xh1+Rw*(Rm*Tw+Tm−Tw) =(Rw*Rm*Rw^T)*xh1+Rw*((Rm−I)*Tw+Tm) ・・・(式6) となる。式6において、以下のように置き換える。 Rv=Rw*Rm*Rw^T Tv=Rw*((Rm−I)*Tw+Tm)・・・(式7)

よって、 xh2=Rv*xh1+Tv・・・(式8) と表すことができる。これは、カメラ座標系で考えた場合の、時刻t1から時刻t2で移動体が回転行列Rm、並進ベクトルTmの移動をする場合の座標間の関係を表している。以降は、このカメラ座標系について考える。

図4で説明したように、カメラの焦点距離は焦点距離fとする。時刻t1での特徴点P1の特徴点位置(px1,py1)、時刻t2での特徴点P2の特徴点位置(px2,py2)はそれぞれ撮像面上での位置を表している。これらの特徴点位置は、xh座標系では、それぞれ、点(px1,py1,f)、点(px2,py2,f)の位置を表している。ここで、 n1=(px1,py1,f) n2=(px2,py2,f)・・・(式9) とする。

このとき、図4に示すように、時刻t1での特徴点位置n1の延長線と、時刻t2での特徴点位置n2の延長線の交わる位置Pが、実際の物体の三次元位置となる。時刻t1でのカメラ座標系での三次元位置を点P1とし、点P1までの距離を距離l1とする。また、時刻t2でのカメラ座標系での三次元位置を点P2とし、点P2までの距離を距離l2とする。以下、説明の都合上、点P1、P2を示すベクトルをそれぞれベクトルP1、P2とし、ベクトルn1、n2を式9の成分を持つ単位ベクトルとすると、下記の式10が成り立つ。 P1=l1*n1 P2=l2*n2・・・(式10)

ここで、ベクトルP1とベクトルP2には、式8より、行列Rv、ベクトルTvを用いて、 P2=Rv*P1+Tv・・・(式11) の関係があるので、式10、式11より、 l2*n2=Rv*l1*n1+Tv・・・(式12) となる。ここで、N1=Rv*n1と置くと、 l2*n2=l1*N1+Tv・・・(式13) となる。ここで、表記を以下のように改めるものとする。 N1=(NX1,NY1,NZ1) n2=(nx2,ny2,nz2)・・・(式14) また、ベクトルTvを成分表示として、以下のように定義する。 Tv=(Tvx,Tvy,Tvz)・・・(式15)

よって、式13から式15により、以下のように成分表示される。 l2*nx2=l1*NX1+Tvx l2*ny2=l1*NY1+Tvy l2*nz2=l1*NZ1+Tvz・・・(式16)

式16の連立方程式は、未知数が(l1、l2)で過拘束の関係にある。よって一意に解が求まらないため、最小二乗の意味で最適な解を求める。この連立方定式を書き直すと、以下の式17で表される。

となる。ここで式17において、式18のように表す。

よって、下記の式19が成り立つ。 N*L=T・・・(式19) したがって、最小二乗での解は、下記の式20で与えられる。 L=(N^T*N)⁻¹*N^T*T・・・(式20)

式20により、距離l1、l2が求まるので、時刻t2でのカメラ座標系で表した三次元位置を示すベクトルP2は、下記の式21で表される。 P2=n2*l2・・・(式21) カメラ座標系xhと移動体の座標系Xwの関係は、式1に示したように、xh=Rw*(Xw−Tw)なので、下記の式22となる。 Xw=Rw^T*xh+Tw・・・(式22) よって、カメラ座標系での三次元位置である点P2を移動体の座標系で表したものをP(X,Y,Z)とすると、下記式23を満たす位置Pが三次元位置を示す最終的な解となる。 P=Rw^T*P2+Tw・・・(式23)

このとき、三次元位置算出部9は、エッジリスト154において、照合処理フラグを参照し、フラグが照合に成功していることを示す場合に、上記のように三次元位置P(X、Y、Z)を算出する。

図14は、物体リスト158の一例を示す図である。物体リスト158は、特徴点照合部5により照合された特徴点o(i)に関する情報である。ここで、変数iは、1からNoまでの整数であり、要素数Noは、特徴点照合部5で照合に成功した特徴点の組数である。物体リスト158は、三次元位置、物体番号、投影画像下位置、投影画像上位置を有している。三次元位置算出部9は、算出された三次元位置を物体リスト158に登録する。

三次元位置は、上記のように算出された三次元位置Pの移動体座標系における座標である。物体番号は、後述する物体検出部11による物体検出処理により物体に応じて割り振られる識別番号である。投影画像上位置とは、特徴点を路面54に投影した位置から移動体座標系のZ軸において所定の高さ上の点を、例えば第2の時刻に撮影された画像に再投影した点のカメラ座標系における座標である。投影画像下位置とは、特徴点を路面54に投影した点から所定距離下側に離れた点を、画像に再投影した点のカメラ座標系における座標である。

次に、物体検出部11について説明する。物体検出部11は、三次元位置を算出した特徴点群から物体を検出する。物体検出方法としては、例えば、特徴点同士の三次元距離が近ければ同一物体と判断する一般的なラベリング処理を適用する。特徴点同士の距離については、三次元位置で計算してもよいし、高さ方向を考慮しない俯瞰座標における二次元位置で計算してもよい。

物体検出部11は、まず物体リスト158のコピーを用意する。以下、コピーした物体リスト158をコピーリストという。物体検出部11は、物体リスト158とコピーリストとの三次元位置を逐次比較し、同一の物体と判断された物体リスト158の物体番号に同一の番号を割り振ることにより、物体を検出する。物体検出部11は、物体検出を行う際に、図15、図16に示す接続先リストを用いる。すなわち、物体検出部11は、最初に特徴点o(i)に初期物体番号を付与するが、この時点では、同じ物体でも物体番号が異なる場合がある。このため、物体検出部11は、初期物体番号の整理を行い、最終的な物体番号を決定するため、接続先リスト160、最終接続先リスト162を用いる。

図15は、接続先リストの一例を示す図、図16は、最終接続先リストの一例を示す図である。図15に示すように、接続先リスト160は、接続先を示す接続先jump(i)を有している。上記のように、接続先リスト160は、先に振られた物体番号と同一の物体と判別される特徴点が抽出された場合に、先に振られた物体番号と特徴点とを関連付けるためのリストである。変数iは、1からNoまでの整数であり、要素数は、付与された初期物体番号数Noである。

図16に示すように、最終接続先リスト162は、整理された最終的な物体番号を示す接続先jump2(i)を有している。接続先jump2(i)の要素数は、最終的な物体番号数である。

続いて、移動物判定部13について説明する。移動物判定部13は、算出した物体の路面54の位置における三次元位置を例えば撮像面64などの撮影画像に投影し、撮影画像における実際の特徴点位置と比較する。これにより移動物判定部13は、対象物体が、三次元位置が正確に計測可能な対象物体か否かを判定する。

移動物判定部13は、特徴点の三次元位置を路面に投影した位置と、路面に投影した位置から一定高さの位置とを設定し、さらに、それぞれの位置を例えば撮像面64の撮影画像に再投影する。移動物判定部13は、一つの物体毎の上記再投影した位置から、物体の下端の特徴点が存在すると見込まれる探索範囲を設定し、この探索範囲内に物体下端特徴点が存在するか調べる。

例えば、移動物判定部13は、探索範囲内に物体下端特徴点が一定以上存在する場合は、算出された三次元位置と物体下端特徴点とが一致するとして、物体は静止物であり、算出された三次元位置は誤算出ではないとみなす。一方、移動物判定部13は、探索範囲内に物体下端特徴点が一定以上存在しない場合は、算出された三次元位置と物体下端特徴点が一致しない、つまり幾何学的に矛盾が生じるとして、物体は移動物であり、三次元位置は誤算出であるとみなす。

図17は、個数リスト164の一例を示す図である。個数リスト164は、移動物判定部13が、物体下端特徴点が探索範囲内にあるか否かを判別する際に用いる情報である。個数リスト164は、物体番号noを用いて要素n(no)と表す。個数リスト164は、同一の物体と判別された特徴点の数numである。個数リスト164の要素数は、最終的な物体番号数である。

図18は、探索リスト166の一例を示す図である。探索リスト166は、一つの物体毎の探索範囲を示す情報である。探索リスト166は、探索範囲におけるx方向の最小値、最大値、y方向の最小値、最大値を有している。探索リスト166の内容は、物体番号noを用いて要素s(no)で表される。探索リスト166の要素数は、最終的な物体番号数である。

図19は、含有リスト168の一例を示す図である。含有リスト168は、物体毎の探索範囲に含まれる特徴点の数cntの情報である。特徴点の数は、物体番号noを用いてcnt(no)と表される。含有リスト168の要素数は、最終的な物体番号数となる。

図20は、動静リスト170を示す図である。動静リスト170は、算出された三次元位置が移動体のものであるか否かを示すフラグmoveの情報である。動静リスト170の内容は、物体番号noを用いて要素m(no)で表される。動静リスト170の要素数は、最終的な物体番号数となる。

図21は、移動物判定の一例を説明する図である。図21に示すように、本実施の形態においては、車両50の、路面54上の点を原点Oとする移動体座標系と、撮像面178を撮影するカメラ位置を原点とするカメラ座標系が定義される。車両50の周囲に移動物109が存在し、移動物109の下端位置174が特徴点として抽出されているとする。また、算出された三次元位置P(X,Y,Z)を路面54に投影した点を、点Ph0(X,Y,h0)、点Ph0からある高さの点を、点Ph1(X,Y,h1)とする。点Ph0,Ph1を撮像面178に投影した点をそれぞれ投影点ph0、ph1とする。なお、点pは、算出された三次元位置Pを撮像面178に再投影した点である。移動物判定部13は、移動物109毎の特徴点に対する投影点に基づき探索範囲180を設定し、探索範囲内に、撮像面178における移動物109の下端が存在しているか否かを判別する。

以上のように構成される距離測定装置1で行われる処理について、フローチャートを参照しながらさらに説明する。図22は、本実施の形態による距離測定装置1の主要な動作を示すフローチャートである。図22に示すように、距離測定装置1は、カメラ21で撮影された画像を取得する(S211)。なお、カメラ21は、車両50が走行している状態で、例えば所定時間毎に撮影を行う。距離測定装置1の特徴点抽出部3は、取得した画像から、特徴点を抽出する(S212)。

特徴点照合部5は、特徴点抽出部3で抽出された現フレームの特徴点と、前フレームで抽出された特徴点とを照合し、共通に表示されていると判断される点を抽出する(S213)。特徴点更新部7は、登録済みリスト156を、エッジリスト154の内容に更新し、特徴点DB17に記憶させる(S214)。

三次元位置算出部9は、例えば、車速センサ23、舵角センサ24から、車両情報を取得する(S215)。車両情報とは、車両50の位置を示す情報である。三次元位置算出部9は、取得した車両情報およびエッジリスト154に基づき、三次元位置を算出し、物体リスト158を生成する(S216)。

物体検出部11は、物体リスト158に基づき、物体を検出する(S217)。移動物判定部13は、生成された物体リスト158から、物体毎の探索範囲を検出し、検出された物体の下端位置が探索範囲に含まれるか否かに基づき、物体が移動物であるか否かを判定する(S218)。出力部15は、移動物であると判定された三次元位置を結果から削除して算出された三次元位置を出力する(S219)。

以下、上記処理の詳細についてさらに説明する。図23は、特徴点抽出処理の詳細を示すフローチャートである。特徴点抽出処理は、図22のS212の処理である。図23に示すように、特徴点抽出部3は、取得した画像において、エッジ処理を行う(S231)。エッジ強度が閾値以上と判定された場合は(S232:YES)、エッジリスト154に、特徴点e(s1)に関する位置(nx,ny)、パターン情報vn(b・a)、照合処理フラグflag=0、要素数Ne=Ne+1を登録する(S233)。S232で、閾値未満の場合には、特徴点抽出部3は、S234に処理を進める(S232:NO)。画像における全画素について処理が終了するまでS231からS233の処理を繰り返し(S234:NO)、終了すると(S234:YES)、特徴点抽出部3は、特徴点抽出処理を終了する。

図24は、特徴点照合処理の詳細を示すフローチャートである。特徴点照合処理は、図22のS213の処理である。図24に示すように、特徴点照合部5は、登録済みリスト156に、未処理の要素があるか否か判別する(S241)。未処理の要素がある場合には、特徴点照合部5は、特徴点DB17に記憶されている登録済みリスト156から、未処理の要素r(s2)を抽出する(S242)。特徴点照合部5は、エッジリスト154に未処理の要素があるか否かを判別し(S243)、ある場合には(S243:YES)、未処理の要素e(s1)をエッジリスト154から取り出す(S244)。

特徴点照合部5は、取り出した要素r(s2)と要素e(s1)とを照合する(S245)。すなわち、上述のように、パラメータ情報に基づき、共通に表示された点であるか否かを判別する。照合が成功し、共通に表示された点であると判断されると(S246:YES)、特徴点照合部5は、エッジリスト154の要素e(s1)の過去特徴点位置に、要素r(s2)の特徴点位置(rx、ry)を登録する(S247)。また、特徴点照合部5は、要素e(s1)の照合フラグflag=1とし(S248)、処理をS241に戻す。特徴点照合部5は、上記の処理を登録済みリスト156に、未処理の要素r(s2)がなくなるまで実行し、なくなると(S241:NO)、特徴点照合処理を終了する。

図25は、特徴点更新処理の詳細を示すフローチャートである。特徴点更新処理は、図22のS214の処理である。図25に示すように、特徴点更新部7は、登録済みリスト156を初期化する(S261)。特徴点更新部7は、エッジリスト154に、未処理の要素e(s1)があるか否か判別し(S262)、ある場合には(S262:YES)、未処理の要素e(s1)を取り出す(S263)。特徴点更新部7は、取り出した要素e(s1)について、現在特徴点位置(nx,ny)とパターン情報vn(b・a)を登録済みリスト156の要素r(s2)の特徴点位置(rx,ry)とパターン情報vr(b・a)に登録する(S264)。特徴点更新部7は、処理をS261に戻し、S262からS264の処理を、エッジリスト154の各要素e(s1)について行う。未処理の要素がなくなると(S262:NO)、特徴点更新部7は、特徴点更新処理を終了する。

図26は、三次元位置算出処理の詳細を示すフローチャートである。三次元位置算出処理は、図22のS216の処理である。図26に示すように、三次元位置算出部9は、エッジリスト154に未処理の要素e(s1)があるか否かを判別し(S271)、ある場合には(S271:YES)、未処理の要素e(s1)を取り出す(S272)。三次元位置算出部9は、取り出した要素e(s1)の照合処理フラグを確認し、照合が成功であることを示していれば(S273:YES)、エッジリスト154の要素e(s1)の情報に基づき、上述のように三次元位置を算出する(S274)。

このとき三次元位置算出部9は、特徴点照合部5で照合に成功した2時刻の特徴点位置と車両挙動(カメラの移動量)から三次元位置を算出する。車両挙動は、車速センサ23、舵角センサ24などセンサ類から取得する。

三次元位置算出部9は、算出された三次元位置(X,Y,Z)を物体リスト158に登録し、照合が成功した要素e(s1)の要素数No=No+1と更新する(S275)。S271に処理を戻し、三次元位置算出部9は、未処理の要素e(s1)がない場合には(S271:NO)、処理を終了する。

図27は、物体検出処理を示すフローチャートである。物体検出処理は、図22のS217の処理である。図27に示すように、物体検出部11は、物体リスト158において、物体特徴点に初期番号を付与する(S281)。物体検出部11は、物体番号の決定処理を行い(S282)、物体検出処理を終了する。

続いて、物体検出処理の詳細について、さらに説明する。図28A、28Bは、初期番号付与処理の詳細を示すフローチャートである。初期番号付与処理は、図27のS281の処理である。図28Aに示すように、物体検出部11は、物体リスト158のコピーであるコピーリストを作成する。また、物体検出部11は、物体番号no=0、物体番号no(ok)=−1と初期化する。さらに、物体検出部11は、後述する接続先jump(i)=iと初期化する(S290)。

ここで、変数iは、1からNoまでの整数である。要素数Noは、物体リスト158の要素数であり、照合が成功した要素e(s1)の数である。なお、物体番号noは、変数iによる区別をせずに用いる場合の物体リスト158における物体番号をいう。後述する物体番号no(oi)、no(ok)とは、それぞれ要素o(i)、o(k)に対応する物体番号noをいう。

物体検出部11は、物体リスト158に未処理の要素o(i)があるか否かを判別し(S291)、ある場合には(S291:YES)、未処理の要素o(i)をとりだす(S292)。なお、物体番号の「−1」は、物体番号noが未付与であることを示す。

物体検出部11は、コピーリストに未処理の要素o(k)(kは、1からNoの整数)があるか否か判別し(S293)、ある場合には(S293:YES)、コピーリストから未処理の要素o(k)を取り出し、物体番号no(oi)=−1と初期化する(S294)。物体検出部11は、要素o(i)と要素o(k)との三次元位置同士の距離が近いか否かを判別する(S295)。距離が近い場合には(S295:YES)、物体検出部11は、処理をS296に進める。距離が近くない場合には(S295:NO)、物体検出部11は、処理をS293に戻す。

S293において、コピーリストに未処理の要素がない場合には(S293:NO)、物体検出部11は、物体番号no(oi)が未付与であるか否かを判別する(S305)。未付与である場合には(S305:YES)、物体検出部11は、物体番号no(oi)=no、と新規物体番号noを付与し、no=no+1とし、処理をS291に戻す。

S291で、物体リスト158に未処理の要素がない場合には(S291:NO)、全ての物体リスト158およびコピーリストの要素について処理を終了したことになり、物体検出部11は、初期番号付与処理を終了する。

図28Bに示すように、S296では、物体検出部11は、物体リスト158の物体番号no(oi)が未付与であるか、すなわち物体番号no(oi)=−1であるか否かを判別する。未付与である場合には(S296:YES)、物体検出部11は、さらに、物体番号no(ok)が未付与であるか、すなわち物体番号no(ok)=−1であるか否かを判別する(S297)。未付与である場合には(S297:YES)、物体検出部11は、物体番号no(oi)=no、物体番号no(ok)=noと新規物体番号noを付与するとともに、no=no+1とし(S298)、処理をS293に戻す。

S297において、物体番号no(ok)が未付与でない場合には(S297:NO)、物体検出部11は、物体番号no(oi)=物体番号no(ok)とし(S299)、処理をS293に戻す。

S296において、物体番号no(oi)が未付与でない場合には(S296:NO)、物体検出部11は、さらに、物体番号no(ok)が未付与であるか、すなわち物体番号no(ok)=−1であるか否かを判別する(S300)。未付与である場合には(S300:YES)、物体検出部11は、物体番号no(ok)=物体番号no(oi)とし(S301)、処理をS293に戻す。

S300において、物体番号no(ok)が未付与でない場合には(S300:NO)、物体検出部11は、物体番号no(oi)>物体番号no(ok)であるか否かを判別する(S302)。物体番号no(oi)>物体番号no(ok)である場合には(S302:YES)、物体検出部11は、接続先jump(i)=物体番号no(ok)とし(S303)、処理を293に戻す。物体番号no(oi)>物体番号no(ok)でない場合には(S302:NO)、物体検出部11は、接続先jump(k)=物体番号no(oi)とし(S304)、処理をS293に戻す。ここで、接続先リスト160は、接続先としての接続先jump(i)の情報の集合である。なお、接続先リスト160の要素数は、付与された初期物体番号数である。

図29は、物体番号の決定処理の詳細を示すフローチャートである。この処理は、図27のS282の処理の詳細であり、物体初期番号を整理して最終的な物体番号を付与する処理である。図29に示すように、物体検出部11は、接続先リスト160に、未処理の要素があるか否かを判別する(S311)。未処理の要素がある場合には(S311:YES)、物体検出部11は、未処理の接続先jump(i)を取り出し、探索用の番号jを、j=iと設定する(S312)。

物体検出部11は、探索用の番号jと接続先jump(j)が同じか否か、すなわち、接続先jump(j)=jか否かを判別する(S313)。同じでない場合には(S313:NO)、物体検出部11は、j=接続先jump(j)と接続先を再設定し(S314)、S313、S314の処理を、番号j=接続先jump(j)となるまで繰り返す。

探索用の番号j=接続先jump(j)であれば(S313:YES)、物体検出部11は、最終接続先jump2(i)=jと決定し(S315)、最終接続先リスト162に登録するとともに、処理をS311に戻す。

S311で、接続先リスト160に未処理の要素がない場合には(S311:NO)、物体検出部11は、物体リスト158に未処理の要素があるか否かを判別する(S316)。未処理の要素がある場合には(S316:YES)、物体検出部11は、未処理の要素o(i)を取り出し、要素o(i)の物体番号no(oi)=接続先jump2(no(oi))と更新する(S317)。最終接続先リスト162の要素数は、整理された最終物体番号数である。S316において、未処理の要素がない場合には(S316:NO)、処理を終了する。この処理により、物体リスト158の各要素o(i)について、最終的な物体番号が決定される。ここで決定された物体番号の数が、最終的な物体番号数となる。

次に、図30を参照しながら、移動物判定部13の詳細な動作について説明する。図30は、移動物判定部13による移動物判定処理の一例を示すフローチャートである。移動物判定処理は、図22のS218の処理である。移動物判定部13では、算出した物体の路面位置における三次元位置を撮影画像に投影し、実際の特徴点位置と比較することで移動物か静止物かを判定する。

図30に示すように、移動物判定部13は、算出された三次元位置に基づき、撮影画像への再投影処理を行う(S321)。移動物判定部13は、探索範囲を算出する(S322)。さらに移動物判定部13は、移動物判定処理を行う(S323)。

図31は、撮影画像への再投影処理の詳細を示すフローチャートである。撮影画像への再投影処理は、図30のS321の処理である。図31に示すように、移動物判定部13は、物体リスト158に未処理の要素があるか否かを判別する(S331)。未処理の要素がある場合には、移動物判定部13は、未処理の要素o(i)を取り出し、物体番号no(oi)を抽出し、個数リスト164の個数numを更新する(S332)。すなわち、移動物判定部13は、no=物体番号no(oi)、num(no)=num(no)+1とおく。

移動物判定部13は、特徴点の三次元位置(X,Y,Z)に対して、路面に下ろした位置(X,Y,h0)および、一定高さの位置(X,Y,h1)を設定する(S333)。ここで、例えば、h0は−10cm、h1は、50cmとしてもよい。

移動物判定部13は、それぞれの位置を例えば図21の撮像面178などの撮影画像に再投影する(S334)。次に、移動物判定部13は、撮影画像に再投影した画像位置(x0,y0)、(x1,y1)を物体リスト158に登録する(S335)。S331で、物体リスト158に未処理の要素がなければ(S331:NO)、移動物判定部13は、再投影処理を終了する。

なお、撮影画像への再投影は、移動体座標系Xwからカメラ座標系xhへの変換を示す回転行列Rw、並進ベクトルTwを用いて上述の式1(xh=Rw・(Xw−Tw))を用いて行うことができる。

図32は、探索範囲算出処理の詳細を示すフローチャートである。探索範囲算出処理は、図30のS322の処理である。移動物判定部13は、物体リスト158に、未処理の要素があるか否かを判別する(S341)。未処理の要素がある場合(S341:YES)、移動物判定部13は、未処理の要素o(i)を取り出し、再投影した画像位置(x0,y0)、(x1,y1)を取り出す(S342)。ここで、x0=x1、y0>y1である。

移動物判定部13は、x0

移動物判定部13は、x0>x_max(no)であるか否かを判別し(S345)、YESの場合には、x_max(no)=x0と設定する(S346)。S345でNOの場合には、移動物判定部13は、S347に処理を進める。

移動物判定部13は、y0>y_max(no)であるか否かを判別し(S347)、YESの場合には、y_max(no)=y0と設定する(S348)。S347でNOの場合には、移動物判定部13は、S349に処理を進める。

移動物判定部13は、y1

S341で、物体リスト158に未処理の要素がないと判別された場合には(S341:NO)、移動物判定部13は、探索範囲算出処理を終了する。

続いて、移動物判定部13は、設定された探索範囲内に物体下端特徴点が存在するか調べる。探索範囲内に物体下端特徴点が一定以上存在すると判断される場合は、測距点と物体下端特徴点が一致するとして静止物とみなす。一方、探索範囲内に物体下端特徴点が一定以上存在しないと判断される場合は、測距点と物体下端特徴点が一致しないつまり幾何学的に矛盾が生じるとして移動物とみなす。具体的には、算出した探索範囲に物体の特徴点が含まれる割合(含有率)によって、対象物体が移動物か静止物か、すなわち算出された三次元位置が誤算出であるか否かを判断する。

図33は、移動物判定処理の詳細を説明するフローチャートである。移動物判定処理は、図30のS323の処理である。図33に示すように、移動物判定部13は、登録済みリスト156に未処理の要素があるか否かを判別し(S361)、ある場合には(S361:YES)、移動物判定部13は、登録済みリスト156の要素r(s2)の画像位置(rx,ry)を取り出す(S362)。

移動物判定部13は、物体検出部11により検出された物体のうち、未処理の物体番号noがあるか否かを判別する(S363)。未処理の物体番号がない場合には(S363:NO)、移動物判定部13は、処理をS361に戻す。未処理の物体番号がある場合には(S363:YES)、移動物判定部13は、画像位置(rx,ry)が探索リスト166の探索範囲(x_min,x_max,y_min,y_max)に含まれるか判断する(S364)。含まれる場合は(S364:YES)、含有リスト168の含有数cntを、cnt=cnt+1と更新し(S365)、処理をS363に戻し、未処理の物体番号がなくなるまでS363からS365の処理を繰り返す。

上記処理を登録済みリスト156の全要素r(s2)に対して行った後、移動物判定部13は、物体番号の処理を、全て未処理に戻す(S366)。移動物判定部13は、未処理の物体番号があるか否か判別する(S367)。ある場合には(S367:YES)、移動物判定部13は、物体no毎の含有率=(cnt(no)/num(no))を算出する。さらに移動物判定部13は、含有率が閾値thres未満であれば(S368:YES)、対象物体は移動物であるとして、動静リスト170の移動フラグmove=1とする(S369)。

移動物判定部13は、含有率が閾値以上であれば(S368:NO)、対象物体は静止物として、動静リスト170の移動フラグmove=0とする(S370)。移動物判定用の閾値thresは、例えば0.3とすることができる。移動物判定部13は、未処理の物体番号がないと判別すると(S367:NO)、処理を終了する。

図34は、結果出力処理の詳細を示すフローチャートである。結果出力処理は、図22のS219の処理である。出力部15は、移動物判定部13により静止物と判断された特徴点のみを表示する。図34に示すように、出力部15は、物体リスト158に未処理の要素があるか否かを判別する(S381)。ある場合には(S381:YES)、出力部15は、物体リスト158の要素o(i)の物体番号no(oi)を取り出し、対応する動静リスト170の移動フラグmoveが「0」であるか判断する(S383)。「0」である場合は(S383:YES)、対象物体は静止物であるとして三次元位置を表示する(S384)。一方、移動フラグmoveが「1」である場合は(S383:NO)、対象物体は移動物であるので、出力部15は、三次元位置を表示せず、処理をS381にもどす。以上の処理を物体リスト158の全要素o(i)に対して行なう。未処理の物体リスト158における物体がなくなると(S381)、出力部15は、出力処理を終了する。

以上説明したように、本実施の形態による距離測定装置1は、車両50に取り付けた一台のカメラ21を用いて、例えば走行しながら2時刻での画像を撮影し、車両50の移動量を考慮して周囲の物体上の特徴点の三次元位置を計測する。このとき、対象物体が移動物体である場合であって、エピポーラ拘束を受ける場合については、三次元位置を正確に計測できないことがある。よって、距離測定装置1は、算出された物体の三次元位置について、三次元位置を路面に下ろした位置を撮影画像に再投影し、見かけ上の画像位置を算出する。次に、見かけ上の画像位置と実際の物体位置を比較する。比較した結果、見かけ上の画像位置付近に実際の物体下端がない場合は、距離測定装置1は、対象物体は三次元位置を正確に算出できない移動物であるとし、結果を出力しない。一方、見かけ上の画像位置付近に実際の物体下端があると判別される場合は、対象物体は正確であるとして、結果を出力する。

物体下端が、見かけ上の画像位置付近にあるか否かの判断においては、距離測定装置1は、算出された三次元位置を路面に投影し、路面に垂直な方向であってそれぞれ逆方向に所定距離離れた2点を設定し、その2点を画像上に再投影した点に基づき物体毎の探索範囲を設定する。探索範囲内で検出された全特徴点の数が、対象物体のものとされた特徴点に対し一定以上の割合であると判断された場合に、物体は位置出力対象であると判別される。

以上のように、本実施の形態による距離測定装置1によれば、単眼カメラによる移動ステレオ方式を用いて周囲に存在する物体の三次元位置を計測することができる。このとき、エピポーラ拘束が成立しない移動物が対象物体の場合には、特徴点がエピポーラ線上に存在せず、特徴点照合部5による照合はされない。さらに、距離測定装置1によれば、エピポーラ拘束を受ける特殊なケースの移動体を識別することが可能である。よって、移動物を静止物として誤検出することで、周囲の物体の誤った距離を算出することを防止することが可能となる。さらに、駐車支援を行う際に、誤った支援を行うことを防止できる。

(変形例) 距離測定装置1の変形例として、周囲に存在する静止物体までの三次元距離を上記実施の形態による距離測定装置1により計測し、その結果を提示する車載画像処理システムに適用する例が考えられる。一例として、図35に、駐車支援システム390を示している。図35に示すように、自車両の前後左右に広角カメラを4台取り付け、カメラ4台の映像を1枚の映像に合成した映像と、左右カメラによる三次元測距によって得られる周囲物(駐車車両を含む)の表面形状を重畳表示する駐車支援システム390が考えられる。このような駐車支援システム390によれば、周囲の距離感をサポートすることができる。ハードウエア構成としては、上記実施の形態による距離測定装置1において、カメラ21を車両50の前後左右に4台取り付けた形態が考えられる。

駐車時は、周囲物と接触しないように自車両の周囲を確認する必要がある。そこで、本変形例による駐車支援システム360は、360度の全周囲の状況が把握できるように、4枚の映像を合成して自車両の真上から見た画像(俯瞰映像)に変換する。これにより、周囲を映像から確認することができる。このとき、4台のカメラ映像を2次元平面上に画像変換・合成して俯瞰映像を作成しているため、自車両周辺にある物体(車両)は伸びる。その結果、周辺に存在する他車両の形状・位置関係が視覚的に分かりにくくなる。そこで、左右カメラによる上記距離測定方法によって物体特徴点までの三次元位置を算出し、その結果から物体表面形状を生成して合成画像に重畳して表示するようにしてもよい。

本変形例による駐車支援システムによれば、映像を確認することで周囲物との距離感および接触可能性が容易に把握でき、駐車作業が苦手なドライバーでも安心かつ簡単に駐車することができる。

なお、上記実施の形態および変形例において、距離測定装置1、駐車支援システム360は、距離測定装置の一例であり、カメラ21は、撮影装置の一例であり、車速センサ23、舵角センサ24は、移動量検出装置の一例である。

なお、本発明は、以上に述べた実施の形態および変形例に限定されるものではなく、本発明の要旨を逸脱しない範囲内で種々の構成または実施形態を採ることができる。例えば、上記実施の形態では、過去1フレーム前の登録済みリスト156のみを特徴点DB17に登録しているが、特徴点の照合精度を高めるために、過去複数フレーム分の特徴点に対応する登録済みリスト156を特徴点DB17に保持してもよい。

本実施の形態では、エッジリスト154の特徴点DB17への登録タイミングを特徴点照合処理の直後に行なっているが、特徴点照合処理の後であればどのタイミングでもよい。その他、各フローチャートにおいて説明した処理の順序は、これに限定するものではない。

さらに、物体の路面54への投影点を画像に再投影する場合には、再投影は現フレームと前フレームのどちらでもよい。

また、含有率は、上記実施の形態においては、特徴点の照合成否に関係なく特徴点数を算出しているが、移動物の判定精度を高めるために、前フレームの特徴点との照合に成功した特徴点のみを対象に含有率を算出してもよい。出力は、三次元位置を画像上に表す方法や、例えばカメラの位置からの距離を算出し、所定距離以下になったときに、警告を発するなどの方法でもよい。

上記実施の形態においては、エピポーラ拘束を受ける移動物の一例として、車両50に平行な方向に移動する移動物を例にして説明したが、上記実施の形態または変形例による距離測定方法では、誤って算出される他の三次元位置も除去することが可能である。

ここで、上記実施の形態および変形例による距離測定方法の動作をコンピュータに行わせるために共通に適用されるコンピュータの例について説明する。図36は、標準的なコンピュータのハードウエア構成の一例を示すブロック図である。図36に示すように、コンピュータ400は、Central Processing Unit(CPU)402、メモリ404、入力装置406、出力装置408、外部記憶装置412、媒体駆動装置414、ネットワーク接続装置等がバス410を介して接続されている。

CPU402は、コンピュータ400全体の動作を制御する演算処理装置である。メモリ404は、コンピュータ400の動作を制御するプログラムを予め記憶したり、プログラムを実行する際に必要に応じて作業領域として使用したりするための記憶部である。メモリ404は、例えばRandom Access Memory(RAM)、Read Only Memory(ROM)等である。入力装置406は、コンピュータの使用者により操作されると、その操作内容に対応付けられている使用者からの各種情報の入力を取得し、取得した入力情報をCPU402に送付する装置であり、例えばキーボード装置、マウス装置などである。出力装置408は、コンピュータ400による処理結果を出力する装置であり、表示装置などが含まれる。例えば表示装置は、CPU402により送付される表示データに応じてテキストや画像を表示する。

外部記憶装置412は、例えば、ハードディスクなどの記憶装置であり、CPU402により実行される各種制御プログラムや、取得したデータ等を記憶しておく装置である。媒体駆動装置414は、可搬記録媒体416に書き込みおよび読み出しを行うための装置である。CPU402は、可搬記録媒体416に記録されている所定の制御プログラムを、記録媒体駆動装置414を介して読み出して実行することによって、各種の制御処理を行うようにすることもできる。可搬記録媒体416は、例えばCompact Disc(CD)−ROM、Digital Versatile Disc(DVD)、Universal Serial Bus(USB)メモリ等である。ネットワーク接続装置418は、有線または無線により外部との間で行われる各種データの授受の管理を行うインタフェース装置である。バス410は、上記各装置等を互いに接続し、データのやり取りを行う通信経路である。

上記実施の形態および変形例による距離測定方法をコンピュータに実行させるプログラムは、例えば外部記憶装置412に記憶させる。CPU402は、外部記憶装置412からプログラムを読み出し、コンピュータ400に距離測定の動作を行なわせる。このとき、まず、距離測定の処理をCPU402に行わせるための制御プログラムを作成して外部記憶装置412に記憶させておく。そして、入力装置406から所定の指示をCPU402に与えて、この制御プログラムを外部記憶装置412から読み出させて実行させるようにする。また、このプログラムは、可搬記録媒体416に記憶するようにしてもよい。

以上の実施形態に関し、さらに以下の付記を開示する。 (付記1) 車両に設けられた距離測定装置であって、 前記車両に設けられた撮影装置により前記車両の位置が第1の位置であるときに撮影された第1の画像、および前記第1の位置とは異なる第2の位置であるときに撮影された第2の画像を受付けるとともに、前記車両に設けられた移動量検出装置から、前記第1の位置と前記第2の位置との間の前記車両の移動量を受付け、前記第1の画像、前記第2の画像、および前記移動量に基づき、物体の三次元位置を算出する三次元位置算出部と、 算出された前記物体の三次元位置に基づき設定された第3の位置を、前記第1の画像および前記第2の画像のうち一方の画像に投影した第4の位置と、前記一方の画像における前記物体の位置とのずれが所定値以上の場合に、算出された三次元位置が誤りであると判別する判別部と、 算出された前記三次元位置が、誤りであると判別されなかった場合には距離測定対象とし、誤りであると判別された場合に距離測定対象から除外して前記距離測定対象の前記三次元位置を出力する出力部と、 を有することを特徴とする距離測定装置。 (付記2) 前記第1の画像と前記第2の画像とのそれぞれの特徴点を抽出する特徴点抽出部と、 抽出された特徴点から前記第1の画像と前記第2の画像とで共通に表示されていると判断される特徴点同士を照合する特徴点照合部と、 前記三次元位置に基づき前記物体を検出する物体検出部と、 をさらに有し、 前記三次元位置算出部は、照合された前記特徴点および前記移動量に基づき、照合された前記特徴点に対応する三次元位置を算出し、 前記物体検出部は、前記三次元位置が互いに所定距離以内の特徴点を同一の物体に対応する特徴点とすることにより物体を検出し、 前記判別部は、前記同一の物体に対応付けられた特徴点の三次元位置を、前記車両が移動する路面に対応する平面上の三次元位置に投影し、投影された前記平面上の三次元位置に対し前記車両の上下方向にそれぞれ所定距離離れた2つの前記第3の位置を、前記一方の画像に投影し、2つの前記第3の位置が投影された2つの前記第4の位置と、前記一方の画像上の前記物体の位置とのずれが所定値以上の場合に、前記三次元位置を誤りであると判別する ことを特徴とする付記1に記載の距離測定装置。 (付記3) 前記判別部は、投影された2つの前記第4の位置に基づき探索範囲を決定し、前記同一の物体に対応付けられた特徴点の前記一方の画像上の位置と前記探索範囲の位置との関係に基づき前記探索範囲に前記同一の物体の下端が存在しないと判定された場合に、算出された前記三次元位置を誤りであると判別する ことを特徴とする付記2に記載の距離測定装置。 (付記4) 前記判別部は、前記同一の物体に対応付けられた特徴点の数に対する前記探索範囲内において抽出された特徴点の数の割合が所定値以下である場合に、算出された前記三次元位置を誤りであると判別する ことを特徴とする付記3に記載の距離測定装置。 (付記5) 前記出力部は、前記車両の左右のいずれかに設けられた前記撮影装置を含む前記車両の前後左右に設けられた少なくとも4台の撮影装置により撮影された画像を受付け、受付けた前記画像を合成した画像と、前記距離測定対象の前記三次元位置とを同時に表示する情報として、前記車両に備えられた表示装置に出力する ことを特徴とする付記1から付記4のいずれかに記載の距離測定装置。 (付記6) 演算処理装置が、 車両に設けられた撮影装置により前記車両の位置が第1の位置であるときに撮影された第1の画像、および前記第1の位置とは異なる第2の位置であるときに撮影された第2の画像を受付けるとともに、前記車両に設けられた移動量検出装置から、前記第1の位置と前記第2の位置との間の前記車両の移動量を受付け、前記第1の画像、前記第2の画像、および前記移動量に基づき、物体の三次元位置を算出し、 算出された前記物体の三次元位置に基づき設定された第3の位置を、前記第1の画像および前記第2の画像のうち一方の画像に投影した第4の位置と、前記一方の画像における前記物体の位置とのずれが所定値以上の場合に、算出された三次元位置が誤りであると判別し、 算出された前記三次元位置が、誤りであると判別されなかった場合には距離測定対象とし、誤りであると判別された場合に距離測定対象から除外する、 ことを特徴とする距離測定方法。 (付記7) さらに、 前記第1の画像と前記第2の画像とのそれぞれの特徴点を抽出し、 抽出された特徴点から前記第1の画像と前記第2の画像とで共通に表示されていると判断される特徴点同士を照合し、 前記三次元位置に基づき前記物体を検出し、 前記三次元位置を算出する処理においては、照合された前記特徴点および前記移動量に基づき、照合された前記特徴点に対応する三次元位置を算出し、 前記三次元位置が互いに所定距離以内の特徴点を同一の物体に対応する特徴点とすることにより物体を検出し、 前記同一の物体に対応付けられた特徴点の三次元位置を、前記車両が移動する路面に対応する平面上の三次元位置に投影し、投影された前記平面上の三次元位置に対し前記車両の上下方向にそれぞれ所定距離離れた2つの前記第3の位置を、前記一方の画像に投影し、 2つの前記第3の位置が投影された2つの前記第4の位置と、前記一方の画像上の前記物体の位置とのずれが所定値以上の場合に、前記三次元位置を誤りであると判別する ことを特徴とする付記6に記載の距離測定方法。 (付記8) さらに、 前記投影された2つの前記第4の位置に基づき探索範囲を決定し、 前記同一の物体に対応付けられた特徴点の前記一方の画像上の位置と前記探索範囲の位置との関係に基づき前記探索範囲に前記同一の物体の下端が存在しないと判定された場合に、算出された前記三次元位置を誤りであると判別する ことを特徴とする付記7に記載の距離測定方法。 (付記9) さらに、 前記同一の物体に対応付けられた特徴点の数に対する前記探索範囲内において抽出された特徴点の数の割合が所定値以下である場合に、算出された前記三次元位置を誤りであると判別する ことを特徴とする付記8に記載の距離測定方法。 (付記10) さらに、前記車両の左右のいずれかに設けられた前記撮影装置を含む前記車両の前後左右に設けられた少なくとも4台の撮影装置により撮影された画像を受付け、受付けた前記画像を合成した画像と、前記距離測定対象とした前記三次元位置とを同時に表示する情報として、前記車両に備えられた表示装置に出力する ことを特徴とする付記6から付記9のいずれかに記載の距離測定方法。 (付記11) 車両に設けられた撮影装置により前記車両の位置が第1の位置であるときに撮影された第1の画像、および前記第1の位置とは異なる第2の位置であるときに撮影された第2の画像を受付けるとともに、前記車両に設けられた移動量検出装置から、前記第1の位置と前記第2の位置との間の前記車両の移動量を受付け、前記第1の画像、前記第2の画像、および前記移動量に基づき、物体の三次元位置を算出し、 算出された前記物体の三次元位置に基づき設定された第3の位置を、前記第1の画像および前記第2の画像のうち一方の画像に投影した第4の位置と、前記一方の画像における前記物体の位置とのずれが所定値以上の場合に、算出された三次元位置が誤りであると判別し、 算出された前記三次元位置が、誤りであると判別されなかった場合には距離測定対象とし、誤りであると判別された場合に距離測定対象から除外する、 処理をコンピュータに実行させるためのプログラム。 (付記12) さらに、 前記第1の画像と前記第2の画像とのそれぞれの特徴点を抽出し、 抽出された特徴点から前記第1の画像と前記第2の画像とで共通に表示されていると判断される特徴点同士を照合し、 前記三次元位置に基づき前記物体を検出し、 前記三次元位置を算出する処理においては、照合された前記特徴点および前記移動量に基づき、照合された前記特徴点に対応する三次元位置を算出し、 前記三次元位置が互いに所定距離以内の特徴点を同一の物体に対応する特徴点とすることにより物体を検出し、 前記同一の物体に対応付けられた特徴点の三次元位置を、前記車両が移動する路面に対応する平面上の三次元位置に投影し、投影された前記平面上の三次元位置に対し前記車両の上下方向にそれぞれ所定距離離れた2つの前記第3の位置を、前記一方の画像に投影し、 2つの前記第3の位置が投影された2つの前記第4の位置と、前記一方の画像上の前記物体の位置とのずれが所定値以上の場合に、前記三次元位置を誤りであると判別する ことを特徴とする付記11に記載のプログラム。 (付記13) さらに、 前記投影された2つの前記第4の位置に基づき探索範囲を決定し、 前記同一の物体に対応付けられた特徴点の前記一方の画像上の位置と前記探索範囲の位置との関係に基づき前記探索範囲に前記同一の物体の下端が存在しないと判定された場合に、算出された前記三次元位置を誤りであると判別する ことを特徴とする付記12に記載のプログラム。 (付記14) さらに、 前記同一の物体に対応付けられた特徴点の数に対する前記探索範囲内において抽出された特徴点の数の割合が所定値以下である場合に、算出された前記三次元位置を誤りであると判別する ことを特徴とする付記13に記載のプログラム。 (付記15) さらに、前記車両の左右のいずれかに設けられた前記撮影装置を含む前記車両の前後左右に設けられた少なくとも4台の撮影装置により撮影された画像を受付け、受付けた前記画像を合成した画像と、前記距離測定対象とした前記三次元位置とを同時に表示する情報として、前記車両に備えられた表示装置に出力する ことを特徴とする付記11から付記14のいずれかに記載のプログラム。

1 距離測定装置 3 特徴点抽出部 5 特徴点照合部 7 特徴点更新部 9 三次元位置算出部 11 物体検出部 13 移動物判定部 15 出力部 16 出力装置 17 特徴点DB 20 演算処理装置 21 カメラ 23 車速センサ 24 舵角センサ 25 記憶装置 27 ディスプレイ 28 スピーカ 50 車両 52 撮像面 54 路面 62 撮像面 64 撮像面 77 エピポーラ線 79 エピポーラ点 81 エピポーラ点