JPH06335876A - 水中ロボット位置・姿勢制御装置 - Google Patents

Abstract

(57)【要約】 【目的】 水中ロボットの移動時における位置及び姿勢
を保持して多関節マニピュレータの作業が行ない易い位
置・姿勢にでき、水中ロボットが浮遊状態でも、多関節
マニピュレータを使っての作業を可能とする。 【構成】 双腕操作指令２及び双腕・アンカーアームの
初期姿勢ビークル重心位置データ３を基に、アンカーア
ーム重心位置データ１０を双腕モデル５と水中ロボット
モデル７により導出し、アンカーアーム操作指令導出器
１１によりアンカーアーム操作指令１２を出力し、アン
カーアーム２４を動かして水中ロボット１９の姿勢を保
持する。また、ビークル位置・姿勢指令４を基にビーク
ル位置・姿勢制御装置１３、ビークル速度制御装置１
５、スラスタ及びバラストタンク制御装置１７によりス
ラスタ・バラストタンク操作指令１８を導出し、スラス
タ２３、バラストタンク２２を駆動して水中ロボット１
９の位置・姿勢を保持する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、水中ロボットにおける
位置及び姿勢の制御に適用される水中ロボット位置・姿
勢制御装置に関する。

【０００２】

【従来の技術】従来の水中ロボットは、図１２に示すよ
うに構成されている。水中ロボット１９は、固定台４２
上に旋回台３１を介して本体２９をに設け、固定台４２
を中心にして旋回可能とすると共に、本体２９に多関節
マニピュレータ３０を装着している。また、旋回台３１
には、昇降用スラスタ３２及び前後進用スラスタ３３を
設けている。更に、水中ロボット１９は、床又は壁を移
動するための車輪３４及び本体２９を床又は壁に固定す
る吸着盤３５を備えている。

【０００３】上記のように構成された水中ロボット１９
は、水中ロボット制御装置３６から指令を与えて動作さ
せる。即ち、水中ロボット制御装置３６は、多関節マニ
ピュレータ指令３７を水中ロボット１９に与えて多関節
マニピュレータ３０を操作する。そして、本体推進指令
３８をスラスタ制御装置４０に入力し、スラスタ指令４
１を昇降用スラスタ３２及び前後進用スラスタ３３に与
え、各スラスタ３２，３３を駆動し、本体２９を推進す
る。また、水中ロボット制御装置３６は、本体旋回指令
３９を旋回台３１に与え、固定台４２を中心にして旋回
させる。

【０００４】

【発明が解決しようとする課題】従来の水中ロボット制
御方法においては、上記のように本体推進指令３８及び
本体旋回指令３９を水中ロボット１９に入力し、目標位
置に水中ロボット１９を移動している。しかし、水中ロ
ボット１９の移動時には、水中ロボット１９の位置・姿
勢制御を行なっていないため、移動する環境が複雑な場
合に水中ロボット１９を移動することや、多関節マニピ
ュレータ３０を使った作業が行ない易い位置・姿勢にす
ることが困難であった。また、水中ロボット１９を浮遊
状態にて、多関節マニピュレータ３０を使って作業する
と、水中ロボット１９の位置及び姿勢が保持できないた
め、作業ができないという問題があった。

【０００５】本発明は上記の問題を解決するためになさ
れたもので、その目的とするところは、水中ロボットの
移動時における位置及び姿勢を保持することができ、多
関節マニピュレータの作業が行ない易い位置・姿勢にで
き、水中ロボットが浮遊状態でも、多関節マニピュレー
タを使って作業することのできる水中ロボット位置姿勢
制御装置を提供することにある。

【０００６】

