Skip to content

Instantly share code, notes, and snippets.

@oikonang

oikonang/Main.ipynb

Last active February 6, 2019 16:04
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save oikonang/6695a21df14d757a0ee02cdc850c6d51 to your computer and use it in GitHub Desktop.
Save oikonang/6695a21df14d757a0ee02cdc850c6d51 to your computer and use it in GitHub Desktop.
Download ZIP
1st Simulation - Interactive
Display the source blob
Display the rendered blob
Raw
Main.ipynb
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Raw
requirements.txt
numpy==1.15.4
scipy==1.1.0
pandas==0.23.4
bokeh==1.0.1
matplotlib==3.0.2
Raw
simulator_functions.py
import numpy as np
import math
from numpy.linalg import inv
def container(x,ui):
'''
container function returns the speed U in m/s (optionally) and the
time derivative of the state vector: x = [ u v r x y psi p phi delta n ] for
a container ship L = 175 m, where
u = surge velocity SOG (m/s)
v = sway velocity SOG (m/s)
r = yaw velocity (rad/s)
x = position in x-direction (m)
y = position in y-direction (m)
psi = yaw angle (rad)
p = roll velocity (rad/s)
phi = roll angle (rad)
delta = actual rudder angle (rad)
n = actual shaft velocity (rpm)
ucx = current surge velocity (m/s) invoke constant current x
ucy = current sway velocity (m/s) invoke constant current y
The input vector is :
ui = [ delta_c n_c ]' where
delta_c = commanded rudder angle (rad)
n_c = commanded shaft velocity (rpm)
Reference: Son og Nomoto (1982). On the Coupled Motion of Steering and
Rolling of a High Speed Container Ship, Naval Architect of Ocean Engineering,
20: 73-83. From J.S.N.A. , Japan, Vol. 150, 1981.
Author: Trygve Lauvdal
Date: 12th May 1994
Revisions: 18th July 2001 (Thor I. Fossen): added output U, changed order of x-vector
20th July 2001 (Thor I. Fossen): changed my = 0.000238 to my = 0.007049
10th January 2019 (Angelos Ikonomakis): translated the whole script in python
'''
assert (len(x) == 12),'x-vector must have dimension 10 !'
assert (len(ui) == 2),'u-vector must have dimension 2 !'
# Normalization variables
L = 175 # length of ship (m)
#U = np.sqrt(x[0]**2 + x[1]**2) # service speed STW (m/s)
U = np.sqrt((x[0]-x[10])**2 + (x[1]-x[11])**2) # service speed STW (m/s)
Uog = np.sqrt(x[0]**2 + x[1]**2) # SOG
# Check service speed
assert (U > 0),'The ship must have speed greater than zero'
assert (x[9] > 0),'The propeller rpm must be greater than zero'
delta_max = 10; # max rudder angle (deg)
Ddelta_max = 5; # max rudder rate (deg/s)
n_max = 160; # max shaft velocity (rpm)
# Non-dimensional states and inputs
delta_c = ui[0];
n_c = ui[1]/60*L/U;
u = (x[0]-x[10])/U; v = (x[1]-x[11])/U;
p = x[6]*L/U; r = x[2]*L/U;
phi = x[7]; psi = x[5];
delta = x[8]; n = x[9]/60*L/U;
# Parameters, hydrodynamic derivatives and main dimensions
m = 0.00792; mx = 0.000238; my = 0.007049;
Ix = 0.0000176; alphay = 0.05; lx = 0.0313;
ly = 0.0313; Ix = 0.0000176; Iz = 0.000456;
Jx = 0.0000034; Jz = 0.000419; xG = 0;
B = 25.40; dF = 8.00; g = 9.81;
dA = 9.00; d = 8.50; nabla = 21222;
KM = 10.39; KB = 4.6154; AR = 33.0376;
Delta = 1.8219; D = 6.533; GM = 0.3/L;
rho = 1025; t = 0.175; T = 0.0005;
W = rho*g*nabla/(rho*L**2*U**2/2);
Xuu = -0.0004226; Xvr = -0.00311; Xrr = 0.00020;
Xphiphi = -0.00020; Xvv = -0.00386;
Kv = 0.0003026; Kr = -0.000063; Kp = -0.0000075;
Kphi = -0.000021; Kvvv = 0.002843; Krrr = -0.0000462;
Kvvr = -0.000588; Kvrr = 0.0010565; Kvvphi = -0.0012012;
Kvphiphi = -0.0000793; Krrphi = -0.000243; Krphiphi = 0.00003569;
Yv = -0.0116; Yr = 0.00242; Yp = 0;
Yphi = -0.000063; Yvvv = -0.109; Yrrr = 0.00177;
Yvvr = 0.0214; Yvrr = -0.0405; Yvvphi = 0.04605;
Yvphiphi = 0.00304; Yrrphi = 0.009325; Yrphiphi = -0.001368;
Nv = -0.0038545; Nr = -0.00222; Np = 0.000213;
Nphi = -0.0001424; Nvvv = 0.001492; Nrrr = -0.00229;
Nvvr = -0.0424; Nvrr = 0.00156; Nvvphi = -0.019058;
Nvphiphi = -0.0053766; Nrrphi = -0.0038592; Nrphiphi = 0.0024195;
kk = 0.631; epsilon = 0.921; xR = -0.5;
wp = 0.184; tau = 1.09; xp = -0.526;
cpv = 0.0; cpr = 0.0; ga = 0.088;
cRr = -0.156; cRrrr = -0.275; cRrrv = 1.96;
cRX = 0.71; aH = 0.237; zR = 0.033;
xH = -0.48;
# Masses and moments of inertia
m11 = (m+mx);
m22 = (m+my);
m32 = -my*ly;
m42 = my*alphay;
m33 = (Ix+Jx);
m44 = (Iz+Jz);
# Rudder saturation and dynamics
if np.abs(delta_c) >= delta_max*math.pi/180:
delta_c = np.sign(delta_c)*delta_max*math.pi/180;
delta_dot = delta_c - delta
if np.abs(delta_dot) >= Ddelta_max*math.pi/180:
delta_dot = np.sign(delta_dot)*Ddelta_max*math.pi/180;
# Shaft velocity saturation and dynamics
n_c = n_c*U/L;
n = n*U/L;
if np.abs(n_c) >= n_max/60:
n_c = np.sign(n_c)*n_max/60;
if n > 0.3:
Tm=5.65/n;
else:
Tm=18.83;
n_dot = 1/Tm*(n_c-n)*60;
# Calculation of state derivatives
vR = ga*v + cRr*r + cRrrr*r**3 + cRrrv*r**2*v;
uP = math.cos(v)*((1 - wp) + tau*((v + xp*r)**2 + cpv*v + cpr*r));
J = uP*U/(n*D);
KT = 0.527 - 0.455*J;
uR = uP*epsilon*np.sqrt(1 + 8*kk*KT/(math.pi*J**2));
alphaR = delta + math.atan(vR/uR);
FN = - ((6.13*Delta)/(Delta + 2.25))*(AR/L**2)*(uR**2 + vR**2)*math.sin(alphaR);
T = 2*rho*D**4/(U**2*L**2*rho)*KT*n*np.abs(n); # Thrust
T_real = rho*D**4*KT*n*np.abs(n); # Denormalized Thrust (surge)
R_real = 0.5*rho*U**2*L**2*Xuu*u**2 # Denormalized Resistance (surge)
# Forces and moments
X = Xuu*u**2 + (1-t)*T + Xvr*v*r + Xvv*v**2 + Xrr*r**2 + Xphiphi*phi**2 + \
cRX*FN*math.sin(delta) + (m + my)*v*r;
Y = Yv*v + Yr*r + Yp*p + Yphi*phi + Yvvv*v**3 + Yrrr*r**3 + Yvvr*v**2*r + \
Yvrr*v*r**2 + Yvvphi*v**2*phi + Yvphiphi*v*phi**2 + Yrrphi*r**2*phi + \
Yrphiphi*r*phi**2 + (1 + aH)*FN*math.cos(delta) - (m + mx)*u*r;
K = Kv*v + Kr*r + Kp*p + Kphi*phi + Kvvv*v**3 + Krrr*r**3 + Kvvr*v**2*r + \
Kvrr*v*r**2 + Kvvphi*v**2*phi + Kvphiphi*v*phi**2 + Krrphi*r**2*phi + \
Krphiphi*r*phi**2 - (1 + aH)*zR*FN*math.cos(delta) + mx*lx*u*r - W*GM*phi;
N = Nv*v + Nr*r + Np*p + Nphi*phi + Nvvv*v**3 + Nrrr*r**3 + Nvvr*v**2*r + \
Nvrr*v*r**2 + Nvvphi*v**2*phi + Nvphiphi*v*phi**2 + Nrrphi*r**2*phi + \
Nrphiphi*r*phi**2 + (xR + aH*xH)*FN*math.cos(delta);
# Dimensional state derivatives xdot = [ u v r x y psi p phi delta n ]'
detM = m22*m33*m44-m32**2*m44-m42**2*m33;
xdot =np.array([ X*(U**2/L)/m11,
-((-m33*m44*Y+m32*m44*K+m42*m33*N)/detM)*(U**2/L),
((-m42*m33*Y+m32*m42*K+N*m22*m33-N*m32**2)/detM)*(U**2/L**2),
(math.cos(psi)*u-math.sin(psi)*math.cos(phi)*v)*U,
(math.sin(psi)*u+math.cos(psi)*math.cos(phi)*v)*U,
math.cos(phi)*r*(U/L),
((-m32*m44*Y+K*m22*m44-K*m42**2+m32*m42*N)/detM)*(U**2/L**2),
p*(U/L),
delta_dot,
n_dot,
0, # hardcode x-current acceleration
0 # hardcode y-current acceleration
]);
return xdot, U, Uog, T_real, R_real
def Lcontainer(x,ui,U0=7.0):
'''Lcontainer(x,ui,U0) returns the speed U in m/s (optionally) and the
time derivative of the state vector: x = [ u v r x y psi p phi delta ]' using the
the LINEARIZED model corresponding to the nonlinear model container.m.
u = surge velocity (m/s)
v = sway velocity (m/s)
r = yaw velocity (rad/s)
x = position in x-direction (m)
y = position in y-direction (m)
psi = yaw angle (rad)
p = roll velocity (rad/s)
phi = roll angle (rad)
delta = actual rudder angle (rad)
The inputs are :
Uo = service speed (optinally. Default speed U0 = 7 m/s
ui = commanded rudder angle (rad)
Reference: Son og Nomoto (1982). On the Coupled Motion of Steering and
Rolling of a High Speed Container Ship, Naval Architect of Ocean Engineering,
20: 73-83. From J.S.N.A. , Japan, Vol. 150, 1981.
Author: Thor I. Fossen
Date: 23rd July 2001
Revisions:
'''
# Check of input and state dimensions
assert (x.shape[0] == 9),'x-vector must have dimension 9 !'
assert (U0 > 0),'The ship must have speed greater than zero'
# Normalization variables
rho = 1025 # water density (kg/m^3)
L = 175 # length of ship (m)
U = np.sqrt(U0**2 + x[1,0]**2) # ship speed (m/s)
# rudder limitations
delta_max = 10 # max rudder angle (deg)
Ddelta_max = 5 # max rudder rate (deg/s)
# States and inputs
delta_c = ui
v = x[1,0]; y = x[4,0];
r = x[2,0]; psi = x[5,0];
p = x[6,0]; phi = x[7,0];
nu = np.array([[v, r, p]]).T
eta = np.array([[y, psi, phi]]).T
delta = x[8,0]
# Linear model using nondimensional matrices and states with dimension (see Fossen 2002):
# TM'inv(T) dv/dt + (U/L) TN'inv(T) v + (U/L)^2 TG'inv(T) eta = (U^2/L) T b' delta
T = np.diag([ 1, 1/L, 1/L]);
Tinv = np.diag([ 1, L, L ]);
M = np.array([[ 0.01497, 0.0003525, -0.0002205],
[ 0.0003525, 0.000875, 0],
[-0.0002205, 0, 0.0000210 ]])
N = np.array([[ 0.012035, 0.00522, 0],
[ 0.0038436, 0.00243, -0.000213],
[-0.000314, 0.0000692, 0.0000075]])
G = np.array([[ 0, 0, 0.0000704],
[ 0, 0, 0.0001468],
[ 0, 0, 0.0004966]])
b = np.array([[-0.002578, 0.00126, 0.0000855]]).T
# Rudder saturation and dynamics
if np.abs(delta_c) >= delta_max*math.pi/180:
delta_c = np.sign(delta_c)*delta_max*math.pi/180
delta_dot = delta_c - delta
if np.abs(delta_dot) >= Ddelta_max*math.pi/180:
delta_dot = np.sign(delta_dot)*Ddelta_max*math.pi/180
# TM'inv(T) dv/dt + (U/L) TN'inv(T) v + (U/L)^2 TG'inv(T) eta = (U^2/L) T b' delta
nudot = inv(T @ M @ Tinv) @ ((U**2/L)*T@b*delta - (U/L)*T@N@Tinv@nu - (U/L)**2*T@G@Tinv@eta)
# Dimensional state derivatives xdot = [ u v r x y psi p phi delta ]'
xdot =np.array([[ 0,
nudot[0,0],
nudot[1,0],
math.cos(psi)*U0-math.sin(psi)*math.cos(phi)*v,
math.sin(psi)*U0+math.cos(psi)*math.cos(phi)*v,
math.cos(phi)*r,
nudot[2,0],
p,
delta_dot ]]).T
return xdot, U
def euler2(xdot,x,h):
'''
EULER2 Integrate a system of ordinary differential equations using
Euler's 2nd-order method.
xnext = euler2(xdot,x,h)
xdot - dx/dt(k) = f(x(k)
x - x(k)
xnext - x(k+1)
h - step size
Author: Thor I. Fossen
Date: 14th June 2001
Revisions:
'''
xnext = x + h*xdot;
return xnext
def zigzag(ship,x,ui,t_final,t_rudderexecute,h,maneuver=[20,20]):
'''
ZIGZAG [t,u,v,r,x,y,psi,U] = zigzag(ship,x,ui,t_final,t_rudderexecute,h,maneuver)
performs the zig-zag maneuver, see ExZigZag.m
Inputs :
'ship' = ship model. Compatible with the models under .../gnc/VesselModels/
x = initial state vector for ship model
ui = [delta,:] where delta=0 and the other values are non-zero if any
t_final = final simulation time
t_rudderexecute = time control input is activated
h = sampling time
maneuver = [rudder angle, heading angle]. Default 20-20 deg that is: maneuver = [20, 20]
rudder is changed to maneuver(1) when heading angle is larger than maneuver(2)
Outputs :
t = time vector
u,v,r,x,y,psi,U = time series
Author: Thor I. Fossen
Date: 22th July 2001
Revisions: 15th July 2002, switching logic has been modified to handle arbitrarily maneuvers
'''
assert (t_final>t_rudderexecute),'t_final must be larger than t_rudderexecute';
N = round(t_final/h) # number of samples
xout = np.zeros((N+1,9)) # memory allocation
print('Simulating...')
u_ship=ui
for i in range(0,N+1):
time = i*h;
psi = x[5]*180/math.pi
r = x[2]
if round(time)==t_rudderexecute:
u_ship[0]=maneuver[0]*math.pi/180
if round(time) > t_rudderexecute:
if (psi>=maneuver[1] and r>0):
u_ship[0] = -maneuver[0]*math.pi/180
elif (psi<=-maneuver[1] and r<0):
u_ship[0] = maneuver[0]*math.pi/180
xdot,U, X, Y, K, N = container(x,ui) # ship model
xout[i,:] = np.hstack((time,x[0:6],U,u_ship[0]))
x = euler2(xdot,x,h) # Euler integration
# time-series
t = xout[:,0];
u = xout[:,1];
v = xout[:,2];
r = xout[:,3]*180/math.pi;
x = xout[:,4];
y = xout[:,5];
psi = xout[:,6]*180/math.pi;
U = xout[:,7];
delta_c = xout[:,8]*180/math.pi;
return t,u,v,r,x,y,psi,U,delta_c
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment