(57)【特許請求の範囲】
【請求項１】
道路ネットワークを記憶した道路網記憶手段と、
前記道路網記憶手段より読み出した道路ネットワーク上における車両の現在位置と進行方位を、自律航法用複合センサの計測情報と絶対位置観測手段の観測情報に基づいて、所定時間毎に推定する位置・方位推定手段と、
前記位置・方位推定手段で推定した車両の現在位置の近傍に存在する道路ネットワークのリンクを検索するとともに、当該リンクがリンクの両端を直線で結んだ直線データであれば、無分岐区間を示す接続関係にあるリンクを一つのリンク群としてまとめ、折れ線データであれば当該リンクをリンク群として扱う道路網読出し手段と、
推定した車両の現在位置を前記リンク群に投影した際のリンク群への最短距離が所定値以下で、かつ同一のリンク群および接続関係にあるリンク群上で位置投影し続けた距離が所定値以上になる接続関係にあるリンク群を車両の走行道路の候補として、また当該リンク群上の投影点を現在位置の候補として設定し管理する走行道路・位置候補管理手段と、
前記位置投影し続けた距離が最長となる走行道路の候補を走行道路として特定して、その走行道路のリンク群上の投影点を車両の現在位置として特定する走行道路・位置特定手段とを備えたロケータ装置。
【請求項２】
位置・方位推定手段が、自律航法用複合センサによって計測された単位時間当たりの移動距離と進行方位変化から、車両の現在位置と進行方位を計算するシステムモデルと、絶対位置観測手段で観測した車両の現在位置と前記システムモデルにおける現在位置の関係を表した観測モデルに基づいて、状態ベクトルの行列要素には前記システムモデルにおける現在位置と進行方位を設定し、観測値には前記絶対位置観測手段で観測した車両の現在位置を設定したカルマンフィルタにより、状態ベクトルの行列要素である車両の現在位置と進行方位を推定するものであることを特徴とする請求項１記載のロケータ装置。
【請求項３】
走行道路・位置候補管理手段が、位置・方位推定手段で推定した車両の進行方位に合致する方向で、走行道路候補の構成リンク群に接続可能な複数のリンク群を所定距離または所定数だけ先読みして保持しておき、この先読みして保持しておいたリンク群を、前記位置・方位推定手段で推定した車両の現在位置を投影する対象として用いるものであることを特徴とする請求項１記載のロケータ装置。
【請求項４】
走行道路・位置候補管理手段が、位置・方位推定手段の推定した車両の現在位置を中心とした所定範囲内に、道路分岐点、あるいは屈曲路がある場合には、所定距離または所定時間以上前から推定した車両の現在位置の軌跡形状と、道路ネットワークで接続関係にあるリンク群の形状とを比較して、リンク群への投影点の調整、およびリンク群の選択を行うものであることを特徴とする請求項１記載のロケータ装置。
【請求項５】
走行道路・位置候補管理手段が、位置・方位推定手段の推定した車両の進行方位に合致する方向で、走行道路候補の構成リンク群にその他のリンク群を前方接続できなくなった場合以降、所定時間が経過するまで、または所定距離を走行するまでの間は、前方接続できなくなった時点の投影点と推定した現在位置の位置関係に基づいて投影点の前方に現在位置の候補を更新し、所定時間が経過した後、または所定距離を走行した後は、他の走行道路候補があれば当該走行道路候補を抹消し、また他の走行道路候補がなければ、推定した現在位置の方向へ現在位置の候補を寄せるように位置更新するものであることを特徴とする請求項１記載のロケータ装置。
【請求項６】
走行道路・位置特定手段が、絶対位置観測手段の所定時間における測位率が所定値以下になった時、あるいは前記絶対位置観測手段で観測した車両の現在位置の分散が所定値以上になる状態が所定時間以上続いた時に、その状態になる直前に道路ネットワーク上に特定した車両の現在位置を起点にして、以後、位置・方位推定手段で用いる車両の単位時間毎の移動距離と進行方位変化をもとに、道路ネットワーク上で現在位置の位置更新をするものであることを特徴とする請求項１記載のロケータ装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、ＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）受信機の観測情報と、自律航法用複合センサの計測情報を用いて、車両の現在位置を道路ネットワーク上に特定するマップマッチングを行うロケータ装置に関するものある。
【０００２】
【従来の技術】
図１６は、例えば、特開平１０−４７９８３号公報に示された、従来のロケータ装置の構成を示すブロック図である。図において、１は車両の移動距離に応じた信号を出力する距離センサ、２は車両の進行方位に応じた信号を出力する方位センサであり、３は絶対位置観測手段としてのＧＰＳ受信機およびアンテナである。なお、この距離センサ１および方位センサ２にて、自律航法用複合センサを形成している。５２は道路網記憶手段（地図専用ディスク）、５３は道路網記憶手段（地図専用ディスク）５２に格納されている道路ネットワークを取得する道路網読出し手段（ＣＤドライブ）であり、５４は道路ネットワークと諸処理データのメモリへの保持を制御するメモリ制御部、５５は車両の位置や方位などの情報の出力を制御する出力制御部である。また、５１１は距離センサ１の出力信号から車両の移動距離を演算する距離取得手段、５１２は方位センサ２の出力信号から車両の進行方位を演算する方位取得手段、５１３はＧＰＳ受信機３で観測した車両の現在位置を受信する位置取得手段であり、５１５から５１７は道路ネットワーク上に車両の現在位置を特定するマップマッチングを行うための移動位置算出手段、ルートおよび推定位置算出手段、表示用現在位置選択手段である。
【０００３】
次に動作について説明する。
ここでは、マップマッチングを行う移動位置算出手段５１５、ルートおよび推定位置算出手段５１６、表示用現在位置選択手段５１７の動作について、図１７〜図２０を用いて説明する。ここで、図１７は車両の現在位置の移動点の算出を示す説明図、図１８はＧＰＳ位置を近傍のリンク上へ投影した投影点の算出を示す説明図、図１９はルートの算出を示す説明図、図２０は現在位置の候補点と移動点、およびＧＰＳ位置の投影点のそれぞれの誤差に基づくカルマンフィルタにて現在位置の推定点が決まる様子を示す説明図である。
【０００４】
移動位置算出手段５１５は、メモリ制御部５４が道路網読出し手段（ＣＤドライブ）５３から得た道路ネットワーク上において、車両の現在位置の候補点を起点として、方位取得手段５１２から得られる車両の進行方位に合致する道路に沿って、距離取得手段５１１から得られる距離情報分だけ前方に現在位置の移動点を算出する。図１７では、現在位置の候補点Ｓ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 とから構成されるカルマンフィルタにより、次に示す式（２）にて算出するものである。
【０００８】
Ｋ＝Ｅms2 ／（Ｅms2 ＋Ｅg2）                    ・・・（２）
【０００９】
そして、図２０に示すように、現在位置の候補点の誤差Ｅs と距離センサ１に係わる誤差Ｅm から現在位置の移動点に係わる誤差Ｅmsを次の式（３）にて算出する。
【００１０】
Ｅms＝（Ｅs2＋Ｅm2）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 )の関係を次の式（１３）〜式（１４）で表す観測モデルとに基づくものである。
【００３６】
0003745165.tif 000002
【００３７】
λ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 を推定する。
【００４０】
【数１】
0003745165.tif 000003
【００４１】
【数２】
0003745165.tif 000004
【００４２】
ｘ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 ＦiT＋Ｇi ΣｗiＧiT            ・・・（２８）
【００４３】
なお、上記式（１５）〜式（１９）および式（２０）〜式（２３）において、ｘ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 を、それぞれ式（２４）〜式（２８）を用いて計算して処理を終了する。
【００４７】
次に、位置・方位推定手段５６５における車両の現在位置の推定動作について図４を用いて説明する。
まず同図（ａ）は、車両が直進走行する途中でＧＰＳ位置の分散が大きくなる時に車両の現在位置が推定される様子を示したもので、この時には、システム誤差の行列要素である方位誤差要素と距離誤差要素がともに小さい状態であったことを想定したものである。この場合、位置・方位推定手段５６５はＧＰＳ位置の分散が大きくなった区間では、観測誤差であるＧＰＳ位置の誤差が大きくなったことを検出するので、ＧＰＳ位置の分散が大きくなった区間のみカルマンゲインを小さく計算する。その結果、ＧＰＳ位置の分散が小さい区間では車両の現在位置がＧＰＳ位置へ寄るように推定するが、ＧＰＳ位置の分散が大きい区間では車両の現在位置がＧＰＳ位置へ寄る量を小さく抑えて推定する。
【００４８】
また図４（ｂ）は、車両が直進走行する途中で相対方位センサ２ａのオフセットがドリフトした時に車両の現在位置が推定される様子を示したもので、この時には、ＧＰＳ位置の分散が小さい状態であったことを想定したものである。この場合、位置・方位推定手段５６５は、ＧＰＳ位置の分散が小さい一方で、単位時間毎の進行方位変化が偏った値となって方位誤差が発生したことを検出するので、カルマンゲインを大きく計算する。その結果、相対方位センサ２ａに生じたドリフトの影響を受けずに車両の現在位置をＧＰＳ位置の方向へ寄せるように推定する。なお、カルマンフィルタを使用しない一般的な自律航法の場合には、相対方位センサ２ａに生じたドリフトの影響を受けて、一点鎖線で示すように、車両の現在位置が偏った方向に徐々に旋回してしまう。
【００４９】
また図４（ｃ）は、相対方位センサ２ａのスケールファクタが真値より小さ目の時に車両が右折した場合の車両の現在位置が推定される様子を示したもので、この時には、ＧＰＳ位置の分散が小さい状態であったことを想定したものである。この場合、位置・方位推定手段５６５は、ＧＰＳ位置の分散が小さい一方で、右折後に方位誤差が発生したことを検出するので、右折後にはカルマンゲインをより大きく計算する。その結果、右折時の旋回角が小さ目になることから推定した進行方位に誤差が生じるものの、右折後に方位誤差を検出した以後は車両の現在位置をＧＰＳ位置の方向へ寄せるように推定する。なお、カルマンフィルタを使用しない一般的な自律航法の場合には、一点鎖線で示すように、旋回角不足のまま車両の現在位置が誤った方向に直進してしまう。
【００５０】
次に、割込処理１について説明する。
この割込処理１は所定時間Δｔ毎に起動され、図５の流れ図に示すように、そのステップＳＴ５０１において、処理開始タイミングを示す処理開始フラグをセットして、この割込処理１を終了する。
【００５１】
次に、割込処理２について説明する。
この割込処理２は距離センサ１から信号が出力されたときに起動され、図６の流れ図に示すように、そのステップＳＴ６０１において車速信号カウンタに１を加算して、この割込処理２を終了する。
【００５２】
次に、割込処理３について説明する。
この割込処理３はＧＰＳ受信機３から観測周期毎に出力された観測情報を受信するものであり、図７の流れ図に示すように、まずステップＳＴ７０１でＧＰＳ受信機３から出力された観測情報を受信して記憶する。次にステップＳＴ７０２において全ての観測情報の受信を完了したか否かを判定し、完了していればステップＳＴ７０３へ進み、完了していなければこの割込処理３を終了する。ステップＳＴ７０３では全ての観測情報を受信完了したとして、ＧＰＳ観測情報受信フラグをセットする。
【００５３】
次に、図２のステップＳＴ２１０で行うマップマッチング法について、このマップマッチング法に係わる道路網読出し手段５３の動作から説明する。
道路網読出し手段５３は、位置・方位推定手段５６５で推定した車両の現在位置と進行方位を受け取り、推定された現在位置が存在する領域と隣接領域の道路ネットワークを道路網記憶手段５２から読み出す。そして、読み出した道路ネットワークのリンクがその両端のノードを直線で結ぶ直線データだった場合には、交差点間、一本道、および行き止まり道などの無分岐区間の接続関係にある複数のリンクを一つのリンク群として管理する。また、読み出した道路ネットワークのリンクが折れ線データだった場合には、その折れ線データによるリンクをリンク群として扱う。
【００５４】
ここで、道路網読出し手段５３が、例えば図８（ａ）に示すような道路ネットワークを道路網記憶手段５２から読み出した場合には、同図（ｂ）に示すような、構成リンク、ノード、補間点よりなるリンク群に編集する。この図８（ａ）において、○印は道路ネットワークのリンクの座標を示すノードであり、○印を直線で結んだものが直線データによるリンクである。また、Ｎ1 からＮ12はノード番号、Ｌ1 からＬ12はリンク番号を示す。
【００５５】
しかしながら、現実にはマップマッチングの負荷を軽減するために、位置・方位推定手段５６５で推定した現在位置を中心とする半径３００ｍ程度の範囲内にあるリンクに対してリンク群の作成を行う。
【００５６】
また、読み出した道路ネットワークに、例えば、図９に示すような一本道が含まれていた場合には、位置・方位推定手段５６５で推定した現在位置を中心とする所定範囲内のリンクに一本道の終端、すなわち、交差点か行き止まりがなければ、上述のように無分岐区間のリンク群を作成して管理することはできない。そこで、道路網読出し手段５３はリンク群の構成リンク数に上限を設けたもので、説明上、この図９の場合にはその上限値を４本としている。まず、同図（ａ）に示したｔ０の時点では、位置・方位推定手段５６５で推定した現在位置ＰE-t0を中心とする所定範囲ＲM 内のリンクＬ1 からＬ3 およびＬ8 対して、図中の表に示すリンク群ＬＳ1 からＬＳ3 を作成する。この時、リンク群ＬＳ3 のノードは一本道の終端ではないので暫定的なリンク群とする。そして、同図（ｂ）に示すｔ１の時点で、位置・方位推定手段５６５で推定した現在位置ＰE-t1を中心とする所定範囲ＲM 内にリンクＬ4 からＬ7 が入るようになった時、図中の表に示すように、リンク群ＬＳ3 にリンクＬ4 とＬ5 のみを接続して、リンクＬ6 とＬ7 は新たなリンク群ＬＳ4 として暫定的に管理する。
【００５７】
次に、走行道路・位置候補管理手段５６６の動作について、図１０を用いて説明する。
まず、電源リセット直後において、走行道路・位置候補管理手段５６６は、位置・方位推定手段５６５で推定した車両の現在位置をリンク群に投影した際の、構成リンクへの最短距離が所定値以下になるものを走行道路の候補とし、投影点を現在位置の候補とする。その後、車両の移動に伴って、位置・方位推定手段５６５で推定した現在位置を近傍のリンク群に対して次々に投影してゆき、同一のリンク群および接続関係にあるリンク群上で位置投影し続けることができた距離を走行道路の候補毎に管理する。例えば、図１０（ａ）は電源リセット直後の時刻ｔ０における走行道路・位置候補管理手段５６６の状態を示しており、以後の車両の移動における時刻ｔ１および時刻ｔ２における走行道路・位置候補管理手段５６６の状態を同図（ｂ）と同図（ｃ）に示している。
【００５８】
まず、図１０（ａ）に示した電源リセット直後の時刻ｔ０において、走行道路・位置候補管理手段５６６は、位置・方位推定手段５６５にて推定した現在位置ＰE-t0の近傍のリンク群ＬＳ2 のみに投影点ＰC1-t0 を求めたとする。そして、リンク群ＬＳ2 と投影点ＰC1-t0 を走行道路の候補ＲｏａｄC1-t0 と現在位置の候補ＰC1-t0 として管理を始める。
【００５９】
次に、時刻ｔ１における状態を示した同図（ｂ）では、位置・方位推定手段５６５で推定した現在位置ＰE-t1を中心とする所定範囲ＲM に含まれたリンク群ＬＳ3 とＬＳ4 が、時刻ｔ０における走行道路の候補ＲｏａｄC1-t0 のリンク群ＬＳ2 に接続できるので、リンク群ＬＳ3 とＬＳ4 上にそれぞれの投影点ＰC1-t1 とＰC2-t1 を求める。そして、時刻ｔ０における走行道路候補ＲｏａｄC1-t0 のリンク群ＬＳ2 からリンク群ＬＳ3 に接続するルートを時刻ｔ１における第一の走行道路の候補ＲｏａｄC1-t1 とし、投影点ＰC1-t1 を第一の現在位置の候補とする。また、時刻ｔ０における走行道路候補ＲｏａｄC1-t0 のリンク群ＬＳ2 からリンク群ＬＳ4 に接続するルートを時刻ｔ１における第二の走行道路の候補ＲｏａｄC2-t1 とし、投影点ＰC2-t1 を現在位置の第二の候補とする。ここで、リンク群ＬＳ2 からＬＳ3 へのルートを第一の候補、リンク群ＬＳ2 からＬＳ4 へのルートを第二の候補と決めた理由は、位置・方位推定手段５６５で推定した現在位置Ｐ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 を、それぞれ時刻ｔ２における走行道路と現在位置として特定する。以後、走行道路と現在位置の候補が複数存在する場合には、位置投影し続けた距離が最長となる候補を優先して、走行道路と現在位置として特定する。
【００６２】
このように、この実施の形態１によれば、所定領域の道路ネットワークを記憶した道路網記憶手段５２と、道路ネットワークの読み出しと管理を行う道路網読出し手段５３と、位置・方位推定手段５６５の推定した現在位置の近傍に存在する道路ネットワークに走行道路と現在位置の候補を設定して管理する走行道路・位置候補管理手段５６６と、走行道路と現在位置の候補の中から走行道路と現在位置の候補を特定する走行道路・位置特定手段５６７とを設けて、位置・方位推定手段５６５で推定した車両の現在位置と進行方向をもとに、近傍の道路ネットワーク上に車両の現在位置を特定するマップマッチングを行うようにするとともに、自律航法用複合センサ（距離センサ１および相対方位センサ２ａ）にて計測した単位時間当たりの移動距離と進行方位変化と、ＧＰＳ受信機３で観測した車両の現在位置を、カルマンフィルタで統合処理しているので、以下のような効果が得られる。すなわち、ＧＰＳ受信機３の観測情報と自律航法用複合センサの計測情報に基づく車両の推定位置が一本化されているので、マップマッチングを簡単に行うことができる。また、マップマッチングを行って道路ネットワーク上に車両の現在位置を求めているので、より正確な車両の現在位置と進行方位を提供することができる。さらに、専用のカルマンフィルタを用いてＧＰＳ位置と自律航法用複合センサの計測情報から車両の現在位置と進行方位を同時に推定しているので、ＧＰＳ位置と自律航法用複合センサの計測情報の誤差が変動しても、車両の現在位置と進行方位を最適に推定することが可能となって信頼性が向上する。また、ＧＰＳ受信機３が非測位状態でも自律航法用複合センサの計測情報から車両の現在位置と進行方位を推定できるので、トンネルや高架下の道路などにおいても、安定して車両の現在位置と進行方位を提供できる。
【００６３】
実施の形態２．
次にこの発明の実施の形態２について説明する。
この実施の形態２によるロケータ装置は、車両の進行方位に合致する方向にある道路ネットワークのリンク群を先読みして保持するようにしたものであり、その構成は図１のブロック図に示した実施の形態１の場合と同様であるため、その図示および説明は省略する。
【００６４】
次に動作について説明する。
なお、その動作についても、基本的には実施の形態１の場合と同様であり、道路網読出し手段５３と走行道路・位置候補管理手段５６６の動作だけが異なるものであるため、その道路網読出し手段５３と走行道路・位置候補管理手段５６６の動作について説明し、その他については説明を省略する。
【００６５】
ここで、図１１は道路網読出し手段５３が走行道路候補の前方に接続可能なリンク群を先読みする際の状態を示した説明図である。道路網読出し手段５３は実施の形態１で説明した処理の他に、位置・方位推定手段５６５で推定した車両の進行方位に合致する方向で、走行道路候補を構成するリンク群に接続可能な複数のリンク群を所定距離または所定数だけ先読みし、この先読みした前方リンク群の保持も行っている。走行道路・位置候補管理手段５６６は、位置・方位推定手段５６５で推定した車両の現在位置を投影する対象にその前方リンク群も含めて処理を行う。
【００６６】
例えば、図１１に示すように、時刻ｔ２における走行道路の候補がリンク群ＬＳ3 を構成リンク群とするＲｏａｄC1-t2 のみであり、時刻ｔ３に位置・方位推定手段５６５で推定した車両の現在位置がPE-t3だったとすると、道路網読出し手段５３は時刻ｔ３に位置・方位推定手段５６５で推定した車両の進行方位に合致する方向で、時刻ｔ２における走行道路の候補ＲｏａｄC1-t2 に接続可能なリンク群から、リンク群ＬＳ4 、ＬＳ6 、およびＬＳ7 を前方リンク群として設定する。
【００６７】
このように、この実施の形態２によれば、走行道路・位置候補管理手段５６６にて、位置・方位推定手段５６５の推定した車両の進行方位に合致する方向で、走行道路候補の構成リンク群に接続可能なリンク群を、所定距離または所定数だけ先読みして保持しているので、車両の前後方向の誤差が推定位置に含まれていても、道路ネットワーク上に車両の現在位置を求めることができ、また、道路分岐点で車両の現在位置を見失うことがなくなるなどの効果が得られる。
【００６８】
実施の形態３．
次にこの発明の実施の形態３について説明する。
この実施の形態３によるロケータ装置は、推定した車両の現在位置の軌跡形状と道路ネットワークの形状を比較して、走行道路と現在位置の設定、管理を行うようにしたものであり、その構成は図１のブロック図に示した実施の形態１の場合と同様であるため、その図示および説明は省略する。
【００６９】
次に動作について説明する。
なお、その動作についても、基本的には実施の形態１の場合と同様であり、走行道路・位置候補管理手段５６６の動作だけが異なるものであるため、その走行道路・位置候補管理手段５６６の動作について説明し、その他については説明を省略する。
【００７０】
ここで、図１２は屈曲路を車両が通過する状態を示した説明図で、同図（ａ）から同図（ｃ）はそれぞれ時刻ｔ１から時刻ｔ３における状態を示している。走行道路・位置候補管理手段５６６は実施の形態１で説明した処理の他に、位置・方位推定手段５６５で推定した車両の現在位置を中心とする所定範囲内に道路分岐点、あるいは屈曲路がある場合には、推定した車両の現在位置に関する所定距離または所定時間以上前からの軌跡形状と、道路ネットワークで接続関係にあるリンク群の形状または接続関係を比較することにより、リンク群への投影点の調整および、リンク群の選択も行っている。
【００７１】
例えば、図１２（ａ）に示すように、時刻ｔ１において位置・方位推定手段５６５が推定した車両の現在位置Ｐ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 より小さな電圧値が出力される。加速度センサ４の出力信号ＳＧACC から加速度ａACC を求めるには、次の式（２９）に示すように、出力信号ＳＧACC からオフセットＯＦACC を減算した後、電圧値を加速度の単位に変換するためのスケールファクタＳＦACC （［ｍ／ｓ2 ／ｍＶ］）を乗算すればよい。
【００８６】
ａACCi＝（ＳＧACCi−ＯＦACCi）×ＳＦACCi×ΔＴ      ・・・（２９）
【００８７】
そして、図２に示すロケータ装置のメインルーチンの処理において、ステップＳＴ２０４で距離・速度計算手段５６１の処理として、加速度センサ４の出力電圧をＡ／Ｄ変換して上記式（２９）にて加速度ａACCiを求めた後、次の式（３０）および式（３１）を用いて速度ＶｅｌDRi と移動距離Δｄi を計算する。
【００８８】
0003745165.tif 000005
【００８９】
また、ステップＳＴ２０５ではオフセット検出手段５６２の処理として、所定時間分の加速度センサ４の出力信号の標準偏差を計算し、当該標準偏差が所定値以下であれば停車状態だと判断し、そうでなけば走行状態だと判断する。そして、停車状態であると判断した場合は、標準偏差を計算した同じ時間帯の出力信号の平均を計算して、オフセットＯＦACC として保持する。
【００９０】
このように、自律航法用複合センサを形成する距離センサ１を加速度センサ４で代替した場合でも、簡単にマップマッチングを行うことができ、車両の現在位置と進行方位をより正確に提供することが可能である。
【００９１】
実施の形態７．
なお、上記実施の形態１においては、道路網読出し手段５３は、リンク群の構成リンク数に上限を設けると説明したが、構成リンク数の代わりに、構成リンクの総リンク長に上限を設けるようにしてもよい。
【００９２】
実施の形態８．
また、上記実施の形態５においては、走行道路・位置特定手段５６７は、ＧＰＳ受信機３の所定時間における測位率が所定値以下になった時、当該状態になる直前に道路ネットワーク上に特定した車両の現在位置を起点にして、以後、位置・方位推定手段５６５で用いた車両の所定時間毎の移動距離と進行方位変化で示される分だけ前方に道路ネットワーク上に位置更新すると説明したが、ＧＰＳ位置の分散が所定値以上になる状態が所定時間継続した時にも、同様に位置更新するようにしてもよい。
【００９３】
【発明の効果】
以上のように、この発明によれば、ＧＰＳ受信機の観測情報と自律航法用複合センサの計測情報に基づく車両の推定位置が一本化されているので、マップマッチングを簡単に行うことが可能となり、また、マップマッチングを行って道路ネットワーク上に車両の現在位置を求めているので、より正確な車両の現在位置と進行方位を提供することができるロケータ装置が得られる効果がある。
【００９４】
この発明によれば、専用のカルマンフィルタにて、ＧＰＳ位置と自律航法用複合センサの計測情報から車両の現在位置と進行方位を同時に推定しているので、ＧＰＳ位置と自律航法用複合センサの計測情報の誤差が変動しても、車両の現在位置と進行方位を最適に推定することが可能となって、信頼性の向上がはかれ、また、ＧＰＳ受信機が非測位状態でも、自律航法用複合センサの計測情報から車両の現在位置と進行方位を推定できるので、トンネルや高架下などの道路を走行中でも、安定的に車両の現在位置と進行方位を提供できる効果がある。
【００９５】
この発明によれば、車両の進行方位に合致する方向にある道路ネットワークを先読みして保持しているので、車両の前後方向の誤差が推定位置に含まれていても、道路ネットワーク上に車両の現在位置を求めることが可能となり、また、道路分岐点で車両の現在位置を見失うことがなくなるなどの効果がある。
【００９６】
この発明によれば、車両の推定現在位置の軌跡形状と道路ネットワークの形状を比較して、道路ネットワーク上に車両の現在位置を求めているので、屈曲路を走行した際にも、安定した車両の現在位置を提供できる効果がある。
【００９７】
この発明によれば、車両の進行方位に合致する方向に道路ネットワークがなくなっても、以後しばらくの間は、連続性をもって継続的に車両の現在位置を求めいてるので、道路ネットワークが未整備経路を走行した際にも、安定した車両の現在位置を提供できる効果がある。
【００９８】
この発明によれば、ＧＰＳ受信機の測位率が低下する区間やＧＰＳ位置の分散が大きくなる区間の道路を走行した場合に、マップマッチングを、誤差が累積される推定位置ではなく、誤差が累積されない車両の移動ベクトルを用いて行っているので、上記区間の道路を走行した際にも、安定した車両の現在位置を提供できる効果がある。
【図面の簡単な説明】
【図１】  この発明の実施の形態１によるロケータ装置の構成を示すブロック図である。
【図２】  実施の形態１におけるメインルーチンの処理内容を示す流れ図である。
【図３】  実施の形態１における位置・方位推定手段の処理内容を示す流れ図である。
【図４】  実施の形態１における位置・方位推定手段による車両の現在位置の推定例を示す説明図である。
【図５】  実施の形態１における所定時間毎に実行される割込処理の内容を示す流れ図である。
【図６】  実施の形態１における距離センサから信号が出力されたときに実行される割込処理の内容を示す流れ図である。
【図７】  実施の形態１におけるＧＰＳ観測周期毎にＧＰＳ受信機から出力された観測情報を受信する割込処理の内容を示す流れ図である。
【図８】  実施の形態１における道路網読出し手段の、無分岐区間の接続関係にある複数のリンクを一つのリンク群として管理する際の状態を示す説明図である。
【図９】  実施の形態１における道路網読出し手段の、特に一本道においてリンク群を管理する際の状態を示す説明図である。
【図１０】  実施の形態１における走行道路・位置候補管理手段の、車両の走行道路と現在位置の候補を管理する際の状態を示す説明図である。
【図１１】  この発明の実施の形態２によるロケータ装置における、道路網読出し手段の走行道路候補の前方に接続可能なリンク群を先読みする際の状態を示す説明図である。
【図１２】  この発明の実施の形態３によるロケータ装置における、屈曲路を車両が通過する際の状態を示す説明図である。
【図１３】  この発明の実施の形態４によるロケータ装置における、車両の進行方位に合致する方向で走行道路候補のリンク群を前方接続できなくなったとき以降の状態を示す説明図である。
【図１４】  この発明の実施の形態５によるロケータ装置における、車両がトンネルを通過する際の前後の状態を示す説明図である。
【図１５】  この発明の実施の形態６のロケータ装置の構成を示すブロック図である。
【図１６】  従来のロケータ装置の構成例を示すブロック図である。
【図１７】  従来のロケータ装置における車両の現在位置の移動点の算出を示す説明図である。
【図１８】  従来のロケータ装置におけるＧＰＳ位置を近傍のリンク上へ投影した投影点の算出を示す説明図である。
【図１９】  従来のロケータ装置におけるルート算出を示す説明図である。
【図２０】  従来のロケータ装置にて、カルマンフィルタで現在位置の推定点が決まる様子を示す説明図である。
【符号の説明】
１  距離センサ（自律航法用複合センサ）、２ａ  相対方位センサ（自律航法用複合センサ）、３  ＧＰＳ受信機（絶対位置観測手段）、４  加速度センサ（自律航法用複合センサ）、５２  道路網記憶手段、５３  道路網読出し手段、５６５  位置・方位推定手段、５６６  走行道路・位置候補管理手段、５６７  走行道路・位置特定手段。
【図１】
0003745165.tif 000006
【図２】
0003745165.tif 000007
【図３】
0003745165.tif 000008
【図４】
0003745165.tif 000009
【図５】
0003745165.tif 000010
【図６】
0003745165.tif 000011
【図７】
0003745165.tif 000012
【図８】
0003745165.tif 000013
【図９】
0003745165.tif 000014
【図１０】
0003745165.tif 000015
【図１１】
0003745165.tif 000016
【図１２】
0003745165.tif 000017
【図１３】
0003745165.tif 000018
【図１４】
0003745165.tif 000019
【図１５】
0003745165.tif 000020
【図１６】
0003745165.tif 000021
【図１７】
0003745165.tif 000022
【図１８】
0003745165.tif 000023
【図１９】
0003745165.tif 000024
【図２０】
0003745165.tif 000025