JP2013159246A - 車両位置姿勢推定装置及びプログラム - Google Patents

Abstract

【課題】外界観測用センサを用いることなく、走行路に対する自車両の位置及び姿勢を推定する。
【解決手段】自車両の操舵角、車速、及びヨー角速度を取得し、パーティクル更新部２０で、走行路に対する自車両の位置及び姿勢を示す確率密度分布を表したパーティクルを、取得した車速及びヨー角速度に基づいて更新する。尤度分布算出部２２で、前方注視モデルに基づいて予め定めた自車両の位置及び姿勢と操舵角との関係を用いて、パーティクルが示す自車両の位置及び姿勢に対する現在の操舵角の尤度を算出する。パーティクルリサンプリング部２４で、更新されたパーティクルを算出された尤度に基づいてリサンプリングする。推定結果が収束するまでパーティクルの更新及びリサンプリングを繰り返し、収束した際に、位置姿勢推定部２６で、現在のパーティクルに基づいて、自車両の位置及び姿勢を推定する。
【選択図】図１

Description

本発明は、車両位置姿勢推定装置及びプログラムに係り、特に、走行路に対する自車両の位置及び姿勢を推定する車両位置姿勢推定装置及びプログラムに関する。

従来、１次元カメラの撮像データを記憶手段に順次記憶し、記憶手段に記憶された過去複数枚の撮像データに基づいて、白線位置検出手段により白線位置を検出し、この検出した白線位置と車速検出手段及び車両姿勢変化量検出手段で検出した車両姿勢挙動とに基づいて、演算手段により、道路曲率、ヨー角、偏位を求める道路形状及び自車両姿勢の計測装置が提案されている（例えば、特許文献１参照）。

特開平７−１９８３４９号公報

しかしながら、上記特許文献１に記載の技術では、車両周辺の外界環境の情報を取得するためにカメラを用いており、装置が高価になる、という問題がある。

本発明は、上記の問題点を解決するためになされたもので、ＧＰＳ装置やカメラ等の外界観測用センサを用いることなく、走行路に対する自車両の位置及び姿勢を推定することができる車両位置姿勢推定装置及びプログラムを提供することを目的とする。

上記目的を達成するために、本発明の車両位置姿勢推定装置は、自車両の運転操作量を示す情報及び車両運動を示す情報を取得する取得手段と、走行路に対する前記自車両の位置及び姿勢を示す確率密度分布を、前記取得手段により取得した前記車両運動を示す情報に基づいて更新する第１更新手段と、ドライバモデルに基づいて予め定めた自車両の位置及び姿勢と運転操作量を示す情報との関係を用いて、前記第１更新手段により更新された確率密度分布が示す前記自車両の位置及び姿勢に対する前記取得手段により取得された前記運転操作量を示す情報の尤度を算出する算出手段と、前記第１更新手段により更新された前記確率密度分布を、前記算出手段により算出された尤度に基づいて更新する第２更新手段と、前記第２更新手段により更新された前記確率密度分布に基づいて、前記自車両の位置及び姿勢を推定する推定手段と、を含んで構成されている。

本発明の車両位置姿勢推定装置によれば、取得手段が、自車両の運転操作量を示す情報及び車両運動を示す情報を取得する。そして、第１更新手段が、走行路に対する自車両の位置及び姿勢を示す確率密度分布を、取得手段により取得した車両運動を示す情報に基づいて更新する。また、算出手段が、ドライバモデルに基づいて予め定めた自車両の位置及び姿勢と運転操作量を示す情報との関係を用いて、第１更新手段により更新された確率密度分布が示す自車両の位置及び姿勢に対する取得手段により取得された運転操作量を示す情報の尤度を算出する。そして、推定手段が、第１更新手段により更新された確率密度分布を、算出手段により算出された尤度に基づいて更新する第２更新手段と、第２更新手段により更新された確率密度分布に基づいて、自車両の位置及び姿勢を推定する。

