JP5122678B1 - 衛星航法システムを用いた位置出力装置 - Google Patents

Abstract

【課題】衛星航法システムによって得られる衛星航法測定位置データを用いて、対象の現在位置データを出力する位置出力装置であって、衛星航法システムからの位置データに位置飛びが発生した場合に、適切な現在位置を出力する。
【解決手段】前の更新タイミングにおける現在位置から対象の予測位置を算出し、予測位置と衛星航法システムで更新される衛星航法測定位置との位置差から位置飛びが発生したかを判定し、位置飛びが発生したと判定された場合に、現在位置を補正するための補正位置ベクトルを算出する。補正位置ベクトルは、前記予測位置と前記衛星航法測定位置との位置差を更新周期よりも長い時間をかけて解消するとした場合の１回の更新タイミング当たりの補正位置ベクトルとし、位置飛びが発生したと判定された場合に、前記予測位置を前記補正位置ベクトルによって補正したものを現在位置とする。
【選択図】図４

Description

本発明は、衛星航法システムによって得られる測定位置データを用いて、移動体等の対象の現在位置データを出力する位置出力装置に関する。

近年、衛星航法システム（例えばＧＰＳ）を用いて得られる位置データの精度が向上している。これは、そのＧＰＳ受信機単体の精度向上も含め、衛星航法システムの補強システム（例えばＭＳＡＳ 運輸多目的衛星航法補強システム）等の運用による寄与も大きい。

しかしながら、依然として衛星航法システムから得られる位置データは、樹木、ビル等によるＧＰＳ衛星からの電波遮断、マルチパス、衛星の切り替り等により、データが大きく変化する「位置飛び」が発生することがあるという欠点がある。

一方、従来から加速度計、ジャイロ、磁気方位センサ等のセンサを用いて自立的に位置を求める慣性航法システム（自立航法システム）が知られている。このシステムの欠点は、センサの誤差が積分によって累積していくということである。また、このようなセンサの誤差を低減させるべくセンサの精度を高めるには、高価になるという問題がある。

これらの各システムの欠点を補うために、従来、衛星航法システムと慣性航法システムとを組み合わせて位置の補正を行うものが種々、提案されている。

例えば、特許文献１では、ＧＰＳ受信装置による測位が可能であるかどうか判定し、可能である場合に、ＧＰＳからのデータと、慣性航法システムからのデータを取り込み、それぞれ直前の測定値と比較し、一定量を越えて変化している場合には、その測定値を偽と判断し、もう一方のシステムからの出力データを使用してナビゲーションを行い、両方の測定値とも一定値以内であれば、いずれかの測定値を選択するか、両者の平均を計算してナビゲーションを行うようにしている。

また、特許文献２では、自立航法によるマップマッチングを行うと共に、自立航法によるマップマッチングが不可能になったときに、ＧＰＳにより得られる位置データを用いて、自立航法により得られる車両位置データを修正するようになっている。

また、慣性航法システムを使用せずに、衛星航法システムにおける位置飛びを解決するためのものとして、特許文献３では、位置飛びが発生したかどうか判定し、位置飛びが発生していない場合には、予想位置を用いて今回測位位置を補正して今回出力位置とし、位置飛びが発生している場合には、予想位置を今回出力位置にするか、または、今回測位位置を今回出力位置にするか、のいずれかにしている。

特許第２８１２７９５号公報 特許第３９８４１１２号公報 特開２００９−９７８９８号公報

しかしながら、従来の位置補正においては、衛星航行システムによる位置飛びが発生した場合に、その補正により出力位置データが急激に変化するおそれがある。

また、慣性航法システムによって得られる位置データに比較して、衛星航法システムからの位置データの更新周期が長いため、瞬間的な変化に対して、衛星航法システムによって得られる位置データでは遅れが生じてしまうという問題もある。

