Notice: Undefined variable: fterm_desc_sub in /mnt/www/biblio_conv.php on line 353
車両用軌跡推定装置
説明

車両用軌跡推定装置

【課題】GPS受信装置がGPS衛星からの信号に基づいて算出する情報をカルマンフィルタの観測量に用いることで、自立センサの誤差を推定し、推定したセンサ誤差を補正する技術において、カルマンフィルタで用いるGPS測位結果の精度の劣化を全体として抑える。
【解決手段】カルマンフィルタは、観測量として、使用対象の複数のGPS衛星毎に、当該GPS衛星の擬似距離およびドップラー周波数について、その量をGPS衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型である。当該車両用軌跡推定装置は、当該カルマンフィルタにおける上記乖離量についてのカイ自乗値と所定の閾値とを比較し(S451)、当該カイ自乗値が当該所定の閾値以上である場合、使用対象の複数のGPS衛星毎うち精度の低い一部を選び、選んだ当該一部を使用対象から除外する(S452)。

【発明の詳細な説明】
【技術分野】
【0001】
本発明は、車両用軌跡推定装置に関するものである。
【背景技術】
【0002】
従来、GPS受信装置が算出する現在位置情報をカルマンフィルタの観測量に用いることで、自立センサ(車速センサ、ジャイロスコープ等)の誤差を推定し、推定したセンサ誤差を補正する技術が知られている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開2000−55678号公報
【特許文献2】特開2008−232761号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかし、GPS受信装置による測位結果は、マルチパス波の受信やGPS衛星配置悪化等の要因により測位精度が大きく変動する。精度が劣化したGPS測位結果を使用してカルマンフィルタを実行するとセンサ誤差の推定を誤り、走行軌跡の推定精度が悪化してしまうという問題がある。
【0005】
本発明はこの問題点に鑑み、GPS受信装置がGPS衛星からの信号に基づいて算出する情報をカルマンフィルタの観測量に用いることで、自立センサの誤差を推定し、推定したセンサ誤差を補正する技術において、カルマンフィルタで用いるGPS測位結果の精度の劣化を全体として抑えることを目的とする。
【課題を解決するための手段】
【0006】
上記目的を達成するための請求項1に記載の発明は、車両に搭載される自立センサ(20)の出力値に基づいて推測航法により前記車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて前記車両の位置および方位を補正する車両用軌跡推定装置であって、前記カルマンフィルタは、観測量として、使用対象の複数のGPS衛星毎に、当該GPS衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をGPS衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、当該車両用軌跡推定装置は、前記タイトカップリング型カルマンフィルタにおける前記使用対象の複数のGPS衛星毎の前記乖離量のいずれかが大きくなるほど大きくなる指標と所定の閾値とを比較する比較手段(S451)と、前記指標が前記所定の閾値以上であると前記比較手段(S451)が判定した場合、前記使用対象の複数のGPS衛星毎の前記乖離量に基づいて、前記使用対象の複数のGPS衛星のうち一部を選び、選んだ前記一部を使用対象から除外する除外手段(S452)と、を備えた車両用軌跡推定装置である。
【0007】
このように、カルマンフィルタをタイトカップリング型とすることで、カルマンフィルタを実行する際に、精度が劣化したGPS測位結果の検出および排除を行い、GPS精度劣化による推定車両軌跡の影響を軽減することで、精度よく車両の位置および方位を補正することができる。 なお、上記および特許請求の範囲における括弧内の符号は、特許請求の範囲に記載された用語と後述の実施形態に記載される当該用語を例示する具体物等との対応関係を示すものである。
【図面の簡単な説明】
【0008】
【図1】本発明の一実施形態に係る車両用現在位置検出装置の構成を示す図である。
【図2】信号生発生過程、観測過程、カルマンフィルタの模式図である。
【図3】制御部による推測航法処理のフローチャートである。
【図4】ジャイロスコープのオフセット補正について説明するための図である。
【図5】ジャイロスコープのゲイン補正について説明するための図である。
【図6】制御部によるGPS複合化処理のフローチャートである。
【図7】制御部による観測量の検定処理のフローチャートである。
【図8】有意水準5%の閾値を示す図表である。
【発明を実施するための形態】
【0009】
以下、本発明の一実施形態について説明する。図1に、本実施形態に係る車両用現在位置検出装置の構成を示す。本実施形態に係る車両用現在位置検出装置は車両用ナビゲーション装置として構成されており、GPS受信装置10、自立センサ20、地図データ記憶装置30、ディスプレイ部40、および制御部50を備えている。
【0010】
GPS受信装置10は、複数のGPS衛星100(図中では、1つのみ示す)からの電波を受信するGPSアンテナ部11、GPSアンテナ部11から入力される高周波信号を中間周波数帯の受信信号に変換するGPS―RF部12、GPS―RF部12から入力される受信信号の同期捕捉や同期保持等の信号処理を行うGPS信号処理部13、GPS信号処理部13から出力されるデータに基づいてGPS測位演算を行い、このGPS測位演算により求めた測位データ、複数のGPS衛星100のそれぞれの絶対位置、複数のGPS衛星100のそれぞれの擬似距離およびドップラー周波数等を出力するGPS測位演算部14を備えている。
【0011】
GPS受信装置10は、GPS衛星100からの電波を受信し、車両の絶対的な現在位置を表すGPS位置、車両の進行方向を特定するための速度ベクトル等を算出し、算出結果を測位データとして出力する。
【0012】
自立センサ20は、車両の走行速度に応じた車速パルス信号を出力する速度センサとしての車速センサ21と、車両の回転角速度に応じた角速度信号を出力する角速度センサとしてのジャイロスコープ22を備えている。
【0013】
地図データ記憶装置30は、地図データが記憶された地図データ記憶媒体から地図データを読み出すための装置である。地図データには、各リンクの距離、道路種別、車線数、一方通行規制、信号機の位置等を表す道路データ、位置検出精度を向上するためのいわゆるマップマッチングデータ、川、湖、海、鉄道、施設などの位置、形状、名称を表す背景データ、各地の施設の名称、所在位置、施設種類等を示す施設データ等が含まれる。なお、地図データ記憶媒体としては、CD−ROM、DVD−ROM、ハードディスクドライブ、メモリカード等がある。
【0014】
ディスプレイ部40は、液晶等の表示部を備え、コンピュータから入力される映像信号に応じた映像を表示部に表示させる。
【0015】
制御部50は、CPU、ROM、RAM、不揮発性メモリ、I/O等を有するコンピュータにより構成されており、CPUは、ROMに記憶されたプログラムに従って各種処理を実施する。
【0016】
制御部50の処理としては、ジャイロスコープ22の出力値に基づいて算出される方位変化量と、速度センサ21の出力値に基づいて算出される車速係数から、車両の相対軌跡を推定するとともに当該車両の相対軌跡とGPS受信装置10からの出力値とに基づいて車両の走行軌跡を推定する走行軌跡推定処理、ジャイロスコープ22の出力値およびGPS受信装置10からの出力値に基づいて方位変化量のオフセット誤差、方位変化量のゲイン誤差、絶対方位誤差等を推定する誤差推定処理、自車の走行軌跡と地図メモリ70に格納されている地図情報の道路データとを照合することによって、自車位置(GPS位置により補正された推測位置)を道路上の位置に修正するマップマッチング処理等がある。なお、走行軌跡推定処理では、誤差推定処理により推定された各誤差を用いて走行軌跡推定処理による車両の走行軌跡の推定に用いられる方位変化量のオフセット、方位変化量のゲインおよび絶対方位等を補正するようになっている。
【0017】
制御部50は、誤差推定処理においてカルマンフィルタを用いて推測航法における各誤差要因を1つの状態量xとしてモデル化し、各誤差の推定値を求める誤差推定処理を実施する。
【0018】
ここで、カルマンフィルタの概要について説明する。カルマンフィルタは、図2および式(1)、(2)に示すような信号発生過程および観測過程を前提としている。
【0019】
【数1】

