JP2016057131A - 走行軌跡推定装置および走行軌跡推定方法 - Google Patents

Abstract

【課題】ドップラー周波数の精度低下を確実に検出し、ドップラー周波数の精度に応じて走行軌跡を補正する走行軌跡推定装置を提供する。
【解決手段】走行軌跡推定装置１０は、位置算出部１１、ＧＰＳ速度算出部１２、ＤＲ速度算出部１３、精度検出部１４および走行軌跡算出部１５を備える。位置算出部１１は、ＧＰＳから得られる擬似距離に基づいて車両の推定位置を算出する。ＧＰＳ速度算出部１２は、ＧＰＳから得られるドップラー周波数に基づいて車両の推定速度ベクトルを算出する。ＤＲ速度算出部１３は、角度センサにより検出される方位変化量と車速パルスとに基づいて車両の基準速度ベクトルを算出する。精度検出部１４は、推定速度ベクトルと基準速度ベクトルとを比較することによりドップラー周波数の精度を検出する。走行軌跡算出部１５は、推定位置と推定速度ベクトルとドップラー周波数の精度とに基づいて走行軌跡を算出する。
【選択図】図１

Description

本発明は、ＧＮＳＳを利用して車両の走行軌跡を推定する走行軌跡推定装置および走行軌跡推定方法に関する。

車両の走行軌跡を推定する走行軌跡推定装置は、車載用ナビゲーション装置等に用いられている。近年、多くの走行軌跡推定装置は、ＧＰＳ（Global Positioning System）を利用して走行軌跡を推定している。例えば、車速は、ＧＰＳから得られるドップラー周波数に基づいて推定されている。

しかし、ドップラー周波数の精度は、車速の低下、衛星の配置、マルチパス等の影響を受けて低下する。そして、ドップラー周波数の精度低下は走行軌跡の精度に悪影響を及ぼす。このため、走行軌跡推定装置はドップラー周波数の精度低下に応じて走行軌跡を補正する必要がある。そのような走行軌跡推定装置として、例えば、特許文献１および２に示すものがある。

特許文献１に記載の走行軌跡推定装置では、車速が低下したとき、ＧＰＳから得られる観測量のうち擬似距離のみを用いて走行軌跡を推定する。これにより、車速の低下によりドップラー周波数の精度が低下したとき、走行軌跡を補正することができる。

特許文献２に記載の走行軌跡推定装置では、使用するＧＰＳ衛星毎にドップラー周波数を算出するとともに、推測航法からドップラー周波数の推定値を算出する。そして、ドップラー周波数と推定値との差分が大きいＧＰＳ衛星を使用対象から除外している。これにより、精度が良いドップラー周波数のみを用いて、走行軌跡を推定することができる。

特許第３２１８８７６号公報 特開２０１２−６３３１３号公報

しかし、特許文献１に記載の走行軌跡推定装置では、車速の低下以外によりドップラー周波数の精度が低下する場合、走行軌跡の精度低下を防止することができない。また、特許文献２に記載の走行軌跡推定装置では、すべてのＧＰＳ衛星においてドップラー周波数の精度が低下する場合、走行軌跡の精度低下を防止することができない。

本発明の目的は、ドップラー周波数の精度低下を確実に検出し、ドップラー周波数の精度に応じて走行軌跡を補正する走行軌跡推定装置を提供することにある。

本発明の走行軌跡推定装置は、位置算出部、第１速度算出部、第２速度算出部、精度検出部および走行軌跡算出部を備える。位置算出部は、ＧＮＳＳから得られる擬似距離に基づいて車両の推定位置を算出する。第１速度算出部は、ＧＮＳＳから得られるドップラー周波数に基づいて車両の推定速度ベクトルを算出する。第２速度算出部は、角度センサにより検出される方位変化量と車速パルスとに基づいて、車両の基準速度ベクトルを算出する。精度検出部は、推定速度ベクトルと基準速度ベクトルとを比較することによりドップラー周波数の精度を検出する。走行軌跡算出部は、推定位置と推定速度ベクトルとドップラー周波数の精度とに基づいて走行軌跡を算出する。