【課題を解決するための手段】本発明に係る水中ロボッ
ト位置・姿勢制御装置は、水中ロボットの双腕を操作す
る双腕操作指令により上記双腕の重心位置データを導出
する双腕モデルと、上記双腕及び双腕の動作に応じて動
き、水中ロボットの姿勢を保つアンカーアームの初期姿
勢及び水中ロボットのビークル重心位置データに基づい
て、水中ロボットの重心位置データを導出する水中ロボ
ットモデルと、上記双腕モデルからの双腕重心位置デー
タ及び水中ロボットモデルからの水中ロボット重心位置
データに基づいて、アンカーアームの重心位置データを
導出するアンカーアームモデルと、上記アンカーアーム
モデルにより導出されたアンカーアーム重心位置データ
に基づいてアンカーアームの操作指令を導出するアンカ
ーアーム操作指令導出器と、上記水中ロボットのビーク
ル位置・姿勢を検出する位置・姿勢検出器と、ビークル
位置・姿勢指令と上記位置・姿勢検出器からのビークル
位置・姿勢信号に基づいてビークル速度信号を出力する
ビークル位置・姿勢制御装置と、上記ビークル位置・姿
勢制御装置からのビークル速度指令と速度導出器により
導出されたビークル速度信号に基づいて、ビークルトル
ク指令を出力するビークル速度制御装置と、上記ビーク
ルトルク指令及びビークル位置・姿勢信号に基づいて、
上記水中ロボットの推進部のスラスタ及びバラストタン
クの操作指令を導出するスラスタ及びバラストタンク制
御装置とを具備したことを特徴とする。

【０００７】

【作用】双腕操作指令及び双腕・アンカーアームの初期
姿勢ビークル重心位置データを基に、アンカーアーム重
心位置データを双腕モデルと水中ロボットモデルにより
導出し、アンカーアーム操作指令導出器に入力してアン
カーアーム操作指令を出力し、この操作指令によりアン
カーアームを動かすことによって水中ロボットの姿勢を
保持する。

【０００８】また、ビークル位置・姿勢指令を基に、ビ
ークル位置・姿勢制御装置、ビークル速度制御装置、ス
ラスタ及びバラストタンク制御装置によりスラスタ・バ
ラストタンク操作指令を導出し、スラスタ及びバラスト
タンクを駆動することによって水中ロボットの位置・姿
勢を保持する。

【０００９】

【実施例】以下、図面を参照して本発明の一実施例を説
明する。図１は本発明の一実施例に係る水中ロボット位
置・姿勢制御装置を示す概要図である。

【００１０】図１において、１は位置・姿勢制御装置
で、双腕モデル５、水中ロボットモデル７、アンカーア
ームモデル９、アンカーアーム操作指令導出器１１、ビ
ークル位置・姿勢制御装置１３、ビークル速度制御装置
１５、スラスタ及びバラストタンク制御装置１７、速度
導出器２６を主体として構成されている。

【００１１】そして、双腕操作指令２が水中ロボット１
９に送られると共に、双腕モデル５に入力されて双腕重
心位置データ６が取り出され、アンカーアームモデル９
に入力される。また、双腕・アンカーアームの初期姿勢
・ビークル重心位置データ３が水中ロボットモデル７に
入力されて水中ロボット重心位置データ８が取り出さ
れ、アンカーアームモデル９に入力される。このアンカ
ーアームモデル９からは、上記両入力データに基づいて
アンカーアーム重心位置データ１０が取り出され、アン
カーアーム操作指令導出器１１に入力される。アンカー
アーム操作指令導出器１１は、アンカーアーム重心位置
データ１０に基づいてアンカーアーム操作指令１２を水
中ロボット１９に出力する。

