特開 2024-103605 情報処理装置、制御方法、プログラム及び記憶媒体

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024103605

(43)【公開日】2024-08-01

(54)【発明の名称】情報処理装置、制御方法、プログラム及び記憶媒体

(51)【国際特許分類】

   G01C 21/30 20060101AFI20240725BHJP

【ＦＩ】

G01C21/30

【審査請求】有

【請求項の数】1

【出願形態】ＯＬ

(21)【出願番号】P 2024087001

(22)【出願日】2024-05-29

(62)【分割の表示】P 2022555528の分割

【原出願日】2021-10-06

(31)【優先権主張番号】P 2020169915

(32)【優先日】2020-10-07

(33)【優先権主張国・地域又は機関】JP

(71)【出願人】

【識別番号】000005016

【氏名又は名称】パイオニア株式会社

(71)【出願人】

【識別番号】520001073

【氏名又は名称】パイオニアスマートセンシングイノベーションズ株式会社

(74)【代理人】

【識別番号】100107331

【弁理士】

【氏名又は名称】中村 聡延

(72)【発明者】

【氏名】鈴木 雅美

(72)【発明者】

【氏名】加藤 将大

(57)【要約】

【課題】地図データと計測データとの照合を好適に実行可能な情報処理装置を提供する。
【解決手段】情報処理装置は、点群地図データ取得手段と、スキャンデータ取得手段と、スキャンデータ処理手段と、照合手段とを有する。点群地図データ取得手段は、反射強度の情報が閾値以上の点群データである点群地図データを取得する。スキャンデータ取得手段は、出射した光の反射光を受光することで計測した点群データであるスキャンデータを取得する。スキャンデータ処理手段は、スキャンデータから、反射強度が所定の閾値以上となる高反射強度スキャンデータを生成する。照合手段は、点群地図データと高反射強度スキャンデータとの照合を行う。
【選択図】図３

【特許請求の範囲】

【請求項1】

反射強度が閾値以上の点群データである点群地図データを取得する点群地図データ取得手段と、
出射した光の反射光を受光することで計測した点群データであるスキャンデータを取得するスキャンデータ取得手段と、
前記スキャンデータから、反射強度が所定の閾値以上となる高反射強度スキャンデータを生成するスキャンデータ処理手段と、
前記点群地図データと前記高反射強度スキャンデータとの照合を行う照合手段と、
を有する情報処理装置。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、車両の姿勢の推定技術に関する。

【背景技術】

【0002】

従来から、レーダやカメラなどの計測部の計測データに基づいて、自車位置推定などを行う技術が知られている。例えば、特許文献１には、計測センサの出力と、予め地図上に登録された地物の位置情報とを照合させることで自己位置を推定する技術が開示されている。また、非特許文献１には、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）による照合（マッチング）であって、ＩＣＰの異なる２つのアルゴリズムであるＰｏｉｎｔ ｔｏ ＰｏｉｎｔアルゴリズムとＰｏｉｎｔ ｔｏ Ｐｌａｎｅアルゴリズムとを組み合わせた位置推定手法が開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１３－２５７７４２号公報

【非特許文献】

【0004】

【非特許文献1】S.Takai, H.Date, S.Kanai, Y.Niina, K.Oda, and T. Ikeda: Accurate registration of MMS point clouds of urban areas using trajectory, ISPRS Annals of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume II-5/W2, pp. 277-282. 2013.

【発明の概要】

【発明が解決しようとする課題】

【0005】

計測データを取得する場所に特徴的な３次元構造を持つ物体が数多く存在しない環境では、取得された計測データの中にマッチングを安定的に行うための特徴的な形状情報が少なくなり、高い精度で地図データと計測データとの照合（マッチング）を行うことが難しくなる。その結果、自車等の位置・姿勢推定処理に位置や方位の誤差が生じるという課題がある。

【0006】

本開示は、上記のような課題を解決するためになされたものであり、地図データと計測データとの照合を好適に実行可能な情報処理装置を提供することを主な目的とする。

【課題を解決するための手段】

【0007】

請求項に記載の発明は、
反射強度が閾値以上の点群データである点群地図データを取得する点群地図データ取得手段と、
出射した光の反射光を受光することで計測した点群データであるスキャンデータを取得するスキャンデータ取得手段と、
前記スキャンデータから、反射強度が所定の閾値以上となる高反射強度スキャンデータを生成するスキャンデータ処理手段と、
前記点群地図データと前記高反射強度スキャンデータとの照合を行う照合手段と、
を有する情報処理装置である。

【0008】

また、請求項に記載の発明は、
コンピュータが実行する制御方法であって、
反射強度が閾値以上の点群データである点群地図データを取得し、
出射した光の反射光を受光することで計測した点群データであるスキャンデータを取得し、
前記スキャンデータから、反射強度が所定の閾値以上となる高反射強度スキャンデータを生成し、
前記点群地図データと前記高反射強度スキャンデータとの照合を行う、制御方法である。

【0009】

また、請求項に記載の発明は、
反射強度が閾値以上の点群データである点群地図データを取得し、
出射した光の反射光を受光することで計測した点群データであるスキャンデータを取得し、
前記スキャンデータから、反射強度が所定の閾値以上となる高反射強度スキャンデータを生成し、
前記点群地図データと前記高反射強度スキャンデータとの照合を行う処理をコンピュータに実行させるプログラムである。

【図面の簡単な説明】

【0010】

【図1】運転支援システムの概略構成図である。

【図2】車載機の機能的構成を示すブロック図である。

【図3】位置推定に関する制御部の機能的なブロック構成図の一例である。

【図4】位置推定処理の手順を示すフローチャートの一例である。

【図5】Ｃｏｍｂｉｎｅｄ－ＩＣＰ処理の手順を示すフローチャートの一例である。

【図6】ＩＣＰループ処理の手順を示すフローチャートの一例である。

【図7】スキャンデータ処理の概要を示す図である。

【図8】反射強度閾値の設定に用いられるヒストグラムの一例である。

【図9】地図データ処理の概要を示す図である。

【図10】シミュレーションにおいて使用する点群地図データの各点を３次元空間にプロットした状態を表す斜視図である。

【図11】比較例におけるマッチング結果を示す図である。

【図12】本実施例に基づくマッチング手法によるマッチング結果を示す図である。

【図13】第２マッチングの結果を個別に示した図である。

【図14】実施例の手法と、比較例の手法とを実行した場合の、ＩＣＰループ処理の反復回数と二乗誤差との関係を示すグラフである。

【発明を実施するための形態】

【0011】

本発明の好適な実施形態によれば、情報処理装置は、反射強度が閾値以上の点群データである点群地図データを取得する点群地図データ取得手段と、出射した光の反射光を受光することで計測した点群データであるスキャンデータを取得するスキャンデータ取得手段と、前記スキャンデータから、反射強度が所定の閾値以上となる高反射強度スキャンデータを生成するスキャンデータ処理手段と、前記点群地図データと前記高反射強度スキャンデータとの照合を行う照合手段と、を有する。なお、「反射強度が所定の閾値以上となる高反射強度スキャンデータ」の「閾値」と、「反射強度が閾値以上の点群データである点群地図データ」の「閾値」とは、それぞれ、点群地図データの取得時に使用されたライダ等の計測装置の反射強度特性と、点群地図データ作成時に使用されたライダ等の計測装置の反射強度特性とに基づいて、最適な値に設定される。この態様により、情報処理装置は、反射強度が高いスキャンデータを利用し、３次元構造を持つ物体が数多く存在しない環境においても、地図データとスキャンデータとの照合を好適に実行することができる。

【0012】

上記情報処理装置の一態様では、前記点群地図データ取得手段は、前記点群地図データとして、前記反射強度が前記閾値以上となる基準面上の点を含む点群データである高反射強度地図データを取得し、前記スキャンデータ処理手段は、前記スキャンデータから、前記反射強度が前記閾値以上となる基準面上の点を含むデータを、前記高反射強度スキャンデータとして生成し、前記照合手段は、前記高反射強度地図データと、前記高反射強度スキャンデータとの照合を行う。この態様により、情報処理装置は、基準面の高反射部分を基準とした照合を好適に実行することができる。

【0013】

上記情報処理装置の他の一態様では、前記照合手段は、Ｐｏｉｎｔ ｔｏ Ｐｏｉｎｔアルゴリズムに基づき前記照合を行う。この態様により、基準面の高反射部分を基準としたマッチングを高精度に実行することができる。

【0014】

