JP5821288B2 - 道路形状予測装置 - Google Patents

Description

本発明は、車載された車両内部情報によって自車両の進行方向前方の道路形状を予測する道路形状予測技術に関する。

特許文献１には、自車両の車速、操舵角、ヨーレートを検出し、それらの検出値をカルマンフィルタに通して、車両状態として道路曲率および曲率変化を推定する道路曲率推定装置が記載されている。
具体的には、特許文献１では、車速およびヨーレートを観測して、第１のカルマンフィルタを用いて加速度および角加速度を推定し、これらの観測値及び推定値（車速推定値、ヨーレート推定値、加速度推定値、角加速度推定値）を用いて、曲率および曲率変化率を算出している。

また、上記算出した曲率および曲率変化を第２のカルマンフィルタに入力して、曲率および曲率変化の推定値を算出する構成となっている。
そして、上記各カルマンフィルタの計算過程で、車速推定値、加速度推定値および計算更新周期を用いた距離発展の差分方程式に変換して算出を実施している。

特表２００７−５１６９０６号公報

一般に上記ヨーレートセンサの観測値には、観測誤差を補正するオフセット補正が重畳される。しかし、カルマンフィルタは、ノイズが平均値０、分散σ²の正規分布に従うという仮定の下で、最小分散不偏推定量を与えるため、この正規分布の仮定に合わないオフセット補正分は除去できない。そのためカルマンフィルタ用として、特別にオフセット補正する仕組みがなければオフセット分の誤差が重畳して推定誤差が悪化する。
本発明は、上記のような点に着目したもので、車載された車両内部情報に基づき、より精度良く進行方向前方の道路形状を予測することを目的とする。

上記課題を解決するために、本発明は、車速と操舵角とに基づき、車両状態量として、車両が通過した軌跡の接線方向である車両姿勢角と、車両が通過した軌跡の曲率である車両軌跡曲率とを算出し、操舵角と、その距離微分値基づき、予め定めた演算式に従って、上記車両軌跡曲率の変化である車両軌跡曲率変化を算出する。また、本発明は、上記算出した車両状態量と車両軌跡曲率変化に基づき、予め定めた演算式に従って、現在地の道路形状の推定値として、車両が走行している道路を曲線で表したときのその曲線の接線方向である道路曲線接線方向と、車両が走行している道路を曲線で表したときのその曲線の曲率である道路曲率と、その道路曲率の変化である道路曲率変化とを推定する。そして、本発明は、上記推定した道路形状の推定値から車両進行方向前方の道路形状を予測する。

本発明によれば、車輪速と操舵角の情報を用いて車両状態量と車両軌跡曲率変化を算出し、これに基づき道路形状を予測し、前方道路形状を予測する。このため、本発明によれば、精度良く前方道路の形状を予測することが可能となる。

本発明に基づく実施形態に係る車両の構成を示す図である。 道路形状予測ＥＣＵの構成を示す図である。 道路形状予測ＥＣＵでの本実施形態に係る処理を示すフローチャートである。 車両状態量算出部の処理を示すフローチャートである。 誤差処理部の処理を示すフローチャートである。 本発明に基づく実施形態に係るブロック図と対応する数式を示す図である。 本発明に基づく実施形態に係るカントがついた直進路を示す図である。 本発明に基づく実施形態に係る道路形状推定部のカルマンフィルタによる現在値の推定ブロックを示す図である。 本発明に基づく実施形態に係る道路形状予測部のカルマンフィルタによる未来値の予測ブロックを示す図である。 横ずれ量と車線逸脱との関係を示す図である。 一定舵角で車線維持できる車両軌道を説明する図である。 車線維持可能な軌道の曲率分布を示す図である。 カルマンフィルタをゲインスケジュールする場合のブロック構成を示す図である。 道路形状予測を示す概念図である。

次に本発明の実施形態について図面を参照しつつ説明する。
（構成）
本実施形態の車両は、運転者の運転操作に応じた状態に車両挙動が制御される。車両は、制駆動制御などによって自動走行させる構成を備えていても良い。
車両は、駆動制御装置、及び制動制御装置を備える。駆動制御装置は、運転者が操作するアクセルペダルのアクセル開度に応じた目標駆動力となるように、エンジンやモータなどの駆動源を制御する。制動制御装置は、運転者が操作するブレーキペダルのブレーキ開度に応じた目標制動力となるように、各輪の制動装置を制御する。