【００１２】また、ビークル位置・姿勢指令４がビーク
ル位置・姿勢制御装置１３に入力される。更に、このビ
ークル位置・姿勢制御装置１３には、水中ロボット１９
の位置及び姿勢を検出する位置・姿勢検出器２５からの
ビークル位置・姿勢信号２７が入力される。ビークル位
置・姿勢制御装置１３は、上記ビークル位置・姿勢指令
４及びビークル位置・姿勢信号２７に基づいてビークル
速度指令１４を発生し、ビークル速度制御装置１５へ出
力する。このビークル速度制御装置１５は、上記ビーク
ル速度指令１４及び速度導出器２６から送られてくるビ
ークル速度信号２８に基づいてビークルトルク指令１６
を発生し、スラスタ及びバラストタンク制御装置１７に
出力する。上記速度導出器２６は、位置・姿勢検出器２
５から送られてくるビークル位置・姿勢信号２７に基づ
いてビークル速度信号２８を発生する。上記スラスタ及
びバラストタンク制御装置１７は、ビークルトルク指令
１６及び位置・姿勢検出器２５からのビークル位置・姿
勢信号２７に基づいてスラスタ・バラストタンク操作指
令１８を発生し、水中ロボット１９に出力する。

【００１３】水中ロボット１９は、双腕２０、ビークル
２１、バラストタンク２２、スラスタ２３、アンカーア
ーム２４を備えている。上記のように構成された位置・
姿勢制御装置１は、水中ロボット１９が浮遊または移動
状態において、双腕２０及び双腕モデル５に双腕操作指
令２を入力し、双腕２０を動作させると共に、双腕モデ
ル５より双腕動作時の双腕重心位置データ６を出力す
る。そして、双腕・アンカーアームの初期姿勢・ビーク
ル重心位置データ３を水中ロボットモデル７に入力す
る。水中ロボットモデル７は、初めの水中ロボット重心
位置データ８を出力し、上記双腕重心位置データ６と共
にアンカーアームモデル９に入力する。このアンカーア
ームモデル９は、初めの水中ロボット重心位置データ８
が一定になるようにアンカーアーム重心位置データ１０
を出力する。アンカーアーム操作指令導出器１１はアン
カーアーム重心位置データ１０からアンカーアーム２４
を駆動するアンカーアーム操作指令１２を出力し、双腕
２０の動作に応じて、水中ロボット１９のアンカーアー
ム２４を動作させる。

【００１４】また、ビークル位置・姿勢指令４と位置・
姿勢検出器２５から出力されたビークル位置・姿勢信号
２７をビークル位置・姿勢制御装置１３に入力すること
で、ビークル位置・姿勢制御装置１３は、ビークル位置
・姿勢指令４の位置・姿勢にビークル２１を保つように
ビークル速度指令１４をビークル速度制御装置１５に入
力する。そして、ビークル速度制御装置１５は、ビーク
ル速度指令１４とビークル位置・姿勢信号２７を基に速
度導出器２６により導出したビークル速度信号２８から
ビークル速度指令１４の速度にビークル２１を動かすよ
うにビークルトルク指令１６を出力する。スラスタ及び
バラストタンク制御装置１７は、ビークルトルク指令１
６とビークル位置・姿勢信号２７を基に、スラスタ・バ
ラストタンク操作指令１８を出力し、スラスタ２３及び
バラストタンクを駆動する。

【００１５】次に上記位置・姿勢制御装置１内の各部の
詳細及び位置・姿勢検出器２５の詳細について説明す
る。図２及び図３は、双腕モデル５の詳細を示すブロッ
ク図である。双腕モデル５は、図２に示す左腕モデル５
Ａと図３に示す右腕モデル５Ｂからなっている。

【００１６】双腕モデル５に与えられる双腕操作指令２
は、左腕１軸角度指令、左腕２軸角度指令、左腕３軸角
度指令、右腕１軸角度指令、右腕２軸角度指令及び右腕
３軸角度指令からなっている。双腕操作指令２内の左腕
１軸角度指令、左腕２軸角度指令、左腕３軸角度指令
は、図２に示す左腕モデル５Ａ内の回転変換器５０１，
５０９，５１７に入力される。回転変換器５０１の出力
は、１軸取付位置メモリ５０３の保持データと乗算器５
０２で乗算され、その乗算結果が１軸重心位置メモリ５
０４の保持データと乗算器５０５で乗算される。この乗
算器５０５の出力は、１軸重量メモリ５０７の保持デー
タと乗算器５０６で乗算されて加算器５０８に入力され
る。

