JP7036080B2 - 慣性航法装置 - Google Patents

Description

本発明は、慣性航法装置に係り、特に、自車両の移動量の蓄積に基づいて自車両の位置を推定する慣性航法装置に関する。

車両の姿勢角及び位置に係る情報は、運転支援技術及び予防安全技術にとって最も重要な情報である。例えば、車体のロール角及びピッチ角は、重力加速度成分を補償する制御入力（操舵角、アクセル及びブレーキ）を決定する際に活用でき、より安定した車両の制御が可能になる。さらに、位置推定は車両の位置制御には必要不可欠である。

特に現在の自動運転技術はＧＰＳ（Global Positioning System）又はＬＩＤＡＲ（Laser Imaging Detection and Ranging）等の車両以外の状況(外界状況)を検知する外界センサの情報が自車両の位置を推定する際に必須となっている。そして、かかる外界センサが機能しなくなった際のフェールセーフとして慣性航法（デッドレコニング）が重要な課題となっている。

デッドレコニングは、自車両の位置を直接検出するのではなく、走行時の車両の挙動を示す３軸の角速度（ピッチレート、ロールレート、ヨーレート）と３軸の加速度（前後加速度、横加速度、上下加速度）とが検出可能なＩＭＵ（Inertial Measurement Unit：慣性計測装置）、車速センサ及び操舵角センサ等の車両自身の状態を検知する内界センサで検出した自車両の移動の蓄積に基づいて自車両の位置を推定する慣性航法の方式である。

外界センサは雨や霧、夜間や逆光といった外界の環境によってその精度が大きく左右される。外界センサからの情報が劣化した場合は、自動運転を中止して運転の主導権を自動運転システムからドライバーに渡す。しかしながら、自動運転システムからドライバーに運転の主導権を渡すまでの間、内界センサのみを用いて自動運転を一定時間継続する事が求められる。

特許文献１には、ＧＰＳ、白線認識、周辺環境認識、道路形状及び地図情報に基づいて自車両の位置を推定する自己位置推定装置の発明が開示されている。

特許文献２には、ＧＰＳとＩＭＵとの出力差より、各々の誤差量を推定し、自車両の位置および自車両の方位の推定精度を向上させる慣性航法の方法の発明が開示されている。

特許文献３には、ＧＰＳ衛星から高精度なＧＰＳ情報が取得可能な時に車両運動モデルを高精度に推定する装置が示されている。

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

しかしながら、特許文献１に記載の発明は、多くの情報の入力を必要としており、入力される情報が一部でも劣化又は欠落すると自己位置推定の精度が低下し、自動運転において自車両を車線内に維持する制御が困難になるおそれがあった。

特許文献２に記載の発明は、ＩＭＵの誤差推定を高精度で行うには、高精度なＧＰＳ情報が必要だが、現在はそのようなサービスは少ないという問題があった。また、使用するＩＭＵは、航空機や船舶に用いられる高精度ＩＭＵが想定されており、ＭＥＭＳ（Micro Electro Mechanical Systems）によって量産されたＩＭＵでは同じアルゴリズムを用いても同等の性能が得られず、位置の推定値が不安定になるという問題があった。

特許文献３に記載の発明は、高精度なＧＰＳ情報が車両運動モデルの同定には必要で、高精度なＧＰＳ情報を取得するために、例えば３つのＧＰＳアンテナを搭載するとコストが嵩むという問題があった。さらにスリップ角を推定する際の基準点について明記されておらず、当該基準点の配置によっては横滑りモデルが破綻するおそれがあった。

本発明は、上記問題に鑑みてなされたものであり、外界センサからの情報が途絶した場合に、内界センサで検出した自車両の移動量の蓄積に基づいて自車両の位置を推定する慣性航法装置を実現することを目的とする。

上記目的を達成するために、請求項１に記載の慣性航法装置は、走行時の車両の挙動を示す角速度及び加速度を検出可能な慣性計測装置と、前記車両の前後速度を検出する車速検出部と、前記車両の操舵角を検出する操舵角検出部と、前記車両の上下速度及び横速度を０とみなし、前記慣性計測装置が検出した前記角速度と、前記慣性計測装置が検出した前記加速度と、前記車速検出部が検出した前後速度と、前記車両の方位角の初期値とに基づいて前記車両の姿勢角であるロール角、ピッチ角及び方位角を推定する姿勢角推定部と、前記車両の上下速度を０とみなし、前記姿勢角推定部が推定した姿勢角と、前記慣性計測装置が検出した前記角速度と、前記慣性計測装置が検出した前記加速度と、前記車速検出部が検出した前後速度と、前記操舵角検出部が検出した操舵角と、前記車両の位置の初期値とに基づいて前記車両の現在位置を推定する位置推定部と、を含んでいる。

また、請求項２に記載の慣性航法装置では、前記慣性計測装置は、ピッチレート、ロールレート及びヨーレートの３軸の角速度と、前後加速度、横加速度及び上下加速度の３軸の加速度を検出可能で、前記姿勢角推定部は、前記慣性計測装置が検出したピッチレート、ロールレート及びヨーレートの３軸の角速度と、前記慣性計測装置が検出した前後加速度及び横加速度の２軸の加速度とを用いて前記車両の姿勢角を推定し、前記位置推定部は、前記慣性計測装置が検出した１軸の角速度であるヨーレートと、前記慣性計測装置が検出した２軸の加速度である前後加速度及び横加速度と、姿勢角とを用いて前記車両の現在位置を推定する。

また、請求項３に記載の慣性航法装置では、前記位置推定部は、前記車両の旋回時の前後速度によらず前記車両の横速度が旋回外方向に生じる位置を基準位置とした横速度を推定する運動方程式を用いて、前記基準位置での加速度と共に前記車両の現在位置を推定する。

