JP6143474B2 - 位置検出装置およびプログラム - Google Patents

Description

本発明は、位置検出装置およびプログラムに関する。

下記の特許文献１には、電波航法技術および慣性航法技術においてセンサ誤差から生じる自車位置の誤差を考慮してマップマッチングを行うことができる位置検出装置が開示されている。センサ誤差は、動的システムの位置および向きを推定するカルマンフィルタの更新過程で算出された誤差共分散行列で表される。

また、下記の特許文献２には、誤差の生成過程をランジュバン方程式でモデル化することによりシステム方程式を単純化し、有色の観測雑音をマルコフ過程でモデル化し白色雑音化することによって、カルマンフィルタの更新周期の制限の問題を解消することができるハイブリッド航法装置が開示されている。

特開２０１１−２３２４号公報 特開２００１−１７４２７５号公報

ところで、カルマンフィルタでは、システムに付随する雑音の性質はガウス分布に従うことが前提となっているが、ジャイロセンサには累積誤差があり、ＧＰＳ信号の測定誤差には自己相関がある。このように、実際に計測される情報に含まれる誤差はガウス分布ではなく有色雑音の性質を有する場合が多い。

この点において、上記特許文献１の位置検出装置は、センサ誤差を白色雑音とみなして演算しているため、算出された誤差の量が実際の誤差よりも低く見積もられる傾向があり、車両の現在位置の算出精度が低くなる場合がある。

また、特許文献２の航法装置は、有色雑音をモデル化してカルマンフィルタ内で誤差を単独推定するものであり、推定された誤差には有色雑音のオフセット分が考慮されている。しかし、特許文献２の航法装置は、カルマンフィルタ内では、有色雑音の成分が行列式の要素として追加されるため、演算に用いられる行列のサイズ（要素数）が大きくなる。そのため、演算処理には大きな処理負荷がかかり、大きなメモリ空間も必要となるなど、多くの計算機リソースが必要となる。

移動体の位置を検出する位置検出装置は、移動体と共に移動するため、小型かつ軽量であることが求められており、電源としても大きなバッテリを搭載できないことが多い。そのため、そのような位置検出装置には、低い処理負荷や必要なメモリ量がすくないことが求められる。

本発明は上記事情を鑑みてなされたものであり、本発明の目的は、有色雑音を考慮した誤差を算出することができると共に、演算リソースの消費量を低く抑えることにある。

上記課題を解決するための本発明の第一の態様は、例えば、移動体の位置を検出する位置検出装置であって、ＧＰＳ信号に基づいて前記移動体の位置を算出し、ＧＰＳ位置として出力するＧＰＳ位置算出部と、前記移動体に設けられた各センサから出力された情報を取得するセンサ情報取得部と、前記ＧＰＳ位置算出部によって算出された前記ＧＰＳ位置の情報と、前記センサ情報取得部によって取得されたセンサ情報とに基づいて前記移動体の位置および向きを示す情報、ならびに、これらの誤差共分散行列を算出する位置誤差算出部と、誤差の累積を示す誤差ベクトルを算出し、算出した誤差ベクトルの長さを示す情報をオフセットとして算出するオフセット算出部と、前記オフセット算出部によって算出されたオフセットに基づいて、前記位置誤差算出部によって算出された誤差共分散行列から求まる誤差楕円の面積が大きくなるように当該誤差共分散行列を補正する誤差共分散行列補正部と、前記位置誤差算出部によって算出された前記移動体の位置および向きを示す情報、および、前記誤差共分散行列補正部によって補正された誤差共分散行列を用いて、地図データに含まれる道路上に前記移動体が存在する確率を算出し、算出した確率に基づいて前記地図データに含まれる道路上に前記移動体の位置を推定するマップマッチ処理部とを備える。

また、本発明の第二の態様は、例えば、コンピュータを、移動体の位置を検出する位置検出装置として機能させるプログラムであって、前記コンピュータに、ＧＰＳ信号に基づいて前記移動体の位置を算出し、ＧＰＳ位置として出力するＧＰＳ位置算出機能と、前記移動体に設けられた各センサから出力された情報を取得するセンサ情報取得機能と、前記ＧＰＳ位置算出機能によって算出された前記ＧＰＳ位置の情報と、前記センサ情報取得機能によって取得されたセンサ情報とに基づいて前記移動体の位置および向きを示す情報、ならびに、これらの誤差共分散行列を算出する位置誤差算出機能と、誤差の累積を示す誤差ベクトルを算出し、算出した誤差ベクトルの長さを示す情報をオフセットとして算出するオフセット算出機能と、前記オフセット算出機能によって算出されたオフセットに基づいて、前記位置誤差算出機能によって算出された誤差共分散行列から求まる誤差楕円の面積が大きくなるように補正する誤差共分散行列補正機能と、前記位置誤差算出機能によって算出された前記移動体の位置および向きを示す情報、および、前記誤差共分散行列補正機能によって補正された誤差共分散行列を用いて、地図データに含まれる道路上に前記移動体が存在する確率を算出し、算出した確率に基づいて前記地図データに含まれる道路上に前記移動体の位置を推定するマップマッチ処理機能とを実現させる。