すなわち、プロセス行列φで定義される線形システム(信号発生過程)があり、そのシステムにおいて状態量Xに対して、観測行列Hで関係付けられた状態量Xの一部が観測可能(観測過程)である場合、カルマンフィルタは、この観測量Yを入力として、状態量Xの最適な推定値xを与えるものである。なお、rは信号発生過程にて発生する雑音であり、qは観測過程において発生する雑音である。
【0020】
そして、時刻kまでの情報を用いた状態量Xの最適推定値x=x(k|k)は、次の式(3)〜(8)により求めることができる。
【0021】
【数2】

ここで、Kはカルマンゲイン、Pは推定値xの共分散(以下では誤差共分散という)、Qは観測過程で発生する観測雑音qの分散、Rは信号発生過程で発生するシステム雑音rの分散である。またSは、式(5)で定義される残差の共分散である。また各量(仮にAとする)の右上に−(マイナス記号)の上付き添字が付された量Aは、時刻k−1までの情報に基づく時刻kでの量Aの推定値(すなわちA(k|k−1))である。また、雑音r、qはいずれも平均0の白色ガウス雑音であり互いに無相関である。また、Iは単位行列であり、上付き添字Tは転置を意味し、上付き添字−1は逆行列を意味する。
【0022】
このように定義されたカルマンフィルタでは、状態量の推定値xを定義すると共に、誤差共分散Pに適当な初期値を設定するとにより、状態量の最適推定値xが得られ、観測が行われ観測量Ykが得られる毎にこれを繰り返すことにより、状態量の最適推定値xの精度が向上する。
【0023】
そして、本実施例では、状態量Xとして、東方向の絶対位置を算出する際に付与される絶対位置東方向誤差εX、北方向の絶対位置を算出する際に付与される絶対位置北方向誤差εY、高度方向の絶対位置を算出する際に付与される絶対位置高度方向誤差εZ、垂直方向の速度を算出する際に付与される垂直方向速度誤差εU、絶対水平方位を算出する際に付与される絶対水平方位誤差εθ、車速を算出する際に、車速センサ21から出力された単位時間当たりのパルス数に乗じられる車速係数の誤差である車速係数誤差εO、ジャイロ出力の大きさによらず、出力値の全領域に均等に付加されるジャイロバイアス誤差εGb、ジャイロ出力から角速度への変換比率として設定される変換ゲインの誤差であるジャイロゲイン誤差εGs、クロックバイアス誤差εCb、クロックドリフト誤差εCd、という10個の誤差値を採用し、これら誤差値を次の式(9)〜(18)により定義する。
【0024】
【数3】

