特許 6677533 車載装置、及び、推定方法

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

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

(11)【特許番号】6677533

(24)【登録日】2020年3月17日

(45)【発行日】2020年4月8日

(54)【発明の名称】車載装置、及び、推定方法

(51)【国際特許分類】

   G01C 21/28 20060101AFI20200330BHJP

【ＦＩ】

   G01C21/28

【請求項の数】10

【全頁数】25

(21)【出願番号】特願2016-38740(P2016-38740)

(22)【出願日】2016年3月1日

(65)【公開番号】特開2017-156186(P2017-156186A)

(43)【公開日】2017年9月7日

【審査請求日】2018年12月25日

(73)【特許権者】

【識別番号】000001487

【氏名又は名称】クラリオン株式会社

(74)【代理人】

【識別番号】110001081

【氏名又は名称】特許業務法人クシブチ国際特許事務所

(72)【発明者】

【氏名】岡田 和弘

【審査官】 上野 博史

(56)【参考文献】

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

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

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

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

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

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

２３／００−２５／００

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

【請求項1】

車両に搭載される車載装置であって、
センサーからの出力に基づき、前記車両の変動に関する観測量を観測する観測部と、
カルマンフィルタにより、前記車両の状態を示す状態量を推定する推定部と、を備え、
前記推定部は、
前記車両の前記状態量の予測値を算出し、
前記観測量と微積分の関係にある前記状態量の誤差として前記観測量の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出し、
算出した前記予測値と前記予測値の誤差とに基づいて、前記カルマンフィルタにより前記車両の前記状態量の推定値と前記推定値の誤差とを算出する、
ことを特徴とする車載装置。

【請求項2】

前記推定部は、
前回算出した前記状態量の前記推定値の誤差と、前記状態量の誤差として入力された前記観測量の誤差との共分散が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
ことを特徴とする請求項１に記載の車載装置。

【請求項3】

前記観測部は、前記車両の速度を検出する車速センサーからの出力に基づき、前記車両の速度を観測し、
前記推定部は、前記車両の位置の誤差として前記車両の速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
ことを特徴とする請求項１又は２に記載の車載装置。

【請求項4】

前記観測部は、前記車両の角速度を検出する角速度センサーからの出力に基づき、前記車両の角速度を観測し、
前記推定部は、前記車両の方位の誤差として前記車両の角速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
ことを特徴とする請求項１又は２に記載の車載装置。

【請求項5】

前記観測部は、前記車両の加速度を検出する加速度センサーからの出力に基づき、前記車両の加速度を観測し、
前記推定部は、前記車両の速度の誤差として前記車両の加速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
ことを特徴とする請求項１又は２に記載の車載装置。

【請求項6】

車両の状態を示す状態量をカルマンフィルタにより推定する推定方法であって、
前記車両に搭載される車載装置が、
前記車両の前記状態量の予測値を算出し、
センサーからの出力に基づき観測された前記車両の変動に関する観測量と微積分の関係にある前記状態量の誤差として、前記観測量の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出し、
算出した前記予測値と前記予測値の誤差とに基づいて、前記カルマンフィルタにより前記車両の前記状態量の推定値と前記推定値の誤差とを算出する、
ことを特徴とする推定方法。

【請求項7】

前記車載装置が、
前回算出した前記状態量の前記推定値の誤差と、前記状態量の誤差として入力された前記観測量の誤差との共分散が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
ことを特徴とする請求項６に記載の推定方法。

【請求項8】

前記車載装置が、
前記車両の速度を検出する車速センサーからの出力に基づき、前記車両の速度を観測し、
前記車両の位置の誤差として前記車両の速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
ことを特徴とする請求項６又は７に記載の推定方法。

【請求項9】

前記車載装置が、
前記車両の角速度を検出する角速度センサーからの出力に基づき、前記車両の角速度を観測し、
前記車両の方位の誤差として前記車両の角速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
ことを特徴とする請求項６又は７に記載の推定方法。

【請求項10】

前記車載装置が、
前記車両の加速度を検出する加速度センサーからの出力に基づき、前記車両の加速度を観測し、
前記車両の速度の誤差として前記車両の加速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
ことを特徴とする請求項６又は７に記載の推定方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、車載装置、及び、推定方法に関する。

【背景技術】

【0002】

本技術分野の背景技術として、特開２０００−５５６７８号公報（特許文献１）がある。特許文献１には、「車速センサ４、ジャイロ６での検出値から移動距離演算部１１、方位変化量演算部１２にて算出される移動距離、方位変化量に基づき、相対軌跡演算部１３、絶対位置演算部１４が算出する推定航法データ（車速、絶対方位、絶対位置）と、ＧＰＳ受信機８からのＧＰＳ測位データ（速度、方位、位置）との差を観測値とするカルマンフィルタ（誤差推定部１５）を備え、その状態量の一つとして、ジャイロ出力から角速度への変換ゲインの誤差（ゲイン誤差）を設定する。このカルマンフィルタにより求められるゲイン誤差の推定値により、ジャイロ出力及び変換ゲインを用いて算出された方位変化量を補正する。」と記載されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０００−５５６７８号公報

【発明の概要】

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

【0004】

ところで、車速パルスやジャイロ等のセンサーからの出力に基づき観測される速度や角速度等の観測量から求められる移動距離や方位変化量は、時間の経過により誤差が累積する場合がある。特許文献１では、累積する誤差を考慮したカルマンフィルタにより車両の状態を推定する技術について開示が無く、カルマンフィルタにより車両の状態を精度よく推定できない。
そこで、本発明は、カルマンフィルタにより車両の状態を精度よく推定できるようにすることを目的とする。

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

【0005】

上記目的を達成するために、車載装置は、車両に搭載される車載装置であって、センサーからの出力に基づき、前記車両の変動に関する観測量を観測する観測部と、カルマンフィルタにより、前記車両の状態を示す状態量を推定する制御部と、を備え、前記制御部は、前記車両の前記状態量の予測値を算出し、前記観測量と微積分の関係にある前記状態量の誤差として前記観測量の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出し、算出した前記予測値と前記予測値の誤差とに基づいて、前記カルマンフィルタにより前記車両の前記状態量の推定値と前記推定値の誤差とを算出することを特徴とする。

【発明の効果】

【0006】

本発明によれば、カルマンフィルタにより車両の状態を精度よく推定できる。

【図面の簡単な説明】

【0007】

【図1】ナビゲーション装置の構成を示すブロック図である。

【図2】ナビゲーション装置の動作を示すフローチャートである。

【図3】車両状態推定処理における推定部の動作を示すフローチャートである。

【図4】推定した車両の状態に基づくマップマッチングの処理を説明するための図である。

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

【0008】

図１は、ナビゲーション装置１（車載装置）の構成を示すブロック図である。

【0009】

ナビゲーション装置１は、車両に搭載される車載装置であり、車両に搭乗しているユーザの操作に従って、地図の表示や、地図における車両の現在位置の表示、目的地までの経路探索、経路案内等を実行する。なお、ナビゲーション装置１は、車両のダッシュボード等に固定されてもよいし、車両に対し着脱可能なものであっても良い。

【0010】

図１に示すように、ナビゲーション装置１は、制御部２と、記憶部３と、タッチパネル４と、ＧＰＳ受信部５と、車速センサー６（センサー）と、ジャイロセンサー７（角速度センサー、センサー）と、加速度センサー８（センサー）とを備える。

