JP6968877B2 - 自己位置推定装置、制御方法、プログラム及び記憶媒体 - Google Patents

Description

本発明は、自己位置推定技術に関する。

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

特開２０１３−２５７７４２号公報 特開２０１７−７２４２２号公報

計測センサの出力と、予め地図上に登録された地物の位置情報とを照合させることで自己位置を推定する場合、地図又は計測センサの出力のいずれかに誤差があると、自己位置が誤って補正され、自己位置推定精度が低下する。計測センサの出力に誤差が生じる状況としては、視線誘導標や他車両の反射板等の反射強度の高い物体が多数存在し、それらを誤って対象の地物と検出してしまう場合や、自車と対象の地物の間に他車両が存在し、計測センサによるスキャンの全てあるいは一部が遮蔽（オクルージョン）される場合などがある。また、地図に誤差が生じる状況としては、最新の情報が地図に反映されておらず、地物の位置座標が不正確な場合などがある。

本発明は、上記のような課題を解決するためになされたものであり、地図又は計測値のいずれかに誤差が生じた場合であっても、自己位置推定精度の低下を好適に抑制することが可能な自己位置推定装置を提供することを主な目的とする。

請求項１に記載の発明は、自己位置推定装置であって、予測された自己位置を示す予測位置情報を取得する第１取得部と、移動体から対象物までの計測部による計測距離である第１距離と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの距離である第２距離とを取得する第２取得部と、前記移動体の自己位置精度情報を取得する第３取得部と、前記第１距離と前記第２距離との差分値に所定の利得を乗じた値により、前記予測された自己位置を補正する補正部と、を備え、前記補正部は、前記自己位置精度情報に基づき、前記利得に対する補正係数を決定する。
請求項６に記載の発明は、自己位置推定装置であって、予測された自己位置を示す予測位置情報を取得する第１取得部と、移動体から対象物までの計測部による計測距離である第１距離と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの距離である第２距離とを取得する第２取得部と、前記移動体の自己位置精度情報を取得する第３取得部と、前記第１距離と前記第２距離の差分値、並びに、前記自己位置精度情報に基づき、前記予測された自己位置を補正する補正部と、を備え、前記補正部は、前記差分値と、前記自己位置精度情報とに基づき、前記差分値を評価する評価値を算出し、前記予測された自己位置を前記差分値により補正する度合いを前記評価値に基づき決定する。

請求項７に記載の発明は、自己位置推定装置が実行する制御方法であって、予測された自己位置を示す予測位置情報を取得する第１取得工程と、移動体から対象物までの計測部による計測距離である第１距離と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの距離である第２距離とを取得する第２取得工程と、前記移動体の自己位置精度情報を取得する第３取得工程と、前記第１距離と前記第２距離との差分値に所定の利得を乗じた値により、前記予測された自己位置を補正する補正工程と、を有し、前記補正工程は、前記自己位置精度情報に基づき、前記利得に対する補正係数を決定する。

請求項８に記載の発明は、コンピュータが実行するプログラムであって、予測された自己位置を示す予測位置情報を取得する第１取得部と、移動体から対象物までの計測部による計測距離である第１距離と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの距離である第２距離とを取得する第２取得部と、前記移動体の自己位置精度情報を取得する第３取得部と、前記第１距離と前記第２距離との差分値に所定の利得を乗じた値により、前記予測された自己位置を補正する補正部として前記コンピュータを機能させ、前記補正部は、前記自己位置精度情報に基づき、前記利得に対する補正係数を決定する。

運転支援システムの概略構成図である。 車載機の機能的構成を示すブロック図である。 地図ＤＢのデータ構造の一例である。 状態変数ベクトルを２次元直交座標で表した図である。 予測ステップと計測更新ステップとの概略的な関係を示す図である。 自車位置推定部の機能ブロックを示す。 車両の推定自車位置と地物との位置関係を示す。 自車位置推定処理のフローチャートである。 本実施例に基づく自車位置推定処理の実験結果を示す。 本実施例に基づく自車位置推定処理の実験結果を示す。 ある地物の地物情報の位置座標を進行方向に８０ｃｍずらした場合の従来の自車位置推定処理の実験結果を示す。 ある地物の地物情報の位置座標を進行方向に８０ｃｍずらした場合の実施例に基づく自車位置推定処理の実験結果を示す。

