JP2014102137A

JP2014102137A - 自己位置推定装置

Info

Publication number: JP2014102137A
Application number: JP2012253842A
Authority: JP
Inventors: Yoshiaki Yokoyama; 良晃横山; Hiroshi Kameda; 洋志亀田
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2012-11-20
Filing date: 2012-11-20
Publication date: 2014-06-05
Anticipated expiration: 2032-11-20
Also published as: JP6060642B2

Abstract

【課題】移動体の自己位置を推定するＧＰＳ／ＩＮＳ複合航法システムを用いた自己位置推定装置において、ＧＰＳが使用できない環境下で、画像情報を用いずに移動体の自己位置の推定を可能とする自己位置推定装置を提供する。
【解決手段】本発明は、通過ポイント座標設定部５により設定した移動体が通過する通過ポイント位置座標、速度、通過時刻に基づいて、参照情報計算部６により移動体の位置座標、速度を算出してＤＲ計算部３で算出した位置座標、速度を補正する構成としたため、ＧＰＳが利用できない場合も画像情報を用いずに自己位置を推定することができる。
【選択図】図１

Description

本発明は、例えば航空機や車両等、移動体の自己位置を推定する自己位置推定装置に関するものである。

航空機や車両等、移動体の自己位置を高精度に推定する装置としてＧＰＳ／ＩＮＳ複合航法システムが知られている。ＧＰＳ／ＩＮＳはＧＰＳ（ＧｌｏｂａｌＰｏｓｉｔｉｏｎｉｎｇＳｙｓｔｅｍ）とＩＮＳ（ＩｎｅｒｔｉａｌＮａｖｉｇａｔｉｏｎＳｙｓｔｅｍ）の２つの装置を組み合わせたものであり、ＩＮＳは慣性計測装置であるＩＭＵ（ＩｎｅｒｔｉａｌＭｅａｓｕｒｉｎｇＵｎｉｔ）から構成され、加速度センサや角速度センサ（ジャイロセンサ）によって、積分を繰り返すことで自己位置を推定できるが、その特性上、ドリフト誤差により時間経過と共に位置推定誤差が増大する。ＧＰＳ／ＩＮＳでは、ＩＮＳのドリフト誤差をＧＰＳの観測データを用いて補正することにより高精度な自己位置推定を行う。ＧＰＳの測位精度は時間が経過しても劣化しないが、ＧＰＳは都市部やトンネルなど測位条件が悪い場合は測位精度が劣化したり、ＧＰＳ信号が得られない場合がある。このようなＧＰＳの観測データが得られない場合に、ＧＰＳの代わりにＳＡＲ（ＳｙｎｔｈｅｔｉｃＡｐｅｒｔｕｒｅＲａｄａｒ）による電波画像を用いて、自己位置を推定する技術が開示されている（例えば、非特許文献１）。電波画像を用いた自己位置の推定は、電波画像から得られる移動体を基準として、目印となる建物等との相対情報（距離、距離変化率）と、事前に保有している地図情報（緯度、経度、高度）とを照合することで自己位置を推定する。

Ｍ．Ｇｒｅｃｏ，Ｇ．Ｐｉｎｅｌｌｉ，Ｋ．Ｋｕｌｐａ，Ｐ．Ｓａｍｃｚｙｎｓｋｉ，Ｂ．Ｑｕｅｒｒｙ，ａｎｄＳ．Ｑｕｅｒｒｙ， "ＴｈｅＳｔｕｄｙｏｎＳＡＲＩｍａｇｅｓＥｘｐｌｏｉｔａｔｉｏｎｆｏｒＡｉｒＰｌａｔｆｏｒｍＮａｖｉｇａｔｉｏｎＰｕｒｐｏｓｅｓ，" ＲａｄａｒＳｙｍｐｏｓｉｕｍ，ｐｐ．３４７−３５２，２０１１．

しかしながら、電波画像を用いた移動体の自己位置の推定は、起伏の激しい山岳地帯などにおいては、電波画像に歪が生じるため地図情報との対応が取れず衛星の軌道情報やＤＥＭ（ＤｉｇｉｔａｌＥｌｅｖａｔｉｏｎＭｏｄｅｌ）を用いて幾何補正を行う必要がある。また、電波画像から目印となる建物等の特徴点を抽出し地図データとの照合（マッチング処理）が必要であり、それらの処理にはさらに誤差が発生する。自己位置推定の誤差モデルには、これらの処理誤差を反映する必要があり、誤差モデルが正しく反映されないと自己位置推定の更新を繰り返すことで、誤差が蓄積してしまう。

