JP7298882B2 - 車両の自己位置推定装置、及び車両 - Google Patents

Description

特許法第３０条第２項適用 １．平成３０年６月２５日発行，Ｔｈｅ ２９ｔｈ ＩＥＥＥ Ｉｎｔｅｌｌｉｇｅｎｔ Ｖｅｈｉｃｌｅｓ Ｓｙｍｐｏｓｉｕｍ ＦＩＮＡＬ ＰＲＯＧＲＡＭ，第９７１～９７７頁，Ｉｎｓｔｉｔｕｔｅ ｏｆ Ｅｌｅｃｔｒｉｃａｌ ａｎｄ Ｅｌｅｃｔｒｏｎｉｃｓ Ｅｎｇｉｎｅｅｒｓ（ＩＥＥＥ）／ＩＥＥＥ Ｉｎｔｅｌｌｉｇｅｎｔ Ｔｒａｎｓｐｏｒｔａｔｉｏｎ Ｓｙｓｔｅｍｓ Ｓｏｃｉｅｔｙ（ＩＴＳＳ） ２．平成３０年６月２８日に開催された、Ｔｈｅ ２９ｔｈ ＩＥＥＥ Ｉｎｔｅｌｌｉｇｅｎｔ Ｖｅｈｉｃｌｅｓ Ｓｙｍｐｏｓｉｕｍにて発表

本発明は、車両の自己位置をリアルタイムに推定する車両の自己位置推定装置、及び当該自己位置推定装置を搭載した車両に関する。

近年、交通事故の削減や交通渋滞の緩和、誰もが利用できる移動手段の提供等を目的として、車両の運転が自動的に行われる自動運転システムの開発が盛んに行われている。自動運転自動車が目的地へ向かって正しく移動するためには、当該車両の位置をリアルタイムに推定する自己位置推定が必要である。
自己位置推定を行うための一般的な手法として、あらかじめ定義した地図（地図画像）を用いたマップマッチングがあり、その手法では主にＬｉＤＡＲ（Light Detection and Ranging）やカメラといったセンサが用いられる。
例えば、非特許文献１では、ＬｉＤＡＲを用いたマップマッチング手法が開示されている。ＬｉＤＡＲを用いた手法は、測定精度が高く、昼夜の変化や環境の変化に対して高いロバスト性を備えているため、デシメートルレベルの精度で車両の自己位置推定を行うことができる。
また、非特許文献２では、カメラで検出した道路面の特徴をベクトル地図（白線、道路標識、信号機等の位置を記録したデジタル地図）とマッチングすることで、車両の自己位置を、車両横方向で０．１ｍ程度、車両前後方向で０．５ｍ程度の精度で推定することが開示されている。

しかしながら、ＬｉＤＡＲやカメラを用いた自己位置推定は、路面が雪で覆われている場合、降雪によって車両周辺の物体の形状が変化している場合、又は雨によって路面が正確に把握できない場合などには、精度が安定しないという問題がある。
非特許文献３では、このような問題を解決するために、過去のフレームを使うことで、雨で濡れた道路や雪道においてＬｉＤＡＲの反射率が低減することを防ぐ手法が開示されている。しかし、この手法では、雪で完全に覆われている道路や、雪壁によって道路形状が変化する場所において、自己位置を精度よく推定することができない。

また、非特許文献４には、ミリ波レーダーとカメラを用いて、道路面の情報に基づき自己位置推定を行う手法が開示されている。しかし、この手法も積雪時に精度よく自己位置推定を行うことはできない。
また、非特許文献５には、クラスタリングしたミリ波レーダーの観測情報を用いて地図生成を行い、さらにパーティクルフィルタを用いて自己位置推定を行う手法が開示されている。しかし、ミリ波レーダーによる観測点が疎らであることから、誤差が最大３ｍを超える推定精度であり、かつメートルレベルの精度でしか自己位置推定ができなかったと記載されている。

また、特許文献１には、横位置補正処理部は、降雪等の影響によりカメラユニットで自車進行路の左右区画線が認識されていないと判定した場合、自車両の道路地図上の走行車線に記憶されている地図曲率を読込み、追従対象先行車の走行軌跡に基づいて先行曲率を求め、両曲率と自車速とに基づいて推定横位置偏差を求め、推定横位置偏差を目標ハンドル角のフィードバック項である横位置ハンドル角の自車横位置に加算して、新たな自車横位置を設定することで、自車両を先行車の走行軌跡に沿って走行させる車両の運転支援装置が開示されている。
しかし、特許文献１の運転支援装置は、降雪時においても車両の自己位置を精度よく推定するものではない。また、先行車の走行軌跡を利用するものであるから、先行車が存在しない場合には、降雪等の影響によりカメラユニットで自車進行路の左右区画線が認識されない状況における自動運転が困難となる。

特開２０１９－３８３９６号公報

J. Levinson and S. Thrun, "Robust Vehicle Localization in Urban Environments Using Probabilistic Maps", Proceedings of 2010 IEEE International Conference on Robotics and Automation, pp. 4372-4378, 2010. Jo, Kichun, et al. "Precise Localization of an Autonomous Car Based on Probabilistic Noise Models of Road Surface Marker Features Using Multiple Cameras." IEEE Trans. Intelligent Transportation Systems vol.16, no.6, pp. 3377-3392, 2015. M. Aldibaja, N. Suganuma and K. Yoneda, "Robust Intensity Based Localization Method for Autonomous Driving on Snow-wet Road Surface", IEEE Transactions on Industrial Informatics, vol. 13, no. 5, pp. 2369-2378, 2017. S. Park, D. Kim and K. Yi, "Vehicle Localization using an AVM camera for An Automated Urban Driving", Proceedings of 2016 IEEE Intelligent Vehicles Symposium, pp. 871-876, 2016. F.Schuster, M.Worner, C.G. Keller, M. Haueis and C. Curio, "Robust localization based on radar signal clustering", Proceedings of 2016 IEEE Intelligent Vehicles Symposium, pp. 839-844, 2016.

