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

Description

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

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

特開２００７−２５６０２９号公報

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

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

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

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

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

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

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

一実施の形態による距離測定装置の機能的な構成の一例を示すブロック図である。 一実施の形態による距離測定装置のハードウエア構成の一例を示すブロック図である。 移動体として車両が路面上を走行している場合の座標系の関係を示す図である。 車両が移動中の撮影状況を概念的に示す図である。 カメラ位置と特徴点との位置関係の一例を示す図である。 エピポーラ拘束がない場合の測定例を概念的に示す図である。 対象物体の移動方向が車両の移動方向と平行で同方向の場合を概念的に示す図である。 対象物体の移動方向が車両の移動方向と平行で逆方向の場合を概念的に示す図である。 距離測定装置による静止物と移動物の距離測定を概念的に示す図である。 三次元位置算出例を撮影画像に再投影した場合を概念的に示す図である。 移動物の判定方法を概念的に説明する図である。 エッジリストの一例を示す図である。 登録済みリストの一例を示す図である。 物体リストの一例を示す図である。 接続先リストの一例を示す図である。 最終接続先リストの一例を示す図である。 個数リストの一例を示す図である。 探索リストの一例を示す図である。 含有リストの一例を示す図である。 動静リストを示す図である。 移動物判定の一例を説明する図である。 距離測定装置の主要な動作を示すフローチャートである。 特徴点抽出処理の詳細を示すフローチャートである。 特徴点照合処理の詳細を示すフローチャートである。 特徴点更新処理の詳細を示すフローチャートである。 三次元位置算出処理の詳細を示すフローチャートである。 物体検出処理を示すフローチャートである。 初期番号付与処理の詳細を示すフローチャートである。 初期番号付与処理の詳細を示すフローチャートである。 物体番号の決定処理の詳細を示すフローチャートである。 移動物判定処理の一例を示すフローチャートである。 撮影画像への再投影処理の詳細を示すフローチャートである。 探索範囲算出処理の詳細を示すフローチャートである。 移動物判定処理の詳細を説明するフローチャートである。 結果出力処理の詳細を示すフローチャートである。 変形例による駐車支援システムの構成例を示す図である。 標準的なコンピュータのハードウエア構成の一例を示すブロック図である。

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

図１は、一実施の形態による距離測定装置１の機能的な構成の一例を示すブロック図である。図２は、距離測定装置１のハードウエア構成の一例を示すブロック図である。図１に示すように、距離測定装置１は、特徴点抽出部３、特徴点照合部５、特徴点更新部７、三次元位置算出部９、物体検出部１１、移動物判定部１３、出力部１５、特徴点ＤａｔａＢａｓｅ（ＤＢ）１７の機能を備えている。

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

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

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

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

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

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

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

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

一方、車両５０に備えられたカメラ２１のカメラ位置（例えば、カメラ２１の対物レンズの中心点：以下、単にカメラ位置という。）を原点ｏとし、光軸をｚ軸とする直交座標系ｏ−ｘｙｚをカメラ座標系という。このとき、カメラ座標系において、ｚ軸に垂直で、カメラ位置から矢印５８のようにカメラの焦点距離ｆ離れた面を、撮像面５２とする。矢印５６は、移動体座標系とカメラ座標系との変換を示しているが、詳細は後述する。なお、上記各座標系の定義は一例であり、これに限られない。

図４は、車両５０が移動中の撮影状況を概念的に示す図である。図４に示すように、時刻ｔ１におけるカメラ位置を原点ｏ１とする第１のカメラ座標系は、座標系ｏ１−ｘｙｚであり、時刻ｔ２におけるカメラ位置を原点ｏ２とする第２のカメラ座標系は、座標系ｏ２−ｘｙｚであるとする。矢印６６は、時刻ｔ１と時刻ｔ２との間の車両５０の移動、および第１のカメラ座標系と第２のカメラ座標系との変換を示しているが、詳細は後述する。また、撮像面６２は、時刻ｔ１における撮像面であり、撮像面６４は、時刻ｔ２における撮像面である。

図４に示すように、時刻ｔ１においては、第１のカメラ座標系において、特徴点Ｐ１（ｐｘ１、ｐｙ１、ｆ）が抽出されている。また、時刻ｔ２においては、第２のカメラ座標系において、特徴点Ｐ２（ｐｘ２、ｐｙ２、ｆ）が抽出されている。図４は、これらの特徴点は、後述する照合処理により同一の物体の点Ｐ（Ｘ，Ｙ，Ｚ）（移動体座標系）を撮影したものであることを概念的に示している。

図５は、カメラ位置と特徴点との位置関係の一例を示す図である。図５に示すように、移動体座標系における時刻ｔ１のカメラ２１の三次元位置と時刻ｔ２のカメラ２１の三次元位置とが分かっており、測定対象の物体（以下、対象物体という）が静止物であるとする。この場合、時刻ｔ２での物体特徴点の撮像面６４における位置は、エピポーラ線７７上に存在する。エピポーラ線７７とは、時刻ｔ１、時刻ｔ２のカメラ位置（原点ｏ１，ｏ２）と、原点ｏ１と特徴点Ｐ１とを結ぶ直線７３とを含む平面（エピポーラ面）と、時刻ｔ２の撮像面６４との交線である。ここでは、エピポーラ面上のエピポーラ点７９、エピポーラ点８１が示されている。

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

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

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

図７は、対象物体の移動方向が車両５０の移動方向と平行で同方向の場合を概念的に示す図である。図７に示すように、カメラ位置９０が車両移動方向９２の方向に移動し、物体位置９４が変わらない場合には、対象物体は、直線９８で示した方向に観察される。一方、対象物体が移動方向９６に移動している場合は、対象物体は、直線１００により示した観察方向に観察される。よって、対象物体の位置は算出位置１０２と算出され、矢印１０４に示すように、実際の位置より水平方向に遠方にあると算出される。同様に、矢印１０６に示すように、算出位置１０２は、上下方向には下側の算出位置１０２にあると算出される。

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

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

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

図１０は、算出例１３４を撮影画像に再投影した場合を概念的に示す図である。図１０に示すように、算出例１３４を例えば、時刻ｔ２において撮影された画像１４０に再投影すると、静止物１２０については、路面位置１２２は、再投影位置１２３に投影され、静止物１２０の下端位置と一致する。しかし、移動物１０９については、路面位置１２４は再投影位置１２５に投影され、実際の移動物１０９とは一致しない。

