(57)【要約】【課題】路面のカント角と共に車両のロール角を推定する。【解決手段】推定装置１０は、目標経路４に対する車両２のずれ量ｅ1、ｅ2、目標経路４の曲率ρ、車両２のヨーレートｒ、及び車両２の操舵角δγのそれぞれの計測値と、車両２のずれ量ｅ1、ｅ2、目標経路４の曲率ρ、車両２のヨーレートｒ、車両２の操舵角δγ、路面のカント角φc、及び車両２のロール角φvを用いて車両２の運動状態をモデル化した複数の運動方程式とを用いて、車両２が走行する路面のカント角φc及び車両２のロール角φvを含む車両２の状態量を推定する。【選択図】図１
2020142568.tif 000002
【特許請求の範囲】
【請求項１】
目標経路に対する車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角のそれぞれの計測値と、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、前記車両の操舵角、路面のカント角、及び前記車両のロール角を用いて前記車両の運動状態をモデル化した複数の運動方程式とを用いて、前記車両が走行する路面のカント角及び前記車両のロール角を含む前記車両の状態量を推定する推定部
を備えた推定装置。
【請求項２】
前記計測値は、更に前記車両のロールレート及び前記車両の横方向の加速度に関するそれぞれの計測値を含み、
前記運動方程式は、前記車両のロールレート、及び前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に用いてモデル化され、
前記車両の状態量は、前記車両のロールレート、前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に含む
請求項１記載の推定装置。
【請求項３】
前記推定部は、前記運動方程式に基づいて、前回推定された前記車両の状態量から、次時刻の前記車両の状態量を予測する予測部と、
観測方程式を用いて、予測された次時刻の前記車両の状態量に対応する前記それぞれの計測値を算出し、前記算出された前記それぞれの計測値と、計測された前記それぞれの計測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を更新する更新部と、
を含む請求項１又は請求項２記載の推定装置。
【請求項４】
前記運動方程式が、前記路面のカント角、前記車両のロール角、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角を用いて前記車両の横方向の運動状態をモデル化した第１の運動方程式と、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角を用いて前記車両のヨー方向の運動状態をモデル化した第２の運動方程式と、前記車両のロール角、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角を用いて前記車両のロール方向の運動状態をモデル化した第３の運動方程式によって示される
請求項１〜請求項３の何れか１項に記載の推定装置。
【請求項５】
前記目標経路に対する車両のずれ量、及び前記目標経路の曲率のそれぞれの計測値は、車両前方又は車両後方を撮像するカメラにより撮像された画像と、前記車両の状態量として前回推定された前記ロール角とに基づいて算出される
請求項１〜請求項４の何れか１項に記載の推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、推定装置に関する。
【背景技術】
【０００２】
従来から、車両に取り付けた各種センサで計測した計測値と、車両の運動状態をモデル化した運動方程式を用いて車両の走行状態を推定する推定装置が提案されている（例えば、特許文献１〜特許文献３参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１６−１２０８８２号公報
【特許文献２】再表２０１０／１４０２３４号
【特許文献３】特開平１０−３１８７４２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
車両の走行状態を推定する場合、路面の傾き（カント角）は車両の運動に影響を与える要素となることから、従来の推定装置の中には、例えばロールレートセンサで計測されたロールレートを用いて、進行方向と直交する車両の横方向の傾き（姿勢角）を推定するものが存在する。
【０００５】
しかしながら、例えば姿勢角には路面のカント角の他、路面に対する車両のロール角も含まれるため、路面のカント角を精度よく推定するためには車両のロール角も推定する必要があるが、従来の推定装置では路面のカント角と共に車両のロール角を推定する推定装置は提案されていない。
【０００６】
本発明は、上記に示した問題点を鑑みてなされたものであり、路面のカント角と共に車両のロール角を推定することができる推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記の目的を達成するために、請求項１に記載の推定装置は、目標経路に対する車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角のそれぞれの計測値と、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、前記車両の操舵角、路面のカント角、及び前記車両のロール角を用いて前記車両の運動状態をモデル化した複数の運動方程式とを用いて、前記車両が走行する路面のカント角及び前記車両のロール角を含む前記車両の状態量を推定する推定部を備える。
【０００８】
請求項２記載の発明では、前記計測値は、更に前記車両のロールレート及び前記車両の横方向の加速度に関するそれぞれの計測値を含み、前記運動方程式は、前記車両のロールレート、及び前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に用いてモデル化され、前記車両の状態量は、前記車両のロールレート、前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に含む。
【０００９】
請求項３記載の発明では、前記推定部は、前記運動方程式に基づいて、前回推定された前記車両の状態量から、次時刻の前記車両の状態量を予測する予測部と、観測方程式を用いて、予測された次時刻の前記車両の状態量に対応する前記それぞれの計測値を算出し、前記算出された前記それぞれの計測値と、計測された前記それぞれの計測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を更新する更新部と、を含む。
【００１０】
請求項４記載の発明では、前記運動方程式が、前記路面のカント角、前記車両のロール角、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角を用いて前記車両の横方向の運動状態をモデル化した第１の運動方程式と、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角を用いて前記車両のヨー方向の運動状態をモデル化した第２の運動方程式と、前記車両のロール角、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角を用いて前記車両のロール方向の運動状態をモデル化した第３の運動方程式によって示される。
【００１１】
請求項５記載の発明では、前記目標経路に対する車両のずれ量、及び前記目標経路の曲率のそれぞれの計測値は、車両前方又は車両後方を撮像するカメラにより撮像された画像と、前記車両の状態量として前回推定された前記ロール角とに基づいて算出される。
【発明の効果】
【００１２】
本発明によれば、路面のカント角と共に車両のロール角を推定することができる、という効果を有する。
【図面の簡単な説明】
【００１３】
【図１】推定装置の機能ブロックの一例を示す図である。
【図２】車両２の運動状態の一例を示す図である。
【図３】推定処理の流れの一例を示すフローチャートである。
【図４】車線認識処理の流れの一例を示すフローチャートである。
【図５】運動方程式に用いられる状態量の入出力例を示す図である。
【発明を実施するための形態】
【００１４】
以下、本実施の形態について図面を参照しながら説明する。なお、同じ構成要素及び処理には全図面を通して同じ符合を付与し、重複する説明を省略するものとする。
【００１５】
図１は、走行中の車両２の状態量を推定する推定装置１０の機能ブロック図の一例である。図１に示すように、推定装置１０は、推定部１２及び車両位置演算部１４を含み、車両位置演算部１４には撮像装置２２が接続され、推定部１２には車両位置演算部１４、ＩＭＵ(Inertial Measurement Unit)２４、及び操舵装置２６がそれぞれ接続されている。
【００１６】
撮像装置２２は車両２の走行方向の画像（「前方画像」ともいう）を撮像する単眼カメラであり、車両位置演算部１４は撮像装置２２から前方画像を受け付けると、車両位置を前方画像から演算する車線認識処理を行う。具体的には、車両位置演算部１４は、目標経路４に対する車両２のずれ量及び目標経路４の曲率を車両２の位置情報として前方画像から演算する。車両位置演算部１４は、目標経路４に対する車両２のずれ量及び目標経路４の曲率を撮像装置２２による計測値として推定部１２に通知する。
【００１７】
また、車両位置演算部１４は、後述する推定部１２で推定された車両２の状態量のうち車両２のロール角を用いて、演算した車両２の位置情報が実際の車両２の位置情報に近づくように位置情報を補正する。すなわち、車両位置演算部１４は、本実施の形態に係る演算部の一例である。
【００１８】
なお、撮像装置２２は必ずしも車両２に必要なセンサでなく、目標経路４に対する車両２のずれ量及び目標経路の曲率を計測することができれば、撮像装置２２の代わりに例えばレーダ装置を用いてもよい。また、撮像装置２２は前方画像の代わりに、車両２の走行方向とは逆方向の画像（「後方画像」ともいう）を撮影してもよい。車両位置演算部１４は後方画像からも車両２の位置情報を演算することができる。
【００１９】
ＩＭＵ２４は、慣性センサを用いて、例えば車両２のロールレート及びヨーレートを計測すると共に、車両２の横加速度を計測する装置である。
【００２０】
以降では、ロールレートを計測するセンサのオフセットを「ロールレートセンサのオフセット」、ヨーレートを計測するセンサのオフセットを「ヨーレートセンサのオフセット」、及び車両２の横加速度を計測するセンサのオフセットを「横加速度センサのオフセット」ということにする。なお、「オフセット」とは各々のセンサにおける計測誤差を意味する。
【００２１】
操舵装置２６は、車両２の操舵角を制御して車両２の進行方向を可変させる装置であり、制御した操舵角を推定部１２に通知する。
【００２２】
撮像装置２２と、ＩＭＵ２４に含まれるロールレートセンサ、ヨーレートセンサ、及び横加速度センサと、操舵装置２６は、各種センサの一例である。
【００２３】
推定部１２は、前回推定した車両２の状態量、及び車両２に設置された各種センサによって計測された計測値を入力として、車両２の運動状態をモデル化した複数の運動方程式に基づく状態方程式、及び各種センサの計測値を車両２の状態量を用いて表した車両２の観測方程式を用いて、現時刻における車両２の状態量を推定する。
【００２４】
具体的には、推定部１２は、状態方程式に基づいて、前回推定された車両２の状態量から、次時刻の車両２の状態量を予測する図示しない予測部と、観測方程式を用いて、予測部で予測された次時刻の前記車両の状態量に対応する各種センサの計測値を算出し、算出した各種センサの計測値と、各種センサで実際に計測された計測値とのそれぞれの差分、及び推定部１２で前回推定された車両２の状態量に基づいて、車両２の状態量を更新する図示しない更新部を含む。
【００２５】
車両の「状態量」とは、例えば車両の走行位置、走行方向、車両の傾き、走行する道路の形状、及び各種センサのオフセットのように、車両の走行状態を特定する場合に用いられる物理量の集合をいう。推定部１２は推定した車両２の状態量を、例えば状態量に基づいて車両２の制御を行う図示しない制御装置に出力する。
【００２６】
＜車両の運動方程式＞
【００２７】
図２は、走行中の車両２における運動状態の一例を示す図である。図２（Ａ）に示すように、目標経路４に対する車両２の横位置偏差をｅ1、ヨー角偏差をｅ2、目標経路の曲率をρ、車両２の車速をＶとすれば、２輪モデルの運動を規定するヨーレートψ・と車両２の横方向速度ｙ・はそれぞれ（１）式及び（２）式で表される。なお、例えば（１）式ではｅ2の上にドット（・）が付加されているが、これはｅ2・とも表記され、ドットの数は微分の階数を表す。以降、各式のドットについては同様の対応付けが適用される。
【００２８】
【数１】
2020142568.tif 000003
【００２９】
更に、推定部１２で用いる状態方程式は、（３）式〜（５）式で表される運動方程式に基づいて定められる。
【００３０】
【数２】
2020142568.tif 000004
【００３１】
ここでｌfは車両２の重心から前輪までの距離、ｌrは車両２の重心から後輪までの距離を表す（図２（Ａ）参照）。Ｋf及びＫrはそれぞれ車両２における前輪及び後輪のコーナリングスティフネスを表す。また、δは車両２の操舵角、ｍは車両２の質量、ｇは重力加速度、ｈはロールセンターから車両重心までの距離、ｋxはバネ上ロール剛性、Ｃrはバネ上ロール方向のダンピング定数、Ｉzはヨー方向の慣性モーメント、Ｉxはロール方向の慣性モーメントをそれぞれ表す。
【００３２】
また、φcは路面のカント角、及びφvは車両２のロール角をそれぞれ表す。図２（Ｂ）に示すように、カント角φcは水平面に対する路面の傾きを表し、ロール角φvは路面と直交する直交線に対する車両２の傾きを表す。
【００３３】
（３）式で表される運動方程式は車両２の横方向の運動を表す運動方程式であり、本実施の形態に係る第１の運動方程式の一例である。（４）式で表される運動方程式は車両２のヨー方向の運動を表す運動方程式であり、本実施の形態に係る第２の運動方程式の一例である。（５）式で表される運動方程式は車両２のロール方向の運動を表す運動方程式であり、本実施の形態に係る第３の運動方程式の一例である。
【００３４】
なお、後述するように、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρは撮像装置２２で撮像した前方画像又は後方画像から取得される。また、車速Ｖは図示しない車速センサから取得される。
【００３５】
＜操舵系の外乱＞
【００３６】
上記に示した車両２の運動方程式では車両２の操舵角をδで表したが、操舵装置２６から取得される操舵角、すなわち、計測された操舵角は、例えば操舵系が有するヒステリシス特性等による制御遅れや、ハンドルの遊び及び操舵系のがたつき等の外乱により、必ずしもタイヤの切れ角と一致しない場合がある。すなわち、計測された操舵角にはオフセットが含まれる。
【００３７】
操舵系の制御遅れは１次遅れとして近似可能であり、１次遅れの時定数をτ、操舵角のオフセットをγ’、時間をｔとすれば、計測された操舵角δγは（６）式で表される。
【００３８】
【数３】
2020142568.tif 000005
【００３９】
（６）式に対して、時間ｔによって変化する操舵角の外乱γを（７）式のように定義した場合、計測された操舵角δγは（８）式によって表されることになる。すなわち、（３）式〜（５）式に示した車両２の運動方程式では、操舵装置２６の過渡応答性による外乱量と操舵装置２６のオフセットによる外乱量が、操舵角の外乱γという１つのパラメータによって表されている。以降では、操舵角δは外乱が含まれない操舵角の真値を表す。
【００４０】
【数４】
2020142568.tif 000006
【００４１】
＜状態方程式＞
【００４２】
車両２の状態量を（９）式のように定義する。
【００４３】
【数５】
2020142568.tif 000007
【００４４】
（９）式の状態量において、αrはヨーレートセンサのオフセット、αφはロールレートセンサのオフセット、αGは横加速度センサのオフセットを表す。
【００４５】
そして、（３）式〜（５）式に示した車両２の運動方程式に基づいて、車両２の状態量ｘの状態方程式を行列式で表した場合、（１０）式のように表記される。
【００４６】
【数６】
2020142568.tif 000008
【００４７】
ここでＥは単位行列であり、Ａは各状態量の係数を表す係数行列である。推定部１２は、カルマンフィルタを用いて（１１）式に示した状態方程式を予め定めた間隔で逐次演算することにより、車両２の状態量ｘを推定する。
【００４８】
実際にカルマンフィルタを用いて車両２の状態量ｘを推定するためには、（１１）式で表される状態方程式を離散化した状態方程式が用いられる。（１２）式は、（１１）式の状態方程式を離散化した状態方程式である。
【００４９】
【数７】
2020142568.tif 000009
【００５０】
変数ｋは時系列の順序を表す整数である。すなわち、ｘkは時刻ｋにおける車両２の状態量を表し、ｘk+1は時刻ｋの次の時刻（ｋ＋１）における車両２の状態量を表す。Ｄは状態行列と呼ばれ、Ｄ＝Ｅ-1Ａである。
【００５１】
したがって、カルマンフィルタを用いて状態量ｘの各要素を推定するためには、要素毎に離散化した状態方程式（１３）式が用いられる。
【００５２】
【数８】
2020142568.tif 000010
【００５３】
ここで（１３）式で表される各要素の状態方程式におけるｗは「システムノイズ」と呼ばれ、状態方程式の白色雑音として定義される分散値である。また、状態量ｘの各要素の分散値を行列値とする対角行列Ｑを、システムノイズを表す共分散行列という。
【００５４】
（１３）式において、ｅ1,k・・、ｅ2,k・・、及びφv,k・・は（３）式〜（５）式で示した車両２の運動方程式をそれぞれｅ1・・、ｅ2・・、φv・・について変形した（１４）式〜（１６）式に示す運動方程式によって定義される。
【００５５】
【数９】
2020142568.tif 000011
【００５６】
このように、本実施の形態に係る車両２の運動方程式には、目標経路４の曲率ρ及び曲率変化率ρ・が含まれる。
【００５７】
以降では（１４）式〜（１６）式に示す運動方程式を車両２の運動方程式として用いることにする。
【００５８】
＜観測方程式＞
【００５９】
推定部１２では各種センサで計測された計測値と、（１１）式に示した状態方程式を用いて予測した状態量とを比較する。このとき、予測した状態量を、各種センサの計測値に変換して比較する。例えばヨーレートセンサの計測値をｒとすれば、ヨーレート計測値ｒは、車両２の状態量に基づいて（１７）式の観測方程式から得られる。
【００６０】
【数１０】
2020142568.tif 000012
【００６１】
（１７）式によれば、目標経路４の曲率ρを加味した車両２のヨーレートを推定することができる。
【００６２】
一方、ロールレートセンサの計測値をφ・とすれば、ロールレート計測値φ・は、車両２の状態量に基づいて（１８）式の観測方程式から得られる。
【００６３】
【数１１】
2020142568.tif 000013
【００６４】
（１８）式によれば、車両２のロールレートは、推定されたカント角の微分値φ・c、ロール角の微分値φv・、及びロールレートセンサのオフセットαφから推定することができる。
【００６５】
更に、横加速度センサの計測値をａyとすれば、横加速度センサ計測値ａyは、車両２の状態量に基づいて（１９）式の観測方程式から得られる。
【００６６】
【数１２】
2020142568.tif 000014
【００６７】
（１９）式によれば、車両２の横方向にかかる加速度は、車両２の横位置偏差の２階微分ｅ1・・、車速Ｖ、曲率ρ、カント角φc、ロール角φv、及び横加速度センサのオフセットαGから推定することができる。また、（１９）式の右辺１項目で表される車両２の運動に伴って車両２の横方向にかかる加速度、及び右辺２項目で表される重力による加速度を分離して推定することも可能である。
【００６８】
車両２の状態量から算出される、各種センサの計測値ｙをｙ＝(ｅ1 ｅ2 ρ r φ・ ａy δγ)とすれば、時刻ｋにおける計測値ｙは（２０）式で表される。
【００６９】
【数１３】
2020142568.tif 000015
【００７０】
行列Ｃは観測行列と呼ばれ、行列Ｒは観測方程式の観測ノイズを示す共分散行列である。
【００７１】
＜推定装置の処理＞
【００７２】
次に、推定装置１０における車両２の状態量ｘの推定方法について説明する。
【００７３】
図３は、例えば推定装置１０が推定の開始指示を受け付けた場合に、推定装置１０に備えられたＣＰＵによって実行される推定処理の流れの一例を示すフローチャートである。
【００７４】
推定処理を規定する推定プログラムは、例えば推定装置１０に備えられたＲＯＭに予め記憶されている。推定装置１０のＣＰＵは、ＲＯＭに記憶される推定プログラムを読み込んで推定処理を実行する。
【００７５】
なお、推定装置１０のＲＡＭには前回推定した時刻ｋにおける車両２の状態量ｘkが記憶されているものとする。前回推定した車両２の状態量ｘkが存在しない場合には、予め設定された状態量ｘの初期値が前回推定した時刻ｋにおける車両２の状態量ｘkとして用いられる。
【００７６】
推定処理ではカルマンフィルタを用いた状態量ｘk+1の推定が行われる。そのため、ステップＳ１０において、推定部１２は撮像装置２２、ＩＭＵ２４、及び操舵装置２６の各種センサで計測された計測値を取得する。具体的には、推定部１２は、車両位置演算部１４を介して、撮像装置２２で撮像された画像から得られた車両２のずれ量の一例である横位置偏差ｅ1及びヨー角偏差ｅ2と、目標経路４の曲率ρを取得する。また、推定部１２は、車両２のロールレート計測値φ・、ヨーレート計測値ｒ、及び横加速度センサ計測値ａyをＩＭＵ２４から取得する。更に、推定部１２は計測された操舵角δγを操舵装置２６から取得する。すなわち、推定部１２は計測値ｙを取得する。
【００７７】
上述したように、車両位置演算部１４は撮像装置２２で撮像された画像に対して車線認識処理を行い、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρを計測することから、車両位置演算部１４の処理について説明する。
【００７８】
図４は、車両位置演算部１４で実行される、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρを計測する車線認識処理の流れの一例を示すフローチャートである。車線認識処理を規定する車線認識プログラムも、例えば推定装置１０に備えられたＲＯＭに予め記憶されている。推定装置１０のＣＰＵは、ＲＯＭに記憶される車線認識プログラムを読み込んで車線認識処理を実行する。
【００７９】
ここでは一例として、車両２の目標経路４が路面上の車線に沿って設定されているものとする。
【００８０】
ステップＳ１００において、車両位置演算部１４は、車両２の前方画像を撮像装置２２から取得する。車両位置演算部１４は後方画像を撮像装置２２から取得してもよいが、ここでは前方画像を取得する例について説明する。
【００８１】
ステップＳ１１０において、車両位置演算部１４は、公知のパターン認識又は画像処理を用いて、ステップＳ１００で取得した前方画像から車線を抽出する。
【００８２】
ステップＳ１２０において、車両位置演算部１４は、ステップＳ１１０で抽出した車線を、路面を車両２の真上から見た図（鳥瞰図）に投影する座標変換を実行する。
【００８３】
ステップＳ１３０において、車両位置演算部１４は、例えば道路をモデル化したクロソイド曲線に車線の３次元位置を当てはめることで、路面上における車線の位置を推定する車線パラメータ推定を実行する。
【００８４】
ステップＳ１４０において、車両位置演算部１４は、ステップＳ１３０で推定した路面上における車線の位置に基づいて、目標経路４に対する車両２のずれ量の一例である横位置偏差ｅ1及びヨー角偏差ｅ2を算出する。また、車両位置演算部１４は、ステップＳ１３０で推定した路面上における車線の位置に基づいて、目標経路４の曲率ρを算出する。
【００８５】
そして、車両位置演算部１４は、算出した車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρを推定部１２に通知する。
【００８６】
なお、車両位置演算部１４は、目標経路４の曲率ρの単位時間における変化量から曲率変化率ρ・を算出して車両位置演算部１４に通知してもよい。以上により、図４に示した車線認識処理を終了する。
【００８７】
図３のステップＳ２０において、推定部１２は、前回推定した車両２の状態量ｘkの各々を（１３）式に代入し、時刻（ｋ＋１）における車両２の状態量ｘk+1を予測する。なお、説明の便宜上、ステップＳ２０で予測した状態量ｘk+1を特にｘk+1-と表すことにする。
【００８８】
上述したように（１３）式におけるｅ1,k・・、ｅ2,k・・、及びφv,k・・は、（１４）式〜（１６）式に示す運動方程式から得られる。
【００８９】
図５は、ｅ1,k・・、ｅ2,k・・、及びφv,k・・が、（１４）式〜（１６）式に示す運動方程式から得られることを示す図である。
【００９０】
車両２の横方向の運動方程式は（１４）式で表されることから、車両２の横位置偏差の２階微分値ｅ1・・は、状態量ｘkに含まれる操舵角δ、操舵角の外乱γ、車両２の横位置偏差の１階微分値ｅ1・、車両２のヨー角偏差ｅ2、車両２のヨー角偏差の１階微分値ｅ2・、目標経路４の曲率ρ、カント角φc、ロール角φv、及びロール角の１階微分値φv・から推定される。
【００９１】
また、車両２のヨー方向の運動方程式は（１５）式で表されることから、車両２のヨー角偏差の２階微分値ｅ2・・は、状態量ｘkに含まれる操舵角δ、操舵角の外乱γ、車両２の横位置偏差の１階微分値ｅ1・、車両２のヨー角偏差ｅ2、車両２のヨー角偏差の１階微分値ｅ2・、目標経路４の曲率ρ、及び目標経路４の曲率変化率ρ・から推定される。
【００９２】
更に、車両２のロール方向の運動方程式は（１６）式で表されることから、車両２のロール角の２階微分値φv・・は、状態量ｘkに含まれる操舵角δ、操舵角の外乱γ、車両２の横位置偏差の１階微分値ｅ1・、車両２のヨー角偏差ｅ2、車両２のヨー角偏差の１階微分値ｅ2・、目標経路４の曲率ρ、ロール角φv、及びロール角の１階微分値φv・から推定される。
【００９３】
（１３）式に示すように、車両２の横位置偏差の２階微分値ｅ1・・、車両２のヨー角偏差の２階微分値ｅ2・・、及び車両２のロール角の２階微分値φv・・を積分した値と、各種センサで計測された計測値を用いてカルマンフィルタによるフィルタリングを行うことで、ｅ1、ｅ1・、ｅ2、ｅ2・、φv、φv・がそれぞれ推定される。
【００９４】
図５に示すように、カント角φcは車両２の横方向の運動のみに影響を与え、ロール角φvは車両２の横方向の運動方程式及びロール方向の運動に影響を与える。したがって、推定部１２は、カント角φcとロール角φvが影響を与える運動に違いがあることを利用して、カント角φcとロール角φvをそれぞれ分離して推定する。
【００９５】
また、図５に示すように、カント角φcは車両２の横方向の運動のみに影響を与え、操舵角の外乱γは車両２の横方向の運動、及びヨー方向の運動に影響を与える。したがって、推定部１２は、カント角φcと操舵角の外乱γが影響を与える運動に違いがあることを利用して、カント角φcと操舵角の外乱γをそれぞれ分離して推定する。
【００９６】
なお、（１６）式に示す車両２のロール方向の運動方程式には操舵角の外乱γが含まれるため、図５では操舵角の外乱γが車両２のロール方向の運動にも影響を与えているように見える。しかし、（５）式で示される変形前の車両２のロール方向の運動方程式には操舵角の外乱γは含まれないことから、操舵角の外乱γは車両２の横方向の運動、及びヨー方向の運動に影響を与えることになる。
【００９７】
また、本実施の形態では、操舵角の外乱γが１つのパラメータでモデル化されているため、例えば操舵装置２６の過渡応答性による外乱量と操舵装置２６のオフセットによる外乱量をそれぞれ分離した運動方程式を用いる場合と比較して、カルマンフィルタを用いた車両２の状態量ｘkの推定が単純になり、車両２の状態量ｘkの推定に要する時間を短縮することができる。
【００９８】
ステップＳ３０において、推定部１２は、前回推定した状態方程式の状態ノイズを示す共分散行列Ｐkを（２１）式に代入し、時刻（ｋ＋１）における共分散行列Ｐk+1を予測する。なお、説明の便宜上、ステップＳ３０で予測した共分散行列Ｐk+1を特にＰk+1-と表すことにする。
【００９９】
【数１４】
2020142568.tif 000016
【０１００】
ステップＳ４０において、推定部１２は（２２）式によって、ステップＳ３０で予測した共分散行列Ｐk+1-と、（２０）式における観測行列Ｃ及び共分散行列Ｒを用いてカルマンゲイン行列Ｇkを算出する。
【０１０１】
【数１５】
2020142568.tif 000017
【０１０２】
ステップＳ５０において、推定部１２は（２３）式に従って、ステップＳ２０で予測した状態量ｘk+1-、ステップＳ４０で算出したカルマンゲイン行列Ｇk、ステップＳ１０で取得した計測値ｙ、及び計測値ｙの観測行列Ｃを用いて、ステップＳ２０で予測した状態量ｘk+1-を更新する。更新後の状態量ｘk+1-は状態量ｘk+1で表される。
【０１０３】
【数１６】
2020142568.tif 000018
【０１０４】
（２３）式に示すように、状態量ｘk+1はセンサの計測値ｙと、観測行列Ｃで重み付けを行った状態量ｘk+1-の差分に対して、カルマンゲイン行列Ｇkを作用させることで更新される。
【０１０５】
ステップＳ６０において、推定部１２は（２４）式に従って、ステップＳ４０で算出したカルマンゲイン行列Ｇk、ステップＳ１０で取得した計測値ｙの観測行列Ｃ、及びステップＳ３０で予測した前回の共分散行列Ｐk-を用いて、ステップＳ３０で予測した共分散行列Ｐk+1-を更新する。更新後の共分散行列Ｐk+1-は共分散行列Ｐk+1で表される。なお、Ｉは単位行列を表す。
【０１０６】
【数１７】
2020142568.tif 000019
【０１０７】
ステップＳ７０において、推定部１２はステップＳ５０で更新した状態量ｘk+1を出力する。
【０１０８】
ステップＳ８０において、推定部１２は、推定処理の終了指示を受け付けたか否かを判定し、終了指示を受け付けていない場合にはステップＳ１０に移行して、現在のセンサの計測値ｙを取得する。ステップＳ１０〜Ｓ８０を繰り返し実行することで、時系列に沿った車両２の状態量ｘk+n(nは１以上の整数）を推定することができる。
【０１０９】
なお、ステップＳ８０の判定処理で終了指示を受け付けていないと判定された場合、予め定めた間隔だけ待機してからステップＳ１０に移行するようにしてもよい。この場合、待機時間を調整することで、例えば１００Ｈｚや１ｋＨｚというように予め定めた周期で車両２の状態量ｘk+nを推定することができる。推定装置１０で推定された状態量ｘk+nは未来の状態量を表すことから、状態量ｘk+nは例えば目標経路４に沿った車両２の自動運転に利用される。
【０１１０】
一方、ステップＳ８０の判定処理で終了指示を受け付けたと判定される場合には、図３に示す推定処理を終了する。
【０１１１】
なお、単眼カメラを用いた車両位置演算部１４における車線認識処理では、単に前方画像だけから車両２の位置情報の演算する場合、路面に対する車両２のロール角φvが加味されない。したがって、車両位置演算部１４には、ステップＳ７０で出力された車両２の状態量のうちロール角φvが入力される。車両位置演算部１４は、入力されたロール角φvを用いて、前方画像だけから得られた車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρを補正する。
【０１１２】
図４に示した車線認識処理において、ロール角φvを用いた車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρの補正位置は複数存在する。
【０１１３】
例えば、車両位置演算部１４はステップＳ１００において、撮像装置２２から取得した車両２の前方画像をロール角φvだけ回転させ、以降の車線認識処理では、回転補正した車両２の前方画像を用いるようにする。これにより、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρが実際の車両２の位置情報に近づくように補正される。
【０１１４】
また、図４のステップＳ１２０において、ステップＳ１１０で抽出した車線を鳥瞰図に投影する座標変換を行う場合、本来であれば路面に対する車両２の姿勢角が必要になるため、車両２の姿勢角の一例であるロール角φvを加味した座標変換を行うことで、車線が精度よく鳥瞰図に投影されることになる。したがって、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρが実際の車両２の位置情報に近づくように補正される。
【０１１５】
また、推定装置１０の実機による実験や推定装置１０の設計仕様に基づくコンピュータシミュレーション等により、前方画像だけから算出した車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρと、ロール角φvを考慮して算出した車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρとのそれぞれの誤差（補正値）をロール角φv毎に予め定めた変換テーブルを準備して例えばＲＯＭに記憶しておき、当該変換テーブルを参照して、図４のステップＳ１４０で前方画像だけから算出した車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρを入力されたロール角φvに対応した補正値で補正してもよい。この場合においても、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρが実際の車両２の位置情報に近づくように補正される。
【０１１６】
したがって、車両位置演算部１４は上述した何れかの補正位置において、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρの補正を行えばよい。
【０１１７】
これまでは、ＩＭＵ２４から車両２のロールレート、ヨーレート、及び横加速度を取得して車両２の状態量ｘを推定する推定装置１０の例について説明したが、カント角φcの推定には必ずしも車両２のヨーレート及び横加速度は必要ではなく、車両２のロールレートだけから推定することができる。ロールレート計測値φ・、カント角φc、及びロールレートセンサのオフセットαφがわかれば、（１８）式からカント角φcを推定することが可能である。
【０１１８】
（１４）式〜（１６）式に示したように、推定装置１０で車両２の状態量ｘを推定するための状態方程式を定義する場合に用いられる車両２の運動方程式には、目標経路４の曲率ρ及び曲率変化率ρ・が含まれる。したがって、車両２の状態量ｘの推定に曲率ρ及び曲率変化率ρ・が含まれない運動方程式を用いる場合と比較して、推定装置１０は、カーブの走行時に車両２が傾いた場合であっても、遠心力の影響を排除して路面のカント角φcを精度よく推定することができる。
【０１１９】
また、従来ヨーレートセンサのオフセットαrは、撮像装置２２で撮像された画像から取得された曲率ρを用いて推定していたが、本実施の形態に係る推定装置１０は、車両２の運動状態を加味した運動方程式及び観測方程式から推定された曲率ρを用いる。したがって、撮像装置２２で撮像された画像から取得された曲率ρを用いる場合と比較して、ヨーレートセンサのオフセットαrを精度よく推定することができる。
【０１２０】
以上、実施の形態を用いて本発明について説明したが、本発明は実施の形態に記載の範囲には限定されない。本発明の要旨を逸脱しない範囲で実施の形態に多様な変更又は改良を加えることができ、当該変更又は改良を加えた形態も本発明の技術的範囲に含まれる。例えば、本発明の要旨を逸脱しない範囲で処理の順序を変更してもよい。
【０１２１】
また、実施の形態では、一例として推定処理及び車線認識処理をソフトウエアで実現する形態について説明したが、図３及び図４に示したフローチャートと同等の処理を、例えばＡＳＩＣ（Application Specific Integrated Circuit）に実装し、ハードウエアで処理させるようにしてもよい。この場合、ソフトウエアを用いた場合と比較して、推定処理及び車線認識処理を高速化することができる。
【０１２２】
また、上述した実施の形態では、推定プログラム及び車線変更プログラムがＲＯＭにインストールされている形態を説明したが、これに限定されるものではない。本発明に係る推定プログラム及び車線変更プログラムは、コンピュータ読み取り可能な記憶媒体に記録された形態で提供することも可能である。例えば、本発明に係る推定プログラム及び車線変更プログラムを、ＣＤ(Compact Disc)−ＲＯＭ、又はＤＶＤ(Digital Versatile Disc)−ＲＯＭ等の光ディスクに記録した形態で提供してもよい。また、本発明に係る推定プログラム及び車線変更プログラムを、ＵＳＢメモリ及びフラッシュメモリ等の半導体メモリに記録した形態で提供してもよい。更に、推定装置１０は、例えばインターネット等の通信回線に接続された外部装置から、本発明に係る推定プログラム及び車線変更プログラムを取得するようにしてもよい。
【符号の説明】
【０１２３】
２・・・車両
４・・・目標経路
１０・・・推定装置
１２・・・推定部
１４・・・車両位置演算部
２２・・・撮像装置
２４・・・ＩＭＵ
２６・・・操舵装置
Ｖ・・・車速
【図１】
2020142568.tif 000020
【図２】
2020142568.tif 000021
【図３】
2020142568.tif 000022
【図４】
2020142568.tif 000023
【図５】
2020142568.tif 000024