JP7724302B2 - 情報処理装置、制御方法、プログラム及び記憶媒体 - Google Patents

Description

本発明は、船舶の自己位置推定に関する。

レーザスキャナなどの計測装置を用いて計測した周辺物体の形状データを、予め周辺物体の形状が記憶された地図情報と照合（マッチング）することで、移動体の自己位置を推定する技術が従来知られている。例えば、特許文献１には、空間を所定の規則で分割したボクセル中における検出物が静止物か移動物かを判定し、静止物が存在するボクセルを対象として地図情報と計測データとのマッチングを行う自律移動システムが開示されている。また、特許文献２には、ボクセル毎の静止物体の平均ベクトルと共分散行列とを含むボクセルデータとライダが出力する点群データとの照合により自己位置推定を行うスキャンマッチング手法が開示されている。

国際公開ＷＯ２０１３／０７６８２９ 国際公開ＷＯ２０１８／２２１４５３

昨今、自動車分野だけでなく、船舶においても、自動操船システム検討が進められており、安全な自動操船には精度の良い自己位置推定は同様に重要である。外洋では周辺に構造物が少ないためＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）により自己位置を測位可能であるが、都市部の沿岸や河川では高層ビル等が隣接するため、受信衛星数の低下やマルチパスなどによりＧＮＳＳ電波の受信環境は悪く、精度の良い測位はできないことが多い。そのため、船舶においても、上記のスキャンマッチングによる高精度な自己位置推定を行うことができるように検討が進められている。

しかし、船舶においては、潮流及び波の影響が推定位置の誤差となって現れることに起因し、自己位置推定の精度が低下してしまう、という課題が生じている。

本発明は、上記の課題を解決するためになされたものであり、船舶における自己位置推定の精度を向上させることが可能な情報処理装置を提供することを主な目的とする。

請求項に記載の発明は、情報処理装置であって、船舶の予測位置を取得する予測位置取得手段と、前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出する推定位置算出手段と、前記照合の信頼度を算出する信頼度算出手段と、第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する速度算出手段と、を有し、前記データは、前記外界センサが出力した点群データである第１点群データに対してダウンサンプリングが行われた点群データである第２点群データであり、前記信頼度算出手段は、少なくとも前記ダウンサンプリングのサイズ又は前記第１点群データの計測点数に基づき、前記信頼度を算出する。
また、請求項に記載の発明は、情報処理装置であって、船舶の予測位置を取得する予測位置取得手段と、前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出する推定位置算出手段と、少なくとも前記データが前記地図データに対応付けされる比率に基づき、前記照合の信頼度を算出する信頼度算出手段と、第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する速度算出手段と、を有する。

また、請求項に記載の発明は、コンピュータが実行する制御方法であって、船舶の予測位置を取得し、前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出し、前記照合の信頼度を算出し、第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を前記コンピュータが算出し、前記データは、前記外界センサが出力した点群データである第１点群データに対してダウンサンプリングが行われた点群データである第２点群データであり、少なくとも前記ダウンサンプリングのサイズ又は前記第１点群データの計測点数に基づき、前記信頼度を前記コンピュータが算出する。
また、請求項に記載の発明は、コンピュータが実行する制御方法であって、船舶の予測位置を取得し、前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出し、少なくとも前記データが前記地図データに対応付けされる比率に基づき、前記照合の信頼度を算出し、第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する。

また、請求項に記載の発明は、プログラムであって、船舶の予測位置を取得し、前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出し、前記照合の信頼度を算出し、第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する処理をコンピュータに実行させ、前記データは、前記外界センサが出力した点群データである第１点群データに対してダウンサンプリングが行われた点群データである第２点群データであり、少なくとも前記ダウンサンプリングのサイズ又は前記第１点群データの計測点数に基づき、前記信頼度を算出する処理を前記コンピュータに実行させる。
また、請求項に記載の発明は、プログラムであって、船舶の予測位置を取得し、前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出し、少なくとも前記データが前記地図データに対応付けされる比率に基づき、前記照合の信頼度を算出し、第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する。

第１実施例に係る運転支援システムの概略構成を示す図。 自己位置推定に用いられる角速度を説明するための図。 第１実施例に係る情報処理装置のハードウェア構成の一例を示すブロック図。 自己位置推定部が推定すべき自己位置を３次元直交座標で表した図。 ボクセルデータＶＤの概略的なデータ構造の一例を示す図。 自己位置推定部により行われる処理の一例を説明するための図。 信頼度値ＮＲＶと信頼度指標ＮＲＩとの関係を表す図。 信頼度指標ＮＲＩと時定数τとの関係を示す図。 信頼度値ＮＲＶと時定数τとの関係を示す図。 フィルタ無しの状態でワールド座標系における速度を算出した場合の例を示す図。 フィルタの時定数τを固定した状態でワールド座標系における速度を算出した場合の例を示す図。 フィルタ無しの状態でワールド座標系における速度を算出した場合の例を示す図。 フィルタの時定数τを固定した状態でワールド座標系における速度を算出した場合の例を示す図。 フィルタ無しの状態でワールド座標系における速度を算出した場合の例を示す図。 フィルタの時定数τを変化させつつワールド座標系における速度を算出した場合の例を示す図。 フィルタ無しの状態で船舶の自己位置推定を行った場合の例を説明するための図。 フィルタ有りの状態で船舶の自己位置推定を行った場合の例を説明するための図。 フィルタ有りの状態で船舶の自己位置推定を行った場合の例を説明するための図。 第１実施例に係る自己位置推定部の機能ブロックの一例を示す図。 自己位置推定処理の手順の一例を示すフローチャート。 実験時におけるライダの設置状態及びスキャン範囲を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図。 第２実施例に係る運転支援システムの概略構成を示す図。 第２実施例に係る情報処理装置のハードウェア構成の一例を示すブロック図。 第２実施例に係る自己位置推定部の機能ブロックの一例を示す図。 加速度センサにより計測された加速度をワールド座標系の加速度に変換する際に行われる演算の概要を示す図。 第３実施例に係る情報処理装置のハードウェア構成の一例を示すブロック図。 第３実施例に係る自己位置推定部の機能ブロックの一例を示す図。

本発明の１つの好適な実施形態では、情報処理装置は、船舶の予測位置を取得する予測位置取得手段と、前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出する推定位置算出手段と、前記照合の信頼度を算出する信頼度算出手段と、第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する速度算出手段と、を有する。

上記の情報処理装置は、予測位置取得手段と、推定位置算出手段と、信頼度算出手段と、速度算出手段と、を有する。予測位置取得手段は、船舶の予測位置を取得する。推定位置算出手段は、船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、予測位置を更新した船舶の推定位置を算出する。信頼度算出手段は、照合の信頼度を算出する。速度算出手段は、第１処理時刻での前記推定位置と、第１処理時刻の直前の第２処理時刻での推定位置と、少なくとも信頼度に基づき設定した時定数とに基づき、第１処理時刻での船舶の速度を算出する。これにより、船舶における自己位置推定の精度を向上させることができる。

上記の情報処理装置の一態様では、前記時定数は、前記信頼度の大きさに応じて小さくなる値として設定される。

上記の情報処理装置の一態様では、前記時定数は、前記信頼度と、前記船舶の加速度と、に基づいて設定される。

上記の情報処理装置の一態様では、前記予測位置取得手段は、前記第２処理時刻での前記推定位置と、前記第２処理時刻での前記船舶の速度と、に基づき、前記第１処理時刻での前記予測位置を取得する。

上記の情報処理装置の一態様では、前記信頼度算出手段は、少なくとも前記照合の適合度合いを示すスコア値に基づき、前記信頼度を算出する。

上記の情報処理装置の一態様では、前記データは、前記外界センサが出力した点群データである第１点群データに対してダウンサンプリングが行われた点群データである第２点群データであり、前記信頼度算出手段は、少なくとも前記ダウンサンプリングのサイズ又は前記第１点群データの計測点数に基づき、前記信頼度を算出する。

上記の情報処理装置の一態様では、前記信頼度算出手段は、少なくとも前記データが前記地図データに対応付けされる比率に基づき、前記信頼度を算出する。

本発明の他の実施形態では、コンピュータが実行する制御方法は、船舶の予測位置を取得し、前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出し、前記照合の信頼度を算出し、第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する。これにより、船舶における自己位置推定の精度を向上させることができる。

本発明のさらに他の実施形態では、プログラムは、船舶の予測位置を取得し、前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出し、前記照合の信頼度を算出し、第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する処理をコンピュータに実行させる。このプログラムをコンピュータで実行することにより、上記の情報処理装置を実現することができる。このプログラムは記憶媒体に記憶して使用することができる。

以下、図面を参照して本発明の好適な実施例について説明する。なお、任意の記号の上に「・」、「＾」または「－」が付された文字を、本明細書では便宜上、「Ａ^・」、「Ａ＾」または「Ａ^－」（「Ａ」は任意の文字）と表す。

＜第１実施例＞
まず、第１実施例について説明する。

［運転支援システムの概要］
図１Ａは、第１実施例に係る運転支援システムの概略構成を示す図である。本実施例に係る運転支援システムは、移動体である船舶と共に移動する情報処理装置１と、当該船舶に搭載されたセンサ群２と、を有する。以後では、情報処理装置１と共に移動する船舶を「対象船舶」とも呼ぶ。

