(57)【特許請求の範囲】
【請求項１】
推進手段により水中を移動する水中航走体と、
前記水中航走体の前方に配置され、対象物を撮像する複眼カメラと、
前記推進手段及び前記複眼カメラを制御する制御手段と、備える水中航走体制御システムであって、
前記制御手段は、
前記複眼カメラにより撮像された画像が入力される画像入力部と、
前記対象物の特徴情報を含むモデルが記憶されるモデル記憶部と、
前記画像入力部に入力された入力画像について前記モデルとの適合度を求めるための所定の適合度関数を演算する適合度演算部と、
前記適合度演算部により求められる適合度に基づいて対象物の遺伝情報を生成する遺伝アルゴリズムを実行する遺伝アルゴリズム実行部と、有し、
前記遺伝アルゴリズム実行部は、
前記対象物に合致するように前記遺伝情報を所定時間において進化させて、前記対象物の位置及び姿勢を推定し、
前記制御手段は、
前記対象物の推定された位置及び姿勢と、前記対象物に対する前記水中航走体の相対的目標位置及び姿勢との誤差に基づくＰ制御によって計算された指令電圧値を前記推進手段に与えることで、前記水中航走体が前記相対的目標位置及び姿勢を保持するように前記推進手段の推力を発生させることを特徴とする水中航走体制御システム。
【請求項２】
前記複眼カメラは、広角レンズを有していることを特徴とする請求項１に記載の水中航走体制御システム。
【請求項３】
前記対象物は、該対象物の位置を示す立体的なマーカーを有することを特徴とする請求項１または請求項２に記載の水中航走体制御システム。
【請求項４】
前記マーカーは、発光するデバイスを備えていることを特徴とする請求項３に記載の水中航走体制御システム。
【請求項５】
前記マーカーは、音波発信源を備え、この音波発信源から発信された音波で前記水中航走体を前記マーカー方向に誘導することを特徴とする請求項３に記載の水中航走体制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、水中航走体制御システムに関する。
【背景技術】
【０００２】
従来、水中航走体（水中ロボット）の自律航行に関する研究はさまざまに行われている。水中ロボットの役割としては、例えば、水中汚染物（放射能汚染物等含む）の発見・回収、深海底地形調査、海底資源採集、機雷撤去、水産資源育成・捕獲、海難事故対応等さまざまなことが期待されている。
【０００３】
例えば、単眼カメラによる遠隔操縦型の水中ロボット（ＲＯＶ:Ｒｅｍｏｔｅｌｙ  Ｏｐｅｒａｔｅｄ  Ｖｅｈｉｃｌｅ）が公知技術としてあるが、単眼カメラによる水中ロボットは、カメラ視線奥行き方向の位置の認識精度がよくないため、魚などの認識対象物と水中ロボットの距離が不正確であった。また、それによって対象物の大きさが分からないなどの不便があった。同様に、単眼カメラによる水中作業型の自律ロボット（ＡＵＶ:Ａｕｔｏｎｏｍｏｕｓ  Ｕｎｄｅｒｗａｔｅｒ  Ｖｅｈｉｃｌｅ）も作業対象物をつかみ損なうなどの作業ミスが発生し、有効な作業を行うことはできなかった。現状、単眼カメラによる水中ロボットによって、対象物に対してカメラ視線奥行き方向の距離情報が必要な作業を行う場合、水中ロボットから送られてくる画像をもとに作業者（操縦者）が水中ロボットを遠隔操縦することによって水中ロボットの運動を制御しており、水中ロボットが自動的に作業を行うことはなかった。
【０００４】
また、単眼カメラではなく複眼カメラで対象物に対してカメラ視線奥行き方向の距離情報を得ることも考えられるが、複眼構成のカメラの画像認識では、３次元空間内の対象物上のある位置が左右のカメラ画像内のどの位置に対応しているのかについて判定し、対応点を確定する必要があった。この対応を間違えると対象物の認識誤差が発生し、正確な立体画像認識が困難となるため、立体画像認識結果に基づく制御の精度が悪化するという問題があった。この問題点は、左右両眼の２次元画像面に射影された情報をもとに３次元情報を復元しようとする試みが、起因しており２次元情報に縮退した情報から３次元情報を得ることの難しさでもあった。
【０００５】
本願発明者は、上記問題点を回避する方法として認識対象物の立体モデル形状をコンピュータ内に定義しこの形状を複眼カメラの撮像面に射影することで、２次元情報から３次元情報の復元ではなく、３次元情報形状と位置／姿勢情報をもとに２次元に射影された画像とを照合する方法を見出し、上記問題点を回避する立体画像認識方法をすでに提案している（非特許文献１参照）。
【０００６】
非特許文献１では、動画像中から対象物の位置／姿勢６変数を同時に認識することを目的として、モデルベーストマッチング法と遺伝的アルゴリズム（ＧＡ：Ｇｅｎｅｔｉｃ  Ａｌｇｏｒｉｔｈｍ）およびクォータニオンを用いた三次元位置／姿勢計測法を提案している。さらに，マニピュレータの手先にＣＣＤカメラが装着されるハンドアイマニピュレータ（アイ・イン・ハンドシステム）で、マニピュレータのダイナミクスが対象物認識に与える影響を解析し、これを抑えるためにＭＦＦ（Ｍｏｔｉｏｎ−Ｆｅｅｄｆｏｒｗａｒｄ）認識方法を提案し、その有効性を示している。非特許文献１記載のハンドアイマニピュレータは、地上等で主に用いられる、いわゆるロボットハンドとして構成されている。
【０００７】
これに対して、水中での正確な動作が求められる水中ロボットは、上記ロボットハンドとは異なり、その動作において水中特有の外乱を考慮しなければならない。例えば、Ｍｏｖｅ  ｏｎ  Ｓｅｎｓｉｎｇ制御知能を搭載したＡｕｔｏｎｏｍｏｕｓ  Ｕｎｄｅｒｗａｔｅｒ  Ｖｅｈｉｃｌｅ（ＭＯＳ／ＡＵＶ）により水中でさまざまな作業（自動充電、生魚捕獲・観察等）を行わせる場合、対象物形状の正確な認識とともに作業環境中の外乱（水の流れや障害物等の衝突）に抗するように対象物に対する正確な位置姿勢制御が自律的に行えなければならない。そのため、水中ロボットにおいて、対象物形状の正確な認識とともに対象物に対する正確な位置／姿勢制御を自律的に行うことができる技術が望まれている。
【先行技術文献】
【非特許文献】
【０００８】
【非特許文献１】宋薇，見浪護，青柳誠司．"クォータニオンを用いたフィードフォワードオンラインポーズ遺伝的認識法"，日本ロボット学会誌，Ｖｏｌ．２８，Ｎｏ．１，ｐｐ．５５−６４，２０１０．
【発明の概要】
【発明が解決しようとする課題】
【０００９】
そこで、本発明は、上記課題に鑑みてなされたものであり、対象物形状の正確な認識とともに対象物に対する正確な位置／姿勢制御を自律的に行うことができる水中航走体制御システムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
本発明者は、上記課題を解決するために鋭意研究し実験を重ねた結果、本発明を完成させるに至った。
【００１１】
即ち、本発明の水中走行体制御システムにおいては、
推進手段により水中を移動する水中航走体と、
前記水中航走体の前方に配置され、対象物を撮像する複眼カメラと、
前記推進手段及び前記複眼カメラを制御する制御手段と、備える水中航走体制御システムであって、
前記制御手段は、
前記複眼カメラにより撮像された画像が入力される画像入力部と、
前記対象物の特徴情報を含むモデルが記憶されるモデル記憶部と、
前記画像入力部に入力された入力画像について前記モデルとの適合度を求めるための所定の適合度関数を演算する適合度演算部と、
前記適合度演算部により求められる適合度に基づいて対象物の遺伝情報を生成する遺伝アルゴリズムを実行する遺伝アルゴリズム実行部と、有し、
前記遺伝アルゴリズム実行部は、
前記対象物に合致するように前記遺伝情報を所定時間において進化させて、前記対象物の位置及び姿勢を推定し、
前記制御手段は、
前記対象物の推定された位置及び姿勢と、前記対象物に対する前記水中航走体の相対的目標位置及び姿勢との誤差に基づくＰ制御によって計算された指令電圧値を前記推進手段に与えることで、前記水中航走体が前記相対的目標位置及び姿勢を保持するように前記推進手段の推力を発生させるものである。
【００１２】
また、本発明の水中走行体制御システムにおいては、
前記複眼カメラは、広角レンズを有しているものである。
【００１４】
また、本発明の水中走行体制御システムにおいては、
前記対象物は、該対象物の位置を示す立体的なマーカーを有するものである。
【発明の効果】
【００１５】
本発明によれば、対象物形状の正確な認識とともに対象物に対する正確な位置／姿勢制御を自律的に行うことができる。
【図面の簡単な説明】
【００１６】
【図１】（Ａ）本発明の実施例１に係る水中ロボットの外観を示す写真であり、ａ）正面、ｂ）側面、ｃ）上面、ｄ）背面を示す写真、（Ｂ）同じく水中航走体制御システムを模式的に示す図。
【図２】（Ａ）同じく水中ロボット及び水中実験装置の構成を示す写真、（Ｂ）水中航走体制御システムの構成を示すブロック図。
【図３】制御系のブロック図。
【図４】水中の対象物及びＧＡ探索エリアを示す図。
【図５】水中実験における座標系を示す図。
【図６】推力、トルク制御電圧の各初期特性、及び、不感帯特性を取り除いて調整された各初期特性、並びに、直線近似を示す図であり、（ａ）はｙ軸方向の初期特性、（ｂ）は不感帯特性を取り除いた特性（実線）とｙ軸方向の調整された特性（破線）、（ｃ）（ｄ）はｚ軸方向の特性、（ｅ）（ｆ）はｙ軸周りの特性を示す図。
【図７】外乱を付加しない場合のレギュレート性能を示す図であり、（ａ）は適合度の時間変化、（ｂ）はｘ軸方向の誤差、（ｃ）はｙ軸方向の誤差、（ｄ）はｚ軸方向の誤差、（ｅ）はｙ軸周りの誤差、（ｆ）は水中ロボットの３Ｄ軌跡、（ｇ）はｘ軸方向の推力、（ｈ）はｙ軸方向の推力、（ｉ）はｚ軸方向の推力、（ｊ）ｙ軸周りのトルクを示す図。
【図８】外乱を加えない場合のレギュレート性能を示す図（１０５［ｓ］から１１０［ｓ］の拡大図）であり、（ａ）はｘ軸方向の誤差、（ｂ）はｙ軸方向の誤差、（ｃ）はｚ軸方向の誤差、（ｄ）はｙ軸周りの誤差、（ｅ）はｘ軸方向の推力、（ｆ）はｙ軸方向の推力、（ｇ）はｚ軸方向の推力、（ｈ）はｙ軸周りのトルクを示す図。
【図９】レギュレート性能テストにおける外乱付加方向を示す図であり、図中の（ａ）はｘ軸方向の外乱、（ｂ）はｙ軸方向の外乱、（ｃ）はｚ軸方向の外乱を示す図。
【図１０】ｘ軸方向に外乱を加えたレギュレート性能を示す図であり、（ａ）は適合度、（ｂ）はｘ軸方向の誤差、（ｃ）はｘ軸方向の誤差（１０５［ｓ］から１１０［ｓ］の拡大図）、（ｄ）はｘ軸方向の推力を示す図。
【図１１】ｙ軸方向に外乱を加えたレギュレート性能を示す図であり、（ａ）は適合度、（ｂ）はｙ軸方向の誤差、（ｃ）はｙ軸方向の誤差（１０５［ｓ］から１１０［ｓ］の拡大図）、（ｄ）はｙ軸方向の推力を示す図。
【図１２】ｚ軸方向に外乱を加えたレギュレート性能を示す図であり、（ａ）は適合度、（ｂ）はｚ軸方向の誤差、（ｃ）はｚ軸方向の誤差（１０５［ｓ］から１１０［ｓ］の拡大図）、（ｄ）はｚ軸方向の推力を示す図。
【図１３】ｙ軸周りに外乱を加えたレギュレート性能を示す図であり、（ａ）は適合度、（ｂ）はｙ軸周りの誤差、（ｃ）はｙ軸周りの誤差（１０５［ｓ］から１１０［ｓ］の拡大図）、（ｄ）はｙ軸周りのトルクを示す図。
【図１４】実施例２に係る水中ロボット及び水中実験装置の構成を示す写真。
【図１５】（ａ）３Ｄマーカーを示す図、（ｂ）水中の対象物及びＧＡ探索エリアを示す図。
【図１６】水中実験における座標系と被嵌合装置を示す図。
【図１７】推力、トルク制御電圧の各初期特性、及び、不感帯特性を取り除いて調整された各初期特性、並びに、直線近似を示す図であり、（ａ）はｙ軸方向の初期特性、（ｂ）は不感帯特性を取り除いた特性（実線）とｙ軸方向の調整された特性（破線）、（ｃ）（ｄ）はｚ軸方向の特性、（ｅ）（ｆ）はｙ軸周りの特性を示す図。
【図１８】外乱を付加しない場合のレギュレート性能を示す図であり、（ａ）は適合度の時間変化、（ｂ）はｘ軸方向の誤差、（ｃ）はｙ軸方向の誤差、（ｄ）はｚ軸方向の誤差、（ｅ）はｙ軸周りの誤差、（ｆ）は水中ロボットの３Ｄ軌跡、（ｇ）はｘ軸方向の推力、（ｈ）はｙ軸方向の推力、（ｉ）はｚ軸方向の推力、（ｊ）ｙ軸周りのトルクを示す図。
【図１９】外乱を加えない場合のレギュレート性能を示す図（１０５［ｓ］から１１０［ｓ］の拡大図）であり、（ａ）はｘ軸方向の誤差、（ｂ）はｙ軸方向の誤差、（ｃ）はｚ軸方向の誤差、（ｄ）はｙ軸周りの誤差、（ｅ）はｘ軸方向の推力、（ｆ）はｙ軸方向の推力、（ｇ）はｚ軸方向の推力、（ｈ）はｙ軸周りのトルクを示す図。
【図２０】嵌合実験結果を示す図：（ａ）嵌合実験の写真、（ｂ）適合度、（ｃ）ｘ軸方向の位置、（ｄ）ｘ軸方向の推力、（ｅ）ｙ軸方向の位置、（ｆ）ｙ軸方向の推力、（ｇ）ｚ軸方向の位置、（ｈ）ｚ軸方向の推力、（ｉ）ｙ軸周りの角度、（ｊ）ｙ軸周りのトルク。
【図２１】嵌合実験結果を示す図：（ａ）ｘ軸方向の誤差、（ｂ）ｙ軸方向の誤差、（ｃ）ｚ軸方向の誤差、（ｄ）ｙ軸周りの誤差。
【図２２】モデルベーストマッチング法を説明するための説明図。
【図２３】モデルベーストマッチング法を説明するための説明図。
【図２４】ＧＡ法を説明するための説明図。
【図２５】ＧＡ法のフローを示す図。
【図２６】複合誘導マーカーを用いた自走型水中ロボットの自己充電のための帰巣システムを説明する説明図。
【発明を実施するための形態】
【００１７】
次に、発明の実施の形態を説明する。
以下に、本発明の実施形態に係る水中航走体制御システムについて実施例を用いて具体的に説明する。
なお、以下の実施例においては、所定の水中実験環境において水中航走体制御システムが有する水中ロボットを実験的に動作させることで水中航走体制御システムを具体的に説明するが、特に水中航走体制御システムの構成や水中航走体制御システムが適用される水中環境を本実施例に限定するものでない。
【実施例１】
【００１８】
［実験方法］
＜水中ロボット系＞
図１に示すように、水中航走体制御システム１００は、水中航走体の一例である水中ロボット１０と、該水中ロボット１０を制御する制御手段２０を主に備える。水中ロボット１０は、テザーケーブル９を介して制御手段２０に電気的に接続されている。
【００１９】
水中ロボット１０は、遠隔操作型の水中ロボット（ＲＯＶ、最大水深５０ｍ）である。水中ロボット１０は、ロボット本体１と、複眼カメラ２（第１カメラ２ａ、第２カメラ２ｂ）と、下方カメラ（図示せず）と、遠隔操作用カメラ（図示せず）と、水中動力系３と、光源部１１を備えている。水中ロボット１０は、推進手段である水中動力系３により水中を移動可能である。
【００２０】
ロボット本体１は、略直方体形状の水中ロボット１０の外形を形成するものである。ロボット本体１には、所定の各部に複眼カメラ２、下方カメラ、遠隔操作用カメラ、水中動力系３、及び光源部１１が配置される。
【００２１】
複眼カメラ２は、図１（Ａ）の（ａ）において点線丸印で示すように、ロボット本体１の前方側に配置され、対象物を撮像するための固定カメラ（Ｆｉｘｅｄ  ｃａｍｅｒａｓ）である。複眼カメラ２は、第１カメラ２ａと第２カメラ２ｂとを水平方向に並べて配置される。第１カメラ２ａと第２カメラ２ｂの各々には、標準レンズや標準レンズよりも画角が広い広角レンズが取り付けられる。複眼カメラ２は、水中ロボット１０の位置／姿勢制御用のカメラである。複眼カメラ２は、ＭＯＳ制御で水中ロボット１０前方の対象物の立体認識を行うため、前方に配置された第１カメラ２ａと第２カメラ２ｂが同時使用される。複眼カメラ２は、対象物との距離等を計測する両眼視覚センサーとして機能する。複眼カメラ２は、制御手段２０が有する画像入力部２１にテザーケーブル９を介して電気的に接続される。第１カメラ２ａと第２カメラ２ｂで撮像された画像は、制御手段２０が有する画像入力部２１に入力される。
【００２２】
下方カメラは、ロボット本体１の下方側に配置される制御用のカメラである。上述したように、水中ロボット１０は、水中ロボット１０の制御用として、前方２台のカメラである複眼カメラ２および下方２台のカメラである下方カメラの４台のカメラ（撮像素子ＣＣＤ、画素数３８万画素、信号方式ＮＴＳＣ、最低被写体照度０．８［ｌｘ］、ズームなし）を搭載している。
【００２３】
遠隔操作用カメラは、１台の遠隔操縦用チルト機構付きカメラ（撮像素子ＣＣＤ、画素数３８万画素、信号方式ＮＴＳＣ、最低被写体照度１．５［ｌｘ］、ズーム光学１０倍）である。
【００２４】
水中動力系３は、水中ロボット１０を水中で移動させるための推進手段である。水中ロボット１０には、水中動力系３として、図１（ｂ）、（ｃ）、（ｄ）において点線丸印で示すように、水平スラスタ（Ｈｏｒｉｚｏｎｔａｌ  ｔｈｒｕｓｔｅｒｓ）６が２基（最大推力９．８［Ｎ］，以下同様）と、垂直スラスタ（Ｖｅｒｔｉｃａｌ  ｔｈｒｕｓｔｅｒ）７が１基（４．９［Ｎ］），横スラスタ（Ｔｒａｖｅｒｓｅ  ｔｈｒｕｓｔｅｒ）８が１基（４．９［Ｎ］）搭載されている。
【００２５】
水平スラスタ６は、ロボット本体１の後部に配置され、水中ロボット１０を前方に移動するための推力を発生する。垂直スラスタ７は、ロボット本体１の上部に配置され、水中ロボット１０を上下方向に移動するための推力を発生する。横スラスタ８は、ロボット本体１の側部に配置され、水中ロボット１０を左右方向に移動するための推力を発生する。水中動力系３（水平スラスタ６、垂直スラスタ７、横スラスタ８）は、テザーケーブル９を介して制御手段２０が有する推力制御部２５に電気的に接続される。制御手段２０は、水平スラスタ６、垂直スラスタ７、横スラスタ８のそれぞれを駆動制御して、水中ロボット１０を上下左右方向に移動可能である。
【００２６】
光源部１１は、ロボット本体１の前方に配置され、水中ロボット１０の前方を照らすための光源であり、複眼カメラ２による対象物等の撮像の際に照度を確保するためのものである。本実施形態の光源部１１としては、ＬＥＤライト（５．８Ｗ）を２基搭載している。
以上説明した水中ロボット１０の各構成部分の性能仕様を表１にまとめて示す。
【００２７】
【表１】
0005777188.tif 000002
【００２８】
制御手段２０は、水中動力系３（水平スラスタ６、垂直スラスタ７、横スラスタ８）及び複眼カメラ２等を制御するものであり、本実施形態ではＰＣ（Ｐｅｒｓｏｎａｌ  Ｃｏｍｐｕｔｅｒ）を用いている。制御手段２０は、テザーケーブル９を介して複眼カメラ２、下方カメラ、遠隔操作用カメラ、水中動力系３、及び光源部１１の各々に電気的に接続されて、各々を制御するものである。本実施形態における制御手段２０は、水中ロボット１０とは遠隔操作用のテザーケーブル９で接続されており、地上に配置されている。
なお、本実施形態では、制御手段２０を水中ロボット１０と離れた場所に配置して水中ロボット１０を遠隔操作型の水中ロボット（ＲＯＶ）として構成しているが、特に限定するものではなく、ロボット本体１の内部に制御手段２０や電源等を搭載して、テザーケーブル９を必要としない自走型水中ロボットとして構成とすることもできる。また、このように自走型水中ロボットを構成するとともに、該制御手段２０に水中ロボットの自動航行や自律的水中作業を行うプログラムを搭載することで水中作業型の自律ロボット（ＡＵＶ）を構成することもできる。
【００２９】
制御手段２０は、図２（Ｂ）に示すように、画像入力部２１、モデル記憶部２２、適合度演算部２３、遺伝アルゴリズム実行部２４、推力制御部２５を有している。画像入力部２１は、複眼カメラ２により撮像された画像が入力される部分である。モデル記憶部２２は、対象物の特徴情報（例えば、形状や色等）を含むモデルが記憶されている部分である。適合度演算部２３は、画像入力部２１に入力された入力画像ついて前記モデルとの適合度を求めるための所定の適合度関数を演算する部分である。適合度演算部２３は、後述するモデルベーストマッチング法により演算が行われる。遺伝アルゴリズム実行部２４は、適合度演算部２３により求められる適合度に基づいて対象物の遺伝情報を生成する遺伝アルゴリズムを実行する部分である。詳細は後述するが、遺伝アルゴリズム実行部２４は、後述するＧＡ法によりＧＡ（Ｇｅｎｅｔｉｃ  Ａｌｇｏｒｉｔｈｍ）が実行される部分である。遺伝アルゴリズム実行部２４は、対象物に合致するように前記遺伝情報を所定時間において進化させて、前記対象物の位置及び姿勢を推定することができる。推力制御部２５は、水中動力系３を制御して水中ロボット１０を所定方向に移動させるとともに水中ロボット１０を対象物に対して所定の位置／姿勢となるように制御をする部分である。
なお、本実施例では、対象物の位置を示すものとして立体的な水中マーカーＭ（図５参照）を実験的に用いているが、対象物が水中マーカーＭを備えることが必須な条件ではない。
【００３０】
＜水中実験系＞
次に、水中ロボット１０を用いた水中実験について図２を用いて説明する。図２には、水中ロボット１０及び水中実験装置の構成を示す。
【００３１】
実施例１に係る水中実験では、水道水を満たした簡易プール（縦×横×高さ、４ｍ×６ｍ×０．７ｍ）を実験槽として使用した。上記水中ロボット（ＲＯＶ）１０を実験槽に水入させ、図２に示すようにテザーケーブル９（本実施形態では、ケーブル長５０ｍ）を介して水中ロボット１０への給電ならびに実験槽脇に設置した制御手段２０であるＰＣを介して対象物の画像情報ならびに制御信号等の通信を行った。制御手段２０側では水中ロボット１０搭載の２台のカメラ（第１カメラ２ａ、第２カメラ２ｂ）で構成される複眼カメラ２から送られる対象物の立体像情報を基に、後述するＧＡ法による３Ｄモデルベーストマッチングを行った。また、提案する制御系のブロック図を図３に示す。提案するシステムでは、複眼カメラ２から取得した画像を制御手段２０（画像入力部２１）に転送し、モデルベーストマッチング法とＧＡ法を組み合わせて対象物との相対位置・姿勢を認識させている。さらに、認識結果と相対的目標位置・姿勢との誤差に基づくＰ制御によって計算された指令電圧値を水中ロボット１０のスラスタ（水中動力系３）に与えることで、推力を発生させている。なお、本実験系におけるＧＡ法は、３３［ｍｓ］あたり９世代進化できることを予備実験で確認している。
【００３２】
＜水中実験条件＞
１．ＧＡ法による３次元対象物認識条件
複眼カメラ２を用いたＧＡによる認識条件について、本実施例の実験は実験槽内水中任意位置に図４に示すボックス（１００ｍｍ×１００ｍｍ×１００ｍｍ）周りに赤・青・緑の三原色球（直径４０ｍｍ）を配置した対象物（図４に示すＴａｒｇｅｔ）を固定設置することによって実施した。また、ＧＡによる３次元モデルベーストマッチングによる対象物の位置・姿勢の認識情報は、同図内に示す水中ロボット１０と対象物間で設定したＧＡ探索エリア内で獲得されると仮定している。各条件を表２に示す。ここで姿勢に関する変数ε２はｙ軸周りの回転角を意味している。また、予備実験によって、他の軸周りの回転は水中ロボット１０の移動に依らず微小であったため，本実験ではこれらをＧＡによる認識の対象としない。
【００３３】
【表２】
0005777188.tif 000003
【００３４】
２．水中制御安定性実験
任意位置に水中固定設置した対象物をＧＡにより認識後、ＭＯＳ制御知能により水中ロボット１０に搭載した４基のスラスタ６、７、８へテザーケーブル９を介して指令電圧値を送ることで、水中ロボット１０と対象物間とで以下の相対的目標位置・姿勢（ｘｄ［ｍｍ］，ｙｄ［ｍｍ］，ｚｄ［ｍｍ］，ε２ｄ［ｄｅｇ］）を保つようにレギュレートさせ、水中での制御性能の検証を行った。図５に本実施例の実験における座標系を示す。
【００３５】
また、ｘ［ｍｍ］，ｙ［ｍｍ］，ｚ［ｍｍ］，ε２［ｄｅｇ］をＧＡで認識した水中マーカーＭの位置・姿勢とおき，相対的目標位置・姿勢に水中ロボット１０をレギュレートさせるため、以下のＰ制御から計算される指令電圧値を各スラスタ（水平スラスタ６、垂直スラスタ７、横スラスタ８）へ与えた。
奥行き方向：ｖ１＝ｋｐ１（ｚｄ−ｚ）＋２．５        （１）
鉛直軸回転：ｖ２＝ｋｐ２（ε２ｄ−ε２）＋２．５    （２）
鉛直方向：ｖ３＝ｋｐ３（ｙｄ−ｙ）＋２．５        （３）
左右方向：ｖ４＝ｋｐ４（ｘｄ−ｘ）                （４）
さらに、外乱に対するロバスト安定性を検証するため、レギュレート中の水中ロボット１０に対して実験槽外部から物理力による外乱を加え、元の位置・姿勢へ復元できるかの確認も行った。
【００３６】
［結果と考察］
次に、実験結果と考察を述べる。
【００３７】
＜対象物のＧＡ法による認識精度＞
１．スラスタ出力制御
従来技術の遠隔操作型ＲＯＶではコントローラージョイスティック指示による作動操作時において、人間の指の動きによる誤動作を防止するためスラスタ推進制御信号発信時にある程度の冗長性（不感帯）を持たせている。これに対し本実施例では、対象物へのスラスタ推進による接近移動の際、ｍｍ単位での高い精度の移動ならびに姿勢制御性能の実現を目指している。そこで、ここでは各スラスタ動力への推力‐制御電圧変化特性における従来の不感帯の制御電圧に対して、推力の計算が容易にできるように制御電圧と推力の近似式を求めた。具体的には、予備実験で確認した水中ロボット１０の不感帯特性（図６（ａ）（ｃ）（ｅ））を取り除き、同図（ｂ）（ｄ）（ｆ）の破線のように直線近似して推力の計算を行う。調整前はニュートラル状態（制御電圧２Ｖ）近傍の電圧変化に対して出力応答に不感帯があったが、調整後は推力‐制御電圧間の近似式に基づき、推力の計算が容易にできるようになった。なお、水中ロボット１０のｘＨ軸方向に関する推力は、正方向、負方向用にオン／オフ制御がそれぞれ実装されており、各方向ともオン時には４．９［Ｎ］の推力が発生し、オフ時には推力は発生しない。
【００３８】
２．水中におけるＧＡ認識精度
図７（ａ）にｘｄ＝０，ｙｄ＝−６７，ｚｄ＝５００，ε２ｄ=０でレギュレートさせた水中ロボット１０のＧＡ認識時の適合度の時間変化を示す。これによると、認識開始から数秒以内で適合度が１以上となるとともに、適合度計測時間中においてほぼ０．８以上の高い適合度を維持していることがわかる。一般的にビジュアルサーボにより精度よく対象物への物理的作業を行う場合、ＧＡ認識精度は０．５以上必要であると考えられており、非特許文献１では、本実験に使用した同一対象物を用いて室内における両眼認識精度の測定を行っている。これら両眼により陸上で得られた適合度の結果と今回水中で得られた結果を比較しても両者において同程度の適合度が得られていることがわかる。すなわちＧＡによる水中での対象物認識精度は、気中における場合との比較においてもほぼ同程度の適合度が得られることがわかった。
【００３９】
３．レギュレート性能
実験槽外から物理的に外乱を与えなかった場合のレギュレート性能を図７、８に示す。ＧＡの認識誤差、ロボット移動時に生じるテザーケーブル９からの反力、さらに移動時に発生する水圧変化による実験槽側面からの反射波を受け、相対的目標位置・姿勢からの誤差が定常的に現れるものの、対象物に対する相対的位置・姿勢を維持するＭＯＳ制御方式では４基のスラスタ６・６、７、８を常時同時稼働させることでこれら外乱要素をキャンセルさせることにより相対的目標位置・姿勢近傍にレギュレートできることが確認できる。なお、図７（ｂ）（図８（ａ））に現れる誤差の緩慢な振る舞いは水中ロボット１０の側面に設置されているスタビライザー板１ａの抵抗による横スラスタ８の推力不足にその主要因があると考えられる。また、各目標値に対する定常誤差は、Ｐ制御によって与えられる推力と釣り合っている状態を意味している。
【００４０】
４．外乱に対するロバスト安定性
ここでは対象物を認識してレギュレート中の水中ロボットに対して、障害物との衝突や海流等の外乱が加わった場合のロバスト安定性を検証するため、実験槽外から棒（木製全長２ｍ）を介した人力により制御中の水中ロボット１０の位置を恣意的にずらすような外力を横方向（図９（ａ））、上方向（図９（ｂ））、後方（図９（ｃ））、鉛直軸周り（図９（ｄ））にそれぞれ加え、水中ロボット１０が対象物との相対的目標位置・姿勢に復元するか確認を行った。横方向、上方向、後方および鉛直軸周りに外乱を加えた場合の制御結果を図１０〜図１３に示す。各図とも（ａ）がＧＡ認識の適合度、（ｂ）は各変数についての相対的目標位置・姿勢と水中ロボット１０が認識した対象物の位置・姿勢との誤差、（ｃ）は（ｂ）のグラフと同様であるが実験開始後１０５〜１１０秒における様子、（ｄ）はスラスタに与えた推力（トルク）を表している。各図とも実験開始から２０［ｓ］および６０［ｓ］あたりで外乱が加えられている。それぞれの図（ａ）（ｂ）（ｄ）に記した（Ａ）および（Ｂ）の区間において、外乱が加えられた際に適合度が一時的に低くなるが、ビジュアルサーボを維持しつつ、相対的目標位置・姿勢からの誤差に応じてスラスタに与えられる推力（トルク）が変化することが分かる。すなわち、誤差を修正するための操作量が発生し、その結果として相対的目標位置・姿勢に復元するよう水中ロボット１０の位置・姿勢が変化していくことを確認できる。
【００４１】
以上より、水中航走体制御システム１００ではこれら全ての外乱に対して数秒〜数１０秒以内に元の位置に復元でき、外乱に対するロバスト安定性も有することを確認できた。ただし、複眼カメラ２の映像から対象物が完全に外れるような大きな変位を加えられた場合、ビジュアルサーボが切れてしまうため、水中ロボット１０が対象物との相対的目標位置・姿勢にレギュレート出来なくなる可能性がある。
【００４２】
［まとめ］
実施例１では複眼カメラ２によるビジュアルサーボを搭載した水中ロボット１０のレギュレート性能の検証を行い、その結果、下記結論を得た。（１）水中に固定した対象物を用いて複眼カメラ２によるＧＡ認識精度実験を行った結果、気中実験と同程度の適合度が得られることを確認した。（２）提案するレギュレートシステムは、物理的外乱に対してロバスト性を有することが分かった。（３）ビジュアルサーボによる３次元位置・姿勢制御のロバスト安定性を確認した。
【００４３】
また、実施例１の水中航走体制御システム１００では、対象物の推定された位置及び姿勢と、前記対象物に対する水中ロボット１０の相対的目標位置及び姿勢との誤差に基づくＰ制御によって計算された指令電圧値を水中動力系３に与えることで、水中ロボット１０が前記相対的目標位置及び姿勢を保持するように水中動力系３の推力を発生させる。これにより、水中ロボット１０の位置姿勢を認識対象物に対して相対的に一定に保つビジュアルサーボを精度良く実現することができる。
【００４４】
また、実施例１によれば、水中におけるＧＡ法の適用により複眼カメラ２のカメラ視線方向の距離を正確に認識できる。また、複眼カメラ２に広角レンズを取り付けて構成する場合、カメラの視線方向の距離に応じて認識対象物の画像内の大きさが大きく変化する特徴（肉眼で見たときに比べて、近くにある被写体はより大きく、遠くにある被写体はより小さく写るデフォルメ効果）を有するため、対象物を認識する際の変位を捕捉しやすくなる。すなわち、広角レンズを通して立体的な対象物を３次元認識（対象物の立体形状とその位置及び姿勢６変数を求めること）することで対象物近傍における認識精度が向上する。
【００４５】
以上のように、実施例１では、ＲＯＶである水中ロボット１０に搭載した複眼カメラ２から得られる画像とＧＡ法により対象物との相対的位置・姿勢を認識するシステムを構築し、水中ロボット１０と対象物との間の相対的位置・姿勢制御に関するレギュレータ性能について説明し、水中ロボット１０が物理的外乱に対してロバスト性を有することを示した。次に、実施例２では、この成果をもとに水中自動給電を想定した水中嵌合手法と、この手法を水中ロボット１０に実装した水中嵌合実験の実施結果について説明する。
【実施例２】
【００４６】
［実験方法］
＜水中ロボット系＞
実施例２に係る水中航走体制御システム１００においては、表３に示すように、水中ロボット１０が有するテザーケーブル９の長さが異なるのみであり、水中航走体制御システム１００の構成部分の形状や機能は実施例１と同じであるため、説明を省略する。
【００４７】
【表３】
0005777188.tif 000004
【００４８】
＜水中実験系＞
次に、水中ロボット１０を用いた水中実験について図１４を用いて説明する。図１４には、水中ロボット１０及び水中実験装置の構成を示す。
【００４９】
実施例２に係る水中実験では、水道水を満たした簡易プール（縦×横×高さ、２［ｍ］×３［ｍ］×０．７５［ｍ］）を実験槽として使用した。上記水中ロボット（ＲＯＶ）１０を実験槽に水入させ、図１４に示すようにテザーケーブル７（本実施形態では、ケーブル長２００［ｍ］）を介して水中ロボット１０への給電ならびに実験槽脇に設置した制御手段２０であるＰＣを介して対象物の画像情報ならびに制御信号等の通信を行った。制御手段２０側では水中ロボット１０搭載の２台のカメラ（第１カメラ２ａ、第２カメラ２ｂ）で構成される複眼カメラ２から送られる対象物の立体像情報を基に、ＧＡ法による３Ｄモデルベーストマッチングを行った。また、水中自動給電を想定した実験を行うため、図１６に示すように、嵌合用の棒１２を水中ロボット１０の右側に取り付けるとともに、水中ロボット１０が対象物と正対した時に、嵌合用の棒１２が嵌合用の穴と正対するよう、被嵌合装置Ｄを対象物横に設置した。さらに、実験条件において設定する時変の相対的目標位置・姿勢に水中ロボット１０が追従し，最終的な目標位置・姿勢でレギュレートすることを嵌合完了と定義する。
【００５０】
提案する制御系のブロック図は、実施例１で説明した図３である。提案するシステムでは，複眼カメラ２から取得した画像を制御手段２０であるＰＣに転送し、モデルベーストマッチング法とＧＡ法を組み合わせて対象物との相対的位置・姿勢を認識させている。さらに、認識結果と相対的目標位置・姿勢との誤差に基づくＰ制御によって計算された指令電圧値を水中ロボット１０のスラスタ（水中動力系３）に与えることで、推力を発生させている。なお，本実験系におけるＧＡ法は、３３［ｍｓ］あたり９世代進化できることを予備実験で確認している。また，ε２は図１５におけるｙ軸回りの姿勢をクォータニオンで表したものである（非特許文献１参照）。ε１（ｘ軸（ピッチング軸）回りの姿勢）、ε３（ｚ軸（ローリング軸）回りの姿勢）については、自律的に安定であり制御は不要と判断し、制御を行っていない。
【００５１】
＜水中実験条件＞
１．ＧＡ法による３次元対象物認識条件
複眼カメラ２を用いたＧＡによる認識条件について、本実施例の実験は実験槽内水中任意位置に図１５（ａ）に示すボックス（１００［ｍｍ］×１００［ｍｍ］×１００［ｍｍ］）周りに赤・青・緑の三原色球（直径４０［ｍｍ］）を配置した対象物を固定設置することによって実施した。また、ＧＡによる３次元モデルベーストマッチングによる対象物の位置・姿勢の認識情報は，同図（ｂ）に示す水中ロボット１０と対象物間で設定したＧＡ探索エリア内で獲得されると仮定している。各条件を表４に示す。
【００５２】
【表４】
0005777188.tif 000005
【００５３】
２．水中嵌合条件
任意位置に水中固定設置した対象物をＧＡにより認識後、ＭＯＳ制御知能により水中ロボット１０に搭載した４基のスラスタ６・６、７、８へ指令電圧値を送ることで、水中ロボット１０と対象物間とで以下の相対的目標位置・姿勢（ｘｄ［ｍｍ］，ｙｄ［ｍｍ］，ｚｄ［ｍｍ］，ε２ｄ［ｄｅｇ］）を保つようにレギュレートしつつ、水中での嵌合を果たす実験を行う。ここで次式の各数値は、対象物の発見直後に水中ロボット１０がレギュレートすべき目標値、括弧（）内の数値は、嵌合完了時の目標値として定義している。図１６に本実施例の実験における座標系を示す。
【００５４】
【数１】
0005777188.tif 000006
【００５５】
また，ｘ［ｍｍ］，ｙ［ｍｍ］，ｚ［ｍｍ］，ε２［ｄｅｇ］をＧＡで認識した対象物の位置・姿勢とおき、相対的目標位置・姿勢に水中ロボット１０をレギュレートさせるため、以下のＰ制御から計算される指令電圧値とオン／オフ制御による指令値を各スラスタ（水平スラスタ６、垂直スラスタ７、横スラスタ８）へ与えた。
奥行き方向：ｖ１＝ｋｐ１（ｚｄ−ｚ）＋２．５        （１）
鉛直軸回転：ｖ２＝ｋｐ２（ε２ｄ−ε２）＋２．５      （２）
鉛直方向：ｖ３＝ｋｐ３（ｙｄ−ｙ）＋２．５          （３）
右水平方向：ｘｄ−ｘ＜−５（オン／オフ制御の条件式）  （４）
左水平方向：５＜ｘｄ−ｘ（オン／オフ制御の条件式）  （５）
【００５６】
［結果と考察］
次に、実験結果と考察を述べる。
＜対象物のＧＡ法による認識精度＞
１．スラスタ出力制御
従来技術の遠隔操作型ＲＯＶではコントローラージョイスティック指示による作動操作時において、人間の指の動きによる誤動作を防止するためスラスタ推進制御信号発信時にある程度の冗長性（不感帯）を持たせている。これに対し本実施例では、対象物へのスラスタ推進による接近移動の際、ｍｍ単位での高い精度の移動ならびに姿勢制御性能の実現を目指している。そこで、ここでは各スラスタ動力への推力‐制御電圧変化特性における従来の不感帯の制御電圧に対して、推力の計算が容易にできるように制御電圧と推力の近似式を求めた。具体的には、予備実験で確認した水中ロボット１０の不感帯特性（図１７（ａ）（ｃ）（ｅ））を取り除き、同図（ｂ）（ｄ）（ｆ）の破線のように直線近似して推力の計算を行う。調整前はニュートラル状態（制御電圧２．５［Ｖ］）近傍の電圧変化に対して出力応答に不感帯があったが、調整後は推力‐制御電圧間の近似式に基づき、推力の計算が容易にできるようになった。なお、水中ロボット１０のｘＨ軸方向に関する推力は、右水平方向、左水平方向用にオン／オフ制御がそれぞれ実装されており、各方向ともオン時には４．９［Ｎ］の推力が発生し、オフ時には推力は発生しない．
【００５７】
２．水中におけるＧＡ認識精度
図１８（ａ）にｘｄ＝０，ｙｄ＝−６７，ｚｄ＝６００，ε２ｄ=０でレギュレートさせた水中ロボット１０のＧＡ認識時の適合度の時間変化を示す。これによると、認識開始から数秒以内で適合度が１以上となるとともに、適合度計測時間中においてほぼ０．８以上の高い適合度を維持していることがわかる。一般的にビジュアルサーボにより精度よく対象物への物理的作業を行う場合、ＧＡ認識精度は０．５以上必要であると考えられており、非特許文献１では、本実験に使用した同一対象物を用いて室内における複眼認識精度の測定を行っている。これら複眼により陸上で得られた適合度の結果と今回水中で得られた結果を比較しても両者において同程度の適合度が得られていることがわかる。すなわちＧＡによる水中での対象物認識精度は、気中における場合との比較においてもほぼ同程度の適合度が得られることがわかる。
【００５８】
３．レギュレート性能
実験槽外から物理的に外乱を与えなかった場合のレギュレート性能を図１８、１９に示す。ＧＡの認識誤差、ロボット移動時に生じるテザーケーブル９からの反力、さらに移動時に発生する水圧変化による実験槽側面からの反射波を受け、相対的目標位置・姿勢からの誤差が定常的に現れるものの、対象物に対する相対的位置・姿勢を維持するＭＯＳ制御方式では４基のスラスタ６・６、７、８を常時同時稼働させることでこれら外乱要素をキャンセルさせることにより相対的目標位置・姿勢近傍にレギュレートできることが確認できる。なお、図１８（ｂ）、（図１９（ａ））に現れる誤差の緩慢な振る舞いは水中ロボット１０の側面に設置されているスタビライザー板１ａの抵抗による横スラスタ８の推力不足にその主要因があると考えられる。また、各目標値に対する定常誤差は、Ｐ制御によって与えられる推力と釣り合っている状態を意味している。
【００５９】
４．水中嵌合実験
本実施例での水中嵌合は図２０（ａ）に示すように次の４つの状態を経て行われる。（Ａ）対象物への接近と発見（Ａｐｐｒｏａｃｈ）、（Ｂ）対象物との相対的目標位置・姿勢を保つビジュアルサーボ（Ｖｉｓｕａｌ  ｓｅｒｖｏｉｎｇ）、（Ｃ）被嵌合装置Ｄへの嵌合（Ｉｎｔｅｒｄｉｇｉｔａｔｉｎｇ）、（Ｄ）被嵌合装置Ｄとの嵌合完了（Ｃｏｍｐｌｅｔｉｏｎ  ｏｆ  ｉｎｔｅｒｄｉｇｉｔａｔｉｏｎ）。
【００６０】
Ａｐｐｒｏａｃｈは、水中ロボット１０が対象物を発見（認識）するまでの状態を意味しており、対象物が水中ロボット１０前方に存在すると仮定したうえで、水中ロボット１０を微速前進させている。すなわち，初期状態では水中ロボット１０は対象物との相対的位置・姿勢を認識していないが、前進していく中で対象物を発見し、Ｖｉｓｕａｌ  ｓｅｒｖｏｉｎｇの状態に遷移する。
【００６１】
次に、Ｖｉｓｕａｌ  ｓｅｒｖｏｉｎｇは水中ロボット１０が対象物を認識している状態を意味している。前述の通り、あらかじめ定義した対象物との相対的目標位置・姿勢を保つような制御がなされる。ただし、この状態に遷移した以降は、対象物と水中ロボット１０の相対的位置の誤差がｘｄおよびｙｄそれぞれに関して±２０［ｍｍ］以内に１６５［ｍｓ］（制御ループ５回）以上連続して収まった場合に、水中ロボット１０はｚＨ軸方向に秒速３０［ｍｍ／ｓ］で前進する（相対的目標位置ｚｄを毎秒３０［ｍｍ］ずつ減少させる）。すなわち被嵌合装置Ｄへの嵌合を実行する状態（Ｉｎｔｅｒｄｉｇｉｔａｔｉｎｇ）に移る。ただし、外乱によって嵌合中に前述の誤差の範囲を超えた場合、嵌合を実行する前述の条件を再び満たすまで、水中ロボット１０はその時点における対象物との相対的位置・姿勢を保つようにビジュアルサーボを行う。
【００６２】
Ｉｎｔｅｒｄｉｇｉｔａｔｉｎｇは、水中ロボット１０が対象物を認識しつつ被嵌合装置Ｄへの嵌合を果たすまでの状態を意味している。Ｖｉｓｕａｌ  ｓｅｒｖｏｉｎｇの状態において、対象物と水中ロボット１０との相対的目標位置・姿勢からの誤差が前述の閾値以下に達すると、本状態に遷移する。相対的目標位置ｚｄを時変で与えつつビジュアルサーボを行うことで、最終的に嵌合を果たす位置・姿勢（ｘｄ＝０，ｙｄ＝−６７，ｚｄ＝３５０，ε２ｄ＝０）まで水中ロボット１０が移動する。
【００６３】
Ｃｏｍｐｌｅｔｉｏｎ  ｏｆ  ｉｎｔｅｒｄｉｇｉｔａｔｉｏｎは、水中ロボット１０が被嵌合装置Ｄと嵌合を果たした状態を意味している。Ｖｉｓｕａｌ  ｓｅｒｖｏｉｎｇの状態と同様に、嵌合状態における対象物との相対的目標位置・姿勢を保つような制御がなされる。
【００６４】
図２０（ｂ）〜（ｊ）は対象物が水中ロボット１０前方にある状態で嵌合実験を行った結果を示している。（ｂ）は適合度、（ｃ）（ｅ）（ｇ）（ｉ）は水中ロボット１０が認識している位置と姿勢、（ｄ）（ｆ）（ｈ）（ｊ）は各軸における推力とトルクを表している。また、図２１は相対的目標位置・姿勢からの誤差を表している。それぞれの図から、（Ａ）Ａｐｐｒｏａｃｈ、（Ｂ）Ｖｉｓｕａｌ  ｓｅｒｖｏｉｎｇ、（Ｃ）Ｉｎｔｅｒｄｉｇｉｔａｔｉｎｇ、（Ｄ）Ｃｏｍｐｌｅｔｉｏｎ  ｏｆ  ｉｎｔｅｒｄｉｇｉｔａｔｉｏｎへと状態が遷移していることが分かる。すなわち、提案システムが水中ロボット１０に嵌合能力を与えていることが確認できる。
【００６５】
［まとめ］
実施例２では複眼カメラ２によるビジュアルサーボを搭載した水中ロボット１０の水中自動給電を想定した水中嵌合実験を行い、下記結論を得た。（１）実施例１の結果に加えて、ｚ軸方向の時変目標値に対する追従性能を有することが分かった。（２）目標位置との誤差の閾値に基づきＶｉｓｕａｌ  ｓｅｒｖｏｉｎｇとＩｎｔｅｒｄｉｇｉｔａｔｉｎｇを切り替えることで、水中嵌合を実現できることが分かった。（３）本実施例２における実験条件では、６０秒以内に水中嵌合を完了できることが分かった。したがって、実施例２によれば、水中マーカーＭに対して相対的位置姿勢を精度よく制御することができる。これにより、例えば、対象物に一例として海底に沈められた充電装置と水中ロボット１０の相対的位置姿勢制御が可能となり、長期的に水中で自律的に作業を行うロボットの自己充電の実現に向けた技術的課題の一つが解決された。
【００６６】
［モデルベーストマッチング法及びＧＡ法よる対象物の認識手法］
次に、実施例１及び実施例２で用いられる対象物の認識手法である、モデルベーストマッチング法とＧＡ（Ｇｅｎｅｔｉｃ  Ａｌｇｏｒｉｔｈｍ）法を組み合わせて対象物との相対位置・姿勢を認識する手法について図２２〜２５を用いて説明する。当該認識手法は、本願発明者がすでに開示している非特許文献１に基づくものであり、より詳細な原理や手法については、本願発明者が開示した非特許文献１に記載されている。モデルベーストマッチング法及びＧＡ法よる対象物の認識方法は、認識対象物の立体モデル形状（図２４参照）を制御手段２０であるＰＣ内（モデル記憶部２２）に定義しこの形状を複眼カメラ２の撮像面に射影することで、２次元情報から３次元情報の復元ではなく、３次元情報形状と位置／姿勢情報をもとに２次元に射影された画像と照合する方法である。以下、具体的に説明する。
【００６７】
図２２は、モデルベーストマッチング法を説明するための説明図である。モデルベーストマッチング法では、予め制御手段２０が有するモデル記憶部２２に対象物の知識（対象物の色、対象物の形状）を与えた立体モデル（以下、対象物モデルともいう）が記憶されている。制御手段２０は、先ず、複眼カメラ２（第１カメラ２ａ及び第２カメラ２ｂ）により対象物を撮像して制御手段２０の画像入力部２１に入力する入力画像を取得する。それから、制御手段２０は、対象物モデルが有する対象物の知識（対象物の色、対象物の形状）に対して、入力画像がモデルの面部分：Ｓｉｎ、モデルの帯部分：Ｓｏｕｔがどの程度適合しているか判定する。具体的には、制御手段２０の適合度演算部２３は、上記対象物モデルと対象物の入力画像との相関関数である適合度関数（図２２参照）を用いて判定する。この適合度関数では、対象物とＳｉｎ領域が重なり、Ｓｏｕｔが色相値の高い背景と一致しているときに高い値となり、この場合に、制御手段２０は適合度が高いと判定する。また、制御手段２０の画像入力部２１に入力される入力画像としては、図２３に示すように、第１カメラ２ａにより対象物が撮像された２次元の右側入力画像と、第２カメラ２ｂにより対象物が撮像された２次元の左側入力画像が取得され、それぞれにおいて適合度関数により適合度が判定される。このように、上記入力画像を用いて対象物を認識する問題は、適合度関数の最大値探索問題に変換される。また、当該適合度関数と複眼カメラ２により時系列で撮像される画像とを用いて全探索を行って適合度の最大値を発見することができるが、時間がかかり過ぎる。そこで、本実施形態では全探索を実行する代わりにＧＡ法を用いた探索を実行する。
【００６８】
図２４は、ＧＡ法を説明するための説明図である。ＧＡ法では、先ず、実施例１、２で挙げるＧＡ探索エリア内に第１世代のＧＡ遺伝子（図２４のＡに示す適合度に基づく複数のフレーム状の立体モデル部分）をばらまく。続いて、適合度から選択された位置／姿勢情報が個々のＧＡにより生成される。そして、ＧＡ遺伝子の交叉、及び突然変異による進化（図２４では、第２世代から第ｉ世代まで進化）を行う（図２４のＢ）。当該進化が進むことで最終の世代へと至り、ＧＡ遺伝子が対象物と重なり（図２４のＣ）、対象物の実際の位置／姿勢を推定することが可能となる。
【００６９】
すなわち、制御手段２０が有する遺伝アルゴリズム実行部２４では、図２５に示すように、ＧＡ法が実行される、ＧＡ法では、先ず、初期化（Ｉｎｉｔｉａｌｉｚａｔｉｏｎ）が行われる（ステップＳ１０）。初期化では、初期個体（立体モデル）をＧＡ探索エリア内にばらまく。次のステップでは、評価（Ｅｖａｌｕａｔｉｏｎ）が行われる（ステップＳ２０）。評価では、ばらまかれた初期個体の適合度関数の計算が行われる。次のステップでは、並び替え（Ｓｏｒｔｉｎｇ）が行われる（ステップＳ３０）。並び替えでは、適合度に基づいて個体を並び替える。次のステップでは、出力（Ｏｕｔｐｕｔ）が行われる（ステップＳ４０）。出力では、最良の個体の位置／姿勢を出力する。次のステップでは、選択（Ｓｅｌｅｃｔｉｏｎ）が行われる（ステップＳ５０）。次のステップでは、交叉及び突然変異（Ｃｒｏｓｓｏｖｅｒ  ａｎｄ  Ｍｕｔａｔｉｏｎ）が行われる（ステップＳ６０）。次のステップでは、新しく画像が入力された時点から所定のビデオレート（本実施形態では、３３［ｍｓ］）が経過したかどうか判断する（ステップＳ７０）。所定のビデオレートが経過している場合（ステップＳ７０のＹＥＳ）、ステップ８０に移行し、所定のビデオレートが経過していない場合（ステップＳ７０のＮＯ）、ステップ２０に戻る。ステップ８０では、新しく対象物の画像入力が行われ、ステップ２０に戻る。このように、ＧＡ法は、新しく入力された画像に対し、所定のビデオレート（本実施形態では、３３［ｍｓ］）で次の画像が入力されるまでの間だけＧＡの進化を進め、その時点で最も高い適合度を与える個体（遺伝子情報）をその時刻における対象物の位置／姿勢として出力する方法（実時間遺伝的認識法）である。このように、ＧＡ法により所定のビデオレートで対象物の位置／姿勢が得られるため、周囲の状況変化に応じて対象物のオンライン認識が可能になる。
【００７０】
以上説明したように、水中航走体制御システム１００によれば、対象物形状の正確な認識とともに対象物に対する正確な位置／姿勢制御を自律的に行うことができる。
【００７１】
［自光マーカーを用いたアプローチ／嵌合システム］
次に、上述した水中航走体制御システム１００に基づいて水中で自己充電するシステムついて図を用いて説明する。上述した水中航走体制御システム１００では、制御手段２０を水中ロボット１０と離れた場所に配置して水中ロボット１０を遠隔操作型の水中ロボット（ＲＯＶ）として構成している。これに対して、自光マーカーを用いたアプローチ／嵌合システムでは、水中航走体制御システム１００の変形例として、ロボット本体１の内部に制御手段２０や電源等を搭載して、テザーケーブル９を必要としない自走型水中ロボット１０Ａとして構成している。また、このように自走型水中ロボット１０Ａを構成するとともに、該制御手段２０に水中ロボット１０Ａの自動航行や自律的水中作業を行うプログラムを搭載することで水中作業型の自律ロボット（ＡＵＶ）を構成することもできる。また、水中ロボット１０Ａは、ソナー（ＳＯＮＡＲ：Ｓｏｕｎｄ  ｎａｖｉｇａｔｉｏｎ  ａｎｄ  ｒａｎｇｉｎｇ）を備える。水中ロボット１０Ａは、ソナーにより音波発信源（ソナー発信源）から発信された音波を受信するとともに音波発信源の位置を特定する。また、水中ロボット１０Ａは、特定された音波発信源に向かって自走する音波追跡手段を有している。
なお、以下で説明する自光マーカーを用いたアプローチ／嵌合システムについては、自走型水中ロボット１０Ａにより水中で実施する形態を挙げるが、特に限定するものではなく、地上等でも実施可能である。
【００７２】
例えば、上述した自走型水中ロボット１０Ａを用いて、例えば海底での移動や作業を想定した場合、作業効率上海底で自己充電することが好ましい。海底で自己充電する場合、自走型水中ロボット１０Ａが自己充電のための充電装置５０に対して所定距離（例えば、数キロ）離れた地点に存在し、該地点から充電装置５０まで自律的にアプローチし、かつ充電装置５０に嵌合（ドッキング）することで充電に至る自走型水中ロボット１０Ａの誘導及び嵌合（ドッキング）のシステムが必要となる。
なお、ここでは、自光マーカーを用いたアプローチ／嵌合を行うシステムを帰巣システムと呼ぶことにする。
また、本実施形態では、自走型水中ロボット１０Ａは充電装置５０に嵌合（ドッキング）することで充電可能に構成しているが、特に限定するものではない。自走型水中ロボット１０Ａが充電装置に当接もしくは近接することで電気的接続部材を使用せず誘導電流により充電するシステムであってもよい。
【００７３】
帰巣システムは、対象物の一例である充電装置５０の位置を示す、立体形状（３次元形状）の自光マーカーを有している。自光マーカーの外形形状は、図１５（ａ）で示す水中マーカーＭと同じである。具体的には、自光マーカーは、３色の色（例えば、赤、青、緑等）が配色された光を透過する球の中に発光するデバイスを有し、それぞれの球が正規直交座標の座標軸先端に位置するように構成されている。自光マーカーの形状は、回転対称、線対称、面対称のすべての対称軸を持たないため自光マーカーの位置／姿勢の認識時に不要な任意性を排除できるため有効なものとなる。自光マーカーは、発光（常時発光や点滅発光等）することができる。このような自光マーカーを用いることにより、自走型水中ロボット１０Ａにより認識される視認距離を延ばすことができる。よって、視覚制御を行うことができる自走型水中ロボット１０Ａと自光マーカー間の相対距離を延ばすことが可能となり自走型水中ロボット１０Ａの帰巣成功の可能性を高め、帰巣失敗率を下げることができる。
【００７４】
さらに、上記自光マーカーに音波発信源（ソナー発信源）を備える複合誘導マーカーＭ２を構成とすることもできる。このように構成された複合誘導マーカーＭ２は、図２６に示すように、音波を発信して遠方（例えば、５０ｋｍ程度）に位置する自律型水中ロボット１０Ａに充電装置５０が存在する方向を知らせる。自光マーカーと音波発信源を一体化して複合誘導マーカーＭ２を構成することで、音波発信源と自光マーカーの位置の差を略ゼロにすることができるため、ソナーにより帰巣する自走型水中ロボット１０Ａのアプローチ誘導制御の状態から視覚制御の状態への切り替えが容易になる。
【００７５】
上記複合誘導マーカーＭ２を用いた場合、（１）複合誘導マーカーＭ２から発信する音波を検出し、音源方向に自走型水中ロボット１０Ａを誘導し（アプローチモード）、（２）自走型水中ロボット１０Ａが複合誘導マーカーＭ２を認識した時点で視覚制御に移行し（視覚制御モード）、（３）複合誘導マーカーＭ２と自走型水中ロボット１０Ａとの位置姿勢誤差が許容範囲内に減衰した時点で嵌合動作に移る（嵌合モード）、という一連の帰巣システムを構築できる。
なお、当該帰巣システムにおける帰巣可能範囲は音波の到達範囲であり、音波発信のアンプ出力を上げることで帰巣可能距離を延ばすことが可能である。
【００７６】
すなわち、帰巣システムは、アプローチモード、視覚制御モード、嵌合モードから構成される。アプローチモードでは、自走型水中ロボット１０Ａが複合誘導マーカーＭ２から発信される音波を受信し、音源の方向を特定し、音源に向かう経路に沿って誘導制御される。視覚制御モードでは、実施例１に記載された位置姿勢制御技術に基づいて相対位置姿勢制御を行って、相対位置姿勢誤差を略ゼロにする制御を行う。嵌合モードでは、相対的位置姿勢誤差を算出する目標位置姿勢を時間関数として減少させ、複合誘導マーカーＭ２に対する自走型水中ロボット１０Ａの奥行き方向の位置を次第に減少させることで、充電装置５０と自走型水中ロボット１０Ａの嵌合を行う。嵌合モードは、実施例２に記載された水中嵌合技術に基づいて実現可能である。
【産業上の利用可能性】
【００７７】
本発明の活用例として、水中放射能汚染物の回収、深海底資源調査・回収（サンプリング等）、（深海）水中未確認生物の発見・回収ならびに生態観察、地球構造観察（海底地殻変動等調査）、水産（栽培漁業・中間育成）、機雷撤去、人命救助（ＭＯＳ／ＡＵＶライフセーバー）等が挙げられる。
【符号の説明】
【００７８】
１    ロボット本体
２    複眼カメラ
３    水中動力系
１０  水中ロボット（水中航走体）
２０  制御手段
２１  画像入力部
２２  モデル記憶部
２３  適合度演算部
２４  遺伝アルゴリズム実行部
１００  水中航走体制御システム
Ｍ２  複合誘導マーカー
【要約】
【課題】対象物形状の正確な認識とともに対象物に対する正確な位置／姿勢制御を自律的に行うことができる水中航走体制御システムを提供する。
【解決手段】水中動力系３と、水中ロボット１０と、対象物を撮像する複眼カメラ２と、水中動力系３及び複眼カメラ２を制御する制御手段２０と、備える水中航走体制御システム１００であって、制御手段２０は、複眼カメラ２により撮像された画像が入力される画像入力部２１と、対象物の特徴情報を含むモデルが記憶されるモデル記憶部２２と、前記入力画像について前記モデルとの適合度を求めるための所定の適合度関数を演算する適合度演算部２３と、前記適合度に基づいて対象物の遺伝情報を生成する遺伝アルゴリズムを実行する遺伝アルゴリズム実行部２４と、有し、遺伝アルゴリズム実行部２４は、対象物に合致するように遺伝情報を所定時間において進化させて、対象物の位置及び姿勢を推定する。
【選択図】図２【図３】
0005777188.tif 000007
【図２５】
0005777188.tif 000008
【図１】
0005777188.tif 000009
【図２】
0005777188.tif 000010
【図４】
0005777188.tif 000011
【図５】
0005777188.tif 000012
【図６】
0005777188.tif 000013
【図７】
0005777188.tif 000014
【図８】
0005777188.tif 000015
【図９】
0005777188.tif 000016
【図１０】
0005777188.tif 000017
【図１１】
0005777188.tif 000018
【図１２】
0005777188.tif 000019
【図１３】
0005777188.tif 000020
【図１４】
0005777188.tif 000021
【図１５】
0005777188.tif 000022
【図１６】
0005777188.tif 000023
【図１７】
0005777188.tif 000024
【図１８】
0005777188.tif 000025
【図１９】
0005777188.tif 000026
【図２０】
0005777188.tif 000027
【図２１】
0005777188.tif 000028
【図２２】
0005777188.tif 000029
【図２３】
0005777188.tif 000030
【図２４】
0005777188.tif 000031
【図２６】
0005777188.tif 000032