本発明はかかる課題に鑑みなされたもので、第１の目的は、衛星航法システムからの位置データに位置飛びが発生した場合に、対象の適切な現在位置を出力することができる衛星航法システムを用いた位置出力装置を提供することである。

また、第２の目的は、衛星航法システムによって生じる位置誤差に対して、慣性航法システムから得られるデータを用いて、対象の適切な現在位置を出力することができる衛星航法システムを用いた位置出力装置を提供することである。

上記目的を達成するために、本発明の請求項１記載の発明は、衛星航法システムによって得られる衛星航法測定位置のデータを用いて、対象の現在位置データを出力する位置出力装置であって、
衛星航法システムで更新される衛星航法測定位置のデータを取り込む衛星航法データ取込手段と、
現在位置を更新タイミングで順次算出して現在位置データとして出力する現在位置算出手段と、
前の更新タイミングにおける現在位置から対象の予測位置を求める予測位置算出手段と、
前記予測位置と前記衛星航法測定位置との位置差から位置飛びが発生したかを判定する位置飛び判定手段と、
位置飛びが発生したと判定された場合に、現在位置を補正するための補正位置ベクトルを算出する補正位置算出手段と、
位置飛びが発生した後に位置飛びが発生していないと判定された場合に、前記予測位置と前記衛星航法測定位置との間の距離が設定距離より大きいかを判定する位置戻り判定手段と、
前記予測位置と前記衛星航法測定位置との間の距離が設定距離より大きいと判定された場合に、現在位置を補正するための第２補正位置ベクトルを算出する第２補正位置算出手段と、
を備え、
前記補正位置算出手段は、前記予測位置と前記衛星航法測定位置との位置差を、更新周期よりも長い時間をかけて解消するとした場合の１回の更新タイミング当たりの補正位置ベクトルを算出しており、
前記第２補正位置算出手段は、前記予測位置と前記衛星航法測定位置との位置差を、更新周期よりも長い時間であるが、前記補正位置算出手段における算出で用いられる時間よりも短い時間をかけて解消するとした場合の１回の更新タイミング当たりの第２補正位置ベクトルを算出しており、
前記現在位置算出手段は、位置飛びが発生したと判定された場合に、前記予測位置を前記補正位置ベクトルによって補正したものを現在位置とするとともに、位置飛びが発生した後に位置飛びが発生していないと判定され、かつ前記予測位置と前記衛星航法測定位置との間の距離が設定距離より大きいと判定された場合に、前記予測位置を前記第２補正位置ベクトルによって補正したものを現在位置とすることを特徴とする。

請求項２記載の発明は、請求項１記載の発明において、前記補正位置ベクトルの大きさは、前記予測位置と前記衛星航法測定位置との距離に更新周期を第１時定数で除算した値を乗算したものであることを特徴とする。

請求項３記載の発明は、請求項２記載の発明において、前記第２補正位置ベクトルの大きさは、前記予測位置と前記衛星航法測定位置との距離に更新周期を第２時定数で除算した値を乗算したものとし、第２時定数＜第１時定数であることを特徴とする。

請求項４記載の発明は、請求項１ないし３のいずれか１項に記載の発明において、
慣性航法システムで更新される慣性航法測定速度のデータを取り込む慣性航法データ取込手段と、をさらに備え、
前記予測位置算出手段は、前記慣性航法測定速度を用いて予測位置を算出することを特徴とする。

請求項５記載の発明は、請求項１ないし４のいずれか１項に記載の発明において、
位置飛びが発生していないと判定され且つ前記衛星航法測定位置データが更新されていない間は、前記現在位置算出手段は、前記予測位置を前記現在位置とすることを特徴とする。

請求項６記載の発明は、請求項１ないし５のいずれか１項に記載の発明において、
位置飛びが発生していないと判定され且つ前記衛星航法測定位置データが更新された場合は、前記現在位置算出手段は、前記衛星航法測定位置を前記現在位置とすることを特徴とする。