本発明の好適な実施形態によれば、自己位置推定装置は、予測された自己位置を示す予測位置情報を取得する第１取得部と、移動体から対象物までの計測部による計測距離を示す第１距離情報と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの距離を示す第２距離情報とを取得する第２取得部と、前記移動体の自己位置精度情報を取得する第３取得部と、前記第１距離情報及び前記第２距離情報が示す距離の差分値、並びに、前記自己位置精度情報に基づき、前記予測された自己位置を補正する補正部と、を備える。この態様によれば、自己位置推定装置は、第１距離情報及び第２距離情報が示す距離の差分値と、自己位置精度情報とに基づき、予測された自己位置を好適に補正することができる。

上記自己位置推定装置の一態様では、前記補正部は、前記差分値と、前記自己位置精度情報と、前記計測部の計測精度情報とに基づき、前記予測された自己位置を補正する。この態様では、自己位置推定装置は、計測部の計測精度をさらに勘案し、自己位置を好適に補正することができる。

上記自己位置推定装置の他の一態様では、前記補正部は、前記差分値と、前記自己位置精度情報とに基づき、前記差分値を評価する評価値を算出し、前記予測された自己位置を前記差分値により補正する度合いを前記評価値に基づいて決定する。この態様によれば、自己位置推定装置は、自己位置精度情報に基づき上述の差分値を評価することで、予測された自己位置を上述の差分値により補正する際の度合いを的確に定めることができる。これにより、第１距離情報又は第２距離情報に誤差が存在する場合であっても、自車位置推定精度が低下するのを好適に抑制することができる。この場合、好適には、前記補正部は、前記差分値に所定の利得を乗じた値により、前記予測された自己位置を補正し、前記補正部は、前記評価値に基づいて、前記利得に対する補正係数を決定するとよい。さらに好適には、前記利得は、カルマンゲインであるとよい。

上記自己位置推定装置の他の一態様では、前記第２取得部は、前記予測位置情報と、地図情報に記録された前記対象物の位置情報とに基づき、前記第２距離情報が示す距離を算出する。この態様により、自己位置推定装置は、地図情報に基づき第２距離情報を生成し、自己位置推定に好適に用いることができる。

本発明の他の好適な実施形態によれば、自己位置推定装置が実行する制御方法であって、予測された自己位置を示す予測位置情報を取得する第１取得工程と、移動体から対象物までの計測部による計測距離を示す第１距離情報、及び、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの距離を示す第２距離情報を取得する第２取得工程と、前記移動体の自己位置精度情報を取得する第３取得工程と、前記第１距離情報及び前記第２距離情報が示す距離の差分値、並びに、前記自己位置精度情報に基づき、前記予測された自己位置を補正する補正工程と、を有する。自己位置推定装置は、この制御方法を実行することで、予測された自己位置を好適に補正することができる。

本発明の他の好適な実施形態によれば、コンピュータが実行するプログラムであって、予測された自己位置を示す予測位置情報を取得する第１取得部と、移動体から対象物までの計測部による計測距離を示す第１距離情報と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの距離を示す第２距離情報とを取得する第２取得部と、前記移動体の自己位置精度情報を取得する第３取得部と、前記第１距離情報及び前記第２距離情報が示す距離の差分値、並びに、前記自己位置精度情報に基づき、前記予測された自己位置を補正する補正部として前記コンピュータを機能させる。コンピュータは、このプログラムを実行することで、予測された自己位置を好適に補正することができる。好適には、上記プログラムは、記憶媒体に記憶される。

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

図１は、本実施例に係る運転支援システムの概略構成図である。図１に示す運転支援システムは、車両に搭載され、車両の運転支援に関する制御を行う車載機１と、ライダ（Ｌｉｄａｒ：Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ、または、Ｌａｓｅｒ Ｉｌｌｕｍｉｎａｔｅｄ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）２と、ジャイロセンサ３と、車速センサ４と、ＧＰＳ受信機５とを有する。

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

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

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

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

記憶部１２は、制御部１５が実行するプログラムや、制御部１５が所定の処理を実行するのに必要な情報を記憶する。本実施例では、記憶部１２は、地物情報を含む地図ＤＢ１０を記憶する。図３は、地図ＤＢ１０のデータ構造の一例を示す。図３に示すように、地図ＤＢ１０は、施設情報、道路データ、及び地物情報を含む。地物情報は、地物ごとに当該地物に関する情報が関連付けられた情報であり、ここでは、地物のインデックスに相当する地物ＩＤと、緯度及び経度（及び標高）等により表わされた地物の絶対的な位置を示す位置情報とを少なくとも含んでいる。なお、地図ＤＢ１０は、定期的に更新されてもよい。この場合、例えば、制御部１５は、図示しない通信部を介し、地図情報を管理するサーバ装置から、自車位置が属するエリアに関する部分地図情報を受信し、地図ＤＢ１０に反映させる。

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