また、請求項４に記載の慣性航法装置では、前記基準位置は、前記車両の操舵機能を有しない車輪側の位置である。

また、請求項５に記載の慣性航法装置では、前記基準位置は、前記車両の操舵機能を有しない車輪の車軸の車幅方向の中心である。

また、請求項６に記載の慣性航法装置では、前記位置推定部は、姿勢角の情報を利用した車両運動モデルにより、路面のバンク角及び路面の前後勾配の影響を考慮して前記車両の横速度を推定する。

また、請求項７に記載の慣性航法装置では、前記姿勢角推定部は、前記慣性計測装置が検出した前記加速度と前記車両の運動との関係を表す式に基づいて、前回推定された、前記車両の姿勢角を含む前記車両の状態量から、次時刻の前記車両の状態量を予測する姿勢角推定用予測部と、予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、車両前後速度、前記車両の角速度、及び前記車両の加速度と、それぞれの実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記姿勢角推定用予測部で予測された前記車両の状態量を補正する姿勢角推定用更新部と、を含み、前記位置推定部は、基準位置とした、前記車両の旋回時の前後速度によらず前記車両の横速度が旋回外方向に生じる基準位置を含む、前記加速度と前記車両の運動との関係を表す式と、前記姿勢角推定部で推定された姿勢角とに基づいて、前回推定された、前記車両の位置及び速度、加速度、角速度を含む前記車両の状態量から、次時刻の前記車両の状態量を予測する位置推定用予測部と、予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、車両前後速度、前記車両の角速度、及び前記車両の加速度、前記車両の位置と、それぞれの実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記位置推定用予測部で予測された前記車両の状態量を補正する位置推定用更新部と、を含んでいる。

本発明によれば、外界センサからの情報が途絶した場合に、内界センサで検出した自車両の移動量の蓄積に基づいて自車両の位置を推定することができるという効果を奏する。

本発明の実施の形態に係る慣性航法装置の一例を示したブロック図である。 本発明の実施の形態における座標系を示した概略図である。 旋回中の車両の後輪車軸中心の軌跡を、コーナリングスティフネスの大小に対応させて示した説明図である。 本発明の実施の形態における変数の一例を示した説明図である。 （Ａ）は外界センサにより自己位置の計測が可能な場合の演算装置の入出力の一例を示した概略図であり、（Ｂ）は外界センサによる自己位置の計測が不可能な場合の演算装置の入出力の一例を示した概略図である。 本発明の実施の形態に係る姿勢角推定部の機能ブロック図の一例である。 事前推定部の機能ブロック図の一例である。 フィルタリング部の機能ブロック図の一例である。 本発明の実施の形態に係る位置推定部の機能ブロック図の一例である。 車両の重心における前後速度及び横速度、後輪車軸中心における前後速度及び横速度、前輪実舵角、重心から前輪車軸中心までの距離、重心から後輪車軸中心までの距離の関係を示した説明図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。図１に示すように、本実施の形態に係る慣性航法装置１０は、後述する演算装置１４の演算に必要なデータ及び演算装置１４による演算結果を記憶する記憶装置１８と、車両が備えたセンシング装置及びＧＰＳ等の外界センサによって取得した車両周辺の情報から車両の現在位置及び現在のヨー角（方位角）を算出する位置計測装置２０と、位置計測装置２０が算出した車両の現在位置及び現在のヨー角、車速センサ２４が検出した車両前後速度、ＩＭＵ２６が検出した車両の方位角の偏差及び加速度、並びに操舵角センサが検出した車両の操舵角が入力される入力装置１２と、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいて車両位置の推定の演算を行なうコンピュータ等で構成された演算装置１４と、演算装置１４で演算された車両の位置等を表示するＣＲＴ又はＬＣＤ等で構成された表示装置１６と、で構成されている。

本実施の形態に係る外界センサであるセンシング装置は、一例として、車載カメラ等の撮像装置、ＬＩＤＡＲ（Light Detection and Ranging）及びソナーのいずれかである。車載カメラ等の撮像装置を車載のセンシング装置とした場合は、一例として、当該撮像装置で取得した車両周辺の画像情報を解析して道路の白線等を検出する。ＬＩＤＡＲを車載のセンシング装置とした場合は、一例として、車両周辺に照射したパルス状のレーザの散乱光から道路の白線等を検出する。ソナーを車載のセンシング装置とした場合は、一例として、アスファルトの路面とペイントされた白線との、超音波の反射率の差を利用して、当該白線を識別する。

続いて、車両２００の挙動に係る座標系を図２に示したように定義する。地球座標系２０４は地球平面を基準として重力加速度方向とｚ_eとが平行で、ｙ_eが北方向を向いている座標系である。路面座標系２０６は、ｚ_rが車両２００の重心を通り路面に垂直な方向に向き、ｘ_rは車両進行方向に向いている座標系である。車体座標系２０８は車体バネ上に固定された座標系で、ｚ_vは車体鉛直上方向、ｘ_vは車体進行方向を向いている。従って、車両２００の前後方向は、車体座標系２０８のｘ軸に平行な方向となる。後述するように、本実施の形態では、車体座標系２０８の基準点を車両２００の重心ではなく、車両２００の後輪の車軸の車幅方向の中心とする。

