JP4536150B2 - ナビゲーション装置 - Google Patents

Description

この発明は、ＧＰＳ（Global Positioning System）受信機と自律航法センサとを用いた測位およびマップマッチングを行うナビゲーション装置に関する。

自動車に搭載されるナビゲーション装置は、地図の道路上の自車位置の表示および道案内などを行う。道路上の自車位置を表示するにあたっては、ナビゲーション装置は、ＧＰＳと、車速センサおよび角速度センサを含む自律航法センサを用いて自車の運動を計測し、マップマッチング処理によって、地図データによって示される道路リンク上で位置同定を行う。しかしながら、自律航法センサについては、速度センサから出力されるパルスを距離に変換するレート（「スケールファクタ：Scale Factor」と呼ぶ）が車両毎に異なり、また、角速度センサのバイアス電圧（「オフセット」と呼ぶ）は温度ドリフトするため、誤差の発生を常時確認し、適宜補正する必要がある。

また、ＧＰＳについては、次のような使用上の問題がある。
（１）ＧＰＳ測位には、その原理上、３個以上のＧＰＳ衛星からの電波が必要である。しかしながら、道路周辺のビルなどにより自車の上空の視界が狭まると、ＧＰＳ衛星からの電波が遮断される。この遮断によって測位に使用できるＧＰＳ衛星の数が３個未満になると、測位が不可能になって利用性（アベイラビリティ）が低下する。以下、これを「アベイラビリティ問題」という。
（２）道路周辺にビルなどが存在すると、ＧＰＳ衛星からの電波がビルなどで反射して、ＧＰＳ衛星からの電波が地上のＧＰＳ受信機に到達するまでに複数の経路（マルチパス）を通るので伝播遅延時間が長くなる。ＧＰＳ測位では、伝播遅延時間を距離に換算して得られる擬似距離から自車位置を計測するため、マルチパスが発生すると自車位置の誤差が大きくなる。以下、これを「マルチパス問題」という。

上述したように、都市部におけるＧＰＳ測位では、アベイラビリティの低下があり、また、マルチパスが発生して自車位置の誤差が大きくなるという問題があるので、従来のナビゲーション装置においては、このようなＧＰＳ測位に起因する問題を排除するために、自律航法センサから得られる計測データを用いて道路リンク上における自車位置を修正することが行われている。

このような技術として、特許文献１は、測位演算結果の精度を高めるためにカルマンフィルタを用いたＧＰＳ受信機およびカーナビゲーションシステムを開示している。この特許文献１に開示されたＧＰＳ受信機およびカーナビゲーションシステムは、ＧＰＳ測位演算で用いられるカルマンフィルタの収束を正しい方向へ導くため、マップマッチングの結果をカルマンフィルタに帰還させるように構成されている。これは、自律航法センサから得られる計測データを用いてマップマッチングした結果は、ＧＰＳ測位データを用いてマップマッチングした結果より位置精度が高いという前提で行われている。

また、特許文献２は、ＤＲ（自律測位：Dead Reckoning）機能付きＧＰＳ受信機を開示している。この特許文献２に開示されたＧＰＳ受信機は、例えば、ナビゲーション装置におけるマップマッチングの結果をＧＰＳ受信機に帰還させて、測位誤差を解消するように構成されている。なお、ＤＲ機能付きＧＰＳ受信機では、ＤＲ測位とＧＰＳ測位とは別々に行われ、カルマンフィルタなどを用いて複合的に測位するものではない。

また、特許文献３は、アベイラビリティの低下を招くことなく擬似距離誤差を低減して、測位精度を高めることができるＧＰＳ受信機を開示している。この特許文献３に開示されたＧＰＳ受信機は、マルチパスの影響を受け易い擬似距離（伝播遅延時間から計測される）を、マルチパスの影響を受け難い擬似距離変化率（搬送波周波数のドップラーシフトから計測される）を用いて平滑化するように構成されており、瞬間的な電波遮断が起きた場合であっても、ＧＰＳ衛星と自車の相対運動から推定した擬似距離変化率を用いて擬似距離を補間し、測位計算に用いる。これにより、マルチパス問題およびアベイラビリティ問題の解消を図っている。

特開２００１−２７２２３９号公報 特開２００２−２１３９７９号公報 特開２００６−３２２８４６号公報

ところで、ＧＰＳ受信機と自律航法センサとを用いて測位およびマップマッチングを行うナビゲーション装置において、ＧＰＳ測位の結果を使用するためには、自車の上空に存在する３個以上のＧＰＳ衛星からの電波を受信し、ＧＰＳ受信機が２次元測位または３次元測位できることが最低条件である。

しかしながら、道路沿いにビルなどが林立する都市部などの道路においては、ビルなどで自車の上空の視界が狭められ、電波を受信できるＧＰＳ衛星の数が３個未満になると、ＧＰＳの利用性が低下するといったアベイラビリティ問題が発生する。また、このような場所では、ビルなどで反射した電波を受信することが多くなり、ＧＰＳ測位の結果に大きな誤差を生じ易いというマルチパス問題が発生する。このため、従来のナビゲーション装置では、自律航法センサで計測された自車（利用者）の移動距離と旋回角度とを用いて自車の位置を求め（自律測位）、道路リンク上などにおける自車の位置を修正できるように構成されている。

しかしながら、例えば、立体駐車場などといった道路外から走行を開始した場合に、自律測位で求めた自車の位置および方位の誤差が大きくなると、その後に道路上を走行するときに、誤った道路リンク上にマッチングするミスマッチングが発生する。このとき、ＧＰＳ測位が不可能な状態であれば、ミスマッチングの状態が継続する。自車の現在位置が正しくなければ、ナビゲーション装置の経路案内なども間違うことになる。

これらの問題に対して、特許文献１に開示された技術では、自律航法センサから得られる計測データを用いてマップマッチングした結果は、ＧＰＳ測位データを用いてマップマッチングした結果より位置精度が高いという前提のもとに、マップマッチングの結果をカルマンフィルタに帰還させて、ＧＰＳ測位演算で用いるカルマンフィルタの収束方向を導くように構成されている。しかしながら、上述したように、マップマッチングの結果が正しくない場合は、ミスマッチングの結果が正常なＧＰＳ測位を阻害して、正しい道路リンク上での位置同定を妨げるという欠点がある。

また、上述した問題に対して、特許文献２に開示された技術では、マップマッチングの結果を用いて自律測位の誤差を解消する試みがなされている。しかしながら、この技術は、上述した特許文献１に開示された技術と同様に、正しくマップマッチングできた場合に有効であって、上述したように、ＧＰＳ測位する前にミスマッチングが発生する状態では所望の効果を得ることができない。

また、上述した問題に対して、特許文献３に開示された技術では、マルチパスの影響を受け易い擬似距離を、マルチパスの影響を受け難い擬似距離変化率を用いて平滑化して、マルチパスの影響を低減する。また、瞬間的な電波遮断が起きた場合には、ＧＰＳ衛星と自車の相対運動から推定された擬似距離変化率を用いて擬似距離を補間し、測位計算に用いることにより、アベイラビリティの低下およびマルチパスの影響を軽減する。しかしながら、電源投入後から測位に使用可能なＧＰＳ衛星からの電波の数が３未満の場合には、この技術でもＧＰＳ測位できないので、所望の効果を得ることができない。

この発明は、上述した諸問題を解消するためになされたものであり、その課題は、アベイラビリティの低下を招くことなくマルチパスの影響を軽減し、測位精度を高めることができるナビゲーション装置を提供することにある。

この発明に係るナビゲーション装置は、上記課題を解決するために、複数のＧＰＳ衛星から放射された電波を受信する電波受信部と、電波受信部で受信された電波の放射元であるＧＰＳ衛星を識別し、該識別したＧＰＳ衛星の擬似距離および擬似距離変化率を計測するとともに、該電波に含まれている航法メッセージを抽出する信号処理部と、信号処理部で抽出された航法メッセージからＧＰＳ衛星の位置および速度を計算するＧＰＳ衛星挙動推定部と、信号処理部で計測された擬似距離と擬似距離変化率とに基づき擬似距離誤差を推定する擬似距離誤差推定部と、信号処理部で計測された擬似距離、ＧＰＳ衛星挙動推定部で計算されたＧＰＳ衛星の位置および擬似距離誤差推定部で推定された擬似距離誤差を用いて自己の位置を計算する利用者位置観測部と、信号処理部で計測された擬似距離変化率、ＧＰＳ衛星挙動推定部で計算されたＧＰＳ衛星の位置および利用者位置観測部で計算された自己の位置から自己の速度を計算する利用者速度・方位観測部と、擬似距離誤差推定部で推定された擬似距離誤差のうちのマルチパス誤差が規定値より小さい場合に、所定角度範囲内で方位を変えたときの利用者位置観測部から得られる位置および利用者速度・方位観測部から得られる速度を有する自己と、ＧＰＳ衛星挙動推定部で推定された位置および速度を有するＧＰＳ衛星との相対運動に基づき計算した擬似距離変化率が、信号処理部で計測された擬似距離変化率と一致する方位を自己の方位として探索する利用者方位探索部とを備えている。

