知財求人 - 知財ポータルサイト「IP Force」
▶ パイオニア株式会社の特許一覧 ▶ パイオニアスマートセンシングイノベーションズ株式会社の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2025-01-06
(45)【発行日】2025-01-15
(54)【発明の名称】情報処理装置、制御方法、プログラム及び記憶媒体
(51)【国際特許分類】
G01S 17/86 20200101AFI20250107BHJP
G01C 21/28 20060101ALI20250107BHJP
G01S 17/89 20200101ALI20250107BHJP
【ＦＩ】
G01S17/86
G01C21/28
G01S17/89
【請求項の数】
9
(21)【出願番号】P 2020196939
(22)【出願日】2020-11-27
(65)【公開番号】P2022085320
(43)【公開日】2022-06-08
【審査請求日】2023-10-20
(73)【特許権者】
【識別番号】000005016
【氏名又は名称】パイオニア株式会社
(73)【特許権者】
【識別番号】520001073
【氏名又は名称】パイオニアスマートセンシングイノベーションズ株式会社
(74)【代理人】
【識別番号】100107331
【弁理士】
【氏名又は名称】中村 聡延
(72)【発明者】
【氏名】加藤 正浩
【審査官】▲高▼場 正光
(56)【参考文献】
【文献】特開２０１９－０４０４４５（ＪＰ，Ａ）
【文献】国際公開第２０２０／１３３１１８（ＷＯ，Ａ１）
【文献】特開２０１８－０８４５７３（ＪＰ，Ａ）
【文献】国際公開第２０１９／１８８７０４（ＷＯ，Ａ１）
【文献】TURNER, Eric 外２名，“Fast, Automated, Scalable Generation of Textured 3D Models of Indoor Environments”，IEEE JOURNAL OF SELECTED TOPICS IN SIGNAL PROCESSING [online]，Volume 9, Number 3，2015年03月18日，Pages 409-421，<URL: https://doi.org/10.1109/JSTSP.2014.2381153 >
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０１Ｓ ７／４８ － Ｇ０１Ｓ ７／５１
Ｇ０１Ｓ １７／００ － Ｇ０１Ｓ １７／９５
(57)【特許請求の範囲】
【請求項1】
第１位置算出方法による移動体の測位位置を取得する位置取得手段と、前記測位位置に基づき第２位置算出方法の基準位置を探索する探索範囲を設定し、当該探索範囲内において、単位領域であるボクセルごとの物体の位置を表すボクセルデータと、前記移動体に設けられた計測装置の出力に基づく点群データとを、前記基準位置に基づき照合する第１照合を行った場合に、前記第１照合の第１評価指標が第１所定値以上となる前記基準位置を探索する基準位置探索手段と、
前記ボクセルデータと前記点群データとを前記基準位置に基づき照合する第２照合に基づき、前記第２位置算出方法による前記移動体の位置推定を行う位置推定手段と、を有し、
前記基準位置探索手段は、前記測位位置の測位の精度を表す精度情報に基づいて、前記探索範囲のサイズを決定し、
直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記直前の処理時刻での前記第２照合の結果に基づき算出される前記第２位置算出方法による位置推定の第２評価指標が第２所定値未満である場合、
前記基準位置探索手段は、現在の処理時刻の前記点群データに基づき、前記基準位置を探索し、
前記位置推定手段は、当該基準位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換し、
前記位置推定手段は、直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記第２評価指標が前記第２所定値以上である場合、前記直前の処理時刻において決定された前記移動体の推定位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換する、情報処理装置。
【請求項2】
前記第１位置算出方法は、ＧＮＳＳによる測位であり、前記基準位置探索手段は、前記ＧＮＳＳの精度情報に基づいて、前記探索範囲のサイズを決定する、請求項１に記載の情報処理装置。
【請求項3】
前記基準位置探索手段は、前記精度情報が示す精度が高いほど、前記探索範囲のサイズを小さくする、請求項１または２に記載の情報処理装置。
【請求項4】
前記位置推定手段は、直前の処理時刻において決定された前記移動体の推定位置が存在しない場合、前記基準位置に基づき前記点群データの座標系を前記ボクセルデータの座標系に変換する、請求項１～３のいずれか一項に記載の情報処理装置。
【請求項5】
前記移動体は車両であり、前記基準位置探索手段は、前記探索範囲内に存在するボクセルごとの高さ方向の位置の度数分布に基づき、前記車両が存在する候補ボクセルを決定する、請求項１～４のいずれか一項に記載の情報処理装置。
【請求項6】
前記移動体は船舶であり、前記基準位置探索手段は、前記探索範囲内にボクセルデータが存在しない仮想のボクセルを、前記船舶が存在する候補ボクセルとして決定する、請求項１～４のいずれか一項に記載の情報処理装置。
【請求項7】
コンピュータが実行する制御方法であって、第１位置算出方法による移動体の測位位置を取得する位置取得工程と、
前記測位位置に基づき第２位置算出方法の基準位置を探索する探索範囲を設定し、当該探索範囲内において、単位領域であるボクセルごとの物体の位置を表すボクセルデータと、前記移動体に設けられた計測装置の出力に基づく点群データとを、前記基準位置に基づき照合する第１照合を行った場合に、前記第１照合の第１評価指標が第１所定値以上となる前記基準位置を探索する基準位置探索工程と、
前記ボクセルデータと前記点群データとを前記基準位置に基づき照合する第２照合に基づき、前記第２位置算出方法による前記移動体の位置推定を行う位置推定工程と、を有し、
前記基準位置探索工程は、前記測位位置の測位の精度を表す精度情報に基づいて、前記探索範囲のサイズを決定し、
直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記直前の処理時刻での前記第２照合の結果に基づき算出される前記第２位置算出方法による位置推定の第２評価指標が第２所定値未満である場合、
前記基準位置探索工程は、現在の処理時刻の前記点群データに基づき、前記基準位置を探索し、
前記位置推定工程は、当該基準位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換し、
前記位置推定工程は、直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記第２評価指標が前記第２所定値以上である場合、前記直前の処理時刻において決定された前記移動体の推定位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換する、制御方法。
【請求項8】
第１位置算出方法による移動体の測位位置を取得する位置取得手段と、前記測位位置に基づき第２位置算出方法の基準位置を探索する探索範囲を設定し、当該探索範囲内において、単位領域であるボクセルごとの物体の位置を表すボクセルデータと、前記移動体に設けられた計測装置の出力に基づく点群データとを、前記基準位置に基づき照合する第１照合を行った場合に、前記第１照合の第１評価指標が第１所定値以上となる前記基準位置を探索する基準位置探索手段と、
前記ボクセルデータと前記点群データとを前記基準位置に基づき照合する第２照合に基づき、前記第２位置算出方法による前記移動体の位置推定を行う位置推定手段
としてコンピュータを機能させ、
前記基準位置探索手段は、前記測位位置の測位の精度を表す精度情報に基づいて、前記探索範囲のサイズを決定し、
直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記直前の処理時刻での前記第２照合の結果に基づき算出される前記第２位置算出方法による位置推定の第２評価指標が第２所定値未満である場合、
前記基準位置探索手段は、現在の処理時刻の前記点群データに基づき、前記基準位置を探索し、
前記位置推定手段は、当該基準位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換し、
前記位置推定手段は、直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記第２評価指標が前記第２所定値以上である場合、前記直前の処理時刻において決定された前記移動体の推定位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換する、プログラム。
【請求項9】
請求項８に記載のプログラムを記憶した記憶媒体。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、位置推定に関する。
【背景技術】
【0002】
従来から、レーザスキャナなどの計測装置を用いて計測した周辺物体の形状データを、予め周辺物体の形状が記憶された地図情報と照合（マッチング）することで、車両の自己位置を推定する技術が知られている。例えば、特許文献１には、空間を所定の規則で分割したボクセル中における検出物が静止物か移動物かを判定し、静止物が存在するボクセルを対象として地図情報と計測データとのマッチングを行う自律移動システムが開示されている。また、特許文献２には、ボクセル毎の静止物体の平均ベクトルと共分散行列とを含むボクセルデータとライダが出力する点群データとの照合により自己位置推定を行うスキャンマッチング手法が開示されている。
【先行技術文献】
【特許文献】
【0003】
【文献】国際公開ＷＯ２０１３／０７６８２９
【文献】国際公開ＷＯ２０１８／２２１４５３
【発明の概要】
【発明が解決しようとする課題】
【0004】
昨今、自動車分野だけでなく、船舶においても、自動操船システム検討が進められており、安全な自動操船には精度の良い自己位置推定は同様に重要である。外洋では周辺に構造物が少ないためＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）により自己位置を測位可能であるが、都市部の沿岸や河川では高層ビル等が隣接するため、受信衛星数の低下やマルチパスなどによりＧＮＳＳ電波の受信環境は悪く、精度の良い測位はできないことが多い。よって、船舶の場合においても、上記のスキャンマッチングによる高精度な位置推定が好適に適用される。
【0005】
一方、自己位置推定処理の開始時は、自動車も船舶も、ＧＮＳＳによる測位位置をイニシャル位置としている。ＧＮＳＳによる測位には誤差が伴うが、その誤差が大きくなければ，上記のスキャンマッチングのためのイニシャル位置として使用することができ，結果として高精度な位置推定が可能である。しかし、ＧＮＳＳ受信環境が悪い場合は，大きく離れた場所をイニシャル位置としてしまい、スキャンマッチングにおいて適切に最適解が求められない状況もあり得る。よって、このような場合において適切に位置推定を行うには、確からしいイニシャル位置を探索する必要がある。また，何らかの原因で推定自己位置が大きくずれた場合においても、確からしいイニシャル位置を同様に設定する必要がある。
【0006】
本開示は、上記のような課題を解決するためになされたものであり、自己位置推定を好適に実行することが可能な情報処理装置を提供することを主な目的とする。
【課題を解決するための手段】
【0007】
請求項に記載の発明は、
第１位置算出方法による移動体の測位位置を取得する位置取得手段と、
前記測位位置に基づき第２位置算出方法の基準位置を探索する探索範囲を設定し、当該探索範囲内において、単位領域であるボクセルごとの物体の位置を表すボクセルデータと、前記移動体に設けられた計測装置の出力に基づく点群データとを、前記基準位置に基づき照合する第１照合を行った場合に、前記第１照合の第１評価指標が第１所定値以上となる前記基準位置を探索する基準位置探索手段と、
前記ボクセルデータと前記点群データとの第２照合に基づき、前記第２位置算出方法による前記移動体の位置推定を行う位置推定手段と、を有し、
前記基準位置探索手段は、前記測位位置の測位の精度を表す精度情報に基づいて、前記探索範囲のサイズを決定し、
直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記直前の処理時刻での前記第２照合の結果に基づき算出される前記第２位置算出方法による位置推定の第２評価指標が第２所定値未満である場合、
前記基準位置探索手段は、現在の処理時刻の前記点群データに基づき、前記基準位置を探索し、
前記位置推定手段は、当該基準位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換し、
前記位置推定手段は、直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記第２評価指標が前記第２所定値以上である場合、前記直前の処理時刻において決定された前記移動体の推定位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換する、
情報処理装置である。
第１位置算出方法による移動体の測位位置を取得する位置取得手段と、
前記測位位置に基づき第２位置算出方法の基準位置を探索する探索範囲を設定し、当該探索範囲内において、単位領域であるボクセルごとの物体の位置を表すボクセルデータと、前記移動体に設けられた計測装置の出力に基づく点群データとを、前記基準位置に基づき照合する第１照合を行った場合に、前記第１照合の第１評価指標が第１所定値以上となる前記基準位置を探索する基準位置探索手段と、
前記ボクセルデータと前記点群データとの第２照合に基づき、前記第２位置算出方法による前記移動体の位置推定を行う位置推定手段と、を有し、
前記基準位置探索手段は、前記測位位置の測位の精度を表す精度情報に基づいて、前記探索範囲のサイズを決定し、
直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記直前の処理時刻での前記第２照合の結果に基づき算出される前記第２位置算出方法による位置推定の第２評価指標が第２所定値未満である場合、
前記基準位置探索手段は、現在の処理時刻の前記点群データに基づき、前記基準位置を探索し、
前記位置推定手段は、当該基準位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換し、
前記位置推定手段は、直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記第２評価指標が前記第２所定値以上である場合、前記直前の処理時刻において決定された前記移動体の推定位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換する、
情報処理装置である。
【0008】
また、請求項に記載の発明は、
コンピュータが実行する制御方法であって、
第１位置算出方法による移動体の測位位置を取得する位置取得工程と、
前記測位位置に基づき第２位置算出方法の基準位置を探索する探索範囲を設定し、当該探索範囲内において、単位領域であるボクセルごとの物体の位置を表すボクセルデータと、前記移動体に設けられた計測装置の出力に基づく点群データとを、前記基準位置に基づき照合する第１照合を行った場合に、前記第１照合の第１評価指標が第１所定値以上となる前記基準位置を探索する基準位置探索工程と、
前記ボクセルデータと前記点群データとの第２照合に基づき、前記第２位置算出方法による前記移動体の位置推定を行う位置推定工程と、を有し、
前記基準位置探索工程は、前記測位位置の測位の精度を表す精度情報に基づいて、前記探索範囲のサイズを決定し、
直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記直前の処理時刻での前記第２照合の結果に基づき算出される前記第２位置算出方法による位置推定の第２評価指標が第２所定値未満である場合、
前記基準位置探索工程は、現在の処理時刻の前記点群データに基づき、前記基準位置を探索し、
前記位置推定工程は、当該基準位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換し、
前記位置推定工程は、直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記第２評価指標が前記第２所定値以上である場合、前記直前の処理時刻において決定された前記移動体の推定位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換する、制御方法である。
コンピュータが実行する制御方法であって、
第１位置算出方法による移動体の測位位置を取得する位置取得工程と、
前記測位位置に基づき第２位置算出方法の基準位置を探索する探索範囲を設定し、当該探索範囲内において、単位領域であるボクセルごとの物体の位置を表すボクセルデータと、前記移動体に設けられた計測装置の出力に基づく点群データとを、前記基準位置に基づき照合する第１照合を行った場合に、前記第１照合の第１評価指標が第１所定値以上となる前記基準位置を探索する基準位置探索工程と、
前記ボクセルデータと前記点群データとの第２照合に基づき、前記第２位置算出方法による前記移動体の位置推定を行う位置推定工程と、を有し、
前記基準位置探索工程は、前記測位位置の測位の精度を表す精度情報に基づいて、前記探索範囲のサイズを決定し、
直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記直前の処理時刻での前記第２照合の結果に基づき算出される前記第２位置算出方法による位置推定の第２評価指標が第２所定値未満である場合、
前記基準位置探索工程は、現在の処理時刻の前記点群データに基づき、前記基準位置を探索し、
前記位置推定工程は、当該基準位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換し、
前記位置推定工程は、直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記第２評価指標が前記第２所定値以上である場合、前記直前の処理時刻において決定された前記移動体の推定位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換する、制御方法である。
【0009】
また、請求項に記載の発明は、
第１位置算出方法による移動体の測位位置を取得する位置取得手段と、
前記測位位置に基づき第２位置算出方法の基準位置を探索する探索範囲を設定し、当該探索範囲内において、単位領域であるボクセルごとの物体の位置を表すボクセルデータと、前記移動体に設けられた計測装置の出力に基づく点群データとを、前記基準位置に基づき照合する第１照合を行った場合に、前記第１照合の第１評価指標が第１所定値以上となる前記基準位置を探索する基準位置探索手段と、
前記ボクセルデータと前記点群データとの第２照合に基づき、前記第２位置算出方法による前記移動体の位置推定を行う位置推定手段
としてコンピュータを機能させ、
前記基準位置探索手段は、前記測位位置の測位の精度を表す精度情報に基づいて、前記探索範囲のサイズを決定し、
直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記直前の処理時刻での前記第２照合の結果に基づき算出される前記第２位置算出方法による位置推定の第２評価指標が第２所定値未満である場合、
前記基準位置探索手段は、現在の処理時刻の前記点群データに基づき、前記基準位置を探索し、
前記位置推定手段は、当該基準位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換し、
前記位置推定手段は、直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記第２評価指標が前記第２所定値以上である場合、前記直前の処理時刻において決定された前記移動体の推定位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換する、プログラムである。
第１位置算出方法による移動体の測位位置を取得する位置取得手段と、
前記測位位置に基づき第２位置算出方法の基準位置を探索する探索範囲を設定し、当該探索範囲内において、単位領域であるボクセルごとの物体の位置を表すボクセルデータと、前記移動体に設けられた計測装置の出力に基づく点群データとを、前記基準位置に基づき照合する第１照合を行った場合に、前記第１照合の第１評価指標が第１所定値以上となる前記基準位置を探索する基準位置探索手段と、
前記ボクセルデータと前記点群データとの第２照合に基づき、前記第２位置算出方法による前記移動体の位置推定を行う位置推定手段
としてコンピュータを機能させ、
前記基準位置探索手段は、前記測位位置の測位の精度を表す精度情報に基づいて、前記探索範囲のサイズを決定し、
直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記直前の処理時刻での前記第２照合の結果に基づき算出される前記第２位置算出方法による位置推定の第２評価指標が第２所定値未満である場合、
前記基準位置探索手段は、現在の処理時刻の前記点群データに基づき、前記基準位置を探索し、
前記位置推定手段は、当該基準位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換し、
前記位置推定手段は、直前の処理時刻において決定された前記移動体の推定位置が存在し、かつ、前記第２評価指標が前記第２所定値以上である場合、前記直前の処理時刻において決定された前記移動体の推定位置に基づき、現在の処理時刻の前記点群データの座標系を前記ボクセルデータの座標系に変換する、プログラムである。
【図面の簡単な説明】
【0010】
【図1】運転支援システムの概略構成図である。
【図2】車載機の機能的構成を示すブロック図である。
【図3】自己位置推定部が推定すべき自己位置を２次元直交座標で表した図である。
【図4】ボクセルデータの概略的なデータ構造の一例を示す。
【図5】自己位置推定部の機能ブロックの一例である。
【図6】ワールド座標系における２次元平面上での、ボクセルデータが存在するボクセルとこれらのボクセル付近の位置を示す計測点との位置関係を示す。
【図7】立体交差がある道路付近での車両の俯瞰図を示す。
【図8】ＧＮＳＳの測位位置及びイニシャル位置探索範囲を明示した道路の俯瞰図を示す。
【図9】（Ａ）イニシャル位置探索範囲内におけるボクセルを明示した図である。（Ｂ）イニシャル位置探索範囲内のボクセルデータが示すｚ座標値の度数分布を示す。
【図10】候補ボクセル及び候補ボクセルの順番を明示した俯瞰図を示す。
【図11】簡易ＮＤＴ処理を行った場合の対応付け比率の遷移を示すグラフである。
【図12】仮想ボクセル及び仮想ボクセルの順番を明示した俯瞰図を示す。
【図13】簡易ＮＤＴ処理を行った場合の対応付け比率の遷移を示すグラフである。
【図14】実施例に係る自己位置推定処理の概要を示すフローチャートの一例である。
【図15】対象移動体が車両である場合の実施例に係るイニシャル位置設定処理の手順を示すフローチャートの一例である。
【図16】対象移動体が船舶である場合の実施例に係るイニシャル位置設定処理の手順を示すフローチャートの一例である。
【図17】変形例に係る自己位置推定処理の概要を示すフローチャートの一例である。
【図18】対象移動体が車両である場合の変形例に係るイニシャル位置設定処理の手順を示すフローチャートの一例である。
【図19】対象移動体が船舶である場合の変形例に係るイニシャル位置設定処理の手順を示すフローチャートの一例である。
【発明を実施するための形態】
【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】
【数1】
【0046】
【数2】
【0047】
次に、ボクセルデータＶＤを用いたＮＤＴスキャンマッチングの概要について説明する。
【0048】
車両又は船舶を想定したＮＤＴによるスキャンマッチングは、水平面（ここではｘｙ座標とする）内の移動量及び車両又は船舶の向きを要素とした推定パラメータ
Ｐ＝［ｔｘ、ｔｙ、ｔψ］Ｔ
を推定することとなる。ここで、「ｔｘ」は、ｘ方向の移動量を示し、「ｔｙ」は、ｙ方向の移動量を示し、「ｔψ」は、ヨー角を示す。
Ｐ＝［ｔｘ、ｔｙ、ｔψ］Ｔ
を推定することとなる。ここで、「ｔｘ」は、ｘ方向の移動量を示し、「ｔｙ」は、ｙ方向の移動量を示し、「ｔψ」は、ヨー角を示す。
【0049】
また、ライダ３が出力する点群データの座標を、
ＸＬ（ｊ）＝［ｘｎ（ｊ）、ｙｎ（ｊ）、ｚｎ（ｊ）］Ｔ
とすると、ＸＬ（ｊ）の平均値「Ｌ´ｎ」は、以下の式（３）により表される。
ＸＬ（ｊ）＝［ｘｎ（ｊ）、ｙｎ（ｊ）、ｚｎ（ｊ）］Ｔ
とすると、ＸＬ（ｊ）の平均値「Ｌ´ｎ」は、以下の式（３）により表される。
【0050】
【数3】
【0051】
【数4】
【0052】
【数5】
【0053】
そして、自己位置推定部１５は、以下の式（６）により示される、マッチングの対象となる全てのボクセルを対象とした総合的な評価関数値（「スコア値」とも呼ぶ。）「Ｅ（ｋ）」を算出する。スコア値Ｅは、マッチングの適合度を示す指標となる。
【0054】
【数6】
【0055】
【数7】
【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】
次に、自己位置推定部１５は、選択された仮想ボクセルの座標により簡易ＮＤＴ処理を実行する（ステップＳ４７）。この場合、自己位置推定部１５は、簡易ＮＤＴ処理においてイニシャル位置とするｚ座標値を、ステップＳ４１で取得した予想潮位に対象移動体である船舶の水面からの高さを加算した値に設定する。
【0111】
次に、自己位置推定部１５は、対応付け比率ＤＡＲが所定値より大きいか否か判定する（ステップＳ４８）。そして、自己位置推定部１５は、対応付け比率ＤＡＲが所定値より大きい場合（ステップＳ４８；Ｙｅｓ）、簡易ＮＤＴ処理の結果を、イニシャル位置に設定する（ステップＳ４９）。
【0112】
一方、自己位置推定部１５は、対応付け比率ＤＡＲが所定値以下である場合（ステップＳ４８；Ｎｏ）、ステップＳ４５で並べた順番において最後のボクセルを対象としていたか否か判定する（ステップＳ５０）。そして、自己位置推定部１５は、最後のボクセルを対象としていた場合（ステップＳ５０；Ｙｅｓ）、イニシャル位置探索範囲Ｒｉを拡大する（ステップＳ５１）。この場合、自己位置推定部１５は、イニシャル位置探索範囲Ｒｉの境界を上下左右に５ｍずつ外側にずらし、イニシャル位置探索範囲Ｒｉを拡大する。その後、自己位置推定部１５は、ステップＳ４４へ処理を戻す。なお、この場合、自己位置推定部１５は、ステップＳ４５においてボクセルを並べる場合には、簡易ＮＤＴ処理を実行済みのボクセルを除外するとよい。
【0113】
一方、自己位置推定部１５は、ステップＳ４５で並べた順番において最後のボクセルを対象としていない場合（ステップＳ５０；Ｎｏ）、次の順番となるボクセルを選択する（ステップＳ５２）。そして、ステップＳ４７において、選択したボクセルを対象として簡易ＮＤＴ処理を行う。
【0114】
（６）変形例
以下、上述の実施例に好適な変形例について説明する。以下の変形例は、組み合わせてこれらの実施例に適用してもよい。
以下、上述の実施例に好適な変形例について説明する。以下の変形例は、組み合わせてこれらの実施例に適用してもよい。
【0115】
（変形例１）
図１４のステップＳ１５で対応付け比率ＤＡＲが所定値未満である場合に実行するイニシャル位置設定処理では、自己位置推定部１５は、イニシャル位置探索範囲ＲｉをＧＮＳＳ測位結果およびＧＮＳＳ測位の精度情報に基づき設定する代わりに、前回の推定自己位置およびＮＤＴスキャンマッチングの信頼度に基づきイニシャル位置探索範囲Ｒｉを設定してもよい。以後では、上記のイニシャル位置設定処理を、「復帰用イニシャル位置設定処理」とも呼ぶ。即ち、復帰用イニシャル位置設定処理は、自己位置推定処理を繰り返し実行中であってＮＤＴ位置ＸＮＤＴの算出に関する信頼度の指標（ここでは対応付け比率ＤＡＲ）が所定値未満の場合に実行するイニシャル位置設定処理である。
図１４のステップＳ１５で対応付け比率ＤＡＲが所定値未満である場合に実行するイニシャル位置設定処理では、自己位置推定部１５は、イニシャル位置探索範囲ＲｉをＧＮＳＳ測位結果およびＧＮＳＳ測位の精度情報に基づき設定する代わりに、前回の推定自己位置およびＮＤＴスキャンマッチングの信頼度に基づきイニシャル位置探索範囲Ｒｉを設定してもよい。以後では、上記のイニシャル位置設定処理を、「復帰用イニシャル位置設定処理」とも呼ぶ。即ち、復帰用イニシャル位置設定処理は、自己位置推定処理を繰り返し実行中であってＮＤＴ位置ＸＮＤＴの算出に関する信頼度の指標（ここでは対応付け比率ＤＡＲ）が所定値未満の場合に実行するイニシャル位置設定処理である。
【0116】
図１７は、変形例１に係る自己位置推定処理の概要を示すフローチャートの一例である。自己位置推定部１５は、図１７のフローチャートの処理を、電源がオンになった場合など、自己位置推定を行う必要が生じた場合に開始する。
【0117】
まず、自己位置推定部１５は、図１５又は図１６において説明したイニシャル位置設定処理を実行する（ステップＳ６１）。次に、自己位置推定部１５は、速度センサ４とジャイロセンサ６等に基づき検出した対象移動体の移動速度及び角速度と、前回の推定自己位置とから、デッドレコニングを実行し、予測自己位置となるＤＲ位置ＸＤＲを算出する（ステップＳ６２）。次に、自己位置推定部１５は、ライダ３の出力に基づく点群データを、ワールド座標系の点群データに変換する（ステップＳ６３）。
【0118】
そして、自己位置推定部１５は、ステップＳ６２で算出したＤＲ位置ＸＤＲを初期値として、ＮＤＴマッチング処理を実施することで、推定自己位置Ｘ＾となるＮＤＴ位置ＸＮＤＴを算出する（ステップＳ６４）。また、コントローラ１３は、ワールド座標系に変換された点群データとボクセルデータＶＤが存在するボクセルとの対応付けの結果に基づき、対応付け比率ＤＡＲの算出を行う。
【0119】
そして、自己位置推定部１５は、対応付け比率ＤＡＲが所定値未満であるか否か判定する（ステップＳ６５）。そして、自己位置推定部１５は、対応付け比率ＤＡＲが所定値未満であると判定した場合（ステップＳ６５；Ｙｅｓ）、復帰用イニシャル位置設定処理を実行する（ステップＳ６６）。この場合、自己位置推定部１５は、ＮＤＴスキャンマッチング（及び算出されたＮＤＴ位置ＸＮＤＴ）の信頼度が低下しており、復帰用イニシャル位置設定処理を実行する必要があると判定する。復帰用イニシャル位置設定処理の詳細については、図１８及び図１９を参照して説明する。一方、自己位置推定部１５は、対応付け比率ＤＡＲが所定値以上である場合（ステップＳ６５；Ｎｏ）、ＮＤＴ位置ＸＮＤＴの信頼度が維持されていると判断し、ステップＳ６７へ処理を進める。
【0120】
次に、自己位置推定部１５は、自己位置推定処理を終了すべきか否か判定する（ステップＳ６７）。そして、コントローラ１３は、自己位置推定処理を終了すべきと判定した場合（ステップＳ６７；Ｙｅｓ）、フローチャートの処理を終了する。一方、コントローラ１３は、自己位置推定処理を継続する場合（ステップＳ６７；Ｎｏ）、ステップＳ６２へ処理を戻し、ステップＳ６４で算出した推定自己位置Ｘ＾を用いて、次の処理時刻での自己位置の推定を行う。
【0121】
【0122】
まず、自己位置推定部１５は、直前に実行したＮＤＴスキャンマッチングにおける対応付け比率ＤＡＲから信頼度情報を生成する（ステップＳ７１）。なお、信頼度情報はイニシャル位置探索範囲Ｒｉのサイズを決定するための情報であり、対応付け比率ＤＡＲそのものであってもよく、対応付け比率ＤＡＲと正の相関を有し対応付け比率ＤＡＲから所定の算出式により求められる指標値であってもよい。自己位置推定部１５は、信頼度情報の値が小さいほど自己位置のずれは大きいと判断し、イニシャル位置探索範囲Ｒｉのサイズを大きくする。また、自己位置推定部１５は、信頼度情報の値が比較的大きければ推定自己位置のずれは小さいと判断し、イニシャル位置探索範囲Ｒｉのサイズを小さくする。そして、自己位置推定部１５は、信頼度情報からイニシャル位置探索範囲Ｒｉを設定する（ステップＳ７２）。この場合、自己位置推定部１５は、中心位置を直前に求めたＮＤＴ位置ＸＮＤＴ（推定自己位置Ｘ＾）とし、サイズを信頼度情報の値が小さいほど大きくしたイニシャル位置探索範囲Ｒｉを設定する。
【0123】
その後、自己位置推定部１５は、設定したイニシャル位置探索範囲Ｒｉに基づき、ステップＳ７３～ステップＳ８３において、図１５のステップＳ２３～ステップＳ３３と同一処理を実行する。これにより、自己位置推定部１５は、イニシャル位置探索範囲Ｒｉ内の候補ボクセルを抽出し、対応付け比率ＤＡＲが所定値より大きくなった候補ボクセルの簡易ＮＤＴ処理の結果を、イニシャル位置として定める。
【0124】
【0125】
まず、自己位置推定部１５は、対象移動体である船舶が存在する地点における予想潮位を、気象庁の公開情報等から取得する（ステップＳ９１）。次に、自己位置推定部１５は、直前に実行したＮＤＴスキャンマッチングにおける対応付け比率ＤＡＲから信頼度情報を生成する（ステップＳ９２）。そして、自己位置推定部１５は、信頼度情報からイニシャル位置探索範囲Ｒｉを設定する（ステップＳ９３）。この場合、自己位置推定部１５は、中心位置を直前に求めたＮＤＴ位置ＸＮＤＴ（推定自己位置Ｘ＾）とし、サイズを信頼度情報の値が小さいほど大きくしたイニシャル位置探索範囲Ｒｉを設定する。
【0126】
その後、自己位置推定部１５は、設定したイニシャル位置探索範囲Ｒｉに基づき、ステップＳ９４～ステップＳ１０２において、図１６のステップＳ４４～ステップＳ５２と同一処理を実行する。これにより、自己位置推定部１５は、イニシャル位置探索範囲Ｒｉ内の仮想ボクセルを抽出し、対応付け比率ＤＡＲが所定値より大きくなった仮想ボクセルの簡易ＮＤＴ処理の結果を、イニシャル位置として定める。
【0127】
このように、本変形例によれば、直前のＮＤＴスキャンマッチングの結果に基づいて復帰用イニシャル位置設定処理を好適に実行することができる。
【0128】
（変形例２）
自己位置推定部１５は、全候補ボクセルの各々の位置を探索開始位置とした簡易ＮＤＴ処理を実行し、対応付け比率ＤＡＲが最大となるボクセルに対する簡易ＮＤＴ処理結果を、イニシャル位置として定めてもよい。例えば、図１５に示す車両のイニシャル位置設定処理では、自己位置推定部１５は、ステップＳ２５で候補ボクセルを抽出後、抽出した全ボクセルの各々を対象としてステップＳ２８の簡易ＮＤＴ処理を実行する。そして、自己位置推定部１５は、対応付け比率ＤＡＲが最も高い簡易ＮＤＴ処理の結果を、イニシャル位置として定める。
自己位置推定部１５は、全候補ボクセルの各々の位置を探索開始位置とした簡易ＮＤＴ処理を実行し、対応付け比率ＤＡＲが最大となるボクセルに対する簡易ＮＤＴ処理結果を、イニシャル位置として定めてもよい。例えば、図１５に示す車両のイニシャル位置設定処理では、自己位置推定部１５は、ステップＳ２５で候補ボクセルを抽出後、抽出した全ボクセルの各々を対象としてステップＳ２８の簡易ＮＤＴ処理を実行する。そして、自己位置推定部１５は、対応付け比率ＤＡＲが最も高い簡易ＮＤＴ処理の結果を、イニシャル位置として定める。
【0129】
本変形例によれば、処理時間は増加するものの、決定されるイニシャル位置の信頼性を好適に向上させることができる。
【0130】
（変形例３）
簡易ＮＤＴ処理の結果をイニシャル位置とするか否かの判定（図１５のステップＳ２９、図１６のステップＳ４８等参照）に用いる評価指標として対応付け比率ＤＡＲを用いる代わりに、他の指標を用いてもよい。
簡易ＮＤＴ処理の結果をイニシャル位置とするか否かの判定（図１５のステップＳ２９、図１６のステップＳ４８等参照）に用いる評価指標として対応付け比率ＤＡＲを用いる代わりに、他の指標を用いてもよい。
【0131】
例えば、自己位置推定部１５は、０～１の値域となるＮＤＴスキャンマッチングの信頼度指標「ＮＲＩ：ＮＤＴ Ｒｅｌｉａｂｉｌｉｔｙ Ｉｎｄｅｘ）」を上述の評価指標として用いてもよい。この場合、自己位置推定部１５は、信頼度指標ＮＲＩが所定値より大きい簡易ＮＤＴ処理の結果をイニシャル位置として定める。
【0132】
ここで、信頼度指標ＮＲＩの算出について説明する。まず、自己位置推定部１５は、各時刻でのダウンサンプリングサイズを「ＤＳＳ」とすると、スコア値Ｅ及び対応付け比率ＤＡＲを用いて、ＮＤＴスキャンマッチングの信頼度値「ＮＲＶ（ＮＤＴ Ｒｅｌｉａｂｉｌｉｔｙ Ｖａｌｕｅ）」を以下の式により算出する。
ＮＲＶ＝ＤＳＳ×Ｅ×ＤＡＲ （８）
なお、この場合、前提として、ダウンサンプリングサイズは、ダウンサンプリング後の点群データにおいてボクセルデータＶＤと対応付けられた計測点数（「対応計測点数」とも呼ぶ。）が所定の目標範囲となるように、各処理時刻において適応的に変更されるものとする。例えば、自己位置推定部１５は、各処理時刻において、対応計測点数が目標範囲の上限より多い時には、次の処理時刻でのダウンサンプリングサイズを所定率（例えば１．１）又は所定値だけ大きくし、対応計測点数が目標範囲の下限より少ない時には、次の処理時刻でのダウンサンプリングサイズを所定率又は所定値だけ小さくする。一方、自己位置推定部１５は、各処理時刻において、対応計測点数が目標範囲内である場合には、ダウンサンプリングサイズを維持する。このようなダウンサンプリングサイズの適応的設定では、対応計測点数が多いほどダウンサンプリングサイズが大きくなるため、ダウンサンプリングサイズが大きいほどＮＤＴ処理の信頼度が高くなることが推察される。
ＮＲＶ＝ＤＳＳ×Ｅ×ＤＡＲ （８）
なお、この場合、前提として、ダウンサンプリングサイズは、ダウンサンプリング後の点群データにおいてボクセルデータＶＤと対応付けられた計測点数（「対応計測点数」とも呼ぶ。）が所定の目標範囲となるように、各処理時刻において適応的に変更されるものとする。例えば、自己位置推定部１５は、各処理時刻において、対応計測点数が目標範囲の上限より多い時には、次の処理時刻でのダウンサンプリングサイズを所定率（例えば１．１）又は所定値だけ大きくし、対応計測点数が目標範囲の下限より少ない時には、次の処理時刻でのダウンサンプリングサイズを所定率又は所定値だけ小さくする。一方、自己位置推定部１５は、各処理時刻において、対応計測点数が目標範囲内である場合には、ダウンサンプリングサイズを維持する。このようなダウンサンプリングサイズの適応的設定では、対応計測点数が多いほどダウンサンプリングサイズが大きくなるため、ダウンサンプリングサイズが大きいほどＮＤＴ処理の信頼度が高くなることが推察される。
【0133】
次に、自己位置推定部１５は、信頼度値ＮＲＶを０～１の値域となるように正規化した信頼度指標ＮＲＩを算出する。例えば、自己位置推定部１５は、以下の式（８）～式（１１）のいずれかを用いて、信頼度値ＮＲＶを信頼度指標ＮＲＩに変換する。
【0134】
【数8】
【0135】
信頼度指標ＮＲＩを上述の評価指標として用いた場合、スコア値Ｅも加味されるため、簡易ＮＤＴ処理での繰り返し回数は多めにする必要があり、処理時間が増加するが、評価指標としての厳密性が向上する。なお、自己位置推定部１５は、対応付け比率ＤＡＲ及び信頼度指標ＮＲＩに代えて、スコア値Ｅを上述の評価指標として用いてもよい。この場合、自己位置推定部１５は、スコア値Ｅが所定値より大きい簡易ＮＤＴ処理の結果を、イニシャル位置として決定する。
【0136】
同様に、自己位置推定部１５は、イニシャル位置設定処理および復帰用イニシャル位置設定処理を行うか否かの判定（図１４のステップＳ１５、図１７のステップＳ６５等参照）に用いる評価指標として対応付け比率ＤＡＲを用いる代わりに、信頼度指標ＮＲＩ又はスコア値Ｅを用いてもよい。また、自己位置推定部１５は、（変形例１）の図１８のステップＳ７１又は図１９のステップＳ９２においても、対応付け比率ＤＡＲに代えて、信頼度指標ＮＲＩ又はスコア値Ｅから信頼度情報を生成してもよい。
【0137】
（変形例４）
ボクセルデータＶＤは、図４に示すように、平均ベクトルと共分散行列とを含むデータ構造に限定されない。例えば、ボクセルデータＶＤは、平均ベクトルと共分散行列を算出する際に用いられる計測整備車両が計測した点群データをそのまま含んでいてもよい。
ボクセルデータＶＤは、図４に示すように、平均ベクトルと共分散行列とを含むデータ構造に限定されない。例えば、ボクセルデータＶＤは、平均ベクトルと共分散行列を算出する際に用いられる計測整備車両が計測した点群データをそのまま含んでいてもよい。
【0138】
以上説明したように、本実施例に係る情報処理装置１のコントローラ１３は、対象移動体を第１位置算出方法であるＧＮＳＳによる測位位置を取得する。そして、コントローラ１３は、ＧＮＳＳによる測位位置に基づきイニシャル位置探索範囲Ｒｉを設定し、当該イニシャル位置探索範囲Ｒｉ内において第２位置算出方法であるＮＤＴスキャンマッチングの基準位置を設定する。そして、コントローラ１３は、設定した基準位置に基づき、ＮＤＴスキャンマッチングによる対象移動体の位置推定を行う。この場合、コントローラ１３は、ＧＮＳＳの測位位置の測位の精度を表す精度情報に基づいて、イニシャル位置探索範囲Ｒｉのサイズを決定する。これにより、情報処理装置１は、ＮＤＴスキャンマッチングにおいて基準となる基準位置を的確に探索して設定することができる。
【0139】
なお、上述した実施例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータであるコントローラ等に供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記憶媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記憶媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記憶媒体（例えば光磁気ディスク）、ＣＤ－ＲＯＭ（Read Only Memory）、ＣＤ－Ｒ、ＣＤ－Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory））を含む。
【0140】
以上、実施例を参照して本願発明を説明したが、本願発明は上記実施例に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。すなわち、本願発明は、請求の範囲を含む全開示、技術的思想にしたがって当業者であればなし得るであろう各種変形、修正を含むことは勿論である。また、引用した上記の特許文献等の各開示は、本書に引用をもって繰り込むものとする。
【符号の説明】
【0141】
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】