図１１は、移動物の判定方法を概念的に説明する図である。図１１に示すように、画像１４０において、再投影位置１２３は、静止物１２０の三次元位置に対応して算出された位置であり、探索範囲１２７は、再投影位置１２３に基づき算出される範囲である。同様に、移動物１０９では、再投影位置１２５、および探索範囲１２８が示されている。探索範囲１２７、１２８の算出例については後述する。

静止物１２０では、画像１４０上の静止物１２０の下端は、再投影位置１２３に基づき算出された探索範囲１２７内に存在している。一方、移動物１０９では、画像１４０上の移動物１０９の下端１２６は、算出された探索範囲１２８を外れている。

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

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

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

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

図１２に示すように、エッジリスト１５４は、ある時刻に取得された画像において抽出された特徴点ｅ（ｓ１）に関する情報である。ここで、変数ｓ１は１〜Ｎｅの整数である。要素数Ｎｅは、取得された画像において抽出された特徴点数を示す整数であり、特徴点の要素数ともいう。エッジリスト１５４は、現在特徴点位置、過去特徴点位置、パターン情報、照合処理フラグを有している。現在特徴点位置は、例えば、時刻ｔ１に撮影された画像から抽出された特徴点の位置であり、画像上に定義された座標（ｎｘ、ｎｙ）で表される。過去特徴点位置は、後述する照合処理で照合された場合に記憶される、例えば、一つ前に撮影された画像における対応する特徴点の座標（ｒｘ、ｒｙ）である。パターン情報ｖｎ（ｂ・ａ）は、特徴点（ｎｘ、ｎｙ）を中心としたｙ軸方向にｂ画素分、ｘ軸方向にａ画素分の画素値を表す。画素値は、例えば輝度、エッジ強度等である。照合処理フラグは、後述する特徴点照合部５により記憶される情報であり、対応する特徴点が抽出されたか否かを示す。

なお、図１２のエッジリスト１５４では、一つの特徴点ｅ（ｓ１）に対応する情報のみを示しているが、複数の特徴点のおのおのに応じて、同様の情報を有することができる。また、エッジリスト１５４は、特徴点抽出処理の前に初期化（要素数Ｎｅ＝０）する。

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

例えば、エッジリスト１５４が時刻ｔ２に撮影された画像に関する情報であるとすると、登録済みリスト１５６は、時刻ｔ２より前の時刻ｔ１に撮影された画像において特徴点抽出部３により抽出された特徴点ｒ（ｓ２）に関する情報とすることができる。ここで、変数ｓ２は、１〜Ｎｒの整数である。要素数Ｎｒは、対象となる画像、例えば前フレームにおいて抽出された特徴点の数である。

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

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

特徴点照合部５は、特徴点抽出部３で抽出した現フレームの特徴点群と、特徴点ＤＢ１７に記録された例えば前フレームの特徴点群について照合を行う。照合の際には、特徴点照合部５は、例えば、特徴点を中心として一定の大きさの領域（例えば、特徴点を含むａ×ｂ個の画素）を設定する。特徴点照合部５は、特徴点同士の上記一定の大きさの領域内の画素値に基づく一般的な映像間の照合処理を適用できる。一般的な照合処理とは、例えば、一定の大きさの領域の輝度値の差の絶対値和（Ｓｕｍ ｏｆ Ａｂｓｏｌｕｔｅｄ Ｄｉｆｆｅｒｅｎｃｅ：ＳＡＤ）や、輝度値の差の自乗和（Ｓｕｍ ｏｆ Ｓｑｕａｒｅｄ Ｄｉｆｆｅｒｅｎｃｅ：ＳＳＤ）等を用いた処理である。また、照合条件に特徴点間の距離が閾値以内であることを加えてもよい。

特徴点照合部５が、エッジリスト１５４と登録済みリスト１５６との間で照合に成功した場合には、登録済みリスト１５６の要素ｒ（ｓ２）の特徴点位置（ｒｘ，ｒｙ）をエッジリストの要素e（ｓ１）の特徴点位置（ｒｘ，ｒｙ）に登録する。また、特徴点照合部５は、要素e（ｓ１）のフラグ（ｆｌａｇ）を例えば「１」にする。

次に特徴点更新部７についてさらに説明する。特徴点更新部７は、登録済みリスト１５６を初期化し、特徴点抽出部３で抽出した特徴点群のエッジリスト１５４の情報を、登録済みリスト１５６に登録することにより、登録済みリスト１５６を更新する。特徴点更新部７は、更新された登録済みリスト１５６を、特徴点ＤＢ１７に記録する。

次に、三次元位置算出部９についてさらに説明する。三次元位置算出部９は、特徴点照合部５で照合に成功した２時刻の特徴点位置と車両５０の挙動（カメラ２１の移動量）から、特徴点に対応する三次元位置を算出する。車両５０の挙動は車速センサ２３、舵角センサ２４等から取得する。以下に、三次元位置の算出方法について述べる。

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

回転行列Ｒｍ、並進ベクトルＴｍで表される車両５０の移動量（Ｒｍ、Ｔｍ）は、移動体座標系で記述されたものである。また、時刻ｔ１での移動体座標系で表記した三次元位置をベクトルＸｗ１、時刻ｔ２での移動体の座標系で表記した三次元位置をベクトルＸｗ２とし、同じ点を、カメラ座標系で表記した三次元位置をベクトルｘｈ１、ｘｈ２とする。ここで、時刻ｔ１から時刻ｔ２での移動体の移動量を回転行列Ｒｍ、並進ベクトルＴｍで表すとする。このとき、移動体の座標系Ｘｗ（Ｘ，Ｙ，Ｚ）からカメラ座標系ｘｈ（ｘ，ｙ，ｚ）への変換は、回転行列Ｒｗ、並進ベクトルＴｗを用いて、以下のように表されるものとする。なお、「＊」は乗算を表す。
ｘｈ＝Ｒｗ＊（Ｘｗ−Ｔｗ）・・・（式１）
よって、時刻ｔ１では、
ｘｈ１＝Ｒｗ＊（Ｘｗ１−Ｔｗ）・・・（式２）
時刻ｔ２では、
ｘｈ２＝Ｒｗ＊（Ｘｗ２−Ｔｗ）・・・（式３）
となる。ここで、時刻ｔ１と時刻ｔ２との間でのベクトルＸｗ１とＸｗ２の関係は、
Ｘｗ２＝Ｒｍ＊Ｘｗ１＋Ｔｍ・・・（式４）
と表されるとすると、式３より以下のように表される。
Ｒｗ^Ｔ＊ｘｈ２＋Ｔｗ＝Ｒｍ＊（Ｒｗ^Ｔ＊ｘｈ１＋Ｔｗ）＋Ｔｍ
＝Ｒｍ＊Ｒｗ^Ｔ＊ｘｈ１＋Ｒｍ＊Ｔｗ＋Ｔｍ・・・（式５）

