JP6354556B2 - 位置推定装置、位置推定方法、位置推定プログラム - Google Patents

Description

本発明は、現在位置を推定する位置推定装置、位置推定方法、位置推定プログラムに関し、特に、高精度に現在位置を推定する技術に関する。

道路上の白線やレーンマーク、電柱などの特徴物の位置データを有する地図を記憶しておき、車両に搭載したセンサで特徴物を検出し、検出した特徴物の位置を、地図が有する特徴物の位置データとマッチングさせて現在位置を推定する装置が知られている。

しかしながら、市街地の道路、高速道路、地方の道路など、様々な道路において、これらの特徴量が必ずしも存在するとは言えない。そこで、非特許文献１には、高精細な三次元点群（以下、三次元地図データ）を記憶しておき、三次元ＬＩＤＡＲ（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).

三次元地図データは、測量車を走行させて作成する。測量車が走行したときと、現在位置を推定する車両が走行したときとで、物体の存否や物体の形状が変化していることもある。たとえば、測量車が走行したときには存在していた駐車車両が、現在位置を推定する車両が走行したときには、存在していない場合がその例である。また、測量車が走行したときと、現在位置を推定する車両が走行したときで、街路樹の形状が変化していることもある。そのため、非特許文献１の方法は、点群の誤対応が生じ、それにより現在位置の推定誤差が大きくなることがあった。

また、三次元地図データおよび観測データが、点に加えて、あるいは点に代えて、線や面により物体の三次元位置を示すデータである場合も、点群の場合と同様に、誤対応が生じる恐れがある。

本発明は、この事情に基づいて成されたものであり、その目的とするところは、より精度よく現在位置を推定することができる位置推定装置、位置推定方法、位置推定プログラムを提供することにある。

上記目的は独立請求項に記載の特徴の組み合わせにより達成され、また、下位請求項は、発明の更なる有利な具体例を規定する。特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、本発明の技術的範囲を限定するものではない。

上記目的を達成するための位置推定装置に係る１つの発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体上の点を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備え、測定部の姿勢を決定して、地図データ取得部が取得した三次元地図データを、測定部の姿勢に基づいて補正する地図補正部（Ｓ１０）とを備え、マッチング処理部は、地図補正部が補正した後の三次元地図データを複数の層に分割することを特徴とする位置推定装置である。
上記目的を達成するための位置推定装置に係る別の発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定装置であって、位置推定装置は移動体に搭載され、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理部は、複数の層に対してマッチング処理を行い、移動体の移動量を計算する移動量計算部（Ｓ２）と、観測データを座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する三次元地図データとの平均距離を、層別に算出する距離算出部（Ｓ１３３）とをさらに備え、位置計算部は、距離算出部が計算した平均距離が最も短い層の座標変換行列と、移動量計算部が計算した移動量とに基づいて、推定位置を逐次計算することを特徴とする。
上記目的を達成するための位置推定装置に係るさらに別の発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定装置であって、位置推定装置は移動体に搭載され、移動体の移動量を計算する移動量計算部（Ｓ２）を備え、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理部は、複数の層に対してマッチング処理を行い、位置計算部は、マッチング処理部が層別に算出した座標変換行列と移動量計算部が計算した移動量とに基づいて、層別に推定位置を算出し、層別に算出した推定位置を平均して、最終的な推定位置とすることを特徴とする。

また、上記目的を達成するための位置推定方法に係る１つの発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、測定結果取得ステップで取得した観測データおよび地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）と、測定部の姿勢を決定して、地図データ取得ステップで取得した三次元地図データを、測定部の姿勢に基づいて補正する地図補正ステップ（Ｓ１０）とを備え、マッチング処理ステップは、地図補正ステップで補正した後の三次元地図データを複数の層に分割することを特徴とする位置推定方法である。
上記目的を達成するための位置推定方法に係る別の発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、測定結果取得ステップで取得した観測データおよび地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）とを備えた位置推定方法であって、位置推定方法は移動体で用いられ、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理ステップでは、複数の層に対してマッチング処理を行い、移動体の移動量を計算する移動量計算ステップ（Ｓ２）と、観測データを座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する三次元地図データとの平均距離を、層別に算出する距離算出ステップ（Ｓ１３３）とをさらに備え、位置計算ステップは、距離算出ステップで算出した平均距離が最も短い層の座標変換行列と、移動量計算ステップで計算した移動量とに基づいて、推定位置を逐次計算することを特徴とする。
上記目的を達成するための位置推定方法に係るさらに別の発明は、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、測定結果取得ステップで取得した観測データおよび地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）とを備えた位置推定方法であって、位置推定方法は移動体で用いられ、移動体の移動量を計算する移動量計算ステップ（Ｓ２）を備え、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理ステップは、複数の層に対してマッチング処理を行い、位置計算ステップは、マッチング処理ステップで層別に算出した座標変換行列と移動量計算ステップで計算した移動量とに基づいて、層別に推定位置を算出し、層別に算出した推定位置を平均して、最終的な推定位置とすることを特徴とする。

