JP4345779B2 - ナビゲーション装置、位置検出方法 - Google Patents

Description

本発明は、移動体の位置を検出するナビゲーション装置に関し、特に、慣性航法による移動体の測位及び測位誤差を評価するナビゲーション装置に関する。

ナビゲーション装置では、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｓｈｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波に基づき自車両の位置を検出する電波航法や、速度など自車両の挙動を検出する挙動センサにより位置を検出する慣性航法が利用されている。

しかしながら、これら電波航法や慣性航法による測位には大小の誤差が含まれることが知られており、自車両の位置を利用した種々の車両制御を実行する場合、誤差の程度が影響を及ぼす場合がある。

このため、電波航法や慣性航法による測位の誤差を評価する方法が要請されるが、電波航法による測位については誤差が小さいかまた誤差が大きくても誤差の評価方法が従来から存在する。

これに対し、慣性航法による測位について誤差の評価方法が提案されている（例えば、特許文献１参照。）。特許文献１記載の誤差の評価方法では、距離センサ／方位センサにより検出される自車両の走行距離及び方位の誤差、方位が一定でない走行をした場合の計算誤差、及び、これらの累積誤差に基づき自車両の測位の精度を推定する。この推定は確率分布により行われ、センサ類の精度に応じた速度で拡大する。したがって、慣性航法による測位位置の誤差を確率分布により評価することができる。
特開平１０−５４７２９号公報

しかしながら、特許文献１記載の評価方法は、結局のところ距離センサ／方位センサによる誤差のみを考慮するに過ぎず、移動体の位置に影響を与える要因を十分に検討していない。例えば、走行中の車両は、重心位置など車両属性に付随する影響を受けて位置を変更しているため、距離センサ／方位センサのみを考慮した誤差では慣性航法による測位の誤差を評価するには十分でない。

本発明は、上記課題に鑑み、慣性航法による測位について精度よく誤差の評価が可能なナビゲーション装置を提供することを目的とする。

上記課題を解決するため、本発明は、移動体の位置を検出するナビゲーション装置において、移動体の挙動情報を検出する自律センサと、自律センサによる検出情報を累積して移動体の慣性測位位置を検出する慣性測位手段と、時刻ｔの、慣性測位位置の誤差分散、自律センサのセンサ誤差分散及び計算誤差を、移動体の移動モデル（例えば、ダイナミックスモデル）に基づく更新式に適用して、時刻ｔ＋１の誤差分散を漸化的に算出する誤差分散算出手段と、誤差分散のうち前記移動体が所定確率で存在する範囲が、所定領域と少なくとも一部が重複するか否かに基づき前記所定領域内に前記移動体が存在するか否かを判定する判定手段と、を有することを特徴とする。

慣性航法による測位について精度よく誤差の評価が可能なナビゲーション装置を提供することができる。

以下、本発明を実施するための最良の形態について、図面を参照しながら説明する。始めに、自車両の位置の誤差評価について概略を説明する。

図１は自車位置の誤差評価の概略を示す図である。本実施形態では、誤差評価として誤差分散を用いる。そして、時刻ｔにおける誤差分散、センサ値の誤差分散及び計算の丸め誤差を更新式に適用し、時刻ｔ＋１における自車位置の誤差分散を算出する。

センサ値の誤差を単に累積するのでなく、更新式により自車位置の誤差分散を漸化的に導出するので、自車位置の誤差を精度よく評価することができる。また、更新式は車両のダイナミックスモデルに基づき設定するため移動体の性質に応じた測位誤差の導出を可能にする。

図２（ａ）はナビゲーション装置１の概略構成図を示す。ナビゲーション装置１は、ナビゲーション装置１を制御するナビＥＣＵ（Ｅｌｅｃｔｒｉｃａｌ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）１０により制御される。ナビＥＣＵ１０は、プログラムを実行するＣＰＵ、プログラムを記憶した記憶装置（ハードディスクドライブ、ＲＯＭ）、データやプログラムを一時的に記憶するＲＡＭ、データを入力及び出力する入出力部、ＮＶ（Ｎｏｎ Ｖｏｌａｔｉｌｅ）−ＲＡＭ等がバスを介して接続されたコンピュータとして構成される。

