JP6463551B2 - 進行方向推定装置 - Google Patents

Description

本発明は、走行中の車両の進行方向を推定するための進行方向推定装置に関する。

従来の進行方向推定装置として、例えば、特許文献１に記載の進行方向推定装置がある。特許文献１に記載の進行方向推定装置は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機から得られる進行方向信号と、回転角速度信号とを入力として、車両の進行方向を推定する。特許文献１に記載の進行方向推定装置は、車両が高速で移動している場合には、進行方向を推定する際に、ＧＰＳ受信機から得られる進行方向信号への依存度を大きくし、回転角速度信号への依存度を小さくする。一方、車両が低速で移動している場合には、特許文献１に記載の進行方向推定装置は、進行方向を推定する際に、ＧＰＳ受信機から得られる進行方向信号への依存度を小さくし、回転角速度信号への依存度を大きくする。

特開平３−２８２３２４

従来の進行方向推定装置は、ＧＰＳ受信機から得られる進行方向信号への依存度と、回転角速度への依存度とを車速に応じて変更して進行方向を演算している。しかし、ＧＰＳ受信機から得られる進行方向信号と、真に求めたい進行方向との誤差は、必ずしも車速に依存していない。そのため、従来の進行方向推定装置では、必ずしも車両の進行方向を精度良く推定することができなかった。特に、従来の進行方向推定装置では、ＧＰＳ受信機から得られる車両の位置情報の更新間隔が長い場合には、実際の進行方向に対する推定される進行方向の誤差が大きくなるおそれがあった。

本発明は、上記の課題を解決するためになされたもので、車両の位置情報の更新間隔が長い場合にも、車両の進行方向を精度良く推定可能な進行方向推定装置を得ることを目的とする。

この発明における進行方向推定装置は、位置検出部と角速度検出部とを備える車両に搭載されて車両の進行方向を推定する進行方向推定装置であって、位置検出部で検出され所定の時間間隔で更新される車両の位置の変化に基づいて第一の角度を算出する第一角度演算部と、角速度検出部で検出された車両の回転角速度と位置検出部で検出される車両の位置の更新間隔とに基づいて第二の角度を算出する第二角度演算部と、第一の角度および第二の角度に基づいて車両の進行方向を算出する進行方向演算部とを備え、第二角度演算部は、回転角速度γと更新間隔Ｔとを用いて、θ _ｃ ＝（γ×Ｔ）／２の演算によって第二の角度θ _ｃ を算出するものである。

この発明における進行方向推定装置は、位置検出部で検出され所定の時間間隔で更新される車両の位置の変化に基づいて第一の角度を算出する第一角度演算部と、角速度検出部で検出された車両の回転角速度と位置検出部で検出される車両の位置の更新間隔とに基づいて第二の角度を算出する第二角度演算部と、第一の角度および第二の角度に基づいて車両の進行方向を算出する進行方向演算部とを備えるので、車両の位置情報の更新間隔が長い場合にも、車両の進行方向を精度良く推定することが可能となる。

本発明の実施の形態１による進行方向推定装置を搭載した車両のシステム構成の一例を示す図である。 本発明の実施の形態１による進行方向推定装置の概略構成と周辺のブロックとを示すブロック図である。 本発明の実施の形態１による進行方向推定装置の処理手順の一例を示すフローチャートである。 本発明の実施の形態１による進行方向推定装置における第一角度演算部の動作を説明するための図である。 本発明の実施の形態１による進行方向推定装置の動作を説明するための図である。 本発明の実施の形態１による進行方向推定装置を搭載した車両システムの動作を説明するための図である。 本発明の実施の形態１による進行方向推定装置を搭載した車両システムのハードウェア構成の一例を示す図である。 本発明の実施の形態２による進行方向推定装置の概略構成と周辺のブロックとを示すブロック図である。 本発明の実施の形態２による進行方向推定装置の動作を説明するための図である。 本発明の実施の形態２による進行方向推定装置の処理手順の一例を示すフローチャートである。 本発明の実施の形態３による進行方向推定装置の概略構成と周辺のブロックとを示すブロック図である。 本発明の実施の形態４による進行方向推定装置の概略構成と周辺のブロックとを示すブロック図である。

