特許 7155284 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2022-10-07

(45)【発行日】2022-10-18

(54)【発明の名称】計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体

(51)【国際特許分類】

   G01C 21/28 20060101AFI20221011BHJP

【ＦＩ】

G01C21/28

【請求項の数】 10

(21)【出願番号】P 2020553079

(86)(22)【出願日】2019-10-07

(86)【国際出願番号】 JP2019039479

(87)【国際公開番号】W WO2020085062

(87)【国際公開日】2020-04-30

【審査請求日】2021-03-05

(31)【優先権主張番号】P 2018200067

(32)【優先日】2018-10-24

(33)【優先権主張国・地域又は機関】JP

(73)【特許権者】

【識別番号】000005016

【氏名又は名称】パイオニア株式会社

(74)【代理人】

【識別番号】100107331

【弁理士】

【氏名又は名称】中村 聡延

(72)【発明者】

【氏名】加藤 正浩

【審査官】吉村 俊厚

(56)【参考文献】

【文献】特開２０１８－０３６０６７（ＪＰ，Ａ）

【文献】特開２０１８－０５９７４４（ＪＰ，Ａ）

【文献】特開２０１８－１１６０１４（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｃ ２１／２８

(57)【特許請求の範囲】

【請求項1】

計測部による地物の計測結果を取得する第１取得部と、
地図データに含まれる前記地物の地物情報を取得する第２取得部と、
計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部と、を有する計測精度算出装置。

【請求項2】

前記算出部は、前記差分が大きいほど、前記計測精度が低い精度情報を生成する請求項１に記載の計測精度算出装置。

【請求項3】

前記算出部は、前記差分と、移動体の進行方向と、前記地物情報が示す前記地物の法線方向とに基づき、前記移動体の進行方向と横方向のそれぞれに対する前記地物の計測精度を示す精度情報を生成する請求項１または２に記載の計測精度算出装置。

【請求項4】

前記算出部は、前記進行方向と前記地物の法線方向との角度差が大きいほど、前記横方向に対する前記計測精度への前記差分の影響度合いを低くし、前記角度差が小さいほど、前記進行方向に対する前記計測精度への前記差分の影響度合いを低くする請求項３に記載の計測精度算出装置。

【請求項5】

予測された自己位置を示す予測位置情報を取得する第３取得部と、
前記精度情報に基づき、前記予測された自己位置を補正する補正部と、
をさらに備える請求項１～４のいずれか一項に記載の計測精度算出装置。

【請求項6】

前記補正部は、移動体から前記地物までの前記計測部による計測距離と、前記地物情報に含まれる前記地物の位置情報に基づき予測された前記移動体から前記地物までの予測距離と、の差分値により前記予測された自己位置を補正する場合の前記差分値に対する利得を、前記精度情報に基づき決定する請求項５に記載の計測精度算出装置。

【請求項7】

計測部による地物の計測結果を取得する第１取得部と、
地図データに含まれる前記地物の地物情報を取得する第２取得部と、
予測された自己位置を示す予測位置情報を取得する第３取得部と、
計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記予測された自己位置を補正する補正部と、
を備える自己位置推定装置。

【請求項8】

計測精度算出装置が実行する制御方法であって、
計測部による地物の計測結果を取得する第１取得工程と、
地図データに含まれる前記地物の地物情報を取得する第２取得工程と、
計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出工程と、
を有する制御方法。

【請求項9】

コンピュータが実行するプログラムであって、
計測部による地物の計測結果を取得する第１取得部と、
地図データに含まれる前記地物の地物情報を取得する第２取得部と、
計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部
として前記コンピュータを機能させるプログラム。

【請求項10】

請求項９に記載のプログラムを記憶した記憶媒体。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、計測部による計測の精度を算出する技術に関する。

【背景技術】

【0002】

従来から、車両の進行先に設置される地物をレーダやカメラを用いて検出し、その検出結果に基づいて自車位置を校正する技術が知られている。例えば、特許文献１には、計測センサの出力と、予め地図上に登録された地物の位置情報とを照合させることで自己位置を推定する技術が開示されている。また、特許文献２には、カルマンフィルタを用いた自車位置推定技術が開示されている。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１３－２５７７４２号公報

【文献】特開２０１７－７２４２２号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

自己位置推定処理において計測対象となる地物が他の地物と隣接している場合には、計測対象の地物と共に他の地物が計測データに含まれてしまい、計測結果の精度（正確性）が低下してしまうことがある。また、計測対象となる地物の一部にオクルージョンが発生した場合にも同様に、本来計測されるべき地物の一部が計測できなくなるため、計測結果の精度が低下してしまうことがある。

【0005】

本発明は、上記のような課題を解決するためになされたものであり、計測部による地物の計測精度を好適に算出可能な計測精度算出装置及び自己位置推定装置を提供することを主な目的とする。

【課題を解決するための手段】

【0006】

請求項に記載の発明は、計測精度算出装置であって、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部と、を有する。

【0007】

また、請求項に記載の発明は、自己位置推定装置であって、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、予測された自己位置を示す予測位置情報を取得する第３取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記予測された自己位置を補正する補正部と、を備える。

【0008】

また、請求項に記載の発明は、計測精度算出装置が実行する制御方法であって、計測部による地物の計測結果を取得する第１取得工程と、地図データに含まれる前記地物の地物情報を取得する第２取得工程と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出工程と、を有する。

【0009】

また、請求項に記載の発明は、コンピュータが実行するプログラムであって、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部として前記コンピュータを機能させる。

【図面の簡単な説明】

【0010】

【図1】運転支援システムの概略構成図である。

【図2】車載機の機能的構成を示すブロック図である。

【図3】地図データベースのデータ構造の一例を示す。

【図4】状態変数ベクトルを２次元直交座標で表した図である。

【図5】予測ステップと計測更新ステップとの概略的な関係を示す図である。

【図6】自車位置推定部の機能ブロックを示す。

【図7】信頼度情報のグラフを示す。

【図8】オクルージョンが発生した場合のライダのレーザ光の照射範囲を概略的に示した図である。

【図9】ランドマークとなる地物に隣接して他の地物が存在する場合のライダのレーザ光の照射範囲を概略的に示した図である。

【図10】２次元のワールド座標系及び車両座標系により表された車両とランドマークとの位置関係を示した図である。

【図11】車両から見てランドマークがほぼ正面を向いているときの信頼度情報の２次元グラフである。

【図12】車両から見てランドマークがやや横向きに近いときの信頼度情報の２次元グラフである。

【図13】自車位置推定処理を示すフローチャートである。

【図14】変形例１に係る信頼度情報のグラフを示す。

【図15】変形例２に係る信頼度情報のグラフを示す。

【発明を実施するための形態】

【0011】

本発明の好適な実施形態によれば、計測精度算出装置は、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部と、を有する。この態様により、計測精度算出装置は、計測対象の地物にオクルージョンが発生して一部の計測データが取得できなかった場合又は計測対象の地物に隣接する他の地物の計測データを計測対象の地物の計測データの一部として取得した場合のいずれにおいても、計測部による地物の計測精度を示す精度情報を的確に算出することができる。

【0012】

上記計測精度算出装置の一態様では、前記算出部は、前記差分が大きいほど、前記計測精度が低い精度情報を生成する。このようにすることで、計測対象の地物にオクルージョンが発生して一部の計測データが取得できなかった場合又は隣接地物の計測データを計測対象の地物の計測データの一部として取得した場合のいずれにおいても、計測精度が低いことを示す精度情報を好適に生成することができる。

【0013】