ただし、Tは前回(の観測)からの経過時間、Dは前回からの方位変化量、Lは前回からの移動距離、θは真の絶対方位である。
【0025】
そして、これら誤差値の時間的な変化を規定するものがプロセス行列φであり、上記式(9)〜(18)を各誤差値にて偏微分し線形化することにより求められ、その結果、プロセス行列φは式(19)のようになる。
【0026】
【数4】

ただし、θ=θ+εθk−1+T×εGk−1/2+DT×εGsk−1/2
であり、このθは絶対方位を意味しており、真の絶対方位θにセンサ誤差に基づく誤差が加わったものとなっている。また、Gsは車速係数であり、Dは前回からの方位変化量であり、Lは前回からの移動距離である。
【0027】
また、観測量Yとしては、各々のGPS衛星の観測擬似距離と推測擬似距離との残差、および、各々のGPS衛星の観測ドップラー周波数と推測ドップラー周波数との残差を採用する。
【0028】
ここで、各GPS衛星の観測擬似距離および観測ドップラー周波数は、GPS受信装置10によって計測され、GPS受信装置10から制御部50に随時出力されているものを用いる。また、各GPS衛星の推測擬似距離および推測ドップラー周波数は、GPS受信装置10から出力される各GPS衛星の現在位置、後述するS100で推測航法によって算出される自車両の移動距離、後述するS300で推測航法によって算出される自車両の絶対方位、絶対位置、および車速等に基づいて算出する。
【0029】
このように、擬似距離、ドップラー周波数等の、GPS衛星毎に算出される量を観測量とするカルマンフィルタを、タイトカップリング型カルマンフィルタという。一方、特許文献1に記載のような観測量を用いるカルマンフィルタをルーズカップリング型カルマンフィルタいう。タイトカップリング型カルマンフィルタも、周知の技術であり、例えば、特許文献2に記載されている。
【0030】
ルーズカップリング型カルマンフィルタは、GPS測位結果(位置、方位、速度)を観測量として利用するが、本実施形態のようなタイトカップリング型カルマンフィルタは、各GPS衛星の観測疑似距離とドップラー周波数を観測量として利用する。つまり、タイトカップリング型カルマンフィルタは、GPS各衛星の観測疑似距離とドップラー周波数とセンサで推定した観測疑似距離と観測ドップラー周波数との差分を、誤差共分散行列に従い、最小にする状態量を推定する。
【0031】
ここで、n番目のGPS衛星の擬似距離PR(n)は、式(20)のように定義される。
【0032】
【数5】

ただし、SV、SV、SVは、それぞれn番目のGPS衛星の位置座標の成分であり、xprediction、yprediction、zpredictionは、GPS受信装置10の位置座標であり、Clkbiasは、クロックバイアス誤差εCbkである。なお、式(20)のx、y、zの向きは、観測量のεX、εY、εZと同じである。
【0033】
また、n番目のGPS衛星の観測ドップラー周波数(積算ドップラー)DP(n)は、式(21)のように定義される。
【0034】
【数6】

ただし、SVの上の黒丸は、時間微分を表し、RpredictionはGPS受信装置10からn番目のGPS衛星までの距離を示し、Clkdriftはクロックドリフト誤差εCdを表す。
【0035】
これらの定義式(20)、(21)を各状態量εX、εY、εZ、εU、εθ、εO、εGb、εGs、εCb、εCdにて偏微分し線形化することにより、観測行列Hが式(22)のように得られる。
【0036】
【数7】

ここで、各偏微分は、式(23)〜(30)のように表される。
【0037】
【数8】

【0038】
【数9】