また、車両は、図１に示すように、運転者が操作するステアリングホイール１の操舵角に応じて、操舵装置２が操向輪３（図１では前輪）を転舵する。
車両は、図１に示すように、操舵角センサ４、車輪速センサ５、道路形状予測ＥＣＵ６を備える。
操舵角センサ４は、ステアリングホイール１やステアリング軸の回転角を検出することで操舵角情報を検出する。検出した操舵角情報は道路形状予測ＥＣＵ６に出力される。

車輪速センサ５は、車輪速情報を検出し、その検出値を道路形状予測ＥＣＵ６に出力する。車輪速センサ５は、車輪の回転に応じて、予め設定した回転角、つまりその回転角に対応する車両の移動距離毎に発生するパルス情報に基づく車両の走行距離から車速を検出する。
道路形状予測ＥＣＵ６は、マイクロコンピュータおよびその周辺回路を備えるコントローラである。この道路形状予測ＥＣＵ６は、図２に示すように、機能的には、少なくとも車両状態量算出部１０、道路形状推定部１１、道路形状予測部１２を備える。

車両状態量算出部１０は、車速と操舵角の情報に基づき車両状態量を算出する。その算出する車両状態量は、車両姿勢角、車両軌跡曲率、及び上記操舵角の距離微分に基づく車両軌跡曲率変化である。この上記車両状態量算出部１０は、車両状態量算出本体部１０Ａ、車両軌跡曲率変化算出部１０Ｂ、及び誤差補正部１０Ｃを備える。
車両状態量算出本体部１０Ａは、車輪速センサ５が検出した車輪速情報と、操舵センサが検出した操舵情報とに基づき、車両姿勢角と車両軌跡曲率を算出する。本実施形態では、車両姿勢角として、車両軌跡接線方向を算出する。この車両軌跡接線方向は、車両姿勢角を表している。

車両軌跡曲率変化算出部１０Ｂは、操舵角センサ４が検出した操舵角と、操舵角を距離微分した値とを用いて車両軌跡曲率変化を算出する。
誤差補正部１０Ｃは、路面カントやうねりなどの平坦路に対する走行路面形状変化や、スリップや車輪空転などの路面に対する車輪の滑り状態からなる、車両状態算出誤差の情報を検出し、その検出した車両状態算出誤差が予め設定した補正閾値以上と判定すると、その検出した車両状態算出誤差の情報に基づき、車両状態量算出本体部１０Ａ及び車両軌跡曲率変化算出部１０Ｂが算出する車両状態量を補正する。

このとき、誤差補正部１０Ｃは、上記検出した車両状態算出誤差が、上記補正閾値よりも大きな値として予め設定した補正限界閾値を越えたと判定した場合には、補正限界を超えた事を示す補正限界状態フラグをＯＮとすると共に、上記補正処理を実施しない。なお、補正限界状態フラグは、初期値がＯＦＦとなっている。
道路形状推定部１１は、車両状態量算出部１０で算出した車両状態量である、車両姿勢角、車両軌跡曲率、及び車両軌跡曲率変化から、現在走行中の道路の道路形状の推定値を推定する。上記道路形状の推定値は、道路曲線接線方向、道路曲率、及び道路曲率変化である。

道路形状予測部１２は、道路形状推定部１１が推定した道路形状の推定値から、予め設定した距離前方までの車両進行方向前方の道路形状を予測する。車両進行方向前方の道路形状の予測値は、現在位置から予め設定した前方距離若しくは当該前方距離までの道路形状に対する、車両進行方向前方の道路の道路曲線接線方向、道路曲率、道路曲率変化である。

次に道路形状予測ＥＣＵ６における道路形状予測の処理を、図３を参照して説明する。
先ずステップＳ１０にて操舵角及び車輪速パルスを検出する。
次に、ステップＳ２０にて、車両状態量算出部１０が、車両状態量を算出する。
次に、ステップＳ３０にて、道路形状推定部１１が、車両状態量算出部１０が算出した車両状態量に基づき道路形状の推定値を求める。
次に、ステップＳ４０にて、道路形状予測部１２が、道路形状推定部１１が推定した道路形状の推定値に基づき、予め設定した前方距離までの道路形状を予測する。その後復帰する。

