JP2010160163A - 方位センサの補正係数演算装置及び演算プログラム - Google Patents

Abstract

【課題】方位変更の前後におけるＧＰＳ情報と方位センサ情報とに基づいて、方位センサ情報を補正するための補正係数を演算する場合において、方位センサの静的な設置角度及びその設置角度の動的な変化を考慮した動的補正係数を適切に演算することが可能な方位センサの補正係数演算装置等を提供する。
【解決手段】方位変更の前後におけるＧＰＳ方位変化量とセンサ方位変化量との差異に基づいて、方位センサ情報の基本補正係数を演算し、方位センサ４の検出軸Ｘの静的な設置角度αに応じて基本補正係数を補正した静的補正係数を演算し、方位センサ４の検出軸Ｘ方向の動的変化角度βに応じて、静的補正係数を動的に補正して動的補正係数を演算する。
【選択図】図５

Description

本発明は、ＧＰＳ衛星からの信号を用いて方位センサからの出力情報を補正するための補正係数を演算する補正係数演算装置及び演算プログラム、並びに自位置認識装置及び認識プログラムに関する。

ジャイロスコープ等の方位センサの出力情報を補正する技術に関して、例えば下記の特許文献１には、以下のような車両用現在位置検出装置に関する技術が開示されている。この車両用現在位置検出装置は、車両の回転角速度に応じた信号を出力するジャイロと、車両の移動距離に応じたパルス信号を出力する距離センサと、ＧＰＳ（Global PositioningSystem）衛星からの送信電波を受信し、車両の位置、方位（進行方向）、速度等を検出
するＧＰＳ受信機と、ジャイロ、距離センサ、ＧＰＳ受信機からの出力に基づいて車両の現在位置や進行方向等、推測航法を行うためのデータを検出する現在位置検出部と、を備えている。

そして、前記現在位置検出部は、距離センサからのパルス信号に基づいて車両の移動距離を算出する移動距離演算部と、ジャイロからの検出信号に基づいて方位変化量を算出する方位変化量演算部と、算出された方位変化量と移動距離とに基づいて、相対軌跡及び車速を算出する相対軌跡演算部と、同じく方位変化量と移動距離とに基づいて、絶対方位及び絶対位置を算出する絶対位置演算部と、相対軌跡演算部及び絶対位置演算部での算出値とＧＰＳ受信機での検出値との差を観測値とし、車速、方位、位置の算出に使用する各種算出パラメータや算出結果の誤差を状態量として、その状態量の推定値を求めるカルマンフィルタからなる誤差推定部と、誤差推定部により算出された状態量（即ち誤差）の推定値に従って、各演算部での算出パラメータや算出値を補正する補正部とを備えている。ここで、前記状態量の一つとして、ジャイロ出力から角速度への変換ゲインの誤差（ゲイン誤差）を推定する。そして、この誤差推定部により求められるゲイン誤差の推定値により、ジャイロ出力及び変換ゲインを用いて算出された方位変化量を補正する。

また、下記の特許文献１の第二実施例では、上記の構成に加えて、更に加速度センサ等からの出力に基づいて車両姿勢の傾斜角度を演算する姿勢傾斜角度演算部も備えている。そして、上記のようにカルマンフィルタの演算結果に基づいて算出されるゲイン補正量と、姿勢傾斜角度演算部により求められる車両姿勢の前後方向の傾斜角度及び横方向の傾斜角度から決定される姿勢変動補正量とを用いて、ゲイン補正量を調整する構成が記載されている。

この車両用現在位置検出装置は、ジャイロの経年変化や設置角度、更には車両姿勢の変化等により、ジャイロの換算ゲインがゲイン誤差を含むような場合であっても、装置構成を複雑化することなく、ジャイロの出力から方位変化量を安定して精度よく求めることを可能とすることを目的としている。

特開２０００−５５６７８号公報（第４−５頁、第９−１０頁、第８−９図）

上記の特許文献１に記載された技術では、車両の方位変更の有無等に関係なく、ＧＰＳ受信機からの情報が得られる毎に誤差を算出する処理を繰り返しており、ジャイロの変換ゲインの誤差を随時修正することができるという利点がある。しかし、カルマンフィルタにより、相対軌跡演算部及び絶対位置演算部での算出値とＧＰＳ受信機での検出値との差を観測値とし、前記状態量の一つとして、ジャイロのゲイン誤差を推定する処理を常に行うためには、演算処理装置による演算負荷が大きく、高性能の演算処理装置が必要になるという問題がある。

一方、ジャイロからの出力とＧＰＳ受信機での検出値との差異が顕著になるのは、比較的大きな角度での方位変更を行った場合である。したがって、このような方位変更の前後における、ジャイロからの情報に基づいて演算される方位変化量とＧＰＳ受信機からの情報に基づいて演算される方位変化量とを比較して誤差を演算し、ジャイロの出力情報を補正する方法とすれば、演算処理装置による演算負荷を低減することができる。ところで、この方法においても、ジャイロの設置角度や車両姿勢の傾斜角度に応じた補正を行うことが望ましい。しかしながら、方位変更の前後におけるジャイロ及びＧＰＳ受信機からの情報に基づいてジャイロの出力情報を補正する場合において、ジャイロの設置角度や車両姿勢の動的な傾斜角度の変化に応じて、適切にジャイロの出力情報の誤差を補正するための技術は確立されていなかった。

本発明は、上記の課題に鑑みてなされたものであり、その目的は、方位変更の前後におけるＧＰＳ情報と方位センサ情報とに基づいて、方位センサ情報を補正するための補正係数を演算する場合において、方位センサの静的な設置角度及びその設置角度の動的な変化を考慮した動的補正係数を適切に演算することが可能な方位センサの補正係数演算装置及びそのプログラム、並びにそれを用いた自位置認識装置及びそのプログラムを提供することにある。

上記目的を達成するための本発明に係る方位センサの補正係数演算装置の特徴構成は、ＧＰＳ衛星からの信号に基づいて位置情報及び進行方位情報を含むＧＰＳ情報を取得するＧＰＳ情報取得手段と、方位センサからの方位センサ情報を取得する方位センサ情報取得手段と、前記ＧＰＳ情報に基づいて進行方位の変化量を演算するＧＰＳ方位変化量演算手段と、前記方位センサ情報に基づいて進行方位の変化量を演算するセンサ方位変化量演算手段と、前記ＧＰＳ方位変化量と前記センサ方位変化量との差異に基づいて、前記方位センサ情報の基本補正係数を演算する基本補正係数演算手段と、前記方位センサの検出軸の静的な設置角度に応じて前記基本補正係数を補正した静的補正係数を演算する静的補正係数演算手段と、前記方位センサの検出軸方向の動的変化を検出する動的変化検出手段と、前記方位センサの検出軸方向の動的変化に応じて、前記静的補正係数を動的に補正して動的補正係数を演算する動的補正係数演算手段と、前記ＧＰＳ方位変化量と前記センサ方位変化量との方位変化比率を演算する方位変化比率演算手段と、前記方位変化比率演算手段の演算結果としての前記方位変化比率の情報を記憶する記憶手段と、自位置近傍の近傍領域を設定する近傍領域設定手段と、前記近傍領域に対して進行方向後方に、前記近傍領域からの距離が異なる複数の後方領域を設定する後方領域設定手段と、を備え、前記方位変化比率演算手段は、複数の前記後方領域について、前記近傍領域との間での前記方位変化比率をそれぞれ演算し、それぞれの前記方位変化比率の情報を前記記憶手段に記憶させ、前記基本補正係数演算手段は、前記記憶手段に記憶された複数の方位変化比率の全部又は一部の平均値に基づいて前記基本補正係数を演算する点にある。

この特徴構成によれば、ＧＰＳ方位変化量とセンサ方位変化量との差異に基づいて基本補正係数を演算し、この基本補正係数に基づいて静的補正係数及び動的補正係数を演算するので、無駄な演算処理を回避して装置の演算負荷を低く抑えることが可能となる。また、方位センサの検出軸の静的な設置角度に応じて基本補正係数を補正して静的補正係数を演算し、更に、地面の勾配や設置位置の揺動等による方位センサの検出軸方向の動的変化に応じて、静的補正係数を動的に補正して動的補正係数を演算するので、方位センサの静的な設置角度の傾斜やその設置角度の動的な変化を考慮した補正係数を適切に演算することができ、方位センサの補正係数の正確性を高めることが可能となる。また、上記のように構成すれば、記憶手段に記憶された複数の方位変化比率を用いることになり、方位変化比率の演算結果の中の特異な情報による影響を低減し、平均的な方位変化比率に基づいて方位センサの補正係数を演算することができるので、基本補正係数の正確性を高め、更にはそれに基づいて演算される静的補正係数及び動的補正係数の正確性を高めることが可能となる。