なお、式(22)では、2個のGPS衛星を用いる場合の観測行列Hの例であり、m個のGPS衛星を用いる場合の観測行列Hは、2n行10列の行列となる。
【0039】
このように、GPS毎に別々に定義された観測量を用いるタイトカップリング型カルマンフィルタを用いる制御部50は、車速センサ21からの信号に基づいて車両の移動距離を算出すると共に、ジャイロスコープ22からの信号に基づいて車両の方位変化量を算出する。そして、これら算出された移動距離及び方位変化量に基づいて、推測航法データとして、車両の速度、相対軌跡、絶対位置、絶対方位を算出する。一方、GPS受信装置10は、GPS測位データとして、車両の位置、方位、車速を随時出力し、また、各衛星の位置座標、各衛星までの観測擬似距離、および各衛星の観測ドップラー周波数を随時出力している。
【0040】
そして、制御部50は、タイトカップリング型カルマンフィルタを用いて、推測航法データおよびGPS測位データ、各衛星の位置座標、観測擬似距離、および観測ドップラー周波数に基づいて、各誤差εX、εY、εZ、εU、εθ、εO、εGb、εGs、εCb、εCdの推定値を算出し、更に、この推定値に基づいて、車両の方位変化量、移動距離、相対軌跡、絶対位置等の算出および補正を行う。
【0041】
以下、制御部50による推測航法処理について説明する。図3に、推測航法処理のフローチャートを示す。本車両用現在位置検出装置が動作状態になると、制御部50は、図4に示す処理を一定周期TM毎に繰り返し実行する。
【0042】
まずS100では、方位変化量・移動距離演算処理を実行する。この処理では、まず、ジャイロスコープ22により検出され出力されたジャイロ出力角速度にメインルーチンの起動周期TMを乗じることにより車両の向きの方位変化量を算出する。
方位変化量=ジャイロ出力角速度×TM (31)
なお、ジャイロ出力角速度は、図4のグラフから明らかなように、ジャイロスコープ22の出力をサンプリングした値から、オフセット値(図4では2.5V) を減じたものに、変換ゲイン(グラフの傾きに相当する) を乗じることにより算出されたものである。
【0043】
次に、後述するオフセット補正量に一定周期TMを乗じたものを、式(31)により求めた方位変化量を減じることにより、方位変化量のオフセット補正を行う。
【0044】
方位変化量=方位変化量−オフセット補正量×TM (32)
すなわち、図4に示すように、角速度0[deg/sec] に対応するジャイロスコープ22の出力電圧(ゼロ点) の変動に基づく誤差を補正する。次に、オフセット補正された方位変化量に、後述するゲイン補正量を減じることにより、方位変化量のゲイン補正を行う。
【0045】
方位変化量=方位変化量×ゲイン補正量 (33)
すなわち、図5に示すように、グラフの傾き(変換ゲイン) の変動に基づく誤差を補正する。
【0046】
そして、一定周期TMの間に検出された車速センサ21からの出力パルス数に、後述する車速係数を乗じることにより移動距離を算出する。
【0047】
移動距離=車速センサパルス数×車速係数 (34)
このように、移動距離を算出すると、方位変化量・移動距離の演算処理を終了する。
【0048】
次のS200の相対軌跡の演算処理では、まず、式(33)を用いて算出した方位変化量を、それまでに求められている相対方位に加算することにより、車両の向きの相対方位を更新する。
【0049】
相対方位=相対方位+方位変化量 (35)
次に、この更新した相対方位および式(34)を用いて算出した移動距離に基づき、相対位置座標の更新を行う。具体的には、南北方向をx座標軸とした相対座標rel.x、東西方向をy座標軸とした相対座標rel.yを、式(36)、(37)に従って更新する。ただし、θsは式(35)を用いて算出した相対方位である。
【0050】
rel.x=rel.x+移動距離×cos(θs) (36)
rel.y=rel.y+移動距離×sin(θs) (37)
すなわち、この更新は、移動距離に対する相対方位のX、Y成分をそれまでの相対位置座標に加算することにより行う。この相対位置座標は相対軌跡を求めるために算出するもので、その相対軌跡と道路形状との関係により、いわゆるマップマッチングが行われる。
【0051】
次に、式(34)を用いて算出した移動距離を、周期TMで除することにより車速を算出する。
【0052】
車速=移動距離/TM (38)
このように、車速を算出すると、相対軌跡演算処理を終了する。
【0053】
次のS300の絶対方位・絶対位置の演算処理では、まず、式(34)を用いて算出した方位変化量を、それまでに求められている車両の向きの絶対方位に加算することにより、絶対方位を更新する。
【0054】
絶対方位=絶対方位+方位変化量 (39)
次に、この更新した絶対方位および式(34)を用いて算出した移動距離に基づき、絶対位置座標の更新を行う。具体的には、南北方向をx座標軸とした絶対座標abs.x、東西方向をy座標軸とした絶対座標abs.yを、式(40)、(41)に従って更新する。ただし、θは式(39)を用いて算出した絶対方位である。
【0055】
abs.x=abs.x+移動距離×cos(θ) (40)
abs.y=abs.y+移動距離×sin(θ) (41)
すなわち、この更新は、移動距離に対する絶対方位のX、Y成分をそれまでの絶対位置座標に加算することにより行う。このようにして更新した車両の絶対方位と絶対位置は後述するGPSとの複合化処理にて利用される。
【0056】
次に、S400のGPS複合化処理を実施する。このGPS複合化処理のフローチャートを図7に示す。ここでは、このGPS複合化処理の実行回数がk回目であるとして説明する。
【0057】
このGPS複合化処理では、まずS401で、車両用現在位置検出装置の起動後、GPS受信装置10が周知の初期位置算出(first fix)を行ったか否か判定し、行っていなければ直ちにGPS復号化処理を終了し、行っていれば続いてS402に進む。なお、GPS受信装置10は、周知の初期位置算出を行ったとき、測位データを制御部50に出力するので、測位データを制御部50は、車両用現在位置検出装置の起動後、一度以上測位データを受け取っていれば、初期位置算出を行ったと判定する。
【0058】
S402では、GPS受信装置10が信号を受信できている衛星数が所定の衛星数THsy以上であるか否かを判定し、衛星数THsy未満であれば直ちにGPS復号化処理を終了し、衛星数THsy以上であればS403に進む。なお、GPS受信装置10は信号を受信できている衛星数の情報を随時制御部50に出力するようになっているので、S402では、この情報に基づいて判定を行えばよい。この閾値THsyは、1でもよいし、2でもよいし、3以上であってもよい。
【0059】
続いてS403では、状態量の推定を行う。具体的には、式(3)を用いて、状態量の推定値xを算出する。続いてS404では、誤差共分散の推定を行う。具体的には、式(4)を用いて、誤差共分散の推定値Pk+1を算出する。ここで、Qは、観測疑似距離、観測ドップラーの観測誤差が設定される。
【0060】
続いてS405では、観測量Yを取得する。具体的には、観測量Yは、上述の通り、n番目のGPS衛星の観測擬似距離および推測擬似距離をそれぞれPR(n)GPS、PR(n)DRとし、n番目のGPS衛星の観測ドップラー周波数および推測ドップラー周波数をDP(n)GPS、DP(n)DRとすると観測量Yは、以下の式(42)のようなベクトル量となる。
【0061】
【数10】

