JP4798487B2

JP4798487B2 - 周辺車両運動状態推定装置、自車運動状態推定装置、自車および周辺車両運動状態推定装置およびプログラム

Info

Publication number: JP4798487B2
Application number: JP2005340005A
Authority: JP
Inventors: 弘和赤羽; 敬大口; 良太堀口
Original assignee: Tokyo Metropolitan University
Current assignee: Tokyo Metropolitan University
Priority date: 2005-11-25
Filing date: 2005-11-25
Publication date: 2011-10-19
Anticipated expiration: 2025-11-25
Also published as: JP2007148615A

Description

本発明は、車両に搭載した観測機器が走行中に観測したデータに基づき、周辺車両の位置と運動状態を精度良く推定する周辺車両運動状態推定装置およびプログラムと、自車の位置と運動状態を精度良く推定する自車運動状態推定装置およびプログラムと、自車および周辺車両の位置と運動状態を精度良く推定する自車および周辺車両運動状態推定装置に関する。

従来の車両挙動の計測方法としては、車両感知器や地点画像計測により、ある道路断面の各車両の速度、車頭時間（交通流率）を計測している。また、フローティング調査やナンバープレート調査により、道路リンクや長い区間の旅行時間計測をしている。さらには、路側にビデオカメラを設置し、定点観測することにより、通過する車両の挙動を計測する手法もある（例えば、非特許文献１）。
しかし、運転者に対する道路幾何構造や信号などによる影響、あるいは運転者と周辺の車両の相互作用などを詳細な時間解像度で比較的に長い区間で調査する必要がある場合、交通流内部からの微視的な観測が必要である。このような観測を行なうために、速度、加速度計を搭載した２台の実験車両の相対距離をレーザ車間距離計で計測し、カルマンスムーザ（Kalman Smoother）により車両運動状態量を推定するシステムがある（例えば、非特許文献２）また、準静的な精密測量技術であるＲＴＫ−ＧＰＳ（Real Time Kinematic Global Positioning System）の適用を移動車両に試み、試験走路において走行実験を行なった例もある（例えば、非特許文献３）。
なお、車両の周囲の状況を観測する技術としては、世界座標系では動かずに静止している道路面を対象としているが、非特許文献４、非特許文献６、非特許文献８が知られている。
また、その他にも、従来技術として、非特許文献５と非特許文献７が知られている。
エイチ．アカハネ（H.Akahane）、エス．ハタケナカ（S.Hatakenaka）、サクセッシブオブザーベーションズオブトラジェクトリーズオブビークルズウィズプルーラルビデオカメラズ（Successive Observations of Trajectories of Vehicles With Plural Video Cameras）、"インターナショナルジャーナルオブＩＴＳリサーチ（International Journal of ITS Research）"、２００４、Ｖｏｌ２、Ｎｏ．１、ｐ．４７−５３赤羽弘和、大庭孝之、桑原雅夫、越正毅、車両の走行挙動計測システム、"土木計画学研究・講演集"、１９８８、Ｎｏ．１１、ｐ．６３−７０グルシン．ジー（Gurusinghe.G）、横粂昌典、中辻隆、吾田洋一、ＲＴＫ−ＧＰＳを用いた車両追従試験と車群中の反応時間特性、"第２１回交通工学研究発表会論文報告集"、２００１、ｐ．２１３−２１６全炳東、桜井勝巳、小林哲也、小沢慎治、車両から見た道路像の解析、"電子情報通信学会論文誌"、１９８８年９月、Ｖｏｌ．Ｊ７１−ＤＮｏ．９、ｐ．１７０９−１７１７農宗千典、芥川清、可変機能可変構造型パイプラインプロセッサをもつ高速画像処理装置、"電気学会論文誌Ｃ"、平成４年、１１２巻２号、ｐ．８１−８８農宗千典、小沢慎治、高速道路走行画像からの消失点推定に基づく実時間白線検出、"電気学会論文誌Ｃ"、平成５年、１１３巻２号、ｐ．１３９−１４８農宗千典、小沢慎治、道路走行シミュレーション画像の実時間合成、"電気学会論文誌Ｃ"、平成５年、１１３巻１１号、ｐ．９８７−９９５農宗千典、小沢慎治、連続道路画像からの道路構造とカメラ姿勢の同時推定、"電子情報通信学会論文誌"、１９９３年３月、Ｖｏｌ．Ｊ７６−Ｄ−ＩＩＮｏ．３、ｐ．５１４−５２３

しかしながら、非特許文献２や非特許文献３に示す装置にあっては、自車両の挙動は解析できるが、計測機器を備えない周辺車両の挙動を充分には解析できないという問題がある。また、計測データに含まれる系統誤差や偶然誤差の影響を受けるという問題や、ＧＰＳ測位の欠測により時空間連続の走行挙動が得られないという問題があった。

本発明は、このような事情に鑑みてなされたもので、その目的は、計測機器を備えない周辺車両の挙動を解析可能にする周辺車両運動状態推定装置を提供することにある。また、他の目的は、計測データに含まれる系統誤差や偶然誤差の影響を無くし、ＧＰＳ測位の欠測が起きても時空間連続の周辺車両および自車両の走行挙動を推定可能な周辺車両運動状態推定装置、自車両運動状態推定装置およびプログラムを提供することにある。

この発明は上述した課題を解決するためになされたもので、請求項１に記載の発明は、自車両に取り付けられ、前記自車両の周辺に存在する周辺車両の前記自車両との相対位置を測定する周辺車両相対位置測定手段と接続される周辺車両運動状態推定装置において、世界座標系における前記自車両の位置と姿勢角を取得する自車両位置取得手段と、前記周辺車両相対位置測定手段が測定した相対位置と、前記自車両に対する前記周辺車両の相対運動状態に関する状態遷移方程式および観測方程式とに基づき、前記周辺車両の相対運動状態を推定する周辺車両相対運動状態推定手段と、前記自車両位置取得手段が取得した前記自車両の位置と姿勢角および、前記周辺車両相対位置推定手段が推定した前記周辺車両の相対運動状態に基づき、前記周辺車両の絶対運動状態を取得する座標変換手段とを備えることを特徴とする周辺車両運動状態推定装置である。

また、請求項２に記載の発明は、請求項１に記載の周辺車両運動状態推定装置であって、前記相対運動状態は、前記自車両に紐付けられた移動座標系における位置、速度、加速度あるいは加加速度であり、前記絶対運動状態は、世界座標系における位置、速度、加速度あるいは加加速度であることを特徴とする。

また、請求項３に記載の発明は、請求項１または請求項２に記載の周辺車両運動状態推定装置であって、前記周辺車両相対位置測定手段が、前記自車両の周辺を撮影するビデオカメラである周辺車両運動状態推定装置において、前記観測方程式の観測変数の要素が、前記ビデオカメラで撮影した画像における前記周辺車両の特徴点の座標を含むことを特徴とする。

また、請求項４に記載の発明は、請求項３に記載の周辺車両運動状態推定装置であって、前記状態遷移方程式の状態変数の要素が、前記ビデオカメラの前記自車両への取付け角、前記ビデオカメラの焦点距離、および、前記ビデオカメラの取付け高さと前記周辺車両の特徴点の高さの差を含むことを特徴とする。

また、請求項５に記載の発明は、請求項１から請求項４のいずれかの項に記載の周辺車両運動状態推定装置であって、前記周辺車両相対位置測定手段が、前記自車両と前記周辺車両との距離を測定するレーザ測距装置を含む周辺車両運動状態推定装置において、前記観測方程式の観測変数の要素が、前記レーザ測距装置で測定した前記自車両と前記周辺車両との距離を含むことを特徴とする。

また、請求項６に記載の発明は、請求項１から請求項５のいずれかの項に記載の周辺車両運動状態推定装置であって、前記周辺車両相対運動状態推定手段は、拡張カルマンスムーザを前記状態遷移方程式および前記観測方程式に適用して、相対運動状態を推定することを特徴とする。

また、請求項７に記載の発明は、請求項１から請求項５のいずれかの項に記載の周辺車両運動状態推定装置であって、前記周辺車両相対運動状態推定手段は、拡張カルマンフィルタを前記状態遷移方程式および前記観測方程式に適用して、相対運動状態を推定することを特徴とする。

また、請求項８に記載の発明は、請求項１から請求項７のいずれかの項に記載の周辺車両運動状態推定装置であって、自車両に取り付けられ、前記自車両の走行状態として位置、前記自車両に紐付けられた移動座標系における速度、加速度および角速度を測定する自車両走行状態測定手段と接続される周辺車両運動状態推定装置において、前記自車両位置取得手段は、前記自車両走行状態測定手段が測定した走行状態と、世界座標系における前記自車両の位置、速度、加速度、加加速度、前記自車両の姿勢方向の加速度バイアス、世界座標系における前記自車両の姿勢角、自車両の姿勢角の角速度および自車両の姿勢角の角加速度を状態変数とする状態遷移方程式および前記自車両の走行状態に関する観測方程式とに基づき、世界座標系における前記自車両の位置と姿勢角を推定することを特徴とする。

また、請求項９に記載の発明は、自車両に取り付けられ、前記自車両の周辺に存在する周辺車両の前記自車両との相対位置を測定する周辺車両相対位置測定手段と接続されるコンピュータシステムを、世界座標系における前記自車両の位置と姿勢角を取得する自車両位置取得手段と、前記周辺車両相対位置測定手段が測定した相対位置と、前記自車両に対する前記周辺車両の相対運動状態に関する状態遷移方程式および観測方程式とに基づき、前記周辺車両の相対運動状態を推定する周辺車両相対運動状態推定手段と、前記自車両位置取得手段が取得した前記自車両の位置と姿勢角および、前記周辺車両相対位置推定手段が推定した前記周辺車両の相対運動状態に基づき、前記周辺車両の絶対運動状態を取得する座標変換手段として機能させることを特徴とするプログラムである。

