JP2000346663A - ロケータ装置 - Google Patents

Abstract

(57)【要約】 【課題】 簡単な方法でマップマッチングが行え、安定
に車両の現在位置と進行方位を提供できるロケータ装置
を得る。 【解決手段】 道路ネットワークを記憶した道路網記憶
手段５２、道路ネットワークを読み出して管理する道路
網読出し手段５３、位置・方位推定手段５６５で推定し
た現在位置近傍の道路ネットワークに走行道路と現在位
置の候補を設定して管理する走行道路・位置候補管理手
段５６６、および走行道路と現在位置の候補の中から走
行道路と現在位置を特定する走行道路・位置特定手段５
６７を備えて、位置・方位推定手段５６５の推定した現
在位置と進行方位より、近傍の道路ネットワーク上に車
両の現在位置を特定するマップマッチングを行う。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】この発明は、ＧＰＳ（Ｇｌｏ
ｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信
機の観測情報と、自律航法用複合センサの計測情報を用
いて、車両の現在位置を道路ネットワーク上に特定する
マップマッチングを行うロケータ装置に関するものあ
る。

【０００２】

【従来の技術】図１６は、例えば、特開平１０−４７９
８３号公報に示された、従来のロケータ装置の構成を示
すブロック図である。図において、１は車両の移動距離
に応じた信号を出力する距離センサ、２は車両の進行方
位に応じた信号を出力する方位センサであり、３は絶対
位置観測手段としてのＧＰＳ受信機およびアンテナであ
る。なお、この距離センサ１および方位センサ２にて、
自律航法用複合センサを形成している。５２は道路網記
憶手段（地図専用ディスク）、５３は道路網記憶手段
（地図専用ディスク）５２に格納されている道路ネット
ワークを取得する道路網読出し手段（ＣＤドライブ）で
あり、５４は道路ネットワークと諸処理データのメモリ
への保持を制御するメモリ制御部、５５は車両の位置や
方位などの情報の出力を制御する出力制御部である。ま
た、５１１は距離センサ１の出力信号から車両の移動距
離を演算する距離取得手段、５１２は方位センサ２の出
力信号から車両の進行方位を演算する方位取得手段、５
１３はＧＰＳ受信機３で観測した車両の現在位置を受信
する位置取得手段であり、５１５から５１７は道路ネッ
トワーク上に車両の現在位置を特定するマップマッチン
グを行うための移動位置算出手段、ルートおよび推定位
置算出手段、表示用現在位置選択手段である。

【０００３】次に動作について説明する。ここでは、マ
ップマッチングを行う移動位置算出手段５１５、ルート
および推定位置算出手段５１６、表示用現在位置選択手
段５１７の動作について、図１７〜図２０を用いて説明
する。ここで、図１７は車両の現在位置の移動点の算出
を示す説明図、図１８はＧＰＳ位置を近傍のリンク上へ
投影した投影点の算出を示す説明図、図１９はルートの
算出を示す説明図、図２０は現在位置の候補点と移動
点、およびＧＰＳ位置の投影点のそれぞれの誤差に基づ
くカルマンフィルタにて現在位置の推定点が決まる様子
を示す説明図である。

【０００４】移動位置算出手段５１５は、メモリ制御部
５４が道路網読出し手段（ＣＤドライブ）５３から得た
道路ネットワーク上において、車両の現在位置の候補点
を起点として、方位取得手段５１２から得られる車両の
進行方位に合致する道路に沿って、距離取得手段５１１
から得られる距離情報分だけ前方に現在位置の移動点を
算出する。図１７では、現在位置の候補点Ｓ_n-1,i から
移動距離ｍの地点に位置する２つの現在位置の移動点Ｍ
_n,i,k （ｋは１と２）を算出している。

【０００５】次に、ルートおよび推定位置算出手段５１
６は、位置取得手段５１３の取得したＧＰＳ位置から近
傍の道路ネットワーク上にＧＰＳ位置の投影点を設定
し、現在位置の移動点とＧＰＳ位置の投影点の組合せか
らなるルートを１つ以上算出する。図１８では、ＧＰＳ
位置ｇ_n,i 点に対して、近傍の道路である２つのリンク
の上にＧ_n,i,j 点（ｊは１と２）を算出している。また
図１９は、リンク番号＃１、＃２、＃３のリンクの上に
現在位置の移動点Ｍ_n,i,1 、Ｍ_n,i,2 、Ｍ_n,i,3がそれ
ぞれ位置し、リンク番号＃１、＃２、＃３のリンクから
道路ネットワーク化されているリンク＃１１、＃１２の
リンク上にＧＰＳ位置ｇ_n,i 点の投影点Ｇ _n,i,1 、Ｇ
_n,i,2 がそれぞれ位置している状態を示す。この場合、
現在位置の各移動点からＧＰＳ位置の各投影点へのルー
トとしては、最大６ルート（例、＃１から＃１１、＃１
から＃１２、＃２から＃１１、…）を求めることができ
る。そして、次に示す式（１）によって、各ルート上の
内分点である位置に現在位置の推定点Ｓ_n,i,r を算出す
る。なお、この場合の係数Ｋは０.５とする。

【０００６】 Ｓ_n,i,r ＝（１−Ｋ）×Ｍ_n,i.k ＋Ｋ×Ｇ_n,i,j ・・・（１）

【０００７】その結果、最大、現在位置の候補点の数と
ルート数の積で示される個数の現在位置の推定点が算出
される。また式（１）における係数Ｋは、現在位置の移
動点に係わる誤差Ｅ_msとＧＰＳ位置に係わる誤差Ｅ_g と
から構成されるカルマンフィルタにより、次に示す式
（２）にて算出するものである。

【０００８】 Ｋ＝Ｅ_ms ² ／（Ｅ_ms ² ＋Ｅ_g ²） ・・・（２）

【０００９】そして、図２０に示すように、現在位置の
候補点の誤差Ｅ_s と距離センサ１に係わる誤差Ｅ_m から
現在位置の移動点に係わる誤差Ｅ_msを次の式（３）にて
算出する。

【００１０】 Ｅ_ms＝（Ｅ_s ²＋Ｅ_m ²）^1/2 ・・・（３）

【００１１】その後、表示用現在位置選択手段５１７
は、これら現在位置の複数の推定点の中から新しい現在
位置を特定して出力する。また、この特定に際しては、
現在位置の各推定点からＧＰＳ位置までの距離の評価を
行い、結果が最もよい現在位置の推定点を基準に所定数
の新たな現在位置の候補点を選択する。そして、この新
たな現在位置の候補点からＧＰＳ位置への方位による評
価をさらに行い、結果が最もよい現在位置の候補点を現
在位置と特定する。

【００１２】

【発明が解決しようとする課題】従来のロケータ装置は
以上のように構成されているので、車両の現在位置の特
定は、車両の進行方位に合致する方向に接続されている
道路ネットワークのリンク上で移動距離分だけ前方に算
出した現在位置の移動点とＧＰＳ位置をリンク上に投影
した投影点の間に、現在位置の推定点を算出することに
よって行われ、また、この現在位置の特定に際しては、
距離誤差や各種位置の誤差による比をカルマンフィルタ
で算出して、ＧＰＳ位置による修正量を決めているた
め、ＧＰＳ受信機３と自律航法用複合センサ（距離セン
サ１と方位センサ２）の両情報に基づいて推定した２つ
の位置情報をマップマッチングで個別に使用し、その処
理が複雑になるという課題があり、また、ＧＰＳ位置の
分散が大きくなる場合は、ＧＰＳ位置の投影先となるリ
ンクの履歴が接続関係にないものになったり、同一リン
ク上でＧＰＳ位置の投影点が大きく前後に移動したりす
ることがあり、現在位置の特定が不連続または不安定に
なるなどの課題もあった。

【００１３】さらに、車両の進行方位については、現在
位置が特定されているリンクのリンク方位が基礎となっ
て更新されるため、誤ったリンク上に現在位置を特定し
た場合には車両の進行方位が不正になり、以後のリンク
上への位置更新が適確に行えなくなるばかりか、その後
にマップマッチングが破綻したことに気付いても、ＧＰ
Ｓ位置の分散により車両の初期位置が不安定になるとい
った課題もあった。

【００１４】この発明は上記のような課題を解決するた
めになされたもので、絶対位置観測手段の観測情報と自
律航法用複合センサの計測情報に基づいて、道路ネット
ワーク上に車両の現在位置を特定するマップマッチング
を、簡単な方法で実現できるロケータ装置を得ることを
目的とする。

【００１５】また、この発明は、マップマッチング法に
係わる手段で扱う推定位置を、絶対位置観測手段の観測
情報と自律航法用複合センサの計測情報に基づいて一本
化した車両の推定位置だけとするロケータ装置を得るこ
とを目的とする。

【００１６】さらに、この発明は、道路分岐点などで効
率よくマップマッチングを行うことができるロケータ装
置を得ることを目的とする。

【００１７】さらに、この発明は、屈曲路などで車両の
現在位置を高精度で特定できるロケータ装置を得ること
を目的とする。