制御部１５は、プログラムを実行するＣＰＵなどを含み、車載機１の全体を制御する。本実施例では、制御部１５は、自車位置推定部１７を有する。制御部１５は、本発明における「第１取得部」、「第２取得部」、「第３取得部」、「補正部」、及びプログラムを実行する「コンピュータ」の一例である。

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

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

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

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

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

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

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

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

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

なお、上記の説明において、地物計測値Ｚ（ｔ）は本発明の「第１距離情報」の一例であり、地物予測値Ｚ⁻（ｔ）は本発明の「第２距離情報」の一例である。

［自車位置推定の詳細］
次に、本実施例における自車位置推定の詳細について説明する。概略的には、自車位置推定部１７は、地物計測値Ｚ（ｔ）と地物予測値Ｚ⁻（ｔ）との差分値を評価する評価値（「差分評価値」とも呼ぶ。）を算出し、差分評価値に応じてカルマンゲインＫ（ｔ）を補正する。これにより、自車位置推定部１７は、地図ＤＢ１０又はライダ２の計測値のいずれかに誤差が生じた場合であっても、自車位置推定精度の低下を好適に抑制する。

ライダを用いた道路標識などによるＥＫＦ（Ｅｘｔｅｎｄｅｄ Ｋａｒｍａｎ Ｆｉｌｔｅｒ）位置推定においては、上記の式（２）に示すように、地物計測値Ｚ（ｔ）と地物予測値Ｚ⁻（ｔ）との差分値を用いて自車位置を計算している。式（２）から理解されるように、式（１）に示す差分値が大きくなると、予測自車位置Ｘ⁻（ｔ）に対する補正量が大きくなる。ここで、差分値が大きくなるのは、以下の（Ａ）〜（Ｃ）のいずれかの原因による。

（Ａ）地物計測値に誤差がある。
これには、例えば、ライダの計測精度が低い場合、又は、ライダの計測精度は低くないが、計測時に計測対象の地物と車両との間に他車両などの動体が存在していたため計測精度が低くなってしまった場合（即ちオクルージョンが発生した場合）、などが該当する。

（Ｂ）地物予測値を求めるために使用する推定自車位置がずれている。
これは、予測自車位置Ｘ⁻（ｔ）あるいは推定自車位置Ｘ^＾（ｔ）の誤差が大きい場合である。

（Ｃ）地物予測値を求めるために使用する地図データの地物の位置座標がずれている。
これは、例えば地図データの作成時以降に、地物である標識が車両の衝突などにより傾いたり、地物である構造物が撤去又は移動されたりして地物の位置が変化した結果、地図データ中のその地物の位置座標と実際のその地物の位置座標とが一致しなくなっている状態である。すなわち、地図データ上の地物の位置と現実環境における地物の位置とにずれが生じている状態である。

上記（Ａ）または（Ｃ）に起因して差分値が大きくなっている場合に式（２）に基づき推定自車位置Ｘ^＾（ｔ）を算出すると、自車位置推定精度が低下する。よって、上記（Ａ）または（Ｃ）のいずれかが生じている場合、予測自車位置Ｘ⁻（ｔ）に対する補正の度合いをなるべく低くすることが好ましい。

図７は、車両の推定自車位置と地物との位置関係を示す。図７に示す座標系は車両を基準としたボディ座標系であり、ｘ軸は車両の進行方向を示し、ｙ軸はそれと垂直な方向（車両の横方向）を示す。図７において、推定自車位置は、誤差楕円４０により示される誤差の範囲を有する。誤差楕円４０は、ｘ方向の位置推定精度「σ_Ｐ（ｘ）」とｙ方向の位置推定精度「σ_Ｐ（ｙ）」により規定される。ここで位置推定精度σ_Ｐは、以下の式（３）により、ヤコビ行列「Ｈ（ｔ）」を用いて共分散行列Ｐ（ｔ）をボディ座標系に変換することにより得られる。