この発明に係るナビゲーション装置によれば、ＧＰＳ衛星から受信される電波毎にマルチパス影響の有無を判断でき、マルチパス影響のないＧＰＳ衛星電波を１つ受信できれば、測位に使用するＧＰＳ衛星の数が１または２のためにＧＰＳ測位ができない場合であっても自車の方位を検出できる。その結果、アベイラビリティの低下を招くことなくマルチパスの影響を軽減し、測位精度を高めることができる。

この発明の実施の形態１に係るナビゲーション装置の構成を、測位に必要な部分を中心に示すブロック図である。 この発明の実施の形態１に係るナビゲーション装置の動作を、測位処理を中心に示すフローチャートである。 この発明の実施の形態１に係るナビゲーション装置を搭載した自動車が都市部の道路を走行したときのＧＰＳ衛星電波の受信状況を説明するための図である。 この発明の実施の形態１に係るナビゲーション装置において、ＧＰＳ測位により得られた自車の方位および速度の推移の例を示す図である。 この発明の実施の形態１に係るナビゲーション装置における擬似距離変化率の計測値と推定値（推定条件：移動／静止）の推移の例を示す図である。 この発明の実施の形態１に係るナビゲーション装置において、−１８０〜−２０度の角度範囲で２０度刻みの方位誤差を与えて計算した擬似距離変化率（推定値）の波形を示す図である。 この発明の実施の形態１に係るナビゲーション装置において、自車の方位が修正される状況を説明するための図である。 この発明の実施の形態１に係るナビゲーション装置において、位置誤差円内の位置更新範囲を説明するための図である。 この発明の実施の形態２に係るナビゲーション装置の構成を、測位に必要な部分を中心に示すブロック図である。 この発明の実施の形態２に係るナビゲーション装置の動作を、測位処理を中心に示すフローチャートである。 この発明の実施の形態３に係るナビゲーション装置の構成を、測位に必要な部分を中心に示すブロック図である。 この発明の実施の形態３に係るナビゲーション装置の動作を、測位処理を中心に示すフローチャートである。 この発明の実施の形態４に係るナビゲーション装置の構成を、測位に必要な部分を中心に示すブロック図である。 この発明の実施の形態４に係るナビゲーション装置の動作を、測位処理を中心に示すフローチャートである。 この発明の実施の形態４に係るナビゲーション装置において、角速度センサのオフセット補正を説明するための図である。 この発明の実施の形態５に係るナビゲーション装置の構成を、測位に必要な部分を中心に示すブロック図である。 この発明の実施の形態５に係るナビゲーション装置の動作を、測位処理を中心に示すフローチャートである。 この発明の実施の形態５に係るナビゲーション装置において、自車の方位修正状況およびマップマッチング状況を説明するための図である。 この発明の実施の形態５に係るナビゲーション装置において、マップマッチングで設定する候補を説明するための図である。 この発明の実施の形態６に係るナビゲーション装置の構成を、測位に必要な部分を中心に示すブロック図である。 この発明の実施の形態６に係るナビゲーション装置の動作を、測位処理を中心に示すフローチャートである。 この発明の実施の形態６に係るナビゲーション装置において、ループ橋走行後の自車の方位修正状況およびマップマッチング状況を説明するための図である。

以下、この発明の実施の形態を、図面を参照しながら詳細に説明する。
実施の形態１．
図１は、この発明の実施の形態１に係るナビゲーション装置の構成を、測位に必要な部分を中心に示すブロック図である。このナビゲーション装置は、電波受信部１１、信号処理部１２、ＧＰＳ衛星挙動推定部１３、擬似距離誤差推定部１４、利用者位置観測部１５、利用者速度・方位観測部１６および利用者方位探索部１７を備えている。

電波受信部１１は、自車の上空に存在する複数のＧＰＳ衛星から放射された電波（以下、「ＧＰＳ衛星電波」という）を受信するＧＰＳアンテナを備えている。電波受信部１１は、このＧＰＳアンテナでＧＰＳ衛星電波を受信することにより得られた受信信号を、信号処理部１２に送る。

信号処理部１２は、電波受信部１１から送られてくる受信信号からＧＰＳ衛星電波を放射したＧＰＳ衛星を識別し、ＧＰＳ衛星毎に、ＧＰＳ衛星電波の伝播遅延時間から擬似距離を算出するとともに、ＧＰＳ衛星電波の搬送波周波数のドップラーシフトから擬似距離変化率を算出する。さらに、信号処理部１２は、電波受信部１１から送られてくる受信信号から、ＧＰＳ衛星の軌道情報などが記述されている航法メッセージおよびＧＰＳ衛星電波の受信時刻などを抽出する。この信号処理部１２における信号処理の結果は、ＧＰＳ衛星挙動推定部１３、擬似距離誤差推定部１４、利用者位置観測部１５、利用者速度・方位観測部１６および利用者方位探索部１７に送られる。

ＧＰＳ衛星挙動推定部１３は、信号処理部１２で抽出されたＧＰＳ衛星電波の受信時刻と航法メッセージに含まれる軌道情報（エフェメリスなど）とを用いて、時々刻々と変化するＧＰＳ衛星の位置および速度を計算する。このＧＰＳ衛星挙動推定部１３における計算結果は、利用者位置観測部１５、利用者速度・方位観測部１６および利用者方位探索部１７に送られる。

擬似距離誤差推定部１４は、信号処理部１２で抽出されたＧＰＳ衛星電波の受信時刻と航法メッセージに含まれる補正パラメータ（電離層通過時の擬似距離誤差の補正パラメータ、ＧＰＳ衛星に搭載されている時計の誤差の補正パラメータ）などを用いて、ＧＰＳ衛星電波がＧＰＳアンテナに届くまでの電波伝播遅延時間に含まれる各種誤差成分（ＧＰＳ衛星搭載時計誤差、電離層電波伝播遅延誤差、対流圏電波伝播遅延誤差、マルチパス誤差および受信機雑音など）を推定する。擬似距離誤差推定部１４で推定された誤差成分は、利用者位置観測部１５および利用者方位探索部１７に送られる。

利用者位置観測部１５は、信号処理部１２から送られてくるＧＰＳ衛星毎の擬似距離、ＧＰＳ衛星挙動推定部１３で計算された受信時刻におけるＧＰＳ衛星の挙動（位置および速度）、擬似距離誤差推定部１４で推定された擬似距離の誤差成分および自車の位置（前回の計算値）を用いて、所定の計算式に従って自車の現在位置を測位するとともに、ナビゲーション装置の内蔵時計の誤差を算出する。

利用者速度・方位観測部１６は、信号処理部１２で計算されたＧＰＳ衛星電波の受信時刻におけるＧＰＳ衛星毎の擬似距離変化率、ＧＰＳ衛星挙動推定部１３で計算された受信時刻におけるＧＰＳ衛星の挙動（位置および速度）、擬似距離誤差推定部１４で推定された擬似距離の誤差および利用者の位置を用いて自車の速度および方位を計算し、利用者方位探索部１７に送る。

利用者方位探索部１７は、擬似距離誤差推定部１４で推定された擬似距離誤差（マルチパス誤差）が規定値以下であるＧＰＳ衛星電波については、信号処理部１２で算出された擬似距離変化率と、この利用者方位探索部１７で推定する擬似距離変化率とを比較し、両者が一致したときの推定値の計算に用いた方位（自車の方位を表す）を探索する。

次に、上記のように構成される、この発明の実施の形態１に係るナビゲーション装置の動作を、所定周期で起動される測位処理を中心に、図２に示すフローチャートおよび図３〜図８に示す説明図を参照しながら説明する。

測位処理では、まず、ＧＰＳ受信があるかどうかが調べられる（ステップＳＴ１１）。すなわち、信号処理部１２は、電波受信部１１から送られてくる受信信号から抽出したＧＰＳ衛星電波の受信時刻の更新状態などを見ることによりＧＰＳ衛星電波の受信の有無を調べる。このステップＳＴ１１において、ＧＰＳ受信がないことが判断されると、測位処理は終了する。