【0011】

制御部２は、ＣＰＵや、ＲＯＭ、ＲＡＭ、その他の制御回路等を備え、ナビゲーション装置１の各部を制御する。制御部２は、ＲＯＭや記憶部３等に記憶された制御プログラムを実行することにより、後述する観測部２１、及び、推定部２２として機能する。

【0012】

記憶部３は、ハードディスクや、ＥＥＰＲＯＭ等の不揮発性メモリを備え、データを書き換え可能に記憶する。記憶部３は、制御部２が実行する制御プログラムのほか、地図データ３ａを記憶する。地図データ３ａは、交差点やその他の道路網上の接続点を示すノードに関するノード情報や、ノードとノードとの道路区間を示すリンクに関するリンク情報、地図の表示に係る情報等を有する。リンク情報は、リンク毎に、少なくとも、リンクの位置に関する情報と、リンクの方位に関する情報とを含む。

【0013】

タッチパネル４は、表示パネル４ａと、タッチセンサー４ｂとを備える。表示パネル４ａは、液晶ディスプレイやＥＬ（Electro Luminescent）ディスプレイ等により構成され、制御部２の制御の下、各種情報を表示パネル４ａに表示する。タッチセンサー４ｂは、表示パネル４ａに重ねて配置され、ユーザのタッチ操作を検出し、タッチ操作された位置を示す信号を制御部２に出力する。制御部２は、タッチセンサー４ｂからの入力に基づいて、タッチ操作に対応する処理を実行する。

【0014】

ＧＰＳ受信部５は、ＧＰＳアンテナ５ａを介してＧＰＳ衛星からのＧＰＳ電波を受信し、ＧＰＳ電波に重畳されたＧＰＳ信号から、車両の位置と、車両の進行方向の方角（以下、「車両の方位」と表現する）とを演算により取得する。ＧＰＳ受信部５は、車両の位置を示す情報、及び、車両の方位を示す情報を制御部２に出力する。

【0015】

車速センサー６は、車両の車速を検出して、検出した車速を示す信号を制御部２に出力する。

【0016】

ジャイロセンサー７は、例えば振動ジャイロにより構成され、車両の回転による角速度を検出する。ジャイロセンサー７は、検出した角速度を示す信号を制御部２に出力する。

【0017】

加速度センサー８は、車両に作用する加速度（例えば、進行方向に対する車両の傾き）を検出する。加速度センサー８は、検出した加速度を示す信号を制御部２に出力する。

【0018】

図１に示すように、制御部２は、観測部２１と、推定部２２とを備える。

【0019】

観測部２１は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８から出力される信号に基づき、車両の変動に関する観測量を観測する。本実施形態では、観測部２１は、車速センサー６から出力される車速を示す信号に基づき、観測量として車両の速度（観測量）を観測する。観測部２１は、所定の演算により、車速を示す信号から、車両の速度と車両の速度の誤差とを算出する。この算出された車両の速度の誤差は、この速度から求めた移動距離に累積する誤差である。

【0020】

また、観測部２１は、ジャイロセンサー７から出力される角速度を示す信号に基づき、観測量として車両の回転による角速度（観測量）を観測する。観測部２１は、所定の演算により、角速度を示す信号から、車両の角速度と車両の角速度の誤差とを算出する。この算出された車両の角速度の誤差は、この角速度から求めた方位変化量に累積する誤差である。

【0021】

また、観測部２１は、加速度センサー８から出力される加速度（観測量）を示す信号に基づき、観測量として車両の加速度を観測する。観測部２１は、所定の演算により、加速度を示す信号から、車両の加速度と車両の加速度の誤差とを算出する。この算出された車両の加速度の誤差は、この加速度から求めた速度に累積する誤差である。

【0022】

また、観測部２１は、ＧＰＳ受信部５から出力された車両の位置を示す情報と車両の方位を示す情報とに基づき、車両の位置と車両の方位とを観測する。

【0023】

推定部２２は、観測部２１が観測する車両の速度、車両の加速度、車両の角速度、車両の位置、及び、車両の方位に基づき、カルマンフィルタにより、車両の状態を示す状態量を推定する。本実施形態では、推定部２２は、車両の位置、車両の方位、車両の速度、及び、車両の角速度を、車両の状態量として推定する。

【0024】

後述するが、制御部２は、推定部２２が推定した車両の状態量に基づき、マップマッチングの対象となるリンクの評価を実行する。

【0025】

なお、推定部２２が推定する車両の速度は、車両の状態量に相当する。また、観測部２１が観測する車両の速度は、観測量に相当する。同様に、推定部２２が推定する車両の角速度は、車両の状態量に相当する。また、観測部２１が観測する車両の角速度は、観測量に相当する。

【0026】

ここで、カルマンフィルタによる基本的な車両の状態量の推定について説明する。

【0027】

本実施形態では、カルマンフィルタにより推定する車両の状態量は、車両の位置、車両の方位、車両の速度、及び、車両の角速度である。以下に、推定する車両の各状態量を示す。

【0028】

ｘ：車両の位置のｘ座標
ｙ：車両の位置のｙ座標
θ：車両の方位
ｖ：車両の速度
ω：車両の角速度

【0029】

ここで、車両の状態量をベクトル表記した状態ベクトルを、（ｘ、ｙ、θ、ｖ、ω）とすると、車両の状態量についての状態方程式は、式（１）で表される。

【0030】

【数1】

【0031】

添え字のｋは、時刻を示す。例えば、式（１）の左辺である（ｘ_ｋ＋１、ｙ_ｋ＋１、θ_ｋ＋１、ｖ_ｋ＋１、ω_ｋ＋１）は、時刻ｋ＋１における車両の状態量を示す。式（１）において、右辺の第２項であるｑ_ｋは、システム雑音（平均０、誤差共分散行列であるＱ_ｋを有する正規分布Ｎ（０、Ｑ_ｋ））である。誤差共分散行列とは、分散と共分散との行列である。

【0032】

本実施形態では、観測部２１は、観測対象として、車両の速度、車両の角速度、車両の加速度、車両の位置、及び、車両の方位を観測する。前述した通り、観測部２１は、車両の速度について、車速センサー６からの出力に基づき観測する。また、観測部２１は、車両の角速度について、ジャイロセンサー７からの出力に基づき観測する。また、観測部２１は、車両の加速度について、加速度センサー８からの出力に基づき観測する。また、観測部２１は、車両の位置、及び、車両の方位について、ＧＰＳ受信部５からの出力に基づき観測する。以下に、観測部２１が観測する観測対象を示す。なお、以下では、観測対象として車両の速度、車両の角速度、車両の位置、及び、車両の方位を例示する。

【0033】

ｖ^ＰＬＳ：車速センサー６からの出力に基づき観測される車両の速度
ω^ＧＹＲ：ジャイロセンサー７からの出力に基づき観測される車両の角速度
ｘ^ＧＰＳ：ＧＰＳ受信部５からの出力に基づき観測される車両の位置のｘ座標
ｙ^ＧＰＳ：ＧＰＳ受信部５からの出力に基づき観測される車両の位置のｙ座標
θ^ＧＰＳ：ＧＰＳ受信部５からの出力に基づき観測される車両の方位

【0034】

ここで、上記の観測対象をベクトル表記した観測ベクトルを（ｖ^ＰＬＳ、ω^ＧＹＲ、ｘ^ＧＰＳ、ｙ^ＧＰＳ、θ^ＧＰＳ）とすると、観測対象についての観測方程式は、式（２）で表される。