次に、車両状態量算出部１０の処理について図４を参照して説明する。
まずステップＳ１００にて、車輪速センサ５からのパルス信号に基づき車両の移動距離を算出する。
次に、ステップＳ１１０にて、車両状態量算出本体部１０Ａが、ステップＳ１００にて求めた移動距離に基づき、車両軌跡接線方向及び曲率を算出する。
次に、ステップＳ１２０にて、操舵角の時間微分を算出する。
次に、ステップＳ１３０にて、車両軌跡曲率変化算出部１０Ｂが、ステップＳ１２０にて算出した操舵角の時間微分に基づき車両軌跡曲率変化を算出する。その後復帰する。

次に、誤差補正部１０Ｃの処理について図５を参照して説明する。
まずステップＳ２００にて、車両状態算出誤差となる道路形状を検出する。
次に、ステップＳ２１０にて、車輪のスリップに起因する車両状態算出誤差の要因を検出する。
次に、ステップＳ２２０にて、予め設定した補正閾値以上の車両状態算出誤差があると判定すると補正項を算出する。後述のように、例えば評価関数値Ｊを演算し、その評価関数値Ｊが補正閾値Ｔｈ１以上の場合に補正項を算出する。

次に、ステップＳ２３０にて、予め設定した補正限界閾値Ｔｈ２（＞Ｔｈ１）を超える誤差があるか否かを判定する。条件を満足する場合には、ステップＳ２５０に移行する。条件を満足しない場合にはステップＳ２４０に移行する。
ステップＳ２４０では、補正閾値Ｔｈ１以上の誤差がある場合には、補正項で車両状態量を補正する。その後復帰する。
ステップＳ２５０では、補正限界状態フラグをＯＮにする。その後復帰する。補正限界状態フラグがＯＮの場合には、例えば、過去の情報をクリアして、道路形状予測のための計算をし直す。

以下、上述の各処理について詳説する。
図６は、本実施形態における処理のブロック図と対応する数式を示す図である。
（車両状態量算出部１０での車両状態量の算出について）
次に、車両状態量算出部１０で算出する車両軌跡接線方向（車両姿勢角）、車両軌跡曲率、及び車両軌跡曲率変化の算出について説明する。

車体速Ｖは、下記（１）式に基づき、左右の後輪車輪速Ｖｌ、Ｖｒから求めることが出来る。
Ｖ ＝（Ｖｌ＋Ｖｒ）／２ ・・・（１）
ここで、（１）式における左右の後輪車輪速Ｖｌ、Ｖｒは、タイヤ径に基づき車輪速を車体速に換算した値である。

また、ヨーレートωは、左右の後輪車輪速Ｖｌ、Ｖｒを用いて式（２）で表すことが出来る。Ｔｒｄ［ｍ］は、後輪トレッド長をトレッド値である。
ω ＝（Ｖｌ−Ｖｒ）／Ｔｒｄ ・・・（２）
さらに、曲率ρ＝ω／Ｖと表すことができる。

これらに基づき、車両軌跡の曲率は、（３）式によって求めることが出来る。
ρ ＝２（Ｖｌ−Ｖｒ）／｛Ｔｒｄ・（Ｖｌ＋Ｖｒ）｝ ・・・（３）
また、Ｖ＝（ｄＬ／ｄｔ）と表せることがから、（１）式は（４）式のように表現できる。
ｄＬ ＝（ｄＬｌ＋ｄＬｒ）／２ ・・・（４）
ここでｄＬは車体の移動距離、ｄＬｌ、ｄＬｒは左右の後輪車輪移動距離を表している。ｄＬｌ、ｄＬｒは後輪車輪速センサ５の情報で検知出来る。

また、（３）式は、操舵角に基づき（５）式で表すことが出来る。
ρ ＝２（ｄＬｌ−ｄＬｒ）／｛Ｔｒｄ・（ｄＬｌ＋ｄＬｒ）｝ ・・・（５）
また、（６）式に基づき、操舵角δから車両軌跡の曲率ρを算出することが出来る。
ρ ＝ｔａｎ（δ／Ｇｒ）／Ｌｗ ・・・・（６）
ここで、
Ｌｗ［ｍ］：ホイールベース長
Ｇｒ ：ステアリングホイールとタイヤの転舵角間のギア比
である。
ここで、（６）式を距離微分すると（７）式が得られる。

（４）式を用いて、（５）式及び（７）式を、オイラー法により近似して差分方程式で表現すると、下記（８）式、（９）式、（１０）式が得られる。

ここで、
θｎ：車両軌跡接線方向
ρｎ：車両軌跡曲率
ζｎ：車両軌跡曲率変化
を表している。また、添え字ｎは、車両移動距離ｄＬ毎に増加する。
そして、車両状態量算出部１０は、（１０）式を算出して、上記（８）式及び（９）式を車両移動距離ｄＬ毎に数値積分することにより、車両状態量を算出する。

