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

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

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

(11)【公開番号】P2024056033

(43)【公開日】2024-04-19

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

(51)【国際特許分類】

   G01C 21/28 20060101AFI20240412BHJP

【ＦＩ】

G01C21/28

【審査請求】有

【請求項の数】1

【出願形態】ＯＬ

(21)【出願番号】P 2024034515

(22)【出願日】2024-03-07

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

【原出願日】2021-11-08

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

(32)【優先日】2020-11-13

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

(71)【出願人】

【識別番号】000005016

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

(71)【出願人】

【識別番号】520001073

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

(74)【代理人】

【識別番号】100107331

【弁理士】

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

(72)【発明者】

【氏名】加藤 正浩

(72)【発明者】

【氏名】鈴木 雅美

(57)【要約】

【課題】ダウンサンプリングを好適に実行可能な情報処理装置を提供する。
【解決手段】車載機１のコントローラ１３は、ライダ２が出力する点群データを取得する。そして、コントローラ１３は、点群データに対してダウンサンプリングを行った加工点群データを生成する。そして、コントローラ１３は、加工点群データと、単位領域であるボクセルごとの物体の位置を表すボクセルデータＶＤとの照合により、加工点群データを構成する計測点とボクセルの各々との対応付けを行う。そして、コントローラ１３は、加工点群データの計測点のうちボクセルと対応付けられた計測点の数である対応計測点数Ｎｃに基づき、次に実行されるダウンサンプリングのサイズを変更する。
【選択図】図１２

【特許請求の範囲】

【請求項1】

計測装置が出力する点群データを取得する取得手段と、
前記点群データに対してダウンサンプリングを行った加工点群データを生成するダウンサンプリング処理手段と、
前記加工点群データと、単位領域であるボクセルごとの物体の位置を表すボクセルデータとの照合により、前記加工点群データを構成する計測点と前記ボクセルの各々との対応付けを行う対応付け手段と、を有する情報処理装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、位置推定に用いるデータのダウンサンプリングに関する。

【背景技術】

【0002】

従来から、レーザスキャナなどの計測装置を用いて計測した周辺物体の形状データを、予め周辺物体の形状が記憶された地図情報と照合（マッチング）することで、車両の自己位置を推定する技術が知られている。例えば、特許文献１には、空間を所定の規則で分割したボクセル中における検出物が静止物か移動物かを判定し、静止物が存在するボクセルを対象として地図情報と計測データとのマッチングを行う自律移動システムが開示されている。また、特許文献２には、ボクセル毎の静止物体の平均ベクトルと共分散行列とを含むボクセルデータとライダが出力する点群データとの照合により自車位置推定を行うスキャンマッチング手法が開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】国際公開ＷＯ２０１３／０７６８２９

【特許文献2】国際公開ＷＯ２０１８／２２１４５３

【発明の概要】

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

【0004】

特許文献２のように、ボクセルデータとライダが出力する点群データとの照合により自車位置推定を行う場合、ライダの点群データを所定サイズに分割した空間で平均化することで間引く処理であるダウンサンプリング処理が行われる。このダウンサンプリングを行うことで、自車に近いエリアは密、遠いエリアは疎となる点群データの密度を一様にし、かつ全体の点群データ数を低減することができるため、自己位置推定精度の向上と計算時間の抑制を両立させている。しかしながら、自車周辺に物体が少ない空間では、ライダで検出される点群データ数が少ないため、ダウンサンプリング処理によって、計算に用いる点群データ数が大きく減少してしまうこともあり、自己位置推定の精度やロバスト性が低下してしまう問題がある。

【0005】

本発明は、上記のような課題を解決するためになされたものであり、ダウンサンプリングを好適に実行可能な情報処理装置を提供することを主な目的とする。

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

【0006】

請求項に記載の発明は、
計測装置が出力する点群データを取得する取得手段と、
前記点群データに対してダウンサンプリングを行った加工点群データを生成するダウンサンプリング処理手段と、
前記加工点群データと、単位領域であるボクセルごとの物体の位置を表すボクセルデータとの照合により、前記加工点群データを構成する計測点と前記ボクセルの各々との対応付けを行う対応付け手段と、を有し、
前記ダウンサンプリング処理手段は、前記計測点のうち前記ボクセルと対応付けられた計測点の数である対応計測点数に基づき、次に実行される前記ダウンサンプリングのサイズを変更する、情報処理装置である。

【0007】

また、請求項に記載の発明は、
コンピュータが実行する制御方法であって、
計測装置が出力する点群データを取得し、
前記点群データに対してダウンサンプリングを行った加工点群データを生成し、
前記加工点群データと、単位領域であるボクセルごとの物体の位置を表すボクセルデータとの照合により、前記加工点群データを構成する計測点と前記ボクセルの各々との対応付けを行い、
前記計測点のうち前記ボクセルと対応付けられた計測点の数である対応計測点数に基づき、次に実行される前記ダウンサンプリングのサイズを変更する、制御方法である。

【0008】

また、請求項に記載の発明は、
計測装置が出力する点群データを取得し、
前記点群データに対してダウンサンプリングを行った加工点群データを生成し、
前記加工点群データと、単位領域であるボクセルごとの物体の位置を表すボクセルデータとの照合により、前記加工点群データを構成する計測点と前記ボクセルの各々との対応付けを行い、
前記計測点のうち前記ボクセルと対応付けられた計測点の数である対応計測点数に基づき、次に実行される前記ダウンサンプリングのサイズを変更する処理をコンピュータに実行させるプログラムである。

【図面の簡単な説明】

【0009】

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

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

【図3】ダウンサンプリング処理部１４の機能ブロックの一例を示す。

【図4】夫々異なるダウンサンプリングサイズによるダウンサンプリングを実行した第１の実行結果を示す図である。

【図5】夫々異なるダウンサンプリングサイズによるダウンサンプリングを実行した第２の実行結果を示す図である。

【図6】夫々異なるダウンサンプリングサイズによるダウンサンプリングを実行した第３の実行結果を示す図である。

【図7】車載機を搭載する車両周辺の俯瞰図を示す。

【図8】自車位置推定部が推定すべき自車位置を２次元直交座標で表した図である。

【図9】ボクセルデータの概略的なデータ構造の一例を示す。

【図10】自車位置推定部の機能ブロックの一例である。

【図11】ワールド座標系における２次元平面上での、ボクセルデータが存在するボクセルとこれらのボクセル付近の位置を示す計測点との位置関係を示す。

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

【図13】１辺が０．５ｍの立方格子サイズでダウンサンプリングを行い、ＮＤＴ処理による自車位置推定を行った結果を示す。

