JP2009058242A - 車両位置・方位修正方法及び車両位置・方位修正装置 - Google Patents

Abstract

【課題】GPS受信可能になってから短時間で車両位置、車両方位を正しい位置、方位に修正する車両位置・方位修正方法及び修正装置を提供する。
【解決手段】GPS受信機と自律航法センサーを併用するナビゲーションシステムにおいて、車両位置・方位修正部は、ナビゲーション制御停止時における車両位置と車両方位を保存し、これらをナビゲーション起動時における車両の初期位置と初期方位とする。そして、ナビゲーション起動後、自律航法センサーから得られるセンサー出力信号と、前記車両初期位置と初期方位とを用いて車両位置と方位を推測すると共に、GPS受信機より得られるGPS方位の信頼度が高くなったか監視する。GPS受信機より得られるGPS方位の信頼度が高くなったとき、車両位置・方位修正部は、該GPS方位と推測した車両位置を用いて車両の現在位置を修正する。
【選択図】図１

Description

本発明は車両位置・方位修正方法及び車両位置・方位修正装置に関わり、特に、GPS受信機と自律航法センサーを併用するナビゲーションシステムのナビゲーション起動時及び走行時における車両位置・方位修正方法及び車両位置・方位修正装置に関する。

ナビゲーション装置は、車両の現在位置に応じた地図データをDVD，ハードディスクHDD等の地図データ記憶部から読み出してディスプレイ画面に描画すると共に、走行に応じて車両位置を示すマ−ク(車両位置マーク)を地図上で移動させ、あるいは車両位置マークをディスプレイ画面の一定位置（例えばディスプレイ画面の中心位置）に固定表示して地図をスクロ−ル表示する。
かかるナビゲ−ション装置において、車両の現在位置を測定することが不可欠である。このため、従来は、車両に搭載した距離センサーと方位センサー（ジャイロ）などの自律航法センサーを用いて車両位置を測定する測定法（自立航法）、衛星を用いたGPS（Global Positioning System)による測定法（衛星航法）、両者を併用した方法が実用化されている。
また、ナビゲーション装置は、車両位置をマップマッチング処理により修正する機能や、目的地までの誘導経路を探索して地図上に表示する経路誘導機能、交差点案内機能、その他の機能を備えている。マップマッチング機能は、自立航法センサーの出力信号を用いて推定した車両位置の誤差が大きくなって車両位置が道路から外れたときに該車両位置を道路リンク上に修正する機能である。位置誤差が大きくなりすぎて道路リンク上にマップマッチングにより位置修正できなくなれば、GPSにより測位した位置データを車両位置として採用し、以後マップマッチング処理により道路リンク上に位置修正する。

自立航法においては、距離センサーと相対方位センサーの出力に基づき積算により以下のようにして車両位置を推測する。図１４は自立航法による車両位置推測方法の説明図であり、距離センサーは車両がある単位距離L₀(たとえば１０ｍ)走行する毎にパルスを出力するものとし、また、基準方位（θ＝0）をＸ軸の正方向、基準方位から反時計方向回りを＋方向とする。前回の車両位置を点P₀（X₀，Y₀）、点P₀での車両進行方向の絶対方位をθ₀、単位距離L₀走行した時点での相対方位センサーの出力をΔθ₁であるとすると、車両位置の変化分は、
ΔX＝L₀・cos（θ₀＋Δθ₁）
ΔY＝L₀・sin（θ₀＋Δθ₁）
となり、今回の点P1での車両進行方向の推定方位θ₁と推定車両位置（X₁，Y₁）は、
θ₁＝θ₀＋Δθ₁ (1)
X₁＝X₀＋ΔX＝X₀＋L₀・cosθ₁ (2)
Y₁＝Y₀＋ΔY＝Y₀＋L₀・sinθ₁ (3)
としてベクトル合成により計算できる。従って、スタート地点での車両の絶対方位と位置座標をGPSにより与えれば、その後、車両が単位距離走行する毎に、(1)〜(3)式の計算を繰り返すことにより車両位置をリアルタイムで検出(推定)できる。

しかし、自立航法では走行するにつれて誤差が累積して推定車両位置が道路から外れる。そこで、マップマッチング処理により推定車両位置を道路データと照合して道路上の実車両位置に修正する。
図１５は投影法によるマップマッチングの説明図である。現車両位置が点P_i-1（X _i-1，Y _i-1）にあり、車両方位がθ_i-1であったとする（図では点P _i-1は道路RDaと一致していない場合を示す）。点P _i-1より一定距離L₀（例えば１０ｍ）走行したときの相対方位がΔθ_iであれば、自立航法による推定車両位置P_i′（X_i′，Y_i′）と、P_i′での推定車両方位θ_iは、次式
θ_i ＝θ_i-1＋Δθ_i
X_i′＝X_i-1＋L₀・cosθ_i
Y_i′＝Y_i-1＋L₀・sinθ_i
により求められる。

このとき、(a) 推定車両位置P_i′を中心に２００ｍ四方に含まれ、しかも、垂線を降ろすことのできるリンク（道路を構成するエレメント）であって、推定車両位置P_i′での推定車両方位θ_iとリンクの成す角度が一定値以内（たとえば45°以内）で、かつ、推定車両位置P_i′からリンクに降ろした垂線の長さが一定距離（たとえば１００ｍ）以内となっているものを探す。ここでは道路ＲＤa上の方位θa₁のリンクLKa₁（ノードNa₀とNa₁を結ぶ直線）と道路ＲＤb上の方位θ_b1のリンクLKb₁（ノードNb₀とNb₁を結ぶ直線）となる。ついで、(b) 推定車両位置P_i′からリンクLKa₁，LKb₁に降ろした垂線RLia、RLibの長さを求める。(c) しかる後、次式
Z=dL・20＋dθ・20 (dθ≦25°) (4)
Z=dL・20＋dθ・40 (dθ＞25°) (4)′
により係数Ｚを演算する。なお、dLは推定車両位置Ｐ_ｉ′からリンクに降ろした垂線の長さ(推定車両位置からリンクまでの距離)、dθは推定車両方位θ_iとリンクの成す角度であり、角度dθが大きいほど重み係数を大きくしている。