一方、ステップＳＴ１１において、ＧＰＳ受信があることが判断されると、ＧＰＳ衛星の挙動（位置および速度）の計算が行われる（ステップＳＴ１２）。すなわち、ＧＰＳ衛星挙動推定部１３は、信号処理部１２で抽出されたＧＰＳ衛星電波の受信時刻からＧＰＳ衛星電波の送信時刻を逆算した後に、ＧＰＳ衛星毎の航法メッセージに含まれる軌道情報（エフェメリス）を用いて、ＧＰＳ衛星電波の送信時刻におけるＧＰＳ衛星の位置および速度を、所定の計算式を用いて計算し、この計算結果を、利用者位置観測部１５、利用者速度・方位観測部１６および利用者方位探索部１７に送る。

次いで、擬似距離誤差（ＧＰＳ衛星搭載時計誤差、大気中電波伝播遅延誤差）の計算が行われる（ステップＳＴ１３）。すなわち、擬似距離誤差推定部１４は、信号処理部１２から送られてくる擬似距離に含まれる誤差として、航法メッセージに含まれる所定パラメータを用いて、ＧＰＳ衛星搭載時計誤差および大気中（電離層と対流圏）電波伝播遅延誤差を、下記の（１）式および（２）式に従って計算する。なお、（２）式において、ＧＰＳ受信機雑音については信号処理部１２の各チャンネルでほぼ共通であるので、マルチパスの影響がないＧＰＳ衛星電波を受信したときに求めておく。

ここで、
dρ（Ｔ₂）：ＧＰＳ衛星・自車間の擬似距離の誤差（≒ｄ_iono＋ｄ_trop＋_dmp） ［ｍ］
Ｔ₁ ：ＧＰＳ衛星電波の送信時刻（＝Ｔ₂−||Ｐs−Ｐo||／Ｃ） ［ｓ］
Ｔ₂ ：ＧＰＳ衛星電波の受信時刻 ［ｓ］
Ｐs ：ＧＰＳ衛星の位置（航法メッセージから算出）、（x_s，y_s，z_s） ［ｍ］
Ｐo ：自車の位置、（x_o，y_o，z_o） ［ｍ］
||Ｐs−Ｐo||：ＧＰＳ衛星位置Ｐsと自車位置Ｐoの直線距離 ［ｍ］
＝（（x_s−x_o）²＋（y_s−y_o）²＋（z_s−z_o）²）^1/2
ｃ ：光の速度（＝２．９９７９２４５８×１０8） [ｍ／ｓ]
τ ：ＧＰＳ衛星・自車間の電波伝搬時間 [s]
ρ_cτ ：擬似距離（＝ｃτ） ［ｍ］
dＴ₁ ：ＧＰＳ衛星搭載時計誤差（航法メッセージを用いて算出） [s]
dＴ₂ ：ナビゲーション装置内蔵時計誤差 ［ｓ］
ε ：擬似距離の誤差（（２）式で算出） ［ｍ］
ｄ_iono ：電離層電波伝播遅延誤差 ［ｍ］
ｄ_trop ：対流圏電波伝播遅延誤差 ［ｍ］
d_mp ：マルチパス誤差（未知数） ［ｍ］

ここで、
ｄ_iono：電離層電波伝播遅延誤差（航法メッセージを用いて算出） ［ｍ］
ｄ_trop：対流圏電波伝播遅延誤差（所定誤差モデルで計算） ［ｍ］
ｄ_rcv ：受信機雑音（受信機チャンネル共通） ［ｍ］
ｄ_mp ：マルチパス誤差（＝０；高仰角のＧＰＳ衛星の場合） ［ｍ］

次いで、擬似距離誤差（マルチパス誤差）が推定される（ステップＳＴ１４）。すなわち、擬似距離誤差推定部１４は、信号処理部１２で計測された擬似距離の差分と擬似距離変化率とを比較して、当該擬似距離に含まれるマルチパス誤差を計算し、計算結果を、利用者位置観測部１５および利用者方位探索部１７に送る。なお、マルチパス誤差の計算については、特許文献３に詳細に記載されているので、必要に応じて参照されたい。

次いで、擬似距離の誤差分散の計算が行われる（ステップＳＴ１５）。すなわち、擬似距離誤差推定部１４は、ステップＳＴ１４で計算した擬似距離誤差（マルチパス誤差）について、所定時間あたりの分散をＧＰＳ衛星電波毎に計算し、計算結果を、利用者位置観測部１５および利用者方位探索部１７に送る。

次いで、擬似距離誤差が規定値より小さいかどうかが調べられる（ステップＳＴ１６）。すなわち、利用者方位探索部１７は、ステップＳＴ１４で計算されて擬似距離誤差推定部１４から送られてくる擬似距離誤差（マルチパス誤差のみ）を規定値と比較する。このステップＳＴ１６において、擬似距離誤差が規定値未満であることが判断されると、自車の方位探索は可能であると判断され、シーケンスはステップＳＴ１７へ進む。一方、ステップＳＴ１６において、擬似距離誤差が規定値以上であれば、自車の方位探索は不可能であると判断され、シーケンスは、ステップＳＴ１７をスキップしてステップＳＴ１８へ進む。

例えば、自車が田園の道路を走行する場合には、受信された殆どのＧＰＳ衛星電波に対しては、ステップＳＴ１７の処理が行われるが、自車が都市部の道路を走行する場合には、ＧＰＳ衛星電波は、例えば図３に示すように、道路沿いに林立するビルなどで遮断され、または、反射されるので、受信された殆どのＧＰＳ衛星電波に対しては、ステップＳＴ１７の処理は行われない。なお、都市部の道路を走行中であっても、自車上空の仰角の大きい方向（天頂方向）のＧＰＳ衛星または道路方向のＧＰＳ衛星（低い仰角の場合もあり）については、マルチパスの影響を受け難いので、都市部であっても離散的にステップＳＴ１７において処理される。

ステップＳＴ１７においては、自車の方位の探索が行われる。すなわち、利用者方位探索部１７は、信号処理部１２で計測された、マルチパスの影響を受けない擬似距離変化率を分析して自車の方位を探索する。図４は、ＧＰＳ測位により得られた自車の方位および速度の推移の例を示す図である。今、自車の方位および速度が、図４（ａ）および図４（ｂ）にそれぞれ示すように変化したとすると、信号処理部１２で計測される擬似距離変化率は、図５に「計測値」として示すようになる。図５は、擬似距離変化率の計測値と推定値（推定条件：移動／静止）の推移の例を示している。図６は、−１８０〜−２０度の角度範囲で２０度刻みの方位誤差を与えて計算した擬似距離変化率（推定値）の波形を示す図である。

この擬似距離変化率は、ＧＰＳ衛星電波の搬送波周波数のドップラーシフトから計測されるので、ＧＰＳ衛星の位置および速度と自車の位置および速度とが既知であれば、両者の相対運動として下記の（３）式で計算できる。自車が静止状態にあれば、（３）式の自車の速度を０にして計算した擬似距離変化率の波形は、図５の「推定値（静止）」で示すようになるが、自車が走行状態にあれば、自車の方位および速度に応じて「推定値（静止）」の波形が変化して「推定値（移動）」で示すようになる。

このステップＳＴ１７では、自車の方位を所定角度範囲内で変えたときの自車の速度ベクトルおよび擬似距離変化率が計算され、この計算により得られた波形と計測により得られた波形（「計測値」）の差異が規定値以下になったときの方位を自車の方位と見なすように探索される。このステップＳＴ１７で方位の探索が行われた時は、探索された方位と自車の方位とが比較される。そして、両者が所定角度以上離れておれば、自車の方位の誤差が大きいと判断され、探索した方位を基準にして、図７に示すように、自車の方位が修正される。

ただし、
LOS_x＝（x_O−x_S）／||Ｐs−Ｐo||
LOS_y＝（y_O−y_S）／||Ｐs−Ｐo||
LOS_z＝（z_O−z_S）／||Ｐs−Ｐo||
||Ｐs−Ｐo||＝{（x_S−x_O）²＋（y_S−y_O）²＋（z_S−z_O）²}^1/2
ここで、
Δρ_rate-est：擬似距離変化率の推定値 [ｍ／ｓ]
Ｐs：ＧＰＳ衛星の位置（航法メッセージから算出）、（x_s，y_s，z_s） ［ｍ］
Ｖs：ＧＰＳ衛星の速度（航法メッセージから算出）、（v_s，v_s，v_s） ［ｍ］
Ｐo：自車の位置、（x_o，y_o，z_o） ［ｍ］
Ｖs：自車の速度、（v_s，v_s，v_s） ［ｍ］
||Ｐs−Ｐo||：ＧＰＳ衛星位置と自車位置の間の距離 ［ｍ］
LOS：自車からＧＰＳ衛星を見る視線方向ベクトル（line of site vector）

