▶ パイオニア株式会社の特許一覧 ▶ パイオニアスマートセンシングイノベーションズ株式会社の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-10-08
(45)【発行日】2024-10-17
(54)【発明の名称】情報処理装置、制御方法、プログラム及び記憶媒体
(51)【国際特許分類】
G08G 3/02 20060101AFI20241009BHJP
【ＦＩ】
G08G3/02 A
【請求項の数】
9
(21)【出願番号】P 2023506399
(86)(22)【出願日】2021-03-15
(86)【国際出願番号】 JP2021010371
(87)【国際公開番号】W WO2022195675
(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】
請求項８に記載のプログラムを記憶した記憶媒体。
【発明の詳細な説明】
【技術分野】
【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】引き波情報の画面表示処理のフローチャートである。
【図28】強調パラメータを説明する図である。
【図29】障害物情報の画面表示処理のフローチャートである。
【図30】変形例１による水面位置推定方法の説明図である。
【図31】変形例２による引き波検出の例を示す。
【発明を実施するための形態】
【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】
次に、障害物／引き波検出部１６は、障害物情報算出処理を実行する（ステップＳ２０）。図２６は、障害物情報算出処理のフローチャートである。まず、障害物情報算出ブロック１２８は、船舶の自己位置を基準とし、障害物として検出されたクラスタデータの中で最短距離となるものを抽出し、それを障害物の位置とする。また、そのデータまでの距離を求め、障害物までの距離とする。さらに、そのデータの座標から障害物の角度を求める（ステップＳ１４１）。
【0115】
次に、障害物情報算出ブロック１２８は、クラスタデータの中で、ｘとｙの２次元平面において最も距離の離れている２点を抽出し、その距離を障害物の横方向のサイズとする。また、クラスタデータの中で、最も高い位置の点のｚ座標から推定した水面位置を減算し、障害物の水面からの高さとする（ステップＳ１４２）。そして、処理は図２０のメインルーチンへ戻る。
【0116】
次に、障害物／引き波検出部１６は、複数のフレームで同様の引き波を検出したか否かを判定する（ステップＳ２１）。船舶自体や引き波が移動する場合は厳密に一致することはないものの、ステップＳ１９で算出する値が少しの違いしかなければ、同様の引き波と判定する。同様の引き波を検出しない場合（ステップＳ２１：Ｎｏ）、処理はステップＳ２４へ進む。一方、同様の引き波を検出した場合（ステップＳ２１：Ｙｅｓ）、引き波情報算出ブロック１２４は、そのデータを引き波と断定し、引き波情報を船体システムに出力する（ステップＳ２２）。
【0117】
次に、表示制御部１３３は、引き波情報の画面表示処理を行う（ステップＳ２３）。図２７は、引き波情報の画面表示処理のフローチャートである。表示制御部１３３は、この処理を、引き波情報を取得する度に行う。
【0118】
まず、表示制御部１３３は、引き波情報算出ブロック１２４から引き波情報を取得し、位置ｐ、距離ｄ、角度θ、高さｈを取得する。また、表示制御部１３３は、前回取得した引き波の位置との差分を求め、相対速度ｖとそのベクトルを算出する（ステップＳ１５１）。
【0119】
次に、表示制御部１３３は、速度ベクトルが自船の方向であるか否かを判定する（ステップＳ１５２）。速度ベクトルが自船の方向でない場合（ステップＳ１５２：Ｎｏ）、表示制御部１３３は、すべてのフォントサイズと直線や枠線の線幅を通常サイズｓｍｉｎとし、位置関係情報を表示装置１７の表示画面に表示する（ステップＳ１５６）。そして、引き波情報の画面表示処理は終了する
【0120】
一方、速度ベクトルが自船の方向である場合（ステップＳ１５２：Ｙｅｓ）、表示制御部１３３は、位置関係情報の各値に対して、強調パラメータｓ１～ｓ４及びＳが大きくなるようにする。具体的には、相対速度ｖが大きいほどｓ１を大きくし、距離ｄが小さいほどｓ２を大きくし、高さｈが大きいほどｓ３を大きくし、角度θ’（＝｜θ－４５°｜）が大きいほどｓ４を大きくする。また、Ｓ＝ｓ１＋ｓ２＋ｓ３＋ｓ４とする（ステップＳ１５３）。
【0121】
図２８（Ａ）は、強調パラメータｓを説明する図である。強調パラメータｓは、予め設定した下限値（通常サイズ）と上限値（最大サイズ）の範囲内で、横軸の変数（ｖ，ｄ，ｈ，θ’）の値に応じて算出される。なお、ａとｂは、変数ｖ，ｄ，ｈ，θ’についてそれぞれ設定される。
【0122】
次に、表示制御部１３３は、強調パラメータｓ１～ｓ４の値をフォントサイズとして、変数ｖ，ｄ，ｈ，θ’の数値を表示画面に表示する。また、表示制御部１３３は、強調パラメータｓ１を線幅として、相対速度ｖの矢印８４を画面に描画する。このとき、矢印８４の長さは相対速度ｖの値に応じたものとする。また、表示制御部１３３は、強調パラメータｓ２を線幅として、自船と引き波の位置までの直線８５を描画する。また、表示制御部１３３は、強調パラメータＳを線幅として、引き波データを囲んだ枠８６を描画する（ステップＳ１５４）。
【0123】
次に、表示制御部１３３は、強調パラメータｓ１～ｓ４の値がそれぞれ所定の閾値を超えた場合、そのフォントや直線や枠線をさらに点滅表示する（ステップＳ１５５）。そして、引き波情報の画面表示処理は終了し、図２０のメインルーチンへ戻る。
【0124】
次に、障害物／引き波検出部１６は、複数のフレームで同様の障害物を検出したか否かを判定する（ステップＳ２４）。船舶自体や障害物が移動する場合は厳密に一致することはないものの、ステップＳ２０で算出する値が少しの違いしかなければ、同様の障害物と判定する。同様の障害物を検出しない場合（ステップＳ２４：Ｎｏ）、処理は終了する。一方、同様の障害物を検出した場合（ステップＳ２４：Ｙｅｓ）、障害物情報算出ブロック１２８は、そのデータを障害物と断定し、障害物情報を船体システムに出力する（ステップＳ２５）。
【0125】
次に、表示制御部１３３は、障害物情報の画面表示処理を行う（ステップＳ２６）。図２９は、障害物情報の画面表示処理のフローチャートである。表示制御部１３３は、この処理を、障害物情報を取得する度に行う。
【0126】
まず、表示制御部１３３は、障害物情報算出ブロック１２８から障害物情報を取得し、位置ｐ、距離ｄ、サイズｗ、高さｈを取得する。また、表示制御部１３３は、前回取得した障害物の位置との差分を求め、相対速度ｖとそのベクトルを算出する（ステップＳ１６１）。
【0127】
次に、表示制御部１３３は、速度ベクトルが自船の方向であるか否かを判定する（ステップＳ１６２）。速度ベクトルが自船の方向でない場合（ステップＳ１６２：Ｎｏ）、表示制御部１３３は、すべてのフォントサイズと直線や枠線の線幅を通常サイズｓｍｉｎとし、位置関係情報を表示画面に表示する（ステップＳ１６６）。そして、障害物情報の画面表示処理は終了する
【0128】
一方、速度ベクトルが自船の方向である場合（ステップＳ１６２：Ｙｅｓ）、表示制御部１３３は、位置関係情報の各値に対して、強調パラメータｓ１～ｓ４及びＳが大きくなるようにする。具体的には、相対速度ｖが大きいほどｓ１を大きくし、距離ｄが小さいほどｓ２を大きくし、高さｈが大きいほどｓ３を大きくし、サイズｗが大きいほどｓ４を大きくする。また、Ｓ＝ｓ１＋ｓ２＋ｓ３＋ｓ４とする（ステップＳ１６３）。
【0129】
図２８（Ｂ）は、強調パラメータｓを説明する図である。強調パラメータｓは、予め設定した下限値（通常サイズ）と上限値（最大サイズ）の範囲内で、横軸の変数（ｖ，ｄ，ｈ，ｗ）の値に応じて算出される。なお、ａとｂは、変数ｖ，ｄ，ｈ，ｗについてそれぞれ設定される。
【0130】
次に、表示制御部１３３は、強調パラメータｓ１～ｓ４の値をフォントサイズとして、変数ｖ，ｄ，ｈ，ｗの数値を表示画面に表示する。また、表示制御部１３３は、強調パラメータｓ１を線幅として、相対速度ｖの矢印８４を画面に描画する。このとき、矢印８４の長さは相対速度ｖの値に応じたものとする。また、表示制御部１３３は、強調パラメータｓ２を線幅として、自船と障害物の位置までの直線８５を描画する。また、表示制御部１３３は、強調パラメータＳを線幅として、障害物データを囲んだ枠８２を描画する（ステップＳ１６４）。
【0131】
次に、表示制御部１３３は、強調パラメータｓ１～ｓ４の値がそれぞれ所定の閾値を超えた場合、そのフォントや直線や枠線をさらに点滅表示する（ステップＳ１６５）。そして、障害物情報の画面表示処理は終了し、図２０の障害物／引き波検出処理も終了する。
【0132】
（４－６）変形例
（変形例１）
上記の水面位置推定では水面反射データの分散値を利用しているが、図３０に例示するように、積み荷の偏り等で船体がロール方向に静的に傾いていると、水面反射データの分散値も大きくなってしまう。このような状況で水面位置を推定する際には、水面位置推定ブロック１３２は、右舷側の水面反射データと、左舷側の水面反射データをそれぞれ別々に処理し、右舷側の水面位置と左舷側の水面位置とを別々に求めてもよい。もしくは、水面位置推定ブロック１３２は、水面反射データの右舷側の平均値と左舷側の平均値との差分が小さくなるように、ロール角を回転させる座標変換を水面反射データに対して施すことにより、右舷と左舷とを分けずに水面位置を推定できるようになる。
（変形例１）
上記の水面位置推定では水面反射データの分散値を利用しているが、図３０に例示するように、積み荷の偏り等で船体がロール方向に静的に傾いていると、水面反射データの分散値も大きくなってしまう。このような状況で水面位置を推定する際には、水面位置推定ブロック１３２は、右舷側の水面反射データと、左舷側の水面反射データをそれぞれ別々に処理し、右舷側の水面位置と左舷側の水面位置とを別々に求めてもよい。もしくは、水面位置推定ブロック１３２は、水面反射データの右舷側の平均値と左舷側の平均値との差分が小さくなるように、ロール角を回転させる座標変換を水面反射データに対して施すことにより、右舷と左舷とを分けずに水面位置を推定できるようになる。
【0133】
（変形例２）
上記の実施例では、直線抽出ブロック１２２は以下の手順１～手順３により引き波の直線を抽出している。
（手順１）ハフ変換で近似直線を算出する。
（手順２）近似直線への距離が所定の閾値（直線距離閾値）以内であるデータを抽出する。
（手順３）抽出した複数のデータを用いて主成分分析を行い、直線をあらためて算出し、引き波の直線とする。
上記の実施例では、直線抽出ブロック１２２は以下の手順１～手順３により引き波の直線を抽出している。
（手順１）ハフ変換で近似直線を算出する。
（手順２）近似直線への距離が所定の閾値（直線距離閾値）以内であるデータを抽出する。
（手順３）抽出した複数のデータを用いて主成分分析を行い、直線をあらためて算出し、引き波の直線とする。
【0134】
これに対し、以下の手順４を追加し、手順４の判定によって手順２と手順３を繰り返し実行してもよい。
（手順４）抽出されるデータが変わり、直線の式が変化した場合は、手順２に戻る。直線の式が変化しなくなったら、それを引き波の直線とする。
（手順４）抽出されるデータが変わり、直線の式が変化した場合は、手順２に戻る。直線の式が変化しなくなったら、それを引き波の直線とする。
【0135】
図３１の左側のグラフは、上記の手順４を実施せずに直線を求めた場合の例を示し、右側のグラフは手順４まで実施して、直線生成を収束させた場合の例を示す。手順４を加えることにより、引き波データの抽出漏れがなくなり、結果として直線の精度を向上させることができる。
【0136】
以上、実施例を参照して本願発明を説明したが、本願発明は上記実施例に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。すなわち、本願発明は、請求の範囲を含む全開示、技術的思想にしたがって当業者であればなし得るであろう各種変形、修正を含むことは勿論である。また、引用した上記の特許文献等の各開示は、本書に引用をもって繰り込むものとする。
【符号の説明】
【0137】
１ 情報処理装置
２ センサ群
３ ライダ
４ 速度センサ
５ ＧＰＳ受信機
６ ＩＭＵ
１０ 地図ＤＢ
１３ コントローラ
１５ 自己位置推定部
１６ 障害物／引き波検出部
１２１ 探索範囲設定ブロック
１２２ 直線抽出ブロック
１２３ 引き波検出ブロック
１２４ 引き波情報算出ブロック
１２５ 引き波データ除去ブロック
１２６ ユークリッドクラスタリングブロック
１２７ 障害物検出ブロック
１２８ 障害物情報算出ブロック
１２９ 障害物データ除去ブロック
１３０ 平均・分散計算ブロック
１３１ 時間フィルタブロック
１３２ 水面位置推定ブロック
１３３ 表示制御部
２ センサ群
３ ライダ
４ 速度センサ
５ ＧＰＳ受信機
６ ＩＭＵ
１０ 地図ＤＢ
１３ コントローラ
１５ 自己位置推定部
１６ 障害物／引き波検出部
１２１ 探索範囲設定ブロック
１２２ 直線抽出ブロック
１２３ 引き波検出ブロック
１２４ 引き波情報算出ブロック
１２５ 引き波データ除去ブロック
１２６ ユークリッドクラスタリングブロック
１２７ 障害物検出ブロック
１２８ 障害物情報算出ブロック
１２９ 障害物データ除去ブロック
１３０ 平均・分散計算ブロック
１３１ 時間フィルタブロック
１３２ 水面位置推定ブロック
１３３ 表示制御部
【図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】
【図28】
【図29】
【図30】
【図31】