実施の形態１．
図１は、本実施の形態１における進行方向推定装置１１を搭載した車両１のシステム構成の一例を示す図である。図１において、車両１は、車輪２、ハンドル３、操舵部４、アンテナ６、衛星信号受信部７、車速センサ８、操舵制御部９、地図配信部１０、進行方向推定装置１１およびヨーレートセンサ１２を備える。図１に示す車両システムでは、操舵部４は、操舵制御部９から入力された操舵指令値に従って動作し、車両１が道路に沿って走行するように操舵制御が行われる。操舵部４は、例えば、ＥＰＳ（電動パワーステアリング）用モータおよびＥＣＵ（Ｅｌｃｔｒｏｎｉｃ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）から構成される。ＥＣＵは、マイコンなどの計算機およびメモリを備え、専用の電子回路を備えても良い。操舵部４が操舵制御部９からの操舵指令値に従って動作することで、ハンドル３の回転および車輪２の向きを制御することができる。

アンテナ６は、車両１の外部の衛星５から送信される衛星信号を受信し、受信した衛星信号を衛星信号受信部７へと出力する。衛星５は、例えば、複数のＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星により構成される。なお、本実施の形態では、衛星５がＧＰＳ衛星により構成される場合を示すが、これに限定されることはなく、ＧＬＯＮＡＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）等の他の測位衛星を用いてもよい。

衛星信号受信部７はＧＰＳ受信機で構成され、アンテナ６で受信した衛星信号を処理し、車両１の位置を検出して位置信号を生成する。衛星信号受信部７は、更新間隔Ｔ毎に車両１の位置を検出し、位置信号を更新する。すなわち、更新間隔Ｔは、衛星信号受信部７における車両１の位置の検出間隔であり、衛星信号受信部７から出力される位置信号の更新間隔である。また、衛星信号受信部７は、位置信号を地図配信部１０および進行方向推定装置１１へと出力する。すなわち、衛星信号受信部７は、車両１の位置を検出する位置検出部として機能する。なお、本実施の形態では、衛星信号受信部７としてＧＰＳ受信機を用いる構成としたが、ＧＬＯＮＡＳＳ等の他のシステムに対応した受信機を用いてもよい。また、検出する位置を高精度とするために、衛星信号受信部７は、衛星５から補正情報を取得する構成、または、インターネット経由で補正情報を取得する構成としても良い。また、衛星信号受信部７は、ＤＧＰＳ（Ｄｉｆｆｅｒｅｎｔｉａｌ ＧＰＳ）に対応した構成としてもよい。

ヨーレートセンサ１２は、車両１の回転角速度であるヨーレートを検出し、検出されたヨーレートを操舵制御部９および進行方向推定装置１１へと出力する。すなわち、ヨーレートセンサ１２は、車両１の回転角速度を検出する角速度検出部として機能する。進行方向推定装置１１は、ヨーレートセンサ１２から入力されたヨーレート、衛星信号受信部７から入力された位置信号に基づいて、車両１の進行方向を推定し、操舵制御部９へと出力する。なお、本実施の形態の進行方向推定装置１１は、衛星信号受信部７における車両１の位置の更新間隔Ｔ毎に、車両１の進行方向を推定する。言い換えると、本実施の形態の進行方向推定装置１１は、衛星信号受信部７において車両１の位置が検出されるたびに、車両１の進行方向を推定する。進行方向推定装置１１で求められる進行方向は、例えば、所定の方向からの角度で表される。進行方向推定装置１１は、例えばＥＣＵで構成される。なお、進行方向推定装置１１の内部構成および動作の詳細については、後述する。

地図配信部１０は、衛星信号受信部７からの位置信号に基づいて、地図上における車両１の位置および車両１の前方の道路情報を出力する。地図配信部１０は、道路情報として、例えば走行する車線における各地点の緯度および経度、車線数、曲率等を出力する。なお、地図配信部１０は、進行方向推定装置１１から出力される車両１の進行方向を入力として、車両１の位置および車両１の前方の道路情報を、車両１もしくは車両１の周辺の位置を原点とし、車両１の進行方向を所定の座標軸とする座標系へ変換したものを出力してもよい。地図配信部１０は、例えばＥＣＵで構成される。

車速センサ８は、車両１の走行速度を検出し、検出された車速Ｖを操舵制御部９へ出力する。なお、図１に示す車両１では、４つの車輪２に設置された車速センサ８のうち、１つの車速センサ８から車速Ｖが検出される構成としているが、他の構成とすることもできる。例えば、４つの車速センサ８で検出された速度の平均を車速Ｖとしても良い。または、車両１の後方の２つの車輪２に設置された車速センサ８を用いて、２つの車速センサ８で検出された速度の平均を車速Ｖとしてもよい。

操舵制御部９は、操舵部４へ送信する操舵指令値を生成するＥＣＵとして構成される。操舵制御部９は、車速センサ８から入力された車速Ｖと、進行方向推定装置１１から入力された進行方向と、地図配信部１０から入力された車両の位置および道路情報と、ヨーレートセンサ１２から入力されたヨーレートとに基づいて、目標とする操舵角を求め、操舵指令値として操舵部４へと出力する。また、操舵制御部９は、操舵部４の状態を取得する。なお、図１では、衛星信号受信部７と進行方向推定装置１１とは別の構成としたが、１つの構成としてもよく、例えば同じＥＣＵの中で両方の処理を行ってもよい。さらに、操舵制御部９も含めて１つの構成としてもよい。

次に、進行方向推定装置１１の構成および動作について、図２および図３を参照しながら説明する。図２は、本実施の形態の進行方向推定装置１１の概略構成と、周辺のブロックとを示すブロック図である。また、図３は、本実施の形態の進行方向推定装置１１の処理手順の一例を示すフローチャートである。図２において、進行方向推定装置１１は、第一角度演算部２１と、第二角度演算部２２と、進行方向演算部２３とを有する。以下、図３も用いて進行方向推定装置１１の具体的な動作を説明する。

まず、第一角度演算部２１は、衛星信号受信部７から現在の車両１の位置を取得する（Ｓ１）。なお、第一角度演算部２１は、衛星信号受信部７における車両１の位置の更新間隔Ｔ毎に、車両１の位置を取得する。ここで、現在の車両１の位置とは、衛星信号受信部７において検出された最新の車両１の位置を意味する。言い換えると、現在の車両１の位置とは、衛星信号受信部７において過去の直近の更新時刻で更新された車両１の位置である。第一角度演算部２１は、前回に取得した車両１の位置も記憶する。前回に取得した車両１の位置は、現在の車両１の位置と比較して、更新間隔Ｔだけ過去における車両１の位置となる。すなわち、前回に取得した車両１の位置とは、衛星信号受信部７において検出された最新の車両１の位置と比較して、１つ前の検出時刻に検出された車両１の位置となる。言い換えると、前回に取得した車両１の位置とは、衛星信号受信部７において最新の車両１の位置に更新される直前の車両１の位置となる。進行方向推定装置１１が、車両１の進行方向を推定する時刻を現在の時刻とすると、現在の車両１の位置は、現在に最も近い過去の更新時刻Ｔ１で更新された車両１の位置である。また、前回に取得した車両１の位置とは、更新時刻Ｔ１よりも更新間隔Ｔだけ過去に更新された車両１の位置である。次に、第一角度演算部２１は、現在の車両１の位置（Ｘ_ｎ、Ｙ_ｎ）および前回に取得した車両１の位置（Ｘ_ｎ−１、Ｙ_ｎ−１）に基づいて、第一の角度を演算する（Ｓ２）。

図４は、本実施の形態の進行方向推定装置１１における第一角度演算部２１の動作を説明するための図である。図４において、例えば、Ｘ方向は経度方向（東西方向）の位置、Ｙ方向は緯度方向（南北方向）の位置を表す。また、図４は、前回に取得した車両１の位置３１（Ｘ_ｎ−１、Ｙ_ｎ−１）から現在の車両１の位置３２（Ｘ_ｎ、Ｙ_ｎ）まで車速Ｖで移動することを表しており、Ｖｃｏｓθ_Ｇは車速のＸ方向成分、Ｖｓｉｎθ_Ｇは車速のＹ方向成分である。ここで、Ｘ_ｎ、Ｙ_ｎは、基準位置からの相対位置として、それぞれ経度方向、緯度方向の位置を表した位置とする。

なお、本実施の形態では、基準位置として現在の車両１の位置３２（Ｘ_ｎ、Ｙ_ｎ）を使用するが、車両１の近傍の所定の位置を基準位置としてもよい。Ｘ_ｎ、Ｙ_ｎの単位は、ｍ（メートル）とする。衛星信号受信部７から入力される車両１の位置信号が経度と緯度とで表されている場合には、第一角度演算部２１は、位置信号をＸ_ｎ、Ｙ_ｎに換算する。第一角度演算部２１は、現在の車両１の位置３２（Ｘ_ｎ、Ｙ_ｎ）および前回に取得した車両１の位置３１（Ｘ_ｎ−１、Ｙ_ｎ−１）を用いて、式（１）によって第一の角度θ_Ｇを算出する。式（１）において、ａｒｃｔａｎ（）は逆正接を求める関数である。以上が、図３のＳ２における第一角度演算部２１の動作である。