【００１８】さらに、この発明は、道路ネットワークが
未整備な箇所を走行した際に、車両の現在位置を連続
的、かつ円滑に推定することができるロケータ装置を得
ることを目的とする。

【００１９】さらに、この発明は、絶対位置観測手段の
測位率が低下する場所でも車両の現在位置を高精度で特
定できるロケータ装置を得ることを目的とする。

【００２０】

【課題を解決するための手段】この発明に係るロケータ
装置は、所定領域の道路ネットワークを道路網記憶手段
に記憶しておき、道路網読出し手段にてこの道路網記憶
手段から、位置・方位推定手段で推定した車両の現在位
置が存在する領域と隣接領域の道路ネットワークを読み
出した上で、推定した車両の現在位置の近傍に存在する
道路ネットワークのリンクについて、そのリンクがリン
クの両端を直線で結んだ直線データであれば、交差点
間、一本道、および行き止まり道などの無分岐区間を示
す接続関係にある複数のリンクをそれぞれリンク群とし
てまとめて、また折れ線データであればそのリンクをリ
ンク群として、道路網読出し手段において管理し、さら
に走行道路・位置候補管理手段で、その推定した車両の
現在位置をリンク群に投影した際の構成リンクへの最短
距離が所定値以下で、かつ同一のリンク群および接続関
係にあるリンク群上で位置投影し続けた距離が所定値以
上になる接続関係にある複数のリンク群と当該リンク群
上の投影点を、車両の走行道路と現在位置の候補として
設定して管理し、さらに、走行道路・位置特定手段によ
って、走行道路と現在位置の候補の中から、位置投影し
続けた距離が最長となる走行道路候補を走行道路と判断
して、その走行道路のリンク群上の投影点を車両の現在
位置として特定するようにしたものである。

【００２１】この発明に係るロケータ装置は、上記道路
網記憶手段、位置・方位推定手段、道路網読出し手段、
走行道路・位置候補管理手段、および走行道路・位置特
定手段を有したロケータ装置の位置・方位推定手段に
て、自律航法用複合センサにより計測した単位時間当た
りの移動距離と進行方位変化から、自律航法的な位置を
カルマンフィルタ方程式に基づいて推定する一方で、そ
の自律航法的な位置を、絶対位置観測手段で観測した車
両の現在位置の方へ、カルマンゲインで決められた割合
だけ寄せるように計算して、車両の現在位置と進行方位
とを推定するようにしたものである。

【００２２】この発明に係るロケータ装置は、上記道路
網記憶手段、位置・方位推定手段、道路網読出し手段、
走行道路・位置候補管理手段、および走行道路・位置特
定手段を有したロケータ装置の走行道路・位置候補管理
手段が、走行道路候補毎に位置・方位推定手段の推定し
た車両の進行方位に合致する方向で、走行道路候補の構
成リンク群に接続可能な複数のリンク群を、所定距離ま
たは所定数だけ先読みして保持しておき、そのリンク群
を位置・方位推定手段で推定した車両の現在位置を投影
する対象として用いるようにしたものである。

【００２３】この発明に係るロケータ装置は、上記道路
網記憶手段、位置・方位推定手段、道路網読出し手段、
走行道路・位置候補管理手段、および走行道路・位置特
定手段を有したロケータ装置の走行道路・位置候補管理
手段にて、位置・方位推定手段の推定した車両の現在位
置を中心とする所定範囲内に道路分岐点、あるいは屈曲
路がある場合、推定した車両の現在位置に関する所定距
離または所定時間以上前からの軌跡形状と、道路ネット
ワークで接続関係にあるリンク群の形状または接続関係
を比較して、リンク群への投影点の調整、およびリンク
群の選択を行うようにしたものである。

【００２４】この発明に係るロケータ装置は、上記道路
網記憶手段、位置・方位推定手段、道路網読出し手段、
走行道路・位置候補管理手段、および走行道路・位置特
定手段を有したロケータ装置の走行道路・位置候補管理
手段にて、位置・方位推定手段で推定した車両の進行方
位に合致する方向に走行道路候補のリンク群を前方接続
できなくなった時点の投影点と、推定した現在位置の位
置関係に基づいて、以後、所定時間が経過するまで、ま
たは所定距離を走行するまでの間は、投影点の前方に現
在位置の候補を更新し、所定時間が経過した後、または
所定距離を走行した後は、他に走行道路候補があれば当
該走行道路候補を抹消し、また他に走行道路候補がなけ
れば、推定した現在位置の方向へ現在位置の候補を寄せ
るような位置更新を行うようにしたものである。

【００２５】この発明に係るロケータ装置は、上記道路
網記憶手段、位置・方位推定手段、道路網読出し手段、
走行道路・位置候補管理手段、および走行道路・位置特
定手段を有したロケータ装置の走行道路・位置特定手段
にて、絶対位置観測手段の所定時間における測位率が所
定値以下になった時には、その状態になる直前に道路ネ
ットワーク上に特定した車両の現在位置を起点にして、
位置・方位推定手段で用いた車両の単位時間毎の移動距
離と進行方位変化をもとに、道路ネットワーク上で現在
位置の更新を行うようにしたものである。

【００２６】

【発明の実施の形態】以下、この発明の実施の一形態を
説明する。 実施の形態１．図１はこの発明の実施の形態１によるロ
ケータ装置の構成を示すブロック図である。図におい
て、１は車両の移動距離を検出する距離センサであり、
車両の走行距離に応じてパルス信号を出力する。２ａは
車両の方位変化を検出する相対方位センサであり、この
実施の形態１ではジャイロを用いて車両のヨーレートに
応じた電圧を出力する。なお、これらは図１６に示した
距離センサ１および方位センサ２に相当するもので、こ
れらによって自律航法用複合センサが形成される。３は
図１６に同一符号を付して示したものと同等の、絶対的
位置観測手段としてのＧＰＳ受信機であり、衛星からの
電波を受信するＧＰＳアンテナが接続され、このＧＰＳ
アンテナによって受信された衛星からの電波より車両の
絶対的な現在位置を観測し、その位置情報（ＧＰＳ位
置）を出力する。５は予めメモリ（図示省略）に記憶し
た制御プログラムに従って車両の進行方位と現在位置を
演算するコンピュータを含む信号処理器である。

【００２７】上記信号処理器５内において、５６１は距
離センサ１からのパルス信号に基づいて車両の速度と移
動距離を計算する距離・速度計算手段であり、５６３は
相対方位センサ２ａのオフセットを検出するオフセット
検出手段である。５６４はこのオフセット検出手段５６
３からの検出信号と相対方位センサ２ａからの車両のヨ
ーレートに応じた電圧とが入力されて、車両の進行方位
変化を計算する方位変化計算手段である。５６５はこの
距離・速度計算手段５６１で計算された車両の速度と移
動距離と、方位変化計算手段５６４で計算された車両の
進行方位変化に基づいて車両の現在位置と進行方位を推
定する位置・方位推定手段である。

【００２８】また、５２は所定範囲の道路ネットワーク
を記録している道路網記憶手段であり、５３はこの道路
網記憶手段５２から指定範囲の道路ネットワークを読み
出す道路網読出し手段である。この道路網記憶手段５２
および道路網読出し手段５３は、図１６に示した道路網
記憶手段（地図専用ディスク）５２と道路網読出し手段
（ＣＤドライブ）５３に相当するものであるが、位置・
方位推定手段５６５で推定した車両の現在位置近傍の道
路ネットワークのリンクを検索し、当該リンクが直線デ
ータであれば無分岐区間を示す接続関係にあるリンクを
一つのリンク群としてまとめ、折れ線データであればそ
のリンクをリンク群として管理する機能も備えている。
５６６はこの道路網読出し手段５３によって道路網記憶
手段５２より読み出された道路ネットワークと、位置・
方位推定手段５６５にて推定された位置および方位とに
基づいて、車両の走行道路と現在位置の候補を複数個管
理する走行道路・位置候補管理手段であり、５６７はこ
の走行道路・位置候補管理手段５６６が管理する車両の
走行道路および現在位置の候補と、位置・方位推定手段
５６５にて推定された位置および方位から、車両の走行
道路と現在位置を特定する走行道路・位置特定手段であ
る。

【００２９】次に動作について説明する。以下、信号処
理器５の各手段の動作について図を用いて説明する。こ
こで、図２はそのメインルーチンの処理内容を示す流れ
図であり、図３は位置・方位推定手段５６５の処理内容
を示す流れ図、図４はこの位置・方位推定手段５６５に
よる車両の現在位置の推定例を示す説明図である。図５
は所定時間Δｔ毎に実行される割込処理１の内容を示す
流れ図、図６は距離センサ１から信号が出力されたとき
に実行される割込処理２の内容を示す流れ図、図７はＧ
ＰＳ受信機３から観測周期毎に出力された観測情報を受
信する割込処理３の内容を示す流れ図である。また、図
８は道路ネットワークのリンクが直線データで示されて
いた場合に、交差点間のリンク、行き止まり道路のリン
ク、あるいは長い１本道におけるリンクのような無分岐
区間の接続関係にある複数のリンクを一つのリンク群と
して道路網読出し手段５３が管理する際の状態を示す説
明図、図９は特に一本道において道路網読出し手段５３
がリンク群を管理する際の状態を示す説明図、図１０は
車両の走行道路と現在位置の候補を走行道路・位置候補
管理手段５６６が管理する際の状態を示す説明図であ
る。

