(57)【要約】【課題】移動体の実時間測位装置において、不動物に関する事前のデータ収集を必ずしも必要としない低コストで高精度で信頼性の高い装置を実現することである。【解決手段】環境特徴評価部１４１は、環境特徴取得装置１２０によって取得した環境特徴の種類や、相対位置に関する情報の精度や信頼度を算定する。自車両運動評価部１４２は、環境特徴評価部１４１にて評価された環境特徴の位置情報の時間変化に基づいて、その変化が観測された期間に移動した自車両の運動（速度ベクトルなど）を求め、同時にその運動の精度や信頼度を算定する。自車両位置評価部１３２では、自車両位置の観測値、環境特徴の位置情報、推定した自車両運動などに基づいて自車両の絶対位置を求める。また、同時にその運動の精度や信頼度を算定する。このとき、自車両の絶対位置と環境特徴の相対位置に基づいて、環境特徴の絶対位置やその精度などを同時に求めてもよい。【選択図】図１
2007322391.tif 000002
【特許請求の範囲】
【請求項１】
走行中の自車両の絶対位置を与える信号を受信するＧＰＳ受信装置と、走行環境中の自車両周辺における不動物の自車両に対する相対的な位置を実時間で測定する測位装置とを有する車両搭載用の自車両位置推定装置であって、
前記測位装置によって収集された位置データに基づいて求められる、前記不動物に対する前記自車両の相対的かつ経時的な位置変化量から、前記自車両の運動を推定して出力する自車両運動推定部と、
同一の前記不動物に対する一連の前記自車両の前記運動と、その一連の運動中における前記自車両の複数受信回に渡る前記絶対位置とに基づいて、前記自車両の絶対位置を補正して出力する自車両位置推定部と
を有する
ことを特徴とする自車両位置推定装置。
【請求項２】
前記自車両位置推定部は、
前記自車両の補正された前記絶対位置を出力する際に、更に当該絶対位置の推定精度をも同時に出力する
ことを特徴とする請求項１に記載の自車両位置推定装置。
【請求項３】
所定の走行環境中における不動物の特徴及び絶対位置が予め記録された環境特徴記憶装置と、
前記環境特徴記憶装置に記憶されている不動物の特徴の中から、前記位置データによって表される不動物の特徴を、所定のデータ照合処理を用いて実時間で検索する検索照合手段と
を有する
ことを特徴とする請求項１または請求項２に記載の自車両位置推定装置。
【請求項４】
前記位置データによって表される不動物の特徴及び絶対位置を所定のデータベースに記憶するデータベース更新手段を有する
ことを特徴とする請求項１乃至請求項３の何れか１項に記載の自車両位置推定装置。
【請求項５】
前記自車両運動推定部は、
前記位置データで示される不動物と移動物とを判別する判別手段を有する
ことを特徴とする請求項１乃至請求項４の何れか１項に記載の自車両位置推定装置。
【請求項６】
前記自車両位置推定部は、
前記複数受信回に渡る前記絶対位置に係わる拡張カルマンフィルタを用いた統計操作に基づいて、前記自車両の補正された絶対位置を出力する
ことを特徴とする請求項１乃至請求項５の何れか１項に記載の自車両位置推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ＧＰＳを用いた測位技術に関し、特に移動中の自車両の位置を高精度に推定するための自車両位置推定装置に関する。
本発明は、カーナビゲーションシステムや自動車のオートクルーズシステムなどに有用なものである。
【背景技術】
【０００２】
近年、ＲＴＫ−ＧＰＳ（Real-Time Kinematic Global Positioning System ）によって、ＧＰＳ衛星を用いた測位技術により誤差２〜３ｃｍ程度の測位精度を得ることができる様になったが、これらの測位装置は非常に高価であり、また、例えば走行中の自動車などの移動体に関する測位を実時間で実施するには必ずしも好適なものではない。
【０００３】
また、これらのＲＴＫ−ＧＰＳ等を用いずに構成可能な、自車両に関する比較的高精度な測位を実時間で実施するための従来の測位技術としては、例えば、以下の特許文献１〜３に記載されているものなどが公知である。特に、特許文献１や特許文献２の測位技術は、通常のナビゲーションシステムでは用いられない程詳細な道路周辺の不動物（例：道路標識や反射板や停止線など）の特徴データや位置データが、事前調査によって予め記憶された記憶装置（「道路周囲環境記憶装置」や「詳細地図ＤＢ」）と、これらの不動物の特徴データと走行中に実時間で検出された不動物の特徴とを照合する照合手段とを備える点に特徴がある。
【０００４】
また、特許文献３に記載の従来の車両位置検出装置は、ビデオカメラで捉えた画像データから道路形状を抽出する画像分析手段を有し、走行中に実時間で抽出した道路形状をカーナビゲーションシステムが有する一般の道路地図データにおける道路形状と照合する点に特徴がある。
【特許文献１】特開平１０−３００４９３
【特許文献２】特開２００５−２６５４９４
【特許文献３】特開平６−９４４７０
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１や特許文献２の測位技術においては、上記の記憶装置に予め道路周辺の不動物の特徴データや位置データを記憶させておく必要があり、これらのデータを予め詳細に収集する事前調査に掛かる作業コストは無視し難い。また、これらの従来技術は、上記の「道路周囲環境記憶装置」や「詳細地図ＤＢ」がエンドユーザに事前に提供されることを前提とするものであるから、これらに記憶させるべき位置データを収集している段階から同時に、そのエンドユーザが所望のドライブ目的を達成しつつこれらの従来技術によって当該車両の位置を高精度に求めることは、当然ながらできない。
【０００６】
一方、上記の特許文献３の測位技術によれば、事前にこの様なデータ収集を行う必要性からは解放されるものの、この様な従来技術は、例えば交差点などの様な特段の特徴的道路形状が存在しない走行環境中では用いることができないため、十分な有用性や信頼性を確保することは難しい。また、車載カメラで撮像した画像データを分析して、そこから特徴的道路形状を抽出する画像処理は必ずしも容易ではないし、時間帯や天候によっては所要の道路形状の抽出が困難となる場合も少なくない。また、交差点の角地やその周辺に家屋やフェンスなどの比較的低い障害物がある場合などでも、分岐方向の道路がその障害物によってカメラの死角に入ってしまうため、当該交差点等の道路形状を適切に抽出することは困難となる。
【０００７】
更に、上記の特許文献３の測位技術では、たとえ道路形状が適切に抽出できた場合であっても、自車両の進行方向即ちカメラの奥行き方向において、道幅以上に詳細な測位精度を得ることは容易ではない。また、それら道路形状の特徴抽出処理に掛かる画像処理オーバーヘッドは非常に大きく、よって、この様な従来技術は、移動体に関する比較的高精度な測位を実時間で実施するには必ずしも有効とは言えない。
【０００８】
本発明は、上記の課題を解決するために成されたものであり、その目的は、移動体の実時間測位装置において、不動物に関する事前のデータ収集を必ずしも必要としない低コストで高精度で信頼性の高い装置を実現することである。
また、本発明の更なる目的は、所望の自車両位置推定装置が適用可能な地域を大幅に拡大することである。
【課題を解決するための手段】
【０００９】
上記の課題を解決するためには、以下の手段が有効である。
即ち、本発明の第１の手段は、走行中の自車両の絶対位置を与える信号を受信するＧＰＳ受信装置と、走行環境中の自車両周辺における不動物の自車両に対する相対的な位置を実時間で測定する測位装置とを有する車両搭載用の自車両位置推定装置において、上記の測位装置によって収集された位置データに基づいて求められる、不動物に対する自車両の相対的かつ経時的な位置変化量から、自車両の運動を推定して出力する自車両運動推定部と、同一の不動物に対する一連の自車両の運動とその一連の運動中における自車両の複数受信回に渡る絶対位置とに基づいて自車両の絶対位置を補正して出力する自車両位置推定部とを設けることである。
【００１０】
ただし、上記の測位装置としては、各種のレーダやカメラなどを用いることができる。また、それらを任意に組み合わせて上記の測位装置を構成しても良い。
なお、上記の測位装置によって収集された位置データは、所定の二次記憶装置などに保存しておいても良いし、或いは、所望の自車両の絶対位置を算定した後は使い捨てにしても良い。
【００１１】
また、本発明の第２の手段は、上記の第１の手段において、上記の自車両位置推定部によって、自車両の補正された絶対位置を出力する際に更に当該絶対位置の推定精度をも同時に出力することである。
ただし、この推定精度としては、例えば絶対位置に係わる平均化処理などを行った際の分散や標準偏差などで表現しても良いし、また、ＧＰＳやレーダやカメラなどの性能や信頼性などを加味して推定精度を算出する様にしても良い。
【００１２】
また、本発明の第３の手段は、上記の第１又は第２の手段において、所定の走行環境中における不動物の特徴及び絶対位置が予め記録された環境特徴記憶装置と、この環境特徴記憶装置に記憶されている不動物の特徴の中から、上記の位置データによって表される不動物の特徴を所定のデータ照合処理を用いて実時間で検索する検索照合手段とを備えることである。
ただし、上記の不動物の特徴は、複数の不動点の並び方（位置関係）のみによって特定しても良い。したがって、この場合には、多数の不動点の絶対位置の情報だけが上記の環境特徴記憶装置に記憶されていれば十分である。また、例えばレーダを用いる場合には、検出物からの反射強度または反射率などを記憶するようにしても良い。
【００１３】
また、本発明の第４の手段は、上記の第１乃至第３の何れか１つの手段において、上記の位置データによって表される不動物の特徴及び絶対位置を所定のデータベースに記憶するデータベース更新手段を備えることである。
【００１４】
ただし、このデータベースは、上記の環境特徴記憶装置とは独立（並列）のものであってもよく、また、上記の環境特徴記憶装置を具現するものであっても良い。即ち、それらのデータが備える物理的かつ具体的なデータ構造は、特に問題とする必要がない。また、上記のデータベース更新手段は、上記の環境特徴記憶装置上のデータを更新（修正）するものであっても良いし、上記の環境特徴記憶装置とは独立に、不動物の絶対位置に係わる収集データを新たに別途蓄積する手段であっても良い。
【００１５】
また、本発明の第５の手段は、上記の第１乃至第４の何れか１つの手段の自車両運動推定部において、上記の位置データで示される不動物と移動物とを判別する判別手段を備えることである。
ただし、この判別処理においては、例えば上記の測位装置によって検出される物体の大半が不動物であると言う仮定の元に実施しても良い。
【００１６】
また、本発明の第６の手段は、上記の第１乃至第５の何れか１つの手段において、上記の自車両位置推定部によって、複数受信回に渡る絶対位置に係わる拡張カルマンフィルタを用いた統計操作に基づいて、自車両の補正された絶対位置を出力することである。
【００１７】
ただし、本発明における絶対位置に係わる統計操作は、拡張カルマンフィルタを用いたものに限定されない。その他にも、例えばパーティクルフィルタなどを用いて、所望の不動物データを逐次更新する方法を用いても良いし、過去の一定時間分の観測データを用いて最適解を求める様にしても良い。
以上の本発明の手段により、前記の課題を効果的、或いは合理的に解決することができる。
【発明の効果】
【００１８】
以上の本発明の手段によって得られる効果は以下の通りである。
同一の不動点に位置変換可能な多数の移動点に関してＧＰＳ信号を受信し、当該ＧＰＳ信号によって与えられる各移動点の絶対位置を、当該不動点の位置座標（絶対位置）に位置変換し、位置変換後のその絶対位置を平均化処理することにより、ＧＰＳ信号が示す絶対位置に含まれるランダム誤差を排除することができる。当該不動点は、道路周辺の走行環境中にある不動物を示す点でもよいし、自車両の走行軌跡上からある固定点を任意に選択してもよい。また、上記の位置変換を与えるデータは、上記の本発明の測位手段によって取得することができる。
【００１９】
即ち、本発明の第１の手段によれば、同一の不動物に対する自車両の相対的な一連の運動（：各時刻のヨー角速度や速度ベクトルやその積分である走行軌跡など）を求めることができ、この運動を正確に求めれば、その一連の運動中に受信された自車両の絶対位置を、何れもその一連の運動中において自車両が占めた位置（走行軌跡）の中の任意の１点の絶対位置に互いに変換することが可能となる。（ただし、上記の同一の不動物の絶対位置に位置変換してもよい。）例えば、当該不動点を自車両の走行軌跡上から任意に１点選択すれば、その期間中の自車両の走行運動は、位置データ上はその１点に固定でき、よって、この運動は仮想的にキャンセルすることができる。
【００２０】
したがって、本発明の第１の手段によれば、自車両が走行中であっても、自車両の仮想的な位置データ上においては、自車両を１点に停止させておいた場合と数学的には等価な効果を得ることができる。また、上記と同様の位置変換操作を用いれば、その一連の運動中における自車両の複数受信回に渡る絶対位置についても、勿論、その一連の運動中における自車両の任意の１点の絶対位置に変換することができる。
【００２１】
このため、本発明の第１の手段によれば、ある同一の不動点に対する自車両の相対的な位置を上記の軌跡上の任意の１点に実際に固定しつつ、その停車位置においてＧＰＳ衛星からその固定点の絶対位置を与える信号を複数回受信した場合と、全く同等の作用効果を得ることができる。このため、上記の一連の運動中における自車両の絶対位置の受信回数を増加させるとともに、その固定点の絶対位置を適当に平均化処理すれば、ＧＰＳ衛星から提供される絶対位置に含まれる位置誤差（ランダムノイズ）が効果的に排除されて、これによって、ＧＰＳ衛星から提供される自車両の絶対位置の精度を大幅に向上させることが可能となる。
【００２２】
言い換えれば、本発明は、上記の様な過去から現在までの多数の観測値を使って、この様な幾何学的な統計操作（上記の平均化処理等）を実時間で繰り返し実行することによって、自車両の絶対位置を実時間で従来よりも高精度に推定するためのものである。
【００２３】
データ上における上記の様な仮想的な固定点は、上記の通り一連の運動中の自車両の軌跡の中から随時任意に選択することができる点であるから、当該固定点は現在までの当該軌跡上において必要に応じて動的に変更することができ、この様な変更も上記の位置変換によって随時任意に実施することができる。したがって、この様な仮想的な固定点は、例えば常時最新の現在位置に合わせるように、随時変更するなどしても良い。
【００２４】
また、この様な仮想的な固定点は、上記の同一の不動物の絶対位置に位置変換してもよく、さらに、その不動物の絶対位置を先に算出し、それに基づいて自車両の絶対位置を算定する手順を採用しても良い。これらの手順は可換かつ任意であり、何れの手順を採用しても数学的に等価な結果を導く。以上のことから、上記の平均化処理等は、一度にまとめて実行しなくても実時間処理によって逐次実行することができることが分かる。また、例えば特に、カルマンフィルタなどの様なマルコフ連鎖的な（即ち再帰的な）アルゴリズムを用いて具現可能なことも分かる。
【００２５】
なお、上記のＧＰＳ受信装置によって自車両の絶対位置を与える信号を受信する上記の複数受信回の回数は、勿論多い程望ましいが、ある一定回数以上の範囲においては、所望の自車両の絶対位置の補正精度はある一定のレベルで飽和する。ただし、この飽和レベルは、上記の測位装置の測位誤差を限定要因とするものである。
【００２６】
また、本発明の第１の手段によれば、上記のような事前調査を予め実施しておく必要がなく、１エンドユーザによって初めて走行される道路上においても、当該自車両の絶対位置を実時間で高精度に求めることができる。
また、本発明で利用する環境特徴（即ち不動物の同一性）は、例えば交差点、道路標識、反射板、所定の路面表示などと言った特定の意味を持つ高級な属性（即ちそれが何であるのかまでをも識別可能な物体特徴）に限定されるものではないので、本発明を適用する際に道路周辺の不動物の位置を特定する上で必要となる環境特徴の出現頻度や検出数が十分に得られない走行環境は、例えば砂漠や草原などのオフロードを除いては殆ど考えられない。言い換えれば、特に高精度な測位精度が要請される様な走行環境においては、たとえその不動物がどの様な物であるかまでは分らなくても、通常、位置認識が可能な何らかの環境特徴（不動物）が数多く存在している。
したがって、本発明の第１の手段によれば、所望の自車両位置推定装置の適用可能な地域を従来よりも大幅に拡張することができる。
【００２７】
また、本発明の第１の手段によれば、上述の従来の記憶装置、即ち「道路周囲環境記憶装置」や「詳細地図ＤＢ」などの二次記憶装置を必ずしも具備する必要がなく、また、例えばジャイロなどの高価な方位センサなどを具備する必要もないので、価格や搭載性などの点でも特に有利である。
【００２８】
また、本発明の第２の手段によれば、本発明の自車両位置推定装置が出力する自車両の絶対位置の推定精度を知ることができるので、当該絶対位置をより適切に利用することが可能となる。また、検出された不動物の絶対位置をデータベースなどに記憶したり、その後更に、それらのデータを更新したり再評価したりする場合などにも、データの重みや精度や信頼性などを常に正確に把握することが可能となる。即ち、本発明の第２の手段は、不動物の位置データの利用や、その後の再利用や、その後の統計操作などに寄与するものである。
また、特に、カルマンフィルタなどの様な再帰的アルゴリズムを用いて自車両や不動物の絶対位置に関する最適解を逐次再計算する場合などには、それらの推定精度（例：分散など）も随時更新（再評価）する必要があるので、本発明の第２の手段は、それらの更新処理をリアルタイムで簡潔に逐次実行する上でも非常に有用である。
【００２９】
また、本発明の第３の手段によれば、ＧＰＳの絶対位置のオフセット誤差を低減したり解消したりすることができる。
ＧＰＳ衛星から提供される絶対位置のオフセット誤差は、例えばＧＰＳ衛星の配置や大気圧変動や、当日の電離層や対流圏の状態などに依存するものであるから、少なくとも分単位の短い期間中においてはこれらは略一定の値を示す。このため、秒単位の短い観測期間における自車両の絶対位置を、たとえ多件数平均化処理したところで、この様なオフセット誤差を低減または解消することはできない。
【００３０】
しかしながら、これらのオフセット誤差も長期的に見れば所謂ランダムノイズであるため、本発明の第３の手段によれば、上記の環境特徴記憶装置に、長期間に渡る同一不動点の絶対位置を多数記録したり、或いは等価のことであるが、長期間に渡って平均化処理された同一不動点の絶対位置を記録したりして、更に、自車両において実時間で検出された当該不動物の絶対位置を、当該環境特徴記憶装置が有する位置データの中から上記の検索照合手段を使って求めれば、ＧＰＳ衛星から提供される絶対位置のオフセット誤差を効果的に低減させたり、解消したりすることが可能となる。
【００３１】
また、自車両によって高速で走行したい場合などには、上記の測位装置によって道路周辺の同一の不動物が観測できている期間中に、ＧＰＳによって自車両の絶対位置を受信できる回数を十分には確保できない場合も有り得る。このため、その様な場合には、自車両の測位装置で実時間で収集した不動物の位置データだけでは、自車両の現在の絶対位置に関して必ずしも十分な測位精度を得ることはできない。しかしながら、上記の環境特徴記憶装置上の不動物の位置情報を利用すれば、十分なＧＰＳ受信回数が得られなかった場合であっても、低速走行時と同等の測位精度を得ることが可能となる。
【００３２】
なお、自車両の運動と絶対位置が正確に定まった時点では、上記の相対的な位置データに基づいて、勿論不動物の絶対位置も正確に知ることができる。また、不動物の絶対位置を先に算出し、それに基づいて自車両の絶対位置を算定する手順を採用しても良い。そして、これらの不動物の絶対位置を所定の二時記憶装置などに記憶しておけば、後から有効に再利用することも可能となる。
即ち、本発明の第４の手段によれば、自車両の測位装置を用いて自ら収集した上記の位置データを利用して、上記の環境特徴記憶装置か又はそれと同様の他の記憶装置に自ら、長期的な観測に基づいた、道路周辺の不動物の位置データのデータベースを構築することが可能となる。したがって、本発明の第４の手段によれば、例えば上記のような環境特徴記憶装置（データベース）を当該自車両において自ら構築することも可能となる。
【００３３】
また、本発明の第５の手段によれば、上記の自車両運動推定部による運動の推定精度をより向上させることが可能となる。
カメラの撮像画像やレーダの受信情報を使って自車両の運動を推定する場合、通常、これらの測位装置によって検出される物体の大半が不動物であると言う仮定の元に、これらの運動は算定される。このため、自車両運動推定部による運動の推定処理を実行する前に、上記の判別手段を用いて検出データ中の移動物と不動物とを分離しておけば、自車両の運動の推定精度やその推定結果の信頼性を効果的に向上させることが可能となる。
また、この様な判別手段を用いれば、検出された移動物の位置や運動などを同時に正確に求めることも可能となる。
【００３４】
また、本発明の第６の手段によれば、必要な計算量を効果的に抑えることができ、所望の平均化処理を最も簡潔に具現することができる。また、自車両位置推定部を具現するプログラムの保守性や拡張性を容易に確保する上でも、この様な平均化処理の数学的定式化は非常に有効である。また、この様な数学的定式化に従えば、自車両の位置推定処理における演算処理のベクトル化率を容易かつ効果的に向上させることができるので、自車両位置推定部のプログラムの処理オーバーヘッドを効果的に削減することもできる。また、カルマンフィルタの演算は、マルコフ連鎖的（再帰的）に実行することができるので、演算に必要となる記憶領域の節約にも有効である。
【発明を実施するための最良の形態】
【００３５】
例えば、上記の測位装置は、レーザレーダ、レーザレンジファインダ、ミリ波レーダ、マイクロ波レーダ、超音波センサ、またはカメラなどを用いて構成することができる。また、カメラの受光帯域は、可視光に限らず近赤・遠赤などの何れでもよく、ステレオカメラのようにして複数台利用してもよい。また、本願発明の測位装置は、これらのセンサなどを任意に組み合わせて構成しても良い。
また、用いるＧＰＳ受信装置は、近年カーナビゲーションシステムなどで一般に用いられている装置で十分であるが、より高精度でロバストなものが搭載可能であればそれらを用いてもよい。
【００３６】
また、不動物の特徴や位置を把握するために収集する上記の位置データとしては、例えばレンジプロファイル（周辺物体までの距離情報）、反射強度マップ、静止物マップ、移動物の位置や運動、背景パターン情報、オプティカルフロー、エッジ情報などを用いても良いし、また、Haar-like 特徴、SIFT特徴、カラー特徴やそれらのデータマップ（空間分布情報）などを用いても良い。また、これらの情報は、任意に組み合わせたり、或いは任意に選択、統合、または分析して用いても良い。
【００３７】
以下、本発明を具体的な実施例に基づいて説明する。
ただし、本発明の実施形態は、以下に示す個々の実施例に限定されるものではない。
【実施例１】
【００３８】
図１に、本実施例１の自車両位置推定装置１００の論理的構造を示す。この自車両位置推定装置１００は、自車両の絶対位置を取得する自車両位置取得装置１１０（ＧＰＳ受信装置）と、走行中の道路周辺における物体の位置情報（環境特徴が含まれる）を取得する環境特徴取得装置１２０（測位装置）と、検出された環境特徴から自車両の運動（速度ベクトルなど）を推定する自車両運動推定部１４０と、推定された自車両運動および自車両の絶対位置に基づいて自車両位置を推定する自車両位置推定部１３０を有している。
更に、自車両位置推定部１３０は、自車両取得位置評価部１３１と自車両位置評価部１３２から構成されている。また、自車両運動推定部１４０は、環境特徴評価部１４１と自車両運動評価部１４２から構成されている。そして、これらの各評価部（１３１，１３２，１４１，１４２）は、それぞれ一次記憶装置１５０にアクセスすることができる。
【００３９】
上記の自車両位置取得装置１１０は、例えば近年カーナビゲーションシステムなどで利用されている一般のＧＰＳ受信装置などにより具現することができる。
図２に、上記の環境特徴取得装置１２０（測位装置）の概念的な構成図を例示する。この環境特徴取得装置１２０は、ミリ波レーダ１２１、レーザレーダ１２２、超音波センサ１２３、カメラ１２４などの周辺監視センサと、これらのセンサから得られた信号を単独または任意に組み合わせて処理する各信号処理部（１２０ａ〜１２０ｋ）を有する。そして、これらの各センサによって取得できる情報は勿論異なるため、取得された信号から抽出可能な各環境特徴の性質なども当然ながら異なる。
【００４０】
例えば、レンジプロファイルとは各方向に存在する物体までの距離情報であり、反射強度マップとは受信した電波や光の強度を３次元空間中に投影したものである。また、Haar-like 特徴やSIFT特徴は主に画像中の輝度パターンに基づく特徴点である。いずれの特徴もそれ自身で必ずしも当該物体がどの様な種類の物であるかを識別するに足る情報とは限らないが、検出されたこれらの特徴の位置や分布は観測した場所に固有であると考えられるため、これらの環境特徴は、自車両の位置や運動を推定するための手掛かりとなる。
【００４１】
また、図示する様な位置に選択／統合／分析部１２８を設けるなどして、上記の異種類の環境特徴を選択したり統合したりして新たに必要な情報を生成したり、或いは、主成分分析などを利用してそれらの環境特徴を別の特徴量に変換したりしてもよい。また、更に、これらの特徴の形状や分布などを例えば部分空間法などを利用して随時学習する様にしてもよい。
【００４２】
以下、本実施例１の自車両位置推定装置１００（図１）の動作について具体的に説明する。自車両取得位置評価部１３１は、自車両位置取得装置１１０（当該ＧＰＳ受信装置）の性能や、例えばＧＰＳ衛星の配置などに依存するＧＰＳ信号の取得状況などに基づいて、そのＧＰＳ信号で与えられる自車両の絶対位置の精度（例：分散）などを求める。
環境特徴評価部１４１は、環境特徴取得装置１２０によって取得した環境特徴の種類や、相対位置に関する情報の精度や信頼度を算定する。
これらの各位置を表現するために用いる座標は、固定座標系での二次元座標でもよいし、緯度経度でもよく、その表現方法はそれらの位置を一意に決定できる形式であれば任意でよい。
【００４３】
自車両運動評価部１４２は、環境特徴評価部１４１にて評価された環境特徴の位置情報の時間変化に基づいて、その変化が観測された期間に移動した自車両の運動（速度ベクトルなど）を求める。また、同時にその運動の精度や信頼度を算定する。
自車両位置評価部１３２では、自車両位置の観測値、環境特徴の位置情報、推定した自車両運動などに基づいて自車両の絶対位置を求める。また、同時にその運動の精度や信頼度を算定する。このとき、自車両の絶対位置と環境特徴の相対位置に基づいて、環境特徴の絶対位置やその精度などを同時に求めてもよい。各評価部（１３１，１３２，１４１，１４２）より算出された絶対位置や相対位置やそれらの精度や信頼度などの情報は、一次記憶装置１５０で一定期間のみ保存される。ただし、この期間は、時刻管理してもよいし、一次記憶装置１５０の使用状況に基づいて管理してもよい。
【００４４】
この様に、上記の各評価部（１３１，１３２，１４１，１４２）では、算出すべき自車両や不動物などの位置（相対位置または絶対位置）の他にも、それらの位置に関する確からしさ（精度または信頼度）も算定する。この確からしさは、センサの分解能や検出状況や、環境特徴の種別や更新回数などに基づいて算定することができる。
したがって、図１の一次記憶装置１５０には、例えば、ＧＰＳ受信信号から得られる自車両の絶対位置とその位置に関する確からしさ等が一時的に保存され、また、外界センサ（ミリ波レーダ１２１、レーザレーダ１２２、超音波センサ１２３、カメラ１２４）から得られる自車両に対する環境特徴の相対位置、及びその位置に関する特徴や確からしさ等も一時的に保存される。
【００４５】
この様な構成に従えば、例えばカルマンフィルタなどの様な再帰的なアルゴリズムを用いて、マルコフ連鎖的に自車両の絶対位置やその確からしさなどの最適解を実時間処理によって逐次算出することができる。例えば、自車両や環境特徴の各絶対位置やその確からしさは、一次記憶装置１５０に保存された上記の１時刻前の（即ち１制御周期過去の）上記のデータ（各絶対位置やその確からしさなど）と、評価部１３１、１４１から出力される現在の位置データによって算出することができる。したがって、自車両や環境特徴の各絶対位置やその確からしさを推定するためには、少なくとも、一時刻前の位置や確からしさを一次記憶装置１５０上に保持していればよい。また環境特徴に関する情報は、それらが観測可能である期間だけ保持していれば十分である。
【００４６】
なお、図１の環境特徴評価部１４１は、環境特徴を有する物体が静止物であるのか移動物であるのかについて識別する手段（即ち、請求項５の判別手段）も有する。自車両の運動を精度よく推定するには、周囲の静止物の位置を観測し続ける方法が有効なため、この様な判別手段では専ら、測位装置によって検出される物体の大半が不動物であると言う仮定が利用される。また、この識別処理には、一次記憶装置１５０に記憶されている一時刻前の観測データなどを利用する。そして、この様な識別手法に基づいて分離された静止物に属する情報のみを利用することによって、自車両の運動推定精度をより向上させることができる。
【００４７】
図３に自車両位置推定装置１００の制御処理手順を例示する。以下、この制御処理手順の動作原理と具体的構成について説明する。
１．動作原理と作用概要
（１）同一の不動点に位置変換可能な多数の移動点に関してＧＰＳ信号を受信し、当該ＧＰＳ信号によって与えられる各移動点の絶対位置を、当該不動点の位置座標（絶対位置）に位置変換後、その絶対位置を平均化処理することにより、ＧＰＳ信号が示す絶対位置に含まれるランダムノイズを排除することができる。特にここでは、道路周辺の走行環境中にある継続的に観測された不動物（環境特徴）の絶対位置の最適解を逐次リアルタイムで再計算する手法を用いる。
【００４８】
（２）詳細な処理方式は、例えば拡張カルマンフィルタなどを用いることによって、再帰的に構成することも可能である。特にこの場合、マルコフ連鎖的な情報処理を逐次実行するだけで、自車両の絶対位置を推定することができるので、一次記憶装置１５０に一時的に蓄えるべき情報の量を極めて効果的に抑制することができる。
【００４９】
２．具体的な処理手順
以下、図３を用いて、自車両位置推定装置１００の処理手順について具体的に説明する。
制御ループの最初のステップ１１００では、不動物に分類された環境特徴の絶対位置とその測位精度について予測する。この予測値は、直近過去（前回の制御周期）に推定された絶対位置から求めることができる。ただし、ここでは、不動物の環境特徴が選択されているため、当該ステップ１１００で予測される今回の当該環境特徴の予測値（今回の制御周期における絶対位置）は基本的には変化しない。
【００５０】
そして、次のステップ１１１０では、自車両の絶対位置とその測位精度について予測する。この予測値は、直近過去（前回の制御周期）に推定された、不動物や自車両の絶対位置と自車両の運動の推定値とこれらに関連する推定精度などから求めることができる。
ただし、過去数回分の自車両の運動を加味してもよい。これらの逐次演算処理で意識すべきこの様な観測期間の長さ（ウィンドサイズ）は、任意に設定することができ、勿論、今回取得した観測データ（確からしさを含む）と、１回分の過去の観測データや推定データだけを取り扱う様に、当該アルゴリズムを再帰的（マルコフ連鎖的）に構成することもできる。
【００５１】
ステップ１２００では、非同期で動作中の図２の環境特徴取得装置１２０によって、自車両周辺の環境特徴が取得されているか否かについて確認する。
ステップ１２１０では、取得した環境特徴の中から静止物に属する特徴を抽出し、その相対位置や確からしさを算定する。この静止物の抽出処理には、請求項５に記載の判別手段が有用である。ただし、これらの処理は、図２の選択／統合／分析部１２８において、予め非同期に実行しておいてもよい。
そして、次のステップ１２２０では、その相対位置の前回観測時からの時間変化を求めることにより、現在の自車両の運動（速度ベクトルやヨー角速度など）とその確からしさを推定する。
【００５２】
ステップ１３００／ステップ１４００では、ＧＰＳ信号の受信の有無を確認し、ステップ１３１０／１４１０では、そのＧＰＳ信号が与える自車両の絶対位置とその確からしさを算定する。
ステップ１３２０では、ＧＰＳ受信装置（自車両位置取得装置１１０）を用いて取得した自車両の絶対位置と、測位手段（環境特徴取得装置１２０）を用いて取得した環境特徴の自車両に対する相対位置から、その環境特徴の絶対位置とその確からしさを算出する。ただし、新たなＧＰＳ信号が得られていない場合には、ステップ１３３０において、過去の自車両の絶対位置に基づいて予測した、当該環境特徴の絶対位置とその確からしさの各予測値を代用する。
【００５３】
その後ステップ１５００では、ステップ１１００で求めた予測値（環境特徴の絶対位置とその確からしさ）と、ステップ１３２０またはステップ１３３０で与えられる環境特徴の絶対位置とその確からしさに基づき、これら双方の確からしさを加味した上で、今回の制御周期で改めて観測された当該環境特徴の絶対位置とその確からしさを更新する。この処理によって、観測された環境特徴の絶対位置／精度と過去から予測された環境特徴の絶対位置／精度とを重畳することで平均化され、誤差分散が小さくなる。
その後ステップ１６００では、更新された環境特徴の絶対位置や精度と、周辺監視センサ（環境特徴取得装置１２０）の精度から、自車両の絶対位置とその精度を算出する。この様に、ステップ１５００で求めた算定結果をステップ１６００における推定処理で効果的に利用することで、従来よりも高精度に自車両の絶対位置を求めることができる。
【００５４】
一方、上記のステップ１４００で自車両の絶対位置が新たに得られたと判定された場合には、ステップ１６１０において、現在の自車両の絶対位置とその確からしさについてのみ、ステップ１１１０で求めた予測値とステップ１４１０で求めた観測値に基づいて更新する。また、上記のステップ１４００で自車両の絶対位置が新たには得られていないと判定された場合には、ステップ１６２０において、現在の自車両の絶対位置とその確からしさについてのみ、ステップ１１１０で求めた予測値のみに基づいて更新する。即ち、何れの取得装置からも新たな情報が得られなかった制御周期においては、ステップ１１１０で求めた予測値（現在の自車両の絶対位置とその確からしさ）が、そのままその推定値として、ステップ１６２０によって出力される。
【００５５】
なお、以上の推定演算処理は、例えば拡張カルマンフィルタなどを用いて構成することもできる。その様な数学的な定式化は、例えば以下に示す共分散行列Σなどを用いて行われ、これによって、上記の図３の処理手順と等価の作用を導くことができる。
以下、外界センサ（測位手段）として、レーザーレーダを用いた場合の拡張カルマンフィルタの構成について説明する。
（拡張カルマンフィルタ）
【数１】
2007322391.tif 000003
【００５６】
ここでは、推定すべき位置パラメータｘ（：推定すべき状態ベクトルｘ）を上記の式（１）の様に設定し、観測値ｙ（測定ベクトルｙ）は上記の式（２）に従って設定する。ただし、ここでは、南北方向の北向きをＺ軸方向の正の向きとし、東西方向の東向きをＸ軸方向の正の向きとし、この絶対座標系における自車両の絶対位置を（Ｘvo，Ｚvo）で表す。また、Ｚ軸に対する自車両の向き（方位角）をφv 、自車両の速度ベクトルをＶv 、自車両のヨー角速度をωv とし、ｉ番目の不動物の絶対位置を（Ｘfi，Ｚfi）で表す。式（１）の位置パラメータｘは、これらの変数からなる。
【００５７】
また、この様な位置パラメータｘを推定するために観測される観測値ｙは、以下の変数からなる。即ち、ＧＰＳ信号が与える自車両の絶対位置（Ｘg ，Ｚg ）と、ＧＰＳ信号が与える自車両の速度ベクトルをＶg と、レーザーレーダによって測位されたｉ番目の環境特徴の自車両に対する相対位置（ｒsfi ，φsfi ）からなる。ただし、ｒsfi は自車両からそのｉ番目の環境特徴までの距離で、φsfi は自車両の正面方向に固定した相対座標軸から計ったそのｉ番目の環境特徴の方位角である。式（２）の観測値ｙは、これらの変数からなる。
そして、以下の（手順ａ）〜（手順ｃ）を周期的に繰り返すことによって、リアルタイムで逐次、上記の位置パラメータｘを算定することができる。
【００５８】
（演算手順）
（手順ａ）一時刻前(t=k-1) において推定された位置パラメータおよびそれらの分散から，現在（t=k ）の位置および分散を予測する。なお、本手順ａは、位置パラメータｘの予測を行う上記の式（３）及び、共分散行列Σの予測を行う上記の式（４）に対応する。また、本手順ａは、図３のフローチャートにおいては、ステップ１１００やステップ１１１０などに対応している。
【００５９】
（手順ｂ）推定パラメータの予測誤差（共分散行列Σk/k-1 として定義）や観測ノイズに基づいて，カルマンゲインを計算する。なお、本手順ｂは、カルマンゲインＫを算出する上記の式（５）に対応する。
【００６０】
（手順ｃ）算出したカルマンゲインに基づいて観測値と予測値を統合し，現在の位置パラメータとそれらの分散を推定する。なお、本手順ｃは、上記の位置パラメータｘを推定する上記の式（６）及び、共分散行列Σを推定する上記の式（７）に対応する。
なお、手順ｂや手順ｃは共に、図３のフローチャートにおいては、ステップ１５００やステップ１６００やステップ１６１０やステップ１６２０などに対応している。
【００６１】
拡張カルマンフィルタを用いて定式化される以上の様な演算手順は、上記の図３の処理手順と等価の演算結果を導く。言い換えれば、上記の図３の処理手順は、拡張カルマンフィルタを用いて表現されるこの様な演算手順と等価の演算結果を導く様に構成することができる。
【００６２】
以上の様な処理手順に従えば、図３のステップ１７００の出力情報（位置パラメータｘ，共分散行列Σ）により、現在の自車両の絶対位置と運動（速度ベクトルやヨー角速度）とそれらの精度をリアルタイムで高精度に得ることができる。
ここで特に注目すべき点は、従来より多用されてきた例えば交差点、電柱、リフレクタ、道路標識、路面表示などといった特定のランドマークは自車両の周囲に存在しない場合もあるが、上記のステップ１２１０で検出される不動物、即ち図２に例示した各種の環境特徴は、外界センサ（１２１〜１２４）からの観測信号の中にほぼ確実に存在することである。さらに、１回のセンシングで得られるそれらの観測信号の中にこれらの環境特徴は非常に多く存在する。このため、上記の自車両位置推定装置１００を車載すれば、特に道路環境などに左右されることなく高精度な自車両の位置推定を行うことができる。
また同時に、不動物の特徴点の数やその位置情報が十分に得られれば自車両の運動の推定精度も向上するため、この作用に基づいて、より高精度に自車両の絶対位置を推定することができる。
【００６３】
図４に、以上の自車両位置推定装置１００に基づく効果（シミュレーション結果）を例示する。このグラフは、測位装置（環境特徴取得装置１２０）を用いて環境特徴の観測に掛けた時間と、自車両の進行方向における絶対位置の測位精度との関係を示すものであり、縦軸には得られた測位精度を誤差分散の平方根の２倍の値（２σ）で示した。
このシミュレーションでは、ＧＰＳ衛星からのＧＰＳ信号の受信を妨げる程の高いビルが周囲にないことを想定し、また、ＧＰＳ単独での測位精度を２σ＝５ｍと仮定した。また、環境特徴を検出するセンサにはレーザーレーダを用い、これによって常時２点〜４点の不動点の相対位置が、２σ＝５０ｍｍ程度の誤差範囲内で検出できるものと仮定した。また、本シミュレーションにおけるＧＰＳ信号の受信周期は１回／秒とし、レーザーレーダによる観測周期は２０回／秒とした。また、車両の走行速度は、４０ｋｍ／ｈを仮定し、レーザーレーダの測位可能な距離範囲を前方５０ｍまでとし、測位可能な角度範囲は、前方正面を０°として前方左４０°〜前方右４０°とした。また、図３のフローチャートに例示した演算処理をより詳細に実現（シミュレート）するに当たっては、これらの演算処理に相当する一般的な拡張カルマンフィルタと略同等の再帰的なアルゴリズムを用いた。
この様なシミュレーション結果より、本自車両位置推定装置１００を用いた場合、ＧＰＳ単独で自車両の絶対位置を求める従来の場合よりも、格段に高い測位精度が得られることが分かる。
【実施例２】
【００６４】
上記の実施例１の自車両位置推定装置１００によると、ＧＰＳで得られる自車両の絶対位置にオフセット誤差が含まれていない場合には、高精度な自車両位置推定を実現できるが、ＧＰＳが与える自車両の絶対位置の信号の中にオフセット誤差が含まれているような場合には、そのオフセット誤差を取り除くことが難しい。そこで、これらのオフセット誤差を低減したり解消したりすることも可能な自車両位置推定装置が期待される。
図５に本実施例２の自車両位置推定装置２００の論理的構造を示す。この自車両位置推定装置２００は、実施例１の自車両位置推定装置１００の拡張変形例に相当するものであり、図中の環境特徴記憶装置１６０には、絶対位置が既知である幾つかの環境特徴（不動物）の位置データを保持する地図データベースが予め構築されている。このため、この自車両位置推定装置２００によれば、上記のオフセット誤差を低減したり解消したりすることも可能となる。
【００６５】
この自車両位置推定装置２００の諸動作における最も大きな特徴は、環境特徴取得装置１２０によって取得した環境特徴と、環境特徴記憶装置１６０が有する地図データベース上にある、絶対位置が既知の環境特徴とを、請求項３の検索照合手段に相当する手段を備える環境特徴対応評価部１７０によって照合し、この照合結果に基づいて、自車両の絶対位置を補正する点にある。
【００６６】
図６−Ａに、自車両位置推定装置２００の適用シーンを例示する。この写真は、左右両路側にそれぞれ歩道およびガードレールを有する道路の横断歩道上を１歩行者が横断しているシーンを示すものである。また、図６−Ｂのデータは、図２のレーザーレーダ１２２と同等の装置を使って、図６−Ａの自車両走行シーンにおいて検出したもので、自車両前方のレンジプロファイル（不動物の位置を表す鳥瞰図）を示している。即ち、図６−Ｂ中のｚ軸の正の向きは自車両の前向きを、ｘ軸の正の向きは自車両の右向きを示している。また、図５の環境特徴記憶装置１６０には、例えばこの様なデータ（レンジプロファイル）を記憶しておくことができ、この様なデータのｘ軸座標やｚ軸座標には、前述のＸＺ座標（東西南北座標）と等価の絶対座標を与えることができる。また、この様なレンジプロファイルデータを多数回、測定日時を替えて収集し、それらの位置データを適当に平均化処理すれば、これによって、前述のオフセット誤差を排除することができるため、このレンジプロファイルデータの絶対座標で表示される各不動物の絶対位置には、高い信頼性と例えば１ｍ程度の十分に高い精度を持たせることができる。また、上記のデータ収集回数が十分に大きな回数には至っていない場合であっても、その収集回数やデータの確からしさを常時保持、更新する様にしていれば、実時間でＧＰＳ信号が与える自車両の絶対位置に含まれているオフセット誤差を効果的に低減することができる。
【００６７】
したがって、図２のレーザーレーダ１２２を有する自車両位置推定装置２００によれば、略同等のレンジプロファイルを走行中に取得し、それを環境特徴記憶装置１６０に記憶されている当該データとリアルタイムで照合することによって、高速走行時においても実時間で高精度に自車両の絶対位置を検知することが可能となる。
【００６８】
なお、当該装置２００においても、先の自車両位置推定装置１００と同様に車両運動推定部１４０が備える前述の識別処理（請求項５の判別手段）によって、自車両運動の高精度推定が可能であるので、上記の様な方式に従えば、比較的長い距離間隔で何かしらの環境特徴の絶対位置やその確からしさが環境特徴記憶装置１６０に登録されている限り、自車両の絶対位置の補正を的確に行うことが可能となる。
【００６９】
このため、この自車両位置推定装置２００を用いれば、比較的高速に走行する場合においても、自車両の絶対位置に対する高い精度と高い信頼性を得ることが可能である。
また、環境特徴記憶装置１６０には、上記の様に比較的長い距離間隔で何かしらの環境特徴の絶対位置やその確からしさが登録されていればよいため、長期的に環境特徴を記憶するデータベース（環境特徴記憶装置１６０）の作成コストや車載コストについても、従来よりも格段に低く抑えることができる。
【００７０】
〔その他の変形例〕
本発明の実施形態は、上記の形態に限定されるものではなく、その他にも以下に例示される様な変形を行っても良い。この様な変形、拡張、または応用によっても、本発明の作用に基づいて本発明の効果を得ることができる。
【００７１】
例えば、実施例１で示したように、自車両の予測値は、過去数回分の運動から随時推定してもよいが、例えば前述の特許文献２などにも例示がある様に、予め運動モデルを設定し、そのモデルに基づいて予測する様にしてもよい。
また、自車両と環境特徴の位置／精度の更新は、カルマンフィルタやパーティクルフィルタなどを用いて逐次実行する様にしてもよいし、一定時間分の過去の観測値を用いて最適な解を求めてもよいし、或いは、逐次推定した推定値を再度、全データベース単位で更新しなおしてもよい。
【００７２】
また、請求項５の判別手段を用いれば、前述の実施例１のように、取得可能な環境特徴のうち、静止物のみを選択することにより自車両の運動推定精度の劣化を防止することができるが、請求項５の判別手段を用いれば、その他にも、移動物と自車両の相対関係、静止物と自車両の相対関係、自車両の絶対位置や運動を基にして、移動物の運動をも同時に高精度に推定することもできる。
【００７３】
例えば、先行車両の自車両に対する相対的な位置または相対的な運動が正確に把握できていて、その先行車両の運動パラメータ（位置、速度、推定精度など）も同時に算出可能であれば、拡張カルマンフィルタなどを利用して、先行車両と自車両の位置や運動を同時に推定することも可能となる。また、このとき、更に、周辺監視センサの検出精度やそれに基づく移動物（先行車両）の運動予測が適正であれば、自車両の運動推定精度をさらにいっそう向上させることができる。
【００７４】
なお、記憶容量に余裕があれば、自車両の絶対位置やその測位精度等を算定した後においても引き続き、道路周辺の不動物の絶対位置やその測位精度等の評価結果を、所定の一次記憶装置または二時記憶装置に保持することは何ら問題ない。また、これらの保存された評価結果の履歴は、例えば道路状況の推定などのその他のアプリケーションにも流用することも可能である。
【産業上の利用可能性】
【００７５】
本発明は、例えば、地上のナビゲーションシステムや、車載用のオートクルーズ制御システムなどに有用である。また、これらの適用は、四輪車両に限定されるものではなく、ロボットや二輪車両などの移動体において利用することも勿論可能である。
【図面の簡単な説明】
【００７６】
【図１】実施例１の自車両位置推定装置１００の論理的構造を示す制御ブロック図
【図２】環境特徴取得装置１２０の構成例を例示するデータフロー図
【図３】自車両位置推定装置１００の制御処理手順を例示するフローチャート
【図４】自車両位置推定装置１００に基づく効果を例示するグラフ
【図５】実施例２の自車両位置推定装置２００の論理的構造を示す制御ブロック図
【図６−Ａ】自車両位置推定装置２００の適用シーンを例示する写真
【図６−Ｂ】環境特徴記憶装置１６０に記憶され得る１データ例
【符号の説明】
【００７７】
１００、２００  ：  自車両位置推定装置
１１０  ：  自車両位置取得装置（ＧＰＳ受信装置）
１２０  ：  環境特徴取得装置（測位装置）
１３０  ：  自車両位置推定部
１４０  ：  自車両運動推定部
１６０  ：  環境特徴記憶装置
１７０  ：  環境特徴対応評価部（検索照合手段）
【図１】
2007322391.tif 000004
【図２】
2007322391.tif 000005
【図３】
2007322391.tif 000006
【図４】
2007322391.tif 000007
【図５】
2007322391.tif 000008
【図６−Ｂ】
2007322391.tif 000009
【図６−Ａ】
2007322391.tif 000010