JP5472538B2 - 距離計測装置及び環境地図生成装置 - Google Patents

Description

本発明は、車両に搭載したカメラにより車両走行路周辺の周辺物を撮像し、撮像した画像を用いて周辺物までの距離を計測する距離計測装置、及び距離計測装置を用いて周辺物の三次元画像を取得して環境地図を生成する環境地図生成装置に関する。

従来より、単眼カメラにより特徴点を三次元化し、特徴点距離を修正する技術として、例えば、特開２００７−２６３６５７号公報（特許文献１）に開示された三次元座標取得装置が知られている。該特許文献１には、所定の距離で設置された２台のカメラを有し、２台のうちの１台のカメラを用いて周辺物標の三次元座標を取得する単眼ステレオ処理部と、２台のカメラの同時撮像画像を用いて周辺物標の三次元座標を取得する複眼ステレオ処理部を有し、それぞれの処理部で演算された三次元座標を、選択あるいは統合して周辺物標の三次元座標を算出する三次元座標取得装置が開示されている。

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

しかしながら、特許文献１の方法では、単眼ステレオ処理、及び複眼ステレオ処理の双方により同時に算出された特徴点のみが修正されることになり、環境地図全体を修正することはできない。

本発明は、このような従来の課題を解決するためになされたものであり、その目的とするところは、周辺物までの距離を高精度に計測する距離計測装置、及び全体的に高精度の環境地図のスケール精度を改善することが可能な環境地図生成装置を提供することにある。

上記目的を達成するため、本発明の実施形態に係わる距離計測装置或いは環境地図生成装置は、車両が移動している時に撮像された車両の周囲の画像から、車両の周囲に存在する複数の物体の特徴点を検出する特徴点検出部と、検出された特徴点の時間変化から、特徴点から撮像部までの第一距離を算出する第一距離算出部と、画像内に存在する複数の物体に含まれる特定物体の一部画素を用いて、特定物体から撮像部までの第二距離を算出する第二距離算出部と、複数の物体の特徴点の中で、特定物体の一部画素と略一致する特定特徴点を抽出して、特定特徴点における第一距離と第二距離との比率を算出し、特徴点検出部にて同時に検出される特定特徴点以外の複数の特徴点の第一距離を、比率に基づいて修正するスケール修正部とを備える。

図１は、検出対象となる周辺物標ＯＢ１、ＯＢ２に存在する特徴点ＦＰ、ＦＰ_Ｅとエッジ部ＥＤとの関係を示す模式図である。 図２は、本発明の第１実施形態に係る環境地図生成装置１００が車両内に搭載された様子を示す模式図である。 図３は、本発明の第１実施形態に係る環境地図生成装置１００の構成を示すブロック図である。 図４は、本発明の第１実施形態に係る環境地図生成装置１００の、環境地図生成処理の手順を示すフローチャートである。 図５は、本発明の第１実施形態に係る環境地図生成装置１００で、スケール修正を実行する際の説明図である。 図６は、本発明の第２実施形態に係る環境地図生成装置２００の構成を示すブロック図である。 図７は、本発明の第２実施形態に係る環境地図生成装置２００の、特徴点検出処理を概略的に示す説明図である。 図８は、本発明の第３実施形態に係る環境地図生成装置３００の構成を示すブロック図である。 図９は、第４実施形態に係る環境地図生成装置４００の構成を示すブロック図である。 図１０は、第４実施形態に係る環境地図生成装置４００の、環境地図生成処理の手順を示すフローチャートである。 図１１は、第５実施形態に係る環境地図生成装置５００の構成を示すブロック図である。 図１２は、第６実施形態に係る環境地図生成装置６００の構成を示すブロック図である。

以下、本発明の実施形態を図面に基づいて説明する。

［第１実施形態の説明］
図１は、検出対象となる周辺物標ＯＢ１、ＯＢ２に存在する特徴点ＦＰ、ＦＰ_Ｅとエッジ部ＥＤとの関係を示す模式図、図２は、車両内に環境地図生成装置１００が搭載された様子を示す模式図、図３は、第１実施形態に係る環境地図生成装置の構成を示すブロック図である。

図１〜図３に示すように、本実施形態に係る環境地図生成装置１００は、車両１に搭載されて周辺物標ＯＢ１、ＯＢ２の三次元情報を取得して環境地図を生成するものであり、撮像部２と、環境地図構成ＥＣＵ３と、投光部４と、環境地図データベース５とを備える。

撮像部２は、例えばＣＣＤ等の固体撮像素子を用いたカメラであり、例えば車両１の車内ミラー部の裏側に、車両前方を撮像可能な方向に向けて設置される。撮像部２は、一般的な画像を取得すると共に、投光部４により照射された光の被測定物からの反射光もあわせて受光する。そして、撮像した画像を環境地図構成ＥＣＵ３へ出力する。

環境地図構成ＥＣＵ３は、投光制御部３１と、検波部３２と、エッジ検出部３３と、距離算出部３４と、特徴点検出部３５と、自車位置推定部３６と、環境地図生成部３７、及びスケール修正部３８を備え、撮像画像を用いた環境地図生成等、後述する様々な処理を行う電子制御ユニットである。なお、該環境地図構成ＥＣＵ３は、他の制御に用いるＥＣＵと兼用しても良い。また、環境地図構成ＥＣＵ３は、一般的なＥＣＵと同様に、ＣＰＵ、ＲＯＭ、ＲＡＭ等で構成され、ＲＯＭには後述する各種処理手段を実現するプログラムが格納されている。

投光部４は、例えば、プロジェクタヘッドライトやリフレクタを備えたヘッドライトであり、水平方向に発光領域を形成する配光特性を有する。この水平方向に形成された発光光は被測定物に照射され、被測定物上に照射領域と非照射領域の輝度境界を鮮明に映し出す配光を実現する。即ち、投光部４は、水平方向に発光領域を有する時間変調のパルス光を投光する機能を備えている。

