JP7014197B2 - 推定装置 - Google Patents

Description

本発明は、推定装置に関する。

従来から、車両に取り付けた各種センサで計測した計測値と、車両の運動状態をモデル化した運動方程式を用いて車両の走行状態を推定する推定装置が提案されている（例えば、特許文献１～特許文献３参照）。

特開２０１６－１２０８８２号公報 再表２０１０／１４０２３４号 特開平１０－３１８７４２号公報

車両の走行状態を推定する場合、路面の傾き（カント角）は車両の運動に影響を与える要素となることから、従来の推定装置の中には、例えばロールレートセンサで計測されたロールレートを用いて、進行方向と直交する車両の横方向の傾き（姿勢角）を推定するものが存在する。

しかしながら、例えば姿勢角には路面のカント角の他、路面に対する車両のロール角も含まれるため、路面のカント角を精度よく推定するためには車両のロール角も推定する必要があるが、従来の推定装置では路面のカント角と共に車両のロール角を推定する推定装置は提案されていない。

本発明は、上記に示した問題点を鑑みてなされたものであり、路面のカント角と共に車両のロール角を推定することができる推定装置を提供することを目的とする。

上記の目的を達成するために、請求項１に記載の推定装置は、目標経路に対する車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角のそれぞれの計測値と、前記計測値、前記車両が走行する路面のカント角、及び前記車両のロール角を用いて前記車両の横方向の運動状態をモデル化した第１の運動方程式、前記計測値を用いて前記車両のヨー方向の運動状態をモデル化した第２の運動方程式、並びに、前記計測値及び前記車両のロール角を用いて前記車両のロール方向の運動状態をモデル化した第３の運動方程式によって示される複数の運動方程式とを用いて、前記車両が走行する路面のカント角及び前記車両のロール角を含む前記車両の状態量を推定する推定部を備える。

請求項２記載の発明では、前記計測値は、更に前記車両のロールレート及び前記車両の横方向の加速度に関するそれぞれの計測値を含み、前記複数の運動方程式は、前記車両のロールレート、及び前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に用いてモデル化され、前記車両の状態量は、前記車両のロールレート、前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に含む。

請求項３記載の発明では、前記推定部は、前記複数の運動方程式に基づいて、前回推定された前記車両の状態量から、次時刻の前記車両の状態量を予測する予測部と、観測方程式を用いて、予測された次時刻の前記車両の状態量に対応する前記それぞれの計測値を算出し、前記算出された前記それぞれの計測値と、計測された前記それぞれの計測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を更新する更新部と、を含む。

請求項４記載の発明では、前記目標経路に対する車両のずれ量、及び前記目標経路の曲率のそれぞれの計測値は、車両前方又は車両後方を撮像するカメラにより撮像された画像と、前記車両の状態量として前回推定された前記ロール角とに基づいて算出される。

本発明によれば、路面のカント角と共に車両のロール角を推定することができる、という効果を有する。

推定装置の機能ブロックの一例を示す図である。 車両２の運動状態の一例を示す図である。 推定処理の流れの一例を示すフローチャートである。 車線認識処理の流れの一例を示すフローチャートである。 運動方程式に用いられる状態量の入出力例を示す図である。

以下、本実施の形態について図面を参照しながら説明する。なお、同じ構成要素及び処理には全図面を通して同じ符合を付与し、重複する説明を省略するものとする。

図１は、走行中の車両２の状態量を推定する推定装置１０の機能ブロック図の一例である。図１に示すように、推定装置１０は、推定部１２及び車両位置演算部１４を含み、車両位置演算部１４には撮像装置２２が接続され、推定部１２には車両位置演算部１４、ＩＭＵ(Inertial Measurement Unit)２４、及び操舵装置２６がそれぞれ接続されている。

撮像装置２２は車両２の走行方向の画像（「前方画像」ともいう）を撮像する単眼カメラであり、車両位置演算部１４は撮像装置２２から前方画像を受け付けると、車両位置を前方画像から演算する車線認識処理を行う。具体的には、車両位置演算部１４は、目標経路４に対する車両２のずれ量及び目標経路４の曲率を車両２の位置情報として前方画像から演算する。車両位置演算部１４は、目標経路４に対する車両２のずれ量及び目標経路４の曲率を撮像装置２２による計測値として推定部１２に通知する。

