JP6149699B2

JP6149699B2 - 車両用走行軌跡算出装置

Info

Publication number: JP6149699B2
Application number: JP2013234757A
Authority: JP
Inventors: 裕也樋口
Original assignee: Denso Corp
Current assignee: Denso Corp
Priority date: 2013-11-13
Filing date: 2013-11-13
Publication date: 2017-06-21
Anticipated expiration: 2033-11-13
Also published as: JP2015094690A

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 ²を、１８０^２＝１３２４０にリセットする。

以下、ステップ１００〜４００の演算処理において、ステップ２０にてＲＡＭに記憶させた緯度、経度、方位、オフセット補正量、距離係数補正量と、各誤差の予測誤差を含む誤差共分散行列のうち、方位の予測誤差を所定値にリセットし、方位の予測誤差以外（緯度、経度、方位、オフセット補正量、距離係数補正量と、誤差共分散行列に含まれる方位の誤差の予測誤差）については、ステップ２０にて不揮発性記憶媒体から読み出してＲＡＭに記憶させた各値を１時刻前の状態として現時刻の車両の位置と方位を特定するとともに、車両の位置、方位および慣性センサの出力値の補正を行う。

カルマンフィルタを用いた走行軌跡の算出において、例えば、入庫方向と出庫方向の向きが１８０度異なるようなターンテーブルが設置された立体駐車場に車両を駐車させた後、ナビゲーション装置が作動を停止している間に、車両がターンテーブル上で回転すると、バックアップしておいた方位が実際の車両の方位と異なっているにもかかわらず、バックアップしておいた方位および方位の予測誤差を１時刻前の値とみなして慣性センサの出力値や車両の位置と方位の補正を行うと、走行軌跡を誤推定してしまう。

そこで、本実施形態では、ナビゲーション装置が作動を停止した後、再起動した場合、ステップ３０にて、誤差共分散行列に含まれる方位の予測誤差を、誤差が最大となる値にリセットして、カルマンフィルタで認識される方位の誤差を大きくしている。これにより、実際の方位の誤差と大きく異なる方位の予測誤差を用いて慣性センサの出力値を誤補正してしまうといったことが防止される。

また、ナビゲーション装置が作動を停止した後、再起動した場合、方位の値そのものまでも毎回リセットして初期値に戻してしまうと、本ナビゲーション装置が停止した状態で、車両がターンテーブル上で回転していない場合に、正しい方位が分からなくなってしまい、再起動時に速やかに方位を推定することができなくなる。このため、再起動時に、誤った方向へ走行しているような走行軌跡が算出されてしまうことになる。実運用上のほとんどの場合は、本ナビゲーション装置が停止した状態で、車両が回転することはないので、上記したように再起動時に速やかに方位を推定することができなくなるのは問題である。

このため、本実施形態では、再起動時に毎回リセットするのは、方位の予測誤差に限定する。方位の値そのものはリセットせず、本ナビゲーション装置が停止状態となる前の状態の値、すなわち、不揮発性記憶媒体に記憶された方位を用いて速やかに車両の方位を算出するようになっている。

上記した構成によれば、本車両用走行軌跡算出装置は、車両用走行軌跡算出装置が作動を停止するとき、補正された車両の位置、方位、慣性センサの補正量および誤差共分散行列を記憶媒体へ記憶させ、車両用走行軌跡算出装置が起動したとき、記憶媒体に記憶された誤差共分散行列のうち、方位の誤差の予測誤差についてのみ所定値にリセットし、方位の誤差の予測誤差以外（車両の位置、方位、慣性センサの補正量および車両の位置の誤差の予測誤差、慣性センサの補正量の誤差の予測誤差）については記憶媒体に記憶された起動前の各値を用いて現時刻の車両の位置と方位を特定するとともに、車両の位置、方位および慣性センサの出力値の補正を行うので、慣性センサの出力値の誤補正が防止され、作動停止中に車両がターンテーブル上で回転した後、再起動時に、より速やかに走行軌跡を精度良く算出することができる。

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

例えば、上記実施形態では、ステップ３０にて、誤差共分散行列に含まれる方位の誤差の予測誤差の初期値に、誤差が最大となる値（１８０^２）を設定するようにしたが、このような値（１８０^２）に限定されるものではない。

また、上記実施形態では、ジャイロスコープの補正量の誤差としてオフセット誤差を推定したが、ジャイロスコープの補正量の誤差としてオフセット誤差とゲイン誤差を別々に推定するように構成してもよい。

なお、上記実施形態における構成と特許請求の範囲の構成との対応関係について説明すると、ステップ１００〜ステップ４００が演算処理手段に相当し、ステップ５０がバックアップ手段に相当する。

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

Claims

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