また、上記目的を達成するための位置推定プログラムに係る１つの発明は、コンピュータを、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）と、測定部の姿勢を決定して、地図データ取得部が取得した三次元地図データを、測定部の姿勢に基づいて補正する地図補正部（Ｓ１０）とを備え、マッチング処理部は、地図補正部が補正した後の三次元地図データを複数の層に分割するものとして機能させるための位置推定プログラムである。
上記目的を達成するための位置推定プログラムに係る別の発明は、コンピュータを、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定プログラムとして機能させ、コンピュータは移動体で用いられ、かつ、コンピュータを、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理部は、複数の層に対してマッチング処理を行い、移動体の移動量を計算する移動量計算部（Ｓ２）と、観測データを座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する三次元地図データとの平均距離を、層別に算出する距離算出部（Ｓ１３３）とをさらに備え、位置計算部は、距離算出部が計算した平均距離が最も短い層の座標変換行列と、移動量計算部が計算した移動量とに基づいて、推定位置を逐次計算するものとして機能させるための位置推定プログラムである。
上記目的を達成するための位置推定プログラムに係るさらに別の発明は、コンピュータを、道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、測定結果取得部が取得した観測データおよび地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、観測データと、三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定プログラムとして機能させ、コンピュータは移動体で用いられ、かつ、コンピュータを、移動体の移動量を計算する移動量計算部（Ｓ２）を備え、マッチング処理は、観測データを三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、マッチング処理部は、複数の層に対してマッチング処理を行い、位置計算部は、マッチング処理部が層別に算出した座標変換行列と移動量計算部が計算した移動量とに基づいて、層別に推定位置を算出し、層別に算出した推定位置を平均して、最終的な推定位置とするものとして機能させるための位置推定プログラムである。

これらの発明によれば、観測データおよび三次元地図データを、それぞれ、高さに基づいて複数層に分割した上で、層別に、観測データと三次元地図データとの対応を決定する。これにより、観測データを、三次元地図データの他の層に含まれるデータと対応させてしまうことを抑制できる。よって、誤対応が減少するので、位置推定精度が向上する。

実施形態の位置推定システム１０の構成を示すブロック図である。 環境地図点群の一例を示す図である。 図２に示した環境地図点群に対応する道路表面地図点群を示す図である。 図１の位置推定処理部１６が実行する処理を示すフローチャートである。 図４に続いて位置推定処理部１６が実行する処理を示すフローチャートである。 図４のステップＳ４で作成する観測地図点群の一例である。 図４のステップＳ５における分割例である。 図４のステップＳ１０の詳細処理を示すフローチャートである。 図８のステップＳ１０２の処理を説明する図である。 図５のステップＳ１２の詳細処理を示すフローチャートである。 本実施形態の位置推定システム１０の測位誤差を従来の車両位置推定システムと比較して示す図である。

以下、本発明の実施形態を図面に基づいて説明する。図１に示す位置推定システム１０は、車両１に搭載されており、周辺監視センサ１１、１１、無線通信部１２、地図管理部１３、衛星測位部１４、車両状態センサ１５、位置推定処理部１６を備える。

周辺監視センサ１１は、車両１の周辺に存在する物体を検出するセンサであり、請求項の測定部に相当する。図１では、２つの周辺監視センサ１１が車両１に搭載されており、一方の周辺監視センサ１１が車両１の前方を向いており、他方の周辺監視センサ１１が車両１の後方を向いている。

これら２つの周辺監視センサ１１、１１による車両上下方向軸周りの物体検出範囲は車両１の周囲３６０度である。なお、１つのセンサでこの物体検出範囲に存在する物体を検出できる場合には周辺監視センサは１つでもよいし、また、３つ以上のセンサで、この物体検出範囲に存在する物体を検出するようにしてもよい。

周辺監視センサ１１は、以下では、三次元ＬＩＤＡＲセンサとする。ただし、周辺監視センサ１１として、マイクロ波、ミリ波など、光以外の送信波を放射するセンサを用いてもよい。周辺監視センサ１１は、物体検出範囲に存在する物体上の点の座標を逐次検出する。以下では、周辺監視センサ１１が座標を検出した点の集合を観測点群という。観測点群は観測データに相当する。

無線通信部１２は、図示しない基地局を介してサーバ２０との間で無線通信を行い、サーバ２０が備えている地図データベース２１に格納されている地図データのうち、必要な部分をサーバ２０から受信する。地図データは、具体的には、環境地図点群と、道路表面地図点群である。

環境地図点群は、道路やその周辺の物体の位置を多数の点群により表した三次元地図データであり、道路表面地図点群は、環境地図点群のうち、道路表面部分のみを抽出した地図データである。図２に環境地図点群の一例を示し、図３に、図２に示した環境地図点群に対応する道路表面地図点群を示す。なお、これら環境地図点群、道路表面地図点群は、高精度ＧＰＳと複数のレーザスキャナを有する計測車両により予め測定された点群である。

地図管理部１３は、書き込み可能な記憶媒体を備えており、この記憶媒体に、無線通信部１２を介してサーバ２０から取得した環境地図点群と、道路表面地図点群を保存する。

