特許 7272910 車載装置、状態推定方法及びプログラム

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

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

(11)【特許番号】

(24)【登録日】2023-05-01

(45)【発行日】2023-05-12

(54)【発明の名称】車載装置、状態推定方法及びプログラム

(51)【国際特許分類】

   G01C 21/28 20060101AFI20230502BHJP

【ＦＩ】

G01C21/28

【請求項の数】 6

(21)【出願番号】P 2019160184

(22)【出願日】2019-09-03

(65)【公開番号】P2021038989

(43)【公開日】2021-03-11

【審査請求日】2022-03-17

(73)【特許権者】

【識別番号】500578216

【氏名又は名称】株式会社ゼンリンデータコム

(74)【代理人】

【識別番号】100079108

【弁理士】

【氏名又は名称】稲葉 良幸

(74)【代理人】

【識別番号】100109346

【弁理士】

【氏名又は名称】大貫 敏史

(74)【代理人】

【識別番号】100117189

【弁理士】

【氏名又は名称】江口 昭彦

(74)【代理人】

【識別番号】100134120

【弁理士】

【氏名又は名称】内藤 和彦

(72)【発明者】

【氏名】岡田 和弘

【審査官】貞光 大樹

(56)【参考文献】

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

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

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

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

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

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

Ｇ０１Ｃ ２１／００ － ２１／３６

Ｇ０８Ｇ １／００ － １／１６

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

【請求項1】

車両に搭載される車載装置であって、
センサから取得される出力値に基づき、前記車両の状態に関する観測量を取得する取得部と、
車両の状態を示す状態量に関する誤差を示す係数を用いて対角成分を修正したヤコビアンを適用した拡張カルマンフィルタと、取得された前記車両の状態に関する観測量とに基づいて、前記車両の状態量を算出する推定部と、
を有する車載装置。

【請求項2】

前記車両の状態量には、前記車両の位置、前記車両の進行方向及び前記車両の進行方向における車速が含まれており、
前記係数は、第１軸方向における車両の位置誤差に関する第１係数と第２軸方向における車両の位置誤差に関する第２係数と前記車両の進行方向の誤差に関する第３係数とを含む、
請求項１に記載の車載装置。

【請求項3】

前記推定部は、
前記車両の前回の状態量と前記車両の状態方程式とに基づいて前記車両の状態量の予測値を算出し、
前回の誤差共分散行列と前記ヤコビアンとに基づき誤差共分散行列の予測値を算出し、
前記観測量と前記状態量の予測値との差分である観測残差を算出し、
前記誤差共分散行列の予測値に基づいてカルマンゲインを算出し、
前記車両の前回の状態量と前記観測残差と前記カルマンゲインとに基づいて、前記車両の状態量を算出し、
前記前回の誤差共分散行列と前記カルマンゲインとに基づいて、誤差共分散行列を算出する、
請求項２に記載の車載装置。

【請求項4】

前記第１係数は、前記前回の誤差共分散行列において前記第１軸方向の誤差の分散に対応する標準偏差と前記車両の進行方向における車速の誤差の分散に対応する標準偏差とに基づいて算出され、
前記第２係数は、前記前回の誤差共分散行列において前記第２軸方向の誤差の分散に対応する標準偏差と前記車両の進行方向における車速の誤差の分散に対応する標準偏差とに基づいて算出され、
前記第３係数は、前記前回の誤差共分散行列において前記車両の進行方向の誤差の分散に対応する標準偏差と前記車両の角速度の誤差の分散に対応する標準偏差とに基づいて算出される、
請求項３に記載の車載装置。

【請求項5】

車両に搭載される車載装置により実行される状態推定方法であって、
センサから取得される出力値に基づき、前記車両の状態に関する観測量を取得するステップと、
車両の状態を示す状態量に関する誤差を示す係数を用いて対角成分を修正したヤコビアンを適用した拡張カルマンフィルタと、取得された前記車両の状態に関する観測量とに基づいて、前記車両の状態量を算出するステップと、
を含む状態推定方法。

【請求項6】

車両に搭載されるコンピュータに、
センサから取得される出力値に基づき、前記車両の状態に関する観測量を取得するステップと、
車両の状態を示す状態量に関する誤差を示す係数を用いて対角成分を修正したヤコビアンを適用した拡張カルマンフィルタと、取得された前記車両の状態に関する観測量とに基づいて、前記車両の状態量を算出するステップと、
を実行させるためのプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、車載装置、状態推定方法及びプログラムに関する。