この構成では、精度が高い基準速度ベクトルと推定速度ベクトルとを比較することにより、走行軌跡推定装置はドップラー周波数の精度低下を確実に検出することができる。また、走行軌跡推定装置はドップラー周波数の精度に応じて走行軌跡を補正することができる。

本発明によれば、ドップラー周波数の精度低下を確実に検出し、ドップラー周波数の精度に応じて走行軌跡を補正する走行軌跡推定装置を実現することができる。

第１の実施形態に係る走行軌跡推定装置のブロック図である。 推定速度ベクトルｖ^→ _ＧＰＳと基準速度ベクトルｖ^→ _ＤＲとの関係を示す図である。 車両が走行しているときの角度εの時間変化を示す図である。 図４（Ａ）は、第１の実施形態に係る走行軌跡推定フローの第１例を示すフローチャートである。図４（Ｂ）は工程Ｓ１０３の詳細なフローを示すフローチャートである。 第１の実施形態に係る走行軌跡推定フローの第２例を示すフローチャートである。 図６（Ａ）は、従来の走行軌跡推定装置により推定された走行軌跡を示す図である。図６（Ｂ）は、走行軌跡推定装置１０により推定された走行軌跡を示す図である。

本発明の第１の実施形態に係る走行軌跡推定装置１０について説明する。図１は走行軌跡推定装置１０のブロック図である。走行軌跡推定装置１０は、位置算出部１１、ＧＰＳ速度算出部１２、ＤＲ（Dead Reckoning）速度算出部１３、精度検出部１４および走行軌跡算出部１５を備える。ＧＰＳ速度算出部１２は本発明の第１速度算出部に相当する。ＤＲ速度算出部１３は本発明の第２速度算出部に相当する。また、ＧＰＳは本発明のＧＮＳＳ（Global Navigation Satellite Systems）に相当する。走行軌跡推定装置１０は、車両に設置され、ＧＰＳ、角度センサおよび車速パルス発生器から所定の情報を受け取る。角度センサは、例えば、ジャイロセンサ、地磁気センサ等である。

位置算出部１１は、ＧＰＳから得られる擬似距離に基づいて車両の推定位置ｒ^→ _ＧＰＳを算出する。例えば、単独測位、ＤＧＰＳ（Differential GPS）等を用いて車両の推定位置を算出する。ＧＰＳ速度算出部１２は、ＧＰＳから得られるドップラー周波数に基づいて車両の推定速度ベクトルｖ^→ _ＧＰＳを算出する。

ＤＲ速度算出部１３は、方位変化量と車速パルスとに基づいて車両の基準速度ベクトルｖ^→ _ＤＲを算出する。方位変化量は角度センサにより検出される。車速パルスは車速パルス発生器により検出される。基準速度ベクトルｖ^→ _ＤＲの向きは、車両の走行方向の推定値であり、方位変化量を積算して算出される。基準速度ベクトルｖ^→ _ＤＲの大きさは、車両の速さの推定値であり、単位時間当たりの車速パルス数から算出される。

なお、車速パルス、角度センサ等を用いる自律航法では、時間の経過とともに推定値の精度が低下する。しかし、ＧＰＳの観測量の精度が高いとき、ＧＰＳの観測量に基づいて車速パルス、角度センサ等を所定時間ごとに較正することにより、推定値の精度を高く維持することができる。なお、ＧＰＳの観測量の精度を判定するために、例えば、ＤＯＰ（Dilution of Precision）、信号強度等を用いてもよい。また、精度検出部１４の検出結果を用いてもよい。

精度検出部１４は、推定速度ベクトルｖ^→ _ＧＰＳと基準速度ベクトルｖ^→ _ＤＲとを比較することにより、ドップラー周波数の精度を検出する。例えば、以下のように行う。図２は、推定速度ベクトルｖ^→ _ＧＰＳと基準速度ベクトルｖ^→ _ＤＲとの関係を示す図である。角度εは推定速度ベクトルｖ^→ _ＧＰＳ，ｖ^→ _ＤＲがなす角度である。角度εの絶対値｜ε｜が所定の閾値Ｔｈ_εに比べて大きいか否かにより、ドップラー周波数の精度が低下したか否かを判定することができる。なお、角度εによる判定に加えて、推定速度ベクトルｖ^→ _ＧＰＳおよびｖ^→ _ＤＲの大きさを比較してもよい。また、角度εの分散が所定の閾値に比べて大きいか否かを判定してもよい。