本発明は、上記課題に鑑みてなされたもので、電波画像を用いることなく、ＧＰＳが利用できない場合も移動体の自己位置の推定を可能とするものである。

本発明の自己位置推定装置は、設定した通過ポイント位置座標を通過する移動体の運動情報を計測する慣性計測装置と、前記慣性計測装置から前記移動体の運動情報を受けて、デッドレコニングにより前記移動体の第一の推定位置及び第一の推定速度を算出するＤＲ計算部と、前記移動体が通過する通過ポイント位置座標と該通過ポイント位置座標を通過する通過時刻と、速度とを設定する通過ポイント座標設定部と、前記移動体の運動情報を単位時間毎に保持し、前記第一の推定位置と前記通過ポイント位置座標とが一致した時点における時刻と、前記通過ポイント座標設定部で設定された通過時刻との差分値に基づいて、前記運動情報を用いてデッドレコニングにより前記移動体の第二の推定位置と第二の推定速度とを算出する参照情報計算部と、前記第一の推定位置と第二の推定位置との差分値と、前記第一の推定速度と第二の推定速度との差分値とを観測値として受けて、前記移動体の現在位置の誤差推定値を算出するカルマンフィルタとを備えることを特徴とする。

本発明の自己位置推定装置は、電波画像を用いず、事前に移動体が通過する通過ポイント位置座標を設定するように構成したので、ＧＰＳが利用できない場合も移動体の自己位置を推定することができる。

実施の形態１に係る自己位置推定装置の構成を示す図。実施の形態１に係る参照情報計算部の構成を示す図。実施の形態１に係る参照情報算出方法の例を示す図。実施の形態１に係る自己位置推定装置の動作フローチャートを示す図。実施の形態１に係る参照情報計算部の動作フローチャートを示す図。実施の形態２に係る自己位置推定装置の構成を示す図実施の形態２に係る参照情報計算部の構成を示す図。実施の形態３に係る自己位置推定装置の構成を示す図。実施の形態３に係る誤差計算部の構成を示す図。

実施の形態１
以下、図１から図５を用いて実施の形態１に係る自己位置推定装置について説明する。図１は実施の形態１に係る自己位置推定装置の構成を示す図である。図２は実施の形態１に係る参照情報計算部の構成を示す図である。図３は実施の形態１に係る参照情報算出方法の例を示す図である。図４は実施の形態１に係る自己位置推定装置の動作フローチャートを示す図である。図５は実施の形態１に係る参照情報計算部の動作フローチャートを示す図である。

以下に図１を用いて実施の形態１に係る自己位置推定装置１００の構成を説明する。

実施の形態１に係る自己位置推定装置１００は、慣性計測装置（ＩＭＵ）１１０、デッドレコニング（ＤＲ）計算部３、通過ポイント座標設定部５、参照情報計算部６、拡張カルマンフィルタ７から構成される。

慣性計測装置（ＩＭＵ）１１０は、加速度センサ１と角速度センサ２から構成される。加速度センサ１は移動体の３軸の加速度を検出し、角速度センサ２は３軸の回転角速度を検出する。

ＤＲ計算部３は加速度センサ１や角速度センサ２から得られる加速度及び角速度からデッドレコニング（推測航法）により移動体の位置座標（第一の推定位置）、速度（第一の推定速度）及び姿勢角を算出する。ここで、デッドレコニングとは、慣性計測装置１１０から出力された加速度及び角速度を積分して前回測位時からの移動量を算出し、前回測位した位置にこの移動量を加算して現在の位置を推測する方法である。しかし、積分によって慣性装置の誤差などが積み重なり、時間経過と共に位置推定誤差が増大する。この誤差をドリフト誤差という。

通過ポイント座標設定部５は、移動体が事前に通過する位置座標（以下、通過ポイント位置座標とする）、速度、通過ポイント位置座標を通過する通過時刻の設定を行う。