次いで、４衛星以上かつ所定回数以内であるかどうかが調べられる（ステップＳＴ１８）。すなわち、利用者速度・方位観測部１６は、受信できたＧＰＳ衛星電波の数が４つ以上であって、かつ、収束計算回数が所定回数以内であるかどうかを調べる。このステップＳＴ１８において、ＧＰＳ衛星電波の数が４つ未満、または、収束計算回数が所定回数を超えた場合には測位不可能と判断されて測位処理は終了する。

一方、ステップＳＴ１８において、４衛星以上かつ所定回数以内であることが判断されると、自車の速度（ベクトル）の計算が行われる（ステップＳＴ１９）。すなわち、利用者速度・方位観測部１６は、擬似距離変化率、ＧＰＳ衛星挙動推定部１３から送られてくる受信時刻におけるＧＰＳ衛星の位置および速度、ならびに、自車の位置および速度と、信号処理部１２から送られてくるＧＰＳ衛星電波の受信時刻におけるＧＰＳ衛星毎の擬似距離変化率とを用いて、（４）式に従って自車の速度ベクトルを計算するとともに、計算したＸ、Ｙ、Ｚ軸の速度成分および自車の位置から自車の方位（スカラー）を計算する（（４）式中のｎは、行列の要素番号であり、ＰＲＮ（Pseudo Random Noise code；擬似雑音符号）ではない）。なお、（４）式は、（３）式において、Ｖｏ＝０としたものである。

ここで、
Δρ_rate-mes：擬似距離変化率の計測値 [ｍ／ｓ]
Δρ_rate-est：擬似距離変化率の推定値 [ｍ／ｓ]
Ｖｏ ：自車の速度（Ｖｏx，Ｖｏy，Ｖｏz，Ｖｏt） [ｍ／ｓ]
Ａ ：航法行列
Ｗ ：重み付き行列
σ_δρ ：擬似距離誤差の標準偏差

次いで、自車の位置（ベクトル）の計算が行われる（ステップＳＴ２０）。すなわち、利用者位置観測部１５は、擬似距離誤差の２乗和を最小にする解として、例えば（５）式を用いて、自車の位置誤差とナビゲーション装置の内蔵時計の誤差を重み付き最小２乗法などで求め、自車の位置とナビゲーション装置の内蔵時計を修正する。ここで、ステップＳＴ１７で方位を探索できた場合には、図８の位置誤差円内の位置更新範囲に示すように、この方位を含む所定角度範囲内で位置を更新するよう調整する。次いで、自車の位置誤差を見て収束したか否かが調べられる（ステップＳＴ２１）。すなわち、利用者位置観測部１５は、自車の位置誤差を見て収束したか否かを判断する。位置誤差が規定値未満であれば、自車の位置を測位できたとして測位処理を終了し、位置誤差が規定値以上であれば、収束していないと判断し、シーケンスはステップＳＴ１２に戻り、上述した処理を繰り返し実行する（数回で収束）。

ここで、
δＰo：自車の位置誤差（δＰo_x，δＰo_y，δＰo_z，δＰo_t） ［ｍ］
（δＰo_t／c、時計誤差）
Ｐo：自車の位置（Ｐo_x，Ｐo_y，Ｐo_z，Ｐo_t） ［ｍ］
Ａ：航法行列
Ｗ：重み付き行列
σ_δρ： 擬似距離誤差の標準偏差
ｃ：光の速度（＝２．９９７９２４５８×１０８） [ｍ／ｓ]

なお、上述した測位処理では、重み付き最小２乗法を用いて自車の速度と位置を計算するように構成したが、逐次計算またはカルマンフィルタなどを用いて計算するように構成することもできる。

以上説明したように、この発明の実施の形態１に係るナビゲーション装置によれば、マルチパスの影響を受け易い擬似距離の差分と、マルチパスの影響を受け難い擬似距離変化率とを比較して、マルチパスの影響による擬似距離誤差を求めるので、ＧＰＳ衛星毎に、マルチパスの影響を把握できる。

また、マルチパスの影響がない擬似距離変化率について、計測値と推定値を比較し、両者が一致したときの推定値の計算に用いた方位から、自車の方位を検出することができるので、例えば、都市部のビル街で自車上空の視界が制限されることにより測位に使用できるＧＰＳ衛星の数が１つまたは２つになったとき、または、電源投入の後にまだ１つまたは２つしかＧＰＳ衛星電波を受信できていないときに、従来のナビゲーション装置はＧＰＳ測位できず、自車の位置と方位の更新が停止していたが、この実施の形態１に係るナビゲーション装置によれば、自車の方位を検出することができ、ドライバーなどに知らせることができる。

実施の形態２．
図９は、この発明の実施の形態２に係るナビゲーション装置の構成を、測位に必要な部分を中心に示すブロック図である。この実施の形態２に係るナビゲーション装置は、実施の形態１に係るナビゲーション装置に速度センサ２１および利用者速度計測部２２が追加されるとともに、利用者速度・方位観測部１６の機能が変更されて構成されている。以下では、実施の形態１に係るナビゲーション装置の構成部分と同一の構成部分については、実施の形態１で使用した符号を付して説明を省略し、異なる構成部分についてのみ説明する。

速度センサ２１は、自車の速度に応じたパルス信号を出力する。この速度センサ２１から出力されるパルス信号は、利用者速度計測部２２に送られる。利用者速度計測部２２は、速度センサ２１から送られてくるパルス信号を、自車の速度に換算する。この利用者速度計測部２２で得られた自車の速度は、利用者速度・方位観測部１６に送られる。利用者速度・方位観測部１６は、ＧＰＳと速度センサ２１とを併用して自車の速度（ベクトル）を計測した上で、ＸＹＺの各軸の速度と自車の位置とから方位を計算する。

次に、上記のように構成される、この発明の実施の形態２に係るナビゲーション装置の動作を、測位処理を中心に、図１０に示すフローチャートを参照しながら説明する。なお、図２のフローチャートに示した実施の形態１に係るナビゲーション装置で行われる処理と同一の処理を行うステップには、図２で使用した符号と同一の符号を付して説明を簡略化し、異なる処理を行うステップを中心に説明する。

測位処理では、まず、自車の速度（スカラー）の計算が行われる（ステップＳＴ３１）。すなわち、利用者速度計測部２２は、速度センサ２１から送られてくるパルス信号を自車の速度（スカラー）に換算し、利用者速度・方位観測部１６に送る。次いで、ＧＰＳ受信があるかどうかが調べられる（ステップＳＴ１１）。このステップＳＴ１１において、ＧＰＳ受信がないことが判断されると、測位処理は終了する。一方、ステップＳＴ１１において、ＧＰＳ受信があることが判断されると、ＧＰＳ衛星の挙動（位置および速度）の計算が行われる（ステップＳＴ１２）。

次いで、擬似距離誤差（ＧＰＳ衛星搭載時計誤差、大気中電波伝播遅延誤差）の計算が行われる（ステップＳＴ１３）。次いで、擬似距離誤差（マルチパス誤差）が推定される（ステップＳＴ１４）。次いで、擬似距離の誤差分散の計算が行われる（ステップＳＴ１５）。次いで、擬似距離誤差が規定値より小さいかどうかが調べられる（ステップＳＴ１６）。このステップＳＴ１６において、擬似距離誤差が規定値未満であることが判断されると、自車の方位探索は可能であると判断され、シーケンスはステップＳＴ３２へ進む。一方、ステップＳＴ１６において、擬似距離誤差が規定値以上であれば、自車の方位探索は不可能であると判断され、シーケンスはステップＳＴ３２をスキップしてステップＳＴ１８へ進む。

ステップＳＴ３２においては、自車の方位（スカラー）と位置の計算が行われる。すなわち、利用者方位探索部１７は、自車の方位を所定角度範囲内で変えたときの自車の速度ベクトルおよび擬似距離変化率を計算し、この計算により得られた波形と計測により得られた波形（図５の「計測値」参照）の差異が規定値以下になったときの方位を自車の方位と見なすように探索するとともに、速度と方位とから自車の移動量を計算し、前回の測位処理で求めた自車の位置を推定する。この後に行われるステップＳＴ２０において自車の位置を計算できなければ、このステップＳＴ３２で推定された位置が、今回の測位処理での自車の位置として出力される。

