JP5831308B2 - 移動体位置検出システム、移動体位置検出装置、移動体位置検出方法及びコンピュータプログラム - Google Patents

Description

本発明は、移動体の位置を検出する移動体位置検出システム、移動体位置検出装置、移動体位置検出方法及びコンピュータプログラムに関する。

近年、車両の走行案内を行い、運転者が所望の目的地に容易に到着できるようにしたナビゲーション装置が車両に搭載されていることが多い。ここで、ナビゲーション装置とは、ＧＰＳ受信機などにより自車の現在位置を検出し、その現在位置に対応する地図データをＤＶＤ−ＲＯＭやＨＤＤなどの記録媒体またはネットワークを通じて取得して液晶モニタに表示することが可能な装置である。更に、かかるナビゲーション装置には、所望する目的地を入力すると、出発地から目的地までの最適経路を探索する経路探索機能を備えている。そして、探索結果に基づいて設定された案内経路をディスプレイ画面に表示するとともに、右左折等の案内の対象となる分岐点（以下、案内分岐点という）に接近した場合には音声やディスプレイ画面を用いた案内を行うことによって、ユーザを所望の目的地まで確実に案内するようになっている。また、近年は携帯電話機、スマートフォン、ＰＤＡ（Personal Digital Assistant）、パーソナルコンピュータ等においても上記ナビゲーション装置と同様の機能を有するものがある。更に、車両以外にも歩行者や二輪車を対象として上記案内を行うことも可能である。

ここで、案内分岐点において右左折等の案内を行う場合には、案内を適切なタイミングで行うことが重要である。そして、案内を適切なタイミングで行う為には、車両等の現在位置を正確に検出することが必要となる。また、案内分岐点の案内以外に、車両が分岐点に接近した場合に行う減速制御等の車両制御を行う際においても、車両の現在位置を正確に検出することは重要である。

そして、車両等の現在位置を検出する手段の一つとして、車両等に搭載された距離センサや角度センサから得られた情報（自立航法情報）を用いて相対的に車両の現在位置を検出する自立航法が従来より知られている。しかし、自立航法のように自立航法情報から車両の位置を相対的に特定する方法では、カーブ道路やロータリー交差点等の特殊な形状の道路を走行する場合において、車両等の現在位置を正確に検出することが難しい。そこで、例えば特許第４６５１５１１号公報では、車両がロータリー交差点を走行する場合において、ロータリー交差点の基準点からの回転角度量変化を検出することによって、車両の現在位置を正確に検出する技術について提案されている。

特許第４６５１５１１号公報（第７頁−第９頁、図１）

しかしながら、上記特許文献１の記載の技術では、以下に説明する状況等において依然として車両の現在位置が正確に検出できない場合があった。

ここで、自立航法では、検出される車両等の位置が道路上から外れた位置とならないように、検出される車両等の現在位置を道路上に特定する。即ち、車両等が地図情報に含まれるリンクに沿って移動すると推定して車両等の現在位置を検出する。しかしながら、車両等が実際に走行する走行軌跡と地図情報に含まれるリンクの形状は必ずしも一致しないので、実際の車両等の位置と検出位置との間にズレが生じる場合が有った。

例えば、図７に示すようにロータリー交差点１０１を車両１０２が走行する場合には、ロータリー交差点１０１を構成する各リンク１０３と車両１０２の走行軌跡１０４とは完全に一致しない。特に、車両１０２がロータリー交差点１０１に進入及び退出する際の走行軌跡１０４は、所定角度で旋回する走行軌跡を描くので、対象となるリンク１０３の長さよりも短くなる。上述したように自立航法では、リンクに沿って車両１０２が走行したと推定して車両位置を検出するので、車両１０２がロータリー交差点１０１から退出した場合に、自立航法により検出される車両１０２の位置Ｘは、実際の位置Ｙよりも後方に位置する。即ち、実際の車両１０２の位置と検出位置との間にズレが生じる。

本発明は前記従来における問題点を解消するためになされたものであり、自立航法情報に基づいて移動体の位置を検出する場合において、移動体の位置を正確に検出することを可能にした移動体位置検出システム、移動体位置検出装置、移動体位置検出方法及びコンピュータプログラムを提供することを目的とする。

前記目的を達成するため本願の請求項１に係る移動体位置検出システム（１）は、移動体の方位を含む自立航法情報を取得する自立航法情報取得手段（１３）と、前記移動体の自立航法情報に基づいて、前記移動体の位置を特定する位置特定手段（１３）と、前記移動体の自立航法情報に基づいて、前記位置特定手段によって特定された前記移動体の位置から所定間隔経過後に前記移動体が位置する予測位置を予測する位置予測手段（１３）と、前記位置特定手段によって特定された前記移動体の位置から前記予測位置への方位と前記移動体の方位との差分である差分方位角を算出する方位角算出手段（１３）と、前記差分方位角が閾値以上である場合に、前記差分方位角を補正する方位角補正手段（１３）と、前記差分方位角に基づいて、前記位置特定手段により特定される前記移動体の位置を更新する位置更新手段（１３）と、を有することを特徴とする。
尚、「移動体」としては、車両以外に、歩行者や二輪車も含む。