次に、第二角度演算部２２は、ヨーレートセンサ１２から入力されるヨーレートγと、衛星信号受信部７から入力される車両１の位置の更新間隔Ｔとを用いて、式（２）によって第二の角度θ_Ｃを算出する（Ｓ３）。なお、本実施の形態の進行方向推定装置１１では、車両１の位置の更新間隔Ｔは、予め定められた更新間隔、例えば１秒を用いる。したがって、衛星信号受信部７から入力される更新間隔Ｔは、固定値となる。更新間隔Ｔが固定値であれば、衛星信号受信部７から更新間隔Ｔを入力する代わりに、第二角度演算部２２が更新間隔Ｔを予め記憶しておいてもよい。なお、通常は、第二角度演算部２２は、ヨーレートセンサ１２から入力された最新のヨーレートγを用いて第二の角度θ_Ｃを算出する。

次に、進行方向演算部２３は、第一角度演算部２１によって算出された第一の角度θ_Ｇと、第二角度演算部２２によって算出された第二の角度θ_Ｃを用いて、式（３）によって車両１の進行方向Ψを算出する（Ｓ４）。

次に、進行方向推定装置１１は、処理を終了すべきか否かを判断する（Ｓ５）。処理を終了すべきと判断すると、進行方向推定装置１１は処理を終了する。処理を終了すべきではないと判断すると、進行方向推定装置１１の処理はＳ６へと移行する。Ｓ６では、進行方向推定装置１１は、Ｓ１で第一角度演算部２１が車両１の位置を取得してからの経過時間が、更新間隔Ｔに達したか否かを判断する。更新間隔Ｔだけの時間が経過していない場合、進行方向推定装置１１は、更新間隔Ｔだけの時間が経過するまで待機する。更新間隔Ｔだけの時間が経過した場合、進行方向推定装置１１の処理はＳ１へと戻る。

進行方向推定装置１１は、以上のように動作して、車両１の進行方向Ψを推定する。ここで、進行方向演算部２３が、第一の角度θ_Ｇに第二の角度θ_Ｃを加算して、車両１の進行方向Ψを算出する理由を説明する。図５は、本実施の形態の進行方向推定装置１１の動作を説明するための図である。図５において、例えば、Ｘ方向は経度方向（東西方向）の位置、Ｙ方向は緯度方向（南北方向）の位置を表す。図５に示すように、定常的に旋回を行っている車両１において、車両１の位置が（Ｘ_ｎ−１、Ｙ_ｎ−１）から（Ｘ_ｎ、Ｙ_ｎ）へと変化した時、車両１の位置から求められる第一の角度は、図５に示すθ_Ｇとなる。ところが、車両１は図５に示すように旋回円の接線方向を向いているため、実際の車両の進行方向Ψとの間に誤差が発生してしまう。この誤差は、衛星信号受信部７における車両１の位置の更新間隔Ｔが長くなるほど大きくなる。

そこで、時刻ｎＴにおける車速をＶ_ｎとし、位置の更新間隔Ｔの間の車速Ｖ_ｎの変化が十分小さいと近似した場合に、図５に示す時刻（ｎ−１）Ｔから時刻ｎＴにおける車両１の位置の変化は、式（４）および式（５）で表わされる。時刻（ｎ−１）Ｔにおける車両１の位置は（Ｘ_ｎ−１、Ｙ_ｎ−１）、時刻ｎＴにおける車両１の位置は（Ｘ_ｎ、Ｙ_ｎ）である。ここで、式における添字ｎは、時刻ｎＴにおける各変数を表わす。なお、ｎは整数である。

式（１）、式（４）、式（５）より、車両１の位置変化から計算される時刻ｎＴにおける第一の角度θ_Gｎは式（６）で表わされる。また、式（６）より、時刻ｎＴにおける実際の車両１の進行方向Ψ_ｎは、式（７）で表わされる。よって、進行方向演算部２３において、時刻ｎＴにおける第一の角度θ_Gｎに、時刻ｎＴにおける第二の角度θ_Cｎを加算することにより、時刻ｎＴにおける実際の車両１の進行方向Ψ_ｎを推定することができる。以上で述べたことから、進行方向演算部２３は、第一の角度θ_Ｇに第二の角度θ_Ｃを加算して、車両１の進行方向Ψを算出する。