そこで本発明は、精度よく自己位置を推定できる車両の自己位置推定装置、及び当該装置を備えた車両を提供することを目的とする。

請求項１記載の本発明の車両の自己位置推定装置は、車両の周辺に存在する物体の観測情報に基づきリアルタイムに作成した観測画像と予め作成された地図画像とのマッチングを行うことにより車両の位置を推定する自己位置推定装置であって、ＧＮＳＳ衛星及び慣性計測装置からのデータを取得するＧＮＳＳ／ＩＭＵデータ取得部と、車両の周辺に存在する物体の観測情報を取得する周辺物体情報取得センサ部と、ＧＮＳＳ／ＩＭＵデータ取得部で取得したデータに基づいてデッドレコニングにより車両の位置を推定するＤＲ部と、ＤＲ部で推定した車両の位置及び周辺物体情報取得センサ部で取得した観測情報に基づき、物体のうち静的物体の位置がマッピングされた観測画像を生成する観測画像生成部と、観測画像と地図画像とのマッチングを行うマッチング部と、マッチングの結果に基づいてオフセットの確率分布を更新する確率更新部と、確率分布に基づいてオフセット量を更新するオフセット更新部と、ＤＲ部で推定した車両の位置をオフセット量で補正することにより車両の補正後位置を導出する補正後位置導出部とを備え、周辺物体情報取得センサ部は、雪を透過して物体の観測情報を取得可能であり、マッチング部は、テンプレートマッチングを行い、確率更新部は、テンプレートマッチングにより得られた相関分布をガンマ補正して尤度分布を導出し、尤度分布を用いて確率分布を更新することを特徴とする。

請求項２記載の本発明は、請求項１に記載の車両の自己位置推定装置において、確率更新部は、各時刻で得られた尤度分布を積算し、積算した尤度分布を正規化して累積分布を導出し、累積分布に基づいて確率分布を更新することを特徴とする。

請求項３記載の本発明は、請求項１に記載の車両の自己位置推定装置において、確率更新部は、尤度分布を用いて事後確率の対数オッズ値を導出し、対数オッズ値を用いて確率分布を更新することを特徴とする。

請求項４記載の本発明は、請求項２又は請求項３に記載の車両の自己位置推定装置において、オフセット更新部は、確率分布の重み付け平均を用いてオフセット量を更新することを特徴とする。

請求項５記載の本発明は、請求項３に記載の車両の自己位置推定装置において、オフセット更新部は、確率分布の最大値をマッチング状態の不確実性とし、不確実性に従った重み付けによりオフセット量を更新することを特徴とする。

請求項６記載の本発明の車両は、請求項１から請求項５のいずれか１項に記載の車両の自己位置推定装置を搭載したことを特徴とする。

本発明によれば、車両の自己位置推定の精度を向上させることができる。

本発明の実施例による車両の自己位置推定装置のブロック図 同自己位置推定装置を搭載した自車両の外観図 同自車両におけるミリ波レーダーの取り付け位置と観測領域を示す図 同デッドレコニングによる自己位置推定とオフセット補正の説明図 同地図画像生成手段のブロック図 同地図画像生成の例を示す図 同観測誤差伝播に基づく共分散行列を使用した存在尤度の定義についての説明図 同生成された観測画像の例を示す図 同尤度フィルタリングを示す図 同複数のピークが発生した場合の処理についての説明図 第一の実施例の夏季（積雪無し）の一つ目のエリアにおける試験結果を示す図 同冬季（部分積雪）の一つ目のエリアにおける試験結果を示す図 同冬季（完全積雪）の二つ目のエリアにおける試験結果を示す図 第二の実施例の試験結果を示す図

本発明の第１の実施の形態による車両の自己位置推定装置は、ＧＮＳＳ衛星及び慣性計測装置からのデータを取得するＧＮＳＳ／ＩＭＵデータ取得部と、車両の周辺に存在する物体の観測情報を取得する周辺物体情報取得センサ部と、ＧＮＳＳ／ＩＭＵデータ取得部で取得したデータに基づいてデッドレコニングにより車両の位置を推定するＤＲ部と、ＤＲ部で推定した車両の位置及び周辺物体情報取得センサ部で取得した観測情報に基づき、物体のうち静的物体の位置がマッピングされた観測画像を生成する観測画像生成部と、観測画像と地図画像とのマッチングを行うマッチング部と、マッチングの結果に基づいてオフセットの確率分布を更新する確率更新部と、確率分布に基づいてオフセット量を更新するオフセット更新部と、ＤＲ部で推定した車両の位置をオフセット量で補正することにより車両の補正後位置を導出する補正後位置導出部とを備えるものである。
本実施の形態によれば、デッドレコニングにより推定した車両の位置をマッチングの結果に基づいて補正することで、自己位置推定の精度を向上させることができる。

本発明の第２の実施の形態は、第１の実施の形態による車両の自己位置推定装置において、周辺物体情報取得センサ部は、雪を透過して物体の観測情報を取得可能であり、マッチング部は、テンプレートマッチングを行い、確率更新部は、テンプレートマッチングにより得られた相関分布をガンマ補正して尤度分布を導出し、尤度分布を用いて確率分布を更新するものである。
本実施の形態によれば、積雪条件下においても精度よく自己位置を推定することができる。また、テンプレートマッチングを行うことにより、処理の高速化が可能となる。

本発明の第３の実施の形態は、第２の実施の形態による車両の自己位置推定装置において、確率更新部は、各時刻で得られた尤度分布を積算し、積算した尤度分布を正規化して累積分布を導出し、累積分布に基づいて確率分布を更新するものである。
本実施の形態によれば、相関の寄与度を低くし確率更新できるので、周辺物体情報取得センサ部による物体観測が疎らであっても精度よく自己位置を推定することができる。