上記計測精度算出装置の一態様では、前記算出部は、前記差分と、移動体の進行方向と、前記地物情報が示す前記地物の法線方向とに基づき、前記移動体の進行方向と横方向のそれぞれに対する前記地物の計測精度を示す精度情報を生成する。一般に、移動体の進行方向に対する地物の向きによって、計測誤差が生じにくい方向と計測誤差が生じやすい方向とがそれぞれ生じる場合がある。従って、この態様によれば、計測精度算出装置は、移動体の進行方向と横方向のそれぞれに対する地物の計測精度を示す精度情報を好適に生成することができる。

【0014】

上記計測精度算出装置の他の一態様では、前記算出部は、前記進行方向と前記地物の法線方向との角度差が大きいほど、前記横方向に対する前記計測精度への前記差分の影響度合いを低くし、前記角度差が小さいほど、前記進行方向に対する前記計測精度への前記差分の影響度合いを低くする。ここでの地物の法線方向は、地物が形成する平面に対し垂直となる地物の正面方向及び背面方向のうち、移動体の進行方向と９０度差以内となる方向を指すものとする。この場合、移動体の進行方向と地物の法線方向との角度差が大きいほど、地物は移動体に対して横向きとなるため、移動体の横方向に対する計測誤差が生じにくくなる。一方、移動体の進行方向と地物の法線方向との角度差が小さいほど、地物は移動体に対して正面向きとなるため、移動体の進行方向に対する計測誤差が生じにくくなる。よって、計測精度算出装置は、この態様により、移動体の進行方向と横方向のそれぞれに対する地物の計測精度を的確に表す精度情報を生成することができる。

【0015】

上記計測精度算出装置の他の一態様では、計測精度算出装置は、予測された自己位置を示す予測位置情報を取得する第３取得部と、前記精度情報に基づき、前記予測された自己位置を補正する補正部と、をさらに備える。この態様により、計測精度算出装置は、生成した精度情報を反映した自己位置推定を好適に実行することができる。

【0016】

上記計測精度算出装置の他の一態様では、前記補正部は、移動体から前記地物までの前記計測部による計測距離と、前記地物情報に含まれる前記地物の位置情報に基づき予測された前記移動体から前記地物までの予測距離と、の差分値により前記予測された自己位置を補正する場合の前記差分値に対する利得を、前記精度情報に基づき決定する。この態様により、計測精度算出装置は、予測された自己位置を計測距離と予測距離との差分値により補正する場合に精度情報に基づきその利得を調整することで、不正確な補正を防止して自己位置推定精度を好適に向上させることができる。

【0017】

本発明の他の好適な実施形態によれば、自己位置推定装置であって、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、予測された自己位置を示す予測位置情報を取得する第３取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記予測された自己位置を補正する補正部と、を備える。上述の差分は、計測対象の地物にオクルージョンが発生して一部の計測データが取得できなかった場合又は計測対象の地物に隣接する他の地物の計測データを計測対象の地物の計測データの一部として取得した場合のいずれにおいても発生する。従って、自己位置推定装置は、この態様により、予測された自己位置を上述の差分を考慮して好適に補正して最終的な自己位置を決定することができる。

【0018】

本発明の他の好適な実施形態によれば、計測精度算出装置が実行する制御方法であって、計測部による地物の計測結果を取得する第１取得工程と、地図データに含まれる前記地物の地物情報を取得する第２取得工程と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出工程と、を有する。計測精度算出装置は、この制御方法を実行することで、計測対象の地物にオクルージョンが発生して一部の計測データが取得できなかった場合又は計測対象の地物に隣接する他の地物の計測データを計測対象の地物の計測データの一部として取得した場合のいずれにおいても、計測部による地物の計測精度を示す精度情報を的確に算出することができる。

【0019】

本発明の他の好適な実施形態によれば、コンピュータが実行するプログラムであって、計測部による地物の計測結果を取得する第１取得部と、地図データに含まれる前記地物の地物情報を取得する第２取得部と、計測した地物のサイズと前記地物情報が示す地物のサイズとの差分に基づいて、前記計測部による前記地物の計測精度を示す精度情報を算出する算出部として前記コンピュータを機能させる。コンピュータは、このプログラムを実行することで、計測対象の地物にオクルージョンが発生して一部の計測データが取得できなかった場合又は計測対象の地物に隣接する他の地物の計測データを計測対象の地物の計測データの一部として取得した場合のいずれにおいても、計測部による地物の計測精度を示す精度情報を的確に算出することができる。好適には、上記プログラムは、記憶媒体に記憶される。

【実施例】

【0020】

以下、図面を参照して本発明の好適な実施例について説明する。なお、任意の記号の上に「＾」または「－」が付された文字を、本明細書では便宜上、「Ａ^＾」または「Ａ^－」（「Ａ」は任意の文字）と表す。

【0021】

［運転支援システムの概要］
図１は、本実施例に係る運転支援システムの概略構成である。図１に示す運転支援システムは、車両に搭載され、車両の運転支援に関する制御を行う車載機１と、ライダ（Ｌｉｄａｒ：Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ、または、Ｌａｓｅｒ Ｉｌｌｕｍｉｎａｔｅｄ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）２と、ジャイロセンサ３と、車速センサ４と、ＧＰＳ受信機５とを有する。なお、以後において、「地図」とは、従来の経路案内用の車載機が参照するデータに加えて、ＡＤＡＳ（Ａｄｖａｎｃｅｄ Ｄｒｉｖｅｒ Ａｓｓｉｓｔａｎｃｅ Ｓｙｓｔｅｍ）や自動運転に用いられるデータも含むものとする。

【0022】

車載機１は、ライダ２、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５と電気的に接続し、これらの出力に基づき、車載機１が搭載される車両の位置（「自車位置」とも呼ぶ。）の推定を行う。そして、車載機１は、自車位置の推定結果に基づき、設定された目的地への経路に沿って走行するように、車両の自動運転制御などを行う。車載機１は、道路データ及び道路付近に設けられた目印となる地物に関する情報である地物情報を記憶した地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶する。上述の目印となる地物は、例えば、道路脇に周期的に並んでいるキロポスト、１００ｍポスト、デリニエータ、交通インフラ設備（例えば標識、方面看板、信号）、電柱、街灯などの地物である。そして、車載機１は、この地物情報に基づき、ライダ２等の出力と照合させて自車位置の推定を行う。車載機１は、本発明における「計測精度算出装置」及び「自己位置推定装置」の一例である。尚、地図ＤＢ１０は車載装置１に記憶される代わりに、外部サーバ等の外部の記憶装置に記憶されてもよい。その場合、車載装置１は無線通信等を介して外部の記憶装置から地図ＤＢ１０の少なくとも一部を取得する。

【0023】

ライダ２は、水平方向および垂直方向の所定の角度範囲に対してパルスレーザを出射することで、外界に存在する物体までの距離を離散的に測定し、当該物体の位置を示す３次元の点群情報を生成する。この場合、ライダ２は、照射方向を変えながらレーザ光を照射する照射部と、照射したレーザ光が物体で反射した反射光（散乱光）を受光する受光部と、受光部が出力する受光信号に基づくスキャンデータを出力する出力部とを有する。スキャンデータは、点群データであり、受光部が受光したレーザ光に対応する照射方向と、上述の受光信号に基づき特定される、その照射方向での物体までの距離とに基づき生成される。ライダ２、ジャイロセンサ３、車速センサ４、ＧＰＳ受信機５は、それぞれ、出力データを車載機１へ供給する。ライダ２は、「計測部」の一例である。

【0024】

図２は、車載機１の機能的構成を示すブロック図である。車載機１は、主に、インターフェース１１と、記憶部１２と、入力部１４と、制御部１５と、情報出力部１６と、を有する。これらの各要素は、バスラインを介して相互に接続されている。

【0025】

インターフェース１１は、ライダ２、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５などのセンサから出力データを取得し、制御部１５へ供給する。

【0026】