【００１７】また、上記乗算器５０２の出力は、１軸リ
ンク長メモリ５１１の保持データと乗算器５１２で乗算
され、その乗算結果が回転変換器５０９の出力と乗算器
５１０で乗算される。この乗算器５１０の乗算結果は、
２軸重心位置メモリ５１４の保持データと乗算器５１３
で乗算され、その乗算結果が２軸重量メモリ５１６の保
持データと乗算器５１５で乗算されて加算器５０８に入
力される。

【００１８】また、上記乗算器５１０の出力は、２軸リ
ンク長メモリ５１９の保持データと乗算器５２０で乗算
され、その乗算結果が回転変換器５１７の出力と乗算器
５１８で乗算される。この乗算器５１８の乗算結果は、
３軸重心位置メモリ５２２の保持データと乗算器５２１
で乗算され、その乗算結果が３軸重量メモリ５２４の保
持データと乗算器５２３で乗算されて加算器５０８に入
力される。

【００１９】そして、加算器５０８の加算結果が左腕重
量分の１メモリ５２６の保持データと乗算されて左腕の
重心位置データが求められる。一方、図３に示す右腕モ
デル５Ｂにおいても、双腕操作指令２内の右腕１軸角度
指令、右腕２軸角度指令、右腕３軸角度指令に基づい
て、上記左腕モデル５Ａの場合と同様にして右腕の重心
位置データが求められる。

【００２０】そして、左腕モデル５Ａで求めた左腕の重
心位置データは、左腕重量メモリ５２８の保持データと
乗算器５２７で乗算されて加算器５２７に入力される。
また、右腕モデル５Ｂで求めた右腕の重心位置データ
は、右腕重量メモリ５３３の保持データと乗算器５３２
で乗算されて加算器５２７に入力される。この加算器５
２７の加算結果は、双椀重量分の１メモリ５３１の保持
データと乗算器５３０で乗算されて双腕重心位置データ
６として出力される。

【００２１】図４は、水中ロボットモデル７の詳細を示
すブロック図である。水中ロボットモデル７は、双腕モ
デル７１０、乗算器７０２，７０６，７０８，７１０、
双腕重量メモリ７０３、アンカーアーム重心位置導出器
７０５、アンカーアーム重量メモリ７０７、ビークル重
量メモリ７０９、加算器７０４及び水中ロボット重量分
の１メモリ７１１により構成される。

【００２２】水中ロボットモデル７には、双腕・アンカ
ーアームの初期姿勢・ビークル重心位置データ３が入力
される。双腕初期姿勢入力は、双腕モデル７１０により
双腕重心位置データに変換される。アンカーアーム初期
姿勢入力は、アンカーアーム重心位置導出器７０５によ
りアンカーアーム重心位置データに変換される。そし
て、上記双腕重心位置・アンカーアーム重心位置及びビ
ークル重心位置のデータをそれぞれ乗算器７０２，７０
６，７０８に入力し、双腕重量メモリ７０３、アンカー
アーム重量メモリ７０７、ビークル重量メモリ７０９の
保持データを掛け、加算器７０４で加算する。この加算
出力を乗算器７１０に入力し、水中ロボット重量分の１
メモリ７１１の保持データを掛けることにより水中ロボ
ット重心位置データ８を出力する。

【００２３】図５は、アンカーアームモデル９の詳細を
示すブロック図である。アンカーアームモデル９は、乗
算器９０１，９０４，９０７，９０９、双腕重量メモリ
９０２、水中ロボット重量メモリ９０５、ビークル重心
位置メモリ９０６、ビークル重量メモリ９０８、減算器
９０３及びアンカーアーム重量分の１メモリ９１０によ
り構成される。