参照情報計算部６は角速度センサ２から得られる移動体の角速度、加速度センサ１から得られる移動体の加速度を保持しておき、通過ポイント座標設定部５で設定した移動体が通過する通過ポイント位置座標、速度、通過時刻、ＤＲ計算部３から出力される移動体の位置座標、速度を用いて、デッドレコニングにより偽の通過時刻における偽の通過ポイント位置座標（第二の推定位置）及び速度（第二の推定速度）（以下、偽の通過時刻における偽の通過ポイント位置座標及び速度を「参照情報」とする）算出する。ここで、偽の通過ポイント位置座標とは、ＤＲ計算部３によって算出された移動体の位置座標と、通過ポイント座標設定部５で設定した移動体が通過する通過ポイント位置座標が一致した場合における実際に移動体がいる位置座標を指す。すなわち、ＤＲ計算部３のデッドレコニングによって算出された移動体の位置座標はドリフト誤差を含むので、実際の移動体の位置座標は、通過ポイント位置座標と異なる。このとき、実際の移動体の位置座標を偽の通過ポイント位置座標とする。また、偽の通過時刻とは、移動体が偽の通過ポイント位置座標を通過した際の通過時刻をいう。具体的には、ＤＲ計算部３から算出された移動体の位置座標と、通過ポイント座標設定部５によって設定された移動体が通過する通過ポイント位置座標とが一致した際の時刻を指す。

ここで、参照情報計算部６の詳細について、図２を用いて説明する。参照情報計算部６は、運動情報保存部６１、偽通過時刻取得部６２および参照情報用ＤＲ計算部６３とから構成される。

運動情報保存部６１は、慣性計測装置１１０から出力される移動体の加速度、角速度を、単位時間Δｔ毎に保存する。なお、慣性計測装置１１０から出力される加速度、角速度を運動情報という。また、運動情報保存部６１は、単位時間Δｔ毎に運動情報を保存しているため、時刻を計測することができる。

偽通過時刻取得部６２は、ＤＲ計算部３から出力されるドリフト誤差を含んだ移動体の位置座標と通過ポイント座標設定部５から出力される移動体が通過する通過ポイント位置座標とに基づいて、運動情報保存部６１から偽の通過時刻を取得する。具体的には、ＤＲ計算部３から出力される移動体の位置座標と、通過ポイント座標設定部５から出力される移動体が通過する位置座標が一致した際の時刻を偽の通過時刻として運動情報保存部６１から取得する。

参照情報用ＤＲ計算部６３は、偽通過時刻取得部６２から出力される偽の通過時刻と通過ポイント座標設定部５から出力される通過時刻との差に基づいて、運動情報保存部６１に保存されている運動情報を用いてデッドレコニングにより参照情報を算出する。

ここで、参照情報の算出例を図３を用いて説明する。なお、通過ポイント座標設定部５で設定された移動体が通過する通過ポイント位置座標をＡ、速度をＶ、通過時刻をｔとする。また、ＤＲ計算部３によって算出した移動体の位置座標をＡ´、速度をＶ´とする。このとき、偽の通過時刻をｔ´とする。また、通過時刻ｔに比べて偽の通過時刻ｔ´が遅延する場合（ａ）と、通過時刻ｔに比べて偽の通過時刻ｔ´が早い場合（ｂ）とに分けて説明を行う。さらに、本発明において、移動体は必ず通過ポイント設定部５で設定した通過ポイント位置座標を通過することを前提とする。