本発明の第４の実施の形態は、第２の実施の形態による車両の自己位置推定装置において、確率更新部は、尤度分布を用いて事後確率の対数オッズ値を導出し、対数オッズ値を用いて確率分布を更新するものである。
本実施の形態によれば、相関の寄与度を低くし確率更新できるので、周辺物体情報取得センサ部による物体観測が疎らであっても精度よく自己位置を推定することができる。

本発明の第５の実施の形態は、第３又は第４の実施の形態による車両の自己位置推定装置において、オフセット更新部は、確率分布の重み付け平均を用いてオフセット量を更新するものである。
本実施の形態によれば、道路脇に同じパターンの静的物体である柵やガードレール等が連続して存在する場合であっても精度よく自己位置を推定することができる。

本発明の第６の実施の形態は、第４の実施の形態による車両の自己位置推定装置において、オフセット更新部は、確率分布の最大値をマッチング状態の不確実性とし、不確実性に従った重み付けによりオフセット量を更新するものである。
本実施の形態によれば、不確実性に従って重み付けすることによって、不安定な状況における過度のオフセット更新を防ぎ、より精度よく自己位置を推定することができる。

本発明の第７の実施の形態による車両は、第１から第６の実施の形態のいずれか一つの車両の自己位置推定装置を搭載したものである。
本実施の形態によれば、自己位置をリアルタイムに精度よく推定し、安全かつ的確に目的地へ移動する自動運転車両を提供することができる。

本発明の第一の実施例による車両の自己位置推定装置について説明する。
図１は本実施例による車両の自己位置推定装置のブロック図である。
自己位置推定装置１０は、ＧＮＳＳ（Global Navigation Satellite System）衛星及び慣性計測装置からのデータを取得するＧＮＳＳ／ＩＭＵデータ取得部１１と、車両の周辺に存在する物体の観測情報をリアルタイムに取得する周辺物体情報取得センサ部１２と、ＧＮＳＳ／ＩＭＵデータ取得部１１で取得したデータに基づいてデッドレコニングにより車両の位置を推定するＤＲ部１３と、ＤＲ部１３で推定した車両の位置及び周辺物体情報取得センサ部１２が取得した観測情報に基づき、物体のうち静的物体の位置がマッピングされた観測画像を生成する観測画像生成部１４と、観測画像と地図画像とのマッチングを行うマッチング部１５と、マッチングの結果に基づいてオフセットの確率分布を更新する確率更新部１６と、確率分布に基づいてオフセット量を更新するオフセット更新部１７と、ＤＲ部１３で推定した車両の位置をオフセット量で補正することにより車両の補正後位置を導出する補正後位置導出部１８と、地図画像が記憶されている地図画像記憶部１９を備える。
観測画像生成部１４は、車両の周辺に存在する物体を追跡し、その物体が静的物体か動的物体かを推定する物体追跡部１４Ａと、追跡した物体の中から静的物体を抽出する静的物体抽出部１４Ｂと、抽出した静的物体のマッピングを行うマッピング部１４Ｃを有する。

自己位置推定装置１０は、車両に搭載され、搭載された車両（以下、「自車両」という）の周辺に存在する物体の観測情報に基づき、リアルタイムに作成した観測画像と予め作成された地図画像とのマッチングを行うことにより自己位置を推定する。図２は自己位置推定装置を搭載した自車両の外観図である。
自車両１には、上部にＧＮＳＳ衛星からの信号を受信するＧＮＳＳ受信部（ＧＮＳＳアンテナ）３０が取り付けられ、後輪に慣性計測装置であるＩＭＵ（Inertial Measurement Unit）４０が取り付けられている。ＧＮＳＳ及びＩＭＵを用いることで、自車両１の位置（緯度、経度、高度）、姿勢（ピッチ、ヨー、ロール）、速度及び加速度等を１００Ｈｚで取得できる。
また、自車両１は、全方位照射可能なレーザーであるＬｉＤＡＲ（Light Detection and Ranging）５０を備えており、自己位置推定装置１０による自己位置推定とは別に、ＬｉＤＡＲ５０から得られる赤外線反射率を利用した自己位置推定を行うことができる。
また、自車両１には、周辺物体情報取得センサ部１２として、複数個の７６ＧＨｚ帯ミリ波レーダー（ＭＷＲ）を取り付けている。図３は自車両におけるミリ波レーダーの取り付け位置と観測領域を示す図である。図３に示すように、本実施例のミリ波レーダーは、視野角が約４０度であり、自車両１の前方バンパーに７個、後方バンパーに２個設置することで全方位を検出可能にしている。本実施例のミリ波レーダーは、車両周辺を２０Ｈｚで観測可能であり、自車両１の周辺に存在する人、他車両、道路設置物等といった周辺に存在する物体までの相対距離、相対角度、相対速度といった観測情報を取得する。ミリ波レーダーは、環境変化に強く、雪を透過して物体検出が可能である。

図４はデッドレコニングによる自己位置推定とオフセット補正の説明図である。
自己位置推定装置１０は、自己位置推定を開始すると、ＤＲ部１３において、ＧＮＳＳ／ＩＭＵデータ取得部１１が取得したデータに基づいてデッドレコニングにより自車両１の位置を推定する。
デッドレコニングとは、車両の速度を時間で積分し、車両の位置を推定する手法である。本実施例では、線速度とヨーレートの時間積分によって自車両１の推定位置［ｘ_d,t，ｙ_d,t］^Tを更新する。しかしながら、ＧＮＳＳ／ＩＭＵデータ取得部１１が取得する位置や速度には誤差が含まれるため、デッドレコニングでは、逐次移動量を計算した際に発生する誤差が累積してしまう。

そこで、自己位置推定装置１０は、マッチング部１６において、地図画像記憶部１９に記憶されている地図画像と、観測画像生成部１４で生成された観測画像とのマッチング（照合）を行うことによりデッドレコニングの誤差（オフセット）［Δｘ_d,t，Δｙ_d,t ]^Tを推定し、デッドレコニングにより推定した自車両１の推定位置［ｘ_d,t，ｙ_d,t］をオフセットで補正する。自車両１の位置Ｘ_t，Ｙ_tは下式（１）で表される。