【００３０】図２において、メインルーチンは信号処理
器５の全手段の動作を管理する。まずステップＳＴ２０
１で全ての処理を初期化する。次のステップＳＴ２０２
では処理開始のタイミングか否かを処理開始フラグを参
照して判断する。その結果、当該処理開始フラグがセッ
トされていれば、処理開始タイミングとしてステップＳ
Ｔ２０３へ進み、クリアされていれば処理開始タイミン
グでないとして処理開始フラグがセットされるまで待機
する。次にステップＳＴ２０３で次回の処理のために処
理開始フラグをクリアした後、ステップＳＴ２０４に進
む。ステップＳＴ２０４では距離・速度計算手段５６１
の処理として、所定時間Δｔ毎に、割込処理２でカウン
トした距離センサ１の出力信号カウント値ΔＮ_i から、
次に示す式（４）によって移動距離Δｄ_i を、式（５）
によって速度Ｖｅｌ_DRi をそれぞれ計算するとともに、
次回の処理のために出力信号カウント値ΔＮ_i を０にす
る。なお、この式（４）と式（５）におけるＳＦ
_SPDjは、距離センサ１の出力信号を距離に変換するスケ
ールファクタである。

【００３１】 Δｄ_i ＝Δｄ_i-1 ＋ΔＮ_i ×ＳＦ_SPDi ・・・（４） Ｖｅｌ_DRj ＝｜ΔＮ_j ×ＳＦ_SPDj｜／ΔＴ_GPS ・・・（５）

【００３２】次に、ステップＳＴ２０５においてオフセ
ット検出手段５６３によるオフセット計算処理が行われ
る。このステップＳＴ２０５では、ステップＳＴ２０４
で計算した速度Ｖｅｌ_DRi が０（すなわち、停車中）の
時は、停車中における相対方位センサ２ａの所定時間分
以上の出力信号ＳＧ_GYRiを平均したものをオフセットＯ
Ｆ_GYRiとする。次にステップＳＴ２０６において方位変
化計算手段５６４による進行方位変化の計算処理が行わ
れる。このステップＳＴ２０６では、次に示す式（６）
を用いて、所定時間Δｔ毎に計測した相対方位センサ２
ａの出力信号ＳＧ_GYRiから進行方位変化Δθ_i を計算す
る。なお、この式（６）中のＯＦ_GYRiはオフセットであ
り、ＳＦ_GYRiは出力信号を角速度に変換するスケールフ
ァクタである。

【００３３】 Δθ_i ＝Δθ_i-1 ＋（ＳＧ_GYRi−ＯＦ_GYRi）×ＳＦ_GYRi×Δｔ ・・・（６）

【００３４】次にステップＳＴ２０７において、ＧＰＳ
受信機３から観測情報の受信を完了したか否かを、ＧＰ
Ｓ観測情報受信フラグを参照して判断する。その結果、
当該ＧＰＳ観測情報受信フラグがセットされていれば、
ＧＰＳ受信機３からの観測情報の受信が完了したとして
ステップＳＴ２０８へ進み、クリアされていればステッ
プＳＴ２０２へ戻る。ステップＳＴ２０８では次回の処
理のためにＧＰＳ観測情報受信フラグのクリアを行う。
次に、ステップＳＴ２０９にて位置・方位推定手段５６
５による位置・方位の推定処理が行われる。このステッ
プＳＴ２０９では、後述する計算式（カルマンフィル
タ）にて車両の現在位置(λ_i ，φ_i )と進行方位θ_i を
推定する。次にステップＳＴ２１０に進み、後述するマ
ップマッチング法にて道路ネットワーク上に車両の現在
位置(λ_i ，φ_i )を特定する。次にステップＳＴ２１１
で車両の現在位置(λ_i ，φ_i )と進行方位θ_i および速
度Ｖｅｌ_DRi を出力する。その後、ステップＳＴ２１２
においてＧＰＳ受信機３から観測情報を受信する間の移
動距離Δｄ_i と進行方位変化Δθ_i を０にしてステップ
ＳＴ２０２へ戻る。

【００３５】位置・方位推定手段５６５は、車両の移動
距離Δｄ_i と進行方位変化Δθ_i から現在位置(λ_i ，
φ_i )と進行方位θ_i を、次に示す式（７）〜式（１
２）で計算するシステムモデルと、ＧＰＳ位置(λ_Gi，
φ_Gi)とそのシステムモデルの車両の現在位置(λ_i ，φ
_i )の関係を次の式（１３）〜式（１４）で表す観測モ
デルとに基づくものである。

【００３６】 λ_i ＝λ_i-1 ＋Δｄ_i ×ｓｉｎ｛θ_i-1 ＋Δθ_i ｝×ＳＦ_{d →λ}＋δλ_i ・・・（７） φ_i ＝φ_i-1 ＋Δｄ_i ×ｃｏｓ｛θ_i-1 ＋Δθ_i ｝×ＳＦ_{d →φ}＋δφ_i ・・・（８） δλ_i ＝｛δΔｄ_i ×ｓｉｎθ_i ＋δΔθ_i ×Δｄ_i ×ｃｏｓθ_i ｝ ×ＳＦ_{d →λ} ・・・（９） δφ_i ＝｛δΔｄ_i ×ｃｏｓθ_i −δΔθ_i ×Δｄ_i ×ｓｉｎθ_i ｝ ×ＳＦ_{d →φ} ・・・（１０） ｓｉｎ{θ_i-１ ＋Δθ_i }＝ｃｏｓθ_i-1 ・ｓｉｎΔθ_i ＋ｓｉｎθ_i-1 ・ｃｏｓΔθ_i ・・・（１１） ｃｏｓ{θ_i-1 ＋Δθ_i }＝ｃｏｓθ_i-1 ・ｃｏｓΔθ_i −ｓｉｎθ_i-1 ・ｓｉｎΔθ_i ・・・（１２）

【００３７】 λ_Gi＝λ_i ＋δλ_Gi ・・・（１３） φ_Gi＝φ_i ＋δφ_Gi ・・・（１４）

【００３８】なお、上記式（７）〜式（１２）と式（１
３）〜式（１４）において、ｉは離散時刻であり、λ
_i-１ とφ_i-１ は離散時刻ｉ-１における車両の現在位
置の経度と緯度、θ_i-1 は離散時刻ｉ-１における車両
の進行方位である。ＳＦ_{d →λ}とＳＦ_{d →φ}は移動距離
の単位［ｍ］を経度方向または緯度方向の移動量［″］
へ変換する係数である。またδλ_i とδφ_i は車両の現
在位置の経度誤差と緯度誤差、δΔθ_i とδΔｄ_i は進
行方位変化Δθ_i と移動距離Δｄ_i の誤差である。また
δλ_Giとδφ_GiはＧＰＳ位置λ_Giとφ_Giの誤差である。

【００３９】そして、次の式（１５）〜式（１９）に示
す状態方程式と式（２０）〜式（２３）に示す観測方程
式、および式（２４）〜式（２８）に示すカルマンフィ
ルタ方程式によって、車両の現在位置(λ_i ，φ_i )と進
行方位θ_i を推定する。

【００４０】

【数１】

【００４１】

【数２】

【００４２】 ｘ_i|i ＝ｘ_i|i-1 ＋Ｋ_i ｛ｙ_i −（Ｈｘ_i|i-1 ＋ｖ_i ）｝ ・・・（２４） ｘ_i+1|i ＝Ｆ_i ｘ_i|i ＋Ｇ_i ｗ_i ・・・（２５） Ｋ_i ＝Σ_i|i-1 Ｈ^T ［ＨΣ_i|i-1 Ｈ^T ＋Σ_vi］^-1 ・・・（２６） Σ_i|i ＝Σ_i|i-1 −Ｋ_i ＨΣ_i|i-1 ・・・（２７） Σ_i+1|i ＝Ｆ_i Σ_i|i Ｆ_i ^T＋Ｇ_i Σ_ｗiＧ_i ^T ・・・（２８）

【００４３】なお、上記式（１５）〜式（１９）および
式（２０）〜式（２３）において、ｘ_i 、Ｆ_i 、Ｇ_i 、
ｗ_i 、ｙ_i 、ｖ_i は、離散時刻ｉにおける状態ベトク
ル、状態遷移行列、駆動行列、システム雑音、観測値、
観測雑音であり、Ｈは観測行列である。また、式（２
４）〜式（２８）において、ｘ_i|i は離散時刻ｉにおけ
る状態ベクトルの推定量、ｘ_i+1|i は離散時刻ｉにおけ
る離散時刻ｉ＋１の状態ベクトルの予測量、Ｋ_i はカル
マンゲイン、Σ_i|i は離散時刻ｉにおける状態ベクトル
の共分散行列の推定量、Σ_i+1|i は離散時刻ｉにおける
離散時刻ｉ＋１の共分散行列の予測量、Σ_viは観測誤差
の共分散行列、Σ_wiはシステム誤差の共分散行列である
が、行列の各要素は式の行列式を計算すれば求まるもの
なので、ここでは説明を省略する。

