(57)【特許請求の範囲】
【請求項１】
自動車に搭載される車載装置であって、
前記自動車周辺を撮影するカメラと、
前記カメラの配置を表す配置情報を校正する校正部とを有し、
前記校正部は、
前記カメラの前記自動車の基準点に対する位置と、前記カメラの姿勢と、前記自動車の位置する車線の幅と、前記車線の幅方向の前記自動車の位置と、前記自動車の位置する車線の方向に対する当該自動車の向きとを要素とする状態ベクトルをＮ個サンプリングするサンプリング処理を繰り返すサンプリング部と、
前記サンプリング部によってサンプリングされたＮ個の状態ベクトルを所定のダイナミクスモデルに基づいた量、もしくは、ランダムな量更新する更新部と、
前記更新部によって更新されたＮ個の状態ベクトルの各々について、当該状態ベクトルの各要素の値が真であった場合に、前記カメラで撮影した画像に含まれることになるレーンマークの画像中の形態を算定して、算定したレーンマークの形態と、前記カメラで撮影した画像中のレーンマークの形態との整合度を、当該状態ベクトルの重みとして算出する重み算出部と、
前記重み算出部が重みを算出した前記Ｎ個の状態ベクトル及び当該Ｎ個の状態ベクトルの重み、もしくは、前記サンプリング部によってサンプリングされたＮ個の状態ベクトルに基づいて、前記カメラの現在の配置を推定し、推定した配置を表す前記配置情報を生成する配置情報生成部とを有し、
前記サンプリング部は、２回目以降のサンプリング処理では、前記重み算出部が算出した前記Ｎ個の状態ベクトルの重みに基づいて、新たな前記Ｎ個の状態ベクトルをサンプリングすることを特徴とする車載装置。
【請求項２】
請求項１記載の車載装置であって、
前記配置情報が表す前記カメラの配置は、前記カメラの前記自動車の基準点に対する位置と、前記カメラの姿勢との少なくとも一方を含むことを特徴とする車載装置。
【請求項３】
請求項１記載の車載装置であって、
前記重み算出部は、前記更新部によって更新されたＮ個の状態ベクトルの各々について、当該状態ベクトルの各要素の値が真であった場合に、前記カメラで撮影した画像に含まれることになるレーンマークの画像中のエッジの位置に対応する、前記カメラで撮影した画像中の位置におけるエッジの有無に応じて、前記整合度を算出することを特徴とする車載装置。
【請求項４】
請求項１記載の車載装置であって、
前記自動車の走行状態を検出する走行状態検出部を備え、
前記更新部は、前記ダイナミクスモデルと前記走行状態検出部が検出した走行状態とに応じて、前記サンプリング部によってサンプリングされたＮ個の状態ベクトルを更新することを特徴とする車載装置。
【請求項５】
請求項１記載の車載装置であって、
前記カメラで撮影した画像に対して、前記校正部で生成された前記配置情報に基づく視点変換を施して、前記自動車周辺を上空から観察したようすを表す俯瞰画像を生成し表示する俯瞰画像表示部を有することを特徴とする車載装置。
【請求項６】
請求項１記載の車載装置であって、
前記サンプリング部は、２回目以降のサンプリング処理では、前記重み算出部が算出した前記Ｎ個の状態ベクトルの重みに基づいて、前記更新部によって更新されたＮ個の状態ベクトルのうちの重みが小さい状態ベクトルを消去し、重みが大きい状態ベクトルを複製することにより、前記新たなＮ個の状態ベクトルをサンプリングすることを特徴とする車載装置。
【請求項７】
請求項１記載の車載装置であって、
前記配置情報生成部は、前記重み算出部が算出した重みを用いた、前記Ｎ個の状態ベクトルの重み付き平均ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定することを特徴とする車載装置。
【請求項８】
請求項１記載の車載装置であって、
前記配置情報生成部は、前記重み算出部が算出した重みを用いた、前記Ｎ個の状態ベクトルの重み付き平均ベクトルの、時間平均ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定することを特徴とする車載装置。
【請求項９】
請求項１記載の車載装置であって、
前記配置情報生成部は、前記サンプリング部によってサンプリングされたＮ個の状態ベクトルの平均ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定することを特徴とする車載装置。
【請求項１０】
請求項１記載の車載装置であって、
前記配置情報生成部は、前記サンプリング部によってサンプリングされたＮ個の状態ベクトルの平均ベクトルの、時間平均ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定することを特徴とする車載装置。
【請求項１１】
請求項１記載の車載装置であって、
前記配置情報生成部は、前記重み算出部が算出した重みが最大となる状態ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定することを特徴とする車載装置。
【請求項１２】
請求項１記載の車載装置であって、
前記配置情報生成部は、前記重み算出部が算出した重みが最大となる状態ベクトルの、時間平均ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定することを特徴とする車載装置。
【請求項１３】
請求項１記載の車載装置であって、
前記配置情報生成部は、前記サンプリング部が所定回数のサンプリング処理を終了するまで、前記配置情報の生成を行わないことを特徴とする車載装置。
【請求項１４】
請求項１記載の車載装置であって、
前記配置情報生成部は、前記サンプリング部がサンプリングしたＮ個の状態ベクトルの分散もしくは標準偏差が所定のしきい値以下となるまで、前記配置情報の生成を行わないことを特徴とする車載装置。
【請求項１５】
請求項１記載の車載装置であって、
前記サンプリング部は、所定回数の前記サンプリング処理を終了したならば、前記サンプリング処理を終了することを特徴とする車載装置。
【請求項１６】
請求項１記載の車載装置であって、
前記サンプリング部は、サンプリングしたＮ個の状態ベクトルの分散もしくは標準偏差が所定のしきい値以下となったならば、前記サンプリング処理を終了することを特徴とする車載装置。
【請求項１７】
前記自動車周辺を撮影するカメラを備えた自動車に搭載されるコンピュータによって読みとられ実行されるコンピュータプログラムであって、
当該コンピュータプログラムは、前記コンピュータを、
前記カメラの前記自動車の基準点に対する位置と、前記カメラの姿勢と、前記自動車の位置する車線の幅と、前記車線の幅方向の前記自動車の位置と、前記自動車の位置する車線の方向に対する当該自動車の向きとを要素とする状態ベクトルをＮ個サンプリングするサンプリング処理を繰り返すサンプリング部と、
前記サンプリング部によってサンプリングされたＮ個の状態ベクトルを所定のダイナミクスモデルに基づいた量、もしくは、ランダムな量更新する更新部と、
前記更新部によって更新されたＮ個の状態ベクトルの各々について、当該状態ベクトルの各要素の値が真であった場合に、前記カメラで撮影した画像に含まれることになるレーンマークの画像中の形態を算定して、算定したレーンマークの形態と、前記カメラで撮影した画像中のレーンマークの形態との整合度を、当該状態ベクトルの重みとして算出する重み算出部と、
前記重み算出部が重みを算出した前記Ｎ個の状態ベクトル及び当該Ｎ個の状態ベクトルの重み、もしくは、前記サンプリング部によってサンプリングされたＮ個の状態ベクトルに基づいて、前記カメラの現在の配置を推定し、推定した配置を表す前記配置情報を生成する配置情報生成部として機能させ、かつ、
前記サンプリング部は、２回目以降のサンプリング処理では、前記重み算出部が算出した前記Ｎ個の状態ベクトルの重みに基づいて、新たな前記Ｎ個の状態ベクトルをサンプリングすることを特徴とするコンピュータプログラム。
【請求項１８】
請求項１７記載のコンピュータプログラムであって、
前記配置情報が表す前記カメラの配置は、前記カメラの前記自動車の基準点に対する位置と、前記カメラの姿勢との少なくとも一方を含むことを特徴とするコンピュータプログラム。
【請求項１９】
請求項１７記載のコンピュータプログラムであって、
前記重み算出部は、前記更新部によって更新されたＮ個の状態ベクトルの各々について、当該状態ベクトルの各要素の値が真であった場合に、前記カメラで撮影した画像に含まれることになるレーンマークの画像中のエッジの位置に対応する、前記カメラで撮影した画像中の位置におけるエッジの有無に応じて、前記整合度を算出することを特徴とするコンピュータプログラム。
【請求項２０】
前記自動車周辺を撮影するカメラを備えた自動車に搭載される車載装置において、前記カメラの配置を表す配置情報を校正するカメラの配置情報の校正方法であって、
前記カメラの前記自動車の基準点に対する位置と、前記カメラの姿勢と、前記自動車の位置する車線の幅と、前記車線の幅方向の前記自動車の位置と、前記自動車の位置する車線の方向に対する当該自動車の向きとを要素とする状態ベクトルをＮ個サンプリングするサンプリング処理を繰り返すサンプリングステップと、
前記サンプリングステップによってサンプリングされたＮ個の状態ベクトルを所定のダイナミクスモデルに基づいた量、もしくは、ランダムな量更新する更新ステップと、
前記更新ステップによって更新されたＮ個の状態ベクトルの各々について、当該状態ベクトルの各要素の値が真であった場合に、前記カメラで撮影した画像に含まれることになるレーンマークの画像中の形態を算定して、算定したレーンマークの形態と、前記カメラで撮影した画像中のレーンマークの形態との整合度を、当該状態ベクトルの重みとして算出する重み算出ステップと、
前記重み算出ステップが重みを算出した前記Ｎ個の状態ベクトル及び当該Ｎ個の状態ベクトルの重み、もしくは、前記サンプリング部によってサンプリングされたＮ個の状態ベクトルに基づいて、前記カメラの現在の配置を推定し、推定した配置を表す前記配置情報を生成する配置情報生成ステップとを有し、
前記サンプリングステップは、２回目以降のサンプリング処理では、前記重み算出ステップで算出した前記Ｎ個の状態ベクトルの重みに基づいて、新たな前記Ｎ個の状態ベクトルをサンプリングすることを特徴とするカメラの配置情報の校正方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自動車に搭載されたカメラの位置や姿勢などの配置を表す配置情報を校正する技術に関するものである。
【背景技術】
【０００２】
自動車に搭載されたカメラの位置や姿勢などの配置を表す配置情報を校正する技術としては、自動車周辺をカメラによって撮影した画像を、自動車上方の視点から自動車周辺を撮影した場合に得られる画像に視点変換して合成することにより、自動車周辺を俯瞰的に表す俯瞰画像を生成し表示する装置において、自動車に搭載したレーザ装置から路面に投影したレーザ光による像をカメラで撮影し、撮影した像よりカメラの俯角を校正する技術が知られている（たとえば、特許文献１）。
【特許文献１】特開2003-274394号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
さて、前記特許文献１記載のカメラの俯角の変化を校正する技術によれば、自動車に、カメラの他にレーザ装置を別途備えなければならないという問題がある。
そこで、本発明は、自動車に搭載されるカメラの位置や姿勢等の配置を表す配置情報を、より簡便な構成において校正することを課題とする。
【課題を解決するための手段】
【０００４】
前記課題達成のために、本発明は、自動車に搭載される車載装置に、前記自動車周辺を撮影するカメラと、前記カメラの配置を表す配置情報を校正する校正部とを備えると共に、前記校正部を、前記カメラの前記自動車の基準点に対する位置と、前記カメラの姿勢と、前記自動車の位置する車線の幅と、前記車線の幅方向の前記自動車の位置と、前記自動車の位置する車線の方向に対する当該自動車の向きとを要素とする状態ベクトルをＮ個サンプリングするサンプリング処理を繰り返すサンプリング部と、前記サンプリング部によってサンプリングされたＮ個の状態ベクトルを所定のダイナミクスモデルに基づいた量、もしくは、ランダムな量更新する更新部と、前記更新部によって更新されたＮ個の状態ベクトルの各々について、当該状態ベクトルの各要素の値が真であった場合に、前記カメラで撮影した画像に含まれることになるレーンマークの画像中の形態を算定して、算定したレーンマークの形態と、前記カメラで撮影した画像中のレーンマークの形態との整合度を、当該状態ベクトルの重みとして算出する重み算出部と、前記重み算出部が重みを算出した前記Ｎ個の状態ベクトル及び当該Ｎ個の状態ベクトルの重み、もしくは、前記サンプリング部によってサンプリングされたＮ個の状態ベクトルに基づいて、前記カメラの現在の配置を推定し、推定した配置を表す前記配置情報を生成する配置情報生成部とより構成したものである。ただし、前記サンプリング部は、２回目以降のサンプリング処理では、前記重み算出部が算出した前記Ｎ個の状態ベクトルの重みに基づいて、新たな前記Ｎ個の状態ベクトルをサンプリングするものである。また、前記配置情報が表す前記カメラの配置は、たとえば、前記カメラの前記自動車の基準点に対する位置と、前記カメラの姿勢との少なくとも一方を含むものである。
【０００５】
このような車載装置によれば、前記カメラの前記自動車の基準点に対する位置と、前記カメラの姿勢と、前記自動車の位置する車線の幅と、前記車線の幅方向の前記自動車の位置と、前記自動車の位置する車線の方向に対する当該自動車の向きとの状態によって、前記カメラに写り込むレーンマークの形態が一義的に定まることを利用して、これら状態をパーティクルとし、パーティクルが真である場合に前記カメラで撮影した画像に含まれることになるレーンマークの画像中の形態とカメラで撮影した画像に含まれるレーンマークの形態との整合度を重みとしてパーティクルフィルタを適用することにより、自動車に搭載されるカメラの位置や姿勢等の配置を求めることができるようになる。よって、レーザ装置等の特段の装置を必要としない簡便な構成において、自動車に搭載されるカメラの位置や姿勢等の配置を表す配置情報を校正することができる。
【０００６】
また、以上の車載装置の前記重み算出部における前記整合度の算定は、簡易的に、状態ベクトルの各要素の値が真であった場合に、前記カメラで撮影した画像に含まれることになるレーンマークの画像中のエッジの位置に対応する、前記カメラで撮影した画像中の位置におけるエッジの有無に応じて、前記整合度を算出することにより行うようにしてもよい。
【０００７】
また、前記車載装置に、前記自動車の走行状態を検出する走行状態検出部を備え、前記更新部において、前記ダイナミクスモデルと前記走行状態検出部が検出した走行状態に応じて、前記サンプリング部によってサンプリングされたＮ個の状態ベクトルを更新するようにすることも、状態ベクトルを真の状態に近似するように更新する上で好ましい。
【０００８】
なお、これらの車載装置の前記校正部で生成された前記配置情報は、カメラで撮影した画像を用いる各種処理に利用することができる。すなわち、たとえば、車載装置に、前記自動車周辺を上空から観察したようすを表す俯瞰画像を生成し表示する俯瞰画像表示部を設け、当該俯瞰画像表示部において、前記カメラで撮影した画像に対して、前記校正部で生成された前記配置情報に基づく視点変換を施して、前記俯瞰画像を生成するようにすれば、精度良く周辺を俯瞰的に表す俯瞰画像を生成することができる。
【０００９】
また、車載装置の前記サンプリング部における２回目以降のサンプリング処理は、より詳細には、前記重み算出部が算出した前記Ｎ個の状態ベクトルの重みに基づいて、前記更新部によって更新されたＮ個の状態ベクトルのうちの重みが小さい状態ベクトルを消去し、重みが大きい状態ベクトルを複製することにより、前記新たなＮ個の状態ベクトルをサンプリングすることにより行うようにしてもよい。
【００１０】
また、以上のような車載装置の前記配置情報生成部における、前記カメラの現在の配置の推定は、より詳細には、前記重み算出部が算出した重みを用いた、前記Ｎ個の状態ベクトルの重み付き平均ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定したり、前記重み算出部が算出した重みを用いた、前記Ｎ個の状態ベクトルの重み付き平均ベクトルの、時間平均ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定したり、前記サンプリング部によってサンプリングされたＮ個の状態ベクトルの平均ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定したり、前記サンプリング部によってサンプリングされたＮ個の状態ベクトルの平均ベクトルの、時間平均ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定したり、前記重み算出部が算出した重みが最大となる状態ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定したり、前記重み算出部が算出した重みが最大となる状態ベクトルの、時間平均ベクトルが表す前記カメラの配置を、前記カメラの現在の配置として推定することにより行うことができる。
【００１１】
また、以上のような車載装置は、生成する前記配置情報の信頼度を確保するために、前記配置情報生成部において、前記サンプリング部が所定回数のサンプリング処理を終了するまで、前記配置情報の生成を行わないようにしたり、前記サンプリング部がサンプリングしたＮ個の状態ベクトルの分散もしくは標準偏差が所定のしきい値以下となるまで、前記配置情報の生成を行わないように構成してもよい。
【００１２】
また、以上のような車載装置において、静的な状況で前記配置情報の校正を行う場合には、前記サンプリング部において、所定回数の前記サンプリング処理を終了したならば、前記サンプリング処理を終了するように構成したり、前記サンプリング部において、サンプリングしたＮ個の状態ベクトルの分散もしくは標準偏差が所定のしきい値以下となったならば、前記サンプリング処理を終了するように構成してもよい。
【発明の効果】
【００１３】
以上のように、本発明によれば、自動車に搭載されるカメラの位置や姿勢等の配置を表す配置情報を、より簡便な構成において校正することができる。
【発明を実施するための最良の形態】
【００１４】
以下、本発明の実施形態について説明する。
図１ａに、本実施形態に係る周辺監視装置の構成を示す。
図示するように、周辺監視装置は、カメラ１、カメラ状態校正部２、俯瞰画像生成部３、表示装置４、制御部５、車両状態センサ６を備えている。
ただし、周辺監視装置は、ハードウエアとしては、マイクロプロセッサやメモリやその他の周辺装置を備えたコンピュータを用いて構成することもでき、この場合、カメラ状態校正部２や俯瞰画像生成部３は、マイクロプロセッサが所定のコンピュータプログラムを実行することにより実現されるものであってよい。
【００１５】
ここで、車両状態センサ６は、自動車の車速やパーキングブレーキの制動状態や舵角や角速度などの、自動車の各種状態を検出周辺監視装置するセンサである。
また、カメラ１は、図１ｂに示すように、車両後端に配置され、たとえば、図２ａのように自動車が道路を走行中には、図２ｂに示すように自動車後方のレーンマーク（車線境界線）を含む映像を撮影する。
そして、俯瞰画像生成部３は、制御部５から俯瞰画像の生成及び表示を指示されたときに、カメラ１で撮影した自動車後方の画像に視点変換を施して、自動車上方の視点から自動車後方を撮影した場合に得られる画像である俯瞰画像を生成し、表示装置４に表示する。
すなわち、俯瞰画像生成部３は、たとえば、図３ａに示すように自動車が駐車場内で後退しているときに制御部５からら俯瞰画像の生成及び表示を指示されたならば、カメラ１が撮影した図３ｂに示すような自動車後方の画像に視点変換を施して、図３ｃに示すような俯瞰画像を生成し、生成した俯瞰画像に、図３ｄに示すように、予め用意しておいた自車後端を表す図形３０１を合成して、表示装置４に表示する。
【００１６】
ここで、このような俯瞰画像生成部３における、カメラ１で撮影した画像から俯瞰画像への視点変換は、カメラ状態校正部２から通知されるカメラ１の位置、姿勢に基づいて求まる、カメラ１で撮影した画像中の各位置と、自動車周辺の地面上の各位置との対応に従って行う。
そして、カメラ状態校正部２は、各時点におけるカメラ１の位置、姿勢を算出し、俯瞰画像生成部３に通知する。
以下、このようなカメラ１の位置、姿勢の通知を行うカメラ状態校正部２の動作について説明する。
さて、カメラ状態校正部２は、カメラ１の位置、姿勢を、パーティカルフィルタを用いて算出するカメラ状態校正処理を行う。
図４に、このカメラ状態校正処理の手順を示す。
図示するように、この処理では、まず、i=1からi=NまでのＮ個の状態ベクトルχ(i)=（tx, ty, tz, rx, ry, rz, w, dy, da)を無作為に作成する（ステップ４０２）。
ここで、図５ａ、ｂに示すように、自動車の左右前後方向中心の地面高さの位置を原点５００とし、自動車の前後方向軸をｘ軸、自動車の左右方向軸をｙ軸、高さ方向軸をｚ軸とする座標系を、座標系ＸＹＺとする。
そして、txは座標系ＸＹＺにおけるカメラ１のｘ座標、tyは座標系ＸＹＺにおけるカメラ１のy座標、 tzは座標系ＸＹＺにおけるカメラ１のｚ座標とし、これらの座標の値は、自動車へのカメラ１の取付位置によって定まる既知の固定値とする。また、図５ｃに示すように、rxは、カメラ１の撮影軸（カメラ１の撮影中心方向）５１０の座標系ＸＹＺにおけるｘ軸回りの回転量（ローリング量）、図５ｄに示すように、ryは、カメラ１の撮影軸５１０の座標系ＸＹＺにおけるｙ軸回りの回転量（ピッチング量）、図５ｅに示すように、rzは、カメラ１の撮影軸５１０の座標系ＸＹＺにおけるｚ軸回りの回転量（ヨーイング量）とする。
【００１７】
また、図５ｆに示すように、wは、自動車の左右のレーンマーク（車線境界線）５２０の間隔（車線幅）、dyは、自動車の左右のレーンマーク５２０の左右方向中央（車線の中央）からの座標系ＸＹＺの原点５００の左右方向の変位量、 daは、自動車の左右のレーンマーク５２０の延伸方向（車線方向）と座標系ＸＹＺのｘ軸（自動車の前後方向）とが水平平面上で成す角とする。
【００１８】
そして、（tx, ty, tz）の要素の値として上述した既知の固定値を有し、（rx, ry, rz, w, dy, da）の各々の要素の値として、予め定めたrx, ry, rz, w, dy, daの各々が取り得る範囲中の任意の値を有する各状態ベクトルχ=（tx, ty, tz, rx, ry, rz, w, dy, da)を元とするベクトル空間をベクトル空間Ｘ（χ∈Ｘ）として、ステップ４０２では、ベクトル空間Ｘから、Ｎ個の状態ベクトルχを無作為に抽出して、Ｎ個の状態ベクトルχ(i)とする。ただし、（tx, ty, tz）の要素の値を固定値としないようにしてもよく、この場合には、（tx, ty, tz, rx, ry, rz, w, dy, da)の各々の要素の値として、予め定めたtx, ty, tz, rx, ry, rz, w, dy, daの各々が取り得る範囲中の任意の値を有する各状態ベクトルχ=（tx, ty, tz, rx, ry, rz, w, dy, da)を元とするベクトル空間をベクトル空間Ｘ（χ∈Ｘ）として、ステップ４０２では、ベクトル空間Ｘから、Ｎ個の状態ベクトルχを無作為に抽出して、Ｎ個の状態ベクトルχ(i)とする。
【００１９】
さて、このようにしてＮ個の状態ベクトルχ(i)を作成したならば（ステップ４０２）、次に、各状態ベクトルχ(i)の重みを1/Ｎに設定する（ステップ４０４）。
そして、各状態ベクトルχ(i)に所定のダイナミクスモデルに従って更新する（ステップ４０６）。ここでは、簡易的に、各状態ベクトルχ(i)の（rx, ry, rz, w, dy, da）の各要素に所定のホワイトノイズを各々加えることにより、各状態ベクトルχ(i)を更新させるものとする。ただし、（tx, ty, tz）を固定値としない場合には、各状態ベクトルχ(i)の（tx, ty, tz, rx, ry, rz, w, dy, da)の各要素にホワイトノイズを各々加えることにより、各状態ベクトルχ(i)を変動させるものとする。
【００２０】
そして、更新した各状態ベクトルχ(i)についてのレーンマークエッジ位置を参照状態ベクトルχr(i)として生成する（ステップ４０８）。
ここで、ある状態ベクトルχ(k)についてのレーンマークエッジ位置とは、（tx, ty, tz, rx, ry, rz, w, dy, da)の各状態の値が、状態ベクトルχ(k)によって示される値であるときに、カメラ１によって撮影される画像中に含まれることになるレーンマークのエッジの位置を表すものであり、次のようにして求めることができる。
【００２１】
すなわち、まず、状態ベクトルχ(k)によって示される（w, dy, da)より、座標系ＸＹＺにおける自動車の左右のレーンマークのエッジの配置を求める。
次に、求めたレーンマークのエッジの配置より、レーンマークのエッジの各位置が写り込む、カメラ１の撮影画像上の座標（up, vp)を、下式１によって求める。
但し、下式１において、k1, k2はカメラ１のレンズの半径方向の歪み係数、p1, p2はカメラ１のレンズの円周方向の歪み係数 ｆx, fyはカメラ１の焦点距離 cx, cyはカメラ１のレンズの画像中の光学的な中心であり、各々使用するカメラ１、レンズに対して予め定まる。
【００２２】
【数１】
0005133783.tif 000002
【００２３】
そして、このようにして、Ｎ個の状態ベクトルχ(i)に対して、Ｎ個の参照状態ベクトルχr(i)を求めたならば（ステップ４０８）、カメラ１が撮影した画像中のレーンマークのエッジの位置を観測ベクトルｚとして、各参照状態ベクトルχr(i)と観測ベクトルｚとの整合度を、状態ベクトルχ(i)の尤度として算出する。
【００２４】
すなわち、いま、たとえば、観測ベクトルｚとして設定されたカメラ１で撮影された画像が図６ａに示すものであり、i=1からi=nの各参照状態ベクトルχr(i)として算出されたレーンマークのエッジ位置が、図６ｂ１、ｂ２...ｂｎに示すものであったとすると、ステップ４０８では、図６ｃ1、ｃ2...ｃｎに示すように、観測ベクトルｚが示すカメラ撮影画像６００中のレーンマークのエッジ６０１と、各参照状態ベクトルχr(i)が位置を示すレーンマークのエッジ６１０とを比較し、両者のエッジが近似しているものほど、尤度が高くなるように、各状態ベクトルχ(i)の尤度を算出する。
【００２５】
なお、観測ベクトルｚが示すカメラ撮影画像中のレーンマークのエッジと、各参照状態ベクトルχr(i)が位置を示すレーンマークのエッジとの比較による尤度の算出は、簡易的には、カメラ撮影画像中の、参照状態ベクトルχr(i)が位置を示すレーンマークのエッジの位置に、任意のエッジが存在するかどうかに応じて行うようにしてもよい。
【００２６】
すなわち、図６ｄに拡大して示すように、観測ベクトルｚが示すカメラ撮影画像６００の、参照状態ベクトルχr(i)が示すレーンマークのエッジ６１０上の各位置周辺６１１における、参照状態ベクトルχr(i)が示すレーンマークのエッジ６１０の延伸方向と垂直な方向６１２の輝度変化の勾配の大きさを求め、求めた輝度変化の勾配の大きさの平均値を、尤度として用いるようにしてもよい。
【００２７】
そして、各状態ベクトルχ(i)について算出した尤度に応じてベイズの定理に従って定まる事後確率を、各状態ベクトルχ(i)の重みとして設定する（ステップ４１２）。ここでは、各状態ベクトルχ(i)の重みの合計が１となり、かつ、算出した尤度が大きいものほど重みが大きくなるように各状態ベクトルχ(i)の重みを変更する。
【００２８】
そして、Ｎ個の状態ベクトルχ(i)の、ステップ４１２で算出した重みを重みとして用いた、重み付き平均状態ベクトルを俯瞰画像生成部３にカメラ１の位置、姿勢を表すカメラ状態ベクトルとして出力する（ステップ４１４）。
なお、カメラ状態ベクトルを受け取った俯瞰画像生成部３は、カメラ状態ベクトルの要素（tx, ty, tz, rx, ry, rz）をカメラ１の位置、姿勢として用いて前述のようにカメラ１の撮影した画像に視点変換を施して俯瞰画像を生成し、表示装置４に表示する。
また、次に、Ｎ個の状態ベクトルχ(i)の重みに応じて、新たなＮ個の状態ベクトルχ(i)を生成（リサンプリング）する（ステップ４１６）。このステップ４１６では、重みが小さい状態ベクトルχを消去し、重みが大きい状態ベクトルを複製して新たなＮ個の状態ベクトルχ(i)を生成する。すなわち、たとえば、まず、各状態ベクトルχ(i)の重みを、各重みが０または１/Ｎの整数倍となり、かつ、各状態ベクトルχ(i)の重みの合計が１となるように各状態ベクトルχ(i)の重みを量子化する。そして、重みが０の状態ベクトルを削除し、重みがＬ/Ｎ（Ｌ＞2）倍の状態ベクトルを複製してＬ-1個の状態ベクトルを生成する。そして、削除しなかった重みが１/Ｎ以上の状態ベクトルと、複製して生成した状態ベクトルとを新たなＮ個の状態ベクトルχ(i)とする。
【００２９】
そして、ステップ４０２からの処理に戻る。
以上、カメラ状態校正部２が行うカメラ状態校正処理について説明した。
なお、以上のカメラ状態校正処理のステップ４０６は、制御部５が車両状態センサ６を用いて算出した自動車の挙動と自動車の所定のダイナミクスモデルより推定される、（rx, ry, rz, w, dy, da）の前回各状態ベクトルχ(i)を作成してから後の変動量に適当なホワイトノイズを加えた値を、各状態ベクトルχ(i)の（rx, ry, rz, w, dy, da）の各要素に各々加えることにより、各状態ベクトルχ(i)を更新させる処理としてもよい。ただし、（tx, ty, tz）を固定値としない場合には、各状態ベクトルχ(i)の（tx, ty, tz, rx, ry, rz, w, dy, da)の各要素に、前回各状態ベクトルχ(i)を作成してから後の変動量に適当なホワイトノイズを加えた値を各々加えることにより、各状態ベクトルχ(i)を変動させるようにする。
【００３０】
また、以上のカメラ状態校正処理のステップ４１４は、生成したＮ個の状態ベクトルχ(i)の重み付き平均状態ベクトルの最近所定時間長の時間平均を俯瞰画像生成部３にカメラ状態ベクトルとして出力する処理としてもよい。または、当該ステップ４１４は、ステップ４１２で設定した各状態ベクトルχ(i)のうちの重みが最大値となった状態ベクトルや、その最近所定時間長の時間平均を俯瞰画像生成部３にカメラ状態ベクトルとして出力する処理としてもよい。または、当該ステップ４１４は、ステップ４１２で設定した各状態ベクトルχ(i)の重み（事後確率）に基づいて、ベクトル空間Ｘ上での事後確率分布を内挿法などにより推定し、事後確率がピーク値をとる状態ベクトルや、その最近所定時間長の時間平均を俯瞰画像生成部３にカメラ状態ベクトルとして出力する処理としてもよい。
【００３１】
または、当該ステップ４１４に代えて、ステップ４１６でリサンプリングしたＮ個の状態ベクトルχ(i)の平均状態ベクトル、または、その最近所定時間長の時間平均を俯瞰画像生成部３にカメラ状態ベクトルとして出力する処理を設けるようにしてもよい。
また、以上のカメラ状態校正処理は、処理を開始してから、ステップ４０４からステップ４１６のステップを所定回数繰り返すまでは、俯瞰画像生成部３にカメラ状態ベクトルを出力しないように構成してもよい。または、ステップ４１４で生成されたＮ個の状態ベクトルχ(i)の分散や標準偏差が所定のしきい値を下回っていない場合には、俯瞰画像生成部３にカメラ１の位置、姿勢を表すカメラ状態ベクトルを出力しないように構成してもよい。
【００３２】
または、以上のカメラ状態校正処理を自動車が静的な状態にあるときに行う場合などには、カメラ状態校正処理は、ステップ４０４からステップ４１６のステップを所定回数繰り返したときや、ステップ４１４で生成されたＮ個の状態ベクトルχ(i)の分散や標準偏差が所定のしきい値を下回ったときに、その処理を終了するものとしてもよい。
【００３３】
以上、本発明の実施形態について説明した。
このように本実施形態によれば、カメラ１の前記自動車の基準点に対する位置と、前記カメラ１の姿勢と、前記自動車の位置する車線の幅と、前記車線の幅方向の前記自動車の位置と、前記自動車の位置する車線の方向に対する当該自動車の向きとの状態によって、前記カメラ１に写り込むレーンマークの形態が一義的に定まることを利用して、これら状態をパーティクルとし、パーティクルが真である場合に前記カメラ１で撮影した画像に含まれることになるレーンマークの画像中の形態とカメラ１で撮影した画像に含まれるレーンマークの形態との整合度を重みとしてパーティクルフィルタを適用することにより、自動車に搭載されるカメラ１の位置や姿勢等の配置を求めることができる。よって、レーザ装置等の特段の装置を必要としない簡便な構成において、自動車に搭載されるカメラ１の位置や姿勢等の配置を表す配置情報を校正することができる。
【００３４】
なお、以上では、カメラ状態校正部２で算出したカメラ状態ベクトルを、俯瞰画像生成部３における俯瞰画像生成のための視点変換に用いる場合について説明したが、カメラ状態校正部２で算出したカメラ状態ベクトルは、各種処理においてカメラ１の位置、姿勢を特定するために用いることができる。すなわち、たとえば、カメラ１で撮影した画像に基づいて周辺障害物を検出する場合に、カメラ状態校正部２で算出したカメラ状態ベクトルが表すカメラ１の位置、姿勢を用いて、カメラ１が撮影した画像中に写り込んだ周辺障害物の位置や方向を算出するなどすることができる。
【図面の簡単な説明】
【００３５】
【図１】本発明の実施形態に係る周辺監視装置の構成を示すブロック図である。
【図２】本発明の実施形態においてカメラが撮影する画像を示す図である。
【図３】本発明の実施形態において表示する俯瞰画像を示す図である。
【図４】本発明の実施形態に係るカメラ状態校正処理を示すフローチャートである。
【図５】本発明の実施形態で用いる状態ベクトルの各要素を示す図である。
【図６】本発明の実施家形態に係るカメラ状態校正処理の処理例を示す図である。
【符号の説明】
【００３６】
１…カメラ、２…カメラ状態校正部、３…俯瞰画像生成部、４…表示装置、５…制御部、６…車両状態センサ。
【図１】
0005133783.tif 000003
【図２】
0005133783.tif 000004
【図３】
0005133783.tif 000005
【図４】
0005133783.tif 000006
【図５】
0005133783.tif 000007
【図６】
0005133783.tif 000008