JP2015094689A - 車両用走行軌跡算出装置 - Google Patents

Abstract

【課題】作動停止中に車両移動が発生した後、再起動時に、より速やかに走行軌跡を精度良く算出できるようにする。【解決手段】装置が起動したとき、車両用走行軌跡算出装置の作動停止中に車両移動が発生したか否かを判定し、車両移動が発生したと判定した場合、ＧＰＳ受信機より出力される位置および方位を現時刻の車両の位置、方位とみなし、誤差共分散行列のうち、車両の位置および方位の各誤差の予測誤差についてのみ所定値にリセットし、慣性センサの補正量および慣性センサの補正量の誤差の予測誤差については、記憶媒体に記憶された起動前の各値を用いて補正を行う。【選択図】図２

Description

本発明は、車両用走行軌跡算出装置に関するものである。

車両の現在位置や走行軌跡を検出する車両用走行軌跡算出装置は、ナビゲーションシステムの他にも、車両の自動走行を実現する運転支援システム等に広く用いられるようになってきている。自車両の挙動のみを計測して制御するシステム（ＶＳＣ(Vehicle Stability Control)、ＡＢＳ（Anti-lock Brake System）など）は従来からあったが、昨今では車両周辺環境に応じた車両制御をするシステムまで既に製品化されつつある。周辺環境に応じた車両制御システムにも種々あり、その中でも特に地図情報に基づいた車両制御システム（以下、地図利用ＡＤＡＳと称す）が注目されている。

地図利用ＡＤＡＳ（地図利用Advanced Driver Assistance System）の実現には、いつでもどこでも地図上の自車位置を特定する技術が欠かせない。こうした技術はマップマッチングと呼ばれ、主に道路地図形状と走行軌跡形状の一致度を指標としている。このため、地図利用ADASを実現する為には、いつでもどこでも正確な走行軌跡形状を計測出来る技術が鍵となる。

ところで、走行軌跡形状は車両の回転角速度を検出するジャイロスコープや、移動距離に応じた車速パルス信号を出力する車速センサ等の慣性センサと、ＧＰＳ受信機を利用した衛星航法出力から得ている。衛星電波環境が良好な場所（オープンスカイ環境下）では衛星航法だけでも十分だが、不調となる場所（トンネル内、ビル街、高架下等）では慣性センサも活用して走行軌跡形状を得る。しかし、廉価な慣性センサは特性の個体差が大きく、そのままでは歪んだ走行軌跡形状しか得ることは出来ない。このため、いつでもどこでも正確な走行軌跡形状を得るには、衛星航法と複合させた慣性センサの自動校正機能が不可欠となる。

自動校正機能は各社様々なものを開発しているが、そのうちの一つとして逐次処理型フィルタ（カルマンフィルタ）による特性推定手法がある。このような手法として、慣性センサの出力値および衛星航法出力に基づいて現時刻の車両の位置と方位を特定するとともに、車両の位置、方位および慣性センサの補正量に対する各誤差を状態量としたカルマンフィルタを用いて各誤差を推定するとともに各誤差の予測誤差を成分として含む誤差共分散行列を算出し、各誤差および誤差共分散行列を用いて慣性センサの自動校正を実現するようにしたものがある（例えば、特許文献１参照）。

この際、衛星航法結果に含まれるノイズを考慮し、自動校正機能は長い時間をかけて緩やかに学習を進める必要がある。このため、車両の位置、方位、慣性センサの補正量およびこれらの各誤差の誤差共分散行列を不揮発性記憶媒体でバックアップしておき、車両のエンジン等の停止に伴って車両用走行軌跡算出装置が停止状態となった後、再起動する際には、バックアップされた各値を読み戻して慣性センサの出力値や車両の位置と方位の補正を行うようにしている。

ところが、カーフェリーやレッカー車等で車両が運搬される場合など、車両用走行軌跡算出装置の停止状態中に車両移動が発生すると、実際の車両の位置および方位と、バックアップされた車両の位置および方位（以下、バックアップ位置／方位）にずれが生じてしまう。このような場合、車両用走行軌跡算出装置が再起動した際に、実際の車両の位置および方位と異なっているにもかかわらず、１時刻前の位置や方位はバックアップ位置／方位であるとみなして上記補正が行われてしまう。この結果、現時刻の衛星航法出力と比較すると１時刻の間に位置／方位が急変したように見え、上記補正処理が慣性センサの補正量を調整する事でつじつまを合わせようとするために、慣性センサの補正量を誤推定してしまうといった問題が発生する。

なお、車両用走行軌跡算出装置の停止状態中に、車両がターンテーブル上で回転する場合に対する従来技術として、装置の再起動時に、衛星航法により測位される最初の現在位置データの表示をキャンセルするようにして、現在位置が狂って表示されることを防止するようにしたものがある（例えば、特許文献２参照）。

特開平８−６８６５４号公報 特開２０１０−１９０７２１号公報

上記特許文献２に記載された装置は、装置の再起動時に、衛星航法により測位される最初の現在位置データの表示をキャンセルするようにしたものであり、慣性センサの補正量の誤推定に関しては言及していない。