【図14】１辺が０．５ｍの立方格子サイズでダウンサンプリングを行い、ＮＤＴ処理による自車位置推定を行った結果を示す。

【図15】１辺が２．０ｍの立方格子サイズでダウンサンプリングを行い、ＮＤＴ処理による自車位置推定を行った結果を示す。

【図16】１辺が２．０ｍの立方格子サイズでダウンサンプリングを行い、ＮＤＴ処理による自車位置推定を行った結果を示す。

【図17】図１２に示す処理手順に従いダウンサンプリング及びＮＤＴ処理による自車位置推定を行った結果を示す。

【図18】図１２に示す処理手順に従いダウンサンプリング及びＮＤＴ処理による自車位置推定を行った結果を示す。

【図19】他車両によるオクルージョンが発生した場合の、図１２に示す処理手順に従いダウンサンプリング及びＮＤＴ処理による自車位置推定を行った結果を示す。

【図20】図１９と同一状況下において、仮にダウンサンプリング後の計測点数を６００～８００に収まるようにダウンサンプリングのサイズを決定した場合の結果を示す。

【図21】変形例１に基づき進行方向のダウンサンプリングのサイズを０．５ｍに固定し、横方向及び高さ方向のダウンサンプリングのサイズを適応的に変更してＮＤＴ処理による自車位置推定を行った結果を示す。

【図22】変形例１に基づき進行方向のダウンサンプリングのサイズを０．５ｍに固定し、横方向及び高さ方向のダウンサンプリングのサイズを適応的に変更してＮＤＴ処理による自車位置推定を行った結果を示す。

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

【0010】

本発明の好適な実施形態によれば、情報処理装置は、計測装置が出力する点群データを取得する取得手段と、前記点群データに対してダウンサンプリングを行った加工点群データを生成するダウンサンプリング処理手段と、前記加工点群データと、単位領域であるボクセルごとの物体の位置を表すボクセルデータとの照合により、前記加工点群データを構成する計測点と前記ボクセルの各々との対応付けを行う対応付け手段と、を有し、前記ダウンサンプリング処理手段は、前記計測点のうち前記ボクセルと対応付けられた計測点の数である対応計測点数に基づき、次に実行される前記ダウンサンプリングのサイズを変更する。この態様によれば、情報処理装置は、ボクセルデータが存在するボクセルと対応付けた加工点群データの計測点数である対応計測点数に基づきダウンサンプリングのサイズを適応的に変更し、対応計測点数を好適に適正化することができる。

【0011】

上記情報処理装置の一態様では、前記ダウンサンプリング処理手段は、前記ダウンサンプリングにおいて空間を分割する方向のうち第１方向における前記サイズを固定とし、前記第１方向以外の方向における前記サイズを前記対応計測点数に基づき変更する。好適な例では、前記第１方向は、前記計測装置の進行方向である。この態様により、情報処理装置は、加工点群データの分解能が必要な方向についてはダウンサンプリングのサイズを固定にして分解能を好適に確保することができる。

【0012】

上記情報処理装置の他の一態様では、前記ダウンサンプリング処理手段は、前記対応計測点数と、当該対応計測点数の目標範囲との比較に基づき、前記サイズを変更する。この態様により、情報処理装置は、ダウンサンプリングのサイズを的確に変更することができる。

【0013】

上記情報処理装置の他の一態様では、前記ダウンサンプリング処理手段は、前記対応計測点数が前記目標範囲の上限よりも多い場合、前記サイズを所定率又は所定値だけ大きくし、前記対応計測点数が前記目標範囲の下限よりも少ない場合、前記サイズを所定率又は所定値だけ小さくし、前記対応計測点数が前記目標範囲内である場合、前記サイズを維持する。この態様により、情報処理装置は、対応計測点数が目標範囲内に維持されるように、ダウンサンプリングのサイズを的確に変更することができる。

【0014】

上記情報処理装置の他の一態様では、前記ダウンサンプリング処理手段は、前記対応計測点数に基づき、次に実行される前記ダウンサンプリングの基準サイズを決定し、当該基準サイズと、前記ダウンサンプリングにおいて空間を分割する方向毎に設定した前記基準サイズに対する比率とに基づき、前記方向毎の前記サイズを決定する。この態様により、情報処理装置は、方向毎の精度や重要度等に応じた比率により、方向毎にダウンサンプリングのサイズを好適に定めることができる。

【0015】

上記情報処理装置の他の一態様では、情報処理装置は、前記対応付け手段による対応付けがなされた前記ボクセルのボクセルデータと、当該ボクセルに対応付けられた計測点との照合結果に基づき、前記計測装置を備える移動体の位置推定を行う位置推定手段をさらに有する。この態様では、情報処理装置は、ダウンサンプリングのサイズを適応的に変更して生成した加工点群データを位置推定に用いることで、位置推定の精度と処理時間の両立を好適に実現することができる。

【0016】

上記情報処理装置の他の一態様では、前記ダウンサンプリング処理手段は、前記ダウンサンプリング処理手段及び前記位置推定手段の処理時間に基づき、前記サイズを変更する。好適な例では、前記ダウンサンプリング処理手段は、前記処理時間と、当該処理時間の目標範囲との比較に基づき、前記サイズを変更するとよい。この態様により、情報処理装置は、ダウンサンプリング処理手段及び位置推定手段の処理時間が適正となるように好適にダウンサンプリングのサイズを設定することができる。

【0017】

本発明の他の好適な実施形態によれば、情報処理装置が実行する制御方法であって、計測装置が出力する点群データを取得し、前記点群データに対してダウンサンプリングを行った加工点群データを生成し、前記加工点群データと、単位領域であるボクセルごとの物体の位置を表すボクセルデータとの照合により、前記加工点群データを構成する計測点と前記ボクセルの各々との対応付けを行い、前記計測点のうち前記ボクセルと対応付けられた計測点の数である対応計測点数に基づき、次に実行される前記ダウンサンプリングのサイズを変更する。情報処理装置は、この制御方法を実行することで、対応計測点数に基づきダウンサンプリングのサイズを適応的に変更し、対応計測点数を好適に適正化することができる。

【0018】

本発明のさらに別の好適な実施形態によれば、プログラムは、計測装置が出力する点群データを取得し、前記点群データに対してダウンサンプリングを行った加工点群データを生成し、前記加工点群データと、単位領域であるボクセルごとの物体の位置を表すボクセルデータとの照合により、前記加工点群データを構成する計測点と前記ボクセルの各々との対応付けを行い、前記計測点のうち前記ボクセルと対応付けられた計測点の数である対応計測点数に基づき、次に実行される前記ダウンサンプリングのサイズを変更する処理をコンピュータに実行させる。コンピュータは、このプログラムを実行することで、対応計測点数に基づきダウンサンプリングのサイズを適応的に変更し、対応計測点数を好適に適正化することができる。好適には、上記プログラムは、記憶媒体に記憶される。