本発明によれば、位置飛びが発生したと判定された場合に、補正位置ベクトルによって、出力される現在位置データが、更新周期よりも長い時間をかけて漸次、衛星航法測定位置データに近づくように補正されるため、出力されるデータが急激に変化することを防ぐことができる。

本発明の第１実施形態による位置出力装置を含む位置測定システムの全体ブロック図である。 本発明の第１実施形態による位置出力装置のブロック図である。 第１実施形態による位置出力装置による処理を表すフローチャートである。 第１実施形態による位置出力装置による出力例を表す説明図である。 本発明の第２実施形態による位置出力装置を含む位置測定システムの全体ブロック図である。 本発明の第２実施形態による位置出力装置のブロック図である。 慣性計測装置のブロック図である。 第２実施形態による位置出力装置による処理を表すフローチャートである。 第２実施形態による位置出力装置による出力例を表す説明図である。 本発明の第３実施形態による位置出力装置のブロック図である。 第３実施形態による位置出力装置による処理を表すフローチャートである。 第３実施形態による位置出力装置による出力例を表す説明図である。

以下、図面を参照しながら、本発明の実施形態について説明する。

（第１実施形態）
図１に示すように、本実施形態による位置出力装置を用いた位置測定システムは、移動体といった対象に搭載されて、その現在位置データを出力するものであり、移動体の移動範囲は、２次元及び３次元とすることができる。

位置測定システム１０は、大まかに、衛星航法システムを構成するＧＰＳアンテナ２０及びＧＰＳ受信機２２と、位置出力装置２６と、出力装置２８と、を備える。出力装置２８は、例えば、表示装置、記録装置等とすることができるが、省略することも可能であり、この位置測定システム１０からの出力を利用する他の装置とすることもできる。

ＧＰＳ受信機２２は、ＧＰＳアンテナ２０で受信されたＧＰＳ衛星からの信号によって、一定の更新周期（この周期をＴ_GPSとする、例えば、２００ｍｓ程度）で、移動体の位置（ＧＰＳ位置と称する）、移動体の速度（ＧＰＳ速度Ｖ_GPSと称する）といったＧＰＳデータを求めて出力している。

位置出力装置２６は、基本的にＧＰＳ受信機２２から得られるＧＰＳ位置を移動体位置として出力するのであるが、ＧＰＳ位置は、衛星からの電波遮断、マルチパス、衛星の切り替りにより、データが急峻に変化する位置飛びが発生する可能性がある。

そのような位置飛びの影響を回避するために、位置出力装置２６は、ＣＰＵ及びメモリを備え、メモリによって格納されるプログラムによって、ＧＰＳデータ取込手段４４と、位置飛び判定手段４６と、現在位置算出手段４８と、予測位置算出手段５０と、補正位置算出手段５２と、データ記憶手段５４として機能する。

各手段による処理を図３を参照しながら、説明する。

ＧＰＳデータ取込手段４４は、ＧＰＳ受信機２２からの更新周期Ｔ_GPSで更新されるＧＰＳ受信機２２からのＧＰＳ速度Ｖ_GPS及びＧＰＳ位置（衛星航法測定位置）のデータを含むＧＰＳデータを取り込み、データ記憶手段５４に格納する(ステップＳ１０１、Ｓ１０２)。

ＧＰＳ受信機２２からのＧＰＳデータが更新されると、位置飛び判定手段４６により位置飛びが発生したか否かが判定される。位置飛びは、ＧＰＳ受信機２２からのＧＰＳ位置が後述の予測位置から所定範囲外にある場合に、発生したと看做すことができる。

具体的には、例えば、ＧＰＳ位置と予測位置との間の距離｜Δ｜を求め（ステップＳ１０３）、その距離｜Δ｜が規定の閾値Δ_th1（例えば、数十ｃｍ）を越えた場合に、位置飛びが発生したとする。即ち、
｜Δ｜＞Δ_th1 （１）
が成り立ったときに位置飛びが発生したものとし、それ以外は位置飛びが発生していないものとする。

