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

Abstract

【課題】現在位置を高精度に推定することが可能な推定装置、制御方法、プログラム及び記憶媒体を提供する。【解決手段】運転支援システムは、車両に搭載され、車両の運転支援に関する制御を行う車載機と、ライダと、ジャイロセンサと、車速センサと、方位センサとを有する。車載機は、インデックスｋの基準ランドマークＬｋと自車両との位置関係を示す計測値ｚｔｋをライダから取得する。車載機は、地図ＤＢ１０に含まれる基準ランドマークＬｋの位置ベクトルｍｋと、計測値ｚｔｋと、方位センサが出力する方位θｔとに基づいて、自車位置の推定値ｘ−ｔ＝（ｘ−ｔ、ｙ−ｔ）を算出する。【選択図】図１３

Description

本発明は、現在位置を高精度に推定する技術に関する。

従来から、周辺に存在する物体との距離を測定する技術が知られている。例えば、特許文献１には、レーザ光を間欠的に発光させつつ水平方向を走査し、その反射光（散乱光）を受信することで、物体表面の点群を検出するライダを車両に搭載した例が開示されている。また、特許文献２には、検索対象となる複数の地点の緯度経度情報を含む地図データベースを有する地点検索装置が開示されている。

特開２０１４−０８９６９１号公報 特開２０１５−１３５６９５号公報

自動運転などの分野では、高精度な現在位置の推定を行うことが必要となっているのに対し、従来のような車両の状態を検知する内界センサの出力を主とした現在位置の推定方法では十分でない場合がある。特許文献１−２には、車両の絶対位置を高精度に算出する方法については何ら開示がない。

本発明は、上記のような課題を解決するためになされたものであり、現在位置を高精度に推定することが可能な推定装置を提供することを主な目的とする。

請求項１に記載の発明は、推定装置であって、地図情報を取得する取得部と、移動体の基準方位を取得する方位取得部と、第１範囲に存在する対象物までの距離及び角度を示す第１情報を取得する第１取得部と、前記地図情報に含まれる前記対象物の位置情報、前記移動体の基準方位及び前記第１情報に基づいて、前記移動体の位置を推定する第１推定部と、を備えることを特徴とする。

請求項６に記載の発明は、移動体の基準方位を取得する推定装置が実行する制御方法であって、地図情報を取得する取得工程と、第１範囲に存在する対象物までの距離及び角度を示す第１情報を取得する第１取得工程と、前記地図情報に含まれる前記対象物の位置情報、前記移動体の基準方位及び前記第１情報に基づいて、前記移動体の位置を推定する第１推定工程と、を有することを特徴とする。

請求項７に記載の発明は、移動体の基準方位を取得するコンピュータが実行するプログラムであって、地図情報を取得する取得部と、第１範囲に存在する対象物までの距離及び角度を示す第１情報を取得する第１取得部と、前記地図情報に含まれる前記対象物の位置情報、前記移動体の基準方位及び前記第１情報に基づいて、前記移動体の位置を推定する第１推定部として前記コンピュータを機能させることを特徴とする。

第１実施例に係る運転支援システムの概略構成図である。 車載機の機能的構成を示すブロック図である。 自車位置を２次元直交座標上で表した図である。 予測ステップと計測更新ステップとの概略的な関係を示す図である。 第１実施例における自車位置推定部の機能的な構成を示すブロック図である。 円運動を行ったと仮定した場合の移動前後での自車位置を各変数により表した図である。 ランドマークの位置と自車位置との関係を示す図である。 ランドマーク抽出ブロックの機能的構成を示す図である。 ライダのスキャン範囲とランドマークの探索範囲との関係を示す図である。 第１実施例において自車位置推定部が実行する処理の手順を示すフローチャートである。 ランドマーク抽出処理の詳細を示すフローチャートである。 第２実施例に係る運転支援システムの概略構成図である。 第２実施例における自車位置推定部の機能的な構成を示すブロック図である。 ランドマークの位置と自車位置との関係を示す図である。

本発明の好適な実施形態によれば、推定装置は、地図情報を取得する取得部と、移動体の基準方位を取得する方位取得部と、第１範囲に存在する対象物までの距離及び角度（即ち、移動体と第１範囲に存在する地物との位置関係）を示す第１情報を取得する第１取得部と、前記地図情報に含まれる前記対象物の位置情報、前記移動体の基準方位及び前記第１情報に基づいて、前記移動体の位置を推定する第１推定部と、を備える。

上記推定装置は、取得部と、方位取得部と、第１取得部と、第１推定部とを有する。取得部は、地図情報を取得する。方位取得部は、移動体の基準方位を取得する。「基準方位」とは、移動体の所定の向き（例えば正面方向）を基準とした絶対的な方位を示す。第１取得部は、対象物と移動体との位置関係を示す第１情報を取得する。第１推定部は、地図情報に含まれる対象物の位置情報、移動体の基準方位及び第１情報に基づいて、移動体の位置を推定する。この態様により、推定装置は、地図情報に登録された対象物の位置情報と、取得した移動体の基準方位とを利用し、的確に移動体の位置を推定することができる。

上記推定装置の一態様では、前記第１推定部は、前記第１情報が示す距離が、前記地図情報が示す前記対象物の位置と前記第１推定部が算出すべき前記移動体の推定位置との距離と等しく、かつ、前記第１情報が示す角度が、前記地図情報が示す前記対象物の位置と前記推定位置と前記基準方位とに基づき算出した、前記移動体に対する前記対象物の角度と等しいとみなすことで、前記推定位置を算出する。この態様により、推定装置は、地図情報に登録された対象物の位置情報と、取得した移動体の基準方位とを利用し、移動体の推定位置を的確に算出することができる。

上記推定装置の他の一態様では、前記方位取得部は、地磁気センサ、方位磁石、又はＧＰＳ受信機の少なくともいずれか一つから、前記基準方位を取得する。この態様により、推定装置は、移動体の絶対的な方位を示す基準方位を好適に取得することができる。

上記推定装置の他の一態様では、前記方位取得部は、所定時間前に取得した前記基準方位と、測定された前記移動体の角速度とに基づき、現在時刻での前記基準方位を算出する。この態様により、推定装置は、移動体の絶対的な方位を示す基準方位の推定値を取得することができる。

上記推定装置の他の一態様では、前記第１取得部は、照射方向を変えながらパルス状にレーザを照射して当該レーザに対応する散乱光を測定する測定装置から前記第１情報を取得する。この態様により、第１取得部は、対象物までの距離及び角度を示す高精度な第１情報を取得することができる。

本発明の他の好適な実施形態によれば、推定装置が実行する制御方法であって、地図情報を取得する取得工程と、移動体の基準方位を取得する方位取得工程と、第１範囲に存在する対象物までの距離及び角度を示す第１情報を取得する第１取得工程と、前記地図情報に含まれる前記対象物の位置情報、前記移動体の基準方位及び前記第１情報に基づいて、前記移動体の位置を推定する第１推定工程と、を有する。情報処理装置は、この制御方法を実行することで、地図情報に登録された対象物の位置情報と、取得した移動体の基準方位とを利用し、的確に移動体の位置を推定することができる。

