JP2008275530A - 位置検出装置及び位置検出方法 - Google Patents

Abstract

【目的】位置検出精度を維持しつつ、CPUの負荷を軽減する「位置検出装置及び位置検出方法」を提供することである。
【構成】車両の現在位置を検出する位置検出装置において、自律航法部は第１の周期で高速に自律航法に基づいて位置計算し、補正部は、該第１の周期以上の長さの第２の周期で自律航法位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角などを補正する第１補正処理を行ない、かつ、GPSの測位周期である第３の周期でGPSデータを用いて自律航法による計算結果を補正する第２補正処理を行う。周期制御部は、補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて少なくとも第１、第２周期を制御する。
【選択図】図１

Description

本発明は、車両の現在位置を検出する位置検出装置および位置検出方法に関わり、特に、処理装置の負荷を軽減しつつ自律航法により計算される位置データの精度を向上できる位置検出装置および位置検出方法に関する。

車載用ナビゲーション装置は、自律航法センサーを用いた自律航法（Dead Reckoning）とGPS（Global Positioning System）レシーバを用いたGPS航法を併用している。
自律航法は、車両の加速度を検出する加速度センサーや車両の方位変化量を検出する相対方位センサー（ジャイロ等）、車両の速度（距離）を検出する距離センサー（車速センサー等）の出力を用いて、車両の位置・方位・車速等を検出する方法である。しかし、自律航法の出力（位置・方位・車速等）には、センサーの誤差が含まれるため、誤差が生じる。特に、位置、方位はセンサー出力を積算して算出するため、誤差が徐々に累積する。一方、GPSは、最大位置誤差が通常環境で３０m程度で絶対的な位置、方位、車速を求めることができる。このため、GPS受信時に自律航法の出力を該GPSの出力に整合させることにより、累積により誤差が大きくなったときの補正が可能である。例えば、自律航法で得られた位置を周知のマップマッチング法により道路地図上の道路位置に自車位置を修正した時の位置と、GPSで得られた位置との差が所定値より大きくなった時に、道路地図上の位置をGPSで得られた位置に修正する。

ところで、自律航法は、上述のようにGPSの出力により補正することができるが、GPS非受信時においてセンサー出力の誤差、センサー取り付け誤差により自律航法の誤差が累積し出力精度が悪くなる問題が生じる。特に、立体駐車場や地下駐車場ではGPS信号が届かないため、１００m程の最大位置誤差が発生し、また、都心部では反射したGPS信号を受けることが多く、マルチパスが発生すると、３００ｍ程の最大位置誤差が発生する。
以上より、センサー出力の誤差を修正して現在位置を求める方法が提案されている。第１従来技術（特許文献１参照）は、自律推測航法から求められる車両の位置、方位、車速の情報とGPSから出力される車両の位置、方位、車速の情報により、カルマンフィルタにて、オフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差を求めて、自律航法におけるそれぞれの補正を行う。
第２従来技術（特許文献２参照）は、車両の前後方向加速度に対応した加速度信号を出力する加速度センサーと、車両の移動距離に対応した距離信号を出力する距離センサーと、カルマンフィルタ部を備え、該カルマンフィルタ部が加速度信号および距離信号に基づいてカルマンフィルタ処理を行い、車両の姿勢角（水平面に対するピッチ角）、速度を離散時刻毎に算出し、該姿勢角を用いて傾斜走行時における位置誤差を補正する。
第１従来技術はGPS受信時に前記自律航法におけるオフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差の補正をするものである。しかし、GPSの測位周期は１秒（１Hz）である。このため、１秒毎に上記補正を行うことになるが、補正周期が長すぎ、補正が不十分となり精度の高い位置検出ができない問題がある。また、第１従来技術は、二次元位置、二次元速度の４つのパラメータを用いるものであり、車両ピッチ角や自律航法用センサーの車両への取り付け角（車両に対する取り付けピッチ角、取り付けヨー角）の補正ができない問題がある。
第２従来技術は、三次元速度パラメータを用いて車両の姿勢角（水平面に対するピッチ角）、前後方向の速度を離散時刻毎に算出し、該姿勢角を用いて傾斜走行時における位置誤差を補正する。また、第２従来技術は、GPSの三次元位置データを用いて高さを含む位置誤差を補正する。しかし、第２従来技術の前者はGPSの三次元位置データを用いて位置誤差を補正するものではないため、誤差が累積して位置精度が落ちる問題がある。また、第２従来技術の後者はGPSから位置情報が得られる周期（１秒）毎に補正を行うことになるため、補正周期が長すぎて補正が不十分となり精度の高い位置検出ができない問題がある。また、第２従来技術は自律航法用センサーの取り付けヨー角の補正ができない問題がある。

このため、本発明者は、精度の高い位置検出を可能にする位置検出装置及び位置検出方法を提案している(特許文献３)。この提案方法によれば、(1)第１の周期で高速に自律航法により位置計算し、(2)該第１の周期より長い周期であって、GPSの測位周期より短い周期で自律航法の位置計算に使用するパラメータ(車両速度、ピッチ角、センサー取り付け姿勢角など)を補正する第１補正処理を行ない、(3)GPSの測位周期（第３の周期）毎にGPSデータを用いて自律航法による計算結果を補正する第２補正処理を行うことにより、精度の高い位置検出を可能にしている。
この提案方法によれば、第１周期、第２周期を短くするほど車両位置検出精度を向上することができるが、ナビゲーション制御用プロセッサ(CPU)の負荷増大し、ナビゲーション処理のための他の処理が実行できなくなる恐れがある。このため、位置検出精度を維持しつつ、CPUの負荷を軽減することが必要である。従来技術として、ナビゲーションにおける走行距離などの物理量を算出するために、短時間誤差推定手段と長時間誤差推定手段を備えた装置が提案されている(特許文献４)。しかし、この従来技術は、予め定めた時間ごとに短時間誤差推定と長時間誤差推定を行って物理量を算出するものであり、補正処理を行う処理部の負荷を軽減するための考慮、例えば補正処理に要する時間を短縮するための考慮がなされていない。
特開平８−６８６５５号公報 特開２００３−７５１７２号公報 特願２００７−５１１５２号 特許第３５８１３９２号