【背景技術】

【0002】

カーナビゲーション装置は、車両の速度を計測する車速センサから得られる車速と、車両に搭載されたジャイロセンサから得られる車両の角速度と、ＧＮＳＳ（Global Navigation Satellite System）から得られる車両の現在位置及び車両の進行方向に基づいて車両の現在位置を推定し、推定した現在位置を地図にマッピングすることで、運転者に車両の位置を伝えている。しかしながら、車速センサから得られる速度と、ジャイロセンサから得られる角速度には誤差が含まれることが一般的である。同様に、ＧＮＳＳから得られる現在位置についても、誤差が含まれることが一般的である。つまり、センサ及びＧＮＳＳの出力値からは、車両の真の位置を直接知ることはできない。

【0003】

このような問題を解決する手法として、カルマンフィルタが知られている。カルマンフィルタとは、誤差のある観測量を用いて、動的なシステムの状態を推定することが可能な手法である。カルマンフィルタをカーナビゲーション装置に利用した例として、例えば特許文献１に記載の技術が知られている。

【先行技術文献】

【特許文献】

【0004】

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

【発明の概要】

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

【0005】

しかしながら、車速センサ及びジャイロセンサからの出力から得られる速度や角速度等の観測量には、ある程度決まった誤差が含まれていることが多い。一方、カルマンフィルタは、誤差は白色雑音であるという前提で設計されている。そのため、従来のカルマンフィルタでは、車両の状態を高精度に推定することができない。

【0006】

そこで、本発明は、カルマンフィルタを用いて、車両の状態をより高精度に推定することを目的とする。

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

【0007】

本発明の一態様に係る車載装置は、車両に搭載される車載装置であって、センサから取得される出力値に基づき、車両の状態に関する観測量を取得する取得部と、車両の状態を示す状態量に関する誤差を示す係数を用いて修正したヤコビアンを適用した拡張カルマンフィルタと、取得された車両の状態に関する観測量とに基づいて、車両の状態量を算出する推定部と、を有する。

【発明の効果】

【0008】

本発明によれば、カルマンフィルタを用いて、車両の状態をより高精度に推定することができる。

【図面の簡単な説明】

【0009】

【図1】本実施形態に係る車載装置のハードウェア構成例を示す図である。

【図2】車載装置の機能ブロック構成例を示す図である。

【図3】車載装置が行う動作の一例を示すフローチャートである。

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

【0010】

添付図面を参照して、本発明の実施形態について説明する。なお、各図において、同一の符号を付したものは、同一又は同様の構成を有する。

【0011】

＜装置構成＞
図１は、本実施形態に係る車載装置１０のハードウェア構成例を示す図である。車載装置１０は、車両に搭載され、ユーザの操作に従って、地図の表示、地図における車両の現在位置の表示、目的地までの経路探索、経路案内等を実行する。車載装置１０は、プロセッサ１１と、記憶装置１２と、通信ＩＦ１３と、入力デバイス１４と、出力デバイス１５と、ＧＮＳＳ装置１６と、ジャイロセンサ１７とを含む。

【0012】

プロセッサ１１は、ＣＰＵ（Central Processing Unit）やＧＰＵ（Graphical processing unit）等であり、車載装置１０の各部を制御する。

【0013】

記憶装置１２は、メモリ、ＨＤＤ（Hard Disk Drive）及び／又はＳＳＤ（Solid State Drive）等であり、車載装置１０が動作するために必要な各種データを記憶する。

【0014】

通信ＩＦ１３は、例えば携帯電話機やスマートフォン等の通信機器と接続するためのインタフェースである。

【0015】

入力デバイス１４は、ユーザから各種の操作を受け付けるデバイスであり、車載装置１０が備えるスイッチ、ボタン、タッチパネル又はマイク等である。また、入力デバイス１４は、車載装置１０が搭載された車両に取り付けられた車速センサ１８から出力された車速データの入力を受け付ける。

【0016】

出力デバイス１５は、例えば、ディスプレイ及び／又はスピーカ等である。

【0017】

