JP2015230218A - 移動距離推定装置 - Google Patents

Abstract

【課題】より精度よく移動距離を推定することができる移動距離推定装置を提供する。
【解決手段】加速度積算値と速度初期値Ｖ^０とに基づいて、移動体の推定速度Ｖ^ｔ _ｅを逐次推定する推定速度決定部１２８と、推定速度Ｖ^ｔ _ｅと時間とに基づいて移動体の推定移動距離Ｌ^ｔ _ｅを逐次算出する移動距離算出部１４１と、前回、初期設定値決定部１２６が速度初期値Ｖ^０を決定してから、今回、速度初期値Ｖ^０を決定するまでに逐次推定した推定速度Ｖ^ｔ _ｅである補正前過去推定速度Ｖ^ｔ（ｉ） _ｅを、今回の速度初期値Ｖ^０と連続するように補正した補正過去速度Ｖ^ｔ（ｉ） _ｃｏを決定する過去速度補正部１４２と、補正過去速度Ｖ^ｔ（ｉ） _ｃｏを決定した場合に、補正過去速度Ｖ^ｔ（ｉ） _ｃｏと時間とに基づいて、前回、速度初期値Ｖ^０を決定してから、今回、速度初期値Ｖ^０を決定するまでの推定移動距離を再計算する移動距離補正部１４３とを備える。
【選択図】図３

Description

本発明は、移動体の移動距離を推定する移動距離推定装置に関し、特に、その移動距離の推定精度を向上させる技術に関する。

移動体の移動距離は、たとえば、移動方向とともに用いて、移動軌跡を推定するために用いられる。移動軌跡が推定できれば、ある時点における位置に対する相対位置を決定することができる。したがって、ＧＰＳ等の衛星測位システムが送信する測位用の電波を受信できない場合であっても、過去に測位した現在位置と移動軌跡から現在位置を推定することができる。

移動距離は、速度と時間の積で計算することができる。移動体の速度を推定する方法として、特許文献１には、加速度を計測し、この加速度から速度を算出することに加え、ＧＰＳ搬送波のドップラーシフト量からも速度（以下、ドップラー速度）を算出する技術が開示されている。この特許文献１では、ドップラー速度により、加速度から算出した速度を逐次補正している。

加速度から算出した速度は、短周期で更新できるが、ドップラー速度よりも誤差が生じやすい。そのため、ドップラー速度により、加速度から算出した速度を逐次補正するのである。この補正により、精度のよい速度が得られる。

特許文献１の技術は、車両に適用する場合に、車速センサの信号を取得するための配線が不要になるメリットがある。

特許文献２には、車両の速度ベクトルを精度よく推定する技術が開示されている。具体的には、特許文献２の技術では、速度ベクトルの推定に、衛星方向車速と、車輪速と、車両から測位システムの衛星への視線ベクトルと、車両方位と、クロックドリフトとの関係を示す式を用いる。この式は、衛星方向速度、視線ベクトル、速度ベクトル、クロックドリフトの関係を示す式を、拘束条件を用いて変形した式である。拘束条件は、速度ベクトルが移動体の速度およびヨーレートにより拘束されるという条件、および、クロックドリフトはその時間変化が線形であるという条件である。

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

特許文献２の技術は、車輪速が必要となる。そのため、特許文献２の技術は、車輪速センサと接続することができない速度推定装置には適用できないという問題があった。

これに対して、特許文献１の技術は、自装置内に備えている加速度センサが検出した加速度から速度を算出しているため、車輪速センサと接続する必要がない。

広く知られているようにセンサにはドリフトという現象があるため、センサ検出値の誤差は時間とともに発散する。そこで、特許文献１の技術は、加速度から算出した速度をドップラー速度により補正している。

しかし、ドップラー速度など、加速度から算出した速度よりも精度のよい速度で、加速度から算出した速度を逐次補正するとしても、加速度から算出した速度を補正してから次に補正するまでの間は、加速度から算出した速度と時間の積で移動距離を算出する。加速度から算出した速度を用いている間は速度誤差が増加していくので、移動距離の誤差も累積的に大きくなってしまう。したがって、移動距離の推定精度が十分ではなかった。

本発明は、この事情に基づいて成されたものであり、その目的とするところは、より精度よく移動距離を推定することができる移動距離推定装置を提供することにある。

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

上記目的を達成するための本発明は、移動体とともに移動する加速度センサ（２０）の検出値の進行方向成分である進行方向加速度を逐次決定する進行方向加速度決定部（１０２）と、進行方向加速度決定部が決定した進行方向加速度を積算した加速度積算値を逐次算出する積算処理部（１０４）と、衛星測位システムが備える衛星からのドップラーシフト量に基づいて算出する移動体の速度、または、移動体の停止時の速度を、移動体の速度初期値として決定する初期設定値決定部（１２６）と、積算処理部が算出した加速度積算値と、初期設定値決定部が決定した速度初期値とに基づいて、移動体の推定速度を逐次推定する推定速度決定部（１２８）と、推定速度決定部が決定した推定速度と時間とに基づいて移動体の推定移動距離を逐次算出する移動距離算出部（１４２）と、初期設定値決定部が速度初期値を決定した場合、前回、初期設定値決定部が速度初期値を決定してから、今回、初期設定値決定部が速度初期値を決定するまでに推定速度決定部が逐次推定した推定速度である補正前過去推定速度を、今回、初期設定値決定部が決定した速度初期値と連続するように補正した補正過去速度を決定する過去速度補正部（１４１）と、過去速度補正部が補正過去速度を決定した場合に、補正過去速度と時間とに基づいて、前回、初期設定値決定部が速度初期値を決定してから、今回、初期設定値決定部が速度初期値を決定するまでの推定移動距離を再計算する移動距離補正部（１４３）と、を備えることを特徴とする移動距離推定装置である。

本発明によれば、初期値設定部により、衛星測位システムが備える衛星からの電波のドップラーシフト量に基づいて算出する移動体の速度、または、移動体の停止時の速度を、移動体の速度初期値として決定している。衛星からの電波のドップラーシフト量に基づいて算出する移動体の速度や、移動体の停止時の速度は、精度よく決定することができる。したがって、速度初期値は、速度初期値を決定した時点における移動体の速度を精度のよく表している。

そして、推定速度は、この速度初期値と加速度積算値とを用いて推定している。したがって、速度初期値を更新した以後は、加速度積算値の累積誤差により低下している推定速度の精度が向上する。

加えて、本発明では、初期設定値決定部が速度初期値を決定した場合、前回、速度初期値を決定してから、今回、速度初期値を決定するまでの推定速度である補正前過去推定速度を、今回の速度初期値と連続するように補正して補正過去速度とする。これにより、加速度積算値の累積誤差により低下している過去の推定速度の精度も向上する。そして、補正過去速度を決定した場合、その補正過去速度を用いて、前回、速度初期値を決定してから、今回、速度初期値を決定するまでの推定移動距離を再計算する。これにより、過去の推定速度の精度低下により低下していた推定移動距離の精度も向上する。

実施形態の現在位置推定装置１の構成図である。 加速度積算値ΔＶ_Ｇの誤差が発散することを示す図である。 図１の位置更新部１４０の詳細構成図である。 図３の過去速度補正部１４２による速度の補正を説明する図である。 図１の制御部１００が行う処理の流れを示すフロチャートである。 図５のステップＳ２２の詳細フロチャートである。 図５のステップＳ２８の詳細フロチャートである。 図５のステップＳ３４の詳細フロチャートである。