次に、操舵制御部９および操舵部４の動作について述べる。操舵制御部９は、地図配信部１０から入力される車両１の位置と、地図配信部１０から入力される車両１が走行する前方の道路情報と、進行方向推定装置１１から入力される車両の進行方向と、ヨーレートセンサ１２から入力される車両のヨーレートとに基づいて、例えば式（８）によって車両１が道路に沿って走行するために必要な操舵角を求めて、操舵指令値θ_ｒｅｆとして操舵部４へと出力する。

式（８）において、ｋ_１、ｋ_２、ｋ_３はそれぞれ制御ゲインである。図６は、本実施の形態の進行方向推定装置１１を搭載した車両システムの動作を説明するための図であり、特に操舵制御部９の動作を説明するための図である。図６を用いて、式（８）におけるｙ_Ｌｄ、ｅ_Ｌｄについて説明する。式（８）におけるｙ_Ｌｄは、現在の進行方向に距離Ｌｄだけ進んだ位置３３と、目標コース３４との距離の差である。ここで、距離Ｌｄは、現在の車速Ｖで、所定時間Ｔｄだけ進んだ場合の距離である。また、式（８）におけるｅ_Ｌｄは、現在の進行方向に距離Ｌｄだけ進んだ位置３３と、目標コース３４との角度の差である。

操舵部４は、操舵制御部９から入力された操舵指令値に基づいて操舵を行い、道路に沿って車両１を走行させる。以上のように、本実施の形態の進行方向推定装置１１は、位置検出部（衛星信号受信部７）で検出された車両１の位置に基づいて第一の角度を算出する第一角度演算部２１と、角速度検出部（ヨーレートセンサ１２）で検出された車両１の回転角速度と位置検出部における車両１の位置の更新間隔とに基づいて第二の角度を算出する第二角度演算部２２と、第一の角度および第二の角度に基づいて車両１の進行方向を算出する進行方向演算部２３とを備える。本実施の形態の進行方向推定装置１１によれば、
車両１の位置情報の更新間隔が長い場合にも、車両１の進行方向を精度よく推定することが可能となる。

図７は、本実施の形態の進行方向推定装置１１を搭載した車両システムのハードウェア構成の一例を示す図である。ＥＣＵ１３ａは、操舵制御部９および進行方向推定装置１１の機能を実現する。また、ＥＣＵ１３ｂは、地図配信部１０の機能を実現する。地図配信部１０の機能は、別に搭載されるカーナビゲーション装置の機能を使用して実現することもできる。また、ＥＣＵ１３ｃおよびモータ１５は、操舵部４の機能を実現する。また、ＧＰＳ受信機１４は、衛星信号受信部７の機能を実現する。

実施の形態２．
実施の形態１における進行方向推定装置１１は、衛星信号受信部７における車両１の位置の更新間隔Ｔ毎に、車両１の進行方向を推定していた。しかし、車両１の位置の更新間隔Ｔよりも、進行方向推定装置１１における進行方向の推定間隔Ｔ_ａが短い場合も考えられる。例えば、Ｔ＝１秒、Ｔ_ａ＝３０ミリ秒の場合である。本実施の形態の進行方向推定装置１１は、このような場合を想定したものである。

図８は、本実施の形態の進行方向推定装置１１の概略構成と、周辺のブロックとを示すブロック図である。本実施の形態の進行方向推定装置１１が搭載される車両１のシステム構成は、図１と同様である。図８において、進行方向推定装置１１は、第一角度演算部２１と、第二角度演算部２２と、第三角度演算部２４と、進行方向演算部２３ｂとを有する。本実施の形態の進行方向推定装置１１において、第一角度演算部２１および第二角度演算部２２は、実施の形態１におけるものと同様のものであり、衛星信号受信部７における車両１の位置の更新間隔Ｔ毎に、第一の角度θ_Ｇおよび第二の角度θ_Ｃを算出する。

第三角度演算部２４は、ヨーレートセンサ１２から入力されるヨーレートγを用いて、進行方向の推定間隔Ｔ_ａ毎に、第三の角度を算出する。進行方向演算部２３ｂは、第一の角度と、第二の角度と、第三の角度とに基づいて、進行方向の推定間隔Ｔ_ａ毎に、車両１の進行方向を求める。図９は、本実施の形態の進行方向推定装置１１の動作を説明するための図である。説明を簡単にするため、図９では、車両１の位置の更新間隔Ｔは、進行方向の推定間隔Ｔ_ａの３倍であるものとしている。図９において、横軸は時刻を表し、縦軸は進行方向を表す。また、図９において、進行方向は所定方向からの角度で表される。

