JP6413946B2 - 測位装置 - Google Patents

Description

本発明は、航法衛星が送信する航法信号を受信し、受信した航法信号を用いて測位する測位装置に関する。

ＧＰＳ（Global Positioning System）など、航法衛星を備えた衛星測位システムが広く知られている。航法衛星は、航法信号を送信し、航法信号を受信する受信機は、受信した航法信号に含まれている情報から種々の演算を行なって、測位、すなわち、現在位置を決定する。

衛星測位システムは、固定点の座標を測量する場合に用いられることもあるが、車両などの移動体の測位にも広く用いられる。

また、移動体の測位技術として、慣性航法も広く知られている。慣性航法は、加速度センサ、ジャイロセンサなどの慣性センサが検出する信号を用いて現在位置を逐次更新する方法である。

さらに、航法信号を用いた測位と慣性航法とを組み合わせて現在位置を逐次決定する測位装置も知られている（たとえば特許文献１）。具体的には、航法信号を用いた測位を周期的に行う。加えて、航法信号に基づいて定まる座標（以下、衛星測位座標）が決定されてから、次に衛星測位座標が決定されるまでの間、慣性航法により移動量を逐次算出する。この移動量を最新の衛星測位座標に加算することで、現在位置を逐次決定する。

特許第３８４８７１２号公報

衛星測位座標は、マルチパスなど、航法信号を受信する受信環境により精度が低下することがある。慣性航法により算出した移動量を加算する基準点として、精度が低い衛星測位座標をそのまま用いて現在位置を決定すると、実際には、移動体は滑らかに移動しているにも関わらず、現在位置の軌跡が段状になってしまうという問題がある。

本発明は、この事情に基づいて成されたものであり、その目的とするところは、衛星測位座標の精度が低下しても、移動体の位置の軌跡が段状に変化してしまうことを抑制できる測位装置を提供することにある。

上記目的は独立請求項に記載の特徴の組み合わせにより達成され、また、下位請求項は、発明の更なる有利な具体例を規定する。特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、本発明の技術的範囲を限定するものではない。

上記目的を達成するための本発明は、移動体で用いられ、移動体の位置を逐次決定する測位装置であって、航法信号受信部（１０）が受信した航法信号に基づいて定まる座標である衛星測位座標を演算する衛星測位部（４１）と、慣性センサが検出する信号を取得して、慣性航法により、航法信号を受信した時点から、慣性センサから信号を取得した時点までの移動体の移動量を逐次算出する移動量算出部（４２）と、航法信号を受信した時点以降の仮想基準点の座標を逐次決定する仮想基準点決定部（４３）と、移動体が、仮想基準点決定部が決定した仮想基準点から、移動量算出部が算出した移動量だけ移動したとして、移動体の位置を逐次決定する位置決定部（４４）と、衛星測位部が演算した衛星測位座標の信頼度を算出する測位座標信頼度算出部（４５）と、移動量算出部が算出した移動量の信頼度を決定する移動量信頼度決定部（４６）とを備え、仮想基準点決定部は、位置決定部が決定した、航法信号を受信した時点における移動体の位置と、衛星測位部が演算した衛星測位座標とに、それぞれ重み係数を乗じて定まる座標を仮想基準点の座標とし、かつ、衛星測位座標に乗じる重み係数を段階的に大きくでき、移動体の位置に乗じる重み係数を、航法信号を受信した時点以降に移動量算出部が移動量を算出した回数に応じて値が減少する減少項を備える第１係数計算式により計算し、衛星測位座標に乗じる重み係数を、航法信号を受信した時点以降に移動量算出部が移動量を算出した回数に応じて値が増加する増加項を備える第２係数計算式により計算し、第１係数計算式および第２係数計算式は、いずれも、移動量算出部が移動量を算出した回数によらず一定であるオフセット項を備え、測位座標信頼度算出部が算出した衛星測位座標の信頼度が高いほど、オフセット項の値を大きくし、減少項は、航法信号を受信した時点以降に移動量算出部が移動量を算出した回数に、移行速度項と、マイナスを乗じる項であり、増加項は、航法信号を受信した時点以降に移動量算出部が移動量を算出した回数に、移行速度項を乗じる項であり、移動量信頼度決定部が決定した移動量の信頼度の高いほど、移行速度項の値を小さくすることを特徴とする。

本発明では、衛星測位座標が得られても、その衛星測位座標を基準点としてそのまま用いるのではなく、移動量の基点となる基準点として仮想基準点を決定する。仮想基準点は、位置決定部が決定した、航法信号を受信した時点における移動体の位置と、衛星測位部が演算した衛星測位座標とに、それぞれ重み係数を乗じて定まる座標である。しかも、衛星測位座標に乗じる重み係数を段階的に大きくできる。

衛星測位座標に乗じる重み係数を段階的に大きくすれば、衛星測位座標の精度が低いとしても、仮想基準点は段階的に衛星測位座標に近づくことになる。仮想基準点が段階的に衛星測位座標に近づくので、仮想基準点と移動量算出部が算出した移動量とから決定する移動体の位置の軌跡が段状に変化してしまうことを抑制できる。

実施形態の測位装置１の構成図である。 図１の制御部４０が実行する処理を示すフローチャートである。 図１の制御部４０が、図２の処理と並列に実行する処理を示すフローチャートである。 図３のステップＳ３４の処理を詳しく示すフローチャートである。 図３のステップＳ４０の処理を詳しく示すフローチャートである。 図３のステップＳ４２の処理を詳しく示すフローチャートである。 実施形態の測位装置１が決定する自車両の位置Ｐを説明する図である。

以下、本発明の実施形態を図面に基づいて説明する。図１に示す測位装置１は、航法信号受信部１０、加速度センサ２０、ヨーレートセンサ３０、制御部４０を備えており、移動体である図示しない車両に搭載される。測位装置１が搭載されている車両を、以下、自車両という。

＜構成の概要＞
航法信号受信部１０は、衛星測位システムの一つであるＧＰＳが備える航法衛星であるＧＰＳ衛星が送信するＧＰＳ電波を受信する受信機である。ＧＰＳ電波は、搬送波に航法信号が重畳されたものである。航法信号受信部１０は、受信したＧＰＳ電波を復調して航法信号を取り出し、制御部４０に出力する。

ＧＰＳ電波から航法信号を取り出すために、コード復調等が必要となるため、航法信号は一定周期で制御部４０に出力される。一定周期は、たとえば、２００ミリ秒〜４００ミリ秒の間である。また、航法信号受信部１０は、航法信号とともに、搬送波、受信信号強度、航法信号を受信した時刻も制御部４０に出力する。

