JP2023095904A - 自己位置推定装置 - Google Patents

Abstract

【課題】周辺構造物が少ない、あるいは同じような構造物が連続しているようなエリアであっても、高精度で自己位置を推定することを可能とする。【解決手段】自己位置推定装置は、所定サイズの３次元仮想領域単位で構成され、３次元仮想領域内に存在する物体に対して照射した光の反射光に対応する第１点群の特徴を示す第１点群特徴データを含む地図データを取得する。第１点群特徴データは第１点群の反射強度が反映されたデータある。また、自己位置推定装置は、３次元仮想領域毎に、当該３次元仮想領域内に存在する物体に対して照射した光の反射光を受光する計測部により計測して得た第２点群の特徴を示す第２点群特徴データを含む計測データを生成する。第２点群特徴データは、第２点群の反射強度が反映されたデータである。そして、自己位置推定装置は、３次元仮想領域毎の第１点群特徴データと第２点群特徴データとに基づいて、移動体の自己位置を推定する。【選択図】図６

Description

本発明は、点群の照合（マッチング）技術に関する。

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

国際公開ＷＯ２０１３／０７６８２９

上記の手法では、道路周辺に存在する構造物が多く、かつ、変化に富んでいると、地図情報と計測データとが一致する場合と一致しない場合との違いが大きくなるためにマッチングしやすくなり、位置推定精度が高くなる。一方、周辺構造物が少なかったり、同じような構造物が連続していたりすると、地図情報と計測データとが一致する場合と一致しない場合との違いが小さくなるためマッチングしにくくなり、位置推定精度が低下しやすい。なお、マッチングしにくいとは、具体的には、マッチングに多くの時間がかかってしまったり、位置推定結果が不正確になってしまうことを意味する。

本発明の解決しようとする課題としては、上記のものが一例として挙げられる。本発明は、周辺構造物が少ない、あるいは同じような構造物が連続しているようなエリアであっても、高精度で自己位置を推定することが可能な自己位置推定装置を提供することを目的とする。

請求項に記載の発明は、移動体の自己位置を推定する自己位置推定装置であって、所定サイズの３次元仮想領域単位で構成され、前記３次元仮想領域内に存在する物体に対して照射した光の反射光に対応する第１点群の特徴を示す第１点群特徴データを含む地図データを取得する地図データ取得手段と、前記３次元仮想領域毎に、当該３次元仮想領域内に存在する物体に対して照射した光の反射光を受光する計測部により計測して得た第２点群の特徴を示す第２点群特徴データを含む計測データを生成する計測データ生成手段と、前記３次元仮想領域毎の前記第１点群特徴データと前記第２点群特徴データとに基づいて、前記自己位置を推定する推定手段と、を備え、前記第１点群特徴データは前記第１点群の反射強度が反映されたデータであり、前記第２点群特徴データは前記第２点群の反射強度が反映されたデータである。

請求項に記載の発明は、移動体の自己位置を推定する自己位置推定装置により実行される自己位置推定方法であって、所定サイズの３次元仮想領域単位で構成され、前記３次元仮想領域内に存在する物体に対して照射した光の反射光に対応する第１点群の特徴を示す第１点群特徴データを含む地図データを取得する地図データ取得工程と、前記３次元仮想領域毎に、当該３次元仮想領域内に存在する物体に対して照射した光の反射光を受光する計測部により計測して得た第２点群の特徴を示す第２点群特徴データを含む計測データを生成する計測データ生成工程と、前記３次元仮想領域毎の前記第１点群特徴データと前記第２点群特徴データとに基づいて、前記自己位置を推定する推定工程と、を備え、前記第１点群特徴データは前記第１点群の反射強度が反映されたデータであり、前記第２点群特徴データは前記第２点群の反射強度が反映されたデータである。

請求項に記載の発明は、コンピュータを備え、移動体の自己位置を推定する自己位置推定装置により実行されるプログラムであって、所定サイズの３次元仮想領域単位で構成され、前記３次元仮想領域内に存在する物体に対して照射した光の反射光に対応する第１点群の特徴を示す第１点群特徴データを含む地図データを取得する地図データ取得手段、前記３次元仮想領域毎に、当該３次元仮想領域内に存在する物体に対して照射した光の反射光を受光する計測部により計測して得た第２点群の特徴を示す第２点群特徴データを含む計測データを生成する計測データ生成手段、前記３次元仮想領域毎の前記第１点群特徴データと前記第２点群特徴データとに基づいて、前記自己位置を推定する推定手段、として前記コンピュータを機能させ、前記第１点群特徴データは前記第１点群の反射強度が反映されたデータであり、前記第２点群特徴データは前記第２点群の反射強度が反映されたデータである。

