特許 7491339 位置推定装置、位置推定方法および位置推定プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-05-20

(45)【発行日】2024-05-28

(54)【発明の名称】位置推定装置、位置推定方法および位置推定プログラム

(51)【国際特許分類】

   G01C 21/28 20060101AFI20240521BHJP

   B60W 40/12 20120101ALI20240521BHJP

【ＦＩ】

G01C21/28

B60W40/12

【請求項の数】 16

(21)【出願番号】P 2022070968

(22)【出願日】2022-04-22

(65)【公開番号】P2023160540

(43)【公開日】2023-11-02

【審査請求日】2023-06-05

(73)【特許権者】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(74)【代理人】

【識別番号】110001519

【氏名又は名称】弁理士法人太陽国際特許事務所

(72)【発明者】

【氏名】森 大輝

(72)【発明者】

【氏名】亀川 雅仁

(72)【発明者】

【氏名】藤枝 延維

【審査官】宮本 礼子

(56)【参考文献】

【文献】特開２０１１－１２２９２１（ＪＰ，Ａ）

【文献】特開２００８－２３２８６７（ＪＰ，Ａ）

【文献】国際公開第２０１３／０４２２４５（ＷＯ，Ａ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０８Ｇ １／００－９９／００

Ｇ０１Ｃ ２１／００－２５／００

Ｂ６０Ｗ １０／００－１０／３０

Ｂ６０Ｗ ３０／００－６０／００

(57)【特許請求の範囲】

【請求項1】

加速度ベクトルおよび角加速度ベクトルから、位置ベクトル、速度ベクトルおよび姿勢角ベクトルを算出する第１演算部と、
状態空間モデルによって、観測量、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルから、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルの各要素に対する補正量を算出する第２演算部とを備え、
前記第１演算部は、前記補正量を用いて前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルを更新し、
前記第２演算部は、前記速度ベクトルの第１方向成分および前記加速度ベクトルの前記第１方向成分と垂直な第２方向成分に基づく確率分布を用いて、前記速度ベクトルの前記第２方向成分の絶対値を制限し、前記補正量の算出を行う、
位置推定装置。

【請求項2】

加速度ベクトルおよび角加速度ベクトルから、位置ベクトル、速度ベクトルおよび姿勢角ベクトルを算出する第１演算部と、
状態空間モデルによって、観測量、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルから、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルの各要素に対する補正量を算出する第２演算部とを備え、
前記第１演算部は、前記補正量を用いて前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルを更新し、
前記第２演算部は、前記速度ベクトルの第１方向成分および前記加速度ベクトルの前記第１方向成分と垂直な第２方向成分に基づき、前記速度ベクトルの前記第２方向成分の絶対値を制限し、前記補正量の算出を行い、
前記第２演算部は、前記位置ベクトルおよび前記速度ベクトルに基づいてスリップ角を算出し、前記スリップ角がしきい値を超えている場合に、前記補正量の算出に使う前記速度ベクトルの前記第２方向成分の絶対値の制限を緩和する、または、前記位置ベクトルおよび前記速度ベクトルに基づいて前記速度ベクトルの第２方向成分におけるすべり量を算出し、前記すべり量がしきい値を超えている場合に、前記補正量の算出に使う前記速度ベクトルの前記第２方向成分の絶対値の制限を緩和する、
位置推定装置。

【請求項3】

前記第２演算部は、前記状態空間モデルにフィルタを適用して、前記補正量を算出する、
請求項１または２に記載の位置推定装置。

【請求項4】

前記フィルタは、カルマンフィルタ、拡張された前記カルマンフィルタ、一般化した前記カルマンフィルタ、粒子フィルタ、モンテカルロフィルタ、適応フィルタ、非線形フィルタ、アンセンテッドカルマンフィルタまたはアンサンブルカルマンフィルタである、
請求項３に記載の位置推定装置。

【請求項5】

前記第１演算部は、ストラップダウン演算によって前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルを算出する、
請求項１または２に記載の位置推定装置。

【請求項6】

前記観測量は、位置、速度及び姿勢の少なくともいずれかを含む、
請求項１または２に記載の位置推定装置。

【請求項7】

前記観測量は、衛星測位、車輪速センサ、ソナー、レーザ計測、またはカメラ画像の少なくともいずれかによって計測された情報に基づいている、
請求項１または２に記載の位置推定装置。

【請求項8】

前記加速度ベクトルおよび前記角加速度ベクトルは、慣性計測装置の計測値に基づいている、
請求項１または２に記載の位置推定装置。

【請求項9】

前記第２演算部は、前記速度ベクトルに基づいて前記慣性計測装置の設置姿勢を推定し、前記設置姿勢に基づき前記補正量を算出する、
請求項８に記載の位置推定装置。

【請求項10】

加速度ベクトルおよび角加速度ベクトルから、位置ベクトル、速度ベクトルおよび姿勢角ベクトルを算出する第１演算部と、
状態空間モデルによって、観測量、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルから、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルの各要素に対する補正量を算出する第２演算部とを備え、
前記第１演算部は、前記補正量を用いて前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルを更新し、
前記第２演算部は、前記速度ベクトルの第１方向成分および前記加速度ベクトルの前記第１方向成分と垂直な第２方向成分に基づき、前記速度ベクトルの前記第２方向成分の絶対値を制限し、前記補正量の算出を行い、
前記加速度ベクトルおよび前記角加速度ベクトルは、慣性計測装置の計測値に基づいており、
前記第２演算部は、前記速度ベクトルに基づいて前記慣性計測装置の設置姿勢を推定し、前記設置姿勢に基づき前記補正量を算出し、
前記第１方向成分は、移動体の進行方向の成分であり、前記第２方向成分は、前記進行方向の成分に対し左右方向の成分であり、
前記第２演算部は、前記速度ベクトルにおける前記移動体の鉛直方向の成分の時間平均と、前記速度ベクトルにおける前記第２方向成分の時間平均とに基づき、前記慣性計測装置の設置姿勢を推定する、
位置推定装置。

【請求項11】

加速度ベクトルおよび角加速度ベクトルから、位置ベクトル、速度ベクトルおよび姿勢角ベクトルを算出する第１演算部と、
状態空間モデルによって、観測量、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルから、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルの各要素に対する補正量を算出する第２演算部とを備え、
前記第１演算部は、前記補正量を用いて前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルを更新し、
前記第２演算部は、前記速度ベクトルの第１方向成分および前記加速度ベクトルの前記第１方向成分と垂直な第２方向成分に基づき、前記速度ベクトルの前記第２方向成分の絶対値を制限し、前記補正量の算出を行い、
前記加速度ベクトルおよび前記角加速度ベクトルは、慣性計測装置の計測値に基づいており、
前記第２演算部は、前記位置ベクトルまたは前記速度ベクトルの少なくともいずれかと、前記観測量とを比較し、前記観測量の遅延時間を算出し、
前記第１演算部は、前記遅延時間に応じて、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルの遅延を補償する、
位置推定装置。

【請求項12】

前記第２演算部は、前記観測量が計測される第１位置と、前記慣性計測装置が設置されている第２位置との相対位置に基づき、前記位置ベクトルに対する前記補正量を算出する、
請求項８に記載の位置推定装置。

【請求項13】

前記第１方向成分は、移動体の前後方向成分であり、前記第２方向成分は、前記移動体の左右方向成分である、
請求項１または２に記載の位置推定装置。

【請求項14】

前記第１方向成分は、移動体の進行方向の成分であり、前記第２方向成分は、前記進行方向の成分に対し左右方向の成分である、
請求項１または２に記載の位置推定装置。

【請求項15】

加速度ベクトルおよび角加速度ベクトルから、位置ベクトル、速度ベクトルおよび姿勢角ベクトルを算出するステップと、
観測量、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルを状態空間モデルに入力するステップと、
前記状態空間モデルにおいて、前記速度ベクトルの第１方向成分および前記加速度ベクトルの前記第１方向成分と垂直な第２方向成分に基づく確率分布を用いて、前記速度ベクトルの前記第２方向成分の絶対値を制限した上で、前記状態空間モデルによって前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルの各要素に対する補正量を算出するステップと、
前記補正量を用いて前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルを更新するステップとを含む、
位置推定方法。

【請求項16】

加速度ベクトルおよび角加速度ベクトルから、位置ベクトル、速度ベクトルおよび姿勢角ベクトルを算出するステップと、
観測量、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルを状態空間モデルに入力するステップと、
前記状態空間モデルにおいて、前記速度ベクトルの第１方向成分および前記加速度ベクトルの前記第１方向成分と垂直な第２方向成分に基づく確率分布を用いて、前記速度ベクトルの前記第２方向成分の絶対値を制限した上で、前記状態空間モデルによって前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルの各要素に対する補正量を算出するステップと、
前記補正量を用いて前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルを更新するステップとをコンピュータに実行させる、
位置推定プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、位置推定装置、位置推定方法および位置推定プログラムに係り、特に、センサから出力される情報であるセンシングデータに基づいて自車両の位置を推定する位置推定装置、位置推定方法および位置推定プログラムに関する。

【背景技術】

【0002】