上記情報処理装置の他の一態様では、前記点群地図データ取得手段は、前記高反射強度地図データと、３次元空間に存在する地物の点群データである３次元空間地図データとを取得し、前記スキャンデータ処理手段は、前記スキャンデータ又は当該スキャンデータに対してフィルタを適用したデータを、３次元空間スキャンデータとして生成し、前記照合手段は、前記３次元空間地図データと前記３次元空間スキャンデータとの第１照合と、前記高反射強度地図データと前記高反射強度スキャンデータとの第２照合とを実行する。この態様により、情報処理装置は、基準面以外のスキャンデータについても利用して好適に照合を実行することができる。好適な例では、前記照合手段は、Ｐｏｉｎｔ ｔｏ Ｐｌａｎｅアルゴリズムに基づき前記第１照合を行い、Ｐｏｉｎｔ ｔｏ Ｐｏｉｎｔアルゴリズムに基づき前記第２照合を行うとよい。

【0015】

上記情報処理装置の他の一態様では、前記照合手段は、前記第１照合において照合する前記３次元空間スキャンデータの数と、前記第２照合において照合する前記高反射強度スキャンデータの点の数とに基づき、前記第１照合と前記第２照合との重み付けを行う。この態様により、情報処理装置は、処理対象となった点の数に応じた重み付けにより第１照合と第２照合とを好適に組み合わせた照合結果を得ることができる。

【0016】

上記情報処理装置の他の一態様では、前記基準面は、道路面を含む。これにより、情報処理装置は、３次元構造を持つ物体が数多く存在しない環境においても、路面ペイントを基準として地図データとスキャンデータとの照合を好適に実行することができる。

【0017】

上記情報処理装置の他の一態様では、前記スキャンデータ取得手段は、出射した光の反射光を受光する外界センサにより計測した前記スキャンデータを取得する。この態様により、情報処理装置は、反射強度の情報を含むスキャンデータを好適に取得することができる。

【0018】

上記情報処理装置の他の一態様では、前記照合手段は、前記照合により、前記スキャンデータの座標系を前記点群地図データの座標系に変換するためのパラメータを算出する。情報処理装置は、上述した照合を高精度に行うことで、スキャンデータの座標系を点群地図データの座標系に変換するためのパラメータを高精度に算出することができる。

【0019】

上記情報処理装置の他の一態様では、前記スキャンデータ処理手段は、前記閾値を、前記スキャンデータの反射強度を対象としたヒストグラムに基づき設定する。この態様により、情報処理装置は、高反射強度スキャンデータの生成に用いる閾値を、適応的に算出することができる。

【0020】

本発明の他の好適な実施形態によれば、コンピュータが実行する制御方法であって、反射強度が閾値以上の点群データである点群地図データを取得し、出射した光の反射光を受光することで計測した点群データであるスキャンデータを取得し、前記スキャンデータから、反射強度が所定の閾値以上となる高反射強度スキャンデータを生成し、前記点群地図データと前記高反射強度スキャンデータとの照合を行う。情報処理装置は、この制御方法を実行することで、地図データとスキャンデータとの照合を好適に実行することができる。

【0021】

本発明の他の好適な実施形態によれば、プログラムは、反射強度が閾値以上の点群データである点群地図データを取得し、出射した光の反射光を受光することで計測した点群データであるスキャンデータを取得し、前記スキャンデータから、反射強度が所定の閾値以上となる高反射強度スキャンデータを生成し、前記点群地図データと前記高反射強度スキャンデータとの照合を行う処理をコンピュータに実行させる。コンピュータは、このプログラムを実行することで、地図データとスキャンデータとの照合を好適に実行することができる。好適には、上記プログラムは、記憶媒体に記憶される。

【実施例0022】

以下、図面を参照して本発明の好適な実施例について説明する。なお、任意の記号の上に「＾」が付された文字を、本明細書では便宜上、「Ａ＾」（「Ａ」は任意の文字）と表す。

【0023】

（１）運転支援システムの概要
図１は、本実施例に係る運転支援システムの概略構成である。運転支援システムは、移動体である車両と共に移動する車載機１と、ライダ（Ｌｉｄａｒ：Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ、または、Ｌａｓｅｒ Ｉｌｌｕｍｉｎａｔｅｄ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）３とを有する。

【0024】

車載機１は、ライダ３などの外界センサ及び図示しない内界センサ等と電気的に接続し、これらの出力に基づき、運転支援システムにおいて基準とする位置（姿勢を含む）の推定を行う。基準とする位置は、車載機１が搭載される車両の中心位置であってもよく、位置推定に用いるライダ３の位置であってもよい。そして、車載機１は、位置推定結果に基づき、設定された目的地への経路に沿って走行するように、車両の自動運転制御などの運転支援を行う。車載機１は、道路及び道路周辺の地物の表面を計測した点毎に３次元位置と反射強度とを示す点群の地図データである点群地図データ１０を記憶する。そして、車載機１は、この点群地図データ１０とライダ３による計測結果であるスキャンデータとを照合させて位置推定を行う。

【0025】

ライダ３は、水平方向および垂直方向の所定の角度範囲に対してパルスレーザを出射することで、外界に存在する物体までの距離を離散的に測定し、当該物体の位置を示す３次元の点群を示すスキャンデータを生成する。この場合、ライダ３は、照射方向を変えながらレーザ光を照射する照射部と、照射したレーザ光の反射光（散乱光）を受光する受光部と、受光部が出力する受光信号に基づくスキャンデータを出力する出力部とを有する。スキャンデータは、受光部が受光したレーザ光に対応する照射方向と、上述の受光信号に基づき特定される当該レーザ光の応答遅延時間とに基づき生成される。１回分の走査により得られるスキャンデータは、１フレーム分のスキャンデータに相当する。

【0026】

なお、ライダ３の設置位置は、図１に示される位置に限らず、少なくとも路面を照射範囲に含む設置位置であればよい。また、ライダ３は、車両に複数設けられてもよい。車載機１は「情報処理装置」の一例であり、ライダ３は「外界センサ」の一例である。

【0027】

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

【0028】

インターフェース１１は、ハードウェアインタフェースとして機能し、センサ群２が出力するデータを、制御部１５へ供給する。また、インターフェース１１は、制御部１５が生成した車両の走行制御に関する信号を、車両の電子制御装置（ＥＣＵ：Ｅｌｅｃｔｒｏｎｉｃ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）へ供給する。

【0029】

センサ群２は、位置推定に必要な種々の外界センサ及び内界センサにより構成される。センサ群２は、ライダ３、ＧＰＳ受信機４、ジャイロセンサ５、及び車速センサ６などを有している。センサ群２は、ジャイロセンサ３に代えて、又はこれに加えて、３軸方向における計測車両の加速度及び角速度を計測する慣性計測装置（ＩＭＵ）などを有してもよい。また、センサ群２は、ライダ３以外の外界センサ（例えばカメラ）などをさらに有してもよい。

【0030】

記憶部１２は、制御部１５が実行するプログラムや、制御部１５が所定の処理を実行するのに必要な情報を記憶する。記憶部１２は、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フラッシュメモリなどの揮発性メモリ及び不揮発性メモリにより構成される。本実施例では、記憶部１２は、点群地図データ１０を記憶する。

【0031】

なお、点群地図データ１０は、定期的に更新されてもよい。この場合、例えば、制御部１５は、通信部１３を介し、地図データを管理するサーバ装置から、自車位置が属するエリアに関する地図データを受信し、点群地図データ１０に反映させる。また、点群地図データ１０は、記憶部１２に記憶される代わりに、車載機１と通信可能な１又は複数のサーバ装置により記憶されてもよい。この場合、例えば、制御部１５は、通信部１３を介して、上記サーバ装置と通信を行うことで、処理に必要な自車位置周辺の点群地図データを、必要なタイミングにより取得する。また、点群地図データ１０は、ＳＬＡＭ（Simultaneous Localization and Mapping）により車載機１が生成するデータであってもよい。

【0032】

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

【0033】

制御部１５は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などの１または複数のプロセッサを含み、記憶部１２等に記憶されたプログラムを実行することで、車載機１の全体を制御する。制御部１５は、「点群地図データ取得手段」、「スキャンデータ取得手段」、「スキャンデータ処理手段」、「照合手段」及びプログラムを実行する「コンピュータ」の一例である。なお、これらの各手段の少なくとも一部は、プログラムによるソフトウェアで実現することに限ることなく、ハードウェア、ファームウェア、及びソフトウェアのうちのいずれかの組合せ等により実現してもよい。また、これらの各構成要素の少なくとも一部は、例えばＦＰＧＡ（field-programmable gate array）又はマイクロコントローラ等の、ユーザがプログラミング可能な集積回路を用いて実現してもよい。