運転支援システムの概略構成である。 車載機及びサーバ装置のブロック構成を示す。 ボクセルデータのデータ構造の例を示す。 基本的なボクセルデータ及びスキャンデータの例を示す。 ボクセルデータの２次元正規分布とスキャンデータの平均値の例を示す。 第１実施例におけるボクセルデータ及びスキャンデータの例を示す。 反射強度平均値を用いたマッチングの例を示す。 自車位置推定処理のフローチャートである。 第２実施例によるボクセルデータの２次元正規分布の例を示す。

本発明の１つの好適な実施形態では、移動体の自己位置を推定する自己位置推定装置は、所定サイズの３次元仮想領域単位で構成され、前記３次元仮想領域内に存在する物体に対して照射した光の反射光に対応する第１点群の特徴を示す第１点群特徴データを含む地図データを取得する地図データ取得手段と、前記３次元仮想領域毎に、当該３次元仮想領域内に存在する物体に対して照射した光の反射光を受光する計測部により計測して得た第２点群の特徴を示す第２点群特徴データを含む計測データを生成する計測データ生成手段と、前記３次元仮想領域毎の前記第１点群特徴データと前記第２点群特徴データとに基づいて、前記自己位置を推定する推定手段と、を備え、前記第１点群特徴データは前記第１点群の反射強度が反映されたデータであり、前記第２点群特徴データは前記第２点群の反射強度が反映されたデータである。

上記の自己位置推定装置は、所定サイズの３次元仮想領域単位で構成され、３次元仮想領域内に存在する物体に対して照射した光の反射光に対応する第１点群の特徴を示す第１点群特徴データを含む地図データを取得する。第１点群特徴データは第１点群の反射強度が反映されたデータある。また、自己位置推定装置は、３次元仮想領域毎に、当該３次元仮想領域内に存在する物体に対して照射した光の反射光を受光する計測部により計測して得た第２点群の特徴を示す第２点群特徴データを含む計測データを生成する。第２点群特徴データは、第２点群の反射強度が反映されたデータである。そして、自己位置推定装置は、３次元仮想領域毎の第１点群特徴データと第２点群特徴データとに基づいて、移動体の自己位置を推定する。この自己位置推定装置は、点群の反射強度を利用して自己位置を推定するので、周辺構造物が少ない、あるいは同じような構造物が連続しているようなエリアであっても、高精度で自己位置を推定することができる。

上記の自己位置推定装置の一態様では、前記第１点群特徴データは、前記第１点群の平均位置、前記１点群の分散、及び、前記第１点群の平均反射強度を含み、前記第２点群特徴データは、前記第２点群の平均位置、及び、前記第２点群の平均反射強度を含む。

上記の自己位置推定装置の他の一態様では、前記推定手段は、前記第１点群の平均反射強度及び前記第２点群の平均反射強度を重み付け値として用いた評価式により、前記３次元仮想領域毎の前記第１点群特徴データと前記第２点群特徴データとのマッチング結果を求める。

上記の自己位置推定装置の他の一態様では、前記第１点群特徴データは、前記第１点群の各点の反射強度を用いて算出した前記第１点群の平均位置及び分散を含み、前記第２点群特徴データは、前記第２点群の各点の反射強度を用いて算出した前記第１点群の平均位置を含む。

本発明の他の好適な実施形態では、移動体の自己位置を推定する自己位置推定装置により実行される自己位置推定方法は、所定サイズの３次元仮想領域単位で構成され、前記３次元仮想領域内に存在する物体に対して照射した光の反射光に対応する第１点群の特徴を示す第１点群特徴データを含む地図データを取得する地図データ取得工程と、前記３次元仮想領域毎に、当該３次元仮想領域内に存在する物体に対して照射した光の反射光を受光する計測部により計測して得た第２点群の特徴を示す第２点群特徴データを含む計測データを生成する計測データ生成工程と、前記３次元仮想領域毎の前記第１点群特徴データと前記第２点群特徴データとに基づいて、前記自己位置を推定する推定工程と、を備え、前記第１点群特徴データは前記第１点群の反射強度が反映されたデータであり、前記第２点群特徴データは前記第２点群の反射強度が反映されたデータである。この自己位置推定方法は、点群の反射強度を利用して自己位置を推定するので、周辺構造物が少ない、あるいは同じような構造物が連続しているようなエリアであっても、高精度で自己位置を推定することができる。