【００４４】次に、図３の流れ図に従って、位置・方位
推定手段５６５の処理内容をより詳細に説明する。ま
ず、ステップＳＴ３０１において、距離・速度計算手段
５６１で計算した速度Ｖｅｌ_DRi が０より大きいか否か
を判定し、０ならば停車中と判断して処理を終了し、０
より大きければステップＳＴ３０２へ進む。ステップＳ
Ｔ３０２では、ＧＰＳ受信機３が２次元あるいは３次元
測位状態で、かつ前回の処理タイミングでカルマンフィ
ルタにより車両の現在位置と進行方位を推定してから所
定距離以上移動したか否かを判定する。その結果、ＧＰ
Ｓ受信機３にて２次元あるいは３次元測位状態で、かつ
前回の処理タイミングでカルマンフィルタにより現在位
置と進行方位を推定してから所定距離以上移動した場合
はステップＳＴ３０３へ進み、逆にＧＰＳ受信機３にて
非測位状態か、あるいは前回の処理タイミングでカルマ
ンフィルタにより現在位置と進行方位を計算してから所
定距離以上移動していない場合はステップＳＴ３０５へ
進む。

【００４５】ステップＳＴ３０３では所定時間における
ＧＰＳ位置(λ_Gi，φ_Gi)とカルマンフィルタによる現在
位置(λ_i ，φ_i )の位置の差分、すなわち緯度差と経度
差についてそれぞれ標準偏差を計算し、その標準偏差を
観測誤差ｖ_i とするとともに、観測誤差の共分散行列Σ
_viを計算する。次にステップＳＴ３０４において、ＧＰ
Ｓ速度の積分値と距離センサ１から求めた距離Δｄ_i の
差分をシステム誤差の行列要素である距離誤差要素δΔ
ｄ_i とするとともに、所定距離だけ離れた複数地点での
ＧＰＳ位置と車両の現在位置の軌跡の両移動方向の方向
差をシステム誤差の行列要素である方位誤差要素δΔθ
_i とし、ステップＳＴ３０６に進む。一方、ステップＳ
Ｔ３０５ではカルマンゲインＫ_i を０にクリアしてステ
ップＳＴ３０６に進む。ステップＳＴ３０６ではＧＰＳ
位置を観測値ｙ_i とし、式（７）〜式（１２）、式（１
３）〜式（１４）、および式（１５）〜式（１９）、式
（２０）〜式（２３）、式（２４）〜式（２８）に従っ
て、状態ベクトルの推定量ｘ_i|i を計算する。なお、そ
のとき、相対方位センサ２ａのオフセット変動量に比例
した値よりカルマンゲインＫ_i の全行列要素が小さくな
るようにカルマンゲインＫ_i の全行列要素を調節して、
状態ベクトルの推定量ｘ_i|i を計算する。

【００４６】次にステップＳＴ３０７において、式（２
４）〜式（２８）を用いて状態ベクトルの予測量ｘ_i+|i
を計算する。次にステップＳＴ３０８にてステップＳＴ
３０２と同様に、ＧＰＳ受信機３で２次元あるいは３次
元測位をしており、かつ前回カルマンフィルタによって
現在位置(λ_i ，φ_i )と進行方位θ_i を推定してから所
定距離以上移動したか否かを判定する。その結果、ＧＰ
Ｓ受信機３にて２次元あるいは３次元測位をしており、
かつ前回カルマンフィルタにより現在位置(λ_i ，φ_i )
と進行方位θ_i を推定してから所定距離以上移動した場
合にはステップＳＴ３０９へ進み、ＧＰＳ受信機３にて
非測位か、あるいは前回カルマンフィルタによって現在
位置(λ_i ，φ_i )と進行方位θ_i を計算してから所定距
離以上移動していない場合には処理を終了する。ステッ
プＳＴ３０９では誤差共分散の推定量Σ_i|i を式（２
４）〜式（２８）を用いて計算し、さらに、ステップＳ
Ｔ３１０では予測量Σ_i+1|i を、ステップＳＴ３１１で
はカルマンゲインＫ_i を、それぞれ式（２４）〜式（２
８）を用いて計算して処理を終了する。

【００４７】次に、位置・方位推定手段５６５における
車両の現在位置の推定動作について図４を用いて説明す
る。まず同図（ａ）は、車両が直進走行する途中でＧＰ
Ｓ位置の分散が大きくなる時に車両の現在位置が推定さ
れる様子を示したもので、この時には、システム誤差の
行列要素である方位誤差要素と距離誤差要素がともに小
さい状態であったことを想定したものである。この場
合、位置・方位推定手段５６５はＧＰＳ位置の分散が大
きくなった区間では、観測誤差であるＧＰＳ位置の誤差
が大きくなったことを検出するので、ＧＰＳ位置の分散
が大きくなった区間のみカルマンゲインを小さく計算す
る。その結果、ＧＰＳ位置の分散が小さい区間では車両
の現在位置がＧＰＳ位置へ寄るように推定するが、ＧＰ
Ｓ位置の分散が大きい区間では車両の現在位置がＧＰＳ
位置へ寄る量を小さく抑えて推定する。

【００４８】また図４（ｂ）は、車両が直進走行する途
中で相対方位センサ２ａのオフセットがドリフトした時
に車両の現在位置が推定される様子を示したもので、こ
の時には、ＧＰＳ位置の分散が小さい状態であったこと
を想定したものである。この場合、位置・方位推定手段
５６５は、ＧＰＳ位置の分散が小さい一方で、単位時間
毎の進行方位変化が偏った値となって方位誤差が発生し
たことを検出するので、カルマンゲインを大きく計算す
る。その結果、相対方位センサ２ａに生じたドリフトの
影響を受けずに車両の現在位置をＧＰＳ位置の方向へ寄
せるように推定する。なお、カルマンフィルタを使用し
ない一般的な自律航法の場合には、相対方位センサ２ａ
に生じたドリフトの影響を受けて、一点鎖線で示すよう
に、車両の現在位置が偏った方向に徐々に旋回してしま
う。

【００４９】また図４（ｃ）は、相対方位センサ２ａの
スケールファクタが真値より小さ目の時に車両が右折し
た場合の車両の現在位置が推定される様子を示したもの
で、この時には、ＧＰＳ位置の分散が小さい状態であっ
たことを想定したものである。この場合、位置・方位推
定手段５６５は、ＧＰＳ位置の分散が小さい一方で、右
折後に方位誤差が発生したことを検出するので、右折後
にはカルマンゲインをより大きく計算する。その結果、
右折時の旋回角が小さ目になることから推定した進行方
位に誤差が生じるものの、右折後に方位誤差を検出した
以後は車両の現在位置をＧＰＳ位置の方向へ寄せるよう
に推定する。なお、カルマンフィルタを使用しない一般
的な自律航法の場合には、一点鎖線で示すように、旋回
角不足のまま車両の現在位置が誤った方向に直進してし
まう。

【００５０】次に、割込処理１について説明する。この
割込処理１は所定時間Δｔ毎に起動され、図５の流れ図
に示すように、そのステップＳＴ５０１において、処理
開始タイミングを示す処理開始フラグをセットして、こ
の割込処理１を終了する。

【００５１】次に、割込処理２について説明する。この
割込処理２は距離センサ１から信号が出力されたときに
起動され、図６の流れ図に示すように、そのステップＳ
Ｔ６０１において車速信号カウンタに１を加算して、こ
の割込処理２を終了する。

【００５２】次に、割込処理３について説明する。この
割込処理３はＧＰＳ受信機３から観測周期毎に出力され
た観測情報を受信するものであり、図７の流れ図に示す
ように、まずステップＳＴ７０１でＧＰＳ受信機３から
出力された観測情報を受信して記憶する。次にステップ
ＳＴ７０２において全ての観測情報の受信を完了したか
否かを判定し、完了していればステップＳＴ７０３へ進
み、完了していなければこの割込処理３を終了する。ス
テップＳＴ７０３では全ての観測情報を受信完了したと
して、ＧＰＳ観測情報受信フラグをセットする。

【００５３】次に、図２のステップＳＴ２１０で行うマ
ップマッチング法について、このマップマッチング法に
係わる道路網読出し手段５３の動作から説明する。道路
網読出し手段５３は、位置・方位推定手段５６５で推定
した車両の現在位置と進行方位を受け取り、推定された
現在位置が存在する領域と隣接領域の道路ネットワーク
を道路網記憶手段５２から読み出す。そして、読み出し
た道路ネットワークのリンクがその両端のノードを直線
で結ぶ直線データだった場合には、交差点間、一本道、
および行き止まり道などの無分岐区間の接続関係にある
複数のリンクを一つのリンク群として管理する。また、
読み出した道路ネットワークのリンクが折れ線データだ
った場合には、その折れ線データによるリンクをリンク
群として扱う。