式５を変形すると、
ｘｈ２＝Ｒｗ＊Ｒｍ＊Ｒｗ^Ｔ＊ｘｈ１＋Ｒｗ＊（Ｒｍ＊Ｔｗ＋Ｔｍ−Ｔｗ）
＝（Ｒｗ＊Ｒｍ＊Ｒｗ^Ｔ）＊ｘｈ１＋Ｒｗ＊（（Ｒｍ−Ｉ）＊Ｔｗ＋Ｔｍ）
・・・（式６）
となる。式６において、以下のように置き換える。
Ｒｖ＝Ｒｗ＊Ｒｍ＊Ｒｗ^Ｔ
Ｔｖ＝Ｒｗ＊（（Ｒｍ−Ｉ）＊Ｔｗ＋Ｔｍ）・・・（式７）

よって、
ｘｈ２＝Ｒｖ＊ｘｈ１＋Ｔｖ・・・（式８）
と表すことができる。これは、カメラ座標系で考えた場合の、時刻ｔ１から時刻ｔ２で移動体が回転行列Ｒｍ、並進ベクトルＴｍの移動をする場合の座標間の関係を表している。以降は、このカメラ座標系について考える。

図４で説明したように、カメラの焦点距離は焦点距離ｆとする。時刻ｔ１での特徴点Ｐ１の特徴点位置（ｐｘ１，ｐｙ１）、時刻ｔ２での特徴点Ｐ２の特徴点位置（ｐｘ２，ｐｙ２）はそれぞれ撮像面上での位置を表している。これらの特徴点位置は、ｘｈ座標系では、それぞれ、点（ｐｘ１，ｐｙ１，ｆ）、点（ｐｘ２，ｐｙ２，ｆ）の位置を表している。ここで、
ｎ１＝（ｐｘ１，ｐｙ１，ｆ）
ｎ２＝（ｐｘ２，ｐｙ２，ｆ）・・・（式９）
とする。

このとき、図４に示すように、時刻ｔ１での特徴点位置ｎ１の延長線と、時刻ｔ２での特徴点位置ｎ２の延長線の交わる位置Ｐが、実際の物体の三次元位置となる。時刻ｔ１でのカメラ座標系での三次元位置を点Ｐ１とし、点Ｐ１までの距離を距離ｌ１とする。また、時刻ｔ２でのカメラ座標系での三次元位置を点Ｐ２とし、点Ｐ２までの距離を距離ｌ２とする。以下、説明の都合上、点Ｐ１、Ｐ２を示すベクトルをそれぞれベクトルＰ１、Ｐ２とし、ベクトルｎ１、ｎ２を式９の成分を持つ単位ベクトルとすると、下記の式１０が成り立つ。
Ｐ１＝ｌ１＊ｎ１
Ｐ２＝ｌ２＊ｎ２・・・（式１０）

ここで、ベクトルＰ１とベクトルＰ２には、式８より、行列Ｒｖ、ベクトルＴｖを用いて、
Ｐ２＝Ｒｖ＊Ｐ１＋Ｔｖ・・・（式１１）
の関係があるので、式１０、式１１より、
ｌ２＊ｎ２＝Ｒｖ＊ｌ１＊ｎ１＋Ｔｖ・・・（式１２）
となる。ここで、Ｎ１＝Ｒｖ＊ｎ１と置くと、
ｌ２＊ｎ２＝ｌ１＊Ｎ１＋Ｔｖ・・・（式１３）
となる。ここで、表記を以下のように改めるものとする。
Ｎ１＝（ＮＸ１，ＮＹ１，ＮＺ１）
ｎ２＝（ｎｘ２，ｎｙ２，ｎｚ２）・・・（式１４）
また、ベクトルＴｖを成分表示として、以下のように定義する。
Ｔｖ＝（Ｔｖｘ，Ｔｖｙ，Ｔｖｚ）・・・（式１５）

よって、式１３から式１５により、以下のように成分表示される。
ｌ２＊ｎｘ２＝ｌ１＊ＮＸ１＋Ｔｖｘ
ｌ２＊ｎｙ２＝ｌ１＊ＮＹ１＋Ｔｖｙ
ｌ２＊ｎｚ２＝ｌ１＊ＮＺ１＋Ｔｖｚ・・・（式１６）

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

となる。ここで式１７において、式１８のように表す。

よって、下記の式１９が成り立つ。
Ｎ＊Ｌ＝Ｔ・・・（式１９）
したがって、最小二乗での解は、下記の式２０で与えられる。
Ｌ＝（Ｎ^Ｔ＊Ｎ）^−１＊Ｎ^Ｔ＊Ｔ・・・（式２０）

式２０により、距離ｌ１、ｌ２が求まるので、時刻ｔ２でのカメラ座標系で表した三次元位置を示すベクトルＰ２は、下記の式２１で表される。
Ｐ２＝ｎ２＊ｌ２・・・（式２１）
カメラ座標系ｘｈと移動体の座標系Ｘｗの関係は、式１に示したように、ｘｈ＝Ｒｗ＊（Ｘｗ−Ｔｗ）なので、下記の式２２となる。
Ｘｗ＝Ｒｗ^Ｔ＊ｘｈ＋Ｔｗ・・・（式２２）
よって、カメラ座標系での三次元位置である点Ｐ２を移動体の座標系で表したものをＰ（Ｘ，Ｙ，Ｚ）とすると、下記式２３を満たす位置Ｐが三次元位置を示す最終的な解となる。
Ｐ＝Ｒｗ^Ｔ＊Ｐ２＋Ｔｗ・・・（式２３）

