Итак, я пишу код для моделирования движения подводной лодки с 6 степенями свободы. Я пытаюсь скопировать конкретную c бумагу , прикрепленную здесь , но мне кажется, что цифры не совпадают ни в малейшей степени. Будем весьма благодарны за любые отзывы о том, как получить то, что у них есть, или о том, как я поступаю неправильно!
Я приложил свой код ниже. Кажется, что время действительно заканчивается, и я не понимаю, почему, и если я изменю входные данные руля, они создают сумасшедшие графики. Кроме того, графики просто не соответствуют бумаге, которую я пытаюсь воспроизвести.
Спасибо!
import numpy as np
from numpy import sin,cos,pi,tan
from scipy.integrate import doesn't
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def Sub(state,t):
#constants to be used for calculation.
g = 9.81
#Hull Parameters
rho = 1.03E3 #Density of water
Af = 2.85E-2 #Hull Frontal Area
Ap = 2.26E-1 #Hull Projected Area (ex plane)
Sw = 7.09E-1 #Hull Wetted Surface Area
delta = 3.15E-2 #Estimated Hull Volyme
W = 2.99E2 #Measured Vehicle Weight
B = 3.08E2 #Measured Vehicle Buoyancy
m = W/g
Best = 3.17E2 #Estimated Hull Buoyancy
xcbest = 5.54E-3 #Est. Long. Centre of Buoyancy
c_d = 3E-1 #Vessel Axial Drag Coeff
c_dc = 1.1 #Cylinder Crossflow drag coeff
c_ydbeta = 1.2 #Hoerner Body Lift Coeff
x_cp = -3.21E-1 #Centre of pressure
alpha = 3.59E-2 #Ellipsoid Added Mass coef
#Moments of Inertia
Ixx =1.77E-1
Iyy = 3.45
Izz = 3.45
#Fin Parameters
Sfin = 6.65E-3 #Platform Area
bfin= 8.57E-2 #Span
x_finpos = 6.38-1 #Moment Arm wrt Vehicle Origin at CB
delta_max = 1.36E1 #Maximum fin angle
a_fin = 5.14 #Max Fin Height Above Centreline
c_mean = 7.47E-2 #Mean Chord Length
t = 6.54E-1 #Fin Taper Ratio
c_df = 5.58E-1 #Finn Crossflow Drag Coefficient
AR_e = 2.21 #Effective Aspect Ratio
a_bar = 9E-1 #Lift Slope Parameter
CL_alpha = 3.12 #Fin Lift Slope
#Centre of Buoyancy wrt Origin at Vehicle Nose
xb = -6.11E-1
yb = 0
zb = 0
#Centre of gravity wrt Origin at CB
xg = 0
yg = 0
zg =1.96
#Non-Linear Force Coefficients
Xuu = -1.62 #kg/m Cross-flow Drag
Xudot = -9.30E-1 #kg Added Mass
Xwq = -3.55E1 #kg/rad Added Mass Cross-term
Xqq = -1.93 #kg.m/rad Added Mass Cross term
Xvr = 3.55E1 #kg/rad Added Mass Cross term
Xrr = -1.93 #kg.m/rad Added Mass Cross Term
Xprop = 3.86 #N Propeller Thrust
Yvv = -1.31E3 #kg/m Cross-flow Drag
Yrr = 6.32E-1 #kg.m/rad^2 Cross Flow Drag
Yuv = -2.86E1 #Body Lift Force and Fin lift
Yvdot = -3.55E1 #kg Added Mass
Yrdot = 1.93 #kg.m/rad Added Mass
Yur = 5.22 #kg/rad Added Mass Cross Term and Fin Lift
Ywp = 3.55E1 #kg/rad Added Mass Cross Term
Ypq = 1.93 #kg.m/rad Added Mass Cross-term
Yuudr = 9.64 #kg/(m.rad) Fin Lift Force
Zww = -1.31E2 #kg/m Cross-Flow Drag
Zqq = -6.32 #kg.m/rad^2 Cross Flow Drag
Zuw = -1.31E2 #kg/m Cross-Flow Drag
Zwdot = -3.55E1 #kg Added Mass
Zqdot = -1.93 #kg.m/rad Added Mass
Zuq = -5.22 #kg/rad Added Mass Cross-term and Fin Lift
Zvp = -3.55E1 #kg/rad Added Mass Cross-term
Zrp = 1.93 #kg/rad Added Mass Cross-term
Zuuds = -9.64 #kg/(m.rad) Fin Lift Force
Kpp = -1.30E-1 #kg.m^2/rad^2 Rolling Resistance
Kpdot = -7.04E-2 #kg.m^2/rad Added Mass
Kprop = -5.43E-1 #Nm Propeller Torque
Mww = 3.18 #kg Cross-Flow Drag
Mqq = -1.88E2 #kg.m^2/rad^2
Muw = 2.4E1 #kg Body and Fin Lift and Munk Moment
Mwdot = -1.93 #kg.m Added Mass
Mqdot = -4.88 #kg.m/rad Added Mass
Muq = -2.00 #kg.m/rad Added Mass Cross Term and Fin Lift
Mvp = -1.93 #kg.m/rad Added Mass Cross Term
Mrp = 4.86 #kg.m/rad^2 Added Mass Cross Term
Muuds = -6.15 #kg/rad Fin Lift Moment
Nvv = -3.18 #kg Cross-Flow Drag
Nrr = -9.40E1 #kg.m^2/rad^2 Cross-Flow Drag
Nuv = -2.40E1 #kg Body and Fin Lift and Munk Moment
Nvdot = 1.93 #kg.m Added Mass
Nrdot = -4.88 #kg.m^2/rad Added Mass
Nur = -2.00 #kg.m/rad Added Mass Cross Term and Fin Lift
Nwp = -1.93 #kg.m^2/rad^2 Added Mass Cross Term
Npq = -4.86 #kg.m^2/rad^2 Added Mass Cross-Term
Nuudr = -6.15 #kg/rad Fin Lift Moment
delta_s = 0 #Stern fin angle
delta_r = 0 #Rudder fin angle
#unpack the state vector(these are
#velocities(u,v,w,p,q,r),positions(x,y,z),angles(phi,theta,psi)
u = state[0]
v = state[1]
w = state[2]
p = state[3]
q = state[4]
r = state[5]
x = state[6]
y = state[7]
z = state[8]
phi = state[9]
theta = state[10]
psi = state[11]
#Forces acting in X,Y,Z direction and Moments acting in X(K), Y(M), Z(N)
X = (-((W-B)*sin(theta)) + Xuu*u*abs(u) + ((Xwq-m)*w*q) + ((Xqq + m*xg)*q**2) \
+ ((Xvr+m)*v*r) + ((Xrr + m*xg)*r**2) - (m*yg*p*q) - (m*zg*p*r) \
+ Xprop)
Y = (((W-B)*cos(theta)*sin(phi)) + Yvv*v*abs(v) + Yrr*r*abs(r) + (Yuv*u*v) \
+ ((Ywp+m)*w*p) + ((Yur-m)*u*r) - ((m*zg)*q*r) + ((Ypq - m*xg)*p*q) \
+ (Yuudr*u**2*delta_r))
Z = (((W-B)*cos(theta)*cos(phi)) + Zww*w*abs(w) + Zqq*q*abs(q) \
+ Zuw*u*w + (Zuq+m)*u*q + ((Zvp-m)*v*p )+ ((m*zg)*p**2) + ((m*zg)*q**2) \
+ ((Zrp - m*xg)*r*p) + Zuuds*u**2*delta_s)
K = ((-(yg*W-yb*B)*cos(theta)*cos(phi)) \
-((zg*W-zb*B)*cos(theta)*sin(phi)) \
+ (Kpp*p*abs(p)) - ((Izz-Iyy)*q*r) - ((m*zg)*w*p) + ((m*zg)*u*r) + Kprop)
M = ((-(zg*W-zb*B)*sin(theta)) - ((xg*W-xb*B)*cos(theta)*cos(phi)) + (Mww*w*abs(w)) \
+ (Mqq*q*abs(q)) + ((Mrp - (Ixx-Izz))*r*p) + ((m*zg)*v*r) - ((m*zg)*w*q) \
+ ((Muq - m*xg)*u*q) + (Muw*u*w) + ((Mvp + m*xg)*v*p) \
+ (Muuds*u**2*delta_s))
N = ((-(xg*W-xb*B)*cos(theta)*sin(phi)) - ((yg*W-yb*B)*sin(theta)) \
+ (Nvv*v*abs(v)) + (Nrr*r*abs(r)) + Nuv*u*v \
+ ((Npq - (Iyy-Ixx))*p*q) + ((Nwp - m*xg)*w*p) + ((Nur + m*xg)*u*r) \
+ (Nuudr*u**2*delta_r))
#Mass Matrix
MASS = np.array([[m-Xudot, 0, 0, 0, m*zg, -m*yg],
[0, m-Yvdot, 0, -m*zg, 0, m*xg-Yrdot],
[0, 0, m-Zwdot, m*yg, -m*xg-Zqdot, 0],
[0, -m*zg, m*yg, Ixx - Kpdot, 0, 0],
[m*zg, 0, -m*xg-Mwdot, 0, Iyy - Mqdot, 0],
[-m*yg, m*xg-Nvdot, 0, 0, 0, Izz-Nrdot]])
#Collect the forces into a single matrix
FORCES = np.array([X,Y,Z,K,M,N])
ACC = np.linalg.solve(MASS,FORCES) #Solving to find body accelerations, ACC = [udot,vdot,wdot,p,q,r]
print(ACC)
#Rotation matrices to turn u,v,w into xdot,ydot and zdot and p,q,r into phidot, thetadot, psidot
J1 = np.array([[cos(psi)*cos(theta), -sin(psi)*cos(phi)*sin(phi), sin(psi)*sin(phi)+cos(psi)*cos(phi)*sin(theta)],
[sin(psi)*cos(theta),cos(psi)*cos(phi)+sin(phi)*sin(theta)*sin(psi),-cos(psi)*sin(phi)+sin(theta)*sin(psi)*cos(phi)],
[-sin(theta),cos(theta)*sin(phi),cos(theta)*cos(phi)]])
#Rotation matrix two
J2 = np.array([[1,sin(phi)*tan(theta),cos(phi)*tan(theta)],
[0,cos(phi),-sin(phi)],
[0,sin(phi)/cos(theta),cos(phi)/cos(theta)]])
#Seperating ACC into E[0]=[udot,vdot,wdot] and E[1]=[rdot,pdot,qdot]
E = np.split(ACC,2) #assigned for holding
#Assign to v1_dot and v2_dot
v1_dot = E[0]
v2_dot = E[1]
#Multiply J1 with v1_dot and J2 with v2_dot
N1_dot = np.matmul(J1,v1_dot)
N2_dot = np.matmul(J2,v2_dot)
#Stack ACC, N1_dot and N2_dot
Y = np.concatenate((ACC,N1_dot,N2_dot))
#transpose so that it is in the form [[],[]....,[]]
Y = np.transpose(Y)
#turn into the form [udot,vdot,wdot,pdot,qdot,rdot,xdot,ydot,zdot,phidot,thetadot,psidot]
N = np.ravel(Y)
return N
#Initialise the state vector
state0 = [0,0,0,0,0,0,0,0,0,0,0,0]
t = np.linspace(0,200,3000)
sol = odeint(Sub,state0,t)
X = sol[:,6]
Y = sol[:,7]
Z = sol[:,8]
theta = sol[:,10]
plt.plot(t,Z,'g')
#plt.plot(t,theta,'b')
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(X,Y,Z)
ax.legend()
ax.set_xlabel('$X$', fontsize=10)
ax.set_ylabel('$Y$', fontsize=10)
ax.zaxis.set_rotate_label(False)
ax.set_zlabel('$-Z$', fontsize=10, rotation = 0)
plt.show()