JP4124249B2 - 測位装置、ナビゲーションシステム - Google Patents

Description

本発明は、移動体の位置を検出する測位装置及びナビゲーションシステムに関し、特に、電波航法による測位位置に自律センサによる検出情報を累積すると共に、地図データを規範にして位置を推定する測位装置及びナビゲーションシステムに関する。

ナビゲーションシステムでは、ＧＰＳ（Ｇｒｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波に基づき自車両の位置を測位して、車速センサ及びジャイロセンサ等を用いて走行距離及び走行方向を累積しながら車両の現在位置を精度よく推定する。

しかしながら、ＧＰＳ衛星からの電波を受信できない状態では、自律航法による測位に含まれる誤差が時間と共に増幅されるため徐々に位置の精度が低下する。このため、自律航法により測位した車両の位置を補正する種々の方法が提案されている。

例えば、ナビゲーションシステムの地図データを利用して自律航法により測位した位置を補正するマップマッチングが知られている。地図データはノード（例えば、交差点）間をリンク（例えば、道路）により結合したものであるため、自律航法により検出した位置を地図データに対応づけることで、車両が道路上を走行するように自車両の位置を補正することができる。

しかしながら、地図データの精度はそれほど高いものではなく、マップマッチングによる補正では十分な精度が得られない場合が多い。そこで、例えば、自律航法により測位された位置・方向に最も整合する位置及び方位を有する道路を地図データから選択して、該道路に対応づけて位置・方向を補正する方法が提案されている（例えば、特許文献１参照。）。
特開２００２−２１３９７９号公報

しかしながら、上記のように一般に市販されているナビゲーションシステムの地図データはそれほど精度が高いものではなく、また、道路網を交差点（ノード）を結ぶ直線状のリンクにより表現するため実際の道路の形状と一致しない場合がある。

図１（ａ）は、地図データのリンク情報と実際の道路形状のズレの一例を示す図である。図１（ａ）に示すように実際の道路は緩やかに湾曲しているが、地図データではそれを２つのリンクにより表現しているため、実際の道路とリンク情報とが異なる箇所が生じる。例えば、「実際の自車位置」を車両が走行している場合、ＧＰＳ衛星を捕捉している状態では「実際の自車位置」を自車両の位置として認識するが、ＧＰＳ衛星が捕捉されない状態で直線状のリンクにマップマッチングした場合、実際の車両位置と測位された位置とが誤差Ｌを有することとなる。

図１（ｂ）は測量により誤差Ｌがどの程度かを調査した地図データの精度の一例を示す。図１（ｂ）では横軸が測量点（測位の識別番号）を、縦軸が実際の道路形状と地図データの較差を示す。図１（ｂ）に示すように、地図データは交差点（ノード）付近では実際の道路との誤差が小さいが、場所によっては６ｍ程度、実際の道路と異なる。したがって、ＧＰＳ衛星が捕捉されない状態で、地図データにマップマッチングして位置を補正すると、精度のよい測位が困難であるという問題がある。

しかしながら一方で、高精度な地図データを得るためには、道路の正確な測量やリンクの高精細化が必要であるため全ての道路でこれを実現するのは困難である。

本発明は、上記課題に鑑み、ＧＰＳ衛星が捕捉困難な状態であっても、従来の地図データを使用して、精度よく測位することができる測位装置、ナビゲーションシステムを提供することを目的とする。

上記課題を解決するため、本発明は、ＧＰＳ等の電波航法測位手段により移動体の位置を検出する測位装置において、移動体の挙動情報を検出する第１の自律センサ（例えば、車速センサ２、舵角センサ４）と、電波航法測位手段による測位に第１の自律センサによる検出情報を累積して推定位置及び推定方向を検出する位置検出手段（例えば、ＩＮＳ位置測位手段）と、位置検出手段による推定位置に基づき地図データ記憶手段（例えば、地図ＤＢ）を参照し、推定位置に対応するリンクから所定距離の地図データ推定位置を検出する地図データ測位手段と、推定位置及び地図データ推定位置をカルマンフィルタに適用して前記移動体の前記位置を推定する最尤位置推定手段（例えば、カルマンフィルタ演算手段）と、を有することを特徴とする。