本発明の他の好適な実施形態によれば、コンピュータが実行するプログラムであって、地図情報を取得する取得部と、移動体の基準方位を取得する方位取得部と、第１範囲に存在する対象物までの距離及び角度を示す第１情報を取得する第１取得部と、前記地図情報に含まれる前記対象物の位置情報、前記移動体の基準方位及び前記第１情報に基づいて、前記移動体の位置を推定する第１推定部として前記コンピュータを機能させる。コンピュータは、このプログラムを実行することで、地図情報に登録された対象物の位置情報と、取得した移動体の基準方位とを利用し、的確に移動体の位置を推定することができる。好適には、上記プログラムは、記憶媒体に記憶される。

以下、図面を参照して本発明の好適な各実施例について説明する。

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

車載機１は、ライダ２、ジャイロセンサ３、及び車速センサ４と電気的に接続し、これらの出力に基づき、車載機１が搭載される車両の位置（「自車位置」とも呼ぶ。）の推定を行う。そして、車載機１は、自車位置の推定結果に基づき、設定されたルートに沿って走行するように、車両の自動運転制御などを行う。車載機１は、道路データ及び道路付近に設けられた目印となるランドマークに関する情報（「ランドマーク情報」とも呼ぶ。）を記憶した地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶する。ランドマーク情報は、各ランドマークに割り当てられたインデックスと、ランドマークの位置情報とが少なくとも関連付けられた情報である。そして、車載機１は、このランドマーク情報に基づき、ライダ２によるランドマークの探索範囲を限定したり、ライダ２等の出力と照合させて自車位置の推定を行ったりする。以後では、車載機１が自車位置推定のために基準とするランドマークを「基準ランドマークＬｋ」と呼び、基準ランドマークＬｋのインデックスを「ｋ」とする。基準ランドマークＬｋは、本発明における「対象物」の一例である。

基準ランドマークＬｋの候補となるランドマークは、例えば、道路脇に周期的に並んでいるキロポスト、１００ｍポスト、デリニエータ、交通インフラ設備（例えば標識、方面看板、信号）、電柱、街灯などの地物である。好適には、上述のランドマークは、安定した計測を可能とするため、人工物であることが好ましく、定期的に自車位置を補正できるように、周期的に設けられた地物であることがより好ましい。なお，その間隔は厳格に一定周期である必要はなく，ある程度の周期性を持って存在するものであれば良い（例えば電柱や街灯など）。また走行エリアごとに異なる間隔となっているものでも良い。

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

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

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

記憶部１２は、制御部１５が実行するプログラムや、制御部１５が所定の処理を実行するのに必要な情報を記憶する。本実施例では、記憶部１２は、ランドマーク情報を含む地図ＤＢ１０を記憶する。なお、地図ＤＢ１０は、定期的に更新されてもよい。この場合、例えば、制御部１５は、図示しない通信部を介し、地図情報を管理するサーバ装置から、自車位置が属するエリアに関する部分地図情報を受信し、地図ＤＢ１０に反映させる。

入力部１４は、ユーザが操作するためのボタン、タッチパネル、リモートコントローラ、音声入力装置等であり、経路探索のための目的地を指定する入力、自動運転のオン及びオフを指定する入力などを受け付ける。出力部１６は、例えば、制御部１５の制御に基づき出力を行うディスプレイやスピーカ等である。

制御部１５は、プログラムを実行するＣＰＵなどを含み、車載機１の全体を制御する。本実施例では、制御部１５は、自車位置推定部１７と、自動運転制御部１８とを有する。

自車位置推定部１７は、基準ランドマークＬｋに対するライダ２による距離及び角度の計測値と、地図ＤＢ１０から抽出した基準ランドマークＬｋの位置情報とに基づき、ジャイロセンサ３及び車速センサ４の出力データから推定した自車位置を補正する。このとき、自車位置推定部１７は、ベイズ推定に基づく状態推定手法に基づき、ジャイロセンサ３及び車速センサ４の出力データから自車位置を推定する予測ステップと、直前の予測ステップで算出した自車位置の推定値を補正する計測更新ステップとを交互に実行する。第１実施例では、ベイズ推定に基づく状態推定手法の一例として、拡張カルマンフィルタを用いた例について後述する。自車位置推定部１７は、本発明における「取得部」、「方位取得部」、「第１取得部」、「第１推定部」、及びプログラムを実行するコンピュータの一例である。

自動運転制御部１８は、地図ＤＢ１０を参照し、設定された経路と、自車位置推定部１７が推定した自車位置とに基づき、車両の自動運転を行う。自動運転制御部１８は、設定された経路に基づき、目標軌道を設定し、自車位置推定部１７が推定した自車位置が目標軌道から所定幅以内のずれ幅となるように、車両に対してガイド信号を送信して車両の位置を制御する。

（２）拡張カルマンフィルタを用いた自車位置推定
次に、自車位置推定部１７による自車位置の推定処理について説明する。

（２−１）基本説明
以下では、自車位置推定部１７が実行する処理の前提となる基本事項について説明する。以後の説明では、自車位置を状態変数ベクトル「ｘ＝（ｘ、ｙ、θ）」で表わす。また、予測ステップで推定された暫定的な推定値には当該推定値を表す文字の上に「⁻」を付し、計測更新ステップで更新された，より精度の高い推定値には当該値を表す文字の上に「^＾」を付す。なお、任意の記号の上に「＾」または「−」が付された文字を、本明細書では便宜上、「Ａ^＾」または「Ａ⁻」（「Ａ」は任意の文字）と表す。

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

図４は、予測ステップと計測更新ステップとの概略的な関係を示す図である。カルマンフィルタのようなベイズ推定に基づく状態推定手法では、上述したように、予測ステップと、計測更新ステップとを交互に実行する２ステップ処理によって推定値の算出及び更新を逐次的に実行する。よって、本実施例では、予測ステップと計測更新ステップとを繰り返すことで状態変数ベクトルｘの推定値の算出及び更新を逐次的に実行する。以後では、計算対象となる基準時刻（即ち現在時刻）「ｔ」の状態変数ベクトルを、「ｘ⁻ _ｔ」または「ｘ^＾ _ｔ」と表記する。

予測ステップでは、自車位置推定部１７は、直前の計測更新ステップで算出された時刻ｔ−１の状態変数ベクトルｘ^＾ _ｔ−１に対し、車両の移動速度「ｖ」と角速度「ω」（これらをまとめて「制御値ｕ_ｔ＝（ｖ_ｔ、ω_ｔ）^Ｔ」と表記する。）を作用させることで、時刻ｔの自車位置の推定値（「事前推定値」とも呼ぶ。）ｘ⁻ _ｔを算出する。制御値ｕ_ｔは本発明における「制御情報」の一例であり、事前推定値ｘ⁻ _ｔ及び事後推定値ｘ^＾ _ｔは本発明における「第１推定位置」の一例である。また、これと同時に、自車位置推定部１７は、事前推定値ｘ⁻ _ｔの誤差分布に相当する共分散行列（「事前共分散行列」とも呼ぶ。）「Σ⁻ _ｔ」を、直前の計測更新ステップで算出された時刻ｔ−１での共分散行列「Σ^＾ _ｔ−１」から算出する。