＜実施形態の構成＞
以下、本発明の実施形態を図面に基づいて説明する。図１に示す現在位置推定装置１は、本発明の移動距離推定装置としての機能を備える。この現在位置推定装置１は、ＧＰＳ信号受信部１０、加速度センサ２０、ヨーレートセンサ３０、および、制御部１００を備えており、図示しない車両に搭載される。

ＧＰＳ信号受信部１０は、衛星測位システムの一つであるグローバルポジショニングシステム（以下、ＧＰＳ）が備えるＧＰＳ衛星が送信するＧＰＳ電波を受信する受信機である。このＧＰＳ電波は、搬送波にＧＰＳ信号が重畳されたものを意味する。ＧＰＳ信号受信部１０は、受信したＧＰＳ電波を復調してＧＰＳ信号を取り出し、制御部１００に送る。また、搬送波、受信信号強度も制御部１００に送る。ＧＰＳ信号は請求項の衛星信号に相当する。

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

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

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

制御部１００は、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータであり、ＣＰＵが、ＲＡＭの一時記憶機能を利用しつつＲＯＭに記憶されているプログラムを実行することで、制御部１００は、図１に示す各部１０２〜１４０として機能する。

進行方向加速度決定部１０２は、加速度センサ２０が検出した検出値から、車両の進行方向加速度を決定する。前述したように、加速度センサ２０は車両に対する向きが予め定められた向きに固定されており、ｙ軸が車両進行方向を向いている。したがって、加速度センサ２０が検出したｙ軸の検出値を、進行方向加速度として抽出する。

積算処理部１０４は、進行方向加速度決定部１０２が決定した進行方向加速度、すなわち、加速度センサ２０のｙ軸の検出値を、逐次、積算する。積算した値を、以下、加速度積算値ΔＶ_Ｇという。また、この積算処理部１０４は、ヨーレートセンサ３０の検出値を積算して、相対方位角θ^ｔ _ｇｙｒｏも算出する。相対方位角θ^ｔ _ｇｙｒｏは、基準時点における車両の進行方位に対する時刻ｔの相対方位角である。この相対方位角θ^ｔ _ｇｙｒｏは、（１）式から算出する。（１）式において、Δｔはタイムステップ、ω^ｔは時刻ｔに検出されたヨーレートセンサ３０の検出値である。

なお、これら加速度積算値ΔＶ_Ｇ、相対方位角θ^ｔ _ｇｙｒｏを算出するための加速度センサ２０の検出値、ヨーレートセンサ３０の検出値は、同じタイミングで取得する。取得するタイミングは、たとえば、一定時間時、あるいは、一定距離走行時である。

衛星情報取得部１０６は、ＧＰＳ信号受信部１０からＧＰＳ信号および搬送波を取得する。ＧＰＳ信号受信部１０が複数のＧＰＳ衛星からＧＰＳ電波を受信している場合、ＧＰＳ信号受信部１０がＧＰＳ電波を受信したすべてのＧＰＳ衛星についてのＧＰＳ信号および搬送波を取得する。この衛星情報取得部１０６は請求項の衛星信号取得部として機能する。

さらに、衛星情報取得部１０６は、これらＧＰＳ信号および搬送波から得られるＧＰＳ衛星ｉに関する情報である、ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）、擬似距離ρ_ｉ、ドップラーシフト量Ｄ_ｉも算出する。

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

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

相対速度算出部１０８は、衛星情報取得部１０６が算出したドップラーシフト量Ｄ_ｉに基づいて、ＧＰＳ衛星ｉに対する車両の相対速度Ｖｒ_ｉを算出する。相対速度Ｖｒ_ｉは、下記（２）式から算出する。（２）式において、Ｖｒ_ｉはＧＰＳ衛星ｉに対する車両の相対速度、Ｄ_ｉは衛星情報取得部１０６が算出したドップラーシフト量、Ｃは光速、ＦはＧＰＳ衛星が送信する電波の搬送波の周波数である。

衛星速度算出部１１０は、衛星情報取得部１０６が算出した各ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）の時系列データから、ケプラーの方程式の微分を用いる公知の方法で、各ＧＰＳ衛星ｉの速度ベクトル、すなわち、三次元速度Ｖｘｓ_ｉ、Ｖｙｓ_ｉ、Ｖｚｓ_ｉを算出する。

現在位置算出部１１２は、衛星情報取得部１０６が算出した各ＧＰＳ衛星ｉの擬似距離ρ_ｉを用いて、車両の現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を算出する。

ＧＰＳ信号を用いた測位では、ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）と、各ＧＰＳ衛星との間の擬似距離ρ_ｉとに基づいて、三角測量の原理に従って、車両の現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を算出する。ここで、ＧＰＳ衛星ｉまでの真の距離ｒ_ｉは（３）式で表される。一方、擬似距離ρ_ｉは（４）式で表される。なお、（４）式において、ｓは時計誤差による距離誤差である。

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

なお、本実施形態では、車両の現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）は、最終的な現在位置として用いる以外に、後述する衛星方向算出部１１４において、ＧＰＳ衛星ｉの方向、すなわち、ＧＰＳ衛星と自車両との角度を求めるためにも用いる。

ＧＰＳ衛星は遠方に存在するため、ＧＰＳ衛星と自車両との角度を求めるために用いる現在位置は、精度が低くてもよい。また、車両の現在位置は、詳細は後述するが、推定移動距離Ｌ^ｔ _ｅあるいは補正移動距離Ｌ^ｔ _ｃｏと推定方位θ^ｔ _ｅから求めることもできる。

したがって、常に擬似距離ρ_ｉを用いて現在位置を決定する必要はなく、擬似距離ρ_ｉを用いた位置決定以外の精度の低い方法で車両の現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を決定してもよい。システム等で許容される推定精度に依存するが、車両の位置誤差が数百ｍの範囲であれば、速度推定誤差は１ｍ／ｓｅｃ以下となり大きな問題はない。そのため、たとえば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、車両の現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を決定してもよい。また、擬似距離ρ_ｉを用いて現在位置を決定する場合にも、その擬似距離ρ_ｉは、信号品質のよいＧＰＳ電波から算出する必要はない。信号品質がよいことの判定基準は、信号品質判定部１２０の説明において、説明する。

視線ベクトル算出部１１３は、車両からＧＰＳ衛星ｉへの視線ベクトル（Ｇｘ_ｉ、Ｇｙ_ｉ、Ｇｚ_ｉ）を算出する。視線ベクトルのｘ成分、ｙ成分、ｚ成分は、（６）式から算出する。

（６）式において、ρ^ｔ _ｉは時刻ｔにおけるＧＰＳ衛星ｉの擬似距離、（Ｘ^ｔ _ｓｉ、Ｙ^ｔ _ｓｉ、Ｚ^ｔ _ｓｉ）は時刻ｔにおけるＧＰＳ衛星ｉの位置座標である。これらは、衛星情報取得部１０６が算出している。（Ｘ^ｔ _Ｖ、Ｙ^ｔ _Ｖ、Ｚ^ｔ _Ｖ）は時刻ｔにおける現在位置であり、現在位置算出部１１２が算出している。