【実施例0019】

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

【0020】

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

【0021】

車載機１は、ライダ２、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５と電気的に接続し、これらの出力に基づき、車載機１が設けられた車両の位置（「自車位置」とも呼ぶ。）の推定を行う。そして、車載機１は、自車位置の推定結果に基づき、設定された目的地への経路に沿って走行するように、車両の自動運転制御などを行う。車載機１は、ボクセルデータ「ＶＤ」を含む地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶する。ボクセルデータＶＤは、３次元空間の最小単位となる立方体（正規格子）を示すボクセルごとに静止構造物の位置情報等を記録したデータである。ボクセルデータＶＤは、各ボクセル内の静止構造物の計測された点群データを正規分布により表したデータを含み、後述するように、ＮＤＴ（Ｎｏｒｍａｌ Ｄｉｓｔｒｉｂｕｔｉｏｎｓ Ｔｒａｎｓｆｏｒｍ）を用いたスキャンマッチングに用いられる。車載機１は、ＮＤＴスキャンマッチングにより少なくとも車両の平面上の位置及びヨー角の推定を行う。なお、車載機１は、車両の高さ位置、ピッチ角及びロール角の推定をさらに行ってもよい。特に言及がない限り、自車位置は、推定対象となる車両のヨー角などの姿勢角も含まれるものとする。

【0022】

ライダ２は、水平方向および垂直方向の所定の角度範囲に対してパルスレーザを出射することで、外界に存在する物体までの距離を離散的に測定し、当該物体の位置を示す３次元の点群データを生成する。この場合、ライダ２は、照射方向を変えながらレーザ光を照射する照射部と、照射したレーザ光の反射光（散乱光）を受光する受光部と、受光部が出力する受光信号に基づくスキャンデータ（点群データを構成する点であり、以後では「計測点」と呼ぶ。）を出力する出力部とを有する。計測点は、受光部が受光したレーザ光に対応する照射方向と、上述の受光信号に基づき特定される当該レーザ光の応答遅延時間とに基づき生成される。なお、一般的に、対象物までの距離が近いほどライダの距離測定値の精度は高く、距離が遠いほど精度は低い。ライダ２、ジャイロセンサ３、車速センサ４、ＧＰＳ受信機５は、それぞれ、出力データを車載機１へ供給する。

【0023】

車載機１は、本発明における「情報処理装置」の一例であり、ライダ２は、本発明における「計測装置」の一例である。なお、運転支援システムは、ジャイロセンサ３に代えて、又はこれに加えて、３軸方向における計測車両の加速度及び角速度を計測する慣性計測装置（ＩＭＵ）を有してもよい。

【0024】

（２）車載機の構成
図２は、車載機１のハードウェア構成の一例を示すブロック図である。車載機１は、主に、インターフェース１１と、メモリ１２と、コントローラ１３と、を有する。これらの各要素は、バスラインを介して相互に接続されている。

【0025】

インターフェース１１は、車載機１と外部装置とのデータの授受に関するインターフェース動作を行う。本実施例では、インターフェース１１は、ライダ２、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５などのセンサから出力データを取得し、コントローラ１３へ供給する。また、インターフェース１１は、コントローラ１３が生成した車両の走行制御に関する信号を車両の電子制御装置（ＥＣＵ：Ｅｌｅｃｔｒｏｎｉｃ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）へ供給する。インターフェース１１は、無線通信を行うためのネットワークアダプタなどのワイヤレスインターフェースであってもよく、ケーブル等により外部装置と接続するためのハードウェアインターフェースであってもよい。インターフェース１１は、入力装置、表示装置、音出力装置等の種々の周辺装置とのインターフェース動作を行ってもよい。

【0026】

メモリ１２は、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ハードディスクドライブ、フラッシュメモリなどの各種の揮発性メモリ及び不揮発性メモリにより構成される。メモリ１２は、コントローラ１３が所定の処理を実行するためのプログラムが記憶される。なお、コントローラ１３が実行するプログラムは、メモリ１２以外の記憶媒体に記憶されてもよい。

【0027】

また、メモリ１２は、目標範囲情報９と、ボクセルデータＶＤを含む地図ＤＢ１０とを記憶する。

【0028】

目標範囲情報９は、ライダ２が１周期分の走査を行った場合に得られる点群データに対してダウンサンプリングを行う場合のダウンサンプリングのサイズの設定に用いられる情報である。具体的には、目標範囲情報９は、ライダ２の走査周期に基づく各処理時刻において行われるＮＤＴマッチングにおいて、ダウンサンプリング後の点群データのうちボクセルデータＶＤと対応付けられた計測点の数（「対応計測点数Ｎｃ」とも呼ぶ。）の目標範囲を表す。以後では、目標範囲情報９が示す対応計測点数Ｎｃの目標範囲を、「目標範囲Ｒ_Ｎｃ」とも呼ぶ。

【0029】

なお、目標範囲情報９又は地図ＤＢ１０の少なくとも一方は、インターフェース１１を介して車載機１と接続されたハードディスクなどの車載機１の外部の記憶装置に記憶されてもよい。上記の記憶装置は、車載機１と通信を行うサーバ装置であってもよい。また、上記の記憶装置は、複数の装置から構成されてもよい。また、地図ＤＢ１０は、定期的に更新されてもよい。この場合、例えば、コントローラ１３は、インターフェース１１を介し、地図情報を管理するサーバ装置から、自車位置が属するエリアに関する部分地図情報を受信し、地図ＤＢ１０に反映させる。

【0030】

コントローラ１３は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＴＰＵ（Ｔｅｎｓｏｒ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などの１又は複数のプロセッサを含み、車載機１の全体を制御する。この場合、コントローラ１３は、メモリ１２等に記憶されたプログラムを実行することで、自車位置推定に関する処理を行う。

【0031】

また、コントローラ１３は、機能的には、ダウンサンプリング処理部１４と、自車位置推定部１５とを有する。そして、コントローラ１３は、「取得手段」、「ダウンサンプリング処理手段」、「対応付け手段」及びプログラムを実行するコンピュータ等として機能する。

【0032】