以上から、本発明の目的は、位置検出精度を維持しつつ、CPUの負荷を軽減することである。
本発明の別の目的は、位置検出精度を維持しつつ、自律航法の位置計算に使用するパラメータ(車両速度、ピッチ角、センサー取り付け姿勢角など)を補正する補正処理や自律航法演算に要する負荷を軽減することである。

・位置検出方法
本発明の第１の態様は車両の現在位置を検出する位置検出方法である。
第１の位置検出方法は、第１の周期で高速に自律航法により位置計算するステップ、該第１の周期以上の長さの第２の周期で自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角などを補正する第１補正処理を行なうステップ、GPSの測位周期である第３の周期でGPSデータを用いて自律航法による計算結果を補正する第２補正処理を行うステップ、補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて少なくとも前記第２周期の長さを制御するステップを有している。
第２の位置検出方法は、(1)車両の加速度や車両の方位変化量に応じた信号を出力する自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに車両移動距離検出部により検出された車両移動距離を用いて、自律航法部において第1の周期で緯度、経度、高さ方向の車両位置を計算すると共に、前記センサーから出力する加速度信号を用いて車両速度を計算する第１ステップ、(2)前記移動距離検出部の出力信号を用いて第１の周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した前記車両速度との速度差に基づいて、自律航法部で計算した車両速度、前記ピッチ角θ、前記センサーの取り付け姿勢角を補正する第２ステップ、(3)GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２の周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第３ステップ、(4)前記複数の補正対象のうち所定の補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて前記第２周期の長さを制御する第４ステップ、を備えている。
上記位置検出方法の前記第３ステップにおいて、カルマンフィルタ処理に基づいて前記補正対象の値を補正し、前記第４ステップにおいて該カルマンフィルタ処理の過程で演算される共分散行列の対角要素を用いて前記推定誤差を計算する。
また、上記位置検出方法の第４ステップにおいて、前記差分に基づいて前記第２周期の長さを制御すると共に、第１周期の長さを制御する。この場合、前記推定誤差が目標誤差より小さければ、前記第１、第２周期を長くし、該推定誤差が目標誤差より大きければ、前記第１、第２周期を短くする。
上記位置検出方法の第４ステップにおける前記目標誤差として、車両位置に対する目標誤差、あるいは車両速度に対する目標誤差、あるいは姿勢角に対する目標誤差を採用する。

・位置検出装置
本発明の第２の態様は車両の現在位置を検出する位置検出装置である。
第１の位置検出装置は、車両の移動距離を測定する移動距離検出部、車両の加速度を検出する加速度センサー、車両の方位変化量に応じた信号を出力する相対方位センサー、GPS衛星からの衛星電波を受信して緯度、経度、高さ方向の車両位置および車両速度情報を出力するGPSレシーバ、第１の周期で高速に自律航法により位置計算する自律航法部、該第１の周期以上の長さの第２の周期で自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角などを補正する第１補正処理を行なう第１の補正部、GPSの測位周期である第３の周期でGPSデータを用いて自律航法による計算結果を補正する第２補正処理を行う第２の補正部、補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて少なくとも前記第２周期の長さを制御する周期制御部、を備えている。
本発明の第２の位置検出装置は、車両の移動距離を測定する移動距離検出部、車両の加速度を検出する加速度センサー、車両の方位変化量に応じた信号を出力する相対方位センサー、GPS衛星からの衛星電波を受信して緯度、経度、高さ方向の車両位置および車両速度情報を出力するGPSレシーバ、第１の周期で、前記自律航法用のセンサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに前記移動距離を用いて緯度、経度、高さ方向の車両位置を計算すると共に、前記加速度センサーから出力する加速度信号を用いて車両速度を計算する自律航法部、前記移動距離検出部の出力信号を用いて第１周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した車両速度との速度差に基づいて前記自律航法部で計算している車両速度、前記ピッチ角θおよび前記センサーの取り付け姿勢角を補正する第１の補正部、前記GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第２の補正部、前記複数の補正対象のうち所定の補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて前記第２周期の長さを制御する周期制御部を備えている。
前記第２の補正部は、カルマンフィルタ処理に基づいて前記補正対象の値を補正し、前記周期制御部は該カルマンフィルタ処理の過程で演算される各補正対象の共分散行列の対角要素を用いて前記推定誤差を計算する。
前記周期制御部は、前記差分に基づいて前記第２周期の長さを制御すると共に、第１周期の長さを制御する。また、前記周期制御部は、前記推定誤差が目標誤差より小さければ、前記第１、第２周期を長くし、該推定誤差が目標誤差より大きければ、前記第１、第２周期を短くする。また、前記周期制御部は前記目標誤差として、車両位置に対する目標誤差、あるいは車両速度に対する目標誤差、あるいは姿勢角に対する目標誤差を採用する。

本発明によれば、所定の補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて、自律航法の位置計算周期や、自律航法の位置計算に使用するパラメータの補正処理周期を制御するため、これら自律航法位置計算やパラメータ補正処理に要する負荷を軽減することができる。
また、所定の補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて、自律航法の位置計算周期や、自律航法の位置計算に使用するパラメータの補正処理周期を制御するため、位置検出精度を維持しつつ、自律航法位置計算やパラメータ補正処理に要する負荷を軽減することができる。
また、本発明によれば、カルマンフィルタ処理に基づいて前記補正対象値を補正し、該カルマンフィルタ処理の過程で演算される各補正対象の共分散行列の対角要素を用いて推定誤差を計算するため、推定誤差を計算するための処理の増加を少なくできる。
また、本発明によれば、目標誤差として車両位置に対する目標誤差あるいは車両速度に対する目標誤差あるいは姿勢角に対する目標誤差などを適宜採用することができる。