また、オイラー姿勢角であるロール角φ、ピッチ角θ及びヨー角ψは、地球座標系２０４に対して、図２に示したように定義される。例えば、ロール角φはｘ軸まわりの回転角であり、ピッチ角θは、y軸まわりの回転角であり、ヨー角ψは、z軸まわりの回転角である。また、ロール角φ、ピッチ角θ及びヨー角ψの各々は、右ネジの方向（図２では、各々の矢印方向）の回転で正の値を示す。本実施の形態では、便宜上、後述するヨー角偏差は基準座標系を路面座標系２０６とし、さらに、本来は地球座標系２０４に対して定義されるオイラー姿勢角を、車体座標系２０８に対して、ロール角φ_v、ピッチ角θ_v及びヨー角ψ_vと定義する。以後、単に、ロール角φ、ピッチ角θ及びヨー角ψと記した場合は、基本的に、車体座標系２０８に対して定義された姿勢角であるとする。

図３は、旋回中の車両２００の後輪車軸中心２０２の軌跡を、コーナリングスティフネスＣpの大小に対応させて示したものである。車両２００の横滑りは、コーナリングスティフネスＣｐの値によって変化する。例えば、Ｃｐが無限大であると、車両２００は旋回時に横滑りを起こさない。しかしながら、旋回時に車両２００が横滑りをしないというのは現実にはあり得ない。図３に示したように、現実に横滑りを生じながら車両２００が旋回する場合のＣｐをＣｐ_realとすると、ＣｐがＣｐ_real小さくなると横滑りは大きくなり、ＣｐがＣｐ_realよりもさらに小さくなると横滑りは極めて大きくなる。

また、図３に示した横滑りモデルは、一例として、車両２００の後輪車軸の車幅方向の中心２０２（以下、「後輪車軸中心２０２」と略記）を車両２００の横滑り推定の基準点としている。

従来は、位置推定を行う際に必要になる横速度推定について、車両重心位置を基準とした車両運動モデルで横速度推定が数多く行われてきた。しかしながら、車両重心位置を基準にすると、旋回方向が同じでも横滑りの方向が車速（前後速度）によって変化してしまい、横滑りを考慮しない場合よりも位置推定が劣化してしまうおそれがあった。

本実施の形態では、横速度が必ず旋回外方向になる位置を基準とした車両運動モデルを利用する事によって、横滑りの左右方向を正しく推定できるようにした。かかる基準点の設定に加えて、横滑りの大きさを正しく推定することにより、車速によらず車両２００の位置を高精度で推定できるようになる。

横速度が必ず旋回外方向になる位置は、具体的には、後輪操舵機能を有しない車両２００の場合は図３に示したように後輪車軸である。従って、本実施の形態では、車両２００の後輪車軸中心２０２を横滑り推定の基準点とする。また、トラック等の通常よりも多い車輪を有する車両も同様に操舵機能を有しない車軸の車幅方向の中心を横滑り推定の基準点とする。また、Ｃpと車両２００の挙動については、後述する位置推定部５０の説明で改めて述べる。

図４は、本実施の形態における変数の一例を示した説明図である。本実施の形態では、車両２００の前後速度Ｕ、車両２００の横速度Ｖ及び車両２００の上下速度Ｗの各々を定義する。Ｕはｘ軸、Ｖはｙ軸及びＷはｚ軸に各々平行する。

また、車両２００のロール角φ、ピッチ角θ、ヨー角ψに対応するＩＭＵ２６の出力値は、角速度であるロールレートＰ、ピッチレートＱ、ヨーレートＲと定義する。

従来はＩＭＵ２６、ジャイロセンサ等の精度が不十分であったこともあり、ヨーレートＲの推定にも車両運動モデルを活用していた。しかし近年、安価なＩＭＵ２６に用いられるＭＥＭＳジャイロの精度が向上していると共に、車速センサ及び操舵角センサ等の複数センサが車両２００に搭載されることで特にヨーレートＲについては補正が容易になった。

本実施の形態では、車両運動モデルのヨーレートは使用せず、ＩＭＵ２６が検出したヨーレートＲの値を後述するように補正して使用する。また、ＩＭＵ２６が検出したロールレートＰ及びピッチレートＱは、車体座標系２０８を必ずしも基準としていないので、後述するように補正して使用する。

図５（Ａ）は外界センサにより自己位置の計測が可能な場合の演算装置１４の入出力の一例を示した概略図であり、図５（Ｂ）は外界センサによる自己位置の計測が不可能な場合の演算装置１４の入出力の一例を示した概略図である。

図５（Ａ）、（Ｂ）に示したように、演算装置１４は、センサ入力ｙ_aに基づいて車両２００の姿勢角を推定する姿勢角推定部４０と、センサ入力ｙ_pに基づいて車両２００の位置を推定する位置推定部５０とを含む。

図５（Ａ）に示したように、自己位置の計測が可能な場合は、姿勢角推定部４０には内界センサであるＩＭＥ２６からロールレートＰ_s、ピッチレートＱ_s、ヨーレートＲ_s、車両２００の進行方向（ｘ軸方向）の前後加速度Ａ_sx、車両２００の横方向（ｙ軸方向）の加速度である横加速度Ａ_syが入力される。なお、ＩＭＥ２６からの各入力値の添え字ｓは、Sensorの略である。

また、内界センサである車速センサ２４からは、車両２００の前後速度Ｕ_wが入力される。

姿勢角推定部４０には、外界センサから現在位置（ＬＯＣ位置）として車両２００の現在のヨー角ψ_locが入力される。ヨー角ψ_locは、姿勢角推定部４０において方位初期値としても用いられる。

姿勢角推定部４０は、内界センサ及び外界センサから入力されたセンサ入力ｙ_aに基づいて、車両２００の姿勢角ｕ_a（ｋ）を推定する。

位置推定部５０は、姿勢角推定部４０で推定した姿勢角ｕ_a（ｋ－１）と、センサ入力ｙ_pとに基づいて、車両２００の位置を推定する。