情報処理装置１は、センサ群２と電気的に接続し、センサ群２に含まれる各種センサの出力に基づき、情報処理装置１が設けられた対象船舶の位置（「自己位置」とも呼ぶ。）の推定を行う。そして、情報処理装置１は、自己位置の推定結果に基づき、対象船舶の自動運転制御等の運転支援を行う。運転支援には、自動着岸などの着岸支援も含まれる。ここで、「着岸」とは、岸壁に対象船舶を着ける場合の他、桟橋等の構造体に対象船舶を着ける場合も含まれる。情報処理装置１は、対象船舶に設けられたナビゲーション装置であってもよく、船舶に内蔵された電子制御装置であってもよい。

また、情報処理装置１は、ボクセルデータ「ＶＤ」を含む地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶する。ボクセルデータＶＤは、３次元空間の最小単位となる立方体（正規格子）を示すボクセルごとに静止構造物の位置情報等を記録したデータである。ボクセルデータＶＤは、各ボクセル内の静止構造物の計測された点群データを正規分布により表したデータを含み、後述するように、ＮＤＴ（Ｎｏｒｍａｌ Ｄｉｓｔｒｉｂｕｔｉｏｎｓ Ｔｒａｎｓｆｏｒｍ）を用いたスキャンマッチングに用いられる。情報処理装置１は、ＮＤＴスキャンマッチングにより、例えば、対象船舶の平面上の位置、高さ位置、ヨー角、ピッチ角及びロール角の推定を行う。なお、特に言及がない限り、自己位置は、対象船舶のヨー角などの姿勢角も含まれるものとする。

図１Ｂは、自己位置推定に用いられる角速度を説明するための図である。センサ群２は、対象船舶に設けられた種々の外界センサ及び内界センサを含んでいる。本実施例では、センサ群２は、ライダ（Ｌｉｄａｒ：Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ、または、Ｌａｓｅｒ Ｉｌｌｕｍｉｎａｔｅｄ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）３と、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機５と、３軸方向における対象船舶の角速度を計測する慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）６とを含んでいる。具体的には、ＩＭＵ６は、図１Ｂに示すように、対象船舶の進行方向を軸とした角速度ω_ｘと、当該対象船舶の横方向（左右方向）を軸とした角速度ω_ｙと、該対象船舶の上下方向を軸とした角速度ω_ｚと、を計測する。

ライダ３は、水平方向および垂直方向の所定の角度範囲に対してパルスレーザを出射することで、外界に存在する物体までの距離を離散的に測定し、当該物体の位置を示す３次元の点群データを生成する。この場合、ライダ３は、照射方向を変えながらレーザ光を照射する照射部と、照射したレーザ光の反射光（散乱光）を受光する受光部と、受光部が出力する受光信号に基づくスキャンデータ（点群データを構成する点であり、以後では「計測点」とも呼ぶ。）を出力する出力部と、を有する。計測点は、受光部が受光したレーザ光に対応する照射方向と、上述の受光信号に基づき特定される当該レーザ光の応答遅延時間とに基づき生成される。なお、一般的に、対象物までの距離が近いほどライダの距離測定値の精度は高く、距離が遠いほど精度は低い。なお、ライダ３は、上述したスキャン型のライダに限らず、２次元アレイ状のセンサの視野にレーザ光を拡散照射することによって３次元データを生成するフラッシュ型のライダであってもよい。

なお、センサ群２は、ＧＰＳ受信機５に代えて、ＧＰＳ以外のＧＮＳＳの測位結果を生成する受信機を有してもよい。

［情報処理装置の構成］
図２は、第１実施例に係る情報処理装置のハードウェア構成の一例を示すブロック図である。情報処理装置１は、主に、インターフェース１１と、メモリ１２と、コントローラ１３と、を有する。これらの各要素は、バスラインを介して相互に接続されている。

インターフェース１１は、情報処理装置１と外部装置とのデータの授受に関するインターフェース動作を行う。本実施例では、インターフェース１１は、ライダ３、ＧＰＳ受信機５及びＩＭＵ６などのセンサ群２の各センサから出力データを取得し、コントローラ１３へ供給する。また、インターフェース１１は、例えば、コントローラ１３が生成した対象船舶の制御に関する信号を、対象船舶の運転を制御する対象船舶の各構成要素に供給する。例えば、対象船舶は、エンジンや電気モータなどの駆動源と、駆動源の駆動力に基づき進行方向の推進力を生成するスクリューと、駆動源の駆動力に基づき横方向の推進力を生成するスラスターと、船舶の進行方向を自在に定めるための機構である舵等とを備える。そして、自動着岸などの自動運転時には、インターフェース１１は、コントローラ１３が生成した制御信号を、これらの各構成要素に供給する。なお、対象船舶に電子制御装置が設けられている場合には、インターフェース１１は、当該電子制御装置に対し、コントローラ１３が生成した制御信号を供給する。インターフェース１１は、無線通信を行うためのネットワークアダプタなどのワイヤレスインターフェースであってもよく、ケーブル等により外部装置と接続するためのハードウェアインターフェースであってもよい。また、インターフェース１１は、入力装置、表示装置、音出力装置等の種々の周辺装置とのインターフェース動作を行ってもよい。

メモリ１２は、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ハードディスクドライブ、フラッシュメモリなどの各種の揮発性メモリ及び不揮発性メモリにより構成される。メモリ１２は、コントローラ１３が所定の処理を実行するためのプログラムが記憶される。なお、コントローラ１３が実行するプログラムは、メモリ１２以外の記憶媒体に記憶されてもよい。

また、メモリ１２は、ボクセルデータＶＤを含む地図ＤＢ１０を記憶する。地図ＤＢ１０には、ボクセルデータＶＤの他、例えば、着岸場所（岸、桟橋を含む）に関する情報、船舶が移動可能な水路に関する情報などが含まれている。なお、地図ＤＢ１０は、インターフェース１１を介して情報処理装置１と接続されたハードディスクなどの情報処理装置１の外部の記憶装置に記憶されてもよい。上記の記憶装置は、情報処理装置１と通信を行うサーバ装置であってもよい。また、上記の記憶装置は、複数の装置から構成されてもよい。また、地図ＤＢ１０は、定期的に更新されてもよい。この場合、例えば、コントローラ１３は、インターフェース１１を介し、地図情報を管理するサーバ装置から、自己位置が属するエリアに関する部分地図情報を受信し、地図ＤＢ１０に反映させる。

また、メモリ１２には、地図ＤＢ１０の他、本実施例において情報処理装置１が実行する処理に必要な情報が記憶される。例えば、メモリ１２には、ライダ３が１周期分の走査を行った場合に得られる点群データに対してダウンサンプリングを行う場合のダウンサンプリングのサイズの設定に用いられる情報が記憶される。

コントローラ１３は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＴＰＵ（Ｔｅｎｓｏｒ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などの１又は複数のプロセッサを含み、情報処理装置１の全体を制御する。この場合、コントローラ１３は、メモリ１２等に記憶されたプログラムを実行することで、自己位置推定及び運転支援等に関する処理を行う。

また、コントローラ１３は、機能的には、自己位置推定部１５を有する。そして、コントローラ１３は、「予測位置取得手段」、「推定位置算出手段」、「信頼度算出手段」、「速度算出手段」及びプログラムを実行するコンピュータ等として機能する。

自己位置推定部１５は、ライダ３の出力に基づく点群データと、当該点群データが属するボクセルに対応するボクセルデータＶＤとに基づき、ＮＤＴに基づくスキャンマッチング（ＮＤＴスキャンマッチング）を行うことで、自己位置の推定を行う。ここで、自己位置推定部１５による処理対象となる点群データは、ライダ３が生成した点群データであってもよく、当該点群データをダウンサンプリング処理した後の点群データであってもよい。

［ＮＤＴスキャンマッチング］
次に、自己位置推定部１５が実行するＮＤＴスキャンマッチングに基づく位置推定に関する説明を行う。

図３は、自己位置推定部１５が推定すべき自己位置を３次元直交座標で表した図である。図３に示すように、ｘｙｚの３次元直交座標上で定義された３次元空間での自己位置は、座標「（ｘ，ｙ，ｚ）」、対象船舶のロール角「φ」、ピッチ角「θ」、ヨー角（方位）「ψ」により表される。ここでは、ロール角φは、対象船舶の進行方向を軸とした回転角、ピッチ角θは、ｘｙ平面に対する対象船舶の進行方向の仰角、ヨー角ψは、対象船舶の進行方向とｘ軸とのなす角として定義されている。座標（ｘ、ｙ、ｚ）は、例えば緯度、経度、標高の組合せに相当する絶対位置、あるいは所定地点を原点とした位置を示すワールド座標である。そして、自己位置推定部１５は、これらのｘ、ｙ、ｚ、φ、θ、ψを推定パラメータとする自己位置推定を行う。

次に、ＮＤＴスキャンマッチングに用いるボクセルデータＶＤについて説明する。ボクセルデータＶＤは、各ボクセル内の静止構造物の計測された点群データを正規分布により表したデータを含む。