ＧＮＳＳ装置１６は、ＧＮＳＳ衛星から送信されるＧＮＳＳ信号に基づき、車載装置１０の位置と、車両の進行方向の方位（以下、「車両の方位」と言う。）とを測定する。なお、ＧＮＳＳにはＧＰＳも含まれる。

【0018】

ジャイロセンサ１７は、例えば振動ジャイロセンサであり、車両の回転速度、すなわち車両の角速度を検出する。

【0019】

＜機能ブロック構成＞
図２は、車載装置１０の機能ブロック構成例を示す図である。車載装置１０は、記憶部１０１と、取得部１０２と、推定部１０３とを含む。記憶部１０１は、車載装置１０が備える記憶装置１２を用いて実現することができる。また、取得部１０２と、推定部１０３とは、車載装置１０のプロセッサ１１が、記憶装置１２に記憶されたプログラムを実行することにより実現することができる。また、当該プログラムは、記憶媒体に格納することができる。当該プログラムを格納した記憶媒体は、コンピュータ読み取り可能な非一時的な記憶媒体（Non-transitory computer readable medium）であってもよい。非一時的な記憶媒体は特に限定されないが、例えば、ＵＳＢメモリ又はＣＤ－ＲＯＭ等の記憶媒体であってもよい。

【0020】

記憶部１０１は、地図データ１０１ａを記憶する。

【0021】

取得部１０２は、センサから取得される出力値に基づき、車両の状態に関する観測量を取得する。車両の状態に関する観測量には、車両の進行方向における速度（以下、「車両の速度」と言う。）と、車両の角速度と、車両の位置と、車両の進行方向とが含まれる。より具体的には、取得部１０２は、ＧＮＳＳ装置１６で観測された、車両の位置と車両の進行方向とを取得し、ジャイロセンサ１７で観測された車両の角速度を取得し、車速センサ１８で観測された車両の速度を取得する。

【0022】

推定部１０３は、車両の状態を示す状態量に関する誤差を示す係数を用いて修正したヤコビアン（以下、「修正ヤコビアン」と言う。）を適用した拡張カルマンフィルタと、車両の状態に関する観測量とに基づいて、車両の状態を示す状態量（以下、「車両の状態量」と言う。）を算出する（推定する）。車両の状態量には、車両の速度と、車両の角速度と、車両の位置と、車両の進行方向とが含まれる。なお、修正ヤコビアンについては後述する。

【0023】

＜拡張カルマンフィルタを用いた車両の状態量の推定＞
まず、拡張カルマンフィルタにより推定する車両の状態量を以下のように定義する。

【0024】

ｖ：車両の速度
ω：車両の角速度
ｘ：車両の位置のｘ座標
ｙ：車両の位置のｙ座標
θ：車両の方位
ここで、車両の状態量をベクトル表記した状態ベクトルを、（ｖ、ω、ｘ、ｙ、θ）と
すると、車両の状態量についての状態方程式は、式（１）で表すことができる。

【数1】

式（１）において、添え字のｔは時刻を示す。例えば、式（１）の左辺は、時刻ｔにおける車両の状態量を示しており、右辺は、時刻ｔ－１における車両の状態量を示している。ｄｔは、所定の単位時間を示しており、車載装置１０が、拡張カルマンフィルタを用いて車両の状態量を推定する周期の間隔に相当する。当該間隔は、例えば、取得部１０２が、車両の状態に関する観測量を取得する間隔であってもよい。右辺の第２項であるｑ_tは、平均が０であり誤差共分散行列Ｑ_tである正規分布Ｎ（０、Ｑ_t）に従うシステム雑音である。

【0025】

式（１）に示す状態方程式によれば、時刻ｔにおける車両の位置のｘ座標は、時刻ｔ－１における車両の位置に、車両の速度のｘ軸方向成分に単位時間を乗算した値を加えることで算出されることを示している。また、時刻ｔにおける車両の位置のｙ座標は、時刻ｔ－１における車両の位置に、車両の速度のｙ軸方向成分に単位時間を乗算した値を加えることで算出されることを示している。また、時刻ｔにおける車両の方位は、時刻ｔ－１における車両の方位に、車両の角速度に単位時間を乗算した角度を加えることで算出されることを示している。

【0026】

次に、取得部１０２が取得する車両の状態に関する観測量を以下のように定義する。

【0027】