周知のように、ＧＰＳ衛星は複数存在している。航法信号受信部１０は、ＧＰＳ電波から復調できる全ての航法信号を制御部４０に出力する。航法信号には、ＧＰＳ衛星の衛星番号、ＧＰＳ衛星の軌道情報であるエフェメリス、ＧＰＳ衛星が電波を送信した時刻などが含まれている。

加速度センサ２０とヨーレートセンサ３０は慣性センサである。加速度センサ２０は、３軸の加速度センサであり、ｚ軸が車両の上下方向に平行になり、ｘ軸が車両の幅方向と平行になり、ｙ軸が車両の前後方向に平行になるように、加速度センサ２０は向きが固定されている。なお、３軸の加速度センサに代えて、ｘ軸、ｙ軸の２軸の加速度を検出する加速度センサを用いてもよい。加速度センサ２０は各軸の加速度の検出値を制御部４０に送る。

ヨーレートセンサ３０は、このヨーレートセンサ３０を通り、車両の垂直軸周りの回転角速度、すなわち、ヨーレートを検出する。そして、検出したヨーレートを制御部４０に供給する。なお、車両の垂直軸とは、車両の車室床に対して垂直な軸であり、車両が水平な地面に位置している時の鉛直軸と平行になる軸である。

制御部４０は、図示しないＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータであり、ＣＰＵは、ＲＡＭの一時記憶機能を利用しつつＲＯＭなどの非遷移的実体的記録媒体に記憶されているプログラムを実行する。これにより、制御部４０は、衛星測位部４１、移動量算出部４２、仮想基準点決定部４３、位置決定部４４、測位座標信頼度算出部４５、移動量信頼度決定部４６として機能する。これらの機能を実行すると、非遷移的実体的記録媒体に記憶されているプログラムに対応する方法が実行される。これらの各部の機能は、図２、図３を用いて説明する。

＜制御部４０が実行する処理＞
次に、制御部４０が実行する処理を説明する。制御部４０は、図２に示す処理と、図３に示す処理とを並列して実行する。たとえば、図２の処理を実行中に、割り込み処理により図３の処理を実行する。まず、図２に示す処理から説明する。制御部４０は、図２に示す処理を周期的に実行する。図２に示す処理全体、すなわち、ステップＳ２〜Ｓ１２は衛星測位部４１に相当する。

ステップＳ２では、航法信号受信部１０から航法信号、搬送波、受信信号強度、航法信号を受信した時刻を取得したか否かを判断する。ステップＳ２の判断がＮＯであれば、図２の処理を終了する。一方、ステップＳ２の判断がＹＥＳである場合にはステップＳ４に進む。

ステップＳ４では、航法信号受信部１０から取得した航法信号および搬送波から、ＧＰＳ衛星ｉに関する情報である、ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）、擬似距離ρ_ｉ、ドップラーシフト量Ｄ_ｉを算出する。なお、ｉはＧＰＳ衛星の番号である。

各ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）は、各ＧＰＳ衛星ｉのエフェメリスおよびＧＰＳ電波を送信した時刻に基づいて算出する。擬似距離ρ_ｉは、ＧＰＳ衛星ｉがＧＰＳ電波を送信した時刻と、航法信号受信部１０がＧＰＳ電波を受信した時刻との時刻差すなわち電波伝播時間に、光速を乗じることで算出する。

なお、ＧＰＳ衛星ｉは連続的にＧＰＳ電波を送信し、航法信号受信部１０は連続的にＧＰＳ電波を受信する。ＧＰＳ衛星ｉがＧＰＳ電波を送信した時刻および航法信号受信部１０がＧＰＳ電波を受信した時刻は、航法信号において、たとえば信号の先頭など、所定部分を送信した時刻およびその所定部分を受信した時刻を意味する。

ドップラーシフト量Ｄ_ｉは、ＧＰＳ衛星ｉが送信した電波の搬送波の周波数と、受信したＧＰＳ電波の搬送波の周波数の周波数差である。ＧＰＳ衛星が送信する電波の搬送波周波数は予め定まっており、この周波数は、制御部４０が備える図示しない記憶部など、所定の記憶部に予め記憶されている。したがって、ステップＳ４では、記憶部からＧＰＳ電波の搬送波の周波数を取得し、この周波数と、航法信号受信部１０から取得した搬送波の周波数から、ドップラーシフト量Ｄ_ｉを算出する。なお、航法信号受信部１０から取得した搬送波の周波数は、公知の周波数解析手法、たとえば、高速フーリエ変換により決定する。

ステップＳ６では、航法信号の信号品質がよいか否かを判定する。信号品質の判定は、取得したすべての航法信号に対して行う。信号品質の判定には、公知の種々の基準を用いることができる。

たとえば、（判定条件１）Ｓ／Ｎが所定値以上であること、（判定条件２）擬似距離ρ_ｉの残差が判定基準距離以下であること、（判定条件３）仰角θ_ｉが判定基準角以上であること、（判定条件４）判定条件１〜３の組み合わせ、などにより信号品質がよいか否かを判定する。

上記判定条件２における擬似距離ρ_ｉの残差とは、ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）と自車両の衛星測位座標Ｂ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）との間の距離と、擬似距離ρ_ｉとの差である。この残差を算出するためには、自車両の衛星測位座標Ｂ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）が必要である。自車両の衛星測位座標Ｂは、後で説明するステップＳ１０において衛星測位座標Ｂを算出する方法と同じ方法で算出する。したがって、擬似距離ρ_ｉの残差を算出する場合には、先に、衛星測位座標Ｂを算出することになる。

擬似距離ρ_ｉの残差が大きい場合には、マルチパス等の影響が考えられるので、信号品質が良くないと判定する。判定条件１は、Ｓ／Ｎが所定値以上であれば信号品質はよいと判定する。判定条件３は、仰角θ_ｉが判定基準角以上であれば信号品質はよいと判定する。

ステップＳ８では、ステップＳ６で信号品質がよいと判定した航法信号の数が４以上であるか否かを判断する。４以上である場合にはステップＳ１０へ進む。

ステップＳ１０では、ステップＳ４で算出した擬似距離ρ_ｉ、ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）から、現在の自車両の衛星測位座標（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を算出する。この座標を、衛星測位座標Ｂという。