【００５４】ここで、道路網読出し手段５３が、例えば
図８（ａ）に示すような道路ネットワークを道路網記憶
手段５２から読み出した場合には、同図（ｂ）に示すよ
うな、構成リンク、ノード、補間点よりなるリンク群に
編集する。この図８（ａ）において、○印は道路ネット
ワークのリンクの座標を示すノードであり、○印を直線
で結んだものが直線データによるリンクである。また、
Ｎ₁ からＮ₁₂はノード番号、Ｌ₁ からＬ₁₂はリンク番号
を示す。

【００５５】しかしながら、現実にはマップマッチング
の負荷を軽減するために、位置・方位推定手段５６５で
推定した現在位置を中心とする半径３００ｍ程度の範囲
内にあるリンクに対してリンク群の作成を行う。

【００５６】また、読み出した道路ネットワークに、例
えば、図９に示すような一本道が含まれていた場合に
は、位置・方位推定手段５６５で推定した現在位置を中
心とする所定範囲内のリンクに一本道の終端、すなわ
ち、交差点か行き止まりがなければ、上述のように無分
岐区間のリンク群を作成して管理することはできない。
そこで、道路網読出し手段５３はリンク群の構成リンク
数に上限を設けたもので、説明上、この図９の場合には
その上限値を４本としている。まず、同図（ａ）に示し
たｔ０の時点では、位置・方位推定手段５６５で推定し
た現在位置Ｐ_E-t0を中心とする所定範囲Ｒ_M 内のリンク
Ｌ₁ からＬ₃ およびＬ₈ 対して、図中の表に示すリンク
群ＬＳ₁ からＬＳ₃ を作成する。この時、リンク群ＬＳ
₃ のノードは一本道の終端ではないので暫定的なリンク
群とする。そして、同図（ｂ）に示すｔ１の時点で、位
置・方位推定手段５６５で推定した現在位置Ｐ_E-t1を中
心とする所定範囲Ｒ_M 内にリンクＬ₄ からＬ₇ が入るよ
うになった時、図中の表に示すように、リンク群ＬＳ₃
にリンクＬ₄ とＬ₅ のみを接続して、リンクＬ₆ とＬ₇
は新たなリンク群ＬＳ₄ として暫定的に管理する。

【００５７】次に、走行道路・位置候補管理手段５６６
の動作について、図１０を用いて説明する。まず、電源
リセット直後において、走行道路・位置候補管理手段５
６６は、位置・方位推定手段５６５で推定した車両の現
在位置をリンク群に投影した際の、構成リンクへの最短
距離が所定値以下になるものを走行道路の候補とし、投
影点を現在位置の候補とする。その後、車両の移動に伴
って、位置・方位推定手段５６５で推定した現在位置を
近傍のリンク群に対して次々に投影してゆき、同一のリ
ンク群および接続関係にあるリンク群上で位置投影し続
けることができた距離を走行道路の候補毎に管理する。
例えば、図１０（ａ）は電源リセット直後の時刻ｔ０に
おける走行道路・位置候補管理手段５６６の状態を示し
ており、以後の車両の移動における時刻ｔ１および時刻
ｔ２における走行道路・位置候補管理手段５６６の状態
を同図（ｂ）と同図（ｃ）に示している。

【００５８】まず、図１０（ａ）に示した電源リセット
直後の時刻ｔ０において、走行道路・位置候補管理手段
５６６は、位置・方位推定手段５６５にて推定した現在
位置Ｐ_E-t0の近傍のリンク群ＬＳ₂ のみに投影点Ｐ
_C1-t0 を求めたとする。そして、リンク群ＬＳ₂ と投影
点Ｐ_C1-t0 を走行道路の候補Ｒｏａｄ_C1-t0 と現在位置
の候補Ｐ_C1-t0 として管理を始める。

【００５９】次に、時刻ｔ１における状態を示した同図
（ｂ）では、位置・方位推定手段５６５で推定した現在
位置Ｐ_E-t1を中心とする所定範囲Ｒ_M に含まれたリンク
群ＬＳ₃ とＬＳ₄ が、時刻ｔ０における走行道路の候補
Ｒｏａｄ_C1-t0 のリンク群ＬＳ₂ に接続できるので、リ
ンク群ＬＳ₃ とＬＳ₄ 上にそれぞれの投影点Ｐ_C1-t1と
Ｐ_C2-t1 を求める。そして、時刻ｔ０における走行道路
候補Ｒｏａｄ_C1-t0 のリンク群ＬＳ₂ からリンク群ＬＳ
₃ に接続するルートを時刻ｔ１における第一の走行道路
の候補Ｒｏａｄ_C1-t1 とし、投影点Ｐ_C1-t1 を第一の現
在位置の候補とする。また、時刻ｔ０における走行道路
候補Ｒｏａｄ_C1-t0 のリンク群ＬＳ₂ からリンク群ＬＳ
₄ に接続するルートを時刻ｔ１における第二の走行道路
の候補Ｒｏａｄ_C2-t1 とし、投影点Ｐ_C2-t1 を現在位置
の第二の候補とする。ここで、リンク群ＬＳ₂ からＬＳ
₃ へのルートを第一の候補、リンク群ＬＳ₂ からＬＳ₄
へのルートを第二の候補と決めた理由は、位置・方位推
定手段５６５で推定した現在位置Ｐ_E-t1と投影点Ｐ
_C1-t1 の間の距離の方が、現在位置Ｐ_E-t1と投影点Ｐ_C2
-t1 の間の距離より短かったためである。

【００６０】次に、時刻ｔ２における状態を示した同図
（ｃ）では、位置・方位推定手段５６５で推定した現在
位置Ｐ_E-t2を中心とする所定範囲Ｒ_M に含まれ、かつ投
影点を求め続けることができた走行道路の候補が第一の
候補Ｒｏａｄ_C1-t2 だけとなるため、同様に現在位置の
第一の候補Ｐ_C1-t2 のみが存続した状態となる。

【００６１】次に、走行道路・位置特定手段５６７の動
作について、上記図１０を用いて説明する。走行道路・
位置特定手段５６７は位置投影し続けた距離が最長とな
る走行道路候補を走行道路と判断して、その走行道路を
構成するリンク群上の投影点を車両の現在位置として特
定する。例えば、図１０（ａ）に示される時刻ｔ０にお
ける電源リセット直後において、走行道路・位置特定手
段５６７は、走行道路と現在位置の候補が一つしかない
ため、走行道路の候補Ｒｏａｄ_C1-t0 と現在位置の候補
Ｐ_C1-t0 を、それぞれ時刻ｔ０における走行道路と現在
位置として特定する。続いて同図（ｂ）に示される時刻
ｔ１の場合には、走行道路と現在位置の候補が二つ存在
するが、両候補とも同時に発生したものとすれば、第一
の走行道路の候補Ｒｏａｄ_C1-t1 と第一の現在位置の候
補Ｐ_C1-t1 を優先して時刻ｔ１における走行道路と現在
位置を特定する。続いて同図（ｃ）に示される時刻ｔ２
の場合には、走行道路と現在位置の候補が一つしかない
ため、走行道路の候補Ｒｏａｄ_{C1 -t2} と現在位置の候補
Ｐ_C1-t2 を、それぞれ時刻ｔ２における走行道路と現在
位置として特定する。以後、走行道路と現在位置の候補
が複数存在する場合には、位置投影し続けた距離が最長
となる候補を優先して、走行道路と現在位置として特定
する。

【００６２】このように、この実施の形態１によれば、
所定領域の道路ネットワークを記憶した道路網記憶手段
５２と、道路ネットワークの読み出しと管理を行う道路
網読出し手段５３と、位置・方位推定手段５６５の推定
した現在位置の近傍に存在する道路ネットワークに走行
道路と現在位置の候補を設定して管理する走行道路・位
置候補管理手段５６６と、走行道路と現在位置の候補の
中から走行道路と現在位置の候補を特定する走行道路・
位置特定手段５６７とを設けて、位置・方位推定手段５
６５で推定した車両の現在位置と進行方向をもとに、近
傍の道路ネットワーク上に車両の現在位置を特定するマ
ップマッチングを行うようにするとともに、自律航法用
複合センサ（距離センサ１および相対方位センサ２ａ）
にて計測した単位時間当たりの移動距離と進行方位変化
と、ＧＰＳ受信機３で観測した車両の現在位置を、カル
マンフィルタで統合処理しているので、以下のような効
果が得られる。すなわち、ＧＰＳ受信機３の観測情報と
自律航法用複合センサの計測情報に基づく車両の推定位
置が一本化されているので、マップマッチングを簡単に
行うことができる。また、マップマッチングを行って道
路ネットワーク上に車両の現在位置を求めているので、
より正確な車両の現在位置と進行方位を提供することが
できる。さらに、専用のカルマンフィルタを用いてＧＰ
Ｓ位置と自律航法用複合センサの計測情報から車両の現
在位置と進行方位を同時に推定しているので、ＧＰＳ位
置と自律航法用複合センサの計測情報の誤差が変動して
も、車両の現在位置と進行方位を最適に推定することが
可能となって信頼性が向上する。また、ＧＰＳ受信機３
が非測位状態でも自律航法用複合センサの計測情報から
車両の現在位置と進行方位を推定できるので、トンネル
や高架下の道路などにおいても、安定して車両の現在位
置と進行方位を提供できる。

