JP7056356B2

JP7056356B2 - 車両状態推定装置

Info

Publication number: JP7056356B2
Application number: JP2018086078A
Authority: JP
Inventors: 孝雄小林; 義和服部; 豪軌杉浦; 大輝森; 裕之山口; 一剛武井
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2018-04-27
Filing date: 2018-04-27
Publication date: 2022-04-19
Anticipated expiration: 2038-04-27
Also published as: JP2019189121A

Description

本発明は車両状態推定装置に関し、特に車両の３次元位置の推定に関する。

従来から、車両状態を推定する技術が提案されている。

特許文献１には、鉛直方向加速度が変化する路面であっても、精度よく姿勢角を推定することができる車両姿勢角推定装置が記載されている。加速度状態量飽和特性補正手段が、ロール角の推定値及びピッチ角の推定値を用いて得られる鉛直加速度状態量を、飽和特性を持たせるように補正する。状態量偏差ローパスフィルタ値演算手段が、姿勢角オブザーバによって算出された前後加速度状態量、横加速度状態量、及び鉛直加速度状態量のローパスフィルタ処理値と、センサ信号、前後車体速度の推定値、及び横車体速度の推定値を用いて得られる前後加速度状態量及び横加速度状態量とに基づいて、ローパスフィルタ処理が行われた、前後加速度状態量の偏差、横加速度状態量の偏差、及び鉛直加速度状態量の偏差を演算する。姿勢角推定手段により、前後加速度状態量の偏差、横加速度状態量の偏差、及び鉛直加速度状態量の偏差をフィードバックして、ロール角及びピッチ角を推定する。

特許文献２には、移動体のロール角を高い精度で推定するロール角推定装置が記載されている。ロール角推定装置のカルマンフィルタは、ロール角速度センサ、ヨー角速度センサ、上下加速度センサ、前後加速度センサ、左右加速度センサおよび後輪速度センサの検出値ならびに前回の推定動作によるロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂr、ヨー角速度センサオフセットｂy、上下加速度センサオフセットｂzの推定値、ピッチ角θの推定値およびピッチ角速度ωpの推定値に基づいて、車体のロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂr、ヨー角速度センサオフセットｂy、上下加速度センサオフセットｂz、ピッチ角θおよびピッチ角速度ωpを推定する。

特開２０１０－１４３３７９号公報特開２０１５－２０９１０６号公報

特許文献１では、車両の並進３自由度、ロール、ピッチ運動に関する運動方程式から非線形のオブザーバを構成しており、車両運動固有の特徴として、長時間の間では
・上下速度は無視できる
・車輪速度と車体の前後速度は等しい（タイヤの前後スリップ率は無視できる）
・スリップ角の微分値は無視できる
・車体に働く鉛直方向の加速度は常に重力加速度と等しい
という仮定を利用してオブザーバに利用している。従って、過渡的な状態量の変化を考慮しきれておらず、ドライバの操作入力や外乱への過渡的な応答に対して推定精度が劣化し得る。

また、特許文献２では、基本的には自動２輪車を対象としているが、４輪車への適用も可能としており、車体の横すべり速度がなく、車輪のすべりもないものとして推定している。しかし、リーンによって旋回する自動２輪車とは異なり、旋回中の４輪車ではこれらを無視することはできず、同様に推定精度が劣化し得る。

本発明の目的は、比較的短い時間においても、車両が外乱により３次元的に運動することに起因する推定精度の劣化を抑制し得る技術を提供することにある。

本発明の車両状態推定装置は、車両に対する指令値を入力する指令値入力部と、前記車両のばね上に設けられたセンサからのセンサ値を入力するセンサ値入力部と、前記指令値を入力変数とし、３次元の車両運動モデルを用いて事前誤差推定値を算出し、算出した前記事前誤差推定値と観測方程式を用いて出力変数を算出し、前記出力変数と前記センサ値との差分を算出することで前記出力変数の誤差を算出し、さらに前記出力変数の誤差と前記事前誤差推定値とに基づいて前記車両の状態変数として速度及び姿勢角を推定し、推定された前記速度及び姿勢角から前記車両の３次元の位置を算出して出力する演算部とを備えることを特徴とする。