（ａ）通過時刻ｔに比べて偽の通過時刻ｔ´が遅延する場合
通過ポイント座標設定部５は、事前に移動体が通過する通過ポイント位置座標Ａと通過時刻ｔ、速度Ｖの移動条件を設定する。例えば、図３（ａ）に示すように、通過ポイント座標設定部５は、移動体が速度Ｖ＝５ｍ／ｓの一定速度で通過ポイント位置座標Ａ＝２０ｍを通過時刻ｔ＝１０ｓに通過するよう設定したと仮定する。移動体はＤＲ計算部３がデッドレコニングにより計算した位置座標Ａ´及び速度Ｖ´を元に通過ポイント位置座標Ａの位置座標へ向かう。したがって、ＤＲ計算部３の計算上では、移動体は位置座標Ａ´＝２０ｍとなったときに通過ポイント位置座標Ａを通過したこととなる。しかし、実際はドリフト誤差があるため、移動体は別の位置（以下、偽の通過ポイント位置座標Ｃとする）にいることとなる。また、通過ポイント位置座標Ａと偽の通過ポイント位置座標Ｃとには、ずれがあるため、通過時刻についてもずれが生じる。このとき、実際に偽の通過ポイント位置座標Ｃを移動体が通過した通過時刻を偽の通過時刻ｔ´＝１２ｓとする。この例では通過時刻ｔに比べて偽の通過時刻ｔ´が遅延する場合を想定しているため、移動体は既に通過ポイント位置座標Ａを通過していることとなる。このとき、偽の通過時刻ｔ´＞通過時刻ｔとなり、その差は２ｓである。（以下、偽の通過時刻ｔ´と通過時刻ｔとの差を適宜「時間誤差」とする。）このとき、時間誤差２ｓは、移動体が通過ポイント位置座標Ａから偽の通過ポイント位置座標Ｃまでを移動した時間といえる。この２ｓ間の運動情報はΔｔ毎に運動情報保存部６１に保存されているため、偽の通過ポイント位置座標Ｃと通過ポイント位置座標Ａまでの距離をデッドレコニングによって計算することができる。すなわち、通過ポイント位置座標Ａを基準として、デッドレコニングにより偽の通過ポイント位置座標Ｃの座標を計算することができる。同様にして、参照情報用ＤＲ計算部６３は時間誤差に基づいてデッドレコニングにより速度Ｖ´についても計算することができる。

（ｂ）通過時刻ｔに比べて偽の通過時刻ｔ´が早い場合
図３（ｂ）は通過時刻ｔに比べて偽の通過時刻ｔ´が早い場合を示す。すなわち、移動体は通過ポイント座標設定部５によって設定された通過ポイント位置座標Ａに到達する前にＤＲ計算部３の計算上において通過ポイント位置座標Ａを通過したこととなる。（ｂ）の場合も（ａ）の場合と同様、通過時刻ｔと偽の通過時刻ｔ´の差によって通過ポイント位置座標Ａと偽の通過ポイント位置座標Ｃとの距離を逆算する。ただし、（ｂ）の場合、移動体は通過ポイント位置座標Ａに到達していないため、偽の通過ポイント位置座標Ｃから通過ポイント位置座標Ａまでの間の運動情報が運動情報保存部６１に保存されていない。したがって、参照情報用ＤＲ計算部６３は通過時刻ｔと偽の通過時刻ｔ´の時間誤差分だけ運動情報保存部６１に運動情報を蓄積した後、デッドレコニングによって偽の通過ポイントＣを逆算する。例えば、通過時刻ｔが１０ｓ、偽の通過時刻ｔ´が８ｓの場合、時間誤差は２ｓである。この時間誤差２ｓの間だけ運動情報保存部６１は、慣性計測装置１１０から出力される運動情報を保存する。移動体が時間誤差２ｓ分だけ移動しながら運動情報を運動情報保存部６１に保存した後、参照情報用ＤＲ計算部６３は偽の通過ポイントＣを逆算する。（ａ）の場合と同様、参照情報用ＤＲ計算部６３は時間誤差に基づいてデッドレコニングにより移動体の速度Ｖ´も逆算する。なお、図３（ａ）（ｂ）の説明において、通過ポイント座標設定部５によって設定される移動体の速度は一定と仮定したが、この速度は一定でなくてもよい。例えば、通過ポイント座標設定部５は、通過時刻ｔ＝１ｓ〜２ｓ間は速度Ｖ＝５ｍ／ｓ、通過時刻ｔ＝２ｓ〜８ｓ間は速度Ｖ＝６ｍ／ｓという条件を設定することも可能である。この場合においても、参照情報用ＤＲ計算部６３は時間誤差に基づいて、デッドレコニングにより参照情報を算出することができる。

拡張カルマンフィルタ７は、一般に、ＧＰＳ／ＩＮＳ複合航法システムにおいて、ＧＰＳの観測値（位置座標、速度）とＩＮＳの演算値（位置座標、速度）との差分（位置座標残差、速度残差）を入力観測値として、移動体の位置座標や姿勢角について誤差推定を行う装置である。実施の形態１に係る拡張カルマンフィルタ７はＤＲ計算部３から推定された移動体の位置座標、速度と参照情報計算部６で算出された参照情報（偽の通過ポイント位置座標、速度）との差を入力観測値とすることで、拡張カルマンフィルタ７により移動体の位置座標、速度、姿勢角誤差推定値を算出することができる。また、拡張カルマンフィルタ７は非線形のカルマンフィルタであり、他の非線形カルマンフィルタであるＵＫＦ（ＵｎｓｅｎｔｅｄＫａｌｍａｎＦｉｌｔｅｒ）等を用いてもよい。

