説明

車両用現在位置検出装置及びプログラム

【課題】角速度センサのゲイン誤差の推定精度を向上させる。
【解決手段】車両の角速度を検出するジャイロ6のゲイン誤差を状態量としてその推定値をカルマンフィルタにより求める誤差推定部15と、ジャイロ6による検出値のゲイン補正に用いるゲイン補正量を、誤差推定部15により求められたゲイン誤差に基づき修正する補正部16とを備える。ここで、補正部16は、車両の旋回方向が右旋回と判定されている状態では右旋回専用のゲイン補正量をゲイン誤差に基づき修正し、左旋回と判定されている状態では左旋回専用のゲイン補正量をゲイン誤差に基づき修正する。

【発明の詳細な説明】
【技術分野】
【0001】
本発明は、角速度センサのゲイン誤差の推定値をカルマンフィルタにより求める車両用現在位置検出装置に関するものである。
【背景技術】
【0002】
従来、車両の現在位置を検出するカーナビゲーション装置等の車両用現在位置検出装置では、GPSに代表される人工衛星を利用した測位用受信機と、角速度センサとしてのジャイロスコープや車速センサ等に代表される自立センサを組み合わせて推測航法軌跡を生成し、推測航法軌跡と地図データを照合(マップマッチング)して地図上における自車位置を特定する。
【0003】
一般に、こうした車両用現在位置検出装置では、ジャイロスコープが検出する車両の角速度と車速センサが検出する移動距離から車両の運動ベクトルを生成し、それを積分することにより車両の走行軌跡を推定する。しかしながら、ジャイロスコープや車速センサは誤差を有するため、走行軌跡が正しく推定できない問題がある。具体的な誤差要因としては、ジャイロスコープのオフセット誤差やゲイン誤差により発生する角速度誤差、タイヤ径の変化による距離係数の誤差などが挙げられる。
【0004】
例えば特許文献1には、オフセット誤差を推定後、車両走行状態(右旋回、直進、左旋回)を判別し、それぞれの状態ごとにGPS測位結果を直接利用してゲイン補正を実施することで、左右ゲイン誤差を推定する構成が示されている。しかしながら、このような構成では、GPS測位結果の誤差の影響により誤差を生じやすいという問題がある。
【0005】
そこで、このような誤差を状態量としてその推定値をカルマンフィルタにより求める構成が提案されている(特許文献2参照)。このような構成であれば、GPS測位結果に誤差が含まれていても、車両の現在位置を適切に検出することが可能となる。
【特許文献1】特開平10−246642号公報
【特許文献2】特開2000−55678号公報
【発明の開示】
【発明が解決しようとする課題】
【0006】
ところで、ジャイロスコープは、図14のグラフに示されるように、あらかじめ定められた検出軸回りの回転角速度に比例した出力(電圧等)を発生するように構成されている。このため、ジャイロスコープの出力から回転角速度への換算比率(すなわちグラフの傾き)を表すためにあらかじめ設定された比例定数(換算ゲイン)を用いて、ジャイロの出力を回転角速度に変換し、この変換値を積算することにより、車両の方位変化量を求めることができる。
【0007】
そして、前述した特許文献2に記載の構成では、図14に示すようにジャイロスコープが左右共通したゲイン特性を有する場合を対象にしている。すなわち、直進時も左右の旋回時も同じ方式で連続的にゲイン誤差の推定を行っている。
【0008】
しかしながら、現実のジャイロスコープは、図15に破線で示すように右旋回と左旋回とで異なるゲイン特性(左右感度差)を有することが一般的である。このため、前述した特許文献2に記載の構成では、左右旋回で角速度誤差が発生してゲイン誤差の推定精度が低くなるという問題がある。
【0009】
なお、カルマンフィルタで右旋回時のゲイン誤差と左旋回時のゲイン誤差とを別々に推定することも考えられるが、1変数増加することによりカルマンフィルタの処理が複雑となる問題がある。
【0010】
本発明は、こうした問題にかんがみてなされたものであり、角速度センサのゲイン誤差の推定精度を向上させることを目的としている。
【課題を解決するための手段】
【0011】
上記目的を達成するためになされた本発明の請求項1に記載の車両用現在位置検出装置では、誤差推定手段が、車両の角速度を検出する角速度センサのゲイン誤差を状態量としてその推定値をカルマンフィルタにより求める。そして、補正量修正手段が、角速度センサによる検出値のゲイン補正に用いるゲイン補正量を、誤差推定手段により求められたゲイン誤差に基づき修正する。
【0012】
具体的には、補正量修正手段は、車両の旋回方向を判定する旋回方向判定手段により右旋回と判定されている状態では右旋回専用のゲイン補正量をゲイン誤差に基づき修正し、左旋回と判定されている状態では左旋回専用のゲイン補正量をゲイン誤差に基づき修正する。
【0013】
つまり、本発明の車両用現在位置検出装置では、ゲイン補正に用いる補正量を車両の走行状態(旋回方向)に応じて時系列的に切り替えるようにしている。
このような車両用現在位置検出装置によれば、カルマンフィルタで右旋回時のゲイン誤差と左旋回時のゲイン誤差とを別々に推定することなく(従来法に比べ状態量の変数を増加させることなく)、旋回方向に応じたゲイン補正量を用いてゲイン補正を行うことができる。その結果、右旋回及び左旋回でゲイン補正量が共通の従来法に比べ、ゲイン誤差の推定精度を向上させることができる。
【0014】
ところで、カルマンフィルタにより求められる状態量の誤差共分散行列におけるゲイン誤差に対応する対角要素は、ゲイン誤差の誤差量を表す。ここで、例えば、車両走行状態で右旋回の頻度が大きい場合、右旋回専用のゲイン補正量は正確に推定できているが、左旋回専用のゲイン補正量は正確に推定できていないといった状態が生じ得る。このような状態で車両が左旋回した場合、左旋回専用のゲイン補正量と誤差共分散行列の値との整合がとれず、ゲイン補正が正しく行われないことが考えられる。
【0015】
そこで、請求項2に記載の車両用現在位置検出装置では、誤差推定手段は、旋回方向判定手段により右旋回と判定されている状態では、カルマンフィルタにより求められる状態量の誤差共分散行列におけるゲイン誤差に対応する対角要素として右旋回専用の値を用い、左旋回と判定されている状態では、対角要素として左旋回専用の値を用いる共分散値変更処理を行う。
【0016】
このような車両用現在位置検出装置によれば、旋回方向が一方に偏った車両走行状態においてもゲイン補正を正しく行うことができる。
ただし、共分散値変更処理は、同処理を行わない場合に比べると処理数が多くなる。
【0017】
そこで、請求項3に記載の車両用現在位置検出装置では、誤差推定手段は、右旋回専用のゲイン補正量又は左旋回専用のゲイン補正量が所定のしきい値以上である場合に共分散値変更処理を行う。
【0018】
このような車両用現在位置検出装置によれば、処理負荷を効率よく低減することができる。
また、請求項4に記載のように、誤差推定手段が、ゲイン誤差に対応する対角要素の値を更新した場合には、誤差共分散行列におけるゲイン誤差に対応する相互相関要素の値を0にすれば、処理を安定化させることができる。
【0019】
ところで、長時間旋回がされない場合には、その旋回方向のゲイン誤差が増大する可能性がある。
そこで、請求項5に記載の車両用現在位置検出装置は、旋回方向判定手段により右旋回と判定されていない時間の経過に応じて右旋回専用の対角要素の値を増大させ、旋回方向判定手段により左旋回と判定されていない時間の経過に応じて左旋回専用の対角要素の値を増大させる。
【0020】
このような車両用現在位置検出装置によれば、長時間旋回がされないことによりその旋回方向のゲイン誤差が増大した場合にも、それに応じたゲイン補正量に調整することが可能となる。
【0021】
次に、請求項6に記載のプログラムは、車両の角速度を検出する角速度センサを備える車両用現在位置検出装置に搭載されたコンピュータを、角速度センサのゲイン誤差を状態量としてその推定値をカルマンフィルタにより求める誤差推定手段と、角速度センサによる検出値のゲイン補正に用いるゲイン補正量を誤差推定手段により求められたゲイン誤差に基づき修正する補正量修正手段と、車両の旋回方向を判定する旋回方向判定手段として機能させるためのものである。ここで、補正量修正手段は、旋回方向判定手段により右旋回と判定されている状態では右旋回専用のゲイン補正量をゲイン誤差に基づき修正し、左旋回と判定されている状態では左旋回専用のゲイン補正量をゲイン誤差に基づき修正する。
【0022】
このようなプログラムによれば、請求項1の車両用現在位置検出装置としてコンピュータを機能させることができ、これにより前述した効果を得ることができる。
【発明を実施するための最良の形態】
【0023】
以下、本発明が適用された実施形態について、図面を用いて説明する。
[1.第1実施形態]
まず、第1実施形態のカーナビゲーション装置(以下、単に「ナビゲーション装置」という。)について説明する。
【0024】
[1−1.全体構成]
図1は、第1実施形態のナビゲーション装置2の概略構成を表すブロック図である。
同図に示すように、このナビゲーション装置2は、車速センサ4と、ジャイロスコープ(以下、単に「ジャイロ」という。)6と、GPS受信機8と、現在位置検出部10と、ナビゲーション実行部20とを備えている。
【0025】
ここで、車速センサ4は、車両の走行速度を検出するためのセンサであり、車速に応じた間隔でパルス信号を出力する。
また、ジャイロ6は、車両の角速度(方位変化量)を検出するためのセンサであり、車両に加わる回転運動の角速度に応じた検出信号を出力する。
【0026】
また、GPS受信機8は、GPS(Global Positioning System)用の人工衛星からの送信電波をGPSアンテナを介して受信し、車両の位置、方位(進行方向)、速度等を検出する。
【0027】
また、現在位置検出部10は、車速センサ4、ジャイロ6、GPS受信機8からの出力に基づいて車両の現在位置や進行方向等、推測航法を行うためのデータを検出する。
また、ナビゲーション実行部20は、現在位置検出部10での検出結果に基づき、表示画面(図示せず)の地図上への自車両の位置の表示や、設定された目的地までの経路案内等を行う。
【0028】
このうち、現在位置検出部10及びナビゲーション実行部20は、マイクロコンピュータを中心に構成された電子制御装置(ECU)の処理として実現される。具体的には、現在位置検出部10は、移動距離演算部11と、方位変化量演算部12と、相対軌跡演算部13と、絶対位置演算部14と、誤差推定部15と、補正部16とを備えている。
【0029】
ここで、移動距離演算部11は、車速センサ4からのパルス信号に基づいて車両の移動距離を算出する。
また、方位変化量演算部12は、ジャイロ6からの検出信号に基づいて方位変化量を算出する。
【0030】
また、相対軌跡演算部13は、移動距離演算部11で算出された移動距離と方位変化量演算部12で算出された方位変化量とに基づいて、相対軌跡及び車速を算出する。
また、絶対位置演算部14は、相対軌跡演算部13と同じく移動距離演算部11で算出された移動距離と方位変化量演算部12で算出された方位変化量とに基づいて、絶対方位及び絶対位置を算出する。
【0031】
また、誤差推定部15は、カルマンフィルタからなるものであり、相対軌跡演算部13及び絶対位置演算部14での算出値とGPS受信機8での検出値との差を観測値とし、車速、方位、位置の算出に使用する各種算出パラメータや算出結果の誤差を状態量として、その状態量の推定値を求める。
【0032】
また、補正部16は、誤差推定部15により算出された状態量(すなわち誤差)の推定値に従って、各演算部11〜14での算出パラメータや算出値を補正する。
[1−2.カルマンフィルタ]
本実施形態のナビゲーション装置2では、推測航法とGPSとの複合化を図るため、カルマンフィルタを用いている。ここで、誤差推定部15を構成するカルマンフィルタの概要について説明する。
【0033】
[1−2−1.カルマンフィルタの概要]
カルマンフィルタにおいては、推定したい誤差信号を状態量xとする。状態量は次の状態方程式により更新される。
【0034】
【数1】