また、請求項１０に記載の発明は、自車両に取り付けられ、前記自車両の走行状態として位置、前記自車両に紐付けられた移動座標系における速度、加速度および角速度を測定する自車両走行状態測定手段と接続される自車運動状態推定装置において、前記自車両走行状態測定手段が測定した走行状態と、世界座標系における前記自車両の位置、速度、加速度、加加速度、前記自車両の姿勢方向の加速度バイアス、世界座標系における前記自車両の姿勢角、自車両の姿勢角の角速度および自車両の姿勢角の角加速度を状態変数とする状態遷移方程式および前記自車両の走行状態に関する観測方程式とに基づき、前記状態変数を推定する自車両走行状態推定手段を備えることを特徴とする自車運動状態推定装置である。

また、請求項１１に記載の発明は、自車両に取り付けられ、前記自車両の走行状態として位置、前記自車両に紐付けられた移動座標系における速度、加速度および角速度を測定する自車両走行状態測定手段と接続されるコンピュータシステムを、前記自車両走行状態測定手段が測定した走行状態と、世界座標系における前記自車両の位置、速度、加速度、加加速度、前記自車両の姿勢方向の加速度バイアス、世界座標系における前記自車両の姿勢角、自車両の姿勢角の角速度および自車両の姿勢角の角加速度を状態変数とする状態遷移方程式および前記自車両の走行状態に関する観測方程式とに基づき、前記状態変数を推定する自車両走行状態推定手段として機能させることを特徴とするプログラムである。

また、請求項１２に記載の発明は、自車両に取り付けられ、前記自車両の走行状態として位置、前記自車両に紐付けられた移動座標系における速度、加速度および角速度を測定する自車両走行状態測定手段と、前記自車両に取り付けられ、前記自車両の周辺に存在する周辺車両の前記自車両との相対位置を測定する周辺車両相対位置測定手段と接続される自車および周辺車両運動状態推定装置において、前記自車両走行状態測定手段が測定した走行状態と、世界座標系における前記自車両の位置、速度、加速度、加加速度、前記自車両の姿勢方向の加速度バイアス、世界座標系における前記自車両の姿勢角、自車両の姿勢角の角速度および自車両の姿勢角の角加速度を状態変数とする第１の状態遷移方程式および前記自車両の走行状態に関する第１の観測方程式とに基づき、前記第１の状態変数を推定する自車両走行状態推定手段と、前記周辺車両相対位置測定手段が測定した相対位置と、前記自車両に対する前記周辺車両の相対運動状態に関する第２の状態遷移方程式および第２の観測方程式とに基づき、前記周辺車両の相対運動状態を推定する周辺車両相対運動状態推定手段と、前記自車両走行状態推定手段が取得した前記状態変数および、前記周辺車両相対位置推定手段が推定した前記周辺車両の相対運動状態に基づき、前記周辺車両の絶対運動状態を取得する座標変換手段とを備えることを特徴とする自車および周辺車両運動状態推定装置である。

この発明によれば、自車両位置取得手段が取得した自車両の絶対位置と姿勢角および、周辺車両相対位置推定手段が推定した周辺車両の相対運動状態に基づき、周辺車両の絶対運動状態を取得するので、計測機器を備えない周辺車両の挙動を得ることができる。

また、本発明によれば、自車両に対する周辺車両の相対運動状態に関する状態遷移方程式および観測方程式に基づき、周辺車両の相対運動状態を推定するので、計測データに含まれる系統誤差や偶然誤差の影響を無くし、ＧＰＳ測位の欠測が起きても時空間連続の周辺車両の走行挙動を得ることができる。

また、本発明によれば、自車両の運動状態に関する状態遷移方程式および観測方程式に基づき、自車両の運動状態を推定するので、計測データに含まれる系統誤差や偶然誤差の影響を無くし、ＧＰＳ測位の欠測が起きても時空間連続の自車両の走行挙動を得ることができる。

以下、図面を参照して、本発明の実施の形態について説明する。図１は、この発明の一実施形態による自車および周辺車両運動状態推定装置１００の構成を示す概略ブロック図である。自車および周辺車両運動状態推定装置１００には、車両に固定して取り付けられている自車走行状態観測機器２００として、ＲＴＫ−ＧＰＳ２０１、速度計２０２、３軸ジャイロ２０３が、自車走行状態観測機器２００同様に車両に固定して取り付けられている周辺車両相対位置観測機器３００として、ビデオカメラ３０１およびレーザ測距装置３０２が接続される。また、結果表示用にディスプレイ４００が接続される。ＲＴＫ−ＧＰＳ２０１は、自車の絶対位置を、速度計２０２は、自車の速度を、３軸ジャイロ２０３は、３軸方向の加速度および３軸回りの角速度を検出する。ビデオカメラ３０１は、図示しないが、ＧＰＳ信号から生成された同期信号により制御され、自車の周辺を走行する車両の様子をＧＰＳ信号ベースのタイムコードと共に録画する。レーザ測距装置３０２は、自車から周辺車両までの距離を計測する。
ここで、本実施形態における座標系は、世界座標系として、図２に示すように、東進方向をＸ軸、北進方向をＹ軸、上昇方向をＺ軸とし、Ｘ軸回りをロール角（ψ）、Ｙ軸回りをピッチ角（θ）、Ｚ軸回りをヨー角（φ）とする。また、自車に固定された移動座標系として、図３に示すように、自車の進行方向をｘ軸、自車左横方向をｙ軸、自車上向きをｚ軸とし、車両の全長の中線と全幅の中線の交点を路面に下ろした点をｘｙｚの原点とする。さらに、ビデオカメラ３０１にて撮影した画像については、図３に示すように、画像の右方向をｕ、上方向をｖとし、画像の中心を原点とする。このとき、移動座標系ｘｙｚを回転変換および平行移動した座標系を（ｐ，ｑ，ｒ）座標系とし、ｆをビデオカメラ３０１の焦点距離とすると、ｕ，ｖと（ｐ，ｑ，ｒ）の関係は［数１］で表される。