本発明の他の好適な実施形態では、コンピュータを備え、移動体の自己位置を推定する自己位置推定装置により実行されるプログラムは、所定サイズの３次元仮想領域単位で構成され、前記３次元仮想領域内に存在する物体に対して照射した光の反射光に対応する第１点群の特徴を示す第１点群特徴データを含む地図データを取得する地図データ取得手段、前記３次元仮想領域毎に、当該３次元仮想領域内に存在する物体に対して照射した光の反射光を受光する計測部により計測して得た第２点群の特徴を示す第２点群特徴データを含む計測データを生成する計測データ生成手段、前記３次元仮想領域毎の前記第１点群特徴データと前記第２点群特徴データとに基づいて、前記自己位置を推定する推定手段、として前記コンピュータを機能させ、前記第１点群特徴データは前記第１点群の反射強度が反映されたデータであり、前記第２点群特徴データは前記第２点群の反射強度が反映されたデータである。このプログラムをコンピュータで実行することにより、上記の自己位置推定装置を実現することができる。このプログラムは、記憶媒体に記憶して取り扱うことができる。

以下、図面を参照して本発明の好適な実施例について説明する。

［運転支援システムの概要］
図１は、実施例に係る運転支援システムの概略構成を示す。運転支援システムは、車両と共に移動する車載機１と、地図情報の配信を行うサーバ装置２と、を備える。なお、図１では、サーバ装置２と通信を行う車載機１及び車両が１組のみ表示されているが、実際には、異なる位置に複数の車載機１及び車両の組が存在する。

車載機１は、ライダ（Ｌｉｄａｒ：Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ、または、Ｌａｓｅｒ Ｉｌｌｕｍｉｎａｔｅｄ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）などの外界センサ、ジャイロセンサや車速センサなどの内界センサと電気的に接続し、これらの出力に基づき、車載機１が搭載される車両の位置（「自車位置」とも呼ぶ。）の推定を行う。そして、車載機１は、自車位置の推定結果に基づき、設定された目的地への経路に沿って走行するように、車両の自動運転制御などを行う。

車載機１は、ボクセルデータを含む地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶する。ボクセルデータは、３次元空間を複数の領域に分割した場合の各領域（「ボクセル」とも呼ぶ。）ごとに静止構造物の位置情報等を記録したデータである。ボクセルデータは、各ボクセル内の静止構造物の計測された点群データを正規分布により表したデータを含み、後述するように、ＮＤＴ（Ｎｏｒｍａｌ Ｄｉｓｔｒｉｂｕｔｉｏｎｓ Ｔｒａｎｓｆｏｒｍ）を用いたスキャンマッチングに用いられる。車載機１は、ライダが計測した点群データと、当該点群データが属するボクセルに対応するボクセルデータとに基づき、ＮＤＴに基づくスキャンマッチングを行う。そして、車載機１は、マッチング結果に基づいて自車位置の推定結果を修正しつつ、自動運転制御などを行う。なお、以下の説明では、ＮＤＴスキャンマッチングに用いられる地図を「ＮＤ地図」とも呼ぶ。また、ＮＤ地図のボクセルデータを作成するために使用された点群データを「地図作成用点群データ」と呼び、スキャンマッチングの際にライダ３０が計測した点群データを「スキャンデータ」と呼んで両者を区別する。

サーバ装置２は、複数の車両の車載機１とデータ通信を行う。サーバ装置２は、複数の車両の車載機１に配信するための地図情報を記憶した配信地図ＤＢ２０を記憶し、その地図情報には各ボクセルに対応するボクセルデータが含まれている。

［車載機の構成］
図２（Ａ）は、車載機１の機能的構成を表すブロック図を示す。図２（Ａ）に示すように、車載機１は、主に、通信部１１と、記憶部１２と、センサ部１３と、入力部１４と、制御部１５と、出力部１６とを有する。通信部１１、記憶部１２、センサ部１３、入力部１４、制御部１５及び出力部１６は、バスラインを介して相互に接続されている。

通信部１１は、制御部１５の制御に基づき、サーバ装置２から配信される地図情報を受信する。また、通信部１１は、サーバ装置２から要求があった場合に、センサ部１３による計測データをサーバ装置２へ送信する。また、通信部１１は、車両を制御するための信号をサーバ装置２から受信したり、車両の状態に関する信号をサーバ装置２へ送信したりする。

記憶部１２は、制御部１５が実行するプログラムや、制御部１５が所定の処理を実行する為に必要な情報を記憶する。また、記憶部１２は、ＮＤ地図のボクセルデータを記憶する地図ＤＢ１０を有する。記憶部１２に記憶されるＮＤ地図のボクセルデータは、サーバ装置２の配信地図ＤＢからダウンロードされたものである。