このとき、三次元位置算出部９は、エッジリスト１５４において、照合処理フラグを参照し、フラグが照合に成功していることを示す場合に、上記のように三次元位置Ｐ（Ｘ、Ｙ、Ｚ）を算出する。

図１４は、物体リスト１５８の一例を示す図である。物体リスト１５８は、特徴点照合部５により照合された特徴点ｏ（ｉ）に関する情報である。ここで、変数ｉは、１からＮｏまでの整数であり、要素数Ｎｏは、特徴点照合部５で照合に成功した特徴点の組数である。物体リスト１５８は、三次元位置、物体番号、投影画像下位置、投影画像上位置を有している。三次元位置算出部９は、算出された三次元位置を物体リスト１５８に登録する。

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

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

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

図１５は、接続先リストの一例を示す図、図１６は、最終接続先リストの一例を示す図である。図１５に示すように、接続先リスト１６０は、接続先を示す接続先ｊｕｍｐ（ｉ）を有している。上記のように、接続先リスト１６０は、先に振られた物体番号と同一の物体と判別される特徴点が抽出された場合に、先に振られた物体番号と特徴点とを関連付けるためのリストである。変数ｉは、１からＮｏまでの整数であり、要素数は、付与された初期物体番号数Ｎｏである。

図１６に示すように、最終接続先リスト１６２は、整理された最終的な物体番号を示す接続先ｊｕｍｐ２（ｉ）を有している。接続先ｊｕｍｐ２（ｉ）の要素数は、最終的な物体番号数である。

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

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

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

図１７は、個数リスト１６４の一例を示す図である。個数リスト１６４は、移動物判定部１３が、物体下端特徴点が探索範囲内にあるか否かを判別する際に用いる情報である。個数リスト１６４は、物体番号ｎｏを用いて要素ｎ（ｎｏ）と表す。個数リスト１６４は、同一の物体と判別された特徴点の数ｎｕｍである。個数リスト１６４の要素数は、最終的な物体番号数である。

図１８は、探索リスト１６６の一例を示す図である。探索リスト１６６は、一つの物体毎の探索範囲を示す情報である。探索リスト１６６は、探索範囲におけるｘ方向の最小値、最大値、ｙ方向の最小値、最大値を有している。探索リスト１６６の内容は、物体番号ｎｏを用いて要素ｓ（ｎｏ）で表される。探索リスト１６６の要素数は、最終的な物体番号数である。

図１９は、含有リスト１６８の一例を示す図である。含有リスト１６８は、物体毎の探索範囲に含まれる特徴点の数ｃｎｔの情報である。特徴点の数は、物体番号ｎｏを用いてｃｎｔ（ｎｏ）と表される。含有リスト１６８の要素数は、最終的な物体番号数となる。

図２０は、動静リスト１７０を示す図である。動静リスト１７０は、算出された三次元位置が移動体のものであるか否かを示すフラグｍｏｖｅの情報である。動静リスト１７０の内容は、物体番号ｎｏを用いて要素ｍ（ｎｏ）で表される。動静リスト１７０の要素数は、最終的な物体番号数となる。

図２１は、移動物判定の一例を説明する図である。図２１に示すように、本実施の形態においては、車両５０の、路面５４上の点を原点Ｏとする移動体座標系と、撮像面１７８を撮影するカメラ位置を原点とするカメラ座標系が定義される。車両５０の周囲に移動物１０９が存在し、移動物１０９の下端位置１７４が特徴点として抽出されているとする。また、算出された三次元位置Ｐ（Ｘ，Ｙ，Ｚ）を路面５４に投影した点を、点Ｐｈ０（Ｘ，Ｙ，ｈ０）、点Ｐｈ０からある高さの点を、点Ｐｈ１（Ｘ，Ｙ，ｈ１）とする。点Ｐｈ０，Ｐｈ１を撮像面１７８に投影した点をそれぞれ投影点ｐｈ０、ｐｈ１とする。なお、点ｐは、算出された三次元位置Ｐを撮像面１７８に再投影した点である。移動物判定部１３は、移動物１０９毎の特徴点に対する投影点に基づき探索範囲１８０を設定し、探索範囲内に、撮像面１７８における移動物１０９の下端が存在しているか否かを判別する。

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

特徴点照合部５は、特徴点抽出部３で抽出された現フレームの特徴点と、前フレームで抽出された特徴点とを照合し、共通に表示されていると判断される点を抽出する（Ｓ２１３）。特徴点更新部７は、登録済みリスト１５６を、エッジリスト１５４の内容に更新し、特徴点ＤＢ１７に記憶させる（Ｓ２１４）。

三次元位置算出部９は、例えば、車速センサ２３、舵角センサ２４から、車両情報を取得する（Ｓ２１５）。車両情報とは、車両５０の位置を示す情報である。三次元位置算出部９は、取得した車両情報およびエッジリスト１５４に基づき、三次元位置を算出し、物体リスト１５８を生成する（Ｓ２１６）。

物体検出部１１は、物体リスト１５８に基づき、物体を検出する（Ｓ２１７）。移動物判定部１３は、生成された物体リスト１５８から、物体毎の探索範囲を検出し、検出された物体の下端位置が探索範囲に含まれるか否かに基づき、物体が移動物であるか否かを判定する（Ｓ２１８）。出力部１５は、移動物であると判定された三次元位置を結果から削除して算出された三次元位置を出力する（Ｓ２１９）。

以下、上記処理の詳細についてさらに説明する。図２３は、特徴点抽出処理の詳細を示すフローチャートである。特徴点抽出処理は、図２２のＳ２１２の処理である。図２３に示すように、特徴点抽出部３は、取得した画像において、エッジ処理を行う（Ｓ２３１）。エッジ強度が閾値以上と判定された場合は（Ｓ２３２：ＹＥＳ）、エッジリスト１５４に、特徴点ｅ（ｓ１）に関する位置（ｎｘ，ｎｙ）、パターン情報ｖｎ（ｂ・ａ）、照合処理フラグｆｌａｇ＝０、要素数Ｎｅ＝Ｎｅ＋１を登録する（Ｓ２３３）。Ｓ２３２で、閾値未満の場合には、特徴点抽出部３は、Ｓ２３４に処理を進める（Ｓ２３２：ＮＯ）。画像における全画素について処理が終了するまでＳ２３１からＳ２３３の処理を繰り返し（Ｓ２３４：ＮＯ）、終了すると（Ｓ２３４：ＹＥＳ）、特徴点抽出部３は、特徴点抽出処理を終了する。