衛星測位部１４は、測位用衛星３０からの電波に基づいて自装置の位置を検出するＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）で用いられるＧＮＳＳ受信機を備えている。このＧＮＳＳ受信機が受信した信号に基づいて、現在位置を検出する。

車両状態センサ１５は、具体的には、車速センサとヨーレートセンサであり、車両状態として車速とヨーレートを逐次検出する。車両状態センサ１５と位置推定処理部１６とは、車内ＬＡＮ１８により接続されている。

位置推定処理部１６は、請求項の位置推定装置に相当しており、車両１の現在位置を逐次推定する。この位置推定処理部１６が推定する現在位置は、衛星測位部１４が検出する現在位置よりも高精度である。以下では、位置推定処理部１６が推定した現在位置を、高精度推定位置という。位置推定処理部１６は、本実施形態では、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータと、このコンピュータが位置推定処理部１６として機能するための位置推定プログラムとを備えた構成である。位置推定プログラムは、ＲＯＭに記憶されていてもよいし、ＲＯＭとは別に、記憶媒体を備え、この記憶媒体に記憶されていてもよい。なお、位置推定処理部１６を、一つあるいは複数のＩＣ等によりハードウェア的に構成してもよい。

位置推定処理部１６は、周辺監視センサ１１が検出した観測点群を示すデータ、地図管理部１３が記憶している地図データ、衛星測位部１４が検出した現在位置、車両状態センサ１５が検出した車速、ヨーレートを用いて、高精度推定位置を逐次決定する。

次に、位置推定処理部１６が実行する処理を詳しく説明する。位置推定処理部１６は、図４に示す処理を周期的に実行する。この図４に示す処理は、位置推定方法に相当する。

ステップＳ１では、初期位置および初期ヨー角θ_０を設定する。これら、初期位置および初期ヨー角θ_０は、衛星測位部１４から取得する。初期ヨー角θ_０は、たとえば、衛星測位部１４が逐次測位した位置の変化から決定される。なお、このステップＳ１は、後述するステップＳ１３を実行して高精度推定位置Ｐ_ｔを更新した後は実行しない。

ステップＳ２では、車速ｖ_ｔ、ヨーレートを車両状態センサ１５から取得し、これら車速、ヨーレートを用いてオドメトリを行い、座標変換行列ΔＭ_ｔを計算する。このステップＳ２の処理は、請求項の移動量計算部に相当する処理である。

座標変換行列ΔＭ_ｔは同次座標変換行列であり、式（１）より計算する。式（１）において、Ｒ_ｚはｚ軸周りの回転行列、Ｔ_ｘはｘ軸方向の並進ベクトルであり、ｔは観測時刻、Δｔは観測周期、Ｉは単位行列である。Ｉ_３の場合、３行×３列の単位行列を意味する。また、ｘ軸は車両１の前後方向、ｙ軸は車両１の左右方向、ｚ軸は車両１の上下方向である。

式（１）の右辺で定義される座標変換行列ΔＭ_ｔは、時刻ｔ−１から時刻ｔの間における車両１のオドメトリ移動量だけ、座標系を移動させることを意味する。換言すれば、座標変換行列ΔＭ_ｔは、時刻ｔ−１から時刻ｔの間における車両１のオドメトリ移動量を意味するとも言える。

ステップＳ３では、周辺監視センサ１１から観測点群の座標データを取得し、取得した座標データをＲＡＭなどの所定の記憶部に記憶する。このステップＳ３の処理は、請求項の測定結果取得部、測定結果取得ステップに相当する処理である。

ステップＳ４では、直近ｍフレームの観測点群を重ね合わせてローカル地図を作成する。ここでは、ｍを１０とする。各フレームで取得した観測点群を重ね合わせる際には、ステップＳ２で逐次計算しているΔＭ_ｔ、ΔＭ_ｔ−１、・・・ΔＭ_ｔ−ｍを用いて、過去に検出した観測点群の座標データを、時刻ｔにおける車両中心座標系（以下、ローカル座標系）に変換する。そして、時刻ｔにおけるローカル座標系に変換後の各フレームの観測点群を重ね合わせる。観測点群を重ねあわせて作成した点群を、以下、観測地図点群という。なお、ローカル座標系の原点は、たとえば、衛星測位部１４が備えるアンテナの位置である。図６は、このステップＳ４で作成した観測地図点群の一例である。

ステップＳ５では、ステップＳ４で作成した観測地図点群を、ローカル座標系のＺ軸（以下、Ｚ_Ｌ軸）方向に複数層に分割する。ここでは複数層は、図７に示すように３層とし、最下層は０．０≦Ｚ_Ｌ＜１．０、中間層は１．０≦Ｚ_Ｌ＜２．０、最上層は２．０≦Ｚ_Ｌ＜３．０とする。

ステップＳ６では、ローカル座標をグローバル座標に変換する座標変換行列Ｍ_ｔ ^ＬｔｏＧを計算する。なお、グローバル座標とは、緯度経度を意味しており、またＺ軸の０点は路面である。また、後に用いるピッチ座標は、グローバル座標をＺ_Ｌ軸方向およびＹ_Ｌ軸周りに補正した座標であり、アース座標は、ピッチ座標をさらにマッチング処理した後の座標である。本明細書において、添字Ｌはローカル座標であることを表し、添字Ｇはグローバル座標であることを表し、添字Ｐはピッチ座標であることを表し、添字Ｅはアース座標であることを表す。