位置飛びが発生していないときには（ステップＳ１０４でｎｏ）、現在位置算出手段４８によって、現在位置はＧＰＳ位置とされ、ＧＰＳ位置が出力位置データとなる（ステップＳ１０５）。

次いで、予測位置算出手段５０によって、次の更新タイミングのための移動体の予測位置が求められ、データ記憶手段５４に格納される。ここで、予測位置は、現在位置、ＧＰＳ速度Ｖ_GPSと周期Ｔ_GPSから求めることができる。即ち、
予測位置＝現在位置＋Ｖ_GPS×Ｔ_GPS （２）
とすることができる（ステップＳ１０６）。尚、（２）式におけるＧＰＳ速度Ｖ_GPSはベクトルとする。また、予測位置は、このように現在位置、ＧＰＳ速度と更新周期から求めるため、初期時には適宜、初期設定されたもの、またはＧＰＳ受信機２２から得られたＧＰＳ位置とするとよい。

一方、位置飛びが発生した場合（ステップＳ１０４でｙｅｓ）、補正位置算出手段５２によって、補正位置ベクトルが求められる（ステップＳ１０７）。この補正位置ベクトルは、位置飛びが発生した状況において、ＧＰＳ位置をそのまま出力位置データとするのではなく、更新周期Ｔ_GPSに比較して長い時間Ｔ₁（例えば、２０〜３０ｓ）をかけて漸次ＧＰＳ位置に近づくように補正するものとする。このように補正する理由は、位置飛びを発生したＧＰＳ位置データが必ずしも正しいデータであるとは限らず、また、急激に出力位置データを変化させることは好ましくない一方で、適当な時間内で出力位置データをＧＰＳ位置に収束させる必要があるからである。

具体的には、補正位置ベクトルの大きさは、予測位置とＧＰＳ位置との距離｜Δ｜を更新周期よりも長い第１時定数Ｔ₁の時間をかけて０にすると想定した場合の１回の更新タイミング当たり分とし、
補正位置ベクトル＝（Δ／Ｔ₁）×Ｔ_GPS （３）
とすることができる。尚、（３）式におけるΔはベクトルとする。

次いで、現在位置算出手段４８により、現在位置が予測位置と補正位置ベクトルとから求められる。即ち、予測位置から補正位置ベクトル分だけ移動させたものが、出力位置データとなる（ステップＳ１０８）。

以降のいずれかの更新タイミングで、ＧＰＳ位置と予測位置との距離｜Δ｜が規定の閾値Δ_th1以内になったときに（ステップＳ１０４でｎｏ）、現在位置算出手段４８によって、現在位置はＧＰＳ位置とされるので、ＧＰＳ位置に収束させることができる。

以上のような処理を行った場合の出力位置データの変遷を図４に示す。図中において、（ｎ）は、更新タイミングを表している。更新タイミングｎ−１までは位置飛びが発生していないために、ＧＰＳ位置が現在位置即ち出力位置となっているが、更新タイミングｎで位置飛びが発生すると、補正位置ベクトル分だけ、予測位置からＧＰＳ位置に近づいた位置を現在位置として、漸次、ＧＰＳ位置に近づけており（更新タイミングｎ＋１）、そして、更新タイミングｎ＋２において、位置飛びが解消されたために、ＧＰＳ位置が現在位置即ち出力位置となる。

このように、位置飛びが発生したときに、更新周期Ｔ_GPSよりも長い時間Ｔ₁をかけて漸次補正していくために、突発的なＧＰＳ位置データの変化にすぐに追随することなく、一方で、除々にＧＰＳ位置データに出力位置データを近づけていくために、出力位置データを円滑に変化させることができ、システムを安定的に動作させることができる。また、任意には、位置飛び判定手段４６は、（１）式が連続して２回以上の更新タイミングに亘って成り立ったときに、位置飛びと判定するようにして、出力位置データをより慎重に変化させるようにすることも可能である。