また、自車および周辺車両運動状態推定装置１００は、観測値取込処理１０１、観測データ１０２、画像取込処理１０３、画像データ１０４、特徴点座標抽出処理１０５、自車運動状態推定処理１０６、自車絶対位置および運動状態１０７、周辺車両相対位置推定処理１０８、周辺車両相対位置および運動状態１０９、座標変換処理１１０、周辺車両絶対位置および運動状態１１１および表示処理１１２からなる。
観測値取込処理１０１は、周辺車両相対位置観測機器３００のレーザ測距装置３０２および自車走行状態観測機器２００にて観測した値を取り込み、観測データ１０２に格納する。画像取込処理１０３は、周辺車両相対位置観測機器３００のビデオカメラ３０１が出力する画像を取り込み、画像データ１０４に格納する。特徴点座標抽出処理１０５は、画像データ１０４に格納されている画像を読み出し、測定対象となる周辺車両の特徴点（ここでは、ナンバプレート）の各時刻における該画像での座標を抽出し、観測データ１０２に格納する。自車運動状態推定処理１０６は、観測データ１０２より、自車走行状態観測機器２００にて観測したデータを取り出し、自車の状態遷移方程式および観測方程式（詳細な説明は後述する）に対して、拡張カルマンスムーザを適用して、観測したデータに基づき、世界座標系ＸＹＺにおける、自車の絶対位置および運動状態を推定して、自車絶対位置および運動状態１０７に格納する。

周辺車両相対位置推定処理１０８は、観測データ１０２より、ナンバプレートの画像での座標を取り出し、周辺車両の状態遷移方程式および観測方程式（詳細な説明は後述する）に対して、拡張カルマンスムーザを適用して、ナンバプレートの画像での座標に基づき、移動座標系ｘｙｚにおける、周辺車両の位置（自車からの相対位置）と運動状態を推定して、周辺車両相対位置および運動状態１０９に格納する。座標変換処理１１０は、自車絶対位置および運動状態１０７より、各時刻における自車の絶対位置と姿勢角を読み出し、これらに基づき、周辺車両相対位置および運動状態１０９に格納されている周辺車両の相対位置と運動状態を、世界座標系ＸＹＺにおける値に変換して、周辺車両絶対位置および運動状態１１１に格納する。表示処理１１２は、自車絶対位置および運動状態１０７と周辺車両絶対位置および運動状態１１１より、自車および周辺車両の絶対位置と運動状態を読み出し、自車および周辺車両の軌跡などをディスプレイ４００に表示する。

次に、自車運動状態推定処理１０６および周辺車両相対位置推定処理１０８にて使用する、状態遷移方程式および観測方程式と、座標変換処理１１０にて使用する座標変換について説明する。状態遷移方程式は、状態変数（ここではベクトルｘ）の時間経過による遷移関係を記述した式であり、一般的に［数２］のように、ある時刻ｔからΔｔ経過したときの状態変数を、時刻ｔにおける状態変数に状態遷移ベクトル関数ｆを施したベクトルと白色雑音ベクトルｗの和で表される。

自車運動状態推定処理１０６では、［数３］および［数４］に示すように、世界座標系ＸＹＺにおける自車両の位置、速度、加速度、加加速度（加速度の時間微分）、自車両姿勢方向の加速度バイアス、自車両の姿勢角、自車両の姿勢角の角速度、自車両の姿勢角の角加速度、３軸ジャイロ２０３の移動座標系ｘｙｚに対する取付不整角、移動座標系ｘｙｚに３軸ジャイロ２０３の取付不整角を加えた方向の角速度のバイアスを状態変数とする。これらの状態変数は、それぞれ３要素からなるため、状態変数ベクトルｘは、合計で３０の要素からなるベクトルとなる。なお、ここで、＜ψ、θ、φ＞で表される姿勢角は、ヨー角（φ）、ピッチ角（θ）、ロール角（ψ）の順に回転変換して得られるＮａｖｉｇａｔｉｏｎ角と呼ばれる姿勢角である（ベクトルの要素の並び順と回転変換の順は逆である）。

本実施形態では、［数３］、［数４］に示した状態変数に関する状態遷移方程式の状態遷移ベクトル関数ｆを［数５］、白色雑音ベクトルｗを［数６］、［数７］とする。

