JP6201762B2 - 速度推定装置 - Google Patents

Description

本発明は、速度推定装置に関し、特に、速度推定精度を向上させる技術に関する。

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

加速度から算出した速度は、短周期で更新できるが、ドップラー速度よりも誤差が生じやすい。そのため、ドップラー速度により、加速度から算出した速度を補正するのである。また、ドップラー速度の良否判定も行い、良否判定の結果に基づいて、ドップラー速度により、加速度から算出した速度を補正する程度を調整している。

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

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

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

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

広く知られているようにセンサにはドリフトという現象があるため、センサ検出値の誤差は時間とともに発散する。そこで、特許文献１の技術は、加速度から算出した速度をドップラー速度により補正している。さらに、ドップラー速度は、マルチパスの影響等により精度が低下することがあるので、特許文献１では、ドップラー速度の良否を判定している。そして、その良否に応じた係数により補正の程度を調整している。

しかし、ドップラー速度の良否判定に、加速度センサの検出値を用いているため、判定の精度が十分によいとは言えない。また、信号品質のよいＧＰＳ電波を複数のＧＰＳ衛星から受信できないときは、ドップラー速度の精度が低下する。その結果、良否に応じた係数によって補正しても、加速度から算出した速度の精度はまだ十分に高いとは言えなかった。

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

上記目的は独立請求項に記載の特徴の組み合わせにより達成され、また、下位請求項は、発明の更なる有利な具体例を規定する。

上記目的を達成するための第１発明は、移動体とともに移動する加速度センサ（２０）と、移動体のヨーレートを検出するヨーレートセンサ（３０）と、加速度センサの検出値の進行方向成分である進行方向加速度を逐次決定する進行方向加速度決定部（１０２）と、進行方向加速度決定部が決定した進行方向加速度を積算した加速度積算値を逐次算出するとともに、ヨーレートセンサが検出したヨーレートを積算することで、基準時点における進行方向に対する相対方位角を逐次算出する積算処理部（１０４）と、衛星測位システムが備える衛星から衛星信号を取得する衛星信号取得部（１０６）と、衛星信号に基づいて、移動体の速度の衛星の方向への成分である衛星方向速度を算出する衛星方向速度算出部（１１６）と、衛星信号に基づいて、移動体から衛星への視線ベクトルを算出する視線ベクトル算出部（１１３）と、衛星方向速度、視線ベクトル、移動体の速度、移動体の進行方位の関係を示す速度推定式と、衛星方向速度算出部が算出した衛星方向速度、視線ベクトル算出部が算出した視線ベクトル、積算処理部が算出した相対方位角から、移動体の速度初期値を決定する初期設定値決定部（１２６）と、積算処理部が算出した加速度積算値と、初期設定値決定部が決定した速度初期値とに基づいて、移動体の速度を推定する推定速度決定部（１２８）とを備え、速度推定式は、衛星方向速度と、視線ベクトルと、移動体の速度ベクトルとの関係を示す式に対して、速度ベクトルの大きさを移動体の速度で拘束し、かつ、速度ベクトルの向きの時間変化を移動体の進行方向の方位角の時間変化で拘束した式であることを特徴とする速度推定装置である。

移動体の衛星方向速度は、移動体から衛星への視線ベクトルと、移動体の速度ベクトルとに基づいて算出することができる。すなわち、衛星方向速度と、視線ベクトルと、移動体の速度ベクトルとの関係を示す式が成立する。本発明では、この式に対して、速度ベクトルを、移動体の速度および移動体の進行方向の方位角の時間変化で拘束することで得られる速度推定式を用いて、速度初期値を決定する。

速度推定式は、移動体の進行方向の方位角の時間変化で拘束されていることから、異なる複数の観測時点の衛星信号を用いた速度推定式を連立方程式とすることができる。したがって、速度推定式に移動体の速度を含む複数の未知パラメータがあっても、未知パラメータを解くことができる数の速度推定式からなる連立方程式を立式しやすい。よって、速度推定式から移動体の速度を求めやすくなるので、初期設定値決定部は、速度初期値を高頻度に求めることができる。そして、速度初期値を高頻度に更新することで、加速度センサの検出値からドリフトの影響を除去する頻度を高くすることができるので、加速度積算値と速度初期値から推定する速度の推定精度が向上する。

また、上記目的を達成するための第２発明は、移動体とともに移動する加速度センサ（２０）と、加速度センサの検出値の進行方向成分である進行方向加速度を逐次決定する進行方向加速度決定部（１０２）と、進行方向加速度決定部が決定した進行方向加速度を積算した加速度積算値を逐次算出する積算処理部（１０４）と、衛星測位システムが備える衛星から衛星信号を取得する衛星信号取得部（１０６）と、衛星信号に基づいて、移動体の速度の衛星の方向への成分である衛星方向速度を算出する衛星方向速度算出部（１１６）と、衛星信号に基づいて、移動体から衛星への視線ベクトルを算出する視線ベクトル算出部（１１３）と、衛星方向速度、視線ベクトル、移動体の速度、移動体の進行方位、クロックドリフトの関係を示す速度推定式と、衛星方向速度算出部が算出した衛星方向速度、視線ベクトル算出部が算出した視線ベクトルから、移動体の速度初期値を決定する初期設定値決定部（１２６）と、積算処理部が算出した加速度積算値と、初期設定値決定部が決定した速度初期値とに基づいて、移動体の速度を推定する推定速度決定部（１２８）とを備え、速度推定式は、衛星方向速度と、視線ベクトルと、移動体の速度ベクトルと、クロックドリフトとの関係を示す式に対して、速度ベクトルの大きさを移動体の速度で拘束し、クロックドリフトは時間変化が線形であるという拘束条件で拘束した式であることを特徴とする速度推定装置である。

