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

Abstract

【課題】地物の計測誤差の可能性があった場合でも，自己位置推定精度の劣化を好適に抑制することが可能な自己位置推定装置を提供する【解決手段】車載機１の自車位置推定部１７は、予測自車位置Ｘ－（ｔ）を示す情報を生成すると共に、ライダ２により計測対象となる地物の法線情報とサイズ情報とを地図ＤＢ１０から取得する。そして、自車位置推定部１７は、車両の進行方向の情報と地物の法線情報から算出される角度差Δθと、地物のサイズ情報が示すサイズＳとに基づき、予測自車位置Ｘ－（ｔ）を補正する。【選択図】図７

Description

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

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

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

自己位置推定処理において計測対象となる道路標識や方面看板などの地物は，運転手が認識しやすいように作製及び設置されているが、全てのサイズや向きが一定となっているわけではなく、表示内容に応じて大きさが異なり、また設置位置に応じて向きが異なる。その結果、自己位置推定を行うシステムが計測対象とする地物が計測しにくいサイズや向きである場合も存在し，そのような場合には計測対象の地物の計測値に誤差が大きく含まれる可能性もある。このように誤差を含んだ計測値を用いて自己位置推定を行うと、自己位置推定精度が低下してしまう。

本発明は、上記のような課題を解決するためになされたものであり、地物の計測誤差の可能性があった場合でも，自己位置推定精度の劣化を好適に抑制することが可能な自己位置推定装置を提供することを主な目的とする。

請求項１に記載の発明は、自己位置推定装置であって、予測された自己位置を示す予測位置情報を取得する第１取得部と、移動体の進行方向の情報を取得する第２取得部と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第３取得部と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正部と、を備える。

請求項８に記載の発明は、自己位置推定装置が実行する制御方法であって、予測された自己位置を示す予測位置情報を取得する第１取得工程と、移動体の進行方向の情報を取得する第２取得工程と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第３取得工程と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正工程と、を有する。

請求項９に記載の発明は、コンピュータが実行するプログラムであって、予測された自己位置を示す予測位置情報を取得する第１取得部と、移動体の進行方向の情報を取得する第２取得部と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第３取得部と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正部として前記コンピュータを機能させる。

運転支援システムの概略構成図である。 車載機の機能的構成を示すブロック図である。 地図ＤＢのデータ構造の一例である。 状態変数ベクトルを２次元直交座標で表した図である。 予測ステップと計測更新ステップとの概略的な関係を示す図である。 自車位置推定部の機能ブロックを示す。 ライダの計測対象となる地物と車両との位置関係の一例を表す。 係数値の設定例を説明するための図である。 自車位置推定処理のフローチャートである。 ライダによるレーザ光が照射される地物の被照射面を示した図である。

本発明の好適な実施形態によれば、自己位置推定装置は、予測された自己位置を示す予測位置情報を取得する第１取得部と、移動体の進行方向の情報を取得する第２取得部と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第３取得部と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正部と、を備える。この態様により、自己位置推定装置は、予測した自己位置を、移動体の進行方向に対する地物の向きやサイズに基づき好適に補正することができる。

上記自己位置推定装置の一態様では、自己位置推定装置は、前記移動体から前記地物までの前記計測部による計測距離を示す第１距離情報と、前記地物の位置情報に基づき予測された前記移動体から前記地物までの距離を示す第２距離情報とを取得する第４取得部をさらに備え、前記補正部は、前記予測された自己位置を前記第１距離情報及び前記第２距離情報が示す距離の差分値により補正する度合いを、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき定める。この態様により、自己位置推定装置は、第１距離情報及び第２距離情報が示す距離の差分値により予測自己位置を補正する度合いを、移動体の進行方向に対する地物の向きやサイズに基づき好適に調整することができる。

上記自己位置推定装置の他の一態様では、前記補正部は、前記差分値に所定の利得を乗じた値により、前記予測された自己位置を補正し、前記補正部は、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づいて、前記利得に対する補正係数を決定する。この態様により、自己位置推定装置は、第１距離情報及び第２距離情報が示す距離の差分値により予測自己位置を補正する度合いを定める利得に対する補正係数を、移動体の進行方向に対する地物の向きやサイズに基づき定めることができる。好適には、前記利得は、カルマンゲインである。

上記自己位置推定装置の他の一態様では、前記補正部は、前記進行方向と前記法線情報が示す前記地物の法線方向との角度差が大きいほど、前記進行方向において前記予測された自己位置を補正する度合いを低くし、前記角度差が小さいほど、前記移動体の横方向において前記予測された自己位置を補正する度合いを低くする。このようにすることで、自己位置推定装置は、地物の位置計測に誤差が生じやすい方向に対する自己位置の補正の度合いを低くし、位置推定精度を好適に向上させることができる。