ｖ_pls：車速センサ１８の出力（パルスデータ等）から得られる車両の速度
ω_gyro：ジャイロセンサ１７の出力から観測される車両の角速度
ｘ_gps：ＧＮＳＳ装置１６の測位結果から観測される車両の位置のｘ座標
ｙ_gps：ＧＮＳＳ装置１６の測位結果から観測される車両の位置のｙ座標
θ_gps：ＧＮＳＳ装置１６の測位結果から観測される車両の方位
ここで、上記の観測量をベクトル表記した観測ベクトルを（ｖ_pls、ω_gyro、ｘ_gps、ｙ_gps、θ_gps）とすると、観測量についての観測方程式は、式（２）で表すことができる。右辺の第２項であるｒ_tは、平均が０であり誤差共分散行列Ｒ_tである正規分布Ｎ（０、Ｒ_t）に従う観測雑音である。

【数2】

【0028】

続いて、拡張カルマンフィルタが行う処理について、車両の状態量を予測する「予測処理」と、車両の状態量を推定する「推定処理」とに分けて説明する。拡張カルマンフィルタは、車両の状態量の推定を、ｄｔの周期で繰り返し行う。予測処理とは、前回の周期で推定された車両の状態量と状態方程式とを用いて今回の周期における車両の状態量を予測することである。また、推定処理とは、予測した今回の周期における車両の状態量を、今回の周期で取得された車両の状態に関する観測量で補正することで、車両の状態量をより高精度に推定することである。

【0029】

なお、以下の説明において、添え字のｔ｜ｔ－１が付与された値は、時刻ｔ－１までの情報に基づき算出された時刻ｔにおける予測値（今回の予測値）を示す。また、添え字のｔ｜ｔが付与された値は、時刻ｔまでの情報に基づき算出された時刻ｔにおける推定値（今回の推定値）を示す。また、添え字のｔ－１｜ｔ－１が付与された値は、時刻ｔ－１までの情報に基づき推定された時刻ｔ－１における推定値（前回の推定値）を示す。

【0030】

（予測処理）
拡張カルマンフィルタにおける予測処理は、より詳細には、車両の状態量の予測値（以下、「車両状態予測値」と言う。）と、誤差共分散行列の予測値と、を算出する処理である。車両状態予測値を算出する式を式（３）に示す。

【数3】

式（３）の左辺は、時刻ｔ－１までの情報に基づき予測された時刻ｔにおける車両状態予測値を示す。例えば、車両の位置のｘ座標の予測値を示すｘ_t|t-1は、時刻ｔ－１までの情報に基づき推定された時刻ｔ－１における、車両の位置のｘ座標（ｘ_t-1|t-1）の推定値と、車両の方位（θ_t-1|t-1）の推定値と、車両の速度（ｖ_t-1|t-1）の推定値とに基づき算出される。

【0031】

誤差共分散行列の予測値、つまり、時刻ｔ－１までの情報に基づき予測された時刻ｔにおける誤差共分散行列Ｐ_t|t-1を算出する式を、式（４）に示す。なお、分散は、誤差を二乗した値を平均した値である。つまり、車両の状態量の分散は、車両の状態量の誤差を二乗した値を平均した値である。したがって、誤差共分散行列の予測値を算出することは、車両状態予測値の誤差を算出することに相当する。

【数4】

式（４）の左辺に示す誤差共分散行列は、時刻ｔ－１までの情報に基づき推定された時刻ｔ－１における誤差共分散行列に基づき算出される。なお、Ｆ_t-1は、式（１）の状態方程式から求められる、時刻ｔ－１におけるヤコビアン（ヤコビ行列とも言う）を示す。また、Ｆにおける添え字のＴは、転置行列を示す。また、Ｑは、時刻ｔ－１におけるシステム雑音の誤差共分散行列を示す。

【0032】

このように、予測処理では、時刻ｔ－１までの情報に基づき算出された時刻ｔにおける車両状態予測値と、時刻ｔ－１までの情報に基づき算出された時刻ｔにおける誤差共分散行列の予測値とを算出する。

【0033】

（推定処理）
拡張カルマンフィルタにおける推定処理は、予測処理にて算出した車両状態予測値、及び、誤差共分散行列の予測値に基づいて、車両の状態量の推定値（以下、「車両状態推定値」と表現する）と、誤差共分散行列の推定値とを算出する処理である。