また、上記目的を達成するための本発明に係る方位センサの補正係数演算プログラムの特徴構成は、ＧＰＳ衛星からの信号に基づいて位置情報及び進行方位情報を含むＧＰＳ情報を取得するＧＰＳ情報取得ステップからのＧＰＳ情報に基づいて進行方位の変化量を演算するＧＰＳ方位変化量演算ステップと、方位センサからの方位センサ情報に基づいて進行方位の変化量を演算するセンサ方位変化量演算ステップと、前記ＧＰＳ方位変化量と前記センサ方位変化量との差異に基づいて、前記方位センサ情報の基本補正係数を演算する基本補正係数演算ステップと、前記方位センサの検出軸の静的な設置角度に応じて前記基本補正係数を補正した静的補正係数を演算する静的補正係数演算ステップと、前記方位センサの検出軸方向の動的変化を検出する動的変化検出ステップと、前記方位センサの検出軸方向の動的変化に応じて、前記静的補正係数を動的に補正して動的補正係数を演算する動的補正係数演算ステップと、前記ＧＰＳ方位変化量と前記センサ方位変化量との比率を演算する方位変化比率演算ステップと、前記方位変化比率演算ステップの演算結果としての方位変化比率の情報を所定の記憶手段に記憶する記憶ステップと、自位置近傍の近傍領域を設定する近傍領域設定ステップと、前記近傍領域に対して進行方向後方に、前記近傍領域からの距離が異なる複数の後方領域を設定する後方領域設定ステップと、をコンピュータに実行させ、前記方位変化比率演算ステップでは、複数の前記後方領域について、前記近傍領域との間での前記方位変化比率をそれぞれ演算し、それぞれの前記方位変化比率の情報を前記記憶手段に記憶させ、前記基本補正係数演算ステップでは、前記記憶手段に記憶された複数の方位変化比率の全部又は一部の平均値に基づいて前記基本補正係数を演算する点にある。

ここで、前記基本補正係数演算手段は、前記記憶手段に記憶された複数の方位変化比率のうち、最新の一部の平均値に基づいて前記基本補正係数を演算する構成とすると好適である。

また、上記方位センサの補正係数演算プログラムは、前記基本補正係数演算ステップでは、前記記憶手段に記憶された複数の方位変化比率のうち、最新の一部の平均値に基づいて前記基本補正係数を演算する構成とすると好適である。

また、前記方位センサ情報に基づいて、前記近傍領域と前記後方領域との間で異なる方向への複数回の方位変更があったか否かを判定する複数回方位変更判定手段を更に備え、前記複数回方位変更判定手段により複数回の方位変更があったと判定された場合には、前記基本補正係数演算手段は、複数回の方位変更があったと判定された前記近傍領域と前記後方領域との間の前記方位変化比率の情報を、前記基本補正係数の演算対象から除外する構成とすると好適である。

また、上記方位センサの補正係数演算プログラムは、前記方位センサ情報に基づいて、前記近傍領域と前記後方領域との間で異なる方向への複数回の方位変更があったか否かを判定する複数回方位変更判定ステップを更に実行し、前記複数回方位変更判定ステップで複数回の方位変更があったと判定された場合には、前記基本補正係数演算ステップでは、複数回の方位変更があったと判定された前記近傍領域と前記後方領域との間の前記方位変化比率の情報を、前記基本補正係数の演算対象から除外する構成とすると好適である。

また、前記静的補正係数演算手段は、前記方位センサの検出軸の静的な設置角度が、鉛直方向に対してα°である場合に、前記静的補正係数を、
（前記静的補正係数）＝（前記基本補正係数）×ｃｏｓα
として演算し、前記動的補正係数演算手段は、鉛直方向に対して前記設置角度α°傾斜した方向を基準として、前記方位センサの検出軸方向の動的変化角度がβ°である場合に、前記動的補正係数を、
（前記動的補正係数）＝（前記静的補正係数）／ｃｏｓα×ｃｏｓ（α＋β）
として演算する構成とすると好適である。

このように構成すれば、方位センサの静的な設置角度の傾斜やその設置角度の動的な変化を考慮した静的補正係数及び動的補正係数を適切に演算することができる。

また、上記方位センサの補正係数演算プログラムは、前記静的補正係数演算ステップにおいて、前記方位センサの検出軸の静的な設置角度が、鉛直方向に対してα°である場合に、前記静的補正係数を、
（前記静的補正係数）＝（前記基本補正係数）×ｃｏｓα
として演算し、
前記動的補正係数演算ステップにおいて、鉛直方向に対して前記設置角度α°傾斜した方向を基準として、前記方位センサの検出軸方向の動的変化角度がβ°である場合に、前記動的補正係数を、
（前記動的補正係数）＝（前記静的補正係数）／ｃｏｓα×ｃｏｓ（α＋β）
として演算する構成とすると好適である。

また、前記方位センサの検出軸方向の動的変化に関する条件を含む所定の動的補正実行条件を満たすか否かを判定し、前記動的補正実行条件を満たさない場合には、前記動的補正係数演算手段による動的補正係数の演算を省略する構成とすると好適である。

このように構成すれば、方位センサの検出軸方向の動的変化がない場合等のように、動的補正係数による方位センサ情報の補正が不要である場合に、動的補正係数の演算を省略することができるので、無駄な演算処理を回避してより一層装置の演算負荷を低く抑えることが可能となる。

また、上記方位センサの補正係数演算プログラムは、前記方位センサの検出軸方向の動的変化に関する条件を含む所定の動的補正実行条件を満たすか否かを判定し、前記動的補正実行条件を満たさない場合には、前記動的補正係数演算ステップを省略する構成とすると好適である。

本発明に係る自位置認識装置の特徴構成は、上記のように構成された方位センサの補正係数演算装置を備えた自位置認識装置であって、前記静的補正係数及び前記動的補正係数の少なくとも一方を用いて前記方位センサ情報を補正する方位センサ情報補正手段と、移動距離を検出する距離センサからの距離センサ情報を取得する距離センサ情報取得手段と、補正後の前記方位センサ情報と前記距離センサ情報とに基づいて自位置の移動軌跡を演算し、自位置の認識を行う自位置演算手段と、を備える点にある。

この特徴構成によれば、静的補正係数及び動的補正係数の少なくとも一方による補正後の方位センサ情報と距離センサ情報とに基づいて自位置の認識を行うことができる。したがって、自律航法による自位置認識を行う場合において、精度の高い自位置認識が可能となる。

また、本発明に係る自位置認識プログラムの特徴構成は、上記のように構成された方位センサの補正係数演算プログラムを備えた自位置認識プログラムであって、前記静的補正係数及び前記動的補正係数の少なくとも一方を用いて前記方位センサ情報を補正する方位センサ情報補正ステップと、移動距離を検出する距離センサからの距離センサ情報を取得する距離センサ情報取得ステップと、補正後の前記方位センサ情報と移動距離を検出する距離センサからの距離センサ情報とに基づいて自位置の移動軌跡を演算し、自位置の認識を行う自位置演算ステップと、をコンピュータに実行させる点にある。

本発明の実施形態に係る自車位置認識装置の概略構成を示すブロック図 方位変更の前後におけるＧＰＳ情報とジャイロ情報との関係の一例を示す模式図 複数回方位変更があったときのジャイロ情報の一例を示す模式図 ジャイロセンサの出力と設置角度との関係を示す図 ジャイロセンサの出力と設置角度及び動的変化角度との関係を示す図 本発明の実施形態に係る補正係数の演算処理を示すフローチャート 本発明の実施形態に係る補正係数の演算処理を示すフローチャート 本発明の実施形態に係る自車位置の認識処理を示すフローチャート 本発明のその他の実施形態に係る基本補正係数を演算するためのヒストグラム

以下に、本発明の実施形態について図面に基づいて説明する。本実施形態においては、本発明に係る方位センサの補正係数演算装置１を備える車両の自車位置認識装置２を例として説明する。図１は、本実施形態に係る自車位置認識装置２の概略構成を示すブロック図である。

本実施形態に係る自車位置認識装置２は、ＧＰＳ（グローバル・ポジショニング・システム）衛星からの信号をＧＰＳ受信機３により受信して取得したＧＰＳ情報に基づいて方位センサとしてのジャイロセンサ４から取得したジャイロ情報を補正し、当該補正後のジャイロ情報と、距離センサ５から取得したパルス数情報とに基づいて、自車位置の移動軌跡を演算し、現在の自車位置の認識を行う。
以下、図１に基づいて、自車位置認識装置２の各部の構成について詳細に説明する。なお、自車位置認識装置２の各機能部及び手段は、ＣＰＵ等の演算処理装置を中核部材として、入力されたデータに対して種々の処理を行うための機能部がハードウエア又はソフトウエア（プログラム）或いはその両方により実装されて構成されている。

ＧＰＳ受信機３は、ＧＰＳ衛星からの信号を受信する。このＧＰＳ衛星からの信号は、１秒おきに受信され、ＧＰＳ情報取得部６に送られる。ＧＰＳ情報取得部６は、受信されたＧＰＳ衛星からの信号を解析し、自車の位置（緯度及び経度）情報、信号受信時の最新のトリップ数情報（以下「ＧＰＳトリップ数情報」という）、進行方位情報、移動速度情報、測位衛星数情報、誤差半径情報、高さ方向速度情報等の情報を取得し、これらの情報を含むＧＰＳ情報を生成する。このＧＰＳ情報は、ＧＰＳ衛星からの信号を受信する毎、すなわち１秒おきに周期的に生成され、ＧＰＳ情報記憶部７に記憶される。ここでは、ＧＰＳ情報取得部６が本発明における「ＧＰＳ情報取得手段」を構成する。

本実施形態に係る自車位置認識装置２では、後述する距離センサ５により検出される自車位置の移動距離に基づいて決定される自車位置認識装置２内における移動距離の最小単位、すなわち単位距離を１トリップとしている。本例では、距離センサ５出力信号の３パルスを１トリップと規定し、１トリップを約１．２〔ｍ〕と規定している。そして、上記ＧＰＳトリップ数情報は、当該ＧＰＳ情報の生成元であるＧＰＳ衛星からの信号の受信時の最新のトリップ数を示す情報である。

