JP6620625B2 - 位置推定装置 - Google Patents

Description

本発明は、車両で用いられる位置推定装置に関する。

特許文献１には、ジャイロセンサで検出した角速度をもとに車両の方位を算出する移動体搭載システムが開示されている。この移動体搭載システムは、車両停止時のジャイロセンサ出力を用いて、ジャイロセンサが出力する角速度の、実際の角速度に対する角速度誤差であるオフセット値を算出する。特許文献１においては、オフセット値を算出した以降、検出した角速度からこのオフセット値を差し引くことで角速度を補正する。

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

特許文献１に開示の手法によれば、停車後のジャイロセンサの出力を補正するので、再発進以降におけるジャイロセンサの出力を用いた車両の位置推定の精度を向上させることができる。しかしながら、特許文献１では、停車以前の走行中のジャイロセンサの角速度誤差を考慮していないので、この角速度誤差分だけ位置推定の精度が低下する。つまり、ジャイロセンサの角速度誤差を推定した時点以前の角速度誤差によって位置推定の精度が低下するという問題点が残っていた。

本発明は、この事情に基づいて成されたものであり、その目的とするところは、ジャイロセンサの角速度誤差を推定した時点以前の角速度誤差による、位置推定の精度の低下を抑制する位置推定装置を提供することにある。

上記目的は独立請求項に記載の特徴の組み合わせにより達成され、また、下位請求項は、発明の更なる有利な具体例を規定する。特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、本発明の技術的範囲を限定するものではない。

上記目的を達成するための本発明は、車両で用いられ、ジャイロセンサ（９）を用いて逐次測定される角速度を用いて、車両の進行方向を逐次推定する方向推定部（５）と、方向推定部で逐次推定する進行方向である推定方向を用いて車両の位置を逐次推定する位置推定部（７）とを備える位置推定装置であって、所定のタイミングごとに、ジャイロセンサを用いて測定される角速度の実際の角速度に対する角速度誤差を推定する誤差推定部（４）を備え、方向推定部は、誤差推定部で推定した角速度誤差の前回推定した角速度誤差からの変化量をもとに、所定のタイミング以前の推定方向を推定し直し、位置推定部は、所定のタイミングに誤差推定部で角速度誤差を推定した場合に、その所定のタイミング直前に推定していた車両の位置を、前回の角速度誤差の推定時における車両の位置を中心に、前回推定した角速度誤差からの変化量に応じて定まる角度だけ回転させることで推定し直した上で、方向推定部で推定し直した推定方向を用いて、所定のタイミング以降の車両の位置を推定するものであり、位置推定部における車両の位置の推定し直しに用いられる角度は、前回推定した角速度誤差と今回推定した角速度誤差との間の誤差差分を、前回の角速度誤差の推定時から所定のタイミングまで累積した角度である累積方向誤差に従う大きさの角度である。

以上の構成によれば、方向推定部が、所定のタイミングにおいて誤差推定部で推定した角速度誤差の、前回推定した角速度誤差からの変化量をもとに、所定のタイミング以前の推定方向を推定し直す。これにより、推定し直された推定方向において、角速度誤差を推定した時点以前の角速度誤差による推定精度の低下が抑制される。位置推定部は、この推定し直された推定方向を用いて、所定のタイミング以降の車両の位置を推定する。従って、所定のタイミング以降に位置推定部が推定する位置についても、角速度誤差を推定した時点以前の角速度誤差による推定精度の低下を抑制することが可能となる。

本発明の第１実施形態に係る位置推定システム１の構成を表す図である。 第１実施形態に係る推定位置の算出方法を示す図である。 第１実施形態に係る誤差推定部４の作動を示すフローチャートである。 第１実施形態に係る方向推定部５の作動を示すフローチャートである。 第１実施形態に係る位置推定部７の作動を示すフローチャートである。 第１実施形態に係る位置推定部７が推定する推定位置の軌跡を示す図である。 第２実施形態に係る方向推定部５ａおよび位置推定部７ａの作動を示すフローチャートである。 第３実施形態に係る推定位置の誤差の算出方法を示す図である。 第３実施形態に係る位置推定部７ｂの作動を示すフローチャートである。 第３実施形態に係る位置推定部７ｂが推定位置を補正することによる推定位置の軌跡の変化を示す図である。

＜第１実施形態＞
以下、本発明の第１の実施形態としての位置推定装置２を備える位置推定システム１を図面に基づいて説明する。位置推定システム１の構成を図１に沿って説明する。位置推定システム１は、位置推定装置２と、ジャイロセンサ９と、車輪速センサ１０と、ＧＮＳＳ受信機１１とを備えて構成される。

ジャイロセンサ９は、位置推定システム１を用いる車両の方向の変化を逐次計測するセンサであり、単位時間あたりの方向変化を角速度として逐次出力する。なお、本実施形態における角速度は、車両上方から見た左回転方向を正の方向とする角度で表すとする。

車輪速センサ１０は、位置推定システム１を用いる車両の車輪の回転速度に応じた周期でパルス信号を逐次出力する。例えば、車輪が１回転するごとに１回パルス信号を出力する。ＧＮＳＳ受信機１１は、ＧＮＳＳ（Global Navigation Satellite System）を構成する測位衛星から送信された電波を受信し、当該ＧＮＳＳ受信機１１の位置を測位する電波航法を行うために用いられる受信機である。

