特許 6904895 位置推定装置、および、位置推定方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6904895

(24)【登録日】2021年6月28日

(45)【発行日】2021年7月21日

(54)【発明の名称】位置推定装置、および、位置推定方法

(51)【国際特許分類】

   G01S 15/89 20060101AFI20210708BHJP

   G01S 15/86 20200101ALI20210708BHJP

   G01H 5/00 20060101ALI20210708BHJP

   G01S 7/526 20060101ALI20210708BHJP

   G21C 17/08 20060101ALN20210708BHJP

【ＦＩ】

   G01S15/89 A

   G01S15/86

   G01H5/00

   G01S7/526 L

   !G21C17/08

【請求項の数】6

【全頁数】16

(21)【出願番号】特願2017-236066(P2017-236066)

(22)【出願日】2017年12月8日

(65)【公開番号】特開2019-101001(P2019-101001A)

(43)【公開日】2019年6月24日

【審査請求日】2020年7月27日

(73)【特許権者】

【識別番号】507250427

【氏名又は名称】日立ＧＥニュークリア・エナジー株式会社

(74)【代理人】

【識別番号】110001807

【氏名又は名称】特許業務法人磯野国際特許商標事務所

(72)【発明者】

【氏名】小林 亮介

【審査官】 ▲高▼場 正光

(56)【参考文献】

【文献】 特開２０１５−１７５８３１（ＪＰ，Ａ）

【文献】 特開２０１７−２０６１３１（ＪＰ，Ａ）

【文献】 特開昭６１−１５９１１５（ＪＰ，Ａ）

【文献】 特開２０１５−２２１０９４（ＪＰ，Ａ）

【文献】 特開２０１５−１３７９６８（ＪＰ，Ａ）

【文献】 特開２０１０−２０３８８８（ＪＰ，Ａ）

【文献】 独国特許出願公開第１９８１８４６０（ＤＥ，Ａ１）

【文献】 特表２００１−５１７２９７（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｓ ７／４８ − Ｇ０１Ｓ ７／６４

Ｇ０１Ｓ １５／００ − Ｇ０１Ｓ １７／９５

Ｇ０１Ｈ ５／００

Ｇ２１Ｃ １７／００ − Ｇ２１Ｃ １７／１４

(57)【特許請求の範囲】

【請求項1】

水面に浮上している水中移動体の気中から所定の構造物の気中箇所に対して光学式の計測信号を照射することにより求まる前記所定の構造物までの距離情報と、前記水面に浮上している水中移動体の水中から前記所定の構造物の水中箇所に対して音響式の計測信号を照射してからその反射信号を受信するまでに要した時間情報とを元に求まる、水中における前記音響式の計測信号の伝搬速度である水中音速情報を記憶する音速情報記憶部と、
前記音響式の計測信号を照射することで前記水中移動体の周囲に形成されるスキャン平面の計測結果に前記音速情報記憶部の前記水中音速情報を反映して計測断面データを作成し、その作成した前記計測断面データと、前記水中移動体の現在位置の候補となる調査空間内の設計断面データと照合することにより、前記水中移動体の現在位置を求める移動体位置算出部とを有することを特徴とする
水中移動体の位置推定装置。

【請求項2】

前記移動体位置算出部は、さらに、既に求めた現在位置における前記調査空間内の仮想カメラ画像と、前記水中移動体の気中を撮影した実撮像画像とを比較して、両画像内の撮影物のずれ量をもとに前記既に求めた現在位置を補正することを特徴とする
請求項１に記載の水中移動体の位置推定装置。

【請求項3】

前記移動体位置算出部は、照合対象とする前記計測断面データおよび前記設計断面データそれぞれについて、前記水中移動体の圧力センサから求めた水中における前記水中移動体の深度情報と、前記水中移動体のジャイロセンサから求めた水中における前記水中移動体の姿勢角情報とを反映した断面データを用いることを特徴とする
請求項１に記載の水中移動体の位置推定装置。

【請求項4】

前記音速情報記憶部には、前記光学式の計測信号を照射するレーザセンサまたはカメラにより求まる前記所定の構造物までの前記距離情報を用いて計算された前記水中音速情報が記憶されることを特徴とする
請求項１に記載の水中移動体の位置推定装置。

【請求項5】

前記音速情報記憶部には、前記音響式の計測信号を照射する超音波センサから求まる前記時間情報を用いて計算された前記水中音速情報が記憶されることを特徴とする
請求項１に記載の水中移動体の位置推定装置。

【請求項6】

水中移動体の位置推定装置は、音速情報記憶部と、移動体位置算出部とを有しており、
前記音速情報記憶部には、水面に浮上している前記水中移動体の気中から所定の構造物の気中箇所に対して光学式の計測信号を照射することにより求まる前記所定の構造物までの距離情報と、前記水面に浮上している水中移動体の水中から前記所定の構造物の水中箇所に対して音響式の計測信号を照射してからその反射信号を受信するまでに要した時間情報とをもとに求まる、水中における前記音響式の計測信号の伝搬速度である水中音速情報が記憶され、
前記移動体位置算出部は、前記音響式の計測信号を照射することで前記水中移動体の周囲に形成されるスキャン平面の計測結果に前記音速情報記憶部の前記水中音速情報を反映して計測断面データを作成し、その作成した前記計測断面データと、前記水中移動体の現在位置の候補となる調査空間内の設計断面データと照合することにより、前記水中移動体の現在位置を求めることを特徴とする
位置推定方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、位置推定装置、および、位置推定方法に関する。