衛星方向算出部１１４は、現在位置算出部１１２が算出した現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）、および、衛星情報取得部１０６が算出したＧＰＳ衛星の位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）に基づいて、各ＧＰＳ衛星ｉの方向Ｒ_ｉを算出する。各ＧＰＳ衛星ｉの方向Ｒ_ｉは、車両からＧＰＳ衛星ｉを見たときの、水平方向に対する仰角θ_ｉ、北方向に対する方位角φ_ｉで表すものとする。

衛星方向速度算出部１１６は、ＧＰＳ衛星ｉの方向への車両の速度である衛星方向速度Ｖｓ_ｉを算出する。衛星方向速度Ｖｓ_ｉは、下記（７）式から算出する。

（７）式において、右辺第１項は相対速度Ｖｒであり、相対速度算出部１０８が算出している。Ｇｘ、Ｇｙ、Ｇｚは、視線ベクトルであり、視線ベクトル算出部１１３が算出している。Ｖｘｓ、Ｖｙｓ、Ｖｚｓは、ＧＰＳ衛星ｉの速度のｘ、ｙ、ｚ成分であり、衛星速度算出部１１０が算出している。（７）式の右辺の第１項は、ＧＰＳ衛星ｉに対する車両の相対速度Ｖｒ_ｉであり、第２〜第４項は、ＧＰＳ衛星ｉの車両方向への速度である。これらの和は、ＧＰＳ衛星ｉの方向への車両の速度を意味することから、（７）式が成り立つのである。

速度ベクトル算出部１１８は、車両の速度ベクトルを算出する。車両の速度ベクトルを（Ｖｘ，Ｖｙ，Ｖｚ）としたとき、衛星方向速度Ｖｓ_ｉと、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）との関係は、以下の（８）式で表される。

各ＧＰＳ衛星ｉについて得られる上記（８）式より、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）およびＣｂｖを未知数とした以下の（９）式で表される連立方程式が得られる。

（９）式において、Ｖｓａｔ_ｉは、車両方向のＧＰＳ衛星ｉの速度であり、Ｖｓａｔ_ｉ＝Ｒ_ｉ［Ｖｘｓ_ｉ，Ｖｙｓ_ｉ，Ｖｚｓ_ｉ］^Ｔにより求める。Ｔは行列の転置を意味する。なお、（７）式の右辺第２〜第４項を計算して車両方向のＧＰＳ衛星ｉの速度Ｖｓａｔ_ｉを求めてもよい。ＣｂｖはＧＰＳ信号受信部１０が備える時計のクロックドリフトである。

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

信号品質判定部１２０は、ＧＰＳ信号受信部１０が受信したＧＰＳ衛星ｉからのＧＰＳ電波の信号品質がよいか否かを判定する。信号品質の判定には、公知の種々の基準を用いることができる。

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

なお、擬似距離ρ_ｉの残差とは、ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）と車両の現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）との間の距離と、擬似距離ρ_ｉとの差である。この残差が大きい場合には、マルチパス等の影響が考えられるので、信号品質が良くないと判定する。判定条件１は、Ｓ／Ｎが所定値以上であれば信号品質はよいと判定する。判定条件３は、仰角θ_ｉが判定基準角以上であれば信号品質はよいと判定する。

停止判定部１２２は、車両が停止しているか否かを判定する。そして、車両が停止していると判定した場合には、車両の速度を０ｋｍ／ｈとする。停止判定の方法は、公知の種々の手法を用いることができる。たとえば、加速度センサ２０のｚ軸の検出値が停止判定値以下であれば、車両が停止していると判定する。車両が走行している場合には、多少の上下振動があるので、ｚ軸の検出値により停止判定を行うことができるのである。ｚ軸の検出値に代えて、ｙ軸の検出値や、ｚ軸の検出値の変化量、ｙ軸の検出値の変化量を用いてもよい。加速度センサ２０の検出値により停止判定を行えば、ブレーキ信号やシフト位置信号を取得するための配線が不要となる利点がある。もちろん、ブレーキ信号やシフト位置信号を取得できるようにして、それらの信号を用いて停止判定をしてもよい。

初期設定値決定部１２６は、推定速度決定部１２８で用いる速度初期値Ｖ^０、推定方位決定部１３０で用いる方位初期値θ^０決定する。

本実施形態では、すでに説明した速度ベクトル算出部１１８でも車両の速度を算出することができる。しかし、速度ベクトル算出部１１８は、信号品質がよいＧＰＳ電波を４個以上のＧＰＳ衛星から受信しなければ、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出することができない。したがって、たとえば、高層ビルの多い都市部では、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出できない時間が長く続くこともある。また、信号品質がよいＧＰＳ電波を４個以上受信できる環境であっても、速度ベクトル算出部１１８では、周波数解析を必要とするドップラーシフト量Ｄを用いるため、たとえば、１００ｍｓ毎などの一定周期でしか、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出することができない。

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

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

加速度センサ２０の検出値には常に誤差があり、その誤差は発散する性質を有する。図２はそのことを示すグラフである。図２は停止状態における加速度積算値ΔＶ_Ｇのグラフである。車両が停止状態であることから、この図２における加速度積算値ΔＶ_Ｇは、加速度積算値ΔＶ_Ｇをそのまま速度とした場合の誤差を意味する。図２から、加速度積算値ΔＶ_Ｇをそのまま速度とする場合、誤差が時間の経過により発散することが分かる。なお、図２では、誤差は負の値であるが、これとは反対に誤差が正の値となることもある。

このように、加速度積算値ΔＶ_Ｇをそのまま速度とすると、誤差が時間の経過により発散する。そこで、精度の高い速度を求めた時点で、その精度の高い速度を速度初期値Ｖ^０とする。

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

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

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

ただし、速度初期値Ｖ^０については、速度ベクトル算出部１１８が速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出できたときは、（１０）式から速度初期値Ｖ^０を決定する。速度ベクトル算出部１１８が算出する速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は精度がよいので、この速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を用いて速度初期値Ｖ^０を決定したほうが、速度初期値Ｖ^０の精度が向上するからである。また、停止判定部１２２が、車両が停止していると判定したときは、速度初期値Ｖ^０を−ΔＶ_Ｇとする。

（１１）式において、ｔは時刻、Ｖ_{ｗｈｅｅｌ}は車輪速センサの検出値、θ^０は初期時刻における車両の進行方向の方位角（以下、方位初期値）、θ_ｇｙｒｏは、車両の進行方向の方位角の積算値すなわち相対方位角、Ｃｂｖ^０は初期時刻におけるクロックドリフト、Ａはクロックドリフトの時間変化の傾き、Ｇｘ、Ｇｙは、車両からＧＰＳ衛星ｉへの視線ベクトルのｘ成分、ｙ成分である。

また、（１２）式において、Ｖ^０は初期時刻における車速である速度初期値、ΔＶ_Ｇは初期時刻以降の加速度積算値である。すなわち、（１２）式は、（１１）式における車輪速センサの検出値Ｖ_{ｗｈｅｅｌ}を、速度初期値Ｖ^０と加速度積算値ΔＶ_Ｇの和に置き換えた式である。この（１２）式が請求項の速度推定式に相当する。