状態遷移ベクトル関数ｆ（［数５］）を参照すると分かるように、状態遷移方程式は線形方程式となっているので、状態遷移方程式は［数８］のように行列表現できる。このとき、状態遷移行列Ｆは［数９］のようになる。

観測方程式は、観測機器により観測した値と、状態変数の関係を表す式であり、一般的に、［数１０］のように、観測変数ベクトルｘ^ｍを、状態変数ベクトルｘに観測ベクトル関数ｈを施したベクトルと白色雑音ベクトルｅの和で表される。

自車運動状態推定処理１０６では、自車走行状態観測機器２００であるＲＴＫ−ＧＰＳ２０１、速度計２０２、３軸ジャイロ２０３の出力である世界座標系ＸＹＺにおける自車の位置、進行方向の速度、３軸ジャイロ２０３取付方向に対する加速度と角速度が、観測変数ベクトルｘ^ｍの要素となるので、観測変数ベクトルｘ^ｍは、［数１１］のように表され、観測ベクトル関数ｈは［数１２］となる。

観測ベクトル関数ｈ［数１２］は、非線形であるため、状態遷移ベクトル関数ｆのように行列表現することはできないが、観測ベクトル関数ｈを状態変数ベクトルｘで偏微分した、観測行列Ｈを用いて、近似的に表すことができる。このとき、観測方程式は、［数１３］のように表され、観測行列Ｈは、［数１４］から［数２２］で表される。

また、自車運動状態推定処理１０６における観測方程式の白色雑音ベクトルｅは、［数２３］である。

次に、周辺車両相対位置推定処理１０８における状態遷移方程式は、自車運動状態推定処理１０６と同様に、［数２］で表される。本実施形態では、［数２４］に示すように、自車両に固定された移動座標系ｘｙｚにおけるビデオカメラ３０１からの周辺車両（ナンバプレート）の相対位置、加速度、加加速度のｘｙ成分、ビデオカメラ３０１の取付高さと周辺車両のナンバプレートの取付高さの差、ビデオカメラ３０１の取付角と焦点距離、レーザ測距装置３０２の測拒点とビデオカメラ３０１によるトラッキング点との較差を状態変数ベクトルｘ_ｉとする。また、状態遷移ベクトル関数ｆは、［数２５］とする。

状態遷移ベクトル関数ｆ（［数２４］）を参照すると分かるように、周辺車両相対位置推定処理１０８においても、状態遷移方程式は線形方程式となっているので、状態遷移方程式は［数８］のように行列表現できる。このとき、状態遷移行列Ｆは［数２６］のようになる。

また、周辺車両相対位置推定処理１０８における状態遷移方程式の白色雑音ベクトルｗは、［数２７］とする。

次に、周辺車両相対位置推定処理１０８における観測方程式は、自車運動状態推定処理１０６と同様に、［数２８］で表される。本実施形態における周辺車両相対位置推定処理１０８では、周辺車両相対位置観測機器３００であるビデオカメラ３０１の出力画像より抽出した周辺車両のナンバプレートの座標およびレーザ測距装置３０２の出力である周辺車両のナンバプレートまでの距離が、観測変数ベクトルｙ_ｉの要素となるので、観測変数ベクトルｙ_ｉは、［数２９］のように表され、観測ベクトル関数ｈは［数３０］となる。

自車運動状態推定処理１０６と同様に、周辺車両相対位置推定処理１０８においても、観測ベクトル関数ｈ［数３０］は、非線形であるため、状態遷移ベクトル関数ｆのように行列表現することはできないが、観測ベクトル関数ｈを状態変数ベクトルｘ_ｉで偏微分した、観測行列Ｈを用いて、近似的に表すことができる。このとき、観測方程式は、［数３１］のように表され、観測行列Ｈは、［数３２］から［数３４］で表される。

また、周辺車両相対位置推定処理１０８における観測方程式の白色雑音ベクトルｅは、［数３５］である。

また、座標変換処理１１０においては、ビデオカメラ３０１からの相対位置＜ｘ_ｉ’，ｙ_ｉ’，ｚ_ｉ’＞を、［数３６］により世界座標系ＸＹＺに変換する。位置以外の速度、加速度、加加速度については、［数３７］にて、世界座標系ＸＹＺに変換する。

次に、この自車および周辺車両運動状態推定装置１００の動作を説明する。車両が走行した状態で、観測値取込処理１０１は、該車両に取り付けられた自車走行状態観測機器２００の各観測機器およびレーザ測距装置３０２から、Δｔ毎に観測値を取込み、ＧＰＳベースのタイムコードと共に、観測データ１０２に登録する。ここでは、ｔ＝０からＴまでの時間の観測値を取り込んで、登録したとする。この間、ビデオカメラ３０１（ここでは、斜め前方を撮影しているカメラとする）は、周辺の車両の様子をＧＰＳベースのタイムコードと共に録画し、録画した画像は、画像取込処理１０３により取り込まれて、画像データ１０４に格納される。特徴点座標抽出処理１０５は、オペレータにより、画像データ１０４に格納されている画像データの最初のフレームにて、観測対象となる周辺車両ｉの特徴点（ここでは、ナンバプレート）を指定されると、ｔ＝０からＴまでΔｔ毎のフレームにおけるナンバプレートを認識し、画像上でのその座標（ｕ_ｉ，ｖ_ｉ）を観測データ１０２に登録する。なお、本実施形態では、特徴点は、オペレータにより指定されるとしたが、オペレータによる指定ではなく、画像認識処理などにより、画像データからナンバプレートなどの特徴点を自動的に認識するようにしても良い。