このように、運転操作量を示す情報及び車両運動を示す情報を用い、ドライバモデルに基づいて、自車両の位置及び姿勢を示す確率密度分布を更新して、自車両の位置及び姿勢を推定するため、ＧＰＳ装置やカメラ等の外界観測用センサを用いることなく、走行路に対する自車両の位置及び姿勢を推定することができる。

また、前記自車両の位置及び姿勢を示す確率密度分布を、正規分布またはパーティクルフィルタにおけるパーティクル密度で表すことができる。これにより、確率密度分布の更新による自車両の位置及び姿勢の推定が容易となる。

また、前記運転操作量を示す情報を、操舵角に関する物理量とすることができる。例えば、操舵角、実舵角、またはこれらの量に容易に変換可能な量とすることができる。

また、前記車両運動を示す情報を、前記自車両の速度に関する物理量、及び前記自車両のヨー角速度に関する物理量とすることができる。例えば、車速、ヨー角速度、これらの１次微積分値、及びそれらの組合せとすることができる。

また、前記ドライバモデルを、前方偏差と操舵角との関係を定めた前方注視モデル、または線形モデルとガウシアンノイズとによるモデルとすることができる。これらのドライバモデルを用いることで、運転操作量と自車両の位置及び姿勢との対応が得られ、自車両の位置及び姿勢の推定が容易となる。

また、前記推定手段は、前記第２更新手段により更新された前記確率密度分布において、確率が最大となる位置及び姿勢を推定結果として用いるか、または、前記第２更新手段により更新された前記確率密度分布から得られる期待値を推定結果として用いることができる。確率密度分布により位置及び姿勢を推定するため、これらの方法で推定結果を得ることにより、確率の高い推定結果を得ることができる。

また、本発明の車両位置姿勢推定プログラムは、コンピュータを、自車両の運転操作量を示す情報及び車両運動を示す情報を取得する取得手段、走行路に対する前記自車両の位置及び姿勢を示す確率密度分布を、前記取得手段により取得した前記車両運動を示す情報に基づいて更新する第１更新手段、ドライバモデルに基づいて予め定めた自車両の位置及び姿勢と運転操作量を示す情報との関係を用いて、前記第１更新手段により更新された確率密度分布が示す前記自車両の位置及び姿勢に対する前記取得手段により取得された前記運転操作量を示す情報の尤度を算出する算出手段、前記第１更新手段により更新された前記確率密度分布を、前記算出手段により算出された尤度に基づいて更新する第２更新手段、及び前記第２更新手段により更新された前記確率密度分布に基づいて、前記自車両の位置及び姿勢を推定する推定手段として機能させるためのプログラムである。

以上説明したように、本発明の車両位置姿勢推定装置及びプログラムによれば、運転操作量を示す情報及び車両運動を示す情報を用い、ドライバモデルに基づいて、自車両の位置及び姿勢を示す確率密度分布を更新して、自車両の位置及び姿勢を推定するため、ＧＰＳ装置やカメラ等の外界観測用センサを用いることなく、走行路に対する自車両の位置及び姿勢を推定することができる、という効果が得られる。

本実施の形態に係る車両位置姿勢推定装置の構成を示すブロック図である。 用語の定義を説明するための概略図である。 前方偏差εであるときの操舵角δの確率分布の一例を示すグラフである。 本実施の形態に係る車両位置姿勢推定装置における車両位置姿勢推定処理ルーチンの内容を示すフローチャートである。 パーティクルフィルタの更新の様子を示す概略図である。 パーティクル更新処理ルーチンの内容を示すフローチャートである。 尤度分布算出処理ルーチンの内容を示すフローチャートである。 パーティクルリサンプリング処理ルーチンの内容を示すフローチャートである。

以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、自車両の位置及び姿勢を推定するための確率密度分布として、パーティクルフィルタを用いる場合を例に説明する。

図１に示すように、本実施の形態に係る車両位置姿勢推定装置１０は、自車両の運転操作量を示す情報として操舵角を検出する操舵角センサ１２と、自車両の車両運動を示す情報として車速を検出する車速センサ１４、及びヨー角速度を検出するヨー角速度センサ１６と、走路に対する自車両の位置及び姿勢を推定する処理を実行するコンピュータ１８とを備えている。

