水下机器人六自由度运动仿真器的设计

X& = E −1 (Fvis + Ft )
（6）
这样就得到水下机器人 6 自由度运动方程的矩阵表达形式。此 方程考虑了非线性水动力，精度较高，适用于任何一般水下机器人， 对于方程中非线性水动力导数，有些不容易得到，可以用零代替。
3.推力器仿真 求解水下机器人推力器推力，首先要通过敞水实验获得推力器
u
xqqq2 + xrrr 2 + xpr pr + xvrvr + xwqwq + xu|u|u | u | + xvvv2 + xwww2 + X t
将右端非惯性水动力表示成：
Fvis = [ X vis Yvis Z vis K vis M vis N vis ]'
（3）
令 X = [u v w p q r]'
2.水动力(Hydrodynamic) 艇体水动力是运动参数u、v、w、p、q、r、u& 、v&、w& 、 p& 、q& 、r&的
函数。在基准点（通常以速度U0作匀速直航）将艇体水动力XH、YH、 ZH、KH、MH、NH作泰勒展开，参考施生达《潜艇操纵性》一书， 我们对艇体水动力作相应的简化，最后得到如下的水下机器人的艇
[ ] m ⋅ (v& − wp + ur) − yG (r 2 + p 2 ) + zG (qr − p& ) + xG (qp + r&) = Y = YH + Yt [ ] m ⋅ (w& − uq + vp) − zG ( p 2 + q 2 ) + xG (rp − q&) + yG (rq + p& ) = Z = Z H + Zt (1)
考虑到 yG = 0 ，所以推得：
⎡m ⎢ ⎢
−X 0
u&
⎢0
E=⎢ ⎢
0
⎢ ⎢
mzG
⎢⎣ 0
0 m − Yv&
0 − mzG − K v&
0 mxG − N v&
0
0 m − Z w&
0 − mxG − M w&
0
0 − mzG − Yp&
0 I x − K p&
0 − N p&
于是得到方程 EX& = Fvis + Ft
I z r& + (I y − I z ) pq + m ⋅ [xG ⋅ (v& + ur − pw) − yG ⋅ (u& + qw − vr )] = N = N H + N t
m——水下机器人的质量； xG ,yG ,zG——水下机器人的重心坐标；
Ix、Iy、Iz——水下机器人的质量m对 0x,0y,0z轴的转动惯量；
图 3.12 航速测定仿真试验与水池试验对比 Fig.3.12 Comparison of velocity test in simulation and tank
推力系数Kt
0.8
0.6
0.4
0.2
0
-1.5
-1
-0.5 -0.2 0
0.5-0.4-0.6图2 推力性能曲线11.5
进速系数J
正转 反转
Fig. 2 Thrust capability Curve
4.环境影响（Environment simulation） 通常，水下机器人受到的环境干扰力较为复杂，如海流影响，
体水动力表达式:
X H = [ X qqq 2 + X rr r 2 + X rprp] + [ X u&u& + X vr vr + X wq wq] + [ X uuu 2 + X vvv2 + X www2 ]
YH = [Yr& r& + Yp& p& + Yp p p p + Ypq pq + Yqr qr] + [Yv&v& + Yvqvq + Ywp wp + Ywr wr]
壁面效应，海水浓度及温度的影响等。这里假定水域为深广水域。
除海流外，忽略其他因素的影响。
设来流平行于大地坐标系的水平面，海流流速为定值，求得相
对流速：
⎪⎨⎧uvrr
= =
u − U c cos(θ) cos(αc v − U c sin(αc − ψ)
−
ψ)
⎪⎩wr = w − U c sin(θ) cos(αc − ψ)
+ [K pup + K rur + Kv&v&] + [Kvqvq + K wp wp + K wr wr]
+ [K 0u 2 + K vuv + K v v v (v 2 + w2 )1/ 2 ] + K vwvw
（2）
M H = [M q& q& + M pp p 2 + M rr r 2 + M rp rp + M q q q q ]
第一步，若螺旋桨转速为 0，通过一个拟合的y=(x-a)2函数求解推
力；
第二步，若螺旋桨转速不为 0，由航行速度与螺旋桨转速（由控制 器的控制指令决定）求得 J；
第三步，若求得的 J 值在 Kt 曲线范围外，同第一步； 第四步，若求得的 J 值在 Kt 曲线范围内，代入神经网络，求得推 力系数；
第五步，由推力系数求得推力 Ft = Ktn2D4 ，再由力臂求得力矩；
E ξ
o
η
y
x
z
ζ
图 1 坐标系
Fig. 1 Coordinate system
根据刚体动力学理论，水下机器人空间六自由度运动的一般方程如 下（其中 XH 指潜艇受到的水动力，Xt 指推力器推力，没有考虑其他复 杂情况受力）：
[ ( ) ] m ⋅ (u& − vr + wq) − xG ⋅ q 2 + r 2 + yG ( pq − r&) + zG ( pr + q&) = X = X H + X t
+ [Z quq + Z w q
w w
(v 2
+ w2 )1/ 2
q ] + [Z0u2
+ Z wuw + Z w w w (v 2
+ w2 )1/ 2 ]
+ [Z w u w + Z ww w(v 2 + w2 )1/ 2 ] + Z vvv 2
K H = [K p& p& + K r& r& + K qr qr + K pq pq + K p p p p ]
图 3.11 4.3V 时纵向坐标 Fig.3.11 Longitudinal coordinate at 2.8V
在保持航向的情况下主推电压与航速之间关系的仿真结果与实
验值之间的比较如图 3.12 所示，仿真得到的结果，与实验结果很接 近，在电压低于 3.2V 时仿真得到的速度大于实际机器人航行的速 的，当推力器电压大于 3.2V 时仿真得到的速度小于实验测得的机器 人航速，在推力器电压大于 4.3V 之后仿真得到的速度又一次大于实 际机器人的航速。相对而言，仿真结果过渡更均匀。
（8）
将得到的相对加速度代到运动方程后, 就可得到所求的仿真方程。
5.仿真结果(Simulation result)
为验证上述水下机器人仿真模型的正确性，进行了如下的仿真
试验，并将仿真结果与试验结果进行对比。下面分别说明。
1) 直航试验：
在仿真环境中设定：微小型水下机器人两个推力器提供一定的
电压值，此时水平舵舵角为零，机器人从静止状态经过加速达到匀
式中 X u& , X uu Yv&,Yv 等都为艇体水动力导数。 将（2）式带入方程（1），并将式中所有惯性水动力移到方程（1）
的左端，所有的非惯性水动力移到方程（1）的右端，可以得到方程：
.
.
.
.
mu− myG r+ mzG q− x. u = m[vr − wq + xG (q2 + r2 ) − yG pq − zG pr] +
大家看时请参考《潜艇操纵性》159 页 水下机器人六自由度运动仿真器的设计
（哈尔滨工程大学船舶与海洋工程系 哈尔滨 150001） 摘 要: 本文首先分析水下机器人的受力，得到水下机器人六自由 度运动的操纵性方程。为了简化程序中的运算，本文将水下机器人 的六个自由度的方程通过用 matlab 解算，转换为矩阵形式。然后考 虑了水下机器人的推力器仿真，环境仿真，完成水下机器人六自由 度运动仿真器的设计。最后用该仿真系统进行了运动仿真，仿真结 果与海上试验吻合得很好，表明该仿真系统是可靠的。该仿真系统 采用一种普遍实用的数学模型，并考虑了非线性水动力，精度较高， 适用于任何一般类型的水下机器人，对研究水下机器人操纵与控制 有很大现实意义。 关键词: 水下机器人；仿真；数学模型
u、v、w、p、q、r——六个自由度的（角）速度； u& 、v&、 w& 、 p& 、q& 、r& ——六个自由度的（角）加速度。 X、Y、Z、K、M、N——六个自由度的力（矩）；
方程左端 X、Y、Z、K、M、N 表示作用在水下机器人上的的作 用力（矩），包括：水下机器人受到的重力和浮力等静力、推进器推 力、水下机器人运动引起的流体水动力和一些环境干扰力等。以下 将分别讨论。