JP3473113B2 - ジャイロ方位算出装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、自動車等の車両上にて
車両の位置を測位し、現在位置情報を知らせる車載用測
位装置において、方位を測定するジャイロの出力データ
からジャイロの示す方位を算出するジャイロ方位算出装
置に関する。

【０００２】

【従来の技術】この種の装置として、特開平５−１７２
５７５号公報に開示された技術がある。この技術によれ
ば、ジャイロの出力に生じる０点の誤差を方位変化誤差
により補正するものである。

【０００３】

【発明が解決しようとする課題】しかし、通常、走行中
は直進の検出が困難であり、停止しているときのみしか
０点補正をしていなかった。このため、高速道路等で長
時間連続して走行するような場合、ジャイロの０点のズ
レが大きくなり、測位の精度が悪化することがあった。

【０００４】そこで、本発明においては、走行中におい
ても０点を補正可能とすることを課題とする。

【０００５】

【課題を解決するための手段】上記の課題を解決するた
めに請求項１に記載の発明においては、車両の方位偏差
を検出するジャイロ；ジャイロの出力からジャイロ方位
を検出するジャイロ方位算出手段；複数の衛星が発射す
る信号からそれぞれの衛星と車両との相対速度を検出
し、車両の基準方位を測定する方位測定手段；車両の速
度が所定値ＶＬより高く、かつ方位測定手段の測定する
方位が時間的に変化かない場合、ジャイロの出力信号が
ゼロとして０点補正する０点補正手段；を備え、前記０
点補正手段は車速が高くなるにつれ、０点を変更するた
めに用いる時間を長くしたことである。た。

【０００６】

【０００７】上記の課題を解決するために請求項２に記
載の発明においては、請求項１において、前記０点補正
手段は、所定の車速ＶＬ，ＶＵ（＞ＶＬ）、所定の時間
ＴＬ，ＴＵ（＞ＴＬ）を設定した場合に、車速Ｖが０＜
Ｖ＜ＶＬのとき、０点補正を行わず、ＶＬ≦Ｖ≦ＶＵの
とき、Ｔ＝ＴＬ＋（ＴＵ−ＴＬ）×（Ｖ−ＶＬ）／（Ｖ
Ｕ−ＶＬ）で示される時間以上、前記方位測定手段の測
定する方位に変化がない場合に０点補正し、ＶＵ＜Ｖの
とき、Ｔ＝ＴＵで示される時間以上、前記方位測定手段
の測定する方位に変化がない場合に０点補正するように
した。

【０００８】

【０００９】

【作用】請求項１に記載した発明によれば、ジャイロの
出力データよりジャイロ方位が算出される。一方、方位
測定手段は、複数の衛星が発射する信号からそれぞれの
衛星と車両との相対速度を検出し、車両の基準方位を測
定する。０点補正手段は車両の速度が所定値ＶＬより高
く、かつ方位測定手段の測定する方位が時間的に変化が
ない場合、ジャイロの出力信号がゼロとして０点補正す
る。よって、走行中においてもジャイロの０点補正が行
える。この場合、車速が高くになるにつれ、０点を変更
するために用いる時間を長くすることによって、車速が
高くなるにつれ方位測定手段の測定する方位の変化しな
い時間が長くならないと０点補正しないようにしてい
る。それ故に、高速走行時において緩いカーブ上を走行
しているような場合には０点補正が行われないため、精
度のよい補正ができる。

【００１０】

【００１１】請求項２に記載した発明によれば、車速が
低いときには０点補正をしない。よって、低車速時の精
度の悪い方位測定手段のデータを利用しないので、精度
の悪化がない。また、車速が高くなるにつれ方位測定手
段の測定する方位の変化しない時間が長くならないと０
点補正しないようにしている。よって、高速走行時にお
いて緩いカーブ上を走行しているような場合には０点補
正が行われないため、精度のよい補正ができる。

【００１２】この場合、方位測定手段からの基準方位と
ジャイロ方位算出手段の出力から求めた方位との方位差
が、０点を補正することにより変化する方位の方向に対
して逆の方向にあるときのみ０点を補正すれば、０点補
正により、方位測定手段とジャイロ方位算出手段とがそ
れぞれに測定した方位がかけ離れる方向への補正は行わ
れないので、より精度のよい補正ができる。

【００１３】

【実施例】以下、本発明を図面を参照して説明する。図
１は車載用測位装置の実施例を示しす。この装置は、自
動車に搭載されており、受信アンテナ１０，ＧＰＳ受信
機１１，ＧＰＳ復調器１２，表示装置１３，車速センサ
１４，圧電振動ジャイロ１５，高度センサ１６，及び電
子制御ユニット１７を備える。ＧＰＳの各衛星からは
１．５７５４２ＧＨｚの電波が送られている。ＧＰＳ受
信機１１は受信アンテナ１０を介してＧＰＳの各衛星か
らの電波を受信する。受信した信号はＧＰＳ復調器１２
に送られる。ＧＰＳ復調器１２は、電波に乗った情報、
即ち衛星の軌道を示す関数，時刻，及び情報の精度を示
すコードを復調し、電子制御ユニット１７に出力する。
車速センサ１４は自動車の車輪軸の回転に応じてパルス
信号を電子制御ユニット１７に出力するものである。車
速センサ１４は、例えば、自動車のトランスミッション
の出力軸，又はこれと同期して回転する軸の回りに設け
られた磁石と、この磁石近傍に設けられたリードスイッ
チから構成し、車輪の回転に伴い、リードスイッチがオ
ン・オフするよう構成すればよい。圧電振動ジャイロ１
５は、自動車に固定されており、自動車の垂直軸を中心
とする回転角速度ωに比例したレベルのアナログ信号を
電子制御ユニット１７に出力するものである。圧電振動
ジャイロ１５は０〜５ボルトの電圧の出力が可能であ
り、２．５ボルトを中心に、右回転の場合２．５ボルト
以上の電圧を、左回転の場合、２．５ボルト以下の信号
を出力するよう調整されている。高度センサ１６は、気
圧の変化、即ち高度変化に応じてレベルが変化するアナ
ログ信号を電子制御ユニット１７に出力するものであ
る。表示装置１３は自動車の存在する緯度，経度，高度
等を表示するものであり、電子制御ユニット１７からの
情報を受け表示を行う。

【００１４】電子制御ユニット１７は、マイクロコンピ
ュータ１９，入出力インターフェース（Ｉ／Ｏ）１８，
及びアナログ・デジタル変換器（Ａ／Ｄ変換器）２０を
備える。入出力インターフェース（Ｉ／Ｏ）１８は、Ｇ
ＰＳ受信機１１，ＧＰＳ復調器１２，及び車速センサ１
４からの信号をマイクロコンピュータ１９に送ると共
に、マイクロコンピュータ１９からの情報を表示装置１
３に送る。アナログ・デジタル変換器（Ａ／Ｄ変換器）
２０は圧電振動ジャイロ１５，及び高度センサ１６から
のアナログ信号をデジタル信号にＡ／Ｄ変換し、マイク
ロコンピュータ１９に送る。車速センサ１４からの信号
はマイクロコンピュータ１９の外部割り込み端子に接続
されている。

【００１５】マイクロコンピュータ１９は図２のフロー
チャートに沿って作動する。マイクロコンピュータ１９
に電源が入りスタートすると、まず、ステップ３０にて
初期化が行われる。ここでは、入出力ポートの設定や、
メモリの初期化、割り込みの設定が行われる。割り込み
は、一定時間毎に実行されるタイマー割り込みと、外部
割り込み端子のレベル変化に応じて実行される外部割り
込みを設定する。これ以後、一定時間毎にメインルーチ
ンの処理を一時中断し、ステップ５４のジャイロ方位算
出ルーチンを実行する。本実施例では、１０ｍＳ毎に割
り込みを行っている。また、外部割り込み端子がレベル
変化する毎にメインルーチンの処理を一時中断し、ステ
ップ５５の積算距離計算ルーチンを実行する。初期化ス
テップが終わると、ステップ３１にて、３衛星以上復調
終了したか否かを判断する。ここで、復調終了した衛星
の数が３以上であれば、ステップ３２の航法方程式１ル
ーチンにて航法方程式を設定し、ステップ３３の測位推
定誤差計算ルーチンにて測位推定誤差を計算する。次
に，ステップ３４にて、測位推定誤差が予め設定された
上限（例えば３００ｍ）より小であるか否かを判断す
る。測位推定誤差が上限より小であれば、ステップ３５
の測位ルーチンにて測位を行い、ステップ３６の高度補
正ルーチンにて高度の補正を行い、更に、ステップ３７
のジャイロ補正ルーチンにてジャイロと車速の補正を行
う。その後、ステップ３８にて現在位置情報（緯度，経
度，高度等）を表示装置１３に対して出力する。次に、
ステップ３９にてジャイロ方位が表示可能かどうか判断
する。ジャイロ方位が表示可能であれば、ステップ４０
にてジャイロ方位を表示装置１３に対して出力する。そ
の後、ステップ４１のジャイロ０点補正ルーチンにてジ
ャイロの補正を行い、ステップ３１に戻る。

【００１６】ステップ３１にて、復調可能な衛星が２以
下の場合，又は、ステップ３４にて測位推定誤差が上限
より大の場合、ステップ４２にて２衛星以上復調終了し
たか否かを判断する。ここで、復調終了した衛星の数が
２であれば、ステップ４３にて方位推定誤差が予め設定
された上限（例えば３０°）より小であるか否かを判断
する。後述するが、方位推定誤差はタイマー割り込みの
ジャイロ方位算出ルーチン内で計算される。この方位推
定誤差が上限より小である場合、ステップ４４の航法方
程式２ルーチンにて航法方程式を設定し、ステップ４５
の測位推定誤差計算ルーチンにて測位推定誤差を計算す
る。次に，ステップ４６にて、測位推定誤差が予め設定
された上限（例えば３００ｍ）より小であるか否かを判
断する。

【００１７】測位推定誤差が上限より小であれば、ステ
ップ４７の測位ルーチンにて測位を行う。その後、ステ
ップ３８に跳ぶ。