センサ部１３は、ライダ３０と、カメラ３１と、ＧＰＳ受信機３２と、ジャイロセンサ３３と、速度センサ３４とを含む。ライダ３０は、水平方向および垂直方向の所定の角度範囲に対してパルスレーザを出射することで、外界に存在する物体までの距離を離散的に測定し、当該物体の位置を示す３次元の点群データであるスキャンデータを生成する。この場合、ライダ３０は、照射方向を変えながらレーザ光を照射する照射部と、照射したレーザ光の反射光（散乱光）を受光する受光部と、受光部が出力する受光信号に基づくスキャンデータを出力する出力部とを有する。スキャンデータは、受光部が受光したレーザ光に対応する照射方向と、上述の受光信号に基づき特定される当該レーザ光のその照射方向での物体までの距離とに基づき生成される。

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

制御部１５は、プログラムを実行するＣＰＵなどを含み、車載機１の全体を制御する。例えば、制御部１５は、ライダ３０から出力されるスキャンデータと、当該スキャンデータデータが属するボクセルに対応するＮＤ地図のボクセルデータとに基づき、ＮＤＴに基づくスキャンマッチングを行うことで自車位置の推定を行う。また、制御部１５は、必要に応じて、サーバ装置２からＮＤ地図のボクセルデータをダウンロードする。さらに、制御部１５は、サーバ装置２から要求があった場合には、センサ部３０による計測データ、例えばライダ３０によるスキャンデータなどをサーバ装置２へ送信する。

上記の構成において、車載機１は本発明の自己位置推定装置の一例であり、制御部１５は本発明の地図データ取得手段及び計測データ生成手段の一例であり、ライダ３０は本発明の計測部の一例である。

［サーバ装置の構成］
図２（Ｂ）は、サーバ装置２の概略構成を示す。図２（Ｂ）に示すように、サーバ装置２は、通信部２１と、記憶部２２と、制御部２５とを有する。通信部２１、記憶部２２、及び制御部２５は、バスラインを介して相互に接続されている。

通信部２１は、制御部２５の制御に基づき、車載機１と各種データの通信を行う。記憶部２２は、サーバ装置２の動作を制御するためのプログラムを保存したり、サーバ装置２の動作に必要な情報を保持したりする。また、記憶部２２は、ＮＤ地図のボクセルデータを含む配信地図ＤＢ２０を記憶している。

制御部２５は、図示しないＣＰＵ、ＲＯＭ及びＲＡＭなどを備え、サーバ装置２内の各構成要素に対して種々の制御を行う。例えば、制御部２５は、車載機１からの要求に応じて、配信地図２０に含まれるボクセルデータを車載機１へ送信する。

［基本スキャンマッチング］
次に、本実施例におけるＮＤＴに基づくスキャンマッチングについて説明する。
（１）ボクセルデータ
まず、ＮＤＴに基づくスキャンマッチングに用いるボクセルデータについて説明する。図３（Ａ）は、ＮＤ地図のボクセルデータのデータ構造の例を示す。

ボクセルデータは、地図作成用点群データから生成され、ボクセル内の点群を正規分布で表現する場合のパラメータの情報を含む。本実施例では、ボクセルデータは、図３（Ａ）に示すように、「ボクセルＩＤ」と、「ボクセル座標」と、「平均ベクトル」と、「共分散行列」と、「点群数情報」とを含む。なお、ボクセルは本発明の３次元仮想領域の一例であり、ボクセルデータは本発明の第１点群特徴データの一例である。

「ボクセル座標」は、各ボクセルの中心位置などの基準となる位置の絶対的な３次元座標を示す。なお、各ボクセルは、空間を格子状に分割した立方体であり、予め形状及び大きさが定められているため、ボクセル座標により各ボクセルの空間を特定することが可能である。ボクセル座標は、ボクセルＩＤとして用いられてもよい。

「平均ベクトル」及び「共分散行列」は、対象のボクセル内での点群を正規分布で表現する場合のパラメータに相当する平均ベクトル及び共分散行列を示す。任意のボクセル内の任意の点「ｉ」の座標をＸ_Ｍ（ｉ）とし、ボクセル内での点群数を「Ｍ」とすると、ボクセル内データＸ_Ｍ（ｉ）における平均ベクトル「μ」及び共分散行列「Ｖ」は以下の式（１）～（３）により表される。

「点群数情報」は、対応する平均ベクトル及び共分散行列の算出に用いた点群の数を示す情報である。点群数情報は、具体的な点群の数を示す情報であってもよく、点群数のレベル（例えば、大、中、小など）を示す情報であってもよい。