本発明の位置検出装置によれば、有色雑音を考慮した誤差を算出することができると共に、演算リソースの消費量を低く抑えることができる。

本発明の一実施形態に係るナビゲーション装置の構成の一例を示すハードウェア構成図である。 記憶装置内に格納されているリンクテーブルのデータ構造の一例を示す概念図である。 演算処理部によって実現される位置検出機能の機能構成の一例を示すブロック図である。 位置検出機能によって実行される位置検出処理の一例を示すフローチャートである。 オフセット算出（Ｓ１００）における処理の一例を示すフローチャートである。 センサ情報取得部によって取得されたセンサデータと、ＧＰＳ位置算出部によって算出されたＧＰＳ位置の時系列を説明するための概念図である。 受信時刻Ｇ_syncにおける向きＤ_DRの算出過程を説明するための概念図である。 受信時刻Ｇ_syncにおける向きＤ_GPSの算出過程を説明するための概念図である。 差分ベクトルＥ_iの算出過程を説明するための概念図である。 誤差共分散行列の補正（Ｓ３００）の処理の一例を示すフローチャートである。 誤差共分散行列の補正の効果を説明するための概念図である。

以下、本発明の実施の形態について、図面を参照しながら説明する。

図１は、本発明の一実施形態における位置検出装置として動作するナビゲーション装置１０の構成の一例を示すハードウェア構成図である。ナビゲーション装置１０は、演算処理部１００と、記憶装置１０７と、音声入出力装置１０８と、入力装置１１１と、ＲＯＭ装置１１５と、車速センサ１１６と、加速度センサ１１７と、ジャイロセンサ１１８と、ＧＰＳ(Global Positioning System)受信装置１１９と、ＦＭ多重放送受信装置１２０と、ビーコン受信装置１２１とを備える。本実施形態におけるナビゲーション装置１０は、例えば自動車等の車両に搭載される。

演算処理部１００は、様々な処理を行う中心的ユニットである。演算処理部１００は、例えば各種センサ（車速センサ１１６、加速度センサ１１７、ジャイロセンサ１１８）、ＧＰＳ受信装置１１９、ＦＭ多重放送受信装置１２０等から出力される情報を基にして演算処理部１００の現在位置を検出する。また、演算処理部１００は、得られた現在位置の情報に基づいて、表示に必要な地図データを記憶装置１０７あるいはＲＯＭ装置１１５から読み出す。

また、演算処理部１００は、読み出した地図データをグラフィックス展開し、そこに現在位置を示すマークを重ねてディスプレイ１０６に表示する。また、演算処理部１００は、記憶装置１０７またはＲＯＭ装置１１５に記憶されている地図データ等を用いて、ユーザから指示された出発地点（または現在位置）と目的地点とを結ぶ最適な経路（推奨経路）を探索する。また、演算処理部１００は、スピーカ１１０やディスプレイ１０６を用いてユーザを誘導する。

また、演算処理部１００は、数値演算および各デバイスを制御するといった様々な処理を実行するＣＰＵ（Central Processing Unit）１０１と、記憶装置１０７から読み出した地図データや演算データ等を格納するＲＡＭ（Random Access Memory）１０２と、プログラムやデータを格納するＲＯＭ（Read Only Memory）１０３と、各種ハードウェアを演算処理部１００と接続するためのＩ／Ｆ（インターフェイス）１０４とを有する。演算処理部１００では、各デバイスがバス１０５で接続されている。

ディスプレイ１０６は、演算処理部１００等で生成されたグラフィックス情報を表示するユニットである。ディスプレイ１０６は、液晶ディスプレイ、有機ＥＬディスプレイなどで構成される。記憶装置１０７は、ＨＤＤや不揮発性メモリカードといった、少なくとも読み書きが可能な記憶媒体で構成される。この記憶媒体には、通常の経路探索装置に必要な地図データ（地図上の道路を構成するリンクのリンクデータを含む）であるリンクテーブル等が格納されている。