また、前述の通り慣性センサの補正は長い期間に渡って学習を進める必要がある為、ひとたび慣性センサの補正量を誤推定してしまうと、再び学習が進み、走行軌跡形状を精度良く算出できるようになるまでに時間がかかるといった問題がある。

本発明は上記問題に鑑みたもので、作動停止中に車両移動が発生した後、再起動時に、より速やかに走行軌跡を精度良く算出できるようにすることを目的とする。

上記目的を達成するため、請求項１に記載の発明は、車両に搭載された慣性センサ（１、２）の出力値およびＧＰＳ受信機（３）の出力値に基づいて現時刻の前記車両の位置と方位を特定するとともに、前記車両の位置、方位および前記慣性センサの補正量に対する各誤差を状態量としたカルマンフィルタを用いて前記各誤差を推定するとともに前記各誤差の予測誤差を成分として含む誤差共分散行列を算出し、前記各誤差および前記誤差共分散行列を用いて前記車両の位置、方位および前記慣性センサの出力値の補正を行う演算処理手段（ステップ１００〜４００）を備え、前記車両の位置と方位に基づく前記車両の走行軌跡を算出する車両用走行軌跡算出装置であって、当該車両用走行軌跡算出装置が作動を停止する際、前記補正処理手段により補正された前記車両の位置、方位、前記慣性センサの補正量、および前記誤差共分散行列を記憶媒体へ記憶するバックアップ手段と、当該車両用走行軌跡算出装置が起動したとき、前記ＧＰＳ受信機の出力値に基づいて前記車両の位置を特定し、当該車両の位置と前記記憶媒体に記憶された前記起動前の前記車両の位置に基づいて、車両用走行軌跡算出装置の作動停止中に車両移動が発生したか否かを判定する移動判定手段を備え、前記移動判定手段により前記作動停止中に車両移動が発生したと判定された場合、前記補正処理手段は、前記ＧＰＳ受信機の出力値より特定される位置および方位を現時刻の前記車両の位置、方位とみなすとともに、前記誤差共分散行列のうち、前記車両の位置および方位の各誤差の予測誤差についてのみ所定値にリセットし、前記慣性センサの補正量および前記慣性センサの補正量の誤差の予測誤差については、前記記憶媒体に記憶された前記起動前の各値を用いて前記車両の位置、方位および前記慣性センサの出力値の補正を行うことを特徴としている。

このような構成によれば、当該車両用走行軌跡算出装置が作動停止中に車両移動が発生したと判定された場合、ＧＰＳ受信機の出力値より特定される位置および方位を現時刻の車両の位置、方位とみなすとともに、誤差共分散行列のうち、車両の位置および方位の各誤差の予測誤差についてのみ所定値にリセットし、慣性センサの補正量および慣性センサの補正量の誤差の予測誤差については、記憶媒体に記憶された起動前の各値を用いて車両の位置、方位および前記慣性センサの出力値の補正が行われるので、慣性センサの出力値の誤補正が防止され、作動停止中に車両移動が発生した後、再起動時に、より速やかに走行軌跡を精度良く算出することができる。

なお、この欄および特許請求の範囲で記載した各手段の括弧内の符号は、後述する実施形態に記載の具体的手段との対応関係を示すものである。

本発明の一実施形態を示す概略構成図である。 航法演算のメインルーチンの演算処理を示すフローチャートである。 方位変化量・移動距離の演算処理を示すフローチャートである。 相対軌跡の演算処理を示すフローチャートである。 絶対方位・絶対位置の演算処理を示すフローチャートである。 ＧＰＳとの複合化処理を示すフローチャートである。 カルマンフィルタのモデルを示す構成図である。

以下、本発明を図に示す実施形態について説明する。本実施形態においては、航法演算とＧＰＳとの複合化を図るため、カルマンフィルタを用いている。このカルマンフィルタの概要について説明する。このカルマンフィルタにおいては、図７に示すように、信号生成過程と観測過程に分けられる。図において、線形システム（φ）があり、そのシステムの状態Ｘ（ｔ）に対して、観測行列Ｈで関係付けられるＸ（ｔ）の一部が観測できる場合に、フィルタはＸ（ｔ）の最適な推定値を与える。ここで、ωは信号生成過程にて発生する雑音であり、ｖは観測過程にて発生する雑音である。このフィルタの入力はＹ（ｔ）であり、出力はＸ（ｔ）の最適推定値である。

時刻ｔまでの情報を用いた状態Ｘの最適推定値、すなわち状態量Ｘ（ｔ｜ｔ）は、数１により求められる。

（数１）Ｘ（ｔ｜ｔ）＝Ｘ（ｔ｜ｔ−１）＋Ｋ（ｔ）｛Ｙ（ｔ）−ＨＸ（ｔ｜ｔ−１）｝
ここで、Ｘ（ｔ｜ｔ−１）は事前推定値、Ｋ（ｔ）はカルマンゲインであり、それぞれ数２、数３により表される。

（数２）Ｘ（ｔ｜ｔ−１）＝φＸ（ｔ−１｜ｔ−１）