図４（Ａ）は、ボクセルデータの一例を示す。ボクセルデータは、ボクセルＩＤにより規定されるボクセル毎に、ボクセル座標、平均ベクトル、共分散行列を含む。ボクセル座標は、ｘ方向、ｙ方向、ｚ方向における座標Ｍ_ｘ、Ｍ_ｙ、Ｍ_ｚを含む。平均ベクトルは、ｘ方向、ｙ方向、ｚ方向における平均ベクトルμ_ｘ、μ_ｙ、μ_ｚを含む。共分散行列は、式（３）で示す行列を構成する各要素を含む。

（２）スキャンデータ
図４（Ｂ）は、ライダ３０によるスキャンデータの一例を示す。図４（Ｂ）は１つのボクセルに対応するスキャンデータを示している。スキャンデータは、ライダ３０により計測された点群データであり、各点群データに割り当てられたスキャンデータ番号毎に、平均ベクトルＬを有する。なお、スキャンデータは、本発明の第２点群特徴データの一例である。

いま、ボクセル内での点群数を「Ｎ」とすると、各ボクセル内のスキャンデータＸ_Ｌ（ｊ）の平均値Ｌは以下の式で与えられる。

（３）スキャンマッチング
次に、ＮＤ地図のボクセルデータを用いたＮＤＴによるスキャンマッチングについて説明する。いま、推定パラメータとして、ｘ方向移動ベクトルｔ_ｘ、ｙ方向移動ベクトルｔ_ｙ、ｚ方向移動ベクトルｔ_ｚ、及び、方位方向（ｘｙ平面）の回転角（即ちヨー角）ｔ_Ψを用いる。なお、ピッチ角及びロール角は道路勾配や振動により生じるもので、無視できる程度に小さいものとする。上記の推定パラメータを用いて、ライダ３０により計測されたスキャンデータの任意の点の座標を座標変換すると、座標変換後の座標は以下の式（５）により得られる。

車載機１は、座標変換後の点群の座標と、ボクセルデータに含まれる平均ベクトルμ及び共分散行列Ｖとを用い、以下の式（６）により示されるインデックス番号ｋのボクセルの評価関数「Ｅ（ｋ）」、及び、式（７）により示されるマッチングの対象となる全てのボクセルを対象とした総合的な評価関数「Ｅ」（以下、「総合評価関数」とも呼ぶ。）を算出する。

なお、ライダ３０により得られる点群データの座標と、ボクセルデータの平均ベクトルの座標は、事前に設定した基準位置を原点とした座標系に統一する必要がある。よってマッチングに先立って、必要に応じて両者の座標変換を行う。

そして、車載機１は、総合評価関数Ｅが最大となるときの推定パラメータ、即ち、移動ベクトルｔ_ｘ、ｔ_ｙ、ｔ_ｚ及び回転角度ｔ_Ψを用いて推定自車位置を修正する。

図５は、ＮＤ地図のボクセルデータと、ライダ３０により得られたスキャンデータとの関係を概念的に示す。なお、説明の便宜上、図５は２次元の表示としている。図５（Ａ）は、４つのボクセルＢ１～Ｂ４のボクセルデータを示す。各ボクセルにおいて、丸印Ｄｍは地図作成用の計測整備車両で走行したときにライダにより計測した地図作成用点群データである。これらの地図作成用点群データに基づき、式（１）と式（２）により作成した２次元正規分布がグラデーションにより示されている。図５（Ａ）に示す正規分布の平均、分散は、ボクセルデータにおける平均ベクトル、共分散行列にそれぞれ対応する。

図５（Ｂ）は、ライダにより計測したスキャンデータの点群Ｄｓを図５（Ａ）に重ねて表示したものである。また、図５（Ｃ）は、図５（Ｂ）に表示したスキャンデータの点群Ｄｓから算出したスキャンデータの平均値Ｌを重ねて表示したものである。

［反射強度を用いたスキャンマッチング］
次に、本実施例の特徴である反射強度を用いたスキャンマッチングについて説明する。上記した基本のスキャンマッチングは、周辺構造物が少なかったり、同じような構造物が連続していたりすると、地図情報と計測データとが一致する場合と一致しない場合との違いが小さくなるためマッチングしにくくなる。そこで、本実施例では、ライダにより計測した点群データの反射強度を利用してスキャンマッチングを行う。

（第１実施例）
第１実施例は、ＮＤ地図のボクセルデータに反射強度の平均値を含め、その平均値とライダ３０により計測したスキャンデータの反射強度の平均値とを用いてスキャンマッチングを行うものである。図３（Ｂ）は第１実施例に係るＮＤ地図のボクセルデータのデータ構造の例を示し、図６（Ａ）は第１実施例に係るボクセルデータの例を示す。また、図６（Ｂ）は、第１実施例に係るスキャンデータの例を示す。図４（Ａ）、４（Ｂ）と比較するとわかるように、ボクセルデータには、各ボクセル内に含まれる点群の反射強度の平均値Ｉ_Ｍが含まれている。また、ライダ３０により計測されたスキャンデータ側においても、ボクセル内にあるスキャンデータの反射強度の平均値Ｉ_Ｌが算出される。

