▶ パイオニア株式会社の特許一覧 ▶ パイオニアスマートセンシングイノベーションズ株式会社の特許一覧
特許7549129地図データ構造、記憶装置、情報処理装置、制御方法、プログラム及び記憶媒体
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-09-02
(45)【発行日】2024-09-10
(54)【発明の名称】地図データ構造、記憶装置、情報処理装置、制御方法、プログラム及び記憶媒体
(51)【国際特許分類】
G01C 21/20 20060101AFI20240903BHJP
G09B 29/00 20060101ALI20240903BHJP
【ＦＩ】
G01C21/20
G09B29/00 Z
【請求項の数】
9
(21)【出願番号】P 2023509930
(86)(22)【出願日】2021-03-29
(86)【国際出願番号】 JP2021013317
(87)【国際公開番号】W WO2022208617
(87)【国際公開日】2022-10-06
【審査請求日】2023-09-14
(73)【特許権者】
【識別番号】000005016
【氏名又は名称】パイオニア株式会社
(73)【特許権者】
【識別番号】520001073
【氏名又は名称】パイオニアスマートセンシングイノベーションズ株式会社
(74)【代理人】
【識別番号】100107331
【弁理士】
【氏名又は名称】中村 聡延
(72)【発明者】
【氏名】幸田 健志
(72)【発明者】
【氏名】加藤 正浩
(72)【発明者】
【氏名】加藤 将大
(72)【発明者】
【氏名】後藤田 明
(72)【発明者】
【氏名】白鳥 邦雄
【審査官】西畑 智道
(56)【参考文献】
【文献】特開２０１７－１６４０３２（ＪＰ，Ａ）
【文献】特開２０２０－０５９４０３（ＪＰ，Ａ）
【文献】特開２００８－２２４３５５（ＪＰ，Ａ）
【文献】韓国登録特許第１０－１６３３８１２（ＫＲ，Ｂ１）
【文献】特開２００１－０６６９８４（ＪＰ，Ａ）
【文献】特開２００６－１７０９２０（ＪＰ，Ａ）
【文献】国際公開第２０１６／１８９７３２（ＷＯ，Ａ１）
【文献】特開２０１２－２２６６７５（ＪＰ，Ａ）
【文献】特開２０１８－９２５４６（ＪＰ，Ａ）
【文献】米国特許出願公開第２０１３／０２５８８０６（ＵＳ，Ａ１）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０１Ｃ ２１／００－２１／３６
Ｇ０１Ｃ ２３／００－２５／００
Ｇ０８Ｇ １／００－９９／００
Ｇ０９Ｂ ２９／００
(57)【特許請求の範囲】
【請求項1】
構造物の位置を示す位置情報と、前記構造物が水面に対する浮遊物であるか否かを示す属性情報と、をデータ構造として有する地図データを参照する地図データ参照手段と、前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する位置特定手段と、
を有する情報処理装置。
【請求項2】
計測装置が計測した前記構造物に関する計測データを取得する計測データ取得手段と、前記計測データと、前記地図データとの照合結果に基づき、自己位置を推定する自己位置推定手段と、
をさらに有する、請求項１に記載の情報処理装置。
【請求項3】
前記位置情報は、前記構造物が前記浮遊物であることを前記属性情報が示す場合に、高さ方向の位置が前記水面の高さ位置を基準とした相対位置を示しており、前記位置特定手段は、前記属性情報が前記浮遊物であることを示す場合、前記位置情報が示す前記相対位置に対して、推定された絶対的な水面の高さ位置を加算することで前記構造物の高さ方向の絶対的な位置を特定する、請求項１または２に記載の情報処理装置。
【請求項4】
前記地図データは、単位領域であるボクセルごとの前記構造物の位置を表すボクセルデータであり、前記位置情報及び前記属性情報は、ボクセルごとに設けられる、請求項１～３のいずれか一項に記載の情報処理装置。
【請求項5】
前記位置情報は、前記ボクセルごとの代表位置を示すボクセル座標又は前記ボクセルごとの計測された前記構造物の位置の平均ベクトルの少なくとも一方である、請求項４に記載の情報処理装置。
【請求項6】
前記地図データは、単位領域であるボクセルごとの前記構造物の位置を表すボクセルデータであり、
前記位置情報及び前記属性情報は、ボクセルごとに設けられ、
前記自己位置推定手段は、前記属性情報が前記浮遊物であることを示す場合、前記位置情報を前記相対的な位置情報とみなして補正した前記ボクセルごとの前記構造物の絶対的な位置と、計測装置から取得した計測データに基づく前記構造物の絶対的な位置との照合結果に基づき、自己位置を推定する、請求項２に記載の情報処理装置。
【請求項7】
情報処理装置が実行する制御方法であって、構造物の位置を示す位置情報と、前記構造物が水面に対する浮遊物であるか否かを示す属性情報と、をデータ構造として有する地図データを参照し、
前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する、
制御方法。
【請求項8】
構造物の位置を示す位置情報と、前記構造物が水面に対する浮遊物であるか否かを示す属性情報と、をデータ構造として有する地図データを参照し、
前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する処理をコンピュータに実行させるプログラム。
【請求項9】
請求項８に記載のプログラムを記憶した記憶媒体。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、地図データのデータ構造及び地図データに基づく処理に関する。
【背景技術】
【0002】
従来から、レーザスキャナなどの計測装置を用いて計測した周辺物体の形状データを、予め周辺物体の形状が記憶された地図情報と照合（マッチング）することで、移動体の自己位置を推定する技術が知られている。例えば、特許文献１には、空間を所定の規則で分割したボクセル中における検出物が静止物か移動物かを判定し、静止物が存在するボクセルを対象として地図情報と計測データとのマッチングを行う自律移動システムが開示されている。また、特許文献２には、ボクセル毎の静止物体の平均ベクトルと共分散行列とを含むボクセルデータとライダが出力する点群データとの照合により自己位置推定を行うスキャンマッチング手法が開示されている。さらに、特許文献３には、船舶の自動着岸を行う自動着岸装置において、ライダから照射される光が着岸位置の周囲の物体に反射してライダにより受光できるように、船舶の姿勢を変化させる制御を行う手法が記載されている。
【先行技術文献】
【特許文献】
【0003】
【文献】国際公開ＷＯ２０１３／０７６８２９
【文献】国際公開ＷＯ２０１８／２２１４５３
【文献】特開２０２０－５９４０３号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
水面に浮かぶ浮桟橋などの浮遊物の位置を地図データに含め、自動着桟時に地図データを参照して桟橋に対する自船の位置・姿勢を推定することが考えられる。この場合、地図データ生成のための浮遊物の計測時の潮位と、地図データを参照するときの潮位とが異なると、地図データを参照して特定した浮遊物の位置に誤差が生じるという課題がある。
【0005】
本開示は、上記のような課題を解決するためになされたものであり、浮遊物の位置を好適に特定可能な地図データ構造、及び、当該地図データ構造を有する地図データに関する記憶装置、情報処理装置、制御方法、プログラム及び記憶媒体を提供することを主な目的とする。
【課題を解決するための手段】
【0006】
請求項に記載の発明は、
構造物の位置を示す位置情報と、前記構造物が水面に対する浮遊物であるか否かを示す属性情報と、をデータ構造として有する地図データを参照する地図データ参照手段と、
前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する位置特定手段と、
を有する情報処理装置である。
また、請求項に記載の発明は、
情報処理装置が実行する制御方法であって、
構造物の位置を示す位置情報と、前記構造物が水面に対する浮遊物であるか否かを示す属性情報と、をデータ構造として有する地図データを参照し、
前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する、
制御方法である。
また、請求項に記載の発明は、
構造物の位置を示す位置情報と、前記構造物が水面に対する浮遊物であるか否かを示す属性情報と、をデータ構造として有する地図データを参照し、
前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する処理をコンピュータに実行させるプログラムである。
構造物の位置を示す位置情報と、前記構造物が水面に対する浮遊物であるか否かを示す属性情報と、をデータ構造として有する地図データを参照する地図データ参照手段と、
前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する位置特定手段と、
を有する情報処理装置である。
また、請求項に記載の発明は、
情報処理装置が実行する制御方法であって、
構造物の位置を示す位置情報と、前記構造物が水面に対する浮遊物であるか否かを示す属性情報と、をデータ構造として有する地図データを参照し、
前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する、
制御方法である。
また、請求項に記載の発明は、
構造物の位置を示す位置情報と、前記構造物が水面に対する浮遊物であるか否かを示す属性情報と、をデータ構造として有する地図データを参照し、
前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する処理をコンピュータに実行させるプログラムである。
【図面の簡単な説明】
【0007】
【図1】運転支援システムの概略構成図である。
【図2】情報処理装置のハードウェア構成の一例を示すブロック図である。
【図3】自己位置推定部が推定すべき自己位置を３次元直交座標で表した図である。
【図4】ボクセルデータの概略的なデータ構造の一例を示す。
【図5】固定岸壁に設けられた浮桟橋の状態を表す図である。
【図6】ボクセルデータの詳細なデータ構造の一例を示す。
【図7】自己位置推定部の機能ブロックの一例である。
【図8】自己位置推定処理の手順を示すフローチャートの一例である。
【発明を実施するための形態】
【0008】
本発明の好適な実施形態によれば、情報処理装置が参照する地図データのデータ構造である地図データ構造であって、構造物の位置を示す位置情報と、前記構造物が水面に対する浮遊物であるか否かを示す属性情報と、を有し、前記属性情報は、当該属性情報が前記浮遊物であることを示す場合、前記情報処理装置が、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する処理に用いられる、地図データ構造である。地図データは、このようなデータ構造を備えることで、当該地図データを参照する情報処理装置が潮位の変化に依らずに適切に水面に浮かぶ浮遊物の位置を特定可能な態様により、浮遊物の位置情報を好適に格納することができる。
【0009】
上記地図データ構造の一態様では、前記位置情報は、前記構造物が前記浮遊物であることを前記属性情報が示す場合に、高さ方向の位置が前記水面の高さ位置を基準とした相対位置を示しており、前記属性情報は、当該属性情報が前記浮遊物であることを示す場合、前記情報処理装置が、前記位置情報が示す前記相対位置に対して、推定された絶対的な水面の高さ位置を加算することで前記構造物の高さ方向の絶対的な位置を特定する処理に用いられる。地図データは、この態様のデータ構造を有することで、地図データを参照する情報処理装置が浮遊物の絶対的な位置を特定することができるように、浮遊物の位置情報を好適に格納することができる。
【0010】
上記地図データ構造の他の一態様では、前記地図データは、単位領域であるボクセルごとの前記構造物の位置を表すボクセルデータであり、前記位置情報及び前記属性情報は、ボクセルごとに設けられる。このように、地図データ構造は、ボクセルデータに好適に適用される。好適な例では、前記位置情報は、前記ボクセルごとの代表位置を示すボクセル座標又は前記ボクセルごとの計測された前記構造物の位置の平均ベクトルの少なくとも一方である。
【0011】
上記地図データ構造の他の一態様では、前記属性情報は、当該属性情報が前記浮遊物であることを示す場合、前記情報処理装置が、前記位置情報を前記相対的な位置情報とみなして補正した前記ボクセルごとの前記構造物の絶対的な位置と、前記情報処理装置が計測装置から取得した計測データに基づく前記構造物の絶対的な位置との照合結果に基づき、前記情報処理装置の自己位置を推定する処理に用いられる。地図データは、この態様のデータ構造を有することで、計測装置から取得した浮遊物を含む構造物の計測データに基づき情報処理装置が自己位置推定を行う場合に、自己位置推定を高精度に実行させることができる。
【0012】
本発明の他の好適な実施形態によれば、記憶装置は、上記記載のデータ構造を有する地図データを記憶する。この場合、記憶装置は、当該記憶装置が記憶する地図データを参照する情報処理装置に対し、潮位の変化に依らずに適切に水面に浮かぶ浮遊物の位置を特定させることができる。
【0013】
本発明のさらに別の実施形態によれば、情報処理装置は、上記記載のデータ構造を有する地図データを参照する地図データ参照手段と、前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する位置特定手段と、を有する。この態様により、情報処理装置は、地図データを参照し、浮遊物の位置を的確に特定することができる。
【0014】
上記情報処理装置の一態様では、情報処理装置は、計測装置が計測した前記構造物に関する計測データを取得する計測データ取得手段と、前記計測データと、前記地図データとの照合結果に基づき、自己位置を推定する自己位置推定手段と、をさらに有する。この態様によれば、情報処理装置は、地図データを参照し、浮遊物等を基準とした自己位置推定を好適に実行することができる。
【0015】
本発明の他の好適な実施形態によれば、情報処理装置が実行する制御方法であって、上記記載のデータ構造を有する地図データを取得し、前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する。情報処理装置は、この制御方法を実行することで、地図データを参照して浮遊物の位置を的確に特定することができる。
【0016】
本発明の他の好適な実施形態によれば、プログラムは、上記記載のデータ構造を有する地図データを取得し、前記地図データにおいて参照した構造物の位置情報に対応する属性情報が浮遊物であることを示す場合、前記位置情報を、水面位置を基準とする相対的な位置情報とみなして前記構造物の位置を特定する処理をコンピュータに実行させる。コンピュータは、このプログラムを実行することで、地図データを参照して浮遊物の位置を的確に特定することができる。好適には、上記プログラムは、記憶媒体に記憶される。
【実施例】
【0017】
以下、図面を参照して本発明の好適な実施例について説明する。なお、任意の文字「Ａ」の上に任意の記号「ｘ」が付された文字を、本明細書では便宜上、「Ａｘ」と表す。
【0018】
（１）運転支援システムの概要
図１は、本実施例に係る運転支援システムの概略構成である。運転支援システムは、移動体である船舶と共に移動する情報処理装置１と、当該船舶に搭載されたセンサ群２とを有する。以後では、情報処理装置１と共に移動する船舶を「対象船舶」とも呼ぶ。
図１は、本実施例に係る運転支援システムの概略構成である。運転支援システムは、移動体である船舶と共に移動する情報処理装置１と、当該船舶に搭載されたセンサ群２とを有する。以後では、情報処理装置１と共に移動する船舶を「対象船舶」とも呼ぶ。
【0019】
情報処理装置１は、センサ群２と電気的に接続し、センサ群２に含まれる各種センサの出力に基づき、情報処理装置１が設けられた対象船舶の位置（「自己位置」とも呼ぶ。）の推定を行う。そして、情報処理装置１は、自己位置の推定結果に基づき、対象船舶の自動運転制御等の運転支援を行う。運転支援には、自動着岸などの着岸支援も含まれる。ここで、「着岸」とは、岸壁に対象船舶を着ける場合の他、桟橋等の構造体に対象船舶を着ける場合も含まれる。情報処理装置１は、対象船舶に設けられたナビゲーション装置であってもよく、船舶に内蔵された電子制御装置であってもよい。
【0020】
また、情報処理装置１は、ボクセルデータ「ＶＤ」を含む地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶する。ボクセルデータＶＤは、３次元空間の最小単位となる立方体（正規格子）を示すボクセルごとに静止構造物の位置情報等を記録したデータである。ボクセルデータＶＤは、各ボクセル内の静止構造物の計測された点群データを正規分布により表したデータを含み、後述するように、ＮＤＴ（Ｎｏｒｍａｌ Ｄｉｓｔｒｉｂｕｔｉｏｎｓ Ｔｒａｎｓｆｏｒｍ）を用いたスキャンマッチングに用いられる。情報処理装置１は、ＮＤＴスキャンマッチングにより、例えば、対象船舶の平面上の位置、高さ位置、ヨー角、ピッチ角及びロール角の推定を行う。なお、特に言及がない限り、自己位置は、対象船舶のヨー角などの姿勢角も含まれるものとする。
【0021】
センサ群２は、対象船舶に設けられた種々の外界センサ及び内界センサを含んでいる。本実施例では、センサ群２は、ライダ（Ｌｉｄａｒ：Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ、または、Ｌａｓｅｒ Ｉｌｌｕｍｉｎａｔｅｄ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）３と、対象船舶の速度を検出する速度センサ４と、ＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）受信機５と、３軸方向における対象船舶の加速度及び角速度を計測する慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）６とを含んでいる。
【0022】
ライダ３は、水平方向および垂直方向の所定の角度範囲に対してパルスレーザを出射することで、外界に存在する物体までの距離を離散的に測定し、当該物体の位置を示す３次元の点群データを生成する。この場合、ライダ３は、照射方向を変えながらレーザ光を照射する照射部と、照射したレーザ光の反射光（散乱光）を受光する受光部と、受光部が出力する受光信号に基づくスキャンデータである点群データを出力する出力部とを有する。計測点は、受光部が受光したレーザ光に対応する照射方向と、上述の受光信号に基づき特定される当該レーザ光の応答遅延時間とに基づき生成される。なお、一般的に、対象物までの距離が近いほどライダの距離測定値の精度は高く、距離が遠いほど精度は低い。ライダ３は、本発明における「計測装置」の一例である。速度センサ４は、例えば、ドップラーを利用した速度計であってもよく、ＧＮＳＳを利用した速度計であってもよい。さらには、速度センサ４は、ライダ３によって計測した距離情報の変化に基づく速度計であってもよい。
【0023】
（２）情報処理装置の構成
図２は、情報処理装置１のハードウェア構成の一例を示すブロック図である。情報処理装置１は、主に、インターフェース１１と、メモリ１２と、コントローラ１３と、を有する。これらの各要素は、バスラインを介して相互に接続されている。
図２は、情報処理装置１のハードウェア構成の一例を示すブロック図である。情報処理装置１は、主に、インターフェース１１と、メモリ１２と、コントローラ１３と、を有する。これらの各要素は、バスラインを介して相互に接続されている。
【0024】
インターフェース１１は、情報処理装置１と外部装置とのデータの授受に関するインターフェース動作を行う。本実施例では、インターフェース１１は、ライダ３、速度センサ４、ＧＮＳＳ受信機５及びＩＭＵ６などのセンサ群２の各センサから出力データを取得し、コントローラ１３へ供給する。また、インターフェース１１は、例えば、コントローラ１３が生成した対象船舶の制御に関する信号を、対象船舶の運転を制御する対象船舶の各構成要素に供給する。例えば、対象船舶は、エンジンや電気モータなどの駆動源と、駆動源の駆動力に基づき進行方向の推進力を生成するスクリューと、駆動源の駆動力に基づき横方向の推進力を生成するスラスターと、船舶の進行方向を自在に定めるための機構である舵等とを備える。そして、自動着岸などの自動運転時には、インターフェース１１は、コントローラ１３が生成した制御信号を、これらの各構成要素に供給する。なお、対象船舶に電子制御装置が設けられている場合には、インターフェース１１は、当該電子制御装置に対し、コントローラ１３が生成した制御信号を供給する。インターフェース１１は、無線通信を行うためのネットワークアダプタなどのワイヤレスインターフェースであってもよく、ケーブル等により外部装置と接続するためのハードウェアインターフェースであってもよい。また、インターフェース１１は、入力装置、表示装置、音出力装置等の種々の周辺装置とのインターフェース動作を行ってもよい。
【0025】
メモリ１２は、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ハードディスクドライブ、フラッシュメモリなどの各種の揮発性メモリ及び不揮発性メモリにより構成される。メモリ１２は、コントローラ１３が所定の処理を実行するためのプログラムが記憶される。なお、コントローラ１３が実行するプログラムは、メモリ１２以外の記憶媒体に記憶されてもよい。
【0026】
また、メモリ１２は、ボクセルデータＶＤを含む地図ＤＢ１０を記憶する。地図ＤＢ１０には、ボクセルデータＶＤの他、例えば、着岸場所（岸、桟橋を含む）に関する情報、船舶が移動可能な水路に関する情報などが含まれている。なお、地図ＤＢ１０は、インターフェース１１を介して情報処理装置１と接続されたハードディスクなどの情報処理装置１の外部の記憶装置に記憶されてもよい。上記の記憶装置は、情報処理装置１と通信を行うサーバ装置であってもよい。また、上記の記憶装置は、複数の装置から構成されてもよい。また、地図ＤＢ１０は、定期的に、あるいは必要なタイミングで更新されてもよい。この場合、例えば、コントローラ１３は、インターフェース１１を介し、地図情報を管理するサーバ装置から、自己位置が属するエリアに関する部分地図情報を受信し、地図ＤＢ１０に反映させる。
【0027】
また、メモリ１２には、地図ＤＢ１０の他、本実施例において情報処理装置１が実行する処理に必要な情報が記憶される。例えば、メモリ１２には、ライダ３が１周期分の走査を行った場合に得られる点群データに対してダウンサンプリングを行う場合のダウンサンプリングのサイズの設定に用いられる情報等が記憶される。
【0028】
コントローラ１３は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＴＰＵ（Ｔｅｎｓｏｒ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などの１又は複数のプロセッサを含み、情報処理装置１の全体を制御する。この場合、コントローラ１３は、メモリ１２等に記憶されたプログラムを実行することで、自己位置推定及び運転支援等に関する処理を行う。
【0029】
また、コントローラ１３は、機能的には、自己位置推定部１５を有する。そして、コントローラ１３は、「地図データ参照手段」、「位置特定手段」、「自己位置推定手段」、及びプログラムを実行するコンピュータ等として機能する。
【0030】
自己位置推定部１５は、ライダ３の出力に基づく点群データと、当該点群データが属するボクセルに対応するボクセルデータＶＤとに基づき、ＮＤＴに基づくスキャンマッチング（ＮＤＴスキャンマッチング）を行うことで、自己位置の推定を行う。ここで、自己位置推定部１５による処理対象となる点群データは、ライダ３が生成した点群データであってもよく、当該点群データをダウンサンプリング処理した後の点群データであってもよい。
【0031】
（３）ＮＤＴスキャンマッチング
次に、自己位置推定部１５が実行するＮＤＴスキャンマッチングに基づく位置推定に関する説明を行う。
次に、自己位置推定部１５が実行するＮＤＴスキャンマッチングに基づく位置推定に関する説明を行う。
【0032】
（３－１）自己位置
図３は、自己位置推定部１５が推定すべき自己位置を３次元直交座標で表した図である。図３に示すように、ｘｙｚの３次元直交座標上で定義された平面での自己位置は、座標「（ｘ、ｙ、ｚ）」、対象船舶のロール角「φ」、ピッチ角「θ」、ヨー角（方位）「ψ」により表される。ここでは、ロール角φは、対象船舶の進行方向を軸とした回転角、ピッチ角θは、ｘｙ平面に対する対象船舶の進行方向の仰角、ヨー角ψは、対象船舶の進行方向とｘ軸とのなす角として定義されている。座標（ｘ、ｙ、ｚ）は、運転支援システムにおいて一意に位置を特定可能な絶対的な座標（「ワールド座標」とも呼ぶ。）であり、例えば緯度、経度、標高の組合せに相当する位置、ＵＴＭ（Universal Transverse Mercator）座標系に基づく位置、その他、所定地点を原点とした位置を示す。そして、自己位置推定部１５は、これらのｘ、ｙ、ｚ、φ、θ、ψを推定パラメータとする自己位置推定を行う。
図３は、自己位置推定部１５が推定すべき自己位置を３次元直交座標で表した図である。図３に示すように、ｘｙｚの３次元直交座標上で定義された平面での自己位置は、座標「（ｘ、ｙ、ｚ）」、対象船舶のロール角「φ」、ピッチ角「θ」、ヨー角（方位）「ψ」により表される。ここでは、ロール角φは、対象船舶の進行方向を軸とした回転角、ピッチ角θは、ｘｙ平面に対する対象船舶の進行方向の仰角、ヨー角ψは、対象船舶の進行方向とｘ軸とのなす角として定義されている。座標（ｘ、ｙ、ｚ）は、運転支援システムにおいて一意に位置を特定可能な絶対的な座標（「ワールド座標」とも呼ぶ。）であり、例えば緯度、経度、標高の組合せに相当する位置、ＵＴＭ（Universal Transverse Mercator）座標系に基づく位置、その他、所定地点を原点とした位置を示す。そして、自己位置推定部１５は、これらのｘ、ｙ、ｚ、φ、θ、ψを推定パラメータとする自己位置推定を行う。
【0033】
（３－２）ボクセルデータのデータ構造
次に、ＮＤＴスキャンマッチングに用いるボクセルデータＶＤのデータ構造について説明する。ボクセルデータＶＤは、各ボクセル内の静止構造物の計測された点群データを正規分布により表したデータを含む。
次に、ＮＤＴスキャンマッチングに用いるボクセルデータＶＤのデータ構造について説明する。ボクセルデータＶＤは、各ボクセル内の静止構造物の計測された点群データを正規分布により表したデータを含む。
【0034】
図４は、ボクセルデータＶＤの概略的なデータ構造の一例を示す。ボクセルデータＶＤは、ボクセル内の点群を正規分布で表現する場合のパラメータの情報と、属性情報とを含み、本実施例では、図４に示すように、ボクセルＩＤと、ボクセル座標と、平均ベクトルと、共分散行列と、属性情報と、を含む。
【0035】
「ボクセル座標」は、各ボクセルの中心位置などの代表位置の絶対的な３次元座標を示す。なお、各ボクセルは、空間を格子状に分割した立方体であり、予め形状及び大きさが定められているため、ボクセル座標により各ボクセルの空間を特定することが可能である。ボクセル座標は、ボクセルＩＤとして用いられてもよい。また、本実施例では、高さ方向の位置を示すｚ座標値については、後述する属性情報が水面に浮かぶ浮遊物であることを示す場合には、水面位置を基準とする相対的な座標値（「水面基準座標値」とも呼ぶ。）となる。「ボクセル座標」は、本発明における「構造物の位置を示す位置情報」の一例である。
【0036】
「平均ベクトル」及び「共分散行列」は、対象のボクセル内での点群を正規分布で表現する場合のパラメータに相当する平均ベクトル及び共分散行列を示す。なお、平均ベクトルは、ボクセル内でのローカル座標系における平均ベクトルであってもよく、ワールド座標系における平均ベクトルであってもよい。なお、「平均ベクトル」がワールド座標系により表されることになっている場合であっても、属性情報が水面に浮かぶ浮遊物であることを示す場合には、「ボクセル座標」と同様、「平均ベクトル」のｚ座標値は水面基準座標値となる。この場合、「平均ベクトル」は、本発明における「構造物の位置を示す位置情報」の一例である。
【0037】
「属性情報」は、対象のボクセルの属性を表す情報であり、本実施例では、少なくとも、対象のボクセル内でのデータが浮桟橋などの水面に浮かぶ浮遊物であるか否かを示す。そして、「属性情報」が浮遊物であることを示す場合、対応する「ボクセル座標」のｚ座標値は、水面基準座標値となり、「属性情報」が浮遊物でないことを示す場合、対応する「ボクセル座標」のｚ座標値は、ワールド座標系の座標値（「絶対座標値」とも呼ぶ。）となる。
【0038】
「属性情報」は、ＮＤＴスキャンマッチングにおいて、対象のボクセルの絶対的な高さ位置を情報処理装置１が特定する処理に好適に用いられる。具体的には、「属性情報」が浮遊物であることを示す場合、ボクセルデータＶＤを参照する情報処理装置１は、「ボクセル座標」のｚ座標値である水面基準座標値と、ワールド座標系における水面位置のｚ座標値とに基づき、対象のボクセルの絶対座標値（＝水面位置のｚ座標値＋「ボクセル座標」のｚ座標値）を認識する。同様に、「平均ベクトル」がワールド座標系により表されることになっている場合においても、情報処理装置１は、「属性情報」が浮遊物であることを示す場合には、ワールド座標系における水面位置のｚ座標値に基づき、対象のボクセルの平均ベクトルのｚ座標値を絶対座標値（＝水面位置のｚ座標値＋「平均ベクトル」のｚ座標値）に変換する。
【0039】
ここで、「属性情報」をボクセルデータＶＤに設けることの効果について補足説明する。
【0040】
自動着桟を行う際、桟橋に対する自船の位置・姿勢を、ボクセルデータＶＤとライダ３の点群データとを照合するＮＤＴマッチングにより推定することで、適切な着桟制御が可能になる。しかしながら、水面に浮かぶ浮桟橋などの浮遊物をボクセルデータＶＤとしてデータ化する場合、データ計測時の潮位によって、生成される浮遊物の高さ情報が固定物に対して相対的に変化してしまうという課題がある。
【0041】
図５（Ａ）は、ボクセルデータＶＤを生成するためのデータ計測時において固定岸壁８に設けられた浮桟橋９の状態を表す図であり、図５（Ｂ）は、ボクセルデータＶＤを参照したＮＤＴスキャンマッチング時において固定岸壁８に設けられた浮桟橋９の状態を表す図である。図５（Ａ）及び図５（Ｂ）に示すように、ボクセルデータＶＤを参照したＮＤＴスキャンマッチング時での潮位は、ボクセルデータＶＤを生成するためのデータ計測時での潮位より低くなっている。その結果、水面に浮いている浮桟橋９の高さ方向の絶対位置は、ボクセルデータＶＤを生成するためのデータ計測時での高さ方向の絶対位置と異なっている。従って、図５（Ｂ）の状態でＮＤＴスキャンマッチングを行うと、周辺の固定物である固定岸壁８と浮桟橋９とのボクセルデータＶＤにおける高さ情報が整合せず、自己位置推定の精度が低下してしまう。
【0042】
以上を勘案し、本実施例に係るボクセルデータＶＤは、対象のボクセル内でのデータが浮桟橋などの水面に浮かぶ浮遊物であるか否かを示す属性情報を有する。これにより、情報処理装置１は、ボクセルデータＶＤを参照してＮＤＴスキャンマッチングを行う場合に、浮遊物に対応するボクセルのｚ座標値を、水面位置を基準として適切に設定し、自己位置推定を高精度に実行することができる。
【0043】
【0044】
項目「Ｈｅａｄｅｒ」は、第１サブ項目として、ボクセルデータＶＤのバージョンを示す「Ｆｏｒｍａｔ Ｖｅｒｓｉｏｎ」、地図のタイプを示す「Ｍａｐ Ｔｙｐｅ」（ここではＮＤＴ地図を示す「ｎｄｔ」に設定）、対象のボクセルデータＶＤとしてデータ化された領域を示す「Ｍａｐ Ｒｅｇｉｏｎ」、対象のボクセルデータＶＤの属性を示す「Ａｔｔｒｉｂｕｔｅ」を含む。また、第１サブ項目「Ａｔｔｒｉｂｕｔｅ」は、データが存在するボクセル数を示す第２サブ項目「Ｎｕｍ ｏｆ Ｖｏｘｅｌｓ」（ここでは「Ｍ」個）を少なくとも含んでいる。
【0045】
項目「Ｂｏｄｙ」は、第１サブ項目として、ボクセル（ここでは第１ボクセル～第Ｍボクセル）ごとのデータを格納する「Ｖｏｘｅｌ ＃１」～「Ｖｏｘｅｌ ＃Ｍ」を有する。また、第１サブ項目「Ｖｏｘｅｌ ＃１」～「Ｖｏｘｅｌ ＃Ｍ」の各々には、第２サブ項目として、対象のボクセルの大きさを示す「Ｖｏｘｅｌ Ｓｉｚｅ」、対象のボクセルの属性情報を格納する「Ｖｏｘｅｌ Ａｔｔｒｉｂｕｔｅ」、対象のボクセルの絶対的な中心位置の座標を格納する「Ｃｅｎｔｅｒ Ｐｏｓｉｔｉｏｎ」、対象のボクセルに対するパラメータを格納する「ＮＤＴ Ｐａｒａｍｅｔｅｒｓ」が少なくとも含まれている。また、第２サブ項目「ＮＤＴ Ｐａｒａｍｅｔｅｒｓ」には、第３サブ項目として、反射強度の値域を示す「Ｉｎｔｅｎｓｉｔｙ Ｒａｎｇｅ」、平均ベクトルを格納する「Ｍｅａｎ Ｖｅｃｔｏｒ」、共分散行列を格納する「Ｃｏｖａｒｉａｎｃｅ Ｍａｔｒｉｘ」、固有ベクトルを格納する「Ｅｉｇｅｎ Ｖｅｃｔｏｒ」が少なくとも含まれている。
【0046】
ここで、は、図５における「属性情報」に相当する情報を格納する。この場合、例えば、「Ｖｏｘｅｌ Ａｔｔｒｉｂｕｔｅ」は、対象のボクセルが浮遊物に対応する場合に「１」となり、その他の場合に「０」となる２値情報を格納するものであってもよい。他の例では、「Ｖｏｘｅｌ Ａｔｔｒｉｂｕｔｅ」は、対象のボクセルが浮遊物に対応する場合に対象のボクセルが浮遊物であることを表す識別情報（例えば「Ｆｌｏａｔ」等）を格納するものであってもよい。
【0047】
なお、図５における「属性情報」に相当する情報を、各ボクセルの属性データ（図６の例では「Ｖｏｘｅｌ Ａｔｔｒｉｂｕｔｅ」）として付与する方法以外の例として、浮遊物が存在する凸包領域の情報を示すフィールドをＨｅａｄｅｒ部（図６では「Ｈｅａｄｅｒ」）に設けるようにしてもよい。このとき、凸包領域は、例えば座標の点列で指定される。その場合、情報処理装置１は、浮遊物が存在する領域として指定された凸包領域内に存在する地図データ（ボクセルデータに限らない）のz座標を水面基準座標値として扱う。このようなデータ構造を地図データに採用した場合であっても、図６に示されるデータ構造を採用した場合と同様の効果が得られる。
【0048】
また、「Ｃｅｎｔｅｒ Ｐｏｓｉｔｉｏｎ」は、図５における「ボクセル座標」に相当する情報を格納する。この場合、「Ｖｏｘｅｌ Ａｔｔｒｉｂｕｔｅ」が浮遊物であることを示す場合、対応する「Ｃｅｎｔｅｒ Ｐｏｓｉｔｉｏｎ」が示すｚ座標値は、水面基準座標値となり「Ｖｏｘｅｌ Ａｔｔｒｉｂｕｔｅ」が浮遊物でないことを示す場合、対応する「Ｃｅｎｔｅｒ Ｐｏｓｉｔｉｏｎ」が示すｚ座標値は、絶対座標値となる。また、「Ｍｅａｎ Ｖｅｃｔｏｒ」、「Ｃｏｖａｒｉａｎｃｅ Ｍａｔｒｉｘ」は、夫々、図５における「平均ベクトル」、「共分散行列」に夫々相当する情報を格納する。なお、「Ｍｅａｎ Ｖｅｃｔｏｒ」がワールド座標系での平均ベクトルを格納することになっている場合、「Ｃｅｎｔｅｒ Ｐｏｓｉｔｉｏｎ」と同様、「Ｍｅａｎ Ｖｅｃｔｏｒ」のｚ座標値は水面基準座標値又は絶対座標値となる。具体的には、「Ｍｅａｎ Ｖｅｃｔｏｒ」が示すｚ座標値は、「Ｖｏｘｅｌ Ａｔｔｒｉｂｕｔｅ」が浮遊物を示す場合に水面基準座標値となり、「Ｖｏｘｅｌ Ａｔｔｒｉｂｕｔｅ」が浮遊物でないことを示す場合に絶対座標値となる。
【0049】
（３－３）ＮＤＴスキャンマッチングの概要
次に、ボクセルデータＶＤを用いたＮＤＴスキャンマッチングの概要について説明する。
次に、ボクセルデータＶＤを用いたＮＤＴスキャンマッチングの概要について説明する。
【0050】
船舶を想定したＮＤＴによるスキャンマッチングは、水平面（ここではｘｙ座標とする）内の移動量及び船舶の向きを要素とした推定パラメータ
Ｐ＝［ｔｘ、ｔｙ、ｔｚ、ｔφ、ｔθ、ｔψ］Ｔ
を推定することとなる。ここで、「ｔｘ」はｘ方向の移動量、「ｔｙ」はｙ方向の移動量、「ｔｚ」はｚ方向の移動量、「ｔφ」はロール角、「ｔθ」はピッチ角、「ｔψ」はヨー角を示す。
Ｐ＝［ｔｘ、ｔｙ、ｔｚ、ｔφ、ｔθ、ｔψ］Ｔ
を推定することとなる。ここで、「ｔｘ」はｘ方向の移動量、「ｔｙ」はｙ方向の移動量、「ｔｚ」はｚ方向の移動量、「ｔφ」はロール角、「ｔθ」はピッチ角、「ｔψ」はヨー角を示す。
【0051】
また、ライダ３が出力する点群データの座標を、
ＸＬ（ｊ）＝［ｘｎ（ｊ）、ｙｎ（ｊ）、ｚｎ（ｊ）］Ｔ
とする（「ｊ」は、点群データを構成する各計測点のインデックス）と、ＸＬ（ｊ）のボクセル「ｎ」（ｎ＝１～Ｍ）ごとの平均値「Ｌ´ｎ」は、以下の式（１）により表される。
ＸＬ（ｊ）＝［ｘｎ（ｊ）、ｙｎ（ｊ）、ｚｎ（ｊ）］Ｔ
とする（「ｊ」は、点群データを構成する各計測点のインデックス）と、ＸＬ（ｊ）のボクセル「ｎ」（ｎ＝１～Ｍ）ごとの平均値「Ｌ´ｎ」は、以下の式（１）により表される。
【0052】
【数1】
【0053】
そして、自己位置推定部１５は、地図ＤＢ１０と同一の座標系である絶対的な座標系（「ワールド座標系」とも呼ぶ。）に変換した点群データに対応するボクセルデータＶＤを抽出する。ここで、抽出したボクセルデータＶＤに基づくボクセルｎの平均ベクトルを「μｎ」とし、抽出したボクセルデータＶＤに基づくボクセルｎの共分散行列を「Ｖｎ」とする。なお、ボクセルｎが浮遊物であることを示す属性情報と紐づいている場合には、自己位置推定部１５は、ボクセルデータＶＤに基づくボクセルｎのｚ座標値が水面基準座標値により表されているとみなし、ボクセルデータＶＤに基づくボクセルｎの平均ベクトルμｎのｚ座標値に、ワールド座標系における水面位置のｚ座標値を加算する。
【0054】
そして、自己位置推定部１５は、平均ベクトルμｎと共分散行列Ｖｎとを用い、ボクセルｎの評価関数値（「個別評価関数値」とも呼ぶ。）「Ｅｎ」を算出する。この場合、自己位置推定部１５は、以下の式（２）に基づき、ボクセルｎの個別評価関数値Ｅｎを算出する。
【0055】
【数2】
【0056】
そして、自己位置推定部１５は、以下の式（３）により示される、マッチングの対象となる全てのボクセルを対象とした総合的な評価関数値（「スコア値」とも呼ぶ。）「Ｅ（ｋ）」を算出する。スコア値Ｅは、マッチングの適合度を示す指標となる。
【0057】
【数3】
【0058】
【数4】
【0059】
（３－４）機能ブロック
図７は、自己位置推定部１５の機能ブロックの一例である。図７に示すように、自己位置推定部１５は、デッドレコニングブロック２１と、座標変換ブロック２２と、水面位置推定ブロック２３と、水面反射データ除去ブロック２４と、高さ方向処理ブロック２５と、ＮＤＴ位置算出ブロック２６とを有する。
図７は、自己位置推定部１５の機能ブロックの一例である。図７に示すように、自己位置推定部１５は、デッドレコニングブロック２１と、座標変換ブロック２２と、水面位置推定ブロック２３と、水面反射データ除去ブロック２４と、高さ方向処理ブロック２５と、ＮＤＴ位置算出ブロック２６とを有する。
【0060】
デッドレコニングブロック２１は、センサ群２が出力する信号に基づき、ＤＲ位置の算出を行う。具体的には、デッドレコニングブロック２１は、速度センサ４及びＩＭＵ６等の出力に基づく対象船舶の移動速度と角速度を用い、前回時刻からの移動距離と方位変化を求める。そして、デッドレコニングブロック２１は、現在の処理時刻ｋに対する直前の処理時刻である時刻ｋ－１の推定自己位置Ｘ＾（ｋ－１）に対し、前回時刻からの移動距離と方位変化を加えた時刻ｋでのＤＲ位置ＸＤＲ（ｋ）を算出する。このＤＲ位置ＸＤＲ（ｋ）は、デッドレコニングに基づき時刻ｋにおいて求められた自己位置であり、予測自己位置Ｘ－（ｋ）に相当する。なお、自己位置推定開始直後であって、時刻ｋ－１の推定自己位置Ｘ＾（ｋ－１）が存在しない場合等には、デッドレコニングブロック２１は、例えば、ＧＮＳＳ受信機５が出力する信号に基づき、ＤＲ位置ＸＤＲ（ｋ）を定める。
【0061】
座標変換ブロック２２は、ライダ３の出力に基づく点群データを、地図ＤＢ１０と同一の座標系であるワールド座標系に変換する。この場合、座標変換ブロック２２は、例えば、時刻ｋでデッドレコニングブロック２１が出力する予測自己位置に基づき、時刻ｋでの点群データの座標変換を行う。なお、移動体（本実施例では船舶）に設置されたライダを基準とした座標系の点群データを移動体の座標系に変換する処理、及び移動体の座標系からワールド座標系に変換する処理等については、例えば、国際公開ＷＯ２０１９／１８８７４５などに開示されている。
【0062】
水面位置推定ブロック２３は、水面高さ位置の推定を行う。例えば、水面位置推定ブロック２３は、水面位置を計測したデータ（「水面位置データ」とも呼ぶ。）を、座標変換ブロック２２が出力する点群データから抽出し、抽出した水面位置データに基づき、水面高さ位置を推定する。この場合、まず、水面位置推定ブロック２３は、対象船舶が岸から所定距離以上離れている場合に取得された点群データのうち、対象船舶の位置から所定距離（上述の所定距離より短い距離）以内の位置を示すデータを、水面位置データとして抽出する。次に、水面位置推定ブロック２３は、抽出した水面位置データの計測点ごとの各データが表すｚ方向の位置（ｚ座標値）の平均値等を、ワールド座標系における水面位置のｚ座標値として推定する。
【0063】
なお、水面位置推定ブロック２３は、水面位置を点群データに基づき推定する代わりに、水面位置の絶対的な高さに関する情報を提供するサーバ装置から当該水面位置の高さに関する情報を取得し、取得した情報に基づき、水面位置の絶対的なｚ座標値を特定してもよい。例えば、気象庁等が提供している潮位情報を取得することで，水面高さ位置を把握することが可能となる。
【0064】
水面反射データ除去ブロック２４は、座標変換ブロック２２から供給される点群データのうち、水面位置推定ブロック２３が推定した水面高さ位置よりも下方（同一高さを含む、以下同じ。）の位置（即ちｚ座標値が同一又は低い位置）を表すデータを水面反射データと判定し、判定した水面反射データを点群データから除去する。なお、好適には、水面反射データ除去ブロック２４は、水面位置推定ブロック２３が抽出した水面位置データについても水面反射データとみなし、ＮＤＴ位置算出ブロック２６に供給すべき点群データから除外するとよい。
【0065】
高さ方向処理ブロック２５は、ＮＤＴ位置算出ブロック２６がＮＤＴスキャンマッチングにおいて使用するボクセルデータＶＤの各ボクセルの高さ方向の位置に関する処理を行う。具体的には、水面反射データ除去ブロック２４は、図４における「属性情報」（又は図６における「Ｖｏｘｅｌ Ａｔｔｒｉｂｕｔｅ」）が浮遊物を示すボクセルである場合には、当該ボクセルのｚ座標値が水面基準座標値により表されているとみなし、当該ｚ座標値に、水面位置推定ブロック２３が推定したワールド座標系における水面位置のｚ座標値を加算する。例えば、高さ方向処理ブロック２５は、浮遊物を示すボクセルの場合に、平均ベクトルμｎのｚ座標値を水面高さ位置に基づき補正し、補正後の平均ベクトルμｎをＮＤＴ位置算出ブロック２６に供給する。
【0066】
ＮＤＴ位置算出ブロック２６は、水面反射データ除去ブロック２４から供給される水面反射データ除去後の点群データに基づいてＮＤＴ位置を算出する。この場合、ＮＤＴ位置算出ブロック２６は、水面反射データ除去ブロック２４から供給されるワールド座標系の点群データと、ワールド座標系で表されたボクセルデータＶＤとを照合することで、点群データとボクセルとの対応付けを行う。この場合、ボクセルデータＶＤのｚ座標値（式（２）では平均ベクトルμｎのｚ座標値）は、水面高さ位置に基づき高さ方向処理ブロック２５により水面基準座標値から絶対座標値に変換されている。そして、ＮＤＴ位置算出ブロック２６は、点群データと対応付けがなされた各ボクセルを対象として、式（２）に基づく個別評価関数値Ｅｎを算出し、式（３）に基づくスコア値Ｅ（ｋ）が最大となるとなる推定パラメータＰを算出する。そして、ＮＤＴ位置算出ブロック２６は、式（４）に基づき、デッドレコニングブロック２１が出力するＤＲ位置ＸＤＲ（ｋ）に対し、時刻ｋで求めた推定パラメータＰを適用することで定まる時刻ｋでのＮＤＴ位置ＸＮＤＴ（ｋ）を求める。ＮＤＴ位置算出ブロック２６は、ＮＤＴ位置ＸＮＤＴ（ｋ）を、時刻ｋでの推定自己位置Ｘ＾（ｋ）として出力する。
【0067】
（３－５）処理フロー
図８は、情報処理装置１の自己位置推定部１５が実行する自己位置推定処理の手順を示すフローチャートの一例である。自己位置推定部１５は、図８のフローチャートの処理を、電源がオンになった場合など、自己位置推定を行う必要が生じた場合に開始する。
図８は、情報処理装置１の自己位置推定部１５が実行する自己位置推定処理の手順を示すフローチャートの一例である。自己位置推定部１５は、図８のフローチャートの処理を、電源がオンになった場合など、自己位置推定を行う必要が生じた場合に開始する。
【0068】
まず、自己位置推定部１５は、自己位置推定処理の開始直後において、ＧＮＳＳ受信機５が出力するデータに基づくＧＮＳＳ測位結果から予測自己位置を算出する（ステップＳ１１）。
【0069】
次に、自己位置推定部１５は、速度センサ４とＩＭＵ６等に基づき検出した対象船舶の移動速度及び角速度と、前回の推定自己位置とから、デッドレコニングを実行し、予測自己位置となるＤＲ位置ＸＤＲを算出する（ステップＳ１２）。次に、自己位置推定部１５は、ライダ３の出力に基づく点群データを、ワールド座標系の点群データに変換する（ステップＳ１３）。
【0070】
次に、自己位置推定部１５は、水面位置（詳しくは水面高さ位置）を推定する（ステップＳ１４）。この場合、自己位置推定部１５は、例えば、ステップＳ１３でワールド座標系に変換した点群データから水面位置データを抽出し、抽出した水面位置データのｚ方向の平均に基づき、水面高さ位置を推定する。なお、自己位置推定部１５は、この場合、複数フレームでの水面位置データ（即ちライダ３の直近の複数周期にわたる走査により得られた水面位置データ）を収集し、収集した水面位置データに基づき、水面高さ位置を推定してもよい。そして、自己位置推定部１５は、推定した水面位置よりも下方に位置するデータを、水面反射データと判定し、水面反射データを、ＮＤＴ処理対象の点群データから除去する（ステップＳ１５）。
【0071】
次に、自己位置推定部１５は、ＮＤＴスキャンマッチングに使用するボクセルに対応するボクセルデータＶＤを抽出する（ステップＳ１６）。この場合、自己位置推定部１５は、例えば、ｘｙ平面においてＤＲ位置から所定距離（例えばライダ３の測距可能距離）以内に存在するボクセルに対応するボクセルデータＶＤを地図ＤＢ１０から抽出する。
【0072】
そして、自己位置推定部１５は、浮遊物と紐づく属性のボクセルが存在するか否か判定する（ステップＳ１７）。そして、自己位置推定部１５は、浮遊物と紐づく属性のボクセルが存在する場合（ステップＳ１７；Ｙｅｓ）、対象のボクセルについて、ステップＳ１４で推定した水面高さ位置に基づきｚ座標値を補正する（ステップＳ１８）。これにより、自己位置推定部１５は、浮遊物と紐づく属性のボクセルについて、水面基準座標値で表されたｚ座標値を絶対座標値に変換する。
【0073】
そして、自己位置推定部１５は、ステップＳ１２で算出したＤＲ位置ＸＤＲを初期値として、ワールド座標系の点群データと、ステップＳ１６で抽出されたボクセルデータ（ステップ１８でｚ座標値が補正されたものを含む）とに基づき、ＮＤＴスキャンマッチングによる自己位置推定を行う（ステップＳ１９）。これにより、自己位置推定部１５は、推定自己位置Ｘ＾とするＮＤＴ位置ＸＮＤＴを算出する。
【0074】
次に、自己位置推定部１５は、自己位置推定処理を終了すべきか否か判定する（ステップＳ２０）。そして、自己位置推定部１５は、自己位置推定処理を終了すべきと判定した場合（ステップＳ２０；Ｙｅｓ）、フローチャートの処理を終了する。一方、自己位置推定部１５は、自己位置推定処理を継続する場合（ステップＳ２０；Ｎｏ）、ステップＳ１２へ処理を戻し、ステップＳ１９で算出した推定自己位置Ｘ＾を用いて、次の処理時刻での自己位置の推定を行う。
【0075】
（４）変形例
以下、上述の実施例に好適な変形例について説明する。以下の変形例は、組み合わせてこれらの実施例に適用してもよい。
以下、上述の実施例に好適な変形例について説明する。以下の変形例は、組み合わせてこれらの実施例に適用してもよい。
【0076】
（変形例１）
情報処理装置１は、点群データからの水面反射データの除去に加えて、水面位置より（高さ方向において）下に位置するボクセルのボクセルデータＶＤを、ＮＤＴスキャンマッチングにおける点群データとの対応付けの対象から除外してもよい。
情報処理装置１は、点群データからの水面反射データの除去に加えて、水面位置より（高さ方向において）下に位置するボクセルのボクセルデータＶＤを、ＮＤＴスキャンマッチングにおける点群データとの対応付けの対象から除外してもよい。
【0077】
ここで、その推定した水面位置より下方に存在するボクセルは水面下にあるため，自己位置推定の対象にはならない。以上を勘案し、本変形例では、情報処理装置１は、点群データとの対応付けの対象から上記のボクセルのボクセルデータＶＤを除外して（即ち無いものとみなして）ＮＤＴ位置の算出を行う。これにより、情報処理装置１は、点群データとボクセルとの対応付けを行う際に、不必要な処理を省くことができ、対応付けの誤りに起因した位置推定精度の低下を好適に抑制することができる。
【0078】
（変形例２）
ボクセルデータＶＤは、図４又は図６に示すように平均ベクトルと共分散行列とを含むデータ構造に限定されない。例えば、ボクセルデータＶＤは、平均ベクトルと共分散行列を算出する際に用いられる点群データをそのまま含んでいてもよい。この場合においても、浮遊物と紐づく属性を有するボクセルの点群データのｚ座標値については、水面基準座標値により表されており、情報処理装置１は、当該ボクセルを参照した場合に、推定した水面高さ位置に基づき、当該ボクセルのｚ座標値を水面基準座標値から絶対座標値に変換する。
ボクセルデータＶＤは、図４又は図６に示すように平均ベクトルと共分散行列とを含むデータ構造に限定されない。例えば、ボクセルデータＶＤは、平均ベクトルと共分散行列を算出する際に用いられる点群データをそのまま含んでいてもよい。この場合においても、浮遊物と紐づく属性を有するボクセルの点群データのｚ座標値については、水面基準座標値により表されており、情報処理装置１は、当該ボクセルを参照した場合に、推定した水面高さ位置に基づき、当該ボクセルのｚ座標値を水面基準座標値から絶対座標値に変換する。
【0079】
（変形例３）
浮遊物であることを示す属性を付与する対象の地図データは、ボクセルデータに限定されない。例えば、水路標識用ブイや、灯浮標など水上のランドマーク的な地物を表す地図データの場合は、ボクセルデータではなく、各地物の位置情報や形状、大きさなどを示す地物ごとの地物データが地図データとして格納される。このような地図データについては、浮遊物に該当する地物データに、浮遊物であることを示す属性情報が付加されたデータ構造を有する。これにより、地図データを参照する情報処理装置１は、対象となるブイなどの浮遊物のｚ座標値を水面基準座標値として扱うことができ、浮遊物のｚ座標値に関する絶対座標値への変換を適切に行うことが可能になる。
浮遊物であることを示す属性を付与する対象の地図データは、ボクセルデータに限定されない。例えば、水路標識用ブイや、灯浮標など水上のランドマーク的な地物を表す地図データの場合は、ボクセルデータではなく、各地物の位置情報や形状、大きさなどを示す地物ごとの地物データが地図データとして格納される。このような地図データについては、浮遊物に該当する地物データに、浮遊物であることを示す属性情報が付加されたデータ構造を有する。これにより、地図データを参照する情報処理装置１は、対象となるブイなどの浮遊物のｚ座標値を水面基準座標値として扱うことができ、浮遊物のｚ座標値に関する絶対座標値への変換を適切に行うことが可能になる。
【0080】
以上説明したように、本実施例に係るボクセルデータＶＤのデータ構造は、情報処理装置１が参照する地図データのデータ構造である地図データ構造であって、構造物の位置を示す位置情報と、構造物が水面に対する浮遊物であるか否かを示す属性情報と、を有する。そして、属性情報は、当該属性情報が浮遊物であることを示す場合、情報処理装置１が、位置情報を、水面位置を基準とする相対的な位置情報とみなして構造物の位置を特定する処理に用いられる。ボクセルデータＶＤは、このようなデータ構造を備えることで、ボクセルデータＶＤを参照する情報処理装置１が潮位の変化に依らずに適切に水面に浮かぶ浮遊物の位置を特定可能な態様により、浮遊物の位置情報を格納することができる。
【0081】
なお、上述した実施例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータであるコントローラ等に供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記憶媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記憶媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記憶媒体（例えば光磁気ディスク）、ＣＤ－ＲＯＭ（Read Only Memory）、ＣＤ－Ｒ、ＣＤ－Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory））を含む。
【0082】
以上、実施例を参照して本願発明を説明したが、本願発明は上記実施例に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。すなわち、本願発明は、請求の範囲を含む全開示、技術的思想にしたがって当業者であればなし得るであろう各種変形、修正を含むことは勿論である。また、引用した上記の特許文献等の各開示は、本書に引用をもって繰り込むものとする。
【符号の説明】
【0083】
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】