環境地図データベース５は、環境地図生成部３７が構成した環境地図情報を、撮像部２で撮像された撮像画像、自車位置推定部３６で取得された自車位置等の付加情報と共に記憶する記憶手段であり、例えば、ハードディスク、メモリーカード等の記憶媒体を用いることができる。

環境地図構成ＥＣＵ３に設けられる投光制御部３１は、ＰＷＭ制御等により投光部４がパルス点灯する際の点灯、及び消灯時間の長さを制御する。投光部４による投光パターンとしては、例えば、水平方向に発光領域を有し、下方領域にも投光光を照射する投光パターンを用いれば良い。また、投光制御部３１は、撮像部２による撮像タイミング信号を出力する。更に、検波部３２に対して、投光部４から照射されたパルス光に同期して撮像された画像からパルス光を抽出するための検波処理に用いる搬送波（キャリア周波数）を与える。

検波部３２は、撮像部２より時系列で得られる画像を用い、画像中の全画素、或いは、画像中に処理領域を制限した場合は、画像処理領域中の全画素において、投光部４より照射されるパルス光に同期した光のみを抽出する。照射した光のみを頑健に検出する処理としては、一般的に用いられる周知の同期検波手法を用いればよい。本実施形態では、撮像部２により撮像された画像の全画素、または処理領域として設定された画像領域中の全画素についてこの同期検波処理を施し、各画素で照射光の抽出を行う。また、移送偏移変調方式としては 、例えば、ＢＰＳＫ（Binary Phase Shift Keying：二位相偏移変調）を用いることができる。

エッジ検出部３３は、検波部３２により抽出された照射光画像より、照射光の上端エッジ部ＥＤを検出する。即ち、図１に示すように、周辺物標ＯＢ１、ＯＢ２に向けてパルス光が照射され、撮像部２にて撮像された画像から照射光の上端となるエッジ部ＥＤを検出する。

距離算出部３４は、第一距離算出部３４ａと、第二距離算出部３４ｂを備えている。第二距離算出部３４ｂは、三角測量の原理に基づいて照射光上端エッジ部ＥＤの照射方向と、撮像部２の視軸がなす角度、及びレイアウトから、エッジ検出部３３により検出・特定されたエッジ部ＥＤ上の各画素について距離（これを、第二距離という）を算出する。算出手法は、一般に光切断法と称する手法を用いればよい。また、第一距離算出部３４ａは、自車位置推定部３６にて推定される自車位置の、撮像部２の設置位置から複数の特徴点までの距離（これを、第一距離という）を算出する。

なお、ここまで説明した投光部４、投光制御部３１、検波部３２、エッジ検出部３３、距離算出部３４の具体的な制御手法は、上記した手法以外の周知の技術を用いることが可能である。

特徴点検出部３５は、撮像部２の撮像画像、或いは、環境地図データベース５に蓄積された過去の撮像画像に対し、画面内の特徴点を検出する。ここで、特徴点とは、画面が移動した際に追従可能な点のことであり、一般的には縦方向、横方向共に輝度差が存在する点を指す。検出方法としては、頑健な特徴点検出方法としてさまざまな手法が提案されており（例えば、ＳＩＦＴ、ＳＵＲＦ、ｆａｓｔ等）、種々の手法を用いることが可能である。

自車位置推定部３６は、撮像部２の撮像画像と、環境地図データベース５に蓄積された撮像画像及び特徴点と、車両上の撮像部２（カメラ）の設置位置の各情報に基づいて自車位置・姿勢を推定する。具体的には、まず事前知識（以前のタイムステップにおける自車位置推定結果、オドメトリ信号、車速、加速度情報等）を用いて自車位置の事前推定を実施する。事前推定により位置と姿勢が得られた場合には、これを用いて、環境地図データベース５に蓄積されている特徴点の中で、撮像部２の撮像画像に写り得る特徴点を選別し、撮像画像内での位置を推定して、その画像内位置推定値を中心として特徴点を画像内で検索を行う。

これにより、事前推定値から想定される画像内位置と実際に検出された画像内位置とのずれを検出する。その後、画像内特徴点位置と、自車位置の事前推定値から算出した特徴点位置のずれから、自車位置を算出する。以下では、これを「事後推定」と称する。ここでは、事前位置推定の結果を、特徴点の位置が実際の計測位置に適合するように調整することで事後推定を実施し、最終的な自車位置推定結果を取得する。

環境地図生成部３７は、撮像部２の撮像画像上の特徴点と、環境地図データベース５に蓄積され、撮像領域に重なりのある過去の撮像画像上の特徴点と、を用いて照合を行い、特徴点の三次元座標を公知の手法（例えば、R. Hartley and A. Zisserman "Multiple View Geometry", Cambridge University pressに開示された手法）で算出する。算出された特徴点の三次元座標は、スケール修正部３８に出力される。また、スケール修正部３８より、該スケール修正部３８でスケール修正された後の修正三次元座標を取得する。

スケール修正後の特徴点の修正三次元座標は、自車位置推定部３６で得られた自車位置・姿勢に基づき、世界座標系に変換した上で、環境地図データベース５にデータ転送し保存される。この際、自車位置・姿勢、撮像画像も、互いに関連付けて、同時に保存される。

上記のように、撮像部２の座標系で三次元化された特徴点を世界座標系に変換するためには、世界座標系における撮像部２の位置と、特徴点の三次元構成とを同時に求めておく必要がある。このように、マップの生成と自車位置の推定を同時に行う手法としては、ＳＬＡＭが提案されており、これに関しては様々なアルゴリズムが発表されている。カメラを使用したＳＬＡＭとしては、Ｍｏｎｏ−ＳＬＡＭ、ＰＴＡＭ等が挙げられる。

スケール修正部３８は、距離算出部３４で算出された画素の距離情報を用いて、環境地図生成部３７で三次元化された特徴点の三次元座標をスケール変換する。詳細な処理手順については、後述する。

