(57)【要約】【課題】外界センサからの情報が途絶した場合に、内界センサで検出した自車両の移動量の蓄積に基づいて自車両の位置を推定する。【解決手段】車両の上下速度及び横速度を０とみなし、ＩＭＵ２６が検出したピッチレート、ロールレート、ヨーレート、前後加速度及び横加速度と、車速センサ２４が検出した車両の前後速度と、車両の方位角の初期値とに基づいて車両の姿勢角であるロール角、ピッチ角及び方位角を推定する姿勢角推定部４０と、前記車両の上下速度を０とみなし、姿勢角推定部４０が推定した姿勢角と、ＩＭＵ２６が検出したヨーレート、前後加速度及び横加速度と、車速センサ２４が検出した車両の前後速度と、操舵角センサ２８が検出した操舵角と、車両の位置の初期値と、に基づいて車両の現在位置を推定する位置推定部５０と、を含んでいる。【選択図】図１
2020169872.tif 000002
【特許請求の範囲】
【請求項１】
走行時の車両の挙動を示す角速度及び加速度を検出可能な慣性計測装置と、
前記車両の前後速度を検出する車速検出部と、
前記車両の操舵角を検出する操舵角検出部と、
前記車両の上下速度及び横速度を０とみなし、前記慣性計測装置が検出した前記角速度と、前記慣性計測装置が検出した前記加速度と、前記車速検出部が検出した前後速度と、前記車両の方位角の初期値とに基づいて前記車両の姿勢角であるロール角、ピッチ角及び方位角を推定する姿勢角推定部と、
前記車両の上下速度を０とみなし、前記姿勢角推定部が推定した姿勢角と、前記慣性計測装置が検出した前記角速度と、前記慣性計測装置が検出した前記加速度と、前記車速検出部が検出した前後速度と、前記操舵角検出部が検出した操舵角と、前記車両の位置の初期値とに基づいて前記車両の現在位置を推定する位置推定部と、
を含む慣性航法装置。
【請求項２】
前記慣性計測装置は、ピッチレート、ロールレート及びヨーレートの３軸の角速度と、前後加速度、横加速度及び上下加速度の３軸の加速度を検出可能で、
前記姿勢角推定部は、前記慣性計測装置が検出したピッチレート、ロールレート及びヨーレートの３軸の角速度と、前記慣性計測装置が検出した前後加速度及び横加速度の２軸の加速度とを用いて前記車両の姿勢角を推定し、
前記位置推定部は、前記慣性計測装置が検出した１軸の角速度であるヨーレートと、前記慣性計測装置が検出した２軸の加速度である前後加速度及び横加速度と、姿勢角とを用いて前記車両の現在位置を推定する請求項１に記載の慣性航法装置。
【請求項３】
前記位置推定部は、前記車両の旋回時の前後速度によらず前記車両の横速度が旋回外方向に生じる位置を基準位置とした横速度の運動方程式を用いて、前記基準位置での加速度と共に前記車両の現在位置を推定する請求項１に記載の慣性航法装置。
【請求項４】
前記基準位置は、前記車両の操舵機能を有しない車輪側の位置である請求項３記載の慣性航法装置。
【請求項５】
前記基準位置は、前記車両の操舵機能を有しない車輪の車軸の車幅方向の中心である請求項４に記載の慣性航法装置。
【請求項６】
前記位置推定部は、姿勢角の情報を利用した車両運動モデルにより、路面のバンク角及び路面の前後勾配の影響を考慮して前記車両の横速度を推定する請求項１〜５のいずれか１項に記載の慣性航法装置。
【請求項７】
前記姿勢角推定部は、
前記慣性計測装置が検出した前記加速度と前記車両の運動との関係を表す式に基づいて、前回推定された、前記車両の姿勢角を含む前記車両の状態量から、次時刻の前記車両の状態量を予測する姿勢角推定用予測部と、
予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、車両前後速度、前記車両の角速度、及び前記車両の加速度と、それぞれの実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記姿勢角推定用予測部で予測された前記車両の状態量を補正する姿勢角推定用更新部と、
を含み、
前記位置推定部は、
基準位置とした、前記車両の旋回時の前後速度によらず前記車両の横速度が旋回外方向に生じる基準位置を含む、前記加速度と前記車両の運動との関係を表す式と、前記姿勢角推定部で推定された姿勢角とに基づいて、前回推定された、前記車両の位置及び速度、加速度、角速度を含む前記車両の状態量から、次時刻の前記車両の状態量を予測する位置推定用予測部と、
予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、車両前後速度、前記車両の角速度、及び前記車両の加速度、前記車両の位置と、それぞれの実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記位置推定用予測部で予測された前記車両の状態量を補正する位置推定用更新部と、
を含む請求項３〜５のいずれか１項に記載の慣性航法装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、慣性航法装置に係り、特に、自車両の移動量の蓄積に基づいて自車両の位置を推定する慣性航法装置に関する。
【背景技術】
【０００２】
車両の姿勢角及び位置に係る情報は、運転支援技術及び予防安全技術にとって最も重要な情報である。例えば、車体のロール角及びピッチ角は、重力加速度成分を補償する制御入力（操舵角、アクセル及びブレーキ）を決定する際に活用でき、より安定した車両の制御が可能になる。さらに、位置推定は車両の位置制御には必要不可欠である。
【０００３】
特に現在の自動運転技術はＧＰＳ（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（ｋ−１）及び状態共分散の前回値Ｐｎax（ｋ−１）から横滑りモデルに基づいて予測値（事前推定値）を推定する事前推定部４２と、事前推定部４２が出力した事前推定値を、センサ入力ｙaと後述する観測共分散行列Ｒｎaとを用いて補正して状態変数の次回値ｘa（ｋ）、状態共分散の次回値Ｐｎax（ｋ）及び姿勢角ｕa（ｋ）を出力するフィルタリング部４４と、を含む。
【００４６】
なお、状態変数、状態共分散、観測共分散行列及び姿勢角の各々の添え字aは、Angleの略である。
【００４７】
続いて、姿勢角推定部４０における処理について説明する。本実施の形態では、下記の式（１）により、車両２００の姿勢角の変化率である姿勢角の角速度を定義する。
2020169872.tif 000003
【００４８】
さらに上記式（１）及び重力加速度ｇとに基づくと、ＩＭＵ２６による各軸の加速度は下記の式で表される。なお、下記の式は、ＩＭＵ２６で検出したロールレートＰ、ピッチレートＱ、ヨーレートＲ、車速センサ２４で検出した前後速度Ｕ、横速度Ｖ及び上下速度Ｗの各変数を含む。
2020169872.tif 000004
【００４９】
本実施の形態では、上下加速度Ａzは検討しない。また、車両２００の横速度Ｖ及び上下速度Ｗが十分に小さいと仮定すると、上記の式は、下記の式のように近似できる。
【００５０】
従来の技術で代表される慣性センサ（ＩＭＵ）を用いた推定手法では、ＩＭＵのジャイロセンサによる３軸の角速度（ロールレートＰ、ピッチレートＱ、ヨーレートＲ）とＩＭＵの加速度センサによる３軸の加速度（前後加速度Ａx、横加速度Ａy、上下加速度Ａz）とをＧＰＳで検出した位置情報を用いて補正しながら位置推定を行う形態が主であった。かかる自己位置推定手法では、慣性センサの出力誤差とＧＰＳの誤差を全て推定しながら位置推定を行うため、安価なＩＭＵを用いた場合に推定が安定せず位置情報が大きく劣化するおそれがある。
【００５１】
一般に、車両２００等の剛体は、前後速度、横速度、上下速度、ロール、ピッチ及びヨーの６自由度の運動を行う。しかしながら、本実施の形態では、車両２００は路面を走行しているため上下速度及び横速度は小さいとみなし、一例として、姿勢角推定は４自由度（上下速度＝横速度＝０と仮定）、後述する位置推定は５自由度（上下速度＝０と仮定）のように次元を縮退して各々の推定を行うことにより、慣性センサであるＩＭＵ２６が劣化した場合でも安定した姿勢角推定及び位置推定を行うようにする。
2020169872.tif 000005
【００５２】
上記の式において、センサで直接計測できない未知数は車体座標系２０８に対する姿勢角であるロール角φv、ピッチ角θvのみとなる。
【００５３】
本実施の形態では、上述の式（１）に基づいて状態方程式ｆ（ｘ）を定義し、姿勢角推定部４０における事前推定とフィルタリングのステップとで活用する。事前推定及びフィルタリングのステップの詳細については、後述する。
【００５４】
本実施の形態では、状態変数ｘaを下記のように定義する。
2020169872.tif 000006
【００５５】
そして、状態方程式を下記の式（２）のように定義する。
2020169872.tif 000007
【００５６】
上記の状態量ｘ（ｔ）に対して、上記式（２）の関係が成り立つ状態方程式ｆ（ｘ）を下記のように立式する。
2020169872.tif 000008
【００５７】
また、本実施の形態では、状態方程式の誤差共分散行列を下記のように定義する。
2020169872.tif 000009
【００５８】
状態方程式に含まれるロールレートＰv、ピッチレートＱv、ヨーレートＲvは、次回値がランダムに出現するような白色ノイズによって駆動されるランダムウォークモデルとする。ロールレートＰv、ピッチレートＱv、ヨーレートＲvは、不規則に変化するシステムをモデル化するマルコフモデルを採用してもよい。
【００５９】
後述するカルマンフィルタでは、状態方程式ｆa（ｘa）に係る上記の微分方程式を単純積分又はルンゲクッタ法を用いて離散化して使用する。従って、関数ｆa（ｘa）の入出力関係を、時間ｔ＝ｋ、ｋ−１に対して、ｘa（ｋ）＝ｆa（ｘa（ｋ−１））となる形で使用する。
【００６０】
次にセンサによる観測変数（観測量）を下記のように定義する。
2020169872.tif 000010
【００６１】
ｙa=ｈa（ｘa）を満たす観測方程式ｈa（ｘa）は、下記の各式のように、状態量（右辺）と観測量（左辺）との対応を定義したものである。
2020169872.tif 000011
【００６２】
観測方程式の誤差共分散行列を下記のように定義する。
2020169872.tif 000012
【００６３】
誤差共分散行列は観測方程式の誤差共分散行列であるので、観測方程式の不確かさとセンサのノイズを共に含んでいる。また、本実施の形態において外界センサが使用できない慣性航法中はψlocが取得できなくなるので、観測変数を下記のように切り替える。
2020169872.tif 000013
【００６４】
観測方程式については車体方位角であるヨー角ψに関する式を除いたものを用いればよい。なお、正常時と慣性航法時に状態変数の共分散行列の初期化を行う必要はなく、観測方程式を上述のｙaとｙ'aとで切り替えるのみでよい。
【００６５】
以上により、カルマンフィルタを利用するのに必要な状態方程式、観測方程式、及び誤差共分散行列が定義された。
【００６６】
カルマンフィルタは線形・非線形を含め種々の手法が提案されている。本実施の形態では、前述のように状態方程式ｆa（ｘa）が非線形である事を踏まえ、非線形カルマンフィルタを活用する例を示す。中でも状態方程式ｆa（ｘa）についての線形化を要さず、かつ計算負荷が比較的小さいアンセンティッドカルマンフィルタを一例として採用する。状態方程式ｆa（ｘa）が線形の場合は、線形カルマンフィルタを用いてもよいし、アンセンティッドカルマンフィルタ以外の非線形カルマンフィルタを用いてもよい。
【００６７】
本実施の形態では、一例として、非線形モデルであるアンセンティッドカルマンフィルタを用いて車両２００の挙動を推定するので、後述する位置推定部５０において、路面傾斜のような非線形運動についても考慮することが可能となる。
【００６８】
従来は、路面傾斜（バンク角、前後勾配）のモデルの非線形性を解くのが難しかったため、横滑りを推定するのに路面傾斜を考慮していなかった。かかる従来モデルでは路面傾斜が大きい場所で推定結果が大きく劣化するおそれがあった。しかしながら、アンセンティッドカルマンフィルタを用いた本実施の形態では、路面傾斜を考慮して、車両２００の横滑りの挙動（横速度）を高精度で推定できる。
【００６９】
図７は、事前推定部４２の機能ブロック図の一例である。図７に示したように、事前推定部４２は、第１アンセンティッド変換部４２Ａと第２アンセンティッド変換部４２Ｂとを含む。
【００７０】
第１アンセンティッド変換部４２Ａは、状態方程式ｆa（ｘa）に基づいて状態量を更新する1回目のアンセンティッド変換(Unscented transfer)を行って、ｘaの平均及びｘaの共分散行列を出力する。
【００７１】
第２アンセンティッド変換部４２Ｂは、第１アンセンティッド変換部４２Ａが出力した状態量ｘaの平均及び状態量ｘaの共分散行列を用いて、観測方程式ｈa（ｘa）に従って、対応する観測量に変換する２回目のアンセンティッド変換を行う。
【００７２】
なお、アンセンティッド変換の目的は、ある非線形関数ｙ＝ｆ（ｘ）による変換において、下記の観測量ｙの平均及び共分散行列を精度よく求めることにある。
2020169872.tif 000014
【００７３】
従って、本実施の形態は、平均値と標準偏差とに対応する2n+1個のサンプル(シグマポイント）を用いて、確率密度関数を近似することを特徴とする。
【００７４】
図７に示したように、第１アンセンティッド変換部４２Ａは、シグマポイント重み係数部４２Ａ１と、関数変換部４２Ａ２と、Ｕ変換部４２Ａ３と、を含む。また、第２アンセンティッド変換部４２Ｂは、シグマポイント重み係数部４２Ｂ１と、関数変換部４２Ｂ２と、Ｕ変換部４２Ｂ３と、を含む。図６に示したように、姿勢角推定部４０における状態変数はｘa（ｋ）、状態共分散はＰna（ｋ）であるが、後述する位置推定部５０においても同様の処理を行うので、図７、図８及び以下の説明では、状態変数の添え字a及び状態共分散の添え字ｎaは省略する。
【００７５】
第１アンセンティッド変換部４２Ａのシグマポイント重み係数部４２Ａ１及び第２アンセンティッド変換部４２Ｂのシグマポイント重み係数部４２Ｂ１では、シグマポイントＸi：ｉ＝０、１、２、…２ｎが、下記のように選択される。なお、Ｐxの平方根行列のｉ番目の列は、一例としてコレスキー分解によって算出される。
2020169872.tif 000015
【００７６】
ただし、スケーリングファクタκは、κ≧０となるように選択する。また、シグマポイントに対する重みは下記のように定義する。
2020169872.tif 000016
【００７７】
第１アンセンティッド変換部４２Ａの関数変換部４２Ａ２における、非線形関数ｆ（ｘ）による各シグマポイントの変換は下記のようになる。下記は、第１アンセンティッド変換部４２Ａの関数変換部４２Ａ２での状態方程式ｆ（ｘ）による変換だが、第２アンセンティッド変換部４２Ｂの関数変換部４２Ｂ２での観測方程式ｈ（ｘ）を用いた変換では、観測値が得られる。
2020169872.tif 000017
【００７８】
第１アンセンティッド変換部４２ＡのＵ変換部４２Ａ３では、関数ｆ（ｘ）によって変換された値と、前述の重み係数とを用いて、下記のように状態量ｘの平均値と状態量ｘの共分散行列を算出する。なお、下記式中のＱnは誤差共分散行列である。
2020169872.tif 000018
【００７９】
第２アンセンティッド変換部４２ＢのＵ変換部４２Ｂ３では、下記の計算を行う。
2020169872.tif 000019
【００８０】
図８は、フィルタリング部４４の機能ブロック図の一例である。フィルタリング部４４では、Ｕ変換部４２Ｂ３で計算された、状態量の事前予測値に対応する観測値と、実際に観測された観測値との差を比較し、状態量の予測値を補正する処理を行う。
【００８１】
状態量の予測値を実際に観測された値でフィードバックする処理はカルマンゲイン(Kalman Gain)と呼ばれ、次式で計算される。なお、次式のＲnは観測ノイズである。
2020169872.tif 000020
【００８２】
次に、このカルマンゲインを用いて、状態量の事前予測値を補正する処理を次のように行う。
2020169872.tif 000021
【００８３】
以上の事前推定部４２及びフィルタリング部４４の処理を各タイムステップごとに繰り返すことにより、姿勢角ｕa（ｋ）を推定する。
【００８４】
続いて位置推定部５０における処理について説明する。図９は、本実施の形態に係る位置推定部５０の機能ブロック図の一例である。図９に示したように、位置推定部５０は、後述する状態変数の前回値ｘp（ｋ−１）及び状態共分散の前回値Ｐｎpx（ｋ−１）から横滑りモデルに基づいて予測値（事前推定値）を推定する事前推定部５２と、事前推定部５２が出力した事前推定値を、センサ入力ｙpと後述する観測共分散行列Ｒｎpとを用いて補正して状態変数の次回値ｘp（ｋ）、状態共分散の次回値Ｐｎpx（ｋ）及び推定位置ｕp（ｋ）を出力するフィルタリング部５４と、を含む。
【００８５】
なお、状態変数、状態共分散、観測共分散行列及び姿勢角の各々の添え字ｐは、Placeの略である。
【００８６】
位置推定部５０では、車体座標系２０８における後輪車軸中心２０２の位置ｕp=(Ｅv、Ｎv)を推定する事を目的とする。ＩＭＵ２６で検出したヨーレートＲs、前後加速度Ａsx、横加速度Ａsy、車速センサで検出したＵw、ＥＮ座標(Ｅloc、Ｎloc)に加え、操舵角δs及び姿勢角推定値ｕa=(φv、 θv、ψv)を入力して位置推定を行う。
【００８７】
一般に車両２００の横速度Ｖはとても小さいため、ＭＥＭＳによって量産された誤差が大きなＩＭＵのみでの推定は難しい。そこで本実施の形態では、操舵角δwと車速Ｕwとを入力値とした車両運動モデルを用いる事によって車両２００の横速度Ｖvを推定する。
【００８８】
しかし、車両運動モデルの精度はタイヤパラメータに大きく依存するため、その精度が課題となっている。特に従来の車両２００の重心を基準にした横速度Ｖ及びヨーレートＲの推定手法では、車速Ｕwに応じて横滑りの方向が旋回内向きと外向きとで変化する。さらに車両２００の重心は積載や人員等で変化する事が想定される。従って、車両２００の重心に基づく推定では、横滑りの推定方向が逆になり、運動モデルによって位置推定を悪化させる場合が考えられる。
【００８９】
本実施の形態では、前述のように、後輪車軸中心２０２を車体座標系２０８の基準とし、車速によらず必ず旋回外方向に横速度Ｖが発生する車両運動モデルを想定した。
【００９０】
前述のように後輪操舵を行わない車両２００については、後輪車軸中心２０２は車両運動の特性から必ず旋回の外側に横速度Ｖvを有する。従って注目すべきはモデルパラメータによる横滑りの大小関係のみになる。従って、前述のように、位置推定では上下速度Ｗv＝０と仮定する。
【００９１】
図３に示したように、横滑りを考慮しない場合はロボットの運動等でよく利用される非ホロノミック運動の事であり、真値に対して必ず旋回内側に位置が推定される。そこからＣpを小さくしていき真値Ｃprealに近づけていくと、位置推定も真値に近づいていく。さらにＣpを小さくしていくと真値から離れていく。以上から、Ｃpを実験等で求めた同定値に対して大きく設定しておくことで、必ず従来の非ホロノミック運動の位置と真の位置との間の推定が可能になる。かかる推定では、路面環境又はタイヤの履き替えによってタイヤパラメータが変化した場合の推定誤差見積もりも容易になる。
【００９２】
以上を踏まえて、座標原点を後輪車軸中心２０２とした２輪モデルによる横速度Ｖの方程式を導出する。
【００９３】
路面姿勢角のロール角をφr、ピッチ角をθrとすると、横方向の運動方程式は、次式によって表される。
2020169872.tif 000022
【００９４】
上記の中のＦyはy軸方向に働く力であり、下記の式に示したように、前輪タイヤ横力Ｆfと後輪タイヤ横力Ｆrとの和で表される。
2020169872.tif 000023
【００９５】
そして、前輪タイヤ横力Ｆf及び後輪タイヤ横力Ｆrは次式で表される。
2020169872.tif 000024
【００９６】
上記式に関連して、図１０に示した車両２００の重心ＣＧ周りの速度をvc=(Ｕc、Ｖc、Ｗc)、ヨーレートをＲc、前輪実舵角をδw、重心ＣＧから前輪車軸中心２１０までの距離をｌf、重心ＣＧから後輪車軸中心２０２までの距離をｌr、前輪、後輪のコーナリングスティフネスを各々Ｋf、Ｋr、車両質量をｍとした。
【００９７】
上記運動方程式のＦyに前輪タイヤ横力Ｆf及び後輪タイヤ横力Ｆrを代入して整理する。
2020169872.tif 000025
【００９８】
その結果、横速度の運動方程式は下記のようになる。
2020169872.tif 000026
【００９９】
上記式の最後の重力加速度gに関する項は、路面姿勢角による重力の影響を表す項であり、横速度推定を行う上で重要な項である。この項を省略してしまうと特にバンクが大きいような走路で位置推定精度が大きく劣化する。上記方程式を後輪車軸中心に変数変換をする上で３つの位置ベクトルを考える。地球座標系の原点から後輪車軸中心へのベクトルｒew、地球座標系の原点から車両重心ＣＧへのベクトルｒec、車両重心ＣＧから後輪車軸中心へのベクトルｒcwを考えると、次式のような関係が成立する。
2020169872.tif 000027
【０１００】
上記式を時間微分すると、重心における回転角速度ωcと後軸中心での回転角速度ωvは同じであることから、dr/dt=δr/δt + ω × ｒの関係を用いて下記のように整理される。
2020169872.tif 000028
【０１０１】
図１０に示したように、車両２００の後輪車軸中心２０２の前後速度をＵv、横速度をＶvとすると、以上の式より、Ｕv＝Ｕc及びＶv＝Ｖc -ｌrＲvとなる。これを横方向の運動方程式に用いると次式が誘導される。
2020169872.tif 000029
【０１０２】
上記の航法の方程式を用いて、以下、状態方程式ｆp（ｘp）を立式する。
【０１０３】
本実施の形態では、状態変数ｘpを下記のように定義する。
2020169872.tif 000030
【０１０４】
そして、姿勢角推定部４０から入力される姿勢角入力値ｕaを下記のように定義する。
2020169872.tif 000031
【０１０５】
上記の状態量に対して、以下のように状態方程式を立式する。
2020169872.tif 000032
【０１０６】
また、本実施の形態では、状態量のシステムノイズを下記のように定義する。
2020169872.tif 000033
【０１０７】
前後速度Ｕvの微分値、横速度Ｖvの微分値及び操舵角δwは、次回値がランダムに出現するような白色ノイズによって駆動されるランダムウォークモデルとする。前後速度Ｕvの微分値、横速度Ｖvの微分値及び操舵角δwに1次のマルコフモデルを採用してもよい。
【０１０８】
本実施の形態は、実舵角も推定対象としている点が特徴で、実舵角は必ずしも操舵角δwに対して一定比率で求まらず、コンプライアンス等で誤差が必ず生じている。この誤差は白色ノイズになっているとは限らないが、ある分散をもった値になっていると仮定する事で、２輪モデルによるモデル化誤差を状態方程式の分散としてカルマンフィルタの中に取り入れて扱う事が可能となる。
【０１０９】
次に観測行列を下記のように定義する。
2020169872.tif 000034
【０１１０】
上記観測変数に対して、観測方程式ｈp（ｘp）を下記のように定義する。
2020169872.tif 000035
【０１１１】
上記式中のεswは実舵角と操舵角との比であり、ここでは定常値を仮定しているが、マップ化したものを適用する事も可能である。また、dVv/dtについては状態方程式と同様の式を利用するので省略した。この値は比較的小さく、加速度センサのノイズや誤差に隠れてしまうため、観測方程式から除いても位置推定精度に対する影響が小さい。
【０１１２】
観測方程式の誤差共分散行列を下記のように定義する。
2020169872.tif 000036
【０１１３】
外界センサが使用できない慣性航法中はＥＮ座標が取得できなくなるので、観測変数を次のように切り替える。
2020169872.tif 000037
【０１１４】
観測方程式についてはＥＮ座標に係るＥloc及びＮlocを各々示す式を除いたものを用いればよい。状態変数の値と共分散行列について特に初期化等をする必要はなく、観測方程式の切り替えで正常時と慣性航法とを組み合わせればよい。
【０１１５】
前述のように、位置推定部５０は、状態変数の前回値ｘp（ｋ−１）及び状態共分散の前回値Ｐｎpx（ｋ−１）から横滑りモデルに基づいて予測値（事前推定値）を推定する事前推定部５２と、事前推定部５２が出力した事前推定値を、センサ入力ｙpと後述する観測共分散行列Ｒｎpとを用いて補正して状態変数の次回値ｘp（ｋ）、状態共分散の次回値Ｐｎpx（ｋ）及び推定位置ｕ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＝０）と仮定して、次元を縮退することにより、慣性センサであるＩＭＵ２６が高精度でなくても、又はＩＭＵ２６の精度が劣化しても、車両２００の姿勢角と車両２００の位置とを安定して推定することができる。
【０１２４】
また、本実施の形態に係る慣性航法装置１０では、横滑りが必ず旋回外方向になる点を横滑りモデルの基準点にしたことにより、車両重心位置ＣＧを基準とした車両運動モデルのように、旋回方向が同じでも横滑りの方向が車速によって変化する現象は生じ得ない。その結果、本実施の形態に係る慣性航法装置１０では、車両２００の横滑りの大きさを高精度で推定することに伴い、車両２００の位置を車速によらず高精度で推定することができる。
【０１２５】
また、本実施の形態に係る慣性航法装置１０は、非線形モデルであるアンセンティッドカルマンフィルタを用いて車両２００の挙動を推定するので、路面傾斜のような非線形運動についても考慮することが可能となり、その結果、路面傾斜を考慮した車両２００の横滑りの挙動を高精度で推定することができる。
【符号の説明】
【０１２６】
１０   慣性航法装置
１２   入力装置
１４   演算装置
１６   表示装置
１８   記憶装置
２０   位置計測装置
２４   車速センサ
２８   操舵角センサ
４０   姿勢角推定部
４２   事前推定部
４４   フィルタリング部
５０   位置推定部
５２   事前推定部
５４   フィルタリング部
２００ 車両
２０２ 後輪車軸中心
２１０ 前輪車軸中心
【図１】
2020169872.tif 000038
【図２】
2020169872.tif 000039
【図３】
2020169872.tif 000040
【図４】
2020169872.tif 000041
【図５】
2020169872.tif 000042
【図６】
2020169872.tif 000043
【図７】
2020169872.tif 000044
【図８】
2020169872.tif 000045
【図９】
2020169872.tif 000046
【図１０】
2020169872.tif 000047