WO2007148546A1

WO2007148546A1 - 測位装置

Info

Publication number: WO2007148546A1
Application number: PCT/JP2007/061607
Authority: WO
Inventors: Norimasa Kobori; Kazunori Kagawa
Original assignee: Toyota Jidosha Kabushiki Kaisha
Priority date: 2006-06-21
Filing date: 2007-06-08
Publication date: 2007-12-27
Also published as: EP2034271A4; US20100004856A1; EP2034271A1; JP2008002906A; JP4600357B2; CN101473195A; US8725412B2; EP2034271B1; CN101473195B

Abstract

　測位装置９は、地図データを記憶した地図データ記憶手段５と、移動体の挙動情報を検出する自律センサ２、３と、ＧＰＳ等の電波航法測位手段による測位結果に自律センサによる検出情報を累積して移動体の推定位置を検出する慣性測位手段８２と、道路周辺の地物を検出する地物検出手段８４と、地物の位置を同定する地物位置同定手段８３と、地物の位置を基準に移動体の地物推定位置を推定する地物基準測位手段８５と、推定位置及び地物推定位置をカルマンフィルタに適用して移動体の位置を推定する位置推定手段８６とを備える。

Description

明細書

測位装置

技術分野

[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] 上記の課題を解決するために、本発明の測位装置は、地図データを記憶した地図データ記憶手段 (例えば、地図データベース 5)と、移動体の挙動情報を検出する自律センサ（例えば、車速センサ 2、ョーレートセンサ 3)と、 GPS等の電波航法測位手段による測位結果に自律センサによる検出情報を累積して移動体の推定位置を検出する慣性測位手段 (例えば、 INS測位手段 82)と、道路周辺の地物を検出する地物検出手段 (例えば、信号機検出手段 84)と、地物の位置を同定する地物位置同定手段 (例えば、信号機位置同定手段 83)と、地物の位置を基準に移動体の地物推定位置を推定する地物基準測位手段と、推定位置及び地物推定位置をカルマンフィルタに適用して移動体の位置を推定する位置推定手段と、を備えることを特徴とする

発明の効果

[0010] 本発明によれば、自律航法による測位結果を修正してより精度よく移動体の位置を測位する測位装置を提供することができる。

図面の簡単な説明

[0011] [図 1]測位装置を適用したナビゲーシヨンシステムの概略構成図である。 [図 2A]測位装置の機能ブロック図である。

[図 2B]図 2Aの測位装置における位置推定手段の動作を説明するための図である。

[図 3A]信号機と自車両の位置との関係を示す図である。

[図 3B]最小 2乗法により同定される信号機の位置を示す図である。

[図 3C]自車両の推定位置と地物推定位置とに基づき推定される最終推定位置を示す図である。

[図 4A]視線べタトノレの一例を示す図である。

[図 4B]信号機の位置を最小 2乗法を利用して同定する場合の一例を示す図である。

[図 5A]サンプル数が十分大きい場合の評価関数と信号機の位置の関係を示す図である。

[図 5B]サンプノレ数が小さい場合の評価関数と信号機の位置の関係を示す図である。

[図 6]測位装置が自律航法及び信号機の位置に基づく測位を利用して自車両の位置を推定する手順を示すフローチャート図である。

[図 7]自律航法により測位された推定位置を最終推定位置に修正する場合の一例を示す図である。

[図 8A]—般道路において GPS電波が遮断された状況における測位装置の測位結果を示す図である。

[図 8B]実際の道路の平面写真に自律航法による推定位置と最終推定位置とをプロットした一例を示す図である。

[図 9]地図データから得られる道路網に信号機の位置を登録した地図の一例を示す図である。

符号の説明

1 GPS受信装置

2 車速センサ

3 ョーレートセンサ

4 舵角センサ

5 地図データベース

6 入力装置 7 表示装置

8 ナビゲーシヨン ECU

9 測位装置

10 ナビゲーシヨンシステ

11 カメラ

21 信号機

22 交差点のノード

23 初期位置

24 推定位置

25 実際の自車位置

26 地物推定位置

27 最終推定位置

発明を実施するための最良の形態

[0013] 以下、本発明を実施するための最良の形態について、図面を参照しながら説明する。