次に、図４、図５を用いて実施の形態１に係る自己位置推定装置１００の動作を説明する。

ステップ２００において、ＤＲ計算部３は慣性計測装置１１０から単位時間Δｔ毎に移動体の角速度、加速度を受けて保存し、デッドレコニングにより移動体の位置座標、速度、姿勢角を算出する。

ステップ２０１において、通過ポイント座標設定部５は移動体が通過する通過ポイント位置座標と通過時刻、速度を設定する。また、設定した通過ポイント位置座標と通過時刻を参照情報計算部６に出力する。

ステップ２０２において、参照情報計算部６は参照情報を算出する。ここで、図５を用いて参照情報計算部６が参照情報を算出する動作の詳細を説明する。

ステップ３００において、運動情報保存部６１は慣性計測装置１１０から移動体の加速度、角速度を単位時間Δｔ毎に保存する。

ステップ３０１において、偽通過時刻取得部６２はＤＲ計算部３からドリフト誤差を含む移動体の位置座標と、通過ポイント座標設定部５から事前に設定した移動体が通過する通過ポイント位置座標をそれぞれ取得する。

ステップ３０２において、偽通過時刻取得部６２は、ＤＲ計算部３から出力されるドリフト誤差を含んだ移動体の位置座標と通過ポイント座標設定部５から出力される移動体が通過する通過ポイント位置座標とを受けて、偽の通過時刻を取得する。具体的には、ＤＲ計算部３から出力される移動体の位置座標と、通過ポイント座標設定部５から出力される移動体が通過する位置座標が一致した際の時刻を偽の通過時刻として運動情報保存部６１から取得する。偽通過時刻取得部６２は、偽の通過時刻を参照情報用ＤＲ計算部６３に出力する。

ステップ３０３において、参照情報用ＤＲ計算部６３は偽通過時刻取得部６２から偽の通過時刻、通過ポイント座標設定部５から予め設定した通過時刻を取得する。次に、参照情報用ＤＲ計算部６３は、偽の通過時刻と通過ポイント座標設定部５で予め設定した通過時刻の差をとることで時間誤差を算出する。このとき、偽の通過時刻と通過ポイント座標設定部５で予め設定した通過時刻の大小関係により、以下の処理が異なる。

［偽の通過時刻＞通過ポイント座標設定部５で予め設定した通過時刻（図３（ａ）参照）の場合］
偽通過時刻取得部６２は、運動情報保存部６１から時間誤差分の運動情報を取得し、参照情報用ＤＲ計算部６３は通過ポイント位置座標を基準にデッドレコニングにより移動体の位置と速度を算出する。

［偽の通過時刻＜通過ポイント座標設定部５で予め設定した通過時刻（図３（ｂ）参照）の場合］
運動情報保存部６１には偽の通過ポイント位置座標から通過ポイント座標設定部５で予め設定した通過ポイント位置座標までの移動体の運動情報が保存されていない。したがって、参照情報用ＤＲ計算部６３は時間誤差分の移動体の運動情報を運動情報保存部６１に格納した後に運動情報を取得し、通過ポイント位置座標を基準にデッドレコニングにより移動体の位置と速度を算出する。

このようにして、参照情報用ＤＲ計算部６３は参照情報を算出する。なお、参照情報用ＤＲ計算部６３によって算出された移動体の位置及び速度は、通過ポイント座標設定部で設定された通過ポイント位置座標を基準として単位時間Δｔ毎に運動情報保存部６１に保存された運動情報を用いて移動体の位置を算出したものであるため、ＤＲ計算部３で算出された位置及び速度よりも誤差が少ない。

ステップ３０４において、参照情報用ＤＲ計算部６３は参照情報を出力し、動作を終了する。

ステップ２０３において、自己位置推定装置１００は、ＤＲ計算部３から出力された移動体の位置座標、速度と、参照情報計算部６から出力された参照情報（偽の通過ポイント位置座標、速度）との差分値を観測値としてカルマンフィルタ７に入力することで位置座標、速度、姿勢角の誤差推定値が算出される。