位置推定装置２は、演算部３と、記憶部８とを有する。演算部３は、ＣＰＵ、ＲＯＭ、ＲＡＭ等よりなるマイクロコンピュータを主体として構成されており、ＣＰＵが、ＲＡＭの一時記憶機能を利用しつつ、ＲＯＭなどの非遷移的実体的記録媒体(non-transitory tangible storage medium)に記憶されているプログラムを実行することで、誤差推定部４、方向推定部５、距離推定部６、位置推定部７、測位部１２、補正部１３としての処理を行う。

位置推定装置２は、誤差推定部４、方向推定部５、距離推定部６、位置推定部７を用いて、ジャイロセンサ９が出力する角速度の誤差と、車両の進行方向と、車両の移動距離と、位置推定装置２が作動を開始した時点の位置に対する相対位置である推定位置とを逐次推定する推測航法を行う。測位部１２は、ＧＮＳＳ受信機１１を用いて位置を測位する電波航法を行い、補正部１３は、推測航法によって得られた位置を、電波航法によって得られた位置を用いて補正する。

また、ＣＰＵがそのプログラムを実行すると、プログラムに対応する方法が実行されることになる。なお、演算部３が実行する機能の一部または全部を、一つあるいは複数のＩＣ等によりハードウェア的に構成してもよい。

記憶部８は電気的に書き換え可能な不揮発性メモリであり、演算部３が推定する移動距離および方向等を記憶する。

これ以降、特に言及がない限り、推測航法による位置推定について説明する。なお、本実施形態においては、位置推定装置２が作動している場合、常に推測航法による位置推定を行っているとする。

位置推定装置２が推定位置を推定する際に用いる値を図２に沿って説明する。本実施形態における推定位置Ｐは、位置推定装置２が作動を開始した時点の車両の位置を原点としたｘｙ座標で表現する。また、位置推定装置２が推定する車両の進行方向である推定方向θは、ｘ軸の正方向と一致する場合を０とし、左回転方向を正の方向とする角度で表現する。ｎ回目の位置推定における推定位置Ｐｎの座標（Ｘｎ，Ｙｎ）は、ｎ−１回目の位置推定における推定位置Ｐ（ｎ−１）の座標（Ｘ（ｎ−１），Ｙ（ｎ−１））に、座標の変化量（ΔＸｎ，ΔＹｎ）を加算することによって求められる。座標の変化量（ΔＸｎ，ΔＹｎ）は、移動距離Ｄｎと、車両の推定方向θｎとを用いてΔＸｎ＝Ｄｎ×ｃｏｓ（θｎ）、ΔＹｎ＝Ｄｎ×ｓｉｎ（θｎ）で表される。車両の推定方向θｎは、前回の位置推定における推定方向θ（ｎ−１）に、角度の変化量Δθｎを加算することによって求められる。なお、角度の変化量Δθｎは、角速度ωｎと、位置推定の時間間隔Δｔとを用いてΔθｎ＝ωｎ×Δｔで表される。

演算部３の作動について説明する。演算部３は、誤差推定部４としての処理、方向推定部５としての処理、距離推定部６としての処理、位置推定部７としての処理を順次行う。この一連の処理を所定の周期で行うことにより、推定位置Ｐを逐次推定する。なお、所定の周期は例えば１秒周期であり、この値が時間間隔Δｔとなる。

誤差推定部４の作動を説明する。誤差推定部４は、ジャイロセンサ９が出力する角速度ωの、実際の値に対する誤差（以下、角速度誤差Ω）を所定のタイミングで推定する。なお、本実施形態における所定のタイミングは、車両が停止後に再発進したことを判断した場合である。

演算部３の誤差推定部４としての作動を、図３のフローチャートに沿って詳しく説明する。演算部３は、ｎ回目の位置推定を開始すると、図３に示す処理をステップＳ１（以下、ステップを省略）から順に所定の周期で実行する。これにより、角速度誤差Ωを逐次推定し、推定した角速度誤差Ωを記憶部８に記憶する誤差推定部４として作動する。なお、位置推定を開始した時点において、記憶部８に記憶された角速度誤差Ω＝０とする。

Ｓ１では、車両が停止しているか否かを判断する。停止していると判断した場合はＳ２に進み、停止していないと判断した場合はＳ６に進む。車両が停止しているか否かの判断は、例えば車輪速センサ１０から出力されるパルス信号の有無をもとに行えばよい。Ｓ２では、ジャイロセンサ９で測定される角速度ωを取得し、記憶部８に記憶する。Ｓ３では、車両が停止しているか否かを判断する。すなわち、車両が再発進しているか否かを判断する。停止していないと判断した場合には、再発進したものとしＳ４に進む。停止していると判断した場合には、車両が停止したままであるものとし、所定の期間待機した後、Ｓ２の処理を行う。所定の期間は、例えば１秒とすればよい。

Ｓ４では、Ｓ２の処理において記憶部８に記憶した角速度ωの平均値を算出する。これにより、車両が停止している期間においてジャイロセンサ９で測定される角速度ωの平均値が算出される。車両が停止している期間において、車両の角速度は本来は０であることから、車両が停止している期間に測定される角速度はジャイロセンサ９の出力誤差を意味する。よって、Ｓ２の処理で記憶された角速度ωはジャイロセンサ９の出力誤差である。