衛星測位座標Ｂは以下のようにして算出する。ＧＰＳ衛星ｉまでの真の距離ｒ_ｉは（１）式で表される。一方、擬似距離ρ_ｉは（２）式で表される。なお、（２）式において、ｓは時計誤差による距離誤差である。

上記（１）式、（２）式より、４つ以上のＧＰＳ衛星の擬似距離ρ_ｉから得られる以下の（３）式の連立方程式を解くことによって、衛星測位座標Ｂ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）が算出できる。

続く、ステップＳ１２ではｋに１を加える。なお、このｋの初期値は０である。つまり、このｋは、衛星測位座標Ｂを算出した回数を意味する。

次に、図３を説明する。図３に示すフローチャートは、センサ値を取得する周期で繰り返し実行する。センサ値を取得する周期は、航法信号受信部１０が航法信号を出力する周期よりも短く、たとえば、数十ミリ秒である。なお、特に明記している場合を除き、各パラメータは、最新の時刻における値を意味する。また、最新の時刻をｔ＝０とし、時刻を負の数字で表している場合には、過去の時刻であることを意味する。

ステップＳ２０では、加速度センサ２０、ヨーレートセンサ３０の検出値を取得し、ＲＡＭなどの記憶部に記憶する。ステップＳ２２では、ステップＳ２０で取得した加速度センサ２０の検出値から、車両の進行方向加速度を決定する。前述したように、加速度センサ２０は車両に対する向きが予め定められた向きに固定されており、ｙ軸が車両進行方向を向いている。したがって、加速度センサ２０が検出したｙ軸の検出値を、進行方向加速度として抽出する。

ステップＳ２４では、ステップＳ２０で取得したヨーレートセンサ３０の検出値から求められる相対方位角変化量を、これまでの相対方位角θ^{ｔ（−１）} _ｇｙｒｏに加算して相対方位角θ^ｔ _ｇｙｒｏを更新する。相対方位角θ^ｔ _ｇｙｒｏは、基準時点における車両の進行方位に対する時刻ｔの相対方位角である。また、このステップＳ２４では、ステップＳ２２で決定した進行方向加速度をこれまでの加速度積算値ΔＶ^{ｔ（−１）} _Ｇに加算して、加速度積算値ΔＶ^ｔ _Ｇも更新する。

ステップＳ２６では、自車両が停止しているかどうかを判断する。この判断に、本実施形態では、ステップＳ２０で取得した加速度センサ２０のｚ軸の検出値を用いる。加速度センサ２０のｚ軸の検出値が停止判定値以下であれば、車両が停止していると判定する。車両が走行している場合には、多少の上下振動があるので、ｚ軸の検出値により停止判定を行うことができるのである。なお、ｚ軸の検出値に代えて、ｙ軸の検出値や、ｚ軸の検出値の変化量、ｙ軸の検出値の変化量を用いてもよい。停止中と判断した場合にはステップＳ２８へ進む。

ステップＳ２８では、速度ベクトルを（０、０、０）とする。もちろん、速度Ｖも０となる。

ステップＳ３０では、速度初期値Ｖ^０を最新の加速度積算値−ΔＶ^ｔ _Ｇとする。ステップＳ３０を実行したらステップＳ４２の位置更新処理を実行する。ステップＳ４２の位置更新処理は後述する。

ステップＳ２６において移動中であると判断した場合にはステップＳ３２へ進む。ステップＳ３２では、前回、このステップＳ３２を実行してから、今回、このステップＳ３２を実行するまでの間に、衛星測位座標Ｂが算出されたか否かを判断する。

衛星測位座標Ｂは、ステップＳ１０で算出している。ただし、衛星測位座標Ｂを算出するには、航法信号を取得する必要があり、航法信号を取得する周期は、センサ値を取得する周期よりも長い。また、航法信号を取得しても、品質のよい航法信号を４つ以上取得していなければ衛星測位座標Ｂは算出されない。また、航法信号を取得した後、演算も必要であり、この演算には時間が必要となる。したがって、ステップＳ３２の判断はＮＯになる場合もある。

ステップＳ３２の判断がＹＥＳになった場合には、ステップＳ３４に進む。ステップＳ３４では、ドップラー速度を算出する。ドップラー速度とは、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）、またはその速度ベクトルの大きさを意味する。速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は、ドップラーシフト量Ｄを用いて算出することから、ここでは、ドップラー速度と称している。

ステップＳ３４の詳細処理は図４に示す。図４において、ステップＳ３４２では、ステップＳ１０で算出した衛星測位座標Ｂ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）、および、ステップＳ４で算出したＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）から、各ＧＰＳ衛星ｉの方向Ｒ_ｉ（θ_ｉ、φ_ｉ）を算出する。各ＧＰＳ衛星ｉの方向Ｒ_ｉは、自車両からＧＰＳ衛星ｉを見たときの、水平方向に対する仰角θ_ｉ、北方向に対する方位角φ_ｉで表すものとする。

ステップＳ３４４では、ステップＳ４で算出した各ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）の時系列データから、ケプラーの方程式の微分を用いる公知の方法で、各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓ_ｉ、Ｖｙｓ_ｉ、Ｖｚｓ_ｉ）を算出する。

ステップＳ３４６では、ステップＳ３４２で算出した各ＧＰＳ衛星ｉの方向Ｒ_ｉと、ステップＳ３４４で算出した各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓ_ｉ、Ｖｙｓ_ｉ、Ｖｚｓ_ｉ）から、Ｖｓａｔ_ｉ＝Ｒ_ｉ［Ｖｘｓ_ｉ，Ｖｙｓ_ｉ，Ｖｚｓ_ｉ］^Ｔにより、自車両方向のＧＰＳ衛星ｉの速度Ｖｓａｔ_ｉを求める。なお、Ｔは行列の転置を意味する。

ステップＳ３４８では、（４）式に示した連立方程式を４つ以上立式し、その連立方程式を解く。これにより、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）および、航法信号受信部１０が備える時計のクロックドリフトＣｂｖ^ｔを求めることができる。

この（４）式を説明する。衛星方向速度Ｖｓ_ｉと、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）との関係は、以下の（５）式で表される。

また、ＧＰＳ衛星ｉに対する車両の相対速度Ｖｒ_ｉについて下記（６）式が成立する。

なお、式６において、Ｖｒ_ｉはＧＰＳ衛星ｉに対する車両の相対速度、Ｄ_ｉはドップラーシフト量、Ｃは光速、ＦはＧＰＳ衛星が送信する電波の搬送波の周波数である。

さらに、（６）式より、（７）式が導出できる。

