2A1-05-004
浮遊型 2 リンク水中ロボット マニピュレータの分解速度制御
Resolved Motion Rate Control of a Floating Underwater Robot
with 2 Link Manipulator
正 相良 慎一 (九工大)
○ 檀上 孝博 (九工大)
谷川 竹司 (九工大)
正 田村 正和 (九工大)
正 加藤 了三 (九工大)
Shinichi SAGARA, Takahiro DANJOH, Takeshi TANIKAWA, Masakazu TAMURA, and Ryozo KATOH
Kyushu Institute of Technology, Tobata, Kitakyushu 804-8550
In this paper, we suggest Resolved Motion Rate Control(RMRC) of Underwater Robot Manipulators using
Generalized Jacobi Matrix of space robots. Since hydrodynamic force act in the water, we treat this force as
a disturbance and add a position error feedback to compentate the disturbance. Simulation and experimental
results show the e ectiveness the control method.
Key words : Underwater Robot Manipulator, Generalized Jacobi Matrix, Resolved Motion Rate Control.
1. はじめに
海洋開発では，人間に代わって作業を行うロボットが
必要であり，近年水中ロボットに関する研究が活発に行
1) 2)
われている
. しかし，その大部分は水中ヴ ィークルに
関するものであり，マニピュレ ータを有する水中ロボッ
トについては，そのモデルが提案され始めたのが現状で
3)
ある . 著者らは，2 次元水平面上で運動する浮遊型 2 リ
4)
ンク水中ロボット マニピュレ ータのモデルを導出する
とともに，分解加速度制御系を構成した実験により手先
5)
位置制御が可能であることを示している ．しかし，分解
加速度制御系は手先の速度情報が必要であるため，ここ
では宇宙ロボットに対して提案されている一般化ヤコビ
6)
行列 を用いて，手先位置誤差フィード バックを付加し
た分解速度制御系を構成し 手先位置制御シミュレ ーショ
ンおよび 実験を行った結果について報告する．
2. モデリング
2.1. モデルの設定
記号]
U ：慣性座標系
0 ：ロボット本体 (リンク 0) の重心を原点とし ，リ
ンク 0 に固定した座標系
i ：関節 i を原点とし ，リンク i に固定し た座標系
(i = 1 2)
r0 ：慣性座標系におけるベース (リンク 0) の重心の
位置ベクトル
pe ：慣性座標系における手先の位置・姿勢ベクトル
i ：各リンクの相対関節角度
T
：関節角度ベクトル ( =
1
2 ] )
2.2. 流体力
流体中で物体が運動するとき，その物体は周囲の流体
により反力を受ける．ここでは，この流体力は物体の移
動速度に比例する慣性力と物体の移動速度の二乗に比例
3)
する流体抗力の和で与えられるものとする ．
慣性力に関しては付加質量および 付加慣性モーメント
を考える．一方，流体抗力および 関節回りの抗力モーメ
ントはそれぞれ次式で与えられるとする．
f di =
1 自由度の回転関節を有する 2 リンクマニピュレ ータ
を搭載し ，2 次元平面上で動作する浮遊型水中ロボットモ
デルに対して Fig.1 に示すモデルを考える．また，本論
文で使用する座標系および主な記号を以下に示す．
pe
Y
X
2
ΣU
Σ2
2
Link 2
l2
X
Joint 2
r0
Y
1
Y
X
Σ1
1
0
X
φ 0 Joint 1
0
Σ0
Base
(Link 0)
2
(1)
Z li
CDiDi
x
^i kwikwidx
^i
(2)
2
0
0
ただし ，wi = p_ i + !i x
^i であり， ,CDi,Di はそれ
ぞれ流体密度，抗力係数，リンク i の鉛直方向長さであ
り，p_ i は i における関節 i の移動速度ベクトル，!i は
関節 i の回転角速度ベクトル，x
^i は 関節 i からの関節
(i + 1) 方向へのベクトルである．
φ2
Y
tdi =
Z li
CDiDi
kwi kwidx
^i
φ1
Link 1
l1
b0
Fig.1 Model of 2-link underwater robot manipulator
2.3. 運動方程式
2.2 の流体力を考慮すると，ニュートン・オイラー法に
より次の運動方程式が得られる．
(M + Ma )q + b(q q_ ) + FD
= 0 0 0
1
2
]T (3)
T ]T であり，X は ベースの重心位
ただし，q = X T
置および姿勢からなるベクトル， は 関節変数ベクトル，
i は 関節 i のトルク，b(q q_ ) は 遠心力およびコリオリ
力からなるベクトル，FD は 流体抗力および抗力モーメ
ントからなるベクトル，M は 慣性行列，Ma は 付加質
量および 付加慣性モーメントからなる行列である．
日本機械学会[No.00-2]ロボティクス・メカトロニクス講演会’００講演論文集 ［2000.5.12∼13,熊本］
2A1-05-004（１）
3. 分解速度制御則
参考文献
Fig.1 のモデルにおいて流体力が作用しない，すなわち
運動量および 角運動量が保存されると仮定すると，手先
速度と関節角速度の関係は浮遊型 2 リンク宇宙ロボット
と同一の式
(4)
p_ e = J _
となる ．ただし ，J は一般化ヤコビ 行列である．
実際には 流体力が存在するので，ここでは手先位置誤
差フィード バックを付加し ，次式の目標関節角速度 _ dを
ロボットへ与える．
6)
1 p_ + K (p
ed
p ed
pe )]
(5)
2.0
1.5
V[m/s ]
ただし，p_ ed ped はそれぞれ目標手先速度，目標手先位
置であり，また Kp はフィード バックゲインである．
3.1. 計算機シミュレーション
ここでは (3) 式の運動方程式を用いて計算機シミュレー
ションを行った結果を示す．Table 1 にシミュレーション
に用いたロボットの緒元を示す．フィード バックゲイは
Kp = 10 とし ，Fig.2 の目標速度パターンを与えた．初
期姿勢を 0 = 0 deg] 1 = 45 deg] 2 = 45 deg] とし ，
手先をX,Y 方向にそれぞれ 0:1 m], 0:1 m] 動かした場
合の結果を Fig.3 に示す．
図より，ロボット 手先は目標軌道に追従していること
が読取れフィード バックを用いることにより，流体力を
制御則において考慮し なくても手先位置制御が行えるこ
とを確認した．
3.2. 実験結果
1.0
0.5
0.0
0
4
8
Time[s ]
0 .2
7DUJHW
0 .1
0
- 0 .4
- 0 .2
0
0 .2
0 .4
0 .6
0 .8
1
- 0 .1
)LQDO VWDWH
X[m ]
- 0 .2
(a) Motion of the robot
× 10
−4
2
1.5
1
0.5
0
0
2
4
6
8
Time[s]
10
12
14
(b) Tracking error
Fig.3 Simulation result of RMRC
Initial state
0.2
Target
0.0
-0.4
-0.2
0.0
0.2
0.4
X [m]
-0.2
0.6
0.8
1.0
Final state
(a) Motion of the robot
Tracking error[m]
2
Link-2
9.68
0.37
{
0.30
32.82
0.33
14
Y[m]
ニピュレータに対し ，宇宙ロボットで提案されている一
般化ヤコビ 行列を用いて，手先位置誤差フィード バック
を付加した分解速度制御を構成してシミュレ ーションお
よび実験を行った結果，流体力を考慮しなくても良好な
制御結果が得られることを確認した．
12
,QLWLDO VWDWH
0.4
2 次元平面運動をする浮遊型 2 リンク水中ロボット マ
10
0 .3
4. おわりに
Table 1 Speci cation of the robot
Base Link-1
Mass
kg 31.72 7.48
Moment of Inertia kg m 0.41 0.20
b0
m 0.16
{
li
m
{
0.30
Added Mass
kg 48.26 32.82
Added Inertia kg m 0.0 0.15
6
Fig.2 Trapezoidal velocity pattern
2.5
ここでは，制御実験を行った結果を示す. なお，実験に
用いたロボットの緒元および条件はシミュレーションと
同じである．
図より，シミュレーション結果と同様にロボット手先
は目標軌道に良好に追従し ており，手先位置誤差フィー
ド バックを付加した分解速度制御の有効性を確認した．
2
Y[m ]
J
Tracking error[m]
_d =
1) J.Yuh ed., ”Underwater Robotic Vehicles Design and Control ”, TSI Press, 1995
2) J.Yuh, T.Ura and G. A.Bekey ed., ”Underwater Robots ”,
Kluwer Academic Publishers, 1996
3) T.W.McLain, S.M.Rock, M.J.Lee, ”Experiments in the
Coordinated Control of an Underwater Arm/Vehicle System ”, Autonomous Robots 3, pp.213-232, 1996
4) 谷川，高橋，相良，加藤，”浮遊型２リンク水中ロボットマ
ニピュレータのモデリング ”，日本機械学会，ロボティクス・メ
カトロニクス講演会'99 講演論文集，2P1-04-026， 1999
5) 谷川，相良，加藤，”浮遊型 2 リンク水中ロボットマニピュ
レータの分解速度制御 ”，第 18 回計測自動制御学会九州支部学
術講演会予稿集，pp.103-106，1999
6) 梅谷, 吉田，”一般化ヤコビ 行列を用いた宇宙用ロボット マ
ニピュレ ータの分解速度制御 ”，日本ロボット学誌会，No.7，
Vol.4，pp.63-73，1988
× 10 −3
1.5
1
0.5
0
0
2
4
6
8
Time[s ]
10
12
14
(b) Tracking error
Fig.4 Experimental result of RMRC
日本機械学会[No.00-2]ロボティクス・メカトロニクス講演会’００講演論文集 ［2000.5.12∼13,熊本］
2A1-05-004（２）
© Copyright 2024 ExpyDoc