まず、（１１）式の導出方法を説明する。特許文献２にも開示されているように、衛星方向速度Ｖｓ^ｔ _ｉと、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）との関係は、（１３）式で表すことができる。

この（１３）式には、Ｖｘ^ｔ、Ｖｙ^ｔ、Ｖｚ^ｔ、Ｃｂｖ^ｔの４つの未知パラメータがある。この未知パラメータの数を少なくするために、特許文献２では、下記に示す拘束条件１〜３を（１３）式に代入することで、前述した（１１）式を導出している。

拘束条件１は、推定する速度ベクトルのｘ成分、ｙ成分の大きさを車輪速で拘束し、かつｘ成分、ｙ成分の時間変化分を方位角の時間変化で拘束したものである。拘束条件１のθ^０は初期時刻における車両の進行方向の方位角である。拘束条件２は、高さ方向速度の変化は常に微小であると仮定するものである。

拘束条件３は、クロックドリフトの時間変化は緩やかであるため、短時間内における変化は線形であると仮定するものである。拘束条件３のＣｂｖ^０は初期時刻におけるクロックドリフト、Ａはクロックドリフトの時間変化の傾きを示す。

なお、拘束条件１および２では、二次元平面内の速度成分のみ時間変化で拘束しているが、３軸ジャイロセンサなどにより取得したピッチレートを用いて、ｚ軸成分も時間変化で拘束してもよい。上記拘束条件１〜３を（１３）式に取り入れることで（１１）式が得られる。そして、（１１）式における車輪速センサの検出値Ｖ_{ｗｈｅｅｌ}を、速度初期値Ｖ^０と加速度積算値ΔＶ_Ｇの和に置き換えることで、（１２）式が得られる。

（１２）式において、衛星方向速度Ｖｓ^ｔ _ｉは、時刻ｔにおけるＧＰＳ衛星ｉ方向の衛星方向速度であり、衛星方向速度算出部１１６が算出する。ΔＶ^ｔ _Ｇは、時刻ｔにおける加速度積算値であり、積算処理部１０４が算出する。（Ｇｘ^ｔ _ｉ、Ｇｙ^ｔ _ｉ、Ｇｚ^ｔ _ｉ）は、時刻ｔにおける視線ベクトルであり、視線ベクトル算出部１１３が算出する。θ^ｔ _ｇｙｒｏは、ヨーレート積算値であり、積算処理部１０４が算出する。よって、（１２）式において未知パラメータは、θ^０、Ｃｂｖ^０、Ａ、および、Ｖ^０の４つである。

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

なお、速度初期値Ｖ^０は、前述したように、初期時刻における車速である。また、速度初期値Ｖ０を算出する（１２）式は、ドップラーシフト量Ｄから算出する衛星方向速度Ｖｓを用いる。したがって、初期設定値決定部１２６は、ドップラーシフト量に基づいて、初期時刻における車両の速度を算出していることにもなる。

推定速度決定部１２８は、初期設定値決定部１２６が決定した速度初期値Ｖ^０に、積算処理部１０４が算出した加速度積算値ΔＶ^ｔ _Ｇを加算して、推定速度Ｖ^ｔ _ｅを加速度取得周期で逐次算出する。

推定方位決定部１３０は、初期設定値決定部１２６が決定した方位初期値^０に、積算処理部１０４が算出した相対方位角θ^ｔ _ｇｙｒｏを加算して、車両の移動方位を示す推定方位θ^ｔ _ｅを、相対方位角決定周期で逐次決定する。

位置更新部１４０は、車両の現在位置を逐次更新する。この位置更新部１４０の詳細構成を図３に示している。図３に示すように、位置更新部１４０は、移動距離算出部１４１、過去速度補正部１４２、移動距離補正部１４３、位置決定部１４４を備える。

移動距離算出部１４１は、車両の速度が更新されるごとに、車両の移動距離を算出する。車両の速度は、加速度取得周期で更新される。車両の速度は、推定速度決定部１２８、速度ベクトル算出部１１８、停止判定部１２２のいずれかが決定する。移動距離算出部１４１は、それら推定速度決定部１２８、速度ベクトル算出部１１８、停止判定部１２２が決定した速度に加速度取得周期を乗じて、加速度取得周期の間に車両が移動した距離の推定値である推定移動距離Ｌ^ｔ _ｅを算出する。

過去速度補正部１４２は、初期設定値決定部１２６が速度初期値Ｖ^０を更新するごとに、前回、速度初期値Ｖ^０が更新されてから、今回、速度初期値Ｖ^０が更新されるまでに推定速度決定部１２８が逐次推定した推定速度Ｖ^ｔ _ｅを補正する。この補正対象となっている推定速度Ｖ^ｔ _ｅを、以下、補正前過去推定速度Ｖ^ｔ（ｉ） _ｅとする。なお、ｉはＮ〜０までの整数であり、ｔ（Ｎ）は、前回、速度初期値Ｖ^０が更新された時刻、ｔ（０）は、今回、速度初期値Ｖ^０が更新された時刻を意味する。また、補正前過去推定速度Ｖ^ｔ（ｉ） _ｅを補正した値を補正過去速度Ｖ^ｔ（ｉ） _ｃｏとする。この補正過去速度Ｖ^ｔ（ｉ） _ｃｏは、下記式（１４）式から算出する。

速度初期値Ｖ^０は精度のよい速度であるとみなすことができる。したがって、（１４）式において、速度初期値Ｖ^０とその速度初期値Ｖ^０に更新された時点における補正前過去推定速度Ｖ^ｔ（０） _ｅとの差分（Ｖ^０−Ｖ^ｔ（０） _ｅ）は、速度初期値Ｖ^０を更新した時点までに補正前過去推定速度Ｖ^ｔ（０） _ｅに累積した誤差を意味する。

そして、（ｔ（ｉ）−ｔ（Ｎ））／（ｔ（０）−ｔ（Ｎ））は、前回、速度初期値Ｖ^０が更新されてから、今回、速度初期値Ｖ^０が更新されるまでの期間（ｔ（０）−ｔ（Ｎ））に対する、前回、速度初期値Ｖ^０が更新されてから、それぞれの補正前過去推定速度Ｖ^ｔ（ｉ） _ｅが推定されるまでの期間の比率を表す係数となっている。（１４）式では、差分（Ｖ^０−Ｖ^ｔ（０） _ｅ）にこの係数を乗じている。したがって、（１４）式から算出する補正過去速度Ｖ^ｔ（ｉ） _ｃｏは、補正前過去推定速度Ｖ^ｔ（ｉ） _ｅを、その補正前過去推定速度Ｖ^ｔ（ｉ） _ｅの誤差が線形に増加すると仮定して補正した値である。

図４は、補正前過去推定速度Ｖ^ｔ（ｉ） _ｅと補正過去速度Ｖ^ｔ（ｉ） _ｃｏとを比較して示す図である。この図４に示すように、ｔ（０）時点では、速度初期値Ｖ_０と補正前過去推定速度Ｖ^ｔ（０） _ｅとの間に差分（Ｖ^０−Ｖ^ｔ（０） _ｅ）がある。この差分は、推定速度Ｖ^ｔ _ｅに累積した誤差を意味する。速度初期値Ｖ^０を更新した時点では、推定速度Ｖ^ｔ _ｅ＝速度初期値Ｖ^０となる。したがって、推定速度Ｖ^ｔ _ｅは速度初期値Ｖ^０を更新した時点で不連続になる。

