JP4154980B2

JP4154980B2 - 移動体周辺監視装置

Info

Publication number: JP4154980B2
Application number: JP2002284661A
Authority: JP
Inventors: 俊明柿並; 修司山本
Original assignee: Aisin Seiki Co Ltd; Aisin Corp
Current assignee: Aisin Corp
Priority date: 2002-09-30
Filing date: 2002-09-30
Publication date: 2008-09-24
Anticipated expiration: 2022-09-30
Also published as: EP1403137A1; JP2004120661A; DE60325819D1; EP1403137B1

Description

【０００１】
【発明の属する技術分野】
本発明は、移動体周辺を監視する移動体周辺監視装置に関し、特に、移動体周辺の環境の中から最も接近した状態の障害物を検出し得る移動体周辺監視装置に係り、例えば、車両の移動予想軌跡上に存在する障害物を検出する装置に好適である。
【０００２】
【従来の技術】
従来から、車両の移動予想軌跡上に存在する障害物を検出する装置が種々提案されており、例えば、超音波を発射し障害物からの反射波を受信するソナー方式、光学方式等の障害物センサを用いた装置が実用化されている。
【０００３】
【特許文献１】
例えば、特開昭６２−２７８４７７号公報には、距離センサおよびハンドル角センサからの出力信号をもとに自動車の走行軌跡を演算するコンピュータを備え、光学式の距離センサによって機械的あるいは電気的にスキャンし、車両周辺の障害物の位置および予想走行軌跡上にある車両と障害物とが予め設定された距離以内にあるときに警報器でアラーム音を発するようにした自動車の車庫入れ支援システムが提案されている。
【０００４】
【特許文献２】
また、特開平１−１４１１３７号公報には、車両の前部両側にソナー方式、赤外光学方式、接触方式等の障害物検出センサを設け、該検出センサにより車両の前部両側の障害物への接近を検出し、該検出情報をモニタ画面に後方視界と同時に表示するようにした車両の後方視界表示装置が提案されている。これにより、運転者はモニタ画面から車両の前方へ視線を移さなくとも車両の前部両側の障害物への接触を回避できる旨記載されている。
【０００５】
【特許文献３】
更に、特開２００１−１８７５５３号公報には、車両の移動中に、単一のカメラによって、異なる第１および第２の地点での周辺画像を、時系列的に視差付けされた第１および第２の画像として撮像する撮像装置、それぞれの画像に撮像された立体物を検出し特定する立体物特定部と、第１の地点から第２の地点まで車両の移動データを算出する車両位置算出部と、立体物の第１および第２の画像上での位置と、車両の移動データとを用いて、車両から立体物までの距離を算出する立体物距離算出部と、撮像装置によって撮像された画像および立体物距離算出部で算出されたデータに基づいて、運転者に伝達するための第３の画像を生成するようにした駐車支援装置が提案されている。これにより、２つの画像に撮像された立体物に対して三角測量の原理で正確に上記立体物までの距離を算出することができる旨記載されている。
【０００６】
【特許文献４】
そして、特開２００２−０５２９９９号公報には、自車両の停止位置マーカと横方向距離計測マーカをモニタ画面に重畳描画し、測距センサを設けることなく、カメラ画像と汎用の舵角センサを利用し、自車両と基準物との相対位置を検出し得る相対位置検出装置、及びこれを備えた駐車補助装置が提案されている。ここで、自車両の停止位置マーカは、自車両と基準物とが所定方位を有する位置関係で自車両を停止させるためのマーカと定義され、横方向距離計測マーカは、モニタ画面上の移動量に応じて自車両と基準物との横方向の距離を計測するマーカと定義されている。
【０００７】
【非特許文献１】
ところで、画像処理技術の分野においては、２台のカメラから３次元形状を復元する技術が知られている。例えば、「情報処理」誌のVol.37 No.7 (１９９６年７月号)に「２．ステレオの仕掛けを解き明かす」と題する出口光一郎氏の論文が掲載されており、ステレオによる立体形状計測について解説されている。同誌には、その前提として、２つのカメラそれぞれの結像の特性（レンズの焦点距離、画像中心、画素サイズなど）、２つのカメラの位置、姿勢、そして２つの画像での対応が分かれば、その対応する点の空間での位置が決定できる旨記載されている。
【０００８】
【非特許文献２】
同様に、「空間データの数理」と題する書籍 (金谷健一著。１９９５年３月１０日初版第１刷。朝倉書店発行)の１６１頁及び１６２頁に「ステレオ視による３次元復元」と題し、ステレオ視（または立体視）とは、２台のカメラから撮った画像間の対応関係から三角測量の原理で物体の３次元形状を計算するものであり、ロボットの制御のために画像から３次元情報を抽出する方法として最も基本的な手段の一つであるとして、原理が解説されている。
【０００９】
【非特許文献３】
更に、上記「情報処理」誌の連載であるVol.37 No.8 (１９９６年８月号)の「３．動画像の扱い」と題する同氏の論文には、上記のステレオと同じ原理に基づき、動画像から対象の３次元形状が復元できると記載されている。特に、動画像として画像の系列が与えられていると、対象の運動の追跡が可能になり、２枚の画像だけが与えられた場合より対応点の抽出が容易である旨説明されている。
【００１０】
【発明が解決しようとする課題】
然し乍ら、前述のソナー方式の障害物センサは、周知のように距離分解能と指向性の点で不十分であり、障害物の正確な位置を特定することは困難である。また、車両と障害物との間が至近距離にあると判定された場合でも、車両の移動軌跡上に障害物が存在せず、実際には至近距離の障害物とは言えないという場合がある。即ち、車両の移動軌跡上に障害物が存在する場合における障害物と移動体との間の最短距離を計測対象とすべきであり、以下、この距離を移動至近距離という。あるいは、障害物が凹凸の外面を有する場合には、ビームの径や方向によっては移動至近距離を検出できない場合もあり得る。従って、従来の障害物センサは、このような事項を前提とした上で利用されている。
【００１１】
例えば、前掲の特許文献１においては、車両の走行予想軌跡を表示し、かつ、予想走行軌跡内に存在する障害物を検出し警報する方法が開示されているが、カメラのほかに光学式の障害物センサが必要とされ、コスト高となっている。しかも、機械的にスキャンする機械走査型障害物センサではスキャン（走査）の応答が遅く、複数個の素子を並べた電気走査型障害物センサでは分解能が低いという点を考慮する必要がある。
【００１２】
また、特許文献２においては、カメラで撮影した後方画面内に、自車と障害物の位置関係を図形化して同時に表示する方法が開示されているが、後方画面のどの物体が障害物となるのかは特定されていない。このように、従来は、特定方向の視界を検出するためにカメラ画像が利用されているのに対し、障害物の検出には別途センサが用いられ、これらを組み合わせることが基本とされている。そして、障害物検出センサとしては、前述のように、光、電波あるいは音波を発射し、その反射波から障害物を検出する方式のセンサが採用されることが多い。しかし、基本的には、カメラで撮影した画像のみに基づき、一層安価に製品化し得る装置が要請されている。
【００１３】
これに対し、特許文献３においては、２つの画像に撮像された立体物に対する距離を算出する手段が開示されており、測距センサを設ける必要はないが、画像に撮像された立体物を検出し特定する立体物特定部と、車両の移動データを算出する車両位置算出部と、車両から立体物までの距離を算出する立体物距離算出部によって、車両から立体物までの距離を算出することとしている。具体的には、車両がａ地点にある時に撮像された画像Ａを、車両の移動データを用いてｂ地点から撮像されたように画像変換し、画像Ｃを算出すると説明されているように、結局は、２つの画像のずれと移動データに基づき幾何学的に車両から立体物までの距離を算出することとしている。従って、例えば車両の移動予想軌跡上で最も接近した障害物を検出するという課題に対しては別途対応が必要であり、上記の立体物距離算出部等によって解決し得るか否かは不明である。
【００１４】
また、特許文献４においては、測距センサを設けることなく自車両と基準物との相対位置を検出し得る相対位置検出装置が提案されているが、駐車時の停止状態を前提とし、停止位置マーカと横方向距離計測マーカを用いた位置検出が行われている。従って、車両の移動予想軌跡上で最も接近した障害物の検出という課題に対しては別途検出手段が必要となる。
【００１５】
一方、前述の非特許文献１乃至３に記載の画像処理技術を利用すれば、例えば１個のカメラによっても障害物の３次元形状を復元し得るように構成することができる。従って、このような画像処理技術を、車両等の移動体の周辺を監視する装置に適切に組み入れることも考え得るが、これらの具体的手段については何れの文献にも言及されていない。
【００１６】
そこで、本発明は、別途特別な障害物検出センサを必要とすることなく、移動体周辺の環境の中から移動体の移動予想軌跡上で最も接近した障害物を確実に検出し得る移動体周辺監視装置を提供することを課題とする。
【００１７】
【課題を解決するための手段】
上記課題を解決するため、本発明の移動体周辺監視装置は、請求項１に記載のように、移動体の周辺の環境情報を検出し蓄積する環境情報検出手段と、前記移動体の移動予想軌跡を推定する軌跡推定手段と、該軌跡推定手段が推定した移動予想軌跡を前記環境情報検出手段が検出した環境情報に重合し、前記移動体の移動予想軌跡上に存在する障害物を特定すると共に、前記移動予想軌跡に沿って前記移動体が移動したときに前記移動体の部分と前記障害物の部分とが接触するまでの前記移動体の部分の移動距離を求め、該移動距離に基づいて最初に接触する前記移動体の部分と前記障害物の部分とを特定する移動至近距離計測手段と、前記最初に接触する場合における前記移動体の部分の移動距離と前記移動体の部分及び前記障害物の部分とに基づいて、前記移動体と前記障害物の位置関係を報知する報知手段とを備えることとしたものである。尚、上記の環境情報とは、移動体周辺の物体の大きさ、位置等を含み、移動体には、車両のほか、移動ロボット等を含む。
【００１８】
前記環境情報検出手段は、請求項２に記載のように、前記移動体に搭載し移動体の周辺を撮像し画像情報を出力する少なくとも一つの撮像手段と、前記移動体が第１の状態から第２の状態に移動したときに少なくとも前記第１の状態及び第２の状態で前記撮像手段によって撮像した二つの画像に基づき、前記第１の状態で撮像した画像から特徴点の座標を検出すると共に、前記第２の状態で撮像した画像での前記特徴点に対応する座標を検出する特徴点追跡手段と、少なくとも前記第１の状態及び前記第２の状態における前記移動体の位置及び姿勢を検出する移動状態検出手段と、該移動状態検出手段で検出した前記第１の状態及び前記第２の状態における前記移動体の位置及び姿勢、前記第１の状態で撮像した画像の前記特徴点の座標、及び前記第２の状態で撮像した画像の前記特徴点に対応する座標に基づき前記特徴点の３次元座標を推定する３次元座標推定手段とを備えたものとするとよい。
【００１９】
更に、請求項３に記載のように、前記環境情報検出手段の検出情報を、前記移動体が移動したときの前記周辺環境に対する相対的な位置及び姿勢に応じて補正して出力する情報合成手段と、該情報合成手段の出力情報に基づき、前記移動体の特定位置周辺の環境情報から前記移動体の特定の視界を作成する特定視界作成手段と、該特定視界作成手段が作成した特定の視界を画像に表示する表示手段とを備え、該表示手段が、前記移動体に搭載するモニタ上に、前記特定の視界と共に、前記移動体と前記障害物の位置関係を表示するように構成するとよい。
【００２０】
上記請求項２又は３に記載の移動体周辺監視装置において、この場合には、前記軌跡推定手段は、請求項４に記載のように、前記移動状態検出手段の検出結果に基づき前記移動体の移動予想軌跡を推定するように構成することができる。また、上記請求項１乃至４の何れかに記載の移動体周辺監視装置において、前記報知手段は、請求項５に記載のように、前記移動体に搭載するモニタ上に前記移動体と前記障害物の位置関係を表示する表示装置と、音声で前記移動体と前記障害物の位置関係を伝える音声装置の、少なくとも一方を備えたものとするとよい。この場合において、前記障害物検出手段の検出結果に応じて自動的に、あるいは運転者の指示に応じて、前記特定の視界を作成するように構成することができる。
【００２１】
上記請求項５に記載の移動体周辺監視装置において、請求項６に記載のように、前記移動体が車両であって、前記移動至近距離計測手段が計測した前記最初に接触する場合における前記車両の部分の移動距離に応じて、前記報知手段が、前記表示装置及び音声装置の少なくとも一方の出力を調整するように構成することができる。
【００２２】
あるいは、上記請求項５に記載の移動体周辺監視装置において、請求項７に記載のように、前記表示装置が、前記移動体に対して前記移動予想軌跡上で前記移動体の部分が最初に接触する前記障害物の部分を特定し、該部分を前記モニタ上に表示するように構成することもできる。
【００２３】
【発明の実施の形態】
以下、本発明の移動体周辺監視装置の実施形態について図面を参照して説明する。図１は本発明の一実施形態の全体構成を示すもので、車両に搭載される場合には、例えば図２及び図３に示す駐車案内装置に供される。図１において、本実施形態の移動体周辺監視装置は、移動体（図示せず）の周辺の物体の大きさ、位置等の環境情報を検出し蓄積する環境情報検出手段ＳＲと、移動体の移動予想軌跡を推定する軌跡推定手段ＴＲとを備えている。そして、この軌跡推定手段ＴＲが推定した移動予想軌跡を環境情報検出手段ＳＲが検出した環境情報に重合し、移動体の移動予想軌跡上に存在する障害物を特定すると共に、移動予想軌跡に沿って移動体が移動したときに移動体の部分と障害物の部分とが接触するまでの移動体の部分の移動距離を求め、この移動距離に基づいて最初に接触する移動体の部分と障害物の部分とを特定する移動至近距離計測手段ＭＣを備えている。更に、最初に接触する場合における移動体の部分の移動距離と移動体の部分及び障害物の部分とに基づいて、移動体と障害物の位置関係を報知する報知手段ＤＰとを備えている。
【００２４】
本実施形態の環境情報検出手段ＳＲは、図１に示すように、移動体に搭載し移動体の周辺を撮像し画像情報を出力する撮像手段ＣＤと、移動体が第１の状態から第２の状態に移動したときに少なくとも第１の状態及び第２の状態で撮像手段ＣＤによって撮像した二つの画像に基づき、第１の状態で撮像した画像から特徴点の座標を検出すると共に、第２の状態で撮像した画像での前記特徴点に対応する座標を検出する特徴点追跡手段ＰＦと、少なくとも第１の状態及び第２の状態における移動体の位置及び姿勢を検出する移動状態検出手段ＤＳと、この移動状態検出手段ＤＳで検出した第１の状態及び第２の状態における移動体の位置及び姿勢、第１の状態で撮像した画像の特徴点の座標、及び第２の状態で撮像した画像の前記特徴点に対応する座標に基づき前記特徴点の３次元座標を推定する３次元座標推定手段ＲＣを備えている。尚、撮像手段ＣＤとしては、後述するｃｃｄカメラ等がある。
【００２５】
移動体が車両である場合には、上記の移動状態検出手段ＤＳは、例えば車両の走行速度、操舵状態、旋回状態等の車両運動を検出するセンサによって構成することができ、これについては後述する。また、これらのセンサのうちの、少なくとも一つのセンサ出力に基づき、車両の位置を表わすファクタとして座標、及び車両の姿勢を表わすファクタとして角度を検出することができるが、これについても後述する。そして、上記の軌跡推定手段ＴＲとしては、例えば図２に示す操舵角センサ４の検出操舵角に基づいて車両の移動予想軌跡を推定する手段があり、軌跡推定手段ＴＲが推定する移動予想軌跡としては、例えば車両の前進時及び後退時の移動予想軌跡があり、これについては図７乃至図１４を参照して後述する。
【００２６】
更に、図１に破線で示すように、環境情報検出手段ＳＲの検出情報を、移動体が移動したときの周辺環境に対する相対的な位置及び姿勢に応じて補正して出力する情報合成手段ＣＢと、情報合成手段ＣＢの出力情報に基づき、移動体の特定位置周辺の環境情報から移動体の特定の視界を作成する特定視界作成手段ＳＶと、この特定視界作成手段ＳＶが作成した特定の視界を画像に表示する表示手段ＶＤとを備えた態様としてもよい。この場合には、表示手段ＶＤにより、移動体に搭載するモニタ上に、特定の視界と共に、移動体と障害物の位置関係を表示することになる。
【００２７】
報知手段ＤＰは、移動体に搭載するモニタ上に移動体と障害物の位置関係を表示する表示装置ＶＳと、音声で移動体と障害物の位置関係を伝える音声装置ＡＶの、少なくとも一方を備えており、移動体が車両の本実施形態においては、移動至近距離計測手段ＭＣが計測した移動予想軌跡に沿って車両が移動したときに車両の部分と障害物の部分とが接触するまでの車両の部分の移動距離（移動至近距離）に応じて、表示装置ＶＳ及び音声装置ＡＶの少なくとも一方の出力を調整するように構成される。例えば、移動至近距離の大きさに応じて、バーグラフが増減し、あるいは警告音が増減するように構成することができる。更に、表示装置ＶＳは、車両に対して移動予想軌跡上で車両の部分が最初に接触する障害物の部分を特定し、当該部分をモニタ上に表示する（例えば当該部分にマークを付す）ように構成されている。
【００２８】
更に、図１に破線で示すように、特定視界作成手段ＳＶは、移動至近距離計測手段ＭＣの検出情報に基づき、移動体に最も接近した障害物を含むように特定の視界を作成する構成としてもよい。例えば、移動至近距離の障害物を含む視界のバーチャル画像を自動的に表示するように構成することができる。更に、当該障害物の移動至近距離部分にマークを付すように構成することもできる。
【００２９】
上記の移動体が車両である場合の一例として、図２及び図３に示す駐車案内装置について説明する。図５は本実施形態の駐車案内装置を搭載した車両１の全体構成を示すもので、車両１の後部には、その周辺を撮像し画像を取得するカメラ２（例えばｃｃｄカメラで上記の撮像手段ＣＤを構成する）が装着されており、画像情報が電子制御装置１０に供給される。この電子制御装置１０は、図２に示すように車両１に搭載された筐体中に収容され、図１の移動状態検出手段ＤＳ、３次元座標推定手段ＲＣ、情報合成手段ＣＢ、特定視界作成手段ＳＶ及び軌跡推定手段ＴＲ、移動至近距離計測手段ＭＣが構成されている。また、ステアリングホイール３の操舵角を検出し軌跡推定手段ＴＲの一部を構成する操舵角センサ４が車両１に装着されると共に、シフトレバー５がリバース位置とされたときにオンとなるシフトレバースイッチ６が装着されており、これらの検出舵角及びオン信号が電子制御装置１０に供給される。
【００３０】
更に、本実施形態では、各車輪の回転速度を検出する車輪速度センサ（代表して７で示す）が装着されており、検出車輪速度が電子制御装置１０に供給される。これらの車輪速度センサ７の検出結果に基づき移動体の少なくとも２位置間の移動距離を測定することができる。そして、運転者から視認可能な車両１内の所定位置にモニタ８が配置されており、電子制御装置１０の出力信号に応じてモニタ８に駐車案内用の視覚情報が表示される。即ち、本実施形態においては、モニタ８が図１の表示手段ＶＤ及び表示装置ＶＳとして機能している。更に、車両１内の所定位置にスピーカ９が配置されており、電子制御装置１０の出力信号に応じてスピーカ９から駐車案内用の音声情報が出力される。
【００３１】
図３は、上記の駐車案内装置のシステム構成図であり、電子制御装置１０の構成を示すものである。上記の操舵角センサ４、シフトレバースイッチ６及び車輪速度センサ７は、図示しない入力インターフェースを介して電子制御装置１０内のＣＰＵ１１に接続されており、図示しない出力インターフェースを介して、モニタ８が電子制御装置１０内のスーパーインポーズ部１４に接続され、スピーカ９が音声出力部１５を介してＣＰＵ１１に接続されている。カメラ２は、図示しない入力インターフェースを介して、画像認識部１２、グラフィックス描画部１３及びスーパーインポーズ部１４に接続されている。尚、ＣＰＵ１１、画像認識部１２、グラフィックス描画部１３、スーパーインポーズ部１４及び音声出力部１５は相互にバスバーによって接続されている。尚、メモリ等は示していないが、各部の情報は所定期間蓄積され、適宜読み出し可能に構成されている。
【００３２】
本実施形態においては、車両１周辺の環境を表示するカメラ２の画像信号が画像認識部１２を介してＣＰＵ１１に供給されると共に、車輪速度センサ７等の出力信号がＣＰＵ１１に供給され、ＣＰＵ１１にて演算処理される。このＣＰＵ１１の演算結果に基づき、グラフィックス描画部１３において、カメラ２の画像から抽出される同期信号に応じて３次元のグラフィックスが描画される。また、ＣＰＵ１１においては、駐車案内のための演算処理が行われ、その演算結果に基づくグラフィックスがグラフィックス描画部１３にて描画される。そして、グラフィックス描画部１３にて描画されるグラフィックスとカメラ２の画像とが適宜スーパーインポーズ部１４にて重畳され、モニタ８の画面に描画される。尚、この場合、カメラ画像は表示されず、グラフィック画像のみが表示されるように設定することも可能である。而して、図１の３次元座標推定手段ＲＣ、移動至近距離計測手段ＭＣ、情報合成手段ＣＢ及び特定視界作成手段ＳＶはＣＰＵ１１、画像認識部１２及びグラフィックス描画部１３によって構成されている。
【００３３】
上記の３次元座標推定手段ＲＣにおいては、特徴点追跡手段ＰＦにて、車両１が第１の状態から第２の状態に移動したときに撮像手段ＣＤたるカメラ２によって第１の状態で撮像された画像から特徴点の座標が検出されると共に、第２の状態で撮像された画像での前記特徴点に対応する座標が検出され、移動状態検出手段ＤＳでは、第１の状態及び第２の状態における車両１の位置及び姿勢が検出される。そして、第１の状態及び第２の状態における車両１の位置及び姿勢、第１の状態で撮像された画像の特徴点の座標、及び第２の状態で撮像された画像の前記特徴点に対応する座標に基づき、３次元再構成が行われ、前記特徴点の３次元座標が求められる。
【００３４】
上記の３次元再構成は、例えば前述の非特許文献２に記載のように、画像を用いて三角測量の原理に基づいて行われるものであり、図１７乃至図１９を参照して以下に説明する。図１７において、少なくとも２つのカメラ位置（左右のｏ及びｏ’で示す）から空間中の同一の点Ｍを観測しているとき、各カメラの画像で観測された点ｍ_１，ｍ_２とそれぞれのカメラの光学中心とを結んだ空気中の光線を定義でき、各光線の交点は空間中の点の位置Ｍを表す。ここで、対象視界（Scene）中の３次元の点Ｍは各画像間で同定される必要がある。また、単眼カメラで３次元の再構成を行う場合には、画像中から対象物体の特徴点（Point of Interest）の抽出とその追跡が必要である。更に、カメラの内部パラメータ（スケールファクタ、光軸位置座標等）と外部パラメータ（カメラの配置）は既知である（予め校正されている）ことが必要であり、これらを前提に３次元再構成が行われる。
【００３５】
次に、対象視界の動きからの３次元構造の復元に関し、時系列画像のそれぞれがどの位置及び姿勢で撮影されたものであるかは、デッドレコニングシステムにより対応が取られて校正されていれば、ステレオ対応という手段を使うことができる。但し、３次元視界の中の同一の点が含まれるように撮影された時系列画像において、その画像列の中から同一の点を見つけることが必要である。即ち、これらの同一の点の対応を取ることによって、対象視界の３次元構造を復元することができる。
【００３６】
対象視界からの特徴点の検出に関しては、画像の中から、対象視界の特徴となるべきいくつかの特徴点を検出することになる。この場合の特徴点とは、画像のシーケンスに沿って追跡可能な適切な特徴を有する対象である。このような特徴点の検出には下記［数１］式に示す行列Ａによる自動相関（auto-correlation）を用いることができる。行列Ａは画像中の点ｐ(x,y)を含むウインドウ領域Ｗ中に含まれる画像信号の導関数を平均化するように機能するので、これを用いて最適な特徴点を得る。即ち、追跡に適する特徴点は、一般にはコーナーや直線の交点（例えば、図１８の左側の図におけるコーナーの黒点）であり、この行列はこれらの特徴点を検出するものである。尚、下記［数１］式において、Ｉ(x,y)は画像の関数、Ｉxはｘ軸に沿った画像の勾配、Ｉyはｙ軸に沿った画像の勾配、(x_ｋ _,y_k) は(x,y)を中心としたウインドウＷ中の画像点を夫々表わす。
【００３７】
【数１】