ＧＰＳ衛星が捕捉困難な状態であっても、従来の地図データを使用して、精度よく測位することができる測位装置、ナビゲーションシステムを提供することができる。

以下、本発明を実施するための最良の形態について、図面を参照しながら説明する。
図２は、本実施の形態の測位装置９を適用したナビゲーションシステム１０の概略構成図を示す。

ナビゲーションシステム１０は、ナビゲーションシステムを制御するナビＥＣＵ（Ｅｌｅｃｔｒｉｃａｌ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）８により制御される。ナビＥＣＵ８は、プログラムを実行するＣＰＵ、プログラムを記憶した記憶装置（ハードディスクドライブ、ＲＯＭ）、データやプログラムを一時的に記憶するＲＡＭ、データを入力及び出力する入出力装置、ＮＶ（Ｎｏｎ Ｖｏｌａｔｉｌｅ）−ＲＡＭ等がバスを介して接続されたコンピュータとして構成される。

ナビＥＣＵ８にはＧＰＳ（Ｇｒｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波を受信するＧＰＳ受信装置１、車両の速度を検出する車速センサ２、車両の重心を通り同車両の前後方向に延びる軸線（ローリング軸）回りの回転角速度を検出するヨーレートセンサ３、操舵輪の舵角を検出する舵角センサ４、地図データを記憶した地図データベース（以下、地図ＤＢという）５、ナビゲーションシステム１０を操作するための入力装置６及び地図に現在位置を表示する液晶やＨＵＤ（Ｈｅａｄ Ｕｐ Ｄｉｓｐｌａｙ）等の表示装置７が接続されている。

地図ＤＢ５には、実際の道路網をノード(例えば、道路と道路が交差する点、交差点から所定間隔毎に区切った点、等)及びリンク(ノードとノードを接続する道路)に対応づけて、テーブル状のデータベースとして構成される。

ナビＥＣＵ８は、車室内に設けられた表示装置７に、検出した現在位置周辺の地図を地図ＤＢ５から抽出し、指定された縮尺に合わせて表示する。ナビＥＣＵ８は、必要に応じて地図に重畳して車両の現在位置を表示する。

また、押下式のキーボードやリモコン等の入力装置６から目的地が入力されると、検出した現在位置から目的地まで、例えばダイクストラ法など周知のルート検索方法でルート検索を行い、ルートを地図に重畳して表示すると共に、右左折する交差点の手前で運転者にルートを案内する。

ナビＥＣＵ５のＣＰＵは、記憶装置に記憶されたプログラムを実行することで、本実施の形態で説明する測位を実現する。図３（ａ）は、測位装置９の機能ブロック図を示す。

本実施の形態の測位装置９は、ＧＰＳ衛星の捕捉が困難な状況やＧＰＳ測位の信頼性が低下した状況下で、車両の位置を高精度に測位する。概略を説明すれば、測位装置９は自律航法による測位に地図ＤＢ５を規範とした測位情報を付加し、カルマンフィルタにより最尤推定される位置を出力する。自律航法による測位結果に単にマップマッチングする場合と比べ測位精度を向上することができる。後述する実際の測位結果により明らかとなるが、本実施形態による位置補正は特に進行方向（道路長方向）に垂直な方向の位置ずれの補正に大きな効果がある。

また、カルマンフィルタの適用においては、カップリングさせる測位情報が互いに独立している必要があるため、本実施の形態では
Ａ．車速センサ２及び舵角センサ４
Ｂ．地図ＤＢ５及びヨーレートセンサ３
の２つのセンサ系統に分けることとした。なお、Ａのセンサ系統にヨーレートセンサ３を、Ｂのセンサ系統に舵角センサ４を用いてもよく、また、車両の方向を検出するセンサにジャイロセンサを用いてもよい。