上記自己位置推定装置の他の一態様では、前記補正部は、前記サイズ情報が示す前記地物のサイズが小さいほど、前記予測された自己位置を補正する度合いを小さくする。一般に、計測する地物のサイズが小さいほど、当該地物に対する計測点が少なくなり、当該地物の位置計測精度が低下する。よって、自己位置推定装置は、この態様により、位置推定精度の低下を好適に抑制することができる。

上記自己位置推定装置の他の一態様では、前記補正部は、前記サイズ情報が示す前記地物のサイズよりも、前記計測部が計測した前記地物のサイズが小さいほど、前記予測された自己位置を補正する度合いを小さくする。一般に、オクルージョン等に起因して地物の一部分しか計測できない場合には、当該地物の位置計測精度は低くなる。よって、自己位置推定装置は、この態様により、オクルージョン等に起因して地物の一部分しか計測できない場合に予測自己位置の補正の度合いを低下させて自己位置推定精度の低下を好適に抑制することができる。

本発明の他の好適な実施形態によれば、自己位置推定装置が実行する制御方法であって、予測された自己位置を示す予測位置情報を取得する第１取得工程と、移動体の進行方向の情報を取得する第２取得工程と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第３取得工程と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正工程と、を有する。自己位置推定装置は、この制御方法を実行することで、予測した自己位置を、移動体の進行方向に対する地物の向きやサイズに基づき好適に補正することができる。

本発明の他の好適な実施形態によれば、コンピュータが実行するプログラムであって、予測された自己位置を示す予測位置情報を取得する第１取得部と、移動体の進行方向の情報を取得する第２取得部と、前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第３取得部と、前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正部として前記コンピュータを機能させる。コンピュータは、このプログラムを実行することで、予測した自己位置を、移動体の進行方向に対する地物の向きやサイズに基づき好適に補正することができる。好適には、上記プログラムは、記憶媒体に記憶される。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

［自車位置推定の詳細］
次に、本実施例における自車位置推定の詳細について説明する。概略的には、自車位置推定部１７は、ライダ２による計測対象の地物の地物情報に含まれる法線情報及びサイズ情報に基づき、カルマンゲインを補正する。これにより、自車位置推定部１７は、計測対象の地物の向き及びサイズに応じて車両の進行方向及び横方向での地物計測値の精度を推定し、当該精度に応じて各方向における予測自車位置Ｘ^－（ｔ）の補正量を的確に定める。

図７は、ライダ２の計測対象となる地物５０と車両との位置関係の一例を表す図である。図７において、矢印４０は、車両の進行方向を示し、矢印４１は、地物５０の法線方向を示す。また、計測点「Ｐ１」～「Ｐ７」は、ライダ２が出力した点群データの各点の位置を示す。さらに、「Δθ」は、矢印４０が示す車両の進行方向と矢印４１が示す地物５０の法線方向との角度差を示す。なお、角度差Δθは、０～９０度の範囲であるものとする。

図７の例では、自車位置推定部１７は、ライダ２から計測点Ｐ１～Ｐ７を示す点群データを取得し、取得した点群データからｘ方向及びｙ方向のそれぞれの地物計測値Ｚ（ｔ）を算出する。この場合、自車位置推定部１７は、地物５０に対する計測点Ｐ１～Ｐ７が示すボディ座標系のｘ座標の重心座標（即ち各計測点が示す座標の平均値）を、ｘ方向の地物計測値Ｚ（ｔ）として算出し、地物５０に対する計測点Ｐ１～Ｐ７が示すボディ座標系のｙ座標の重心座標を、ｙ方向の地物計測値Ｚ（ｔ）として算出する。

このとき、角度差Δθが小さい（即ち平行に近い）ほど、地物計測値の進行方向成分の精度が高まり、横方向成分の精度が低くなる。言い換えると、角度差Δθが大きい（即ち垂直に近い）ほど、地物計測値の横方向成分の精度が高まり、進行方向成分の精度が低くなる。また、地物５０のサイズが大きいほど、当該地物５０に対するライダ２の計測点が多くなるため、地物計測値の精度が高まる。

以上を勘案し、自車位置推定部１７は、角度差Δθと、計測対象の地物のサイズ情報とに基づき、カルマンゲインを小さくするための係数値「ａ_Ｘ（ｔ）」、「ａ_Ｙ（ｔ）」を算出し、以下の式（３）に示すように、これらをカルマンゲインＫ（ｔ）に乗じることで、補正後のカルマンゲイン「Ｋ（ｔ）´」を算出する。

これにより、自車位置推定部１７は、地物計測値の精度が高い方向に対する予測自車位置Ｘ^－（ｔ）への地物計測値Ｚ（ｔ）と地物予測値Ｚ^－（ｔ）との差分値による補正量を相対的に大きくし、地物計測値の精度が低い方向に対する予測自車位置Ｘ^－（ｔ）への上述の差分値による補正量を相対的に小さくする。