以上の結果に基づき、車両状態量算出部１０は、（８）式〜（１０）式によって、車両移動距離ｄＬ毎に、車両状態量として、車両軌跡接線方向、車両軌跡曲率、車両軌跡曲率変化を求める。
ここで、上記車両状態量を算出する際のモデルは、車両が平坦な舗装路面を走行し、且つ、横滑りなどの車輪の滑りが発生していないという条件でのモデルである。
このため、上記車両状態量の算出に影響を与えるだけの走行路面形状変化が存在する場合、若しくは上記車両状態量の算出に影響を与えるだけの走行路面に対する車輪の滑りを検出すると、誤差補正部１０Ｃは、車両状態量を補正する。

車両状態算出誤差になりうる走行路面形状として路面カントやうねりなどが存在する。また、車両状態算出誤差になりうる走行路面に対する車輪の滑りとなりうるものとして、横滑りや片流れ、スリップなどの現象がある。そして、誤差補正部１０Ｃは、このようなことで発生する車両状態算出誤差が予め設定した補正閾値以上となったことを検出すると、例えば（８）式〜（１０）式で示される上記算出した車両状態量、若しくは観測値としての車速値や操舵角を補正する。

上記誤差補正部１０Ｃの処理について補足説明する。
路面カントやうねり等の車両状態算出誤差要因の有無を検出する。予め設定した平坦路に対する、予め設定した以上の路面形状変動が存在すると判定すると、車両状態算出誤差要因があると判定する。
車両状態算出誤差は、例えば次のようにして検出する。
すなわち、後輪車輪速センサ５から算出した車両軌跡状態量（（５）式に基づく後輪車輪速センサ５から求めた曲率ρ_w）と前輪舵角から算出した車両軌跡状態量（（６）式に基づく操舵角から求めた曲率ρ_s）とを算出し、その偏差（ρ_w-ρ_s）に基づき、路面カントやうねりなどの車両状態算出誤差を検出する。

誤差補正部１０Ｃは、例えば、予め設定した一定走行距離L[ｍ]毎に、逐次、下記（１１）式に基づき、車両状態算出誤差として評価関数値Jを評価する、そして、その評価関数値Ｊが予め設定した補正閾値Ｔｈ１以上となったと判定したら、上記車両状態量を補正する。

また、誤差補正部１０Ｃは、上記式で示した評価関数値Jが、上記補正閾値Ｔｈ１よりも大きい値として予め設定した補正限界閾値Ｔｈ２を超えた場合に、補正限界と判定する。
ここで、路面カントによる路面形状誤差要因を例にして補正の一例を説明する。
ここで、図７に示すような路面カントがついた直進路を走行した場合について考える。図７の路面では、紙面上側の路面高さが高く且つ紙面下側が低いとする。

この路面を、左から右に走行する際に、ハンドルを中立状態のままにして走行すると、路面の高低差の影響により車両軌跡は実線の軌跡をたどる。従って、この路面を直進するためには、左側に補舵を加える必要がある。ここで、適切な補舵を加えた結果、実線で示す直進軌跡が得られたとする。この場合、舵角を用いて算出した車両軌跡は、一点鎖線の軌跡となり、後輪車輪速センサ５を用いて算出した車両軌跡は実線の軌跡となる。つまり、両者の軌跡の曲率が異なる事から、この差分を補正項として、上記（８）式〜（１０）式で表される算出した車両状態量を補正する。

なお、通常走行状態では、横滑りが発生するほどのカントやうねりは通常の舗装道路上には存在しないので、車体は横滑りしないと仮定する。上記車両状態を求めるためのモデルは、平坦路面を仮定したモデルを用いているため、路面カントやうねりがある場合に、先に示したような推定誤差が生じる。
また通常、横滑りは車速が大きい（概ね５０ｋｍ／ｈ以上）状態で旋回した場合に、タイヤのグリップ力が飽和することにより発生する。

車体の横滑り成分は、他のセンサ情報（例えば、加速度センサやヨーレートセンサ）の情報を用いて横滑り成分を推定すれば良い。そして、推定した横すべり成分を補正項として補正する。
すなわち、誤差補正部１０Ｃは、横滑り成分を検出し、その検出した横滑り分を補正項として、上記（８）式〜（１０）式で表される算出した車両状態量を補正する。