ただし、式(42)では、GPS受信装置10が信号を受信できているGPS衛星のうち、使用対象のGPS衛星としてm個が選択されている。使用対象のGPS衛星のリストは、制御部50のRAM等に記録されている。なお、S402の直後においては、初期設定として、現在信号を受信できているGPS衛星すべてを、使用対象のGPS衛星とする。
【0062】
なお、観測擬似距離PR(n)GPSおよび観測ドップラー周波数DP(n)GPSとしては、上述の通り、GPS受信装置10から出力されたn番目の衛星の擬似距離およびドップラー周波数を用いる。
【0063】
また、推測擬似距離DP(n)DRについては、式(20)を用いてGPS受信装置10から受信したn番目のGPS衛星の現在位置座標をSV、SV、SVに代入し、直前のS300で算出した推測航法による車両の位置座標をxprediction、yprediction、zpredictionに代入し、前回の図6のS410において算出したクロックバイアス誤差εCbをClkbiasに代入することで、算出する。
【0064】
また、推測ドップラー周波数DP(n)DRについては、式(21)を用いてGPS受信装置10から受信したn番目のGPS衛星の現在位置座標をSV、SV、SVに代入し、GPS受信装置10から受信したn番目のGPS衛星の現在位置座標の時間微分値をSV、SV、SVの微分値に代入し、直前のS300で算出した推測航法による車両の位置座標をxprediction、yprediction、zpredictionに代入し、直前のS300で算出した推測航法による車両の位置座標とGPS受信装置10から受信したn番目のGPS衛星の現在位置座標とに基づいて車両から当該GPS衛星までの距離を算出し、その算出結果をRpredictionに代入し、****をVに代入し、****をνに代入し、****をδSFodoに代入し、****ψに代入し、前回の図6のS410において算出したクロックドリフト誤差εCdをClkdriftに代入することで、算出する。
【0065】
続いてS406では、式(5)を用いて残差の共分散Sの導出を行う。ここで、Rは、TM間に発生する状態量の推定誤差が設定される。また、観測行列Hとしては、式(22)を用い、式(22)中の各偏微分値については、式(23)〜(30)に対して、上記の推測ドップラー周波数の算出時と同様の代入を行うことで算出する。
【0066】
このようになっているので、観測量Yの各成分のうち、n番目のGPS衛星に関する成分PR(n)GPS−PR(n)DRおよびDP(n)GPS−DP(n)DRは、使用対象のGPS衛星のうち、当該n番目のGPS衛星のみの軌道(位置および移動速度)に依存する量である。
【0067】
続いてS407では、観測量の検定処理を行う。図7に、この観測量の検定処理のフローチャートを示す。この観測量の検定処理において制御部50は、まずS450で、カイ自乗値の計算を行う。カイ自乗値χは以下の式(43)で定義されている。
【0068】
【数11】

この定義式は、以下の式(44)〜(46)のように変形できる。
【0069】
【数12】

ここで、R−1は、誤差σσで表される誤差共分散行列であり、[z−h(a,x)]は残差を示す。これを本実施形態のタイトカップリング型カルマンフィルタに適用すると、カイ自乗値は以下の式(47)のようになる。
【0070】
【数13】