式（３）において、「σ_Ｐ（ｘ）^２」はｘ方向の分散、「σ_Ｐ（ｙ）^２」はｙ方向の分散であり、これらの平方根としてｘ方向の位置推定精度σ_Ｐ（ｘ）とｙ方向の位置推定精度σ_Ｐ（ｙ）が得られる。こうして、位置推定精度σ_Ｐ（σ_Ｐ（ｘ），σ_Ｐ（ｙ））が得られる。

図７において、地物予測値Ｚ⁻（ｔ）は、予測自車位置Ｘ⁻（ｔ）及び地図ＤＢ１０に登録された地物の位置ベクトルを用いてライダ２による計測処理をモデル化して求めたものである。また、地物計測値Ｚ（ｔ）は、地図ＤＢ１０に登録された地物の位置ベクトルとライダ２のスキャンデータとの対応付けができた場合の地物のライダ２による計測値である。地物計測値Ｚ（ｔ）は、誤差楕円４３で示す誤差の範囲を有する。誤差楕円４３は、ｘ方向の計測精度「σ_Ｌ（ｘ）」とｙ方向の計測精度「σ_Ｌ（ｙ）」により規定される。一般的に、ライダによる計測誤差はライダと測定対象となる地物との距離の２乗に比例して大きくなるため、推定自車位置から地物計測値Ｚ（ｔ）までの距離に基づいて誤差楕円４３を算出することにより、ライダ計測精度σ_Ｌ（σ_Ｌ（ｘ），σ_Ｌ（ｙ））が求められる。また、地物予測値Ｚ⁻（ｔ）と地物計測値Ｚ（ｔ）の差分値は、式（１）に示すように、ｘ方向が「ｄｘ」、ｙ方向が「ｄｙ」である。

なお、位置推定精度σ_Ｐは本発明の「自己位置精度情報」の一例であり、ライダ計測精度σ_Ｌは本発明の「計測精度情報」の一例である。

次に、地物計測値Ｚ（ｔ）と地物予測値Ｚ⁻（ｔ）との差分値を評価する評価値の算出方法について説明する。自車位置推定部１７は、位置推定精度σ_Ｐ（σ_Ｐ（ｘ），σ_Ｐ（ｙ））とライダ計測精度σ_Ｌ（σ_Ｌ（ｘ），σ_Ｌ（ｙ））とを用いて、地物計測値Ｚ（ｔ）と地物予測値Ｚ⁻（ｔ）との差分値を評価する。言い換えると、自車位置推定部１７は、推定自車位置の誤差範囲４０とライダ誤差範囲４３に対する差分値の割合に基づき、上述の差分値の妥当性を判断する。具体的には、自車位置推定部１７は、以下の評価式（４）により、差分評価値「Ｅｘ」、「Ｅｙ」を算出する。なお、この場合、差分値ｄｘ、ｄｙの絶対値が用いられる。

この場合、差分評価値Ｅｘ、Ｅｙが高いほど、上記の（Ａ）又は（Ｃ）のいずれかが生じている可能性が高いことが推定される。よって、自車位置推定部１７は、差分評価値Ｅｘ、Ｅｙの少なくとも一方が所定値より大きい場合、差分値が大きくなった原因として上記の（Ａ）又は（Ｃ）が生じていると判定する。よって、この場合、自車位置推定部１７は、差分評価値Ｅｘ、Ｅｙに応じて、カルマンゲインを小さくするための係数値「ａ_Ｘ（ｔ）」、「ａ_Ｙ（ｔ）」を算出し、カルマンゲインに乗じる。この場合、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）は、０から１までの値となり、かつ、差分評価値Ｅｘ、Ｅｙが大きいほど小さい値になるように、式又はテーブルを用いて設定される。例えば、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）は、以下の式（５）に基づき設定される。

そして、自車位置推定部１７は、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を用いて、以下の式（６）に基づき更新後のカルマンゲイン「Ｋ（ｔ）´」を算出する。

このようにカルマンゲインを設定することで、自車位置推定部１７は、地物予測値の信頼度が低い場合に，その信頼度に応じてカルマンゲインを小さくし、予測自車位置Ｘ⁻（ｔ）に対する補正量を少なくすることができる。よって、この場合，不正確な補正を防止できるため、推定自車推定精度が向上する。係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）は、本発明における「補正係数」の一例である。