（数３）Ｋ（ｔ）＝Ｐ（ｔ｜ｔ−１）Ｈ^T （ＨＰ（ｔ｜ｔ−１）Ｈ^T ＋Ｖ）⁻¹
ここで、Ｐは状態量Ｘの誤差共分散行列であり、Ｐ（ｔ｜ｔ−１）は誤差共分散行列の予測値、Ｐ（ｔ−１｜ｔ−１）は誤差共分散行列であり、それぞれ数４、数５により表される。

（数４）Ｐ（ｔ｜ｔ−１）＝φＰ（ｔ−１｜ｔ−１）φ^T ＋Ｗ

（数５）Ｐ（ｔ−１｜ｔ−１）＝（Ｉ−Ｋ（ｔ−１）Ｈ）Ｐ（ｔ−１｜ｔ−２）
なお、Ｖは観測過程で発生する雑音ｖの分散、Ｗは信号過程で発生する雑音ωの分散である。また、Ａ（ｉ｜ｊ）は時刻ｊまでの情報に基づく時刻ｉでのＡの推定値を表す。なお、添字の^T は転置行列を意味し、^-1 は逆行列を意味する。Ｉは単位行列である。

さらに、ＶとＷは平均０の白色ガウス雑音であり、互いに無相関である。上記のようなカルマンフィルタにおいて、状態量Ｘと誤差共分散行列Ｐの初期値に適当な誤差を与えてやり、新しい観測が行われる度に以上の計算を繰り返し行うことにより、状態量Ｘを精度良く推定することが可能となる。このようなカルマンフィルタを推測航法へ適用したのが本実施形態である。

まず、上記の信号生成過程の定義について説明する。推測航法でのカルマンフィルタは、航法演算の誤差の補正を目的とするので、状態量Ｘは以下の５つの誤差値を定義する。この誤差値の時間的な変化を与えるものがプロセス行列φである。
（１）オフセット誤差（εＧ）

（数６）εＧ_t ＝εＧ_t-1 ＋ω₀
確定的な変化はなく、前回の誤差にノイズが付加される。
（２）絶対方位誤差（εＡ）

（数７）εＡ_t ＝Ｔ×εＧ_t-1 ＋εＡ_t-1＋ω₁
前回の誤差に、オフセット誤差に前回からの経過時間をかけて求める方位誤差とノイズが付加される。
（３）距離係数誤差（εＫ）

（数８）εＫ_t ＝εＫ_t-1 ＋ω₂
確定的な変化はなく、前回の誤差にノイズが付加される。
（４）絶対位置北方向誤差（εＹ）

（数９）εＹ_t ＝ｓｉｎ（Ａ_T ＋εＡ_t-1＋εＧ_t-1 ×Ｔ／２）×Ｌ×（１＋εＫ_t-1 ）
−ｓｉｎ（Ａ_T ）×Ｌ＋εＹ_t-1
前回の誤差に方位誤差・距離誤差によって生じる誤差が付加される。
（５）絶対位置東方向誤差（εＸ）

（数１０）εＸ_t ＝ｃｏｓ（Ａ_T ＋εＡ_t-1＋εＧ_t-1×Ｔ／２）×Ｌ×（１＋εＫ_t-1 ）
−ｃｏｓ（Ａ_T ）×Ｌ＋εＸ_t-1
前回の誤差に方位誤差・距離誤差によって生じる誤差が付加される。上記の定義において、Ａ_T は真の絶対方位、Ｌは前回からの移動距離、Ｔは前回からの経過時間である。

上記の各式を状態量で偏微分し線形化すると信号生成過程は以下のように定義される。

（数１１）

上記Ａは、絶対方位Ａ_T ＋εＡ_t-1 ＋εＧ_t-1 ×Ｔ／２を意味する。この値は真の絶対方位Ａ_Tにセンサ誤差が加わったものであり、後述するように、方位変化量から求められる絶対方位Ａとする。また、ω₀は、オフセット雑音（温度ドリフト等によるオフセットの変動分）、ω₁は絶対方位雑音（ジャイロのゲイン的な誤差）、ω₂ は距離係数雑音（経年変化）を意味する。

次に、上記観測過程の定義について説明する。観測値は航法演算の出力と、ＧＰＳの出力の差より求める。それぞれの出力には誤差が含まれるため、観測値において、航法演算の誤差とＧＰＳの誤差の和が得られる。この観測値Ｙと状態量Ｘを関係付け、数１２のように定義される。

（数１２）

但し、観測過程で発生する雑音ｖはＧＰＳの雑音であり、数１３のように定義される。

（数１３）

以上の定義を基に、カルマンフィルタを用いた推測航法について説明する。図１に、本発明の一実施形態に係る車両用走行軌跡算出装置の全体構成を示す。本車両用走行軌跡算出装置は、車両に搭載されるナビゲーション装置における現在位置検出機能を実現する現在位置検出部７として構成されている。

本ナビゲーション装置は、車速センサ１およびジャイロスコープ（以下、単に「ジャイロ」という。）２の出力値およびＧＰＳ受信機（以下、単に「ＧＰＳ」という。）３の出力値に基づいて現時刻の車両の位置と方位を特定するとともに、車両の位置、方位および慣性センサの出力値の補正を行い、車両の位置と方位に基づく車両の走行軌跡を算出する。更には、走行軌跡に基づいて走行軌跡形状の算出も行う。

