JP5664059B2 - 車両用軌跡推定装置 - Google Patents

Description

本発明は、車両用軌跡推定装置に関するものである。

従来、ＧＰＳ受信装置が算出する現在位置情報をカルマンフィルタの観測量に用いることで、自立センサ（車速センサ、ジャイロスコープ等）の誤差を推定し、推定したセンサ誤差を補正する技術が知られている。

特開２０００−５５６７８号公報 特開２００８−２３２７６１号公報

しかし、ＧＰＳ受信装置による測位結果は、マルチパス波の受信やＧＰＳ衛星配置悪化等の要因により測位精度が大きく変動する。精度が劣化したＧＰＳ測位結果を使用してカルマンフィルタを実行するとセンサ誤差の推定を誤り、走行軌跡の推定精度が悪化してしまうという問題がある。

本発明はこの問題点に鑑み、ＧＰＳ受信装置がＧＰＳ衛星からの信号に基づいて算出する情報をカルマンフィルタの観測量に用いることで、自立センサの誤差を推定し、推定したセンサ誤差を補正する技術において、カルマンフィルタで用いるＧＰＳ測位結果の精度の劣化を全体として抑えることを目的とする。

上記目的を達成するための請求項１に記載の発明は、車両に搭載される自立センサ（２０）の出力値に基づいて推測航法により前記車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて前記車両の位置および方位を補正する車両用軌跡推定装置であって、前記カルマンフィルタは、観測量として、使用対象の複数のＧＰＳ衛星毎に、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、当該車両用軌跡推定装置は、前記タイトカップリング型カルマンフィルタにおいて前記観測量の共分散（Ｓ_ｋ）の導出を行う第１共分散導出手段（Ｓ４０６）と、観測量の検定処理（Ｓ４０７）において、前記第１共分散導出手段によって導出された前記共分散（Ｓ_ｋ）を用いて、前記タイトカップリング型カルマンフィルタにおける前記使用対象の複数のＧＰＳ衛星毎の前記乖離量のいずれかが大きくなるほど大きくなる指標を算出する指標算出手段（Ｓ４５０）と、前記検定処理において、前記指標算出手段（Ｓ４５０）が算出した指標と所定の閾値とを比較する比較手段（Ｓ４５１）と、前記検定処理において、前記指標が前記所定の閾値以上であると前記比較手段（Ｓ４５１）が判定した場合、前記使用対象の複数のＧＰＳ衛星毎の前記乖離量に基づいて、前記使用対象の複数のＧＰＳ衛星のうち一部を選び、選んだ前記一部を使用対象から除外する除外手段（Ｓ４５２）と、前記タイトカップリング型カルマンフィルタにおいて、前記除外手段によって除外されたことで数が減った使用対象のＧＰＳ衛星に基づいて、前記観測量の共分散（Ｓ_ｋ）の導出を再度行う第２共分散算出手段（Ｓ４０６）と、前記タイトカップリング型カルマンフィルタにおいて、前記第１共分散導出手段によって算出された前記観測量の共分散（Ｓ_ｋ）の代わりに、前記第２共分散導出手段によって再度算出された前記観測量の共分散（Ｓ_ｋ）を用いて、前記状態量の推定値を算出する状態量推定値算出手段（Ｓ４０９、Ｓ４１０）と、を備えた車両用軌跡推定装置である。
また、請求項２に記載の発明は、前記指標算出手段が算出する前記指標は、前記使用対象の複数のＧＰＳ衛星毎の前記乖離量のそれぞれが大きくなるほど大きくなる量であることを特徴とする請求項１に記載の車両用軌跡推定装置である。
また、請求項３に記載の発明は、前記比較手段が前記指標と比較する前記所定の閾値は、使用する対象のＧＰＳ衛星の数に基づいて設定されることを特徴とする請求項２に記載の車両用軌跡推定装置である。
また、請求項４に記載の発明は、前記指標算出手段が算出する前記指標は、カイ自乗値であることを特徴とする請求項１ないし３のいずれか１つに記載の車両用軌跡推定装置である。

このように、カルマンフィルタをタイトカップリング型とすることで、カルマンフィルタを実行する際に、精度が劣化したＧＰＳ測位結果の検出および排除を行い、ＧＰＳ精度劣化による推定車両軌跡の影響を軽減することで、精度よく車両の位置および方位を補正することができる。 なお、上記および特許請求の範囲における括弧内の符号は、特許請求の範囲に記載された用語と後述の実施形態に記載される当該用語を例示する具体物等との対応関係を示すものである。

本発明の一実施形態に係る車両用現在位置検出装置の構成を示す図である。 信号生発生過程、観測過程、カルマンフィルタの模式図である。 制御部による推測航法処理のフローチャートである。 ジャイロスコープのオフセット補正について説明するための図である。 ジャイロスコープのゲイン補正について説明するための図である。 制御部によるＧＰＳ複合化処理のフローチャートである。 制御部による観測量の検定処理のフローチャートである。 有意水準５％の閾値を示す図表である。

以下、本発明の一実施形態について説明する。図１に、本実施形態に係る車両用現在位置検出装置の構成を示す。本実施形態に係る車両用現在位置検出装置は車両用ナビゲーション装置として構成されており、ＧＰＳ受信装置１０、自立センサ２０、地図データ記憶装置３０、ディスプレイ部４０、および制御部５０を備えている。

ＧＰＳ受信装置１０は、複数のＧＰＳ衛星１００（図中では、１つのみ示す）からの電波を受信するＧＰＳアンテナ部１１、ＧＰＳアンテナ部１１から入力される高周波信号を中間周波数帯の受信信号に変換するＧＰＳ―ＲＦ部１２、ＧＰＳ―ＲＦ部１２から入力される受信信号の同期捕捉や同期保持等の信号処理を行うＧＰＳ信号処理部１３、ＧＰＳ信号処理部１３から出力されるデータに基づいてＧＰＳ測位演算を行い、このＧＰＳ測位演算により求めた測位データ、複数のＧＰＳ衛星１００のそれぞれの絶対位置、複数のＧＰＳ衛星１００のそれぞれの擬似距離およびドップラー周波数等を出力するＧＰＳ測位演算部１４を備えている。