座標変換行列Ｍ_ｔ ^ＬｔｏＧは式（２）より計算する。式（２）において、Ｍ_ｔ−１ ^ＰｔｏＥはピッチ座標をアース座標へ変換する変換行列である。また、Ｍ_ｔ−１ ^ＧｔｏＰは、グローバル座標を、ピッチ座標、すなわち、Ｚ_Ｌ軸方向およびピッチ角補正した座標へ変換する座標変換行列である。

座標変換行列Ｍ_ｔ ^ＧｔｏＰの前回値Ｍ_ｔ−１ ^ＧｔｏＰは、後述するステップＳ１０で計算している。また、座標変換行列Ｍ_ｔ ^ＰｔｏＥの前回値Ｍ_ｔ−１ ^ＰｔｏＥは、後述するステップＳ１２で計算している。なお、座標変換行列Ｍ_ｔ ^ＬｔｏＧの初期値Ｍ_０ ^ＬｔｏＧは式（３）で表される。そして、詳しくは後述するが、座標変換行列Ｍ_ｔ ^ＧｔｏＰは座標変換行列Ｍ_ｔ ^ＬｔｏＧから計算でき、座標変換行列Ｍ_ｔ ^ＰｔｏＥは座標変換行列Ｍ_ｔ ^ＧｔｏＰから計算できる。したがって、これら座標変換行列Ｍ_ｔ ^ＧｔｏＰ、Ｍ_ｔ ^ＰｔｏＥの初期値は、座標変換行列Ｍ_ｔ ^ＬｔｏＧの初期値から計算できる。

式（２）の意味を説明する。ΔＭ_ｔ−１は、時刻ｔ−１における車両１の移動量をローカル座標系で表している。このΔＭ_ｔ−１にＭ_ｔ−１ ^ＬｔｏＧを乗じると、時刻ｔ−１における車両１の移動量が、グローバル座標で表される。さらに、Ｍ_ｔ−１ ^ＧｔｏＰを乗じると、グローバル座標からピッチ座標系に変換され、さらに、Ｍ_ｔ−１ ^ＧｔｏＰを乗じると、ピッチ座標からアース座標に変換される。すなわち、（２）式は、ｔ時刻において、その１フレーム前の時刻ｔ−１における車両１のローカル座標系での移動量をアース座標で表していることになる。

ここで、ローカル座標系での移動量は、移動後のローカル座標を意味する。したがって、ローカル座標系での移動量をアース座標に変換することは、移動後の座標をアース座標に変換することを意味する。そして、アース座標は、車両位置を高い精度で表したグローバル座標を意味する。そこで、式２の右辺の計算を、時刻ｔにおいて、ローカル座標をグローバル座標に変換する座標変換行列Ｍ_ｔ ^ＬｔｏＧとするのである。

ステップＳ７では、時刻ｔにおける車両１のグローバル位置、すなわち、車両１の位置を表すグローバル座標を計算する。この計算は、ステップＳ６で計算した座標変換行列Ｍ_ｔ ^ＬｔｏＧと、ステップＳ２で計算したΔＭｔを用い、下記式（４）より計算する。

前述したように、ΔＭ_ｔは時刻ｔにおける車両のローカル座標を表しているので、式（４）より、時刻ｔにおける車両１のグローバル位置が計算できる。

ステップＳ８では、地図管理部１３に記憶されている環境地図点群から、ステップＳ７で計算したグローバル位置を基準として用いて、車両１の周辺の点群を抽出する。なお、環境地図点群は、グローバル座標で表されている。ステップＳ８の処理は、請求項の地図データ取得部、地図データ取得ステップに相当する処理である。

ステップＳ９では、ステップＳ８で抽出した点群を、Ｍ_ｔ ^ＬｔｏＧの逆行列を用いてローカル座標に変換する。

ステップＳ１０では、道路表面に対する車両１の高度偏差Δｚおよびピッチ角偏差Δφを補正する補正行列Ｍ_ｔ ^ΔＺと、グローバル座標をピッチ座標に変換する座標変換行列Ｍ_ｔ ^ＧｔｏＰを計算する。ステップＳ１０の処理は、請求項の地図補正部に相当する処理である。ステップＳ１０の詳細は、図８に示す。

まず、ステップＳ１０１では、地図管理部１３に記憶している道路表面地図点群から、ステップＳ７で計算したグローバル位置を基準として用いて、車両１の周辺の半径ｒ（ｍ）の点群を抽出する。ｒ（ｍ）はたとえば１０（ｍ）である。環境地図点群と同様、道路表面地図点群もグローバル座標で表されている。