ダウンサンプリング処理部１４は、ライダ２から出力される点群データに対してダウンサンプリングを行うことで、計測点数を削減するように補正された点群データ（「加工点群データ」とも呼ぶ。）を生成する。このダウンサンプリングでは、ダウンサンプリング処理部１４は、点群データを所定サイズに分割した空間内で平均することで、計測点数を削減した加工点群データを生成する。この場合、ダウンサンプリング処理部１４は、各処理時刻でのＮＤＴマッチングにおける対応計測点数Ｎｃが目標範囲Ｒ_Ｎｃ内に収まるように、ダウンサンプリングにおける上述した所定サイズを適応的に設定する。

【0033】

自車位置推定部１５は、ダウンサンプリング処理部１４が生成する加工点群データと、当該点群データが属するボクセルに対応するボクセルデータＶＤとに基づき、ＮＤＴに基づくスキャンマッチング（ＮＤＴスキャンマッチング）を行うことで、自車位置の推定を行う。

【0034】

（３）ダウンサンプリングサイズの設定
次に、ダウンサンプリング処理部１４の処理の詳細について説明する。概略的には、ダウンサンプリング処理部１４は、各処理時刻において、対応計測点数Ｎｃと、目標範囲情報９が示す目標範囲Ｒ_Ｎｃとをダウンサンプリング後に比較し、その大小結果に応じて次の処理時刻でのダウンサンプリングのサイズを設定する。これにより、ダウンサンプリング処理部１４は、対応計測点数Ｎｃが目標範囲Ｒ_Ｎｃ内に維持されるように適応的にダウンサンプリングのサイズを定める。

【0035】

図３は、ダウンサンプリング処理部１４の機能ブロックの一例を示す。ダウンサンプリング処理部１４は、機能的には、ダウンサンプリングブロック１６と、対応計測点数取得ブロック１７と、比較ブロック１８と、ダウンサンプリングサイズ設定ブロック１９とを有する。なお、図３では、データの授受が行われるブロック同士を実線により結んでいるが、データの授受が行われるブロックの組合せは図３に限定されない。後述する他の機能ブロックの図においても同様である。

【0036】

ダウンサンプリングブロック１６は、ライダ２が処理時刻毎に生成する一周期分の走査の点群データのダウンサンプリングを行うことで、処理時刻毎に加工点群データを生成する。この場合、ダウンサンプリングブロック１６は、設定されたダウンサンプリングのサイズに基づき、空間をグリッドに分割し、夫々のグリッドの中に含まれるライダ２の点群データの計測点の代表点を算出する。この場合、上記空間は、例えば、車載機１の進行方向、高さ方向、及び横方向（即ち進行方向と高さ方向と垂直な方向）を夫々軸とする３次元座標系の空間であり、これらの各方向に沿って等間隔に分割されることでグリッドが形成される。ダウンサンプリングのサイズは、メモリ１２等に記憶された初期サイズ又はダウンサンプリングサイズ設定ブロック１９により直前に設定されたサイズとなる。ダウンサンプリングブロック１６は、生成した加工点群データを自車位置推定部１５に供給する。

【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】

【数1】

【0068】

【数2】

【0069】

次に、ボクセルデータＶＤを用いたＮＤＴスキャンマッチングの概要について説明する。

【0070】

車両を想定したＮＤＴによるスキャンマッチングは、道路平面（ここではｘｙ座標とする）内の移動量及び車両の向きを要素とした推定パラメータ
Ｐ＝［ｔ_ｘ、ｔ_ｙ、ｔ_ψ］^Ｔ
を推定することとなる。ここで、「ｔ_ｘ」は、ｘ方向の移動量を示し、「ｔ_ｙ」は、ｙ方向の移動量を示し、「ｔ_ψ」は、ヨー角を示す。

【0071】

また、ライダ２が出力する点群データの座標を、
Ｘ_Ｌ（ｊ）＝［ｘ（ｊ）、ｙ（ｊ）、ｚ（ｊ）］^Ｔ
とすると、Ｘ_Ｌ（ｊ）の平均値「Ｌ´_ｎ」は、以下の式（３）により表される。

【0072】

【数3】

そして、上述の推定パラメータＰを用い、平均値Ｌ´を座標変換すると、変換後の座標「Ｌ_ｎ」は、以下の式（４）により表される。

【0073】

【数4】

そして、自車位置推定部１５は、地図ＤＢ１０と同一の座標系である絶対的な座標系（「ワールド座標系」とも呼ぶ。）に変換した加工点群データに対応付けられるボクセルデータＶＤを探索し、そのボクセルデータＶＤに含まれる平均ベクトルμ_ｎと共分散行列Ｖ_ｎとを用い、ボクセルｎの評価関数値（「個別評価関数値」とも呼ぶ。）「Ｅ_ｎ」を算出する。この場合、自車位置推定部１５は、以下の式（５）に基づき、ボクセルｎの個別評価関数値Ｅ_ｎを算出する。

【0074】

【数5】

【0075】

そして、自車位置推定部１５は、以下の式（６）により示される、マッチングの対象となる全てのボクセルを対象とした総合的な評価関数値（「スコア値」とも呼ぶ。）「Ｅ（ｋ）」を算出する。

【0076】

【数6】

その後、自車位置推定部１５は、ニュートン法などの任意の求根アルゴリズムによりスコア値Ｅ（ｋ）が最大となるとなる推定パラメータＰを算出する。そして、自車位置推定部１５は、デッドレコニングにより暫定的に算出した予測自車位置「Ｘ^－（ｋ）」に対し、推定パラメータＰを適用することで、以下の式（７）を用いて高精度な推定自車位置「Ｘ＾（ｋ）」を算出する。

【0077】

【数7】

なお、ここでは、計算対象となる基準時刻（即ち現在時刻）「ｋ」の自車位置を示す状態変数ベクトルを、「Ｘ^－（ｋ）」または「Ｘ＾（ｋ）」と表記している。

【0078】

図１０は、自車位置推定部１５の機能ブロックの一例である。図１０に示すように、自車位置推定部１５は、デッドレコニングブロック２１と、位置予測ブロック２２と、座標変換ブロック２３と、点群データ対応付けブロック２４と、位置補正ブロック２５とを有する。

【0079】

デッドレコニングブロック２１は、ジャイロセンサ３、車速センサ４、及びＧＰＳ受信機５等の出力に基づく車両の移動速度と角速度を用い、前回時刻からの移動距離と方位変化を求める。位置予測ブロック２２は、直前の計測更新ステップで算出された時刻ｋ－１の推定自車位置Ｘ^＾（ｋ－１）に対し、求めた移動距離と方位変化を加えて、時刻ｋの予測自車位置Ｘ^－（ｋ）を算出する。

【0080】