さらに、観測過程を次のように示す。
【0035】
【数2】

なお、式(1)及び式(2)の関係をシグナルフローグラフで表すと図2のようになる。
【0036】
[1−2−2.観測信号から状態量の推定]
カルマンフィルタでは、観測信号を用いて次のような巡回計算により状態量を最小二乗推定する。
【0037】
【数3】

ただし、G(t)はカルマンゲイン、P(t|t−1)は状態量xの誤差共分散の予測値、P(t|t)は誤差共分散、V(t)は雑音v(t)の共分散行列、W(t)は雑音w(t)の共分散行列を表す。
【0038】
カルマンフィルタでは、推測航法における誤差を状態量として、観測信号から推定する。この誤差の推定値を推測航法にフィードバックして補正することにより、式(5)に示す事前推定値x(t|yt-1)を零と仮定できる。したがって、式(6)は次の式(8)のように変形できる。
【0039】
【数4】

[1−3.推測航法のモデル化とカルマンフィルタの定式化]
次に、推測航法のモデル化とカルマンフィルタの定式化について説明する。
【0040】
本実施形態では、カルマンフィルタを推測航法へ適用している。ここでは、カルマンフィルタ定式化のための推測航法のモデル化、それによる状態量と観測信号の設定、及び、状態方程式と観測方程式の設定について説明する。
【0041】
車両のセンサが出力する情報に基づき車両の位置・方位・車速を求める推測航法は、車速センサ4、ジャイロ6、相対軌跡演算部13、絶対位置演算部14の4つの部位から構成される。
【0042】
[1−3−1.状態量生成過程のモデル化]
推測航法のカルマンフィルタは、状態量xとして以下の6つの誤差を用いる。
【0043】
【数5】