次に、第１実施形態に係る環境地図生成装置１００による環境地図生成手順を、図４に示すフローチャートを参照して説明する。なお、図４中のステップＳ３〜ステップＳ５と、ステップＳ６〜ステップＳ１０は、スレッド処理を用いて同時に実行しても良いし、ステップＳ５の後に、ステップＳ６を実施しても良い。また、ステップＳ１〜ステップＳ５までを速い周期で実施して、ステップＳ６〜ステップＳ１３を遅い周期で非同期に実施させる手法をとっても良い。

初めに、図４に示すステップＳ１では、投光制御部３１の指令に基づき、投光部４をパルス点灯させると同時に、撮像タイミング信号を撮像部２に送信する。その後、ステップＳ２に処理を進める。

ステップＳ２では、ステップＳ１で送信された撮像タイミング信号に基づき、撮像部２で撮像した画像を取り込む。その後、ステップＳ３、及びステップＳ６に処理を進める。

ステップＳ３では、ステップＳ２での撮像画像と、投光部４から送付される搬送波（キャリア周波数）に基づき、検波部３２により、投光部４から照射されるパルス光に同期した光のみを抽出する。その後、ステップＳ４に処理を進める。

ステップＳ４では、検波部３２により抽出された照射光画像から、エッジ検出部３３により、照射光上端エッジ部ＥＤを検出する。その後、ステップＳ５に処理を進める。

ステップＳ５では、距離算出部３４の第二距離算出部３４ｂにより、エッジ検出部３３にて検出・特定されたエッジ部ＥＤ上の各画素について距離を算出し、算出した距離を第二距離とする。その後、ステップＳ１１に処理を進める。なお、ステップＳ１〜Ｓ５までの、同期検波により距離検出処理をステップＳ６以降とは非同期に、高い更新頻度で実施する場合には、ステップＳ５の終了後に、算出データをメモリに保存した上で、ステップＳ６に進んでも良い。

ステップＳ６では、撮像部２で撮像された撮像画像中から、ステップＳ５にて第二距離を算出する際に用いた画像を選択し、特徴点検出部３５を用いて特徴点を検出する。その後、ステップＳ７に処理を進める。

ステップＳ７では、環境地図データベース５で既に初期化処理が実施済みか否かを確認する。初期化が未実施の場合には（ステップＳ７でＹＥＳ）、ステップＳ８に処理を進め、初期化済みの場合には（ステップＳ７でＮＯ）、ステップＳ９に処理を進める。

ステップＳ８では、環境地図の初期化処理を行う。この処理では、まずステップＳ６で算出した特徴点情報をメモリに保持した上で、ステップＳ２の処理に戻り、再び撮像部２から撮像画像を取り込み、同様に特徴点を算出し、既に取得している特徴点と比較することで、特徴点の追跡を行う。なお、特徴点追跡手法は、ＫＬＴ Ｔｒａｃｋｅｒ等、一般に知られる画像処理手法を適用すれば良い。継続的に特徴点追跡を行い、車両が一定距離、例えば０．５ｍ進行した時点で最初の撮像画像と、現在の撮像画像の特徴点を比較し、一般に知られた８点法などの手法を用いて、特徴点の三次元化を行うと共に、特徴点との相対関係として、車両の初期自車位置・姿勢を規定し、環境地図の初期化処理を終了する。

なお、この処理は連続的に撮像された複数の撮像画像を用いるため、この処理の間はステップＳ９以降の処理は実施されない。

ステップＳ９では、自車位置推定部３６を用いて、ステップＳ６で算出した特徴点と、環境地図データベース５に蓄積された三次元特徴点群との相対関係で、自車位置・姿勢を推定する。その後、ステップＳ１０に処理を進める。

ステップＳ１０では、ステップＳ９で得られた自車位置・姿勢情報に基づき、環境地図データベース５から、撮像領域に重なりのある過去の撮像画像を取得する。過去の撮像画像中の特徴点と、ステップＳ６の処理で取得された特徴点を比較し、一致する複数の特徴点群を用いて、環境地図生成部３７により三次元特徴点群を得る。また、撮像部２の設置位置から各特徴点までの距離を算出し、これを第一距離とする。この第一距離の算出は、図１に示す距離算出部３４の第一距離算出部３４ａにより行われる。その後、ステップＳ１１に処理を進める。

ステップＳ１１では、ステップＳ１０の処理で求められた三次元特徴点群の現在画像での画素位置と、ステップＳ５の処理で、エッジ部ＥＤ上に存在し第二距離が算出された画素位置と、を比較することで、第二距離が算出された特徴点（特定特徴点）が存在するか否かを確認する。第二距離が算出された特徴点（特定特徴点）が存在する場合には（ステップＳ１１でＹＥＳ）、ステップＳ１２に処理を進め、存在しない場合には（ステップＳ１１でＮＯ）、ステップＳ１３に処理を進める。

ステップＳ１２では、特徴点の距離情報から、自車位置と同時に算出された特徴点全てのスケール修正を行う。その後、ステップＳ１３に処理を進める。

ステップＳ１３では、ステップＳ１１の処理で求められた、或いはステップＳ１２の処理で修正された、特徴点群及び自車位置・姿勢を世界座標系に変換した上で、撮像画像と共に環境地図データベース５に保存する。その後、処理をステップＳ１に戻す。

次に、具体的な手法を図５を用いて説明する。なお、図５では距離検出された特徴点が一つの場合（即ち、特徴点の画像位置と、距離算出部３４にて算出される距離算出画像位置との一致点が一つの場合）を想定する。図５及び図１において、符号Ｃtは、ステップＳ９の処理で推定された現在の撮像位置・姿勢を示し、符号Ｃt-1は、ステップＳ１０の処理で特徴点の三次元化のために選択した撮像画像の撮像時の撮像位置・姿勢を示す。符号Ｐは、距離検出された特徴点（特定特徴点）を示す。即ち、特徴点Ｐは、その画像位置と、距離算出部３４で算出された距離算出画像位置とが一致している。