オフセット量［Δｘ_d,t，Δｙ_d,t ]^Tは、以下の手順（１）～（４）で推定する。なお、本実施例では、自車両１の位置と姿勢のうち、姿勢についてはＧＮＳＳ／ＩＭＵデータ取得部１１から直接取得し、位置に関するオフセット量［Δｘ_d,t，Δｙ_d,t ]^Tのみを推定する。このように位置のみを推定することで、計算の簡易化及び高速化を図ることができる。
（１）周辺物体情報取得センサ部１２で取得した最新のＮフレームのデータを用いて観測画像生成部１４で観測画像を生成する。
（２）地図画像記憶部１９に記憶されている地図画像の中から、自車両１周辺の地図画像を抽出する。
（３）マッチング部１５における観測画像と抽出した地図画像とのマップマッチングを行い、確率更新部１６でオフセットの確率Ｐ_ｔを推定する。
（４）得られた確率Ｐ_ｔに基づいてオフセット更新部１７でオフセット量［Δｘ_d,t，Δｙ_d,t ]^Tを更新する。
以下、各手順について説明する。

まず、地図画像記憶部１９に記憶されている地図画像の生成方法について説明する。
図５は地図画像生成手段のブロック図である。
地図画像生成手段２０は、リアルタイムキネマティック測位方式のＲＴＫ－ＧＮＳＳ部２１と、自車両１周辺に存在する物体を追跡し、その物体が静的物体か動的物体かを推定する物体追跡部１４Ａ（本実施例では観測画像生成部１０と共用）と、追跡した物体の中から静的物体を抽出する静的物体抽出部１４Ｂ（本実施例では観測画像生成部１０と共用）と、抽出した静的物体のマッピングを行う地図画像マッピング部２２を有し、地図画像を生成する。
地図画像は、後処理でＲＴＫ－ＧＮＳＳを使用して、周辺物体情報取得センサ部１２が取得した観測値をマッピングすることによって生成される。後処理補正後のＧＮＳＳ位置を利用することで、０．０３ｍ程度の位置精度を実現することができる。
ここで、７６ＧＨｚ帯ミリ波レーダーにおける測定角度の検出精度は、ＬｉＤＡＲや７９ＧＨｚ帯ミリ波レーダーと比較すると一般的には正確ではない。したがって、地図画像のマッピングの際には測定精度を考慮する必要がある。地図画像は、以下の手順（Ａ）～（Ｃ）により生成される。地図画像生成部２４が生成した地図は、地図画像生成部１９に記憶される。
（Ａ）物体追跡：静的／動的物体を推定する。
（Ｂ）静的物体抽出：動的物体を削除する。
（Ｃ）マッピング：各ピクセルにおける静的物体の確率を更新する。

物体追跡部２２は、ミリ波レーダーによる観測情報を用いて静的／動的物体を推定する。なお、本実施例では、自車両１を用いて地図画像を生成するため、周辺物体情報取得センサ部１２（ミリ波レーダー）を使用して観測情報を取得する。
ミリ波レーダーは、静的物体だけでなく、移動する物体や、移動し得る静的物体など様々な物体を観測する。そこで、観測情報から地図画像の生成に使用する静的物体のみを抽出するために、（i）ミリ波レーダー照射方向の絶対速度が閾値以下であること、（ii）道路の車線外に存在すること、及び（iii）動的物体と対応付けられていない非追跡物体であること［非特許文献６：永野聖巳, et al.“全方位ミリ波レーダを用いた自動運転自動車のための周辺移動車両追跡に関する研究”自動車技術会論文集 vol.48,no.2,pp.411-418,2017.］の条件をすべて満たす観測情報を静的物体と判断する。

ＩＭＭ（interactive multiple model）［非特許文献７：R. Helmick, “IMM Estimator with Nearest-Neighbor Joint Probabilistic Data Association”, Multiagent-Multisensor Tracking: Applications and Advances Volume III, Artech House Publishers, pp. 161-198, 2000.］は、観測対象物体の位置、速度、及び加速度を推定するために複数種類の運動を統合するために採用される。等加速度、等速、及び停止モデルは、運動モデルとして定義される。状態変数ｘ^mwrは、下式（２）で定義される。

ここで、ｐ^xとｐ^yは、ＵＴＭ（Universal Transverse Mercator）のグローバル座標における物体位置である［非特許文献８：J. P. Snyder, “Map Projections: A Working Manual”, Geological Survey(U.S.), 1987.］。ｖ^x、ａ^x、ｖ^y及びａ^yは、それぞれｘ座標とｙ座標の速度と加速度である。
下式（３）のｗ^mwrは、対応する変数に対するプロセスノイズベクトルである。

例えば、独立定加速度モデルの状態方程式は下式（４）のように定式化できる。

等速モデルの場合は、加速度値はゼロに設定される。さらに、一定のヨーレートを有するモデルは、非ホロノミックシステムを用いて車両運動を表現するように定義される。ＩＭＭは、自車両１の周囲に存在する自動車や自転車などの動的物体を追跡できる。したがって、動的物体は、地図画像生成では無視することができる。
地図画像は、観測した物体のうち動的物体を除去することによって得られた静的物体を使用して生成される。グローバル座標内の観測点は、２－Ｄ画像座標に変換され、対応するピクセルにマッピングされる。ミリ波レーダーの低い検知精度を考慮するために、存在尤度は、観測誤差伝播に基づいて共分散行列Ｐを使用して定義される。
図６は地図画像生成の例を示す図であり、図６（ａ）はマッピング処理の一例を示し、図６（ｂ）はミリ波レーダーを用いて生成した地図画像の一例を示し、図６（ｃ）はＬｉＤＡＲを用いて生成した地図画像の一例を示している。図６（ａ）においては、色が薄いほど静的物体Ａの存在確率が高いことを示している。なお、図６（ａ）の右図に示すように、静的物体の存在確率は誤差を考慮した確率密度分布とするため、９５％の確率で静的物体が存在する領域とする。
存在尤度は、周辺物体情報取得センサ部１２の照射方向に対して垂直な分散を生成するように計算される。各ピクセルに対する尤度値は、尤度分布の対応する領域における積分によって得られる。そして、存在確率は、尤度値に基づいて更新される。得られた地図画像である図６（ｂ）の各ピクセルは、緯度と経度に関する情報を有する。図６（ｂ）の地図画像には、図６（ｃ）に示されるＬｉＤＡＲを用いた地図画像と比較すると、歩道上又は道路脇に位置する電信柱やガードレール等の静的物体をランドマークとして描写できていることが分かる。