ＧＰＳ受信装置１０は、ＧＰＳ衛星１００からの電波を受信し、車両の絶対的な現在位置を表すＧＰＳ位置、車両の進行方向を特定するための速度ベクトル等を算出し、算出結果を測位データとして出力する。

自立センサ２０は、車両の走行速度に応じた車速パルス信号を出力する速度センサとしての車速センサ２１と、車両の回転角速度に応じた角速度信号を出力する角速度センサとしてのジャイロスコープ２２を備えている。

地図データ記憶装置３０は、地図データが記憶された地図データ記憶媒体から地図データを読み出すための装置である。地図データには、各リンクの距離、道路種別、車線数、一方通行規制、信号機の位置等を表す道路データ、位置検出精度を向上するためのいわゆるマップマッチングデータ、川、湖、海、鉄道、施設などの位置、形状、名称を表す背景データ、各地の施設の名称、所在位置、施設種類等を示す施設データ等が含まれる。なお、地図データ記憶媒体としては、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、ハードディスクドライブ、メモリカード等がある。

ディスプレイ部４０は、液晶等の表示部を備え、コンピュータから入力される映像信号に応じた映像を表示部に表示させる。

制御部５０は、ＣＰＵ、ＲＯＭ、ＲＡＭ、不揮発性メモリ、Ｉ／Ｏ等を有するコンピュータにより構成されており、ＣＰＵは、ＲＯＭに記憶されたプログラムに従って各種処理を実施する。

制御部５０の処理としては、ジャイロスコープ２２の出力値に基づいて算出される方位変化量と、速度センサ２１の出力値に基づいて算出される車速係数から、車両の相対軌跡を推定するとともに当該車両の相対軌跡とＧＰＳ受信装置１０からの出力値とに基づいて車両の走行軌跡を推定する走行軌跡推定処理、ジャイロスコープ２２の出力値およびＧＰＳ受信装置１０からの出力値に基づいて方位変化量のオフセット誤差、方位変化量のゲイン誤差、絶対方位誤差等を推定する誤差推定処理、自車の走行軌跡と地図メモリ７０に格納されている地図情報の道路データとを照合することによって、自車位置（ＧＰＳ位置により補正された推測位置）を道路上の位置に修正するマップマッチング処理等がある。なお、走行軌跡推定処理では、誤差推定処理により推定された各誤差を用いて走行軌跡推定処理による車両の走行軌跡の推定に用いられる方位変化量のオフセット、方位変化量のゲインおよび絶対方位等を補正するようになっている。

制御部５０は、誤差推定処理においてカルマンフィルタを用いて推測航法における各誤差要因を１つの状態量ｘとしてモデル化し、各誤差の推定値を求める誤差推定処理を実施する。

ここで、カルマンフィルタの概要について説明する。カルマンフィルタは、図２および式（１）、（２）に示すような信号発生過程および観測過程を前提としている。

すなわち、プロセス行列φ_ｋで定義される線形システム（信号発生過程）があり、そのシステムにおいて状態量Ｘ_ｋに対して、観測行列Ｈ_ｋで関係付けられた状態量Ｘ_ｋの一部が観測可能（観測過程）である場合、カルマンフィルタは、この観測量Ｙ_ｋを入力として、状態量Ｘ_ｋの最適な推定値ｘ_ｋを与えるものである。なお、ｒ_ｋは信号発生過程にて発生する雑音であり、ｑ_ｋは観測過程において発生する雑音である。

そして、時刻ｋまでの情報を用いた状態量Ｘ_ｋの最適推定値ｘ_ｋ＝ｘ_{（ｋ｜ｋ）}は、次の式（３）〜（８）により求めることができる。

ここで、Ｋ_ｋはカルマンゲイン、Ｐ_ｋは推定値ｘ_ｋの共分散（以下では誤差共分散という）、Ｑ_ｋは観測過程で発生する観測雑音ｑ_ｋの分散、Ｒ_ｋは信号発生過程で発生するシステム雑音ｒ_ｋの分散である。またＳ_ｋは、式（５）で定義される残差の共分散である。また各量（仮にＡ_ｋとする）の右上に−（マイナス記号）の上付き添字が付された量Ａ_ｋ ⁻は、時刻ｋ−１までの情報に基づく時刻ｋでの量Ａ_ｋの推定値（すなわちＡ_{（ｋ｜ｋ−１）}）である。また、雑音ｒ_ｋ、ｑ_ｋはいずれも平均０の白色ガウス雑音であり互いに無相関である。また、Ｉは単位行列であり、上付き添字Ｔは転置を意味し、上付き添字−１は逆行列を意味する。

このように定義されたカルマンフィルタでは、状態量の推定値ｘ_ｋを定義すると共に、誤差共分散Ｐ_ｋに適当な初期値を設定するとにより、状態量の最適推定値ｘ_ｋが得られ、観測が行われ観測量Ｙｋが得られる毎にこれを繰り返すことにより、状態量の最適推定値ｘ_ｋの精度が向上する。

そして、本実施例では、状態量Ｘ_ｋとして、東方向の絶対位置を算出する際に付与される絶対位置東方向誤差εＸ_ｋ、北方向の絶対位置を算出する際に付与される絶対位置北方向誤差εＹ_ｋ、高度方向の絶対位置を算出する際に付与される絶対位置高度方向誤差εＺ_ｋ、垂直方向の速度を算出する際に付与される垂直方向速度誤差εＵ_ｋ、絶対水平方位を算出する際に付与される絶対水平方位誤差εθ_ｋ、車速を算出する際に、車速センサ２１から出力された単位時間当たりのパルス数に乗じられる車速係数の誤差である車速係数誤差εＯ_ｋ、ジャイロ出力の大きさによらず、出力値の全領域に均等に付加されるジャイロバイアス誤差εＧｂ_ｋ、ジャイロ出力から角速度への変換比率として設定される変換ゲインの誤差であるジャイロゲイン誤差εＧｓ_ｋ、クロックバイアス誤差εＣｂ_ｋ、クロックドリフト誤差εＣｄ_ｋ、という１０個の誤差値を採用し、これら誤差値を次の式（９）〜（１８）により定義する。