ステップ２０４において、位置座標、速度、姿勢角のそれぞれの誤差推定値とＤＲ計算部３から出力された位置座標、速度、姿勢角とをそれぞれ和をとることによって、補正した移動体の位置座標、速度姿勢角の更新値が得られる。またカルマンフィルタ７から誤差推定値をＤＲ計算部３に返すことで、ＤＲ計算部３内の移動体の位置座標、速度、姿勢角を更新する。以上の動作によって、自己位置推定装置１００の動作が終了する。なお、移動体はＧＰＳが使用できない区間において、この動作を複数回実行することで、随時移動体の現在位置の座標を補正することができる。

なお、実施の形態１に係る自己位置推定装置１００は慣性計測装置１１０から出力される加速度、角速度から求めたが、本発明はこれに限られるものではなく、ドップラー航法計算部を設け、このドップラー航法計算部から出力される速度を使用して参照情報を算出するように構成してもよい。ここで、ドップラー航法計算部とは、移動体に搭載されたレーダを用いたドップラー効果によって移動体の対地速度を測定する速度計測装置である。但し、車両においては、ドップラー航法計算部は車速を計測できるものであればよく、車速センサなどの速度計測装置を使用してもよい。

また、実施の形態１に係る自己位置推定装置１００はＤＲ計算部３から単位時間Δｔ毎に位置座標、速度を算出するようにし、参照情報用ＤＲ計算部６３によって通過ポイント位置座標設定部５から偽の通過ポイント位置座標までの間の参照情報を単位時間Δｔ毎にデッドレコニングにより算出するように構成してもよい。このように構成した場合、ＤＲ計算部３によって単位時間Δｔ毎に算出された位置座標及び速度と参照情報用ＤＲ計算部６３によって単位時間Δｔ毎に算出された参照情報との差を単位時間Δｔ毎に複数算出して、それぞれ拡張カルマンフィルタに入力することによって、移動体の位置座標誤差及び速度誤差を算出する。このように構成することで、観測雑音に対する平滑効果が得られるため、より精度の高い誤差推定値を求めることが可能となる。

以上のように実施の形態１に係る自己位置推定装置１００は構成されるので、画像情報を用いず、事前に指定した移動体が通過する通過ポイント位置座標を設定することで、ＧＰＳが利用できない場合も自己位置の算出することができる。

また、実施の形態１に係る自己位置推定装置１００はＤＲ計算部３及び運動情報保存部６１に単位時間Δｔ毎に保存した加速度及び角速度に基づいてデッドレコニングにより移動体の位置座標及び速度を算出するため、より精度の高い誤差推定値を求めることが可能となる。

実施の形態２
以下、図６、図７を用いて実施の形態２に係る自己位置推定装置１００について説明する。図６は実施の形態２に係る自己位置推定装置の構成を示す図である。図７は実施の形態２に係る自己位置推定装置の参照情報計算部を示す図である。なお、実施の形態２に係る自己位置推定装置１００の構成において、実施の形態１に係る自己位置推定装置１００の構成と同じものについては説明を省略する。

以下、図６を用いて実施の形態２に係る自己位置推定装置１００の構成を説明する。

実施の形態２に係る自己位置推定装置１００は、実施の形態１の構成に加えて地図データ８を備える。実施の形態２に係る自己位置推定装置１００はこの地図データ８を用いて移動体が通過する通過ポイント位置座標を指定することを特徴とする。

地図データ８は、例えばカーナビゲーションシステムで用いられる道路地図などの地図であり、事前に自己位置推定装置１００に保存されている。

次に実施の形態２の自己位置推定装置１００の動作について説明する。

ＤＲ計算部３は慣性計測装置１１０から得られる移動体の加速度と角速度からデッドレコニングにより移動体の位置座標、速度及び姿勢角を算出し、地図データ８から得られる地図データ８を用いて公知のマップマッチング処理により移動体の位置座標、速度及び姿勢角を補正する。ここで、マップマッチング処理とは、カーナビゲーションシステムなどに用いられる機能の一つで、ＧＰＳで観測した移動体の位置座標の誤差を補正するために地図情報を利用する技術である。ＧＰＳで観測した移動体の位置座標の精度が低いと、内蔵した地図上に描画したときに道から大きく外れてしまっていることがある。そこで、移動体の移動方向などから、付近の地図で該当する道路を探し出して正しい移動体の位置座標を推測する処理を行う。このように構成することで、より精度の高い移動体の位置座標、速度及び姿勢角を算出することができる。