記憶部１２は、制御部１５が実行するプログラムや、制御部１５が所定の処理を実行するのに必要な情報を記憶する。本実施例では、記憶部１２は、地物情報を含む地図ＤＢ１０を記憶する。図３は、地図ＤＢ１０のデータ構造の一例を示す。図３に示すように、地図ＤＢ１０は、施設情報、道路データ、及び地物情報を含む。

【0027】

地物情報は、地物ごとに当該地物に関する情報が関連付けられた情報であり、ここでは、地物のインデックスに相当する地物ＩＤと、位置情報と、向き情報（法線情報）と、サイズ情報とを含む。位置情報は、緯度及び経度（及び標高）等により表わされた地物の絶対的な位置を示す。向き情報及びサイズ情報は、例えば、看板などの平面的な形状を有している地物に対して設けられる情報である。向き情報は、地物の向きを表す情報であり、例えば地物に形成された面に対する法線ベクトル等を示す。サイズ情報は、地物の大きさを表す情報であり、本実施例では、地物に形成された被計測面の横幅及び縦幅の情報を含んでいる。

【0028】

なお、地図ＤＢ１０は、定期的に更新されてもよい。この場合、例えば、制御部１５は、図示しない通信部を介し、地図情報を管理するサーバ装置から、自車位置が属するエリアに関する部分地図情報を受信し、地図ＤＢ１０に反映させる。

【0029】

入力部１４は、ユーザが操作するためのボタン、タッチパネル、リモートコントローラ、音声入力装置等である。情報出力部１６は、例えば、制御部１５の制御に基づき出力を行うディスプレイやスピーカ等である。

【0030】

制御部１５は、プログラムを実行するＣＰＵなどを含み、車載機１の全体を制御する。本実施例では、制御部１５は、インターフェース１１から供給される各センサの出力信号及び地図ＤＢ１０に基づき、自車位置の推定を行う自車位置推定部１７を有する。そして、制御部１５は、自車位置の推定結果に基づいて自動運転制御を含む車両の運転支援に関する制御などを行う。制御部１５は、本発明における「第１取得部」、「第２取得部」、「第３取得部」、「算出部」、「補正部」、及びプログラムを実行する「コンピュータ」の一例である。

【0031】

［自車位置推定処理の概要］
まず、自車位置推定部１７による自車位置の推定処理の概要について説明する。

【0032】

自車位置推定部１７は、計測対象となる地物（「ランドマーク」とも呼ぶ。）に対するライダ２による距離及び角度の計測値と、地図ＤＢ１０から抽出したランドマークの位置情報とに基づき、ジャイロセンサ３、車速センサ４、及び／又はＧＰＳ受信機５の出力データから推定した自車位置を補正する。本実施例では、一例として、自車位置推定部１７は、ベイズ推定に基づく状態推定手法に基づき、ジャイロセンサ３、車速センサ４等の出力データから自車位置を予測する予測ステップと、直前の予測ステップで算出した自車位置の予測値を補正する計測更新ステップとを交互に実行する。これらのステップで用いる状態推定フィルタは、ベイズ推定を行うように開発された様々のフィルタが利用可能であり、例えば、拡張カルマンフィルタ、アンセンテッドカルマンフィルタ、パーティクルフィルタなどが該当する。このように、ベイズ推定に基づく位置推定は、種々の方法が提案されている。

【0033】

以下では、拡張カルマンフィルタを用いた自車位置推定について簡略的に説明する。

【0034】

図４は、状態変数ベクトルを２次元直交座標で表した図である。図４に示すように、ｘｙの２次元直交座標上で定義された平面での自車位置は、座標「（ｘ、ｙ）」、自車の方位（ヨー角）「ψ」により表される。ここでは、ヨー角ψは、車の進行方向とｘ軸とのなす角として定義されている。座標（ｘ、ｙ）は、例えば緯度及び経度の組合せに相当する絶対位置、あるいは所定地点を原点とした位置を示すワールド座標である。

【0035】

図５は、予測ステップと計測更新ステップとの概略的な関係を示す図である。また、図６は、自車位置推定部１７の機能ブロックの一例を示す。図５に示すように、予測ステップと計測更新ステップとを繰り返すことで、自車位置を示す状態変数ベクトル「Ｘ」の推定値の算出及び更新を逐次的に実行する。また、図６に示すように、自車位置推定部１７は、予測ステップを実行する位置予測部２１と、計測更新ステップを実行する位置推定部２２とを有する。位置予測部２１は、デッドレコニングブロック２３及び位置予測ブロック２４を含み、位置推定部２２は、ランドマーク探索・抽出ブロック２５及び位置補正ブロック２６を含む。なお、図５では、計算対象となる基準時刻（即ち現在時刻）「ｔ」の状態変数ベクトルを、「Ｘ^－（ｔ）」または「Ｘ^＾（ｔ）」と表記している（「状態変数ベクトルＸ（ｔ）＝（ｘ（ｔ）、ｙ（ｔ）、ψ（ｔ））^Ｔ」と表記する）。ここで、予測ステップで推定された暫定的な推定値（予測値）には当該予測値を表す文字の上に「^－」を付し、計測更新ステップで更新された，より精度の高い推定値には当該値を表す文字の上に「^＾」を付す。

【0036】

予測ステップでは、自車位置推定部１７のデッドレコニングブロック２３は、車両の移動速度「ｖ」と角速度「ω」（これらをまとめて「制御値ｕ（ｔ）＝（ｖ（ｔ）、ω（ｔ））^Ｔ」と表記する。）を用い、前回時刻からの移動距離と方位変化を求める。自車位置推定部１７の位置予測ブロック２４は、直前の計測更新ステップで算出された時刻ｔ－１の状態変数ベクトルＸ^＾（ｔ－１）に対し、求めた移動距離と方位変化を加えて、時刻ｔの自車位置の予測値（「予測自車位置」とも呼ぶ。）Ｘ^－（ｔ）を算出する。また、これと同時に、予測自車位置Ｘ^－（ｔ）の誤差分布に相当する共分散行列「Ｐ^－（ｔ）」を、直前の計測更新ステップで算出された時刻ｔ－１での共分散行列「Ｐ^＾（ｔ－１）」から算出する。

【0037】

計測更新ステップでは、自車位置推定部１７のランドマーク探索・抽出ブロック２５は、地図ＤＢ１０に登録された計測対象の地物（ランドマーク）の位置ベクトルとライダ２のスキャンデータとの対応付けを行う。そして、自車位置推定部１７のランドマーク探索・抽出ブロック２５は、この対応付けができた場合に、対応付けができたランドマークのライダ２による計測値「Ｚ（ｔ）」と、予測自車位置Ｘ^－（ｔ）及び地図ＤＢ１０に登録されたランドマークの位置ベクトルを用いてライダ２による計測処理をモデル化して求めたランドマークの計測値（「計測予測値」と呼ぶ。）「Ｚ^－（ｔ）」と、をそれぞれ取得する。計測値Ｚ（ｔ）は、時刻ｔにライダ２が計測したランドマークの距離及びスキャン角度から、車両の進行方向と横方向を軸とした成分に変換した車両座標系におけるベクトル値である。そして、自車位置推定部１７の位置補正ブロック２６は、計測値Ｚ（ｔ）と計測予測値Ｚ^－（ｔ）との差分値を算出する。

【0038】

なお、実際には、ランドマークは所定の大きさを有し、ライダ２のレーザ光が照射された被照射面上の複数の計測点に対応するデータが点群データとして取得される。よって、例えば、ランドマーク探索・抽出ブロック２５は、対象のランドマークを計測した各計測点における計測値を平均化することで、対象のランドマークの中心位置に相当する計測値Ｚ（ｔ）を算出する。

【0039】

そして、自車位置推定部１７の位置補正ブロック２６は、以下の式（１）に示すように、計測値Ｚ（ｔ）と計測予測値Ｚ^－（ｔ）との差分値にカルマンゲイン「Ｋ（ｔ）」を乗算し、これを予測自車位置Ｘ^－（ｔ）に加えることで、更新された状態変数ベクトル（「推定自車位置」とも呼ぶ。）Ｘ^＾（ｔ）を算出する。