センシングデータに基づき、移動体の位置、速度、姿勢などを求める技術の開発が進められている。センシングデータには各種の誤差が含まれ得るが、当該センシングデータに含まれる誤差に関わらず、移動体の位置、速度、姿勢などを高精度で推定する技術が求められる。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０２０－１６９８７２号公報

【文献】特開２０１１－１２２９２１号公報

【文献】特開２０１３－１７０９０３号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

本開示は、移動体の位置を高精度で推定する、位置推定装置、位置推定方法および位置推定プログラムに関する。

【課題を解決するための手段】

【0005】

本開示による位置推定装置は、加速度ベクトルおよび角加速度ベクトルから、位置ベクトル、速度ベクトルおよび姿勢角ベクトルを算出する第１演算部と、状態空間モデルによって、観測量、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルから、前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルの各要素に対する補正量を算出する第２演算部とを備え、前記第１演算部は、前記補正量を用いて前記位置ベクトル、前記速度ベクトルおよび前記姿勢角ベクトルを更新し、前記第２演算部は、前記速度ベクトルの第１方向成分および前記加速度ベクトルの前記第１方向成分と垂直な第２方向成分に基づき、前記速度ベクトルの前記第２方向成分の絶対値を制限し、前記補正量の算出を行う。

【図面の簡単な説明】

【0006】

【図1】本実施形態に係る車載システムの構成の一例を示したブロック図である。

【図2】（Ａ）は、地球上に設定される座標系を示した説明図であり、（Ｂ）は、車両座標系を示した説明図である。

【図3】本実施形態に係る位置推定装置のブロック図の一例である。

【図4】本実施形態に係る位置推定装置のハードウェア構成の一例を示したブロック図である。

【図5】本実施形態に係る位置推定装置の処理の一例を示したフローチャートである。

【図6】本実施形態に係る位置推定装置の処理の一例を示したシーケンス図である。

【図7】車両を２輪車に擬制した２輪モデルの一例を示した概略図である。

【図8】車両を２輪車に擬制した２輪モデルの一例を示した概略図である。

【図9】Moving baseにおけるアンテナ配置の一例を示した概略図である。

【図10】本実施形態に係る位置推定装置における補正アルゴリズムブロックの非線形フィルタに係る機能ブロック図の一例である。

【図11】第１アンセンティッド変換部の機能ブロック図の一例である。

【図12】センサ異常検知部の機能ブロック図の一例である。

【図13】本発明の実施形態に係る位置推定装置のフィルタリング部の機能ブロック図の一例である。

【図14】横速度の発生確率分布の例を示した概略図である。

【図15】速度一定を仮定した場合の車両の横加速度に対する横速度上限の確率標準偏差の変化の一例を示した概略図である。

【図16】ＩＭＵの実際の出力である実出力と理想的な出力である理想出力とを比較した概略図である。

【図17】（Ａ）はＩＭＵ座標系における横方向の速度発生の説明図であり、（Ｂ）は横速度の確率密度関数の一例を示した概略図である。

【図18】ＩＭＵから得られる車両運動の波形、及び補助センサから得られる車両運動の波形の一例を示した概略図である。

【発明を実施するための形態】

【0007】

移動体である車両の位置及び速度の推定アルゴリズムは、理想的にはあらゆる環境、あらゆる車両運動の領域で精度よく演算できることが求められる。あらゆる環境とは、舗装路から不整地又は雪道等の走行路の様々な状態を示し、あらゆる運動とは、微低速での走行からスポーツ走行、又は車輪のスリップを伴うような不整地での走行等といった様々な走行の態様を示している。

【0008】

従来は、多くの場合、車両の横速度をゼロとみなす非ホロノミック運動を前提とした車両運動モデルによって車両の位置、速度及び姿勢を演算した。かかる場合、不整地及び雪道等の車両が大きくスリップするような条件では車両の位置、速度及び姿勢の演算精度が非常に劣化するおそれがある。車両の位置、速度及び姿勢の演算に、非ホロノミック運動等を前提としたのは、車両ではＭＥＭＳ（Micro Electro Mechanical Systems）方式のＩＭＵ（Inertial Measurement Unit：慣性計測装置）のような精度がさほど高くない安価な慣性センサを用いることがコスト面から要求されるためであった。

【0009】

運動の演算には、非ホロノミック運動のような運動拘束又は運動モデルを用いないストラップダウン演算という方式がある。ストラップダウン演算は、航空機及び宇宙機等の３次元空間を比較的自由に運動する移動体に利用されてきた技術であり、移動体に搭載されたＩＭＵ等の慣性センサで検出した当該移動体の座標系における軸系の加速度及び方位角速度から算出した任意の座標系の移動位置に基づいて移動体の位置を推定する。

【0010】

ストラップダウン演算に際しては、移動体に係る加速度及び方位角速度を高精度で取得可能なＦＯＧ（Fiber optic gyro）又はＲＬＧ（Ring laser gyro）等の高価な慣性センサを用いることが一般的だった。しかしながら、車両では、上述のようにＭＥＭＳ方式のような安価な慣性センサが用いられる場合が多く、かかる安価な慣性センサは、センサ出力のオフセット誤差やスケール誤差が上述の高価な慣性センサに比して大きいことが問題となる。

【0011】

図１６は、ＩＭＵの実際の出力である実出力８２と理想的な出力である理想出力８０とを比較した概略図である。理想的な出力であれば、図１６のように横軸に真値、縦軸にセンサ値をとると、理想出力８０のように、原点を通り、傾き１の直線を示す。しかしながら、スケールファクタ誤差（比例誤差）が含まれる実出力８２では、原点を通る直線を示すものの、当該直線の傾きが理想出力８０の直線の傾きと一致しない。

【0012】

ゼロ点オフセットは、ＩＭＵを搭載した車両が静止状態の場合に、ＩＭＵの出力が０になるように補正すればよいため、比較的容易に検出及び補正が可能である。しかしながら、スケールファクタ誤差は、例えば、ＧＮＳＳ（Global Navigation Satellite System：全地球航法衛星システム）等の外部からの情報を参照し、非線形フィルタ等を用いて推定した補正量で補正する等によって解消することを要する。

【0013】

以下、図面を参照して本開示の実施形態を詳細に説明する。本実施形態の位置推定装置、位置推定方法および位置推定プログラムは、各種の移動体に適用可能である。移動体は、車両だけでなく、ロボットや船舶であってもよい。

【0014】

図１に示すように、本実施形態に係る車載システム１０は、例えば、入力装置１２と、位置推定装置１４と、表示装置１６と、記憶装置１８と、画像情報処理部２０と、撮像装置２２と、車速センサ２４と、ＩＭＵ２６と、操舵角センサ２８と、ＧＮＳＳ３０と、ＬＩＤＡＲ（Laser Imaging Detection and Ranging）３２と、Ｖ２Ｘ通信部３４と、を備えていてもよい。

【0015】

入力装置１２には、画像情報処理部２０が算出した横位置偏差、ヨー角偏差及び曲率、車速センサ２４が検出した車両前後速度、ＩＭＵ２６が検出した車両の方位角の偏差及び加速度、操舵角センサ２８が検出した車両の操舵角、ＧＮＳＳ３０で検出した車両の現在位置、ＬＩＤＡＲ３２で検出した車両の現在位置及び現在のヨー角、並びにＶ２Ｘ通信部３４が無線通信で取得した情報が各々入力される。位置推定装置１４は、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいて車両の自己位置の推定の演算を行なうコンピュータ等で構成されている。表示装置１６は、位置推定装置１４で演算された車両の位置等を表示するＣＲＴ又はＬＣＤ等で構成されている。記憶装置１８は、位置推定装置１４の演算に必要なデータ及び位置推定装置１４による演算結果を記憶する。

【0016】

画像情報処理部２０は、撮像装置２２が取得した画像情報から目標経路と車両との相対横位置、車両のヨー角及び目標経路の曲率を算出する。撮像装置２２は、例えば、車載カメラである。車速センサ２４は、車両の車輪の回転数に基づいて車両の速度を検出する。ＩＭＵ２６は、３次元の角速度及び加速度を検出する慣性センサである。操舵角センサ２８は、車両の操舵角を検出する。ＧＮＳＳ３０は、衛星から受信した情報に基づいて測位を行う。ＬＩＤＡＲ（Laser Imaging Detection and Ranging）３２は、照射した光の反射光を検出し、反射の起こった物体までの距離を計測する。Ｖ２Ｘ通信部３４は、車両外との無線通信を行う。

【0017】

本実施形態に係る外界センサである撮像装置２２は車載カメラ等であり、一例として、撮影により取得した車両周辺の画像情報を解析して道路の白線等を検出する。位置推定装置１４は、撮像装置２３で得られた画像と、地図情報とを照合し、車両の現在位置及び車両の方位角の少なくともいずれかを推定してもよい。

【0018】

車両の挙動を記述するために、例えば、図２に示す座標系を使うことができる。図２（Ａ）は、地球３００上に設定される座標系を示した説明図である。図２（Ａ）には、（Ｘ_I，Ｙ_I，Ｚ_I）で示された地球３００中心を原点とした慣性座標系（Ｉ座標系）と、（Ｘ_e，Ｙ_e，Ｚ_e）で示された地球３００中心を原点として自転と共に回転する地球座標系（Ｅ座標系）と、（Ｅ，Ｎ，Ｕ）で示された車両位置を原点に東、北、鉛直上方向に軸をもつナビゲーション座標系（Ｎ座標系）と、が設定されている。