そこで、本実施形態では、このＳ４で算出された角速度ωの平均値を角速度誤差Ωの値であるとし、Ｓ５において、角速度誤差Ωの値をＳ４で算出した角速度ωの平均値に更新する。さらに、このＳ５では、更新前の角速度誤差Ω（以下、角速度誤差ΩＯ）とこのＳ５での更新後の角速度誤差Ω（以下、角速度誤差ΩＬ）の間の差分である誤差差分ΔΩ＝ΩＬ−ΩＯ（deg/s）を求める。誤差差分ΔΩは、正しい角速度誤差と更新前の角速度誤差ΩＯとの差を意味する。本実施形態では、正しい角速度誤差は更新後の角速度誤差ΩＬで一定であるとし、更新後の角速度誤差ΩＬと更新前の角速度誤差ΩＯとの差を誤差差分ΔΩとする。

また、Ｓ５では、前回角速度誤差Ωを更新した時点から前回推定方向を更新した時点（すなわちｎ−１時点）までの経過時間である経過時間ΔＴを算出する。例えば、前回の更新がｎ−ａ回目の位置推定であった場合、経過時間ΔＴはΔＴ＝Δｔ×（（ｎ−１）−（ｎ−ａ））で求められる。その後、誤差推定部４としての処理を終了し、方向推定部５としての処理を開始する。以上の処理により、誤差推定部４は、車両の停止時にジャイロセンサ９で測定される角速度ωを用いて、角速度誤差Ωを算出する。

次に、方向推定部５の作動を説明する。方向推定部５は、角速度誤差Ωを用いて補正した角速度ωを用いて、車両の進行方向を推定方向θとして推定する。また、方向推定部５は、角速度誤差Ωの値が更新されると、更新される以前の推定方向θの値を、角速度誤差Ωを用いて補正する。本実施形態では、角速度誤差Ωの値が更新される直前に推定された推定方向θを補正する。

演算部３の方向推定部５としての作動を図４のフローチャートに沿って詳しく説明する。演算部３は、誤差推定部４としての作動を終了し、方向推定部５としての作動を開始すると、図４に示す処理をＳ１１から順に実行し、ｎ回目の位置推定における推定方向θｎを推定する。

Ｓ１１では、前回の位置推定以降にＳ５の処理が行われることによって角速度誤差Ωが更新されているか否かを判断する。更新されていると判断した場合はＳ１２に進み、更新されていないと判断した場合はＳ１５に進む。

Ｓ５で角速度誤差Ωが更新されるのは、車両停止後、車両が停止していないと判断した場合である。よって、Ｓ１２に進むのは車両再発進時である。Ｓ１２では、Ｓ５で算出した、誤差差分ΔΩと、経過時間ΔＴとを取得する。Ｓ１３では、前回角速度誤差Ωが更新された時点以降の推定方向θの算出において、ΩＯを用いて推定した場合の推定方向θ（ｎ−１）と、ΩＬを用いて推定した場合の推定方向Θ（ｎ−１）との間の累積方向誤差ΔΘ（ｎ−１）を算出する。具体的には、ΔΘ（ｎ−１）＝ΔΩ×ΔＴとして累積方向誤差ΔΘ（ｎ−１）を算出する。前回、すなわちｎ−１回目の推定方向の算出には、前回の図３の処理において更新した角速度誤差Ωを用いて推定方向を算出している。前回の図３の処理において更新した角速度誤差Ωは、今回の処理における更新前の角速度誤差ΩＯに相当する。

誤差差分ΔΩは、前述したように、正しい角速度誤差と更新前の角速度誤差ΩＯとの差である。また、ΔＴは、前回角速度誤差Ωを更新した時点から前回推定方向を更新した時点（すなわちｎ−１時点）までの経過時間である。

ここで、前回角速度誤差Ωを更新した時点から前回推定方向を更新した時点まで、正しい角速度誤差がΩＬで一定であると仮定していることから、単位時間あたりの誤差差分ΔΩが一定である。従って、ΔΩにΔＴを乗じた値であるΔΘ（ｎ−１）は、前回角速度誤差Ωを更新した時点から前回推定方向を更新した時点までに累積している方向の累積誤差を意味する。そこで、以下では、ΔΘ（ｎ−１）を累積方向誤差とする。

Ｓ１４では、Ｓ１３で算出した累積方向誤差ΔΘ（ｎ−１）を用いて推定方向θ（ｎ−１）を補正し、推定方向Θ（ｎ−１）を算出する。具体的にはΘ（ｎ−１）＝θ（ｎ−１）−ΔΘ（ｎ−１）として求める。

Ｓ１５では、ジャイロセンサ９が出力する角速度ωｎを取得する。Ｓ１６では、Ｓ１５で取得した角速度ωｎから、Ｓ５の処理より逐次更新されている角速度誤差Ωを差し引くことによって角速度を補正する。Ｓ１７では、Ｓ１６で補正された角速度ωｎ−Ωに、位置推定の時間間隔Δｔを乗算して方向の変化量Δθ＝Δｔ×（ωｎ−Ω）を算出し、算出した方向の変化量Δθを直前の推定方向θ（ｎ−１）に加算することによって推定方向θｎを算出する。その後、方向推定部５としての処理を終了し、距離推定部６としての処理を開始する。