図４は、ボクセルデータＶＤの概略的なデータ構造の一例を示す図である。ボクセルデータＶＤは、ボクセル内の点群を正規分布で表現する場合のパラメータの情報を含み、本実施例では、図４に示すように、ボクセルＩＤと、ボクセル座標と、平均ベクトルと、共分散行列とを含む。

「ボクセル座標」は、各ボクセルの中心位置などの基準となる位置の絶対的な３次元座標を示す。なお、各ボクセルは、空間を格子状に分割した立方体であり、予め形状及び大きさが定められているため、ボクセル座標により各ボクセルの空間を特定することが可能である。ボクセル座標は、ボクセルＩＤとして用いられてもよい。

「平均ベクトル」及び「共分散行列」は、対象のボクセル内での点群を正規分布で表現する場合のパラメータに相当する平均ベクトル及び共分散行列を示す。なお、任意のボクセル「ｎ」内の任意の点「ｉ」の座標を
Ｘ_ｎ（ｉ）＝［ｘ_ｎ（ｉ）、ｙ_ｎ（ｉ）、ｚ_ｎ（ｉ）］^Ｔ
と定義し、ボクセルｎ内での点群数を「Ｎ_ｎ」とすると、ボクセルｎでの平均ベクトル「μ_ｎ」及び共分散行列「Ｖ_ｎ」は、それぞれ以下の式（１）及び式（２）により表される。

次に、ボクセルデータＶＤを用いたＮＤＴスキャンマッチングの概要について説明する。

船舶を想定したＮＤＴによるスキャンマッチングは、３次元空間（ここではｘｙｚ座標とする）内の移動量及び船舶の向きを要素とした推定パラメータ
Ｐ＝［ｔ_ｘ、ｔ_ｙ、ｔ_ｚ、ｔ_φ、ｔ_θ、ｔ_ψ］^Ｔ
を推定することとなる。ここで、「ｔ_ｘ」はｘ方向の移動量、「ｔ_ｙ」はｙ方向の移動量、「ｔ_ｚ」はｚ方向の移動量、「ｔ_φ」はロール角、「ｔ_θ」はピッチ角、「ｔ_ψ」はヨー角を示す。

また、ライダ３が出力する点群データの座標を、
Ｘ_Ｌ（ｊ）＝［ｘ_ｎ（ｊ）、ｙ_ｎ（ｊ）、ｚ_ｎ（ｊ）］^Ｔ
とすると、Ｘ_Ｌ（ｊ）の平均値「Ｌ´_ｎ」は、以下の式（３）により表される。このような演算により、点群データがダウンサンプリングされる。

そして、上述の推定パラメータＰを用い、平均値Ｌ´を公知の座標変換処理に基づき座標変換する。以後では、変換後の座標を「Ｌ_ｎ」とする。

そして、自己位置推定部１５は、地図ＤＢ１０と同一の座標系である絶対的な座標系（「ワールド座標系」とも呼ぶ。）に変換した点群データに対応付けられるボクセルデータＶＤを探索し、そのボクセルデータＶＤに含まれる平均ベクトルμ_ｎと共分散行列Ｖ_ｎとを用い、ボクセルｎの評価関数値（「個別評価関数値」とも呼ぶ。）「Ｅ_ｎ」を算出する。この場合、自己位置推定部１５は、以下の式（４）に基づき、ボクセルｎの個別評価関数値Ｅ_ｎを算出する。

そして、自己位置推定部１５は、以下の式（５）により示される、マッチングの対象となる全てのボクセルを対象とした総合的な評価関数値（「スコア値」とも呼ぶ。）「Ｅ（ｋ）」を算出する。スコア値Ｅは、マッチングの適合度を示す指標となる。

その後、自己位置推定部１５は、ニュートン法などの任意の求根アルゴリズムによりスコア値Ｅ（ｋ）が最大となるとなる推定パラメータＰを算出する。そして、自己位置推定部１５は、処理インデックス番号（以降、処理番号と称する）ｋにおいてデッドレコニングにより算出した位置（「ＤＲ位置」とも呼ぶ。）「Ｘ_ＤＲ（ｋ）」に対し、推定パラメータＰを適用することで、ＮＤＴスキャンマッチングに基づく自己位置（「ＮＤＴ位置」とも呼ぶ。）「Ｘ_ＮＤＴ（ｋ）」を算出する。なお、ＤＲ位置及びＮＤＴ位置には、位置及び姿勢が含まれている。ここで、ＤＲ位置Ｘ_ＤＲ（ｋ）は、推定自己位置Ｘ＾（ｋ）の算出前の暫定的な自己位置に相当し、予測自己位置「Ｘ^－（ｋ）」とも表記する。この場合、ＮＤＴ位置Ｘ_ＮＤＴ（ｋ）は、以下の式（６）により表される。

そして、自己位置推定部１５は、ＮＤＴ位置Ｘ_ＮＤＴ（ｋ）を、処理番号ｋでの最終的な自己位置の推定結果（「推定自己位置」とも呼ぶ。）「Ｘ＾（ｋ）」とみなす。

［姿勢予測］
自己位置推定部１５は、以下の数式（７）～（９）に基づき、ワールド座標系における処理番号ｋでの角速度「φ^・（ｋ）」、「θ^・（ｋ）」及び「ψ^・（ｋ）」を算出する。なお、以下の数式（７）～（９）において、「φ＾（ｋ－１）」、「θ＾（ｋ－１）」及び「ψ＾（ｋ－１）」は、ワールド座標系における処理番号ｋ－１での推定自己姿勢を表している。また、以下の数式（７）～（９）において、「ω_ｘ（ｋ）」、「ω_ｙ（ｋ）」及び「ω_ｚ（ｋ）」は、ＩＭＵ６により計測された船舶座標系における処理番号ｋでの角速度を表している。

また、自己位置推定部１５は、以下の数式（１０）～（１２）に対し、角速度「φ^・（ｋ）」、「θ^・（ｋ）」及び「ψ^・（ｋ）」と、推定自己姿勢「φ＾（ｋ－１）」、「θ＾（ｋ－１）」及び「ψ＾（ｋ－１）」と、を適用することにより、ワールド座標系における処理番号ｋでの予測自己姿勢「φ^－（ｋ）」、「θ^－（ｋ）」及び「ψ^－（ｋ）」を算出する。なお、以下の数式（１０）～（１２）において、「Δｔ（ｋ）」は、処理番号ｋの処理時刻「ｔ（ｋ）」から、処理番号ｋ－１の処理時刻「ｔ（ｋ－１）」を減ずることにより得られる時刻差を表している。

すなわち、上記の数式（１０）～（１２）における右辺第２項は、処理番号ｋ－１から処理番号ｋまでの間に生じたと予測される姿勢の変化の大きさに対応する変化量を表している。

そして、自己位置推定部１５は、予測自己姿勢「φ^－（ｋ）」、「θ^－（ｋ）」及び「ψ^－（ｋ）」に対し、前述のＮＤＴスキャンマッチングと同様の演算を適用することにより、ワールド座標系における処理番号ｋでの推定自己姿勢「φ＾（ｋ）」、「θ＾（ｋ）」及び「ψ＾（ｋ）」を算出する。

［位置予測及び速度算出］
自己位置推定部１５は、以下の数式（１３）～（１５）に対し、ワールド座標系における処理番号ｋ－１での推定自己位置「ｘ＾（ｋ－１）」、「ｙ＾（ｋ－１）」及び「ｚ＾（ｋ－１）」と、当該ワールド座標系における当該処理番号ｋ－１の速度「ｘ^・（ｋ－１）」、「ｙ^・（ｋ－１）」及び「ｚ^・（ｋ－１）」と、を適用することにより、当該ワールド座標系における処理番号ｋでの予測自己位置「ｘ^－（ｋ）」、「ｙ^－（ｋ）」及び「ｚ^－（ｋ）」を算出する。

すなわち、上記の数式（１３）～（１５）における右辺第２項は、処理番号ｋ－１から処理番号ｋまでの間に生じたと予測される位置の変化の大きさに対応する変化量を表している。

自己位置推定部１５は、予測自己位置「ｘ^－（ｋ）」、「ｙ^－（ｋ）」及び「ｚ^－（ｋ）」に対し、前述のＮＤＴスキャンマッチングと同様の演算を適用することにより、ワールド座標系における処理番号ｋでの推定自己位置「ｘ＾（ｋ）」、「ｙ＾（ｋ）」及び「ｚ＾（ｋ）」を算出する。

その後、自己位置推定部１５は、以下の数式（１６）～（１８）に基づき、ワールド座標系における処理番号ｋでの速度「ｘ^・（ｋ）」、「ｙ^・（ｋ）」及び「ｚ^・（ｋ）」を算出する。なお、以下の数式（１６）～（１８）において、「τ」は時定数を表し、「ｓ」はラプラス演算子を表している。

上記の数式（１３）～（１８）を用いた演算処理は、例えば、図５のように図示することができる。なお、図５において、「直近の推定位置」は処理番号ｋ－１での推定自己位置を表し、「ＤＲ位置」は処理番号ｋでの予測自己位置を表し、「ＮＤＴ推定結果」は処理番号ｋでの推定自己位置を表している。図５は、自己位置推定部により行われる処理の一例を説明するための図である。