この場合、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）は、０から１までの値となり、かつ、計測対象の地物のサイズが大きいほど大きい値となるように定められる。さらに、係数ａ_Ｘ（ｔ）は、角度差Δθが大きいほど小さい値となり、ａ_Ｙ（ｔ）は、角度差Δθが大きいほど大きい値となるように定められる。言い換えると、係数ａ_Ｘ（ｔ）は、角度差Δθが小さいほど大きい値となり、ａ_Ｙ（ｔ）は、角度差Δθが小さいほど小さい値となるように定められる。係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）は、式又はテーブルを用いて設定され、例えば、計測対象の地物のサイズ（例えば面積）を「Ｓ」とすると、以下の式（４）に基づき設定される。

このように算出した係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）に基づきカルマンゲインを補正することで、誤差量が多い可能性のある小さいサイズの地物（看板等）を計測対象とした場合にはカルマンゲインが小さくなり，誤差量が少ないと思われる大きいサイズの地物を計測対象とした場合にはカルマンゲインは小さくならない。また、車両から見て誤差が大きくなる方向のカルマンゲインが小さくなり、誤差が少ない方向のカルマンゲインは小さくならない。よって、計測誤差が多く含まれる可能性のあるデータの重みが相対的に小さくなり、計算される自車位置の精度が向上する。係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）は、本発明における「補正係数」の一例である。

次に、図８を参照して係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）の設定例について説明する。

図８（Ａ）は、ライダ２によるスキャンを実行中の車両の俯瞰図を示す。図８（Ａ）では、ライダ２の計測範囲内に法線方向が異なる地物５１、５２が存在している。図８（Ｂ）は、ライダ２の計測点「Ｐ１１」～「Ｐ１５」を明示した地物５１の拡大図である。また、図８（Ｃ）は、ライダ２の計測点「Ｐ１６」～「Ｐ１９」を明示した地物５２の拡大図である。図８（Ｂ）、（Ｃ）では、それぞれ、各照射点の位置座標から算出した車両の横方向及び進行方向の重心座標が破線により示されている。なお、図８（Ａ）～（Ｃ）では、説明便宜上、ライダ２が所定の走査面に沿ってレーザ光を出射した場合を例示しているが、高さの異なる複数の走査面に沿ってレーザ光を出射してもよい。

図８（Ａ）及び図８（Ｂ）に示すように、地物５１の法線方向は、車両の横方向と略一致している。従って、照射点Ｐ１１～Ｐ１５は、車両の進行方向に沿って並んでおり、進行方向におけるばらつきが大きく、横方向のばらつきが小さい。従って、地物５１を用いた位置推定では、照射点Ｐ１１～Ｐ１５のばらつきが小さい横方向成分の地物計測値の精度が高くなることが予測される。そして、この場合、角度差Δθは、ほぼ９０度であり、「ｃｏｓΔθ≒０」かつ「ｓｉｎΔθ≒１」となるため、式（４）に基づく係数値ａ_Ｘ（ｔ）は０に近い値となり、係数値ａ_Ｙ（ｔ）はサイズＳに応じた値となる。よって、この場合、地物計測値の精度が低くなるｘ方向（即ち進行方向）に対する予測自車位置Ｘ^－（ｔ）への補正量を相対的に小さくすることができる。

一方、図８（Ａ）及び図８（Ｃ）に示すように、地物５２の法線方向は、車両の進行方向と略一致している。従って、照射点Ｐ１６～Ｐ１９は、車両の横方向に沿って並んでおり、横方向におけるばらつきが大きく、進行方向のばらつきが小さい。従って、地物５２を用いた位置推定では、照射点Ｐ１６～Ｐ１９のばらつきが小さい進行方向成分の地物計測値の精度が高くなることが予測される。そして、この場合、角度差Δθは、ほぼ０度であり、「ｃｏｓΔθ≒１」かつ「ｓｉｎΔθ≒０」となるため、式（４）に基づく係数値ａ_Ｘ（ｔ）はサイズＳに応じた値となり、係数値ａ_Ｙ（ｔ）は０に近い値となる。よって、この場合、地物計測値の精度が低くなるｙ方向（即ち横方向）に対する予測自車位置Ｘ^－（ｔ）への補正量を相対的に小さくすることができる。なお、係数値ａ_Ｘ（ｔ）とａ_Ｙ（ｔ）は、下限値を設けてもよい。その場合、式（４）で計算される結果が所定の下限値以下となる場合は，その下限値が設定される。これにより、Δθによっては補正されない方向が継続してしまう状況を回避できる。

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

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

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

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