【００１８】ステップ４２にて、復調可能な衛星が１以
下の場合、又は、ステップ４３にて方位推定誤差が上限
より大の場合、又は、ステップ４６にて測位推定誤差が
上限より大の場合、ステップ４８にて方位推定誤差が予
め設定された上限（例えば３０°）より小であるか否か
を判断する。方位推定誤差が上限より小である場合、ス
テップ４９にて積算距離推定誤差ＳＴＥＰerror を計算
する。積算距離推定誤差ＳＴＥＰerror は後述する積算
距離算出ルーチン内で取り込んだ一定時間内のパルス数
と、後述の数値補正ルーチンで求めた回転誤差ROLLerro
r をかけて求める。ステップ５０ではこの積算距離推定
誤差ＳＴＥＰerror が予め設定された上限（例えば１０
０ｍ）より小であるか否かを判断する。積算距離推定誤
差ＳＴＥＰerror がが上限より小である場合、ステップ
５１の航法方程式３ルーチンにて航法方程式を設定し、
ステップ５２の測位推定誤差計算ルーチンにて測位推定
誤差を計算する。次に，ステップ５３にて、測位推定誤
差が予め設定された上限（例えば３００ｍ）より小であ
るか否かを判断する。測位推定誤差が上限より小であれ
ば、ステップ４７の測位ルーチンに跳ぶ。

【００１９】ステップ４８にて方位推定誤差が上限より
大の場合、又は、ステップ５０にて積算距離推定誤差が
上限より大の場合、又は、ステップ５３にて測位推定誤
差が上限より大の場合、ステップ４１に跳ぶ。

【００２０】以下、各サブルーチンの詳細を説明する。

【００２１】図３はジャイロ方位算出ルーチンのフロー
チャートである。ここでは、まず、ステップ５６にて方
位偏差Δθを次式に従って求める。

【００２２】

【数１】Δθ＝Δθ＋ＡＤＡ（ＡＤＧ−ＡＤＣ） ここで、ＡＤＧは圧電振動ジャイロ１５のアナログ出力
をデジタル変換した値であり、ＡＤＡ，ＡＤＣは変換係
数である。係数ＡＤＣは回転角速度ωがゼロの時のアナ
ログ電圧値（２．５ボルト）をデジタル変換した値がセ
ットされている。

【００２３】係数ＡＤＡは、圧電振動ジャイロ１５のア
ナログ出力をデジタル変換した値を更に角度に変換する
ように設定された値である。数１式の計算はタイマー割
り込みの周期毎に行われるので、Δθは単位時間あたり
の方位偏差量を示す。次に、ステップ５７にて車速がゼ
ロか否かを判断し、車速がゼロであれば、ステップ５８
にてジャイロ方位誤差turnを数２式に基づき更新し、ま
たステップ５９にてジャイロのオフセット値offsetを数
３式に基づき更新する。ステップ５７にて車速がゼロで
なければステップ５８，５９をとばして次へ進む。

【００２４】

【数２】turn＝turn＋ＡＤＡerror （ＡＤＧ−ＡＤＣ）

【００２５】

【数３】offset＝offset＋ＡＤＣerror ここで、ＡＤＡerror は圧電振動ジャイロの出力の傾き
のずれを表す変換係数であり、ＡＤＣerror は圧電振動
ジャイロのゼロ点からのずれを示す変換係数である。

【００２６】次に、ステップ６０にて基準方位が有効で
あるか否かを判断する。基準方位が有効であれば、ステ
ップ６２へ進む。基準方位が有効でなければ、表示用誤
差上限又はそれ以上の値を方位推定誤差ψerror にセッ
トする。

【００２７】次に、ステップ６４にて圧電振動ジャイロ
１５の割り込み用出力値和ＡＤＳ１にデジタル出力値Ａ
ＤＧが加算されて、割り込み用出力値和ＡＤＳ１が更新
される。次に、ステップ６５にて測位解出力タイミング
か否かを判断する。このタイミングは本実施例では１秒
毎としている。測位解出力タイミングであれば、ステッ
プ２９２及びステップ６２以降を実行し、測位解出力タ
イミングでなければジャイロ方位算出ルーチンを終了す
る。ステップ２９２ではバンク補正を行う。これは路面
のバンク量に応じて制御を調整するものである。ステッ
プ６２では割り込み用方位ψ１に方位偏差Δθが加算さ
れて、割り込み用方位ψ１が更新される。次に、ステッ
プ６３にて割り込み用方位ψ１ｒａｗに方位偏差Δθが
加算されて、割り込み用方位ψ１ｒａｗが更新される。
次に、ステップ６６では方位推定誤差計算ルーチンを実
行し、本位誤差を求める。次にステップ６７にて、割り
込み用方位ψ１に補正量を加える。次にステップ６８に
て、方位ψに割り込み用方位ψ１を代入し、方位ψｒａ
ｗに割り込み用方位ψ１ｒａｗを代入し、出力値和ＡＤ
Ｓに割り込み用出力値和ＡＤＳ１を代入する。次に、ス
テップ６９にて高度センサ１６の出力の遅延データを更
新する。高度センサ１６の出力の遅延データには、３秒
前の高度Ｈ〔−３〕，２秒前の高度Ｈ〔−２〕，１秒前
の高度Ｈ〔−１〕，現時点の高度Ｈ

〔０〕の４つの出力
データを備えている。ここでは、２秒前の高度Ｈ〔−
２〕を３秒前の高度Ｈ〔−３〕に、１秒前の高度Ｈ〔−
１〕を２秒前の高度Ｈ〔−２〕に、現時点の高度Ｈ

〔０〕を１秒前の高度Ｈ〔−１〕に、高度センサ１６の
出力を現時点の高度Ｈ

〔０〕に、それぞれ更新する。次
に、ステップ７０にて圧電振動ジャイロ１５の出力の遅
延データを更新する。圧電振動ジャイロ１５の方位の遅
延データには、３秒前の方位ψ〔−３〕，２秒前の方位
ψ〔−２〕，１秒前の方位ψ〔−１〕，現時点の方位ψ

〔０〕の４つの出力データを備えている。ここでは、２
秒前の方位ψ〔−２〕を３秒前の方位ψ〔−３〕に、１
秒前の方位ψ〔−１〕を２秒前の方位ψ〔−２〕に、現
時点の方位ψ

〔０〕を１秒前の方位ψ〔−１〕に、ステ
ップ６３で得た方位ψを現時点の方位ψ

〔０〕に、それ
ぞれ更新する。次に、ステップ７１にて車速センサ１４
の一定時間中のパルス数の遅延データを更新する。パル
ス数の遅延データには、３秒前のパルス数Ｐ〔−３〕，
２秒前のパルス数Ｐ〔−２〕，１秒前のパルス数Ｐ〔−
１〕，現時点のパルス数Ｐ

〔０〕の４つの出力データを
備えている。ここでは、２秒前のパルス数Ｐ〔−２〕を
３秒前のパルス数Ｐ〔−３〕に、１秒前のパルス数Ｐ
〔−１〕を２秒前のパルス数Ｐ〔−２〕に、現時点のパ
ルス数Ｐ

〔０〕を１秒前のパルス数Ｐ〔−１〕に、高度
センサ１６の出力を現時点のパルス数Ｐ

〔０〕に、それ
ぞれ更新する。ステップ６６が終了するとジャイロ方位
算出ルーチンを終了する。

【００２８】図４は方位推定誤差計算ルーチンのフロー
チャートである。ここでは、まず、ステップ７２にて、
フラグsignが１か否かを判定し、フラグsignが１であれ
ばステップ７３を、フラグsignが１でなければステップ
７４を実行する。このフラグsignは車両が右回転してい
るか左回転しているかを示すものであり、初期値は１に
設定してある。ステップ７３では方位偏差ＲＬにジャイ
ロ方位誤差turnを加算する。ステップ７４では方位偏差
ＲＬにジャイロ方位誤差turnを減算する。次に、ステッ
プ７５にて方位偏差ＲＬがゼロより上か否かを判断し、
ゼロ以下であれば、ステップ７６にて、方位偏差ＲＬの
符号を反転し、ステップ７７にてフラグsignの状態を変
更（１であれば０に、０であれば１に変更）する。次に
ステップ７８にて方位絶対値の補正があるか否かを判断
し、方位絶対値の補正があれば、ステップ７９にてオフ
セット値offsetにＧＰＳ推定誤差ＧＰＳerror を代入
し、ステップ８０にて回転方向信号ＲＬをゼロとする。
その後、ステップ８１にて方位偏差推定誤差Δθerror
を数４式に従って求め、方位推定誤差計算ルーチンを終
了する。この処理により、オフセットの誤差量を加味し
た方位推定誤差Δθerror が求められる。

【００２９】

【数４】Δθerror ＝ＲＬ＋offset 図５は積算距離算出ルーチンのフローチャートである。
ここでは、まず、ステップ８２にて一定時間中のパルス
数を取り込む。次に、ステップ８３にて一定時間中のパ
ルス数に変数ROLLをかけて、積算距離ＳＴＥＰを求め
る。変数ROLLはタイヤの有効半径や車速センサ１４の出
力特性を加味して求めた変数で、車速センサ１４のパル
ス数を距離に変換するためのものである。ステップ８３
を終了すると、積算距離算出ルーチンを終了する。

【００３０】図６は航法方程式１ルーチンのフローチャ
ートである。ここでは、まず、ステップ８４にて４衛星
以上復調終了したか否かを判断する。４衛星以上復調終
了していれば、ステップ８６にて表１中の方程式１をセ
ットし、ステップ８９にてσ_m ² 計算ルーチンを実行し
てσ_m ² （σ_m ² は各衛星の予測誤差を示す。_m は復調
終了した衛星の数を示し、１〜４番目の衛星に対してそ
れぞれσ₁ ² ，σ₂ ²，σ₃ ² ，σ₄ ² が計算される）
を求めた後、ステップ９３にて表２中の行列式１に従っ
て測定値の予測誤差を示す行列Σε_i+1 （_i は前回まで
の測位の回数を示す。_i+1 は今回の測位となる。）を定
めて、このルーチンを終了する。ここで、航法方程式は
ＡＸ＝Ｌの形で表されている。ＡはＬ_m ，Ｍ_m ，Ｎ_m （
_m は復調終了した衛星の数で_m ＝１〜４）を備える４×
４の行列，Ｘは〔Δｘ Δｙ Δｚ ｃΔｔ〕^T ，Ｌは
１×４の行列で表されている。Ａは測位のための計画行
列である。

【００３１】

【表１】