図９に示すように、本実施の形態の進行方向推定装置１１は、時刻ｎＴにおいて、第一の角度θ_Ｇに第二の角度θ_Ｃを加算して、進行方向Ψ_ｎを推定する。この動作は、実施の形態１で述べた動作と同様である。ここで、時刻ｎＴは、衛星信号受信部７が車両１の位置を最新の情報に更新した時刻Ｔ１と同一とする。時刻ｎＴから時刻（ｎ＋１）Ｔまでの間においては、進行方向推定装置１１は、進行方向の推定間隔Ｔ_ａ毎に、ヨーレートγを積分した値を加算して新たな進行方向とする。ヨーレートγの積分値を積算したものが第三の角度である。ヨーレートγとしては、随時検出された値を用いればよい。ただし、ヨーレートとして、時刻ｎＴにおけるヨーレートγ_ｎを用いれば、時刻ｎＴから時刻（ｎ＋１）Ｔまでの間においては、ヨーレートγ_ｎは変化しないため、積分値は時刻ｎＴにおけるヨーレートγ_ｎと、時刻ｎＴからの経過時間との積で計算できる。図９において、時刻ｎＴ＋Ｔ_ａにおける第三の角度は、ヨーレートγ_ｎと時間Ｔ_ａとの積で求められる。また、図９において、時刻ｎＴ＋２Ｔ_ａにおける第三の角度は、ヨーレートγ_ｎと時間２Ｔ_ａとの積で求められる。

第三角度演算部２４は、車両１の位置の更新間隔Ｔ毎に、第三の角度をリセットして０にする。したがって、時刻ｎＴにおいては、第三の角度は０となる。時刻ｎＴから時刻（ｎ＋１）Ｔまでの間においては、第三角度演算部２４は、進行方向の推定間隔Ｔ_ａ毎に、時刻ｎＴからの経過時間分のヨーレートγを積分し、第三の角度として出力する。すなわち、第三の角度は、衛星信号受信部７が車両１の位置を更新した後の経過時間分のヨーレートを積分したものである。進行方向演算部２３ｂは、進行方向の推定間隔Ｔ_ａ毎に、第一の角度と、第二の角度と、第三の角度とを加算して、車両１の進行方向を求める。

図１０は、本実施の形態の進行方向推定装置１１の処理手順の一例を示すフローチャートである。図１０を用いて、本実施の形態の進行方向推定装置１１の処理を説明する。まず、第三角度演算部２４は、第三の角度をリセットし、第三の角度の値を０にする（Ｓ７）。次に、第一角度演算部２１は、衛星信号受信部７から現在の車両１の位置を取得する（Ｓ１）。次に、第一角度演算部２１は、現在の車両１の位置と、前回に取得した車両１の位置とに基づいて、第一の角度を演算する（Ｓ２）。次に、第二角度演算部２２は、ヨーレートセンサ１２から入力されるヨーレートγと、衛星信号受信部７から入力される車両１の位置の更新間隔Ｔとを用いて、第二の角度θ_Ｃを算出する（Ｓ３）。なお、Ｓ１、Ｓ２およびＳ３の処理は、実施の形態１における処理と同様である。

次に、進行方向演算部２３は、第一の角度と、第二の角度と、第三の角度とを用いて、車両１の進行方向を算出する（Ｓ４）。なお、第三の角度がリセットされた状態では、Ｓ４の処理は、実施の形態１における処理と同様となる。次に、進行方向推定装置１１は、処理を終了すべきか否かを判断する（Ｓ５）。処理を終了すべきと判断すると、進行方向推定装置１１は処理を終了する。処理を終了すべきではないと判断すると、進行方向推定装置１１の処理はＳ６へと移行する。Ｓ６では、進行方向推定装置１１は、Ｓ１で第一角度演算部２１が車両１の位置を取得してからの経過時間が、更新間隔Ｔに達したか否かを判断する。更新間隔Ｔだけの時間が経過していない場合、進行方向推定装置１１の処理はＳ８へと移行する。更新間隔Ｔだけの時間が経過した場合、進行方向推定装置１１の処理はＳ７へと戻る。

Ｓ８では、進行方向推定装置１１は、Ｓ４で進行方向演算部２３が車両１の進行方向を算出してからの経過時間が、推定間隔Ｔ_ａに達したか否かを判断する。推定間隔Ｔ_ａだけの時間が経過していない場合、進行方向推定装置１１は、推定間隔Ｔ_ａだけの時間が経過するまで待機する。推定間隔Ｔ_ａだけの時間が経過した場合、進行方向推定装置１１の処理はＳ９へと移行する。Ｓ９では、第三角度演算部２４は、第三の角度を算出する。Ｓ９の処理が終わると、進行方向推定装置１１の処理はＳ４へと戻る。本実施の形態の進行方向推定装置１１は、以上のように動作する。