各ＧＰＳ衛星ｉについて、（７）式におけるＶｓ_ｉに（５）式の左辺を代入すれば、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）およびＣｂｖを未知数とした（４）式で表される連立方程式が得られる。

ＧＰＳ電波を受信したＧＰＳ衛星が４個以上である場合に、上記（４）式の連立方程式を解くことができる。ただし、ＧＰＳ電波を受信していても、信号品質がよいと判定できないＧＰＳ電波は除外する。よって、ステップＳ８で航法信号の信号品質がよいと判定したＧＰＳ電波を４個以上のＧＰＳ衛星から受信した場合に、（４）式から自車両の速度ベクトルを算出する。

説明を図３に戻す。ステップＳ３６では、ステップＳ３４８で算出した自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）とステップＳ２４で更新した最新の加速度積算値ΔＶ_Ｇから、（８）式により、速度初期値Ｖ^０を決定する。

ステップＳ３６を実行した場合にも、ステップＳ４２の位置更新処理を実行する。ステップＳ４２を説明する前に、ステップＳ３８以下を説明する。

ステップＳ３２において衛星測位座標Ｂは算出されていないと判断した場合にはステップＳ３８へ進む。ステップＳ３８では、初期値を更新することができるか否かを判断する。この判断は、具体的には、速度初期値Ｖ^０を更新してからの航法信号を用いて、（９）式に示す速度推定式を４つ以上、立式できるか否かを判断するものである。

（９）式において、ｔは時刻、Ｖ^０は初期時刻における車速である速度初期値、ΔＶ_Ｇは初期時刻以降の加速度積算値、θ^０は初期時刻における車両の進行方向の方位角（以下、方位初期値）、θ_ｇｙｒｏは、車両の進行方向の方位角の積算値すなわち相対方位角、Ｃｂｖ^０は初期時刻におけるクロックドリフト、Ａはクロックドリフトの時間変化の傾き、Ｇｘ、Ｇｙは、車両からＧＰＳ衛星ｉへの視線ベクトルのｘ成分、ｙ成分である。この（９）式について説明する。

本実施形態では、すでに説明したステップＳ３４でも自車両の速度を算出することができる。しかし、ステップＳ３４で自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出するには、衛星測位座標Ｂを必要とする。したがって、信号品質がよいＧＰＳ電波を４個以上のＧＰＳ衛星から受信できなければ、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出することができない。

そのため、たとえば、高層ビルの多い都市部では、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出できない時間が長く続くこともある。また、信号品質がよいＧＰＳ電波を４個以上受信できる環境であっても、ステップＳ３２は、２００ミリ秒〜４００ミリ秒周期の一定周期でしかＹＥＳにならない。このステップＳ３２がＹＥＳになる周期でしか、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出することができない。

これに対して、加速度センサ２０の検出値は、走行環境に依存せずに一定周期で取得でき、しかも、たとえば２０ｍｓごとなど、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）が算出できる周期よりも短い周期で取得できる。

そこで、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出してから、次に、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出するまでの間、加速度積算値ΔＶ_Ｇをもとにして速度推定を行う。

加速度センサ２０の検出値には常に誤差があり、その誤差は、広く知られているように、発散する性質を有する。したがって、加速度積算値ΔＶ_Ｇをそのまま速度とすると、誤差が時間の経過により発散する。そこで、精度の高い速度を求めた時点で、その精度の高い速度を速度初期値Ｖ^０とする。

精度の高い速度を求めた時点での加速度積算値ΔＶ_Ｇを、精度の高い速度から引いた値を速度初期値Ｖ^０とする。このようにして求める速度初期値Ｖ^０は、加速度積算値ΔＶ_Ｇを真の速度とみなす精度の高い速度にするためのオフセット分を意味する。よって、ΔＶ_Ｇ−Ｖ^０により推定速度Ｖ_ｅを求めることになる。

加速度積算値ΔＶ_Ｇと真の速度との誤差は、時間の経過とともに増大する。したがって、速度初期値Ｖ^０の更新周期が短いほど、加速度積算値ΔＶ_Ｇを用いた速度の推定精度が向上する。

そこで、本実施形態では、（９）式を用いて速度初期値Ｖ^０を更新する。この（９）式は、特許文献２において最終的な速度ベクトル算出式として開示されている下記（１０）式を改良した式である。（９）式は、速度初期値Ｖ^０が算出できるだけでなく、同時に方位初期値θ^０も算出できる。

（１０）式において、Ｖ_{ｗｈｅｅｌ}は車輪速センサの検出値である。（９）式と（１０）式の比較から分かるように、（９）式は、（１０）式における車輪速センサの検出値Ｖ_{ｗｈｅｅｌ}を、速度初期値Ｖ^０と加速度積算値ΔＶ_Ｇの和に置き換えた式である。

この（９）式を用いるステップＳ４０では、詳しくは図５に示す処理を実行する。図５において、ステップＳ４０２では、ステップＳ４で算出した各ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）の時系列データから、各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓ_ｉ、Ｖｙｓ_ｉ、Ｖｚｓ_ｉ）を算出する。

ステップＳ４０４では、ステップＳ４で算出したドップラーシフト量Ｄ_ｉを（１１）式に代入して、ＧＰＳ衛星ｉに対する自車両の相対速度Ｖｒ_ｉを算出する。

ステップＳ４０６では、現在の自車両の位置Ｐ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を算出する。このステップＳ４０６を実行する場合、ステップＳ３２がＮＯになっているのであるから、品質のよい航法信号を４つ以上受信していない。しかし、このステップＳ４０６で算出する位置Ｐ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）は、次のステップＳ４０８において、自車両からＧＰＳ衛星ｉへの視線ベクトル（Ｇｘ_ｉ、Ｇｙ_ｉ、Ｇｚ_ｉ）を算出するために用いる。

ＧＰＳ衛星ｉは遠方に存在するため、ＧＰＳ衛星ｉと自車両との角度を求めるために用いる現在位置は、精度が低くてもよい。したがって、品質のよい信号を４つ以上受信していなくてもよい。

そこで、たとえば、このステップＳ４０６では、品質がよいとは判断できない信号も含めて４つ以上の航法信号を用いて、ステップＳ１０と同様にして、現在の自車両の衛星測位座標Ｂを算出し、この衛星測位座標Ｂを現在の自車両の位置Ｐ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）とする。

