이 애플리케이션의 일부 콘텐츠는 현재 사용할 수 없습니다.
이 상황이 계속되면 다음 주소로 문의하십시오피드백 및 연락
1. (WO2017150106) IN-VEHICLE DEVICE AND ESTIMATION METHOD
Document

明 細 書

発明の名称 車載装置、及び、推定方法

技術分野

0001  

背景技術

0002  

先行技術文献

特許文献

0003  

発明の概要

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

0004  

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

0005  

発明の効果

0006  

図面の簡単な説明

0007  

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

0008   0009   0010   0011   0012   0013   0014   0015   0016   0017   0018   0019   0020   0021   0022   0023   0024   0025   0026   0027   0028   0029   0030   0031   0032   0033   0034   0035   0036   0037   0038   0039   0040   0041   0042   0043   0044   0045   0046   0047   0048   0049   0050   0051   0052   0053   0054   0055   0056   0057   0058   0059   0060   0061   0062   0063   0064   0065   0066   0067   0068   0069   0070   0071   0072   0073   0074   0075   0076   0077   0078   0079   0080   0081   0082   0083   0084   0085   0086   0087   0088   0089   0090   0091   0092   0093   0094   0095   0096   0097   0098   0099   0100   0101   0102   0103   0104   0105   0106   0107   0108   0109   0110   0111   0112   0113   0114   0115   0116   0117   0118   0119   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   5   6   7   8   9   10  

図面

1   2   3   4  

明 細 書

発明の名称 : 車載装置、及び、推定方法

技術分野

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

背景技術

[0002]
 本技術分野の背景技術として、特開2000-55678号公報(特許文献1)がある。特許文献1には、「車速センサ4、ジャイロ6での検出値から移動距離演算部11、方位変化量演算部12にて算出される移動距離、方位変化量に基づき、相対軌跡演算部13、絶対位置演算部14が算出する推定航法データ(車速、絶対方位、絶対位置)と、GPS受信機8からのGPS測位データ(速度、方位、位置)との差を観測値とするカルマンフィルタ(誤差推定部15)を備え、その状態量の一つとして、ジャイロ出力から角速度への変換ゲインの誤差(ゲイン誤差)を設定する。このカルマンフィルタにより求められるゲイン誤差の推定値により、ジャイロ出力及び変換ゲインを用いて算出された方位変化量を補正する。」と記載されている。

先行技術文献

特許文献

[0003]
特許文献1 : 特開2000-55678号公報

発明の概要

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

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

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

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

発明の効果

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

図面の簡単な説明

[0007]
[図1] 図1は、ナビゲーション装置の構成を示すブロック図である。
[図2] 図2は、ナビゲーション装置の動作を示すフローチャートである。
[図3] 図3は、車両状態推定処理における推定部の動作を示すフローチャートである。
[図4] 図4は、推定した車両の状態に基づくマップマッチングの処理を説明するための図である。

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

[0008]
 図1は、ナビゲーション装置1(車載装置)の構成を示すブロック図である。
[0009]
 ナビゲーション装置1は、車両に搭載される車載装置であり、車両に搭乗しているユーザの操作に従って、地図の表示や、地図における車両の現在位置の表示、目的地までの経路探索、経路案内等を実行する。なお、ナビゲーション装置1は、車両のダッシュボード等に固定されてもよいし、車両に対し着脱可能なものであっても良い。
[0010]
 図1に示すように、ナビゲーション装置1は、制御部2と、記憶部3と、タッチパネル4と、GPS受信部5と、車速センサー6(センサー)と、ジャイロセンサー7(角速度センサー、センサー)と、加速度センサー8(センサー)とを備える。
[0011]
 制御部2は、CPUや、ROM、RAM、その他の制御回路等を備え、ナビゲーション装置1の各部を制御する。制御部2は、ROMや記憶部3等に記憶された制御プログラムを実行することにより、後述する観測部21、及び、推定部22として機能する。
[0012]
 記憶部3は、ハードディスクや、EEPROM等の不揮発性メモリを備え、データを書き換え可能に記憶する。記憶部3は、制御部2が実行する制御プログラムのほか、地図データ3aを記憶する。地図データ3aは、交差点やその他の道路網上の接続点を示すノードに関するノード情報や、ノードとノードとの道路区間を示すリンクに関するリンク情報、地図の表示に係る情報等を有する。リンク情報は、リンク毎に、少なくとも、リンクの位置に関する情報と、リンクの方位に関する情報とを含む。
[0013]
 タッチパネル4は、表示パネル4aと、タッチセンサー4bとを備える。表示パネル4aは、液晶ディスプレイやEL(Electro Luminescent)ディスプレイ等により構成され、制御部2の制御の下、各種情報を表示パネル4aに表示する。タッチセンサー4bは、表示パネル4aに重ねて配置され、ユーザのタッチ操作を検出し、タッチ操作された位置を示す信号を制御部2に出力する。制御部2は、タッチセンサー4bからの入力に基づいて、タッチ操作に対応する処理を実行する。
[0014]
 GPS受信部5は、GPSアンテナ5aを介してGPS衛星からのGPS電波を受信し、GPS電波に重畳されたGPS信号から、車両の位置と、車両の進行方向の方角(以下、「車両の方位」と表現する)とを演算により取得する。GPS受信部5は、車両の位置を示す情報、及び、車両の方位を示す情報を制御部2に出力する。
[0015]
 車速センサー6は、車両の車速を検出して、検出した車速を示す信号を制御部2に出力する。
[0016]
 ジャイロセンサー7は、例えば振動ジャイロにより構成され、車両の回転による角速度を検出する。ジャイロセンサー7は、検出した角速度を示す信号を制御部2に出力する。
[0017]
 加速度センサー8は、車両に作用する加速度(例えば、進行方向に対する車両の傾き)を検出する。加速度センサー8は、検出した加速度を示す信号を制御部2に出力する。
[0018]
 図1に示すように、制御部2は、観測部21と、推定部22とを備える。
[0019]
 観測部21は、車速センサー6、ジャイロセンサー7、及び、加速度センサー8から出力される信号に基づき、車両の変動に関する観測量を観測する。本実施形態では、観測部21は、車速センサー6から出力される車速を示す信号に基づき、観測量として車両の速度(観測量)を観測する。観測部21は、所定の演算により、車速を示す信号から、車両の速度と車両の速度の誤差とを算出する。この算出された車両の速度の誤差は、この速度から求めた移動距離に累積する誤差である。
[0020]
 また、観測部21は、ジャイロセンサー7から出力される角速度を示す信号に基づき、観測量として車両の回転による角速度(観測量)を観測する。観測部21は、所定の演算により、角速度を示す信号から、車両の角速度と車両の角速度の誤差とを算出する。この算出された車両の角速度の誤差は、この角速度から求めた方位変化量に累積する誤差である。