また、車両位置演算部１４は、後述する推定部１２で推定された車両２の状態量のうち車両２のロール角を用いて、演算した車両２の位置情報が実際の車両２の位置情報に近づくように位置情報を補正する。すなわち、車両位置演算部１４は、本実施の形態に係る演算部の一例である。

なお、撮像装置２２は必ずしも車両２に必要なセンサでなく、目標経路４に対する車両２のずれ量及び目標経路の曲率を計測することができれば、撮像装置２２の代わりに例えばレーダ装置を用いてもよい。また、撮像装置２２は前方画像の代わりに、車両２の走行方向とは逆方向の画像（「後方画像」ともいう）を撮影してもよい。車両位置演算部１４は後方画像からも車両２の位置情報を演算することができる。

ＩＭＵ２４は、慣性センサを用いて、例えば車両２のロールレート及びヨーレートを計測すると共に、車両２の横加速度を計測する装置である。

以降では、ロールレートを計測するセンサのオフセットを「ロールレートセンサのオフセット」、ヨーレートを計測するセンサのオフセットを「ヨーレートセンサのオフセット」、及び車両２の横加速度を計測するセンサのオフセットを「横加速度センサのオフセット」ということにする。なお、「オフセット」とは各々のセンサにおける計測誤差を意味する。

操舵装置２６は、車両２の操舵角を制御して車両２の進行方向を可変させる装置であり、制御した操舵角を推定部１２に通知する。

撮像装置２２と、ＩＭＵ２４に含まれるロールレートセンサ、ヨーレートセンサ、及び横加速度センサと、操舵装置２６は、各種センサの一例である。

推定部１２は、前回推定した車両２の状態量、及び車両２に設置された各種センサによって計測された計測値を入力として、車両２の運動状態をモデル化した複数の運動方程式に基づく状態方程式、及び各種センサの計測値を車両２の状態量を用いて表した車両２の観測方程式を用いて、現時刻における車両２の状態量を推定する。

具体的には、推定部１２は、状態方程式に基づいて、前回推定された車両２の状態量から、次時刻の車両２の状態量を予測する図示しない予測部と、観測方程式を用いて、予測部で予測された次時刻の前記車両の状態量に対応する各種センサの計測値を算出し、算出した各種センサの計測値と、各種センサで実際に計測された計測値とのそれぞれの差分、及び推定部１２で前回推定された車両２の状態量に基づいて、車両２の状態量を更新する図示しない更新部を含む。

車両の「状態量」とは、例えば車両の走行位置、走行方向、車両の傾き、走行する道路の形状、及び各種センサのオフセットのように、車両の走行状態を特定する場合に用いられる物理量の集合をいう。推定部１２は推定した車両２の状態量を、例えば状態量に基づいて車両２の制御を行う図示しない制御装置に出力する。

＜車両の運動方程式＞

図２は、走行中の車両２における運動状態の一例を示す図である。図２（Ａ）に示すように、目標経路４に対する車両２の横位置偏差をｅ₁、ヨー角偏差をｅ₂、目標経路の曲率をρ、車両２の車速をＶとすれば、２輪モデルの運動を規定するヨーレートψ^・と車両２の横方向速度ｙ^・はそれぞれ（１）式及び（２）式で表される。なお、例えば（１）式ではｅ₂の上にドット（・）が付加されているが、これはｅ₂ ^・とも表記され、ドットの数は微分の階数を表す。以降、各式のドットについては同様の対応付けが適用される。

更に、推定部１２で用いる状態方程式は、（３）式～（５）式で表される運動方程式に基づいて定められる。

ここでｌ_fは車両２の重心から前輪までの距離、ｌ_rは車両２の重心から後輪までの距離を表す（図２（Ａ）参照）。Ｋ_f及びＫ_rはそれぞれ車両２における前輪及び後輪のコーナリングスティフネスを表す。また、δは車両２の操舵角、ｍは車両２の質量、ｇは重力加速度、ｈはロールセンターから車両重心までの距離、ｋ_xはバネ上ロール剛性、Ｃ_rはバネ上ロール方向のダンピング定数、Ｉ_zはヨー方向の慣性モーメント、Ｉ_xはロール方向の慣性モーメントをそれぞれ表す。