［時定数τの算出］
自己位置推定部１５は、以下の数式（１９）に基づき、信頼度値ＮＲＶ（ＮＤＴ Ｒｅｌｉａｂｉｌｉｔｙ Ｖａｌｕｅ）を算出する。

上記の数式（１９）における「ＤＳＳ」は、ダウンサンプリングのサイズを表している。数式（３）によって実施されるダウンサンプリングは、空間を適切なサイズの格子で区切り、格子ごとに平均値を算出することで実施される。ライダ３の点群データ数が多い場合には、格子サイズを大きくすることによりデータ量を抑制することができるため、所定時間内にＮＤＴマッチング処理を終わらせることができる。そのため、ライダ３が１周期分の走査を行った場合に得られる点群データが多くなるに伴い、「ＤＳＳ」が大きくなるとともに、信頼度値ＮＲＶが大きくなる。すなわち、「ＤＳＳ」が大きな値をとる場合には、自己位置推定の推定結果の信頼性が高くなる。

上記の数式（１９）における「ＤＡＲ」は、ダウンサンプリング後の点群データが地図に対応付けされる比率を表している。そのため、ライダ３が１周期分の走査を行った場合に得られる点群データにおいて、オクルージョンが少なく、かつ、地図ＤＢ１０に格納されている地図に対する実際の状況の変化が少ない場合に、「ＤＡＲ」が大きくなるとともに、信頼度値ＮＲＶが大きくなる。すなわち、「ＤＡＲ」が大きな値をとる場合には、自己位置推定の推定結果の信頼性が高くなる。

上記の数式（１９）における「Ｓｃｏｒｅ」は、前述のスコア値Ｅに相当する値である。そのため、自己位置推定の推定結果が最適解に近い場合に、「Ｓｃｏｒｅ」が大きくなるとともに、信頼度値ＮＲＶが大きくなる。すなわち、「Ｓｃｏｒｅ」が大きな値をとる場合には、自己位置推定の推定結果の信頼性が高くなる。

自己位置推定部１５は、以下の数式（２０）～（２３）のうちのいずれか１つに対して信頼度値ＮＲＶを適用することにより、当該信頼度値ＮＲＶを０以上かつ１以下の範囲に属する値に変換した信頼度指標ＮＲＩ（ＮＤＴ Ｒｅｌｉａｂｉｌｉｔｙ Ｉｎｄｅｘ）を算出する。なお、信頼度値ＮＲＶと信頼度指標ＮＲＩとの関係は、図６のように表される。図６は、信頼度値ＮＲＶと信頼度指標ＮＲＩとの関係を表す図である。

自己位置推定部１５は、以下の数式（２４）に対し、信頼度指標ＮＲＩを適用することにより、時定数τを算出する。なお、信頼度指標ＮＲＩと時定数τとの関係は、図７のように表される。また、信頼度値ＮＲＶと時定数τとの関係は、図８のように表される。図７は、信頼度指標ＮＲＩと時定数τとの関係を示す図である。図８は、信頼度値ＮＲＶと時定数τとの関係を示す図である。

ところで、自己位置推定部１５により算出された推定自己位置には，程度の違いはあるが、誤差が含まれている。したがって、直前の推定自己位置との差分にも誤差が含まれるとともに、ワールド座標系における速度の算出値にも誤差が含まれる。つまり、推定自己位置の誤差が大きいほど、ワールド座標系における速度の算出値の誤差も大きいものとなる。速度の算出値において大きな誤差が生じた場合には、上記の数式（１３）～（１５）により算出される予測位置が大きくずれてしまう可能性があるがゆえに、ＮＤＴスキャンマッチングにおける探索範囲を超えてしまう可能性がある。そのため、ＮＤＴスキャンマッチングにおける初期値として精度の良い予測位置を得るためには、速度誤差を低減する必要がある。

推定自己位置の誤差は、ランダムノイズ的なものと仮定できる。そのため、ローパスフィルタによって高域ノイズ成分を除去する方法と同様の方法により、推定自己位置の誤差を除去できるものと考えられる。本実施例においては、例えば、上記の数式（１６）～（１８）のように、１次遅れ系のローパスフィルタ（１／（τｓ＋１））を用いることにより、高域ノイズ成分を抑制している。しかし、上記のようなローパスフィルタを用いた場合には、早い動きに追従できないというデメリットが生じるおそれがある。

従って、本実施例においては、自己位置推定の信頼度に応じ、上記のローパスフィルタにおける時定数τを変化させるようにしている。そのため、例えば、自己位置推定の信頼度が高く、誤差が少ないと考えられる状況においては、上記のローパスフィルタの時定数τを小さな値に設定することにより、帯域を広げて追従性を高めることができる。また、例えば、自己位置推定の信頼度が低く、誤差が多いと考えられる状況においては、上記のローパスフィルタの時定数τを大きな値に設定することにより、極力ノイズを抑制することができる。なお、以降においては、上記のローパスフィルタを単にフィルタとも称する。

図９Ａ、１０Ａ及び１１Ａは、フィルタ無しの状態でワールド座標系における速度を算出した場合の例を示す図である。より具体的には、図９Ａ、１０Ａ及び１１Ａは、船舶の移動開始時から２秒経過時に急な速度変化が生じ、当該船舶の移動開始時から４秒経過時以降にノイズが生じている状態を示している。図９Ｂ及び図１０Ｂは、フィルタの時定数τを固定した状態でワールド座標系における速度を算出した場合の例を示す図である。図１１Ｂは、フィルタの時定数τを変化させつつワールド座標系における速度を算出した場合の例を示す図である。

図９Ａ及び９Ｂに示すように、フィルタの時定数τを０．１に固定した場合には、速度変化に追従することができている一方で、ノイズを抑制することができていない。また、図１０Ａ及び１０Ｂに示すように、フィルタの時定数τを１．０に固定した場合には、ノイズを抑制することができている一方で、速度変化に追従することができていない。そして、本実施例においては、図１１Ａ及び１１Ｂに示すように、例えば、速度の算出を開始してから４秒が経過する直前まではフィルタの時定数τを０．１に維持し、４秒が経過したタイミングでフィルタの時定数τを０．１から１．０に変化させることにより、速度変化に追従しつつ、ノイズを抑制することができるようにしている。

なお、本実施例のフィルタをプログラムとして実装する場合には、連続時間の伝達関数（ｓ領域）→離散時間の伝達関数（ｚ領域）→差分方程式（時間領域）の手順で変換するようにすればよい。

［フィルタによる効果］
ここで、本実施例のフィルタにより発揮される効果について説明する。なお、以下の説明においては、簡単のため、対象船舶が２次元平面上で移動しているとともに、当該対象船舶の方向がワールド座標系と同じであるものと仮定する。また、以下の説明において、「真の位置」は対象船舶の実際の位置を意味し、「推定位置」は当該対象船舶の推定自己位置を意味し、「予測位置」は当該対象船舶の予測自己位置を意味するものとする。

まず、位置の誤差が大きくなった場合において、フィルタを用いずに自己位置推定を行うことによる問題点について、図１２Ａを参照しつつ説明する。図１２Ａは、フィルタ無しの状態で船舶の自己位置推定を行った場合の例を説明するための図である。

図１２Ａの処理番号ｋ－３及びｋ－２のタイミングにおいては、自己位置推定が良好に行われることにより、真の位置と推定位置とがほぼ一致している。

その後、例えば、図１２Ａの処理番号ｋ－１のタイミングにおいて、対象船舶の周辺が疎な空間であること、及び、当該対象船舶の近くを大型船が通過したこと等のような、ＮＤＴスキャンマッチングの実施に不都合な状況が生じた場合には、真の位置に対してｙ方向にずれた推定位置が算出される。そのため、処理番号ｋ－２での推定位置と、処理番号ｋ－１での推定位置と、の差を用いて算出される処理番号ｋ－１での速度には、ｙ方向の誤った速度ｙ^・（ｋ―１）が含まれてしまう。

そして、図１２Ａの処理番号ｋのタイミングにおいては、適正ではない推定位置ｙ＾（ｋ―１）と、誤った速度ｙ^・（ｋ―１）と、を用いて予測位置ｙ^－（ｋ）が算出されることに起因し、当該予測位置ｙ^－（ｋ）が真の位置からさらにずれてしまう。また、予測位置ｙ^－（ｋ）の大きなずれは、ＮＤＴスキャンマッチングにおける探索範囲の超過を発生させる。その結果、推定位置ｙ＾（ｋ）は、真の位置から遠いものとなってしまっている。よって、ｙ^・（ｋ）が真の位置から離れる方向の速度として算出されることに起因し、図１２Ａに示した状況よりも後に算出される推定位置がますます真の位置から遠ざかってしまうおそれがある。

すなわち、フィルタを用いずに自己位置推定を行った場合には、ＮＤＴスキャンマッチングの実施に不都合な状況の発生に起因して予測位置のずれ（誤差）が次第に大きくなるため、真の位置に近づくような適正な推定自己位置を得ることができない、という問題点が生じる。

次に、本実施例のフィルタを用いて自己位置推定を行うことによる利点について、図１２Ｂを参照しつつ説明する。図１２Ｂ及び図１２Ｃは、フィルタ有りの状態で船舶の自己位置推定を行った場合の例を説明するための図である。