車載機１は、スキャンマッチングの評価関数に、ボクセルデータの反射強度平均値Ｉ_Ｍとスキャンデータの反射強度平均値Ｉ_Ｌを重み付け値として用いる。具体的に、車載機１は、あるボクセルｋの評価関数Ｅ（ｋ）を式（８）とし、全てのボクセルの総合評価関数Ｅを式（９）により求める。

式（８）の評価関数には、ボクセルデータの反射強度平均値Ｉ_Ｍとスキャンデータの反射強度平均値Ｉ_Ｌの積「Ｉ_Ｍ・Ｉ_Ｌ」が重み付け値として含まれている。この評価関数を用いることにより、反射強度の高いオブジェクト（例えば道路標識や白線など）を含むボクセルの評価関数値と、反射強度の高いオブジェクトを含まないボクセルの評価関数値とに差異が生まれる。これについて、簡単な例を挙げて説明する。

図７は、式（８）において用いられている「Ｉ_Ｍ・Ｉ_Ｌ」の値の算出例を示す図である。いま、車両が矢印の方向に走行し、車両の左側方に反射強度が比較的一様な壁などのオブジェクトがあるものとする。この壁の部分がボクセルＢ_Ｍ１～Ｂ_Ｍ６に対応し、それらのうちボクセルＢ_Ｍ４の反射強度平均値Ｉ_Ｍが「１．０」であり、他のボクセルＢ_Ｍ１～Ｂ_Ｍ３、Ｂ_Ｍ５～Ｂ_Ｍ６の反射強度平均値Ｉ_Ｍは全て「０．１」であると仮定する。即ち、ボクセルＢ_Ｍ４のみが他のボクセルＢ_Ｍ１～Ｂ_Ｍ３、Ｂ_Ｍ５～Ｂ_Ｍ６よりも高い反射強度を有している。

さて、車両がこの壁の前を通過し、横方向からライダで壁をスキャンしてスキャンデータを生成し、図示のように複数のボクセル単位のスキャンデータＢ_Ｌａ～Ｂ_Ｌｅを得たものとする。得られたスキャンデータにおいては、ボクセルＢ_Ｌｄの反射強度平均値Ｉ_Ｌが「１．０」であり、それ以外のボクセルＢ_Ｌａ～Ｂ_Ｌｃ、Ｂ_Ｌｅの反射強度Ｉ_Ｌは「０．１」であるとする。なお、説明の簡単のため、ボクセルデータとスキャンデータとは、車両の進行方向の移動のみによりマッチングできるものとする。

この場合、図７からわかるように、スキャンデータのボクセルＢ_Ｌａ～Ｂ_Ｌｅが、それぞれボクセルデータのボクセルＢ_Ｍ１～Ｂ_Ｍ５に対応すると判断するのが正しい。これを、「Ｉ_Ｍ・Ｉ_Ｌ」の値を使用する式（８）の評価関数を用いて判断するとどうなるかを検討する。

まず、仮にスキャンデータのボクセルＢ_Ｌａ～Ｂ_Ｌｅが、ボクセルデータのボクセルＢ_Ｍ１～Ｂ_Ｍ５にそれぞれ対応する（以下、「第１の対応関係」と呼ぶ。）ものとすると、そのときの「Ｉ_Ｍ・Ｉ_Ｌ」の値は、
Ｉ_Ｍ・Ｉ_Ｌ＝０．１×０．１＋０．１×０．１＋０．１×０．１
＋１．０×１．０＋０．１×０．１＝１．０４
となる。

一方、仮にスキャンデータのボクセルＢ_Ｌａ～Ｂ_Ｌｅが、ボクセルデータのボクセルＢ_Ｍ２～Ｂ_Ｍ６にそれぞれ対応する（以下、「第２の対応関係」と呼ぶ。）ものとすると、そのときの「Ｉ_Ｍ・Ｉ_Ｌ」の値は、
Ｉ_Ｍ・Ｉ_Ｌ＝０．１×０．１＋０．１×０．１＋１．０×０．１
＋０．１×１．０＋０．１×０．１＝０．２３
となる。式（８）の評価関数値が大きくなるのは、「Ｉ_Ｍ・Ｉ_Ｌ」の値が大きい方、即ち、第１の対応関係の方であり、これは図７に示すように正しい対応関係であることがわかる。