ここで、図７を用いて、観測誤差伝播に基づく共分散行列Ｐを使用した存在尤度の定義について説明する。
ミリ波レーダーのデータから提ｖ供される観測値は、下式（５）として表される。

ミリ波レーダーのデータは、距離ｒ^mwr、照射方向θ^mwr、照射方向の相対速度ｖ^mwrである。図７によれば、下式（６）、（７）、（８）のようにして、物体の位置をセンサ座標からグローバル座標に変換することができる。

ここで、[ｘ^mwr，ｙ^mwr]^Tと[ｘ_v ^mwr，ｙ_v ^mwr]は、それぞれセンサ座標と車両座標における物体の位置である。［ｘ_s，ｙ_s，θ_s］^Tは、車両座標におけるセンサ設置位置である。［ｘ_ego，ｙ_ego，θ_ego］はグローバル座標での車両位置である。

照射方向における車両の絶対速度ｖ_ego ^mwrは、車両速度ｖ_egoを用いて下式（９）のように計算される。

したがって、観測値は下式（１０）で表される。

ここで、φ＝θ_ego＋θ_s＋θ^mwr，θ＝θ_s＋θ^mwrであり、Ｓ_ego＋ｓｉｎθ_ego，Ｃ_ego＝ｃｏｓθ_ego，Ｓ_φ＝ｓｉｎφ，Ｃ_φ＝ｃｏｓφである。
したがって、観測値ｚは、下式（１１）のように、１０個の変数を有する関数ｈによって表される。

誤差伝播の理論に基づき、観測値ｚの誤差Δzは下式（１２）のように近似される。

そして、下式（１３）～（１９）のように、その二乗誤差の期待値を解くことにより、観測用の共分散行列Ｐが得られる。

σ_*は、対応する変数の標準偏差である。

手順（１）における観測画像の生成においては、地図画像の生成と同様に、周辺物体情報取得センサ手段１２（ミリ波レーダー）から取得された静的物体が、北東（ＮＥ）座標で観測画像を生成するためにマッピングされる。
なお、観測画像の生成はリアルタイムで行う必要があるため、後処理補正は行わない。また、地図画像の生成では、道路の車線外に存在することを静的物体か否かを判断する条件の一つとしていたが、観測画像を生成する際は、誤差のあるデッドレコニングによる推定位置を用いるため、本来道路外にある静止物体を誤って除去してしまう可能性がある。そこで、例えば動的物体の位置とその大きさを認識できるＬｉＤＡＲによる移動物体の追跡情報をミリ波レーダーによる観測情報と対応付けることで、道路の車線内の観測情報を除去することが好ましい。

図８は生成された観測画像の例を示す図である。
図８（ａ）は観測画像の一例である。観測画像とは、リアルタイムに走行している際に周辺物体情報取得センサ手段１２で観測した静的物体の位置を真上から見たような画像であり、自車両１の移動量に応じてパノラマ画像のように結合することで生成される。観測画像の大きさは２４ｍ×２４ｍであり、観測画像の中央は常にＤＲ部１３で推定した自車両１の位置になるようにしている。観測画像は、観測点が領域全体をカバーするように、観測した複数フレームを使用して生成される。
手順（２）における自車両１周辺の地図画像の抽出では、図８（ｂ）に示すように、観測画像とのマッチングに用いる地図画像が、デッドレコニングにより推定した自車両１の位置に基づき、地図画像記憶部１９に記憶されている地図画像から抽出される。地図画像は、観測画像と同様にＮＥ座標で生成されている。抽出された地図画像の大きさは、３２ｍ×３２ｍの範囲をカバーしている。

手順（３）のマッチング部１５における観測画像と抽出した地図画像とのマップマッチングにおいては、現在位置周辺の相関分布を得るためにテンプレートマッチングが適用される。
マップマッチングでは、特定位置の地図画像と観測画像が類似しているほど自車両１がその位置にいる確率が高いとすることで、オフセット量及び自車両１の位置を推定する。また、テンプレートマッチングとは、テンプレートと呼ばれる画像が探索対象画像内に存在するか否かを調べるアルゴリズムである。本実施例では、テンプレートマッチングのテンプレート画像に該当するのが観測画像であり、抽出した地図画像がテンプレート画像と照合される画像となる。テンプレート画像となる観測画像を地図画像の中で順番に移動させながら、観測画像と抽出した地図画像の類似度を計算する。

下式（２０）のように、テンプレートマッチングのコスト関数として、正規化相互相関（ＺＮＣＣ）Ｒ_t,ref(Δｘ)を使用する。
類似度の計算には、ＳＳＤ（Sum of Squared Difference）やＳＡＤ（Sum of Absolute Difference）等を使用することもできるが、他の手法と比べて画像の明るさの変動に強いという特徴を持つ正規化相互相関（ＺＮＣＣ）を使用することが好ましい。

ここでＲ_t,ref(Δｘ)は、オフセットΔｘ＝［Δｘ，Δｙ］^TでのＺＮＣＣ特徴である。
しかし、ミリ波レーダーによる観測は、観測点が疎らであるため、ＺＮＣＣから高い相関値を得ることが困難である。したがって、各フレームにおける相関分布の結果は信頼性が高くない可能性がある。そこで、下式（２１）のように、確率に対する過度の応答を回避するためにガンマ補正が適用される。