[0014] 図 1は、本実施の形態の測位装置 9を適用したナビゲーシヨンシステム 10の概略構成図を示す。ナビゲーシヨンシステム 10は、ナビゲーシヨンシステムを制御するナビゲーシヨン ECU (Electrical Control Unit) 8 (ナビ ECU8)により制御される。ナビグーシヨン ECU8は、プログラムを実行する CPU、プログラムを記憶した記憶装置（ハードディスクドライブ、 ROM)、データやプログラムを一時的に記憶する RAM、データを入力及び出力する入出力装置、 NV (Nonvolatile)—RAM等がバスを介して接続されたコンピュータとして構成される。

[0015] ナビゲーシヨン ECU8には GPS (Grobal Positioning System)衛星からの電波を受信する GPS受信装置 1、車両の速度を検出する車速センサ 2、車両の重心回りの回転角速度を検出するョーレートセンサ 3 (又はジャイロセンサ）、操舵輪の舵角を検出する舵角センサ 4、地図データを記憶した地図データベース（地図 DB) 5、ナビゲーシヨンシステム 10を操作するための入力装置 6及び地図に現在位置を表示する液晶や HUD (Head Up Display)等の表示装置 7が接続されている。 [0016] 地図データベース 5は、実際の道路網をノード (例えば、道路と道路が交差する点、交差点から所定間隔毎に区切った点、等)及びリンク (ノードとノードを接続する道路）に対応づけて、テーブル状のデータベースとして構成される。

[0017] ナビゲーシヨン ECU8は、車室内に設けられた表示装置 7に、検出した現在位置周辺の地図を地図データベース 5から抽出し、指定された縮尺に合わせて表示する。ナビゲーシヨン ECU8は、必要に応じて地図に重畳して車両の現在位置を表示する

[0018] また、押下式のキーボードやリモコン等の入力装置 6から目的地が入力されると、検出した現在位置から目的地まで、例えばダイクストラ法など周知のルート検索方法でルート検索を行い、ルートを地図に重畳して表示すると共に、右左折する交差点の手前で運転者にルートを案内する。

[0019] カメラ 11は車両前方、より好ましくは車両前方の所定範囲を撮影するようにルームミラーの背面やフロントガラスの上方に取り付けられている。

[0020] カメラ 11は、例えば CCD (Charge Coupled Device) , CMOS (Complementary M etal Oxide Semiconductor)等の光電変換素子を有し、入射した光を光電変換素子により光電変換し、蓄積した電荷を電圧として読み出し増幅して A/D変換した後、所定の輝度階調 (例えば、 256階調）のデジタル画像に変換する。

[0021] カメラ 11は交差点などの道路に沿って設置された人工の地物 (信号機、標識、横断歩道などのペイント、電柱等）と自車両との位置を検出するため、距離情報が得られるカメラであることが好ましい。このため、例えばカメラ 11は 2台のカメラからなるステレオカメラ、移動体に搭載した 1台のカメラの時系列画像からステレオ視を行うモーシヨンステレオカメラである。また、 LED (発光ダイオード）力も所定の時間間隔で近赤外光を照射し、反射光を光電変換素子が受光するまでの時間を計測することで距離を求めることが可能なカメラであってもよい。

[0022] ナビゲーシヨン ECU8の CPUは、記憶装置に記憶されたプログラムを実行することで、本実施の形態で説明する測位を実現する。図 2Aは、測位装置 9の機能ブロック図を示す。測位装置 9は、電波航法測位により自車両の位置を測位する GPS測位手段 81、自律センサ（車速センサ 2，舵角センサ 4)を用いて自律航法により自車両の位置を測位する INS測位手段 82、カメラ 11により撮影された画像データから信号機等の地物を検出する信号機検出手段 84，検出された信号機の位置を同定する信号機位置同定手段 83、信号機の位置に基づき自車両の位置を測位する地物基準測位手段 85、カルマンフィルタにより自車両の位置の最尤値を出力する位置推定手段 86、及び信号機位置同定手段 83が同定した信号機など地物の位置情報を地物に対応づけて地図データベース 5に登録する地図データ登録手段 87を有する。以下、図 2Aに示した測位装置 9について説明する。

[0023] 本実施の形態の測位装置 9は、 GPS衛星の捕捉が困難な状況や GPS測位の信頼性が低下した状況下で、自律航法による車両の位置を補正して精度よい測位を可能にする。図 2Bは、図 2Aの測位装置における位置推定手段 86の動作を説明するための図である。

[0024] 図 2Bにより概略を説明すれば、 GPS電波が遮断された場合、測位装置 9は自律航法により測位された位置及び信号機など地物に対する視線ベクトルに基づき測位された位置を、カルマンフィルタによりカップリングして、位置 Yを精度よく推定する。

[0025] まず、 GPS測位手段 81は、周知の方法で GPS衛星からの電波に基づき自車両の位置を測位する。 GPS測位手段 81は所定の軌道を周回する複数の GPS衛星のうち現在の車両の位置から所定の仰角に入る GPS衛星を 4つ以上選択し、それらの GP S衛星から発信される電波を受信する。 GPS測位手段 81は電波の到達時間を計算し、到達時間と光速 cから捕捉した GPS衛星までの距離を算出する。そして、 3つの G PS衛星と自車両の距離が交差する 1点を自車両の位置として測位する。

[0026] 測位装置 9は、 GPS電波を受信している間は所定の時間毎に自車両の位置を測位する。そして、 GPS電波が遮断されると、最後に測位した位置を初期位置、その時の走行方向を初期方向として、これらに走行距離及び方向を累積する自律航法による測位を開始する。

[0027] 図 3Aは自律航法により検出される位置を示す図である。自車両は交差点の方向に向かって走行しており初期位置 23で GPS電波が遮断される。また、地図データべ一ス 5から交差点のノード 22が抽出されているため、ノード 22の位置は既知となっている。 [0028] INS (Inertial Navigation Sensor)測位手段 82は車速センサ 2から車速、舵角センサ 4から舵角を検出して、初期位置 23及び初期方向に走行距離及び方向を累積して、自律航法による位置及び方向を推定する（以下、推定位置、推定方向という）。なお、自車両の走行方向を検出する自律センサは、ジャイロセンサゃョーレートセンサでもよい。

[0029] また、カルマンフィルタを適用するためには推定位置の誤差分散を必要とするが、車速センサ 2及び舵角センサ 4の誤差は速度や GPS電波遮断時間等に応じて既知である。したがって、これらに基づき累積した推定位置 24の誤差分散も既知である。図 3Aでは推定位置 24の誤差分散を点線で囲まれた楕円で示した。なお、この時（時刻 t)、自車両は実際の位置 25に存在する。

[0030] 自律航法により位置を検出している間、信号機検出手段 84はカメラ 11が撮影した画像データ力信号機を検出する。検出した信号機の位置を利用して、自車両の位置を推定するためである。なお、信号機 21は車両から利用できる地物であればよぐ信号機 21の他に標識や電信柱を検出してもよい。

[0031] 信号機検出手段 84は、予め記憶してある信号機の標準パターンを用いてパターンマッチングにより信号機を検出する。信号機検出手段 84は、画像データの画素値（輝度）を水平方向及び垂直方向に走査し、勾配が所定以上のエッジ部を抽出する。そして隣接するエッジ部を接続して撮影対象物の輪郭を抽出し、これに標準パターンをパターンマッチングする。なお、信号機の場合その輪郭は長方形であるので、所定の縦横比の輪郭となるエッジ部のみにパターンマッチングを施してもよい。信号機検出手段 84は、輪郭に区切られる領域と標準パターンとを画素毎に輝度を比較し、所定以上に相関する場合に信号機が撮影されているとして信号機を検出する。

[0032] 信号機が検出された場合、信号機検出手段 84は画像データから距離情報を抽出する。上記のように、距離情報は例えば 2つの画像データの視差から抽出する。カメラ 11により撮影された 1対のステレオ画像力同一の撮像対象物 (信号機）が写ってレ、る部分を抽出し、 1対のステレオ画像同士で信号機の同一点を対応づけ、対応づけられた点（対応点）のずれ量 (視差)を求めることによって信号機までの距離を算出する。すなわち、 1対の画像データを重ね合わせると、視差により信号機が左右横方向にずれる。一方の画像を 1画素ずつシフトしながら最も重なり合う位置を、画素値の相関に基づいて求める。このときシフトした画素数を n、レンズの焦点距離を f、光軸間の距離を m、画素ピッチを dとすると、撮像対象物までの距離 Lは、関係式 L= (f · m) Z (n' d)に従って算出される。ここで、（n' d)は視差を表す。

[0033] また、信号機検出手段 84は、検出された信号機と自車両との視線べ外ルを算出する。視線ベクトルの方向 Θは、カメラ 11の取付方向（カメラ 11の正面方向）をゼロにして、光電変換素子における信号機の撮影位置と距離 Lにより求められる。図 4Aは視線ベクトルの一例を示す図である。

[0034] ところで、地図データベース 5にはノードの座標、交差点の有無及び種類に加え、信号機の設置有無や信号機の設置座標 (以下、信号機座標という）が記憶されている場合がある。信号機座標が記憶されている場合には、信号機の絶対位置が予め確定しているので検出された信号機の信号機座標を取得することができる。

[0035] し力ながら、 1つの画像データに複数の信号機が撮影されている場合や地図データベース 5に信号機座標が記憶されていない場合には、信号機が検出された画像データから信号機の位置を同定する必要がある。

[0036] 本実施形態の信号位置同定手段 83は、 i)信号機座標、 ii)最小 2乗法、 iii)最急勾配法を適用した最小 2乗法、のいずれかを利用して信号機の位置を同定する。

[0037] 図 4Bは信号機の位置を最小 2乗法を利用して同定する場合の一例を示す図である。信号機が検出される度に、距離 Ll、 L2、•••Ln,同様に視線ベクトルの方向 Θ 1、 Θ 2· · · θ η、が得られるので、自車両からの距離と視線べタトノレの方向の組（L、 Θ )により点の集合（a、 b)が得られる。なお、信号機（灯）の高さ方向の位置はおおよそ一定であるので、（a、 b)は道路に平行な平面の座標である。