[0021]
 また、観測部21は、加速度センサー8から出力される加速度(観測量)を示す信号に基づき、観測量として車両の加速度を観測する。観測部21は、所定の演算により、加速度を示す信号から、車両の加速度と車両の加速度の誤差とを算出する。この算出された車両の加速度の誤差は、この加速度から求めた速度に累積する誤差である。
[0022]
 また、観測部21は、GPS受信部5から出力された車両の位置を示す情報と車両の方位を示す情報とに基づき、車両の位置と車両の方位とを観測する。
[0023]
 推定部22は、観測部21が観測する車両の速度、車両の加速度、車両の角速度、車両の位置、及び、車両の方位に基づき、カルマンフィルタにより、車両の状態を示す状態量を推定する。本実施形態では、推定部22は、車両の位置、車両の方位、車両の速度、及び、車両の角速度を、車両の状態量として推定する。
[0024]
 後述するが、制御部2は、推定部22が推定した車両の状態量に基づき、マップマッチングの対象となるリンクの評価を実行する。
[0025]
 なお、推定部22が推定する車両の速度は、車両の状態量に相当する。また、観測部21が観測する車両の速度は、観測量に相当する。同様に、推定部22が推定する車両の角速度は、車両の状態量に相当する。また、観測部21が観測する車両の角速度は、観測量に相当する。
[0026]
 ここで、カルマンフィルタによる基本的な車両の状態量の推定について説明する。
[0027]
 本実施形態では、カルマンフィルタにより推定する車両の状態量は、車両の位置、車両の方位、車両の速度、及び、車両の角速度である。以下に、推定する車両の各状態量を示す。
[0028]
 x:車両の位置のx座標
 y:車両の位置のy座標
 θ:車両の方位
 v:車両の速度
 ω:車両の角速度
[0029]
 ここで、車両の状態量をベクトル表記した状態ベクトルを、(x、y、θ、v、ω)とすると、車両の状態量についての状態方程式は、式(1)で表される。
[0030]
[数1]


[0031]
 添え字のkは、時刻を示す。例えば、式(1)の左辺である(x k+1、y k+1、θ k+1、v k+1、ω k+1)は、時刻k+1における車両の状態量を示す。式(1)において、右辺の第2項であるq は、システム雑音(平均0、誤差共分散行列であるQ を有する正規分布N(0、Q ))である。誤差共分散行列とは、分散と共分散との行列である。
[0032]
 本実施形態では、観測部21は、観測対象として、車両の速度、車両の角速度、車両の加速度、車両の位置、及び、車両の方位を観測する。前述した通り、観測部21は、車両の速度について、車速センサー6からの出力に基づき観測する。また、観測部21は、車両の角速度について、ジャイロセンサー7からの出力に基づき観測する。また、観測部21は、車両の加速度について、加速度センサー8からの出力に基づき観測する。また、観測部21は、車両の位置、及び、車両の方位について、GPS受信部5からの出力に基づき観測する。以下に、観測部21が観測する観測対象を示す。なお、以下では、観測対象として車両の速度、車両の角速度、車両の位置、及び、車両の方位を例示する。
[0033]
 v PLS:車速センサー6からの出力に基づき観測される車両の速度
 ω GYR:ジャイロセンサー7からの出力に基づき観測される車両の角速度
 x GPS:GPS受信部5からの出力に基づき観測される車両の位置のx座標
 y GPS:GPS受信部5からの出力に基づき観測される車両の位置のy座標
 θ GPS:GPS受信部5からの出力に基づき観測される車両の方位
[0034]
 ここで、上記の観測対象をベクトル表記した観測ベクトルを(v PLS、ω GYR、x GPS、y GPS、θ GPS)とすると、観測対象についての観測方程式は、式(2)で表される。
[0035]
[数2]


[0036]
 式(2)において、r は、観測雑音(平均0、誤差共分散行列であるR を有する正規分布N(0、R ))である。
[0037]
 以下、カルマンフィルタについて、車両の状態量を予測する予測処理と、車両の状態量を推定する推定処理とに分けて説明する。
[0038]
 なお、以下の説明において、添え字のk+1|kが付与された値は、時刻kまでの情報に基づき予測された時刻k+1における予測値を示す。また、添え字のk|k-1が付与された値は、時刻k-1までの情報に基づき予測された時刻kにおける予測値を示す。また、添え字のk+1|k+1が付与された値は、時刻k+1までの情報に基づき推定された時刻k+1における推定値を示す。また、添え字のk|kが付与された値は、時刻kまでの情報に基づき推定された時刻kにおける推定値を示す。また、添え字のk-1|k-1が付与された値は、時刻k-1までの情報に基づき推定された時刻k-1における推定値を示す。
[0039]
<予測処理>
 カルマンフィルタにおいて予測処理は、車両の状態量の予測値(以下、「車両状態予測値」と表現する)と、車両状態予測値の誤差共分散行列と、を算出する処理である。車両状態予測値は、式(3)に基づき算出される。なお、誤差共分散行列の算出とは、誤差共分散行列の各成分の値の算出を示す。
[0040]
[数3]


[0041]
 式(3)では、時刻kまでの情報に基づき予測された時刻k+1における車両状態予測値の算出を示す。この車両状態予測値は、式(3)の右辺に示すように、時刻kまでの情報に基づき推定された時刻kにおける車両の状態量の推定値により算出される。例えば、車両の位置のx座標の予測値を示すx k+1|kは、時刻kまでの情報に基づき推定された時刻kにおける、車両の位置のx座標(x k|k)の推定値と、車両の方位(θ k|k)の推定値と、車両の速度(v k|k)の推定値とに基づき算出される。なお、式(3)において、Tは、車速センサー6、ジャイロセンサー7、及び、加速度センサー8からの出力に基づき観測部21が観測量を観測する間隔を示す。
[0042]
 なお、時刻k-1までの情報に基づき予測された時刻kの車両状態予測値を算出する場合、式(3)において時刻を1ステップずつ下げた式により算出できる。つまり、時刻k-1までの情報に基づき予測された時刻kにおける車両状態予測値は、時刻k-1までの情報に基づき推定された時刻k-1における車両の状態量の推定値に基づき算出される。
[0043]
 車両状態予測値の誤差共分散行列は、式(4)に基づき算出される。誤差共分散行列は、本実施形態において、車両の状態量に関する分散及び共分散の行列である。分散は、誤差を二乗したものである。つまり、車両の状態量の分散は、車両の状態量の誤差を二乗したものである。したがって、車両状態予測値の誤差共分散行列を算出することは、車両状態予測値の誤差を算出することに相当する。
[0044]
[数4]