【0035】

【数2】

【0036】

式（２）において、ｒ_ｋは、観測雑音（平均０、誤差共分散行列であるＲ_ｋを有する正規分布Ｎ（０、Ｒ_ｋ））である。

【0037】

以下、カルマンフィルタについて、車両の状態量を予測する予測処理と、車両の状態量を推定する推定処理とに分けて説明する。

【0038】

なお、以下の説明において、添え字のｋ＋１｜ｋが付与された値は、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における予測値を示す。また、添え字のｋ｜ｋ−１が付与された値は、時刻ｋ−１までの情報に基づき予測された時刻ｋにおける予測値を示す。また、添え字のｋ＋１｜ｋ＋１が付与された値は、時刻ｋ＋１までの情報に基づき推定された時刻ｋ＋１における推定値を示す。また、添え字のｋ｜ｋが付与された値は、時刻ｋまでの情報に基づき推定された時刻ｋにおける推定値を示す。また、添え字のｋ−１｜ｋ−１が付与された値は、時刻ｋ−１までの情報に基づき推定された時刻ｋ−１における推定値を示す。

【0039】

＜予測処理＞
カルマンフィルタにおいて予測処理は、車両の状態量の予測値（以下、「車両状態予測値」と表現する）と、車両状態予測値の誤差共分散行列と、を算出する処理である。車両状態予測値は、式（３）に基づき算出される。なお、誤差共分散行列の算出とは、誤差共分散行列の各成分の値の算出を示す。

【0040】

【数3】

【0041】

式（３）では、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における車両状態予測値の算出を示す。この車両状態予測値は、式（３）の右辺に示すように、時刻ｋまでの情報に基づき推定された時刻ｋにおける車両の状態量の推定値により算出される。例えば、車両の位置のｘ座標の予測値を示すｘ_{ｋ＋１｜ｋ}は、時刻ｋまでの情報に基づき推定された時刻ｋにおける、車両の位置のｘ座標（ｘ_ｋ｜ｋ）の推定値と、車両の方位（θ_ｋ｜ｋ）の推定値と、車両の速度（ｖ_ｋ｜ｋ）の推定値とに基づき算出される。なお、式（３）において、Ｔは、車速センサー６、ジャイロセンサー７、及び、加速度センサー８からの出力に基づき観測部２１が観測量を観測する間隔を示す。

【0042】

なお、時刻ｋ−１までの情報に基づき予測された時刻ｋの車両状態予測値を算出する場合、式（３）において時刻を１ステップずつ下げた式により算出できる。つまり、時刻ｋ−１までの情報に基づき予測された時刻ｋにおける車両状態予測値は、時刻ｋ−１までの情報に基づき推定された時刻ｋ−１における車両の状態量の推定値に基づき算出される。

【0043】

車両状態予測値の誤差共分散行列は、式（４）に基づき算出される。誤差共分散行列は、本実施形態において、車両の状態量に関する分散及び共分散の行列である。分散は、誤差を二乗したものである。つまり、車両の状態量の分散は、車両の状態量の誤差を二乗したものである。したがって、車両状態予測値の誤差共分散行列を算出することは、車両状態予測値の誤差を算出することに相当する。

【0044】

【数4】

【0045】

式（４）において、Ｐは、誤差共分散行列を示す。式（４）の左辺は、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における誤差共分散行列を示す。式（４）の左辺に示す誤差共分散行列は、時刻ｋまでの情報に基づき推定された時刻ｋにおける誤差共分散行列に基づき算出される。なお、Ｆは、式（１）の状態方程式から求められるヤコビ行列を示す。また、Ｆにおける添え字のＴは、転置行列を示す。また、Ｑは、システム雑音の誤差共分散行列を示す。

【0046】

なお、時刻ｋ−１までの情報に基づき予測された時刻ｋにおける車両状態予測値の誤差共分散行列を算出する場合、式（４）においてＰの添え字の時刻を１ステップずつ下げた式により算出できる。つまり、時刻ｋ−１までの情報に基づき予測された時刻ｋにおける車両状態予測値の誤差共分散行列は、時刻ｋ−１までの情報に基づき推定された時刻ｋ−１における車両の状態量の推定値の誤差共分散行列により算出される。

【0047】

このように、予測処理では、例えば、時刻ｋにおける車両の状態量を予測する場合、時刻ｋ−１までの情報に基づき算出された時刻ｋにおける車両状態予測値と、時刻ｋ−１までの情報に基づき算出された時刻ｋにおける車両状態予測値の誤差共分散行列とを算出する。つまり、予測処理は、車両の状態量の確立分布を予測している。

【0048】

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

【0049】

推定処理では、式（５）により観測残差が算出される。観測残差とは、観測対象の値と、車両状態予測値から算出される観測対象に対応する値との誤差である。

【0050】

【数5】

【0051】

式（５）において、左辺は、観測残差をベクトル表記した観測残差ベクトルである。また、式（５）において、右辺の第１項は、観測部２１の観測対象とする観測対象ベクトルである。また、式（５）において、右辺の第２項は、予測処理で予測した車両状態予測値に、観測方程式から求められる観測行列であるＨを掛けたものである。

【0052】

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

【0053】

【数6】

【0054】

式（６）では、時刻ｋ＋１までの情報に基づき予測された時刻ｋ＋１における車両状態推定値を示す。この車両状態推定値は、式（６）の右辺に示すように、時刻ｋまでの情報に基づき推定された時刻ｋ＋１における車両状態予測値を、観測残差より補正して算出される。なお、時刻ｋまでの情報に基づき予測された時刻ｋにおける車両状態推定値を算出する場合、時刻ｋ−１までの情報に基づき予測された時刻ｋにおける車両状態予測値を観測残差により補正することで算出される。

【0055】

前述の通り、式（６）は、観測残差を用いて、車両状態予測値を補正し、車両状態推定値を算出する式を示す。式（６）に示すように、車両状態予測値を観測残差にて補正する際の補正係数としてＫ_ｋが用いられる。Ｋ_ｋは、カルマンゲインと呼ばれ、式（７）で表される。

【0056】

【数7】

【0057】

式（７）において、Ｒ_ｋは、観測雑音の誤差共分散行列である。また、添え字の「−１」は、逆行列を示す。式（７）に示すカルマンゲインＫ_ｋは、時刻ｋまでの情報に基づく時刻ｋ＋１における車両状態予測値の誤差共分散行列に基づき算出される。

【0058】

式（６）において、カルマンゲインＫ_ｋは、車両状態推定値について、車両状態予測値を重視して算出するか、観測部２１が観測する観測対象の値を重視して算出するかを決定付ける係数である。

【0059】

例えば、車速センサー６、ジャイロセンサー７、及び、ＧＰＳ受信部５からの出力に基づく観測対象の値の誤差が充分に小さい場合、車両状態推定値は、当該誤差が充分に小さいことから、観測対象の値となることが望ましい。これは、車両状態推定値が、誤差の充分に小さい値、つまり、精度の高い値になるためである。つまり、誤差共分散行列であるＲ_ｋの値が充分に小さい場合、車両状態推定値は、観測対象の値となることが望ましい。ここで、Ｒ_ｋの値が充分に小さい場合のカルマンゲインＫ_ｋをＫ_ｋ＝Ｈ^−１として式（６）に与えると、式（６）の右辺の第１項は、無くなる。つまり、車両状態推定値は、誤差が充分に小さい観測対象の値となる。

