知財求人 - 知財ポータルサイト「IP Force」
▶ パイオニア株式会社の特許一覧 ▶ パイオニアスマートセンシングイノベーションズ株式会社の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-12-23
(45)【発行日】2025-01-07
(54)【発明の名称】情報処理装置、制御方法、プログラム及び記憶媒体
(51)【国際特許分類】
B63H 25/04 20060101AFI20241224BHJP
B63H 25/02 20060101ALI20241224BHJP
B63B 49/00 20060101ALI20241224BHJP
G01S 17/93 20200101ALI20241224BHJP
【ＦＩ】
B63H25/04 G
B63H25/02 Z
B63H25/04 D
B63B49/00 Z
G01S17/93
【請求項の数】
12
(21)【出願番号】P 2021027603
(22)【出願日】2021-02-24
(65)【公開番号】P2022129058
(43)【公開日】2022-09-05
【審査請求日】2024-01-12
(73)【特許権者】
【識別番号】000005016
【氏名又は名称】パイオニア株式会社
(73)【特許権者】
【識別番号】520001073
【氏名又は名称】パイオニアスマートセンシングイノベーションズ株式会社
(74)【代理人】
【識別番号】100107331
【弁理士】
【氏名又は名称】中村 聡延
(72)【発明者】
【氏名】加藤 正浩
【審査官】中川 隆司
(56)【参考文献】
【文献】国際公開第２０２１／０８４５８３（ＷＯ，Ａ１）
【文献】特開２０１２－１９８２４３（ＪＰ，Ａ）
【文献】国際公開第２０１８／２２１４５８（ＷＯ，Ａ１）
【文献】米国特許出願公開第２０１７／０３２８６８３（ＵＳ，Ａ１）
【文献】中国特許出願公開第１１１９１３１７５（ＣＮ，Ａ）
【文献】米国特許出願公開第２００２／０１２６０２４（ＵＳ，Ａ１）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｂ６３Ｈ ２５／０４
Ｂ６３Ｈ ２５／０２
Ｂ６３Ｂ ４９／００
Ｇ０１Ｓ １７／９３
(57)【特許請求の範囲】
【請求項1】
水面位置を取得する取得手段と、船舶に設けられた計測装置が生成する点群データのうち、前記水面位置より下方の位置を示すデータを、前記計測装置が出射する光が水面で反射されることで生成された水面反射データとして検出する検出手段と、
前記水面反射データを誤検出データと判定する判定手段と、
を有する情報処理装置。
【請求項2】
前記取得手段は、前記点群データを構成する計測点のうち、水面位置を表すと推定される計測点が表す高さ方向の位置に基づき、前記水面位置を決定する、請求項１に記載の情報処理装置。
【請求項3】
前記取得手段は、前記水面位置を表すと推定される計測点の高さ方向の位置の分散値に基づき、前記検出手段が用いる前記水面位置の更新の要否を判定する、請求項２に記載の情報処理装置。
【請求項4】
前記取得手段は、前記水面位置を表すと推定される計測点の高さ方向の位置の分散値が所定の閾値未満の場合、当該計測点の高さ方向の位置の平均値に基づき、前記検出手段が用いる前記水面位置を更新し、
前記分散値が前記閾値以上の場合、前記検出手段が用いる前記水面位置の更新を行わない、請求項３に記載の情報処理装置。
【請求項5】
単位領域であるボクセルごとの物体の位置を表すボクセルデータと、前記点群データから前記水面位置の暫定位置に基づき前記水面反射データを除去したデータとを照合して前記船舶の位置推定を行った場合の当該位置推定の信頼度指標を算出する信頼度指標算出手段をさらに有し、前記取得手段は、複数の前記暫定位置に対する前記信頼度指標に基づき、前記水面位置を推定する、請求項１に記載の情報処理装置。
【請求項6】
前記取得手段は、前記点群データに基づき算出した水面の推定位置に対してオフセットを設けた位置を、前記水面位置として決定する、請求項１～５のいずれか一項に記載の情報処理装置。
【請求項7】
前記取得手段が取得した最新の所定個数分の水面位置に基づき、前記検出手段が用いる水面位置を決定するフィルタ手段をさらに有する、請求項１～６のいずれか一項に記載の情報処理装置。
【請求項8】
単位領域であるボクセルごとの物体の位置を表すボクセルデータと、前記点群データから前記水面反射データを除去したデータとの照合に基づき、前記船舶の位置推定を行う位置推定手段をさらに有する、請求項１～７のいずれか一項に記載の情報処理装置。
【請求項9】
前記位置推定手段は、前記水面位置より下に存在するボクセルのボクセルデータを、前記照合の対象から除外する、請求項８に記載の情報処理装置。
【請求項10】
コンピュータが実行する制御方法であって、水面位置を取得し、
船舶に設けられた計測装置が生成する点群データのうち、前記水面位置より下方の位置を示すデータを、前記計測装置が出射する光が水面で反射されることで生成された水面反射データとして検出し、
前記水面反射データを誤検出データと判定する、
制御方法。
【請求項11】
水面位置を取得し、船舶に設けられた計測装置が生成する点群データのうち、前記水面位置より下方の位置を示すデータを、前記計測装置が出射する光が水面で反射されることで生成された水面反射データとして検出し、
前記水面反射データを誤検出データと判定する処理をコンピュータに実行させるプログラム。
【請求項12】
請求項１１に記載のプログラムを記憶した記憶媒体。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、船舶において計測したデータの処理に関する。
【背景技術】
【0002】
従来から、レーザスキャナなどの計測装置を用いて計測した周辺物体の形状データを、予め周辺物体の形状が記憶された地図情報と照合（マッチング）することで、移動体の自己位置を推定する技術が知られている。例えば、特許文献１には、空間を所定の規則で分割したボクセル中における検出物が静止物か移動物かを判定し、静止物が存在するボクセルを対象として地図情報と計測データとのマッチングを行う自律移動システムが開示されている。また、特許文献２には、ボクセル毎の静止物体の平均ベクトルと共分散行列とを含むボクセルデータとライダが出力する点群データとの照合により自己位置推定を行うスキャンマッチング手法が開示されている。さらに、特許文献３には、船舶の自動着岸を行う自動着岸装置において、ライダから照射される光が着岸位置の周囲の物体に反射してライダにより受光できるように、船舶の姿勢を変化させる制御を行う手法が記載されている。
【先行技術文献】
【特許文献】
【0003】
【文献】国際公開ＷＯ２０１３／０７６８２９
【文献】国際公開ＷＯ２０１８／２２１４５３
【文献】特開２０２０－５９４０３号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
昨今、自動車分野だけでなく、船舶においても、自動操船システム検討が進められており，安全な自動操船には精度の良い自己位置推定は同様に重要である。外洋では周辺に構造物が少ないためＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）により自己位置を測位可能であるが、都市部の沿岸や河川では高層ビル等が隣接するため、受信衛星数の低下やマルチパスなどによりＧＮＳＳ電波の受信環境は悪く、精度の良い測位はできないことが多い。よって、船舶の場合においても、上記のスキャンマッチングによる高精度な位置推定が好適に適用される。
【0005】
一方、船舶にライダを搭載すると、ライダの下方向に向いたビームの一部が水面で反射し、実際には存在しない場所からの検出データが得られてしまう。そのような水面反射によるデータは誤検出（誤警報）データであり、自己位置推定の精度に影響を与える。しかし，水面反射による不正確なデータなのか、あるいは正確なデータなのかは、ライダの検出信号からでは識別できない。
【0006】
本開示は、上記のような課題を解決するためになされたものであり、点群データに含まれる誤検出データを的確に判定することが可能な情報処理装置を提供することを主な目的とする。
【課題を解決するための手段】
【0007】
請求項に記載の発明は、
水面位置を取得する取得手段と、
船舶に設けられた計測装置が生成する点群データのうち、前記水面位置より下方の位置を示すデータを、前記計測装置が出射する光が水面で反射されることで生成された水面反射データとして検出する検出手段と、
前記水面反射データを誤検出データと判定する判定手段と、
を有する情報処理装置である。
水面位置を取得する取得手段と、
船舶に設けられた計測装置が生成する点群データのうち、前記水面位置より下方の位置を示すデータを、前記計測装置が出射する光が水面で反射されることで生成された水面反射データとして検出する検出手段と、
前記水面反射データを誤検出データと判定する判定手段と、
を有する情報処理装置である。
【0008】
また、請求項に記載の発明は、
コンピュータが実行する制御方法であって、
水面位置を取得し、
船舶に設けられた計測装置が生成する点群データのうち、前記水面位置より下方の位置を示すデータを、前記計測装置が出射する光が水面で反射されることで生成された水面反射データとして検出し、
前記水面反射データを誤検出データと判定する、
制御方法である。
コンピュータが実行する制御方法であって、
水面位置を取得し、
船舶に設けられた計測装置が生成する点群データのうち、前記水面位置より下方の位置を示すデータを、前記計測装置が出射する光が水面で反射されることで生成された水面反射データとして検出し、
前記水面反射データを誤検出データと判定する、
制御方法である。
【0009】
また、請求項に記載の発明は、
水面位置を取得し、
船舶に設けられた計測装置が生成する点群データのうち、前記水面位置より下方の位置を示すデータを、前記計測装置が出射する光が水面で反射されることで生成された水面反射データとして検出し、
前記水面反射データを誤検出データと判定する処理をコンピュータに実行させるプログラムである。
水面位置を取得し、
船舶に設けられた計測装置が生成する点群データのうち、前記水面位置より下方の位置を示すデータを、前記計測装置が出射する光が水面で反射されることで生成された水面反射データとして検出し、
前記水面反射データを誤検出データと判定する処理をコンピュータに実行させるプログラムである。
【図面の簡単な説明】
【0010】
【図1】運転支援システムの概略構成図である。
【図2】情報処理装置の機能的構成を示すブロック図である。
【図3】自己位置推定部が推定すべき自己位置を３次元直交座標で表した図である。
【図4】ボクセルデータの概略的なデータ構造の一例を示す。
【図5】（Ａ）対象船舶に設けられたライダの配置の一例を示す対象船舶の俯瞰図である。（Ｂ）構造物に近接する対象船舶を後方から観察した図である。
【図6】（Ａ）ボクセルデータが存在するボクセルを明示した対象船舶の後方図である。（Ｂ）ライダの高さ位置を表す破線と水面位置を表す破線とを明示した対象船舶の後方図である。
【図7】自己位置推定部の機能ブロックの一例である。
【図8】自己位置推定処理の手順を示すフローチャートの一例である。
【図9】水面反射データを除去せずにＮＤＴ位置推定を行う比較例に係る自車位置推定結果を示す。
【図10】水面反射データを除去せずにＮＤＴ位置推定を行う比較例に係る自車位置推定結果を示す。
【図11】水面反射データを除去する実施例に係る自車位置推定結果を示す。
【図12】水面反射データを除去する実施例に係る自車位置推定結果を示す。
【図13】変形例における水面位置推定処理の概要を示す図である。
【図14】変形例における水面位置推定処理の手順を示すフローチャートの一例である。
【発明を実施するための形態】
【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】
【数1】
【0044】
【数2】
【0045】
次に、ボクセルデータＶＤを用いたＮＤＴスキャンマッチングの概要について説明する。
【0046】
船舶を想定したＮＤＴによるスキャンマッチングは、水平面（ここではｘｙ座標とする）内の移動量及び船舶の向きを要素とした推定パラメータ
Ｐ＝［ｔｘ、ｔｙ、ｔｚ、ｔφ、ｔθ、ｔψ］Ｔ
を推定することとなる。ここで、「ｔｘ」はｘ方向の移動量、「ｔｙ」はｙ方向の移動量、「ｔｚ」はｚ方向の移動量、「ｔφ」はロール角、「ｔθ」はピッチ角、「ｔψ」はヨー角を示す。
Ｐ＝［ｔｘ、ｔｙ、ｔｚ、ｔφ、ｔθ、ｔψ］Ｔ
を推定することとなる。ここで、「ｔｘ」はｘ方向の移動量、「ｔｙ」はｙ方向の移動量、「ｔｚ」はｚ方向の移動量、「ｔφ」はロール角、「ｔθ」はピッチ角、「ｔψ」はヨー角を示す。
【0047】
また、ライダ３が出力する点群データの座標を、
ＸＬ（ｊ）＝［ｘｎ（ｊ）、ｙｎ（ｊ）、ｚｎ（ｊ）］Ｔ
とすると、ＸＬ（ｊ）の平均値「Ｌ´ｎ」は、以下の式（３）により表される。
ＸＬ（ｊ）＝［ｘｎ（ｊ）、ｙｎ（ｊ）、ｚｎ（ｊ）］Ｔ
とすると、ＸＬ（ｊ）の平均値「Ｌ´ｎ」は、以下の式（３）により表される。
【0048】
【数3】
【0049】
そして、自己位置推定部１５は、地図ＤＢ１０と同一の座標系である絶対的な座標系（「ワールド座標系」とも呼ぶ。）に変換した点群データに対応付けられるボクセルデータＶＤを探索し、そのボクセルデータＶＤに含まれる平均ベクトルμｎと共分散行列Ｖｎとを用い、ボクセルｎの評価関数値（「個別評価関数値」とも呼ぶ。）「Ｅｎ」を算出する。この場合、自己位置推定部１５は、以下の式（４）に基づき、ボクセルｎの個別評価関数値Ｅｎを算出する。
【0050】
【数4】
【0051】
そして、自己位置推定部１５は、以下の式（５）により示される、マッチングの対象となる全てのボクセルを対象とした総合的な評価関数値（「スコア値」とも呼ぶ。）「Ｅ（ｋ）」を算出する。スコア値Ｅは、マッチングの適合度を示す指標となる。
【0052】
【数5】
【0053】
【数6】
【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】
そして、水面位置データ抽出ブロック２３は、対応付け比率ＤＡＲが変わらなかった場合（例えば、向上しなかった場合、又は、変化した比率が所定の閾値未満の場合）、水面反射データが十分に除外できていると判断し、前回の暫定位置を、水面位置として推定する。即ち、水面位置データ抽出ブロック２３は、対応付け比率ＤＡＲが変化しなくなる暫定位置を、水面位置として推定する。例えば、水面位置データ抽出ブロック２３は、図１３（Ｃ）に示す破線位置を暫定位置とした場合に対応付け比率ＤＡＲが向上したことから、図１３（Ｄ）に示す破線位置に暫定位置を更新したとする。その後、水面位置データ抽出ブロック２３は、更新した暫定位置に基づき対応付け比率ＤＡＲを算出し、算出した対応付け比率ＤＡＲが前回の対応付け比率ＤＡＲと変わらなかったことから、図１３（Ｃ）に示す破線位置を、水面位置として推定する。
【0111】
この方法によれば、水面位置データ抽出ブロック２３は、潮位情報を用いることなく水面位置を推定できる。例えば、海から上流の河川の場合や、気圧の変化が大きい場合等、実際の水面位置は潮位情報との差異が大きい状況も存在するが、そのような場合でも水面位置を的確に推定することが可能となる。なお、この処理は、常に行う必要はなく、最初および所定時間ごと、あるいは船舶が多く移動したときに実施すれば良い。
【0112】
図１４は、本変形例における水面位置推定処理の手順を示すフローチャートの一例である。
【0113】
まず、水面位置データ抽出ブロック２３は、対応付け比率ＤＡＲを０に初期化する（ステップＳ３１）。そして、水面位置データ抽出ブロック２３は、現在位置周辺（例えば現在位置から所定距離以内）のボクセルデータＶＤの中で、最も下方にあるデータ「ＶＤ＿ｍｉｎ」を抽出し、そのｚ値「ＶＤ＿ｍｉｎ（ｚ）」を暫定位置「ｚ＿ｔｍｐ」として設定する（ステップＳ３２）。即ち、水面位置データ抽出ブロック２３は、「ｚ＿ｔｍｐ＝ＶＤ＿ｍｉｎ（ｚ）」とする。
【0114】
次に、水面位置データ抽出ブロック２３は、ライダ３が出力する点群データのうち、暫定位置ｚ＿ｔｍｐ以下のデータを除外してＮＤＴ処理を実施し、対応付け比率ＤＡＲを算出する（ステップＳ３３）。そして、水面位置データ抽出ブロック２３は、ステップＳ３３で算出した対応付け比率ＤＡＲが前回の対応付け比率ＤＡＲよりも向上したか否か判定する（ステップＳ３４）。そして、水面位置データ抽出ブロック２３は、ステップＳ３３で算出した対応付け比率ＤＡＲが前回の対応付け比率ＤＡＲよりも向上したと判定した場合（ステップＳ３４；Ｙｅｓ）、暫定位置ｚ＿ｔｍｐを所定量「ｈ」だけ上にあげた位置を、新たな暫定位置ｚ＿ｔｍｐとして設定する（ステップＳ３６）。即ち、この場合、水面位置データ抽出ブロック２３は、前回の暫定位置を「ｚ＿ｔｍｐ（前回）」、更新後の暫定位置を「ｚ＿ｔｍｐ（今回）」とすると、「ｚ＿ｔｍｐ（今回）＝ｚ＿ｔｍｐ（前回）＋ｈ」を計算する。そして、ステップＳ３３へ処理を戻す。
【0115】
一方、水面位置データ抽出ブロック２３は、ステップＳ３３で算出した対応付け比率ＤＡＲが前回の対応付け比率ＤＡＲよりも向上しなかったと判定した場合（ステップＳ３４；Ｎｏ）、前回の推定位置を推定水面位置として設定する（ステップＳ３５）。この場合、水面位置データ抽出ブロック２３は、推定水面位置を「ｚ＿ｅｓｔ」とすると、「ｚ＿ｅｓｔ＝ｚ＿ｔｍｐ（前回）」とする。
【0116】
このように、水面位置データ抽出ブロック２３は、水面位置に対する複数の暫定位置を設定して算出した信頼度指標に基づき、各暫定位置の妥当性を判定する。これにより、信頼度指標が高くなるような暫定位置を推定水面位置として好適に設定することができる。
【0117】
以上説明したように、本実施例に係る情報処理装置１のコントローラ１３は、水面位置を取得し、対象船舶に設けられたライダ３が生成する点群データのうち、水面位置より下方の位置を示すデータを、ライダ３が出射する光が水面で反射されることで生成された水面反射データとして検出する。そして、コントローラ１３は、水面反射データを誤検出データと判定する。これにより、情報処理装置１は、ライダ３が生成する点群データから誤検出データを好適に除外して位置推定などの種々の処理を高精度に行うことができる。
【0118】
なお、上述した実施例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータであるコントローラ等に供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記憶媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記憶媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記憶媒体（例えば光磁気ディスク）、ＣＤ－ＲＯＭ（Read Only Memory）、ＣＤ－Ｒ、ＣＤ－Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory））を含む。
【0119】
以上、実施例を参照して本願発明を説明したが、本願発明は上記実施例に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。すなわち、本願発明は、請求の範囲を含む全開示、技術的思想にしたがって当業者であればなし得るであろう各種変形、修正を含むことは勿論である。また、引用した上記の特許文献等の各開示は、本書に引用をもって繰り込むものとする。
【符号の説明】
【0120】
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】