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