なお、自車の速度ベクトルを算出するにあたっては、周期的に実行される測位処理の最初の計算では、前回の予測処理で求められた自車の方位を含む大きな角度範囲で方位を変えて、速度センサ２１および利用者速度計測部２２で計測された速度（スカラー）が速度ベクトルに変換されるが、収束計算が進むにつれ、ステップＳＴ３３で求められた自車の方位を含む角度範囲が徐々に狭められる。なお、ステップＳＴ１８の条件に合わず、ステップＳＴ３３で自車の方位が計算されていない場合には、角度範囲は大きくしたままである。そして、このステップＳＴ３２で方位が探索された時は、自車の方位と比較される。両者が所定角度以上離れておれば、自車方位の誤差が大きいと判断されて、探索した方位を基準にして図７に示すように自車の方位が修正される。

以上説明したように、この発明の実施の形態２に係るナビゲーション装置によれば、速度センサ２１からのパルス信号に基づき自車の速度が計測されるので、精度の高い自車の速度を常に求めることができる。また、マルチパスの影響がない擬似距離変化率が得られた場合に行われる自車の方位探索では、この精度の高い速度が用いられるので、精度の高い自車の方位を求めることができる。

これらのことから、例えば、都市部のビル街で自車上空の視界が制限されたために測位に使用できるＧＰＳ衛星の数が１つまたは２つになったとき、または、電源投入の後に未だ１つまたは２つしかＧＰＳ衛星電波を受信できていないときに、従来のナビゲーション装置はＧＰＳ測位できず、自車の位置と方位の更新が停止していたが、この実施の形態２に係るナビゲーション装置によれば、マルチパスの影響がない擬似距離変化率を１つのＧＰＳ衛星から受信するだけで自車の方位を検出できることから、自車の速度と方位に基づいて自車の位置を推定することができ、利用性（アベイラビリティ）が向上する。

実施の形態３．
図１１は、この発明の実施の形態３に係るナビゲーション装置の構成を、測位に必要な部分を中心に示すブロック図である。この実施の形態３に係るナビゲーション装置は、実施の形態２に係るナビゲーション装置から利用者速度・方位観測部１６が除去されるとともに、角速度センサ２３、利用者旋回角度計測部２４および利用者挙動計測部２５が追加され、また、利用者位置観測部１５および利用者方位探索部１７の機能が変更され、さらに、利用者速度計測部２２の出力は、利用者速度・方位観測部１６に送られる代わりに、利用者挙動計測部２５に送られるように変更されて構成されている。以下では、実施の形態２に係るナビゲーション装置の構成部分と同一の構成部分については、実施の形態２で使用した符号を付して説明を省略し、異なる構成部分についてのみ説明する。

角速度センサ２３は、自車の旋回角速度に応じた信号を出力する。この角速度センサ２３から出力される信号は、利用者旋回角度計測部２４に送られる。利用者旋回角度計測部２４は、角速度センサ２３から送られてくる信号から自車の旋回角度を計測する。この利用者旋回角度計測部２４で計測された、自車の旋回角度は、利用者挙動計測部２５に送られる。

利用者挙動計測部２５は、利用者速度計測部２２から送られてくる速度と利用者旋回角度計測部２４から送られてくる旋回角度とを用いて自律測位計算を実行し、自車の位置、速度および方位を更新する。

利用者位置観測部１５は、実施の形態１に係るナビゲーション装置と同様に、ＧＰＳ衛星電波を用いて自車の位置を観測するが、この実施の形態３に係るナビゲーション装置では、利用者挙動計測部１５で計測された自車の位置が修正される。

また、利用者方位探索部１７は、擬似距離変化率を分析して自車の方位を探索するが、自車の位置、方位および速度については、利用者挙動計測部２５で計算された結果を用い、方位を変える角度範囲については、利用者挙動計測部２５で計算された方位を含む所定の角度範囲とする。

次に、上記のように構成される、この発明の実施の形態３に係るナビゲーション装置の動作を、測位処理を中心に、図１２に示すフローチャートを参照しながら説明する。なお、図１０のフローチャートに示した実施の形態２に係るナビゲーション装置で行われる処理と同一の処理を行うステップには、図１０で使用した符号と同一の符号を付して説明を簡略化し、異なる処理を行うステップを中心に説明する。

測位処理では、まず、自車の速度（スカラー）の計算が行われる（ステップＳＴ３１）。次いで、自車の旋回角度（スカラー）の計算が行われる（ステップＳＴ４１）。すなわち、利用者旋回角度計測部２４は、角速度センサ２３から送られてくる信号から自車の旋回角度を計測する。次いで、自車の位置（ベクトル）の計算が行われる（ステップＳＴ４２）。すなわち、利用者挙動計測部２５は、利用者速度計測部２２から送られてくる速度と利用者旋回角度計測部２４から送られてくる旋回角度とを用いて自律測位計算を実行し、自車の位置、方位および速度を求める。

次いで、ＧＰＳ受信があるかどうかが調べられる（ステップＳＴ１１）。このステップＳＴ１１において、ＧＰＳ受信がないことが判断されると、測位処理は終了する。一方、ステップＳＴ１１において、ＧＰＳ受信があることが判断されると、ＧＰＳ衛星の挙動（位置および速度）の計算が行われる（ステップＳＴ１２）。次いで、擬似距離誤差（ＧＰＳ衛星搭載時計誤差、大気中電波伝播遅延誤差）の計算が行われる（ステップＳＴ１３）。次いで、擬似距離誤差（マルチパス誤差）が推定される（ステップＳＴ１４）。次いで、擬似距離の誤差分散の計算が行われる（ステップＳＴ１５）。次いで、擬似距離誤差が規定値より小さいかどうかが調べられる（ステップＳＴ１６）。このステップＳＴ１６において、擬似距離誤差が規定値未満であることが判断されると、自車の方位探索は可能であると判断され、シーケンスはステップＳＴ４３へ進む。一方、ステップＳＴ１６において、擬似距離誤差が規定値以上であれば、自車の方位探索は不可能であると判断され、シーケンスはステップＳＴ１８へ進む。

ステップＳＴ４３においては、自車の方位（スカラー）の計算が行われる。すなわち、利用者方位探索部１７は、自車の方位を所定の方位範囲で変えたときの自車の速度ベクトルおよび擬似距離変化率を計算し、この計算により得られた波形と計測により得られた波形（図５の「計測値」参照）の差異が規定値以下になったときの方位を自車の方位と見なすように探索する。

なお、自車の速度ベクトルを算出するにあたっては、ステップＳＴ４２において利用者挙動計測部２５で求められた自車の方位を含む所定角度範囲内で方位を変えて、速度センサ２１および利用者速度計測部２２で計測された速度（スカラー）が速度ベクトルに変換されるが、ステップＳＴ１２からステップＳＴ２１までのＧＰＳ測位の収束計算が進むにつれ、ステップＳＴ４２で求められた自車の方位を含む角度範囲が徐々に狭められる。そして、このステップＳＴ４３で方位が探索された時は、ステップＳＴ４２で求められた自車の方位と比較される。両者が所定角度以上離れておれば、自車方位の誤差が大きいと判断されて、探索した方位を基準にして図７に示すように自車の方位が修正される。

以上説明したように、この実施の形態３に係るナビゲーション装置によれば、速度センサ２１からの信号と角速度センサ２３からの信号とに基づき自車の速度および旋回角度が計測されることから、精度の高い自車の速度および旋回角度を常に求めることができる。また、マルチパスの影響がない擬似距離変化率を受信した場合に行われる自車の方位探索では、この精度の高い速度が用いられるので、精度の高い自車の方位を求めることができる。これにより、都市部であっても自車の方位を適正に修正できるので、自律測位の方位精度が安定する。また、マルチパスの影響が小さいときにＧＰＳ測位で求めた位置を基準にして、利用者挙動計測部２５で求めた自車の位置（自律測位）が適正に修正されるので、自律測位の欠点である誤差の累積が改善され、自律測位の位置精度も安定する。

実施の形態４．
図１３は、この発明の実施の形態４に係るナビゲーション装置の構成を、測位に必要な部分を中心に示すブロック図である。この実施の形態４に係るナビゲーション装置は、実施の形態３に係るナビゲーション装置に角速度センサ補正部２６が追加されて構成されている。以下では、実施の形態３に係るナビゲーション装置の構成部分と同一の構成部分については、実施の形態３で使用した符号を付して説明を省略し、異なる構成部分についてのみ説明する。