また、φ_cは路面のカント角、及びφ_vは車両２のロール角をそれぞれ表す。図２（Ｂ）に示すように、カント角φ_cは水平面に対する路面の傾きを表し、ロール角φ_vは路面と直交する直交線に対する車両２の傾きを表す。

（３）式で表される運動方程式は車両２の横方向の運動を表す運動方程式であり、本実施の形態に係る第１の運動方程式の一例である。（４）式で表される運動方程式は車両２のヨー方向の運動を表す運動方程式であり、本実施の形態に係る第２の運動方程式の一例である。（５）式で表される運動方程式は車両２のロール方向の運動を表す運動方程式であり、本実施の形態に係る第３の運動方程式の一例である。

なお、後述するように、車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρは撮像装置２２で撮像した前方画像又は後方画像から取得される。また、車速Ｖは図示しない車速センサから取得される。

＜操舵系の外乱＞

上記に示した車両２の運動方程式では車両２の操舵角をδで表したが、操舵装置２６から取得される操舵角、すなわち、計測された操舵角は、例えば操舵系が有するヒステリシス特性等による制御遅れや、ハンドルの遊び及び操舵系のがたつき等の外乱により、必ずしもタイヤの切れ角と一致しない場合がある。すなわち、計測された操舵角にはオフセットが含まれる。

操舵系の制御遅れは１次遅れとして近似可能であり、１次遅れの時定数をτ、操舵角のオフセットをγ^’、時間をｔとすれば、計測された操舵角δ_γは（６）式で表される。

（６）式に対して、時間ｔによって変化する操舵角の外乱γを（７）式のように定義した場合、計測された操舵角δ_γは（８）式によって表されることになる。すなわち、（３）式～（５）式に示した車両２の運動方程式では、操舵装置２６の過渡応答性による外乱量と操舵装置２６のオフセットによる外乱量が、操舵角の外乱γという１つのパラメータによって表されている。以降では、操舵角δは外乱が含まれない操舵角の真値を表す。

＜状態方程式＞

車両２の状態量を（９）式のように定義する。

（９）式の状態量において、α_rはヨーレートセンサのオフセット、α_φはロールレートセンサのオフセット、α_Gは横加速度センサのオフセットを表す。

そして、（３）式～（５）式に示した車両２の運動方程式に基づいて、車両２の状態量ｘの状態方程式を行列式で表した場合、（１０）式のように表記される。

ここでＥは単位行列であり、Ａは各状態量の係数を表す係数行列である。推定部１２は、カルマンフィルタを用いて（１１）式に示した状態方程式を予め定めた間隔で逐次演算することにより、車両２の状態量ｘを推定する。

実際にカルマンフィルタを用いて車両２の状態量ｘを推定するためには、（１１）式で表される状態方程式を離散化した状態方程式が用いられる。（１２）式は、（１１）式の状態方程式を離散化した状態方程式である。

変数ｋは時系列の順序を表す整数である。すなわち、ｘ_kは時刻ｋにおける車両２の状態量を表し、ｘ_k+1は時刻ｋの次の時刻（ｋ＋１）における車両２の状態量を表す。Ｄは状態行列と呼ばれ、Ｄ＝Ｅ^-1Ａである。

したがって、カルマンフィルタを用いて状態量ｘの各要素を推定するためには、要素毎に離散化した状態方程式（１３）式が用いられる。

ここで（１３）式で表される各要素の状態方程式におけるｗは「システムノイズ」と呼ばれ、状態方程式の白色雑音として定義される分散値である。また、状態量ｘの各要素の分散値を行列値とする対角行列Ｑを、システムノイズを表す共分散行列という。

（１３）式において、ｅ_1,k ^・・、ｅ_2,k ^・・、及びφ_v,k ^・・は（３）式～（５）式で示した車両２の運動方程式をそれぞれｅ₁ ^・・、ｅ₂ ^・・、φ_v ^・・について変形した（１４）式～（１６）式に示す運動方程式によって定義される。