尚、この例では、ＧＰＳ速度を用いて予測位置を求めていたが、これに限らず、ＧＰＳ速度の代わりにその他の車速センサといった速度センサを用いて予測位置を求めることも可能である。

（第２実施形態）
図５〜図９は、本発明の第２実施形態を表している。前実施形態と同一／同様の部材は同一の符号を付し、その詳細説明を省略する。

衛星航法システムによって更新されるデータの更新周期は、通常比較的長いため（例えば、２００ｍｓ程度）、移動体の瞬間的な変化に対して衛星航法システムによって得られるデータだけで現在位置データを出力すると、遅れが生じてしまうおそれがある。

そこで、この実施形態ではより更新周期が短い（例えば、１０ｍｓ程度）慣性計測装置２４を用いて、ＧＰＳ受信機２２から得られるＧＰＳ位置を補間する。

このため、本実施形態による位置出力装置を用いた位置測定システムは、慣性航法システムを構成する慣性計測装置２４を備える。また、図６に示すように、位置出力装置２６は、慣性データ取込手段４２としても機能する。

慣性計測装置２４は、図７に示すように、３軸まわりの角速度をそれぞれ検出するジャイロ３０Ｘ、３０Ｙ、３０Ｚ、３軸方向の加速度をそれぞれ検出する加速度計３２Ｘ、３２Ｙ、３２Ｚ、及び磁気方位角を検出する磁気方位センサ３４のセンサと、演算手段３６とを備える。

演算手段３６は、一定の更新周期（この周期をＴ_INSとする）で、ジャイロ３０Ｘ、３０Ｙ、３０Ｚから出力されるジャイロ信号、加速度計３２Ｘ、３２Ｙ、３２Ｚから出力される加速度信号、及び磁気方位センサ３４から出力される磁気方位信号が入力されて、センサが取り付けられた移動体の座標系（ボディ座標系という）からＸ軸方向が北方向で且つＺ軸方向が重力方向に一致している座標系（ローカル座標系という）へ変換する座標変換行列を演算し、座標変換を行って加速度を積分することで速度（慣性速度Ｖ_INSと称する）を求め、さらに速度を積分することにより位置（慣性位置と称する）を求めて、出力している。

この演算手段３６において、座標変換行例を演算する際に、例えば、特許第４６１５２８７号に開示されるように、加速度計３２により検出された重力加速度及びＧＰＳ受信機２２からの移動体のＧＰＳ速度Ｖ_GPSを用いて運動加速度の影響を除去してレベル誤差を修正し、且つ、ＧＰＳ受信機２２からのＧＰＳ方位及び磁気方位センサ３４からの磁気方位を用いて方位誤差を修正することで、各誤差を除去することができる。

次に、位置出力装置２６の各手段による処理を、図８を参照しながら説明するが、主として図３と異なる点を説明する。

この例では、位置飛びが発生したか否かを表す位置飛び発生フラグＦｌｇを導入しており、このフラグの初期値が０に設定される（ステップＳ２０１）。ここで、位置飛び発生フラグＦｌｇは１が位置飛び発生有り、０が位置飛び発生無し、をそれぞれ表すものとする。

そして、慣性データ取込手段４２は、慣性計測装置２４からのデータの更新周期Ｔ_INSで更新される慣性速度Ｖ_INSを含む慣性データを取り込み、データ記憶手段５４に格納する(ステップＳ２０２、Ｓ２０３)。

また、ＧＰＳデータ取込手段４４は、ＧＰＳ受信機２２からの更新周期Ｔ_GPSで更新されるＧＰＳ受信機２２からのＧＰＳ速度Ｖ_GPS、ＧＰＳ方位及びＧＰＳ位置を含むＧＰＳデータを取り込み、データ記憶手段５４に格納する(ステップＳ２０４、Ｓ２０５)。取り込まれたＧＰＳ速度Ｖ_GPSは、前述の通り、慣性計測装置２４の修正に用いられるために慣性計測装置２４に取り込まれる（ステップＳ２０６）。