ジャイロセンサ４は、自車方位の変化を検出する方位センサである。このジャイロセンサ４としては、振動ジャイロ、光ファイバジャイロ等の各種のジャイロスコープが使用可能であるが、本例では、振動ジャイロを用いることとする。そして、このジャイロセンサ４は、所定の検出軸Ｘ（図４参照）回りの回転角速度に比例した電圧等の出力を発生する。ジャイロセンサ４からの出力は、ジャイロ情報取得部８に送られる。ジャイロ情報取得部８は、ジャイロセンサ４からの電圧等のアナログ信号の出力をデジタル信号に変換するアナログ／デジタル変換を行い、複数段階（例えば２５６段階）のデジタル信号でなるジャイロ出力値を生成する。そして、生成されたジャイロ出力値及び単位出力値当りの分解能に基づいて方位変化量を表すジャイロ情報を生成する。このジャイロ情報は、ここでは、自車位置の移動に伴って１トリップ毎にジャイロ情報取得部８において生成され、ジャイロ情報記憶部９に記憶される。そして、このジャイロ情報を積算することで自車の進行方位を求めることができる。ここでは、ジャイロセンサ４が本発明における「方位センサ」に相当し、ジャイロ情報が本発明における「方位センサ情報」に相当し、ジャイロ情報取得部８が本発明における「方位センサ情報取得手段」を構成する。

距離センサ５は、自車位置の移動距離を検出するセンサである。本例では、距離センサ５として車速パルスセンサを用いることとする。そして、この距離センサ５は、車両のドライブシャフトやホイール等が一定量回転する毎にパルス信号を出力する。距離センサ５からの出力は、パルス数情報取得部１０に送られる。パルス数情報取得部１０は、距離センサ５から出力されたパルス信号のパルス数を積算してパルス数情報を生成する。このパルス数情報は、ここでは、自車位置の移動に伴って１トリップ（本例では３パルス）毎にパルス数情報取得部１０において生成され、パルス数情報記憶部１１に記憶される。このパルス数情報記憶部１１に記憶された任意の２点間のパルス数情報の差分に１パルス当りの標準距離（本例では０．４〔ｍ〕）を乗じることで、移動距離を演算することができる。そして、このパルス数情報から演算される移動距離の情報と上記ジャイロ情報から演算される進行方位の情報とを組み合わせることにより、自律航法による自位置認識が可能となる。ここでは、パルス数情報が本発明における「距離センサ情報」に相当し、パルス数情報取得部１０が本発明における「距離センサ情報取得手段」を構成する。

勾配センサ４１は、自車の前後方向の傾斜角度、すなわち勾配角度を検出するためのセンサである。本例では、勾配センサ４１として加速度センサ等を用いることとする。勾配情報取得部４２は、勾配センサ４１からの出力に基づいて、自車の前後方向の傾斜角度を表す勾配角度情報を取得する。本例では、ジャイロセンサ４は、図４に示すように、鉛直方向Ｖに対してα〔°〕傾いた状態で自車両に設置されており、その検出軸Ｘはジャイロセンサ４の底面に直交する方向に固定されている。したがって、この勾配角度情報に示される自車の前後方向の傾斜角度（図５におけるβ）の変化により、ジャイロセンサ４の検出軸Ｘ方向の動的変化を検出することができる。よって、ここでは、勾配センサ４１及び勾配情報取得部４２が、本発明における「動的変化検出手段」を構成する。

領域設定部１２は、近傍領域設定手段１３及び後方領域設定手段１４を有している。近傍領域設定手段１３は、自車位置近傍に所定の近傍領域ａｒＡを設定する手段である。また、後方領域設定手段１４は、近傍領域ａｒＡに対して進行方向後方に所定距離離間した後方領域ａｒＢ、ａｒＣ・・・を設定する手段である。近傍領域ａｒＡ及び後方領域ａｒＢ、ａｒＣ・・・については、図２を用いて説明する。
図２は、方位変更（右折）の前後におけるＧＰＳ情報とジャイロ情報との関係の一例を示す模式図である。この図においては、ＧＰＳ軌跡ＬｇがＧＰＳ情報に基づく自車位置の移動軌跡を表し、センサ軌跡Ｌｓがパルス数情報及びジャイロ情報に基づいて演算された自車位置の移動軌跡を表している。ＧＰＳ軌跡Ｌｇ上には、演算に使用されるＧＰＳ情報Ａ１〜Ａ３、Ｂ１〜Ｂ３等が丸印で表され、それらの各ＧＰＳ情報に示される進行方位が矢印で表されている。また、センサ軌跡Ｌｓ上には、演算に使用される所定点の位置が丸印で表されている。ここで、センサ軌跡Ｌｓ上の各点は、トリップ数を基準として規定されており、このトリップ数によりＧＰＳトリップ数情報を有する各ＧＰＳ情報と関連付けられている。

図２に示すように、本例では、近傍領域ａｒＡは、現在のトリップ数の点である現在点Ｐｇ０から３個前までのＧＰＳ情報Ａ１〜Ａ３を選択した上で、現在点Ｐｇ０を前端とし、ＧＰＳ情報Ａ３のＧＰＳトリップ数情報に示されるトリップ数に対して１０トリップ進行方向後方に遡った点Ｐｇ１を後端とする領域に設定している。なお、ここでは、ＧＰＳ情報の名称は、進行方向前方側から順にＡ１、Ａ２、Ａ３としている。また、ＧＰＳ情報Ａ１、Ａ２、Ａ３にそれぞれ対応するセンサ軌跡Ｌｓ上の点はＰａ１、Ｐａ２、Ｐａ３としている。
また、本例では、後方領域ａｒＢは、現在点Ｐｇ０から１００トリップ進行方向後方に遡った点を基準点としてそこから３個前までのＧＰＳ情報Ｂ１〜Ｂ３を選択した上で、ＧＰＳ情報Ｂ１のＧＰＳトリップ数情報に示されるトリップ数に対して１０トリップ進行方向前方に進んだ点Ｐｇ２を前端とし、ＧＰＳ情報Ｂ３のＧＰＳトリップ数情報に示されるトリップ数に対して１０トリップ進行方向後方に遡った点Ｐｇ３を後端とする領域に設定している。なお、ここでは、ＧＰＳ情報の名称は、進行方向前方側から順にＢ１、Ｂ２、Ｂ３としている。また、ＧＰＳ情報Ｂ１、Ｂ２、Ｂ３にそれぞれ対応するセンサ軌跡Ｌｓ上の点はＰｂ１、Ｐｂ２、Ｐｂ３としている。
また、後方領域設定手段１４は、近傍領域ａｒＡからの距離が異なる複数の領域を後方領域ａｒＢ、ａｒＣ・・・として順に設定する処理を行う。そのため、後方領域ａｒＣ以降についても、現在点Ｐｇ０から進行方向後方に、例えば、１５０トリップ、２００トリップ、２５０トリップ、３００トリップ、３５０トリップ、４００トリップ遡った点を順に基準点として、同様に後方領域ａｒＣ、ａｒＤ・・・ａｒＨまで設定する。なお、以下の説明においては、後方領域ａｒＢが設定されている場合を基本として説明するが、他の後方領域ａｒＣ、ａｒＤ・・・ａｒＨが設定されている場合であっても同様の処理が行われる。

図１に示すように、ＧＰＳ条件判定部１５は、ＧＰＳ直進判定手段１６、速度判定手段１７、受信状態判定手段１８、及びＧＰＳ方位変更判定手段１９を有している。
ＧＰＳ直進判定手段１６は、ＧＰＳ情報に基づいて、近傍領域ａｒＡ内で自車位置が直進したか否かを判定する手段である。本例では、ＧＰＳ直進判定手段１６は、ＧＰＳ情報Ａ１、Ａ２、Ａ３に含まれる進行方位情報に基づいて、ＧＰＳ情報Ａ１とＡ２、Ａ２とＡ３、Ａ１とＡ３のそれぞれの間での方位変化量の絶対値が３〔°〕以下であるか否かを判定している。ここでは、このＧＰＳ直進判定手段１６が本発明における「第一直進判定手段」を構成する。

速度判定手段１７は、ＧＰＳ情報に基づいて、自車が所定速度以上で走行中か否かを判定する手段である。本例では、ＧＰＳ情報Ａ１、Ａ２、Ａ３に含まれる移動速度情報に基づいて、ＧＰＳ情報Ａ１、Ａ２、Ａ３の全てで速度が５〔ｍ／ｓ〕以上であるか否かを判定している。
受信状態判定手段１８は、ＧＰＳ情報を取得した際のＧＰＳ衛星からの信号の受信状態の良否を判定する手段である。本例では、ＧＰＳ情報Ａ１、Ａ２、Ａ３に含まれる測位衛星数情報に基づいて、ＧＰＳ情報Ａ１、Ａ２、Ａ３の全てで測位衛星数が４個であるか否かを判定している。
ＧＰＳ方位変更判定手段１９は、後述するＧＰＳ方位変化量演算手段２２により演算されるＧＰＳ方位変化量ａｎｇ−ＧＰＳ（図２参照）に基づいて、近傍領域ａｒＡと後方領域ａｒＢとの間で方位変更があったか否かを判定する手段である。本例では、ＧＰＳ方位変化量ａｎｇ−ＧＰＳの絶対値が６０°以上１２０°以下（６０°≦｜ａｎｇ−ＧＰＳ｜≦１２０°）であるか否かを判定している。

