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

Description

本発明は、道路形状予測装置に関する。

特許文献１には、自車両の車速、操舵角、ヨーレートを検出し、それらの検出値をカルマンフィルタに通し、車両状態として道路曲率および曲率変化率を推定し、進行方向前方の道路形状を予測する技術が開示されている。

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

しかしながら、上記従来技術にあっては、道路曲率を算出する際に車体横滑り角の影響を考慮していないため、荷重変動や路面状態の変化などによって車体横滑り角が発生した場合、道路形状の予測精度が著しく低下するという問題があった。
本発明の目的は、車体横滑り角が生じた場合であっても精度良く進行方向前方の道路形状を予測できる道路形状予測装置を提供することにある。

本発明では、検出された車速と操舵角とに基づき、第１車両状態量として車両姿勢角、車両軌跡曲率および車両軌跡曲率変化率を算出する第１車両状態量算出部と、
検出された車速、加速度およびヨーレートと、推定された車体横滑り角とに基づき、第２車両状態量として車両姿勢角、車両軌跡曲率および車両軌跡曲率変化率を算出する第２車両状態量算出部と、推定された車体横滑り角に応じて前記第１車両状態量と前記第２車両状態量を重み付けした車両状態量を算出する車両状態量統合部と、からなる車両状態量算出部と、算出された前記車両状態量に基づき、道路形状を表すパラメータとして道路曲線接線方向、道路曲率および道路曲率変化率を推定する道路形状推定部と、推定された道路形状のパラメータを積分して車両進行方向前方の道路形状を予測する道路形状予測部と、を備え、前記車両状態量統合部は、推定された車体横滑り角の絶対値が大きいほど前記第１車両状態量よりも前記第２車両状態量の重み付けを大きくするようにした。

よって、車体横滑り角の影響を加味した車両状態量に基づき道路形状を予測するため、車体横滑り角が生じた場合であっても精度良く進行方向前方の道路形状を予測できる。

実施例１の車両の構成図である。 道路形状予測ECUの構成図である。 第１車両状態量算出部の構成を示す図である。 第２車両状態量算出部の構成を示す図である。 車体横滑り角推定部の構成図である。 車両状態量統合部の構成図である。 道路形状予測処理の流れを示すフローチャートである。 第１車両状態量算出処理の流れを示すフローチャートである。 第２車両状態量算出処理を示すフローチャートである。 車両状態量統合処理を示すフローチャートである。 車両状態量統合部の動作説明図である。 車体横滑り角に応じた車両状態量算出量の切り替えを示す概念図である。 誤差補正処理の流れを示すフローチャートである。 異常検出処理の流れを示すフローチャートである。 カントがついた直進路を示す図である。 道路形状推定部のカルマンフィルタによる現在値の推定ブロックを示す図である。 カルマンフィルタの構成図である。 横ずれ量と車線逸脱との関係を示す図である。 一定舵角で車線維持できる車両軌道を説明する図である。 車線維持可能な軌道の曲率分布を示す図である。 道路形状予測の概念図である。

〔実施例１〕
図１は、実施例１の車両の構成図である。実施例１の車両は、運転者の運転操作に応じた状態に車両挙動が制御される。車両は、制駆動制御などによって自動走行させる構成を備えていても良い。車両は、駆動制御装置および制動制御装置を備える。駆動制御装置は、運転者が操作するアクセルペダルのアクセル開度に応じた目標駆動力となるように、エンジンやモータなどの駆動源を制御する。制動制御装置は、運転者が操作するブレーキペダルのブレーキ開度に応じた目標制動力となるように、各輪の制動装置を制御する。
図１に示すように、実施例１の車両は、運転者が操作するステアリングホイール1の操舵角に応じて、操舵装置2が操向輪3（図１では前輪）を転舵する。車両は、操舵角センサ（操舵角検出部）4、車輪速センサ（車速検出部）5、道路形状予測ECU6、ヨーレートセンサ（ヨーレート検出部）7および加速度センサ（加速度検出部）8を備える。

操舵角センサ4は、ステアリングホイール1やステアリング軸の回転角を検出することで操舵角情報を検出する。検出した操舵角情報は道路形状予測ECU6に出力される。
車輪速センサ5は、車輪速情報を検出し、その検出値を道路形状予測ECU6に出力する。車輪速センサ5は、車輪の回転に応じて、あらかじめ設定した回転角、つまりその回転角に対応する車両の移動距離毎に発生するパルス情報に基づく車両の走行距離から車速（車体速）を検出する。
ヨーレートセンサ7は車両のヨー回転角速度を検出し、その検出値を道路形状予測ECU6に出力する。
加速度センサ8は、車両の前後方向および横方向の加速度を検出し、その検出値を道路形状予測ECU6に出力する。

道路形状予測ECU6は、マイクロコンピュータおよびその周辺回路を備えるコントローラである。この道路形状予測ECU6は、図２に示すように、機能的には、少なくとも車両状態量算出部10、車体横滑り角推定部11、道路形状推定部12、道路形状予測部13および曲率分散値算出部14を備える。
なお、パラメータの添え字「^*」は推定値、「~」は予測値、「'」は微分値を表す。
車両状態量算出部10は、車速と操舵角の情報に基づき車両状態量（第１車両状態量）を算出する第１車両状態量算出部10aと、ヨーレートと加速度と車速の情報に基づき車両状態量（第２車両状態量）を算出する第２車両状態量算出部10bと、２つの異なる車両状態量の算出値を車体横滑り角推定値に基づいて切り替える車両状態量統合部10cとから構成される。ここで、算出される車両状態量は、車両姿勢角、車両軌跡曲率および車両軌跡曲率変化率である。