【0034】

（３）位置推定
次に、車載機１の制御部１５が実行する、点群地図データ１０とライダ３によるスキャンデータとのマッチング（照合）による位置推定について説明する。

【0035】

（３－１）概要
概略的には、制御部１５は、ライダ３が出力するスキャンデータから、３次元空間のスキャンデータと、反射強度が所定の閾値以上となる道路面のスキャンデータとを夫々抽出し、これらのデータの各々に対して用意した地図データとのマッチングを行う。これにより、制御部１５は、特徴的な３次元構造を持つ地物が存在しない環境であっても、路面ペイントに対するスキャンデータに基づき、位置推定のためのマッチングを安定的に行う。

【0036】

以後では、３次元空間のスキャンデータを「３Ｄ空間スキャンデータＤｓ１」と呼び、反射強度が所定の閾値以上となる道路面のスキャンデータを「高反射強度スキャンデータＤｓ２」と呼ぶ。また、３Ｄ空間スキャンデータＤｓ１とのマッチングに使用するために点群地図データ１０から抽出された３次元空間の地図データを、「３Ｄ空間地図データＤｍ１」と呼び、高反射強度スキャンデータＤｓ２とのマッチングに使用するために点群地図データ１０から抽出された道路面の高反射部分（即ち路面ペイント）の地図データを、「高反射強度地図データＤｍ２」と呼ぶ。

【0037】

また、制御部１５は、ＩＣＰを用いて、上述のマッチングを行う。ここで、ＩＣＰのアルゴリズムとして、点同士のマッチングの度合いを評価指標とするＰｏｉｎｔ ｔｏ Ｐｏｉｎｔ（Ｐ２Ｐ）アルゴリズムと、法線情報を利用した評価指標を用いるＰｏｉｎｔ ｔｏ Ｐｌａｎｅ（Ｐ２ＰＬ）アルゴリズムとが存在する。そして、本実施例では、制御部１５は、Ｐ２Ｐアルゴリズムと、Ｐ２ＰＬアルゴリズムとを組み合わせた「Ｃｏｍｂｉｎｅｄ ＩＣＰ」アルゴリズムに基づき、上述のマッチングを行う。具体的には、制御部１５は、３Ｄ空間スキャンデータＤｓ１と３Ｄ空間地図データＤｍ１とのマッチング（「第１マッチング」とも呼ぶ。）ではＰ２ＰＬの評価指標を用い、高反射強度スキャンデータＤｓ２と高反射強度地図データＤｍ２とのマッチング（「第２マッチング」とも呼ぶ。）ではＰ２Ｐの評価指標を用いてマッチングを行う。また、以後では、マッチングされる側のデータを「ターゲット」と呼び、マッチングする側のデータを、「ソース」とも呼ぶ。

【0038】

（３－２）ブロック構成図
図３は、位置推定に関する制御部１５の機能的なブロック構成図の一例である。制御部１５は、機能的には、地図データ処理部２０と、法線ベクトル算出部２１と、第１対応付け部２２と、スキャンデータ処理部２３と、選択部２４、２５と、第２対応付け部２６と、統合連立方程式構築部２７と、特異値分解部２８と、変換部２９と、３Ｄ空間二乗誤差計算部３０と、高反射強度二乗誤差計算部３１と、適応重み付け算出部３２と、重み付け加算部３３と、収束判定部３４と、変換行列更新部３５と、切替部３６とを有する。

【0039】

地図データ処理部２０は、点群地図データ１０に基づき、位置推定のマッチングに必要な３Ｄ空間地図データＤｍ１と高反射強度地図データＤｍ２とを生成する。３Ｄ空間地図データＤｍ１と高反射強度地図データＤｍ２の生成方法については、「（３－３）データ処理」のセクションにて詳しく説明する。

【0040】

法線ベクトル算出部２１は、３Ｄ空間地図データＤｍ１が示す各点について、第１マッチングに必要な法線ベクトルを算出する。なお、第１マッチングにおいて実行されるＰ２ＰＬアルゴリズムは、ターゲットの法線情報を使用する方式であり、第１マッチングでは、３Ｄ空間地図データＤｍ１がターゲットに相当し、３Ｄ空間スキャンデータＤｓ１がソースに相当する。

【0041】

第１対応付け部２２は、３Ｄ空間地図データＤｍ１と３Ｄ空間スキャンデータＤｓ１との対応付けを行う。この場合、第１対応付け部２２は、３Ｄ空間スキャンデータＤｓ１に含まれている各点に対して、３Ｄ空間地図データＤｍ１内の最も近い点（最近傍点）を探索する。そして、第１対応付け部２２は、点同士を対応付けた３Ｄ空間地図データＤｍ１及び３Ｄ空間スキャンデータＤｓ１と、法線ベクトルの情報とを、統合連立方程式構築部２７に供給する。

【0042】

スキャンデータ処理部２３は、ライダ３が生成したスキャンデータに対して所定の処理を行い、３Ｄ空間スキャンデータＤｓ１と、高反射強度スキャンデータＤｓ２とを生成する。この場合、スキャンデータ処理部２３は、例えば、ライダ３が走査を行う周期（フレーム周期）に従い、得られた最新の１フレーム分のスキャンデータから、３Ｄ空間スキャンデータＤｓ１と高反射強度スキャンデータＤｓ２とを生成する処理を行う。３Ｄ空間スキャンデータＤｓ１と高反射強度スキャンデータＤｓ２の生成方法については後述する。

【0043】

ここで、ライダ３が生成したスキャンデータは、ライダ３を基準とする座標系となっている。従って、スキャンデータ処理部２３は、例えば、ライダ３以外のセンサ群２の出力信号等に基づき暫定的に推定した車両位置及び予め記憶部１２に記憶されたライダ３の設置位置等の情報に基づき、スキャンデータを、点群地図データ１０において採用される地図座標系に暫定的に変換してもよい。この場合、スキャンデータ処理部２３は、地図座標系での３Ｄ空間スキャンデータＤｓ１と高反射強度スキャンデータＤｓ２を生成する。

【0044】

選択部２４は、スキャンデータ処理部２３が生成した３Ｄ空間スキャンデータＤｓ１、又は、現在のスキャンデータのフレームにおける前回のループ処理において変換部２９により暫定的に変換された３Ｄ空間スキャンデータＤｓ１のいずれかを選択する。そして、選択部２４は、選択した３Ｄ空間スキャンデータＤｓ１を、第１対応付け部２２に供給する。

【0045】

具体的には、選択部２４は、処理対象となるスキャンデータのフレームを対象としたＩＣＰに関するループ処理（「ＩＣＰループ処理」とも呼ぶ。）のループ回数「ｉ」が初回（ｉ＝１）の場合、スキャンデータ処理部２３から供給される３Ｄ空間スキャンデータＤｓ１を、第１対応付け部２２に供給する。一方、選択部２４は、ＩＣＰループ処理が２回目以降（ｉ＞１）であって、かつ、ＩＣＰループ処理の終了条件が満たされていない場合に、変換部２９により変換処理がなされた３Ｄ空間スキャンデータＤｓ１を、第１対応付け部２２に供給する。この場合、ＩＣＰループ処理を所定回数実行することを、ＩＣＰループ処理の終了条件としてもよく、ＩＣＰループ処理が収束することを、ＩＣＰループ処理の終了条件としてもよい。なお、ＩＣＰループ処理の終了条件の判定は、後述する収束判定部３４により行われる。

【0046】

選択部２５は、スキャンデータ処理部２３が生成した高反射強度スキャンデータＤｓ２、又は、現在のスキャンデータのフレームにおける前回のループ処理において変換部２９により暫定的に変換された高反射強度スキャンデータＤｓ２のいずれかを選択する。そして、選択部２５は、選択した高反射強度スキャンデータＤｓ２を、第２対応付け部２６に供給する。具体的には、選択部２５は、ＩＣＰループ処理の回数ｉが初回（ｉ＝１）の場合、スキャンデータ処理部２３から供給される高反射強度スキャンデータＤｓ２を、第２対応付け部２６に供給する。一方、選択部２５は、ＩＣＰループ処理が２回目以降（ｉ＞１）であって、かつ、ＩＣＰループ処理の終了条件が満たされていない場合に、変換部２９により変換処理がなされた高反射強度スキャンデータＤｓ２を第２対応付け部２６に供給する。