【0019】

また、図２（Ａ）における、（φ_e，λ_e，ｈ）は、各々緯度、経度、高度を示し、ω_eは地球３００の自転角速度を示す。以降は上述の座標系のうち、（Ｅ，Ｎ，Ｕ）で示されたＮ座標系を考慮する。なお、地球３００の表面上に点線で描かれた曲線３０２は、本初子午線であるグリニッジ子午線である。

【0020】

図２（Ｂ）は、車両座標系（Ｖ座標系）を示した説明図である。図２（Ｂ）に示したように、車両座標系は、（ｘ，ｙ，ｚ）で示される。また、図２（Ｂ）において、（Ｐ，Ｑ，Ｒ）の各々はロールレート、ピッチレート、ヨーレートを示し、（ｖ_x，ｖ_y，ｖ_z）の各々は車両座標系における座標軸の各々での速度を表す。

【0021】

図３は、位置推定装置１４のブロック図の一例である。第１演算部６０Ａで実行可能な演算方法の例として、ストラップダウン演算が挙げられる。ただし、その他の演算方法を使って位置ベクトル、速度ベクトルおよび姿勢角ベクトルの時間発展を計算してもよい。

【0022】

上述のように、位置推定装置１４は、車両の３次元運動を推定するストラップダウン演算を行う。また、位置推定装置１４は、補助センサから得られたセンシング情報を活用して位置推定の精度を高めることができる。補助センサの例としては、車速センサ２４、操舵角センサ２８、ＧＮＳＳ３０、ＬＩＤＡＲ３２、カメラである撮像装置２２等が挙げられる。ただし、補助センサの種類を限定するものではない。車載システム１０は、これらの補助センサのすべてを備えていなくてもよい。

【0023】

図３の第１演算部６０Ａは、例えば、加速度ベクトルおよび角加速度ベクトルより、位置ベクトル、速度ベクトルおよび姿勢角ベクトルを計算する。ここで、加速度ベクトルおよび角加速度ベクトルは、ＩＭＵ２６から得られたデータまたは、ＩＭＵ２６の出力信号から生成されたデータであってもよい。第２演算部６０Ｂは、例えば、状態空間モデルを利用して、観測量、位置ベクトル、速度ベクトルおよび姿勢角ベクトルより、位置ベクトル、速度ベクトルおよび姿勢角ベクトルの各要素に対する補正量を計算する。

【0024】

ここで、観測量は、例えば、位置、速度または姿勢の少なくともいずれかを含むセンシングデータである。観測量として、例えば、衛星測位、車輪速センサ、ソナー、レーザ計測またはカメラ画像の少なくともいずれかによって計測された情報を使うことができる。例えば、車速センサ２４、操舵角センサ２８、ＧＮＳＳ３０、ＬＩＤＡＲ３２などの補助センサの出力信号または補助センサの出力信号に基づくデータより観測量を得ることができる。

【0025】

第１演算部６０Ａは、上述の補正量を用いて位置ベクトル、速度ベクトルおよび姿勢角ベクトルを更新することができる。第１演算部６０Ａは、例えば、ストラップダウン演算によって位置ベクトル、速度ベクトルおよび姿勢角ベクトルを計算してもよい。ただし、第１演算部６０Ａの実行する演算方法を限定するものではない。例えば、その他の種類の積分演算で位置ベクトル、速度ベクトルおよび姿勢角ベクトルの時間更新を行ってもよい。

【0026】

第２演算部６０Ｂは、速度ベクトルの第１方向成分および速度ベクトルの第１方向成分と垂直な第２方向成分に基づき、速度ベクトルの第２方向成分の絶対値を制限し、補正量の計算を行うことができる。

【0027】

ここで、第１方向として、移動体（例えば、車両）の進行方向を設定することができる。この場合、速度ベクトルの第１方向成分は、移動体の進行方向の速度となる。また、速度ベクトルの第２方向成分は、移動体の進行方向に対し、左右方向の速度となる。

【0028】

また、第１方向成分を移動体の前後方向に設定してもよい。この場合、速度ベクトルの第１方向成分は、移動体の前後方向の速度となる。また、速度ベクトルの第２成分方向成分は、移動体の左右方向の速度となる。なお、以降の説明では、必要に応じて、速度ベクトルの第２方向成分を「横速度」と記載するものとする。また、必要に応じて、速度ベクトルの第２方向成分におけるすべり量を「横すべり量」と記載するものとする。

【0029】

第２演算部６０Ｂは、速度ベクトルの第１方向成分および加速度ベクトルの第２方向成分に基づく確率分布を用いて、速度ベクトルの第２方向成分の絶対値を制限し、補正量の計算を行ってもよい。例えば、図１４に示したような正規分布の分散σに基づき、演算処理における速度ベクトルの第２方向成分の絶対値を制限することができる。ただし、演算処理においては、これとは異なる方法で速度ベクトルの第２方向成分の絶対値を制限してもよい。

【0030】

第２演算部６０Ｂは、状態空間モデルにフィルタを適用し、位置ベクトル、速度ベクトルおよび姿勢角ベクトルの各要素に対する補正量を計算してもよい。第２演算部６０Ｂでは、例えば、状態空間モデルにカルマンフィルタまたアンセンテッドカルマンフィルタを適用し、補正量を計算する。状態空間モデルは、時系列モデルの一種であり、後述する観測方程式と状態方程式とによって記述される。状態方程式は、時系列で変化する値（状態量という）を記述する微分方程式である。観測方程式は、状態量と観測量の関係を規定する方程式である。カルマンフィルタまたアンセンテッドカルマンフィルタに代えて、状態空間モデルに拡張されたカルマンフィルタ、一般化したカルマン、粒子フィルタ、モンテカルロフィルタ、適応フィルタ、非線形フィルタ、またはアンサンブルカルマンフィルタなど、その他のフィルタを適用してもよい。第２演算部６０Ｂは、その他の時系列データモデルを使って補正量の計算を行うことができる。

【0031】

図３に示した第１演算部６０Ａでは、ＩＭＵ２６による状態の時間更新６００のプロセスにおいて、ＩＭＵ２６から入力された角速度及び加速度のデータを積分することで、ＩＭＵ２６を搭載した車両の位置、速度、姿勢角、加速度、及び角速度を演算し、ＩＭＵ２６による状態の時間更新を行う。さらに、ＩＭＵ２６の出力は、ノイズフィルタや通信の影響で遅延することがある。第１演算部６０Ａでは、ＩＭＵ遅延補償６０２のプロセスにおいて、遅延時間を補償するＩＭＵ遅延補償を行うことで、真の現時点での車両状態を出力する。第１演算部６０Ａは、後述する第２演算部６０Ｂが出力した補正量を用いて補正した車両の位置、速度、及び姿勢角を出力する。

【0032】

第２演算部６０Ｂの状態判定６１０のプロセスでは、後述する事前推定部４２において、補助センサ２２、２４、２８、３０、３２からのセンサデータと、第１演算部６０Ａで算出した車両の位置、速度、姿勢角、加速度、及び角速度が入力され、センサ状態の判定、位置、速度及び姿勢の少なくともいずれかを含む観測量の生成、及び車両運動状態判定が行われる。

【0033】

状態判定６１０のプロセスでは、第２演算部６０Ｂで推定した車体の横すべり量、またはスリップ角の少なくともいずれかに基づいて、非線形フィルタでの横速度絶対値制限（速度ベクトルの横速度の成分の絶対値の制限）の適用可否を判定する。例えば、横速度の確率標準偏差を前後速度と横加速度とから算出し、当該確率標準偏差に基づき横速度絶対値制限を行う。

【0034】

状態判定６１０のプロセスで生成した観測量は、後述するセンサ異常検知部４４で実行されるセンサ異常検知６１４のプロセスに入力される。センサ異常検知６１４のプロセスでは、ストラップダウン演算からの位置、速度及び姿勢の情報と補助センサの値とを比較し、センサデータの異常検知をする。異常と判定されたセンサデータについては、後述するフィルタリング部４６で実行されるフィルタリング６１６のプロセスに入力しなくてもよい。

【0035】

フィルタリング６１６のプロセスでは、センサ異常検知６１４のプロセスが出力した観測値と、状態判定６１０が出力した車両運動状態とに基づいて、各種センサのデータ遅延時間の補償とＩＭＵ２６の取付オフセットの推定とを行う。各種センサの遅延時間は、時刻同期サーバ５０によって同期された位置推定装置１４の時刻と、各々のセンサデータの時間との差分を演算することで把握する。さらにフィルタリング６１６のプロセスは、ＩＭＵ２６の車体設置オフセット、ゼロ点オフセット、及びスケールファクタ誤差の推定を実施する。そして、フィルタリング６１６のプロセスは、推定した位置・速度・姿勢誤差、ＩＭＵ２６の誤差、ＩＭＵ２６搭載姿勢、補助センサ遅延時間及び補助センサに係るその他の誤差等に対する補正量を算出する。