第１車両状態量算出部10aは、図３に示すように、車輪速センサ5により検出された車速情報と、操舵角センサ4により検出された操舵情報とに基づき、車両姿勢角、車両軌跡曲率および車両軌跡曲率変化率を算出する。実施例１では、車両姿勢角として、車両軌跡接線方向を算出する。この車両軌跡接線方向は、車両姿勢角を表している。なお、第１車両状態量算出ロジックについては後述する。
第２車両状態量算出部10bは、図４に示すように、車輪速センサ5により検出された車速情報と、ヨーレートセンサ7により検出されたヨーレート情報と、加速度センサ8により検出された前後加速度および横加速度情報とに基づき、車両姿勢角、車両軌跡曲率および車両軌跡曲率変化率を算出する。実施例１では、車両姿勢角として、車両軌跡接線方向を算出する。この車両軌跡接線方向は、車両姿勢角を表している。第２車両状態量算出部10bは、曲率算出部10b1、車両前後加速度推定部10b2、車両公転成分推定部10b3および曲率変化率算出部10b4を有する。曲率算出部10b1は、横加速度と車速から車両軌跡曲率を算出する。車両前後加速度推定部10b2は、前後加速度と車速から車速推定値と前後加速度推定値を推定する。車両公転成分推定部10b3は、横加速度と車速から車両の公転速度推定値と公転加速度推定値を算出する。曲率変化率算出部10b4は、車速推定値、前後加速度推定値、公転速度推定値および公転加速度推定値から車体姿勢角および車両軌跡曲率変化率を算出する。なお、第２車両状態量算出ロジックについては後述する。

車体横滑り角推定部11は、図５に示すように、ヨーレート、横加速度、操舵角および車速に基づき、車体横滑り角を推定する。車体横滑り角推定部11は、車体横滑り角算出部11a、車体横滑り角速度算出部11bおよび車体横滑り角推定カルマンフィルタ11cを有する。車体横滑り角算出部11aは、操舵角センサ4により検出された操舵角情報と、車輪速センサ5により検出された車速情報と、ヨーレートセンサ7により検出されたヨーレート情報に基づき車体横滑り角を算出する。車体横滑り角速度算出部11bは、車輪速センサ5により検出された車速情報と、ヨーレートセンサ7により検出されたヨーレート情報と、加速度センサ8により検出された加速度情報に基づき車体横滑り角速度を算出する。車体横滑り角推定カルマンフィルタ11cは、算出された車体横滑り角と車体横滑り角速度情報を用いて車体横滑り角を推定し、車体横滑り角推定値として出力する。なお、車体横滑り角推定ロジックについては後述する。

車両状態量統合部10cは、図６に示すように、切り替え判定部10c1、信号切り替えおよび誤差補正部10c2および異常検出部10c3を有する。切り替え判定部10c1は、車体横滑り角推定部11で推定された車体横滑り角推定値に基づき、車両状態量算出値の切り替え判定を行う。信号切り替えおよび誤差補正部（第１補正部，第２補正部）10c2は、切り替え判定部10c1による判定値に基づき、第１車両状態量算出部10aと第２車両状態量算出部10bの算出値を連続的に切り替える。異常検出部10c3は、第１車両状態量算出部10aと第２車両状態量算出部10bの算出値を比較して異常を判定し、異常判定信号を出力する。なお、信号切り替えおよび誤差補正部10c2による誤差補正処理および異常検出部10c3による異常検出処理については後述する。

道路形状推定部12は、車両状態量算出部10で算出した車両状態量である、車両姿勢角、車両軌跡曲率および車両軌跡曲率変化率から、現在走行中の道路の道路形状の推定値を推定する。上記道路形状の推定値は、道路曲線接線方向、道路曲率および道路曲率変化率である。なお、道路形状推定ロジックについては後述する。
道路形状予測部13は、道路形状推定部12により推定された道路形状の推定値から、あらかじめ設定した距離までの車両進行方向前方の道路形状を予測する。車両進行方向前方の道路形状の予測値は、現在位置からあらかじめ設定した前方距離もしくは当該前方距離までの道路形状に対する、車両進行方向前方の道路の道路曲線接線方向、道路曲率、道路曲率変化率である。なお、道路形状ロジックについては後述する。

［道路形状予測処理］
次に、道路形状予測ECU6の道路形状予測処理について、図７のフローチャートを参照して説明する。図７は、道路形状予測処理の流れを示すフローチャートである。
ステップS10では、各センサの観測量を入力する。
ステップS20では、車両状態量算出部10において、車両状態量算出部10aおよび第２車両状態量算出部10bによりそれぞれ車両状態量を算出する。
ステップS30では、車両状態量統合部10cにおいて、第１車両状態量算出部10aと第２車両状態量算出部10bでそれぞれ算出された車両状態量を、車体横滑り角推定部11で推定した車体横滑り角推定値に基づいて統合する。統合方法については後述する。
ステップS40では、道路形状推定部12において、車両状態量算出部10により算出された車両状態量に基づき道路形状の推定値を求める。
ステップS50では、道路形状予測部13において、道路形状推定部12により推定された道路形状の推定値に基づき、あらかじめ設定した前方距離までの道路形状を予測する。

［第１車両状態量算出処理］
次に、第１車両状態量算出部10aの第１車両状態量算出処理について、図８のフローチャートを参照して説明する。図８は第１車両状態量算出処理の流れを示すフローチャートである。
ステップS100では、車輪速センサ5からの車速情報に基づき車両の移動距離を算出する。
ステップS110では、ステップS100で求めた移動距離に基づき、車両軌跡接線方向および曲率を算出する。
ステップS120では、操舵角の距離微分を算出する。
ステップS130では、ステップS120にて算出した操舵角の距離微分に基づき車両軌跡曲率変化率を算出する。