【背景技術】

【0002】

３次元移動が可能な水中移動体の位置を検出する位置推定装置を水中移動体の内部に備えることにより、水中移動体が水中深く沈んで地上から目視できない場合でも、水中の移動制御を行いやすくすることができる。水中移動体は、例えば、３次元移動可能な水中検査装置を用いて原子炉内の構造物の検査作業を行う原子炉内検査システムに応用される。

【0003】

特許文献１には、水中移動体の水平位置を算出する位置推定装置の一例が記載されている。この位置推定装置は、任意の水平面で水中移動体から周囲の構造物までの距離をレーザ距離計や超音波距離計で計測することで当該水平面における当該構造物の形状を示す画像データ（測定画像）を作成する。次に、位置推定装置は、当該構造物の設計情報などから得られる当該水平面における当該構造物の形状を示す画像データであって、水平位置情報が付されている画像データ（記憶画像）を測定画像とマッチングすることで、自身の水平位置を特定する。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２０１０−２０３８８８号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

しかし、特許文献１に係る位置推定装置は、水中環境として清水環境下での利用を想定しており、濁水環境下での利用には向いていない。
まず、清水環境下では、水中でも視界が良好なため、レーザやカメラなどの光学センサが位置推定手段として利用できる。一方、濁水環境下では、光の吸収や散乱などの影響で減衰が生じるため、光学センサでは構造物までの距離を計測できない。

【0006】

別の位置推定手段として、光学センサを用いる代わりに、超音波距離計を用いる例を検討する。超音波距離計では視界が悪い濁水環境下も超音波信号の影響は少ない。しかし、超音波信号では、減衰の影響を排除できるが、周辺構造物からの多重反射の影響や水温変化による音速変化の影響により距離計測誤差が発生し、位置推定精度が低下する恐れがある。

【0007】

そこで、本発明は、濁水環境下での水中移動体の推定位置を高精度に求めることを、主な課題とする。

【課題を解決するための手段】

【0008】

前記課題を解決するために、本発明の位置推定装置は、以下の特徴を有する。
本発明は、水面に浮上している水中移動体の気中から所定の構造物の気中箇所に対して光学式の計測信号を照射することにより求まる前記所定の構造物までの距離情報と、前記水面に浮上している水中移動体の水中から前記所定の構造物の水中箇所に対して音響式の計測信号を照射してからその反射信号を受信するまでに要した時間情報とを元に求まる、水中における前記音響式の計測信号の伝搬速度である水中音速情報を記憶する音速情報記憶部と、
前記音響式の計測信号を照射することで前記水中移動体の周囲に形成されるスキャン平面の計測結果に前記音速情報記憶部の前記水中音速情報を反映して計測断面データを作成し、その作成した前記計測断面データと、前記水中移動体の現在位置の候補となる調査空間内の設計断面データと照合することにより、前記水中移動体の現在位置を求める移動体位置算出部とを有することを特徴とする。
その他の手段は、後記する。

【発明の効果】

【0009】

本発明によれば、濁水環境下での水中移動体の推定位置を高精度に求めることができる。

【図面の簡単な説明】

【0010】

【図1】本発明の一実施形態に関する水中調査システムの側面図である。

【図2】本発明の一実施形態に関する水中調査装置の斜視図である。

【図3】本発明の一実施形態に関する制御装置の構成図である。

【図4】本発明の一実施形態に関するモニタの表示画面図である。

【図5】本発明の一実施形態に関する制御装置による調査工程のメイン処理を示すフローチャートである。

【図6】本発明の一実施形態に関する図５の調査工程が行われたときの水中調査システムの側面図である。

【図7】本発明の一実施形態に関するキャリブレーション処理の概要を示す側面図である。

【図8】本発明の一実施形態に関するキャリブレーション処理の詳細を示すフローチャートである。

【図9】本発明の一実施形態に関する水中調査装置の操作と移動中の位置を推定する処理の詳細を示すフローチャートである。

【図10】本発明の一実施形態に関する水中調査装置の推定位置補正処理の詳細を示すフローチャートである。

【図11】本発明の一実施形態に関する水中調査装置の推定位置補正処理が行われる具体例を示す側面図である。

【図12】本発明の一実施形態に関する図１１の推定位置の補正前後それぞれで用いられる撮像画像の一例を示す画像図である。

【発明を実施するための形態】

【0011】

以下、本発明の一実施形態を、図面を参照して詳細に説明する。

【0012】

図１は、水中調査システムの側面図である。なお、図１などの側面図は、水中調査システムの立体構造をＹ軸（Ｙ＋軸、Ｙ−軸）と、Ｚ軸（Ｚ＋軸、Ｚ−軸）とからみたときの図である。これらの軸の定義は、図２の説明で明らかにする。

【0013】

この水中調査システムが適用される建屋１には、調査対象である水槽２が設置されている。建屋１の上部には作業員８の作業スペースであるフロア９がある。なお、水中調査装置３は、図１に示した建屋１内に設置された水槽２内の原子炉内検査だけでなく、ダムや河川の水中での構造物検査などの濁水環境下での水中移動体の位置推定に広く適用可能である。つまり、水槽２の外であるフロア９にいる作業員が水中調査装置３を直接目視できない濁水環境下でも、水中調査装置３の位置を把握することができる。

【0014】

水中調査システムは、水中調査装置３と、ケーブル４と、制御装置５と、モニタ６と、コントローラ７とを含めて構成される。
水中調査装置３は、水槽２内の壁面や構造物の目視検査に用いる水中移動体であり、その水中移動体自身の位置を推定するための位置推定用の各種デバイスを内部に備えている（詳細は図２）。

【0015】

制御装置５は、ケーブル４を介して水中調査装置３に接続される。制御装置５は、コンピュータであり、ハードウェアとして、各種プログラムを実行するための演算手段としてのＣＰＵ（Central Processing Unit）などの演算処理装置と、当該プログラムをはじめ各種データを記憶するための記憶手段としての記憶装置と、各装置と水中調査装置３に係る各センサなどへのデータおよび指示などの入出力制御を行うための入出力装置を備えている。記憶装置は、例えば、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）およびフラッシュメモリなどの半導体メモリや、ハードディスクドライブなどの磁気記憶装置として構成される。

【0016】

モニタ６は、制御装置５に接続され、水中調査装置３のカメラ画像を表示するとともに水中調査装置３の位置や姿勢などを表示する。
コントローラ７は、制御装置５に接続され、作業員８に水中調査装置３を操作させるために使用される。水槽２内の壁面や構造物の目視検査作業を行う場合、フロア９上の作業員８は、水槽２内に水中調査装置３を投入し、この水中調査装置３の位置や姿勢をモニタ６で確認しつつ、コントローラ７を操作する。

【0017】

図２は、水中調査装置３の斜視図である。なお、本明細書での座標系は、水中調査装置３の本体をもとにした、以下の右手座標系として定義される。
Ｘ軸は、水中調査装置３からみて左右の（筐体側面の）方向であり、右方をプラス側（Ｘ＋軸）とし、左方をマイナス側（Ｘ−軸）とする。
Ｙ軸は、水中調査装置３からみて前後の方向であり、前方をプラス側（Ｙ＋軸）とし、後方をマイナス側（Ｙ−軸）とする。
Ｚ軸は、水中調査装置３からみて垂直の（高さの）方向であり、下方をプラス側（Ｚ＋軸）とし、上方をマイナス側（Ｚ−軸）とする。
以上の３軸は、互いに直交しているＸＹＺ座標系である。なお、以下の説明では、ＸＹ平面上での水中調査装置３の位置を「水平位置」とし、Ｚ軸での水中調査装置３の位置を「垂直位置」とする。例えば、水中調査装置３が前方に１ｍ進んだ場合は水平位置は変化するが、垂直位置は変化しない。別の例で、水中調査装置３が下方に１ｍ沈んだ場合は水平位置は変化しないが、垂直位置は変化する。

【0018】

水中調査装置３は、水中調査用カメラ１０と、画像取込部１１と、調査装置側信号伝送部１２と、慣性センサ部１３と、超音波距離計１７（音響式相対距離検出器）と、気中環境撮像用カメラ１８と、レーザ距離計１９（光学式相対距離検出器）と、圧力センサ２０と、スラスタ２１とを備える。
スラスタ２１は、上方（Ｚ−軸）、後方（Ｙ−軸）、左方（Ｘ−軸）にそれぞれ設けられた推進機構である。３つのスラスタ２１は、それぞれ、スクリューと、スクリューを正回転又は逆回転に駆動するモータ（図示せず）で構成されている。スラスタ２１は、水中調査装置３に対して上下方向（Ｚ軸）の推力、前後方向（Ｙ軸）の推力、および、左右方向（Ｘ軸）の推力をそれぞれ付与する。すなわち、このスラスタ２１により、水中調査装置３は水で満たされた３次元空間を自在に移動可能となっている。

【0019】

慣性センサ部１３は、水中調査装置３の姿勢（姿勢角）を検出するための姿勢角検出器であり、慣性センサ部１３は、３軸ジャイロ１４と、地磁気センサ１５と、傾斜計１６とを備える。
３軸ジャイロ１４は、Ｘ軸、Ｙ軸、およびＺ軸周りの角速度をそれぞれ検出する角速度検出器である。
地磁気センサ１５は、Ｚ軸周りの角度（方位角）を検出する方位角検出器である。
傾斜計１６は、Ｘ軸およびＹ軸周りの角度（傾斜角）を検出する傾斜角検出器である。

【0020】

圧力センサ２０は、水中調査装置３に作用する水圧を検出する垂直位置検出器であり、水中調査装置３の底面から外部に露出したセンサ部を有している。このセンサ部に作用する水圧を検出することで圧力を検出している。なお、圧力センサ２０の設置の有無は、センサ部を外部から視認することで容易に確認できる。圧力センサ２０で検出された圧力は、水槽２内における水中調査装置３の垂直位置（Ｚ軸深度）の検出に用いられる。