図１２Ｂの処理番号ｋ－３及びｋ－２のタイミングにおいては、自己位置推定が良好に行われることにより、真の位置と推定位置とがほぼ一致している。

その後、例えば、図１２Ｂの処理番号ｋ－１のタイミングにおいて、ＮＤＴスキャンマッチングの実施に不都合な状況が生じることにより、信頼度指標ＮＲＩの値が小さくなった場合には、推定位置が不正確な可能性が高いため、自己位置推定部１５は、速度の算出に用いるフィルタの時定数τを大きな値に設定する。その結果、大きな速度変化が抑制されたｙ^・（ｋ―１）が算出される。

そして、図１２Ｂの処理番号ｋのタイミングにおいては、予測位置ｙ^－（ｋ）の真の位置からのずれ量が少ないため、ＮＤＴスキャンマッチングにおける探索範囲の範囲内であり、推定位置ｙ＾（ｋ）が真の位置に近づくことができている。また、ｙ^・（ｋ）が真の位置に近づく方向の速度として算出されることに起因し、図１２Ｂに示した状況よりも後に算出される推定位置が真の位置に漸次近づいてゆく。

すなわち、フィルタを用いて自己位置推定を行った場合には、ＮＤＴスキャンマッチングの実施に不都合な状況が発生下場合であっても、真の位置に近づくような適正な推定自己位置を得ることができる、という利点が生じる。

ところで、慣性力が大きくかつ速度変化量が小さい大型船舶においては、大きな速度変化が生じないような、一定の周波数特性のフィルタを用いて自己位置推定を行ったとしても問題ないものと考えられる。しかし、プレジャーボートのような小型船舶は、加速性能が高く、速度変化量が必ずしも小さいとは言えない。

そのため、例えば、自己位置推定の信頼性が十分に高い場合には、時定数τの値を小さくすることにより、小型船舶において生じ得る大きな加減速に対応できるようにしたほうがよいと考えられる。そこで、本実施例においては、信頼度指標ＮＲＩの値に応じて時定数τを設定することにより、適応的に特性を変化させることが可能なフィルタを用いて自己位置推定を行うようにしている。

また、例えば、図１２Ｃの処理番号ｋ―１及び処理番号ｋに示すように、対象船舶がｙ方向に大きく移動した一方で、ＮＤＴスキャンマッチングによる自己位置推定が正しく行われている場合には、信頼度指標ＮＲＩの値が大きくなり、かつ、フィルタの時定数τの値が小さくなるため、速度ｙ^・（ｋ―１）が抑制されずに算出される。その結果、速度ｙ^・（ｋ―１）が大きい値として算出される一方で、正しい値として算出されるため、処理番号ｋでの予測位置が真の位置に近いものとして算出される。

［機能ブロック］
図１３は、第１実施例に係る自己位置推定部の機能ブロックの一例を示す図である。自己位置推定部１５は、図１３に示すように、デッドレコニングブロック２１と、座標変換ブロック２２と、ＮＤＴ位置算出ブロック２３と、信頼度算出ブロック２４と、時定数算出ブロック２５と、速度算出ブロック２６と、フィルタブロック２７と、を有する。

デッドレコニングブロック２１は、処理番号ｋのタイミングにおいてＩＭＵ６から出力された角速度（ω_ｘ（ｋ）、ω_ｙ（ｋ）及びω_ｚ（ｋ））と、ＮＤＴ位置算出ブロック２３により算出された当該処理番号ｋの直前の処理番号ｋ－１での推定自己姿勢（φ＾（ｋ－１）、θ＾（ｋ－１）及びψ＾（ｋ－１））と、に基づき、当該処理番号ｋでの角速度（φ^・（ｋ）、θ^・（ｋ）及びψ^・（ｋ））を算出する。また、デッドレコニングブロック２１は、ＮＤＴ位置算出ブロック２３により算出された処理番号ｋ－１での推定自己姿勢と、処理番号ｋでの角速度と、に基づき、当該処理番号ｋでの予測自己姿勢（φ^－（ｋ）、θ^－（ｋ）及びψ^－（ｋ））を算出する。また、デッドレコニングブロック２１は、ＮＤＴ位置算出ブロック２３により算出された処理番号ｋ－１での推定自己位置（ｘ＾（ｋ－１）、ｙ＾（ｋ－１）及びｚ＾（ｋ－１））と、速度算出ブロック２６からフィルタブロック２７を経て出力される当該処理番号ｋ－１での速度（ｘ^・（ｋ－１）、ｙ^・（ｋ－１）及びｚ^・（ｋ－１））と、に基づき、処理番号ｋでの予測自己位置（ｘ^－（ｋ）、ｙ^－（ｋ）及びｚ^－（ｋ））を算出する。なお、自己位置推定の開始直後であって、処理番号ｋ－１での推定自己姿勢及び推定自己位置が存在しない場合等には、デッドレコニングブロック２１は、例えば、ＩＭＵ６から出力される角速度に基づいて処理番号ｋでの予測自己姿勢を算出するとともに、ＧＰＳ受信機５から出力される信号に基づいて当該処理番号ｋでの予測自己位置を算出する。

座標変換ブロック２２は、ライダ３の出力に基づく点群データを、地図ＤＢ１０と同一の座標系であるワールド座標系に変換する。この場合、座標変換ブロック２２は、例えば、デッドレコニングブロック２１の処理結果として得られた処理番号ｋでの予測自己姿勢及び予測自己位置に基づき、当該処理番号ｋの点群データの座標変換を行う。なお、移動体（本実施例では船舶）に設置されたライダを基準とした座標系の点群データを移動体の座標系に変換する処理、及び移動体の座標系からワールド座標系に変換する処理等については、例えば、国際公開ＷＯ２０１９／１８８７４５などに開示されている。

ＮＤＴ位置算出ブロック２３は、座標変換ブロック２２の処理結果として得られた点群データに基づき、推定自己位置及び推定自己姿勢を算出する。この場合、ＮＤＴ位置算出ブロック２３は、座標変換ブロック２２から供給されるワールド座標系の点群データと、同じワールド座標系で表されたボクセルデータＶＤとを照合することで、点群データとボクセルとの対応付けを行う。また、ＮＤＴ位置算出ブロック２３は、点群データと対応付けがなされた各ボクセルを対象として、数式（４）に基づく個別評価関数値を算出し、数式（５）に基づくスコア値Ｅ（ｋ）が最大となるとなる推定パラメータＰを算出する。そして、ＮＤＴ位置算出ブロック２３は、数式（６）に基づき、デッドレコニングブロック２１の処理結果として得られた処理番号ｋでの予測自己位置に対し、処理番号ｋで求めた推定パラメータＰを適用することにより、当該処理番号ｋでの推定自己位置（ｘ＾（ｋ）、ｙ＾（ｋ）及びｚ＾（ｋ））を算出する。また、ＮＤＴ位置算出ブロック２３は、このような処理と同様の処理を行うことにより、処理番号ｋでの推定自己姿勢（φ＾（ｋ）、θ＾（ｋ）及びψ＾（ｋ））を算出する。なお、ＮＤＴ位置算出ブロック２３は、例えば、座標変換ブロック２２の処理結果として得られた点群データのうち、水面位置及びそれより下方のデータを誤データとして除去した後で推定自己位置及び推定自己姿勢を算出するようにしてもよい。

信頼度算出ブロック２４は、ＮＤＴ位置算出ブロック２３において行われた処理の内容に基づいてＤＳＳ、ＤＡＲ及びＳｃｏｒｅの３つのパラメータを特定する。そして、信頼度算出ブロック２４は、前述の３つのパラメータを用いて信頼度値ＮＲＶを算出し、当該算出した信頼度値ＮＲＶを信頼度指標ＮＲＩに変換する。

時定数算出ブロック２５は、信頼度算出ブロック２４の処理結果として得られた信頼度指標ＮＲＩを用いて時定数τを算出する。

速度算出ブロック２６は、ＮＤＴ位置算出ブロック２３の処理結果として得られた処理番号ｋでの推定自己位置と、処理番号ｋ－１での推定自己位置と、の差分値を時刻差Δｔ（ｋ）で除することにより、フィルタ無しの状態における当該処理番号ｋでの速度を算出する。

フィルタブロック２７は、時定数算出ブロック２５の処理結果として得られた時定数τを用いてフィルタ（１／（τｓ＋１））を設定するとともに、当該設定したフィルタを速度算出ブロック２６により算出された速度に対して適用することにより、処理番号ｋでの速度（ｘ^・（ｋ）、ｙ^・（ｋ）及びｚ^・（ｋ））を算出する。

［処理フロー］
図１４は、情報処理装置１の自己位置推定部１５が実行する自己位置推定処理の手順の一例を示すフローチャートである。自己位置推定部１５は、図８のフローチャートの処理を、電源がオンになった場合など、自己位置推定を行う必要が生じた場合に開始する。

まず、自己位置推定部１５は、自己位置推定処理の開始直後において、ＧＰＳ受信機５が出力するデータに基づくＧＰＳ測位結果から予測自己位置を算出する（ステップＳ１１）。