【0047】

第２対応付け部２６は、地図データ処理部２０から供給される高反射強度地図データＤｍ２と、選択部２５から供給される高反射強度スキャンデータＤｓ２との対応付けを行う。この場合、第２対応付け部２６は、高反射強度スキャンデータＤｓ２に含まれている各点に対して、高反射強度地図データＤｍ２内の最も近い点（最近傍点）を探索する。そして、第２対応付け部２６は、点毎に対応付けた高反射強度地図データＤｍ２及び高反射強度スキャンデータＤｓ２を、統合連立方程式構築部２７に供給する。

【0048】

統合連立方程式構築部２７は、第１マッチングと第２マッチングとにおいて必要な連立方程式を構築する。統合連立方程式構築部２７は、第１構築部２７Ｘと、第２構築部２７Ｙとを含む。第１構築部２７Ｘは、第１対応付け部２２から供給される情報に基づき、第１マッチングに関する連立方程式（「第１連立方程式」とも呼ぶ。）を構築する。第２構築部２７Ｙは、第２対応付け部２６から供給される情報に基づき、第２マッチングに関する連立方程式（「第２連立方程式」とも呼ぶ。）を構築する。そして、統合連立方程式構築部２７は、第１連立方程式と、第２連立方程式とを統合した連立方程式（「統合連立方程式」とも呼ぶ。）を構築し、構築した統合連立方程式を特異値分解部２８へ供給する。

【0049】

特異値分解部２８は、統合連立方程式構築部２７から供給される統合連立方程式の解を、特異値分解により算出する。統合連立方程式の解は、３Ｄ空間スキャンデータＤｓ１と高反射強度スキャンデータＤｓ２とに夫々適用すべき変換行列のパラメータを含んでいる。この変換行列は、回転行列「Ｒ」と、並進ベクトル「Ｔ」とを含み、以後では、「［Ｒ Ｔ］」とも表記する。なお、特異値分解部２８が各ループにおいて算出する変換行列は、暫定的な変換行列であり、１回のループにおいて算出する変換行列を「暫定変換行列」と呼び、その回転行列及び並進ベクトルを、夫々、「Ｒ＿ｔｍｐ」及び「Ｔ＿ｔｍｐ」と表記する。

【0050】

変換部２９は、３Ｄ空間スキャンデータＤｓ１と、高反射強度スキャンデータＤｓ２とを、特異値分解部２８が算出する暫定変換行列に基づき変換する。変換部２９は、３Ｄ空間スキャンデータＤｓ１を変換する第１変換部２９Ｘと、高反射強度スキャンデータＤｓ２を変換する第２変換部２９Ｙとを有する。第１変換部２９Ｘは、変換後の３Ｄ空間スキャンデータＤｓ１を、選択部２４及び３Ｄ空間二乗誤差計算部３０に夫々供給し、第２変換部２９Ｙは、変換後の高反射強度スキャンデータＤｓ２を、選択部２５及び高反射強度二乗誤差計算部３１に供給する。

【0051】

３Ｄ空間二乗誤差計算部３０は、第１変換部２９Ｘが変換した３Ｄ空間スキャンデータＤｓ１と、３Ｄ空間地図データＤｍ１との二乗誤差（「３Ｄ空間二乗誤差Ｅ_{ＰＴ＿ＰＬ}」とも呼ぶ。）の計算を行う。高反射強度二乗誤差計算部３１は、第２変換部２９Ｙが変換した高反射強度スキャンデータＤｓ２と、高反射強度地図データＤｍ２との二乗誤差（「高反射強度二乗誤差Ｅ_{ＰＴ＿ＰＴ}」とも呼ぶ。）の計算を行う。

【0052】

適応重み付け算出部３２は、３Ｄ空間二乗誤差Ｅ_{ＰＴ＿ＰＬ}と高反射強度二乗誤差Ｅ_{ＰＴ＿ＰＴ}とに対して夫々適用する重みの算出を行う。例えば、適応重み付け算出部３２は、処理対象のフレームにおいて、第１マッチングと第２マッチングとで扱う処理点数比率に基づき、上記の重みを決定する。例えば、第１マッチングでの処理点数（即ち、３Ｄ空間スキャンデータＤｓ１に含まれる点の数）を「Ｎ１」、第２マッチングでの処理点数（即ち、高反射強度スキャンデータＤｓ２に含まれる点の数）を「Ｎ２」、３Ｄ空間二乗誤差Ｅ_{ＰＴ＿ＰＬ}に対する重みを「ｗ１」、高反射強度二乗誤差Ｅ_{ＰＴ＿ＰＴ}に対する重みを「ｗ２」とすると、適応重み付け算出部３２は、以下の式に基づき、重みｗ１と重みｗ２とをフレーム毎に算出する。
ｗ１＝Ｎ１／（Ｎ１＋Ｎ２）
ｗ２＝１－（Ｎ１／（Ｎ１＋Ｎ２））

【0053】

重み付け加算部３３は、適応重み付け算出部３２が算出した重みｗ１、ｗ２を用い、以下の式に示されるように、３Ｄ空間二乗誤差と高反射強度二乗誤差との重み付け和に相当する総合誤差「Ｅ_{＿ｔｏｔａｌ}」を算出する。
Ｅ_{＿ｔｏｔａｌ}＝ｗ１・Ｅ_{ＰＴ＿ＰＬ}＋ｗ２・Ｅ_{ＰＴ＿ＰＴ}

【0054】

収束判定部３４は、ＩＣＰループ処理の収束等の判定を行うことで、ＩＣＰループ処理の終了条件が満たされたか否か判定する。具体的には、収束判定部３４は、現フレームにおいて、ＩＣＰループ処理が所定回数だけ実行された場合、又は、今回のループでの総合誤差Ｅ_{＿ｔｏｔａｌ}と前回のループでの総合誤差Ｅ_{＿ｔｏｔａｌ}との差が所定差以内である場合、ＩＣＰループ処理の終了条件が満たされたと判定する。そして、収束判定部３４は、判定結果を切替部３６に供給する。

【0055】

変換行列更新部３５は、特異値分解部２８がＩＣＰループ処理のループ毎に算出する暫定変換行列に基づき、現在のフレームにおける累計での回転及び並進を表す変換行列を算出する。

【0056】

切替部３６は、収束判定部３４から供給される判定結果に基づき、ＩＣＰループ処理の終了を検知した場合に、変換行列更新部３５から供給される変換行列を、求めるべき変換行列として出力する。そして、切替部３６が出力した変換行列に基づき、車載機１は、地図座標系でのライダ３の位置（姿勢を含む）を認識する。なお、ライダ３の車両に対する相対的な位置（姿勢を含む）を示す設置情報が記憶部１２等に記憶されている場合には、車載機１は、設置情報と、上述の変換行列とに基づき、車両の位置を推定してもよい。

【0057】

（３－３）処理フロー
図４は、本実施例における位置推定処理の手順を示すフローチャートの一例である。車載機１の制御部１５は、図４に示すフローチャートの処理を、例えば、スキャンデータのフレーム周期に従い、繰り返し実行する。

【0058】

まず、制御部１５は、点群地図データ１０の読み込みを行う（ステップＳ１１）。これにより、制御部１５は、３Ｄ空間地図データＤｍ１と高反射強度地図データＤｍ２とを取得する。この場合、制御部１５は、３Ｄ空間地図データＤｍ１と高反射強度地図データＤｍ２とを夫々識別可能に含む点群地図データ１０を参照してもよく、「（３－５）地図データ処理」のセクションにおいて後述する手法により、点群地図データ１０から３Ｄ空間地図データＤｍ１と高反射強度地図データＤｍ２とを生成してもよい。

【0059】

次に、制御部１５は、３Ｄ空間地図データＤｍ１の各点の法線ベクトルを算出する（ステップＳ１２）。さらに、制御部１５は、ＩＣＰループ処理の収束判定に用いるパラメータ（ＩＣＰ収束パラメータ）をセットする（ステップＳ１３）。ここでは、制御部１５は、ＩＣＰ収束パラメータとして、ループ回数最大値「Ｍａｘ＿ｉｔｅｒａｔｉｏｎ」と、収束判定閾値「ｅｐｓｉｌｏｎ」とをセットする。なお、ステップＳ１１～ステップＳ１３の処理は、システムの初期化処理に相当する。

【0060】