因みに、図７において、壁のボクセルＢ_Ｍ４の部分の反射強度が周囲と同様であり、ボクセルデータのボクセルＢ_Ｍ４の反射強度平均値Ｉ_Ｍが周囲と同様に「０．１」であった場合、ライダにより得られるスキャンデータのボクセルＢ_Ｌｄの反射強度平均値Ｉ_Ｌも「０．１」となる。この場合、第１の対応関係と第２の対応関係のいずれにおいても、「Ｉ_Ｍ・Ｉ_Ｌ」の値は、
Ｉ_Ｍ・Ｉ_Ｌ＝０．１×０．１＋０．１×０．１＋０．１×０．１
＋０．１×０．１＋０．１×０．１＝０．０５
となって一致してしまい、第１の対応関係と第２の対応関係のいずれが正しいかを判別することは難しい。

このように、第１実施例では、周辺構造物が少なかったり、同じような構造物が連続していたりしても、スキャンマッチングにおいてボクセルデータとスキャンデータの反射強度平均値を利用し、それらの積を重み付け値として評価関数に含めることにより、道路標識や白線などの反射強度の高いオブジェクトを含むボクセルのマッチング精度を向上させることができる。その結果、車両の自己位置推定精度を向上させることが可能となる。

図８は、第１実施例による自車位置推定処理のフローチャートである。この処理は、車載機１の制御部１５が予め用意されたプログラムを実行することにより実施される。なお、車載機１はこの処理を繰り返し実行する。

まず、車載機１は、１時刻前の推定自車位置があるか否かを判定する（ステップＳ１０）。１時刻前の推定自車位置がある場合（ステップＳ１０：Ｙｅｓ）、車載機１はそれを自車位置の初期値に設定する（ステップＳ１１）。一方、１時刻前の推定自車位置が無い場合（ステップＳ１０：Ｎｏ）、車載機１は、ＧＰＳ受信機３２等の出力に基づき、自車位置の初期値を設定する（ステップＳ１２）。次に、車載機１は、速度センサ３４から車体速度を取得すると共に、ジャイロセンサ３３からヨー方向の角速度を取得する（ステップＳ１３）。そして、車載機１は、ステップＳ１３の取得結果に基づき、車両の移動距離と車両の方位変化を計算する（ステップＳ１４）。

次に、車載機１は、自車位置の初期値に、ステップＳ１４で計算した移動距離と方位変化を加算し、予測自車位置を算出する（ステップＳ１５）。そして、車載機１は、ステップＳ１５で算出した予測自車位置に基づき、地図ＤＢ１０を参照して、自車位置周辺に存在するボクセルのボクセルデータを取得する（ステップＳ１６）。このボクセルデータには、図６（Ａ）に示すように反射強度平均値Ｉ_Ｍが含まれている。さらに、車載機１は、ステップＳ１５で算出した予測自車位置に基づき、ライダ３０から得られたスキャンデータをボクセルごとに分割する（ステップＳ１７）。この際、図６（Ｂ）に示すように、ボクセル内のスキャンデータの反射強度平均値Ｉ_Ｌが算出される。

次に、車載機１は、評価関数を用いてＮＤＴスキャンマッチングの計算を行う。具体的には、車載機１は、式（８）及び式（９）に基づき、評価関数Ｅ_ｋ及び総合評価関数Ｅを算出し、総合評価関数Ｅが最大となる推定パラメータ、即ち、移動ベクトルｔ_ｘ、ｔ_ｙ、ｔｚ及び回転角度ｔ_Ψを算出する（ステップＳ１８）。そして、車載機１は、得られた移動ベクトルｔ_ｘ、ｔ_ｙ、ｔ_ｚ及び回転角度ｔ_Ψに基づいて、推定自車位置を修正する（ステップＳ１９）。こうして、自車位置推定処理は終了する。

（第２実施例）
第２実施例では、第１実施例と同様にボクセルデータとスキャンデータの反射強度を利用するが、ボクセル内の点群の各点の反射強度を用いて平均ベクトルや共分散行列などのパラメータを算出する点で第１実施例と異なる。

具体的には、まず、地図作成用点群データからＮＤ地図のボクセルデータを生成する際に、各点の反射強度Ｉ_Ｍ（ｉ）を用いて、以下の式（１０）により平均ベクトルを算出し、式（１１）により共分散行列を算出する。

なお、ＮＤ地図のボクセルデータは、図３（Ａ）に示すデータ構造を有し、その内容は図４（Ａ）に示すものとなる。

また、ライダによるスキャンデータについても、各ボクセル内の各点の反射強度Ｉ_Ｌ（ｊ）を用いて、以下の式（１２）により平均値ベクトルＬを算出する。

そして、車載機１は、式（６）の評価関数Ｅ（ｋ）及び式（７）の総合評価関数Ｅを用いてスキャンマッチングを行う。