位置推定部５０には、センサ入力ｙ_pとして、内界センサであるＩＭＵ２６からヨーレートＲ_s、前後加速度Ａ_sx、横加速度Ａ_syが、内界センサである操舵角センサ２８から操舵角δが、内界センサである車速センサ２４から前後速度Ｕ_wが、外界センサからＬＯＣ位置としてＥＮ座標（Ｅ_loc、Ｎ_loc）が、各々入力される。ＥＮ座標は、地球座標系２０４において、基準となる地点から車両２００までの相対的な位置を示し、Ｅ_locは当該地点から経度方向の位置を、Ｎ_locは当該地点から緯度方向の位置を、各々メートルで表したものである。位置推定部５０は、以上の入力に基づいて、車両２００の位置を推定する。

図５（Ｂ）は、外界センサからの入力が途絶え、外界センサを用いた自己位置の計測が不可能な場合である。かかる場合に本実施の形態に係る慣性航法装置１０は、内界センサであるＩＭＥ２６、車速センサ２４及び操舵角センサ２８からの入力に基づいて、車両２００の姿勢角及び位置を推定する。

図６は、本実施の形態に係る姿勢角推定部４０の機能ブロック図の一例である。図６に示したように、姿勢角推定部４０は、後述する状態変数の前回値ｘ_a（ｋ－１）及び状態共分散の前回値Ｐｎ_a ^x（ｋ－１）から横滑りモデルに基づいて予測値（事前推定値）を推定する事前推定部４２と、事前推定部４２が出力した事前推定値を、センサ入力ｙ_aと後述する観測共分散行列Ｒｎ_aとを用いて補正して状態変数の次回値ｘ_a（ｋ）、状態共分散の次回値Ｐｎ_a ^x（ｋ）及び姿勢角ｕ_a（ｋ）を出力するフィルタリング部４４と、を含む。

なお、状態変数、状態共分散、観測共分散行列及び姿勢角の各々の添え字aは、Angleの略である。

続いて、姿勢角推定部４０における処理について説明する。本実施の形態では、下記の式（１）により、車両２００の姿勢角の変化率である姿勢角の角速度を定義する。

さらに上記式（１）及び重力加速度ｇとに基づくと、ＩＭＵ２６による各軸の加速度は下記の式で表される。なお、下記の式は、ＩＭＵ２６で検出したロールレートＰ、ピッチレートＱ、ヨーレートＲ、車速センサ２４で検出した前後速度Ｕ、横速度Ｖ及び上下速度Ｗの各変数を含む。

本実施の形態では、上下加速度Ａ_zは検討しない。また、車両２００の横速度Ｖ及び上下速度Ｗが十分に小さいと仮定すると、上記の式は、下記の式のように近似できる。

従来の技術で代表される慣性センサ（ＩＭＵ）を用いた推定手法では、ＩＭＵのジャイロセンサによる３軸の角速度（ロールレートＰ、ピッチレートＱ、ヨーレートＲ）とＩＭＵの加速度センサによる３軸の加速度（前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_z）とをＧＰＳで検出した位置情報を用いて補正しながら位置推定を行う形態が主であった。かかる自己位置推定手法では、慣性センサの出力誤差とＧＰＳの誤差を全て推定しながら位置推定を行うため、安価なＩＭＵを用いた場合に推定が安定せず位置情報が大きく劣化するおそれがある。

一般に、車両２００等の剛体は、前後速度、横速度、上下速度、ロール、ピッチ及びヨーの６自由度の運動を行う。しかしながら、本実施の形態では、車両２００は路面を走行しているため上下速度及び横速度は小さいとみなし、一例として、姿勢角推定は４自由度（上下速度＝横速度＝０と仮定）、後述する位置推定は５自由度（上下速度＝０と仮定）のように次元を縮退して各々の推定を行うことにより、慣性センサであるＩＭＵ２６が劣化した場合でも安定した姿勢角推定及び位置推定を行うようにする。

上記の式において、センサで直接計測できない未知数は車体座標系２０８に対する姿勢角であるロール角φ_v、ピッチ角θ_vのみとなる。

本実施の形態では、上述の式（１）に基づいて状態方程式ｆ（ｘ）を定義し、姿勢角推定部４０における事前推定とフィルタリングのステップとで活用する。事前推定及びフィルタリングのステップの詳細については、後述する。

本実施の形態では、状態変数ｘ_aを下記のように定義する。

そして、状態方程式を下記の式（２）のように定義する。

上記の状態量ｘ（ｔ）に対して、上記式（２）の関係が成り立つ状態方程式ｆ（ｘ）を下記のように立式する。

また、本実施の形態では、状態方程式の誤差共分散行列を下記のように定義する。

状態方程式に含まれるロールレートＰ_v、ピッチレートＱ_v、ヨーレートＲ_vは、次回値がランダムに出現するような白色ノイズによって駆動されるランダムウォークモデルとする。ロールレートＰ_v、ピッチレートＱ_v、ヨーレートＲ_vは、不規則に変化するシステムをモデル化するマルコフモデルを採用してもよい。

後述するカルマンフィルタでは、状態方程式ｆ_a（ｘ_a）に係る上記の微分方程式を単純積分又はルンゲクッタ法を用いて離散化して使用する。従って、関数ｆ_a（ｘ_a）の入出力関係を、時間ｔ＝ｋ、ｋ－１に対して、ｘ_a（ｋ）＝ｆ_a（ｘ_a（ｋ－１））となる形で使用する。

次にセンサによる観測変数（観測量）を下記のように定義する。

ｙ_a=ｈ_a（ｘ_a）を満たす観測方程式ｈ_a（ｘ_a）は、下記の各式のように、状態量（右辺）と観測量（左辺）との対応を定義したものである。