コンピュータ１８は、ＣＰＵと、ＲＡＭと、後述する車両位置姿勢推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備えており、機能的には、図１に示すように、パーティクル更新部２０と、尤度分布算出部２２と、パーティクルリサンプリング部２４と、位置姿勢推定部２６とを含んだ構成で表すことができる。なお、パーティクル更新部２０が本発明の第１更新手段の一例であり、尤度分布算出部２２が本発明の算出手段の一例であり、パーティクルリサンプリング部２４が本発明の第２更新手段の一例であり、位置姿勢推定部２６が本発明の推定手段の一例である。

ここで、図２を参照して、以下の説明で用いる用語を定義する。図２に示すように、自車両が走行する車線と平行な軸をｘ軸、車線幅方向をｙ軸とし、車線中心をｙ＝０とする。車両位置ｙはｙ軸方向に沿ったｙ＝０の位置からの距離である。自車両の姿勢は、x軸方向を基準とした車両の回転角度であるヨー角θとする。また、横速度ｖ_ｙはｙ軸方向の自車両の速度である。また、予見時間ΔＴ秒後の自車両の横位置を前方偏差εとする。前方偏差εは、現在の自車両の横位置ｙ及び横速度ｖ_ｙを用いて、ε＝ｙ＋ｖ_ｙΔＴと表せる。以下、各部について詳述する。

パーティクル更新部２０は、自車両の位置及び姿勢を推定するための確率密度分布としてパーティクルフィルタを用い、予め与えられた初期分布に従ってパーティクルを生成する。具体的には、ｉ＝１，・・・，Ｎ（Ｎはパーティクル数）、ｔ＝０について、下記（１）式で表されるパーティクルｓⁱ _t-1|t-1を、下記（２）式の範囲においてランダムに生成する。ただし、Θ及びＹは定数である。

また、パーティクル更新部２０は、時刻ｔにおいて、車速センサ１４から取得した自車両の車速ｖ_ｔ、及びヨー角速度センサ１６から取得した自車両のヨー角速度γ_ｔに基づいて、パーティクルを更新する。ここで、時刻ｔにおけるヨー角θ_ｔ及び車両位置ｙ_ｔは、車速ｖ_ｔ及びヨー角速度γ_ｔを用いて、下記（３）式及び（４）式にように表現できる。

そこで、（３）式及び（４）式を用い、ｉ＝１，・・・，Ｎについて各パーティクルを更新して、下記（５）式とする。ただし、Δε_ｔはＮ（０，σ）に従うガウスノイズである。

尤度分布算出部２２は、パーティクル更新部２０で更新されたパーティクルに対して、操舵角センサ１２から取得した自車両の操舵角の尤度を算出する。ドライバが走行路を通常走行している場合のドライバモデルとして前方注視モデルが存在する。前方注視モデルは、下記（６）式に示すように、ドライバの操作量δが前方偏差εに比例することを仮定したモデルであり、車両がコースを追従走行している場合によく一致することが知られている。

δ＝ａ_１ε＋ｂ_１ （６）

そこで、前方偏差がεであるときの操舵角δの確率分布をＧＭＭ（Gaussian Mixture Model）により予め学習しておき、この確率分布に基づいて各パーティクルにおける尤度（下記（７）式）を算出する。

前方偏差がεであるときの操舵角δの確率分布の一例を図３に示す。図３では、横軸を前方偏差ε、縦軸を操舵角δとし、各座標が表す前方偏差εであるときの操舵角δの確率を濃淡で表している。各パーティクルが示すｙ及びθと、ｖ_ｙ＝ｖ_ｔｓｉｎθの関係から、ε＝ｙ＋ｖ_ｙΔＴにより前方偏差εを求め、そのεに対応する尤度ｐ（δ｜ε）を確率分布から取得する。以下では、各パーティクルについて各々算出された尤度全体を尤度分布という。