【００３２】図２８に示すように、（Ｌ_m ，Ｍ_m ，
Ｎ_m ）はｍ番目の衛星から前回位置（前回測位した時の
測位点）へ向かうベクトルを示す。このベクトルは衛星
からの情報から得られる。（Δｘ，Δｙ，Δｚ）は前回
位置から測位点に向かうベクトルを示す。Δｔは前回位
置における時刻と測位点に達したときの時刻の時間差を
示す。ｃは光速を示す。ここでは、４衛星以上復調終了
しているので、_m ＝１〜４の４つの衛星から前回位置へ
向かうベクトルの要素を用いて行列Ａが表されている。
また、行列Ｌは〔ΔL1 ΔL2 ΔL3 ΔL4〕^T で表され
る。このΔL1，ΔL2，ΔL3，ΔL4はそれぞれ１〜４番目
の衛星と測位点との間の距離Ｌ_m （_m ＝１〜４）から１
〜４番目の衛星と前回位置との間の距離Ｌ_0mを引いた距
離を示す。この航法方程式１を解くことによって、４つ
の衛星から得た情報から３次元の相対位置（Δｘ，Δ
ｙ，Δｚ）を求めることができる。

【００３３】図６において、ステップ８４にて復調終了
した衛星の数が３の場合、ステップ８５が実行される。
ステップ８５では高度データがある否かを判断する。高
度データがあれば、ステップ８７にて表１中の方程式２
をセットし、ステップ９０にてσ_m ² 計算ルーチンを実
行して３つの衛星に対するσ₁ ² ，σ₂ ² ，σ₃ ² を求
め、ステップ９２にてσ_a ² 計算ルーチンを実行してσ
_a ² （σ_a ² は高度に対する予測誤差を示す。）を求め
た後、ステップ９４にて表２中の行列式２に従って測定
値の予測誤差を示す行列Σε_i+1 を定めて、このルーチ
ンを終了する。

【００３４】

【表２】

【００３５】ここで、航法方程式２は、航法方程式１と
同様に、ＡＸ＝Ｌの形で表されている。ここでは、３衛
星が復調終了しているので、_m ＝１〜３の３つの衛星か
ら前回位置へ向かうベクトルの要素を用いて行列Ａが表
されている。また、行列Ｌは〔ΔL1 ΔL2 ΔL3 Δ
ｈ〕^T で表される。このΔｈは高度センサ１６により求
めた測位点の高度と前回位置の高度との差を示す。この
航法方程式２を解くことによって、３つの衛星から得た
情報と高度センサから得た高度差Δｈから３次元の相対
位置（Δｘ，Δｙ，Δｚ）を求めることができる。

【００３６】ステップ８５において、高度データがなけ
れば、ステップ８８にて表１中の方程式３をセットし、
ステップ９１にてσ_m ² 計算ルーチンを実行して３つの
衛星に対するσ₁ ² ，σ₂ ² ，σ₃ ² を求めた後、ステ
ップ９５にて表２中の行列式３に従って測定値の予測誤
差を示す行列Σε_i+1 を定めて、このルーチンを終了す
る。ここで、航法方程式３は、航法方程式１と同様に、
ＡＸ＝Ｌの形で表されている。ここでは、３衛星が復調
終了しているので、_m ＝１〜３の３つの衛星から前回位
置へ向かうベクトルの要素を用いて行列Ａが表されてい
る。また、行列Ｌは〔ΔL1 ΔL2 ΔL3 ０〕^T で表さ
れる。高さ方向の差Δｚはゼロと仮定している。この航
法方程式３を解くことによって、３つの衛星から得た情
報から２次元相対位置（Δｘ，Δｙ）を求めることがで
きる。

【００３７】図７は航法方程式２ルーチンのフローチャ
ートである。ここでは、まず、ステップ９６にて高度デ
ータがある否かを判断する。高度データがあれば、ステ
ップ９７にて表１中の方程式４をセットし、ステップ９
９にてσ_m ² 計算ルーチンを実行して２つの衛星に対す
るσ₁ ² ，σ₂ ² を求め、ステップ１０１にてσ_a ²計
算ルーチンを実行してσ_a ² を求め、ステップ１０２に
てσ_G ² 計算ルーチンを実行してσ_G ² （σ_G ² は圧電
振動ジャイロの出力の予測誤差を示す。）を求めた後、
ステップ１０４にて表２中の行列式４に従って測定値の
予測誤差を示す行列Σε_i+1 を定めて、このルーチンを
終了する。ここでは、２衛星が復調終了しているので、
_m ＝１〜２の２つの衛星から前回位置へ向かうベクトル
の要素と、Ｇｘ及びＧｙを用いて行列Ａが表されてい
る。Δｘ及びΔｙとＧｘ及びＧｙとの間には次の関係が
ある。

【００３８】

【数５】 Δｙ／Δｘ＝ｓｉｎ（１８０°−ψ）／ｃｏｓ（１８０°−ψ） ＝Ｇｘ／Ｇｙ また、行列Ｌは〔ΔL1 ΔL2 Δｈ ０〕^T で表され
る。数３式を用いて航法方程式４を解くことによって、
２つの衛星から得た情報，圧電振動ジャイロから得た方
位ψ及び高度センサから得た高度差Δｈから３次元相対
位置（Δｘ，Δｙ，Δｚ）を求めることができる。

【００３９】ステップ９６にて高度データがなければ、
ステップ９８にて表１中の方程式５をセットし、ステッ
プ１００にてσ_m ² 計算ルーチンを実行して２つの衛星
に対するσ₁ ² ，σ₂ ² を求め、ステップ１０３にてσ
_G ² 計算ルーチンを実行してσ_G ² を求めた後、ステッ
プ１０５にて表２中の行列式５に従って測定値の予測誤
差を示す行列Σε_i+1 を定めて、このルーチンを終了す
る。ここでは、２衛星が復調終了しているので、_m ＝１
〜２の２つの衛星から前回位置へ向かうベクトルの要素
と、Ｇｘ及びＧｙを用いて行列Ａが表されている。ま
た、行列Ｌは〔ΔL1 ΔL2 ０ ０〕^T で表される。高
さ方向の差Δｚはゼロと仮定している。数３式を用いて
航法方程式５を解くことによって、２つの衛星から得た
情報及び圧電振動ジャイロから得た方位ψから２次元相
対位置（Δｘ，Δｙ）を求めることができる。

【００４０】図８は航法方程式３ルーチンのフローチャ
ートである。ここでは、まず、ステップ１０６にて１衛
星以上復調終了したか否かを判断する。１衛星以上復調
終了していれば、ステップ１０７が実行される。ステッ
プ１０７では高度データがある否かを判断する。高度デ
ータがあれば、ステップ１０９にて表１中の方程式６を
セットし、ステップ１１３にてσ_m ² 計算ルーチンを実
行して１つの衛星に対するσ₁ ² を求め、ステップ１１
５にてσδ_x ² ，σδ_y ² 計算ルーチンを実行してσδ
_x ² ，σδ_y ² （σδ_x ² ，σδ_y ² は車速センサと圧
電振動ジャイロの出力から求まる２次元位置のＸ座標及
びＹ座標の予測誤差を示す。）を求め、ステップ１１９
にてσ_a ² 計算ルーチンを実行してσ_a ² を求めた後、
ステップ１２１にて表２中の行列式６に従って測定値の
予測誤差を示す行列Σε_i+1 を定めて、このルーチンを
終了する。ここでは、１衛星が復調終了しているので、
_m＝１の１つの衛星から前回位置へ向かうベクトルの要
素を用いて行列Ａが表されている。また、行列Ｌは〔Δ
L1 δ_x δ_y Δｈ〕^T で表される。この内、δ_x 及
びδ_y は次式により、後述の測位推定誤差計算ルーチン
の中で求められる。

【００４１】

【数６】δ_x ＝ＳＴＥＰ・ｃｏｓ（１８０−ψ） δ_y ＝ＳＴＥＰ・ｓｉｎ（１８０−ψ） 数６式を用いて航法方程式６を解くことによって、１つ
の衛星から得た情報，圧電振動ジャイロから得た方位
ψ，車速センサから得た積算距離ＳＴＥＰ，及び高度セ
ンサから得た高度差Δｈから３次元相対位置（Δｘ，Δ
ｙ，Δｚ）を求めることができる。

【００４２】ステップ１０７にて高度データがない場
合、ステップ１１０にて表１中の方程式７をセットし、
ステップ１１４にてσ_m ² 計算ルーチンを実行して１つ
の衛星に対するσ₁ ² を求め、ステップ１１６にてσδ
_x ² ，σδ_y ² 計算ルーチンを実行してσδ_x ² ，σδ
_y ² を求めた後、ステップ１２２にて表２中の行列式７
に従って測定値の予測誤差を示す行列Σε_i+1 を定め
て、このルーチンを終了する。ここでは、１衛星が復調
終了しているので、_m ＝１の１つの衛星から前回位置へ
向かうベクトルの要素を用いて行列Ａが表されている。
また、行列Ｌは〔ΔL1 δ_x δ_y ０〕^T で表され
る。高さ方向の差Δｚはゼロと仮定している。

【００４３】数６式を用いて航法方程式７を解くことに
よって、１つの衛星から得た情報，圧電振動ジャイロか
ら得た方位ψ及び車速センサから得た積算距離ＳＴＥＰ
から２次元相対位置（Δｘ，Δｙ）を求めることができ
る。

【００４４】ステップ１０６にて復調終了した衛星の数
がない場合、ステップ１０８が実行される。ステップ１
０８では高度データがある否かを判断する。高度データ
があれば、ステップ１１１にて表１中の方程式８をセッ
トし、ステップ１１７にてσδ_x ² ，σδ_y ² 計算ルー
チンを実行してσδ_x ² ，σδ_y ² を求め、ステップ１
２０にてσ_a ² 計算ルーチンを実行してσ_a ² を求めた
後、ステップ１２３にて表２中の行列式８に従って測定
値の予測誤差を示す行列Σε_i+1 を定めて、このルーチ
ンを終了する。ここでは、復調終了している衛星はない
ので、行列Ａは単位行列で表されている。また、行列Ｌ
は〔δ_x δ_y Δｈ ０〕^T で表される。数６式を用
いて航法方程式８を解くことによって、圧電振動ジャイ
ロから得た方位ψ，車速センサから得た積算距離ＳＴＥ
Ｐ，及び高度センサから得た高度差Δｈから３次元相対
位置（Δｘ，Δｙ，Δｚ）を求めることができる。

【００４５】ステップ１０８にて高度データがない場
合、ステップ１１２にて表１中の方程式９をセットし、
ステップ１１８にてσδ_x ² ，σδ_y ² 計算ルーチンを
実行してσδ_x ² ，σδ_y ² を求めた後、ステップ１２
４にて表２中の行列式９に従って測定値の予測誤差を示
す行列Σε_i+1 を定めて、このルーチンを終了する。こ
こでは、復調終了している衛星はないので、行列Ａは単
位行列で表されている。

【００４６】また、行列Ｌは〔δ_x δ_y ０ ０〕^T
で表される。高さ方向の差Δｚはゼロと仮定している。
数６式を用いて航法方程式９を解くことによって、圧電
振動ジャイロから得た方位ψ及び車速センサから得た積
算距離ＳＴＥＰから２次元相対位置（Δｘ，Δｙ）を求
めることができる。

【００４７】図９はσ_m ² 計算ルーチンのフローチャー
トである。ここでは、まず、ステップ１２５にてＵＥＲ
Ｅ計算ルーチンを実行し、ｉ＝１〜４番目の衛星の内、
復調終了したそれぞれの衛星のＵＥＲＥ（ＵＥＲＥは衛
星の配置によらない１σ誤差量であり、車速Ｖに応じて
補正したものを示す。）を求める。次に、ステップ１２
６にて、それぞれの衛星について次式に基づきσ_m ² を
求める。

【００４８】

【数７】σ_m ² ＝ＵＥＲＥ² この計算の後、このルーチンを終了する。

【００４９】図１０はσ_a ² 計算ルーチンのフローチャ
ートである。ここでは、まず、ステップ１２７にて誤差
用のＵＥＲＥ及びＶＤＯＰ（ＶＤＯＰは高度に対する誤
差指標を示す。）があるか否かを判断する。誤差用のＵ
ＥＲＥ及びＶＤＯＰがあれば、ステップ１２８にて（Ｖ
ＤＯＰ×ＵＥＲＥ）² をσ_a ² に代入して、このルーチ
ンを終了する。誤差用のＵＥＲＥ及びＶＤＯＰがなけれ
ば、ステップ１２９にて所定の定数ｃｏｎｓｔ² をσ_a
² に代入して、このルーチンを終了する。

【００５０】図１１はσ_G ² 計算ルーチンのフローチャ
ートである。ここでは、まず、ステップ１３０にて積算
距離ＳＴＥＰをメモリーから読みだす。次に、ステップ
１３１にて方位推定誤差計算ルーチンにて計算した方位
偏差推定誤差Δθerror を取り出し、ステップ１３２に
て次式に基づきσ_G ² を計算する。

【００５１】

【数８】 σ_G ² ＝（ｎ×ＳＴＥＰ×ｓｉｎΔθerror ）² ここでｎは定数である。上記計算をした後、このルーチ
ンを終了する。

【００５２】図１２はσδ_x ² ，σδ_y ² 計算ルーチン
のフローチャートである。ここでは、まず、ステップ１
３３にてＥψ_x ，Ｅψ_y 計算ルーチンを実行して、Ｅψ
_x 及びＥψ_y （Ｅψ_x 及びＥψ_y は圧電振動ジャイロに
よるＸ座標及びＹ座標の誤差を示す。）を求める。次
に、ステップ１３４にて積算距離推定誤差ＳＴＥＰerro
r を求める。積算距離推定誤差ＳＴＥＰerror は前述の
積算距離算出ルーチン内で取り込んだ一定時間内のパル
ス数と、後述の数値補正ルーチンで求めた回転誤差ROLL
error をかけて求める。次に、ステップ１３５及びステ
ップ１３６にて次式に基づきσδ_x ² 及びσδ_y ² を計
算する。

【００５３】

【数９】σδ_x ² ＝（Ｅψ_x ＋ＳＴＥＰerror ・ｃｏｓ
（１８０°−ψ））² σδ_y ² ＝（Ｅψ_y ＋ＳＴＥＰerror ・ｓｉｎ（１８０
°−ψ））² ここで、ψはジャイロ方位算出ルーチンで得た方位であ
る。上記計算をした後、このルーチンを終了する。

【００５４】図１３はＥψ_x ，Ｅψ_y 計算ルーチンのフ
ローチャートである。ここでは、まず、ステップ１３７
にてジャイロ方位算出ルーチンで得た方位ψを取り出
す。次に、ステップ１３８にて方位推定誤差計算ルーチ
ンで得た方位推定誤差ψerrorを取り出す。次に、ステ
ップ１３９にて積算距離算出ルーチンで得た積算距離Ｓ
ＴＥＰを取り出す。次に、ステップ１４０にて数１０式
によりψ_T を求める。次に、ステップ１４１及び１４２
にて数１１式に基づきＥψ_x 及びＥψ_y を計算する。

【００５５】

【数１０】ψ_T ＝ψ−ψerror

【００５６】

【数１１】Ｅψ_x ＝ＳＴＥＰ〔ｃｏｓ（１８０°−ψ）
−ｃｏｓ（１８０°−ψ_T ）〕 Ｅψ_y ＝ＳＴＥＰ〔ｓｉｎ（１８０°−ψ）−ｓｉｎ
（１８０°−ψ_T ）〕 上記計算をした後、このルーチンを終了する。

【００５７】図１４は測位推定誤差計算ルーチンのフロ
ーチャートである。ここでは、まず、ステップ１４３に
て基準方位があるか否かを判断し、基準本位が有ればス
テップ１４５へ、無ければステップ１４４に進む。ステ
ップ１４４では１回目の測位か否かを判断する。１回目
の測位であればステップ１４５を実行し、２回目以降の
測位であればステップ１４６以下を実行する。ステップ
１４５では次式に基づきΣＸ₁ を計算する。

【００５８】

【数１２】ΣＸ₁ ＝（Ａ₁ ^T Σε₁ ^-1 Ａ₁ ）^-1 ここで、Ａ₁ は１回目の測位の時点における航法方程式
の左辺の行列Ａである。

【００５９】Σε₁ は１回目の測位の時点における表２
の行列式より求めた測定値の予測誤差を示す行列Σε
_i+1 （ｉ＝０） である。ΣＸ_i+1 は次に示す数１３式
により定められた誤差を表す行列であり、ΣＸ₁ は１回
目の測位の時点のものである。ステップ１５６では、数
１２式及び数１３式から求まったσ_x ² 及びσ_y ² を用
いて、数１４式に従って測位推定誤差を計算して、この
ルーチンを終了する。

【００６０】

【数１３】

【００６１】

【数１４】測位推定誤差＝（σ_x ² ＋σ_y ² ）^1/2 ２回目以降の測位においては、まず、ステップ１４６に
て積算距離ＳＴＥＰを読みだし、ステップ１４７にて方
位ψを読みだす。次に、ステップ１４８にて、数６式に
従ってδ_x 及びδ_y を求める。次に、ステップ１４９に
て数１５式に従って行列Ｙ_i+1 を定義する。

【００６２】

【数１５】Ｙ_i+1 ＝〔δ_x δ_y ０ ０〕^T 次に、ステップ１５０にて、前述のσδ_x ² ，σδ_y ²
計算ルーチンを実行し、σδ_x ² 及びσδ_y ² を得る。
次に、ステップ１５１にてσ_a1 ² を定数const²に設定す
る。次に、ステップ１５２にて、数１６式に示すように
予想誤差モデルを示す行列ΣＵｉを定義する。

【００６３】

【数１６】

【００６４】次に、ステップ１５３にて、１回前（ｉ回
目）の情報から求めた推定誤差行列ΣＸ_i ，行列ΣＵ_i
から、推定誤差の予測値を示す行列ΣＹ_i+1 を数１７式
に従って計算する。

【００６５】

【数１７】ΣＹ_i+1 ＝ΣＸ_i ＋ΣＵ_i 次に、ステップ１５４にて、カルマンゲインを示す行列
Ｋｇを数１８式に従って計算する。ここで、Ａ_i+1 は表
１に基づき航法方程式１〜３ルーチンにてそれぞれ設定
した方程式１〜９の左辺の行列であり、Σε_i+1 は表２
に基づきそれぞれ定義した行列である。

【００６６】

【数１８】Ｋｇ＝ΣＹ_i+1 Ａ_i+1 ^T 〔Σε_i+1 ＋Ａ_i+1
ΣＹ_i+1 Ａ_i+1 ^T 〕^-1 次に、ステップ１５５にて、数１９式に基づき今回の推
定誤差行列ΣＸ_i+1 を求める。ここで、Ｉは４×４の単
位行列である。

【００６７】

【数１９】 ΣＸ_i+1 ＝２・〔Ｉ−Ｋｇ・Ａ_i+1 〕ΣＹ_i+1 この後、ステップ１５６にて、数１９式を計算し数１３
式に当てはめて求まったσ_x ² 及びσ_y ² を用いて、数
１４式に従って測位推定誤差を計算して、このルーチン
を終了する。

【００６８】このルーチンでの推定誤差行列ΣＸ_i+1 の
計算方法はカルマンフィルターと呼ばれる最適フィルタ
ーを適用したものであり、測位解を求めるカルマンフィ
ルターによる誤差の分散・共分散行列（ΣＸ_i+1 ）の１
１成分（σ_x ² ）と２２成分（σ_y ² ）の和を平方根と
した。必要であれば、３３成分（σ_z ² ）も加えて平方
根としてもよい。

【００６９】図１５は測位ルーチンのフローチャートで
ある。ここでは、まず、ステップ１５７にて２回目以降
の測位か否かを判断する。１回目の測位であればステッ
プ１６１を実行し、２回目以降の測位であればステップ
１５８〜１６０を実行する。

【００７０】ステップ１６１では次式に基づきＸ₁ を計
算する。

【００７１】

【数２０】Ｘ₁ ＝Ａ₁ ^-1Ｌ₁ ここで、Ａ₁ は１回目の測位の時点における航法方程式
の左辺の行列Ａである。

【００７２】Ｌ₁ は１回目の測位の時点における航法方
程式の右辺の行列Ｌである。Ｘ₁ は１回目の測位の時点
における求める解を示すものである。ステップ１６２で
は、求めた解（ＸＹＺ座標）を緯度，経度及び高度に変
換してこのルーチンを終了する。

【００７３】ステップ１５７にて、２回目以降の測位の
場合にはステップ１５８にて、測位推定誤差計算ルーチ
ンにて求めた解の予測値を示す行列Ｙ_i+1 を取り出す。
次に、ステップ１５９にて、改善量を示す行列Ｄを数２
１式に基づいて計算する。

【００７４】

【数２１】Ｄ＝Ｌ_i+1 −Ａ_i+1 Ｙ_i+1 ここで、Ａ_i+1 は今回の測位における航法方程式の左辺
の行列Ａである。Ｌ_ｉ＋１は今回の測位における航法方
程式の右辺の行列Ｌである。次に、ステップ１６０にて
今回の測位の測位解を示す行列Ｘ_ｉ＋１ を数２２式に
基づいて計算する。