次に、自己位置推定部１５は、ライダ３からのスキャンデータとして、ＮＤＴスキャンマッチングを実施可能な量のデータが得られたか否かを判定する（ステップＳ１２）。

自己位置推定部１５は、ＮＤＴスキャンマッチングを実施可能な量のデータが得られなかった場合（ステップＳ１２：ＮＯ）には、ライダ３からのスキャンデータを再度取得し、当該取得したデータに基づいてステップＳ１２の処理を再度行う。また、自己位置推定部１５は、ＮＤＴスキャンマッチングを実施可能な量のデータが得られた場合（ステップＳ１２：ＹＥＳ）には、時刻差Δｔ（ｋ）を算出し（ステップＳ１３）、さらに、船舶座標系における角速度（ω_ｘ（ｋ）、ω_ｙ（ｋ）及びω_ｚ（ｋ））をＩＭＵ６から取得する（ステップＳ１４）。

その後、自己位置推定部１５は、ステップＳ１４により取得した船舶座標系における角速度と、数式（７）～（９）と、に基づき、ワールド座標系における角速度（φ^・（ｋ）、θ^・（ｋ）及びψ^・（ｋ））を算出する（ステップＳ１５）。

続いて、自己位置推定部１５は、ステップＳ１３により算出した時刻差Δｔ（ｋ）と、ステップＳ１５により算出したワールド座標系における角速度と、数式（１０）～（１２）と、に基づき、ワールド座標系における予測自己姿勢（φ^－（ｋ）、θ^－（ｋ）及びψ^－（ｋ））を算出する（ステップＳ１６）。

続いて、自己位置推定部１５は、ステップＳ１３により算出した時刻差Δｔ（ｋ）と、数式（１３）～（１５）と、に基づき、ワールド座標系における予測自己位置（ｘ^－（ｋ）、ｙ^－（ｋ）及びｚ^－（ｋ））を算出する（ステップＳ１７）。

続いて、自己位置推定部１５は、ステップＳ１６により取得した予測自己姿勢及びステップＳ１７により取得した予測自己位置に相当するＤＲ位置を初期値としてＮＤＴスキャンマッチングを行うことにより、推定自己位置（ｘ＾（ｋ）、ｙ＾（ｋ）及びｚ＾（ｋ））と、推定自己姿勢（φ＾（ｋ）、θ＾（ｋ）及びψ＾（ｋ））と、をそれぞれ算出する（ステップＳ１８）。

続いて、自己位置推定部１５は、ステップＳ１８の処理内容に基づいて特定されるＤＳＳ、ＤＡＲ及びＳｃｏｒｅの３つのパラメータと、数式（１９）と、基づいて信頼度値ＮＲＶを算出する（ステップＳ１９）。

続いて、自己位置推定部１５は、ステップＳ１９により算出した信頼度値ＮＲＶを、数式（２０）～（２３）のうちのいずれか１つに適用することにより、信頼度指標ＮＲＩに変換する（ステップＳ２０）。

続いて、自己位置推定部１５は、ステップＳ２０により得た信頼度指標ＮＲＩと、数式（２４）と、基づいて時定数τを算出する（ステップＳ２１）。

続いて、自己位置推定部１５は、ステップＳ１３により算出した時刻差Δｔ（ｋ）と、ステップＳ２１により算出した時定数τをフィルタ（１／（τｓ＋１））に適用した数式（１６）～（１８）と、に基づき、速度（ｘ^・（ｋ）、ｙ^・（ｋ）及びｚ^・（ｋ））を算出する（ステップＳ２２）。

自己位置推定部１５は、ステップＳ２２を行った後、自己位置推定処理を終了すべきか否か判定する（ステップＳ２３）。そして、自己位置推定部１５は、自己位置推定処理を終了すべきと判定した場合（ステップＳ２３：ＹＥＳ）には、フローチャートの処理を終了する。一方、自己位置推定部１５は、自己位置推定処理を継続する場合（ステップＳ２３：ＮＯ）には、ステップＳ１２に戻り、次の処理番号での推定自己位置、推定自己姿勢及び速度等を算出する

［実験結果に基づく考察］
上述した実施例に関する実験結果について考察する。

図１５は、実験時におけるライダの設置状態及びスキャン範囲を示す図である。出願人は、ある水路に対して事前に作成した沿岸用のボクセルデータ（ＮＤ地図）と、運航中の船舶に設けたライダにより得られた点群データをマッチングさせてＮＤＴ位置推定を行った。この実験は、図１５に例示するように、中距離かつ水平６０度タイプのライダであって、動作周波数１２Ｈｚ（周期８３．３ｍｓ）のライダを船舶の左右に２つずつ設けた状態で行われた。また、精度評価のために、正解となる位置データとして、ＲＴＫ－ＧＰＳの測位結果を用いた。

なお、本実験では、ライダにより得られた点群データに対してダウンサンプリングを行っている。この場合、ダウンサンプリングのサイズは、ＮＤＴスキャンマッチングにおいて、ダウンサンプリング後の点群データのうちボクセルデータＶＤと対応付けられた計測点数（「対応計測点数」とも呼ぶ。）が所定の目標範囲となるように、各処理時刻において適応的に変更されるものとする。例えば、自己位置推定部１５は、各処理時刻において、対応計測点数が目標範囲の上限より多い時には、次の処理時刻でのダウンサンプリングサイズを所定率（例えば１．１倍）又は所定値だけ大きくし、対応計測点数が目標範囲の下限より少ない時には、次の処理時刻でのダウンサンプリングサイズを所定率又は所定値だけ小さくする。一方、自己位置推定部１５は、各処理時刻において、対応計測点数が目標範囲内である場合には、ダウンサンプリングサイズを維持する。このようなダウンサンプリングサイズの適応的設定では、対応計測点数が多いほどダウンサンプリングサイズが大きくなるため、ダウンサンプリングサイズが大きいほどＮＤＴ処理の信頼度が高くなることが推察される。

図１６Ａ～図１６Ｇ、及び、図１７Ａ～図１７Ｇは、フィルタ無しの状態で速度を算出する比較例に係る自己位置推定結果を示す図である。ここで、図１６Ａはダウンサンプリング前の点群データの計測点数、図１６Ｂはダウンサンプリングのサイズ、図１６Ｃはダウンサンプリング後の計測点数及び対応計測点数、図１６Ｄは対応付け比率ＤＡＲ、図１６Ｅはスコア値Ｅ、図１６Ｆは信頼度値ＮＲＶ、図１６Ｇは信頼度指標ＮＲＩを夫々示す。また、図１７Ａはｘ方向の速度、図１７Ｂはｙ方向の速度、図１７Ｃはｚ方向の速度を夫々示す。また、ＲＴＫ－ＧＰＳの測位結果に対して、図１７Ｄは進行方向の誤差、図１７Ｅは横方向の誤差、図１７Ｆは高さ方向の誤差、図１７Ｇはヨー角の誤差を夫々示す。

比較例に係る実験結果では、図１６Ｄに示すように、対応付け比率ＤＡＲの値が９０～１２０ｓ付近で低下しており、ＮＤＴスキャンマッチングの演算精度に対して悪影響を及ぼすような状況となっている。このような状況の発生に伴い、図１７Ａ～図１７Ｃに示す速度によれば、特に９０～１２０ｓ付近において、オフセットと考えられる乱れが生じている。また、図１７Ｄ～図１７Ｇに示すように、速度の乱れが生じている部分においては、他の部分よりも推定自己位置及び推定自己姿勢の誤差が大きくなっている。特に、図１７Ｄの進行方向の誤差については、便宜上図示できていないが、－２０ｍ程度まで増大している。

図１８Ａ～図１８Ｇ、及び、図１９Ａ～図１９Ｈは、フィルタ有りの状態で速度を算出する実施例に係る自己位置推定結果を示す図である。ここで、図１８Ａはダウンサンプリング前の点群データの計測点数、図１８Ｂはダウンサンプリングのサイズ、図１８Ｃはダウンサンプリング後の計測点数及び対応計測点数、図１８Ｄは対応付け比率ＤＡＲ、図１８Ｅはスコア値Ｅ、図１８Ｆは信頼度値ＮＲＶ、図１８Ｇは信頼度指標ＮＲＩを夫々示す。また、図１９Ａは時定数τ、図１９Ｂはｘ方向の速度、図１９Ｃはｙ方向の速度、図１９Ｄはｚ方向の速度を夫々示す。また、ＲＴＫ－ＧＰＳの測位結果に対して、図１９Ｅは進行方向の誤差、図１９Ｆは横方向の誤差、図１９Ｇは高さ方向の誤差、図１９Ｈはヨー角の誤差を夫々示す。

本実施例に係る実験結果では、図１７Ａ～図１７Ｃの速度に比べて、図１９Ｂ～図１９Ｄの速度は全般的に滑らかになっているとともに、１００～１２０ｓ付近の速度の乱れが低減している。その結果、本実施例に係る実験結果では、図１９Ｅ～図１９Ｈに示すように、比較例に係る実験結果に比べ、推定自己位置及び推定自己姿勢の誤差が減少している。特に、図１９Ｅの進行方向の誤差については、図１７Ｄに示したような大きな誤差が抑制されているとともに、１００ｓまでの誤差振幅も平均的に小さくなっている。また、図１８Ｇに示すように、信頼度指標ＮＲＩの値が低減している期間が比較例に係る実験結果（図１６Ｇ）よりも短くなっており、フィルタ無しの状態に比べて安定性が向上したと判断することができる。