まず、ステップＳ１０の処理で三次元化した特徴点群の座標位置を用いて、カメラ位置Ｃｔから特徴点Ｐまでの距離Ｄ１（第一距離）を算出する。次に、ステップＳ５の処理で算出される特徴点画素位置での距離をＤ２（第二距離）として、スケール比Ｒを、Ｒ＝Ｄ２／Ｄ１で算出する。

次に、カメラ位置Ｃt-1を中心として、カメラ位置Ｃｔ、特徴点Ｐ、及び他の特徴点の全てを、スケール比Ｒに基づき拡大、或いは縮小させる。図５に示す例では、カメラ位置Ｃ’t、及び特徴点Ｐ’がそれぞれカメラ位置Ｃt、及び特徴点Ｐのスケール変換後の位置となっており、「Ｃt-1、Ｃｔ、Ｐ」を頂点とする三角形と、「Ｃt-1、Ｃ’t、Ｐ’」を頂点とする三角形は相似であり、相似比は１：Ｒ（＝Ｄ１：Ｄ２）となり、スケール変換後のカメラ位置Ｃ’tと特徴点Ｐ’との間の距離はＤ２と一致することになる。

そして、他の特徴点についても上記のスケール比Ｒに基づいて、カメラ位置Ｃt-1を中心として、拡大、縮小させる。例えば、特徴点Ｐは、特徴点Ｐ’の位置にスケールが修正されることになる。図５中の白い丸は修正前の特徴点を示し、黒い丸は修正後の特徴点を示す。

また、上記の説明では、第二距離が検出された特徴点（特定特徴点）が一つの場合について説明したが、実際には、複数の特徴点について第二距離が算出されることが多い。以下では、第二距離が算出された特徴点がＰ１，Ｐ２，…，ＰｎのＮ個算出された場合の、スケール比の算出手順について説明する。まず、Ｎ個の特徴点Ｐ１〜Ｐｎそれぞれについて、上記と同様にスケール比Ｒ１，Ｒ２，…，Ｒｎを算出する。次に、特徴点検出時の特徴点強度ｆ１，ｆ２，… ，ｆｎを、特徴が強い点ほど数字が大きくなるよう定義して算出する。例えば、輝度値の差を利用する特徴点検出手法であるＦＡＳＴでは、中心ピクセルと周辺の比較ピクセルの輝度差の平均値を用いてスコアリングすれば良い。ここで、ｆ１からｆｎまでの総和をｆｓとする。算出した特徴点強度を用いてスケール比の代表値Ｒを、Ｒ＝（Ｒ１×ｆ１＋Ｒ２×ｆ２＋…＋Ｒｎ×ｆｎ）／ｆｓで算出し、このスケール比の代表値Ｒ（重み付け平均値）を用いてスケール修正を行えば良い。

なお、スケール比の代表値Ｒの算出方法としては、上記の重み付け平均値以外に、単純な平均値を利用しても良いし、最も強度の高い特徴点でのスケール比Ｒを利用しても良い。また、カメラ（撮像部２）からの距離が既知である特徴点に関しては、それぞれのスケール比でスケールを修正し、自車位置、及び他の特徴点群は、スケール比の代表値Ｒを用いて修正しても良い。

このようにして、本発明の第１実施形態に係る環境地図生成装置１００では、単眼カメラでステレオ処理する場合において、同期検波技術を用いた絶対距離情報を組み込むことができるので、周辺物標の三次元位置を示すマップを実寸に合わせることができる。また、演算処理の経過で、自車位置の修正も同時に行われるので、自車位置推定精度を向上させることができる。

また、第二距離が算出される特徴点（特定特徴点）が複数存在する場合には、各特徴点の強度に応じた重み付けでスケール修正処理を行うので、信頼性の低い特徴点の影響を減らし、マッチングミスを回避し、且つ誤差の累積を防止したスケール修正を行うことができる。

また、ステレオ処理に用いる単眼カメラと、同期検波を行うカメラを同一としており、また、パルス光を投光する投光部４として、車両１に設置されるヘッドランプを用いるようにすれば、ステレオカメラで距離検出を行う場合と対比して、コストダウンを図ることが可能となる。

［第２実施形態の説明］
次に、本発明の第２実施形態について説明する。図６は、第２実施形態に係る環境地図生成装置２００の構成を示すブロック図である。図６は、前述した第１実施形態で説明した図３と対比して、エッジ検出部３３による検出信号を、特徴点検出部３５に出力している点のみが相違し、その他の構成は同一である。よって、同一構成については同一符号を付して構成説明を省略する。

図６に示す特徴点検出部３５は、撮像部２の撮像画像、或いは、環境地図データベース５に蓄積された過去の撮像画像に対し、画面内特徴点を検出する。更に、該特徴点検出部３５は、エッジ検出部３３により求められたエッジ部ＥＤの画素情報を用いて、領域毎に特徴点検出基準（特徴点検出しきい値）を変更する。一般的に特徴点検出手法にはしきい値が存在し、そのしきい値を高く設定すると検出される特徴点が減少し、低く設定すると検出される特徴点が増大する。

特徴点検出基準の変更エリアを図７に示す説明図を参照して説明する。車両に搭載されたヘッドランプで水平方向にエッジラインを持つパルス光を投光し撮像する場合には、一般的にエッジ部ＥＤの検出エリアは、車両のピッチ角変動に応じて、撮像画素上で上下方向に振れる。そこで、図７に示す如くの、エッジ検出部３３により算出された検出エッジ部ＥＤの画素の上下一定エリアＧＡを、特徴点検出基準の変更エリアとして設定し、このエリアでは特徴点の検出しきい値を低く設定して、特徴点を検出し易くする。