また、擬似距離ρ_ｉを用いた位置決定以外の精度の低い方法で自車両の位置Ｐ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を決定してもよい。システム等で許容される推定精度に依存するが、自車両の位置誤差が数百ｍの範囲であれば、速度推定誤差は１ｍ／ｓｅｃ以下となり大きな問題はない。そのため、たとえば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、自車両の位置Ｐ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を決定してもよい。

また、精度が低くても良いので、後述する位置更新処理（Ｓ４２）において、前回、更新した自車両の位置Ｐを、ここでの自車両の位置Ｐ（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）としてもよい。

ステップＳ４０８では、自車両からＧＰＳ衛星ｉへの視線ベクトル（Ｇｘ_ｉ、Ｇｙ_ｉ、Ｇｚ_ｉ）を算出する。視線ベクトルのｘ成分、ｙ成分、ｚ成分は、（１２）式から算出する。

（１２）式において、ρ^ｔ _ｉは時刻ｔにおけるＧＰＳ衛星ｉの擬似距離、（Ｘ^ｔ _ｓｉ、Ｙ^ｔ _ｓｉ、Ｚ^ｔ _ｓｉ）は時刻ｔにおけるＧＰＳ衛星ｉの位置座標である。これらは、ステップＳ４で算出している。また、ステップＳ４と同様の方法で、このステップＳ４０８において算出してもよい。（Ｘ^ｔ _Ｖ、Ｙ^ｔ _Ｖ、Ｚ^ｔ _Ｖ）は時刻ｔにおける自車両の現在位置であり、ステップＳ４０６で算出している。

ステップＳ４１０では、（１３）式に、ステップＳ４０４で算出した相対速度Ｖｒ_ｉ、ステップＳ４０８で算出した視線ベクトル（Ｇｘ_ｉ，Ｇｙ_ｉ，Ｇｚ_ｉ）、ステップＳ４０２で算出したＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓ_ｉ、Ｖｙｓ_ｉ、Ｖｚｓ_ｉ）を代入して、ＧＰＳ衛星ｉの方向への車両の衛星方向速度Ｖｓ_ｉを算出する。

（１３）式の右辺の第１項は、ＧＰＳ衛星ｉに対する車両の相対速度Ｖｒ_ｉを意味しており、第２〜第４項は、ＧＰＳ衛星ｉの自車両方向への速度である。これらの和は、ＧＰＳ衛星ｉの方向への車両の速度を意味することから、（１３）式が成り立つのである。

ステップＳ４１２では、（９）式に、ステップＳ４１０で算出した衛星方向速度Ｖｓ_ｉ、ステップＳ２４で更新した加速度積算値ΔＶ_Ｇ、相対方位角θ_ｇｙｒｏ、ステップＳ４０８で算出した視線ベクトル（Ｇｘ_ｉ，Ｇｙ_ｉ，Ｇｚ_ｉ）を代入する。これにより、（９）式において未知パラメータは、θ^０、Ｃｂｖ^０、Ａ、および、Ｖ^０の４つとなる。

したがって、（９）式に、これら衛星方向速度Ｖｓ_ｉ、加速度積算値ΔＶ_Ｇ、相対方位角θ_ｇｙｒｏ、視線ベクトル（Ｇｘ_ｉ，Ｇｙ_ｉ，Ｇｚ_ｉ）を代入した式を４つ以上立式する。そして、それら４つ以上の式からなる連立方程式を解く。これにより、（９）式において未知パラメータとなっている速度初期値Ｖ^０、方位初期値θ^０を同時に求めることができる。

しかも、未知パラメータθ^０、Ｃｂｖ^０、Ａ、Ｖ^０は、初期時刻以降であれば、時刻が異なっても同じである。そのため、同一時刻で４つの式を立式する必要はなく、複数の時刻において立式した式数が合計４式以上であれば、未知パラメータを求めることができる。たとえば、例えば、４時刻（ｔ_０、ｔ_１、ｔ_２、ｔ_３）の観測衛星数がそれぞれ１であったとしても、観測されたＧＰＳ衛星からのデータを用いて、速度初期値Ｖ^０および方位初期値θ^０を求めることができる。

説明を図３に戻す。ステップＳ３０、Ｓ３６、Ｓ４０のいずれかを実行した場合、ステップＳ４２に進む。ステップＳ４２では、位置更新処理を実行する。この位置更新処理の詳細は図６に示す。

図６において、ステップＳ４２２では、最新の速度初期値Ｖ^０と、ステップＳ２４で更新した加速度積算値ΔＶ_Ｇとから、推定速度Ｖ_ｅを算出する。

速度初期値Ｖ^０を更新しても加速度積算値ΔＶ_Ｇをリセットしない場合、加速度積算値ΔＶ_Ｇに速度初期値Ｖ^０を加算することで推定速度Ｖ_ｅを算出する。一方、速度初期値Ｖ^０を更新したときに加速度積算値ΔＶ_Ｇをリセットする場合には、速度初期値Ｖ^０に加速度積算値ΔＶ_Ｇを加算することで、推定速度Ｖ_ｅを算出する。

ステップＳ４２４では、最新の方位初期値θ^０と、ステップＳ２４で更新した相対方位角θ_ｇｙｒｏとから、推定方位θ_ｅを算出する。具体的な推定方位θ_ｅの計算方法は、速度初期値Ｖ^０と加速度積算値ΔＶ_Ｇから推定速度Ｖ_ｅを算出する方法において、速度初期値Ｖ^０を方位初期値θ^０に置き換え、加速度積算値ΔＶ_Ｇを相対方位角θ_ｇｙｒｏに置き換えた場合と同じである。

ステップＳ４２６では、α＋ｊ_ｋβが１以上になっていないｊ_ｋに１を加える。このｊ_ｋの初期値は０である。また、ｋは、ステップＳ１２で説明したように、衛星測位座標Ｂを算出するごとに１増加する。したがって、衛星測位座標Ｂを算出するごとに、新たなｊ_ｋが生成される。ｊ_ｋの初期値は０であり、時刻ｔ_ｋにおいてｊ_ｋは０となる。時刻ｔ_ｋは、衛星測位座標Ｂ（ｔ_ｋ）を算出するために用いた航法信号を受信した時刻である。そして、ｊ_ｋは、この図６を実行するごとに１ずつ増加する。したがって、ｊ_ｋは、時刻ｔ_ｋ以降に図６の処理を実施した回数を意味する。なお、α、βの意味は、ステップＳ４３２、Ｓ４３６で説明する。