以上に述べたように、本実施例によれば、信頼度指標ＮＲＩ（信頼度値ＮＲＶ）に応じて時定数τが変化するフィルタ（１／（τｓ＋１））を用いて船舶の速度を算出し、当該算出した船舶の速度を用いて自己位置推定処理を行うことができる。そのため、本実施例によれば、船舶における自己位置推定の精度を向上させることができる。

［変形例］
以下、上述の実施例に好適な変形例について説明する。

（変形例１）
自己位置推定部１５は、信頼度指標ＮＲＩの代わりに、対応付け比率ＤＡＲを用いて時定数τを設定するようにしてもよい。

例えば、対応付け比率ＤＡＲの値が小さい場合には、自己位置推定の推定結果の信頼性が低く、誤差が多いと考えられる。そのため、本変形例に係る自己位置推定部１５は、対応付け比率ＤＡＲの値が小さい場合には、時定数τの値を大きな値に設定すればよい。

また、例えば、対応付け比率ＤＡＲの値が大きい場合には、自己位置推定の推定結果の信頼性が高く、誤差が少ないと考えられる。そのため、本変形例に係る自己位置推定部１５は、対応付け比率ＤＡＲの値が大きい場合には、時定数τの値を小さな値に設定すればよい。

すなわち、本変形例に係る自己位置推定部１５は、数式（２４）の「ＮＲＩ」の値として、０以上１以下の範囲に属する「ＤＡＲ」の値を適用することにより、適切な時定数τを算出することができる。

（変形例２）
自己位置推定部１５は、ＩＭＵ６により計測された角速度（ω_ｘ（ｋ）、ω_ｙ（ｋ）及びω_ｚ（ｋ））を用いる代わりに、例えば、以下の数式（２５）～（２７）を用いた演算を行うことにより、ワールド座標系における処理番号ｋでの角速度「φ^・（ｋ）」、「θ^・（ｋ）」及び「ψ^・（ｋ）」を算出するようにしてもよい。なお、以下の数式（２５）～（２７）において、「τ」は時定数を表し、「ｓ」はラプラス演算子を表している。

すなわち、本変形例によれば、数式（２５）～（２７）を用いた演算により、ＩＭＵ６を搭載していない船舶においても、自己位置推定処理を行うことができる。

＜第２実施例＞
次に、第２実施例について説明する。なお、本実施例においては、第１実施例と同様の構成等を適用可能な部分についての説明を適宜省略するとともに、第１実施例とは異なる部分に主眼を置いて説明を行うものとする。

［運転支援システムの概要］
図２０は、第２実施例に係る運転支援システムの概略構成を示す図である。本実施例に係る運転支援システムは、対象船舶に設けられた情報処理装置１Ａと、当該対象船舶に搭載されたセンサ群２Ａと、を有する。

センサ群２Ａは、対象船舶に設けられた種々の外界センサ及び内界センサを含んでいる。本実施例では、センサ群２Ａは、ライダ３と、ＧＰＳ受信機５と、３軸方向における対象船舶の角速度を計測するＩＭＵ６と、３軸方向における当該対象船舶の加速度を計測する加速度センサ７と、を含んでいる。なお、本実施例によれば、ＩＭＵ６において加速度を計測可能である場合には、加速度センサ７がセンサ群２Ａに含まれていなくともよい。

［情報処理装置の構成］
図２１は、第２実施例に係る情報処理装置のハードウェア構成の一例を示すブロック図である。情報処理装置１Ａは、図２１に示すように、情報処理装置１の自己位置推定部１５の代わりに自己位置推定部１５Ａを有している。

［機能ブロック］
図２２は、第２実施例に係る自己位置推定部の機能ブロックの一例を示す図である。自己位置推定部１５Ａは、図２２に示すように、デッドレコニングブロック２１と、座標変換ブロック２２と、ＮＤＴ位置算出ブロック２３と、時定数算出ブロック２５Ａと、速度算出ブロック２６と、フィルタブロック２７Ａと、を有する。

時定数算出ブロック２５Ａは、加速度センサ７により計測された３軸方向の加速度を用い、後述の時定数τ_ｘ、τ_ｙ及びτ_ｚを算出する。

フィルタブロック２７Ａは、数式（１６）の時定数「τ」の代わりに時定数「τ_ｘ」を適用し、数式（１７）の時定数「τ」の代わりに時定数「τ_ｙ」を適用し、数式（１８）の時定数「τ」の代わりに時定数「τ_ｚ」を適用して演算を行うことにより、ワールド座標系における処理番号ｋでの速度「ｘ^・（ｋ）」、「ｙ^・（ｋ）」及び「ｚ^・（ｋ）」を算出する。

［時定数τ_ｘ、τ_ｙ及びτ_ｚの算出］
ここで、自己位置推定部１５Ａによる時定数τ_ｘ、τ_ｙ及びτ_ｚの算出方法について説明する。なお、以下においては、ロール方向及びピッチ方向の変位が無い（または略無い）場合であって、２次元平面上で加速度が変化する場合の例について説明する。

自己位置推定部１５Ａは、加速度センサ７により計測された３軸方向の加速度「ａ_ｘｂ」、「ａ_ｙｂ」及び「ａ_ｚｂ」を以下の数式（２８）～（３０）に適用することにより、ワールド座標系における３軸方向の加速度「ａ_ｘ」、「ａ_ｙ」及び「ａ_ｚ」を算出する。なお、加速度「ａ_ｘｂ」は対象船舶の進行方向に対応するｘ_ｂ方向の加速度であり、加速度「ａ_ｙｂ」は当該対象船舶の横方向（左右方向）に対応するｙ_ｂ方向の加速度であり、加速度「ａ_ｚｂ」は当該対象船舶の上下方向に対応するｚ_ｂ軸方向の加速度である。また、以下の数式（２８）～（３０）の演算の概要は、例えば、図２３のように示すことができる。図２３は、加速度センサにより計測された加速度をワールド座標系の加速度に変換する際に行われる演算の概要を示す図である

自己位置推定部１５Ａは、以下の数式（３１）～（３３）に対し、ワールド座標系における３軸方向の加速度「ａ_ｘ」、「ａ_ｙ」及び「ａ_ｚ」を適用することにより、時定数「τ_ｘ」、「τ_ｙ」及び「τ_ｚ」を算出する。

上記の数式（３１）によれば、例えば、加速度「ａ_ｘ」の絶対値が小さく、ｘ方向の速度変化が小さい場合には、時定数τ_ｘが大きな値として算出される。また、上記の数式（３１）によれば、例えば、加速度「ａ_ｘ」の絶対値が大きく、ｘ方向の速度変化が大きい場合には、時定数τ_ｘが小さな値として算出される。

上記の数式（３２）によれば、例えば、加速度「ａ_ｙ」の絶対値が小さく、ｙ方向の速度変化が小さい場合には、時定数τ_ｙが大きな値として算出される。また、上記の数式（３２）によれば、例えば、加速度「ａ_ｙ」の絶対値が大きく、ｙ方向の速度変化が大きい場合には、時定数τ_ｙが小さな値として算出される。

上記の数式（３３）によれば、例えば、加速度「ａ_ｚ」の絶対値が小さく、ｚ方向の速度変化が小さい場合には、時定数τ_ｚが大きな値として算出される。また、上記の数式（３３）によれば、例えば、加速度「ａ_ｚ」の絶対値が大きく、ｚ方向の速度変化が大きい場合には、時定数τ_ｚが小さな値として算出される。

以上に述べたように、本実施例によれば、加速度ａ_ｘに応じて時定数τ_ｘが変化するフィルタ（１／（τ_ｘｓ＋１））と、加速度ａ_ｙに応じて時定数τ_ｙが変化するフィルタ（１／（τ_ｙｓ＋１））と、加速度ａ_ｚに応じて時定数τ_ｚが変化するフィルタ（１／（τ_ｚｓ＋１））と、を用いて船舶の速度船舶の速度を算出し、当該算出した船舶の速度を用いて自己位置推定処理を行うことができる。そのため、本実施例によれば、船舶における自己位置推定の精度を向上させることができる。

（変形例）
以下、上述の実施例に好適な変形例について説明する。

自己位置推定部１５Ａは、対象船舶の状態を示す船舶状態情報を取得し、当該取得した船舶状態情報に基づいて時定数τを算出するようにしてもよい。なお、本変形例における時定数τは、速度ｘ^・（ｋ）、ｙ^・（ｋ）及びｚ^・（ｋ）を算出する際に、時定数τ_ｘ、τ_ｙ及びτ_ｚの代わりに用いることができる。

具体的には、自己位置推定部１５Ａは、例えば、対象船舶のスロットルレバーの変化量ＳＶ、当該対象船舶のプロペラ回転数の変化量ＰＶ、または、対象船舶のインペラ回転数の変化量ＩＶのいずれかを船舶状態情報として取得する。

そして、自己位置推定部１５Ａは、船舶状態情報として取得した変化量ＳＶ、ＰＶまたはＩＶの値を以下の数式（３４）の「ｎ」に代入することにより、時定数τを算出する。