本実施の形態の進行方向推定装置１１によれば、車両１の位置の更新間隔Ｔ毎に進行方向を更新するよりも進行方向の出力が連続的となる。したがって、本実施の形態の進行方向推定装置１１を搭載した車両１では、操舵制御部９における操舵指令値の値を滑らかにすることが可能となり、乗員の乗り心地を改善することが可能となる。なお、ここでは進行方向推定装置１１について説明したが、進行方向推定装置１１の外部に、時刻ｎＴから時刻（ｎ＋１）Ｔまでの間の車両１の位置を補間する構成を備え、本実施の形態の進行方向推定装置１１と合わせて車両に搭載してもよい。

実施の形態３．
実施の形態１における進行方向推定装置１１においては、衛星信号受信部７における車両１の位置の更新間隔Ｔは、予め定められた時間間隔であるとした。しかし、車両１の位置の更新間隔Ｔは、必ずしも予め定められた時間間隔である必要はない。本実施の形態の進行方向推定装置１１は、車両１の位置の更新間隔Ｔが予め定められていない場合を想定したものである。

図１１は、本実施の形態の進行方向推定装置１１の概略構成と、周辺のブロックとを示すブロック図である。本実施の形態の進行方向推定装置１１が搭載される車両１のシステム構成は、基本的には図１と同様である。ただし、衛星信号受信部７は、位置信号が更新された時刻の情報を位置信号に付加して出力するものとする。図１１において、進行方向推定装置１１は、第一角度演算部２１と、第二角度演算部２２ｂと、進行方向演算部２３と、更新間隔演算部２５とを有する。本実施の形態の進行方向推定装置１１において、第一角度演算部２１および進行方向演算部２３は、実施の形態１におけるものと同様のものである。また、本実施の形態の進行方向推定装置１１において、第二角度演算部２２ｂは、車両１の位置の更新間隔Ｔが更新間隔演算部２５から入力される点を除いては、実施の形態１におけるものと同様のものである。

更新間隔演算部２５は、衛星信号受信部７から出力される位置信号が更新された時刻の情報を参照し、更新された時刻の差分を計算して更新間隔Ｔを求める。衛星５から送信される衛星信号には時刻情報も含まれているため、衛星信号受信部７は、この情報を利用して位置を検出した時刻を出力する。本実施の形態の進行方向推定装置１１によれば、衛星信号受信部７から出力される位置情報の更新間隔が変動した場合や、不連続になった場合にも、車両１の進行方向を精度よく推定することが可能となる。

実施の形態４．
実施の形態１における進行方向推定装置１１は、車両１の位置の更新間隔Ｔの間における車速の変化が十分小さいと仮定して構成されていた。しかし、車両１の位置の更新間隔Ｔの間において、必ずしも車速は一定ではない。本実施の形態の進行方向推定装置１１は、車両１の位置の更新間隔Ｔの間において、無視できない程度に車速が変化する場合を想定したものである。

図１２は、本実施の形態の進行方向推定装置１１の概略構成と、周辺のブロックとを示すブロック図である。本実施の形態の進行方向推定装置１１が搭載される車両１のシステム構成は、加速度センサ１６を新たに備える他は、図１と同様である。加速度センサ１６は、車両１の前後方向の加速度（進行方向の加速度）を検出する。図１２において、進行方向推定装置１１は、第一角度演算部２１と、第二角度演算部２２ｃと、進行方向演算部２３と、更新間隔演算部２５とを有する。本実施の形態の進行方向推定装置１１において、第一角度演算部２１および進行方向演算部２３は、実施の形態１におけるものと同様のものである。また、本実施の形態の進行方向推定装置１１において、更新間隔演算部２５は、実施の形態３におけるものと同様のものである。

第二角度演算部２２ｃは、ヨーレートセンサ１２から入力されるヨーレートγと、更新間隔演算部２５から入力される車両１の位置の更新間隔Ｔとを用いて、式（９）によって第二の角度θ_Ｃを算出する。式（９）において、ｋ_ａは、加速度センサで検出される加速度に応じて変化する係数である。式（９）は、係数ｋ_ａが乗算されている他は、式（２）と同一である。進行方向演算部２３は、第一の角度と第二の角度とを加算して、車両１の進行方向を求める。