このように、本実施の形態に係る車両２の運動方程式には、目標経路４の曲率ρ及び曲率変化率ρ^・が含まれる。

以降では（１４）式～（１６）式に示す運動方程式を車両２の運動方程式として用いることにする。

＜観測方程式＞

推定部１２では各種センサで計測された計測値と、（１１）式に示した状態方程式を用いて予測した状態量とを比較する。このとき、予測した状態量を、各種センサの計測値に変換して比較する。例えばヨーレートセンサの計測値をｒとすれば、ヨーレート計測値ｒは、車両２の状態量に基づいて（１７）式の観測方程式から得られる。

（１７）式によれば、目標経路４の曲率ρを加味した車両２のヨーレートを推定することができる。

一方、ロールレートセンサの計測値をφ^・とすれば、ロールレート計測値φ^・は、車両２の状態量に基づいて（１８）式の観測方程式から得られる。

（１８）式によれば、車両２のロールレートは、推定されたカント角の微分値φ^・ _c、ロール角の微分値φ_v ^・、及びロールレートセンサのオフセットα_φから推定することができる。

更に、横加速度センサの計測値をａ_yとすれば、横加速度センサ計測値ａ_yは、車両２の状態量に基づいて（１９）式の観測方程式から得られる。

（１９）式によれば、車両２の横方向にかかる加速度は、車両２の横位置偏差の２階微分ｅ₁ ^・・、車速Ｖ、曲率ρ、カント角φ_c、ロール角φ_v、及び横加速度センサのオフセットα_Gから推定することができる。また、（１９）式の右辺１項目で表される車両２の運動に伴って車両２の横方向にかかる加速度、及び右辺２項目で表される重力による加速度を分離して推定することも可能である。

車両２の状態量から算出される、各種センサの計測値ｙをｙ＝(ｅ₁ ｅ₂ ρ r φ^・ ａ_y δ_γ)とすれば、時刻ｋにおける計測値ｙは（２０）式で表される。

行列Ｃは観測行列と呼ばれ、行列Ｒは観測方程式の観測ノイズを示す共分散行列である。

＜推定装置の処理＞

次に、推定装置１０における車両２の状態量ｘの推定方法について説明する。

図３は、例えば推定装置１０が推定の開始指示を受け付けた場合に、推定装置１０に備えられたＣＰＵによって実行される推定処理の流れの一例を示すフローチャートである。

推定処理を規定する推定プログラムは、例えば推定装置１０に備えられたＲＯＭに予め記憶されている。推定装置１０のＣＰＵは、ＲＯＭに記憶される推定プログラムを読み込んで推定処理を実行する。

なお、推定装置１０のＲＡＭには前回推定した時刻ｋにおける車両２の状態量ｘ_kが記憶されているものとする。前回推定した車両２の状態量ｘ_kが存在しない場合には、予め設定された状態量ｘの初期値が前回推定した時刻ｋにおける車両２の状態量ｘ_kとして用いられる。

推定処理ではカルマンフィルタを用いた状態量ｘ_k+1の推定が行われる。そのため、ステップＳ１０において、推定部１２は撮像装置２２、ＩＭＵ２４、及び操舵装置２６の各種センサで計測された計測値を取得する。具体的には、推定部１２は、車両位置演算部１４を介して、撮像装置２２で撮像された画像から得られた車両２のずれ量の一例である横位置偏差ｅ₁及びヨー角偏差ｅ₂と、目標経路４の曲率ρを取得する。また、推定部１２は、車両２のロールレート計測値φ^・、ヨーレート計測値ｒ、及び横加速度センサ計測値ａ_yをＩＭＵ２４から取得する。更に、推定部１２は計測された操舵角δ_γを操舵装置２６から取得する。すなわち、推定部１２は計測値ｙを取得する。

上述したように、車両位置演算部１４は撮像装置２２で撮像された画像に対して車線認識処理を行い、車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρを計測することから、車両位置演算部１４の処理について説明する。