【0021】

水中調査用カメラ１０は、本体の前方（Ｙ＋軸）に設けられ水槽２内の壁面や構造物などを撮像する。画像取込部１１は、水中調査用カメラ１０の画像を電子情報化する。
気中環境撮像用カメラ１８は、水中調査装置３が水面上にいるときに気中部を撮像する。

【0022】

レーザ距離計１９は、本体上部に取り付けられた光学式の相対距離検出器であり、例えば、レーザセンサまたはカメラである。レーザ距離計１９は、それぞれ、レーザを投光する投光部と、投光したレーザを受光する受光部を有している。投光部は、走査装置によってＺ軸周りに回転され、同一平面上（ここでは水平面上となる）にレーザを走査する。投光部より投光および走査されたレーザは周囲の構造物などに反射して受光部で受光される。レーザ距離計１９は、レーザの投光時刻からその反射光の受光時刻までのレーザ飛行時間に基づいて周囲の構造物までの相対距離を測定している。

【0023】

超音波距離計１７は、本体下部（底面）に取り付けられた音響式の相対距離検出器であり、例えば、超音波センサである。超音波距離計１７も、基本原理はレーザ距離計１９と同様であり、超音波の送信時間と受信時間の差と超音波が伝播する媒質（水）の音速から、構造物までの相対距離を算出する。
調査装置側信号伝送部１２は、ケーブル４を介して、圧力センサ２０、慣性センサ部１３（３軸ジャイロ１４、傾斜計１６、地磁気センサ１５）、超音波距離計１７、レーザ距離計１９からの検出信号と、画像取込部１１からの画像信号とを制御装置５に出力する。

【0024】

制御装置５は、調査装置側信号伝送部１２からの検出信号などに基づいて水中調査装置３の位置や姿勢を算出し、この算出した水中調査装置３の位置や姿勢をモニタ６に出力して表示している。また、制御装置５は、調査装置側信号伝送部１２からの画像信号をモニタ６に出力して、水中調査用カメラ１０の画像を表示している。
さらに、制御装置５は、コントローラ７からの操作信号に応じてスラスタ２１を駆動制御する制御信号を生成し、この生成した制御信号をケーブル４および調査装置側信号伝送部１２を介してスラスタ２１に出力している。

【0025】

図３は、制御装置５の構成図である。制御装置５は、水中調査装置３の位置および姿勢を算出するために、角速度算出部２２と、角度算出部２３と、姿勢角算出部２４と、垂直位置算出部２５と、音速補正処理部２６と、音速情報記憶部２７と、水平位置算出部２８と、設計情報記憶部２９と、位置・姿勢記憶部３０とを有する。

【0026】

角速度算出部２２は、３軸ジャイロ１４の角速度信号に基づきＸ軸、Ｙ軸、およびＺ軸周りの角速度をそれぞれ算出する。
角度算出部２３は、傾斜計１６の角度信号に基づきＸ軸およびＹ軸周りの傾斜角をそれぞれ算出するとともに、地磁気センサ１５の角度信号に基づきＺ軸周りの方位角を算出する。
姿勢角算出部２４は、角速度算出部２２が算出した角速度と、角度算出部２３が算出した傾斜角および方位角とに基づき、水中調査装置３の姿勢角（３軸周りの姿勢角）を算出する。
垂直位置算出部２５は、圧力センサ２０の圧力信号に基づき水中における水中調査装置３の深度（垂直位置）を算出する。

【0027】

音速補正処理部２６は、レーザ距離計１９と超音波距離計１７で検出された水中調査装置３の位置と、その水中調査装置３の周辺に位置する構造物データの位置との相対距離に基づいて、超音波の音速を補正する（詳細は図７）。
音速情報記憶部２７は、音速補正処理部２６の結果を保存するための記憶部であり、音速補正処理部２６により補正された超音波の水中音速情報（以下、「補正音速」）が、補正時点の垂直位置に対応付けて保存される。

【0028】

水平位置算出部２８は、計測断面データと設計断面データとのマップマッチング処理により、水中調査装置３の水平位置を算出する。
計測断面データは、水中調査装置３の周辺についての超音波距離計１７の計測データを、音速情報記憶部２７から読み出した補正音速を反映して断面化したデータである。
設計断面データは、設計情報記憶部２９の構造物設計データの各位置について、仮想空間から断面化されたデータである。なお、計測断面データおよび設計断面データは、それぞれ姿勢角算出部２４が出力する姿勢角と、垂直位置算出部２５が出力する垂直位置とを反映してもよい。
設計情報記憶部２９は、調査環境中の構造物設計データとして、各構造物の設置位置データを保存するための記憶部である。

【0029】

位置・姿勢記憶部３０は、姿勢角算出部２４が出力する姿勢角と、垂直位置算出部２５が出力する垂直位置と、水平位置算出部２８が出力する水平位置とを保存するための記憶部である。位置・姿勢記憶部３０に保存された各情報は、水中調査装置３の現在状態を示す情報としてモニタ６に送信され、画像取込部１１で電子情報化された目視検査用の水中調査用カメラ１０の映像とともに表示される。

【0030】