このカイ自乗値χは、現在の使用対象のGPS衛星のそれぞれについて、観測擬似距離と推測擬似距離の乖離が大きくなるほど大きくなる量であり、また、観測ドップラー周波数と推測ドップラー周波数の乖離が大きくなるほど大きくなる量である。
【0071】
続いてS451では、算出したカイ自乗値が閾値TH以上であるか否かを判定する。閾値としては、例えば、有意水準を5%として、現在使用しているGPS衛星の数に応じて、図5に示すようなχ自乗値を閾値THとして採用する。例えば、現在使用しているGPS衛星が5個であれば、閾値THとして、1.145477299を採用する。カイ自乗値が閾値TH以上であると判定すれば、続いてS452に進み、使用しているGPS衛星のうち一部を選び、選んだ一部を使用対象から除外する。つまり、使用対象のGPS衛星のリストから、当該一部のGPS衛星を削除する。
【0072】
選んで除外するGPS衛星としては、直前のS406で算出した残差の共分散Sに基づいて決定する。例えば、残差の共分散Sの対角成分のうち、最も値の大きい成分を特定し、その成分に対応するGPS衛星を除外対象としてもよい。例えば、残差の共分散Sの2行2列目の成分の値が最も大きければ、1番目のGPS衛星を除外対象とする。より一般化して言えば、残差の共分散Sのj行j列目の成分の値が最も大きければ、j/2を下回らない最小の整数i番目のGPS衛星を除外対象とする。
【0073】
このように、カルマンゲインの計算前に、精度の悪いGPS衛星を排除することで、高精度にジャイロスコープ22の検出値、クロックゲイン誤差、クロックバイアス誤差の補正が可能となる。S452の後は、検定処理を終了して図6のS408に進む。また、S451でカイ自乗値が閾値TH以上でないと判定すれば、使用しているGPS衛星のいずれも使用対象から除外することなく、検定処理を終了してS408に進む。
【0074】
S408では、S407で排除した衛星があるか否かを判定し、あればS415に進み、なければS409に進む。
【0075】
S409では、式(6)を用いてカルマンゲインKを算出する。ここで、観測行列Hとしては、直前に実行したS406で残差の共分散Sを算出したときと同じものを用いる。
【0076】
続いてS410では、式(7)を用いて、更新された状態量Xの推定値xを算出する。ここで、観測値Yとしては、直前に実行したS407の算出結果を用いる。また、カルマンゲインKとしては、直前に実行したS409の算出結果を用いる。また、観測行列Hとしては、直前に実行したS406で残差の共分散Sを算出したときと同じものを用いる。
【0077】
続いてS411では、更新された誤差共分散の推定を行う。具体的には、式(8)を用いて誤差共分散Pを算出する。ここで、カルマンゲインKとしては、直前に実行したS409の算出結果を用いる。また、観測行列Hとしては、直前に実行したS406で残差の共分散Sを算出したときと同じものを用いる。
【0078】
続いてS412では、S410で算出されたx、すなわち、状態量εX、εY、εZ、εU、εθ、εO、εGb、εGsに基づき、推測航法誤差の補正(ジャイロ6のオフセット補正、同じくゲイン補正、車速センサ21の車速係数補正、絶対方位補正、絶対位置補正)を(48)〜(57)式に従って行う。
abs.y(東方向絶対位置)=abs.y−εY (48)
abs.x(北方向絶対位置)=abs.x−εX (49)
abs.z(高度方向絶対位置)=abs.z−εZ (50)
垂直方向速度=スイチョウ方向速度−εU (51)
絶対水平方位=絶対水平方位−εθ (52)
車速係数=車速係数×(1−εO) (53)
オフセット補正量=オフセット補正量−εGb (54)
ゲイン補正量=ゲイン補正量×(1−εGs) (55)
クロックバイアス補正量=クロックバイアス補正量−εCbk (56)
クロックドリフト補正量=クロックドリフト補正量−εCdk (57)
なお、クロックバイアス補正量、クロックゲイン誤差補正量については、次回の図6の処理において、式(20)、(21)を用いて推測擬似距離、推測ドップラー周波数を算出する際に用いる。
【0079】
例えば、ジャイロスコープ22のオフセット補正およびゲイン補正により、S100にて用いられるオフセット補正量及びゲイン補正量が修正され、車速センサ21の車速係数補正により、S100にて用いられる車速係数が修正され、絶対方位補正により、S300にて用いられる絶対方位θが修正され、絶対位置補正によりS300にて用いられる絶対位置が修正される。
【0080】
そして、上記S401→S402→S403〜S408→S409〜S412の処理が、GPS受信装置10からのGPS測位データがある毎に繰り返されることにより、上記誤差が随時修正されるため、より正確な推測航法データが得られることになる。
【0081】
そして、上記S401→S402→S403〜S408→S409〜S412の処理が、GPS受信装置10からのGPS測位データがある毎に繰り返されることにより、上記誤差が随時修正されるため、より正確な推測航法データが得られることになる。
【0082】
なお、初期位置算出ができた時点では、続くS402の判定は殆どの場合YES(S403に進む)となるが、初期位置算出が一度できてしまえば、その後車両用現在位置検出装置の起動中は、常にS401からS402に進むので、例えば、初期位置算出が一度できた後に、例えば車両がビルの陰やトンネルに入ることで、それまで受信できていたGPS受信装置からの信号を受信できなると、S402で受信できている衛星数がTHsy未満であると判定し、続いてS413に進む。
【0083】
S413では、受信できている衛星数がTHsy未満である状態が長時間(具体的にはT1秒以上)継続しているか否かを判定し、長時間継続していないと判定した場合は、そのまま今回の図6の処理を終了するが、長時間継続していると判定した場合は続いてS414で、プロセス行列φの計算および誤差共分散Pの予測計算を行う。
【0084】
これは、GPS受信装置10から十分な数のGPS測位データが得られない場合、何も補正を行わないと誤差が大きくなってしまうため、誤差共分散Pの予測計算、即ち誤差の見積もりだけは定期的に行っておくことにより、その後にGPS受信装置10のGPS測位データの入力が再開された時に行われるカルマンフィルタの処理を正確に行えるようにするためのものである。
【0085】
また、S407における観測量の検定処理において、GPS衛星が使用対象から1つでも排除された場合、S408で、排除衛星があると判定し、続いてS415に進み、排除後の使用対象のGPS衛星の数がTHsy未満であるか否かを判定し、THsy未満であると判定した場合は、衛星数が不十分であるので、上述のS413に進む。
【0086】
THsy未満でないと判定した場合は、S405に処理を戻す。これにより、排除したことで数が減った使用対象のGPS衛星を用いて、観測量Yを再計算する。その際、観測量Yの数が減った分、Yの要素数も2s個減る(ただしsは除外されたGPS衛星の数)。つまり、使用対象から排除したGPS衛星についての擬似距離およびドップラー周波数の成分がYから除去される。更に、S406で再度残差の共分散Pを算出するが、その際に用いる観測行列Hは、その時点における使用対象のGPS衛星についてのみの成分を有するように作成されるので、行数が2s個減ることになる。
【0087】
このように、各衛星の観測量を直接利用可能なタイトカップリング方式カルマンフィルタを用いることで、残差の共分散Sを用いて精度が劣化したGPS衛星を特定することができ、更に、観測量Yの各成分が、特定の1つの衛星についての量(擬似距離、ドップラー周波数)となっているので、観測量Yから精度が劣化したGPS衛星の成分を選んで除外した上でカルマンフィルタの計算を継続することができるので、精度の高い補正処理を行うことができる。
【0088】
つまり、GPS精度は非連続に変化するが、推測航法軌跡精度は連続的に変化する特徴に着目し、各衛星の観測量を直接利用可能なタイトカップリング方式カルマンフィルタとその観測量(GPS観測結果とセンサ推定結果の残差)の推定観測誤差共分散で検定することにより、GPS測位精度劣化を正確に推定可能とする。これにより、精度劣化したGPS衛星を排除することで精度よくセンサ補正することが可能となる。
【0089】
また、従来のルースカップリング型カルマンフィルタでは、車両位置を観測量として利用するので、GPS測位に4衛星以上(3次元測位の場合)または3衛星以上(2次元測位の場合)の受信が必要であったが、本実施形態のようにタイトカップリング型カルマンフィルタを用いた場合は、擬似距離およびドップラー周波数という、最悪1衛星だけ受信できる場合でも得ることができる量を観測量としているので、GPS衛星を1つでも受信できれば、上記の処理を継続し、自立センサ21、22の補正をすることが可能となる。
【0090】
以上説明した通り、実施形態の車両用軌跡推定装置は、車両に搭載される自立センサ20の出力値に基づいて推測航法により当該車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて当該車両の位置および方位を補正し、当該カルマンフィルタは、観測量として、使用対象の複数のGPS衛星毎に、当該GPS衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をGPS衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、当該車両用軌跡推定装置は、当該タイトカップリング型カルマンフィルタにおける当該使用対象の複数のGPS衛星毎の当該乖離量のいずれかが大きくなるほど大きくなる指標と所定の閾値とを比較し(S451)、当該指標が当該所定の閾値以上である場合、当該使用対象の複数のGPS衛星毎の当該乖離量に基づいて、当該使用対象の複数のGPS衛星のうち一部を選び、選んだ当該一部を使用対象から除外する(S452)。
【0091】
このように、カルマンフィルタをタイトカップリング型とすることで、カルマンフィルタを実行する際に、精度が劣化したGPS測位結果の検出および排除を行い、GPS精度劣化による推定車両軌跡の影響を軽減することで、ローコストなセンサでも精度よい軌跡を得ることができる。
【0092】
なお、上記実施形態における構成と特許請求の範囲の構成との対応関係について説明すると、ジャイロスコープ22が角速度センサに相当し、車速センサ21が速度センサに相当し、図4のS100〜S300が走行軌跡推定手段に相当し、図4のS400、図7のS402〜S412、図8のS500〜S506が誤差推定手段に相当し、S502が取り付け角ゲイン誤差推定手段に相当し、S504が判定手段に相当し、S506が角速度ゲイン誤差推定手段に相当し、S451が比較手段に相当し、S452が除外手段に相当する。
(他の実施形態)
以上、本発明の実施形態について説明したが、本発明の範囲は、上記実施形態のみに限定されるものではなく、本発明の各発明特定事項の機能を実現し得る種々の形態を包含するものである。
【0093】
例えば、上記実施形態では、推定する状態量xとして、絶対位置東方向誤差εX、絶対位置北方向誤差εY、絶対位置高度方向誤差εZ、垂直方向速度誤差εU、絶対水平方位誤差εθ、車速係数誤差εO、ジャイロバイアス誤差εGb、ジャイロゲイン誤差εGs、クロックバイアス誤差εCb、クロックドリフト誤差εCd、という10個の誤差値を採用しているが、必ずしもこのようになっておらずともよく、推定する状態量xは、自立航法用の自立センサの出力値に影響する誤差の組であれば、どのようなものであってもよい。例えば、推定する状態量xとして、絶対位置東方向誤差εX、絶対位置北方向誤差εY、絶対水平方位誤差εθ、車速係数誤差εO、ジャイロバイアス誤差εGb、ジャイロゲイン誤差εGsという6個の誤差値を採用してもよい。
【0094】
また、上記実施形態において、S407の検定処理およびS408の判定処理は、S406の次に行うようになっているが、かならずしもこのタイミングで行わなくてもよい。例えば、S407、S408の処理は、S409の後に行われてもよいし、S411の後に行われてもよい。また、図6の処理を実行する度にS407、S408を実行する必要はなく、図6の処理を複数回(たとえば10回)実行するうちの9回は、S406に続いてS409を実行し、1回のみ、S406に続いてS407、S408を実行するようになっていてもよい。
【0095】
また、上記実施形態では、タイトカップリング型カルマンフィルタで用いる観測量として、使用対象の複数のGPS衛星100のそれぞれについて、当該GPS衛星の観測擬似距離と、当該自立センサ20の出力値に基づいて推測航法によって算出される車両から当該GPS衛星までの推測擬似距離との残差、および、GPS受信装置10から出力される当該GPS衛星の観測ドップラー周波数と、自立センサ20の出力値に基づいて推測航法によって算出される当該GPS衛星の推測ドップラー周波数との残差を採用している。
【0096】
しかし、タイトカップリング型カルマンフィルタで用いる観測量としては、必ずしもこのようなものに限らず、例えば、観測擬似距離と観測ドップラー周波数の関数f1(PR(n)GPS,DP(n)GPS)と、同じ関数型の観測擬似距離と観測ドップラー周波数の関数f1(PR(n)DR,DP(n)DR)との残差を採用してもよい。
【0097】
つまり、タイトカップリング型カルマンフィルタで用いる観測量としては、使用対象の複数のGPS衛星100の毎の、当該GPS衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をGPS衛星からの信号に基づいて算出した値と推測航法で算出した値の乖離量(例えば差の自乗、差の絶対値)を採用するようになっていればよい。
【0098】
したがって、図7のS451で閾値THと比較する量は、必ずしも上記のようなカイ自乗値χに限らず、使用対象の複数のGPS衛星100毎の上記乖離量のいずれかが大きくなるほど大きくなる指標であれば、どのようなものであってもよい。
【0099】
また、上記の実施形態において、制御回路17がプログラムを実行することで実現している各機能は、それらの機能を有するハードウェア(例えば回路構成をプログラムすることが可能なFPGA)を用いて実現するようになっていてもよい。
【符号の説明】
【0100】
10 GPS受信装置
11 GPSアンテナ部
12 GPS−RF部
13 GPS信号処理部
14 GPS測位演算部
20 自立センサ
21 車速センサ
22 ジャイロスコープ
30 地図データ記憶装置
40 ディスプレイ部
50 制御部
100 GPS衛星