以上の処理により、方向推定部５は、ジャイロセンサ９が出力する角速度ωｎと、誤差推定部４が推定する角速度誤差Ωとを用いて推定方向θｎを推定する。また、角速度誤差Ωが更新されると、直前の推定位置Ｐ（ｎ−１）を、角速度誤差Ωの値と、角速度誤差Ωの値が前回更新されてからの経過時間ΔＴとを用いて、再び推定することによって補正する。本実施形態では、車両が停止後に再発進したことを判断した場合に角速度誤差Ωを推定するので、推定方向の補正も、この再発進を判断した場合に行う構成とすればよい。

距離推定部６の作動を説明する。距離推定部６は、直前の推定位置Ｐ（ｎ−１）からの移動距離Ｄｎを算出する。演算部３は、方向推定部５としての処理を終了し、距離推定部６としての処理を開始すると、車輪速センサ１０が出力するパルス信号を用いて移動距離を算出する。具体的には、直前の位置推定以降に出力されたパルス信号のパルス数に、車輪の周長を乗算することによって求めればよい。その後、距離推定部６としての処理を終了し、位置推定部７としての処理を開始する。

位置推定部７の作動を、図５のフローチャートに沿って説明する。位置推定部７は、推定方向θと移動距離Ｄとを用いて推定位置Ｐを推定する。演算部３は、距離推定部６としての処理を終了し、位置推定部７としての処理を開始すると、図５に示す処理をＳ２１から順に実行する。この処理により、演算部３は推定方向θと移動距離Ｄとを用いて推定位置Ｐを推定する位置推定部７として作動する。

Ｓ２１では、距離推定部６が算出した移動距離Ｄｎと、方向推定部５によって推定された推定方向θｎとを取得する。Ｓ２２では、Ｓ２１で取得した移動距離Ｄｎと推定方向θｎとを用いて車両のｘｙ座標上の移動量を算出する。具体的には、ｘ軸方向の変化量ΔＸｎ＝Ｄｎ×ｃｏｓ（θｎ）およびｙ軸方向の変化量ΔＹｎ＝Ｄｎ×ｓｉｎ（θｎ）を求める。Ｓ２３では、Ｓ２１で算出したｘ軸方向およびｙ軸方向の移動量を、前回推定した推定位置Ｐ（ｎ−１）の座標（Ｘ（ｎ−１），Ｙ（ｎ−１））に加算することによって推定位置Ｐｎの座標（Ｘｎ，Ｙｎ）＝（Ｘ（ｎ−１）＋ΔＸｎ，Ｙ（ｎ−１）＋ΔＹｎ）を求める。距離推定部６は、方向推定部５で推定方向が補正されていた場合には、この補正が行われた推定方向を用いて推定位置Ｐを推定することで、再発進後の車両の位置を推定することになる。

方向推定部５が推定方向θを推定方向Θに補正することによる、推定位置Ｐの補正について図６に沿って説明する。グラフＧ１は、位置推定装置２が作動を開始した時点を原点とした車両の相対位置の、実際の変化を表すグラフである。グラフＧ２は、本実施形態の位置推定システム１が推定する推定位置Ｐの変化を表すグラフである。位置Ｐａにおいて角速度誤差Ωが更新されたことにより、推定方向θａが推定方向Θａに補正され、以降の角速度ωｎが補正されている。グラフＧ３は、位置Ｐａにおいて角速度誤差Ωが更新された際に推定方向θａを補正せず、以降の角速度ωの補正のみ行った場合の推定位置Ｐの変化を表すグラフである。

位置Ｐａ以前のグラフＧ２およびグラフＧ３は、角速度誤差Ωが０であり、角速度ωが補正されていない。このため、実際の位置を示すグラフＧ１が直線状になっているのに対し、グラフＧ２およびグラフＧ３は角速度誤差Ωの影響により曲線状となっている。

位置Ｐａ以降のグラフＧ２およびグラフＧ３は、角速度誤差Ωが更新され、角速度ωが補正される。従って、グラフＧ１と同様の直線状となる。しかし、グラフＧ３は位置Ｐａにおいて推定方向θａを補正していないため、位置Ｐａ以降もグラフＧ１との差が広がり続ける。一方、グラフＧ２は位置Ｐａにおいて推定方向θａを補正しているため、位置Ｐａ以降においてグラフＧ１との差が広がることを抑制することが可能となっている。すなわち、推定方向θを補正することにより、推定位置Ｐの推定精度が向上する。

以上、説明した第１実施形態によれば、位置推定システム１は、位置推定装置２が作動を開始した時点の車両の位置を原点とした相対位置を推定位置Ｐとして推定する。また、誤差推定部４が、ジャイロセンサ９が出力する角速度ωの誤差である角速度誤差Ωの値を更新すると、方向推定部５が、角速度誤差Ωの値が更新される以前におけるジャイロセンサ９の出力誤差により精度が低下していた推定方向θ（ｎ−１）を補正する。これにより、推定方向θ（ｎ−１）を補正しない場合と比較して、角速度誤差Ωの値が更新される以前におけるジャイロセンサ９の出力誤差による推定位置Ｐの推定精度の低下が抑制される。

また、誤差推定部４は、車両が停止した場合に、車両の停止中にジャイロセンサ９で測定された角速度ωを用いて、角速度誤差Ωを推定している。従って、推定方向θの推定に用いられるジャイロセンサ９を用いて、角速度誤差Ωも推定することが可能となる。