図４は、モニタ６の表示画面図である。表示画面１２０は、位置座標表示欄９５と、水平位置画像表示欄９６と、カメラ映像表示欄９９を有している。

【0031】

位置座標表示欄９５には、制御装置５の位置・姿勢記憶部３０から読み込んだ水中調査装置３の現在位置が例えば絶対位置として表示される。水平位置画像表示欄９６には、水中調査装置３が位置する垂直位置（ＸＹ座標）における水槽２内の水平断面画像とともに、水中調査装置３の水平位置を示すマーカ９４が表示される。

【0032】

水平位置画像表示欄９６における水槽２内の水平断面画像は、例えば、制御装置５における設計情報記憶部（図示せず）に記憶されている水槽２の形状データ（例えば、ＣＡＤデータ）と、垂直位置算出部２５で算出された水中調査装置３の垂直位置とを利用して描かれている。この水平断面画像は、水中調査装置３の垂直方向の移動に追従して随時変化する。
また、水平位置画像表示欄９６には、水中調査装置３の投入位置（初期位置）をマーカ９７でマークする機能や、水中調査装置３の移動軌跡９８を表示又は非表示する機能が具備されている。

【0033】

カメラ映像表示欄９９は、水中調査装置３に搭載された水中調査用カメラ１０の映像が表示される部分である。
なお、モニタ６は、図示しない他の表示画面に切り換えられるようになっており、位置・姿勢記憶部３０から読み込んだ水中調査装置３の姿勢なども表示されるようになっている。このように構成された表示画面１２０によれば、作業員８は水槽２内のどこに水中調査装置３があるかを視覚的に把握しながら検査することができる。

【0034】

このように、作業員８が自身の立ち位置からは目視できない各種情報を、表示画面１２０を介して的確に把握させるためには、水中調査装置３の現在状態（位置・姿勢記憶部３０内の姿勢角と、垂直位置と、水平位置）が高精度に求まっている必要がある。そこで、図５からの説明では、水中調査装置３の現在状態を高精度に求める具体的な処理内容を説明する。

【0035】

図５は、制御装置５による調査工程のメイン処理を示すフローチャートである。
Ｓ１１は、超音波距離計１７のキャリブレーション処理である。このキャリブレーション処理は、水中調査装置３が水面に浮上し、レーザ距離計１９が気中部、超音波距離計１７が水中部に位置するときに実行される。
Ｓ１２は、水中調査装置３の操作処理と、その操作に従って移動中の水中調査装置３の位置推定処理である。作業員８がコントローラ７を操作して、水中部の水中調査装置３を操縦する。
Ｓ１３は、水中調査装置３の推定位置の補正処理である。この補正処理は、水中調査装置３が水面に浮上し、気中環境撮像用カメラ１８が気中部に位置するときに実行される。

【0036】

図６は、図５の調査工程が行われたときの水中調査システムの側面図である。
位置４０１において、調査工程が開始されると、水中調査装置３が水面に浮上した状態で超音波距離計１７のキャリブレーション処理（Ｓ１１）が実行される。
位置４０１から位置４０２までは、水中調査装置３の操作処理（Ｓ１２）により、水中調査装置３が水中部を移動（潜行）する。そして、位置４０２において、水中調査装置３の推定位置の累積誤差が増加してきたので、水中調査装置３は水面に向かって浮上を開始する。
位置４０３において、水中調査装置３が水面に浮上した状態で水中調査装置３の推定位置の補正処理（Ｓ１３）が実行される。これにより、推定位置の累積誤差がリセットされ、水中調査装置３の位置精度が回復する。

【0037】

位置４０３から位置４０４までは、水中調査装置３の操作処理（Ｓ１２）により、再び、水中調査装置３が水中部を移動（潜行）する。位置４０５において、再び水中調査装置３の推定位置の補正処理（Ｓ１３）が実行される。
そして、位置４０５から位置４０６まで移動することで、水中調査装置３は構造物の亀裂などの欠陥箇所４１０を発見することで、調査工程を終了させる。
このように、Ｓ１１，Ｓ１２，Ｓ１３は繰り返し実行される。なお、浮上位置４０３，４０５においても、初回の浮上位置４０１と同様に、キャリブレーション処理（Ｓ１１）を実行してもよい。

【0038】

図７は、キャリブレーション処理（Ｓ１１）の概要を示す側面図である。
まず、音速補正処理部２６は、レーザ距離計１９から周囲の壁面などの構造物（の気中箇所）に対して、レーザ信号（第１測定用信号）を発射してから、その反射信号を受信するまでの時間をもとに気中距離Ｌａを求める。
次に、音速補正処理部２６は、水槽２の設計データ（例えば、壁面の曲率から求めた気中の壁面位置と水中の壁面位置との差分Ｌｃ）と、水中調査装置３の設計データ（例えば、レーザ距離計１９の位置と超音波距離計１７の位置との差分Ｌｄ）とをもとに、気中距離Ｌａを水中距離Ｌｂに修正する。なお、凹凸のない垂直な壁面の形状を頼りにキャリブレーション処理を行う場合は、差分Ｌｃ＝０である。

【0039】

そして、音速補正処理部２６は、超音波距離計１７から周囲の壁面などの構造物（の水中箇所）に対して、超音波信号（第２測定用信号）を発射してから、その反射信号を受信するまでの音の伝搬時間（受信時刻−送信時刻）と、水中距離Ｌｂとをもとに、超音波信号の水中環境下での音速情報を求める。水中環境下での音速情報は、気中環境下での音速情報を水中環境下で補正した音速情報とも言える。
このように、気中部と水中部で相対距離が異なる場合については、レーザ距離計１９と超音波距離計１７の水中調査装置３への設置位置関係を考慮して、レーザ距離計１９の計測値を超音波距離計１７の計測面での計測値になるように音速情報を求めればよい。

【0040】

図８は、音速補正処理部２６が実行する超音波距離計１７のキャリブレーション処理（Ｓ１１）の詳細を示すフローチャートである。
Ｓ１０１は、水中調査装置３の初期位置入力処理である。
Ｓ１０２は、レーザ距離計１９による構造物までの距離計測処理であり、気中部周辺構造物までの相対距離Ｌａを計測する。
Ｓ１０３は、超音波距離計１７による構造物までの距離計測処理であり、水中部周辺構造物までの相対距離Ｌｂを計測する。
Ｓ１０４は、超音波距離計１７の音速補正処理である。図７で説明したように、音速補正処理部２６は、レーザ距離計１９で計測した相対距離Ｌａを真値として、超音波距離計１７で計測した距離が実際の距離Ｌｂと一致するように音速を逆算する。
Ｓ１０５は、Ｓ１０４で計算した音速補正情報を音速情報記憶部２７に保存する処理である。

【0041】

図９は、水中調査装置３の操作と移動中の位置を推定する処理（Ｓ１２）の詳細を示すフローチャートである。
Ｓ２０１は、水平位置算出部２８へのデータ入力処理である。水平位置算出部２８は、設計情報記憶部２９から調査環境の構造物設計情報を読み込み、音速情報記憶部２７から音速補正情報を読み込む。

【0042】

Ｓ２０２は、姿勢角算出部２４が慣性センサ（３軸ジャイロ１４、地磁気センサ１５、傾斜計１６）の計測結果から、水中調査装置３の姿勢角を算出する処理である。Ｓ２０２の詳細を、以下の手順（１ａ）〜（５ａ）の順に説明する。
手順（１ａ）として、角速度算出部２２は３軸ジャイロ１４の角速度信号を取り込み、角度算出部２３は傾斜計１６および地磁気センサ１５の角度信号を取り込む。

【0043】

手順（２ａ）として、角速度算出部２２は、３軸ジャイロ１４の角速度信号から各軸（Ｘ軸、Ｙ軸、Ｚ軸）周りの角速度を算出する。
３軸ジャイロ１４は、静電浮上型ジャイロであり、角速度に比例する増減値が基準電圧（一定の電圧値）に加えられた正の電圧値を出力する。そのため、まず３軸ジャイロ１４の各軸（Ｘ軸、Ｙ軸、Ｚ軸）周りの測定信号に対し基準電圧を減じる基本処理を行う。
ここで、基準電圧は、通常、３軸ジャイロ１４の固有スペックとして示されているが、本実施形態では、角速度信号が入力されないときの電圧値をあらかじめ計測して平均化したものを用いる。
その後、角速度算出部２２は、基本処理がなされた測定信号に対して、電圧−角速度換算係数（３軸ジャイロ１４の固有のスペックとして示される一定値）を乗じることで、各軸周りの角速度を算出する。

【0044】

手順（３ａ）として、角度算出部２３は、傾斜計１６の角度信号から各軸（Ｘ軸、Ｙ軸）周りの傾斜角を算出する。本実施の形態の傾斜計１６は、封入された電解液の液面変化（Ｘ軸およびＹ軸周りの傾斜角）を電圧変化に変換して出力するものである。そのため、まず各軸（Ｘ軸、Ｙ軸）周りの測定信号から基準電圧（傾斜計１６の固有スペックとして示される一定の電圧値）を減じる基本処理を行う。
その後、角度算出部２３は、基本処理がなされた測定信号に対して、傾斜角換算係数（傾斜計１６の固有スペックとして示される一定値）を乗じることで、各軸周りの傾斜角を算出する。

【0045】

手順（４ａ）として、角度算出部２３は、地磁気センサ１５の角度信号からＺ軸周りの方位角を算出する。本実施の形態の地磁気センサ１５は、Ｘ軸方向およびＹ軸方向に感度を有するホール素子で捉えた磁力を出力するものである。そのため、まずＸ軸およびＹ軸の地磁気信号から基準電圧を減じ、ゲインを乗じる基本処理を行う。ここで、基準電圧およびゲインは、地磁気センサ１５を使用する環境により異なるため、あらかじめ使用する領域で測定したものを用いる。
その後、角度算出部２３は、基本処理がなされたＸ軸およびＹ軸の測定信号を用い、Ｚ軸周りの方位角を算出する。

【0046】