【0036】

フィルタリング６１６のプロセスで算出した補正量は第１演算部６０Ａに出力されると共に、後述する更新部４８で実行される状態量の時間変更６１２のプロセスによる予測値算出に使われる。状態量の時間変更６１２プロセスで出力した予測値は、センサ異常検知６１４のプロセスで適否を判断される。正常な予測値は正常な観測値と共にフィルタリング６１６のプロセスにおける補正量の算出に使われる。第２演算部６０Ｂでは、時刻同期サーバ５０から取得した時刻情報を用いて処理を行ってもよい。しかしながら、時刻情報は、ＧＮＳＳ３０を介しても取得できるので、時刻同期サーバ５０からの情報取得は、必須ではない。

【0037】

なお、図３に示した位置、速度、姿勢角、加速度、及び角速度等の運動情報は、ベクトルでもスカラー量でもよい。

【0038】

図４に示したように、プロセッサ上で実行されるプログラムによって位置推定装置１４の機能を実現してもよい。この場合、本実施形態における位置推定装置１４は、一種のコンピュータであり、演算処理を司るプロセッサ１１４Ａ、ＲＯＭ（Read Only Memory）、又はＲＡＭ（Random Access Memory）である記憶部１１４Ｂ、及び入力装置１２介して補助センサ２２、２４、２８、３０、３２等からの入力を受け付ける通信部１１４Ｃ等を備える。プロセッサ１１４Ａ、記憶部１１４Ｂ及び通信部１１４Ｃの各々は、バス等を介して接続されている。なお、位置推定装置１４の機能をハードウェア回路または、ハードウェア回路とプログラムの組み合わせによって実装してもよい。

【0039】

図５は、本実施形態に係る位置推定装置１０の処理の一例を示したフローチャートである。ステップＳ１０では、ストラップダウン演算により、加速度ベクトル及び角速度ベクトルに基づいて車両２００の位置ベクトル、速度ベクトル及び姿勢角ベクトルを算出する。

【0040】

ステップＳ１１で、第２演算部６０Ｂは、位置ベクトル、速度ベクトル及び姿勢角ベクトルより車両２００のスリップ角を算出する。ステップＳ１１で、第２演算部６０Ｂは、車両２００の横すべり量を算出してもよい。

【0041】

ステップＳ１２で、第２演算部６０Ｂは、ステップＳ１１で算出したスリップ角がしきい値より大きいか否かを判定する。また、ステップ１２で、第２演算部６０ＢはステップＳ１１で算出した横すべり量がしきい値より大きいか否かを判定してもよい。ステップＳ１２で、スリップ角が閾値よりも大きい場合は処理をステップＳ１４に進め、スリップ角が閾値以下の場合は、処理をステップＳ１３に進める。

【0042】

ステップＳ１３で、第２演算部６０Ｂは、位置ベクトル、速度ベクトル及び姿勢角ベクトルの各要素に対する補正量の算出に用いる速度ベクトルの横方向成分の絶対値を制限する。

【0043】

ステップＳ１４で、第２演算部６０Ｂは、位置ベクトル、速度ベクトル及び姿勢角ベクトルの各要素に対する補正量の算出に用いる速度ベクトルの横方向成分の絶対値を緩和する。

【0044】

ステップＳ１５で、第２演算部６０Ｂは、速度ベクトルの前後方向成分及び速度ベクトルの横方向成分に基づいて、位置ベクトル、速度ベクトル及び姿勢角ベクトルの各々の補正量を算出する。

【0045】

ステップＳ１６で、第１演算部６０Ａは、第２演算部６０Ｂが算出した補正量を用いて位置ベクトル、速度ベクトル及び姿勢角ベクトルを更新する。

【0046】

ステップＳ１７では、位置推定を継続するか否かを判定する。位置推定を継続する場合は処理をステップＳ１１に進め、位置推定を終了する場合は、上記の一連の処理を終了する。

【0047】

図６は、本実施形態に係る位置推定装置１０の処理の一例を示したシーケンス図である。ＩＭＵ２６では、検出結果から車両２００の加速度、及び角速度の算出が行われ、その結果、車両２００の加速度ベクトル及び角速度ベクトルが第１演算部６０Ａに出力される。図６のシーケンス図に示したように、ＩＭＵ２６、補助センサ、第１演算部６０Ａ、および第２演算部６０Ｂは、非同期で動作していてもよい。例えば、ＩＭＵ２６は、周期Ｔ１、第１演算部６０Ａは、周期Ｔ２で動作するものであってもよい。ここで、Ｔ１≠Ｔ２であってもよいし、Ｔ１＝Ｔ２であってもよい。ただし、ＩＭＵ２６、補助センサ、第１演算部および第２演算部の動作タイミングについては特に問わない。各構成要素は一定の周期で動作してもよいし、いずれかの構成要素は不定期で動作するものであってもよい。動作周期が規定されている場合、周期については限定しない。

【0048】

補助センサ２２、２４、２８、３０、３２では、観測量の計測、及び算出がなされ、取得された観測量は第２演算部６０Ｂに出力される。

【0049】

第１演算部６０Ａは、ＩＭＵ２６からの入力信号に基づき、車両２００の位置ベクトル、速度ベクトル、及び姿勢角ベクトルを算出する。ここで、第１演算部６０Ａは、ストラップダウン演算を実行してもよい。

【0050】

第２演算部６０Ｂでは、補助センサ２２、２４、２８、３０、３２の少なくともいずれかから取得した観測量、及び第１演算部６０Ａから取得した位置ベクトル、速度ベクトル、及び姿勢角ベクトルに基づいて、位置ベクトル、速度ベクトル、及び姿勢角ベクトルの補正量を算出する。第２演算部６０Ｂで算出した補正量は第１演算部６０Ａにフィードバックされ、第１演算部６０Ａでは、フィードバックされた補正量を用いて位置ベクトル、速度ベクトル、及び姿勢角ベクトルの時間更新を行う。

【0051】

以下、車両の３次元運動を算出するストラップダウン演算について説明する。ストラップダウン演算は、加速度ベクトルおよび角加速度ベクトルに基づいて、移動体の位置ベクトル、速度ベクトルおよび姿勢角ベクトルの時間発展を推定する際に利用可能な演算方式の一例である。移動体の位置ベクトル、速度ベクトルおよび姿勢角ベクトルの時間発展は、ストラップダウン演算以外の他の演算方式で求めてもよい。以下の演算は、ストラップダウン演算の一例にしかすぎない。従って、必要に応じて項の追加、係数の追加などの変形を含むストラップダウン演算が実行されてもよい。

【0052】

姿勢更新の演算では、Ｎ座標系の角速度ωⁿ _n/iを、下記の式（１）のように定義する。Ｎ座標系の角速度ωⁿ _n/iは、Ｎ座標系での地球自転成分ωⁿ _e/iと、Ｎ座標系で表されたトランスポートレートである角速度ρとの和で示される。なお、トランスポートレートは、球体に擬制された地球表面を移動することによって生じる角速度である。

・・・（１）

【0053】

ここで、Ｅ座標系をＮ座標系に変換する方向余弦行列Ｃ_n/eを導入すると、Ｎ座標系での地球自転成分ωⁿ _e/iを地球座標系の地球自転成分ω^e _e/iで示したものは下記の式（２）のようになる。

・・・（２）

【0054】

トランスポートレートρは、Ｎ座標系における速度（ｖ_e, ｖ_n, ｖ_u）を用いて、下記の式（３）で表される。

・・・（３）

【0055】

Ｎ座標系を車両座標系に変換する方向余弦行列Ｃ_v/nを導入すると、Ｎ座標系での角速度ωⁿ _n/iをＮ座標系での地球自転成分ω^e _e/iで示したものは下記の式（４）のようになる。

・・・（４）

【0056】

ＩＭＵ２６の出力である角速度計の値ω^v _v/iは慣性座標系における角速度を検出するため、Ｎ座標系に対する車両座標系の角速度ω^v _v/nは、上記の式（１）、（４）により、下記の式（５）のようになる。

・・・（５）

【0057】

上記の角速度成分を下記の式（６）に示したクォータニオン更新の公式に代入することで、姿勢更新を演算する。式（６）中のδtは演算のステップ時間、Ω^v _v/nは角速度計の値で構成される歪対称行列である。

・・・（６）

【0058】

続いてストラップダウン演算における位置更新について説明する。Ｎ座標系における速度（ｖ_e, ｖ_n, ｖ_u）、トランスポートレートをρ＝（ρ_x，ρ_y，ρ_z）、Ｎ座標系での地球自転成分をωⁿ _e/i＝（ω_e,x，ω_e,y，ω_e,z）、加速度計の値をＡ＝（Ａ_x，Ａ_y，Ａ_z）とし、要素ごとに整理すると下記の式（７）となる。式（７）中のｇ_zは鉛直方向の重力加速度であり、並進方向の重力加速度は０とみなす。

・・・（７）

【0059】

第１演算部６０ＡでＮ座標系に変換した車両２００の加速度、及び角速度のデータを、例えば積分することにより、車両２００の３次元運動に係る速度、姿勢角、及び位置が算出される。

【0060】