＜第２実施形態＞
次に、第２実施形態を説明する。この第２実施形態以下の説明において、それまでに使用した符号と同一番号の符号を有する要素は、特に言及する場合を除き、それ以前の実施形態における同一符号の要素と同一である。また、構成の一部のみを説明している場合、構成の他の部分については先に説明した実施形態を適用できる。

第２実施形態における位置推定システム１ａは、角速度誤差Ωが更新されると、直前の推定方向θ（ｎ−１）を補正することに加えて、角速度誤差Ωを前回更新した時点の推定位置Ｐ（ｎ−ａ）から直前の推定位置Ｐ（ｎ−１）までの推定位置Ｐを再計算する。位置推定システム１ａが備える位置推定部７ａの作動について図７に沿って説明する。第２実施形態における演算部３ａは、距離推定部６としての作動を終了すると、図７に示す処理をＳ３１から順に実行し、方向推定部５ａおよび位置推定部７ａとして作動する。なお、位置推定装置２は、位置推定部７ａが位置の推定に用いた推定方向θおよび移動距離Ｄを逐次記憶部８に記憶しており、図７の処理を開始する以前に、推定方向θおよび移動距離Ｄが互いに対応付けられた状態で記憶部８に記憶されているとする。

Ｓ３１は位置推定部７ａとしての作動であり、直前の位置推定以降に、Ｓ５の処理によって角速度誤差Ωが更新されているか否かを判断する。更新されていると判断した場合はＳ３２に進み、更新されていないと判断した場合はＳ２１に進む。Ｓ３２は位置推定部７ａとしての作動であり、記憶部８に記憶された、移動距離Ｄおよび推定方向θと、Ｓ５で算出した、誤差差分ΔΩと経過時間ΔＴとを取得する。

Ｓ３３は方向推定部５ａとしての作動であり、Ｓ３２で取得した推定方向θのうち、前回Ωを更新したｎ−ａ回目の位置推定以降における値を、誤差差分ΔΩを用いて補正し、補正した推定方向Θを求める。具体的には、誤差差分ΔΩによる推定方向θの変化が累積したことによる累積誤差ｍ×ΔΩを算出し、推定方向θから差し引くことによって補正する。例えば、前回Ωを更新した位置推定がｎ−１０回目であるならば、Θ（ｎ−１０）＝θ（ｎ−１０）−１×ΔΩ、Θ（ｎ−９）＝θ（ｎ−９）−２×ΔΩ、．．．、Θ（ｎ−１）＝θ（ｎ−１）−１０×ΔΩとして求める。

Ｓ３４は位置推定部７ａとしての作動であり、Ｓ３２で取得した速度Ｖの履歴、および、Ｓ３３で補正した推定方向Θを用いて前回の位置推定における推定位置Ｐ（ｎ−１）を再計算する。すなわち、各位置推定におけるｘ軸方向の変化量ΔＸｎ＝Ｄｎ×ｃｏｓ（θｎ）およびｙ軸方向の変化量ΔＹｎ＝Ｄｎ×ｓｉｎ（θｎ）をｎ＝１、２、．．．、ｎ−１について求め、合計することによって推定位置Ｐ（ｎ−１）の座標（Ｘ（ｎ−１），Ｙ（ｎ−１））を再計算することにより、推定位置Ｐ（ｎ−１）の位置を補正する。Ｓ３４の処理に続く、Ｓ２１、Ｓ２２、およびＳ２３の処理は位置推定部７ａとしての作動であり、図５におけるＳ２１、Ｓ２２、およびＳ２３の処理とそれぞれ同一とする。

以上、説明した第２実施形態によれば、位置推定システム１ａは、第１実施形態における位置推定システム１と同様に、位置推定装置２が作動を開始した時点の車両の位置を原点とした相対位置を推定位置として推定する。また、方向推定部５が、推定方向θを補正することにより、角速度誤差Ωを更新する以前の車両位置である原点を基準とした推定位置Ｐの推定精度が向上する。

さらに、位置推定部７ａが、角速度誤差Ωを前回更新した時点の推定位置Ｐ（ｎ−ａ）から直前の推定位置Ｐ（ｎ−１）までの推定位置Ｐを、角速度誤差ΩＬを用いて再計算する。これにより、角速度誤差Ωが角速度誤差ΩＬに更新される以前の推定位置Ｐの推定精度が向上する。これに加えて、角速度誤差Ωを更新した以降の推定位置Ｐは、更新する以前の推定位置Ｐを用いて推定している。従って、推定位置Ｐを再計算しない場合と比較して、角速度誤差を更新した以降の推定位置Ｐについても推定精度が向上する。

＜第３実施形態＞
次に、第３実施形態を説明する。第３実施形態における位置推定システム１ｂは、角速度誤差Ωが更新されると、直前の推定位置Ｐ（ｎ−１）を、前回角速度誤差を更新した時点の推定位置Ｐ（ｎ−ａ）を中心に回転させることによって補正する。

推定位置Ｐを回転させる角度の決定方法について図８に沿って説明する。角速度誤差ΩＬを用いて推定した推定位置がｘ軸に沿っているとする。角速度誤差ΩＯを用いて推定した推定方向をグラフＧ４とする。誤差差分ΔΩにより、位置推定ごとに、Ｙ軸方向の移動量誤差εの形で、角速度誤差ΩＬを用いて算出した移動量と、角速度誤差ΩＯを用いて算出した移動量との間に誤差が生じている。ただし、推定方向θに生じる方向誤差は微小であるとし、車両は等速で移動し、位置推定ごとにΔＤ移動しているとする。