手順（５ａ）として、姿勢角算出部２４は、手順（２ａ）〜（４ａ）で求めた各パラメータをカルマンフィルタ（この種のものとして公知のもの）に入力し、水中調査装置３の姿勢角（３軸周りの姿勢角）の最適値を推定する。入力される各パラメータとは、Ｘ軸、Ｙ軸、およびＺ軸周りの角速度、Ｘ軸およびＹ軸周りの傾斜角、Ｚ軸周りの方位角である。
その後、姿勢角算出部２４は、手順（５ａ）で推定した水中調査装置３の姿勢角を位置・姿勢記憶部３０に記憶する。

【0047】

Ｓ２０３は、圧力センサ２０の計測結果から水中調査装置３の垂直位置（上下位置）を算出する処理である。Ｓ２０３の詳細を、以下の手順（１ｂ）〜（３ｂ）の順に説明する。
手順（１ｂ）として、垂直位置算出部２５は、まず圧力センサ２０の圧力信号（検出電圧Vp）を取り込む。そして、垂直位置算出部２５は、検出電圧Vpから基準電圧Vp_baseを減じた結果に、圧力換算係数Kv_pを乗じて圧力Ｐを算出する。
基準電圧Vp_baseは、圧力センサ２０の固有スペックとして示される一定の電圧値である。
圧力換算係数Kv_pは、圧力センサ２０の固有スペックとして示される一定値である。

【0048】

手順（２ｂ）として、垂直位置算出部２５は、算出した圧力Ｐと水槽２内の水の密度ρと重力加速度ｇとを用い、水中調査装置３の深度Ｈを算出する。そして、垂直位置算出部２５は、算出した深度Ｈに例えばフロア９から水面までの距離を加えて、水中調査装置３の垂直位置とする。
手順（３ｂ）として、垂直位置算出部２５は、算出した水中調査装置３の垂直位置を位置・姿勢記憶部３０に記憶する。

【0049】

Ｓ２０４は、超音波距離計１７の計測結果から構造物形状を算出する処理である。水平位置算出部２８は、超音波距離計１７で計測した構造物までの相対距離情報（走査角度と距離）を、Ｓ２０１で取得した音速補正情報を用いて補正する。
そして、水平位置算出部２８は、補正した相対距離情報をスキャン平面上において構造物の表面（外形）が位置する座標値Ｌ（ｘＬ，ｙＬ）へ変換することで、構造物形状の計測断面データを作成する。計測断面データは、例えば、水中調査装置３周囲のＸＹ平面をスキャン平面とし、立体的な構造物形状（例えば、配管）がスキャン平面に沿って切り出されたものである（例えば、配管の断面であるだ円形状）。

【0050】

Ｓ２０５は、設計情報の設計断面データと、構造物形状の算出結果（計測断面データ）とから、調査装置の水平位置を算出する処理である。水平位置算出部２８は、水中調査装置３の垂直位置および姿勢角に基づいて、設計情報記憶部２９内に記憶されている設計情報（仮想空間）上で、水中調査装置３の水平位置の候補ごとの設計断面データを作成する。この作成される設計断面データは、水中調査装置３の垂直位置および姿勢角が反映される。
そして、水平位置算出部２８は、特許文献１などに記載される断面形状データ間のマップマッチング法を利用して、設計断面データと計測断面データとを照合する。この照合処理により、計測断面データにマッチングする設計断面データの水平位置を、水中調査装置３の水平位置として採用する。

【0051】

Ｓ２０６として、水平位置算出部２８は、Ｓ２０２で求めた水中調査装置３の姿勢角と、Ｓ２０３で求めた水中調査装置３の上下位置と、Ｓ２０６で求めた水中調査装置３の水平位置とを位置・姿勢記憶部３０に保存する。

【0052】

図１０は、水中調査装置の推定位置補正処理（Ｓ１３）の詳細を示すフローチャートである。
Ｓ３０１は、気中環境撮像用カメラ１８の画像（実撮像画像）と、水中調査装置３の現在位置・姿勢（推定値）とを水平位置算出部２８に入力する処理である。
Ｓ３０２は、水平位置算出部２８が、Ｓ３０１の現在位置・姿勢における撮像画像を、設計情報を用いて推定する処理である。水平位置算出部２８は、設計情報記憶部２９の設計情報上で、水中調査装置３の推定位置・姿勢における仮想カメラ画像（推定撮像画像）を生成する。なお、設計情報上の仮想カメラの画角は、気中環境撮像用カメラ１８に設定された画角と同じ値を設定すればよい。
Ｓ３０３は、水平位置算出部２８が、Ｓ３０２の推定撮像画像とＳ３０１の実撮像画像とを比較して、画像内のずれ量（画素値）を算出する処理である。

【0053】

Ｓ３０４は、水平位置算出部２８が、Ｓ３０３で算出されたずれ量をＳ３０１の現在位置・姿勢へ反映することで、Ｓ３０１の推定値を補正する処理である。
Ｓ３０５は、水平位置算出部２８が、Ｓ３０４で補正された水中調査装置３の位置・姿勢補正結果を、現在位置・姿勢として位置・姿勢記憶部３０に保存する処理である。

【0054】