また、計測更新ステップでは、自車位置推定部１７は、基準ランドマークＬｋのライダ２による計測値「ｚ_ｔ」と、ライダ２による計測処理を事前推定値ｘ⁻ _ｔからモデル化して求めた上述の基準ランドマークＬｋの計測推定値「ｚ^＾ _ｔ」とをそれぞれ取得する。計測値ｚ_ｔは、後述するように、時刻ｔにライダ２が計測した基準ランドマークＬｋの距離及びスキャン角度を表す２次元ベクトルである。そして、自車位置推定部１７は、以下の式（１）に示すように、計測値ｚ_ｔと計測推定値ｚ^＾ _ｔとの差分に別途求めるカルマンゲイン「Ｋ_ｔ」を乗算し、これを事前推定値ｘ⁻ _ｔに加えることで、更新された状態変数ベクトル（「事後推定値」とも呼ぶ。）ｘ^＾ _ｔを算出する。

また、計測更新ステップでは、自車位置推定部１７は、予測ステップと同様、事後推定値ｘ^＾の誤差分布に相当する共分散行列（「事後共分散行列」とも呼ぶ。）Σ^＾ _ｔを事前共分散行列Σ⁻ _ｔから求める。

（２−２）処理概要
図５は、自車位置推定部１７の機能的な構成を示すブロック図である。自車位置推定部１７は、主に、状態遷移モデルブロック２０と、共分散計算ブロック２１と、ランドマーク抽出ブロック２２と、計測モデルブロック２３と、共分散計算ブロック２４と、カルマンゲイン計算ブロック２５と、共分散更新ブロック２６と、演算ブロック３１〜３３と、単位遅延ブロック３４，３５と、を有する。

状態遷移モデル（速度動作モデル）ブロック２０は、時刻ｔ−１での事後推定値ｘ^＾ _ｔ−１から、制御値ｕ_ｔ＝（ｖ_ｔ、ω_ｔ）^Ｔに基づき、事前推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ、θ⁻ _ｔ）^Ｔを算出する。図６は、基準時刻ｔでの事前推定値「ｘ⁻ _ｔ」、「ｙ⁻ _ｔ」、「θ⁻ _ｔ」と時刻ｔ−１での事後推定値「ｘ^＾ _ｔ−１」、「ｙ^＾ _ｔ−１」、「θ^＾ _ｔ−１」との関係を示す図である。図６に示す幾何学的関係によれば、事前推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ、θ⁻ _ｔ）^Ｔは、以下の式（２）により表される。なお、「Δｔ」は、時刻ｔと時刻ｔ−１との時間差を表す。

よって、状態遷移モデルブロック２０は、時刻ｔ−１での事後推定値ｘ^＾ _ｔ−１と、制御値ｕ_ｔ＝（ｖ_ｔ、ω_ｔ）^Ｔとから、式（２）に基づき、事前推定値ｘ⁻ _ｔを算出する。方位「θ⁻ _ｔ」は、本発明における「基準方位」の一例である。

共分散計算ブロック２１は、制御値ｕ_ｔの誤差分布を状態変数ベクトル（ｘ、ｙ、θ）の３次元空間に変換した誤差分布を示す行列「Ｒｔ」と、式（２）に示される状態遷移モデルをｘ^＾ _ｔ−１のまわりで線形化したヤコビ行列「Ｇｔ」を用いて、式（３）に基づき事前共分散行列Σ⁻ _ｔを算出する。

ここで、ヤコビ行列Ｇｔは、以下の式（４）により表される。

ランドマーク抽出ブロック２２は、ライダ２の出力及び事前推定値ｘ⁻ _ｔから、基準ランドマークＬｋの地図ＤＢ１０内でのｘｙ座標での位置ベクトル「ｍ_ｋ＝（ｍ_ｋ，ｘ、ｍ_ｋ，ｙ）」を抽出する。また、ランドマーク抽出ブロック２２は、抽出したインデックスｋの基準ランドマークＬｋに対応するライダ２の計測値「ｚ_ｔ ^ｋ＝（ｒ_ｔ ^ｋ、φ_ｔ ^ｋ）^Ｔ」を、演算ブロック３１へ供給する。ここで、計測値ｚ_ｔ ^ｋは、インデックスｋのランドマークに対する距離「ｒ_ｔ ^ｋ」と、車両の正面方向を０度としたときのインデックスｋのランドマークに対するスキャン角度「φ_ｔ ^ｋ」とを要素とするベクトル値である。計測値ｚ_ｔ ^ｋは、本発明における「第１情報」の一例である。ランドマーク抽出ブロック２２が実行する基準ランドマークＬｋの特定方法等については後述する。

計測モデルブロック２３は、インデックスｋの基準ランドマークＬｋの位置ベクトルｍ_ｋ及び事前推定値ｘ⁻ _ｔから、計測値ｚ_ｔ ^ｋの推定値「ｚ^＾ _ｔ ^ｋ＝（ｒ^＾ _ｔ ^ｋ、φ^＾ _ｔ ^ｋ）^Ｔ」を算出し、演算ブロック３１へ供給する。ここで、「ｒ^＾ _ｔ ^ｋ」は、事前推定値ｘ⁻ _ｔを基準とした場合のインデックスｋのランドマークに対する距離を示し、「φ^＾ _ｔ ^ｋ」は、事前推定値ｘ⁻ _ｔを基準とした場合のインデックスｋのランドマークに対するスキャン角度を示す。図７は、基準ランドマークＬｋの位置と自車位置との関係を示す図である。図７に示す幾何学的関係に基づき、距離ｒ^＾ _ｔ ^ｋは以下の式（５）のように表される。

また、図７に示す関係に基づき、以下の式（６）が成立する。

よって、スキャン角度φ^＾ _ｔ ^ｋは以下の式（７）のように表される。

従って、計測モデルブロック２３は、式（５）及び式（７）を参照し、計測値ｚ_ｔ ^ｋの推定値ｚ^＾ _ｔ ^ｋ＝（ｒ^＾ _ｔ ^ｋ、φ^＾ _ｔ ^ｋ）^Ｔを算出し、演算ブロック３１へ供給する。推定値ｚ^＾ _ｔ ^ｋ＝（ｒ^＾ _ｔ ^ｋ、φ^＾ _ｔ ^ｋ）^Ｔは、本発明における「第２情報」の一例である。

さらに、計測モデルブロック２３は、式（５）及び式（７）に示される計測モデルを事前推定値ｘ⁻ _ｔのまわりで線形化したヤコビ行列「Ｈ_ｔ ^ｋ」を算出する。ここで、ヤコビ行列Ｈ_ｔ ^ｋは、以下の式（８）により表される。

計測モデルブロック２３は、ヤコビ行列Ｈ_ｔ ^ｋを、共分散計算ブロック２４、カルマンゲイン計算ブロック２５、及び共分散更新ブロック２６へそれぞれ供給する。

共分散計算ブロック２４は、カルマンゲインＫ_ｔ ^ｋの算出に必要な共分散行列「Ｓ_ｔ ^ｋ」を、以下の式（９）に基づき算出する。