角速度誤差の間の誤差差分ΔΩにより、推定方向θ（ｎ−ａ＋１）に生じる誤差がΔΩ×Δｔであることから、移動量誤差ε（ｎ−ａ＋１）は、ε（ｎ−ａ＋１）＝ΔＤ×ｓｉｎ（ΔΩ×Δｔ）である。ここで、ΔΩ×Δｔが微小であるため、ｓｉｎ（ΔΩ×Δｔ）はΔΩ×Δｔに近似することから、移動量誤差ε（ｎ−ａ＋１）＝ΔＤ×ΔΩ×Δｔと近似することが可能である。同様に、推定方向θ（ｎ−ａ＋２）に生じる誤差が２×ΔΩ×Δｔであることから、移動量誤差ε（ｎ−ａ＋２）＝２×ΔＤ×ΔΩ×Δｔと近似可能である。

以上の移動量誤差の累積により、位置Ｐ（ｎ−１）に生じる位置誤差ΔＰはΔＰ＝（ΔＤ×ΔΩ×Δｔ）＋（２×ΔＤ×ΔΩ×Δｔ）＋．．．＋（（ａ−１）×ΔＤ×ΔΩ×Δｔ）で表される。ここで、１＋２＋．．．＋（ａ−１）は（ａ−１）×（ａ−１）／２で近似可能であることから、ΔＰ＝（ａ−１）×ΔＤ×（（ａ−１）×Δｔ×ΔΩ）／２と近似可能である。ΔΘ＝ΔΩ×ΔＴ＝（ａ−１）×ΔΩ×Δｔであることから、ΔＰ＝（ａ−１）×ΔＤ×（ΔΘ／２）である。ΔΘ／２が微小であるため、ΔＰ＝（ａ−１）×ΔＤ×ｓｉｎ（ΔΘ／２）と近似可能である。よって、角速度誤差ΩＯを用いて算出した移動量は、角速度誤差ΩＬを用いて算出した移動量を、推定位置Ｐ（ｎ−ａ）を中心にΔΘ／２回転させた値であるとみなすことが可能である。従って、角速度誤差ΩＯを用いて推定した推定位置Ｐ（ｎ−１）を、推定位置Ｐ（ｎ−ａ）を中心に−ΔΘ／２回転させることにより、角速度誤差ΩＬを用いて推定した推定位置Ｐ（ｎ−１）に補正することが可能である。

第３実施形態における位置推定システム１ｂが備える位置推定部７ｂの作動について図９に沿って説明する。第３実施形態における演算部３ｂは、方向推定部５としての作動を終了すると、図５に示す処理に代えて図９に示す処理をＳ４１から順に実行し、位置推定部７ｂとして作動する。なお、位置推定装置２は、角速度誤差Ωが更新された時点の位置を逐次記憶部８に記憶しており、前回角速度誤差Ωが更新された時点の位置Ｐ（ｎ−ａ）が予め記憶部８に記憶されているとする。

Ｓ４１では、角速度誤差ΩがＳ５の処理によって更新されているか否かを判断する。更新されていると判断した場合はＳ４２に進み、更新されていないと判断した場合はＳ２１に進む。Ｓ４２では、角速度誤差ΩＬと角速度誤差ΩＯとの間の誤差差分ΔΩと、経過時間ΔＴと、前回角速度誤差Ωが更新された時点の位置Ｐ（ｎ−ａ）とを取得する。Ｓ４３では、誤差差分ΔΩの累積による推定方向の誤差、すなわち累積方向誤差ΔΘ＝ΔΩ×ΔＴを算出する。

Ｓ４４では、直前の推定位置Ｐ（ｎ−１）を、前回角速度誤差を更新した時点の推定位置Ｐ（ｎ−ａ）を中心に−ΔΘ／２回転させることで補正する。回転させた推定位置Ｐ（ｎ−１）の座標は、回転行列を用いて、（（Ｘ（ｎ−１）×Ｘ（ｎ−ａ））×ｃｏｓ（−ΔΘ／２）−（Ｙ（ｎ−１）×Ｙ（ｎ−ａ））×ｓｉｎ（−ΔΘ／２），（Ｘ（ｎ−１）×Ｘ（ｎ−ａ））×ｓｉｎ（−ΔΘ／２）＋（Ｙ（ｎ−１）×Ｙ（ｎ−ａ））×ｃｏｓ（−ΔΘ／２））で表される。Ｓ４４の処理に続く、Ｓ２１、Ｓ２２、およびＳ２３の処理は、図５におけるＳ２１、Ｓ２２、およびＳ２３の処理とそれぞれ同一である。

以上の処理により、位置推定部７ｂは、角速度誤差Ωが更新されると、角速度誤差Ωの値と、角速度誤差Ωの値が前回更新されてからの経過時間ΔＴとを用いて、直前の推定位置Ｐ（ｎ−１）を、角速度誤差Ωの値が前回更新された時点の位置Ｐ（ｎ−ａ）を中心に回転させることにより補正する。