【００７５】

【数２２】Ｘ_i+1 ＝Ｙ_i+1 ＋Ｋｇ・Ｄ ここで、Ｋｇは前述の測位推定誤差計算ルーチンにて数
１８式に従って計算したカルマンゲインを示す行列であ
る。この後、ステップ１６２にて、求めた解（ＸＹＺ座
標）を緯度，経度及び高度に変換してこのルーチンを終
了する。

【００７６】このルーチンの計算方法はカルマンフィル
ターと呼ばれる最適フィルターを適用したものであり、
異常な解の変化を抑制するものである。圧電振動ジャイ
ロと車速センサーの出力を用いた解が方程式の解の予想
を与えるようになっているので、衛星の異常等で測位解
が大きく変動する場合の歯止めとなる。特に、車速＝０
の場合は解の予想として移動なしとなり、予想誤差も０
となるため停車中の解のふらつきを抑制する効果があ
る。

【００７７】図１６は高度補正ルーチンのフローチャー
トである。ここでは、まず、ステップ１６３にてＶＤＯ
ＰがＶＤＯＰ上限より低いか否かを判断する。ＶＤＯＰ
がＶＤＯＰ上限より低ければステップ１６４へ跳び、Ｖ
ＤＯＰがＶＤＯＰ上限より高ければ何もせずこのルーチ
ンを終了する。ステップ１６４では、測位により得られ
た高度と高度センサにより得られた高度の差をとり補正
量を得る。ここで、この高度補正ルーチンは３衛星以上
復調が終了し、４衛星により測位を行っているので、測
位により得られた高度はＧＰＳのみにより得られたもの
となる。ＧＰＳによる測位は通常、数秒遅れて得られる
ので、高度センサにより得られた高度との間には時間差
が生じている。そこで、ＧＰＳの遅れに応じて、ジャイ
ロ方位算出ルーチン中のステップ６９にて更新している
過去の高度データを用いる。例えば、ＧＰＳの遅れが３
秒の場合、測位により得られた高度から高度Ｈ〔−３〕
を引き補正量とする。次に、ステップ１６５にて過去の
高度データに補正量を加えて補正する。次に、ステップ
１６６にてＵＥＲＥ計算ルーチンを実行し、ＵＥＲＥを
得る。次に、ステップ１６７にてＶＤＯＰとＵＥＲＥを
記憶してこのルーチンを終了する。尚、ＶＤＯＰはσ_z
² の平方根をとって求める。

【００７８】図１７はＵＥＲＥ計算ルーチンのフローチ
ャートである。ここでは、まず、各々の衛星の情報から
衛星の配置によらない指標であるＳＶＡＣＣ（ＳＶaccu
racy：個々の衛星の内部状態から、その衛星１ 個が測
位に及ぼす影響を計算した指標）を得て、ステップ１６
８にて、ＳＶＡＣＣが０以上７未満であるか否かを判断
する。ＳＶＡＣＣが０以上７未満であれば、ステップ１
７０にてユーザーレンジアキュラシーＵＲＡ（User Ran
ge Accuracy)を数２３式により求める。ＳＶＡＣＣが７
以上であれば、ステップ１６９にてユーザーレンジアキ
ュラシーＵＲＡを数２４式により求める。

【００７９】

【数２３】 ＵＲＡ＝２^(1+SVACC/2) （０≦ＡＶＡＣＣ＜７）

【００８０】

【数２４】 ＵＲＡ＝２^(SVACC-2) （７≦ＡＶＡＣＣ） 次に、ステップ１７１にて数２５式に基づきＵＥＲＥを
求める。

【００８１】

【数２５】 ＵＥＲＥ＝（ＵＥＡ² ＋Σ（意図的劣化係数以外の値）² ）^1/2 ＝（ＵＥＡ² ＋１１９．７）^1/2 この内、意図的劣化係数は、衛星の時計の誤差や衛星の
軌道の予測誤差によるものである。また、意図的劣化係
数以外の値は、衛星軌道の摂動予測誤差（１σの値＝
１．０ｍ）、電離層伝搬遅延補正誤差（１σの値＝７．
５ｍ）、対流圏伝搬遅延補正誤差（１σの値＝２．０
ｍ）、受信機雑音（１σの値＝７．５ｍ）、マルチパス
（１σの値＝１．２ｍ）、衛星に基づくその他の誤差
（１σの値＝０．５ｍ）、制御に基づくその他の誤差
（１σの値＝０．５ｍ）、利用者に基づくその他の誤差
（１σの値＝０．５ｍ）である。これらの誤差の大きさ
が変わった場合には、数２５式の数値を変更すればよ
い。

【００８２】次に、ステップ２９７にて車速Ｖが０から
所定値Ｖｃｏｎｓｔの範囲内であるか否かを判断する。
ここで、Ｖｃｏｎｓｔは最大車速を当てはめている。ス
テップ２９７において、車速Ｖが範囲内であれば、ステ
ップ２９８にて数２６式に応じて変数ａを求めるが、車
速Ｖが範囲外であれば、ステップ２９９にて変数ａを１
とする。

【００８３】

【数２６】ａ＝（１−ｎ）Ｖ／Ｖｃｏｎｓｔ＋ｎ 次にステップ３００にてＵＥＲＥに変数ａを掛けてＵＥ
ＲＥを更新する。この後、このルーチンを終了する。ス
テップ２９７〜３００の処理によって、ＵＥＲＥは基本
的には次式のように設定される。

【００８４】

【数２７】ＵＥＲＥ＝（（１−ｎ）Ｖ／Ｖｃｏｎｓｔ＋
ｎ）×ＵＥＲＥ 図１８はジャイロ補正ルーチンのフローチャートであ
る。補正モードは、補正の状況を示すフラグである。こ
こでは、まず、補正モードがいずれかを判断し、補正モ
ードに応じてステップ１７３，１７４，１７５，１７６
のモード−１，モード０，モード１，モード２のいずれ
かのルーチンを実行し、このルーチンを終了する。

【００８５】ここで、補正は走行中に順次通過する第１
の地点、第２の地点、第３の地点の間のジャイロやＧＰ
Ｓの出力データを用いて行う。モード−１では補正の開
始を判断し、モード０では第１の地点のデータを収集
し、モード１では第２の地点のデータを収集し、モード
２では第３の地点のデータを収集し、これらのデータを
基に補正を行うものである。

【００８６】図１９はモード−１ルーチンのフローチャ
ートである。補正モードがモード−１のとき、このルー
チンが実行される。補正モードは他のルーチンにより、
初期状態及び補正終了後にモード−１に設定されてい
る。よって、初期状態ではモード−１ルーチンが選択さ
れる。まず、ステップ１７７にて、車速が所定の下限値
より大か否かを判断する。車速が下限値（例えば１０Ｋ
ｍ／ｈ）より低ければ，ステップ１８３にて補正モード
をモード−１のままとし、終了する。車速が下限値より
高ければ、ステップ１７８にて遅延時間分のジャイロデ
ータを保存しているか否かを調べる。ここで、遅延時間
分のジャイロデータを保存しているときにはステップ１
７９へ進むが、遅延時間分のジャイロデータを保存して
いない場合にはステップ１８３へ進み、補正モードをモ
ード−１のままとし、終了する。ステップ１７９では補
正モードをモード０とし、ステップ１８０にて方位をφ
ｍ１

〔０〕に、また、ＧＰＳの遅延量をφｍ１〔１〕に
代入する。次に、ステップ１８１にてタイマーＴφを０
にセットし、ステップ１８２にてカウンタcount0を０に
セットしてこのルーチンを終了する。タイマーＴφは、
この後、経過時間をカウントする。

【００８７】上述したように、モード−１ルーチンで
は、車速が下限値以上であり、かつ、ＧＰＳが出力する
データの遅延時間分のジャイロのデータを保存している
場合に、ジャイロ補正を開始する。

【００８８】図２０はモード０ルーチンのフローチャー
トである。補正モードがモード０のとき、このルーチン
が実行される。まず、ステップ１８４にてカウンタcoun
t0が１だけインクリメントされる。次に、ステップ１８
５にて車速が所定の下限値より大となったか否かを判断
する。ステップ１８６では遅延時間分のジャイロデータ
が保存してあるか否かを判断する。ステップ１８７では
モード−１と連続しているか否かを判断する。ステップ
１８８ではモード−１とこの時点の方位が同じであるか
否かを判断する。ステップ１８５〜１８８のいずれかが
不成立の場合、ステップ１８９にて補正モードをモード
−１に変更し、このルーチンを終了する。ステップ１８
５〜１８８のいずれも成立しているとき、ステップ１９
０にて方位安定化がＯＫか否かが判断される。方位が不
安定であればこのルーチンを終了する。方位が安定して
おれば、ステップ１９１にて数値補正ルーチンを実行
し、ステップ１９２にて補正モードをモード１とする。
数値補正ルーチンの詳細は後述するが、ＧＰＳ方位とジ
ャイロ方位の差を補正量とし、この補正量に応じてジャ
イロ方位等を補正するものである。次に、ステップ１９
３にて、ＧＰＳにて測位した方位，遅延，誤差をそれぞ
れφ０

〔０〕，φ０〔１〕，φ０〔２〕に代入する。次
に、ステップ１９４にてタイマーＴφを０にセットす
る。次に、ステップ１９５にて遅延量に応じたジャイロ
方位gldelay-raw 〔遅延量〕をφ０raw に代入する。次
に、ステップ１９６にてカウンタcount0及びカウンタco
unt1を０にセットしてこのルーチンを終了する。

【００８９】このように、モード０ルーチンでは、モー
ド−１での開始条件（車速が下限値以上であり、かつ、
ＧＰＳが出力するデータの遅延時間分のジャイロのデー
タを保存している）が継続しており、モード−１から続
いて実行されており、かつモード−１から方位に変更が
ない場合に、ジャイロ方位であるφ０raw ，ＧＰＳ方位
であるφ０

〔０〕等のデータを記憶する。この点が第１
の地点となる。

【００９０】図２１はモード１ルーチンのフローチャー
トである。補正モードがモード１のとき、このルーチン
が実行される。まず、ステップ１９７にてカウンタcoun
t1が１だけインクリメントされる。次に、ステップ１９
８にて車速が所定の下限値より大となったか否かを判断
する。ステップ１９９では遅延時間分のジャイロデータ
が保存してあるか否かを判断する。ステップ１９８，１
９９のいずれかが不成立の場合、ステップ２０３にてカ
ウンタcount1を０とし、このルーチンを終了する。ステ
ップ１９８，１９９のいずれも成立しているとき、ステ
ップ２００にてモード１が２回目以降か否かを判断す
る。１回目であればステップ２０１にて測位した方位，
遅延をそれぞれφ１

