測位装置
【課題】自律航法による測位結果を修正してより精度よく移動体の位置を測位する測位装置を提供すること。
【解決手段】地図データを記憶した地図データ記憶手段５と、移動体の挙動情報を検出する自律センサ２、３）と、ＧＰＳ等の電波航法測位手段による測位結果に自律センサによる検出情報を累積して移動体の推定位置を検出する慣性測位手段８２と、道路周辺の地物を検出する地物検出手段８４と、地物の位置を同定する地物位置同定手段８３と、地物の位置を基準に移動体の地物推定位置を推定する地物基準測位手段８５と、推定位置及び地物推定位置をカルマンフィルタに適用して移動体の位置を推定する位置推定手段８６と、を有することを特徴とする測位装置９を提供する。
【解決手段】地図データを記憶した地図データ記憶手段５と、移動体の挙動情報を検出する自律センサ２、３）と、ＧＰＳ等の電波航法測位手段による測位結果に自律センサによる検出情報を累積して移動体の推定位置を検出する慣性測位手段８２と、道路周辺の地物を検出する地物検出手段８４と、地物の位置を同定する地物位置同定手段８３と、地物の位置を基準に移動体の地物推定位置を推定する地物基準測位手段８５と、推定位置及び地物推定位置をカルマンフィルタに適用して移動体の位置を推定する位置推定手段８６と、を有することを特徴とする測位装置９を提供する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は移動体の位置を検出する測位装置に関し、特に、自律航法により測位された移動体の位置を精度よく補正する測位装置に関する。
【背景技術】
【０００２】
ナビゲーションシステムでは、ＧＰＳ（Ｇｒｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波に基づき自車両の位置を測位して、車速センサ及びジャイロセンサ等を用いて走行距離及び走行方向を累積しながら車両の現在位置を精度よく推定する。
【０００３】
しかしながら、ＧＰＳ衛星からの電波を受信できない状態では、自律航法による測位に含まれる誤差が時間と共に増幅されるため徐々に位置の精度が低下する。
【０００４】
このため、自律航法により測位した自車両の位置を補正する種々の方法が提案されている。例えば、マップマッチングでは、ナビゲーションシステムの地図データを利用して自律航法により測位した位置を補正する（例えば、特許文献１参照。）。特許文献１では、自律航法により測位された位置・方向に最も整合する位置及び方位を有する道路を地図データから選択して、該道路に対応づけて位置・方向を補正する方法が提案されている。
【０００５】
しかしながら、一般に市販されているナビゲーションシステムの地図データはそれほど精度が高いものではなく、また、道路網を交差点（ノード）を結ぶ直線状のリンクにより表現するため実際の道路の形状と一致しない場合がある。このため、マップマッチングでは自車両の位置の修正を十分に修正できないことがある。
【０００６】
また、カメラにより撮影された画像から信号機や横断歩道などの交差点シンボルが検出された場合に、自車両と交差点までの距離を算出して、算出された距離に応じて自車両の位置を補正するナビゲーション装置が提案されている（例えば、特許文献２参照。）。特許文献２では、交差点との距離を算出することで、長い直線道路を走行している場合であっても進行方向に対する自車両の位置を修正することができるとしている。
【特許文献１】特開２００２−２１３９７９号公報
【特許文献２】特開平９−２４３３８９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００７】
しかしながら、特許文献２に記載されたように、撮影された画像データから交差点との距離を算出しそのまま自車両の位置を補正すると、画像データから算出される距離にも誤差が含まれるため、補正後の位置が正しいものとは限らないという問題がある。例えば、自車両がピッチ運動する場合、算出される距離に大きな誤差が含まれることになる。
【０００８】
本発明は、上記課題に鑑み、自律航法による測位結果を修正してより精度よく移動体の位置を測位する測位装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記課題を解決するため、本発明は、地図データを記憶した地図データ記憶手段（例えば、地図ＤＢ５）と、移動体の挙動情報を検出する自律センサ（例えば、車速センサ２、ヨーレートセンサ３）と、ＧＰＳ等の電波航法測位手段による測位結果に自律センサによる検出情報を累積して移動体の推定位置を検出する慣性測位手段（例えば、ＩＮＳ測位手段８２）と、道路周辺の地物を検出する地物検出手段（例えば、信号機検出手段８４）と、地物の位置を同定する地物位置同定手段（例えば、信号機位置同定手段８３）と、地物の位置を基準に移動体の地物推定位置を推定する地物基準測位手段と、推定位置及び地物推定位置をカルマンフィルタに適用して移動体の位置を推定する位置推定手段と、を有することを特徴とする測位装置を提供する。
【発明の効果】
【００１０】
自律航法による測位結果を修正してより精度よく移動体の位置を測位する測位装置を提供することができる。
【発明を実施するための最良の形態】
【００１１】
以下、本発明を実施するための最良の形態について、図面を参照しながら説明する。
図１は、本実施の形態の測位装置９を適用したナビゲーションシステム１０の概略構成図を示す。
【００１２】
ナビゲーションシステム１０は、ナビゲーションシステムを制御するナビＥＣＵ（Ｅｌｅｃｔｒｉｃａｌ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）８により制御される。ナビＥＣＵ８は、プログラムを実行するＣＰＵ、プログラムを記憶した記憶装置（ハードディスクドライブ、ＲＯＭ）、データやプログラムを一時的に記憶するＲＡＭ、データを入力及び出力する入出力装置、ＮＶ（Ｎｏｎ Ｖｏｌａｔｉｌｅ）−ＲＡＭ等がバスを介して接続されたコンピュータとして構成される。
【００１３】
ナビＥＣＵ８にはＧＰＳ（Ｇｒｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波を受信するＧＰＳ受信装置１、車両の速度を検出する車速センサ２、車両の重心回りの回転角速度を検出するヨーレートセンサ３（又はジャイロセンサ）、操舵輪の舵角を検出する舵角センサ４、地図データを記憶した地図データベース（以下、地図ＤＢという）５、ナビゲーションシステム１０を操作するための入力装置６及び地図に現在位置を表示する液晶やＨＵＤ（Ｈｅａｄ Ｕｐ Ｄｉｓｐｌａｙ）等の表示装置７が接続されている。
【００１４】
地図ＤＢ５は、実際の道路網をノード(例えば、道路と道路が交差する点、交差点から所定間隔毎に区切った点、等)及びリンク(ノードとノードを接続する道路)に対応づけて、テーブル状のデータベースとして構成される。
【００１５】
ナビＥＣＵ８は、車室内に設けられた表示装置７に、検出した現在位置周辺の地図を地図ＤＢ５から抽出し、指定された縮尺に合わせて表示する。ナビＥＣＵ８は、必要に応じて地図に重畳して車両の現在位置を表示する。
【００１６】
また、押下式のキーボードやリモコン等の入力装置６から目的地が入力されると、検出した現在位置から目的地まで、例えばダイクストラ法など周知のルート検索方法でルート検索を行い、ルートを地図に重畳して表示すると共に、右左折する交差点の手前で運転者にルートを案内する。
【００１７】
カメラ１１は車両前方、より好ましくは車両前方の所定範囲を撮影するようにルームミラーの背面やフロントガラスの上方に取り付けられている。
【００１８】
カメラ１１は、例えばＣＣＤ（Charge Coupled Device）、ＣＭＯＳ（Complementary Metal Oxide Semiconductor）等の光電変換素子を有し、入射した光を光電変換素子により光電変換し、蓄積した電荷を電圧として読み出し増幅してＡ／Ｄ変換した後、所定の輝度階調（例えば、２５６階調）のデジタル画像に変換する。
【００１９】
カメラ１１は交差点などの道路に沿って設置された人工の地物（信号機、標識、横断歩道などのペイント、電柱等）と自車両との位置を検出するため、距離情報が得られるカメラであることが好ましい。このため、例えばカメラ１１は２台のカメラからなるステレオカメラ、移動体に搭載した1台のカメラの時系列画像からステレオ視を行うモーションステレオカメラである。また、LED（発光ダイオード）から所定の時間間隔で近赤外光を照射し、反射光を光電変換素子が受光するまでの時間を計測することで距離を求めることが可能なカメラであってもよい。
【００２０】
ナビＥＣＵ８のＣＰＵは、記憶装置に記憶されたプログラムを実行することで、本実施の形態で説明する測位を実現する。図２（ａ）は、測位装置９の機能ブロック図を示す。測位装置９は、電波航法測位により自車両の位置を測位するＧＰＳ測位手段８１、自律センサ（車速センサ２，舵角センサ４）を用いて自律航法により自車両の位置を測位するＩＮＳ測位手段８２、カメラ１１により撮影された画像データから信号機等の地物を検出する信号機検出手段８４，検出された信号機の位置を同定する信号機位置同定手段８３、信号機の位置に基づき自車両の位置を測位する地物基準測位手段８５、カルマンフィルタにより自車両の位置の最尤値を出力する位置推定手段８６及び信号機位置同定手段８３が同定した信号機など地物の位置情報を地物に対応づけて地図ＤＢ５に登録する地図データ登録手段８７を有する。以下、詳細に説明する。
【００２１】
本実施の形態の測位装置９は、ＧＰＳ衛星の捕捉が困難な状況やＧＰＳ測位の信頼性が低下した状況下で、自律航法による車両の位置を補正して精度よい測位を可能にする。
【００２２】
図２（ｂ）により概略を説明すれば、ＧＰＳ電波が遮断された場合、測位装置９は自律航法により測位された位置及び信号機など地物に対する視線ベクトルに基づき測位された位置を、カルマンフィルタによりカップリングして、位置Ｙを精度よく推定する。
【００２３】
まず、ＧＰＳ測位手段８１は、周知の方法でＧＰＳ衛星からの電波に基づき自車両の位置を測位する。ＧＰＳ測位手段８１は所定の軌道を周回する複数のＧＰＳ衛星のうち現在の車両の位置から所定の仰角に入るＧＰＳ衛星を４つ以上選択し、それらのＧＰＳ衛星から発信される電波を受信する。ＧＰＳ測位手段８１は電波の到達時間を計算し、到達時間と光速ｃから捕捉したＧＰＳ衛星までの距離を算出する。そして、３つのＧＰＳ衛星と自車両の距離が交差する１点を自車両の位置として測位する。
【００２４】
測位装置９は、ＧＰＳ電波を受信している間は所定の時間毎に自車両の位置を測位する。そして、ＧＰＳ電波が遮断されると、最後に測位した位置を初期位置、その時の走行方向を初期方向として、これらに走行距離及び方向を累積する自律航法による測位を開始する。
【００２５】
図３（ａ）は自律航法により検出される位置を示す図である。自車両は交差点の方向に向かって走行しており初期位置２３でＧＰＳ電波が遮断される。また、地図ＤＢ５から交差点のノード２２が抽出されているため、ノード２２の位置は既知となっている。
【００２６】
ＩＮＳ（Inertial Navigation Sensor)測位手段８２は車速センサ２から車速、舵角センサ４から舵角を検出して、初期位置２３及び初期方向に走行距離及び方向を累積して、自律航法による位置及び方向を推定する（以下、推定位置、推定方向という）。なお、自車両の走行方向を検出する自律センサは、ジャイロセンサやヨーレートセンサでもよい。
【００２７】
また、カルマンフィルタを適用するためには推定位置の誤差分散を必要とするが、車速センサ２及び舵角センサ４の誤差は速度やＧＰＳ電波遮断時間等に応じて既知である。したがって、これらに基づき累積した推定位置２４の誤差分散も既知である。図３（ａ）では推定位置２４の誤差分散を点線で囲まれた楕円で示した。なお、この時（時刻ｔ）、自車両は実際の位置２５に存在する。
【００２８】
自律航法により位置を検出している間、信号機検出手段８４はカメラ１１が撮影した画像データから信号機を検出する。検出した信号機の位置を利用して、自車両の位置を推定するためである。なお、信号機２１は車両から利用できる地物であればよく、信号機２１の他に標識や電信柱を検出してもよい。
【００２９】
信号機検出手段８４は、予め記憶してある信号機の標準パターンを用いてパターンマッチングにより信号機を検出する。信号機検出手段８４は、画像データの画素値（輝度）を水平方向及び垂直方向に走査し、勾配が所定以上のエッジ部を抽出する。そして隣接するエッジ部を接続して撮影対象物の輪郭を抽出し、これに標準パターンをパターンマッチングする。なお、信号機の場合その輪郭は長方形であるので、所定の縦横比の輪郭となるエッジ部のみにパターンマッチングを施してもよい。信号機検出手段８４は、輪郭に区切られる領域と標準パターンとを画素毎に輝度を比較し、所定以上に相関する場合に信号機が撮影されているとして信号機を検出する。
【００３０】
信号機が検出された場合、信号機検出手段８４は画像データから距離情報を抽出する。上記のように、距離情報は例えば２つの画像データの視差から抽出する。カメラ１１により撮影された１対のステレオ画像から同一の撮像対象物（信号機）が写っている部分を抽出し、１対のステレオ画像同士で信号機の同一点を対応づけ、対応づけられた点（対応点）のずれ量（視差）を求めることによって信号機までの距離を算出する。すなわち、１対の画像データを重ね合わせると、視差により信号機が左右横方向にずれる。一方の画像を１画素ずつシフトしながら最も重なり合う位置を、画素値の相関に基づいて求める。このときシフトした画素数をｎ、レンズの焦点距離をｆ、光軸間の距離をｍ、画素ピッチをｄとすると、撮像対象物までの距離Ｌは、『Ｌ＝（ｆ・ｍ）／（ｎ・ｄ）』という関係式が成立する。（ｎ・ｄ）が視差である。
【００３１】
また、信号機検出手段８４は、検出された信号機と自車両との視線ベクトルを算出する。視線ベクトルの方向θは、カメラ１１の取付方向（カメラ１１の正面方向）をゼロにして、光電変換素子における信号機の撮影位置と距離Ｌにより求められる。図４（ａ）は視線ベクトルの一例を示す図である。
【００３２】
ところで、地図ＤＢ５にはノードの座標、交差点の有無及び種類に加え、信号機の設置有無や信号機の設置座標（以下、信号機座標という）が記憶されている場合がある。信号機座標が記憶されている場合には、信号機の絶対位置が予め確定しているので検出された信号機の信号機座標を取得することができる。
【００３３】
しかしながら、１つの画像データに複数の信号機が撮影されている場合や地図ＤＢ５に信号機座標が記憶されていない場合には、信号機が検出された画像データから信号機の位置を同定する必要がある。
【００３４】
本実施形態の信号位置同定手段は８３は、ｉ）信号機座標、ｉｉ）最小２乗法、ｉｉｉ）最急勾配法を適用した最小２乗法、のいずれかにより信号機の位置を同定する。
【００３５】
図４（ｂ）は信号機の位置を最小２乗法を利用して同定するイメージを示す図である。信号機が検出される度に、距離Ｌ１、Ｌ２、…Ｌｎ、同様に視線ベクトルの方向θ１、θ２…θｎ、が得られるので、自車両からの距離と視線ベクトルの方向の組（L、θ）により点の集合（ａ、ｂ）が得られる。なお、信号機（灯）の高さ方向の位置はおおよそ一定であるので、（ａ、ｂ）は道路に平行な平面の座標である。
【００３６】
例えば、ａｉ≡ｋ０＋ｋ１Ｌｉ＋ｋ２θｉ ｂｉ≡ｍ０＋ｍ１Ｌｉ＋ｍ２θｉ と線型モデルを立てれば、２乗誤差ε２ｋ（ｋ０、ｋ１，ｋ２）、ε２ｍ（ｍ０、ｍ１，ｍ２）は次のようになる。但し、サンプリング数をＮとし、ｉは１〜Ｎまでの値を取る。
ε２ｋ（ｋ０、ｋ１，ｋ２）＝（１／Ｎ）Σ｛ａｉ−（ｋ０＋ｋ１Ｌｉ＋ｋ２θｉ）｝２
ε２ｍ（ｍ０、ｍ１，ｍ２）＝（１／Ｎ）Σ｛ｂｉ−（ｍ０＋ｍ１Ｌｉ＋ｍ２θｉ）｝２
ε２ｋをｋ０、ｋ１，ｋ２について偏微分し、ａε２ｋ／ａｋ０＝０、ａε２ｋ／ａｋ１＝０、ａε２ｋ／ａｋ２＝０、から（ｋ０、ｋ１，ｋ２）を求めることができる。ａε／ａｋの「a」は偏微分を表す。ｂｉについても同様に求められる。
【００３７】
なお、上記の線型モデルは一例であり、（ａ、ｂ）と（L、θ）の関係は非線形であってもよい。例えば、ａｉ≡ｆ（Ｌ）＋ｇ（θ）である。
【００３８】
図３（ｂ）は最小２乗法により同定される信号機２１の位置を示す図である。時刻ｔにおける推定位置２４に至るまでに検出される信号機までの距離Ｌ、視線ベクトルの方向θにより、信号機２１の位置が同定される。
【００３９】
ところで、最小２乗法により好適な演算結果を得るためには、サンプル数Ｎが４以上である必要がある。このため、信号位置同定手段８３は、サンプル数Ｎが４未満の場合、すなわち信号機が検出され視線ベクトルが算出された回数が４未満の場合、信号機の位置を同定しない。これにより、カルマンフィルタを適用することで自車両の測位位置の精度が低下することを防止できる。この場合、測位装置９は推定位置２４と推定方向を出力する。
【００４０】
また、このような最小２乗法の考え方は、「各時刻の自車位置と信号機の位置との距離の２乗誤差の総和」を評価関数として設定し、それを最小にする信号機の位置を求めるものである。そこで、最急勾配法を使用して評価関数の傾きに基づき評価関数が最小となる信号機の位置を求める方法について説明する。
【００４１】
図５（ａ）は評価関数と信号機の位置の関係を示す図である。算出された信号機の位置に応じて評価関数の値は変動し、評価関数に最小値が存在することが分かる。最急勾配法は、適当な初期値からはじめて、パラメータの値を微分値と逆の方向に少しずつ変化させて徐々に最適なパラメータに近づけて行く。
【００４２】
最小２乗法を用いた場合、評価関数は最小２乗誤差であるので各パラメータでの偏微分を計算する。例えば、パラメータｋ０、ｋ１，ｋ２についてはそれぞれ次式が更新式となる。
ｋ０（ｊ＋１）＝ｋ０（ｊ）＋２・（１／Ｎ）Σ｛（ａｉ−（ｋ０＋ｋ１Ｌｉ＋ｋ２θｉ））
ｋ１（ｊ＋１）＝ｋ１（ｊ）＋２・（１／Ｎ）Σ｛（ａｉ−（ｋ０＋ｋ１Ｌｉ＋ｋ２θｉ））Ｌｉ
ｋ２（ｊ＋１）＝ｋ２（ｊ）＋２・（１／Ｎ）Σ｛（ａｉ−（ｋ０＋ｋ１Ｌｉ＋ｋ２θｉ））θｉ
添字の「ｊ」は０から始まり、ｋ０（ｊ＋１）が最小となったら、そのパラメータが、評価関数が最小となるパラメータであるので計算を打ち切る。評価関数が最小となったか否かは微分値（傾き）がほぼゼロになったか否かで判定される。このようにして、パラメータｋ０、ｋ１，ｋ２がえられる。図５（ａ）では評価関数が最小となる信号機の位置をグローバルｍｉｎとして示した。
【００４３】
最急勾配法では、ｊ＝０の時のｋ０（０）等が各パラメータの初期値となるが、この初期値に最小値の精度が依存することが知られている。図５（ｂ）は、評価関数と信号機の位置の関係図の一例であるが、極小値が複数ある場合を示している。図５（ｂ）に示すように、グローバルｍｉｎは右側の極小値であるが、初期値によっては左側のローカルｍｉｎに陥る場合がある。また、サンプル数が少ない場合は値が発散しやすい。
【００４４】
しかしながら、信号機は交差点付近に存在すると推定してよい。そこで、最急勾配法の初期値に交差点のノード位置から求めた値を設定する。これは初期値をノード位置とすることと等価である。
【００４５】
このような処理により、サンプル数Ｎが少ない場合でもグローバルｍｉｎの信号機の位置を検出することができる。
以上のようにして、図３（ｂ）の信号機２１の位置が同定された。
【００４６】
図２（ａ）に戻り、地物基準測位手段８５は、ｉ）信号機座標、ｉｉ）最小２乗法、又は、ｉｉｉ）最急勾配法を適用した最小２乗法、のいずれかにより取得した信号機２１の位置に基づき、自車両の位置を測位する（以下、地物推定位置、地物推定方向という）。地物推定方向は、地物推定位置を数点蓄積することで検出される。
【００４７】
図３（ｃ）は、信号機の位置から距離Ｌ、視線ベクトルの方向θにより測位された自車両の地物推定位置２６を示す図である。地物基準測位手段８５は、信号機の位置を原点に、時刻ｔの距離Ｌ及び視線ベクトルの方向θから地物推定位置２６及び地物推定方向を算出する。
【００４８】
また、地物推定位置２６の誤差分散は、ｉ）信号機座標を用いた場合、信号機座標の場合は距離Ｌの誤差ΔＬや視線ベクトルθの誤差Δθから求められ、ｉｉ）及びｉｉｉ）では最小２乗法により信号機の位置を同定した場合にはｉ）に加えパラメータを算出する過程で得られる誤差により求められる。図３（ｃ）では地物推定位置２６の誤差分散を点線で囲まれた楕円で示した。
【００４９】
位置推定手段８６は、推定位置２４と地物推定位置２６とをカルマンフィルタによりカップリングして、確率的に最も高い自車両の位置及び方向を出力する。
【００５０】
図３（ｃ）には、推定位置２４と地物推定位置２６に基づき推定される最終推定位置２７を示した。図３（ｃ）では、それぞれの誤差分散を推定位置２４と地物推定位置２６に凸部を有する分散で示したが、分散は実際には３次元の広がりを有する。図２（ｂ）にはその分散及びカルマンフィルタによる最尤位置Ｙの推定のイメージを示した。
【００５１】
カルマンフィルタは、１つの系の状態がそれぞれ独立に推定される場合、状態の確率密度の分布に基づき最も確率の高い状態（分布の積が最大になる状態）を推定する。したがって、２つの測位情報をカルマンフィルタでカップリングすることで、存在する確率が最も高い最終推定位置２７を推定することができる。
【００５２】
カルマンフィルタでは、推定位置２４をＺベクトル、地物推定位置２６をＸベクトルとした場合、既知の観測方程式Ｈを用いてＺとＸの間には次の関係があるものとする。
Ｚ−ＨＸ＝０
Ｚの誤差分散をＲ、Ｘの誤差分散をＭとすると、最終推定位置２７に相当する最尤値Ｙの誤差分散は、解析的にＡ＝（Ｍ−１＋ＨｔＲ−１Ｈ）−１になる。また、カルマンフィルタでは、最尤値Ｙが次式により求められる。なお、添字のｉは自車両の位置を観測した番号、ｔは転置行列、−１は逆行列である。
Ｙ（ｉ）＝Ｘ（ｉ−１）＋Ｋ（ｉ）・｛Ｚ（ｉ）−Ｈ（ｉ）・Ｘ（ｉ−１）｝
Ｋ（ｉ）はカルマンゲイン行列であり、次のように表せる。
Ｋ（ｉ）＝Ａ（ｉ）・ＨｉｔＲｉ−１
ただし、Ａ（ｉ）＝（Ｍ−１＋ＨｔＲ−１Ｈ）−１
＝Ｍ（ｉ）−Ｍ（ｉ）・Ｈ（ｉ）ｔ｛Ｈ（ｉ）Ｍ（ｉ）Ｈ（ｉ）ｔ＋Ｒ（ｉ）｝−１Ｈ（ｉ）Ｍ（ｉ）
である。
【００５３】
推定位置２４の誤差分散、地物推定位置２６の誤差分散はすでに求めているので、位置推定手段８６はこれらの式から最終推定位置２７を出力する。なお、自車両の方向についても同様にして求めることができる。
【００５４】
図６は、測位装置９が自律航法及び信号機の位置に基づく測位を利用して自車両の位置を推定する手順を示すフローチャート図である。
【００５５】
測位装置９は例えばＧＰＳ測位手段８１の測位間隔毎にＧＰＳ電波が遮断されたか否かを判定する（Ｓ１）。ＧＰＳ電波が遮断されていなければ（Ｓ１のＮｏ）、ＧＰＳ電波を利用して自車両の位置を測位する（Ｓ２）。
【００５６】
ＧＰＳ電波が遮断された場合（Ｓ１のＹｅｓ）、ＧＰＳ測位手段８１は初期位置、初期方向をナビＥＣＵ８の記憶装置に記憶しておき、ＩＮＳ測位手段８２は車速及び舵角に基づき、初期位置２３及び初期方向に走行距離及び走行方向を累積して自律航法による推定位置２４及び推定方向を推定する（Ｓ３）。なお、ＩＮＳ測位手段８２は、車速センサ２及び舵角センサ４の誤差等に応じて、これらに基づき累積した推定位置２４、推定方向の分散を算出する。
【００５７】
また、信号機検出手段８４は、自律航法による測位に並行して信号機２１の検出を繰り返す（Ｓ４）。そして、信号機が検出された場合、信号機検出手段８４は１対の画像データから距離情報を抽出する。また、信号機検出手段８４は、検出された信号機と自車両との視線ベクトルを算出する（Ｓ５）。
【００５８】
ついで、信号機位置同定手段８３は過去Ｎ回分の信号機との距離、視線ベクトルを参照する（Ｓ６）。
【００５９】
上述したように、このＮが小さいと最小２乗法により同定した信号機の位置の精度が低下するので、信号機位置同定手段８３はＮが４以上か否かを判定する（Ｓ７）。４つ以上の距離及び視線ベクトルが取得されていない場合（Ｓ７のＮｏ）、測位装置９は自律航法による測位結果を出力する。
【００６０】
４つ以上の距離及び視線ベクトルが取得されている場合（Ｓ７のＹｅｓ）、信号機位置同定手段８３は、進行方向にある交差点のノードの位置情報を地図ＤＢ５から抽出する（Ｓ８）。
【００６１】
ついで、信号機位置同定手段８３は、最小２乗法により信号機の位置を算出する（Ｓ９）。最小２乗法を適用する場合、最急勾配法を利用して更にその初期値にノードの位置情報を使用する。これにより信号機の位置が同定された。
【００６２】
ついで、地物基準測位手段８５は、同定された信号機の位置を原点に、距離Ｌ及び視線ベクトルの方向θから地物推定位置２６及び地物推定方向を算出する（Ｓ１０）。
【００６３】
そして、位置推定手段８６は、推定位置２４及び地物推定位置２６をカルマンフィルタによりカップリングして、最終推定位置２７を出力する（Ｓ１１）。
【００６４】
図７は、自律航法により測位された推定位置２４を最終推定位置２７に修正するイメージを示す図である。図７では、最終推定位置２７が出力されるたびに自律航法による推定位置を補正する様子を示す。本実施の形態では、走行方向にある信号機の位置を基準に自車両の位置を測位し、自律航法により測位された位置を補正するので、特に走路方向の位置を精度よく補正することができる。
【００６５】
図８は、一般道路においてＧＰＳ電波が遮断された状況における測位装置９の測位結果を示す。図８（ａ）はカメラ１１に撮影された画像データを示す。図８（ａ）では画像データの上側に信号機２１が検出されている。
【００６６】
図８（ｂ）は実際の道路の平面写真に、自律航法による推定位置２４と最終推定位置２７とをプロットした図である。自車両は平面写真の下方から上方向に走行した。図８（ｂ）に示すように、走行するにつれて推定位置２４は実際の道路とのズレが徐々に大きくなるが、地物推定位置２６で補正することにより、道路とのズレを大幅に低減することができる。
【００６７】
〔信号機の位置のデータベース〕
ところで、本実施形態ではＧＰＳ電波が遮断された後、自律航法により測位された自車両の位置を補正する過程で信号機等のランドマークの位置を検出する。これは、上述のように、一般的なナビゲーションシステムの地図ＤＢ５には道路インフラ（信号機・標識等）の位置情報が記憶されていないためである。
【００６８】
運転者に適切なタイミングで交差点等における運転支援（車両制御、注意喚起等）を提供するためには、道路インフラの位置情報が不可欠といえる。しかし、全国の道路インフラを測位し、データベース化するには多大な労力とコスト必要である。また、道路インフラが新たに設置された場合、すぐには対応できないという問題もある。
【００６９】
そこで、測位装置９は、自車両の位置を補正する過程で検出した信号機等の位置、また、ＧＰＳ電波が遮断されていない状態においても信号機等の位置を検出し、地図ＤＢ５に登録することとすれば、道路インフラの位置情報のデータベースを作成できる。
【００７０】
図９は地図データから得られる道路網に信号機の位置を登録した地図の一例を示す。白丸が登録した信号機の位置を示す。
【００７１】
車載したカメラによる道路インフラの位置の検出は、短時間かつ低コストに実現でき、道路インフラが新たに設置されても車両が走行すればデータベースに反映できるため、優れた冗長性を兼ね備える。
【００７２】
以上のように、本実施形態の測位装置９は、信号機などの地物の位置を同定し地物の位置を基準に測位した自車両の位置と、自律航法により測位した位置をカルマンフィルタに適用することで、ＧＰＳ電波が遮断されても自律航法による測位結果を精度よく補正することができる。特に、進行方向に対する位置を精度よく補正することができる。また、同定した地物の位置を地図ＤＢに登録することで、道路インフラの位置情報が登録された地図ＤＢを、更新しながら利用することができる。
【図面の簡単な説明】
【００７３】
【図１】測位装置を適用したナビゲーションシステムの概略構成図である。
【図２】測位装置の機能ブロック図である。
【図３】信号機と自車両の位置との関係を示す図である。
【図４】視線ベクトルの一例を示す図である。
【図５】評価関数と信号機の位置の関係を示す図である。
【図６】測位装置が自律航法及び信号機の位置に基づく測位を利用して自車両の位置を推定する手順を示すフローチャート図である。
【図７】自律航法により測位された推定位置を最終推定位置に修正するイメージを示す図である。
【図８】一般道路においてＧＰＳ電波が遮断された状況における測位装置の測位結果を示す図ある。
【図９】地図データから得られる道路網に信号機の位置を登録した地図の一例を示す図である。
【符号の説明】
【００７４】
１ ＧＰＳ受信装置
２ 車速センサ
３ ヨーレートセンサ
４ 舵角センサ
５ 地図ＤＢ
６ 入力装置
７ 表示装置
８ ナビＥＣＵ
９ 測位装置
１０ ナビゲーションシステム
１１ カメラ
２１ 信号機
２２ 交差点のノード
２３ 初期位置
２４ 推定位置
２５ 実際の自車位置
２６ 地物推定位置
２７ 最終推定位置
【技術分野】
【０００１】
本発明は移動体の位置を検出する測位装置に関し、特に、自律航法により測位された移動体の位置を精度よく補正する測位装置に関する。
【背景技術】
【０００２】
ナビゲーションシステムでは、ＧＰＳ（Ｇｒｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波に基づき自車両の位置を測位して、車速センサ及びジャイロセンサ等を用いて走行距離及び走行方向を累積しながら車両の現在位置を精度よく推定する。
【０００３】
しかしながら、ＧＰＳ衛星からの電波を受信できない状態では、自律航法による測位に含まれる誤差が時間と共に増幅されるため徐々に位置の精度が低下する。
【０００４】
このため、自律航法により測位した自車両の位置を補正する種々の方法が提案されている。例えば、マップマッチングでは、ナビゲーションシステムの地図データを利用して自律航法により測位した位置を補正する（例えば、特許文献１参照。）。特許文献１では、自律航法により測位された位置・方向に最も整合する位置及び方位を有する道路を地図データから選択して、該道路に対応づけて位置・方向を補正する方法が提案されている。
【０００５】
しかしながら、一般に市販されているナビゲーションシステムの地図データはそれほど精度が高いものではなく、また、道路網を交差点（ノード）を結ぶ直線状のリンクにより表現するため実際の道路の形状と一致しない場合がある。このため、マップマッチングでは自車両の位置の修正を十分に修正できないことがある。
【０００６】
また、カメラにより撮影された画像から信号機や横断歩道などの交差点シンボルが検出された場合に、自車両と交差点までの距離を算出して、算出された距離に応じて自車両の位置を補正するナビゲーション装置が提案されている（例えば、特許文献２参照。）。特許文献２では、交差点との距離を算出することで、長い直線道路を走行している場合であっても進行方向に対する自車両の位置を修正することができるとしている。
【特許文献１】特開２００２−２１３９７９号公報
【特許文献２】特開平９−２４３３８９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００７】
しかしながら、特許文献２に記載されたように、撮影された画像データから交差点との距離を算出しそのまま自車両の位置を補正すると、画像データから算出される距離にも誤差が含まれるため、補正後の位置が正しいものとは限らないという問題がある。例えば、自車両がピッチ運動する場合、算出される距離に大きな誤差が含まれることになる。
【０００８】
本発明は、上記課題に鑑み、自律航法による測位結果を修正してより精度よく移動体の位置を測位する測位装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記課題を解決するため、本発明は、地図データを記憶した地図データ記憶手段（例えば、地図ＤＢ５）と、移動体の挙動情報を検出する自律センサ（例えば、車速センサ２、ヨーレートセンサ３）と、ＧＰＳ等の電波航法測位手段による測位結果に自律センサによる検出情報を累積して移動体の推定位置を検出する慣性測位手段（例えば、ＩＮＳ測位手段８２）と、道路周辺の地物を検出する地物検出手段（例えば、信号機検出手段８４）と、地物の位置を同定する地物位置同定手段（例えば、信号機位置同定手段８３）と、地物の位置を基準に移動体の地物推定位置を推定する地物基準測位手段と、推定位置及び地物推定位置をカルマンフィルタに適用して移動体の位置を推定する位置推定手段と、を有することを特徴とする測位装置を提供する。
【発明の効果】
【００１０】
自律航法による測位結果を修正してより精度よく移動体の位置を測位する測位装置を提供することができる。
【発明を実施するための最良の形態】
【００１１】
以下、本発明を実施するための最良の形態について、図面を参照しながら説明する。
図１は、本実施の形態の測位装置９を適用したナビゲーションシステム１０の概略構成図を示す。
【００１２】
ナビゲーションシステム１０は、ナビゲーションシステムを制御するナビＥＣＵ（Ｅｌｅｃｔｒｉｃａｌ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）８により制御される。ナビＥＣＵ８は、プログラムを実行するＣＰＵ、プログラムを記憶した記憶装置（ハードディスクドライブ、ＲＯＭ）、データやプログラムを一時的に記憶するＲＡＭ、データを入力及び出力する入出力装置、ＮＶ（Ｎｏｎ Ｖｏｌａｔｉｌｅ）−ＲＡＭ等がバスを介して接続されたコンピュータとして構成される。
【００１３】
ナビＥＣＵ８にはＧＰＳ（Ｇｒｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星からの電波を受信するＧＰＳ受信装置１、車両の速度を検出する車速センサ２、車両の重心回りの回転角速度を検出するヨーレートセンサ３（又はジャイロセンサ）、操舵輪の舵角を検出する舵角センサ４、地図データを記憶した地図データベース（以下、地図ＤＢという）５、ナビゲーションシステム１０を操作するための入力装置６及び地図に現在位置を表示する液晶やＨＵＤ（Ｈｅａｄ Ｕｐ Ｄｉｓｐｌａｙ）等の表示装置７が接続されている。
【００１４】
地図ＤＢ５は、実際の道路網をノード(例えば、道路と道路が交差する点、交差点から所定間隔毎に区切った点、等)及びリンク(ノードとノードを接続する道路)に対応づけて、テーブル状のデータベースとして構成される。
【００１５】
ナビＥＣＵ８は、車室内に設けられた表示装置７に、検出した現在位置周辺の地図を地図ＤＢ５から抽出し、指定された縮尺に合わせて表示する。ナビＥＣＵ８は、必要に応じて地図に重畳して車両の現在位置を表示する。
【００１６】
また、押下式のキーボードやリモコン等の入力装置６から目的地が入力されると、検出した現在位置から目的地まで、例えばダイクストラ法など周知のルート検索方法でルート検索を行い、ルートを地図に重畳して表示すると共に、右左折する交差点の手前で運転者にルートを案内する。
【００１７】
カメラ１１は車両前方、より好ましくは車両前方の所定範囲を撮影するようにルームミラーの背面やフロントガラスの上方に取り付けられている。
【００１８】
カメラ１１は、例えばＣＣＤ（Charge Coupled Device）、ＣＭＯＳ（Complementary Metal Oxide Semiconductor）等の光電変換素子を有し、入射した光を光電変換素子により光電変換し、蓄積した電荷を電圧として読み出し増幅してＡ／Ｄ変換した後、所定の輝度階調（例えば、２５６階調）のデジタル画像に変換する。
【００１９】
カメラ１１は交差点などの道路に沿って設置された人工の地物（信号機、標識、横断歩道などのペイント、電柱等）と自車両との位置を検出するため、距離情報が得られるカメラであることが好ましい。このため、例えばカメラ１１は２台のカメラからなるステレオカメラ、移動体に搭載した1台のカメラの時系列画像からステレオ視を行うモーションステレオカメラである。また、LED（発光ダイオード）から所定の時間間隔で近赤外光を照射し、反射光を光電変換素子が受光するまでの時間を計測することで距離を求めることが可能なカメラであってもよい。
【００２０】
ナビＥＣＵ８のＣＰＵは、記憶装置に記憶されたプログラムを実行することで、本実施の形態で説明する測位を実現する。図２（ａ）は、測位装置９の機能ブロック図を示す。測位装置９は、電波航法測位により自車両の位置を測位するＧＰＳ測位手段８１、自律センサ（車速センサ２，舵角センサ４）を用いて自律航法により自車両の位置を測位するＩＮＳ測位手段８２、カメラ１１により撮影された画像データから信号機等の地物を検出する信号機検出手段８４，検出された信号機の位置を同定する信号機位置同定手段８３、信号機の位置に基づき自車両の位置を測位する地物基準測位手段８５、カルマンフィルタにより自車両の位置の最尤値を出力する位置推定手段８６及び信号機位置同定手段８３が同定した信号機など地物の位置情報を地物に対応づけて地図ＤＢ５に登録する地図データ登録手段８７を有する。以下、詳細に説明する。
【００２１】
本実施の形態の測位装置９は、ＧＰＳ衛星の捕捉が困難な状況やＧＰＳ測位の信頼性が低下した状況下で、自律航法による車両の位置を補正して精度よい測位を可能にする。
【００２２】
図２（ｂ）により概略を説明すれば、ＧＰＳ電波が遮断された場合、測位装置９は自律航法により測位された位置及び信号機など地物に対する視線ベクトルに基づき測位された位置を、カルマンフィルタによりカップリングして、位置Ｙを精度よく推定する。
【００２３】
まず、ＧＰＳ測位手段８１は、周知の方法でＧＰＳ衛星からの電波に基づき自車両の位置を測位する。ＧＰＳ測位手段８１は所定の軌道を周回する複数のＧＰＳ衛星のうち現在の車両の位置から所定の仰角に入るＧＰＳ衛星を４つ以上選択し、それらのＧＰＳ衛星から発信される電波を受信する。ＧＰＳ測位手段８１は電波の到達時間を計算し、到達時間と光速ｃから捕捉したＧＰＳ衛星までの距離を算出する。そして、３つのＧＰＳ衛星と自車両の距離が交差する１点を自車両の位置として測位する。
【００２４】
測位装置９は、ＧＰＳ電波を受信している間は所定の時間毎に自車両の位置を測位する。そして、ＧＰＳ電波が遮断されると、最後に測位した位置を初期位置、その時の走行方向を初期方向として、これらに走行距離及び方向を累積する自律航法による測位を開始する。
【００２５】
図３（ａ）は自律航法により検出される位置を示す図である。自車両は交差点の方向に向かって走行しており初期位置２３でＧＰＳ電波が遮断される。また、地図ＤＢ５から交差点のノード２２が抽出されているため、ノード２２の位置は既知となっている。
【００２６】
ＩＮＳ（Inertial Navigation Sensor)測位手段８２は車速センサ２から車速、舵角センサ４から舵角を検出して、初期位置２３及び初期方向に走行距離及び方向を累積して、自律航法による位置及び方向を推定する（以下、推定位置、推定方向という）。なお、自車両の走行方向を検出する自律センサは、ジャイロセンサやヨーレートセンサでもよい。
【００２７】
また、カルマンフィルタを適用するためには推定位置の誤差分散を必要とするが、車速センサ２及び舵角センサ４の誤差は速度やＧＰＳ電波遮断時間等に応じて既知である。したがって、これらに基づき累積した推定位置２４の誤差分散も既知である。図３（ａ）では推定位置２４の誤差分散を点線で囲まれた楕円で示した。なお、この時（時刻ｔ）、自車両は実際の位置２５に存在する。
【００２８】
自律航法により位置を検出している間、信号機検出手段８４はカメラ１１が撮影した画像データから信号機を検出する。検出した信号機の位置を利用して、自車両の位置を推定するためである。なお、信号機２１は車両から利用できる地物であればよく、信号機２１の他に標識や電信柱を検出してもよい。
【００２９】
信号機検出手段８４は、予め記憶してある信号機の標準パターンを用いてパターンマッチングにより信号機を検出する。信号機検出手段８４は、画像データの画素値（輝度）を水平方向及び垂直方向に走査し、勾配が所定以上のエッジ部を抽出する。そして隣接するエッジ部を接続して撮影対象物の輪郭を抽出し、これに標準パターンをパターンマッチングする。なお、信号機の場合その輪郭は長方形であるので、所定の縦横比の輪郭となるエッジ部のみにパターンマッチングを施してもよい。信号機検出手段８４は、輪郭に区切られる領域と標準パターンとを画素毎に輝度を比較し、所定以上に相関する場合に信号機が撮影されているとして信号機を検出する。
【００３０】
信号機が検出された場合、信号機検出手段８４は画像データから距離情報を抽出する。上記のように、距離情報は例えば２つの画像データの視差から抽出する。カメラ１１により撮影された１対のステレオ画像から同一の撮像対象物（信号機）が写っている部分を抽出し、１対のステレオ画像同士で信号機の同一点を対応づけ、対応づけられた点（対応点）のずれ量（視差）を求めることによって信号機までの距離を算出する。すなわち、１対の画像データを重ね合わせると、視差により信号機が左右横方向にずれる。一方の画像を１画素ずつシフトしながら最も重なり合う位置を、画素値の相関に基づいて求める。このときシフトした画素数をｎ、レンズの焦点距離をｆ、光軸間の距離をｍ、画素ピッチをｄとすると、撮像対象物までの距離Ｌは、『Ｌ＝（ｆ・ｍ）／（ｎ・ｄ）』という関係式が成立する。（ｎ・ｄ）が視差である。
【００３１】
また、信号機検出手段８４は、検出された信号機と自車両との視線ベクトルを算出する。視線ベクトルの方向θは、カメラ１１の取付方向（カメラ１１の正面方向）をゼロにして、光電変換素子における信号機の撮影位置と距離Ｌにより求められる。図４（ａ）は視線ベクトルの一例を示す図である。
【００３２】
ところで、地図ＤＢ５にはノードの座標、交差点の有無及び種類に加え、信号機の設置有無や信号機の設置座標（以下、信号機座標という）が記憶されている場合がある。信号機座標が記憶されている場合には、信号機の絶対位置が予め確定しているので検出された信号機の信号機座標を取得することができる。
【００３３】
しかしながら、１つの画像データに複数の信号機が撮影されている場合や地図ＤＢ５に信号機座標が記憶されていない場合には、信号機が検出された画像データから信号機の位置を同定する必要がある。
【００３４】
本実施形態の信号位置同定手段は８３は、ｉ）信号機座標、ｉｉ）最小２乗法、ｉｉｉ）最急勾配法を適用した最小２乗法、のいずれかにより信号機の位置を同定する。
【００３５】
図４（ｂ）は信号機の位置を最小２乗法を利用して同定するイメージを示す図である。信号機が検出される度に、距離Ｌ１、Ｌ２、…Ｌｎ、同様に視線ベクトルの方向θ１、θ２…θｎ、が得られるので、自車両からの距離と視線ベクトルの方向の組（L、θ）により点の集合（ａ、ｂ）が得られる。なお、信号機（灯）の高さ方向の位置はおおよそ一定であるので、（ａ、ｂ）は道路に平行な平面の座標である。
【００３６】
例えば、ａｉ≡ｋ０＋ｋ１Ｌｉ＋ｋ２θｉ ｂｉ≡ｍ０＋ｍ１Ｌｉ＋ｍ２θｉ と線型モデルを立てれば、２乗誤差ε２ｋ（ｋ０、ｋ１，ｋ２）、ε２ｍ（ｍ０、ｍ１，ｍ２）は次のようになる。但し、サンプリング数をＮとし、ｉは１〜Ｎまでの値を取る。
ε２ｋ（ｋ０、ｋ１，ｋ２）＝（１／Ｎ）Σ｛ａｉ−（ｋ０＋ｋ１Ｌｉ＋ｋ２θｉ）｝２
ε２ｍ（ｍ０、ｍ１，ｍ２）＝（１／Ｎ）Σ｛ｂｉ−（ｍ０＋ｍ１Ｌｉ＋ｍ２θｉ）｝２
ε２ｋをｋ０、ｋ１，ｋ２について偏微分し、ａε２ｋ／ａｋ０＝０、ａε２ｋ／ａｋ１＝０、ａε２ｋ／ａｋ２＝０、から（ｋ０、ｋ１，ｋ２）を求めることができる。ａε／ａｋの「a」は偏微分を表す。ｂｉについても同様に求められる。
【００３７】
なお、上記の線型モデルは一例であり、（ａ、ｂ）と（L、θ）の関係は非線形であってもよい。例えば、ａｉ≡ｆ（Ｌ）＋ｇ（θ）である。
【００３８】
図３（ｂ）は最小２乗法により同定される信号機２１の位置を示す図である。時刻ｔにおける推定位置２４に至るまでに検出される信号機までの距離Ｌ、視線ベクトルの方向θにより、信号機２１の位置が同定される。
【００３９】
ところで、最小２乗法により好適な演算結果を得るためには、サンプル数Ｎが４以上である必要がある。このため、信号位置同定手段８３は、サンプル数Ｎが４未満の場合、すなわち信号機が検出され視線ベクトルが算出された回数が４未満の場合、信号機の位置を同定しない。これにより、カルマンフィルタを適用することで自車両の測位位置の精度が低下することを防止できる。この場合、測位装置９は推定位置２４と推定方向を出力する。
【００４０】
また、このような最小２乗法の考え方は、「各時刻の自車位置と信号機の位置との距離の２乗誤差の総和」を評価関数として設定し、それを最小にする信号機の位置を求めるものである。そこで、最急勾配法を使用して評価関数の傾きに基づき評価関数が最小となる信号機の位置を求める方法について説明する。
【００４１】
図５（ａ）は評価関数と信号機の位置の関係を示す図である。算出された信号機の位置に応じて評価関数の値は変動し、評価関数に最小値が存在することが分かる。最急勾配法は、適当な初期値からはじめて、パラメータの値を微分値と逆の方向に少しずつ変化させて徐々に最適なパラメータに近づけて行く。
【００４２】
最小２乗法を用いた場合、評価関数は最小２乗誤差であるので各パラメータでの偏微分を計算する。例えば、パラメータｋ０、ｋ１，ｋ２についてはそれぞれ次式が更新式となる。
ｋ０（ｊ＋１）＝ｋ０（ｊ）＋２・（１／Ｎ）Σ｛（ａｉ−（ｋ０＋ｋ１Ｌｉ＋ｋ２θｉ））
ｋ１（ｊ＋１）＝ｋ１（ｊ）＋２・（１／Ｎ）Σ｛（ａｉ−（ｋ０＋ｋ１Ｌｉ＋ｋ２θｉ））Ｌｉ
ｋ２（ｊ＋１）＝ｋ２（ｊ）＋２・（１／Ｎ）Σ｛（ａｉ−（ｋ０＋ｋ１Ｌｉ＋ｋ２θｉ））θｉ
添字の「ｊ」は０から始まり、ｋ０（ｊ＋１）が最小となったら、そのパラメータが、評価関数が最小となるパラメータであるので計算を打ち切る。評価関数が最小となったか否かは微分値（傾き）がほぼゼロになったか否かで判定される。このようにして、パラメータｋ０、ｋ１，ｋ２がえられる。図５（ａ）では評価関数が最小となる信号機の位置をグローバルｍｉｎとして示した。
【００４３】
最急勾配法では、ｊ＝０の時のｋ０（０）等が各パラメータの初期値となるが、この初期値に最小値の精度が依存することが知られている。図５（ｂ）は、評価関数と信号機の位置の関係図の一例であるが、極小値が複数ある場合を示している。図５（ｂ）に示すように、グローバルｍｉｎは右側の極小値であるが、初期値によっては左側のローカルｍｉｎに陥る場合がある。また、サンプル数が少ない場合は値が発散しやすい。
【００４４】
しかしながら、信号機は交差点付近に存在すると推定してよい。そこで、最急勾配法の初期値に交差点のノード位置から求めた値を設定する。これは初期値をノード位置とすることと等価である。
【００４５】
このような処理により、サンプル数Ｎが少ない場合でもグローバルｍｉｎの信号機の位置を検出することができる。
以上のようにして、図３（ｂ）の信号機２１の位置が同定された。
【００４６】
図２（ａ）に戻り、地物基準測位手段８５は、ｉ）信号機座標、ｉｉ）最小２乗法、又は、ｉｉｉ）最急勾配法を適用した最小２乗法、のいずれかにより取得した信号機２１の位置に基づき、自車両の位置を測位する（以下、地物推定位置、地物推定方向という）。地物推定方向は、地物推定位置を数点蓄積することで検出される。
【００４７】
図３（ｃ）は、信号機の位置から距離Ｌ、視線ベクトルの方向θにより測位された自車両の地物推定位置２６を示す図である。地物基準測位手段８５は、信号機の位置を原点に、時刻ｔの距離Ｌ及び視線ベクトルの方向θから地物推定位置２６及び地物推定方向を算出する。
【００４８】
また、地物推定位置２６の誤差分散は、ｉ）信号機座標を用いた場合、信号機座標の場合は距離Ｌの誤差ΔＬや視線ベクトルθの誤差Δθから求められ、ｉｉ）及びｉｉｉ）では最小２乗法により信号機の位置を同定した場合にはｉ）に加えパラメータを算出する過程で得られる誤差により求められる。図３（ｃ）では地物推定位置２６の誤差分散を点線で囲まれた楕円で示した。
【００４９】
位置推定手段８６は、推定位置２４と地物推定位置２６とをカルマンフィルタによりカップリングして、確率的に最も高い自車両の位置及び方向を出力する。
【００５０】
図３（ｃ）には、推定位置２４と地物推定位置２６に基づき推定される最終推定位置２７を示した。図３（ｃ）では、それぞれの誤差分散を推定位置２４と地物推定位置２６に凸部を有する分散で示したが、分散は実際には３次元の広がりを有する。図２（ｂ）にはその分散及びカルマンフィルタによる最尤位置Ｙの推定のイメージを示した。
【００５１】
カルマンフィルタは、１つの系の状態がそれぞれ独立に推定される場合、状態の確率密度の分布に基づき最も確率の高い状態（分布の積が最大になる状態）を推定する。したがって、２つの測位情報をカルマンフィルタでカップリングすることで、存在する確率が最も高い最終推定位置２７を推定することができる。
【００５２】
カルマンフィルタでは、推定位置２４をＺベクトル、地物推定位置２６をＸベクトルとした場合、既知の観測方程式Ｈを用いてＺとＸの間には次の関係があるものとする。
Ｚ−ＨＸ＝０
Ｚの誤差分散をＲ、Ｘの誤差分散をＭとすると、最終推定位置２７に相当する最尤値Ｙの誤差分散は、解析的にＡ＝（Ｍ−１＋ＨｔＲ−１Ｈ）−１になる。また、カルマンフィルタでは、最尤値Ｙが次式により求められる。なお、添字のｉは自車両の位置を観測した番号、ｔは転置行列、−１は逆行列である。
Ｙ（ｉ）＝Ｘ（ｉ−１）＋Ｋ（ｉ）・｛Ｚ（ｉ）−Ｈ（ｉ）・Ｘ（ｉ−１）｝
Ｋ（ｉ）はカルマンゲイン行列であり、次のように表せる。
Ｋ（ｉ）＝Ａ（ｉ）・ＨｉｔＲｉ−１
ただし、Ａ（ｉ）＝（Ｍ−１＋ＨｔＲ−１Ｈ）−１
＝Ｍ（ｉ）−Ｍ（ｉ）・Ｈ（ｉ）ｔ｛Ｈ（ｉ）Ｍ（ｉ）Ｈ（ｉ）ｔ＋Ｒ（ｉ）｝−１Ｈ（ｉ）Ｍ（ｉ）
である。
【００５３】
推定位置２４の誤差分散、地物推定位置２６の誤差分散はすでに求めているので、位置推定手段８６はこれらの式から最終推定位置２７を出力する。なお、自車両の方向についても同様にして求めることができる。
【００５４】
図６は、測位装置９が自律航法及び信号機の位置に基づく測位を利用して自車両の位置を推定する手順を示すフローチャート図である。
【００５５】
測位装置９は例えばＧＰＳ測位手段８１の測位間隔毎にＧＰＳ電波が遮断されたか否かを判定する（Ｓ１）。ＧＰＳ電波が遮断されていなければ（Ｓ１のＮｏ）、ＧＰＳ電波を利用して自車両の位置を測位する（Ｓ２）。
【００５６】
ＧＰＳ電波が遮断された場合（Ｓ１のＹｅｓ）、ＧＰＳ測位手段８１は初期位置、初期方向をナビＥＣＵ８の記憶装置に記憶しておき、ＩＮＳ測位手段８２は車速及び舵角に基づき、初期位置２３及び初期方向に走行距離及び走行方向を累積して自律航法による推定位置２４及び推定方向を推定する（Ｓ３）。なお、ＩＮＳ測位手段８２は、車速センサ２及び舵角センサ４の誤差等に応じて、これらに基づき累積した推定位置２４、推定方向の分散を算出する。
【００５７】
また、信号機検出手段８４は、自律航法による測位に並行して信号機２１の検出を繰り返す（Ｓ４）。そして、信号機が検出された場合、信号機検出手段８４は１対の画像データから距離情報を抽出する。また、信号機検出手段８４は、検出された信号機と自車両との視線ベクトルを算出する（Ｓ５）。
【００５８】
ついで、信号機位置同定手段８３は過去Ｎ回分の信号機との距離、視線ベクトルを参照する（Ｓ６）。
【００５９】
上述したように、このＮが小さいと最小２乗法により同定した信号機の位置の精度が低下するので、信号機位置同定手段８３はＮが４以上か否かを判定する（Ｓ７）。４つ以上の距離及び視線ベクトルが取得されていない場合（Ｓ７のＮｏ）、測位装置９は自律航法による測位結果を出力する。
【００６０】
４つ以上の距離及び視線ベクトルが取得されている場合（Ｓ７のＹｅｓ）、信号機位置同定手段８３は、進行方向にある交差点のノードの位置情報を地図ＤＢ５から抽出する（Ｓ８）。
【００６１】
ついで、信号機位置同定手段８３は、最小２乗法により信号機の位置を算出する（Ｓ９）。最小２乗法を適用する場合、最急勾配法を利用して更にその初期値にノードの位置情報を使用する。これにより信号機の位置が同定された。
【００６２】
ついで、地物基準測位手段８５は、同定された信号機の位置を原点に、距離Ｌ及び視線ベクトルの方向θから地物推定位置２６及び地物推定方向を算出する（Ｓ１０）。
【００６３】
そして、位置推定手段８６は、推定位置２４及び地物推定位置２６をカルマンフィルタによりカップリングして、最終推定位置２７を出力する（Ｓ１１）。
【００６４】
図７は、自律航法により測位された推定位置２４を最終推定位置２７に修正するイメージを示す図である。図７では、最終推定位置２７が出力されるたびに自律航法による推定位置を補正する様子を示す。本実施の形態では、走行方向にある信号機の位置を基準に自車両の位置を測位し、自律航法により測位された位置を補正するので、特に走路方向の位置を精度よく補正することができる。
【００６５】
図８は、一般道路においてＧＰＳ電波が遮断された状況における測位装置９の測位結果を示す。図８（ａ）はカメラ１１に撮影された画像データを示す。図８（ａ）では画像データの上側に信号機２１が検出されている。
【００６６】
図８（ｂ）は実際の道路の平面写真に、自律航法による推定位置２４と最終推定位置２７とをプロットした図である。自車両は平面写真の下方から上方向に走行した。図８（ｂ）に示すように、走行するにつれて推定位置２４は実際の道路とのズレが徐々に大きくなるが、地物推定位置２６で補正することにより、道路とのズレを大幅に低減することができる。
【００６７】
〔信号機の位置のデータベース〕
ところで、本実施形態ではＧＰＳ電波が遮断された後、自律航法により測位された自車両の位置を補正する過程で信号機等のランドマークの位置を検出する。これは、上述のように、一般的なナビゲーションシステムの地図ＤＢ５には道路インフラ（信号機・標識等）の位置情報が記憶されていないためである。
【００６８】
運転者に適切なタイミングで交差点等における運転支援（車両制御、注意喚起等）を提供するためには、道路インフラの位置情報が不可欠といえる。しかし、全国の道路インフラを測位し、データベース化するには多大な労力とコスト必要である。また、道路インフラが新たに設置された場合、すぐには対応できないという問題もある。
【００６９】
そこで、測位装置９は、自車両の位置を補正する過程で検出した信号機等の位置、また、ＧＰＳ電波が遮断されていない状態においても信号機等の位置を検出し、地図ＤＢ５に登録することとすれば、道路インフラの位置情報のデータベースを作成できる。
【００７０】
図９は地図データから得られる道路網に信号機の位置を登録した地図の一例を示す。白丸が登録した信号機の位置を示す。
【００７１】
車載したカメラによる道路インフラの位置の検出は、短時間かつ低コストに実現でき、道路インフラが新たに設置されても車両が走行すればデータベースに反映できるため、優れた冗長性を兼ね備える。
【００７２】
以上のように、本実施形態の測位装置９は、信号機などの地物の位置を同定し地物の位置を基準に測位した自車両の位置と、自律航法により測位した位置をカルマンフィルタに適用することで、ＧＰＳ電波が遮断されても自律航法による測位結果を精度よく補正することができる。特に、進行方向に対する位置を精度よく補正することができる。また、同定した地物の位置を地図ＤＢに登録することで、道路インフラの位置情報が登録された地図ＤＢを、更新しながら利用することができる。
【図面の簡単な説明】
【００７３】
【図１】測位装置を適用したナビゲーションシステムの概略構成図である。
【図２】測位装置の機能ブロック図である。
【図３】信号機と自車両の位置との関係を示す図である。
【図４】視線ベクトルの一例を示す図である。
【図５】評価関数と信号機の位置の関係を示す図である。
【図６】測位装置が自律航法及び信号機の位置に基づく測位を利用して自車両の位置を推定する手順を示すフローチャート図である。
【図７】自律航法により測位された推定位置を最終推定位置に修正するイメージを示す図である。
【図８】一般道路においてＧＰＳ電波が遮断された状況における測位装置の測位結果を示す図ある。
【図９】地図データから得られる道路網に信号機の位置を登録した地図の一例を示す図である。
【符号の説明】
【００７４】
１ ＧＰＳ受信装置
２ 車速センサ
３ ヨーレートセンサ
４ 舵角センサ
５ 地図ＤＢ
６ 入力装置
７ 表示装置
８ ナビＥＣＵ
９ 測位装置
１０ ナビゲーションシステム
１１ カメラ
２１ 信号機
２２ 交差点のノード
２３ 初期位置
２４ 推定位置
２５ 実際の自車位置
２６ 地物推定位置
２７ 最終推定位置
【特許請求の範囲】
【請求項１】
地図データを記憶した地図データ記憶手段と、
移動体の挙動情報を検出する自律センサと、
ＧＰＳ等の電波航法測位手段による測位結果に前記自律センサによる検出情報を累積して前記移動体の推定位置を検出する慣性測位手段と、
道路周辺の地物を検出する地物検出手段と、
前記地物検出手段により検出された前記地物の位置を同定する地物位置同定手段と、
前記地物の位置を基準に前記移動体の地物推定位置を推定する地物基準測位手段と、
前記推定位置及び前記地物推定位置をカルマンフィルタに適用して前記移動体の前記位置を推定する位置推定手段と、
を有することを特徴とする測位装置。
【請求項２】
前記地物検出手段は、
撮影手段により撮影された画像データから前記地物を検出し、該地物までの視線ベクトルを取得する視線ベクトル検出手段を有し、
前記地物位置同定手段は、
複数の前記視線ベクトルの長さ及び方向を使用して最小２乗法により前記地物の地物位置情報を算出する地物位置算出手段を有する、
を有することを特徴とする請求項１記載の測位装置。
【請求項３】
前記地物位置算出手段は、
前記地図データ記憶手段から、前記地物から所定距離内にある交差点の交差点位置情報を抽出し、最小２乗法により前記地物位置情報を算出する際の初期値に前記交差点位置情報を設定する、
ことを特徴とする請求項２記載の測位装置。
【請求項４】
前記地物位置同定手段が取得した前記地物の前記地物位置情報を、該地物及びリンク又はノードと対応づけて前記地図データ記憶手段に登録する地図データ登録手段、
を有することを特徴とする請求項１ないし３いずれか記載の測位装置。
【請求項５】
前記視線ベクトル検出手段により検出された前記視線ベクトルが所定数以下の場合、前記地物検出手段は、最小２乗法による前記地物位置情報を算出せず、前記位置推定手段は前記推定位置を出力する、
ことを特徴とする請求項１記載の測位装置。
【請求項１】
地図データを記憶した地図データ記憶手段と、
移動体の挙動情報を検出する自律センサと、
ＧＰＳ等の電波航法測位手段による測位結果に前記自律センサによる検出情報を累積して前記移動体の推定位置を検出する慣性測位手段と、
道路周辺の地物を検出する地物検出手段と、
前記地物検出手段により検出された前記地物の位置を同定する地物位置同定手段と、
前記地物の位置を基準に前記移動体の地物推定位置を推定する地物基準測位手段と、
前記推定位置及び前記地物推定位置をカルマンフィルタに適用して前記移動体の前記位置を推定する位置推定手段と、
を有することを特徴とする測位装置。
【請求項２】
前記地物検出手段は、
撮影手段により撮影された画像データから前記地物を検出し、該地物までの視線ベクトルを取得する視線ベクトル検出手段を有し、
前記地物位置同定手段は、
複数の前記視線ベクトルの長さ及び方向を使用して最小２乗法により前記地物の地物位置情報を算出する地物位置算出手段を有する、
を有することを特徴とする請求項１記載の測位装置。
【請求項３】
前記地物位置算出手段は、
前記地図データ記憶手段から、前記地物から所定距離内にある交差点の交差点位置情報を抽出し、最小２乗法により前記地物位置情報を算出する際の初期値に前記交差点位置情報を設定する、
ことを特徴とする請求項２記載の測位装置。
【請求項４】
前記地物位置同定手段が取得した前記地物の前記地物位置情報を、該地物及びリンク又はノードと対応づけて前記地図データ記憶手段に登録する地図データ登録手段、
を有することを特徴とする請求項１ないし３いずれか記載の測位装置。
【請求項５】
前記視線ベクトル検出手段により検出された前記視線ベクトルが所定数以下の場合、前記地物検出手段は、最小２乗法による前記地物位置情報を算出せず、前記位置推定手段は前記推定位置を出力する、
ことを特徴とする請求項１記載の測位装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２００８−２９０６（Ｐ２００８−２９０６Ａ）
【公開日】平成２０年１月１０日（２００８．１．１０）
【国際特許分類】
【出願番号】特願２００６−１７１７５５（Ｐ２００６−１７１７５５）
【出願日】平成１８年６月２１日（２００６．６．２１）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２０年１月１０日（２００８．１．１０）
【国際特許分類】
【出願日】平成１８年６月２１日（２００６．６．２１）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]