図４は、車両位置演算部１４で実行される、車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρを計測する車線認識処理の流れの一例を示すフローチャートである。車線認識処理を規定する車線認識プログラムも、例えば推定装置１０に備えられたＲＯＭに予め記憶されている。推定装置１０のＣＰＵは、ＲＯＭに記憶される車線認識プログラムを読み込んで車線認識処理を実行する。

ここでは一例として、車両２の目標経路４が路面上の車線に沿って設定されているものとする。

ステップＳ１００において、車両位置演算部１４は、車両２の前方画像を撮像装置２２から取得する。車両位置演算部１４は後方画像を撮像装置２２から取得してもよいが、ここでは前方画像を取得する例について説明する。

ステップＳ１１０において、車両位置演算部１４は、公知のパターン認識又は画像処理を用いて、ステップＳ１００で取得した前方画像から車線を抽出する。

ステップＳ１２０において、車両位置演算部１４は、ステップＳ１１０で抽出した車線を、路面を車両２の真上から見た図（鳥瞰図）に投影する座標変換を実行する。

ステップＳ１３０において、車両位置演算部１４は、例えば道路をモデル化したクロソイド曲線に車線の３次元位置を当てはめることで、路面上における車線の位置を推定する車線パラメータ推定を実行する。

ステップＳ１４０において、車両位置演算部１４は、ステップＳ１３０で推定した路面上における車線の位置に基づいて、目標経路４に対する車両２のずれ量の一例である横位置偏差ｅ₁及びヨー角偏差ｅ₂を算出する。また、車両位置演算部１４は、ステップＳ１３０で推定した路面上における車線の位置に基づいて、目標経路４の曲率ρを算出する。

そして、車両位置演算部１４は、算出した車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρを推定部１２に通知する。

なお、車両位置演算部１４は、目標経路４の曲率ρの単位時間における変化量から曲率変化率ρ^・を算出して車両位置演算部１４に通知してもよい。以上により、図４に示した車線認識処理を終了する。

図３のステップＳ２０において、推定部１２は、前回推定した車両２の状態量ｘ_kの各々を（１３）式に代入し、時刻（ｋ＋１）における車両２の状態量ｘ_k+1を予測する。なお、説明の便宜上、ステップＳ２０で予測した状態量ｘ_k+1を特にｘ_k+1 ^-と表すことにする。

上述したように（１３）式におけるｅ_1,k ^・・、ｅ_2,k ^・・、及びφ_v,k ^・・は、（１４）式～（１６）式に示す運動方程式から得られる。

図５は、ｅ_1,k ^・・、ｅ_2,k ^・・、及びφ_v,k ^・・が、（１４）式～（１６）式に示す運動方程式から得られることを示す図である。

車両２の横方向の運動方程式は（１４）式で表されることから、車両２の横位置偏差の２階微分値ｅ₁ ^・・は、状態量ｘ_kに含まれる操舵角δ、操舵角の外乱γ、車両２の横位置偏差の１階微分値ｅ₁ ^・、車両２のヨー角偏差ｅ₂、車両２のヨー角偏差の１階微分値ｅ₂ ^・、目標経路４の曲率ρ、カント角φ_c、ロール角φ_v、及びロール角の１階微分値φ_v ^・から推定される。

また、車両２のヨー方向の運動方程式は（１５）式で表されることから、車両２のヨー角偏差の２階微分値ｅ₂ ^・・は、状態量ｘ_kに含まれる操舵角δ、操舵角の外乱γ、車両２の横位置偏差の１階微分値ｅ₁ ^・、車両２のヨー角偏差ｅ₂、車両２のヨー角偏差の１階微分値ｅ₂ ^・、目標経路４の曲率ρ、及び目標経路４の曲率変化率ρ^・から推定される。

更に、車両２のロール方向の運動方程式は（１６）式で表されることから、車両２のロール角の２階微分値φ_v ^・・は、状態量ｘ_kに含まれる操舵角δ、操舵角の外乱γ、車両２の横位置偏差の１階微分値ｅ₁ ^・、車両２のヨー角偏差ｅ₂、車両２のヨー角偏差の１階微分値ｅ₂ ^・、目標経路４の曲率ρ、ロール角φ_v、及びロール角の１階微分値φ_v ^・から推定される。