［第２車両状態量算出処理］
次に、第２車両状態量算出部10bの第２車両状態量算出処理について、図９のフローチャートを参照して説明する。図９は、第２車両状態量算出処理の流れを示すフローチャートである。
ステップS200では、車輪速センサ5からの車速情報および加速度センサ8からの加速度情報に基づき車両の移動距離を算出する。
ステップS210では、直接観測できない中間状態量を推定する。
ステップS220では、ステップS200で求めた移動距離およびステップS210で推定した中間状態量推定値に基づき、車両軌跡接線方向および曲率、曲率変化率を算出する。

［車両状態量統合処理］
次に、車両状態量統合部10cの車両状態量統合処理について、図１０〜図１２を参照して説明する。
図１０は、車両状態量切り替え処理の流れを示すフローチャートである。
ステップS300では、車体横滑り角推定値β^*の絶対値|β^*|と閾値β_TH1とを比較して|β^*|＜β_TH1であるか否かを判定し、YESの場合はステップS320へ進み、NOの場合はステップS310へ進む。
ステップS310では、車体横滑り角推定値の大きさ|β^*|と２つの閾値β_TH1,β_TH2(>β_TH1)とを比較してβ_TH1≦|β^*|≦β_TH2であるか否かを判定し、YESの場合はステップS330へ進み、NOの場合はステップS340へ進む。
ステップS320では、第１車両状態量算出部10aの算出値（第１車両状態量）を出力する。
ステップS330では、２つの算出値の加重平均値を使用する。すなわち、第１車両状態量算出部10aの算出値と第２車両状態量算出部10bの算出値を車体横滑り角推定値の大きさ|β^*|に基づき重み付けした値を出力する。ここでは、|β^*|が大きいほど第１車両状態量算出部10aの算出値よりも第２車両状態量算出部10bの算出値の重み付けを大きくする。
ステップS340では、第２車両状態量算出部10bの算出値（第２車両状態量）を出力する。

図１１は、車両状態量統合部10cの動作説明図を示す。同図は横軸が時間で、縦軸が車体横滑り角推定値の大きさを表している。この図では時間と共に車体横滑り角推定値がゼロの状態からの大きさが大きくなり、その後減少し、再びゼロになる場合の図１０における３つの動作モード作動区間を表している。このように、異なる２つの車両状態量算出部10a,10bで算出した異なる算出値を車体横滑り角推定値に基づき連続的に切り替える。

図１２は、車体横滑り角に応じた車両状態量算出量の切り替えを示す概念図である。同図において、横軸は車体横滑り角を表し、縦軸は前後輪横力差を現している。前後輪横力差が発生しない場合は横滑りが発生しないので第１車両状態量を用いる。なお、このとき、車両は後輪車軸中心を基点に自転運動が発生する。
前後輪横力差が増加するに従って、車体横滑り角も増加する。これに従い、車両の自転運動の基点は後輪車軸中心から重心へと移行する。そのため、車体横滑り角推定値に応じて第１車両状態量と第２車両状態量から車体横滑り角推定値に基づく加重平均値を算出して用いる。
前後輪横力のバランスが崩れ、十分に大きい車体横滑り角が発生している場合においては第２車両状態量を用いる。

［誤差補正処理］
次に、信号切り替えおよび誤差補正部10c2の誤差補正処理について、図１３のフローチャートを参照して説明する。図１３は、誤差補正処理の流れを示すフローチャートである。
ステップS400では、車両状態算出誤差となる道路形状（路面カント，うねり）を検出する。
ステップS410では、車輪のスリップに起因する車両状態算出誤差の要因（スリップ，車輪空転）を検出する。
ステップS420では、あらかじめ設定した補正閾値以上の車両状態算出誤差があると判定すると補正項を算出する。詳細については後述するが、例えば、評価関数値Jを演算し、その評価関数値Jが補正閾値Th1以上の場合に補正項を算出する。
ステップS430（第１補正限界判定部，第２補正限界判定部）では、あらかじめ設定した補正限界閾値Th2(>Th1)を超える誤差があるか否かを判定し、YESの場合はステップS450へ進み、NOの場合はステップS440へ進む。
ステップS440では、補正閾値Th1以上の誤差がある場合には、補正項で車両状態量を補正する。
ステップS450では、補正限界状態フラグをONにする。補正限界状態フラグがONの場合には、例えば、過去の情報をクリアし、道路形状予測のための計算をやり直す。

［異常検出処理］
次に、異常検出部10c3の異常検出処理について、図１４のフローチャートを参照して説明する。図１４は、異常検出処理の流れを示すフローチャートである。
ステップS500では、車体横滑り角推定値の絶対値|β^*|が閾値β_TH2未満であるか否かを判定し、YESの場合はステップS510へ進み、NOの場合はステップS500を繰り返す。
ステップS510では、第１車両状態量算出部10aで算出した車両状態量と第２車両状態量算出部10bで算出した車両状態量との差を算出する。
ステップS520では、２つの異なる車両状態量算出部10a,10bで算出した車両状態量の差異が異常判定閾値以上であるか否かを判定し、YESの場合はステップS530へ進み、NOの場合はステップS500へ戻る。
ステップS530では、異常判定信号を出力する。
すなわち、車体横滑り角の絶対値が十分に小さい場合(|β^*|<β_TH2)、車体横滑り角の発生による第１車両状態量の算出誤差は微小であるため、このとき第２車両状態量との差が大きい場合には、異常が発生している可能性が高い。よって、推定された車体横滑り角の絶対値が十分に小さい場合には、２つの車両状態量を比較することで、異常を精度良く判定できる。