【0034】

まず、観測残差を算出する式を式（５）に示す。

【数5】

式（５）において、左辺は、観測残差をベクトル表記した観測残差ベクトルである。また、式（５）において、右辺の第１項は、時刻ｔにおける観測対象ベクトルである。また、式（５）において、右辺の第２項は、予測処理で予測した車両状態予測値である。つまり、観測残差とは、車両の状態に関する観測量と、車両状態予測値から算出される車両状態推定値との差分である。

【0035】

次に、車両状態推定値は、式（５）に示す観測残差を用いて、式（６）により算出される。

【数6】

式（６）の左辺は、時刻ｔまでの情報に基づき推定された時刻ｔにおける車両状態推定値を示す。車両状態推定値は、式（６）の右辺に示すように、時刻ｔ－１までの情報に基づき推定された時刻ｔにおける車両状態予測値を、観測残差で補正することで算出される。ここで、車両状態予測値を観測残差にて補正する際の補正係数としてＫ_tが用いられる。Ｋ_tは、カルマンゲインと呼ばれ、式（７）で表される。

【数7】

式（７）において、Ｒ_tは、観測雑音の誤差共分散行列である。また、添え字の「－１
」は、逆行列を示す。式（７）に示すカルマンゲインＫ_tは、時刻ｔ－１までの情報に基づく時刻ｔにおける誤差共分散行列の予測値Ｐ_t|t-1に基づき算出される。

【0036】

式（６）において、カルマンゲインＫ_tは、車両状態推定値を算出する際、車両状態予測値を重視するか、車両の状態に関する観測量を重視して算出するかを決定付ける係数である。例えば、ＧＮＳＳ装置１６、ジャイロセンサ１７及び車速センサ１８から得られる観測量の誤差が充分に小さい場合、つまり車両の状態に関する観測量が信頼できる値である場合、車両状態推定値は観測量に近い値になることが望ましい。

【0037】

一方、車両状態予測値の誤差が、ＧＮＳＳ装置１６、ジャイロセンサ１７及び車速センサ１８からの出力に基づく観測量より充分に小さい場合、つまり、車両の状態に関する観測量があまり信頼できない場合、車両状態推定値は、車両状態予測値に近い値になることが望ましい。例えば、カルマンゲインＫ_k＝０として式（６）に与えると、車両状態推定値は、車両状態予測値と等しくなる。このように、カルマンゲインＫ_tは、車両状態推定値が適切な値となるように設定する係数である。

【0038】

次に、誤差共分散行列の推定値、つまり、時刻ｔまでの情報に基づき推定された時刻ｔにおける誤差共分散行列Ｐｔ｜ｔは、式（８）により算出される。

【数8】

式（８）において、Ｉは、単位行列を示す。Ｋ_tは、式（７）で算出された、時刻ｔにおけるカルマンゲインである。Ｐ_t|t-1は、時刻ｔ－１までの情報に基づき予測された時刻ｔにおける誤差共分散行列（つまり、式（４）で算出された誤差共分散行列）である。

【0039】

以上説明した拡張カルマンフィルタの処理手順に基づき推定部１０３が行う処理は、以下のように説明することができる。

【0040】

（１）車両の前回の状態量と車両の状態方程式とに基づいて車両の状態量の予測値を算出し、前回の誤差共分散行列とヤコビアンとに基づき誤差共分散行列の予測値を算出する
（２）観測量と状態量の予測値との差分である観測残差を算出
（３）誤差共分散行列の予測値に基づいてカルマンゲインを算出
（４）車両の前回の状態量と観測残差とカルマンゲインとに基づいて、車両の状態量を算出
（５）前回の誤差共分散行列とカルマンゲインとに基づいて、誤差共分散行列を算出

【0041】

カルマンフィルタでは、推定された車両の状態量の精度は、カルマンゲインＫ_tに依存する。カルマンゲインＫ_tが正しくない場合、例えば、車両の状態に関する観測量が信頼できないにも関わらず、車両状態推定値が観測量に近くなるよう補正してしまうといったことが生じ得る。

【0042】