続いて、横速度演算式の導出について説明する。図７は、車両２００を２輪車に擬制した２輪モデルの一例を示した概略図である。横速度演算式の導出に際しては、図７に示した２輪モデルの重心位置での運動方程式を導出し、その後、速度と加速度を後輪車軸中心に変換する。図７の２輪モデルでは、車両２００の重心ＣＧ、後輪車軸中心２０２、及び前輪車軸中心２１０に作用する物理量を扱う。また、重心ＣＧにおいて、前方向をｘ_c、横方向をｙ_cと定義すると共に、後輪車軸中心２０２において、前方向をｘ_w、横方向をｙ_wと定義する、

【0061】

重心ＣＧにおける横方向の運動方程式は下記の式（８）のようになる。式（８）中の、ｍは車体質量、ａ_c,yは重心ＣＧにおける横加速度、Ｆ_f、Ｆ_rの各々は前輪、後輪の各々のタイヤ横力、φは路面ロール角、そしてθは路面ピッチ角である。

・・・（８）

【0062】

回転の運動方程式は下記の式（９）のようになる。式（９）中のＩは重心回りの車体慣性モーメントであり、Ｒは車体ヨーレート、Ｒの微分値は回転加速度、ｌ_fは重心ＣＧから前輪車軸中心２１０までの距離、ｌ_rは重心ＣＧから後輪車軸中心２０２までの距離である。そして、本実施形態では、車両の常用域を想定していることから、回転加速度を０に近似させると、式（９）から下記の式（１０）が得られる。

・・・（９）

・・・（１０）

【0063】

式（１０）と横方向の運動方程式である式（８）とから下記の式（１１）が得られ、車両２００のホイールベースｌをｌ＝ｌ_f＋ｌ_rとすると、式（１１）から式（１２）が導かれる。また、後輪位置における鉛直荷重ｍ_rｇは、ｍ_rｇ＝(ｌf/ｌr)ｍｇとなるので、式（１２）から式（１３）が得られる。

・・・（１１）

・・・（１２）

・・・（１３）

【0064】

さらに、タイヤ横力がスリップ角に比例する線形領域では、後輪横力Ｆ_rは下記の式（１４）で示される。式（１４）中のＣ_rはコーナリングスティフネスを輪荷重で除算して得た正規化コーナリングスティフネス、Ｖ_c,y、Ｖ_c,xはそれぞれ横速度と前後速度である。

・・・（１４）

【0065】

式（１３）と式（１４）から、下記の式（１５）が得られる。

・・・・（１５）

【0066】

重心ＣＧにおける車体横方向の速度Ｖ_c,yと、後輪車軸中心２０２における車体横方向の速度Ｖ_w,yとの関係は、下記の式（１６）で示され、重心ＣＧにおける車体横方向の加速度ａ_c,yと、後輪車軸中心２０２における車体横方向の加速度ａ_w,yとの関係は、下記の式（１７）で示される。

・・・（１６）

・・・（１７）

【0067】

式（１５）に、式（１６）及び式（１７）を代入すると、下記の式（１８）が得られる。

・・・（１８）

【0068】

図７の２輪モデルでは、車体ヨーレートＲの微分値である回転加速度を０に近似させていることと、路面勾配の変動が微小であると仮定するとＰＱは０とみなせるので、後輪車軸中心２０２における加速度センサの横加速度の出力Ａ_w,yは、下記の式（１９）のようになる。

・・・（１９）

【0069】

上述のように、車体ヨーレートＲの微分値、及びＰＱの各々は０とみなせるので、式（１８）と式（１９）から下記の式（２０）が得られる。

・・・（２０）

【0070】

式（２０）から下記の横速度Ｖ_w,yを算出する式（２１）が導出される。式（２１）は、横速度Ｖ_w,yが、前後速度Ｖ_w,xと加速度センサの横加速度の出力Ａ_w,yとの積に比例することを示している。式（２１）は、横速度Ｖ_w,yを算出する式の一例であり、図７の示したような２輪モデル以外のモデルを用いた場合は、横速度を算出する式の態様も異なる。横速度Ｖ_w,yは、下記の式（２１）以外の式を用いて算出してもよい。

・・・（２１）

【0071】

続いて、アンセンティッドカルマンフィルタを用いて、ストラップダウン演算の各種誤差を推定する。アンセンティッドカルマンフィルタの名称は、アンセンティッド変換（ＵＴ：Unscented Transform）に由来する。以下、アンセンティッドカルマンフィルタによるアンセンティッド変換を適宜「ＵＴ」と略記する。ＵＴにおいて推定するパラメータは下記の通りである。・位置誤差×３・速度誤差×３・姿勢角×３・加速度センサバイアス×３・角速度センサバイアス×３・ヨーレートスケールファクタ・前後・横加速度スケールファクタ・車輪速スケールファクタ・操舵角誤差・ＩＭＵ搭載姿勢オフセット（方位、ピッチ）・ＧＮＳＳアンテナ設置誤差（方位、位置）・補助センサのデータ遅延時間

【0072】

アンセンティッドカルマンフィルタで推定するには、下記の状態方程式ｆ（ｘ）と観測方程式ｈ（ｘ）との各々を定義する。状態方程式は、時系列で変化する値である状態量ｘを記述する微分方程式である。

【0073】

ｘは状態変数（状態量）、ｙは観測変数（観測量）、ｆは状態方程式、ｈは観測方程式、Ｑ_Nはシステムノイズ、Ｒ_Nは観測ノイズである。観測方程式は、状態量と観測量との関係を規定する方程式である。状態方程式及び観測方程式は各々非線形関数を適用することが可能である。

【0074】

カルマンフィルタは、目標物の時間変化を支配する法則を活用して、現在、未来又は過去における目標物の位置を推定することができる。以下、式の流れでは全体像が見えにくいが、本実施形態では、ストラップダウン演算の結果を、ＩＭＵ２６等で検出した観測量を用いて、ＧＮＳＳ等の補助センサの計測データが示す過去に内挿（伝搬）することにより、ストラップダウン演算の結果と補助センサのデータとの差分が最小になる状態を推定する。観測量の例としては、センサの計測値および運動拘束量が挙げられる。しかしながら、観測量は、これらの値に限定されない。

【0075】

状態量は下記のように定義する。状態量の例としては、位置、速度、姿勢角、または各種の推定パラメータが挙げられる。しかしながら、状態量は、これらの値に限定されない。状態量の構成は、計測システムに利用するセンサ構成に応じて変化させる。

【0076】

δＲは位置誤差、δＶは速度誤差、 Ψは姿勢誤差、ｂ_ωは角速度バイアス誤差、ｂ_A は加速度バイアス誤差、Ｓ_ωは角速度スケールファクタ誤差、 Ｓ_Aは加速度スケールファクタ誤差、θ_imuはＩＭＵ搭載ピッチオフセット、 φ_imuはＩＭＵ搭載ヨーオフセット、φ_mbはMoving baseヨーオフセット、Ｓ_mbはMoving base相対距離スケール誤差、 θ_whは操舵角オフセット、Ｓ_whは車輪速スケールファクタ誤差、ｔ_dは補助センサのデータ遅延時間であり、時刻同期サーバ５０によって同期された位置推定装置１４の時刻と、各々のセンサデータが示す時刻との差分である。

【0077】

状態方程式は以下の方程式で構成される。状態方程式中のνはシステムノイズであり、一連のνを対角成分に持つ行列をシステムノイズ行列Ｑ_Nとする。また、各変数の設定されているτは、1次マルコフモデルの特定数パラメータである。

【0078】

複数のＧＮＳＳアンテナを用いたMoving baseデータを利用する場合は、相対位置の方位誤差δφ_mbと、長さ誤差δS_mbを推定パラメータとして扱う。また、車輪速センサ（車速センサ２４）を用いる場合は、速度スケールファクタ誤差δＳ_whと舵角誤差δθ_whとを推定パラメータに追加する。

【0079】

舵角誤差は、誤差量をフィルタ内部に保持する方式で1次マルコフモデルを用いる。かかる場合、駆動ノイズがゼロの場合は、推定操舵角誤差もゼロに収束する。同じく、ＩＭＵ２６の取付誤差であるピッチ角δθ_imuと方位角δφ_imuはフィードバックしない値なので、下記のようにモデル化する。

【0080】

最後に、ＧＮＳＳデータの遅延時間をオンライン推定する場合は、ホワイトノイズに駆動されるものとして下記のようにモデル化する。

【0081】

続いてタイヤ位置における車両運動拘束に係る観測方程式について説明する。一般的に観測方程式には、センサからの値ｙと状態量ｘとの関係性を定義する。車両２００は路面上を走行することから、上下速度はゼロとみなすことができる。また、タイヤの特性から、横速度の長い時間の平均はゼロとみなすこともできる。そこで、観測値にゼロを用いた横速度と上下速度との拘束条件を適応する。

【0082】

微小な姿勢変化量の歪対象行列Ψを用いて、ａ座標系とｂ座標系の関係は下記のように表すことができる。

【0083】