なお、検出しきい値の変更エリア幅は、車両の実際ピッチ角変動値や処理速度要求値等を勘案して任意に定めればよい。また、検出エッジ部ＥＤ上の画素では、更に検出しきい値を低く設定しても良い。

第２実施形態に係る環境地図生成装置２００の処理手順は、前述した第１実施形態で説明した図４に示したステップＳ３〜Ｓ５の処理を、ステップＳ６の前に実行する。それ以外は、図４と同一であるので、詳細な動作説明を省略する。

このようにして、第２実施形態に係る環境地図生成装置２００では、エッジ検出部３３が算出したエッジ部ＥＤ周辺での特徴点密度を恣意的に高めることができる。即ち、図７に示すエッジ部ＥＤ近傍の領域において、特徴点の検出しきい値を低くすることにより、特徴点密度が多くなるように設定できる。従って、三次元化された特徴点の第二距離が検出されている確率を高めることができ、結果として周辺物標の三次元位置を示すマップの作成精度を向上させることができる。

また、図７に示したように、同期検波技術で距離情報が算出される領域の、エッジ部ＥＤに垂直な方向の一定幅ＧＡにおいて重点的に特徴点を取得するため、特徴点検出時には第二距離が検出されない場合でも、車両の移動と共に第二距離が検出される可能性の高い特徴点を増やすことが可能となる。そして、後に、第二距離が検出された特徴点が見つかった場合に、対応する過去画像からも同じ特徴点を算出する必要があるが、事前に特徴点候補として算出済みなので、追加処理をせずに容易に対応特徴点を見つけることができる。

なお、パルス光はエッジラインが不連続な場合、その左右方向ではなく、エッジ部ＥＤに垂直な方向で第二距離が検出される可能性が高いため、エッジ部ＥＤ周辺ではなく、エッジ部ＥＤ垂直方向のみとすることで、メモリ容量・計算コストをいたずらに増加させず、合理的に特徴点（特定特徴点）を増やすことができる。

［第３実施形態の説明］
次に、本発明の第３実施形態について説明する。図８は、第３実施形態に係る環境地図生成装置３００の構成を示すブロック図である。

図８は、前述した第１実施形態で説明した図３と対比して、距離算出部３４、スケール修正部３８の処理が異なり、更に、環境地図更新部３９が追加されている点で相違する。その他の構成は同一である。よって、同一構成については同一符号を付して構成説明を省略する。

図８に示す距離算出部３４の第一距離算出部３４ａ及び第二距離算出部３４ｂは、三角測量の原理に基づき、照射光上端エッジ部ＥＤの照射方向と、撮像部２の視軸がなす角度、及びレイアウトに基づいて、エッジ検出部３３により検出・特定されたエッジ部上の各画素について第一距離、及び第二距離を算出する。算出手法は、一般に光切断法と呼ばれる手法を用いれば良い。

そして、第３実施形態に係る環境地図生成装置３００では、距離算出部３４にて算出された画素の第二距離の情報は、この画素の位置が、環境地図生成部３７で検出された特徴点の位置と一致した場合に、この特徴点に付随する情報として、環境地図データベース５に保存する。即ち、第二距離の情報が付加された特徴点が環境地図データベース５に保存される。

スケール修正部３８は、環境地図データベース５に蓄積された一定フレーム数前（例えば、１フレーム前）の特徴点群と自車位置情報を取得し、該特徴点群の中に、距離情報が付加された特徴点が存在するか否かを判断し、存在する場合には、該特徴点群のスケール修正処理を行う。このスケール修正処理は、前述の第１実施形態で説明した図４のステップＳ１２の処理で説明した手法を用いることができる。

更に、スケール修正が行われた場合には、一定フレーム数前の自車位置も修正されるので、該当フレームよりも後に算出された自車位置と特徴点群に対して、補正を行う必要がある。具体的には、前記スケール修正で自車位置に対して行われたのと同じ座標変換、すなわち、図５に示したカメラ位置ＣtからＣ’tへの座標変換と同じ座標変換を行い、位置を修正する。

環境地図更新部３９は、環境地図データベース５に、互いに関連付けられて保存されている撮像画像、特徴点群、及び撮像時の自車位置情報を用いて、メインの処理ステップとは非同期に、環境地図データベース５の更新を行う。更新手法としては、公知の技術であるBundle Adjustment（例えば、G. Klein and D.Murray "Parallel Tracking and Mapping for Small AR Workspaces", ISMAR 2007に開示された手法）を用いれば良い。これにより、特徴点群の位置の補正と同時に、複数の撮像画像から整合性のとれない特徴点に関しては削除される。

次に、環境地図生成装置３００の処理手順を説明する。第３の実施の形態における処理手順は、前述した第１実施形態で説明した図４のフローチャートに示したステップＳ１２のスケール修正処理が他の処理ステップとは非同期に別スレッドで実施されること以外は、図４のフローチャートと同一であるので、詳細な動作説明を省略する。

このようにして、第３実施形態に係る環境地図生成装置３００では、環境地図更新部３９によって、環境地図内の特徴点群は、新たな画像の取得と共に更新されるだけでなく、非同期で、位置の補正や整合性の取れない場合には削除が行われるため、信頼性の高い特徴点のみを残すことができる。そして、距離算出特徴点を、特徴点検出時ではなく、複数回の更新の後に残った場合にのみ、スケール修正を適用することで、信頼性の高い特徴点のみをスケール修正に利用することができ、環境地図の精度信頼性を向上させることが可能となる。

［第４実施形態の説明］
次に、本発明の第４実施形態について説明する。図９は、第４実施形態に係る環境地図生成装置４００の構成を示すブロック図である。

図９に示す環境地図生成装置４００は、第１実施形態で説明した図３と対比して、撮像部２が異なり、投光制御部３１、検波部３２、エッジ検出部３３、投光部４が省略されている点で相違する。その他の構成は同一である。よって、同一構成については同一符号を付して構成説明を省略する。