自車運動状態推定処理１０６は、ｔ＝０からＴまでΔｔ毎の各時刻における自車走行状態観測機器による観測値を、観測データ１０２から読み出し、前述の状態遷移方程式と観測方程式に拡張カルマンスムーザを適用して、読み出した観測値に基づき、ｔ＝０からＴまでΔｔ毎の各時刻における［数３］に示した状態変数ベクトルの各要素を推定し、推定値を自車絶対位置および運動状態１０７に格納する。次に、周辺車両相対位置推定処理１０８は、ｔ＝０からＴまでΔｔ毎の各時刻における特徴点座標抽出処理１０５が抽出した、周辺車両ｉのナンバプレートの座標と、観測値取込処理１０１がレーザ測距装置３０２から取込んだ周辺車両ｉのナンバプレートまでの距離を観測データ１０２から読み出し、前述の状態遷移方程式と観測方程式に拡張カルマンスムーザを適用して、読み出した観測値に基づき、ｔ＝０からＴまでΔｔ毎の各時刻における［数２４］に示した状態変数ベクトルの各要素を推定し、推定値を周辺車両相対位置および運動状態１０９に格納する。

座標変換処理１１０は、ｔ＝０からＴまでΔｔ毎の各時刻に対して、周辺車両相対位置および運動状態１０９から、状態変数ベクトルの要素であるビデオカメラ３０１からの周辺車両相対位置＜ｘ_ｉ’，ｙ_ｉ’，ｚ_ｉ’＞を読み出し、また、自車絶対位置および運動状態１０７から、自車両の位置＜Ｘ，Ｙ，Ｚ＞および自車両の姿勢角＜ψ，θ，φ＞を読み出す。次に、［数３６］の＜Ｘ^ｅｐ，Ｙ^ｅｐ，Ｚ^ｅｐ＞に自車両の位置＜Ｘ，Ｙ，Ｚ＞を、Ｎａｖｉｇａｔｉｏｎ角変換の＜ψ^ｅｐ，θ^ｅｐ，φ^ｅｐ＞に自車両の姿勢角＜ψ，θ，φ＞を代入し、さらに、周辺車両相対位置および運動状態１０９から読み出した周辺車両相対位置＜ｘ_ｉ’，ｙ_ｉ’，ｚ_ｉ’＞を代入して、ｔ＝０からＴまでΔｔ毎の各時刻における周辺車両ｉの絶対位置＜Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ＞を得る。

さらに、ｔ＝０からＴまでΔｔ毎の各時刻に対して、周辺車両相対位置および運動状態１０９から、移動座標系における速度、加速度、加加速度を読み出し、自車絶対位置および運動状態１０７から、自車両の姿勢角＜ψ，θ，φ＞を読み出す。次に、［数３７］のＮａｖｉｇａｔｉｏｎ角変換の＜ψ^ｅｐ，θ^ｅｐ，φ^ｅｐ＞に自車両の姿勢角＜ψ，θ，φ＞を代入し、さらに、周辺車両相対位置および運動状態１０９から読み出した速度、加速度、加加速度を代入して、ｔ＝０からＴまでΔｔ毎の各時刻における周辺車両ｉの世界座標系ＸＹＺでの速度、加速度、加加速度を得る。座標変換処理１１０は、得られた周辺車両ｉの世界座標系ＸＹＺにおける位置、速度、加速度、加加速度を周辺車両絶対位置および運動状態１１１に格納する。

表示処理１１２は、ｔ＝０からＴまでΔｔ毎の各時刻における自車両および周辺車両ｉの位置を、自車絶対位置および運動状態１０７と周辺車両絶対位置および運動状態１１１から読み出し、これらの車両の通過した軌跡として、ディスプレイ４００に表示する。

なお、上記実施形態における周辺車両相対位置観測機器３００として、ビデオカメラ３０１とレーザ測距装置３０２を備えるとして説明したが、レーザ測距装置３０２は、必ずしも必要ではない。なお、レーザ測距装置３０２がない場合は、周辺車両相対位置推定処理１０８における状態変数ベクトル（［数２４］）における要素ｓ_ｉ’（レーザー測距装置３０２の測距点とトラッキング点との較差）、状態遷移行列Ｆ（［数２６］）の最終列、白色雑音ベクトルｗ（［数２７］）の１４番目（最後）の要素、観測変数ベクトル（［数２９］）の３番目の要素、観測行列Ｈ（［数３２］）の３行目、白色雑音ベクトルｅ（［数３５］）の３番目の要素が不要となる。
また、特徴座標抽出処理１０５にて、抽出する特徴点は、ある１台の周辺車両の特徴点であるとして説明したが、複数台の周辺車両の特徴点であってもよい。その場合、周辺車両相対位置推定処理１０８や座標変換処理１１０は、各周辺車両に対して、前述の処理と同様の処理を繰り返す。さらに、取り付けられるビデオカメラ３０１が、複数となった場合も同様に、各ビデオカメラ３０１が撮影した画像の各周辺車両に対して、前述の処理と同様の処理を繰り返す。