本ナビゲーション装置は、車両に搭載されるもので、車速センサ１と、ジャイロ２と、ＧＰＳ３と、現在位置検出部７と、ナビゲーション実行部８とを備えている。なお、現在位置検出部７とナビゲーション実行部８は、マイクロコンピュータの演算処理により行われるものである。すなわち、本ナビゲーション装置は、現在位置検出部７とナビゲーション実行部８の機能を実現するためのマイクロコンピュータ、ＲＡＭ、ＲＯＭ、不揮発性記憶媒体（いずれも図示せず）を備えている。

ここで、車速センサ１は、車両の移動距離を検出するためのセンサであり、移動距離に応じた間隔でパルス信号を出力する。

また、ジャイロ２は、車両のヨー（Ｙａｗ）角速度（方位変化量）を検出するためのセンサであり、車両に加わる回転運動の角速度に応じた検出信号を出力する。

また、ＧＰＳ３は、ＧＰＳ衛星からの送信電波をＧＰＳアンテナを介して受信し、車両の位置（緯度、経度）、方位（進行方向）、速度等を検出する。

また、現在位置検出部７は、車速センサ１、ジャイロ２、ＧＰＳ３からの出力に基づいて絶対位置、絶対方位、車速、相対軌跡等、航法演算を行うためのデータを検出する。

また、ナビゲーション実行部８は、現在位置検出部７での検出結果に基づいて現在位置を特定し、表示画面（図示せず）の地図上への自車両の位置の表示や、設定された目的地までの経路案内等を行う。

図１に示したように、車速センサ１、ジャイロ２からの信号を基に、相対軌跡演算部４、絶対位置演算部５での演算が行われ、それらの演算（航法演算）により、車速、相対軌跡、絶対位置、絶対方位が出力される。また、ＧＰＳ３からは位置、方位および車速の出力が得られる。カルマンフィルタ６は、航法演算により得られた車速、絶対位置、絶対方位の情報およびＧＰＳ３からの車速、位置、方位の情報を基に、車速センサ１の距離係数補正、ジャイロ２のオフセット補正、絶対位置補正、絶対方位補正を行う。

このようなナビゲーション装置へカルマンフィルタを適用すると、車速センサ１の距離係数補正、ジャイロ２のオフセット補正、および絶対位置補正、絶対方位補正により、数２に示す、事前推定Ｘ（ｔ｜ｔ−１）は０となる。従って、数１は数１４に示すようになる。

（数１４）Ｘ（ｔ｜ｔ）＝Ｋ（ｔ）Ｙ（ｔ）
従って、上記信号生成過程にて定義された５つの誤差値による状態量Ｘは、数３〜数５によって求められるカルマンゲインＫ（ｔ）および観測値Ｙ（ｔ）により求められる。

ここで、数３における誤差共分散行列Ｐは、数１５により定義される。

（数１５）

この誤差共分散行列Ｐは、状態量ｘの確からしさを表した行列である。この誤差共分散行列Ｐにおけるσ_GG ²はオフセット誤差の大きさの見積もりを表し、σ_AA ² は絶対方位誤差の大きさの見積もりを表し、σ_KK ²は距離係数誤差の大きさの見積もりを表し、σ_YY ² は絶対方位北方誤差の大きさの見積もりを表し、σ_XX ²は絶対方位東方誤差の大きさの見積もりを表す。それら以外のσ_ij ² はｉ行とｊ列の相互相関値を表す。例えばσ_AG ²はオフセット誤差と絶対方位誤差の相互相関値を表す。

なお、誤差共分散行列Ｐにおける対角成分σ_GG ²、σ_AA ² 、σ_KK ² 、σ_YY ²、σ_XX ² は、予測誤差とも呼ばれる。また、誤差共分散行列Ｐにおける各成分σ_GG ²、σ_AA ² 、σ_KK ² 、σ_YY ²、σ_XX ² が小さく収束すると正確に各変数が推定できたことを意味する。

この誤差共分散行列Ｐの値は、数４の計算によって更新される。なお、初期値においては、σ_GG ²、σ_AA ² 、σ_KK ² 、σ_YY ²、σ_XX ² の各値を誤差が最大となる値に設定しておき、また相互相関値については全て０に設定しておく。また、数３におけるＨは数１２で示される行列を用い、Ｖについては数１３に示されるものを用いる。また、数４におけるＷは数１１に示されるωの分散を用いる。

観測過程における観測値Ｙとしては、数１２に示すように、εＡ_DRt−εＡ_GPSt、εＫ_DRt−Ｋ_GPSt、εＹ_DRt−εＹ_GPSt 、εＸ_DRt −εＸ_GPSt を用いている。ここで、添字の_GPStは時刻ｔにおいて車速センサ１、ジャイロ２からの信号に基づく航法演算にて求められた値を意味し、_GPStは時刻ｔにおいてＧＰＳ３から出力される値を意味する。