［第１車両状態量算出ロジック］
第１車両状態量算出部10aで算出する車両軌跡接線方向（車両姿勢角）、車両軌跡曲率および車両軌跡曲率変化率の算出について説明する。
車速Vは、下記の式(1)に基づき、左後輪の車輪速Vlと右後輪の車輪速Vrから求めることができる。
V = (Vl + Vr)/2 …(1)
ここで、式(1)における左右の後輪車輪速Vl,Vrは、タイヤ径に基づき車輪速を車体速に換算した値である。
また、ヨーレートγは、左右の後輪車輪速Vl,Vrを用いて下記の式(2)で表すことができる。Trd[m]は、後輪トレッド長をトレッド値である。
γ = (Vl - Vr)/Trd …(2)
また、ヨーレートセンサ7の観測値を用いても良い。
さらに、曲率ρ = γ/Vと表すことができる。

これらに基づき、車両軌跡の曲率は、下記の式(3)によって求めることができる。
ρ = 2(Vl - Vr)/{Trd×(V1 + Vr)} …(3)
また、V = (dL/dt)と表せることから、式(1)は下記の式(4)のように表現できる。
dL = (dLl + dLr)/2 …(4)
ここでdLは車体の移動距離、dLl,dLrは左右の後輪車輪移動距離を表している。dLl,dLrは後輪車輪速センサ5の情報で検知できる。
また、式(3)は、操舵角に基づき下記の式(5)で表すことができる。
ρ = 2(dLl - dLr)/{Trd×(dLl + dLr)} …(5)
また、下記の式(6)に基づき、操舵角δから車両軌跡の曲率ρを算出することができる。
ρ = tan(δ/G_r)/Lw …(6)
ここで、Lw[m]はホイールベース長、G_rはステアリングホイールとタイヤの転舵角間のギア比である。

ここで、式(6)を距離微分すると、下記の式(7)が得られる。

式(4)を用いて、式(5)および式(7)を、オイラー法により近似して差分方程式で表現すると、下記の式(8)〜(10)が得られる。

ここで、θ_nは車両軌跡接線方向、ρ_nは車両軌跡曲率、ζ_nは車両軌跡曲率変化率を表している。また、添え字「n」は、車両移動距離dL毎に増加する。
式(10)を算出し、式(8)および式(9)を車両移動距離dL毎に数値積分することにより、車両状態量を算出できる。
すなわち、第１車両状態量算出部10aは、式(8)〜(10)によって、車両移動距離dL毎に、車両状態量として、車両軌跡接線方向、車両軌跡曲率および車両軌跡曲率変化率を求める。

ここで、上記車両状態量を算出する際のモデルは、車両が平坦な舗装路面を走行し、かつ、横滑りなどの車輪の滑りが発生していないという条件でのモデルである。
このため、上記車両状態量の算出に影響を与えるだけの走行路面形状変化が存在する場合、もしくは上記車両状態量の算出に影響を与えるだけの走行路面に対する車輪の滑りを検出すると、信号切り替えおよび誤差補正部10c2は、車両状態量を補正する。
車両状態算出誤差になり得る走行路面形状として路面カントやうねりなどが存在する。また、車両状態算出誤差になり得る走行路面に対する車輪の滑りとなり得るものとして、横滑りや片流れ、スリップなどの現象がある。そして、信号切り替えおよび誤差補正部10c2は、上記要因で発生する車両状態算出誤差があらかじめ設定した補正閾値以上となったことを検出した場合、例えば、式(8)〜(10)により算出した車両状態量、もしくは観測値としての車速値や操舵角を補正する。

信号切り替えおよび誤差補正部10c2の処理について補足説明する。
路面カントやうねり等の車両状態算出誤差要因の有無を検出する。あらかじめ設定した平坦路に対する、あらかじめ設定した以上の路面形状変動が存在すると判定すると、車両状態算出誤差要因があると判定する。
車両状態算出誤差は、例えば、次のようにして検出する。すなわち、後輪車輪速から算出した車両軌跡状態量（式(5)に基づく後輪車輪速から求めた曲率ρ_w）と前輪舵角から算出した車両軌跡状態量（式(6)に基づく操舵角から求めた曲率ρ_s）とを算出し、その偏差(ρ_w - ρ_s)に基づき、路面カントやうねりなどの車両状態算出誤差を検出する。
信号切り替えおよび誤差補正部10c2は、例えば、あらかじめ設定した一定走行距離L[m]毎に、逐次、下記の式(11)に基づき、車両状態算出誤差として評価関数値Jを評価する、そして、その評価関数値Jがあらかじめ設定した補正閾値Th1以上となったと判定したら、上記車両状態量を補正する。

また、上記信号切り替えおよび誤差補正部10c2は、上記式で示した評価関数値Jが、上記補正閾値Th1よりも大きい値としてあらかじめ設定した補正限界閾値Th2を超えた場合に、補正限界と判定する。
ここで、路面カントによる路面形状誤差要因を例にして補正の一例を説明する。図１５に示すような路面カントがついた直進路を走行した場合について考える。図１５の路面では、紙面上側の路面高さが高く、かつ、紙面下側が低いものとする。この路面を、左から右に走行する際に、ハンドルを中立状態のままにして走行すると、路面の高低差の影響により車両軌跡は破線の軌跡を辿る。従って、この路面を直進するためには、左側に補舵を加える必要がある。ここで、適切な補舵を加えた結果、実線で示す直進軌跡が得られたとする。この場合、舵角を用いて算出した車両軌跡は、一点鎖線の軌跡となり、後輪車輪速を用いて算出した車両軌跡は実線の軌跡となる。つまり、両者の軌跡の曲率が異なることから、この差分を補正項として、式(8)〜(10)で表される算出した車両状態量を補正する。