車速の変化が十分小さいと仮定して進行方向を推定すると、車両１が加速している場合には車両１の進行方向を実際よりも大きく推定してしまい、車両１が減速している場合には車両１の進行方向を実際よりも小さく推定してしまう。したがって、本実施の形態の進行方向推定装置１１において、第二角度演算部２２ｃは、車両１の加速度に基づいて、係数ｋ_ａの値を調整し、第二の角度θ_Ｃの大きさを調整する。具体的には、本実施の形態の進行方向推定装置１１において、第二角度演算部２２ｃは、車両１が加速中の場合には係数ｋ_ａを１よりも小さくし、車両１が減速中の場合には係数ｋ_ａを１よりも大きくする。また、第二角度演算部２２ｃは、車両１の加速の度合いが大きいほど係数ｋ_ａの値を大きくし、車両１の減速の度合いが大きいほど係数ｋ_ａの値を小さくする。なお、車両１が加速も減速もしていない場合には、係数ｋ_ａの値は１となる。この場合、式（９）は式（２）と同一となる。本実施の形態の進行方向推定装置１１によれば、車両１が加減速中であっても、より高精度に進行方向を推定することが可能となる。

１ 車両、２ 車輪、３ ハンドル、４ 操舵部、５ 衛星、６ アンテナ、７ 衛星信号受信部、８ 車速センサ、９ 操舵制御部、１０ 地図配信部、１１ 進行方向推定装置、１２ ヨーレートセンサ、１３ａ、１３ｂ ＥＣＵ、１４ ＧＰＳ受信機、１５ モータ、１６ 加速度センサ、２１ 第一角度演算部、２２、２２ｂ、２２ｃ 第二角度演算部、２３、２３ｂ 進行方向演算部、２４ 第三角度演算部、２５ 更新間隔演算部、３１ 前回に取得した車両の位置、３２ 現在の車両の位置、３３ 現在の進行方向に進んだ位置、３４ 目標コース。

Claims (6)

1. 位置検出部と角速度検出部とを備える車両に搭載されて前記車両の進行方向を推定する進行方向推定装置であって、
   前記位置検出部で検出され所定の時間間隔で更新される前記車両の位置の変化に基づいて第一の角度を算出する第一角度演算部と、
   前記角速度検出部で検出された前記車両の回転角速度と前記位置検出部で検出される前記車両の位置の更新間隔とに基づいて第二の角度を算出する第二角度演算部と、
   前記第一の角度および前記第二の角度に基づいて前記車両の進行方向を算出する進行方向演算部とを備え、
   前記第二角度演算部は、前記回転角速度γと前記更新間隔Ｔとを用いて、θ _ｃ ＝（γ×Ｔ）／２の演算によって前記第二の角度θ _ｃ を算出することを特徴とする進行方向推定装置。
2. 前記第二角度演算部は、外部に備えられる加速度センサで検出された前記車両の加速度に基づいて、前記第二の角度の大きさを調整することを特徴とする請求項１に記載の進行方向推定装置。
3. 位置検出部と角速度検出部とを備える車両に搭載されて前記車両の進行方向を推定する進行方向推定装置であって、
   前記位置検出部で検出され所定の時間間隔で更新される前記車両の位置の変化に基づいて第一の角度を算出する第一角度演算部と、
   前記角速度検出部で検出された前記車両の回転角速度と前記位置検出部で検出される前記車両の位置の更新間隔とに基づいて第二の角度を算出する第二角度演算部と、
   前記第一の角度および前記第二の角度に基づいて前記車両の進行方向を算出する進行方向演算部とを備え、
   前記第二角度演算部は、外部に備えられる加速度センサで検出された前記車両の加速度に基づいて、前記第二の角度の大きさを調整することを特徴とする進行方向推定装置。
4. 前記第二角度演算部は、前記車両が加速している場合には、前記車両が減速している場合と比較して、前記第二の角度を小さくすることを特徴とする請求項２または請求項３に載の進行方向推定装置。
5. 前記第一角度演算部は、前記車両の最新の位置と、前記最新の位置に更新される直前の前記車両の位置との間の変化に基づいて前記第一の角度を算出することを特徴とする請求項１から請求項４のいずれか１項に記載の進行方向推定装置。
6. 前記位置検出部が前記車両の位置を最新の位置に更新した時刻からの経過時間における前記回転角速度を積分して第三の角度を算出する第三角度演算部を備え、
   前記進行方向演算部は、前記第一の角度、前記第二の角度および前記第三の角度に基づいて前記車両の進行方向を算出することを特徴とする請求項１から請求項５のいずれか１項に記載の進行方向推定装置。

Images