知財求人 - 知財ポータルサイト「IP Force」
▶ パイオニア株式会社の特許一覧 ▶ パイオニアスマートセンシングイノベーションズ株式会社の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2022138383
(43)【公開日】2022-09-26
(54)【発明の名称】情報処理装置、制御方法、プログラム及び記憶媒体
(51)【国際特許分類】
G01B 11/00 20060101AFI20220915BHJP
G01C 15/00 20060101ALI20220915BHJP
G08G 3/02 20060101ALI20220915BHJP
【ＦＩ】
G01B11/00 A
G01C15/00 104C
G08G3/02 A
【審査請求】未請求
【請求項の数】10
【出願形態】ＯＬ
(21)【出願番号】P 2021038238
(22)【出願日】2021-03-10
(71)【出願人】
【識別番号】000005016
【氏名又は名称】パイオニア株式会社
(71)【出願人】
【識別番号】520001073
【氏名又は名称】パイオニアスマートセンシングイノベーションズ株式会社
(74)【代理人】
【識別番号】100107331
【弁理士】
【氏名又は名称】中村 聡延
(72)【発明者】
【氏名】加藤 正浩
(72)【発明者】
【氏名】幸田 健志
(72)【発明者】
【氏名】加藤 将大
(72)【発明者】
【氏名】後藤田 明
(72)【発明者】
【氏名】白鳥 邦雄
【テーマコード（参考）】
2F065
5H181
【Ｆターム（参考）】
2F065AA04
2F065AA09
2F065AA53
2F065BB15
2F065BB27
2F065CC14
2F065DD04
2F065FF11
2F065FF61
2F065FF67
2F065GG04
2F065HH04
2F065JJ01
2F065LL62
2F065MM06
2F065MM16
2F065PP22
2F065QQ03
2F065QQ25
2F065QQ28
2F065UU05
5H181AA25
5H181BB04
5H181CC03
5H181CC04
5H181CC12
5H181FF04
5H181FF05
5H181LL01
5H181LL02
5H181LL04
(57)【要約】
【課題】太陽光の影響を受けることなく、自己位置や周辺情報などを取得する。
【解決手段】情報処理装置において、点群データ取得手段は、船舶に設けられた計測装置が生成する点群データを取得する。太陽位置情報取得手段は、点群データの取得時点における、太陽の位置に関する太陽位置情報を取得する。誤検出データ抽出手段は、太陽位置情報に基づき、点群データから誤検出データを抽出する。
【選択図】図７
【解決手段】情報処理装置において、点群データ取得手段は、船舶に設けられた計測装置が生成する点群データを取得する。太陽位置情報取得手段は、点群データの取得時点における、太陽の位置に関する太陽位置情報を取得する。誤検出データ抽出手段は、太陽位置情報に基づき、点群データから誤検出データを抽出する。
【選択図】図７
【特許請求の範囲】
【請求項1】
船舶に設けられた計測装置が生成する点群データを取得する点群データ取得手段と、
前記点群データの取得時点における、太陽の位置に関する太陽位置情報を取得する太陽位置情報取得手段と、
前記太陽位置情報に基づき、前記点群データから誤検出データを抽出する誤検出データ抽出手段と、
を備える情報処理装置。
前記点群データの取得時点における、太陽の位置に関する太陽位置情報を取得する太陽位置情報取得手段と、
前記太陽位置情報に基づき、前記点群データから誤検出データを抽出する誤検出データ抽出手段と、
を備える情報処理装置。
【請求項2】
前記誤検出データは、太陽光が、水面で反射して前記計測装置に入射することにより生成されたデータであり、
前記誤検出データ抽出手段は、前記点群データから前記誤検出データを除去する請求項１に記載の情報処理装置。
前記誤検出データ抽出手段は、前記点群データから前記誤検出データを除去する請求項１に記載の情報処理装置。
【請求項3】
前記誤検出データ抽出手段は、前記太陽位置情報に基づき、太陽光が水面で反射して前記計測装置に入射する領域である太陽光反射領域を推定し、前記太陽光反射領域に対応する点群データを前記誤検出データとして前記点群データから除去する請求項１に記載の情報処理装置。
【請求項4】
前記太陽位置情報は、太陽の方位及び仰角を含み、
前記誤検出データ抽出手段は、前記太陽の方位及び仰角に基づいて前記太陽光反射領域を推定する請求項３に記載の情報処理装置。
前記誤検出データ抽出手段は、前記太陽の方位及び仰角に基づいて前記太陽光反射領域を推定する請求項３に記載の情報処理装置。
【請求項5】
前記誤検出データが除去された点群データを用いて、前記船舶の自己位置を推定する自己位置推定手段を備える請求項２乃至４のいずれか一項に記載の情報処理装置。
【請求項6】
前記誤検出データが除去された点群データを用いて、前記船舶の周辺に存在する障害物及び引き波の少なくとも一方を検出する検出手段を備える請求項２乃至５のいずれか一項に記載の情報処理装置。
【請求項7】
前記誤検出データが除去された点群データを用いて、前記船舶が着岸しようとする対象物までの距離を算出する算出手段を備える請求項２乃至６のいずれか一項に記載の情報処理装置。
【請求項8】
コンピュータが実行する制御方法であって、
船舶に設けられた計測装置が生成する点群データを取得し、
前記点群データの取得時点における、太陽の位置に関する太陽位置情報を取得し、
前記太陽位置情報に基づき、前記点群データから誤検出データを除去する制御方法。
船舶に設けられた計測装置が生成する点群データを取得し、
前記点群データの取得時点における、太陽の位置に関する太陽位置情報を取得し、
前記太陽位置情報に基づき、前記点群データから誤検出データを除去する制御方法。
【請求項9】
船舶に設けられた計測装置が生成する点群データを取得し、
前記点群データの取得時点における、太陽の位置に関する太陽位置情報を取得し、
前記太陽位置情報に基づき、前記点群データから誤検出データを除去する処理をコンピュータに実行させるプログラム。
前記点群データの取得時点における、太陽の位置に関する太陽位置情報を取得し、
前記太陽位置情報に基づき、前記点群データから誤検出データを除去する処理をコンピュータに実行させるプログラム。
【請求項10】
請求項９に記載のプログラムを記憶した記憶媒体。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、船舶において計測したデータの処理に関する。
【背景技術】
【0002】
従来から、レーザスキャナなどの計測装置を用いて計測した周辺物体の形状データを、予め周辺物体の形状が記憶された地図情報と照合（マッチング）することで、移動体の自己位置を推定する技術が知られている。例えば、特許文献１には、空間を所定の規則で分割したボクセル中における検出物が静止物か移動物かを判定し、静止物が存在するボクセルを対象として地図情報と計測データとのマッチングを行う自律移動システムが開示されている。また、特許文献２には、ボクセル毎の静止物体の平均ベクトルと共分散行列とを含むボクセルデータとライダが出力する点群データとの照合により自己位置推定を行うスキャンマッチング手法が開示されている。また、特許文献３には、車両に搭載されたセンサについて、太陽光の入射による性能の低下を推定する手法が記載されている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】国際公開ＷＯ２０１３／０７６８２９
【特許文献2】国際公開ＷＯ２０１８／２２１４５３
【特許文献3】特開２０１９－１４８５３１号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
昨今、自動車分野だけでなく、船舶においても運転支援システムの検討が進められている。例えば、桟橋までの距離を計測することで安全な着桟を支援したり、船舶近くの障害物を検知して回避を促したり、引き波を検知して安全な乗り越えに活用したりするシステムの開発が進められている。
【0005】
上記のような運転支援システムは、自船周囲のデータを検出することで、船舶の位置や周辺情報の計測や検知を行う。しかし、晴天の日中は、水面で反射した太陽光がライダなどの計測装置に到達してしまうことがある。この場合、水面付近のデータを集める際に、太陽光の影響を受けてしまう。
【0006】
本開示は、上記のような課題を解決するためになされたものであり、太陽光の影響を受けることなく、自己位置や周辺情報などを取得することが可能な情報処理装置を提供することを主な目的とする。
【課題を解決するための手段】
【0007】
請求項に記載の発明は、情報処理装置であって、船舶に設けられた計測装置が生成する点群データを取得する点群データ取得手段と、前記点群データの取得時点における、太陽の位置に関する太陽位置情報を取得する太陽位置情報取得手段と、前記太陽位置情報に基づき、前記点群データから誤検出データを抽出する誤検出データ抽出手段と、を備える。
【0008】
また、請求項に記載の発明は、コンピュータが実行する制御方法であって、船舶に設けられた計測装置が生成する点群データを取得し、前記点群データの取得時点における、太陽の位置に関する太陽位置情報を取得し、前記太陽位置情報に基づき、前記点群データから誤検出データを除去する。
【0009】
また、請求項に記載の発明は、プログラムであって、船舶に設けられた計測装置が生成する点群データを取得し、前記点群データの取得時点における、太陽の位置に関する太陽位置情報を取得し、前記太陽位置情報に基づき、前記点群データから誤検出データを除去する処理をコンピュータに実行させる。
【図面の簡単な説明】
【0010】
【図1】運転支援システムの概略構成図である。
【図2】情報処理装置の構成を示すブロック図である。
【図3】自己位置推定部が推定すべき自己位置を３次元直交座標で表した図である。
【図4】ボクセルデータの概略的なデータ構造の一例を示す。
【図5】（Ａ）対象船舶に設けられたライダの配置の一例を示す対象船舶の俯瞰図である。（Ｂ）構造物に近接する対象船舶を後方から観察した図である。
【図6】（Ａ）ボクセルデータが存在するボクセルを明示した対象船舶の後方図である。（Ｂ）ライダの高さ位置を表す破線と水面位置を表す破線とを明示した対象船舶の後方図である。（Ｃ）太陽光の反射成分を示す図である。
【図7】太陽光の反射成分を除去する方法を説明する図である。
【図8】太陽光の反射成分の除去範囲を調整する方法を説明する図である。
【図9】自己位置推定部の機能ブロックの一例である。
【図10】自己位置推定処理の手順を示すフローチャートの一例である。
【図11】障害物及び引き波の検出方法を説明する図である。
【図12】障害物及び引き波の検出方法を説明する他の図である。
【図13】障害物及び引き波の検出方法を説明する他の図である。
【図14】障害物／引き波検出処理のフローチャートである。
【図15】障害物及び引き波を検出する例を示す。
【図16】障害物などの探索範囲を変化させる方法を説明する図である。
【図17】桟橋の検出方法を説明する図である。
【発明を実施するための形態】
【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】
（変形例５）
上述した実施例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータであるコントローラ等に供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記憶媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記憶媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記憶媒体（例えば光磁気ディスク）、ＣＤ－ＲＯＭ（Read Only Memory）、ＣＤ－Ｒ、ＣＤ－Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory））を含む。
上述した実施例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータであるコントローラ等に供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記憶媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記憶媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記憶媒体（例えば光磁気ディスク）、ＣＤ－ＲＯＭ（Read Only Memory）、ＣＤ－Ｒ、ＣＤ－Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory））を含む。
【0106】
（５）障害物／引き波の検出
次に、障害物／引き波検出部１６による障害物及び引き波の検出について説明する。障害物／引き波検出部１６は、自己位置推定部１５が算出した水面高さを障害物や引き波の検出に使用する。船舶の近くに障害物がある場合、障害物との衝突や接触を避けて航行することが必要となる。障害物とは、例えば、他船、杭、橋脚、ブイ、網、ゴミなどである。また、近くに他船による引き波がある場合、船舶の航行には注意を要する。そこで、障害物／引き波検出部１６は、水面高さを用いて、船舶の近くに障害物や引き波があることを検出する。
次に、障害物／引き波検出部１６による障害物及び引き波の検出について説明する。障害物／引き波検出部１６は、自己位置推定部１５が算出した水面高さを障害物や引き波の検出に使用する。船舶の近くに障害物がある場合、障害物との衝突や接触を避けて航行することが必要となる。障害物とは、例えば、他船、杭、橋脚、ブイ、網、ゴミなどである。また、近くに他船による引き波がある場合、船舶の航行には注意を要する。そこで、障害物／引き波検出部１６は、水面高さを用いて、船舶の近くに障害物や引き波があることを検出する。
【0107】
具体的には、障害物／引き波検出部１６は、図１１（Ａ）に示すように、船舶の周辺に設定した探索範囲Ｒに属する点群データを取得し、点間距離によるクラスタリング（ユークリッドクラスタリング）などにより点群データのかたまり（クラスタ）を検出する。図１１（Ｂ）に示すように、船舶の近くに障害物や引き波が無い場合、船舶の近くで得られた点群データには点群データのかたまりは含まれない。一方、図１２（Ａ）に示すように、障害物は点群データのかたまりとして検出できる。また、図１２（Ｂ）に示すように、引き波は点群データの線状のかたまりとして検出できる。よって、障害物／引き波検出部１６は、船舶の周辺で得られた点群データのかたまりを検出することにより、障害物や引き波を検出することができる。
【0108】
船舶の周辺の点群データを取得する際、障害物／引き波検出部１６は、船舶の自己位置及び水面高さＨｗに基づいて探索範囲を決定する。図１１（Ａ）、図１１（Ｂ）に示すように、探索範囲Ｒは、水面上の平面範囲Ｒｐと、高さ範囲Ｒｈにより規定される。平面範囲Ｒｐは、ｘｙ平面における範囲であり、例えば自己位置を中心とする円形又は多角形の範囲とすることができる。図１１（Ｂ）は円形の平面範囲Ｒｐの例を示している。高さ範囲Ｒｈは、ｚ軸方向の範囲であり、水面高さＨｗを中心として上下方向に所定距離の範囲とすることができる。
【0109】
障害物や引き波を検出する際にも、太陽光の反射成分の影響を排除する必要がある。図１３（Ａ）に示すように、晴天の日中などには、太陽光が水面で反射し、ライダ３に入射する。図１３（Ａ）において、船舶の左側では、太陽光の影響を受けず、ライダ３は障害物（ブイ）７２による反射光を受光する。一方、船舶の右側では、ライダ３は、出射光の水面での反射光に混ざって、太陽光の反射成分を受光してしまう。この場合、図１３（Ｂ）に示すように、障害物／引き波検出部１６はブイ７２を正しく検出することはできる。一方、太陽光の反射成分が太陽光反射領域７１で検出されるため、障害物／引き波検出部１６は、太陽光反射領域７１をクラスタと判定し、何らかの障害物があると誤判定してしまう恐れがある。また、太陽光反射領域７１がクラスタと判定されると、引き波７３を線状と判定することができず、引き波７３の検出漏れが生じる可能性もある。
【0110】
そこで、障害物／引き波検出部１６は、船舶の周辺で取得された点群データから太陽光反射データを除去してから、障害物及び引き波の検出を行う。なお、太陽光反射データの特定方法は、基本的に「（４－２）太陽光反射データの除去」の欄で説明した方法と同様である。
【0111】
図１４は、上記の手法による障害物／引き波検出処理のフローチャートである。この処理は、コントローラ１３が予め用意されたプログラムを実行し、障害物／引き波検出部１６として動作することにより実現される。なお、この処理は、所定時間毎に繰り返し実行される。
【0112】
まず、コントローラ１３は、船舶の自己位置及び水面高さＨｗを取得し、自己位置及び水面高さに基づき探索範囲Ｒを設定する（ステップＳ３１）。具体的に、コントローラ１３は、船舶の自己位置から水平方向に所定範囲を平面範囲Ｒｐとし、水面高さＨｗを中心として上下に所定距離の範囲を高さ範囲Ｒｈとする。
【0113】
次に、コントローラ１３は、探索範囲Ｒに属する点群データを抽出する（ステップＳ３２）。次に、コントローラ１３は、日時に基づいて太陽の方位及び仰角を取得し、ステップＳ３２で抽出された点群データから太陽光反射データを除去する（ステップＳ３３）。次に、コントローラ１３は、クラスタリングなどにより点群データのかたまりを検出する（ステップＳ３４）。点群データのかたまりが検出されなかった場合（ステップＳ３４：Ｎｏ）、処理は終了する。一方、点群データのかたまりが検出された場合（ステップＳ３４：Ｙｅｓ）、コントローラ１３は、そのかたまりが所定時間継続して、即ち、複数フレームにわたって検出されたか否かを判定する（ステップＳ３５）。
【0114】
点群データのかたまりが複数フレームにわたって検出されなかった場合（ステップＳ３５：Ｎｏ）、処理は終了する。一方、点群データのかたまりが複数フレームにわたって検出された場合（ステップＳ３５：Ｙｅｓ）、コントローラ１３は、例えばハフ変換などの手法を用いて、点群データのかたまりが線状であるか否かを判定する（ステップＳ３６）。点群データのかたまりが線状でない場合（ステップＳ３６：Ｎｏ）、コントローラ１３はそのかたまりを障害物と判定する（ステップＳ３７）。一方、点群データのかたまりが線状である場合（ステップＳ３６：Ｙｅｓ）、コントローラ１３はそのかたまりを引き波と判定する（ステップＳ３８）。そして、処理は終了する。
【0115】
図１５（Ａ）は、障害物の近くに太陽光反射領域７１がある場合の例を示す。この場合、障害物／引き波検出部１６は、太陽の方位及び仰角に基づいて太陽光反射領域７１を特定し、太陽光反射領域７１内で取得された点群データを全て除去する。なお、太陽光反射領域７１内に実際の障害物の点群データが含まれる場合もあるが、通常は船舶と障害物は異なる移動速度で移動するため、上記のように複数フレームにわたって検出を行うことにより、障害物は正しく検出される。
【0116】
図１５（Ｂ）は、引き波上に太陽光反射領域７１がある場合の例を示す。この場合も、障害物／引き波検出部１６は、太陽光反射領域７１内で取得された点群データを全て除去する。なお、クラスタリングにより検出されたかたまりに対してハフ変換を適用する際、直線が破線であることを許容してハフ変換を適用することにより、太陽光反射領域７１を除去しても引き波を検出することが可能となる。なお、直線が破線であることを許容するとは、ハフ変換における直線判定の閾値を少し緩和することであり、適度な閾値の緩和により、短い直線や破線も検出されるようになる。
【0117】
なお、上記のように障害物／引き波検出部１６は、水平高さＨｗを基準として高さ範囲Ｒｈを設定し、探索範囲Ｒに属する点群データを抽出する。しかしながら、波が大きい場合、水面に浮遊している障害物は高さ範囲Ｒｈから外れてしまい、検出できなくなる恐れがある。一方、それを防止するために高さ範囲Ｒｈを大きくすると、波が小さい場合に無関係なデータが入り込み、検出精度が低下してしまう。
【0118】
そこで、障害物／引き波検出部１６は、探索範囲Ｒに属する点群データのｚ値の分散を用いて波の高さ（波高）を推定し、波高に応じて高さ範囲Ｒｈを設定する。具体的に、点群データのｚ値の分散が小さい場合、図１６（Ａ）に示すように波高は小さいと推測される。この場合、障害物／引き波検出部１６は、高さ範囲Ｒｈを狭くする。例えば、障害物／引き波検出部１６は、高さ範囲Ｒｈを、水面高さＨｗ付近に設定する。これにより、ノイズの混入を低減できるため、障害物や引き波の検出精度が向上する。
【0119】
一方、探索範囲Ｒに属する点群データのｚ値の分散が大きい場合、図１６（Ｂ）に示すように波高が大きいと推測される。よって、障害物／引き波検出部１６は、高さ範囲Ｒｈを広くする。即ち、障害物／引き波検出部１６は、水面高さＨｗを中心に、波高が小さいときよりも広い高さ範囲Ｒｈを設定する。一例としては、障害物／引き波検出部１６は、図１６（Ｃ）に示すように、探索範囲Ｒで得られた点群データのｚ値の標準偏差σを求め、高さＲｈ範囲を、例えば水面高さＨｗを中心として±３σの範囲などと決定してもよい。これにより、波が高い場合でも、探索範囲を広くすることができるため、障害物や引き波の検出漏れを防止することができる。
【0120】
（６）桟橋の検出
次に、桟橋検出部１７による桟橋の検出において太陽光の影響を排除する方法について説明する。船舶が桟橋に着桟する際、船舶から桟橋までの距離、速度、角度などを逐次計測し、それを操船者又は運転支援システムに提供することは、安全で円滑な着桟を行う上で役立つ。通常、桟橋は水面より少し上の位置にあるため、桟橋検出部１７は、ライダ３による水面より少し上の計測データを対象として桟橋検出を実施することで、桟橋までの距離、速度、角度などを算出する。
次に、桟橋検出部１７による桟橋の検出において太陽光の影響を排除する方法について説明する。船舶が桟橋に着桟する際、船舶から桟橋までの距離、速度、角度などを逐次計測し、それを操船者又は運転支援システムに提供することは、安全で円滑な着桟を行う上で役立つ。通常、桟橋は水面より少し上の位置にあるため、桟橋検出部１７は、ライダ３による水面より少し上の計測データを対象として桟橋検出を実施することで、桟橋までの距離、速度、角度などを算出する。
【0121】
桟橋は、上面がほぼフラットであり、側面にも大きな凹凸はないため、桟橋検出部１７は、ライダ３による計測データの法線ベクトルの向きや分散を評価することで、対象物が桟橋であることを検知する。対象物が桟橋であると判定できれば、桟橋検出部１７は、再近傍探索などの手法で船舶から桟橋までの距離を算出し、その時間変化に基づいて船舶の速度を算出する。また、桟橋の側面は概略フラットであるため、桟橋検出部１７は、その直線を求めることで、船舶との相対角度を算出する。
【0122】
しかし、上記のように桟橋を検出する際、晴天の日中などでは、ライダ３が太陽光の反射成分を受光してしまうことがある。図１７（Ａ）及び図１７（Ｂ）は、ライダ３に太陽光の反射成分が入射する例を示す。図１７（Ａ）では、太陽光が水面や桟橋上の水たまりなどで反射して、破線で示すようにライダ３に入射している。図１７（Ａ）及び図１７（Ｂ）では、ライダ３からの出射光が計測された点群を黒点７５で示し、太陽光の反射成分がライダ３に入射したことにより計測された点群を白点７６で示している。太陽光が水面や岸壁の上面で反射してライダ３に入射した結果、多数の白点７６が計測されている。
【0123】
ライダ３による計測データに太陽光反射データが含まれていると、桟橋以外の計測データが増えてしまい、法線ベクトルのばらつきが大きくなり、対象物を桟橋と判定できなくなる可能性がある。また、対象物が正しく桟橋と判定されたとしても、太陽光反射データが原因で、最近傍探索により得られる桟橋までの最短距離が、正しい距離からずれてしまう可能性がある。
【0124】
そこで、桟橋検出部１７は、ライダ３の計測データから太陽光反射データを除去し、桟橋の検出を行う。桟橋検出部１７は、基本的に「（４－２）太陽光反射データの除去」の欄で説明した方法と同様の方法を用いる。即ち、桟橋検出部１７は、現在の日時から太陽の方位及び仰角を取得し、ライダ３の計測データに含まれる太陽光反射データを除去する。
【0125】
図１７（Ｃ）は、太陽光反射データを除去して桟橋検出を行う例を示す。桟橋検出部１７は、太陽光の方位及び仰角に基づいて、太陽光がライダ３に入射する範囲を特定する。図１７（Ｃ）の例では、桟橋検出部１７は、水平方向において太陽光の方位に対応する角度αの範囲、かつ、垂直方向において太陽の仰角θ付近の角度の範囲で計測される点群データを太陽光反射データとして除去する。そして、桟橋検出部１７は、太陽光反射データを除去した後の点群データを用いて桟橋面を検出し、桟橋の直線Ｌを作成する。さらに、桟橋検出部１７は、船舶から桟橋までの最短距離、即ち、船舶の自己位置から直線Ｌへの垂線Ｄの長さを船舶から桟橋までの距離と算出する。
【0126】
このように、太陽光の反射成分を除去することにより、正しく桟橋を検出することができる。なお、この手法は、桟橋の他に岸壁など、船舶が着岸しようとする各種の対象物の検出に適用することができる。
【0127】
以上、実施例を参照して本願発明を説明したが、本願発明は上記実施例に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。すなわち、本願発明は、請求の範囲を含む全開示、技術的思想にしたがって当業者であればなし得るであろう各種変形、修正を含むことは勿論である。また、引用した上記の特許文献等の各開示は、本書に引用をもって繰り込むものとする。
【符号の説明】
【0128】
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