また、請求項２に係る移動体位置検出システム（１）は、請求項１に記載の移動体位置検出システムであって、前記方位角補正手段（１３）は、前記位置特定手段によって特定された前記移動体の位置を前記移動体の進行方向後方へと移動した第１補正位置（６３）を特定する第１補正位置特定手段（１３）と、前記予測位置を前記移動体の進行方向前方へと移動した第２補正位置（６４）を特定する第２補正位置特定手段（１３）と、を備え、前記差分方位角を、前記第１補正位置から前記第２補正位置への方位と前記移動体の方位との差分に補正することを特徴とする。

また、請求項３に係る移動体位置検出システム（１）は、請求項１又は請求項２に記載の移動体位置検出システムであって、前記位置予測手段（１３）は、前記予測位置を複数箇所予測し、前記位置更新手段（１３）は、前記差分方位角に基づいて複数の前記予測位置毎にコストを算出するコスト算出手段（１３）を備え、複数の前記予測位置の内、最もコストの低い前記予測位置へと前記移動体の位置を更新することを特徴とする。

また、請求項４に係る移動体位置検出システム（１）は、請求項３に記載の移動体位置検出システムであって、前記コスト算出手段（１３）は、前記差分方位角が小さい程、コストを低く算出することを特徴とする。

また、請求項５に係る移動体位置検出システム（１）は、請求項１乃至請求項４のいずれかに記載の移動体位置検出システムであって、前記方位角補正手段（１３）は、前記移動体がロータリー交差点内を移動する場合、前記移動体がロータリー交差点へと進入する場合又は前記移動体がロータリー交差点から退出する場合に、前記差分方位角を補正することを特徴とする。

また、請求項６に係る移動体位置検出装置（１）は、移動体の方位を含む自立航法情報を取得する自立航法情報取得手段（１３）と、前記移動体の自立航法情報に基づいて、前記移動体の位置を特定する位置特定手段（１３）と、前記移動体の自立航法情報に基づいて、前記位置特定手段によって特定された前記移動体の位置から所定間隔経過後に前記移動体が位置する予測位置を予測する位置予測手段（１３）と、前記位置特定手段によって特定された前記移動体の位置から前記予測位置への方位と前記移動体の方位との差分である差分方位角を算出する方位角算出手段（１３）と、前記差分方位角が閾値以上である場合に、前記差分方位角を補正する方位角補正手段（１３）と、前記差分方位角に基づいて、前記位置特定手段により特定される前記移動体の位置を更新する位置更新手段（１３）と、を有することを特徴とする。

また、請求項７に係る移動体位置検出方法は、自立航法情報取得手段が、移動体の方位を含む自立航法情報を取得するステップと、位置特定手段が、前記移動体の自立航法情報に基づいて、前記移動体の位置を特定するステップと、位置予測手段が、前記移動体の自立航法情報に基づいて、前記位置特定手段によって特定された前記移動体の位置から所定間隔経過後に前記移動体が位置する予測位置を予測するステップと、方位角算出手段が、前記位置特定手段によって特定された前記移動体の位置から前記予測位置への方位と前記移動体の方位との差分である差分方位角を算出するステップと、方位角補正手段が、前記差分方位角が閾値以上である場合に、前記差分方位角を補正するステップと、位置更新手段が、前記差分方位角に基づいて、前記位置特定手段により特定される前記移動体の位置を更新するステップと、を有することを特徴とする。

更に、請求項８に係るコンピュータプログラムは、コンピュータを、移動体の方位を含む自立航法情報を取得する自立航法情報取得手段と、前記移動体の自立航法情報に基づいて、前記移動体の位置を特定する位置特定手段と、前記移動体の自立航法情報に基づいて、前記位置特定手段によって特定された前記移動体の位置から所定間隔経過後に前記移動体が位置する予測位置を予測する位置予測手段と、前記位置特定手段によって特定された前記移動体の位置から前記予測位置への方位と前記移動体の方位との差分である差分方位角を算出する方位角算出手段と、前記差分方位角が閾値以上である場合に、前記差分方位角を補正する方位角補正手段と、前記差分方位角に基づいて、前記位置特定手段により特定される前記移動体の位置を更新する位置更新手段と、して機能させることを特徴とする。

前記構成を有する請求項１に記載の移動体位置検出システムによれば、自立航法情報に基づいて取得された移動体の方位と移動体位置の変位方向との差分である差分方位角が閾値以上である場合に、差分方位角を修正し、修正した差分方位角に基づいて移動体の検出位置の更新を行うので、自立航法情報に基づいて移動体の位置を検出する場合において、移動体の位置をより正確に検出することが可能となる。特に、カーブ道路やロータリー交差点等のリンク方位が大きく変化する形状の道路を移動体が移動する場合において、移動体が実際に移動する軌跡と地図情報に含まれるリンクの形状が一致しない場合であっても、実際の移動体の位置と検出位置との間にズレが生じることを防止することが可能となる。

また、請求項２に記載の移動体位置検出システムによれば、差分方位角の補正は、移動体位置の変位方向を算出する際の基準となる移動体の位置を前後方向に広げることにより行うので、移動体位置の変位方向が実際の移動体の方位と大きく異ならないように修正することが可能となる。その結果、移動体の検出位置が実際の移動体の位置と大きく異なる位置に検出されることを防止することが可能となる。

また、請求項３に記載の移動体位置検出システムによれば、移動体の更新候補先である予測位置を複数特定し、各予測位置について差分方位角に基づいてコスト算出を行い、最もコストの小さい予測位置へと移動体の検出位置の更新を行うので、カーブ道路やロータリー交差点等のリンク方位が大きく変化する形状の道路を移動体が移動する場合においても、正確なコスト計算を行うことが可能となる。従って、実際の移動体の位置と検出位置との間にズレが生じることを防止することが可能となる。