（１３）式に示すように、車両２の横位置偏差の２階微分値ｅ₁ ^・・、車両２のヨー角偏差の２階微分値ｅ₂ ^・・、及び車両２のロール角の２階微分値φ_v ^・・を積分した値と、各種センサで計測された計測値を用いてカルマンフィルタによるフィルタリングを行うことで、ｅ₁、ｅ₁ ^・、ｅ₂、ｅ₂ ^・、φ_v、φ_v ^・がそれぞれ推定される。

図５に示すように、カント角φ_cは車両２の横方向の運動のみに影響を与え、ロール角φ_vは車両２の横方向の運動方程式及びロール方向の運動に影響を与える。したがって、推定部１２は、カント角φ_cとロール角φ_vが影響を与える運動に違いがあることを利用して、カント角φ_cとロール角φ_vをそれぞれ分離して推定する。

また、図５に示すように、カント角φ_cは車両２の横方向の運動のみに影響を与え、操舵角の外乱γは車両２の横方向の運動、及びヨー方向の運動に影響を与える。したがって、推定部１２は、カント角φ_cと操舵角の外乱γが影響を与える運動に違いがあることを利用して、カント角φ_cと操舵角の外乱γをそれぞれ分離して推定する。

なお、（１６）式に示す車両２のロール方向の運動方程式には操舵角の外乱γが含まれるため、図５では操舵角の外乱γが車両２のロール方向の運動にも影響を与えているように見える。しかし、（５）式で示される変形前の車両２のロール方向の運動方程式には操舵角の外乱γは含まれないことから、操舵角の外乱γは車両２の横方向の運動、及びヨー方向の運動に影響を与えることになる。

また、本実施の形態では、操舵角の外乱γが１つのパラメータでモデル化されているため、例えば操舵装置２６の過渡応答性による外乱量と操舵装置２６のオフセットによる外乱量をそれぞれ分離した運動方程式を用いる場合と比較して、カルマンフィルタを用いた車両２の状態量ｘ_kの推定が単純になり、車両２の状態量ｘ_kの推定に要する時間を短縮することができる。

ステップＳ３０において、推定部１２は、前回推定した状態方程式の状態ノイズを示す共分散行列Ｐ_kを（２１）式に代入し、時刻（ｋ＋１）における共分散行列Ｐ_k+1を予測する。なお、説明の便宜上、ステップＳ３０で予測した共分散行列Ｐ_k+1を特にＰ_k+1 ^-と表すことにする。

ステップＳ４０において、推定部１２は（２２）式によって、ステップＳ３０で予測した共分散行列Ｐ_k+1 ^-と、（２０）式における観測行列Ｃ及び共分散行列Ｒを用いてカルマンゲイン行列Ｇ_kを算出する。

ステップＳ５０において、推定部１２は（２３）式に従って、ステップＳ２０で予測した状態量ｘ_k+1 ^-、ステップＳ４０で算出したカルマンゲイン行列Ｇ_k、ステップＳ１０で取得した計測値ｙ、及び計測値ｙの観測行列Ｃを用いて、ステップＳ２０で予測した状態量ｘ_k+1 ^-を更新する。更新後の状態量ｘ_k+1 ^-は状態量ｘ_k+1で表される。

（２３）式に示すように、状態量ｘ_k+1はセンサの計測値ｙと、観測行列Ｃで重み付けを行った状態量ｘ_k+1 ^-の差分に対して、カルマンゲイン行列Ｇ_kを作用させることで更新される。

ステップＳ６０において、推定部１２は（２４）式に従って、ステップＳ４０で算出したカルマンゲイン行列Ｇ_k、ステップＳ１０で取得した計測値ｙの観測行列Ｃ、及びステップＳ３０で予測した前回の共分散行列Ｐ_k ^-を用いて、ステップＳ３０で予測した共分散行列Ｐ_k+1 ^-を更新する。更新後の共分散行列Ｐ_k+1 ^-は共分散行列Ｐ_k+1で表される。なお、Ｉは単位行列を表す。

ステップＳ７０において、推定部１２はステップＳ５０で更新した状態量ｘ_k+1を出力する。