図３は、車両が走行しているときの角度εの時間変化を示す図である。時間ｔ_１から時間ｔ_２までの間でドップラー周波数の精度が低下したことを反映して、時間ｔ_１から時間ｔ_２までの間で角度εが大きくなっている。

走行軌跡算出部１５は、推定位置ｒ^→ _ＧＰＳと推定速度ベクトルｖ^→ _ＧＰＳとドップラー周波数の精度とに基づいて走行軌跡を算出する。例えば、以下のような状態方程式にカルマンフィルタを適用する。
ｘ_ｔ＋１＝Ｆｘ_ｔ＋ｕ_ｔ＋ｗ^ｐ _ｔ
ｙ_ｔ＝Ｈｘ_ｔ＋ｗ^ｏ _ｔ
ここで、ｘ_ｔは車両の位置および速度を成分とする状態ベクトルである。Ｆは位置および速度の関係を表す正方行列である。ｕ_ｔは車両の制御に対応する制御入力ベクトルである。ｗ^ｐ _ｔはプロセス雑音である。ｙ_ｔは推定位置ｒ^→ _ＧＰＳおよび推定速度ベクトルｖ^→ _ＧＰＳの成分からなる観測ベクトルである。Ｈは、非対角成分がほとんど０である疎行列である。ｗ^ｏ _ｔは観測過程で生じる観測雑音である。走行軌跡算出部１５は、カルマンフィルタから得られる状態ベクトルｘ_ｔの推定値を走行軌跡として出力する。

走行軌跡算出部１５は、精度検出部１４がドップラー周波数の精度低下を検出するとき、ドップラー周波数を観測する過程で大きい雑音が生じると仮定して、走行軌跡を算出する。例えば、観測雑音ｗ^ｏ _ｔのうち、ドップラー周波数を観測する過程で生じる部分を大きくする。これにより、ドップラー周波数の精度が低下したときでも、走行軌跡を精度良く算出することができる。

次に、車両の走行軌跡を推定する工程について説明する。図４（Ａ）は第１の実施形態に係る走行軌跡推定フローの第１例を示すフローチャートである。まず、ＧＰＳから得られる擬似距離に基づいて車両の推定位置ｒ^→ _ＧＰＳを算出する（Ｓ１０１）。ＧＰＳから得られるドップラー周波数に基づいて車両の推定速度ベクトルｖ^→ _ＧＰＳを算出する（Ｓ１０２）。ドップラー周波数の精度を検出し、走行軌跡算出部１５のパラメータを調整する（Ｓ１０３）。推定位置ｒ^→ _ＧＰＳおよび推定速度ベクトルｖ^→ _ＧＰＳに基づいて車両の走行軌跡を算出する（Ｓ１０４）。

図４（Ｂ）は工程Ｓ１０３の詳細なフローを示すフローチャートである。まず、角度センサにより検出される方位変化量と車速パルスとに基づいて、車両の基準速度ベクトルｖ^→ _ＤＲを算出する（Ｓ３０１）。推定速度ベクトルｖ^→ _ＧＰＳと基準速度ベクトルｖ^→ _ＤＲとの間の角度εを算出する（Ｓ３０２）。角度εの絶対値｜ε｜が閾値Ｔｈ_ε以上であるとき（Ｓ３０３：Ｙｅｓ）、観測雑音ｗ^ｏ _ｔのうち、ドップラー周波数を観測する過程で生じる部分を大きくする（Ｓ３０４）。角度εの絶対値｜ε｜が閾値Ｔｈ_ε未満であるとき、観測雑音ｗ^ｏ _ｔを調整せずに終了する（Ｓ３０３：Ｎｏ）。

また、図５のように、ＧＰＳの観測量の精度が低下したときのみ、ドップラー周波数の精度を検出してもよい。図５は第１の実施形態に係る走行軌跡推定フローの第２例を示すフローチャートである。