【0060】

また、例えば、車両状態予測値の誤差が、車速センサー６、ジャイロセンサー７、及び、ＧＰＳ受信部５からの出力に基づく観測対象の誤差より充分に小さい場合、車両状態推定値は、車両状態予測値となることが望ましい。これは、車両状態推定値が、観測対象の誤差より充分に誤差が小さい値、つまり、精度の高い値になるためである。つまり、車両状態予測値の誤差共分散行列が、Ｒ_ｋの値より充分に小さい場合、車両状態推定値は、車両状態予測値となることが望ましい。ここで、車両状態予測値の誤差共分散行列がＲ_ｋの値より充分に小さい場合のカルマンゲインＫ_ｋとしてＫ_ｋ＝０として式（６）に与えると、車両状態推定値は、車両状態予測値となる。

【0061】

このように、カルマンゲインＫ_ｋは、観測雑音の誤差共分散行列であるＲ_ｋ、及び、車両状態予測値の誤差共分散行列に応じて、車両状態推定値が適切な値となるように設定する係数である。つまり、観測雑音の誤差共分散行列が観測対象の誤差共分散行列である場合、カルマンゲインＫ_ｋは、車両状態予測値の誤差共分散行列が正確に予測されていれば、車速センサー６、ジャイロセンサー７、及び、ＧＰＳ受信部５からの出力に基づく観測対象の誤差共分散行列に応じて、車両状態推定値が適切な値となるように設定する。

【0062】

車両状態推定値の誤差共分散行列は、式（８）により算出される。前述した通り、誤差共分散行列は、本実施形態では、車両の状態量に関する、分散及び共分散の行列である。車両の状態量の分散は、車両の状態量の誤差を二乗したものである。したがって、車両状態推定値の誤差共分散行列を算出することは、車両状態推定値の誤差を算出することに相当する。

【0063】

【数8】

【0064】

式（８）において、Ｐは、式（４）と同様、誤差共分散行列を示す。また、式（８）において、Ｉは、単位行列を示す。式（８）の左辺は、時刻ｋ＋１までの情報に基づき推定された時刻ｋ＋１における誤差共分散行列を示す。式（８）の左辺に示す誤差共分散行列は、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における誤差共分散行列に基づき算出される。

【0065】

なお、時刻ｋまでの情報に基づき予測された時刻ｋにおける車両状態推定値の誤差共分散行列を算出する場合、式（８）においてＰの添え字の時刻を１ステップずつ下げた式により算出できる。つまり、時刻ｋまでの情報に基づき予測された時刻ｋにおける車両状態推定値の誤差共分散行列は、時刻ｋ−１までの情報に基づき推定された時刻ｋにおける車両状態予測値の誤差共分散行列により算出される。

【0066】

式（８）は、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列に、（Ｉ−Ｋ_ｋＨ）を掛けた式である。式（８）に示すように、車両状態推定値の誤差共分散行列は、カルマンゲインＫ_ｋの値に依存する。

【0067】

例えば、観測雑音の誤差が充分に小さい場合のカルマンゲインＫ_ｋとしてＫ_ｋ＝Ｈ^−１を式（８）に与えると、車両状態推定値の誤差共分散行列は零行列となる。これは、推定した車両状態推定値の誤差が充分に小さいことを示す。

【0068】

また、例えば、観測雑音の誤差より車両状態予測値の誤差が充分に小さい場合のカルマンゲインの値としてＫ_ｋ＝０を与えることにより、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列になる。これは、車両状態推定値の誤差が、誤差の充分に小さい車両状態予測値の誤差であることを示す。

【0069】

このように、式（８）において、カルマンゲインＫ_ｋは、観測雑音の誤差共分散行列と、車両状態予測値の誤差共分散行列に応じて、車両状態推定値の誤差共分散行列が適切になるように設定する。つまり、カルマンゲインＫ_ｋは、観測雑音の精度と車両状態予測値の精度とに基づいて、車両状態推定値の誤差共分散行列が適切になるように設定する。

【0070】

以上、推定処理では、例えば、時刻ｋにおける車両の状態量を推定する場合、時刻ｋまでの情報に基づき算出された時刻ｋにおける車両状態推定値と、時刻ｋまでの情報に基づき算出された時刻ｋにおける車両状態推定値の誤差共分散行列とを算出する。つまり、推定処理は、予測処理で予測した車両の状態量の確立分布に基づいて、車両の状態量の確立分布を推定している。

【0071】

以上の算出により、推定部２２は、カルマンフィルタによって、車両状態推定値と、車両状態推定値の誤差共分散行列とを算出することにより、車両の状態を推定する。

【0072】

前述した通り、カルマンゲインＫ_ｋは、車両状態推定値と、車両状態推定値の誤差共分散行列とを適切に設定する係数である。つまり、車両の状態の推定の精度は、カルマンゲインＫ_ｋに依存する。カルマンゲインＫ_ｋは、前述した通り、観測雑音と、車両状態予測値の誤差共分散行列とに応じて、車両状態推定値と車両状態推定値の誤差共分散行列とを適切に設定する係数である。そのため、車両状態推定値と車両状態推定値の誤差共分散行列とを適切に算出するためには、車両状態予測値の誤差共分散行列が、正確に算出される必要がある。しかしながら、カルマンフィルタでは、システム雑音、及び、観測雑音が白色性の雑音であることが仮定される。つまり、カルマンフィルタにおいて誤差を雑音として扱う場合、誤差の平均が０であることが前提となる。そのため、平均が０とならない誤差、すなわち、累積する誤差を考慮して、車両状態予測値の誤差共分散行列を算出できない場合があり、この場合、カルマンゲインＫ_ｋは、正確に算出されない。これは、車両状態推定値と車両状態推定値の誤差共分散行列とが精度よく算出できない、すなわち、車両の状態を精度よく推定できないことを示す。

【0073】

そこで、本実施形態の推定部２２は、以下のカルマンフィルタにより、車両の状態を推定する。

【0074】

以下、車両の状態を推定する際のナビゲーション装置１の動作を通して、推定部２２による車両の状態の推定について説明する。

【0075】

図２は、ナビゲーション装置１の動作を示すフローチャートである。

【0076】

ナビゲーション装置１の観測部２１は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８から出力される信号に基づき、車両の速度、車両の角速度、及び、車両の加速度を観測する（ステップＳＡ１）。観測部２１は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８が信号を出力する度に、車両の速度、車両の角速度、及び、車両の加速度を観測する。つまり、観測部２１がこれらを観測する間隔は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８が検出する間隔と同じ間隔である。

【0077】

次いで、観測部２１は、ＧＰＳ受信部５の出力に基づき、車両の位置、及び、車両の方位を観測する（ステップＳＡ２）。観測部２１は、車両の位置を示す情報と車両の方位を示す情報とがＧＰＳ受信部５から出力される度に、車両の位置、及び、車両の方位を観測する。つまり、観測部２１が車両の位置及び車両の方位を観測する間隔は、ＧＰＳ受信部５がＧＰＳ電波を受信する間隔となる。

【0078】

次いで、ナビゲーション装置１の推定部２２は、観測部２１が観測した観測対象に基づき、車両の状態を推定する車両状態推定処理を実行する（ステップＳＡ３）。

【0079】