図８は、車載機１の自車位置推定部１７により行われる自車位置推定処理のフローチャートである。車載機１は、図８のフローチャートの処理を繰り返し実行する。

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

その後、自車位置推定部１７は、１時刻前の推定自車位置Ｘ^＾（ｔ−１）に、ステップＳ１０３で計算した移動距離と方位変化を加算し、予測自車位置Ｘ⁻（ｔ）を算出する（ステップＳ１０４）。さらに、ステップＳ１０４では、自車位置推定部１７は、共分散行列から、式（３）により位置推定精度σ_Ｐ（ｘ），σ_Ｐ（ｙ）を求める。さらに、自車位置推定部１７は、予測自車位置Ｘ⁻（ｔ）に基づき、地図ＤＢ１０の地物情報を参照し、ライダ２の計測範囲となる地物を探索する（ステップＳ１０５）。

そして、自車位置推定部１７は、予測自車位置Ｘ⁻（ｔ）及びステップＳ１０５で探索した地物の地物情報が示す位置座標から、車両の進行方向と横方向それぞれの地物の予測位置（即ち地物予測値Ｚ⁻（ｔ））を算出する（ステップＳ１０６）。その後、自車位置推定部１７は、ライダ２のスキャンデータから車両の進行方向と横方向それぞれの地物までの計測距離（即ち地物計測値Ｚ（ｔ））を算出し、当該計測距離からライダ計測精度σ_Ｌ（ｘ），σ_Ｌ（ｙ）を算出する（ステップＳ１０７）。

次に、自車位置推定部１７は、地物計測値と地物予測値との差分値ｄｘ、ｄｙを式（１）に基づき算出する（ステップＳ１０８）。そして、自車位置推定部１７は、式（４）に基づき、差分評価値Ｅｘ、Ｅｙを算出する（ステップＳ１０９）。

そして、自車位置推定部１７は、差分評価値Ｅｘ、Ｅｙが所定値より大きい場合（ステップＳ１１０；Ｙｅｓ）、差分評価値Ｅｘ、Ｅｙから係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を生成する（ステップＳ１１１）。例えば、自車位置推定部１７は、式（５）に基づき係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を算出することで、０から１までの値域において、差分評価値Ｅｘ、Ｅｙが大きいほど小さい値になるように、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を算出する。なお、自車位置推定部１７は、所定値を超えていない差分評価値が存在する場合には、当該差分評価値に対応する係数値（差分評価値Ｅｘの場合には係数値ａ_Ｘ（ｔ））を式（５）によらず１に設定してもよい。

一方、自車位置推定部１７は、差分評価値Ｅｘ、Ｅｙが所定値以下の場合（ステップＳ１１０；Ｎｏ）、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を「１」に設定する（ステップＳ１１２）。すなわち、差分評価値Ｅｘと所定値の比較により係数値ａ_Ｘ（ｔ）が設定され、差分評価値Ｅｙと所定値の比較により係数値ａ_Ｙ（ｔ）が設定される。

次に、自車位置推定部１７は、式（６）に基づき、カルマンゲインＫ（ｔ）に係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を乗じることで、カルマンゲインＫ（ｔ）´を生成する（ステップＳ１１３）。その後、自車位置推定部１７は、式（２）に基づき、生成したカルマンゲインＫ（ｔ）´をＫ（ｔ）の代わりに用いて予測自車位置Ｘ⁻（ｔ）を補正し、推定自車位置Ｘ^＾（ｔ）を算出する（ステップＳ１１４）。

［具体例］
次に、本実施例に基づく自車位置推定処理の効果について説明する。

図９（Ａ）は、ある走行試験コースにおいて図８に示す自車位置推定処理を実行しながら車両を走行させた場合のｘ方向とｙ方向の位置の推移を示すグラフである。なお、図９（Ａ）では、ＲＴＫ−ＧＰＳを用いて高精度に計測した自車位置（「リファレンス位置」とも呼ぶ。）と、図８に示す自車位置推定処理により算出した推定自車位置とがそれぞれ描画されているが、これらは視覚上区別できない程度に重なっている。

図９（Ｂ）は、図９（Ａ）の枠７０内の走行区間におけるデータを拡大した図である。なお、図９（Ｂ）では、計測対象となった地物の位置が丸印によりプロットされている。また、図１０（Ａ）は、枠７０の走行区間での差分値ｄｘの推移を示すグラフであり、図１０（Ｂ）は、枠７０の走行区間での実施例に基づく推定自車位置とリファレンス位置との進行方向における差を示し、図１０（Ｃ）は、枠７０の走行区間での実施例に基づく推定自車位置とリファレンス位置との横方向における差を示し、図１０（Ｄ）は、枠７０の走行区間での実施例に基づく推定自車位置とリファレンス位置との方位差を示す。