拡張カルマンフィルタは、システム雑音、及び、観測雑音が白色性の雑音であることを前提としている。つまり、拡張カルマンフィルタでは、誤差の平均が０であることが前提でなる。しかしながら、現実の世界では、ジャイロセンサ１７及び車速センサ１８から得られる値に含まれる誤差は、平均が０になる白色性の雑音ではなく、常にオフセットが掛かったような誤差であることが多い。つまり、ジャイロセンサ１７及び車速センサ１８から得られる値を平均すると、誤差の平均が０にならずに徐々に累積していくことが多い。

【0043】

このような誤差を有する観測量に対して拡張カルマンフィルタを使用すると、誤差共分散行列を適切に算出することができなくなる。また、その結果、カルマンゲインＫ_tも正確に算出されないため、車両の状態量を精度よく推定できなくなってしまうという問題がある。

【0044】

そこで、本実施形態では、状態方程式に対応するヤコビアンを、車両の状態量に関する誤差を示す係数を用いて修正した修正ヤコビアンを用いることで、車両の状態量をより高精度に算出する。

【0045】

まず、従来のカルマンフィルタの考え方に基づいて算出される、式（１）の状態法定式に対応するヤコビアンを式（９）に示す。以下の説明では、式（９）に示すヤコビアンを、「従来のヤコビアン」を呼ぶことがある。

【数9】

式（９）に示す行列のうち、１行１列目から１行５列目までは、式（１）に示す状態法定式の右辺の縦ベクトルにおける行目を、それぞれｖ、ω、ｘ、ｙ及びθで微分した値である。２行１列目から２行５列目までは、式（１）に示す状態法定式の右辺の縦ベクトルにおける２行目を、それぞれｖ、ω、ｘ、ｙ及びθで微分した値である。３行１列目から３行５列目までは、式（１）に示す状態法定式の右辺の縦ベクトルにおける３行目を、それぞれｖ、ω、ｘ、ｙ及びθで微分した値である。４行１列目から４行５列目までは、式（１）に示す状態法定式の右辺の縦ベクトルにおける４行目を、それぞれｖ、ω、ｘ、ｙ及びθで微分した値である。５行１列目から５行５列目までは、式（１）に示す状態法定式の右辺の縦ベクトルにおける５行目を、それぞれｖ、ω、ｘ、ｙ及びθで微分した値である。

【0046】

次に、本実施形態で用いる修正ヤコビアンを、式（１０）に示す。

【数10】

また、ｄｘ、ｄｙ及びｄθの値を、それぞれ、式（１１）、式（１２）及び式（１３）に示す。

【数11】

【数12】

【数13】

式（１１）に示す係数ｄｘ（第１係数）は、車両の位置のうちｘ軸（第１軸）方向における車両の位置誤差を示す。式（１２）に示す係数ｄｙ（第２係数）は、車両の位置のうちｙ軸（第１軸）方向における車両の位置誤差を示す。式（１３）に示す係数ｄθ（第３係数）は、車両の位置のうち進行方向における車両の位置誤差を示す。

【0047】

より具体的には、式（１１）に示す係数（第１係数）は、前回の誤差共分散行列の推定値において第１軸方向の誤差の分散に対応する標準偏差と車両の進行方向における車速の誤差の分散に対応する標準偏差とに基づいて算出される。

【0048】

式（１２）に示す係数（第２係数）は、前回の誤差共分散行列の推定値において第２軸方向の誤差の分散に対応する標準偏差と車両の進行方向における車速の誤差の分散に対応する標準偏差とに基づいて算出される。

【0049】

式（１３）に示す係数（第３係数）は、前回の誤差共分散行列の推定値において車両の進行方向の誤差の分散に対応する標準偏差と車両の角速度の誤差の分散に対応する標準偏差とに基づいて算出される。

【0050】

ここで、誤差共分散行列の各成分の例を式（１４）に示す。

【数14】

式（１４）に示すように、誤差共分散行列の対角成分は、車両の位置、車両の方位、車両の速度、及び、車両の角速度に関する各々の分散である。また、対角成分以外の成分は、車両の位置、車両の方位、車両の速度、及び、車両の角速度のうち２つの組み合わせに関する共分散である。なお、分散は、誤差の散らばり具合を示しており、共分散は、２種類の車両の状態量における相関関係を示している。

【0051】