εＡ_DRt−εＡ_GPStは、航法演算により求められた絶対方位とＧＰＳ３から出力される方位の差、すなわち航法演算により求められた絶対方位には真の絶対方位とその誤差εＡ_DRtが含まれており、またＧＰＳ３から出力される方位には真の絶対方位とその誤差εＡ_GPStが含まれているため、それらの差を取ることによりεＡ_DRt−εＡ_GPStが得られる。

同様に、εＫ_DRt−εＫ_GPStは、航法演算により求められる速度とＧＰＳ３から出力される速度の差から求まる距離係数誤差であり、具体的には、（航法演算による速度−ＧＰＳによる速度）／（航法演算による速度）により求められる。また、εＹ_DRt−εＹ_GPStは、航法演算により求められる絶対位置（緯度）とＧＰＳ３から出力される位置（緯度）の誤差の差であり、εＸ_DRt−εＸ_GPStは、航法演算により求められる絶対位置（経度）とＧＰＳ３から出力される位置（経度）の誤差の差である。

また、数１３に示す、観測過程で発生する雑音ｖはＧＰＳ３の雑音であり、以下のようにして求められる。ＧＰＳ３における擬似距離の計測誤差（ＵＥＲＥ）とＨＤＯＰ（Horizontal Dilution of Precision）の関係により測位精度が、ＵＥＲＥ×ＨＤＯＰで求められ、この測位精度を２乗することにより、ｖ_2t、ｖ_3tが求められる。また、ドップラー周波数の計測誤差とＨＤＯＰの関係より速度精度が、ドップラー周波数の計測誤差×ＨＤＯＰで求められ、この速度精度／車速にて距離係数計測誤差が求められ、これを２乗することによりｖ_1tが求められる。さらに、車両の速度Ｖc と速度精度から方位精度がｔａｎ^-1 （速度精度／Ｖc）で求められ、この方位精度を２乗するこによりｖ_0tが求められる。

従って、観測過程におけるεＡ_DRt−εＡ_GPSt、εＫ_DRt−εＫ_GPSt、εＹ_DRt−εＹ_GPSt、εＸ_DRt−εＸ_GPStおよび上記雑音Ｖを入力とし、数３〜数５および数１を実行することにより、信号生成過程にて定義された５つの誤差値による状態量Ｘが求められ、これらにより車速センサ１の距離係数補正、ジャイロ２のオフセット補正、絶対位置補正、絶対方位補正が行われる。

上記の相対軌跡演算、絶対位置演算、カルマンフィルタは本ナビゲーション装置のマイクロコンピュータ（図示せず）による演算処理にて行われるため、以下これについて説明する。図２に航法演算のメインルーチンの演算処理を示す。

まず、メインルーチンの演算処理の概略について説明する。本ナビゲーション装置のマイクロコンピュータは、ステップ２０にて、初期化処理を実施し、ステップ３０にて、車両のアクセサリ電源がオフ状態からオン状態になったか否かを判定する。また、ステップ１００〜３００では、車速センサ１およびジャイロ２により構成される慣性センサの出力値に基づいて航法演算により１時刻前の車両位置（緯度、経度）と方位から現時刻の車両の位置（緯度、経度）と方位を算出する。そして、ステップ４００では、現時刻の車両の位置と方位およびＧＰＳ受信機３の出力値に基づき、車両の位置、方位および慣性センサの補正量（オフセット補正量、距離係数補正量）の各誤差を状態量、同各誤差の分散値（予測誤差）を誤差共分散行列の対角成分とするカルマンフィルタを用いて、現時刻の車両の位置、方位および慣性センサの補正量の各誤差の推定および補正を行う。

また、カルマンフィルタは常時「１時刻前の状態」を必要とする。本ナビゲーション装置のマイクロコンピュータは、ナビゲーション装置が再起動した後も、シームレスにカルマンフィルタを動作できるよう、ステップ３０にて、車両のアクセサリ電源がオン状態からオフ状態になったことを判定すると、ステップ４０にて、本ナビゲーション装置が作動を停止する前の状態の緯度、経度、方位、オフセット補正量、距離係数補正量と、誤差共分散行列を不揮発性記憶媒体へバックアップする。そして、ナビゲーション装置が作動を停止した後、再起動する際に、本ナビゲーション装置のマイクロコンピュータは、ステップ２０にて、不揮発性記憶媒体でバックアップされた各値を読み戻し、この読み戻した各値を１時刻前の状態とみなして航法演算（相対軌跡演算４、絶対位置演算５）やカルマンフィルタ６の演算を再開する。

次に、図２に従って、メインルーチンの演算処理の詳細について説明する。本ナビゲーション装置は、常時、車両バッテリより給電されている。また、本ナビゲーション装置は、ユーザ操作に応じて動作中にアクセサリ電源（ＡＣＣ電源）がオフ状態になったことを判定すると、一定期間経過後に低消費電力モードに遷移して停止状態となる。また、本ナビゲーション装置は、停止状態となった後、ユーザ操作に応じてアクセサリ電源がオン状態になると、通常モードに遷移して動作状態となる。本ナビゲーション装置のマイクロコンピュータは、ユーザ操作に応じてアクセサリ電源がオン状態になると、図２に示す処理を実施する。