図９（Ｂ）に示すように、自車位置推定部１７は、不規則な間隔で存在する地物を対象として地物情報に基づき推定自車位置を推定しており、図１０（Ａ）に示す差分値等に基づき予測自車位置を補正している。その結果、図１０（Ｂ）〜（Ｄ）に示されるように、車両の進行方向及び横方向でのリファレンス位置と推定自車位置との差は、およそ０．１ｍ以内となっており、方位差についても、およそ０．１度以内となっている。

図１１（Ａ）〜（Ｄ）は、図９（Ｂ）のプロット７１が示す地物に対する地物情報の位置座標を進行方向に８０ｃｍずらした場合において、カルマンゲインＫ（ｔ）を係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）により補正せずに用いたときの図１０（Ａ）〜（Ｄ）と同様の実験結果を示す。

図１１（Ａ）に示すように、図９（Ｂ）のプロット７１が示す地物に対する地物情報の位置座標のずれに起因して、プロット７１が示す地物の地物情報の位置座標を用いて算出した差分値ｄｘ（丸枠７２参照）は、他の地物の地物情報の位置座標を用いて算出した差分値ｄｘと比較して、ずれ量が顕著に大きくなっている。その結果、図１１（Ｂ）に示すように、図９（Ｂ）のプロット７１が示す地物に対する地物情報の位置座標を用いて算出した推定自車位置は、車両の進行方向においてリファレンス位置とずれが生じている（丸枠７３参照）。このようにして生じた推定自車位置の誤差は、次に自車位置推定に用いる地物（図９のプロット７４参照）の検出時まで継続してしまう。

図１２（Ａ）〜（Ｄ）は、図９（Ｂ）のプロット７１が示す地物に対する地物情報の位置座標を進行方向に８０ｃｍずらした場合において、カルマンゲインＫ（ｔ）を係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）により補正したときの図１１（Ａ）〜（Ｄ）と同様の実験結果を示す。

この場合、図９（Ｂ）のプロット７１が示す地物を検出した時点での差分値ｄｘ、位置推定精度σ_Ｐ（ｘ）、ライダ計測精度σ_Ｌ（ｘ）は、以下のようになる。
ｄｘ＝−０．７４９
σ_Ｐ（ｘ）＝０．０４８
σ_Ｌ（ｘ）＝０．０５８

したがって、これらの値を式（５）に示す係数値ａ_Ｘ（ｔ）に代入すると、係数値ａ_Ｘ（ｔ）は
０．８５４×１０^−３≒０
となる。この場合、カルマンゲインＫ（ｔ）´は、以下の式（７）に示すように求められる。

よって、この場合、車両の進行方向については、予測自車位置Ｘ⁻（ｔ）の補正が行われないため、精度の良い位置推定が維持されることになる。

以上説明したように、本実施例に係る車載機１の自車位置推定部１７は、予測自車位置Ｘ⁻（ｔ）を示す情報を生成する位置予測部２１と、位置推定部２２とを有する。位置推定部２２は、移動体から対象物までのライダ２による計測距離を示す地物計測値Ｚ（ｔ）及び地物情報に基づき予測された地物予測値Ｚ⁻（ｔ）を算出する。そして、位置推定部２２は、地物計測値と地物予測値との差分値ｄｘ、ｄｙと、位置推定精度σ_Ｐと、ライダ計測精度σ_Ｌとに基づく差分評価値Ｅｘ、Ｅｙに応じて、カルマンゲインＫ（ｔ）を補正する係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を決定する。これにより、車載機１は、地図ＤＢ１０又はライダ２の計測値のいずれかに誤差が生じた場合であっても、自車位置推定精度の低下を好適に抑制することができる。

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

（変形例１）
信頼度が低いと判断される地物情報に基づく予測自車位置Ｘ⁻（ｔ）の補正量を低下させる方法は、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）をカルマンゲインＫ（ｔ）に乗じることに限定されない。

これに代えて、第１の例では、自車位置推定部１７は、差分値ｄｘ、ｄｙに対して係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を乗じてもよい。即ち、この場合、自車位置推定部１７は、以下の式（８）に基づき、推定自車位置を算出する。

