JP4374850B2 - Moving object periphery monitoring device - Google Patents

Description

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

【００３９】
次に、特徴点の追跡（Feature Tracking）に関し、u＝[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）が決まっておれば下記［数８］式のように表すことができる。
【００５８】
【数８】

【００５９】
図５は、所定の３次元座標における車両１の移動開始位置の座標及び姿勢（方向）を基準に、移動後の位置の座標と方向を検出し追跡する方法の一例を示す。即ち、図５は（Ｘｏ，Ｙｏ，Ｚｏ）の３次元座標において、Ｘｏ−Ｙｏ平面（Ｚｏ＝０）上で車両が旋回運動する状態を示し、位置（座標（ｘ１，ｙ１））及び方向（角度θ１）で決まる第１の状態から、位置（座標（ｘ２，ｙ２））及び方向（角度θ２）で決まる第２の状態に移動したときの位置関係を特定することができる。本実施形態では、左右後輪の車輪速度センサ７の検出車輪速度に基づき、車両１の位置（座標）及び方向（角度）が検出されるが、操舵角センサ４、ヨーレイトセンサ（図示せず）、距離センサ（図示せず）等を用いることとしてもよく、これらのセンサを組み合わせて用いることとしてもよい。図５は単純円旋回の例であるが、複数円弧や直線が組み合わされた軌跡に対し、上記のように検出された車両１の座標と方向に基づき、同様に追跡することができる。
【００６０】
一方、図１に記載の３次元座標推定手段ＲＣにおいて推定された３次元座標が、図６に破線の格子で示すように、（Ｘ，Ｙ，Ｚ）の３次元座標を基準とする３次元座標マップとしてメモリに格納されている。尚、図６に破線の格子で示した３次元座標マップの中には、カメラ２で撮像された他の車両、障害物、建物等も存在するが、これらは図示を省略している。この３次元座標マップに対し、図５の３次元座標の対応が行われる。即ち、図６に破線の格子で示す３次元座標マップの中に、図５に示す車両１が配置され、３次元座標マップでの車両１の位置（座標）、及び姿勢、即ち方向（角度）が特定される。
【００６１】
上記のように特定された３次元座標マップにおける車両１の位置（座標）及び方向（角度）に基づき、３次元座標に存在する障害物との位置関係が明確になる。例えば、図２のステアリングホイール３が中立位置にあって、図７に示すように車両１が前方に直進している場合において、その前方に障害物ＯＢが存在するときには、車両１がそのまま前進すると図７のバンパ部分１ａが障害物ＯＢの左方角部に接触することになり、両者間の距離ｄ１が移動至近距離となることが分かる。
【００６２】
これに対し、図２のステアリングホイール３が操作され、例えば図８に示すように車両１が旋回中心Ｃを中心に半径Ｒで旋回している場合には、その前方に障害物ＯＢが存在しても、車両１がそのまま旋回すれば車両１のどの部分も障害物ＯＢに接触しないことが分かる。このことは、旋回中心Ｃを中心とする障害物ＯＢのどの部分の円弧（図８には最小半径の円弧をＡ１で示す）も車両１とは重合しないことから、図８に示す位置関係に基づき推定することができる。尚、旋回中心Ｃは車両１の後車軸の延長上にあり、その位置はステアリングホイール３の操舵角によって決まる。
【００６３】
一方、図９に示すように車両１が旋回中心Ｃを中心に半径Ｒで旋回し、その前方の障害物ＯＢが旋回中心Ｃを中心とする円弧Ａ２（半径Ｒａ）上を相対移動する場合、即ち円弧Ａ２が車両１と重合する場合には、車両１がそのまま旋回すれば車両１のバンパ部分１ｂが障害物ＯＢに接触することになる。この場合には、両者間の距離ｄ２が移動至近距離となる。
【００６４】
図９から明らかなように、旋回中心Ｃを中心とし障害物ＯＢを通過する円弧と、旋回中心Ｃを中心とし車両１を通過する円弧とが一致する場合が、車両１と障害物ＯＢが接触する場合であり、両者間の距離が最小となる障害物ＯＢの円弧をＡ２として特定することができる。従って、図１０に示す関係となり、車両１のバンパ部分１ｂと障害物ＯＢとの間の距離が移動至近距離ｄ２となり、車両１のバンパ部分１ａと障害物ＯＢ間の直線距離ｄ１より長くなる。尚、この移動至近距離ｄ２はＣＰＵ１１による演算処理によって求められる。
【００６５】
上記の図７乃至図１０においては説明の便宜上Ｘ−Ｙ平面上で説明したが、本実施形態では３次元座標に存在する障害物との位置関係が明確になることから、Ｚ軸方向での状況も考慮に入れた移動至近距離を測定することができる。例えば、図１１に示すように、車両１のバンパ部分１ａと障害物たる他の車両ＯＶのバンパ部分Ｏｂとの間の水平距離はｄ３であるが、他の車両ＯＶの後端部Ｏａとの間の水平距離はｄ４（＜ｄ３）という状況が生じ得る。従って、仮に車両１のバンパ部分１ａと他の車両ＯＶとの間が距離ｄ３とされると、距離ｄ４に達した時点で車両１の前方上部が他の車両ＯＶの後端部Ｏａに接触することになるが、本実施形態ではこのような事態も回避することができる。即ち、自車両（車両１）が移動するときの移動空間予想軌跡を推定し、これを複数の検出画像のうちの、移動空間予想軌跡が適合する所定の画像に重合させて、移動空間予想軌跡内に存在する障害物を検出することができる。ここで、移動空間予想軌跡は、自車両（車両１）が移動したときに通過すると予想される空間の軌跡である。従って、移動空間予想軌跡は３次元となるが、これも基本的には図２の操舵角センサ４の検出結果に基づいて演算することができ、自車両（車両１）が所定の操舵角でステアリング操作が行なわれたときに自車両（車両１）の外郭部分が通過する空間が予測演算される。尚、この演算処理については、前掲の特許文献４等に記載されているので説明は省略する。
【００６６】
而して、上記のように推定された車両１の移動空間予想軌跡が、それまでに検出された複数の画像のうち、この移動空間予想軌跡に適合する所定の画像に重合され、この所定の画像上で車両１の移動空間予想軌跡内に存在する障害物が検出され、図１の表示手段ＶＳに以下のように表示される。即ち、図１の表示手段ＶＳを構成する図２のモニタ８に、カメラ２で撮像された画像のうち、車両１の移動空間予想軌跡内に存在する障害物を含む画像が表示されると共に、強調手段ＡＴによる表示等が重合して表示される。
【００６７】
例えば、図１２に示すように、他の車両ＯＶ及びＰＶ等が存在する駐車領域に車両１の駐車作動を行う場合には、図１３に示すように車両１の移動空間予想軌跡８ａ及び８ｂ（図１３の点描領域）が、図１の対象特定手段ＤＯによって注意喚起対象として特定され、濃淡及び色彩変化の少なくとも一方の変調によって強調される。また、障害物（図１３では他の車両ＰＶの一部）が強調されて表示される。このとき、そのまま後退すれば接触すると予想される車両ＰＶの一部は図１の対象特定手段ＤＯによって特定注意喚起対象として特定され、更に当該部分に濃淡もしくは色彩を変化させたマーカ８ｃが表示される。例えば、移動空間予想軌跡８ａ及び８ｂ内に侵入している障害物の領域（図１３の格子領域）の画像濃度が濃く設定され、そうでない領域は薄く設定され、両領域が明確に識別可能となる。あるいは、障害物の実際の色彩に対して、そのカラーパターンを変化（色彩変化）させることとしても、両領域が明確に識別可能となる（以下における濃淡及び色彩変化についても同様）。
【００６８】
そして、更に車両１が後退し他の車両ＯＶに接近すると、図１４に示すように、色彩を施したフレーム８ｄが画像周囲に表示され、これがブリンクする。また、そのまま後退すれば接触すると予想される車両ＯＶの一部が特定注意喚起対象とされ、この部分に対し、濃淡及び色彩変化の少なくとも一方の変調によって強調するためのマーカ８ｃが表示され、更に、この位置を示すメッセージ８ｅ（例えば「左後方です」）が表示される。この表示に加え、図１の音声装置ＡＶにより、例えば、移動至近距離の値に応じて、音声による報知あるいはビープ音による報知を行うこととしてもよい。尚、説明の便宜上省略したが、図１３に示すようにマーカ８ｃが表示される状態においても、マーカ８ｃと共に画像周囲にフレーム８ｄが表示され、これがブリンクすると共に、図１４と同様のメッセージ（図１３では省略）が表示される。また、図１５に示すように、特定注意喚起対象（車両ＯＶの一部）に対し、所定の指示マーク（例えば図１５の矢印８ｆ）を重合し、この矢印８ｆを濃淡及び色彩変化の少なくとも一方の変調によって強調することとしてもよい。
【００６９】
尚、そのまま後退すれば接触すると予想される上記の特定注意喚起対象のみではなく、移動空間予想軌跡内に存在する車両ＯＶを注意喚起対象とし、図１６に示す車両ＯＶの最外輪郭線８ｇ、及び図１７に示す車両ＯＶの主要輪郭線８ｈの少なくとも一方を、濃淡及び色彩変化の少なくとも一方の変調によって強調することとしてもよい。ここで、主要輪郭線８ｈとしては、例えば所定長さ以上の長さを有する輪郭線とし、これを選択して表示するように構成することができる。
【００７０】
あるいは、図１８及び図１９に示すように、車両ＯＶに対し半透明スクリ−ン８ｊ（図１８の点描領域）もしくは不透明スクリ−ン８ｋ（図１９のハッチング領域）を重合し、これら半透明スクリ−ン８ｊもしくは不透明スクリ−ン８ｋを画像の濃淡及び色彩変化の少なくとも一方の変調によって強調することとしてもよい。尚、図示は省略するが、図１３の表示画像における移動空間予想軌跡内の領域あるいは車両ＯＶそのもの（即ち、原画像）を、画像の濃淡及び色彩変化の少なくとも一方の変調によって強調することとしてもよい。
【００７１】
更に、図２２に示すように、バーチャル画像を用いて上記表示による報知を行うこともできる。図２２は自車両たる車両１の一部（図２２では後部）と障害物ＯＢを表す図形が表示されると共に、濃淡もしくは色彩を変化させた強調パターン８ｐ（図２２の点描領域）及びマーカ８ｃ（図２２の太線部分）も重畳表示されているので、車両１と障害物ＯＢとの関係を確認することができる。もちろん、アナウンス（音声）、アラーム音あるいはビープ音による報知と組み合わせることとしてもよい。上記のバーチャル画像は、図１に破線で示すように、特定視界作成手段ＳＶ等を構成することによって実現でき、以下、これについて説明する。
【００７２】
即ち、特定視界作成手段ＳＶによって、周辺状況を含む３次元座標内における車両１の任意の位置で、任意の方向に向けてカメラが装着されたと仮定した場合の特定の視界が作成され、この仮想カメラから見た視界の画像が、図１の表示手段ＶＳ（図２のモニタ８）に表示される。この場合において、例えば図２０に示すように、車両１の後方に仮想カメラ２ａが配置されているとしたときに、移動後の車両１から見た３次元座標マップ（図２０の破線の格子で示す）において、どの領域が仮想カメラ２ａの視界に入っているかが分かる。換言すれば、図２０にＳａで示す領域が、仮想カメラ２ａの視界に入り、モニタ８にバーチャル画像として表示し得る領域（以下、表示視界という）である。尚、仮想カメラを前方あるいは側方に配置する場合も同様である。
【００７３】
上記図２０の例では、実際のカメラ２の画角と類似する視界が設定されているが、図２１の仮想カメラ２ｅのように、１８０度の表示視界Ｓｅを設定することもできる。これにより、実際のカメラ２では視界に入らない部分をも視界に入れたバーチャル画像を表示するように構成することもできる。更に、図示は省略するが、車両１の上空部分に仮想カメラを配置して３６０度の表示視界を設定し、この視界のバーチャル画像を表示するように構成することもできる。尚、図２０及び図２１においても、破線の格子で示した３次元座標マップの中には、カメラ２で撮像された他の車両、障害物、建物等も存在するが、これらは図示を省略している。
【００７４】
このように形成されるバーチャル画像は、車両１の移動、即ち仮想カメラ２ａ等の移動に伴う視界の移動に応じて変化することになるが、本実施形態においては、移動中に撮像した障害物を含むバーチャル画像を、状況に応じて適宜（移動後も）表示し得るように構成されている。例えば、車両１と障害物ＯＢとの距離が所定距離未満となったと判定されると、自動的に、図２２に示すバーチャル画像がモニタ８に表示される。あるいは、運転者の指示に応じてバーチャル画像に切り替えるように構成することもできる。
【００７５】
以上のように、上記の各実施形態によれば、例えば障害物が存在する画像に自動的に切り替えて表示することができ、また、画像中の所定の視界範囲内の障害物に関し、接触が予想される部分を明示することができる。従って、車両１の運転者は単に障害物の接近を認識することができるだけでなく、その障害物が車両１に対しどの方向に存在しているかを短時間で認識することができる。しかも、移動空間予想軌跡内に障害物が存在する場合には、画像中の障害物の最外輪郭線や主要輪郭線、あるいは障害物そのものの濃淡や色彩変化によって強調して表示されるので、注意喚起すべき対象を容易且つ迅速に把握することができる。
【００７６】
尚、図２及び図１２乃至図２２は、カメラ２が車両１の後方に配置された場合の実施形態に係り、図７乃至図１１は、カメラ２が車両１の前方に配置された場合の実施形態に係るものであることから明らかなように、前後いずれの障害物に対しても同様に処理することができる。また、上記の実施形態においては１台のカメラ２のみで３次元座標マップを作成することとしたが、もちろん複数台のカメラを用いることとしてもよい。
【００７７】
【発明の効果】
本発明は上述のように構成されているので以下に記載の効果を奏する。即ち、請求項１に記載の移動体周辺監視装置によれば、画像を用いて移動体の移動空間予想軌跡内に存在する特徴点を有し移動体に接触すると予想される部分を含む障害物を検出することができ、移動体のどの部分と障害物のどの部分が接触するかを容易に判定することができる。しかも、障害物の当該部分が特定注意喚起対象として強調して表示されるので、容易且つ迅速に把握することができる。
【図面の簡単な説明】
【図１】本発明の移動体周辺監視装置の一実施形態の構成を示すブロック図である。
【図２】本発明の移動体周辺監視装置を駐車案内装置に適用したときの一実施形態として、駐車案内装置を搭載した車両を透視して示す斜視図である。
【図３】本発明の一実施形態に係る駐車案内装置の構成を示すブロック図である。
【図４】本発明の一実施形態における車両モデルの平面図である。
【図５】本発明の一実施形態において、所定の３次元座標における車両の移動開始位置の座標及び方向を基準に、移動後の位置の座標と方向を検出し追跡する方法の一例を示す平面図である。
【図６】本発明の一実施形態において、３次元座標マップでの車両の位置（座標）及び姿勢（方向）を特定する状態の一例を示す平面図である。
【図７】本発明の一実施形態において、車両が直進している場合の前方の障害物に対する移動至近距離の一例を示す平面図である。
【図８】本発明の一実施形態において、車両が旋回している場合に前方の障害物に接触しない状態の一例を示す平面図である。
【図９】本発明の一実施形態において、車両が旋回している場合に前方の障害物に接触するときの移動至近距離の一例を示す平面図である。
【図１０】図９の車両と障害物との間の移動至近距離部分を拡大して示す平面図である。
【図１１】本発明の一実施形態において、垂直方向の位置関係でみたときの車両と障害物との間の移動至近距離の一例を示す正面図である。
【図１２】本発明の一実施形態において、車両の駐車作動を行う場合の一例を示す平面図である。
【図１３】本発明の一実施形態において、図１２に示すように車両の駐車作動を行う場合に、移動空間予想軌跡を強調して示すと共に、障害物を強調して表示した状態の画像の正面図である。
【図１４】本発明の一実施形態において、表示画像のフレームをブリンクすると共に、そのまま後退すれば接触すると予想される車両の一部に対し、接触予想部分を濃淡及び色彩変化の少なくとも一方の変調によって強調する状態を示す画像の正面図である。
【図１５】本発明の一実施形態において、注意喚起対象として車両の一部に対し、所定の指示マークを重合し、これを濃淡及び色彩変化の少なくとも一方の変調によって強調する状態を示す画像の正面図である。
【図１６】本発明の一実施形態において、注意喚起対象として車両に対し、その最外輪郭線を、濃淡及び色彩変化の少なくとも一方の変調によって強調する状態を示す画像の正面図である。
【図１７】本発明の一実施形態において、注意喚起対象として車両に対し、その主要輪郭線を、濃淡及び色彩変化の少なくとも一方の変調によって強調する状態を示す画像の正面図である。
【図１８】本発明の一実施形態において、注意喚起対象として車両に対し、半透明スクリ−ンを重合し、この半透明スクリ−ンを濃淡及び色彩変化の少なくとも一方の変調によって強調する状態を示す画像の正面図である。
【図１９】本発明の一実施形態において、注意喚起対象として車両に対し、不透明スクリ−ンを重合し、この不透明スクリ−ンを濃淡及び色彩変化の少なくとも一方の変調によって強調する状態を示す画像の正面図である。
【図２０】本発明の一実施形態において、車両の後方に仮想カメラが配置されたときの表示視界を示す平面図である。
【図２１】本発明の一実施形態において、車両の後方上部に仮想カメラが配置されたときの１８０度の表示視界を示す平面図である。
【図２２】本発明の一実施形態において、車両の後方のカメラ画像に障害物を表示したバーチャル画像における注意喚起対象に対し、濃淡及び色彩変化の少なくとも一方の変調によって強調する状態を示す平面図である。
【図２３】本発明の一実施形態における三次元再構成に関し、２台のカメラで空間中の物体の特徴点を撮影するときの幾何学的な関係を示す説明図である。
【図２４】本発明の一実施形態における三次元再構成に関し、特徴点の追跡の一例を示す説明図である。
【図２５】本発明の一実施形態における三次元再構成に関し、トラッキング状況の一例を示す説明図である。
【符号の説明】
ＳＲ 環境情報検出手段， ＣＤ 撮像手段， ＤＳ 移動状態検出手段，
ＲＣ ３次元座標推定手段， ＳＶ 特定視界作成手段， ＶＳ 表示手段，
ＴＲ 移動空間予想軌跡推定手段， ＯＪ 障害物検出手段，
ＤＯ 対象特定手段， 強調手段ＡＴ， １ 車両， ２ カメラ，
３ ステアリングホイール， ４ 操舵角センサ， ５ シフトレバー，
６ シフトレバースイッチ， ７ 車輪速度センサ， ８ モニタ，
９ スピーカ， １０ 電子制御装置[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a moving body periphery monitoring device that monitors the surroundings of a moving body, and more particularly, to a moving body periphery monitoring device that detects and notifies an obstacle present in a predicted moving space locus from an environment around the moving body. Related.
[0002]
[Prior art]
Various devices for detecting obstacles existing on the predicted movement trajectory of the vehicle have been proposed in the past. For example, in the following Patent Document 1 (Japanese Patent Laid-Open No. 10-117340), according to the operation of the vehicle, When it is estimated that at least one obstacle is present in the own vehicle travel expected trajectory range, which is an estimated range of the trajectory traveled by the vehicle, based on the captured image, an arbitrary specified from the obstacles A vehicle periphery monitoring device that emphasizes and displays an image of an obstacle has been proposed.
[0003]
Further, in the following Patent Document 2 (Japanese Patent Laid-Open No. Hei 8-48198), in a vehicle surrounding monitor device that displays an image synthesized based on a signal from a vehicle surrounding detection means, a display in which the surroundings are fixed and the own vehicle moves is displayed. In addition, there has been proposed a vehicle surrounding monitor device that has two display modes of display in which the host vehicle is fixed and the surroundings rotate, and each display mode can be switched.
[0004]
Further, in the following Patent Document 3 (Japanese Patent Laid-Open No. 9-254726), a running state detecting unit that detects the running state of the host vehicle, a relative position calculating unit that obtains a relative position between the host vehicle and a surrounding object, Contact that determines a surrounding object that may contact the host vehicle based on the driving state detected by the driving state detection unit and the relative position calculated by the relative position calculation unit, and calculates a part of the host vehicle that contacts the host vehicle Predictive means, sensitive object selecting means for selecting the object that should be most noticed by the host vehicle from the objects determined to be likely to be contacted by the contact predicting means, and the sensitive object are in contact with each other A vehicle driving support device is provided that includes notification means for notifying a part of the vehicle to be operated and output means for outputting to at least the notification means when the object requiring attention satisfies a predetermined setting condition. Have .
[0005]
And in the following Patent Document 4 (Japanese Patent Laid-Open No. 2002-52999), a stop position marker of the host vehicle and a lateral distance measurement marker are superimposed and drawn on a monitor screen, and a camera image and a general-purpose camera are provided without providing a distance measuring sensor. A relative position detection device that can detect a relative position between the host vehicle and a reference object using a steering angle sensor, and a parking assist device including the relative position detection device have been proposed. Here, the stop position marker of the host vehicle is defined as a marker for stopping the host vehicle in a positional relationship in which the host vehicle and the reference object have a predetermined direction, and the lateral direction distance measurement marker is the amount of movement on the monitor screen. Accordingly, it is defined as a marker that measures the distance in the lateral direction between the host vehicle and the reference object.
[0006]
On the other hand, in the following Patent Document 5 (Japanese Patent Laid-Open No. 2001-187553), peripheral images at different first and second points are parallaxed in time series by a single camera while the vehicle is moving. An imaging device that captures images as first and second images, a three-dimensional object specifying unit that detects and specifies a three-dimensional object captured in each image, and calculates vehicle movement data from the first point to the second point A vehicle position calculator that calculates the distance from the vehicle to the three-dimensional object using the position of the three-dimensional object on the first and second images and the movement data of the vehicle, and imaging There has been proposed a parking assist device that generates a third image for transmission to a driver based on an image captured by the device and data calculated by a three-dimensional object distance calculation unit. Thus, it is described that the distance to the three-dimensional object can be accurately calculated for the three-dimensional object captured in the two images by the principle of triangulation.
[0007]
In this regard, in the field of image processing technology, a technology for restoring a three-dimensional shape from two cameras is known. For example, the following non-patent document 1 describes stereo three-dimensional shape measurement. In this Non-Patent Document 1, as a premise, the imaging characteristics (lens focal length, image center, pixel size, etc.) of each of the two cameras, the position and orientation of the two cameras, and the correspondence between the two images Indicates that the position of the corresponding point in the space can be determined.
[0008]
Similarly, Non-Patent Document 2 below also discloses a technique for calculating the three-dimensional shape of an object based on the principle of triangulation from the correspondence between images taken from two cameras. The principle is explained as one of the most basic methods for extracting three-dimensional information from an image.
[0009]
Non-Patent Document 3 below describes that the target three-dimensional shape can be restored from a moving image based on the same principle as the stereo described above. In particular, it is described that when a sequence of images is given as a moving image, it is possible to track the movement of the target, and it is easier to extract corresponding points than when only two images are given.
[0010]
[Patent Document 1]
JP-A-10-117340
[Patent Document 2]
JP-A-8-48198
[Patent Document 3]
JP 9-254726 A
[Patent Document 4]
JP 2002-52999 A
[Patent Document 5]
JP 2001-187553 A
[Non-Patent Document 1]
By Koichiro Deguchi in "Information Processing" Vol.37 No.7 (July 1996)
"2. Unravel the stereo mechanism"
[Non-Patent Document 2]
“Mathematics of Spatial Data” (by Kenichi Kanaya, first edition on March 10, 1995)
First print. Described on pages 161 and 162 of Asakura Shoten)
"3D reconstruction with stereo vision"
[Non-Patent Document 3]
By Koichiro Deguchi in "Information Processing" Vol.37 No.8 (August 1996 issue)
"3. Handling of moving images"
[0011]
[Problems to be solved by the invention]
In the device described in the above-mentioned Patent Document 1, when an obstacle is present in the predicted traveling locus range of the vehicle, the obstacle is highlighted as being closest to the vehicle in the traveling direction. At this time, a mark having a predetermined shape is superimposed in synchronization with the movement of the obstacle so that the position of the obstacle can be easily identified on the display screen. Further, as a highlighting method, marking that is surrounded by a rectangle including a specified obstacle is used. However, although such marking is effective for technical technical explanations, the marking shape does not match the shape of the obstacle, so it cannot always be easily recognized by general users. .
[0012]
Further, in the device described in the above-mentioned Patent Document 2, there are shown a mode in which the periphery rotates with reference to the own vehicle and a mode in which the periphery is fixed and the own vehicle moves. Measures are taken such as switching from a moving mode to a mode in which the surroundings rotate, and enlarging the display. In the embodiment, the arrangement of surrounding obstacles is displayed using the image of the top view, but the display is limited to the frame map display in the conventional navigation technology. That is, which part of the host vehicle is in contact with, what kind of object is in contact with, and which part is in contact with is not specifically shown. Therefore, it is difficult to help determine what action should be taken to avoid contact.
[0013]
Furthermore, in the apparatus described in the above-mentioned Patent Document 3, an obstacle around the vehicle is detected, a relative positional relationship with the own vehicle is obtained, a part of the own vehicle that may come into contact is predicted, and the vehicle Display on the display device using the top view of the outer shape. Here, the time to contact is predicted, and depending on whether or not the time exceeds a set value, notification is given by a combination of blinking display or lighting display and sound regarding a part that may contact the host vehicle. . However, it is not displayed which obstacles in the peripheral field of view are in contact with each other, and it is difficult to grasp the relationship with the behavior of the host vehicle, so this is also what action should be taken to avoid contact It is difficult to help determine.
[0014]
Moreover, in the above-mentioned Patent Document 4, a relative position detection device that can detect the relative position between the host vehicle and the reference object without providing a distance measuring sensor has been proposed. Position detection is performed using a stop position marker and a lateral distance measurement marker. Similarly, Patent Document 5 discloses a means for calculating a distance to a three-dimensional object captured in two images, and it is not necessary to provide a distance measuring sensor, but a three-dimensional object captured in an image is used. The distance from the vehicle to the three-dimensional object is calculated by the three-dimensional object specifying unit that detects and specifies, the vehicle position calculation unit that calculates the movement data of the vehicle, and the three-dimensional object distance calculation unit that calculates the distance from the vehicle to the three-dimensional object. I am going to do that. Accordingly, in any of Patent Documents 4 and 5, for example, a separate response is required for the problem of detecting an obstacle present in the predicted movement space of the vehicle. It is unclear whether it can be resolved.
[0015]
On the other hand, if the image processing techniques described in Non-Patent Documents 1 to 3 described above are used, the three-dimensional shape of the obstacle can be restored with, for example, one camera. Therefore, it can be considered that such an image processing technique is appropriately incorporated in a device for monitoring the periphery of a moving body such as a vehicle, but these specific means are not mentioned in any document.
[0016]
SUMMARY OF THE INVENTION It is therefore an object of the present invention to provide a moving body periphery monitoring device that can reliably detect and notify a moving space predicted locus of a moving body and obstacles in the locus from the environment around the moving body. To do.
[0017]
[Means for Solving the Problems]
  In order to solve the above-described problems, the mobile object periphery monitoring device according to the present invention is configured to detect environmental information around the mobile object according to the movement of the mobile object and sequentially detect and store the environmental information according to the movement of the mobile object. At least one image pickup means mounted on the moving body for imaging the periphery of the moving body and outputting image information; and at least when the moving body moves from the first state to the second state Based on the two images picked up by the image pickup means in the first state and the second state, the coordinates of the feature points are detected from the image picked up in the first state and picked up in the second state. Feature point tracking means for detecting coordinates corresponding to the feature points in the image, movement state detection means for detecting the position and orientation of the moving body in at least the first state and the second state, and the movement With state detection means The position and posture of the moving body in the first state and the second state, the coordinates of the feature points of the image captured in the first state, and the image of the image captured in the second state Environment information detection means comprising three-dimensional coordinate estimation means for estimating the three-dimensional coordinates of the feature points based on the coordinates corresponding to the feature points, and a movement space prediction for estimating a movement space prediction trajectory when the moving body moves A trajectory estimating means and a predicted moving space trajectory estimated by the predicted moving space trajectory estimating means are adapted to the predicted moving space trajectory of the plurality of images created based on the environmental information detected by the environmental information detecting means. The three-dimensional coordinates of the feature points superimposed on a predetermined image and estimated by the three-dimensional coordinate estimation means are converted into a three-dimensional coordinate map.StoreThe three-dimensional coordinate mapPosition and direction of the moving body inBased on the predetermined image, obstacle detection means for detecting an obstacle including the portion having the feature point existing in the movement space predicted locus of the moving body and expected to contact the moving body; The moving space predicted trajectory is a target for alerting, and a part of the obstacle detected by the obstacle detecting unit based on the three-dimensional coordinate map that is expected to contact the moving object is specified Target specifying means, display means for displaying the predetermined image together with the obstacle detected by the obstacle detecting means, and at least the target specifying means for specifying the predetermined image displayed by the display means And highlighting means for highlighting and displaying the specific alerting target. The environmental information includes the size, position, and the like of an object around the moving body. The moving body includes a mobile robot and the like in addition to the vehicle.
[0023]
DETAILED DESCRIPTION OF THE INVENTION
  Hereinafter, embodiments of a mobile object periphery monitoring device of the present invention will be described with reference to the drawings. FIG. 1 shows an overall configuration of an embodiment of the present invention. When mounted on a vehicle, the parking guide device shown in FIGS. 2 and 3 is used. In FIG. 1, the mobile object periphery monitoring device of this embodiment sequentially detects and accumulates environmental information around a mobile object (not shown) according to the movement of the mobile object.DoThe environment information detecting means SR and the predicted moving space trajectory estimating means TR for estimating the moving space expected trajectory when the moving body moves are provided.As shown by the broken line in FIG. 1, the environment information detection means SR is mounted on a moving body, picks up an image of the periphery of the moving body and outputs image information, and the moving body is moved from the first state to the second state. The coordinates of the feature point are detected from the image picked up in the first state based on at least two images picked up by the image pickup means CD in the first state and the second state when moving to the second state. A feature point tracking means PF for detecting coordinates corresponding to the feature points in the image captured in step (a), a movement state detection means DS for detecting the position and orientation of the moving body in at least the first state and the second state, and The position and orientation of the moving body in the first state and the second state detected by the movement state detection means DS, the coordinates of the feature points of the image captured in the first state, and the image captured in the second state To the coordinates corresponding to the feature points It is provided with a three-dimensional coordinate estimating means RC for Hazuki estimating the three-dimensional coordinates of the feature point.
[0024]
  Then, a predetermined image that matches the predicted moving space trajectory among the plurality of images created based on the environmental information detected by the environmental information detecting means SR, the predicted moving space trajectory estimated by the predicted moving space trajectory estimating means TR. The three-dimensional coordinates of the feature points estimated by the three-dimensional coordinate estimation means RC are converted into a three-dimensional coordinate map.StoreThis 3D coordinate mapPosition and direction of the moving body inBased on the above, an obstacle (not shown) including the portion having the feature point existing in the movement space predicted locus of the moving object on the predetermined image and expected to contact the moving object is detected on the predetermined image. Obstacle detection means OJ that performs the movement space prediction locus and a portion that is expected to contact the moving body among the obstacles detected by the obstacle detection means OJ based on the three-dimensional coordinate map. A target specifying means DO to be a specific alerting target and a display means VS for displaying the predetermined image together with the obstacle detected by the obstacle detecting means OJ are provided. Furthermore, it has an emphasis unit AT for emphasizing and displaying a specific attention calling object by the object specifying unit DO with respect to the predetermined image displayed by the display unit VS. The imaging means CD includes a ccd camera described later.
[0025]
When the moving body is a vehicle, the moving state detecting means DS can be constituted by a sensor that detects a vehicle motion such as a traveling speed, a steering state, and a turning state of the vehicle, which will be described later. . Further, based on the output of at least one of these sensors, coordinates can be detected as a factor representing the position of the vehicle and an angle can be detected as a factor representing the posture of the vehicle, which will be described later. And as said movement space estimated locus | trajectory estimation means TR (henceforth only the locus | trajectory estimation means TR), for example, the movement space at the time of forward or backward movement of the vehicle based on the detected steering angle of the steering angle sensor 4 shown in FIG. There is a means for estimating the expected trajectory, which will be described later with reference to FIGS.
[0026]
  Further, as shown by a broken line in FIG. 1, a specific view creation means SV for creating a specific view of the moving object based on the detection information of the environment information detection means SR may be provided. In this case, the display means VS displays an image of the specific field of view including the obstacle, and the emphasizing means AT displays the image of the specific field of view with the specific field of view.specificThe alert target will be highlighted and displayed. This automatically displays a virtual image of the field of view including the obstacle, and thespecificIt can be configured to highlight and display the alert target. The details will be described later with reference to FIG.
[0029]
  In addition, as shown by the broken line in FIG.specificIt is good also as providing the audio | voice apparatus AV which alert | reports the information regarding an alerting target with an audio | voice.
[0030]
The parking guide apparatus shown in FIGS. 2 and 3 will be described as an example when the moving body is a vehicle. FIG. 2 shows the overall configuration of a vehicle 1 equipped with the parking guidance apparatus of the present embodiment. A camera 2 (for example, the above-described imaging means using a ccd camera) captures an image of the periphery of the vehicle 1 at the rear portion of the vehicle 1. The image information is supplied to the electronic control device 10. The electronic control device 10 is housed in a housing mounted on the vehicle 1 as shown in FIG. 2, and the feature point tracking means PF, the movement state detection means DS, the three-dimensional coordinate estimation means RC, the specific field of view shown in FIG. The creating means SV, the trajectory estimating means TR, the obstacle detecting means OJ, and the object specifying means DO are configured. Further, the steering angle sensor 4 which detects the steering angle of the steering wheel 3 and constitutes a part of the trajectory estimation means TR is mounted on the vehicle 1, and the shift lever which is turned on when the shift lever 5 is in the reverse position. A switch 6 is mounted, and the detected steering angle and ON signal are supplied to the electronic control unit 10.
[0031]
Furthermore, in this embodiment, a wheel speed sensor (typically indicated by 7) that detects the rotational speed of each wheel is mounted, and the detected wheel speed is supplied to the electronic control device 10. Based on the detection results of these wheel speed sensors 7, the moving distance between at least two positions of the moving body can be measured. A monitor 8 is arranged at a predetermined position in the vehicle 1 that can be visually recognized by the driver, and visual information for parking guidance is displayed on the monitor 8 in accordance with an output signal of the electronic control device 10. That is, in the present embodiment, the monitor 8 functions as the display unit VS of FIG. Furthermore, a speaker 9 is arranged at a predetermined position in the vehicle 1, and parking guidance audio information is output from the speaker 9 in accordance with an output signal of the electronic control device 10. The speaker 9 functions as the audio device AV of this embodiment.
[0032]
FIG. 3 is a system configuration diagram of the parking guidance device described above, and shows a configuration of the electronic control device 10. The steering angle sensor 4, the shift lever switch 6, and the wheel speed sensor 7 are connected to the CPU 11 in the electronic control device 10 via an input interface (not shown), and the monitor 8 is electronically connected via an output interface (not shown). The speaker 9 is connected to the CPU 11 via the audio output unit 15. The speaker 9 is connected to the superimpose unit 14 in the control device 10. The camera 2 is connected to the image recognition unit 12, the graphics drawing unit 13, and the superimpose unit 14 via an input interface (not shown). The CPU 11, the image recognition unit 12, the graphics drawing unit 13, the superimpose unit 14, and the audio output unit 15 are connected to each other by a bus bar. Although not shown in the memory or the like, the information of each unit is stored for a predetermined period and is configured to be read out appropriately.
[0033]
In the present embodiment, an image signal of the camera 2 that displays the environment around the vehicle 1 is supplied to the CPU 11 via the image recognition unit 12, and an output signal from the wheel speed sensor 7 and the like is supplied to the CPU 11, Are processed. Based on the calculation result of the CPU 11, the graphics drawing unit 13 draws three-dimensional graphics according to the synchronization signal extracted from the image of the camera 2. In the CPU 11, calculation processing for parking guidance is performed, and graphics based on the calculation result are drawn by the graphics drawing unit 13. The graphics drawn by the graphics drawing unit 13 and the image of the camera 2 are appropriately superimposed by the superimposing unit 14 and drawn on the screen of the monitor 8. In this case, it is possible to set so that only the graphic image is displayed without displaying the camera image. Thus, the three-dimensional coordinate estimation means RC in FIG. 1 is constituted by the CPU 11, the image recognition unit 12, and the graphics drawing unit 13.
[0034]
In the above three-dimensional coordinate estimation means RC, the feature point tracking means PF is imaged in the first state by the camera 2 as the imaging means CD when the vehicle 1 moves from the first state to the second state. The coordinates of the feature points are detected from the captured image, and the coordinates corresponding to the feature points in the image captured in the second state are detected. In the movement state detection means DS, the first state and the second state are detected. The position and posture of the vehicle 1 in the state are detected. And corresponding to the position and posture of the vehicle 1 in the first state and the second state, the coordinates of the feature points of the image captured in the first state, and the feature points of the image captured in the second state A three-dimensional reconstruction is performed based on the coordinates to be obtained, and the three-dimensional coordinates of the feature points are obtained.
[0035]
The above-described three-dimensional reconstruction is performed based on the principle of triangulation using an image as described in Non-Patent Document 2, for example, and will be described below with reference to FIGS. To do. In FIG. 23, when the same point M in the space is observed from at least two camera positions (indicated by o and o ′ on the left and right), the point m observed in the image of each camera₁, M₂And the optical center of each camera can be defined, and the intersection of each ray represents the position M of the point in space. Here, a three-dimensional point M in the target field of view (Scene) needs to be identified between the images. In addition, when performing three-dimensional reconstruction with a monocular camera, it is necessary to extract and track the feature point (Point of Interest) of the target object from the image. Furthermore, the internal parameters of the camera (scale factor, optical axis position coordinates, etc.) and external parameters (camera placement) must be known (pre-calibrated). Done.
[0036]
Next, regarding the restoration of the three-dimensional structure from the movement of the target field of view, the position and orientation of each of the time-series images taken by the dead reckoning system can be calibrated by taking correspondence. You can use stereo support. However, it is necessary to find the same point from the image sequence in a time-series image photographed so as to include the same point in the three-dimensional field of view. That is, by taking correspondence between these same points, the three-dimensional structure of the target field of view can be restored.
[0037]
Regarding the detection of feature points from the target field of view, several feature points that should be the features of the target field of view are detected from the image. A feature point in this case is an object having an appropriate feature that can be tracked along a sequence of images. Such feature points can be detected by using auto-correlation based on the matrix A shown in the following [Equation 1]. Since the matrix A functions to average the derivative of the image signal included in the window region W including the point p (x, y) in the image, an optimum feature point is obtained using this. That is, feature points suitable for tracking are generally intersections of corners and straight lines (for example, black points at corners in the left side of FIG. 24), and this matrix detects these feature points. In the following [Equation 1], I (x, y) is a function of the image, Ix is the gradient of the image along the x-axis, Iy is the gradient of the image along the y-axis, (x_k,y_k) Represents image points in the window W with (x, y) as the center.
[0038]
[Expression 1]

[0039]
Next, regarding feature tracking, u = [x, y]^TConsider an image sequence I (u, t) where is the coordinates of the image points. If the sample period is sufficiently short, it is assumed that the luminance value does not change in the image after the minute time, and the following [Equation 2] holds.
[0040]
[Expression 2]

[0041]
In the above [Expression 2], δ (u) represents a motion field (projection of a three-dimensional motion vector onto an image). Since the sample period is sufficiently short, the motion can be approximated only by translation as shown in the following [Equation 3]. In the equation (3), d represents a displacement vector.
[0042]
[Equation 3]

[0043]
A Tracker is used to calculate the transition vector d for each selected feature point and for each tracked point pair in the image sequence. Actually, since the motion model of the image cannot be completely expressed due to the influence of noise, the formula [2] does not function sufficiently. Therefore, as shown in the following [Equation 4], the shift vector d is calculated by the sum of square differences. Then, a transition vector d that minimizes the residue of the formula [4] is obtained, and the feature point is traced in that direction. Note that W in the following [Expression 4] represents a window for extracting feature points centered on the point u.
[0044]
[Expression 4]

[0045]
FIG. 25 illustrates an example of tracking. A track (Tracks) is a set of points indicating the same object in an image sequence, and an image point of each track is a projection image of each three-dimensional point. The track is not allowed to be lost in the image sequence, but if the point to be tracked is out of the image, hidden, or cannot be extracted due to noise or the like, tracking must be stopped. Here, starting from a set of feature points extracted on the first screen, feature points in the previous image (Img A in FIG. 25) are tracked in the next image (Img B in FIG. 25). When a part of the track of the image (Img A) stops, a new feature point is searched for in the image (Img B), and each new feature point becomes a starting point of subsequent tracking.
[0046]
Regarding the reconstruction of the three-dimensional structure, when the distance of the point of the position M in the three-dimensional space is Z, the relationship of the following [Equation 5] holds when the projection position on the image is m. In the following [Equation 5], the distance f is called a focal length, but here it is simplified using a unit length of f = 1. When modeled in this way, the geometrical relationship in which feature points (such as the vertices of an object) in space are photographed with two cameras is as shown in FIG.
[0047]
[Equation 5]

[0048]
In FIG. 23, the second camera coordinate system is at a position obtained by rotating the first camera coordinate system by R around its origin and then translating it by T. This vector T is called a baseline vector. Since the parameter {T, R} represents the relative position and orientation relationship between the two camera viewpoints, when taking a picture while moving one camera, it can be measured by a dead reckoning system.
[0049]
As shown in FIG. 23, a projected image on each image plane at a position M in a certain three-dimensional space is m.₁, M₂Suppose that In this state, the direction of the feature point viewed from the origin 0 ′ of the second camera coordinate system is the vector m with respect to the second camera coordinate system.₂Since the second camera coordinate system is rotated by R with respect to the first camera coordinate system, the object M viewed from the first camera via the second camera coordinate system is represented by The direction can be expressed as a vector Rm₂It is. Therefore, the relationship of the following [Equation 6] is established from FIG.
[0050]
[Formula 6]

[0051]
From the above [Equation 6], the distances Z and Z ′ are calculated as the following [Equation 7].
[0052]
[Expression 7]

[0053]
In this way, the coordinate m of the projected image on the image plane from two viewpoints.₁, M₂The distances Z and Z ′ in the three-dimensional space can be restored from the above. Since the aforementioned track is a group of feature points in the target field of view, the three-dimensional structure can be restored by calculating the above [Equation 7] for all the tracks.
[0054]
Returning to FIG. 1, as the moving state detecting means DS, a dead reckoning system can be configured by sensors for detecting vehicle movement such as the traveling speed of the vehicle, the steering state, and the turning state as described above. The wheel speed sensor 7 is configured as described later. The position and posture of the vehicle 1 in the first state and the second state specified in this way, and between the images specified by extraction and tracking of feature points in the images captured by the camera 2 in each state Based on the corresponding points, processing is performed as described above, an object existing in the imaging region is specified, and the three-dimensional coordinates of the imaging region are estimated.
[0055]
In the present embodiment, based on the wheel speed signal detected by the wheel speed sensor 7, the vehicle 1 is dead so as to detect the state of the vehicle 1, that is, the position and direction (angle around the vertical axis) of the vehicle 1 as follows. A reckoning system is configured. Hereinafter, a method for detecting the state of the vehicle 1 in the present embodiment will be described. FIG. 4 shows a model of the vehicle 1 in the present embodiment, and the position and direction of the vehicle 1 are defined by three variables (x, y, θ). When the speed (vehicle speed) of the vehicle 1 is V and the angular speed of the vehicle 1 is ω, the following equation is established between the vehicle state, the vehicle speed, and the angular speed. That is, dx / dt = V · cos θ, dy / dt = V · sin θ, and dθ / dt = ω.
[0056]
The vehicle speed V and the angular speed ω cannot be directly measured, but can be obtained as follows using the wheel speeds of the left and right rear wheels detected by the wheel speed sensor 7. That is, V = (Vrr + Vrl) / 2 and ω = (Vrr−Vrl) / Lt. Here, Vrr is the wheel speed of the right rear wheel, Vrl is the wheel speed of the left rear wheel, and Lt is the tread length of the vehicle 1.
[0057]
Using the above equation, the vehicle state (x, y, θ) at time t can be expressed as the following [Equation 8] if the initial value (x0, y0, θ0) at t = 0 is determined. be able to.
[0058]
[Equation 8]

[0059]
FIG. 5 shows an example of a method for detecting and tracking the coordinates and direction of the position after movement based on the coordinates and orientation (direction) of the movement start position of the vehicle 1 in predetermined three-dimensional coordinates. That is, FIG. 5 shows a state in which the vehicle turns on the Xo-Yo plane (Zo = 0) in the three-dimensional coordinates (Xo, Yo, Zo), and the position (coordinates (x1, y1)) and direction ( It is possible to specify the positional relationship when moving from the first state determined by the angle θ1) to the second state determined by the position (coordinates (x2, y2)) and the direction (angle θ2). In this embodiment, the position (coordinates) and direction (angle) of the vehicle 1 are detected based on the wheel speeds detected by the wheel speed sensors 7 for the left and right rear wheels, but the steering angle sensor 4 and the yaw rate sensor (not shown). A distance sensor (not shown) or the like may be used, or these sensors may be used in combination. FIG. 5 shows an example of a simple circular turn, but it is possible to track a locus in which a plurality of arcs and straight lines are combined based on the coordinates and direction of the vehicle 1 detected as described above.
[0060]
On the other hand, the three-dimensional coordinates estimated by the three-dimensional coordinate estimation means RC shown in FIG. 1 are three-dimensional based on the three-dimensional coordinates (X, Y, Z) as shown by the dashed grid in FIG. It is stored in memory as a coordinate map. Note that other vehicles, obstacles, buildings, and the like imaged by the camera 2 exist in the three-dimensional coordinate map indicated by the dashed grid in FIG. 6, but these are not shown. The three-dimensional coordinates in FIG. 5 are associated with the three-dimensional coordinate map. That is, the vehicle 1 shown in FIG. 5 is arranged in the three-dimensional coordinate map indicated by the dashed grid in FIG. 6, and the position (coordinates) and attitude of the vehicle 1 in the three-dimensional coordinate map, that is, the direction (angle). Is identified.
[0061]
Based on the position (coordinates) and direction (angle) of the vehicle 1 in the three-dimensional coordinate map specified as described above, the positional relationship with the obstacle existing in the three-dimensional coordinates becomes clear. For example, when the steering wheel 3 of FIG. 2 is in the neutral position and the vehicle 1 is traveling straight ahead as shown in FIG. 7, when the obstacle OB exists in front of the vehicle 1, the vehicle 1 moves forward as it is. It can be seen that the bumper portion 1a in FIG. 7 comes into contact with the left-hand corner of the obstacle OB, and the distance d1 between them is the closest distance to the movement.
[0062]
On the other hand, when the steering wheel 3 of FIG. 2 is operated and the vehicle 1 is turning with a radius R around the turning center C as shown in FIG. 8, for example, there is an obstacle OB in front of it. However, it can be seen that no part of the vehicle 1 contacts the obstacle OB if the vehicle 1 turns as it is. This is because the arc of any part of the obstacle OB centered on the turning center C (the arc of the minimum radius is indicated by A1 in FIG. 8) does not overlap with the vehicle 1, so that the positional relationship shown in FIG. Can be estimated. The turning center C is on the extension of the rear axle of the vehicle 1, and its position is determined by the steering angle of the steering wheel 3.
[0063]
On the other hand, as shown in FIG. 9, when the vehicle 1 turns with a radius R around the turning center C and the obstacle OB in front of the vehicle 1 moves relatively on an arc A2 (radius Ra) around the turning center C, That is, when the arc A2 overlaps with the vehicle 1, if the vehicle 1 turns as it is, the bumper portion 1b of the vehicle 1 comes into contact with the obstacle OB. In this case, the distance d2 between the two is the closest movement distance.
[0064]
As is clear from FIG. 9, the vehicle 1 and the obstacle OB come into contact with each other when the arc passing through the obstacle OB centered on the turning center C coincides with the arc passing through the vehicle 1 around the turning center C. The arc of the obstacle OB that minimizes the distance between the two can be specified as A2. Therefore, the relationship shown in FIG. 10 is established, and the distance between the bumper portion 1b of the vehicle 1 and the obstacle OB becomes the moving distance d2, which is longer than the linear distance d1 between the bumper portion 1a of the vehicle 1 and the obstacle OB. The moving distance d2 is obtained by calculation processing by the CPU 11.
[0065]
7 to 10 are described on the XY plane for convenience of explanation, but in this embodiment, the positional relationship with the obstacle existing in the three-dimensional coordinates is clarified. It is possible to measure the close distance of movement taking the situation into consideration. For example, as shown in FIG. 11, the horizontal distance between the bumper portion 1a of the vehicle 1 and the bumper portion Ob of another vehicle OV that is an obstacle is d3. A situation in which the horizontal distance between them may be d4 (<d3). Therefore, if the distance between the bumper portion 1a of the vehicle 1 and the other vehicle OV is a distance d3, the front upper portion of the vehicle 1 contacts the rear end Oa of the other vehicle OV when the distance d4 is reached. However, this situation can also be avoided in this embodiment. That is, an estimated moving space trajectory when the host vehicle (vehicle 1) moves is estimated, and this is overlapped with a predetermined image that matches the predicted moving space trajectory among a plurality of detected images, thereby predicting the moving space expected trajectory. It is possible to detect obstacles existing inside. Here, the predicted travel space trajectory is a trajectory of a space that is expected to pass when the host vehicle (vehicle 1) moves. Accordingly, although the predicted moving space trajectory is three-dimensional, this can also be basically calculated based on the detection result of the steering angle sensor 4 in FIG. 2, and the host vehicle (vehicle 1) can be operated at a predetermined steering angle. A space through which the outer portion of the host vehicle (vehicle 1) passes when the steering operation is performed is predicted. Since this calculation process is described in the above-mentioned Patent Document 4 and the like, description thereof is omitted.
[0066]
Thus, the predicted movement space trajectory of the vehicle 1 estimated as described above is superimposed on a predetermined image that matches the predicted movement space trajectory among the plurality of images detected so far. Obstacles present in the predicted movement space trajectory of the vehicle 1 are detected on the image and displayed on the display means VS of FIG. 1 as follows. That is, an image including an obstacle present in the predicted movement space of the vehicle 1 among the images captured by the camera 2 is displayed on the monitor 8 of FIG. 2 constituting the display means VS of FIG. The display by the emphasis means AT is superimposed and displayed.
[0067]
  For example, as shown in FIG. 12, when the parking operation of the vehicle 1 is performed in a parking area where other vehicles OV and PV exist, as shown in FIG. 13, predicted movement spaces 8a and 8b ( The stippled region in FIG. 13) is specified as an alerting target by the target specifying unit DO in FIG. 1, and is emphasized by modulation of at least one of shading and color change. Further, the obstacle (a part of another vehicle PV in FIG. 13) is highlighted and displayed. At this time, a part of the vehicle PV that is expected to come into contact if it moves backward as it isIsBy the object specifying means DO in FIG.specificA marker 8c, which is specified as a target for alerting and is further changed in shade or color, is displayed in the portion. For example, the image density of the obstacle area (lattice area in FIG. 13) intruding into the movement space predicted trajectories 8a and 8b is set to be high, and the other areas are set to be light so that both areas can be clearly identified. Become. Alternatively, even if the color pattern is changed (color change) with respect to the actual color of the obstacle, both regions can be clearly identified (the same applies to the shading and color change below).
[0068]
  When the vehicle 1 further moves backward and approaches another vehicle OV, a colored frame 8d is displayed around the image as shown in FIG. 14 and blinks. In addition, a part of the vehicle OV that is expected to come into contact if the vehicle moves backwardspecificA marker 8c for emphasizing this portion by modulation of at least one of shading and color change is displayed on this portion, and a message 8e indicating this position (for example, “left rear”) is displayed. The In addition to this display, the audio device AV of FIG. 1 may perform notification by voice or beep sound according to the value of the moving distance, for example. Although omitted for convenience of explanation, even when the marker 8c is displayed as shown in FIG. 13, the frame 8d is displayed around the image together with the marker 8c, blinks, and the same message as in FIG. 13 is omitted) is displayed.Further, as shown in FIG. 15, a predetermined instruction mark (for example, an arrow 8f in FIG. 15) is superimposed on a specific alerting target (a part of the vehicle OV), and this arrow 8f is used as at least one of shading and color change. Emphasis may also be achieved by modulation.