ナビＥＣＵ１０にはＧＰＳ衛星からの電波を受信するＧＰＳ受信装置１１、車速を検出する車速センサ１２、車両の鉛直軸回りの回転角速度を検出するヨーレートセンサ１３、地図データを記憶した地図データベース（以下、地図ＤＢという）１４、ナビゲーション装置１を操作するための入力装置１５及び地図や自車両の現在位置を表示する表示装置１６が接続されている。なお、ジャイロセンサ、舵角センサ等、その他の自律センサを備えていてもよい。

ＧＰＳ受信器１１は、周知の方法でＧＰＳ衛星からの電波に基づき自車両の位置を出力する。ＧＰＳ受信器１１は、複数のＧＰＳ衛星から発信される電波の到達時間に基づきＧＰＳ衛星までの距離を算出し、３つ以上のＧＰＳ衛星と自車両との距離が交差する１点を自車両の位置として測位する。

車速センサ１２は、タイヤの回転に伴ってパルス信号を出力するセンサであり、サンプリング時間毎の検出されるパルス数に基づき車速を検出する。パルス数と走行距離の関係は予め走行距離が知られた状態で「走行距離÷パルス数」の関係からパルス係数として算出しておく。しかしながら、タイヤの空気圧（径）が変化するとパルス係数が異なり、また、低速状態ではパルスによる走行距離の検出は誤差を生じる場合がある。

ヨーレートセンサ１３は、車両の鉛直軸回りの回転角速度に比例した電圧等を発生するセンサであり、比例定数を用いてヨーレートセンサ１３の出力を電圧等から回転角速度へ換算する。ヨーレートセンサ１３は、車体へ取り付けた検出軸と車両の旋回軸とが一致していない場合、予め設定された比例定数が実際の換算比率と異なってしまい、誤差を含んだ回転角速度を出力してしまう。また、ヨーレートセンサ１３は、検出軸を所定方向（例えば重力方向）に沿って配置されるので、車両が傾斜面で旋回した場合、換算比率が変化して誤差を含むことになる。

地図ＤＢ１４は、ハードディスクやＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ等で構成され、道路網や交差点などの道路地図情報が、緯度・経度に対応づけて格納されている。地図ＤＢ５には実際の道路網に対応づけて、ノード(道路と道路が交差する点、交差点から所定間隔毎に区切った点等)に関係する情報と、リンク(ノードとノードを接続する道路)に関係する情報とからなるテーブル状のデータベースに格納される。

入力装置１５は、タッチパネル、押下式のキーボード、ボタン、リモコン、十字キー等で構成される、運転者からの操作を入力するためのインターフェイスである。また、マイクを備え運転者の発する音声を音声認識回路で認識して操作を入力してもよい。目的地までのルート検索を行う場合、運転者は目的地を住所、地名、ランドマーク名、郵便番号等で入力することができる。

表示装置１６は、液晶や有機ＥＬ、ＨＵＤ（Head Up Display）等により構成され、自車両周辺の道路地図や指定された地域の道路地図を、指定された縮尺に合わせ表示すると共に、必要に応じて、自車両の位置及び目的地までの経路等を道路地図に表示する。また、表示装置１６はスピーカを備え、スピーカにより右左折する交差点など経路に沿った進行方向を音声により案内する。

図２（ｂ）は、ナビＥＣＵ１０の機能ブロック図を示す。ナビＥＣＵ１０のＣＰＵがプログラムを実行することで、車速センサ１２やヨーレートセンサ１３による検出情報を累積して自車両の測位位置を検出する慣性測位手段１０ａ、ダイナミックスモデルに基づき測位位置の誤差分散を漸化的に導出する誤差分散算出手段１０ｂ、信頼性誤差楕円を算出する誤差楕円算出手段１０ｄ、信頼性誤差楕円が所定領域と重複するか否かを判定する判定手段１０ｄ、が実現される。なお、ダイナミックスモデル及びマハラノビス距離はナビＥＣＵ１０の記憶装置に記憶されている。

本実施形態では、慣性測位手段１０ａが車速センサ１２やヨーレートセンサ１３からの検出情報を累積して周知の方法で自車両の測位位置を検出（推定）する。そしてこの側位位置の誤差分散を誤差分散算出手段１０ｂが算出する。

〔更新式による誤差分散の導出〕
誤差分散算出手段１０ｂによる誤差分散の導出について詳細に説明する。本実施形態では、車両の位置を推定するダイナミックスモデルを設定する。時刻ｔの自車両の位置情報及びセンサ情報をダイナミックスモデルに入力すると、ダイナミックスモデルは時刻ｔ＋１の推定位置を出力する。そこで、ダイナミックスモデルを変形して時刻ｔの誤差分散から時刻ｔ＋１の推定位置の誤差分散を導出する。なお、ダイナミックスモデルについては後述する。

車両の真の位置・方位Ｘ_ｔ、並進速度・回転角速度Ｕ_ｔをそれぞれ次のよう表す（いずれもベクトル量）。なお、並進速度は、進行方向の車速でなく車長方向及び車幅方向への車速である。

Ｘ_ｔ＝（ｘ，ｙ，θ）
Ｕ_ｔ＝（ｖ_t，ｗ_t）
Ｘ_ｔ、Ｕ_ｔの入力により自車両の位置を推定する式（以下、単にダイナミックスモデルという）をｆ（Ｘ_ｔ，Ｕ_ｔ）とすれば、時刻ｔ＋１における自車両の位置Ｘ_t+1は次のように表すことができる。

Ｘ_t+1 ＝ ｆ（Ｘ_ｔ，Ｕ_ｔ） ＋ ｎ_t … （１）
但し、ｎ_tは量子化や計算の丸め誤差によって生じる誤差である。

ここで、ΔＸ_ｔをＸ_ｔの誤差、ΔＵ_ｔをＵ_ｔの誤差、Ｘ^_tをＸ_tの推定値（平均値）、Ｕ^_tをＵ_tの推定値（平均値）、と置く。

また、自車位置推定式を次のように定義する。

Ｘ^_t+1 ＝ｆ（Ｘ^_t，Ｕ^_t） … （２）
式（２）を線形化するためテーラー展開する。

Ｘ_t+1 ＝ ｆ（Ｘ_ｔ，Ｕ_ｔ）＋ ｎ_t
＝ ｆ（Ｘ^_t＋ΔＸ_ｔ，Ｕ^_t＋ΔＵ_t）＋ｎ_t
≒ ｆ（Ｘ^_t，Ｕ^_t）＋ Ｊ_xΔＸ_ｔ ＋ Ｊ_uΔＵ_ｔ ＋ ｎ_t
＝ Ｘ^_t+1 ＋ ΔＸ_t+1
但し、Ｊ_x、Ｊ_uは次のように表され、また、３行目から４行目にかけて式（２）の関係を用いた。

また、３行目と４行目から次の関係が得られるので、この式から誤差分散を導出する更新式を求めることができる。
ΔＸ_t+1 ＝ Ｊ_xΔＸ_ｔ ＋ Ｊ_uΔＵ_ｔ ＋ ｎ_t
以上から、推定位置の誤差分散は次のように表すことができる。式（３）が誤差分散を漸化的に導出する更新式となる。但し、
Σ_Xt＝Ｅ（ΔＸ_tΔＸ_t ^T）、Σ_Ut＝Ｅ（ΔＵ_tΔＵ_t ^T）、Σ_n＝Ｅ（ΔｎΔｎ^T）
と置いた（右上のＴは転置行列を表す）。Ｅは期待値を表す記号であり、したがってΣは共分散行列を示す。

第１項〜第３項の内容はそれぞれ次のようになる。

第１項：時刻ｔの推定位置誤差が時刻ｔ＋１の推定位置誤差に及ぼす量
第２項：センサ値の誤差
第３項：計算の丸め誤差、その他の要因による誤差
したがって、更新式から分かるように、時刻ｔ＝０のΣ_X０、Σ_U0、Σ_nをそれぞれ決定すれば、推定位置の誤差分散を漸化的に導出することができる。

〔ダイナミックスモデル〕
続いて、ダイナミックスモデルについて説明する。図３はダイナミックスモデルの一例として幾何学車両モデルを示す図である。なお、本実施形態のダイナミックスモデルは一例であり、その他のモデルを適宜設定しても本実施形態の誤差分散の導出に好適に用いることができる。

図３のダイナミックスモデルでは、車両２０は４輪車であるが説明のため、前側Ｆｃ（操舵輪）、後輪Ｒｌ、Ｒｒの３つの車輪を示した。車両の全長Ｌは重心を起点にＬ＝Ｌｆ（前側）＋Ｌｒ（後ろ側）となっている。

以下、重心点における車両の並進速度ｖ_t、回転角速度ｗ_tを求める。
Ａ．車速センサ１２により検出される４つの車輪の平均の車速を車速Ｖstrと近似する。
Ｂ．車速Ｖstrを車両の車長方向と車幅方向に分解する。分解するため舵角センサにより舵角αを検出する。

・車長方向の速度〔ｍ／ｓ〕
Ｖ_model_ｘ ＝ Ｖstr×cos（α）
・車幅方向の速度
Ｖ_model_fy ＝ Ｖstr×sin（α）
Ｃ．重心の並進速度・回転角速度を算出する。車両の後輪の位置における横方向の並進速度をゼロと仮定して、前輪位置からの重心までの距離に応じて重心位置の並進速度・回転角速度を算出する。

・前輪位置からの重心位置までの距離により定まる係数
Ｋ_wheelbase ＝ Ｌｒ／Ｌ
・重心の車長方向の並進速度〔ｍ／ｓ〕
Ｖ_model_x
・重心の車幅方向の並進速度〔ｍ／ｓ〕
Ｖ_model_gy ＝ Ｖ_model_fy×Ｋ_wheelbase
・重心回転角速度〔rad／ｓ〕
yaw_model_g ＝ Ｖ_model_fy／Ｌ
以上からＶt、Ｗtは次のようになる。

車両の真の位置・方位Ｘ_tについては並進速度・回転角速度から算出される。すなわち、前回の位置・方位に、並進速度・回転角速度に基づく変化量を累積する。但し、Ｓ_tはサンプリング時間。

ｘ_t+1＝ ｘ_t＋ Ｖ_model_x ×Ｓ_t
ｙ_t+1＝ ｙ_t＋ Ｖ_model_gy×Ｓ_t
θ_t+1＝ θ_t＋ ｗ_ｔ×Ｓ_t
以上のように、ダイナミックスモデルを設定することで、車両の真の位置・方位及び並進速度・回転角速度を算出する関数形（ダイナミックスモデル）を導出することができる。

本実施例によれば、時刻ｔにおける誤差分散、センサ値の誤差分散及び計算の丸め誤差を更新式に適用することで、時刻ｔ＋１における自車位置(推定位置)の誤差分散を漸化的に導出することができる。誤差分散により慣性航法による測位について精度よくその誤差の評価を行うことができる。

図４は、慣性航法による測位結果及び誤差分散の一例を示す図である。自車両は真の位置Ｏに存在し、交差点Ｐに向かって走行している。慣性航法により測位された測位位置Ｑに対し、更新式により導出される点線で示す誤差分散Ｑｖが得られている。なお、交差点Ｐの領域を示すため交差点のノードＰを中心にした同心円状の交差点領域Ｐｖを示した。

測位位置Ｑによれば、自車両は交差点領域Ｐｖに存在しない。しかしながら、測位位置Ｑは誤差を有するものであり、実際は交差点領域Ｐｖに存在するかもしれない。

したがって、例えば、自車両が交差点に存在する場合に運転者に注意を促す警報を吹聴するような場合、測位位置Ｑにのみ基づく判定は十分でない。

これに対し、本実施形態では測位位置Ｑだけでなく推定位置の誤差分散Ｑｖを導出しているので、誤差分散Ｑｖが交差点領域Ｐｖと重なる場合には、交差点付近に自車両が存在する可能性があることを検出できる。

本実施例によれば、慣性航法により自車位置を検出する場合でも、自車両の位置を広がりを持って検出するので的確な車両制御が可能になる。また、自車両の位置の広がりは、移動体の位置に影響を与える要因を考慮したダイナミックスモデルに基づき導出したものであるため、慣性航法の測位誤差を好適に評価できる。

ところで、図４のように自車両の測位位置Ｑと誤差分散Ｑｖを求めた場合、誤差分散Ｑｖによっては交差点から遠い位置に車両が存在しても交差点に存在すると判定するおそれがある。

そこで、本実施例では、マハラノビス距離Ｄによる信頼性誤差楕円を算出し、信頼性誤差楕円が交差点領域Ｐｖと重なるか否かに基づき、自車両が交差点領域Ｐｖに存在するか否かを判定する。

図５は信頼性誤差楕円による自車位置の判定の概略を示す図である。図５に示すように、信頼性誤差楕円を生成するため、「自車位置（測位位置）」、「自車位置の誤差分散」及び「自車の存在確率（マハラのビス距離Ｄ）」を用いる。

信頼性誤差楕円の算出式は次のようになる。
信頼性誤差楕円 ＝ （Ｘ−Ｘ^*）^TΣ_Xt ^-1（Ｘ−Ｘ^*） ＝ Ｄ …（４）
Ｘ^*は「自車位置（測位位置）」であり、Σ_Xtは「自車位置の誤差分散（更新式の第１項）」であるので、適当な存在確率を想定しマハラノビス距離Ｄを入力すれば、誤差楕円算出手段１０ｃは、信頼性誤差楕円を決定することができる。

また、判定手段１０ｄは、地図ＤＢ１４から交差点ノードの位置情報を抽出し、例えば幅員に応じて所定半径の領域を交差点領域Ｐｖと決定する。

そして、判定手段１０ｄは、信頼性誤差楕円と交差点領域Ｐｖが重なれば交差点内に自車両が存在すると、信頼性誤差楕円と交差点領域Ｐｖが重ならなければ交差点内に自車両が存在しないと判定できる。

図６は、信頼性誤差楕円に基づき実際に走行した道路の交差点に自車両が存在するか否かを判定した判定結果を示す図である。

自車両は左上方向（矢印方向）から交差点を右折して図６の下方向へ走行する。ナビＥＣＵは走行中、所定時間毎に信頼性誤差楕円を算出するので、複数の信頼性誤差楕円が記載されている。信頼性誤差楕円は上記のように測位位置の誤差分散を示すものなので、測位した位置の軌跡に沿って算出される。

信頼性誤差楕円により図示した交差点に自車両が存在すると判定されるか否かを実験走行した。すなわち、交差点を通過するまでの間、自車両が実際に交差点に入っている時間は、信頼性誤差楕円の少なくとも一部が交差点領域と重畳するか否かを判定した。なお、自車両が実際に交差点に入っていることは目視により確認した。

実験走行は２０回行い、比較のため、測位誤差を考慮しない測位位置のみにより判定する実験走行を同様に２０回行った。

信頼性誤差楕円により判定した場合は、２０回の走行のうち２０回全てにおいて、交差点領域に入っていると判定された。これに対し、測位誤差を考慮せず測位位置のみにより判定した場合、２０回の走行のうち１４回は交差点に入っていると判定されたが、残りの６回は自車両が実際に交差点に入っていても、それが検出されない場合があった。このような実験結果から、信頼性誤差楕円に基づく判定が有効であることが分かる。