なお、通常走行状態では、横滑りが発生するほどのカントやうねりは通常の舗装道路上には存在しないので、車体は横滑りしないと仮定する。上記車両状態を求めるためのモデルは、平坦路面を仮定したモデルを用いているため、路面カントやうねりがある場合に、先に示したような推定誤差が生じる。
また通常、横滑りは車速が大きい（概ね50km/h以上）状態で旋回した場合や、スプリットミュー路のような、各輪の路面摩擦が異なる場合に、タイヤのグリップ力が飽和することにより発生する。
車体の横滑り成分は、他のセンサ情報（例えば、ヨーレートセンサ7や加速度センサ8）の情報を用いて横滑り成分を推定すれば良い。そして、推定した横すべり成分に対して、各輪の横滑り成分を検知し、補正項として補正する。
すなわち、信号切り替えおよび誤差補正部10c2は、各輪の横滑り成分を検出し、その検出した横滑り分を補正項として、式(8)〜(10)で表される算出した車両状態量を補正する。

［第２車両状態量算出ロジック］
次に、第２車両状態量算出部10bで算出する車両軌跡接線方向（車両姿勢角）、車両軌跡曲率および車両軌跡曲率変化率の算出について説明する。
車速Vxは、第１車両状態量算出部10aと同様に式(1)に基づき、左右の後輪車輪速Vl,Vrから求めることができる。また、加速度センサ8の観測値を前後加速度a_xs、横加速度a_ys、車体横滑り角推定値βとすると、車両軌跡の曲率は、下記の式(12)で表すことができる。
ρ = a_yscosβ/Vx² …(12)
また、曲率変化率は下記の式(13)で表すことができる。

ここで、式(13)における第１項の時間微分値は観測可能な物理量ではないため、観測可能な横加速度a_ysを車速Vxで割り算出した値にカルマンフィルタを適用することにより推定する。
また、前後加速度a_xsは路面勾配による重力項が誤差として重畳するため、車速Vxにカルマンフィルタを適用して推定した前後加速度a_v ^*との差から路面勾配による重力誤差項を算出する。この重力誤差項を差し引いた加速度センサ8の前後加速度a_xsaと車速Vxにカルマンフィルタを適用して推定することにより前後加速度a_xを得る。
なお、車両軌跡接線方向は、下記の式(14)で表される。
θ_n+1 = θ_n + ρ_ndL …(14)
ここで、上記車両状態量を算出する際のモデルは、車両が平坦な舗装路面を走行し、かつ、十分大きな車速での走行において車体横滑りが発生しているという条件でのモデルである。
すなわち、第２車両状態量算出部10bは、式(12)〜(14)によって、車両移動距離dL毎に、車両状態量として、車両軌跡接線方向、車両軌跡曲率および車両軌跡曲率変化率を求める。

［車体横滑り角推定ロジック］
次に、車体横滑り角推定部11における車体横滑り角の推定方法について説明する。
車体横滑り角βは、下記の式(15)を用いて算出する。ここで、δは前輪転舵角、Lfは重心から前輪車軸までの距離である。
β = tan^-1(tanδ - Lfγ/Vx) …(15)
また、車体横滑り角速度は下記の式(16)を用いて算出する。

観測可能な物理量から算出された車体横滑り角と車体横滑り角速度にカルマンフィルタを適用することにより車体横滑り角を推定する。

［道路形状推定ロジック］
次に、道路形状推定部12における道路曲線接線方向と道路曲率と道路曲率変化率の推定方法について説明する。
道路形状推定部12では、下記の式(17),(18)で表されるダイナミクスに基づき、式(19)で表すカルマンフィルタを用いて、車両状態量算出部10により算出された車両軌跡接線方向推定値θ_n[rad/m]、車両軌跡曲率ρ_n[rad/m]、車両軌跡曲率変化率ζ_n[rad/m²]から、道路曲線接線方向推定値θ_n ^*[rad/m]、道路曲線曲率ρ_n ^*[rad/m]、道路曲線曲率変化ζ_n ^*[rad/m²]を推定する。
ここで、F_nは車両線型モデル、H_nは観測モデル、Q_n,R_nは共分散行列、w_n,v_nは雑音、Pは誤差の共分散行列、K_nはカルマンゲインである。

［道路形状予測ロジック］
次に、道路形状予測部13における前方道路形状予測方法について説明する。
道路形状予測部13では、上記道路形状推定部12で推定した道路曲線接線方向推定値θ_n ^*[rad/m]、道路曲線曲率ρ_n ^*[rad/m]、道路曲線曲率変化ζ_n ^*[rad/m2]と、これらの推定値を数値積分することで、未来の予測誤差を算出し、その予測誤差としての誤差共分散行列Pから式(19)の第１式、第２式を用いて算出することで、進行方向前方の形状予測を行う。
すなわち、道路形状推定部12では、式(19)に基づき予測および更新をして推定することとなるが、道路形状予測部13では、式(19)の第１式、第２式を用いて予測する。

さらに、道路形状推定部12、道路形状予測部13の処理について補足説明する。
上述のように、車両状態量算出部10は、第１車両状態量算出部10aおよび第２車両状態量算出部10bにより算出された２つの車両状態量を統合し、下記の式(20)で表される車両状態量Xを出力する。

ただし、θ_vは車両姿勢角、ρ_vは車両軌跡曲率、(dρ_v/dl)は車両軌跡曲率である。なお、状態量X_n=[θ_n, ρ_n, ζ_n]^Tに基づいて算出する推定値がX_n+1=[θ_n+1 ^*, ρ_n+1 ^*, ζ_n+1 ^*]^Tである。
そして、道路形状推定部12は、車両状態量算出部10で算出した車両状態量Xを入力とし、カルマンフィルタにて予測および観測更新の処理を行い、現在値の道路形状の推定を行う。さらに、道路形状予測部13は、その推定値を入力として、カルマンフィルタにて、進行方向前方に位置する道路形状の予測を行う。