[0069]
  It should be noted that not only the above-mentioned specific alerting target that is expected to come into contact if retreating as it is, but also the vehicle OV that exists within the predicted movement space trajectory is the alerting target.16, at least one of the outermost contour 8g of the vehicle OV shown in FIG. 16 and the main contour 8h of the vehicle OV shown in FIG. 17 may be emphasized by modulation of at least one of shading and color change. Here, as the main outline 8h, for example, an outline having a length equal to or longer than a predetermined length can be selected and displayed.
[0070]
Alternatively, as shown in FIG. 18 and FIG. 19, a semi-transparent screen 8j (dotted area in FIG. 18) or an opaque screen 8k (hatched area in FIG. 19) is superposed on the vehicle OV, and these translucent screens are superposed. The image 8j or the opaque screen 8k may be enhanced by modulation of at least one of shading and color change of the image. Although not shown in the figure, it is also possible to emphasize the region in the predicted trajectory of the moving space in the display image of FIG. 13 or the vehicle OV itself (that is, the original image) by modulating at least one of shading and color change of the image. Good.
[0071]
Furthermore, as shown in FIG. 22, notification by the display can be performed using a virtual image. FIG. 22 shows a part of the vehicle 1 that is the own vehicle (the rear part in FIG. 22) and a graphic representing the obstacle OB, and an emphasis pattern 8p (dotted area in FIG. 22) and a marker 8c in which the shade or color is changed. (The bold line portion in FIG. 22) is also superimposed and displayed, so that the relationship between the vehicle 1 and the obstacle OB can be confirmed. Of course, it may be combined with an announcement (voice), alarm sound or beep sound. The virtual image can be realized by configuring the specific visual field creating means SV or the like as shown by a broken line in FIG. 1, and this will be described below.
[0072]
That is, the specific visual field creation means SV creates a specific visual field when it is assumed that the camera is mounted in an arbitrary direction at an arbitrary position of the vehicle 1 within the three-dimensional coordinates including the surrounding situation. An image of the field of view seen from the camera is displayed on the display means VS (monitor 8 in FIG. 2) in FIG. In this case, for example, as shown in FIG. 20, when a virtual camera 2a is arranged behind the vehicle 1, a three-dimensional coordinate map viewed from the vehicle 1 after the movement (with a broken-line grid in FIG. 20). It can be seen which region is in the field of view of the virtual camera 2a. In other words, the area indicated by Sa in FIG. 20 is an area (hereinafter referred to as a display view) that can enter the field of view of the virtual camera 2a and display it on the monitor 8 as a virtual image. The same applies to the case where the virtual camera is arranged forward or sideward.
[0073]
In the example of FIG. 20 above, a field of view similar to the angle of view of the actual camera 2 is set, but a display field of view Se of 180 degrees can also be set like the virtual camera 2e of FIG. Thereby, it can also comprise so that the virtual image which put the part which does not enter into a visual field with the actual camera 2 also into the visual field can be displayed. Furthermore, although illustration is omitted, a virtual camera may be arranged in the upper part of the vehicle 1 to set a 360-degree display field of view and display a virtual image of this field of view. 20 and 21 also include other vehicles, obstacles, buildings, etc. captured by the camera 2 in the three-dimensional coordinate map indicated by the dashed grid, but these are not shown. is doing.
[0074]
The virtual image formed in this way changes in accordance with the movement of the vehicle 1, that is, the movement of the field of view accompanying the movement of the virtual camera 2a, etc. In this embodiment, the obstacle imaged during the movement Can be displayed as appropriate according to the situation (after movement). For example, when it is determined that the distance between the vehicle 1 and the obstacle OB is less than a predetermined distance, a virtual image shown in FIG. 22 is automatically displayed on the monitor 8. Or it can also comprise so that it may switch to a virtual image according to a driver | operator's instruction | indication.
[0075]
As described above, according to each of the embodiments described above, for example, an image in which an obstacle exists can be automatically switched and displayed, and contact with respect to an obstacle within a predetermined view range in the image can be performed. The expected part can be specified. Therefore, the driver of the vehicle 1 can not only recognize the approach of the obstacle, but also recognize in which direction the obstacle exists with respect to the vehicle 1 in a short time. Moreover, if there is an obstacle in the predicted trajectory of the moving space, it is displayed with emphasis by the outermost outline and main outline of the obstacle in the image, or by the shade or color change of the obstacle itself, The object to be alerted can be grasped easily and quickly.
[0076]
2 and FIGS. 12 to 22 relate to an embodiment in which the camera 2 is arranged behind the vehicle 1, and FIGS. 7 to 11 show a case where the camera 2 is arranged in front of the vehicle 1. As is clear from the embodiment, any obstacles before and after can be processed in the same manner. In the above embodiment, the three-dimensional coordinate map is created by using only one camera 2. However, a plurality of cameras may be used as a matter of course.
[0077]
【The invention's effect】
  Since this invention is comprised as mentioned above, there exists an effect as described below. In other words, according to the moving object periphery monitoring device according to the first aspect, the moving object is present in the predicted moving space locus of the moving object using the image.Includes features that have feature points and are expected to touch moving objectsAn obstacle can be detected, and it can be easily determined which part of the moving body is in contact with which part of the obstacle. And that part of the obstaclespecificSince it is displayed with emphasis as an alerting target, it can be grasped easily and quickly.