ただし、Ｔは前回（の観測）からの経過時間、Ｄ_Ｔは前回からの方位変化量、Ｌ_Ｔは前回からの移動距離、θ_Ｔは真の絶対方位である。

そして、これら誤差値の時間的な変化を規定するものがプロセス行列φ_ｋであり、上記式（９）〜（１８）を各誤差値にて偏微分し線形化することにより求められ、その結果、プロセス行列φ_ｋは式（１９）のようになる。

ただし、θ_ｋ＝θ_Ｔ＋εθ_ｋ−１＋Ｔ×εＧ_ｋ−１／２＋Ｄ_Ｔ×εＧｓ_ｋ−１／２
であり、このθ_ｋは絶対方位を意味しており、真の絶対方位θ_Ｔにセンサ誤差に基づく誤差が加わったものとなっている。また、Ｇｓは車速係数であり、Ｄは前回からの方位変化量であり、Ｌは前回からの移動距離である。

また、観測量Ｙ_ｋとしては、各々のＧＰＳ衛星の観測擬似距離と推測擬似距離との残差、および、各々のＧＰＳ衛星の観測ドップラー周波数と推測ドップラー周波数との残差を採用する。

ここで、各ＧＰＳ衛星の観測擬似距離および観測ドップラー周波数は、ＧＰＳ受信装置１０によって計測され、ＧＰＳ受信装置１０から制御部５０に随時出力されているものを用いる。また、各ＧＰＳ衛星の推測擬似距離および推測ドップラー周波数は、ＧＰＳ受信装置１０から出力される各ＧＰＳ衛星の現在位置、後述するＳ１００で推測航法によって算出される自車両の移動距離、後述するＳ３００で推測航法によって算出される自車両の絶対方位、絶対位置、および車速等に基づいて算出する。

このように、擬似距離、ドップラー周波数等の、ＧＰＳ衛星毎に算出される量を観測量とするカルマンフィルタを、タイトカップリング型カルマンフィルタという。一方、特許文献１に記載のような観測量を用いるカルマンフィルタをルーズカップリング型カルマンフィルタいう。タイトカップリング型カルマンフィルタも、周知の技術であり、例えば、特許文献２に記載されている。

ルーズカップリング型カルマンフィルタは、ＧＰＳ測位結果（位置、方位、速度）を観測量として利用するが、本実施形態のようなタイトカップリング型カルマンフィルタは、各ＧＰＳ衛星の観測疑似距離とドップラー周波数を観測量として利用する。つまり、タイトカップリング型カルマンフィルタは、ＧＰＳ各衛星の観測疑似距離とドップラー周波数とセンサで推定した観測疑似距離と観測ドップラー周波数との差分を、誤差共分散行列に従い、最小にする状態量を推定する。

ここで、ｎ番目のＧＰＳ衛星の擬似距離ＰＲ^（ｎ）は、式（２０）のように定義される。

ただし、ＳＶ^ｎ _ｘ、ＳＶ^ｎ _ｙ、ＳＶ^ｎ _ｚは、それぞれｎ番目のＧＰＳ衛星の位置座標の成分であり、ｘ_{ｐｒｅｄｉｃｔｉｏｎ}、ｙ_{ｐｒｅｄｉｃｔｉｏｎ}、ｚ_{ｐｒｅｄｉｃｔｉｏｎ}は、ＧＰＳ受信装置１０の位置座標であり、Ｃｌｋ_ｂｉａｓは、クロックバイアス誤差εＣｂｋである。なお、式（２０）のｘ、ｙ、ｚの向きは、観測量のεＸ、εＹ、εＺと同じである。

また、ｎ番目のＧＰＳ衛星の観測ドップラー周波数（積算ドップラー）ＤＰ^（ｎ）は、式（２１）のように定義される。

ただし、ＳＶの上の黒丸は、時間微分を表し、Ｒ^ｎ _{ｐｒｅｄｉｃｔｉｏｎ}はＧＰＳ受信装置１０からｎ番目のＧＰＳ衛星までの距離を示し、Ｃｌｋ_{ｄｒｉｆｔ}はクロックドリフト誤差εＣｄ_ｋを表す。

これらの定義式（２０）、（２１）を各状態量εＸ_ｋ、εＹ_ｋ、εＺ_ｋ、εＵ_ｋ、εθ_ｋ、εＯ_ｋ、εＧｂ_ｋ、εＧｓ_ｋ、εＣｂ_ｋ、εＣｄ_ｋにて偏微分し線形化することにより、観測行列Ｈ_ｋが式（２２）のように得られる。

ここで、各偏微分は、式（２３）〜（３０）のように表される。

なお、式（２２）では、２個のＧＰＳ衛星を用いる場合の観測行列Ｈ_ｋの例であり、ｍ個のＧＰＳ衛星を用いる場合の観測行列Ｈ_ｋは、２ｎ行１０列の行列となる。

このように、ＧＰＳ毎に別々に定義された観測量を用いるタイトカップリング型カルマンフィルタを用いる制御部５０は、車速センサ２１からの信号に基づいて車両の移動距離を算出すると共に、ジャイロスコープ２２からの信号に基づいて車両の方位変化量を算出する。そして、これら算出された移動距離及び方位変化量に基づいて、推測航法データとして、車両の速度、相対軌跡、絶対位置、絶対方位を算出する。一方、ＧＰＳ受信装置１０は、ＧＰＳ測位データとして、車両の位置、方位、車速を随時出力し、また、各衛星の位置座標、各衛星までの観測擬似距離、および各衛星の観測ドップラー周波数を随時出力している。