ステップＳ１０２では、ステップＳ１０１で抽出した点群を、Ｍ_ｔ ^ＬｔｏＧの逆行列（Ｍ_ｔ ^ＬｔｏＧ）^−１を用いてローカル座標に変換した後、Ｘ_Ｌ−Ｚ_Ｌ平面上に投影する。これにより、図９に示すように、車両１の真横から道路表面地図点群を見た状態となる。なお、ステップＳ１０２において、ステップＳ９でローカル座標に変換した環境地図点群から、道路表面部分を抽出し、この抽出した道路表面部分をＸ_Ｌ−Ｚ_Ｌ平面上に投影してもよい。

ステップＳ１０３では、ロバスト推定において広く用いられているＲＡＮＳＡＣ（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）により、ステップＳ１０２でＸ_Ｌ−Ｚ_Ｌ平面上に投影した道路表面地図点群に相当する直線を算出する。これにより、図９に示す、現在位置を基準とした地面との高度偏差Δｚ、および、この地面と車両１のピッチ角偏差Δφが得られる。周辺監視センサ１１は車両１に搭載されているので、ピッチ角偏差Δφは周辺監視センサ１１の姿勢の一要素を表しているとも言える。

ステップＳ１０４では、これら高度偏差Δｚ、ピッチ角偏差Δφを補正する、式（５）に示す補正行列Ｍ_ｔ ^ΔＺを作成する。この式（５）において、Ｒｙ（）はＹ軸周りの回転行列、Ｔｚ（）はＺ軸方向の並進ベクトルである。

ステップＳ１０５では、ステップＳ１０４で作成した補正行列Ｍ_ｔ ^ΔＺを用い、グローバル座標をピッチ座標に変換する変換行列Ｍ_ｔ ^ＧｔｏＰを計算する。この変換行列Ｍ_ｔ ^ＧｔｏＰは式（６）により計算する。

式６は、グローバル座標を一旦、ローカル座標に変換し、補正行列Ｍ_ｔ ^ΔＺにより、高度偏差Δｚ、ピッチ角偏差Δφを補正した後に、グローバル座標に変換することを意味する。

ステップＳ１０５を終了したら、ステップＳ１０を終了したことになる。ステップＳ１０を終了したら、図５に示すステップＳ１１を実行する。

ステップＳ１１では、ステップＳ８で抽出した環境地図点群を、ローカル座標系に変換する。この変換は、環境地図点群を構成する各点に、式（６）の右辺の左２項の逆行列、すなわち、（Ｍ_ｔ ^ＬｔｏＧ Ｍ_ｔ ^Δｚ）^−１を乗じることで行う。この変換を行うことにより、ステップＳ８で抽出した環境地図点群を、車両１の高度偏差Δｚ、ピッチ角偏差Δφを補正した座標系にするのである。

ステップＳ１２では、ステップＳ１１でローカル座標系に変換した環境地図点群を、図７と同様の３層に分割する。そして、分割した環境地図点群およびステップＳ５で分割した観測地図点群を、高さが同じ層同士、層ごとにマッチング処理を行う。このマッチング処理は、Iterative Closest Pointアルゴリズム（以下、ＩＣＰアルゴリズム）を用いて行う。このステップＳ１２およびステップＳ５が請求項のマッチング処理部に相当する。

ステップＳ１２の詳細処理は図１０に示す。なお、図１０の説明において、観測地図点群を構成する点群のうちｉ層を構成する点群をＡ＝｛ａ_ｊ｝（｜Ａ｜＝Ｎ_Ａ）、ステップＳ１１で分割した環境地図点群のうち観測地図点群のｉ層と同じ高さの層の点群をＢ＝｛ｂ_ｊ｝（｜Ｂ｜＝Ｎ_Ｂ）とする。

まず、ステップＳ１２１において、座標変換行列ＭをＩとする。なお、座標変換行列Ｍは４行４列であるので、Ｉは具体的にはＩ_４である。ステップＳ１２２では、ｊ＝１とする。ステップＳ１２３では、Ｍ_ｊにＭを代入する。ステップＳ１２４では、ｊがＮ_Ａになったか否かを判断する。ステップＳ１２４の判断がＮＯであればステップＳ１２５に進む。

ステップＳ１２５では、環境地図点群ＢからＭ_ｊａ_ｊに対応する対応点ｍ_ｊを決定する。この対応点の決定には、ユークリッド距離に基づく最近傍探索を行う。

ステップＳ１２６では、対応点ｍ_ｊと入力点Ｍ_ｊａ_ｊとの距離が、ｄ_ｍａｘ以下であるか否かを判断する。この判断がＹＥＳであればステップＳ１２７に進み、ｗ_ｊを１とし、ＮＯであればステップＳ１２８に進み、ｗ_ｊを０とする。

ステップＳ１２９ではｊに１を加える。ステップＳ１２９を実行後はステップＳ１２４に戻る。ステップＳ１２４の判断がＹＥＳになった場合にはステップＳ１３０に進む。ステップＳ１３０では、ｗ_ｊ＝１の対応点と入力点Ｍ_ｊａ_ｊとの距離を最小化する変換行列を、最小二乗法を用いて計算する。