通過ポイント座標設定部５はＧＰＳ稼動時において任意の近距離の位置座標を便宜上の移動体が通過する通過ポイント位置座標としてその通過時刻と位置座標を登録する。通過ポイント位置座標はＧＰＳ稼働時に適宜更新される。

ここで、図７を用いて実施の形態２に係る参照情報計算部６が参照情報を算出する動作について説明する。

参照情報用ＤＲ計算部６３は運動情報保存部６１から出力される、移動体の加速度（または速度）、角速度と偽通過時刻取得部６２から出力される偽の通過時刻に基づいて、参照情報を計算する。参照情報ＤＲ計算部６３は地図データ８から得られる地図データ８を用いてマップマッチング処理により参照情報を補正する。このように構成することで、より精度の高い参照情報を算出することができる。

自己位置推定装置１００は、出力された参照情報（偽の通過ポイント位置座標、速度）とＤＲ計算部３から得られた移動体の位置座標、速度の差を拡張カルマンフィルタ７の入力観測値とすることで、拡張カルマンフィルタ７により移動体の位置座標、速度及び姿勢角誤差推定値が算出される。ＤＲ計算部３から出力される移動体の位置座標、速度、姿勢角は拡張カルマンフィルタ７から得られる誤差推定値を用いて修正される。

以上のように、実施の形態２に係る自己位置推定装置１００は地図データ８によるマップマッチング処理によって補正した参照情報と、ＤＲ計算部３から出力された移動体の位置座標、速度との差を観測値として拡張カルマンフィルタ７に入力する構成としたので、ＧＰＳが利用できない場合においても、より正確な自己位置の修正を可能とする。

実施の形態３
以下、図８、図９を用いて実施の形態３に係る自己位置推定装置１００について説明する。図８は実施の形態３に係る自己位置推定装置の構成を示す図である。図９は実施の形態３に係る誤差計算部の構成を示す図である。なお、実施の形態３に係る自己位置推定装置１００の構成において、実施の形態１に係る自己位置推定装置１００の構成と同じものについては説明を省略する。

実施の形態１に係る自己位置推定装置１００は、ＤＲ計算部３から出力される移動体の位置座標と通過ポイント座標設定部５で予め設定された通過ポイント位置座標とが一致する場合の偽の通過時刻と、通過ポイント座標設定部５で設定した通過時刻との時間誤差からデッドレコニングにより偽の通過ポイント位置座標を算出する構成とした。一方、実施の形態３に係る自己位置推定装置１００は、運動情報保存部６１で計測した現在時刻が通過ポイント座標設定部５で設定した通過時刻になった時点で、移動体が通過ポイント座標設定部５で設定した通過ポイント位置座標にいると判断することを特徴とする。

以下、実施の形態３に係る自己位置推定装置１００の構成について、図８、図９を用いて説明する。

実施の形態３に係る自己位置推定装置１００は、実施の形態１に係る参照情報計算部６に代えて誤差計算部９を備える。また、実施の形態１に係る偽通過時刻取得部６２と参照情報用ＤＲ計算部６３に代えて通過ポイント判定部８２を備える。

誤差計算部９は、運動情報保存部６１で計測した時刻が通過ポイント座標設定部５で設定された通過時刻と一致した場合におけるＤＲ計算部３から算出された通過ポイント位置座標と、通過ポイント座標設定部５で設定された通過ポイント座標設定部５で設定された通過ポイント位置座標との差を計算する。誤差計算部９はＤＲ計算部３から出力された移動体の位置座標、速度と、通過ポイント座標設定部５から出力された移動体が通過する通過ポイント位置座標、速度、通過時刻とを受ける。

次に図９を用いて誤差計算部９の構成について説明する。

通過ポイント判定部８２は、通過ポイント座標設定部５で設定した通過時刻になった際の通過ポイント位置座標を移動体の現在の位置座標として出力する。すなわち、運動情報保存部６１で計測された時刻と通過ポイント座標設定部５で設定した通過時刻が一致したとき、通過ポイント判定部８２は通過ポイント座標設定部５で設定した移動体が通過する通過ポイント位置座標を出力する。また、このときの移動体の速度は通過ポイント座標設定部５で設定した速度をそのまま出力する。

このように、通過ポイント判定部８２から出力された移動体が通過する通過ポイント位置座標、速度と、ＤＲ計算部３から出力された移動体の位置座標、速度との差をそれぞれとり、この差分値を観測値として拡張カルマンフィルタ７へ出力する。