角速度センサ補正部２６は、利用者挙動計測部２５で計算された方位と利用者方位探索部１７で計算された方位を入力して、角速度センサ２３の出力電圧から自車の旋回角度を計測する際に用いる補正パラメータ（バイアス電圧）を補正する。この角速度センサ方正部２６で補正された補正パラメータは、オフセット誤差として、利用者旋回角度計測部２４に送られる。利用者旋回角度計測部２４は、角速度センサ補正部２６から送られてくるオフセット誤差を用いてバイアス電圧を補正した後に、角速度センサ２３から送られてくる信号から自車の旋回角度を計測する。

次に、上記のように構成される、この発明の実施の形態４に係るナビゲーション装置の動作を、測位処理を中心に、図１４に示すフローチャートおよび図１５に示す角速度センサのオフセット誤差の補正を説明する説明図を参照しながら説明する。なお、図１２のフローチャートに示した実施の形態３に係るナビゲーション装置で行われる処理と同一の処理を行うステップには、図１２で使用した符号と同一の符号を付して説明を省略し、異なる処理を行うステップを中心に説明する。

図１４のフローチャートに示す測位処理では、図１２のフローチャートに示した測位処理のステップＳＴ４３とステップＳＴ１８との間に、角速度センサの補正処理（ステップＳＴ５１）が追加されて構成されている。この角速度センサの補正処理においては、利用者旋回角度計測部２４は、角速度センサ補正部２６から送られてくる補正パラメータを用いてバイアス電圧を補正した後に、角速度センサ２３から送られてくる電圧信号から自車の旋回角度を計測する。

例えば、図１５に示すように、自車が直進走行（図１５中の点線矢印で示す走行方向）しているときに、利用者挙動計測部２５で計算された自車の位置の軌跡（図１５中の実線で示す走行軌跡）がやや左方向に旋回気味になれば、利用者挙動計測部２５で計算された方位と利用者方位探索部１７で計算された方位との差異は、図１５中の破線で示すようになる。この方位差の推移を、例えば多項式近似曲線を用いて分析し、一定の傾向が確認できると、多項式近似曲線のパラメータから角速度センサ２３のバイアス電圧の誤差を検出できる。角速度センサ補正部２６は、このバイアス電圧の誤差を検出したときは、これを利用者旋回角度計測部２４に送る。利用者旋回角度計測部２４は、角速度センサ補正部２６から送られてきたバイアス電圧の誤差を用いてバイアス電圧を補正し、角速度センサの出力電圧から角速度を計測する。

以上説明したように、この発明の実施の形態４に係るナビゲーション装置によれば、角速度センサ２３に温度ドリフトが生じても、角速度センサ２３のバイアス電圧誤差を検出して補正することができるので、利用者旋回角度計測部２４は、旋回角度を高い精度で計測できる。これにより、利用者挙動計測部２５で計測（自律測位）する自車の位置および方位の精度を良好に維持できる。

実施の形態５．
図１６は、この発明の実施の形態５に係るナビゲーション装置の構成を、測位およびマップマッチングに必要な部分を中心に示すブロック図である。この実施の形態４に係るナビゲーション装置は、実施の形態２に係るナビゲーション装置に、地図データ記憶部３１および道路照合部３２が追加されて構成されている。以下では、実施の形態２に係るナビゲーション装置の構成部分と同一の構成部分については、実施の形態２で使用した符号を付して説明を省略し、異なる構成部分についてのみ説明する。

地図データ記憶部３１は、所定範囲の道路について線形データと座標点などで表現された道路リンクなどからなる地図データを記憶している。この地図データ記憶部３１に記憶されている地図データは、道路照合部３２によって読み出される。

道路照合部３２は、利用者位置観測部１５で観測された自車の位置および利用者方位探索部１７で探索された方位と、地図データ記憶部３１から読み出した道路リンクの位置および方位とを照合して、規定以上の信頼度で整合が確認できた道路リンク上に自車の現在位置の候補を複数設定し、複数の候補の中から表示する候補を１つ選び、候補の位置を自車の位置とする。さらに、道路照合部３２は、利用者速度・方位観測部で計算された速度と利用者方位探索部１７で探索された方位とに基づいて、道路リンク上の候補の位置を更新するとともに、利用者方位探索部１７で探索された方位と道路リンクの方位との差異に応じて候補の信頼性を評価し、全ての候補の中で信頼度が最高の候補の位置を利用者の位置として選出する。

次に、上記のように構成される、この発明の実施の形態５に係るナビゲーション装置の動作を、測位処理を中心に図１７に示すフローチャート、図１８に示す自車の方位修正状況およびマップマッチング状況を示す説明図および図１９に示すマップマッチングで設定する候補について説明するための説明図を参照しながら説明する。なお、マップマッチングの処理内容については、特許第３７４５１６５号公報に説明されているので必要に応じて参照されたい。

図１７のフローチャートに示す測位処理は、図１０のフローチャートに示した測位処理のステップＳＴ２１の次に、マップマッチング処理（ステップＳＴ６１）が追加されて構成されている。このマップマッチング処理においては、ステップＳＴ３２またはステップＳＴ２０において自車位置が計算された場合は、道路照合部３２は、地図データ記憶部３１に記憶されている地図データから、自車位置を中心とする所定範囲内の道路リンクを検索し、自車の位置と道路リンクとの最短距離（垂線距離）が規定値以下であって方位と走行軌跡が整合する道路リンク上の位置を探す。

例えば、図１８に示すケースでは、時刻ｔ０に電源を投入してから時刻ｔ１までの間は、方位誤差のため、自車の方位と道路リンクの方位との差異が規定角度以内である道路リンクを抽出できないので、道路リンク上に候補を設定できない。しかしながら、時刻ｔ１に利用者方位探索部１７で方位が検出されたときには、自車の方位誤差が分かるので、電源投入直後の位置を中心にして走行軌跡をアフィン変換して自車の位置をＰ１Ａ（ｔ１）からＰ１Ｂ（ｔ２）へ修正する。その直後には、自車の方位と近傍の道路リンクの方位の差が規定角度以内となる道路リンクを抽出できるので、道路リンク上に候補を設定して、以後、道路リンク上で位置を更新できる。

また、図１９に示すように、１本道から２つに分岐した道路が並走関係にある場合は、例えば、時刻ｔ２の時点で方位を検出できれば、検出した方位と道路リンク方位がより近い方の道路リンク上の候補の信頼度を他の候補より大きくして、自車の位置として決定できるようにする。

以上説明したように、この発明の実施の形態５に係るナビゲーション装置によれば、ＧＰＳ衛星電波毎にマルチパス影響の有無を判断でき、マルチパスの影響を受けないＧＰＳ衛星電波を１つ受信すれば、自車の方位を検出できる。これにより、ＧＰＳ位置誤差円について自車が存在する方位角領域を制限できるので、マルチパスにより精度が低下したＧＰＳ測位結果の影響を大きく受けることなくマップマッチングを行うことができる。その結果、自車の方位および位置を地図表示画面の道路上で安定かつスムーズに求めることができる。これにより、ナビゲーション装置は、道案内などを適切に行うことができる。

また、マップマッチングで設定した複数の候補について、マルチパスの影響を受けないＧＰＳ衛星電波を１つ受信すれば、マルチパス環境下でも方位の信頼度を評価できるので、方位の異なる道路リンクにマッチングするというミスマッチングを低減できる。

実施の形態６．
図２０は、この発明の実施の形態６に係るナビゲーション装置の構成を、測位とマップマッチングに必要な部分を中心に示すブロック図である。この実施の形態６に係るナビゲーション装置は、実施の形態４に係るナビゲーション装置に、地図データ記憶部３１および道路照合部３２が追加されて構成されている。以下では、実施の形態４に係るナビゲーション装置の構成部分と同一の構成部分については、実施の形態４で使用した符号を付して説明を省略し、異なる構成部分についてのみ説明する。

地図データ記憶部３１は、所定範囲の道路について線形データと座標点などで表現された道路リンクなどからなる地図データを記憶する。この地図データ記憶部３１に記憶されている地図データは、道路照合部３２によって読み出される。