本発明の１つの実施形態では、前記指令値入力部は、操舵角指令値、及び４輪への制駆動力指令値を入力し、前記センサ値入力部は、前記ばね上の３軸の加速度センサ、３軸の角速度センサ、及び前記４輪の回転角速度センサからのセンサ値を入力する。

本発明の他の実施形態では、前記演算部は、さらに、前記３次元の車両運動モデルを用いて外乱を推定する。

本発明のさらに他の実施形態では、前記演算部は、前記外乱として、路面変位、路面カント角、路面勾配、及び横風横力の少なくともいずれかを推定する。

本発明のさらに他の実施形態では、前記演算部は、拡張カルマンフィルタを用いて推定する。

本発明によれば、車両が外乱により３次元的に運動することに起因する推定精度の劣化を抑制することができる。

また、本発明によれば、３次元の車両運動モデルで車両の３次元位置を算出しているので、平面運動を仮定した線形２輪モデルを用いて推定する場合に比べて推定誤差の劣化を効果的に抑制できる。

さらに、本発明によれば、３次元の車両運動モデルで外乱を推定することで、外乱による推定誤差の劣化を効果的に抑制できる。

実施形態の座標系と車両モデルの説明図である。実施形態の全体構成図である。実施形態の構成ブロック図である。実施形態の処理フローチャートである。

以下、図面に基づき本発明の実施形態について説明する。

図１は、本実施形態における車両モデルと各種の座標系を示す。

座標系として、グルーバル座標系（ｘ_ｇ，ｙ_ｇ，ｚ_ｇ）、路面座標系（ｘ_ｒ，ｙ_ｒ，ｚ_ｒ）、及びばね上座標系（ｘ_ｙ，ｙ_ｙ，ｚ_ｙ）を設定する。路面座標系（ｘ_ｒ，ｙ_ｒ，ｚ_ｒ）は、車両が走行する路面に固定された直交座標系であり、ｘ軸とｙ軸を水平面内にとり、ｚ軌を上向きにとる。通常、車両の位置を指定するために用いられ、運動の軌跡はこの座標系上で示すことができる。ばね上座標系（ｘ_ｙ，ｙ_ｙ，ｚ_ｙ）は、・車両のばね上に固定し、ばね上の重心に原点をおいた直交座標系をいう。静止状態でｘ軸を前向きに、ｙ軸を左向きに水平にとり、ｚ軸を上向きにとる。ばね上座標系（ｘ_ｙ，ｙ_ｙ，ｚ_ｙ）は、ばね上とともに回転運動及び並進運動を行う。

車両は、ばね上で６自由度（各軸の並進及び各軸回りの回転）、ばね下で２自由度（上下及び回転）を有し、空気抵抗を受けるとともに路面から転がり抵抗を受けるものとする。
状態変数を

とする。

また、入力変数を

とする。

さらに、出力変数を

とする。

ドライバあるいは自動運転制御システムが入力する、操舵角、４輪（左前輪、右前輪、左後輪、右後輪）への制駆動力指令値、及び車体のばね上に設けられた３軸の加速度センサ、３軸の角速度センサ、４輪の回転角速度から、状態変数を推定する。

いま、車両の３次元運動方程式を、以下のように離散時間状態空間で表現する。

ここで、ｆは車両の３次元運動方程式である。

ばね上の姿勢角、すなわちロール、ピッチ、ヨー角のオイラー角が定まれば、グローバル座標系からばね上座標系への座標変換マトリクスは、次式で与えられる。

ここで、Ｒｘ，Ｒｙ，Ｒｚは、それぞれ各軸回りの回転行列である。

路面座標系のヨー角は、ばね上のヨー角と等しいと定義し、同様にグローバル座標系から路面座標系への座標変換マトリクスは、次式で与えられる。

従って、これらの式より、路面座標系に対するばね上座標系への座標変換マトリクスは、次式で与えられる。

これらを用いて、車両の３次元運動方程式を以下のように表す。