（道路形状推定部１１及び道路形状予測部１２について）
次に、道路形状推定部１１における道路曲線接線方向と道路曲率と道路曲率変化の推定方法について説明する。
道路形状推定部１１では、下記の（１１）式、（１２）式で表されるダイナミクスに基づき、（１３）式で表すカルマンフィルタを用いて、上記車両状態量算出部１０が算出した車両軌跡接線方向推定値θn［ｒａｄ／ｍ］、車両軌跡曲率ρn ［ｒａｄ／ｍ］、車両軌跡曲率変化ζn［ｒａｄ／ｍ2］から、道路曲線接線方向推定値＾θn［ｒａｄ／ｍ］、道路曲線曲率＾ρn［ｒａｄ／ｍ］、道路曲線曲率変化＾ζn［ｒａｄ／ｍ2］を推定する。
ここで、Ｆｎは車両線型モデル、Ｈｎは観測モデル、Ｑｎ、Ｒｎが共分散行列、ｗｎ、ｖｎが雑音、Ｐが誤差の共分散行列、Ｋｎがカルマンゲインである。

次に、道路形状予測部１２における前方道路形状予測方法について説明する。
道路形状予測部１２では、上記道路形状推定部１１で推定した道路曲線接線方向推定値＾θ_n［ｒａｄ／ｍ］、道路曲線曲率＾ρ_n［ｒａｄ／ｍ］、道路曲線曲率変化＾ζ_n［ｒａｄ／ｍ²］と、これらの推定値を数値積分することで、未来の予測誤差を算出し、その予測誤差としての誤差共分散行列Ｐから（１４）式の第１式、第２式を用いて算出することで、進行方向前方の形状予測を行う。

すなわち、道路形状推定部１１では、（１３）式に基づき予測及び更新をして推定することとなるが、道路形状予測部１２では、（１４）式の第１式、第２式を用いて予測する。
更に、上記道路形状推定部１１、道路形状予測部１２の処理について補足説明する。
上述のように、車両状態量算出部１０は、操舵角δおよび後輪車輪速パルスVpr、Vplを観測量として、下記（１５）式で表される車両状態量を算出する。

そして、道路形状推定部１１は、車両状態量算出部１０で算出した上記状態量Ｘを入力とし、カルマンフィルタにて予測及び観測更新の処理を行って、現在値の道路形状の推定を行う。さらに、道路形状予測部１２が、その推定値を入力として、カルマンフィルタにて、進行方向前方に位置する道路形状の予測を行う。

次に、道路形状推定部１１が用いるカルマンフィルタの構成について説明する。
図８は、現在値推定に使用する道路形状推定部１１のカルマンフィルタの構成を示す。この図８から分かるように、カルマンフィルタの演算処理は２つの機能ブロックで構成される。一つ目の機能ブロックは、「予測」処理を行い、２つ目の機能ブロックは「観測更新」処理を行う。

上記予測ブロックでは、ｎステップ目の推定値＾Ｘ_nと推定誤差共分散＾Ｐ_nを基に、ｎ＋１ステップ目の予測値￣Ｘ_n+1と予測誤差共分散￣Ｐ_n+1を算出する。そして、２つ目の機能ブロックでの観測更新で、ｎ＋１ステップ目の予測値￣Ｘ_n+1とｎ＋１ステップ目の観測値Y_n+1をカルマンゲインKで重み付けしてブレンドすることによりｎ＋１ステップ目の推定値＾Ｘ_n+1と推定誤差共分散＾Ｐ_n+1を算出する。
以上の演算処理を演算ステップ毎に逐次繰り返す。ここで、これらの演算ステップは予め設定した走行距離ｄｌ[ｍ]毎に処理を実行する。

次に、道路形状予測部１２が用いる、つまり未来値の予測に用いているカルマンフィルタの構成について説明する。
図９は、未来値の予測に使用する道路形状予測部１２のカルマンフィルタの構成を示す。
道路形状予測部１２が使用するカルマンフィルタの構成は、図９に示すように、上記道路形状推定部１１のカルマンフィルタの構成と基本的な構成は同じある。ただし、未来値の予測では、予測の機能ブロックのみが動作する。すなわち、未来値は観測できないため、観測更新の機能ブロックは使用しない。