【００３８】
次に、特徴点の追跡（Feature Tracking）に関し、ｕ＝[x,y]^Tが画像点の座標であるような画像列Ｉ(u,t)を考慮する。サンプル周期が十分短ければ微小時刻後の画像ではその輝度値は変化しないと仮定し、下記［数２］式が成り立つ。
【００３９】
【数２】

【００４０】
上記の［数２］式において、δ(u)はモーションフィールド（３次元の動きベクトルの画像への投影）を表す。サンプル周期が十分に短いので、下記［数３］式に示すように、モーションは並進（translation）のみで近似することができる。尚、［数３］式のｄは変移ベクトル（displacement vector）を表わす。
【００４１】
【数３】

【００４２】
選択された各特徴点に対し、また、画像列での追跡された各点のペアに対して、その変移ベクトルｄを計算するため、トラッカー（Tracker）が用いられる。実際には、ノイズの影響により画像のモーションモデルは完全には表せないので［数２］式は十分に機能しない。従って、下記［数４］式に示すように、差の自乗和(Sum of Square Difference)によって変移ベクトルｄを計算する。そして、この［数４］式の残渣を最小にする変移ベクトルｄを求め、その方向に特徴点を追跡する（図１８にこの状態を示す）。尚、下記［数４］式のＷは、点ｕを中心とする特徴点検出のためのウインドウを表わす。
【００４３】
【数４】