また、カルマンフィルタを適用する場合、２つのセンサ系統による出力結果を観測方程式によりカップリングするが、Ａのセンサ系統では位置及び方向が検出されるのに対し、Ｂのセンサ系統では地図ＤＢ５の地図データとの相対距離及び相対方向が検出され、両者を直接カップリングすることができない。そこで、本実施形態の測位装置９は２つのセンサ系統からの検出値をカップリングするための観測方程式を導出した。以下、詳細に説明する。

まず、ＧＰＳ測位手段８１は、周知の方法でＧＰＳ衛星からの電波に基づき自車両の位置を測位する。ＧＰＳ測位手段８１は所定の軌道を周回する複数のＧＰＳ衛星のうち現在の車両の位置から所定の仰角に入るＧＰＳ衛星を４つ以上選択し、それらのＧＰＳ衛星から発信される電波を受信する。ＧＰＳ測位手段８１は電波の到達時間を計算し、到達時間と光速ｃから捕捉したＧＰＳ衛星までの距離を算出する。そして、３つのＧＰＳ衛星と自車両の距離が交差する１点を自車両の位置として測位する。

測位装置９は、ＧＰＳ電波を受信している間は所定の時間毎に自車両の位置を測位する。そして、ＧＰＳ電波が遮断されると、最後に測位した位置を初期位置、その時の走行方向を初期方向として、これらに走行距離及び方向を累積する自律航法による測位を開始する。

図４（ａ）は自律航法（Ａのセンサ系統）により検出される位置を示す図である。図４（ａ）の道路２１は、地図ＤＢ５から抽出された２つのリンク２２、２３から成る地図データで表されている。自車両はリンク２２からリンク２３の方向へ走行しており、初期位置２４でＧＰＳ電波が遮断されたものとする。

ＧＰＳ衛星からの電波が遮断されると、ＩＮＳ（Inertial Navigation Sensor)測位手段８２は車速センサ２から車速、舵角センサ４から舵角を検出して、初期位置２４及び初期方向に走行距離及び方向を累積して自律航法による位置及び方向を推定する（以下、推定位置、推定方向という）。図４（ａ）では自車両の実際の自車位置２５に対し自律航法により推定された推定位置２６を示す。

また、カルマンフィルタを適用するためにはセンサ系統毎に誤差分散を必要とするが、車速センサ２及び舵角センサ４の誤差は速度やＧＰＳ電波遮断時間等に応じて既知である。したがって、これらに基づき累積した推定位置２６の誤差分散も既知である。図４（ａ）では推定位置２６の誤差分散を点線で囲まれた楕円で示した。

図４（ｂ）は、ＩＮＳ測位手段８２が推定した推定位置２６に基づき、地図ＤＢ（Ｂのセンサ系統）５のリンクに対応させて検出した位置を示す図である。ＩＮＳ測位手段８２が推定した推定位置２６は地図データ測位手段８３に入力され、地図データ測位手段８３は推定位置２６に対応したリンク２３を地図ＤＢ５から抽出する。

自車両は推定位置２６とリンク２３の周辺に存在すると推定されるため、地図データ測位手段８３はリンク２３から推定位置２６の方向へ所定距離ｒ（以下、オフセット量ｒという）を隔てた線上のいずれかに自車両が存在するものと推定する（以下、地図データ推定位置２７という）。

このオフセット量ｒは、ＧＰＳ遮断直前の初期位置２４とリンク２２との距離である。すなわち、ＧＰＳ遮断直前における初期位置２４とリンク２２とのオフセットが継続していると推定する。

また、推定位置２６もオフセット量ｒも推定値に過ぎないので、地図データ推定位置２７は「オフセット量Ｄを隔てた線上」のいずれかとなる。つまり、地図データ推定位置２７は一点に絞られない。

図４（ｃ）は、ヨーレートセンサ（Ｂのセンサ系統）の検出値により推定された自車両の方向を示す図である。ＧＰＳ測位手段８１による初期方向は方向検出手段８４に入力され、方向検出手段８４は初期方向にヨーレートセンサ３により検出されたヨーレートの積分値（方向を示す）を累積して、自車両の方向を推定する（以下、累積推定方向２８という）。したがって、Ａのセンサ系統とは別に車両の走行方向が推定される。