［現在値推定に用いるカルマンフィルタの構成］
次に、道路形状推定部12が用いるカルマンフィルタの構成について説明する。
図１６は、現在値推定に使用する道路形状推定部12のカルマンフィルタの構成を示す。この図１６から分かるように、カルマンフィルタの演算処理は２つの機能ブロックで構成される。一つ目の機能ブロックは、「予測」処理を行い、２つ目の機能ブロックは「観測更新」処理を行う。
上記予測ブロックでは、nステップ目の推定値X_n ^*と推定誤差共分散P_n ^*を基に、n+1ステップ目の予測値X_n+1~と予測誤差共分散P_n+1~を算出する。そして、２つ目の機能ブロックでの観測更新で、n+1ステップ目の予測値X_n+1~とn+1ステップ目の観測値Y_n+1をカルマンゲインKで重み付けしてブレンドすることによりn+1ステップ目の推定値X_n+1 ^*と推定誤差共分散P_n+1 ^*を算出する。
以上の演算処理を演算ステップ毎に逐次繰り返す。ここで、これらの演算ステップはあらかじめ設定した走行距離dl[m]毎に処理を実行する。

［未来値の予測に用いるカルマンフィルタの構成］
次に、道路形状予測部13が未来値の予測に用いているカルマンフィルタの構成について説明する。
図１７は、カルマンフィルタの構成図である。道路形状予測部13が使用するカルマンフィルタの構成は、図１７に示すように、上記道路形状推定部12のカルマンフィルタの構成と基本的な構成は同じある。ただし、未来値の予測では、予測の機能ブロックのみが動作する。すなわち、未来値は観測できないため、観測更新の機能ブロックは使用しない。
そして、予測の演算処理はnステップ目の推定値に基づき、所望の予測長に至るまで予測の機能ブロックの演算処理を演算ステップ毎に逐次繰り返し実施する。これらの演算ステップはあらかじめ設定した走行距離dl[m]毎に処理を実行する。
例えば、上記走行距離dl=0.1[m]とし、現在から進行方向前方の道路形状予測位置を、予測長10[m]に設定したとする。この場合には、nステップ目の推定値を初期値として100回ステップ分の予測演算処理を実行することにより、nステップ目における(n+100)ステップ目までの予測値が算出できることになる。言い換えると、現在位置から10[m]前方までの予測値を算出できることになる。

［現在値推定に用いるカルマンゲインのゲインスケジュール構成］
次に、上記道路形状推定部12において、カルマンゲインをゲインスケジュールしながら車両の走行距離毎に道路形状を推定する場合について説明する。
まず、現実の道路は、道路曲率が連続して変化するというように仮定できるため、局所的には曲率一定であるとみなして解析する。
いま、横滑りが発生しない低速域において、曲率半径R[m]、車線幅4[m]の道路での走行を考える。例えば、R50以上の道路において車両全長5[m]、全幅1.9[m]とすると車線逸脱することなく走行するためには、後輪車軸中心の車線中心からの横ずれ量dが±0.9[m]以内であれば良い。この様子を図１８に示す。

次に、曲率半径R[m]、車線幅4[m]の道路に対して、一定舵角で車線維持できる車両軌道を考える。図１９に一定舵角で車線維持できる車両軌道を示す。最大曲率半径R_maxとなる軌道をL4の線、最小曲率半径R_minとなる軌道をL5の線で示す。
ここで、点P,Qを通る円の方程式からR_maxが得られる。また、点S,R_pを通る円の方程式からR_minが得られる。下記の式(21),(22)にこれらの導出式を示す。
R_maxの導出：
(x + a)² + (y + a)² = R_max ²にP(R+d,0)
およびQ((R-d)/(√2),(R-d)/(√2))を代入して、
(x + d + a)² + a² = R_max ²
←→R² + 2Rd +d² + 2a(R + d) + 2a² = R_max ²
2((R-d)/(√2))² = R_max ²
←→2{1/2(R² + 2Rd +d²)} + (2/(√2))a(R + d) + 2a²} = R_max ² …(21)
辺々引いて、
a = 2Rd/(((√2)-1)R - ((√2) +1)d)
よって、R_max = (√2)a +R - d

R_minの導出：
(x - a)² + (y - a)² = R_min ²にP(R-d,0)
およびQ((R+d)/(√2),(R+d)/(√2))を代入して、
(x - d - a)² + a² = R_min ²
←→R² - 2Rd +d² - 2a(R - d) + 2a² = R_min ²
2((R+d)/(√2))² = R_min ²
←→2{1/2(R² + 2Rd +d²)} - (2/(√2))a(R + d) + 2a²} = R_min ² …(22)
辺々引いて、
a = -2Rd/(((√2)-1)R - ((√2) +1)d)
よって、R_min = -(√2)a +R + d
そして、式(21),(22)から、最大曲率ρ_maxおよび最小曲率ρ_minは、下記の式(23)で表せる。

つまり、曲率半径R、逸脱限界幅dに対する曲率ρの取り得る値は、
ρ_min≦ρ≦ ρ_max
となることが分かる。

ここで、曲率半径Rの車線に対して、車線維持可能な曲率分布が図２０に示すような正規分布に従うと仮定すると、その分散σρ²は式(23)から算出できる。
ここで、道路曲率に応じてカルマンゲインをゲインスケジュールするために、現在走行中の車線の曲率半径Rを、操舵角δを用いて推定する。前出の式(6)とR=1/ρの関係からRを算出し式(23)に代入すれば、ρ_minおよびρ_maxを算出できる。例えば、下記の式(24)のようにおくことで曲率分散値σρ²を算出できる。
3σ_ρ = max(|ρ - ρ_max|, |ρ - ρ_min|) …(24)

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