[Brief description of the drawings]
FIG. 1 is a block diagram showing a configuration of an embodiment of a mobile object periphery monitoring device of the present invention.
FIG. 2 is a perspective view illustrating a vehicle equipped with a parking guide device as an embodiment when the mobile object periphery monitoring device of the present invention is applied to the parking guide device.
FIG. 3 is a block diagram showing a configuration of a parking guide device according to an embodiment of the present invention.
FIG. 4 is a plan view of a vehicle model in one embodiment of the present invention.
FIG. 5 is a plan view showing an example of a method for detecting and tracking the coordinates and direction of a position after movement based on the coordinates and direction of the movement start position of the vehicle in predetermined three-dimensional coordinates in one embodiment of the present invention. FIG.
FIG. 6 is a plan view showing an example of a state in which the position (coordinates) and posture (direction) of a vehicle on a three-dimensional coordinate map are specified in an embodiment of the present invention.
FIG. 7 is a plan view showing an example of a close distance to a front obstacle when the vehicle is traveling straight in the embodiment of the present invention.
FIG. 8 is a plan view showing an example of a state in which the vehicle does not come in contact with a front obstacle when the vehicle is turning in the embodiment of the present invention.
FIG. 9 is a plan view showing an example of a close distance when the vehicle contacts a front obstacle when the vehicle is turning in the embodiment of the present invention.
10 is an enlarged plan view showing a close-up portion of movement between the vehicle and the obstacle in FIG. 9;
FIG. 11 is a front view showing an example of a close distance of movement between a vehicle and an obstacle when viewed in a vertical positional relationship in one embodiment of the present invention.
FIG. 12 is a plan view showing an example in the case where the vehicle is parked in the embodiment of the present invention.
FIG. 13 shows an image of a state in which an expected moving space locus is highlighted and an obstacle is highlighted when a vehicle is parked as shown in FIG. 12 in one embodiment of the present invention. It is a front view.
FIG. 14 shows an embodiment of the present invention that blinks a frame of a display image and modulates at least one of shading and color change with respect to a part of a vehicle that is expected to come into contact if the vehicle moves backward as it is. It is a front view of the image which shows the state emphasized by.
FIG. 15 is an image showing a state in which a predetermined instruction mark is superimposed on a part of a vehicle as an alert target and is emphasized by modulation of at least one of shading and color change in an embodiment of the present invention. It is a front view.
FIG. 16 is a front view of an image showing a state in which an outermost contour line is emphasized by modulation of at least one of shading and color change with respect to a vehicle as an alert target in the embodiment of the present invention.
FIG. 17 is a front view of an image showing a state in which a main contour line is emphasized by modulation of at least one of shading and color change with respect to a vehicle as an alert target in the embodiment of the present invention.
FIG. 18 shows a state in which a translucent screen is superposed on a vehicle as an alert target in the embodiment of the present invention, and the translucent screen is emphasized by modulation of at least one of shading and color change. It is a front view of the image shown.
FIG. 19 is an image showing a state in which an opaque screen is superimposed on a vehicle as an object of alerting in the embodiment of the present invention, and the opaque screen is emphasized by modulation of at least one of shading and color change. FIG.
FIG. 20 is a plan view showing a display field of view when a virtual camera is arranged behind the vehicle in one embodiment of the present invention.
FIG. 21 is a plan view showing a display field of view of 180 degrees when a virtual camera is arranged at the rear upper part of the vehicle in one embodiment of the present invention.
FIG. 22 is a plan view showing a state in which attention is to be emphasized in a virtual image in which an obstacle is displayed on a camera image behind the vehicle, according to an embodiment of the present invention, by emphasizing at least one of shading and color change; It is.
FIG. 23 is an explanatory diagram showing a geometric relationship when photographing feature points of an object in a space with two cameras, regarding three-dimensional reconstruction in one embodiment of the present invention.
FIG. 24 is an explanatory diagram showing an example of feature point tracking regarding the three-dimensional reconstruction in the embodiment of the present invention.
FIG. 25 is an explanatory diagram showing an example of a tracking situation regarding three-dimensional reconstruction in an embodiment of the present invention.
[Explanation of symbols]
SR environment information detection means, CD imaging means, DS movement state detection means,
RC 3D coordinate estimation means, SV specific field of view creation means, VS display means,
TR movement space prediction trajectory estimation means, OJ obstacle detection means,
DO target identification means, emphasis means AT, 1 vehicle, 2 camera,
3 steering wheel, 4 steering angle sensor, 5 shift lever,
6 shift lever switch, 7 wheel speed sensor, 8 monitor,
9 Speaker, 10 Electronic control unit