また、本実施形態では、観測機器を取り付けられた車両に自車および周辺車両運動状態推定装置１００も取り付けられているとして説明したが、観測が終わってから、各観測機器に自車および周辺車両運動状態推定装置１００を接続して、観測データや画像データを取り込むようにしてもよい。また、逆に自車および周辺車両運動状態推定装置１００は、常に観測機器に接続されており、リアルタイムに観測データおよび画像データ取り込み、さらに自車運動状態推定処理１０６、周辺車両相対位置推定処理１０８および座標変換１１０は、Δｔ毎に各状態変数の推定値を算出するようにしてもよい。この場合、推定処理には、拡張カルマンスムーザではなく、拡張カルマンフィルタを適用してもよい。

また、図１における観測値取込処理１０１、画像取込処理１０３、特徴点座標抽出処理１０５、自車運動状態推定処理１０６、周辺車両相対位置推定処理１０８、座標変換処理１１０、表示処理１１２の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより、これらの処理を行ってもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。

また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。

以上、この発明の実施形態を図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計等も含まれる。

計測機器を備えない周辺車両の挙動を微視的に計測することで、道路幾何構造や交通信号などによる運転者に対する影響や、周辺の車両と運転者との相互作用を解析するための周辺車両運動状態推定装置および自車運動状態推定装置に用いて好適である。

この発明の一実施形態による自車および周辺車両運動状態推定装置１００の構成を示すブロック図である。同実施形態における世界座標系ＸＹＺを説明する図である。同実施形態における移動座標系ｘｙｚを説明する図である。

符号の説明

１００…自車および周辺車両運動状態推定装置
１０１…観測値取込処理
１０２…観測データ
１０３…画像取込処理
１０４…画像データ
１０５…特徴点座標抽出処理
１０６…自車運動状態推定処理
１０７…自車絶対位置および運動状態
１０８…周辺車両相対位置推定処理
１０９…周辺車両相対位置および運動状態
１１０…座標変換処理
１１１…周辺車両絶対位置および運動状態
１１２…表示処理
２００…自車走行状態観測機器
２０１…ＲＴＫ−ＧＰＳ
２０２…速度計
２０３…３軸ジャイロ
３００…周辺車両相対位置観測機器
３０１…ビデオカメラ
３０２…レーザ測距装置
４００…ディスプレイ

Claims