また、請求項４に記載の移動体位置検出システムによれば、差分方位角が小さい程、コストを低く算出するので、複数の予測位置の内、移動体の方位に基づいて実際の移動体の位置と最も近いと推定される予測位置へと移動体の検出位置を更新することが可能となる。

また、請求項５に記載の移動体位置検出システムによれば、移動体がロータリー交差点内を移動する場合、移動体がロータリー交差点へと進入する場合又は移動体がロータリー交差点から退出する場合に、差分方位角を修正することによる移動体の位置の更新を行うので、特に、ロータリー交差点周辺を移動体が移動する場合において、移動体の位置を正確に検出することが可能となる。

また、請求項６に記載の移動体位置検出装置によれば、自立航法情報に基づいて取得された移動体の方位と移動体位置の変位方向との差分である差分方位角が閾値以上である場合に、差分方位角を修正し、修正した差分方位角に基づいて移動体の検出位置の更新を行うので、自立航法情報に基づいて移動体の位置を検出する場合において、移動体の位置をより正確に検出することが可能となる。特に、カーブ道路やロータリー交差点等のリンク方位が大きく変化する形状の道路を移動体が移動する場合において、移動体が実際に移動する軌跡と地図情報に含まれるリンクの形状が一致しない場合であっても、実際の移動体の位置と検出位置との間にズレが生じることを防止することが可能となる。

また、請求項７に記載の移動体位置検出方法によれば、自立航法情報に基づいて取得された移動体の方位と移動体位置の変位方向との差分である差分方位角が閾値以上である場合に、差分方位角を修正し、修正した差分方位角に基づいて移動体の検出位置の更新を行うので、自立航法情報に基づいて移動体の位置を検出する場合において、移動体の位置をより正確に検出することが可能となる。特に、カーブ道路やロータリー交差点等のリンク方位が大きく変化する形状の道路を移動体が移動する場合において、移動体が実際に移動する軌跡と地図情報に含まれるリンクの形状が一致しない場合であっても、実際の移動体の位置と検出位置との間にズレが生じることを防止することが可能となる。

更に、請求項８に記載のコンピュータプログラムによれば、自立航法情報に基づいて取得された移動体の方位と移動体位置の変位方向との差分である差分方位角が閾値以上である場合に、差分方位角を修正させ、修正した差分方位角に基づいて移動体の検出位置の更新を行わせるので、自立航法情報に基づいて移動体の位置を検出する場合において、移動体の位置をより正確に検出することが可能となる。特に、カーブ道路やロータリー交差点等のリンク方位が大きく変化する形状の道路を移動体が移動する場合において、移動体が実際に移動する軌跡と地図情報に含まれるリンクの形状が一致しない場合であっても、実際の移動体の位置と検出位置との間にズレが生じることを防止することが可能となる。

本実施形態に係るナビゲーション装置を示したブロック図である。 本実施形態に係る車両位置検出処理プログラムのフローチャートである。 車両がロータリー交差点内を走行する状態、車両がロータリー交差点へと進入する状態及び車両がロータリー交差点から退出する状態をそれぞれ示した図である。 補正目標位置の設定方法について説明した図である。 補正目標位置に基づいて行われる現在位置候補の更新処理について説明した図である。 補正目標位置に基づいて行われる現在位置候補の更新処理について説明した図である。 従来技術の問題点について説明した図である。

以下、本発明に係る移動体位置検出システム及び移動体位置検出装置をナビゲーション装置に具体化した一実施形態に基づき図面を参照しつつ詳細に説明する。先ず、本実施形態に係るナビゲーション装置１の概略構成について図１を用いて説明する。図１は本実施形態に係るナビゲーション装置１を示したブロック図である。

図１に示すように本実施形態に係るナビゲーション装置１は、ナビゲーション装置１が搭載された車両の現在位置を検出する現在位置検出部１１と、各種のデータが記録されたデータ記録部１２と、入力された情報に基づいて、各種の演算処理を行うナビゲーションＥＣＵ１３と、ユーザからの操作を受け付ける操作部１４と、ユーザに対して車両周辺の地図や施設の関する施設情報を表示する液晶ディスプレイ１５と、経路案内に関する音声ガイダンスを出力するスピーカ１６と、記憶媒体であるＤＶＤを読み取るＤＶＤドライブ１７と、プローブセンタやＶＩＣＳ（登録商標：Vehicle Information and Communication System）センタ等の情報センタとの間で通信を行う通信モジュール１８と、から構成されている。

以下に、ナビゲーション装置１を構成する各構成要素について順に説明する。
現在位置検出部１１は、ＧＰＳ２１、車速センサ２２、ステアリングセンサ２３、ジャイロセンサ２４等からなり、現在の車両の位置、方位、車両の走行速度、現在時刻等を検出することが可能となっている。ここで、特に車速センサ２２は、車両の移動距離や車速を検出する為のセンサであり、車両の駆動輪の回転に応じてパルスを発生させ、パルス信号をナビゲーションＥＣＵ１３に出力する。そして、ナビゲーションＥＣＵ１３は発生するパルスを計数することにより駆動輪の回転速度や移動距離を算出する。更に、ナビゲーションＥＣＵ１３は、車速センサ２２とジャイロセンサ２４とＧＰＳ２１と協働し、自立航法情報（車速センサ２２やジャイロセンサ２４の出力信号であり、即ち車両の走行距離情報や方位情報）に基づいて生成される自立航法軌跡を高精度に生成する（即ち、車両の現在位置を検出する）。尚、上記４種類のセンサをナビゲーション装置１が全て備える必要はなく、これらの内の１又は複数種類のセンサのみをナビゲーション装置１が備える構成としても良い。