パーティクルリサンプリング部２４は、尤度分布算出部２２で算出された尤度分布に従って、パーティクルをリサンプリングする。具体的には、各パーティクルにおいて操舵角δの尤度に比例した重みπⁱ _tを下記（８）式により算出し、パーティクルＳ_t|t＝｛ｓ¹ _t|t-1，・・・，ｓ^N _t|t-1｝から、重みΠ_t＝｛π¹ _t，・・・，π^N _t｝に比例する割合でＮ個のパーティクルを復元抽出する。

より具体的には、下記（９）式を満たすｊを返す関数Ｆ（ｘ）を定義し、０から１までの実数を生成する乱数ｄに基づき、下記（１０）式〜（１２）式に従ってＮ個のパーティクルを復元抽出する。

位置姿勢推定部２６は、パーティクルリサンプリング部２４によりリサンプリングされたパーティクルに基づいて、下記（１３）式及び（１４）式により、時刻ｔにおける自車両の位置の期待値ｙ^_ｔ、及び姿勢の期待値θ^_ｔを算出し、これを位置及び姿勢の推定結果とする。

なお、位置及び姿勢の推定結果は、上記の期待値ｙ^_ｔ及びθ^_ｔとする場合に限定されず、確率が最大となるパーティクルの値を推定結果として用いてもよい。

次に、本実施の形態に係る車両位置姿勢推定装置１０の作用について説明する。自車両が走行中に、操舵角センサ１２による操舵角の検出、車速センサ１４による車速の検出、ヨー角速度センサ１６によるヨー角速度の検出が行われている状態で、コンピュータ１８により、図４に示す車両位置姿勢推定処理ルーチンが実行される。

ステップ１００で、操舵角センサ１２により検出された操舵角δ_ｔ、車速センサ１４により検出された車速ｖ_ｔ、及びヨー角速度センサ１６により検出されたヨー角速度γ_ｔを取得する。

次に、ステップ１０２で、（１）式及び（２）式により、予め与えられた初期分布に従ってパーティクルを生成する。この様子を図５（ａ）に示す。

次に、ステップ１０４で、パーティクル更新処理を実行する。パーティクル更新処理は、詳細には、図６に示すように、ステップ１０４０で、変数ｉに１をセットし、ステップ１０４２で、上記ステップ１００で取得した車速ｖ_ｔ及びヨー角速度γ_ｔに基づいて（５）式によりパーティクルを更新する。

次に、ステップ１０４４で、変数ｉがパーティクルの数Ｎを超えたか否かを判定することにより、未更新のパーティクルが存在するか否かを判定する。未更新のパーティクルが存在する場合には、ステップ１０４６へ移行して、変数ｉを１インクリメントしてステップ１０４２へ戻る。全てのパーティクルについて更新が終了した場合には、車両位置姿勢推定処理へリターンする。この様子を図５（ｂ）に示す。

次に、ステップ１０６で、尤度分布算出処理を実行する。尤度分布算出処理は、詳細には、図７に示すように、ステップ１０６０で、変数ｉに１をセットし、ステップ１０６２で、例えば図３に示すような前方偏差がεであるときの操舵角δの確率分布に基づいて、各パーティクルに対する操舵角の尤度を算出する。

次に、ステップ１０６４で、変数ｉがパーティクルの数Ｎを超えたか否かを判定することにより、全てのパーティクルについて尤度を算出したか否かを判定する。未処理のパーティクルが存在する場合には、ステップ１０６６へ移行して、変数ｉを１インクリメントしてステップ１０６２へ戻る。全てのパーティクルについて処理が終了した場合には、車両位置姿勢推定処理へリターンする。

次に、ステップ１０８で、パーティクルリサンプリング処理を実行する。パーティクルリサンプリング処理は、詳細には、図８に示すように、ステップ１０８０で、変数ｉに１をセットすると共に、上記ステップ１０６で算出した尤度分布に基づいて、各パーティクルに対する操舵角δの尤度に比例した重みπⁱ _tを（８）式により算出する。この様子を図５（ｃ）に示す。

次に、ステップ１０８２で、乱数ｄを発生させ、次に、ステップ１０８４で、（９）式を満たすｊを返す関数Ｆ（ｘ）を定義し、上記ステップ１０８２で発生させた乱数ｄに基づき、（１０）式〜（１２）式に従ってＮ個のパーティクルを復元抽出する。