【００４４】
図１９はトラッキングの一例を説明するもので、トラック（Tracks）とは画像列中で同一の対象を示す点の集合であり、各トラックの画像点は夫々の３次元点の投影像である。トラックは画像列の中で見失うことは許されないが、追跡する点が画像から外れたり、隠れたり、ノイズ等によって検出できない場合には、トラッキングを停止せざるを得ない。ここでは、最初の画面において検出された特徴点の集合から開始し、前の画像（図１９のImg Ａ）での特徴点は次の画像（図１９のImg Ｂ）で追跡される。画像（Img Ａ）のうちの一部のトラックが停止すると、画像（Img Ｂ）において新しい特徴点を探すことになり、この新しい各特徴点はその後のトラッキングの起点となる。
【００４５】
そして、３次元構造の再構成に関しては、３次元空間中の位置Ｍの点の距離がＺであるとき、その画像上の投影位置をｍとすると下記［数５］式の関係が成り立つ。下記の［数５］式において、距離ｆは焦点距離と呼ばれるが、ここではｆ＝１なる単位長さを用いて簡単化している。このようにモデル化すると、２台のカメラで空間中の（物体の頂点等の）特徴点を撮影する幾何学的な関係は図１７に示すようになる。
【００４６】
【数５】

【００４７】
図１７において、第２のカメラ座標系は第１のカメラ座標系をその原点の周りにＲだけ回転してからＴだけ平行移動して得られる位置にある。尚、このベクトルＴは基線ベクトルと呼ばれる。パラメータ｛Ｔ，Ｒ｝は２つのカメラ視点の相対的な位置姿勢関係を表しているので、１台のカメラを移動しながら撮影することとした場合は、デッドレコニングシステムにより計測することができる。
【００４８】
図１７に示すように、ある３次元空間中の位置Ｍのそれぞれの画像面上の投影像がｍ_１，ｍ_２にあるとする。この状態で、第２のカメラ座標系の原点０’から見た特徴点の方向は第２のカメラ座標系に関してベクトルｍ_２で表されるが、第２のカメラ座標系は第１のカメラ座標系に対してＲだけ回転しているから、第１のカメラ座標系に関してはＲｍ_２である。従って、図１７より下記［数６］式の関係が成立する。
【００４９】
【数６】