共分散計算ブロック２４は、算出した共分散行列Ｓ_ｔ ^ｋをカルマンゲイン計算ブロック２５へ供給する。

カルマンゲイン計算ブロック２５は、カルマンゲインＫ_ｔ ^ｋを、以下の式（１０）に基づき算出する。

共分散更新ブロック２６は、共分散計算ブロック２１から供給される事前共分散行列Σ⁻ _ｔと、計測モデルブロック２３から供給されるヤコビ行列Ｈ_ｔ ^ｋと、カルマンゲイン計算ブロック２５から供給されるカルマンゲインＫ_ｔ ^ｋとに基づき、事後共分散行列Σ^＾ _ｔを、単位行列「Ｉ」を用いて以下の式（１１）に基づき算出する。

演算ブロック３１は、ランドマーク抽出ブロック２２から供給される計測値ｚ_ｔ ^ｋと計測モデルブロック２３から供給される推定値ｚ^＾ _ｔ ^ｋとの差分（即ち「ｚ_ｔ ^ｋ−ｚ^＾ _ｔ ^ｋ」）を算出する。演算ブロック３２は、演算ブロック３１が算出した値にカルマンゲイン計算ブロック２５から供給されたカルマンゲインＫ_ｔ ^ｋを乗じる。演算ブロック３３は、以下の式（１２）に示すように、事前推定値ｘ⁻ _ｔに対して演算ブロック３２が算出した値を加算することで、事後推定値ｘ^＾を算出する。

以上のように，自車位置推定部１７は、予測ステップと計測更新ステップを逐次的に繰返すことにより，高精度に状態推定を行うことが可能である。これらのステップで用いる状態推定フィルタには、拡張カルマンフィルタの他、ベイズ推定を行うように開発された様々のフィルタが利用可能である。例えば、拡張カルマンフィルタに代えて、アンセンテッドカルマンフィルタやパーティクルフィルタなどを利用してもよい。

（２−３）ランドマーク抽出ブロックの詳細
図８は、ランドマーク抽出ブロック２２の機能的構成を示す図である。図８に示すように、ランドマーク抽出ブロック２２は、探索候補選定ブロック４１と、計測推定値計算ブロック４２と、探索範囲絞込みブロック４３と、抽出ブロック４４とを有する。

探索候補選定ブロック４１は、事前推定値ｘ⁻ _ｔに基づき_、ライダ２によるスキャン範囲（「スキャン範囲Ｒｓｃ」とも呼ぶ。）を認識し、認識したスキャン範囲Ｒｓｃ内に存在するランドマークを、地図ＤＢ１０から選定する。この場合、探索候補選定ブロック４１は、方位θ⁻ _ｔから±９０°であって、位置（ｘ⁻ _ｔ、ｙ⁻ _ｔ）からライダ２の測距可能距離以内となるエリアを、スキャン範囲Ｒｓｃとして設定する。そして、探索候補選定ブロック４１は、インデックスｋに対応する位置ベクトルｍ_ｋ＝（ｍ_ｋ，ｘ、ｍ_ｋ，ｙ）を地図ＤＢ１０から抽出する。スキャン範囲Ｒｓｃは、本発明における「第１範囲」の一例である。

計測推定値計算ブロック４２は、事前推定値ｘ⁻ _ｔと、地図ＤＢ１０から抽出したインデックスｋに対応する位置ベクトルｍ_ｋとに基づき、計測値ｚ_ｔ ^ｋの推定値ｚ^＾ _ｔ ^ｋ＝（ｒ^＾ _ｔ ^ｋ、φ^＾ _ｔ ^ｋ）^Ｔを算出する。具体的には、計測推定値計算ブロック４２は、上述した式（５）及び式（７）に基づき、推定値ｚ^＾ _ｔ ^ｋ＝（ｒ^＾ _ｔ ^ｋ、φ^＾ _ｔ ^ｋ）^Ｔを算出する。

探索範囲絞込みブロック４３は、スキャン範囲Ｒｓｃから、スキャン角度が角度φ^＾ _ｔ ^ｋから所定の探索角度幅「Δ_φ」以内の差であって、測距範囲が距離ｒ^＾ _ｔ ^ｋから探索距離幅「Δ_ｒ」となる範囲（「探索範囲Ｒｔａｇ」とも呼ぶ。）を設定する。探索角度幅Δ_φ及び探索距離幅Δ_ｒは、それぞれ、想定される計測値ｚ_ｔ ^ｋと推定値ｚ^＾ _ｔ ^ｋとの誤差を勘案して実験等に基づき予め設定される。上述の誤差は、推定値ｚ^＾ _ｔ ^ｋ＝（ｒ^＾ _ｔ ^ｋ、φ^＾ _ｔ ^ｋ）^Ｔの算出（式５及び７参照）に必要な事前推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ、θ⁻ _ｔ）^Ｔの推定精度と、計測値ｚ_ｔ ^ｋに相当するスキャンデータを出力するライダ２の精度とに依存する。よって、好適には、探索角度幅Δ_φ及び探索距離幅Δ_ｒは、事前推定値ｘ⁻ _ｔの推定精度と、ライダ２の精度との少なくとも一方を勘案して設定される。具体的には、探索角度幅Δ_φ及び探索距離幅Δ_ｒは、事前推定値ｘ⁻ _ｔの推定精度が高いほど短くなり、かつ、ライダ２の精度が高いほど短くなる。

抽出ブロック４４は、探索範囲絞込みブロック４３が設定した探索範囲Ｒｔａｇに基づき、基準ランドマークＬｋの点群に相当する計測値ｚ_ｔ ^ｋを、ライダ２の時刻ｔにおける全スキャンデータから抽出する。具体的には、抽出ブロック４４は、以下の式（１３）及び式（１４）を満たす計測値ｚ_ｔ ^ｉ（「ｉ」はライダ２が１回のスキャンで出射する各ビームのインデックス）が存在するか否か判定する。

そして、抽出ブロック４４は、式（１３）及び式（１４）を満たす（ｒ_ｔ ^ｉ、φ_ｔ ^ｉ）の組となる計測値ｚ_ｔ ^ｉが存在する場合に、探索候補選定ブロック４１が選定したランドマークが実際に存在すると判断する。そして、抽出ブロック４４は、インデックスｋに対応する位置ベクトルｍ^ｋを計測モデルブロック２３へ供給すると共に、式（１３）及び式（１４）を満たす（ｒ_ｔ ^ｉ、φ_ｔ ^ｉ）の組となる計測値ｚ_ｔ ^ｉを、基準ランドマークＬｋの点群に相当する計測値ｚ_ｔ ^ｋとして演算ブロック３１へ供給する。

この場合、好適には、抽出ブロック４４は、探索範囲Ｒｔａｇ内のスキャンデータから、基準ランドマークＬｋに対応するスキャンデータを選定する処理をさらに実行し、演算ブロック３１へ供給すべき計測値ｚ_ｔ ^ｋを決定するとよい。