まず、ステップ２０にて、ＲＯＭに記憶されたプログラムに従った初期化処理を実施すると共に不揮発性記憶媒体でバックアップした各値の読み戻し処理を行う。具体的には、予め定められた初期化処理を実施すると共に不揮発性記憶媒体でバックアップした緯度、経度、方位、オフセット補正量、距離係数補正量と、誤差共分散行列を読み戻し、ＲＡＭに記憶させる。

次のステップ３０では、車両のアクセサリ電源がオン状態からオフ状態になったか否かを判定する。ここで、車両のアクセサリ電源がオン状態からオフ状態になるまで、ステップ１００〜４００の処理を繰り返し実施する。

まず、ステップ１００にて方位変化量・移動距離の演算を行う。この処理の詳細を図３に示す。まず、ステップ１０１にてジャイロ２の出力角速度にメインルーチンの起動周期Ｔ_Mを掛けて方位変化量を算出する。次のステップ１０２にて、その方位変化量から、オフセット補正量（この補正量については後述する）にメインルーチンの起動周期Ｔ_Mを掛けたものを引き、方位変化量のオフセット補正を行う。次のステップ１０３では、車速センサ１からの車速パルス数に距離係数（この距離係数についても後述する）を掛けて移動距離を算出する。

このステップ１００の次に、ステップ２００の相対軌跡演算処理を行う。この処理の詳細を図４に示す。まず、ステップ２０１にて、方位変化量（ステップ１０２にて求めたもの）を基に相対方位を更新する。この更新した相対方位およびステップ１０３にて求めた移動距離によりステップ２０２にて相対位置座標の更新を行う。この更新は、移動距離に対する相対方位のＸ、Ｙ成分をそれまでの相対位置座標に加算することにより行う。この相対位置座標は相対軌跡を求めるたに行うもので、その相対軌跡と道路形状との関係により、いわゆるマップマッチングが行われる。

このステップ２００の次に、ステップ３００の絶対方位・絶対位置の演算処理を行う。この処理の詳細を図５に示す。まず、ステップ３０１にて、方位変化量（ステップ１０２にて求めたもの）を基に絶対方位を更新する。

次のステップ３０２では、この更新した絶対方位およびステップ１０３にて求めた移動距離によりステップ２０２にて絶対位置座標の更新を行う。このステップ２００の処理にて更新された絶対方位Ａと絶対位置は後述するＧＰＳとの複合化処理にて利用される。

このＧＰＳとの複合化処理を行うステップ４００の詳細を図６に示す。まず、ステップ４０２にてＧＰＳ３からの測位データがあるか否かを判定する。ＧＰＳ３からの測位データがあると、ステップ４０３にて、フェリージャンプしたか否かを判定する。なお、フェリージャンプとは、ナビゲーション装置の停止状態中に、車両移動が発生したことを意味する。具体的には、ＧＰＳ３からの測位データに基づいて車両の位置を特定し、この測位データに基づいて特定した車両の位置（緯度、経度）と、不揮発性記憶媒体から読み戻したナビゲーション装置が作動を停止する前の位置（緯度、経度）を比較して、測位データに基づいて特定した車両の位置とナビゲーション装置が作動を停止する前の車両の位置が、所定距離以上離れているか否かに基づいてフェリージャンプしたか否かを判定する。

ここでは、フェリージャンプしていないものとし、フェリージャンプした場合については後で説明する。

フェリージャンプしていない場合、ステップ４０３の判定はＮＯとなり、ステップ４０５以降のカルマンフィルタの演算処理に進む。まず、ステップ４０５にて観測値Ｙの計算を行う。これは、ＧＰＳ３から出力される速度、位置、方位データおよび航法演算におけるステップ３００の処理にて求めた絶対方位、絶対位置および図示しない速度演算処理により車速センサ１からの車速パルスに基づく車両の速度とから、数１２に示した、εＡ_DRt−εＡ_GPSt、εＫ_DRt−εＫ_GPSt、εＹ_DRt−εＹ_GPSt、εＸ_DRt−εＸ_GPStを計算するとともに、数１３に示す、観測過程で発生する雑音ｖをＧＰＳ３の測位データ等を基に計算する。

ステップ４０６では、プロセス行列φの計算を行う。これは、前回のプロセス行列の計算時点からの移動距離Ｌ、経過時間Ｔ（これらは図示しない計測処理により別途求められている）およびステップ３０１にて求めた絶対方位Ａにより、数１１に示すプロセス行列φを求める。このようにして計算した観測値Ｙおよびプロセス行列φを基に、上述した数３〜数５の計算を行って数１４に示す状態量Ｘを求める。

また、ステップ４０７では、数３により誤差共分散行列Ｐの予測計算を行う。ステップ４０８では、数４によりカルマンゲインＫの計算を行う。ステップ４０９では、数５により誤差共分散行列Ｐの計算を行う。