続くステップＳ１３１では、終了条件を満たすか否かを判断する。終了条件は、最大繰り返し回数に到達したこと、あるいは、この処理を繰り返し実行したことによる入力点Ｍ_ｊａ_ｊが収束したことのいずれかが成立したことである。終了条件を満たしていない場合には、ステップＳ１２２に戻る。ステップＳ１２２に戻ると、直近のステップＳ１３０で決定した座標変換行列Ｍで、入力点Ｍ_ｊａ_ｊに対応する対応点ｍ_ｊの探索が行われる。

ステップＳ１３１の判断がＹＥＳであればステップＳ１３２に進む。ステップＳ１３２では、最新のＭをＭ_ｔ ^ｉとする。

続くステップＳ１３３では、観測地図点群を構成する点群１点あたりの対応点への平均距離ｅ_ｔ ^ｉを式（７）から算出する。このステップＳ１３３の処理は、請求項の距離算出部に相当する処理である。

この図１０に示す処理を観測地図点群を構成する各層について行う。したがって、各層ｉに対して、グローバル座標系において、観測地図点群を構成する点群を環境地図点群にマッチングさせる座標変換行列Ｍ_ｔ ^ｉと、観測地図点群を構成する点群１点あたりの対応点への平均距離ｅ_ｔ ^ｉが得られる。

次に、ステップＳ１３へ進む。ステップＳ１３では、時刻ｔにおいてピッチ座標をアース座標に変換する座標変換行列Ｍ_ｔ ^ＰｔｏＥを計算する。この座標変換行列Ｍ_ｔ ^ＰｔｏＥは、ステップＳ１２で得た座標変換行列Ｍ_ｔ ^ｉに基づいて計算する。本実施形態では、３層それぞれについて計算した座標変換行列Ｍ_ｔ ^ｉのうち、平均距離ｅ_ｔ ^ｉが最も小さい座標変換行列Ｍ_ｔ ^ｉをＭ_ｔ ^ｉｃｐとし、下記式（８）により、Ｍ_ｔ ^ＰｔｏＥを算出する。

この式（８）は、ピッチ座標を、一旦、グローバル座標に変換し、ＩＣＰアルゴリズムによるマッチング処理をした後に、ピッチ座標に戻すことを意味する。アース座標は、ピッチ座標をＩＣＰアルゴリズムによるマッチング処理した座標であるが、ＩＣＰ処理はグローバル座標で行うので、ピッチ座標を、一旦、グローバル座標に変換するのである。

ステップＳ１４３では、式（９）により、高精度推定位置Ｐ_ｔを更新する。このステップＳ１４の処理は、請求項の位置計算部に相当する処理である。

式（９）において、ΔＭ_ｔはステップＳ２で計算しており、Ｍ_ｔ ^ＬｔｏＧはステップＳ６で計算しており、Ｍ_ｔ ^ＧｔｏＰはステップＳ１０で計算しており、Ｍ_ｔ ^ＰｔｏＥはステップＳ１３で計算している。この式（９）は、ローカル移動量を表す座標変換行列ΔＭ_ｔを、Ｍ_ｔ ^ＬｔｏＧによりローカル座標からグローバル座標に変換し、Ｍ_ｔ ^ＧｔｏＰによりグローバル座標からピッチ座標に変換し、Ｍ_ｔ ^ＰｔｏＥによりピッチ座標からアース座標に変換することを意味する。また、ローカル移動量は、ローカル座標系における移動後の車両位置を示している。よって、式（９）は、ローカル座標系における車両座標を、アース座標に変換することを意味している。

（測位結果）
図１１は、本実施形態の位置推定システム１０が決定した高精度推定位置の誤差を、従来の車両位置推定システムによる推定位置の誤差と比較して示す図である。誤差を算出するための正解位置には、ＲＴＫ−ＧＰＳにより測位を行う高精度ＧＰＳの測位値を用いた。また、ユークリッド距離は、正解位置と高精度推定位置、あるいは、従来システムによる推定位置との距離を意味する。

図１１から分かるように、本実施形態の位置推定システム１０が決定した高精度推定位置は、従来の推定位置よりも高い精度で位置を推定できていることが分かる。

（実施形態の効果）
以上、説明したように、本実施形態の位置推定システム１０は、この位置推定システム１０を搭載した車両１の周辺の点群を観測し、観測点群から作成した観測地図点群と、車両１の周辺の環境地図点群とをマッチングさせるマッチング処理を行う。

このマッチング処理は、観測地図点群を高さ方向に複数層に分割して、層ごとに行う。これにより、観測地図点群を構成する点が、環境地図点群を構成する点群のうち、層が異なる点群に含まれる点と対応してしまうことを抑制できる。したがって、観測地図点群を構成する点と、環境地図点群を構成する点との誤対応が減少するので、図１１に示したように、高精度推定位置は、真の現在位置を高い精度で推定できる。

以上、本発明の実施形態を説明したが、本発明は上述の実施形態に限定されるものではなく、次の変形例も本発明の技術的範囲に含まれ、さらに、下記以外にも要旨を逸脱しない範囲内で種々変更して実施できる。

＜変形例１、２＞
たとえば、前述の実施形態では、３層それぞれについて計算した座標変換行列Ｍ_ｔ ^ｉのうち、平均距離ｅ_ｔ ^ｉが最も小さい座標変換行列Ｍ_ｔ ^ｉをＭ_ｔ ^ｉｃｐとして、高精度推定位置Ｐ_ｔを計算していた（Ｓ１３、Ｓ１４）。