例えば、抽出ブロック４４は、探索候補選定ブロック４１が選定したランドマークの形状情報を地図ＤＢ１０から取得し、探索範囲Ｒｔａｇ内のスキャンデータの点群が構成する３次元形状とのマッチング処理を行うことで、基準ランドマークＬｋの点群に相当する計測値ｚ_ｔ ^ｋを選定する。他の例では、抽出ブロック４４は、探索範囲Ｒｔａｇ内のスキャンデータに対応する受光強度の大きさを調べ，予め設定された閾値情報と比較することで、基準ランドマークＬｋの点群に相当する計測値ｚ_ｔ ^ｋを選定する。なお、抽出ブロック４４は、選定した計測値ｚ_ｔ ^ｋが複数存在する場合には、任意の１つの計測値ｚ_ｔ ^ｋを演算ブロック３１へ供給してもよく、これらの計測値ｚ_ｔ ^ｋから統計処理により算出又は選定した代表値を演算ブロック３１へ供給してもよい。

図９は、スキャン範囲Ｒｓｃと探索範囲Ｒｔａｇとの関係を示す図である。図９の例では、スキャン範囲Ｒｓｃは、車両の正面から左右９０°の範囲であって車両から測距可能距離以内となる半円エリアとなっている。なお、図９の矢印７０はスキャン方向を示す。

図９の例では、まず、探索候補選定ブロック４１は、事前推定値ｘ⁻ _ｔに基づきスキャン範囲Ｒｓｃを特定すると共に、スキャン範囲Ｒｓｃ内に存在するランドマークを基準ランドマークＬｋとみなし、対応する位置ベクトルｍ^ｋを選定する。そして、計測推定値計算ブロック４２は、基準ランドマークＬｋの位置ベクトルｍ^ｋ及び事前推定値ｘ⁻ _ｔから、計測値ｚ_ｔ ^ｋの推定値ｚ^＾ _ｔ ^ｋ＝（ｒ^＾ _ｔ ^ｋ、φ^＾ _ｔ ^ｋ）^Ｔを算出する。探索範囲絞込みブロック４３は、式（１３）及び式（１４）に基づき探索範囲Ｒｔａｇを設定する。抽出ブロック４４は、設定された探索範囲Ｒｔａｇ内にスキャンデータが存在する場合、位置ベクトルｍ^ｋを計測モデルブロック２３へ供給すると共に、探索範囲Ｒｔａｇ内のスキャンデータに基づく計測値ｚ_ｔ ^ｋを演算ブロック３１へ供給する。なお、設定された探索範囲Ｒｔａｇ内にスキャンデータが存在しない場合には、自車位置推定部１７は、基準ランドマークＬｋがライダ２により検出できなかったと判断し、事前推定値ｘ⁻ _ｔを事後推定値ｘ^＾ _ｔに設定すると共に、事前共分散行列Σ⁻ _ｔを事後共分散行列Σ^＾ _ｔに設定する。

（３）処理フロー
（３−１）処理概要
図１０は、第１実施例において自車位置推定部１７が実行する処理の手順を示すフローチャートである。自車位置推定部１７は、図１０のフローチャートを繰り返し実行する。

まず、自車位置推定部１７は、時刻ｔ−１での事後推定値ｘ^＾ _ｔ−１と事後共分散行列Σ^＾ _ｔ−１とを取り込むと共に、ジャイロセンサ３及び車速センサ４から時刻ｔにおける制御値ｕ_ｔを取得する（ステップＳ１０１）。次に、自車位置推定部１７の状態遷移モデルブロック２０は、式（２）に基づき、事前推定値ｘ⁻ _ｔを計算する（ステップＳ１０２）。そして、自車位置推定部１７の共分散計算ブロック２１は、式（３）に基づき、事前共分散行列Σ^ー _ｔを計算する（ステップＳ１０３）。

次に、自車位置推定部１７のランドマーク抽出ブロック２２は、地図ＤＢ１０を参照し、後述する図１１に示すランドマーク抽出処理を実行する（ステップＳ１０４）。そして、自車位置推定部１７は、地図ＤＢ１０に登録されたランドマークの位置ベクトルｍ^ｋとライダ２のスキャンデータとの対応付けができたか否か判定する（ステップＳ１０５）。この場合、自車位置推定部１７は、図１１に示すランドマーク抽出処理に基づき、上述の対応付けができたことを示す所定のフラグが設定されたか否か判定する。

そして、上述の対応付けができた場合（ステップＳ１０５；Ｙｅｓ）、共分散計算ブロック２４は、上述の式（９）に基づき、共分散行列Ｓ_ｔ ^ｋを計算する（ステップＳ１０６）。次に、カルマンゲイン計算ブロック２５は、カルマンゲインＫ_ｔ ^ｋを、上述の式（１０）に基づき算出する（ステップＳ１０７）。そして、演算ブロック３３は、式（１２）に示すように、事前推定値ｘ⁻ _ｔに対し、ランドマーク抽出ブロック２２から供給される計測値ｚ_ｔ ^ｋと計測モデルブロック２３から供給される推定値ｚ^＾ _ｔ ^ｋとの差分（即ち「ｚ_ｔ ^ｋ−ｚ^＾ _ｔ ^ｋ」）にカルマンゲインＫ_ｔ ^ｋを乗じた値を加算することで、事後推定値ｘ^＾ _ｔを計算する（ステップＳ１０８）。また、共分散更新ブロック２６は、共分散計算ブロック２１から供給される事前共分散行列Σ⁻ _ｔと、計測モデルブロック２３から供給されるヤコビ行列Ｈ_ｔ ^ｋと、カルマンゲイン計算ブロック２５から供給されるカルマンゲインＫ_ｔ ^ｋとに基づき、事後共分散行列Σ^＾ _ｔを、上述の式（１１）に基づき計算する（ステップＳ１０９）。その後、単位遅延ブロック３４は、事後推定値ｘ^＾ _ｔを事後推定値ｘ^＾ _ｔ−１として状態遷移モデルブロック２０に供給し、単位遅延ブロック３５は、事後共分散行列Σ^＾ _ｔを事後共分散行列Σ^＾ _ｔ−１として共分散計算ブロック２１に供給する。

一方、上述の対応付けができなかった場合（ステップＳ１０５；Ｎｏ）、自車位置推定部１７は、事前推定値ｘ⁻ _ｔを事後推定値ｘ^＾ _ｔに設定すると共に、事前共分散行列Σ⁻ _ｔを事後共分散行列Σ^＾ _ｔに設定する（ステップＳ１１０）。

（３−２）ランドマーク抽出処理
図１１は、図１０のステップＳ１０４のランドマーク抽出処理の詳細を示すフローチャートである。

まず、ランドマーク抽出ブロック２２の探索範囲絞込みブロック４３は、ライダ２のスキャンデータを取り込む（ステップＳ２０１）。ここでは、ライダ２は、１周期分のスキャンにより出射角度を徐々に変化させた「ｎ」本のビームを出射するものとし、各ビームの反射光の受光強度及び応答時間を計測することで、それぞれ計測値ｚ_ｔ ^１〜ｚ_ｔ ^ｎを出力するものとする。