ＧＰＳ情報演算部２０は、ＧＰＳ方位演算手段２１及びＧＰＳ方位変化量演算手段２２を有している。
ＧＰＳ方位演算手段２１は、ＧＰＳ情報に基づいて、近傍領域ａｒＡ内の進行方位、及び後方領域ａｒＢ内の進行方位を演算する手段である。本例では、ＧＰＳ方位演算手段２１は、図２に示すように、ＧＰＳ情報Ａ１、Ａ２、Ａ３に含まれる進行方位情報に基づいて、ＧＰＳ情報Ａ１、Ａ２、Ａ３に含まれる進行方位の平均値を演算し、それを近傍領域ａｒＡ内のＧＰＳ方位ａｎｇ−Ａとする。また、ＧＰＳ方位演算手段２１は、ＧＰＳ情報Ｂ１、Ｂ２、Ｂ３に含まれる進行方位情報に基づいて、ＧＰＳ情報Ｂ１、Ｂ２、Ｂ３に含まれる進行方位の平均値を演算し、それを後方領域ａｒＢ内のＧＰＳ方位ａｎｇ−Ｂとする。
ＧＰＳ方位変化量演算手段２２は、ＧＰＳ方位演算手段２１により演算された近傍領域ａｒＡ内のＧＰＳ方位ａｎｇ−Ａと後方領域ａｒＢ内のＧＰＳ方位ａｎｇ−Ｂとの差を、ＧＰＳ方位変化量ａｎｇ−ＧＰＳとして演算する。なお、ここでは、右方向に方位変化したときはＧＰＳ方位変化量ａｎｇ−ＧＰＳの値が正となり、左方向に方位変化したときはＧＰＳ方位変化量ａｎｇ−ＧＰＳの値が負となるように設定している。なお、左右と正負の関係を逆にすることも当然に可能である。

センサ条件判定部２３は、センサ直進判定手段２４、センサ方位変更判定手段２５、及び複数回方位変更判定手段２６を有している。
センサ直進判定手段２４は、ジャイロ情報に基づいて、近傍領域ａｒＡ及び後方領域ａｒＢの双方の領域内で自車位置が直進したか否かを判定する手段である。本例では、図２に示すように、センサ直進判定手段２４は、近傍領域ａｒＡの両端の点である現在点Ｐｇ０と点Ｐｇ１との間での方位変化量の絶対値が３〔°〕以下であり、且つ後方領域ａｒＢの両端の点である点Ｐｇ２と点Ｐｇ３との間での方位変化量の絶対値が３〔°〕以下であるか否かを判定している。ここでは、このセンサ直進判定手段２４が本発明における「第二直進判定手段」を構成する。
センサ方位変更判定手段２５は、後述するセンサ方位変化量演算手段２９により演算されるセンサ方位変化量ａｎｇ−ＳＥＮ（図２参照）に基づいて、近傍領域ａｒＡと後方領域ａｒＢとの間で方位変更があったか否かを判定する手段である。本例では、センサ方位変化量ａｎｇ−ＳＥＮの絶対値が３０°以上（３０°≦｜ａｎｇ−ＳＥＮ｜）であるか否かを判定している。

複数回方位変更判定手段２６は、ジャイロ情報に基づいて、近傍領域ａｒＡと後方領域ａｒＢとの間で異なる方向への複数回の方位変更があったか否かを判定する手段である。本例では、複数回方位変更判定手段２６は、近傍領域ａｒＡと後方領域ａｒＢとの間におけるジャイロ情報に基づいて、直進状態を基準として、右方向及び左方向の双方に方位変化したときに複数回の方位変更があったものと判定する。そして、この複数回方位変更判定手段２６により複数回の方位変更があったと判定された場合には、当該近傍領域ａｒＡと後方領域ａｒＢとの間に関する基本補正係数の演算を省略する。これにより、例えば図３に示すように、近傍領域ａｒＡの後端点Ｐｇ１と後方領域ａｒＢの前端点Ｐｇ２との間で全体としては右に方位変更している場合であっても、その中で右、左、右と細かく方位変更している場合には、当該近傍領域ａｒＡと後方領域ａｒＢとの間の情報は、基本補正係数の演算対象の情報から除外される。

センサ情報演算部２７は、センサ方位演算手段２８及びセンサ方位変化量演算手段２９を有している。
センサ方位演算手段２８は、ジャイロ情報に基づいて、近傍領域ａｒＡ内の進行方位、及び後方領域ａｒＢ内の進行方位を演算する手段である。本例では、センサ方位演算手段２８は、図２に示すように、ＧＰＳ情報Ａ１、Ａ２、Ａ３に含まれるＧＰＳトリップ数情報に示されるトリップ数（センサ軌跡Ｌｓ上の点Ｐａ１、Ｐａ２、Ｐａ３に対応）の平均値に最も近いトリップ数の点Ｐｍａを、近傍領域ａｒＡを代表する代表点として決定する。そして、この点Ｐｍａにおけるジャイロ情報に基づく進行方位を近傍領域ａｒＡのセンサ方位ａｎｇ−Ｐｍａとする。ここで、点Ｐｍａにおける進行方位は、前回取得した自車の進行方位を起点としてジャイロ情報に示される方位変化量を逐次積算して演算することができる。また、センサ方位演算手段２８は、ＧＰＳ情報Ｂ１、Ｂ２、Ｂ３に含まれるＧＰＳトリップ数情報に示されるトリップ数（センサ軌跡Ｌｓ上の点Ｐｂ１、Ｐｂ２、Ｐｂ３に対応）の平均値に最も近いトリップ数の点Ｐｍｂを、後方領域ａｒＢを代表する代表点として決定する。そして、この点Ｐｍｂにおけるジャイロ情報に基づく進行方位を後方領域ａｒＢのセンサ方位ａｎｇ−Ｐｍｂとする。ここで、点Ｐｍｂにおける進行方位は、前回取得した自車の進行方位を起点としてジャイロ情報に示される方位変化量を逐次積算して演算することができる。
センサ方位変化量演算手段２９は、センサ方位演算手段２８により演算された近傍領域ａｒＡのセンサ方位ａｎｇ−Ｐｍａと後方領域ａｒＢのセンサ方位ａｎｇ−Ｐｍｂとの差を、センサ方位変化量ａｎｇ−ＳＥＮとして演算する。なお、ここでは、右方向に方位変化したときはセンサ方位変化量ａｎｇ−ＳＥＮの値が正となり、左方向に方位変化したときはセンサ方位変化量ａｎｇ−ＳＥＮの値が負となるように設定している。なお、左右と正負の関係を逆にすることも当然に可能である。

方位変化比率演算部３０は、ＧＰＳ方位変化量演算手段２２により演算されたＧＰＳ方位変化量ａｎｇ−ＧＰＳと、センサ方位変化量演算手段２９により演算されたセンサ方位変化量ａｎｇ−ＳＥＮとの比率である方位変化比率を演算する手段である。具体的には、下記の式（１）により演算している。
（方位変化比率）＝ａｎｇ−ＳＥＮ／｜ａｎｇ−ＧＰＳ｜・・・（１）
ここでは、分子をセンサ方位変化量ａｎｇ−ＳＥＮとし、分母をＧＰＳ方位変化量ａｎｇ−ＧＰＳの絶対値としている。これは、右方向に方位変化したときは方位変化比率の値が正となり、左方向に方位変化したときは方位変化比率の値が負となるようにするためである。すなわち、ＧＰＳ方位変化量ａｎｇ−ＧＰＳ及びセンサ方位変化量ａｎｇ−ＳＥＮの値は、いずれも右方向に方位変化したときは正となり、左方向に方位変化したときは負となるように設定されている。したがって、ＧＰＳ方位変化量ａｎｇ−ＧＰＳ及びセンサ方位変化量ａｎｇ−ＳＥＮのいずれか一方を絶対値とすれば、方位変化比率の値を左右で正負が異なるようにできる。よって、センサ方位変化量ａｎｇ−ＳＥＮを絶対値とすることも可能である。ここでは、この方位変化比率演算部３０が本発明における「方位変化比率演算手段」を構成する。

また、本例では、方位変化比率演算部３０は、上記式（１）により演算した方位変化比率の絶対値が許容範囲、具体的には０．５以上１．１以下（０．５≦｜方位変化比率｜≦１．１）であるか否かを判定する。そして、方位変化比率演算部３０は、許容範囲外の値であるときには、許容範囲内の値に修正する処理を行う。具体的には、方位変化比率の絶対値が０．５未満である場合には、方位変化比率の絶対値を０．５に修正し、方位変化比率の絶対値が１．１より大きい場合には、方位変化比率の絶対値を１．１に修正する。なお、一般的に、センサ方位変化量ａｎｇ−ＳＥＮがＧＰＳ方位変化量ａｎｇ−ＧＰＳよりも絶対値で小さい値となることが多いため、本例における許容範囲は、１以下の範囲が広く設定されている。

比率記憶部３１は、方位変化比率演算部３０による演算結果としての方位変化比率の情報を記憶する手段である。この比率演算部３１は、メモリやバッファで構成され、複数の方位変化比率の情報を記憶可能となっている。ここでは、この比率記憶部３１が本発明における「記憶手段」を構成する。

基本補正係数演算部３２は、ＧＰＳ方位変化量ａｎｇ−ＧＰＳとセンサ方位変化量ａｎｇ−ＳＥＮとの差異に基づいて、ジャイロ情報を補正するための基本補正係数Ｄｂを演算する手段である。具体的には、基本補正係数演算部３２は、比率記憶部３１に記憶された複数の方位変化比率の全部又は一部の平均値に基づいて基本補正係数Ｄｂを演算する。本例では、比率記憶部３１に記憶された最新の８個の方位変化比率の平均値を演算し、その値を基本補正係数Ｄｂとする演算処理を行う。この基本補正係数Ｄｂの演算は、自車位置の移動中はその移動に従って繰り返し行なわれる。ここでは、この基本補正係数演算部３２が本発明における「基本補正係数演算手段」を構成する。