[0038] 例えば、 a≡k +k L +k Θ b≡m +m L +m Θ と線型モデルを立てれば、

i 0 1 i 2 i i 0 1 i 2 i

2乗誤差 ε ² (k 、 k , k )、 ε ² (m 、 m ， m )は次のようになる。但し、サンプリング

k 0 1 2 m 0 1 2

数を Nとし、 iは 1〜Nまでの値を取る。

ε ² (k 、 k ， k ) = (1/N)∑ { ai— (k +k L +k θ ) Ϋ

k 0 1 2 0 1 i 2 i

ε ² (m 、 m ， m ) = (l/N)∑ {bi— (m +m L +m θ ) Ϋ

m 0 1 2 0 1 i 2 i

ε ²を k 、 k , k ίこつレヽて偏微分し、 a ε ² /ak = 0、 a ε ² /ak = 0、 a ε ² /ak =0、力（k、 k , k )を求めることができる。 a ε /akの「a」は偏微分を表す。 bにつ

0 1 2 i いても同様に求められる。

[0039] なお、上記の線型モデルは一例であり、（a、 b)と（L、 θ )の関係は非線形であってもよレ、。例えば、 ai≡f(L) +g( Θ )である。

[0040] 図 3Bは最小 2乗法により同定される信号機 21の位置を示す図である。時刻 tにおける推定位置 24に至るまでに検出される信号機までの距離 L、視線ベクトルの方向 Θにより、信号機 21の位置が同定される。