【0040】

【数1】

また、計測更新ステップでは、自車位置推定部１７の位置補正ブロック２６は、予測ステップと同様、推定自車位置Ｘ^＾（ｔ）の誤差分布に相当する共分散行列Ｐ^＾（ｔ）（単にＰ（ｔ）とも表記する）を共分散行列Ｐ^－（ｔ）から求める。カルマンゲインＫ（ｔ）等のパラメータについては、例えば拡張カルマンフィルタを用いた公知の自己位置推定技術と同様に算出することが可能である。

【0041】

本実施例では、自車位置推定部１７は、カルマンゲインＫ（ｔ）を、計測値Ｚ（ｔ）（即ちランドマークの中心点までの計測距離）の信頼度を表す指標である後述の信頼度情報に応じて補正する。これにより、オクルージョンの発生やランドマークに隣接する地物の存在等に起因して計測値Ｚ（ｔ）の信頼度が低い場合にはカルマンゲインＫ（ｔ）を低くし、予測自車位置Ｘ^－（ｔ）に対する補正量を少なくする。この場合、予測自車位置Ｘ^－（ｔ）に対する不正確な補正が好適に抑制され、自己位置推定精度が向上する。

【0042】

なお、自車位置推定部１７は、複数の地物に対し、地図ＤＢ１０に登録された地物の位置ベクトルとライダ２のスキャンデータとの対応付けができた場合、選定した任意の一組の計測予測値及び計測値等に基づき計測更新ステップを行ってもよく、対応付けができた全ての計測予測値及び計測値等に基づき計測更新ステップを複数回行ってもよい。なお、複数の計測予測値及び計測値等を用いる場合には、自車位置推定部１７は、ライダ２から遠い地物ほどライダ計測精度が悪化することを勘案し、ライダ２と地物との距離が長いほど、当該地物に関する重み付けを小さくするとよい。

【0043】

このように、予測ステップと計測更新ステップが繰り返し実施され、予測自車位置Ｘ^－（ｔ）と推定自車位置Ｘ^＾（ｔ）が逐次的に計算されることにより、もっとも確からしい自車位置が計算される。

【0044】

なお、上記の説明において、計測値Ｚ（ｔ）は本発明の「計測距離」の一例であり、計測予測値Ｚ^－（ｔ）は本発明の「予測距離」の一例である。

【0045】

［信頼度情報の算出］
次に、計測値Ｚ（ｔ）の信頼度を表す指標である信頼度情報の算出方法について説明する。概略的には、自車位置推定部１７は、地物情報に含まれるサイズ情報が示すランドマークの地図上のサイズと、ライダ２により計測されたランドマークの計測点が分布する範囲（計測範囲）のサイズとの差分を算出し、当該差分を正規化することで、信頼度情報を算出する。これにより、自車位置推定部１７は、計測値Ｚ（ｔ）の信頼度を的確に表した信頼度情報を算出する。

【0046】

（１）方向によらない信頼度情報の算出
まず、計測値Ｚ（ｔ）に対して方向によらない信頼度情報を生成する場合について説明する。

【0047】

この場合、自車位置推定部１７は、地物情報に含まれるランドマークの地図上の横幅「Ｗ_Ｍ」及び縦幅（高さ方向の幅）「Ｈ_Ｍ」と、時刻ｔにおけるライダ２の点群データに基づき特定されたランドマークの計測範囲の横幅「Ｗ_Ｌ（ｔ）」及び縦幅「Ｈ_Ｌ（ｔ）」と、感度係数「ｋ」とに基づき、以下の式（２）に示すように信頼度情報ａ（ｔ）を定める。

【0048】

【数2】

上述の式（２）では、信頼度情報ａ（ｔ）は、０から１までの値域となるように正規化され、計測された横幅Ｗ_Ｌ（ｔ）及び縦幅Ｈ_Ｌ（ｔ）が地図上の横幅Ｗ_Ｍ及び縦幅Ｈ_Ｍと近いほど最大値の１に近づき、横幅Ｗ_Ｌ（ｔ）及び縦幅Ｈ_Ｌ（ｔ）が横幅Ｗ_Ｍ及び縦幅Ｈ_Ｍから離れるほど０に近づく。なお、信頼度情報ａ（ｔ）は、１に近いほど計測値Ｚ（ｔ）に対する信頼度が高い（即ち計測値Ｚ（ｔ）が正確）であることを示す。また、感度係数ｋは、計測された横幅Ｗ_Ｌ（ｔ）及び縦幅Ｈ_Ｌ（ｔ）と地図上の横幅Ｗ_Ｍ及び縦幅Ｈ_Ｍとの差分に対する信頼度情報ａ（ｔ）への感度を調整する係数であり、感度係数ｋが大きいほど算出される信頼度情報ａ（ｔ）が小さくなる。なお、感度係数ｋは、例えば予め定められた値に設定される。

【0049】

図７（Ａ）は、横幅Ｗ_Ｍが６０ｃｍ、縦幅Ｈ_Ｍが６０ｃｍとなるランドマークに対して縦幅Ｈ_Ｌ（ｔ）を６０ｃｍに固定した場合の横幅Ｗ_Ｌ（ｔ）と式（２）に基づく信頼度情報ａ（ｔ）との関係を示す２次元グラフである。また、図７（Ｂ）は、図７（Ａ）と同一のランドマークに対して横幅Ｗ_Ｌ（ｔ）を６０ｃｍに固定した場合の縦幅Ｈ_Ｌ（ｔ）と信頼度情報ａ（ｔ）との関係を示す２次元グラフである。さらに、図７（Ｃ）は、図７（Ａ）、（Ｂ）と同一のランドマークに対する横幅Ｗ_Ｌ（ｔ）と縦幅Ｈ_Ｌ（ｔ）と信頼度情報ａ（ｔ）との関係を示す３次元グラフである。また、図７（Ｄ）は、図７（Ａ）における信頼度情報ａ（ｔ）の逆数「１／ａ（ｔ）」を示したグラフである。

【0050】

図７（Ａ）及び図７（Ｃ）に示すように、横幅Ｗ_Ｌ（ｔ）が横幅Ｗ_Ｍである６０ｃｍに近づくほど、信頼度情報ａ（ｔ）は最大値である１に近づく。一方、図７（Ｂ）及び図７（Ｃ）に示すように、縦幅Ｈ_Ｌ（ｔ）が縦幅Ｈ_Ｍである６０ｃｍに近づくほど、信頼度情報ａ（ｔ）は最大値である１に近づく。なお、図７（Ｄ）に示すように、信頼度情報ａ（ｔ）の逆数は、横幅Ｗ_Ｌ（ｔ）が横幅Ｗ_Ｍに近づくほど最小値である１に近づき、横幅Ｗ_Ｌ（ｔ）が横幅Ｗ_Ｍから遠ざかるほど大きい値となる。同様に、信頼度情報ａ（ｔ）の逆数は、縦幅Ｈ_Ｌ（ｔ）が縦幅Ｈ_Ｍに近づくほど最小値である１に近づき、縦幅Ｈ_Ｌ（ｔ）が縦幅Ｈ_Ｍから遠ざかるほど大きい値となる。信頼度情報ａ（ｔ）の逆数は、後述するように、信頼度情報に応じてカルマンゲインを設定する際に用いられる。

【0051】

ここで、式（２）の妥当性について、図８及び図９に示す具体例を用いて補足説明する。

【0052】

図８（Ａ）は、ランドマークとなる地物２０のライダ２による計測時においてオクルージョンが発生した場合のライダ２のレーザ光の照射範囲を概略的に示した図であり、図８（Ｂ）は、ライダ２のレーザ光が照射された被照射面上の計測点を明示した地物２０の平面図である。なお、ここではレーザ光の反射強度が高い道路標識を地物２０の例としている。

