(57)【要約】【目的】本発明は精度の高い位置検出を可能にする「位置検出装置および位置検出方法」を提供することである。【構成】自律航法部12は、自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対するセンサー取り付けピッチ角A、センサー取り付けヨー角A2並びに車速センサーより求まる移動距離を用いて車両位置を計算すると共に、加速度信号を用いて車両速度を計算する。第1の補正部21は車速センサーの出力信号を用いて第1の周期で車両速度を計算し、該車両速度と自律航法部で計算した車両速度との速度差に基づいてピッチ角θ、センサー取り付けピッチ角A、センサー取り付けヨー角A2を補正する。また、第２の補正部22は、GPSレシーバが出力する車両位置、車両速度および自律航法部が出力する車両位置、車両速度を用いて、第１の周期より長い第2の周期(GPS周期)でピッチ角θ、センサー取り付けピッチ角A、ヨー角Y、センサー取り付けヨー角A2を補正する。【選択図】図１
2008215917.tif 000002
【特許請求の範囲】
【請求項１】
車両の現在位置を検出する位置検出装置において、
車両の移動距離を測定する移動距離検出部、
車両の加速度を検出する加速度センサー、
車両の方位変化量に応じた信号を出力する相対方位センサー、
GPS衛星からの衛星電波を受信して緯度、経度、高さ方向の車両位置および車両速度情報を出力するGPSレシーバ、
第１の周期で、前記自律航法用のセンサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対するセンサー取り付けピッチ角A、センサー取り付けヨー角A2並びに前記移動距離を用いて緯度、経度、高さ方向の車両位置を計算すると共に、前記加速度センサーから出力する加速度信号を用いて車両速度を計算する自律航法部、
前記移動距離検出部の出力信号を用いて第１の周期より長い第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した車両速度との速度差に基づいて前記自律航法部で計算している車両速度、前記ピッチ角θ、センサー取りつけピッチ角A、センサー取り付けヨー角A2を補正する第１の補正部、
前記GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２の周期より長い第３の周期で前記自律航法が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、センサー取り付けピッチ角A、前記ヨー角Y、センサー取り付けヨー角A2、角速度信号オフセット、加速度信号オフセットを補正する第２の補正部、
を備えたことを特徴とする位置検出装置。
【請求項２】
車両が停止しているとき、前記第２の周期で前記相対方位センサーから出力する角速度信号と前記自律航法部で計算した角速度信号オフセットとの差に基づいて角速度信号のオフセットを補正するオフセット補正部、
を備えたことを特徴とする請求項１記載の位置検出装置。
【請求項３】
前記第１の補正部は、前記自律航法部で計算している車両速度を補正する毎に、角速度信号オフセット、加速度信号オフセットを補正し、
前記自律航法部は加速度センサーから出力する加速度信号から前記オフセットを減算した信号を真の加速度信号として用い、また、相対方位センサーからから出力する信号から前記オセットを減算した信号を真の角速度信号として用いて前記緯度、経度、高さ方向の車両位置、前記車両速度、前記ピッチ角θ、前記ヨー角Yを計算する、
ことを特徴とする請求項１記載の位置検出装置。
【請求項４】
前記移動距離検出部は車両が所定距離走行する毎にパルスを発生する車速センサーであり、前記第１の補正部は該車速センサーから前記第２の周期の間に発生したパルスの数に前記所定距離を乗算し、前記第２周期で除算することにより前記車両速度を計算する、
ことを特徴とする請求項１記載の位置検出装置。
【請求項５】
前記移動距離検出部は、車両が所定距離走行する毎にパルスを発生する車速センサーであり、前記自律航法部は、該車速センサーから第１の周期の間に発生したパルスの数に前記所定距離を乗算して得られた走行距離の経度成分、緯度成分、高さ成分を、第1または第2の補正手段によって補正された緯度、経度、高さ方向の車両位置に累積して緯度、経度、高さ方向の車両位置を計算する、
ことを特徴とする請求項１記載の位置検出装置。
【請求項６】
車両の現在位置を検出する位置検出方法において、
車両の加速度や車両の方位変化量に応じた信号を出力する自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対するセンサー取り付けピッチ角A、センサー取り付けヨー角A2並びに車両移動距離検出部により検出された車両移動距離を用いて、自律航法部において第1の周期で緯度、経度、高さ方向の車両位置を計算すると共に、前記センサーから出力する加速度信号を用いて車両速度を計算する第１ステップ、
前記移動距離検出部の出力信号を用いて第１の周期より長い第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した前記車両速度との速度差に基づいて、自律航法部で計算した車両速度、前記ピッチ角θ、センサー取りつけピッチ角A、センサー取り付けヨー角A2を補正する第２ステップ、
GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２の周期より長い第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、センサー取り付けピッチ角A、前記ヨー角Y、センサー取り付けヨー角A2、角速度信号オフセット、加速度信号オフセットを補正する第３ステップ、
を備えたことを特徴とする位置検出方法。
【請求項７】
車両が停止しているとき、前記第２の周期で前記相対方位センサーから出力する角速度信号と前記自律航法部で計算した角速度信号オフセットとの差に基づいて角速度信号のオフセットを補正するステップ、
を有することを特徴とする請求項６記載の位置検出方法。
【請求項８】
前記位置検出方法は、更に、
前記自律航法部で車両速度を補正する毎に、角速度信号オフセット、加速度信号オフセットを補正するステップ、
前記自律航法部において、加速度センサーから出力する加速度信号から前記加速度信号オフセットを減算した信号を真の加速度信号として用いて、また、相対方位センサーからから出力する角速度信号から前記オセットを減算した信号を真の角速度信号として用いて、前記緯度、経度、高さ方向の車両位置、前記車両速度、前記ピッチ角θ、前記ヨー角Yを計算するステップ、
を有することを特徴とする請求項６記載の位置検出方法。
【請求項９】
前記移動距離検出部は車両が所定距離走行する毎にパルスを発生する車速センサーであれば、前記第2ステップにおいて、該車速センサーから前記第２の周期の間に発生したパルスの数に前記所定距離を乗算し、前記第２周期で除算することにより前記車両速度を計算する、
ことを特徴とする請求項６記載の位置検出方法。
【請求項１０】
前記移動距離検出部は車両が所定距離走行する毎にパルスを発生する車速センサーであり、前記第１ステップにおいて、該車速センサーから第１の周期の間に発生したパルスの数に前記所定距離を乗算して得られた走行距離の経度成分、緯度成分、高さ成分を、第1または第2の補正手段によって補正された緯度、経度、高さ方向の車両位置に累積して緯度、経度、高さ方向の車両位置を計算する、
ことを特徴とする請求項６記載の位置検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両の現在位置を検出する位置検出装置および位置検出方法に関わり、特に、GPSの非受信時において自律航法により計算される位置データの精度を向上できる位置検出装置および位置検出方法に関する。
【背景技術】
【０００２】
車載用ナビゲーション装置は、自律航法センサーを用いた自律航法（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から位置情報が得られる周期（１秒）毎に補正を行うことになるため、補正周期が長すぎて補正が不十分となり精度の高い位置検出ができない問題がある。また、第２従来技術は自律航法用センサーの取り付けヨー角の補正ができない問題がある。
【０００６】
以上から、本発明の目的はGPSの測位周期より短い周期で第１の補正処理を行ない、GPSの測位周期（１秒）毎にGPSデータを用いて第２の補正処理を行うことにより、精度の高い位置検出を可能にすることである。
本発明の別の目的は第１の補正処理において、車両ピッチ角や自律航法用センサーの車両に対する取り付けピッチ角の補正を行い、補正されたこれらパラメータを用いて速度や位置計算することにより位置検出精度を向上することである。
本発明の別の目的は第２の補正処理においてGPSから得られる緯度、経度、高さ方向の車両位置および緯度、経度、高さ方向の車両速度を用いて、ピッチ角θ、センサー取り付けピッチ角A、ヨー角Y、センサー取り付けヨー角A2を補正し、補正されたこれらパラメータを用いて速度や位置計算することにより位置検出精度を向上することである。
本発明の目的は加速度センサーや相対方位センサーのオフセット値を補正することにより位置検出精度を向上することである。
【課題を解決するための手段】
【０００７】
・位置検出装置
本発明の第１の態様は、車両の現在位置を検出する位置検出装置であり、車両の移動距離を測定する移動距離検出部、車両の加速度を検出する加速度センサー、車両の方位変化量に応じた信号を出力する相対方位センサー、GPS衛星からの衛星電波を受信して緯度、経度、高さ方向の車両位置および車両速度情報を出力するGPSレシーバ、第１の周期で、前記自律航法用のセンサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対するセンサー取り付けピッチ角A、センサー取り付けヨー角A2並びに前記移動距離を用いて緯度、経度、高さ方向の車両位置を計算すると共に、前記加速度センサーから出力する加速度信号を用いて車両速度を計算する自律航法部、前記移動距離検出部の出力信号を用いて第１の周期より長い第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した車両速度との速度差に基づいて前記自律航法部で計算している車両速度、前記ピッチ角θ、センサー取りつけピッチ角A、センサー取り付けヨー角A2を補正する第１の補正部、前記GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２の周期より長い第３の周期で前記自律航法が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、センサー取り付けピッチ角A、前記ヨー角Y、センサー取り付けヨー角A2、角速度信号オフセット、加速度信号オフセットを補正する第２の補正部を備えている。
上記の位置検出装置は、車両が停止しているとき、前記第２の周期で前記相対方位センサーから出力する角速度信号と前記自律航法部で計算した角速度信号オフセットとの差に基づいて角速度信号のオフセットを補正するオフセット補正部を備え、角速度信号から該オフセットを差し引いて真の角速度信号とする。
上記の位置検出装置において、前記第１の補正部は、前記自律航法部で計算している車両速度を補正する毎に、角速度信号オフセット、加速度信号オフセットを補正し、前記自律航法部は加速度センサーから出力する加速度信号から前記オフセットを減算した信号を真の加速度信号として用い、また、相対方位センサーからから出力する信号から前記オセットを減算した信号を真の角速度信号として用いて前記緯度、経度、高さ方向の車両位置、前記車両速度、前記ピッチ角θ、前記ヨー角Yを計算する。
【０００８】
・位置検出方法
本発明の第２の態様は、車両の現在位置を検出する位置検出方法であり、車両の加速度や車両の方位変化量に応じた信号を出力する自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対するセンサー取り付けピッチ角A、センサー取り付けヨー角A2並びに車両移動距離検出部により検出された車両移動距離を用いて、自律航法部において第1の周期で緯度、経度、高さ方向の車両位置を計算すると共に、前記センサーから出力する加速度信号を用いて車両速度を計算する第１ステップ、前記移動距離検出部の出力信号を用いて第１の周期より長い第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した前記車両速度との速度差に基づいて、自律航法部で計算した車両速度、前記ピッチ角θ、センサー取りつけピッチ角A、センサー取り付けヨー角A2を補正する第２ステップ、GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２の周期より長い第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、センサー取り付けピッチ角A、前記ヨー角Y、センサー取り付けヨー角A2、角速度信号オフセット、加速度信号オフセットを補正する第３ステップを有している。
上記位置検出方法は、更に、車両が停止しているとき、前記第２の周期で前記相対方位センサーから出力する角速度信号と前記自律航法部で計算した角速度信号オフセットとの差に基づいて角速度信号のオフセットを補正するステップを有している。
上記位置検出方法は、更に、前記自律航法部で車両速度を補正する毎に、角速度信号オフセット、加速度信号オフセットを補正するステップ、前記自律航法部において、加速度センサーから出力する加速度信号から前記加速度信号オフセットを減算した信号を真の加速度信号として用いて、また、相対方位センサーからから出力する角速度信号から前記オセットを減算した信号を真の角速度信号として用いて、前記緯度、経度、高さ方向の車両位置、前記車両速度、前記ピッチ角θ、前記ヨー角Yを計算するステップを有している。
【発明の効果】
【０００９】
本発明によれば、GPSの測位周期より短い周期で第１の補正処理を行ない、GPSの測位周期（１秒）毎にGPSデータを用いて第２の補正処理を行うことにより、精度の高い位置検出ができる。
また、本発明によれば、加速度センサーから得られる加速度信号および推定ピッチ角を用いて算出した車両速度と車両パルスから算出した車両速度とを用いて補正処理によりピッチ角θ、センサー取り付けピッチ角A、センサー取り付けヨー角A2を補正し、これらパラメータを用いて速度や位置計算することにより位置検出精度を向上することができる。
また、本発明によれば、停車時にジャイロ出力のオフセットを測定して補正し、また、加速度計のオフセットも修正するため、位置検出精度を向上することができる。
また、本発明によれば、第２の補正処理においてGPSから得られる緯度、経度、高さ方向の車両位置および緯度、経度、高さ方向の車両速度を用いて、ピッチ角θ、センサー取り付けピッチ角A、ヨー角Y、センサー取り付けヨー角A2を補正し、これらパラメータを用いて速度や位置計算することにより位置検出精度を向上することができる。
【発明を実施するための最良の形態】
【００１０】
（A）本発明の位置検出装置の構成
図１は本発明の位置検出装置の構成図であり、ナビゲーション装置の位置検出に利用することができる。この位置検出装置には、自律航法用センサーとして、車両の移動距離を測定する移動距離検出部、たとえば車両が所定距離走行する毎に１個のパルスを発生する車速センサー１１a、車両の方位変化量に応じた信号を出力する相対方位センサーであるジャイロ１１b、車両の加速度を検出する加速度センサー１１cが設けられている。車速センサー１１aは車輪に取り付けられ、ジャイロ１１bおよび加速度センサー１１cは一体にダッシュボードの所定位置に装着される。ジャイロ１１bおよび加速度センサー１１cは、側面から見たとき、車両方向と平行して車両に取り付けられるのが理想であるが、図２（Ａ）に示すように取り付け誤差があり、センサー方向は車両方向に角度A（取り付けピッチ角）を成して取り付けられる。なお、水平方向とセンサー方向の角度θをピッチ角といい、ピッチ角は傾斜角と取り付けピッチ角の和である。また、ジャイロ１１bおよび加速度センサー１１cは、平面に投影したとき、車両方向と一致して車両に取り付けられるのが理想であるが、取り付け誤差があり、図２（Ｂ）に示すように、センサー方向は車両方向に角度A2（取り付けヨー角）を成して取り付けられる。なお、北方向とセンサー方向の角度Ｙをヨー角といい、ヨー角Ｙは車両方向と取り付けヨー角の和である。
【００１１】
自律航法部１２は、各自律センサーからの出力信号を用いて高速度で、たとえば２５Hzの周期で前後方向の車両速度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×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の周期)で車速センサー１１aから出力するパルス数Nと1パルスあたりの移動距離Lを用いて次式
Vx＝N×L／T2       (4)
により車速度を計算する。
GPSレシーバ１4はGPS測位周期、たとえば1秒間隔でGPS衛星から受信した信号に基づいて三次元位置（緯度、経度、高さ）、三次元速度(北方向速度、東方向速度、上下方向速度)を計算して出力する。
【００１４】
カルマンフィルタ部１５は、ジャイロオフセット補正部２０と第１の補正部２１と第２の補正部２２を備えている。
ジャイロオフセット補正部２０は、速度Vxが零（即ち、停車中）の時、停車中における角速度信号が[オフセット＋ノイズ]であることを利用し、該角速度信号出力ωOFと自律航法部１２で計算する角速度信号オフセットとの差をとり、後述のカルマンフィルタ処理により角速度信号オフセットωOFの補正を短時間で行う。
自律航法部１２はジャイロ１１bの出力信号を用いて計測した角速度信号ωから方位変化Δω(k）を次式
Δω（k）＝（ω−ωOF）×T1
により計算し、また周知の慣性航法システムの技術から導出される次式に基づいてピッチ角θとヨー角Yを更新する。
ｃ00=cosθ(k+1) ×cosY(k+1) =−sinY(k)× Δω（ｋ）
ｃ10=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）式により演算して出力する。
【００１５】
（Ｂ）本発明の位置検出装置の処理
図４は本発明の位置検出装置の全体の処理フローである。
はじめに、自律航法部１２に3次元車両位置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レシーバ１４が出力する３次元の車両位置（NGPS、EGPS、DGPS)と３次元の車両速度（VNGPS、VEGPS、VDGPS）を用いて、車両位置、車両速度、ピッチ角θ、車両取り付けピッチ角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マルチパスが起きると間違った道路にマップマッチングしてしまう。
図７は地下の立体駐車場内における本発明の位置検出装置を備えたナビゲーションシステムによる走行軌跡拡大図であり、Aで示すように高さ方向変化(ピッチ角、高さ位置)も正確にトラックでき、地下階層が判断できる。
【００１８】
（Ｄ）本発明のカルマンフィルタ処理
カルマンフィルタ処理は各時刻における予測値と観測値との誤差を修正しながら、各時刻における最適な推定値を逐次求める方法である。カルマンフィルタ処理においては、予め、ある値を予測するための算出式を設定し、この算出式を用いて、観測値が得られる時刻ｎまで予測を繰り返す。時刻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、δc00、δc10、δc20、δp00、δp10、δp20、bwz、bax]
である。但し、Vbx=Vsp((２)式参照)、bwz=ωOF、bax=αOFとしている。また、c00〜p20のパラメータは座標変換行列の要素で、
ｃ00＝cosθcosY
ｃ10＝cosθsinY
ｃ20=−sinθ
ｐ00＝cosAcosA2
ｐ10＝cosAsinA2
p20＝−sinA
である。(7)式における線形システムFは(２)、(３)、(５)式のシステムモデルを表す式より図９に示す行列で表現でき、太枠内が行列要素である。なお、cijはセンサー座標系からN−E−D座標系への座標変換行列要素、pijはセンサー座標系から車両固定座標系への座標変換行列要素であり、それぞれ次式により表現される。
【数１】
2008215917.tif 000003
また、本発明のカルマンフィルタの観測式は
δZ(k)=H(k)δX(k)+v(k)      (8)
で表される。(8)式の観測行列Hは図１０に示す行列で表される。図１０において、観測行列Hの行列部分(１)は10Hz周期での速度誤差δVbxを計算する部分、（２）は10Hz周期での停車時における角速度オフセット誤差bwzを計算する部分、(3)は1Hz周期でのGPSの車両位置誤差δN,δE,δDおよび車両速度誤差δvnx,δvny,δvnzを計算する部分である。
【００２０】
観測行列Hの行列部分(１)は図４の処理ステップ１０７の第１補正処理において使用するカルマンフィルタの観測行列H1を構成し、
【数２】
2008215917.tif 000004
である。また、観測行列Hの行列部分(１)、（３）は図４の処理ステップ１０８の第２補正処理において使用するカルマンフィルタの観測行列H2を構成し、
【数３】
2008215917.tif 000005
である。また、観測行列Hの行列部分(１)、（２）は図４の処理ステップ１１０の第３補正処理において使用するカルマンフィルタの観測行列H3を構成し、
【数４】
2008215917.tif 000006
である。また、観測行列Hの行列部分(１)、（２）、（３）は図４の処理ステップ１１１の第４補正処理において使用するカルマンフィルタの観測行列H4を構成し、
【数５】
2008215917.tif 000007
である。
【００２１】
カルマンフィルタは、入力をZ(t)(=δ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)]        (９  )
ここでX(t｜t−1)は事前推定値、K(t)はカルマンゲインであり、それぞれ
X(t｜t−1)＝FX(t−1｜t−1)
K(t)＝P(t｜t−1)HT (HP(t｜t−1)HT＋V)−1
と表現できる。また、Pは状態量Xの誤差共分散であり、P(t｜t−1)は誤差共分散の予測値、P(t−1｜t−1)は誤差共分散であり、それぞれ
P(t｜t−1)＝FP(t−1｜t−1)FT ＋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の精度を向上することができる。
以上では、カルマンフィルタを使用して各パラメータを補正する場合であるが、カルマンフィルタに限らず、Hインフィニティフィルタ、パーティクルフィルタなど、確率論に基づくフィルタリングシステムを利用して補正することが可能である。
【図面の簡単な説明】
【００２２】
【図１】本発明の位置検出装置の構成図である。
【図２】姿勢パラメータ(ピッチ角θ、センサー取り付けピッチ角A、ヨー角Y、センサー取り付けヨー角A2)説明図である。
【図３】加速度センサーから出力する加速度信号を用いて車両速度Vsp(k)を計算する方法の説明図である。
【図４】本発明の位置検出装置の全体の処理フローである。
【図５】GPS受信時とGPS非受信時における本発明の位置検出誤差の説明図である。
【図６】GPS受信が不可能な都庁の立体駐車場をぐるぐる回って出てきたときの車両の走行軌跡説明図である。
【図７】地下の立体駐車場内における本発明の位置検出装置を備えたナビゲーションシステムによる走行軌跡拡大図である。
【図８】カルマンフィルタ処理の概要説明図である。
【図９】カルマンフィルタの線形システムFを示す行列例である。
【図１０】カルマンフィルタの観測行列例である。
【符号の説明】
【００２３】
１１a  車速センサー
１１b  相対方位センサーであるジャイロ
１１c  加速度センサー
１２  自律航法部
１３  速度計算部
１４  GPSレシーバ
１５  カルマンフィルタ部
２０  ジャイロオフセット補正部
２１  第１の補正部
２２  第２の補正部
【図１】
2008215917.tif 000008
【図２】
2008215917.tif 000009
【図３】
2008215917.tif 000010
【図４】
2008215917.tif 000011
【図５】
2008215917.tif 000012
【図６】
2008215917.tif 000013
【図７】
2008215917.tif 000014
【図８】
2008215917.tif 000015
【図９】
2008215917.tif 000016
【図１０】
2008215917.tif 000017