続くステップＳ４２８では、慣性航法位置Ｐ_Ｄ（ｔ）を更新する。慣性航法位置Ｐ_Ｄ（ｔ）は、前回の慣性航法位置Ｐ_Ｄ（ｔ−１）に、今回のステップＳ４２４で算出した推定方位θ_ｅの方向に、今回のステップＳ４２２で算出した推定速度Ｖ_ｅに慣性航法位置Ｐ_Ｄの更新周期を乗じて求められる距離を加算することで更新する。

ステップＳ４３０では、前回、このステップＳ４３０を実行してから、今回、このステップＳ４３０を実行するまでの間に、衛星測位座標Ｂが更新されたか否かを判断する。このステップＳ４３０の判断がＮＯであれば、ステップＳ４３１〜Ｓ４３６を実行することなく、ステップＳ４３８へ進む。ステップＳ４３０の判断がＹＥＳであればステップＳ４３１へ進む。

ステップＳ４３１では、衛星測位座標Ｂの信頼度を決定する。衛星測位座標Ｂの信頼度は、公知の種々の方法で算出することができる。たとえば、（１）衛星測位座標Ｂの算出に用いた航法信号の数、（２）航法信号のＳ／Ｎ、（３）擬似距離ρ_ｉの残差、（４）ＧＰＳ衛星ｉの仰角θ_ｉのいずれか１つ、または（１）〜（４）の組み合わせ、と信頼度との間の予め設定した対応関係を用いて、衛星測位座標Ｂの信頼度を決定する。このステップＳ４３１が測位座標信頼度算出部４５に相当する。

ステップＳ４３２では、（１５）式に示す仮想基準点算出式が備えるオフセット項αを更新する。この（１５）式は、本実施形態において、（１４）式に示す仮想基準点導出関数Ｆ｛Ｐ｝を表す式である。

（１４）式、（１５）式において、時刻ｔ_ｋにおけるｋは整数であり、時刻ｔ_ｋは、衛星測位座標Ｂを算出するために用いた航法信号が受信できた時点である。

また、（１４）式において、Δｔは、慣性航法により自車両の位置Ｐを更新する周期である。この周期を、以下、慣性航法周期という。加速度センサ２０、ヨーレートセンサ３０から検出値を取得するごとに、自車両の位置Ｐは更新できる。したがって、慣性航法周期は、加速度センサ２０、ヨーレートセンサ３０から検出値を取得する周期となる。

また、Δｔが慣性航法周期であり、ｊ_ｋは衛星測位座標Ｂが更新されてからの図６の処理の実施回数を意味することから、ｊ_ｋΔｔは、衛星測位座標Ｂが更新されてから現時点までの経過時間を意味する。

（１４）式は次のことを表している。時刻ｔ_ｋ＋ｊ_ｋΔｔにおける仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）は、時刻ｔ_ｋ＋ｊ_ｋΔｔにおいて仮想基準点導出関数Ｆ｛Ｐ｝が示す座標である。

（１５）式は次のことを表している。時刻ｔ_ｋ＋ｊ_ｋΔｔにおける仮想基準点導出関数Ｆ｛Ｐ｝が示す座標は、その時点における最新の衛星測位座標Ｂ（ｔ_ｋ）と、時刻ｔ_ｋにおいて決定した自車両の位置Ｐ（ｔ_ｋ）とを、重み係数を乗じて加算した値である。

ｊ_ｋは０以上の整数であることから、ｊ_ｋが増加するごとに−ｊ_ｋβの値は減少する。また、この図６では、後述するステップＳ４４０において自車両の移動量ΔＤを算出する。したがって、−ｊ_ｋβは、移動量ΔＤを算出した回数に応じて値が減少する減少項である。一方、ｊ_ｋβは、移動量ΔＤを算出した回数に応じて値が増加する増加項である。また、減少項である−ｊ_ｋβを備える（１−α−ｊ_ｋβ）を第１係数計算式とし、増加項であるｊ_ｋβを備える（α＋ｊ_ｋβ）を第２係数計算式とする。

この仮想基準点算出式において、自車両の位置Ｐ（ｔ_ｋ）に乗じる（１−α−ｊ_ｋβ）と、衛星測位座標Ｂ（ｔ_ｋ）に乗じる（α＋ｊ_ｋβ）は、重み係数を意味する。時刻ｔ_ｋ以降の仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）は、時刻ｔ_ｋにおける自車両の位置Ｐ（ｔ_ｋ）と、時刻ｔ_ｋにおける衛星測位座標Ｂ（ｔ_ｋ）の間において、（１−α−ｊ_ｋβ）と（α＋ｊ_ｋβ）により定まる比率の位置となる。

そして、移動量ΔＤを算出した回数が増えるほど、仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）は、衛星測位座標Ｂ（ｔ_ｋ）に近くづくことになる。

また、仮想基準点算出式においてオフセット項αはｊ_ｋによらず一定である。このオフセット項αが大きいほど、仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）は、ｊ_ｋ＝０のときから、衛星測位座標Ｂ（ｔ_ｋ）に近くなる。

この仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）を算出する理由は、衛星測位座標Ｂ（ｔ_ｋ）の精度が悪い場合にも、自車両の位置が、突然大きく変化してしまうことを抑制するためである。したがって、衛星測位座標Ｂ（ｔ_ｋ）が信頼できるほど、仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）を衛星測位座標Ｂ（ｔ_ｋ）に近づけてもよい。

そこで、ステップＳ４３２では、衛星測位座標Ｂの信頼度に応じて、αの値を大きくする。詳しくは、衛星測位座標Ｂの信頼度とαとの対応関係を予め設定しておき、今回、決定した信頼度と、この対応関係から、αの値を更新する。衛星測位座標Ｂの信頼度とαとの対応関係は、衛星測位座標Ｂの信頼度が高いほど、段階的に、あるいは、連続的に、αが大きくなる関係である。

ステップＳ４３４では、移動量信頼度を決定する。移動量信頼度は、後述するステップＳ４４０で算出する移動量ΔＤの信頼度を意味する。周知のように、慣性航法では、誤差が積算する。つまり、慣性航法により自車両の位置Ｐを更新している期間が長いほど、誤差が大きくなる。そこで、このステップＳ４３４では、衛星測位座標Ｂを更新せずに、慣性航法のみにより自車両の位置Ｐを更新している期間が長いほど、移動量信頼度を低くする。衛星測位座標Ｂを更新していない期間としては、ステップＳ４２６で更新したｊ_ｋの値を用いることができる。このステップＳ４３４が移動量信頼度決定部４６に相当する。