音声入出力装置１０８は、マイクロフォン１０９およびスピーカ１１０を有する。マイクロフォン１０９は、運転者等の搭乗者が発した音声を取得する。スピーカ１１０は、演算処理部１００で生成されたユーザへのメッセージ等を音声信号として出力する。

マイクロフォン１０９とスピーカ１１０とは、当該ナビゲーション装置１０が搭載された車両内の所定の部位に、別個に配されている。ただし、マイクロフォン１０９とスピーカ１１０とは、ナビゲーション装置１０の筐体内に収納されていてもよい。ナビゲーション装置１０は、マイクロフォン１０９およびスピーカ１１０を、それぞれ複数備えることができる。

入力装置１１１は、ユーザからの指示をユーザによる操作を介して受け付ける装置である。入力装置１１１は、方向キー１１２と、ダイヤルスイッチ１１３と、タッチパネル１１４とを有するが、その他のハードスイッチ（例えば縮尺変更キーなど）が設けられていてもよい。

ＲＯＭ装置１１５は、ＣＤ-ＲＯＭやＤＶＤ-ＲＯＭ等のＲＯＭ（Read Only Memory）や、ＩＣ（Integrated Circuit）カードといった、少なくとも読み取りが可能な記憶媒体で構成されている。この記憶媒体には、例えば、動画データや、音声データなどが記憶されている。

車速センサ１１６、加速度センサ１１７、およびジャイロセンサ１１８は、ナビゲーション装置１０で現在位置を検出するために使用されるセンサである。

車速センサ１１６は、車速を算出するのに用いられる信号を出力するセンサである。車速センサ１１６は、例えば、車輪の回転数を検出してパルス信号に変換し、所定の時間内におけるパルス信号数といった形で車速情報を出力する。

加速度センサ１１７は、車両の前後方向の加速度に応じて所定の検出信号を一定周期で出力するセンサである。加速度センサ１１７は、例えば、車両の加速度が増加している場合には正の信号を出力し、減速度が発生している場合には負の信号を出力する。

ジャイロセンサ１１８は、光ファイバジャイロや振動ジャイロ等で構成され、移動体の回転による角速度を検出するセンサである。

ＧＰＳ受信装置１１９は、ＧＰＳ衛星からの信号を受信し、受信した信号を演算処理部１００に提供する。

ＦＭ多重放送受信装置１２０は、ＦＭ多重放送局から送られてくるＦＭ多重放送信号を受信する。ＦＭ多重放送には、ＶＩＣＳ（Vehicle Information Communication System：登録商標）情報の概略現況交通情報、規制情報、ＳＡ／ＰＡ（サービスエリア／パーキングエリア）情報、駐車場情報、天気情報などやＦＭ多重一般情報としてラジオ局が提供する文字情報などがある。

ビーコン受信装置１２１は、ＶＩＣＳ情報などの概略現況交通情報、規制情報、ＳＡ／ＰＡ情報、駐車場情報、天気情報や緊急警報などを受信する。ビーコン受信装置１２１は、例えば、光により通信する光ビーコン、電波により通信する電波ビーコン等を受信する。

図２は、記憶装置１０７内に格納されるリンクテーブル２０のデータ構造の一例を示す概念図である。リンクテーブル２０は、予め定められた地図領域であるメッシュ毎に、当該メッシュに含まれる道路を示すリンクに関する情報を含むメッシュデータ２１を有する。それぞれのメッシュデータ２１には、それぞれのメッシュを識別するメッシュＩＤ２２および当該メッシュ内のリンクに関するデータであるリンクデータ２３が含まれる。

それぞれのリンクデータ２３には、それぞれのリンクを識別するリンクＩＤ２３０、当該リンクの開始ノード座標２３１、当該リンクの終了ノード座標２３２、当該リンクの道路種別２３３、当該リンクのリンク長２３４、当該リンクの旅行時間２３５、当該リンクの開始ノードに接続されている他のリンクのリンクＩＤを示す開始接続リンク２３６、および当該リンクの終了ノードに接続されている他のリンクのリンクＩＤを示す終了接続リンク２３７等が格納されている。

図３は、演算処理部１００によって実現される位置検出機能３０の機能構成の一例を示すブロック図である。位置検出機能３０は、ＧＰＳ位置算出部３１、センサ情報取得部３２、オフセット算出部３３、位置誤差算出部３４、誤差共分散行列補正部３５、およびマップマッチ処理部３６を有する。