本実施形態では、（１４）式を用いて補正前過去推定速度Ｖ^ｔ（ｉ） _ｅを補正して補正過去速度Ｖ^ｔ（ｉ） _ｃｏとする。この補正過去速度Ｖ^ｔ（ｉ） _ｃｏは、図４に示すように、ｔ（０）以降の推定速度Ｖ^ｔ _ｅとの間に連続性がある。実際の車両の速度変化には連続性があることから、（１４）式を用いて算出した補正過去速度Ｖ^ｔ（ｉ） _ｃｏは、補正前過去推定速度Ｖ^ｔ（ｉ） _ｅよりも、より真の速度に近いと言える。

移動距離補正部１４３は、過去速度補正部１４２が補正過去速度Ｖ^ｔ（ｉ） _ｃｏを算出した場合、その補正過去速度Ｖ^ｔ（ｉ） _ｃｏを推定速度Ｖ^ｔ _ｅの代わりに用いて、移動距離算出部１４１と同様にして推定移動距離を再計算する。この推定移動距離を補正移動距離Ｌ^ｔ（ｉ） _ｃｏとする。

位置決定部１４４は、信号品質判定部１２０で信号品質がよいと判定されたＧＰＳ電波を用いて、現在位置算出部１１２が（５）式を用いて車両の現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を算出した場合には、現在位置算出部１１２が算出した現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を、更新後の現在位置とする。

現在位置算出部１１２が算出した現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を更新後の現在位置としない場合には、移動距離算出部１４１が算出した移動距離Ｌ^ｔ _ｅと、推定方位決定部１３０が逐次決定した推定方位θ^ｔ _ｅを用いて、現在位置（Ｘ_ｖ，Ｙ_ｖ）を逐次算出する。

具体的には、下記（１５）式、（１６）式を用いて、前回の現在位置（Ｘ_ｖ ^{ｔ（−１）}，Ｙ_ｖ ^{ｔ（−１）}）と、今回の推定移動距離Ｌ^ｔ（０） _ｅと、今回の推定方位θ^ｔ（０） _ｅとから、現時点ｔ（０）の現在位置（Ｘ_ｖ ^ｔ（０），Ｙ_ｖ ^ｔ（０））を算出する。なお、この現在位置（Ｘ_ｖ ^ｔ（０），Ｙ_ｖ ^ｔ（０））は、もちろん、前回の現在位置（Ｘ_ｖ ^{ｔ（−１）}，Ｙ_ｖ ^{ｔ（−１）}）に対する相対位置でもある。
Ｘ_ｖ ^ｔ（０）＝Ｘ_ｖ ^{ｔ（−１）}＋Ｌ^ｔ（０） _ｅ×ｃｏｓθ^ｔ（０） _ｅ （１５）
Ｙ_ｖ ^ｔ（０）＝Ｙ_ｖ ^{ｔ（−１）}＋Ｌ^ｔ（０） _ｅ×ｓｉｎθ^ｔ（０） _ｅ （１６）

さらに、位置決定部１４４は、移動距離補正部１４３が補正移動距離Ｌ^ｔ _ｃｏを算出した場合には、前回、速度初期値Ｖ^０が更新された時点Ｔ（Ｎ）から、今回、速度初期値Ｖ^０が更新される時点ｔ（０）までの位置（Ｘ_ｖ ^ｔ（ｉ），Ｙ_ｖ ^ｔ（ｉ））を逐次補正して、現在位置（Ｘ_ｖ ^ｔ（０），Ｙ_ｖ ^ｔ（０））を補正する。
Ｘ_ｖ ^ｔ（ｉ）＝Ｘ_ｖ ^{ｔ（ｉ−１）}＋Ｌ^ｔ（ｉ） _ｅ×ｃｏｓθ^ｔ（ｉ） _ｅ （１７）
Ｙ_ｖ ^ｔ（ｉ）＝Ｙ_ｖ ^{ｔ（ｉ−１）}＋Ｌ^ｔ（ｉ） _ｅ×ｓｉｎθ^ｔ（ｉ） _ｅ （１８）

＜制御部１００の処理の流れ＞
次に、制御部１００の処理の流れの一例を図５〜図８のフロチャートを用いて説明する。図５に示すフロチャートは、センサ値を取得する周期で繰り返し実行する。なお、特に明記している場合を除き、各パラメータは、最新の時刻、すなわち、時刻ｔ＝０における値を意味する。

図５において、ステップＳ２では、加速度センサ２０、ヨーレートセンサ３０の検出値を取得し、ＲＡＭなどの記憶部に記憶する。この処理は、たとえば積算処理部１０４が行う。

ステップＳ４は進行方向加速度決定部１０２が行う処理であり、ステップＳ２で取得した加速度センサ２０の検出値から、進行方向加速度を決定する。

ステップＳ６は積算処理部１０４が行う処理であり、ステップＳ２で取得したヨーレートセンサ３０の検出値を、これまでの相対方位角θ^{ｔ（−１）} _ｇｙｒｏに加算して相対方位角θ^ｔ _ｇｙｒｏを更新する。また、ステップＳ２で決定した進行方向加速度をこれまでの加速度積算値ΔＶ^{ｔ（−１）} _Ｇに加算して加速度積算値ΔＶ^ｔ _Ｇを更新する。

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

ステップＳ１２は、停止判定部１２２の処理であり、たとえば、ステップＳ２で取得した加速度センサ２０のｚ軸の検出値から、車両が停止しているか否かを判断する。停止中と判断した場合にはステップＳ１４へ進み、移動中であると判断した場合にはステップＳ１８へ進む。

ステップＳ１４も停止判定部１２２の処理であり、速度ベクトルを（０、０、０）とする。もちろん、速度も０となる。

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

ステップＳ１２において移動中と判断した場合に実行するステップＳ１８は信号品質判定部１２０の処理であり、前述した判定条件に基づいて、ＧＰＳ信号の信号品質がよいか否かを判定する。信号品質の判定は、取得したすべてのＧＰＳ信号に対して行う。

ステップＳ２０は速度ベクトル算出部１１８の処理であり、ステップＳ１８で信号品質がよいと判定したＧＰＳ信号の数が４以上であるか否かを判断する。４以上である場合にはステップＳ２２へ進む。

ステップＳ２２では、ドップラー速度を算出する。このドップラー速度とは、速度ベクトル算出部１１８が算出する速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）またはその速度ベクトルの大きさを意味する。この速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は、ドップラーシフト量Ｄを用いて算出することから、ここでは、ドップラー速度と称している。

ステップＳ２２の詳細処理は図６に示す。図６において、ステップＳ２２２は現在位置算出部１１２の処理であり、ステップＳ１０で算出した擬似距離ρｉ、ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）から、車両の現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を算出する。

ステップＳ２２４は衛星方向算出部１１４が行う処理であり、ステップＳ２２２で算出した車両の現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）、および、ステップＳ１０で算出したＧＰＳ衛星の位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）から、各ＧＰＳ衛星ｉの方向Ｒ_ｉ（θ_ｉ、φ_ｉ）を算出する。