ただし、θTは真の絶対方位、Dは前回からの方位変化量、Lは前回からの移動距離、Tは前回からの経過時間を表す。また、θは真の絶対方位θTにセンサ誤差が加わったものであり、方位変化量から求められる絶対方位θとする。
【0044】
式(9)から式(14)の各式を状態量で偏微分すると、状態量生成過程は次のように表される。
【0045】
【数6】

ε0はオフセット雑音(温度ドリフト等によるオフセットの変動分)、ε1は、ゲイン雑音(温度ドリフト等によるジャイロゲインの変動分)、ε2は絶対方位雑音(ジャイロのクロスカップリング等による変動分)、ε3は距離係数雑音(速度センサの経年変化による変動分)を意味する。
【0046】
[1−3−2.観測過程のモデル化]
観測値は推測航法の出力と、GPSの出力の差により求める。それぞれの出力には誤差が含まれるため、観測値において推測航法の誤差とGPSの誤差の和が得られる。
【0047】
この観測値yと状態量xを関係付け、以下のようにモデル化する。
【0048】
【数7】

ここで、添字のDRtは時刻tにおいて車速センサ、ジャイロからの信号に基づく推測航法にて求められた値を意味し、GPStは時刻tにおいてGPSから出力される値を意味する。
【0049】
[1−3−3.観測信号の設定]
式(16)におけるεθDRt−εθGPStは、推測航法により求められた絶対方位とGPSから出力される方位との差、つまり推測航法により求められた絶対方位には真の絶対方位と、誤差εθDRtが含まれており、GPSから出力される方位には真の絶対方位と、誤差εθGPStが含まれているため、これらの差を取ることにより、εθDRt−εθGPStが得られる。
【0050】
同様に、εKDRt−εKGPStは、推測航法により求められる速度とGPSから出力される速度との差から求まる距離係数誤差である。また、εYDRt−εYGPSt、εXDRt−εXGPStは、それぞれ推測航法により求められる絶対位置とGPSから出力される絶対位置とのX成分、Y成分の誤差の差である。
【0051】
[1−3−4.観測過程で発生する雑音]
式(16)における、観測過程で発生する雑音vはGPSの雑音であり、以下のように定義される。
【0052】
すなわち、GPSにおける疑似距離の計測誤差(UERE)とHDOP(Horizontal Dilution of Presision)の関係により測位精度が、UERE×HDOPで求められ、この測位精度を2乗することにより、εYGPSt、εXGPStが求められる。
【0053】
また、ドップラー周波数の計測誤差とHDOPの関係より速度精度が、ドップラー周波数の計測誤差×HDOPで求められ、速度精度/車速にて距離係数誤差が求められ、これを2乗することによりεKGPStが求められる。
【0054】
さらに、車両の速度Vcと速度精度から方位精度がtan-1(速度精度/Vc)で求められ、この方位精度を2乗することによりεθGPStが求められる。よって、カルマンフィルタでの演算を実行することにより、状態量生成過程にて定義された6つの誤差値による状態量xが求められ、これらにより車速センサの距離係数補正、ジャイロのオフセット補正、ゲイン補正、更には絶対位置補正、絶対方位補正が行われる。
【0055】
[1−4.現在位置検出部が実行する処理]
次に、現在位置検出部10が一定周期TMで繰り返し実行する推測航法処理について、図3のフローチャートを用いて説明する。なお、この処理は、電子制御装置のCPUがROMに記憶されているプログラムに従い実行することにより実現される。
【0056】
この推測航法処理が開始されると、まずS100では、移動距離演算部11及び方位変化量演算部12に相当する方位変化量・移動距離演算処理を実行する。
この処理の詳細を図4に沿って説明する。
【0057】
すなわち、S110では、ジャイロ6により検出されたジャイロ出力角速度にメインルーチンの起動周期TMを乗じることにより方位変化量を算出する。
方位変化量=ジャイロ出力角速度×TM …式(17)
なお、ジャイロ出力角速度は、図14のグラフから明らかなように、ジャイロ6の出力をサンプリングした値から、オフセット値(図14では2.5V)を減じたものに、変換ゲイン(グラフの傾きに相当)を乗じることにより算出されたものである。ここでは、変換ゲインとして、ジャイロ6のセンサ仕様で定義された理想値を設定する。
【0058】
続くS120では、後述するオフセット補正量にメインルーチンの起動周期TMを乗じたものを、S110にて求めた方位変化量から減じることにより、方位変化量のオフセット補正を行う。
【0059】
方位変化量=方位変化量−オフセット補正量×TM …式(18)
すなわち、図5に示すように、角速度0[deg/sec]に対応するジャイロ6の出力電圧(ゼロ点)の変動に基づく誤差を補正する。
【0060】
続くS130〜S150では、S120にてオフセット補正された方位変化量に、後述するゲイン補正量を乗じることにより、方位変化量のゲイン補正を行う。すなわち、図6に示すように、グラフの傾き(変換ゲイン)の変動に基づく誤差を補正する。
【0061】
本実施形態では、ゲイン補正量(ゲイン補正係数)として、旋回方向に応じて異なる2つのゲイン補正量を用いる。具体的には、右旋回時には右旋回専用のゲイン補正量(以下「右ゲイン補正量」という。)εS_rightを用い、左旋回時には左旋回専用のゲイン補正量(以下「左ゲイン補正量」という。)εS_leftを用いる。
【0062】
すなわち、まずS130で、車両の方位変化(角速度)が0以上であるか否かを判定する。なお、角速度が正の場合に右旋回、負の場合に左旋回と判定する。
そして、S130で車両の方位変化が0以上(右旋回)であると判定した場合には、S140へ移行し、右ゲイン補正量に基づきゲイン補正を実施する。
【0063】
方位変化量=方位変化量×εS_right(t) …式(19a)
一方、S130で車両の方位変化が0未満(左旋回)であると判定した場合には、S150へ移行し、左ゲイン補正量に基づきゲイン補正を実施する。
【0064】
方位変化量=方位変化量×εS_left(t) …式(19b)
そしてS160では、本処理が前回起動されてから今回起動されるまでの間に検出された車速センサ4からの出力パルス数(車速センサパルス数)に、後述する距離係数を乗じることにより移動距離を算出して、方位変化量・移動距離演算処理を終了する。
【0065】
移動距離=車速センサパルス数×距離係数 …式(20)
図3に戻って、続くS200では、相対軌跡演算部13に相当する相対軌跡演算処理を実行する。
【0066】
この処理の詳細を図7に沿って説明する。
すなわち、S210では、先のS140又はS150にて算出した方位変化量を、それまでに求められている相対方位に加算することにより、相対方位を更新する。
【0067】
相対方位=相対方位+方位変化量 …式(21)
続くS220では、この更新した相対方位、及び先のS160にて算出した移動距離に基づき、相対位置座標の更新を行う。具体的には、南北方向をx座標軸とした相対座標rel.x、東西方向をy座標軸とした相対座標rel.yを、式(22)に従って更新する。ただし、θsはS210にて算出した相対方位である。
【0068】
rel.x=rel.x+移動距離×cos(θs)
rel.y=rel.y+移動距離×sin(θs) …式(22)
すなわち、この更新は、移動距離に対する相対方位のX,Y成分をそれまでの相対位置座標に加算することにより行う。この相対位置座標は相対軌跡を求めるために算出するもので、その相対軌跡と道路形状との関係により、いわゆるマップマッチングが行われる。
【0069】
そしてS230では、S160にて算出した移動距離を、メインルーチンの起動周期TMで除することにより車速を算出して、相対軌跡演算処理を終了する。
車速=移動距離/TM …式(23)
図3に戻って、続くS300では、絶対位置演算部14に相当する絶対方位・絶対位置演算処理を実行する。
【0070】
この処理の詳細を図8に沿って説明する。
すなわちS310では、先のS140又はS150にて算出した方位変化量を、それまでに求められている絶対方位に加算することにより、絶対方位を更新する。
【0071】
絶対方位=絶対方位+方位変化量 …式(24)
続くS320では、この更新した絶対方位、及び先のS160にて算出した移動距離に基づき、絶対位置座標の更新を行う。具体的には、南北方向をx座標軸とした絶対座標abs.x、東西方向をy座標軸とした絶対座標abs.yを、式(25)に従って更新する。ただし、θはS310にて算出した絶対方位である。
【0072】
abs.x=abs.x+移動距離×cos(θ)
abs.y=abs.y+移動距離×sin(θ) …式(25)
すなわち、この更新は、移動距離に対する絶対方位のX,Y成分をそれまでの絶対位置座標に加算することにより行う。このようにして更新した絶対方位と絶対位置は後述するGPSとの複合化処理にて利用される。
【0073】
図3に戻って、最後にS400では、誤差推定部15及び補正部16に相当するGPSとの複合化処理を実行する。
この処理の詳細を図9に沿って説明する。
【0074】
まず、S410では、後述するGPS測位データに基づくカルマンフィルタの処理(S430〜S520)又は予測計算処理(S530,S540)が前回実行されてからT1秒経過したか否かを判定し、否定判定された場合にはS420に移行する。
【0075】
S420では、GPS受信機8が測位を行うことによりGPS受信機8からGPS測位データが入力されたか否かを判断し、肯定判定された場合には、S430に移行して、カルマンフィルタによる処理を行い、一方、否定判定された場合はそのまま本処理を終了する。
【0076】
そして、S430では、GPS受信機8からのGPS測位データ(速度、位置、方位)、及び先の推測航法処理により算出した推測航法データ(車速(S230)、絶対位置(S320)、絶対方位(S310))に基づいて観測値y(εKDRt−εKGPSt、εθDRt−εθGPSt、εYDRt−εYGPSt、εXDRt−εXGPSt)の計算を行う。これとともに、観測過程で発生する雑音w(t)をGPS受信機8のGPS測位データ等に基づき計算する。
【0077】
続くS440では、状態遷移行列Aの計算を行う。これは、前回のプロセス行列の計算時点からの移動距離L、経過時間T(これらは図示しない計測処理により別途求められている)、及びS310にて求めた絶対方位θにより、式(15)に示す状態遷移行列Aを求める。
【0078】
そして、このようにして計算した観測値y及び状態遷移行列Aに基づき状態量xを求める。
すなわち、S450では式(3)により誤差共分散Pの予測計算を行い、続くS460では式(4)によりカルマンゲインGの計算を行い、続くS470では式(7)により誤差共分散Pの計算を行い、続くS480では、カルマンゲインG及び観測値yに基づき状態量xを求める。この状態量xは、式(15)の左辺に示された、オフセット誤差εG、εS、絶対方位誤差εθ、距離係数誤差εK、絶対位置北方向誤差εY、絶対位置東方向誤差εXを表している。
【0079】
そして、S490〜S520では、状態量xとして算出したこれらの誤差値εG,εS,εK,εθ,εY,εXに基づき、推測航法誤差の補正(ジャイロ6のオフセット補正、同じくゲイン補正、車速センサ4の距離係数補正、絶対方位補正、絶対位置補正)を行う。
【0080】
オフセット補正量=オフセット補正量−εG …式(26)
距離係数=距離係数×(1−εK) …式(27)
絶対方位=絶対方位−εθ …式(28)
abs.y(絶対位置)=abs.y−εY
abs.x(絶対位置)=abs.x−εX …式(29)
すなわち、S490では、式(26)のジャイロ6のオフセット補正により、S120にて用いられるオフセット補正量を修正し、式(27)の車速センサ4の距離係数補正により、S160にて用いられる距離係数を修正し、式(28)の絶対方位補正により、S310にて用いられる絶対方位θを修正し、式(29)の絶対位置補正により、S320にて用いられる絶対位置を修正する。
【0081】
さらに、S500〜S520では、状態量xとして算出した誤差値εSに基づき、推測航法誤差のゲイン補正を行う。
具体的には、まずS500で、車両の方位変化が0以上であるか否か(つまり右旋回中であるか否か)を判定する。なお、この判定は、前述したS130の判定結果をそのまま利用してもよい。
【0082】
そして、S500で車両の方位変化が0以上(右旋回)であると判定した場合には、S510へ移行し、式(30a)のゲイン補正により、S140にて用いられるゲイン補正量を修正する。
【0083】
εS_right(t+1)
=εS_right(t)×(1−εS) …式(30a)
一方、S500で車両の方位変化が0未満(左旋回)であると判定した場合には、S520へ移行し、式(30b)のゲイン補正により、S150にて用いられるゲイン補正量を修正する。
【0084】
εS_left(t+1)
=εS_left(t)×(1−εS) …式(30b)
そして、上記S430〜S520の処理を、GPS受信機8からのGPS測位データがあるごとに繰り返し行い、左右ゲインを独立して推定することにより誤差修正を行って、より正確な推測航法データを得ることができる。
【0085】
また、GPS受信機8からGPS測位データが長時間(T1秒以上)得られなかった場合(先のS410にて肯定判定されると)、S530,S540に移行して、状態遷移行列Aの計算及び誤差共分散Pの予測計算を行う。これは、GPS受信機8からGPS測位データが得られない場合、何も補正を行わないと誤差が大きくなってしまうため、誤差共分散Pの見積もりだけは定期的に行っておくことにより、その後にGPS受信機8からのGPS測位データが得られた時に行われるカルマンフィルタの処理を正確に行うことができるようにするためのものである。
【0086】
[1−5.シミュレーション]
次に、本実施形態のナビゲーション装置2によるシミュレーション結果について説明する。
【0087】
走行条件として、0km/hから45km/hへ加速し、その後定速で左右の旋回を交互に2回繰り返すように設定した。また、ジャイロ6のゲインは、左方向1.1倍、右方向0.9倍と設定した。
【0088】
従来法(右旋回及び左旋回のゲイン補正量が共通)におけるジャイロ6のオフセット誤差と補正量との関係、ゲイン誤差と推定値との関係の結果を図10に、本実施形態における結果を図11に示す。なお、グラフにおいて、ブレの少ない方が実際の値(実際のオフセット誤差、実際のゲイン)である。
【0089】
従来法では直進時のゲイン推定のずれの影響を旋回時に受け、正しいゲイン推定ができていないが、本実施形態では直進時にゲイン補正量をリセットし、更に旋回開始時に、前回の旋回時に保存していたゲイン補正量を用いることにより、うまくゲイン推定できている。また、従来法ではオフセット誤差とゲイン誤差の分離がうまくできておらず、旋回時のゲイン推定のずれをオフセット誤差として認識してしまい、旋回時にオフセット補正量が大きく真値からずれてしまっていたが、本実施形態では改善され、旋回時のオフセット補正量のずれをかなり抑制することができた。
【0090】
[1−6.効果]
以上説明したように、第1実施形態のナビゲーション装置2においては、ジャイロ6のゲイン誤差をカルマンフィルタにより推定して、ジャイロ6の出力に基づいて算出される方位変化量をゲイン補正するようにされている。したがって、このナビゲーション装置2によれば、ジャイロ6の経年変化があったり、ジャイロ6が傾斜した状態で車両に取り付けられる等して、あらかじめ設定された換算ゲインが実際の換算比率と異なってしまった場合でも、これに基づく誤差を補償でき、方位変化量、ひいては車両の現在位置や進行方向(方位)を精度よく求めることができる。
【0091】
特に、このナビゲーション装置2では、ジャイロ6が左右異なるゲイン誤差を持つ場合において、方位変化時のゲイン補正量を保存し、再び同じ方向に方位変化したときに、保存したゲイン補正量を初期値として推定する。具体的には、補正部16が、車両の旋回方向が右旋回と判定されている状態では右ゲイン補正量をゲイン誤差に基づき修正し、左旋回と判定されている状態では左ゲイン補正量をゲイン誤差に基づき修正する。つまり、ゲイン補正に用いるゲイン補正量を車両の走行状態(旋回方向)に応じて時系列的に切り替えるようにしている。
【0092】
このようなナビゲーション装置2によれば、カルマンフィルタで右旋回時のゲイン誤差と左旋回時のゲイン誤差とを別々に推定することなく(従来法に比べ状態量の変数を増加させることなく)、旋回方向に応じたゲイン補正量を用いてゲイン補正を行うことができる。その結果、右旋回及び左旋回のゲイン補正量が共通の従来法に比べ、ゲイン誤差の推定精度を向上させることができる。
【0093】
[1−7.特許請求の範囲との対応]
なお、第1実施形態では、S410〜S480,S530,S540の処理を実行する現在位置検出部10(具体的には誤差推定部15)が、誤差推定手段に相当し、S490,S510,S520の処理を実行する現在位置検出部10(具体的には補正部16)が、補正量修正手段に相当し、S500の処理を実行する現在位置検出部10(具体的には補正部16)が、旋回方向判定手段に相当する。
【0094】
[2.第2実施形態]
次に、第2実施形態のナビゲーション装置について説明する。
第2実施形態のナビゲーション装置では、カルマンフィルタの誤差共分散についても右旋回と左旋回とで独立化する。
【0095】
すなわち、誤差共分散行列の対角要素は、カルマンフィルタで推定する変数の誤差量を示す。例えば、車両走行状態で右旋回の頻度が大きい場合、右ゲイン補正係数εS_rightは正確に推定できているが、左ゲイン補正係数εS_leftは、まだ正確に補正できていないことになる。このような状態で左旋回した場合、第1実施形態の処理では、左ゲイン補正係数εS_leftと誤差共分散の値の整合がとれず、ゲイン補正が正しく行われない事象が発生し得る。
【0096】
そこで、第2実施形態のナビゲーション装置では、右ジャイロゲイン誤差の共分散Ps_rightと、左ジャイロゲイン誤差の共分散Ps_leftとを定義する。Psは誤差共分散行列P(t|t)のゲイン誤差に対応する対角成分である(式(7)、図13参照)。
【0097】
このため、第2実施形態のナビゲーション装置は、第1実施形態のナビゲーション装置と比較すると、図9に示したGPSとの複合化処理に代えて、図12に示すGPSとの複合化処理を行う点のみが異なる。その他、共通する部分については、同一符号を用いるとともに説明を省略する。
【0098】
第2実施形態のナビゲーション装置により実行されるGPSとの複合化処理(図12)は、第1実施形態のGPSとの複合化処理(図9)と対比すると、S610〜S640,S680〜S740,S760,S780,S790の各処理は、S410〜S540の各処理と同一である。つまり、S650〜S670,S750,S770の処理が追加された点のみが異なる。そこで、追加されたこれらの処理についてのみ説明する。
【0099】
S650〜S670では、誤差共分散行列Pにおけるゲイン誤差に対応する対角要素として、右旋回専用の値Ps_right及び左旋回専用の値Ps_leftのいずれか一方を選択する。
【0100】
具体的には、まずS650で、車両の方位変化が0以上であるか否か(つまり右旋回中であるか否か)を判定する。なお、この判定は、前述したS130の判定結果をそのまま利用してもよい。
【0101】
そして、S650で車両の方位変化が0以上(右旋回)であると判定した場合には、S660へ移行し、誤差共分散の対角成分のうちゲインに対応する値を右旋回専用の値Ps_rightに更新する。つまり、式(3)のP(t−1|t−1)のゲイン誤差に対応する対角要素をPs_rightに置き換える。
【0102】
一方、S650で車両の方位変化が0未満(左旋回)であると判定した場合には、S670へ移行し、誤差共分散の対角成分のうちゲインに対応する値を左旋回専用の値Ps_leftに更新する。つまり、式(3)のP(t−1|t−1)のゲイン誤差に対応する対角要素をPs_leftに置き換える。
【0103】
また、S750では、誤差共分散の対角成分のうちゲインに対応する値(この場合には右旋回専用の値Ps_right)を保持(メモリに記憶)する。つまり、式(7)のP(t|t)のゲイン誤差に対応する対角要素をPs_rightとして保持する。
【0104】
一方、S770では、誤差共分散の対角成分のうちゲインに対応する値(この場合には左旋回専用の値Ps_left)を保持(メモリに記憶)する。つまり、式(7)のP(t|t)のゲイン誤差に対応する対角要素をPs_leftとして保持する。
【0105】
なお、Ps_right及びPs_leftは、初期値は最大値に設定され、その後にカルマンフィルタの処理が繰り返されて精度が向上することに伴い値が小さくなる。また、車両のエンジン停止時等にも値は保持される。
【0106】
このような第2実施形態のナビゲーション装置2によれば、旋回方向が一方に偏った車両走行状態においてもゲイン補正を正しく行うことができる。
なお、第2実施形態では、S610〜S640,S660〜S710,S780,S790の処理を実行する現在位置検出部10(具体的には誤差推定部15)が、誤差推定手段に相当し、S720,S740〜S770の処理を実行する現在位置検出部10(具体的には補正部16)が、補正量修正手段に相当し、S650,S730の処理を実行する現在位置検出部10(具体的には補正部16)が、旋回方向判定手段に相当する。
【0107】
[3.他の形態]
以上、本発明の一実施形態について説明したが、本発明は、種々の形態を採り得ることは言うまでもない。
【0108】
例えば、上記第2実施形態のGPSとの複合化処理(図12)は、第1実施形態のGPSとの複合化処理(図9)に比べると処理数が多くなる。そこで、左右ゲイン誤差がまだ十分に推定できていない状態(例えば、右ゲイン補正量又は左ゲイン補正量が所定のしきい値以上である場合)には、図12のGPSとの複合化処理を行い、十分に推定できた状態(右ゲイン補正量及び左ゲイン補正量が所定のしきい値未満である場合)になった時点で、図9のGPSとの複合化処理に切り替えるようにしてもよい。さらに、GPS精度悪化等によって右ゲイン補正量又は左ゲイン補正量が大きくなり所定のしきい値以上となったら、再び図12のGPSとの複合化処理に切り替えるようにしてもよい。このようにすれば、処理負荷を効率よく低減することができる。
【0109】
また、S660,S670の処理でゲイン誤差に対応する対角要素の値を更新するごとに、図13に示すように、誤差共分散行列Pにおけるゲイン誤差に対応する相互相関要素の値を0にしてもよい。このようにすれば、処理を安定化させることができる。
【0110】
ところで、長時間右旋回又は左旋回が実施されない場合には、その旋回方向のゲイン誤差が増大する可能性がある。そこで、右旋回と判定されていない時間の経過に応じて右誤差共分散行列におけるゲイン誤差に対応する対角要素の値を増大させ、左旋回と判定されていない時間の経過に応じて左誤差共分散行列におけるゲイン誤差に対応する対角要素の値を増大させるようにしてもよい。具体的には、時間の経過に伴いある係数で徐々に増大させるようにする。このようにすれば、長時間旋回がされないことによりその旋回方向のゲイン誤差が増大した場合にも、それに応じたゲイン補正量に調整することが可能となる。
【0111】
また、上記実施形態では、右旋回及び左旋回のいずれであるかを判定するようにしているが、これに限定されるものではなく、例えば以下のステップ1〜3に基づく処理のように、直進状態についても判定するようにしてもよい。なお、ジャイロのゲインは直進に対してほぼ1であり、左旋回に対して1以上、右旋回に対して1以下である。
【0112】
すなわち、まずステップ1として、現在推定しているゲイン補正量を保存する。
続いて、ステップ2として、走行状態の変化を検出する。具体的には、過去3秒間の方位変化量(オフセット・ゲイン補正後の値)が10deg/secより小さい場合に直進と判定する。また、過去1秒間の方位変化量(オフセット・ゲイン補正前の値)が10deg/sec未満で、その時刻の方位変化量が10deg/secより大きい場合に直進→旋回と判定する。
【0113】
そして、ステップ3として、走行状態に応じて、直進の場合はゲイン=1にリセット、旋回の場合は、以前の同じ走行状態で保存してあったゲイン補正量を初期値として推定を行う。
【0114】
ところで、ジャイロ6の出力電圧にはノイズが含まれるため、上記実施形態のように右旋回/左旋回を判定する処理(S130等)においては、直進状態で左右切替え処理が頻発する状態が生じ得る。また、直進に近い走行状態では、ジャイロ6が検出する角速度が小さく、このような状態ではカルマンフィルタに入力される方位変化量も小さいため、カルマンフィルタゲイン補正の効果が得られにくく、また左右を取り違えて誤った補正を実施する可能性がある。このため、直線に近い走行状態においては、左右感度の補正を抑制することが好ましい。
【0115】
そこで、頻繁な切替えを抑制し、直線走行状態でも左右ゲイン誤差の推定を損なわないようにしてもよい。具体的には、図16に示すように、しきい値THr[rad]を定義し、旋回量が小さい(直進に近い)状態においては、推定した左右ゲイン誤差値を使用せずにデフォルトゲイン(理論値)を使用し、しきい値THr又はしきい値−THrを越える旋回量の場合に、左右感度を使用した軌跡計算、カルマンフィルタを用いた誤差推定を実施する。
【0116】
このような処理を実現するためのフローチャートを図17に示す。
まず、現在から過去Ta秒間の方位変化量ωaがしきい値THrを越えているか否かを判定し(S801)、超えていると判定した場合(右旋回と判定した場合)には(S801:YES)、右ゲイン補正係数(右ゲイン補正量)を選択する(S802)。続いて、現在から過去Tb秒間の方位変化量ωbがしきい値THrを下回るか否かを判定し(S803)、下回ると判定した場合には(S803:YES)、直線走行状態となったと判定してデフォルトゲインに切り替える(S804)。
【0117】
一方、方位変化量ωaがしきい値THrを越えていないと判定した場合には(S801:NO)、方位変化量ωaがしきい値−THrを下回るか否かを判定し(S805)、下回ると判定した場合(左旋回と判定した場合)には(S805:YES)、左ゲイン補正係数(左ゲイン補正量)を選択する(S806)。続いて、現在から過去Tb秒間の方位変化量ωbがしきい値−THrを越えているか否かを判定し(S807)、超えていると判定した場合には(S807:YES)、直線走行状態となったと判定してデフォルトゲインに切り替える(S808)。
【0118】
これに対し、方位変化量ωaがしきい値−THrを下回らないと判定した場合、つまり、方位変化量ωaがしきい値−THrからしきい値THrまでの間の値である場合には(S805:NO)、ジャイロ6の仕様や設計で一意に決まる理論値であるデフォルトゲインを使用する(S809)。つまり、デフォルトゲインが設定されている状態では、左右ゲイン誤差の推定は実施されない。
【0119】
なお、しきい値THrは、固定値でも動的に変更してもよい。動的に変更する場合は、誤差共分散P(t|t)(式(7))のジャイロオフセットに対応する要素の平方根を使用する。つまり、P(t|t)の(1,1)の平方根を使用する。例を下式に示す。なお、A、Bは任意の定数である。
【0120】
THr=A√(P(t|t)の(1,1))+B
P(t|t)の(1,1)が大きい場合、つまり、ジャイロオフセットがまだ正しく推定できていないときは、しきい値THrを大きく設定することを意味する。
【図面の簡単な説明】
【0121】
【図1】実施形態の装置の概略構成を表すブロック図である。
【図2】カルマンフィルタのシグナルフローグラフである。
【図3】推測航法処理のフローチャートである。
【図4】方位変化量・移動距離演算処理のフローチャートである。
【図5】ジャイロのオフセット補正の説明図である。
【図6】ジャイロのゲイン補正の説明図である。
【図7】相対軌跡演算処理のフローチャートである。
【図8】絶対方位・絶対位置演算処理のフローチャートである。
【図9】第1実施形態のGPSとの複合化処理のフローチャートである。
【図10】従来法におけるジャイロのオフセット誤差と補正量の関係、ゲイン誤差と推定値との関係のシミュレーション結果を示すグラフである。
【図11】本実施形態におけるジャイロのオフセット誤差と補正量の関係、ゲイン誤差と推定値との関係のシミュレーション結果を示すグラフである。
【図12】第2実施形態のGPSとの複合化処理のフローチャートである。
【図13】ゲイン誤差に対応する相互相関要素の値を0にした誤差共分散行列の説明図である。
【図14】ジャイロ出力と角速度との関係を示すグラフである。
【図15】左右異なるゲイン特性を有するジャイロのグラフである。
【図16】旋回量が小さい状態で理論値を用いる変形例の説明図である。
【図17】図16の変形例の処理を示すフローチャートである。
【符号の説明】
【0122】
2…カーナビゲーション装置、4…車速センサ、6…ジャイロ、8…GPS受信機、10…現在位置検出部、11…移動距離演算部、12…方位変化量演算部、13…相対軌跡演算部、14…絶対位置演算部、15…誤差推定部、16…補正部、20…ナビゲーション実行部