【００２４】水中ロボットモデル７から送られてくる水
中ロボット重心位置データ８は乗算器９０４に入力さ
れ、水中ロボット重量メモリ９０５の保持データと乗算
されて減算器９０３の端子ａに入力される。ビークル重
心位置メモリ９０６の保持データは、乗算器９０７に入
力され、ビークル重量メモリ９０８の保持データと乗算
されて減算器９０３の端子ｂに入力される。

【００２５】また、双腕モデル５から送られてくる入力
双腕重心位置データ６は、乗算器９０１に入力され、双
腕重量メモリ９０２の保持データと乗算されて減算器９
０３の端子ｃに入力される。この減算器９０３は、端子
ａの入力データから端子ｂ，ｃの入力データを減算し、
乗算器９０９に出力する。乗算器９０９は、減算器９０
３の減算結果とアンカーアーム重量分の１メモリ９１０
の保持データを乗算し、その乗算結果をアンカーアーム
重心位置データ１０として出力する。

【００２６】図６は、アンカーアーム操作指令導出器１
１の詳細を示すブロック図である。アンカーアーム操作
指令導出器１１は、アンカーアーム２４の操作指令を入
力し、アンカーアーム重心位置データを出力するアンカ
ーアーム重心位置導出器の逆モデルである。例えば、ア
ンカアーム重心位置導出器を以下の式で表せるとする。

【００２７】

【数１】 この式の行列［Ａ］の逆行列［Ａ］^-1を用いて、アンカ
ーアーム重心位置導出器の逆モデルは、

【００２８】

【数２】 で表される。

【００２９】この様なアンカアーム重心位置導出器逆モ
デルを用いることにより、アンカアーム操作指令導出器
１１は、アンカーアームモデル９からのアンカーアーム
重心位置データ１０を入力すると、アンカーアーム操作
指令１２を出力する。

【００３０】図７は、ビークル位置・姿勢制御装置１３
の詳細を示すブロック図である。ビークル位置・姿勢制
御装置１３は、減算器１３１ａ〜１３１ｆ、乗算器１３
２ａ〜１３２ｆ、比例ゲイン１３３ａ〜１３３ｆ，加算
器１３４ａ〜１３４ｆ、積分器１３５ａ〜１３５ｆによ
り構成される。

【００３１】ビークル位置・姿勢制御装置１３には、ビ
ークル位置・姿勢指令４の位置指令１〜３、姿勢指令１
〜３及びビークル位置・姿勢信号２７のビークル位置１
〜３、姿勢１〜３が入力される。位置指令１〜３、姿勢
指令１〜３からビークル位置１〜３、姿勢１〜３をそれ
ぞれ減算器１３１ａ〜１３１ｆにより減算し、指令との
偏差を出力する。この偏差を乗算器１３２ａ〜１３２ｆ
に入力して比例ゲイン１３３ａ〜１３３ｆをそれぞれ掛
ける。そして、定常偏差をなくすため、乗算器１３２ａ
〜１３２ｆの出力を積分器１３５ａ〜１３５ｆに入力
し、乗算器１３２ａ〜１３２ｆの出力と積分器積分器１
３５ａ〜１３５ｆの出力を加算器１３４ａ〜１３４ｆに
より加算し、ビークル２１の位置・姿勢を修正するビー
クル速度指令１４として出力する。

【００３２】図８は、ビークル速度制御装置１５の詳細
を示すブロック図である。ビークル速度制御装置１５
は、減算器１５１ａ〜１５１ｆ、乗算器１５２ａ〜１５
２ｆ、比例ゲイン１５３ａ〜１５３ｆにより構成され
る。

【００３３】ビークル速度制御装置１５には、ビークル
速度指令１４のビークル速度指令１〜６とビークル速度
信号２８のビークル速度１〜６が入力される。上記各ビ
ークル速度指令１〜６とビークル速度１〜６を減算器１
５１ａ〜１５１ｆにより減算して速度偏差を出力する。
この各速度偏差と比例ゲイン１５３ａ〜１５３ｆを乗算
器１５２ａ〜１５２ｆで乗算し、ビークルトルク指令１
６を出力する。