【0053】

自車位置推定部１７は、ライダ２の計測範囲内にランドマークとなる地物２０が存在することを認識し、当該地物２０が存在することが予測される範囲を示すウィンドウ２１を設定する。例えば、自車位置推定部１７は、予測自車位置Ｘ^－（ｔ）から所定距離以内の位置を示す位置情報が地物情報に含まれる地物２０をランドマークとして特定し、当該地物２０の位置情報が示す位置を中心とする所定サイズの矩形領域をウィンドウ２１として設定する。なお、この場合、自車位置推定部１７は、地物２０のサイズ情報をさらに参照することで、ウィンドウ２１の大きさを決定してもよい。そして、自車位置推定部１７は、ウィンドウ２１内の位置を示す計測点であって、反射強度が所定の閾値以上となる計測点の点群データを、地物２０の点群データとみなす。

【0054】

この場合、図８（Ｂ）に示すように、ウィンドウ２１内のライダ２の点群データに基づき特定する地物２０の横幅Ｗ_Ｌ（ｔ）は、地図上の地物２０の横幅Ｗ_Ｍより短くなる。よって、この場合、自車位置推定部１７は、式（２）に基づき、信頼度情報ａ（ｔ）を１より小さい値に設定する（図７（Ａ）及び図７（Ｃ）参照）。

【0055】

一方、自車位置推定部１７は、ウィンドウ２１内の各計測点の計測値を平均化することで、推定自車位置Ｘ^＾（ｔ）の算出（式（１）参照）に用いる地物２０の計測値Ｚ（ｔ）を算出する。ここで、地物２０の左側の一部にオクルージョンが発生しているため、上述の平均化により算出される計測値Ｚ（ｔ）は、本来算出されるべき値（即ち地物２０の中心位置を示す値）よりも右側にずれた値となる。

【0056】

このように、式（２）によれば、自車位置推定部１７は、ランドマークにオクルージョンが発生し、計測値Ｚ（ｔ）にずれが生じると推測される場合に、信頼度情報ａ（ｔ）を低い値に設定することができる。これにより、後述するように、自車位置推定部１７は、計測値Ｚ（ｔ）の信頼度が低いときの予測自車位置Ｘ^－（ｔ）に対する補正量を少なくし、不正確な補正防止によって自己位置推定精度を好適に向上させる。

【0057】

図９（Ａ）は、ランドマークとなる地物２０に隣接して他の地物２５が存在する場合のライダ２のレーザ光の照射範囲を概略的に示した図であり、図９（Ｂ）は、図９（Ａ）においてライダ２のレーザ光が照射された被照射面上の計測点を明示した地物２０の平面図である。

【0058】

図９（Ａ）の例では、自車位置推定部１７は、ウィンドウ２１を設定後、ウィンドウ２１内において所定閾値以上の反射強度となる計測点の点群データを、地物２０の点群データとみなす。ここで、ウィンドウ２１には、地物２０の計測点に加えて、地物２０に隣接する地物２５の一部の計測点も含まれている。

【0059】

この場合、図９（Ｂ）に示すように、ウィンドウ２１内のライダ２の点群データに基づき特定される横幅Ｗ_Ｌ（ｔ）は、地図上の地物２０の横幅Ｗ_Ｍより長くなる。よって、この場合、自車位置推定部１７は、横幅Ｗ_Ｌ（ｔ）と地図上の横幅Ｗ_Ｍとの差異が大きくなることから、式（２）に基づき、信頼度情報ａ（ｔ）を１より小さい値に設定する（図７（Ａ）及び図７（Ｃ）参照）。

【0060】

一方、自車位置推定部１７は、ウィンドウ２１内の各計測点の計測値を平均化することで、推定自車位置Ｘ^＾（ｔ）の算出（式（１）参照）に用いる地物２０の計測値Ｚ（ｔ）を算出する。ここで、ウィンドウ２１内の点群データには、地物２０を計測した点群データに加えて、地物２０に隣接する地物２５を計測した点群データも含まれているため、上述の平均化により算出される計測値Ｚ（ｔ）は、本来算出されるべき値（即ち地物２０の中心位置を示す値）よりも左側にずれた値となる。

【0061】

このように、式（２）によれば、計測対象の地物に他の地物が隣接することに起因して計測値Ｚ（ｔ）にずれが生じた場合においても、自車位置推定部１７は、信頼度情報ａ（ｔ）を低い値に設定することができる。これにより、後述するように、自車位置推定部１７は、計測値Ｚ（ｔ）の信頼度が低いときの予測自車位置Ｘ^－（ｔ）に対する補正量を少なくし、不正確な補正防止によって自己位置推定精度を好適に向上させる。

【0062】

なお、ランドマークとなる地物は、再帰性反射材が使用されているものが多く、ライダ２に戻ってくる反射光の強度が高いため、反射強度に対して閾値を用いたフィルタリングにより、点群データを抽出しやすい対象と言える。一方、走行環境において、視線誘導標や他車両の反射板等の反射強度の高い物体が多数存在し、それらの点群データをランドマークの点群データと誤って検出してしまう可能性がある。このような場合、横幅Ｗ_Ｌ（ｔ）が横幅Ｗ_Ｍよりも長くなる、又は／及び、縦幅Ｈ_Ｌ（ｔ）が縦幅Ｈ_Ｍよりも長くなる。このような場合において、本実施例では、自車位置推定部１７は、式（２）に基づき、計測値の信頼度が低いことを示す信頼度情報ａ（ｔ）を適切に設定することができる。

【0063】

（２）方向毎の信頼度情報の算出
次に、車両の進行方向と横方向のそれぞれに対して信頼度情報を算出する例について説明する。この場合、概略的には、自車位置推定部１７は、地物情報に含まれる向き情報を参照し、車両の進行方向に対するランドマークの相対的な向きに基づき、車両の進行方向及び横方向の信頼度情報をそれぞれ算出する。以後では、時刻ｔにおける車両の進行方向の信頼度情報を「ａ_Ｘ（ｔ）」、横方向の信頼度情報を「ａ_Ｙ（ｔ）」とする。

【0064】

ランドマークが車両の進行方向と同一の方向を向いている場合、ランドマークの被照射面が車両の進行方向に対して垂直となる。この場合、計測された横幅Ｗ_Ｌ（ｔ）及び縦幅Ｈ_Ｌ（ｔ）と地図上の横幅Ｗ_Ｍ及び縦幅Ｈ_Ｍとの差異が大きいと、計測値Ｚ（ｔ）の中心点はずれるものの、車両の進行方向成分の計測値Ｌ_Ｘ（ｔ）には大きな誤差は生じない。よって、信頼度情報ａ_Ｘ（ｔ）の値をあまり小さくする必要はない。一方、車両の横方向成分の計測値Ｌ_Ｙ（ｔ）には誤差が生じることから、信頼度情報ａ_Ｙ（ｔ）の値を小さくする必要がある。

【0065】

以上を勘案し、自車位置推定部１７は、信頼度情報ａ_Ｘ（ｔ）と信頼度情報ａ_Ｙ（ｔ）とを算出する場合とで異なる感度係数「ｋ_Ｘ」、「ｋ_Ｙ」をそれぞれ用い、式（２）の感度係数ｋと置き換えて計算を行う。具体的には、自車位置推定部１７は、感度係数ｋ_Ｘを用いた以下の式（３）により信頼度情報ａ_Ｘ（ｔ）を算出し、感度係数ｋ_Ｙを用いた以下の式（４）によりａ_Ｙ（ｔ）を算出する。

【0066】

【数3】

【0067】

【数4】