しかしこれに限られず、３層それぞれについて計算した座標変換行列Ｍ_ｔ ^ｉを、それぞれＭ_ｔ ^ｉｃｐとして用いて式（８）、式（９）を計算し、３つの推定位置を算出し、その３つの推定位置に基づいて、最終的な高精度推定位置Ｐ_ｔを算出してもよい。

これら３つの推定位置から最終的な高精度推定位置Ｐ_ｔを算出する方法は、たとえば、それら３つの推定位置の単純平均である（変形例１）。また、加重平均でもよい（変形例２）。加重平均の際の重みは、たとえば、平均距離ｅ_ｔ ^ｉの逆数とする。

＜変形例３＞
前述の実施形態では、道路表面地図点群をローカル座標系のＸ_Ｌ−Ｚ_Ｌ平面に投影することで、ピッチ角偏差Δφ、高度偏差Δｚを算出していた。しかし、これらピッチ角偏差Δφ、高度偏差Δｚを、ピッチ角、高度を検出するセンサの検出値に基づいて計算してもよい。

＜変形例４＞
前述の実施形態では、地図管理部１３に道路表面地図点群を記憶していた。しかし、道路表面地図点群は環境地図点群から高さ方向の点群を除去したものであるので、道路表面地図点群を記憶せず、環境地図点群から、都度、道路表面地図点群を作成してもよい。

＜変形例５−８＞
周辺監視センサ１１として、カメラを用いてもよい（変形例５）。位置推定システムを車両以外の移動体で搭載してもよい（変形例６）。観測地図点群を分割する層数は３以外の複数でもよい（変形例７）。観測地図点群を分割した各層の高さ方向長さが、互いに異なっていてもよい（変形例８）。

＜変形例９＞
前述の実施形態では、三次元地図データは、道路およびその周囲に存在する物体の三次元位置を、点群により示しているデータであった。また、観測データとして観測点群を取得していた。

しかし、三次元地図データ、観測データが点群でなくても、本発明は実施できる。したがって、三次元地図データとして、点に加えて、あるいは点に代えて、線や面により物体の三次元位置を示すデータを取得し、観測点群に代えて、周囲に存在する物体の形状を、点に加えて、あるいは点に代えて、線や面により物体の三次元位置を示す観測データを取得してもよい。

１：車両、 １０：位置推定システム、 １１：周辺監視センサ、 １２：無線通信部、 １３：地図管理部、 １４：衛星測位部、 １５：車両状態センサ、 １６：位置推定処理部、 １８：車内ＬＡＮ、 ２０：サーバ、 ２１：地図データベース、 ３０：測位用衛星

Claims (12)

1. 道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
   周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
   前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
   前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）と、
   前記測定部の姿勢を決定して、前記地図データ取得部が取得した前記三次元地図データを、前記測定部の姿勢に基づいて補正する地図補正部（Ｓ１０）とを備え、
   前記マッチング処理部は、前記地図補正部が補正した後の三次元地図データを前記複数の層に分割することを特徴とする位置推定装置（１６）。
2. 請求項１において、
   前記位置推定装置は移動体に搭載され、
   前記移動体の移動量を計算する移動量計算部（Ｓ２）を備え、
   前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、
   前記位置計算部は、前記マッチング処理部が決定した前記座標変換行列と、前記移動量計算部が計算した前記移動量とに基づいて、前記推定位置を逐次計算することを特徴とする位置推定装置。
3. 道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
   周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
   前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
   前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定装置であって、
   前記位置推定装置は移動体に搭載され、
   前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理部は、前記複数の層に対して前記マッチング処理を行い、
   前記移動体の移動量を計算する移動量計算部（Ｓ２）と、
   前記観測データを前記座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する前記三次元地図データとの平均距離を、前記層別に算出する距離算出部（Ｓ１３３）とをさらに備え、
   前記位置計算部は、前記距離算出部が計算した前記平均距離が最も短い層の前記座標変換行列と、前記移動量計算部が計算した前記移動量とに基づいて、前記推定位置を逐次計算することを特徴とする位置推定装置。
4. 道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
   周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
   前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
   前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定装置であって、
   前記位置推定装置は移動体に搭載され、
   前記移動体の移動量を計算する移動量計算部（Ｓ２）を備え、
   前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理部は、前記複数の層に対して前記マッチング処理を行い、
   前記位置計算部は、前記マッチング処理部が前記層別に算出した前記座標変換行列と前記移動量計算部が計算した前記移動量とに基づいて、前記層別に前記推定位置を算出し、層別に算出した推定位置を平均して、最終的な前記推定位置とすることを特徴とする位置推定装置。
5. 請求項４において、
   前記観測データを前記座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する前記三次元地図データとの平均距離を、前記層別に算出する距離算出部（Ｓ１３３）を備え、
   前記位置計算部は、前記距離算出部が前記層別に算出した前記平均距離に基づいて定まる重みを用いて前記層別に算出した前記推定位置を加重平均して、最終的な前記推定位置を算出することを特徴とする位置推定装置。