式（１０）～（１２）により得たＮＤ地図のボクセルデータ及びスキャンデータは、３次元的な位置情報に対して反射強度の特徴を加えた表現となる。例えば、ほぼ平らな道路面の場合は、点群の位置としてはｘｙ平面上で一様であるため、ｘ方向とｙ方向の平均や分散は一定である。しかし、白線や横断歩道など、反射強度の高い部分があると、それによりｘ方向とｙ方向の平均ベクトルや共分散行列に特徴が生まれる。

図９は、あるＮＤ地図のボクセルデータの２次元正規分布を示す。図９（Ａ）は、反射強度を利用せずに算出した２次元正規分布５１を示す。複数の点Ｄｍの反射強度は考慮されないため、各点Ｄｍの位置に基づいて正規分布５１が得られる。これに対し、図９（Ｂ）は、第２実施例により各点Ｄｍの反射強度を用いて算出した２次元正規分布５２を示す。この例では、複数の点Ｄｍに反射強度の高い点Ｄｍｘが含まれていることにより、正規分布５２は正規分布５１よりも点Ｄｍｘの方向に偏った分布となり、反射強度の高い部分の特徴を反映したものとなる。

このように、第２実施例では、ＮＤ地図のボクセルデータにおいて各点の反射強度を用いて平均ベクトルと共分散行列を算出するとともに、ライダ３０によるスキャンデータについても各点の反射強度を用いて平均値ベクトルを算出する。よって、道路周辺が形状的に変化の乏しい環境であっても、反射強度に変化があれば、その反射強度により平均ベクトル、共分散行列などのパラメータに特徴を持たせることができ、スキャンマッチング時の一致と不一致の違いが大きくなる。従って、スキャンマッチングの精度が向上し、その結果、自車位置推定精度を向上させることができる。

第２実施例における自車位置推定処理は、基本的に図８に示す第１実施例の場合と同様である。但し、ステップＳ１６において地図ＤＢ１０から取得するボクセルデータは、図４（Ａ）に示すデータ構造及び内容を有するが、その平均ベクトルμ及び共分散行列Ｖはそれぞれ前述の式（１０）、（１１）により算出されたものとなっている。また、ステップＳ１７においては、車載機１は、各ボクセル内の各点の反射強度Ｉ_Ｌ（ｊ）を用いて平均値ベクトルＬを算出する。そして、ステップＳ１８では、式（６）、（７）の評価関数を用いてスキャンマッチングが行われる。

（変形例）
上記の第１実施例と第２実施例を組み合わせて実施してもよい。その場合、ＮＤ地図のボクセルデータは、図６（Ａ）に示すようにボクセル内の各点の反射強度の平均値である反射強度平均値Ｉ_Ｍを含む。また、図６（Ｂ）に示すように、ライダによるスキャンデータについても、各点の反射強度に基づいて反射強度平均値Ｉ_Ｌを算出する。これに加えて、ボクセルデータに含まれる平均ベクトルμ及び共分散行列Ｖは、それぞれ前述の式（１０）、（１１）で示すように各点の反射強度Ｉ_Ｍ（ｉ）を用いて算出されたものとなっている。また、ライダによるスキャンデータの平均ベクトルＬも、前述の式（１２）に示すようにボクセル内の各点の反射強度Ｉ_Ｌ（ｊ）を用いて算出される。そして、これらのボクセルデータとスキャンデータを用いて、式（８）、（９）により総合評価関数Ｅが最大となるようにスキャンマッチングを行う。

１ 車載機
２ サーバ装置
１０ 地図ＤＢ
２０ 配信地図ＤＢ
１１、２１ 通信部
１２、２２ 記憶部
１５、２５ 制御部
１３ センサ部
１４ 入力部
１６ 出力部

Claims (1)

1. 移動体の自己位置を推定する自己位置推定装置であって、
   所定サイズの３次元仮想領域単位で構成され、前記３次元仮想領域内に存在する物体に対して照射した光の反射光に対応する第１点群の特徴を示す第１点群特徴データを含む地図データを取得する地図データ取得手段と、
   前記３次元仮想領域毎に、当該３次元仮想領域内に存在する物体に対して照射した光の反射光を受光する計測部により計測して得た第２点群の特徴を示す第２点群特徴データを含む計測データを生成する計測データ生成手段と、
   前記３次元仮想領域毎の前記第１点群特徴データと前記第２点群特徴データとに基づいて、前記自己位置を推定する推定手段と、
   を備え、
   前記第１点群特徴データは前記第１点群の反射強度が反映されたデータであり、前記第２点群特徴データは前記第２点群の反射強度が反映されたデータである自己位置推定装置。

Images