車両用軌跡推定装置
【課題】ＧＰＳ受信装置がＧＰＳ衛星からの信号に基づいて算出する情報をカルマンフィルタの観測量に用いることで、自立センサの誤差を推定し、推定したセンサ誤差を補正する技術において、カルマンフィルタで用いるＧＰＳ測位結果の精度の劣化を全体として抑える。
【解決手段】カルマンフィルタは、観測量として、使用対象の複数のＧＰＳ衛星毎に、当該ＧＰＳ衛星の擬似距離およびドップラー周波数について、その量をＧＰＳ衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型である。当該車両用軌跡推定装置は、当該カルマンフィルタにおける上記乖離量についてのカイ自乗値と所定の閾値とを比較し（Ｓ４５１）、当該カイ自乗値が当該所定の閾値以上である場合、使用対象の複数のＧＰＳ衛星毎うち精度の低い一部を選び、選んだ当該一部を使用対象から除外する（Ｓ４５２）。
【解決手段】カルマンフィルタは、観測量として、使用対象の複数のＧＰＳ衛星毎に、当該ＧＰＳ衛星の擬似距離およびドップラー周波数について、その量をＧＰＳ衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型である。当該車両用軌跡推定装置は、当該カルマンフィルタにおける上記乖離量についてのカイ自乗値と所定の閾値とを比較し（Ｓ４５１）、当該カイ自乗値が当該所定の閾値以上である場合、使用対象の複数のＧＰＳ衛星毎うち精度の低い一部を選び、選んだ当該一部を使用対象から除外する（Ｓ４５２）。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両用軌跡推定装置に関するものである。
【背景技術】
【０００２】
従来、ＧＰＳ受信装置が算出する現在位置情報をカルマンフィルタの観測量に用いることで、自立センサ（車速センサ、ジャイロスコープ等）の誤差を推定し、推定したセンサ誤差を補正する技術が知られている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０００−５５６７８号公報
【特許文献２】特開２００８−２３２７６１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、ＧＰＳ受信装置による測位結果は、マルチパス波の受信やＧＰＳ衛星配置悪化等の要因により測位精度が大きく変動する。精度が劣化したＧＰＳ測位結果を使用してカルマンフィルタを実行するとセンサ誤差の推定を誤り、走行軌跡の推定精度が悪化してしまうという問題がある。
【０００５】
本発明はこの問題点に鑑み、ＧＰＳ受信装置がＧＰＳ衛星からの信号に基づいて算出する情報をカルマンフィルタの観測量に用いることで、自立センサの誤差を推定し、推定したセンサ誤差を補正する技術において、カルマンフィルタで用いるＧＰＳ測位結果の精度の劣化を全体として抑えることを目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための請求項１に記載の発明は、車両に搭載される自立センサ（２０）の出力値に基づいて推測航法により前記車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて前記車両の位置および方位を補正する車両用軌跡推定装置であって、前記カルマンフィルタは、観測量として、使用対象の複数のＧＰＳ衛星毎に、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、当該車両用軌跡推定装置は、前記タイトカップリング型カルマンフィルタにおける前記使用対象の複数のＧＰＳ衛星毎の前記乖離量のいずれかが大きくなるほど大きくなる指標と所定の閾値とを比較する比較手段（Ｓ４５１）と、前記指標が前記所定の閾値以上であると前記比較手段（Ｓ４５１）が判定した場合、前記使用対象の複数のＧＰＳ衛星毎の前記乖離量に基づいて、前記使用対象の複数のＧＰＳ衛星のうち一部を選び、選んだ前記一部を使用対象から除外する除外手段（Ｓ４５２）と、を備えた車両用軌跡推定装置である。
【０００７】
このように、カルマンフィルタをタイトカップリング型とすることで、カルマンフィルタを実行する際に、精度が劣化したＧＰＳ測位結果の検出および排除を行い、ＧＰＳ精度劣化による推定車両軌跡の影響を軽減することで、精度よく車両の位置および方位を補正することができる。 なお、上記および特許請求の範囲における括弧内の符号は、特許請求の範囲に記載された用語と後述の実施形態に記載される当該用語を例示する具体物等との対応関係を示すものである。
【図面の簡単な説明】
【０００８】
【図１】本発明の一実施形態に係る車両用現在位置検出装置の構成を示す図である。
【図２】信号生発生過程、観測過程、カルマンフィルタの模式図である。
【図３】制御部による推測航法処理のフローチャートである。
【図４】ジャイロスコープのオフセット補正について説明するための図である。
【図５】ジャイロスコープのゲイン補正について説明するための図である。
【図６】制御部によるＧＰＳ複合化処理のフローチャートである。
【図７】制御部による観測量の検定処理のフローチャートである。
【図８】有意水準５％の閾値を示す図表である。
【発明を実施するための形態】
【０００９】
以下、本発明の一実施形態について説明する。図１に、本実施形態に係る車両用現在位置検出装置の構成を示す。本実施形態に係る車両用現在位置検出装置は車両用ナビゲーション装置として構成されており、ＧＰＳ受信装置１０、自立センサ２０、地図データ記憶装置３０、ディスプレイ部４０、および制御部５０を備えている。
【００１０】
ＧＰＳ受信装置１０は、複数のＧＰＳ衛星１００（図中では、１つのみ示す）からの電波を受信するＧＰＳアンテナ部１１、ＧＰＳアンテナ部１１から入力される高周波信号を中間周波数帯の受信信号に変換するＧＰＳ―ＲＦ部１２、ＧＰＳ―ＲＦ部１２から入力される受信信号の同期捕捉や同期保持等の信号処理を行うＧＰＳ信号処理部１３、ＧＰＳ信号処理部１３から出力されるデータに基づいてＧＰＳ測位演算を行い、このＧＰＳ測位演算により求めた測位データ、複数のＧＰＳ衛星１００のそれぞれの絶対位置、複数のＧＰＳ衛星１００のそれぞれの擬似距離およびドップラー周波数等を出力するＧＰＳ測位演算部１４を備えている。
【００１１】
ＧＰＳ受信装置１０は、ＧＰＳ衛星１００からの電波を受信し、車両の絶対的な現在位置を表すＧＰＳ位置、車両の進行方向を特定するための速度ベクトル等を算出し、算出結果を測位データとして出力する。
【００１２】
自立センサ２０は、車両の走行速度に応じた車速パルス信号を出力する速度センサとしての車速センサ２１と、車両の回転角速度に応じた角速度信号を出力する角速度センサとしてのジャイロスコープ２２を備えている。
【００１３】
地図データ記憶装置３０は、地図データが記憶された地図データ記憶媒体から地図データを読み出すための装置である。地図データには、各リンクの距離、道路種別、車線数、一方通行規制、信号機の位置等を表す道路データ、位置検出精度を向上するためのいわゆるマップマッチングデータ、川、湖、海、鉄道、施設などの位置、形状、名称を表す背景データ、各地の施設の名称、所在位置、施設種類等を示す施設データ等が含まれる。なお、地図データ記憶媒体としては、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、ハードディスクドライブ、メモリカード等がある。
【００１４】
ディスプレイ部４０は、液晶等の表示部を備え、コンピュータから入力される映像信号に応じた映像を表示部に表示させる。
【００１５】
制御部５０は、ＣＰＵ、ＲＯＭ、ＲＡＭ、不揮発性メモリ、Ｉ／Ｏ等を有するコンピュータにより構成されており、ＣＰＵは、ＲＯＭに記憶されたプログラムに従って各種処理を実施する。
【００１６】
制御部５０の処理としては、ジャイロスコープ２２の出力値に基づいて算出される方位変化量と、速度センサ２１の出力値に基づいて算出される車速係数から、車両の相対軌跡を推定するとともに当該車両の相対軌跡とＧＰＳ受信装置１０からの出力値とに基づいて車両の走行軌跡を推定する走行軌跡推定処理、ジャイロスコープ２２の出力値およびＧＰＳ受信装置１０からの出力値に基づいて方位変化量のオフセット誤差、方位変化量のゲイン誤差、絶対方位誤差等を推定する誤差推定処理、自車の走行軌跡と地図メモリ７０に格納されている地図情報の道路データとを照合することによって、自車位置（ＧＰＳ位置により補正された推測位置）を道路上の位置に修正するマップマッチング処理等がある。なお、走行軌跡推定処理では、誤差推定処理により推定された各誤差を用いて走行軌跡推定処理による車両の走行軌跡の推定に用いられる方位変化量のオフセット、方位変化量のゲインおよび絶対方位等を補正するようになっている。
【００１７】
制御部５０は、誤差推定処理においてカルマンフィルタを用いて推測航法における各誤差要因を１つの状態量ｘとしてモデル化し、各誤差の推定値を求める誤差推定処理を実施する。
【００１８】
ここで、カルマンフィルタの概要について説明する。カルマンフィルタは、図２および式（１）、（２）に示すような信号発生過程および観測過程を前提としている。
【００１９】
【数１】
すなわち、プロセス行列φｋで定義される線形システム（信号発生過程）があり、そのシステムにおいて状態量Ｘｋに対して、観測行列Ｈｋで関係付けられた状態量Ｘｋの一部が観測可能（観測過程）である場合、カルマンフィルタは、この観測量Ｙｋを入力として、状態量Ｘｋの最適な推定値ｘｋを与えるものである。なお、ｒｋは信号発生過程にて発生する雑音であり、ｑｋは観測過程において発生する雑音である。
【００２０】
そして、時刻ｋまでの情報を用いた状態量Ｘｋの最適推定値ｘｋ＝ｘ（ｋ｜ｋ）は、次の式（３）〜（８）により求めることができる。
【００２１】
【数２】
ここで、Ｋｋはカルマンゲイン、Ｐｋは推定値ｘｋの共分散（以下では誤差共分散という）、Ｑｋは観測過程で発生する観測雑音ｑｋの分散、Ｒｋは信号発生過程で発生するシステム雑音ｒｋの分散である。またＳｋは、式（５）で定義される残差の共分散である。また各量（仮にＡｋとする）の右上に−（マイナス記号）の上付き添字が付された量Ａｋ−は、時刻ｋ−１までの情報に基づく時刻ｋでの量Ａｋの推定値（すなわちＡ（ｋ｜ｋ−１））である。また、雑音ｒｋ、ｑｋはいずれも平均０の白色ガウス雑音であり互いに無相関である。また、Ｉは単位行列であり、上付き添字Ｔは転置を意味し、上付き添字−１は逆行列を意味する。
【００２２】
このように定義されたカルマンフィルタでは、状態量の推定値ｘｋを定義すると共に、誤差共分散Ｐｋに適当な初期値を設定するとにより、状態量の最適推定値ｘｋが得られ、観測が行われ観測量Ｙｋが得られる毎にこれを繰り返すことにより、状態量の最適推定値ｘｋの精度が向上する。
【００２３】
そして、本実施例では、状態量Ｘｋとして、東方向の絶対位置を算出する際に付与される絶対位置東方向誤差εＸｋ、北方向の絶対位置を算出する際に付与される絶対位置北方向誤差εＹｋ、高度方向の絶対位置を算出する際に付与される絶対位置高度方向誤差εＺｋ、垂直方向の速度を算出する際に付与される垂直方向速度誤差εＵｋ、絶対水平方位を算出する際に付与される絶対水平方位誤差εθｋ、車速を算出する際に、車速センサ２１から出力された単位時間当たりのパルス数に乗じられる車速係数の誤差である車速係数誤差εＯｋ、ジャイロ出力の大きさによらず、出力値の全領域に均等に付加されるジャイロバイアス誤差εＧｂｋ、ジャイロ出力から角速度への変換比率として設定される変換ゲインの誤差であるジャイロゲイン誤差εＧｓｋ、クロックバイアス誤差εＣｂｋ、クロックドリフト誤差εＣｄｋ、という１０個の誤差値を採用し、これら誤差値を次の式（９）〜（１８）により定義する。
【００２４】
【数３】
ただし、Ｔは前回（の観測）からの経過時間、ＤＴは前回からの方位変化量、ＬＴは前回からの移動距離、θＴは真の絶対方位である。
【００２５】
そして、これら誤差値の時間的な変化を規定するものがプロセス行列φｋであり、上記式（９）〜（１８）を各誤差値にて偏微分し線形化することにより求められ、その結果、プロセス行列φｋは式（１９）のようになる。
【００２６】
【数４】
ただし、θｋ＝θＴ＋εθｋ−１＋Ｔ×εＧｋ−１／２＋ＤＴ×εＧｓｋ−１／２
であり、このθｋは絶対方位を意味しており、真の絶対方位θＴにセンサ誤差に基づく誤差が加わったものとなっている。また、Ｇｓは車速係数であり、Ｄは前回からの方位変化量であり、Ｌは前回からの移動距離である。
【００２７】
また、観測量Ｙｋとしては、各々のＧＰＳ衛星の観測擬似距離と推測擬似距離との残差、および、各々のＧＰＳ衛星の観測ドップラー周波数と推測ドップラー周波数との残差を採用する。
【００２８】
ここで、各ＧＰＳ衛星の観測擬似距離および観測ドップラー周波数は、ＧＰＳ受信装置１０によって計測され、ＧＰＳ受信装置１０から制御部５０に随時出力されているものを用いる。また、各ＧＰＳ衛星の推測擬似距離および推測ドップラー周波数は、ＧＰＳ受信装置１０から出力される各ＧＰＳ衛星の現在位置、後述するＳ１００で推測航法によって算出される自車両の移動距離、後述するＳ３００で推測航法によって算出される自車両の絶対方位、絶対位置、および車速等に基づいて算出する。
【００２９】
このように、擬似距離、ドップラー周波数等の、ＧＰＳ衛星毎に算出される量を観測量とするカルマンフィルタを、タイトカップリング型カルマンフィルタという。一方、特許文献１に記載のような観測量を用いるカルマンフィルタをルーズカップリング型カルマンフィルタいう。タイトカップリング型カルマンフィルタも、周知の技術であり、例えば、特許文献２に記載されている。
【００３０】
ルーズカップリング型カルマンフィルタは、ＧＰＳ測位結果（位置、方位、速度）を観測量として利用するが、本実施形態のようなタイトカップリング型カルマンフィルタは、各ＧＰＳ衛星の観測疑似距離とドップラー周波数を観測量として利用する。つまり、タイトカップリング型カルマンフィルタは、ＧＰＳ各衛星の観測疑似距離とドップラー周波数とセンサで推定した観測疑似距離と観測ドップラー周波数との差分を、誤差共分散行列に従い、最小にする状態量を推定する。
【００３１】
ここで、ｎ番目のＧＰＳ衛星の擬似距離ＰＲ（ｎ）は、式（２０）のように定義される。
【００３２】
【数５】
ただし、ＳＶｎｘ、ＳＶｎｙ、ＳＶｎｚは、それぞれｎ番目のＧＰＳ衛星の位置座標の成分であり、ｘｐｒｅｄｉｃｔｉｏｎ、ｙｐｒｅｄｉｃｔｉｏｎ、ｚｐｒｅｄｉｃｔｉｏｎは、ＧＰＳ受信装置１０の位置座標であり、Ｃｌｋｂｉａｓは、クロックバイアス誤差εＣｂｋである。なお、式（２０）のｘ、ｙ、ｚの向きは、観測量のεＸ、εＹ、εＺと同じである。
【００３３】
また、ｎ番目のＧＰＳ衛星の観測ドップラー周波数（積算ドップラー）ＤＰ（ｎ）は、式（２１）のように定義される。
【００３４】
【数６】
ただし、ＳＶの上の黒丸は、時間微分を表し、ＲｎｐｒｅｄｉｃｔｉｏｎはＧＰＳ受信装置１０からｎ番目のＧＰＳ衛星までの距離を示し、Ｃｌｋｄｒｉｆｔはクロックドリフト誤差εＣｄｋを表す。
【００３５】
これらの定義式（２０）、（２１）を各状態量εＸｋ、εＹｋ、εＺｋ、εＵｋ、εθｋ、εＯｋ、εＧｂｋ、εＧｓｋ、εＣｂｋ、εＣｄｋにて偏微分し線形化することにより、観測行列Ｈｋが式（２２）のように得られる。
【００３６】
【数７】
ここで、各偏微分は、式（２３）〜（３０）のように表される。
【００３７】
【数８】
【００３８】
【数９】
なお、式（２２）では、２個のＧＰＳ衛星を用いる場合の観測行列Ｈｋの例であり、ｍ個のＧＰＳ衛星を用いる場合の観測行列Ｈｋは、２ｎ行１０列の行列となる。
【００３９】
このように、ＧＰＳ毎に別々に定義された観測量を用いるタイトカップリング型カルマンフィルタを用いる制御部５０は、車速センサ２１からの信号に基づいて車両の移動距離を算出すると共に、ジャイロスコープ２２からの信号に基づいて車両の方位変化量を算出する。そして、これら算出された移動距離及び方位変化量に基づいて、推測航法データとして、車両の速度、相対軌跡、絶対位置、絶対方位を算出する。一方、ＧＰＳ受信装置１０は、ＧＰＳ測位データとして、車両の位置、方位、車速を随時出力し、また、各衛星の位置座標、各衛星までの観測擬似距離、および各衛星の観測ドップラー周波数を随時出力している。
【００４０】
そして、制御部５０は、タイトカップリング型カルマンフィルタを用いて、推測航法データおよびＧＰＳ測位データ、各衛星の位置座標、観測擬似距離、および観測ドップラー周波数に基づいて、各誤差εＸｋ、εＹｋ、εＺｋ、εＵｋ、εθｋ、εＯｋ、εＧｂｋ、εＧｓｋ、εＣｂｋ、εＣｄｋの推定値を算出し、更に、この推定値に基づいて、車両の方位変化量、移動距離、相対軌跡、絶対位置等の算出および補正を行う。
【００４１】
以下、制御部５０による推測航法処理について説明する。図３に、推測航法処理のフローチャートを示す。本車両用現在位置検出装置が動作状態になると、制御部５０は、図４に示す処理を一定周期ＴＭ毎に繰り返し実行する。
【００４２】
まずＳ１００では、方位変化量・移動距離演算処理を実行する。この処理では、まず、ジャイロスコープ２２により検出され出力されたジャイロ出力角速度にメインルーチンの起動周期ＴＭを乗じることにより車両の向きの方位変化量を算出する。
方位変化量＝ジャイロ出力角速度×ＴＭ （３１）
なお、ジャイロ出力角速度は、図４のグラフから明らかなように、ジャイロスコープ２２の出力をサンプリングした値から、オフセット値(図４では２．５Ｖ) を減じたものに、変換ゲイン(グラフの傾きに相当する) を乗じることにより算出されたものである。
【００４３】
次に、後述するオフセット補正量に一定周期ＴＭを乗じたものを、式（３１）により求めた方位変化量を減じることにより、方位変化量のオフセット補正を行う。
【００４４】
方位変化量＝方位変化量−オフセット補正量×ＴＭ （３２）
すなわち、図４に示すように、角速度０［ｄｅｇ／ｓｅｃ] に対応するジャイロスコープ２２の出力電圧(ゼロ点) の変動に基づく誤差を補正する。次に、オフセット補正された方位変化量に、後述するゲイン補正量を減じることにより、方位変化量のゲイン補正を行う。
【００４５】
方位変化量＝方位変化量×ゲイン補正量 （３３）
すなわち、図５に示すように、グラフの傾き(変換ゲイン) の変動に基づく誤差を補正する。
【００４６】
そして、一定周期ＴＭの間に検出された車速センサ２１からの出力パルス数に、後述する車速係数を乗じることにより移動距離を算出する。
【００４７】
移動距離＝車速センサパルス数×車速係数 （３４）
このように、移動距離を算出すると、方位変化量・移動距離の演算処理を終了する。
【００４８】
次のＳ２００の相対軌跡の演算処理では、まず、式（３３）を用いて算出した方位変化量を、それまでに求められている相対方位に加算することにより、車両の向きの相対方位を更新する。
【００４９】
相対方位＝相対方位＋方位変化量 （３５）
次に、この更新した相対方位および式（３４）を用いて算出した移動距離に基づき、相対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした相対座標ｒｅｌ．ｘ、東西方向をｙ座標軸とした相対座標ｒｅｌ．ｙを、式（３６）、（３７）に従って更新する。ただし、θｓは式（３５）を用いて算出した相対方位である。
【００５０】
ｒｅｌ．ｘ＝ｒｅｌ．ｘ＋移動距離×ｃｏｓ（θｓ） （３６）
ｒｅｌ．ｙ＝ｒｅｌ．ｙ＋移動距離×ｓｉｎ（θｓ） （３７）
すなわち、この更新は、移動距離に対する相対方位のＸ、Ｙ成分をそれまでの相対位置座標に加算することにより行う。この相対位置座標は相対軌跡を求めるために算出するもので、その相対軌跡と道路形状との関係により、いわゆるマップマッチングが行われる。
【００５１】
次に、式（３４）を用いて算出した移動距離を、周期ＴＭで除することにより車速を算出する。
【００５２】
車速＝移動距離／ＴＭ （３８）
このように、車速を算出すると、相対軌跡演算処理を終了する。
【００５３】
次のＳ３００の絶対方位・絶対位置の演算処理では、まず、式（３４）を用いて算出した方位変化量を、それまでに求められている車両の向きの絶対方位に加算することにより、絶対方位を更新する。
【００５４】
絶対方位＝絶対方位＋方位変化量 （３９）
次に、この更新した絶対方位および式（３４）を用いて算出した移動距離に基づき、絶対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした絶対座標ａｂｓ．ｘ、東西方向をｙ座標軸とした絶対座標ａｂｓ．ｙを、式（４０）、（４１）に従って更新する。ただし、θは式（３９）を用いて算出した絶対方位である。
【００５５】
ａｂｓ．ｘ＝ａｂｓ．ｘ＋移動距離×ｃｏｓ（θ） （４０）
ａｂｓ．ｙ＝ａｂｓ．ｙ＋移動距離×ｓｉｎ（θ） （４１）
すなわち、この更新は、移動距離に対する絶対方位のＸ、Ｙ成分をそれまでの絶対位置座標に加算することにより行う。このようにして更新した車両の絶対方位と絶対位置は後述するＧＰＳとの複合化処理にて利用される。
【００５６】
次に、Ｓ４００のＧＰＳ複合化処理を実施する。このＧＰＳ複合化処理のフローチャートを図７に示す。ここでは、このＧＰＳ複合化処理の実行回数がｋ回目であるとして説明する。
【００５７】
このＧＰＳ複合化処理では、まずＳ４０１で、車両用現在位置検出装置の起動後、ＧＰＳ受信装置１０が周知の初期位置算出（ｆｉｒｓｔ ｆｉｘ）を行ったか否か判定し、行っていなければ直ちにＧＰＳ復号化処理を終了し、行っていれば続いてＳ４０２に進む。なお、ＧＰＳ受信装置１０は、周知の初期位置算出を行ったとき、測位データを制御部５０に出力するので、測位データを制御部５０は、車両用現在位置検出装置の起動後、一度以上測位データを受け取っていれば、初期位置算出を行ったと判定する。
【００５８】
Ｓ４０２では、ＧＰＳ受信装置１０が信号を受信できている衛星数が所定の衛星数ＴＨｓｙ以上であるか否かを判定し、衛星数ＴＨｓｙ未満であれば直ちにＧＰＳ復号化処理を終了し、衛星数ＴＨｓｙ以上であればＳ４０３に進む。なお、ＧＰＳ受信装置１０は信号を受信できている衛星数の情報を随時制御部５０に出力するようになっているので、Ｓ４０２では、この情報に基づいて判定を行えばよい。この閾値ＴＨｓｙは、１でもよいし、２でもよいし、３以上であってもよい。
【００５９】
続いてＳ４０３では、状態量の推定を行う。具体的には、式（３）を用いて、状態量の推定値ｘｋ―を算出する。続いてＳ４０４では、誤差共分散の推定を行う。具体的には、式（４）を用いて、誤差共分散の推定値Ｐｋ＋１−を算出する。ここで、Ｑｋは、観測疑似距離、観測ドップラーの観測誤差が設定される。
【００６０】
続いてＳ４０５では、観測量Ｙｋを取得する。具体的には、観測量Ｙｋは、上述の通り、ｎ番目のＧＰＳ衛星の観測擬似距離および推測擬似距離をそれぞれＰＲ（ｎ）ＧＰＳ、ＰＲ（ｎ）ＤＲとし、ｎ番目のＧＰＳ衛星の観測ドップラー周波数および推測ドップラー周波数をＤＰ（ｎ）ＧＰＳ、ＤＰ（ｎ）ＤＲとすると観測量Ｙｋは、以下の式（４２）のようなベクトル量となる。
【００６１】
【数１０】
ただし、式（４２）では、ＧＰＳ受信装置１０が信号を受信できているＧＰＳ衛星のうち、使用対象のＧＰＳ衛星としてｍ個が選択されている。使用対象のＧＰＳ衛星のリストは、制御部５０のＲＡＭ等に記録されている。なお、Ｓ４０２の直後においては、初期設定として、現在信号を受信できているＧＰＳ衛星すべてを、使用対象のＧＰＳ衛星とする。
【００６２】
なお、観測擬似距離ＰＲ（ｎ）ＧＰＳおよび観測ドップラー周波数ＤＰ（ｎ）ＧＰＳとしては、上述の通り、ＧＰＳ受信装置１０から出力されたｎ番目の衛星の擬似距離およびドップラー周波数を用いる。
【００６３】
また、推測擬似距離ＤＰ（ｎ）ＤＲについては、式（２０）を用いてＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標をＳＶｎｘ、ＳＶｎｙ、ＳＶｎｚに代入し、直前のＳ３００で算出した推測航法による車両の位置座標をｘｐｒｅｄｉｃｔｉｏｎ、ｙｐｒｅｄｉｃｔｉｏｎ、ｚｐｒｅｄｉｃｔｉｏｎに代入し、前回の図６のＳ４１０において算出したクロックバイアス誤差εＣｂｋをＣｌｋｂｉａｓに代入することで、算出する。
【００６４】
また、推測ドップラー周波数ＤＰ（ｎ）ＤＲについては、式（２１）を用いてＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標をＳＶｎｘ、ＳＶｎｙ、ＳＶｎｚに代入し、ＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標の時間微分値をＳＶｎｘ、ＳＶｎｙ、ＳＶｎｚの微分値に代入し、直前のＳ３００で算出した推測航法による車両の位置座標をｘｐｒｅｄｉｃｔｉｏｎ、ｙｐｒｅｄｉｃｔｉｏｎ、ｚｐｒｅｄｉｃｔｉｏｎに代入し、直前のＳ３００で算出した推測航法による車両の位置座標とＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標とに基づいて車両から当該ＧＰＳ衛星までの距離を算出し、その算出結果をＲｐｒｅｄｉｃｔｉｏｎに代入し、＊＊＊＊をＶＵに代入し、＊＊＊＊をνに代入し、＊＊＊＊をδＳＦｏｄｏに代入し、＊＊＊＊ψに代入し、前回の図６のＳ４１０において算出したクロックドリフト誤差εＣｄｋをＣｌｋｄｒｉｆｔに代入することで、算出する。
【００６５】
続いてＳ４０６では、式（５）を用いて残差の共分散Ｓｋの導出を行う。ここで、Ｒｋは、ＴＭ間に発生する状態量の推定誤差が設定される。また、観測行列Ｈｋとしては、式（２２）を用い、式（２２）中の各偏微分値については、式（２３）〜（３０）に対して、上記の推測ドップラー周波数の算出時と同様の代入を行うことで算出する。
【００６６】
このようになっているので、観測量Ｙｋの各成分のうち、ｎ番目のＧＰＳ衛星に関する成分ＰＲ（ｎ）ＧＰＳ−ＰＲ（ｎ）ＤＲおよびＤＰ（ｎ）ＧＰＳ−ＤＰ（ｎ）ＤＲは、使用対象のＧＰＳ衛星のうち、当該ｎ番目のＧＰＳ衛星のみの軌道（位置および移動速度）に依存する量である。
【００６７】
続いてＳ４０７では、観測量の検定処理を行う。図７に、この観測量の検定処理のフローチャートを示す。この観測量の検定処理において制御部５０は、まずＳ４５０で、カイ自乗値の計算を行う。カイ自乗値χ２は以下の式（４３）で定義されている。
【００６８】
【数１１】
この定義式は、以下の式（４４）〜（４６）のように変形できる。
【００６９】
【数１２】
ここで、Ｒ−１は、誤差σｉσｉで表される誤差共分散行列であり、［ｚｉ−ｈ（ａｉ，ｘ）］は残差を示す。これを本実施形態のタイトカップリング型カルマンフィルタに適用すると、カイ自乗値は以下の式（４７）のようになる。
【００７０】
【数１３】
このカイ自乗値χ２は、現在の使用対象のＧＰＳ衛星のそれぞれについて、観測擬似距離と推測擬似距離の乖離が大きくなるほど大きくなる量であり、また、観測ドップラー周波数と推測ドップラー周波数の乖離が大きくなるほど大きくなる量である。
【００７１】
続いてＳ４５１では、算出したカイ自乗値が閾値ＴＨ以上であるか否かを判定する。閾値としては、例えば、有意水準を５％として、現在使用しているＧＰＳ衛星の数に応じて、図５に示すようなχ自乗値を閾値ＴＨとして採用する。例えば、現在使用しているＧＰＳ衛星が５個であれば、閾値ＴＨとして、１．１４５４７７２９９を採用する。カイ自乗値が閾値ＴＨ以上であると判定すれば、続いてＳ４５２に進み、使用しているＧＰＳ衛星のうち一部を選び、選んだ一部を使用対象から除外する。つまり、使用対象のＧＰＳ衛星のリストから、当該一部のＧＰＳ衛星を削除する。
【００７２】
選んで除外するＧＰＳ衛星としては、直前のＳ４０６で算出した残差の共分散Ｓｋに基づいて決定する。例えば、残差の共分散Ｓｋの対角成分のうち、最も値の大きい成分を特定し、その成分に対応するＧＰＳ衛星を除外対象としてもよい。例えば、残差の共分散Ｓｋの２行２列目の成分の値が最も大きければ、１番目のＧＰＳ衛星を除外対象とする。より一般化して言えば、残差の共分散Ｓｋのｊ行ｊ列目の成分の値が最も大きければ、ｊ／２を下回らない最小の整数ｉ番目のＧＰＳ衛星を除外対象とする。
【００７３】
このように、カルマンゲインの計算前に、精度の悪いＧＰＳ衛星を排除することで、高精度にジャイロスコープ２２の検出値、クロックゲイン誤差、クロックバイアス誤差の補正が可能となる。Ｓ４５２の後は、検定処理を終了して図６のＳ４０８に進む。また、Ｓ４５１でカイ自乗値が閾値ＴＨ以上でないと判定すれば、使用しているＧＰＳ衛星のいずれも使用対象から除外することなく、検定処理を終了してＳ４０８に進む。
【００７４】
Ｓ４０８では、Ｓ４０７で排除した衛星があるか否かを判定し、あればＳ４１５に進み、なければＳ４０９に進む。
【００７５】
Ｓ４０９では、式（６）を用いてカルマンゲインＫｋを算出する。ここで、観測行列Ｈｋとしては、直前に実行したＳ４０６で残差の共分散Ｓｋを算出したときと同じものを用いる。
【００７６】
続いてＳ４１０では、式（７）を用いて、更新された状態量Ｘの推定値ｘｋを算出する。ここで、観測値Ｙｋとしては、直前に実行したＳ４０７の算出結果を用いる。また、カルマンゲインＫｋとしては、直前に実行したＳ４０９の算出結果を用いる。また、観測行列Ｈｋとしては、直前に実行したＳ４０６で残差の共分散Ｓｋを算出したときと同じものを用いる。
【００７７】
続いてＳ４１１では、更新された誤差共分散の推定を行う。具体的には、式（８）を用いて誤差共分散Ｐｋを算出する。ここで、カルマンゲインＫｋとしては、直前に実行したＳ４０９の算出結果を用いる。また、観測行列Ｈｋとしては、直前に実行したＳ４０６で残差の共分散Ｓｋを算出したときと同じものを用いる。
【００７８】
続いてＳ４１２では、Ｓ４１０で算出されたｘｋ、すなわち、状態量εＸｋ、εＹｋ、εＺｋ、εＵｋ、εθｋ、εＯｋ、εＧｂｋ、εＧｓｋに基づき、推測航法誤差の補正（ジャイロ６のオフセット補正、同じくゲイン補正、車速センサ２１の車速係数補正、絶対方位補正、絶対位置補正）を（４８）〜（５７）式に従って行う。
abs.y(東方向絶対位置)＝abs.y−εＹｋ （４８）
abs.x(北方向絶対位置)＝abs.x−εＸｋ （４９）
abs.z(高度方向絶対位置)＝abs.z−εＺｋ （５０）
垂直方向速度＝スイチョウ方向速度−εＵｋ （５１）
絶対水平方位＝絶対水平方位−εθｋ （５２）
車速係数＝車速係数×（１−εＯｋ） （５３）
オフセット補正量＝オフセット補正量−εＧｂｋ （５４）
ゲイン補正量＝ゲイン補正量×（１−εＧｓｋ） （５５）
クロックバイアス補正量＝クロックバイアス補正量−εＣｂｋ （５６）
クロックドリフト補正量＝クロックドリフト補正量−εＣｄｋ （５７）
なお、クロックバイアス補正量、クロックゲイン誤差補正量については、次回の図６の処理において、式（２０）、（２１）を用いて推測擬似距離、推測ドップラー周波数を算出する際に用いる。
【００７９】
例えば、ジャイロスコープ２２のオフセット補正およびゲイン補正により、Ｓ１００にて用いられるオフセット補正量及びゲイン補正量が修正され、車速センサ２１の車速係数補正により、Ｓ１００にて用いられる車速係数が修正され、絶対方位補正により、Ｓ３００にて用いられる絶対方位θが修正され、絶対位置補正によりＳ３００にて用いられる絶対位置が修正される。
【００８０】
そして、上記Ｓ４０１→Ｓ４０２→Ｓ４０３〜Ｓ４０８→Ｓ４０９〜Ｓ４１２の処理が、ＧＰＳ受信装置１０からのＧＰＳ測位データがある毎に繰り返されることにより、上記誤差が随時修正されるため、より正確な推測航法データが得られることになる。
【００８１】
そして、上記Ｓ４０１→Ｓ４０２→Ｓ４０３〜Ｓ４０８→Ｓ４０９〜Ｓ４１２の処理が、ＧＰＳ受信装置１０からのＧＰＳ測位データがある毎に繰り返されることにより、上記誤差が随時修正されるため、より正確な推測航法データが得られることになる。
【００８２】
なお、初期位置算出ができた時点では、続くＳ４０２の判定は殆どの場合ＹＥＳ（Ｓ４０３に進む）となるが、初期位置算出が一度できてしまえば、その後車両用現在位置検出装置の起動中は、常にＳ４０１からＳ４０２に進むので、例えば、初期位置算出が一度できた後に、例えば車両がビルの陰やトンネルに入ることで、それまで受信できていたＧＰＳ受信装置からの信号を受信できなると、Ｓ４０２で受信できている衛星数がＴＨｓｙ未満であると判定し、続いてＳ４１３に進む。
【００８３】
Ｓ４１３では、受信できている衛星数がＴＨｓｙ未満である状態が長時間（具体的にはＴ１秒以上）継続しているか否かを判定し、長時間継続していないと判定した場合は、そのまま今回の図６の処理を終了するが、長時間継続していると判定した場合は続いてＳ４１４で、プロセス行列φｋの計算および誤差共分散Ｐｋの予測計算を行う。
【００８４】
これは、ＧＰＳ受信装置１０から十分な数のＧＰＳ測位データが得られない場合、何も補正を行わないと誤差が大きくなってしまうため、誤差共分散Ｐの予測計算、即ち誤差の見積もりだけは定期的に行っておくことにより、その後にＧＰＳ受信装置１０のＧＰＳ測位データの入力が再開された時に行われるカルマンフィルタの処理を正確に行えるようにするためのものである。
【００８５】
また、Ｓ４０７における観測量の検定処理において、ＧＰＳ衛星が使用対象から１つでも排除された場合、Ｓ４０８で、排除衛星があると判定し、続いてＳ４１５に進み、排除後の使用対象のＧＰＳ衛星の数がＴＨｓｙ未満であるか否かを判定し、ＴＨｓｙ未満であると判定した場合は、衛星数が不十分であるので、上述のＳ４１３に進む。
【００８６】
ＴＨｓｙ未満でないと判定した場合は、Ｓ４０５に処理を戻す。これにより、排除したことで数が減った使用対象のＧＰＳ衛星を用いて、観測量Ｙｋを再計算する。その際、観測量Ｙの数が減った分、Ｙｋの要素数も２ｓ個減る（ただしｓは除外されたＧＰＳ衛星の数）。つまり、使用対象から排除したＧＰＳ衛星についての擬似距離およびドップラー周波数の成分がＹｋから除去される。更に、Ｓ４０６で再度残差の共分散Ｐｋを算出するが、その際に用いる観測行列Ｈｋは、その時点における使用対象のＧＰＳ衛星についてのみの成分を有するように作成されるので、行数が２ｓ個減ることになる。
【００８７】
このように、各衛星の観測量を直接利用可能なタイトカップリング方式カルマンフィルタを用いることで、残差の共分散Ｓｋを用いて精度が劣化したＧＰＳ衛星を特定することができ、更に、観測量Ｙｋの各成分が、特定の１つの衛星についての量（擬似距離、ドップラー周波数）となっているので、観測量Ｙｋから精度が劣化したＧＰＳ衛星の成分を選んで除外した上でカルマンフィルタの計算を継続することができるので、精度の高い補正処理を行うことができる。
【００８８】
つまり、ＧＰＳ精度は非連続に変化するが、推測航法軌跡精度は連続的に変化する特徴に着目し、各衛星の観測量を直接利用可能なタイトカップリング方式カルマンフィルタとその観測量（ＧＰＳ観測結果とセンサ推定結果の残差）の推定観測誤差共分散で検定することにより、ＧＰＳ測位精度劣化を正確に推定可能とする。これにより、精度劣化したＧＰＳ衛星を排除することで精度よくセンサ補正することが可能となる。
【００８９】
また、従来のルースカップリング型カルマンフィルタでは、車両位置を観測量として利用するので、ＧＰＳ測位に４衛星以上（３次元測位の場合）または３衛星以上（２次元測位の場合）の受信が必要であったが、本実施形態のようにタイトカップリング型カルマンフィルタを用いた場合は、擬似距離およびドップラー周波数という、最悪１衛星だけ受信できる場合でも得ることができる量を観測量としているので、ＧＰＳ衛星を１つでも受信できれば、上記の処理を継続し、自立センサ２１、２２の補正をすることが可能となる。
【００９０】
以上説明した通り、実施形態の車両用軌跡推定装置は、車両に搭載される自立センサ２０の出力値に基づいて推測航法により当該車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて当該車両の位置および方位を補正し、当該カルマンフィルタは、観測量として、使用対象の複数のＧＰＳ衛星毎に、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、当該車両用軌跡推定装置は、当該タイトカップリング型カルマンフィルタにおける当該使用対象の複数のＧＰＳ衛星毎の当該乖離量のいずれかが大きくなるほど大きくなる指標と所定の閾値とを比較し（Ｓ４５１）、当該指標が当該所定の閾値以上である場合、当該使用対象の複数のＧＰＳ衛星毎の当該乖離量に基づいて、当該使用対象の複数のＧＰＳ衛星のうち一部を選び、選んだ当該一部を使用対象から除外する（Ｓ４５２）。
【００９１】
このように、カルマンフィルタをタイトカップリング型とすることで、カルマンフィルタを実行する際に、精度が劣化したＧＰＳ測位結果の検出および排除を行い、ＧＰＳ精度劣化による推定車両軌跡の影響を軽減することで、ローコストなセンサでも精度よい軌跡を得ることができる。
【００９２】
なお、上記実施形態における構成と特許請求の範囲の構成との対応関係について説明すると、ジャイロスコープ２２が角速度センサに相当し、車速センサ２１が速度センサに相当し、図４のＳ１００〜Ｓ３００が走行軌跡推定手段に相当し、図４のＳ４００、図７のＳ４０２〜Ｓ４１２、図８のＳ５００〜Ｓ５０６が誤差推定手段に相当し、Ｓ５０２が取り付け角ゲイン誤差推定手段に相当し、Ｓ５０４が判定手段に相当し、Ｓ５０６が角速度ゲイン誤差推定手段に相当し、Ｓ４５１が比較手段に相当し、Ｓ４５２が除外手段に相当する。
（他の実施形態）
以上、本発明の実施形態について説明したが、本発明の範囲は、上記実施形態のみに限定されるものではなく、本発明の各発明特定事項の機能を実現し得る種々の形態を包含するものである。
【００９３】
例えば、上記実施形態では、推定する状態量ｘｋとして、絶対位置東方向誤差εＸｋ、絶対位置北方向誤差εＹｋ、絶対位置高度方向誤差εＺｋ、垂直方向速度誤差εＵｋ、絶対水平方位誤差εθｋ、車速係数誤差εＯｋ、ジャイロバイアス誤差εＧｂｋ、ジャイロゲイン誤差εＧｓｋ、クロックバイアス誤差εＣｂｋ、クロックドリフト誤差εＣｄｋ、という１０個の誤差値を採用しているが、必ずしもこのようになっておらずともよく、推定する状態量ｘｋは、自立航法用の自立センサの出力値に影響する誤差の組であれば、どのようなものであってもよい。例えば、推定する状態量ｘｋとして、絶対位置東方向誤差εＸｋ、絶対位置北方向誤差εＹｋ、絶対水平方位誤差εθｋ、車速係数誤差εＯｋ、ジャイロバイアス誤差εＧｂｋ、ジャイロゲイン誤差εＧｓｋという６個の誤差値を採用してもよい。
【００９４】
また、上記実施形態において、Ｓ４０７の検定処理およびＳ４０８の判定処理は、Ｓ４０６の次に行うようになっているが、かならずしもこのタイミングで行わなくてもよい。例えば、Ｓ４０７、Ｓ４０８の処理は、Ｓ４０９の後に行われてもよいし、Ｓ４１１の後に行われてもよい。また、図６の処理を実行する度にＳ４０７、Ｓ４０８を実行する必要はなく、図６の処理を複数回（たとえば１０回）実行するうちの９回は、Ｓ４０６に続いてＳ４０９を実行し、１回のみ、Ｓ４０６に続いてＳ４０７、Ｓ４０８を実行するようになっていてもよい。
【００９５】
また、上記実施形態では、タイトカップリング型カルマンフィルタで用いる観測量として、使用対象の複数のＧＰＳ衛星１００のそれぞれについて、当該ＧＰＳ衛星の観測擬似距離と、当該自立センサ２０の出力値に基づいて推測航法によって算出される車両から当該ＧＰＳ衛星までの推測擬似距離との残差、および、ＧＰＳ受信装置１０から出力される当該ＧＰＳ衛星の観測ドップラー周波数と、自立センサ２０の出力値に基づいて推測航法によって算出される当該ＧＰＳ衛星の推測ドップラー周波数との残差を採用している。
【００９６】
しかし、タイトカップリング型カルマンフィルタで用いる観測量としては、必ずしもこのようなものに限らず、例えば、観測擬似距離と観測ドップラー周波数の関数ｆ１（ＰＲ（ｎ）ＧＰＳ，ＤＰ（ｎ）ＧＰＳ）と、同じ関数型の観測擬似距離と観測ドップラー周波数の関数ｆ１（ＰＲ（ｎ）ＤＲ，ＤＰ（ｎ）ＤＲ）との残差を採用してもよい。
【００９７】
つまり、タイトカップリング型カルマンフィルタで用いる観測量としては、使用対象の複数のＧＰＳ衛星１００の毎の、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値と推測航法で算出した値の乖離量（例えば差の自乗、差の絶対値）を採用するようになっていればよい。
【００９８】
したがって、図７のＳ４５１で閾値ＴＨと比較する量は、必ずしも上記のようなカイ自乗値χ２に限らず、使用対象の複数のＧＰＳ衛星１００毎の上記乖離量のいずれかが大きくなるほど大きくなる指標であれば、どのようなものであってもよい。
【００９９】
また、上記の実施形態において、制御回路１７がプログラムを実行することで実現している各機能は、それらの機能を有するハードウェア（例えば回路構成をプログラムすることが可能なＦＰＧＡ）を用いて実現するようになっていてもよい。
【符号の説明】
【０１００】
１０ ＧＰＳ受信装置
１１ ＧＰＳアンテナ部
１２ ＧＰＳ−ＲＦ部
１３ ＧＰＳ信号処理部
１４ ＧＰＳ測位演算部
２０ 自立センサ
２１ 車速センサ
２２ ジャイロスコープ
３０ 地図データ記憶装置
４０ ディスプレイ部
５０ 制御部
１００ ＧＰＳ衛星
【技術分野】
【０００１】
本発明は、車両用軌跡推定装置に関するものである。
【背景技術】
【０００２】
従来、ＧＰＳ受信装置が算出する現在位置情報をカルマンフィルタの観測量に用いることで、自立センサ（車速センサ、ジャイロスコープ等）の誤差を推定し、推定したセンサ誤差を補正する技術が知られている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０００−５５６７８号公報
【特許文献２】特開２００８−２３２７６１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、ＧＰＳ受信装置による測位結果は、マルチパス波の受信やＧＰＳ衛星配置悪化等の要因により測位精度が大きく変動する。精度が劣化したＧＰＳ測位結果を使用してカルマンフィルタを実行するとセンサ誤差の推定を誤り、走行軌跡の推定精度が悪化してしまうという問題がある。
【０００５】
本発明はこの問題点に鑑み、ＧＰＳ受信装置がＧＰＳ衛星からの信号に基づいて算出する情報をカルマンフィルタの観測量に用いることで、自立センサの誤差を推定し、推定したセンサ誤差を補正する技術において、カルマンフィルタで用いるＧＰＳ測位結果の精度の劣化を全体として抑えることを目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための請求項１に記載の発明は、車両に搭載される自立センサ（２０）の出力値に基づいて推測航法により前記車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて前記車両の位置および方位を補正する車両用軌跡推定装置であって、前記カルマンフィルタは、観測量として、使用対象の複数のＧＰＳ衛星毎に、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、当該車両用軌跡推定装置は、前記タイトカップリング型カルマンフィルタにおける前記使用対象の複数のＧＰＳ衛星毎の前記乖離量のいずれかが大きくなるほど大きくなる指標と所定の閾値とを比較する比較手段（Ｓ４５１）と、前記指標が前記所定の閾値以上であると前記比較手段（Ｓ４５１）が判定した場合、前記使用対象の複数のＧＰＳ衛星毎の前記乖離量に基づいて、前記使用対象の複数のＧＰＳ衛星のうち一部を選び、選んだ前記一部を使用対象から除外する除外手段（Ｓ４５２）と、を備えた車両用軌跡推定装置である。
【０００７】
このように、カルマンフィルタをタイトカップリング型とすることで、カルマンフィルタを実行する際に、精度が劣化したＧＰＳ測位結果の検出および排除を行い、ＧＰＳ精度劣化による推定車両軌跡の影響を軽減することで、精度よく車両の位置および方位を補正することができる。 なお、上記および特許請求の範囲における括弧内の符号は、特許請求の範囲に記載された用語と後述の実施形態に記載される当該用語を例示する具体物等との対応関係を示すものである。
【図面の簡単な説明】
【０００８】
【図１】本発明の一実施形態に係る車両用現在位置検出装置の構成を示す図である。
【図２】信号生発生過程、観測過程、カルマンフィルタの模式図である。
【図３】制御部による推測航法処理のフローチャートである。
【図４】ジャイロスコープのオフセット補正について説明するための図である。
【図５】ジャイロスコープのゲイン補正について説明するための図である。
【図６】制御部によるＧＰＳ複合化処理のフローチャートである。
【図７】制御部による観測量の検定処理のフローチャートである。
【図８】有意水準５％の閾値を示す図表である。
【発明を実施するための形態】
【０００９】
以下、本発明の一実施形態について説明する。図１に、本実施形態に係る車両用現在位置検出装置の構成を示す。本実施形態に係る車両用現在位置検出装置は車両用ナビゲーション装置として構成されており、ＧＰＳ受信装置１０、自立センサ２０、地図データ記憶装置３０、ディスプレイ部４０、および制御部５０を備えている。
【００１０】
ＧＰＳ受信装置１０は、複数のＧＰＳ衛星１００（図中では、１つのみ示す）からの電波を受信するＧＰＳアンテナ部１１、ＧＰＳアンテナ部１１から入力される高周波信号を中間周波数帯の受信信号に変換するＧＰＳ―ＲＦ部１２、ＧＰＳ―ＲＦ部１２から入力される受信信号の同期捕捉や同期保持等の信号処理を行うＧＰＳ信号処理部１３、ＧＰＳ信号処理部１３から出力されるデータに基づいてＧＰＳ測位演算を行い、このＧＰＳ測位演算により求めた測位データ、複数のＧＰＳ衛星１００のそれぞれの絶対位置、複数のＧＰＳ衛星１００のそれぞれの擬似距離およびドップラー周波数等を出力するＧＰＳ測位演算部１４を備えている。
【００１１】
ＧＰＳ受信装置１０は、ＧＰＳ衛星１００からの電波を受信し、車両の絶対的な現在位置を表すＧＰＳ位置、車両の進行方向を特定するための速度ベクトル等を算出し、算出結果を測位データとして出力する。
【００１２】
自立センサ２０は、車両の走行速度に応じた車速パルス信号を出力する速度センサとしての車速センサ２１と、車両の回転角速度に応じた角速度信号を出力する角速度センサとしてのジャイロスコープ２２を備えている。
【００１３】
地図データ記憶装置３０は、地図データが記憶された地図データ記憶媒体から地図データを読み出すための装置である。地図データには、各リンクの距離、道路種別、車線数、一方通行規制、信号機の位置等を表す道路データ、位置検出精度を向上するためのいわゆるマップマッチングデータ、川、湖、海、鉄道、施設などの位置、形状、名称を表す背景データ、各地の施設の名称、所在位置、施設種類等を示す施設データ等が含まれる。なお、地図データ記憶媒体としては、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、ハードディスクドライブ、メモリカード等がある。
【００１４】
ディスプレイ部４０は、液晶等の表示部を備え、コンピュータから入力される映像信号に応じた映像を表示部に表示させる。
【００１５】
制御部５０は、ＣＰＵ、ＲＯＭ、ＲＡＭ、不揮発性メモリ、Ｉ／Ｏ等を有するコンピュータにより構成されており、ＣＰＵは、ＲＯＭに記憶されたプログラムに従って各種処理を実施する。
【００１６】
制御部５０の処理としては、ジャイロスコープ２２の出力値に基づいて算出される方位変化量と、速度センサ２１の出力値に基づいて算出される車速係数から、車両の相対軌跡を推定するとともに当該車両の相対軌跡とＧＰＳ受信装置１０からの出力値とに基づいて車両の走行軌跡を推定する走行軌跡推定処理、ジャイロスコープ２２の出力値およびＧＰＳ受信装置１０からの出力値に基づいて方位変化量のオフセット誤差、方位変化量のゲイン誤差、絶対方位誤差等を推定する誤差推定処理、自車の走行軌跡と地図メモリ７０に格納されている地図情報の道路データとを照合することによって、自車位置（ＧＰＳ位置により補正された推測位置）を道路上の位置に修正するマップマッチング処理等がある。なお、走行軌跡推定処理では、誤差推定処理により推定された各誤差を用いて走行軌跡推定処理による車両の走行軌跡の推定に用いられる方位変化量のオフセット、方位変化量のゲインおよび絶対方位等を補正するようになっている。
【００１７】
制御部５０は、誤差推定処理においてカルマンフィルタを用いて推測航法における各誤差要因を１つの状態量ｘとしてモデル化し、各誤差の推定値を求める誤差推定処理を実施する。
【００１８】
ここで、カルマンフィルタの概要について説明する。カルマンフィルタは、図２および式（１）、（２）に示すような信号発生過程および観測過程を前提としている。
【００１９】
【数１】
すなわち、プロセス行列φｋで定義される線形システム（信号発生過程）があり、そのシステムにおいて状態量Ｘｋに対して、観測行列Ｈｋで関係付けられた状態量Ｘｋの一部が観測可能（観測過程）である場合、カルマンフィルタは、この観測量Ｙｋを入力として、状態量Ｘｋの最適な推定値ｘｋを与えるものである。なお、ｒｋは信号発生過程にて発生する雑音であり、ｑｋは観測過程において発生する雑音である。
【００２０】
そして、時刻ｋまでの情報を用いた状態量Ｘｋの最適推定値ｘｋ＝ｘ（ｋ｜ｋ）は、次の式（３）〜（８）により求めることができる。
【００２１】
【数２】
ここで、Ｋｋはカルマンゲイン、Ｐｋは推定値ｘｋの共分散（以下では誤差共分散という）、Ｑｋは観測過程で発生する観測雑音ｑｋの分散、Ｒｋは信号発生過程で発生するシステム雑音ｒｋの分散である。またＳｋは、式（５）で定義される残差の共分散である。また各量（仮にＡｋとする）の右上に−（マイナス記号）の上付き添字が付された量Ａｋ−は、時刻ｋ−１までの情報に基づく時刻ｋでの量Ａｋの推定値（すなわちＡ（ｋ｜ｋ−１））である。また、雑音ｒｋ、ｑｋはいずれも平均０の白色ガウス雑音であり互いに無相関である。また、Ｉは単位行列であり、上付き添字Ｔは転置を意味し、上付き添字−１は逆行列を意味する。
【００２２】
このように定義されたカルマンフィルタでは、状態量の推定値ｘｋを定義すると共に、誤差共分散Ｐｋに適当な初期値を設定するとにより、状態量の最適推定値ｘｋが得られ、観測が行われ観測量Ｙｋが得られる毎にこれを繰り返すことにより、状態量の最適推定値ｘｋの精度が向上する。
【００２３】
そして、本実施例では、状態量Ｘｋとして、東方向の絶対位置を算出する際に付与される絶対位置東方向誤差εＸｋ、北方向の絶対位置を算出する際に付与される絶対位置北方向誤差εＹｋ、高度方向の絶対位置を算出する際に付与される絶対位置高度方向誤差εＺｋ、垂直方向の速度を算出する際に付与される垂直方向速度誤差εＵｋ、絶対水平方位を算出する際に付与される絶対水平方位誤差εθｋ、車速を算出する際に、車速センサ２１から出力された単位時間当たりのパルス数に乗じられる車速係数の誤差である車速係数誤差εＯｋ、ジャイロ出力の大きさによらず、出力値の全領域に均等に付加されるジャイロバイアス誤差εＧｂｋ、ジャイロ出力から角速度への変換比率として設定される変換ゲインの誤差であるジャイロゲイン誤差εＧｓｋ、クロックバイアス誤差εＣｂｋ、クロックドリフト誤差εＣｄｋ、という１０個の誤差値を採用し、これら誤差値を次の式（９）〜（１８）により定義する。
【００２４】
【数３】
ただし、Ｔは前回（の観測）からの経過時間、ＤＴは前回からの方位変化量、ＬＴは前回からの移動距離、θＴは真の絶対方位である。
【００２５】
そして、これら誤差値の時間的な変化を規定するものがプロセス行列φｋであり、上記式（９）〜（１８）を各誤差値にて偏微分し線形化することにより求められ、その結果、プロセス行列φｋは式（１９）のようになる。
【００２６】
【数４】
ただし、θｋ＝θＴ＋εθｋ−１＋Ｔ×εＧｋ−１／２＋ＤＴ×εＧｓｋ−１／２
であり、このθｋは絶対方位を意味しており、真の絶対方位θＴにセンサ誤差に基づく誤差が加わったものとなっている。また、Ｇｓは車速係数であり、Ｄは前回からの方位変化量であり、Ｌは前回からの移動距離である。
【００２７】
また、観測量Ｙｋとしては、各々のＧＰＳ衛星の観測擬似距離と推測擬似距離との残差、および、各々のＧＰＳ衛星の観測ドップラー周波数と推測ドップラー周波数との残差を採用する。
【００２８】
ここで、各ＧＰＳ衛星の観測擬似距離および観測ドップラー周波数は、ＧＰＳ受信装置１０によって計測され、ＧＰＳ受信装置１０から制御部５０に随時出力されているものを用いる。また、各ＧＰＳ衛星の推測擬似距離および推測ドップラー周波数は、ＧＰＳ受信装置１０から出力される各ＧＰＳ衛星の現在位置、後述するＳ１００で推測航法によって算出される自車両の移動距離、後述するＳ３００で推測航法によって算出される自車両の絶対方位、絶対位置、および車速等に基づいて算出する。
【００２９】
このように、擬似距離、ドップラー周波数等の、ＧＰＳ衛星毎に算出される量を観測量とするカルマンフィルタを、タイトカップリング型カルマンフィルタという。一方、特許文献１に記載のような観測量を用いるカルマンフィルタをルーズカップリング型カルマンフィルタいう。タイトカップリング型カルマンフィルタも、周知の技術であり、例えば、特許文献２に記載されている。
【００３０】
ルーズカップリング型カルマンフィルタは、ＧＰＳ測位結果（位置、方位、速度）を観測量として利用するが、本実施形態のようなタイトカップリング型カルマンフィルタは、各ＧＰＳ衛星の観測疑似距離とドップラー周波数を観測量として利用する。つまり、タイトカップリング型カルマンフィルタは、ＧＰＳ各衛星の観測疑似距離とドップラー周波数とセンサで推定した観測疑似距離と観測ドップラー周波数との差分を、誤差共分散行列に従い、最小にする状態量を推定する。
【００３１】
ここで、ｎ番目のＧＰＳ衛星の擬似距離ＰＲ（ｎ）は、式（２０）のように定義される。
【００３２】
【数５】
ただし、ＳＶｎｘ、ＳＶｎｙ、ＳＶｎｚは、それぞれｎ番目のＧＰＳ衛星の位置座標の成分であり、ｘｐｒｅｄｉｃｔｉｏｎ、ｙｐｒｅｄｉｃｔｉｏｎ、ｚｐｒｅｄｉｃｔｉｏｎは、ＧＰＳ受信装置１０の位置座標であり、Ｃｌｋｂｉａｓは、クロックバイアス誤差εＣｂｋである。なお、式（２０）のｘ、ｙ、ｚの向きは、観測量のεＸ、εＹ、εＺと同じである。
【００３３】
また、ｎ番目のＧＰＳ衛星の観測ドップラー周波数（積算ドップラー）ＤＰ（ｎ）は、式（２１）のように定義される。
【００３４】
【数６】
ただし、ＳＶの上の黒丸は、時間微分を表し、ＲｎｐｒｅｄｉｃｔｉｏｎはＧＰＳ受信装置１０からｎ番目のＧＰＳ衛星までの距離を示し、Ｃｌｋｄｒｉｆｔはクロックドリフト誤差εＣｄｋを表す。
【００３５】
これらの定義式（２０）、（２１）を各状態量εＸｋ、εＹｋ、εＺｋ、εＵｋ、εθｋ、εＯｋ、εＧｂｋ、εＧｓｋ、εＣｂｋ、εＣｄｋにて偏微分し線形化することにより、観測行列Ｈｋが式（２２）のように得られる。
【００３６】
【数７】
ここで、各偏微分は、式（２３）〜（３０）のように表される。
【００３７】
【数８】
【００３８】
【数９】
なお、式（２２）では、２個のＧＰＳ衛星を用いる場合の観測行列Ｈｋの例であり、ｍ個のＧＰＳ衛星を用いる場合の観測行列Ｈｋは、２ｎ行１０列の行列となる。
【００３９】
このように、ＧＰＳ毎に別々に定義された観測量を用いるタイトカップリング型カルマンフィルタを用いる制御部５０は、車速センサ２１からの信号に基づいて車両の移動距離を算出すると共に、ジャイロスコープ２２からの信号に基づいて車両の方位変化量を算出する。そして、これら算出された移動距離及び方位変化量に基づいて、推測航法データとして、車両の速度、相対軌跡、絶対位置、絶対方位を算出する。一方、ＧＰＳ受信装置１０は、ＧＰＳ測位データとして、車両の位置、方位、車速を随時出力し、また、各衛星の位置座標、各衛星までの観測擬似距離、および各衛星の観測ドップラー周波数を随時出力している。
【００４０】
そして、制御部５０は、タイトカップリング型カルマンフィルタを用いて、推測航法データおよびＧＰＳ測位データ、各衛星の位置座標、観測擬似距離、および観測ドップラー周波数に基づいて、各誤差εＸｋ、εＹｋ、εＺｋ、εＵｋ、εθｋ、εＯｋ、εＧｂｋ、εＧｓｋ、εＣｂｋ、εＣｄｋの推定値を算出し、更に、この推定値に基づいて、車両の方位変化量、移動距離、相対軌跡、絶対位置等の算出および補正を行う。
【００４１】
以下、制御部５０による推測航法処理について説明する。図３に、推測航法処理のフローチャートを示す。本車両用現在位置検出装置が動作状態になると、制御部５０は、図４に示す処理を一定周期ＴＭ毎に繰り返し実行する。
【００４２】
まずＳ１００では、方位変化量・移動距離演算処理を実行する。この処理では、まず、ジャイロスコープ２２により検出され出力されたジャイロ出力角速度にメインルーチンの起動周期ＴＭを乗じることにより車両の向きの方位変化量を算出する。
方位変化量＝ジャイロ出力角速度×ＴＭ （３１）
なお、ジャイロ出力角速度は、図４のグラフから明らかなように、ジャイロスコープ２２の出力をサンプリングした値から、オフセット値(図４では２．５Ｖ) を減じたものに、変換ゲイン(グラフの傾きに相当する) を乗じることにより算出されたものである。
【００４３】
次に、後述するオフセット補正量に一定周期ＴＭを乗じたものを、式（３１）により求めた方位変化量を減じることにより、方位変化量のオフセット補正を行う。
【００４４】
方位変化量＝方位変化量−オフセット補正量×ＴＭ （３２）
すなわち、図４に示すように、角速度０［ｄｅｇ／ｓｅｃ] に対応するジャイロスコープ２２の出力電圧(ゼロ点) の変動に基づく誤差を補正する。次に、オフセット補正された方位変化量に、後述するゲイン補正量を減じることにより、方位変化量のゲイン補正を行う。
【００４５】
方位変化量＝方位変化量×ゲイン補正量 （３３）
すなわち、図５に示すように、グラフの傾き(変換ゲイン) の変動に基づく誤差を補正する。
【００４６】
そして、一定周期ＴＭの間に検出された車速センサ２１からの出力パルス数に、後述する車速係数を乗じることにより移動距離を算出する。
【００４７】
移動距離＝車速センサパルス数×車速係数 （３４）
このように、移動距離を算出すると、方位変化量・移動距離の演算処理を終了する。
【００４８】
次のＳ２００の相対軌跡の演算処理では、まず、式（３３）を用いて算出した方位変化量を、それまでに求められている相対方位に加算することにより、車両の向きの相対方位を更新する。
【００４９】
相対方位＝相対方位＋方位変化量 （３５）
次に、この更新した相対方位および式（３４）を用いて算出した移動距離に基づき、相対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした相対座標ｒｅｌ．ｘ、東西方向をｙ座標軸とした相対座標ｒｅｌ．ｙを、式（３６）、（３７）に従って更新する。ただし、θｓは式（３５）を用いて算出した相対方位である。
【００５０】
ｒｅｌ．ｘ＝ｒｅｌ．ｘ＋移動距離×ｃｏｓ（θｓ） （３６）
ｒｅｌ．ｙ＝ｒｅｌ．ｙ＋移動距離×ｓｉｎ（θｓ） （３７）
すなわち、この更新は、移動距離に対する相対方位のＸ、Ｙ成分をそれまでの相対位置座標に加算することにより行う。この相対位置座標は相対軌跡を求めるために算出するもので、その相対軌跡と道路形状との関係により、いわゆるマップマッチングが行われる。
【００５１】
次に、式（３４）を用いて算出した移動距離を、周期ＴＭで除することにより車速を算出する。
【００５２】
車速＝移動距離／ＴＭ （３８）
このように、車速を算出すると、相対軌跡演算処理を終了する。
【００５３】
次のＳ３００の絶対方位・絶対位置の演算処理では、まず、式（３４）を用いて算出した方位変化量を、それまでに求められている車両の向きの絶対方位に加算することにより、絶対方位を更新する。
【００５４】
絶対方位＝絶対方位＋方位変化量 （３９）
次に、この更新した絶対方位および式（３４）を用いて算出した移動距離に基づき、絶対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした絶対座標ａｂｓ．ｘ、東西方向をｙ座標軸とした絶対座標ａｂｓ．ｙを、式（４０）、（４１）に従って更新する。ただし、θは式（３９）を用いて算出した絶対方位である。
【００５５】
ａｂｓ．ｘ＝ａｂｓ．ｘ＋移動距離×ｃｏｓ（θ） （４０）
ａｂｓ．ｙ＝ａｂｓ．ｙ＋移動距離×ｓｉｎ（θ） （４１）
すなわち、この更新は、移動距離に対する絶対方位のＸ、Ｙ成分をそれまでの絶対位置座標に加算することにより行う。このようにして更新した車両の絶対方位と絶対位置は後述するＧＰＳとの複合化処理にて利用される。
【００５６】
次に、Ｓ４００のＧＰＳ複合化処理を実施する。このＧＰＳ複合化処理のフローチャートを図７に示す。ここでは、このＧＰＳ複合化処理の実行回数がｋ回目であるとして説明する。
【００５７】
このＧＰＳ複合化処理では、まずＳ４０１で、車両用現在位置検出装置の起動後、ＧＰＳ受信装置１０が周知の初期位置算出（ｆｉｒｓｔ ｆｉｘ）を行ったか否か判定し、行っていなければ直ちにＧＰＳ復号化処理を終了し、行っていれば続いてＳ４０２に進む。なお、ＧＰＳ受信装置１０は、周知の初期位置算出を行ったとき、測位データを制御部５０に出力するので、測位データを制御部５０は、車両用現在位置検出装置の起動後、一度以上測位データを受け取っていれば、初期位置算出を行ったと判定する。
【００５８】
Ｓ４０２では、ＧＰＳ受信装置１０が信号を受信できている衛星数が所定の衛星数ＴＨｓｙ以上であるか否かを判定し、衛星数ＴＨｓｙ未満であれば直ちにＧＰＳ復号化処理を終了し、衛星数ＴＨｓｙ以上であればＳ４０３に進む。なお、ＧＰＳ受信装置１０は信号を受信できている衛星数の情報を随時制御部５０に出力するようになっているので、Ｓ４０２では、この情報に基づいて判定を行えばよい。この閾値ＴＨｓｙは、１でもよいし、２でもよいし、３以上であってもよい。
【００５９】
続いてＳ４０３では、状態量の推定を行う。具体的には、式（３）を用いて、状態量の推定値ｘｋ―を算出する。続いてＳ４０４では、誤差共分散の推定を行う。具体的には、式（４）を用いて、誤差共分散の推定値Ｐｋ＋１−を算出する。ここで、Ｑｋは、観測疑似距離、観測ドップラーの観測誤差が設定される。
【００６０】
続いてＳ４０５では、観測量Ｙｋを取得する。具体的には、観測量Ｙｋは、上述の通り、ｎ番目のＧＰＳ衛星の観測擬似距離および推測擬似距離をそれぞれＰＲ（ｎ）ＧＰＳ、ＰＲ（ｎ）ＤＲとし、ｎ番目のＧＰＳ衛星の観測ドップラー周波数および推測ドップラー周波数をＤＰ（ｎ）ＧＰＳ、ＤＰ（ｎ）ＤＲとすると観測量Ｙｋは、以下の式（４２）のようなベクトル量となる。
【００６１】
【数１０】
ただし、式（４２）では、ＧＰＳ受信装置１０が信号を受信できているＧＰＳ衛星のうち、使用対象のＧＰＳ衛星としてｍ個が選択されている。使用対象のＧＰＳ衛星のリストは、制御部５０のＲＡＭ等に記録されている。なお、Ｓ４０２の直後においては、初期設定として、現在信号を受信できているＧＰＳ衛星すべてを、使用対象のＧＰＳ衛星とする。
【００６２】
なお、観測擬似距離ＰＲ（ｎ）ＧＰＳおよび観測ドップラー周波数ＤＰ（ｎ）ＧＰＳとしては、上述の通り、ＧＰＳ受信装置１０から出力されたｎ番目の衛星の擬似距離およびドップラー周波数を用いる。
【００６３】
また、推測擬似距離ＤＰ（ｎ）ＤＲについては、式（２０）を用いてＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標をＳＶｎｘ、ＳＶｎｙ、ＳＶｎｚに代入し、直前のＳ３００で算出した推測航法による車両の位置座標をｘｐｒｅｄｉｃｔｉｏｎ、ｙｐｒｅｄｉｃｔｉｏｎ、ｚｐｒｅｄｉｃｔｉｏｎに代入し、前回の図６のＳ４１０において算出したクロックバイアス誤差εＣｂｋをＣｌｋｂｉａｓに代入することで、算出する。
【００６４】
また、推測ドップラー周波数ＤＰ（ｎ）ＤＲについては、式（２１）を用いてＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標をＳＶｎｘ、ＳＶｎｙ、ＳＶｎｚに代入し、ＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標の時間微分値をＳＶｎｘ、ＳＶｎｙ、ＳＶｎｚの微分値に代入し、直前のＳ３００で算出した推測航法による車両の位置座標をｘｐｒｅｄｉｃｔｉｏｎ、ｙｐｒｅｄｉｃｔｉｏｎ、ｚｐｒｅｄｉｃｔｉｏｎに代入し、直前のＳ３００で算出した推測航法による車両の位置座標とＧＰＳ受信装置１０から受信したｎ番目のＧＰＳ衛星の現在位置座標とに基づいて車両から当該ＧＰＳ衛星までの距離を算出し、その算出結果をＲｐｒｅｄｉｃｔｉｏｎに代入し、＊＊＊＊をＶＵに代入し、＊＊＊＊をνに代入し、＊＊＊＊をδＳＦｏｄｏに代入し、＊＊＊＊ψに代入し、前回の図６のＳ４１０において算出したクロックドリフト誤差εＣｄｋをＣｌｋｄｒｉｆｔに代入することで、算出する。
【００６５】
続いてＳ４０６では、式（５）を用いて残差の共分散Ｓｋの導出を行う。ここで、Ｒｋは、ＴＭ間に発生する状態量の推定誤差が設定される。また、観測行列Ｈｋとしては、式（２２）を用い、式（２２）中の各偏微分値については、式（２３）〜（３０）に対して、上記の推測ドップラー周波数の算出時と同様の代入を行うことで算出する。
【００６６】
このようになっているので、観測量Ｙｋの各成分のうち、ｎ番目のＧＰＳ衛星に関する成分ＰＲ（ｎ）ＧＰＳ−ＰＲ（ｎ）ＤＲおよびＤＰ（ｎ）ＧＰＳ−ＤＰ（ｎ）ＤＲは、使用対象のＧＰＳ衛星のうち、当該ｎ番目のＧＰＳ衛星のみの軌道（位置および移動速度）に依存する量である。
【００６７】
続いてＳ４０７では、観測量の検定処理を行う。図７に、この観測量の検定処理のフローチャートを示す。この観測量の検定処理において制御部５０は、まずＳ４５０で、カイ自乗値の計算を行う。カイ自乗値χ２は以下の式（４３）で定義されている。
【００６８】
【数１１】
この定義式は、以下の式（４４）〜（４６）のように変形できる。
【００６９】
【数１２】
ここで、Ｒ−１は、誤差σｉσｉで表される誤差共分散行列であり、［ｚｉ−ｈ（ａｉ，ｘ）］は残差を示す。これを本実施形態のタイトカップリング型カルマンフィルタに適用すると、カイ自乗値は以下の式（４７）のようになる。
【００７０】
【数１３】
このカイ自乗値χ２は、現在の使用対象のＧＰＳ衛星のそれぞれについて、観測擬似距離と推測擬似距離の乖離が大きくなるほど大きくなる量であり、また、観測ドップラー周波数と推測ドップラー周波数の乖離が大きくなるほど大きくなる量である。
【００７１】
続いてＳ４５１では、算出したカイ自乗値が閾値ＴＨ以上であるか否かを判定する。閾値としては、例えば、有意水準を５％として、現在使用しているＧＰＳ衛星の数に応じて、図５に示すようなχ自乗値を閾値ＴＨとして採用する。例えば、現在使用しているＧＰＳ衛星が５個であれば、閾値ＴＨとして、１．１４５４７７２９９を採用する。カイ自乗値が閾値ＴＨ以上であると判定すれば、続いてＳ４５２に進み、使用しているＧＰＳ衛星のうち一部を選び、選んだ一部を使用対象から除外する。つまり、使用対象のＧＰＳ衛星のリストから、当該一部のＧＰＳ衛星を削除する。
【００７２】
選んで除外するＧＰＳ衛星としては、直前のＳ４０６で算出した残差の共分散Ｓｋに基づいて決定する。例えば、残差の共分散Ｓｋの対角成分のうち、最も値の大きい成分を特定し、その成分に対応するＧＰＳ衛星を除外対象としてもよい。例えば、残差の共分散Ｓｋの２行２列目の成分の値が最も大きければ、１番目のＧＰＳ衛星を除外対象とする。より一般化して言えば、残差の共分散Ｓｋのｊ行ｊ列目の成分の値が最も大きければ、ｊ／２を下回らない最小の整数ｉ番目のＧＰＳ衛星を除外対象とする。
【００７３】
このように、カルマンゲインの計算前に、精度の悪いＧＰＳ衛星を排除することで、高精度にジャイロスコープ２２の検出値、クロックゲイン誤差、クロックバイアス誤差の補正が可能となる。Ｓ４５２の後は、検定処理を終了して図６のＳ４０８に進む。また、Ｓ４５１でカイ自乗値が閾値ＴＨ以上でないと判定すれば、使用しているＧＰＳ衛星のいずれも使用対象から除外することなく、検定処理を終了してＳ４０８に進む。
【００７４】
Ｓ４０８では、Ｓ４０７で排除した衛星があるか否かを判定し、あればＳ４１５に進み、なければＳ４０９に進む。
【００７５】
Ｓ４０９では、式（６）を用いてカルマンゲインＫｋを算出する。ここで、観測行列Ｈｋとしては、直前に実行したＳ４０６で残差の共分散Ｓｋを算出したときと同じものを用いる。
【００７６】
続いてＳ４１０では、式（７）を用いて、更新された状態量Ｘの推定値ｘｋを算出する。ここで、観測値Ｙｋとしては、直前に実行したＳ４０７の算出結果を用いる。また、カルマンゲインＫｋとしては、直前に実行したＳ４０９の算出結果を用いる。また、観測行列Ｈｋとしては、直前に実行したＳ４０６で残差の共分散Ｓｋを算出したときと同じものを用いる。
【００７７】
続いてＳ４１１では、更新された誤差共分散の推定を行う。具体的には、式（８）を用いて誤差共分散Ｐｋを算出する。ここで、カルマンゲインＫｋとしては、直前に実行したＳ４０９の算出結果を用いる。また、観測行列Ｈｋとしては、直前に実行したＳ４０６で残差の共分散Ｓｋを算出したときと同じものを用いる。
【００７８】
続いてＳ４１２では、Ｓ４１０で算出されたｘｋ、すなわち、状態量εＸｋ、εＹｋ、εＺｋ、εＵｋ、εθｋ、εＯｋ、εＧｂｋ、εＧｓｋに基づき、推測航法誤差の補正（ジャイロ６のオフセット補正、同じくゲイン補正、車速センサ２１の車速係数補正、絶対方位補正、絶対位置補正）を（４８）〜（５７）式に従って行う。
abs.y(東方向絶対位置)＝abs.y−εＹｋ （４８）
abs.x(北方向絶対位置)＝abs.x−εＸｋ （４９）
abs.z(高度方向絶対位置)＝abs.z−εＺｋ （５０）
垂直方向速度＝スイチョウ方向速度−εＵｋ （５１）
絶対水平方位＝絶対水平方位−εθｋ （５２）
車速係数＝車速係数×（１−εＯｋ） （５３）
オフセット補正量＝オフセット補正量−εＧｂｋ （５４）
ゲイン補正量＝ゲイン補正量×（１−εＧｓｋ） （５５）
クロックバイアス補正量＝クロックバイアス補正量−εＣｂｋ （５６）
クロックドリフト補正量＝クロックドリフト補正量−εＣｄｋ （５７）
なお、クロックバイアス補正量、クロックゲイン誤差補正量については、次回の図６の処理において、式（２０）、（２１）を用いて推測擬似距離、推測ドップラー周波数を算出する際に用いる。
【００７９】
例えば、ジャイロスコープ２２のオフセット補正およびゲイン補正により、Ｓ１００にて用いられるオフセット補正量及びゲイン補正量が修正され、車速センサ２１の車速係数補正により、Ｓ１００にて用いられる車速係数が修正され、絶対方位補正により、Ｓ３００にて用いられる絶対方位θが修正され、絶対位置補正によりＳ３００にて用いられる絶対位置が修正される。
【００８０】
そして、上記Ｓ４０１→Ｓ４０２→Ｓ４０３〜Ｓ４０８→Ｓ４０９〜Ｓ４１２の処理が、ＧＰＳ受信装置１０からのＧＰＳ測位データがある毎に繰り返されることにより、上記誤差が随時修正されるため、より正確な推測航法データが得られることになる。
【００８１】
そして、上記Ｓ４０１→Ｓ４０２→Ｓ４０３〜Ｓ４０８→Ｓ４０９〜Ｓ４１２の処理が、ＧＰＳ受信装置１０からのＧＰＳ測位データがある毎に繰り返されることにより、上記誤差が随時修正されるため、より正確な推測航法データが得られることになる。
【００８２】
なお、初期位置算出ができた時点では、続くＳ４０２の判定は殆どの場合ＹＥＳ（Ｓ４０３に進む）となるが、初期位置算出が一度できてしまえば、その後車両用現在位置検出装置の起動中は、常にＳ４０１からＳ４０２に進むので、例えば、初期位置算出が一度できた後に、例えば車両がビルの陰やトンネルに入ることで、それまで受信できていたＧＰＳ受信装置からの信号を受信できなると、Ｓ４０２で受信できている衛星数がＴＨｓｙ未満であると判定し、続いてＳ４１３に進む。
【００８３】
Ｓ４１３では、受信できている衛星数がＴＨｓｙ未満である状態が長時間（具体的にはＴ１秒以上）継続しているか否かを判定し、長時間継続していないと判定した場合は、そのまま今回の図６の処理を終了するが、長時間継続していると判定した場合は続いてＳ４１４で、プロセス行列φｋの計算および誤差共分散Ｐｋの予測計算を行う。
【００８４】
これは、ＧＰＳ受信装置１０から十分な数のＧＰＳ測位データが得られない場合、何も補正を行わないと誤差が大きくなってしまうため、誤差共分散Ｐの予測計算、即ち誤差の見積もりだけは定期的に行っておくことにより、その後にＧＰＳ受信装置１０のＧＰＳ測位データの入力が再開された時に行われるカルマンフィルタの処理を正確に行えるようにするためのものである。
【００８５】
また、Ｓ４０７における観測量の検定処理において、ＧＰＳ衛星が使用対象から１つでも排除された場合、Ｓ４０８で、排除衛星があると判定し、続いてＳ４１５に進み、排除後の使用対象のＧＰＳ衛星の数がＴＨｓｙ未満であるか否かを判定し、ＴＨｓｙ未満であると判定した場合は、衛星数が不十分であるので、上述のＳ４１３に進む。
【００８６】
ＴＨｓｙ未満でないと判定した場合は、Ｓ４０５に処理を戻す。これにより、排除したことで数が減った使用対象のＧＰＳ衛星を用いて、観測量Ｙｋを再計算する。その際、観測量Ｙの数が減った分、Ｙｋの要素数も２ｓ個減る（ただしｓは除外されたＧＰＳ衛星の数）。つまり、使用対象から排除したＧＰＳ衛星についての擬似距離およびドップラー周波数の成分がＹｋから除去される。更に、Ｓ４０６で再度残差の共分散Ｐｋを算出するが、その際に用いる観測行列Ｈｋは、その時点における使用対象のＧＰＳ衛星についてのみの成分を有するように作成されるので、行数が２ｓ個減ることになる。
【００８７】
このように、各衛星の観測量を直接利用可能なタイトカップリング方式カルマンフィルタを用いることで、残差の共分散Ｓｋを用いて精度が劣化したＧＰＳ衛星を特定することができ、更に、観測量Ｙｋの各成分が、特定の１つの衛星についての量（擬似距離、ドップラー周波数）となっているので、観測量Ｙｋから精度が劣化したＧＰＳ衛星の成分を選んで除外した上でカルマンフィルタの計算を継続することができるので、精度の高い補正処理を行うことができる。
【００８８】
つまり、ＧＰＳ精度は非連続に変化するが、推測航法軌跡精度は連続的に変化する特徴に着目し、各衛星の観測量を直接利用可能なタイトカップリング方式カルマンフィルタとその観測量（ＧＰＳ観測結果とセンサ推定結果の残差）の推定観測誤差共分散で検定することにより、ＧＰＳ測位精度劣化を正確に推定可能とする。これにより、精度劣化したＧＰＳ衛星を排除することで精度よくセンサ補正することが可能となる。
【００８９】
また、従来のルースカップリング型カルマンフィルタでは、車両位置を観測量として利用するので、ＧＰＳ測位に４衛星以上（３次元測位の場合）または３衛星以上（２次元測位の場合）の受信が必要であったが、本実施形態のようにタイトカップリング型カルマンフィルタを用いた場合は、擬似距離およびドップラー周波数という、最悪１衛星だけ受信できる場合でも得ることができる量を観測量としているので、ＧＰＳ衛星を１つでも受信できれば、上記の処理を継続し、自立センサ２１、２２の補正をすることが可能となる。
【００９０】
以上説明した通り、実施形態の車両用軌跡推定装置は、車両に搭載される自立センサ２０の出力値に基づいて推測航法により当該車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて当該車両の位置および方位を補正し、当該カルマンフィルタは、観測量として、使用対象の複数のＧＰＳ衛星毎に、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、当該車両用軌跡推定装置は、当該タイトカップリング型カルマンフィルタにおける当該使用対象の複数のＧＰＳ衛星毎の当該乖離量のいずれかが大きくなるほど大きくなる指標と所定の閾値とを比較し（Ｓ４５１）、当該指標が当該所定の閾値以上である場合、当該使用対象の複数のＧＰＳ衛星毎の当該乖離量に基づいて、当該使用対象の複数のＧＰＳ衛星のうち一部を選び、選んだ当該一部を使用対象から除外する（Ｓ４５２）。
【００９１】
このように、カルマンフィルタをタイトカップリング型とすることで、カルマンフィルタを実行する際に、精度が劣化したＧＰＳ測位結果の検出および排除を行い、ＧＰＳ精度劣化による推定車両軌跡の影響を軽減することで、ローコストなセンサでも精度よい軌跡を得ることができる。
【００９２】
なお、上記実施形態における構成と特許請求の範囲の構成との対応関係について説明すると、ジャイロスコープ２２が角速度センサに相当し、車速センサ２１が速度センサに相当し、図４のＳ１００〜Ｓ３００が走行軌跡推定手段に相当し、図４のＳ４００、図７のＳ４０２〜Ｓ４１２、図８のＳ５００〜Ｓ５０６が誤差推定手段に相当し、Ｓ５０２が取り付け角ゲイン誤差推定手段に相当し、Ｓ５０４が判定手段に相当し、Ｓ５０６が角速度ゲイン誤差推定手段に相当し、Ｓ４５１が比較手段に相当し、Ｓ４５２が除外手段に相当する。
（他の実施形態）
以上、本発明の実施形態について説明したが、本発明の範囲は、上記実施形態のみに限定されるものではなく、本発明の各発明特定事項の機能を実現し得る種々の形態を包含するものである。
【００９３】
例えば、上記実施形態では、推定する状態量ｘｋとして、絶対位置東方向誤差εＸｋ、絶対位置北方向誤差εＹｋ、絶対位置高度方向誤差εＺｋ、垂直方向速度誤差εＵｋ、絶対水平方位誤差εθｋ、車速係数誤差εＯｋ、ジャイロバイアス誤差εＧｂｋ、ジャイロゲイン誤差εＧｓｋ、クロックバイアス誤差εＣｂｋ、クロックドリフト誤差εＣｄｋ、という１０個の誤差値を採用しているが、必ずしもこのようになっておらずともよく、推定する状態量ｘｋは、自立航法用の自立センサの出力値に影響する誤差の組であれば、どのようなものであってもよい。例えば、推定する状態量ｘｋとして、絶対位置東方向誤差εＸｋ、絶対位置北方向誤差εＹｋ、絶対水平方位誤差εθｋ、車速係数誤差εＯｋ、ジャイロバイアス誤差εＧｂｋ、ジャイロゲイン誤差εＧｓｋという６個の誤差値を採用してもよい。
【００９４】
また、上記実施形態において、Ｓ４０７の検定処理およびＳ４０８の判定処理は、Ｓ４０６の次に行うようになっているが、かならずしもこのタイミングで行わなくてもよい。例えば、Ｓ４０７、Ｓ４０８の処理は、Ｓ４０９の後に行われてもよいし、Ｓ４１１の後に行われてもよい。また、図６の処理を実行する度にＳ４０７、Ｓ４０８を実行する必要はなく、図６の処理を複数回（たとえば１０回）実行するうちの９回は、Ｓ４０６に続いてＳ４０９を実行し、１回のみ、Ｓ４０６に続いてＳ４０７、Ｓ４０８を実行するようになっていてもよい。
【００９５】
また、上記実施形態では、タイトカップリング型カルマンフィルタで用いる観測量として、使用対象の複数のＧＰＳ衛星１００のそれぞれについて、当該ＧＰＳ衛星の観測擬似距離と、当該自立センサ２０の出力値に基づいて推測航法によって算出される車両から当該ＧＰＳ衛星までの推測擬似距離との残差、および、ＧＰＳ受信装置１０から出力される当該ＧＰＳ衛星の観測ドップラー周波数と、自立センサ２０の出力値に基づいて推測航法によって算出される当該ＧＰＳ衛星の推測ドップラー周波数との残差を採用している。
【００９６】
しかし、タイトカップリング型カルマンフィルタで用いる観測量としては、必ずしもこのようなものに限らず、例えば、観測擬似距離と観測ドップラー周波数の関数ｆ１（ＰＲ（ｎ）ＧＰＳ，ＤＰ（ｎ）ＧＰＳ）と、同じ関数型の観測擬似距離と観測ドップラー周波数の関数ｆ１（ＰＲ（ｎ）ＤＲ，ＤＰ（ｎ）ＤＲ）との残差を採用してもよい。
【００９７】
つまり、タイトカップリング型カルマンフィルタで用いる観測量としては、使用対象の複数のＧＰＳ衛星１００の毎の、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値と推測航法で算出した値の乖離量（例えば差の自乗、差の絶対値）を採用するようになっていればよい。
【００９８】
したがって、図７のＳ４５１で閾値ＴＨと比較する量は、必ずしも上記のようなカイ自乗値χ２に限らず、使用対象の複数のＧＰＳ衛星１００毎の上記乖離量のいずれかが大きくなるほど大きくなる指標であれば、どのようなものであってもよい。
【００９９】
また、上記の実施形態において、制御回路１７がプログラムを実行することで実現している各機能は、それらの機能を有するハードウェア（例えば回路構成をプログラムすることが可能なＦＰＧＡ）を用いて実現するようになっていてもよい。
【符号の説明】
【０１００】
１０ ＧＰＳ受信装置
１１ ＧＰＳアンテナ部
１２ ＧＰＳ−ＲＦ部
１３ ＧＰＳ信号処理部
１４ ＧＰＳ測位演算部
２０ 自立センサ
２１ 車速センサ
２２ ジャイロスコープ
３０ 地図データ記憶装置
４０ ディスプレイ部
５０ 制御部
１００ ＧＰＳ衛星
【特許請求の範囲】
【請求項１】
車両に搭載される自立センサ（２０）の出力値に基づいて推測航法により前記車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて前記車両の位置および方位を補正する車両用軌跡推定装置であって、
前記カルマンフィルタは、観測量として、使用対象の複数のＧＰＳ衛星毎に、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、
当該車両用軌跡推定装置は、前記タイトカップリング型カルマンフィルタにおける前記使用対象の複数のＧＰＳ衛星毎の前記乖離量のいずれかが大きくなるほど大きくなる指標と所定の閾値とを比較する比較手段（Ｓ４５１）と、
前記指標が前記所定の閾値以上であると前記比較手段（Ｓ４５１）が判定した場合、前記使用対象の複数のＧＰＳ衛星毎の前記乖離量に基づいて、前記使用対象の複数のＧＰＳ衛星のうち一部を選び、選んだ前記一部を使用対象から除外する除外手段（Ｓ４５２）と、を備えた車両用軌跡推定装置。
【請求項１】
車両に搭載される自立センサ（２０）の出力値に基づいて推測航法により前記車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて前記車両の位置および方位を補正する車両用軌跡推定装置であって、
前記カルマンフィルタは、観測量として、使用対象の複数のＧＰＳ衛星毎に、当該ＧＰＳ衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をＧＰＳ衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、
当該車両用軌跡推定装置は、前記タイトカップリング型カルマンフィルタにおける前記使用対象の複数のＧＰＳ衛星毎の前記乖離量のいずれかが大きくなるほど大きくなる指標と所定の閾値とを比較する比較手段（Ｓ４５１）と、
前記指標が前記所定の閾値以上であると前記比較手段（Ｓ４５１）が判定した場合、前記使用対象の複数のＧＰＳ衛星毎の前記乖離量に基づいて、前記使用対象の複数のＧＰＳ衛星のうち一部を選び、選んだ前記一部を使用対象から除外する除外手段（Ｓ４５２）と、を備えた車両用軌跡推定装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１２−６３３１３（Ｐ２０１２−６３３１３Ａ）
【公開日】平成２４年３月２９日（２０１２．３．２９）
【国際特許分類】
【出願番号】特願２０１０−２０９５２２（Ｐ２０１０−２０９５２２）
【出願日】平成２２年９月１７日（２０１０．９．１７）
【出願人】（０００００４２６０）株式会社デンソー (27,639)
【Ｆターム（参考）】
【公開日】平成２４年３月２９日（２０１２．３．２９）
【国際特許分類】
【出願日】平成２２年９月１７日（２０１０．９．１７）
【出願人】（０００００４２６０）株式会社デンソー (27,639)
【Ｆターム（参考）】
[ Back to top ]