対象船舶のスロットルレバーの変化量ＳＶは、当該対象船舶の加速量（または減速量）が大きくなるに従って大きくなる。そのため、数式（３４）の「ｎ」に代入される変化量ＳＶの値が大きい場合には、時定数τが小さな値として算出される。

対象船舶のプロペラ回転数の変化量ＰＶは、当該対象船舶の加速量（または減速量）が大きくなるに従って大きくなる。そのため、数式（３４）の「ｎ」に代入される変化量ＰＶの値が大きい場合には、時定数τが小さな値として算出される。

対象船舶のインペラ回転数の変化量ＩＶは、当該対象船舶の加速量（または減速量）が大きくなるに従って大きくなる。そのため、数式（３４）の「ｎ」に代入される変化量ＩＶの値が大きい場合には、時定数τが小さな値として算出される。

なお、自己位置推定部１５Ａは、加速度センサ７により計測された対象船舶の進行方向の加速度を船舶状態情報として取得するものであってもよい。すなわち、船舶状態情報には、対象船舶のスロットルレバーの変化量ＳＶ、当該対象船舶のプロペラ回転数の変化量ＰＶ、対象船舶のインペラ回転数の変化量ＩＶ、または、当該対象船舶の加速度のいずれかが含まれていればよい。

＜第３実施例＞
次に、第３実施例について説明する。なお、本実施例においては、第１または第２実施例と同様の構成等を適用可能な部分についての説明を適宜省略するとともに、第１及び第２実施例のいずれとも異なる部分に主眼を置いて説明を行うものとする。

［運転支援システムの概要］
本実施例に係る運転支援システムは、対象船舶に設けられた情報処理装置１Ｂと、当該対象船舶に搭載されたセンサ群２Ａと、を有する。

［情報処理装置の構成］
図２４は、第３実施例に係る情報処理装置のハードウェア構成の一例を示すブロック図である。情報処理装置１Ｂは、図２４に示すように、情報処理装置１の自己位置推定部１５の代わりに自己位置推定部１５Ｂを有している。

［機能ブロック］
図２５は、第３実施例に係る自己位置推定部の機能ブロックの一例を示す図である。自己位置推定部１５Ｂは、図２５に示すように、デッドレコニングブロック２１と、座標変換ブロック２２と、ＮＤＴ位置算出ブロック２３と、時定数算出ブロック２５Ｂと、速度算出ブロック２６と、フィルタブロック２７と、を有する。

時定数算出ブロック２５Ｂは、信頼度算出ブロック２４の処理結果として得られた信頼度指標ＮＲＩと、加速度センサ７により計測された加速度と、を用いて時定数τを算出する。

［時定数τの算出］
ここで、自己位置推定部１５Ｂによる時定数τの算出方法について説明する。

自己位置推定部１５Ｂは、信頼度指標ＮＲＩを数式（２４）に適用することにより、第１実施例と同様の時定数τ_ｔを算出する。また、自己位置推定部１５Ｂは、加速度センサ７により計測された3軸方向の加速度（ａ_ｘｂ、ａ_ｙｂ及びａ_ｚｂ）を数式（２８）～（３０）に適用することにより、ワールド座標系における３軸方向の加速度（ａ_ｘ、ａ_ｙ及びａ_ｚ）を算出する。また、自己位置推定部１５Ｂは、ワールド座標系における３軸方向の加速度（ａ_ｘ、ａ_ｙ及びａ_ｚ）を数式（３１）～（３３）に適用することにより、第２実施例と同様の時定数τ_ｘ、τ_ｙ及びτ_ｚを算出する。そして、自己位置推定部１５Ｂは、時定数τ_ｔ及びτ_ｘの平均値に相当する（τ_ｔ＋τ_ｘ）／２と、時定数τ_ｔ及びτ_ｙの平均値に相当する（τ_ｔ＋τ_ｙ）／２と、時定数τ_ｔ及びτ_ｚの平均値に相当する（τ_ｔ＋τ_ｚ）／２と、を時定数τの算出結果として取得する。

以上に述べたように、本実施例によれば、信頼度指標ＮＲＩと、加速度ａ_ｘ、ａ_ｙ及びａ_ｚと、に応じて時定数τが変化するフィルタ（１／（τｓ＋１））を用いて船舶の速度船舶の速度を算出し、当該算出した船舶の速度を用いて自己位置推定処理を行うことができる。そのため、本実施例によれば、船舶における自己位置推定の精度を向上させることができる。

なお、上述した各実施例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（ｎｏｎ－ｔｒａｎｓｉｔｏｒｙ ｃｏｍｐｕｔｅｒ ｒｅａｄａｂｌｅ ｍｅｄｉｕｍ）を用いて格納され、コンピュータである制御部等に供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記憶媒体（ｔａｎｇｉｂｌｅ ｓｔｏｒａｇｅ ｍｅｄｉｕｍ）を含む。非一時的なコンピュータ可読媒体の例は、磁気記憶媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記憶媒体（例えば光磁気ディスク）、ＣＤ－ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＣＤ－Ｒ、ＣＤ－Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Ｐｒｏｇｒａｍｍａｂｌｅ ＲＯＭ）、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ ＰＲＯＭ）、フラッシュＲＯＭ、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ））を含む。

以上、実施形態を参照して本願発明を説明したが、本願発明は上記実施形態に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。すなわち、本願発明は、請求の範囲を含む全開示、技術的思想にしたがって当業者であればなし得るであろう各種変形、修正を含むことは勿論である。また、引用した上記の特許文献等の各開示は、本書に引用をもって繰り込むものとする。

１、１Ａ、１Ｂ 情報処理装置
２、２Ａ センサ群
３ ライダ
５ ＧＰＳ受信機
６ ＩＭＵ
７ 加速度センサ
１０ 地図ＤＢ

Claims (11)

1. 船舶の予測位置を取得する予測位置取得手段と、
   前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出する推定位置算出手段と、
   前記照合の信頼度を算出する信頼度算出手段と、
   第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する速度算出手段と、
   を有し、
   前記データは、前記外界センサが出力した点群データである第１点群データに対してダウンサンプリングが行われた点群データである第２点群データであり、
   前記信頼度算出手段は、少なくとも前記ダウンサンプリングのサイズ又は前記第１点群データの計測点数に基づき、前記信頼度を算出する情報処理装置。
2. 船舶の予測位置を取得する予測位置取得手段と、
   前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出する推定位置算出手段と、
   少なくとも前記データが前記地図データに対応付けされる比率に基づき、前記照合の信頼度を算出する信頼度算出手段と、
   第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する速度算出手段と、
   を有する情報処理装置。
3. 前記時定数は、前記信頼度の大きさに応じて小さくなる値として設定される請求項１または２に記載の情報処理装置。
4. 前記時定数は、前記信頼度と、前記船舶の加速度と、に基づいて設定される請求項１または２に記載の情報処理装置。
5. 前記予測位置取得手段は、前記第２処理時刻での前記推定位置と、前記第２処理時刻での前記船舶の速度と、に基づき、前記第１処理時刻での前記予測位置を取得する請求項１乃至４のいずれか一項に記載の情報処理装置。
6. 前記信頼度算出手段は、さらに前記照合の適合度合いを示すスコア値に基づき、前記信頼度を算出する請求項１乃至５のいずれか一項に記載の情報処理装置。
7. コンピュータが実行する制御方法であって、
   船舶の予測位置を取得し、
   前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出し、
   前記照合の信頼度を算出し、
   第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を前記コンピュータが算出し、
   前記データは、前記外界センサが出力した点群データである第１点群データに対してダウンサンプリングが行われた点群データである第２点群データであり、
   少なくとも前記ダウンサンプリングのサイズ又は前記第１点群データの計測点数に基づき、前記信頼度を前記コンピュータが算出する制御方法。
8. コンピュータが実行する制御方法であって、
   船舶の予測位置を取得し、
   前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出し、
   少なくとも前記データが前記地図データに対応付けされる比率に基づき、前記照合の信頼度を算出し、
   第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する制御方法。
9. 船舶の予測位置を取得し、
   前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出し、
   前記照合の信頼度を算出し、
   第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する処理をコンピュータに実行させ、
   前記データは、前記外界センサが出力した点群データである第１点群データに対してダウンサンプリングが行われた点群データである第２点群データであり、
   少なくとも前記ダウンサンプリングのサイズ又は前記第１点群データの計測点数に基づき、前記信頼度を算出する処理を前記コンピュータに実行させるプログラム。
10. 船舶の予測位置を取得し、
    前記船舶に設けられた外界センサの出力に基づくデータと地図データとの照合に基づき、前記予測位置を更新した前記船舶の推定位置を算出し、
    少なくとも前記データが前記地図データに対応付けされる比率に基づき、前記照合の信頼度を算出し、
    第１処理時刻での前記推定位置と、前記第１処理時刻の直前の第２処理時刻での前記推定位置と、少なくとも前記信頼度に基づき設定した時定数とに基づき、前記第１処理時刻での前記船舶の速度を算出する処理をコンピュータに実行させるプログラム。
11. 請求項９または１０に記載のプログラムを記憶した記憶媒体。