そして、制御部５０は、タイトカップリング型カルマンフィルタを用いて、推測航法データおよびＧＰＳ測位データ、各衛星の位置座標、観測擬似距離、および観測ドップラー周波数に基づいて、各誤差εＸ_ｋ、εＹ_ｋ、εＺ_ｋ、εＵ_ｋ、εθ_ｋ、εＯ_ｋ、εＧｂ_ｋ、εＧｓ_ｋ、εＣｂ_ｋ、εＣｄ_ｋの推定値を算出し、更に、この推定値に基づいて、車両の方位変化量、移動距離、相対軌跡、絶対位置等の算出および補正を行う。

以下、制御部５０による推測航法処理について説明する。図３に、推測航法処理のフローチャートを示す。本車両用現在位置検出装置が動作状態になると、制御部５０は、図４に示す処理を一定周期ＴＭ毎に繰り返し実行する。

まずＳ１００では、方位変化量・移動距離演算処理を実行する。この処理では、まず、ジャイロスコープ２２により検出され出力されたジャイロ出力角速度にメインルーチンの起動周期ＴＭを乗じることにより車両の向きの方位変化量を算出する。
方位変化量＝ジャイロ出力角速度×ＴＭ （３１）
なお、ジャイロ出力角速度は、図４のグラフから明らかなように、ジャイロスコープ２２の出力をサンプリングした値から、オフセット値(図４では２．５Ｖ) を減じたものに、変換ゲイン(グラフの傾きに相当する) を乗じることにより算出されたものである。

次に、後述するオフセット補正量に一定周期ＴＭを乗じたものを、式（３１）により求めた方位変化量を減じることにより、方位変化量のオフセット補正を行う。

方位変化量＝方位変化量−オフセット補正量×ＴＭ （３２）
すなわち、図４に示すように、角速度０［ｄｅｇ／ｓｅｃ] に対応するジャイロスコープ２２の出力電圧(ゼロ点) の変動に基づく誤差を補正する。次に、オフセット補正された方位変化量に、後述するゲイン補正量を減じることにより、方位変化量のゲイン補正を行う。

方位変化量＝方位変化量×ゲイン補正量 （３３）
すなわち、図５に示すように、グラフの傾き(変換ゲイン) の変動に基づく誤差を補正する。

そして、一定周期ＴＭの間に検出された車速センサ２１からの出力パルス数に、後述する車速係数を乗じることにより移動距離を算出する。

移動距離＝車速センサパルス数×車速係数 （３４）
このように、移動距離を算出すると、方位変化量・移動距離の演算処理を終了する。

次のＳ２００の相対軌跡の演算処理では、まず、式（３３）を用いて算出した方位変化量を、それまでに求められている相対方位に加算することにより、車両の向きの相対方位を更新する。

相対方位＝相対方位＋方位変化量 （３５）
次に、この更新した相対方位および式（３４）を用いて算出した移動距離に基づき、相対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした相対座標ｒｅｌ．ｘ、東西方向をｙ座標軸とした相対座標ｒｅｌ．ｙを、式（３６）、（３７）に従って更新する。ただし、θｓは式（３５）を用いて算出した相対方位である。

ｒｅｌ．ｘ＝ｒｅｌ．ｘ＋移動距離×ｃｏｓ（θｓ） （３６）
ｒｅｌ．ｙ＝ｒｅｌ．ｙ＋移動距離×ｓｉｎ（θｓ） （３７）
すなわち、この更新は、移動距離に対する相対方位のＸ、Ｙ成分をそれまでの相対位置座標に加算することにより行う。この相対位置座標は相対軌跡を求めるために算出するもので、その相対軌跡と道路形状との関係により、いわゆるマップマッチングが行われる。

次に、式（３４）を用いて算出した移動距離を、周期ＴＭで除することにより車速を算出する。

車速＝移動距離／ＴＭ （３８）
このように、車速を算出すると、相対軌跡演算処理を終了する。

次のＳ３００の絶対方位・絶対位置の演算処理では、まず、式（３４）を用いて算出した方位変化量を、それまでに求められている車両の向きの絶対方位に加算することにより、絶対方位を更新する。

絶対方位＝絶対方位＋方位変化量 （３９）
次に、この更新した絶対方位および式（３４）を用いて算出した移動距離に基づき、絶対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした絶対座標ａｂｓ．ｘ、東西方向をｙ座標軸とした絶対座標ａｂｓ．ｙを、式（４０）、（４１）に従って更新する。ただし、θは式（３９）を用いて算出した絶対方位である。

ａｂｓ．ｘ＝ａｂｓ．ｘ＋移動距離×ｃｏｓ（θ） （４０）
ａｂｓ．ｙ＝ａｂｓ．ｙ＋移動距離×ｓｉｎ（θ） （４１）
すなわち、この更新は、移動距離に対する絶対方位のＸ、Ｙ成分をそれまでの絶対位置座標に加算することにより行う。このようにして更新した車両の絶対方位と絶対位置は後述するＧＰＳとの複合化処理にて利用される。

次に、Ｓ４００のＧＰＳ複合化処理を実施する。このＧＰＳ複合化処理のフローチャートを図６に示す。ここでは、このＧＰＳ複合化処理の実行回数がｋ回目であるとして説明する。

このＧＰＳ複合化処理では、まずＳ４０１で、車両用現在位置検出装置の起動後、ＧＰＳ受信装置１０が周知の初期位置算出（ｆｉｒｓｔ ｆｉｘ）を行ったか否か判定し、行っていなければ直ちにＧＰＳ復号化処理を終了し、行っていれば続いてＳ４０２に進む。なお、ＧＰＳ受信装置１０は、周知の初期位置算出を行ったとき、測位データを制御部５０に出力するので、測位データを制御部５０は、車両用現在位置検出装置の起動後、一度以上測位データを受け取っていれば、初期位置算出を行ったと判定する。

Ｓ４０２では、ＧＰＳ受信装置１０が信号を受信できている衛星数が所定の衛星数ＴＨｓｙ以上であるか否かを判定し、衛星数ＴＨｓｙ未満であれば直ちにＧＰＳ復号化処理を終了し、衛星数ＴＨｓｙ以上であればＳ４０３に進む。なお、ＧＰＳ受信装置１０は信号を受信できている衛星数の情報を随時制御部５０に出力するようになっているので、Ｓ４０２では、この情報に基づいて判定を行えばよい。この閾値ＴＨｓｙは、１でもよいし、２でもよいし、３以上であってもよい。