撮像部２は、位置関係が既知に固定された２つのカメラ（カメラＡ及びカメラＢ）で構成される、所謂ステレオカメラであり、例えば、車両１の車内ミラー部の裏側に、車両前方を撮像可能な方向に向けて設置される。撮像部２は、２つのカメラを同期させて撮像し、撮像した画像を環境地図構成ＥＣＵ３へ出力する。なお、距離算出部３４では、カメラＡとカメラＢの２つの画像を用いて距離を算出するが、特徴点検出部３５、自車位置推定部３６、環境地図生成部３７では、カメラＡ或いはカメラＢで撮像された画像のいずれか一方の画像のみを用いる。

距離算出部３４は、第一距離算出部３４ａと、第二距離算出部３４ｂとを備える。第二距離算出部３４ｂは、カメラＡ及びカメラＢの画像を比較し、画像間で対応する画素を見つける。そして、対応画素間で三角測量の原理に基づいて、カメラ間の位置関係から、対応が取れた各画素について距離（これを、第二距離という）を算出する。算出手法は、一般的なステレオ視の手法を用いればよい。

ただし、一般的なステレオ視の手法では、画面全体で距離算出を行うため、画像間での画素対応を誤ったことにより距離を誤って算出される画素が生じる場合がある。第４実施形態では、画面全体で距離を算出することよりも、誤りを排除した正確な距離算出を優先する。そこで、画像間で画素の誤対応による距離の誤算出を抑制するため、一般的なステレオ視の手法よりも、画像の対応付けを行う際の閾値を厳しく設定することが望ましい。

また、第一距離算出部３４ａは、自車位置推定部３６にて推定される自車位置の撮像部２の設置位置から複数の特徴点までの距離（これを第一距離という）を、カメラＡの画像を基に算出する。

次に、第４実施形態に係る環境地図生成装置４００による環境地図生成手順を、図１０に示すフローチャートを参照して説明する。環境地図生成装置４００による環境地図生成手順では、図４のフローチャートからステップＳ１、ステップＳ３、ステップＳ４が省略され、ステップＳ５の処理内容が異なる。その他は、図４のフローチャートと同一である。ステップＳ５の処理内容について説明する。

ステップＳ５では、距離算出部３４の第二距離算出部３４ｂにより算出した距離を第二距離とする。その後、ステップＳ１１に処理を進める。

このように、第４実施形態に係わる環境地図生成装置４００では、単眼カメラを使ったモーションステレオによる特徴点群の位置を、ステレオカメラによって、算出範囲は狭くなるが、精度の高い距離算出が可能となる特徴点を用いて、修正する。これにより、特にステレオカメラでカバーできない遠方での環境地図精度を向上させることが可能となる。

［第５実施形態の説明］
次に、本発明の第５実施形態について説明する。図１１は、第５実施形態に係る環境地図生成装置５００の構成を示すブロック図である。

図１１に示す環境地図生成装置５００は、第１実施形態で説明した図３と対比して、測域センサ６が追加され、投光制御部３１、検波部３２、エッジ検出部３３、投光部４が省略されている点で相違する。その他の構成は同一である。よって、同一構成については同一符号を付して構成説明を省略する。

測域センサ６は、光源と受光部を有し、光源から射出された光が物体に反射して受光部に届くまでの時間を計測して、タイムオブフライト方式により測域センサ６から物体までの距離を計測する機器であり、レーザレンジファインダとも呼ばれる。一般に、測域センサ６は、ライン状に距離を計測することができる。車両１上において、撮像部２と測域センサ６の位置関係は既知である。

距離算出部３４は、第一距離算出部３４ａと、第二距離算出部３４ｂとを備えている。第二距離算出部３４ｂは、測域センサ６で算出された距離データを、撮像部２との幾何学的な位置関係を用いて座標変換することで、撮像画像に重ね合わせ、距離データが重畳された各画素について距離（これを、第二距離という）を算出する。

また、第一距離算出部３４ａは、自車位置推定部３６にて推定される自車位置の、撮像部２の設置位置から複数の特徴点までの距離（これを、第一距離という）を算出する。

次に、第５実施形態に係る環境地図生成装置５００による環境地図生成手順を説明する。環境地図生成装置５００による環境地図生成手順では、図４のフローチャートと対比して、ステップＳ２の処理内容が異なる。その他は、図４のフローチャートと同一である。ステップＳ２において、画像取得と同時に測域センサ６で距離計測を行う。

このように、第５実施形態に係る環境地図生成装置５００では、測域センサ６で取得した距離情報を、撮像部２で撮像した画像に画像変換することで、単眼カメラを使ったモーションステレオによる特徴点群に対し、正確な距離情報を付加することができる。

［第６実施形態の説明］
次に、本発明の第６実施形態について説明する。図１２は、第６実施形態に係る環境地図生成装置６００の構成を示すブロック図である。

図１２に示す環境地図生成装置６００は、第１実施形態で説明した図３と対比して、投光部４、投光制御部３１、検波部３２の処理内容が異なり、エッジ検出部３３が省略されている点で相違する。その他の構成は同一である。よって、同一構成については同一符号を付して構成説明を省略する。

投光部４は、例えば、レーザー光源や赤外光源であり、パターン光を投光可能な手段である。投光部４から射出された光は、被測定物に照射され、撮像部２により自然光と分離されて検出可能な波長を持つ光である。パターン光としては、例えばレーザーをライン状に投光してもよいし、キネクト（Ｋｉｎｅｃｔ）で採用されているように、ドットパターンの赤外光を投光してもよい。

投光制御部３１は、投光部４の点灯時間、及び消灯時間の長さを制御する。また、投光制御部３１は、撮像部２による撮像タイミング信号を出力する。

検波部３２は、撮像部２で撮像された画像を用い、画像中の全画素において、投光部４より照射されるパターン光の波長の光のみを抽出する。照射した光のみを頑健に検出する処理としては、一般的に用いられている周知のパターン光検出手法を用いればよい。