次に、制御部１５は、フレーム周期によるスキャンデータの入力を受け付ける（ステップＳ１４）。この場合、制御部１５は、ライダ３が生成したスキャンデータを、フレーム単位により取得し、ステップＳ１５～ステップＳ１７の処理を実行する。そして、制御部１５は、ステップＳ１４で入力されたスキャンデータから、３Ｄ空間スキャンデータＤｓ１と高反射強度スキャンデータＤｓ２とを生成する（ステップＳ１５）。

【0061】

次に、制御部１５は、Ｃｏｍｂｉｎｅｄ－ＩＣＰ処理を実行する（ステップＳ１６）。Ｃｏｍｂｉｎｅｄ－ＩＣＰ処理の詳細については、図５のフローチャートを参照して具体的に説明する。そして、制御部１５は、ステップＳ１６のＣｏｍｂｉｎｅｄ－ＩＣＰ処理で得られた推定位置及び姿勢に関する情報を出力する（ステップＳ１７）。推定位置及び姿勢に関する情報は、例えば、自動運転や経路案内などの運転支援を行う処理部に供給される。

【0062】

図５は、ステップＳ１６のＣｏｍｂｉｎｅｄ－ＩＣＰ処理の手順を示すフローチャートの一例である。

【0063】

まず、制御部１５は、３Ｄ空間スキャンデータＤｓ１をセットする（ステップＳ２１）。また、制御部１５は、高反射強度スキャンデータＤｓ２をセットする（ステップＳ２２）。そして、制御部１５は、ＩＣＰループ処理に用いる変数（ＩＣＰループ処理用変数）を初期化する（ステップＳ２３）。具体的には、求めるべき変換行列の要素である回転行列Ｒ及び並進ベクトルを、夫々「Ｒ＝Ｉ」、「Ｔ＝０」、ループ回数カウンタ「Ｎ＿ｉｔｅｒ」を「０」にセットする。

【0064】

そして、制御部１５は、ＩＣＰループ処理を実行する（ステップＳ２４）。ＩＣＰループ処理の詳細については、図６を参照して説明する。

【0065】

そして、制御部１５は、ＩＣＰ収束時の変換行列［Ｒ Ｔ］を出力する（ステップＳ２５）。そして、制御部１５は、ステップＳ２５で出力した回転行列Ｒに基づき、点群地図データ１０において採用される地図座標系でのライダ３の姿勢を表す３軸周りの回転角度「φ」、「θ」、「ψ」を算出する（ステップＳ２６）。さらに、制御部１５は、並進ベクトル「Ｔ＝（ｔｘ、ｔｙ、ｔｚ）」を、ライダ３の相対位置として抽出する（ステップＳ２７）。これにより、地図座標系の原点とライダ３が基準とするライダ座標系の原点との相対位置関係が特定される。

【0066】

図６は、ステップＳ２４で実行されるＩＣＰループ処理の手順を示すフローチャートの一例である。

【0067】

まず、制御部１５は、３Ｄ空間スキャンデータＤｓ１と３Ｄ空間地図データＤｍ１との対応付けを行う（ステップＳ３１）。この場合、制御部１５は、３Ｄ空間スキャンデータＤｓ１に含まれている各点に対して、３Ｄ空間地図データＤｍ１内の最近傍点を、ＫＤ－Ｔｒｅｅなどの任意の探索アルゴリズムを用いて探索する。次に、制御部１５は、高反射強度スキャンデータＤｓ２と高反射強度地図データＤｍ２との対応付けを行う（ステップＳ３２）。この場合、制御部１５は、高反射強度スキャンデータＤｓ２に含まれている各点に対して、高反射強度地図データＤｍ２内の最近傍点を、ＫＤ－Ｔｒｅｅなどの任意の探索アルゴリズムを用いて探索する。

【0068】

次に、制御部１５は、３Ｄ空間データ用の連立方程式である第１連立方程式「Ａｘ－ｂ」を構築する（ステップＳ３３）。ここで、「ｘ」は、変換行列に関するパラメータのベクトル、「ｂ」は、３Ｄ空間地図データＤｍ１と３Ｄ空間スキャンデータＤｓ１とで対応付けられた点同士の差分と対応する法線ベクトルとの内積を成分とするベクトル、「Ａ」は、３Ｄ空間地図データＤｍ１と３Ｄ空間スキャンデータＤｓ１と法線ベクトルとにより算出される行列を示す。なお、第１連立方程式（即ち、Ｐ２ＰＬアルゴリズムに基づく連立方程式）の詳細については、例えば、非特許文献１に開示されている。

【0069】

また、制御部１５は、高反射強度データ用の連立方程式である第２連立方程式「Ａ＾ｉｘ＝ｂ＾ｉ」を構築する（ステップＳ３４）。ここで、「ｉ」は、高反射強度スキャンデータＤｓ２の点のインデックスを示し、ベクトル「ｂ＾ｉ」は、インデックスｉの点についての高反射強度スキャンデータＤｓ２と高反射強度地図データＤｍ２との点同士の差分ベクトル、「Ａ＾ｉ」は、高反射強度スキャンデータＤｓ２のインデックスｉの点の位置座標に基づく行列を示す。第２連立方程式（即ち、Ｐ２Ｐアルゴリズムに基づく連立方程式）の詳細については、例えば、非特許文献１に開示されている。

【0070】

そして、制御部１５は、第１連立方程式及び第２連立方程式を統合した統合連立方程式を構築する（ステップＳ３５）。なお、Ｐ２ＰＬアルゴリズムに基づく連立方程式と、Ｐ２Ｐアルゴリズムに基づく連立方程式とを統合した連立方程式の構築については、例えば、非特許文献１に開示されている。

【0071】

そして、制御部１５は、特異値分解等を用い、ステップＳ３５で構築した統合連立方程式の解「Ｘｏｐｔ＝（α、β、γ、ｔｘ、ｔｙ、ｔｚ）」を算出する（ステップＳ３６）。そして、制御部１５は、３軸周りの回転に関するパラメータの解（α、β、γ）により、暫定回転行列「Ｒ＿ｔｍｐ」を算出する（ステップＳ３７）。さらに、制御部１５は、３軸の並進に関するパラメータの解（ｔｘ、ｔｙ、ｔｚ）を、暫定的な並進ベクトル「Ｔ＿ｔｍｐ」にセットする（ステップＳ３８）。そして、制御部１５は、ステップＳ３７及びステップＳ３８に基づき得られた暫定変換行列［Ｒ＿ｔｍｐ Ｔ＿ｔｍｐ］を用いて、入力されたスキャンデータ（即ち３Ｄ空間スキャンデータＤｓ１及び高反射強度スキャンデータＤｓ２）を変換した変換スキャンデータを生成する（ステップＳ３９）。

【0072】

そして、制御部１５は、現在のループにおいて算出した暫定変換行列［Ｒ＿ｔｍｐ Ｔ＿ｔｍｐ］に基づき、変換行列［Ｒ Ｔ］を更新する（ステップＳ４０）。具体的には、制御部１５は、更新後のＲを、更新前のＲとＲ＿ｔｍｐとの積とし、更新後のＴを、更新前のＴ及びＲ＿ｔｍｐの積と、Ｔ＿ｔｍｐとの和に設定する。

【0073】

次に、制御部１５は、変換スキャンデータの各点と、対応する地図データの点との二乗誤差に相当する３Ｄ空間二乗誤差「Ｅ_{ＰＴ＿ＰＬ}」と高反射強度二乗誤差「Ｅ_{ＰＴ＿ＰＴ}」を算出する（ステップＳ４１）。そして、制御部１５は、３Ｄ空間二乗誤差Ｅ_{ＰＴ＿ＰＬ}と高反射強度二乗誤差Ｅ_{ＰＴ＿ＰＴ}との重み付け和である総合誤差Ｅ_{＿ｔｏｔａｌ}を算出する（ステップＳ４２）。

【0074】