[0045]
 式(4)において、Pは、誤差共分散行列を示す。式(4)の左辺は、時刻kまでの情報に基づき予測された時刻k+1における誤差共分散行列を示す。式(4)の左辺に示す誤差共分散行列は、時刻kまでの情報に基づき推定された時刻kにおける誤差共分散行列に基づき算出される。なお、Fは、式(1)の状態方程式から求められるヤコビ行列を示す。また、Fにおける添え字のTは、転置行列を示す。また、Qは、システム雑音の誤差共分散行列を示す。
[0046]
 なお、時刻k-1までの情報に基づき予測された時刻kにおける車両状態予測値の誤差共分散行列を算出する場合、式(4)においてPの添え字の時刻を1ステップずつ下げた式により算出できる。つまり、時刻k-1までの情報に基づき予測された時刻kにおける車両状態予測値の誤差共分散行列は、時刻k-1までの情報に基づき推定された時刻k-1における車両の状態量の推定値の誤差共分散行列により算出される。
[0047]
 このように、予測処理では、例えば、時刻kにおける車両の状態量を予測する場合、時刻k-1までの情報に基づき算出された時刻kにおける車両状態予測値と、時刻k-1までの情報に基づき算出された時刻kにおける車両状態予測値の誤差共分散行列とを算出する。つまり、予測処理は、車両の状態量の確立分布を予測している。
[0048]
<推定処理>
 次に、推定処理について説明する。
 カルマンフィルタにおいて推定処理は、予測処理にて算出した車両状態予測値、及び、車両状態予測値の誤差共分散行列に基づいて、車両の状態量の推定値(以下、「車両状態推定値」と表現する)と、車両状態推定値の誤差共分散行列とを算出する処理である。
[0049]
 推定処理では、式(5)により観測残差が算出される。観測残差とは、観測対象の値と、車両状態予測値から算出される観測対象に対応する値との誤差である。
[0050]
[数5]


[0051]
 式(5)において、左辺は、観測残差をベクトル表記した観測残差ベクトルである。また、式(5)において、右辺の第1項は、観測部21の観測対象とする観測対象ベクトルである。また、式(5)において、右辺の第2項は、予測処理で予測した車両状態予測値に、観測方程式から求められる観測行列であるHを掛けたものである。
[0052]
 車両状態推定値は、式(5)に示す観測残差を用いて、式(6)により算出される。
[0053]
[数6]


[0054]
 式(6)では、時刻k+1までの情報に基づき予測された時刻k+1における車両状態推定値を示す。この車両状態推定値は、式(6)の右辺に示すように、時刻kまでの情報に基づき推定された時刻k+1における車両状態予測値を、観測残差より補正して算出される。なお、時刻kまでの情報に基づき予測された時刻kにおける車両状態推定値を算出する場合、時刻k-1までの情報に基づき予測された時刻kにおける車両状態予測値を観測残差により補正することで算出される。
[0055]
 前述の通り、式(6)は、観測残差を用いて、車両状態予測値を補正し、車両状態推定値を算出する式を示す。式(6)に示すように、車両状態予測値を観測残差にて補正する際の補正係数としてK が用いられる。K は、カルマンゲインと呼ばれ、式(7)で表される。
[0056]
[数7]


[0057]
 式(7)において、R は、観測雑音の誤差共分散行列である。また、添え字の「-1」は、逆行列を示す。式(7)に示すカルマンゲインK は、時刻kまでの情報に基づく時刻k+1における車両状態予測値の誤差共分散行列に基づき算出される。
[0058]
 式(6)において、カルマンゲインK は、車両状態推定値について、車両状態予測値を重視して算出するか、観測部21が観測する観測対象の値を重視して算出するかを決定付ける係数である。
[0059]
 例えば、車速センサー6、ジャイロセンサー7、及び、GPS受信部5からの出力に基づく観測対象の値の誤差が充分に小さい場合、車両状態推定値は、当該誤差が充分に小さいことから、観測対象の値となることが望ましい。これは、車両状態推定値が、誤差の充分に小さい値、つまり、精度の高い値になるためである。つまり、誤差共分散行列であるR の値が充分に小さい場合、車両状態推定値は、観測対象の値となることが望ましい。ここで、R の値が充分に小さい場合のカルマンゲインK をK =H -1として式(6)に与えると、式(6)の右辺の第1項は、無くなる。つまり、車両状態推定値は、誤差が充分に小さい観測対象の値となる。
[0060]
 また、例えば、車両状態予測値の誤差が、車速センサー6、ジャイロセンサー7、及び、GPS受信部5からの出力に基づく観測対象の誤差より充分に小さい場合、車両状態推定値は、車両状態予測値となることが望ましい。これは、車両状態推定値が、観測対象の誤差より充分に誤差が小さい値、つまり、精度の高い値になるためである。つまり、車両状態予測値の誤差共分散行列が、R の値より充分に小さい場合、車両状態推定値は、車両状態予測値となることが望ましい。ここで、車両状態予測値の誤差共分散行列がR の値より充分に小さい場合のカルマンゲインK としてK =0として式(6)に与えると、車両状態推定値は、車両状態予測値となる。
[0061]
 このように、カルマンゲインK は、観測雑音の誤差共分散行列であるR 、及び、車両状態予測値の誤差共分散行列に応じて、車両状態推定値が適切な値となるように設定する係数である。つまり、観測雑音の誤差共分散行列が観測対象の誤差共分散行列である場合、カルマンゲインK は、車両状態予測値の誤差共分散行列が正確に予測されていれば、車速センサー6、ジャイロセンサー7、及び、GPS受信部5からの出力に基づく観測対象の誤差共分散行列に応じて、車両状態推定値が適切な値となるように設定する。
[0062]
 車両状態推定値の誤差共分散行列は、式(8)により算出される。前述した通り、誤差共分散行列は、本実施形態では、車両の状態量に関する、分散及び共分散の行列である。車両の状態量の分散は、車両の状態量の誤差を二乗したものである。したがって、車両状態推定値の誤差共分散行列を算出することは、車両状態推定値の誤差を算出することに相当する。
[0063]
[数8]


[0064]
 式(8)において、Pは、式(4)と同様、誤差共分散行列を示す。また、式(8)において、Iは、単位行列を示す。式(8)の左辺は、時刻k+1までの情報に基づき推定された時刻k+1における誤差共分散行列を示す。式(8)の左辺に示す誤差共分散行列は、時刻kまでの情報に基づき予測された時刻k+1における誤差共分散行列に基づき算出される。
[0065]
 なお、時刻kまでの情報に基づき予測された時刻kにおける車両状態推定値の誤差共分散行列を算出する場合、式(8)においてPの添え字の時刻を1ステップずつ下げた式により算出できる。つまり、時刻kまでの情報に基づき予測された時刻kにおける車両状態推定値の誤差共分散行列は、時刻k-1までの情報に基づき推定された時刻kにおける車両状態予測値の誤差共分散行列により算出される。
