トップ :: G 物理学 :: G01 測定;試験

【発明の名称】 測位装置
【発明者】 【氏名】小堀 訓成

【氏名】香川 和則

【要約】 【課題】自律航法による測位結果を修正してより精度よく移動体の位置を測位する測位装置を提供すること。

【構成】地図データを記憶した地図データ記憶手段5と、移動体の挙動情報を検出する自律センサ2、3)と、GPS等の電波航法測位手段による測位結果に自律センサによる検出情報を累積して移動体の推定位置を検出する慣性測位手段82と、道路周辺の地物を検出する地物検出手段84と、地物の位置を同定する地物位置同定手段83と、地物の位置を基準に移動体の地物推定位置を推定する地物基準測位手段85と、推定位置及び地物推定位置をカルマンフィルタに適用して移動体の位置を推定する位置推定手段86と、を有することを特徴とする測位装置9を提供する。
【特許請求の範囲】
【請求項1】
地図データを記憶した地図データ記憶手段と、
移動体の挙動情報を検出する自律センサと、
GPS等の電波航法測位手段による測位結果に前記自律センサによる検出情報を累積して前記移動体の推定位置を検出する慣性測位手段と、
道路周辺の地物を検出する地物検出手段と、
前記地物検出手段により検出された前記地物の位置を同定する地物位置同定手段と、
前記地物の位置を基準に前記移動体の地物推定位置を推定する地物基準測位手段と、
前記推定位置及び前記地物推定位置をカルマンフィルタに適用して前記移動体の前記位置を推定する位置推定手段と、
を有することを特徴とする測位装置。
【請求項2】
前記地物検出手段は、
撮影手段により撮影された画像データから前記地物を検出し、該地物までの視線ベクトルを取得する視線ベクトル検出手段を有し、
前記地物位置同定手段は、
複数の前記視線ベクトルの長さ及び方向を使用して最小2乗法により前記地物の地物位置情報を算出する地物位置算出手段を有する、
を有することを特徴とする請求項1記載の測位装置。
【請求項3】
前記地物位置算出手段は、
前記地図データ記憶手段から、前記地物から所定距離内にある交差点の交差点位置情報を抽出し、最小2乗法により前記地物位置情報を算出する際の初期値に前記交差点位置情報を設定する、
ことを特徴とする請求項2記載の測位装置。
【請求項4】
前記地物位置同定手段が取得した前記地物の前記地物位置情報を、該地物及びリンク又はノードと対応づけて前記地図データ記憶手段に登録する地図データ登録手段、
を有することを特徴とする請求項1ないし3いずれか記載の測位装置。
【請求項5】
前記視線ベクトル検出手段により検出された前記視線ベクトルが所定数以下の場合、前記地物検出手段は、最小2乗法による前記地物位置情報を算出せず、前記位置推定手段は前記推定位置を出力する、
ことを特徴とする請求項1記載の測位装置。