図２４は、特徴点照合処理の詳細を示すフローチャートである。特徴点照合処理は、図２２のＳ２１３の処理である。図２４に示すように、特徴点照合部５は、登録済みリスト１５６に、未処理の要素があるか否か判別する（Ｓ２４１）。未処理の要素がある場合には、特徴点照合部５は、特徴点ＤＢ１７に記憶されている登録済みリスト１５６から、未処理の要素ｒ（ｓ２）を抽出する（Ｓ２４２）。特徴点照合部５は、エッジリスト１５４に未処理の要素があるか否かを判別し（Ｓ２４３）、ある場合には（Ｓ２４３：ＹＥＳ）、未処理の要素ｅ（ｓ１）をエッジリスト１５４から取り出す（Ｓ２４４）。

特徴点照合部５は、取り出した要素ｒ（ｓ２）と要素ｅ（ｓ１）とを照合する（Ｓ２４５）。すなわち、上述のように、パラメータ情報に基づき、共通に表示された点であるか否かを判別する。照合が成功し、共通に表示された点であると判断されると（Ｓ２４６：ＹＥＳ）、特徴点照合部５は、エッジリスト１５４の要素ｅ（ｓ１）の過去特徴点位置に、要素ｒ（ｓ２）の特徴点位置（ｒｘ、ｒｙ）を登録する（Ｓ２４７）。また、特徴点照合部５は、要素ｅ（ｓ１）の照合フラグｆｌａｇ＝１とし（Ｓ２４８）、処理をＳ２４１に戻す。特徴点照合部５は、上記の処理を登録済みリスト１５６に、未処理の要素ｒ（ｓ２）がなくなるまで実行し、なくなると（Ｓ２４１：ＮＯ）、特徴点照合処理を終了する。

図２５は、特徴点更新処理の詳細を示すフローチャートである。特徴点更新処理は、図２２のＳ２１４の処理である。図２５に示すように、特徴点更新部７は、登録済みリスト１５６を初期化する（Ｓ２６１）。特徴点更新部７は、エッジリスト１５４に、未処理の要素ｅ（ｓ１）があるか否か判別し（Ｓ２６２）、ある場合には（Ｓ２６２：ＹＥＳ）、未処理の要素ｅ（ｓ１）を取り出す（Ｓ２６３）。特徴点更新部７は、取り出した要素ｅ（ｓ１）について、現在特徴点位置（ｎｘ，ｎｙ）とパターン情報ｖｎ（ｂ・ａ）を登録済みリスト１５６の要素ｒ（ｓ２）の特徴点位置（ｒｘ，ｒｙ）とパターン情報ｖｒ（ｂ・ａ）に登録する（Ｓ２６４）。特徴点更新部７は、処理をＳ２６１に戻し、Ｓ２６２からＳ２６４の処理を、エッジリスト１５４の各要素ｅ（ｓ１）について行う。未処理の要素がなくなると（Ｓ２６２：ＮＯ）、特徴点更新部７は、特徴点更新処理を終了する。

図２６は、三次元位置算出処理の詳細を示すフローチャートである。三次元位置算出処理は、図２２のＳ２１６の処理である。図２６に示すように、三次元位置算出部９は、エッジリスト１５４に未処理の要素ｅ（ｓ１）があるか否かを判別し（Ｓ２７１）、ある場合には（Ｓ２７１：ＹＥＳ）、未処理の要素ｅ（ｓ１）を取り出す（Ｓ２７２）。三次元位置算出部９は、取り出した要素ｅ（ｓ１）の照合処理フラグを確認し、照合が成功であることを示していれば（Ｓ２７３：ＹＥＳ）、エッジリスト１５４の要素ｅ（ｓ１）の情報に基づき、上述のように三次元位置を算出する（Ｓ２７４）。

このとき三次元位置算出部９は、特徴点照合部５で照合に成功した２時刻の特徴点位置と車両挙動（カメラの移動量）から三次元位置を算出する。車両挙動は、車速センサ２３、舵角センサ２４などセンサ類から取得する。

三次元位置算出部９は、算出された三次元位置（Ｘ，Ｙ，Ｚ）を物体リスト１５８に登録し、照合が成功した要素ｅ（ｓ１）の要素数Ｎｏ＝Ｎｏ＋１と更新する（Ｓ２７５）。Ｓ２７１に処理を戻し、三次元位置算出部９は、未処理の要素ｅ（ｓ１）がない場合には（Ｓ２７１：ＮＯ）、処理を終了する。

図２７は、物体検出処理を示すフローチャートである。物体検出処理は、図２２のＳ２１７の処理である。図２７に示すように、物体検出部１１は、物体リスト１５８において、物体特徴点に初期番号を付与する（Ｓ２８１）。物体検出部１１は、物体番号の決定処理を行い（Ｓ２８２）、物体検出処理を終了する。

続いて、物体検出処理の詳細について、さらに説明する。図２８Ａ、２８Ｂは、初期番号付与処理の詳細を示すフローチャートである。初期番号付与処理は、図２７のＳ２８１の処理である。図２８Ａに示すように、物体検出部１１は、物体リスト１５８のコピーであるコピーリストを作成する。また、物体検出部１１は、物体番号ｎｏ＝０、物体番号ｎｏ（ｏｋ）＝−１と初期化する。さらに、物体検出部１１は、後述する接続先ｊｕｍｐ（ｉ）＝ｉと初期化する（Ｓ２９０）。

ここで、変数ｉは、１からＮｏまでの整数である。要素数Ｎｏは、物体リスト１５８の要素数であり、照合が成功した要素ｅ（ｓ１）の数である。なお、物体番号ｎｏは、変数ｉによる区別をせずに用いる場合の物体リスト１５８における物体番号をいう。後述する物体番号ｎｏ（ｏｉ）、ｎｏ（ｏｋ）とは、それぞれ要素ｏ（ｉ）、ｏ（ｋ）に対応する物体番号ｎｏをいう。