[0066]
 式(8)は、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列に、(I-K H)を掛けた式である。式(8)に示すように、車両状態推定値の誤差共分散行列は、カルマンゲインK の値に依存する。
[0067]
 例えば、観測雑音の誤差が充分に小さい場合のカルマンゲインK としてK =H -1を式(8)に与えると、車両状態推定値の誤差共分散行列は零行列となる。これは、推定した車両状態推定値の誤差が充分に小さいことを示す。
[0068]
 また、例えば、観測雑音の誤差より車両状態予測値の誤差が充分に小さい場合のカルマンゲインの値としてK =0を与えることにより、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列になる。これは、車両状態推定値の誤差が、誤差の充分に小さい車両状態予測値の誤差であることを示す。
[0069]
 このように、式(8)において、カルマンゲインK は、観測雑音の誤差共分散行列と、車両状態予測値の誤差共分散行列に応じて、車両状態推定値の誤差共分散行列が適切になるように設定する。つまり、カルマンゲインK は、観測雑音の精度と車両状態予測値の精度とに基づいて、車両状態推定値の誤差共分散行列が適切になるように設定する。
[0070]
 以上、推定処理では、例えば、時刻kにおける車両の状態量を推定する場合、時刻kまでの情報に基づき算出された時刻kにおける車両状態推定値と、時刻kまでの情報に基づき算出された時刻kにおける車両状態推定値の誤差共分散行列とを算出する。つまり、推定処理は、予測処理で予測した車両の状態量の確立分布に基づいて、車両の状態量の確立分布を推定している。
[0071]
 以上の算出により、推定部22は、カルマンフィルタによって、車両状態推定値と、車両状態推定値の誤差共分散行列とを算出することにより、車両の状態を推定する。
[0072]
 前述した通り、カルマンゲインK は、車両状態推定値と、車両状態推定値の誤差共分散行列とを適切に設定する係数である。つまり、車両の状態の推定の精度は、カルマンゲインK に依存する。カルマンゲインK は、前述した通り、観測雑音と、車両状態予測値の誤差共分散行列とに応じて、車両状態推定値と車両状態推定値の誤差共分散行列とを適切に設定する係数である。そのため、車両状態推定値と車両状態推定値の誤差共分散行列とを適切に算出するためには、車両状態予測値の誤差共分散行列が、正確に算出される必要がある。しかしながら、カルマンフィルタでは、システム雑音、及び、観測雑音が白色性の雑音であることが仮定される。つまり、カルマンフィルタにおいて誤差を雑音として扱う場合、誤差の平均が0であることが前提となる。そのため、平均が0とならない誤差、すなわち、累積する誤差を考慮して、車両状態予測値の誤差共分散行列を算出できない場合があり、この場合、カルマンゲインK は、正確に算出されない。これは、車両状態推定値と車両状態推定値の誤差共分散行列とが精度よく算出できない、すなわち、車両の状態を精度よく推定できないことを示す。
[0073]
 そこで、本実施形態の推定部22は、以下のカルマンフィルタにより、車両の状態を推定する。
[0074]
 以下、車両の状態を推定する際のナビゲーション装置1の動作を通して、推定部22による車両の状態の推定について説明する。
[0075]
 図2は、ナビゲーション装置1の動作を示すフローチャートである。
[0076]
 ナビゲーション装置1の観測部21は、車速センサー6、ジャイロセンサー7、及び、加速度センサー8から出力される信号に基づき、車両の速度、車両の角速度、及び、車両の加速度を観測する(ステップSA1)。観測部21は、車速センサー6、ジャイロセンサー7、及び、加速度センサー8が信号を出力する度に、車両の速度、車両の角速度、及び、車両の加速度を観測する。つまり、観測部21がこれらを観測する間隔は、車速センサー6、ジャイロセンサー7、及び、加速度センサー8が検出する間隔と同じ間隔である。
[0077]
 次いで、観測部21は、GPS受信部5の出力に基づき、車両の位置、及び、車両の方位を観測する(ステップSA2)。観測部21は、車両の位置を示す情報と車両の方位を示す情報とがGPS受信部5から出力される度に、車両の位置、及び、車両の方位を観測する。つまり、観測部21が車両の位置及び車両の方位を観測する間隔は、GPS受信部5がGPS電波を受信する間隔となる。
[0078]
 次いで、ナビゲーション装置1の推定部22は、観測部21が観測した観測対象に基づき、車両の状態を推定する車両状態推定処理を実行する(ステップSA3)。
[0079]
 図3は、車両状態推定処理における推定部22の動作を示すフローチャートである。
[0080]
 推定部22は、予測処理を実行する(ステップSB1)。前述した通り、予測処理は、車両状態予測値、及び、車両状態予測値の誤差共分散行列を算出する処理である。車両状態予測値は、式(3)によって算出される。車両状態予測値の誤差共分散行列は、式(9)によって算出される。
[0081]
[数9]


[0082]
 式(9)において、F は、式(1)の状態方程式から求められるヤコビ行列であり、式(10)で表される。また、Q は、システム雑音の誤差共分散行列であり、式(11)で表される。また、C は、前回推定した車両状態推定値の誤差と、観測部21が車速センサー6及びジャイロセンサー7からの出力に基づき観測する観測量の誤差との共分散行列であり、式(12)で表される。Qk+2・Ckがシステム雑音である。
[0083]
[数10]


[0084]
[数11]


[0085]
[数12]


[0086]
 式(12)において、σ vPLSは、観測部21が算出する車両の速度の誤差を示す。また、σ ωGYRは、観測部21が算出する車両の角速度の誤差を示す。前述した通り、σ vPLSは、時間の経過と共に、この速度から求めた移動距離に累積する誤差である。また、σ ωGYRは、σ vPLSと同様、時間と共に、この角速度から求めた方位変化量に累積する誤差である。
[0087]
 式(12)に示すように、車両状態予測値の誤差共分散行列を算出する式(9)において、車速センサー6から出力に基づき観測部21が算出する車両の速度の誤差は、車両の位置の誤差として入力される。すなわち、σ vPLSは、cos(θ k|k)σ vPLST、及び、sin(θ k|k)σ vPLSTとして入力される。また、ジャイロセンサー7からの出力に基づき観測部21が算出する車両の角速度の誤差は、車両の方位の誤差として入力される。すなわち、σ ωGYRは、σ ωGYRTとして入力される。
[0088]
 このように、時間の経過と共に累積する誤差、すなわち、σ vPLSとσ ωGYRとについて、σ vPLSを車両の位置の誤差として入力し、σ ωGYRを車両の方位の誤差として入力したカルマンフィルタにより、推定部22は、車両状態予測値の誤差共分散行列を算出する。