続いてＳ４０３では、状態量の推定を行う。具体的には、式（３）を用いて、状態量の推定値ｘ_ｋ ^―を算出する。続いてＳ４０４では、誤差共分散の推定を行う。具体的には、式（４）を用いて、誤差共分散の推定値Ｐ_ｋ＋１ ⁻を算出する。ここで、Ｑ_ｋは、観測疑似距離、観測ドップラーの観測誤差が設定される。

続いてＳ４０５では、観測量Ｙ_ｋを取得する。具体的には、観測量Ｙ_ｋは、上述の通り、ｎ番目のＧＰＳ衛星の観測擬似距離および推測擬似距離をそれぞれＰＲ^（ｎ） _ＧＰＳ、ＰＲ^（ｎ） _ＤＲとし、ｎ番目のＧＰＳ衛星の観測ドップラー周波数および推測ドップラー周波数をＤＰ^（ｎ） _ＧＰＳ、ＤＰ^（ｎ） _ＤＲとすると観測量Ｙ_ｋは、以下の式（４２）のようなベクトル量となる。

ただし、式（４２）では、ＧＰＳ受信装置１０が信号を受信できているＧＰＳ衛星のうち、使用対象のＧＰＳ衛星としてｍ個が選択されている。使用対象のＧＰＳ衛星のリストは、制御部５０のＲＡＭ等に記録されている。なお、Ｓ４０２の直後においては、初期設定として、現在信号を受信できているＧＰＳ衛星すべてを、使用対象のＧＰＳ衛星とする。

なお、観測擬似距離ＰＲ^（ｎ） _ＧＰＳおよび観測ドップラー周波数ＤＰ^（ｎ） _ＧＰＳとしては、上述の通り、ＧＰＳ受信装置１０から出力されたｎ番目の衛星の擬似距離およびドップラー周波数を用いる。

また、推測擬似距離ＤＰ^（ｎ） _ＤＲについては、式（２０）を用いてＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標をＳＶ^ｎ _ｘ、ＳＶ^ｎ _ｙ、ＳＶ^ｎ _ｚに代入し、直前のＳ３００で算出した推測航法による車両の位置座標をｘ_{ｐｒｅｄｉｃｔｉｏｎ}、ｙ_{ｐｒｅｄｉｃｔｉｏｎ}、ｚ_{ｐｒｅｄｉｃｔｉｏｎ}に代入し、前回の図６のＳ４１０において算出したクロックバイアス誤差εＣｂ_ｋをＣｌｋ_ｂｉａｓに代入することで、算出する。

また、推測ドップラー周波数ＤＰ^（ｎ） _ＤＲについては、式（２１）を用いてＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標をＳＶ^ｎ _ｘ、ＳＶ^ｎ _ｙ、ＳＶ^ｎ _ｚに代入し、ＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標の時間微分値をＳＶ^ｎ _ｘ、ＳＶ^ｎ _ｙ、ＳＶ^ｎ _ｚの微分値に代入し、直前のＳ３００で算出した推測航法による車両の位置座標をｘ_{ｐｒｅｄｉｃｔｉｏｎ}、ｙ_{ｐｒｅｄｉｃｔｉｏｎ}、ｚ_{ｐｒｅｄｉｃｔｉｏｎ}に代入し、直前のＳ３００で算出した推測航法による車両の位置座標とＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標とに基づいて車両から当該ＧＰＳ衛星までの距離を算出し、その算出結果をＲ_{ｐｒｅｄｉｃｔｉｏｎ}に代入し、前回の図６のＳ４１０において算出したクロックドリフト誤差εＣｄ_ｋをＣｌｋ_{ｄｒｉｆｔ}に代入することで、算出する。

続いてＳ４０６では、式（５）を用いて残差の共分散Ｓ_ｋの導出を行う。ここで、Ｒ_ｋは、ＴＭ間に発生する状態量の推定誤差が設定される。また、観測行列Ｈ_ｋとしては、式（２２）を用い、式（２２）中の各偏微分値については、式（２３）〜（３０）に対して、上記の推測ドップラー周波数の算出時と同様の代入を行うことで算出する。

このようになっているので、観測量Ｙ_ｋの各成分のうち、ｎ番目のＧＰＳ衛星に関する成分ＰＲ^（ｎ） _ＧＰＳ−ＰＲ^（ｎ） _ＤＲおよびＤＰ^（ｎ） _ＧＰＳ−ＤＰ^（ｎ） _ＤＲは、使用対象のＧＰＳ衛星のうち、当該ｎ番目のＧＰＳ衛星のみの軌道（位置および移動速度）に依存する量である。

続いてＳ４０７では、観測量の検定処理を行う。図７に、この観測量の検定処理のフローチャートを示す。この観測量の検定処理において制御部５０は、まずＳ４５０で、カイ自乗値の計算を行う。カイ自乗値χ^２は以下の式（４３）で定義されている。

この定義式は、以下の式（４４）〜（４６）のように変形できる。

ここで、Ｒ^−１は、誤差σ_ｉσ_ｉで表される誤差共分散行列であり、［ｚ_ｉ−ｈ（ａ_ｉ，ｘ）］は残差を示す。これを本実施形態のタイトカップリング型カルマンフィルタに適用すると、カイ自乗値は以下の式（４７）のようになる。

このカイ自乗値χ^２は、現在の使用対象のＧＰＳ衛星のそれぞれについて、観測擬似距離と推測擬似距離の乖離が大きくなるほど大きくなる量であり、また、観測ドップラー周波数と推測ドップラー周波数の乖離が大きくなるほど大きくなる量である。

続いてＳ４５１では、算出したカイ自乗値が閾値ＴＨ以上であるか否かを判定する。閾値としては、例えば、有意水準を５％として、現在使用しているＧＰＳ衛星の数に応じて、図８に示すようなχ自乗値を閾値ＴＨとして採用する。例えば、現在使用しているＧＰＳ衛星が５個であれば、閾値ＴＨとして、１．１４５４７７２９９を採用する。カイ自乗値が閾値ＴＨ以上であると判定すれば、続いてＳ４５２に進み、使用しているＧＰＳ衛星のうち一部を選び、選んだ一部を使用対象から除外する。つまり、使用対象のＧＰＳ衛星のリストから、当該一部のＧＰＳ衛星を削除する。