自車両に取り付けられ、前記自車両の走行状態として位置、前記自車両に紐付けられた移動座標系における速度、加速度および角速度を測定する自車両走行状態測定手段と、前記自車両の周辺に存在する周辺車両の前記自車両との相対位置を測定する周辺車両相対位置測定手段とが接続される周辺車両運動状態推定装置において、
前記自車両走行状態測定手段が測定した走行状態と、世界座標系における前記自車両の位置、速度、加速度、加加速度、前記自車両の姿勢方向の加速度バイアス、世界座標系における前記自車両の姿勢角、自車両の姿勢角の角速度および自車両の姿勢角の角加速度を状態変数とする状態遷移方程式および前記自車両の走行状態に関する観測方程式とに基づき、世界座標系における前記自車両の位置と姿勢角を推定する自車両位置取得手段と、
前記周辺車両相対位置測定手段が測定した相対位置と、前記自車両に対する前記周辺車両の相対運動状態に関する状態遷移方程式および観測方程式とに基づき、前記周辺車両の相対運動状態を推定する周辺車両相対運動状態推定手段と、
前記自車両位置取得手段が取得した前記自車両の位置と姿勢角および、前記周辺車両相対位置推定手段が推定した前記周辺車両の相対運動状態に基づき、前記周辺車両の絶対運動状態を取得する座標変換手段と
を備えることを特徴とする周辺車両運動状態推定装置。
前記相対運動状態は、前記自車両に紐付けられた移動座標系における位置、速度、加速度あるいは加加速度であり、
前記絶対運動状態は、世界座標系における位置、速度、加速度あるいは加加速度であること
を特徴とする請求項１に記載の周辺車両運動状態推定装置。
前記周辺車両相対位置測定手段が、前記自車両の周辺を撮影するビデオカメラである周辺車両運動状態推定装置において、
前記観測方程式の観測変数の要素が、前記ビデオカメラで撮影した画像における前記周辺車両の特徴点の座標を含むこと
を特徴とする請求項１または請求項２に記載の周辺車両運動状態推定装置。
前記状態遷移方程式の状態変数の要素が、前記ビデオカメラの前記自車両への取付け角、前記ビデオカメラの焦点距離、および、前記ビデオカメラの取付け高さと前記周辺車両の特徴点の高さの差を含むこと
を特徴とする請求項３に記載の周辺車両運動状態推定装置。
前記周辺車両相対位置測定手段が、前記自車両と前記周辺車両との距離を測定するレーザ測距装置を含む周辺車両運動状態推定装置において、
前記観測方程式の観測変数の要素が、前記レーザ測距装置で測定した前記自車両と前記周辺車両との距離を含むこと
を特徴とする請求項１から請求項４のいずれかの項に記載の周辺車両運動状態推定装置。
前記周辺車両相対運動状態推定手段は、拡張カルマンスムーザを前記状態遷移方程式および前記観測方程式に適用して、相対運動状態を推定することを特徴とする請求項１から請求項５のいずれかの項に記載の周辺車両運動状態推定装置。
前記周辺車両相対運動状態推定手段は、拡張カルマンフィルタを前記状態遷移方程式および前記観測方程式に適用して、相対運動状態を推定することを特徴とする請求項１から請求項５のいずれかの項に記載の周辺車両運動状態推定装置。
自車両に取り付けられ、前記自車両の走行状態として位置、前記自車両に紐付けられた移動座標系における速度、加速度および角速度を測定する自車両走行状態測定手段と、前記自車両の周辺に存在する周辺車両の前記自車両との相対位置を測定する周辺車両相対位置測定手段とが接続されるコンピュータシステムを、
前記自車両走行状態測定手段が測定した走行状態と、世界座標系における前記自車両の位置、速度、加速度、加加速度、前記自車両の姿勢方向の加速度バイアス、世界座標系における前記自車両の姿勢角、自車両の姿勢角の角速度および自車両の姿勢角の角加速度を状態変数とする状態遷移方程式および前記自車両の走行状態に関する観測方程式とに基づき、世界座標系における前記自車両の位置と姿勢角を推定する自車両位置取得手段と、
前記周辺車両相対位置測定手段が測定した相対位置と、前記自車両に対する前記周辺車両の相対運動状態に関する状態遷移方程式および観測方程式とに基づき、前記周辺車両の相対運動状態を推定する周辺車両相対運動状態推定手段と、
前記自車両位置取得手段が取得した前記自車両の位置と姿勢角および、前記周辺車両相対位置推定手段が推定した前記周辺車両の相対運動状態に基づき、前記周辺車両の絶対運動状態を取得する座標変換手段と
して機能させることを特徴とするプログラム。
自車両に取り付けられ、前記自車両の走行状態として位置、前記自車両に紐付けられた移動座標系における速度、加速度および角速度を測定する自車両走行状態測定手段と接続される自車運動状態推定装置において、
前記自車両走行状態測定手段が測定した走行状態と、世界座標系における前記自車両の位置、速度、加速度、加加速度、前記自車両の姿勢方向の加速度バイアス、世界座標系における前記自車両の姿勢角、自車両の姿勢角の角速度および自車両の姿勢角の角加速度を状態変数とする状態遷移方程式および前記自車両の走行状態に関する観測方程式とに基づき、前記状態変数を推定する自車両走行状態推定手段を備えることを特徴とする自車運動状態推定装置。
自車両に取り付けられ、前記自車両の走行状態として位置、前記自車両に紐付けられた移動座標系における速度、加速度および角速度を測定する自車両走行状態測定手段と接続されるコンピュータシステムを、
前記自車両走行状態測定手段が測定した走行状態と、世界座標系における前記自車両の位置、速度、加速度、加加速度、前記自車両の姿勢方向の加速度バイアス、世界座標系における前記自車両の姿勢角、自車両の姿勢角の角速度および自車両の姿勢角の角加速度を状態変数とする状態遷移方程式および前記自車両の走行状態に関する観測方程式とに基づき、前記状態変数を推定する自車両走行状態推定手段として機能させることを特徴とするプログラム。
自車両に取り付けられ、前記自車両の走行状態として位置、前記自車両に紐付けられた移動座標系における速度、加速度および角速度を測定する自車両走行状態測定手段と、前記自車両に取り付けられ、前記自車両の周辺に存在する周辺車両の前記自車両との相対位置を測定する周辺車両相対位置測定手段と接続される自車および周辺車両運動状態推定装置において、
前記自車両走行状態測定手段が測定した走行状態と、世界座標系における前記自車両の位置、速度、加速度、加加速度、前記自車両の姿勢方向の加速度バイアス、世界座標系における前記自車両の姿勢角、自車両の姿勢角の角速度および自車両の姿勢角の角加速度を状態変数とする第１の状態遷移方程式および前記自車両の走行状態に関する第１の観測方程式とに基づき、前記状態変数を推定する自車両走行状態推定手段と、
前記周辺車両相対位置測定手段が測定した相対位置と、前記自車両に対する前記周辺車両の相対運動状態に関する第２の状態遷移方程式および第２の観測方程式とに基づき、前記周辺車両の相対運動状態を推定する周辺車両相対運動状態推定手段と、
前記自車両走行状態推定手段が取得した前記状態変数および、前記周辺車両相対位置推定手段が推定した前記周辺車両の相対運動状態に基づき、前記周辺車両の絶対運動状態を取得する座標変換手段と
を備えることを特徴とする自車および周辺車両運動状態推定装置。