ＧＰＳ位置算出部３１は、所定の時間間隔で（例えば１秒毎に）、ＧＰＳ受信装置１１９から提供されたＧＰＳ信号に基づいて、ナビゲーション装置１０が搭載された車両とＧＰＳ衛星間の距離およびその変化率を３個以上の衛星に対して測定することで、車両の位置（ＧＰＳ位置）、速度、方位を示す位置情報、速度情報、および方位情報、ならびに、算出した車両位置の確かさを示す誤差情報を生成する。そして、ＧＰＳ位置算出部３１は、生成したこれらの情報をオフセット算出部３３および位置誤差算出部３４へそれぞれ出力する。

センサ情報取得部３２は、所定の時間間隔で（例えば０．２秒毎に）、車速センサ１１６、加速度センサ１１７、およびジャイロセンサ１１８のそれぞれから出力された情報を取得し、取得したこれらの情報を、オフセット算出部３３および位置誤差算出部３４へそれぞれ出力する。

オフセット算出部３３は、後述する手順により、誤差の累積を示す誤差ベクトルを算出し、算出した誤差ベクトルの長さを示す情報をオフセットとして誤差共分散行列補正部３５へ出力する。

位置誤差算出部３４は、例えばカルマンフィルタ（「拡張カルマンフィルタ」と呼ばれるものを含む）によって構成され、誤差のある測定値を用いて、車両等の移動体の位置や方位などを特定する情報を算出する。本実施形態に係る位置誤差算出部３４は、車両の位置情報、速度情報、方位情報、誤差情報や自律情報といった複数の情報を受け付けて、これらの平均と分散を求めることにより、車両の位置および向きを示す情報、ならびに、これらの情報の誤差を示す誤差共分散行列を算出する。そして、位置誤差算出部３４は、算出したこれらの情報を誤差共分散行列補正部３５へ出力する。

誤差共分散行列補正部３５は、後述する手順により、位置誤差算出部３４によって算出された誤差共分散行列を、オフセット算出部３３によって算出されたオフセットを用いて補正し、補正後の誤差共分散行列を、位置誤差算出部３４によって算出された車両の位置および向きを示す情報と共にマップマッチ処理部３６へ出力する。

マップマッチ処理部３６は、記憶装置１０７内のリンクテーブル２０を参照し、誤差共分散行列補正部３５から受け取った車両の位置および向きを示す情報ならびに補正後の誤差共分散行列を用いて、車両が存在している最も確からしいリンク上の位置を特定する。そして、マップマッチ処理部３６は、特定した位置近傍の地図データを記憶装置１０７から読み出してグラフィックス展開し、表示された地図上で特定された位置にカーマークを重ねてディスプレイ１０６に表示する。

図４は、位置検出機能３０によって実行される位置検出処理の一例を示すフローチャートである。位置検出機能３０は、例えば所定の時間間隔で（例えば１秒毎に）、本フローチャートに示す処理を実行する。

なお、これとは別に、ＧＰＳ位置算出部３１は、所定の時間間隔でナビゲーション装置１０が搭載された車両のＧＰＳ位置等を算出し、これらの情報をオフセット算出部３３および位置誤差算出部３４に提供しており、センサ情報取得部３２は、所定の時間間隔で各センサから出力された情報を取得してオフセット算出部３３および位置誤差算出部３４に提供している。

まず、オフセット算出部３３は、図５において後述する手順により、誤差の累積を示す誤差ベクトルを算出し、算出した誤差ベクトルの長さを示す情報をオフセットとして算出し、算出したオフセットを示す値を誤差共分散行列補正部３５へ送る（Ｓ１００）。

次に、位置誤差算出部３４は、ＧＰＳ位置算出部３１によって算出されたＧＰＳ位置を含む位置情報、速度情報、方位情報、および誤差情報、ならびに、各センサから出力された情報を用いて、これらの平均と分散を求めることにより、車両の位置および向きを示す情報、ならびに、これらの情報の誤差を示す誤差共分散行列を算出する（Ｓ２００）。そして、位置誤差算出部３４は、算出したこれらの情報を誤差共分散行列補正部３５へ送る。

なお、ステップＳ１００における処理と、ステップＳ２００における処理とは、ステップＳ３００が実行される前までに終了していればよく、本フローチャートに示す順番に限定されない。

次に、誤差共分散行列補正部３５は、図１０において後述する手順により、位置誤差算出部３４によって算出された誤差共分散行列を、オフセット算出部３３によって算出されたオフセットを用いて補正し（Ｓ３００）、補正後の誤差共分散行列を、位置誤差算出部３４によって算出された車両の位置および向きを示す情報と共にマップマッチ処理部３６へ送る。