次に、ステップ１０８６で、変数ｉが復元抽出すべきパーティクルの数Ｎを超えたか否かを判定する。復元抽出されたパーティクルがＮ個に到達していない場合には、ステップ１０８８へ移行して、変数ｉを１インクリメントしてステップ１０８２へ戻る。復元抽出されたパーティクルがＮ個に到達した場合には、車両位置姿勢推定処理へリターンする。この様子を図５（ｄ）に示す。

次に、ステップ１１０で、上記ステップ１０８でリサンプリングされたパーティクルに基づいて、（１３）及び（１４）式により、自車両の位置の期待値ｙ^_ｔ、及び姿勢の期待値θ^_ｔを算出する。

次に、ステップ１１２で、上記ステップ１１０で算出された期待値ｙ^_ｔ及びθ^_ｔが収束したか否かを判定する。収束したか否か判定は、前回の算出結果との差、すなわちｙ^_ｔ−ｙ^_ｔ−１及びθ^_ｔ−θ^_ｔ−１が所定値以下となったか否かにより判定することができる。また、繰り返し回数が所定回数となった場合に期待値ｙ^_ｔ及びθ^_ｔが収束したと判定するようにしてもよい。期待値ｙ^_ｔ及びθ^_ｔがまだ収束していないと判定された場合には、現在のパーティクルの分布を初期分布として、ステップ１０４へ戻り、処理を繰り返す。期待値ｙ^_ｔ及びθ^_ｔが収束したと判定された場合には、ステップ１１４へ移行して、上記ステップ１１０で算出された期待値ｙ^_ｔ及びθ^_ｔを推定結果として出力し、車両位置姿勢推定処理を終了する。

以上説明したように、本実施の形態の車両位置姿勢推定装置によれば、自車両の操舵角、車速、及びヨー角速度を用い、ドライバの前方注視モデルに基づいて、自車両の位置及び姿勢を示す確率密度分布を逐次更新しながら推定するため、ＧＰＳ装置やカメラ等の外界観測用センサを用いることなく、走行路に対する自車両の位置及び姿勢を推定することができる。

なお、上記実施の形態では、ドライバモデルとして前方注視モデルを用いる場合について説明したが、これに限定されない。例えば、線形モデル（重回帰モデル）を用いたドライバモデルを適用してもよい。この場合、車両位置及び姿勢などから構成される特徴量ベクトルをｘ、操舵角をδ、各特徴量ベクトルの係数ベクトルをｗとする。このとき操舵角δを、δ＝ｗｘ＋ｕによりモデル化する。ただし、ｕは平均０、分散σ^２で表される正規分布とする。また、その他のドライバモデルを適用してもよい。その場合、適用したドライバモデルに応じた運転操作量を示す情報及び車両運動を示す情報を取得するようにする。

また、上記実施の形態では、パーティクルフィルタの逐次更新により自車両の位置及び姿勢を推定する場合について説明したが、これに限定されず、例えばカルマンフィルタや正規分布を用いてもよい。

以下、拡張カルマンフィルタを用いた場合について、具体的に説明する。

まず、ｔ＝０において、下記（１５）式で表される初期パラメータｓ_t|tを、下記（１６）式の範囲においてランダムに生成する。ただし、Θ及びＹは定数である。また、以下では、ｓを状態と呼ぶ。

次に、状態ｓ_t|tを更新する。状態方程式を下記（１７）式のように定義する。ただし、δは操舵角、ｕ及びｗはそれぞれＮ（０，Ｑ）及びＮ（０，Ｒ）に従うガウスノイズである。

ここで、Δｔはサンプリング周期、ΔＴは予見時間、ｋは所定の係数である。このとき、下記の方法により、状態ｓの予測値ｓ_t|tを更新する。

ただし、▽ｆ（ｓ_t|t）及び▽ｈ（ｓ_t+1|t）は、それぞれｓ_t|tで線形化したｆのヤコビ行列、ｓ_t+1|tで線形化したｈのヤコビ行列を表す。