ここで、（数９）におけるρは空気密度、Ａは前方投影面積、Ｃ_ｄは空気抵抗係数であり、（数１１）における右辺第２項の行列はばね上回転慣性、Ｍｓはモーメントベクトルであり、（数１３）における右辺第２項のＦ_ｉｚ，ｉは接地荷重、Ｆ_ｓｚ，ｉはサスペンション反力、（数１４）における右辺第２項のＭ_ｉ，ｊはばね下回転モーメント、Ｉ_ｉはばね下回転慣性、（数１５）における右辺第２項の各ｗは、外乱推定のための白色雑音を示す。

本実施形態では、（数１５）に示すように路面変位、路面カント角、路面勾配、及び横風横力を外乱として３次元運動方程式に組み込んでいる点に留意されたい。

以上が車両の３次元運動方程式であり、これらのダイナミクスを有するものとして、例えばカルマンフィルタ等の推定アルゴリズムを構成し、状態変数を推定する。非線形性があることに加え、外乱の推定を行うために、例えば拡張カルマンフィルタを用いて推定し得る。但し、拡張カルマンフィルタに限定されるものではなく、その他のフィルタ、例えばアンセンテッドカルマンフィルタ（ＵＫＦ）等を用いてもよい。

拡張カルマンフィルタでは、次式のような予測式に従って、推定を行う。

ここで、ｇはカルマンゲインであり、非線形カルマンフィルタにより逐次更新される。そして、最終的に状態推定の速度ベクトルと姿勢角から、グローバル座標系における速度を算出し、当該速度を積分することで車両の自己位置を推定する。

図２は、本実施形態の全体構成ブロック図を示す。車両状態推定装置は、車両運動状態推定部１０と、自己位置推定部２０を備える。車両運動状態推定部１０及び自己位置推定部２０は、演算部として機能する。

車両運動状態推定部１０は、入力変数として取得指令値を入力し、出力変数として取得センサ値を入力し、上記の運動方程式を用いて状態変数、具体的には速度ベクトルと姿勢角を推定して自己位置推定部２０に出力する。

自己位置推定部２０は、車両運動状態推定部１０で推定された状態変数、具体的には速度ベクトルと姿勢角からグローバル座標系における速度を算出し、さらにこれを積分して車両の自己位置を算出して出力する。

図３は、図２における車両運動状態推定部１０の詳細構成ブロック図を示す。車両運動状態推定部１０は、各種指令値を入力する指令値入力部１１、各種センサ値を入力するセンサ値入力部１３を備え、さらに、車両運動モデル部１２、観測方程式部１４、差分器１５、カルマンゲイン計算部１６、加算器１７、及びフィードバック部１８を備える。

指令値入力部１１は、操舵角指令値、左前輪制駆動力指令値、右前輪制駆動力指令値、左後輪制駆動力指令値、右後輪制駆動力指令値を入力し、これらを取得指令値（入力変数）として車両運動モデル部１２に出力する。

センサ値入力部１３は、前後加速度センサ値、横加速度センサ値、上下加速度センサ値、ロール角速度センサ値、ピッチ角速度センサ値、ヨー角速度センサ値、左前輪角速度センサ値、右前輪角速度センサ値、左後輪角速度センサ値、右後輪角速度センサ値を入力し、これらを差分器１５に出力する。

車両運動モデル部１２は、既述した車両の３次元運動方程式を用いて事前誤差推定値を算出するとともに、誤差共分散行列を算出する。車両運動モデル部１２は、前回の事後状態推定値と、前回の誤差共分散行列と、指令値入力部１１からの取得指令値とを用いて、事前誤差推定値及び誤差共分散行列を算出する。なお、最初に事前誤差推定値及び誤差共分散行列を算出する際には、前回の事後状態推定値及び前回の誤差共分散行列として予め設定された初期値を用いる。車両運動モデル部１２は、算出した事前誤差推定値を加算器１７に出力するとともに観測方程式部１４に出力する。また、車両運動モデル部１２は、算出した誤差共分散行列をカルマンゲイン計算部１６に出力する。

