知財求人 - 知財ポータルサイト「IP Force」
2023-164553位置推定装置、推定装置、制御方法、プログラム及び記憶媒体
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2023164553
(43)【公開日】2023-11-10
(54)【発明の名称】位置推定装置、推定装置、制御方法、プログラム及び記憶媒体
(51)【国際特許分類】
G01C 21/28 20060101AFI20231102BHJP
【ＦＩ】
G01C21/28
【審査請求】有
【請求項の数】1
【出願形態】ＯＬ
(21)【出願番号】P 2023147040
(22)【出願日】2023-09-11
(62)【分割の表示】P 2021513587の分割
【原出願日】2020-03-31
(31)【優先権主張番号】P 2019074347
(32)【優先日】2019-04-09
(33)【優先権主張国・地域又は機関】JP
(71)【出願人】
【識別番号】000005016
【氏名又は名称】パイオニア株式会社
(74)【代理人】
【識別番号】100107331
【弁理士】
【氏名又は名称】中村 聡延
(72)【発明者】
【氏名】加藤 正浩
(72)【発明者】
【氏名】藤谷 多史
(72)【発明者】
【氏名】加藤 将大
(57)【要約】
【課題】検出できるランドマークの数が少ない状況であっても、好適に自車位置推定を行うことが可能な位置推定装置を提供する。
【解決手段】車載機１の自車位置推定部１７は、移動体の現在位置と方位との予測値である予測自車位置Ｘ－（ｔ）を取得する。そして、自車位置推定部１７は、ライダ２によるランドマークの計測データに基づく車両に対するランドマークの向きを示す第１向き情報である相対角度Ｌψ（ｔ）を取得し、かつ、ランドマーク情報に含まれるランドマークの向きを車両に対する向きに変換した向きを示す第２向き情報である相対角度Ｌψ －（ｔ）を取得する。そして、自車位置推定部１７は、相対角度Ｌψ（ｔ）と相対角度Ｌψ －（ｔ）との差分に基づいて、予測自車位置Ｘ－（ｔ）を補正する。
【選択図】図９
【解決手段】車載機１の自車位置推定部１７は、移動体の現在位置と方位との予測値である予測自車位置Ｘ－（ｔ）を取得する。そして、自車位置推定部１７は、ライダ２によるランドマークの計測データに基づく車両に対するランドマークの向きを示す第１向き情報である相対角度Ｌψ（ｔ）を取得し、かつ、ランドマーク情報に含まれるランドマークの向きを車両に対する向きに変換した向きを示す第２向き情報である相対角度Ｌψ －（ｔ）を取得する。そして、自車位置推定部１７は、相対角度Ｌψ（ｔ）と相対角度Ｌψ －（ｔ）との差分に基づいて、予測自車位置Ｘ－（ｔ）を補正する。
【選択図】図９
【特許請求の範囲】
【請求項1】
移動体の現在位置と、前記移動体の向きと、を含む予測値を取得する取得部と、
前記移動体に搭載された計測装置による地物の計測値に基づき、前記移動体の座標系における前記地物の向きを示す第１方向を算出する第１算出部と、
地図情報に含まれる前記地物の向く方位を前記移動体の座標系に変換した向きを示す第２方向を算出する第２算出部と、
前記第１方向と前記第２方向との差分に基づいて、前記予測値を補正する補正部と、
を有する位置推定装置。
前記移動体に搭載された計測装置による地物の計測値に基づき、前記移動体の座標系における前記地物の向きを示す第１方向を算出する第１算出部と、
地図情報に含まれる前記地物の向く方位を前記移動体の座標系に変換した向きを示す第２方向を算出する第２算出部と、
前記第１方向と前記第２方向との差分に基づいて、前記予測値を補正する補正部と、
を有する位置推定装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、位置推定技術に関する。
【背景技術】
【0002】
従来から、車両の進行先に設置される地物をレーダやカメラを用いて検出し、その検出結果に基づいて自車位置を校正する技術が知られている。例えば、特許文献１には、計測センサの出力と、予め地図上に登録された地物の位置情報とを照合させることで自己位置を推定する技術が開示されている。また、特許文献２には、カルマンフィルタを用いた自車位置推定技術が開示されている。さらに、特許文献３には、ライダが計測した標識の点群データに基づき、標識の向きを示す法線角度を算出する技術が開示されている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開２０１３－２５７７４２号公報
【特許文献2】特開２０１７－０７２４２２号公報
【特許文献3】特開２０１７－２１１３０７号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
高速道路や主要国道など、道路標識や白線が整備されている道路においては，地図と計測値を照合できるランドマーク数が多い。一方、郊外においては、道路によってランドマークが少なく、例えば１つの道路標識しか検出できなかったり、片側の白線しか検出できなかったりする場所も存在する。また、高性能な外界センサを搭載している場合は十分な距離と角度範囲を計測可能であるが、比較的低価格の外界センサでは、計測可能距離が短かったり，検出角度も狭かったりする。その場合、道路上には多くのランドマークがあったとしても、計測できるランドマーク数が少なくなってしまう。
【0005】
本発明は、上記のような課題を解決するためになされたものであり、本発明は，検出できるランドマークの数が少ない状況であっても、好適に自車位置推定を行うことを主な目的とする。
【課題を解決するための手段】
【0006】
請求項に記載の発明は、位置推定装置であって、移動体の現在位置と、前記移動体の向きと、を含む予測値を取得する取得部と、前記移動体に搭載された計測装置による地物の計測値に基づき、前記移動体の座標系における前記地物の向きを示す第１方向を算出する第１算出部と、地図情報に含まれる前記地物の向く方位を前記移動体の座標系に変換した向きを示す第２方向を算出する第２算出部と、前記第１方向と前記第２方向との差分に基づいて、前記予測値を補正する補正部と、を有する。
【0007】
また、請求項に記載の発明は、推定装置であって、移動体に搭載された計測装置による地物の計測値に基づく前記移動体の座標系における前記地物の向きを示す第１方向を算出する第１算出部と、地図情報に含まれる前記地物の向く方位を前記移動体の座標系に変換した向きを示す第２方向を算出する第２算出部と、前記第１方向と前記第２方向との差分に基づいて、前記移動体の現在位置及び向きの少なくとも一方を推定する推定部と、を備える。
【0008】
また、請求項に記載の発明は、位置推定装置が実行する制御方法であって、移動体の現在位置と、前記移動体の向きと、を含む予測値を取得する取得工程と、前記移動体に搭載された計測装置による地物の計測値に基づき、前記移動体の座標系における前記地物の向きを示す第１方向を算出する第１算出工程と、地図情報に含まれる前記地物の向く方位を前記移動体の座標系に変換した向きを示す第２方向を算出する第２算出工程と、前記第１方向と前記第２方向との差分に基づいて、前記予測値を補正する補正工程と、を有する。
【0009】
また、請求項に記載の発明は、コンピュータが実行するプログラムであって、移動体の現在位置と、前記移動体の向きと、を含む予測値を取得する取得部と、前記移動体に搭載された計測装置による地物の計測値に基づき、前記移動体の座標系における前記地物の向きを示す第１方向を算出する第１算出部と、地図情報に含まれる前記地物の向く方位を前記移動体の座標系に変換した向きを示す第２方向を算出する第２算出部と、前記第１方向と前記第２方向との差分に基づいて、前記予測値を補正する補正部として前記コンピュータを機能させる。
【図面の簡単な説明】
【0010】
【図1】運転支援システムの概略構成図である。
【図2】車載機の機能的構成を示すブロック図である。
【図4】状態変数ベクトルを２次元直交座標で表した図である。
【図5】予測ステップと計測更新ステップとの概略的な関係を示す図である。
【図6】車両とランドマークとの位置関係をワールド座標系及び車両座標系において示した図である。
【図7】自車位置推定部の機能ブロックの一例を示す。
【図8】（Ａ）ライダによる計測点を明示したランドマークの被照射面の正面図を示す。（Ｂ）区画線が破線である場合の予測ウィンドウ内の走査ライン及び各走査ラインの中心点を示した図である。（Ｃ）区画線が劣化している場合の予測ウィンドウ内の走査ライン及び各走査ラインの中心点を示した図である。
【図9】自車位置推定処理のフローチャートである。
【図10】車両とランドマークとの位置関係をワールド座標系及び車両座標系において示した図である。
【図11】比較例に係るカルマンフィルタに基づく自車位置推定のシミュレーション結果を示す図である。
【図12】実施例に係るカルマンフィルタに基づく車両の推定位置の軌跡を車両の正しい位置及びランドマークの位置と共に示した平面図である。
【図13】（Ａ）真値に対する推定値のｘ方向の誤差を示したグラフである。（Ｂ）真値に対する推定値のｙ方向の誤差を示したグラフである。（Ｃ）真値に対する推定値の方位ψの誤差を示したグラフである。
【発明を実施するための形態】
【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】
【数1】
【0037】
なお、自車位置推定部１７は、複数のランドマークに対し、地図ＤＢ１０に登録されたランドマークの位置ベクトルとライダ２のスキャンデータとの対応付けができた場合、選定した任意の一組の計測予測値及び計測値等に基づき計測更新ステップを行ってもよく、対応付けができた全ての計測予測値及び計測値等に基づき計測更新ステップを複数回行ってもよい。なお、複数の計測予測値及び計測値等を用いる場合には、自車位置推定部１７は、ライダ２から遠いランドマークほどライダ計測精度が悪化することを勘案し、ライダ２とランドマークとの距離が長いほど、当該ランドマークに関する重み付けを小さくしてもよい。
【0038】
［ランドマークの向き情報を用いた自車位置推定］
次に、ランドマークの向き情報を用いた自車位置推定処理について説明する。概略的には、自車位置推定部１７は、計測対象のランドマークの向きを計測すると共に、地図ＤＢ１０に登録された当該ランドマークの向き情報を取得することで、位置に加えて方位の計測値と予測値の差分値を用いたカルマンフィルタを形成し、推定自車位置Ｘ＾（ｔ）を算出する。これにより、１つのランドマークを計測対象として自車位置推定処理を行った場合であっても、高精度な推定自車位置Ｘ＾（ｔ）を算出する。
次に、ランドマークの向き情報を用いた自車位置推定処理について説明する。概略的には、自車位置推定部１７は、計測対象のランドマークの向きを計測すると共に、地図ＤＢ１０に登録された当該ランドマークの向き情報を取得することで、位置に加えて方位の計測値と予測値の差分値を用いたカルマンフィルタを形成し、推定自車位置Ｘ＾（ｔ）を算出する。これにより、１つのランドマークを計測対象として自車位置推定処理を行った場合であっても、高精度な推定自車位置Ｘ＾（ｔ）を算出する。
【0039】
（１）処理概要
図６（Ａ）は、計測対象のランドマークが看板である場合の、車両とランドマークとの位置関係をワールド座標系及び車両座標系において示した図である。ここで、ワールド座標系は、所定地点を原点とし、互いに垂直な座標軸「ｘｗ」及び座標軸「ｙｗ」を有し、車両座標系は、車両の中心を原点とし、車両の正面方向に沿った座標軸「ｘｂ」と車両の側面方向に沿った座標軸「ｙｂ」を有する。そして、図６（Ａ）では、ワールド座標系における予測自車位置「Ｘ－＝（ｘ－、ｙ－、ψ－）Ｔ」、ワールド座標系におけるランドマークの地図ＤＢ１０に基づく位置ベクトル「Ｍ＝（Ｍｘ、Ｍｙ、Ｍψ）Ｔ」、車両座標系における計測値「Ｌ＝（Ｌｘ、Ｌｙ、Ｌψ）Ｔ」の各要素が図示されている。
図６（Ａ）は、計測対象のランドマークが看板である場合の、車両とランドマークとの位置関係をワールド座標系及び車両座標系において示した図である。ここで、ワールド座標系は、所定地点を原点とし、互いに垂直な座標軸「ｘｗ」及び座標軸「ｙｗ」を有し、車両座標系は、車両の中心を原点とし、車両の正面方向に沿った座標軸「ｘｂ」と車両の側面方向に沿った座標軸「ｙｂ」を有する。そして、図６（Ａ）では、ワールド座標系における予測自車位置「Ｘ－＝（ｘ－、ｙ－、ψ－）Ｔ」、ワールド座標系におけるランドマークの地図ＤＢ１０に基づく位置ベクトル「Ｍ＝（Ｍｘ、Ｍｙ、Ｍψ）Ｔ」、車両座標系における計測値「Ｌ＝（Ｌｘ、Ｌｙ、Ｌψ）Ｔ」の各要素が図示されている。
【0040】
この場合、自車位置推定部１７は、計測値Ｌ（ｔ）＝（Ｌｘ（ｔ）、Ｌｙ（ｔ）、Ｌψ（ｔ））Ｔを、ライダ２が出力する計測対象のランドマークに対する距離「ｒ（ｔ）」及び方位「θ」を用いて、以下の式（２）により算出する。
【0041】
【数2】
【0042】
ここで、自車位置推定部１７は、計測されたランドマークの向きを示す式（２）の相対角度Ｌψを、当該ランドマークの被計測面を計測したライダ２の点群データにより算出することが可能である。具体的には、自車位置推定部１７は、ランドマークの平面上の各計測点の座標から、ランドマークの平面を表す式を回帰分析により求め、この平面に対する法線ベクトルを求め、それをｘｙ平面に投影することにより、車両座標系におけるランドマークの相対角度Ｌψを算出する。この処理の詳細は、例えば特許文献３に開示されている。相対角度Ｌψは、本発明における「第１方向」の一例である。
【0043】
また、自車位置推定部１７は、計測予測値Ｌ－（ｔ）＝（Ｌ－
ｘ（ｔ）、Ｌ－
ｙ（ｔ）、Ｌ－
ψ（ｔ））Ｔを、予測自車位置Ｘ－（ｔ）＝（ｘ－（ｔ）、ｙ－（ｔ）、ψ－（ｔ））Ｔと、ランドマークの地図上の位置ベクトルＭ＝（Ｍｘ（ｔ）、Ｍｙ（ｔ）、Ｍψ（ｔ））Ｔとを用いて、以下の式（３）に基づき算出する。
【0044】
【数3】
【0045】
さらに、自車位置推定部１７は、相対角度Ｌψを含む計測値Ｌ（ｔ）と相対角度Ｌ－
ψを含む計測予測値Ｌ－（ｔ）との差分に対してカルマンフィルタＫ（ｔ）を乗じた値により、予測自車位置Ｘ－（ｔ）を補正した推定自車位置Ｘ＾（ｔ）を算出する。即ち、自車位置推定部１７は、推定自車位置Ｘ＾（ｔ）＝（ｘ＾（ｔ）、ｙ＾（ｔ）、ψ＾（ｔ））Ｔを、式（１）に相当する以下の式（４）に基づき算出する。
【0046】
【数4】
【0047】
図６（Ｂ）は、計測対象のランドマークが区画線である場合の車両とランドマークとの位置関係をワールド座標系及び車両座標系において示した図である。計測対象のランドマークが区画線であった場合も同様に、自車位置推定部１７は、式（２）～（４）に基づき、計測値Ｌ（ｔ）、計測予測値Ｌ－（ｔ）、及び推定自車位置Ｘ＾（ｔ）をそれぞれ算出する。
【0048】
ここで、計測対象のランドマークが区画線である場合の式（３）に基づく計測予測値Ｌ－（ｔ）の算出方法について補足説明する。ランドマークが区画線の場合、対応するランドマーク情報には、向き情報として、図６（Ｂ）に示すように、ワールド座標系における区画線に沿った方向（延在方向）を示す向きＭψの情報が含まれている。よって、自車位置推定部１７は、区画線のランドマーク情報を参照して計測予測値Ｌ－（ｔ）を算出する場合、この向きＭψを用いて式（３）の計算を行う。なお、向きＭψの情報がランドマーク情報に含まれてない場合は，その前後数点の座標値をつないで，最小２乗法等により向き乗法を算出する。
【0049】
次に、ランドマークが区画線の場合の計測値Ｌ（ｔ）＝（Ｌｘ（ｔ）、Ｌｙ（ｔ）、Ｌψ（ｔ））Ｔの算出方法について補足説明する。なお、ランドマークが区画線の場合、ランドマーク情報には、位置情報として、例えば数メートル間隔で、区画線の離散的な位置（離散点）を示す緯度経度等の座標情報が含まれている。
【0050】
この場合、まず、自車位置推定部１７は、車両の左前方、左後方、右前方、右後方の少なくともいずれかの方向（図６（Ｂ）では左前方）において、所定距離だけ離れた位置から最も近い区画線の離散点の予測位置を中心として、区画線を検出する範囲を定める予測ウィンドウ「Ｗｐ」を設定する。そして、自車位置推定部１７は、ライダ２が出力する計測データから、予測ウィンドウＷｐ内において、路面上かつ、所定の閾値以上の反射率となる高反射率の計測点を抽出し、抽出した計測点が示す各位置の中心位置のｘｂ座標及びｙｂ座標を、それぞれＬｘ（ｔ）、Ｌｙ（ｔ）として算出する。また、自車位置推定部１７は、区画線上の走査ラインごとの中心点（例えば走査ラインごとの計測点の中心位置）を算出し、これらの中心点から最小２乗法等により求めた回帰直線の傾きに基づき、相対角度Ｌψ（ｔ）を算出する。
【0051】
（２）ブロック構成
次に、自車位置推定部１７の機能的なブロック構成について説明する。
次に、自車位置推定部１７の機能的なブロック構成について説明する。
【0052】
図７は、自車位置推定部１７の機能ブロックの一例を示す。図７に示すように、自車位置推定部１７は、デッドレコニングブロック２０と、移動補正ブロック２１と、ランドマーク抽出ブロック２２と、ローカライズＥＫＦ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）ブロック２３と、を有する。
【0053】
デッドレコニングブロック２０は、ＩＭＵ３、車速センサ４及びＧＰＳ受信機５の出力に基づき、車両の移動速度ｖ、ヨーレートω、及び車両の姿勢情報（例えば、車両のヨー角、ピッチ角、及びロール角）などを算出する。そして、ローカライズＥＫＦブロック２３は、移動補正ブロック２１に対して移動速度ｖ及び車両の姿勢情報を供給すると共に、ローカライズＥＫＦブロック２３に対して制御値ｕ（ｔ）＝（ｖ（ｔ）、ω（ｔ））Ｔを供給する。
【0054】
移動補正ブロック２１は、ライダ２から出力される計測データに対し、デッドレコニングブロック２０から供給される移動速度ｖ及び姿勢情報に基づき補正を行う。例えば、スキャン周期が１００ｍｓの場合、その１００ｍｓの間に車両が移動するため、前方に位置する同じ物体を計測したとしても、１スキャンフレームのスキャン開始時では計測距離が長くなり，終了時では計測距離が短くなってしまう。そのため、上記の補正によって、１スキャンフレーム内での計測距離を一定にしている。そして、移動補正ブロック２１は、補正後のライダ２の計測データをランドマーク抽出ブロック２２へ供給する。
【0055】
ランドマーク抽出ブロック２２は、ローカライズＥＫＦブロック２３が前回時刻に算出した推定自車位置Ｘ＾（ｔ－１）を基に、ライダ２で計測可能な自車周辺のランドマークの位置ベクトルＭ＝（Ｍｘ、Ｍｙ、Ｍψ）Ｔを地図ＤＢ１０から抽出し、そのランドマークに対するライダ２の計測データに基づく計測値Ｌ（ｔ）＝（Ｌｘ、Ｌｙ、Ｌψ）Ｔと、位置ベクトルＭ＝（Ｍｘ、Ｍｙ、Ｍψ）Ｔとを、ローカライズＥＫＦブロック２３に供給する。
【0056】
この場合、ランドマーク抽出ブロック２２は、移動補正ブロック２１から供給された（ｒ（ｔ）、θ（ｔ））の組合せを示すライダ２の計測データを、式（２）に基づき、車両の正面方向と横方向を軸とした成分に変換した車両座標系におけるベクトル値である計測値Ｌ（ｔ）＝（Ｌｘ、Ｌｙ、Ｌψ）Ｔに変換する。この場合、ランドマーク抽出ブロック２２は、例えば、記憶部１２等に記憶されたライダ２の車両に対する設置位置及び設置姿勢に関する情報に基づき、ライダ２を基準とした座標系で表されたライダ２の計測データを、車両座標系の計測値Ｌ（ｔ）に変換するとよい。
【0057】
なお、実際には、ランドマークは所定の大きさを有し、ライダ２のレーザ光が照射された被照射面上の複数の計測点に対応するデータが点群データとして取得される。よって、例えば、ランドマーク抽出ブロック２２は、対象のランドマークを計測した各計測点における計測値を平均化することで、対象のランドマークの中心位置に相当する計測値Ｌ（ｔ）を算出し、ローカライズＥＫＦブロック２３に供給する。
【0058】
ローカライズＥＫＦブロック２３は、式（４）に示す計算を行うことで、推定自車位置Ｘ＾（ｔ）を算出すると共に、カルマンゲインＫ（ｔ）、共分散行列Ｐ＾（ｔ）などの各パラメータの更新を行う。この場合、ローカライズＥＫＦブロック２３は、前回時刻に算出した推定自車位置Ｘ＾（ｔ－１）とデッドレコニングブロック２０から供給される制御値ｕ（ｔ）とに基づき予測自車位置Ｘ－（ｔ）を算出すると共に、ランドマーク抽出ブロック２２から供給される位置ベクトルＭ及び予測自車位置Ｘ－（ｔ）に基づき式（３）を用いて計測予測値Ｌ－（ｔ）を算出する。カルマンゲインＫ（ｔ）の更新方法については後述する。
【0059】
（３）カルマンゲインの更新方法
次に、カルマンゲインＫ（ｔ）の更新方法について説明する。概略的には、自車位置推定部１７は、カルマンゲインＫ（ｔ）を算出する際に用いる観測雑音行列「Ｒ（ｔ）」のｘ方向、ｙ方向、及び方位ψの３要素のそれぞれに対応する対角成分を、ライダ２の計測精度及び地図ＤＢ１０（即ちランドマーク情報）の精度に基づき算出する。これにより、自車位置推定部１７は、ｘ方向、ｙ方向、及び方位ψの３要素に対する計測値Ｌ（ｔ）と計測予測値Ｌ－（ｔ）との差分値に乗じるカルマンゲインＫ（ｔ）の各要素を好適に定める。
次に、カルマンゲインＫ（ｔ）の更新方法について説明する。概略的には、自車位置推定部１７は、カルマンゲインＫ（ｔ）を算出する際に用いる観測雑音行列「Ｒ（ｔ）」のｘ方向、ｙ方向、及び方位ψの３要素のそれぞれに対応する対角成分を、ライダ２の計測精度及び地図ＤＢ１０（即ちランドマーク情報）の精度に基づき算出する。これにより、自車位置推定部１７は、ｘ方向、ｙ方向、及び方位ψの３要素に対する計測値Ｌ（ｔ）と計測予測値Ｌ－（ｔ）との差分値に乗じるカルマンゲインＫ（ｔ）の各要素を好適に定める。
【0060】
カルマンゲインＫ（ｔ）は、観測雑音行列Ｒ（ｔ）と、計測予測値Ｌ－（ｔ）に対する３行３列のヤコビ行列「Ｈ（ｔ）」と、３行３列の共分散行列Ｐ－（ｔ）とを用いて、以下の一般式（５）により算出される。
【0061】
【数5】
【0062】
【数6】
【0063】
次に、上述の計測精度σＬψ（ｉ）及び雑音係数ａＬψの設定方法について説明する。なお、計測精度σＬｘ（ｉ）、σＬｙ（ｉ）については、例えば、仕様書等に記載の（又は実験等に基づき求めた）ライダ２の計測精度をｘｂ方向及びｙｂ方向にそれぞれ分解することで定めることが可能である。雑音係数ａＬｘ、ａＬｙは、予め定められた値に設定してもよく、後述する雑音係数ａＬψと同一値に設定してもよい。また、地図精度σＭｘ（ｉ）、σＭｙ（ｉ）、σＭψ（ｉ）及び地図の雑音係数ａＭｘ、ａＭｙ、ａＭψは、例えば地図ＤＢ１０に記録された精度に関する情報に基づき決定してもよく、予め定められた値に設定してもよい。なお、地図精度は、ワールド座標系を基準としているため、車両座標系の精度に変換する必要があるが、ｘｗ方向とｙｗ方向とで精度が同じである場合には、車両座標系の精度に変換しても同じ値となるため、変換の必要がない。
【0064】
まず、計測精度σＬψ（ｉ）については、例えば、自車位置推定部１７は、計測対象のランドマークまでのｘｂ方向の計測精度とｙｂ方向の計測精度との両方を用いて、以下の式（７）により算出する。
【0065】
【数7】
【0066】
次に、雑音係数ａＬψの設定方法について、計測対象のランドマークが区画線以外の地物（例えば看板や標識など）である場合について説明する。図８（Ａ）は、ライダ２による計測点を明示したランドマークの被照射面の正面図を示す。図８（Ａ）では、ランドマークの計測点は点により表され、ランドマークの縁に当たっている点やランドマークに照射していないその他の計測点は丸により示されている。
【0067】
自車位置推定部１７は、計測対象のランドマークに対して取得したライダ２の点群データの計測点の数に基づき、雑音係数ａＬψを設定する。具体的には、自車位置推定部１７は、計測対象のランドマークに対する計測点の数を「Ｎｐ」、予め定めた定数を「Ｃ」とすると、以下の式（８）に基づき雑音係数ａＬψを算出する。
【0068】
【数8】
【0069】
次に、計測対象のランドマークが区画線である場合の雑音係数ａＬψの設定方法について説明する。
【0070】
この場合、自車位置推定部１７は、予測ウィンドウＷｐ内の走査ライン数及び区画線の幅の期待値と計測値との差に基づき、雑音係数ａＬψを設定する。具体的には、自車位置推定部１７は、予測ウィンドウＷｐ内の区画線上の走査ライン数の期待値（即ち予測ウィンドウＷｐ内の走査ライン数）を「ＮＭ」、予測ウィンドウＷｐ内の区画線上の走査ライン数の計測値を「ＮＬ」、予測ウィンドウＷｐ内の区画線の走査ラインごとの幅の期待値（即ち地図ＤＢ１０に登録された計測対象の区画線の幅）を「ＷＭ」、予測ウィンドウＷｐ内の区画線の走査ラインごとの幅の計測値を「ＷＬ」とすると、以下の式（９）に基づき、雑音係数ａＬψを算出する。
【0071】
【数9】
【0072】
図８（Ｂ）は、区画線が破線である場合の予測ウィンドウＷｐ内の走査ラインＬ１～Ｌ４及び各走査ラインの中心点Ｐ１～Ｐ４を示した図である。また、図８（Ｃ）は、区画線が劣化している場合の予測ウィンドウＷｐ内の走査ラインＬ１～Ｌ１０及び各走査ラインの中心点Ｐ１～Ｐ１０を示した図である。
【0073】
図８（Ｂ）の例では、計測対象の区画線が破線となっているため、予測ウィンドウＷｐ内において途切れた部分が生じている。よって、この場合、予測ウィンドウＷｐ内の走査ライン数の期待値ＮＭ（ここでは１０）と計測値ＮＬ（ここでは４）との差が大きくなり、式（９）に基づく雑音係数ａＬψは、計測値ＮＬに対する期待値ＮＭの比に応じて大きくなる。また、図８（Ｃ）の例では、計測対象の区画線に劣化が生じており、区画線の部分的な欠損等に起因して走査ラインによっては期待値ＷＭよりも計測値ＷＬが短くなっている。よって、この場合、走査ラインごとの期待値ＷＭと計測値ＷＬとの差の積算値が大きくなり、式（９）に基づく雑音係数ａＬψは、上記の積算値に応じて大きくなる。
【0074】
ここで、式（９）について補足説明する。自車位置推定部１７は、上述したように、区画線上の各走査ラインの中心点から最小２乗法等により求めた回帰直線に基づき、区画線が延在する方向を示す車両座標系での相対角度Ｌψ（ｔ）を算出する。従って、走査ラインの中心点の数が少ないほど、相対角度Ｌψ（ｔ）の算出に用いる回帰直線の算出精度が低くなることが推定される。以上を勘案し、自車位置推定部１７は、式（９）に基づき、計測値ＮＬが小さいほど（即ち計測値ＮＬに対する期待値ＮＭの比が大きいほど）、雑音係数ａＬψを大きくする。
【0075】
また、計測対象の区画線が劣化している場合、相対角度Ｌψ（ｔ）の算出に用いる区画線の剥がれ箇所の存在等に起因して区画線の幅にばらつきが生じ、それに応じて各走査ラインの中心点にばらつきが発生し、結果として相対角度Ｌψ（ｔ）の算出精度が低下する。以上を勘案し、自車位置推定部１７は、式（９）に基づき、計測対象の区画線の幅ＷＭと、区画線の幅の計測値ＷＬとの差が大きいほど、雑音係数ａＬψを大きくする。
【0076】
【0077】
まず、自車位置推定部１７は、ＧＰＳ受信機５等の出力に基づき、自車位置の初期値を設定する（ステップＳ１０１）。次に、自車位置推定部１７は、車速センサ４から車体速度を取得すると共に、ＩＭＵ３からヨー方向の角速度を取得する（ステップＳ１０２）。そして、自車位置推定部１７は、ステップＳ１０２の取得結果に基づき、車両の移動距離と車両の方位変化を計算する（ステップＳ１０３）。
【0078】
その後、自車位置推定部１７は、１時刻前の推定自車位置Ｘ＾（ｔ－１）に、ステップＳ１０３で計算した移動距離と方位変化を加算し、予測自車位置Ｘ－（ｔ）を算出する（ステップＳ１０４）。さらに、自車位置推定部１７は、予測自車位置Ｘ－（ｔ）に基づき、地図ＤＢ１０のランドマーク情報を参照し、ライダ２の計測範囲となるランドマークを探索する（ステップＳ１０５）。
【0079】
そして、自車位置推定部１７は、式（３）に基づき、予測自車位置Ｘ－（ｔ）及びステップＳ１０５で探索したランドマークのランドマーク情報が示す位置情報及び向き情報から、予測されたランドマークの向きを示す相対角度Ｌ－
ψを含む計測予測値Ｌ－（ｔ）を算出する（ステップＳ１０６）。さらに、ステップＳ１０６では、自車位置推定部１７は、式（２）に基づき、ステップＳ１０５で探索したランドマークに対するライダ２の計測データから、予測されたランドマークの向きを示す相対角度Ｌψを含む計測値Ｌψ（ｔ）を算出する。
【0080】
そして、自車位置推定部１７は、ライダ２の計測精度及び地図精度に基づき、方位ψの雑音係数を含む式（６）に示す観測雑音行列Ｒ（ｔ）を算出する（ステップＳ１０７）。そして、自車位置推定部１７は、算出した観測雑音行列Ｒ（ｔ）に基づき、式（５）を用いて、３行３列のカルマンゲインＫ（ｔ）を算出する（ステップＳ１０８）。これにより、位置に加えて方位ψの計測値と予測値の差分値を用いたカルマンフィルタを好適に形成する。
【0081】
そして、自車位置推定部１７は、式（４）に基づき、生成したカルマンゲインＫ（ｔ）を用いて予測自車位置Ｘ－（ｔ）を補正することで、推定自車位置Ｘ＾（ｔ）を算出する（ステップＳ１０９）。
【0082】
（５）効果
次に、本実施例におけるランドマークの向き情報を用いた自車位置推定処理の効果について補足説明する。
次に、本実施例におけるランドマークの向き情報を用いた自車位置推定処理の効果について補足説明する。
【0083】
ここで、本実施例に係るカルマンフィルタとの比較のため、方位ψの計測値と予測値の差分値を用いないカルマンフィルタ（「比較例に係るカルマンフィルタ」とも呼ぶ。）について検討する。この比較例では、自車位置推定部１７は、以下の式（１０）に基づき計測値Ｌ（ｔ）を算出し、以下の式（１１）に基づき計測予測値Ｌ－（ｔ）を算出する。
【0084】
【数10】
【0085】
【数11】
【0086】
そして、自車位置推定部１７は、式（１２）に基づき推定自車位置Ｘ＾（ｔ）を算出する。
【0087】
【数12】
【0088】
この場合、式（４）では、推定自車位置Ｘ＾の各要素であるｘ＾、ｙ＾、ψ＾の３つのパラメータを算出するのに、ｘ方向、ｙ方向の２要素に対する計測値Ｌ（ｔ）と計測予測値Ｌ－（ｔ）との差分値を用いることになる。よって、この場合、１つのランドマークを対象として推定自車位置Ｘ＾（ｔ）を算出する場合、位置と方位が一意に定まらないという問題が生じる。
【0089】
図１０（Ａ）、（Ｂ）は、同一の計測値Ｌｘ、Ｌｙに対して車両とランドマークとの異なる位置関係をワールド座標系及び車両座標系において示した図である。図１０（Ａ）、（Ｂ）に示すように、同一の計測値Ｌｘ、Ｌｙが算出される場合であっても、計測値Ｌｘ、Ｌｙのみによっては車両とランドマークとの位置関係が一意に定まらず、車両とランドマークとの位置関係が互いに異なっている。具体的には、図１０（Ｂ）の例では、図１０（Ａ）の例と比較してｘ－が大きく、ｙ－が小さく、方位ψ－が大きくなっている。
【0090】
次に、本実施例に係るカルマンフィルタを適用した場合と比較例に係るカルマンフィルタを適用した場合のそれぞれのシミュレーション結果について説明する。出願人は、一例として、車両が停止状態であって、車両から１０ｍ離れた位置に１つランドマークが存在する仮想環境下において、意図的に予測自車位置Ｘ－（ｔ）に誤差情報（速度４［ｍ／ｓ］、角速度０．４［ｒａｄ／ｓ］）を付して６０秒間にわたりシミュレーションを実行した。
【0091】
図１１（Ａ）、（Ｂ）は、比較例に係るカルマンフィルタに基づく自車位置推定のシミュレーション結果を示す図である。具体的には、図１１（Ａ）は、比較例に係るカルマンフィルタに基づく車両の推定位置（推定値）の軌跡を、車両の正しい位置（真値）及びランドマークの位置と共に示している。なお、図中の矢印は、ランドマーク又は車両の向きを表している。また、図１１（Ｂ）は、真値に対する推定値の誤差をｘ方向、ｙ方向、方位ψごとに示したグラフである。
【0092】
図１１（Ａ）、（Ｂ）に示すように、比較例では、車両からランドマークまでの距離は一定を保っているものの、徐々に車両の推定位置が真値からずれていってしまう。このように、比較例では、計測対象のランドマークが１つの場合、推定自車位置Ｘ＾の各要素であるｘ＾、ｙ＾、ψ＾の３つのパラメータを算出するのに、ｘ方向、ｙ方向の２要素に対する計測値Ｌ（ｔ）と計測予測値Ｌ－（ｔ）との差分値を用いるため、誤差が発生した予測自車位置Ｘ－（ｔ）を正確に補正することができない。
【0093】
図１２及び図１３は、本実施例に係るカルマンフィルタに基づく自車位置推定のシミュレーション結果を示す図である。具体的には、図１２は、本実施例に係るカルマンフィルタ（式（４）参照）に基づく車両の推定位置（推定値）の軌跡を、車両の実際の位置（真値）及びランドマークの位置と共に示している。また、図１３（Ａ）は、真値に対する推定値のｘ方向の誤差を示したグラフであり、図１３（Ｂ）は、真値に対する推定値のｙ方向の誤差を示したグラフであり、図１３（Ｃ）は、真値に対する推定値の方位ψの誤差を示したグラフである。なお、図１３（Ａ）～（Ｃ）に示すグラフの縦軸の縮尺は、比較例に関する誤差を示した図１１（Ｂ）に示すグラフの縦軸の縮尺よりも遥かに小さい。
【0094】
図１２及び図１３に示すように、本実施例では、ランドマークの向き情報を用いたカルマンフィルタを適用することにより、車両からみたランドマークの向きが好適に固定されるため、誤差が発生した予測自車位置Ｘ－（ｔ）は好適に補正され、位置推定に殆どずれが生じない。このように、本実施例に係るカルマンフィルタを用いた場合、１つのランドマークを計測対象とした場合であっても、正確な推定自車位置Ｘ＾（ｔ）を好適に算出することができる。
【0095】
以上説明したように、本実施例に係る車載機１の自車位置推定部１７は、移動体の現在位置と向きとの予測値である予測自車位置Ｘ－（ｔ）を取得する。そして、自車位置推定部１７は、ライダ２によるランドマークの計測データに基づく車両に対するランドマークの向きを示す第１向き情報である相対角度Ｌψ（ｔ）を取得し、かつ、ランドマーク情報に含まれるランドマークの向きを車両に対する向きに変換した向きを示す第２向き情報である相対角度Ｌ－
ψ（ｔ）を取得する。そして、自車位置推定部１７は、相対角度Ｌψ（ｔ）と相対角度Ｌ－
ψ（ｔ）との差分に基づいて、予測自車位置Ｘ－（ｔ）を補正する。これにより、自車位置推定部１７は、１つのランドマークしか検出できない状況であっても、移動体の現在位置と向きとの予測値である予測自車位置Ｘ－（ｔ）を的確に補正して推定自車位置Ｘ＾（ｔ）を定めることができる。
【0096】
【符号の説明】
【0097】
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