そして、予測の演算処理はｎステップ目の推定値に基づき、所望の予測長に至るまで予測の機能ブロックの演算処理を演算ステップ毎に逐次繰り返し実施する。これらの演算ステップは予め設定した走行距離ｄｌ[ｍ]毎に処理を実行する。
例えば、上記走行距離ｄｌ＝０．１[ｍ]とし、現在から進行方向前方の道路形状予測位置を、予測長１０[ｍ]に設定したとする。この場合には、ｎステップ目の推定値を初期値として１００回ステップ分の予測演算処理を実行することにより、ｎステップ目における（ｎ＋１００）ステップ目までの予測値が算出できる事になる。言い換えると、現在位置から１０[ｍ]前方までの予測値を算出できる事になる。

次に、上記道路形状推定部１１において、カルマンゲインをゲインスケジュールしながら車両の走行距離毎に道路形状を推定する場合について説明する。
道路と車両の幾何モデルを用いた車線維持可能な曲率分散値の解析として
まず、現実の道路は、道路曲率が連続して変化するというように仮定できるため、局所的には曲率一定であるとみなして解析する。

いま、横滑りが発生しない低速域において、曲率半径Ｒ[ｍ]、車線幅４[ｍ]の道路での走行を考える。例えば、Ｒ５０以上の道路において車両全長５[ｍ]、全幅１．９[ｍ]とすると車線逸脱することなく走行するためには、後輪車軸中心の車線中心からの横ずれ量ｄが±０．９[ｍ]以内であれば良い。この様子を図１０に示す。
次に、曲率半径Ｒ[ｍ]、車線幅４[ｍ]の道路に対して、一定舵角で車線維持できる車両軌道を考える。図１１に一定舵角で車線維持できる車両軌道を示す。最大曲率Ｒｍaxとなる軌道をＬ４の線、最小曲率半径Ｒｍinとなる軌道をＬ５の線で示す。

ここで、点Ｐ、Ｑを通る円の方程式からＲｍaxが得られる。また、点Ｓ、Ｒvを通る円の方程式からＲｍinが得られる。（１６）式、（１７）式にこれらの導出式を示す。
Ｒｍaxの導出：
（ｘ＋ａ）²＋（ｙ＋ａ）²＝Ｒ² _maxにＰ（Ｒ＋ｄ、０）
及びＱ（（Ｒ-ｄ）／√２、（Ｒ-ｄ）／√２）を代入して、
（ｘ＋ｄ＋ａ）²＋ａ² ＝Ｒ² _max
←→Ｒ²＋２Ｒｄ＋ｄ²＋２ａ（Ｒ＋ｄ）＋２ａ²＝Ｒ² _max
２（（Ｒ-ｄ）／√２）²＝Ｒ² _max
←→２｛１／２（Ｒ²＋２Ｒｄ＋ｄ²）＋（２／√２）ａ（Ｒ＋ｄ）＋２ａ²｝＝Ｒ² _max
・・・（１６）

辺々引いて
ａ ＝２Ｒｄ／（（（√２）−１）Ｒ −（（√２）＋１）ｄ）
よって、Ｒ_max ＝√２ａ ＋Ｒ−ｄとなる。
Ｒｍinの導出：
（ｘ−ａ）²＋（ｙ−ａ）²＝Ｒ² _maxにＰ（Ｒ−ｄ、０）
及びＱ（（Ｒ＋ｄ）／√２、（Ｒ＋ｄ）／√２）を代入して、
（ｘ−ｄ−ａ）²＋ａ² ＝Ｒ² _min
←→Ｒ²−２Ｒｄ＋ｄ²−２ａ（Ｒ−ｄ）＋２ａ²＝Ｒ² _min
２（（Ｒ＋ｄ）／√２）²＝Ｒ² _max
←→２｛１／２（Ｒ²＋２Ｒｄ＋ｄ²）−（２／√２）ａ（Ｒ＋ｄ）＋２ａ²｝＝Ｒ² _min
・・・（１７）

辺々引いて
ａ ＝−２Ｒｄ／（（（√２）−１）Ｒ −（（√２）＋１）ｄ）
よって、Ｒ_max ＝−√２ａ ＋Ｒ＋ｄとなる。
そして、（１６）式、（１７）式から、最大曲率ρ_maxおよび最小曲率ρ_minは（１８）式で表せる．