観測方程式の誤差共分散行列を下記のように定義する。

誤差共分散行列は観測方程式の誤差共分散行列であるので、観測方程式の不確かさとセンサのノイズを共に含んでいる。また、本実施の形態において外界センサが使用できない慣性航法中はψ_locが取得できなくなるので、観測変数を下記のように切り替える。

観測方程式については車体方位角であるヨー角ψに関する式を除いたものを用いればよい。なお、正常時と慣性航法時に状態変数の共分散行列の初期化を行う必要はなく、観測方程式を上述のｙ_aとｙ'_aとで切り替えるのみでよい。

以上により、カルマンフィルタを利用するのに必要な状態方程式、観測方程式、及び誤差共分散行列が定義された。

カルマンフィルタは線形・非線形を含め種々の手法が提案されている。本実施の形態では、前述のように状態方程式ｆ_a（ｘ_a）が非線形である事を踏まえ、非線形カルマンフィルタを活用する例を示す。中でも状態方程式ｆ_a（ｘ_a）についての線形化を要さず、かつ計算負荷が比較的小さいアンセンティッドカルマンフィルタを一例として採用する。状態方程式ｆ_a（ｘ_a）が線形の場合は、線形カルマンフィルタを用いてもよいし、アンセンティッドカルマンフィルタ以外の非線形カルマンフィルタを用いてもよい。

本実施の形態では、一例として、非線形モデルであるアンセンティッドカルマンフィルタを用いて車両２００の挙動を推定するので、後述する位置推定部５０において、路面傾斜のような非線形運動についても考慮することが可能となる。

従来は、路面傾斜（バンク角、前後勾配）のモデルの非線形性を解くのが難しかったため、横滑りを推定するのに路面傾斜を考慮していなかった。かかる従来モデルでは路面傾斜が大きい場所で推定結果が大きく劣化するおそれがあった。しかしながら、アンセンティッドカルマンフィルタを用いた本実施の形態では、路面傾斜を考慮して、車両２００の横滑りの挙動（横速度）を高精度で推定できる。

図７は、事前推定部４２の機能ブロック図の一例である。図７に示したように、事前推定部４２は、第１アンセンティッド変換部４２Ａと第２アンセンティッド変換部４２Ｂとを含む。

第１アンセンティッド変換部４２Ａは、状態方程式ｆ_a（ｘ_a）に基づいて状態量を更新する1回目のアンセンティッド変換(Unscented transfer)を行って、ｘ_aの平均及びｘ_aの共分散行列を出力する。

第２アンセンティッド変換部４２Ｂは、第１アンセンティッド変換部４２Ａが出力した状態量ｘ_aの平均及び状態量ｘ_aの共分散行列を用いて、観測方程式ｈ_a（ｘ_a）に従って、対応する観測量に変換する２回目のアンセンティッド変換を行う。

なお、アンセンティッド変換の目的は、ある非線形関数ｙ＝ｆ（ｘ）による変換において、下記の観測量ｙの平均及び共分散行列を精度よく求めることにある。