選んで除外するＧＰＳ衛星としては、直前のＳ４０６で算出した残差の共分散Ｓ_ｋに基づいて決定する。例えば、残差の共分散Ｓ_ｋの対角成分のうち、最も値の大きい成分を特定し、その成分に対応するＧＰＳ衛星を除外対象としてもよい。例えば、残差の共分散Ｓ_ｋの２行２列目の成分の値が最も大きければ、１番目のＧＰＳ衛星を除外対象とする。より一般化して言えば、残差の共分散Ｓ_ｋのｊ行ｊ列目の成分の値が最も大きければ、ｊ／２を下回らない最小の整数ｉ番目のＧＰＳ衛星を除外対象とする。

このように、カルマンゲインの計算前に、精度の悪いＧＰＳ衛星を排除することで、高精度にジャイロスコープ２２の検出値、クロックゲイン誤差、クロックバイアス誤差の補正が可能となる。Ｓ４５２の後は、検定処理を終了して図６のＳ４０８に進む。また、Ｓ４５１でカイ自乗値が閾値ＴＨ以上でないと判定すれば、使用しているＧＰＳ衛星のいずれも使用対象から除外することなく、検定処理を終了してＳ４０８に進む。

Ｓ４０８では、Ｓ４０７で排除した衛星があるか否かを判定し、あればＳ４１５に進み、なければＳ４０９に進む。

Ｓ４０９では、式（６）を用いてカルマンゲインＫ_ｋを算出する。ここで、観測行列Ｈ_ｋとしては、直前に実行したＳ４０６で残差の共分散Ｓ_ｋを算出したときと同じものを用いる。

続いてＳ４１０では、式（７）を用いて、更新された状態量Ｘの推定値ｘ_ｋを算出する。ここで、観測値Ｙ_ｋとしては、直前に実行したＳ４０７の算出結果を用いる。また、カルマンゲインＫ_ｋとしては、直前に実行したＳ４０９の算出結果を用いる。また、観測行列Ｈ_ｋとしては、直前に実行したＳ４０６で残差の共分散Ｓ_ｋを算出したときと同じものを用いる。

続いてＳ４１１では、更新された誤差共分散の推定を行う。具体的には、式（８）を用いて誤差共分散Ｐ_ｋを算出する。ここで、カルマンゲインＫ_ｋとしては、直前に実行したＳ４０９の算出結果を用いる。また、観測行列Ｈ_ｋとしては、直前に実行したＳ４０６で残差の共分散Ｓ_ｋを算出したときと同じものを用いる。

続いてＳ４１２では、Ｓ４１０で算出されたｘ_ｋ、すなわち、状態量εＸ_ｋ、εＹ_ｋ、εＺ_ｋ、εＵ_ｋ、εθ_ｋ、εＯ_ｋ、εＧｂ_ｋ、εＧｓ_ｋに基づき、推測航法誤差の補正（ジャイロ６のオフセット補正、同じくゲイン補正、車速センサ２１の車速係数補正、絶対方位補正、絶対位置補正）を（４８）〜（５７）式に従って行う。
abs.y(東方向絶対位置)＝abs.y−εＹ_ｋ （４８）
abs.x(北方向絶対位置)＝abs.x−εＸ_ｋ （４９）
abs.z(高度方向絶対位置)＝abs.z−εＺ_ｋ （５０）
垂直方向速度＝スイチョウ方向速度−εＵ_ｋ （５１）
絶対水平方位＝絶対水平方位−εθ_ｋ （５２）
車速係数＝車速係数×（１−εＯ_ｋ） （５３）
オフセット補正量＝オフセット補正量−εＧｂ_ｋ （５４）
ゲイン補正量＝ゲイン補正量×（１−εＧｓ_ｋ） （５５）
クロックバイアス補正量＝クロックバイアス補正量−εＣｂｋ （５６）
クロックドリフト補正量＝クロックドリフト補正量−εＣｄｋ （５７）
なお、クロックバイアス補正量、クロックゲイン誤差補正量については、次回の図６の処理において、式（２０）、（２１）を用いて推測擬似距離、推測ドップラー周波数を算出する際に用いる。

例えば、ジャイロスコープ２２のオフセット補正およびゲイン補正により、Ｓ１００にて用いられるオフセット補正量及びゲイン補正量が修正され、車速センサ２１の車速係数補正により、Ｓ１００にて用いられる車速係数が修正され、絶対方位補正により、Ｓ３００にて用いられる絶対方位θが修正され、絶対位置補正によりＳ３００にて用いられる絶対位置が修正される。

そして、上記Ｓ４０１→Ｓ４０２→Ｓ４０３〜Ｓ４０８→Ｓ４０９〜Ｓ４１２の処理が、ＧＰＳ受信装置１０からのＧＰＳ測位データがある毎に繰り返されることにより、上記誤差が随時修正されるため、より正確な推測航法データが得られることになる。

なお、初期位置算出ができた時点では、続くＳ４０２の判定は殆どの場合ＹＥＳ（Ｓ４０３に進む）となるが、初期位置算出が一度できてしまえば、その後車両用現在位置検出装置の起動中は、常にＳ４０１からＳ４０２に進むので、例えば、初期位置算出が一度できた後に、例えば車両がビルの陰やトンネルに入ることで、それまで受信できていたＧＰＳ受信装置からの信号を受信できなると、Ｓ４０２で受信できている衛星数がＴＨｓｙ未満であると判定し、続いてＳ４１３に進む。

Ｓ４１３では、受信できている衛星数がＴＨｓｙ未満である状態が長時間（具体的にはＴ１秒以上）継続しているか否かを判定し、長時間継続していないと判定した場合は、そのまま今回の図６の処理を終了するが、長時間継続していると判定した場合は続いてＳ４１４で、プロセス行列φ_ｋの計算および誤差共分散Ｐ_ｋの予測計算を行う。