ステップＳ２２６は衛星速度算出部１１０が行う処理であり、ステップＳ１０で算出した各ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）の時系列データから、各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓ_ｉ、Ｖｙｓ_ｉ、Ｖｚｓ_ｉ）を算出する。

ステップＳ２２８、Ｓ２３０は速度ベクトル算出部１１８の処理である。ステップＳ２２８では、ステップＳ２２４で算出した各ＧＰＳ衛星ｉの方向Ｒ_ｉと、ステップＳ２２６で算出した各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓ_ｉ、Ｖｙｓ_ｉ、Ｖｚｓ_ｉ）から、Ｖｓａｔ_ｉ＝Ｒ_ｉ［Ｖｘｓ_ｉ，Ｖｙｓ_ｉ，Ｖｚｓ_ｉ］^Ｔにより、車両方向のＧＰＳ衛星ｉの速度Ｖｓａｔ_ｉを求める。

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

説明を図５に戻す。ステップＳ２４は初期設定値決定部１２６の処理であり、ステップＳ２２で算出した車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）から、（１０）式により、速度初期値Ｖ^０を決定する。ステップＳ２４を実行した場合にも、ステップＳ３４の位置更新処理を実行する。ステップＳ３４を説明する前に、先に、ステップＳ２６以下を説明する。

ステップＳ２０において品質のよい信号が３以下であると判断した場合にはステップＳ２６へ進む。ステップＳ２６は初期設定値決定部１２６の処理であり、初期値を更新することができるか否かを判断する。この判断は、具体的には、速度初期値Ｖ^０を更新してからのＧＰＳ信号を用いて、（１２）式を４つ以上、立式できるか否かを判断するものである。この判断がＮＯであればステップＳ３０へ進み、ＹＥＳであればステップＳ２８へ進む。

ステップＳ２８では、タイトカップリング型推定式を用いて速度初期値Ｖ^０および方位初期値θ^０を決定する。タイトカップリング型推定式とは、具体的には（１２）式のことである。

ステップＳ２８の詳細処理は図７に示す。図７において、ステップＳ２８２は衛星速度算出部１１０が行う処理であり、ステップＳ１０で算出した各ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）の時系列データから、各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓ_ｉ、Ｖｙｓ_ｉ、Ｖｚｓ_ｉ）を算出する。

ステップＳ２８４は相対速度算出部１０８が行う処理であり、ステップＳ１０で算出したドップラーシフト量Ｄ_ｉを前述した（２）式に代入して、ＧＰＳ衛星ｉに対する車両の相対速度Ｖｒ_ｉを算出する。

ステップＳ２８６は現在位置算出部１１２の処理であり、ステップＳ１０で算出した擬似距離ρｉ、ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）から、車両の現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を算出する。

ステップＳ２８８は視線ベクトル算出部１１３が行う処理である。このステップＳ２８８では、前述した（６）式に、ステップＳ１０で算出した擬似距離ρ_ｉ、ＧＰＳ衛星ｉの位置座標（Ｘ_ｓｉ、Ｙ_ｓｉ、Ｚ_ｓｉ）、ステップＳ２８６で算出した車両の現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を代入して、視線ベクトル（Ｇｘ_ｉ，Ｇｙ_ｉ，Ｇｚ_ｉ）を算出する。

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

ステップＳ２９２は初期設定値決定部１２６が行う処理である。このステップＳ２９２では、前述した（１２）式に、ステップＳ２９０で算出した衛星方向速度Ｖｓ_ｉ、ステップＳ６で更新した加速度積算値ΔＶ_Ｇ、相対方位角θ_ｇｙｒｏ、ステップＳ２８８で算出した視線ベクトル（Ｖｘｓ_ｉ、Ｖｙｓ_ｉ、Ｖｚｓ_ｉ）を代入した式を４つ以上立式する。そして、それら４つ以上の式からなる連立方程式を解く。これにより、（１２）式において未知パラメータとなっている速度初期値Ｖ^０、方位初期値θ^０を同時に求めることができる。

説明を図５に戻す。ステップＳ２８を実行した場合、または、ステップＳ２６の判断がＮＯであった場合には、ステップＳ３０を実行する。ステップＳ３０は推定速度決定部１２８が行う処理であり、最新の速度初期値Ｖ^０と、ステップＳ６で更新した加速度積算値ΔＶ_Ｇとから、推定速度Ｖ_ｅを算出する。

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

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

ステップＳ３４の詳細処理は図８に示す。なお、図８において、ステップＳ３４８は移動距離算出部１４１が実行し、ステップＳ３５２は過去速度補正部１４２が実行し、ステップＳ３５４は移動距離補正部１４３が実行する。その他のステップは位置決定部１４４が実行する。

ステップＳ３４２では、ＧＰＳ現在位置を決定したか否かを判断する。ＧＰＳ現在位置とは、信号品質判定部１２０で信号品質がよいと判定されたＧＰＳ電波を用いて、現在位置算出部１１２が（５）式から算出した現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）のことである。この判断がＹＥＳであれば、ステップＳ３４４に進む。ステップＳ３４４では、ＧＰＳ現在位置（Ｘ_ｖ，Ｙ_ｖ，Ｚ_ｖ）を、最新の現在位置とする。

ステップＳ３４２の判断がＮＯである場合には、ステップＳ３４６に進む。ステップＳ３４６では、速度初期値Ｖ^０を更新したか否かを判断する。図５のステップＳ１６、Ｓ２４、Ｓ２８のいずれかを実行している場合には、このステップＳ３４６の判断がＹＥＳになる。これに対して、図５のステップＳ２６の判断がＮＯであった場合には、ステップＳ３４６の判断もＮＯになる。ステップＳ３４６の判断がＮＯである場合には、ステップＳ３４８に進む。

ステップＳ３４８では、ステップＳ３０で算出した推定速度Ｖ_ｅに加速度取得周期を乗じて推定移動距離Ｌ_ｅを算出する。ステップＳ３５０では、ステップＳ３４８で算出した推定移動距離Ｌ_ｅと、ステップＳ３２で推定した推定方位θ_ｅと、式１５、式１６を用いて、最新の現在位置（Ｘ_ｖ，Ｙ_ｖ）を算出する。なお、ｚ座標は変化なしとする。

ステップＳ３４６の判断がＹＥＳであった場合に実行するステップＳ３５２では、前回、速度初期値Ｖ^０を更新した時点ｔ（Ｎ）から、今回、速度初期値Ｖ^０を更新した時点ｔ（０）までの推定速度Ｖ^ｔ _ｅ、すなわち、補正前過去推定速度Ｖ^ｔ（ｉ） _ｅを、（１４）式で補正して、補正過去速度Ｖ^ｔ（ｉ） _ｃｏを算出する。

ステップ３５４では、ステップＳ３５２で算出した補正過去速度Ｖ^ｔ（ｉ） _ｃｏに加速度取得周期を乗じて、補正移動距離Ｌ^ｔ（ｉ） _ｃｏを算出する。

ステップＳ３５６では、ステップＳ３５４で算出した補正移動距離Ｌ^ｔ（ｉ） _ｃｏと、ステップＳ３２で推定した推定方位θ^ｔ（ｉ） _ｅと、式１７、式１８を用いて、現在位置（Ｘ_ｖ，Ｙ_ｖ）を再算出する。なお、ｚ座標は変化なしとする。