図３は、車両状態推定処理における推定部２２の動作を示すフローチャートである。

【0080】

推定部２２は、予測処理を実行する（ステップＳＢ１）。前述した通り、予測処理は、車両状態予測値、及び、車両状態予測値の誤差共分散行列を算出する処理である。車両状態予測値は、式（３）によって算出される。車両状態予測値の誤差共分散行列は、式（９）によって算出される。

【0081】

【数9】

【0082】

式（９）において、Ｆ_ｋは、式（１）の状態方程式から求められるヤコビ行列であり、式（１０）で表される。また、Ｑ_ｋは、システム雑音の誤差共分散行列であり、式（１１）で表される。また、Ｃ_ｋは、前回推定した車両状態推定値の誤差と、観測部２１が車速センサー６及びジャイロセンサー７からの出力に基づき観測する観測量の誤差との共分散行列であり、式（１２）で表される。Ｑｋ＋２・Ｃｋがシステム雑音である。

【0083】

【数10】

【0084】

【数11】

【0085】

【数12】

【0086】

式（１２）において、σ_ｋ^ｖＰＬＳは、観測部２１が算出する車両の速度の誤差を示す。また、σ_ｋ^ωＧＹＲは、観測部２１が算出する車両の角速度の誤差を示す。前述した通り、σ_ｋ^ｖＰＬＳは、時間の経過と共に、この速度から求めた移動距離に累積する誤差である。また、σ_ｋ^ωＧＹＲは、σ_ｋ^ｖＰＬＳと同様、時間と共に、この角速度から求めた方位変化量に累積する誤差である。

【0087】

式（１２）に示すように、車両状態予測値の誤差共分散行列を算出する式（９）において、車速センサー６から出力に基づき観測部２１が算出する車両の速度の誤差は、車両の位置の誤差として入力される。すなわち、σ_ｋ^ｖＰＬＳは、ｃｏｓ（θ_ｋ｜ｋ）σ_ｋ^ｖＰＬＳＴ、及び、ｓｉｎ（θ_ｋ｜ｋ）σ_ｋ^ｖＰＬＳＴとして入力される。また、ジャイロセンサー７からの出力に基づき観測部２１が算出する車両の角速度の誤差は、車両の方位の誤差として入力される。すなわち、σ_ｋ^ωＧＹＲは、σ_ｋ^ωＧＹＲＴとして入力される。

【0088】

このように、時間の経過と共に累積する誤差、すなわち、σ_ｋ^ｖＰＬＳとσ_ｋ^ωＧＹＲとについて、σ_ｋ^ｖＰＬＳを車両の位置の誤差として入力し、σ_ｋ^ωＧＹＲを車両の方位の誤差として入力したカルマンフィルタにより、推定部２２は、車両状態予測値の誤差共分散行列を算出する。

【0089】

ここで、カルマンフィルタにσ_ｋ^ｖＰＬＳを車両の位置の誤差として入力し、σ_ｋ^ωＧＹＲを車両の方位の誤差として入力することについて説明する。以下では、カルマンフィルタにσ_ｋ^ｖＰＬＳを車両の位置の誤差として入力することについて詳述する。

【0090】

ここで説明の便宜上、車両の状態量を、車両の位置のｘ座標と車両の速度として例示し、カルマンフィルタにσ_ｋ^ｖＰＬＳを車両の位置の誤差として入力することについて説明する。この例では、車両の位置のｘ座標は、車両の速度により決定する。この例では、観測対象は、車両の速度である。

【0091】

したがって、車両の状態量についての状態方程式は、式（１３）で表される。また、観測対象についての観測方程式は、式（１４）で表される。

【0092】

【数13】

【0093】

【数14】

【0094】

式（１３）及び式（１４）に基づき、時刻ｋまでの情報に基づき推定した時刻ｋにおける車両状態推定値の誤差共分散行列を、式（４）、式（６）、及び、式（８）に基づき算出する。ここで、車両状態推定値の誤差共分散行列を算出するに際し、Ｐ_{（ｋ｜ｋ）}を式（１５）とする。Ｐ_{（ｋ｜ｋ）}は、時刻ｋまでの情報に基づき推定した時刻ｋにおける車両状態推定値の誤差共分散行列である。

【0095】

【数15】

【0096】

式（１５）において、ｐ_１１^{（ｋ｜ｋ）}は、車両の位置の誤差分散を示す。また、ｐ_２２^{（ｋ｜ｋ）}は、車両の速度の誤差分散を示す。また、ｐ_１２^{（ｋ｜ｋ）}は、車両の位置と、車両の速度との誤差共分散を示す。

【0097】

また、車両状態推定値の誤差共分散行列を算出するあたり、Ｆ、Ｑ、Ｈ、及び、Ｒのそれぞれを式（１６）〜式（１９）とする。ここで、Ｆは、状態方程式から求められるヤコビ行列である。また、Ｑは、システム雑音の分散行列である。また、Ｈは、観測方程式から求められるヤコビ行列である。また、Ｒは、観測雑音の分散行列である。観測量が車両の速度であることから、ｒ_ｋは、車両の速度の誤差である。つまり、ｒ_ｋ^２は、観測部２１が観測する車両の速度の誤差分散を示す。

【0098】

【数16】

【0099】

【数17】

【0100】

【数18】

【0101】

【数19】

【0102】

式（１５）〜式（１９）に基づき、車両状態推定値の誤差共分散行列、すなわち、Ｐ_{（ｋ｜Ｋ）}を算出すると、車両状態推定値の誤差共分散行列は、式（２０）で表される。

【0103】

【数20】

【0104】

したがって、車両の位置の推定値の誤差分散は、式（２１）で表される。

【0105】

【数21】

【0106】

式（２１）は、共通因数であるｐ_１１^{（ｋ｜ｋ−１）}でくくると、式（２２）に変形される。ｐ_１１^{（ｋ｜ｋ−１）}は、時刻ｋ−１までの情報に基づく時刻ｋにおける車両の位置の予測値の誤差分散である。

【0107】

【数22】

【0108】

式（２２）に示すように、車両の位置の推定値の誤差分散であるｐ_１１^{（ｋ｜ｋ）}は、車両の位置の予測値の誤差分散であるｐ_１１^{（ｋ｜ｋ−１）}に、式（２３）を掛けることで求められる。

【0109】

【数23】

【0110】

式（２３）は、式（２２）に示すように、車両の位置の予測値の分散であるｐ_１１^{（ｋ｜ｋ−１）}に掛けられる係数である。この係数は、分母に車両の速度の誤差（ｒ_ｋ）が含まれることを除くと、式（２４）と一致する。

【0111】

【数24】

【0112】

式（２４）に示す係数は、「１−（車両の位置の誤差と車両の速度の誤差との相関係数）^２」である。これは、式（２４）に示す係数が、最小二乗法を用いた線形回帰モデルの非決定係数であることを示す。つまり、車両の位置の誤差と、車両の速度の誤差との相関性が小さければ小さいほど、当該係数は大きい値となり、車両の位置の予測値の分散は大きい値となる。一方で、当該相関性が大きければ大きいほど、当該係数は小さい値となり、車両の位置の予測値の分散は小さい値となる。また、式（２２）は、車両の位置の推定値の分散が、車両の速度の誤差に対して指数関数的に減少することを示す。

【0113】