車両運動拘束の適用は、図８に示した２輪モデルを想定し、タイヤ位置での速度情報を扱う。従って、車両運動拘束を適用するための、タイヤ位置での速度ベクトルを導出する。ストラップダウン演算では、Ｎ座標系におけるＥＮＵ方向の速度Ｖ_nが演算される。同時に、姿勢角（φv，θｖ，ψv）又は姿勢クォータニオン（前述の式（６）参照）でｑ_vの予測値が得られる。

【0084】

ストラップダウン演算では厳密には車両座標系はＩＭＵ２６の座標系を表しており、車両の２００進行方向とＩＭＵ２６のｘ座標とが完全に一致することは難しい。本実施形態では、車両２００の進行方向に対するピッチ誤差δθ_imuとヨー方向誤差δψ_imuが存在するとして、それが微小だと仮定すると車両座標系における速度ベクトルＶ_v＝（ｖ_x，ｖ_y，ｖ_z）は下記の式のように求められる。下記の式中のＣ_v/nは、Ｎ座標系からＶ座標系に変換する方向余弦行列の推定値であり、Ω_imuはＩＭＵ２６の設置誤差の歪対称行列である。

【0085】

続いて、タイヤ位置での速度ベクトルを考える。ＩＭＵ２６の位置からタイヤまでの相対座標がＬ_odom＝（ｘ_odom，ｙ_odom，ｚ_odom）とする。また操舵角分のヨー成分（オフセット誤差δθ_whを含む）を有する。回転変換行列Ｃ_odom/vを用いると、タイヤ位置での速度ベクトルＶ_odom＝（ｖ_odom,x， ｖ_odom,y，ｖ_odom,z）は下記のように計算される。下記の式中のω_vはＩＭＵ２６の角速度センサの値である。

【0086】

舵角に基づいた回転行列をＣ_odom/v＝（Ｉ－Ω_steer）Ｃ_steer/vとする。Ω_steerを操舵角オフセットδθ_steerに関する歪対称行列、Ｃ_steer/vを実舵角分の回転変換とすると、次の式が導出される。

【0087】

推定値と真値と誤差の関係より、下記の関係式を想定する。

【0088】

ここまでの議論は真値を扱ってきたので、ストラップダウン演算の推定値と誤差を代入して、誤差の２次の項を無視すると下記の式（２２）のように整理される。

・・・（２２）

【0089】

ここで、前後車速のセンサ値をＵ_sとし、観測ベクトルをＶ_s＝（Ｕ_s，0，0）とすると、Ｖ_sとＶ_odomとの関係は、車速センサのスケールファクタ誤差Ｓ_whを用いて下記のような関係となる。

【0090】

車両のタイヤ座標系において、横速度と上下速度の長い時間の平均はゼロになると仮定した。車速ベクトルの精度を観測ノイズで定義するため、厳密な非ホロノミック運動は仮定していない。速度に関する観測方程式は下記のように導出される。下記の式中のμ_Vodomは観測ノイズである。前後速度はセンサ値との差分方程式だが、横と上下速度はストラップダウン演算の速度そのものである。横および上下速度は、長い時間の平均はゼロになる、白色外乱に駆動された非ゼロの値である。

【0091】

舵角情報が得られない場合はＣ_steer/v＝Ｉ、Ω_steer＝０、また車輪速のデータが得られない場合はＳ_odom＝０とし、Ｌ_odomには非操舵輪の位置を利用する。その場合、式（２２）は、下記の式（２２Ａ）のように単純化される。式（２２Ａ）は、タイヤ位置での速度ベクトルＶ_odomは、Ｎ座標系を車両座標系に変換する方向余弦行列Ｃ_v/nの予測値とＮ座標系における速度Ｖ_nの予測値との積と、ＩＭＵ２６の角速度センサの出力ω_vとＩＭＵ２６の位置からタイヤまでの相対座標Ｌ_odomとの積との和で表される。また、速度誤差δＶ_odomは、方向余弦行列Ｃ_v/nの予測値と速度Ｖ_nの誤差との積から、歪対象行列Ψと方向余弦行列Ｃ_v/nの予測値と速度Ｖ_nの予測値との積、ＩＭＵ２６の設置誤差の歪対称行列Ω_imuと方向余弦行列Ｃ_v/nの予測値と速度Ｖ_nの予測値との積、及びＩＭＵ２６の角速度センサの値の誤差δω_vとＩＭＵ２６の位置からタイヤまでの相対座標Ｌ_odomとの積の各々を減算したもので表される。

・・・（２２Ａ）

【0092】

続いて、車両２００の位置及び速度を計測するセンサの観測方程式を説明する。図３に示したように、本実施形態では、情報の遅延時間を把握するため、時刻同期サーバ５０からＶ２Ｘ通信部３４等を介して得た時刻情報によって位置推定装置１４をＧＮＳＳ時刻に同期させる。ＧＮＳＳ信号には高精度な時刻情報が含まれているため、位置推定装置１４の時刻をＧＮＳＳ時刻に同期させてもよい。ＲＴＫ（Real Time Kinematic）処理された位置データには時刻情報も含まれているので、位置推定装置１４の時刻と、位置データが含む時刻との差分により、遅れ時間を算出できる。以後は、遅れ時間ｔ_d＜０が把握できているとする。ここで、位置と速度の各々におけるセンサ値と推定値との差分ΔＲ、ΔＶ_nを下記の式（２３）、（２４）のように設定する。

【0093】

遅延時間をδｔとすると、テイラー展開により下記の式（２５）、（２６）、（２７）が導出される。

【0094】

さらにＩＭＵ２６の座標系である車両座標系におけるアンテナ位置をＬ_gps1とすると、下記の式（２８）、（２９）が得られる。

【0095】

ここで、下記の関係式と、

方向余弦行列を過去に時間伝搬させた場合の下記の関係式と、に基づいて式（３０）に示した観測方程式を導出する。なお、Ω_vは、速度に関する歪行列である。

【0096】

・・・（３０）

【0097】

速度誤差については、下記の式（３１）のようになる。

・・・（３１）

【0098】

式（３１）中のμ_Rvは位置に関する観測ノイズであり、μ_Vnは速度に関する観測ノイズである。誤差項と遅延時間ｔ_dに関する２次の項を省略することも可能である。これらノイズに対して、カルマンフィルタの考え方を導入する事で、より安定した推定が実施できる。

【0099】

続いて、Moving baseによる観測方程式について説明する。図９は、Moving baseにおけるアンテナ配置の一例を示した概略図である。図９に示したように、Moving base では、ベースアンテナ７０に対するもう１つのアンテナであるローバアンテナ７２の相対的なＥＮＵ座標における位置の予測値が得られる。Moving baseは移動しているアンテナから補正情報を送信するので、精度が安定しにくい。従って、本実施形態では、方位の修正にのみMoving baseを用いる。

【0100】

車両座標系におけるベースアンテナ７０の位置をＬ_gps1、ローバアンテナ７２の位置をＬ_gps2とすると、下記の式（３２）が導かれる。式（３２）の左辺は、上述したローバアンテナ７２の相対的なＥＮＵ座標における位置の予測値である。

・・・（３２）

【0101】

方位誤差のみを含む歪行列をΨ_ψvとし、下記の関係式と、

方向余弦行列を過去に時間伝搬させた場合の下記の関係式と、

に基づいて式（３３）を得る。

・・・（３３）

【0102】

ＧＮＳＳの位置情報は水平方向の位置に比べて、高さ方向の精度が一般的に低い。方位誤差のみを含む歪行列はＺ成分に何も値がないため、実質高さ方向の情報を使わないのと同じ事を意味する。２次の誤差項を無視すると式（３３）は、下記の式（３４）のように整理される。

・・・（３４）

【0103】

また、下記の関係式に基づいて、

Moving baseの観測値をＰ_mb,sとすると、観測方程式は下記の式（３５）のようになる。式（３５）中のμ_PmbはMoving base によって演算される位置の観測ノイズである。

・・・（３５）

【0104】

下記に観測方程式ｈをまとめた。下記の式中におけるＶ_odomはタイヤ位置での速度ベクトルであり、Ｖ_odom =（v_odom,x，v_odom,y，v_odom,z）である。μは観測ノイズであり、一連の観測ノイズを対角項に持つ行列を観測ノイズ行列Ｒ_Nとする。ただし、車両運動拘束に関する観測ノイズは車両運動に応じて動的に適応する。

【0105】

以上により、カルマンフィルタを利用するのに必要な状態方程式、観測方程式、及び誤差共分散行列が定義された。以下では、アンセンテッドカルマンフィルタを用いた予測更新処理の例を説明する。

【0106】

図１０は、本実施形態に係る車載システム１０の位置推定装置１４における第２演算部６０Ｂの非線形フィルタに係る機能ブロック図の一例である。位置推定装置１４は、状態方程式ｆ（ｘ）を用いて、車両２００の運動に基づく状態の推定を行うと共に、観測方程式ｈ（ｘ）を用いて、対応するセンサ情報を推定する事前推定部４２と、異常値を出力するセンサを判定するセンサ異常検知部４４と、事前推定部４２が出力した事前推定値を、センサ情報を用いて補正するフィルタリング部４６と、観測方程式の誤差共分散行列Ｒ_nを更新する更新部４８と、を含む。フィルタリング部４６におけるフィルタリングは、状態空間モデルにフィルタを適用し、状態量を観測量で補正するデータ処理手法である。フィルタの例としては、カルマンフィルタが挙げられるがそれに限定されない。なお、フィルタリング部４６における補正は、図３に示したように、第１演算部６０Ａで行ってもよい。