Claims (1)

移動体の周辺の環境情報を該移動体の移動に応じて順次検出し蓄積する環境情報検出手段であって、前記移動体に搭載し前記移動体の周辺を撮像し画像情報を出力する少なくとも一つの撮像手段と、前記移動体が第１の状態から第２の状態に移動したときに少なくとも前記第１の状態及び第２の状態で前記撮像手段によって撮像した二つの画像に基づき、前記第１の状態で撮像した画像から特徴点の座標を検出すると共に、前記第２の状態で撮像した画像での前記特徴点に対応する座標を検出する特徴点追跡手段と、少なくとも前記第１の状態及び前記第２の状態における前記移動体の位置及び姿勢を検出する移動状態検出手段と、該移動状態検出手段で検出した前記第１の状態及び前記第２の状態における前記移動体の位置及び姿勢、前記第１の状態で撮像した画像の前記特徴点の座標、及び前記第２の状態で撮像した画像の前記特徴点に対応する座標に基づき前記特徴点の３次元座標を推定する３次元座標推定手段を具備した環境情報検出手段と、前記移動体が移動するときの移動空間予想軌跡を推定する移動空間予想軌跡推定手段と、該移動空間予想軌跡推定手段が推定した移動空間予想軌跡を、前記環境情報検出手段が検出した環境情報に基づいて作成する複数の画像のうちの前記移動空間予想軌跡に適合する所定の画像に重合し、前記３次元座標推定手段が推定した前記特徴点の３次元座標を３次元座標マップに格納し、該３次元座標マップにおける前記移動体の位置及び方向に基づき、前記所定の画像上で前記移動体の移動空間予想軌跡内に存在する前記特徴点を有し前記移動体に接触すると予想される部分を含む障害物を検出する障害物検出手段と、前記移動空間予想軌跡を注意喚起対象とすると共に、前記３次元座標マップに基づいて前記障害物検出手段が検出した前記障害物のうちの前記移動体に接触すると予想される部分を特定注意喚起対象とする対象特定手段と、前記障害物検出手段が検出した前記障害物と共に前記所定の画像を表示する表示手段と、該表示手段が表示する前記所定の画像に対し、前記対象特定手段が特定した前記特定注意喚起対象を強調して表示する強調手段とを備えたことを特徴とする移動体周辺監視装置。Environment information detection means for sequentially detecting and accumulating environmental information around a moving body according to the movement of the moving body, and is mounted on the moving body, picks up the surroundings of the moving body, and outputs image information. Based on at least two images picked up by the image pickup means in the first state and the second state when the moving body moves from the first state to the second state. Feature point tracking means for detecting coordinates of feature points from an image captured in the state of the image and detecting coordinates corresponding to the feature points in the image imaged in the second state; and at least the first state and Moving state detecting means for detecting the position and posture of the moving body in the second state; and the position and posture of the moving body in the first state and the second state detected by the moving state detecting means; Said 3D coordinate estimation means for estimating the 3D coordinates of the feature points based on the coordinates of the feature points of the image captured in the state 1 and the coordinates corresponding to the feature points of the image captured in the second state. The environment information detection means provided, the movement space prediction locus estimation means for estimating the movement space prediction locus when the moving body moves, and the movement space prediction locus estimated by the movement space prediction locus estimation means are represented as the environment information. The three-dimensional coordinates of the feature points estimated by the three-dimensional coordinate estimation means are superimposed on a predetermined image that matches the predicted trajectory of the moving space among a plurality of images created based on the environmental information detected by the detection means. stored in the three-dimensional coordinate map, based on the position and direction of the moving body in the three-dimensional coordinate map having the feature points exists in the space within the prediction path of the moving body on the predetermined image The obstacle detecting means for detecting an obstacle including a portion expected to contact the moving body, the moving space predicted locus as a target for alerting, and the obstacle detecting means based on the three-dimensional coordinate map Target identification means for targeting a specific portion of the detected obstacle that is expected to contact the moving body, and a display for displaying the predetermined image together with the obstacle detected by the obstacle detection means And a highlighting means for highlighting and displaying the specific alerting target specified by the target specifying means for the predetermined image displayed by the display means. .

Images