図１１は、図１０は、水中調査装置の推定位置補正処理が行われる具体例を示す側面図である。水中調査装置３の水平位置が、補正前の位置５０１から補正後の位置５０２へ、距離Ｌｐ分だけ前方（Ｙ＋方向）に補正されている。なお、この補正処理は、気中環境撮像用カメラ１８の撮像画像に撮影される２つの特徴点（建屋１の天井の照明器具５０８，５０９）が手がかりとなった。

【0055】

図１２は、図１１の推定位置の補正前後それぞれで用いられる撮像画像の一例を示す画像図である。
撮像画像５１０は、補正前の位置５０１を撮影位置としたＳ３０２の仮想カメラ画像である。撮像画像５２０は、補正後の位置５０２を撮影位置としたＳ３０３の実撮像画像である。
そして、２つの撮像画像５１０には、ともに照明器具５０８，５０９が撮影されているが、画像内の撮影された位置が互いに異なる。撮像画像５１０内では、照明器具５０８が画像内位置５１１に撮影され、照明器具５０９が画像内位置５１２に撮影されている。撮像画像５２０内では、照明器具５０８が画像内位置５２３に撮影され、照明器具５０９が画像内位置５２４に撮影されている。

【0056】

制御装置５は、撮像画像５１０，５２０間で、互いに対応する特徴点（照明器具５０８，５０９）のずれ量Ｌｑを算出する（Ｓ３０３）。例えば、撮像画像５２０内では、照明器具５０８の位置が、画像内位置５２１（画像内位置５１１と同じ位置）から右側にずれ量Ｌｑだけずれた画像内位置５２３にずれている。照明器具５０９の位置も、画像内位置５２２（画像内位置５１２と同じ位置）から右側にずれ量Ｌｑだけずれた画像内位置５２４にずれている。
制御装置５が、画像内のずれ量Ｌｑから建屋１内のずれ量Ｌｐを計算し、そのずれ量Ｌｐを現在位置５０１に反映することで、補正後の現在位置５０２を求める（Ｓ３０４）。つまり、画像内で右側にずれているということは、水中調査装置３は実際には逆側である左側に存在するはずである。

【0057】

以上説明した本実施形態では、３次元移動可能な水中移動体の位置を推定する制御装置５を説明した。水中調査装置３に搭載された位置測定用の各種センサを用いて水中調査装置３の位置を推定するときに、各種センサの得手不得手の状況をあらかじめ考慮し、各種センサの測定データを状況に応じて組み合わせることで、総合的な水中調査装置３の位置精度を向上させることが、本実施形態の主な特徴である。

【0058】

具体的には、気中で高精度な測定が可能な気中環境撮像用カメラ１８やレーザ距離計１９を用いるときには、水中調査装置３を浮上させる（図５のＳ１１，Ｓ１３）。一方、濁水環境下でも精度劣化が発生しづらい超音波距離計１７を用いることで、水中調査装置３の水中時の位置を推定する（図５のＳ１２）。この水中時の位置を推定する処理において、超音波距離計１７のキャリブレーション処理（Ｓ１１）で求めた音速補正情報を用いることで、濁水環境下での推定精度を向上させている。

【0059】

なお、本発明は前記した実施例に限定されるものではなく、様々な変形例が含まれる。例えば、前記した実施例は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。
また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能であり、また、ある実施例の構成に他の実施例の構成を加えることも可能である。
また、各実施例の構成の一部について、他の構成の追加・削除・置換をすることが可能である。また、上記の各構成、機能、処理部、処理手段などは、それらの一部または全部を、例えば集積回路で設計するなどによりハードウェアで実現してもよい。
また、前記の各構成、機能などは、プロセッサがそれぞれの機能を実現するプログラムを解釈し、実行することによりソフトウェアで実現してもよい。

【0060】

各機能を実現するプログラム、テーブル、ファイルなどの情報は、メモリや、ハードディスク、ＳＳＤ（Solid State Drive）などの記録装置、または、ＩＣ（Integrated Circuit）カード、ＳＤカード、ＤＶＤ（Digital Versatile Disc）などの記録媒体に置くことができる。
また、制御線や情報線は説明上必要と考えられるものを示しており、製品上必ずしも全ての制御線や情報線を示しているとは限らない。実際にはほとんど全ての構成が相互に接続されていると考えてもよい。
さらに、各装置を繋ぐ通信手段は、無線ＬＡＮに限定せず、有線ＬＡＮやその他の通信手段に変更してもよい。

【符号の説明】

【0061】

１ 建屋
２ 水槽
３ 水中調査装置（水中移動体）
４ ケーブル
５ 制御装置（位置推定装置）
６ モニタ
７ コントローラ
８ 作業員
９ フロア
１０ 水中調査用カメラ
１１ 画像取込部
１２ 調査装置側信号伝送部
１３ 慣性センサ部
１４ ３軸ジャイロ
１５ 地磁気センサ
１６ 傾斜計
１７ 超音波距離計
１８ 気中環境撮像用カメラ
１９ レーザ距離計
２０ 圧力センサ
２１ スラスタ
２２ 角速度算出部
２３ 角度算出部
２４ 姿勢角算出部
２５ 垂直位置算出部
２６ 音速補正処理部
２７ 音速情報記憶部
２８ 水平位置算出部（移動体位置算出部）
２９ 設計情報記憶部
３０ 位置・姿勢記憶部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】