【００５０】
上記［数６］式から、距離Ｚ，Ｚ’が下記［数７］式のように計算される。
【００５１】
【数７】

【００５２】
このようにして、２つの視点から画像面上の投影像の座標ｍ_１，ｍ_２から３次元空間上の距離Ｚ，Ｚ’を復元することができる。そして、前述のトラックは対象視界中の特徴点群であるから、全てのトラックに関して上記［数７］式を計算すれば、３次元的な構造を復元することができる。
【００５３】
図１に戻り、移動状態検出手段ＤＳとしては、前述のように車両の走行速度、操舵状態、旋回状態等の車両運動を検出するセンサによってデッドレコニングシステムを構成することができるが、本実施形態においては、車輪速度センサ７によって構成されている。このようにして特定される第１の状態及び第２の状態における車両１の位置及び姿勢と、夫々の状態でカメラ２によって撮像された画像における特徴点の抽出と追跡によって特定される画像間の対応点に基づき、上記のように処理され、撮像領域内に存在する物体が特定され、撮像領域の３次元座標が推定される。
【００５４】
本実施形態においては、車輪速度センサ７によって検出された車輪速度信号に基づき、以下のように、車両１の状態、即ち車両１の位置及び方向（鉛直軸周りの角度）を検出するようにデッドレコニングシステムが構成されている。以下、本実施形態における車両１の状態を検出する方法について説明する。図４は、本実施形態における車両１のモデルを表したものであり、車両１の位置及び方向は、（ｘ，ｙ，θ）の３つの変数により定義される。また、車両１の速度（車速）をＶ、車両１の角速度をωとすると、車両の状態、車速及び角速度の間に次の等式が成立する。
即ち、ｄｘ／ｄｔ＝Ｖ・ｃｏｓθ、ｄｙ／ｄｔ＝Ｖ・ｓｉｎθ、及びｄθ／ｄｔ＝ωとなる。
【００５５】
上記の車速Ｖ及び角速度ωは直接計測することはできないが、車輪速度センサ７によって検出された左右後輪の車輪速度を用いて、次のように求めることができる。即ち、Ｖ＝（Ｖrr＋Ｖrl）／２、及びω＝（Ｖrr−Ｖrl）／Ｌｔとなる。ここで、Ｖrrは右後輪の車輪速度、Ｖrlは左後輪の車輪速度で、Ｌｔは車両１のトレッド長を表す。
【００５６】
上記の式を用いると、時刻ｔにおける車両の状態（ｘ，ｙ，θ）は、ｔ＝０における初期値（ｘ0，ｙ0，θ0）が決まっておれば次のように表すことができる。ｘ＝ｘ0＋∫（ｄｘ／ｄｔ）ｄｔここで、∫はｔ＝０→ｔ＝ｔ。
ｙ＝ｙ0＋∫（ｄｙ／ｄｔ）ｄｔここで、∫はｔ＝０→ｔ＝ｔ。
θ＝θ0＋∫（ｄθ／ｄｔ）ｄｔここで、∫はｔ＝０→ｔ＝ｔ。
即ち、下記［数８］式のようになる。
【００５７】
【数８】

