▶ パイオニア株式会社の特許一覧 ▶ パイオニアスマートセンシングイノベーションズ株式会社の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-08-15
(45)【発行日】2024-08-23
(54)【発明の名称】情報処理装置、制御方法、プログラム及び記憶媒体
(51)【国際特許分類】
G01S 17/89 20200101AFI20240816BHJP
G01S 17/93 20200101ALI20240816BHJP
【ＦＩ】
G01S17/89
G01S17/93
【請求項の数】
12
(21)【出願番号】P 2023506395
(86)(22)【出願日】2021-03-15
(86)【国際出願番号】 JP2021010365
(87)【国際公開番号】W WO2022195670
(87)【国際公開日】2022-09-22
【審査請求日】2023-09-11
(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】
前記船舶の自己位置に基づいて船舶着岸支援を行う着岸支援手段を備える請求項１乃至８のいずれか一項に記載の情報処理装置。
【請求項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】ユークリッドクラスタリングのシミュレーション結果を示す。
【図15】物体の距離と点群データの間隔との関係を示す図である。
【図16】グルーピング閾値と点数閾値を固定にした場合と、適応的に設定した場合のシミュレーション結果を示す。
【図17】船舶の周辺で得られた水面反射データを示す。
【図18】水面反射データから引き波と障害物を除去する方法を示す。
【図19】障害物／引き波検出処理のフローチャートである。
【図20】引き波検出処理のフローチャートである。
【図21】直線検出の方法を説明する図である。
【図22】障害物検出処理のフローチャートである。
【図23】水面位置推定処理のフローチャートである。
【図24】引き波情報算出処理のフローチャート及び説明図である。
【図25】障害物情報算出処理のフローチャートである。
【図26】変形例１による水面位置推定方法の説明図である。
【図27】変形例２による引き波検出の例を示す。
【発明を実施するための形態】
【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】
【数1】
【0042】
【数2】
【0043】
次に、ボクセルデータＶＤを用いたＮＤＴスキャンマッチングの概要について説明する。
【0044】
船舶を想定したＮＤＴによるスキャンマッチングは、水平面（ここではｘｙ座標とする）内の移動量及び船舶の向きを要素とした推定パラメータ
Ｐ＝［ｔｘ、ｔｙ、ｔｚ、ｔφ、ｔθ、ｔψ］Ｔ
を推定することとなる。ここで、「ｔｘ」はｘ方向の移動量、「ｔｙ」はｙ方向の移動量、「ｔｚ」はｚ方向の移動量、「ｔφ」はロール角、「ｔθ」はピッチ角、「ｔψ」はヨー角を示す。
Ｐ＝［ｔｘ、ｔｙ、ｔｚ、ｔφ、ｔθ、ｔψ］Ｔ
を推定することとなる。ここで、「ｔｘ」はｘ方向の移動量、「ｔｙ」はｙ方向の移動量、「ｔｚ」はｚ方向の移動量、「ｔφ」はロール角、「ｔθ」はピッチ角、「ｔψ」はヨー角を示す。
【0045】
また、ライダ３が出力する点群データの座標を、
ＸＬ（ｊ）＝［ｘｎ（ｊ）、ｙｎ（ｊ）、ｚｎ（ｊ）］Ｔ
とすると、ＸＬ（ｊ）の平均値「Ｌ´ｎ」は、以下の式（３）により表される。
ＸＬ（ｊ）＝［ｘｎ（ｊ）、ｙｎ（ｊ）、ｚｎ（ｊ）］Ｔ
とすると、ＸＬ（ｊ）の平均値「Ｌ´ｎ」は、以下の式（３）により表される。
【0046】
【数3】
【0047】
そして、自己位置推定部１５は、地図ＤＢ１０と同一の座標系である絶対的な座標系（「ワールド座標系」とも呼ぶ。）に変換した点群データに対応付けられるボクセルデータＶＤを探索し、そのボクセルデータＶＤに含まれる平均ベクトルμｎと共分散行列Ｖｎとを用い、ボクセルｎの評価関数値（「個別評価関数値」とも呼ぶ。）「Ｅｎ」を算出する。この場合、自己位置推定部１５は、以下の式（４）に基づき、ボクセルｎの個別評価関数値Ｅｎを算出する。
【0048】
【数4】
【0049】
そして、自己位置推定部１５は、以下の式（５）により示される、マッチングの対象となる全てのボクセルを対象とした総合的な評価関数値（「スコア値」とも呼ぶ。）「Ｅ（ｋ）」を算出する。スコア値Ｅは、マッチングの適合度を示す指標となる。
【0050】
【数5】
【0051】
【数6】
【0052】
（４）障害物／引き波の検出
次に、障害物／引き波検出部１６による障害物及び引き波の検出について説明する。障害物／引き波検出部１６は、１時刻前までの処理で算出した水面高さを障害物や引き波の検出に使用する。船舶の近くに障害物がある場合、障害物との衝突や接触を避けて航行することが必要となる。障害物とは、例えば、他船、杭、橋脚、ブイ、網、ゴミなどである。また、近くに他船による引き波がある場合、それの影響を受けて揺れが大きくならないように船舶の航行には注意を要する。そこで、障害物／引き波検出部１６は、水面高さを用いて、船舶の近くに障害物や引き波があることを検出する。
次に、障害物／引き波検出部１６による障害物及び引き波の検出について説明する。障害物／引き波検出部１６は、１時刻前までの処理で算出した水面高さを障害物や引き波の検出に使用する。船舶の近くに障害物がある場合、障害物との衝突や接触を避けて航行することが必要となる。障害物とは、例えば、他船、杭、橋脚、ブイ、網、ゴミなどである。また、近くに他船による引き波がある場合、それの影響を受けて揺れが大きくならないように船舶の航行には注意を要する。そこで、障害物／引き波検出部１６は、水面高さを用いて、船舶の近くに障害物や引き波があることを検出する。
【0053】
（４－１）水面位置の推定
図５は、ライダ３から見た水面高さを説明する図である。船舶は、乗船者数や積荷量に応じて喫水位置が変わる。即ち、ライダ３から見た水面までの高さが変わることになる。図５（Ａ）に示すように、船舶の喫水位置が低い場合、ライダ３から見た水面位置は低めとなる。一方、図５（Ｂ）に示すように、船舶の喫水位置が高い場合、ライダ３から見た水面位置は高めとなる。よって、図５（Ｃ）に示すように、水面位置を基準として所定の幅を有する探索範囲を設定することにより、障害物や引き波を正しく検出することが可能となる。
図５は、ライダ３から見た水面高さを説明する図である。船舶は、乗船者数や積荷量に応じて喫水位置が変わる。即ち、ライダ３から見た水面までの高さが変わることになる。図５（Ａ）に示すように、船舶の喫水位置が低い場合、ライダ３から見た水面位置は低めとなる。一方、図５（Ｂ）に示すように、船舶の喫水位置が高い場合、ライダ３から見た水面位置は高めとなる。よって、図５（Ｃ）に示すように、水面位置を基準として所定の幅を有する探索範囲を設定することにより、障害物や引き波を正しく検出することが可能となる。
【0054】
図６は、ライダ３の出射光の水面反射を説明する図である。下方に向いたライダ３の出射光の一部は、水面で反射してライダ３に戻ってくる場合がある。いま、図６（Ａ）に示すように、船舶上のライダ３がレーザ光を出射しているものとする。図６（Ｂ）は、岸壁近くにいる船舶においてライダ３が受光する光を示す。図６（Ｂ）において、ビーム１０１は、水面で反射することなく、直接対象物に照射した光の散乱光の一部がライダ３に戻って受光された光である。ビーム１０２は、ライダ３から出射され、水面で反射して直接ライダ３に戻って受光された光であり、水面反射光の１つ（以下、「直接水面反射光」とも呼ぶ。）である。ビーム１０３は、ライダ３から出射され、水面で反射した光が岸壁等に当たり、その錯乱光の一部が再度水面で反射し、それがライダ３に戻って受光された光であり、水面反射光の１つ（以下、「間接水面反射光」とも呼ぶ。）である。ライダ３は水面で反射した光であることを認識できないので、ビーム１０２を受光した場合、ライダ３はその水面位置に物体があるかのように認識する。また、ビーム１０３を受光した場合、ライダ３は水面より下方に物体があるかのように認識する。よって、ビーム１０３を受光したライダは図示のように岸壁内部の位置を示す正しくない点群データを出力することになる。
【0055】
図７は、水面高さ（以下、「水面位置」とも呼ぶ。）の推定に用いる点群データを説明する図である。図７（Ａ）は船舶を後方から見た図であり、図７（Ｂ）は船舶を上方から見た図である。船舶の周辺では、水面の揺らぎによって、ライダ３からのビームが水面とほぼ垂直となる場合が時々あり、前述のビーム１０２のような直接水面反射光が発生する。一方、船舶が岸に近い場合は、ライダ３からのビームが岸などで反射して前述のビーム１０３のような間接水面反射光が発生する。よって、障害物／引き波検出部１６は、船舶の近くにおける直接水面反射光の点群データを複数個取得し、そのｚ座標値を平均化することにより、水面位置を推定する。船舶は水上に浮いているため、乗船者数や積荷量に応じて水中に沈みこむ量が変わり、ライダ３から水面までの高さが変わる。従って、上記の方法により、常にライダ３から水面までの距離を算出することが可能となる。
【0056】
具体的に、障害物／引き波検出部１６は、ライダ３が出力する点群データから、岸から遠い位置で計測され、かつ、船舶に近い位置で計測された点群データを抽出する。ここで、岸から遠い位置とは、岸から所定距離以上離れた位置をいう。岸の位置は、地図ＤＢ１０に記憶されている着岸場所（岸、桟橋を含む）などを用いることができる。また、岸とは、着岸場所以外の地上位置や構造物であってもよい。岸から遠い位置で計測された点群データを使用することにより、間接水面反射光の点群データを除外することができる。
【0057】
また、船舶に近い位置とは、船舶の自己位置から所定範囲内の位置をいう。船舶に近い位置で計測された点群データを用いることにより、直接水面反射光を計測した点群データ（以下、「直接水面反射データ」とも呼ぶ。）を使用して、水面位置を高精度に推定することが可能となる。
【0058】
（４－２）障害物の検出
次に、障害物の検出方法を説明する。図８は、障害物の検出方法を説明する図である。上記のようにして水面位置が推定された後、障害物／引き波検出部１６は、水面位置付近の高さの点群データに対して、ユークリッドクラスタリング処理を行う。図８（Ａ）に示すように、ユークリッドクラスタリング処理によって、「かたまり」（以下、「クラスタ」とも呼ぶ。）を検出した場合、障害物／引き波検出部１６は、そのクラスタを障害物候補として仮判定する。障害物／引き波検出部１６は、複数の時刻のフレームで同様にクラスタの検出を行い、各時刻で同様のサイズのクラスタを検出した場合、そのクラスタを何らかの障害物と判定する。
次に、障害物の検出方法を説明する。図８は、障害物の検出方法を説明する図である。上記のようにして水面位置が推定された後、障害物／引き波検出部１６は、水面位置付近の高さの点群データに対して、ユークリッドクラスタリング処理を行う。図８（Ａ）に示すように、ユークリッドクラスタリング処理によって、「かたまり」（以下、「クラスタ」とも呼ぶ。）を検出した場合、障害物／引き波検出部１６は、そのクラスタを障害物候補として仮判定する。障害物／引き波検出部１６は、複数の時刻のフレームで同様にクラスタの検出を行い、各時刻で同様のサイズのクラスタを検出した場合、そのクラスタを何らかの障害物と判定する。
【0059】
なお、ブイなどの水上の小さい障害物などを検出する場合、検出の観点からは水面反射成分も価値ある情報となり得る。図８（Ｂ）において、ビーム１１１は、ライダ３から出射され、ブイで反射してライダ３に戻ったものである。一方、ビーム１１２は、ライダ３から出射され、水面で反射してブイに当たり、その錯乱光の一部が再度水面で反射し、ライダ３へ戻って受光されたものである。ブイなどの小さい障害物の場合、ビーム１１１として直接ブイから反射して戻ってくるデータ数は少ないため、ビーム１１２のように水面で反射した成分のデータも含めることで、解析に用いるデータ数を増やし、クラスタリングに活用する。これにより、クラスタリング処理の対象となるデータ数が増えるため、クラスタリングの性能が向上する。
【0060】
障害物／引き波検出部１６は、検出したクラスタを障害物と判定した場合、その障害物の最も高い位置の点のｚ座標から、水面位置を減算し、図８（Ｂ）に示すように障害物の水面から出ている高さＨｏを算出する。
【0061】
（４－３）引き波の検出方法
次に、引き波の検出方法を説明する。図９は、引き波の検出方法を説明する図である。前述のように水面位置が算出された後、障害物／引き波検出部１６は、水面位置付近の高さの点群データに対して、ｚ座標を無視して２次元平面の点群としてハフ変換を行う。図９（Ａ）に示すように、ハフ変換処理によって「直線」を検出した場合、障害物／引き波検出部１６は、その直線を引き波候補として仮判定する。障害物／引き波検出部１６は、複数の時刻のフレームで同様に直線の検出を行い、各時刻で同様の係数を持つ直線を検出した場合、その直線を引き波と判定する。
次に、引き波の検出方法を説明する。図９は、引き波の検出方法を説明する図である。前述のように水面位置が算出された後、障害物／引き波検出部１６は、水面位置付近の高さの点群データに対して、ｚ座標を無視して２次元平面の点群としてハフ変換を行う。図９（Ａ）に示すように、ハフ変換処理によって「直線」を検出した場合、障害物／引き波検出部１６は、その直線を引き波候補として仮判定する。障害物／引き波検出部１６は、複数の時刻のフレームで同様に直線の検出を行い、各時刻で同様の係数を持つ直線を検出した場合、その直線を引き波と判定する。
【0062】
なお、引き波を検出する場合、検出の観点からは水面反射成分も価値ある情報となり得る。図９（Ｂ）において、ビーム１１３は、ライダ３から出射され、引き波で反射してライダ３に戻ったものである。一方、ビーム１１４は、ライダ３から出射され、水面で反射して引き波に当たり、その錯乱光の一部が再度水面で反射し、ライダ３へ戻って受光されたものである。引き波の場合、ビーム１１３として直接引き波から反射して戻ってくるデータ数は少ないため、ビーム１１４のように水面で反射した成分のデータも含めることで、解析に用いるデータ数を増やし、ハフ変換に活用する。これにより、ハフ変換処理の対象となるデータ数が増えるため、ハフ変換の性能が向上する。
【0063】
障害物／引き波検出部１６は、上記のように２次元データを用いて引き波の判定を行った後、引き波の一部と判定された点に対して、あらためてｚ座標を評価する。具体的には、障害物／引き波検出部１６は、水面高さよりもｚ座標値が高い点のみを用いてｚ座標の平均を求め、その平均値から水面位置を減算して引き波の水面からの高さＨｗを算出する。
【0064】
（４－４）障害物／引き波検出部の実施例
次に、障害物／引き波検出部１６の実施例を説明する。以下の実施例では、障害物／引き波検出部１６は、引き波検出→障害物検出→水面位置推定の順で処理を行うことにより、後段の処理を行いやすくする。具体的には、水面位置推定ブロック１３２が推定した水面位置を用いて引き波と障害物の水面からの高さを求めるとともに、次の時刻の点群データに対する探索範囲の設定に使用する。
次に、障害物／引き波検出部１６の実施例を説明する。以下の実施例では、障害物／引き波検出部１６は、引き波検出→障害物検出→水面位置推定の順で処理を行うことにより、後段の処理を行いやすくする。具体的には、水面位置推定ブロック１３２が推定した水面位置を用いて引き波と障害物の水面からの高さを求めるとともに、次の時刻の点群データに対する探索範囲の設定に使用する。
【0065】
図１０は、障害物／引き波検出部１６の機能構成を示すブロック図である。障害物／引き波検出部１６は、ライダ３で計測された点群データを受け取り、引き波情報と障害物情報を出力する。障害物/引き波検出部１６は、探索範囲設定ブロック１２１と、直線抽出ブロック１２２と、引き波検出ブロック１２３と、引き波情報算出ブロック１２４と、引き波データ除去ブロック１２５と、ユークリッドクラスタリングブロック１２６と、障害物検出ブロック１２７と、障害物情報算出ブロック１２８と、障害物データ除去ブロック１２９と、平均・分散計算ブロック１３０と、時間フィルタブロック１３１と、水面位置推定ブロック１３２とを備える。
【0066】
探索範囲設定ブロック１２１は、入力された点群データから、直接水面反射光の点群データを抽出し、障害物及び引き波の高さ方向の探索範囲を設定する。障害物／引き波検出部１６は、図５（Ｃ）に示すように水面位置を中心として設定した探索範囲に属する点群データを抽出し分析することで障害物や引き波を検出するが、船舶の揺れが大きい場合や波が大きい場合、水面に浮遊している障害物や引き波は探索範囲から外れてしまい、検出できなくなる恐れがある。一方、それを回避するために探索範囲を大きくすると、波が小さい場合に無関係なデータが入り込み、検出精度が低下してしまう。
【0067】
そこで、探索範囲設定ブロック１２１は、前述のように船舶の周辺で得られた直接水面反射データのｚ座標値の標準偏差を求め、その標準偏差の値を用いて探索範囲を設定する。具体的には、探索範囲設定ブロック１２１は、直接水面反射データのｚ座標値の標準偏差を用いて波の高さ（波高）を推定し、波高に応じて探索範囲を設定する。直接水面反射データのｚ座標値の標準偏差が小さい場合、図１１（Ａ）に示すように波高は小さいと推測される。この場合、探索範囲設定ブロック１２１は探索範囲を狭くする。例えば、探索範囲設定ブロック１２１は、探索範囲を、直接水面反射データのｚ座標値の平均値付近に設定する。これにより、ノイズの混入を低減できるため、障害物や引き波の検出精度が向上する。
【0068】
一方、直接水面反射のｚ座標値の標準偏差が大きい場合、図１１（Ｂ）に示すように波高が大きいと推測される。よって、探索範囲設定ブロック１２１は探索範囲を広くする。即ち、探索範囲設定ブロック１２１は、直接水面反射データのｚ座標値の平均値を中心に、波高が小さいときよりも広い探索範囲を設定する。
【0069】
一例としては、探索範囲設定ブロック１２１は、図１１（Ｃ）に示すように、直接水面反射データのｚ座標値の標準偏差σを用いて、探索範囲を直接水面反射データのｚ値の平均値を中心として±３σの範囲などと設定してもよい。これにより、波が高い場合でも、探索範囲を広くすることができるため、障害物や引き波の検出漏れを防止することができる。探索範囲設定ブロック１２１は、設定した探索範囲を直線抽出ブロック１２２へ出力する。
【0070】
直線抽出ブロック１２２は、船舶周辺の探索範囲内で計測された直接水面反射データ（以下、「探索データ」とも呼ぶ。）から、ハフ変換を用いて直線を抽出する。直線抽出ブロック１２２は、抽出した直線を引き波検出ブロック１２３へ出力する。ハフ変換で直線を検出する際には離散化した２次元配列を用いるため、結果として求まる直線は近似的なものとなる。従って、直線抽出ブロック１２２と引き波検出ブロック１２３では、以下の手順により、より正確な直線を求める。
（手順１）ハフ変換で近似直線を算出する。
（手順２）近似直線への距離が所定の閾値（直線距離閾値）以内であるデータを抽出する。
（手順３）抽出した複数のデータを用いて主成分分析を行い、直線をあらためて算出し、引き波の直線とする。
（手順１）ハフ変換で近似直線を算出する。
（手順２）近似直線への距離が所定の閾値（直線距離閾値）以内であるデータを抽出する。
（手順３）抽出した複数のデータを用いて主成分分析を行い、直線をあらためて算出し、引き波の直線とする。
【0071】
上記の手順で直線を検出するシミュレーションを行った結果を図１２に示す。図示のように、ハフ変換により求めた直線１４１は近似直線のため、データに対して少しずれがあることがわかる。直線１４１から直線距離閾値以内のデータを抽出し（図１２では□でマーキング）、抽出されたデータを用いて主成分分析であらためて直線を算出することで、正確な引き波直線１４２を得ることができる。
【0072】
引き波検出ブロック１２３は、あらためて算出した直線を引き波と判定し、引き波を示す引き波データを引き波情報算出ブロック１２４及び引き波データ除去ブロック１２５へ出力する。引き波情報算出ブロック１２４は、引き波を示す直線の式と、船舶の自己位置とに基づいて引き波の位置、距離、角度、及び、高さを算出し、引き波情報として出力する。
【0073】
引き波データ除去ブロック１２５は、船舶周辺の探索範囲内で計測された探索データから引き波データを除去し、ユークリッドクラスタリングブロック１２６へ出力する。ユークリッドクラスタリングブロック１２６は、入力された探索データに対してユークリッドクラスタリング処理を行い、探索データのクラスタを検出して障害物検出ブロック１２７へ出力する。
【0074】
ユークリッドクラスタリングでは、まず対象となる全ての点に対し、他の全ての点との間の距離を計算し、得られた点間距離が所定値（以下、「グルーピング閾値」と呼ぶ。）よりも短い点を同じグループに入れる。次に、各グループのうち、所定の点数（以下、「点数閾値」と呼ぶ。）以上の点を含むグループをクラスタと見なす。なお、点数が少ないグループはノイズである可能性が高いため、クラスタとは見なさない。
【0075】
図１３は、ユークリッドクラスタリングの例を示す。図１３（Ａ）は、ユークリッドクラスタリングの対象となる複数の点を示す。図１３（Ａ）に示す各点の点間距離を計算し、グルーピング閾値と比較することによりグルーピングを行った。図１３（Ｂ）の各矢印が示す距離がグルーピング閾値より大きいので、図１３（Ｂ）に破線で示す５つのグループＡ～Ｅが得られた。次に、各グループに属する点数を点数閾値（ここでは「６」とする。）と比較し、図１３（Ｃ）に示すように、点数閾値よりも多数の点を含むグループＡとＣのみが最終的にクラスタと判定された。
【0076】
図１４は、ユークリッドクラスタリングのシミュレーション結果を示す。図１４（Ａ）は、ユークリッドクラスタリングの際に引き波データが残っている場合のシミュレーション結果を示す。ユークリッドクラスタリングにおいて、グルーピング閾値でグループ判別を行う際、引き波データが残っていると、障害物と引き波を一緒のクラスタと判定してしまう恐れがある。図１４（Ａ）の例では、引き波と障害物が近かったために障害物と引き波のデータが同じグループとなり、そのグループの点数が点数閾値よりも高かったため、同じクラスタとして検出されてしまっている。
【0077】
図１４（Ｂ）は、引き波データを除去した後でユークリッドクラスタリングを行った場合のシミュレーション結果を示す。障害物を引き波と区別するため、引き波検出を先に行い、引き波データと判定されたデータを除いた上で、ユークリッドクラスタリングを行っている。この場合、引き波データの影響を受けることなく、障害物が正しくクラスタとして検出されている。
【0078】
一般的に、ライダの光ビームは放射状に出力されるため、データは遠くなるほど位置の間隔が広がる。よって、図１５に示すように、距離の遠いデータほど、隣接するデータとの間隔が離れることになる。また、同じ大きさの物体であっても、近くに存在する場合は検出点数が多くなり、遠くに存在する場合は検出点数が少なくなる。従って、ユークリッドクラスタリング処理において、データの距離値に応じてグルーピング閾値と点数閾値を設定することで、ライダから近い物体でも遠い物体でもできるだけ同様の条件でクラスタリング判定を行うことができる。
【0079】
図１６は、データの距離が遠いほどグルーピング閾値を大きくし、グループの重心点までの距離が遠いほど点数閾値を小さくしてシミュレーションを行った結果を示す。
【0080】
【0081】
図１６（Ｂ）は、
グルーピング閾値＝ａ×（データ距離）、
点数閾値＝ｂ／（グループの重心点までの距離）
としたときのシミュレーション結果を示す。なお、このシミュレーションでは、ａ＝０．２、ｂ＝８０としたが、実際にはライダ３の特性により好適な値に設定される。
グルーピング閾値＝ａ×（データ距離）、
点数閾値＝ｂ／（グループの重心点までの距離）
としたときのシミュレーション結果を示す。なお、このシミュレーションでは、ａ＝０．２、ｂ＝８０としたが、実際にはライダ３の特性により好適な値に設定される。
【0082】
図１６（Ａ）と図１６（Ｂ）を比較するとわかるように、図１６（Ｂ）では、船舶の近くに位置するクラスタ１に加えて、船舶の遠くに位置するクラスタ２も検出できている。クラスタ２は、各データ間の距離は３ｍ近辺の値であるが、船舶からデータまでの距離を用いて算出されたグルーピング閾値は約４．５ｍであるため、その閾値よりも距離が近いため同じグループとなり、またデータ点数は４点であるが、グループの重心点までの距離を用いて算出された点数閾値は約３．２であるため、その閾値よりも点数が多かったため，クラスタと判定された。なお、上記の式を用いると、クラスタ１は、船舶からデータまでの距離を用いて算出されたグルーピング閾値は２．５ｍ近辺の値であり、点数閾値は約７．１となるため、図１６（Ａ）の固定値と比べて大きく変わっていないことがわかる。このように適応的な閾値設定により、クラスタの未検出と誤検出を極力防止することができ、障害物検出の性能を向上させることができる。
【0083】
障害物検出ブロック１２７は、ユークリッドクラスタリングにより検出された障害物を示す点群データ（以下、「障害物データ」呼ぶ。）を、障害物情報算出ブロック１２８及び障害物データ除去ブロック１２９へ出力する。障害物情報算出ブロック１２８は、障害物データに基づき、船舶の自己位置を基準とした障害物の位置、距離、角度、サイズ及び高さを算出し、障害物情報として出力する。
【0084】
障害物データ除去ブロック１２９は、船舶周辺の探索範囲内で計測された探索データから障害物データを除去し、平均・分散計算ブロック１３０へ出力する。船舶周辺の直接水面反射データから水面位置を推定する際、引き波や障害物が存在すると、正しく水面位置を推定することができないためである。
【0085】
図１７（Ａ）は、船舶の周辺に引き波や障害物がある場合に得られた直接水面反射データを示す。この場合、水面よりも高い位置のデータや、障害物や引き波による間接水面反射光（図８（Ｂ）のビーム１１２、図９（Ｂ）のビーム１１４など）は、水面位置推定における誤差要因となる。よって、引き波データ除去ブロック１２５及び障害物データ除去ブロック１２９により、図１７（Ｂ）に示すように引き波や障害物を除去した後の探索データを用いて水面位置推定が行われる。具体的には、図１８に示すように、船舶の周辺に引き波と障害物がある状態１から、状態２に示すように引き波を検出して除去し、状態３とする。次に、状態４に示すように障害物を検出して除去し、状態５に示すように、引き波や障害物を含まない直接水面反射データを得る。
【0086】
具体的に、平均・分散計算ブロック１３０は、船舶周辺で得られた直接水面反射データのｚ座標値の平均値及び分散値を算出し、時間フィルタブロック１３１へ出力する。時間フィルタブロック１３１は、入力された直接水面反射データのｚ座標値の平均値を、過去の水面位置と平均化処理又はフィルタリング処理する。水面位置推定ブロック１３２は、平均化処理又はフィルタリング処理後のｚ座標値の平均値と、探索データのｚ座標値の分散値とを用いて、水面位置を推定する。
【0087】
水面位置を推定する際、船舶周辺の直接水面反射データの分散値が大きい場合、他船の通過などによって波が高くなっている、あるいは障害物として検出されなかった浮遊物等が存在すると予想することができる。よって、分散値が所定値より小さい場合、水面位置推定ブロック１３２は、直接水面反射データの平均値を用いて水面位置を推定、更新する。一方、分散値が所定値以上である場合、水面位置推定ブロック１３２は、水面位置を更新せず、前回の値を維持する。ここで、「所定値」は、固定値でもよく、過去の分散値の平均値を基に設定した値、例えば分散値の平均値の２倍などとしてもよい。そして、水面位置推定ブロック１３２は、推定した水面位置を探索範囲設定ブロック１２１、引き波情報算出ブロック１２４及び障害物情報算出ブロック１２８へ出力する。こうして、新たに得られた直接水面反射データに基づいて水面位置を更新しつつ、引き波及び障害物が検出される。
【0088】
（４－５）障害物／引き波検出処理
次に、障害物／引き波検出部１６により実行される障害物／引き波検出処理について説明する。図１９は、障害物／引き波検出処理のフローチャートである。この処理は、図２に示すコントローラが予め用意されたプログラムを実行し、図１０に示す各要素として動作することにより実現される。
次に、障害物／引き波検出部１６により実行される障害物／引き波検出処理について説明する。図１９は、障害物／引き波検出処理のフローチャートである。この処理は、図２に示すコントローラが予め用意されたプログラムを実行し、図１０に示す各要素として動作することにより実現される。
【0089】
まず、障害物／引き波検出部１６は、ライダ３により計測された点群データを取得する（ステップＳ１１）。次に、探索範囲設定ブロック１２１は、１時刻前までの推定水面位置と、船舶の周辺で得られた直接水面反射データのｚ座標値の標準偏差σとから、探索範囲を決定する（ステップＳ１２）。例えば、探索範囲設定ブロック１２１は、標準偏差σのとき、
探索範囲＝推定水面位置±３σ
とする。
そして、探索範囲設定ブロック１２１は、決定した探索範囲内の点群データを抽出して、引き波検出用の探索データとする（ステップＳ１３）。
探索範囲＝推定水面位置±３σ
とする。
そして、探索範囲設定ブロック１２１は、決定した探索範囲内の点群データを抽出して、引き波検出用の探索データとする（ステップＳ１３）。
【0090】
次に、障害物／引き波検出部１６は、引き波検出処理を実行する（ステップＳ１４）。図２０は、引き波検出処理のフローチャートである。まず、直線抽出ブロック１２２は、探索範囲から得られた探索データの各点に対し、一旦ｚ値を無視してｘとｙの２次元のデータとする（ステップＳ１０１）。次に、直線抽出ブロック１２２は、全ての探索点に対し、以下の式（７）を用いてθを０～１８０度の範囲で変化させて、（θ，ρ）を算出する（ステップＳ１０２）。（θ，ρ）を要素とする離散化した２次元配列を作成するため、「θ」と「ρ」は整数で表現する。
【0091】
【0092】
次に、直線抽出ブロック１２２は、各（θ，ρ）の個数を調べて、所定値よりも大きく極大値となるものを抽出する（ステップＳ１０３）。（θ，ρ）をｎ個抽出した場合、（θ１，ρ１）～（θｎ，ρｎ）が得られる。次に、直線抽出ブロック１２２は、抽出した（θ１，ρ１）～（θｎ，ρｎ）を式（７）に代入し、ｎ本の直線Ｌ１～Ｌｎを生成する（ステップＳ１０４）。
【0093】
次に、引き波検出ブロック１２３は、あらためて、全ての探索点に対して、生成したｎ本の直線Ｌ１～Ｌｎまでの距離を算出し、所定の距離以下となるものを引き波データとする（ステップＳ１０５）。次に、引き波検出ブロック１２３は、上記の引き波データについて、ｚ値も含めた３次元のデータを引き波データとする（ステップＳ１０６）。次に、引き波検出ブロック１２３は、抽出した引き波データを用いて、最小二乗法あるいは主成分分析にて、ｎ本の直線の式をあらためて算出する（ステップＳ１０７）。そして、処理は図１９のメインルーチンへ戻る。
【0094】
次に、引き波データ除去ブロック１２５は、探索データから引き波データを除去して、障害物検出用の探索データとする（ステップＳ１５）。
【0095】
次に、障害物／引き波検出部１６は、障害物検出処理を実行する（ステップＳ１６）。図２２は、障害物検出処理のフローチャートである。まず、ユークリッドクラスタリングブロック１２６は、全ての探索データに対し、他の全ての探索データとの間の点間距離を算出する（ステップＳ１１１）。もし探索データがｎ個の場合は、ｎ（ｎ－１）個の点間距離が算出されることになる。次に、ユークリッドクラスタリングブロック１２６は、最初の対象データを選択し（ステップＳ１１２）、船舶から対象データまでの距離ｒ１を算出し、所定の係数ａを用いてグルーピング閾値Ｔ１を算出する（ステップＳ１１３）。例えば、Ｔ１＝ａ・ｒ１とする。すなわち、グルーピング閾値Ｔ１は対象データごとに異なる値となる。
【0096】
次に、ユークリッドクラスタリングブロック１２６は、対象データとの点間距離が、グルーピング閾値Ｔ１より小さいデータを同じグループとする（ステップＳ１１４）。次に、ユークリッドクラスタリングブロック１２６は、全ての探索データを対象にしたか否かを判定する（ステップＳ１１５）。全ての探索データを対象にしていない場合（ステップＳ１１５：Ｎｏ）、ユークリッドクラスタリングブロック１２６は、次の対象データを選択し（ステップＳ１１６）、ステップＳ１１３へ戻る。
【0097】
一方、全ての探索データを対象とした場合（ステップＳ１１５：Ｙｅｓ）、ユークリッドクラスタリングブロック１２６は、抽出したグループに対してそれぞれ重心位置を求め、その重心位置までの距離ｒ２を算出する。そして、所定の係数ｂを用いて、点数閾値Ｔ２を設定する（ステップＳ１１７）。例えば、Ｔ２＝ｂ／ｒ２とする。すなわち、点数閾値Ｔ２はグループごとに異なる値となる。
【0098】
次に、ユークリッドクラスタリングブロック１２６は、グループごとに、点数閾値Ｔ２以上のデータ数が含まれるものをクラスタと判定し、障害物検出ブロック１２７は、そのクラスタを障害物とする（ステップＳ１１８）。そして、処理は図１９のメインルーチンへ戻る。
【0099】
次に、障害物データ除去ブロック１２９は、探索データから障害物と判定されたデータを除去して、水面位置推定のためのデータとする（ステップＳ１７）。
【0100】
次に、障害物／引き波検出部１６は、水面位置推定処理を実行する（ステップＳ１８）。図２３は、水面位置推定処理のフローチャートである。まず、平均・分散計算ブロック１３０は、岸から遠く、船舶位置に近く、水面位置付近に存在するデータを水面反射データと判定する（ステップＳ１２１）。次に、平均・分散計算ブロック１３０は、複数スキャンフレームの水面反射データを求め、所定のデータ数に達したら、それらのｚ方向の平均値と分散値を求める（ステップＳ１２２）。
【0101】
次に、平均・分散計算ブロック１３０は、分散値が所定値未満であるか否かを判定する（ステップＳ１２３）。分散値が所定値未満でない場合（ステップＳ１２３：Ｎｏ）、処理はステップＳ１２５へ進む。一方、分散値が所定値未満である場合（ステップＳ１２３：Ｙｅｓ）、時間フィルタブロック１３１は、求めたｚ値の平均値と、過去の推定水面位置とのフィルタリング処理を行い、水面位置を更新する（ステップＳ１２４）。次に、水面位置推定ブロック１３２は、算出した水面位置と分散値を出力する（ステップＳ１２５）。そして、処理は図１９のメインルーチンへ戻る。
【0102】
次に、障害物／引き波検出部１６は、引き波情報算出処理を実行する（ステップＳ１９）。図２４（Ａ）は、引き波情報算出処理のフローチャートである。まず、引き波情報算出ブロック１２４は、船舶の自己位置を基準とし、引き波検出ブロック１２３が検出した直線までの最短距離を計算し、引き波までの距離とする。また、その距離となる位置を求め、引き波の位置とする。また、その直線の係数から傾きを算出し、引き波の角度とする（ステップＳ１３１）。
【0103】
なお、図２４（Ｂ）に示すように、船舶の自己位置から直線までの最短距離は、直線に下した垂線の足までの距離となる。しかし、引き波として検出された直線は線分であるため、図２４（Ｃ）に示すように、引き波として検出されたデータの端点が最短距離となる場合もある。従って、引き波情報算出ブロック１２４は、垂線の足までの座標が線分内にあるかどうかを確認し、ない場合は線分の端点までの距離を最短距離とする。
【0104】
次に、引き波情報算出ブロック１２４は、推定した水面位置よりもｚ値が高い点のみを用いてｚ座標値の平均を求め、推定した水面位置を用いて、引き波の水面からの高さを算出する（ステップＳ１３２）。なお、ｚ座標値の平均値の代わりに、最大値を用いて引き波の高さとしてもよい。そして、処理は図１９のメインルーチンへ戻る。
【0105】
次に、障害物／引き波検出部１６は、障害物情報算出処理を実行する（ステップＳ２０）。図２５は、障害物情報算出処理のフローチャートである。まず、障害物情報算出ブロック１２８は、船舶の自己位置を基準とし、障害物として検出されたクラスタデータの中で最短距離となるものを抽出し、それを障害物の位置とする。また、そのデータまでの距離を求め、障害物までの距離とする。さらに、そのデータの座標から障害物の角度を求める（ステップＳ１４１）。
【0106】
次に、障害物情報算出ブロック１２８は、クラスタデータの中で、ｘとｙの２次元平面において最も距離の離れている２点を抽出し、その距離を障害物の横方向のサイズとする。また、クラスタデータの中で、最も高い位置の点のｚ座標から推定した水面位置を減算し、障害物の水面からの高さとする（ステップＳ１４２）。そして、処理は図１９のメインルーチンへ戻る。
【0107】
次に、障害物／引き波検出部１６は、複数のフレームで同様の引き波を検出したか否かを判定する（ステップＳ２１）。船舶自体や引き波が移動する場合は厳密に一致することはないものの、ステップＳ１９で算出する値が少しの違いしかなければ、同様の引き波と判定する。同様の引き波を検出しない場合（ステップＳ２１：Ｎｏ）、処理はステップＳ２３へ進む。一方、同様の引き波を検出した場合（ステップＳ２１：Ｙｅｓ）、引き波情報算出ブロック１２４は、そのデータを引き波と断定し、引き波情報を船体システムに出力する（ステップＳ２２）。
【0108】
次に、障害物／引き波検出部１６は、複数のフレームで同様の障害物を検出したか否かを判定する（ステップＳ２３）。船舶自体や障害物が移動する場合は厳密に一致することはないものの、ステップＳ２０で算出する値が少しの違いしかなければ、同様の障害物と判定する。同様の障害物を検出しない場合（ステップＳ２３：Ｎｏ）、処理は終了する。一方、同様の障害物を検出した場合（ステップＳ２３：Ｙｅｓ）、障害物情報算出ブロック１２８は、そのデータを障害物と断定し、障害物情報を船体システムに出力する（ステップＳ２４）。そして、処理は終了する。
【0109】
（４－６）変形例
（変形例１）
上記の水面位置推定では水面反射データの分散値を利用しているが、図２６に例示するように、積み荷の偏り等で船体がロール方向に静的に傾いていると、水面反射データの分散値も大きくなってしまう。このような状況で水面位置を推定する際には、水面位置推定ブロック１３２は、右舷側の水面反射データと、左舷側の水面反射データをそれぞれ別々に処理し、右舷側の水面位置と左舷側の水面位置とを別々に求めてもよい。もしくは、水面位置推定ブロック１３２は、水面反射データの右舷側の平均値と左舷側の平均値との差分が小さくなるように、ロール角を回転させる座標変換を水面反射データに対して施すことにより、右舷と左舷とを分けずに水面位置を推定できるようになる。
（変形例１）
上記の水面位置推定では水面反射データの分散値を利用しているが、図２６に例示するように、積み荷の偏り等で船体がロール方向に静的に傾いていると、水面反射データの分散値も大きくなってしまう。このような状況で水面位置を推定する際には、水面位置推定ブロック１３２は、右舷側の水面反射データと、左舷側の水面反射データをそれぞれ別々に処理し、右舷側の水面位置と左舷側の水面位置とを別々に求めてもよい。もしくは、水面位置推定ブロック１３２は、水面反射データの右舷側の平均値と左舷側の平均値との差分が小さくなるように、ロール角を回転させる座標変換を水面反射データに対して施すことにより、右舷と左舷とを分けずに水面位置を推定できるようになる。
【0110】
（変形例２）
上記の実施例では、直線抽出ブロック１２２は以下の手順１～手順３により引き波の直線を抽出している。
（手順１）ハフ変換で近似直線を算出する。
（手順２）近似直線への距離が所定の閾値（直線距離閾値）以内であるデータを抽出する。
（手順３）抽出した複数のデータを用いて主成分分析を行い、直線をあらためて算出し、引き波の直線とする。
上記の実施例では、直線抽出ブロック１２２は以下の手順１～手順３により引き波の直線を抽出している。
（手順１）ハフ変換で近似直線を算出する。
（手順２）近似直線への距離が所定の閾値（直線距離閾値）以内であるデータを抽出する。
（手順３）抽出した複数のデータを用いて主成分分析を行い、直線をあらためて算出し、引き波の直線とする。
【0111】
これに対し、以下の手順４を追加し、手順４の判定によって手順２と手順３を繰り返し実行してもよい。
（手順４）抽出されるデータが変わり、直線の式が変化した場合は、手順２に戻る。直線の式が変化しなくなったら、それを引き波の直線とする。
（手順４）抽出されるデータが変わり、直線の式が変化した場合は、手順２に戻る。直線の式が変化しなくなったら、それを引き波の直線とする。
【0112】
図２７の左側のグラフは、上記の手順４を実施せずに直線を求めた場合の例を示し、右側のグラフは手順４まで実施して、直線生成を収束させた場合の例を示す。手順４を加えることにより、引き波データの抽出漏れがなくなり、結果として直線の精度を向上させることができる。
【0113】
以上、実施例を参照して本願発明を説明したが、本願発明は上記実施例に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。すなわち、本願発明は、請求の範囲を含む全開示、技術的思想にしたがって当業者であればなし得るであろう各種変形、修正を含むことは勿論である。また、引用した上記の特許文献等の各開示は、本書に引用をもって繰り込むものとする。
【符号の説明】
【0114】
１ 情報処理装置
２ センサ群
３ ライダ
４ 速度センサ
５ ＧＰＳ受信機
６ ＩＭＵ
１０ 地図ＤＢ
１３ コントローラ
１５ 自己位置推定部
１６ 障害物／引き波検出部
１２１ 探索範囲設定ブロック
１２２ 直線抽出ブロック
１２３ 引き波検出ブロック
１２４ 引き波情報算出ブロック
１２５ 引き波データ除去ブロック
１２６ ユークリッドクラスタリングブロック
１２７ 障害物検出ブロック
１２８ 障害物情報算出ブロック
１２９ 障害物データ除去ブロック
１３０ 平均・分散計算ブロック
１３１ 時間フィルタブロック
１３２ 水面位置推定ブロック
２ センサ群
３ ライダ
４ 速度センサ
５ ＧＰＳ受信機
６ ＩＭＵ
１０ 地図ＤＢ
１３ コントローラ
１５ 自己位置推定部
１６ 障害物／引き波検出部
１２１ 探索範囲設定ブロック
１２２ 直線抽出ブロック
１２３ 引き波検出ブロック
１２４ 引き波情報算出ブロック
１２５ 引き波データ除去ブロック
１２６ ユークリッドクラスタリングブロック
１２７ 障害物検出ブロック
１２８ 障害物情報算出ブロック
１２９ 障害物データ除去ブロック
１３０ 平均・分散計算ブロック
１３１ 時間フィルタブロック
１３２ 水面位置推定ブロック
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図21】
【図22】
【図23】
【図24】
【図25】
【図26】
【図27】