[0089]
 ここで、カルマンフィルタにσ vPLSを車両の位置の誤差として入力し、σ ωGYRを車両の方位の誤差として入力することについて説明する。以下では、カルマンフィルタにσ vPLSを車両の位置の誤差として入力することについて詳述する。
[0090]
 ここで説明の便宜上、車両の状態量を、車両の位置のx座標と車両の速度として例示し、カルマンフィルタにσ vPLSを車両の位置の誤差として入力することについて説明する。この例では、車両の位置のx座標は、車両の速度により決定する。この例では、観測対象は、車両の速度である。
[0091]
 したがって、車両の状態量についての状態方程式は、式(13)で表される。また、観測対象についての観測方程式は、式(14)で表される。
[0092]
[数13]


[0093]
[数14]


[0094]
 式(13)及び式(14)に基づき、時刻kまでの情報に基づき推定した時刻kにおける車両状態推定値の誤差共分散行列を、式(4)、式(6)、及び、式(8)に基づき算出する。ここで、車両状態推定値の誤差共分散行列を算出するに際し、P (k|k)を式(15)とする。P (k|k)は、時刻kまでの情報に基づき推定した時刻kにおける車両状態推定値の誤差共分散行列である。
[0095]
[数15]


[0096]
 式(15)において、p 11 (k|k)は、車両の位置の誤差分散を示す。また、p 22 (k|k)は、車両の速度の誤差分散を示す。また、p 12 (k|k)は、車両の位置と、車両の速度との誤差共分散を示す。
[0097]
 また、車両状態推定値の誤差共分散行列を算出するあたり、F、Q、H、及び、Rのそれぞれを式(16)~式(19)とする。ここで、Fは、状態方程式から求められるヤコビ行列である。また、Qは、システム雑音の分散行列である。また、Hは、観測方程式から求められるヤコビ行列である。また、Rは、観測雑音の分散行列である。観測量が車両の速度であることから、r は、車両の速度の誤差である。つまり、r は、観測部21が観測する車両の速度の誤差分散を示す。
[0098]
[数16]


[0099]
[数17]


[0100]
[数18]


[0101]
[数19]


[0102]
 式(15)~式(19)に基づき、車両状態推定値の誤差共分散行列、すなわち、P (k|K)を算出すると、車両状態推定値の誤差共分散行列は、式(20)で表される。
[0103]
[数20]


[0104]
 したがって、車両の位置の推定値の誤差分散は、式(21)で表される。
[0105]
[数21]


[0106]
 式(21)は、共通因数であるp 11 (k|k-1)でくくると、式(22)に変形される。p 11 (k|k-1)は、時刻k-1までの情報に基づく時刻kにおける車両の位置の予測値の誤差分散である。
[0107]
[数22]


[0108]
 式(22)に示すように、車両の位置の推定値の誤差分散であるp 11 (k|k)は、車両の位置の予測値の誤差分散であるp 11 (k|k-1)に、式(23)を掛けることで求められる。
[0109]
[数23]


[0110]
 式(23)は、式(22)に示すように、車両の位置の予測値の分散であるp 11 (k|k-1)に掛けられる係数である。この係数は、分母に車両の速度の誤差(r )が含まれることを除くと、式(24)と一致する。
[0111]
[数24]


[0112]
 式(24)に示す係数は、「1-(車両の位置の誤差と車両の速度の誤差との相関係数) 」である。これは、式(24)に示す係数が、最小二乗法を用いた線形回帰モデルの非決定係数であることを示す。つまり、車両の位置の誤差と、車両の速度の誤差との相関性が小さければ小さいほど、当該係数は大きい値となり、車両の位置の予測値の分散は大きい値となる。一方で、当該相関性が大きければ大きいほど、当該係数は小さい値となり、車両の位置の予測値の分散は小さい値となる。また、式(22)は、車両の位置の推定値の分散が、車両の速度の誤差に対して指数関数的に減少することを示す。
[0113]
 したがって、車両の位置の推定値の誤差分散は、車両の位置の誤差と、車両の速度の誤差との相関性に基づき決定されていることを示している。また、車両の位置の推定値の誤差は、車両の速度の誤差によって累積しないことも示す。つまり、車両の速度の誤差の累積に伴い、車両の位置の推定値の誤差分散であるp 11 (k|k)を累積させるためには、車両の位置の誤差と、車両の速度の誤差との相関性を小さくする必要がある。つまり、p 11 (k|k)を累積させるためには、カルマンフィルタにおいて、車両の位置の誤差と、車両の速度の誤差との相関性を小さくする必要がある。具体的には、移動距離に累積する速度の誤差に関しては、カルマンフィルタに車両の速度の誤差を入力し、車両の位置の誤差を推定するのではなく、カルマンフィルタに車両の位置の誤差として車両の速度の誤差を入力し、車両の位置の誤差を推定する必要がある。
[0114]
 したがって、式(12)に示すように、移動距離に累積する速度の誤差に関しては、車両の速度の誤差を、車両の速度と微積分の関係にある車両の位置の誤差としてカルマンフィルタに入力する。これによって、移動距離に累積する速度の誤差の分だけ、車両の速度の誤差と車両の位置の誤差との相関性を小さくでき、累積する車両の速度の誤差を累積する車両の位置の誤差として、車両の位置の予測値の誤差共分散行列を算出できる。つまり、累積する車両の速度の誤差を考慮した、車両の位置の予測値の誤差共分散行列を算出できる。
[0115]
 しかしながら、車両の位置の誤差として車両の速度の誤差をカルマンフィルタに入力する際、式(4)をそのまま用いることができない。
[0116]
 式(4)において、右辺の第1項は、前回に推定した車両状態推定値の誤差共分散行列を、状態方程式から求められるヤコビ行列により今回の車両の状態量の誤差共分散行列に変換している。一方、右辺の第2項は、システム雑音の誤差共分散行列である。つまり、式(4)は、前回の車両状態推定値の誤差共分散行列を今回の車両状態推定値の誤差共分散行列に変換したものに、システム雑音の誤差共分散行列を加算することで、車両状態予測値の誤差共分散行列を算出する式である。ここで、式(4)は、式(25)に示す命題が用いられている。
[0117]
[数25]


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


[0120]
 式(26)において、Cov()は共分散を示す。したがって、式(24)と比較すると、式(26)は、確率変数Xと確率変数Yとの共分散が入力されている。
[0121]
 式(26)に従った式(9)を用いることにより、車両の状態量の誤差共分散行列と、累積する誤差の誤差共分散行列との独立性をなくして、正確に車両状態予測値の誤差共分散行列を算出できる。
[0122]
 以上、車両の速度の誤差を車両の位置の誤差としてカルマンフィルタに入力することを説明した。角速度の誤差を方位の誤差としてカルマンフィルタに入力することについての説明も、同様に説明できる。