【特許請求の範囲】
【請求項1】
車両に搭載される自立センサ(20)の出力値に基づいて推測航法により前記車両の位置および方位を推定すると共に、推測航法における誤差を状態量としたカルマンフィルタを用いて、各誤差の推定値を求め、求めた推定値に基づいて前記車両の位置および方位を補正する車両用軌跡推定装置であって、
前記カルマンフィルタは、観測量として、使用対象の複数のGPS衛星毎に、当該GPS衛星と車両との相対距離および相対速度のいずれか一方または両方に基づく量について、その量をGPS衛星からの信号に基づいて算出した値とその量を推測航法で算出した値との乖離量を採用するタイトカップリング型カルマンフィルタであり、
当該車両用軌跡推定装置は、前記タイトカップリング型カルマンフィルタにおける前記使用対象の複数のGPS衛星毎の前記乖離量のいずれかが大きくなるほど大きくなる指標と所定の閾値とを比較する比較手段(S451)と、
前記指標が前記所定の閾値以上であると前記比較手段(S451)が判定した場合、前記使用対象の複数のGPS衛星毎の前記乖離量に基づいて、前記使用対象の複数のGPS衛星のうち一部を選び、選んだ前記一部を使用対象から除外する除外手段(S452)と、を備えた車両用軌跡推定装置。

【図1】
image rotate

【図2】
image rotate

【図3】
image rotate

【図4】
image rotate

【図5】
image rotate

【図6】
image rotate

【図7】
image rotate

【図8】
image rotate


【公開番号】特開2012−63313(P2012−63313A)
【公開日】平成24年3月29日(2012.3.29)
【国際特許分類】
【出願番号】特願2010−209522(P2010−209522)
【出願日】平成22年9月17日(2010.9.17)
【出願人】(000004260)株式会社デンソー (27,639)
【Fターム(参考)】