距離算出部３４は、第一距離算出部３４ａと、第二距離算出部３４ｂとを備える。第二距離算出部３４ｂは、三角測量の原理に基づいて照射パターン光の照射方向と、撮像部２の視軸がなす角度、及びレイアウトから、検波部３２により検出・特定されたパターン光上の各画素について距離（これを、第二距離という）を算出する。算出手法は、パターン光がライン状の場合には、一般に用いられる光切断法と称する手法を用いればよい。パターン光がドットパターンであれば、キネクトに使われる処理手法（米国特許公開２０１０／０１１８１２３Ａ１公報）を用いればよい。

また、第一距離算出部３４ａは、自車位置推定部３６にて推定される自車位置の、撮像部２の設置位置から複数の特徴点までの距離（これを、第一距離という）を算出する。

次に、第６実施形態に係る環境地図生成装置６００による環境地図生成手順を説明する。環境地図生成装置６００による環境地図生成手順では、図４のフローチャートから、ステップＳ４のエッジ部検出処理が省略されている。その他の処理は、図４のフローチャートと同一である。

このように、第６実施形態に係わる環境地図生成装置６００では、投光光として、赤外光やレーザー光など、自然光と分離可能な波長の光を用いるため、第１実施形態で必要であった、連続画像からの同期検波が不要となる。

以上、実施形態に沿って本発明の内容を説明したが、本発明はこれらの記載に限定されるものではなく、種々の変形及び改良が可能であることは、当業者には自明である。

第１〜第６の実施形態では、車両に搭載された、複数の特徴点を前記第一距離算出部で算出した第一距離に基づいて、三次元化して走行路周囲の環境地図とする環境地図生成装置について説明したが、本発明の実施形態は、環境地図生成装置に限らない。例えば、車両１の周囲に存在する複数の物体ＯＢ１、ＯＢ２上の特徴点ＦＰまでの距離を、第一距離と第二距離との比率に基づいて修正することにより、距離測定の精度を向上させた距離計測装置として実施することもできる。

他の実施形態に係わる距離計測装置は、撮像部２と、特徴点検出部３５と、第一距離算出部３４ａと、第二距離算出部３４ｂと、スケール修正部３８とを備える。撮像部２は、車両１が移動している時に、車両１の周囲画像を撮像する。特徴点検出部３５は、撮像部２にて撮像された画像から、車両１の周囲に存在する複数の物体ＯＢ１、ＯＢ２の特徴点ＦＰを検出する。第一距離算出部３４ａは、特徴点検出部３５にて検出された特徴点の時間変化から、特徴点ＦＰから撮像部２までの第一距離を算出する。第二距離算出部３４ｂは、画像内に存在する複数の物体ＯＢ１、ＯＢ２に含まれる特定物体の一部画素を用いて、特定物体から撮像部２までの第二距離を算出する。スケール修正部３８は、複数の物体の特徴点の中で、特定物体の一部画素と略一致する特定特徴点を抽出して、特定特徴点における第一距離と第二距離との比率を算出し、特徴点検出部３５にて同時に検出される特定特徴点以外の複数の特徴点の第一距離を、第一距離と第二距離との比率に基づいて修正する。

他の実施形態に係わる距離計測装置は、水平方向に延びるエッジ部ＥＤを有する光を投光する投光部４と、光のエッジ部ＥＤを検出するエッジ検出部３３とを更に備えていてもよい。この場合、第二距離算出部３４ｂは、特定物体におけるエッジ部ＥＤと撮像部２との第二距離を算出する。

第１〜第６の実施形態で説明した環境地図生成装置は、上記した距離計測装置を用いて実現することも可能である。

日本国特許願第２０１１−１３１８８８号（出願日：２０１１年６月１４日）の全内容は、ここに援用される。

本発明の実施形態に係る距離計測装置及び環境地図生成装置では、特徴点検出部３５により検出される画像中の特徴点と撮像部２との距離である第一距離を算出する。距離算出部３４にて第二距離が算出できた特徴点が存在する場合に、第一距離と第二距離の比率であるスケール比に基づいて、三次元化されている複数の特徴点の三次元位置のスケールを修正する。これにより、全体的に高精度なスケール精度の環境地図を生成することが可能となる。従来手法で補正を掛けることができなかった遠方特徴点において、特に精度向上の効果が高い。よって、本発明の実施形態に係る距離計測装置及び環境地図生成装置は、産業上利用可能である。

１ 車両
２ 撮像部
３ 環境地図構成ＥＣＵ
４ 投光部
５ 環境地図データベース
６ 測域センサ
３１ 投光制御部
３２ 検波部
３３ エッジ検出部
３４ 距離算出部
３４ａ 第一距離算出部
３４ｂ 第二距離算出部
３５ 特徴点検出部
３６ 自車位置推定部
３７ 環境地図生成部
３８ スケール修正部
３９ 環境地図更新部
１００、２００、３００、４００、５００ 環境地図生成装置

Claims (8)

1. 車両が移動している時に、前記車両の周囲画像を撮像する撮像部と、
   前記撮像部にて撮像された画像から、前記車両の周囲に存在する複数の物体の特徴点を検出する特徴点検出部と、
   前記特徴点検出部にて検出された前記特徴点の時間変化から、前記特徴点から前記撮像部までの第一距離を算出する第一距離算出部と、
   水平方向に延びるエッジ部を有する光を投光する投光部と、
   前記撮像部にて撮像された画像から、前記光のエッジ部を検出するエッジ検出部と、
   前記エッジ部検出部により検出された前記エッジ部から前記撮像部までの第二距離を算出する第二距離算出部と、
   前記複数の物体の特徴点の中で、前記エッジ部と略一致する特定特徴点を抽出して、前記特定特徴点における前記第一距離と前記第二距離との比率を算出し、前記特徴点検出部にて同時に検出される前記特定特徴点以外の複数の特徴点の前記第一距離を、前記比率に基づいて修正するスケール修正部と、
   を備えたことを特徴とする距離計測装置。