まず、推定位置ｒ^→ _ＧＰＳおよび推定速度ベクトルｖ^→ _ＧＰＳを算出する（Ｓ１１１，Ｓ１１２）。ＧＰＳの観測量の精度が低下しているとき（Ｓ１１３：Ｙｅｓ）、ドップラー周波数の精度を検出し、走行軌跡算出部１５のパラメータを調整する（Ｓ１１４）。そして、車両の走行軌跡を算出する（Ｓ１１５）。ＧＰＳの観測量の精度が低下していないとき（Ｓ１１３：Ｎｏ）、工程Ｓ１１４を行わず、車両の走行軌跡を算出する（Ｓ１１５）。

図６（Ａ）は、従来の走行軌跡推定装置により推定された走行軌跡を示す図である。従来の走行軌跡推定装置は、位置算出部１１、ＧＰＳ速度算出部１２および走行軌跡算出部１５から構成されている。図６（Ｂ）は、走行軌跡推定装置１０により推定された走行軌跡を示す図である。車両は道路を走行している。車両の推定位置は、一定時間ごとに推定され、丸印で示されている。

従来の走行軌跡推定装置では、推定された走行軌跡は時間ｔ_１から時間ｔ_２までの間で実際の走行軌跡から大きく外れている。一方、走行軌跡推定装置１０は従来の走行軌跡推定装置に比べて走行軌跡を精度良く推定している。

第１の実施形態によると、走行軌跡推定装置１０は基準速度ベクトルｖ^→ _ＤＲと推定速度ベクトルｖ^→ _ＧＰＳとの間の角度εを算出する。そして、角度εの絶対値｜ε｜に基づいて観測雑音ｗ^ｏ _ｔを調整し、車両の走行軌跡を算出する。これにより、走行軌跡推定装置１０は、ドップラー周波数の精度低下を確実に検出し、ドップラー周波数の精度に応じて走行軌跡を補正することができる。

１０…走行軌跡推定装置
１１…位置算出部
１２…ＧＰＳ速度算出部
１３…ＤＲ速度算出部
１４…精度算出部
１５…走行軌跡算出部

Claims (4)

1. ＧＮＳＳから得られる擬似距離に基づいて、車両の推定位置を算出する位置算出部と、
   前記ＧＮＳＳから得られるドップラー周波数に基づいて、前記車両の推定速度ベクトルを算出する第１速度算出部と、
   角度センサにより検出される方位変化量と車速パルスとに基づいて、前記車両の基準速度ベクトルを算出する第２速度算出部と、
   前記推定速度ベクトルと前記基準速度ベクトルとを比較することにより、前記ドップラー周波数の精度を検出する精度検出部と、
   前記推定位置と前記推定速度ベクトルと前記精度とに基づいて走行軌跡を算出する走行軌跡算出部とを備える走行軌跡推定装置。
2. 請求項１に記載の走行軌跡推定装置であって、
   前記精度検出部は、前記推定速度ベクトルが向く方位と前記基準速度ベクトルが向く方位との差分に基づいて、前記精度を検出する、走行軌跡推定装置。
3. 請求項１または２に記載の走行軌跡推定装置であって、
   前記走行軌跡算出部は、前記精度検出部が前記精度の低下を検出するとき、前記ドップラー周波数を観測する過程で大きい雑音が生じると仮定して、前記走行軌跡を算出する、走行軌跡推定装置。
4. ＧＮＳＳから得られる擬似距離に基づいて、車両の推定位置を算出する工程と、
   前記ＧＮＳＳから得られるドップラー周波数に基づいて、前記車両の推定速度ベクトルを算出する工程と、
   角度センサにより検出される方位変化量と車速パルスとに基づいて、前記車両の基準速度ベクトルを算出する工程と、
   前記推定速度ベクトルと前記基準速度ベクトルとを比較することにより、前記ドップラー周波数の精度を検出する工程と、
   前記推定位置と前記推定速度ベクトルと前記精度とに基づいて走行軌跡を算出する工程とを備える走行軌跡推定方法。

Images