道路照合部３２は、利用者挙動計測部２５で計算された位置および方位を、地図データ記憶部３１から読み出した道路リンクの位置および方位とを照合して、規定以上の信頼度で整合が確認できた道路リンク上に自車の現在位置の候補を複数設定し、複数の候補の中から表示する候補を１つ選び、候補の位置を自車の位置とする。さらに、道路照合部３２は、利用者速度計測部２２で計算された速度と利用者旋回角度計測部２４で計算された旋回角度とに基づいて、道路リンク上の候補の位置を更新するとともに、利用者方位探索部で探索された方位と道路リンクの方位との差異に応じて候補の信頼性を評価し、全ての候補の中で信頼度が最高の候補位置を利用者の位置として選出する。

次に、上記のように構成される、この発明の実施の形態６に係るナビゲーション装置の動作を、測位処理を中心に図２１に示すフローチャート、図１９に示すマップマッチングで設定する候補について説明するための説明図および図２２に示すループ橋走行後の自車の方位修正状況およびマップマッチング状況を説明するための説明図を参照しながら説明する。

図２１のフローチャートに示す測位処理では、図１４のフローチャートに示した測位処理のステップＳＴ２１の次に、マップマッチング処理（ステップＳＴ６１）が追加されて構成されている。このマップマッチング処理においては、ステップＳＴ３２またはステップＳＴ２０において自車位置が計算された場合は、道路照合部３２は、地図データ記憶部３１に記憶されている地図データから、自車位置を中心とする所定範囲内の道路リンクを検索し、自車の位置と道路リンクとの最短距離（垂線距離）が規定値以下であって方位と走行軌跡が整合する道路リンク上の位置を探す。

例えば、図１９に示すような、１本道から２つに分岐した道路が並走関係にあるケースでは、速度センサ２１で計測した速度と角速度センサ２３で計測した旋回角度とに基づいて道路リンク上で位置を更新すると、一般的には、分岐した道路リンクのどちらか一方のみに候補を設定するか、または両方に候補を設定するか判断することになる。図１９に示すケースでは、速度と旋回角度の誤差を考慮した結果、両方の道路リンクに候補を設定した場合を示す。このときには、時刻ｔ２の時点で方位を検出できれば、検出した方位と道路リンク方位がより近い方の道路リンク上の候補の信頼度を他の候補より大きくして、自車の位置として決定する。

また、図２２に示すように、自車がループ橋を１周して高度が低い道路に進むケースでは、ループ橋を走行時に道路リンク長より速度センサ２１で計測した走行距離が長めになると、ループ橋を通過し終えて高度が低い道路に進んだ時に、従来は自車の方位誤差のために自車の方位と近傍の道路リンク方位の差が規定角度以内の道路リンクを抽出できず、道路リンク上で自車の位置の候補を設定できなくなる。しかしながら、利用者方位探索部１７が方位を検出すると、自車の方位誤差を検出して補正できるので、その直後に近傍の道路リンクを抽出し、自車の位置を道路上に修正するようになる。

以上説明したように、この発明の実施の形態６に係るナビゲーション装置によれば、速度センサ２１で計測した速度と角速度センサ２３で計測した旋回角度とを用いて道路リンク上の候補の位置を更新し続けた際に、道路リンクのデジタイズ誤差、センサの誤差、計算値の丸め誤差の累積などの原因で、道路リンク上の候補の位置に誤差が生じても、マルチパスの影響がない擬似距離変化率を１つのＧＰＳ衛星から受信するだけで正しい方位が分かるので、道路リンク上の候補の位置を修正できる。

また、ＧＰＳ衛星電波毎にマルチパス影響の有無を判断でき、さらに、マルチパスの影響を受けないＧＰＳ衛星電波を１つ受信すれば、自車の方位を検出できるので、ＧＰＳ位置誤差円について自車が存在する方位角領域を制限でき、マルチパスにより精度が低下したＧＰＳ測位結果の影響を大きく受けることなく、スムーズなマップマッチングを行うことができる。

また、マップマッチングで設定した複数の候補について、マルチパスの影響を受けないＧＰＳ衛星電波を１つ受信すれば、マルチパス環境下でも方位の信頼度を評価できるので、方位の異なる道路リンクにマップマッチングするというミスマッチングを防止することができ、よりスムーズに位置を更新できる。また、方位が整合する候補に限定して候補数を削減できるので、マップマッチング処理の負荷が軽減することができるので、マップマッチングをより高い周期で実行することができる。

また、擬似距離変化率の推定値を計算する自車の方位を、候補が存在する道路リンクの方位を含む所定の角度に制限できるので、受信した全てのＧＰＳ衛星電波にマルチパス影響がない場合であっても方位探索の処理負荷が増大しない。これにより、マップマッチングを実施する周期を下げなくても良い。

さらに、マルチパスの影響を受けないＧＰＳ衛星電波を１つ受信すれば、自車の方位を検出できるので、道路リンク上の自動車の位置に誤差があっても、分岐点、交差点または方位変化点付近で自動車の向きが変わる度に自車の位置を速やかに修正できる。また、例えば、大きく湾曲した１本道であっても、道路リンクの長さと実際に走行した距離に顕著な差異を生じる場合があるが、この場合にも、自車の向きが変わる度に自動車の位置を適正な位置に速やかに修正できる。

実施の形態７．
上述した実施の形態１から実施の形態６に係るナビゲーション装置では、自車の速度を正しく計測できた後に、正しい自車の方位を検出するように構成したが、この実施の形態７に係るナビゲーション装置では、自車の速度誤差は大きいが、自車の方位を先に正しく検出できた場合には、自車の速度が適正になるように補正するものである。

ナビゲーション装置において、例えば、直進走行中にＧＰＳ測位により自車の方位が正しいことを確認できた場合には、擬似距離変化率を推定するにあたり、自車の速度に乗算する係数を所定範囲内で変えて計算した推定値と受信した計測値とを比較し、両者の波形が一致したときの係数（速度センサ２１のスケールファクタ）を探す利用者速度探索部（図示は省略する）をさらに追加する。そして、利用者速度探索部が速度センサ２１のスケールファクタを検出すると、利用者速度計測部は、このスケールファクタを用いて、速度センサ２１のパルスから自車の速度を計測する。

この構成によれば、自車の方位誤差が小さいときは、擬似距離変化率を推定するにあたり、方位を一定にして、逆に自車の速度に乗算する係数を所定範囲内で変えて計算した推定値と受信した計測値を比較し、両者の波形が一致したときの係数を用いて速度を補正するようにしたので、マルチパス影響のないＧＰＳ衛星電波を１つ受信できれば、測位に使用できるＧＰＳ衛星の数が１つまたは２つのためにＧＰＳ測位ができない場合であっても、自車の速度が適正になるよう補正できる。

実施の形態８．
上述した実施の形態１から実施の形態６に係るナビゲーション装置では、マルチパス影響のないＧＰＳ衛星電波を受信した際に、擬似距離変化率を分析して検出した方位を用いて、自車の方位修正またはマップマッチングの候補の信頼度評価に用いるように構成したが、マルチパスの影響がない複数のＧＰＳ衛星電波を同時刻に受信して複数の方位を検出した場合には、検出した複数の方位を統計処理して、上述した処理を行うように構成できる。この構成によれば、より正確な自車の方位を検出できる。

また、マルチパスの影響がないＧＰＳ衛星電波が１つしかない場合であっても、複数地点で方位を検出した場合には、検出した複数の方位を統計処理して、上述した処理を行うように構成できる。この構成によれば、より正確な自車の方位を検出できる。

以上のように、この発明に係るナビゲーション装置は、ＧＰＳ衛星から受信される電波毎にマルチパス影響の有無を判断でき、マルチパス影響のないＧＰＳ衛星電波を１つ受信できれば、測位に使用するＧＰＳ衛星の数が１または２のためにＧＰＳ測位ができない場合であっても自車の方位を検出できるように構成したので、アベイラビリティの低下を招くことなくマルチパスの影響を軽減し、測位精度を高めることができるため、ＧＰＳ受信機と自律航法センサとを用いた測位およびマップマッチングを行なう自動車搭載のナビゲーション装置などに用いるのに適している。

Claims (18)