ＧＰＳ受信機２２からのデータが更新されておらず、位置飛びが発生していない場合は（ステップＳ２０４及びステップＳ２０７がいずれもｎｏ）、ステップＳ２０８に進み、現在位置算出手段４８によって、現在位置は予測位置算出手段５０で既に求めていた予測位置とされ、予測位置が出力位置データとなる（ステップＳ２０８）。

次いで、予測位置算出手段５０によって、次の更新タイミングのための移動体の予測位置が求められ、データ記憶手段５４に格納される。ここで、予測位置は、現在位置から慣性速度Ｖ_INSと更新周期Ｔ_INSから求めることができる。即ち、
予測位置＝現在位置＋Ｖ_INS×Ｔ_INS （４）
とすることができる（ステップＳ２０９）。尚、（４）式における慣性速度Ｖ_INSはベクトルとする。

但し、予測位置算出手段５０は、演算手段３６で各更新タイミングで得られる慣性位置を取り込み、この慣性位置を予測位置とするか、または慣性位置を用いて予測位置を求めることとしてもよい。

ＧＰＳ受信機２２からのＧＰＳデータが更新された場合には、位置飛び判定手段４６により位置飛びが発生したか否かが判定され（ステップＳ２１１、Ｓ２１２）、位置飛びが発生しておらず、且つ、前回の更新タイミングで位置飛びが発生していない場合、即ち、位置飛び発生フラグがＦｌｇ＝０である場合には、現在位置はＧＰＳ位置とされ、ＧＰＳ位置が出力位置データとなり（ステップＳ２１３）、位置飛び発生フラグがＦｌｇ＝０とされ（ステップＳ２１４）、予測位置が（４）式から求められる（ステップ２０９）。

以降の処理では、位置飛びが発生しない条件下で現在位置算出手段４８によって現在位置は、ＧＰＳデータが更新されると、ＧＰＳ位置にされ（ステップＳ２１３）、ＧＰＳデータが更新されておらず慣性データが更新されたタイミングにおいては、予測位置にされる（ステップＳ２０８）。即ち、原則的にはＧＰＳ位置が現在位置として出力されるが、それ以外の処理タイミングにおいては、慣性計測装置２４で得られるデータに基づき、現在位置の補間がなされることとなる。

一方、位置飛びが発生した場合には、補正位置算出手段５２によって補正位置（補正位置ベクトル）が求められる（ステップＳ２１５）。

具体的には、補正位置ベクトルは、
補正位置ベクトル＝（Δ／Ｔ₁）×Ｔ_INS （５）
とすることができる。尚、（５）式におけるΔはベクトルとする。

そして、位置飛び発生フラグＦｌｇ＝１とされて(ステップＳ２１６)、現在位置算出手段４８により、現在位置が補正位置ベクトルと予測位置から求められる(ステップＳ２１０)。

また、位置飛びが発生した後、ＧＰＳデータが更新されておらず慣性データが更新されたタイミングにおいては、予測位置を上記（５）式で求めた補正位置ベクトルで補正した位置が現在位置として出力されて（ステップＳ２１０）、補間がなされる。補正位置ベクトルは、ＧＰＳデータが更新されるまでは一定である。

以上のような処理を行った場合の出力位置データの変遷を図９に示す。図中において、（ｎ）は、更新タイミングを表している。更新タイミングｎ−２、ｎ−１までは位置飛びが発生されておらずＧＰＳデータも更新されていないために、予測位置が現在位置即ち出力位置となっているが、更新タイミングｎで更新されたＧＰＳデータにより位置飛びが発生したと判定されると、補正位置ベクトル分だけ、予測位置からＧＰＳ位置に近づいた位置を現在位置として、漸次、ＧＰＳ位置に近づけており（更新タイミングｎ＋１）、そして、更新タイミングｎ＋ｍにおいて、位置飛びが解消されると、ＧＰＳ位置が現在位置即ち出力位置となる。