また、データ記録部１２は、外部記憶装置及び記録媒体としてのハードディスク（図示せず）と、ハードディスクに記録された地図情報ＤＢ３１や所定のプログラム等を読み出すとともにハードディスクに所定のデータを書き込む為のドライバである記録ヘッド（図示せず）とを備えている。尚、データ記録部１２をハードディスクの代わりにメモリーカードやＣＤやＤＶＤ等の光ディスクにより構成しても良い。

ここで、地図情報ＤＢ３１は、例えば、道路（リンク）に関するリンクデータ３２、ノード点に関するノードデータ３３、各分岐点に関する分岐点データ、施設に関する施設データ、地図を表示するための地図表示データ、経路を探索するための探索データ、地点を検索するための検索データ等が記憶された記憶手段である。

ここで、リンクデータ３２としては、例えば、該リンクを識別するリンクＩＤ、該リンクの端部に位置するノードを特定する端部ノード情報、該リンクを構成する道路の道路種別（リンク種別）、該リンクを構成する道路の道路属性（リンク属性）、リンク方位、ノード間のリンク形状（例えばカーブ道路ではカーブの形状）を特定する為の形状補完点の位置座標等が記憶される。尚、リンク種別としては、一般道、有料道路、高速道路等の道路種別を特定する情報が含まれる。また、リンク属性としては、ロータリー交差点を構成する道路か否かを特定する情報、車線数、一方通行か双方向通行かを特定する情報等が含まれる。
また、ノードデータ３３としては、該ノードを識別するノードＩＤ、該ノードの位置座標、該ノードがリンクを介して接続される接続先ノードを特定する接続先ノード情報等が記憶される。

一方、ナビゲーションＥＣＵ（エレクトロニック・コントロール・ユニット）１３は、ナビゲーション装置１の全体の制御を行う電子制御ユニットであり、演算装置及び制御装置としてのＣＰＵ４１、並びにＣＰＵ４１が各種の演算処理を行うにあたってワーキングメモリとして使用されるとともに、経路が探索されたときの経路データ等が記憶されるＲＡＭ４２、制御用のプログラムのほか、後述の車両位置検出処理プログラム（図２参照）等が記録されたＲＯＭ４３、ＲＯＭ４３から読み出したプログラムを記憶するフラッシュメモリ４４等の内部記憶装置を備えている。尚、ナビゲーションＥＣＵ１３は、処理アルゴリズムとしての各種手段を構成する。例えば、自立航法情報取得手段は、車両（移動体）の方位を含む自立航法情報を取得する。位置特定手段は、車両の自立航法情報に基づいて、車両の位置を特定する。位置予測手段は、車両の自立航法情報に基づいて、位置特定手段によって特定された車両の位置から所定間隔経過後に車両が位置する予測位置を予測する。方位角算出手段は、位置特定手段によって特定された車両の位置から予測位置への方位と車両の方位との差分である差分方位角を算出する。方位角補正手段は、差分方位角が閾値以上である場合に、差分方位角を補正する。位置更新手段は、差分方位角に基づいて、位置特定手段により特定される車両の位置を更新する。第１補正位置特定手段は、位置特定手段によって特定された車両の位置を車両の進行方向後方へと移動した第１補正位置を特定する。第２補正位置特定手段は、予測位置を車両の進行方向前方へと移動した第２補正位置を特定する。コスト算出手段は、差分方位角に基づいて複数の予測位置毎にコストを算出する。

操作部１４は、走行開始地点としての出発地及び走行終了地点としての目的地を入力する際等に操作され、各種のキー、ボタン等の複数の操作スイッチ（図示せず）から構成される。そして、ナビゲーションＥＣＵ１３は、各スイッチの押下等により出力されるスイッチ信号に基づき、対応する各種の動作を実行すべく制御を行う。尚、操作部１４は液晶ディスプレイ１５の前面に設けたタッチパネルによって構成することもできる。また、マイクと音声認識装置によって構成することもできる。

また、液晶ディスプレイ１５には、道路を含む地図画像、交通情報、操作案内、操作メニュー、キーの案内、出発地から目的地までの案内経路、案内経路に沿った案内情報、ニュース、天気予報、時刻、メール、テレビ番組等が表示される。また、地図画像を表示する場合には、後述の車両位置検出処理プログラム（図２参照）によって特定された車両の現在位置を示す現在位置マークを表示する。

また、スピーカ１６は、ナビゲーションＥＣＵ１３からの指示に基づいて案内経路に沿った走行を案内する音声ガイダンスや、交通情報の案内を出力する。

また、ＤＶＤドライブ１７は、ＤＶＤやＣＤ等の記録媒体に記録されたデータを読み取り可能なドライブである。そして、読み取ったデータに基づいて音楽や映像の再生、地図情報ＤＢ３１の更新等が行われる。

また、通信モジュール１８は、交通情報センタ、例えば、ＶＩＣＳ（登録商標）センタやプローブセンタ等から送信された渋滞情報、規制情報、交通事故情報等の各情報から成る交通情報を受信する為の通信装置であり、例えば携帯電話機やＤＣＭが該当する。