(d) 係数値Zが求まれば、以下の（1）、（2）、（3）の条件、
(1）距離dL≦７５ｍ（最大引き付け距離７５ｍ）
(2）角度差ｄθ≦30°（最大引き付け角度30°）
（3）係数値Z≦1500
を満足するリンクを求め、係数値が最小のリンクをマッチング候補(最適道路)とする。ここではリンクＬＫa_１となる。(e) そして、点P_i-1と点P_i′を結ぶ走行軌跡SH_iを垂線RLiaの方向に点P_i-1がリンクLKa₁上（またはリンクLKa₁の延長線上）に来るまで平行移動して、点P_i-1とP_i′の移動点PT_i-1とPT_i′を求め、(f) 最後に、点PT _i-1を中心にPT _i′がリンクLKa₁上（またはリンクLKa₁の延長線上）に来るまで回転移動して移動点を求め、実車両位置Pi（X _i，Y _i）とする。なお、上記条件を満足するリンクが存在しないときには、マップマッチング不可能状態になったものと見なす。

誤差が累積してマップマッチング不可能となってミスマッチングにより道路外を走行したり、間違った道路を走行することがある。かかる場合、GPS受信機より得られるGPSデータ(GPS位置データ、GPS方位データ)を利用して自車位置・方位を修正し、しかる後マップマッチングにより道路上に位置、方位を修正して自律航法を継続する。
ところで、従来のGPSデータを利用して自車位置・方位を修正するタイミングは、GPS位置とGPS方位の両方の信頼度(精度)が共に高くなったときである。このため、周辺に建物があるなど、GPS受信環境が悪いとき、GPS位置・方位が同時に良くなることが少ないため、マップマッチング不可能になってミスマッチングが起きても自車位置・方位修正がなかなかできず、車両位置が正しい位置に戻る復帰タイミングが遅れる問題があった。特に、以下の場合において顕著である。

(1) 第1の場合
第1の場合は、建物内のターンテーブルに乗ってナビゲーションシステムの電源を切り、ターンテーブル回転後にナビゲーションを起動して、建物を脱出する場合である。ナビゲーションシステムでは、ナビゲーション電源を切断(ナビゲーション制御停止)したときの車両位置、車両方位が、次のナビゲーション起動時の車両の初期位置、初期方位になる。このため、ターンテーブルの回転により車両の向きが変化すると、ナビゲーション起動時の自車方位が実際の方向とずれ、建物脱出後にミスマッチングする。図１６はかかる状況の説明図であり、PBLは立体駐車場、TBLは立体駐車場のターンテーブル、RDは道路である。ナビゲーション電源切断時における車両方位がθsのときに車両がターンテーブルTBLにより180⁰回転するものとすれば、ナビゲーション起動時において、初期方位θsと実際の車両方位との間に180⁰のズレが生じる。このため、以後、点線に沿って走行を開始し、かつ自律航法センサーを用いて自車位置と方位を推測すると、推測自車位置の軌跡(自律航法軌跡)DRTは実線で示すように実際の走行軌跡ARTからずれて建物脱出後にミスマッチングする。

(2) 第２の場合
第２の場合は、GPS受信が不可能な立体駐車場内においてナビゲーションを起動し、上りながら、あるいは下りながら旋回を繰り返して、駐車場を脱出する場合である。立体駐車場内では車両が傾いた状態で旋回するため、ジャイロ感度誤差が大きくなり、自車方位が徐々にずれてゆき、駐車場脱出時における実際の車両方位と推測方位が大幅にずれ、結果として、駐車場脱出後にミスマッチングする。
上記第１、第２の場合、建物脱出後においてGPSデータを利用して自車位置・方位を修正するタイミングは、GPS位置とGPS方位の両方の信頼度(精度)が共に高くなったときであり、GPS受信環境が悪いときに車両位置を正しい位置に戻す復帰タイミングが遅れる。なお、一般的にGPS受信環境が悪いときは、 GPS方位に比べて、GPS位置の精度が悪い。

従来技術として駐車場などの利用により発生した車両の現在位置および進行方向の誤差を、極力早いタイミングで補正する技術が提案されている(特許文献１)。この従来技術においては、基準位置から自律航法により車両位置、方位を推測し、GPS受信可能状態になった後、(1)基準位置とGPS位置を結ぶベクトルと、基準位置と自律航法による推測位置を結ぶベクトルとがなす角度が設定角度以上になったとき、あるいは、(2) 基準位置とGPS位置を結ぶ距離と基準位置と推測位置を距離差が設定値以上になったとき、GPS位置データ、GPS方位データを車両の現在位置、方位として採用するものである。
特開２００２−１４８０６３号公報

しかし、従来技術は、角度誤差あるいは距離誤差が設定値以上になったときにGPS位置データ、GPS方位データを車両の現在位置、方位として採用するものであるが、そのときのGPS位置、GPS方位に信頼性があるか否かは不明であり、正確な位置、方位修正ができない問題がある。また、GPS位置、GPS方位に信頼性がある状態になっても、角度誤差あるいは距離誤差が設定値以上にならずに位置、方位修正が遅れる問題がある。
以上から、本発明の目的は、GPS受信可能になったとき、GPS方位に基づいて車両位置、車両方位を修正することである。
本発明の別の目的は、GPS受信可能になってから短時間で車両位置、車両方位を正しい位置、方位に修正することである。

・車両位置・方位修正方法
本発明は、GPS受信機と自律航法センサーを併用するナビゲーションシステムのナビゲーション起動時における車両位置・方位修正方法である。
本発明の第１の車両位置・方位修正方法は、ナビゲーション停止時における車両位置と車両方位を保存し、これらをナビゲーション起動時における車両の初期位置と初期方位とするステップ、ナビゲーション起動後、自律航法センサーから得られるセンサー出力信号と、前記車両の初期位置と初期方位とを用いて車両位置と方位を推測するステップ、ナビゲーション起動後、GPS受信機より得られるGPS方位の信頼度が高くなったか監視するステップ、GPS受信機より得られるGPS方位の信頼度が高くなったとき、該GPS方位と前記推測した車両位置を用いて車両の現在位置を修正するステップ、を有している。前記車両位置修正ステップにおいて、前記GPS方位と前記推測した車両方位との差Δθを計算し、前記車両初期位置を中心に前記推測車両位置を該差Δθだけ回転することにより得られる車両位置を車両の現在位置とする。あるいは、前記車両位置修正ステップにおいて、前記車両初期位置と推測車両位置間の距離を計算し、前記車両初期位置からGPS方位の方向に該距離だけ離れた位置を車両の現在位置とする。
本発明の第２の車両位置・方位修正方法は、自動車が立体駐車場に入ったことを検出した時の車両位置と車両方位を保存するステップ、前記立体駐車場に入ったことを検出した後、自律航法センサーから得られるセンサー出力信号と、前記保存した車両位置と車両方位とを用いて車両の現在位置と現在方位を推測するステップ、前記立体駐車場に入ったことを検出した後、GPS受信機より得られるGPS方位の信頼度が高くなったか監視するステップ、GPS受信機より得られるGPS方位の信頼度が高くなったとき、該GPS方位と前記推測した車両位置を用いて車両の現在位置を修正するステップ、を有している。

・車両位置・方位修正装置
本発明はGPS受信機と自律航法センサーを併用するナビゲーションシステムのナビゲーション起動時における車両位置・方位修正装置である。
本発明の第１の車両位置・方位修正装置は、ナビゲーション停止時における車両位置と車両方位をナビゲーション起動時における車両の初期位置と初期方位として保存する保存部、ナビゲーション起動後、自律航法センサーから得られるセンサー出力信号と、前記車両の初期位置と初期方位とを用いて車両位置と方位を推測する位置・方位推測部、ナビゲーション起動後、GPS受信機より得られるGPS方位の信頼度が高くなったか監視するGPS方位信頼度判定部、GPS受信機より得られるGPS方位の信頼度が高くなったとき、該GPS方位と前記推測した車両位置を用いて現在の車両位置を修正する修正部、を有する。
前記修正部は、前記GPS方位と前記推測した車両方位との差Δθを計算する計算手段、前記車両初期位置を中心に前記推測車両位置を該差Δθだけ回転することにより車両の現在位置を修正する修正手段、を備えている。あるいは、前記修正部、前記車両初期位置と推測車両位置間の距離を計算する手段、前記車両初期位置からGPS方位の方向に該距離だけ離れた位置を車両の現在位置とする手段、を備えている。
本発明の第２の車両位置・方位修正装置は、自動車が立体駐車場に進入したことを検出する検出部、自動車が立体駐車場に進入したことを検出した時の車両位置と車両方位を保存する保存部、前記立体駐車場に進入したことを検出した後、自律航法センサーから得られるセンサー出力信号と、前記保存した位置と方位とを用いて車両の現在位置と現在方位を推測する位置・方位推測部、前記立体駐車場に入ったことを検出した後、GPS受信機より得られるGPS方位の信頼度が高くなったか監視するGPS方位信頼度判定部、GPS受信機より得られるGPS方位の信頼度が高くなったとき、該GPS方位と前記推測した車両位置を用いて車両の現在位置を修正する修正部、を有している。

本発明によれば、GPS方位の信頼度が高くなったとき、該GPS方位と推測位置を用いて車両の現在位置を修正するため、GPS受信不可能状態からGPS受信可能になったとき、短時間で車両位置、車両方位を正しい位置、方位に修正することができる。
本発明によれば、GPS受信不可能な立体駐車場に駐車してナビゲーション電源を切り、その後ナビゲーションを起動して立体駐車場から出る場合においても、短時間で車両位置、車両方位を正しい位置、方位に修正することができる。
本発明によれば、ナビゲーション電源を切り、その後ターンテーブルで回転された後に走行する場合であっても短時間で車両位置、車両方位を正しい位置、方位に修正することができる。
本発明によれば、立体駐車場内で停車しない場合であっても立体駐車場から出る際、短時間で車両位置、車両方位を正しい位置、方位に修正することができる。

（A）本発明の概要
図１は、立体駐車場などの建物内のターンテーブルに乗ってナビゲーションシステムの電源を切り、ターンテーブル回転後にナビゲーションを起動して、建物を脱出する状況説明図、図２は図１の状況において本発明を適用した例であり、図１において、PBLは立体駐車場、TBLは立体駐車場のターンテーブル、RDは道路である。
ターンテーブル上に車両が移動した後にナビゲーション電源を切断し、ついで、ターンテーブルTBLにより車両の向きが所定角度、例えば180°回転し、その後にナビゲーション起動して走行を開始するものとする。ただし、ナビゲーション電源切断時における車両方位はθs、車両位置はPs(xs,ys)であり、また、ナビゲーション起動後、ナビゲーションシステムは自律航法により車両位置と方位を推測する。
ターンテーブルの回転により、ナビゲーション起動時において、ナビゲーションシステムが記憶する車両方位θsと実際の車両方位との間に180°のズレが生じる。このため、以後、点線に沿って走行を開始すると自律航法軌跡DRTは実線で示すように実際の走行軌跡ARTから逆方向に向く。
しかし、本発明では、GPS方位の信頼度が高くなったか監視しており、車両がポイントPgに到達した時点でGPS方位θgの信頼度が高くなれば、該GPS方位を車両の現在方位とすると共に、該GPS方位と自律航法により推測した位置(推測位置)Pd(xd,yd)、方位 (推測方位) θdを用いて車両の現在位置を修正する。このとき、初期位置Ps(xs,ys)からGPS方位の信頼度が高くなる推測位置Pd(xd,yd)までの走行距離は短いため、該初期位置に対する相対的な推測位置、推測方位の信頼度が高く、これらを用いて修正した車両の現在位置は正しい。たとえ、修正位置が道路から外れてもその後に行なわれるマップマッチング処理により道路上に修正できる。

図２(A)において、推測位置Pdの座標xd，yd、方位θd、および車両現在位置Pgの方位θgは既知であり、GPS方位θｇsと推測方位θdよりそれらの差Δθを計算する。ついで該差Δθだけ初期位置Ps(xs,ys)を中心に推測位置Pd(xd,yd)を回転すれば、車両の現在位置Pgの座標（xg,yg）を得ることができる。すなわち、図２(B)に示すように、(1) GPS方位θgと推測方位θdよりそれらの差Δθを計算し、(2)ついで、該差Δθだけ初期位置Ps(xs,ys)を中心に推測位置Pd(xd,yd)を回転して車両現在位置Pgの座標（xg,yg）を計算する。

（B）ナビゲーション装置
図３は本発明の車両位置・方位修正部を備えたナビゲーション装置の構成図であり、地図記録媒体(CD-ROM、DVD、ＨＤＤなど)１には地図データが記録されており、必要に応じて読み取られるようになっている。GPS受信機２はGPS衛星から送られてくるGPS電波を受信し、受信信号に基づいて１秒周期で車両の位置（GPS位置）と方位（GPS方位）を測定してナビゲーション装置４に入力する。自立航法センサー３は車両回転角度を検出するジャイロなどの角度センサーと一定走行距離毎にパルスを発生する走行距離センサーを備え、各センサー検出信号をナビゲーション装置４に入力する。ナビゲーション制御部４は車両の位置・方位の計算及び修正制御や車両周辺地図画像発生制御、経路探索/誘導制御、交差点案内制御等を行う。モニター５はナビゲーション装置４からの指示に従って車両周辺の地図や誘導経路、車両位置マーク、その他の案内情報やメニューを表示する。

ナビゲーション制御部４において、地図読取部１１は自車位置周辺の地図データを地図記憶媒体１から読み取って地図バッファ１２に保存する。地図バッファ１２は、自車位置に応じた図葉と周辺図葉の合計９つの図葉を少なくとも保持し、走行に応じて地図をスクロール表示できるようになっている。地図描画部１３は地図バッファ１２に読み出されている地図データを用いて車両周辺の地図画像を発生してVRAM １４に保存し、合成部１５はVRAMから読取った地図画像と車両位置マーク、誘導経路画像などを合成してモニター５に表示する。
車両位置・方位修正部１６は車両の現在位置、方位を推定する制御(車両位置トラッキング制御)やマップマッチング制御、後述する位置修正制御等を行う。車両位置マーク発生部１７は地図上の車両位置に車両位置マークを発生して合成部１５に入力する。ナビゲーション制御部４は、その他、目的地までの経路を探索して表示する経路探索・表示制御部、交差点案内制御を行う案内制御部などを備えているが図示を省略する。

図４は車両位置・方位修正部１６の構成図であり、位置・方位推測部２１は、位置不明な最初の時点でGPS位置を車両位置として採用し、以後、自立航法センサーの出力信号を用いて車両の位置(xd，yd)および方位(θd)を推測し、推測位置xd，ydを車両の現在位置ｘ、ｙをとして地図読取部１１や車両位置マーク発生部１９等に入力する。また、位置・方位推測部２１は、車両停車後にナビゲーション電源が切断されると、その時の車両位置および車両方位をそれぞれナビゲーション起動時の初期位置（xs、ys）、初期方位θsとしてメモリMEMに保存する。マップマッチング制御部２２はマップマッチング処理を行って位置・方位推測部２１より推測された車両位置(x、y)を道路リンクの上に修正し、また、位置・方位推測部２１により推測された車両位置の誤差が累積してマップマッチング処理による修正が不可能となれば、GPS位置を車両現在位置として該位置・方位推測部２１に採用させ、以後、自立航法センサーの出力信号を用いた車両位置推定を継続させる。GPS信頼度判定部２３は、GPS受信機により受信されたGPS信号より算出したGPS方位の信頼度が高いか否かを判定し、判定結果をマップマッチング処理部２２や位置・方位修正部２４に入力する。位置・方位修正部２４はナビゲーション起動時あるいは立体駐車場からの脱出時などにおいてGPS方位が安定した時に車両の現在位置及び方位を修正する。

(Ｃ) GPS方位の信頼度判定処理
図５はGPS方位の信頼度が高いか否かを判定するGPS信頼度判定部２３の判定処理フローの一例である。
まず、GPS測定可能であるかチェックし(ステップ１０１)、測定不可能であれば、GPS方位θ_GPSは信頼度が無く、信頼度フラグをオフにする(ステップ１０５)。測位可能であれば、以下の(１)〜（４）の測位条件を満たすか否か判定する（ステップ１０２）。
(1)GPS測位状態が３次元測位(3D測位)であること。これは、３次元測位が２次元測位に比べて精度が高いためである。
(2)GPS速度が閾値以上であること（例えば、10km／h）。これはドプラーシフトに基づいて方位計算をするため、低速であるとGPS方位の精度が悪化するためである。
(3)走行距離比率αが一定の範囲以内であること（例えば、０．９≦走行距離比率≦１．１）。
走行距離比率は、GPS位置から得られる走行距離と、車速パルスから得られる走行距離の比である。この比が１に近いほど、GPS測位データの精度が高いことを示している。
(4)進行角誤差βが閾値以下（例えば、進行角誤差≦３０度）であること。
進行角誤差は、GPS位置の２点間の角度とGPS方位との差である。進行角誤差が０に近いほど、GPS測位データの精度が高いことを示している。
以上の測位条件(1)ないし(4)のすべてを満足するとき、GPS方位θ_GPSは信頼度が高いとして、信頼度フラグをオンにする (１０６)。

ステップ１０２のいずれかの測位条件を満足しなければ、GPS信頼度判定部２３は、GPS方位θ_GPSと自律航法により取得した推測方位θgyroとの間に類似性があるか判定する(ステップ１０３)。すなわち、GPS信頼度判定部２３はGPS方位とθ_GPSと推測方位θgyroの類似性を次式に
Δθ＝｜(θ_GPS1−θ_GPS2)−θgyro｜≦１０（degree） （5）
より判定する。上式において、θ_GPS1は今回のGPS方位、θ_GPS2は前回のGPS方位、θgyroは今回の推測方位である。Δθが１０度より大きい場合には、両者の方位に類似性がなく、GPS方位の信頼度は低いと判定し、信頼度フラグをオフにする(ステップ１０５)。
Δθが１０度より小さければ、今回のGPS方位θ_GPS1が予測範囲内であるかチェックし(ステップ１０４)、予測範囲外であればGPS方位の信頼度は低いと判定して信頼度フラグをオフにし(ステップ１０５)、予測範囲内であればGPS方位θ_GPSは信頼度が高いと判定して信頼度フラグをオンにする(ステップ１０６)。なお、予測範囲は、車両の直線性に依存するため、まず、移動が直線性か否かを判定する。直線性の移動であれば次式
Δθ′＝｜θ_GPS1−（θ_t-1+θgyro）｜≦１０（degree） (6)
により予測範囲を設定し、GPS方位θ_GPS1が予測範囲内に存在するか否かを判定する。θ_t-1に前回の進行角である。直線性の移動でなければ、Δθ′≦１４（degree）により予測範囲を設定し、GPS方位θ_GPS1が予測範囲内に存在するか否かを判定する。

（Ｄ）ナビゲーションシステムの起動時における修正処理
図６はナビゲーションシステムの起動時における車両位置・方位修正部１６の修正処理フローの概略である。
電源が投入されてナビゲーションシステムが起動すると、位置・方位推測部２１はメモリMEMに保存してある自車の初期位置、初期方位を取り出す(ステップ２０１)。図１の例では、初期位置、初期方位は、ターンテーブルTBLを回転する前に電源切断した時の車両位置Ps（xs、ys）、車両方位θsである。この初期方位θsはターンテーブルの回転により実際の車両方位との間に180°のズレが生じている。
ついで、車両が走行を開始すると、位置・方位推測部２１は自律航法センサー３から出力するセンサー出力信号と車両の初期位置（xs、ys）、初期方位θsとを用いて車両の現在位置（x，y）と方位θを推測して出力する(ステップ２０２)。図１の例では初期方位θsが実際の車両方位から180°ずれているため、点線ARTに沿って車両が走行しても、ナビゲーションシステムのモニター５には実線で示す走行軌跡DRTが表示される。

GPS信頼度判定部２３はナビゲーションシステム起動後、図５の処理フローに従ってGPS方位の信頼度が高くなったか監視すると共に、監視結果を位置・方位修正部２４に入力する(ステップ２０３)。位置・方位修正部２４はGPS方位の信頼度が低ければ位置・方位の修正を行なわず、位置・方位推測部２１は自律航法センサー３から出力するセンサー出力信号を用いて車両の現在位置（x，y）、方位を推測し、出力する。
以後、車両の走行と並行してステップ２０２〜２０３の処理が繰り返され、GPS方位の信頼度が高くなれば(ステップ２０３で「YES」)、位置・方位修正部２４は該GPS方位θ_gと前記推測位置（xd，yd）、推測方位θdを用いて車両の現在位置、方位を修正する(ステップ２０４)。なお、以上の修正処理により得られた現在位置が道路上に存在しないこともあるが、この修正処理後に行なわれるマップマッチング処理により車両現在位置が道路上に修正がなされる。

図７はステップ２０４の位置・方位修正処理フローである。
位置・方位修正部２４は、図２で説明したように、GPS方位の信頼度が高くなった時のGPS方位θ_gと推測方位θdとの差Δθを次式

により計算する（ステップ３０１）。ついで、位置・方位修正部２４は、初期位置Ps(xs，ys)を中心に推測位置Pd(xd，yd)を該差Δθだけ反時計方向に回転する回転演算を実行し、該回転により得られる位置を車両の新たな現在位置とする(ステップ３０２)。しかる後、位置・方位修正部２４は、GPS方位の信頼度が高くなった時のGPS方位θ_gを車両の新たな現在方位θとする(ステップ３０３)。以後、位置・方位推測部２１は修正された現在位置、現在方位を初期位置、初期方位として自律航法センサーの出力信号を用いて自律航法演算により車両現在位置、方位を推測する。

図８はステップ２０４の別の位置・方位修正処理フローである。
位置・方位修正部２４は、初期位置Ps(xs,ys)と推測位置Pd(xd,yd)間の距離L(図２（A）参照)を次式

により計算し(ステップ４０１)、初期位置Ps(xs,ys)からGPS方位θgの方向に該距離Lだけ離れた位置Pgの座標（xg,yg）を計算し、該位置を車両の現在位置(x，y)とする(ステップ４０２)。具体的には、次式

により（xg,yg）を計算して車両の現在位置(x，y)とする。しかる後、位置・方位修正部２４は、GPS方位の信頼度が高くなった時のGPS方位θ_gを車両の新たな現在方位θとする(ステップ４０３)。以後、位置・方位推測部２１は修正された現在位置、現在方位を初期位置、初期方位として自律航法センサーの出力信号を用いて自律航法演算により車両現在位置、方位を推測する。
以上では、ターンテーブルの回転により、ナビゲーションシステムが保持する初期方位と実際の方位とがずれた場合を想定して説明したが、本発明はかかる場合には限らず、一般に、ナビゲーションシステムを起動した場合に実行される。特に、GPS受信が不可能な立体駐車場内において駐車した後、ナビゲーションを起動して該立体駐車場を脱出する場合などに効果的に実行される。

（Ｅ）本発明の効果
図９は本発明の第１の効果説明図であり、ターンテーブル上に車両が移動した後にナビゲーション電源を切断し、ついで、ターンテーブルにより車両の向きが180⁰回転し、その後にナビゲーション起動して走行を開始した場合において、自律航法による推測位置が正しい車両現在位置を示すようになった位置(復帰位置)を示している。比較のために従来方法による復帰位置も同時に示している。なお、図９において、Aで示す走行区間(走行時間１分１２秒)ではGPSの３次元測位が不可能であった。
本発明によれば、３次元測位可能状態になってから５秒後の地点Pgにおいて、すなわち、走行開始後の１分１７秒においてGPS方位の信頼度が高くなって車両現在位置の修正が行なわれ、これにより推測位置(xd，yd)が正しい車両現在位置(x,y)を示すようになった。しかし、従来技術ではGPSの位置とGPS方位の両方の信頼度が高くなってから車両現在位置の修正を行なうものであるため、３次元測位可能状態になってから３８秒後、すなわち、走行開始後の１分５０秒において車両現在位置の修正が行なわれて推測位置(xd，yd)が正しい車両現在位置(x,y)を示すようになった。
図９より本願発明によれば、ターンテーブルの回転によりナビゲーションシステムが認識する車両の方位と実際の車両方位がずれていても、３次元測位可能状態になってから、あるいは走行開始後、従来技術に比べて短時間で自動車位置マークや走行軌跡を地図上の正しい位置に表示することが可能となった。

図１０は本発明の第２の効果説明図であり、GPS受信が不可能な立体駐車場内において駐車した後、ナビゲーションを起動して該立体駐車場を脱出する場合における復帰位置を示しており、従来方法による復帰位置も同時に示している。図において、PBLは立体駐車場、EXITは立体駐車場出口、Psは立体駐車場内の駐車位置(初期位置)、DRTはナビゲーション起動後の自律航法軌跡、ARTは実際の走行軌跡である。
本発明によれば、立体駐車場から出て３次元測位可能状態になってから５秒後の地点Pgにおいて、GPS方位の信頼度が高くなって車両現在位置の修正が行なわれ、これにより推測位置(xd，yd)が正しい車両現在位置(x,y)を示すようになった。しかし、従来技術ではGPSの位置とGPS方位の両方の信頼度が高くなってから車両現在位置の修正を行なうものであるため、３次元測位可能状態になってから３９秒後において車両現在位置の修正が行なわれて推測位置(xd，yd)が正しい車両現在位置(x,y)を示すようになった。
図１０より本願発明によれば、立体駐車場内を走行することによりナビゲーションシステムが認識する車両の方位と実際の車両方位がずれても、３次元測位可能状態になってから、あるいは走行開始後、従来技術に比べて短時間で自動車位置マークや走行軌跡を地図上の正しい位置に表示することが可能となった。

(Ｆ)第２実施例
第１実施例は、ナビゲーションシステムを起動したときに車両位置・方位を修正するが、第２実施例は車両走行中に第１実施例と同等の車両位置・方位を修正する。
立体駐車場内に進入したが、ナビゲーションシステムの電源を切ることなく該立体駐車場から脱出する場合がある。例えば、空き駐車位置がない場合などである。かかる場合、立体駐車場に進入してから、上りながら、あるいは下りながら旋回を繰り返して、駐車場を脱出することになるが、車両が傾いた状態で旋回するため、ジャイロ感度誤差が大きくなり、駐車場脱出時における実際の車両方位と推測方位がずれる。
そこで、第２実施例では立体駐車場内に進入したことを検出し、検出した時点の推測位置、推測方位を第１実施例の初期位置、初期方位として記憶し、以後、第１実施例と同様の制御を行う。
図１１は第２実施例の車両位置・方位修正部１６の構成図であり、図４の第１実施例と同一部分には同一符号を付している。異なる点は、立体駐車場進入検出部３１を設けた点であり、この立体駐車場進入検出部３１は、GPSの３次元測位が不可能であり、かつ、マップマッチング制御が不可能となったとき車両が立体駐車場内に進入したと検出する。

図１２は第２実施例における車両位置・方位修正部の修正処理フローである。
車両位置・方位修正部１６の立体駐車場進入検出部３１はGPSの３次元測位が不可能であり、かつ、マップマッチング制御が不可能であれば車両が立体駐車場内に進入したと判定する(ステップ５０１〜５０２)。ここでマップマッチング制御不可能を立体駐車場進入の条件とした理由は、トンネル内に進入した場合と区別するためである。
車両が立体駐車場内に進入してなければ位置・方位推定部２１は自律航法とマップマッチングに基づいた車両の現在位置、方位の推定処理を継続するする(ステップ５０３)。しかし、GPSの３次元測位が不可能となり、かつ、マップマッチング制御が不可能となって車両が立体駐車場内に進入したことが検出されると、位置・方位推定部２１はその時点の車両現在位置、方位を初期位置（xs、ys）、初期方位θsとして記憶する(ステップ５０４)。
以後、位置・方位推測部２１は自律航法センサー３が出力するセンサー出力信号と、車両の前記初期位置（xs、ys）と初期方位θsとを用いて車両の現在位置（x，y）と方位θを推測する(ステップ５０５)。また、GPS信頼度判定部２３は立体駐車場進入後、図５の処理フローに従ってGPS方位の信頼度が高くなったか監視すると共に、監視結果を位置・方位修正部２４に入力する(ステップ５０６)。位置・方位修正部２４はGPS方位の信頼度が低ければ位置・方位の修正を行なわず、位置・方位推測部２１は自律航法センサー３が出力するセンサー出力信号を用いて車両の現在位置（x，y）、方位θを推測する。

以後、車両の走行と並行してステップ５０５〜５０６の処理が繰り返され、車両が立体駐車場から脱出してGPS方位の信頼度が高くなれば(ステップ５０６で「YES」)、位置・方位修正部２４は該GPS方位θ_gと前記推測位置（xd，yd）、推測方位θdを用いて車両の現在位置、方位を修正する(ステップ５０７)。ステップ５０７の車両の現在位置、方位の修正方法は第１実施例(図７、図８)と同じである。
なお、ステップ５０７の修正処理により得られた現在位置が道路上に存在しないこともあるが、この修正処理後に行なわれるマップマッチング処理により車両現在位置が道路上に修正がなされる。
第２実施例によれば、立体駐車場内でナビゲーション制御を停止しない場合であっても立体駐車場から出る場合において、短時間で車両位置、車両方位を正しい位置、方位に修正することができる。

・変形例
第２実施例ではGPSの３次元測位が不可能であり、かつ、マップマッチング制御が不可能であれば車両が立体駐車場内に進入したと判定したが、GPSの３次元測位が不可能であり、かつ、車両が所定時間内に旋回を繰り返したとき、車両が立体駐車場内に進入したと判定することもできる。図１３はかかる立体駐車場検出を採用した場合の車両位置・方位修正部１６の構成図であり、図１１の第２実施例と同一部分には同一符号を付している。異なる点は、旋回検出部３２を設け、立体駐車場進入検出部３１がGPSの３次元測位が不可能であり、かつ、車両が所定時間内に旋回を繰り返したとき、車両が立体駐車場内に進入したと検出する点である。
以上本発明を説明したが本発明は実施例に限定されず請求項の範囲内で種々変形が可能である。

立体駐車場などの建物のターンテーブルに乗ってナビゲーションシステムの電源を切り、ターンテーブル回転後にナビゲーションを起動して、建物を脱出する状況説明図である。 図１の状況において本発明を適用した例である。 本発明の車両位置・方位修正部を備えたナビゲーション装置の構成図である。 車両位置・方位修正部の構成図である。 GPS方位の信頼度が高いか否かを判定するGPS信頼度判定部の判定処理フローである。 ナビゲーションシステムの起動時における車両位置・方位修正部の修正処理フローである。 位置・方位修正処理フローである。 別の位置・方位修正処理フローである。 本発明の第１の効果説明図である。 本発明の第２の効果説明図である。 第２実施例の車両位置・方位修正部の構成図である。 第２実施例における車両位置・方位修正部の修正処理フローである。 変形例の車両位置・方位修正部の構成図である。 自立航法による車両位置推測方法の説明図である。 投影法によるマップマッチングの説明図である。 ナビゲーション起動時の自車方位が実際の方向とずれ、建物脱出後にミスマッチングする状況の説明図である。

符号の説明

１６ 車両位置・方位修正部
２１ 位置・方位推測部
２２ マップマッチング制御部
２３ GPS信頼度判定部
２４ 位置・方位修正部

Claims (20)

1. GPS受信機と自律航法センサーを併用するナビゲーションシステムのナビゲーション起動時における車両位置・方位修正方法において、
   ナビゲーション停止時における車両位置と車両方位を保存し、これらをナビゲーション起動時における車両の初期位置と初期方位とするステップ、
   ナビゲーション起動後、自律航法センサーから得られるセンサー出力信号と、前記車両初期位置と初期方位とを用いて車両位置と方位を推測するステップ、
   ナビゲーション起動後、GPS受信機より得られるGPS方位の信頼度が高くなったか監視するステップ、
   GPS受信機より得られるGPS方位の信頼度が高くなったとき、該GPS方位と前記推測した車両位置を用いて車両の現在位置を修正するステップ、
   を有することを特徴とする車両位置・方位修正方法。
2. 前記GPS方位の信頼度が高くなったとき該GPS方位を車両の現在方位とする、ことを特徴とする請求項１記載の車両位置・方位修正方法。
3. 前記監視ステップにおいて、
   GPS受信機が三次元GPS測位状態にあり、かつ、車両移動速度が設定速度以上であり、かつ、自律航法センサーより計算される走行距離とGPS位置から得られる走行距離との比率が設定値以下であり、かつ２つのGPS位置を結ぶ直線の方位とGPS方位との差が設定値以下であるとき、GPS方位の信頼度が高いと判定する、
   ことを特徴とする請求項１または２記載の車両位置・方位修正方法。
4. 前記車両位置修正ステップにおいて、
   前記GPS方位と前記推測した車両方位との差Δθを計算し、
   前記車両初期位置を中心に前記推測車両位置を該差Δθだけ回転することにより得られる車両位置を車両の現在位置とする、
   ことを特徴とする請求項１または２記載の車両位置・方位修正方法。
5. 前記車両位置修正ステップにおいて、
   前記車両初期位置と推測車両位置間の距離を計算し、
   前記車両初期位置からGPS方位の方向に該距離だけ離れた位置を車両の現在位置とする、
   ことを特徴とする請求項１または２記載の車両位置・方位修正方法。
6. GPS受信機と自律航法センサーを併用するナビゲーションシステムのナビゲーション起動時における車両位置・方位修正装置において、
   ナビゲーション停止時における車両位置と車両方位をナビゲーション起動時における車両の初期位置と初期方位として保存する保存部、
   ナビゲーション起動後、自律航法センサーから得られるセンサー出力信号と、前記車両の初期位置と初期方位とを用いて車両位置と方位を推測する位置・方位推測部、
   ナビゲーション起動後、GPS受信機より得られるGPS方位の信頼度が高くなったか監視するGPS方位信頼度判定部、
   GPS受信機より得られるGPS方位の信頼度が高くなったとき、該GPS方位と前記推測した車両位置を用いて現在の車両位置を修正する修正部、
   を有することを特徴とする車両位置・方位修正装置。
7. 前記修正部は前記GPS方位の信頼度が高くなったとき、該GPS方位を車両の現在方位とする、ことを特徴とする請求項６記載の車両位置・方位修正装置。
8. 前記GPS方位信頼度判定部は、
   GPS受信機が三次元GPS測位状態にあり、かつ、車両移動速度が設定速度以上であり、かつ、自律航法センサーより計算される走行距離とGPS位置から得られる走行距離との比率が設定値以下であり、かつ２つのGPS位置を結ぶ直線の方位とGPS方位との差が設定値以下であるとき、GPS方位の信頼度が高いと判定する、
   ことを特徴とする請求項６または７記載の車両位置・方位修正装置。
9. 前記修正部は、
   前記GPS方位と前記推測した車両方位との差Δθを計算する計算手段、
   前記車両初期位置を中心に前記推測車両位置を該差Δθだけ回転することにより車両の現在位置を修正する修正手段、
   を備えることを特徴とする請求項６または７記載の車両位置・方位修正装置。
10. 前記修正部は、
    前記車両初期位置と推測車両位置間の距離を計算する手段、
    前記車両初期位置からGPS方位の方向に該距離だけ離れた位置を車両の現在位置とする手段、
    を備えることを特徴とする請求項６または７記載の車両位置・方位修正装置。
11. GPS受信機と自律航法センサーを併用するナビゲーションシステムの車両位置・方位修正方法において、
    自動車が立体駐車場に入ったことを検出した時の車両位置と車両方位を保存するステップ、
    前記立体駐車場に入ったことを検出した後、自律航法センサーから得られるセンサー出力信号と、前記保存した車両位置と車両方位とを用いて車両の現在位置と現在方位を推測するステップ、
    前記立体駐車場に入ったことを検出した後、GPS受信機より得られるGPS方位の信頼度が高くなったか監視するステップ、
    GPS受信機より得られるGPS方位の信頼度が高くなったとき、該GPS方位と前記推測した車両位置を用いて車両の現在位置を修正するステップ、
    を有することを特徴とする車両位置・方位修正方法。
12. GPS信号を受信できず、かつマップマッチングが不可能な状態になった時、立体駐車場に入ったと判定する、
    ことを特徴とする請求項１１記載の車両位置・方位修正方法。
13. GPS信号を受信できず、かつ、車両が旋回を繰り返したとき立体駐車場に入ったと判定する、
    ことを特徴とする請求項１１記載の車両位置・方位修正方法。
14. 前記GPS方位の信頼度が高くなったとき該GPS方位を車両の現在方位とする、
    ことを特徴とする請求項１１記載の車両位置・方位修正方法。
15. 前記車両位置修正ステップにおいて、
    前記GPS方位と前記推測した車両方位との差Δθを計算し、
    前記保存した車両位置を中心に前記推測車両位置を該差Δθだけ回転することにより得られる車両位置を車両の現在位置とする、
    ことを特徴とする請求項１１記載の車両位置・方位修正方法。
16. 前記車両位置修正ステップにおいて、
    前記保存した車両位置と推測車両位置間の距離を計算し、
    前記保存した車両位置GPS方位の方向に該距離だけ離れた位置を車両の現在位置とする、
    ことを特徴とする請求項１１記載の車両位置・方位修正方法。
17. GPS受信機と自律航法センサーを併用するナビゲーションシステムのナビゲーション起動時における車両位置・方位修正装置において、
    自動車が立体駐車場に入ったことを検出する検出部、
    自動車が立体駐車場に進入したことを検出した時の車両位置と車両方位を保存する保存部、
    前記立体駐車場に進入したことを検出した後、自律航法センサーから得られるセンサー出力信号と、前記保存した車両位置と車両方位とを用いて車両の現在位置と現在方位を推測する位置・方位推測部、
    前記立体駐車場に進入したことを検出した後、GPS受信機より得られるGPS方位の信頼度が高くなったか監視するGPS方位信頼度判定部、
    GPS受信機より得られるGPS方位の信頼度が高くなったとき、該GPS方位と前記推測した車両位置を用いて車両の現在位置を修正する修正部、
    を有することを特徴とする車両位置・方位修正装置。
18. 前記修正部は前記GPS方位の信頼度が高くなったとき、該GPS方位を車両の現在方位とする、ことを特徴とする請求項１７記載の車両位置・方位修正装置。
19. 前記修正部は、
    前記GPS方位と前記推測した車両方位との差Δθを計算する計算手段、
    前記車両初期位置を中心に前記推測車両位置を該差Δθだけ回転することにより車両の現在位置を修正する修正手段、
    を備えることを特徴とする請求項１７記載の車両位置・方位修正装置。
20. 前記修正部は、
    前記車両初期位置と推測車両位置間の距離を計算する手段、
    前記車両初期位置からGPS方位の方向に該距離だけ離れた位置を車両の現在位置とする手段、
    を備えることを特徴とする請求項１７記載の車両位置・方位修正装置。

Images