そして、自車位置推定部１７は、ステップＳ１０５で探索した地物の地物情報のサイズ情報及び法線情報を参照し、当該地物の被照射面のサイズＳと、車両の進行方向と対象の地物の法線ベクトルとの角度差Δθを算出する（ステップＳ１０７）。この場合、自車位置推定部１７は、角度差Δθを、予測自車位置Ｘ^－（ｔ）が示す自車の方位Ψと、地物情報に含まれる法線情報が示す地物の法線方向とに基づき算出する。

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

以上説明したように、本実施例に係る車載機１の自車位置推定部１７は、予測自車位置Ｘ^－（ｔ）を示す情報を生成すると共に、ライダ２により計測対象となる地物の法線情報とサイズ情報とを地図ＤＢ１０から取得する。そして、自車位置推定部１７は、車両の進行方向の情報及び地物の法線情報から算出される角度差Δθと、地物のサイズ情報が示すサイズＳとに基づき、予測自車位置Ｘ^－（ｔ）を補正する。これにより、自車位置推定部１７は、計測対象の地物の向き及びサイズに応じて車両の進行方向及び横方向での地物計測値の精度を推定し、当該精度に応じて各方向における予測自車位置の補正量を的確に定めることができる。

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

（変形例１）
予測自車位置Ｘ^－（ｔ）の補正量を調整する方法は、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）をカルマンゲインＫ（ｔ）に乗じることに限定されない。

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

なお、この場合、カルマンゲインＫ（ｔ）は後述する式（７）で生成された後、式（３）に基づく更新が行われないため、以下の一般式（６）により示される共分散行列Ｐ（ｔ）の更新に、地物の向き及びサイズに基づく地物計測値の信頼度が反映されない。すなわち、位置推定は信頼度を反映する一方、共分散行列は信頼度が反映されないため、その違いを考慮した処理が必要となる。

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

この場合、自車位置推定部１７は、係数値ａ_Ｘ（ｔ）を、サイズＳが大きいほど小さくなり、かつ、角度差Δθが小さいほど小さい値となるように設定し、係数値ａ_Ｙ（ｔ）を、サイズＳが大きいほど小さい値となり、かつ、角度差Δθが大きいほど小さい値となるように設定する。例えば、自車位置推定部１７は、以下の式（９）に示すように係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を設定する。

本変形例によっても、自車位置推定部１７は、予測自車位置Ｘ^－（ｔ）の補正量を、サイズＳ及び角度差Δθに基づき決定した係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）を用いて好適に調整することができる。

（変形例２）
自車位置推定部１７は、地図ＤＢ１０に登録されたサイズ情報から特定される計測対象の地物のサイズと、ライダ２の計測データから特定されるサイズとの比を勘案してカルマンゲインを補正してもよい。

図１０は、ライダ２によるレーザ光が照射される地物５５の被照射面を示した図である。図１０の例では、地物５５の被照射面上での計測点が明示されている。なお、地物５５の左側の一部は、障害物によるオクルージョン等に起因してライダ２のレーザ光が照射されていない。

この場合、自車位置推定部１７は、地物５５に対応する地物情報に含まれるサイズ情報に基づき、地物５５の高さ方向の幅「Ｈ_Ｍ」と、横方向の幅「Ｗ_Ｍ」とをそれぞれ特定する。また、自車位置推定部１７は、ライダ２が出力する点群データに基づき、地物５５に対する計測点が分布する範囲（計測範囲）の高さ方向の幅「Ｈ_Ｌ」と、横方向の幅「Ｗ_Ｌ」とをそれぞれ特定する。

そして、自車位置推定部１７は、地物５５に対する距離計測の正確さを示す信頼度情報「ａ（ｔ）」を、以下の式（１０）を用いて生成する。

式（１０）に示すように、信頼度情報ａ（ｔ）は、地図ＤＢ１０に基づき特定される地物のサイズ（面積）に対するライダ２の計測データに基づき特定される地物のサイズの比に相当し、０から１までの値域を有し、かつオクルージョンが少ないほど高い値となる。

そして、自車位置推定部１７は、例えば、算出した信頼度情報ａ（ｔ）を、係数値ａ_Ｘ（ｔ）、ａ_Ｙ（ｔ）と共にカルマンゲインに乗じることで、地物に対する距離計測の正確さを示す信頼度に応じた予測自車位置の補正量を定めることができる。

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

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

Claims (1)

1. 予測された自己位置を示す予測位置情報を取得する第１取得部と、
   移動体の進行方向の情報を取得する第２取得部と、
   前記移動体と共に移動する計測部により計測対象となる地物の法線情報とサイズ情報とを取得する第３取得部と、
   前記進行方向の情報と、前記法線情報と、前記サイズ情報とに基づき、前記予測された自己位置を補正する補正部と、
   を備える自己位置推定装置。

Images