続いて、前記構成を有するナビゲーション装置１においてナビゲーションＥＣＵ１３が実行する車両位置検出処理プログラムについて図２に基づき説明する。図２は本実施形態に係る車両位置検出処理プログラムのフローチャートである。ここで、車両位置検出処理プログラムは車両のＡＣＣがＯＮされた後に所定間隔（車両の現在位置の検出周期であり、車両が所定の検出間隔距離（例えば６ｍ）を走行する毎）で繰り返し実行され、自立航法により車両の現在位置を検出するプログラムである。尚、以下の図２にフローチャートで示されるプログラムは、ナビゲーション装置１が備えているＲＡＭ４２やＲＯＭ４３に記憶されており、ＣＰＵ４１により実行される。

先ず、車両位置検出処理プログラムではステップ（以下、Ｓと略記する）１において、ＣＰＵ４１は、車両の現在位置の候補である現在位置候補を取得する。ここで、現在位置候補は、一又は複数の候補からなる。尚、現在位置候補が複数の候補から構成される場合には、複数の候補の内、後述するようにコスト計算に基づいて最も有力な現在位置と推定される候補が最有力の現在位置候補に特定されている。一方、一の候補のみから構成される場合には、該一の候補が最有力の現在位置候補に特定されている。そして、最有力の現在位置候補に車両が位置するとして、車両の現在位置を用いる他の処理（車両の現在位置表示制御や分岐点案内制御や車両減速制御等）が実行される（即ち、最有力の現在位置候補に車両が位置すると検出される）。

以下のＳ２〜Ｓ１５の処理は、前記Ｓ１で取得された現在位置候補毎に実行され、全ての現在位置候補に対してＳ２〜Ｓ１５の処理が実行された後にＳ１６の処理へと移行する。

Ｓ２においてＣＰＵ４１は、処理対象の現在位置候補の位置を更新する。具体的には、現在の処理対象の現在位置候補の位置からリンクに沿って車両の進行方向前方に更新移動距離だけ移動させた位置に、現在位置候補の位置を更新する（即ち、所定間隔経過後に車両が位置する予測位置を予測する）。尚、更新移動距離は、基本的に当該車両位置検出処理プログラムが実行される車両の走行距離間隔である検出間隔距離（例えば６ｍ）である。

次に、Ｓ３においてＣＰＵ４１は、処理対象の現在位置候補が最有力候補であるか否か判定する。

そして、処理対象の現在位置候補が最有力候補であると判定された場合（Ｓ３：ＹＥＳ）には、Ｓ４へと移行する。それに対して、処理対象の現在位置候補が最有力候補以外の候補であると判定された場合（Ｓ３：ＮＯ）には、Ｓ１３へと移行する。

続いて、Ｓ４においてＣＰＵ４１は、車両が以下の（Ａ）〜（Ｃ）のいずれの状態にあるか否か判定する。
（Ａ）車両がロータリー交差点内を移動する状態。
（Ｂ）車両がロータリー交差点へと進入する状態。
（Ｃ）車両がロータリー交差点から退出する状態。

ここで、車両が上記（Ａ）〜（Ｃ）のいずれの状態にあるか否かについては、具体的に以下の処理により判定される。
先ず、ＣＰＵ４１は、処理対象の現在位置候補が位置するリンク（以下、現在地リンクという）を特定する。次に、現在地リンクのリンク属性をデータ記録部１２に記憶されたリンクデータ３２から取得する。そして、図３に示すように処理対象の現在位置候補５１が位置する現在地リンク５２がロータリー交差点を構成するリンクである場合には、“（Ａ）車両がロータリー交差点内を移動する状態”にあると判定される。また、現在地リンク５２がロータリー交差点を構成するリンクでなかった場合であっても、図３に示すように処理対象の現在位置候補５１の進行方向前方の所定距離以内（例えば１２ｍ以内）に、ロータリー交差点を構成するリンク５３がある場合には、“（Ｂ）車両がロータリー交差点へと進入する状態”にあると判定される。また、現在地リンク５２がロータリー交差点を構成するリンクでなかった場合であっても、図３に示すように処理対象の現在位置候補５１の進行方向後方の所定距離以内（例えば１８ｍ以内）に、ロータリー交差点を構成するリンク５４がある場合には、“（Ｃ）車両がロータリー交差点へと退出する状態”にあると判定される。

そして、車両が上記（Ａ）〜（Ｃ）のいずれの状態にあると判定された場合（Ｓ４：ＹＥＳ）には、Ｓ５へと移行する。それに対して、車両が上記（Ａ）〜（Ｃ）のいずれの状態にも無いと判定された場合（Ｓ４：ＮＯ）には、Ｓ１３へと移行する。

Ｓ５においてＣＰＵ４１は、自立航法情報として特にジャイロセンサ２４の検出信号（即ち、現在の車両の方位情報）を取得する。

次に、Ｓ６においてＣＰＵ４１は、前記Ｓ２による更新を行う前の処理対象の現在位置候補の位置（以下、更新前位置という）に対する、前記Ｓ２による更新を行った後の処理対象の現在位置候補の位置（以下、更新後位置という）の方位を車両移動方位として取得する。

続いて、Ｓ７においてＣＰＵ４１は、前記Ｓ５で取得した現在の車両方位と、前記Ｓ６で取得した車両移動方位との差分である差分方位角を算出する。