次に、制御部１５は、ループ回数カウンタＮ＿ｉｔｅｒが０であるか否か判定する（ステップＳ４３）。そして、ループ回数カウンタＮ＿ｉｔｅｒが０である場合（ステップＳ４３；Ｙｅｓ）、即ちＩＣＰループ処理の初回ループである場合、制御部１５は、今回のループでの総合誤差と前回のループでの総合誤差との差「ｄｉｆｆ＿Ｅ＿ｔｏｔａｌ」を無限大にセットする（ステップＳ４４）。一方、ループ回数カウンタＮ＿ｉｔｅｒが０ではない場合（ステップＳ４３；Ｙｅｓ）、即ちＩＣＰループ処理の２回目以降のループである場合、制御部１５は、今回のループでの総合誤差Ｅ_{＿ｔｏｔａｌ}と前回のループでの総合誤差「Ｐｒｅｖ＿Ｅ_{＿ｔｏｔａｌ}」との差分を、差ｄｉｆｆ＿Ｅ＿ｔｏｔａｌとして計算する（ステップＳ４５）。そして、制御部１５は、今回のループでの総合誤差Ｅ_{＿ｔｏｔａｌ}を、前回のループでの総合誤差Ｐｒｅｖ＿Ｅ_{＿ｔｏｔａｌ}として保存する（ステップＳ４６）。そして、制御部１５は、ループ回数カウンタＮ＿ｉｔｅｒのインクリメントを行う（ステップＳ４７）。

【0075】

そして、制御部１５は、ＩＣＰループ処理の終了条件に関する判定を行う（ステップＳ４８）。具体的には、制御部１５は、差ｄｉｆｆ＿Ｅ＿ｔｏｔａｌが収束判定閾値ｅｐｓｉｌｏｎより大きく、かつ、ループ回数カウンタＮ＿ｉｔｅｒがループ回数最大値Ｍａｘ＿ｉｔｅｒａｔｉｏｎ未満であるか否か判定する（ステップＳ４８）。そして、差ｄｉｆｆ＿Ｅ＿ｔｏｔａｌが収束判定閾値ｅｐｓｉｌｏｎより大きく、かつ、ループ回数カウンタＮ＿ｉｔｅｒがループ回数最大値Ｍａｘ＿ｉｔｅｒａｔｉｏｎ未満である場合（ステップＳ４８；Ｙｅｓ）、制御部１５は、ＩＣＰループ処理の終了条件が満たされていないと判定する。よって、この場合、制御部１５は、ステップＳ３１へ処理を戻す。一方、差ｄｉｆｆ＿Ｅ＿ｔｏｔａｌが収束判定閾値ｅｐｓｉｌｏｎ以下、又は、ループ回数カウンタＮ＿ｉｔｅｒがループ回数最大値Ｍａｘ＿ｉｔｅｒａｔｉｏｎ以上である場合（ステップＳ４８；Ｎｏ）、制御部１５は、ＩＣＰループ処理の終了条件が満たされたと判定する。よって、この場合、制御部１５は、ＩＣＰループ処理を終了する。

【0076】

（３－４）スキャンデータ処理
次に、図３のスキャンデータ処理部２３が実行する処理であって、図４のステップＳ１５に相当する処理であるスキャンデータ処理の詳細について説明する。

【0077】

図７は、スキャンデータ処理の概要を示す図である。図７は、ライダ３が生成するスキャンデータから、３Ｄ空間スキャンデータＤｓ１と、高反射強度スキャンデータＤｓ２とを夫々生成する処理の流れを示している。ライダ３が生成するスキャンデータの各点は、ライダ座標系における３軸（Ｘ，Ｙ，Ｚ）の座標値と、反射強度とを含んでいる。

【0078】

まず、第１マッチングに使用する３Ｄ空間スキャンデータＤｓ１の生成について説明する。図７に示されるように、スキャンデータ処理部２３は、必要なグリッドサイズのスキャンデータになるように、スキャンデータに対してＶｏｘｅｌ Ｇｒｉｄ Ｆｉｌｔｅｒ等の間引き処理（即ちダウンサンプリング）を適用し、３Ｄ空間スキャンデータＤｓ１を生成する。また、不要であればスキャンデータに付いていた反射強度の情報は３Ｄ空間スキャンデータＤｓ１において削除しても良いし、処理上で問題なければ、残しておいても良い。

【0079】

次に、第２マッチングに使用する高反射強度スキャンデータＤｓ２の生成について説明する。スキャンデータ処理部２３は、図７に示すように、道路面抽出処理、高反射強度抽出処理、及びＶｏｘｅｌ Ｇｒｉｄ Ｆｉｌｔｅｒ等の間引き処理をスキャンデータに対して行うことで、高反射強度スキャンデータＤｓ２を生成する。以下、その詳細について説明する。

【0080】

スキャンデータ処理部２３は、まず、スキャンデータに対し、道路面を抽出する処理である道路面抽出処理を行う。スキャンデータ処理部２３は、例えば、道路面抽出処理として、
（ａ）ＭＬＳ（Moving Least Squares）による平滑化と法線算出、
（ｂ）法線方向のフィルタリング、
（ｃ）高さを基準とする各点のヒストグラム作成、
（ｄ）高さを基準とするフィルタリング処理
を順次行う。「（ｂ）法線方向のフィルタリング」では、スキャンデータ処理部２３は、例えば、（ａ）において算出した法線が垂直に近い（即ち法線と所定角度差以内となる）点のみを残す処理を行う。また、「（ｃ）高さを基準とする各点のヒストグラム作成」では、スキャンデータ処理部２３は、スキャンデータの高さ方向の成分（例えばＺ座標値）に基づき、ヒストグラムを作成する。「（ｄ）高さを基準とするフィルタリング処理」では、スキャンデータ処理部２３は、（ｃ）で生成したヒストグラムに基づき、道路面の高さに対応する各点を抽出する。この場合、スキャンデータ処理部２３は、例えば、ヒストグラムにおいて最も度数が大きい（及びそれに近似する）高さに属する点を、道路面の点として抽出する。

【0081】

さらに、スキャンデータ処理部２３は、スキャンデータから道路面抽出処理により抽出された点（「道路面候補点」とも呼ぶ。）に対し、反射強度が相対的に大きい点の抽出処理である高反射強度抽出処理を行う。スキャンデータ処理部２３は、例えば、高反射強度抽出処理として、
（ｅ）道路面候補点の反射強度を基準とするヒストグラム作成、
（ｆ）ヒストグラムの分布に基づく反射強度閾値の選択
（ｇ）反射強度閾値Ｉｔｈより大きな反射強度値となる点の抽出
（ｈ）ＳＯＲ（Statistical Outlier Removal）などのノイズ除去処理、
を順次行う。

【0082】

図８は、「（ｅ）道路面候補点の反射強度を基準とするヒストグラム作成」において生成されるヒストグラムの一例を示す。図８では、スキャンデータ処理部２３は、反射強度に基づき、道路面候補点のヒストグラムを生成している。ここで、図８に示すヒストグラムでは、反射強度「Ｉａ」がピークとなっている。この場合、スキャンデータ処理部２３は、（ｆ）において決定する反射強度閾値「Ｉｔｈ」を、ピークの反射強度Ｉａに基づき決定する。例えば、スキャンデータ処理部２３は、ピークの反射強度Ｉａより所定値又は所定率だけ大きい反射強度を、反射強度閾値Ｉｔｈとして決定する。この場合、上述の所定値又は所定率は、例えば、使用するライダ３の種類に応じて予め定められ、スキャンデータ処理部２３が参照できるように記憶部１２に記憶されている。そして、反射強度閾値Ｉｔｈの設定後、スキャンデータ処理部２３は、上述の（ｇ）に基づき、反射強度閾値Ｉｔｈより大きな反射強度値となる道路面候補点を、路面ペイントの候補点として抽出する。その後、スキャンデータ処理部２３は、さらに、（ｈ）に基づき、抽出した路面ペイントの候補点に対してＳＯＲなどのノイズ除去処理を行う。

【0083】

次に、スキャンデータ処理部２３は、高反射強度抽出処理により抽出した点に対して、必要に応じてＶｏｘｅｌ Ｇｒｉｄ Ｆｉｌｔｅｒ等による間引き処理と、必要に応じて反射強度の情報の削除を行う。これにより、スキャンデータ処理部２３は、道路面上の高反射強度を有する点から構成される高反射強度スキャンデータＤｓ２を生成する。

【0084】

なお、スキャンデータ処理は、適用するシステムによってはリアルタイムで行う必要がある。例えば、リアルタイム自車位置・姿勢推定に適用する場合などはそれに該当する。オフラインのＳＬＡＭ（Simultaneous Localization and Mapping）による地図生成など、リアルタイム性が要求されない場合はその限りではない。

【0085】

（３－５）地図データ処理
次に、図３の地図データ処理部２０が実行する３Ｄ空間地図データＤｍ１及び高反射強度地図データＤｍ２の生成処理である地図データ処理の詳細について説明する。

【0086】