この後、カルマンゲインＫおよび観測値Ｙに基づき、ステップ４１０にて、数１４の計算により状態量Ｘを求める。この状態量Ｘは、数１１の左辺に示すように、オフセット誤差（εＧ）、絶対方位誤差（εＡ）、距離係数誤差（εＫ）、絶対位置北方向誤差（εＹ）、絶対位置東方向誤差（εＸ）を表している。

これらの誤差により、ステップ４１１にて、図に示す計算にて航法演算誤差の修正、すなわちジャイロ２のオフセット補正、車速センサ１の距離係数補正、絶対方位補正、絶対位置補正が行われる。ジャイロ２のオフセット補正により、ステップ１０２にて用いられるオフセット補正量が修正され、車速センサ１の距離係数補正により、ステップ１０３にて用いられる距離係数が修正され、絶対方位補正により、ステップ３０１にて用いられる絶対方位Ａが修正され、絶対位置補正によりステップ３０２にて用いられる絶対位置が修正される。

上記の処理を、ＧＰＳ３からの測位データが有る毎に繰り返し行い、上記誤差修正を行って、より正確な走行軌跡を得ることができる。また、測位データがない場合には、ステップ４０２の判定はＮＯとなり、ステップ４１２、４１３に進み、プロセス行列φの計算および誤差共分散行列Ｐの予測計算を行う。これによって、ＧＰＳ３の測位ができない場合の誤差に対応した誤差共分散行列の予測計算を行い、その後にＧＰＳ３が測位できた時に行われるカルマンフィルタの処理を正確に行えるようにする。

また、フェリージャンプした場合については、以下に示すようになる。ナビゲーション装置の停止状態中に車両移動が発生しており、測位データに基づいて特定した車両の位置と、不揮発性記憶媒体から読み戻したナビゲーション装置が作動を停止する前の位置が、所定距離以上離れている場合、ステップ４０３の判定はＹＥＳとなり、ステップ４０４にて、過去の状態を修正する。

ここでは、ステップ３００の絶対方位・絶対位置の演算で用いられる１時刻前の車両の位置（緯度、経度）および方位を、ＧＰＳ３を用いた衛星航法による測位値で上書きする。このように、１時刻前の車両の位置（緯度、経度）および方位が修正されると、次の時刻に、ステップ３００の絶対方位・絶対位置の演算が実施されたときに、その修正された値が使われる。具体的には、修正された絶対方位は、図５中のステップ３０１の数式の右辺に示した絶対方位として使われ、修正された車両の位置（緯度、経度）は、図５中のステップ３０２の数式の右辺に示した絶対位置（ａｂｓ．ｘおよびａｂｓ．ｙ）として使われる。

また、後述するステップ４０７やステップ４１３の入力となる誤差共分散行列のうち、方位の予測誤差に対応する成分σ_AA ²の値と、緯度の予測誤差に対応する成分σ_YY ²と、経度の予測誤差に対応する成分σ_XX ² を、誤差が最大となる値（初期値）にリセットする。ただし、慣性センサの補正量および慣性センサの補正量の誤差の予測誤差σ_GG ²の値とσ_KK ²の値ついては修正しない。

例えば、位置および方位の予測誤差の初期値は以下のような値に設定する。方位の予測誤差は、０度〜１８０度の２乗の範囲に設定することが可能である。したがって、ここでは、σ_AA ²を、１８０^２＝１３２４０にリセットする。また、緯度の予測誤差に対応する成分σ_YY ²および経度の予測誤差に対応する成分σ_XX ² は、距離の２乗で表される。したがって、ここでは、σ_YY ²およびσ_XX ² の初期値を、それぞれ１００キロメートルの２乗＝１０００００^２にリセットする。

この後、図２のステップ３０の処理へ戻り、車両のアクセサリ電源がオン状態からオフ状態になるまで、ステップ１００〜４００の処理を繰り返し実施する。このステップ１００〜４００の処理では、ステップ４０４にて修正された後の各値を用いて各種演算が行われる。

カルマンフィルタによる慣性センサの出力値の補正において、例えば、カーフェリーやレッカー車等で車両が運搬された場合など、車両用位置検出装置の停止状態中に車両移動が発生した後、不揮発性記憶媒体でバックアップされた位置、方位と、実際の位置、方位との間に乖離（以下、実誤差）が発生してしまう。

カルマンフィルタは（実誤差に対して）適切な予測誤差を推定出来ている場合には状態量を精度良く推定することができるが、この場合、実誤差>>予測誤差となることが予想される。実誤差が大きいにも関わらず小さな値の予測誤差を用いた場合には、状態量の推定を誤り、車両の位置、方位や慣性センサの出力値を誤補正してしまうことになる。

そこで、本実施形態では、フェリージャンプが観測された場合、まず車両の位置および方位の各値をＧＰＳ受信機より出力される車両の位置および方位の各値で上書きする処理を設けている。ＧＰＳ３より出力される車両の位置および方位は観測誤差の影響で必ずしも正しいとは限らない為、（実誤差>>予測誤差とならないよう）車両の位置および方位に関する予測誤差を十分大きくする（初期値にリセットする）。

ただし、車両用位置検出装置の停止状態中に車両移動が発生しても、慣性センサの補正量は変化しないため、慣性センサの出力値の補正については、起動する前の慣性センサの補正量を用いるようにしている。