この第２実施形態によれば、第１実施形態の作用・効果に加えて、ＧＰＳデータの補間を行うことができ、移動体の位置の急激な変化に追従することができる。

（第３実施形態）
図１０〜図１２は、本発明の第３実施形態を表している。前実施形態と同一／同様の部材は同一の符号を付し、その詳細説明を省略する。

この実施形態では、前実施形態に対してさらに現在位置を円滑に出力できるようにしたものであり、位置出力装置２６は、さらに、位置戻り判定手段５６と、第２補正位置算出手段５８と、して機能する。

前実施形態では、位置飛びが発生した後、補正によりまたはＧＰＳ位置が元に戻り、位置飛びが解消され、ＧＰＳ位置と予測位置との間の距離｜Δ｜が閾値Δ_th1以内となると、現在位置算出手段４８はＧＰＳ位置を現在位置として出力するようにしており、この際に出力位置データが急に変化するおそれがある。

そこで、第３実施形態では、位置飛びが発生した後に、位置飛びが解消された場合でも、ＧＰＳ位置と予測位置との距離｜Δ｜がより小さくなるまでは、補正を継続することとする。このとき好ましくは、ＧＰＳ位置により早く収束させるような補正を行うとよい。

具体的には、位置飛び判定手段４６が位置飛びが発生していないと判定した場合、即ち、（１）式が成立しないと判定した場合（ステップＳ２１２でｎｏ）、位置戻り判定手段５６が、位置飛び発生フラグｆｌｇが１であり且つＧＰＳ位置と予測位置との間の距離｜Δ｜が、規定の第２閾値Δ_th2（＜Δ_th1、例えば、数ｃｍ）より大きいか否かを判定し（ステップＳ２２０）、判定結果がｙｅｓである場合には、第２補正位置算出手段５８が、補正位置ベクトル（第２補正位置ベクトル）を求める(ステップＳ２２２)。

この場合の補正位置ベクトルは、予測位置とＧＰＳ位置との距離｜Δ｜を更新周期よりも長いけれども第１時定数Ｔ₁よりも短い第２時定数Ｔ₂（＜Ｔ₁）の時間をかけて０にすると想定した場合の１回の更新タイミング当たり分とし、具体的には、補正位置ベクトルは、
補正位置ベクトル＝（Δ／Ｔ₂）×Ｔ_INS （６）
とすることができる。尚、（６）式におけるΔはベクトルとする。

そして、以降の更新タイミングにおいて、ＧＰＳ位置と予測位置との間の距離｜Δ｜が、第２閾値Δ_th2以内になったときに（ステップＳ２２０でｎｏ）、現在位置算出手段４８によって、現在位置はＧＰＳ位置とされ、ＧＰＳ位置が出力位置データとなる（ステップＳ２１３）。そして、位置飛び発生フラグｆｌｇを０に戻す(ステップＳ２１４)。

以上のような処理を行った場合の出力位置データの変遷を図１２に示す。図中において、（ｎ）は、更新タイミングを表している。位置飛び発生後に、更新タイミングｎ＋ｍにおいて、ＧＰＳ位置と予測位置との距離が｜Δ_th1｜より小さいが｜Δ_th2｜より大きい場合には、それまでの補正位置ベクトルよりも大きな補正位置ベクトルで補正することにより、出力位置データをより円滑にＧＰＳ位置へと収束させることができる。

尚、この位置戻り判定手段５６及び第２補正位置算出手段５８は、慣性データを用いない第１実施形態においても同様に適用可能である。