(A) 本発明の概要
図１は本発明の位置検出装置の概要説明図であり、位置検出部１０および周期制御部３０を備えている。位置検出部１０において、自律航法部１２は自律センサー１１の出力信号を用いて第１の周期で高速に自律航法により車両位置を計算して出力する。補正部１５の第１補正部２１は該第１の周期より長い周期であって、GPSの測位周期より短い周期で自律航法の位置計算に使用するパラメータを補正し、第２の補正部２２はGPSの測位周期（第３の周期）毎にGPSデータを用いて自律航法による計算結果を補正する。周期制御部３０は目標誤差と推定誤差の差分に基づいて上記第１、第２の周期を制御する。
周期制御部３０において、推定誤差算出部３１は該カルマンフィルタ処理の過程で演算される共分散行列の対角要素を用い、差分演算部３２は、車両位置の該推定誤差と目標誤差(例えば１０ｍ)との差分を演算し、周期変更部３３は該差分に基づいて前記第１、第２周期を制御する。すなわち、周期変更部３３は推定誤差が目標誤差より小さければ、第１、第２周期を長くし、該推定誤差が目標誤差より大きければ、第１、第２周期を短くする。
図２は第２周期の制御例であり、最初、第２周期として１秒(１Hz)でゆっくりと第１の補正制御を行う。しかし、自律線センサーやGPSの検出精度及び車両の動き（Motion）の激しさなどにより、推定誤差が大きくなり目標誤差(＝１０m)を超えると、目標変更部３３は第２周期として０．２５秒(４Hz)で補正処理を行う。補正周期を短くしたことにより推定誤差が減少する。そして、第２周期として０．２５秒(４Hz)で補正処理を行うと検出精度が良すぎるため、目標変更部３３は第２周期として０．５秒(２Hz)で補正処理を行う。
以上のようにすれば、図２に示すように、目標誤差を満たしつつ、補正周期を長くして処理装置の負荷を減少することが可能になる。

(B)位置検出部
図３は本発明を適用できる位置検出部１０の詳細な構成図である。この位置検出部には、自律航法用センサーとして、車両の移動距離を測定する移動距離検出部、たとえば車両が所定距離走行する毎に１個のパルスを発生する車速センサー１１a、車両の方位変化量に応じた信号を出力する相対方位センサーであるジャイロ１１b、車両の加速度を検出する加速度センサー１１cが設けられている。車速センサー１１aは車輪に取り付けられ、ジャイロ１１bおよび加速度センサー１１cは一体にダッシュボードの所定位置に装着される。ジャイロ１１bおよび加速度センサー１１cは、側面から見たとき、車両方向と平行して車両に取り付けられるのが理想であるが、図４（Ａ）に示すように取り付け誤差があり、センサー方向は車両方向に角度A（取り付けピッチ角）を成して取り付けられる。なお、水平方向とセンサー方向の角度θをピッチ角といい、ピッチ角は傾斜角と取り付けピッチ角の和である。また、ジャイロ１１bおよび加速度センサー１１cは、平面に投影したとき、車両方向と一致して車両に取り付けられるのが理想であるが、取り付け誤差があり、図４（Ｂ）に示すように、センサー方向は車両方向に角度A2（取り付けヨー角）を成して取り付けられる。なお、北方向とセンサー方向の角度Ｙをヨー角といい、ヨー角Ｙは車両方向と取り付けヨー角の和である。