図９は、地図データ処理の概要を示す図である。図９は、点群地図データ１０から、３Ｄ空間地図データＤｍ１と高反射強度地図データＤｍ２とを夫々生成する処理の流れを示している。点群地図データ１０を構成する各点は、ライダ座標系における３軸（Ｘ，Ｙ，Ｚ）の座標値と、反射強度とを含んでいる。

【0087】

まず、第１マッチングに使用する３Ｄ空間地図データＤｍ１の生成について説明する。図９に示されるように、地図データ処理部２０は、必要なグリッドサイズのスキャンデータになるように、スキャンデータに対してＶｏｘｅｌ Ｇｒｉｄ Ｆｉｌｔｅｒ等の間引き処理を適用し、３Ｄ空間地図データＤｍ１を生成する。また、不要であればスキャンデータに付いていた反射強度の情報は３Ｄ空間地図データＤｍ１において削除しても良いし、処理上で問題なければ、残しておいても良い。

【0088】

次に、第２マッチングに使用する高反射強度地図データＤｍ２の生成について説明する。スキャンデータ処理部２３は、図９に示すように、道路面抽出処理、高反射強度抽出処理、及びＶｏｘｅｌ Ｇｒｉｄ Ｆｉｌｔｅｒ等の間引き処理を点群地図データ１０に対して行うことで、高反射強度地図データＤｍ２を生成する。以下、その詳細について説明する。

【0089】

スキャンデータ処理部２３は、まず、点群地図データ１０に対し、道路面を抽出する処理である道路面抽出処理を行う。地図データ処理部２０は、例えば、道路面抽出処理として、
（ｉ）ＭＬＳによる平滑化と法線算出、
（ｊ）法線方向のフィルタリング、
（ｋ）クラスタリング
（ｌ）道路面に相当するクラスタの抽出
を順次行う。ここで、（ｉ）及び（ｊ）は、「（３－４）スキャンデータ処理」の（ａ）及び（ｂ）と同一処理となる。「（ｋ）クラスタリング」では、地図データ処理部２０は、少なくとも高さを基準とする任意のクラスタリング手法を実行することで、少なくとも高さが近似する点同士の集まりである複数個のクラスタを生成する。そして、地図データ処理部２０は、「（ｌ）道路面に相当するクラスタの抽出」では、例えば、点数が所定の閾値より少ないクラスタを、ノイズ（例えば中央分離帯の平面部分等を含む）とみなして削除する処理を行う。

【0090】

さらに、地図データ処理部２０は、点群地図データ１０から道路面抽出処理により抽出された道路面候補点に対し、反射強度が相対的に大きい点の抽出処理である高反射強度抽出処理を行う。スキャンデータ処理部２３は、例えば、高反射強度抽出処理として、
（ｍ）道路面候補点の反射強度を基準とするヒストグラム作成、
（ｎ）ヒストグラムの分布に基づく反射強度閾値の選択
（ｏ）反射強度閾値Ｉｔｈより大きな反射強度値となる点の抽出
を順次行う。（ｍ）～（ｏ）は、例えば、「（３－４）スキャンデータ処理」の（ｅ）～（ｇ）と同一処理となる。

【0091】

次に、地図データ処理部２０は、高反射強度抽出処理により抽出した点に対して、必要に応じてＶｏｘｅｌ Ｇｒｉｄ Ｆｉｌｔｅｒ等による間引き処理と、必要に応じて反射強度の情報の削除を行う。これにより、地図データ処理部２０は、高反射強度地図データＤｍ２を生成する。

【0092】

なお、地図データ処理部２０による地図データ処理は必ずしもリアルタイムで行う場合に限定されない。これに代えて、車載機１は、予めオフライン処理により生成済みの３Ｄ空間地図データＤｍ１及び高反射強度地図データＤｍ２を、図４のステップＳ１１において制御部１５が読み込めるように記憶部１２等に予め記憶してもよい。

【0093】

（４）技術的効果
次に、本実施例の技術的効果について説明する。

【0094】

第１の効果として、路面ペイントの活用が挙げられる。本実施例では、車載機１は、３次元空間データ、及び、路面ペイントを示す高反射強度データの両方を活用してＩＣＰに基づくマッチングを行っている。一方、一般的なＩＣＰでは、物体の形状情報のみ（３次元座標値のみ）を用いてマッチングを行うため、道路平面上に特徴的な形状の路面ペイントが存在しても、それらは活用されていない。以上を勘案し、本実施例では、車載機１は、３Ｄ空間地図データＤｍ１及び高反射強度地図データＤｍ２を用意し、３次元空間と路面ペイントの夫々に対するマッチングを行うことで、道路平面上に特徴的な形状の路面ペイントを活用したＩＣＰを好適に実行することができる。

【0095】

第２の効果として、反射強度値の「大きさ」に依存しない路面ペイントのマッチングが挙げられる。一般に、スキャンデータ中の反射強度は，同じ計測対象であっても距離，入射角等で値が変化する。よって、反射強度値そのものを用いるには、距離，入射角に依存しないように、高精度なキャリブレーションが必要となる。以上を勘案し、本実施例では、車載機１は、反射強度の値そのものを用いず、反射強度閾値を用いた閾値処理により、高反射強度の路面ペイントの点を抽出している。これにより、車載機１は、反射強度値そのものを使用せず，高反射強度の形状の3次元座標値を使用してマッチング処理を行うことができる。

【0096】

第３の効果として、路面ペイントのマッチング（第２マッチング）にＰ２Ｐアルゴリズムを使用することによる高精度なマッチングの実現が挙げられる。一般的な３次元空間については、Ｐ２ＰＬアルゴリズムがＰ２Ｐアルゴリズムよりも高い精度でのマッチングが可能であることが知られている。しかしながら、後述する理由により、道路平面上の路面ペイントのマッチングでは、Ｐ２ＰＬアルゴリズムは不適切であると考えられる。以上を勘案し、本実施例では、車載機１は、３次元空間のマッチングである第１マッチングをＰ２ＰＬアルゴリズムの評価指標を用いて行い、路面ペイントのマッチングである第２マッチングをＰ２Ｐアルゴリズムの評価指標を用いて行うＣｏｍｂｉｎｅｄ ＩＣＰアルゴリズムを実行する。これにより、車載機１は、高精度にマッチングを行うことができる。

【0097】

ここで、道路平面上の路面ペイントのマッチングではＰ２ＰＬアルゴリズムは不適切となる理由について説明する。マッチングさせる対応点同士が道路平面にある場合，それらの点を結ぶベクトルと、道路平面の法線が垂直になるため、Ｐ２ＰＬの評価指標がゼロとなり、ずれていることが検出できない。具体的には、ＩＣＰループ処理のループにおいて、スキャンデータを道路平面とほぼ一致する高さまで追い込めたとすると（この場合，残りは横方向のマッチングとなる）、ソースとなる高反射強度スキャンデータＤｓ２と、ターゲットとなる高反射強度地図データＤｍ２との対応点のベクトルは、ほぼ道路面に平行の向きとなる。ここで、ＰＴＰＬアルゴリズムでは、この対応点間ベクトルと、ターゲットとなる高反射強度地図データＤｍ２の点の法線との内積の和に相当する評価関数の最小化を行うため、その評価関数がほぼ０となる。よって、ＰＴＰＬアルゴリズムでは、平面のＺ方向がほぼマッチングした後は、横方向（Ｘ、Ｙ方向）のズレ量が検出できないことになる。以上を勘案し、車載機１は、路面ペイントのマッチングである第２マッチングをＰ２Ｐアルゴリズムの評価指標を用いて行う。

【0098】

次に、本実施例の手法に基づくシミュレーション結果について説明する。

【0099】

図１０は、本シミュレーションにおいて使用する点群地図データ１０の各点を３次元空間にプロットした状態を表す斜視図である。図１０では、反射強度が高い点ほど、白くなるように表示されている。図１０に示すように、白線などの路面ペイントの反射強度は、相対的に高くなっている。なお、本シミュレーションでは、地面から高さ７０ｃｍ以上となる点を削除した点群地図データ１０及びスキャンデータを用いている。これは、周辺に構造物が少なくガードレール程度しか存在しない状態を想定したものである。

【0100】

図１１は、本実施例に基づく手法との比較のための比較例におけるマッチング結果を示す。比較例では、第２マッチングを行わず、Ｐ２ＰＬアルゴリズムにより３Ｄ空間地図データＤｍ１と３Ｄ空間スキャンデータＤｓ１とのマッチングを行う第１マッチングに基づき、変換行列を算出している。図１１では、マッチング結果に基づき変換した３Ｄ空間スキャンデータＤｓ１の点を白により示し、点群地図データ１０の点をグレー（薄い白）により示している。この場合、円７０に示される範囲において、ズレ（上下方向のズレ、反時計回りへの若干の回転ずれ）が見られる。