【００５８】
図５は、所定の３次元座標における車両１の移動開始位置の座標及び姿勢（方向）を基準に、移動後の位置の座標と方向を検出し追跡する方法の一例を示す。即ち、図５は（Ｘｏ，Ｙｏ，Ｚｏ）の３次元座標において、Ｘｏ−Ｙｏ平面（Ｚｏ＝０）上で車両が旋回運動する状態を示し、位置（座標（ｘ１，ｙ１））及び方向（角度θ１）で決まる第１の状態から、位置（座標（ｘ２，ｙ２））及び方向（角度θ２）で決まる第２の状態に移動したときの位置関係を特定することができる。本実施形態では、左右後輪の車輪速度センサ７の検出車輪速度に基づき、車両１の位置（座標）及び方向（角度）が検出されるが、操舵角センサ４、ヨーレイトセンサ（図示せず）、距離センサ（図示せず）等を用いることとしてもよく、これらのセンサを組み合わせて用いることとしてもよい。図５は単純円旋回の例であるが、複数円弧や直線が組み合わされた軌跡に対し、上記のように検出された車両１の座標と方向に基づき、同様に追跡することができる。
【００５９】
一方、図１に記載の３次元座標推定手段ＲＣにおいて推定された３次元座標が、図６に破線の格子で示すように、（Ｘ，Ｙ，Ｚ）の３次元座標を基準とする３次元座標マップとしてメモリに格納されている。尚、図６に破線の格子で示した３次元座標マップの中には、カメラ２で撮像された他の車両、障害物、建物等も存在するが、これらは図示を省略している。この３次元座標マップに対し、図５の３次元座標の対応が行われる。即ち、図６に破線の格子で示す３次元座標マップの中に、図５に示す車両１が配置され、３次元座標マップでの車両１の位置（座標）、及び姿勢、即ち方向（角度）が特定される。
【００６０】
上記のように特定された３次元座標マップにおける車両１の位置（座標）及び方向（角度）に基づき、３次元座標に存在する障害物との位置関係が明確になる。例えば、図２のステアリングホイール３が中立位置にあって、図７に示すように車両１が前方に直進している場合において、その前方に障害物ＯＢが存在するときには、車両１がそのまま前進すると図７のバンパ部分１ａが障害物ＯＢの左方角部に接触することになり、両者間の距離ｄ１が移動至近距離となることが分かる。
【００６１】
これに対し、図２のステアリングホイール３が操作され、例えば図８に示すように車両１が旋回中心Ｃを中心に半径Ｒで旋回している場合には、その前方に障害物ＯＢが存在しても、車両１がそのまま旋回すれば車両１のどの部分も障害物ＯＢに接触しないことが分かる。このことは、旋回中心Ｃを中心とする障害物ＯＢのどの部分の円弧（図８には最小半径の円弧をＡ１で示す）も車両１とは重合しないことから、図８に示す位置関係に基づき推定することができる。尚、旋回中心Ｃは車両１の後車軸の延長上にあり、その位置はステアリングホイール３の操舵角によって決まる。
【００６２】
一方、図９に示すように車両１が旋回中心Ｃを中心に半径Ｒで旋回し、その前方の障害物ＯＢが旋回中心Ｃを中心とする円弧Ａ２（半径Ｒａ）上を相対移動する場合、即ち円弧Ａ２が車両１と重合する場合には、車両１がそのまま旋回すれば車両１のバンパ部分１ｂが障害物ＯＢに接触することになる。この場合には、両者間の距離ｄ２が移動至近距離となる。
【００６３】
図９から明らかなように、旋回中心Ｃを中心とし障害物ＯＢを通過する円弧と、旋回中心Ｃを中心とし車両１を通過する円弧とが一致する場合が、車両１と障害物ＯＢが接触する場合であり、両者間の距離が最小となる障害物ＯＢの円弧をＡ２として特定することができる。従って、図１０に示す関係となり、車両１のバンパ部分１ｂと障害物ＯＢとの間の距離が移動至近距離ｄ２となり、車両１のバンパ部分１ａと障害物ＯＢ間の直線距離ｄ１より長くなる。尚、この移動至近距離ｄ２はＣＰＵ１１による演算処理によって求められる。
【００６４】
上記の図７乃至図１０においては説明の便宜上Ｘ−Ｙ平面上で説明したが、本実施形態では３次元座標に存在する障害物との位置関係が明確になることから、Ｚ軸方向での状況も考慮に入れた移動至近距離を測定することができる。例えば、図１１に示すように、車両１のバンパ部分１ａと障害物たる他の車両ＯＶのバンパ部分Ｏｂとの間の水平距離はｄ３であるが、他の車両ＯＶの後端部Ｏａとの間の水平距離はｄ４（＜ｄ３）という状況が生じ得る。従って、仮に車両１のバンパ部分１ａと他の車両ＯＶとの間が距離ｄ３とされると、距離ｄ４に達した時点で車両１の前方上部が他の車両ＯＶの後端部Ｏａに接触することになるが、本実施形態ではこのような事態を回避することができる。
【００６５】
上記のように計測された車両１と障害物ＯＢ（他の車両ＯＶ）間の移動至近距離に基づき、図１の報知手段ＤＰによって以下のように報知される。図１の表示装置ＶＳを構成する図２のモニタ８には、カメラ２で撮像された画像が表示されるが、本実施形態においては、図１２に示すように、障害物（図１２では他の車両ＯＶ）までの移動至近距離が画面内のバーグラフ８ｂによって表示されている。このバーグラフ８ｂは、図１２と図１３との比較から明らかなように、移動至近距離の大きさ（接近程度）に応じて変化し、移動至近距離が小さくなる程バーグラフ８ｂの濃い部分が減少するように構成されている。尚、これに加え、バーグラフ８ｂに距離目盛りを付してもよいし、バーグラフ８ｂに代えて距離数値を直接表示することとしてもよい。
【００６６】
上記の表示に加え、図１２及び図１３に示すように、「移動至近距離」の測定対象部分（そのときの状態を維持して車両１を移動させたときに接触する部分）を特定するマーカ８ｃを、モニタ８の画面上に表示することとしてもよい。また、メッセージ８ｄをモニタ８の画面上に表示することとしてもよい。更に、図１の音声装置ＡＶにより、例えば、移動至近距離の値に応じて、音声による報知あるいはビープ音による報知を行うこととしてもよい。これらの報知手段は一つだけでも、あるいは複数を組み合わせて用いてもよい。
【００６７】
更に、図１４に示すように、バーチャル画像を用いて上記の報知を行うこともできる。図１４には自車たる車両１の一部（図１４では後部）と障害物ＯＢを表す図形が表示されると共に、バーグラフ８ｂ及びマーカ８ｃも表示されているので、車両１と障害物ＯＢとの関係を確認することができる。もちろん、図１２及び図１３と同様、メッセージ８ｄ並びに音声及びビープ音による報知と組み合わせてもよい。上記のバーチャル画像は、図１に破線で示すように、特定視界作成手段ＳＶ等を構成することによって実現でき、以下これについて説明する。
【００６８】
即ち、特定視界作成手段ＳＶによって、周辺状況を含む３次元座標内における車両１の任意の位置で、任意の方向に向けてカメラが装着されたと仮定した場合の特定の視界が作成され、この仮想カメラから見た視界の画像が、図１の表示手段ＶＤ（図２のモニタ８）に表示される。この場合において、例えば図１５に示すように、車両１の後方に仮想カメラ２ａが配置されているとしたときに、移動後の車両１から見た３次元座標マップ（図１５の破線の格子で示す）において、どの領域が仮想カメラ２ａの視界に入っているかが分かる。換言すれば、図１５にＳａで示す領域が、仮想カメラ２ａの視界に入り、モニタ８にバーチャル画像として表示し得る領域（以下、表示視界という）である。尚、仮想カメラを前方あるいは側方に配置する場合も同様である。
【００６９】
上記図１５の例では、実際のカメラ２の画角と類似する視界が設定されているが、図１６の仮想カメラ２ｅのように、１８０度の表示視界Ｓｅを設定することもできる。これにより、実際のカメラ２では視界に入らない部分をも視界に入れたバーチャル画像を表示するように構成することもできる。更に、図示は省略するが、車両１の上空部分に仮想カメラを配置して３６０度の表示視界を設定し、この視界のバーチャル画像を表示するように構成することもできる。尚、図１５及び図１６においても、破線の格子で示した３次元座標マップの中には、カメラ２で撮像された他の車両、障害物、建物等も存在するが、これらは図示を省略している。
【００７０】
上記のように形成されるバーチャル画像は、車両１の移動、即ち仮想カメラ２ａ等の移動に伴う視界の移動に応じて変化することになるが、本実施形態においては、移動中に撮像した障害物を含むバーチャル画像を、状況に応じて適宜（移動後も）表示し得るように構成されている。例えば、図１４のバーチャル画像は、車両１が一旦前方まで前進した後に後退する際に、カメラ２によって撮像された環境情報が３次元座標マップとしてメモリ（図示せず）に格納されているので、この３次元座標マップでの車両１の位置（座標）及び姿勢（方向）が特定される。そして、３次元座標マップにおいて、例えば車両１と障害物ＯＢとの距離が所定距離未満となったと判定されると、自動的に、図１４に示すバーチャル画像がモニタ８に表示される。あるいは、運転者の指示に応じてバーチャル画像に切り替えるように構成することもできる。
【００７１】
尚、図２及び図１２乃至図１６は、カメラ２が車両１の後方に配置された場合の実施形態に係り、図７乃至図１１は、カメラ２が車両１の前方に配置された場合の実施形態に係るものであることから明らかなように、前後いずれの障害物に対しても同様に処理することができる。また、上記の実施形態においては１台のカメラ２のみで３次元座標マップを作成することとしたが、複数台のカメラを用いてもよい。
【００７２】
【発明の効果】
本発明は上述のように構成されているので以下に記載の効果を奏する。即ち、請求項１に記載の移動体周辺監視装置においては、別途特別な障害物検出センサを必要とすることなく、例えば画像を用いて障害物を検出し、移動体のどの部分と障害物のどの部分が接触するかを判定することができ、移動体の移動予想軌跡上の移動至近距離を適切に計測することができる。また、低コストで提供でき、製造時の搭載性も向上する。
【００７３】
前記環境情報検出手段は、請求項２に記載のように構成すれば、一つの撮像手段によっても容易且つ確実に環境情報を検出することができ、安価な装置を提供することができる。
【００７４】
また、請求項３に記載のように構成すれば、移動体における所望の位置での視界を確保し、移動体と障害物の位置関係を適切に画像表示することができる。
【００７５】
前記軌跡推定手段は、請求項４に記載のように構成すれば、別途特別のセンサを用いることなく、移動体の移動予想軌跡を容易且つ確実に推定することができ、安価な装置を提供することができる。
【００７６】
前記報知手段は、請求項５に記載のように構成すれば、運転者は移動体と障害物の位置関係を容易に確認することができる。そして、請求項６に記載のように構成すれば、車両の移動至近距離に応じた報知が行われるので、運転者は移動体のどの部分と障害物のどの部分が接触するかを容易に判定することができる。そして、前記表示装置を請求項７に記載のように構成すれば、例えば、視野内で注意すべき障害物の部分を画面上にマーカで表示することができるので、運転者は移動体のどの部分と障害物のどの部分が接触するかを容易に判定することができる。従って、例えば駐車案内装置に適用すれば、報知手段を介して障害物を確実に回避するように案内することができる。
【図面の簡単な説明】
【図１】本発明の移動体周辺監視装置の一実施形態の構成を示すブロック図である。
【図２】本発明の移動体周辺監視装置を駐車案内装置に適用したときの一実施形態として、駐車案内装置を搭載した車両を透視して示す斜視図である。
【図３】本発明の一実施形態に係る駐車案内装置の構成を示すブロック図である。
【図４】本発明の一実施形態における車両モデルの平面図である。
【図５】本発明の一実施形態において、所定の３次元座標における車両の移動開始位置の座標及び方向を基準に、移動後の位置の座標と方向を検出し追跡する方法の一例を示す平面図である。
【図６】本発明の一実施形態において、３次元座標マップでの車両の位置（座標）及び姿勢（方向）を特定する状態の一例を示す平面図である。
【図７】本発明の一実施形態において、車両が直進している場合の前方の障害物に対する移動至近距離の一例を示す平面図である。
【図８】本発明の一実施形態において、車両が旋回している場合に前方の障害物に接触しない状態の一例を示す平面図である。
【図９】本発明の一実施形態において、車両が旋回している場合に前方の障害物に接触するときの移動至近距離の一例を示す平面図である。
【図１０】図９の車両と障害物との間の移動至近距離部分を拡大して示す平面図である。
【図１１】本発明の一実施形態において、垂直方向の位置関係でみたときの車両と障害物との間の移動至近距離の一例を示す正面図である。
【図１２】本発明の一実施形態において、車両の後方のカメラ画像に障害物までの移動至近距離を表示した一例を示す正面図である。
【図１３】本発明の一実施形態において、車両の後方のカメラ画像に障害物までの移動至近距離を表示した他の例を示す正面図である。
【図１４】本発明の一実施形態において、車両の後方のカメラ画像に障害物までの移動至近距離及び両者の位置関係を表示したバーチャル画像の一例を示す正面図である。
【図１５】本発明の一実施形態において、車両の後方に仮想カメラが配置されたときの表示視界を示す平面図である。
【図１６】本発明の一実施形態において、車両の後方上部に仮想カメラが配置されたときの１８０度の表示視界を示す平面図である。
【図１７】本発明の一実施形態における三次元再構成に関し、２台のカメラで空間中の物体の特徴点を撮影するときの幾何学的な関係を示す説明図である。
【図１８】本発明の一実施形態における三次元再構成に関し、特徴点の追跡の一例を示す説明図である。
【図１９】本発明の一実施形態における三次元再構成に関し、トラッキング状況の一例を示す説明図である。
【符号の説明】
ＳＲ環境情報検出手段，ＣＤ撮像手段，ＤＳ移動状態検出手段，
ＲＣ３次元座標推定手段，ＣＢ情報合成手段，
ＳＶ特定視界作成手段，ＶＤ表示手段，１車両，２カメラ，
３ステアリングホイール，４操舵角センサ，
５シフトレバー，６シフトレバースイッチ，７車輪速度センサ，
８モニタ，９スピーカ，１０電子制御装置