ここで、γは、ガンマ補正用のガンマ値である。Ｒ_t（Δｘ）は、Δｘにおける尤度分布である。

手順（４）において、確率更新部１６でオフセット［Δｘ_d,t，Δｙ_d,t ]^Tの確率分布Ｐ（Δx）は尤度分布を用いて更新される。図９は尤度フィルタリングを示す図である。
確率分布とは、尤度分布と同様に自車両１の位置がどこに存在する可能性が大きいかを確率で示した分布である。
図９の上図に示すように、各フレームで得られる尤度分布は不安定である。明確なピークを示すフレームがいくつかあるが、それらは突然異なる分布を示すことがある。 したがって、累積分布Ｐ_t(Δｘ)は、尤度分布の累積処理によって計算される。累積分布とは、尤度分布と同様に自車両１の位置がどこに存在する可能性が大きいかを示した分布である。下式（２２）、（２３）のように、確率分布Ｐ(Δｘ)は、Ｐ_t(Δｘ)を正規化することにより得られる。これにより、安定した信頼のある分布を得ることができる。

ここで、βは累積過程の逓減係数である（０≦β≦１）。

図１０は複数のピークが発生した場合の処理についての説明図である。
自車両１の進行方向に沿って例えば道路脇に同じパターンの静的物体である柵やガードレール等が連続して存在する場合、図１０（ａ）（ｂ）に示すように複数のピークが発生する可能性がある。複数のピークが発生すると、オフセット量を正確に計算することができないため、正しいピークだけを抽出する必要がある。そこで、図１０（ｃ）に示すようにオフセット更新部１７ではピーク位置を追跡することで誤った位置でのピークを棄却し、下式（２４）、（２５）のように、得られた確率の重み付け平均値を用いてオフセット量を推定する。

ここで、ｇ_posは、オフセットを更新する際のゲインである（０≦ｇ_pos≦１）。
補正後位置導出部１８は、ＤＲ部１３で推定した自車両１の位置をオフセット量で補正することにより自車両１の補正後位置を導出し、導出した補正後位置を自車両１の運転制御装置（図示無し）へ出力する。出力された補正後位置を用いて運転制御装置が自車両１を制御することで、自車両１の自動運転をより安全かつ的確なものとすることができる。

次に、公道で実施した第一の実施例による車両の自己位置推定装置の性能評価試験の結果について説明する。
性能評価試験においては、自己推定位置と実際の位置との間における車両前後方向及び横方向の誤差を計算した。ＧＮＳＳの情報は、自車両１の初期位置の初期化に使用した。ＲＴＫ－ＧＮＳＳの情報は、自己位置推定の精度を評価する基準となる実際の位置を求めるために使用した。また、比較例１として、ＬｉＤＡＲを用いた従来の自己位置推定［非特許文献９：N. Suganuma, D. Yamamoto and K. Yoneda, “Localization for Au- tonomous Driving on Urban Road”, Journal of Advanced Control, Automation and Robotics, Vol. 1, No. 1, pp. 47-53, 2016.］も併せて行い、第一の実施例による自己位置推定の結果と比較した。
第一の実施例による自己位置推定と、比較例１との主な相違点は、周辺物体情報取得センサ部１２としてのセンサ（すなわち、ミリ波レーダーかＬｉＤＡＲか）、及びマッチングに用いる画像（すなわち、ミリ波レーダー画像かＬｉＤＡＲ画像か）である。比較例１では、路面のレーザー反射率のマップマッチングによってオフセット量を推定する。
性能評価試験は、２種類の走行エリアで行った。
一つ目のエリアは、石川県の金沢市内の公道である。このルートの全長は約２．１５ｋｍであり、夏季（積雪無し）と冬季（部分積雪：路側帯や歩道に積雪が有り車道路面の積雪は殆ど無し）に試験を行った。
二つ目のエリアは、北海道の網走市内の公道である。このルートの全長は約２５ｋｍであり、冬季（完全積雪：路側帯や歩道に積雪が有り車道路面も雪で完全に覆われている）に試験を行った。二つ目のエリアでは、路面が雪で覆われており比較例１のＬｉＤＡＲを用いた従来の自己位置推定は使用できないため、第一の実施例による自己位置推定についてのみ試験を行った。

下表１に試験結果を示す。表１では、「積雪無し」、「部分積雪」、及び「完全積雪」のそれぞれの条件下における車両前後方向及び横方向の二乗平均平方根（ＲＭＳ）誤差を示している。

表１から分かるように、第一の実施例も比較例１も、積雪無しの条件下においては妥当な推定位置精度を得ることができる。しかし、部分積雪又は完全積雪の条件下においては、第一の実施例は雪の無い条件下と同程度の精度を有するのに対し、比較例１は精度がかなり悪化した。

図１１は夏季（積雪無し）の一つ目のエリアにおける試験結果を示すグラフであり、図１２は冬季（部分積雪）の一つ目のエリアにおける試験結果を示すグラフである。
図１１及び図１２において、横軸は経過時間［ｓｅｃ］であり、縦軸は、図１１（ａ）及び図１２（ａ）が車両前後方向の誤差［ｍ］、図１１（ｂ）及び図１２（ｂ）が車両横方向の誤差［ｍ］、図１１（ｃ）及び図１２（ｃ）がヨーレート［ｒａｄ／ｓ］である。
図１１から、積雪無しの場合は、交差点や直線道路に関係なく、第一の実施例においても比較例１においても１ｍ以内の誤差に自己位置推定の精度が維持されていることが分かる。
しかし、図１２から分かるように、比較例１においては降雪時には、車両横方向の誤差はすぐに２ｍ以上に増加し、その誤差は解消されなかった。これは、比較例１においては、ＬｉＤＡＲが道路脇の積雪を反射率の高い地域として観測するため、道路脇を車線と混同することにより車両横方向のずれが発生したためである。
一方、第１の実施例においては、環境変化に強く、雪を透過して物体検出が可能なミリ波レーダーを周辺物体情報取得センサ部１２として用いることで、降雪等の悪天候条件の下であっても、積雪無しの条件下と同じ程度の誤差で自車両１の位置を推定することができた。

