知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024022650
(43)【公開日】2024-02-16
(54)【発明の名称】情報処理装置、制御方法、プログラム及び記憶媒体
(51)【国際特許分類】
G01C 21/30 20060101AFI20240208BHJP
G08G 1/0969 20060101ALI20240208BHJP
G01S 17/89 20200101ALI20240208BHJP
【ＦＩ】
G01C21/30
G08G1/0969
G01S17/89
【審査請求】有
【請求項の数】1
【出願形態】ＯＬ
(21)【出願番号】P 2023206837
(22)【出願日】2023-12-07
(62)【分割の表示】P 2021562716の分割
【原出願日】2020-12-03
(31)【優先権主張番号】P 2019219675
(32)【優先日】2019-12-04
(33)【優先権主張国・地域又は機関】JP
(71)【出願人】
【識別番号】000005016
【氏名又は名称】パイオニア株式会社
(74)【代理人】
【識別番号】100107331
【弁理士】
【氏名又は名称】中村 聡延
(72)【発明者】
【氏名】加藤 正浩
(57)【要約】
【課題】推定した位置の信頼度を好適に表した指標を算出することが可能な情報処理装置を提供する。
【解決手段】車載機１の制御部１５は、ライダ２が出力する点群データを取得する。そして、制御部１５は、取得した点群データと、空間を区切った単位領域（ボクセル）ごとの物体の位置情報であるボクセルデータＶＤとの照合により、点群データを構成する計測点とボクセルの各々との対応付けを行う。制御部１５は、ボクセルデータＶＤが存在するボクセルのいずれかと対応付けられた計測点と、当該単位領域における物体の位置情報とに基づき、ライダ２を備える移動体の位置推定を行う。制御部１５は、点群データの計測点の数に対する、ボクセルのいずれかと対応付けられた計測点の数の割合であるＤＡＲを用いた、位置推定により得られた位置の信頼度指標を算出する。
【選択図】図１２
【解決手段】車載機１の制御部１５は、ライダ２が出力する点群データを取得する。そして、制御部１５は、取得した点群データと、空間を区切った単位領域（ボクセル）ごとの物体の位置情報であるボクセルデータＶＤとの照合により、点群データを構成する計測点とボクセルの各々との対応付けを行う。制御部１５は、ボクセルデータＶＤが存在するボクセルのいずれかと対応付けられた計測点と、当該単位領域における物体の位置情報とに基づき、ライダ２を備える移動体の位置推定を行う。制御部１５は、点群データの計測点の数に対する、ボクセルのいずれかと対応付けられた計測点の数の割合であるＤＡＲを用いた、位置推定により得られた位置の信頼度指標を算出する。
【選択図】図１２
【特許請求の範囲】
【請求項1】
計測装置が出力する点群データを取得する取得部と、
前記点群データと、地図情報に含まれる物体の位置情報との照合により、前記点群データを構成する計測点と前記地図情報との対応付けを行う対応付け部と、
前記地図情報と対応付けられた計測点と、当該地図情報における物体の位置情報とに基づき、前記計測装置を備える移動体の位置推定を行う位置推定部と、
前記点群データの計測点の数に対する、当該計測点のうち前記地図情報と対応付けられた計測点の数の割合を用いた、前記位置推定により得られた位置の信頼度指標を算出する算出部と、
を備える、情報処理装置。
前記点群データと、地図情報に含まれる物体の位置情報との照合により、前記点群データを構成する計測点と前記地図情報との対応付けを行う対応付け部と、
前記地図情報と対応付けられた計測点と、当該地図情報における物体の位置情報とに基づき、前記計測装置を備える移動体の位置推定を行う位置推定部と、
前記点群データの計測点の数に対する、当該計測点のうち前記地図情報と対応付けられた計測点の数の割合を用いた、前記位置推定により得られた位置の信頼度指標を算出する算出部と、
を備える、情報処理装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、位置推定において用いる信頼度指標の算出に関する。
【背景技術】
【0002】
従来から、レーザスキャナなどの計測装置を用いて計測した周辺物体の形状データを、予め周辺物体の形状が記憶された地図情報と照合（マッチング）することで、車両の自己位置を推定する技術が知られている。例えば、特許文献１には、空間を所定の規則で分割したボクセル中における検出物が静止物か移動物かを判定し、静止物が存在するボクセルを対象として地図情報と計測データとのマッチングを行う自律移動システムが開示されている。また、特許文献２には、ボクセル毎の静止物体の平均ベクトルと共分散行列とを含むボクセルデータとライダが出力する点群データとの照合により自車位置推定を行うスキャンマッチング手法が開示されている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】国際公開ＷＯ２０１３／０７６８２９
【特許文献2】国際公開ＷＯ２０１８／２２１４５３
【発明の概要】
【発明が解決しようとする課題】
【0004】
ボクセルデータとライダが出力する点群データとの照合により自車位置推定を行う場合、当該照合の度合を示すスコア値が最大となる推定パラメータを探索する処理が行われる。この探索処理は、所定の処理時間内に完了できるように、探索範囲を限定して行うため、スコア値が最大であっても，局所解に陥ってしまっている可能性がある。また、計測対象の地物に対する他車両によるオクルージョンが発生したときでも、上述のスコア値はあまり悪化しない場合があり、そのような場合には、正確な推定パラメータが算出できていない可能性がある。このように、上述のスコア値では、推定した位置の信頼度を表す指標として不十分な場合があった。
【0005】
本発明は、上記のような課題を解決するためになされたものであり、推定した位置の信頼度を好適に表した指標を算出することが可能な情報処理装置を提供することを主な目的とする。
【課題を解決するための手段】
【0006】
請求項に記載の発明は、情報処理装置であって、計測装置が出力する点群データを取得する取得部と、前記点群データと、空間を区切った単位領域ごとの物体の位置情報との照合により、前記点群データを構成する計測点と前記単位領域の各々との対応付けを行う対応付け部と、前記単位領域のいずれかと対応付けられた計測点と、当該単位領域における物体の位置情報とに基づき、前記計測装置を備える移動体の位置推定を行う位置推定部と、前記点群データの計測点の数に対する、当該計測点のうち前記単位領域と対応付けられた計測点の数の割合を用いた、前記位置推定により得られた位置の信頼度指標を算出する算出部と、を備えることを特徴とする。
【0007】
また、請求項に記載の発明は、情報処理装置が実行する制御方法であって、計測装置が出力する点群データを取得し、前記点群データと、空間を区切った単位領域ごとの物体の位置情報との照合により、前記点群データを構成する計測点と前記単位領域の各々との対応付けを行い、前記単位領域のいずれかと対応付けられた計測点と、当該単位領域における物体の位置情報とに基づき、前記計測装置を備える移動体の位置推定を行い、前記点群データの計測点の数に対する、当該計測点のうち前記単位領域と対応付けられた計測点の数の割合を用いた、前記位置推定により得られた位置の信頼度指標を算出することを特徴とする。
【0008】
また、請求項に記載の発明は、計測装置が出力する点群データを取得する取得部と、前記点群データと、空間を区切った単位領域ごとの物体の位置情報との照合により、前記点群データを構成する計測点と前記単位領域の各々との対応付けを行う対応付け部と、前記単位領域のいずれかと対応付けられた計測点と、当該単位領域における物体の位置情報とに基づき、前記計測装置を備える移動体の位置推定を行う位置推定部と、前記点群データの計測点の数に対する、当該計測点のうち前記単位領域と対応付けられた計測点の数の割合を用いた、前記位置推定により得られた位置の信頼度指標を算出する算出部としてコンピュータを機能させるプログラムであることを特徴とする。
【図面の簡単な説明】
【0009】
【図1】運転支援システムの概略構成図である。
【図2】車載機の機能的構成を示すブロック図である。
【図3】状態変数ベクトルを２次元直交座標で表した図である。
【図4】ボクセルデータの概略的なデータ構造の一例を示す。
【図5】自車位置推定部の機能ブロックの一例である。
【図6】車載機を搭載する車両周辺の俯瞰図を示す。
【図7】車載機を搭載する車両周辺の俯瞰図を示す。
【図8】ワールド座標系における２次元平面上での、ボクセルデータが存在するボクセルとこれらのボクセル付近の位置を示す計測点との位置関係を示す。
【図9】推定パラメータの値とスコア値との関係を示すグラフである。
【図11】オクルージョンに起因してＤＡＲが閾値に達しないときの車両周辺の俯瞰図を示す。
【図12】推定パラメータを推定する度にＤＡＲに基づき推定パラメータの再探索の要否を判定する自車位置推定処理の手順を示すフローチャートの一例である。
【図13】推定パラメータの探索を行う回数をＤＡＲに基づき決定する自車位置推定処理の手順を示すフローチャートの一例である。
【図14】変形例に係る自車位置推定処理の手順を示すフローチャートの一例である。
【発明を実施するための形態】
【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】
【数1】
【0037】
【数2】
【0038】
次に、ボクセルデータＶＤを用いたＮＤＴスキャンマッチングの概要について説明する。
【0039】
車両を想定したＮＤＴによるスキャンマッチングは、道路平面（ここではｘｙ座標とする）内の移動量及び車両の向きを要素とした推定パラメータ
Ｐ＝［ｔｘ、ｔｙ、ｔψ］Ｔ
を推定することとなる。ここで、「ｔｘ」は、ｘ方向の移動量を示し、「ｔｙ」は、ｙ方向の移動量を示し、「ｔψ」は、ヨー角を示す。
Ｐ＝［ｔｘ、ｔｙ、ｔψ］Ｔ
を推定することとなる。ここで、「ｔｘ」は、ｘ方向の移動量を示し、「ｔｙ」は、ｙ方向の移動量を示し、「ｔψ」は、ヨー角を示す。
【0040】
また、ライダ２により得られた点群データに対して、マッチングさせるべきボクセルとの対応付けを行い、対応するボクセルｎでの任意の点の座標を
ＸＬ（ｊ）＝［ｘｎ（ｊ）、ｙｎ（ｊ）、ｚｎ（ｊ）］Ｔ
とすると、ボクセルｎでのＸＬ（ｊ）の平均値「Ｌ´ｎ」は、以下の式（３）により表される。
ＸＬ（ｊ）＝［ｘｎ（ｊ）、ｙｎ（ｊ）、ｚｎ（ｊ）］Ｔ
とすると、ボクセルｎでのＸＬ（ｊ）の平均値「Ｌ´ｎ」は、以下の式（３）により表される。
【0041】
【数3】
【0042】
【数4】
【0043】
【数5】
【0044】
そして、車載機１は、以下の式（６）により示される、マッチングの対象となる全てのボクセルを対象とした総合的な評価関数値（「スコア値」とも呼ぶ。）「Ｅ（ｋ）」を算出する。
【0045】
【数6】
【0046】
【数7】
【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】
そして、自車位置推定部１８は、ステップＳ１６で算出したＤＡＲが所定の閾値未満であるか否か判定する（ステップＳ１７）。そして、自車位置推定部１８は、ＤＡＲが閾値未満である場合（ステップＳ１７；Ｙｅｓ）、推定パラメータＰを探索した回数を表す変数ｎが探索上限回数未満であるか否か判定する（ステップＳ１８）。そして、自車位置推定部１８は、推定パラメータＰを探索した回数を表す変数ｎが探索上限回数未満である場合（ステップＳ１８；Ｙｅｓ）、変数ｎを１だけ加算する（ステップＳ１９）。その後、自車位置推定部１８は、ステップＳ１３へ進み、ステップＳ１５で算出した自車位置に基づき、現処理時刻において処理対象となっている点群データのワールド座標系への座標変換を再び実行する。
【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】
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