[0123]
 以上の説明のように、観測量と微積分の関係にある状態量の誤差として、観測量の誤差が入力され、また、観測量の誤差と、車両の状態量の誤差との独立性のない式により、累積する誤差を考慮した、車両状態予測値の誤差共分散行列を算出できる。
[0124]
 図3に示すフローチャートの説明に戻り、推定部22は、予測処理を実行すると、推定処理を実行する。推定部22は、式(5)~式(8)に基づいて、車両状態推定値と、車両状態推定値の誤差共分散行列とを算出する。
[0125]
 前述の通り、カルマンゲインK は、観測雑音の誤差共分散行列と、車両予測値の誤差共分散行列とに応じて、車両状態推定値と車両状態推定値の誤差共分散行列とを適切に設定する係数である。予測処理において、推定部22は、累積する誤差を考慮した車両状態予測値の誤差共分散行列を算出できるため、カルマンゲインK を正確に算出できる。したがって、推定部22は、車両状態推定値と車両状態推定値の誤差とを精度よく算出できる。
[0126]
 また、式(8)に示すように、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列に(I-K H)を掛けて算出される。したがって、車両状態予測値の誤差共分散行列は、累積する誤差を考慮した誤差共分散行列であるため、算出される車両状態推定値の誤差共分散行列も累積する誤差を考慮した誤差共分散行列である。
[0127]
 このように、推定部22は、累積する誤差を考慮した車両状態予測値の誤差共分散行列を算出できるため、カルマンゲインK が正確に算出でき、カルマンフィルタにより精度よく車両の状態を推定できる。
 また、カルマンフィルタで車両の状態を推定しているため、誤差を推定の対象としたカルマンフィルタに基づき車両の状態を推定する構成と比較し、誤差とは別に車両の状態を推定する必要がなく、容易に累積する誤差を考慮した車両の状態を推定できる。
[0128]
 なお、状態量として車両の速度を推定する場合、推定部22は、車両の加速度の誤差を車両の速度の誤差として入力したカルマンフィルタに基づき車両の速度の予測値の誤差共分散行列を算出し、車両の速度を推定する構成としてもよい。これにより、累積する加速度の誤差を考慮した車両の速度の予測値の誤差共分散行列が算出されるため、カルマンゲインK を正確に算出でき、カルマンフィルタにより車両の速度を精度よく推定できる。
[0129]
 上述した通り、推定部22は、GPS受信部5が出力する車両の位置を示す情報と車両の方位を示す情報とに基づき、観測部21が観測した車両の位置と車両の方位とを加味して、車両状態推定値と車両状態推定値の誤差共分散行列とを算出し、車両の状態を推定する構成である。しかしながら、観測部21が観測する車両の位置と車両の方位とを加味して、推定部22が車両の状態を推定する構成に限定されない。すなわち、推定部22は、GPS受信部5からの出力に基づくことなく、車両の状態を推定してもよい。
[0130]
 例えば、車両がGPS電波をGPS受信部5により受信できない環境下にいる場合、推定部22は、GPS受信部5からの出力に基づく車両の位置及び車両の方位の成分のない観測方程式に基づき車両の状態量を推定する。
[0131]
 式(12)に示すように、推定部22は、車両状態予測値の誤差共分散行列を算出する際、車速センサー6及びジャイロセンサー7から出力される信号の間隔に基づき、車両状態予測値の誤差共分散行列を算出する。また、推定部22は、式(3)に示すように、車両状態予測値を、前回推定した車両状態推定値と当該間隔に基づき算出する。したがって、推定部22は、車両状態予測値、及び、車両状態予測値の誤差共分散行列の算出に際し、GPS受信部5の受信間隔に依存しない。したがって、推定部22は、式(5)において、GPS受信部5からの出力に基づく観測対象の成分のない式に基づくことにより、車両状態推定値と車両状態推定値の誤差共分散行列とを算出できる。
[0132]
 つまり、推定部22は、GPS受信部5のGPS電波の受信の間隔に依存せず、車両の状態を推定できる。これは、推定部22が車速センサー6、ジャイロセンサー7、及び、加速度センサー8の検出間隔に基づき、車両の状態を推定することを示す。したがって、推定部22が車速センサー6、ジャイロセンサー7、及び、加速度センサー8の検出間隔が、GPS受信部5の受信間隔より短い場合において以下の効果を奏する。すなわち、当該受信間隔に基づいてカルマンフィルタにより車両の状態を推定する構成より、車両の状態を推定について高い頻度で実行でき、また、GPS受信部5の受信環境に依存することなく、車両の状態を推定できる。
[0133]
 前述した通り、ナビゲーション装置1の制御部2は、推定した車両の状態に基づき、マップマッチングの対象となるリンクの評価を実行する。
[0134]
 図4は、推定した車両の状態に基づくマップマッチングの処理を説明するため、地図上の道路R1と、道路R2とを示す図である。図4において、道路R1は、方向Y1に向かって延在する道路である。また、道路R2は、方向Y1と並行でない方向Y2に向かって延在する道路である。
[0135]
 図4において、リンクL1は、道路R1に対応するリンクである。また、リンクL2は、道路R2に対応するリンクである。
[0136]
 また、図4において、マークαは、推定部22により推定された車両の位置を示すマークである。以下、図4を用いた説明では、推定部22により、車両が、位置M1に位置し、進行方向X1に向かって走行しているものと推定されたとする。図4に示すように、位置M1は、道路R1上の位置であって、道路R1の幅方向の中心から、方向Y1に向かって右側に離間した位置である。
[0137]
 道路R1と、道路R2と、推定された車両の位置との関係が、図4に示す関係である場合、マップマッチングに際し、まず、制御部2は、地図データ3aを参照し、マップマッチングの候補となるリンクであるマップマッチング候補リンクを取得する。制御部2は、マップマッチング候補リンクとして、推定された車両の位置から予め設定された所定の範囲内に位置し、かつ、車両とリンクとの方位の誤差が所定範囲内である1又は複数のリンクを取得する。図4の例では、リンクL1、及び、リンクL2は、推定された車両の位置である位置M1から所定範囲に位置し、また、車両とリンクとの方位の誤差が所定範囲内のリンクである。このため、制御部2は、リンクL1、及び、リンクL2をマップマッチング候補リンクとして取得する。
[0138]
 次いで、制御部2は、取得したマップマッチング候補リンクのそれぞれについて、リンクを評価する評価量を算出する。
[0139]
 評価量は、推定された車両の位置とマップマッチング候補リンクの位置との誤差、及び、推定された車両の方位とマップマッチング候補リンクの方位との誤差に基づいて、下記に示す式(27)により算出される値である。
[0140]
 τ=δx /Δx +δy /Δy +δθ /Δθ ・・・(27)