【発明の詳細な説明】【技術分野】
【0001】
本発明は移動体の位置を検出する測位装置に関し、特に、自律航法により測位された移動体の位置を精度よく補正する測位装置に関する。
【背景技術】
【0002】
ナビゲーションシステムでは、GPS(Grobal Positioning System)衛星からの電波に基づき自車両の位置を測位して、車速センサ及びジャイロセンサ等を用いて走行距離及び走行方向を累積しながら車両の現在位置を精度よく推定する。
【0003】
しかしながら、GPS衛星からの電波を受信できない状態では、自律航法による測位に含まれる誤差が時間と共に増幅されるため徐々に位置の精度が低下する。
【0004】
このため、自律航法により測位した自車両の位置を補正する種々の方法が提案されている。例えば、マップマッチングでは、ナビゲーションシステムの地図データを利用して自律航法により測位した位置を補正する(例えば、特許文献1参照。)。特許文献1では、自律航法により測位された位置・方向に最も整合する位置及び方位を有する道路を地図データから選択して、該道路に対応づけて位置・方向を補正する方法が提案されている。
【0005】
しかしながら、一般に市販されているナビゲーションシステムの地図データはそれほど精度が高いものではなく、また、道路網を交差点(ノード)を結ぶ直線状のリンクにより表現するため実際の道路の形状と一致しない場合がある。このため、マップマッチングでは自車両の位置の修正を十分に修正できないことがある。
【0006】
また、カメラにより撮影された画像から信号機や横断歩道などの交差点シンボルが検出された場合に、自車両と交差点までの距離を算出して、算出された距離に応じて自車両の位置を補正するナビゲーション装置が提案されている(例えば、特許文献2参照。)。特許文献2では、交差点との距離を算出することで、長い直線道路を走行している場合であっても進行方向に対する自車両の位置を修正することができるとしている。
【特許文献1】特開2002−213979号公報
【特許文献2】特開平9−243389号公報
【発明の開示】
【発明が解決しようとする課題】
【0007】
しかしながら、特許文献2に記載されたように、撮影された画像データから交差点との距離を算出しそのまま自車両の位置を補正すると、画像データから算出される距離にも誤差が含まれるため、補正後の位置が正しいものとは限らないという問題がある。例えば、自車両がピッチ運動する場合、算出される距離に大きな誤差が含まれることになる。
【0008】
本発明は、上記課題に鑑み、自律航法による測位結果を修正してより精度よく移動体の位置を測位する測位装置を提供することを目的とする。
【課題を解決するための手段】
【0009】
上記課題を解決するため、本発明は、地図データを記憶した地図データ記憶手段(例えば、地図DB5)と、移動体の挙動情報を検出する自律センサ(例えば、車速センサ2、ヨーレートセンサ3)と、GPS等の電波航法測位手段による測位結果に自律センサによる検出情報を累積して移動体の推定位置を検出する慣性測位手段(例えば、INS測位手段82)と、道路周辺の地物を検出する地物検出手段(例えば、信号機検出手段84)と、地物の位置を同定する地物位置同定手段(例えば、信号機位置同定手段83)と、地物の位置を基準に移動体の地物推定位置を推定する地物基準測位手段と、推定位置及び地物推定位置をカルマンフィルタに適用して移動体の位置を推定する位置推定手段と、を有することを特徴とする測位装置を提供する。
【発明の効果】
【0010】
自律航法による測位結果を修正してより精度よく移動体の位置を測位する測位装置を提供することができる。
【発明を実施するための最良の形態】
【0011】
以下、本発明を実施するための最良の形態について、図面を参照しながら説明する。
図1は、本実施の形態の測位装置9を適用したナビゲーションシステム10の概略構成図を示す。
【0012】
ナビゲーションシステム10は、ナビゲーションシステムを制御するナビECU(Electrical Control Unit)8により制御される。ナビECU8は、プログラムを実行するCPU、プログラムを記憶した記憶装置(ハードディスクドライブ、ROM)、データやプログラムを一時的に記憶するRAM、データを入力及び出力する入出力装置、NV(Non Volatile)−RAM等がバスを介して接続されたコンピュータとして構成される。
【0013】
ナビECU8にはGPS(Grobal Positioning System)衛星からの電波を受信するGPS受信装置1、車両の速度を検出する車速センサ2、車両の重心回りの回転角速度を検出するヨーレートセンサ3(又はジャイロセンサ)、操舵輪の舵角を検出する舵角センサ4、地図データを記憶した地図データベース(以下、地図DBという)5、ナビゲーションシステム10を操作するための入力装置6及び地図に現在位置を表示する液晶やHUD(Head Up Display)等の表示装置7が接続されている。
【0014】
地図DB5は、実際の道路網をノード(例えば、道路と道路が交差する点、交差点から所定間隔毎に区切った点、等)及びリンク(ノードとノードを接続する道路)に対応づけて、テーブル状のデータベースとして構成される。
【0015】
ナビECU8は、車室内に設けられた表示装置7に、検出した現在位置周辺の地図を地図DB5から抽出し、指定された縮尺に合わせて表示する。ナビECU8は、必要に応じて地図に重畳して車両の現在位置を表示する。
【0016】
また、押下式のキーボードやリモコン等の入力装置6から目的地が入力されると、検出した現在位置から目的地まで、例えばダイクストラ法など周知のルート検索方法でルート検索を行い、ルートを地図に重畳して表示すると共に、右左折する交差点の手前で運転者にルートを案内する。
【0017】
カメラ11は車両前方、より好ましくは車両前方の所定範囲を撮影するようにルームミラーの背面やフロントガラスの上方に取り付けられている。
【0018】
カメラ11は、例えばCCD(Charge Coupled Device)、CMOS(Complementary Metal Oxide Semiconductor)等の光電変換素子を有し、入射した光を光電変換素子により光電変換し、蓄積した電荷を電圧として読み出し増幅してA/D変換した後、所定の輝度階調(例えば、256階調)のデジタル画像に変換する。
【0019】
カメラ11は交差点などの道路に沿って設置された人工の地物(信号機、標識、横断歩道などのペイント、電柱等)と自車両との位置を検出するため、距離情報が得られるカメラであることが好ましい。このため、例えばカメラ11は2台のカメラからなるステレオカメラ、移動体に搭載した1台のカメラの時系列画像からステレオ視を行うモーションステレオカメラである。また、LED(発光ダイオード)から所定の時間間隔で近赤外光を照射し、反射光を光電変換素子が受光するまでの時間を計測することで距離を求めることが可能なカメラであってもよい。
【0020】
ナビECU8のCPUは、記憶装置に記憶されたプログラムを実行することで、本実施の形態で説明する測位を実現する。図2(a)は、測位装置9の機能ブロック図を示す。測位装置9は、電波航法測位により自車両の位置を測位するGPS測位手段81、自律センサ(車速センサ2,舵角センサ4)を用いて自律航法により自車両の位置を測位するINS測位手段82、カメラ11により撮影された画像データから信号機等の地物を検出する信号機検出手段84,検出された信号機の位置を同定する信号機位置同定手段83、信号機の位置に基づき自車両の位置を測位する地物基準測位手段85、カルマンフィルタにより自車両の位置の最尤値を出力する位置推定手段86及び信号機位置同定手段83が同定した信号機など地物の位置情報を地物に対応づけて地図DB5に登録する地図データ登録手段87を有する。以下、詳細に説明する。
【0021】
本実施の形態の測位装置9は、GPS衛星の捕捉が困難な状況やGPS測位の信頼性が低下した状況下で、自律航法による車両の位置を補正して精度よい測位を可能にする。
【0022】
図2(b)により概略を説明すれば、GPS電波が遮断された場合、測位装置9は自律航法により測位された位置及び信号機など地物に対する視線ベクトルに基づき測位された位置を、カルマンフィルタによりカップリングして、位置Yを精度よく推定する。
【0023】
まず、GPS測位手段81は、周知の方法でGPS衛星からの電波に基づき自車両の位置を測位する。GPS測位手段81は所定の軌道を周回する複数のGPS衛星のうち現在の車両の位置から所定の仰角に入るGPS衛星を4つ以上選択し、それらのGPS衛星から発信される電波を受信する。GPS測位手段81は電波の到達時間を計算し、到達時間と光速cから捕捉したGPS衛星までの距離を算出する。そして、3つのGPS衛星と自車両の距離が交差する1点を自車両の位置として測位する。
【0024】
測位装置9は、GPS電波を受信している間は所定の時間毎に自車両の位置を測位する。そして、GPS電波が遮断されると、最後に測位した位置を初期位置、その時の走行方向を初期方向として、これらに走行距離及び方向を累積する自律航法による測位を開始する。
【0025】
図3(a)は自律航法により検出される位置を示す図である。自車両は交差点の方向に向かって走行しており初期位置23でGPS電波が遮断される。また、地図DB5から交差点のノード22が抽出されているため、ノード22の位置は既知となっている。
【0026】
INS(Inertial Navigation Sensor)測位手段82は車速センサ2から車速、舵角センサ4から舵角を検出して、初期位置23及び初期方向に走行距離及び方向を累積して、自律航法による位置及び方向を推定する(以下、推定位置、推定方向という)。なお、自車両の走行方向を検出する自律センサは、ジャイロセンサやヨーレートセンサでもよい。
【0027】
また、カルマンフィルタを適用するためには推定位置の誤差分散を必要とするが、車速センサ2及び舵角センサ4の誤差は速度やGPS電波遮断時間等に応じて既知である。したがって、これらに基づき累積した推定位置24の誤差分散も既知である。図3(a)では推定位置24の誤差分散を点線で囲まれた楕円で示した。なお、この時(時刻t)、自車両は実際の位置25に存在する。
【0028】
自律航法により位置を検出している間、信号機検出手段84はカメラ11が撮影した画像データから信号機を検出する。検出した信号機の位置を利用して、自車両の位置を推定するためである。なお、信号機21は車両から利用できる地物であればよく、信号機21の他に標識や電信柱を検出してもよい。
【0029】
信号機検出手段84は、予め記憶してある信号機の標準パターンを用いてパターンマッチングにより信号機を検出する。信号機検出手段84は、画像データの画素値(輝度)を水平方向及び垂直方向に走査し、勾配が所定以上のエッジ部を抽出する。そして隣接するエッジ部を接続して撮影対象物の輪郭を抽出し、これに標準パターンをパターンマッチングする。なお、信号機の場合その輪郭は長方形であるので、所定の縦横比の輪郭となるエッジ部のみにパターンマッチングを施してもよい。信号機検出手段84は、輪郭に区切られる領域と標準パターンとを画素毎に輝度を比較し、所定以上に相関する場合に信号機が撮影されているとして信号機を検出する。
【0030】
信号機が検出された場合、信号機検出手段84は画像データから距離情報を抽出する。上記のように、距離情報は例えば2つの画像データの視差から抽出する。カメラ11により撮影された1対のステレオ画像から同一の撮像対象物(信号機)が写っている部分を抽出し、1対のステレオ画像同士で信号機の同一点を対応づけ、対応づけられた点(対応点)のずれ量(視差)を求めることによって信号機までの距離を算出する。すなわち、1対の画像データを重ね合わせると、視差により信号機が左右横方向にずれる。一方の画像を1画素ずつシフトしながら最も重なり合う位置を、画素値の相関に基づいて求める。このときシフトした画素数をn、レンズの焦点距離をf、光軸間の距離をm、画素ピッチをdとすると、撮像対象物までの距離Lは、『L=(f・m)/(n・d)』という関係式が成立する。(n・d)が視差である。
【0031】
また、信号機検出手段84は、検出された信号機と自車両との視線ベクトルを算出する。視線ベクトルの方向θは、カメラ11の取付方向(カメラ11の正面方向)をゼロにして、光電変換素子における信号機の撮影位置と距離Lにより求められる。図4(a)は視線ベクトルの一例を示す図である。
【0032】
ところで、地図DB5にはノードの座標、交差点の有無及び種類に加え、信号機の設置有無や信号機の設置座標(以下、信号機座標という)が記憶されている場合がある。信号機座標が記憶されている場合には、信号機の絶対位置が予め確定しているので検出された信号機の信号機座標を取得することができる。
【0033】
しかしながら、1つの画像データに複数の信号機が撮影されている場合や地図DB5に信号機座標が記憶されていない場合には、信号機が検出された画像データから信号機の位置を同定する必要がある。
【0034】
本実施形態の信号位置同定手段は83は、i)信号機座標、ii)最小2乗法、iii)最急勾配法を適用した最小2乗法、のいずれかにより信号機の位置を同定する。
【0035】
図4(b)は信号機の位置を最小2乗法を利用して同定するイメージを示す図である。信号機が検出される度に、距離L1、L2、…Ln、同様に視線ベクトルの方向θ1、θ2…θn、が得られるので、自車両からの距離と視線ベクトルの方向の組(L、θ)により点の集合(a、b)が得られる。なお、信号機(灯)の高さ方向の位置はおおよそ一定であるので、(a、b)は道路に平行な平面の座標である。
【0036】
例えば、a≡k+k+kθ≡m+m+mθ と線型モデルを立てれば、2乗誤差ε(k、k,k)、ε(m、m,m)は次のようになる。但し、サンプリング数をNとし、iは1〜Nまでの値を取る。
ε(k、k,k)=(1/N)Σ{ai−(k+k+kθ)}
ε(m、m,m)=(1/N)Σ{bi−(m+m+mθ)}
εをk、k,kについて偏微分し、aε/ak=0、aε/ak=0、aε/ak=0、から(k、k,k)を求めることができる。aε/akの「a」は偏微分を表す。bについても同様に求められる。
【0037】
なお、上記の線型モデルは一例であり、(a、b)と(L、θ)の関係は非線形であってもよい。例えば、ai≡f(L)+g(θ)である。
【0038】
図3(b)は最小2乗法により同定される信号機21の位置を示す図である。時刻tにおける推定位置24に至るまでに検出される信号機までの距離L、視線ベクトルの方向θにより、信号機21の位置が同定される。
【0039】
ところで、最小2乗法により好適な演算結果を得るためには、サンプル数Nが4以上である必要がある。このため、信号位置同定手段83は、サンプル数Nが4未満の場合、すなわち信号機が検出され視線ベクトルが算出された回数が4未満の場合、信号機の位置を同定しない。これにより、カルマンフィルタを適用することで自車両の測位位置の精度が低下することを防止できる。この場合、測位装置9は推定位置24と推定方向を出力する。
【0040】
また、このような最小2乗法の考え方は、「各時刻の自車位置と信号機の位置との距離の2乗誤差の総和」を評価関数として設定し、それを最小にする信号機の位置を求めるものである。そこで、最急勾配法を使用して評価関数の傾きに基づき評価関数が最小となる信号機の位置を求める方法について説明する。
【0041】
図5(a)は評価関数と信号機の位置の関係を示す図である。算出された信号機の位置に応じて評価関数の値は変動し、評価関数に最小値が存在することが分かる。最急勾配法は、適当な初期値からはじめて、パラメータの値を微分値と逆の方向に少しずつ変化させて徐々に最適なパラメータに近づけて行く。
【0042】
最小2乗法を用いた場合、評価関数は最小2乗誤差であるので各パラメータでの偏微分を計算する。例えば、パラメータk、k,kについてはそれぞれ次式が更新式となる。
(j+1)=k(j)+2・(1/N)Σ{(a−(k+k+kθ))
(j+1)=k(j)+2・(1/N)Σ{(a−(k+k+kθ))L
(j+1)=k(j)+2・(1/N)Σ{(a−(k+k+kθ))θ
添字の「j」は0から始まり、k(j+1)が最小となったら、そのパラメータが、評価関数が最小となるパラメータであるので計算を打ち切る。評価関数が最小となったか否かは微分値(傾き)がほぼゼロになったか否かで判定される。このようにして、パラメータk、k,kがえられる。図5(a)では評価関数が最小となる信号機の位置をグローバルminとして示した。
【0043】
最急勾配法では、j=0の時のk(0)等が各パラメータの初期値となるが、この初期値に最小値の精度が依存することが知られている。図5(b)は、評価関数と信号機の位置の関係図の一例であるが、極小値が複数ある場合を示している。図5(b)に示すように、グローバルminは右側の極小値であるが、初期値によっては左側のローカルminに陥る場合がある。また、サンプル数が少ない場合は値が発散しやすい。
【0044】
しかしながら、信号機は交差点付近に存在すると推定してよい。そこで、最急勾配法の初期値に交差点のノード位置から求めた値を設定する。これは初期値をノード位置とすることと等価である。
【0045】
このような処理により、サンプル数Nが少ない場合でもグローバルminの信号機の位置を検出することができる。
以上のようにして、図3(b)の信号機21の位置が同定された。
【0046】
図2(a)に戻り、地物基準測位手段85は、i)信号機座標、ii)最小2乗法、又は、iii)最急勾配法を適用した最小2乗法、のいずれかにより取得した信号機21の位置に基づき、自車両の位置を測位する(以下、地物推定位置、地物推定方向という)。地物推定方向は、地物推定位置を数点蓄積することで検出される。
【0047】
図3(c)は、信号機の位置から距離L、視線ベクトルの方向θにより測位された自車両の地物推定位置26を示す図である。地物基準測位手段85は、信号機の位置を原点に、時刻tの距離L及び視線ベクトルの方向θから地物推定位置26及び地物推定方向を算出する。
【0048】
また、地物推定位置26の誤差分散は、i)信号機座標を用いた場合、信号機座標の場合は距離Lの誤差ΔLや視線ベクトルθの誤差Δθから求められ、ii)及びiii)では最小2乗法により信号機の位置を同定した場合にはi)に加えパラメータを算出する過程で得られる誤差により求められる。図3(c)では地物推定位置26の誤差分散を点線で囲まれた楕円で示した。
【0049】
位置推定手段86は、推定位置24と地物推定位置26とをカルマンフィルタによりカップリングして、確率的に最も高い自車両の位置及び方向を出力する。
【0050】
図3(c)には、推定位置24と地物推定位置26に基づき推定される最終推定位置27を示した。図3(c)では、それぞれの誤差分散を推定位置24と地物推定位置26に凸部を有する分散で示したが、分散は実際には3次元の広がりを有する。図2(b)にはその分散及びカルマンフィルタによる最尤位置Yの推定のイメージを示した。
【0051】
カルマンフィルタは、1つの系の状態がそれぞれ独立に推定される場合、状態の確率密度の分布に基づき最も確率の高い状態(分布の積が最大になる状態)を推定する。したがって、2つの測位情報をカルマンフィルタでカップリングすることで、存在する確率が最も高い最終推定位置27を推定することができる。
【0052】
カルマンフィルタでは、推定位置24をZベクトル、地物推定位置26をXベクトルとした場合、既知の観測方程式Hを用いてZとXの間には次の関係があるものとする。
Z−HX=0
Zの誤差分散をR、Xの誤差分散をMとすると、最終推定位置27に相当する最尤値Yの誤差分散は、解析的にA=(M−1+H−1H)−1になる。また、カルマンフィルタでは、最尤値Yが次式により求められる。なお、添字のiは自車両の位置を観測した番号、tは転置行列、−1は逆行列である。
Y(i)=X(i−1)+K(i)・{Z(i)−H(i)・X(i−1)}
K(i)はカルマンゲイン行列であり、次のように表せる。
K(i)=A(i)・HiRi−1
ただし、A(i)=(M−1+H−1H)−1
=M(i)−M(i)・H(i){H(i)M(i)H(i)+R(i)}−1H(i)M(i)
である。
【0053】
推定位置24の誤差分散、地物推定位置26の誤差分散はすでに求めているので、位置推定手段86はこれらの式から最終推定位置27を出力する。なお、自車両の方向についても同様にして求めることができる。
【0054】
図6は、測位装置9が自律航法及び信号機の位置に基づく測位を利用して自車両の位置を推定する手順を示すフローチャート図である。
【0055】
測位装置9は例えばGPS測位手段81の測位間隔毎にGPS電波が遮断されたか否かを判定する(S1)。GPS電波が遮断されていなければ(S1のNo)、GPS電波を利用して自車両の位置を測位する(S2)。
【0056】
GPS電波が遮断された場合(S1のYes)、GPS測位手段81は初期位置、初期方向をナビECU8の記憶装置に記憶しておき、INS測位手段82は車速及び舵角に基づき、初期位置23及び初期方向に走行距離及び走行方向を累積して自律航法による推定位置24及び推定方向を推定する(S3)。なお、INS測位手段82は、車速センサ2及び舵角センサ4の誤差等に応じて、これらに基づき累積した推定位置24、推定方向の分散を算出する。
【0057】
また、信号機検出手段84は、自律航法による測位に並行して信号機21の検出を繰り返す(S4)。そして、信号機が検出された場合、信号機検出手段84は1対の画像データから距離情報を抽出する。また、信号機検出手段84は、検出された信号機と自車両との視線ベクトルを算出する(S5)。
【0058】
ついで、信号機位置同定手段83は過去N回分の信号機との距離、視線ベクトルを参照する(S6)。
【0059】
上述したように、このNが小さいと最小2乗法により同定した信号機の位置の精度が低下するので、信号機位置同定手段83はNが4以上か否かを判定する(S7)。4つ以上の距離及び視線ベクトルが取得されていない場合(S7のNo)、測位装置9は自律航法による測位結果を出力する。
【0060】
4つ以上の距離及び視線ベクトルが取得されている場合(S7のYes)、信号機位置同定手段83は、進行方向にある交差点のノードの位置情報を地図DB5から抽出する(S8)。
【0061】
ついで、信号機位置同定手段83は、最小2乗法により信号機の位置を算出する(S9)。最小2乗法を適用する場合、最急勾配法を利用して更にその初期値にノードの位置情報を使用する。これにより信号機の位置が同定された。
【0062】
ついで、地物基準測位手段85は、同定された信号機の位置を原点に、距離L及び視線ベクトルの方向θから地物推定位置26及び地物推定方向を算出する(S10)。
【0063】
そして、位置推定手段86は、推定位置24及び地物推定位置26をカルマンフィルタによりカップリングして、最終推定位置27を出力する(S11)。
【0064】
図7は、自律航法により測位された推定位置24を最終推定位置27に修正するイメージを示す図である。図7では、最終推定位置27が出力されるたびに自律航法による推定位置を補正する様子を示す。本実施の形態では、走行方向にある信号機の位置を基準に自車両の位置を測位し、自律航法により測位された位置を補正するので、特に走路方向の位置を精度よく補正することができる。
【0065】
図8は、一般道路においてGPS電波が遮断された状況における測位装置9の測位結果を示す。図8(a)はカメラ11に撮影された画像データを示す。図8(a)では画像データの上側に信号機21が検出されている。
【0066】
図8(b)は実際の道路の平面写真に、自律航法による推定位置24と最終推定位置27とをプロットした図である。自車両は平面写真の下方から上方向に走行した。図8(b)に示すように、走行するにつれて推定位置24は実際の道路とのズレが徐々に大きくなるが、地物推定位置26で補正することにより、道路とのズレを大幅に低減することができる。
【0067】
〔信号機の位置のデータベース〕
ところで、本実施形態ではGPS電波が遮断された後、自律航法により測位された自車両の位置を補正する過程で信号機等のランドマークの位置を検出する。これは、上述のように、一般的なナビゲーションシステムの地図DB5には道路インフラ(信号機・標識等)の位置情報が記憶されていないためである。
【0068】
運転者に適切なタイミングで交差点等における運転支援(車両制御、注意喚起等)を提供するためには、道路インフラの位置情報が不可欠といえる。しかし、全国の道路インフラを測位し、データベース化するには多大な労力とコスト必要である。また、道路インフラが新たに設置された場合、すぐには対応できないという問題もある。
【0069】
そこで、測位装置9は、自車両の位置を補正する過程で検出した信号機等の位置、また、GPS電波が遮断されていない状態においても信号機等の位置を検出し、地図DB5に登録することとすれば、道路インフラの位置情報のデータベースを作成できる。
【0070】
図9は地図データから得られる道路網に信号機の位置を登録した地図の一例を示す。白丸が登録した信号機の位置を示す。
【0071】
車載したカメラによる道路インフラの位置の検出は、短時間かつ低コストに実現でき、道路インフラが新たに設置されても車両が走行すればデータベースに反映できるため、優れた冗長性を兼ね備える。
【0072】
以上のように、本実施形態の測位装置9は、信号機などの地物の位置を同定し地物の位置を基準に測位した自車両の位置と、自律航法により測位した位置をカルマンフィルタに適用することで、GPS電波が遮断されても自律航法による測位結果を精度よく補正することができる。特に、進行方向に対する位置を精度よく補正することができる。また、同定した地物の位置を地図DBに登録することで、道路インフラの位置情報が登録された地図DBを、更新しながら利用することができる。
【図面の簡単な説明】
【0073】
【図1】測位装置を適用したナビゲーションシステムの概略構成図である。
【図2】測位装置の機能ブロック図である。
【図3】信号機と自車両の位置との関係を示す図である。
【図4】視線ベクトルの一例を示す図である。
【図5】評価関数と信号機の位置の関係を示す図である。
【図6】測位装置が自律航法及び信号機の位置に基づく測位を利用して自車両の位置を推定する手順を示すフローチャート図である。
【図7】自律航法により測位された推定位置を最終推定位置に修正するイメージを示す図である。
【図8】一般道路においてGPS電波が遮断された状況における測位装置の測位結果を示す図ある。
【図9】地図データから得られる道路網に信号機の位置を登録した地図の一例を示す図である。
【符号の説明】
【0074】
1 GPS受信装置
2 車速センサ
3 ヨーレートセンサ
4 舵角センサ
5 地図DB
6 入力装置
7 表示装置
8 ナビECU
9 測位装置
10 ナビゲーションシステム
11 カメラ
21 信号機
22 交差点のノード
23 初期位置
24 推定位置
25 実際の自車位置
26 地物推定位置
27 最終推定位置


【出願人】 【識別番号】000003207
【氏名又は名称】トヨタ自動車株式会社
【出願日】 平成18年6月21日(2006.6.21)
【代理人】 【識別番号】100070150
【弁理士】
【氏名又は名称】伊東 忠彦


【公開番号】 特開2008−2906(P2008−2906A)
【公開日】 平成20年1月10日(2008.1.10)
【出願番号】 特願2006−171755(P2006−171755)