これは、ＧＰＳ受信装置１０から十分な数のＧＰＳ測位データが得られない場合、何も補正を行わないと誤差が大きくなってしまうため、誤差共分散Ｐの予測計算、即ち誤差の見積もりだけは定期的に行っておくことにより、その後にＧＰＳ受信装置１０のＧＰＳ測位データの入力が再開された時に行われるカルマンフィルタの処理を正確に行えるようにするためのものである。

また、Ｓ４０７における観測量の検定処理において、ＧＰＳ衛星が使用対象から１つでも排除された場合、Ｓ４０８で、排除衛星があると判定し、続いてＳ４１５に進み、排除後の使用対象のＧＰＳ衛星の数がＴＨｓｙ未満であるか否かを判定し、ＴＨｓｙ未満であると判定した場合は、衛星数が不十分であるので、上述のＳ４１３に進む。

ＴＨｓｙ未満でないと判定した場合は、Ｓ４０５に処理を戻す。これにより、排除したことで数が減った使用対象のＧＰＳ衛星を用いて、観測量Ｙ_ｋを再計算する。その際、観測量Ｙの数が減った分、Ｙ_ｋの要素数も２ｓ個減る（ただしｓは除外されたＧＰＳ衛星の数）。つまり、使用対象から排除したＧＰＳ衛星についての擬似距離およびドップラー周波数の成分がＹ_ｋから除去される。更に、Ｓ４０６で再度残差の共分散Ｐ_ｋを算出するが、その際に用いる観測行列Ｈ_ｋは、その時点における使用対象のＧＰＳ衛星についてのみの成分を有するように作成されるので、行数が２ｓ個減ることになる。

このように、各衛星の観測量を直接利用可能なタイトカップリング方式カルマンフィルタを用いることで、残差の共分散Ｓ_ｋを用いて精度が劣化したＧＰＳ衛星を特定することができ、更に、観測量Ｙ_ｋの各成分が、特定の１つの衛星についての量（擬似距離、ドップラー周波数）となっているので、観測量Ｙ_ｋから精度が劣化したＧＰＳ衛星の成分を選んで除外した上でカルマンフィルタの計算を継続することができるので、精度の高い補正処理を行うことができる。

つまり、ＧＰＳ精度は非連続に変化するが、推測航法軌跡精度は連続的に変化する特徴に着目し、各衛星の観測量を直接利用可能なタイトカップリング方式カルマンフィルタとその観測量（ＧＰＳ観測結果とセンサ推定結果の残差）の推定観測誤差共分散で検定することにより、ＧＰＳ測位精度劣化を正確に推定可能とする。これにより、精度劣化したＧＰＳ衛星を排除することで精度よくセンサ補正することが可能となる。

また、従来のルースカップリング型カルマンフィルタでは、車両位置を観測量として利用するので、ＧＰＳ測位に４衛星以上（３次元測位の場合）または３衛星以上（２次元測位の場合）の受信が必要であったが、本実施形態のようにタイトカップリング型カルマンフィルタを用いた場合は、擬似距離およびドップラー周波数という、最悪１衛星だけ受信できる場合でも得ることができる量を観測量としているので、ＧＰＳ衛星を１つでも受信できれば、上記の処理を継続し、自立センサ２１、２２の補正をすることが可能となる。

以上説明した通り、実施形態の車両用軌跡推定装置は、車両に搭載される自立センサ２０の出力値に基づいて推測航法により当該車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて当該車両の位置および方位を補正し、当該カルマンフィルタは、観測量として、使用対象の複数のＧＰＳ衛星毎に、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、当該車両用軌跡推定装置は、当該タイトカップリング型カルマンフィルタにおける当該使用対象の複数のＧＰＳ衛星毎の当該乖離量のいずれかが大きくなるほど大きくなる指標と所定の閾値とを比較し（Ｓ４５１）、当該指標が当該所定の閾値以上である場合、当該使用対象の複数のＧＰＳ衛星毎の当該乖離量に基づいて、当該使用対象の複数のＧＰＳ衛星のうち一部を選び、選んだ当該一部を使用対象から除外する（Ｓ４５２）。

このように、カルマンフィルタをタイトカップリング型とすることで、カルマンフィルタを実行する際に、精度が劣化したＧＰＳ測位結果の検出および排除を行い、ＧＰＳ精度劣化による推定車両軌跡の影響を軽減することで、ローコストなセンサでも精度よい軌跡を得ることができる。

なお、ジャイロスコープ２２が角速度センサに相当し、車速センサ２１が速度センサに相当し、図３のＳ１００〜Ｓ３００が走行軌跡推定手段に相当し、図３のＳ４００、図６のＳ４０２〜Ｓ４１２が誤差推定手段に相当し、Ｓ４５１が比較手段に相当し、Ｓ４５２が除外手段に相当する。
（他の実施形態）
以上、本発明の実施形態について説明したが、本発明の範囲は、上記実施形態のみに限定されるものではなく、本発明の各発明特定事項の機能を実現し得る種々の形態を包含するものである。

例えば、上記実施形態では、推定する状態量ｘ_ｋとして、絶対位置東方向誤差εＸ_ｋ、絶対位置北方向誤差εＹ_ｋ、絶対位置高度方向誤差εＺ_ｋ、垂直方向速度誤差εＵ_ｋ、絶対水平方位誤差εθ_ｋ、車速係数誤差εＯ_ｋ、ジャイロバイアス誤差εＧｂ_ｋ、ジャイロゲイン誤差εＧｓ_ｋ、クロックバイアス誤差εＣｂ_ｋ、クロックドリフト誤差εＣｄ_ｋ、という１０個の誤差値を採用しているが、必ずしもこのようになっておらずともよく、推定する状態量ｘ_ｋは、自立航法用の自立センサの出力値に影響する誤差の組であれば、どのようなものであってもよい。例えば、推定する状態量ｘ_ｋとして、絶対位置東方向誤差εＸ_ｋ、絶対位置北方向誤差εＹ_ｋ、絶対水平方位誤差εθ_ｋ、車速係数誤差εＯ_ｋ、ジャイロバイアス誤差εＧｂ_ｋ、ジャイロゲイン誤差εＧｓ_ｋという６個の誤差値を採用してもよい。