＜実施形態の効果＞
本実施形態では、ドップラー速度を算出した場合（Ｓ２２）には、そのドップラー速度を速度初期値Ｖ^０としており（Ｓ２４）、車両の速度が０ｋｍ／ｈである場合（Ｓ１４）には、その速度を速度初期値Ｖ^０としている（Ｓ１６）。また、タイトカップリング型推定式である（１２）式を解くことができる場合、（１２）式から求まる初期時刻における速度を速度初期値Ｖ^０としている（Ｓ２８）。これらの方法で決定する速度初期値Ｖ^０は精度がよい。

そして、推定速度Ｖ_ｅは、このようにして決定した速度初期値Ｖ^０と加速度積算値ΔＶ_Ｇとを用いて推定している（Ｓ３０）。したがって、速度初期値Ｖ^０を更新した以後は、加速度積算値ΔＶ_Ｇの累積誤差により低下している推定速度Ｖ_ｅの精度が向上する。

加えて、本実施形態では、速度初期値Ｖ^０を決定した場合、前回、速度初期値Ｖ^０を更新した時点ｔ（Ｎ）から、今回、速度初期値Ｖ^０を更新する時点ｔ（０）までの推定速度Ｖ_ｅである補正前過去推定速度Ｖ^ｔ（ｉ） _ｅを、今回の速度初期値Ｖ^０と連続するように補正して補正過去速度Ｖ^ｔ（ｉ） _ｃｏとする（Ｓ３５２）。これにより、加速度積算値ΔＶ_Ｇの累積誤差により低下している過去の推定速度Ｖ_ｅの精度も向上する。そして、補正過去速度Ｖ^ｔ（ｉ） _ｃｏを決定した場合、その補正過去速度Ｖ^ｔ（ｉ） _ｃｏを用いて、前回、速度初期値Ｖ^０を更新してから、今回、速度初期値Ｖ^０を更新するまでの推定移動距離を再計算した補正移動距離Ｌ^ｔ（ｉ） _ｃｏを決定する（Ｓ３５４）。これにより、過去の推定速度Ｖ_ｅの精度低下により低下していた推定移動距離の精度も向上する。

また、本実施形態では、補正移動距離Ｌ^ｔ（ｉ）を算出した場合には、その補正移動距離Ｌ^ｔ（ｉ）を用いて、現在位置を補正する（Ｓ３５６）。したがって、ＧＰＳ現在位置が決定できない状況が継続している状況でも、現在位置を精度よく決定し続けることができる。

このように、速度初期値Ｖ^０の更新時に、過去の推定速度である補正前過去推定速度Ｖ^ｔ（ｉ） _ｅを補正して、現在位置を補正することに加えて、本実施形態では、以下の効果も得られる。

車両の衛星方向速度Ｖｓ_ｉは、（１３）式に示したように、車両からＧＰＳ衛星ｉへの視線ベクトル（Ｇｘ，Ｇｙ，Ｇｚ）と、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）と、クロックドリフトＣｂｖに基づいて算出することができる。

本実施形態では、この（１３）式に対して、速度ベクトルのｘ成分、ｙ成分を、車両の速度、および車両の進行方向の方位角の時間変化すなわち相対方位角θ_ｇｙｒｏで拘束している（拘束条件１）。初期設定値決定部１２６は、これにより得られる（１２）式を用いて速度初期値Ｖ^０を決定する。

このように（１２）式は時間変化に関する条件で拘束されていることから、異なる複数の観測時点のＧＰＳ信号を用いた（１２）式を連立方程式とすることができる。（１２）式には、速度初期値Ｖ^０以外にも未知パラメータがあるため、未知パラメータの数に相当する複数の（１２）式を連立方程式とする必要がある。しかし、本実施形態では、異なる複数の観測時点のＧＰＳ信号を用いた（１２）式を連立方程式とすることができるので、未知パラメータを解くことができる数の（１２）式からなる連立方程式を立式しやすい。

よって、初期設定値決定部１２６は、速度初期値Ｖ^０を高頻度に求めることができる。そして、速度初期値Ｖ^０を高頻度に更新することで、加速度センサ２０の検出値からドリフトの影響を除去する頻度が高くなるので、加速度積算値ΔＶ_Ｇと速度初期値Ｖ^０から算出する推定速度Ｖ_ｅの推定精度が向上する。従って、推定速度Ｖ_ｅを用いて決定する現在位置の精度も向上する。

また、本実施形態では、拘束条件１に加えて、クロックドリフトの時間変化が線形であるとする拘束条件３も用いて（１２）式を導出している。これにより、（１２）式における未知パラメータの数をより少なくすることができる。その結果、未知パラメータを解く数の連立方程式を立式しやすくなるので、速度初期値Ｖ^０の更新頻度をさらに高くすることができる。

また、（１２）式は、車両の速度の項として、速度初期値Ｖ^０と加速度積算値ΔＶ_Ｇの項とを含んでいる。よって、未知パラメータの数の（１２）式からなる連立方程式を解くことで、直接、速度初期値Ｖ^０を得ることができる。

また、本実施形態では、加速度積算値ΔＶ_Ｇを用いないで速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出する速度ベクトル算出部１１８を備える。この速度ベクトル算出部１１８が速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出した場合、速度ベクトル算出部１１８が算出した速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を用いて速度初期値Ｖ_０を決定する（Ｓ２４）。速度ベクトル算出部１１８が算出した速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は精度がよいことから、このようにすれば、速度初期値Ｖ^０の精度が向上する。

また、本実施形態では、停止判定部１２２が、車両が停止していると判定したときは、車両の速度がゼロであるとして、速度初期値Ｖ^０を決定する（Ｓ１６）。これによっても、速度初期値Ｖ^０の精度が向上する。そして、速度初期値Ｖ^０の精度が向上する結果、速度初期値Ｖ^０と加速度積算値ΔＶ_Ｇから算出する推定速度Ｖｅの精度も向上する。

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

＜変形例１＞
前述の実施形態では、拘束条件３により、時間変化が線形であるという条件でクロックドリフトを拘束していたが、この拘束条件３をなしにしてもよい。拘束条件３をなしにする場合、（１２）式におけるＣｂｖ^０＋Ａｔを、未知パラメータであるＣｂｖ^ｔに置き換えることになる。

＜変形例２＞
また、拘束条件３は残し、拘束条件１における方位角の時間変化による拘束をなしにしてもよい。この場合には、拘束条件１が、速度ベクトルの大きさを車両の速度で拘束したのみの条件となり、（１２）式におけるθ^ｔ _ｇｙｒｏが未知パラメータになる。

＜変形例３＞
前述の実施形態では、衛星測位システムとしてＧＰＳを利用していたが、その他の衛星測位システムを利用してもよい。また、ＧＰＳが備える衛星と、とその他の衛星測位システムが備える衛星を両方用いてもよい。

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