したがって、車両の位置の推定値の誤差分散は、車両の位置の誤差と、車両の速度の誤差との相関性に基づき決定されていることを示している。また、車両の位置の推定値の誤差は、車両の速度の誤差によって累積しないことも示す。つまり、車両の速度の誤差の累積に伴い、車両の位置の推定値の誤差分散であるｐ_１１^{（ｋ｜ｋ）}を累積させるためには、車両の位置の誤差と、車両の速度の誤差との相関性を小さくする必要がある。つまり、ｐ_１１^{（ｋ｜ｋ）}を累積させるためには、カルマンフィルタにおいて、車両の位置の誤差と、車両の速度の誤差との相関性を小さくする必要がある。具体的には、移動距離に累積する速度の誤差に関しては、カルマンフィルタに車両の速度の誤差を入力し、車両の位置の誤差を推定するのではなく、カルマンフィルタに車両の位置の誤差として車両の速度の誤差を入力し、車両の位置の誤差を推定する必要がある。

【0114】

したがって、式（１２）に示すように、移動距離に累積する速度の誤差に関しては、車両の速度の誤差を、車両の速度と微積分の関係にある車両の位置の誤差としてカルマンフィルタに入力する。これによって、移動距離に累積する速度の誤差の分だけ、車両の速度の誤差と車両の位置の誤差との相関性を小さくでき、累積する車両の速度の誤差を累積する車両の位置の誤差として、車両の位置の予測値の誤差共分散行列を算出できる。つまり、累積する車両の速度の誤差を考慮した、車両の位置の予測値の誤差共分散行列を算出できる。

【0115】

しかしながら、車両の位置の誤差として車両の速度の誤差をカルマンフィルタに入力する際、式（４）をそのまま用いることができない。

【0116】

式（４）において、右辺の第１項は、前回に推定した車両状態推定値の誤差共分散行列を、状態方程式から求められるヤコビ行列により今回の車両の状態量の誤差共分散行列に変換している。一方、右辺の第２項は、システム雑音の誤差共分散行列である。つまり、式（４）は、前回の車両状態推定値の誤差共分散行列を今回の車両状態推定値の誤差共分散行列に変換したものに、システム雑音の誤差共分散行列を加算することで、車両状態予測値の誤差共分散行列を算出する式である。ここで、式（４）は、式（２５）に示す命題が用いられている。

【0117】

【数25】

【0118】

式（２５）は、確率変数Ｘと確率変数Ｙとが互いに独立である場合に、分散Ｖａｒ（）について成立する式を示す。つまり、カルマンフィルタにおいて車両状態予測値の誤差共分散行列を算出する際に用いられる式（４）は、車両の状態量の誤差共分散行列と、システム雑音の誤差共分散行列とが互いに独立であることが前提となっている。しかしながら、システム雑音の誤差共分散行列を累積する誤差の誤差共分散行列とする場合、式（４）では、車両状態予測値の誤差共分散行列が正確に算出できない。これは、車両の状態量の誤差共分散行列と、車両の状態量として入力した累積する誤差の誤差共分散行列とが独立でないためである。例えば、車両の位置の誤差が時間の経過で累積する速度の誤差に決定されることから、車両の位置の誤差と、車両の速度の誤差とは、一方の変化に伴い一方も変化する変数、すなわち、互いに独立しない変数である。そのため、確率変数Ｘと確率変数Ｙとが独立でない場合に成立する命題を用いる必要がある。当該命題は、式（２６）により表される。

【0119】

【数26】

【0120】

式（２６）において、Ｃｏｖ（）は共分散を示す。したがって、式（２４）と比較すると、式（２６）は、確率変数Ｘと確率変数Ｙとの共分散が入力されている。

【0121】

式（２６）に従った式（９）を用いることにより、車両の状態量の誤差共分散行列と、累積する誤差の誤差共分散行列との独立性をなくして、正確に車両状態予測値の誤差共分散行列を算出できる。

【0122】

以上、車両の速度の誤差を車両の位置の誤差としてカルマンフィルタに入力することを説明した。角速度の誤差を方位の誤差としてカルマンフィルタに入力することについての説明も、同様に説明できる。

【0123】

以上の説明のように、観測量と微積分の関係にある状態量の誤差として、観測量の誤差が入力され、また、観測量の誤差と、車両の状態量の誤差との独立性のない式により、累積する誤差を考慮した、車両状態予測値の誤差共分散行列を算出できる。

【0124】

図３に示すフローチャートの説明に戻り、推定部２２は、予測処理を実行すると、推定処理を実行する。推定部２２は、式（５）〜式（８）に基づいて、車両状態推定値と、車両状態推定値の誤差共分散行列とを算出する。

【0125】

前述の通り、カルマンゲインＫ_ｋは、観測雑音の誤差共分散行列と、車両予測値の誤差共分散行列とに応じて、車両状態推定値と車両状態推定値の誤差共分散行列とを適切に設定する係数である。予測処理において、推定部２２は、累積する誤差を考慮した車両状態予測値の誤差共分散行列を算出できるため、カルマンゲインＫ_ｋを正確に算出できる。したがって、推定部２２は、車両状態推定値と車両状態推定値の誤差とを精度よく算出できる。

【0126】

また、式（８）に示すように、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列に（Ｉ−Ｋ_ｋＨ）を掛けて算出される。したがって、車両状態予測値の誤差共分散行列は、累積する誤差を考慮した誤差共分散行列であるため、算出される車両状態推定値の誤差共分散行列も累積する誤差を考慮した誤差共分散行列である。

【0127】

このように、推定部２２は、累積する誤差を考慮した車両状態予測値の誤差共分散行列を算出できるため、カルマンゲインＫ_ｋが正確に算出でき、カルマンフィルタにより精度よく車両の状態を推定できる。
また、カルマンフィルタで車両の状態を推定しているため、誤差を推定の対象としたカルマンフィルタに基づき車両の状態を推定する構成と比較し、誤差とは別に車両の状態を推定する必要がなく、容易に累積する誤差を考慮した車両の状態を推定できる。

【0128】

なお、状態量として車両の速度を推定する場合、推定部２２は、車両の加速度の誤差を車両の速度の誤差として入力したカルマンフィルタに基づき車両の速度の予測値の誤差共分散行列を算出し、車両の速度を推定する構成としてもよい。これにより、累積する加速度の誤差を考慮した車両の速度の予測値の誤差共分散行列が算出されるため、カルマンゲインＫ_ｋを正確に算出でき、カルマンフィルタにより車両の速度を精度よく推定できる。

【0129】

上述した通り、推定部２２は、ＧＰＳ受信部５が出力する車両の位置を示す情報と車両の方位を示す情報とに基づき、観測部２１が観測した車両の位置と車両の方位とを加味して、車両状態推定値と車両状態推定値の誤差共分散行列とを算出し、車両の状態を推定する構成である。しかしながら、観測部２１が観測する車両の位置と車両の方位とを加味して、推定部２２が車両の状態を推定する構成に限定されない。すなわち、推定部２２は、ＧＰＳ受信部５からの出力に基づくことなく、車両の状態を推定してもよい。

【0130】

例えば、車両がＧＰＳ電波をＧＰＳ受信部５により受信できない環境下にいる場合、推定部２２は、ＧＰＳ受信部５からの出力に基づく車両の位置及び車両の方位の成分のない観測方程式に基づき車両の状態量を推定する。

【0131】