次に、作用を説明する。
実施例１では、車両状態量算出部10により車両状態量を算出し、算出した車両状態量から道路形状推定部12により現在地の道路形状を推定し、さらに、道路形状予測部13により道路形状推定部12により算出された推定値を基に進行方向前方の道路形状を予測する。
図２１に、道路形状予測の概念図を示す。図２１に示すように、車両の走行軌跡から現在の道路形状を推定し、その推定値から未来の道路形状の予測値を求める。
このように、カメラなどの外界認識センサによって進行方向前方の道路の観測を行うことなく、つまり高価なセンサを使用することなく、通常車両に搭載されているセンサからの情報だけで、進行方向前方の道路形状を精度良く予測できる。

ここで、車両状態量算出部10は、車体横滑り角が発生しないことを条件としたモデル（式(8)〜(10)）を用いて算出した第１車両状態量と、車体横滑り角が発生していることを条件としたモデル（式(12)〜(14)）を用いて算出して第２車両状態量とを、車体横滑り角に応じて重み付けして車両状態量を算出する。このとき、推定した車体横滑り角の絶対値が大きいほど第２車両状態量の重み付けを大きくし、車体横滑り角推定値の絶対値が閾値β_TH1よりも小さい場合には第２車両状態量の重み付けをゼロとし、車体横滑り角推定値の絶対値が閾値β_TH2よりも大きい場合には第１車両状態量の重み付けをゼロとする。
車体横滑り角が発生していない場合、または、車体横滑り角の絶対値が小さい場合、第１車両状態量の算出精度は高いのに対し、第２車両状態量の算出精度は低い。一方、車体横滑り角が発生している場合には、車体横滑り角の絶対値が大きくなるほど、第１車両状態量の算出精度は低下し、第２車両状態量の算出精度は向上する。よって、車体横滑り角の絶対値が大きいほど第２車両状態量の重み付けを大きく（第１車両状態量の重み付けを小さく）することにより、車体横滑り角の大きさにかかわらず、車両状態量の算出精度を向上できる。

実施例１では、以下に列挙する効果を奏する。
(1) 車速を検出する車輪速センサ5と、操舵角を検出する操舵角センサ4と、車両に作用する加速度を検出する加速度センサ8と、車両に作用するヨーレートを検出するヨーレートセンサ7と、車体横滑り角を推定する車体横滑り角推定部11と、検出された車速、操舵角、加速度およびヨーレートと、推定された車体横滑り角とに基づき、車両状態量として車両姿勢角、車両軌跡曲率および車両軌跡曲率変化率を算出する車両状態量算出部10と、算出された車両状態量に基づき、道路形状を表すパラメータとして道路曲線接線方向、道路曲率および道路曲率変化率を推定する道路形状推定部12と、推定された道路形状のパラメータを積分して車両進行方向前方の道路形状を予測する道路形状予測部13と、を備えた。
よって、車体横滑り角の影響を加味した車両状態量に基づき道路形状を予測するため、車体横滑り角が生じた場合であっても精度良く進行方向前方の道路形状を予測できる。

(2) 車両状態量算出部10は、検出された車速と操舵角とに基づき、第１車両状態量として車両姿勢角、車両軌跡曲率および車両軌跡曲率変化率を算出する第１車両状態量算出部10aと、検出された車速、加速度およびヨーレートと、推定された車体横滑り角とに基づき、第２車両状態量として車両姿勢角、車両軌跡曲率および車両軌跡曲率変化率を算出する第２車両状態量算出部10bと、推定された車体横滑り角に応じて第１車両状態量と第２車両状態量を重み付けした車両状態量を算出する車両状態量統合部10cと、を備え、車両状態量統合部10cは、推定された車体横滑り角の絶対値が大きいほど第１車両状態量よりも第２車両状態量の重み付けを大きくする。
よって、車体横滑り角の大きさにかかわらず、車両状態量の算出精度を向上できる。

(3) 推定された車体横滑り角の絶対値が十分に小さいとみなせる場合、第１車両状態量と第２車両状態量との差が異常判定閾値以上であるとき、異常と判定する異常検出部10c3を備えた。
よって、推定された車体横滑り角の絶対値が十分に小さい場合には、２つの車両状態量を比較することで、異常を精度良く判定できる。

(4) 車輪速センサ5は、車輪の回転に応じてあらかじめ設定した距離間隔毎に発生するパルス情報に基づく車両の走行距離から車速を検出し、車両状態量算出部10は、あらかじめ設定した走行距離毎に操舵角を検出し、検出した操舵角の距離微分に基づき車両軌跡曲率変化率を算出する。
よって、低車速時にも誤差が重畳されにくく、高精度な道路形状の予測を実現できる。

(5) 道路形状推定部12は、カルマンゲインを含むカルマンフィルタを有し、カルマンゲインは、推定された道路形状のパラメータに基づいて算出した曲率分散値からなる共分散行列を含み、この曲率分散値をあらかじめ設定した所定の値に調整しながら、あらかじめ設定した車両の走行距離毎に道路形状を推定する。
よって、低車速時にも誤差が重畳されにくく、高精度な道路形状の予測を実現できる。また、操舵角を用いて道路形状の推定に用いる曲率分散値を調整するため、多様な道路形状に対して精度良く道路形状の予測を実現できる。

(6) 車両状態量算出部10は、車両状態量の算出に対して誤差要因となる道路形状を検出し、その検出した誤差要因となる道路形状に基づき前記車両状態量を補正する信号切り替えおよび誤差補正部10c2を備えた。
よって、路面カントやうねりなどの道路形状による誤差を補正することで、路面状況などによらず安定した道路形状の予測を実現できる。