２６ 位置出力装置
４２ 慣性データ取込手段（慣性航法データ取込手段）
４４ ＧＰＳデータ取込手段（衛星航法データ取込手段）
４６ 位置飛び判定手段
４８ 現在位置算出手段
５０ 予測位置算出手段
５２ 補正位置算出手段
５６ 位置戻り判定手段
５８ 第２補正位置算出手段
Ｔ₁ 第１時定数
Ｔ₂ 第２時定数
Ｔ_GPS、Ｔ_INS 更新周期

Claims (6)

1. 衛星航法システムによって得られる衛星航法測定位置のデータを用いて、対象の現在位置データを出力する位置出力装置であって、
   衛星航法システムで更新される衛星航法測定位置のデータを取り込む衛星航法データ取込手段と、
   現在位置を更新タイミングで順次算出して現在位置データとして出力する現在位置算出手段と、
   前の更新タイミングにおける現在位置から対象の予測位置を求める予測位置算出手段と、
   前記予測位置と前記衛星航法測定位置との位置差から位置飛びが発生したかを判定する位置飛び判定手段と、
   位置飛びが発生したと判定された場合に、現在位置を補正するための補正位置ベクトルを算出する補正位置算出手段と、
   位置飛びが発生した後に位置飛びが発生していないと判定された場合に、前記予測位置と前記衛星航法測定位置との間の距離が設定距離より大きいかを判定する位置戻り判定手段と、
   前記予測位置と前記衛星航法測定位置との間の距離が設定距離より大きいと判定された場合に、現在位置を補正するための第２補正位置ベクトルを算出する第２補正位置算出手段と、
   を備え、
   前記補正位置算出手段は、前記予測位置と前記衛星航法測定位置との位置差を、更新周期よりも長い時間をかけて解消するとした場合の１回の更新タイミング当たりの補正位置ベクトルを算出しており、
   前記第２補正位置算出手段は、前記予測位置と前記衛星航法測定位置との位置差を、更新周期よりも長い時間であるが、前記補正位置算出手段における算出で用いられる時間よりも短い時間をかけて解消するとした場合の１回の更新タイミング当たりの第２補正位置ベクトルを算出しており、
   前記現在位置算出手段は、位置飛びが発生したと判定された場合に、前記予測位置を前記補正位置ベクトルによって補正したものを現在位置とするとともに、位置飛びが発生した後に位置飛びが発生していないと判定され、かつ前記予測位置と前記衛星航法測定位置との間の距離が設定距離より大きいと判定された場合に、前記予測位置を前記第２補正位置ベクトルによって補正したものを現在位置とすることを特徴とする衛星航法システムを用いた位置出力装置。
2. 前記補正位置ベクトルの大きさは、前記予測位置と前記衛星航法測定位置との距離に更新周期を第１時定数で除算した値を乗算したものであることを特徴とする請求項１記載の衛星航法システムを用いた位置出力装置。
3. 前記第２補正位置ベクトルの大きさは、前記予測位置と前記衛星航法測定位置との距離に更新周期を第２時定数で除算した値を乗算したものとし、第２時定数＜第１時定数であることを特徴とする請求項２記載の衛星航法システムを用いた位置出力装置。
4. 慣性航法システムで更新される慣性航法測定速度のデータを取り込む慣性航法データ取込手段と、をさらに備え、
   前記予測位置算出手段は、前記慣性航法測定速度を用いて予測位置を算出することを特徴とする請求項１ないし３のいずれか１項に記載の衛星航法システムを用いた位置出力装置。
5. 位置飛びが発生していないと判定され且つ前記衛星航法測定位置データが更新されていない間は、前記現在位置算出手段は、前記予測位置を前記現在位置とすることを特徴とする請求項１ないし４のいずれか１項に記載の衛星航法システムを用いた位置出力装置。
6. 位置飛びが発生していないと判定され且つ前記衛星航法測定位置データが更新された場合は、前記現在位置算出手段は、前記衛星航法測定位置を前記現在位置とすることを特徴とする請求項１ないし５のいずれか１項に記載の衛星航法システムを用いた位置出力装置。

Images