そして、自車位置推定部１７は、これらの感度係数ｋ_Ｘ、ｋ_Ｙを、車両の進行方向に対するランドマークの相対的な向きに基づいて決定する。具体的には、自車位置推定部１７は、ランドマークの向きと車両の進行方向とが平行に近づくほど、信頼度情報ａ_Ｘ（ｔ）に対する感度係数ｋ_Ｘを小さくし、信頼度情報ａ_Ｙ（ｔ）に対する感度係数ｋ_Ｙを大きくする。一方、自車位置推定部１７は、ランドマークの向きと車両の進行方向とが垂直に近づくほど、信頼度情報ａ_Ｘ（ｔ）に対する感度係数ｋ_Ｘを大きくし、信頼度情報ａ_Ｙ（ｔ）に対する感度係数ｋ_Ｙを小さくする。

【0068】

図１０は、２次元のワールド座標系（絶対座標系）及び車両座標系により表された車両とランドマークとの位置関係を示した図である。ここで、ワールド座標系は、所定地点を原点とし、互いに垂直な座標軸「ｘ_ｗ」及び座標軸「ｙ_ｗ」を有し、車両座標系は、車両の中心を原点とし、車両の進行方向に沿った座標軸「ｘ_ｂ」と車両の側面方向に沿った座標軸「ｙ_ｂ」を有する。そして、図１０では、ワールド座標系における車両のヨー角を「ψ^－」、車両の位置を［ｘ^－、ｙ^－］^Ｔとしている。また、ワールド座標系におけるランドマークのヨー角を「Ｍ_ψ」、位置を［Ｍ_ｘ、Ｍ_ｙ］^Ｔとし、車両座標系におけるランドマークのヨー角を「Ｌ_ψ」、位置を［Ｌ_ｘ、Ｌ_ｙ］^Ｔとしている。

【0069】

この場合、車両から見たランドマークの向きは、車両座標系におけるランドマークのヨー角Ｌ_ψに等しく、ヨー角Ｌ_ψは、図１０に示すように、「Ｍ_ψ－ψ^－」に等しい。従って、自車位置推定部１７は、感度係数ｋ_Ｘ、ｋ_Ｙを、「Ｍ_ψ－ψ^－」と定数「ｃ」とを用いてそれぞれ以下の式（５）及び式（６）により定める。

【0070】

【数5】

【0071】

【数6】

上記の式（５）及び式（６）によれば、車両から見てランドマークが正面を向いているほど、感度係数ｋ_Ｘは最小値ｃに近づき、感度係数ｋ_Ｙは大きな値となる。言い換えると、車両から見てランドマークが横を向いているほど、感度係数ｋ_Ｘは大きくなり、感度係数ｋ_Ｙは最小値ｃに近づく。

【0072】

図１１（Ａ）は、「Ｍ_ψ－ψ^－」が１０°の場合において、横幅Ｗ_Ｍが６０ｃｍ、縦幅Ｈ_Ｍが６０ｃｍとなるランドマークに対して縦幅Ｈ_Ｌ（ｔ）を６０ｃｍに固定したときの横幅Ｗ_Ｌ（ｔ）と式（３）に基づく信頼度情報ａ_Ｘ（ｔ）との関係を示す２次元グラフである。また、図１１（Ｂ）は、図１１（Ａ）と同一条件下での横幅Ｗ_Ｌ（ｔ）と式（４）に基づく信頼度情報ａ_Ｙ（ｔ）との関係を示す２次元グラフである。なお、図１１及び後述の図１２の各グラフでは、定数ｃは２０に設定されている。

【0073】

図１１（Ａ）の例では、「Ｍ_ψ－ψ^－」が１０°であることから、感度係数ｋ_Ｘは、式（５）に基づき「１．０１５ｃ」となる。一方、図１１（Ｂ）の例では、感度係数ｋ_Ｙは、式（６）に基づき「５．７５９ｃ」となる。このように、車両から見てランドマークがほぼ正面を向いているときには、感度係数ｋ_Ｘはｃに近い値となり、感度係数ｋ_Ｙは感度係数ｋ_Ｘよりも遥かに大きい値となる。そして、この場合、信頼度情報ａ_Ｙ（ｔ）は、計測された横幅Ｗ_Ｌ（ｔ）及び縦幅Ｈ_Ｌ（ｔ）と地図上の横幅Ｗ_Ｍ及び縦幅Ｈ_Ｍとのずれに対する感度が高くなる。

【0074】

図１２（Ａ）は、「Ｍ_ψ－ψ^－」が６０°の場合において、横幅Ｗ_Ｍが６０ｃｍ、縦幅Ｈ_Ｍが６０ｃｍとなるランドマークに対して縦幅Ｈ_Ｌ（ｔ）を６０ｃｍに固定したときの横幅Ｗ_Ｌ（ｔ）と式（３）に基づく信頼度情報ａ_Ｘ（ｔ）との関係を示す２次元グラフである。また、図１２（Ｂ）は、図１２（Ａ）と同一条件下での横幅Ｗ_Ｌ（ｔ）と式（４）に基づく信頼度情報ａ_Ｙ（ｔ）との関係を示す２次元グラフである。

【0075】

図１２（Ａ）の例では、「Ｍ_ψ－ψ^－」が６０°であることから、感度係数ｋ_Ｘは、式（５）に基づき「２．０ｃ」となる。一方、図１２（Ｂ）の例では、感度係数ｋ_Ｙは、式（６）に基づき「１．１５５ｃ」となる。このように、車両から見てランドマークがやや横向きに近いときには、感度係数ｋ_Ｘは感度係数ｋ_Ｙよりも大きい値となる。そして、この場合、信頼度情報ａ_Ｘ（ｔ）は、計測された横幅Ｗ_Ｌ（ｔ）及び縦幅Ｈ_Ｌ（ｔ）と地図上の横幅Ｗ_Ｍ及び縦幅Ｈ_Ｍとのずれに対する感度が信頼度情報ａ_Ｙ（ｔ）と比べて相対的に高くなる。

【0076】

［信頼度情報に応じたカルマンゲイン］
次に、信頼度情報に応じてカルマンゲインを設定する方法について説明する。

【0077】

自車位置推定部１７は、以下の一般式（７）によりカルマンゲインＫ（ｔ）を算出する際に用いる観測雑音行列「Ｒ（ｔ）」の対角成分に、信頼度情報ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）の逆数をそれぞれ乗じる。

【0078】

【数7】

ここで、対角成分に信頼度情報ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）の逆数が乗じられた後の観測雑音行列Ｒ（ｔ）（「修正観測雑音行列Ｒ（ｔ）’」とも呼ぶ。）は、以下の式（８）により表される。

【0079】

【数8】

このように、式（３）及び式（４）に基づき信頼度情報ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）をそれぞれ生成した場合、観測雑音行列の各成分に乗じる係数は個別となる。なお、自車位置推定部１７は、式（２）に基づき信頼度情報ａ（ｔ）を算出した場合には、式（８）において「ａ_Ｘ（ｔ）＝ａ_Ｙ（ｔ）＝ａ（ｔ）」とみなして修正観測雑音行列Ｒ（ｔ）’を算出すればよい。

【0080】

式（７）によれば、信頼度情報が最高値の「１」であった場合には、その逆数も１であるため、修正観測雑音行列Ｒ（ｔ）’が元の観測雑音行列Ｒ（ｔ）のままとなる。一方、信頼度情報が「０」に近い値であった場合には、その逆数は１より大きい値となり、修正観測雑音行列Ｒ（ｔ）’の対角成分は増倍されることになる。

【0081】

そして、自車位置推定部１７は、式（７）に示す修正観測雑音行列Ｒ（ｔ）’を用いて、以下の式（９）から適応的なカルマンゲイン「Ｋ（ｔ）’」を算出する。

【0082】

【数9】