6. 請求項１〜５のいずれか１項において、
   前記三次元地図データは、点群により、道路およびその周囲に存在する物体の三次元位置を示しているデータであり、
   前記観測データは、周囲に存在する物体上の点を三次元的に測定したデータであることを特徴とする位置推定装置。
7. 道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、
   周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、
   前記測定結果取得ステップで取得した観測データおよび前記地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、
   前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）と、
   前記測定部の姿勢を決定して、前記地図データ取得ステップで取得した前記三次元地図データを、前記測定部の姿勢に基づいて補正する地図補正ステップ（Ｓ１０）とを備え、
   前記マッチング処理ステップは、前記地図補正ステップで補正した後の三次元地図データを前記複数の層に分割することを特徴とする位置推定方法。
8. 道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、
   周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、
   前記測定結果取得ステップで取得した観測データおよび前記地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、
   前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）とを備えた位置推定方法であって、
   前記位置推定方法は移動体で用いられ、
   前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理ステップでは、前記複数の層に対して前記マッチング処理を行い、
   前記移動体の移動量を計算する移動量計算ステップ（Ｓ２）と、
   前記観測データを前記座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する前記三次元地図データとの平均距離を、前記層別に算出する距離算出ステップ（Ｓ１３３）とをさらに備え、
   前記位置計算ステップは、前記距離算出ステップで算出した前記平均距離が最も短い層の前記座標変換行列と、前記移動量計算ステップで計算した前記移動量とに基づいて、前記推定位置を逐次計算することを特徴とする位置推定方法。
9. 道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得ステップ（Ｓ８）と、
   周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得ステップ（Ｓ３）と、
   前記測定結果取得ステップで取得した観測データおよび前記地図データ取得ステップで取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理ステップ（Ｓ５、Ｓ１２）と、
   前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算ステップ（Ｓ１４）とを備えた位置推定方法であって、
   前記位置推定方法は移動体で用いられ、
   前記移動体の移動量を計算する移動量計算ステップ（Ｓ２）を備え、
   前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理ステップは、前記複数の層に対して前記マッチング処理を行い、
   前記位置計算ステップは、前記マッチング処理ステップで前記層別に算出した前記座標変換行列と前記移動量計算ステップで計算した前記移動量とに基づいて、前記層別に前記推定位置を算出し、層別に算出した推定位置を平均して、最終的な前記推定位置とすることを特徴とする位置推定方法。
10. コンピュータを、
    道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
    周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
    前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
    前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）と、
    前記測定部の姿勢を決定して、前記地図データ取得部が取得した前記三次元地図データを、前記測定部の姿勢に基づいて補正する地図補正部（Ｓ１０）とを備え、
    前記マッチング処理部は、前記地図補正部が補正した後の三次元地図データを前記複数の層に分割するものとして機能させるための位置推定プログラム。
11. コンピュータを、
    道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
    周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
    前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
    前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定プログラムとして機能させ、
    前記コンピュータは移動体で用いられ、
    かつ、前記コンピュータを、
    前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理部は、前記複数の層に対して前記マッチング処理を行い、
    前記移動体の移動量を計算する移動量計算部（Ｓ２）と、
    前記観測データを前記座標変換行列により座標変換した後のデータと、座標変換後のデータに対応する前記三次元地図データとの平均距離を、前記層別に算出する距離算出部（Ｓ１３３）とをさらに備え、
    前記位置計算部は、前記距離算出部が計算した前記平均距離が最も短い層の前記座標変換行列と、前記移動量計算部が計算した前記移動量とに基づいて、前記推定位置を逐次計算するものとして機能させるための位置推定プログラム。
12. コンピュータを、
    道路およびその周囲に存在する物体の三次元位置を示している三次元地図データを取得する地図データ取得部（Ｓ８）と、
    周囲に存在する物体の形状を三次元的に測定する測定部（１１）から、測定結果である観測データを取得する測定結果取得部（Ｓ３）と、
    前記測定結果取得部が取得した観測データおよび前記地図データ取得部が取得した三次元地図データを、それぞれ、高さに基づいて複数の層に分割し、層別に、前記観測データと、前記三次元地図データとの対応を決定するマッチング処理を行うマッチング処理部（Ｓ５、Ｓ１２）と、
    前記マッチング処理の結果に基づいて、現在位置を推定した推定位置を計算する位置計算部（Ｓ１４）とを備えた位置推定プログラムとして機能させ、
    前記コンピュータは移動体で用いられ、
    かつ、前記コンピュータを、
    前記移動体の移動量を計算する移動量計算部（Ｓ２）を備え、
    前記マッチング処理は、前記観測データを前記三次元地図データにマッチングさせるための座標変換行列を決定する処理であり、前記マッチング処理部は、前記複数の層に対して前記マッチング処理を行い、
    前記位置計算部は、前記マッチング処理部が前記層別に算出した前記座標変換行列と前記移動量計算部が計算した前記移動量とに基づいて、前記層別に前記推定位置を算出し、層別に算出した推定位置を平均して、最終的な前記推定位置とするものとして機能させるための位置推定プログラム。