つまり、曲率半径Ｒ、逸脱限界幅ｄに対する曲率ρのとりうる値は
ρ_min≦ ρ ≦ ρ_maxとなる事が分かる。
ここで、曲率半径Ｒの車線に対して、車線維持可能な曲率分布が図１２に示すような正規分布に従うと仮定すると、その分散σρ²は（１８）式から算出できる。
ここで、道路曲率に応じてカルマンゲインをゲインスケジュールするために、現在走行中の車線の曲率半径Rを操舵角δを用いて推定する。前出の（式６）とR=1/ρの関係からRを算出し（１８）式に代入すれば、ρ_min およびρ_maxが算出できる．例えば、下記式のようにおけば曲率分散値σρ²が算出できる。

図１３は、カルマンゲインをゲインスケジュールしながら車両の走行距離毎に道路形状を推定する際のロジックの構成を示す図である。
すなわち、曲率分散値算出部１３を備える。そして、曲率分散値算出部１３は、検出した操舵角から道路曲率を算出し、先に述べた曲率分散値σρ²を算出する。
そして、道路形状推定部１１は、この曲率分散値算出部１３が算出した曲率分散σρ²の算出値を所定の値に調整する。すなわち、現在の状態推定に用いているカルマンゲインをゲインスケジュールする構成とする。

（動作その他）
本実施形態では、車両状態量算出部１０が、車輪速と操舵角情報の情報から車両状態量と車両軌跡曲率変化を算出し、算出した車両状態量と車両軌跡曲率変化から、道路形状推定部１１が現在地の道路形状を推定し、更に、道路形状予測部１２が、道路形状推定部１１が算出した推定値を基に、進行方向前方の道路形状を予測する。

図１４に、道路形状予測の概念図を示す。図１４に示すように、車両の走行軌跡から現在の道路形状を推定し、その推定値から未来の道路形状の予測値を求める。
このように、カメラなどの外界認識センサによって進行方向前方の道路の観測を行うことなく、つまり高価なセンサを使用することなく、通常車両に搭載されているセンサからの情報だけで、進行方向前方の道路形状を精度良く予測可能となる。

ここで、車輪速センサ５が車速検出部を構成する。操舵角センサ４が操舵角検出部を構成する。誤差補正部１０Ｃが第１補正部及び第２補正部を構成する。ステップＳ２３０が補正限界判定部を構成する。車両状態量算出部１０は、車両状態量算出部のほか車両軌跡曲率変化算出部を兼ねる。

（本実施形態の効果）
（１）車速検出部が車両の車速を検出する。操舵角検出部が車両に設けられた操舵装置２の操舵角を検出する。車両状態量算出部１０は、上記車速検出部が検出した車速と上記操舵角検出部が検出した操舵角とに基づき車両状態量として、車両姿勢角、車両軌跡曲率、及び上記操舵角の距離微分に基づく車両軌跡曲率変化を算出する。道路形状推定部１１は、車両状態量算出部１０が算出した車両状態量に基づき、道路形状の推定値として道路曲線接線方向、道路曲率、及び道路曲率変化を推定する。道路形状予測部１２は、上記道路形状推定部１１が推定した道路形状の推定値から車両進行方向前方の道路形状を予測する。
車輪速と操舵角情報を用いて車両状態量と車両軌跡曲率変化を算出し、これに基づき道路形状を予測し、前方道路形状を予測する。このため、精度良く進行方向前方の道路形状予測が実現できる。

（２）上記車速検出部は、車輪の回転に応じて予め設定した距離間隔毎に発生するパルス情報に基づく車両の走行距離から車速を検出する。上記車両状態量算出部１０は、予め設定した走行距離毎に操舵装置の操舵角を検出し、その検出した操舵角の距離微分に基づき上記車両軌跡曲率変化を算出する。
車両の走行距離毎に車両状態を算出し、道路形状を推定、前方道路形状を予測する。この結果、低車速時にも誤差が重畳されにくく、高精度な道路形状予測が実現できる。

（３）上記道路形状推定部１１は、カルマンゲインを含むカルマンフィルタを有し、カルマンゲインは、操舵角に基づき算出した曲率分散値からなる共分散行列を含み、この曲率分散値を所定の値に調整しながら、予め設定した車両の走行距離毎に道路形状を推定する。
車両の走行距離毎に車両状態を算出し、道路形状を推定、前方道路形状を予測する。この結果、低車速時にも誤差が重畳されにくく、高精度な道路形状予測が実現できる。
また、操舵角を用いて道路形状推定に用いる曲率分散値を調整するため色々な道路形状に対しても精度良く道路形状の予測が実現できる。