【００６３】実施の形態２．次にこの発明の実施の形態
２について説明する。この実施の形態２によるロケータ
装置は、車両の進行方位に合致する方向にある道路ネッ
トワークのリンク群を先読みして保持するようにしたも
のであり、その構成は図１のブロック図に示した実施の
形態１の場合と同様であるため、その図示および説明は
省略する。

【００６４】次に動作について説明する。なお、その動
作についても、基本的には実施の形態１の場合と同様で
あり、道路網読出し手段５３と走行道路・位置候補管理
手段５６６の動作だけが異なるものであるため、その道
路網読出し手段５３と走行道路・位置候補管理手段５６
６の動作について説明し、その他については説明を省略
する。

【００６５】ここで、図１１は道路網読出し手段５３が
走行道路候補の前方に接続可能なリンク群を先読みする
際の状態を示した説明図である。道路網読出し手段５３
は実施の形態１で説明した処理の他に、位置・方位推定
手段５６５で推定した車両の進行方位に合致する方向
で、走行道路候補を構成するリンク群に接続可能な複数
のリンク群を所定距離または所定数だけ先読みし、この
先読みした前方リンク群の保持も行っている。走行道路
・位置候補管理手段５６６は、位置・方位推定手段５６
５で推定した車両の現在位置を投影する対象にその前方
リンク群も含めて処理を行う。

【００６６】例えば、図１１に示すように、時刻ｔ２に
おける走行道路の候補がリンク群ＬＳ₃ を構成リンク群
とするＲｏａｄ_C1-t2 のみであり、時刻ｔ３に位置・方
位推定手段５６５で推定した車両の現在位置がP_E-t3だ
ったとすると、道路網読出し手段５３は時刻ｔ３に位置
・方位推定手段５６５で推定した車両の進行方位に合致
する方向で、時刻ｔ２における走行道路の候補Ｒｏａｄ
_C1-t2 に接続可能なリンク群から、リンク群ＬＳ₄ 、Ｌ
Ｓ₆ 、およびＬＳ₇ を前方リンク群として設定する。

【００６７】このように、この実施の形態２によれば、
走行道路・位置候補管理手段５６６にて、位置・方位推
定手段５６５の推定した車両の進行方位に合致する方向
で、走行道路候補の構成リンク群に接続可能なリンク群
を、所定距離または所定数だけ先読みして保持している
ので、車両の前後方向の誤差が推定位置に含まれていて
も、道路ネットワーク上に車両の現在位置を求めること
ができ、また、道路分岐点で車両の現在位置を見失うこ
とがなくなるなどの効果が得られる。

【００６８】実施の形態３．次にこの発明の実施の形態
３について説明する。この実施の形態３によるロケータ
装置は、推定した車両の現在位置の軌跡形状と道路ネッ
トワークの形状を比較して、走行道路と現在位置の設
定、管理を行うようにしたものであり、その構成は図１
のブロック図に示した実施の形態１の場合と同様である
ため、その図示および説明は省略する。

【００６９】次に動作について説明する。なお、その動
作についても、基本的には実施の形態１の場合と同様で
あり、走行道路・位置候補管理手段５６６の動作だけが
異なるものであるため、その走行道路・位置候補管理手
段５６６の動作について説明し、その他については説明
を省略する。

【００７０】ここで、図１２は屈曲路を車両が通過する
状態を示した説明図で、同図（ａ）から同図（ｃ）はそ
れぞれ時刻ｔ１から時刻ｔ３における状態を示してい
る。走行道路・位置候補管理手段５６６は実施の形態１
で説明した処理の他に、位置・方位推定手段５６５で推
定した車両の現在位置を中心とする所定範囲内に道路分
岐点、あるいは屈曲路がある場合には、推定した車両の
現在位置に関する所定距離または所定時間以上前からの
軌跡形状と、道路ネットワークで接続関係にあるリンク
群の形状または接続関係を比較することにより、リンク
群への投影点の調整および、リンク群の選択も行ってい
る。

【００７１】例えば、図１２（ａ）に示すように、時刻
ｔ１において位置・方位推定手段５６５が推定した車両
の現在位置Ｐ_E-t1から近傍のリンク群に投影可能な地点
がａ点、ｂ点およびｃ点の３ヵ所あった場合には、走行
道路・位置候補管理手段５６６は、投影地点が１点のみ
であった時刻ｔ０から位置・方位推定手段５６５で推定
した車両の現在位置の軌跡形状（Ｐ_E-t0→Ｐ_E-t1）と、
道路ネットワークのそれら各点への接続形状（Ｐ_C-t0→
ａ、Ｐ_C-t0→ｂ、Ｐ_C-t0→ｃ）を比較して、時刻ｔ１に
おける投影点はｃ点（Ｐ_C-t1）であると決める。同図
（ｂ）と同図（ｃ）に示す場合も同図（ａ）の場合と同
様に、時刻ｔ２および時刻ｔ３における投影点をそれぞ
れｅ点（Ｐ_C-t2）とｇ点（Ｐ_C-t3）であると決める。

【００７２】このように、この実施の形態３によれば、
走行道路・位置候補管理手段５６６にて、位置・方位推
定手段５６５の推定した車両の現在位置の軌跡形状と道
路ネットワークの形状とを比較して、リンク群への投影
点の調整、およびにリンク群の選択を行っているので、
屈曲路を走行した場合でも、安定した車両の現在位置を
提供できるという効果が得られる。

【００７３】実施の形態４．次にこの発明の実施の形態
４について説明する。この実施の形態４によるロケータ
装置は、車両の進行方位に合致する方向に道路ネットワ
ークがなくなっても、しばらくの間は車両の現在位置を
求めるようにしたものであり、その構成は図１のブロッ
ク図に示した実施の形態１の場合と同様であるため、そ
の図示および説明は省略する。

【００７４】次に動作について説明する。なお、その動
作についても、基本的には実施の形態１の場合と同様で
あり、走行道路・位置候補管理手段５６６の動作だけが
異なるものであるため、その走行道路・位置候補管理手
段５６６の動作について説明し、その他については説明
を省略する。

【００７５】ここで、図１３は位置・方位推定手段５６
５が推定した車両の進行方位に合致する方向で走行道路
候補のリンク群を前方接続できなくなったとき以降の状
態を示した説明図で、同図（ａ）と同図（ｂ）はそれぞ
れ時刻ｔ１と時刻ｔ３における状態を示している。走行
道路・位置候補管理手段５６６は実施の形態１で説明し
た処理の他に、次の処理も行っている。すなわち、位置
・方位推定手段５６５が推定した車両の進行方位に合致
する方向で走行道路候補のリンク群を前方接続できなく
なった時点の投影点と、推定した現在位置の位置関係に
基づいて、それ以後、所定時間が経過するまでの間、ま
たは車両が所定距離を走行するまでの間は、投影点の前
方に現在位置の候補を更新する。また、所定時間が経過
した後、または所定距離を走行した後は、他の走行道路
候補があれば当該走行道路候補を抹消し、他の走行道路
候補がなければ、推定した現在位置の方へ現在位置の候
補を寄せるように位置更新する。

【００７６】例えば、図１３（ａ）に示すように、時刻
ｔ０に位置・方位推定手段５６５で推定した車両の現在
位置Ｐ_E-t0を近傍のリンク群に投影したのを最後に、時
刻ｔ１に位置・方位推定手段５６５で推定した車両の現
在位置Ｐ_E-t1を投影する近傍のリンク群がなくなった場
合、時刻ｔ０から位置・方位推定手段５６５で推定した
車両の現在位置の軌跡形状（Ｐ_E-t0→Ｐ_E-t1）と相似な
軌跡が示されるように、時刻ｔ０における投影点Ｐ_C-t0
と位置・方位推定手段５６５で推定した現在位置Ｐ_E-t0
の位置関係（経度差：Δλ_t0、緯度差Δφ_t0）に基づい
て、投影点Ｐ_{C- t0}の前方に現在位置の候補Ｐ_C-t1の更新
を行う。また、図１３（ｂ）に示す時刻ｔ３の場合に
は、時刻ｔ０から位置・方位推定手段５６５で推定した
車両の現在位置Ｐ_E-t0が所定距離Δｄ_FRE を走行するま
では同図（ａ）と同様の位置更新を行い、所定距離Δｄ
_FRE に達した時刻ｔ２以降は、位置・方位推定手段５６
５で推定した現在位置Ｐ_E-t3の方向へ現在位置の候補Ｐ
_C-t3を寄せるように位置更新する。

