JPH0814923A - 車上測位装置 - Google Patents

Abstract

(57)【要約】 【目的】 精度をより正確にする。 【構成】 車両と地表との間の相対位置を測定する相対
センサと、複数の衛星が発射する信号からそれぞれの衛
星と車両との相対位置を検出するＧＰＳと、ＧＰＳの検
出した複数の衛星の情報から測位可能であり、かつ測位
の推定誤差が所定のレベルより小さいとき、ＧＰＳの出
力のみから車両の絶対位置を測位し、それ以外のとき、
ＧＰＳの出力と相対センサの出力、又は相対センサの出
力のみから車両の絶対位置を測位するステップ３１〜３
５，４２〜５３を設けた。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、自動車等の車両上にて
車両の位置を測位し、現在位置情報を知らせる車上測位
装置に関する。

【０００２】

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

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

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

【０００５】

【発明が解決しようとする課題】しかし、ＧＰＳのみで
測位するか、他のセンサで測位するかの切換は受信でき
る衛星の数で決めている。このため、衛星による測位よ
りも車両と地表との間の相対位置を測定する相対センサ
による測位の方の誤差が少なくても衛星による測位を優
先させるため、測位の精度を衛星による測位の誤差より
小さくすることはできない。

【０００６】そこで本発明においては、測位の精度をよ
り正確にすることを第１の課題とする。

【０００７】また、測位が全くできないときや、測位の
精度が悪いときには、誤った情報を与えないようにする
ことを第２の課題とする。

【０００８】また、測位方法を切り換えたとき測位結果
が急に変化しないようにすることを第３の課題とする。

【０００９】

【課題を解決するための手段】第１の課題を解決するた
めに請求項１に記載の発明においては、車両と地表との
間の相対位置を測定する相対センサ；複数の衛星が発射
する信号からそれぞれの衛星と車両との相対位置を検出
する衛星情報検出手段；衛星情報検出手段の検出した複
数の衛星の情報から測位可能であり、かつ測位の推定誤
差が所定のレベルより小さいとき、衛星情報検出手段の
出力のみから車両の絶対位置を測位し、それ以外のと
き、衛星情報検出手段の出力と相対センサの出力、又は
相対センサの出力のみから車両の絶対位置を測位する、
測位手段；を備えた。

【００１０】第１の課題を解決するために請求項２に記
載の発明においては、車両の方位偏差を検出するジャイ
ロ；車両の走行距離を検出する距離センサ；複数の衛星
が発射する信号からそれぞれの衛星と車両との相対速度
を検出する相対速度検出手段；ジャイロの出力から車両
の方位角を求める方位角計算手段；距離センサの出力か
ら車両の積算距離を求める積算距離計算手段； （１）相対速度検出手段が３衛星以上との間の相対速度
を検出しており、かつ３衛星以上との間の相対速度を用
いた測位の測位推定誤差が所定のレベルより小さいと
き、３衛星以上との間の相対速度を用いた測位を行い、
（２）相対速度検出手段が２衛星との間の相対速度を検
出しており、方位角計算手段の計算した方位角の方位推
定誤差が所定のレベルより小さく、かつ２衛星との間の
相対速度と前記方位角検出手段の計算した方位角を用い
た測位の測位推定誤差が所定のレベルより小さいとき、
２衛星との間の相対速度と方位角を用いた測位を行い、
（３）方位角計算手段の計算した方位角の方位推定誤差
が所定のレベルより小さく、積算距離計算手段の計算し
た積算距離の積算距離推定誤差が所定のレベルより小さ
く、かつ方位角検出手段の計算した方位角と積算距離計
算手段の計算した積算距離を用いた測位の測位推定誤差
が所定のレベルより小さいとき、方位角と積算距離を用
いた測位を行う、測位手段；を備えた。

【００１１】前記第１の課題に加え、第２の課題を解決
するために請求項３に記載の発明においては、請求項２
の発明の測位手段を（１），（２），（３）のいずれに
も該当しない場合、測位しないようにした。

【００１２】前記第１の課題に加え、第３の課題を解決
するために請求項４に記載の発明においては、請求項
１，２の発明の測位手段に対してカルマンフィルターに
より測位解にフィルターをかけるようにした。

【００１３】

【作用】請求項１の手段によれば、相対センサは車両と
地表との間の相対位置を測定する。衛星情報検出手段は
複数の衛星が発射する信号からそれぞれの衛星と車両と
の相対位置を検出する。測位手段は、衛星情報検出手段
の検出した複数の衛星の情報から測位可能であり、かつ
測位の推定誤差が所定のレベルより小さいとき、衛星情
報検出手段の出力のみから車両の絶対位置を測位し、そ
れ以外のとき、衛星情報検出手段の出力と相対センサの
出力、又は相対センサの出力のみから車両の絶対位置を
測位する。これにより、衛星情報検出手段の出力のみ，
衛星情報検出手段の出力と相対センサの出力、又は相対
センサの出力のみから車両の絶対位置が測位できる。

【００１４】ここで、測位手段は、衛星情報検出手段の
検出した複数の衛星の情報から測位可能であっても、測
位の推定誤差が所定のレベルより小さいとき、衛星情報
検出手段の出力のみから車両の絶対位置を測位は行わ
ず、衛星情報検出手段の出力と相対センサの出力、又は
相対センサの出力のみから車両の絶対位置を測位する。

【００１５】よって、より誤差の少ない測位ができる。