式（１２）に示すように、推定部２２は、車両状態予測値の誤差共分散行列を算出する際、車速センサー６及びジャイロセンサー７から出力される信号の間隔に基づき、車両状態予測値の誤差共分散行列を算出する。また、推定部２２は、式（３）に示すように、車両状態予測値を、前回推定した車両状態推定値と当該間隔に基づき算出する。したがって、推定部２２は、車両状態予測値、及び、車両状態予測値の誤差共分散行列の算出に際し、ＧＰＳ受信部５の受信間隔に依存しない。したがって、推定部２２は、式（５）において、ＧＰＳ受信部５からの出力に基づく観測対象の成分のない式に基づくことにより、車両状態推定値と車両状態推定値の誤差共分散行列とを算出できる。

【0132】

つまり、推定部２２は、ＧＰＳ受信部５のＧＰＳ電波の受信の間隔に依存せず、車両の状態を推定できる。これは、推定部２２が車速センサー６、ジャイロセンサー７、及び、加速度センサー８の検出間隔に基づき、車両の状態を推定することを示す。したがって、推定部２２が車速センサー６、ジャイロセンサー７、及び、加速度センサー８の検出間隔が、ＧＰＳ受信部５の受信間隔より短い場合において以下の効果を奏する。すなわち、当該受信間隔に基づいてカルマンフィルタにより車両の状態を推定する構成より、車両の状態を推定について高い頻度で実行でき、また、ＧＰＳ受信部５の受信環境に依存することなく、車両の状態を推定できる。

【0133】

前述した通り、ナビゲーション装置１の制御部２は、推定した車両の状態に基づき、マップマッチングの対象となるリンクの評価を実行する。

【0134】

図４は、推定した車両の状態に基づくマップマッチングの処理を説明するため、地図上の道路Ｒ１と、道路Ｒ２とを示す図である。図４において、道路Ｒ１は、方向Ｙ１に向かって延在する道路である。また、道路Ｒ２は、方向Ｙ１と並行でない方向Ｙ２に向かって延在する道路である。

【0135】

図４において、リンクＬ１は、道路Ｒ１に対応するリンクである。また、リンクＬ２は、道路Ｒ２に対応するリンクである。

【0136】

また、図４において、マークαは、推定部２２により推定された車両の位置を示すマークである。以下、図４を用いた説明では、推定部２２により、車両が、位置Ｍ１に位置し、進行方向Ｘ１に向かって走行しているものと推定されたとする。図４に示すように、位置Ｍ１は、道路Ｒ１上の位置であって、道路Ｒ１の幅方向の中心から、方向Ｙ１に向かって右側に離間した位置である。

【0137】

道路Ｒ１と、道路Ｒ２と、推定された車両の位置との関係が、図４に示す関係である場合、マップマッチングに際し、まず、制御部２は、地図データ３ａを参照し、マップマッチングの候補となるリンクであるマップマッチング候補リンクを取得する。制御部２は、マップマッチング候補リンクとして、推定された車両の位置から予め設定された所定の範囲内に位置し、かつ、車両とリンクとの方位の誤差が所定範囲内である１又は複数のリンクを取得する。図４の例では、リンクＬ１、及び、リンクＬ２は、推定された車両の位置である位置Ｍ１から所定範囲に位置し、また、車両とリンクとの方位の誤差が所定範囲内のリンクである。このため、制御部２は、リンクＬ１、及び、リンクＬ２をマップマッチング候補リンクとして取得する。

【0138】

次いで、制御部２は、取得したマップマッチング候補リンクのそれぞれについて、リンクを評価する評価量を算出する。

【0139】

評価量は、推定された車両の位置とマップマッチング候補リンクの位置との誤差、及び、推定された車両の方位とマップマッチング候補リンクの方位との誤差に基づいて、下記に示す式（２７）により算出される値である。

【0140】

τ＝δｘ^２／Δｘ^２＋δｙ^２／Δｙ^２＋δθ^２／Δθ^２・・・（２７）

【0141】

ここで、推定された車両の位置とマップマッチング候補リンクの位置との誤差とは、推定された車両の位置からマップマッチング候補リンクに対して垂線を延ばした場合における垂線及びマップマッチング候補リンクとの交点と、推定された車両の位置との、ｘ軸方向、及び、ｙ軸方向における差である。式（２７）において、δｘは、当該交点のｘ座標と、推定された車両の位置のｘ座標との差を示す。また、δｙは、当該交点のｙ座標と、推定された車両の位置のｙ座標との差を示す。

【0142】

図４の例において、推定された車両の位置とリンクＬ１との位置の誤差は、位置Ｍ１からリンクＬ１に対して延ばした垂線Ｓ１及びリンクＬ１の交点ＭＭ１と、位置Ｍ１とのｘ軸方向、及び、ｙ軸方向における差である。また、図４において、推定された車両の位置とリンクＬ２との位置の誤差は、位置Ｍ１からリンクＬ２に対して延ばした垂線Ｓ２及びリンクＬ２の交点ＭＭ２と、位置Ｍ１とのｘ軸方向、及び、ｙ軸方向における差である。

【0143】

また、推定された車両の方位とマップマッチング候補リンクの方位との誤差とは、推定された車両の方位に対応する角度と、マップマッチング候補リンクの方位に対応する角度との差である。

【0144】

前述した通り、車両の方位とは、車両の進行方向の方角を意味する。図４の例では、推定された車両の方位は、進行方向Ｘ１の方角である。また、車両の方位に対応する角度とは、東に向かう方向を基準として、東に向かう方向と、車両の方位との反時計回りの離間角度を意味する。図４の例では、推定された車両の方位に対応する角度は、仮想直線Ｋ１が東西に延びる仮想直線であるとした場合に、角度θ１である。

【0145】

また、リンクの方位とは、リンクが延在する方向の方角を意味する。リンクが延在する方向とは、リンクに沿った２つの方向のうち、リンクに対応する道路において車両が走行可能な方向に対応する方向を意味する。また、リンクの方位に対応する角度とは、東に向かう方向を基準として、東に向かう方向と、リンクの方位との反時計回りの離間角度を意味する。

【0146】

図４の例では、リンクＬ１の方位は、方向Ｚ１の方角である。また、リンクＬ１の方位に対応する角度は、仮想直線Ｋ２が東西に延びる仮想直線であるとした場合に、角度θ２である。また、図４の例では、リンクＬ２の方位は、方向Ｚ２の方角である。また、リンクＬ２の方位に対応する角度は、仮想直線Ｋ３が東西に延びる仮想直線であるとした場合に、角度θ３である。

【0147】

図４の例において、車両とリンクＬ１との方位誤差は、角度θ１と角度θ２との差である。また、車両とリンクＬ２との方位誤差は、角度θ１と角度θ３との差である。つまり、図４の例において、式（２７）では、δθは、角度θ１と角度θ２との差、または、角度θ１と角度θ３との差を示す。

【0148】

式（２７）に示すように、評価量τは、δｘを二乗した値にΔｘを二乗した値で割ったものと、δｙを二乗した値にΔｙを二乗した値で割ったものと、δθを二乗した値にΔθを二乗した値で割ったものと、の和により算出される。Δｘは、推定部２２が推定した車両のｘ軸方向の位置の誤差を示す。つまり、Δｘ^２は、推定部２２が推定した車両のｘ軸方向の位置の分散を示す。また、Δｙは、推定部２２が推定した車両のｙ軸方向の位置の誤差を示す。つまり、Δｙ^２は、推定部２２が推定した車両のｙ軸方向の位置の分散を示す。また、Δθは、推定部２２が推定した車両の方位の誤差を示す。つまり、Δθ^２は、推定部２２が推定した車両の方位の分散を示す。Δｘ^２、Δｙ^２、及び、Δθ^２は、推定部２２が推定した車両状態推定値の誤差共分散行列が含む分散である。このように、評価量は、車両の位置とリンクの位置との誤差を推定した車両の位置の誤差により無次元化した値と、車両の方位とリンクの方位との誤差を、推定した車両の位置の誤差により無次元化した値との和により算出される値である。