【0107】

図１０に示した変数及び観測量は、下記の通りである。

【0108】

第１アンセンティッド変換部４２Ａは、状態方程式ｆ（ｘ）に基づいて状態量を更新する１回目のアンセンティッド変換を行って、ｘの平均及びｘの誤差とその広がりを示す共分散行列を出力する。

【0109】

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

【0110】

ＵＴの目的は、下記のような状態量ｘ及びｘの共分散行列Ｐ_Nが与えられた際に、ある非線形関数ｙ＝ｇ（ｘ）における変換において、ｙの平均と共分散行列Ｐ_Nを精度よく求めることである。ＵＴでは、共分散行列に基づいて２ｎ＋１個のサンプル（シグマポイント）選択し、そのサンプルの変換後の平均と共分散とを利用することで非線形性を表現する。

【0111】

図１１に示したように、第１アンセンティッド変換部４２Ａは、シグマポイント重み係数部４２Ａ１と、関数変換部４２Ａ２と、Ｕ変換部４２Ａ３と、を含む。また、第２アンセンティッド変換部４２Ｂは、シグマポイント重み係数部４２Ｂ１と、関数変換部４２Ｂ２と、Ｕ変換部４２Ｂ３と、を含む。

【0112】

第１アンセンティッド変換部４２Ａのシグマポイント重み係数部４２Ａ１及び第２アンセンティッド変換部４２Ｂのシグマポイント重み係数部４２Ｂ１では、シグマポイントＸ_i：ｉ＝０，１，２、…，２ｎが、下記のように選択される。κはスケーリングパラメータと呼ばれ、κ ≧０となるように選択されることで、共分散行列の半正定値性が確保される。なお、Ｐ_Nの平方根行列のｉ番目の列は標準偏差に相当し、一例としてコレスキー分解によって算出される。

【0113】

前述の非線形関数gによって各シグマポイントを変換したものを次式のように表す。

【0114】

ＵＴ後のｙの平均と共分散行列Ｐ^yy _N、Ｐ^xy _Nは下記のように算出される。

【0115】

シグマポイントに対する重みｗ₀、ｗ_iは下記のように定義する。

【0116】

第１アンセンティッド変換部４２Ａの関数変換部４２Ａ２における、アンセンティッドカルマンフィルタの最初のステップは、状態方程式による状態量の予測と、観測方程式を用いた観測値の予測である。状態量をｘ、その共分散行列をＰ_Nとする。離散化された状態方程式ｆ_dを用いてＵＴを適用すると、下記の状態量の予測値、及び共分散行列の予測値が得られる。

【0117】

第１アンセンティッド変換部４２ＡでのＵＴ後の状態量と共分散行列と観測方程式ｈを用いて、第２アンセンティッド変換部４２ＢでさらにＵＴを適用すると、下記のような観測値の予測値、及び共分散行列の予測値が得られる。

【0118】

以上のように、状態量、観測値、及び共分散行列を予測する。ここまで、アンセンテッドカルマンフィルタを使った場合に実行される処理について説明した。しかしながら、状態空間に適用するフィルタに応じて異なる予測更新処理を実行してもよい。

【0119】

図１２は、センサ異常検知部４４の機能ブロック図の一例である。センサ異常検知部４４は、すべてのセンサの観測量及び誤差共分散に基づいて、各々のセンサの異常度を計算する異常度算出部４４Ａと、閾値に基づいて異常値を出力するセンサを検出する異常検出部４４Ｂと、異常値を出力するセンサを除外するように各種ベクトル及び行列を再構成する再構成部４４Ｃと、を備える。

【0120】

以下、異常度算出部４４Ａにおいて、一例として、あるセンサεについての異常度を計算する手法について説明する。あるセンサεの観測値ｙ_εが、下記のように観測値全体のｙの要素i であるとする。

【0121】

以下に、観測値ｙ_εの異常度を算出することを考える。前述した予測値からセンサεの要素を抽出する。

【0122】

異常度を下記のように定義する。下記はマハラノビス距離と呼ばれる。

【0123】

ａ_ε,k+1はホテリングのＴ²理論より、自由度ｎでスケール因子が１のχ²(u|n, 1)分布に従う。従って、χ²分布の累積分布によりａ_ε,k+1の発生確率ξ_εを算出することができる。

【0124】