また、本例では、基本補正係数演算部３２は、方位変更の向きが右向きか左向きかに応じて、それぞれの向きに方位変更したときのＧＰＳ方位変化量ａｎｇ−ＧＰＳとセンサ方位変化量ａｎｇ−ＳＥＮとの差異に基づいて、右用基本補正係数Ｄｂｒ及び左用基本補正係数Ｄｂｌをそれぞれ演算する。すなわち、上記のとおり、方位変化比率の値は、右方向に方位変化したときは正、左方向に方位変化したときは負となるようにされている。したがって、正の値を有する方位変化比率のみを用いて右用基本補正係数Ｄｂｒを演算し、負の値を有する方位変化比率のみを用いて左用基本補正係数Ｄｂｌを演算する。なお、本実施形態の説明において基本補正係数Ｄｂというときは、右用基本補正係数Ｄｂｒ及び左用基本補正係数Ｄｂｌを総称するものとする。

静的補正係数演算部４３は、ジャイロセンサ４の検出軸Ｘの静的な設置角度に応じて基本補正係数Ｄｂを補正した静的補正係数Ｄｓを演算する手段である。具体的には、静的補正係数演算部４３は、ジャイロセンサ４の検出軸Ｘの静的な設置角度が、鉛直方向に対してα〔°〕である場合に、静的補正係数Ｄｓを、下記の式（２）により演算する。
（静的補正係数Ｄｓ）＝（基本補正係数Ｄｂ）×ｃｏｓα・・・（２）
ここで、図４は、ジャイロセンサ４の出力Ｊと設置角度α〔°〕との関係を示す図である。本願では、角度は、鉛直軸Ｖを基準とし、上り勾配側（図４及び図５における時計回りの方向）を正、下り勾配側（図４及び図５における反時計回りの方向）を負とする。この図４に示すように、ジャイロセンサ４の検出軸Ｘが水平走行時（勾配角度が０〔°〕のとき）における鉛直軸Ｖに対してα〔°〕傾斜して設置されている場合、ジャイロセンサ４の出力Ｊはｃｏｓα倍のＪｃｏｓαとなる。この場合、このジャイロセンサ４の出力Ｊｃｏｓαから生成されるジャイロ情報に示される方位変化量もｃｏｓα倍されることになる。したがって、上記の式（２）のように、基本補正係数Ｄｂをｃｏｓα倍した値を静的補正係数Ｄｓとすることにより、ジャイロセンサ４の検出軸Ｘの設置角度αに応じてジャイロ情報を適切に補正することができる。

本例では、設置角度α〔°〕は、ジャイロセンサ４の車両への取付時に、車両を水平にした状態を基準として測定した数値を入力して予め静的補正係数演算部４３において記憶しているものを用いる。ただし、この設置角度α〔°〕は、例えばセンサ等を用いて自動的に検出する構成とすることも可能である。
なお、静的補正係数Ｄｓについても、基本補正係数Ｄｂと同様に、右用静的補正係数Ｄｓｒと左用静的補正係数Ｄｓｌとがそれぞれ演算される。すなわち、右用基本補正係数Ｄｂｒを補正して右用静的補正係数Ｄｓｒが演算され、左用基本補正係数Ｄｂｌを補正して左用静的補正係数Ｄｓｌが演算される。ここでは、静的補正係数演算部４３が本発明における「静的補正係数演算手段」を構成する。

動的補正係数演算部４４は、ジャイロセンサ４の検出軸Ｘ方向の動的変化に応じて、静的補正係数Ｄｓを動的に補正して動的補正係数Ｄａを演算する手段である。具体的には、動的補正係数演算部４４は、鉛直方向に対して上記静的な設置角度α〔°〕傾斜した方向（図５におけるＸ´の方向）を基準として、ジャイロセンサ４の検出軸Ｘ方向の動的変化角度がβ〔°〕である場合に、動的補正係数Ｄａを、下記の式（３）により演算する。
（動的補正係数Ｄａ）＝（静的補正係数Ｄｓ）／ｃｏｓα×ｃｏｓ（α＋β）・・・（３）
ここで、図５は、ジャイロセンサ４の出力Ｊと設置角度α〔°〕及び動的変化角度β〔°〕との関係を示す図である。この図に示すように、動的変化角度β〔°〕は、水平走行時（勾配角度が０〔°〕のとき）のジャイロセンサ４の検出軸の方向Ｘ´、すなわち鉛直方向に対して上記静的な設置角度α〔°〕傾斜した方向を基準とする角度である。したがって、勾配角度すなわち動的変化角度がβ〔°〕である場合、検出軸Ｘは、水平走行時の検出軸Ｘ´に対してβ〔°〕傾斜するので、検出軸Ｘは、鉛直軸Ｖに対してα＋β〔°〕傾斜することになる。よって、ジャイロセンサ４の出力Ｊはｃｏｓ（α＋β）倍のＪｃｏｓ（α＋β）となる。この場合、このジャイロセンサ４の出力Ｊｃｏｓ（α＋β）から生成されるジャイロ情報に示される方位変化量もｃｏｓ（α＋β）倍されることになる。一方、上記のとおり、静的補正係数Ｄｓは基本補正係数Ｄｂをｃｏｓα倍して演算される。そこで、静的補正係数Ｄｓを（１／ｃｏｓα）倍して設置角度α〔°〕の影響を除去してからｃｏｓ（α＋β）倍した値を動的補正係数Ｄａとすることにより、ジャイロセンサ４の検出軸Ｘ方向の動的変化に応じてジャイロ情報を適切に補正することができる。

ここで、動的変化角度β〔°〕としては、勾配センサ４１及び勾配情報取得部４２により取得された勾配角度情報に示される角度が用いられる。ところで、図５では、動的変化角度β〔°〕が道路の傾斜角度に等しい場合を例として説明したが、実際の動的変化角度には、車両のピッチングによる傾斜等、静的な設置角度α〔°〕以外の自車の前後方向の全ての角度変化が含まれる。
なお、動的補正係数Ｄａについても、基本補正係数Ｄｂ及び静的補正係数Ｄｓと同様に、右用動的補正係数Ｄａｒと左用動的補正係数Ｄａｌとがそれぞれ演算される。すなわち、右用静的補正係数Ｄｓｒを補正して右用動的補正係数Ｄａｒが演算され、左用静的補正係数Ｄｓｌを補正して左用動的補正係数Ｄａｌが演算される。ここでは、動的補正係数演算部４４が本発明における「動的補正係数演算手段」を構成する。

ジャイロ情報補正部３３は、動的補正係数演算部４４により演算された最新の動的補正係数Ｄａを用いてジャイロ情報を補正する手段である。本例では、ジャイロ情報補正部３３は、動的補正係数Ｄａの逆数をジャイロ情報取得部８で取得されたジャイロ情報に乗じることにより補正後のジャイロ情報を取得する。具体的には、下記の式（４）により演算している。
（補正後のジャイロ情報）＝（ジャイロ情報）×（１／Ｄａ）・・・（４）
なお、上記のとおり、ジャイロ情報は、ジャイロセンサ４の出力値及び単位出力値当りの分解能に基づいて生成された方位変化量を表す情報である。一方、上記のとおり、動的補正係数Ｄａは、ジャイロセンサ４の検出軸Ｘ方向の動的変化に応じて静的補正係数Ｄｓを補正して演算され、この静的補正係数Ｄｓはジャイロセンサ４の検出軸Ｘの静的な設置角度に応じて基本補正係数Ｄｂを補正して演算されている。そして、基本補正係数Ｄｂは、複数（本例では８個）の方位変化比率（＝ａｎｇ−ＳＥＮ／｜ａｎｇ−ＧＰＳ｜）の平均値である。したがって、動的補正係数Ｄａの逆数をジャイロ情報に乗算することにより演算される補正後のジャイロ情報は、ジャイロセンサ４の検出軸Ｘ方向の動的変化及び静的な設置角度を反映するとともに、ＧＰＳ情報に基づいて最近のＧＰＳ方位変化量ａｎｇ−ＧＰＳとセンサ方位変化量ａｎｇ−ＳＥＮとの差異を反映して補正される。よって、補正後のジャイロ情報は、これらが考慮された正確な方位変化量を表すものとなる。ここでは、このジャイロ情報補正部３３が本発明における「方位センサ情報補正手段」を構成する。

自位置演算部３４は、補正後のジャイロ情報とパルス数情報とに基づいて自車位置の移動軌跡を演算する。すなわち、自位置演算部３４は、後述する自位置記憶部３５に記憶されている前回演算した自車の位置及び進行方位を起点として、パルス数情報取得部１０により取得したパルス数情報とジャイロ情報補正部３３による補正後のジャイロ情報とに基づいて、前記起点からの自車位置の移動軌跡を求める。ここで、自車位置の移動軌跡の演算は、前記起点における進行方位と前記補正後のジャイロ情報に示される方位変化量に基づいて演算される自車の進行方向に向かって、前記パルス数情報に予め定めた標準距離を乗じて演算される移動距離だけ前記起点の位置から移動したものとして行う。そして、この移動軌跡に基づいて自車の位置及び進行方位が演算される。自車の位置及び進行方位は、少なくともＧＰＳ情報取得部６によるＧＰＳ情報の取得時、ここでは１秒おきに周期的に演算される。この自位置演算部３４による演算結果は、現在の自車の位置及び進行方位として自位置記憶部３５に記憶される。また、この自車の位置及び進行方位の情報は、自車位置認識装置２の出力として、図示しないナビゲーション装置や車両制御装置等へ送られる。ここでは、この自位置演算部３４が本発明における「自位置演算手段」を構成する。