また、上記のように地図データ推定位置２７は本来的に誤差分散を含む値であり、累積推定方向２８もヨーレートセンサ３の検出誤差等による誤差を含む。地図データ測位手段８３は、オフセット量ｒや速度、ＧＰＳ電波遮断時間等に応じて地図データ推定位置２７，累積推定方向２８の誤差分散を算出する。

以上のようにしてＡ及びＢのセンサ系統によりそれぞれ自車両の位置及び方向が推定されると、カルマンフィルタ演算手段８５が自車両の位置及び方向の最尤位置を出力する。図４（ｄ）は、Ａのセンサ系統による推定位置２６及び推定方向と、Ｂのセンサ系統による地図データ推定位置２７及び累積推定方向２８とに基づき演算される最終推定位置２９を示す図である。最終推定位置２９には方向を示す情報が含まれる。

図４（ｄ）では２つのセンサ系統の誤差分散を、推定位置２６と地図データ推定位置２７に凸部を有する分散で示したが、分散は実際には３次元の広がりを有する。図３（ｂ）にはその分散及びカルマンフィルタによる最尤位置の推定のイメージを示した。カルマンフィルタは、１つの系の状態がそれぞれ独立に推定される場合、状態の確率密度の分布に基づき最も確率の高い状態（分布の積が最大になる状態）を推定する。したがって、２つの系統の測位情報をカルマンフィルタでカップリングすることで、存在する確率が最も高い位置を推定することができる。

続いて、カルマンフィルタに用いる観測方程式について説明する。
図５はカルマンフィルタに用いる観測方程式を説明するための図である。適当な座標系（例えば、Ｙ方向が北向き）を設定してＸ_ｔを自車両の実際の自車位置、走行方向とすれば、Ｘ_ｔは２次元平面の位置（ｘ、ｙ）及び走行方向θの関数で表される。

Ｘ_ｔ＝（ｘ、ｙ、θ） …（１）
また、太い実線はリンク２３を示し、ｘ軸に対するリンク２３の傾きをαとする。また、リンク２３の交差点ノード位置がｙ軸と交差する点を（０、ｃ）とした。このようにリンク２３を取ると、リンク２３は次のような走路の式により表される。

-ｘ・ｓｉｎα＋ｙ・ｃｏｓα＋ｃ＝０ …（２）
Ｘ_ｔのリンク２３からの傾きをφ、Ｘｔとリンク２３との距離をｒとする。傾きφはヨーレートセンサ３の検出値により方向検出手段８４が検出する値であり、距離ｒはＧＰＳ電波遮断直前のオフセット量ｒである。なお、図５ではαは反時計回りが正であり、φは時計回りが正である。

したがって、まず、リンク２３からの距離ｒがゼロになるように観測方程式を立てることができる。また、リンク２３に対する走行方向の傾きθがゼロになるように観測方程式を立てることができる。

そこで、観測量Ｓを
Ｓ＝（α、ｃ、ｒ、φ ）
とすると、観測方程式は式（３）により表すことができる。なお、式（３）に示すように観測方程式は２行１列の行列である。

上側の要素は走路からの距離の関係式を、下側の要素は走路からの傾きの関係式をそれぞれ示す。また、自車両が走行直線より下側にある場合、上側の要素は「ｘ・ｓｉｎα−ｙｃｏｓα−ｃ−ｒ」となる。

式（３）の観測方程式のカルマンフィルタへの適用については後に詳細に説明する。

図６は測位装置９がＧＰＳ電波の捕捉状況に応じて自車位置を測位する手順を示すフローチャート図である。

測位装置９は例えばＧＰＳ測位手段８１の測位間隔毎にＧＰＳ電波が遮断されたか否かを判定する（Ｓ１）。ＧＰＳ電波が遮断されていなければ（Ｓ１のＮｏ）、ＧＰＳ電波を利用して自車両の位置を測位する（Ｓ２）。

ＧＰＳ電波が遮断された場合（Ｓ１のＹｅｓ）、ＩＮＳ測位手段８２は車速及び舵角に基づき、初期位置２４及び初期方向に走行距離及び走行方向を累積して自律航法による推定位置及び推定方向を推定する（Ｓ３）。なお、ＩＮＳ測位手段８２は、車速センサ２及び舵角センサ４の誤差等に応じて、これらに基づき累積した推定位置２６、推定方向の分散を算出する。

