(57)【特許請求の範囲】
【請求項１】
移動体の前方に取り付けられ、時間的に連続した第１画像を撮像する第１撮像手段と、
前記第１撮像手段の撮像方向に対して反対の撮像方向となるよう移動体の後方に取り付けられ、時間的に連続した第２画像を撮像する第２撮像手段と、
前記第１撮像手段により撮像された第１画像から当該第１画像上の第１特徴点を検出する第１特徴検出手段と、
前記第２撮像手段により撮像された第２画像から当該第２画像上の第２特徴点を検出する第２特徴検出手段と、
前記第１特徴検出手段により検出された第１特徴点の前記第１画像上における移動方向を計測する第１移動計測手段と、
前記第２特徴検出手段により検出された第２特徴点の前記第２画像上における移動方向を計測する第２移動計測手段と、
前記第１移動計測手段により計測された第１特徴点の前記第１画像上における移動方向と前記第２移動計測手段により計測された第２特徴点の前記第２画像上における移動方向とを比較して、当該双方の移動方向が異なる場合には移動体が横方向に移動していると判別し、当該双方の移動方向が同じである場合には移動体がヨー方向に移動していると判別する移動体挙動判別手段と
を備えることを特徴とする移動体挙動検出装置。
【請求項２】
予め設定された移動体の周囲における特徴点及び当該特徴点の位置情報を取得する周囲特徴取得手段と、
前記移動体の位置を推定する位置推定手段と、
前記位置推定手段により推定された移動体の位置と、前記第１撮像手段の取付位置とに基づいて、前記第１撮像手段の第１撮像範囲を取得する第１撮像範囲取得手段と、
前記位置推定手段により推定された移動体の位置と、前記第２撮像手段の取付位置に基づいて、前記第２撮像手段の第２撮像範囲を取得する第２撮像範囲取得手段と、
前記周囲特徴取得手段により取得された位置情報と前記第１撮像範囲とを比較して、当該第１撮像範囲に含まれる特徴点を選択する第１特徴選択手段と、
前記周囲特徴取得手段により取得された位置情報と前記第２撮像範囲とを比較して、当該第２撮像範囲に含まれる特徴点を選択する第２特徴選択手段と、
前記第１移動計測手段は、前記第１撮像手段により撮像された第１画像に含まれる特徴点と、前記第１特徴選択手段により選択された特徴点とから、前記第１特徴点の移動方向を算出し、
前記第２移動計測手段は、前記第２撮像手段により撮像された第２画像に含まれる特徴点と、前記第２特徴選択手段により選択された特徴点とから前記第２特徴点の移動方向を算出することを
を特徴とする請求項１に記載の移動体挙動検出装置。
【請求項３】
前記第２撮像手段は、前記第１撮像手段の撮像方向に対して９０度方向の画像を撮像することを特徴とする請求項１に記載の移動体挙動検出装置。
【請求項４】
前記移動体挙動判別手段は、前記第２撮像手段により撮像された後方撮像画像に含まれる特徴点の動きに基づいて、移動体の回転量を算出することを特徴とする請求項３に記載の移動体挙動検出装置。
【請求項５】
移動体の前方に取り付けられ、時間的に連続した第１画像を撮像する第１撮像手段と、前記第１撮像手段の撮像方向に対して反対の撮像方向となるよう移動体の後方に取り付けられ、時間的に連続した第２画像を撮像する第２撮像手段と、演算手段と、を備えた移動体挙動検出装置における移動体挙動検出方法であって、
前記演算手段は、
前記第１撮像手段により撮像された第１画像から当該第１画像上の第１特徴点を検出する第１特徴検出処理と、
前記第２撮像手段により撮像された第２画像から当該第２画像上の第２特徴点を検出する第２特徴検出処理と、
前記第１特徴検出処理で検出された第１特徴点の前記第１画像上における移動方向を計測する第１移動計測処理と、
前記第２特徴検出処理で検出された第２特徴点の前記第２画像上における移動方向を計測する第２移動計測処理と、
前記第１移動計測処理で計測された第１特徴点の前記第１画像上における移動方向と前記第２移動計測処理で計測された第２特徴点の前記第２画像上における移動方向とを比較する移動方向比較処理と、
前記移動方向比較処理において、前記第１特徴点の移動方向と前記第２特徴点の移動方向が異なる場合には移動体が横方向に移動していると判別し、前記第１特徴点の移動方向と前記第２特徴点の移動方向が同じである場合には移動体がヨー方向に移動していると判別する移動体挙動判別処理と、
を行うことを特徴とする移動体挙動検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体挙動検出装置及び方法に関する。
【背景技術】
【０００２】
従来より、自車両の挙動の変化を検出する技術としては、下記の特許文献１に記載された車両挙動検出装置が知られている。
【０００３】
この車両挙動検出装置は、車両前方を撮像した画像を用いて、自車両から遠方に離れた特徴点を特徴点として検出する。この車両挙動検出装置は、当該遠方の特徴点の動きを監視して、自車両の挙動の変化を検出する。この車両挙動検出装置は、遠方の特徴点が、自車両の前進する方向における挙動変化の影響を受けないことを利用し、遠方特徴点の動きに基づいて自車両のヨー方向の挙動を検出している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００６−３３８２７２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
上述した車両挙動検出装置は、充分に遠方の特徴点が検出できない場合には、画像内の特徴点の全てが自車両の挙動の影響を受けて画像内で移動する。このため、上述した車両挙動検出装置は、自車両のヨー方向と横方向の挙動が区別できないという問題があった。
【０００６】
そこで、本発明は、上述した実情に鑑みて提案されたものであり、移動体のヨー方向と横方向の挙動を高い精度で区別できる移動体挙動検出装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明は、移動体に対して異なる方向に含まれる第１特徴点及び第２特徴点の移動方向を比較して、当該双方の移動方向が異なる場合には移動体が横方向に移動している判別し、当該双方の移動方向が同じである場合には移動体がヨー方向に移動していると判別する。
【発明の効果】
【０００８】
本発明によれば、移動体に対して異なる方向に含まれる特徴点の移動方向が異なるか同じかによって移動体の移動方向を判別できるので、移動体のヨー方向と横方向の挙動を高い精度で区別できる。
【図面の簡単な説明】
【０００９】
【図１】本発明の第１実施形態として示す車両挙動検出システムにおいて、前方撮像カメラ及び後方撮像カメラの配置を示す上面図である。
【図２】本発明の第１実施形態として示す車両挙動検出システムの機能的な構成を示すブロック図である。
【図３】本発明の第１実施形態として示す車両挙動検出システムにおいて、移動体が横方向と回転方向に移動したときの特徴点の移動方向を示す図であり、（Ａ）が移動体が横方向に移動したときにおける（ａ）前方特徴点、（ｂ）後方特徴点の移動を示し、（Ｂ）が移動体が回転方向に移動したときにおける（ａ）前方特徴点、（ｂ）後方特徴点の移動を示す。
【図４】本発明の第１実施形態として示す車両挙動検出システムにおいて、移動体の挙動を推定する処理手順を示すフローチャートである。
【図５】本発明の第１実施形態として示す車両挙動検出システムにおいて、特徴点の位置を推定する処理手順を示すフローチャートである。
【図６】本発明の第１実施形態として示す車両挙動検出システムにおいて、事前推定値と実際値とのズレについて説明する図である。
【図７】本発明の第１実施形態として示す車両挙動検出システムにおいて、移動体に対する注目特徴点の位置関係を示す図である。
【図８】本発明の第１実施形態として示す車両挙動検出システムにおいて、注目特徴点がｚ軸方向となるよう座標変換した様子を示す図である。
【図９】本発明の第１実施形態として示す車両挙動検出システムにおいて、注目特徴点の反対方向に他の注目特徴点と設定した様子を示す図である。
【図１０】本発明の第１実施形態として示す車両挙動検出システムにおいて、（Ａ）が移動体が横方向に移動したときにおける（ａ）前方特徴点、（ｂ）後方特徴点の移動を示し、（Ｂ）が移動体が回転方向に移動したときにおける（ａ）前方特徴点、（ｂ）後方特徴点の移動を示す。
【図１１】本発明の第２実施形態として示す車両挙動検出システムにおいて、前方撮像カメラの撮像方向に対して、後方撮像カメラが９０度の範囲の広がりで撮像することを説明する上面図である。
【発明を実施するための形態】
【００１０】
以下、本発明の実施の形態について図面を参照して説明する。
【００１１】
［第１実施形態］
本発明の第１実施形態として示す車両挙動検出システムは、移動体の挙動を検出する。この車両挙動検出システムは、例えば、自車両の挙動を検出する。この車両挙動検出システムは、例えば、自車両の挙動に基づいて運転者に警告を行うために、自車両の挙動を検出する。
【００１２】
この車両挙動検出システムは、図１に示すように、自車両１の前方に設けられた前方撮像カメラ１２と、自車両１の後方に設けられた後方撮像カメラ１３とを有する。この車両挙動検出システムは、図２に示すように、演算装置１１と、前方撮像カメラ１２と、後方撮像カメラ１３と、周囲特徴記憶部１４とを含む。
【００１３】
前方撮像カメラ１２は、自車両１の前方を撮像方向として取り付けられている。前方撮像カメラ１２は、所定の時間毎に時間的に連続する前方撮像画像を撮像して、演算装置１１に供給する。前方撮像カメラ１２は、自車両１に取り付けられ、時間的に連続した第１画像を撮像する第１撮像手段に相当する。
【００１４】
後方撮像カメラ１３は、自車両１の後方を撮像方向として取り付けられている。後方撮像カメラ１３は、所定の時間毎に時間的に連続する後方撮像画像を撮像して、演算装置１１に供給する。後方撮像カメラ１３は、第１撮像手段の撮像方向に対して反対の撮像方向となるよう自車両１に取り付けられ、時間的に連続した第２画像を撮像する第２撮像手段に相当する。
【００１５】
前方撮像カメラ１２及び後方撮像カメラ１３は、当該前方撮像カメラ１２の光軸方向と、当該後方撮像カメラ１３の光軸方向とが１８０度の角度を以て取り付けられている。
【００１６】
周囲特徴記憶部１４は、自車両１周囲の特徴点と当該特徴点の位置情報とを対応付けて記憶している。この周囲特徴記憶部１４は、例えば予め計測された特徴点と位置情報とが登録された地図データベースを備えている。特徴点としては、物体のエッジやコーナー等の周囲画像に対してエッジ検出処理をしてエッジが得られた部分画像等が挙げられる。周囲特徴記憶部１４は、ハードディスク等の記録媒体に対して情報を記録再生ができる情報記録再生装置である。
【００１７】
なお、周囲特徴記憶部１４には、自車両１が移動中にMotion Stereoなどの既存手法によりオンラインで逐次的に特徴点及び位置情報を保存していく処理を行っても良い。
【００１８】
演算装置１１は、前方特徴検出部２１、前方位置取得部２２、前方撮像範囲取得部２３、前方特徴選択部２４、前方移動計測部２５、後方特徴検出部２６、後方位置取得部２７、後方撮像範囲取得部２８、後方特徴選択部２９、後方移動計測部３０、及び、移動体挙動判定部３１を備える。
【００１９】
なお、演算装置１１は、実際にはＲＯＭ、ＲＡＭ、ＣＰＵ等にて構成されているが、当該ＣＰＵがＲＯＭに格納された車両挙動検出用のプログラムに従って処理をすることによって実現できる機能をブロックとして説明する。すなわち、以下では、演算装置１１の物理的な構成は単一であるものの、プログラムによって演算装置１１が実行できる処理内容に対応して、演算装置１１の各機能部２１〜３１を説明する。
【００２０】
前方特徴検出部２１は、前方撮像カメラ１２により撮像された前方撮像画像が供給される。前方特徴検出部２１は、前方撮像画像に対してエッジ検出処理等を行って、前方撮像画像に含まれる前方特徴点を検出する。この前方特徴点としては、周囲特徴記憶部１４に記憶されたエッジやコーナー等と同様に、実際の前方撮像画像から検出した部分画像である。
【００２１】
同様に、後方撮像範囲取得部２８は、後方撮像カメラ１３により撮像された後方撮像画像が供給される。後方撮像範囲取得部２８は、後方撮像画像に対してエッジ検出処理等を行って、後方撮像画像に含まれる後方特徴点を検出する。この後方特徴点としては、周囲特徴記憶部１４に記憶されたエッジやコーナー等と同様に、実際の後方撮像画像から検出した部分画像である。
【００２２】
前方位置取得部２２は、前方撮像カメラ１２により撮像される前方撮像画像内における前方特徴点の位置を推定する。
【００２３】
同様に、後方位置取得部２７は、後方撮像カメラ１３により撮像される後方撮像画像内における後方特徴点の位置を推定する。
【００２４】
前方位置取得部２２及び後方位置取得部２７は、実際に前方撮像カメラ１２及び後方撮像カメラ１３によって撮像して特徴点を検出するよりも事前に、自車両１の位置を推定しておく。自車両１の位置の事前推定値は、様々な手法により取得することができる。例えば、ＧＰＳ（Global Positioning System）等のインフラを利用して自車両１の位置を推定しても良く、自車両１の車輪の回転量から直接的に自車両１の位置を推定しても良い。
【００２５】
前方撮像範囲取得部２３、前方特徴選択部２４及び前方移動計測部２５は、前方特徴検出部２１により検出された前方特徴点の移動方向を検出する。
【００２６】
前方撮像範囲取得部２３は、前方位置取得部２２により推定された自車両１の位置と、前方撮像カメラ１２の取付位置とに基づいて、前方撮像カメラ１２の前方撮像範囲を取得する。前方特徴選択部２４は、周囲特徴記憶部１４により取得された位置情報と前方撮像範囲とを比較して、当該前方撮像範囲に含まれる前方特徴点を選択する。前方移動計測部２５は、前方撮像カメラ１２により撮像された前方撮像画像に含まれる前方特徴点と、前方特徴選択部２４により選択された前方特徴点とのズレから、前方特徴点の移動方向を算出する。
【００２７】
すなわち、前方移動計測部２５は、周囲特徴記憶部１４に記憶された特徴点のうち前方撮像カメラ１２の撮像範囲に現れると推定される前方特徴点の位置と、当該推定された前方特徴点の位置に対する実際の前方特徴点の移動方向を計測する。
【００２８】
同様に、後方撮像範囲取得部２８、後方特徴選択部２９及び後方移動計測部３０は、後方特徴検出部２６により検出された後方特徴点の移動方向を検出する。
【００２９】
後方撮像範囲取得部２８は、後方位置取得部２７により推定された自車両１の位置と、後方撮像カメラ１３の取付位置とに基づいて、後方撮像カメラ１３の後方撮像範囲を取得する。後方特徴選択部２９は、周囲特徴記憶部１４により取得された位置情報と後方撮像範囲とを比較して、当該後方撮像範囲に含まれる後方特徴点を選択する。後方移動計測部３０は、後方撮像カメラ１３により撮像された後方撮像画像に含まれる後方特徴点と、後方特徴選択部２９により選択された後方特徴点とのズレから、後方特徴点の移動方向を算出する。
【００３０】
すなわち、後方移動計測部３０は、周囲特徴記憶部１４に記憶された特徴点のうち後方撮像カメラ１３の撮像範囲に現れると推定される後方特徴点の位置と、当該推定された後方特徴点の位置に対する実際の後方特徴点の移動方向を計測する。
【００３１】
前方撮像範囲取得部２３及び後方撮像範囲取得部２８が撮像範囲を取得するとき、前方位置取得部２２及び後方位置取得部２７により取得された自車両位置の事前推定値を用いる。これにより、前方撮像範囲取得部２３及び後方撮像範囲取得部２８は、前方撮像カメラ１２及び後方撮像カメラ１３から観測できる領域を計算する。このとき、前方撮像範囲取得部２３及び後方撮像範囲取得部２８は、予め周囲特徴記憶部１４に記憶されている世界座標上での特徴点の位置と、前方撮像カメラ１２及び後方撮像カメラ１３の取り付け位置とに基づいて、前方撮像範囲及び後方撮像範囲内における特徴点の位置を推定する。また、前方撮像範囲取得部２３及び後方撮像範囲取得部２８は、前方撮像カメラ１２及び後方撮像カメラ１３の取り付け方向、カメラ内部パラメータ（画角、ゆがみ）も考慮して、より高い精度で前方撮像範囲及び後方撮像範囲内における特徴点の位置を推定することもできる。
【００３２】
移動体挙動判定部３１は、前方移動計測部２５により計測された前方特徴点の移動方向と後方移動計測部３０により計測された後方特徴点の移動方向とを比較する。移動体挙動判定部３１は、当該双方の移動方向が異なる場合には、自車両１が横方向に移動していると判別する。移動体挙動判定部３１は、当該双方の移動方向が同じである場合には、自車両１がヨー方向に移動していると判別する。
【００３３】
ここで、図３に示すように、自車両１のヨー方向における特徴点の挙動と、自車両１の横方向における特徴点の挙動は、前方特徴点の移動方向と後方特徴点の移動方向とが異なる動きとして現れる。
【００３４】
自車両１が横方向に移動したとき、図３（Ａ）の（ａ）に示すように、前方撮像範囲４０ｆにおける事前推定位置の特徴点Ｐ’は、実際には左方向に移動した特徴点Ｐの位置に現れる。これに対し、自車両１が横方向に移動したとき、図３（Ａ）の（ｂ）に示すように、後方撮像範囲４０ｂにおける事前推定位置の特徴点Ｐ’は、実際には右方向に移動した特徴点Ｐの位置に現れる。このように、自車両１が横方向に移動したとき、前方と後方とで“異なる”移動方向に特徴点Ｐがずれる。
【００３５】
自車両１が回転方向に移動したとき、図３（Ｂ）の（ａ）に示すように、前方撮像範囲４０ｆにおける事前推定位置の特徴点Ｐ’は、実際には左方向に移動した特徴点Ｐの位置に現れる。これに対し、自車両１が回転方向に移動したとき、図３（Ａ）の（ｂ）に示すように、後方撮像範囲４０ｂにおける事前推定位置の特徴点Ｐ’は、実際には左方向に移動した特徴点Ｐの位置に現れる。このように、自車両１が回転方向に移動したとき、前方と後方とで“同じ”移動方向に特徴点がずれる。
【００３６】
このことを利用して、車両挙動検出システムは、自車両１のヨー方向の挙動と自車両１の横方向の挙動とを判別することができる。
【００３７】
つぎに、上述した車両挙動検出システムにおいて、自車両１の挙動を推定する処理手順について、図４及び図５のフローチャート等を参照して説明する。なお、車両挙動検出システムは、所定時間ごとにステップＳ１以降の処理を実行することによって、所定時間ごとに自車両１の挙動を推定する。
【００３８】
先ず、ステップＳ１において、前方撮像カメラ１２によって前方撮像画像を取得すると共に、ステップＳ４において、後方撮像カメラ１３によって後方撮像範囲を取得する。ここで、前方撮像カメラ１２による前方撮像画像の取得タイミングと、後方撮像カメラ１３による後方撮像画像との取得タイミングとは同期している。
【００３９】
次に、ステップＳ２において、前方特徴点の前方撮像画像内位置を推定すると共に、ステップＳ５において、後方特徴点の後方撮像画像内位置を推定する。
【００４０】
このステップＳ２及びステップＳ５は、図５に示す処理を行う。先ずステップＳ１１において、前方位置取得部２２及び後方位置取得部２７は、自車両１（移動体）の位置を事前に推定する。この自車両位置の事前推定とは、前方撮像カメラ１２及び後方撮像カメラ１３による撮像画像を用いない。前方位置取得部２２及び後方位置取得部２７は、例えば、自車両１の運動モデル、自車両１が移動するトルクを発生させるための制御指令値、ＧＰＳ等の他のセンサーを用いて、世界座標系に対して自車両１の重心位置Ｅを求める。ここで自車両１の重心位置Ｅとは、下記の式１に示すように表現される。
【数１】
0005604987.tif 000002
【００４１】
この式１において、Ｒは３×３の回転行列、ｔは３次の移動ベクトルを表す。なお、この自車両１の位置の推定は、他にもさまざまな手法で実施することが可能である。
【００４２】
前方位置取得部２２及び後方位置取得部２７は、例えば、自車両１の制御指令値を用いて、事前に自車両１の位置推定を行う。自車両１の制御前における自車両１の重心位置Ｅｔ−１は求められているとし、自車両１の制御指令値としての直進制御指令値ＶＺ［ｍ／ｓ］，回転制御指令値ω［ｒａｄ／ｓ］による制御が実施されたとする。ただし、自車両１の制御には誤差が伴い、また、制御どおりに自車両１が制御されたとしても、自車両１に対する路面とのフリクションなどにより、制御通りに自車両１が移動するとは限らない。したがって、制御が実施された後の自車両位置の期待値を、
Ｅｐｒｉｏｒ＝Ｍｃ・Ｅｔ−１
とし、これを自車両１の事前推定値Ｅｐｒｉｏｒとする。ここで、Ｍｃは、下記の式２により表現される。
【数２】
0005604987.tif 000003
【００４３】
次のステップＳ１２において、前方撮像範囲取得部２３及び後方撮像範囲取得部２８は、前方位置取得部２２及び後方位置取得部２７により取得された自車両位置の事前推定値と、前方撮像カメラ１２及び後方撮像カメラ１３の取り付け位置とから、前方撮像カメラ１２及び後方撮像カメラ１３の撮像範囲を取得する。
【００４４】
ここで、自車両１の重心位置Ｅに対する前方撮像カメラ１２及び後方撮像カメラ１３の取り付け位置をＣｆとする。このとき、世界座標系における前方撮像カメラ１２及び後方撮像カメラ１３の取り付け位置の事前推定値は、
Ｃｆｐｒｉｏｒ＝Ｃｆ・Ｅｐｒｉｏｒ
として表すことができる。このとき、前方撮像カメラ１２及び後方撮像カメラ１３の取り付け位置の事前推定値Ｃｆｐｒｉｏｒから撮像した撮像範囲は、世界座標を前方撮像カメラ１２及び後方撮像カメラ１３の取り付け位置の事前推定値Ｃｆｐｒｉｏｒを中心とした座標に変換することによって表される。この前方撮像カメラ１２及び後方撮像カメラ１３の撮像範囲は、世界座標系（ｘ，ｙ，ｚ）において、ｚがある値以上であり、ｘ／ｚとｙ／ｚの絶対値が、ある値を下回っている範囲として表すことができる。
【００４５】
次のステップＳ１３において、前方特徴選択部２４及び後方特徴選択部２９は、周囲特徴記憶部１４に記憶されている特徴点のうち、前方撮像カメラ１２及び後方撮像カメラ１３の撮像範囲に入っていると考えられる特徴点を選択する。ここで、特徴点は、予め周辺地図内に世界座標系としての位置情報Ｐｉとして表されているとする。この世界座標系の位置情報Ｐｉをカメラ座標系で表すと、カメラ座標系の位置情報Ｐｉｃは、
Ｐｉｃ＝Ｃｆｐｒｉｏｒ−１・Ｐｉ
として表すことができる。前方特徴選択部２４及び後方特徴選択部２９は、当該カメラ座標系の位置情報Ｐｉｃを用いて、カメラ座標系の位置情報Ｐｉｃ３４の移動ベクトルｚ成分がある値以上、Ｐｉｃ１４／Ｐｉｃ３４，Ｐｉｃ２４／Ｐｉｃ３４の絶対値がある値を下回っていることを条件として、前方撮像カメラ１２及び後方撮像カメラ１３の撮像範囲に入っていると考えられる特徴点を選択する。ここで、Ｐｉｃ１４は、カメラ座標系の位置情報Ｐｉｃの移動ベクトルｘ成分であり、Ｐｉｃ２４は、カメラ座標系の位置情報Ｐｉｃの移動ベクトルｙ成分である。
【００４６】
図４に戻り、ステップＳ３において、前方移動計測部２５によって、ステップＳ２にて推定した前方撮像画像内における前方特徴点の推定位置と、実際に撮像された前方撮像画像内における前方特徴点の位置とのズレｄｐを計測する。このとき、前方移動計測部２５は、前方特徴検出部２１により検出された実際の前方撮像画像から検出された前方特徴点及び画像内位置を取得しておく。同様に、ステップＳ６において、後方移動計測部３０によって、ステップＳ５にて推定した後方撮像画像内における後方特徴点の推定位置と、実際に撮像された後方撮像画像内における後方特徴点の位置とのズレｄｐを計測する。このとき、後方移動計測部３０は、後方特徴検出部２６により検出された実際の後方撮像画像から検出された後方特徴点及び画像内位置を取得しておく。
【００４７】
次のステップＳ７においては、移動体挙動判定部３１によって、ステップＳ３及びステップＳ６にて計測されたズレｄｐから、自車両１の挙動を推定する。このとき、移動体挙動判定部３１は、ステップＳ３にて計測された前方特徴点におけるズレｄｐによって表される移動方向と、ステップＳ６にて計測された後方特徴点におけるズレｄｐによって表される移動方向とを比較する。移動体挙動判定部３１は、前方特徴点の移動方向と後方特徴点の移動方向とが同じ場合には、自車両１がヨー方向に移動していると判別する。移動体挙動判定部３１は、前方特徴点の移動方向と後方特徴点の移動方向とが異なる場合には、自車両１が横方向に移動していると判別する。これにより、車両挙動検出システムは、自車両１の挙動を推定できる。
【００４８】
ステップＳ７は、ステップＳ３，ステップＳ６にて得られた前方特徴点及び後方特徴点のズレｄｐから、自車両１の運動量が、事前に推定した自車両１の運動量に対してどの程度ずれていたのかの推定を行う。
【００４９】
つぎに、車両挙動検出システムによって自車両１の挙動を推定する処理について具体的に説明する。
【００５０】
先ず、上述したように、前方撮像カメラ１２の前方撮像範囲と、後方撮像カメラ１３の後方撮像範囲とが反対となっており、移動体挙動判定部３１が、前方撮像カメラ１２により撮像された前方撮像画像内における前方特徴点の動きと、後方撮像カメラ１３より撮像された後方撮像画像内における後方特徴点の動きとの差分に基づいて、自車両１の回転量を算出することについて説明する。
【００５１】
図６に示す三次元座標系において、自車両１の重心位置Ｅが４１’であるときの自車両位置の事前推定値Ｅｐｒｉｏｒ（４０’）と、自車両１の重心位置Ｅが４１であるときの自車両位置の実際値（４０）とのズレについて、ｘ方向のズレをＭｘ、ｚ方向のズレをＭｚ、回転方向のズレをＭθとして表すとする。図６において、自車両１の事前の向きを４２’で表し、実際の自車両１の向きを４２で表している。
【００５２】
また、図７に示すように、ステップＳ１３において選択された複数の特徴点のうち任意の特徴点を選択し、当該選択された特徴点を注目特徴点Ｐ１とし、カメラ座標系において注目特徴点Ｐ１が存在するｘ方向位置をＰ１ｘ、ｚ方向位置をＰ１ｚとする。このとき、注目特徴点Ｐ１の画像内におけるｘ方向のズレｄＰ１ｘは、
ｄＰ１ｘ＝−ａＭθ−ｆ（Ｍｘ，Ｍｚ，Ｐｘ，Ｐｚ）  （式３）
として表すことができる。上記式３における関数ｆは、回転方向におけるズレＭθ以外の自車両１のズレによる画像内での移動量を示す。上記式におけるａは、前方撮像カメラ１２及び後方撮像カメラ１３の画角により決定される定数である。
【００５３】
ここで、自車両１の挙動計算を容易にするために、図８に示すように、自車両１の重心位置Ｅを中心とし、注目特徴点Ｐ１の中心軸がｚ軸方向になるような座標変換を実施する。座標変換の回転角度が十分小さいとき、座標変換後の画像内における注目特徴点Ｐ１のズレｄｐｘ１’は、図７に示した変換前の画像内における注目特徴点Ｐ１のズレｄｐｘ１とほぼ等しく、
ｄｐｘ１’≒ｄｐｘ１  （式４）
となる。自車両１の回転方向のズレ（−Ｍθ）は、自車両１の回転による座標変換の影響を受けない。注目特徴点Ｐ１の変換後のｘ方向位置Ｐ１ｘ’、ｚ方向位置Ｐ１ｚ’は、ｚ軸方向に注目特徴点Ｐ１が存在するため、
Ｐ１ｘ’＝０  （式５）
Ｐ１ｚ’＝｛（（Ｐ１ｘ）２  ＋（Ｐ１ｚ）２）｝１／２  （式６）
として表される。変換後の注目特徴点Ｐ１の位置が式５のようにＰ１ｘ’＝０の場合、画像上では、自車両１のズレＭｚ’の動きの影響は受けず、画像上のズレｄｐは、自車両１の回転方向におけるズレＭθと、自車両１のｘ方向における変換後のズレＭｘ’によってのみ決定される。自車両１のｘ方向における変換後のズレＭｘ’による画像の動きは、自車両１からｚ方向における注目特徴点Ｐ１までの距離（Ｐ１ｚ’）に反比例するので、
−Ｍｘ’／Ｐ１ｚ’  （式７）
として表される。結果として、画像内のｘ方向における注目特徴点Ｐ１のズレｄＰ１ｘは、式７を利用して、
ｄＰ１ｘ＝−ａＭθ−Ｍｘ’／Ｐ１ｚ’  （式８）
として表される。
【００５４】
自車両１の回転方向θにおける変換後のズレＭθ’と、自車両１のｘ方向における変換後のズレＭｘ’を求めるため、注目特徴点Ｐ１とは異なる位置の注目特徴点Ｐ２を選択する。この注目特徴点Ｐ２は、前方特徴点及び後方特徴点の移動方向を計測する精度向上の観点から、変換後のｚ軸の近傍にあり、注目特徴点Ｐ１からなるべく離れていることが望ましい。
【００５５】
これは、変換後のｚ軸上に注目特徴点Ｐ２が存在すれば、変換後のｚ軸と平行な自車両１の移動があっても、画像内においては注目特徴点Ｐ２は動かない。このため、自車両１が移動しても、画像内における注目特徴点Ｐ２のｚ方向の動きを無視して考えることが可能となる。一方、変換後のｚ軸から注目特徴点Ｐ２がずれていると、変換後のｚ軸と平行に自車両１が移動したときに、注目特徴点Ｐ２の画像内位置がｘ方向又はｙ方向にずれてしまう。このため、前方特徴点及び後方特徴点の移動方向を計測する精度が低下してしまう。
【００５６】
上述のステップＳ７において、自車両１の挙動は、画像内における注目特徴点Ｐ１のｘ方向のズレｄＰ１ｘに基づいて、ヨー方向移動か、横方向移動かの判別がされる。この判別は、前方撮像カメラ１２及び後方撮像カメラ１３の双方に対し、自車両１の回転方向の移動は同じように作用することを利用している。このため、自車両１の回転方向θにおける変換後のズレＭθ’の項には、前方特徴点と後方特徴点とで差異がない。したがって、自車両１の回転方向θにおける変換後のズレＭθ’及び自車両１のｘ方向における変換後のズレＭｘ’を計算するためには、式７，８におけるＭｘ’／Ｐ１ｚ’の項に差が発生する必要があり、この差が大きいほど、自車両１の挙動を精度よく求めることが可能となる。したがって、注目特徴点Ｐ１からなるべく距離が大きい特徴点を、注目特徴点Ｐ２としてを選択することが望ましい。
【００５７】
この場合、注目特徴点Ｐ１が前方撮像カメラ１２により撮像された前方撮像画像内に存在するとき、注目特徴点Ｐ２は自車両１に対して、前方撮像カメラ１２の逆方向に存在していることが望ましい。例えば図９に示すように、後方撮像カメラ１３により撮像される特徴点のうち、変換後のｚ’軸の近傍に位置する特徴点を注目特徴点Ｐ２として選択する。この注目特徴点Ｐ２のズレ量ｄｐ２ｘは、下記の式９に示すように、
ｄＰ２ｘ＝−ａＭθ−Ｍｘ’／Ｐ２ｚ’  （式９）
として表される。式８、式９に関して、注目特徴点Ｐ１の画像内におけるｘ方向のズレｄｐ１ｘ、注目特徴点Ｐ１が存在するｘ方向位置Ｐ１ｘ、注目特徴点Ｐ１が存在するｚ方向位置Ｐ１ｚ、前方撮像カメラ１２及び後方撮像カメラ１３の画角により決定される定数ａ、注目特徴点Ｐ２のｘ方向のズレ量ｄＰ２ｘ、注目特徴点Ｐ２が存在するｚ方向位置Ｐ２ｚは既知である。したがって、式８により求められた画像内のｘ方向における注目特徴点Ｐ１のズレｄＰ１ｘと、式９により求められた画像内のｘ方向における注目特徴点Ｐ２のズレｄＰ２ｘとの差分から、自車両１の回転方向の移動量Ｍθ、自車両１のｘ方向の移動量Ｍｘ’を求めることが可能となる。また、自車両１のｘ方向の移動量Ｍｘ’は、変換前座標系におけるｘ方向の移動量Ｍｘ、ｚ方向の移動量Ｍｚとの合成成分となっているので、座標変換前の座標軸に射影することで、変換前座標系におけるｘ方向の移動量Ｍｘ、ｚ方向の移動量Ｍｚを求めることが可能となる。
【００５８】
以上のように、この車両挙動検出システムによれば、自車両１が横方向に移動した場合、図１０（Ａ）の（ａ）に示すように、前方撮像範囲５０ｆにおける事前推定位置の特徴点事前に推定した特徴点Ｐｆ’は、実際には左方向に移動した特徴点Ｐｆの位置に現れる。これに対し、図１０（Ａ）の（ｂ）に示すように、後方撮像範囲５０ｂにおける事前に推定した特徴点Ｐｂ’は、実際には右方向に移動した特徴点Ｐｂの位置に現れる。このように、自車両１が横方向に移動したとき、前方と後方とで“異なる”移動方向に事前特徴点Ｐ’と実際の特徴点Ｐとがずれる。
【００５９】
自車両１が回転方向に移動したとき、図１０（Ｂ）の（ａ）に示すように、前方撮像範囲５０ｆにおける事前に推定した特徴点Ｐｆ’は、実際には左方向に移動した特徴点Ｐｆの位置に現れる。これに対し、図１０（Ａ）の（ｂ）に示すように、後方撮像範囲５０ｂにおける事前に推定した特徴点Ｐｂ’は、実際には左方向に移動した特徴点Ｐｂの位置に現れる。このように、自車両１が回転方向に移動したとき、前方と後方とで“同じ”移動方向に特徴点がずれる。
【００６０】
このように車両挙動検出システムは、予め記憶した周囲の特徴点と、前方撮像カメラ１２及び後方撮像カメラ１３によって撮像した撮像画像に現れる特徴点との移動方向が同じであるか異なるかによって、自車両１の挙動を検出できる。
【００６１】
以上詳細に説明したように、第１実施形態として示した車両挙動検出システムによれば、前方特徴点の移動方向と後方特徴点の移動方向とを比較して、当該双方の移動方向が異なる場合には自車両１が横方向に移動している判別し、当該双方の移動方向が同じである場合には自車両１がヨー方向に移動していると判別できる。これにより、車両挙動検出システムによれば、自車両１のヨー方向の挙動と横方向の挙動とを、高い精度で区別でき、自車両１の位置を求めることが可能となる。
【００６２】
例えば、自車両１の駐車操作の支援機能などを実現する場合、自車両位置が目的位置に対してどのような位置関係にあるのか（Localization）を取得することはきわめて重要である。例えば、以前に自車両１を駐車した位置に同じように駐車するときには、以前に駐車した位置と、現在の自車両１の位置とを正確に把握する必要がある。これに対し、車両挙動検出システムは、自車両１の挙動を検出して、自車両１の位置を精度良く検出できる。また、常に位置検出を実施しながら移動すれば、移動軌跡を得ることが可能となる。次回以降この軌跡をたどれば、複雑な経路計算を行うことなく、自車両１の制御を行うことが可能となる。
【００６３】
また、車両挙動検出システムによれば、事前に特徴点を記憶しておき、事前に求めた特徴点と実際に得られた特徴点とから、前方特徴点の移動方向及び後方特徴点の移動方向を得ることができ、自車両１のヨー方向の挙動と横方向の挙動とを、高い精度で区別でき、自車両１の位置を求めることが実現できる。
【００６４】
更に、この車両挙動検出システムによれば、前方撮像カメラ１２の前方撮像範囲と、後方撮像カメラ１３の後方撮像範囲とが反対となっているので、自車両１が横方向に移動したとき前方と後方とで異なる移動方向に特徴点がずれ、自車両１が回転方向に移動したとき前方と後方とで同じ移動方向に特徴点がずれることを利用して、精度良く自車両１の挙動を検出できる。
【００６５】
更に、この車両挙動検出システムによれば、前方撮像カメラ１２により撮像された前方撮像画像内における前方特徴点の動きと、後方撮像カメラ１３より撮像された後方撮像画像内における後方特徴点の動きとの差分に基づいて、自車両１の回転量を算出するので、自車両１の運動状態にかかわらず、代数的に自車両１の横方向移動とヨー方向回転を切り分けることができる。これにより、車両挙動検出システムは、低い計算負荷で自車両１の挙動を求めることができる。
【００６６】
［第２実施形態］
つぎに、第２実施形態の車両挙動検出システムとして、自車両１の挙動を推定する他の処理について具体的に説明する。
【００６７】
この車両挙動検出システムは、後方撮像カメラ１３によって、前方撮像カメラ１２の撮像方向に対して９０度方向の画像を撮像する。具体的には、図１１に示すように、前方撮像カメラ１２の光軸方向は、自車両１の前方となっている。これに対して、後方撮像カメラ１３の光軸方向は、前方撮像カメラ１２の光軸方向とは１８０度だけずれているが、後方撮像カメラ１３の撮像範囲は、前方撮像カメラ１２の光軸方向に対して９０度の広がりを有する。
【００６８】
この車両挙動検出システムは、移動体挙動判定部３１によって、後方撮像カメラ１３により撮像された後方撮像画像に含まれる特徴点の動きに基づいて、自車両１の回転量を算出する。
【００６９】
このような車両挙動検出システムにおいて、後方撮像カメラ１３の後方撮像範囲に含まれると推定される後方特徴点と、実際に後方撮像画像に含まれた後方特徴点とのズレは、回転方向におけるズレＭθのみによると考えられる。したがって、この場合、後方撮像カメラ１３によって観測されるズレｄＰ１ｘは、
ｄＰ１ｘ＝−Ｍθ  （式１０）
となり、当該式１０によって、自車両１の回転量を求めることが可能となる。
【００７０】
ここで、自車両１を駐車枠に停車させる駐車操作において、駐車枠内の停止位置と予想される位置に自車両１を停止した状況を考える。このとき、演算装置１１は、自車両１の車輪回転量等を検出することにより、自車両１が直進方向に進行していないことが認識できる。また、前方撮像カメラ１２及び後方撮像カメラ１３により撮像された特徴点群より、自車両１の直進方向に関しての停止位置が正しいことを確認できるとする。
【００７１】
この状況において、車両挙動検出システムは、停止位置に対して自車両１が横方向にずれているのか、又は、停止位置に対して自車両１が回転方向にずれているのか正確に判別できる。これにより、車両挙動検出システムは、駐車操作をやり直す経路計画を正確に立てるために、車両挙動を検出できる。
【００７２】
以上説明したように、第２実施形態として示した車両挙動検出システムによれば、第１実施形態と同様に、前方特徴点の移動方向と後方特徴点の移動方向とを比較して、自車両１の横方向移動とヨー方向移動とを判別でき、自車両１のヨー方向の挙動と横方向の挙動とを高い精度で区別でき、自車両１の位置を求めることが可能となる。
【００７３】
また、この車両挙動検出システムによれば、後方撮像カメラ１３が、前方撮像カメラ１２の撮像方向に対して９０度方向の後方撮像画像を撮像するように構成している。これにより、車両挙動検出システムによれば、後方撮像カメラ１３を用いて自車両１の位置を推定することができるため、自車両１が停止しているときに高い精度で自車両１の位置を求めることが可能となる。
【００７４】
更に、この車両挙動検出システムによれば、後方撮像カメラ１３により撮像された後方撮像画像に含まれる特徴点の動きに基づいて、移動体挙動判定部３１によって自車両１の回転量を算出できる。これにより、車両挙動検出システムは、低い計算負荷で自車両１の挙動を求めることができる。
【００７５】
なお、上述の実施の形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施の形態以外であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計等に応じて種々の変更が可能であることは勿論である。
【００７６】
すなわち、上述した車両挙動検出システムは、自車両１の挙動を検出するものについて説明したが、他の移動体、例えばロボット等の挙動を検出するものであっても良い。
【符号の説明】
【００７７】
１  自車両
１１  演算装置
１２  前方撮像カメラ
１３  後方撮像カメラ
１４  周囲特徴記憶部
２１  前方特徴検出部
２２  前方位置取得部
２３  前方撮像範囲取得部
２４  前方特徴選択部
２５  前方移動計測部
２６  後方特徴検出部
２７  後方位置取得部
２８  後方撮像範囲取得部
２９  後方特徴選択部
３０  後方移動計測部
３１  移動体挙動判定部
【図１】
0005604987.tif 000004
【図２】
0005604987.tif 000005
【図３】
0005604987.tif 000006
【図４】
0005604987.tif 000007
【図５】
0005604987.tif 000008
【図６】
0005604987.tif 000009
【図７】
0005604987.tif 000010
【図８】
0005604987.tif 000011
【図９】
0005604987.tif 000012
【図１０】
0005604987.tif 000013
【図１１】
0005604987.tif 000014