そして、自車位置推定部１７は、式（９）のカルマンゲインＫ（ｔ）’を用いて、式（１）に基づき予測自車位置Ｘ^－（ｔ）を補正した推定自車位置Ｘ^＾（ｔ）を算出する。従って、信頼度情報が低い場合は、カルマンゲインＫ（ｔ）’が小さくなるため、予測自車位置Ｘ^－（ｔ）に対する補正量が少なくなる。この場合、不正確な補正を防止できるため、推定自車位置Ｘ^＾（ｔ）の精度が好適に向上する。

【0083】

なお、自車位置推定部１７は、観測雑音行列Ｒ（ｔ）の対角成分に、信頼度情報ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）の逆数をそれぞれ乗じる代わりに、カルマンゲインＫ（ｔ）に対して信頼度情報ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）の逆数をそれぞれ乗じてもよい。この場合、自車位置推定部１７は、カルマンゲインＫ（ｔ）’を以下の式（１０）に基づき算出する。

【0084】

【数10】

この式（１０）を用いた場合であっても、信頼度情報が低い場合は、カルマンゲインＫ（ｔ）’が小さくなるため、予測自車位置Ｘ^－（ｔ）に対する補正量が少なくなり、不正確な補正を好適に防止することができる。

【0085】

［処理フロー］
図１３は、車載機１の自車位置推定部１７により行われる自車位置推定処理のフローチャートである。車載機１は、図１３のフローチャートの処理を繰り返し実行する。ここでは、一例として、車両の進行方向及び横方向のそれぞれに対する信頼度情報ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を算出する例について説明する。

【0086】

まず、自車位置推定部１７は、ＧＰＳ受信機５等の出力に基づき、自車位置の初期値を設定する（ステップＳ１０１）。次に、自車位置推定部１７は、車速センサ４から車体速度を取得すると共に、ジャイロセンサ３からヨー方向の角速度を取得する（ステップＳ１０２）。そして、自車位置推定部１７は、ステップＳ１０２の取得結果に基づき、車両の移動距離と車両の方位変化を計算する（ステップＳ１０３）。

【0087】

その後、自車位置推定部１７は、１時刻前の推定自車位置Ｘ^＾（ｔ－１）に、ステップＳ１０３で計算した移動距離と方位変化を加算し、予測自車位置Ｘ^－（ｔ）を算出する（ステップＳ１０４）。さらに、自車位置推定部１７は、予測自車位置Ｘ^－（ｔ）に基づき、地図ＤＢ１０の地物情報を参照し、ライダ２の計測範囲内となるランドマークを探索する（ステップＳ１０５）。

【0088】

そして、自車位置推定部１７は、予測自車位置Ｘ^－（ｔ）及びステップＳ１０５で探索したランドマークの地物情報が示す位置座標から、計測予測値Ｚ^－（ｔ）を算出する（ステップＳ１０６）。さらに、ステップＳ１０６では、自車位置推定部１７は、ステップＳ１０５で探索したランドマークに対するライダ２の計測データから計測値Ｚ（ｔ）を算出する。

【0089】

そして、自車位置推定部１７は、予測自車位置Ｘ^－（ｔ）が示す自車の方位（ヨー角）ψ^－に相当する車両の進行方向と、地図ＤＢ１０に記録されたランドマークの向き情報とに基づき、車両の進行方向に対する感度係数ｋ_Ｘと、車両の横方向に対する感度係数ｋ_Ｙとをそれぞれ決定する（ステップＳ１０７）。この場合、例えば、自車位置推定部１７は、式（５）に基づき感度係数ｋ_Ｘを算出し、式（６）に基づき感度係数ｋ_Ｙを算出する。

【0090】

次に、自車位置推定部１７は、ランドマークの地図上の横幅Ｗ_Ｍ及び縦幅Ｈ_Ｍと計測した横幅Ｗ_Ｌ（ｔ）及び縦幅Ｈ_Ｌ（ｔ）との各差分と、感度係数ｋ_Ｘ、ｋ_Ｙとに基づき、式（３）及び式（４）を参照して、信頼度情報ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）をそれぞれ算出する（ステップＳ１０８）。そして、自車位置推定部１７は、信頼度情報ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）に応じてカルマンゲインＫ（ｔ）’を生成する（ステップＳ１０９）。例えば、自車位置推定部１７は、信頼度情報ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を用いて式（８）に基づき修正観測雑音行列Ｒ（ｔ）’を算出後、式（９）に基づきカルマンゲインＫ（ｔ）’を算出する。その後、自車位置推定部１７は、カルマンゲインＫ（ｔ）´を式（１）のＫ（ｔ）の代わりに用いることで予測自車位置Ｘ^－（ｔ）を補正し、推定自車位置Ｘ^＾（ｔ）を算出する（ステップＳ１１０）。

【0091】

以上説明したように、本実施例に係る車載機１の自車位置推定部１７は、ライダ２によるランドマークの計測結果である点群データを取得すると共に、地図ＤＢ１０に含まれるランドマークの地物情報を取得する。そして、自車位置推定部１７は、取得した点群データに基づき特定したランドマークのサイズと地物情報が示すランドマークのサイズとの差分に基づいて、ライダ２によるランドマークの計測精度を示す信頼度情報を算出する。これにより、自車位置推定部１７は、推定自車位置Ｘ^＾（ｔ）を算出する際の予測自車位置Ｘ^－（ｔ）に対する補正量を、ライダ２の計測結果の正確さに応じて的確に定めることができる。

【0092】

［変形例］
以下、実施例に好適な変形例について説明する。以下の変形例は、組み合わせてこれらの実施例に適用してもよい。

【0093】

（変形例１）
自車位置推定部１７は、式（２）に代えて、計測された横幅及び縦幅と地図上の横幅及び縦幅とのそれぞれの差の絶対値｜Ｗ_Ｌ（ｔ）－Ｗ_Ｍ｜、｜Ｈ_Ｌ（ｔ）－Ｈ_Ｍ｜を用いた以下の式（１１）に基づき信頼度情報ａ（ｔ）を算出してもよい。

【0094】

【数11】

式（１１）に基づく信頼度情報ａ（ｔ）は、式（２）に基づく信頼度情報ａ（ｔ）と同様、０から１までの値域となるように正規化され、計測された横幅Ｗ_Ｌ（ｔ）及び縦幅Ｈ_Ｌ（ｔ）が地図上の横幅Ｗ_Ｍ及び縦幅Ｈ_Ｍと近いほど最大値の１に近づき、横幅Ｗ_Ｌ（ｔ）及び縦幅Ｈ_Ｌ（ｔ）が横幅Ｗ_Ｍ及び縦幅Ｈ_Ｍから離れるほど０に近づく。

【0095】

図１４（Ａ）は、横幅Ｗ_Ｍが６０ｃｍ、縦幅Ｈ_Ｍが６０ｃｍとなるランドマークに対して計測した縦幅Ｈ_Ｌ（ｔ）を６０ｃｍに固定した場合の横幅Ｗ_Ｌ（ｔ）と式（１１）に基づく信頼度情報ａ（ｔ）との関係を示す２次元グラフである。また、図１４（Ｂ）は、図１４（Ａ）と同一のランドマークに対して計測した横幅Ｗ_Ｌ（ｔ）を６０ｃｍに固定した場合の縦幅Ｈ_Ｌ（ｔ）と式（１１）に基づく信頼度情報ａ（ｔ）との関係を示す２次元グラフである。さらに、図１４（Ｃ）は、図１４（Ａ）、（Ｂ）と同一のランドマークに対する横幅Ｗ_Ｌ（ｔ）と縦幅Ｈ_Ｌ（ｔ）と式（１１）に基づく信頼度情報ａ（ｔ）との関係を示す３次元グラフである。また、図１４（Ｄ）は、図１４（Ａ）における信頼度情報ａ（ｔ）の逆数を示したグラフである。

【0096】