[0041] ところで、最小 2乗法により好適な演算結果を得るためには、サンプル数 Nが 4以上である必要がある。このため、信号位置同定手段 83は、サンプノレ数 Nが 4未満の場合、すなわち信号機が検出され視線べ外ルが算出された回数が 4未満の場合、信号機の位置を同定しなレ、。これにより、カルマンフィルタを適用することで自車両の測位位置の精度が低下することを防止できる。この場合、測位装置 9は推定位置 24と推定方向を出力する。

[0042] また、このような最小 2乗法の考え方は、「各時刻の自車位置と信号機の位置との距離の 2乗誤差の総和」を評価関数として設定し、それを最小にする信号機の位置を求めるものである。そこで、最急勾配法を使用して評価関数の傾きに基づき評価関数が最小となる信号機の位置を求める方法について説明する。

[0043] 図 5Aは評価関数と信号機の位置の関係を示す図である。算出された信号機の位置に応じて評価関数の値は変動し、評価関数に最小値が存在することが分かる。最急勾配法は、適当な初期値からはじめて、パラメータの値を微分値と逆の方向に少しずつ変化させて徐々に最適なパラメータに近づけて行く。

[0044] 最小 2乗法を用いた場合、評価関数は最小 2乗誤差であるので各パラメータでの偏微分を計算する。例えば、ノラメータ k、 k , kについてはそれぞれ次式が更新式と