[0141]
 ここで、推定された車両の位置とマップマッチング候補リンクの位置との誤差とは、推定された車両の位置からマップマッチング候補リンクに対して垂線を延ばした場合における垂線及びマップマッチング候補リンクとの交点と、推定された車両の位置との、x軸方向、及び、y軸方向における差である。式(27)において、δxは、当該交点のx座標と、推定された車両の位置のx座標との差を示す。また、δyは、当該交点のy座標と、推定された車両の位置のy座標との差を示す。
[0142]
 図4の例において、推定された車両の位置とリンクL1との位置の誤差は、位置M1からリンクL1に対して延ばした垂線S1及びリンクL1の交点MM1と、位置M1とのx軸方向、及び、y軸方向における差である。また、図4において、推定された車両の位置とリンクL2との位置の誤差は、位置M1からリンクL2に対して延ばした垂線S2及びリンクL2の交点MM2と、位置M1とのx軸方向、及び、y軸方向における差である。
[0143]
 また、推定された車両の方位とマップマッチング候補リンクの方位との誤差とは、推定された車両の方位に対応する角度と、マップマッチング候補リンクの方位に対応する角度との差である。
[0144]
 前述した通り、車両の方位とは、車両の進行方向の方角を意味する。図4の例では、推定された車両の方位は、進行方向X1の方角である。また、車両の方位に対応する角度とは、東に向かう方向を基準として、東に向かう方向と、車両の方位との反時計回りの離間角度を意味する。図4の例では、推定された車両の方位に対応する角度は、仮想直線K1が東西に延びる仮想直線であるとした場合に、角度θ1である。
[0145]
 また、リンクの方位とは、リンクが延在する方向の方角を意味する。リンクが延在する方向とは、リンクに沿った2つの方向のうち、リンクに対応する道路において車両が走行可能な方向に対応する方向を意味する。また、リンクの方位に対応する角度とは、東に向かう方向を基準として、東に向かう方向と、リンクの方位との反時計回りの離間角度を意味する。
[0146]
 図4の例では、リンクL1の方位は、方向Z1の方角である。また、リンクL1の方位に対応する角度は、仮想直線K2が東西に延びる仮想直線であるとした場合に、角度θ2である。また、図4の例では、リンクL2の方位は、方向Z2の方角である。また、リンクL2の方位に対応する角度は、仮想直線K3が東西に延びる仮想直線であるとした場合に、角度θ3である。
[0147]
 図4の例において、車両とリンクL1との方位誤差は、角度θ1と角度θ2との差である。また、車両とリンクL2との方位誤差は、角度θ1と角度θ3との差である。つまり、図4の例において、式(27)では、δθは、角度θ1と角度θ2との差、または、角度θ1と角度θ3との差を示す。
[0148]
 式(27)に示すように、評価量τは、δxを二乗した値にΔxを二乗した値で割ったものと、δyを二乗した値にΔyを二乗した値で割ったものと、δθを二乗した値にΔθを二乗した値で割ったものと、の和により算出される。Δxは、推定部22が推定した車両のx軸方向の位置の誤差を示す。つまり、Δx は、推定部22が推定した車両のx軸方向の位置の分散を示す。また、Δyは、推定部22が推定した車両のy軸方向の位置の誤差を示す。つまり、Δy は、推定部22が推定した車両のy軸方向の位置の分散を示す。また、Δθは、推定部22が推定した車両の方位の誤差を示す。つまり、Δθ は、推定部22が推定した車両の方位の分散を示す。Δx 、Δy 、及び、Δθ は、推定部22が推定した車両状態推定値の誤差共分散行列が含む分散である。このように、評価量は、車両の位置とリンクの位置との誤差を推定した車両の位置の誤差により無次元化した値と、車両の方位とリンクの方位との誤差を、推定した車両の位置の誤差により無次元化した値との和により算出される値である。
[0149]
 図4の例では、制御部2は、マップマッチング候補リンクとして取得したリンクL1の評価量とリンクL2の評価量とを算出する。
[0150]
 リンクL1の評価量をτ1とし、位置M1と交点MM1とのx座標の差をδx1とし、位置M1と交点MM1とのy座標の差をδy1とし、角度θ1と角度θ2との差をδθ1とした場合、評価量τ1は、下記に示す式(28)で表される。
[0151]
 τ1=δx1 /Δx +δy1 /Δy +δθ1 /Δθ ・・・(28)
[0152]
 一方、リンクL2の評価量をτ2とし、位置M1と交点MM2とのx座標の差をδx2とし、位置M1と交点MM2とのy座標の差をδy2とし、角度θ1と角度θ3との差をδθ2とした場合、評価量τ2は、下記に示す式(29)で表される。
[0153]
 τ2=δx2 /Δx +δy2 /Δy +δθ2 /Δθ ・・・(29)
[0154]
 図4の例では、距離l2より距離l1が小さい。すなわち、位置M1と交点MM2とにおけるx軸座標及びy座標の差より、位置M1と交点MM2とにおけるx座標及びy座標の差が小さい。また、図4の例では、角度θ1と角度θ3との差より角度θ1と角度θ2との差が小さい。このため、リンクL1の評価量であるτ1の値の方が、リンクL2の評価量であるτ2の値よりも小さくなる。したがって、制御部2は、評価量が小さいリンクL1を、車両の現在位置を対応付けるリンクとして決定する。
[0155]
 このように、制御部2は、推定した車両の状態に基づいて、マップマッチングの対象となるリンクの評価を実行する。上述したように、制御部2は、リンクを評価する際、推定部22が推定した車両状態推定値の誤差共分散行列が含む分散に基づき、リンクを評価する。当該誤差共分散行列は、累積する誤差を考慮して算出された車両状態予測値の誤差共分散行列に基づき算出された行列であり、精度よく算出された誤差を含む行列である。したがって、制御部2は、当該誤差共分散行列が含む分散をリンクの評価に用いることにより、正確にリンクを評価できる。
[0156]
 以上、説明したように、ナビゲーション装置1(車載装置)は、車両の変動に関する観測量を観測する観測部21と、カルマンフィルタにより、車両の状態を示す状態量を推定する推定部22と、を備える。推定部22は、車両の状態量の予測値を算出し、観測量と微積分の関係にある状態量の誤差として観測量の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列(誤差)を算出し、算出した予測値と予測値の誤差共分散行列とに基づいて、カルマンフィルタにより車両の状態量の推定値と推定値の誤差共分散行列とを算出する。
[0157]
 これにより、観測量の誤差を微積分の関係にある状態量の誤差として入力することにより、観測量の誤差と状態量の誤差との相関性を小さくし、累積する観測量の誤差を考慮して車両状態予測値の誤差共分散行列を算出できるため、カルマンゲインK を正確に算出でき、カルマンフィルタにより車両の状態を精度よく推定できる。