座標変換ブロック２３は、ダウンサンプリング処理部１４から出力されるダウンサンプリング後の加工点群データを、地図ＤＢ１０と同一の座標系であるワールド座標系に変換する。この場合、座標変換ブロック２３は、例えば、時刻ｋで位置予測ブロック２２が出力する予測自車位置に基づき、時刻ｋでの加工点群データの座標変換を行う。なお、ダウンサンプリング後の加工点群データに対して上述の座標変換が行われる代わりに、ダウンサンプリング前の点群データに対して上述の座標変換が行われてもよい。この場合、ダウンサンプリング処理部１４は、座標変換後のワールド座標系の点群データをダウンサンプリングしたワールド座標系の加工点群データを生成する。なお、車両に設置されたライダを基準とした座標系の点群データを車両座標系に変換する処理、及び車両座標系からワールド座標系に変換する処理等については、例えば、国際公開ＷＯ２０１９／１８８７４５などに開示されている。

【0081】

点群データ対応付けブロック２４は、座標変換ブロック２３が出力するワールド座標系の加工点群データと、同じワールド座標系で表されたボクセルデータＶＤとを照合することで、加工点群データとボクセルとの対応付けを行う。位置補正ブロック２５は、加工点群データと対応付けがなされた各ボクセルを対象として、式（５）に基づく個別評価関数値を算出し、式（６）に基づくスコア値Ｅ（ｋ）が最大となるとなる推定パラメータＰを算出する。そして、位置補正ブロック２５は、式（７）に基づき、位置予測ブロック２２が出力する予測自車位置Ｘ^－（ｋ）に対し、時刻ｋで求めた推定パラメータＰを適用することで、推定自車位置Ｘ^＾（ｋ）を算出する。

【0082】

ここで、計測点とボクセルデータＶＤとの対応付けの具体的手順について、簡単な例を用いて補足説明する。

【0083】

図１１は、ワールド座標系におけるｘ－ｙの２次元平面上でのボクセルデータＶＤが存在するボクセル「Ｖｏ１」～「Ｖｏ６」とこれらのボクセル付近の位置を示す計測点６１～６５との位置関係を示す。ここでは、説明の便宜上、ボクセルＶｏ１～Ｖｏ６の中心位置のワールド座標系のｚ座標と、計測点６１～６５のワールド座標系のｚ座標とは同一であるものとする。

【0084】

まず、座標変換ブロック２３は、計測点６１～６５を含む点群データをワールド座標系に変換する。その後、点群データ対応付けブロック２４は、ワールド座標系の計測点６１～６５の端数等の丸め処理を行う。図１１の例では、立方体である各ボクセルのサイズが１ｍであることから、点群データ対応付けブロック２４は、各計測点６１～６５のｘ、ｙ、ｚ座標の夫々の小数点以下を四捨五入する。

【0085】

次に、点群データ対応付けブロック２４は、ボクセルＶｏ１～Ｖｏ６に対応するボクセルデータＶＤと、各計測点６１～６５の座標との照合を行うことで、各計測点６１～６５に対応するボクセルを判定する。図１１の例では、計測点６１の（ｘ，ｙ）座標は、上述の四捨五入により（２，１）となることから、点群データ対応付けブロック２４は、計測点６１をボクセルＶｏ１と対応付ける。同様に、計測点６２及び計測点６３の（ｘ，ｙ）座標は、上述の四捨五入により（３，２）となることから、点群データ対応付けブロック２４は、計測点６２及び計測点６３をボクセルＶｏ５と対応付ける。また、計測点６４の（ｘ，ｙ）座標は、上述の四捨五入により（２，３）となることから、点群データ対応付けブロック２４は、計測点６４をボクセルＶｏ６と対応付ける。一方、計測点６５の（ｘ，ｙ）座標は、上述の四捨五入により（４，１）となることから、点群データ対応付けブロック２４は、計測点６５に対応するボクセルデータＶＤが存在しないと判定する。その後、位置補正ブロック２５は、点群データ対応付けブロック２４が対応付けた計測点とボクセルデータＶＤとを用いて推定パラメータＰの推定を行う。なお、この例では、ダウンサンプリング後の計測点数は５個であるが、対応計測点数Ｎｃは４個となる。前述したように、対応計測点数Ｎｃは、道路周辺に構造物が少なく疎な空間の場合や、また自車両の近くに他車両が存在してオクルージョンが生じた場合に少なくなる。

【0086】

（５）処理フロー
図１２は、車載機１のコントローラ１３が実行する自車位置推定に関する処理の手順を示すフローチャートの一例である。コントローラ１３は、図１２のフローチャートの処理を、電源がオンになった場合など、自車位置推定を行う必要が生じた場合に開始する。

【0087】

まず、コントローラ１３は、ダウンサンプリングのサイズを初期値に設定する（ステップＳ１１）。ここでは、一例として、ダウンサンプリングのサイズは、進行方向、横方向、高さ方向のいずれも１ｍであるものとする。次に、コントローラ１３は、ＧＰＳ受信機５の測位結果に基づき、予測自車位置を決定する（ステップＳ１２）。

【0088】

そして、コントローラ１３は、ライダ２の点群データを取得できたか否か判定する（ステップＳ１３）。そして、コントローラ１３は、ライダ２の点群データを取得できない場合（ステップＳ１３；Ｎｏ）、引き続きステップＳ１３の判定を行う。また、コントローラ１３は、点群データを取得できない期間では、引き続きＧＰＳ受信機５の測位結果に基づき、予測自車位置を決定する。なお、コントローラ１３は、ＧＰＳ受信機５のみの測位結果に限らず、点群データ以外の任意のセンサの出力に基づき、予測自車位置を決定してもよい。

【0089】

そして、コントローラ１３は、ライダ２の点群データを取得できた場合（ステップＳ１３；Ｙｅｓ）、ライダ２が現処理時刻において走査する１周期分の点群データに対し、ダウンサンプリングのサイズの現在の設定値によるダウンサンプリングを実施する（ステップＳ１４）。この場合、ダウンサンプリングのサイズの設定値は、ステップＳ１１で決定された初期値、又は直前に実行されたステップＳ１９～ステップＳ２２において決定された更新値に相当する。

【0090】

そして、コントローラ１３は、ジャイロセンサ３及び車速センサ４等の出力に基づく車両の移動速度と角速度を用いて、前回時刻からの移動距離と方位変化を求める。これにより、位置予測ブロック２２は、１時刻前（直前の処理時刻）に得られた推定自車位置（ヨー角などの姿勢角を含んでもよい）から、現処理時刻の予測自車位置（姿勢角を含んでもよい）を算出する（ステップＳ１５）。なお、１時刻前の推定自車位置が算出されていない場合（即ちステップＳ２３が未実行の場合）には、コントローラ１３は、ステップＳ１２で決定した予測自車位置を推定自車位置とみなしてステップＳ１５の処理を行ってもよい。