図１４（Ａ）及び図１４（Ｃ）に示すように、横幅Ｗ_Ｌ（ｔ）は、横幅Ｗ_Ｍである６０ｃｍに近づくほど信頼度情報ａ（ｔ）が１に近づく。一方、図１４（Ｂ）及び図１４（Ｃ）に示すように、縦幅Ｈ_Ｌ（ｔ）は、縦幅Ｈ_Ｍである６０ｃｍに近づくほど信頼度情報ａ（ｔ）が１に近づく。また、図１４（Ｄ）に示すように、信頼度情報ａ（ｔ）の逆数は、横幅Ｗ_Ｌ（ｔ）が横幅Ｗ_Ｍに近づくほど１に近づき、横幅Ｗ_Ｌ（ｔ）が横幅Ｗ_Ｍから遠ざかるほど大きい値となる。同様に、信頼度情報ａ（ｔ）の逆数は、縦幅Ｈ_Ｌ（ｔ）が縦幅Ｈ_Ｍに近づくほど１に近づき、縦幅Ｈ_Ｌ（ｔ）が縦幅Ｈ_Ｍから遠ざかるほど大きい値となる。

【0097】

このように、式（１１）に基づく信頼度情報ａ（ｔ）は、式（２）に基づく信頼度情報ａ（ｔ）と同様、最大値が１となり、計測された横幅及び縦幅と地図上の横幅及び縦幅とのそれぞれの差が大きいほど小さい値となる。よって、自車位置推定部１７は、式（１１）に基づく信頼度情報ａ（ｔ）の逆数を用いた式（８）に基づき修正観測雑音行列Ｒ（ｔ）’を算出することで、推定自車位置Ｘ^＾（ｔ）を算出する際の予測自車位置Ｘ^－（ｔ）に対する補正量を好適に定めることができる。

【0098】

なお、自車位置推定部１７は、実施例と同様、車両の進行方向及び横方向のそれぞれに対する信頼度情報ａ_Ｘ（ｔ）及び信頼度情報ａ_Ｙ（ｔ）を算出してもよい。この場合、自車位置推定部１７は、感度係数ｋ_Ｘ、ｋ_Ｙを式（５）、（６）に基づき算出することで、信頼度情報ａ_Ｘ（ｔ）及び信頼度情報ａ_Ｙ（ｔ）をそれぞれ算出する。

【0099】

（変形例２）
自車位置推定部１７は、式（２）に代えて、信頼度が高いほど最小値の１に近づき、信頼度が低いほど大きな値になるように信頼度情報ａ（ｔ）を定めてもよい。

【0100】

例えば、自車位置推定部１７は、以下の式（１２）に基づき、信頼度情報ａ（ｔ）を算出する。

【0101】

【数12】

式（１２）に基づく信頼度情報ａ（ｔ）は、１以上の値域となるように正規化され、計測された横幅Ｗ_Ｌ（ｔ）及び縦幅Ｈ_Ｌ（ｔ）が地図上の横幅Ｗ_Ｍ及び縦幅Ｈ_Ｍと近いほど最小値の１に近づき、横幅Ｗ_Ｌ（ｔ）及び縦幅Ｈ_Ｌ（ｔ）が横幅Ｗ_Ｍ及び縦幅Ｈ_Ｍから離れるほど大きい値となる。

【0102】

図１５（Ａ）は、横幅Ｗ_Ｍが６０ｃｍ、縦幅Ｈ_Ｍが６０ｃｍとなるランドマークに対して計測した縦幅Ｈ_Ｌ（ｔ）を６０ｃｍに固定した場合の横幅Ｗ_Ｌ（ｔ）と式（１２）に基づく信頼度情報ａ（ｔ）との関係を示す２次元グラフである。また、図１５（Ｂ）は、図１５（Ａ）と同一のランドマークに対して計測した横幅Ｗ_Ｌ（ｔ）を６０ｃｍに固定した場合の縦幅Ｈ_Ｌ（ｔ）と式（１２）に基づく信頼度情報ａ（ｔ）との関係を示す２次元グラフである。さらに、図１５（Ｃ）は、図１５（Ａ）、（Ｂ）と同一のランドマークに対する横幅Ｗ_Ｌ（ｔ）と縦幅Ｈ_Ｌ（ｔ）と式（１２）に基づく信頼度情報ａ（ｔ）との関係を示す３次元グラフである。

【0103】

図１５（Ａ）及び図１５（Ｃ）に示すように、信頼度情報ａ（ｔ）は、横幅Ｗ_Ｍである６０ｃｍに横幅Ｗ_Ｌ（ｔ）が近づくほど最小値である１に近づき、横幅Ｗ_Ｍから横幅Ｗ_Ｌ（ｔ）が遠ざかるほど大きい値となる。同様に、図１５（Ｂ）及び図１５（Ｃ）に示すように、信頼度情報ａ（ｔ）は、縦幅Ｈ_Ｍである６０ｃｍに縦幅Ｈ_Ｌ（ｔ）が近づくほど最小値である１に近づき、縦幅Ｈ_Ｍから縦幅Ｈ_Ｌ（ｔ）が遠ざかるほど大きい値となる。

【0104】

なお、自車位置推定部１７は、実施例と同様、車両の進行方向及び横方向のそれぞれに対する信頼度情報ａ_Ｘ（ｔ）及び信頼度情報ａ_Ｙ（ｔ）を算出してもよい。この場合、自車位置推定部１７は、感度係数ｋ_Ｘ、ｋ_Ｙを式（５）、（６）に基づき算出することで、信頼度情報ａ_Ｘ（ｔ）及び信頼度情報ａ_Ｙ（ｔ）をそれぞれ算出する。

【0105】

また、式（１２）に基づく信頼度情報ａ（ｔ）は、式（２）に基づく信頼度情報ａ（ｔ）の逆数（図７（Ｄ）参照）と同様に、最小値が１となり、かつ、横幅Ｗ_Ｍから横幅Ｗ_Ｌ（ｔ）が遠ざかる又は縦幅Ｈ_Ｍから縦幅Ｈ_Ｌ（ｔ）が遠ざかるほど大きい値となる。よって、自車位置推定部１７は、修正観測雑音行列Ｒ（ｔ）’を算出する際に、信頼度情報ａ（ｔ）を逆数にして観測雑音行列Ｒ（ｔ）の対角成分に乗じる必要がない。具体的には、自車位置推定部１７は、信頼度情報ａ_Ｘ（ｔ）及び信頼度情報ａ_Ｙ（ｔ）をそれぞれ算出した場合には、以下の式（１３）に基づき、修正観測雑音行列Ｒ（ｔ）’を算出すればよい。

【0106】

【数13】

これにより、実施例と同様、自車位置推定部１７は、推定自車位置Ｘ^＾（ｔ）を算出する際の予測自車位置Ｘ^－（ｔ）に対する補正量を好適に定めることができる。

【0107】

（変形例３）
図１に示す運転支援システムの構成は一例であり、本発明が適用可能な運転支援システムの構成は図１に示す構成に限定されない。例えば、運転支援システムは、車載機１を有する代わりに、車両の電子制御装置が車載機１の自車位置推定部１７の処理を実行してもよい。この場合、地図ＤＢ１０は、例えば車両内の記憶部に記憶され、車両の電子制御装置は、図１３のフローチャートの処理を実行することで推定自車位置を算出する。

【0108】

（変形例４）
信頼度情報は、推定自車位置Ｘ^＾（ｔ）を算出する際の予測自車位置Ｘ^－（ｔ）の補正量を決定するパラメータとして用いられることに限定されない。例えば、車載機１は、算出した信頼度情報を、ライダ２が出力する点群データに基づく障害物検知などの他の用途に用いてもよい。

【符号の説明】

【0109】

１ 車載機
２ ライダ
３ ジャイロセンサ
４ 車速センサ
５ ＧＰＳ受信機
１０ 地図ＤＢ

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】