このようにして更新された状態ｓの予測値ｓ_t|tを、現在の自車両の位置及び姿勢の推定値として用いる。

また、上記実施の形態において、ドライバモデルに基づいて尤度分布を算出する際に、ＧＰＳを用いて得られる道路種別やＶＩＣＳ（登録商標）から得られる交通量等の情報に応じたモデルを用いるようにしてもよい。例えば、図３に示すような確率分布を、道路種別や交通量に応じて複数用意しておき、尤度分布算出の際に取得された情報に基づいて選択した確率分布から尤度を求めるようにするとよい。

なお、本発明のプログラムは、記録媒体に格納して提供することができる。

１０ 車両位置姿勢推定装置
１２ 操舵角センサ
１４ 車速センサ
１６ ヨー角速度センサ
１８ コンピュータ
２０ パーティクル更新部
２２ 尤度分布算出部
２４ パーティクルリサンプリング部
２６ 位置姿勢推定部

Claims (8)

1. 自車両の運転操作量を示す情報及び車両運動を示す情報を取得する取得手段と、
   走行路に対する前記自車両の位置及び姿勢を示す確率密度分布を、前記取得手段により取得した前記車両運動を示す情報に基づいて更新する第１更新手段と、
   ドライバモデルに基づいて予め定めた自車両の位置及び姿勢と運転操作量を示す情報との関係を用いて、前記第１更新手段により更新された確率密度分布が示す前記自車両の位置及び姿勢に対する前記取得手段により取得された前記運転操作量を示す情報の尤度を算出する算出手段と、
   前記第１更新手段により更新された前記確率密度分布を、前記算出手段により算出された尤度に基づいて更新する第２更新手段と、
   前記第２更新手段により更新された前記確率密度分布に基づいて、前記自車両の位置及び姿勢を推定する推定手段と、
   を含む車両位置姿勢推定装置。
2. 前記自車両の位置及び姿勢を示す確率密度分布を、正規分布またはパーティクルフィルタにおけるパーティクル密度で表した請求項１記載の車両位置姿勢推定装置。
3. 前記運転操作量を示す情報を、操舵角に関する物理量とした請求項１または請求項２記載の車両位置姿勢推定装置。
4. 前記車両運動を示す情報を、前記自車両の速度に関する物理量、及び前記自車両のヨー角速度に関する物理量とした請求項１〜請求項３のいずれか１項記載の車両位置姿勢推定装置。
5. 前記ドライバモデルを、前方偏差と操舵角との関係を定めた前方注視モデル、または線形モデルとガウシアンノイズとによるモデルとした請求項１〜請求項４のいずれか１項記載の車両位置姿勢推定装置。
6. 前記推定手段は、前記第２更新手段により更新された前記確率密度分布において、確率が最大となる位置及び姿勢を推定結果として用いるか、または、前記第２更新手段により更新された前記確率密度分布から得られる期待値を推定結果として用いる請求項１〜請求項５のいずれか１項記載の車両位置姿勢推定装置。
7. コンピュータを、
   自車両の運転操作量を示す情報及び車両運動を示す情報を取得する取得手段、
   走行路に対する前記自車両の位置及び姿勢を示す確率密度分布を、前記取得手段により取得した前記車両運動を示す情報に基づいて更新する第１更新手段、
   ドライバモデルに基づいて予め定めた自車両の位置及び姿勢と運転操作量を示す情報との関係を用いて、前記第１更新手段により更新された確率密度分布が示す前記自車両の位置及び姿勢に対する前記取得手段により取得された前記運転操作量を示す情報の尤度を算出する算出手段、
   前記第１更新手段により更新された前記確率密度分布を、前記算出手段により算出された尤度に基づいて更新する第２更新手段、及び
   前記第２更新手段により更新された前記確率密度分布に基づいて、前記自車両の位置及び姿勢を推定する推定手段
   として機能させるための車両位置姿勢推定プログラム。
8. コンピュータを、請求項１〜請求項６のいずれか１項記載の車両位置姿勢推定装置を構成する各手段として機能させるための車両位置姿勢推定プログラム。