【0091】

次に、コントローラ１３は、ＮＤＴマッチング処理を実行する（ステップＳ１６）。この場合、コントローラ１３は、加工点群データをワールド座標系のデータに変換する処理、及び、ワールド座標系に変換された加工点群データとボクセルデータＶＤが存在するボクセルとの対応付けなどを行う。なお、コントローラ１３は、ＮＤＴマッチングに基づき、現処理時刻での推定自車位置（ヨー角などの姿勢角を含む）を算出する処理を行ってもよい。

【0092】

また、コントローラ１３は、ＮＤＴマッチング処理での加工点群データとボクセルデータＶＤとの対応付け結果に基づいて、対応計測点数Ｎｃを算出する（ステップＳ１７）。そして、コントローラ１３は、対応計測点数Ｎｃを、目標範囲情報９が示す目標範囲Ｒ_Ｎｃと比較する（ステップＳ１８）。ここでは、一例として、目標範囲Ｒ_Ｎｃが６００～８００であるものとして説明を行う。

【0093】

そして、対応計測点数Ｎｃが目標範囲Ｒ_Ｎｃの下限に相当する６００未満である場合、コントローラ１３は、ダウンサンプリングのサイズの設定値を、１／１．１倍に変更する（ステップＳ１９）。なお、「１／１．１」は一例であり、コントローラ１３は、１倍未満の任意の倍率又は０より大きい値による減算により上述の設定値を変更してもよい。そして、コントローラ１３は、ダウンサンプリングのサイズの下限値が設けられている場合には、当該下限値未満とならないようにダウンサンプリングのサイズの設定値の変更を制限する（ステップＳ２０）。

【0094】

また、対応計測点数Ｎｃが目標範囲Ｒ_Ｎｃの上限に相当する８００より大きい場合、コントローラ１３は、ダウンサンプリングのサイズの設定値を、１．１倍に変更する（ステップＳ２１）。なお、「１．１」は一例であり、コントローラ１３は、１倍より大きい任意の倍率又は０より大きい値による加算により上述の設定値を変更してもよい。そして、コントローラ１３は、ダウンサンプリングのサイズの上限値が設けられている場合には、当該上限値を超えないようにダウンサンプリングのサイズの設定値の変更を制限する（ステップＳ２２）。なお、コントローラ１３は、対応計測点数Ｎｃが目標範囲Ｒ_Ｎｃに属する場合には、ダウンサンプリングのサイズの設定を維持する。

【0095】

そして、コントローラ１３は、ＮＤＴマッチングの結果に基づき、現処理時刻での推定自車位置（ヨー角などの姿勢角を含む）を算出する（ステップＳ２３）。そして、コントローラ１３は、自車位置推定処理を終了すべきか否か判定する（ステップＳ２４）。そして、コントローラ１３は、自車位置推定処理を終了すべきと判定した場合（ステップＳ２４；Ｙｅｓ）、フローチャートの処理を終了する。一方、コントローラ１３は、自車位置推定処理を継続する場合（ステップＳ２４；Ｎｏ）、ステップＳ１３へ処理を戻し、次の処理時刻での自車位置の推定を行う。

【0096】

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

【0097】

（変形例１）
車載機１は、ダウンサンプリングを行う空間となる３次元座標系の座標軸を構成する各方向のうち、相対的に位置推定精度が悪い方向（「低精度方向」とも呼ぶ。）のダウンサンプリングのサイズを固定値にし、その他の方向のダウンサンプリングのサイズを上述の実施例に基づき適応的に変更してもよい。低精度方向は、例えば進行方向である。なお、低精度方向は、横方向又は高さ方向であってもよい。

【0098】

この場合、例えば、使用するライダ２又は運転支援システムの仕様等により、低精度方向が自車位置推定処理の実行前に経験的に特定されており、メモリ１２には、低精度方向のダウンサンプリングのサイズの固定値が記憶されている。そして、車載機１は、低精度方向のダウンサンプリングのサイズを、メモリ１２に記憶された固定値にし、他の方向のダウンサンプリングのサイズを図１２のフローチャートのステップＳ１９～ステップＳ２２において変更する。このようにすることで、車載機１は、低精度方向での加工点群データの分解能を確保し、低精度方向での位置推定精度の低下を確実に抑制することができる。

【0099】

（変形例２）
車載機１は、低精度方向のダウンサンプリングのサイズを、他の方向のダウンサンプリングのサイズの１倍未満のサイズとなるように設定してもよい。

【0100】

例えば、車載機１は、対応計測点数Ｎｃに基づき決定したサイズ（「基準サイズ」とも呼ぶ。）と、精度や重要度等に応じて予め方向毎に設定した比率とに基づき、各方向のダウンサンプリングのサイズを決定する。例えば、車載機１は、進行方向のダウンサンプリングのサイズを、図１２のフローチャートのステップＳ１１又はステップＳ１９～ステップＳ２２において決定した基準サイズの１／２倍とする。一方、車載機１は、横方向のダウンサンプリングのサイズを基準サイズの２倍とし、高さ方向のダウンサンプリングのサイズを基準サイズと同一（等倍）とする。このようにすることで、車載機１は、低精度方向の他の方向に対する加工点群データの相対的な分解能を常に上げることができる。

【0101】

（変形例３）
車載機１は、処理時刻毎に実行する自車位置推定処理（ダウンサンプリングを含む）の処理時間（処理時間の長さ）を計測し、計測した処理時間に応じてダウンサンプリングのサイズを設定してもよい。

【0102】

例えば、車載機１は、自車位置推定処理の実行周期が８３．３ｍｓである場合には、処理時間が３０ｍｓ未満の場合に、ダウンサンプリングのサイズを所定値又は所定率だけ小さくする。一方、車載機１は、処理時間が７０ｍｓより長い場合、ダウンサンプリングのサイズを所定値又は所定率だけ大きくする。また、車載機１は、処理時間が３０ｍｓ～７０ｍｓである場合、ダウンサンプリングのサイズの設定を維持する。なお、車載機１は、本変形例に基づくダウンサンプリングのサイズの設定の変更を、例えば、図１２のステップＳ２３の実行後に行ってもよく、ステップＳ１８～ステップＳ２２の直前又は直後において実行してもよい。

【0103】

本変形例によれば、車載機１は、処理時間が実行周期に対して適切な範囲に収まるようにダウンサンプリングのサイズを好適に設定することができる。

【0104】