〔０〕，φ１〔１〕に代入し、この
ルーチンを終了する。ステップ２００にてモード１が２
回目以降であれば、ステップ２０２にて前回のモード１
とこの時点の方位が同じであるか否かを判断する。方位
が連続していなければ、ステップ２０３にてカウンタco
unt1を０とし、このルーチンを終了する。方位が同じで
あれば、ステップ２０４にて方位安定化がＯＫか否かが
判断される。方位が不安定であればこのルーチンを終了
する。方位が安定しておれば、ステップ２０５にて数値
補正ルーチンを実行し、ステップ２０６にてＧＰＳにて
測位した方位，遅延をそれぞれφ１

〔０〕，φ１〔１〕
に代入する。次に、ステップ２０７にて誤差がφ０
〔２〕より大きいか否かを判断する。大きければステッ
プ２０８にてφ１〔２〕に誤差を代入し、小さければス
テップ２０９にてφ１〔２〕にφ０〔２〕を代入する。
次に、ステップ２１０にて遅延量に応じたジャイロ方位
gldelay-raw 〔遅延量〕をφ１raw に代入する。次に、
ステップ２１１にてタイマーＴφの値にφ０〔１〕を加
え、φ１〔１〕を引いてＴ１０とする。次に、ステップ
２１２にてφ１

〔０〕からφ０

〔０〕を引いて変数col1
とする。次にステップ２１３にてこの角度差が採用ＯＫ
か否かを判断する。採用ＯＫでなければこのルーチンを
終了する。採用ＯＫであれば、ステップ２１４にてφ１
raw からφ０raw を引いてraw1とする。次に、ステップ
２１５にてcol1，raw1をそれぞれＴ１０で割って更新す
る。次に、ステップ２１６にて補正モードをモード２に
変更し、ステップ２１７にてタイマーＴφを０にリセッ
トし、ステップ２１８にてカウンタcount1及びカウンタ
count2を０にセットしてこのルーチンを終了する。

【００９１】このように、モード１ルーチンでは、モー
ド−１での開始条件（車速が下限値以上であり、かつ、
ＧＰＳが出力するデータの遅延時間分のジャイロのデー
タを保存している）が継続しており、モード１が２回以
上連続して実行されており、同じ方位が連続して安定し
ている場合に、ジャイロ方位であるφ１raw ，ＧＰＳ方
位であるφ１

〔０〕等のデータを記憶する。この点が第
２の地点となる。また、第１の地点と第２の地点間の時
間の差Ｔ１０、ＧＰＳ方位の差φ１

〔０〕−φ０

〔０〕
（ステップ２１２でのcol1），及びジャイロ方位の差φ
１raw −φ０raw （ステップ２１４でのraw1）を計算
し、ステップ２１５にて時間当たりのＧＰＳ方位col1，
及び時間当たりのジャイロ方位raw1を求めておく。

【００９２】図２２はモード２ルーチンのフローチャー
トである。補正モードがモード２のとき、このルーチン
が実行される。まず、ステップ２１９にてカウンタcoun
t2が１だけインクリメントされる。次に、ステップ２２
０にて車速が所定の下限値より大となったか否かを判断
する。ステップ２２１では遅延時間分のジャイロデータ
が保存してあるか否かを判断する。ステップ２２０，２
２１のいずれかが不成立の場合、ステップ２２５にて補
正モードをモード２のままとし、ステップ２３８にてカ
ウンタcount2を０とし、このルーチンを終了する。ステ
ップ２２０，２２１のいずれも成立しているとき、ステ
ップ２２２にてモード２が２回目以降か否かを判断す
る。１回目であればステップ２２３にて測位した方位，
遅延をそれぞれφ２

〔０〕，φ２〔１〕に代入し、この
ルーチンを終了する。ステップ２２２にてモード１が２
回目以降であれば、ステップ２２４にて前回のモード２
とこの時点の方位が同じであるか否かを判断する。方位
が連続していなければ、ステップ２２５て補正モードを
モード２のままとし、ステップ２３８にてカウンタcoun
t2を０とし、このルーチンを終了する。方位が同じであ
れば、ステップ２２６にて方位安定化がＯＫか否かが判
断される。方位が不安定であればこのルーチンを終了す
る。方位が安定しておれば、ステップ２２７にて数値補
正ルーチンを実行し、ステップ２２８ にて測位した方
位，遅延をそれぞれφ２

〔０〕，φ２〔１〕に代入す
る。次に、ステップ２２９にて誤差がφ１〔２〕より大
きいか否かを判断する。大きければステップ２３１にて
φ２〔２〕に誤差を代入し、小さければステップ２３０
にてφ２〔２〕にφ１〔２〕を代入する。次に、ステッ
プ２３２にて遅延量に応じたジャイロ方位gldelay-raw
〔遅延量〕をφ２rawに代入し、タイマーＴφの値にφ
１〔１〕を加えφ２〔１〕を引いてＴ２１とし、φ２

〔０〕からφ１

〔０〕を引いてcol2とする。次にステッ
プ２３３にてこの角度差が採用ＯＫか否かを判断する。
採用ＯＫでなければ、ステップ２３４にてカウンタcoun
t2を０とし、ステップ２３５にてcol2を９９９（最大
値）に設定し、このルーチンを終了する。採用ＯＫであ
れば、ステップ２３６にてφ２rawからφ１rawを引いて
raw2とし、col2，raw2をそれぞれＴ２１で割って更新す
る。次に、ステップ２３７にて、col1とcol2の差の絶対
値が所定の下限値をＴ２１で割った値よりも大きいか否
かを判断する。小さければステップ２３８にてカウンタ
count2を０とし、このルーチンを終了する。大きけれ
ば、ステップ２３９を実行する。ステップ２３９ではco
l1とcol2の差の絶対値が所定の上限値をＴ２１で割った
値よりも小さく、かつ、raw1とraw2の差の絶対値が所定
の上限値をＴ２１で割った値よりも小さいか否かを判断
する。成立しなければステップ２４６へ跳び、成立すれ
ばステップ２４０を実行する。ステップ２４０ではraw1
とraw2の差の絶対値が所定の下限値をＴ２１で割った値
よりも大きいか否かを判断する。小さければこのルーチ
ンを終了し、大きければステップ２４１を実行する。ス
テップ２４１では補正量和が所定の下限値より大きいか
否かを判断する。大きければステップ２４２でＡＤ変換
係数補正値を初期化し、ステップ２４３でＡＤ変換係数
補正ルーチンを実行する。小さければステップ２４４に
て逐次近似ルーチンを実行する。次に、ステップ２４５
にて補正モードをモード−１に設定しステップ２４６へ
進む。ステップ２４６では、φ２

〔０〕をφ１

〔０〕に
代入し、φ２〔１〕をφ１〔１〕に代入し、φ２〔２〕
をφ１〔２〕に代入し、col2をcol1に代入し、raw2をra
w1に代入する。次に、ステップ２４７にてタイマーＴφ
を０にリセットし、このルーチンを終了する。

【００９３】このように、モード２ルーチンでは、モー
ド−１での開始条件（車速が下限値以上であり、かつ、
ＧＰＳが出力するデータの遅延時間分のジャイロのデー
タを保存している）が継続しており、モード２が２回以
上連続して実行されており、同じ方位が連続して安定し
ている場合に、ジャイロ方位であるφ２raw ，ＧＰＳ方
位であるφ２

〔０〕等のデータを記憶する。この点が第
３の地点となる。また、第３の地点と第２の地点間の時
間の差Ｔ２１、ＧＰＳ方位の差φ２

〔０〕−φ１

〔０〕
（ステップ２３２でのcol2），及びジャイロ方位の差φ
２raw −φ１raw を計算し、ステップ２３５にて時間当
たりのＧＰＳ方位col2，及び時間当たりのジャイロ方位
raw2を求めておく。

【００９４】図２３は数値補正ルーチンのフローチャー
トである。まず、ステップ２４８にてＧＰＳ方位から遅
れ量を加味した方位ψ〔遅れ〕を引いて補正量を得る。
次に、ステップ２４９にてこの補正量がＧＰＳ方位誤差
のＮ倍以上でありかつＧＰＳ方位出力の最小単位の半分
以上であるか否かを判断し、この条件が成立すればステ
ップ２５０にて補正量和に補正量を加算して補正量の積
算値を求める。ここで、ＧＰＳ方位誤差は次式に基づき
求めている。尚、ＶｅはＧＰＳの速度誤差定数である。

【００９５】

【数２８】 ＧＰＳ方位誤差＝arctan（Ｖｅ／（車速＋Ｖｅ）） 次にステップ２５１にてジャイロ方位に補正量を加え、
ジャイロ方位を補正する。次にステップ２５２にて、方
位の遅延量であるψ〔−３〕，ψ〔−２〕，ψ〔−
１〕，ψ

〔０〕にそれぞれ補正量を加え、補正する。次
にステップ２５３にて基準方位を有効とし、ステップ２
５４にて方位推定誤差ψerror をゼロとする。

【００９６】次に、ステップ２５５にてＧＰＳ車速が予
め定めた車速補正下限（例えば１０Ｋｍ／ｈ）以上であ
るか否かを判断し、車速補正下限以下であればこのルー
チンを終了し、車速補正下限以上であればステップ２５
６にて、ＧＰＳ車速を、遅れを加味したパルス数Ｐ〔遅
れ〕で割って変数ROLLnew を求める。また、ＧＰＳ車速
誤差を、遅れを加味したパルス数Ｐ〔遅れ〕で割って変
数ROLLerrornewを求める。次に、ステップ２５７にて数
２９式に基づき変数ROLLを補正し、ステップ２５８にて
数３０式に基づき変数ROLLerror を補正する。

【００９７】

【数２９】

【００９８】

【数３０】

【００９９】次に、ステップ２５９にて、パルス数の遅
延量であるＰ〔−３〕，Ｐ〔−２〕，Ｐ〔−１〕，Ｐ

〔０〕にそれぞれ補正量を加え、補正し、このルーチン
を終了する。

【０１００】図２４はＡＤ変換係数補正ルーチン及び逐
次近似ルーチンのフローチャートである。まず、ここで
はＧＰＳ方位であるcol1，col2、ジャイロ方位であるra
w1，raw2を用いて、行列Ａ_i ^G 及びＬ_i ^G を次のように
定義する。

