(57)【特許請求の範囲】
【請求項１】
道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）と、
前記測定部の姿勢を決定して、前記地図データ取得部が取得した前記三次元地図データを、前記測定部の姿勢に基づいて補正する地図補正部（Ｓ１０）とを備え、
前記マッチング処理部は、前記地図補正部が補正した後の三次元地図データを前記複数の層に分割することを特徴とする位置推定装置（１６）。
【請求項２】
請求項１において、
前記位置推定装置は移動体に搭載され、
前記移動体の移動量を計算する移動量計算部（Ｓ２）を備え、
前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、
前記位置計算部は、前記マッチング処理部が決定した前記座標変換行列と、前記移動量計算部が計算した前記移動量とに基づいて、前記推定位置を逐次計算することを特徴とする位置推定装置。
【請求項３】
道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定装置であって、
前記位置推定装置は移動体に搭載され、
前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理部は、前記複数の層に対して前記マッチング処理を行い、
前記移動体の移動量を計算する移動量計算部（Ｓ２）と、
前記観測データを前記座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する前記三次元地図データとの平均距離を、前記層別に算出する距離算出部（Ｓ１３３）とをさらに備え、
前記位置計算部は、前記距離算出部が計算した前記平均距離が最も短い層の前記座標変換行列と、前記移動量計算部が計算した前記移動量とに基づいて、前記推定位置を逐次計算することを特徴とする位置推定装置。
【請求項４】
道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定装置であって、
前記位置推定装置は移動体に搭載され、
前記移動体の移動量を計算する移動量計算部（Ｓ２）を備え、
前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理部は、前記複数の層に対して前記マッチング処理を行い、
前記位置計算部は、前記マッチング処理部が前記層別に算出した前記座標変換行列と前記移動量計算部が計算した前記移動量とに基づいて、前記層別に前記推定位置を算出し、層別に算出した推定位置を平均して、最終的な前記推定位置とすることを特徴とする位置推定装置。
【請求項５】
請求項４において、
前記観測データを前記座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する前記三次元地図データとの平均距離を、前記層別に算出する距離算出部（Ｓ１３３）を備え、
前記位置計算部は、前記距離算出部が前記層別に算出した前記平均距離に基づいて定まる重みを用いて前記層別に算出した前記推定位置を加重平均して、最終的な前記推定位置を算出することを特徴とする位置推定装置。
【請求項６】
請求項１〜５のいずれか１項において、
前記三次元地図データは、点群により、道路およびその周囲に存在する物体の三次元位置を示しているデータであり、
前記観測データは、周囲に存在する物体上の点を三次元的に測定したデータであることを特徴とする位置推定装置。
【請求項７】
道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、
周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、
前記測定結果取得ステップで取得した観測データおよび前記地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、
前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）と、
前記測定部の姿勢を決定して、前記地図データ取得ステップで取得した前記三次元地図データを、前記測定部の姿勢に基づいて補正する地図補正ステップ（Ｓ１０）とを備え、
前記マッチング処理ステップは、前記地図補正ステップで補正した後の三次元地図データを前記複数の層に分割することを特徴とする位置推定方法。
【請求項８】
道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、
周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、
前記測定結果取得ステップで取得した観測データおよび前記地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、
前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）とを備えた位置推定方法であって、
前記位置推定方法は移動体で用いられ、
前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理ステップでは、前記複数の層に対して前記マッチング処理を行い、
前記移動体の移動量を計算する移動量計算ステップ（Ｓ２）と、
前記観測データを前記座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する前記三次元地図データとの平均距離を、前記層別に算出する距離算出ステップ（Ｓ１３３）とをさらに備え、
前記位置計算ステップは、前記距離算出ステップで算出した前記平均距離が最も短い層の前記座標変換行列と、前記移動量計算ステップで計算した前記移動量とに基づいて、前記推定位置を逐次計算することを特徴とする位置推定方法。
【請求項９】
道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、
周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、
前記測定結果取得ステップで取得した観測データおよび前記地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、
前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）とを備えた位置推定方法であって、
前記位置推定方法は移動体で用いられ、
前記移動体の移動量を計算する移動量計算ステップ（Ｓ２）を備え、
前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理ステップは、前記複数の層に対して前記マッチング処理を行い、
前記位置計算ステップは、前記マッチング処理ステップで前記層別に算出した前記座標変換行列と前記移動量計算ステップで計算した前記移動量とに基づいて、前記層別に前記推定位置を算出し、層別に算出した推定位置を平均して、最終的な前記推定位置とすることを特徴とする位置推定方法。
【請求項１０】
コンピュータを、
道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）と、
前記測定部の姿勢を決定して、前記地図データ取得部が取得した前記三次元地図データを、前記測定部の姿勢に基づいて補正する地図補正部（Ｓ１０）とを備え、
前記マッチング処理部は、前記地図補正部が補正した後の三次元地図データを前記複数の層に分割するものとして機能させるための位置推定プログラム。
【請求項１１】
コンピュータを、
道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定プログラムとして機能させ、
前記コンピュータは移動体で用いられ、
かつ、前記コンピュータを、
前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理部は、前記複数の層に対して前記マッチング処理を行い、
前記移動体の移動量を計算する移動量計算部（Ｓ２）と、
前記観測データを前記座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する前記三次元地図データとの平均距離を、前記層別に算出する距離算出部（Ｓ１３３）とをさらに備え、
前記位置計算部は、前記距離算出部が計算した前記平均距離が最も短い層の前記座標変換行列と、前記移動量計算部が計算した前記移動量とに基づいて、前記推定位置を逐次計算するものとして機能させるための位置推定プログラム。
【請求項１２】
コンピュータを、
道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定プログラムとして機能させ、
前記コンピュータは移動体で用いられ、
かつ、前記コンピュータを、
前記移動体の移動量を計算する移動量計算部（Ｓ２）を備え、
前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理部は、前記複数の層に対して前記マッチング処理を行い、
前記位置計算部は、前記マッチング処理部が前記層別に算出した前記座標変換行列と前記移動量計算部が計算した前記移動量とに基づいて、前記層別に前記推定位置を算出し、層別に算出した推定位置を平均して、最終的な前記推定位置とするものとして機能させるための位置推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、現在位置を推定する位置推定装置、位置推定方法、位置推定プログラムに関し、特に、高精度に現在位置を推定する技術に関する。
【背景技術】
【０００２】
道路上の白線やレーンマーク、電柱などの特徴物の位置データを有する地図を記憶しておき、車両に搭載したセンサで特徴物を検出し、検出した特徴物の位置を、地図が有する特徴物の位置データとマッチングさせて現在位置を推定する装置が知られている。
【０００３】
しかしながら、市街地の道路、高速道路、地方の道路など、様々な道路において、これらの特徴量が必ずしも存在するとは言えない。そこで、非特許文献１には、高精細な三次元点群（以下、三次元地図データ）を記憶しておき、三次元ＬＩＤＡＲ（Light Detection and Ranging）センサを用いて測定した点群とマッチングさせることで現在位置を推定する方法が提案されている。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】米陀佳祐（K. Yoneda）, テヘラニ  ホセイン（H. Tehrani）, 小川高志（T. Ogawa）, 福山尚久（N. Hukuyama） 、三田誠一（S. Mita）、 “Lidar scan feature for localization with highly precise 3-D map”, Proceedings of 2014 IEEE Intelligent Vehicles Symposium, p.1345-1350 (2014).
【発明の概要】
【発明が解決しようとする課題】
【０００５】
三次元地図データは、測量車を走行させて作成する。測量車が走行したときと、現在位置を推定する車両が走行したときとで、物体の存否や物体の形状が変化していることもある。たとえば、測量車が走行したときには存在していた駐車車両が、現在位置を推定する車両が走行したときには、存在していない場合がその例である。また、測量車が走行したときと、現在位置を推定する車両が走行したときで、街路樹の形状が変化していることもある。そのため、非特許文献１の方法は、点群の誤対応が生じ、それにより現在位置の推定誤差が大きくなることがあった。
【０００６】
また、三次元地図データおよび観測データが、点に加えて、あるいは点に代えて、線や面により物体の三次元位置を示すデータである場合も、点群の場合と同様に、誤対応が生じる恐れがある。
【０００７】
本発明は、この事情に基づいて成されたものであり、その目的とするところは、より精度よく現在位置を推定することができる位置推定装置、位置推定方法、位置推定プログラムを提供することにある。
【課題を解決するための手段】
【０００８】
上記目的は独立請求項に記載の特徴の組み合わせにより達成され、また、下位請求項は、発明の更なる有利な具体例を規定する。特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、本発明の技術的範囲を限定するものではない。
【０００９】
上記目的を達成するための位置推定装置に係る１つの発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体上の点を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備え、測定部の姿勢を決定して、地図データ取得部が取得した三次元地図データを、測定部の姿勢に基づいて補正する地図補正部（Ｓ１０）とを備え、マッチング処理部は、地図補正部が補正した後の三次元地図データを複数の層に分割することを特徴とする位置推定装置である。
上記目的を達成するための位置推定装置に係る別の発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定装置であって、位置推定装置は移動体に搭載され、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理部は、複数の層に対してマッチング処理を行い、移動体の移動量を計算する移動量計算部（Ｓ２）と、観測データを座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する三次元地図データとの平均距離を、層別に算出する距離算出部（Ｓ１３３）とをさらに備え、位置計算部は、距離算出部が計算した平均距離が最も短い層の座標変換行列と、移動量計算部が計算した移動量とに基づいて、推定位置を逐次計算することを特徴とする。
上記目的を達成するための位置推定装置に係るさらに別の発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定装置であって、位置推定装置は移動体に搭載され、移動体の移動量を計算する移動量計算部（Ｓ２）を備え、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理部は、複数の層に対してマッチング処理を行い、位置計算部は、マッチング処理部が層別に算出した座標変換行列と移動量計算部が計算した移動量とに基づいて、層別に推定位置を算出し、層別に算出した推定位置を平均して、最終的な推定位置とすることを特徴とする。
【００１０】
また、上記目的を達成するための位置推定方法に係る１つの発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、測定結果取得ステップで取得した観測データおよび地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）と、測定部の姿勢を決定して、地図データ取得ステップで取得した三次元地図データを、測定部の姿勢に基づいて補正する地図補正ステップ（Ｓ１０）とを備え、マッチング処理ステップは、地図補正ステップで補正した後の三次元地図データを複数の層に分割することを特徴とする位置推定方法である。
上記目的を達成するための位置推定方法に係る別の発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、測定結果取得ステップで取得した観測データおよび地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）とを備えた位置推定方法であって、位置推定方法は移動体で用いられ、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理ステップでは、複数の層に対してマッチング処理を行い、移動体の移動量を計算する移動量計算ステップ（Ｓ２）と、観測データを座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する三次元地図データとの平均距離を、層別に算出する距離算出ステップ（Ｓ１３３）とをさらに備え、位置計算ステップは、距離算出ステップで算出した平均距離が最も短い層の座標変換行列と、移動量計算ステップで計算した移動量とに基づいて、推定位置を逐次計算することを特徴とする。
上記目的を達成するための位置推定方法に係るさらに別の発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、測定結果取得ステップで取得した観測データおよび地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）とを備えた位置推定方法であって、位置推定方法は移動体で用いられ、移動体の移動量を計算する移動量計算ステップ（Ｓ２）を備え、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理ステップは、複数の層に対してマッチング処理を行い、位置計算ステップは、マッチング処理ステップで層別に算出した座標変換行列と移動量計算ステップで計算した移動量とに基づいて、層別に推定位置を算出し、層別に算出した推定位置を平均して、最終的な推定位置とすることを特徴とする。
【００１１】
また、上記目的を達成するための位置推定プログラムに係る１つの発明は、コンピュータを、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）と、測定部の姿勢を決定して、地図データ取得部が取得した三次元地図データを、測定部の姿勢に基づいて補正する地図補正部（Ｓ１０）とを備え、マッチング処理部は、地図補正部が補正した後の三次元地図データを複数の層に分割するものとして機能させるための位置推定プログラムである。
上記目的を達成するための位置推定プログラムに係る別の発明は、コンピュータを、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定プログラムとして機能させ、コンピュータは移動体で用いられ、かつ、コンピュータを、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理部は、複数の層に対してマッチング処理を行い、移動体の移動量を計算する移動量計算部（Ｓ２）と、観測データを座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する三次元地図データとの平均距離を、層別に算出する距離算出部（Ｓ１３３）とをさらに備え、位置計算部は、距離算出部が計算した平均距離が最も短い層の座標変換行列と、移動量計算部が計算した移動量とに基づいて、推定位置を逐次計算するものとして機能させるための位置推定プログラムである。
上記目的を達成するための位置推定プログラムに係るさらに別の発明は、コンピュータを、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定プログラムとして機能させ、コンピュータは移動体で用いられ、かつ、コンピュータを、移動体の移動量を計算する移動量計算部（Ｓ２）を備え、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理部は、複数の層に対してマッチング処理を行い、位置計算部は、マッチング処理部が層別に算出した座標変換行列と移動量計算部が計算した移動量とに基づいて、層別に推定位置を算出し、層別に算出した推定位置を平均して、最終的な推定位置とするものとして機能させるための位置推定プログラムである。
【００１２】
これらの発明によれば、観測データおよび三次元地図データを、それぞれ、高さに基づいて複数層に分割した上で、層別に、観測データと三次元地図データとの対応を決定する。これにより、観測データを、三次元地図データの他の層に含まれるデータと対応させてしまうことを抑制できる。よって、誤対応が減少するので、位置推定精度が向上する。
【図面の簡単な説明】
【００１３】
【図１】実施形態の位置推定システム１０の構成を示すブロック図である。
【図２】環境地図点群の一例を示す図である。
【図３】図２に示した環境地図点群に対応する道路表面地図点群を示す図である。
【図４】図１の位置推定処理部１６が実行する処理を示すフローチャートである。
【図５】図４に続いて位置推定処理部１６が実行する処理を示すフローチャートである。
【図６】図４のステップＳ４で作成する観測地図点群の一例である。
【図７】図４のステップＳ５における分割例である。
【図８】図４のステップＳ１０の詳細処理を示すフローチャートである。
【図９】図８のステップＳ１０２の処理を説明する図である。
【図１０】図５のステップＳ１２の詳細処理を示すフローチャートである。
【図１１】本実施形態の位置推定システム１０の測位誤差を従来の車両位置推定システムと比較して示す図である。
【発明を実施するための形態】
【００１４】
以下、本発明の実施形態を図面に基づいて説明する。図１に示す位置推定システム１０は、車両１に搭載されており、周辺監視センサ１１、１１、無線通信部１２、地図管理部１３、衛星測位部１４、車両状態センサ１５、位置推定処理部１６を備える。
【００１５】
周辺監視センサ１１は、車両１の周辺に存在する物体を検出するセンサであり、請求項の測定部に相当する。図１では、２つの周辺監視センサ１１が車両１に搭載されており、一方の周辺監視センサ１１が車両１の前方を向いており、他方の周辺監視センサ１１が車両１の後方を向いている。
【００１６】
これら２つの周辺監視センサ１１、１１による車両上下方向軸周りの物体検出範囲は車両１の周囲３６０度である。なお、１つのセンサでこの物体検出範囲に存在する物体を検出できる場合には周辺監視センサは１つでもよいし、また、３つ以上のセンサで、この物体検出範囲に存在する物体を検出するようにしてもよい。
【００１７】
周辺監視センサ１１は、以下では、三次元ＬＩＤＡＲセンサとする。ただし、周辺監視センサ１１として、マイクロ波、ミリ波など、光以外の送信波を放射するセンサを用いてもよい。周辺監視センサ１１は、物体検出範囲に存在する物体上の点の座標を逐次検出する。以下では、周辺監視センサ１１が座標を検出した点の集合を観測点群という。観測点群は観測データに相当する。
【００１８】
無線通信部１２は、図示しない基地局を介してサーバ２０との間で無線通信を行い、サーバ２０が備えている地図データベース２１に格納されている地図データのうち、必要な部分をサーバ２０から受信する。地図データは、具体的には、環境地図点群と、道路表面地図点群である。
【００１９】
環境地図点群は、道路やその周辺の物体の位置を多数の点群により表した三次元地図データであり、道路表面地図点群は、環境地図点群のうち、道路表面部分のみを抽出した地図データである。図２に環境地図点群の一例を示し、図３に、図２に示した環境地図点群に対応する道路表面地図点群を示す。なお、これら環境地図点群、道路表面地図点群は、高精度ＧＰＳと複数のレーザスキャナを有する計測車両により予め測定された点群である。
【００２０】
地図管理部１３は、書き込み可能な記憶媒体を備えており、この記憶媒体に、無線通信部１２を介してサーバ２０から取得した環境地図点群と、道路表面地図点群を保存する。
【００２１】
衛星測位部１４は、測位用衛星３０からの電波に基づいて自装置の位置を検出するＧＮＳＳ（Ｇｌｏｂａｌ  Ｎａｖｉｇａｔｉｏｎ  Ｓａｔｅｌｌｉｔｅ  Ｓｙｓｔｅｍ）で用いられるＧＮＳＳ受信機を備えている。このＧＮＳＳ受信機が受信した信号に基づいて、現在位置を検出する。
【００２２】
車両状態センサ１５は、具体的には、車速センサとヨーレートセンサであり、車両状態として車速とヨーレートを逐次検出する。車両状態センサ１５と位置推定処理部１６とは、車内ＬＡＮ１８により接続されている。
【００２３】
位置推定処理部１６は、請求項の位置推定装置に相当しており、車両１の現在位置を逐次推定する。この位置推定処理部１６が推定する現在位置は、衛星測位部１４が検出する現在位置よりも高精度である。以下では、位置推定処理部１６が推定した現在位置を、高精度推定位置という。位置推定処理部１６は、本実施形態では、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータと、このコンピュータが位置推定処理部１６として機能するための位置推定プログラムとを備えた構成である。位置推定プログラムは、ＲＯＭに記憶されていてもよいし、ＲＯＭとは別に、記憶媒体を備え、この記憶媒体に記憶されていてもよい。なお、位置推定処理部１６を、一つあるいは複数のＩＣ等によりハードウェア的に構成してもよい。
【００２４】
位置推定処理部１６は、周辺監視センサ１１が検出した観測点群を示すデータ、地図管理部１３が記憶している地図データ、衛星測位部１４が検出した現在位置、車両状態センサ１５が検出した車速、ヨーレートを用いて、高精度推定位置を逐次決定する。
【００２５】
次に、位置推定処理部１６が実行する処理を詳しく説明する。位置推定処理部１６は、図４に示す処理を周期的に実行する。この図４に示す処理は、位置推定方法に相当する。
【００２６】
ステップＳ１では、初期位置および初期ヨー角θ０を設定する。これら、初期位置および初期ヨー角θ０は、衛星測位部１４から取得する。初期ヨー角θ０は、たとえば、衛星測位部１４が逐次測位した位置の変化から決定される。なお、このステップＳ１は、後述するステップＳ１３を実行して高精度推定位置Ｐｔを更新した後は実行しない。
【００２７】
ステップＳ２では、車速ｖｔ、ヨーレートを車両状態センサ１５から取得し、これら車速、ヨーレートを用いてオドメトリを行い、座標変換行列ΔＭｔを計算する。このステップＳ２の処理は、請求項の移動量計算部に相当する処理である。
【００２８】
座標変換行列ΔＭｔは同次座標変換行列であり、式（１）より計算する。式（１）において、Ｒｚはｚ軸周りの回転行列、Ｔｘはｘ軸方向の並進ベクトルであり、ｔは観測時刻、Δｔは観測周期、Ｉは単位行列である。Ｉ３の場合、３行×３列の単位行列を意味する。また、ｘ軸は車両１の前後方向、ｙ軸は車両１の左右方向、ｚ軸は車両１の上下方向である。
【００２９】
式（１）の右辺で定義される座標変換行列ΔＭｔは、時刻ｔ−１から時刻ｔの間における車両１のオドメトリ移動量だけ、座標系を移動させることを意味する。換言すれば、座標変換行列ΔＭｔは、時刻ｔ−１から時刻ｔの間における車両１のオドメトリ移動量を意味するとも言える。
【数１】
0006354556.tif 000002
【００３０】
ステップＳ３では、周辺監視センサ１１から観測点群の座標データを取得し、取得した座標データをＲＡＭなどの所定の記憶部に記憶する。このステップＳ３の処理は、請求項の測定結果取得部、測定結果取得ステップに相当する処理である。
【００３１】
ステップＳ４では、直近ｍフレームの観測点群を重ね合わせてローカル地図を作成する。ここでは、ｍを１０とする。各フレームで取得した観測点群を重ね合わせる際には、ステップＳ２で逐次計算しているΔＭｔ、ΔＭｔ−１、・・・ΔＭｔ−ｍを用いて、過去に検出した観測点群の座標データを、時刻ｔにおける車両中心座標系（以下、ローカル座標系）に変換する。そして、時刻ｔにおけるローカル座標系に変換後の各フレームの観測点群を重ね合わせる。観測点群を重ねあわせて作成した点群を、以下、観測地図点群という。なお、ローカル座標系の原点は、たとえば、衛星測位部１４が備えるアンテナの位置である。図６は、このステップＳ４で作成した観測地図点群の一例である。
【００３２】
ステップＳ５では、ステップＳ４で作成した観測地図点群を、ローカル座標系のＺ軸（以下、ＺＬ軸）方向に複数層に分割する。ここでは複数層は、図７に示すように３層とし、最下層は０．０≦ＺＬ＜１．０、中間層は１．０≦ＺＬ＜２．０、最上層は２．０≦ＺＬ＜３．０とする。
【００３３】
ステップＳ６では、ローカル座標をグローバル座標に変換する座標変換行列ＭｔＬｔｏＧを計算する。なお、グローバル座標とは、緯度経度を意味しており、またＺ軸の０点は路面である。また、後に用いるピッチ座標は、グローバル座標をＺＬ軸方向およびＹＬ軸周りに補正した座標であり、アース座標は、ピッチ座標をさらにマッチング処理した後の座標である。本明細書において、添字Ｌはローカル座標であることを表し、添字Ｇはグローバル座標であることを表し、添字Ｐはピッチ座標であることを表し、添字Ｅはアース座標であることを表す。
【００３４】
座標変換行列ＭｔＬｔｏＧは式（２）より計算する。式（２）において、Ｍｔ−１ＰｔｏＥはピッチ座標をアース座標へ変換する変換行列である。また、Ｍｔ−１ＧｔｏＰは、グローバル座標を、ピッチ座標、すなわち、ＺＬ軸方向およびピッチ角補正した座標へ変換する座標変換行列である。
【００３５】
座標変換行列ＭｔＧｔｏＰの前回値Ｍｔ−１ＧｔｏＰは、後述するステップＳ１０で計算している。また、座標変換行列ＭｔＰｔｏＥの前回値Ｍｔ−１ＰｔｏＥは、後述するステップＳ１２で計算している。なお、座標変換行列ＭｔＬｔｏＧの初期値Ｍ０ＬｔｏＧは式（３）で表される。そして、詳しくは後述するが、座標変換行列ＭｔＧｔｏＰは座標変換行列ＭｔＬｔｏＧから計算でき、座標変換行列ＭｔＰｔｏＥは座標変換行列ＭｔＧｔｏＰから計算できる。したがって、これら座標変換行列ＭｔＧｔｏＰ、ＭｔＰｔｏＥの初期値は、座標変換行列ＭｔＬｔｏＧの初期値から計算できる。
【数２】
0006354556.tif 000003
【数３】
0006354556.tif 000004
【００３６】
式（２）の意味を説明する。ΔＭｔ−１は、時刻ｔ−１における車両１の移動量をローカル座標系で表している。このΔＭｔ−１にＭｔ−１ＬｔｏＧを乗じると、時刻ｔ−１における車両１の移動量が、グローバル座標で表される。さらに、Ｍｔ−１ＧｔｏＰを乗じると、グローバル座標からピッチ座標系に変換され、さらに、Ｍｔ−１ＧｔｏＰを乗じると、ピッチ座標からアース座標に変換される。すなわち、（２）式は、ｔ時刻において、その１フレーム前の時刻ｔ−１における車両１のローカル座標系での移動量をアース座標で表していることになる。
【００３７】
ここで、ローカル座標系での移動量は、移動後のローカル座標を意味する。したがって、ローカル座標系での移動量をアース座標に変換することは、移動後の座標をアース座標に変換することを意味する。そして、アース座標は、車両位置を高い精度で表したグローバル座標を意味する。そこで、式２の右辺の計算を、時刻ｔにおいて、ローカル座標をグローバル座標に変換する座標変換行列ＭｔＬｔｏＧとするのである。
【００３８】
ステップＳ７では、時刻ｔにおける車両１のグローバル位置、すなわち、車両１の位置を表すグローバル座標を計算する。この計算は、ステップＳ６で計算した座標変換行列ＭｔＬｔｏＧと、ステップＳ２で計算したΔＭｔを用い、下記式（４）より計算する。
【数４】
0006354556.tif 000005
【００３９】
前述したように、ΔＭｔは時刻ｔにおける車両のローカル座標を表しているので、式（４）より、時刻ｔにおける車両１のグローバル位置が計算できる。
【００４０】
ステップＳ８では、地図管理部１３に記憶されている環境地図点群から、ステップＳ７で計算したグローバル位置を基準として用いて、車両１の周辺の点群を抽出する。なお、環境地図点群は、グローバル座標で表されている。ステップＳ８の処理は、請求項の地図データ取得部、地図データ取得ステップに相当する処理である。
【００４１】
ステップＳ９では、ステップＳ８で抽出した点群を、ＭｔＬｔｏＧの逆行列を用いてローカル座標に変換する。
【００４２】
ステップＳ１０では、道路表面に対する車両１の高度偏差Δｚおよびピッチ角偏差Δφを補正する補正行列ＭｔΔＺと、グローバル座標をピッチ座標に変換する座標変換行列ＭｔＧｔｏＰを計算する。ステップＳ１０の処理は、請求項の地図補正部に相当する処理である。ステップＳ１０の詳細は、図８に示す。
【００４３】
まず、ステップＳ１０１では、地図管理部１３に記憶している道路表面地図点群から、ステップＳ７で計算したグローバル位置を基準として用いて、車両１の周辺の半径ｒ（ｍ）の点群を抽出する。ｒ（ｍ）はたとえば１０（ｍ）である。環境地図点群と同様、道路表面地図点群もグローバル座標で表されている。
【００４４】
ステップＳ１０２では、ステップＳ１０１で抽出した点群を、ＭｔＬｔｏＧの逆行列（ＭｔＬｔｏＧ）−１を用いてローカル座標に変換した後、ＸＬ−ＺＬ平面上に投影する。これにより、図９に示すように、車両１の真横から道路表面地図点群を見た状態となる。なお、ステップＳ１０２において、ステップＳ９でローカル座標に変換した環境地図点群から、道路表面部分を抽出し、この抽出した道路表面部分をＸＬ−ＺＬ平面上に投影してもよい。
【００４５】
ステップＳ１０３では、ロバスト推定において広く用いられているＲＡＮＳＡＣ（ＲＡＮｄｏｍ  ＳＡｍｐｌｅ  Ｃｏｎｓｅｎｓｕｓ）により、ステップＳ１０２でＸＬ−ＺＬ平面上に投影した道路表面地図点群に相当する直線を算出する。これにより、図９に示す、現在位置を基準とした地面との高度偏差Δｚ、および、この地面と車両１のピッチ角偏差Δφが得られる。周辺監視センサ１１は車両１に搭載されているので、ピッチ角偏差Δφは周辺監視センサ１１の姿勢の一要素を表しているとも言える。
【００４６】
ステップＳ１０４では、これら高度偏差Δｚ、ピッチ角偏差Δφを補正する、式（５）に示す補正行列ＭｔΔＺを作成する。この式（５）において、Ｒｙ（）はＹ軸周りの回転行列、Ｔｚ（）はＺ軸方向の並進ベクトルである。
【数５】
0006354556.tif 000006
【００４７】
ステップＳ１０５では、ステップＳ１０４で作成した補正行列ＭｔΔＺを用い、グローバル座標をピッチ座標に変換する変換行列ＭｔＧｔｏＰを計算する。この変換行列ＭｔＧｔｏＰは式（６）により計算する。
【数６】
0006354556.tif 000007
【００４８】
式６は、グローバル座標を一旦、ローカル座標に変換し、補正行列ＭｔΔＺにより、高度偏差Δｚ、ピッチ角偏差Δφを補正した後に、グローバル座標に変換することを意味する。
【００４９】
ステップＳ１０５を終了したら、ステップＳ１０を終了したことになる。ステップＳ１０を終了したら、図５に示すステップＳ１１を実行する。
【００５０】
ステップＳ１１では、ステップＳ８で抽出した環境地図点群を、ローカル座標系に変換する。この変換は、環境地図点群を構成する各点に、式（６）の右辺の左２項の逆行列、すなわち、（ＭｔＬｔｏＧ  ＭｔΔｚ）−１を乗じることで行う。この変換を行うことにより、ステップＳ８で抽出した環境地図点群を、車両１の高度偏差Δｚ、ピッチ角偏差Δφを補正した座標系にするのである。
【００５１】
ステップＳ１２では、ステップＳ１１でローカル座標系に変換した環境地図点群を、図７と同様の３層に分割する。そして、分割した環境地図点群およびステップＳ５で分割した観測地図点群を、高さが同じ層同士、層ごとにマッチング処理を行う。このマッチング処理は、Iterative Closest Pointアルゴリズム（以下、ＩＣＰアルゴリズム）を用いて行う。このステップＳ１２およびステップＳ５が請求項のマッチング処理部に相当する。
【００５２】
ステップＳ１２の詳細処理は図１０に示す。なお、図１０の説明において、観測地図点群を構成する点群のうちｉ層を構成する点群をＡ＝｛ａｊ｝（｜Ａ｜＝ＮＡ）、ステップＳ１１で分割した環境地図点群のうち観測地図点群のｉ層と同じ高さの層の点群をＢ＝｛ｂｊ｝（｜Ｂ｜＝ＮＢ）とする。
【００５３】
まず、ステップＳ１２１において、座標変換行列ＭをＩとする。なお、座標変換行列Ｍは４行４列であるので、Ｉは具体的にはＩ４である。ステップＳ１２２では、ｊ＝１とする。ステップＳ１２３では、ＭｊにＭを代入する。ステップＳ１２４では、ｊがＮＡになったか否かを判断する。ステップＳ１２４の判断がＮＯであればステップＳ１２５に進む。
【００５４】
ステップＳ１２５では、環境地図点群ＢからＭｊａｊに対応する対応点ｍｊを決定する。この対応点の決定には、ユークリッド距離に基づく最近傍探索を行う。
【００５５】
ステップＳ１２６では、対応点ｍｊと入力点Ｍｊａｊとの距離が、ｄｍａｘ以下であるか否かを判断する。この判断がＹＥＳであればステップＳ１２７に進み、ｗｊを１とし、ＮＯであればステップＳ１２８に進み、ｗｊを０とする。
【００５６】
ステップＳ１２９ではｊに１を加える。ステップＳ１２９を実行後はステップＳ１２４に戻る。ステップＳ１２４の判断がＹＥＳになった場合にはステップＳ１３０に進む。ステップＳ１３０では、ｗｊ＝１の対応点と入力点Ｍｊａｊとの距離を最小化する変換行列を、最小二乗法を用いて計算する。
【００５７】
続くステップＳ１３１では、終了条件を満たすか否かを判断する。終了条件は、最大繰り返し回数に到達したこと、あるいは、この処理を繰り返し実行したことによる入力点Ｍｊａｊが収束したことのいずれかが成立したことである。終了条件を満たしていない場合には、ステップＳ１２２に戻る。ステップＳ１２２に戻ると、直近のステップＳ１３０で決定した座標変換行列Ｍで、入力点Ｍｊａｊに対応する対応点ｍｊの探索が行われる。
【００５８】
ステップＳ１３１の判断がＹＥＳであればステップＳ１３２に進む。ステップＳ１３２では、最新のＭをＭｔｉとする。
【００５９】
続くステップＳ１３３では、観測地図点群を構成する点群１点あたりの対応点への平均距離ｅｔｉを式（７）から算出する。このステップＳ１３３の処理は、請求項の距離算出部に相当する処理である。
【数７】
0006354556.tif 000008
【００６０】
この図１０に示す処理を観測地図点群を構成する各層について行う。したがって、各層ｉに対して、グローバル座標系において、観測地図点群を構成する点群を環境地図点群にマッチングさせる座標変換行列Ｍｔｉと、観測地図点群を構成する点群１点あたりの対応点への平均距離ｅｔｉが得られる。
【００６１】
次に、ステップＳ１３へ進む。ステップＳ１３では、時刻ｔにおいてピッチ座標をアース座標に変換する座標変換行列ＭｔＰｔｏＥを計算する。この座標変換行列ＭｔＰｔｏＥは、ステップＳ１２で得た座標変換行列Ｍｔｉに基づいて計算する。本実施形態では、３層それぞれについて計算した座標変換行列Ｍｔｉのうち、平均距離ｅｔｉが最も小さい座標変換行列ＭｔｉをＭｔｉｃｐとし、下記式（８）により、ＭｔＰｔｏＥを算出する。
【数８】
0006354556.tif 000009
【００６２】
この式（８）は、ピッチ座標を、一旦、グローバル座標に変換し、ＩＣＰアルゴリズムによるマッチング処理をした後に、ピッチ座標に戻すことを意味する。アース座標は、ピッチ座標をＩＣＰアルゴリズムによるマッチング処理した座標であるが、ＩＣＰ処理はグローバル座標で行うので、ピッチ座標を、一旦、グローバル座標に変換するのである。
【００６３】
ステップＳ１４３では、式（９）により、高精度推定位置Ｐｔを更新する。このステップＳ１４の処理は、請求項の位置計算部に相当する処理である。
【数９】
0006354556.tif 000010
【００６４】
式（９）において、ΔＭｔはステップＳ２で計算しており、ＭｔＬｔｏＧはステップＳ６で計算しており、ＭｔＧｔｏＰはステップＳ１０で計算しており、ＭｔＰｔｏＥはステップＳ１３で計算している。この式（９）は、ローカル移動量を表す座標変換行列ΔＭｔを、ＭｔＬｔｏＧによりローカル座標からグローバル座標に変換し、ＭｔＧｔｏＰによりグローバル座標からピッチ座標に変換し、ＭｔＰｔｏＥによりピッチ座標からアース座標に変換することを意味する。また、ローカル移動量は、ローカル座標系における移動後の車両位置を示している。よって、式（９）は、ローカル座標系における車両座標を、アース座標に変換することを意味している。
【００６５】
（測位結果）
図１１は、本実施形態の位置推定システム１０が決定した高精度推定位置の誤差を、従来の車両位置推定システムによる推定位置の誤差と比較して示す図である。誤差を算出するための正解位置には、ＲＴＫ−ＧＰＳにより測位を行う高精度ＧＰＳの測位値を用いた。また、ユークリッド距離は、正解位置と高精度推定位置、あるいは、従来システムによる推定位置との距離を意味する。
【００６６】
図１１から分かるように、本実施形態の位置推定システム１０が決定した高精度推定位置は、従来の推定位置よりも高い精度で位置を推定できていることが分かる。
【００６７】
（実施形態の効果）
以上、説明したように、本実施形態の位置推定システム１０は、この位置推定システム１０を搭載した車両１の周辺の点群を観測し、観測点群から作成した観測地図点群と、車両１の周辺の環境地図点群とをマッチングさせるマッチング処理を行う。
【００６８】
このマッチング処理は、観測地図点群を高さ方向に複数層に分割して、層ごとに行う。これにより、観測地図点群を構成する点が、環境地図点群を構成する点群のうち、層が異なる点群に含まれる点と対応してしまうことを抑制できる。したがって、観測地図点群を構成する点と、環境地図点群を構成する点との誤対応が減少するので、図１１に示したように、高精度推定位置は、真の現在位置を高い精度で推定できる。
【００６９】
以上、本発明の実施形態を説明したが、本発明は上述の実施形態に限定されるものではなく、次の変形例も本発明の技術的範囲に含まれ、さらに、下記以外にも要旨を逸脱しない範囲内で種々変更して実施できる。
【００７０】
＜変形例１、２＞
たとえば、前述の実施形態では、３層それぞれについて計算した座標変換行列Ｍｔｉのうち、平均距離ｅｔｉが最も小さい座標変換行列ＭｔｉをＭｔｉｃｐとして、高精度推定位置Ｐｔを計算していた（Ｓ１３、Ｓ１４）。
【００７１】
しかしこれに限られず、３層それぞれについて計算した座標変換行列Ｍｔｉを、それぞれＭｔｉｃｐとして用いて式（８）、式（９）を計算し、３つの推定位置を算出し、その３つの推定位置に基づいて、最終的な高精度推定位置Ｐｔを算出してもよい。
【００７２】
これら３つの推定位置から最終的な高精度推定位置Ｐｔを算出する方法は、たとえば、それら３つの推定位置の単純平均である（変形例１）。また、加重平均でもよい（変形例２）。加重平均の際の重みは、たとえば、平均距離ｅｔｉの逆数とする。
【００７３】
＜変形例３＞
前述の実施形態では、道路表面地図点群をローカル座標系のＸＬ−ＺＬ平面に投影することで、ピッチ角偏差Δφ、高度偏差Δｚを算出していた。しかし、これらピッチ角偏差Δφ、高度偏差Δｚを、ピッチ角、高度を検出するセンサの検出値に基づいて計算してもよい。
【００７４】
＜変形例４＞
前述の実施形態では、地図管理部１３に道路表面地図点群を記憶していた。しかし、道路表面地図点群は環境地図点群から高さ方向の点群を除去したものであるので、道路表面地図点群を記憶せず、環境地図点群から、都度、道路表面地図点群を作成してもよい。
【００７５】
＜変形例５−８＞
周辺監視センサ１１として、カメラを用いてもよい（変形例５）。位置推定システムを車両以外の移動体で搭載してもよい（変形例６）。観測地図点群を分割する層数は３以外の複数でもよい（変形例７）。観測地図点群を分割した各層の高さ方向長さが、互いに異なっていてもよい（変形例８）。
【００７６】
＜変形例９＞
前述の実施形態では、三次元地図データは、道路およびその周囲に存在する物体の三次元位置を、点群により示しているデータであった。また、観測データとして観測点群を取得していた。
【００７７】
しかし、三次元地図データ、観測データが点群でなくても、本発明は実施できる。したがって、三次元地図データとして、点に加えて、あるいは点に代えて、線や面により物体の三次元位置を示すデータを取得し、観測点群に代えて、周囲に存在する物体の形状を、点に加えて、あるいは点に代えて、線や面により物体の三次元位置を示す観測データを取得してもよい。
【符号の説明】
【００７８】
１：車両、      １０：位置推定システム、      １１：周辺監視センサ、      １２：無線通信部、      １３：地図管理部、      １４：衛星測位部、      １５：車両状態センサ、      １６：位置推定処理部、      １８：車内ＬＡＮ、      ２０：サーバ、      ２１：地図データベース、      ３０：測位用衛星
【図１】
0006354556.tif 000011
【図２】
0006354556.tif 000012
【図３】
0006354556.tif 000013
【図４】
0006354556.tif 000014
【図５】
0006354556.tif 000015
【図６】
0006354556.tif 000016
【図７】
0006354556.tif 000017
【図８】
0006354556.tif 000018
【図９】
0006354556.tif 000019
【図１０】
0006354556.tif 000020
【図１１】
0006354556.tif 000021