（変形例４）
図１に示す運転支援システムの構成は一例であり、本発明が適用可能な運転支援システムの構成は図１に示す構成に限定されない。例えば、運転支援システムは、車載機１を有する代わりに、車両の電子制御装置が車載機１のダウンサンプリング処理部１４及び自車位置推定部１５の処理を実行してもよい。この場合、地図ＤＢ１０は、例えば車両内の記憶部又は車両とデータ通信を行うサーバ装置に記憶され、車両の電子制御装置は、この地図ＤＢ１０を参照することで、ダウンサンプリング及びＮＤＴスキャンマッチングに基づく自車位置推定などを実行する。

【0105】

（変形例５）
ボクセルデータＶＤは、図９に示すように、平均ベクトルと共分散行列とを含むデータ構造に限定されない。例えば、ボクセルデータＶＤは、平均ベクトルと共分散行列を算出する際に用いられる計測整備車両が計測した点群データをそのまま含んでいてもよい。

【0106】

（７）実験結果に基づく考察
次に、上述した実施例（変形例も含む）に関する実験結果について考察する。出願人は、水平視野角が６０度及び動作周波数１２Ｈｚ（周期８３．３ｍｓ）となるライダを前方２台及び後方２台搭載した車両をある走行路に対して走行させ、当該走行路に対して事前に作成したボクセルデータ（ＮＤ地図）と、走行中に得られたライダの点群データとをマッチングさせてＮＤＴに基づく位置推定を行った。また、精度評価のために、正解となる位置データとして、ＲＴＫ－ＧＰＳの測位結果を用いた。

【0107】

図１３（Ａ）～図１３（Ｆ）及び図１４（Ａ）～図１４（Ｅ）は、１辺が０．５ｍの立方格子サイズでダウンサンプリングを行い、ＮＤＴ処理による自車位置推定を行った結果を示す。ここで、ＲＴＫ－ＧＰＳの測位結果に対して、図１３（Ａ）は進行方向の誤差、図１３（Ｂ）は横方向の誤差、図１３（Ｃ）は高さ方向の誤差、図１３（Ｄ）はヨー角の誤差を示し、図１３（Ｅ）はダウンサンプリング前の点群データの計測点数、図１３（Ｆ）はダウンサンプリングのサイズを夫々示す。また、図１４（Ａ）はダウンサンプリング後の加工点群データの計測点数、図１４（Ｂ）は対応計測点数Ｎｃ、図１４（Ｃ）は加工点群データの計測点数に対する対応計測点数Ｎｃの比率（「対応率」とも呼ぶ。）、図１４（Ｄ）はスコア値Ｅ（ｋ）、図１４（Ｅ）は処理時間を夫々示す。なお、図１４（Ｅ）では、ライダの周期（８３．３ｍｓ）を横線により明示している。

【0108】

ここで、図１４（Ｅ）に示されるように、処理時間がライダの周期８３．３ｍｓを超えてしまっている期間が存在する。この期間では、図１３（Ｅ）に示されるダウンサンプリング前の計測点数及び図１４（Ａ）に示されるダウンサンプリング後の計測点数、図１４（Ｂ）に示される対応計測点数Ｎｃがいずれも多くなっている。このように、点群データ数と処理時間には正の相関が存在することが把握される。

【0109】

また、図１３（Ａ）に示されるように、１７０ｓ付近において瞬間的に進行方向の誤差が大きくなっている。このときの走行場所が進行方向の特徴量が少ないエリアであったことに加えて、処理時間がライダの周期を超えている部分であり、処理が間に合っていないことが重なって推定位置がずれてしまったと推測できる。

【0110】

このように、ライダの周期に間に合うような処理を行う必要があり、ダウンサンプリングを固定サイズにした場合には、この課題を解決することができない場合がある。

【0111】

図１５（Ａ）～図１５（Ｆ）及び図１６（Ａ）～図１６（Ｅ）は、１辺が２．０ｍの立方格子サイズでダウンサンプリングを行い、ＮＤＴ処理による自車位置推定を行った結果を示す。ここで、ＲＴＫ－ＧＰＳの測位結果に対して、図１５（Ａ）は進行方向の誤差、図１５（Ｂ）は横方向の誤差、図１５（Ｃ）は高さ方向の誤差、図１５（Ｄ）はヨー角の誤差を示し、図１５（Ｅ）はダウンサンプリング前の点群データの計測点数、図１５（Ｆ）はダウンサンプリングのサイズを夫々示す。また、図１６（Ａ）はダウンサンプリング後の加工点群データの計測点数、図１６（Ｂ）は対応計測点数Ｎｃ、図１６（Ｃ）は対応率、図１６（Ｄ）はスコア値Ｅ（ｋ）、図１６（Ｅ）は処理時間を夫々示す。なお、図１６（Ｅ）では、ライダの周期（８３．３ｍｓ）を横線により明示している。

【0112】

この場合、図１３（Ａ）～図１３（Ｆ）及び図１４（Ａ）～図１４（Ｅ）の例と比較して、ダウンサンプリングのサイズを大きくしたことで、図１６（Ｂ）に示される対応計測点数Ｎｃが減少し、図１６（Ｅ）に示される処理時間も低下している。結果として、処理時間は、いずれの時間においてもライダの周期８３．３ｍｓ以内に収まっている。一方、図１５（Ａ）に示される進行方向の誤差が増大している（特に２５０ｓ～２８０ｓ）。これは、加工点群データの分解能が低下し、ダウンサンプリング後の計測点数が減少したことが要因と推察される。なお、加工点群データの計測点数が少なすぎると、ＮＤＴ処理でマッチングさせる計測点数が不足するため、安定性が低下することがある。

【0113】

このように、ダウンサンプリングのサイズを大きめの固定サイズに設定した場合においても、自車位置推定精度の維持と処理時間の抑制との両立を行うことはできない。

【0114】

図１７（Ａ）～図１７（Ｆ）及び図１８（Ａ）～図１８（Ｅ）は、本実施例に係る図１２に示す処理手順に従いダウンサンプリング及びＮＤＴ処理による自車位置推定を行った結果を示す。ここで、ＲＴＫ－ＧＰＳの測位結果に対して、図１７（Ａ）は進行方向の誤差、図１７（Ｂ）は横方向の誤差、図１７（Ｃ）は高さ方向の誤差、図１７（Ｄ）はヨー角の誤差を示し、図１７（Ｅ）はダウンサンプリング前の点群データの計測点数、図１７（Ｆ）はダウンサンプリングのサイズを夫々示す。また、図１８（Ａ）はダウンサンプリング後の加工点群データの計測点数、図１８（Ｂ）は対応計測点数Ｎｃ、図１８（Ｃ）は対応率、図１８（Ｄ）はスコア値Ｅ（ｋ）、図１８（Ｅ）は処理時間を夫々示す。なお、図１８（Ｅ）では、ライダの周期（８３．３ｍｓ）を横線により明示している。