【特許請求の範囲】
【請求項1】
車両の角速度を検出する角速度センサと、
前記角速度センサのゲイン誤差を状態量としてその推定値をカルマンフィルタにより求める誤差推定手段と、
前記角速度センサによる検出値のゲイン補正に用いるゲイン補正量を、前記誤差推定手段により求められたゲイン誤差に基づき修正する補正量修正手段と、
を備える車両用現在位置検出装置において、
車両の旋回方向を判定する旋回方向判定手段を備え、
前記補正量修正手段は、前記旋回方向判定手段により右旋回と判定されている状態では右旋回専用のゲイン補正量を前記ゲイン誤差に基づき修正し、左旋回と判定されている状態では左旋回専用のゲイン補正量を前記ゲイン誤差に基づき修正すること
を特徴とする車両用現在位置検出装置。
【請求項2】
前記誤差推定手段は、前記旋回方向判定手段により右旋回と判定されている状態では、カルマンフィルタにより求められる状態量の誤差共分散行列における前記ゲイン誤差に対応する対角要素として右旋回専用の値を用い、左旋回と判定されている状態では、前記対角要素として左旋回専用の値を用いる共分散値変更処理を行うこと
を特徴とする請求項1に記載の車両用現在位置検出装置。
【請求項3】
前記誤差推定手段は、前記右旋回専用のゲイン補正量又は前記左旋回専用のゲイン補正量が所定のしきい値以上である場合に前記共分散値変更処理を行うこと
を特徴とする請求項2に記載の車両用現在位置検出装置。
【請求項4】
前記誤差推定手段は、前記対角要素の値を更新した場合には、前記誤差共分散行列における前記ゲイン誤差に対応する相互相関要素の値を0にすること
を特徴とする請求項2又は請求項3に記載の車両用現在位置検出装置。
【請求項5】
前記旋回方向判定手段により右旋回と判定されていない時間の経過に応じて前記右旋回専用の対角要素の値を増大させ、前記旋回方向判定手段により左旋回と判定されていない時間の経過に応じて前記左旋回専用の対角要素の値を増大させること
を特徴とする請求項2から請求項4までのいずれか1項に記載の車両用現在位置検出装置。
【請求項6】
車両の角速度を検出する角速度センサを備える車両用現在位置検出装置に搭載されたコンピュータを、
前記角速度センサのゲイン誤差を状態量としてその推定値をカルマンフィルタにより求める誤差推定手段と、
前記角速度センサによる検出値のゲイン補正に用いるゲイン補正量を前記誤差推定手段により求められたゲイン誤差に基づき修正する補正量修正手段と、
車両の旋回方向を判定する旋回方向判定手段
として機能させるためのプログラムであって、
前記補正量修正手段は、前記旋回方向判定手段により右旋回と判定されている状態では右旋回専用のゲイン補正量を前記ゲイン誤差に基づき修正し、左旋回と判定されている状態では左旋回専用のゲイン補正量を前記ゲイン誤差に基づき修正すること
を特徴とするプログラム。

【図1】
image rotate

【図2】
image rotate

【図3】
image rotate

【図4】
image rotate

【図5】
image rotate

【図6】
image rotate

【図7】
image rotate

【図8】
image rotate

【図9】
image rotate

【図10】
image rotate

【図11】
image rotate

【図12】
image rotate

【図13】
image rotate

【図14】
image rotate

【図15】
image rotate

【図16】
image rotate

【図17】
image rotate


【公開番号】特開2009−115545(P2009−115545A)
【公開日】平成21年5月28日(2009.5.28)
【国際特許分類】
【出願番号】特願2007−287454(P2007−287454)
【出願日】平成19年11月5日(2007.11.5)
【出願人】(000004260)株式会社デンソー (27,639)
【出願人】(504160781)国立大学法人金沢大学 (282)
【Fターム(参考)】