物体検出部１１は、物体リスト１５８に未処理の要素ｏ（ｉ）があるか否かを判別し（Ｓ２９１）、ある場合には（Ｓ２９１：ＹＥＳ）、未処理の要素ｏ（ｉ）をとりだす（Ｓ２９２）。なお、物体番号の「−１」は、物体番号ｎｏが未付与であることを示す。

物体検出部１１は、コピーリストに未処理の要素ｏ（ｋ）（ｋは、１からＮｏの整数）があるか否か判別し（Ｓ２９３）、ある場合には（Ｓ２９３：ＹＥＳ）、コピーリストから未処理の要素ｏ（ｋ）を取り出し、物体番号ｎｏ（ｏｉ）＝−１と初期化する（Ｓ２９４）。物体検出部１１は、要素ｏ（ｉ）と要素ｏ（ｋ）との三次元位置同士の距離が近いか否かを判別する（Ｓ２９５）。距離が近い場合には（Ｓ２９５：ＹＥＳ）、物体検出部１１は、処理をＳ２９６に進める。距離が近くない場合には（Ｓ２９５：ＮＯ）、物体検出部１１は、処理をＳ２９３に戻す。

Ｓ２９３において、コピーリストに未処理の要素がない場合には（Ｓ２９３：ＮＯ）、物体検出部１１は、物体番号ｎｏ（ｏｉ）が未付与であるか否かを判別する（Ｓ３０５）。未付与である場合には（Ｓ３０５：ＹＥＳ）、物体検出部１１は、物体番号ｎｏ（ｏｉ）＝ｎｏ、と新規物体番号ｎｏを付与し、ｎｏ＝ｎｏ＋１とし、処理をＳ２９１に戻す。

Ｓ２９１で、物体リスト１５８に未処理の要素がない場合には（Ｓ２９１：ＮＯ）、全ての物体リスト１５８およびコピーリストの要素について処理を終了したことになり、物体検出部１１は、初期番号付与処理を終了する。

図２８Ｂに示すように、Ｓ２９６では、物体検出部１１は、物体リスト１５８の物体番号ｎｏ（ｏｉ）が未付与であるか、すなわち物体番号ｎｏ（ｏｉ）＝−１であるか否かを判別する。未付与である場合には（Ｓ２９６：ＹＥＳ）、物体検出部１１は、さらに、物体番号ｎｏ（ｏｋ）が未付与であるか、すなわち物体番号ｎｏ（ｏｋ）＝−１であるか否かを判別する（Ｓ２９７）。未付与である場合には（Ｓ２９７：ＹＥＳ）、物体検出部１１は、物体番号ｎｏ（ｏｉ）＝ｎｏ、物体番号ｎｏ（ｏｋ）＝ｎｏと新規物体番号ｎｏを付与するとともに、ｎｏ＝ｎｏ＋１とし（Ｓ２９８）、処理をＳ２９３に戻す。

Ｓ２９７において、物体番号ｎｏ（ｏｋ）が未付与でない場合には（Ｓ２９７：ＮＯ）、物体検出部１１は、物体番号ｎｏ（ｏｉ）＝物体番号ｎｏ（ｏｋ）とし（Ｓ２９９）、処理をＳ２９３に戻す。

Ｓ２９６において、物体番号ｎｏ（ｏｉ）が未付与でない場合には（Ｓ２９６：ＮＯ）、物体検出部１１は、さらに、物体番号ｎｏ（ｏｋ）が未付与であるか、すなわち物体番号ｎｏ（ｏｋ）＝−１であるか否かを判別する（Ｓ３００）。未付与である場合には（Ｓ３００：ＹＥＳ）、物体検出部１１は、物体番号ｎｏ（ｏｋ）＝物体番号ｎｏ（ｏｉ）とし（Ｓ３０１）、処理をＳ２９３に戻す。

Ｓ３００において、物体番号ｎｏ（ｏｋ）が未付与でない場合には（Ｓ３００：ＮＯ）、物体検出部１１は、物体番号ｎｏ（ｏｉ）＞物体番号ｎｏ（ｏｋ）であるか否かを判別する（Ｓ３０２）。物体番号ｎｏ（ｏｉ）＞物体番号ｎｏ（ｏｋ）である場合には（Ｓ３０２：ＹＥＳ）、物体検出部１１は、接続先ｊｕｍｐ（ｉ）＝物体番号ｎｏ（ｏｋ）とし（Ｓ３０３）、処理を２９３に戻す。物体番号ｎｏ（ｏｉ）＞物体番号ｎｏ（ｏｋ）でない場合には（Ｓ３０２：ＮＯ）、物体検出部１１は、接続先ｊｕｍｐ（ｋ）＝物体番号ｎｏ（ｏｉ）とし（Ｓ３０４）、処理をＳ２９３に戻す。ここで、接続先リスト１６０は、接続先としての接続先ｊｕｍｐ（ｉ）の情報の集合である。なお、接続先リスト１６０の要素数は、付与された初期物体番号数である。

図２９は、物体番号の決定処理の詳細を示すフローチャートである。この処理は、図２７のＳ２８２の処理の詳細であり、物体初期番号を整理して最終的な物体番号を付与する処理である。図２９に示すように、物体検出部１１は、接続先リスト１６０に、未処理の要素があるか否かを判別する（Ｓ３１１）。未処理の要素がある場合には（Ｓ３１１：ＹＥＳ）、物体検出部１１は、未処理の接続先ｊｕｍｐ（ｉ）を取り出し、探索用の番号ｊを、ｊ＝ｉと設定する（Ｓ３１２）。

物体検出部１１は、探索用の番号ｊと接続先ｊｕｍｐ（ｊ）が同じか否か、すなわち、接続先ｊｕｍｐ（ｊ）＝ｊか否かを判別する（Ｓ３１３）。同じでない場合には（Ｓ３１３：ＮＯ）、物体検出部１１は、ｊ＝接続先ｊｕｍｐ（ｊ）と接続先を再設定し（Ｓ３１４）、Ｓ３１３、Ｓ３１４の処理を、番号ｊ＝接続先ｊｕｍｐ（ｊ）となるまで繰り返す。

探索用の番号ｊ＝接続先ｊｕｍｐ（ｊ）であれば（Ｓ３１３：ＹＥＳ）、物体検出部１１は、最終接続先ｊｕｍｐ２（ｉ）＝ｊと決定し（Ｓ３１５）、最終接続先リスト１６２に登録するとともに、処理をＳ３１１に戻す。