次に、マップマッチ処理部３６は、記憶装置１０７内のリンクテーブル２０を参照し、誤差共分散行列補正部３５から受け取った車両の位置および向きを示す情報ならびに補正後の誤差共分散行列を用いて、車両が存在している最も確からしいリンク上の位置にカーマークを重ねてディスプレイ１０６に表示し（Ｓ４００）、位置検出機能３０は、本フローチャートに示す位置検出処理を終了する。

図５は、オフセット算出（Ｓ１００）における処理の一例を示すフローチャートである。

ここで、オフセット算出の処理を説明する前に、各センサによって測定されたセンサデータとＧＰＳ位置算出部３１によって算出されたＧＰＳ位置の時間的な関係を図示すると、例えば図６のようになる。オフセット算出部３３および位置誤差算出部３４では、例えば図６に示すように、センサ情報取得部３２によって取得された各センサデータおよびＧＰＳ位置算出部３１によって算出されたＧＰＳ位置の情報を保持している。

図６に示す例では、センサデータおよびＧＰＳ位置の情報は、新しい順に図６の上から下へ格納されている。また、最新のＧＰＳ位置Ｐ_nextを示すＧＰＳ信号の受信時刻をＧ_next、その１つ前のＧＰＳ位置Ｐ_syncを示すＧＰＳ信号の受信時刻をＧ_sync、さらにその１つ前のＧＰＳ位置Ｐ_prevを示すＧＰＳ信号の受信時刻をＧ_prevと定義する。

まず、オフセット算出部３３は、受信時刻Ｇ_prevにおける車両のＧＰＳ位置Ｐ_prev（Ｘ_gn-2，Ｙ_gn-2，θ_gn-2）を基準として、受信時刻Ｇ_prevから受信時刻Ｇ_syncまでのセンサデータを用いて、ナビゲーション装置１０が搭載された車両のデッドレコニングを行い、受信時刻Ｇ_syncにおける車両の位置Ｐ_DR（Ｘ_DR，Ｙ_DR）および向きＤ_DRを算出する（Ｓ１０１）。

オフセット算出部３３は、例えば図７に示すように、受信時刻Ｇ_prevから受信時刻Ｇ_syncまでのセンサデータを時系列につなげて受信時刻Ｇ_syncにおける車両の位置Ｐ_DR（Ｘ_DR，Ｙ_DR）および向きＤ_DRを算出する。受信時刻Ｇ_syncにおける車両の向きＤ_DRは、受信時刻Ｇ_prevにおける車両の向きθ_gn-2と角度θ_DRをなしている。

次に、オフセット算出部３３は、受信時刻Ｇ_prevにおける車両の位置Ｐ_prevから、受信時刻Ｇ_nextにおける車両の位置Ｐ_nextまでのＧＰＳ位置の変化から、受信時刻Ｇ_syncのＧＰＳ信号から算出されたＧＰＳ位置Ｐ_syncにおける向きＤ_GPSを算出する（Ｓ１０２）。オフセット算出部３３は、例えば図８に示すように、ＧＰＳ位置Ｐ_prevからＧＰＳ位置Ｐ_syncに至るベクトルＭ₁の向きと、ＧＰＳ位置Ｐ_syncからＧＰＳ位置Ｐ_nextに至るベクトルＭ₂の向きとの中間の向きをＤ_GPSとして算出する。図８に示した例では、ベクトルＭ₁とベクトルＭ₂とのなす角θ_GPSの１／２の角度となる方向を、ＧＰＳ位置Ｐ_syncにおける向きＤ_GPSとして算出している。

次に、オフセット算出部３３は、差分ベクトルＥ_i（Ｅ_Xi，Ｅ_Yi）を算出する（Ｓ１０３）。オフセット算出部３３は、例えば図９に示すように、ステップＳ１０１において算出した車両の位置Ｐ_DR（Ｘ_DR，Ｙ_DR）を、受信時刻Ｇ_syncにおけるＧＰＳ位置Ｐ_syncに一致させる。そして、オフセット算出部３３は、ステップＳ１０１において算出した車両の向きＤ_DRを、ステップＳ１０２において算出した向きＤ_GPSに一致させるように回転させる。

そして、オフセット算出部３３は、ステップＳ１０１において位置Ｐ_DR（Ｘ_DR，Ｙ_DR）の算出基準となっていた受信時刻Ｇ_prevにおける回転および平行移動させた後の位置を位置Ｐ'_prevとして算出する。受信時刻Ｇ_syncにおけるＧＰＳ位置Ｐ_syncをＸ−Ｙ座標の原点とすると、位置Ｐ'_prevは（−Ｘ_DR，−Ｙ_DR）の位置に算出される。そして、オフセット算出部３３は、ＧＰＳ位置Ｐ_prevから位置Ｐ'_prevに至るベクトルを差分ベクトルＥ_i（Ｅ_Xi，Ｅ_Yi）として算出する。