(7) 信号切り替えおよび誤差補正部10c2により検出される道路形状が補正可能か否かを判定する第１補正限界判定部（S430）を備えた。
よって、道路形状の予測精度が低下したことを判定できる。

(8) 車両状態量算出部10は、路面に対する車輪の滑りを検出し、検出した車輪の滑りに基づき車両状態量を補正する信号切り替えおよび誤差補正部10c2を備えた。
よって、車輪のスリップや空転などによる誤差を補正することで、路面状況などによらず安定した道路形状の予測を実現できる。

(9) 信号切り替えおよび誤差補正部10c2により検出された滑りが補正可能な否かを判定する第２補正限界判定部（S430）を備えた。
よって、道路形状の予測精度が低下したことを判定できる。

（他の実施例）
以上、本発明を実施するための形態を、実施例に基づいて説明したが、本発明の具体的な構成は、実施例に限定されるものではなく、発明の要旨を逸脱しない範囲の設計変更等があっても本発明に含まれる。

1 ステアリングホイール
2 操舵装置
3 操向輪
4 操舵角センサ（操舵角検出部）
5 車輪速センサ（車速検出部
6 道路形状予測ECU
7 ヨーレートセンサ（ヨーレート検出部）
8 加速度センサ（加速度検出部）
10 車両状態量算出部
10a 第１車両状態量算出部
10b 第２車両状態量算出部
10b1 曲率算出部
10b2 車両前後加速度推定部
10b3 車両公転成分推定部
10b4 曲率変化率算出部
10c 車両状態量統合部
10c1 切り替え判定部
10c2 信号切り替えおよび誤差補正部（第１補正部，第２補正部）
10c3 異常検出部
11 車体横滑り角推定部
11a 車体横滑り角算出部
11b 車体横滑り角速度算出部
11c 車体横滑り角推定カルマンフィルタ
12 道路形状推定部
13 道路形状予測部
14 曲率分散値算出部
S430 第１補正限界判定部，第２補正限界判定部

Claims (8)

1. 車速を検出する車速検出部と、
   操舵角を検出する操舵角検出部と、
   車両に作用する加速度を検出する加速度検出部と、
   車両に作用するヨーレートを検出するヨーレート検出部と、
   車体横滑り角を推定する車体横滑り角推定部と、
   検出された車速と操舵角とに基づき、第１車両状態量として車両姿勢角、車両軌跡曲率および車両軌跡曲率変化率を算出する第１車両状態量算出部と、
   検出された車速、加速度およびヨーレートと、推定された車体横滑り角とに基づき、第２車両状態量として車両姿勢角、車両軌跡曲率および車両軌跡曲率変化率を算出する第２車両状態量算出部と、
   推定された車体横滑り角に応じて前記第１車両状態量と前記第２車両状態量を重み付けした車両状態量を算出する車両状態量統合部と、
   からなる車両状態量算出部と、
   算出された前記車両状態量に基づき、道路形状を表すパラメータとして道路曲線接線方向、道路曲率および道路曲率変化率を推定する道路形状推定部と、
   推定された道路形状のパラメータを積分して車両進行方向前方の道路形状を予測する道路形状予測部と、
   を備え、
   前記車両状態量統合部は、推定された車体横滑り角の絶対値が大きいほど前記第１車両状態量よりも前記第２車両状態量の重み付けを大きくすること、
   を特徴とする道路形状予測装置。
2. 請求項１に記載の道路形状予測装置において、
   推定された車体横滑り角の絶対値が十分に小さいとみなせる場合、前記第１車両状態量と前記第２車両状態量との差が異常判定閾値以上であるとき、異常と判定する異常検出部を備えたことを特徴とする道路形状予測装置。
3. 請求項１ないし請求項２のいずれか１項に記載の道路形状予測装置において、
   前記車速検出部は、車輪の回転に応じてあらかじめ設定した距離間隔毎に発生するパルス情報に基づく車両の走行距離から車速を検出し、
   前記第1車両状態量算出部は、あらかじめ設定した走行距離毎に操舵角を検出し、検出した操舵角の距離微分に基づき前記車両軌跡曲率変化率を算出することを特徴とする道路形状予測装置。
4. 請求項１ないし３のいずれか１項に記載の道路形状予測装置において、
   前記道路形状推定部は、カルマンゲインを含むカルマンフィルタを有し、
   前記カルマンゲインは、推定された道路形状のパラメータに基づいて算出した曲率分散値からなる共分散行列を含み、この曲率分散値をあらかじめ設定した所定の値に調整しながら、あらかじめ設定した車両の走行距離毎に道路形状を推定することを特徴とする道路形状予測装置。
5. 請求項１ないし４のいずれか１項に記載の道路形状予測装置において、
   前記車両状態量算出部は、車両状態量の算出に対して誤差要因となる道路形状を検出し、その検出した誤差要因となる道路形状に基づき前記車両状態量を補正する第１補正部を備えたことを特徴とする道路形状予測装置。
6. 請求項５に記載の道路形状予測装置において、
   前記第１補正部により検出される道路形状が補正可能か否かを判定する第１補正限界判定部を備えたことを特徴とする道路形状予測装置。
7. 請求項１ないし６のいずれか１項に記載の道路形状予測装置において、
   前記車両状態量算出部は、路面に対する車輪の滑りを検出し、検出した車輪の滑りに基づき前記車両状態量を補正する第２補正部を備えたことを特徴とする道路形状予測装置。
8. 請求項７に記載の道路形状予測装置において、
   前記第２補正部により検出された滑りが補正可能な否かを判定する第２補正限界判定部を備えたことを特徴とする道路形状予測装置。

Images