測位装置、ナビゲーションシステム
【課題】より精度よく測位した位置を補正し、マップマッチングした場合でも非連続な走行軌跡となることが低減される測位装置、ナビゲーションシステムを提供すること。
【解決手段】ＧＰＳ等の電波航法測位手段１により移動体の位置を検出する測位装置１０において、移動体の挙動情報を検出する自律センサ２、３と、電波航法測位手段１による測位位置に自律センサ２，２による検出情報を累積して自律航法測位位置を検出する位置検出手段と、を有し、位置検出手段は、測位位置又は自律航法測位位置のいずれか、及び、過去の自律航法測位位置の履歴に基づき予測した予測位置に基づき、移動体の位置を推定することを特徴とする。
【解決手段】ＧＰＳ等の電波航法測位手段１により移動体の位置を検出する測位装置１０において、移動体の挙動情報を検出する自律センサ２、３と、電波航法測位手段１による測位位置に自律センサ２，２による検出情報を累積して自律航法測位位置を検出する位置検出手段と、を有し、位置検出手段は、測位位置又は自律航法測位位置のいずれか、及び、過去の自律航法測位位置の履歴に基づき予測した予測位置に基づき、移動体の位置を推定することを特徴とする。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の位置を検出する測位装置及びナビゲーションシステムに関し、特に、電波航法による測位位置に自律センサによる検出情報を累積して位置を検出する測位装置及びナビゲーションシステムに関する。
【背景技術】
【０００２】
ナビゲーションシステムでは、ＧＰＳ（Ｇｒｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波に基づき自車両の位置を測位して、車速センサ及びジャイロセンサを用いて走行距離及び走行方向を累積しながら自律航法により車両の現在位置を精度よく推定する。
【０００３】
しかしながら、ＧＰＳ衛星からの電波を受信できない状態では、自律航法による測位に含まれる誤差が時間と共に増幅されるため好ましくない。このため、測位した車両の位置を補正する種々の方法が提案されている。例えば、車速センサにより速度ベクトルを取得し、現在位置と速度ベクトルにより次回の測位タイミングの位置を推定し、次回の側位位置と推定位置との平均を測位位置とする測位方法が提案されている（例えば、特許文献１参照。）。過去の速度ベクトルを利用して現在位置を検出するので、ばらつきが低減され安定した軌跡が得られる。しかしながら、このように過去の速度ベクトルを用いて現在位置を補正しても徐々に誤差は大きくなってしまう。
【０００４】
また、ナビゲーションシステムの道路地図を利用して測位した位置を補正するマップマッチングが知られている。道路地図はノード（例えば、交差点）間をリンク（例えば、道路）により結合したものであるため、自律航法により検出した位置を道路地図に対応づけることで、車両が道路上を走行するように自車両の位置を補正することができる。
【０００５】
図１（ａ）〜（ｃ）はマップマッチングの様子を示す図である。図１（ａ）は地図データベースに記憶されている道路地図を、図１（ｂ）は自律航法による走行軌跡をそれぞれ示す。ナビゲーションシステムは、図１（ｂ）の走行軌跡を図１（ａ）の道路地図に対応づけ、走行軌跡と最も相関の高いリンクを選択し、道路地図上に車両の位置を表示する。
【特許文献１】特開平８−６８６５１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、マップマッチングは所定の時間間隔で行うため、右左折等により走行方向が大きく変わった場合やリンクが複雑な道路網では、道路地図における車両の位置と実際の位置とが大きく異なる場合が生じる。
【０００７】
図１（ｄ）〜（ｆ）はマップマッチングした車両の位置が実際の車両の位置と異なる場合の一例を示す。図１（ｄ）は道路地図を、図１（ｅ）は走行軌跡を、図１（ｆ）はリンクにマッチングさせた位置をｎ１〜ｎ１２でそれぞれ示す。車両はノードＡで左折したにもかかわらず、ｎ８のマッチング位置では左折が未だに反映されていない。そして、ｎ９のマッチング位置で左折による走行軌跡がマッチングに反映されている。このような場合、ナビゲーションシステムの表示装置に表示される車両は、ｎ８の位置から突然ｎ９の位置に移動することになる。すなわち、従来の測位結果をマップマッチングにより補正すると、走行軌跡が非連続になる場合があり適切な走行支援が困難となる場合が生じる。
【０００８】
本発明は、上記課題に鑑み、より精度よく測位した位置を補正し、マップマッチングした場合でも非連続な走行軌跡となることが低減される測位装置、ナビゲーションシステムを提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記課題を解決するため、本発明は、ＧＰＳ等の電波航法測位手段により移動体の位置を検出する測位装置において、移動体の挙動情報を検出する自律センサと、電波航法測位手段による測位位置に自律センサによる検出情報を累積して自律航法測位位置を検出する位置検出手段と、を有し、位置検出手段は、測位位置又は前記自律航法測位位置のいずれか、及び、過去の前記自律航法測位位置の履歴に基づき予測した予測位置に基づき、移動体の位置を推定することを特徴とする。
【００１０】
履歴に基づき予測位置を予測するので、移動体の位置の滑らかな推移が得られ、非連続な走行軌跡となることが防止できる。例えば、予測位置は、自律航法測位位置の履歴を平滑化する平滑化曲線を延長して予測する。平滑化することで、適切な予測位置を予測することができる。
【００１１】
また、予測位置の予測に使用した過去の前記自律航法測位位置の分散を算出することで、測位位置又は自律航法測位位置のいずれか、及び、予測位置をカルマンフィルタによりカップリングすることができる。すなわち、最も確率の高い位置を推定できる。
【発明の効果】
【００１２】
より精度よく測位した位置を補正し、マップマッチングした場合でも非連続な走行軌跡となることが低減される測位装置、ナビゲーションシステムを提供することができる。
【発明を実施するための最良の形態】
【００１３】
以下、本発明を実施するための最良の形態について、図面を参照しながら説明する。
図２（ａ）は、本実施の形態の測位装置９を適用したナビゲーションシステム１０の概略構成図を示す。ナビゲーションシステム１０は、ナビゲーションシステムを制御するナビＥＣＵ（Ｅｌｅｃｔｒｉｃａｌ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）５により制御され、ナビＥＣＵ５にはＧＰＳ（Ｇｒｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波を受信するＧＰＳ受信装置１、車両の速度を検出する車速センサ２、車両の向きを検出するジャイロセンサ３、道路地図データを記憶した地図データベース４、道路地図に現在位置を表示する液晶やＨＵＤ（Ｈｅａｄ Ｕｐ Ｄｉｓｐｌａｙ）等の表示装置６及びナビゲーションシステム１０を操作するための入力装置７が接続されている。
【００１４】
ナビＥＣＵ５は、プログラムを実行するＣＰＵ、プログラムを記憶したＲＯＭ、データやプログラムを一時的に記憶するＲＡＭ、データを入力及び出力する入出力装置、ＮＶ（Ｎｏｎ Ｖｏｌａｔｉｌｅ）−ＲＡＭ等がバスを介して接続されたコンピュータとして構成される。
【００１５】
ナビＥＣＵ５のＣＰＵがプログラムを実行することで、後述するように自車両の位置を検出する位置検出手段を実現する。
【００１６】
ナビＥＣＵ５の位置検出手段は、ＧＰＳ衛星からの電波に基づき自車両の位置を測位して、さらに自律航法等により自車両の位置を推定する。本実施の形態の位置検出手段は、自律航法により検出した自車両の位置から走行軌跡を取得し、自律航法により算出した自車位置ｚと走行軌跡から予測した予測位置ｘとをカルマンフィルタによりカップリングして確率的に最適な位置ｙを推定する。
【００１７】
位置検出手段は、所定の軌道を周回する複数のＧＰＳ衛星のうち現在の車両の位置から所定の仰角に入るＧＰＳ衛星を４つ以上選択し、それらのＧＰＳ衛星から発信される電波を受信する。なお、ＧＰＳ衛星からの電波を位置の知られた地上の基地局が受信してもよい。
【００１８】
位置検出手段は電波の到達時間を計算し、到達時間と光速ｃから捕捉したＧＰＳ衛星までの距離を算出する。自車両の位置は、緯度・経度・標高で特定される３次元上の１点であると考えられるので、３つのＧＰＳ衛星と自車両の距離を算出し、各ＧＰＳ衛星との距離を半径とする球面が交差する点を自車両の位置として測位する。
【００１９】
また、位置検出手段は、車速センサ２及びジャイロセンサ３を用いて自律航法により自車両の位置を推定することができる。主にＧＰＳ衛星を捕捉できない場合、位置検出手段は、ＧＰＳ衛星からの電波により測位された位置に、車速センサ２による走行距離及びジャイロセンサ３による走行方向を累積しながら自律航法により車両の現在位置を精度よく推定する。
【００２０】
なお、推定した自車両の位置を道路地図に対応づけるマップマッチング法により、地図上に表示する自車両の位置をさらに補正してもよい。この場合、マップマッチング後の自車位置が自車位置ｚとなる。
【００２１】
地図データベース４は、ハードディスク、ＤＶＤ（Digital Versatile Disk）、フラッシュメモリ等の記録媒体により構成される。地図データベース４に記憶された道路地図データには、道路網や交差点などの道路地図情報が、緯度・経度に対応づけて格納されている。道路地図は、実際の道路網をノード(道路と道路が交差する点、例えば交差点)及びリンク(ノードとノードを接続する道路)に対応づけて、テーブル状のデータベースとして構成される。
【００２２】
入力装置２０は、押下式のキーボード、ボタン、リモコン、十字キー、タッチパネル等で構成され、運転者からの操作を入力するためのインターフェイスである。また、マイクを備え運転者の発する音声を音声認識回路で認識して操作を入力してもよい。目的地までのルート検索を行う場合、運転者は目的地を住所、地名、ランドマーク名、郵便番号等で入力することができる。
【００２３】
本実施の形態で用いるカルマンフィルタについて説明する。カルマンフィルタはある系の状態（ここでは自車両の位置）を推定するためのフィルタリング理論である。例えば、ｘ（真）を真の位置、現在の時刻ｔ
において推定される位置をｙ とすると、過去から現在に至るまでのすべての観測値ｚの線形結合で現在の最適推定値ｙを推定する。この推定方法は、時刻k における真の値ｘ（真） と最適推定値ｙ の差の二乗の総和を最小にすることと等価であるが、カルマンフィルタを用いると現在の最適推定値ｙは一つ前の最適推定値に既知の行列をかけ、現在の観測値ｚで決まる補正項を加えることにより求まることになる。
【００２４】
図３（ａ）はカルマンフィルタにより最適推定値ｙを推定するための概念図を示す。なお、図３（ａ）ではｘ、ｚをスカラ量として示したが、本実施の形態ではＸ、Ｚはベクトル量である。
【００２５】
ここで、ＺとＸの間には次の関係があるものとする。
Ｚ＝ＨＸ＋Ｖ …（１）
但し、Ｈは既知の観測行列である。また、Ｖは白色ガウス雑音の測定誤差を表すベクトルであり、その共分散をＲとする。また、位置Ｘは統計適性質が明らかであり好ましくは正規分布で与えられ、図３（ａ）では平均値を＜ｘ＞、共分散をＭとした。
【００２６】
２つの不規則な事象ＸとＺが互いに従属である場合、ベイズの公式から事象Ｚが起こった時に事象Ｘが起こる確率はｐ（Ｘ／Ｚ）＝ｐ（Ｚ／Ｘ）・ｐ（Ｘ）／ｐ（Ｚ）と表すことができる。
【００２７】
この式から、例えば、ｐ（ｘ／ｚ３）はｐ（ｚ３／ｘ）とｐ（ｘ）の積に比例するため、図３（ａ）では＜ｘ＞とｘ３の間の一点で最大になる。このｘがｐ（ｘ／ｚ３）を最大にするｘの最適推定値ｙである。なお、解析的にｙの分散はＡ＝（Ｍ−１＋ＨｔＲ−１Ｈ）−１になる。
【００２８】
カルマンフィルタでは、位置Ｘの最適推定値Ｙが次式により求められる。なお、添字のｉは観測値Ｚを観測した番号、ｔは転置行列、−１は逆行列である。
Ｙ（ｉ）＝Ｘ（ｉ−１）＋Ｋ（ｉ）・｛Ｚ（ｉ）−Ｈ（ｉ）・Ｘ（ｉ−１）｝ …（２）
Ｋ（ｉ）はカルマンゲイン行列であり、次のように表せる。
Ｋ（ｉ）＝Ａ（ｉ）・ＨｉｔＲｉ−１ …（３）
ただし、Ａ（ｉ）＝（Ｍ−１＋ＨｔＲ−１Ｈ）−１
＝Ｍ（ｉ）−Ｍ（ｉ）・Ｈ（ｉ）ｔ｛Ｈ（ｉ）Ｍ（ｉ）Ｈ（ｉ）ｔ＋Ｒ（ｉ）｝−１Ｈ（ｉ）Ｍ（ｉ） …（３）
である。
【００２９】
図２（ｂ）に本実施の形態においてカルマンフィルタにより最適推定値Ｙを推定するための構成を示した。まず、上述したように位置検出手段は、ＧＰＳ受信装置１、車速センサ２、ジャイロセンサ３の少なくともいずれかにより自車両の観測値Ｚ（以下、測位位置Zという）を演算する。なお、マップマッチングにより道路地図に対応づけた後の値を測位位置Ｚとしてもよい。そして、過去の側位位置Ｚを使用して修正目標値Ｘを演算し、測位位置Ｚ及び修正目標値Ｘを用いてカルマンフィルタにより最適推定値Ｙを推定する。
【００３０】
観測行列Ｈは、式（１）や図３（ａ）に示すように観測した測位位置Ｚにより目標推定値Ｘを推定するものであるので、後述する平滑化方法や修正目標値Ｘの予測方法に応じて定められる既知の行列である。例えば、４つ以上のＧＰＳ衛星を捕捉できる状態で走行した際の目標推定誤差Ｘ及び測位位置Ｚから、ＧＰＳにより測位した位置が推定されるようにシミュレーションにより求めてもよい。
【００３１】
以上の構成により、最適推定値Ｙを推定する処理の手順を図４のフローチャート図に基づき説明する。
【００３２】
〔ステップＳ１〕
まず、位置検出手段は、時刻ｔの自車両の測位位置Ｚを測位する。測位位置Ｚは少なくとも移動平面上の緯度及び経度を示すベクトルであり、より好ましくは標高を含んでもよい。ＧＰＳ受信装置１による測位誤差は例えば捕捉した衛星数に応じて既知であり、車速センサ２及びジャイロセンサ３の誤差は速度等に応じて既知である。したがって、これらに基づき演算した測位位置Ｚの誤差の共分散Ｒも既知である。図２（ｂ）に示すように、カルマンフィルタ（式（２））にはこの測位位置Ｚ及び共分散Ｒが入力される。
【００３３】
〔ステップＳ２〕
また、位置検出手段は測位位置ＺをＲＡＭ等に記憶し、測位位置Ｚを履歴を得る。図５（ａ）は移動平面において測位位置Ｚの履歴を示す図である。図５（ａ）ではｚ（ｔ−１）までが演算された測位位置Ｚである。
【００３４】
〔ステップＳ３〕
位置検出手段は、時刻ｔ−１から過去の数ステップ分の走行履歴を平滑化処理して走行軌跡を取得する。平滑化処理にはどのような手法を用いてもよい。例えば、直線の道路を走行している場合には１次のスプライン平滑化を、右左折したように大きく走行軌跡が変化している場合には２次又は３次以上のスプライン平滑化を行う。なお、平滑化処理は、加重平均、核型平滑化や局所回帰平滑化等の算出方法を用いてもよい。位置検出手段はこのように測位の度に走行履歴を平滑化する。なお、平滑化の目的は、時刻ｔにおける自車両の位置を予測することであるので、直近の数秒〜数十秒又は所定数の測位位置Zのみを平滑化処理すればよい。
【００３５】
〔ステップＳ４〕
ついで、位置検出手段は平滑化処理により得られた走行軌跡から時刻ｔにおける自車両の位置（修正目標値Ｘ）を予測する。スプライン平滑化した場合、時刻ｔまで平滑化曲線を延長することで時刻ｔにおける自車両の位置を予測できる。ここで予測した位置が修正目標値Ｘとなる。なお、平滑化曲線の延長が困難な場合、走行軌跡を取得した複数の測位位置Ｚから線形予測してもよい。目標推定値Ｘは測位位置Ｚと同様に移動平面上の緯度及び経度を示すベクトルであり、より好ましくは標高を含んでもよい。
【００３６】
この修正目標値Ｘは、走行軌跡を取得し更に時刻ｔにおける自車位置を予測したものであるため誤差を含む。そこで、位置検出手段は平滑化処理を行った測位位置Ｚに基づき分散を求め、この分散を目標推定値Ｘの共分散Ｍとする。
【００３７】
図５（ｂ）は修正目標値Ｘ及びその分散のイメージを示す図である。点線で示した範囲が修正目標値Ｘの分散となる。なお、図５（ｂ）では測位結果Ｚを平滑化曲線に重ねて示した。分散の算出は、例えば、測位位置Ｚの平均と各測位位置Ｚ（ｉ）との差の２乗和を標本数で割るなど、一般的な方法で算出する。図２（ｂ）に示すように、カルマンフィルタ（式（２））にはこの修正目標値Ｘ及び共分散Ｍが入力される。
【００３８】
〔ステップＳ５〕
位置検出手段は、測位位置Ｚ及び共分散Ｒ、並びに、修正目標値Ｘ及び共分散Ｍにより式（２）を用いて最適推定値Ｙを演算する。図３（ｂ）にはカルマンフィルタにより演算される最適推定値Ｙのイメージを示した。図３（ｂ）に示すように、測位位置Ｚの分布と修正目標値Ｘの分布から、確率的に最も高い自車両の位置、最適推定値Ｙが演算される。
【００３９】
図６は本実施の形態の測位装置９を用いて一般道路を走行しながら演算した最適推定値Ｙの履歴を示す。図６では比較のため従来のナビゲーションシステム（以下、単に従来品という）による測位結果を重ねてプロットしている。従来品による測位結果はマップマッチングにより道路地図に対応づけたものであり、最適推定値Ｙの履歴はマップマッチングは行っていない。
【００４０】
図６に示すように、従来品による測位結果ではｓ１〜ｓ３において測位結果に非連続な測位点が生じる。これは、マップマッチングした際に実際に走行している道路とは異なる道路に対応づけたものである。これに対し、本実施の形態の測位装置９では、非連続な測位はなく、走行履歴は滑らかに推移している。したがって、マップマッチングした場合でも測位結果に非連続な箇所が生じることがない。
【００４１】
以上のように、本実施の形態の測位装置９によれば、測位位置Ｚを修正するために、過去の測位結果の走行軌跡を平滑化し修正目標値Ｘを使用することから、ＧＰＳによる測位の誤差が大きい場合やＧＰＳ衛星を捕捉できない場合でも精度よく自車両の位置を推定することができる。測位位置Ｚと修正目標値Ｘをカルマンフィルタに入力して最も確度の高い位置を推定するので、精度よく位置を検出できる。
【００４２】
このため、非連続な測位結果を生じることも大幅に減少される。また、非連続な測位結果が生じないので、マップマッチングした場合にも走行している道路（リンク）が切り替わることがないので、運転者が戸惑うことが防止できる。
【図面の簡単な説明】
【００４３】
【図１】マップマッチングの様子を示す図である。
【図２】測位装置を適用したナビゲーションシステムの概略構成図である。
【図３】カルマンフィルタにより位置を推定する概念図である。
【図４】最適推定値を推定する処理の手順を示すフローチャート図である。
【図５】移動平面において測位位置の履歴及び平滑化した曲線を示す図である。
【図６】一般道路を走行しながら演算した最適推定値Ｙの履歴を示す図である。
【符号の説明】
【００４４】
１ ＧＰＳ受信装置
２ 車速センサ
３ ジャイロセンサ
４ 地図データベース
５ ナビＥＣＵ
６ 表示装置
７ 入力装置
１０ ナビゲーションシステム
【技術分野】
【０００１】
本発明は、移動体の位置を検出する測位装置及びナビゲーションシステムに関し、特に、電波航法による測位位置に自律センサによる検出情報を累積して位置を検出する測位装置及びナビゲーションシステムに関する。
【背景技術】
【０００２】
ナビゲーションシステムでは、ＧＰＳ（Ｇｒｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波に基づき自車両の位置を測位して、車速センサ及びジャイロセンサを用いて走行距離及び走行方向を累積しながら自律航法により車両の現在位置を精度よく推定する。
【０００３】
しかしながら、ＧＰＳ衛星からの電波を受信できない状態では、自律航法による測位に含まれる誤差が時間と共に増幅されるため好ましくない。このため、測位した車両の位置を補正する種々の方法が提案されている。例えば、車速センサにより速度ベクトルを取得し、現在位置と速度ベクトルにより次回の測位タイミングの位置を推定し、次回の側位位置と推定位置との平均を測位位置とする測位方法が提案されている（例えば、特許文献１参照。）。過去の速度ベクトルを利用して現在位置を検出するので、ばらつきが低減され安定した軌跡が得られる。しかしながら、このように過去の速度ベクトルを用いて現在位置を補正しても徐々に誤差は大きくなってしまう。
【０００４】
また、ナビゲーションシステムの道路地図を利用して測位した位置を補正するマップマッチングが知られている。道路地図はノード（例えば、交差点）間をリンク（例えば、道路）により結合したものであるため、自律航法により検出した位置を道路地図に対応づけることで、車両が道路上を走行するように自車両の位置を補正することができる。
【０００５】
図１（ａ）〜（ｃ）はマップマッチングの様子を示す図である。図１（ａ）は地図データベースに記憶されている道路地図を、図１（ｂ）は自律航法による走行軌跡をそれぞれ示す。ナビゲーションシステムは、図１（ｂ）の走行軌跡を図１（ａ）の道路地図に対応づけ、走行軌跡と最も相関の高いリンクを選択し、道路地図上に車両の位置を表示する。
【特許文献１】特開平８−６８６５１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、マップマッチングは所定の時間間隔で行うため、右左折等により走行方向が大きく変わった場合やリンクが複雑な道路網では、道路地図における車両の位置と実際の位置とが大きく異なる場合が生じる。
【０００７】
図１（ｄ）〜（ｆ）はマップマッチングした車両の位置が実際の車両の位置と異なる場合の一例を示す。図１（ｄ）は道路地図を、図１（ｅ）は走行軌跡を、図１（ｆ）はリンクにマッチングさせた位置をｎ１〜ｎ１２でそれぞれ示す。車両はノードＡで左折したにもかかわらず、ｎ８のマッチング位置では左折が未だに反映されていない。そして、ｎ９のマッチング位置で左折による走行軌跡がマッチングに反映されている。このような場合、ナビゲーションシステムの表示装置に表示される車両は、ｎ８の位置から突然ｎ９の位置に移動することになる。すなわち、従来の測位結果をマップマッチングにより補正すると、走行軌跡が非連続になる場合があり適切な走行支援が困難となる場合が生じる。
【０００８】
本発明は、上記課題に鑑み、より精度よく測位した位置を補正し、マップマッチングした場合でも非連続な走行軌跡となることが低減される測位装置、ナビゲーションシステムを提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記課題を解決するため、本発明は、ＧＰＳ等の電波航法測位手段により移動体の位置を検出する測位装置において、移動体の挙動情報を検出する自律センサと、電波航法測位手段による測位位置に自律センサによる検出情報を累積して自律航法測位位置を検出する位置検出手段と、を有し、位置検出手段は、測位位置又は前記自律航法測位位置のいずれか、及び、過去の前記自律航法測位位置の履歴に基づき予測した予測位置に基づき、移動体の位置を推定することを特徴とする。
【００１０】
履歴に基づき予測位置を予測するので、移動体の位置の滑らかな推移が得られ、非連続な走行軌跡となることが防止できる。例えば、予測位置は、自律航法測位位置の履歴を平滑化する平滑化曲線を延長して予測する。平滑化することで、適切な予測位置を予測することができる。
【００１１】
また、予測位置の予測に使用した過去の前記自律航法測位位置の分散を算出することで、測位位置又は自律航法測位位置のいずれか、及び、予測位置をカルマンフィルタによりカップリングすることができる。すなわち、最も確率の高い位置を推定できる。
【発明の効果】
【００１２】
より精度よく測位した位置を補正し、マップマッチングした場合でも非連続な走行軌跡となることが低減される測位装置、ナビゲーションシステムを提供することができる。
【発明を実施するための最良の形態】
【００１３】
以下、本発明を実施するための最良の形態について、図面を参照しながら説明する。
図２（ａ）は、本実施の形態の測位装置９を適用したナビゲーションシステム１０の概略構成図を示す。ナビゲーションシステム１０は、ナビゲーションシステムを制御するナビＥＣＵ（Ｅｌｅｃｔｒｉｃａｌ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）５により制御され、ナビＥＣＵ５にはＧＰＳ（Ｇｒｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波を受信するＧＰＳ受信装置１、車両の速度を検出する車速センサ２、車両の向きを検出するジャイロセンサ３、道路地図データを記憶した地図データベース４、道路地図に現在位置を表示する液晶やＨＵＤ（Ｈｅａｄ Ｕｐ Ｄｉｓｐｌａｙ）等の表示装置６及びナビゲーションシステム１０を操作するための入力装置７が接続されている。
【００１４】
ナビＥＣＵ５は、プログラムを実行するＣＰＵ、プログラムを記憶したＲＯＭ、データやプログラムを一時的に記憶するＲＡＭ、データを入力及び出力する入出力装置、ＮＶ（Ｎｏｎ Ｖｏｌａｔｉｌｅ）−ＲＡＭ等がバスを介して接続されたコンピュータとして構成される。
【００１５】
ナビＥＣＵ５のＣＰＵがプログラムを実行することで、後述するように自車両の位置を検出する位置検出手段を実現する。
【００１６】
ナビＥＣＵ５の位置検出手段は、ＧＰＳ衛星からの電波に基づき自車両の位置を測位して、さらに自律航法等により自車両の位置を推定する。本実施の形態の位置検出手段は、自律航法により検出した自車両の位置から走行軌跡を取得し、自律航法により算出した自車位置ｚと走行軌跡から予測した予測位置ｘとをカルマンフィルタによりカップリングして確率的に最適な位置ｙを推定する。
【００１７】
位置検出手段は、所定の軌道を周回する複数のＧＰＳ衛星のうち現在の車両の位置から所定の仰角に入るＧＰＳ衛星を４つ以上選択し、それらのＧＰＳ衛星から発信される電波を受信する。なお、ＧＰＳ衛星からの電波を位置の知られた地上の基地局が受信してもよい。
【００１８】
位置検出手段は電波の到達時間を計算し、到達時間と光速ｃから捕捉したＧＰＳ衛星までの距離を算出する。自車両の位置は、緯度・経度・標高で特定される３次元上の１点であると考えられるので、３つのＧＰＳ衛星と自車両の距離を算出し、各ＧＰＳ衛星との距離を半径とする球面が交差する点を自車両の位置として測位する。
【００１９】
また、位置検出手段は、車速センサ２及びジャイロセンサ３を用いて自律航法により自車両の位置を推定することができる。主にＧＰＳ衛星を捕捉できない場合、位置検出手段は、ＧＰＳ衛星からの電波により測位された位置に、車速センサ２による走行距離及びジャイロセンサ３による走行方向を累積しながら自律航法により車両の現在位置を精度よく推定する。
【００２０】
なお、推定した自車両の位置を道路地図に対応づけるマップマッチング法により、地図上に表示する自車両の位置をさらに補正してもよい。この場合、マップマッチング後の自車位置が自車位置ｚとなる。
【００２１】
地図データベース４は、ハードディスク、ＤＶＤ（Digital Versatile Disk）、フラッシュメモリ等の記録媒体により構成される。地図データベース４に記憶された道路地図データには、道路網や交差点などの道路地図情報が、緯度・経度に対応づけて格納されている。道路地図は、実際の道路網をノード(道路と道路が交差する点、例えば交差点)及びリンク(ノードとノードを接続する道路)に対応づけて、テーブル状のデータベースとして構成される。
【００２２】
入力装置２０は、押下式のキーボード、ボタン、リモコン、十字キー、タッチパネル等で構成され、運転者からの操作を入力するためのインターフェイスである。また、マイクを備え運転者の発する音声を音声認識回路で認識して操作を入力してもよい。目的地までのルート検索を行う場合、運転者は目的地を住所、地名、ランドマーク名、郵便番号等で入力することができる。
【００２３】
本実施の形態で用いるカルマンフィルタについて説明する。カルマンフィルタはある系の状態（ここでは自車両の位置）を推定するためのフィルタリング理論である。例えば、ｘ（真）を真の位置、現在の時刻ｔ
において推定される位置をｙ とすると、過去から現在に至るまでのすべての観測値ｚの線形結合で現在の最適推定値ｙを推定する。この推定方法は、時刻k における真の値ｘ（真） と最適推定値ｙ の差の二乗の総和を最小にすることと等価であるが、カルマンフィルタを用いると現在の最適推定値ｙは一つ前の最適推定値に既知の行列をかけ、現在の観測値ｚで決まる補正項を加えることにより求まることになる。
【００２４】
図３（ａ）はカルマンフィルタにより最適推定値ｙを推定するための概念図を示す。なお、図３（ａ）ではｘ、ｚをスカラ量として示したが、本実施の形態ではＸ、Ｚはベクトル量である。
【００２５】
ここで、ＺとＸの間には次の関係があるものとする。
Ｚ＝ＨＸ＋Ｖ …（１）
但し、Ｈは既知の観測行列である。また、Ｖは白色ガウス雑音の測定誤差を表すベクトルであり、その共分散をＲとする。また、位置Ｘは統計適性質が明らかであり好ましくは正規分布で与えられ、図３（ａ）では平均値を＜ｘ＞、共分散をＭとした。
【００２６】
２つの不規則な事象ＸとＺが互いに従属である場合、ベイズの公式から事象Ｚが起こった時に事象Ｘが起こる確率はｐ（Ｘ／Ｚ）＝ｐ（Ｚ／Ｘ）・ｐ（Ｘ）／ｐ（Ｚ）と表すことができる。
【００２７】
この式から、例えば、ｐ（ｘ／ｚ３）はｐ（ｚ３／ｘ）とｐ（ｘ）の積に比例するため、図３（ａ）では＜ｘ＞とｘ３の間の一点で最大になる。このｘがｐ（ｘ／ｚ３）を最大にするｘの最適推定値ｙである。なお、解析的にｙの分散はＡ＝（Ｍ−１＋ＨｔＲ−１Ｈ）−１になる。
【００２８】
カルマンフィルタでは、位置Ｘの最適推定値Ｙが次式により求められる。なお、添字のｉは観測値Ｚを観測した番号、ｔは転置行列、−１は逆行列である。
Ｙ（ｉ）＝Ｘ（ｉ−１）＋Ｋ（ｉ）・｛Ｚ（ｉ）−Ｈ（ｉ）・Ｘ（ｉ−１）｝ …（２）
Ｋ（ｉ）はカルマンゲイン行列であり、次のように表せる。
Ｋ（ｉ）＝Ａ（ｉ）・ＨｉｔＲｉ−１ …（３）
ただし、Ａ（ｉ）＝（Ｍ−１＋ＨｔＲ−１Ｈ）−１
＝Ｍ（ｉ）−Ｍ（ｉ）・Ｈ（ｉ）ｔ｛Ｈ（ｉ）Ｍ（ｉ）Ｈ（ｉ）ｔ＋Ｒ（ｉ）｝−１Ｈ（ｉ）Ｍ（ｉ） …（３）
である。
【００２９】
図２（ｂ）に本実施の形態においてカルマンフィルタにより最適推定値Ｙを推定するための構成を示した。まず、上述したように位置検出手段は、ＧＰＳ受信装置１、車速センサ２、ジャイロセンサ３の少なくともいずれかにより自車両の観測値Ｚ（以下、測位位置Zという）を演算する。なお、マップマッチングにより道路地図に対応づけた後の値を測位位置Ｚとしてもよい。そして、過去の側位位置Ｚを使用して修正目標値Ｘを演算し、測位位置Ｚ及び修正目標値Ｘを用いてカルマンフィルタにより最適推定値Ｙを推定する。
【００３０】
観測行列Ｈは、式（１）や図３（ａ）に示すように観測した測位位置Ｚにより目標推定値Ｘを推定するものであるので、後述する平滑化方法や修正目標値Ｘの予測方法に応じて定められる既知の行列である。例えば、４つ以上のＧＰＳ衛星を捕捉できる状態で走行した際の目標推定誤差Ｘ及び測位位置Ｚから、ＧＰＳにより測位した位置が推定されるようにシミュレーションにより求めてもよい。
【００３１】
以上の構成により、最適推定値Ｙを推定する処理の手順を図４のフローチャート図に基づき説明する。
【００３２】
〔ステップＳ１〕
まず、位置検出手段は、時刻ｔの自車両の測位位置Ｚを測位する。測位位置Ｚは少なくとも移動平面上の緯度及び経度を示すベクトルであり、より好ましくは標高を含んでもよい。ＧＰＳ受信装置１による測位誤差は例えば捕捉した衛星数に応じて既知であり、車速センサ２及びジャイロセンサ３の誤差は速度等に応じて既知である。したがって、これらに基づき演算した測位位置Ｚの誤差の共分散Ｒも既知である。図２（ｂ）に示すように、カルマンフィルタ（式（２））にはこの測位位置Ｚ及び共分散Ｒが入力される。
【００３３】
〔ステップＳ２〕
また、位置検出手段は測位位置ＺをＲＡＭ等に記憶し、測位位置Ｚを履歴を得る。図５（ａ）は移動平面において測位位置Ｚの履歴を示す図である。図５（ａ）ではｚ（ｔ−１）までが演算された測位位置Ｚである。
【００３４】
〔ステップＳ３〕
位置検出手段は、時刻ｔ−１から過去の数ステップ分の走行履歴を平滑化処理して走行軌跡を取得する。平滑化処理にはどのような手法を用いてもよい。例えば、直線の道路を走行している場合には１次のスプライン平滑化を、右左折したように大きく走行軌跡が変化している場合には２次又は３次以上のスプライン平滑化を行う。なお、平滑化処理は、加重平均、核型平滑化や局所回帰平滑化等の算出方法を用いてもよい。位置検出手段はこのように測位の度に走行履歴を平滑化する。なお、平滑化の目的は、時刻ｔにおける自車両の位置を予測することであるので、直近の数秒〜数十秒又は所定数の測位位置Zのみを平滑化処理すればよい。
【００３５】
〔ステップＳ４〕
ついで、位置検出手段は平滑化処理により得られた走行軌跡から時刻ｔにおける自車両の位置（修正目標値Ｘ）を予測する。スプライン平滑化した場合、時刻ｔまで平滑化曲線を延長することで時刻ｔにおける自車両の位置を予測できる。ここで予測した位置が修正目標値Ｘとなる。なお、平滑化曲線の延長が困難な場合、走行軌跡を取得した複数の測位位置Ｚから線形予測してもよい。目標推定値Ｘは測位位置Ｚと同様に移動平面上の緯度及び経度を示すベクトルであり、より好ましくは標高を含んでもよい。
【００３６】
この修正目標値Ｘは、走行軌跡を取得し更に時刻ｔにおける自車位置を予測したものであるため誤差を含む。そこで、位置検出手段は平滑化処理を行った測位位置Ｚに基づき分散を求め、この分散を目標推定値Ｘの共分散Ｍとする。
【００３７】
図５（ｂ）は修正目標値Ｘ及びその分散のイメージを示す図である。点線で示した範囲が修正目標値Ｘの分散となる。なお、図５（ｂ）では測位結果Ｚを平滑化曲線に重ねて示した。分散の算出は、例えば、測位位置Ｚの平均と各測位位置Ｚ（ｉ）との差の２乗和を標本数で割るなど、一般的な方法で算出する。図２（ｂ）に示すように、カルマンフィルタ（式（２））にはこの修正目標値Ｘ及び共分散Ｍが入力される。
【００３８】
〔ステップＳ５〕
位置検出手段は、測位位置Ｚ及び共分散Ｒ、並びに、修正目標値Ｘ及び共分散Ｍにより式（２）を用いて最適推定値Ｙを演算する。図３（ｂ）にはカルマンフィルタにより演算される最適推定値Ｙのイメージを示した。図３（ｂ）に示すように、測位位置Ｚの分布と修正目標値Ｘの分布から、確率的に最も高い自車両の位置、最適推定値Ｙが演算される。
【００３９】
図６は本実施の形態の測位装置９を用いて一般道路を走行しながら演算した最適推定値Ｙの履歴を示す。図６では比較のため従来のナビゲーションシステム（以下、単に従来品という）による測位結果を重ねてプロットしている。従来品による測位結果はマップマッチングにより道路地図に対応づけたものであり、最適推定値Ｙの履歴はマップマッチングは行っていない。
【００４０】
図６に示すように、従来品による測位結果ではｓ１〜ｓ３において測位結果に非連続な測位点が生じる。これは、マップマッチングした際に実際に走行している道路とは異なる道路に対応づけたものである。これに対し、本実施の形態の測位装置９では、非連続な測位はなく、走行履歴は滑らかに推移している。したがって、マップマッチングした場合でも測位結果に非連続な箇所が生じることがない。
【００４１】
以上のように、本実施の形態の測位装置９によれば、測位位置Ｚを修正するために、過去の測位結果の走行軌跡を平滑化し修正目標値Ｘを使用することから、ＧＰＳによる測位の誤差が大きい場合やＧＰＳ衛星を捕捉できない場合でも精度よく自車両の位置を推定することができる。測位位置Ｚと修正目標値Ｘをカルマンフィルタに入力して最も確度の高い位置を推定するので、精度よく位置を検出できる。
【００４２】
このため、非連続な測位結果を生じることも大幅に減少される。また、非連続な測位結果が生じないので、マップマッチングした場合にも走行している道路（リンク）が切り替わることがないので、運転者が戸惑うことが防止できる。
【図面の簡単な説明】
【００４３】
【図１】マップマッチングの様子を示す図である。
【図２】測位装置を適用したナビゲーションシステムの概略構成図である。
【図３】カルマンフィルタにより位置を推定する概念図である。
【図４】最適推定値を推定する処理の手順を示すフローチャート図である。
【図５】移動平面において測位位置の履歴及び平滑化した曲線を示す図である。
【図６】一般道路を走行しながら演算した最適推定値Ｙの履歴を示す図である。
【符号の説明】
【００４４】
１ ＧＰＳ受信装置
２ 車速センサ
３ ジャイロセンサ
４ 地図データベース
５ ナビＥＣＵ
６ 表示装置
７ 入力装置
１０ ナビゲーションシステム
【特許請求の範囲】
【請求項１】
ＧＰＳ等の電波航法測位手段により移動体の位置を検出する測位装置において、
前記移動体の挙動情報を検出する自律センサと、
前記電波航法測位手段による測位位置に前記自律センサによる検出情報を累積して自律航法測位位置を検出する位置検出手段と、を有し、
前記位置検出手段は、前記測位位置又は前記自律航法測位位置のいずれか、及び、過去の前記自律航法測位位置の履歴に基づき予測した予測位置に基づき、前記移動体の前記位置を推定する、
ことを特徴とする測位装置。
【請求項２】
前記位置検出手段は、過去の前記自律航法測位位置の履歴を平滑化する平滑化曲線を延長して前記予測位置を予測する、
ことを特徴とする請求項１記載の測位装置。
【請求項３】
前記位置検出手段は、前記自律航法測位位置、前記自律航法測位位置の誤差分散、前記予測位置、及び、前記予測位置の予測に使用した過去の前記自律航法測位位置の分散をカルマンフィルタに入力して前記位置を推定する、
ことを特徴とする請求項１又は２記載の測位装置。
【請求項４】
請求項１ないし３いずれか記載の測位装置を適用したナビゲーションシステム。
【請求項１】
ＧＰＳ等の電波航法測位手段により移動体の位置を検出する測位装置において、
前記移動体の挙動情報を検出する自律センサと、
前記電波航法測位手段による測位位置に前記自律センサによる検出情報を累積して自律航法測位位置を検出する位置検出手段と、を有し、
前記位置検出手段は、前記測位位置又は前記自律航法測位位置のいずれか、及び、過去の前記自律航法測位位置の履歴に基づき予測した予測位置に基づき、前記移動体の前記位置を推定する、
ことを特徴とする測位装置。
【請求項２】
前記位置検出手段は、過去の前記自律航法測位位置の履歴を平滑化する平滑化曲線を延長して前記予測位置を予測する、
ことを特徴とする請求項１記載の測位装置。
【請求項３】
前記位置検出手段は、前記自律航法測位位置、前記自律航法測位位置の誤差分散、前記予測位置、及び、前記予測位置の予測に使用した過去の前記自律航法測位位置の分散をカルマンフィルタに入力して前記位置を推定する、
ことを特徴とする請求項１又は２記載の測位装置。
【請求項４】
請求項１ないし３いずれか記載の測位装置を適用したナビゲーションシステム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２００７−３３３６５２（Ｐ２００７−３３３６５２Ａ）
【公開日】平成１９年１２月２７日（２００７．１２．２７）
【国際特許分類】
【出願番号】特願２００６−１６７９１２（Ｐ２００６−１６７９１２）
【出願日】平成１８年６月１６日（２００６．６．１６）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成１９年１２月２７日（２００７．１２．２７）
【国際特許分類】
【出願日】平成１８年６月１６日（２００６．６．１６）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]