0 1 2

なる。

k ^a+1)=k ⁽ⁱ⁾ + 2-(l/N)∑{(a-(k +k L+k Θ )}

0 0 i 0 1 i 2 i

k ^a+1)=k ⁽ⁱ⁾ + 2-(l/N)∑{(a-(k +k L+k Θ )}L

1 1 i 0 1 i 2 i i

k ^a+1)=k ⁽ⁱ⁾ + 2-(l/N)∑{(a-(k +k L+k θ )}θ

2 2 i 0 1 i 2 i i

添字の「は 0から始まり、 k が最小となったら、そのパラメータ力評価関数が最小となるパラメータであるので計算を打ち切る。評価関数が最小となったか否かは微分値 (傾き）がほぼゼロになったか否かで判定される。このようにして、ノラメータ k

0

、 k , kが得られる。図 5Aでは、評価関数が最小となる信号機の位置をグローバル

1 2

minとして示している。

[0045] 最急勾配法では、 j = 0の時の k ^(Q)等が各パラメータの初期値となる力この初期値

0

に最小値の精度が依存することが知られている。図 5Bは、評価関数と信号機の位置の関係図の一例を示す。この例では、極小値が複数ある場合を示している。図 5Bに示すように、グローバル minは右側の極小値に対応する信号機の位置を示す。初期値によっては左側の極小値に対応する信号機の位置を示すローカル minに陥る場合がある。また、サンプノレ数 Nが小さい場合、評価関数の値は発散しやすい。

[0046] しかしながら、信号機は交差点付近に存在すると推定してよい。そこで、最急勾配法の初期値に交差点のノード位置から求めた値を設定する。これは初期値をノード位置とすることと等価である。このような処理により、サンプル数 Nが小さい場合でもグローバル minの信号機の位置を検出することができる。

[0047] 以上のようにして、図 3Bの信号機 21の位置を同定することができる。

[0048] 図 2Aに戻り、地物基準測位手段 85は、 i)信号機座標、 ii)最小 2乗法、又は、 iii) 最急勾配法を適用した最小 2乗法、のいずれかを用いて取得した信号機 21の位置に基づき、自車両の位置を測位する（以下、地物推定位置、地物推定方向という）。地物推定方向は、地物推定位置を数点蓄積することで検出される。

[0049] 図 3Cは、信号機の位置から距離 L、視線ベクトルの方向 Θにより測位された自車両の地物推定位置 26を示す図である。地物基準測位手段 85は、信号機の位置を原点に、時刻 tの距離 L及び視線ベクトルの方向 Θから地物推定位置 26及び地物推定方向を算出する。

[0050] また、地物推定位置 26の誤差分散は、 i)信号機座標を用いた場合、信号機座標の場合は距離 Lの誤差 A Lや視線ベクトル Θの誤差 Δ Θから求められ、 ii)及び iii)では最小 2乗法により信号機の位置を同定した場合には i)に加えパラメータを算出する過程で得られる誤差により求められる。図 3Cでは地物推定位置 26の誤差分散を点線で囲まれた楕円で示した。 [0051] 位置推定手段 86は、推定位置 24と地物推定位置 26とをカルマンフィルタにより力ップリングして、確率的に最も高い自車両の最終推定位置及びその方向を出力する

[0052] 図 3Cには、推定位置 24と地物推定位置 26に基づき推定される最終推定位置 27 を示した。図 3Cでは、それぞれの誤差分散を推定位置 24と地物推定位置 26に凸部を有する分散で示したが、分散は実際には 3次元の広がりを有する。図 2Bはその分散及びカルマンフィルタにより最尤位置 Yを推定する様子を示す。