Ｓ３１１で、接続先リスト１６０に未処理の要素がない場合には（Ｓ３１１：ＮＯ）、物体検出部１１は、物体リスト１５８に未処理の要素があるか否かを判別する（Ｓ３１６）。未処理の要素がある場合には（Ｓ３１６：ＹＥＳ）、物体検出部１１は、未処理の要素ｏ（ｉ）を取り出し、要素ｏ（ｉ）の物体番号ｎｏ（ｏｉ）＝接続先ｊｕｍｐ２（ｎｏ（ｏｉ））と更新する（Ｓ３１７）。最終接続先リスト１６２の要素数は、整理された最終物体番号数である。Ｓ３１６において、未処理の要素がない場合には（Ｓ３１６：ＮＯ）、処理を終了する。この処理により、物体リスト１５８の各要素ｏ（ｉ）について、最終的な物体番号が決定される。ここで決定された物体番号の数が、最終的な物体番号数となる。

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

図３０に示すように、移動物判定部１３は、算出された三次元位置に基づき、撮影画像への再投影処理を行う（Ｓ３２１）。移動物判定部１３は、探索範囲を算出する（Ｓ３２２）。さらに移動物判定部１３は、移動物判定処理を行う（Ｓ３２３）。

図３１は、撮影画像への再投影処理の詳細を示すフローチャートである。撮影画像への再投影処理は、図３０のＳ３２１の処理である。図３１に示すように、移動物判定部１３は、物体リスト１５８に未処理の要素があるか否かを判別する（Ｓ３３１）。未処理の要素がある場合には、移動物判定部１３は、未処理の要素ｏ（ｉ）を取り出し、物体番号ｎｏ（ｏｉ）を抽出し、個数リスト１６４の個数ｎｕｍを更新する（Ｓ３３２）。すなわち、移動物判定部１３は、ｎｏ＝物体番号ｎｏ（ｏｉ）、ｎｕｍ（ｎｏ）＝ｎｕｍ（ｎｏ）＋１とおく。

移動物判定部１３は、特徴点の三次元位置（Ｘ，Ｙ，Ｚ）に対して、路面に下ろした位置（Ｘ，Ｙ，ｈ０）および、一定高さの位置（Ｘ，Ｙ，ｈ１）を設定する（Ｓ３３３）。ここで、例えば、ｈ０は−１０ｃｍ、ｈ１は、５０ｃｍとしてもよい。

移動物判定部１３は、それぞれの位置を例えば図２１の撮像面１７８などの撮影画像に再投影する（Ｓ３３４）。次に、移動物判定部１３は、撮影画像に再投影した画像位置（ｘ０，ｙ０）、（ｘ１，ｙ１）を物体リスト１５８に登録する（Ｓ３３５）。Ｓ３３１で、物体リスト１５８に未処理の要素がなければ（Ｓ３３１：ＮＯ）、移動物判定部１３は、再投影処理を終了する。

なお、撮影画像への再投影は、移動体座標系Ｘｗからカメラ座標系ｘｈへの変換を示す回転行列Ｒｗ、並進ベクトルＴｗを用いて上述の式１（ｘｈ＝Ｒｗ・（Ｘｗ−Ｔｗ））を用いて行うことができる。

図３２は、探索範囲算出処理の詳細を示すフローチャートである。探索範囲算出処理は、図３０のＳ３２２の処理である。移動物判定部１３は、物体リスト１５８に、未処理の要素があるか否かを判別する（Ｓ３４１）。未処理の要素がある場合（Ｓ３４１：ＹＥＳ）、移動物判定部１３は、未処理の要素ｏ（ｉ）を取り出し、再投影した画像位置（ｘ０，ｙ０）、（ｘ１，ｙ１）を取り出す（Ｓ３４２）。ここで、ｘ０＝ｘ１、ｙ０＞ｙ１である。

移動物判定部１３は、ｘ０＜ｘ＿ｍｉｎ（ｎｏ）であるか否かを判別し（Ｓ３４３）、ＹＥＳの場合には、ｘ＿ｍｉｎ（ｎｏ）＝ｘ０と設定する（Ｓ３４４）。Ｓ３４３でＮＯの場合には、移動物判定部１３は、Ｓ３４５に処理を進める。

移動物判定部１３は、ｘ０＞ｘ＿ｍａｘ（ｎｏ）であるか否かを判別し（Ｓ３４５）、ＹＥＳの場合には、ｘ＿ｍａｘ（ｎｏ）＝ｘ０と設定する（Ｓ３４６）。Ｓ３４５でＮＯの場合には、移動物判定部１３は、Ｓ３４７に処理を進める。

移動物判定部１３は、ｙ０＞ｙ＿ｍａｘ（ｎｏ）であるか否かを判別し（Ｓ３４７）、ＹＥＳの場合には、ｙ＿ｍａｘ（ｎｏ）＝ｙ０と設定する（Ｓ３４８）。Ｓ３４７でＮＯの場合には、移動物判定部１３は、Ｓ３４９に処理を進める。

移動物判定部１３は、ｙ１＜ｙ＿ｍｉｎ（ｎｏ）であるか否かを判別し（Ｓ３４９）、ＹＥＳの場合には、ｙ＿ｍｉｎ（ｎｏ）＝ｙ１と設定する（Ｓ３５０）。Ｓ３４９でＮＯの場合には、移動物判定部１３は、Ｓ３４１に処理を戻す。

Ｓ３４１で、物体リスト１５８に未処理の要素がないと判別された場合には（Ｓ３４１：ＮＯ）、移動物判定部１３は、探索範囲算出処理を終了する。

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

図３３は、移動物判定処理の詳細を説明するフローチャートである。移動物判定処理は、図３０のＳ３２３の処理である。図３３に示すように、移動物判定部１３は、登録済みリスト１５６に未処理の要素があるか否かを判別し（Ｓ３６１）、ある場合には（Ｓ３６１：ＹＥＳ）、移動物判定部１３は、登録済みリスト１５６の要素ｒ（ｓ２）の画像位置（ｒｘ，ｒｙ）を取り出す（Ｓ３６２）。