次に、オフセット算出部３３は、過去に算出した所定数（例えば１０個）の差分ベクトルＥ_iのベクトル和をとり差分ベクトルＥを算出し、算出した差分ベクトルＥの長さをｄ_offsetとして誤差共分散行列補正部３５へ送り、オフセット算出部３３は、本フローチャートに示したオフセット算出の処理を終了する。

図１０は、誤差共分散行列の補正（Ｓ３００）の処理の一例を示すフローチャートである。

まず、誤差共分散行列補正部３５は、位置誤差算出部３４から出力された誤差共分散行列に基づいて、誤差楕円の長軸ｅｒｒ_aおよび短軸ｅｒｒ_bの長さをそれぞれ算出する（Ｓ３０１）。

次に、誤差共分散行列補正部３５は、誤差楕円の長軸ｅｒｒ_aに、ステップＳ１００においてオフセット算出部３３が算出したｄ_offsetを加算して、補正後の長軸ｅｒｒ'_aを算出する（Ｓ３０２）。

次に、誤差共分散行列補正部３５は、以下の算出式（１）により、誤差楕円の短軸ｅｒｒ_bについても長軸と同じ比率で延伸させ、補正後の短軸ｅｒｒ'_bを算出する（Ｓ３０３）。
ｅｒｒ'_b＝ｅｒｒ_b×ｅｒｒ'_a／ｅｒｒ_a ・・・（１）

このように、長軸と短軸とを同じ比率で延伸させることにより、誤差楕円の面積を大きくする補正を行うことで誤差の分布特性に与える影響を少なくすることができる。

次に、誤差共分散行列補正部３５は、補正後の長軸ｅｒｒ'_aおよび短軸ｅｒｒ'_bを用いて、誤差共分散行列を再度作成する（Ｓ３０４）。そして、誤差共分散行列補正部３５は、作成した補正後の誤差共分散行列を、位置誤差算出部３４から受け取った車両の位置および向きの情報と共にマップマッチ処理部３６へ送り、誤差共分散行列補正部３５は、本フローチャートに示した誤差共分散行列の補正の処理を終了する。

ここで、例えば図１１（ａ）に示すように、有色性の雑音の影響により符号４４の位置に現在位置の候補点が算出された場合、白色雑音として誤差が見積もられると、例えば誤差楕円は符号４３のような大きさで算出される。

そして、マップマッチ処理部３６が、少なくとも一部が誤差楕円に含まれるようなリンクについて尤度を算出し、その中で最も確からしいリンクを決定するとすれば、たとえ実際の車両の位置がリンク４１で示される道路上にある場合であっても、リンク４１は尤度の算出対象から除外されてしまう。そのため、カーマーク４２の位置が、実際には走行していない道路を示すリンク４０上に算出されてしまう場合がある。

これに対して、本実施形態のナビゲーション装置１０では、各センサの誤差を累積し、累積した誤差に基づいて、誤差楕円の面積が大きくなるように誤差共分散行列を補正する。これにより、例えば図１１（ｂ）に示すように、リンク４１の一部が補正後の誤差楕円に含まれることになり、カーマーク４２の位置は、実際には走行している道路を示すリンク４１上に算出される可能性が高まる。

以上、本発明の実施の形態について説明した。

上記説明から明らかなように、本実施形態のナビゲーション装置１０によれば、有色雑音を考慮した誤差を算出することができると共に、演算リソースの消費量を低く抑えることができる。

例えば、特許文献２における航行装置では、有色雑音の成分がカルマンフィルタ内で行列式の要素として追加されるため、演算に用いられる行列のサイズ（要素数）が大きくなり、多くの計算機リソースが必要となる。

これに対して、本実施形態のナビゲーション装置１０では、カルマンフィルタによる行列演算とは別個に、各センサの誤差を累積し、累積した誤差に基づいて誤差共分散行列を補正する。これにより、サイズの大きな行列の演算に必要な多くのリソースを確保する必要がなく、オフセットの計算も行列演算ほど多くのリソースを必要としないため、リソースの限られた組込み機器において精度の高い位置検出を実現することができる。

なお、本発明は、上記した実施形態に限定されるものではなく、その要旨の範囲内で数々の変形が可能である。