[0053] カルマンフィルタは、 1つの系の状態がそれぞれ独立に推定される場合、状態の確率密度の分布に基づき最も確率の高い状態 (分布の積が最大になる状態）を推定する。したがって、 2つの測位情報をカルマンフィルタでカップリングすることで、存在する確率が最も高い最終推定位置 27を推定することができる。

[0054] カルマンフィルタでは、推定位置 24を Zベクトル、地物推定位置 26を Xベクトルとした場合、既知の観測方程式 Hを用いて Zと Xの間には次の関係があるものとする。

[0055] Z-HX = 0

Zの誤差分散を R、 Xの誤差分散を Mとすると、最終推定位置 27に相当する最尤値 Yの誤差分散は、解析的に Α= (Μ^_1 + Η ^_1Η)^_1になる。また、カルマンフィルタでは、最尤値 Υが次式により求められる。

[0056] Y(i) =X(i— 1) +K(i) · {Z(i)— H(i) -X(i-l) }

ここで、添字の iは自車両の位置を観測した番号、 tは転置行列、 1は逆行列を表す。 K(i)はカルマンゲイン行列であり、次のように表せる。

[0057] K(i)=A(i) 'HfRi— ¹

A(i) = (M—丄+ — )— ¹

R(i)}^_1H(i)M(i)

推定位置 24の誤差分散、地物推定位置 26の誤差分散はすでに求めているので、位置推定手段 86はこれらの式から最終推定位置 27を出力する。なお、自車両の方向についても同様にして求めることができる。

[0058] 図 6は、測位装置 9が自律航法及び信号機の位置に基づく測位を利用して自車両の位置を推定する手順を示すフローチャート図である。

[0059] 測位装置 9は例えば GPS測位手段 81の測位間隔毎に GPS電波が遮断されたか否かを判定する（S l)。 GPS電波が遮断されていなければ（S Iの No)、 GPS電波を利用して自車両の位置を測位する（S 2)。

[0060] GPS電波が遮断された場合（S 1の Yes)、 GPS測位手段 81は初期位置、初期方向をナビゲーシヨン ECU8の記憶装置に記憶しておき、 INS測位手段 82は車速及び舵角に基づき、初期位置 23及び初期方向に走行距離及び走行方向を累積して自律航法による推定位置 24及び推定方向を推定する（S3)。なお、 INS測位手段 8 2は、車速センサ 2及び舵角センサ 4の誤差等に応じて、これらに基づき累積した推定位置 24、推定方向の分散を算出する。

[0061] また、信号機検出手段 84は、自律航法による測位に並行して信号機 21の検出を繰り返す (S4)。そして、信号機が検出された場合、信号機検出手段 84は 1対の画像データから距離情報を抽出する。また、信号機検出手段 84は、検出された信号機と自車両との視線べクトノレを算出する（S5)。

[0062] ついで、信号機位置同定手段 83は過去 N回分の信号機との距離、視線ベクトルを参照する（S6)。

[0063] 上述したように、この Nが小さいと最小 2乗法により同定した信号機の位置の精度が低下するので、信号機位置同定手段 83は Nが 4以上か否かを判定する（S7)。 4つ以上の距離及び視線ベクトルが取得されていない場合（S7の No)、測位装置 9は自律航法による測位結果を出力する。

[0064] 4つ以上の距離及び視線ベクトルが取得されてレ、る場合（S7の Yes)、信号機位置同定手段 83は、進行方向にある交差点のノードの位置情報を地図データベース 5から抽出する（S8)。

[0065] ついで、信号機位置同定手段 83は、最小 2乗法により信号機の位置を算出する（S 9)。最小 2乗法を適用する場合、最急勾配法を利用して更にその初期値にノードの位置情報を使用する。これにより信号機の位置を同定することができる。

[0066] ついで、地物基準測位手段 85は、同定された信号機の位置を原点に、距離 L及び視線ベクトルの方向 Θ力地物推定位置 26及び地物推定方向を算出する（S 10)。 [0067] ついで、位置推定手段 86は、推定位置 24及び地物推定位置 26をカルマンフィルタによりカップリングして、最終推定位置 27を出力する（Sl l)。

[0068] 図 7は、自律航法により測位された推定位置 24を最終推定位置 27に修正する場合の一例を示す図である。図 7では、最終推定位置 27が出力されるたびに自律航法による推定位置を補正する様子を示す。本実施の形態では、走行方向にある信号機の位置を基準に自車両の位置を測位し、自律航法により測位された位置を補正するので、特に走路方向の位置を精度よく補正することができる。

[0069] 図 8は、一般道路において GPS電波が遮断された状況における測位装置 9の測位結果を示す。図 8Aはカメラ 11に撮影された画像データを示す。図 8Aでは画像データの上側に信号機 21が検出されている。