図１３は冬季（完全積雪）の二つ目のエリアにおける試験結果を示すグラフである。図１３において、横軸は経過時間［ｓｅｃ］であり、縦軸は、図１３（ａ）が車両前後方向の誤差［ｍ］、図１３（ｂ）が車両横方向の誤差［ｍ］、図１３（ｃ）がヨーレート［ｒａｄ／ｓ］である。
図１３により、第一の実施例による自己位置推定は、降雪量に関わらず精度を維持していることが分かる。

以上の試験結果から、第一の実施例による自己位置推定装置１０の有効性が積雪条件下においても確認された。

次に、本発明の第二の実施例による車両の自己位置推定装置について説明する。なお、上記した第一の実施例と同一構成要素については重複説明を省略する。
上記した第一の実施例では、得られた相関を累積し、それらを正規化することによって事後確率を計算した。事後確率の応答性を低下させるためには、対数オッズスケールで値を徐々に更新することがより適切である（対数オッズ：ｌ＝ｌｏｇ（Ｐ／（１－Ｐ）））。そこで本実施例では、得られた相関を対数オッズスケールでの尤度値として、マップマッチングにより得られた相関分布を累積して事後確率を計算する。
これにより、相関を累積して得られた分布を対数オッズスケールの事後確率として扱い、マッチング状態の安定性を推定し、得られた確率の大きさに応じてオフセット量（Δｘ_d,t、Δｙ_d,t）を更新してより安定した自己位置推定を実現することができる。

事後確率ｌ_tの対数オッズ値は、下式（２６）を用いて更新される。

ここで、α₁は確率更新の逓減係数である（０．０≦α≦１．０）。α₁は、経験的に０．９９５と決定した。Ｒ_t（Δｘ）は、自車周辺で離散化した位置決めオフセットの変位量Δｘ＝［Δｘ，Δｙ］^Ｔにおけるマップマッチングの相関分布から得られる尤度分布である。

マップマッチングを適用するために、観測画像と地図画像を用意する。観測画像と地図画像の生成又は抽出方法は、第一の実施例と同様である。
現在位置周辺の相関分布を得るためにテンプレートマッチングが適用される。下式（２７）のように、テンプレートマッチングのコスト関数として、正規化相互相関（ＺＮＣＣ）Ｒ_t,zncc(Δｘ)を使用する。

ここでＲ_t,zncc(Δｘ)は、ＺＮＣＣ特徴である。
しかし、ミリ波レーダーによる観測は観測点が疎らであるため、ＺＮＣＣから高い相関値を得ることが困難である。したがって、各フレームにおける相関分布の結果は信頼性が高くない可能性がある。そこで、下式（２１）のように、確率に対する過度の応答を回避するためにガンマ補正が適用される。

ここで、βは非負のスケール因子である。γは、ガンマ補正用のガンマ値である。これらのパラメータは、β＝ ０．１及びγ＝ ３．０とした。

オフセット量［Δｘ_d,t，Δｙ_d,t ]^Tの確率分布Ｐ（Δｘ）は、尤度分布を用いて更新される。本実施例では、確率分布Ｐ（Δｘ）は、式（２６）を用いて更新したｌ_t（Δｘ）から求めることができる。

最後に、得られた確率の重みづけ平均値を用いてオフセット量を推定する。
上述のように、事後確率を理論的に正しく計算することで、得られた確率の最大値をマッチング状態の不確実性とみなすことができる。したがって、下式（２９）のように不確実性に従って重み付けすることによって、不安定な状況における過度のオフセット更新を防ぐことが可能である。
なお、第一の実施例と同様に式（２４）及び式（２５）を用いてオフセット量を更新することもできる。

次に、公道で実施した第二の実施例による車両の自己位置推定装置の性能評価試験について説明する。
性能評価試験においては、自己推定位置と実際の位置との間における車両前後方向及び横方向の誤差を計算した。ＧＮＳＳの情報は、自車両１の初期位置の初期化に使用した。ＲＴＫ－ＧＮＳＳの情報は、自己位置推定の精度を評価する基準となる実際の位置を求めるために使用した。
性能評価試験の運転データは、北海道の網走市と大空町の公道において、秋季（積雪無し）と冬季（完全積雪：路側帯や歩道に積雪が有り車道路面も雪で完全に覆われている）に記録された。試験を行った経路の全長は、約８．５ｋｍである。試験に用いた自車両１に搭載されている機器は、第一の実施例と同様である。
ミリ波レーダー地図である地図画像は、秋季に取得した運転データを用いて生成して地図画像記憶部１９に記憶させ、自己位置推定の精度は、冬季に取得した運転データについて評価した。そのため、雪の影響により、地図画像と観測画像とでは道路脇のランドマークが異なる場合がある。

第二の実施例による自己位置推定結果を、デッドレコニングによる自己位置推定結果（比較例２）と、ＬｉＤＡＲを用いた従来の推定方法［非特許文献９］による自己位置推定結果（比較例３）と比較した。
なお、第二の実施例による自己位置推定は、式（２４）及び式（２５）を用いる場合（実施例２－１）と、式（２９）を用いる場合（実施例２－２）の二つのパターンで実施し、オフセット値更新時の不確実性を利用するか否かの有効性についても評価した。

試験結果を、下表２及び図１４に示す。
図１４において、横軸は経過時間［ｓｅｃ］であり、縦軸は、図１４（ａ）が車両前後方向の誤差［ｍ］、図１４（ｂ）が車両横方向の誤差［ｍ］、図１４（ｃ）においてはヨーレートとピーク確率を示しており、左縦軸がヨーレート［ｒａｄ／ｓ］、右縦軸がピーク確率である。ピーク確率は、式（２９）の不確実性に対応する。
表２においては、車両前後方向及び横方向についての二乗平均平方根（ＲＭＳ）誤差、標準偏差（Ｓ．Ｄ．）、及び最大誤差の絶対値についての結果を示している。