Claims

移動体の周辺の環境情報を検出し蓄積する環境情報検出手段と、前記移動体の移動予想軌跡を推定する軌跡推定手段と、該軌跡推定手段が推定した移動予想軌跡を前記環境情報検出手段が検出した環境情報に重合し、前記移動体の移動予想軌跡上に存在する障害物を特定すると共に、前記移動予想軌跡に沿って前記移動体が移動したときに前記移動体の部分と前記障害物の部分とが接触するまでの前記移動体の部分の移動距離を求め、該移動距離に基づいて最初に接触する前記移動体の部分と前記障害物の部分とを特定する移動至近距離計測手段と、前記最初に接触する場合における前記移動体の部分の移動距離と前記移動体の部分及び前記障害物の部分とに基づいて、前記移動体と前記障害物の位置関係を報知する報知手段とを備えたことを特徴とする移動体周辺監視装置。
前記環境情報検出手段は、前記移動体に搭載し前記移動体の周辺を撮像し画像情報を出力する少なくとも一つの撮像手段と、前記移動体が第１の状態から第２の状態に移動したときに少なくとも前記第１の状態及び第２の状態で前記撮像手段によって撮像した二つの画像に基づき、前記第１の状態で撮像した画像から特徴点の座標を検出すると共に、前記第２の状態で撮像した画像での前記特徴点に対応する座標を検出する特徴点追跡手段と、少なくとも前記第１の状態及び前記第２の状態における前記移動体の位置及び姿勢を検出する移動状態検出手段と、該移動状態検出手段で検出した前記第１の状態及び前記第２の状態における前記移動体の位置及び姿勢、前記第１の状態で撮像した画像の前記特徴点の座標、及び前記第２の状態で撮像した画像の前記特徴点に対応する座標に基づき前記特徴点の３次元座標を推定する３次元座標推定手段とを備えたことを特徴とする請求項１記載の移動体周辺監視装置。
前記環境情報検出手段の検出情報を、前記移動体が移動したときの前記周辺環境に対する相対的な位置及び姿勢に応じて補正して出力する情報合成手段と、該情報合成手段の出力情報に基づき、前記移動体の特定位置周辺の環境情報から前記移動体の特定の視界を作成する特定視界作成手段と、該特定視界作成手段が作成した特定の視界を画像に表示する表示手段とを備え、該表示手段が、前記移動体に搭載するモニタ上に、前記特定の視界と共に、前記移動体と前記障害物の位置関係を表示するように構成したことを特徴とする請求項２記載の移動体周辺監視装置。
前記軌跡推定手段が、前記移動状態検出手段の検出結果に基づき前記移動体の移動予想軌跡を推定するように構成したことを特徴とする請求項２又は３記載の移動体周辺監視装置。
前記報知手段が、前記移動体に搭載するモニタ上に前記移動体と前記障害物の位置関係を表示する表示装置と、音声で前記移動体と前記障害物の位置関係を伝える音声装置の、少なくとも一方を備えたことを特徴とする請求項１、２，３又は４記載の移動体周辺監視装置。
前記移動体が車両であって、前記移動至近距離計測手段が計測した前記最初に接触する場合における前記車両の部分の移動距離に応じて、前記報知手段が、前記表示装置及び音声装置の少なくとも一方の出力を調整するように構成したことを特徴とする請求項５記載の移動体周辺監視装置。
前記表示装置が、前記移動体に対して前記移動予想軌跡上で前記移動体の部分が最初に接触する前記障害物の部分を特定し、該部分を前記モニタ上に表示するように構成したことを特徴とする請求項５記載の移動体周辺監視装置。