なお、この場合、カルマンゲインＫ（ｔ）は式（１０）で生成された後、式（６）に基づく更新が行われないため、以下の一般式（９）により示される共分散行列Ｐ（ｔ）の更新には、信頼度が低い地物情報を用いたということが反映されない。すなわち、位置推定は信頼度を反映する一方、共分散行列は信頼度が反映されないため、その違いを考慮した処理が必要となる。

第２の例では、自車位置推定部１７は、以下の一般式（１０）によりカルマンゲインＫ（ｔ）を算出する際に用いる観測雑音行列「Ｒ（ｔ）」の対角成分に、以下の式（１１）に示すように係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を乗じてもよい。

この場合、自車位置推定部１７は、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を、式（４）に示される評価値Ｅｘ、Ｅｙが大きいほど大きくなるように設定する。例えば、自車位置推定部１７は、以下の式（１２）に示すように係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を設定する。

この場合、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）の取り得る範囲は、１から無限大まであるため、上限値を定めるなどのオーバーフローを回避する処理が必要となる。

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

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

Claims (9)

1. 予測された自己位置を示す予測位置情報を取得する第１取得部と、
   移動体から対象物までの計測部による計測距離である第１距離と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの距離である第２距離とを取得する第２取得部と、
   前記移動体の自己位置精度情報を取得する第３取得部と、
   前記第１距離と前記第２距離との差分値に所定の利得を乗じた値により、前記予測された自己位置を補正する補正部と、を備え、
   前記補正部は、前記自己位置精度情報に基づき、前記利得に対する補正係数を決定する、
   自己位置推定装置。
2. 前記補正部は、前記自己位置精度情報と、前記計測部の計測精度情報とに基づき、前記補正係数を決定する請求項１に記載の自己位置推定装置。
3. 前記補正部は、前記差分値と、前記自己位置精度情報とに基づき、前記差分値を評価する評価値を算出し、前記補正係数を前記評価値に基づき決定する請求項１または２に記載の自己位置推定装置。
4. 前記利得は、カルマンゲインである請求項１〜３のいずれか一項に記載の自己位置推定装置。
5. 前記第２取得部は、前記予測位置情報と、地図情報に記録された前記対象物の位置情報とに基づき、前記第２距離を算出する請求項１〜４のいずれか一項に記載の自己位置推定装置。
6. 予測された自己位置を示す予測位置情報を取得する第１取得部と、
   移動体から対象物までの計測部による計測距離である第１距離と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの距離である第２距離とを取得する第２取得部と、
   前記移動体の自己位置精度情報を取得する第３取得部と、
   前記第１距離と前記第２距離の差分値、並びに、前記自己位置精度情報に基づき、前記予測された自己位置を補正する補正部と、を備え、
   前記補正部は、前記差分値と、前記自己位置精度情報とに基づき、前記差分値を評価する評価値を算出し、前記予測された自己位置を前記差分値により補正する度合いを前記評価値に基づき決定する自己位置推定装置。
7. 自己位置推定装置が実行する制御方法であって、
   予測された自己位置を示す予測位置情報を取得する第１取得工程と、
   移動体から対象物までの計測部による計測距離である第１距離と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの距離である第２距離とを取得する第２取得工程と、
   前記移動体の自己位置精度情報を取得する第３取得工程と、
   前記第１距離と前記第２距離との差分値に所定の利得を乗じた値により、前記予測された自己位置を補正する補正工程と、を有し、
   前記補正工程は、前記自己位置精度情報に基づき、前記利得に対する補正係数を決定する制御方法。
8. コンピュータが実行するプログラムであって、
   予測された自己位置を示す予測位置情報を取得する第１取得部と、
   移動体から対象物までの計測部による計測距離である第１距離と、前記対象物の位置情報に基づき予測された前記移動体から前記対象物までの距離である第２距離とを取得する第２取得部と、
   前記移動体の自己位置精度情報を取得する第３取得部と、
   前記第１距離と前記第２距離との差分値に所定の利得を乗じた値により、前記予測された自己位置を補正する補正部
   として前記コンピュータを機能させ、
   前記補正部は、前記自己位置精度情報に基づき、前記利得に対する補正係数を決定するプログラム。
9. 請求項８に記載のプログラムを記憶した記憶媒体。

Images