次に、探索候補選定ブロック４１は、ライダ２のスキャン範囲Ｒｓｃに存在するランドマークの位置ベクトルｍ^ｋを地図ＤＢ１０から選定する（ステップＳ２０２）。この場合、探索候補選定ブロック４１は、スキャン範囲Ｒｓｃとして、方位θ⁻ _ｔから±９０°であって位置（ｘ⁻ _ｔ、ｙ⁻ _ｔ）からライダ２の測距可能距離以内となるエリアを特定し、特定したエリア内の位置を示す位置ベクトルｍ_ｋを、地図ＤＢ１０から抽出する。

その後、計測推定値計算ブロック４２は、事前推定値ｘ⁻ _ｔから位置ベクトルｍ_ｋを計測した場合の計測値ｚ_ｔ ^ｋの推定値ｚ^＾ _ｔ ^ｋ＝（ｒ^＾ _ｔ ^ｋ、φ^＾ _ｔ ^ｋ）^Ｔを算出する（ステップＳ２０３）。具体的には、計測推定値計算ブロック４２は、上述した式（５）及び式（７）に基づき、推定値ｚ^＾ _ｔ ^ｋ＝（ｒ^＾ _ｔ ^ｋ、φ^＾ _ｔ ^ｋ）^Ｔを算出する。

次に、探索範囲絞込みブロック４３は、式（１３）を満たすスキャン角度φ_ｔ ^ｉ（ｉ＝１〜ｎ）を絞り込む（ステップＳ２０４）。抽出ブロック４４は、式（１３）を満たすスキャン角度φ_ｔ ^ｉとなる計測値ｚ_ｔ ^ｉのうち、距離ｒ^＾ _ｔ ^ｉが式（１４）に示す範囲となるものが存在するか否か判定する（ステップＳ２０５）。そして、式（１３）を満たすスキャン角度φ_ｔ ^ｉとなる計測値ｚ_ｔ ^ｉのうち、距離ｒ_ｔ ^ｉが式（１４）に示す範囲となるものが存在する場合（ステップＳ２０５；Ｙｅｓ）、抽出ブロック４４は、この探索範囲に含まれる計測値ｚ_ｔ ^ｉの中から、基準ランドマークＬｋに対応する計測値を特定する処理（公知の形状・特徴抽出処理，受光強度の利用など）をさらに実行し，最終的に選定された計測値をｚ_ｔ ^ｋとみなして抽出する（ステップＳ２０６）。そして、抽出ブロック４４は、抽出した位置ベクトルｍ^ｋ及び当該位置ベクトルｍ^ｋに対応するスキャンデータである計測値ｚ_ｔ ^ｋを出力すると共に、対応付けができた旨を示すフラグをセットする（ステップＳ２０７）。このフラグは、前述した図１０のステップＳ１０５の判定処理において参照される。

一方、抽出ブロック４４は、式（１３）を満たすスキャン角度φ_ｔ ^ｉとなる計測値ｚ_ｔ ^ｉのうち、距離ｒ^＾ _ｔ ^ｉが式（１４）に示す範囲となるものが存在しない場合（ステップＳ２０５；Ｎｏ）、対応付けができた旨のフラグをセットしない（ステップＳ２０８）。

＜第２実施例＞
図１２は、第２実施例に係る運転支援システムのブロック構成図である。図１２の例では、車載機１は、方位センサ５と電気的に接続されている。方位センサ５は、例えば地磁気センサ、方位磁石、又はＧＰＳコンパス等であって、車両の進行方向に対応する方位の情報を車載機１へ供給する。車載機１は、第１実施例と同様に図２に示す構成を有し、地図ＤＢ１０に登録されたランドマーク情報と、ライダ２、ジャイロセンサ３、車速センサ４、及び方位センサ５の各出力データとに基づき、自車位置の推定を行う。以後では、第１実施例と同一の構成要素については、適宜同一の符号を付し、その説明を省略する。

図１３は、第２実施例における車載機１の自車位置推定部１７Ａの概略構成を示す。図１３の例では、自車位置推定部１７Ａは、ランドマーク抽出ブロック２２と、位置推定ブロック２８とを有する。以後において、方位センサ５が出力する基準時刻ｔでの車両の方位を「θ_ｔ」と表記し、位置推定ブロック２８が出力する基準時刻ｔでの自車位置の推定値を「ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ）」と表記する。なお、方位センサ５が出力する方位θ_ｔは、本発明における「基準方位」の一例である。

ランドマーク抽出ブロック２２は、探索候補選定ブロック４１、計測推定値計算ブロック４２、探索範囲絞込みブロック４３、抽出ブロック４４を有し、基準ランドマークＬｋの位置ベクトルｍ^ｋと、基準ランドマークＬｋに対するライダ２のスキャンデータである計測値ｚ_ｔ ^ｋとを出力する。

具体的には、探索候補選定ブロック４１は、第１実施例では、状態遷移モデルブロック２０から供給される事前推定値ｘ⁻ _ｔに基づきスキャン範囲Ｒｓｃを特定していた。これに代えて、第２実施例では、探索候補選定ブロック４１は、まず、第１実施例の状態遷移モデルブロック２０と同様に、図６に示す幾何学的関係に基づき、自車位置のｘｙ座標の仮の推定値を算出する。具体的には、探索候補選定ブロック４１は、一時刻前に位置推定ブロック２８が推定した自車位置の推定値ｘ⁻ _ｔ−１及び一時刻前に測定された方位θ_ｔ−１と、制御値ｕ_ｔ＝（ｖ_ｔ、ω_ｔ）^Ｔとに基づき、式（２）に従い、自車位置のｘｙ座標の仮の推定値を算出する。そして、探索候補選定ブロック４１は、算出した自車位置のｘｙ座標の仮の推定値と方位θ_ｔに基づき、スキャン範囲Ｒｓｃを特定する。具体的には、探索候補選定ブロック４１は、方位θ_ｔから±９０°であって、算出した自車位置のｘｙ座標からライダ２の測距可能距離以内となるエリアをスキャン範囲Ｒｓｃとして特定する。そして、探索候補選定ブロック４１は、特定したエリア内の位置を示すランドマークの位置ベクトルｍ_ｋを、地図ＤＢ１０から抽出する。

計測推定値計算ブロック４２は、探索候補選定ブロック４１が算出した自車位置のｘｙ座標の仮の推定値（第１実施例の事前推定値ｘ⁻ _ｔに相当）と、地図ＤＢ１０から抽出した位置ベクトルｍ_ｋとに基づき、式（５）及び式（７）を参照し、計測値ｚ_ｔ ^ｋの推定値ｚ^＾ _ｔ ^ｋ＝（ｒ^＾ _ｔ ^ｋ、φ^＾ _ｔ ^ｋ）^Ｔを算出する。なお、計測推定値計算ブロック４２は、式（５）及び式（７）を参照する場合、事前推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ）の代わりに、探索候補選定ブロック４１が算出した自車位置のｘｙ座標の仮の推定値を用い、方位θ⁻ _ｔの代わりに方位θ_ｔを用いる。そして、探索範囲絞込みブロック４３は、式（１３）及び式（１４）に示す探索範囲Ｒｔａｇを設定し、抽出ブロック４４は、式（１３）及び式（１４）を満たす（ｒ_ｔ ^ｉ、φ_ｔ ^ｉ）の組となる計測値ｚ_ｔ ^ｉ（ここではｚ_ｔ ^ｋとする）が存在する場合に、当該計測値と位置ベクトルｍ^ｋを位置推定ブロック２８に供給する。この場合、好適には、抽出ブロック４４は、第１実施例と同様に、探索範囲Ｒｔａｇ内のスキャンデータから、基準ランドマークＬｋに対応するスキャンデータを選定する処理をさらに実行し、供給すべき計測値ｚ_ｔ ^ｋを決定してもよい。