自位置記憶部３５は、前回までに逐次認識された自車の位置及び進行方位の情報を記憶する記憶手段である。この自位置記憶部３５に記憶されているこれまでの自車の位置及び進行方位の情報は、自位置演算部３４に送られて、現在の自車の位置及び進行方位の演算に使用される。

次に、図６〜８に示すフローチャートを用いて、本実施形態に係る補正係数演算装置１による補正係数Ｄの演算処理、及び自車位置認識装置２による自車位置の認識処理について説明する。なお、本実施形態の説明において補正係数Ｄというときは、基本補正係数Ｄｂ、静的補正係数Ｄｓ及び動的補正係数Ｄａを総称するものとする。

まず、図６及び図７を用いて補正係数Ｄの演算処理について説明する。以下に説明する各演算処理ステップは、補正係数演算プログラムに従って動作する補正係数演算装置１の各機能部及び手段により実行される。この処理では、初めに、近傍領域設定手段１３により、図２に示すように、現在の自車位置を基準として近傍領域ａｒＡを設定する（ステップ＃０１）。次に、ステップ＃０２〜＃０４において、近傍領域ａｒＡにおけるＧＰＳ情報が所定の演算開始条件を満たすか否かの判定を行う。この演算開始条件は、近傍領域ａｒＡにおいて自車位置が直進走行中であり、ＧＰＳ衛星からの信号の受信状態が良好であることについての条件である。すなわち、ステップ＃０２では、ＧＰＳ直進判定手段１６により、ＧＰＳ情報が近傍領域ａｒＡ内で直進状態を示しているか否かについて判定する。具体的には、上記のとおり、ＧＰＳ情報Ａ１とＡ２、Ａ２とＡ３、Ａ１とＡ３のそれぞれの間での方位変化量の絶対値が３〔°〕以下であるか否かを判定する。ステップ＃０３では、速度判定手段１７により、ＧＰＳ情報が近傍領域ａｒＡ内で走行中状態を示しているか否かについて判定する。具体的には、上記のとおり、ＧＰＳ情報Ａ１、Ａ２、Ａ３の全てで速度が５〔ｍ／ｓ〕以上であるか否かを判定する。ステップ＃０４では、受信状態判定手段１８により、ＧＰＳ情報を取得した際のＧＰＳ衛星からの信号の受信状態の良否を判定する。具体的には、上記のとおり、ＧＰＳ情報Ａ１、Ａ２、Ａ３の全てで測位衛星数が４個であるか否かを判定する。なお、これらのステップ＃０２〜＃０４の順序は特に限定されず、入れ替えることも可能である。そして、これらのステップ＃０２〜＃０４のいずれかの条件を満たしていない場合には、その後の補正係数Ｄの演算処理は実行されない。

上記ステップ＃０２〜＃０４の全ての条件を満たす場合には、次に、図２に示すように、センサ方位演算手段２８により近傍領域ａｒＡの代表点Ｐｍａを演算するとともに、ＧＰＳ方位演算手段２１により近傍領域ａｒＡ内のＧＰＳ方位ａｎｇ−Ａを演算する（ステップ＃０５）。次に、後方領域設定手段１４により、図２に示すように、後方領域ａｒＢを設定する（ステップ＃０６）。そして、センサ方位演算手段２８により後方領域ａｒＢの代表点Ｐｍｂを演算するとともに、ＧＰＳ方位演算手段２１により後方領域ａｒＢ内のＧＰＳ方位ａｎｇ−Ｂを演算する（ステップ＃０７）。その後、ＧＰＳ方位変化量演算手段２２により、近傍領域ａｒＡ内のＧＰＳ方位ａｎｇ−Ａと後方領域ａｒＢ内のＧＰＳ方位ａｎｇ−Ｂとの差を、ＧＰＳ方位変化量ａｎｇ−ＧＰＳとして演算する（ステップ＃０８）。

そして、ＧＰＳ方位変更判定手段１９により、ステップ＃０８で演算されたＧＰＳ方位変化量ａｎｇ−ＧＰＳの絶対値が６０°以上１２０°以下（６０°≦｜ａｎｇ−ＧＰＳ｜≦１２０°）であるか否かを判定する（ステップ＃０９）。これにより、近傍領域ａｒＡと後方領域ａｒＢとの間で方位変更があったか否かをＧＰＳ情報に基づいて判定する。近傍領域ａｒＡと後方領域ａｒＢとの間で方位変更があった場合には（ステップ＃０９：ＹＥＳ）、次に、センサ直進判定手段２４により、ジャイロ情報が、近傍領域ａｒＡ及び後方領域ａｒＢの双方の領域内で直進状態を示しているか否かについて判定する（ステップ＃１０）。具体的には、上記のとおり、近傍領域ａｒＡの両端の点である現在点Ｐｇ０と点Ｐｇ１との間での方位変化量の絶対値が３〔°〕以下であり、且つ後方領域ａｒＢの両端の点である点Ｐｇ２と点Ｐｇ３との間での方位変化量の絶対値が３〔°〕以下であるか否かを判定する。ステップ＃０９又は＃１０において条件を満たさない場合には、処理はステップ＃１７へ進み、次の後方領域ａｒＣに関する処理が実行される。

ジャイロ情報が、近傍領域ａｒＡ及び後方領域ａｒＢの双方の領域内で直進状態を示している場合には（ステップ＃１０：ＹＥＳ）、センサ方位演算手段２８により、近傍領域ａｒＡのセンサ方位ａｎｇ−Ｐｍａ及び後方領域ａｒＢのセンサ方位ａｎｇ−Ｐｍｂを演算する（ステップ＃１１）。そして、センサ方位変化量演算手段２９により、近傍領域ａｒＡのセンサ方位ａｎｇ−Ｐｍａと後方領域ａｒＢのセンサ方位ａｎｇ−Ｐｍｂとの差を、センサ方位変化量ａｎｇ−ＳＥＮとして演算する（ステップ＃１２）。

次に、センサ方位変更判定手段２５により、ステップ＃１２で演算されたセンサ方位変化量ａｎｇ−ＳＥＮの絶対値が３０°以上（３０°≦｜ａｎｇ−ＳＥＮ｜）であるか否かを判定する（ステップ＃１３）。これにより、近傍領域ａｒＡと後方領域ａｒＢとの間で方位変更があったか否かをジャイロ情報に基づいて確認的に判定する。センサ方位変化量ａｎｇ−ＳＥＮの絶対値が３０°未満である場合（ステップ＃１３：ＮＯ）には、処理はステップ＃１７へ進み、次の後方領域ａｒＣに関する処理が実行される。

一方、センサ方位変化量ａｎｇ−ＳＥＮの絶対値が３０°以上である場合（ステップ＃１３：ＹＥＳ）には、複数回方位変更判定手段２６により、ジャイロ情報に基づいて、例えば図３に示すような、近傍領域ａｒＡと後方領域ａｒＢとの間で異なる方向への複数回の方位変更があったか否かを判定する（ステップ＃１４）。そして、異なる方向への複数回の方位変更があった場合には（ステップ＃１５：ＹＥＳ）、当該近傍領域ａｒＡと後方領域ａｒＢとの間に関する補正係数の演算を省略し、処理は終了する。

一方、異なる方向への複数回の方位変更がない場合には（ステップ＃１５：ＮＯ）、方位変化比率演算部３０により、ステップ＃０８で演算されたＧＰＳ方位変化量ａｎｇ−ＧＰＳとステップ＃１２で演算されたセンサ方位変化量ａｎｇ−ＳＥＮとに基づいて、方位変化比率を演算する（ステップ＃１６）。具体的には、方位変化比率は、上記の式（１）により、分子をセンサ方位変化量ａｎｇ−ＳＥＮとし、分母をＧＰＳ方位変化量ａｎｇ−ＧＰＳの絶対値とする比率として演算される。そして、演算された方位変化比率の情報は、比率記憶部３１に記憶される（ステップ＃１７）。以上のような処理とすることにより、例えば交差点における右左折等の方位変更を行った場合等であって、自車の状態が直進状態から一方向に方位変更が行われた後に直進状態に戻った場合に、方位変化比率が演算され比率記憶部３１に記憶されることになる。

その後、全ての後方領域ａｒＢ、ａｒＣ、ａｒＤ・・・ａｒＨについてステップ＃０７〜＃１７の処理が終了したか否かについて判断する（ステップ＃１８）。すなわち、本例では、後方領域として、近傍領域ａｒＡからの距離が異なるａｒＢからａｒＨまでの７つの領域を順に設定することとしている。したがって、全ての後方領域ａｒＢ、ａｒＣ、ａｒＤ・・・ａｒＨについての処理が終了していない場合には（ステップ＃１８：ＮＯ）、一つの後方領域、例えばａｒＢについて処理が終了した後は、次の後方領域、例えばａｒＣを設定する（ステップ＃１９）。そして、当該次の後方領域について、同様にステップ＃０７〜＃１７の処理を実行する。