以下に、上記Ｓ７における差分方位角の算出に係る処理について図４を用いて詳細に説明する。
図４に示すように、ＣＰＵ４１は、上記Ｓ２において処理対象の現在位置候補を、更新前位置６１から当該車両位置検出処理プログラムが実行される車両の走行距離間隔である検出間隔距離（例えば６ｍ）だけ車両の進行方向前方に移動させた更新後位置６２に更新する。そして、自立航法情報として取得した車両方位θ１と更新前位置６１に対する更新後位置６２の方位である車両移動方位θ２とを比較し、｜θ１−θ２｜を差分方位角として算出する。

その後、Ｓ８においてＣＰＵ４１は、前記Ｓ７で算出した差分方位角が閾値（例えば２０度）以上であるか否か判定する。尚、閾値は適宜設定することが可能であり、ＲＡＭ４２等の記憶手段に記憶される。また、閾値は車両が走行する道路の道路種別や道路幅等によって変更しても良い。

そして、差分方位角が閾値以上であると判定された場合（Ｓ８：ＹＥＳ）には、Ｓ９へと移行する。それに対して、差分方位角が閾値未満であると判定された場合（Ｓ８：ＮＯ）にはＳ１２へと移行し、後述のように算出された差分方位角に基づいて処理対象の現在位置候補のコスト計算を行う。

Ｓ９においてＣＰＵ４１は、更新前位置を車両の進行方向後方へと所定距離（例えば１２ｍ）移動した位置である第１補正位置を特定する。また、更新後位置（予測位置）を車両の進行方向前方へと所定距離（例えば１２ｍ）移動した位置である第２補正位置を特定する。例えば、図５に示すように更新前位置６１と更新後位置６２とが位置する場合には、更新前位置６１を車両の進行方向後方へと１２ｍ移動した第１補正位置６３と、更新後位置６２を車両の進行方向前方へと１２ｍ移動した第２補正位置６４とがそれぞれ特定される。

次に、Ｓ１０においてＣＰＵ４１は、前記Ｓ６で取得された車両移動方位を第１補正位置に対する第２補正位置の方位へと修正する。

続いて、Ｓ１１においてＣＰＵ４１は、前記Ｓ５で取得した現在の車両方位と、前記Ｓ１０で修正した車両移動方位との差分である差分方位角を再度算出する。

以下に、上記Ｓ１１における差分方位角の再算出に係る処理について図６を用いて詳細に説明する。
図６に示すように、ＣＰＵ４１は、自立航法情報として取得した車両方位θ１と第１補正位置６３に対する第２補正位置６４の方位である修正後の車両移動方位θ３とを比較し、｜θ１−θ３｜を差分方位角として新たに算出（差分方位角を補正）する。そして、算出された差分方位角は、後述のように処理対象の現在位置候補のコスト計算に用いられ、基本的に差分方位角が小さいほどコスト値も小さくなる（即ち、実際の車両位置に近いと判定され、最有力の現在位置候補となる）。ここで、特に車両がロータリー分岐点の進入口や退出口等のリンク方位が大きく変化する箇所を走行する際には、車両の走行軌跡とリンク形状が大きく異なる問題がある。従って、一般的な算出方法である更新前位置に対する更新後位置の方位を車両移動方位とすると、実際の車両位置に近い現在位置候補であっても、差分方位角が大きくなる（即ち、コスト値が大きくなる）場合があり、最有力の現在位置候補でないと誤って判定される場合があった。そこで、本実施形態では上記Ｓ９〜Ｓ１１のように、更新前位置と更新後位置をそれぞれ前後に広げた第１補正位置に対する第２補正位置の方位を車両移動方位に修正することによって、実際の車両位置に近い現在位置候補の差分方位角が大きくなる（即ち、コスト値が大きくなる）ことを防止し、最有力の現在位置候補に選ばれ易くすることが可能となる。

その後、Ｓ１２においてＣＰＵ４１は、前記Ｓ１１で算出（Ｓ１１の処理が行われていない場合にはＳ７又はＳ１５で算出）された差分方位角に基づいて、処理対象の現在位置候補に対するコスト計算を行う。ここで、コスト計算は、処理対象の現在位置候補が車両の現在位置として適当か否かを判定する為の指標であるコスト値を算出するものであり、算出されたコスト値が小さい程、車両の現在位置として適当である（即ち、実際の車両位置に近い）と判定される。尚、基本的に差分方位角が小さいほど算出されるコスト値も小さくなる。そして、現在位置候補が複数ある場合には、後述するようにコスト値が最も小さい現在位置候補が最も実際の現在の車両位置に近いと推定され、最有力の現在位置候補に特定される。

一方で、Ｓ１３〜Ｓ１５では上述したＳ５〜Ｓ７と同様の処理が実行され、一般的な算出方法に従って更新前位置に対する更新後位置の方位を車両移動方位とし、差分方位角が算出される。そして、算出された差分方位角に基づいてコスト計算が行われる（Ｓ１２）。

そして、前記Ｓ１で取得された全ての現在位置候補に対して上記Ｓ２〜Ｓ１５の処理を順次行い、前記Ｓ１で取得された全ての現在位置候補に対して上記Ｓ２〜Ｓ１５の処理が実行された後にＳ１６へと移行する。