2. 車両に搭載され、走行路周囲の環境地図を生成する環境地図生成装置において、
   車両の周囲画像を撮像する撮像部と、
   前記撮像部にて撮像された画像から複数の特徴点を検出する特徴点検出部と、
   前記特徴点検出部にて検出された複数の連続的な配置の特徴点から前記撮像部の設置位置を含む自車位置を推定する自車位置推定部と、
   前記自車位置推定部にて推定された自車位置の前記撮像部の設置位置から前記複数の特徴点までの第一距離を算出する第一距離算出部と、
   前記特徴点検出部で検出された複数の特徴点を前記第一距離算出部で算出した第一距離に基づいて、三次元化して環境地図とする環境地図生成部と、
   水平方向に発光領域を有するパルス光を投光する投光部と、
   前記撮像部により撮像された画像から、同期検波により前記パルス光を抽出する検波部と、
   前記検波部により検出されるパルス光上端のエッジ部を検出するエッジ検出部と、
   前記エッジ検出部により検出されるエッジ部と、前記撮像部との第二距離を算出する第二距離算出部と、を有し、
   前記環境地図生成部は、前記複数の特徴点の画像位置と、前記エッジ部の画像位置との一致、不一致を判断し、
   判断が一致した特徴点を特定特徴点とし、前記特定特徴点の第一距離と前記第二距離の比率であるスケール比を算出し、前記特徴点検出部にて同時に検出される前記特定特徴点以外の複数の特徴点の前記第一距離を、前記スケール比に基づいて修正するスケール修正部と、
   を備えたことを特徴とする環境地図生成装置。
3. 前記特徴点検出部は、前記特定特徴点であるか否かを判断する特徴点検出しきい値に基づいて、特定特徴点であるか否かを判断し、
   該特徴点検出部は、前記第二距離算出部が距離を算出する前記エッジ部の近傍領域を、他の領域よりも前記特徴点検出しきい値を低くすることにより、前記エッジ部の近傍領域にて特徴点が検出され易くすることを特徴とする請求項２に記載の環境地図生成装置。
4. 前記特徴点検出部は、前記エッジ部の近傍領域を、前記エッジ部上下の所定幅の領域に設定することを特徴とする請求項３に記載の環境地図生成装置。
5. 前記スケール修正部は、前記複数の特徴点毎にそれぞれ特徴点強度を求め、前記各特徴点強度に応じて前記スケール比の重み付け平均値を求め、この重み付け平均値に基づいて、前記特定特徴点以外の複数の特徴点の三次元位置を修正することを特徴とする請求項２〜請求項４のいずれか１項に記載の環境地図生成装置。
6. 前記撮像部は、複数の画像を時系列的に撮像し、
   前記時系列的に撮像された画像を用いて、前記環境地図を更新する地図更新部を更に有し、
   前記スケール修正部は、前記特定特徴点が、所定期間を超えて前記地図更新部により削除されなかった場合に、この特徴点と同時に検出されたそれ以外の複数の特徴点の三次元位置を、前記スケール比に基づいて修正することを特徴とする請求項２〜請求項５のいずれか１項に記載の環境地図生成装置。
7. 車両が移動している時に、前記車両の周囲画像を撮像する撮像手段と、
   前記撮像手段にて撮像された画像から、前記車両の周囲に存在する複数の物体の特徴点を検出する特徴点検出手段と、
   前記特徴点検出手段にて検出された前記特徴点の時間変化から、前記特徴点から前記撮像手段までの第一距離を算出する第一距離算出手段と、
   水平方向に延びるエッジ部を有する光を投光する投光手段と、
   前記撮像手段にて撮像された画像から、前記光のエッジ部を検出するエッジ検出手段と、
   前記エッジ部検出手段により検出された前記エッジ部から前記撮像手段までの第二距離を算出する第二距離算出手段と、
   前記複数の物体の特徴点の中で、前記エッジ部と略一致する特定特徴点を抽出して、前記特定特徴点における前記第一距離と前記第二距離との比率を算出し、前記特徴点検出手段にて同時に検出される前記特定特徴点以外の複数の特徴点の前記第一距離を、前記比率に基づいて修正するスケール修正手段と、
   を備えたことを特徴とする距離計測装置。
8. 車両に搭載され、走行路周囲の環境地図を生成する環境地図生成装置において、
   車両の周囲画像を撮像する撮像手段と、
   前記撮像手段にて撮像された画像から複数の特徴点を検出する特徴点検出手段と、
   前記特徴点検出手段にて検出された複数の連続的な配置の特徴点から前記撮像手段の設置位置を含む自車位置を推定する自車位置推定手段と、
   前記自車位置推定手段にて推定された自車位置の前記撮像手段の設置位置から前記複数の特徴点までの第一距離を算出する第一距離算出手段と、
   前記特徴点検出手段で検出された複数の特徴点を前記第一距離算出手段で算出した第一距離に基づいて、三次元化して環境地図とする環境地図生成手段と、
   水平方向に発光領域を有するパルス光を投光する投光手段と、
   前記撮像手段により撮像された画像から、同期検波により前記パルス光を抽出する検波手段と、
   前記検波手段により検出されるパルス光上端のエッジ部を検出するエッジ検出手段と、
   前記エッジ検出手段により検出されるエッジ部と、前記撮像手段との第二距離を算出する第二距離算出手段と、を有し、
   前記環境地図生成手段は、前記複数の特徴点の画像位置と、前記エッジ部の画像位置との一致、不一致を判断し、
   判断が一致した特徴点を特定特徴点とし、前記特定特徴点の第一距離と前記第二距離の比率であるスケール比を算出し、前記特徴点検出手段にて同時に検出される前記特定特徴点以外の複数の特徴点の前記第一距離を、前記スケール比に基づいて修正するスケール修正手段と、
   を備えたことを特徴とする環境地図生成装置。

Images