【００７７】このように、この実施の形態４によれば、
走行道路・位置候補管理手段５６６にて、位置・方位推
定手段５６５の推定した車両の進行方位に合致する方向
に道路ネットワークがなくなった場合でも、所定時間が
経過、あるいは所定距離を走行するまでの間は、現在位
置の候補を連続性をもたせて継続的に更新しているの
で、道路ネットワーク化されていない新しい道路や農道
などが含まれる経路を走行した場合でも、安定した車両
の現在位置を提供できるという効果が得られる。

【００７８】実施の形態５．次にこの発明の実施の形態
５について説明する。この実施の形態５によるロケータ
装置は、ＧＰＳ受信機の測位率が低下する区間やＧＰＳ
位置の分散が大きくなる区間を走行する場合には、車両
の移動ベクトルを用いてマップマッチングを行うように
したものであり、その構成は図１のブロック図に示した
実施の形態１の場合と同様であるため、その図示および
説明は省略する。

【００７９】次に動作について説明する。なお、その動
作についても、基本的には実施の形態１の場合と同様で
あり、走行道路・位置特定手段５６７の動作だけが異な
るものであるため、その走行道路・位置特定手段５６７
の動作について説明し、その他については説明を省略す
る。

【００８０】ここで、図１４は車両がトンネルを通過す
る前後の状態を示す説明図である。走行道路・位置特定
手段５６７は、実施の形態１で説明した処理の他に、Ｇ
ＰＳ受信機３の所定時間における測位率が所定値以下に
なった場合、またはＧＰＳ位置の分散が大きくなった場
合に、車両の現在位置を、当該状態になる直前に道路ネ
ットワーク上に特定した車両の現在位置を起点にして、
以後、位置・方位推定手段５６５で用いた車両の所定時
間毎の移動距離と進行方位変化で示される分だけ前方に
道路ネットワーク上に位置更新する処理も行っている。

【００８１】例えば、図１４に示すように、トンネル進
入直前に位置・方位推定手段５６５で推定した車両の現
在位置Ｐ_E-t0に対して近傍のリンク群上に現在位置Ｐ_t0
を特定したとする。トンネル通過中およびＧＰＳ受信機
３の所定時間における測位率が所定値以下になる時刻ｔ
１から時刻ｔ７までの間は、時刻ｔ０で特定した現在位
置Ｐ_t0を起点に、位置・方位推定手段５６５で用いた車
両の所定時間毎の移動距離と進行方位変化、すなわち、
図１４中に太線の矢印で示した、所定時間ごとの移動ベ
クトルに示される分だけ、前方に接続されるリンク群上
に位置更新（図中のＰ_t1〜Ｐ_t7）する。そして、ＧＰＳ
受信機３の所定時間における測位率が所定値以上になっ
た時刻ｔ８では、位置・方位推定手段５６５で推定した
車両の現在位置Ｐ_E-t8を近傍のリンク群上に投影して現
在位置Ｐ_t8を決めてゆく。

【００８２】このように、この実施の形態５によれば、
ＧＰＳ受信機３の所定時間における測位率が所定値以下
になると、走行道路・位置特定手段５６７にて、その直
前に道路ネットワーク上に特定した車両の現在位置を起
点にして、車両の単位時間毎の移動距離と進行方位変化
をもとに、道路ネットワーク上に現在位置を更新してい
るので、ＧＰＳ受信機３の測位率が低下する区間やＧＰ
Ｓ位置の分散が大きくなる区間の道路を走行した場合で
も、マップマッチングは誤差が累積されない車両の移動
ベクトルを用いて行われるため、安定した車両の現在位
置を提供することができるという効果が得られる。

【００８３】実施の形態６．次にこの発明の実施の形態
６について説明する。この実施の形態６によるロケータ
装置は、距離センサ１の代わりに、当該ロケータ装置に
内蔵した加速度センサを用いて自律航法用複合センサを
形成したものであり、図１５はそのようなこの発明の実
施の形態６によるロケータ装置の構成を示すブロック図
である。図において、４は距離センサ１の代わりに用い
られて車両の加速度を検出する、当該ロケータ装置内の
加速度センサであり、５６２は信号処理器５内に設けら
れて、加速度センサ４のオフセットを検出するオフセッ
ト検出手段である。なお、他の部分は、図１に同一符号
を付して示した実施の形態１の各部と同等の部分である
ためその説明は省略する。

【００８４】次に動作について説明する。なお、その動
作については、距離・速度計算手段５６１にて加速度セ
ンサ４の出力信号から移動距離および速度を計算するこ
と以外は、実施の形態１の場合と同様であるため、加速
度センサ４の動作と、オフセット検出手段５６２による
この加速度センサ４のオフセット検出および距離・速度
計算手段５６１に係わる、図２のステップＳＴ２０２動
作についての説明を行い、その他については説明を省略
する。

【００８５】まず、加速度センサ４の動作について説明
する。加速度センサ４は、加速度ａ_ACC が０の時でも出
力信号ＳＧ_ACC が所定の電圧値（［ｍＶ］、以下、オフ
セットＯＦ_ACC という）を示すように調整されたもの
で、加速時にはオフセットＯＦ_ACC より大きな電圧値
が、また減速時にはオフセットＯＦ_ACC より小さな電圧
値が出力される。加速度センサ４の出力信号ＳＧ_{AC C} か
ら加速度ａ_ACC を求めるには、次の式（２９）に示すよ
うに、出力信号ＳＧ _ACC からオフセットＯＦ_ACC を減算
した後、電圧値を加速度の単位に変換するためのスケー
ルファクタＳＦ_ACC （［ｍ／ｓ² ／ｍＶ］）を乗算すれ
ばよい。

【００８６】 ａ_ACCi＝（ＳＧ_ACCi−ＯＦ_ACCi）×ＳＦ_ACCi×ΔＴ ・・・（２９）

【００８７】そして、図２に示すロケータ装置のメイン
ルーチンの処理において、ステップＳＴ２０４で距離・
速度計算手段５６１の処理として、加速度センサ４の出
力電圧をＡ／Ｄ変換して上記式（２９）にて加速度ａ
_ACCiを求めた後、次の式（３０）および式（３１）を用
いて速度Ｖｅｌ_DRi と移動距離Δｄ_i を計算する。

【００８８】 Ｖｅｌ_DRi ＝Ｖｅｌ_DRi-1 ＋ａ_ACCi×ΔＴ ・・・（３０） Δｄ_i ＝Δｄ_i-1 ＋Ｖｅｌ_DRi ×ΔＴ＋１／２×ａ_ACCi×ΔＴ² ・・・（３１）

【００８９】また、ステップＳＴ２０５ではオフセット
検出手段５６２の処理として、所定時間分の加速度セン
サ４の出力信号の標準偏差を計算し、当該標準偏差が所
定値以下であれば停車状態だと判断し、そうでなけば走
行状態だと判断する。そして、停車状態であると判断し
た場合は、標準偏差を計算した同じ時間帯の出力信号の
平均を計算して、オフセットＯＦ_ACC として保持する。

【００９０】このように、自律航法用複合センサを形成
する距離センサ１を加速度センサ４で代替した場合で
も、簡単にマップマッチングを行うことができ、車両の
現在位置と進行方位をより正確に提供することが可能で
ある。

【００９１】実施の形態７．なお、上記実施の形態１に
おいては、道路網読出し手段５３は、リンク群の構成リ
ンク数に上限を設けると説明したが、構成リンク数の代
わりに、構成リンクの総リンク長に上限を設けるように
してもよい。

【００９２】実施の形態８．また、上記実施の形態５に
おいては、走行道路・位置特定手段５６７は、ＧＰＳ受
信機３の所定時間における測位率が所定値以下になった
時、当該状態になる直前に道路ネットワーク上に特定し
た車両の現在位置を起点にして、以後、位置・方位推定
手段５６５で用いた車両の所定時間毎の移動距離と進行
方位変化で示される分だけ前方に道路ネットワーク上に
位置更新すると説明したが、ＧＰＳ位置の分散が所定値
以上になる状態が所定時間継続した時にも、同様に位置
更新するようにしてもよい。

【００９３】

【発明の効果】以上のように、この発明によれば、ＧＰ
Ｓ受信機の観測情報と自律航法用複合センサの計測情報
に基づく車両の推定位置が一本化されているので、マッ
プマッチングを簡単に行うことが可能となり、また、マ
ップマッチングを行って道路ネットワーク上に車両の現
在位置を求めているので、より正確な車両の現在位置と
進行方位を提供することができるロケータ装置が得られ
る効果がある。

【００９４】この発明によれば、専用のカルマンフィル
タにて、ＧＰＳ位置と自律航法用複合センサの計測情報
から車両の現在位置と進行方位を同時に推定しているの
で、ＧＰＳ位置と自律航法用複合センサの計測情報の誤
差が変動しても、車両の現在位置と進行方位を最適に推
定することが可能となって、信頼性の向上がはかれ、ま
た、ＧＰＳ受信機が非測位状態でも、自律航法用複合セ
ンサの計測情報から車両の現在位置と進行方位を推定で
きるので、トンネルや高架下などの道路を走行中でも、
安定的に車両の現在位置と進行方位を提供できる効果が
ある。