また、方向検出手段８４は、ＧＰＳ電波が遮断される前の初期方向にヨーレートの積分値を累積して、自車両の累積推定方向を検出する（Ｓ４）。なお、方向検出手段８４は、ヨーレートセンサ３の検出誤差等に応じて累積推定方向の分散を算出する。

また、地図データ測位手段８３は、ＩＮＳ測位手段８２が推定した推定位置に基づき地図ＤＢ５を参照し、対応したリンク２３からオフセット量ｒを隔てた線上のいずれかに地図データ推定位置を検出する（Ｓ５）。

ついで、カルマンフィルタ演算手段８５は、Ａのセンサ系統による推定位置及び推定方向、Ｂのセンサ系統による地図データ推定位置及び累積推定方向、をカルマンフィルタによりカップリングする（Ｓ５）。

以上のようにして、２つのセンサ系統から独立に検出された位置及び方向により確率的に最も高い最終推定位置を出力することができる。なお、ステップＳ４とＳ５は順不同である。

自律航法による位置推定は、自車両の走行経過を反映しているという長所と、時間経過により誤差が拡大するという短所を有し、地図ＤＢを規範とした位置推定は、地図データにより自車両の位置を拘束できるという長所と、地図データの精度に起因する誤差を含むという短所を有する。

本実施形態の測位装置９は、これら２つの系統から推定された位置の誤差を考慮して、カルマンフィルタに適用して最尤位置を推定するので、双方の長所を取り出すと共に短所を補うことができる。したがって、地図データを使用して、自律航法により検出された位置を精度よく補正することができる。

ＧＰＳ電波が遮断されていても精度のよい測位が可能であるので、交差点等における運転支援（車両制御、注意喚起等）を適切なタイミングで運転者に提供することができる。
〔カルマンフィルタへの適用〕
カルマンフィルタに観測方程式（３）を適用するため観測方程式（３）の操作について説明する。

まず、観測方程式（３）を線形化する。このため、観測方程式（３）を時刻ｔにおける推定位置

の周りでＴａｙｌｏｒ展開する。

また、Ｊ’_x、Ｊ’_ｓは観測方程式を偏微分したものである。

式（４）を変形して次の式が得られる。

ここで、Ｊ_x、−Ｗｓ、Ｊ_ｓ、はそれぞれ次のように表すことができる。

また、カルマンフィルタでは、状態方程式及び観測方程式ともに線型モデルを前提にしているため、Ｘ_ｓを次のように定義する。

そしてＪ_ｘを乗じたｍ次元空間で考えれば、線型モデルにより記述することができる。式（７）から、
−Ｗ_ｓ＝Ｘ_ｓ＋Ｊ_ｓΔＳ …（８）
式（８）が線型モデルである。

観測量の測定誤差ΔＳは白色ガウス分散であると仮定されるので、その平均Ｅ（ΔＳ）＝０である。従って、
Ｅ（Ｊ_ｓΔＳ）＝０
Ｅ（Ｊ_ｓΔＳΔＳ^TＪ_s ^T）＝Ｊ_ｓＥ（ΔＳΔＳ^T）Ｊ_ｓ ^T＝Ｊ_ｓＱ_ｓＪ_ｓ ^T≡Ｑ’_s…（９）
自律航法により算出された推定位置の平均・分散を、Ｊ_ｘを乗じたｍ次元空間で考えると次のように表すことができる。

以上から最尤推定値は次にようにして求められる。

式（１２）をＸ＝（ｘ、ｙ、θ）の空間に戻すため、逆写像をかける。

〔測位装置９による測定結果〕
図７は、一般道路においてＧＰＳ電波が遮断された状況における測位装置９の測位結果を示す。測位した時間は、ＧＰＳ電波が遮断されていた時間とほぼ等しく約９５０秒である。