ステップＳ４３６では、ステップＳ４３４で決定した移動量信頼度が高いほど、βを小さい値に更新する。この理由を説明する。

βが大きいほど、仮想基準点算出式により算出できる仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）は、小さいｊ_ｋで衛星測位座標Ｂ（ｔ_ｋ）に一致する。つまり、βは、衛星測位座標Ｂ（ｔ_ｋ）が得られた後、慣性航法周期の何周期で仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）を衛星測位座標Ｂ（ｔ_ｋ）とするかを規定する。このβは請求項の移行速度項に相当する。

移動量信頼度が高いほど、仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）を、早く衛星測位座標Ｂ（ｂｋ）に近づける必要性が低くなる。そこで、移動量信頼度が高いほど、βを大きい値に更新するのである。このステップＳ４３６では、移動量信頼度が高いほど、段階的に、あるいは、連続的に、βが小さくなる予め設定した関係と、ステップＳ４３４で決定した移動量信頼度から、更新後のβを決定する。

ステップＳ４３８では、最新の仮想基準点算出式に、時刻ｔ_ｋにおける自車両の位置Ｐ（ｔ_ｋ）と、時刻ｔ_ｋに受信した航法信号から算出した衛星測位座標Ｂ（ｔ_ｋ）を代入して、時刻ｔ_ｋ＋ｊ_ｋΔｔにおける仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）を算出する。なお、ステップＳ４３０、Ｓ４３２、Ｓ４３６と、このステップＳ４３８が仮想基準点決定部４３に相当する。

ステップＳ４４０では、時刻ｔ_ｋから現在の時刻ｔ_ｋ＋ｊ_ｋΔｔまでに自車両が移動した移動量ΔＤを、（１６）式から算出する。（１６）式から分かるように、この移動量ΔＤは、ステップＳ４２８で算出した慣性航法位置Ｐ_Ｄを用いて算出する。なお、このステップＳ４４０と、図３のステップＳ２０〜Ｓ４０が移動量算出部４２に相当する。

ステップＳ４４２では、（１７）式を用いて、現在の自車両の位置Ｐを更新する。このステップＳ４４２が位置決定部４４に相当する。

＜実施形態のまとめ＞
本実施形態の測位装置１は、衛星測位座標Ｂが得られても、その衛星測位座標Ｂを基準点としてそのまま用いるのではなく、移動量ΔＤの基点となる基準点として仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）を決定する（Ｓ４３８）。

この仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）は、（１５）式に示すように、衛星測位座標Ｂ（ｔ_ｋ）を算出するために用いた航法信号を受信した時刻ｔ_ｋにおける自車両の位置Ｐ（ｔ_ｋ）と、その衛星測位座標Ｂ（ｔ_ｋ）とに、それぞれ重み係数を乗じた後に、それらを加算することで定まる座標である。また、衛星測位座標Ｂ（ｔ_ｋ）に乗じる重み係数（α＋ｊ_ｋβ）は、位置更新処理（図６）が実行されるごとに大きくなる。

衛星測位座標Ｂに乗じる重み係数（α＋ｊ_ｋβ）が、位置更新処理が実行されるごとに大きくなることで、図７に示すように、仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）は、ｊ_ｋが大きくなるごとに、衛星測位座標Ｂ（ｔ_ｋ）に近づいていく。

この図７に示す衛星測位座標Ｂ（ｔ_ｋ）は、測位精度が悪かった例である。また、図７において、破線で示す軌跡は慣性航法軌跡であり、二重線で示す軌跡は、ステップＳ４４２で逐次更新する自車両の位置Ｐを表す軌跡である。一方、一点鎖線は、仮想基準点ｂを用いずに、衛星測位座標Ｂ（ｔ_ｋ）が演算された時点で、即座に、その衛星測位座標Ｂ（ｔ_ｋ）を基準点として用いて自車両の位置を更新した場合の軌跡である。以下では、この軌跡を、従来手法軌跡という。

航法信号を受信してから、衛星測位座標Ｂが得られるまでには、演算処理時間を要する。図７の例では、演算処理時間は３Δｔであるとしている。したがって、たとえば、図７に示す衛星測位座標Ｂ（ｔ_ｋ−１）が得られるのは、衛星測位座標Ｂ（ｔ_ｋ−１）を演算するための航法信号を受信した後、３回目の位置更新処理からである。そのため、従来手法軌跡における点Ｑ_１、Ｑ_２では、まだ、衛星測位座標Ｂ（ｔ_ｋ−１）は基準点とはされていない。

衛星測位座標Ｂ（ｔ_ｋ−１）を演算するための航法信号を受信した後、３回目の位置更新処理により得られる点Ｑ_３では、衛星測位座標Ｂ（ｔ_ｋ−１）を基準点とする。この基準点に、慣性航法軌跡における点Ｄ（ｔ_ｋ−１）から点Ｄ（ｔ_ｋ−１＋３Δｔ）までの移動量が加算されて、点Ｑ_３が得られる。

衛星測位座標Ｂ（ｔ_ｋ）は、測位精度が悪かったため、この衛星測位座標Ｂ（ｔ_ｋ）を基準点として用いる点Ｑ（ｔ_ｋ＋３Δｔ）において、直前の点Ｑ（ｔ_ｋ＋２Δｔ）から大きく位置が変化してしまい、従来手法軌跡には段形状が生じる。また、次に、衛星測位座標Ｂ（ｔ_ｋ＋１）が得られたときにも、従来手法軌跡は、点Ｑ（ｔ_ｋ＋１＋３Δｔ）において、段状に変化してしまう。

これに対して、本実施形態では、衛星測位座標Ｂ（ｔ_ｋ）をそのまま基準点として用いるのではなく、仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）を決定する。この仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）は、（１５）式により算出することから、衛星測位座標Ｂ（ｔ_ｋ）を算出するために用いた航法信号を受信した時刻ｔ_ｋにおける自車両の位置Ｐ（ｔ_ｋ）と、衛星測位座標Ｂ（ｔ_ｋ）の間に位置する。加えて、仮想基準点ｂ（ｔ_ｋ＋ｊ_ｋΔｔ）は、ｊ_ｋが大きくなるごとに、衛星測位座標Ｂ（ｔ_ｋ）に近づいていく。