[0070] 図 8Bは実際の道路の平面写真に、自律航法による推定位置 24と最終推定位置 2 7とをプロットした図である。自車両は平面写真の下方から上方向に走行した。図 8B に示すように、走行するにつれて推定位置 24は実際の道路とのズレが徐々に大きくなるが、地物推定位置 26で補正することにより、道路とのズレを大幅に低減することができる。

[0071] ところで、本実施形態では GPS電波が遮断された後、自律航法により測位された自車両の位置を補正する過程で信号機等のランドマークの位置を検出する。これは、上述のように、一般的なナビゲーシヨンシステムの地図データベース 5には道路インフラ (信号機 ·標識等）の位置情報が記憶されていないためである。

[0072] 運転者に適切なタイミングで交差点等における運転支援（車両制御、注意喚起等）を提供するためには、道路インフラの位置情報が不可欠といえる。しかし、全国の道路インフラを測位し、データベース化するには多大な労力とコスト必要である。また、道路インフラが新たに設置された場合、すぐには対応できないという問題もある。

[0073] そこで、測位装置 9は、自車両の位置を補正する過程で検出した信号機等の位置、また、 GPS電波が遮断されていない状態においても信号機等の位置を検出し、地図データベース 5に登録することとすれば、道路インフラの位置情報のデータベースを作成できる。

[0074] 図 9は地図データから得られる道路網に信号機の位置を登録した地図の一例を示す。白丸が登録した信号機の位置を示す。

[0075] 車載したカメラによる道路インフラの位置の検出は、短時間かつ低コストに実現でき、道路インフラが新たに設置されても車両が走行すればデータベースに反映できるため、優れた冗長性を兼ね備える。

[0076] 以上のように、本実施形態の測位装置 9は、信号機などの地物の位置を同定し地物の位置を基準に測位した自車両の位置と、自律航法により測位した位置をカルマンフィルタに適用することで、 GPS電波が遮断されても自律航法による測位結果を精度よく補正することができる。特に、進行方向に対する位置を精度よく補正することができる。また、同定した地物の位置を地図データベースに登録することで、道路インフラの位置情報が登録された地図データベースを、更新しながら利用することができる

[0077] 本発明は具体的に開示された上述の実施形態に限定されるものではなぐ特許請求の範囲から逸脱することなく種々の変形や変更が可能である。

[0078] 本出願は、 2006年 6月 21日出願の優先権主張日本特許出願第 2006— 171755 号に基づいており、その全内容がここに援用されている。

Claims

請求の範囲

[1] 地図データを記憶した地図データ記憶手段と、

移動体の挙動情報を検出する自律センサと、

GPS等の電波航法測位手段による測位結果に前記自律センサによる検出情報を累積して前記移動体の推定位置を検出する慣性測位手段と、

道路周辺の地物を検出する地物検出手段と、

前記地物検出手段により検出された前記地物の位置を同定する地物位置同定手段と、

前記地物の位置を基準に前記移動体の地物推定位置を推定する地物基準測位手段と、

前記推定位置及び前記地物推定位置をカルマンフィルタに適用して前記移動体の前記位置を推定する位置推定手段と、

を備えることを特徴とする測位装置。

[2] 前記地物検出手段は、撮影手段により撮影された画像データから前記地物を検出し、該地物までの視線ベクトルを取得する視線ベクトル検出手段を備え、前記地物位置同定手段は、複数の前記視線ベクトルの長さ及び方向を使用して最小 2乗法により前記地物の地物位置情報を算出する地物位置算出手段を備えることを特徴とする請求項 1に記載の測位装置。

[3] 前記地物位置算出手段は、前記地図データ記憶手段から、前記地物から所定距離内にある交差点の交差点位置情報を抽出し、最小 2乗法により前記地物位置情報を算出する際の初期値に前記交差点位置情報を設定することを特徴とする請求項 2 に記載の測位装置。

[4] 前記地物位置同定手段が取得した前記地物の前記地物位置情報を、該地物及びリンク又はノードと対応づけて前記地図データ記憶手段に登録する地図データ登録手段を備えることを特徴とする請求項 1に記載の測位装置。

[5] 前記視線ベクトル検出手段により検出された前記視線べクトルが所定数以下の場合、前記地物検出手段は、最小 2乗法による前記地物位置情報を算出せず、前記位置推定手段は前記推定位置を出力することを特徴とする請求項 1記載の測位装

L09l90/L00Zd /lDd 91 91^8 /·00Ζ OAV