【0101】

図１２は、本実施例に基づくマッチング手法による（即ち、Ｃｏｍｂｉｎｅｄ ＩＣＰアルゴリズムによる）マッチング結果を示す図である。図１２では、マッチング結果に基づき変換したスキャンデータの点を白により示し、点群地図データ１０の点をグレー（薄い白）により示している。図１２に示すように、この場合、本実施例に基づくマッチング手法により変換したスキャンデータは、点群地図データ１０と殆どズレが存在せず、良好なマッチング結果が得られていることが把握できる。

【0102】

図１３は、本実施例に基づくマッチング手法を実行した場合において、Ｐ２Ｐアルゴリズムにより高反射強度地図データＤｍ２と高反射強度スキャンデータＤｓ２とのマッチング結果を示した図である。図１３では、マッチング結果に基づき変換した高反射強度スキャンデータＤｓ２の点を白により示し、高反射強度地図データＤｍ２の点をグレー（薄い白）により示している。図１３に示すように、本実施例に基づくマッチング手法に基づき変換した高反射強度スキャンデータＤｓ２は、高反射強度地図データＤｍ２と殆どズレが存在せず、良好なマッチング結果が得られていることが把握できる。

【0103】

図１４は、実施例の手法（即ちＣｏｍｂｉｎｅｄ ＩＣＰアルゴリズム）と、比較例の手法（即ちＰ２ＰＬアルゴリズム）とを実行した場合の、ＩＣＰループ処理の反復回数と二乗誤差との関係を示すグラフである。図１４に示すように、ＩＣＰループ処理の反復回数を重ねるにつれて、本実施例の手法の方が比較例の手法よりも誤差が小さくなることがわかる。また、比較例の手法を実行した場合の誤差と、本実施例の手法を実行した場合の誤差を１つの点（計測点）あたりで比較すると、前者の誤差は、「０．１２ｍ」（点の数６７０６）となり、後者の誤差は、「０．１０ｍ」（点の数６７１１）となった。このように、本実施例の手法は、路面ペイントの形状をマッチング時の特徴として活用できたため，空間に周辺構造物が少ない状態でもマッチングさせることが出来き、その結果としてＰ２ＰＬアルゴリズムのみを採用する比較例の手法と比較し、良好な結果が得られたものと考えられる。

【0104】

（５）変形例
以下、上述の実施例に好適な変形例について説明する。以下の変形例は、組み合わせてこれらの実施例に適用してもよい。

【0105】

（第１変形例）
制御部１５は、第１マッチングと第２マッチングとにおいて使用するデータの１点あたりのグリッドサイズ（即ち、Ｖｏｘｅｌ Ｇｒｉｄ Ｆｉｌｅｒにおいて定めるグリッドサイズ）を異ならせてもよい。これにより処理時間と精度とのバランスを調整することが出来る。例えば、第２マッチングに用いる高反射強度スキャンデータＤｓ２及び高反射強度地図データＤｍ２のグリッドサイズを、第１マッチングに用いる３Ｄ空間スキャンデータＤｓ１及び３Ｄ空間地図データＤｍ１のグリッドサイズよりも小さくすることで、第２マッチングを重視したマッチング結果を得ることができる。

【0106】

（第２変形例）
制御部１５は、ライダ３が出力するスキャンデータに含まれる反射強度を、計測方向又は／及び計測距離に応じて補正してもよい。

【0107】

この場合、例えば、制御部１５は、計測方向及び計測距離ごとに適切な補正量を示すルックアップテーブル等を予め記憶部１２に記憶しておく。そして、制御部１５は、当該ルックアップテーブル等を参照することで、ライダ３が出力するスキャンデータの各点の反射強度を、対応する計測方向又は／及び計測距離に応じて補正する。これにより、制御部１５は、より正確な反射強度の情報を含むスキャンデータに基づき、本実施例に基づくマッチングを実行することができる。

【0108】

（第３変形例）
制御部１５は、高反射強度スキャンデータＤｓ２及び高反射強度地図データＤｍ２に基づく第２マッチングにおいて、路面ペイントに加えて、他の高反射強度を有する地物を基準としてマッチングを行ってもよい。

【0109】

この場合、高反射強度スキャンデータＤｓ２及び高反射強度地図データＤｍ２には、路面ペイントの点に加えて、方面看板などの標識、ガードレール、高反射強度のペイントがなされたその他の地物の表面（道路境界のコンクリートウォール、トンネル内の壁等を含む。）の点も含まれる。この場合、制御部１５は、高反射強度スキャンデータＤｓ２及び高反射強度地図データＤｍ２の生成処理において、図７及び図９に示した路面ペイントの点を抽出する処理に加えて、上述した高反射強度の点を抽出する処理を実行する。この場合、制御部１５は、後者の処理として、例えば、対象となる高反射強度の点が存在する基準面を抽出する処理（路面ペイントにおける道路面抽出処理）、及び、高反射強度抽出処理等を行うことで、スキャンデータ及び点群地図データ１０から、対象となる高反射強度の地物の点を抽出する。

【0110】

本変形例によれば、制御部１５は、路面ペイントを含む高反射強度の点（高反射強度データ）を利用し、マッチング精度を好適に向上させることができる。なお、本変形例において、道路面及び高反射強度のペイントがなされた地物の表面は、「基準面」の一例である。

【0111】

（第４変形例）
制御部１５は、地図データ処理部２０及びスキャンデータ処理部２３において使用する反射強度閾値を、ヒストグラムに応じて適応に定める代わりに、予め記憶部１２に記憶された所定値に定めてもよい。この場合、上述の所定値は、例えば、路面ペイント以外の道路面の反射強度より高く、かつ、路面ペイントの反射強度未満となるような値に予め決定され、記憶部１２に記憶されている。

【0112】

（第５変形例）
制御部１５は、第１マッチングと第２マッチングとを組み合わせたＣｏｍｂｉｎｅｄ ＩＣＰアルゴリズムを実行する代わりに、第２マッチングのみを実行してもよい。この場合、制御部１５は、高反射強度スキャンデータＤｓ２と高反射強度地図データＤｍ２とを、Ｐ２Ｐアルゴリズムに基づきマッチングさせることで、変換行列を算出する。この場合であっても、制御部１５は、３次元構造を持つ物体が数多く存在しない環境であっても、地図データとスキャンデータとの照合を好適に実行することができる。

【0113】

（第６変形例）
図１に示す運転支援システムの構成は一例であり、本発明が適用可能な運転支援システムの構成は図１に示す構成に限定されない。例えば、運転支援システムは、車載機１を有する代わりに、車両の電子制御装置が図３に示されるブロック図の処理を実行してもよい。この場合、点群地図データ１０は、例えば車両内の記憶部又は車両とデータ通信を行うサーバ装置に記憶され、車両の電子制御装置は、この点群地図データ１０を参照することで、ライダ３が出力するスキャンデータとのマッチング処理などを実行する。

【0114】

以上説明したように、本実施例に係る情報処理装置は、点群地図データ取得手段と、スキャンデータ取得手段と、スキャンデータ処理手段と、照合手段とを有する。点群地図データ取得手段は、反射強度が閾値以上の点群データである点群地図データを取得する。スキャンデータ取得手段は、出射した光の反射光を受光することで計測した点群データであるスキャンデータを取得する。スキャンデータ処理手段は、スキャンデータから、反射強度が所定の閾値以上となる高反射強度スキャンデータを生成する。照合手段は、点群地図データと高反射強度スキャンデータとの照合を行う。これにより、情報処理装置は、３次元構造を持つ物体が数多く存在しない環境であっても、地図データとスキャンデータとの照合を好適に実行することができる。

【0115】

以上、実施形態を参照して本願発明を説明したが、本願発明は上記実施形態に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。すなわち、本願発明は、請求の範囲を含む全開示、技術的思想にしたがって当業者であればなし得るであろう各種変形、修正を含むことは勿論である。また、引用した上記の特許文献等の各開示は、本書に引用をもって繰り込むものとする。

【符号の説明】

【0116】

１ 車載機
２ センサ群
３ ライダ
４ ＧＰＳ受信機
５ ジャイロセンサ
６ 車速センサ
１０ 点群地図データ

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】