【0149】

図４の例では、制御部２は、マップマッチング候補リンクとして取得したリンクＬ１の評価量とリンクＬ２の評価量とを算出する。

【0150】

リンクＬ１の評価量をτ１とし、位置Ｍ１と交点ＭＭ１とのｘ座標の差をδｘ１とし、位置Ｍ１と交点ＭＭ１とのｙ座標の差をδｙ１とし、角度θ１と角度θ２との差をδθ１とした場合、評価量τ１は、下記に示す式（２８）で表される。

【0151】

τ１＝δｘ１^２／Δｘ^２＋δｙ１^２／Δｙ^２＋δθ１^２／Δθ^２・・・（２８）

【0152】

一方、リンクＬ２の評価量をτ２とし、位置Ｍ１と交点ＭＭ２とのｘ座標の差をδｘ２とし、位置Ｍ１と交点ＭＭ２とのｙ座標の差をδｙ２とし、角度θ１と角度θ３との差をδθ２とした場合、評価量τ２は、下記に示す式（２９）で表される。

【0153】

τ２＝δｘ２^２／Δｘ^２＋δｙ２^２／Δｙ^２＋δθ２^２／Δθ^２・・・（２９）

【0154】

図４の例では、距離ｌ２より距離ｌ１が小さい。すなわち、位置Ｍ１と交点ＭＭ２とにおけるｘ軸座標及びｙ座標の差より、位置Ｍ１と交点ＭＭ２とにおけるｘ座標及びｙ座標の差が小さい。また、図４の例では、角度θ１と角度θ３との差より角度θ１と角度θ２との差が小さい。このため、リンクＬ１の評価量であるτ１の値の方が、リンクＬ２の評価量であるτ２の値よりも小さくなる。したがって、制御部２は、評価量が小さいリンクＬ１を、車両の現在位置を対応付けるリンクとして決定する。

【0155】

このように、制御部２は、推定した車両の状態に基づいて、マップマッチングの対象となるリンクの評価を実行する。上述したように、制御部２は、リンクを評価する際、推定部２２が推定した車両状態推定値の誤差共分散行列が含む分散に基づき、リンクを評価する。当該誤差共分散行列は、累積する誤差を考慮して算出された車両状態予測値の誤差共分散行列に基づき算出された行列であり、精度よく算出された誤差を含む行列である。したがって、制御部２は、当該誤差共分散行列が含む分散をリンクの評価に用いることにより、正確にリンクを評価できる。

【0156】

以上、説明したように、ナビゲーション装置１（車載装置）は、車両の変動に関する観測量を観測する観測部２１と、カルマンフィルタにより、車両の状態を示す状態量を推定する推定部２２と、を備える。推定部２２は、車両の状態量の予測値を算出し、観測量と微積分の関係にある状態量の誤差として観測量の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列（誤差）を算出し、算出した予測値と予測値の誤差共分散行列とに基づいて、カルマンフィルタにより車両の状態量の推定値と推定値の誤差共分散行列とを算出する。

【0157】

これにより、観測量の誤差を微積分の関係にある状態量の誤差として入力することにより、観測量の誤差と状態量の誤差との相関性を小さくし、累積する観測量の誤差を考慮して車両状態予測値の誤差共分散行列を算出できるため、カルマンゲインＫ_ｋを正確に算出でき、カルマンフィルタにより車両の状態を精度よく推定できる。

【0158】

また、推定部２２は、前回算出した車両状態推定値の誤差と、車両の状態量の誤差として入力された観測量の誤差との共分散が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。

【0159】

前述した通り、式（４）は、状態量の誤差共分散行列と、システム雑音の誤差共分散行列とが互いに独立であることが前提となる命題が用いられている。しかしながら、システム雑音の誤差共分散行列を累積する誤差の誤差共分散行列とする場合、車両の状態量の誤差共分散行列と、累積する誤差の誤差共分散行列とが独立でない。そこで、式（２６）に従った式（９）、すなわち、前回推定した車両状態推定値の誤差と、状態量の誤差として入力された観測量の誤差との共分散が入力された式を用いることにより、車両の状態量の誤差共分散行列と、累積する誤差の誤差共分散行列との独立性をなくして、正確に車両状態予測値の誤差共分散行列を算出できる。

【0160】

また、観測部２１は、車両の速度を検出する車速センサー６からの出力に基づき、車両の速度を観測する。推定部２２は、車両の位置の誤差として車両の速度の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。

【0161】

これにより、車両の速度の誤差と車両の位置の誤差との相関性を小さくすることで、累積する車両の速度の誤差を考慮でき、カルマンフィルタにより車両の位置を精度よく推定できる。

【0162】

また、観測部２１は、車両の角速度を検出するジャイロセンサー７（角速度センサー）からの出力に基づき、車両の角速度を観測する。推定部２２は、車両の方位の誤差として車両の角速度の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。

【0163】

これにより、車両の角速度の誤差と車両の方位の誤差との相関性を小さくすることで、累積する車両の角速度の誤差を考慮でき、カルマンフィルタにより車両の方位を精度よく推定できる。

【0164】

観測部２１は、車両の加速度を検出する加速度センサー８からの出力に基づき、車両の加速度を観測する。推定部２２は、車両の速度の誤差として車両の加速度の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。

【0165】

これにより、車両の加速度の誤差と車両の速度の誤差との相関性を小さくすることで、累積する車両の加速度の誤差を考慮でき、カルマンフィルタにより車両の速度を精度よく推定できる。

【0166】

上述した実施形態は、あくまでも本発明の一態様を例示するものであって、本発明の趣旨を逸脱しない範囲で任意に変形、及び応用が可能である。

【0167】

例えば、図１は、本願発明を理解容易にするために、ナビゲーション装置１の機能構成を主な処理内容に応じて分類して示した概略図であり、ナビゲーション装置１の構成は、処理内容に応じて、さらに多くの構成要素に分類することもできる。また、１つの構成要素がさらに多くの処理を実行するように分類することもできる。

【0168】

また、例えば、図２、及び、図３のフローチャートの処理単位は、制御部２の処理を理解容易にするために、主な処理内容に応じて分割したものであり、処理単位の分割の仕方や名称によって、本発明が限定されることはない。制御部２の処理は、処理内容に応じて、さらに多くの処理単位に分割してもよい。また、１つの処理単位がさらに多くの処理を含むように分割してもよい。

【0169】

また、例えば、上述した実施形態では、ナビゲーション装置１として車載型を例示したが、ナビゲーション装置１の形態は任意であり、例えば歩行者が携帯するポータブル型の装置でも良い。

【符号の説明】

【0170】

１ ナビゲーション装置（車載装置）
６ 車速センサー（センサー）
７ ジャイロセンサー（センサー）
８ 加速度センサー（センサー）
２１ 観測部
２２ 推定部

【図1】

【図2】

【図3】

【図4】