JPH0835839A

JPH0835839A - ジャイロ方位測定装置

Info

Publication number: JPH0835839A
Application number: JP17148194A
Authority: JP
Inventors: Seiji Ishikawa; 川誠司石; Naoji Nakahara; 原直司中; Takeshi Ito; 藤毅伊; Tomio Yasuda; 田富夫保
Original assignee: Aisin Seiki Co Ltd
Current assignee: Aisin Corp
Priority date: 1994-07-22
Filing date: 1994-07-22
Publication date: 1996-02-06

Abstract

(57)【要約】【目的】バンク路においても、精度のよい方位を得
る。【構成】車両の垂直軸を中心とする回転角速度ωを出
力するジャイロと、ジャイロ１５の出力から方位Δθを
演算する方位演算手段と、回転角速度ω及び車両の速度
ｖからバンク補正量Ｆを求めるバンク補正量演算手段
と、バンク補正量演算手段により求めたバンク補正量Ｆ
に応じて方位演算手段の演算する方位Δθを補正する方
位補正手段を備えたジャイロ方位測定装置。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、自動車等の車両上にて
車両の位置を測位し、現在位置情報を知らせる車上測位
装置であって、車両の方位の測定に車両の垂直軸を中心
とする回転角速度を出力するジャイロを用いるものにお
いて、ジャイロの出力する回転角速度を処理して方位を
得るジャイロ方位の測定装置に関する。

【０００２】

【従来の技術】例えば、特開昭６１−１３７００９号公
報及び特開昭６１−１６７８８６号公報等に開示されて
いるように、グローバルポジショニングシステム（Grob
al Positioning System ；以下ＧＰＳと称す）を利用し
た自動車用測位装置の技術が従来より提案されている。
ＧＰＳは複数の衛星を備える。衛星は、正確な時刻，軌
道の関数，及び情報の精度を示すデータを電波に乗せて
所定のタイミングで地上に送信している。地上では、衛
星から得た時刻と軌道の関数から衛星の位置を知ること
ができ、衛星の時刻と受信点の時刻との差、即ち電波の
伝搬遅延時間に基づいて、衛星から受信点までの距離を
知ることができる。互いに異なる３つの衛星のそれぞれ
について、衛星の位置と衛星から受信点までの距離がわ
かれば、３元連立方程式を解くことにより、未知数であ
る受信点の３次元位置を求めることができる。但し、通
常、受信点の時刻には比較的大きな誤差が含まれるの
で、受信点の時計の誤差を補償するためには、同時に４
つの衛星の情報が必要になる。

【０００３】また、受信点の高度が殆ど変化しないもの
と仮定すれば、３つの衛星の情報から受信点の地表上の
２次元位置を求めることができる。

【０００４】しかしながら、実際に存在するＧＰＳの衛
星の数は限られているし、自動車は、例えばトンネルの
中やビルの影では電波が遮断され、同時に３つ或いは４
つの衛星から電波を受信することが困難な場合がある。
そこで、特開昭６１−１３７００９号公報の技術では、
光ファイバージャイロを用いて車両のの垂直軸を中心と
する回転角速度を求め、自車の方位変化を検出すると共
に、車速センサを用いて自車の移動距離を検出し、これ
らの検出情報を補助的に使用して１衛星から電波しか受
信できない場合でも、自車の位置検出を可能にしてい
る。

【０００５】

【発明が解決しようとする課題】しかし、車両が左右方
向に傾いた路面上を走行する場合や、路面のカーブによ
り車体が左右にロールした場合には、車両の垂直軸が傾
き、ジャイロの出力する回転角速度に誤差が生じてしま
う。

【０００６】そこで本発明においては、上記のような状
況においても、精度のよい方位を求めることを課題とす
る。

【０００７】

【課題を解決するための手段】前記課題を解決するため
に請求項１に記載の発明においては、車両の垂直軸を中
心とする回転角速度を出力するジャイロ；ジャイロの出
力から方位を演算する方位演算手段；回転角速度及び車
両の速度からバンク補正量を求めるバンク補正量演算手
段；バンク補正量演算手段により求めたバンク補正量に
応じて方位演算手段の演算する方位を補正する方位補正
手段；を備えた。