そこで、推定部１０３は、式（４）を用いて誤差共分散行列を計算する際、ヤコビアンＦ_t-1を、式（１０）に示す修正ヤコビアンに置き換えてカルマンゲインを計算する。このとき、推定部１０３は、ｖの誤差の標準偏差、ωの誤差の標準偏差、ｘの誤差の標準偏差、ｙの誤差の標準偏差、及び、θの誤差の標準偏差を、それぞれ、式（４）における誤差共分散行列Ｐ_t-1|t-1における１行１列目の成分、２行２列目の成分、３行３列目の成分、４行４列目の成分、及び、５行５列目の成分から取得し、式（１１）～式（１３）に代入する。

【0052】

修正ヤコビアンＦ_t|t-1の３行目３列目、４行目４列目及び５行目５列目の成分に車両の状態量に関する誤差を示す係数が設定されることで、式（９）に示すヤコビアンを用いる場合と比較して、式（４）により算出される誤差共分散行列Ｐ_t|t-1における、車両位置のｘ座標の分散、ｙ座標の分散及びθの分散に該当する３行３列目の成分、４行４列目の成分、及び、５行５列目の対角成分の値が増加することになる。また、式（７）において、当該対角成分の値が増加した誤差共分散行列Ｐ_t|t-1を用いてカルマンゲインＫ_tを算出することで、カルマンゲインＫ_tにおける３行目３列目、４行目４列目及び５行目５列目の対角成分の値も増加する。そうすると、式（６）において、本実施形態に係るカルマンフィルタを用いて時刻ｔにおける車両の状態量を推定する場合、平均が０になる白色性の雑音が含まれることを前提とする従来のカルマンフィルタを用いる場合と比較して、車両の状態量のうちｘ座標、ｙ座標及びθについては、車両状態予測値よりも、ＧＮＳＳから得られる観測量が多く反映されることになる。

【0053】

これにより、白色性の雑音ではなく、常にオフセットが掛かったような誤差を有するジャイロセンサ１７及び車速センサ１８を用いて車両の状態を推定する場合、ｘ座標、ｙ座標及びθについては、ＧＮＳＳから取得した車両位置（ｘ、ｙ、θ）による補正が多く行われることになるため、車両位置をより高精度に推定することが可能になる。

【0054】

＜車載装置が行う動作＞
図３は、車載装置１０が行う動作の一例を示すフローチャートである。まず、取得部１０２は、ＧＮＳＳ装置１６、ジャイロセンサ１７及び車速センサ１８から、車両の速度、車両の角速度、車両の位置、及び、車両の方位に関する観測量を取得する（Ｓ１０）。続いて、推定部１０３は、式（１０）～式（１３）に示すヤコビアンに置き換えたカルマンフィルタを用いて、車両の速度、車両の角速度、車両の位置、及び、車両の方位車両の状態量を推定する（Ｓ１１）。続いて、表示制御部１０４は、推定部１０３で推定された車両の位置及び車両の方位に基づいて、地図上に描画されている車両の位置及び車両の向きを更新する（Ｓ１２）。車両位置の更新を終了する場合（例えばナビゲーション画面の表示を終了する場合）は処理を終了する。車両位置の更新を続ける場合はステップＳ１０の処理手順に戻る。

【0055】

＜まとめ＞
以上説明した実施形態によれば、車両の状態量に関する誤差を示す係数を含むヤコビアンを用いることで、白色性の雑音ではなく平均が０にならないような誤差を有するセンサを用いる場合であっても、より高精度に車両の位置を推定することが可能になる。

【0056】

以上説明した実施形態は、本発明の理解を容易にするためのものであり、本発明を限定して解釈するためのものではない。実施形態で説明したフローチャート、シーケンス、実施形態が備える各要素並びにその配置、材料、条件、形状及びサイズ等は、例示したものに限定されるわけではなく適宜変更することができる。また、異なる実施形態で示した構成同士を部分的に置換し又は組み合わせることが可能である。

【符号の説明】

【0057】

１０…車載装置、１１…プロセッサ、１２…記憶装置、１３…通信ＩＦ、１４…入力デバイス、１５…出力デバイス、１６…ＧＮＳＳ装置、１７…ジャイロセンサ、１８…車速センサ、１０１…記憶部、１０１ａ…地図データ、１０２…取得部、１０３…推定部、１０４…表示制御部

【図1】

【図2】

【図3】