以下、発生確率をセンサ毎に計算し、異常検出部４４Ｂは、発生確率が閾値を超えた場合に異常な値と判定する。正常なセンサの要素番号の集合をＭ_Eとした場合、以降のフィルタリングで用いる値を以下のように整理する。閾値は予め算出しておき、閾値が記述されたテーブル(マップ）を予め備えていてもよい。

【0125】

上記のように、センサの異常度ａ_ε,k+1の発生確率ξ_εが閾値を超えた場合は、ａ_ε,k＋1の出力に係るセンサを異常とみなし、当該センサの出力をフィルタリング部４６で使用しない。

【0126】

再構成部４４Ｃでは、異常検出部４４Ｂでの異常なセンサと正常なセンサとの識別後に、フィルタリング部４６で使用する各種ベクトル及び行列を整理する。

【0127】

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

【0128】

状態量の予測値を実際に観測された値でフィードバックする処理はカルマンゲイン(Kalman Gain)と呼ばれ、次式で計算される。以下では、カルマンフィルタのフィルタリング処理の例を説明する。なお、次式のＲ_Nは観測ノイズである誤差共分散行列Ｒ_Nである。具体的には、カルマンゲインを用いて、センサ値及び予測値の重み付けを行う。

【0129】

次に、このカルマンゲインを用いて、状態量の事前予測値を補正する処理を次のように行うと、最終的な状態量とその共分散行列は下記のようになる。

【0130】

以上の事前推定部４２及びフィルタリング部４６の処理を各タイムステップで繰り返すことにより、最終的な状態量に基づいてストラップダウン演算の補正量を推定する。結果的に、ＵＴにより、ストラップダウン演算の結果と補助センサによる計測データとの差分が最小となるように、ストラップダウン演算の補正量が推定される。

【0131】

更新部４８では、観測方程式の誤差共分散行列Ｒ_Nの更新を行う。更新部４８では、ある時間ｋ（例えば現在）から過去の観測量及び状態量を保存し、保存した観測量及び状態量を用いて誤差共分散行列Ｒ_Nを更新する。なお、状態空間に適用するフィルタに応じて上述とは異なるフィルタリング処理を実行してもよい。

【0132】

続いて、車両状態判定における、タイヤ位置での横速度ｖ_odom,yの拘束条件、観測ノイズを導出する。タイヤ横方向の速度に関する観測方程式は下記の式（３６）のようになる。

・・・（３６）

【0133】

車両運動に図７に示したような２輪モデルと線形タイヤモデルを仮定すると、車体横速度ｖ_yには下記の式（３７）のような関係がある。

・・・（３７）

【0134】

上記の式（３７）中のＣは正規化コーナリングスティフネスであり、一般的に１０～２５程度の値になる。ｖ_xは車両２００の前後速度である。コーナリングスティフネスは、積載や車体ロール角などの運動要因や路面状態の変化など、様々な要因で変化する。本実施形態では、上記の式（３７）を横速度の具体的な数値を計算するのに利用するのではなく、図１４に示したような横速度の発生確率分布を定義するのに利用する。図１４に示したように、本実施形態では、車両２００の左右方向の各々における横速度の範囲、換言すれば、横速度の絶対値の上限を規定している。

【0135】

さらに、下記の式（３８）のような観測ノイズを定義する。

・・・（３８）

【0136】

上記の式（３８）中のσ_vodom,yは正規分布における標準偏差を表す。観測ノイズは１σの値に相当するので、例えば３σの領域を設定する場合は、σ_vodom,y＝３とする。本式は線形タイヤモデルを仮定しているが、例えば非線形なブラッシュモデルやマジックフォーミュラを用いた立式でもよい。

【0137】

車両状態判定では、車両の状態に応じて横速度に応じた拘束を動的に切り替える。例えば、車両が大きく滑っている状況では、車両運動モデルに基づく拘束条件は適応できない。そこで、ストラップダウン演算から求められるスリップ角が閾値β_slipを超えるような場合には、下記の式（３９）に示したような処理を行う。

・・・（３９）

【0138】

横速度の標準偏差の最小値をμ^min _vodom,yとすると、車両の運動条件による標準偏差は下記の式（４０）のように定義される。式（４０）でまとめて示した最後の式は、上述の式（３８）と同じである。また、式（４０）におけるｍａｘは、２つの数値から大きい方が選択される関数である。式（４０）に示したように、車両の運動条件による標準偏差は、車両の前後速度と横加速度との積を所定の定数で除算して得た商に比例する。

・・・（４０）

【0139】

速度一定を仮定した場合のμ_vodom,yの値の変化の例を図１５に示した。図１５は、横軸に車両２００の横加速度、縦軸に横速度上限の確率標準偏差を設定しており、横速度上限の確率標準偏差が車両の運動状態によって変化することを示している。

【0140】

車両２００の運動による横加速度が大きい場合は、加速度ベクトルの方位精度が向上するので、車両方位補正の精度も向上する。しかし定常走行のような横加速度が小さい運動では、方位精度が低下しやすい。本実施形態では、横加速度が小さいときは横速度上限の確率標準偏差を下限閾値１１０に拘束し、横加速度大きいときは横速度上限の確率標準偏差を横加速度に対して線形的に変化するように拘束する。

【0141】

横方向の拘束条件はＩＭＵ２６の搭載方位オフセットδψ_imuの推定にも影響する。本実施形態では、横速度及び上下速度の各々の長い時間での平均が０になるように、ＩＭＵ２６の搭載姿勢角を推定するからである。具体的には、本来、０であるはずの横速度及び上下速度の各々の長い時間での平均が０以外の値を示すことに基づいて、ＩＭＵ２６のＸ、Ｙ、Ｚ軸と車両２００の進行方向とのオフセット及び、上下（ピッチ）方向のオフセット量を推定することができる。

【0142】

ただし、横速度上限の確率標準偏差が上限閾値１１２以上になる車両２００の横滑りが大きい場面では誤った推定を避けるためにμ_vodom,y＝∞として、横速度の上限を撤廃する。車両２００の横加速度の上限を横速度上限の確率標準偏差で拘束することは、ＩＭＵ２６の搭載オフセット誤差推定において、当該誤差の推定精度の改善に資する。

【0143】

続いて車両上下方向の拘束条件について説明する。車両２００又は車両２００の車輪位置での上下方向の速度変動量の最大値の拘束条件は、下記の式（４１）のようになる。

・・・（４１）

【0144】

特に上下速度については、車両の運動によって左右されない値なので、観測ノイズは固定値を用いる。バンプ乗り越えやオフロードも想定する場合は、下記の式（４２）を用いてもよい。式（４２）中のｋ_vodom,zは設定パラメータである。

・・・（４２）

【0145】

さらに、車速センサ２４によって車輪速情報が取得可能な場合は、下記の式（４３）で示した観測方程式を導出して使用する。

・・・（４３）

【0146】

車輪速情報が取得できない場合は、上記の式（４３）は用いない。本実施形態では、ＩＭＵ２６が最小構成のアルゴリズムなため、車輪速情報なしでも車両位置推定が可能となる。

【0147】

以上を踏まえて、センサ構成の違いによる観測方程式の扱いは下記のようになる。なお、下記の表に例示したように、センシングデータの内容に応じて異なるフィルタリング処理を実行してもよい。

【0148】

説明が前後したが、車両状態判定では、第１演算部６０Ａからの車体横滑り量等に基づいて、上述のように、非線形フィルタでの横速度拘束の適用可否を判定した。適用する場合は、横速度の確率標準偏差を前後速度と横加速度とから算出し、非線形フィルタの観測ノイズとして利用する。

【0149】

上述のように、非線形フィルタであるＵＴは、各種センサのデータ遅延時間の補償とＩＭＵ２６の取付オフセットの推定とを行う。各種センサの遅延時間は、ストラップダウン演算の結果と補助センサの計測結果との差分が最小になる場合の時刻同期サーバ５０によって同期された位置推定装置１４の時刻と、各々のセンサデータの時間との差分として把握する。さらにＩＭＵ２６の車体設置オフセット、ゼロ点オフセット、及びスケールファクタ誤差の推定を実施し、これらの誤差の補正量を推定する。

【0150】

第１演算部６０Ａは、ＵＴによって推定した補正量に基づいてストラップダウン演算の結果を補正する。そして、当該補正により誤差を排除した車両２００の３次元運動に基づいて、車両２００の速度、姿勢角、及び位置を推定する。

【0151】

補正したストラップダウン演算の結果に対してＩＭＵ遅延補償を行う。ＩＭＵ遅延補償では、ＩＭＵ２６の最新の値を用いて予測演算をする。ＩＭＵ２６の最新の値を参照するのは、本実施形態では、ＩＭＵ２６が、車両２００に搭載されているセンサの中で最も応答性が高いからである。ＩＭＵ遅延補償で用いるＩＭＵ２６の最新の値は、ストラップダウン演算によってＮ座標系に変換されているものとする。

【0152】

ＩＭＵ遅延補償において、遅延時間をδtとすると、テイラー展開によって、下記の一群の式が導出される。

【0153】

位置Ｒに係るストラップダウン演算の結果は２次のテイラー展開を、速度Ｖに係るストラップダウン演算の結果と姿勢角ｑに係るストラップダウン演算の結果の各々は１次のテイラー展開を各々行った。上記の式を用いて、遅延時間を考慮した予測演算を行い、当該予測演算結果を用いて車両２００の位置推定を行う。

【0154】

車両運動計測においては、ＩＭＵ２６ではなく車体の進行方向と速度ベクトルとを高精度で計測することが求められる。しかしながら、ＩＭＵ２６は取付穴やブラケットの位置、形状等の誤差によって十分な精度で車両に取り付けることが困難である。

【0155】

本実施形態では、車体進行方向の横方向と上下方向との各々の速度の長い時間の平均がゼロになることを利用して、第２演算部６０Ｂにおいて、ＩＭＵ２６の車両進行方向からのオフセット角度を推定する。ＩＭＵ２６の積分によって計算される上に高精度な横速度を用いるため、搭載角を０．１ｄｅｇ程度の精度で推定可能なのが最大の特徴である。

【0156】

例えば、図１７（Ａ）のように、ＩＭＵ２６が進行方向に対して左右方向にオフセットした姿勢で取り付けられているとすると、ＩＭＵ座標系で見ると常に横方向の速度が発生する。このような場合、図１７（Ｂ）に示したような、横速度の確率密度関数の平均がゼロからオフセットをする。このオフセット量を推定することにより、車体２００に対するＩＭＵ２６の搭載姿勢を推定することができる。

【0157】

同様の処理を上下速度に対しても実行し、ピッチ方向のオフセット量を推定することもできる。

【0158】

また、補助センサ２２、２４、２８、３０、３２からの出力信号は、通信の遅延、又はデータの１次処理等による遅延を伴って位置推定装置１４に入力される。一方でＩＭＵは１次処理の必要がないため最新のデータが得られる。図１８に示したように、ＩＭＵ２６から得られる車両運動の波形と、補助センサ２２、２４、２８、３０、３２から得られる車両運動の波形が最もよく重なるような時間オフセットを推定することによって、補助センサ２２、２４、２８、３０、３２の遅延時間を推定する。

【0159】

高精度なＩＭＵ２６による６自由度の運動演算を行っていることから、高い精度で遅延時間を推定することができ、その精度は２ｍｓ以内である。

【0160】

以上説明したように、本実施形態に係る車載システム１０によれば、６自由度の運動に拘束条件を設けないストラップダウン演算をベースに、車両運動特有の拘束条件を、車両の状態に応じて変動させることで、安価なＩＭＵでも多様な環境で精度よく車両運動を推定可能にする。

【0161】

具体的には、横速度の上限値の確率標準偏差を、車速と横加速度に関して比例増加するようにして拘束する。その際、横速度の最小値を設定すると共に、大きなスリップ時を想定して滑りが大きくなってきた段階で拘束をしないように上限値を無限大とする。

【0162】

安価なＩＭＵはゼロ点オフセットのみならず、スケールファクタ誤差が比較的大きい。本実施形態では、車両運動において大きな値を取るヨーレートと、ＩＭＵに含まれる前後・横加速度センサのスケールファクタの補正量とを、非線形フィルタであるＵＴを用いる事で推定する。かかる推定のよって、安価なＩＭＵを用いた場合でも精度よく運動計測が可能になる。

【0163】

また、ＧＳＮＮ又はＬＩＤＡＲ等の他のセンサを組み合わせた慣性センサの積分誤差の補正を行う際に、当該他のセンサは処理又は通信が要因で遅延している場合が少なくない。かかる遅延を含むデータについて、時刻を遡って評価をするアルゴリズムを導出することにより、現在の車両状態の推定値を最適化することが可能である。

【0164】

本実施形態によれば、特に不整地又は雪道等の環境での運動計測で、他の技術に比して大きな差が生じる。整地での走行を前提とした運動モデルでは演算が難しい不整地又は雪道において、ＩＭＵの６自由度の運動推定により、精度の良い自動制御の達成、又は車両の走行時の安全装置等への応用が期待される。

【0165】

いくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することを意図していない。これらの実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、様々な省略、置き換え、変更、実施形態同士の組み合わせを行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

【符号の説明】

【0166】

１０ 車載システム
１２ 入力装置
１４ 位置推定装置
１６ 表示装置
１８ 記憶装置
２０ 画像情報処理部
２２ 撮像装置
２４ 車速センサ
２８ 操舵角センサ
３４ 通信部
４２ 事前推定部
４４ センサ異常検知部
４６ フィルタリング部
４８ 更新部
５０ 時刻同期サーバ
６０Ａ 第１演算部
６０Ｂ 第２演算部
７０ ベースアンテナ
７２ ローバアンテナ
１１２ 上限閾値
２００ 車両

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】