[0158]
 また、推定部22は、前回算出した車両状態推定値の誤差と、車両の状態量の誤差として入力された観測量の誤差との共分散が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。
[0159]
 前述した通り、式(4)は、状態量の誤差共分散行列と、システム雑音の誤差共分散行列とが互いに独立であることが前提となる命題が用いられている。しかしながら、システム雑音の誤差共分散行列を累積する誤差の誤差共分散行列とする場合、車両の状態量の誤差共分散行列と、累積する誤差の誤差共分散行列とが独立でない。そこで、式(26)に従った式(9)、すなわち、前回推定した車両状態推定値の誤差と、状態量の誤差として入力された観測量の誤差との共分散が入力された式を用いることにより、車両の状態量の誤差共分散行列と、累積する誤差の誤差共分散行列との独立性をなくして、正確に車両状態予測値の誤差共分散行列を算出できる。
[0160]
 また、観測部21は、車両の速度を検出する車速センサー6からの出力に基づき、車両の速度を観測する。推定部22は、車両の位置の誤差として車両の速度の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。
[0161]
 これにより、車両の速度の誤差と車両の位置の誤差との相関性を小さくすることで、累積する車両の速度の誤差を考慮でき、カルマンフィルタにより車両の位置を精度よく推定できる。
[0162]
 また、観測部21は、車両の角速度を検出するジャイロセンサー7(角速度センサー)からの出力に基づき、車両の角速度を観測する。推定部22は、車両の方位の誤差として車両の角速度の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。
[0163]
 これにより、車両の角速度の誤差と車両の方位の誤差との相関性を小さくすることで、累積する車両の角速度の誤差を考慮でき、カルマンフィルタにより車両の方位を精度よく推定できる。
[0164]
 観測部21は、車両の加速度を検出する加速度センサー8からの出力に基づき、車両の加速度を観測する。推定部22は、車両の速度の誤差として車両の加速度の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。
[0165]
 これにより、車両の加速度の誤差と車両の速度の誤差との相関性を小さくすることで、累積する車両の加速度の誤差を考慮でき、カルマンフィルタにより車両の速度を精度よく推定できる。
[0166]
 上述した実施形態は、あくまでも本発明の一態様を例示するものであって、本発明の趣旨を逸脱しない範囲で任意に変形、及び応用が可能である。
[0167]
 例えば、図1は、本願発明を理解容易にするために、ナビゲーション装置1の機能構成を主な処理内容に応じて分類して示した概略図であり、ナビゲーション装置1の構成は、処理内容に応じて、さらに多くの構成要素に分類することもできる。また、1つの構成要素がさらに多くの処理を実行するように分類することもできる。
[0168]
 また、例えば、図2、及び、図3のフローチャートの処理単位は、制御部2の処理を理解容易にするために、主な処理内容に応じて分割したものであり、処理単位の分割の仕方や名称によって、本発明が限定されることはない。制御部2の処理は、処理内容に応じて、さらに多くの処理単位に分割してもよい。また、1つの処理単位がさらに多くの処理を含むように分割してもよい。
[0169]
 また、例えば、上述した実施形態では、ナビゲーション装置1として車載型を例示したが、ナビゲーション装置1の形態は任意であり、例えば歩行者が携帯するポータブル型の装置でも良い。

符号の説明

[0170]
 1 ナビゲーション装置(車載装置)
 6 車速センサー(センサー)
 7 ジャイロセンサー(センサー)
 8 加速度センサー(センサー)
 21 観測部
 22 推定部

請求の範囲

[請求項1]
 車両に搭載される車載装置であって、
 センサーからの出力に基づき、前記車両の変動に関する観測量を観測する観測部と、
 カルマンフィルタにより、前記車両の状態を示す状態量を推定する推定部と、を備え、
 前記推定部は、
 前記車両の前記状態量の予測値を算出し、
 前記観測量と微積分の関係にある前記状態量の誤差として前記観測量の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出し、
 算出した前記予測値と前記予測値の誤差とに基づいて、前記カルマンフィルタにより前記車両の前記状態量の推定値と前記推定値の誤差とを算出する、
 ことを特徴とする車載装置。
[請求項2]
 前記推定部は、
 前回算出した前記状態量の前記推定値の誤差と、前記状態量の誤差として入力された前記観測量の誤差との共分散が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
 ことを特徴とする請求項1に記載の車載装置。
[請求項3]
 前記観測部は、前記車両の速度を検出する車速センサーからの出力に基づき、前記車両の速度を観測し、
 前記推定部は、前記車両の位置の誤差として前記車両の速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
 ことを特徴とする請求項1又は2に記載の車載装置。
[請求項4]
 前記観測部は、前記車両の角速度を検出する角速度センサーからの出力に基づき、前記車両の角速度を観測し、
 前記推定部は、前記車両の方位の誤差として前記車両の角速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
 ことを特徴とする請求項1又は2に記載の車載装置。
[請求項5]
 前記観測部は、前記車両の加速度を検出する加速度センサーからの出力に基づき、前記車両の加速度を観測し、
 前記推定部は、前記車両の速度の誤差として前記車両の加速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
 ことを特徴とする請求項1又は2に記載の車載装置。
[請求項6]
 車両の状態を示す状態量をカルマンフィルタにより推定する推定方法であって、
 前記車両の前記状態量の予測値を算出し、
 センサーからの出力に基づき観測された前記車両の変動に関する観測量と微積分の関係にある前記状態量の誤差として、前記観測量の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出し、
 算出した前記予測値と前記予測値の誤差とに基づいて、前記カルマンフィルタにより前記車両の前記状態量の推定値と前記推定値の誤差とを算出する、
 ことを特徴とする推定方法。
[請求項7]
 前回算出した前記状態量の前記推定値の誤差と、前記状態量の誤差として入力された前記観測量の誤差との共分散が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
 ことを特徴とする請求項6に記載の推定方法。
[請求項8]
 前記車両の速度を検出する車速センサーからの出力に基づき、前記車両の速度を観測し、
 前記車両の位置の誤差として前記車両の速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
 ことを特徴とする請求項6又は7に記載の推定方法。
[請求項9]
 前記車両の角速度を検出する角速度センサーからの出力に基づき、前記車両の角速度を観測し、
 前記車両の方位の誤差として前記車両の角速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
 ことを特徴とする請求項6又は7に記載の推定方法。
[請求項10]
 前記車両の加速度を検出する加速度センサーからの出力に基づき、前記車両の加速度を観測し、
 前記車両の速度の誤差として前記車両の加速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
 ことを特徴とする請求項6又は7に記載の推定方法。

図面

[ 図 1]

[ 図 2]

[ 図 3]

[ 図 4]