【００９５】この発明によれば、車両の進行方位に合致
する方向にある道路ネットワークを先読みして保持して
いるので、車両の前後方向の誤差が推定位置に含まれて
いても、道路ネットワーク上に車両の現在位置を求める
ことが可能となり、また、道路分岐点で車両の現在位置
を見失うことがなくなるなどの効果がある。

【００９６】この発明によれば、車両の推定現在位置の
軌跡形状と道路ネットワークの形状を比較して、道路ネ
ットワーク上に車両の現在位置を求めているので、屈曲
路を走行した際にも、安定した車両の現在位置を提供で
きる効果がある。

【００９７】この発明によれば、車両の進行方位に合致
する方向に道路ネットワークがなくなっても、以後しば
らくの間は、連続性をもって継続的に車両の現在位置を
求めいてるので、道路ネットワークが未整備経路を走行
した際にも、安定した車両の現在位置を提供できる効果
がある。

【００９８】この発明によれば、ＧＰＳ受信機の測位率
が低下する区間やＧＰＳ位置の分散が大きくなる区間の
道路を走行した場合に、マップマッチングを、誤差が累
積される推定位置ではなく、誤差が累積されない車両の
移動ベクトルを用いて行っているので、上記区間の道路
を走行した際にも、安定した車両の現在位置を提供でき
る効果がある。

【図面の簡単な説明】

【図１】 この発明の実施の形態１によるロケータ装置
の構成を示すブロック図である。

【図２】 実施の形態１におけるメインルーチンの処理
内容を示す流れ図である。

【図３】 実施の形態１における位置・方位推定手段の
処理内容を示す流れ図である。

【図４】 実施の形態１における位置・方位推定手段に
よる車両の現在位置の推定例を示す説明図である。

【図５】 実施の形態１における所定時間毎に実行され
る割込処理の内容を示す流れ図である。

【図６】 実施の形態１における距離センサから信号が
出力されたときに実行される割込処理の内容を示す流れ
図である。

【図７】 実施の形態１におけるＧＰＳ観測周期毎にＧ
ＰＳ受信機から出力された観測情報を受信する割込処理
の内容を示す流れ図である。

【図８】 実施の形態１における道路網読出し手段の、
無分岐区間の接続関係にある複数のリンクを一つのリン
ク群として管理する際の状態を示す説明図である。

【図９】 実施の形態１における道路網読出し手段の、
特に一本道においてリンク群を管理する際の状態を示す
説明図である。

【図１０】 実施の形態１における走行道路・位置候補
管理手段の、車両の走行道路と現在位置の候補を管理す
る際の状態を示す説明図である。

【図１１】 この発明の実施の形態２によるロケータ装
置における、道路網読出し手段の走行道路候補の前方に
接続可能なリンク群を先読みする際の状態を示す説明図
である。

【図１２】 この発明の実施の形態３によるロケータ装
置における、屈曲路を車両が通過する際の状態を示す説
明図である。

【図１３】 この発明の実施の形態４によるロケータ装
置における、車両の進行方位に合致する方向で走行道路
候補のリンク群を前方接続できなくなったとき以降の状
態を示す説明図である。

【図１４】 この発明の実施の形態５によるロケータ装
置における、車両がトンネルを通過する際の前後の状態
を示す説明図である。

【図１５】 この発明の実施の形態６のロケータ装置の
構成を示すブロック図である。

【図１６】 従来のロケータ装置の構成例を示すブロッ
ク図である。

【図１７】 従来のロケータ装置における車両の現在位
置の移動点の算出を示す説明図である。

【図１８】 従来のロケータ装置におけるＧＰＳ位置を
近傍のリンク上へ投影した投影点の算出を示す説明図で
ある。

【図１９】 従来のロケータ装置におけるルート算出を
示す説明図である。

【図２０】 従来のロケータ装置にて、カルマンフィル
タで現在位置の推定点が決まる様子を示す説明図であ
る。

【符号の説明】

１ 距離センサ（自律航法用複合センサ）、２ａ 相対
方位センサ（自律航法用複合センサ）、３ ＧＰＳ受信
機（絶対位置観測手段）、４ 加速度センサ（自律航法
用複合センサ）、５２ 道路網記憶手段、５３ 道路網
読出し手段、５６５ 位置・方位推定手段、５６６ 走
行道路・位置候補管理手段、５６７ 走行道路・位置特
定手段。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 中堀 一郎 東京都千代田区丸の内二丁目２番３号 三 菱電機株式会社内 Ｆターム(参考） 2F029 AA02 AB01 AB07 AB09 AC02 AC14 AD01 AD05

Claims (6)

【特許請求の範囲】
1. 【請求項１】 道路ネットワークを記憶した道路網記憶
   手段と、 前記道路網記憶手段より読み出した道路ネットワーク上
   における車両の現在位置と進行方位を、自律航法用複合
   センサの計測情報と絶対位置観測手段の観測情報に基づ
   いて、所定時間毎に推定する位置・方位推定手段と、 前記位置・方位推定手段で推定した車両の現在位置の近
   傍に存在する道路ネットワークのリンクを検索するとと
   もに、当該リンクがリンクの両端を直線で結んだ直線デ
   ータであれば、無分岐区間を示す接続関係にあるリンク
   を一つのリンク群としてまとめ、折れ線データであれば
   当該リンクをリンク群として扱う道路網読出し手段と、 推定した車両の現在位置を前記リンク群に投影した際の
   リンク群への最短距離が所定値以下で、かつ同一のリン
   ク群および接続関係にあるリンク群上で位置投影し続け
   た距離が所定値以上になる接続関係にあるリンク群を車
   両の走行道路の候補として、また当該リンク群上の投影
   点を現在位置の候補として設定し管理する走行道路・位
   置候補管理手段と、 前記位置投影し続けた距離が最長となる走行道路の候補
   を走行道路として特定して、その走行道路のリンク群上
   の投影点を車両の現在位置として特定する走行道路・位
   置特定手段とを備えたロケータ装置。
2. 【請求項２】 位置・方位推定手段が、自律航法用複合
   センサによって計測された単位時間当たりの移動距離と
   進行方位変化から、車両の現在位置と進行方位を計算す
   るシステムモデルと、絶対位置観測手段で観測した車両
   の現在位置と前記システムモデルにおける現在位置の関
   係を表した観測モデルに基づいて、状態ベクトルの行列
   要素には前記システムモデルにおける現在位置と進行方
   位を設定し、観測値には前記絶対位置観測手段で観測し
   た車両の現在位置を設定したカルマンフィルタにより、
   状態ベクトルの行列要素である車両の現在位置と進行方
   位を推定するものであることを特徴とする請求項１記載
   のロケータ装置。
3. 【請求項３】 走行道路・位置候補管理手段が、位置・
   方位推定手段で推定した車両の進行方位に合致する方向
   で、走行道路候補の構成リンク群に接続可能な複数のリ
   ンク群を所定距離または所定数だけ先読みして保持して
   おき、この先読みして保持しておいたリンク群を、前記
   位置・方位推定手段で推定した車両の現在位置を投影す
   る対象として用いるものであることを特徴とする請求項
   １記載のロケータ装置。
4. 【請求項４】 走行道路・位置候補管理手段が、位置・
   方位推定手段の推定した車両の現在位置を中心とした所
   定範囲内に、道路分岐点、あるいは屈曲路がある場合に
   は、所定距離または所定時間以上前から推定した車両の
   現在位置の軌跡形状と、道路ネットワークで接続関係に
   あるリンク群の形状とを比較して、リンク群への投影点
   の調整、およびリンク群の選択を行うものであることを
   特徴とする請求項１記載のロケータ装置。
5. 【請求項５】 走行道路・位置候補管理手段が、位置・
   方位推定手段の推定した車両の進行方位に合致する方向
   で、走行道路候補の構成リンク群にその他のリンク群を
   前方接続できなくなった場合以降、所定時間が経過する
   まで、または所定距離を走行するまでの間は、前方接続
   できなくなった時点の投影点と推定した現在位置の位置
   関係に基づいて投影点の前方に現在位置の候補を更新
   し、所定時間が経過した後、または所定距離を走行した
   後は、他の走行道路候補があれば当該走行道路候補を抹
   消し、また他の走行道路候補がなければ、推定した現在
   位置の方向へ現在位置の候補を寄せるように位置更新す
   るものであることを特徴とする請求項１記載のロケータ
   装置。
6. 【請求項６】 走行道路・位置特定手段が、絶対位置観
   測手段の所定時間における測位率が所定値以下になった
   時、あるいは前記絶対位置観測手段で観測した車両の現
   在位置の分散が所定値以上になる状態が所定時間以上続
   いた時に、その状態になる直前に道路ネットワーク上に
   特定した車両の現在位置を起点にして、以後、位置・方
   位推定手段で用いる車両の単位時間毎の移動距離と進行
   方位変化をもとに、道路ネットワーク上で現在位置の位
   置更新をするものであることを特徴とする請求項１記載
   のロケータ装置。