【０１０１】

【数３１】

【０１０２】

【数３２】Ｌ_i ^G ＝〔col1 col2〕^-T 更に、補正係数α及びθ₀ を用いて、行列Ｘ_i ^G を次の
ように定義する。

【０１０３】

【数３３】Ｘ_i ^G ＝〔α θ₀ 〕^-T ここで、図２８を参照して補正係数α及びθ₀ の関係を
説明する。ここでは、道路Ｒ上を車両が進行中、第１の
地点ａ１から第２の地点ａ２までの間モード１となり、
第２の地点ａ２から第３の地点ａ３までの間モード２と
なっているものとする。第１の地点ａ１の車両の実際の
方位をｄ１、第２の地点ａ２の車両の実際の方位をｄ
２、第３の地点ａ３の車両の実際の方位をｄ３とする。
ここで、第１の地点ａ１から第２の地点ａ２までに時間
Ｔ１０がかかり、第２の地点ａ２から第３の地点ａ３ま
でに時間Ｔ２１がかかったものとする。ここで、モード
１でのＧＰＳの方位変化量はcol1・Ｔ１０となり、時間
当たりの方位変化量はcol1となる。また、モード２での
ＧＰＳの方位変化量はcol2・Ｔ２１となり、時間当たり
の方位変化量はcol2となる。モード２でのジャイロの方
位変化量はraw1・Ｔ１０となり、時間当たりの方位変化
量はraw1となる。また、モード２でのジャイロの方位変
化量はraw2・Ｔ２１となり、時間当たりの方位変化量は
raw2となる。ここで、Ａ_i ^G ・Ｘ_i ^G ＝Ｌ_i ^G （ｉは整
数）とおくと、Ａ_i ^G 及びＬ_i ^G は算出できるので、Ｘ
_i ^G の各項である補正係数α，θ₀ が求められる。

【０１０４】ＡＤ変換係数補正ルーチンにおいては、ま
ず、ステップ２６０にて変数ｉに０をセットし、次に、
ステップ２６１にて、col1，col2の算出時に使用したＧ
ＰＳ方位誤差の内最大値を示すφ2 2 から１／σθ² を
数３４式により求める。

【０１０５】

【数３４】１／σθ² ＝１／φ2 2 ² 次に、ステップ２６２にて、行列Σε₁ ^{G -1}を次式に基
づき算出する。

【０１０６】

【数３５】

【０１０７】次に、ステップ２６３にて行列ΣＸ₁ ^G を
次式に基づき算出する。

【０１０８】

【数３６】ΣＸ₁ ^G ＝（Ａ₁ ^{G T} Σε₁ ^{G -1}Ａ₁ ^G ）^-1 次に、ステップ２６４にて行列Ｘ₁ ^G を次式に基づき算
出する。

【０１０９】

【数３７】Ｘ₁ ^G ＝Ａ₁ ^{G -1}Ｌ₁ ^G 尚、補正中の逆行列無しの場合には補正キャンセルとす
る。これにより、行列Ｘ₁ ^G が求まり、補正係数α及び
θ₀ が求まる。次に、ステップ２６５にて、圧電振動ジ
ャイロ１５の変換係数ＡＤＡに補正係数αを掛けて変換
係数ＡＤＡを更新する。また、ステップ２６６にて行列
ΣＸ_i+1 ^G の１１成分の平方根を取って、圧電振動ジャ
イロ１５の誤差係数ＡＤＡerror とする。ここではｉ＝
０であるので、ここでは行列ΣＸ₁ ^G の１１成分の平方
根を取る。次に、ステップ２６７にて、θ₀ をサンプル
数samples 及び変換係数ＡＤＡで割った値を変換係数Ａ
ＤＣから引いて、変換係数ＡＤＣを更新する。次に、ス
テップ２６８にて行列ΣＸ_{i+ 1} ^G の２１成分の平方根を
取り、サンプル数samples 及び変換係数ＡＤＡで割って
圧電振動ジャイロ１５の誤差係数ＡＤＣerror とする。
ここではｉ＝０であるので、ここでは行列ΣＸ₁ ^G の２
１成分の平方根を取る。次に、ステップ２６９にて遅延
データの再収集指定を行い、ステップ２７０にて補正量
和を零としてこのルーチンを終了する。

【０１１０】逐次近似ルーチンでは、まずステップ２７
１にて２回目移行の係数補正か否かを判断する。１回目
の係数補正であれば、ステップ２６０に進み、以下ＡＤ
変換係数補正ルーチンと同一の処理を行う。２回目以降
の係数補正であれば、ステップ２７２に進み、変数ｉに
１を加える。次に、ステップ２７３にて、col1，col2の
算出時に使用したＧＰＳ方位誤差の内最大値を示すφ2
2 から数３８式に応じてσθ² を求める。

【０１１１】

【数３８】σθ² ＝φ2 2 ² 次に、ステップ２７４にて、行列Σε_i+1 ^{G -1}を次式に
基づき算出する。

【０１１２】

【数３９】

【０１１３】次に、ステップ２７５及びステップ２７６
にて行列ΣＸ_i ^G をそれぞれ行列ΣＵ_i ^G 及び行列ΣＹ
_i ^G に代入する。次に、ステップ２７７にて、行列Ｙ
_i+1 ^G を次式に基づいて定義する。

【０１１４】

【数４０】Ｙ_i+1 ^G ＝〔１ ０〕^T 次に、ステップ２７８にて次式に基づきカルマンゲイン
を示す行列Ｋｇ^G を求める。

【０１１５】

【数４１】Ｋｇ^G ＝ΣＹ_i+1 ^G Ａ_i+1 ^{G T} 〔Σε_i+1 ^G
＋Ａ_i+1 ^G ΣＹ_i+1 ^G Ａ_i+1 ^{G T} 〕^-1 次に、ステップ２７９にて次式に基づき推定誤差行列Σ
_i+1 ^G を求める。ここで、Ｉは２×２の単位行列であ
る。

【０１１６】

【数４２】 ΣＸ_i+1 ^G ＝２〔Ｉ−Ｋｇ^G Ａ_i+1 ^G 〕ΣＹ_i+1 ^G 次に、ステップ２８０にて改善量を示す行列Ｄ^G を次式
に基づき求める。

【０１１７】

【数４３】Ｄ^G ＝Ｌ_i+1 ^G −Ａ_i+1 ^G Ｙ_i+1 ^G 次に、ステップ２８１にて行列Ｙ_i+1 ^G を次式に基づ
き、カルマンゲインに改善量を掛けた値を加えて更新す
る。

【０１１８】

【数４４】Ｙ_i+1 ^G ＝Ｙ_i+1 ^G ＋Ｋｇ^G Ｄ^G この後、ステップ２６５に進む。ステップ２６５にて、
圧電振動ジャイロ１５の変換係数ＡＤＡにαを掛けて変
換係数ＡＤＡを更新する。また、ステップ２６６にて行
列ΣＸ_i+1 ^G の１１成分の平方根を取って、圧電振動ジ
ャイロ１５の誤差係数ＡＤＡerror とする。次に、ステ
ップ２６７にて、θ₀ をサンプル数samples 及び変換係
数ＡＤＡで割った値を変換係数ＡＤＣから引いて、変換
係数ＡＤＣを更新する。次に、ステップ２６８にて行列
ΣＸ_i+1 ^G の２１成分の平方根を取り、サンプル数samp
les 及び変換係数ＡＤＡで割って圧電振動ジャイロ１５
の誤差係数ＡＤＣerror とする。次に、ステップ２６９
にて遅延データの再収集指定を行い、ステップ２７０に
て補正量和を零としてこのルーチンを終了する。

【０１１９】上記ＡＤ変換係数補正ルーチンにより、補
正係数α及びθ₀ が求められ、ジャイロの変換係数ＡＤ
Ａ，ＡＤＣや誤差係数ＡＤＡerror ，ＡＤＣerror がＧ
ＰＳ及びジャイロの測定値により補正され、より正確な
測位ができるようになる。

【０１２０】図２５はジャイロ０点補正ルーチンのフロ
ーチャートである。まず、ここではステップ２８２にて
車速＝０かつ補正量和が所定の下限値以下であるか否か
を判断し、否であればステップ２９３にてＶ≠０時０点
補正ルーチンを実行してこのルーチンを終了し、否でな
ければ次のステップ２８３に進む。ステップ２８３では
電源が入った直後であるか、又は、ドリフトを検出して
いるか否かを判断する。否であればステップ２８５にて
車速＝０の１秒き平均を取り、否でなければステップ２
８４にて全遅延データの平均を取る。次にステップ２８
６にて１秒間の和の内の最大値と最小値を求める。ステ
ップ２８７にて、この最大値と最小値の差が所定の上限
値（例えば２０ｄｅｇ／ｓ相当）より小さいか否かを判
断し、上限値より大きければ採用不可としてこのルーチ
ンを終了し、上限値より小さければステップ２８８にて
求めた平均値をゼロ点に変更する。その後ステップ２８
９にて遅延データの再収集指定を行い、ステップ２９０
にて補正モードをモード−１に指定し、ステップ２９１
にて補正量和を零にしてこのルーチンを終了する。

【０１２１】このジャイロ０点補正ルーチンおいては、
ジャイロの０点を測定し、ドリフト量が零になるように
補正している。

【０１２２】図２６は、Ｖ≠０の時の０点補正ルーチン
に関するフローチャートである。まず、ここでは、ステ
ップ２９４およびステップ２９５にて、それぞれ、ＧＰ
Ｓ測位中であるか、及び車速が所定値ＶＬより大である
かを判断する。ＧＰＳ測位中であり、かつ車速が所定値
ＶＬより大である場合にはステップ２９６へ進み、それ
以外はステップ３０４に進む。ステップ２９６では前回
方位を記憶しているか否かを判断する。

【０１２３】ここで、前回方位を記憶している場合には
ステップ２９７にて前回方位が今回方位と同じであるか
を判断する。ステップ２９６にて前回方位を記憶してい
ない場合、及びステップ２９７にて前回方位が今回方位
と同じでない場合には、ステップ２９８に進み現時点の
ＧＰＳ方位である今回方位を前回方位として記憶する。