例えば、上記した実施形態において、誤差共分散行列補正部３５は、オフセット算出部３３が算出したｄ_offsetを誤差楕円の長軸に加算したが、本発明はこれに限られず、誤差楕円の短軸に加算してもよい。短軸にｄ_offsetを加算した場合、誤差共分散行列補正部３５は、長軸を同じ比率で延伸させる。また、誤差共分散行列補正部３５は、誤差楕円の長軸または短軸以外の方向に、オフセット算出部３３が算出したｄ_offset分、誤差楕円を拡大することにより、誤差楕円の面積を大きくするようにしてもよい。

また、誤差共分散行列補正部３５は、オフセット算出部３３が算出したｄ_offsetを、誤差楕円の長軸および短軸にそれぞれ加算してもよく、長軸または短軸のいずれか一方にｄ_offsetを加算した場合には、他方の軸の長さを変更しないようにしてもよい。

また、上記した実施形態は、車両に搭載されるナビゲーション装置１０を例に説明したが、本発明はこれに限られず、位置検出機能３０を有する装置であれば、汎用コンピュータや携帯情報端末等であってもよい。また、各センサやＧＰＳ受信装置を移動体と共に移動させ、それらの機器からの情報を無線通信により位置検出機能３０を有する外部のコンピュータへ送信し、当該外部のコンピュータが位置検出機能３０の処理を実行して地図上にカーマークを重ねた画像を作成し、作成した画像を無線通信により移動体へ送信して移動体の画面に表示させるようにしてもよい。

なお、上記したナビゲーション装置１０内の各構成要素は、本実施形態に係るナビゲーション装置１０の構成の理解を容易にするために、主な処理内容に応じて機能別に区分したものである。そのため、構成要素の区分方法やその名称によって、本願発明が制限されることはない。本実施形態に係るナビゲーション装置１０の構成は、処理内容に応じて、さらに多くの構成要素に区分することもできるし、１つの構成要素がさらに多くの処理を実行するように区分することもできる。また、それぞれの処理は、ソフトウェアによる処理として実現されてもよく、ＡＳＩＣ（Application Specific Integrated Circuit）等の専用のハードウェアにより実現されてもよい。

１０：ナビゲーション装置、１００：演算処理部、１０１：ＣＰＵ、１０２：ＲＡＭ、１０３：ＲＯＭ、１０４：Ｉ／Ｆ、１０５：バス、１０６：ディスプレイ、１０７：記憶装置、１０８：音声入出力装置、１０９：マイクロフォン、１１０：スピーカ、１１１：入力装置、１１２：方向キー、１１３：ダイヤルスイッチ、１１４：タッチパネル、１１５：ＲＯＭ装置、１１６：車速センサ、１１７：加速度センサ、１１８：ジャイロセンサ、１１９：ＧＰＳ受信装置、１２０：ＦＭ多重放送受信装置、１２１：ビーコン受信装置、２０：リンクテーブル、２１：メッシュデータ、２２：メッシュＩＤ、２３：リンクデータ、２３０：リンクＩＤ、２３１：開始ノード座標、２３２：終了ノード座標、２３３：道路種別、２３４：リンク長、２３５：旅行時間、２３６：開始接続リンク、２３７：終了接続リンク、３０：位置検出機能、３１：ＧＰＳ位置算出部、３２：センサ情報取得部、３３：オフセット算出部、３４：位置誤差算出部、３５：誤差共分散行列補正部、３６：マップマッチ処理部、４０：リンク、４１：リンク、４２：カーマーク、４３：誤差楕円、４４：候補点、４５：補正後の誤差楕円、４６：カーマーク

Claims (7)

1. 移動体の位置を検出する位置検出装置であって、
   ＧＰＳ信号に基づいて前記移動体の位置を算出し、ＧＰＳ位置として出力するＧＰＳ位置算出部と、
   前記移動体に設けられた各センサから出力された情報を取得するセンサ情報取得部と、
   前記ＧＰＳ位置算出部によって算出された前記ＧＰＳ位置の情報と、前記センサ情報取得部によって取得されたセンサ情報とに基づいて前記移動体の位置および向きを示す情報、ならびに、これらの誤差共分散行列を算出する位置誤差算出部と、
   誤差の累積を示す誤差ベクトルを算出し、算出した誤差ベクトルの長さを示す情報をオフセットとして算出するオフセット算出部と、
   前記オフセット算出部によって算出されたオフセットに基づいて、前記位置誤差算出部によって算出された誤差共分散行列から求まる誤差楕円の面積が大きくなるように当該誤差共分散行列を補正する誤差共分散行列補正部と、
   前記位置誤差算出部によって算出された前記移動体の位置および向きを示す情報、および、前記誤差共分散行列補正部によって補正された誤差共分散行列を用いて、地図データに含まれる道路上に前記移動体が存在する確率を算出し、算出した確率に基づいて前記地図データに含まれる道路上に前記移動体の位置を推定するマップマッチ処理部と
   を備えることを特徴とする位置検出装置。