位置推定ブロック２８は、抽出ブロック４４から供給される位置ベクトルｍ^ｋ及び計測値ｚ_ｔ ^ｋ＝（ｒ_ｔ ^ｋ、φ_ｔ ^ｋ）^Ｔと、方位センサ５が出力する方位θ_ｔとから、自車位置の推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ）を算出する。具体的には、位置推定ブロック２８は、絶対値｜θ_ｔ＋φ_ｔ ^ｋ｜が９０°未満の場合には、以下の式（１５）及び式（１６）に基づき、推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ）を算出する。

一方、位置推定ブロック２８は、絶対値｜θ_ｔ＋φ_ｔ ^ｋ｜が９０°より大きい場合には、以下の式（１７）と上述の式（１６）とに基づき、推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ）を算出する。

また、位置推定ブロック２８は、絶対値｜θ_ｔ＋φ_ｔ ^ｋ｜が９０°の場合には、以下の式（１８）と上述の式（１６）とに基づき、推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ）を算出する。

ここで、式（１５）〜式（１８）の導出方法について説明する。

まず、距離ｒ_ｔ ^ｋ及びスキャン角度φ_ｔ ^ｋは、距離ｒ^＾ _ｔ ^ｋ及びスキャン角度φ^＾ _ｔ ^ｋと同様に、前述した図７に示す幾何学的関係を有することから、推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ）と方位θ_ｔとの間で、それぞれ上述した式（５）及び式（６）と同様の関係が成立する。ここで、以下の式（１９）は式（６）を変形した式に相当し、式（２０）は式（５）を変形した式に相当する。

そして、式（１９）を式（２０）に代入して整理すると、以下の式（２１）が得られる。

式（２１）には、符号「±」が含まれるため、推定値ｘ⁻ _ｔのｘ座標値「ｘ⁻ _ｔ」と位置ベクトルｍ^ｋのｘ座標値「ｍ_ｋ、ｘ」との大小関係に基づき場合分けする必要がある。図１４（Ａ）〜（Ｃ）は、推定値ｘ⁻ _ｔのｘ座標値「ｘ⁻ _ｔ」と位置ベクトルｍ^ｋのｘ座標値「ｍ_ｋ、ｘ」との大小関係を示す図である。具体的には、図１４（Ａ）は、推定値ｘ⁻ _ｔのｘ座標値「ｘ⁻ _ｔ」よりも位置ベクトルｍ^ｋのｘ座標値「ｍ_ｋ、ｘ」が大きい場合を示す。また、図１４（Ｂ）は、推定値ｘ⁻ _ｔのｘ座標値「ｘ⁻ _ｔ」と位置ベクトルｍ^ｋのｘ座標値「ｍ_ｋ、ｘ」とが等しい場合を示し、図１４（Ｃ）は、推定値ｘ⁻ _ｔのｘ座標値「ｘ⁻ _ｔ」よりも位置ベクトルｍ^ｋのｘ座標値「ｍ_ｋ、ｘ」が小さい場合を示す。

図１４（Ａ）に示すように、「θ_ｔ＋φ_ｔ ^ｋ」が９０°未満の場合には、「ｘ⁻ _ｔ＜ｍ_ｋ、ｘ」が成立する。また、図１４（Ｂ）に示すように、「θ_ｔ＋φ_ｔ ^ｋ」が９０°の場合には、「ｘ⁻ _ｔ＝ｍ_ｋ、ｘ」が成立する。さらに、図１４（Ｃ）に示すように、「θ_ｔ＋φ_ｔ ^ｋ」が９０°より大きい場合には、「ｘ⁻ _ｔ＞ｍ_ｋ、ｘ」が成立する。

従って、「θ_ｔ＋φ_ｔ ^ｋ」が負値になる場合を含めると、
（ａ）｜θ_ｔ＋φ_ｔ ^ｋ｜＜９０° → ｘ⁻ _ｔ＜ｍ_ｋ、ｘ
（ｂ）｜θ_ｔ＋φ_ｔ ^ｋ｜＝９０° → ｘ⁻ _ｔ＝ｍ_ｋ、ｘ
（ｃ）｜θ_ｔ＋φ_ｔ ^ｋ｜＞９０° → ｘ⁻ _ｔ＞ｍ_ｋ、ｘ
となる。そして、上記（ａ）の場合には、式（２１）の左辺は正値になることから、右辺の符号は「＋」となるため、上述の式（１５）が導出される。また、上記（ｂ）の場合には、式（１８）に示す関係「ｘ⁻ _ｔ＝ｍ_ｋ、ｘ」が成立する。さらに、上記（ｃ）の場合には、式（２１）の左辺は負値になることから、右辺の符号は「−」となるため、上述の式（１７）が導出される。また、推定値ｘ⁻ _ｔのｙ座標値「ｙ⁻ _ｔ」は、式（１９）を変形することで、式（１６）のように表される。

以上のように、第２実施例によれば、自車位置推定部１７Ａは、方位センサ５から得られる方位θ_ｔを利用することで、地図ＤＢ１０に登録されているランドマークの位置ベクトルｍ^ｋ及び計測値ｚ_ｔ ^ｋ＝（ｒ^＾ _ｔ ^ｋ、φ^＾ _ｔ ^ｋ）^Ｔから、自車位置の推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ）を好適に算出することができる。

なお、地図ＤＢ１０に登録されたランドマークの位置ベクトルｍ^ｋとライダ２のスキャンデータとの対応付けができなかった場合には、自車位置推定部１７Ａは、探索候補選定ブロック４１が算出した自車位置のｘｙ座標の仮の推定値を、自車位置の推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ）として設定する。即ち、この場合、自車位置の推定値ｘ⁻ _ｔ＝（ｘ⁻ _ｔ、ｙ⁻ _ｔ）は、一時刻前に位置推定ブロック２８が推定した自車位置の推定値ｘ⁻ _ｔ−１と、車速センサ４及びジャイロセンサ５が基準時刻ｔに測定した制御値ｕ_ｔと、一時刻前に測定された方位θ_ｔ−１とに基づき算出された推定値となる。

＜変形例＞
以下では、第１及び第２実施例に好適な変形例について説明する。

（変形例１）
第１及び第２実施例において、探索候補選定ブロック４１は、スキャン範囲Ｒｓｃ内に存在する複数のランドマークの位置ベクトルを地図ＤＢ１０から抽出し、ライダ２のスキャンデータとの照合処理を行ってもよい。