【００３４】図９は、スラスタ及びバラストタンク制御
装置１７の詳細を示すブロック図である。スラスタ及び
バラストタンク制御装置１７は、スラスタ及びバラスト
タンク指令−ビークルトルク指令導出モデルの逆モデル
を用いてビークルトルク指令１６とビークル位置・姿勢
信号２７を入力し、スラスタ操作指令１〜８，バラスト
タンク操作指令９〜１２のスラスタ・バラストタンク操
作指令１８を出力する。スラスタ及びバラストタンク指
令−ビークルトルク指令導出モデルは以下の式で表せ
る。

【００３５】

【数３】 この式よりスラスタ及びバラストタンク指令−ビークル
トルク指令導出モデルの逆モデルは次式となる。

【００３６】

【数４】

【００３７】このようなスラスタ及びバラストタンク指
令−ビークルトルク指令導出モデルの逆モデルを用いる
ことにより、ビークルトルク指令１６とビークル位置・
姿勢信号２７からスラスタ・バラストタンク操作指令１
８を得ることができる。

【００３８】図１０は、速度導出器２６の詳細を示すブ
ロック図である。速度導出器２６は、減算器２６１、前
回検出位置・姿勢記憶器２６２、第１の比例器２６３、
加算器２６４、前回速度記憶器２６５及び第２の比例器
２により構成される。

【００３９】速度導出器２６には、ビークル位置・姿勢
信号２７を入力し、この入力と前回検出位置・姿勢記憶
器２６２からの前回検出位置・姿勢出力とを減算器２６
１により減算する。この減算器２６１の出力を第１の比
例器２６３に入力し、比例ゲインを掛けた値を出力す
る。前回速度記憶器２６５では、ビークル速度２８の前
回速度を保持し、第２の比例器２６６に出力して前回速
度と比例ゲインを掛けた値を加算器２６４に出力する。
加算器２６４では比例器２６３，２６６の出力を加算
し、その加算結果をビークル速度２８として出力する。

【００４０】図１１は、位置・姿勢検出器２５の詳細を
示すブロック図である。位置・姿勢検出器２５は、ビー
クルの位置と姿勢の検出を行なうカメラ２５１，２５
２，２５３、及びこれらのカメラ２５１，２５２，２５
３の撮影データよりビークルの位置・姿勢計測値、つま
りビークル位置・姿勢信号２７を導出する位置・姿勢計
測器２５４により構成される。

【００４１】位置・姿勢検出器２５は、３台のカメラ２
５１，２５２，２５３による３点の検出値を基に、３次
元のビークル位置・姿勢信号２７を出力する。上記のよ
うに、双腕操作指令２及び双腕・アンカーアームの初期
姿勢ビークル重心位置データ３を基に、アンカーアーム
重心位置データ１０を双腕モデル５と水中ロボットモデ
ル７により導出し、アンカーアーム操作指令導出器１１
に入力してアンカーアーム操作指令１２を出力し、アン
カーアーム２４を動かすことによって水中ロボット１９
の姿勢を保つことができる。また、ビークル位置・姿勢
指令４を基に、ビークル位置・姿勢制御装置１３、ビー
クル速度制御装置１５、スラスタ及びバラストタンク制
御装置１７によりスラスタ・バラストタンク操作指令１
８を導出し、スラスタ２３及びバラストタンク２２を駆
動することによって、水中ロボット１９の位置・姿勢を
保持することができる。

【００４２】

【発明の効果】以上詳記したように本発明による水中ロ
ボット位置・姿勢制御装置を適用することによって、移
動する環境が複雑な場合でも、水中ロボットを移動する
ことができ、多関節マニピュレータを使った作業を行な
い易い位置・姿勢にすることができる。また、水中ロボ
ットが浮遊状態の場合においても、多関節マニピュレー
タを使った作業が可能となる。

【図面の簡単な説明】