位置推定部７ｂが直前の推定位置Ｐ（ｎ−１）を、角速度誤差Ωの値が前回更新された時点の位置Ｐ（ｎ−ａ）を中心に回転させることによる、推定位置Ｐの変化について図１０に沿って説明する。

グラフＧ５は、車両の実際の位置を、位置推定装置２が作動を開始した時点を原点として表したグラフである。グラフＧ６は、角速度ωを、実際の角速度に角速度誤差Ωを加算した値とし、角速度ωの補正を行わず位置推定に用いた場合の推定位置を表したグラフである。グラフＧ７は、グラフＧ６で表される推定位置を、Ｓ４４の処理により原点を中心に回転させることで位置補正を行った位置を表すグラフである。グラフＧ７が示す軌跡が、グラフＧ６が示す軌跡と比較してグラフＧ５に近いことから、Ｓ４４の処理によって角速度誤差による推定位置の誤差を軽減可能であることがわかる。

以上、説明した第３実施形態によれば、位置推定システム１ｂは、第１実施形態における位置推定システム１と同様に、位置推定を開始した時点の車両の位置を原点とした相対位置を推定位置として推定する。また、方向推定部５が、推定方向θを補正することにより、角速度誤差Ωを更新する以前の車両位置である原点を基準とした推定位置Ｐの推定精度が向上する。

さらに、位置推定部７ｂが、Ｓ４４の処理によって、直前の推定位置Ｐ（ｎ−１）を、角速度誤差を前回更新した時点の推定位置Ｐ（ｎ−ａ）を中心に、−ΔΘ／２回転させることによって推定位置を補正する。従って、推定位置Ｐ（ｎ−１）を補正しない場合と比較して、推定位置Ｐの推定精度が向上する。また、推定位置の回転は、直前の推定位置Ｐ（ｎ−１）および角速度誤差を前回更新した時点の推定位置Ｐ（ｎ−ａ）、および累積方向誤差ΔΘを用いて、Ｓ４４の処理の説明で示した式に従って行う。従って、移動距離Ｄおよび推定方向θの履歴を用いて角速度誤差Ωを前回更新した時点の推定位置Ｐ（ｎ−ａ）から直前の推定位置Ｐ（ｎ−１）までの推定位置Ｐを再計算する場合と比較して、記憶部８に記憶すべきデータの量および演算の負荷を軽減可能である。

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

＜変形例１＞
第１実施形態において、演算部３は、位置推定装置２の外部に備える車輪速センサ１０およびジャイロセンサ９から車両の速度に応じた値および車両の方向の変化を取得するとしている。しかし、位置推定装置２が備えるセンサから車両の速度に応じた値と車両の方向の変化との少なくとも一つを取得するとしてもよい。例えば、位置推定装置２がジャイロセンサ９を備え、ジャイロセンサ９から取得した車両の方向の変化を用いて推定方向θおよび推定位置Ｐを求めるとしてもよい。

＜変形例２＞
第１実施形態において、誤差推定部４が角速度誤差Ωを推定する所定のタイミングを、車両が停止後に再発進した時点とし、車両が停止している期間におけるジャイロセンサ９の出力を用いて角速度誤差Ωを算出するとしている。しかし、角速度誤差Ωを推定する所定のタイミングは、車両が停止後に再発進した時点に限られない。例えば、車両が停止した時点であってもよいし、車両の走行中の時点であってもよい。

また、角速度誤差Ωを推定する手法は、車両が停止している期間におけるジャイロセンサ９の出力を用いるものに限られない。前述した実施形態では、補正部１３は位置を補正していたが、補正部１３が、車両の推定位置Ｐの補正に加えて、走行中に車両の推定方向θを補正してもよい。この場合、補正部１３が推定方向θを補正するごとに、角速度誤差Ωを推定するようにしてもよい。

補正部１３が車両の推定方向を走行中に補正する場合、ジャイロセンサ９から得られた信号により定まる推定方向を補正するのであるから、ジャイロセンサ９からの信号とは異なる情報を用いて、車両の進行方向を逐次決定する必要がある。補正部１３が車両の進行方向を決定するために用いる情報を、走行中方向情報とする。走行中走行情報には、たとえば、ＧＮＳＳ受信機１１を用いて測位部１２が測位した、現在位置および現在時点から一定期間内に測位した１点以上の過去の位置とを含む複数の位置がある。

これら複数の位置に基づいて形成される走行軌跡が表す走行軌跡線の終端点（すなわち現在位置）の接線方向が、車両の進行方向である。なお、複数の位置に基づいて形成する走行軌跡は、複数の位置を非線形最小二乗法により近似した曲線でもよいし、複数の位置間を直線により繋いだ形状でもよい。補正部１３は、推定方向θを、この走行軌跡から求められる車両の進行方向とする。そして、推定方向θを、この進行方向に補正する。

この場合、走行軌跡から求められる車両の進行方向と補正前の推定方向θとの角度差が累積方向誤差ΔΘになる。また、この変形例２では累積方向誤差ΔΘが請求項の補正量に相当する。

累積方向誤差ΔΘが算出できると、ΔΘ＝ΔΩ×ΔＴの関係から誤差差分ΔΩが算出できる。この誤差差分ΔΩを更新前の角速度誤差ΩＯに加える補正することによって、新たな角速度誤差Ωを算出するとしてもよい。