従って、本実施の形態は、平均値と標準偏差とに対応する2n+1個のサンプル(シグマポイント）を用いて、確率密度関数を近似することを特徴とする。

図７に示したように、第１アンセンティッド変換部４２Ａは、シグマポイント重み係数部４２Ａ１と、関数変換部４２Ａ２と、Ｕ変換部４２Ａ３と、を含む。また、第２アンセンティッド変換部４２Ｂは、シグマポイント重み係数部４２Ｂ１と、関数変換部４２Ｂ２と、Ｕ変換部４２Ｂ３と、を含む。図６に示したように、姿勢角推定部４０における状態変数はｘ_a（ｋ）、状態共分散はＰn_a（ｋ）であるが、後述する位置推定部５０においても同様の処理を行うので、図７、図８及び以下の説明では、状態変数の添え字a及び状態共分散の添え字ｎ_aは省略する。

第１アンセンティッド変換部４２Ａのシグマポイント重み係数部４２Ａ１及び第２アンセンティッド変換部４２Ｂのシグマポイント重み係数部４２Ｂ１では、シグマポイントＸ_i：ｉ＝０、１、２、…２ｎが、下記のように選択される。なお、Ｐ_xの平方根行列のｉ番目の列は、一例としてコレスキー分解によって算出される。

ただし、スケーリングファクタκは、κ≧０となるように選択する。また、シグマポイントに対する重みは下記のように定義する。

第１アンセンティッド変換部４２Ａの関数変換部４２Ａ２における、非線形関数ｆ（ｘ）による各シグマポイントの変換は下記のようになる。下記は、第１アンセンティッド変換部４２Ａの関数変換部４２Ａ２での状態方程式ｆ（ｘ）による変換だが、第２アンセンティッド変換部４２Ｂの関数変換部４２Ｂ２での観測方程式ｈ（ｘ）を用いた変換では、観測値が得られる。

第１アンセンティッド変換部４２ＡのＵ変換部４２Ａ３では、関数ｆ（ｘ）によって変換された値と、前述の重み係数とを用いて、下記のように状態量ｘの平均値と状態量ｘの共分散行列を算出する。なお、下記式中のＱ_nは誤差共分散行列である。

第２アンセンティッド変換部４２ＢのＵ変換部４２Ｂ３では、下記の計算を行う。

図８は、フィルタリング部４４の機能ブロック図の一例である。フィルタリング部４４では、Ｕ変換部４２Ｂ３で計算された、状態量の事前予測値に対応する観測値と、実際に観測された観測値との差を比較し、状態量の予測値を補正する処理を行う。

状態量の予測値を実際に観測された値でフィードバックする処理はカルマンゲイン(Kalman Gain)と呼ばれ、次式で計算される。なお、次式のＲ_nは観測ノイズである。

次に、このカルマンゲインを用いて、状態量の事前予測値を補正する処理を次のように行う。

以上の事前推定部４２及びフィルタリング部４４の処理を各タイムステップごとに繰り返すことにより、姿勢角ｕ_a（ｋ）を推定する。

続いて位置推定部５０における処理について説明する。図９は、本実施の形態に係る位置推定部５０の機能ブロック図の一例である。図９に示したように、位置推定部５０は、後述する状態変数の前回値ｘ_p（ｋ－１）及び状態共分散の前回値Ｐｎ_p ^x（ｋ－１）から横滑りモデルに基づいて予測値（事前推定値）を推定する事前推定部５２と、事前推定部５２が出力した事前推定値を、センサ入力ｙ_pと後述する観測共分散行列Ｒｎ_pとを用いて補正して状態変数の次回値ｘ_p（ｋ）、状態共分散の次回値Ｐｎ_p ^x（ｋ）及び推定位置ｕ_p（ｋ）を出力するフィルタリング部５４と、を含む。

なお、状態変数、状態共分散、観測共分散行列及び姿勢角の各々の添え字ｐは、Placeの略である。

位置推定部５０では、車体座標系２０８における後輪車軸中心２０２の位置ｕ_p=(Ｅ_v、Ｎ_v)を推定する事を目的とする。ＩＭＵ２６で検出したヨーレートＲ_s、前後加速度Ａ_sx、横加速度Ａ_sy、車速センサで検出したＵ_w、ＥＮ座標(Ｅ_loc、Ｎ_loc)に加え、操舵角δ_s及び姿勢角推定値ｕ_a=(φ_v、 θ_v、ψ_v)を入力して位置推定を行う。

一般に車両２００の横速度Ｖはとても小さいため、ＭＥＭＳによって量産された誤差が大きなＩＭＵのみでの推定は難しい。そこで本実施の形態では、操舵角δ_wと車速Ｕ_wとを入力値とした車両運動モデルを用いる事によって車両２００の横速度Ｖ_vを推定する。

しかし、車両運動モデルの精度はタイヤパラメータに大きく依存するため、その精度が課題となっている。特に従来の車両２００の重心を基準にした横速度Ｖ及びヨーレートＲの推定手法では、車速Ｕ_wに応じて横滑りの方向が旋回内向きと外向きとで変化する。さらに車両２００の重心は積載や人員等で変化する事が想定される。従って、車両２００の重心に基づく推定では、横滑りの推定方向が逆になり、運動モデルによって位置推定を悪化させる場合が考えられる。

本実施の形態では、前述のように、後輪車軸中心２０２を車体座標系２０８の基準とし、車速によらず必ず旋回外方向に横速度Ｖが発生する車両運動モデルを想定した。

前述のように後輪操舵を行わない車両２００については、後輪車軸中心２０２は車両運動の特性から必ず旋回の外側に横速度Ｖ_vを有する。従って注目すべきはモデルパラメータによる横滑りの大小関係のみになる。従って、前述のように、位置推定では上下速度Ｗ_v＝０と仮定する。

図３に示したように、横滑りを考慮しない場合はロボットの運動等でよく利用される非ホロノミック運動の事であり、真値に対して必ず旋回内側に位置が推定される。そこからＣpを小さくしていき真値Ｃp_realに近づけていくと、位置推定も真値に近づいていく。さらにＣpを小さくしていくと真値から離れていく。以上から、Ｃpを実験等で求めた同定値に対して大きく設定しておくことで、必ず従来の非ホロノミック運動の位置と真の位置との間の推定が可能になる。かかる推定では、路面環境又はタイヤの履き替えによってタイヤパラメータが変化した場合の推定誤差見積もりも容易になる。

以上を踏まえて、座標原点を後輪車軸中心２０２とした２輪モデルによる横速度Ｖの方程式を導出する。

路面姿勢角のロール角をφ_r、ピッチ角をθ_rとすると、横方向の運動方程式は、次式によって表される。

上記の中のＦ_yはy軸方向に働く力であり、下記の式に示したように、前輪タイヤ横力Ｆ_fと後輪タイヤ横力Ｆ_rとの和で表される。

そして、前輪タイヤ横力Ｆ_f及び後輪タイヤ横力Ｆ_rは次式で表される。

上記式に関連して、図１０に示した車両２００の重心ＣＧ周りの速度をv_c=(Ｕ_c、Ｖ_c、Ｗ_c)、ヨーレートをＲ_c、前輪実舵角をδ_w、重心ＣＧから前輪車軸中心２１０までの距離をｌ_f、重心ＣＧから後輪車軸中心２０２までの距離をｌ_r、前輪、後輪のコーナリングスティフネスを各々Ｋ_f、Ｋ_r、車両質量をｍとした。

上記運動方程式のＦ_yに前輪タイヤ横力Ｆ_f及び後輪タイヤ横力Ｆ_rを代入して整理する。

その結果、横速度の運動方程式は下記のようになる。

上記式の最後の重力加速度gに関する項は、路面姿勢角による重力の影響を表す項であり、横速度推定を行う上で重要な項である。この項を省略してしまうと特にバンクが大きいような走路で位置推定精度が大きく劣化する。上記方程式を後輪車軸中心に変数変換をする上で３つの位置ベクトルを考える。地球座標系の原点から後輪車軸中心へのベクトルｒ_ew、地球座標系の原点から車両重心ＣＧへのベクトルｒ_ec、車両重心ＣＧから後輪車軸中心へのベクトルｒ_cwを考えると、次式のような関係が成立する。

上記式を時間微分すると、重心における回転角速度ω_cと後軸中心での回転角速度ω_vは同じであることから、dr/dt=δr/δt + ω × ｒの関係を用いて下記のように整理される。

図１０に示したように、車両２００の後輪車軸中心２０２の前後速度をＵ_v、横速度をＶ_vとすると、以上の式より、Ｕ_v＝Ｕ_c及びＶ_v＝Ｖ_c -ｌ_rＲ_vとなる。これを横方向の運動方程式に用いると次式が誘導される。

上記の航法の方程式を用いて、以下、状態方程式ｆ_p（ｘ_p）を立式する。

本実施の形態では、状態変数ｘ_pを下記のように定義する。

そして、姿勢角推定部４０から入力される姿勢角入力値ｕ_aを下記のように定義する。

上記の状態量に対して、以下のように状態方程式を立式する。

また、本実施の形態では、状態量のシステムノイズを下記のように定義する。

前後速度Ｕ_vの微分値、横速度Ｖ_vの微分値及び操舵角δ_wは、次回値がランダムに出現するような白色ノイズによって駆動されるランダムウォークモデルとする。前後速度Ｕ_vの微分値、横速度Ｖ_vの微分値及び操舵角δ_wに1次のマルコフモデルを採用してもよい。

本実施の形態は、実舵角も推定対象としている点が特徴で、実舵角は必ずしも操舵角δ_wに対して一定比率で求まらず、コンプライアンス等で誤差が必ず生じている。この誤差は白色ノイズになっているとは限らないが、ある分散をもった値になっていると仮定する事で、２輪モデルによるモデル化誤差を状態方程式の分散としてカルマンフィルタの中に取り入れて扱う事が可能となる。

次に観測行列を下記のように定義する。

上記観測変数に対して、観測方程式ｈ_p（ｘ_p）を下記のように定義する。

上記式中のε_swは実舵角と操舵角との比であり、ここでは定常値を仮定しているが、マップ化したものを適用する事も可能である。また、dV_v/dtについては状態方程式と同様の式を利用するので省略した。この値は比較的小さく、加速度センサのノイズや誤差に隠れてしまうため、観測方程式から除いても位置推定精度に対する影響が小さい。

観測方程式の誤差共分散行列を下記のように定義する。

外界センサが使用できない慣性航法中はＥＮ座標が取得できなくなるので、観測変数を次のように切り替える。

観測方程式についてはＥＮ座標に係るＥ_loc及びＮ_locを各々示す式を除いたものを用いればよい。状態変数の値と共分散行列について特に初期化等をする必要はなく、観測方程式の切り替えで正常時と慣性航法とを組み合わせればよい。

前述のように、位置推定部５０は、状態変数の前回値ｘ_p（ｋ－１）及び状態共分散の前回値Ｐｎ_p ^x（ｋ－１）から横滑りモデルに基づいて予測値（事前推定値）を推定する事前推定部５２と、事前推定部５２が出力した事前推定値を、センサ入力ｙ_pと後述する観測共分散行列Ｒｎ_pとを用いて補正して状態変数の次回値ｘ_p（ｋ）、状態共分散の次回値Ｐｎ_p ^x（ｋ）及び推定位置ｕ_p（ｋ）を出力するフィルタリング部５４と、を含む。

位置推定部５０の事前推定部５２は、姿勢角推定部４０の事前推定部４２と同様に、図７に示したような第１アンセンティッド変換部と第２アンセンティッド変換部とを含む。

位置推定部５０の第１アンセンティッド変換部は、状態方程式ｆ_p（ｘ_p）に基づいて状態量を更新する1回目のアンセンティッド変換を行って、ｘ_pの平均及びｘ_pの共分散行列を出力する。

位置推定部５０の第２アンセンティッド変換部は、第１アンセンティッド変換部が出力した状態量ｘ_pの平均及び状態量ｘ_pの共分散行列を用いて、観測方程式ｈ_p（ｘ）_pに従って、対応する観測量に変換する２回目のアンセンティッド変換を行う。

そして、位置推定部５０のフィルタリング部は、姿勢角推定部４０のフィルタリング部４４と同様に、状態量の事前予測値に対応する観測値と、実際に観測された観測値との差を比較し、状態量の予測値を補正する処理を行う。

以下、姿勢角推定部４０と同様に、事前推定部５２及びフィルタリング部５４の処理を各タイムステップごとに繰り返すことにより、車両２００の前後速度Ｕ_v、横速度Ｖ_v及びヨーレートＲ_vの各々を推定する。そして、当該推定の直前の車両２００の位置（ＥＮ座標）を車両２００の位置の初期値とし、当該初期値と、姿勢角推定部４０で推定した車両２００の姿勢角（ピッチ角θ_v、ロール角φ_v及びヨー角ψ_v）と、推定した前後速度Ｕ_v、横速度Ｖ_v及びヨーレートＲ_vとに基づいて、車両２００の現在位置を推定する。

以上説明したように、本実施の形態に係る慣性航法装置１０によれば、ＩＭＵ２６及び車速センサ２４の各々の出力結果を用いて、車両２００の姿勢角（ピッチ角θ_v、ロール角φ_v、ヨー角ψ_v）を推定すると共に、推定した姿勢角と、ＩＭＵ２６、車速センサ２４及び操舵角センサ２８の各々の出力結果とを用いて、車両２００の位置を推定する。ＩＭＵ２６、車速センサ２４及び操舵角センサ２８は、車両２００自身の状態を検知する内界センサである。

従って、本実施の形態に係る慣性航法装置１０によれば、外界状況を検知するＧＰＳ等の外界センサからの情報が途絶した場合に、内界センサで検出した自車両の移動量の蓄積に基づいて自車両の位置を推定することができる。

また、前述のように本実施の形態に係る慣性航法装置１０は、路面を走行している車両２００は上下速度Ｗ_v及び横速度Ｖ_vが小さいという特徴を利用して、姿勢角推定では４自由度（上下速度Ｗ_v＝横速度Ｖ_v＝０）と仮定し、位置推定では５自由度（上下速度Ｗ_v＝０）と仮定して、次元を縮退することにより、慣性センサであるＩＭＵ２６が高精度でなくても、又はＩＭＵ２６の精度が劣化しても、車両２００の姿勢角と車両２００の位置とを安定して推定することができる。

また、本実施の形態に係る慣性航法装置１０では、横滑りが必ず旋回外方向になる点を横滑りモデルの基準点にしたことにより、車両重心位置ＣＧを基準とした車両運動モデルのように、旋回方向が同じでも横滑りの方向が車速によって変化する現象は生じ得ない。その結果、本実施の形態に係る慣性航法装置１０では、車両２００の横滑りの大きさを高精度で推定することに伴い、車両２００の位置を車速によらず高精度で推定することができる。

また、本実施の形態に係る慣性航法装置１０は、非線形モデルであるアンセンティッドカルマンフィルタを用いて車両２００の挙動を推定するので、路面傾斜のような非線形運動についても考慮することが可能となり、その結果、路面傾斜を考慮した車両２００の横滑りの挙動を高精度で推定することができる。

１０ 慣性航法装置
１２ 入力装置
１４ 演算装置
１６ 表示装置
１８ 記憶装置
２０ 位置計測装置
２４ 車速センサ
２８ 操舵角センサ
４０ 姿勢角推定部
４２ 事前推定部
４４ フィルタリング部
５０ 位置推定部
５２ 事前推定部
５４ フィルタリング部
２００ 車両
２０２ 後輪車軸中心
２１０ 前輪車軸中心

Claims (7)

1. 走行時の車両の挙動を示す角速度及び加速度を検出可能な慣性計測装置と、
   前記車両の前後速度を検出する車速検出部と、
   前記車両の操舵角を検出する操舵角検出部と、
   前記車両の上下速度及び横速度を０とみなし、前記慣性計測装置が検出した前記角速度と、前記慣性計測装置が検出した前記加速度と、前記車速検出部が検出した前後速度と、前記車両の方位角の初期値とに基づいて前記車両の姿勢角であるロール角、ピッチ角及び方位角を推定する姿勢角推定部と、
   前記車両の上下速度を０とみなし、前記姿勢角推定部が推定した姿勢角と、前記慣性計測装置が検出した前記角速度と、前記慣性計測装置が検出した前記加速度と、前記車速検出部が検出した前後速度と、前記操舵角検出部が検出した操舵角と、前記車両の位置の初期値とに基づいて前記車両の現在位置を推定する位置推定部と、
   を含む慣性航法装置。
2. 前記慣性計測装置は、ピッチレート、ロールレート及びヨーレートの３軸の角速度と、前後加速度、横加速度及び上下加速度の３軸の加速度を検出可能で、
   前記姿勢角推定部は、前記慣性計測装置が検出したピッチレート、ロールレート及びヨーレートの３軸の角速度と、前記慣性計測装置が検出した前後加速度及び横加速度の２軸の加速度とを用いて前記車両の姿勢角を推定し、
   前記位置推定部は、前記慣性計測装置が検出した１軸の角速度であるヨーレートと、前記慣性計測装置が検出した２軸の加速度である前後加速度及び横加速度と、姿勢角とを用いて前記車両の現在位置を推定する請求項１に記載の慣性航法装置。
3. 前記位置推定部は、前記車両の旋回時の前後速度によらず前記車両の横速度が旋回外方向に生じる位置を基準位置とした横速度を推定する運動方程式を用いて、前記基準位置での加速度と共に前記車両の現在位置を推定する請求項１に記載の慣性航法装置。
4. 前記基準位置は、前記車両の操舵機能を有しない車輪側の位置である請求項３記載の慣性航法装置。
5. 前記基準位置は、前記車両の操舵機能を有しない車輪の車軸の車幅方向の中心である請求項４に記載の慣性航法装置。
6. 前記位置推定部は、姿勢角の情報を利用した車両運動モデルにより、路面のバンク角及び路面の前後勾配の影響を考慮して前記車両の横速度を推定する請求項１～５のいずれか１項に記載の慣性航法装置。
7. 前記姿勢角推定部は、
   前記慣性計測装置が検出した前記加速度と前記車両の運動との関係を表す式に基づいて、前回推定された、前記車両の姿勢角を含む前記車両の状態量から、次時刻の前記車両の状態量を予測する姿勢角推定用予測部と、
   予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、車両前後速度、前記車両の角速度、及び前記車両の加速度と、それぞれの実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記姿勢角推定用予測部で予測された前記車両の状態量を補正する姿勢角推定用更新部と、
   を含み、
   前記位置推定部は、
   基準位置とした、前記車両の旋回時の前後速度によらず前記車両の横速度が旋回外方向に生じる基準位置を含む、前記加速度と前記車両の運動との関係を表す式と、前記姿勢角推定部で推定された姿勢角とに基づいて、前回推定された、前記車両の位置及び速度、加速度、角速度を含む前記車両の状態量から、次時刻の前記車両の状態量を予測する位置推定用予測部と、
   予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、車両前後速度、前記車両の角速度、及び前記車両の加速度、前記車両の位置と、それぞれの実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記位置推定用予測部で予測された前記車両の状態量を補正する位置推定用更新部と、
   を含む請求項３～５のいずれか１項に記載の慣性航法装置。

Images