上記した構成によれば、当該車両用走行軌跡算出装置が作動停止中に車両移動が発生したと判定された場合、ＧＰＳ受信機より出力される位置および方位を現時刻の車両の位置、方位とみなすとともに、誤差共分散行列のうち、車両の位置および方位の各誤差の予測誤差についてのみ所定値にリセットし、慣性センサの補正量および慣性センサの補正量の誤差の予測誤差については、記憶媒体に記憶された起動前の各値を用いて車両の位置、方位および前記慣性センサの出力値の補正が行われるので、慣性センサの出力値の誤補正が防止され、作動停止中に車両移動が発生した後、再起動時に、より速やかに走行軌跡を精度良く算出することができる。なお、所定値としては、予め定められた初期値とすることができる。

なお、本発明は上述の実施形態に限定されることなく、本発明の趣旨を逸脱しない範囲内で、以下のように種々変形可能である。

例えば、上記実施形態では、ステップ４０４にて、誤差共分散行列に含まれる位置および方位の各誤差の予測誤差を、予め定められた初期値にリセットするように修正したが、ＧＰＳ受信機の出力値に基づく衛星航法により、車両の位置および方位の各誤差の予測誤差が提供される場合には、ステップ４０４にて、誤差共分散行列に含まれる位置および方位の各誤差の予測誤差を、衛星航法により特定された車両の位置および方位の各誤差の予測誤差に修正するようにしてもよい。このようにすることで、誤差共分散行列に含まれる位置および方位の各誤差の予測誤差を、予め定められた初期値にリセットする場合と比較して、より速やかに現在位置を精度良く推定することが可能である。

また、上記実施形態では、ジャイロスコープのオフセット誤差と車速センサの距離係数誤差を慣性センサの誤差として推定し、慣性センサの出力値を補正するようにしたが、ジャイロスコープのオフセット誤差と車速センサの距離係数誤差以外の誤差（例えば、ジャイロスコープのゲイン誤差）を含むように慣性センサの誤差を推定し、慣性センサの出力値を補正するようにしてもよい。

また、上記実施形態では、車両の位置、方位および慣性センサの補正量に対する各誤差を状態量としたカルマンフィルタを用いて車両の位置と方位の各誤差および誤差共分散行列を用いて車両の位置と方位を補正するようにしたが、車両の位置、方位および慣性センサの補正量以外の誤差を状態量に加えるようにしてもよい。

なお、上記実施形態における構成と特許請求の範囲の構成との対応関係について説明すると、ステップ１００〜ステップ４００が演算処理手段に相当し、不揮発性記憶媒体が記憶手段に相当し、ステップ４０３が移動判定手段に相当する。

１ 車速センサ
２ ジャイロスコープ
３ ＧＰＳ受信機
７ 現在位置検出部
８ ナビゲーション実行部

Claims (3)

1. 車両に搭載された慣性センサ（１、２）の出力値およびＧＰＳ受信機（３）の出力値に基づいて現時刻の前記車両の位置と方位を特定するとともに、前記車両の位置、方位および前記慣性センサの補正量に対する各誤差を状態量としたカルマンフィルタを用いて前記各誤差を推定するとともに前記各誤差の予測誤差を成分として含む誤差共分散行列を算出し、前記各誤差および前記誤差共分散行列を用いて前記車両の位置、方位および前記慣性センサの出力値の補正を行う演算処理手段（ステップ１００〜４００）を備え、前記車両の位置と方位に基づく前記車両の走行軌跡を算出する車両用走行軌跡算出装置であって、
   当該車両用走行軌跡算出装置が作動を停止する際、前記補正処理手段により補正された前記車両の位置、方位、前記慣性センサの補正量、および前記誤差共分散行列を記憶媒体へ記憶するバックアップ手段と、
   当該車両用走行軌跡算出装置が起動したとき、前記ＧＰＳ受信機の出力値に基づいて前記車両の位置を特定し、当該車両の位置と前記記憶媒体に記憶された前記起動前の前記車両の位置に基づいて、車両用走行軌跡算出装置の作動停止中に車両移動が発生したか否かを判定する移動判定手段を備え、
   前記移動判定手段により前記作動停止中に車両移動が発生したと判定された場合、前記演算処理手段は、前記ＧＰＳ受信機の出力値より特定される位置および方位を現時刻の前記車両の位置、方位とみなすとともに、前記誤差共分散行列のうち、前記車両の位置および方位の各誤差の予測誤差についてのみ所定値にリセットし、前記慣性センサの補正量および前記慣性センサの補正量の誤差の予測誤差については、前記記憶媒体に記憶された前記起動前の各値を用いて前記車両の位置、方位および前記慣性センサの出力値の補正を行うことを特徴とする車両用走行軌跡算出装置。
2. 前記所定値は、誤差が最大となる初期値であることを特徴とする請求項１に記載の車両用走行軌跡算出装置。
3. 前記所定値は、前記ＧＰＳ受信機の出力値に基づいて特定された予測誤差であることを特徴とする請求項１に記載の車両用走行軌跡算出装置。

Images