【0115】

この場合、図１７（Ｆ）に示されるように、ダウンサンプリングのサイズは動的に変化している。その結果、図１８（Ｅ）に示される処理時間は、常にライダの周期８３．３ｍｓより下となり、かつ、図１７（Ａ）に示される進行方向の誤差も好適に抑制されている。

【0116】

このように、本実施例に基づきダウンサンプリングのサイズを適応的に変化させることで、自車位置推定精度の維持と処理時間の抑制との両立を行うことができる。

【0117】

図１９（Ａ）～図１９（Ｇ）は、他車両によるオクルージョンが発生した場合の、本実施例に係る図１２に示す処理手順に従いダウンサンプリング及びＮＤＴ処理による自車位置推定を行った結果を示す。また、図２０（Ａ）～図２０（Ｇ）は、図１９（Ａ）～図１９（Ｇ）と同一状況下において、対応計測点数Ｎｃではなく、仮にダウンサンプリング後の計測点数を６００～８００に収まるようにダウンサンプリングのサイズを決定した場合の結果を示す。ここで、図１９（Ａ）及び図２０（Ａ）はダウンサンプリング前の計測点数、図１９（Ｂ）及び図２０（Ｂ）はダウンサンプリングサイズ、図１９（Ｃ）及び図２０（Ｃ）はダウンサンプリング後の計測点数、図１９（Ｄ）及び図２０（Ｄ）は対応計測点数Ｎｃ、図１９（Ｅ）及び図２０（Ｅ）は対応率、図１９（Ｆ）及び図２０（Ｆ）はスコア値Ｅ（ｋ）、図１９（Ｇ）及び図２０（Ｇ）は処理時間を夫々示す。

【0118】

これらの例では、１８０ｓ以降しばらく他車両によるオクルージョンが発生している。そして、図２０（Ａ）～図２０（Ｇ）の例のように、ボクセルデータＶＤと対応付けされる前の計測点数を対象としてダウンサンプリングのサイズを制御していると、オクルージョンが発生した場合に、対応計測点数Ｎｃが過少となる。その結果、図２０（Ｄ）に示すように、オクルージョンが発生した期間において、対応計測点数Ｎｃが低下している。一方、本実施例に係るダウンサンプリングのサイズを制御した場合、図１９（Ｄ）に示すように、オクルージョンが発生した期間においても、対応計測点数Ｎｃの変動が小さい。従って、図２０（Ａ）～図２０（Ｇ）の例は、図１９（Ａ）～図１９（Ｇ）の例と比較して、位置推定の安定性が低下することとなる。この例から、オクルージョンの発生によって、静止構造物を基とするボクセルデータに対応付けられない計測点が多い場合は、ダウンサンプリング後の計測点が目標範囲になるような制御をしても良くないことがわかる。

【0119】

図２１（Ａ）～図２１（Ｆ）及び図２２（Ａ）～図２２（Ｅ）は、変形例１に基づき進行方向のダウンサンプリングのサイズを０．５ｍに固定し、横方向及び高さ方向のダウンサンプリングのサイズを適応的に変更してＮＤＴ処理による自車位置推定を行った結果を示す。具体的には、ＲＴＫ－ＧＰＳの測位結果に対して、図２１（Ａ）は進行方向の誤差、図２１（Ｂ）は横方向の誤差、図２１（Ｃ）は高さ方向の誤差、図２１（Ｄ）はヨー角の誤差を示し、図２１（Ｅ）はダウンサンプリング前の点群データの計測点数、図２１（Ｆ）はダウンサンプリングのサイズを夫々示す。また、図２２（Ａ）はダウンサンプリング後の加工点群データの計測点数、図２２（Ｂ）は対応計測点数Ｎｃ、図２２（Ｃ）は対応率、図２２（Ｄ）はスコア値Ｅ（ｋ）、図２２（Ｅ）は処理時間を夫々示す。なお、図２２（Ｅ）では、ライダの周期（８３．３ｍｓ）を横線により明示している。

【0120】

この例では、走行路の両脇にガードレール、路肩、中央分離帯が数多く存在し、横方向の誤差は進行方向の誤差よりも良好であることを勘案し、進行方向を低精度方向とみなして進行方向のダウンサンプリングのサイズを０．５ｍに固定している。そして、進行方向のダウンサンプリングのサイズが０．５ｍに固定されていることにより、図２１（Ｆ）に示されるように、横方向と高さ方向のダウンサンプリングのサイズは、進行方向を含めて適応的にダウンサンプリングのサイズを変更する場合（図１７（Ｆ）参照）よりも大きくなっている。一方、変形例１においても、図２２（Ｂ）に示される対応計測点数Ｎｃが６００～８００に維持されるように制御することにより、図２２（Ｅ）に示される処理時間もライダの周期以内に好適に収まっている。

【0121】

結果として、変形例１を実行した場合には、図２１（Ｂ）に示される横方向の誤差はほぼ変化せず、図２１（Ａ）に示される進行方向の誤差（特に１７０ｓ付近）が小さくなっている。このように、変形例１に基づくダウンサンプリングのサイズの制御においても、自車位置推定精度の維持と処理時間の抑制とを好適に両立させることができる。

【0122】

以上説明したように、本実施例に係る車載機１のコントローラ１３は、ライダ２が出力する点群データを取得する。そして、コントローラ１３は、点群データに対してダウンサンプリングを行った加工点群データを生成する。そして、コントローラ１３は、加工点群データと、単位領域であるボクセルごとの物体の位置を表すボクセルデータＶＤとの照合により、加工点群データを構成する計測点とボクセルの各々との対応付けを行う。そして、コントローラ１３は、加工点群データの計測点のうちボクセルと対応付けられた計測点の数である対応計測点数Ｎｃに基づき、次に実行されるダウンサンプリングのサイズを変更する。これにより、車載機１は、ダウンサンプリングのサイズを適応的に決定し、自己位置推定の精度と処理時間との両立を好適に実現することができる。

【0123】

なお、上述した実施例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータであるコントローラ等に供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記憶媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記憶媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記憶媒体（例えば光磁気ディスク）、ＣＤ－ＲＯＭ（Read Only Memory）、ＣＤ－Ｒ、ＣＤ－Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory））を含む。

【0124】

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

【符号の説明】

【0125】

１ 車載機
２ ライダ
３ ジャイロセンサ
４ 車速センサ
５ ＧＰＳ受信機
１０ 地図ＤＢ

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】

【図21】

【図22】