Ｓ１６においてＣＰＵ４１は、前記Ｓ１で取得された現在位置候補の内、更新後の現在位置候補に対して算出されたコスト値が最も小さい現在位置候補の更新後の位置（予測位置）を最有力の現在位置候補に特定する。そして、最有力の現在位置候補に車両が位置するとして、車両の現在位置を用いる他の処理（車両の現在位置表示制御や分岐点案内制御や車両減速制御等）が以後実行される。即ち、新たな最有力の現在位置候補に車両が位置すると検出される（以前の最有力の現在位置候補から車両の現在位置が更新されることとなる）。

尚、上述した車両位置検出処理プログラムでの自立航法による車両位置の検出は、基準位置に対する車両の相対位置を検出するものである。そして、基準位置は必要なタイミング（例えばＡＣＣをオンした時点、所定間隔経過毎等）で、ＧＰＳ２１や高精度ロケーション技術等の自立航法によらずに検出された車両の位置へと更新される。

以上詳細に説明した通り、本実施形態に係るナビゲーション装置１、ナビゲーション装置１を用いた移動体位置検出方法及びナビゲーション装置１で実行されるコンピュータプログラムによれば、車両の現在位置の候補である現在位置候補について、自立航法情報に基づいて所定間隔経過後の位置へと現在位置候補を更新し（Ｓ２）、更新を行う前の位置（更新前位置）に対する更新後の位置（更新後位置）の方位を車両移動方位として取得し（Ｓ６）、自立航法情報に基づいて取得された現在の車両方位と車両移動方位との差分である差分方位角を算出し（Ｓ７）、差分方位角が閾値以上である場合に差分方位角を補正し（Ｓ９〜Ｓ１１）、補正された差分方位角に基づいて、車両の検出位置の更新を行う（Ｓ１６）ので、自立航法情報に基づいて車両の位置を検出する場合において、車両の位置をより正確に検出することが可能となる。特に、カーブ道路やロータリー交差点等のリンク方位が大きく変化する形状の道路を車両が移動する場合において、車両が実際に移動する軌跡と地図情報に含まれるリンクの形状が一致しない場合であっても、実際の車両の位置と検出位置との間にズレが生じることを防止することが可能となる。
また、差分方位角の補正は、更新前位置と更新後位置をそれぞれ前後に広げた第１補正位置に対する第２補正位置の方位を車両移動方位に修正することにより行うので、車両の変位方向を算出する際の基準となる車両の位置を前後方向に広げることとなり、車両の変位方向が実際の車両の方位と大きく異ならないように修正することが可能となる。その結果、車両の検出位置が実際の車両の位置と大きく異なる位置に検出されることを防止することが可能となる。
また、車両の更新候補先である現在位置候補が複数あって、各現在位置候補について差分方位角に基づいてコスト算出を行い、最もコストの小さい現在位置候補の更新後の位置へと車両の検出位置の更新を行うので、カーブ道路やロータリー交差点等のリンク方位が大きく変化する形状の道路を車両が移動する場合においても、正確なコスト計算を行うことが可能となる。従って、実際の車両の位置と検出位置との間にズレが生じることを防止することが可能となる。
また、差分方位角が小さい程、コストを低く算出するので、複数の現在位置候補の内、車両の方位に基づいて実際の車両の位置と最も近いと推定される現在位置候補の更新後の位置へと、車両の検出位置を更新することが可能となる。
また、車両がロータリー交差点内を移動する場合、車両がロータリー交差点へと進入する場合又は車両がロータリー交差点から退出する場合に、差分方位角を修正することによる車両の位置の更新を行うので、特に、ロータリー交差点周辺を車両が移動する場合において、車両の位置を正確に検出することが可能となる。

尚、本発明は前記実施形態に限定されるものではなく、本発明の要旨を逸脱しない範囲内で種々の改良、変形が可能であることは勿論である。
例えば、本実施形態では上記Ｓ９〜Ｓ１１のように、更新前位置と更新後位置をそれぞれ前後に広げた第１補正位置に対する第２補正位置の方位を車両移動方位に修正することとしているが、更新前位置と更新後位置の一方のみを広げることとしても良い。即ち、第１補正位置に対する更新後位置の方位を車両移動方位に修正しても良いし、更新前位置に対する第２補正位置の方位を車両移動方位に修正しても良い。

また、本実施形態では、車両がロータリー交差点内を移動する場合、車両がロータリー交差点へと進入する場合又は車両がロータリー交差点から退出する場合に、Ｓ５〜Ｓ１２の各処理を実行することとしているが、上記条件以外でも実行する構成としても良い。例えば、所定曲率以上のカーブ形状の道路を走行する状態において実行する構成としても良い。また、本実施形態では最有力の現在位置候補に対してのみＳ５〜Ｓ１２の各処理を実行することとしているが、最有力候補以外の現在位置候補に対しても実行する構成としても良い。

また、本実施形態では、車両が所定の検出間隔距離（例えば６ｍ）を走行する毎に、車両位置検出処理プログラム（図２）を実行し、車両の検出位置の更新を行う構成としているが、車両の検出位置の更新を行うタイミングは他のタイミングとしても良い。例えば、所定時間経過する度に車両の検出位置の更新を行う構成としても良い。その場合には、所定時間間隔で車両が走行した距離を車速センサ２２によって検出し、検出された距離を検出間隔距離とする。