1. 複数のＧＰＳ衛星から放射された電波を受信する電波受信部と、
   前記電波受信部で受信された電波の放射元であるＧＰＳ衛星を識別し、該識別したＧＰＳ衛星の擬似距離および擬似距離変化率を計測するとともに、該電波に含まれている航法メッセージを抽出する信号処理部と、
   前記信号処理部で抽出された航法メッセージからＧＰＳ衛星の位置および速度を計算するＧＰＳ衛星挙動推定部と、
   前記信号処理部で計測された擬似距離と擬似距離変化率とに基づき擬似距離誤差を推定する擬似距離誤差推定部と、
   前記信号処理部で計測された擬似距離、前記ＧＰＳ衛星挙動推定部で計算されたＧＰＳ衛星の位置および前記擬似距離誤差推定部で推定された擬似距離誤差を用いて自己の位置を計算する利用者位置観測部と、
   前記信号処理部で計測された擬似距離変化率、前記ＧＰＳ衛星挙動推定部で計算されたＧＰＳ衛星の位置および前記利用者位置観測部で計算された自己の位置から自己の速度を計算する利用者速度・方位観測部と、
   前記擬似距離誤差推定部で推定された擬似距離誤差のうちのマルチパス誤差が規定値より小さい場合に、所定角度範囲内で方位を変えたときの前記利用者位置観測部から得られる位置および前記利用者速度・方位観測部から得られる速度を有する自己と、前記ＧＰＳ衛星挙動推定部で推定された位置および速度を有するＧＰＳ衛星との相対運動に基づき計算した擬似距離変化率が、前記信号処理部で計測された擬似距離変化率と一致する方位を自己の方位として探索する利用者方位探索部
   とを備えたナビゲーション装置。
2. 自己の速度に応じた信号を出力する速度センサと、
   前記速度センサからの信号から自己の速度を計測する利用者速度計測部
   とを備え、
   利用者速度・方位観測部は、前記利用者速度計測部で計測された速度に基づき自己の速度を計算する
   ことを特徴とする請求項１記載のナビゲーション装置。
3. 道路リンクを含む所定範囲の地図データを記憶した地図データ記憶部と、
   利用者位置観測部で計算された位置および利用者方位探索部で探索された方位を、前記地図データ記憶部に記憶されている道路リンクの位置および方位と照合して、自己が存在している可能性が高い道路リンク上に自己の位置の候補を設定する道路照合部
   とを備えたことを特徴とする請求項１記載のナビゲーション装置。
4. 道路照合部は、利用者速度・方位観測部で計算された速度と利用者方位探索部で探索された方位とに基づいて、道路リンク上の候補の位置を更新するとともに、前記利用者方位探索部で探索された方位と道路リンクの方位との差異に応じて候補の信頼性を評価し、全ての候補の中で信頼度が最高の候補の位置を自己の位置として選出する
   ことを特徴とする請求項３記載のナビゲーション装置。
5. 自己の速度に応じた信号を出力する速度センサと、
   前記速度センサからの信号から自己の速度を計測する利用者速度計測部と、
   自己の旋回角度に応じた信号を出力する角速度センサと、
   前記角速度センサからの信号から自己の旋回角度を計測する利用者旋回角度計測部と、
   前記利用者速度計測部で計測された速度と前記利用者旋回角度計測部で計測された旋回角度とを用いて、自己の位置、速度および方位を計算する利用者挙動計測部と、
   複数のＧＰＳ衛星から放射された電波を受信する電波受信部と、
   前記電波受信部で受信された電波の放射元であるＧＰＳ衛星を識別し、該識別したＧＰＳ衛星の擬似距離および擬似距離変化率を計測するとともに、該電波に含まれている航法メッセージを抽出する信号処理部と、
   前記信号処理部で抽出された航法メッセージからＧＰＳ衛星の位置および速度を計算するＧＰＳ衛星挙動推定部と、
   前記信号処理部で計測された擬似距離と擬似距離変化率とに基づき擬似距離誤差を推定する擬似距離誤差推定部と、
   前記信号処理部で計測された擬似距離、前記ＧＰＳ衛星挙動推定部で計算されたＧＰＳ衛星の位置および前記擬似距離誤差推定部で推定された擬似距離誤差を用いて自己の位置を求め、前記利用者挙動計測部で計算された位置を修正する利用者位置観測部と、
   前記擬似距離誤差推定部で推定された擬似距離誤差のうちのマルチパス誤差が規定値より小さい場合に、所定角度範囲内で方位を変えたときの前記利用者挙動計測部で計算された位置および速度を有する自己と、前記ＧＰＳ衛星挙動推定部で推定された位置および速度を有するＧＰＳ衛星との相対運動に基づき計算した擬似距離変化率が、前記信号処理部で計測された擬似距離変化率と一致する方位を自己の方位として探索する利用者方位探索部
   とを備えたナビゲーション装置。
6. 利用者挙動計測部で計算された方位と利用者方位探索部で探索された方位とに基づき角速度センサのオフセット誤差を求める角速度センサ補正部を備え、
   利用者旋回角度計測部は、前記角速度センサ補正部から送られてくるオフセット誤差に従って角速度センサからの信号を修正した後に自己の旋回角度を計測する
   ことを特徴とする請求項５記載のナビゲーション装置。
7. 道路リンクを含む所定範囲の地図データを記憶した地図データ記憶部と、
   利用者挙動計測部で計算された位置および方位を、前記地図データ記憶部に記憶されている道路リンクの位置および方位と照合して、自己が存在している可能性が高い道路リンク上に自己の位置の候補を設定する道路照合部
   とをさらに備えたことを特徴とする請求項６記載のナビゲーション装置。
8. 道路照合部は、利用者速度計測部で計算された速度と利用者旋回角度計測部で計算された旋回角度とに基づいて、道路リンク上の候補の位置を更新するとともに、利用者方位探索部で探索された方位と道路リンクの方位との差異に応じて候補の信頼性を評価し、全ての候補の中で信頼度が最高の候補位置を自己の位置として選出する
   ことを特徴とする請求項７記載のナビゲーション装置。
9. 利用者方位探索部は、擬似距離変化率の計測値と推定値が一致する方位を複数のＧＰＳ衛星から放射された電波に基づき求めた場合には、求めた複数の方位を統計処理した結果に基づいて、自己の方位を決める
   ことを特徴とする請求項１記載のナビゲーション装置。
10. 利用者方位探索部は、擬似距離変化率の計測値と推定値が一致する方位を複数のＧＰＳ衛星から放射された電波に基づき求めた場合には、求めた複数の方位を統計処理した結果に基づいて、自己の方位を決める
    ことを特徴とする請求項５記載のナビゲーション装置。
11. 利用者方位探索部は、擬似距離変化率の計測値と推定値が一致する方位を複数の地点で求めた場合には、求めた複数の方位を統計処理した結果に基づいて、自己の方位を決める
    ことを特徴とする請求項１記載のナビゲーション装置。
12. 利用者方位探索部は、擬似距離変化率の計測値と推定値が一致する方位を複数の地点で求めた場合には、求めた複数の方位を統計処理した結果に基づいて、自己の方位を決める
    ことを特徴とする請求項５記載のナビゲーション装置。
13. 利用者方位探索部は、道路照合部で設定された複数の候補の各々が存在する道路リンクの方位を含む所定角度範囲で、擬似距離変化率の計測値と推定値が一致する方位を自己の方位として探索する
    ことを特徴する請求項３記載のナビゲーション装置。
14. 利用者方位探索部は、道路照合部で設定された複数の候補の各々が存在する道路リンクの方位を含む所定角度範囲で、擬似距離変化率の計測値と推定値が一致する方位を自己の方位として探索する
    ことを特徴する請求項７記載のナビゲーション装置。
15. 道路照合部は、設定した候補が道路リンクの分岐点、交差点または方位変化点の所定範囲内に存在する場合には、利用者方位探索部で求められた方位に最も近い道路リンク上に位置を修正する
    ことを特徴とする請求項３記載のナビゲーション装置。
16. 道路照合部は、設定した候補が道路リンクの分岐点、交差点または方位変化点の所定範囲内に存在する場合には、利用者方位探索部で求められた方位に最も近い道路リンク上に位置を修正する
    ことを特徴とする請求項７記載のナビゲーション装置。
17. 自己の方位が利用者方位探索部で探索された方位と一致する場合には、自己とＧＰＳ衛星との相対運動に基づき計算された擬似距離変化率が信号処理部で計測された擬似距離変化率と一致するように、自己の速度に乗算する係数を探す利用者速度探索部をさらに備えるとともに、利用者速度計測部は、速度センサからの信号に該係数を乗算して自己の速度を計測する
    ことを特徴とする請求項２記載のナビゲーション装置。
18. 自己の方位が利用者方位探索部で探索された方位と一致する場合には、自己とＧＰＳ衛星との相対運動に基づき計算された擬似距離変化率が信号処理部で計測された擬似距離変化率と一致するように、自己の速度に乗算する係数を探す利用者速度探索部をさらに備えるとともに、利用者速度計測部は、速度センサからの信号に該係数を乗算して自己の速度を計測する
    ことを特徴とする請求項５記載のナビゲーション装置。

Images