自律航法部１２は、各自律センサーからの出力信号を用いて高速度で、たとえば25Hzの周期で前後方向の車両速度Vsp(k)、車両の３次元位置（緯度方向距離N(k)、経度方向距離E(k)、下方向距離D（ｋ））を計算して出力する。図５は加速度センサー１１cから出力する加速度信号を用いて車両速度Vsp(k)を計算する方法の説明図である。車両CARには鉛直方向に重力加速度Gが加わっており、取り付けピッチ角Ａが０の場合、（Ａ）に示すように、その傾斜方向成分G0は
G0＝G×sinβ
である。したがって、加速度センサー１１cが測定する加速度Accは、車両の移動に伴う進行方向の加速度G1と重力の傾斜方向成分の和となり、次式
Acc= G×sinβ＋G1
で表現できる。取り付けピッチ角Ａが０でない場合、（Ｂ）に示すように、加速度センサー１１ｃはピッチ角θ（＝β＋Ａ）方向の加速度Accが測定される。したがって、（C）に示すようにピッチ角方向の重力加速度成分はG×sinθとなり、ピッチ角方向の車両移動に伴う加速度成分はG1×cosAとなり、次式
Acc= G×sinθ+ G1×cosA
が成立し、取り付けヨー角A2を考慮すると、次式
Acc= G×sinθ+ G1×cosA×cosA2
が成立する。この結果、傾斜方向の加速度G1は次式
G1=( Acc−G×sinθ)/ (cosA×cosA2） (1)
で表現できる。加速度測定周期をT1とすれば、変化速度ΔVは次式
ΔV=T1×( Acc−G×sinθ)/ (cosA×cosA2)
で与えられる。したがって、速度Vsp(k+1)は1つ前の離散時刻kにおける速度Vsp(k)とΔVより
Vsp(k+1)＝Vsp(k)＋T１×( Acc−G×sinθ)/ (cosA×cosA2) （2）
で与えられる。尚、加速度Accのオフセットをα_OFとすれば加速度センサーの出力信号Accからα_OFを差し引いた値をAccとして(２)式の演算を行う。すなわち、
Acc=Acc−α_OF
とする。

また、自律航法部１２は、車両の３次元位置（緯度N(k)、経度E(k)、高さD（ｋ））を次式により計算して出力する。
N(k+1)= N(k) +S(cosθcosY cosAcosA2＋sinY sinA2＋sinθcosY sinAcosA2)
E(k+1)= E(k) +S(cosθsinY cosAcosA2−cosY sinA2＋sinθsinY sinAcosA2)
D(k+1)= D(k) +S(−sinθcosAcosA2＋cosθ sinAcosA2) (3)
ただし、S=（サンプル時間T1あたりの車速パルス数×パルス間距離）
=車がサンプル時間当たりに車両方向に進んだ距離
であり、４つの角度（θ、A、Y、A2）を使って、SをN-E-D座標系(North−East−Down座標系）に投影している。

速度計算部１３は、所定のサンプル時間T2(たとえば10Hzの周期に相当する0.1秒)で車速センサー１１aから出力するパルス数Nと1パルスあたりの移動距離Lを用いて次式
Vx＝N×L／T2 (4)
により車速度を計算する。
GPSレシーバ１4はGPS測位周期、たとえば1秒間隔でGPS衛星から受信した信号に基づいて三次元位置（緯度、経度、高さ）、三次元速度(北方向速度、東方向速度、上下方向速度)を計算して出力する。

カルマンフィルタ部１５は、ジャイロオフセット補正部２０と第１の補正部２１と第２の補正部２２を備えている。
ジャイロオフセット補正部２０は、速度Vxが零（即ち、停車中）の時、停車中における角速度信号が[オフセット＋ノイズ]であることを利用し、該角速度信号出力ω_OFと自律航法部１２で計算する角速度信号オフセットとの差をとり、後述のカルマンフィルタ処理により角速度信号オフセットω_OFの補正を短時間で行う。
自律航法部１２はジャイロ１１bの出力信号を用いて計測した角速度信号ωから方位変化Δω(k）を次式
Δω（k）＝（ω−ω_OF）×T1
により計算し、また周知の慣性航法システムの技術から導出される次式に基づいてピッチ角θとヨー角Yを求めて更新する。
ｃ₀₀=cosθ(k+1) ×cosY(k+1) =−sinY(k)× Δω（ｋ）
ｃ₁₀=cosθ(k+1) ×sinY(k+1) = cosY(k) ×Δω（ｋ） (5)
なお、自律航法部１２はセンサー取り付けピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットω_OF、加速度信号オフセットα_OFについては次式により、
A(k+1)=A(k)
A2(k+1)=A2(k)
ω_OF(k+1)=ω_OF(k)
α_OF(k+1)=α_OF(k） (6)
補正されるまで一定とする。

カルマンフィルタ部の第１の補正部２１は、第１の周期(たとえば１０Hz周期)で第1のカルマンフィルタ処理を行う。第1のカルマンフィルタ処理において第１の補正部２１は、速度計算部１３が計算した車両速度Vxと自律航法部１２が計算した車両速度Vspとの差に基づいて、該自律航法部で計算している車両速度Vsp、ピッチ角θ、センサー取り付けピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットω_OF、加速度信号オフセットα_OFを補正する。
カルマンフィルタ部の第２の補正部２２は、第１の周期より長い第２の周期(たとえば1Hz周期)でGPSレシーバ１４が出力する三次元の車両位置と三次元の車両速度、ならびに自律航法部１２が出力する三次元の車両位置と三次元の車両速度を用いて、該自律航法部で計算している緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、センサー取り付けピッチ角A、前記ヨー角Y、センサー取り付けヨー角A2、角速度信号オフセットω_OF、加速度信号オフセットα_OF（自律航法で計算している全てのパラメータ）を補正する。なお、第１、第２の補正部２１，２２によるカルマンフィルタ処理の詳細は後述する。
自律航法部１２は第１の補正部２１により１０Hz周期で更新されるピッチ角θ、車両取り付けピッチ角A、センサー取り付けヨー角A2を用いて車両速度や車両位置を（2）、（3）式により演算し、また、第２の補正部２２により1Hz周期で更新されるピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2を用いて車両速度や車両位置を（2）、（3）式により演算して出力する。

（C）位置検出部の処理
図６は図３の位置検出部１０の全体の処理フローである。
はじめに、自律航法部１２に３次元車両位置N、E、D、車両速度Vsp、ピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2、ジャイロ１１ｂのオフセットω_OF、加速度センサーのオフセットα_OFの初期値を設定する（ステップ１０１）。以後、自律航法部１２は車速センサー１１ａ、ジャイロ１１ｂ、加速度センサー１１ｃの出力を取り込み（ステップ１０２）、第１周期（２５Hz周期）で（2）、（3）、(5)式の演算を行って車両速度Vsp（k+1）及び車両の３次元位置（緯度N(k+1)、経度E(k+1)、高さD(k+1)、ピッチ角θとヨー角Yに関する2つの値：
cosθ(k+1) ×cosY(k+1)、
cosθ(k+1) ×sinY(k+1)
を演算して出力する(ステップ１０３)。ついで、第２周期(１０Hz周期)になったかチェックし(ステップ１０４)、第２周期になっていなければ、ステップ１０２以降の処理を繰り返す。
第２周期になっていれば、車両速度Vxが零である状態が2秒以上続いていたかどうかによって停車判定を行う（ステップ１０５）。
停車中でなければ、第３周期(1Hz周期=GPS測位周期)になっているかチェックし（ステップ１０６）、第３周期でなければカルマンフィルタ１５の第1補正部２１は速度計算部１３が(4)式により計算した車両速度Vxと自律航法部１２が(2)式より計算した車両速度Vsp（ｋ）を用いてカルマンフィルタ処理により車両速度、前記ピッチ角θ、センサー取りつけピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットω_OF、加速度信号オフセットα_OFを補正する（ステップ１０７）。このステップ１０７では後述するカルマンフィルタの観測行列H1を用いた第１補正処理が行われる。
ステップ１０６において、第３周期であればカルマンフィルタ１５の第２補正部２２はGPSレシーバ１４が出力する３次元の車両位置（N_GPS、E_GPS、D_GPS)と３次元の車両速度（VN_GPS、VE_GPS、VD_GPS）を用いて、車両位置、車両速度、ピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2、角速度信号オフセットω_OF、加速度信号オフセットα_OFを補正する(ステップ１０８)。このステップ１０８では後述するカルマンフィルタの観測行列H2を用いた第２補正処理が行われる。
ステップ１０５において、停車中であれば、第３周期(１Hz周期=GPS測位周期)になっているかチェックし（ステップ１０９）、第３周期でなければカルマンフィルタ１５の第１補正部２１は前記ステップ１０７の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１０）。このステップ１１０では後述するカルマンフィルタの観測行列H3を用いた第３補正処理が行われる。
ステップ１０９において、第３周期であれば、カルマンフィルタ１５の第２補正部２２は前記ステップ１０８の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１１）。このステップ１１１では後述するカルマンフィルタの観測行列H4を用いた第４補正処理が行われる。（ステップ１１１）

以上の位置検出処理によれば、GPSによる推定誤差補正より早い周波数で、第１補正部２１が誤差累積の補正を行うため精度の高い位置検出ができる。図７はGPS受信時とGPS非受信時における位置検出誤差の説明図であり、比較のために第１補正処理をしない場合（従来技術という）の位置検出誤差も示している。図３の位置検出部によれば、GPS受信時に第１の補正部２１は１０Hz周期でピッチ角、センサー取り付けピッチ角、センサー取り付けヨー角の補正を行い、第２の補正部２１は１Hz周期(GPS測位周期)で補正を行うため、誤差の累積を小さくできる。なお、１Hz周期(GPS測位周期)でGPS測位データを用いて補正処理する従来技術では、１Hz周期で累積誤差がリセットされるがその間の累積誤差が大きくなる。また、図３の位置検出部によれば、GPS非受信時でも第１の補正部２１は１０Hz周期でピッチ角、センサー取り付けピッチ角、センサー取り付けヨー角の補正を行うため、誤差の累積度合を小さくできる。しかし、GPS測位データでのみ補正処理する従来技術では補正ができず、誤差の累積度合が大きく誤差が大きくなる。

図８はGPS受信が不可能な都庁の立体駐車場をぐるぐる回って出てきたときの車両の走行軌跡を示すもので、(A)は図３の位置検出部をナビゲーションシステムに適用した場合の走行軌跡、（B）はマップマッチング機能を備えた従来のナビゲーションシステムの走行軌跡である。図３の位置検出部を備えたナビゲーションシステムによれば、GPSの届かない立体駐車場内での方向ずれが小さく、しかも立体駐車場出口での方向ずれが小さく、更には、自律航法の精度が高くGPSマルチパスが起きても位置精度の劣化が少ない。しかし、従来技術では、GPSの届かない立体駐車場内での方向ずれが大きく、しかも立体駐車場出口での方向ずれが大きく、更には、GPSマルチパスが起きると間違った道路にマップマッチングしてしまう。
図９は地下の立体駐車場内における図３の位置検出部を備えたナビゲーションシステムによる走行軌跡拡大図であり、In方向から進入して地下に降り、しかる後地下で数回周回して一階に戻りOut方向から駐車場より出た場合の走行軌跡を示している。図９より図３の位置検出部を備えたナビゲーションシステムによれば、高さ方向変化(ピッチ角、高さ位置)も正確にトラックでき、地下階層が判断できる。

（Ｄ）カルマンフィルタ処理
カルマンフィルタ処理は各時刻における予測値と観測値との誤差を修正しながら、各時刻における最適な推定値を逐次求める方法である。カルマンフィルタ処理においては、予め、ある値を予測するための算出式を設定し、この算出式を用いて観測値が得られる時刻ｎまで予測を繰り返す。時刻nで観測値が取得できれば、該観測値を用いて時刻ｎでの推定値について確率論的に定義された誤差を最小化させるような推定値補正計算を行う。
図１０はカルマンフィルタ処理の概要説明図である。カルマンフィルタにおいては、図１０に示すように、信号生成過程３１と観測過程４１に分けられる。図において、線形システムFがあり、そのシステムの状態をX(t)とするとき、観測行列Hを介してX(t)の一部が観測できる場合、カルマンフィルタはX(t)の最適な推定値を与える。ここで、ｗは信号生成過程にて発生する雑音であり、ｖは観測過程にて発生する雑音である。カルマンフィルタは、入力をZ(t)としてカルマン処理を所定周期で繰り返し実行することにより、最適推定値X(t)を求める。

本発明のカルマンフィルタ処理におけるシステムモデルの状態式は次式
δX（k+１）＝F（k）δX（k）＋w(k） (7)
で表され、システム状態変数δXは
δX＝[δN、δE、δD、δVbx、δc₀₀、δc₁₀、δc₂₀、δp₀₀、δp₁₀、δp₂₀、bwz、bax]
である。但し、Vbx=Vsp((２)式参照)、bwz=ω_OF、bax=α_OFとしている。また、c₀₀〜p₂₀のパラメータは座標変換行列の要素で、
ｃ₀₀＝cosθcosY
ｃ₁₀＝cosθsinY
ｃ₂₀=−sinθ
ｐ₀₀＝cosAcosA2
ｐ₁₀＝cosAsinA2
p20＝−sinA
である。(7)式における線形システムFは(２)、(３)、(５)式のシステムモデルを表す式より図１１に示す行列で表現でき、太枠内が行列要素である。なお、c_ijはセンサー座標系からN−E−D座標系への座標変換行列要素、p_ijはセンサー座標系から車両固定座標系への座標変換行列要素であり、それぞれ次式により表現される。

また、本発明のカルマンフィルタの観測式は
δZ(k)=H(k)δX(k)+v(k) (8)
で表される。(8)式の観測行列Hは図１２に示す行列で表される。図１２において、観測行列Hの行列部分(1)は10Hz周期での速度誤差δVbxを計算する部分、(2)は10Hz周期での停車時における角速度オフセット誤差bwzを計算する部分、(3)は1Hz周期でのGPSの車両位置誤差δN,δE,δDおよび車両速度誤差δvnx,δvny,δvnzを計算する部分である。なお、観測行列Hは

と表現する。
カルマンフィルタは、Z(t) が観測できるタイミングで(8)式によりZ(t)(=δZ(t))を計算し、計算した値と観測された値の差に基づいてX(t)(＝δ（X）)を推定し、以後、次のZ（t)が観測されるまでX(t)を(7)式により更新する。そして、Z（t)が観測されると上記差を再び演算し、該差に基づいてX(t)(＝δ（X）)を推定し、以後、同様の処理を繰り返す。

観測行列Hの行列部分(1)は図６の処理ステップ１０７の第１補正処理において使用するカルマンフィルタの観測行列H1を構成し、

である。また、観測行列Hの行列部分(1)、（3）は図６の処理ステップ１０８の第２補正処理において使用するカルマンフィルタの観測行列H2を構成し、

である。また、観測行列Hの行列部分(1)、（2）は図６の処理ステップ１１０の第３補正処理において使用するカルマンフィルタの観測行列H3を構成し、

である。また、観測行列Hの行列部分(1)、（2）、(3)は図6の処理ステップ１１１の第４補正処理において使用するカルマンフィルタの観測行列H4を構成し、

である。

カルマンフィルタは、入力をZ(t)(=δZ(t))として以下の(9)式を所定周期（Z(t)の入力周期）で繰り返し実行することにより、最適推定値X(t｜t)(=δX(t｜t))を求める。ただし、時刻ｊまでの情報に基づく時刻iでのAの推定値をA(i｜j)と表記する。
X(t｜t)＝X(t｜t−1)＋K(t)[Z(t)−HX(t｜t−1)] (9)
ここでX(t｜t−1)は事前推定値、K(t)はカルマンゲインであり、それぞれ
X(t｜t−1)＝FX(t−1｜t−1) (9)′
K(t)＝P(t｜t−1)H^T (HP(t｜t−1)HT＋V)^-1
と表現でき、事前推定値X(t｜t−1)は、Z(t)の入力周期より短い周期で(9)′式により更新される。
また、Pは状態量Xの誤差共分散行列であり、P(t｜t−1)は誤差共分散の予測値、P(t−1｜t−1)は誤差共分散であり、それぞれ
P(t｜t−1)＝FP(t−1｜t−1)F^T ＋W
P(t−1｜t−1)＝(I−K(t−1)H)P(t−1｜t−2)
である。Vは観測過程で発生する雑音ｖの分散、Ｗは信号過程で発生する雑音wの分散である。添字の（・）^T は転置行列を意味し、(・)^-1は逆行列を意味する。また、Ｉは単位行列である。さらに、VとWは平均０の白色ガウス雑音であり、互いに無相関である。上記のようなカルマンフィルタにおいて、状態量Xと誤差共分散Pの初期値に適当な誤差を与えてやり、新しい観測が行われる度に(７)式の計算を繰り返し行うことにより、状態量Xの精度を向上することができる。

共分散行列Pは次式で与えられる。

この共分散行列Pの対角要素は、誤差δN、δE、δD、δVbx、δc₀₀、δc₁₀、δc₂₀、δp₀₀、δp₁₀、δp₂₀、bwz、baxの分散であり、

と表現する。
以上では、カルマンフィルタを使用して各パラメータを補正する場合であるが、カルマンフィルタに限らず、Hインフィニティフィルタ、パーティクルフィルタなど、確率論に基づくフィルタリングシステムを利用して補正することが可能である。

（E）周期制御
図１３はGPSの測位状態が良好のときの自律航法演算周波数HF、第１補正周波数LFを変えたときの走行軌跡とGPS測位軌跡である。GPSの測位状態が良好のとき、(1)自律航法部１２の自律航法演算周波数HFを25Hz、第１補正部２１の補正周波数LFを５Hzとした場合の走行軌跡RTRとGPS測位軌跡GTRは図１３(A)に示すようになり、(2)自律航法演算周波数HFを１Hz、第１補正周波数LFを１Hzとした場合の走行軌跡RTRとGPS測位軌跡GTRは図１３(B)に示すようになる。
この図１３（A）,(B)より、GPSの測位状態が良好であれば、周期を長くしても短い場合と同程度の位置精度を得ることが可能になる。すなわち、GPSの測位状態が良好であれば、自律航法演算周波数HFを１Hz、第１補正周波数LFを１Hzにして計算負荷を著しく削減することができる。
図１４は車両が立体駐車場内に入りGPSの測位状態が不良のときの自律航法演算周波数HF、第１補正周波数LFを変えたときの走行軌跡とGPS測位軌跡である。GPSの測位状態が不良な立体駐車場内では、(1)自律航法演算周波数HFを２５Hz、第１補正周波数LFを５Hzとした場合の走行軌跡RTRとGPS測位軌跡GTRは図１４(A)に示すようになり、(2)HF=5Hz,LF=5Hzの場合は図１４(B)に示すようになり、(3)HF=1Hz,LF=1Hzの場合は、図１４(C)に示すようになる。この図１４(A)〜(C)より、車両が立体駐車場内に入りGPSの測位状態が不良になると、HF=1Hz，LF=1Hzでは位置誤差が大きくなり採用できない。一方、HF=５Hz，LF=５HzであればHF=25Hz,LF=5Hzの場合と遜色なく位置精度が得られている。このため、GPSの測位状態が不良になったら自律航法演算周波数HFを25Hz、第１補正周波数LFを5Hzと極端に早くする必要はなく、HF=5Hz，LF=5Hzを採用できる。このようにすれば、位置精度を維持しつつ計算負荷を削減することができる。

図１５は本発明の位置検出装置の構成図であり、図１と同一部分には同一符号を付している。尚、位置検出部１０はカルマンフィルタ処理部１５のみ示している。
カルマンフィルタ処理部１５の第２推定部は１Hz周期(GPS測位周期)で第２の補正処理を行い、その補正処理の過程で得られる共分散行列Pの対角要素（（10）式参照）、すなわち、
δN、δE、δD、δVbx、δc₀₀、δc₁₀、δc₂₀、δp₀₀、δp₁₀、δp₂₀、bwz、bax
の分散を出力する。
推定誤差算出部３１は、目標位置からの最大誤差(例えば１０m)が目標誤差εtとして入力されているものとすれば、カルマンフィルタ処理部１５から出力する緯度方向及び経度方向の位置誤差δN、δEの分散

を用いて車両位置の推定誤差を演算する。すなわち、推定誤差算出部３１は、次式

により車両位置の推定誤差εiを演算する。
差分演算部３２は、車両位置の該推定誤差と目標誤差(例えば１０ｍ)との差分を演算し、周期変更部３３は該差分に基づいて前記第１、第２周期（自律航法演算周波数HFと第１補正周波数LF）を制御する。すなわち、周期変更部３３は推定誤差が目標誤差より小さければ、第１、第２周期を長くし、該推定誤差が目標誤差より大きければ、第１、第２周期を短くする。これを関数を用いて行うことが可能であり、また、予め自律航法演算周波数HFと第１補正周波数LFの組み合わせを複数個記憶しておき、その中から所定の組み合わせを選択してHF,LFを制御することも可能である。関数の例としては、正のゲインGを仮定し、
出力周期＝[1−G(εt−εi )]×元の周期
等が考えられる。組み合わせの例としては、
(1) HF=25Hｚ、LF=5Hz、
(2) HF=5Hz、LF=5Hｚ、
(3) HF=2Hz、LF=1Hz、
(4) HF=１Hz、LF=1Hz
が考えられる。
以上では自律航法演算周波数HFと第１補正周波数LFの両方を制御する場合を説明したがが、第１補正周波数LFのみ制御することもできる。また、以上の説明では、目標誤差として、二次元の車両位置に関して目標誤差を設定したが、三次元位置、あるいは車両速度、あるいは姿勢角等に関して目標誤差を設定することもできる。三次元位置に関して目標誤差を設定する場合、次式

により車両位置の推定誤差εiを演算する。また、車両速度に関して目標誤差を設定する場合には、次式

により車両位置の推定誤差εiを演算する。また、車両位置と車両速度とに関して目標誤差を設定する場合には、次式

により車両位置の推定誤差εiを演算し、他の場合にも同様に推定誤差εiを演算する。ここでα，β，γは正の重み係数である。

図１６は本発明の位置検出装置の全体の処理フローであり、図１を参照して説明する。
自律航法部１２は自律航法（Dead Reckoning）センサー１１の出力を取り込み(ステップ２０１)、予め設定した第１周波数HFで自律航法により車両位置を計算して出力する(ステップ２０２)。上記自律航法計算と並行して、カルマンフィルタ処理部１５の第１補正部２１は該第１周波数以下の第２の周波数LFで自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角などを補正する第１補正処理を行ない(ステップ２０３)、第２補正部２２はGPSの測位周期(１秒＝１Hz)である第３の周波数でGPSデータを用いて自律航法による計算結果を補正する第２補正処理を行う(ステップ２０４)。周期制御部３０は、例えば車両位置に関する推定誤差を(12)式により算出し、該推定誤差と目標誤差の差分に基づいて第１周波数HF、第２周波数LFを制御し(ステップ２０５)、位置検出装置は上記補正処理の結果である推定状態量Xを出力する(ステップ２０６)。
以上本発明を実施例に従って説明したが、本発明は実施例に限定されるものではない。

本発明の位置検出装置の概要説明図である 本発明の第２周期の制御例である 本発明の位置検出部の構成図である。 姿勢パラメータ(ピッチ角θ、センサー取り付けピッチ角A、ヨー角Y、センサー取り付けヨー角A2)説明図である。 加速度センサーから出力する加速度信号を用いて車両速度Vsp(k)を計算する方法の説明図である。 位置検出部の全体の処理フローである。 GPS受信時とGPS非受信時における位置検出誤差の説明図である。 GPS受信が不可能な都庁の立体駐車場をぐるぐる回って出てきたときの車両の走行軌跡説明図である。 地下の立体駐車場内におけるナビゲーションシステムによる走行軌跡拡大図である。 カルマンフィルタ処理の概要説明図である。 カルマンフィルタの線形システムFを示す行列例である。 カルマンフィルタの観測行列例である。 GPSの測位状態が良好のときの自律航法演算周波数HF、第１補正周波数LFを変えたときの走行軌跡とGPS測位軌跡である。 GPSの測位状態が不良のときの自律航法演算周波数HF、第１補正周波数LFを変えたときの走行軌跡とGPS測位軌跡である。である。 本発明の位置検出装置の構成図である。 本発明の位置検出装置の全体の処理フローである。

符号の説明

１１ 自律センサー
１２ 自律航法部
１５ カルマンフィルタ処理部
２１ 第１の補正部
２２ 第２の補正部
３０ 周期制御部
３１ 推定誤差算出部
３２ 誤差演算部
３３ 周期変更部

Claims (13)

1. 車両の現在位置を検出する位置検出方法において、
   第１の周期で高速に自律航法により位置計算し、
   該第１の周期以上の長さの第２の周期で自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角などを補正する第１補正処理を行ない、
   GPSの測位周期である第３の周期でGPSデータを用いて自律航法による計算結果を補正する第２補正処理を行い、
   補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて少なくとも前記第２周期の長さを制御する、
   ことを特徴とする位置検出方法。
2. 車両の現在位置を検出する位置検出方法において、
   車両の加速度や車両の方位変化量に応じた信号を出力する自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに車両移動距離検出部により検出された車両移動距離を用いて、自律航法部において第1の周期で緯度、経度、高さ方向の車両位置を計算すると共に、前記センサーから出力する加速度信号を用いて車両速度を計算する第１ステップ、
   前記移動距離検出部の出力信号を用いて前記第１周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した前記車両速度との速度差に基づいて、自律航法部で計算した車両速度、前記ピッチ角θ、前記センサーの取り付け姿勢角を補正する第２ステップ、
   GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第３ステップ、
   前記複数の補正対象のうち所定の補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて前記第２周期の長さを制御する第４ステップ、
   を備えたことを特徴とする位置検出方法。
3. 前記第３ステップにおいて、カルマンフィルタ処理に基づいて前記補正対象の値を補正し、該カルマンフィルタ処理の過程で演算される各補正対象の共分散行列の対角要素を用いて前記推定誤差を計算する、
   ことを特徴とする請求項２記載の位置検出方法。
4. 前記センサーの取り付け姿勢角は、センサー取り付けピッチ角A、センサー取り付けヨー角A2を含み、前記センサーオフセット値は角速度信号オフセット、加速度信号オフセットを含む、
   ことを特徴とする請求項２または３記載の位置検出方法。
5. 前記第４ステップにおいて、前記差分に基づいて前記第２周期の長さを制御すると共に、第１周期の長さを制御する、
   ことを特徴とする請求項２または３記載の位置検出方法。
6. 前記推定誤差が目標誤差より小さければ、前記第1、第２周期を長くし、該推定誤差が目標誤差より大きければ、前記第1、第２周期を短くする、
   ことを特徴とする請求項５記載の位置検出方法。
7. 前記目標誤差として、車両位置に対する目標誤差、あるいは車両速度に対する目標誤差、あるいは姿勢角に対する目標誤差を採用することを特徴とする請求項２または３記載の位置検出方法。
8. 車両の現在位置を検出する位置検出装置において、
   車両の移動距離を測定する移動距離検出部、
   車両の加速度を検出する加速度センサー、
   車両の方位変化量に応じた信号を出力する相対方位センサー、
   GPS衛星からの衛星電波を受信して緯度、経度、高さ方向の車両位置および車両速度情報を出力するGPSレシーバ、
   第１の周期で高速に自律航法により位置計算する自律航法部、
   該第１の周期以上の長さの第２の周期で自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角などを補正する第１補正処理を行なう第１の補正部、
   GPSの測位周期である第３の周期でGPSデータを用いて自律航法による計算結果を補正する第２補正処理を行う第２の補正部、
   補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて少なくとも前記第２周期の長さを制御する周期制御部、
   を備えたことを特徴とする位置検出装置。
9. 車両の現在位置を検出する位置検出装置において、
   車両の移動距離を測定する移動距離検出部、
   車両の加速度を検出する加速度センサー、
   車両の方位変化量に応じた信号を出力する相対方位センサー、
   GPS衛星からの衛星電波を受信して緯度、経度、高さ方向の車両位置および車両速度情報を出力するGPSレシーバ、
   第１の周期で、前記自律航法用のセンサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに前記移動距離を用いて緯度、経度、高さ方向の車両位置を計算すると共に、前記加速度センサーから出力する加速度信号を用いて車両速度を計算する自律航法部、
   前記移動距離検出部の出力信号を用いて第１周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した車両速度との速度差に基づいて前記自律航法部で計算している車両速度、前記ピッチ角θおよび前記センサーの取り付け姿勢角を補正する第１の補正部、
   前記GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第２の補正部、
   前記複数の補正対象のうち所定の補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて前記第２周期の長さを制御する周期制御部、
   を備えたことを特徴とする位置検出装置。
10. 前記第２の補正部は、カルマンフィルタ処理に基づいて前記補正対象の値を補正し、該カルマンフィルタ処理の過程で演算される各補正対象の共分散行列の対角要素を用いて前記推定誤差を計算する、
    ことを特徴とする請求項９記載の位置検出装置。
11. 前記周期制御部は、前記差分に基づいて前記第２周期の長さを制御すると共に、第１周期の長さを制御する、
    ことを特徴とする請求項９または１０記載の位置検出装置。
12. 前記周期制御部は、前記推定誤差が目標誤差より小さければ、前記第1、第２周期を長くし、該推定誤差が目標誤差より大きければ、前記第1、第２周期を短くする、
    ことを特徴とする請求項１１記載の位置検出装置。
13. 前記目標誤差として、車両位置に対する目標誤差、あるいは車両速度に対する目標誤差、あるいは姿勢角に対する目標誤差を採用することを特徴とする請求項９乃至１２記載の位置検出装置。

Images