特許文献２にも記載されているように、衛星方向速度と、視線ベクトルと、移動体の速度ベクトルと、クロックドリフトとの関係を示す式が成立する。本発明では、この式に対して、速度ベクトルの大きさを移動体の速度で拘束し、クロックドリフトの時間変化が線形であるという拘束条件で拘束することで得られる速度推定式を用いて、速度初期値を決定する。

速度推定式が、クロックドリフトの時間変化に関する条件で拘束されていることから、異なる複数の観測時点の衛星信号を用いた速度推定式を連立方程式とすることができる。したがって、速度推定式に移動体の速度を含む複数の未知パラメータがあっても、未知パラメータを解くことができる数の速度推定式からなる連立方程式を立式しやすい。よって、速度推定式から移動体の速度を求めやすくなるので、初期設定値決定部は、速度初期値を高頻度に求めることができる。そして、速度初期値を高頻度に更新することで、加速度センサの検出値からドリフトの影響を除去する頻度を高くすることができるので、加速度積算値と速度初期値から推定する速度の推定精度が向上する。

なお、特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、本発明の技術的範囲を限定するものではない。

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

＜実施形態の構成＞
以下、本発明の実施形態を図面に基づいて説明する。図１に示すように、本実施形態の速度推定装置１は、ＧＰＳ信号受信部１０、加速度センサ２０、ヨーレートセンサ３０、制御部１００を備える。この速度推定装置１は、図示しない車両に、予め定められた向きで固定される。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

上記（９）式は、ドップラーシフト量Ｄを含んだ式であることから、この速度ベクトル算出部１１８は、ドップラーシフト量に基づいて車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出している。よって、速度ベクトル算出部１１８は、請求項のドップラー速度算出部に相当する。

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

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

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

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

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

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

これに対して、加速度センサ２０の検出値は、たとえば２０ｍｓごとなど、速度ベクトル算出部１１８が速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出するよりも短い周期で取得する構成とすることができる。そこで、速度ベクトル算出部１１８が車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出してから、次に、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出するまでの間は、加速度積算値ΔＶ_Ｇをもとにして速度推定を行う。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

ただし、速度ベクトル算出部１１８が速度ベクトルを算出したとき、および、停止判定部１２２が、車両は停止していると判定したときは、推定速度Ｖ^ｔ _ｅは算出しない。速度ベクトル算出部１１８が速度ベクトルを算出したときに推定速度Ｖ^ｔ _ｅを算出しないのは、速度ベクトル算出部１１８が算出した速度ベクトルのほうが精度がよいからである。停止判定部１２２が、車両が停止していると判定したときに推定速度Ｖ^ｔ _ｅを算出しないのは、このときは、速度は計算しなくても０ｋｍ／ｈだからである。

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

速度ベクトル推定部１３２は、推定速度決定部１２８が決定した推定速度Ｖ^ｔ _ｅと、推定方位決定部１３０が決定した推定方位θ^ｔ _ｅとから、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を求める。推定方位θ^ｔ _ｅに向かう速度が推定速度Ｖ^ｔ _ｅであることから、この推定速度をｘ、ｙ、ｚ成分に分けることで、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を求めることができる。この速度ベクトル推定部１３２で推定した速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は、たとえば、ＧＰＳ信号を用いた車両の現在位置決定時から、次のＧＰＳ信号を用いた車両の現在位置決定時までの間の車両位置推定に用いる。

なお、停止判定部１２２が、車両が停止していると判定した場合には、速度ベクトル推定部１３２は、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を（０，０，０）とする。

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

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

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

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

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

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

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

ステップＳ１６は初期設定値決定部１２６の処理であり、速度初期値Ｖ^０を、最新の加速度積算値−ΔＶ^ｔ _Ｇとする。

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

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

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

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

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

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

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

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

説明を図３に戻す。ステップＳ２４は初期設定値決定部１２６の処理であり、ステップＳ２２で算出した車両の速度ベクトル（Ｖｘ^ｔ，Ｖｙ^ｔ，Ｖｚ^ｔ）から、（１０）式により、速度初期値Ｖ^０を決定する。

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

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

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

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

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

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

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

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

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

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

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

ステップＳ３４は速度ベクトル推定部１３２が行う処理であり、ステップＳ３０で推定した推定速度Ｖ_ｅと、ステップＳ３４で推定した推定方位θ_ｅとから、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を決定する。これにより、ステップＳ２２でドップラー速度が算出できない場合でも、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を決定することができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Claims (7)

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

Images