2. 請求項１に記載の位置検出装置であって、
   前記ＧＰＳ位置算出部は、所定時間毎に前記ＧＰＳ位置を算出し、
   前記オフセット算出部は、
   第一のタイミングにおいて算出された前記ＧＰＳ位置を基準位置として、当該第一のタイミングよりも前記所定時間後のタイミングである第二のタイミングまでの間に取得したセンサ情報を累積して、当該第二のタイミングにおける前記移動体の位置および向きを算出し、
   前記第一のタイミングにおいて算出された前記ＧＰＳ位置から、前記第二のタイミングで算出された前記ＧＰＳ位置までを結ぶ第一のベクトルの向きと、当該第二のタイミングで算出された前記ＧＰＳ位置から、当該第二のタイミングよりも前記所定時間後のタイミングである第三のタイミングで算出された前記ＧＰＳ位置までを結ぶ第二のベクトルの向きとの中間の向きを示す第一の方向を算出し、
   前記センサ情報に基づいて算出した前記第二のタイミングにおける前記移動体の位置と、当該第二のタイミングで算出された前記ＧＰＳ位置とを一致させると共に、前記センサ情報に基づいて算出した前記第二のタイミングにおける前記移動体の向きを前記第一の方向に一致させるように、前記第一のタイミングから前記第二のタイミングまでの間に取得したセンサ情報の軌跡を移動させ、
   移動後の前記基準位置と、前記第一のタイミングにおいて算出された前記ＧＰＳ位置との差に基づいて前記オフセットを算出することを特徴とする位置検出装置。
3. 請求項２に記載の位置検出装置であって、
   前記オフセット算出部は、
   前記第一のベクトルと前記第二のベクトルとのなす角の１／２の角度となる方向を前記第一の方向として算出することを特徴とする位置検出装置。
4. 請求項２または３に記載の位置検出装置であって、
   前記オフセット算出部は、
   前記所定時間毎に取得された前記センサ情報および算出された前記ＧＰＳ位置を用いて、前記第一のタイミングにおいて算出された前記ＧＰＳ位置から、前記基準位置までのベクトルを算出し、前記所定時間毎に連続して算出された所定数の当該ベクトルを加算して求めたベクトルの長さをオフセットとして算出することを特徴とする位置検出装置。
5. 請求項１から４のいずれか一項に記載の位置検出装置であって、
   前記誤差共分散行列補正部は、
   前記位置誤差算出部によって算出された誤差共分散行列から求まる誤差楕円の長軸または短軸の長さに、前記オフセット算出部によって算出されたオフセットを加算することにより、当該誤差楕円の面積が大きくなるように補正することを特徴とする位置検出装置。
6. 請求項５に記載の位置検出装置であって、
   前記誤差共分散行列補正部は、
   前記誤差楕円の長軸または短軸のいずれか一方の長さに前記オフセットを加算した場合に、他方の軸の長さを、当該一方の軸と同じ比率で延伸させることを特徴とする位置検出装置。
7. コンピュータを、移動体の位置を検出する位置検出装置として機能させるプログラムであって、
   前記コンピュータに、
   ＧＰＳ信号に基づいて前記移動体の位置を算出し、ＧＰＳ位置として出力するＧＰＳ位置算出機能と、
   前記移動体に設けられた各センサから出力された情報を取得するセンサ情報取得機能と、
   前記ＧＰＳ位置算出機能によって算出された前記ＧＰＳ位置の情報と、前記センサ情報取得機能によって取得されたセンサ情報とに基づいて前記移動体の位置および向きを示す情報、ならびに、これらの誤差共分散行列を算出する位置誤差算出機能と、
   誤差の累積を示す誤差ベクトルを算出し、算出した誤差ベクトルの長さを示す情報をオフセットとして算出するオフセット算出機能と、
   前記オフセット算出機能によって算出されたオフセットに基づいて、前記位置誤差算出機能によって算出された誤差共分散行列から求まる誤差楕円の面積が大きくなるように補正する誤差共分散行列補正機能と、
   前記位置誤差算出機能によって算出された前記移動体の位置および向きを示す情報、および、前記誤差共分散行列補正機能によって補正された誤差共分散行列を用いて、地図データに含まれる道路上に前記移動体が存在する確率を算出し、算出した確率に基づいて前記地図データに含まれる道路上に前記移動体の位置を推定するマップマッチ処理機能と
   を実現させることを特徴とするプログラム。

Images