図７の例では、時刻ｔ_ｋ＋３Δｔ、ｔ_ｋ＋４Δｔ、ｔ_ｋ＋５Δｔ、ｔ_ｋ＋６Δｔ、ｔ_ｋ＋７Δｔでは、それぞれ仮想基準点ｂ（ｔ_ｋ＋３Δｔ）、ｂ（ｔ_ｋ＋４Δｔ）、ｂ（ｔ_ｋ＋５Δｔ）ｂ（ｔ_ｋ＋６Δｔ）、ｂ（ｔ_ｋ＋７Δｔ）を用いて、自車両の位置Ｐ（ｔ_ｋ＋３Δｔ）、Ｐ（ｔ_ｋ＋４Δｔ）、Ｐ（ｔ_ｋ＋５Δｔ）、Ｐ（ｔ_ｋ＋６Δｔ）、Ｐ（ｔ_ｋ＋７Δｔ）を算出する。したがって、自車両の位置Ｐを表す軌跡が段状に変化することを抑制できる。

また、本実施形態では、衛星測位座標Ｂの信頼度を決定しており（Ｓ４３１）、この信頼度が高いほど、オフセット項αを大きくしている（Ｓ４３２）。これにより、衛星測位座標Ｂの信頼度が高い場合には、仮想基準点ｂを衛星測位座標Ｂに近づけることになる。したがって、自車両の位置Ｐの精度が向上する。

また、本実施形態では、移動量信頼度を決定しており（Ｓ４３４）、この移動量信頼度が高いほど、βを小さい値に更新する（Ｓ４３６）。βを小さい値に更新することで、衛星測位座標Ｂの精度が低い場合であっても、その影響をより少なくすることができる。

以上、本発明の実施形態を説明したが、本発明は上述の実施形態に限定されるものではなく、次の変形例も本発明の技術的範囲に含まれ、さらに、下記以外にも要旨を逸脱しない範囲内で種々変更して実施することができる。

＜変形例１＞
前述の実施形態では、衛星測位座標Ｂの信頼度に応じてオフセット項αの値を決定していたが、オフセット項αの値を変更しないようにしてもよい。この場合、衛星測位座標Ｂの信頼度は決定しなくてもよい。

＜変形例２＞
前述の実施形態では、移動量信頼度に応じてβの値を更新していたが、βの値を変更しないようにしてもよい。この場合、移動量信頼度は決定しなくてもよい。

＜変形例３＞
（１５）式に示した仮想基準点算出式において、オフセット項αをなしとしてもよい。

＜変形例４＞
前述の実施形態では、移動体は車であったが、車以外の移動体にも、本発明は適用できる。

＜変形例５＞
前述の実施形態では、衛星測位座標Ｂを（３）式を解くことにより算出していた。しかし、衛星測位座標Ｂは航法信号を用いて演算していれば、前述の実施形態の演算方法に限られない。

たとえば、衛星測位座標Ｂを、複数の時刻において航法信号を受信して算出した座標と、慣性航法軌跡を組み合わせて決定してもよい。詳しくは、１周期前の航法信号から算出した座標に、慣性航法により得た、１周期前の航法信号を受信した時刻から最新の航法信号を受信した時刻までの移動体の移動量を加算する。これにより、１周期前の航法信号に基づいて、最新の航法信号を受信した時刻の座標が定まる。

さらに、１周期前の航法信号から最新の航法信号を受信した時刻の座標を求めるのと同様の方法で、２周期前、あるいは、３周期以上前の航法信号に基づいて、最新の航法信号を受信した時刻の座標を求めてもよい。このようにして、過去の航法信号に基づいて、最新の航法信号を受信した時刻の座標を１つ以上求める。そして、過去の航法信号に基づいて求めた、最新の航法信号を受信した時刻の座標と、最新の航法信号から求めた座標とを平均等して、最終的な衛星測位座標Ｂとする。

１：測位装置 １０：航法信号受信部 ２０：加速度センサ ３０：ヨーレートセンサ ４０：制御部 ４１：衛星測位部 ４２：移動量算出部 ４３：仮想基準点決定部 ４４：位置決定部 ４５：測位座標信頼度算出部 ４６：移動量信頼度決定部

Claims (1)

1. 移動体で用いられ、前記移動体の位置を逐次決定する測位装置であって、
   航法信号受信部（１０）が受信した航法信号に基づいて定まる座標である衛星測位座標を演算する衛星測位部（４１）と、
   慣性センサが検出する信号を取得して、慣性航法により、前記航法信号を受信した時点から、前記慣性センサから信号を取得した時点までの前記移動体の移動量を逐次算出する移動量算出部（４２）と、
   前記航法信号を受信した時点以降の仮想基準点の座標を逐次決定する仮想基準点決定部（４３）と、
   前記移動体が、前記仮想基準点決定部が決定した前記仮想基準点から、前記移動量算出部が算出した前記移動量だけ移動したとして、前記移動体の位置を逐次決定する位置決定部（４４）と、
   前記衛星測位部が演算した前記衛星測位座標の信頼度を算出する測位座標信頼度算出部（４５）と、
   前記移動量算出部が算出した前記移動量の信頼度を決定する移動量信頼度決定部（４６）とを備え、
   前記仮想基準点決定部は、
   前記位置決定部が決定した、前記航法信号を受信した時点における前記移動体の位置と、前記衛星測位部が演算した前記衛星測位座標とに、それぞれ重み係数を乗じて定まる座標を前記仮想基準点の座標とし、かつ、前記衛星測位座標に乗じる重み係数を段階的に大きくでき、
   前記移動体の位置に乗じる前記重み係数を、前記航法信号を受信した時点以降に前記移動量算出部が前記移動量を算出した回数に応じて値が減少する減少項を備える第１係数計算式により計算し、前記衛星測位座標に乗じる前記重み係数を、前記航法信号を受信した時点以降に前記移動量算出部が前記移動量を算出した回数に応じて値が増加する増加項を備える第２係数計算式により計算し、
   前記第１係数計算式および前記第２係数計算式は、いずれも、前記移動量算出部が前記移動量を算出した回数によらず一定であるオフセット項を備え、
   前記測位座標信頼度算出部が算出した前記衛星測位座標の信頼度が高いほど、前記オフセット項の値を大きくし、
   前記減少項は、前記航法信号を受信した時点以降に前記移動量算出部が前記移動量を算出した回数に、移行速度項と、マイナスを乗じる項であり、
   前記増加項は、前記航法信号を受信した時点以降に前記移動量算出部が前記移動量を算出した回数に、移行速度項を乗じる項であり、
   前記移動量信頼度決定部が決定した前記移動量の信頼度の高いほど、前記移行速度項の値を小さくすることを特徴とする測位装置。

Images