観測方程式部１４は、観測方程式、すなわち、ｙ＝ｈ（ｘ_ｋ，ｕ_ｋ）を用いて、車両運動モデル部１２で算出された事前誤差推定値を用いて出力変数ｙを算出する。また、そのヤコビ行列（ヤコビアン）を算出する。観測方程式部１４は、算出した出力変数を差分器１５に出力する。また、観測方程式部１４は、算出したヤコビ行列をカルマンゲイン計算部１６に出力する。

差分器１５は、センサ値入力部１３からのセンサ値と、観測方程式部１４で算出された出力変数との差分、すなわち観測方程式部１４で算出された出力変数の誤差を算出してフィードバック部１８に出力する。

カルマンゲイン計算部１６は、車両運動モデル部１２で算出された誤差共分散行列と、観測方程式部１４で算出されたヤコビ行列を用いて、カルマンゲインを算出する。カルマンゲイン計算部１６は、算出したカルマンゲインをフィードバック部１８に出力する。

フィードバック部１８は、差分器１５で算出された誤差を、カルマンゲイン計算部１６で算出されたカルマンゲインを用いて加算器１７に出力することでフィードバックする。また、フィードバック部１８は、誤差共分散行列を更新して、次の時刻ステップにおける前回の誤差共分散行列として車両運動モデル部１２に出力する。

加算器１７は、車両運動モデル部１２で算出された事前状態推定値と、フィードバック部１８からのフィードバック値を加算して事後状態推定値を算出し出力する。また、算出された事後状態推定値を、次の時刻ステップにおける前回の事後状態推定値として車両運動モデル部１２に出力する。

車両運動モデル部１２、観測方程式部１４、差分器１５、カルマンゲイン計算部１６、加算器１７及びフィードバック部１８は、プロセッサ、メモリ及び入出力インタフェースを備える電子制御装置（ＥＣＵ）で構成され得る。プロセッサは、プログラムメモリに記憶された処理プログラムを実行することで、車両運動モデル部１２、観測方程式部１４、カルマンゲイン計算部１６、フィードバック部１８における処理を実現する。なお、これらの機能の一部は、プログラムの実行によるソフトウェア処理ではなく、ハードウェア処理により実現してもよい。ハードウェア処理は、例えばＡＳＩＣやＦＰＧＡ（フィールドプログラマブルゲートアレイ）などの回路を用いて行ってもよい。

図４は、本実施形態の処理フローチャートを示す。現在時刻ステップｋから、次の時刻ステップｋ＋１の状態推定を行うフローチャートである。

まず、指令値入力部１１で操舵角指令値、左前輪制駆動力指令値、右前輪制駆動力指令値、左後輪制駆動力指令値、右後輪制駆動力指令値を取得する。また、センサ値入力部１３で前後加速度センサ値、横加速度センサ値、上下加速度センサ値、ロール角速度センサ値、ピッチ角速度センサ値、ヨー角速度センサ値、左前輪角速度センサ値、右前輪角速度センサ値、左後輪角速度センサ値、右後輪角速度センサ値を取得する（Ｓ１０１）。

次に、車両運動モデル部１２は、取得指令値を入力変数とし、前回の事後状態推定値を状態変数とし、前回の誤差共分散行列値を用いて、車両の３次元運動方程式から、事前誤差推定値、及び誤差共分散行列を次式により算出する（Ｓ１０２）。

ここで、Ｑはシステムノイズを表す共分散行列である。

次に、観測方程式部１４は、車両運動モデル部１２で算出された事前誤差推定値を用いて、出力変数、及びそのヤコビ行列を次式により算出する（Ｓ１０３）。

次に、カルマンゲイン計算部１６は、観測方程式部１４で算出されたヤコビ行列を用いて、カルマンゲインを次式により算出する（Ｓ１０４）。

次に、センサ値入力部１３で前後加速度センサ値、横加速度センサ値、上下加速度センサ値、ロール角速度センサ値、ピッチ角速度センサ値、ヨー角速度センサ値、左前輪角速度センサ値、右前輪角速度センサ値、左後輪角速度センサ値、右後輪角速度センサ値を取得し、差分器１５でこれらのセンサ値と観測方程式１４で算出された事前状態推定値に基づく出力値との差分を算出することで誤差を算出する。そして、フィードバック部１８でこの誤差とカルマンゲイン計算部１６で算出されたカルマンゲインとを用いてフィードバックし、事後状態変数を算出する。また、誤差共分散行列を更新する（Ｓ１０５、Ｓ１０６）。