【０００８】また、前記課題を解決するために請求項２
に記載の発明においては、請求項１において、Ｆ＝１／
（cos(ａ×Arctan（ｖ×ω／ｇ）））よりバンク補正量
Ｆを求め、方位演算手段により演算された方位にバンク
補正量Ｆを掛けて方位を補正した。

【０００９】

【作用】請求項１の手段によれば、ジャイロは車両の垂
直軸を中心とする回転角速度を出力する。方位演算手段
はジャイロの出力から方位を演算する。一方、バンク補
正量演算手段は回転角速度及び車両の速度からバンク補
正量を求める。方位補正手段はバンク補正量演算手段に
より求めたバンク補正量に応じて方位演算手段の演算す
る方位を補正する。よって、バンク補正量に応じて方位
が補正されるので、精度のよい方位が得られる。

【００１０】請求項２の手段によれば、車速ｖ、ジャイ
ロの出力する回転角速度ωよりバンク補正量Ｆが求ま
り、この値を方位に掛けて方位を補正する。図２９に示
すように路面が角度θだけ傾斜している場合、ジャイロ
の出力する回転角速度ωと求めるべき車両の回転角速度
ωｔとを用いて方位の補正量Ｆは、Ｆ＝１／ (cos(Arcs
in(vω／g)))となる。しかし、実際にはサスペンション
の影響により車両のバンク角を小さくする等の効果があ
ると考えられ、実験的にはＦ＝１／ (cos(ａ×Arctan(v
ω／g)))とした計算式を採用することが良いことが分か
った。ここで、ａは１以下の数値で０．５程度が望まし
い。これにより、車両の傾斜を加味した補正が行われ、
精度のよい測定が行われる。

【００１１】

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

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

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

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

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

【００１６】ステップ４２にて、復調可能な衛星が１以
下の場合、又は、ステップ４３にて方位推定誤差が上限
より大の場合、又は、ステップ４６にて測位推定誤差が
上限より大の場合、ステップ４８にて方位推定誤差が予
め設定された上限（例えば３０°）より小であるか否か
を判断する。方位推定誤差が上限より小である場合、ス
テップ４９にて積算距離推定誤差ＳＴＥＰ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を数２２式に基づい
て計算する。

【００７３】

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

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

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

【００７６】図１７はＵＥＲＥ計算ルーチンのフローチ
ャートである。ここでは、まず、各々の衛星の情報から
衛星の配置によらない指標であるＳＶＡＣＣ（ＳＶaccu
racy：個々の衛星の内部状態から、その衛星１個が測位
に及ぼす影響を計算した指標）を得て、ステップ１６８
にて、ＳＶＡＣＣが０以上６未満であるか否かを判断す
る。ＳＶＡＣＣが０以上６未満であれば、ステップ１７
０にてユーザーレンジアキュラシーＵＲＡ（User Range
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を０とし、このルーチンを終了する。ステ
ップ２２０，２２１のいずれも成立しているとき、ステ
ップ２２２にてモード２が２回目以降か否かを判断す
る。１回目であればステップ２２３にて測位した方位，
遅延をそれぞれφ２

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

〔０〕，φ２〔１〕に代入す
る。次に、ステップ２２９にて誤差がφ１〔２〕より大
きいか否かを判断する。大きければステップ２３１にて
φ２〔２〕に誤差を代入し、小さければステップ２３０
にてφ２〔２〕にφ１〔２〕を代入する。次に、ステッ
プ２３２にて遅延量に応じたジャイロ方位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）^−１次に、ステップ２６４にて行列Ｘ_１ ^Ｇを次式に基づ
き算出する。

【０１０７】

【数３７】Ｘ₁ ^G＝Ａ₁ ^{G -1}Ｌ₁ ^G 尚、補正中の逆行列無しの場合には補正キャンセルとす
る。これにより、行列Ｘ₁ ^Gが求まり、補正係数α及び
θ₀が求まる。次に、ステップ２６５にて、圧電振動ジ
ャイロ１５の変換係数ＡＤＡに補正係数αを掛けて変換
係数ＡＤＡを更新する。また、ステップ２６６にて行列
ΣＸ_i+1 ^Gの１１成分の平方根を取って、圧電振動ジャ
イロ１５の誤差係数ＡＤＡerror とする。ここではｉ＝
０であるので、ここでは行列ΣＸ₁ ^Gの１１成分の平方
根を取る。次に、ステップ２６７にて、θ₀をサンプル
数samples 及び変換係数ＡＤＡで割った値を変換係数Ａ
ＤＣから引いて、変換係数ＡＤＣを更新する。次に、ス
テップ２６８にて行列ΣＸ_i+ ₁ ^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 がＧ
ＰＳ及びジャイロの測定値により補正され、より正確な
測位ができるようになる。

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

【０１１９】その後ステップ２８９にて遅延データの再
収集指定を行い、ステップ２９０にて補正モードをモー
ド−１に指定し、ステップ２９１にて補正量和を零にし
てこのルーチンを終了する。

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

【０１２１】図２６はバンク補正ルーチンのフローチャ
ートである。ここでは、まず、ステップ２９３にて、次
式に基づきバンク補正量Ｆを計算する。

【０１２２】

【数４５】Ｆ＝１／cos （Arctan(vω／ｇ）×０．５）ここで、v は車両の速度、ωはジャイロ１５の出力から
得られる回転角速度、ｇは重力加速度である。尚、０．
５は車両の特性等に合わせて１以下のたの数値に変えて
もよい。次に、ステップ２９４にて方位Δθにバンク補
正量Ｆを掛け、方位Δθを更新する。次にステップ２９
５にてバンク補正量Ｆが１．００１より大きいか否かを
判断する。大きければステップ２９６にて遅延データの
再収集指定を行いこのステップを終了する。小さけれ
ば、そのままこのステップを終了する。

【０１２３】以上説明したように、本実施例は、車両の
垂直軸を中心とする回転角速度ωを出力するジャイロ１
５と、ジャイロ１５の出力から方位Δθを演算する方位
演算手段であるステップ５６と、回転角速度ω及び車両
の速度ｖからバンク補正量Ｆを求めるバンク補正量演算
手段であるステップ２９３と、バンク補正量演算手段に
より求めたバンク補正量Ｆに応じて方位演算手段の演算
する方位Δθを補正する方位補正手段であるステップ２
９４を備えた。よって、バンク補正量に応じて方位が補
正されるので、精度のよい方位が得られる。

【０１２４】

【発明の効果】請求項１，２の発明によれば、バンク補
正量に応じて方位が補正されるので、精度のよい方位が
得られる。

【図面の簡単な説明】

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

【符号の説明】

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

フロントページの続き (72)発明者保田富夫愛知県刈谷市朝日町２丁目１番地アイシン精機株式会社内

Claims

【特許請求の範囲】

【請求項１】車両の垂直軸を中心とする回転角速度を
出力するジャイロ；該ジャイロの出力から方位を演算す
る方位演算手段；前記回転角速度及び車両の速度からバ
ンク補正量を求めるバンク補正量演算手段；前記バンク
補正量演算手段により求めたバンク補正量に応じて前記
方位演算手段の演算する方位を補正する方位補正手段；
を備えるジャイロ方位測定装置。
【請求項２】請求項１において、前記バンク補正量演
算手段は、Ｆ＝１／（cos(ａ×Arctan（ｖ×ω／ｇ）））ただし、ａは１以下の定数、ｖは車両の速度、ωはジャ
イロの出力する回転角速度、ｇは重力加速度を示す、よ
りバンク補正量Ｆを求め、前記方位補正手段は、前記方
位演算手段により演算された方位に前記バンク補正量Ｆ
を掛けて方位を補正することを特徴とするジャイロ方位
測定装置。