JP3745165B2 - Locator device - Google Patents

Description

【００３７】
λ_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 より小さな電圧値が出力される。加速度センサ４の出力信号ＳＧ_ACC から加速度ａ_ACC を求めるには、次の式（２９）に示すように、出力信号ＳＧ_ACC からオフセットＯＦ_ACC を減算した後、電圧値を加速度の単位に変換するためのスケールファクタＳＦ_ACC （［ｍ／ｓ² ／ｍＶ］）を乗算すればよい。
【００８６】
ａ_ACCi＝（ＳＧ_ACCi−ＯＦ_ACCi）×ＳＦ_ACCi×ΔＴ ・・・（２９）
【００８７】
そして、図２に示すロケータ装置のメインルーチンの処理において、ステップＳＴ２０４で距離・速度計算手段５６１の処理として、加速度センサ４の出力電圧をＡ／Ｄ変換して上記式（２９）にて加速度ａ_ACCiを求めた後、次の式（３０）および式（３１）を用いて速度Ｖｅｌ_DRi と移動距離Δｄ_i を計算する。
【００８８】

【００８９】
また、ステップＳＴ２０５ではオフセット検出手段５６２の処理として、所定時間分の加速度センサ４の出力信号の標準偏差を計算し、当該標準偏差が所定値以下であれば停車状態だと判断し、そうでなけば走行状態だと判断する。そして、停車状態であると判断した場合は、標準偏差を計算した同じ時間帯の出力信号の平均を計算して、オフセットＯＦ_ACC として保持する。
【００９０】
このように、自律航法用複合センサを形成する距離センサ１を加速度センサ４で代替した場合でも、簡単にマップマッチングを行うことができ、車両の現在位置と進行方位をより正確に提供することが可能である。
【００９１】
実施の形態７．
なお、上記実施の形態１においては、道路網読出し手段５３は、リンク群の構成リンク数に上限を設けると説明したが、構成リンク数の代わりに、構成リンクの総リンク長に上限を設けるようにしてもよい。
【００９２】
実施の形態８．
また、上記実施の形態５においては、走行道路・位置特定手段５６７は、ＧＰＳ受信機３の所定時間における測位率が所定値以下になった時、当該状態になる直前に道路ネットワーク上に特定した車両の現在位置を起点にして、以後、位置・方位推定手段５６５で用いた車両の所定時間毎の移動距離と進行方位変化で示される分だけ前方に道路ネットワーク上に位置更新すると説明したが、ＧＰＳ位置の分散が所定値以上になる状態が所定時間継続した時にも、同様に位置更新するようにしてもよい。
【００９３】
【発明の効果】
以上のように、この発明によれば、ＧＰＳ受信機の観測情報と自律航法用複合センサの計測情報に基づく車両の推定位置が一本化されているので、マップマッチングを簡単に行うことが可能となり、また、マップマッチングを行って道路ネットワーク上に車両の現在位置を求めているので、より正確な車両の現在位置と進行方位を提供することができるロケータ装置が得られる効果がある。
【００９４】
この発明によれば、専用のカルマンフィルタにて、ＧＰＳ位置と自律航法用複合センサの計測情報から車両の現在位置と進行方位を同時に推定しているので、ＧＰＳ位置と自律航法用複合センサの計測情報の誤差が変動しても、車両の現在位置と進行方位を最適に推定することが可能となって、信頼性の向上がはかれ、また、ＧＰＳ受信機が非測位状態でも、自律航法用複合センサの計測情報から車両の現在位置と進行方位を推定できるので、トンネルや高架下などの道路を走行中でも、安定的に車両の現在位置と進行方位を提供できる効果がある。
【００９５】
この発明によれば、車両の進行方位に合致する方向にある道路ネットワークを先読みして保持しているので、車両の前後方向の誤差が推定位置に含まれていても、道路ネットワーク上に車両の現在位置を求めることが可能となり、また、道路分岐点で車両の現在位置を見失うことがなくなるなどの効果がある。
【００９６】
この発明によれば、車両の推定現在位置の軌跡形状と道路ネットワークの形状を比較して、道路ネットワーク上に車両の現在位置を求めているので、屈曲路を走行した際にも、安定した車両の現在位置を提供できる効果がある。
【００９７】
この発明によれば、車両の進行方位に合致する方向に道路ネットワークがなくなっても、以後しばらくの間は、連続性をもって継続的に車両の現在位置を求めいてるので、道路ネットワークが未整備経路を走行した際にも、安定した車両の現在位置を提供できる効果がある。
【００９８】
この発明によれば、ＧＰＳ受信機の測位率が低下する区間やＧＰＳ位置の分散が大きくなる区間の道路を走行した場合に、マップマッチングを、誤差が累積される推定位置ではなく、誤差が累積されない車両の移動ベクトルを用いて行っているので、上記区間の道路を走行した際にも、安定した車両の現在位置を提供できる効果がある。
【図面の簡単な説明】
【図１】 この発明の実施の形態１によるロケータ装置の構成を示すブロック図である。
【図２】 実施の形態１におけるメインルーチンの処理内容を示す流れ図である。
【図３】 実施の形態１における位置・方位推定手段の処理内容を示す流れ図である。
【図４】 実施の形態１における位置・方位推定手段による車両の現在位置の推定例を示す説明図である。
【図５】 実施の形態１における所定時間毎に実行される割込処理の内容を示す流れ図である。
【図６】 実施の形態１における距離センサから信号が出力されたときに実行される割込処理の内容を示す流れ図である。
【図７】 実施の形態１におけるＧＰＳ観測周期毎にＧＰＳ受信機から出力された観測情報を受信する割込処理の内容を示す流れ図である。
【図８】 実施の形態１における道路網読出し手段の、無分岐区間の接続関係にある複数のリンクを一つのリンク群として管理する際の状態を示す説明図である。
【図９】 実施の形態１における道路網読出し手段の、特に一本道においてリンク群を管理する際の状態を示す説明図である。
【図１０】 実施の形態１における走行道路・位置候補管理手段の、車両の走行道路と現在位置の候補を管理する際の状態を示す説明図である。
【図１１】 この発明の実施の形態２によるロケータ装置における、道路網読出し手段の走行道路候補の前方に接続可能なリンク群を先読みする際の状態を示す説明図である。
【図１２】 この発明の実施の形態３によるロケータ装置における、屈曲路を車両が通過する際の状態を示す説明図である。
【図１３】 この発明の実施の形態４によるロケータ装置における、車両の進行方位に合致する方向で走行道路候補のリンク群を前方接続できなくなったとき以降の状態を示す説明図である。
【図１４】 この発明の実施の形態５によるロケータ装置における、車両がトンネルを通過する際の前後の状態を示す説明図である。
【図１５】 この発明の実施の形態６のロケータ装置の構成を示すブロック図である。
【図１６】 従来のロケータ装置の構成例を示すブロック図である。
【図１７】 従来のロケータ装置における車両の現在位置の移動点の算出を示す説明図である。
【図１８】 従来のロケータ装置におけるＧＰＳ位置を近傍のリンク上へ投影した投影点の算出を示す説明図である。
【図１９】 従来のロケータ装置におけるルート算出を示す説明図である。
【図２０】 従来のロケータ装置にて、カルマンフィルタで現在位置の推定点が決まる様子を示す説明図である。
【符号の説明】
１ 距離センサ（自律航法用複合センサ）、２ａ 相対方位センサ（自律航法用複合センサ）、３ ＧＰＳ受信機（絶対位置観測手段）、４ 加速度センサ（自律航法用複合センサ）、５２ 道路網記憶手段、５３ 道路網読出し手段、５６５ 位置・方位推定手段、５６６ 走行道路・位置候補管理手段、５６７ 走行道路・位置特定手段。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a locator device that performs map matching for specifying a current position of a vehicle on a road network using observation information of a GPS (Global Positioning System) receiver and measurement information of a composite sensor for autonomous navigation.
[0002]
[Prior art]
FIG. 16 is a block diagram showing a configuration of a conventional locator device disclosed in, for example, Japanese Patent Laid-Open No. 10-47983. In the figure, 1 is a distance sensor that outputs a signal according to the moving distance of the vehicle, 2 is an azimuth sensor that outputs a signal according to the traveling direction of the vehicle, and 3 is a GPS receiver and antenna as absolute position observation means. It is. The distance sensor 1 and the direction sensor 2 form a composite sensor for autonomous navigation. 52 is a road network storage means (map dedicated disk), 53 is a road network reading means (CD drive) for acquiring a road network stored in the road network storage means (map dedicated disk) 52, 54 is a road network A memory control unit 55 controls the holding of various processing data in a memory, and 55 is an output control unit that controls output of information such as the position and direction of the vehicle. Further, 511 is a distance acquisition unit that calculates the moving distance of the vehicle from the output signal of the distance sensor 1, 512 is an azimuth acquisition unit that calculates the traveling direction of the vehicle from the output signal of the direction sensor 2, 513 is observed by the GPS receiver 3 515 to 517 are moving position calculating means, route and estimated position calculating means for performing map matching for specifying the current position of the vehicle on the road network, and display current. Position selection means.
[0003]
Next, the operation will be described.
Here, the operations of the moving position calculation means 515, the route and estimated position calculation means 516, and the display current position selection means 517 that perform map matching will be described with reference to FIGS. Here, FIG. 17 is an explanatory diagram showing calculation of a moving point of the current position of the vehicle, FIG. 18 is an explanatory diagram showing calculation of a projected point obtained by projecting a GPS position onto a nearby link, and FIG. 19 shows calculation of a route. FIG. 20 is an explanatory diagram showing how an estimated point of the current position is determined by a Kalman filter based on errors of the candidate point and moving point of the current position and the projected point of the GPS position.
[0004]
On the road network obtained by the memory control unit 54 from the road network reading means (CD drive) 53, the moving position calculation means 515 is a vehicle progress obtained from the direction obtaining means 512, starting from a candidate point of the current position of the vehicle. A moving point of the current position is calculated ahead by the distance information obtained from the distance acquisition unit 511 along the road that matches the direction. In FIG. 17, the candidate point S of the current position_{n-1, i}The moving point M of the two current positions located at the point of the moving distance m from_{n, i, k}(K is 1 and 2).
[0005]
Next, the route and estimated position calculation means 516 sets a GPS point projection point on the nearby road network from the GPS position acquired by the position acquisition means 513, and a combination of the current position movement point and the GPS position projection point. One or more routes are calculated. In FIG. 18, the GPS position g_{n, i}G over the two links that are nearby roads for the point_{n, i, j}Points (j is 1 and 2) are calculated. FIG. 19 shows the moving point M of the current position on the links of the link numbers # 1, # 2, and # 3._{n, i, 1}, M_{n, i, 2}, M_{n, i, 3}Are located on the links of links # 11 and # 12, which are road networks from the links of link numbers # 1, # 2 and # 3._{n, i}Projection point G of point_{n, i, 1}, G_{n, i, 2}Indicates a state where each is located. In this case, a maximum of six routes (eg, # 1 to # 11, # 1 to # 12, # 2 to # 11,...) Are obtained as routes from each moving point of the current position to each projected point of the GPS position. be able to. Then, the estimated point S of the current position is set at the position that is the internal dividing point on each route by the following equation (1)._{n, i, r}Is calculated. In this case, the coefficient K is 0.5.
[0006]
S_{n, i, r}= (1-K) × M_{n, ik}+ K × G_{n, i, j}        ... (1)
[0007]
As a result, the maximum number of estimated points of the current position indicated by the product of the number of candidate points of the current position and the number of routes is calculated. The coefficient K in the equation (1) is an error E related to the moving point of the current position._msAnd error E related to GPS position_gIs calculated by the following equation (2).
[0008]
K = E_ms ²/ (E_ms ²+ E_g ²(2)
[0009]
Then, as shown in FIG. 20, the error E of the candidate point of the current position_sAnd error E related to distance sensor 1_mTo error E from the current position to the moving point_msIs calculated by the following equation (3).
[0010]
E_ms= (E_s ²+ E_m ²)^1/2                        ... (3)
[0011]
Thereafter, the display current position selection means 517 specifies and outputs a new current position from among the plurality of estimated points of the current position. In this specification, the distance from each estimated point of the current position to the GPS position is evaluated, and a predetermined number of new current position candidate points are selected based on the estimated point of the current position with the best result. Then, the evaluation based on the direction from the new candidate point of the current position to the GPS position is further performed, and the candidate point of the current position with the best result is identified as the current position.
[0012]
[Problems to be solved by the invention]
Since the conventional locator device is configured as described above, the current position of the vehicle is calculated forward by the moving distance on the link of the road network connected in the direction matching the traveling direction of the vehicle. This is done by calculating an estimated point of the current position between the moving point of the current position and the projection point obtained by projecting the GPS position onto the link. The two positions estimated based on both information of the GPS receiver 3 and the autonomous navigation composite sensor (distance sensor 1 and direction sensor 2) are calculated by the Kalman filter to determine the correction amount by the GPS position. There is a problem that the information is used individually for map matching and the processing becomes complicated, and if the dispersion of GPS positions becomes large, it becomes the projection destination of GPS positions There is a problem that the link history is not connected, the projected point of the GPS position may move back and forth on the same link, and the identification of the current position becomes discontinuous or unstable there were.
[0013]
Furthermore, the traveling direction of the vehicle is updated based on the link direction of the link where the current position is specified, so if the current position is specified on the wrong link, the traveling direction of the vehicle is incorrect. As a result, there is a problem that the initial position of the vehicle becomes unstable due to the dispersion of the GPS position even if it is noticed that the map matching has failed after that, not only the position update on the link can not be performed properly thereafter. It was.
[0014]
The present invention has been made to solve the above-described problems, and is a map for identifying the current position of a vehicle on a road network based on observation information of absolute position observation means and measurement information of a compound sensor for autonomous navigation. An object of the present invention is to obtain a locator device that can realize matching by a simple method.
[0015]
In addition, the present invention provides a locator device that uses only the estimated position of a vehicle integrated based on the observation information of the absolute position observation means and the measurement information of the composite sensor for autonomous navigation as the estimated position handled by the means related to the map matching method The purpose is to obtain.
[0016]
Furthermore, an object of the present invention is to obtain a locator device that can efficiently perform map matching at a road junction or the like.
[0017]
Another object of the present invention is to provide a locator device that can specify the current position of the vehicle on a curved road with high accuracy.
[0018]
Another object of the present invention is to provide a locator device that can continuously and smoothly estimate the current position of a vehicle when traveling on a road network that has not been developed.
[0019]
Another object of the present invention is to provide a locator device that can specify the current position of the vehicle with high accuracy even in a place where the positioning rate of the absolute position observation means decreases.
[0020]
[Means for Solving the Problems]
In the locator device according to the present invention, a road network of a predetermined area is stored in the road network storage means, and the current position of the vehicle estimated by the position / orientation estimation means is determined from the road network storage means by the road network reading means. After reading the road network of the existing area and the adjacent area, if the link of the road network exists near the estimated current position of the vehicle and the link is straight line data connecting both ends of the link, the intersection In the road network reading means, a plurality of links having a connection relationship indicating a non-branching section such as a road, a single road, and a dead-end road are grouped together as a link group, and in the case of line data, the links are managed as a link group. In addition, the running road / position candidate management means applies the estimated current position of the vehicle to the link group when it is projected onto the link group. A plurality of link groups having a connection relationship in which a short distance is equal to or less than a predetermined value and the distance continuously projected on the same link group and a link group having a connection relationship is a predetermined value or more and projection points on the link group Is set and managed as a candidate for the driving road and the current position of the vehicle. The road candidate is determined to be a traveling road, and the projection point on the link group of the traveling road is specified as the current position of the vehicle.
[0021]
A locator apparatus according to the present invention is a position / orientation estimation of a locator apparatus having the road network storage means, position / orientation estimation means, road network reading means, traveling road / position candidate management means, and traveling road / position specifying means. Means to estimate the autonomous navigation position based on the Kalman filter equation from the movement distance per unit time measured by the compound sensor for autonomous navigation and the change in heading direction, while the autonomous navigation position is The current position of the vehicle and the heading direction are estimated by calculating so as to approach the current position of the vehicle observed by the observation means by a ratio determined by the Kalman gain.
[0022]
A locator device according to the present invention is a travel road / position of a locator device having the road network storage means, position / orientation estimation means, road network reading means, travel road / position candidate management means, and travel road / position specifying means. The candidate management means sets a predetermined distance or a predetermined number of link groups connectable to the constituent link groups of the traveling road candidates in a direction that matches the traveling direction of the vehicle estimated by the position / orientation estimating means for each traveling road candidate. Only the pre-read is stored and the link group is used as an object to project the current position of the vehicle estimated by the position / orientation estimation means.
[0023]
A locator device according to the present invention is a travel road / position of a locator device having the road network storage means, position / orientation estimation means, road network reading means, travel road / position candidate management means, and travel road / position specifying means. If there is a road junction or a curved road within a predetermined range centered on the current position of the vehicle estimated by the position / orientation estimation means in the candidate management means, a predetermined distance or a predetermined time or more regarding the estimated current position of the vehicle The trajectory shape from the front is compared with the shape or connection relationship of the link group that is connected in the road network, and the projection point on the link group is adjusted and the link group is selected.
[0024]
A locator device according to the present invention is a travel road / position of a locator device having the road network storage means, position / orientation estimation means, road network reading means, travel road / position candidate management means, and travel road / position specifying means. Based on the positional relationship between the projected current point and the estimated current position when the candidate roadway can no longer connect the link group of traveling road candidates in a direction that matches the traveling direction of the vehicle estimated by the position / orientation estimating means. After that, until the predetermined time elapses or until the vehicle travels the predetermined distance, the current position candidate is updated in front of the projection point, and after the predetermined time has elapsed or after traveling the predetermined distance If there are other driving road candidates, the driving road candidates are deleted, and if there are no other driving road candidates, the position is updated so that the current position candidates are brought in the direction of the estimated current position. Those were.
[0025]
A locator device according to the present invention is a travel road / position of a locator device having the road network storage means, position / orientation estimation means, road network reading means, travel road / position candidate management means, and travel road / position specifying means. When the positioning rate of the absolute position observing means at a predetermined time is less than or equal to a predetermined value by the specifying means, the position / orientation estimating means starts from the current position of the vehicle specified on the road network immediately before entering the state. The current position is updated on the road network based on the travel distance and travel direction change of the vehicle used in the above.
[0026]
DETAILED DESCRIPTION OF THE INVENTION
An embodiment of the present invention will be described below.
Embodiment 1 FIG.
FIG. 1 is a block diagram showing a configuration of a locator device according to Embodiment 1 of the present invention. In the figure, reference numeral 1 denotes a distance sensor that detects a moving distance of a vehicle, and outputs a pulse signal according to the traveling distance of the vehicle. Reference numeral 2a denotes a relative azimuth sensor that detects a change in the azimuth of the vehicle. In the first embodiment, a voltage corresponding to the yaw rate of the vehicle is output using a gyro. These correspond to the distance sensor 1 and the azimuth sensor 2 shown in FIG. 16, and these form an autonomous navigation composite sensor. Reference numeral 3 denotes a GPS receiver as an absolute position observing means equivalent to that shown with the same reference numerals in FIG. 16, and is connected to a GPS antenna for receiving radio waves from a satellite and is received by this GPS antenna. The absolute current position of the vehicle is observed from radio waves from the satellite and the position information (GPS position) is output. Reference numeral 5 denotes a signal processor including a computer that calculates the traveling direction and the current position of the vehicle according to a control program stored in advance in a memory (not shown).
[0027]
In the signal processor 5, reference numeral 561 denotes distance / speed calculation means for calculating the speed and moving distance of the vehicle based on the pulse signal from the distance sensor 1, and reference numeral 563 denotes offset detection for detecting the offset of the relative direction sensor 2a. Means. Reference numeral 564 denotes an azimuth change calculation means for inputting a detection signal from the offset detection means 563 and a voltage corresponding to the yaw rate of the vehicle from the relative azimuth sensor 2a and calculating a traveling azimuth change of the vehicle. Reference numeral 565 denotes a position / speed for estimating the current position and traveling direction of the vehicle based on the vehicle speed and moving distance calculated by the distance / speed calculating unit 561 and the moving direction change of the vehicle calculated by the direction change calculating unit 564. It is a direction estimation means.
[0028]
Reference numeral 52 denotes road network storage means for recording a predetermined range of road networks. Reference numeral 53 denotes road network reading means for reading a road network of a specified range from the road network storage means 52. The road network storage means 52 and the road network reading means 53 correspond to the road network storage means (map dedicated disk) 52 and the road network reading means (CD drive) 53 shown in FIG. If the link of the road network in the vicinity of the current position of the vehicle estimated by the estimating means 565 is searched and the link is straight line data, the links having a connection relation indicating the non-branching section are collected as one link group. For example, it has a function of managing the link as a link group. Reference numeral 566 denotes the road of the vehicle and the current position based on the road network read from the road network storage means 52 by the road network reading means 53 and the position and direction estimated by the position / orientation estimation means 565. Running road / position candidate management means for managing a plurality of candidates, 567 is a candidate for the road and current position of the vehicle managed by the running road / position candidate management means 566 and is estimated by the position / orientation estimation means 565. This is a traveling road / position specifying means for specifying the traveling road of the vehicle and the current position from the determined position and direction.
[0029]
Next, the operation will be described.
Hereinafter, the operation of each means of the signal processor 5 will be described with reference to the drawings. 2 is a flowchart showing the processing contents of the main routine, FIG. 3 is a flowchart showing the processing contents of the position / orientation estimation means 565, and FIG. 4 shows the current position of the vehicle by the position / orientation estimation means 565. It is explanatory drawing which shows an example of an estimation. FIG. 5 is a flowchart showing the contents of the interrupt process 1 executed every predetermined time Δt, FIG. 6 is a flowchart showing the contents of the interrupt process 2 executed when a signal is output from the distance sensor 1, and FIG. These are the flowcharts which show the content of the interruption process 3 which receives the observation information output for every observation period from the GPS receiver 3. FIG. In addition, FIG. 8 shows a plurality of links that are connected in a non-branching section such as a link between intersections, a dead-end road link, or a long one-way link when the road network link is indicated by straight line data. FIG. 9 is an explanatory diagram showing a state when the road network reading unit 53 manages a link group especially on a single road, FIG. These are explanatory drawings showing the state when the traveling road / position candidate management means 566 manages the traveling road and current position candidates of the vehicle.
[0030]
In FIG. 2, the main routine manages the operation of all means of the signal processor 5. First, in step ST201, all processes are initialized. In the next step ST202, it is determined with reference to the process start flag whether or not it is the process start timing. As a result, if the process start flag is set, the process proceeds to step ST203 as the process start timing. If cleared, the process waits until the process start flag is set as the process start timing. Next, after clearing the processing start flag for the next processing in step ST203, the process proceeds to step ST204. In step ST204, as the processing of the distance / speed calculation means 561, the output signal count value ΔN of the distance sensor 1 counted in the interrupt processing 2 every predetermined time Δt._iFrom the following equation (4), the movement distance Δd_iWith the velocity Vel according to equation (5)_DRiAnd the output signal count value ΔN for the next processing._iSet to 0. In addition, SF in this formula (4) and formula (5)_SPDjIs a scale factor for converting the output signal of the distance sensor 1 into a distance.
[0031]
Δd_i= Δd_i-1+ ΔN_i× SF_SPDi                ... (4)
Vel_DRj= | ΔN_j× SF_SPDj| / ΔT_GPS        ... (5)
[0032]
Next, an offset calculation process is performed by the offset detection unit 563 in step ST205. In step ST205, the speed Vel calculated in step ST204 is displayed._DRiIs 0 (that is, when the vehicle is stopped), the output signal SG for a predetermined time or more of the relative direction sensor 2a when the vehicle is stopped._GYRiThe average of the offset OF_GYRiAnd Next, in step ST206, the processing for calculating the traveling azimuth change by the azimuth change calculating means 564 is performed. In this step ST206, the output signal SG of the relative azimuth sensor 2a measured every predetermined time Δt using the following equation (6)._GYRiChange in direction of travel Δθ_iCalculate In addition, OF in this formula (6)_GYRiIs the offset, SF_GYRiIs a scale factor that converts the output signal into angular velocity.
[0033]
Δθ_i= Δθ_i-1+ (SG_GYRi-OF_GYRi) X SF_GYRi× Δt (6)
[0034]
Next, in step ST207, it is determined with reference to the GPS observation information reception flag whether reception of observation information from the GPS receiver 3 is completed. As a result, if the GPS observation information reception flag is set, it is determined that reception of observation information from the GPS receiver 3 is completed, and the process proceeds to step ST208. If it is cleared, the process returns to step ST202. In step ST208, the GPS observation information reception flag is cleared for the next processing. Next, in step ST209, the position / orientation estimation unit 565 performs position / orientation estimation processing. In this step ST209, the current position of the vehicle (λ_i, Φ_i) And heading θ_iIs estimated. Next, the process proceeds to step ST210, where the current position of the vehicle (λ_i, Φ_i). Next, in step ST211, the current position of the vehicle (λ_i, Φ_i) And heading θ_iAnd speed Vel_DRiIs output. Thereafter, the moving distance Δd during reception of observation information from the GPS receiver 3 in step ST212._iAnd travel direction change Δθ_iAnd return to step ST202.
[0035]
The position / orientation estimation means 565 is a vehicle travel distance Δd._iAnd travel direction change Δθ_iTo the current position (λ_i, Φ_i) And heading θ_iIs calculated by the following equations (7) to (12) and the GPS position (λ_Gi, Φ_Gi) And the current position of the system model vehicle (λ_i, Φ_i) Based on the observation model represented by the following equations (13) to (14).
[0036]

[0037]
λ_Gi= Λ_i+ Δλ_Gi                                ... (13)
φ_Gi= Φ_i+ Δφ_Gi                                (14)
[0038]
In the above formulas (7) to (12) and formulas (13) to (14), i is a discrete time, and λ_{i- 1} And φ_{i- 1} Is the longitude and latitude of the current position of the vehicle at discrete time i-1, θ_i-1Is the traveling direction of the vehicle at the discrete time i-1. SF_{d → λ}And SF_{d → φ}Is a coefficient for converting the unit [m] of the moving distance into the moving amount [″] in the longitude direction or the latitude direction._iAnd δφ_iIs the longitude error and latitude error of the current position of the vehicle, δΔθ_iAnd δΔd_iIs the heading change Δθ_iAnd travel distance Δd_iError. Also δλ_GiAnd δφ_GiIs the GPS position λ_GiAnd φ_GiError.
[0039]
Then, by the state equations shown in the following equations (15) to (19), the observation equations shown in equations (20) to (23), and the Kalman filter equations shown in equations (24) to (28), Current position (λ_i, Φ_i) And heading θ_iIs estimated.
[0040]
[Expression 1]

[0041]
[Expression 2]

[0042]
x_{i | i}= X_{i | i-1}+ K_i{Y_i-(Hx_{i | i-1}+ V_i)} (24)
x_{i + 1 | i}= F_ix_{i | i}+ G_iw_i                      ... (25)
K_i= Σ_{i | i-1}H^T[HΣ_{i | i-1}H^T+ Σ_vi]^-1        ... (26)
Σ_{i | i}= Σ_{i | i-1}-K_iHΣ_{i | i-1}                    ... (27)
Σ_{i + 1 | i}= F_iΣ_{i | i}F_i ^T+ G_iΣ_{w i}G_i ^T            ... (28)
[0043]
In the above formula (15) to formula (19) and formula (20) to formula (23), x_i, F_i, G_i, W_i, Y_i, V_iIs a state vehicle, state transition matrix, driving matrix, system noise, observation value, observation noise at discrete time i, and H is an observation matrix. Moreover, in Formula (24)-Formula (28), x_{i | i}Is the estimator of the state vector at discrete time i, x_{i + 1 | i}Is the predicted amount of the state vector at discrete time i + 1 at discrete time i, K_iIs Kalman gain, Σ_{i | i}Is the estimator of the state vector covariance matrix at discrete time i, Σ_{i + 1 | i}Is the predicted amount of the covariance matrix at discrete time i + 1 at discrete time i, Σ_viIs the covariance matrix of the observation error, Σ_wiIs a system error covariance matrix, and since each element of the matrix can be obtained by calculating the determinant of the equation, the description is omitted here.
[0044]
Next, the processing contents of the position / orientation estimating means 565 will be described in more detail with reference to the flowchart of FIG.
First, in step ST301, the speed Vel calculated by the distance / speed calculation means 561 is displayed._DRiIs determined to be greater than 0. If it is 0, it is determined that the vehicle is stopped, and the process ends. If it is greater than 0, the process proceeds to step ST302. In step ST302, it is determined whether or not the GPS receiver 3 has moved in a two-dimensional or three-dimensional positioning state and has moved a predetermined distance or more after estimating the current position and traveling direction of the vehicle by the Kalman filter at the previous processing timing. As a result, if the GPS receiver 3 is in a two-dimensional or three-dimensional positioning state and the current position and traveling direction are estimated by the Kalman filter at the previous processing timing and the vehicle moves more than a predetermined distance, the process proceeds to step ST303. If the GPS receiver 3 is in a non-positioning state or has not moved more than a predetermined distance after calculating the current position and traveling direction by the Kalman filter at the previous processing timing, the process proceeds to step ST305.
[0045]
In step ST303, the GPS position at a predetermined time (λ_Gi, Φ_Gi) And the current position (λ_i, Φ_i) Position difference, that is, the standard deviation is calculated for each of the latitude difference and the longitude difference, and the standard deviation is calculated as the observation error v._iAnd the covariance matrix of observation error Σ_viCalculate Next, in step ST304, the integrated value of the GPS speed and the distance Δd obtained from the distance sensor 1_iIs a distance error element δΔd which is a matrix element of system error._iAnd the directional error element δΔθ, which is a matrix element of the system error, as the direction difference between the movement directions of the GPS position and the locus of the current position of the vehicle at a plurality of points separated by a predetermined distance._iThen, the process proceeds to step ST306. On the other hand, in step ST305, Kalman gain K_iIs cleared to 0 and the process proceeds to step ST306. In step ST306, the GPS position is changed to the observed value y._iFormula (7) to Formula (12), Formula (13) to Formula (14), Formula (15) to Formula (19), Formula (20) to Formula (23), Formula (24) to Formula ( 28) according to the state vector estimator x_{i | i}Calculate At that time, the Kalman gain K is calculated from a value proportional to the offset fluctuation amount of the relative azimuth sensor 2a._iKalman gain K so that all matrix elements of_iThe state vector estimator x by adjusting all matrix elements of_{i | i}Calculate
[0046]
Next, in step ST307, using equation (24) to equation (28), the state vector prediction amount x_{i + | i}Calculate Next, in step ST308, as in step ST302, the GPS receiver 3 performs two-dimensional or three-dimensional positioning, and the current position (λ_i, Φ_i) And heading θ_iIt is determined whether it has moved more than a predetermined distance after estimating. As a result, the GPS receiver 3 performs two-dimensional or three-dimensional positioning, and the previous position (λ_i, Φ_i) And heading θ_iIf it has moved a predetermined distance or more after estimation, the process proceeds to step ST309, whether the GPS receiver 3 is not positioning or the current position (λ_i, Φ_i) And heading θ_iIf it has not moved more than a predetermined distance after calculating, the process ends. In step ST309, the error covariance estimator Σ_{i | i}Is calculated using Expression (24) to Expression (28), and in step ST310, the predicted amount Σ_{i + 1 | i}In step ST311, Kalman gain K_iAre calculated using Equation (24) to Equation (28), respectively, and the process is terminated.
[0047]
Next, the operation of estimating the current position of the vehicle in the position / orientation estimating means 565 will be described with reference to FIG.
First, FIG. 4A shows a state in which the current position of the vehicle is estimated when the variance of the GPS position becomes large while the vehicle is traveling straight ahead. At this time, an azimuth error that is a matrix element of the system error is shown. It is assumed that both the element and the distance error element are small. In this case, since the position / orientation estimation means 565 detects that the GPS position error, which is an observation error, increases in the section where the GPS position variance is large, only the section where the GPS position variance is large is detected. Calculate the gain smaller. As a result, in the section where the GPS position variance is small, the current position of the vehicle is estimated to be close to the GPS position, but in the section where the GPS position dispersion is large, the amount of the current position of the vehicle is close to the GPS position is estimated to be small. .
[0048]
FIG. 4B shows a state where the current position of the vehicle is estimated when the offset of the relative direction sensor 2a drifts while the vehicle is traveling straight ahead. At this time, the GPS position variance is small. It is assumed that it was. In this case, the position / orientation estimation means 565 detects that an azimuth error has occurred because the GPS position variance is small but the traveling azimuth change per unit time is biased. To do. As a result, it is estimated that the current position of the vehicle is moved toward the GPS position without being affected by the drift generated in the relative direction sensor 2a. In the case of general autonomous navigation that does not use the Kalman filter, the vehicle gradually turns in the direction in which the current position of the vehicle is biased, as indicated by the alternate long and short dash line, under the influence of drift generated in the relative direction sensor 2a. End up.
[0049]
FIG. 4C shows a state in which the current position of the vehicle is estimated when the vehicle turns right when the scale factor of the relative direction sensor 2a is smaller than the true value. Is assumed to be small. In this case, the position / orientation estimating means 565 detects that an azimuth error has occurred after the right turn while the variance of the GPS position is small, and therefore calculates the Kalman gain larger after the right turn. As a result, although an error occurs in the estimated traveling direction because the turning angle at the time of the right turn becomes small, after the azimuth error is detected after the right turn, it is estimated that the current position of the vehicle is moved toward the GPS position. In the case of general autonomous navigation that does not use the Kalman filter, as indicated by the alternate long and short dash line, the current position of the vehicle goes straight in the wrong direction while the turning angle is insufficient.
[0050]
Next, the interrupt process 1 will be described.
This interrupt process 1 is started every predetermined time Δt, and as shown in the flowchart of FIG. 5, in step ST501, a process start flag indicating a process start timing is set, and this interrupt process 1 is ended.
[0051]
Next, the interrupt process 2 will be described.
This interrupt process 2 is started when a signal is output from the distance sensor 1, and as shown in the flowchart of FIG. 6, 1 is added to the vehicle speed signal counter in step ST601, and this interrupt process 2 is ended. To do.
[0052]
Next, the interrupt process 3 will be described.
This interrupt process 3 receives observation information output from the GPS receiver 3 for each observation period. First, as shown in the flowchart of FIG. 7, the observation information output from the GPS receiver 3 in step ST701. Is received and stored. Next, in step ST702, it is determined whether or not reception of all the observation information has been completed. If completed, the process proceeds to step ST703, and if not completed, the interrupt process 3 is terminated. In step ST703, assuming that all the observation information has been received, the GPS observation information reception flag is set.
[0053]
Next, the map matching method performed in step ST210 of FIG. 2 will be described from the operation of the road network reading means 53 related to this map matching method.
The road network reading unit 53 receives the current position and traveling direction of the vehicle estimated by the position / orientation estimation unit 565, and reads the road network of the area where the estimated current position exists and the adjacent area from the road network storage unit 52. If the link of the read road network is straight line data that connects the nodes at both ends with a straight line, a plurality of links that are connected in a non-branching section such as between intersections, single roads, and dead ends Manage as a link group. If the link of the read road network is broken line data, the link based on the broken line data is handled as a link group.
[0054]
Here, when the road network reading means 53 reads a road network as shown in FIG. 8A from the road network storage means 52, for example, as shown in FIG. Edit the link group consisting of interpolation points. In FIG. 8A, a circle mark is a node indicating the coordinates of the link of the road network, and a link formed by connecting the circle marks with a straight line is a link based on straight line data. N₁To N₁₂Is the node number, L₁To L₁₂Indicates a link number.
[0055]
However, in reality, in order to reduce the load of map matching, a link group is created for links within a radius of about 300 m centered on the current position estimated by the position / orientation estimation means 565.
[0056]
If the read road network includes, for example, a single road as shown in FIG. 9, the single road is connected to a link within a predetermined range centered on the current position estimated by the position / orientation estimation means 565. If there is no end point, i.e., an intersection or a dead end, a link group of a non-branching section cannot be created and managed as described above. Therefore, the road network reading means 53 has an upper limit for the number of constituent links of the link group. For the sake of explanation, the upper limit value is four in the case of FIG. First, at the time t0 shown in FIG. 5A, the current position P estimated by the position / orientation estimating means 565 is used._E-t0A predetermined range R centered on_MLink L in₁To L_ThreeAnd L₈On the other hand, the link group LS shown in the table in the figure.₁To LS_ThreeCreate At this time, the link group LS_ThreeSince this node is not the end of a single road, it is a temporary link group. The current position P estimated by the position / orientation estimating means 565 at time t1 shown in FIG._E-t1A predetermined range R centered on_MLink L in_FourTo L₇As shown in the table in the figure, the link group LS_ThreeLink L to_FourAnd L_FiveOnly connect the link L₆And L₇Is a new link group LS_FourAs a tentative management.
[0057]
Next, the operation of the traveling road / position candidate management means 566 will be described with reference to FIG.
First, immediately after the power is reset, the traveling road / position candidate management means 566 causes the shortest distance to the constituent links when the current position of the vehicle estimated by the position / orientation estimation means 565 is projected to the link group is equal to or less than a predetermined value. Is set as a candidate for a traveling road, and a projection point is set as a candidate for a current position. After that, as the vehicle moves, the current position estimated by the position / orientation estimation means 565 is projected one after another on the neighboring link group, and the position is projected on the same link group and the link group having the connection relation. The distance that can be continued is managed for each candidate road. For example, FIG. 10A shows the state of the traveling road / position candidate management means 566 at time t0 immediately after the power reset, and the traveling road / position candidate management means 566 at time t1 and time t2 in the subsequent movement of the vehicle. This state is shown in FIG. 2B and FIG.
[0058]
First, at time t0 immediately after the power reset shown in FIG. 10 (a), the traveling road / position candidate management means 566 has the current position P estimated by the position / orientation estimation means 565._E-t0Link group LS near₂Projection point P only_C1-t0Suppose that And the link group LS₂And projection point P_C1-t0Road candidate road_C1-t0And current position candidate P_C1-t0Start managing as.
[0059]
Next, in FIG. 5B showing the state at time t1, the current position P estimated by the position / orientation estimating means 565 is shown._E-t1A predetermined range R centered on_MLink group LS included in_ThreeAnd LS_FourIs the road candidate Road at time t0_C1-t0Link group LS₂Link group LS_ThreeAnd LS_FourAbove each projection point P_C1-t1And P_C2-t1Ask for. And the road candidate Road at time t0_C1-t0Link group LS₂To link group LS_ThreeA candidate road for the first driving road at time t1 as a route connecting to_C1-t1And the projection point P_C1-t1Is a candidate for the first current position. In addition, the road candidate road at time t0_C1-t0Link group LS₂To link group LS_FourA candidate road for the second traveling road at time t1 as a route connecting to_C2-t1And the projection point P_C2-t1Is the second candidate for the current position. Here, the link group LS₂To LS_ThreeRoute to the first candidate, link group LS₂To LS_FourThe reason for determining the route to the second candidate is that the current position P estimated by the position / orientation estimation means 565_E-t1And projection point P_C1-t1The distance between is the current position P_E-t1And projection point P_C2-t1It was because it was shorter than the distance between.
[0060]
Next, in the same figure (c) showing the state at time t2, the current position P estimated by the position / orientation estimation means 565 is shown._E-t2A predetermined range R centered on_MThe candidate for the road that was included in the list and was able to continue to obtain the projection point is the first candidate Load_C1-t2Similarly, the first candidate P of the current position_C1-t2Only remains.
[0061]
Next, the operation of the traveling road / position specifying means 567 will be described with reference to FIG.
The traveling road / position specifying unit 567 determines the traveling road candidate having the longest distance for which the position has been projected as the traveling road, and specifies the projection point on the link group constituting the traveling road as the current position of the vehicle. For example, immediately after the power reset at time t0 shown in FIG. 10A, the traveling road / position specifying unit 567 has only one candidate for the traveling road and the current position, so the candidate road for the traveling road is loaded._C1-t0And current position candidate P_C1-t0Are identified as the traveling road and the current position at time t0, respectively. Subsequently, at time t1 shown in FIG. 5B, there are two candidates for the traveling road and the current position, but if both candidates occur at the same time, the candidate road for the first traveling road is loaded._C1-t1And first current position candidate P_C1-t1Priority is given to the travel road and the current position at time t1. Subsequently, in the case of time t2 shown in FIG. 5C, there is only one candidate for the traveling road and the current position, so the candidate road for traveling road is Load._C1-t2And current position candidate P_C1-t2Are identified as the traveling road and the current position at time t2, respectively. Thereafter, when there are a plurality of candidates for the traveling road and the current position, the candidate having the longest distance for which the position has been projected is prioritized and specified as the traveling road and the current position.
[0062]
As described above, according to the first embodiment, the road network storage unit 52 that stores the road network of the predetermined area, the road network reading unit 53 that reads and manages the road network, and the position / orientation estimation unit 565. Driving road / position candidate management means 566 for setting and managing driving road and current position candidates in a road network existing in the vicinity of the estimated current position, and the driving road and the current position from the driving road and current position candidates And a road / position specifying means 567 for specifying a candidate for the vehicle, and specifying the current position of the vehicle on the nearby road network based on the current position and traveling direction of the vehicle estimated by the position / orientation estimating means 565 Map matching is performed, and a unit of time per unit time measured by an autonomous navigation composite sensor (distance sensor 1 and relative orientation sensor 2a). A moving distance and heading changes, the current position of the vehicle observed by the GPS receiver 3, since the integration process in the Kalman filter, the following effects can be obtained. That is, since the estimated position of the vehicle based on the observation information of the GPS receiver 3 and the measurement information of the autonomous navigation composite sensor is unified, map matching can be easily performed. Further, since the current position of the vehicle is obtained on the road network by performing map matching, a more accurate current position and traveling direction of the vehicle can be provided. Furthermore, since the current position and traveling direction of the vehicle are simultaneously estimated from the GPS position and the measurement information of the autonomous navigation composite sensor using a dedicated Kalman filter, the error in the GPS position and the measurement information of the autonomous navigation composite sensor fluctuates. Even so, the current position and traveling direction of the vehicle can be optimally estimated, and the reliability is improved. In addition, since the current position and traveling direction of the vehicle can be estimated from the measurement information of the autonomous navigation composite sensor even when the GPS receiver 3 is in a non-positioning state, the current position of the vehicle can be stably detected even on a tunnel or an underpass road. Advancing direction can be provided.
[0063]
Embodiment 2. FIG.
Next, a second embodiment of the present invention will be described.
The locator device according to the second embodiment is configured to pre-read and hold a link group of a road network in a direction that matches the traveling direction of the vehicle, and the configuration thereof is shown in the block diagram of FIG. Since it is the same as that of the case of form 1, the illustration and description are omitted.
[0064]
Next, the operation will be described.
The operation is basically the same as in the first embodiment, and only the operations of the road network reading means 53 and the traveling road / position candidate management means 566 are different. The operation of the means 53 and the traveling road / position candidate management means 566 will be described, and description of the other will be omitted.
[0065]
Here, FIG. 11 is an explanatory view showing a state when the road network reading means 53 pre-reads a link group connectable in front of the traveling road candidate. In addition to the processing described in the first embodiment, the road network reading unit 53 includes a plurality of links that can be connected to the link group constituting the traveling road candidate in a direction that matches the traveling direction of the vehicle estimated by the position / orientation estimating unit 565. The link group is pre-read by a predetermined distance or a predetermined number, and the pre-read front link group is also held. The traveling road / position candidate management means 566 performs processing including the forward link group on the object to be projected on the current position of the vehicle estimated by the position / orientation estimation means 565.
[0066]
For example, as shown in FIG. 11, the candidate for the road at time t2 is the link group LS._ThreeLoad with a component link group_C1-t2The current position of the vehicle estimated by the position / orientation estimation means 565 at time t3 is P_E-t3If so, the road network reading means 53 is in a direction that matches the traveling direction of the vehicle estimated by the position / orientation estimation means 565 at time t3, and the road candidate road at time t2 is loaded._C1-t2From the link group connectable to the link group LS_Four, LS₆, And LS₇Is set as the forward link group.
[0067]
As described above, according to the second embodiment, the traveling road / position candidate management means 566 includes the configuration link group of traveling road candidates in a direction that matches the traveling direction of the vehicle estimated by the position / orientation estimation means 565. Since the link group that can be connected to is pre-read by a predetermined distance or a predetermined number, the current position of the vehicle is obtained on the road network even if the estimated position includes an error in the longitudinal direction of the vehicle. In addition, there is an effect that the current position of the vehicle is not lost at the road junction.
[0068]
Embodiment 3 FIG.
Next, a third embodiment of the present invention will be described.
The locator device according to Embodiment 3 compares the estimated trajectory shape of the current position of the vehicle with the shape of the road network, and sets and manages the traveling road and the current position. Since it is the same as that of the first embodiment shown in the block diagram of FIG. 1, its illustration and description are omitted.
[0069]
Next, the operation will be described.
The operation is basically the same as that in the first embodiment, and only the operation of the traveling road / position candidate management means 566 is different. The operation will be described, and the description of the other will be omitted.
[0070]
Here, FIG. 12 is an explanatory view showing a state where the vehicle passes through the curved road, and FIGS. 12A to 12C show states from time t1 to time t3, respectively. In addition to the processing described in the first embodiment, the traveling road / position candidate management unit 566 has a road branch point or a curved road within a predetermined range centered on the current position of the vehicle estimated by the position / orientation estimation unit 565. In some cases, the projected shape on the link group is compared by comparing the shape of the trajectory from a predetermined distance or more than a predetermined time before the estimated current position of the vehicle with the shape or connection relation of the link group in the road network. Point adjustment and link group selection are also performed.
[0071]
For example, as shown in FIG. 12A, the current position P of the vehicle estimated by the position / orientation estimation means 565 at time t1._E-t1When there are three points a, b, and c that can be projected onto a nearby link group from the time t0, the traveling road / position candidate management means 566 starts from time t0 when only one point is projected. Trajectory shape of current position of vehicle estimated by position / orientation estimation means 565 (P_E-t0→ P_E-t1) And the connection shape (P_C-t0→ a 、 P_C-t0→ b, P_C-t0→ c), the projection point at time t1 is point c (P_C-t1). Similarly to the case of FIG. 7B, the projection points at time t2 and time t3 are also represented by point e (P)._C-t2) And g point (P_C-t3).
[0072]
Thus, according to the third embodiment, the traveling road / position candidate managing means 566 compares the trajectory shape of the current position of the vehicle estimated by the position / orientation estimating means 565 with the shape of the road network. Since the adjustment of the projection point to the link group and the selection of the link group are performed, there is an effect that it is possible to provide a stable current position of the vehicle even when traveling on a curved road.
[0073]
Embodiment 4 FIG.
Next, a fourth embodiment of the present invention will be described.
The locator device according to the fourth embodiment obtains the current position of the vehicle for a while even if the road network disappears in the direction that matches the traveling direction of the vehicle. Since it is the same as the case of Embodiment 1 shown in the figure, its illustration and description are omitted.
[0074]
Next, the operation will be described.
The operation is basically the same as that in the first embodiment, and only the operation of the traveling road / position candidate management means 566 is different. The operation will be described, and the description of the other will be omitted.
[0075]
Here, FIG. 13 is an explanatory diagram showing a state after the link group of the traveling road candidate cannot be connected in the forward direction in the direction matching the traveling direction of the vehicle estimated by the position / orientation estimating means 565. (a) and (b) show the states at time t1 and time t3, respectively. The traveling road / position candidate management means 566 performs the following processing in addition to the processing described in the first embodiment. That is, based on the positional relationship between the projection point at the time when the link group of the traveling road candidate cannot be connected in the forward direction in the direction that matches the traveling direction of the vehicle estimated by the position / orientation estimation unit 565 and the estimated current position. Thereafter, the current position candidate is updated in front of the projection point until a predetermined time elapses or until the vehicle travels a predetermined distance. In addition, after a predetermined time has elapsed or after traveling a predetermined distance, if there are other driving road candidates, the driving road candidates are deleted, and if there are no other driving road candidates, the estimated current position is moved to. The position is updated so that the current position candidates are gathered.
[0076]
For example, as shown in FIG. 13A, the current position P of the vehicle estimated by the position / orientation estimating means 565 at time t0._E-t0Is projected onto the nearby link group, and the current position P of the vehicle estimated by the position / orientation estimation means 565 at time t1._E-t1When there is no link group in the vicinity to project the trajectory shape of the current position of the vehicle estimated by the position / orientation estimation means 565 from time t0 (P_E-t0→ P_E-t1), The projected point P at time t0 is shown._C-t0And the current position P estimated by the position / orientation estimation means 565_E-t0Positional relationship (longitude difference: Δλ_t0, Latitude difference Δφ_t0) Based on the projection point P_C-t0Current position candidate P in front of_C-t1Update. In the case of time t3 shown in FIG. 13B, the current position P of the vehicle estimated by the position / orientation estimating means 565 from time t0._E-t0Is the predetermined distance Δd_FREUntil the vehicle travels in the same manner as in FIG._FREAfter the time t2 when the current position P is reached, the current position P estimated by the position / orientation estimation means 565 is used._E-t3Current position candidate P in the direction of_C-t3Update the position so that
[0077]
As described above, according to the fourth embodiment, even when the road / location candidate management unit 566 has no road network in a direction that matches the traveling direction of the vehicle estimated by the position / orientation estimation unit 565, Since the current position candidates are continuously updated with a continuity until the predetermined time has elapsed or the vehicle has traveled a predetermined distance, routes that include new roads or farm roads that are not road networks are included. Even when the vehicle travels, it is possible to provide a stable current vehicle position.
[0078]
Embodiment 5. FIG.
Next, a fifth embodiment of the present invention will be described.
The locator device according to the fifth embodiment performs map matching using a vehicle movement vector when traveling in a section where the positioning rate of the GPS receiver is reduced or in a section where the GPS position variance is large. Since the configuration is the same as that of the first embodiment shown in the block diagram of FIG. 1, the illustration and description thereof are omitted.
[0079]
Next, the operation will be described.
The operation is basically the same as that in the first embodiment, and only the operation of the traveling road / position specifying means 567 is different. Therefore, the operation of the traveling road / position specifying means 567 is different. The description is omitted, and the description of the others is omitted.
[0080]
Here, FIG. 14 is an explanatory view showing a state before and after the vehicle passes through the tunnel. In addition to the processing described in the first embodiment, the traveling road / position specifying means 567 is used when the positioning rate of the GPS receiver 3 at a predetermined time becomes a predetermined value or less, or when the dispersion of the GPS position becomes large. In addition, the current position of the vehicle is determined based on the current position of the vehicle specified on the road network immediately before entering the state. A process of updating the position on the road network ahead by the amount indicated by the change in direction is also performed.
[0081]
For example, as shown in FIG. 14, the current position P of the vehicle estimated by the position / orientation estimation means 565 immediately before entering the tunnel._E-t0Current position P on the neighboring links_t0Is specified. The current position P specified at time t0 is between the time t1 and the time t7 when the positioning rate in the predetermined time of the GPS receiver 3 is not more than the predetermined value while passing through the tunnel._t0The starting distance of the vehicle used in the position / orientation estimating means 565 and the travel direction change of the vehicle at a predetermined time, that is, the amount indicated by the movement vector at the predetermined time indicated by the bold arrow in FIG. , Update the position on the forward link group (P in the figure)_t1~ P_t7) At the time t8 when the positioning rate of the GPS receiver 3 at a predetermined time becomes equal to or greater than a predetermined value, the current position P of the vehicle estimated by the position / orientation estimation means 565 is obtained._E-t8Is projected onto the neighboring links, and the current position P_t8I will decide.
[0082]
As described above, according to the fifth embodiment, when the positioning rate of the GPS receiver 3 at a predetermined time becomes equal to or less than the predetermined value, the vehicle specified on the road network immediately before by the traveling road / position specifying means 567. Since the current position is updated on the road network based on the travel distance and travel direction change of the vehicle per unit time from the current position of the vehicle, the section where the positioning rate of the GPS receiver 3 decreases Even when traveling on a road in a section where the GPS position variance is large, map matching is performed using a vehicle movement vector in which no error is accumulated, so that it is possible to provide a stable current position of the vehicle. It is done.
[0083]
Embodiment 6 FIG.
Next, a sixth embodiment of the present invention will be described.
In the locator device according to the sixth embodiment, a composite sensor for autonomous navigation is formed using an acceleration sensor built in the locator device instead of the distance sensor 1, and FIG. 15 shows such an embodiment of the present invention. It is a block diagram which shows the structure of the locator apparatus by the form 6. In the figure, reference numeral 4 denotes an acceleration sensor in the locator device that is used in place of the distance sensor 1 to detect vehicle acceleration, and 562 is provided in the signal processor 5 to detect an offset of the acceleration sensor 4. Offset detecting means. The other parts are the same as the parts of the first embodiment shown with the same reference numerals in FIG.
[0084]
Next, the operation will be described.
The operation is the same as that in the first embodiment except that the distance / speed calculation means 561 calculates the moving distance and speed from the output signal of the acceleration sensor 4. 2 will be described with respect to the offset detection of the acceleration sensor 4 by the offset detection means 562 and the operation of step ST202 in FIG.
[0085]
First, the operation of the acceleration sensor 4 will be described.
The acceleration sensor 4 has an acceleration a_ACCOutput signal SG even when is 0_ACCIs a predetermined voltage value ([mV], hereinafter, offset OF_ACCAt the time of acceleration offset OF_ACCLarger voltage value and offset OF when decelerating_ACCA smaller voltage value is output. Output signal SG of acceleration sensor 4_ACCTo acceleration a_ACCIs obtained as shown in the following equation (29)._ACCFrom offset OF_ACCAfter subtracting, the scale factor SF for converting the voltage value into the unit of acceleration_ACC([M / s²/ MV]).
[0086]
a_ACCi= (SG_ACCi-OF_ACCi) X SF_ACCi× ΔT (29)
[0087]
Then, in the processing of the main routine of the locator device shown in FIG. 2, the output voltage of the acceleration sensor 4 is A / D converted as the processing of the distance / speed calculation means 561 in step ST204, and the acceleration a is expressed by the above equation (29)._ACCiAfter calculating the velocity Vel using the following equations (30) and (31):_DRiAnd travel distance Δd_iCalculate
[0088]

[0089]
In step ST205, as the processing of the offset detection means 562, the standard deviation of the output signal of the acceleration sensor 4 for a predetermined time is calculated, and if the standard deviation is equal to or less than the predetermined value, it is determined that the vehicle is in a stopped state. It is determined that the vehicle is in a running state. If it is determined that the vehicle is in a stopped state, the average of the output signals in the same time zone in which the standard deviation is calculated is calculated, and the offset OF_ACCHold as.
[0090]
Thus, even when the distance sensor 1 forming the composite sensor for autonomous navigation is replaced with the acceleration sensor 4, map matching can be easily performed, and the current position and traveling direction of the vehicle can be provided more accurately. Is possible.
[0091]
Embodiment 7 FIG.
In the first embodiment, it has been described that the road network reading unit 53 sets an upper limit on the number of constituent links of the link group. However, instead of the number of constituent links, an upper limit is set on the total link length of the constituent links. It may be.
[0092]
Embodiment 8 FIG.
In the fifth embodiment, the traveling road / position specifying means 567 specifies the road network on the road network immediately before entering the state when the positioning rate of the GPS receiver 3 at a predetermined time is equal to or lower than the predetermined value. It has been described that the current position of the vehicle is used as a starting point, and thereafter, the position is updated on the road network forward by the amount indicated by the moving distance and the moving direction change of the vehicle at a predetermined time used by the position / orientation estimation means 565. The position may be updated in the same manner when the GPS position variance is equal to or greater than a predetermined value for a predetermined time.
[0093]
【The invention's effect】
As described above, according to the present invention, since the estimated position of the vehicle based on the observation information of the GPS receiver and the measurement information of the composite sensor for autonomous navigation is unified, map matching can be easily performed. In addition, since the current position of the vehicle is obtained on the road network by performing map matching, there is an effect that a locator device that can provide a more accurate current position and traveling direction of the vehicle is obtained.
[0094]
According to this invention, since the current position and traveling direction of the vehicle are simultaneously estimated from the measurement information of the GPS position and the autonomous navigation composite sensor by the dedicated Kalman filter, the GPS position and the measurement information of the autonomous navigation composite sensor are estimated. Even if the error of the vehicle fluctuates, it is possible to optimally estimate the current position and traveling direction of the vehicle, improving reliability, and even if the GPS receiver is in a non-positioning state, it is a compound for autonomous navigation Since the current position and traveling direction of the vehicle can be estimated from the measurement information of the sensor, there is an effect that the current position and traveling direction of the vehicle can be stably provided even while traveling on a road such as a tunnel or an overhead road.
[0095]
According to the present invention, since the road network in the direction matching the traveling direction of the vehicle is pre-read and held, even if an error in the front-rear direction of the vehicle is included in the estimated position, It is possible to obtain the current position, and there is an effect that the current position of the vehicle is not lost at the road junction.
[0096]
According to the present invention, the current vehicle position is obtained on the road network by comparing the trajectory shape of the estimated current position of the vehicle with the shape of the road network, so that the stable vehicle even when traveling on a curved road There is an effect that can provide the current position.
[0097]
According to the present invention, even if the road network disappears in a direction that matches the traveling direction of the vehicle, the road network continuously determines the current position of the vehicle for a while after that. Even when the vehicle travels, it is possible to provide a stable current position of the vehicle.
[0098]
According to the present invention, when driving on a road in a section where the positioning rate of the GPS receiver is reduced or a section where the GPS position variance is large, map matching is not an estimated position where the error is accumulated, but an error is accumulated. Since the movement vector of the vehicle that is not used is used, there is an effect that it is possible to provide a stable current position of the vehicle even when traveling on the road in the section.
[Brief description of the drawings]
FIG. 1 is a block diagram showing a configuration of a locator device according to Embodiment 1 of the present invention.
FIG. 2 is a flowchart showing processing contents of a main routine in the first embodiment.
FIG. 3 is a flowchart showing the processing contents of a position / orientation estimation unit in the first embodiment.
4 is an explanatory diagram showing an example of estimation of the current position of the vehicle by the position / orientation estimation means in Embodiment 1. FIG.
FIG. 5 is a flowchart showing the contents of interrupt processing executed every predetermined time in the first embodiment.
FIG. 6 is a flowchart showing the content of interrupt processing executed when a signal is output from the distance sensor in the first embodiment.
7 is a flowchart showing the contents of an interrupt process for receiving observation information output from a GPS receiver for each GPS observation period in Embodiment 1. FIG.
FIG. 8 is an explanatory diagram showing a state when a plurality of links that are connected in a non-branching section are managed as one link group by the road network reading unit according to the first embodiment.
FIG. 9 is an explanatory diagram showing a state of the road network reading unit according to the first embodiment, particularly when a link group is managed on a single road.
FIG. 10 is an explanatory diagram illustrating a state when a traveling road / position candidate management unit according to the first embodiment manages candidates for a traveling road and a current position of a vehicle.
FIG. 11 is an explanatory diagram showing a state when pre-reading a link group connectable in front of a traveling road candidate of a road network reading unit in a locator device according to Embodiment 2 of the present invention;
FIG. 12 is an explanatory diagram showing a state when a vehicle passes through a curved road in a locator device according to Embodiment 3 of the present invention;
FIG. 13 is an explanatory diagram showing a state after a link group of traveling road candidates can no longer be connected forward in a direction that matches the traveling direction of a vehicle in a locator apparatus according to Embodiment 4 of the present invention;
FIG. 14 is an explanatory diagram showing a state before and after a vehicle passes through a tunnel in a locator apparatus according to Embodiment 5 of the present invention;
FIG. 15 is a block diagram showing a configuration of a locator device according to a sixth embodiment of the present invention.
FIG. 16 is a block diagram illustrating a configuration example of a conventional locator device.
FIG. 17 is an explanatory diagram showing calculation of a moving point of the current position of a vehicle in a conventional locator device.
FIG. 18 is an explanatory diagram illustrating calculation of a projection point obtained by projecting a GPS position onto a nearby link in a conventional locator device.
FIG. 19 is an explanatory diagram showing route calculation in a conventional locator device.
FIG. 20 is an explanatory diagram showing how an estimated point of a current position is determined by a Kalman filter in a conventional locator device.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 Distance sensor (combined sensor for autonomous navigation), 2a Relative bearing sensor (composite sensor for autonomous navigation), 3 GPS receiver (absolute position observation means), 4 Acceleration sensor (composite sensor for autonomous navigation), 52 Road network storage means 53 Road network reading means, 565 position / orientation estimation means, 566 traveling road / position candidate management means, 567 traveling road / position specifying means.

Claims (6)

Road network storage means for storing the road network;
A position / orientation in which the current position and traveling direction of the vehicle on the road network read from the road network storage means are estimated at predetermined intervals based on the measurement information of the autonomous navigation composite sensor and the observation information of the absolute position observation means. An estimation means;
Search for a link in the road network that exists in the vicinity of the current position of the vehicle estimated by the position / orientation estimation means, and if the link is straight line data connecting both ends of the link with a straight line, a connection indicating a no-branch section Road network reading means that collects related links as one link group and treats the link as a link group if it is broken line data;
The shortest distance to the link group when the estimated current position of the vehicle is projected onto the link group is equal to or less than a predetermined value, and the distance continuously projected on the link group having the same link group and connection relationship is a predetermined value. A traveling road / position candidate managing means for setting and managing the link group having the connection relationship as a candidate for a traveling road of the vehicle and a projection point on the link group as a candidate for the current position;
A driving road / position specifying means for specifying a driving road candidate having the longest distance for which the position has been projected as a driving road and specifying a projection point on a link group of the driving road as a current position of the vehicle; Locator device. A vehicle model observed by an absolute position observation means and a system model in which the position / orientation estimation means calculates the current position and traveling direction of the vehicle from the movement distance per unit time measured by the composite sensor for autonomous navigation and changes in the traveling direction. Based on an observation model that represents the relationship between the current position of the system model and the current position in the system model, the current position and traveling direction in the system model are set in the matrix elements of the state vector, and the absolute position observation means is used in the observed value 2. The locator apparatus according to claim 1, wherein the current position and traveling direction of the vehicle, which are matrix elements of the state vector, are estimated by a Kalman filter in which the current position of the vehicle observed in step 1 is set. A predetermined distance or a predetermined number of link groups that can be connected to the constituent link group of the driving road candidate in a direction that matches the traveling direction of the vehicle estimated by the driving road / position candidate management unit by the position / orientation estimation unit. 2. The link group stored in advance and held in advance is used as a target for projecting the current position of the vehicle estimated by the position / orientation estimation means. The locator device described. If the road / position candidate management means has a road junction or a curved road within a predetermined range centered on the current position of the vehicle estimated by the position / orientation estimation means, a predetermined distance or a predetermined time or more before Compare the trajectory shape of the current position of the vehicle estimated from the above and the shape of the link group that is connected in the road network, and adjust the projection point to the link group and select the link group The locator device according to claim 1, characterized in that: After the time when the traveling road / position candidate management means cannot connect the other link group forward to the constituent link group of the traveling road candidate in a direction that matches the traveling direction of the vehicle estimated by the position / orientation estimation means, a predetermined time Until a predetermined distance elapses or until the vehicle travels a predetermined distance, the current position candidate is updated in front of the projection point based on the positional relationship between the projection point at the time when the forward connection is not possible and the estimated current position. After a lapse of time or after traveling a predetermined distance, if there are other driving road candidates, the driving road candidates are deleted, and if there are no other driving road candidates, the current position is in the direction of the estimated current position. The locator apparatus according to claim 1, wherein the position is updated so as to bring the candidates. When the traveling road / position specifying means has a positioning rate of the absolute position observing means at a predetermined time below a predetermined value, or when the variance of the current position of the vehicle observed by the absolute position observing means is above a predetermined value. If the vehicle's current position specified on the road network starts just before entering the state when it lasts for more than a predetermined time, the movement distance and travel direction change of the vehicle used in the position / orientation estimation means will be 2. The locator apparatus according to claim 1, wherein the position of the current position is updated on the road network.

Images