（４）上記車両状態量算出部１０は、車両状態量の算出に対して誤差要因となる道路形状を検出し、その検出した誤差要因となる道路形状に基づき車両状態量を補正する第１補正部を備える。
路面カントやうねりなどの道路形状による誤差を補正することで、路面状況などによらず安定して道路形状の予測が実現できる。
（５）上記第１補正部が検出する道路形状が補正可能な否かを判定する補正限界判定部を備える。
これによって、予測した道路形状予測の精度が悪くなったことを検出可能となる。

（６）車両状態量算出部１０は、路面に対する車輪の滑りを検出し、検出した車輪の滑りに基づき車両状態量を補正する第２補正部を備える。特に、横すべりに対する補正をすることが好ましい。
車輪のスリップや車輪空転などによる誤差を補正することで、路面状況などによらず安定して道路形状の予測が実現できる。
（７）上記第２補正部が検出する滑りが補正可能な否かを判定する補正限界判定部を備える。
これによって、予測した道路形状予測の精度が悪くなったことを検出可能となる。

２ 操舵装置
３ 操向輪
４ 操舵角センサ
５ 車輪速センサ
６ 道路形状予測ＥＣＵ
１０ 車両状態量算出部
１０Ａ 車両状態量算出本体部
１０Ｂ 車両軌跡曲率変化算出部
１０Ｃ 誤差補正部
１１ 道路形状推定部
１２ 道路形状予測部
１３ 曲率分散値算出部

Claims (7)

1. 車両の車速を検出する車速検出部と、
   車両に設けられた操舵装置の操舵角を検出する操舵角検出部と、
   上記車速検出部が検出した車速と上記操舵角検出部が検出した操舵角とに基づき、車両状態量として、車両が通過した軌跡の接線方向である車両姿勢角と、車両が通過した軌跡の曲率である車両軌跡曲率とを算出する車両状態量算出部と、
   上記操舵角検出部が検出した操舵角と、その距離微分値とに基づき、前記車両軌跡曲率を求める差分方程式を距離で微分してなる予め定めた演算式に従って、上記車両軌跡曲率の変化である車両軌跡曲率変化を算出する車両軌跡曲率変化算出部と、
   車両状態量算出部が算出した車両状態量と車両軌跡曲率変化算出部が算出した車両軌跡曲率変化とに基づき、予め定めた演算式であるカルマンフィルタに従って、現在地の道路形状の推定値として、車両が走行している道路を曲線で表したときのその曲線の接線方向である道路曲線接線方向と、車両が走行している道路を曲線で表したときのその曲線の曲率である道路曲率と、その道路曲率の変化である道路曲率変化とを推定する道路形状推定部と、
   上記道路形状推定部が推定した道路形状の推定値を積分して車両進行方向前方の道路形状を予測する道路形状予測部と、
   を備えることを特徴とする道路形状予測装置。
2. 上記車速検出部は、車輪の回転に応じて予め設定した距離間隔毎に発生するパルス情報に基づく車両の走行距離から車速を検出し、
   上記車両軌跡曲率変化算出部は、予め設定した走行距離毎に操舵装置の操舵角を検出し、その検出した操舵角の距離微分に基づき、予め定めた演算式に従って、上記車両軌跡曲率変化を算出することを特徴とする請求項１に記載した道路形状予測装置。
3. 上記道路形状推定部は、上記カルマンフィルタのカルマンゲインは、上記操舵角に基づいて算出した曲率分散値からなる共分散行列を含み、この曲率分散値を予め設定した所定の値に調整しながら、予め設定した車両の走行距離毎に道路形状を推定することを特徴とする請求項１又は請求項２に記載した道路形状予測装置。
4. 上記車両状態量算出部は、車両状態量の算出に対して誤差要因となる道路形状を検出し、その検出した誤差要因となる道路形状に基づき車両状態量を補正する第１補正部を備えることを特徴とする請求項１〜請求項３のいずれか１項に記載した道路形状予測装置。
5. 上記第１補正部が検出する道路形状が補正可能な否かを判定する補正限界判定部を備えることを特徴とする請求項４に記載した道路形状予測装置。
6. 車両状態量算出部は、路面に対する車輪の滑りを検出し、検出した車輪の滑りに基づき車両状態量を補正する第２補正部を備えることを特徴とする請求項１〜請求項５のいずれか１項に記載した道路形状予測装置。
7. 上記第２補正部が検出する滑りが補正可能な否かを判定する補正限界判定部を備えることを特徴とする請求項６に記載した道路形状予測装置。

Images