ステップＳ８０において、推定部１２は、推定処理の終了指示を受け付けたか否かを判定し、終了指示を受け付けていない場合にはステップＳ１０に移行して、現在のセンサの計測値ｙを取得する。ステップＳ１０～Ｓ８０を繰り返し実行することで、時系列に沿った車両２の状態量ｘ_k+n(nは１以上の整数）を推定することができる。

なお、ステップＳ８０の判定処理で終了指示を受け付けていないと判定された場合、予め定めた間隔だけ待機してからステップＳ１０に移行するようにしてもよい。この場合、待機時間を調整することで、例えば１００Ｈｚや１ｋＨｚというように予め定めた周期で車両２の状態量ｘ_k+nを推定することができる。推定装置１０で推定された状態量ｘ_k+nは未来の状態量を表すことから、状態量ｘ_k+nは例えば目標経路４に沿った車両２の自動運転に利用される。

一方、ステップＳ８０の判定処理で終了指示を受け付けたと判定される場合には、図３に示す推定処理を終了する。

なお、単眼カメラを用いた車両位置演算部１４における車線認識処理では、単に前方画像だけから車両２の位置情報の演算する場合、路面に対する車両２のロール角φ_vが加味されない。したがって、車両位置演算部１４には、ステップＳ７０で出力された車両２の状態量のうちロール角φ_vが入力される。車両位置演算部１４は、入力されたロール角φ_vを用いて、前方画像だけから得られた車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρを補正する。

図４に示した車線認識処理において、ロール角φ_vを用いた車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρの補正位置は複数存在する。

例えば、車両位置演算部１４はステップＳ１００において、撮像装置２２から取得した車両２の前方画像をロール角φ_vだけ回転させ、以降の車線認識処理では、回転補正した車両２の前方画像を用いるようにする。これにより、車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρが実際の車両２の位置情報に近づくように補正される。

また、図４のステップＳ１２０において、ステップＳ１１０で抽出した車線を鳥瞰図に投影する座標変換を行う場合、本来であれば路面に対する車両２の姿勢角が必要になるため、車両２の姿勢角の一例であるロール角φ_vを加味した座標変換を行うことで、車線が精度よく鳥瞰図に投影されることになる。したがって、車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρが実際の車両２の位置情報に近づくように補正される。

また、推定装置１０の実機による実験や推定装置１０の設計仕様に基づくコンピュータシミュレーション等により、前方画像だけから算出した車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρと、ロール角φ_vを考慮して算出した車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρとのそれぞれの誤差（補正値）をロール角φ_v毎に予め定めた変換テーブルを準備して例えばＲＯＭに記憶しておき、当該変換テーブルを参照して、図４のステップＳ１４０で前方画像だけから算出した車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρを入力されたロール角φ_vに対応した補正値で補正してもよい。この場合においても、車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρが実際の車両２の位置情報に近づくように補正される。

したがって、車両位置演算部１４は上述した何れかの補正位置において、車両２の横位置偏差ｅ₁、ヨー角偏差ｅ₂、及び目標経路４の曲率ρの補正を行えばよい。

これまでは、ＩＭＵ２４から車両２のロールレート、ヨーレート、及び横加速度を取得して車両２の状態量ｘを推定する推定装置１０の例について説明したが、カント角φ_cの推定には必ずしも車両２のヨーレート及び横加速度は必要ではなく、車両２のロールレートだけから推定することができる。ロールレート計測値φ^・、カント角φ_c、及びロールレートセンサのオフセットα_φがわかれば、（１８）式からカント角φ_cを推定することが可能である。

（１４）式～（１６）式に示したように、推定装置１０で車両２の状態量ｘを推定するための状態方程式を定義する場合に用いられる車両２の運動方程式には、目標経路４の曲率ρ及び曲率変化率ρ^・が含まれる。したがって、車両２の状態量ｘの推定に曲率ρ及び曲率変化率ρ^・が含まれない運動方程式を用いる場合と比較して、推定装置１０は、カーブの走行時に車両２が傾いた場合であっても、遠心力の影響を排除して路面のカント角φ_cを精度よく推定することができる。

また、従来ヨーレートセンサのオフセットα_rは、撮像装置２２で撮像された画像から取得された曲率ρを用いて推定していたが、本実施の形態に係る推定装置１０は、車両２の運動状態を加味した運動方程式及び観測方程式から推定された曲率ρを用いる。したがって、撮像装置２２で撮像された画像から取得された曲率ρを用いる場合と比較して、ヨーレートセンサのオフセットα_rを精度よく推定することができる。

以上、実施の形態を用いて本発明について説明したが、本発明は実施の形態に記載の範囲には限定されない。本発明の要旨を逸脱しない範囲で実施の形態に多様な変更又は改良を加えることができ、当該変更又は改良を加えた形態も本発明の技術的範囲に含まれる。例えば、本発明の要旨を逸脱しない範囲で処理の順序を変更してもよい。

また、実施の形態では、一例として推定処理及び車線認識処理をソフトウエアで実現する形態について説明したが、図３及び図４に示したフローチャートと同等の処理を、例えばＡＳＩＣ（Application Specific Integrated Circuit）に実装し、ハードウエアで処理させるようにしてもよい。この場合、ソフトウエアを用いた場合と比較して、推定処理及び車線認識処理を高速化することができる。

また、上述した実施の形態では、推定プログラム及び車線変更プログラムがＲＯＭにインストールされている形態を説明したが、これに限定されるものではない。本発明に係る推定プログラム及び車線変更プログラムは、コンピュータ読み取り可能な記憶媒体に記録された形態で提供することも可能である。例えば、本発明に係る推定プログラム及び車線変更プログラムを、ＣＤ(Compact Disc)－ＲＯＭ、又はＤＶＤ(Digital Versatile Disc)－ＲＯＭ等の光ディスクに記録した形態で提供してもよい。また、本発明に係る推定プログラム及び車線変更プログラムを、ＵＳＢメモリ及びフラッシュメモリ等の半導体メモリに記録した形態で提供してもよい。更に、推定装置１０は、例えばインターネット等の通信回線に接続された外部装置から、本発明に係る推定プログラム及び車線変更プログラムを取得するようにしてもよい。

２・・・車両
４・・・目標経路
１０・・・推定装置
１２・・・推定部
１４・・・車両位置演算部
２２・・・撮像装置
２４・・・ＩＭＵ
２６・・・操舵装置
Ｖ・・・車速

Claims (4)

1. 目標経路に対する車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角のそれぞれの計測値と、前記計測値、前記車両が走行する路面のカント角、及び前記車両のロール角を用いて前記車両の横方向の運動状態をモデル化した第１の運動方程式、前記計測値を用いて前記車両のヨー方向の運動状態をモデル化した第２の運動方程式、並びに、前記計測値及び前記車両のロール角を用いて前記車両のロール方向の運動状態をモデル化した第３の運動方程式によって示される複数の運動方程式とを用いて、前記車両が走行する路面のカント角及び前記車両のロール角を含む前記車両の状態量を推定する推定部
   を備えた推定装置。
2. 前記計測値は、更に前記車両のロールレート及び前記車両の横方向の加速度に関するそれぞれの計測値を含み、
   前記複数の運動方程式は、前記車両のロールレート、及び前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に用いてモデル化され、
   前記車両の状態量は、前記車両のロールレート、前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に含む
   請求項１記載の推定装置。
3. 前記推定部は、前記複数の運動方程式に基づいて、前回推定された前記車両の状態量から、次時刻の前記車両の状態量を予測する予測部と、
   観測方程式を用いて、予測された次時刻の前記車両の状態量に対応する前記それぞれの計測値を算出し、前記算出された前記それぞれの計測値と、計測された前記それぞれの計測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を更新する更新部と、
   を含む請求項１又は請求項２記載の推定装置。
4. 前記目標経路に対する車両のずれ量、及び前記目標経路の曲率のそれぞれの計測値は、車両前方又は車両後方を撮像するカメラにより撮像された画像と、前記車両の状態量として前回推定された前記ロール角とに基づいて算出される
   請求項１～請求項３の何れか１項に記載の推定装置。

Images