【０１２４】この後、ステップ２９９にて回数を１とし
ておく。その後、ステップ３０５へ進む。ステップ２９
７にて前回方位が今回方位と同じ場合にはステップ３０
０にて回数を＋１し、データを記憶しておく。この回数
は前回方位と今回方位が同じ場合に＋１されるので、方
位が変化しない同一方位の回数を示すことになる。次
に、ステップ３０１にて同一方位の回数が所定の要求回
数以上になったか否かを判断する。ここで、所定の要求
回数は、次のように定める。まず、所定の車速ＶＬ，Ｖ
Ｕ（ただし、ＶＬ＜ＶＵ）および所定の時間ＴＬ，ＴＵ
（ただし、ＴＬ＜ＴＵ）を定める。ここで、ＶＬはＧＰ
Ｓ測位の誤差が大きくなる車速を設定し、ＶＵは自動車
の最高車速を設定しておくとよい。次に車速に応じて次
式のように時間Ｔを設定する。

【０１２５】

【数４５】 Ｔ＝ＴＬ＋（ＴＵ−ＴＬ）×（Ｖ−ＶＬ）／（ＶＵ−ＶＬ）・・・ＶＬ≦Ｖ≦Ｖ Ｕ Ｔ＝ＴＵ ・・・ＶＵ＜Ｖ この時間Ｔは、車速がＶＬに等しいとき最低の時間ＴＬ
に等しくなり、車速がＶＵに等しいとき最高の時間ＴＵ
に等しくなり、その間の車速では車速に比例して時間が
多くなる。要求回数は、この時間Ｔをこのルーチンが実
行される間隔で割って求める。

【０１２６】ステップ３０１で、同一方位回数が要求回
数よりも小のときにはステップ３０５へ進み、満たした
場合にはステップ３０２に進む。ステップ３０２では、
計算するデータが予め定めたデータ範囲内であるか否か
を判断する。データ範囲内の場合、ステップ３０３に進
み、新０点を計算する。この新０点にはジャイロ１５の
出力するデータを当てはめる。その後、ステップ３０４
にて回数を０に戻し、ステップ３０５に進む。

【０１２７】ステップ２９４にてＧＰＳ測位中でない場
合、ステップ２９５にて車速がＶＬより低い場合、及
び、ステップ３０２でデータ範囲内でない場合には、ス
テップ３０４にて回数を０とする。よって、これらの場
合には新０点は計算されない。

【０１２８】ステップ３０５では、補正量和が０より大
きく、かつ現０点が新０点より大きいか否かを判断す
る。条件が成立した場合にはステップ３０７に進み、成
立していない場合にはステップ３０６に進む。ステップ
３０６では、補正量和が０未満であり、かつ現０点が新
０点より小さいか否かを判断する。条件が成立した場合
にはステップ３０７に進み、成立していない場合にはこ
のルーチンを終了する。

【０１２９】ステップ３０７では現０点に新０点を代入
し更新する。補正量和は数値補正ルーチン中のステップ
２５０（図２３）にて設定されるものでありＧＰＳ方位
とジャイロ方位の差を累積したものである。ステップ３
０５及び３０６では、補正量のズレ方向と、０点の補正
方向が逆向きであるか否かを判断し、逆向きの場合のみ
０点を更新するようにしたものである。例えば、補正量
和＞０のとき、ジャイロは左回りにずれると考えられる
ので０点は右回りに補正されるはずである。この状況に
矛盾が生じている場合には、０点補正を行わない。

【０１３０】以上説明したように、本実施例は、車両の
方位偏差を検出するジャイロ１５と、ジャイロの出力か
らジャイロ方位を検出するジャイロ方位算出手段である
ステップ５６と、複数の衛星が発射する信号からそれぞ
れの衛星と車両との相対速度を検出し、車両の基準方位
を測定する方位測定手段であるアンテナ１０，ＧＰＳ受
信機１１，ＧＰＳ復調器１２と、車両の速度が高く、か
つ方位測定手段の測定する方位が時間的に変化かない場
合、ジャイロの出力信号がゼロとして０点補正する０点
補正手段であるステップ２９４〜３０７とを備えた。よ
って、走行中においてもジャイロの０点補正が行える。

【０１３１】また、ステップ３０１では０点を変更する
ために用いる時間を車速が高くなるにつれ長くするよう
にしている。具体的には、所定の車速ＶＬ，ＶＵ（ただ
しＶＬ＜ＶＵ）、所定の時間ＴＬ，ＴＵ（ただしＴＬ＜
ＴＵ）に対して、車速Ｖが０＜Ｖ＜ＶＬのとき、補正を
行わず、ＶＬ＜Ｖ＜ＶＵのとき、ＴＬ＋（ＴＵ−ＴＬ）
×（Ｖ−ＶＬ）／（ＶＵ−ＶＬ）で示される時間以上前
記方位測定手段の測定する方位に変化がない場合０点補
正し、ＶＵ＜Ｖのとき、ＴＵで示される時間以上前記方
位測定手段の測定する方位に変化がない場合０点補正す
るようにした。

【０１３２】よって、低車速時の精度の悪い方位測定手
段のデータを利用しないので、精度の悪化がない。ま
た、高速走行時において緩いカーブ上を走行しているよ
うな場合には０点補正が行われないため、精度のよい補
正ができる。

【０１３３】また、ステップ３０５，３０６では方位測
定手段とジャイロ方位算出手段の出力から求めた方位と
の差の方向が、０点を補正することにより変化する方位
の方向に対して逆の方向にあるときのみ０点を補正する
ようにした。よって、０点補正により、方位測定手段と
ジャイロ方位算出手段とがそれぞれに測定した方位がか
け離れる方向への補正は行われないので、より精度のよ
い補正ができる。

【０１３４】

【発明の効果】請求項１の発明によれば、走行中におい
てもジャイロの０点補正が行える。

【０１３５】この場合、高速走行時において緩いカーブ
上を走行しているような場合には０点補正が行われない
ため、精度のよい補正ができる。

【０１３６】請求項２の発明によれば、低車速時の精度
の悪い方位測定手段のデータを利用しないので、精度の
悪化がない。また、高速走行時において緩いカーブ上を
走行しているような場合には０点補正が行われないた
め、精度のよい補正ができる。

【０１３７】

【図面の簡単な説明】

【図１】本発明の実施例の回路構成図である。

【図２】本発明の実施例のメインルーチンのフローチャ
ートである。

【図３】本発明の実施例のジャイロ方位算出ルーチンの
フローチャートである。

【図４】本発明の実施例の方位推定誤差計算ルーチンの
フローチャートである。

【図５】本発明の実施例の積算距離算出ルーチンのフロ
ーチャートである。

【図６】本発明の実施例の航法方程式１ルーチンのフロ
ーチャートである。

【図７】本発明の実施例の航法方程式２ルーチンのフロ
ーチャートである。

【図８】本発明の実施例の航法方程式３ルーチンのフロ
ーチャートである。

【図９】本発明の実施例のσi²計算ルーチンのフローチ
ャートである。

【図１０】本発明の実施例のσa²計算ルーチンのフロー
チャートである。

【図１１】本発明の実施例のσG²計算ルーチンのフロー
チャートである。

【図１２】本発明の実施例のσδx²，σδy²計算ルーチ
ンのフローチャートである。

【図１３】本発明の実施例のＥψx Ｅψy 計算ルーチン
のフローチャートである。

【図１４】本発明の実施例の測位推定誤差計算ルーチン
のフローチャートである。

【図１５】本発明の実施例の測位ルーチンのフローチャ
ートである。

【図１６】本発明の実施例の高度補正ルーチンのフロー
チャートである。

【図１７】本発明の実施例のＵＥＲＥ計算ルーチンのフ
ローチャートである。

【図１８】本発明の実施例のジャイロ補正ルーチンのフ
ローチャートである。

【図１９】本発明の実施例のモード−１ルーチンのフロ
ーチャートである。

【図２０】本発明の実施例のモード０ルーチンのフロー
チャートである。

【図２１】本発明の実施例のモード１ルーチンのフロー
チャートである。

【図２２】本発明の実施例のモード２ルーチンのフロー
チャートである。

【図２３】本発明の実施例の数値補正ルーチンのフロー
チャートである。

【図２４】本発明の実施例のＡＤ変換係数補正ルーチン
及び逐次近似ルーチンのフローチャートである。

【図２５】本発明の実施例のジャイロ０点補正ルーチン
のフローチャートである。

【図２６】本発明の実施例のＶ≠０時０点補正ルーチン
のフローチャートである。

【図２７】本発明の実施例の説明図である。

【図２８】本発明の実施例の説明図である。

【符号の説明】

１０ 受信アンテナ １１ ＧＰＳ受信機 １２ ＧＰＳ復調器 １３ 表示装置 １４ 車速センサ １５ 圧電振動ジャイロ １６ 高度センサ １７ 電子制御ユニット １８ 入出力インターフェース １９ マイクロコンピュータ ２０ アナログ・デジタル変換器

フロントページの続き (56)参考文献 特開 平２−38916（ＪＰ，Ａ) 特開 平４−62419（ＪＰ，Ａ) 特開 平６−26865（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁷，ＤＢ名) G01C 19/00 - 19/72 G01C 21/00

Claims (2)

(57)【特許請求の範囲】
1. 【請求項１】 車両の方位偏差を検出するジャイロ； 該ジャイロの出力からジャイロ方位を検出するジャイロ
   方位算出手段； 複数の衛星が発射する信号からそれぞれの衛星と車両と
   の相対速度を検出し、車両の基準方位を測定する方位測
   定手段； 車両の速度が所定値ＶＬより高く、かつ前記方位測定手
   段の測定する方位が時間的に変化かない場合、ジャイロ
   の出力信号がゼロとして０点補正する０点補正手段；を
   備え、 前記０点補正手段は車速が高くなるにつれ、０点を変更
   するために用いる時間を長くした ことを特徴とするジャ
   イロ方位算出装置。
2. 【請求項２】 請求項１において、前記０点補正手段
   は、所定の車速ＶＬ，ＶＵ（＞ＶＬ）、所定の時間Ｔ
   Ｌ，ＴＵ（＞ＴＬ）を設定した場合に、 車速Ｖが０＜Ｖ＜ＶＬのとき、０点補正を行わず、 ＶＬ≦Ｖ≦ＶＵのとき、Ｔ＝ＴＬ＋（ＴＵ−ＴＬ）×
   （Ｖ−ＶＬ）／（ＶＵ−ＶＬ）で示される時間以上、前
   記方位測定手段の測定する方位に変化がない場合に０点
   補正し、 ＶＵ＜Ｖのとき、Ｔ＝ＴＵで示される時間以上、前記方
   位測定手段の測定する方位に変化がない場合に０点補正
   する ことを特徴とするジャイロ方位算出装置。