(57)【要約】【課題】高精度に速度を推定することができる速度推定装置を提供する。【解決手段】  衛星方向速度、視線ベクトル、移動体の速度、移動体の進行方位の関係を示す速度推定式と、衛星方向速度算出部１１６が算出した衛星方向速度Ｖｓ、視線ベクトル算出部１１３が算出した視線ベクトル（Ｇｘ，Ｇｙ，Ｇｚ）、積算処理部１０４が算出した加速度積算値ΔＶＧ、相対方位角θｇｙｒｏから、移動体の速度初期値Ｖ０を決定する初期設定値決定部１２６と、加速度積算値ΔＶＧと速度初期値Ｖ０とに基づいて、移動体の速度を推定する推定速度決定部１２８とを備える。速度推定式は、衛星方向速度Ｖｓと、視線ベクトル（Ｇｘ，Ｇｙ，Ｇｚ）と、移動体の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）との関係を示す式に対して、速度ベクトルの大きさを移動体の速度で拘束し、かつ、速度ベクトルの向きの時間変化を移動体の進行方向の方位角の時間変化で拘束した式である。【選択図】図１
2015129714.tif 000002
【特許請求の範囲】
【請求項１】
移動体とともに移動する加速度センサ（２０）と、
前記移動体のヨーレートを検出するヨーレートセンサ（３０）と、
前記加速度センサの検出値の進行方向成分である進行方向加速度を逐次決定する進行方向加速度決定部（１０２）と、
前記進行方向加速度決定部が決定した進行方向加速度を積算した加速度積算値を逐次算出するとともに、前記ヨーレートセンサが検出したヨーレートを積算することで、基準時点における進行方向に対する相対方位角を逐次算出する積算処理部（１０４）と、
衛星測位システムが備える衛星から衛星信号を取得する衛星信号取得部（１０６）と、
前記衛星信号に基づいて、前記移動体の速度の前記衛星の方向への成分である衛星方向速度を算出する衛星方向速度算出部（１１６）と、
前記衛星信号に基づいて、前記移動体から前記衛星への視線ベクトルを算出する視線ベクトル算出部（１１３）と、
前記衛星方向速度、前記視線ベクトル、前記移動体の速度、前記移動体の進行方位の関係を示す速度推定式と、前記衛星方向速度算出部が算出した衛星方向速度、前記視線ベクトル算出部が算出した視線ベクトル、前記積算処理部が算出した相対方位角から、前記移動体の速度初期値を決定する初期設定値決定部（１２６）と、
前記積算処理部が算出した加速度積算値と、前記初期設定値決定部が決定した速度初期値とに基づいて、前記移動体の速度を推定する推定速度決定部（１２８）とを備え、
前記速度推定式は、
前記衛星方向速度と、前記視線ベクトルと、前記移動体の速度ベクトルとの関係を示す式に対して、前記速度ベクトルの大きさを前記移動体の速度で拘束し、かつ、前記速度ベクトルの向きの時間変化を前記移動体の進行方向の方位角の時間変化で拘束した式であることを特徴とする速度推定装置。
【請求項２】
移動体とともに移動する加速度センサ（２０）と、
前記加速度センサの検出値の進行方向成分である進行方向加速度を逐次決定する進行方向加速度決定部（１０２）と、
前記進行方向加速度決定部が決定した進行方向加速度を積算した加速度積算値を逐次算出する積算処理部（１０４）と、
衛星測位システムが備える衛星から衛星信号を取得する衛星信号取得部（１０６）と、
前記衛星信号に基づいて、前記移動体の速度の前記衛星の方向への成分である衛星方向速度を算出する衛星方向速度算出部（１１６）と、
前記衛星信号に基づいて、前記移動体から前記衛星への視線ベクトルを算出する視線ベクトル算出部（１１３）と、
前記衛星方向速度、前記視線ベクトル、前記移動体の速度、前記移動体の進行方位、クロックドリフトの関係を示す速度推定式と、前記衛星方向速度算出部が算出した衛星方向速度、前記視線ベクトル算出部が算出した視線ベクトルから、前記移動体の速度初期値を決定する初期設定値決定部（１２６）と、
前記積算処理部が算出した加速度積算値と、前記初期設定値決定部が決定した速度初期値とに基づいて、前記移動体の速度を推定する推定速度決定部（１２８）とを備え、
前記速度推定式は、
前記衛星方向速度と、前記視線ベクトルと、前記移動体の速度ベクトルと、前記クロックドリフトとの関係を示す式に対して、前記速度ベクトルの大きさを前記移動体の速度で拘束し、前記クロックドリフトは時間変化が線形であるという拘束条件で拘束した式であることを特徴とする速度推定装置。
【請求項３】
請求項１において、
前記速度推定式は、前記衛星方向速度、前記視線ベクトル、前記移動体の速度、前記移動体の進行方位に加えて、クロックドリフトを含む式であることを特徴とする速度推定装置。
【請求項４】
請求項３において、
前記速度推定式は、前記クロックドリフトの時間変化が線形であるという条件でも拘束されていることを特徴とする速度推定装置。
【請求項５】
請求項１〜４のいずれか１項において、
前記速度推定式は、前記移動体の速度の項として、前記速度初期値の項と、前記加速度積算値の項とを含んでおり、
前記初期設定値決定部は、前記速度推定式、前記衛星方向速度算出部が算出した衛星方向速度、前記視線ベクトル算出部が算出した前記視線ベクトルに加えて、前記積算処理部が算出した加速度積算値に基づいて、前記速度初期値を決定することを特徴とする速度推定装置。
【請求項６】
請求項１〜５のいずれか１項において、
前記衛星信号を受信する信号受信部（１０）が受信した前記衛星信号の信号品質を判定する信号品質判定部（１２０）と、
前記信号品質判定部が信号品質がよいと判定した前記衛星からの電波のドップラーシフト量に基づいて、前記移動体の速度を算出するドップラー速度算出部（１１８）と、
を備え、
前記初期設定値決定部は、前記ドップラー速度算出部が前記移動体の速度を算出した場合、前記速度推定式を用いて前記速度初期値を決定することに代えて、前記ドップラー速度算出部が算出した前記移動体の速度に基づいて、前記速度初期値を決定することを特徴とする速度推定装置。
【請求項７】
請求項１〜６のいずれか１項において、
前記移動体が停止しているか否かを判定する停止判定部（１２２）を備え、
前記初期設定値決定部は、前記停止判定部が、前記移動体が停止していると判定した場合、前記移動体の速度がゼロであるとして、前記速度初期値を決定することを特徴とする速度推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、速度推定装置に関し、特に、速度推定精度を向上させる技術に関する。
【背景技術】
【０００２】
移動体の速度を推定する技術が種々知られている。たとえば特許文献１には、加速度を計測し、この加速度から速度を算出することに加え、ＧＰＳ搬送波のドップラーシフト量からも速度（以下、ドップラー速度）を算出する技術が開示されている。この特許文献１では、ドップラー速度により、加速度から算出した速度を補正している。
【０００３】
加速度から算出した速度は、短周期で更新できるが、ドップラー速度よりも誤差が生じやすい。そのため、ドップラー速度により、加速度から算出した速度を補正するのである。また、ドップラー速度の良否判定も行い、良否判定の結果に基づいて、ドップラー速度により、加速度から算出した速度を補正する程度を調整している。
【０００４】
特許文献２には、車両の速度ベクトルを精度よく推定する技術が開示されている。具体的には、特許文献２の技術では、速度ベクトルの推定に、衛星方向車速と、車輪速と、車両から測位システムの衛星への視線ベクトルと、車両方位と、クロックドリフトとの関係を示す式を用いる。この式は、衛星方向速度、視線ベクトル、速度ベクトル、クロックドリフトの関係を示す式を、拘束条件を用いて変形した式である。拘束条件は、速度ベクトルが移動体の速度およびヨーレートにより拘束されるという条件、および、クロックドリフトはその時間変化が線形であるという条件である。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１２−４２３１８号公報
【特許文献２】特開２０１３−１１３７８９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
特許文献２の技術は、車輪速が必要となる。そのため、特許文献２の技術は、車輪速センサと接続することができない速度推定装置には適用できないという問題があった。
【０００７】
これに対して、特許文献１の技術は、自装置内に備えている加速度センサが検出した加速度から速度を算出しているため、車輪速センサと接続する必要がない。
【０００８】
広く知られているようにセンサにはドリフトという現象があるため、センサ検出値の誤差は時間とともに発散する。そこで、特許文献１の技術は、加速度から算出した速度をドップラー速度により補正している。さらに、ドップラー速度は、マルチパスの影響等により精度が低下することがあるので、特許文献１では、ドップラー速度の良否を判定している。そして、その良否に応じた係数により補正の程度を調整している。
【０００９】
しかし、ドップラー速度の良否判定に、加速度センサの検出値を用いているため、判定の精度が十分によいとは言えない。また、信号品質のよいＧＰＳ電波を複数のＧＰＳ衛星から受信できないときは、ドップラー速度の精度が低下する。その結果、良否に応じた係数によって補正しても、加速度から算出した速度の精度はまだ十分に高いとは言えなかった。
【００１０】
本発明は、この事情に基づいて成されたものであり、その目的とするところは、高精度に速度を推定することができる速度推定装置を提供することにある。
【課題を解決するための手段】
【００１１】
上記目的は独立請求項に記載の特徴の組み合わせにより達成され、また、下位請求項は、発明の更なる有利な具体例を規定する。
【００１２】
上記目的を達成するための第１発明は、移動体とともに移動する加速度センサ（２０）と、移動体のヨーレートを検出するヨーレートセンサ（３０）と、加速度センサの検出値の進行方向成分である進行方向加速度を逐次決定する進行方向加速度決定部（１０２）と、進行方向加速度決定部が決定した進行方向加速度を積算した加速度積算値を逐次算出するとともに、ヨーレートセンサが検出したヨーレートを積算することで、基準時点における進行方向に対する相対方位角を逐次算出する積算処理部（１０４）と、衛星測位システムが備える衛星から衛星信号を取得する衛星信号取得部（１０６）と、衛星信号に基づいて、移動体の速度の衛星の方向への成分である衛星方向速度を算出する衛星方向速度算出部（１１６）と、衛星信号に基づいて、移動体から衛星への視線ベクトルを算出する視線ベクトル算出部（１１３）と、衛星方向速度、視線ベクトル、移動体の速度、移動体の進行方位の関係を示す速度推定式と、衛星方向速度算出部が算出した衛星方向速度、視線ベクトル算出部が算出した視線ベクトル、積算処理部が算出した相対方位角から、移動体の速度初期値を決定する初期設定値決定部（１２６）と、積算処理部が算出した加速度積算値と、初期設定値決定部が決定した速度初期値とに基づいて、移動体の速度を推定する推定速度決定部（１２８）とを備え、速度推定式は、衛星方向速度と、視線ベクトルと、移動体の速度ベクトルとの関係を示す式に対して、速度ベクトルの大きさを移動体の速度で拘束し、かつ、速度ベクトルの向きの時間変化を移動体の進行方向の方位角の時間変化で拘束した式であることを特徴とする速度推定装置である。
【００１３】
移動体の衛星方向速度は、移動体から衛星への視線ベクトルと、移動体の速度ベクトルとに基づいて算出することができる。すなわち、衛星方向速度と、視線ベクトルと、移動体の速度ベクトルとの関係を示す式が成立する。本発明では、この式に対して、速度ベクトルを、移動体の速度および移動体の進行方向の方位角の時間変化で拘束することで得られる速度推定式を用いて、速度初期値を決定する。
【００１４】
速度推定式は、移動体の進行方向の方位角の時間変化で拘束されていることから、異なる複数の観測時点の衛星信号を用いた速度推定式を連立方程式とすることができる。したがって、速度推定式に移動体の速度を含む複数の未知パラメータがあっても、未知パラメータを解くことができる数の速度推定式からなる連立方程式を立式しやすい。よって、速度推定式から移動体の速度を求めやすくなるので、初期設定値決定部は、速度初期値を高頻度に求めることができる。そして、速度初期値を高頻度に更新することで、加速度センサの検出値からドリフトの影響を除去する頻度を高くすることができるので、加速度積算値と速度初期値から推定する速度の推定精度が向上する。
【００１５】
また、上記目的を達成するための第２発明は、移動体とともに移動する加速度センサ（２０）と、加速度センサの検出値の進行方向成分である進行方向加速度を逐次決定する進行方向加速度決定部（１０２）と、進行方向加速度決定部が決定した進行方向加速度を積算した加速度積算値を逐次算出する積算処理部（１０４）と、衛星測位システムが備える衛星から衛星信号を取得する衛星信号取得部（１０６）と、衛星信号に基づいて、移動体の速度の衛星の方向への成分である衛星方向速度を算出する衛星方向速度算出部（１１６）と、衛星信号に基づいて、移動体から衛星への視線ベクトルを算出する視線ベクトル算出部（１１３）と、衛星方向速度、視線ベクトル、移動体の速度、移動体の進行方位、クロックドリフトの関係を示す速度推定式と、衛星方向速度算出部が算出した衛星方向速度、視線ベクトル算出部が算出した視線ベクトルから、移動体の速度初期値を決定する初期設定値決定部（１２６）と、積算処理部が算出した加速度積算値と、初期設定値決定部が決定した速度初期値とに基づいて、移動体の速度を推定する推定速度決定部（１２８）とを備え、速度推定式は、衛星方向速度と、視線ベクトルと、移動体の速度ベクトルと、クロックドリフトとの関係を示す式に対して、速度ベクトルの大きさを移動体の速度で拘束し、クロックドリフトは時間変化が線形であるという拘束条件で拘束した式であることを特徴とする速度推定装置である。
【００１６】
特許文献２にも記載されているように、衛星方向速度と、視線ベクトルと、移動体の速度ベクトルと、クロックドリフトとの関係を示す式が成立する。本発明では、この式に対して、速度ベクトルの大きさを移動体の速度で拘束し、クロックドリフトの時間変化が線形であるという拘束条件で拘束することで得られる速度推定式を用いて、速度初期値を決定する。
【００１７】
速度推定式が、クロックドリフトの時間変化に関する条件で拘束されていることから、異なる複数の観測時点の衛星信号を用いた速度推定式を連立方程式とすることができる。したがって、速度推定式に移動体の速度を含む複数の未知パラメータがあっても、未知パラメータを解くことができる数の速度推定式からなる連立方程式を立式しやすい。よって、速度推定式から移動体の速度を求めやすくなるので、初期設定値決定部は、速度初期値を高頻度に求めることができる。そして、速度初期値を高頻度に更新することで、加速度センサの検出値からドリフトの影響を除去する頻度を高くすることができるので、加速度積算値と速度初期値から推定する速度の推定精度が向上する。
【００１８】
なお、特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、本発明の技術的範囲を限定するものではない。
【図面の簡単な説明】
【００１９】
【図１】実施形態の速度推定装置１の構成図である。
【図２】加速度積算値ΔＶＧの誤差が発散することを示す図である。
【図３】図１の制御部１００が行う処理の流れを示すフロチャートである。
【図４】図３のステップＳ２２の詳細フロチャートである。
【図５】図３のステップＳ２８の詳細フロチャートである。
【発明を実施するための形態】
【００２０】
＜実施形態の構成＞
以下、本発明の実施形態を図面に基づいて説明する。図１に示すように、本実施形態の速度推定装置１は、ＧＰＳ信号受信部１０、加速度センサ２０、ヨーレートセンサ３０、制御部１００を備える。この速度推定装置１は、図示しない車両に、予め定められた向きで固定される。
【００２１】
ＧＰＳ信号受信部１０は、衛星測位システムの一つであるグローバルポジショニングシステム（以下、ＧＰＳ）が備えるＧＰＳ衛星が送信するＧＰＳ電波を受信する受信機である。このＧＰＳ電波は、搬送波にＧＰＳ信号が重畳されたものを意味する。ＧＰＳ信号受信部１０は受信したＧＰＳ電波を復調してＧＰＳ信号を取り出し、制御部１００に送る。また、搬送波、受信信号強度も制御部１００に送る。このＧＰＳ信号受信部１０は請求項の信号受信部に相当し、ＧＰＳ信号は請求項の衛星信号に相当する。
【００２２】
周知のように、ＧＰＳ衛星は複数存在している。ＧＰＳ信号受信部１０は、受信できるすべてのＧＰＳ衛星からのＧＰＳ電波を受信する。ＧＰＳ信号には、ＧＰＳ衛星の衛星番号、ＧＰＳ衛星の軌道情報であるエフェメリス、ＧＰＳ衛星が電波を送信した時刻などが含まれている。
【００２３】
加速度センサ２０は、３軸の加速度センサであり、ｚ軸が車両の上下方向に平行になり、ｘ軸が車両の幅方向と平行になり、ｙ軸が車両の前後方向に平行になるように、加速度センサ２０は向きが固定されている。なお、３軸の加速度センサに代えて、ｘ軸、ｙ軸の２軸の加速度を検出する加速度センサを用いてもよい。加速度センサ２０は各軸の加速度の検出値を制御部１００に送る。
【００２４】
ヨーレートセンサ３０は、このヨーレートセンサ３０を通り、車両の垂直軸周りの回転角速度、すなわち、ヨーレートを検出する。そして、検出したヨーレートを制御部１００に供給する。なお、車両の垂直軸とは、車両の車室床に対して垂直な軸であり、車両が水平な地面に位置している時の鉛直軸と平行になる軸である。
【００２５】
制御部１００は、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータであり、ＣＰＵが、ＲＡＭの一時記憶機能を利用しつつＲＯＭに記憶されているプログラムを実行することで、制御部１００は、図１に示す各部１０２〜１３２として機能する。
【００２６】
進行方向加速度決定部１０２は、加速度センサ２０が検出した検出値から、車両の進行方向加速度を決定する。前述したように、加速度センサ２０は車両に対する向きが予め定められた向きに固定されており、ｙ軸が車両進行方向を向いている。したがって、加速度センサ２０が検出したｙ軸の検出値を、進行方向加速度として抽出する。
【００２７】
積算処理部１０４は、進行方向加速度決定部１０２が決定した進行方向加速度、すなわち、加速度センサ２０のｙ軸の検出値を、逐次、積算する。積算した値を、以下、加速度積算値ΔＶＧという。また、この積算処理部１０４は、ヨーレートセンサ３０の検出値を積算して、相対方位角θｔｇｙｒｏも算出する。相対方位角θｔｇｙｒｏは、基準時点における車両の進行方位に対する時刻ｔの相対方位角である。この相対方位角θｔｇｙｒｏは、（１）式から算出する。（１）式において、Δｔはタイムステップ、ωｔは時刻ｔに検出されたヨーレートセンサ３０の検出値である。
【数１】
2015129714.tif 000003
【００２８】
なお、これら加速度積算値ΔＶＧ、相対方位角θｔｇｙｒｏを算出するための加速度センサ２０の検出値、ヨーレートセンサ３０の検出値は、同じタイミングで取得する。取得するタイミングは、たとえば、一定時間時、あるいは、一定距離走行時である。
【００２９】
衛星情報取得部１０６は、ＧＰＳ信号受信部１０からＧＰＳ信号および搬送波を取得する。ＧＰＳ信号受信部１０が複数のＧＰＳ衛星からＧＰＳ電波を受信している場合、ＧＰＳ信号受信部１０がＧＰＳ電波を受信したすべてのＧＰＳ衛星についてのＧＰＳ信号および搬送波を取得する。この衛星情報取得部１０６は請求項の衛星信号取得部として機能する。
【００３０】
さらに、衛星情報取得部１０６は、これらＧＰＳ信号および搬送波から得られるＧＰＳ衛星ｉに関する情報である、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）、疑似距離ρｉ、ドップラーシフト量Ｄｉも算出する。
【００３１】
各ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）は、各ＧＰＳ衛星ｉのエフェメリスおよび電波を送信した時刻に基づいて算出する。疑似距離ρｉは、ＧＰＳ衛星ｉがＧＰＳ電波を送信した時刻と、ＧＰＳ信号受信部１０がＧＰＳ電波を受信した時刻との時刻差すなわち電波伝播時間に、光速を乗じることで算出する。
【００３２】
ドップラーシフト量Ｄｉは、ＧＰＳ衛星ｉが送信した電波の搬送波の周波数と、受信したＧＰＳ電波の搬送波の周波数の周波数差である。ＧＰＳ衛星が送信する電波の搬送波周波数は予め定まっており、この周波数は、制御部１００が備える図示しない記憶部など、所定の記憶部に予め記憶されている。したがって、衛星情報取得部１０６は、記憶部からＧＰＳ電波の搬送波の周波数を取得し、この周波数と、ＧＰＳ信号受信部１０から取得した搬送波の周波数から、ドップラーシフト量Ｄｉを算出する。なお、衛星情報取得部１０６が取得した搬送波の周波数は、公知の周波数解析手法、たとえば、高速フーリエ変換により決定する。
【００３３】
相対速度算出部１０８は、衛星情報取得部１０６が算出したドップラーシフト量Ｄｉに基づいて、ＧＰＳ衛星ｉに対する車両の相対速度Ｖｒｉを算出する。相対速度Ｖｒｉは、下記（２）式から算出する。（２）式において、ＶｒｉはＧＰＳ衛星ｉに対する車両の相対速度、Ｄｉは衛星情報取得部１０６が算出したドップラーシフト量、Ｃは光速、ＦはＧＰＳ衛星が送信する電波の搬送波の周波数である。
【数２】
2015129714.tif 000004
【００３４】
衛星速度算出部１１０は、衛星情報取得部１０６が算出した各ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）の時系列データから、ケプラーの方程式の微分を用いる公知の方法で、各ＧＰＳ衛星ｉの速度ベクトル、すなわち、三次元速度Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉを算出する。
【００３５】
現在位置算出部１１２は、衛星情報取得部１０６が算出した各ＧＰＳ衛星ｉの疑似距離ρｉを用いて、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を算出する。
【００３６】
ＧＰＳ信号を用いた測位では、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）と、各ＧＰＳ衛星との間の疑似距離ρｉとに基づいて、三角測量の原理に従って、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を算出する。ここで、ＧＰＳ衛星ｉまでの真の距離ｒｉは（３）式で表される。一方、疑似距離ρｉは（４）式で表される。なお、（４）式において、ｓは時計誤差による距離誤差である。
【数３】
2015129714.tif 000005
【００３７】
上記（３）式、（４）式より、４つ以上のＧＰＳ衛星の疑似距離ρｉから得られる以下の（５）式の連立方程式を解くことによって、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）が算出できる。
【数４】
2015129714.tif 000006
【００３８】
なお、本実施形態では、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を、後述する衛星方向算出部１１４において、ＧＰＳ衛星ｉの方向、すなわち、ＧＰＳ衛星と自車両との角度を求めるために算出する。
【００３９】
ＧＰＳ衛星は遠方に存在するため、ここでの車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）の精度は低くてもよい。したがって、疑似距離ρｉを用いた位置決定以外の精度の低い方法で車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を決定してもよい。システム等で許容される推定精度に依存するが、車両の位置誤差が数百ｍの範囲であれば、速度推定誤差は１ｍ／ｓｅｃ以下となり大きな問題はない。そのため、たとえば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を決定してもよい。
【００４０】
視線ベクトル算出部１１３は、車両からＧＰＳ衛星ｉへの視線ベクトル（Ｇｘｉ、Ｇｙｉ、Ｇｚｉ）を算出する。視線ベクトルのｘ成分、ｙ成分、ｚ成分は、（６）式から算出する。
【数５】
2015129714.tif 000007
【００４１】
（６）式において、ρｔｉは時刻ｔにおけるＧＰＳ衛星ｉの疑似距離、（Ｘｔｓｉ、Ｙｔｓｉ、Ｚｔｓｉ）は時刻ｔにおけるＧＰＳ衛星ｉの位置座標である。これらは、衛星情報取得部１０６が算出している。（ＸｔＶ、ＹｔＶ、ＺｔＶ）は時刻ｔにおける現在位置であり、現在位置算出部１１２が算出している。
【００４２】
衛星方向算出部１１４は、現在位置算出部１１２が算出した現在位置（Ｘｖ，Ｙｖ，Ｚｖ）、および、衛星情報取得部１０６が算出したＧＰＳ衛星の位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）に基づいて、各ＧＰＳ衛星ｉの方向Ｒｉを算出する。各ＧＰＳ衛星ｉの方向Ｒｉは、車両からＧＰＳ衛星ｉを見たときの、水平方向に対する仰角θｉ、北方向に対する方位角φｉで表すものとする。
【００４３】
衛星方向速度算出部１１６は、ＧＰＳ衛星ｉの方向への車両の速度である衛星方向速度Ｖｓｉを算出する。衛星方向速度Ｖｓｉは、下記（７）式から算出する。
【数６】
2015129714.tif 000008
【００４４】
（７）式において、右辺第１項は相対速度Ｖｒであり、相対速度算出部１０８が算出している。Ｇｘ、Ｇｙ、Ｇｚは、視線ベクトルであり、視線ベクトル算出部１１３が算出している。Ｖｘｓ、Ｖｙｓ、Ｖｚｓは、ＧＰＳ衛星ｉの速度のｘ、ｙ、ｚ成分であり、衛星速度算出部１１０が算出している。（７）式の右辺の第１項は、ＧＰＳ衛星ｉに対する車両の相対速度Ｖｒｉであり、第２〜第４項は、ＧＰＳ衛星ｉの車両方向への速度である。これらの和は、ＧＰＳ衛星ｉの方向への車両の速度を意味することから、（７）式が成り立つのである。
【００４５】
速度ベクトル算出部１１８は、車両の速度ベクトルを算出する。車両の速度ベクトルを（Ｖｘ，Ｖｙ，Ｖｚ）としたとき、衛星方向速度Ｖｓｉと、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）との関係は、以下の（８）式で表される。
【数７】
2015129714.tif 000009
【００４６】
各ＧＰＳ衛星ｉについて得られる上記（８）式より、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）およびＣｂｖを未知数とした以下の（９）式で表される連立方程式が得られる。
【００４７】
（９）式において、Ｖｓａｔｉは、車両方向のＧＰＳ衛星ｉの速度であり、Ｖｓａｔｉ＝Ｒｉ［Ｖｘｓｉ，Ｖｙｓｉ，Ｖｚｓｉ］Ｔにより求める。Ｔは行列の転置を意味する。なお、（７）式の右辺第２〜第４項を計算して車両方向のＧＰＳ衛星ｉの速度Ｖｓａｔｉを求めてもよい。ＣｂｖはＧＰＳ信号受信部１０が備える時計のクロックドリフトである。
【数８】
2015129714.tif 000010
【００４８】
ＧＰＳ電波を受信したＧＰＳ衛星が４個以上である場合に、上記（９）式の連立方程式を解くことができる。ただし、ＧＰＳ電波を受信していても、信号品質がよいと判定できないＧＰＳ電波は除外する。よって、速度ベクトル算出部１１８は、信号品質がよいと判定したＧＰＳ電波を４個以上のＧＰＳ衛星から受信した場合に、（９）式から車両の速度ベクトルを算出する。信号品質の良否は、次に説明する信号品質判定部１２０が判定する。
【００４９】
上記（９）式は、ドップラーシフト量Ｄを含んだ式であることから、この速度ベクトル算出部１１８は、ドップラーシフト量に基づいて車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出している。よって、速度ベクトル算出部１１８は、請求項のドップラー速度算出部に相当する。
【００５０】
信号品質判定部１２０は、ＧＰＳ信号受信部１０が受信したＧＰＳ衛星ｉからのＧＰＳ電波の信号品質がよいか否かを判定する。信号品質の判定には、公知の種々の基準を用いることができる。
【００５１】
たとえば、（判定条件１）Ｓ／Ｎが所定値以上であること、（判定条件２）疑似距離ρｉの残差が判定基準距離以下であること、（判定条件３）仰角θｉが判定基準角以上であること、（判定条件４）判定条件１〜３の組み合わせ、などにより信号品質がよいか否かを判定する。
【００５２】
なお、疑似距離ρｉの残差とは、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）と車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）との間の距離と、疑似距離ρｉとの差である。この残差が大きい場合には、マルチパス等の影響が考えられるので、信号品質が良くないと判定する。なお、判定条件１は、Ｓ／Ｎが所定値以上であれば信号品質はよいと判定する。判定条件３は、仰角θｉが判定基準角以上であれば信号品質はよいと判定する。
【００５３】
停止判定部１２２は、車両が停止しているか否かを判定する。停止判定の方法も、公知の種々の手法を用いることができる。たとえば、加速度センサ２０のｚ軸の検出値が停止判定値以下であれば、車両が停止していると判定する。車両が走行している場合には、多少の上下振動があるので、ｚ軸の検出値により停止判定を行うことができるのである。ｚ軸の検出値に代えて、ｙ軸の検出値や、ｚ軸の検出値の変化量、ｙ軸の検出値の変化量を用いてもよい。加速度センサ２０の検出値により停止判定を行えば、ブレーキ信号やシフト位置信号を取得するための配線が不要となる利点がある。もちろん、ブレーキ信号やシフト位置信号を取得できるようにして、それらの信号を用いて停止判定をしてもよい。
【００５４】
初期設定値決定部１２６は、推定速度決定部１２８で用いる速度初期値Ｖ０、推定方位決定部１３０で用いる方位初期値θ０決定する。
【００５５】
本実施形態では、すでに説明した速度ベクトル算出部１１８でも車両の速度を算出することができる。しかし、速度ベクトル算出部１１８は、信号品質がよいＧＰＳ電波を４個以上のＧＰＳ衛星から受信しなければ、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出することができない。また、信号品質がよいＧＰＳ電波を４個以上受信できる環境であっても、速度ベクトル算出部１１８では、周波数解析を必要とするドップラーシフト量Ｄを用いるため、たとえば、１００ｍｓ毎などの一定周期でしか、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出することができない。
【００５６】
これに対して、加速度センサ２０の検出値は、たとえば２０ｍｓごとなど、速度ベクトル算出部１１８が速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出するよりも短い周期で取得する構成とすることができる。そこで、速度ベクトル算出部１１８が車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出してから、次に、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出するまでの間は、加速度積算値ΔＶＧをもとにして速度推定を行う。
【００５７】
しかし、加速度センサ２０の検出値には常に誤差があり、その誤差は発散する性質を有する。図２はそのことを示すグラフである。図２は停止状態における加速度積算値ΔＶＧのグラフである。車両が停止状態であることから、この図２における加速度積算値ΔＶＧは、加速度積算値ΔＶＧをそのまま速度とした場合の誤差を意味する。図２から、加速度積算値ΔＶＧをそのまま速度とする場合、誤差が時間の経過により発散することが分かる。なお、図２では、誤差は負の値であるが、これとは反対に誤差が正の値となることもある。
【００５８】
このように、加速度積算値ΔＶＧをそのまま速度とすると、誤差が時間の経過により発散する。そこで、本実施形態では、精度の高い速度を求めた時点で、その精度の高い速度を速度初期値Ｖ０とする。
【００５９】
この時点でのΔＶＧを精度の高い速度から引いた値を速度初期値Ｖ０とする。このようにして求める速度初期値Ｖ０は、加速度積算値ΔＶＧを真の速度とみなす精度の高い速度にするためのオフセット分を意味する。よって、ΔＶＧ−Ｖ０により推定速度Ｖｅを求める。
【００６０】
加速度積算値ΔＶＧと真の速度との誤差は、時間の経過とともに増大する。したがって、速度初期値Ｖ０の更新周期が短いほど、加速度積算値ΔＶＧを用いた速度の推定精度が向上する。
【００６１】
そこで、本実施形態では、特許文献２において最終的な速度ベクトル算出式として開示されている下記（１１）式を改良した（１２）式により速度初期値Ｖ０を算出する。また、同時に方位初期値θ０に算出する。（１２）式は、速度初期値Ｖ０を更新できるからである。
【００６２】
ただし、速度初期値Ｖ０については、速度ベクトル算出部１１８が速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出できたときは、（１０）式から速度初期値Ｖ０を決定する。速度ベクトル算出部１１８が算出する速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は精度がよいので、この速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を用いて速度初期値Ｖ０を決定したほうが、速度初期値Ｖ０の精度が向上するからである。また、停止判定部１２２が、車両が停止していると判定したときは、速度初期値Ｖ０を−ΔＶＧとする。
【数９】
2015129714.tif 000011
【００６３】
（１１）式において、ｔは時刻、Ｖｗｈｅｅｌは車輪速センサの検出値、θ０は初期時刻における車両の進行方向の方位角（以下、方位初期値）、θｇｙｒｏは、車両の進行方向の方位角の積算値すなわち相対方位角、Ｃｂｖ０は初期時刻におけるクロックドリフト、Ａはクロックドリフトの時間変化の傾き、Ｇｘ、Ｇｙは、車両からＧＰＳ衛星ｉへの視線ベクトルのｘ成分、ｙ成分である。
【００６４】
また、（１２）式において、Ｖ０は初期時刻における車速である速度初期値、ΔＶＧは初期時刻以降の加速度積算値である。すなわち、（１２）式は、（１１）式における車輪速センサの検出値Ｖｗｈｅｅｌを、速度初期値Ｖ０と加速度積算値ΔＶＧの和に置き換えた式である。この（１２）式が請求項の速度推定式に相当する。
【００６５】
まず、（１１）式の導出方法を説明する。特許文献２にも開示されているように、衛星方向速度Ｖｓｔｉと、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）との関係は、（１３）式で表すことができる。
【数１０】
2015129714.tif 000012
【００６６】
この（１３）式には、Ｖｘｔ、Ｖｙｔ、Ｖｚｔ、Ｃｂｖｔの４つの未知パラメータがある。この未知パラメータの数を少なくするために、特許文献２では、下記に示す拘束条件１〜３を（１３）式に代入することで、前述した（１１）式を導出している。
【数１１】
2015129714.tif 000013
【００６７】
拘束条件１は、推定する速度ベクトルのｘ成分、ｙ成分の大きさを車輪速で拘束し、かつｘ成分、ｙ成分の時間変化分を方位角の時間変化で拘束したものである。拘束条件１のθ０は初期時刻における車両の進行方向の方位角である。拘束条件２は、高さ方向速度の変化は常に微小であると仮定するものである。
【００６８】
拘束条件３は、クロックドリフトの時間変化は緩やかであるため、短時間内における変化は線形であると仮定するものである。拘束条件３のＣｂｖ０は初期時刻におけるクロックドリフト、Ａはクロックドリフトの時間変化の傾きを示す。
【００６９】
なお、拘束条件１および２では、二次元平面内の速度成分のみ時間変化で拘束しているが、３軸ジャイロセンサなどにより取得したピッチレートを用いて、ｚ軸成分も時間変化で拘束してもよい。上記拘束条件１〜３を（１３）式に取り入れることで（１１）式が得られる。そして、（１１）式における車輪速センサの検出値Ｖｗｈｅｅｌを、速度初期値Ｖ０と加速度積算値ΔＶＧの和に置き換えることで、（１２）式が得られる。
【００７０】
（１２）式において、衛星方向速度Ｖｓｔｉは、時刻ｔにおけるＧＰＳ衛星ｉ方向の衛星方向速度であり、衛星方向速度算出部１１６が算出する。ΔＶｔＧは、時刻ｔにおける加速度積算値であり、積算処理部１０４が算出する。（Ｇｘｔｉ、Ｇｙｔｉ、Ｇｚｔｉ）は、時刻ｔにおける視線ベクトルであり、視線ベクトル算出部１１３が算出する。θｔｇｙｒｏは、ヨーレート積算値であり、積算処理部１０４が算出する。よって、（１２）式において未知パラメータは、θ０、Ｃｂｖ０、Ａ、および、Ｖ０の４つである。
【００７１】
したがって、４つの式を立式できれば、未知パラメータになっている速度初期値Ｖ０および方位初期値θ０を求めることができる。しかも、未知パラメータθ０、Ｃｂｖ０、Ａ、Ｖ０は、初期時刻以降であれば、時刻が異なっても同じである。そのため、同一時刻で４つの式を立式する必要はなく、複数の時刻において立式した式数が合計４式以上であれば、未知パラメータを求めることができる。たとえば、例えば、４時刻（ｔ０、ｔ１、ｔ２、ｔ３）の観測衛星数がそれぞれ１であったとしても、観測されたＧＰＳ衛星からのデータを用いて、速度初期値Ｖ０および方位初期値θ０を求めることができる。
【００７２】
推定速度決定部１２８は、初期設定値決定部１２６が決定した速度初期値Ｖ０に、積算処理部１０４が算出した加速度積算値ΔＶｔＧを加算して、推定速度Ｖｔｅを加速度取得周期で逐次算出する。
【００７３】
ただし、速度ベクトル算出部１１８が速度ベクトルを算出したとき、および、停止判定部１２２が、車両は停止していると判定したときは、推定速度Ｖｔｅは算出しない。速度ベクトル算出部１１８が速度ベクトルを算出したときに推定速度Ｖｔｅを算出しないのは、速度ベクトル算出部１１８が算出した速度ベクトルのほうが精度がよいからである。停止判定部１２２が、車両が停止していると判定したときに推定速度Ｖｔｅを算出しないのは、このときは、速度は計算しなくても０ｋｍ／ｈだからである。
【００７４】
推定方位決定部１３０は、初期設定値決定部１２６が決定した方位初期値０に、積算処理部１０４が算出した相対方位角θｔｇｙｒｏを加算して、推定方位θｔｅを相対方位角決定周期で逐次算出する。
【００７５】
速度ベクトル推定部１３２は、推定速度決定部１２８が決定した推定速度Ｖｔｅと、推定方位決定部１３０が決定した推定方位θｔｅとから、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を求める。推定方位θｔｅに向かう速度が推定速度Ｖｔｅであることから、この推定速度をｘ、ｙ、ｚ成分に分けることで、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を求めることができる。この速度ベクトル推定部１３２で推定した速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は、たとえば、ＧＰＳ信号を用いた車両の現在位置決定時から、次のＧＰＳ信号を用いた車両の現在位置決定時までの間の車両位置推定に用いる。
【００７６】
なお、停止判定部１２２が、車両が停止していると判定した場合には、速度ベクトル推定部１３２は、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を（０，０，０）とする。
【００７７】
＜制御部１００の処理の流れ＞
次に、制御部１００の処理の流れの一例を図３〜図５のフロチャートを用いて説明する。図３に示すフロチャートは、センサ値を取得する周期で繰り返し実行する。なお、特に明記している場合を除き、各パラメータは、時刻ｔにおける値を意味する。
【００７８】
図３において、ステップＳ２では、加速度センサ２０、ヨーレートセンサ３０の検出値を取得し、ＲＡＭなどの記憶部に記憶する。この処理は、たとえば積算処理部１０４が行う。
【００７９】
ステップＳ４は進行方向加速度決定部１０２が行う処理であり、ステップＳ２で取得した加速度センサ２０の検出値から、進行方向加速度を決定する。
【００８０】
ステップＳ６は積算処理部１０４が行う処理であり、ステップＳ２で取得したヨーレートセンサ３０の検出値を、これまでの相対方位角θｔ−１ｇｙｒｏに加算して相対方位角θｔｇｙｒｏを更新する。また、ステップＳ２で決定した進行方向加速度をこれまでの加速度積算値ΔＶｔ−１Ｇに加算して加速度積算値ΔＶｔＧを更新する。
【００８１】
ステップＳ８、Ｓ１０は衛星情報取得部１０６が行う処理である。ステップＳ８では、ＧＰＳ信号受信部１０からＧＰＳ信号および搬送波を取得する。ステップＳ１０では、ステップＳ８で取得したＧＰＳ信号および搬送波から、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）、疑似距離ρｉ、ドップラーシフト量Ｄｉを算出する。
【００８２】
ステップＳ１２は、停止判定部１２２の処理であり、たとえば、ステップＳ２で取得した加速度センサ２０のｚ軸の検出値から、車両が停止しているか否かを判断する。停止中と判断した場合には、ステップＳ１４へ進み、移動中であると判断した場合にはステップＳ１８へ進む。
【００８３】
ステップＳ１４は速度ベクトル推定部１３２の処理であり、速度ベクトルを（０、０，０）とする。もちろん、速度も０となる。
【００８４】
ステップＳ１６は初期設定値決定部１２６の処理であり、速度初期値Ｖ０を、最新の加速度積算値−ΔＶｔＧとする。
【００８５】
ステップＳ１２において移動中と判断した場合に実行するステップＳ１８は信号品質判定部１２０の処理であり、前述した判定条件に基づいて、ＧＰＳ信号の信号品質がよいか否かを判定する。信号品質の判定は、取得したすべてのＧＰＳ信号に対して行う。
【００８６】
ステップＳ２０は速度ベクトル算出部１１８の処理であり、ステップＳ１８で信号品質がよいと判定したＧＰＳ信号の数が４以上であるか否かを判断する。４以上である場合にはステップＳ２２へ進む。
【００８７】
ステップＳ２２では、ドップラー速度を算出する。このドップラー速度とは、速度ベクトル算出部１１８が算出する速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）またはその速度ベクトルの大きさを意味する。この速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は、ドップラーシフト量Ｄを用いて算出することから、ここでは、ドップラー速度と称している。
【００８８】
ステップＳ２２の詳細処理は図４に示す。図４において、ステップＳ２２２は現在位置算出部１１２の処理であり、ステップＳ１０で算出した疑似距離ρｉ、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）から、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を算出する。
【００８９】
ステップＳ２２４は衛星方向算出部１１４が行う処理であり、ステップＳ２２２で算出した車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）、および、ステップＳ１０で算出したＧＰＳ衛星の位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）から、各ＧＰＳ衛星ｉの方向Ｒｉ（θｉ、φｉ）を算出する。
【００９０】
ステップＳ２２６は衛星速度算出部１１０が行う処理であり、ステップＳ１０で算出した各ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）の時系列データから、各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）を算出する。
【００９１】
ステップＳ２２８、Ｓ２３０は速度ベクトル算出部１１８の処理である。ステップＳ２２８では、ステップＳ２２４で算出した各ＧＰＳ衛星ｉの方向Ｒｉと、ステップＳ２２６で算出した各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）から、Ｖｓａｔｉ＝Ｒｉ［Ｖｘｓｉ，Ｖｙｓｉ，Ｖｚｓｉ］Ｔにより、車両方向のＧＰＳ衛星ｉの速度Ｖｓａｔｉを求める。
【００９２】
ステップＳ２３０では、（９）式に示した連立方程式を４つ以上立式し、その連立方程式を解く。これにより、車両の速度ベクトル（Ｖｘｔ，Ｖｙｔ，Ｖｚｔ）およびクロックドリフトＣｂｖｔを求めることができる。
【００９３】
説明を図３に戻す。ステップＳ２４は初期設定値決定部１２６の処理であり、ステップＳ２２で算出した車両の速度ベクトル（Ｖｘｔ，Ｖｙｔ，Ｖｚｔ）から、（１０）式により、速度初期値Ｖ０を決定する。
【００９４】
ステップＳ２０において品質のよい信号が３以下であると判断した場合にはステップＳ２６へ進む。ステップＳ２６は初期設定値決定部１２６の処理であり、初期値を更新することができるか否かを判断する。この判断は、具体的には、速度初期値Ｖ０を更新してからの衛星信号を用いて、（１２）式を４つ以上、立式できるか否かを判断するものである。この判断がＮｏであればステップＳ３０へ進み、ＹｅｓであればステップＳ２８へ進む。
【００９５】
ステップＳ２８では、タイトカップリング型推定式を用いて速度初期値Ｖ０および方位初期値θ０を決定する。タイトカップリング型推定式とは、具体的には（１２）式のことである。
【００９６】
ステップＳ２８の詳細処理は図５に示す。図５において、ステップＳ２８２は衛星速度算出部１１０が行う処理であり、ステップＳ１０で算出した各ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）の時系列データから、各ＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）を算出する。
【００９７】
ステップＳ２８４は相対速度算出部１０８が行う処理であり、ステップＳ１０で算出したドップラーシフト量Ｄｉを前述した（２）式に代入して、ＧＰＳ衛星ｉに対する車両の相対速度Ｖｒｉを算出する。
【００９８】
ステップＳ２８６は現在位置算出部１１２の処理であり、ステップＳ１０で算出した疑似距離ρｉ、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）から、車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を算出する。
【００９９】
ステップＳ２８８は視線ベクトル算出部１１３が行う処理である。このステップＳ２８８では、前述した（６）式に、ステップＳ１０で算出した疑似距離ρｉ、ＧＰＳ衛星ｉの位置座標（Ｘｓｉ、Ｙｓｉ、Ｚｓｉ）、ステップＳ２８６で算出した車両の現在位置（Ｘｖ，Ｙｖ，Ｚｖ）を代入して、視線ベクトル（Ｇｘｉ，Ｇｙｉ，Ｇｚｉ）を算出する。
【０１００】
ステップＳ２９０は衛星方向速度算出部１１６が行う処理である。このステップＳ２９０では、前述した（７）式に、ステップＳ２８４で算出した相対速度Ｖｒｉ、ステップＳ２８８で算出した視線ベクトル（Ｇｘｉ，Ｇｙｉ，Ｇｚｉ）、ステップＳ２８２で算出したＧＰＳ衛星ｉの速度ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）を代入して、ＧＰＳ衛星ｉの方向への車両の衛星方向速度Ｖｓｉを算出する。
【０１０１】
ステップＳ２９２は初期設定値決定部１２６が行う処理である。このステップＳ２９２では、前述した（１２）式に、ステップＳ２９０で算出した衛星方向速度Ｖｓｉ、ステップＳ６で更新した加速度積算値ΔＶＧ、相対方位角θｇｙｒｏ、ステップＳ２８８で算出した視線ベクトル（Ｖｘｓｉ、Ｖｙｓｉ、Ｖｚｓｉ）を代入した式を４つ以上立式する。そして、それら４つ以上の式からなる連立方程式を解く。これにより、（１２）式において未知パラメータとなっている速度初期値Ｖ０、方位初期値θ０を同時に求めることができる。
【０１０２】
説明を図３に戻す。ステップＳ２８を実行した場合、または、ステップＳ２６の判断がＮｏであった場合には、ステップＳ３０を実行する。ステップＳ３０は推定速度決定部１２８が行う処理であり、最新の速度初期値Ｖ０と、ステップＳ６で更新した加速度積算値ΔＶＧとから、推定速度Ｖｅを算出する。
【０１０３】
図３のように、速度初期値Ｖ０を更新しても加速度積算値ΔＶＧをリセットしない場合には、加速度積算値ΔＶＧに速度初期値Ｖ０を加算することで推定速度Ｖｅを算出する。なお、速度初期値Ｖ０を更新したときに加速度積算値ΔＶＧをリセットする場合には、速度初期値Ｖ０に加速度積算値ΔＶＧを加算することで、推定速度Ｖｅを算出する。
【０１０４】
ステップＳ３２は推定方位決定部１３０が行う処理であり、最新の方位初期値θ０と、ステップＳ６で更新した相対方位角θｇｙｒｏとから、推定方位θｅを算出する。具体的な推定方位θｅの計算方法は、速度初期値Ｖ０と加速度積算値ΔＶＧから推定速度Ｖｅを算出する方法において、速度初期値Ｖ０を方位初期値θ０に置き換え、加速度積算値ΔＶＧを相対方位角θｇｙｒｏに置き換えた場合と同じである。
【０１０５】
ステップＳ３４は速度ベクトル推定部１３２が行う処理であり、ステップＳ３０で推定した推定速度Ｖｅと、ステップＳ３４で推定した推定方位θｅとから、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を決定する。これにより、ステップＳ２２でドップラー速度が算出できない場合でも、速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を決定することができる。
【０１０６】
＜実施形態の効果＞
車両の衛星方向速度Ｖｓｉは、（１３）式に示したように、車両からＧＰＳ衛星ｉへの視線ベクトル（Ｇｘ，Ｇｙ，Ｇｚ）と、車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）と、クロックドリフトＣｂｖに基づいて算出することができる。
【０１０７】
本実施形態では、この（１３）式に対して、速度ベクトルのｘ成分、ｙ成分を、車両の速度、および車両の進行方向の方位角の時間変化すなわち相対方位角θｇｙｒｏで拘束している（拘束条件１）。初期設定値決定部１２６は、これにより得られる（１２）式を用いて速度初期値Ｖ０を決定する。
【０１０８】
このように（１２）式は時間変化に関する条件で拘束されていることから、異なる複数の観測時点のＧＰＳ信号を用いた（１２）式を連立方程式とすることができる。（１２）式には、速度初期値Ｖ０以外にも未知パラメータがあるため、未知パラメータの数に相当する複数の（１２）式を連立方程式とする必要がある。しかし、本実施形態では、異なる複数の観測時点のＧＰＳ信号を用いた（１２）式を連立方程式とすることができるので、未知パラメータを解くことができる数の（１２）式からなる連立方程式を立式しやすい。
【０１０９】
よって、初期設定値決定部１２６は、速度初期値Ｖ０を高頻度に求めることができる。そして、速度初期値Ｖ０を高頻度に更新することで、加速度センサ２０の検出値からドリフトの影響を除去する頻度が高くなるので、加速度積算値ΔＶＧと速度初期値Ｖ０から推定する速度の推定精度が向上する。
【０１１０】
また、本実施形態では、拘束条件１に加えて、クロックドリフトの時間変化が線形であるとする拘束条件３も用いて（１２）式を導出している。これにより、（１２）式における未知パラメータの数をより少なくすることができる。その結果、未知パラメータを解く数の連立方程式を立式しやすくなるので、速度初期値Ｖ０の更新頻度をさらに高くすることができる。
【０１１１】
また、（１２）式は、車両の速度の項として、速度初期値Ｖ０と加速度積算値ΔＶＧの項とを含んでいる。よって、未知パラメータの数の（１２）式からなる連立方程式を解くことで、直接、速度初期値Ｖ０を得ることができる。
【０１１２】
また、本実施形態では、加速度積算値ΔＶＧを用いないで速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出する速度ベクトル算出部１１８を備える。この速度ベクトル算出部１１８が速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を算出した場合、速度ベクトル算出部１１８が算出した速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）を用いて速度初期値Ｖ０を決定する（Ｓ２４）。速度ベクトル算出部１１８が算出した速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）は精度がよいことから、このようにすれば、速度初期値Ｖ０の精度が向上する。
【０１１３】
また、本実施形態では、停止判定部１２２が、車両が停止していると判定したときは、車両の速度がゼロであるとして、速度初期値Ｖ０を決定する（Ｓ１６）。これによっても、速度初期値Ｖ０の精度が向上する。そして、速度初期値Ｖ０の精度が向上する結果、速度初期値Ｖ０と加速度積算値ΔＶＧから算出する推定速度Ｖｅの精度も向上する。
【０１１４】
以上、本発明の実施形態を説明したが、本発明は上述の実施形態に限定されるものではなく、次の変形例も本発明の技術的範囲に含まれ、さらに、下記以外にも要旨を逸脱しない範囲内で種々変更して実施することができる。
【０１１５】
＜変形例１＞
前述の実施形態では、拘束条件３により、時間変化が線形であるという条件でクロックドリフトを拘束していたが、この拘束条件３をなしにしてもよい（変形例１）。拘束条件３をなしにする場合、（１２）式におけるＣｂｖ０＋Ａｔを、未知パラメータであるＣｂｖｔに置き換えることになる。
【０１１６】
＜変形例２＞
また、拘束条件３は残し、拘束条件１における方位角の時間変化による拘束をなしにしてもよい。この場合には、拘束条件１が、速度ベクトルの大きさを車両の速度で拘束したのみの条件となり、（１２）式におけるθｔｇｙｒｏが未知パラメータになる。
【０１１７】
＜変形例３＞
前述の実施形態では、衛星測位システムとしてＧＰＳを利用していたが、その他の衛星測位システムを利用してもよい。また、ＧＰＳが備える衛星と、とその他の衛星測位システムが備える衛星を両方用いてもよい。
【０１１８】
＜変形例４＞
前述の実施形態では、移動体は車であったが、車以外の移動体にも、本発明は適用できる。
【符号の説明】
【０１１９】
１：速度推定装置、      １０：ＧＰＳ信号受信部、      ２０：加速度センサ、      ３０：ヨーレートセンサ、      １００：制御部、      １０２：進行方向加速度決定部、      １０４：積算処理部、      １０６：衛星情報取得部、      １０８：相対速度算出部、      １１０：衛星速度算出部、      １１２：現在位置算出部、      １１３：視線ベクトル算出部、      １１４：衛星方向算出部、      １１６：衛星方向速度算出部、      １１８：速度ベクトル算出部、      １２０：信号品質判定部、      １２２：停止判定部
１２６：初期設定値決定部、      １２８：推定速度決定部、      １３０：推定方位決定部、      １３２：速度ベクトル推定部
【図１】
2015129714.tif 000014
【図２】
2015129714.tif 000015
【図３】
2015129714.tif 000016
【図４】
2015129714.tif 000017
【図５】
2015129714.tif 000018