【００１６】請求項２の手段によれば、ジャイロは車両
の方位偏差を検出する。距離センサは車両の走行距離を
検出する。相対速度検出手段は、複数の衛星が発射する
信号からそれぞれの衛星と車両との相対速度を検出す
る。方位角計算手段はジャイロの出力から車両の方位角
を求める。積算距離計算手段は距離センサの出力から車
両の積算距離を求める。測位手段は、相対速度検出手
段，方位角計算手段及び積算距離計算手段の出力に応じ
て、次の（１），（２），（３）のいずれかの測位を選
択する。

【００１７】（１）相対速度検出手段が３衛星以上との
間の相対速度を検出しており、かつ３衛星以上との間の
相対速度を用いた測位の測位推定誤差が所定のレベルよ
り小さいとき、３衛星以上との間の相対速度を用いた測
位を行う。

【００１８】（２）相対速度検出手段が２衛星との間の
相対速度を検出しており、方位角計算手段の計算した方
位角の方位推定誤差が所定のレベルより小さく、かつ２
衛星との間の相対速度と前記方位角検出手段の計算した
方位角を用いた測位の測位推定誤差が所定のレベルより
小さいとき、２衛星との間の相対速度と方位角を用いた
測位を行う。

【００１９】（３）方位角計算手段の計算した方位角の
方位推定誤差が所定のレベルより小さく、積算距離計算
手段の計算した積算距離の積算距離推定誤差が所定のレ
ベルより小さく、かつ方位角検出手段の計算した方位角
と積算距離計算手段の計算した積算距離を用いた測位の
測位推定誤差が所定のレベルより小さいとき、方位角と
積算距離を用いた測位を行う。

【００２０】よって、それぞれの測位の前にそれぞれの
測位の測位推定誤差を求め、測位推定誤差が大きければ
その測位を行わないので、精度の良い測位方法が選択さ
れる。また、測位に方位角計算手段の計算した方位角を
用いる場合には、方位推定誤差を考慮し、測位に積算距
離計算手段の計算した積算距離を用いる場合には、積算
距離推定誤差を考慮しているので、精度の悪い情報を用
いた測位は除外され、結果として精度の良い測位ができ
る。

【００２１】請求項３の手段によれば、測位手段におい
て、前記（１），（２），（３）のいずれにも該当しな
い場合、測位しない。よって、測位推定誤差，方位推定
誤差及び積算距離推定誤差がいずれも悪いときには測位
が行われない。

【００２２】請求項４の手段によれば、測位手段におい
て、カルマンフィルターにより測位解にフィルターをか
ける。よって、車両の走行中や停車中に測位の方法が変
わっても異常な解の変化が抑制される。

【００２３】

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

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

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

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

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

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

〔０〕からφ０

〔０〕を引いて変数ｃｏ
ｌ１とする。次にステップ２１３にてこの角度差が採用
ＯＫか否かを判断する。採用ＯＫでなければこのルーチ
ンを終了する。採用ＯＫであれば、ステップ２１４にて
φ１ｒａｗ からφ０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 ）^-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 計算ルーチン
のフローチャートである。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

【符号の説明】

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

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

Claims (4)

【特許請求の範囲】
1. 【請求項１】 車両と地表との間の相対位置を測定する
   相対センサ；複数の衛星が発射する信号からそれぞれの
   衛星と車両との相対位置を検出する衛星情報検出手段；
   前記衛星情報検出手段の検出した複数の衛星の情報から
   測位可能であり、かつ測位の推定誤差が所定のレベルよ
   り小さいとき、前記衛星情報検出手段の出力のみから車
   両の絶対位置を測位し、それ以外のとき、前記衛星情報
   検出手段の出力と前記相対センサの出力、又は前記相対
   センサの出力のみから車両の絶対位置を測位する、測位
   手段；を備える車上測位装置。
2. 【請求項２】 車両の方位偏差を検出するジャイロ；車
   両の走行距離を検出する距離センサ；複数の衛星が発射
   する信号からそれぞれの衛星と車両との相対速度を検出
   する相対速度検出手段；前記ジャイロの出力から車両の
   方位角を求める方位角計算手段；前記距離センサの出力
   から車両の積算距離を求める積算距離計算手段； （１）前記相対速度検出手段が３衛星以上との間の相対
   速度を検出しており、かつ３衛星以上との間の相対速度
   を用いた測位の測位推定誤差が所定のレベルより小さい
   とき、３衛星以上との間の相対速度を用いた測位を行
   い、（２）前記相対速度検出手段が２衛星との間の相対
   速度を検出しており、前記方位角計算手段の計算した方
   位角の方位推定誤差が所定のレベルより小さく、かつ２
   衛星との間の相対速度と前記方位角検出手段の計算した
   方位角を用いた測位の測位推定誤差が所定のレベルより
   小さいとき、２衛星との間の相対速度と方位角を用いた
   測位を行い、（３）前記方位角計算手段の計算した方位
   角の方位推定誤差が所定のレベルより小さく、前記積算
   距離計算手段の計算した積算距離の積算距離推定誤差が
   所定のレベルより小さく、かつ前記方位角検出手段の計
   算した方位角と前記積算距離計算手段の計算した積算距
   離を用いた測位の測位推定誤差が所定のレベルより小さ
   いとき、方位角と積算距離を用いた測位を行う、測位手
   段；を備える車上測位装置。
3. 【請求項３】 請求項２において、前記測位手段は
   （１），（２），（３）のいずれにも該当しない場合、
   測位しないことを特徴とする車上測位装置。
4. 【請求項４】 請求項１，２において、前記測位手段は
   カルマンフィルターにより測位解にフィルターをかけた
   ことを特徴とする車上測位装置。