以上のようにして算出された事後状態推定値、具体的には車両の速度ベクトル（並進速度）及び姿勢角は、自己位置推定部２０に出力される。

自己位置推定部２０では、推定された速度ベクトル及び姿勢角から、グローバル座標系における速度を次式により算出する（Ｓ１０７）。

最後に、自己位置推定部２０では、算出されたグローバル座標系における速度を積分することで、次式により自車位置を推定する（Ｓ１０８）。

なお、状態変数及び誤差共分散行列の初期値は、適当な値を設定し得るが、例えばＧＰＳやカメラ等で取得した自己位置推定の結果と慣性計測装置から取得した挙動データを組み合わせた、精度を上げた状態値とすることができる。

以上のように、本実施形態では、車両のばね上が６自由度、すなわち並進３自由度、回転３自由度のダイナミクスを有するものとして３次元の運動方程式で車両運動をモデル化して車両の３次元位置及び速度を算出しているので、平面運動を仮定した線形２輪モデルを用いて推定する場合の推定誤差や、例えば路面の起伏や高速道度のランプで大きな勾配やカント角が複合的に生じる等の外乱が存在する際の推定誤差の劣化を効果的に抑制することができる。

また、本実施形態では、３次元の運動方程式で車両運動をモデル化して車両の３次元位置及び速度を算出しているので、自己位置推定を担うカメラ等のセンサから情報が得られない状況、あるいはトンネル等でＧＰＳ信号が遮蔽されて得られない状況においても、自己位置を高精度に推定することが可能である。

以上、本発明の実施形態について説明したが、本発明はこれに限定されるものではなく、種々の変形が可能である。

例えば、本実施形態では、外乱として、路面変位、路面カント角、路面勾配、及び横風横力を示しているが、これらの全てを外乱として組み込むのではなく、これらのパラメータの少なくともいずれか、例えば路面カント角と路面勾配、あるいは路面カント角と路面勾配と横風横力を組み込んでもよい。

１０車両運動状態推定部、１１指令値入力部、１２車両運動モデル部、１３センサ値入力部、１４観測方程式部、１５差分器、１６カルマンゲイン計算部、１８フィードバック部。

Claims

車両に対する指令値を入力する指令値入力部と、
前記車両のばね上に設けられたセンサからのセンサ値を入力するセンサ値入力部と、
前記指令値を入力変数とし、３次元の車両運動モデルを用いて事前誤差推定値を算出し、算出した前記事前誤差推定値と観測方程式を用いて出力変数を算出し、前記出力変数と前記センサ値との差分を算出することで前記出力変数の誤差を算出し、さらに前記出力変数の誤差と前記事前誤差推定値とに基づいて前記車両の状態変数として速度及び姿勢角を推定し、推定された前記速度及び姿勢角から前記車両の３次元の位置を算出して出力する演算部と、
を備えることを特徴とする車両状態推定装置。
請求項１に記載の車両状態推定装置において、
前記指令値入力部は、操舵角指令値、及び４輪への制駆動力指令値を入力し、
前記センサ値入力部は、前記ばね上の３軸の加速度センサ、３軸の角速度センサ、及び前記４輪の回転角速度センサからのセンサ値を入力する
ことを特徴とする車両状態推定装置。
請求項１、２のいずれかに記載の車両状態推定装置において、
前記演算部は、さらに、前記３次元の車両運動モデルを用いて外乱を推定する
ことを特徴とする車両状態推定装置。
請求項３に記載の車両状態推定装置において、
前記演算部は、前記外乱として、路面変位、路面カント角、路面勾配、及び横風横力の少なくともいずれかを推定する
ことを特徴とする車両状態推定装置。
請求項１～４のいずれかに記載の車両状態推定装置において、
前記演算部は、拡張カルマンフィルタを用いて推定する
ことを特徴とする車両状態推定装置。