以上のように、実施の形態３に係る自己位置推定装置１００は構成される。なお、実施の形態３に係る自己位置推定装置１００は、実施の形態２に係る自己位置推定装置１００の地図データ８を適用することも可能である。

実施の形態３に係る自己位置推定装置１００は、通過ポイント座標設定部５で設定した通過時刻に達した場合に、移動体の現在地を通過ポイント座標設定部５で設定した位置座標に補正する構成としたので、実施の形態１のように通過ポイント座標設定部５で設定された通過ポイント位置座標に到達する前でも即座に現在位置を補正することができる。

１加速度センサ、２角速度センサ、３ＤＲ計算部、５通過ポイント座標設定部、６参照情報計算部、７拡張カルマンフィルタ、８地図データ、９誤差計算部、６１運動情報保存部、６２偽通過時刻取得部、６３参照情報用ＤＲ計算部、８２通過ポイント判定部、１００自己位置推定装置、１１０慣性計測装置

Claims

設定した通過ポイント位置座標を通過する移動体の運動情報を計測する慣性計測装置と、
前記慣性計測装置から前記移動体の運動情報を受けて、デッドレコニングにより前記移動体の第一の推定位置及び第一の推定速度を算出するＤＲ計算部と、
前記移動体が通過する通過ポイント位置座標と該通過ポイント位置座標を通過する通過時刻と、速度とを設定する通過ポイント座標設定部と、
前記移動体の運動情報を単位時間毎に保持し、前記第一の推定位置と前記通過ポイント位置座標とが一致した時点における時刻と、前記通過ポイント座標設定部で設定された通過時刻との差分値に基づいて、前記運動情報を用いてデッドレコニングにより前記移動体の第二の推定位置と第二の推定速度とを算出する参照情報計算部と、
前記第一の推定位置と第二の推定位置との差分値と、前記第一の推定速度と第二の推定速度との差分値とを観測値として受けて、前記移動体の現在位置の誤差推定値を算出するカルマンフィルタと
を備えることを特徴とする自己位置推定装置。
前記ＤＲ計算部は前記移動体の第一の推定位置までの位置座標及び速度をデッドレコニングにより単位時間毎に複数算出し、
前記参照情報計算部は前記通過ポイント座標設定部で設定された通過ポイント位置座標から前記第二の推定位置までの位置座標及び速度を単位時間毎にデッドレコニングにより複数算出し、
前記カルマンフィルタは、前記ＤＲ計算部によって単位時間毎に算出された複数の位置座標及び速度と、前記参照情報計算部によって算出された複数の位置座標及び速度との差を単位時間毎に算出した複数の差分値を観測値として受けて、前記移動体の現在位置の誤差推定値を算出することを特徴とする請求項１に記載の自己位置推定装置。
設定した通過ポイント位置座標を通過する移動体の運動情報を計測する慣性計測装置と、
前記慣性計測装置から前記移動体の運動情報を受けて、デッドレコニングにより前記移動体の第一の推定位置及び第一の推定速度を算出するＤＲ計算部と、
前記移動体が通過する通過ポイント位置座標と該通過ポイント位置座標を通過する通過時刻と、速度とを設定する通過ポイント座標設定部と、
現在時刻と前記通過ポイント座標設定部で設定された通過時刻とが一致した場合に、前記通過ポイント位置座標を前記移動体の第二の推定位置とする誤差計算部と、
前記第一の推定位置と第二の推定位置との差分値と、前記第一の推定速度と前記通過ポイント座標設定部で設定した速度との差分値とを観測値として受けて、現在位置の誤差推定値を算出するカルマンフィルタと
を備えることを特徴とする自己位置推定装置。
地図データを備え、
前記通過ポイント位置座標を前記地図データ上に設定し、前記第一及び第二の推定位置をマップマッチング処理により補正することを特徴とする請求項１ないし請求項３のいずれかに記載の自己位置推定装置。
前記移動体の速度を計測する速度計測装置を備え、
前記参照情報計計算部は、前記移動体の運動情報と前記速度計測装置で計測された移動体の速度とに基づいてデッドレコニングにより第二の推定位置と第二の推定速度とを算出することを特徴とする請求項１、請求項２又は請求項４のいずれかに記載の自己位置推定装置。