【図１】本発明の一実施例に係る水中ロボットの位置・
姿勢制御装置の全体構成を示すブロック図。

【図２】図１における双腕モデルの左腕モデル部分の詳
細を示すブロック図。

【図３】図１における双腕モデルの右腕モデル部分の詳
細を示すブロック図。

【図４】図１における水中ロボットモデルの詳細を示す
ブロック図。

【図５】図１におけるアンカーアームモデルの詳細を示
すブロック図。

【図６】図１におけるアンカーアーム操作指令導出器の
詳細を示すブロック図。

【図７】図１におけるビークル位置・姿勢制御装置の詳
細を示すブロック図。

【図８】図１におけるビークル速度制御装置の詳細を示
すブロック図。

【図９】図１におけるスラスタ及びバラストタンク制御
装置の詳細を示すブロック図。

【図１０】図１における速度導出器の詳細を示すブロッ
ク図。

【図１１】図１における位置・姿勢検出器の詳細を示す
ブロック図。

【図１２】従来の水中ロボットの制御装置を示す概要
図。

【符号の説明】

１…位置・姿勢制御装置、２…双腕操作指令、３…双腕
・アンカーアームの初期姿勢・ビークル重心位置デー
タ、４…ビークル位置・姿勢指令、５…双腕モデル、６
…双腕重心位置データ、７…水中ロボットモデル、８…
水中ロボット重心位置データ、９…アンカーアームモデ
ル、１０…アンカーアーム重心位置データ、１１…アン
カーアーム操作指令導出器、１２…アンカーアーム操作
指令、１３…ビークル位置・姿勢制御装置、１４…ビー
クル速度指令、１５…ビークル速度制御装置、１６…ビ
ークルトルク指令、１７…スラスタ及びバラストタンク
制御装置、１８…スラスタ・バラストタンク操作指令、
１９…水中ロボット、２０…双腕、２１…ビークル、２
２…バラストタンク、２３…スラスタ、２４…アンカー
アーム、２５…位置・姿勢検出器、２６…速度導出器、
２７…ビークル位置・姿勢信号、２８…ビークル速度信
号。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 矢口 誓児 兵庫県神戸市兵庫区和田崎町一丁目１番１ 号 三菱重工業株式会社神戸造船所内

Claims (1)

【特許請求の範囲】
1. 【請求項１】 水中ロボットの双腕を操作する双腕操作
   指令により上記双腕の重心位置データを導出する双腕モ
   デルと、 上記双腕及び双腕の動作に応じて動き、水中ロボットの
   姿勢を保つアンカーアームの初期姿勢及び水中ロボット
   のビークル重心位置データに基づいて、水中ロボットの
   重心位置データを導出する水中ロボットモデルと、 上記双腕モデルからの双腕重心位置データ及び水中ロボ
   ットモデルからの水中ロボット重心位置データに基づい
   て、アンカーアームの重心位置データを導出するアンカ
   ーアームモデルと、 上記アンカーアームモデルにより導出されたアンカーア
   ーム重心位置データに基づいてアンカーアームの操作指
   令を導出するアンカーアーム操作指令導出器と、 上記水中ロボットのビークル位置・姿勢を検出する位置
   ・姿勢検出器と、 ビークル位置・姿勢指令と上記位置・姿勢検出器からの
   ビークル位置・姿勢信号に基づいてビークル速度信号を
   出力するビークル位置・姿勢制御装置と、 上記ビークル位置・姿勢制御装置からのビークル速度指
   令と速度導出器により導出されたビークル速度信号に基
   づいて、ビークルトルク指令を出力するビークル速度制
   御装置と、 上記ビークルトルク指令及びビークル位置・姿勢信号に
   基づいて、上記水中ロボットの推進部のスラスタ及びバ
   ラストタンクの操作指令を導出するスラスタ及びバラス
   トタンク制御装置と、を具備したことを特徴とする水中
   ロボット位置・姿勢制御装置。