また、本発明はナビゲーション装置以外に、車両等の移動体の現在位置を検出する機能を有する装置に対して適用することが可能である。例えば、携帯電話機やスマートフォン等の携帯端末、パーソナルコンピュータ、携帯型音楽プレイヤ等（以下、携帯端末等という）に適用することも可能である。また、サーバと携帯端末等から構成されるシステムに対しても適用することが可能となる。その場合には、上述した車両位置検出処理プログラム（図２）の各ステップは、サーバと携帯端末等のいずれが実施する構成としても良い。また、本発明を携帯端末等に適用する場合には、車両以外の移動体、例えば、携帯端末等のユーザや２輪車等の車両位置の検出を行う場合もある。

１ ナビゲーション装置
１３ ナビゲーションＥＣＵ
４１ ＣＰＵ
４２ ＲＡＭ
４３ ＲＯＭ
６１ 更新前位置
６２ 更新後位置
６３ 第１補正位置
６４ 第２補正位置

Claims (8)

1. 移動体の方位を含む自立航法情報を取得する自立航法情報取得手段と、
   前記移動体の自立航法情報に基づいて、前記移動体の位置を特定する位置特定手段と、
   前記移動体の自立航法情報に基づいて、前記位置特定手段によって特定された前記移動体の位置から所定間隔経過後に前記移動体が位置する予測位置を予測する位置予測手段と、
   前記位置特定手段によって特定された前記移動体の位置から前記予測位置への方位と前記移動体の方位との差分である差分方位角を算出する方位角算出手段と、
   前記差分方位角が閾値以上である場合に、前記差分方位角を補正する方位角補正手段と、
   前記差分方位角に基づいて、前記位置特定手段により特定される前記移動体の位置を更新する位置更新手段と、を有することを特徴とする移動体位置検出システム。
2. 前記方位角補正手段は、
   前記位置特定手段によって特定された前記移動体の位置を前記移動体の進行方向後方へと移動した第１補正位置を特定する第１補正位置特定手段と、
   前記予測位置を前記移動体の進行方向前方へと移動した第２補正位置を特定する第２補正位置特定手段と、を備え、
   前記差分方位角を、前記第１補正位置から前記第２補正位置への方位と前記移動体の方位との差分に補正することを特徴とする請求項１に記載の移動体位置検出システム。
3. 前記位置予測手段は、前記予測位置を複数箇所予測し、
   前記位置更新手段は、
   前記差分方位角に基づいて複数の前記予測位置毎にコストを算出するコスト算出手段を備え、
   複数の前記予測位置の内、最もコストの低い前記予測位置へと前記移動体の位置を更新することを特徴とする請求項１又は請求項２に記載の移動体位置検出システム。
4. 前記コスト算出手段は、前記差分方位角が小さい程、コストを低く算出することを特徴とする請求項３に記載の移動体位置検出システム。
5. 前記方位角補正手段は、前記移動体がロータリー交差点内を移動する場合、前記移動体がロータリー交差点へと進入する場合又は前記移動体がロータリー交差点から退出する場合に、前記差分方位角を補正することを特徴とする請求項１乃至請求項４のいずれかに記載の移動体位置検出システム。
6. 移動体の方位を含む自立航法情報を取得する自立航法情報取得手段と、
   前記移動体の自立航法情報に基づいて、前記移動体の位置を特定する位置特定手段と、
   前記移動体の自立航法情報に基づいて、前記位置特定手段によって特定された前記移動体の位置から所定間隔経過後に前記移動体が位置する予測位置を予測する位置予測手段と、
   前記位置特定手段によって特定された前記移動体の位置から前記予測位置への方位と前記移動体の方位との差分である差分方位角を算出する方位角算出手段と、
   前記差分方位角が閾値以上である場合に、前記差分方位角を補正する方位角補正手段と、
   前記差分方位角に基づいて、前記位置特定手段により特定される前記移動体の位置を更新する位置更新手段と、を有することを特徴とする移動体位置検出装置。
7. 自立航法情報取得手段が、移動体の方位を含む自立航法情報を取得するステップと、
   位置特定手段が、前記移動体の自立航法情報に基づいて、前記移動体の位置を特定するステップと、
   位置予測手段が、前記移動体の自立航法情報に基づいて、前記位置特定手段によって特定された前記移動体の位置から所定間隔経過後に前記移動体が位置する予測位置を予測するステップと、
   方位角算出手段が、前記位置特定手段によって特定された前記移動体の位置から前記予測位置への方位と前記移動体の方位との差分である差分方位角を算出するステップと、
   方位角補正手段が、前記差分方位角が閾値以上である場合に、前記差分方位角を補正するステップと、
   位置更新手段が、前記差分方位角に基づいて、前記位置特定手段により特定される前記移動体の位置を更新するステップと、を有することを特徴とする移動体位置検出方法。
8. コンピュータを、
   移動体の方位を含む自立航法情報を取得する自立航法情報取得手段と、
   前記移動体の自立航法情報に基づいて、前記移動体の位置を特定する位置特定手段と、
   前記移動体の自立航法情報に基づいて、前記位置特定手段によって特定された前記移動体の位置から所定間隔経過後に前記移動体が位置する予測位置を予測する位置予測手段と、
   前記位置特定手段によって特定された前記移動体の位置から前記予測位置への方位と前記移動体の方位との差分である差分方位角を算出する方位角算出手段と、
   前記差分方位角が閾値以上である場合に、前記差分方位角を補正する方位角補正手段と、
   前記差分方位角に基づいて、前記位置特定手段により特定される前記移動体の位置を更新する位置更新手段と、
   して機能させる為のコンピュータプログラム。

Images