その後、最後の後方領域ａｒＨについての処理が終了した場合には（ステップ＃１８：ＹＥＳ）、次に、比率記憶部３１に方位変化比率の情報が８個以上記憶されているか否かについて判断する（ステップ＃２０）。ここで、比率記憶部３１に記憶されている方位変化比率の情報の数が８個未満である場合には（ステップ＃２０：ＮＯ）、基本補正係数Ｄｂを演算するには方位変化比率の情報が少ないため、基本補正係数Ｄｂの値を、予め定めた初期値とする（ステップ＃２１）。そして、基本補正係数Ｄｂの演算処理を終了する。一方、比率記憶部３１に記憶されている方位変化比率の情報の数が８個以上である場合には（ステップ＃２０：ＹＥＳ）、基本補正係数演算部３２により、基本補正係数Ｄｂを演算する（ステップ＃２２）。具体的には、上記のとおり、比率記憶部３１に記憶された最新の８個の方位変化比率の平均値を演算し、その値を基本補正係数Ｄｂとする。またこの際、方位変更の向きが右向きか左向きかに応じて、右用基本補正係数Ｄｂｒ及び左用基本補正係数Ｄｂｌをそれぞれ演算する。

次に、静的補正係数演算部４３により、静的補正係数Ｄｓを演算する（ステップ＃２３）。具体的には、上記のとおり、ジャイロセンサ４の検出軸Ｘの静的な設置角度が、鉛直方向に対してα〔°〕である場合に、基本補正係数Ｄｂをｃｏｓα倍することにより静的補正係数Ｄｓを演算する。そして、静的補正係数Ｄｓの値を新たに演算された値に更新する（ステップ＃２４）。

次に、勾配センサ４１及び勾配情報取得部４２により取得される勾配角度情報に基づいて、ジャイロセンサ４の検出軸Ｘ方向の動的変化を検出する（ステップ＃２５）。検出軸Ｘ方向の動的変化が検出されない場合（ステップ＃２５：ＮＯ）、すなわち水平走行している場合には、最新の静的補正係数Ｄｓを用いてジャイロ情報を補正することとし、補正係数Ｄの演算処理は終了する。一方、検出軸Ｘ方向の動的変化が検出された場合（ステップ＃２５：ＹＥＳ）には、動的補正係数演算部４４により、動的補正係数Ｄａを演算する（ステップ＃２６）。具体的には、上記のとおり、鉛直方向に対して上記静的な設置角度α〔°〕傾斜した方向（図５におけるＸ´の方向）を基準として、ジャイロセンサ４の検出軸Ｘ方向の動的変化角度がβ〔°〕である場合に、静的補正係数Ｄｓを（１／ｃｏｓα）倍して設置角度α〔°〕の影響を除去してからｃｏｓ（α＋β）倍することにより動的補正係数Ｄａを演算する。以上で、補正係数Ｄの演算処理を終了する。

次に、図８を用いて自車位置の認識処理について説明する。以下に説明する各演算処理ステップは、自位置認識プログラムに従って動作する自車位置認識装置２の各機能部及び手段により実行される。この処理では、初めに、補正係数Ｄの演算処理を行う（ステップ＃１０１）。この補正係数Ｄの演算処理は、図６及び図７を用いて既に説明したとおりである。そして、ジャイロ情報補正部３３により、ステップ＃１０１で演算された最新の動的補正係数Ｄａ（動的補正係数Ｄａが演算されなかった場合には最新の静的補正係数Ｄｓ）を用いて、ジャイロ情報を補正する（ステップ＃１０２）。具体的には、補正後のジャイロ情報は、上記の式（４）により、補正前のジャイロ情報に動的補正係数Ｄａ（静的補正係数Ｄｓ）の逆数を乗算して演算される。これにより、ＧＰＳ情報に基づいて補正された方位変化量を表すジャイロ情報（補正後のジャイロ情報）が取得される。

次に、自位置演算部３４により、補正後のジャイロ情報とパルス数情報とに基づいて自車位置の移動軌跡を演算する。具体的には、自位置記憶部３５に記憶されている前回演算した自車の位置及び進行方位を起点とし、この起点における進行方位と補正後のジャイロ情報に示される方位変化量に基づいて演算される自車の進行方向に向かって、パルス数情報に予め定めた標準距離を乗じて演算される移動距離だけ前記起点の位置から移動したものとして自車位置の移動軌跡を演算する。そして、この移動軌跡に基づいて自車位置認識、すなわち自車の位置及び進行方位が認識される（ステップ＃１０４）。その後、自車の位置が移動中である場合には（ステップ＃１０５：ＹＥＳ）、処理はステップ＃１０１へ戻り、更に補正係数Ｄの演算処理（ステップ＃１０１）から自車位置の認識（ステップ＃１０４）までの処理を繰り返し行う。そして、自車が停止した場合には（ステップ＃１０５：ＮＯ）、自車位置認識処理は終了する。

〔その他の実施形態〕
（１）上記の実施形態においては、ジャイロセンサ４の検出軸Ｘ方向の動的変化が検出されない場合（水平走行している場合）に動的補正係数Ｄａの演算を省略し、静的補正係数Ｄｓを用いてジャイロ情報を補正する場合について説明した。しかし、本発明の適用範囲はこの実施形態に限定されない。すなわち、所定の動的補正実行条件を満たすか否かを判定し、この動的補正実行条件を満たさない場合に、動的補正係数演算部４４による動的補正係数Ｄａの演算を省略することも好適な実施形態の一つである。ここで、所定の動的補正実行条件には、ジャイロセンサ４の検出軸Ｘ方向の動的変化に関する条件、具体的には、検出軸Ｘ方向の動的変化が所定範囲内（例えば±３〔°〕以下）であるという条件を含めると好適である。そして、検出軸Ｘ方向の動的変化が所定範囲内である場合には、動的補正係数Ｄａの演算を省略する。これにより、ジャイロ情報の動的な補正が不要である場合に、無駄な演算処理を回避してより一層装置の演算負荷を低く抑えることができる。
また、所定の動的補正実行条件としては、この他にも、例えば、自車位置がマップマッチングされていない状態であるという条件、自車位置が地図情報に基づいて勾配のない平坦な道路を走行中であるという条件等、検出軸Ｘ方向の動的変化に応じた補正を行うことが適切でない場合を規定する条件とすると好適である。
なお、動的補正係数演算部４４により常に動的補正係数Ｄａを演算し、常に最新の動的補正係数Ｄａを用いてジャイロ情報を補正する構成とすることも好適な実施形態の一つである。

（２）上記の実施形態においては、基本補正係数演算部３２が、比率記憶部３１に記憶された最新の８個の方位変化比率の平均値を演算し、その値を基本補正係数Ｄｂとする場合について説明した。しかし、基本補正係数Ｄｂの演算方法はこれに限定されるものではない。すなわち、例えば、図９に示すようなヒストグラムを用いて基本補正係数Ｄｂを演算することも好適な実施形態の一つである。すなわち、このヒストグラムは、横軸を方位変化比率、縦軸に度数としている。そして、方位変化比率演算部３０により演算された各方位変化比率に１度数を与え、ヒストグラムに登録していく。このヒストグラムは比率記憶部３１に記憶されている。登録数がある一定数以上（例えば１００個以上）になったときにヒストグラムに登録された方位変化比率の平均値を演算する。この際、ヒストグラムの全体平均値から両側に標準偏差σの範囲を設定し、この範囲内の方位変化比率の平均値を演算し、これを基本補正係数Ｄｂとする。これにより、比率記憶部３１に記憶された複数の方位変化比率の一部の平均値に基づいて基本補正係数Ｄｂが演算されることになる。

（３）上記の実施形態においては、受信状態判定手段１８が、ＧＰＳ衛星からの信号の受信状態として測位衛星数についてのみ判定する場合について説明した。しかし、これ以外のＧＰＳ衛星からの信号の受信状態について判定することも好適な実施形態の一つである。このような受信状態の判定条件としては、例えばＧＰＳ情報に含まれる誤差半径情報に示される誤差半径の条件を用いることができる。すなわち、例えば、誤差半径が３００〔ｍ〕以下であるときに受信状態が良好であると判定することができる。

（４）また、基本補正係数Ｄｂの正確性を高めるため、ＧＰＳ直進判定手段１６及び速度判定手段１７における判定条件を更に厳しくしても好適である。例えば、上記の実施形態においては、ＧＰＳ直進判定手段１６は、ＧＰＳ情報Ａ１とＡ２、Ａ２とＡ３、Ａ１とＡ３のそれぞれの間での方位変化量の絶対値が３〔°〕以下であるか否かを判定することとしているが、これを１．５〔°〕以下、或いは０．７〔°〕以下としても好適である。また、上記の実施形態においては、速度判定手段１７は、ＧＰＳ情報Ａ１、Ａ２、Ａ３の全てで速度が５〔ｍ／ｓ〕以上であるか否かを判定することとしているが、これを７．５〔ｍ／ｓ〕以上としても好適である。

（５）同様に、基本補正係数Ｄｂの正確性を高めるため、センサ直進判定手段２４における判定条件を更に厳しくしても好適である。例えば、上記の実施形態においては、センサ直進判定手段２４は、近傍領域ａｒＡの両端の点である現在点Ｐｇ０と点Ｐｇ１との間での方位変化量の絶対値が３〔°〕以下であり、且つ後方領域ａｒＢの両端の点である点Ｐｇ２と点Ｐｇ３との間での方位変化量の絶対値が３〔°〕以下であるか否かを判定しているが、これを共に１〔°〕以下としても好適である。