一般に、車両の走行時には、自車前方や側方に他の車両などの障害物が存在するため、地図ＤＢ１０から選定した基準ランドマークＬｋのスキャンデータがライダ２から得られない状況（即ちオクルージョン）が発生する。以上を勘案し、本変形例では、探索候補選定ブロック４１は、スキャン範囲Ｒｓｃ内に存在する複数のランドマークの位置ベクトルを地図ＤＢ１０から抽出することで、いずれかのランドマークに対してオクルージョンが発生した場合であっても、オクルージョンが発生していないランドマークを基準として自車位置推定を行う。これにより、自車位置推定に必要な基準ランドマークＬｋをより高確率で検出することができる。また，複数のランドマークを抽出することができて，それらの全てを利用できる場合は，計測更新ステップを複数回行うことができる（複数のランドマークを使って事前推定値の補正を行うことができる）。この場合，自車位置の推定精度を統計的に向上させることが出来る。

本変形例の処理を、図１１を参照して補足説明する。ランドマーク抽出ブロック２２は、ステップＳ２０２において、地図ＤＢ１０を参照し、スキャン範囲Ｒｓｃ内に複数のランドマークが存在すると判断した場合には、少なくとも２つ以上のランドマークの位置ベクトルを地図ＤＢ１０から選定する。そして、Ｓ２０３では、ランドマーク抽出ブロック２２は、選定した位置ベクトルに対し、計測推定値を計算する。そして、ランドマーク抽出ブロック２２は、ステップＳ２０４及びＳ２０５において、計算した各計測推定値に対して式（１３）及び式（１４）に示す探索範囲Ｒｔａｇを設定し、それぞれの探索範囲の中で先に選定したランドマークに対応するスキャンデータの抽出を試みる。この対応付けが成功した場合，当該スキャンデータに相当する計測値と位置ベクトルをステップＳ２０７で出力する。

また，複数のランドマークの抽出がなされ，それらの全ての利用できる場合の処理を図１０を参照して補足説明する。この場合は，Ｓ１０４に示すランドマーク抽出処理で複数のランドマークが抽出されており，ステップＳ１０５がＹｅｓと判断された状態になっている。その先で行われるＳ１０６からＳ１０９の一連の処理が，抽出された個々のランドマークに対して行われるべき処理であり，個々のランドマークに対応するデータ（ステップＳ２０７の出力に対応）を入力として，これら一連の処理が抽出されたランドマークの数の分，ループ的に処理される。

（変形例２）
第２実施例において、自車位置推定部１７Ａは、方位センサ５の出力に基づき方位θ_ｔを特定する代わりに、ジャイロセンサ３の出力に基づき方位θ_ｔを推定してもよい。

この場合、自車位置推定部１７Ａは、図６及び式（２）と同様に、一時刻前に算出した方位θ_ｔ−１に対し、角速度ω_ｔに時刻ｔ−１と時刻ｔとの時間幅Δｔを乗じた値（ω_ｔΔｔ）を加算することで、基準時刻における方位θ_ｔの推定値を算出する。この例によっても、第２実施例と同様に、拡張カルマンフィルタなどのベイズ推定によらずに自車位置の推定値ｘ⁻ _ｔを好適に算出することができる。

（変形例３）
ライダ２によるインデックスｋのランドマークに対する計測値ｚ_ｔ ^ｋは，距離「ｒ_ｔ ^ｋ」と、車両の正面方向を０度としたときのインデックスｋのランドマークに対するスキャン角度「φ_ｔ ^ｋ」とを要素とするベクトル値であるとして上記の説明を記載したが，ライダ製品によっては，ターゲットまでの距離と角度を３次元空間上の座標値に変換した上で出力がなされるものもある。例えば２次元でスキャンを行うライダの場合，ｒ_ｔ ^ｋとφ_ｔ ^ｋと用いて，ｘ_ｔ ^ｋ＝ｒ_ｔ ^ｋｃｏｓφ_ｔ ^ｋ，ｙ_ｔ ^ｋ＝ｒ_ｔ ^ｋｓｉｎφ_ｔ ^ｋという直行座標形式で出力がなされる。このような出力が得られるライダ製品については，例えば，単純にｘ_ｔ ^ｋ，ｙ_ｔ ^ｋの値からｒ_ｔ ^ｋ，φ_ｔ ^ｋの値を逆算した上で，本発明で説明した手法を適用すれば良い。

（変形例４）
車載機１は、地図ＤＢ１０を記憶部１２に記憶する構成に代えて、図示しないサーバ装置が地図ＤＢ１０を有してもよい。この場合、車載機１は、図示しない通信部でサーバ装置と通信することにより、必要なランドマーク情報を取得する。

１ 車載機
２ ライダ
３ ジャイロセンサ
４ 車速センサ
５ 方位センサ
１０ 地図ＤＢ

Claims (8)

1. 地図情報を取得する取得部と、
   移動体の基準方位を取得する方位取得部と、
   第１範囲に存在する対象物までの距離及び角度を示す第１情報を取得する第１取得部と、
   前記地図情報に含まれる前記対象物の位置情報、前記移動体の基準方位及び前記第１情報に基づいて、前記移動体の位置を推定する第１推定部と、
   を備える推定装置。
2. 前記第１推定部は、
   前記第１情報が示す距離が、前記地図情報が示す前記対象物の位置と前記第１推定部が算出すべき前記移動体の推定位置との距離と等しく、かつ、
   前記第１情報が示す角度が、前記地図情報が示す前記対象物の位置と前記推定位置と前記基準方位とに基づき算出した、前記移動体に対する前記対象物の角度と等しい
   とみなすことで、前記推定位置を算出することを特徴とする請求項１に記載の推定装置。
3. 前記方位取得部は、地磁気センサ、方位磁石、又はＧＰＳ受信機の少なくともいずれか一つから、前記基準方位を取得することを特徴とする請求項１または２に記載の推定装置。
4. 前記方位取得部は、所定時間前に取得した前記基準方位と、測定された前記移動体の角速度とに基づき、現在時刻での前記基準方位を算出することを特徴とする請求項１または２に記載の推定装置。
5. 前記第１取得部は、照射方向を変えながらパルス状にレーザを照射して当該レーザに対応する散乱光を測定する測定装置から前記第１情報を取得する請求項１〜４のいずれか一項に記載の推定装置。
6. 推定装置が実行する制御方法であって、
   地図情報を取得する取得工程と、
   移動体の基準方位を取得する方位取得工程と、
   第１範囲に存在する対象物までの距離及び角度を示す第１情報を取得する第１取得工程と、
   前記地図情報に含まれる前記対象物の位置情報、前記移動体の基準方位及び前記第１情報に基づいて、前記移動体の位置を推定する第１推定工程と、
   を有することを特徴とする制御方法。
7. コンピュータが実行するプログラムであって、
   地図情報を取得する取得部と、
   移動体の基準方位を取得する方位取得部と、
   第１範囲に存在する対象物までの距離及び角度を示す第１情報を取得する第１取得部と、
   前記地図情報に含まれる前記対象物の位置情報、前記移動体の基準方位及び前記第１情報に基づいて、前記移動体の位置を推定する第１推定部
   として前記コンピュータを機能させることを特徴とするプログラム。
8. 請求項７に記載のプログラムを記憶した記憶媒体。