オフセット量ｒは標準偏差に相当する１０〔ｍ〕、観測量Ｓ＝（α、ｃ、ｒ、φ）の誤差共分散行列Ｑｓは次のように設定した。なお、誤差共分散行列Ｑｓはα、ｃ、ｒ、φの順番である。

図７（ａ）は別の系統で測位した実際の走路（真値とする）に対し、自律航法により検出した推定位置と本実施形態の測位装置９が測位した位置のズレの大きさの実験結果である。図７（ａ）では測位時間（走行時間）を横軸にして、累積誤差平均の距離〔ｍ〕を縦軸に示した。

図７（ａ）に示すように、自律航法により検出した推定位置では真値に対し３０ｍ以上ずれることがあるのに対し、本実施形態の測位装置では９５０秒走行しても１０ｍ程度しか真値に対しずれないことが分かる。

また、図７（ｂ）には、Ｉ．別の系統で測位した実際の走路、ＩＩ．自律航法により検出した推定位置、及び、ＩＩＩ．本実施形態の測位装置９が測位した位置、がそれぞれ示されている。なお、黒丸点は交差点である。なお、図７（ｂ）では、Ｉ．別の系統で測位した実際の走路は、地図ＤＢ５から抽出した走路（リンク：細実線）にほぼ重畳している。

自律航法により検出した推定位置では、リンクに沿った経路ではあるもの大きく外れることがあるため、マップマッチングするとしてもマッチングさせる際に誤差を含まざるを得ない。しかし、本実施形態の測位装置９が測位した位置は高精度に実際の走路と一致していることが分かる。また、図７（ｂ）に示すように、本実施形態の位置補正は、特に進行方向に垂直な方向の位置ずれの補正に大きな効果がある。

地図データのリンク情報と実際の道路形状のズレの一例を示す図である。 測位装置を適用したナビゲーションシステムの概略構成図である。 測位装置の機能ブロック図、カルマンフィルタのカップリングイメージを示す図である。 カルマンフィルタにカップリングするため、２つのセンサ系統により検出されるそれぞれの位置を示す図である。 カルマンフィルタに用いる観測方程式を説明するための図である。 測位装置がＧＰＳ電波の捕捉状況に応じて自車位置を測位する手順を示すフローチャート図である。 測位装置により実際に測位した実験結果を示す図である。

符号の説明

１ ＧＰＳ受信装置
２ 車速センサ
３ ヨーレートセンサ
４ 舵角センサ
５ 地図データベース
６ 入力装置
７ 表示装置
８ ナビＥＣＵ
９ 測位装置
１０ ナビゲーションシステム

Claims (4)

1. ＧＰＳ等の電波航法測位手段により移動体の位置を検出する測位装置において、
   前記移動体の挙動情報を検出する第１の自律センサと、
   前記電波航法測位手段による測位結果に第１の自律センサによる検出情報を累積して前記移動体の推定位置を検出する位置検出手段と、
   前記位置検出手段による前記推定位置に基づき地図データ記憶手段を参照し、前記推定位置に対応するリンクから所定距離の地図データ推定位置を検出する地図データ測位手段と、
   前記推定位置及び前記地図データ推定位置をカルマンフィルタに適用して前記移動体の前記位置を推定する最尤位置推定手段と、
   を有することを特徴とする測位装置。
2. 前記移動体の挙動情報を検出する第２の自律センサと、
   前記電波航法測位手段による測位結果に第２の自律センサによる検出情報を累積して累積推定方向を検出する方向検出手段と、を有し、
   前記最尤位置推定手段は、前記推定位置及び前記地図データ推定位置に加え、前記位置検出手段検出する推定方向及び前記累積推定方向、をカルマンフィルタに適用して前記移動体の前記位置を推定する、
   ことを特徴とする請求項１記載の測位装置。
3. 前記第１の自律センサは舵角センサ及び車速センサ、前記第２の自律センサはヨーレートセンサであって、
   前記カルマンフィルタの観測方程式は、
   

   

   
   
   であることを特徴とする請求項１又は２記載の測位装置。
   
   
4. 請求項１ないし３いずれか記載の測位装置を適用したナビゲーションシステム。
   
   
   

Images