試験結果から、式（２４）及び式（２５）を用いて自己位置推定を行う実施例２－１の場合は、比較例３と同等の精度を有することが分かる。図１４に示すように、実２－１と比較例３について、２２０秒から２８０秒の間に、メートルレベルの車両横方向誤差が確認された。この区間では、ピーク確率が０．７前後で振動しており、マッチング状態は不安定であると言える。
一方、式（２９）を用いて自己位置推定を行う実施例２－２では、この区間での車両横方向誤差を小さく出来ることが確認された。したがって、式（２９）を用いてオフセット更新に不確実性を導入することで、測位精度の急激な低下を防ぐことができる。また、特に実施例２－２では、すべての評価指標に対して安定した性能を示している。

さらに、本実施例の有効性を検証するために、同じく北海道の網走市と大空町の公道において雪道での自動運転評価を行った。結果、殆どの区間で自動運転が成功していることが確認され、本実施例による自己位置推定装置が安定したパフォーマンスを発揮することが実証され、吹雪等の悪天候の条件下において自車両１の自己位置推定を精度よく行えることが、自動運転評価によって示された。

以上のように、本発明によれば、デッドレコニングにより推定した車両１の位置をマッチングの結果に基づいて補正することで、自己位置推定の精度を向上させることができる。
また、周辺物体情報取得センサ部１２は、雪を透過して物体の観測情報を取得可能であり、マッチング部１５は、テンプレートマッチングを行い、確率更新部１６は、テンプレートマッチングにより得られた相関分布をガンマ補正して尤度分布を導出し、尤度分布を用いて確率分布を更新することで、積雪条件下においても精度よく自己位置を推定することができる。また、テンプレートマッチングを行うことにより、処理の高速化が可能となる。
また、確率更新部１６は、各時刻で得られた尤度分布を積算し、積算した尤度分布を正規化して累積分布を導出し、累積分布に基づいて確率分布を更新することで、相関の寄与度を低くし確率更新できるので、周辺物体情報取得センサ部１２による物体観測が疎らであっても精度よく自己位置を推定することができる。
また、確率更新部１６は、尤度分布を用いて事後確率の対数オッズ値を導出し、対数オッズ値を用いて確率分布を更新することで、相関の寄与度を低くし確率更新できるので、周辺物体情報取得センサ部１２による物体観測が疎らであっても精度よく自己位置を推定することができる。
また、オフセット更新部１７は、確率分布の重み付け平均を用いてオフセット量を更新することで、道路脇に同じパターンの静的物体である柵やガードレール等が連続して存在する場合であっても精度よく自己位置を推定することができる。
また、オフセット更新部１７は、確率分布の最大値をマッチング状態の不確実性とし、不確実性に従った重み付けによりオフセット量を更新することで、不安定な状況における過度のオフセット更新を防ぎ、より精度よく自己位置を推定することができる。

１ 車両（自車両）
１０ 自己位置推定装置
１１ ＧＮＳＳ／ＩＭＵデータ取得部
１２ 周辺物体情報取得センサ部
１３ ＤＲ部
１４ 観測画像生成部
１５ マッチング部
１６ 確率更新部
１７ オフセット更新部
１８ 補正後位置導出部
４０ 慣性計測装置（ＩＭＵ）

Claims (6)

1. 車両の周辺に存在する物体の観測情報に基づきリアルタイムに作成した観測画像と予め作成された地図画像とのマッチングを行うことにより前記車両の位置を推定する自己位置推定装置であって、
   ＧＮＳＳ衛星及び慣性計測装置からのデータを取得するＧＮＳＳ／ＩＭＵデータ取得部と、
   前記車両の周辺に存在する前記物体の前記観測情報を取得する周辺物体情報取得センサ部と、
   前記ＧＮＳＳ／ＩＭＵデータ取得部で取得した前記データに基づいてデッドレコニングにより前記車両の位置を推定するＤＲ部と、
   前記ＤＲ部で推定した前記車両の位置及び前記周辺物体情報取得センサ部で取得した前記観測情報に基づき、前記物体のうち静的物体の位置がマッピングされた前記観測画像を生成する観測画像生成部と、
   前記観測画像と前記地図画像とのマッチングを行うマッチング部と、
   前記マッチングの結果に基づいてオフセットの確率分布を更新する確率更新部と、
   前記確率分布に基づいてオフセット量を更新するオフセット更新部と、
   前記ＤＲ部で推定した前記車両の位置を前記オフセット量で補正することにより前記車両の補正後位置を導出する補正後位置導出部とを備え、
   前記周辺物体情報取得センサ部は、雪を透過して前記物体の前記観測情報を取得可能であり、
   前記マッチング部は、テンプレートマッチングを行い、
   前記確率更新部は、前記テンプレートマッチングにより得られた相関分布をガンマ補正して尤度分布を導出し、前記尤度分布を用いて前記確率分布を更新することを特徴とする車両の自己位置推定装置。
2. 前記確率更新部は、各時刻で得られた前記尤度分布を積算し、積算した前記尤度分布を正規化して累積分布を導出し、前記累積分布に基づいて前記確率分布を更新することを特徴とする請求項１に記載の車両の自己位置推定装置。
3. 前記確率更新部は、前記尤度分布を用いて事後確率の対数オッズ値を導出し、前記対数オッズ値を用いて前記確率分布を更新することを特徴とする請求項１に記載の車両の自己位置推定装置。
4. 前記オフセット更新部は、前記確率分布の重み付け平均を用いて前記オフセット量を更新することを特徴とする請求項２又は請求項３に記載の車両の自己位置推定装置。
5. 前記オフセット更新部は、前記確率分布の最大値をマッチング状態の不確実性とし、前記不確実性に従った重み付けにより前記オフセット量を更新することを特徴とする請求項３に記載の車両の自己位置推定装置。
6. 請求項１から請求項５のいずれか１項に記載の車両の自己位置推定装置を搭載したことを特徴とする車両。

Images