また、上記実施形態において、Ｓ４０７の検定処理およびＳ４０８の判定処理は、Ｓ４０６の次に行うようになっているが、かならずしもこのタイミングで行わなくてもよい。例えば、Ｓ４０７、Ｓ４０８の処理は、Ｓ４０９の後に行われてもよいし、Ｓ４１１の後に行われてもよい。また、図６の処理を実行する度にＳ４０７、Ｓ４０８を実行する必要はなく、図６の処理を複数回（たとえば１０回）実行するうちの９回は、Ｓ４０６に続いてＳ４０９を実行し、１回のみ、Ｓ４０６に続いてＳ４０７、Ｓ４０８を実行するようになっていてもよい。

また、上記実施形態では、タイトカップリング型カルマンフィルタで用いる観測量として、使用対象の複数のＧＰＳ衛星１００のそれぞれについて、当該ＧＰＳ衛星の観測擬似距離と、当該自立センサ２０の出力値に基づいて推測航法によって算出される車両から当該ＧＰＳ衛星までの推測擬似距離との残差、および、ＧＰＳ受信装置１０から出力される当該ＧＰＳ衛星の観測ドップラー周波数と、自立センサ２０の出力値に基づいて推測航法によって算出される当該ＧＰＳ衛星の推測ドップラー周波数との残差を採用している。

しかし、タイトカップリング型カルマンフィルタで用いる観測量としては、必ずしもこのようなものに限らず、例えば、観測擬似距離と観測ドップラー周波数の関数ｆ１（ＰＲ^（ｎ） _ＧＰＳ，ＤＰ^（ｎ） _ＧＰＳ）と、同じ関数型の観測擬似距離と観測ドップラー周波数の関数ｆ１（ＰＲ^（ｎ） _ＤＲ，ＤＰ^（ｎ） _ＤＲ）との残差を採用してもよい。

つまり、タイトカップリング型カルマンフィルタで用いる観測量としては、使用対象の複数のＧＰＳ衛星１００の毎の、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値と推測航法で算出した値の乖離量（例えば差の自乗、差の絶対値）を採用するようになっていればよい。

したがって、図７のＳ４５１で閾値ＴＨと比較する量は、必ずしも上記のようなカイ自乗値χ^２に限らず、使用対象の複数のＧＰＳ衛星１００毎の上記乖離量のいずれかが大きくなるほど大きくなる指標であれば、どのようなものであってもよい。

また、上記の実施形態において、制御回路１７がプログラムを実行することで実現している各機能は、それらの機能を有するハードウェア（例えば回路構成をプログラムすることが可能なＦＰＧＡ）を用いて実現するようになっていてもよい。

１０ ＧＰＳ受信装置
１１ ＧＰＳアンテナ部
１２ ＧＰＳ−ＲＦ部
１３ ＧＰＳ信号処理部
１４ ＧＰＳ測位演算部
２０ 自立センサ
２１ 車速センサ
２２ ジャイロスコープ
３０ 地図データ記憶装置
４０ ディスプレイ部
５０ 制御部
１００ ＧＰＳ衛星

Claims (4)

1. 車両に搭載される自立センサ（２０）の出力値に基づいて推測航法により前記車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて前記車両の位置および方位を補正する車両用軌跡推定装置であって、
   前記カルマンフィルタは、観測量として、使用対象の複数のＧＰＳ衛星毎に、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、
   当該車両用軌跡推定装置は、
   前記タイトカップリング型カルマンフィルタにおいて前記観測量の共分散（Ｓ_ｋ）の導出を行う第１共分散導出手段（Ｓ４０６）と、
   観測量の検定処理（Ｓ４０７）において、前記第１共分散導出手段によって導出された前記共分散（Ｓ_ｋ）を用いて、前記タイトカップリング型カルマンフィルタにおける前記使用対象の複数のＧＰＳ衛星毎の前記乖離量のいずれかが大きくなるほど大きくなる指標を算出する指標算出手段（Ｓ４５０）と、
   前記検定処理において、前記指標算出手段（Ｓ４５０）が算出した指標と所定の閾値とを比較する比較手段（Ｓ４５１）と、
   前記検定処理において、前記指標が前記所定の閾値以上であると前記比較手段（Ｓ４５１）が判定した場合、前記使用対象の複数のＧＰＳ衛星毎の前記乖離量に基づいて、前記使用対象の複数のＧＰＳ衛星のうち一部を選び、選んだ前記一部を使用対象から除外する除外手段（Ｓ４５２）と、
   前記タイトカップリング型カルマンフィルタにおいて、前記除外手段によって除外されたことで数が減った使用対象のＧＰＳ衛星に基づいて、前記観測量の共分散（Ｓ_ｋ）の導出を再度行う第２共分散算出手段（Ｓ４０６）と、
   前記タイトカップリング型カルマンフィルタにおいて、前記第１共分散導出手段によって算出された前記観測量の共分散（Ｓ_ｋ）の代わりに、前記第２共分散導出手段によって再度算出された前記観測量の共分散（Ｓ_ｋ）を用いて、前記状態量の推定値を算出する状態量推定値算出手段（Ｓ４０９、Ｓ４１０）と、を備えた車両用軌跡推定装置。
2. 前記指標算出手段が算出する前記指標は、前記使用対象の複数のＧＰＳ衛星毎の前記乖離量のどれが大きくなっても大きくなる量であることを特徴とする請求項１に記載の車両用軌跡推定装置。
3. 前記比較手段が前記指標と比較する前記所定の閾値は、使用する対象のＧＰＳ衛星の数に基づいて設定されることを特徴とする請求項２に記載の車両用軌跡推定装置。
4. 前記指標算出手段が算出する前記指標は、カイ自乗値であることを特徴とする請求項１ないし３のいずれか１つに記載の車両用軌跡推定装置。

Images