本実施例によれば、自車両が所定の位置に存在するか否かを確実に判定することができる。信頼性誤差楕円の大きさはマハラのビス距離Ｄにより制御できるので、交差点から遠いのに交差点に存在すると判定するような誤判定を最小限にすることができる。

自車位置の誤差評価の概略を示す図である。 ナビゲーション装置の概略構成図である。 ダイナミックスモデルの一例を示す図である。 慣性航法による測位結果及び誤差分散の一例を示す図である。 信頼性誤差楕円による自車位置の判定の概略を示す図である。 信頼性誤差楕円に基づき実際に走行した道路の交差点に自車両が存在するか否かを判定した判定結果を示す図である。

符号の説明

１ ナビゲーション装置
１０ ナビＥＣＵ
１１ ＧＰＳ受信器
１２ 車速センサ
１３ ヨーレートセンサ
１４ 地図ＤＢ
１５ 入力装置
１６ 表示装置

Claims (8)

1. 移動体の位置を検出するナビゲーション装置において、
   前記移動体の挙動情報を検出する自律センサと、
   前記自律センサによる検出情報を累積して前記移動体の慣性測位位置を検出する慣性測位手段と、
   時刻ｔの、前記慣性測位位置の誤差分散、前記自律センサのセンサ誤差分散及び計算誤差を、前記移動体の移動モデルに基づく更新式に適用して、時刻ｔ＋１の前記誤差分散を漸化的に算出する誤差分散算出手段と、
   前記誤差分散のうち前記移動体が所定確率で存在する範囲が、所定領域と少なくとも一部が重複するか否かに基づき前記所定領域内に前記移動体が存在するか否かを判定する判定手段と、
   を有することを特徴とするナビゲーション装置。
2. 前記慣性測位位置、前記誤差分散、及び、マハラノビス距離により信頼性誤差楕円を算出する誤差楕円算出手段と、
   前記信頼性誤差楕円が、前記所定領域と少なくとも一部が重複するか否かに基づき前記所定領域内に前記移動体が存在するか否かを判定する判定手段と、
   を有することを特徴とする請求項１項記載のナビゲーション装置。
3. 前記移動体の移動モデルは、前記移動体のダイナミックスモデルである、
   ことを特徴とする請求項１又は２記載のナビゲーション装置。
4. 前記自律センサは、車速センサ、ヨーレートセンサ、ジャイロセンサ及び舵角センサの少なくとも１つである、
   ことを特徴とする請求項１又は２記載のナビゲーション装置。
5. 前記誤差分散算出手段は、前記誤差分散を下記の前記更新式に基づき算出することを特徴とする請求項１又は２項記載のナビゲーション装置。
   

   
6. 移動体の位置を検出する位置検出方法において、
   自律センサが、前記移動体の挙動情報を検出するステップと、
   慣性測位手段が、前記自律センサによる検出情報を累積して前記移動体の慣性測位位置を検出するステップと、
   誤差分散算出手段が、時刻ｔの、前記慣性測位位置の誤差分散、前記自律センサのセンサ誤差分散及び計算誤差を、前記移動体の移動モデルに基づく更新式に適用して、時刻ｔ＋１の前記誤差分散を漸化的に算出するステップと、
   判定手段が、前記誤差分散のうち前記移動体が所定確率で存在する範囲が、所定領域と少なくとも一部が重複するか否かに基づき前記所定領域内に前記移動体が存在するか否かを判定するステップ、
   を有することを特徴とする位置検出方法。
7. 誤差楕円算出手段が、前記慣性測位位置、前記誤差分散、及び、マハラノビス距離により信頼性誤差楕円を算出するステップと、
   判定手段が、前記所定領域と少なくとも一部が重複するか否かに基づき前記所定領域内に前記移動体が存在するか否かを判定するステップと、
   を有することを特徴とする請求項６記載の位置検出方法。
8. 前記誤差分散は下記の前記更新式に基づき算出される、
   ことを特徴とする請求項６又は７項記載の位置検出方法。
   

   

Images