１：速度推定装置、 １０：ＧＰＳ信号受信部、 ２０：加速度センサ、 ３０：ヨーレートセンサ、 １００：制御部、 １０２：進行方向加速度決定部、 １０４：積算処理部、 １０６：衛星情報取得部、 １０８：相対速度算出部、 １１０：衛星速度算出部、 １１２：現在位置算出部、 １１３：視線ベクトル算出部、 １１４：衛星方向算出部、 １１６：衛星方向速度算出部、 １１８：速度ベクトル算出部、 １２０：信号品質判定部、 １２２：停止判定部
１２６：初期設定値決定部、 １２８：推定速度決定部、 １３０：推定方位決定部、 １４０：位置更新部、 １４１：移動距離算出部、 １４２：過去速度補正部、 １４３：移動距離補正部、 １４４：位置決定部

Claims (8)

1. 移動体とともに移動する加速度センサ（２０）の検出値の進行方向成分である進行方向加速度を逐次決定する進行方向加速度決定部（１０２）と、
   前記進行方向加速度決定部が決定した進行方向加速度を積算した加速度積算値を逐次算出する積算処理部（１０４）と、
   衛星測位システムが備える衛星からのドップラーシフト量に基づいて算出する前記移動体の速度、または、前記移動体の停止時の速度を、前記移動体の速度初期値として決定する初期設定値決定部（１２６）と、
   前記積算処理部が算出した加速度積算値と、前記初期設定値決定部が決定した速度初期値とに基づいて、前記移動体の推定速度を逐次推定する推定速度決定部（１２８）と、
   前記推定速度決定部が決定した前記推定速度と時間とに基づいて前記移動体の推定移動距離を逐次算出する移動距離算出部（１４１）と、
   前記初期設定値決定部が前記速度初期値を決定した場合、前回、前記初期設定値決定部が前記速度初期値を決定してから、今回、初期設定値決定部が前記速度初期値を決定するまでに前記推定速度決定部が逐次推定した推定速度である補正前過去推定速度を、今回、初期設定値決定部が決定した前記速度初期値と連続するように補正した補正過去速度を決定する過去速度補正部（１４２）と、
   前記過去速度補正部が前記補正過去速度を決定した場合に、前記補正過去速度と時間とに基づいて、前回、前記初期設定値決定部が前記速度初期値を決定してから、今回、初期設定値決定部が前記速度初期値を決定するまでの前記推定移動距離を再計算する移動距離補正部（１４３）と、を備えることを特徴とする移動距離推定装置。
2. 請求項１において、
   前記移動体の移動方位を逐次推定する推定方位決定部（１３０）と、
   前記移動距離算出部が算出した前記推定移動距離と、前記推定方位決定部が決定した移動方位とに基づいて、前回の位置決定時に対する前記移動体の相対位置を逐次決定する位置決定部（１４４）とを備え、
   前記位置決定部は、前記移動距離補正部が前記推定移動距離を再計算した場合には、再計算した前記推定移動距離と、前記推定方位決定部が決定した移動方位とに基づいて、前回、前記初期設定値決定部が前記速度初期値を決定してから、今回、初期設定値決定部が前記速度初期値を決定するまでの前記移動体の前記相対位置を再計算することを特徴とする移動距離推定装置。
3. 請求項１または２において、
   前記過去速度補正部は、
   前記補正前過去推定速度を、その補正前過去推定速度の誤差が線形に増加するとして、
   今回、前記初期設定値決定部が決定した速度初期値と最新の前記補正前過去推定速度との差分に、前回、前記初期設定値決定部が前記速度初期値を決定してから、それぞれの前記補正前過去推定速度を推定するまでの期間に応じた係数を乗じた補正量で、それぞれの前記補正前過去推定速度を補正して、前記補正過去速度を決定することを特徴とする移動距離推定装置。
4. 請求項１〜３のいずれか１項において、
   前記積算処理部は、前記加速度積算値を逐次算出するとともに、移動体のヨーレートを検出するヨーレートセンサ（３０）が検出したヨーレートを積算することで、基準時点における進行方向に対する相対方位角を逐次算出し、
   衛星測位システムが備える衛星から衛星信号を取得する衛星信号取得部（１０６）と、
   前記衛星からの電波のドップラーシフト量に基づいて、前記移動体の速度の前記衛星の方向への成分である衛星方向速度を算出する衛星方向速度算出部（１１６）と、
   前記衛星信号に基づいて、前記移動体から前記衛星への視線ベクトルを算出する視線ベクトル算出部（１１３）とを備え、
   前記初期設定値決定部は、
   前記衛星測位システムが備える衛星からの電波のドップラーシフト量に基づいて算出する前記移動体の速度を、前記速度初期値として決定するようになっており、
   前記衛星方向速度と、前記視線ベクトルと、前記移動体の速度ベクトルとの関係を示す式に対して、前記速度ベクトルの大きさを前記移動体の速度で拘束し、かつ、前記速度ベクトルの向きの時間変化を前記移動体の進行方向の方位角の時間変化で拘束して得られる式であって、前記衛星方向速度、前記視線ベクトル、前記移動体の速度、前記移動体の進行方位の関係を示す速度推定式と、
   前記衛星方向速度算出部が算出した衛星方向速度、前記視線ベクトル算出部が算出した視線ベクトル、前記積算処理部が算出した相対方位角から、前記速度初期値を決定することを特徴とする移動距離推定装置。
5. 請求項１〜３のいずれか１項において、
   衛星測位システムが備える衛星から衛星信号を取得する衛星信号取得部（１０６）と、
   前記衛星からの電波のドップラーシフト量に基づいて、前記移動体の速度の前記衛星の方向への成分である衛星方向速度を算出する衛星方向速度算出部（１１６）と、
   前記衛星信号に基づいて、前記移動体から前記衛星への視線ベクトルを算出する視線ベクトル算出部（１１３）とを備え、
   前記初期設定値決定部は、
   前記衛星測位システムが備える衛星からの電波のドップラーシフト量に基づいて算出する前記移動体の速度を、前記速度初期値として決定するようになっており、
   前記衛星方向速度と、前記視線ベクトルと、前記移動体の速度ベクトルと、クロックドリフトとの関係を示す式に対して、前記速度ベクトルの大きさを前記移動体の速度で拘束し、前記クロックドリフトは時間変化が線形であるという拘束条件で拘束して得られる式であって、前記衛星方向速度、前記視線ベクトル、前記移動体の速度、前記移動体の進行方位、クロックドリフトの関係を示す速度推定式と、
   前記衛星方向速度算出部が算出した衛星方向速度、前記視線ベクトル算出部が算出した視線ベクトルから、前記移動体の速度初期値を決定することを特徴とする移動距離推定装置。
6. 請求項４において、
   前記速度推定式は、前記衛星方向速度、前記視線ベクトル、前記移動体の速度、前記移動体の進行方位に加えて、クロックドリフトを含む式であることを特徴とする移動距離推定装置。
7. 請求項６において、
   前記速度推定式は、前記クロックドリフトの時間変化が線形であるという条件でも拘束されていることを特徴とする移動距離推定装置。
8. 請求項４〜７のいずれか１項において、
   前記速度推定式は、前記移動体の速度の項として、前記速度初期値の項と、前記加速度積算値の項とを含んでおり、
   前記初期設定値決定部は、前記速度推定式を用いて前記速度初期値を決定する場合、前記速度推定式、前記衛星方向速度算出部が算出した衛星方向速度、前記視線ベクトル算出部が算出した前記視線ベクトルに加えて、前記積算処理部が算出した加速度積算値に基づいて、前記速度初期値を決定することを特徴とする移動距離推定装置。

Images