（６）上記の実施形態においては、ＧＰＳ直進判定手段１６、速度判定手段１７、及び受信状態判定手段１８が、近傍領域ａｒＡ内のＧＰＳ情報Ａ１、Ａ２、Ａ３に関してのみ判定を行う場合について説明した。しかし、ＧＰＳ直進判定手段１６、速度判定手段１７、及び受信状態判定手段１８が、各後方領域ａｒＣ、ａｒＤ・・・ａｒＨについても同様の判定を行う構成とすることも好適な実施形態の一つである。この場合、具体的な処理手順としては、図４のステップ＃０６で例えば後方領域ａｒＢを設定した後、ステップ＃０７の処理の前に、ステップ＃０２〜＃０４と同様の判定処理を当該後方領域ａｒＢについて行う。そして、これらの判定条件の全てを満たす場合にはステップ＃０７へ進む。一方、これらの判定条件の何れかを満たさない場合には、処理はステップ＃１８へ進む。ここでは全ての後方領域ａｒＢ、ａｒＣ、ａｒＤ・・・ａｒＨについて処理が終了していないので（ステップ＃１８：ＮＯ）、次の後方領域ａｒＣの設定を行う（ステップ＃１９）。このようにして、全ての後方領域ａｒＢ、ａｒＣ、ａｒＤ・・・ａｒＨについて、ステップ＃０２〜＃０４と同様の判定処理をおこない、これらの判定条件を満たす後方領域のみを方位変化比率の演算に用いる。

（７）上記実施形態においては、車両の自車位置認識装置２を例として説明したが、本発明は、車両以外の携帯型や船舶等の他の乗り物の自位置認識装置等に適用することも当然に可能である。

方位センサの静的な設置角度及びその設置角度の動的な変化を考慮した動的補正係数を適切に演算することが可能な方位センサの補正係数演算装置及び演算プログラム、並びに自位置認識装置及び認識プログラムを得ることができた。

１：補正係数演算装置
２：自車位置認識装置
４：ジャイロセンサ（方位センサ）
５：距離センサ
６：ＧＰＳ情報取得部（ＧＰＳ情報取得手段）
８：ジャイロ情報取得部（方位センサ情報取得手段）
１０：パルス数情報取得部（距離センサ情報取得手段）
１４：後方領域設定手段
１６：ＧＰＳ直進判定手段（第一直進判定手段）
１９：ＧＰＳ方位変更判定手段
２２：ＧＰＳ方位変化量演算手段
２４：センサ直進判定手段（第二直進判定手段）
２６：複数回方位変更判定手段
２９：センサ方位変化量演算手段
３０：方位変化比率演算部（方位変化比率演算手段）
３１：比率記憶部（記憶手段）
３２：基本補正係数演算部（基本補正係数演算手段）
３３：ジャイロ情報補正部（方位センサ情報補正手段）
３４：自位置演算部（自位置演算手段）
４１：勾配センサ（動的変化検出手段）
４２：勾配情報取得部（動的変化検出手段）
４３：静的補正係数演算部（静的補正係数演算手段）
４４：動的補正係数演算部（動的補正係数演算手段）
Ｄｂ：基本補正係数
Ｄｓ：静的補正係数
Ｄａ：動的補正係数
Ｘ：ジャイロセンサの検出軸
ａｎｇ−ＧＰＳ：ＧＰＳ方位変化量
ａｎｇ−ＳＥＮ：センサ方位変化量

Claims (10)

1. ＧＰＳ衛星からの信号に基づいて位置情報及び進行方位情報を含むＧＰＳ情報を取得するＧＰＳ情報取得手段と、
   方位センサからの方位センサ情報を取得する方位センサ情報取得手段と、
   前記ＧＰＳ情報に基づいて、方位変更の前後での進行方位の変化量を演算するＧＰＳ方位変化量演算手段と、
   前記方位センサ情報に基づいて、前記方位変更の前後での進行方位の変化量を演算するセンサ方位変化量演算手段と、
   前記方位変更の前後における前記ＧＰＳ方位変化量と前記センサ方位変化量との差異に基づいて、前記方位センサ情報の基本補正係数を演算する基本補正係数演算手段と、
   前記方位センサの検出軸の静的な設置角度に応じて前記基本補正係数を補正した静的補正係数を演算する静的補正係数演算手段と、
   前記方位センサの検出軸方向の動的変化を検出する動的変化検出手段と、
   前記方位センサの検出軸方向の動的変化に応じて、前記静的補正係数を動的に補正して動的補正係数を演算する動的補正係数演算手段と、
   を備える方位センサの補正係数演算装置。
2. 前記静的補正係数演算手段は、前記方位センサの検出軸の静的な設置角度が、鉛直方向に対してα°である場合に、前記静的補正係数を、
   （前記静的補正係数）＝（前記基本補正係数）×ｃｏｓα
   として演算し、
   前記動的補正係数演算手段は、鉛直方向に対して前記設置角度α°傾斜した方向を基準として、前記方位センサの検出軸方向の動的変化角度がβ°である場合に、前記動的補正係数を、
   （前記動的補正係数）＝（前記静的補正係数）／ｃｏｓα×ｃｏｓ（α＋β）
   として演算する請求項１に記載の方位センサの補正係数演算装置。
3. 前記方位センサの検出軸方向の動的変化に関する条件を含む所定の動的補正実行条件を満たすか否かを判定し、前記動的補正実行条件を満たさない場合には、前記動的補正係数演算手段による動的補正係数の演算を省略する請求項１又は２に記載の方位センサの補正係数演算装置。
4. 前記方位変更の前後における前記ＧＰＳ方位変化量と前記センサ方位変化量との比率を演算する方位変化比率演算手段と、前記方位変化比率演算手段の演算結果としての方位変化比率の情報を記憶する記憶手段と、を更に備え、
   前記基本補正係数演算手段は、前記記憶手段に記憶された複数の方位変化比率の全部又は一部の平均値に基づいて前記基本補正係数を演算する請求項１から３の何れか一項に記載の方位センサの補正係数演算装置。
5. 請求項１から４の何れか一項に記載の方位センサの補正係数演算装置を備えた自位置認識装置であって、
   前記静的補正係数及び前記動的補正係数の少なくとも一方を用いて前記方位センサ情報を補正する方位センサ情報補正手段と、
   移動距離を検出する距離センサからの距離センサ情報を取得する距離センサ情報取得手段と、
   補正後の前記方位センサ情報と前記距離センサ情報とに基づいて自位置の移動軌跡を演算し、自位置の認識を行う自位置演算手段と、
   を備える自位置認識装置。
6. ＧＰＳ衛星からの信号に基づいて位置情報及び進行方位情報を含むＧＰＳ情報を取得するＧＰＳ情報取得ステップからのＧＰＳ情報に基づいて、方位変更の前後での進行方位の変化量を演算するＧＰＳ方位変化量演算ステップと、
   方位センサからの方位センサ情報に基づいて、前記方位変更の前後での進行方位の変化量を演算するセンサ方位変化量演算ステップと、
   前記方位変更の前後における前記ＧＰＳ方位変化量と前記センサ方位変化量との差異に基づいて、前記方位センサ情報の基本補正係数を演算する基本補正係数演算ステップと、
   前記方位センサの検出軸の静的な設置角度に応じて前記基本補正係数を補正した静的補正係数を演算する静的補正係数演算ステップと、
   前記方位センサの検出軸方向の動的変化を検出する動的変化検出ステップと、
   前記方位センサの検出軸方向の動的変化に応じて、前記静的補正係数を動的に補正して動的補正係数を演算する動的補正係数演算ステップと、
   をコンピュータに実行させる方位センサの補正係数演算プログラム。
7. 前記静的補正係数演算ステップにおいて、前記方位センサの検出軸の静的な設置角度が、鉛直方向に対してα°である場合に、前記静的補正係数を、
   （前記静的補正係数）＝（前記基本補正係数）×ｃｏｓα
   として演算し、
   前記動的補正係数演算ステップにおいて、鉛直方向に対して前記設置角度α°傾斜した方向を基準として、前記方位センサの検出軸方向の動的変化角度がβ°である場合に、前記動的補正係数を、
   （前記動的補正係数）＝（前記静的補正係数）／ｃｏｓα×ｃｏｓ（α＋β）
   として演算する請求項６に記載の方位センサの補正係数演算プログラム。
8. 前記方位センサの検出軸方向の動的変化に関する条件を含む所定の動的補正実行条件を満たすか否かを判定し、前記動的補正実行条件を満たさない場合には、前記動的補正係数演算ステップを省略する請求項６又は７に記載の方位センサの補正係数演算プログラム。
9. 前記方位変更の前後における前記ＧＰＳ方位変化量と前記センサ方位変化量との比率を演算する方位変化比率演算ステップと、前記方位変化比率演算ステップの演算結果としての方位変化比率の情報を所定の記憶手段に記憶する記憶ステップと、を更に実行し、
   前記基本補正係数演算ステップにおいて、前記記憶ステップに記憶された複数の方位変化比率の全部又は一部の平均値に基づいて前記基本補正係数を演算する請求項６から８の何れか一項に記載の方位センサの補正係数演算プログラム。
10. 請求項６から９の何れか一項に記載の方位センサの補正係数演算プログラムを備えた自位置認識プログラムであって、
    前記静的補正係数及び前記動的補正係数の少なくとも一方を用いて前記方位センサ情報を補正する方位センサ情報補正ステップと、
    移動距離を検出する距離センサからの距離センサ情報を取得する距離センサ情報取得ステップと、
    補正後の前記方位センサ情報と移動距離を検出する距離センサからの距離センサ情報とに基づいて自位置の移動軌跡を演算し、自位置の認識を行う自位置演算ステップと、
    をコンピュータに実行させる自位置認識プログラム。

Images