移動物判定部１３は、物体検出部１１により検出された物体のうち、未処理の物体番号ｎｏがあるか否かを判別する（Ｓ３６３）。未処理の物体番号がない場合には（Ｓ３６３：ＮＯ）、移動物判定部１３は、処理をＳ３６１に戻す。未処理の物体番号がある場合には（Ｓ３６３：ＹＥＳ）、移動物判定部１３は、画像位置（ｒｘ，ｒｙ）が探索リスト１６６の探索範囲（ｘ＿ｍｉｎ，ｘ＿ｍａｘ，ｙ＿ｍｉｎ，ｙ＿ｍａｘ）に含まれるか判断する（Ｓ３６４）。含まれる場合は（Ｓ３６４：ＹＥＳ）、含有リスト１６８の含有数ｃｎｔを、ｃｎｔ＝ｃｎｔ＋１と更新し（Ｓ３６５）、処理をＳ３６３に戻し、未処理の物体番号がなくなるまでＳ３６３からＳ３６５の処理を繰り返す。

上記処理を登録済みリスト１５６の全要素ｒ（ｓ２）に対して行った後、移動物判定部１３は、物体番号の処理を、全て未処理に戻す（Ｓ３６６）。移動物判定部１３は、未処理の物体番号があるか否か判別する（Ｓ３６７）。ある場合には（Ｓ３６７：ＹＥＳ）、移動物判定部１３は、物体ｎｏ毎の含有率＝（ｃｎｔ（ｎｏ）/ｎｕｍ（ｎｏ））を算出する。さらに移動物判定部１３は、含有率が閾値ｔｈｒｅｓ未満であれば（Ｓ３６８：ＹＥＳ）、対象物体は移動物であるとして、動静リスト１７０の移動フラグｍｏｖｅ＝１とする（Ｓ３６９）。

移動物判定部１３は、含有率が閾値以上であれば（Ｓ３６８：ＮＯ）、対象物体は静止物として、動静リスト１７０の移動フラグｍｏｖｅ＝０とする（Ｓ３７０）。移動物判定用の閾値ｔｈｒｅｓは、例えば０．３とすることができる。移動物判定部１３は、未処理の物体番号がないと判別すると（Ｓ３６７：ＮＯ）、処理を終了する。

図３４は、結果出力処理の詳細を示すフローチャートである。結果出力処理は、図２２のＳ２１９の処理である。出力部１５は、移動物判定部１３により静止物と判断された特徴点のみを表示する。図３４に示すように、出力部１５は、物体リスト１５８に未処理の要素があるか否かを判別する（Ｓ３８１）。ある場合には（Ｓ３８１：ＹＥＳ）、出力部１５は、物体リスト１５８の要素ｏ（ｉ）の物体番号ｎｏ（ｏｉ）を取り出し、対応する動静リスト１７０の移動フラグｍｏｖｅが「０」であるか判断する（Ｓ３８３）。「０」である場合は（Ｓ３８３：ＹＥＳ）、対象物体は静止物であるとして三次元位置を表示する（Ｓ３８４）。一方、移動フラグｍｏｖｅが「１」である場合は（Ｓ３８３：ＮＯ）、対象物体は移動物であるので、出力部１５は、三次元位置を表示せず、処理をＳ３８１にもどす。以上の処理を物体リスト１５８の全要素ｏ（ｉ）に対して行なう。未処理の物体リスト１５８における物体がなくなると（Ｓ３８１）、出力部１５は、出力処理を終了する。

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

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

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

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

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

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

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

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

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

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

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

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

ここで、上記実施の形態および変形例による距離測定方法の動作をコンピュータに行わせるために共通に適用されるコンピュータの例について説明する。図３６は、標準的なコンピュータのハードウエア構成の一例を示すブロック図である。図３６に示すように、コンピュータ４００は、Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ（ＣＰＵ）４０２、メモリ４０４、入力装置４０６、出力装置４０８、外部記憶装置４１２、媒体駆動装置４１４、ネットワーク接続装置等がバス４１０を介して接続されている。

ＣＰＵ４０２は、コンピュータ４００全体の動作を制御する演算処理装置である。メモリ４０４は、コンピュータ４００の動作を制御するプログラムを予め記憶したり、プログラムを実行する際に必要に応じて作業領域として使用したりするための記憶部である。メモリ４０４は、例えばＲａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ（ＲＡＭ）、Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ（ＲＯＭ）等である。入力装置４０６は、コンピュータの使用者により操作されると、その操作内容に対応付けられている使用者からの各種情報の入力を取得し、取得した入力情報をＣＰＵ４０２に送付する装置であり、例えばキーボード装置、マウス装置などである。出力装置４０８は、コンピュータ４００による処理結果を出力する装置であり、表示装置などが含まれる。例えば表示装置は、ＣＰＵ４０２により送付される表示データに応じてテキストや画像を表示する。

外部記憶装置４１２は、例えば、ハードディスクなどの記憶装置であり、ＣＰＵ４０２により実行される各種制御プログラムや、取得したデータ等を記憶しておく装置である。媒体駆動装置４１４は、可搬記録媒体４１６に書き込みおよび読み出しを行うための装置である。ＣＰＵ４０２は、可搬記録媒体４１６に記録されている所定の制御プログラムを、記録媒体駆動装置４１４を介して読み出して実行することによって、各種の制御処理を行うようにすることもできる。可搬記録媒体４１６は、例えばＣｏｍｐａｃｔ Ｄｉｓｃ（ＣＤ）−ＲＯＭ、Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ（ＤＶＤ）、Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ（ＵＳＢ）メモリ等である。ネットワーク接続装置４１８は、有線または無線により外部との間で行われる各種データの授受の管理を行うインタフェース装置である。バス４１０は、上記各装置等を互いに接続し、データのやり取りを行う通信経路である。

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

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

１ 距離測定装置
３ 特徴点抽出部
５ 特徴点照合部
７ 特徴点更新部
９ 三次元位置算出部
１１ 物体検出部
１３ 移動物判定部
１５ 出力部
１６ 出力装置
１７ 特徴点ＤＢ
２０ 演算処理装置
２１ カメラ
２３ 車速センサ
２４ 舵角センサ
２５ 記憶装置
２７ ディスプレイ
２８ スピーカ
５０ 車両
５２ 撮像面
５４ 路面
６２ 撮像面
６４ 撮像面
７７ エピポーラ線
７９ エピポーラ点
８１ エピポーラ点

Claims (9)

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

Images