＜変形例３＞
また、第１実施形態において、補正部１３が、測位部１２が電波航法により測位した位置を用いて推定位置Ｐを補正するとしていたが、補正部１３が行う推定位置Ｐの補正はこれに限られない。例えば、推定位置Ｐの軌跡を地図上の道路形状に照らし合わせるマップマッチングを行うことによって、推定方向θの補正を行うとしてもよい。マップマッチング前の推定位置Ｐの軌跡により定まる車両の進行方向と、マップマッチング後の推定位置Ｐの軌跡により定まる車両の進行方向との角度差が累積方向誤差ΔΘになる。累積方向誤差ΔΘを算出した以降の処理は変形例２と同じである。なお、この変形例３では、推定位置Ｐの軌跡と地図上の道路形状が、請求項の走行中方向情報である。この場合、角速度誤差Ωを推定する所定のタイミングを、マップマッチングを行う時点としてもよい。

変形例２や変形例３に示した構成によって角速度誤差Ωを推定するとした場合、車両が走行中の場合においても、角速度誤差Ωを推定することが可能となるため、推定位置Ｐの推定精度を向上させることが可能となる。また、第１実施形態、変形例２、および変形例３にそれぞれ示した構成による角速度誤差Ωの推定方法を組み合わせて用いるとしてもよい。この場合、各推定方法を単独で用いる場合と比較して、角速度誤差Ωを推定する頻度が高まる。従って、推定位置Ｐの推定精度を向上させることが可能となる。

＜変形例４＞
第１実施形態において、位置推定装置２が作動している場合、常に推測航法による位置推定を行っているとしていたが、推測航法による位置推定を行う条件はこれに限られない。例えば、位置推定装置２が作動し、走行を開始した時点から推測航法による位置推定を開始するとしてもよい。また、位置推定装置２が作動し、かつ電波航法による位置推定を停止している場合に推測航法による位置推定を行うとしてもよい。

＜変形例５＞
前述の実施形態では、前回角速度誤差Ωを更新した時点から前回推定方向を更新した時点まで、正しい角速度誤差がΩＬで一定であるとしていた。これにより、正しい角速度誤差とΩＯとの差である誤差差分ΔΩがΩＬ−ΩＯで一定となり、ΔΘ（ｎ−１）＝ΔΩ×ΔＴの関係となることから累積方向誤差ΔΘ（ｎ−１）を算出していた。しかし、正しい角速度誤差を時間的に変化する関数としてもよい。たとえば、正しい角速度誤差が経過時間ΔＴかけてΩＯからΩＬまで一次関数的に変化するとした場合には、誤差差分ΔΩが経過時間ΔＴかけて０からΩＬ−ΩＯまで一次関数的に変化する。この場合、累積方向誤差ΔΘ（ｎ−１）は、経過時間ΔＴかけて０からΩＬ−ΩＯまで変化する誤差差分ΔΩの積分値となる。

１：位置推定システム ２：位置推定装置 ３：演算部 ４：誤差推定部 ５：方向推定部 ６：距離推定部 ７：位置推定部 ８：記憶部 ９：ジャイロセンサ １０：車輪速センサ １１：ＧＮＳＳ受信機 １２：測位部 １３：補正部

Claims (3)

1. 車両で用いられ、
   ジャイロセンサ（９）を用いて逐次測定される角速度を用いて、前記車両の進行方向を逐次推定する方向推定部（５）と、
   前記方向推定部で逐次推定する進行方向である推定方向を用いて前記車両の位置を逐次推定する位置推定部（７）とを備える位置推定装置であって、
   所定のタイミングごとに、前記ジャイロセンサを用いて測定される角速度の実際の角速度に対する角速度誤差を推定する誤差推定部（４）を備え、
   前記方向推定部は、前記誤差推定部で推定した前記角速度誤差の、前回推定した前記角速度誤差からの変化量をもとに、前記所定のタイミング以前に推定した前記推定方向を推定し直し、
   前記位置推定部は、前記所定のタイミングに前記誤差推定部で前記角速度誤差を推定した場合に、その所定のタイミング直前に推定していた前記車両の位置を、前回の前記角速度誤差の推定時における前記車両の位置を中心に、前回推定した前記角速度誤差からの変化量に応じて定まる角度だけ回転させることで推定し直した上で、前記方向推定部で推定し直した前記推定方向を用いて、前記所定のタイミング以降の前記車両の位置を推定するものであり、
   前記位置推定部における前記車両の位置の推定し直しに用いられる角度は、前回推定した前記角速度誤差と今回推定した角速度誤差との間の誤差差分を、前回の前記角速度誤差の推定時から前記所定のタイミングまで累積した角度である累積方向誤差に従う大きさの角度である位置推定装置。
2. 請求項１において、
   前記誤差推定部は、前記車両の停止時に前記ジャイロセンサで測定される角速度を用いて、前記角速度誤差を推定する位置推定装置。
3. 請求項１または２において、
   前記ジャイロセンサからの信号とは異なる情報であって、走行中に得られる前記車両の進行方向に関する情報である走行中方向情報を用いて、前記車両の走行中に前記車両の進行方向を逐次決定するとともに、当該進行方向を用いて前記推定方向を補正する補正部（１３）をさらに備え、
   前記誤差推定部は、前記車両の走行中に前記補正部で補正が行われるごとに、その補正における補正量をもとに、前記角速度誤差を推定する位置推定装置。

Images