├── README.md ├── functions ├── __init__.py ├── control.py ├── gnc.py ├── guidance.py ├── mainLoop.py ├── models.py └── plotTimeSeries.py ├── imgs ├── lqr_control.png ├── lqr_state.png ├── pid_control.png └── pid_state.png ├── main.py └── vehicles ├── DSRV.py ├── ROVzefakkel.py ├── __init__.py ├── frigate.py ├── otter.py ├── semisub.py ├── shipClarke83.py ├── supply.py ├── swift.py └── tanker.py /README.md: -------------------------------------------------------------------------------- 1 | # Python Vehicle Simulator 2 | 3 | The Python Vehicle Simulator is a supplement to the Matlab MSS (Marine Systems Simulator) toolbox. It includes models for autonomous underwater vehicles (AUVs), unmanned surface vehicles (USVs), and ships. The vehicle models are based on the MSS vessel models located in /MSS/VESSELS/ catalogue. Each vehicle is modeled as an object in Python and the vehicle class has methods for guidance, navigation and control. The main program: 4 | 5 | main.py 6 | 7 | is used to define multiple vehicle objects for real-time simulation. The python modules are located under the catalogues: 8 | 9 | /vehicles/ Vehicle classes/methods: 10 | swift.py - Autonomous underwater vehicle, controlled by 8 thrusters 11 | DSRV.py - Deep submergence rescue vehicle (DSRV) controlled by a stern plane, L = 5.0 m 12 | frigate.py - Frigate, rudder-controlled ship described by a nonlinear Nomoto model, L = 100.0 m 13 | otter.py - Otter unmanned surface vehicle (USV) controlled by two propellers, L = 2.0 m 14 | ROVzefakkel.py - ROV Zefakkel, rudder-controlled ship described by a nonlinear Nomoto model, L = 54.0 m 15 | semisub.py - Semisubmersible controlled by tunnel thrusters and main propellers, L = 84.5 m 16 | shipClarke83.py - Ship, linear maneuvering model specified by L, B and T using the Clarke (1983) formulas 17 | supply.py - Offshore supply vessel controlled by tunnel thrusters and main propellers, L = 76.2 m 18 | tanker.py - Tanker, rudder-controlled ship model including shallow water effects, L = 304.8 m 19 | /functions/ Functions used by the main program: 20 | control.py - feedback control systems 21 | gnc.py - generic GNC functions 22 | guidance.py - guidance functions 23 | mainLoop.py - main simulation loop 24 | plotTimeSeries.py - plotting functions 25 | 26 | In order to run the main program main.py the following modules must be installed: 27 | 28 | numpy https://numpy.org/install/ 29 | matplotlib https://matplotlib.org/stable/users/installing.html 30 | scipy https://scipy.org/install/ 31 | 32 | For more information about mathematical modeling of marine craft and methods for guidance, navigation and control, please consult: 33 | 34 | T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion Control. 2nd. Edition, Wiley. 35 | URL: www.fossen.biz/wiley 36 | 37 | -------- 38 | Results for swift AUV control 39 | -------- 40 | 41 | The reference waypoint is (5m, 5m, 5m) with heading angle (45 degree) 42 | 43 | ### Nonlinear MIMO PID using pole placement 44 | 45 | ![pid1](./imgs/pid_state.png) 46 | ![pid2](./imgs/pid_control.png) 47 | 48 | ### Linear Quadratic regulator 49 | ![lqr1](./imgs/lqr_state.png) 50 | ![lqr2](./imgs/lqr_control.png) 51 | -------------------------------------------------------------------------------- /functions/__init__.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | from .gnc import * 4 | from .mainLoop import * 5 | from .plotTimeSeries import * 6 | from .guidance import * 7 | from .control import * 8 | from .models import * -------------------------------------------------------------------------------- /functions/control.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Control methods. 4 | 5 | Reference: T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and 6 | Motion Control. 2nd. Edition, Wiley. 7 | URL: www.fossen.biz/wiley 8 | 9 | Author: Thor I. Fossen 10 | Revisions: 7 dec 2021 added DPpolePlacement6 method (Mohamed Magdy) 11 | 8 dec 2021 added LQR method (Mohamed Magdy) 12 | """ 13 | 14 | import numpy as np 15 | import scipy.linalg 16 | import math 17 | from functions.guidance import refModel3, LPfilter 18 | from functions.gnc import ssa, Rzyx, Tzyx, eulerang, Dmtx, m2c, gvect 19 | 20 | # SISO PID pole placement 21 | def PIDpolePlacement(e_int,e_x,e_v, \ 22 | x_d,v_d,a_d,m,d,k,wn_d,zeta_d,wn,zeta,r,v_max,sampleTime): 23 | 24 | # PID gains based on pole placement 25 | Kp = m * wn**2 - k 26 | Kd = m * 2 * zeta * wn - d 27 | Ki = (wn/10) * Kp 28 | 29 | # PID control law 30 | u = -Kp * e_x - Kd * e_v - Ki * e_int 31 | 32 | # Integral error, Euler's method 33 | e_int += sampleTime * e_x 34 | 35 | # 3rd-order reference model for smooth position, velocity and acceleration 36 | [x_d, v_d, a_d] = refModel3(x_d, v_d, a_d, r, wn_d, zeta_d, v_max, sampleTime) 37 | 38 | return u, e_int, x_d, v_d, a_d 39 | 40 | 41 | # MIMO nonlinear PID pole placement [3DOF] 42 | def DPpolePlacement(e_int,M3,D3,eta3,nu3, \ 43 | x_d,y_d,psi_d,wn,zeta,eta_ref,sampleTime): 44 | 45 | # PID gains based on pole placement 46 | Kp = wn @ wn @ M3 47 | Kd = 2.0 * zeta @ wn @ M3 - D3 48 | Ki = (1.0 / 10.0) * wn @ Kp 49 | 50 | # DP control law - setpoint regulation 51 | e = eta3 - eta_ref 52 | e[2] = ssa( e[2] ) 53 | R = Rzyx( 0.0, 0.0, eta3[2] ) 54 | tau = -np.matmul( (R.T @ Kp), e) \ 55 | -np.matmul( (R.T @ Kd @ R), nu3) \ 56 | -np.matmul( (R.T @ Ki), e_int) 57 | 58 | # Low-pass filters, Euler's method 59 | T = 5.0 * np.array([ 1/wn[0][0], 1/wn[1][1], 1/wn[2][2] ]) 60 | x_d += sampleTime * ( eta_ref[0] - x_d ) / T[0] 61 | y_d += sampleTime * ( eta_ref[1] - y_d ) / T[1] 62 | psi_d += sampleTime * ( eta_ref[2] - psi_d ) / T[2] 63 | 64 | return tau, e_int, x_d, y_d, psi_d 65 | 66 | # MIMO nonlinear PID pole placement [6DOF] 67 | def DPpolePlacement6(e_int,M,D,eta,nu, \ 68 | eta_d,wn,zeta,eta_ref,sampleTime): 69 | 70 | # PID gains based on pole placement 71 | Kp = wn @ wn @ M 72 | Kd = 2.0 * zeta @ wn @ M - D 73 | Ki = (1.0 / 10.0) * wn @ Kp 74 | 75 | # DP control law - setpoint regulation 76 | # Low-pass filters, Euler's method 77 | T = 5.0 * np.array([ 1/wn[0][0], 1/wn[1][1], 1/wn[2][2], 1/wn[3][3], 1/wn[4][4], 1/wn[5][5] ]) 78 | eta_d = LPfilter(eta_ref, eta_d, T, sampleTime) 79 | 80 | e = eta - eta_d 81 | e[3] = ssa( e[3] ) 82 | e[4] = ssa( e[4] ) 83 | e[5] = ssa( e[5] ) 84 | 85 | R = Rzyx( eta[3], eta[4], eta[5] ) 86 | T = Tzyx(eta[3], eta[4]) 87 | # Jacobi matrix for body to world transformation 88 | J = np.zeros(shape=(6,6), dtype=float) 89 | J[0:3, 0:3] = R 90 | J[3:6, 3:6] = T 91 | # Inverse of the Jacobi matrix for world to body transformation 92 | Jinv = np.zeros(shape=(6, 6), dtype=float) 93 | Jinv[0:3, 0:3] = R.T 94 | Jinv[3:6, 3:6] = np.linalg.inv(T) 95 | tau = -np.matmul( (Jinv @ Kp), e) \ 96 | -np.matmul( (Jinv @ Kd @ J), nu) \ 97 | -np.matmul( (Jinv @ Ki), e_int) 98 | 99 | 100 | 101 | # Integral error, Euler's method 102 | e_int += sampleTime * e 103 | 104 | return tau, e_int, eta_d 105 | 106 | def LQR(M,Dlin,Dquad,r_bb,r_bg,weight,eta,nu, \ 107 | eta_d,eta_ref,nu_d,sampleTime,wn,Q,R): 108 | """Solve the discrete time lqr controller. 109 | 110 | x[k+1] = A x[k] + B u[k] 111 | 112 | cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] 113 | """ 114 | # ref Bertsekas, p.151 115 | # Constructing LTV model 116 | # state x [eta, nu] 12x1 vector 117 | x = np.hstack((eta,nu)).reshape(-1,1) 118 | # Low-pass filters, Euler's method 119 | T = 5.0 * np.array([ 1/wn[0][0], 1/wn[1][1], 1/wn[2][2], 1/wn[3][3], 1/wn[4][4], 1/wn[5][5] ]) 120 | eta_d = LPfilter(eta_ref, eta_d, T, sampleTime) 121 | # Compute desired state xd 122 | x_d = np.hstack((eta_d, np.zeros(6))).reshape(-1,1) 123 | 124 | 125 | # System matrix A 12x12 126 | A = np.zeros(shape=(12,12), dtype=float) 127 | C = m2c(M,nu) 128 | D = Dmtx(Dlin,Dquad,nu) 129 | J = eulerang(eta) 130 | A[0:6 , 6:12] = J 131 | A[6:12, 6:12] = -1 * np.linalg.inv(M) * (D + C) 132 | # Input matrix B 12x6 133 | B = np.zeros(shape=(12,6), dtype=float) 134 | B[6:12, 0:6] = np.linalg.inv(M) 135 | 136 | # first, try to solve the ricatti equation 137 | P = np.matrix(scipy.linalg.solve_continuous_are(A, B, Q, R)) 138 | K = -1 * scipy.linalg.inv(R) @ B.T @ P 139 | # compute the LQR gain 140 | # eigVals, eigVecs = scipy.linalg.eig(A-B*K) 141 | # error state 142 | e = x - x_d 143 | # saturating 144 | e[3] = ssa( e[3] ) 145 | e[4] = ssa( e[4] ) 146 | e[5] = ssa( e[5] ) 147 | tau = K * e 148 | 149 | return tau,eta_d 150 | -------------------------------------------------------------------------------- /functions/gnc.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | GNC functions. 4 | 5 | Reference: T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and 6 | Motion Control. 2nd. Edition, Wiley. 7 | URL: www.fossen.biz/wiley 8 | 9 | Author: Thor I. Fossen 10 | Revisions: 5 dec 2021 added gvect method (Mohamed Magdy) 11 | 6 dec 2021 added Dmtx method (Mohamed Magdy) 12 | """ 13 | 14 | import numpy as np 15 | import math 16 | 17 | # angle = ssa(angle) returns the smallest-signed angle in [ -pi, pi ) 18 | def ssa(angle): 19 | angle = (angle + math.pi) % (2 * math.pi) - math.pi 20 | 21 | return angle 22 | 23 | # x = sat(x,x_min,x_max) saturates a signal x such that x_min <= x <= x_max 24 | def sat(x, x_min, x_max): 25 | if x > x_max: 26 | x = x_max 27 | elif x < x_min: 28 | x = x_min 29 | 30 | return x 31 | 32 | # S = Smtrx(a) computes the 3x3 vector skew-symmetric matrix S(a) = -S(a)'. 33 | # The cross product satisfies: a x b = S(a)b. 34 | def Smtrx(a): 35 | 36 | S = np.array([ 37 | [ 0, -a[2], a[1] ], 38 | [ a[2], 0, -a[0] ], 39 | [-a[1], a[0], 0 ] ]) 40 | 41 | return S 42 | 43 | # H = Hmtrx(r) computes the 6x6 system transformation matrix 44 | # H = [eye(3) S' 45 | # zeros(3,3) eye(3) ] Property: inv(H(r)) = H(-r) 46 | # 47 | # If r = r_bg is the vector from the CO to the CG, the model matrices in CO and 48 | # CG are related by: M_CO = H(r_bg)' * M_CG * H(r_bg). Generalized position and 49 | # force satisfy: eta_CO = H(r_bg)' * eta_CG and tau_CO = H(r_bg)' * tau_CG 50 | def Hmtrx(r): 51 | 52 | H = np.identity(6,float) 53 | H[0:3, 3:6] = Smtrx(r).T 54 | 55 | return H 56 | 57 | # R = Rzyx(phi,theta,psi) computes the Euler angle rotation matrix R in SO(3) 58 | # using the zyx convention 59 | def Rzyx(phi,theta,psi): 60 | 61 | cphi = math.cos(phi) 62 | sphi = math.sin(phi) 63 | cth = math.cos(theta) 64 | sth = math.sin(theta) 65 | cpsi = math.cos(psi) 66 | spsi = math.sin(psi) 67 | 68 | R = np.array([ 69 | [ cpsi*cth, -spsi*cphi+cpsi*sth*sphi, spsi*sphi+cpsi*cphi*sth ], 70 | [ spsi*cth, cpsi*cphi+sphi*sth*spsi, -cpsi*sphi+sth*spsi*cphi ], 71 | [ -sth, cth*sphi, cth*cphi ] ]) 72 | 73 | return R 74 | 75 | # T = Tzyx(phi,theta) computes the Euler angle attitude 76 | # transformation matrix T using the zyx convention 77 | def Tzyx(phi,theta): 78 | 79 | cphi = math.cos(phi) 80 | sphi = math.sin(phi) 81 | cth = math.cos(theta) 82 | sth = math.sin(theta) 83 | 84 | try: 85 | T = np.array([ 86 | [ 1, sphi*sth/cth, cphi*sth/cth ], 87 | [ 0, cphi, -sphi], 88 | [ 0, sphi/cth, cphi/cth] ]) 89 | 90 | except ZeroDivisionError: 91 | print ("Tzyx is singular for theta = +-90 degrees." ) 92 | 93 | return T 94 | 95 | # eta = attitudeEuler(eta,nu,sampleTime) computes the generalized 96 | # position/Euler angles eta[k+1] 97 | def attitudeEuler(eta,nu,sampleTime): 98 | 99 | p_dot = np.matmul( Rzyx(eta[3], eta[4], eta[5]), nu[0:3] ) 100 | v_dot = np.matmul( Tzyx(eta[3], eta[4]), nu[3:6] ) 101 | 102 | # Forward Euler integration 103 | eta[0:3] = eta[0:3] + sampleTime * p_dot 104 | eta[3:6] = eta[3:6] + sampleTime * v_dot 105 | 106 | return eta 107 | 108 | # C = m2c(M,nu) computes the Coriolis and centripetal matrix C from the 109 | # mass matrix M and generalized velocity vector nu (Fossen 2021, Ch. 3) 110 | def m2c(M, nu): 111 | 112 | M = 0.5 * (M + M.T) # systematization of the inertia matrix 113 | 114 | if (len(nu) == 6): # 6-DOF model 115 | 116 | M11 = M[0:3,0:3] 117 | M12 = M[0:3,3:6] 118 | M21 = M12.T 119 | M22 = M[3:6,3:6] 120 | 121 | nu1 = nu[0:3] 122 | nu2 = nu[3:6] 123 | dt_dnu1 = np.matmul(M11,nu1) + np.matmul(M12,nu2) 124 | dt_dnu2 = np.matmul(M21,nu1) + np.matmul(M22,nu2) 125 | 126 | #C = [ zeros(3,3) -Smtrx(dt_dnu1) 127 | # -Smtrx(dt_dnu1) -Smtrx(dt_dnu2) ] 128 | C = np.zeros( (6,6) ) 129 | C[0:3,3:6] = -Smtrx(dt_dnu1) 130 | C[3:6,0:3] = -Smtrx(dt_dnu1) 131 | C[3:6,3:6] = -Smtrx(dt_dnu2) 132 | 133 | else: # 3-DOF model (surge, sway and yaw) 134 | #C = [ 0 0 -M(2,2)*nu(2)-M(2,3)*nu(3) 135 | # 0 0 M(1,1)*nu(1) 136 | # M(2,2)*nu(2)+M(2,3)*nu(3) -M(1,1)*nu(1) 0 ] 137 | C = np.zeros( (3,3) ) 138 | C[0,2] = -M[1,1] * nu[1] - M[1,2] * nu[2] 139 | C[1,2] = M[0,0] * nu[0] 140 | C[2,0] = -C[0,2] 141 | C[2,1] = -C[1,2] 142 | 143 | return C 144 | 145 | # CY_2D = Hoerner(B,T) 146 | # Hoerner computes the 2D Hoerner cross-flow form coeff. as a function of beam 147 | # B and draft T.The data is digitized and interpolation is used to compute others 148 | # data point than those in the table 149 | def Hoerner(B,T): 150 | 151 | # DATA = [B/2T C_D] 152 | DATA1 = np.array([ 153 | 0.0109,0.1766,0.3530,0.4519,0.4728,0.4929,0.4933,0.5585,0.6464,0.8336, 154 | 0.9880,1.3081,1.6392,1.8600,2.3129,2.6000,3.0088,3.4508, 3.7379,4.0031 155 | ]) 156 | DATA2 = np.array([ 157 | 1.9661,1.9657,1.8976,1.7872,1.5837,1.2786,1.2108,1.0836,0.9986,0.8796, 158 | 0.8284,0.7599,0.6914,0.6571,0.6307,0.5962,0.5868,0.5859,0.5599,0.5593 159 | ]) 160 | 161 | CY_2D = np.interp( B / (2 * T), DATA1, DATA2 ) 162 | 163 | return CY_2D 164 | 165 | # tau_crossflow = crossFlowDrag(L,B,T,nu_r) computes the cross-flow drag 166 | # integrals for a marine craft using strip theory. 167 | # 168 | # M d/dt nu_r + C(nu_r)*nu_r + D*nu_r + g(eta) = tau + tau_crossflow 169 | def crossFlowDrag(L,B,T,nu_r): 170 | 171 | rho = 1026 # density of water 172 | n = 20 # number of strips 173 | 174 | dx = L/20 175 | Cd_2D = Hoerner(B,T) # 2D drag coefficient based on Hoerner's curve 176 | 177 | Yh = 0 178 | Nh = 0 179 | xL = -L/2 180 | 181 | for i in range(0,n+1): 182 | v_r = nu_r[1] # relative sway velocity 183 | r = nu_r[5] # yaw rate 184 | Ucf = abs(v_r + xL * r) * (v_r + xL * r) 185 | Yh = Yh - 0.5 * rho * T * Cd_2D * Ucf * dx # sway force 186 | Nh = Nh - 0.5 * rho * T * Cd_2D * xL * Ucf * dx # yaw moment 187 | xL += dx 188 | 189 | tau_crossflow = np.array([0, Yh, 0, 0, 0, Nh],float) 190 | 191 | return tau_crossflow 192 | 193 | # g = gvect(W,B,theta,phi,r_bg,r_bb) computes the 6x1 vector of restoring 194 | # forces about an arbitrarily point CO for a submerged body. For floating 195 | # vessels, use Gmtrx.m 196 | # 197 | # Inputs: W, B: weight and buoyancy 198 | # phi,theta: roll and pitch angles 199 | # r_bg = [x_g y_g z_g]: location of the CG with respect to the CO 200 | # r_bb = [x_b y_b z_b]: location of the CB with respect to th CO 201 | def gvect(W,B,theta,phi,r_bg,r_bb): 202 | sth = np.sin(theta) 203 | cth = np.cos(theta) 204 | sphi = np.sin(phi) 205 | cphi = np.cos(phi) 206 | g = np.array([ 207 | (W-B) * sth, 208 | -(W-B) * cth * sphi, 209 | -(W-B) * cth * cphi, 210 | -(r_bg[1]*W-r_bb[1]*B) * cth * cphi + (r_bg[2]*W-r_bb[2]*B) * cth * sphi, 211 | (r_bg[2]*W-r_bb[2]*B) * sth + (r_bg[0]*W-r_bb[0]*B) * cth * cphi, 212 | -(r_bg[0]*W-r_bb[0]*B) *cth * sphi - (r_bg[1]*W-r_bb[1]*B) * sth ]) 213 | return g 214 | 215 | # D = Dmtx(Dlin,Dquad,nu) computes the 6x6 total Damping matrix 216 | # given the linear and quadratic damping coefficients matrices and 217 | # the current body velocity vector 218 | def Dmtx(Dlin,Dquad,nu): 219 | D = -1 * Dlin 220 | for i in range(6): 221 | D[i,i] += -1 * Dquad[i,i] * np.abs(nu[i]) 222 | return D 223 | 224 | def eulerang(eta): 225 | J = np.zeros(shape=(6,6), dtype=float) 226 | J[0:3, 0:3] = Rzyx( eta[3], eta[4], eta[5] ) 227 | J[3:6, 3:6] = Tzyx(eta[3], eta[4]) 228 | return J 229 | -------------------------------------------------------------------------------- /functions/guidance.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Guidance algorithms. 4 | 5 | Reference: T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and 6 | Motion Control. 2nd. Edition, Wiley. 7 | URL: www.fossen.biz/wiley 8 | 9 | Author: Thor I. Fossen 10 | Revisions: 7 dec 2021 added LPfilter method (Mohamed Magdy) 11 | 6 dec 2021 added ILOS method (Mohamed Magdy) 12 | """ 13 | 14 | import numpy as np 15 | import math 16 | 17 | # [x_d,v_d,a_d] = refModel3(x_d,v_d,a_d,r,wn_d,zeta_d,v_max,sampleTime) is a 3-order 18 | # reference model for generation of a smooth desired position x_d, velocity |v_d| < v_max, 19 | # and acceleration a_d. Inputs are natural frequency wn_d and relative damping zeta_d. 20 | def refModel3(x_d, v_d, a_d, r, wn_d, zeta_d, v_max, sampleTime): 21 | 22 | # Velocity saturation 23 | if (v_d > v_max): 24 | v_d = v_max 25 | elif (v_d < -v_max): 26 | v_d = -v_max 27 | 28 | # desired "jerk" 29 | j_d = wn_d**3 * (r -x_d) - (2*zeta_d+1) * wn_d**2 * v_d - (2*zeta_d+1) * wn_d * a_d 30 | 31 | # Forward Euler integration 32 | x_d += sampleTime * v_d # desired position 33 | v_d += sampleTime * a_d # desired velocity 34 | a_d += sampleTime * j_d # desired acceleration 35 | 36 | return x_d, v_d, a_d 37 | 38 | # eta_d = LPfiler(eta_reference, eta_d, T, sampleTime) computes smooth desired pose state 39 | # using low-pass filter 40 | # Inputs: 41 | # eta_ref, reference state 42 | # eta_d, desired state 43 | # T, time contant vector for each state 44 | def LPfilter(eta_ref, eta_d, T, sampleTime): 45 | eta_d += sampleTime * (eta_ref - eta_d) / T 46 | return eta_d 47 | 48 | # psi_d = ILOS(x,y,Delta,kappa,h,R_switch,wpt,psi) computes the desired 49 | # heading angle when the path is straight lines going through the waypoints 50 | # (wpt.pos.x, wpt.pos.y). The desired heading angle psi_d is computed using 51 | # the ILOS guidance law by Børhaug et al. (2008) 52 | # Inputs: 53 | # (x,y), craft North-East positions (m) 54 | # Delta, positive look-ahead distance (m) 55 | # kappa, positive integral gain constant, Ki = kappa * Kp 56 | # U, speed of the craft (m/s) 57 | # h, sampling time (s) 58 | # R_switch, go to next waypoint when the along-track distance x_e 59 | # is less than R_switch (m) 60 | # wpt.pos.x = [x1, x2,..,xn]' array of waypoints expressed in NED (m) 61 | # wpt.pos.y = [y1, y2,..,yn]' array of waypoints expressed in NED (m) 62 | # psi, current heading angle 63 | # y_int 64 | # Output: 65 | # psi_d: desired heading angle (rad) 66 | def ILOS(x,y,Delta,kappa,h,R_switch,wpt,psi,y_int): 67 | # integral state 68 | y_int = y_int 69 | # number of waypoints to follow 70 | n = wpt.shape[1] 71 | xk = wpt[0][0] 72 | yk = wpt[1][0] 73 | if n > 1: 74 | xk_next = wpt[0][1] 75 | yk_next = wpt[1][1] 76 | # Compute the desired course angle 77 | pi_p = np.atan2(yk_next-yk,xk_next-xk); # path-tangential angle w.r.t. to North 78 | # along-track and cross-track errors (x_e, y_e) expressed in NED 79 | x_e = (x-xk) * np.cos(pi_p) + (y-yk) * np.sin(pi_p) 80 | y_e = -(x-xk) * np.sin(pi_p) + (y-yk) * np.cos(pi_p) 81 | Kp = 1/Delta 82 | Ki = kappa * Kp 83 | psi_d = pi_p - np.atan(Kp * y_e + Ki * y_int) 84 | # kinematic differential equation 85 | Dy_int = Delta * y_e / ( Delta**2 + (y_e + kappa * y_int)**2) 86 | # Euler integration 87 | y_int = y_int + h * Dy_int; 88 | return psi_d , y_int 89 | -------------------------------------------------------------------------------- /functions/mainLoop.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Main simulation loop. 4 | 5 | Author: Thor I. Fossen 6 | """ 7 | 8 | from .gnc import attitudeEuler 9 | import numpy as np 10 | 11 | # Function simInfo(vehicle) 12 | def simInfo(vehicle): 13 | print('\nThe Python Vehicle Simulator:') 14 | print('Vehicle: %s, L = %s' % (vehicle.name, vehicle.L)) 15 | print('Control system: %s' % (vehicle.controlDescription)) 16 | 17 | 18 | # Function simulate(N, sampleTime, vehicle) 19 | def simulate(N, sampleTime, vehicle): 20 | 21 | DOF = 6 # degrees of freedom 22 | t = 0 # initial simulation time 23 | 24 | # Initial state vectors 25 | eta = np.array( [0, 0, 0, 0, 0,0], float) # position/attitude, user editable 26 | nu = vehicle.nu # velocity, defined by vehicle class 27 | u_actual = vehicle.u_actual # actual inputs, defined by vehicle class 28 | 29 | simInfo(vehicle) # print simulation info 30 | 31 | # Initialization of table used to store the simulation data 32 | simData = np.empty( [0, 2*DOF + 2 * vehicle.dimU], float) 33 | 34 | # Simulator for-loop 35 | for i in range(0,N+1): 36 | 37 | t = i * sampleTime # simulation time 38 | 39 | # Vehicle specific control systems 40 | if (vehicle.controlMode == 'depthAutopilot'): 41 | u_control = vehicle.depthAutopilot(eta,nu,sampleTime) 42 | elif (vehicle.controlMode == 'headingAutopilot'): 43 | u_control = vehicle.headingAutopilot(eta,nu,sampleTime) 44 | elif (vehicle.controlMode == 'DPcontrol'): 45 | u_control = vehicle.DPcontrol(eta,nu,sampleTime) 46 | elif (vehicle.controlMode == 'DPcontrol1'): 47 | u_control = vehicle.DPcontrol(eta,nu,sampleTime) 48 | elif (vehicle.controlMode == 'DPcontrol2'): 49 | u_control = vehicle.DPcontrol(eta,nu,sampleTime) 50 | elif (vehicle.controlMode == 'stepInput'): 51 | u_control = vehicle.stepInput(t) 52 | 53 | # Store simulation data in simData 54 | signals = np.append( np.append( np.append(eta,nu),u_control), u_actual ) 55 | simData = np.vstack( [simData, signals] ) 56 | 57 | # Propagate vehicle and attitude dynamics 58 | [nu, u_actual] = vehicle.dynamics(eta,nu,u_actual,u_control,sampleTime) 59 | eta = attitudeEuler(eta,nu,sampleTime) 60 | 61 | # Store simulation time vector 62 | simTime = np.arange(start=0, stop=t+sampleTime, step=sampleTime)[:, None] 63 | 64 | return(simTime,simData) 65 | -------------------------------------------------------------------------------- /functions/models.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Vehicle models. 4 | 5 | Author: Thor I. Fossen 6 | """ 7 | 8 | import numpy as np 9 | import math 10 | 11 | def clarke83(U,L,B,T,Cb,R66,xg,T_surge): 12 | """ 13 | [M,N] = clarke83(U,L,B,T,Cb,R66,xg,T_surge) computes the system matrices 14 | of a linear maneuvering model based on Clarke et al. (1983). The 15 | hydrodynamic derivatives are based on multiple linear regression from two 16 | sets of model tests. The first data set (Yv, Yr, Nv, Nr) is obtained from 17 | rotating arm model experiments, while the second data set 18 | (Yvdot, Yrdot, Nvdot, Nrdot, Yv, Yr, Nv, Nr) was obtained from a PMM model. 19 | 20 | Outputs: 3x3 model matrices M and N in surge, sway and yaw 21 | 22 | M nu_dot + N(U) nu = tau, where N(U) = C(U) + D 23 | 24 | corresponding to the linear maneuvering model 25 | 26 | (m - Xudot) u_dot - Xu u = (1-t) T 27 | (m - Yvdot) v_dot + (m - Yrdot) r_dot - Yv v - Yr r = Yd delta 28 | (m - Yvdot) v_dot + (Iz - Nrdot) r_dot - Nv v - Nr r = Nd delta 29 | 30 | Note that the coefficients Yv, Yr, Nv and Nr in the N(U) matrix includes 31 | linear damping D and the linearized Coriolis and centripetal matrix C(U). 32 | 33 | Inputs: 34 | 35 | U: speed (m/s) 36 | L: length (m) 37 | B: beam (m) 38 | T: draft (m) 39 | Cb: block coefficient (-), Cb = V / (L*B*T) where V is the displaced volume 40 | R66: radius of gyration in yaw (smaller vessels R66 ≈ 0.25L, tankers R66 ≈ 0.27L) 41 | xg: x-coordinate of the CG (m) 42 | T_surge: time constant in surge (s) 43 | 44 | Reference: CLARKE, D., GEDLING, P. and HINE. G. (1983). The application of 45 | manoeuvring criteria in hull design using linear thory. Trans. R. lnsm nav. 46 | Archit. 125, 45-68. 47 | """ 48 | 49 | # Rigid-body parameters 50 | rho = 1025 # density of water 51 | V = Cb * L * B * T # volume displacment 52 | m = rho * V # mass 53 | Iz = m * R66**2 + m * xg**2 # moment of inerta about the CO 54 | 55 | # Rigid-body inertia matrix 56 | MRB = np.array([ 57 | [ m, 0 ,0], 58 | [ 0, m ,m*xg], 59 | [ 0, m*xg ,Iz] ],float) 60 | 61 | # Nondimenisonal hydrodynamic derivatives in surge 62 | Xudot = -0.1 * m 63 | U += 0.001 # avoid the singularity for U = 0 64 | Xu = -( ( m-Xudot )/T_surge ) / ( 0.5 * rho * L**2 * U ) 65 | Xudot = Xudot / ( 0.5 * rho * L**3 ) 66 | 67 | # Nondimenisonal hydrodynamic derivatives in sway and yaw 68 | # from Clarke et al. (1983) 69 | S = math.pi * (T/L)**2 # scale factor 70 | 71 | Yvdot = -S * ( 1 + 0.16 * Cb * B/T - 5.1 * (B/L)**2 ) 72 | Yrdot = -S * ( 0.67 * B/L - 0.0033 * (B/T)**2 ) 73 | Nvdot = -S * ( 1.1 * B/L - 0.041 * (B/T) ) 74 | Nrdot = -S * ( 1/12 + 0.017 * Cb * (B/T) - 0.33 * (B/L) ) 75 | Yv = -S * ( 1 + 0.4 * Cb * (B/T) ) 76 | Yr = -S * ( -1/2 + 2.2 * (B/L) - 0.08 * (B/T) ) 77 | Nv = -S * ( 1/2 + 2.4 * (T/L) ) 78 | Nr = -S * ( 1/4 + 0.039 * (B/T) - 0.56 * (B/L) ) 79 | 80 | # Nondimenisonal hydrodynamic matrices 81 | MA_prime = np.array([ 82 | [ -Xudot, 0 ,0], 83 | [ 0, -Yvdot, -Yrdot], 84 | [ 0, -Nvdot, -Nrdot] ],float) 85 | 86 | N_prime = np.array([ 87 | [ -Xu, 0 ,0], 88 | [ 0, -Yv, -Yr], 89 | [ 0, -Nv, -Nr] ],float) 90 | 91 | # Dimensional model (Fossen 2021, Appendix D) 92 | T = np.diag([1, 1, 1/L]) 93 | Tinv = np.diag([1, 1, L]) 94 | 95 | MA = (0.5 * rho * L**3) * Tinv @ Tinv @ \ 96 | np.matmul( T, np.matmul( MA_prime,Tinv ) ) 97 | 98 | N = (0.5 * rho * L**2 * U) * Tinv @ Tinv @ \ 99 | np.matmul( T, np.matmul( N_prime, Tinv ) ) 100 | 101 | M = MRB + MA # system inertia matrix 102 | 103 | return M, N 104 | 105 | 106 | 107 | 108 | 109 | -------------------------------------------------------------------------------- /functions/plotTimeSeries.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Simulator plotting functions: 4 | 5 | plotVehicleStates(simTime, simData, figNo) 6 | plotControls(simTime, simData)) 7 | 8 | Author: Thor I. Fossen 9 | """ 10 | 11 | import math 12 | import matplotlib.pyplot as plt 13 | import numpy as np 14 | from functions.gnc import ssa 15 | 16 | legendSize = 10 # legend size 17 | figSize1 = [25, 13] # figure1 size in cm 18 | figSize2 = [25, 13] # figure2 size in cm 19 | dpiValue = 150 # figure dpi value 20 | 21 | def R2D(value): # radians to degrees 22 | return value * 180 / math.pi 23 | 24 | def cm2inch(value): # inch to cm 25 | return value/2.54 26 | 27 | # plotVehicleStates(simTime, simData, figNo) plots the 6-DOF vehicle 28 | # position/attitude and velocities versus time in figure no. figNo 29 | def plotVehicleStates(simTime, simData, figNo): 30 | 31 | # Time vector 32 | t = simTime 33 | 34 | # State vectors 35 | x = simData[:,0] 36 | y = simData[:,1] 37 | z = simData[:,2] 38 | phi = R2D(ssa(simData[:,3])) 39 | theta = R2D(ssa(simData[:,4])) 40 | psi = R2D(ssa(simData[:,5])) 41 | u = simData[:,6] 42 | v = simData[:,7] 43 | w = simData[:,8] 44 | p = R2D(simData[:,9]) 45 | q = R2D(simData[:,10]) 46 | r = R2D(simData[:,11]) 47 | 48 | # Speed 49 | U = np.sqrt( np.multiply(u,u) + np.multiply(v,v) + np.multiply(w,w) ) 50 | 51 | beta_c = R2D(ssa(np.arctan2(v,u))) # crab angle, beta_c 52 | alpha_c = R2D(ssa(np.arctan2(w,u))) # flight path angle 53 | chi = R2D(ssa(simData[:,5]+np.arctan2(v,u))) # course angle, chi=psi+beta_c 54 | 55 | # Plots 56 | plt.figure(figNo,figsize=(cm2inch(figSize1[0]),cm2inch(figSize1[1])), 57 | dpi=dpiValue) 58 | plt.grid() 59 | 60 | plt.subplot(3, 3, 1) 61 | plt.plot(y, x) 62 | plt.legend(['North-East positions (m)'],fontsize=legendSize) 63 | plt.grid() 64 | 65 | plt.subplot(3, 3, 2) 66 | plt.plot(t, z) 67 | plt.legend(['Depth (m)'],fontsize=legendSize) 68 | plt.grid() 69 | 70 | plt.title('Vehicle states', fontsize=12) 71 | 72 | plt.subplot(3, 3, 3) 73 | plt.plot(t, phi, t, theta) 74 | plt.legend(['Roll angle (deg)','Pitch angle (deg)'],fontsize=legendSize) 75 | plt.grid() 76 | 77 | plt.subplot(3, 3, 4) 78 | plt.plot(t, U) 79 | plt.legend(['Speed (m/s)'],fontsize=legendSize) 80 | plt.grid() 81 | 82 | plt.subplot(3, 3, 5) 83 | plt.plot(t, chi) 84 | plt.legend(['Course angle (deg)'],fontsize=legendSize) 85 | plt.grid() 86 | 87 | plt.subplot(3, 3, 6) 88 | plt.plot(t, theta, t, alpha_c) 89 | plt.legend(['Pitch angle (deg)','Flight path angle (deg)'], 90 | fontsize=legendSize) 91 | plt.grid() 92 | 93 | plt.subplot(3, 3, 7) 94 | plt.plot(t, u, t, v, t, w) 95 | plt.xlabel('Time (s)', fontsize=12) 96 | plt.legend(['Surge velocity (m/s)','Sway velocity (m/s)', 97 | 'Heave velocity (m/s)'],fontsize=legendSize) 98 | plt.grid() 99 | 100 | plt.subplot(3, 3, 8) 101 | plt.plot(t, p, t, q, t, r) 102 | plt.xlabel('Time (s)', fontsize=12) 103 | plt.legend(['Roll rate (deg/s)','Pitch rate (deg/s)', 104 | 'Yaw rate (deg/s)'],fontsize=legendSize) 105 | plt.grid() 106 | 107 | plt.subplot(3, 3, 9) 108 | plt.plot(t, psi, t, beta_c) 109 | plt.xlabel('Time (s)', fontsize=12) 110 | plt.legend(['Yaw angle (deg)','Crab angle (deg)'],fontsize=legendSize) 111 | plt.grid() 112 | 113 | # plotControls(simTime, simData) plots the vehicle control inputs versus time 114 | # in figure no. figNo 115 | def plotControls(simTime, simData, vehicle, figNo): 116 | 117 | DOF = 6 118 | 119 | # Time vector 120 | t = simTime 121 | 122 | plt.figure(figNo,figsize=(cm2inch(figSize2[0]),cm2inch(figSize2[1])), 123 | dpi=dpiValue) 124 | 125 | # Columns and rows needed to plot vehicle.dimU control inputs 126 | col = 2 127 | row = int( math.ceil(vehicle.dimU / col) ) 128 | 129 | # Plot the vehicle.dimU active control inputs 130 | for i in range(0,vehicle.dimU): 131 | 132 | u_control = simData[:,2*DOF+i] # control input, commands 133 | u_actual = simData[:,2*DOF+vehicle.dimU+i] # actual control input 134 | 135 | if vehicle.controls[i].find("deg") != -1: # convert angles to deg 136 | u_control = R2D(u_control) 137 | u_actual = R2D(u_actual) 138 | 139 | plt.subplot(row, col, i+1) 140 | plt.plot(t, u_control, t, u_actual) 141 | plt.legend([ 142 | vehicle.controls[i] + ', command', 143 | vehicle.controls[i] + ', actual' 144 | ], fontsize=legendSize) 145 | plt.xlabel('Time (s)', fontsize=12) 146 | plt.grid() 147 | 148 | 149 | -------------------------------------------------------------------------------- /imgs/lqr_control.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MoMagDii/uuv_python_simulator/ffee74a0644739f814604a58c0a8303e6e9ba3c3/imgs/lqr_control.png -------------------------------------------------------------------------------- /imgs/lqr_state.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MoMagDii/uuv_python_simulator/ffee74a0644739f814604a58c0a8303e6e9ba3c3/imgs/lqr_state.png -------------------------------------------------------------------------------- /imgs/pid_control.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MoMagDii/uuv_python_simulator/ffee74a0644739f814604a58c0a8303e6e9ba3c3/imgs/pid_control.png -------------------------------------------------------------------------------- /imgs/pid_state.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MoMagDii/uuv_python_simulator/ffee74a0644739f814604a58c0a8303e6e9ba3c3/imgs/pid_state.png -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | main.py: Main program for the Python Vehicle Simulator, which can be used 4 | to simulate and test guidance, navigation and control (GNC) systems. 5 | 6 | Reference: T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and 7 | Motion Control. 2nd. Edition, Wiley. 8 | URL: www.fossen.biz/wiley 9 | 10 | Author: Thor I. Fossen 11 | """ 12 | import matplotlib.pyplot as plt 13 | from functions import plotVehicleStates,plotControls,simulate 14 | from vehicles import * 15 | 16 | # Simulation parameters: sample time and number of samples 17 | sampleTime = 0.02 18 | N = 10000 19 | 20 | """ 21 | swift('DPcontrol1',x_d,y_d,z_d,phi_d,theta_d,psi_d,V_c,beta_c)) 22 | swift('DPcontrol2',x_d,y_d,z_d,phi_d,theta_d,psi_d,V_c,beta_c)) 23 | DSRV('depthAutopilot',z_d) 24 | frigate('headingAutopilot',U,psi_d) 25 | otter('headingAutopilot',psi_d,V_c,beta_c,tau_X) 26 | ROVzefakkel('headingAutopilot',U,psi_d) 27 | semisub('DPcontrol',x_d,y_d,psi_d,V_c,beta_c) 28 | shipClarke83('headingAutopilot',psi_d,L,B,T,Cb,V_c,beta_c,tau_X) 29 | supply('DPcontrol',x_d,y_d,psi_d,V_c,beta_c) 30 | tanker('headingAutopilot',psi_d,V_current,beta_c,depth) 31 | 32 | Call constructors without arguments to test step inputs, e.g. DSRV(), otter(), etc. 33 | """ 34 | 35 | # vehicle1 = DSRV('depthAutopilot',60.0) 36 | #vehicle1 = otter('headingAutopilot',100.0,0.3,-30.0,200.0) 37 | # vehicle2 = ROVzefakkel('headingAutopilot',3.0,100.0) 38 | #vehicle2 = frigate('headingAutopilot',10.0,100.0) 39 | # vehicle2 = tanker('headingAutopilot',-20,0.5,150,20,80) 40 | # vehicle2 = shipClarke83('headingAutopilot',-20.0,70,8,6,0.7,0.5,-10.0,1e5) 41 | #vehicle1 = semisub('DPcontrol',10.0,2.0,20.0,0.5,-20.0) 42 | vehicle1 = swift('DPcontrol1',5.0,5.0,5.0,0.0,0.0,45.0,0.0000,-0.000) 43 | vehicle2 = swift('DPcontrol2',5.0,5.0,5.0,0.0,0.0,45.0,0.0000,-0.000) 44 | 45 | 46 | # Main simulation loop 47 | def main(): 48 | 49 | [simTime1, simData1] = simulate(N, sampleTime, vehicle1) 50 | plotVehicleStates(simTime1, simData1, 1) 51 | plotControls(simTime1, simData1, vehicle1, 2) 52 | 53 | [simTime2, simData2] = simulate(N, sampleTime, vehicle2) 54 | plotVehicleStates(simTime2, simData2, 3) 55 | plotControls(simTime2, simData2, vehicle2, 4) 56 | plt.show() 57 | 58 | 59 | main() 60 | -------------------------------------------------------------------------------- /vehicles/DSRV.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | DSRV.py: 4 | 5 | Class for the Naval Postgraduate School deep submergence rescue vehicle 6 | (DSRV). The length of the vehicle is L = 5.0 m and the state vector is 7 | nu = [ 0 0 w 0 q 0]' where w is the heave velocity (m/s) and q is the 8 | pitch rate (rad/s). The constructors are: 9 | 10 | DSRV() 11 | Step input, rudder angel 12 | DSRV('deptAutopilot',z_d) 13 | Depth autopilot with option: 14 | z_d: desired depth (m) 15 | 16 | Methods: 17 | 18 | [nu, u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) returns 19 | nu[k+1] and u_actual[k+1] using Euler's method. The control input is: 20 | 21 | u_control = delta_s (rad): DSRV stern plane. 22 | 23 | u = depthAutopilot(eta,nu,sampleTime) 24 | PID controller for automatic heading control based on pole placement. 25 | 26 | u = stepInput(t) generates stern plane step inputs. 27 | 28 | References: 29 | A. J. Healey (1992). Marine Vehicle Dynamics Lecture Notes and 30 | Problem Sets, Naval Postgraduate School (NPS), Monterey, CA. 31 | T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion 32 | Control. 2nd. Edition, Wiley. URL: www.fossen.biz/wiley 33 | 34 | Author: Thor I. Fossen 35 | """ 36 | import numpy as np 37 | import math 38 | from functions.control import PIDpolePlacement 39 | 40 | # Class Vehicle 41 | class DSRV: 42 | """ 43 | DSRV() Step input, rudder angle 44 | DSRV('deptAutopilot',z_d) Depth autopilot, desired depth (m) 45 | """ 46 | def __init__(self, controlSystem = 'stepInput', r = 0): 47 | 48 | if (controlSystem == 'depthAutopilot'): 49 | self.controlDescription = 'Depth autopilot, z_d = ' + str(r) + ' (m)' 50 | 51 | else: 52 | self.controlDescription = "Step input for delta_s" 53 | controlSystem = 'stepInput' 54 | 55 | self.ref = r 56 | self.controlMode = controlSystem 57 | 58 | # Initialize the DSRV model 59 | self.name = "DSRV" 60 | self.L = 5.0 # Length 61 | self.deltaMax = 20 # max stern plane angle (deg) 62 | self.T_delta = 1.0 # rudder time constants (s) 63 | self.U0 = 4.11 # Cruise speed: 4.11 m/s = 8 knots 64 | self.W0 = 0 65 | self.nu = np.array([ self.U0, 0, self.W0, 0, 0, 0], float ) # velocity vector 66 | self.u_actual = np.array([0],float) # control input vector 67 | 68 | self.controls = ['Stern plane (deg)'] 69 | self.dimU = len(self.controls) 70 | 71 | # Non-dimensional mass matrix 72 | Iy = 0.001925 73 | m = 0.036391 74 | Mqdot = -0.001573; Zqdot = -0.000130 75 | Mwdot = -0.000146; Zwdot = -0.031545 76 | 77 | self.m11 = m - Zwdot 78 | self.m12 = -Zqdot 79 | self.m22 = Iy - Mqdot 80 | self.m21 = -Mwdot 81 | 82 | self.detM = self.m11 * self.m22 - self.m12 * self.m21; 83 | 84 | # Non-dimensional hydrodynamic derivatives 85 | self.Mq = -0.01131; self.Zq = -0.017455 86 | self.Mw = 0.011175; self.Zw = -0.043938 87 | self.Mdelta = -0.012797 88 | self.Zdelta = 0.027695 89 | 90 | # Depth autopilot 91 | self.e_int = 0.0 # integral state, initial value 92 | self.wn = 1 # PID pole placement parameters 93 | self.zeta = 1 94 | 95 | # Reference model 96 | self.w_max = 1 # maximum heave velocity 97 | self.z_d = 0 # position, velocity and acc. states 98 | self.w_d = 0 99 | self.a_d = 0 100 | self.wn_d = self.wn / 5 101 | self.zeta_d = 1 102 | 103 | 104 | def dynamics(self,eta,nu,u_actual,u_control,sampleTime): 105 | """ 106 | [nu, u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) 107 | integrates the DSRV equations of motion using Euler's method. 108 | """ 109 | 110 | # States and inputs 111 | delta_c = u_control[0] 112 | delta = u_actual[0] 113 | w = nu[2] 114 | q = nu[4] 115 | theta = eta[4] 116 | 117 | # Speed 118 | U = math.sqrt( self.U0**2 + (self.W0 + w)**2 ) 119 | 120 | # Speed dependent pitch moment 121 | Mtheta = -0.156276 / U**2 122 | 123 | # Rudder angle saturation 124 | if ( abs(delta) >= self.deltaMax * math.pi/180 ): 125 | delta = np.sign(delta) * self.deltaMax * math.pi/180 126 | 127 | # Forces and moments 128 | Z = self.Zq * q + self.Zw * w + self.Zdelta * delta 129 | M = self.Mq * q + self.Mw * w + Mtheta * theta + self.Mdelta * delta 130 | 131 | # State derivatives (with dimension) 132 | nu_dot = np.zeros(6) 133 | nu_dot[2] = ( self.m22 * Z - self.m12 * M) / self.detM 134 | nu_dot[4] = ( -self.m21 * Z + self.m11 * M) / self.detM 135 | 136 | # stern plane dynamics 137 | delta_dot = (delta_c - delta) / self.T_delta # rudder dynamics 138 | 139 | # Forward Euler integration [k+1] 140 | nu = nu + sampleTime * nu_dot 141 | delta = delta + sampleTime * delta_dot 142 | 143 | # Cruise speed (constant) 144 | nu[0] = self.U0 145 | 146 | u_actual = np.array([delta],float) 147 | 148 | return nu, u_actual 149 | 150 | 151 | def stepInput(self,t): 152 | """ 153 | delta_c = stepInput(t) generates stern plane step inputs. 154 | """ 155 | delta_c = 20 * (math.pi/180) 156 | if t > 30: 157 | delta_c = 10 * (math.pi/180) 158 | if t > 50: 159 | delta_c = 0 160 | 161 | u_control = np.array([delta_c],float) 162 | 163 | return u_control 164 | 165 | def depthAutopilot(self,eta,nu,sampleTime): 166 | """ 167 | delta_c = depthAutopilot(eta,nu,sampleTime) is a PID controller for 168 | automatic depth control based on pole placement. 169 | """ 170 | z = eta[2] # heave position 171 | w = nu[2] # heave velocity 172 | e_z = z - self.z_d # heave position tracking error 173 | e_w = w - self.w_d # heave velocity tracking error 174 | r = self.ref # heave setpoint 175 | 176 | wn = self.wn # PID natural frequency 177 | zeta = self.zeta # PID natural relative damping factor 178 | wn_d = self.wn_d # reference model natural frequency 179 | zeta_d = self.zeta_d # reference model relative damping factor 180 | 181 | m = self.m11 # mass in heave including added mass 182 | d = 0 183 | k = 0 184 | 185 | # PID feedback controller with 3rd-order reference model 186 | [delta_c, self.e_int, self.z_d, self.w_d, self.a_d] = PIDpolePlacement( \ 187 | self.e_int, e_z, e_w, \ 188 | self.z_d, self.w_d, self.a_d, \ 189 | m, d, k, wn_d, zeta_d, wn, zeta, r, self.w_max, sampleTime ) 190 | 191 | u_control = np.array([delta_c],float) 192 | 193 | return u_control 194 | 195 | 196 | -------------------------------------------------------------------------------- /vehicles/ROVzefakkel.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | ROVzefakkel.py: 4 | 5 | Class for the ROV Zefakkel (Length 45 m) based on the Norrbin (1963) 6 | nonlinear autopilot model 7 | 8 | psi_dot = r - yaw kinematics 9 | T r_dot + n3 r^3 + n1 r = K delta + d_r - Norrbin (1963) model 10 | dot_delta = f(delta, delta_c) - rudder dynamics 11 | 12 | The ship is controlled by a controllable pitch propeller and a rudder with 13 | rudder dynamics. The model parameters K, T and n3 are speed dependent, 14 | while n = 1 (course stable ship). The parameter sare interpolated for 15 | varying speeds U. The velocity state vector is nu = [ 0 0 0 0 0 r]' 16 | where r is the yaw vwlocity (rad/s). The constructors are: 17 | 18 | ROVzefakkel() 19 | Step input, rudder angel 20 | ROVzefakkel('headingAutopilot',psi_d) 21 | Heading autopilot with option: 22 | psi_d: desired heading (m) 23 | 24 | Methods: 25 | 26 | [nu, u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) returns 27 | nu[k+1] and u_actual[k+1] using Euler's method. The control input is: 28 | 29 | u_control = delta (rad): rudder angle 30 | 31 | u = headingAutopilot(eta,nu,sampleTime) 32 | PID controller for automatic heading control based on pole placement. 33 | 34 | u = stepInput(t) generates rudder angle step inputs. 35 | 36 | References: 37 | J. Van Amerongen (1982). Adaptive Steering of Ships – A Model Reference 38 | Approach to Improved Maneuvering and Economical Course Keeping. PhD thesis. 39 | Delft University of Technology, Netherlands. 40 | T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion 41 | Control. 2nd. Edition, Wiley. URL: www.fossen.biz/wiley 42 | 43 | Author: Thor I. Fossen 44 | """ 45 | import numpy as np 46 | import math 47 | import sys 48 | from functions.control import PIDpolePlacement 49 | 50 | # Class Vehicle 51 | class ROVzefakkel: 52 | """ 53 | ROVzefakkel() Step input, rudder angle 54 | ROVzefakkel('headingAutopilot',U,psi_d) Heading autopilot, speed (m/s) 55 | and desired heading(deg) 56 | """ 57 | def __init__(self, controlSystem = 'stepInput', U = 3.0, r = 0.0): 58 | 59 | if (controlSystem == 'headingAutopilot'): 60 | self.controlDescription = 'Heading autopilot, psi_d = ' + str(r) + ' (deg)' 61 | 62 | else: 63 | self.controlDescription = "Step input for delta" 64 | controlSystem = 'stepInput' 65 | 66 | # Check if the speed U is within the valid range 67 | if (U < 1.0 or U > 7.0): 68 | sys.exit('The speed U should be between 5-12 m/s') 69 | 70 | self.ref = r 71 | self.controlMode = controlSystem 72 | 73 | # Initialize the ship model 74 | self.name = "ROV Zefakkel" 75 | self.L = 45.0 # Length 76 | self.deltaMax = 30 # max rudder angle (deg) 77 | self.DdeltaMax = 10 # max rudder rate (deg/s) 78 | self.nu = np.array([ U, 0, 0, 0, 0, 0],float) # velocity vector 79 | self.u_actual = np.array([0],float) # control input vector 80 | 81 | self.controls = ['Rudder angle (deg)'] 82 | self.dimU = len(self.controls) 83 | 84 | # ROV Zefakkel (Van Amerongen 1982) 85 | self.n1 = 1 86 | self.n3 = 0.4 87 | 88 | # interpolate to find K and T as a function of U 89 | U_data = np.array([ 2.0, 2.6, 3.6, 4.0, 5.0, 6.2 ],float) 90 | K_data = np.array([ 0.15, 0.19, 0.29, 0.37, 0.50, 0.83 ],float) 91 | T_data = np.array([ 33.0, 33.0, 33.0, 33.0, 31.0, 43.0 ],float) 92 | 93 | U = self.nu[0] 94 | self.K = np.interp( U, U_data, K_data ) 95 | self.T = np.interp( U, U_data, T_data ) 96 | 97 | # Heading autopilot 98 | self.e_int = 0.0 # integral state, initial value 99 | self.wn = 0.4 # PID pole placement parameters 100 | self.zeta = 1 101 | 102 | # Reference model 103 | self.r_max = 1 * math.pi / 180 # maximum yaw rate 104 | self.psi_d = 0 # position, velocity and acc. states 105 | self.r_d = 0 106 | self.a_d = 0 107 | self.wn_d = self.wn / 5 108 | self.zeta_d = 1 109 | 110 | 111 | def dynamics(self,eta,nu,u_actual,u_control,sampleTime): 112 | """ 113 | [nu, u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) 114 | integrates the ship equations of motion using Euler's method. 115 | """ 116 | 117 | # States and inputs 118 | delta_c = u_control[0] 119 | delta = u_actual[0] 120 | r = nu[5] 121 | 122 | # Rudder angle saturation and dynamics 123 | if ( abs(delta) >= self.deltaMax * math.pi/180 ): 124 | delta = np.sign(delta) * self.deltaMax * math.pi/180 125 | 126 | delta_dot = delta_c - delta 127 | if ( abs(delta_dot) >= self.DdeltaMax * math.pi/180 ): 128 | delta_dot = np.sign(delta_dot) * self.DdeltaMax * math.pi/180 129 | 130 | # Dynamics 131 | r_dot = (1 / self.T) * ( self.K * delta - self.n3 * r**3 - self.n1 * r ) 132 | nu_dot = np.array( [0, 0, 0, 0, 0, r_dot], float) 133 | 134 | # Forward Euler integration [k+1] 135 | nu = nu + sampleTime * nu_dot 136 | delta = delta + sampleTime * delta_dot 137 | 138 | u_actual = np.array([delta],float) 139 | 140 | return nu, u_actual 141 | 142 | 143 | def stepInput(self,t): 144 | """ 145 | delta_c = stepInput(t) generates stern plane step inputs. 146 | """ 147 | delta_c = 20 * (math.pi/180) 148 | if t > 30: 149 | delta_c = 10 * (math.pi/180) 150 | if t > 50: 151 | delta_c = 0 152 | 153 | u_control = np.array([delta_c],float) 154 | 155 | return u_control 156 | 157 | 158 | def headingAutopilot(self,eta,nu,sampleTime): 159 | """ 160 | u = headingAutopilot(eta,nu,sampleTime) is a PID controller 161 | for automatic heading control based on pole placement. 162 | 163 | delta = (T/K) * a_d + (1/K) * rd 164 | - Kp * ( ssa( psi-psi_d ) + Td * (r - r_d) + (1/Ti) * z ) 165 | 166 | """ 167 | psi = eta[5] # yaw angle 168 | r = nu[5] # yaw rate 169 | e_psi = psi - self.psi_d # yaw angle tracking error 170 | e_r = r - self.r_d # yaw rate tracking error 171 | psi_ref = self.ref * math.pi / 180 # yaw angle setpoint 172 | 173 | wn = self.wn # PID natural frequency 174 | zeta = self.zeta # PID natural relative damping factor 175 | wn_d = self.wn_d # reference model natural frequency 176 | zeta_d = self.zeta_d # reference model relative damping factor 177 | 178 | m = self.T / self.K 179 | d = self.n1 / self.K 180 | k = 0 181 | 182 | # PID feedback controller with 3rd-order reference model 183 | [delta, self.e_int, self.psi_d, self.r_d, self.a_d] = \ 184 | PIDpolePlacement( self.e_int, e_psi, e_r, self.psi_d, self.r_d, self.a_d, \ 185 | m, d, k, wn_d, zeta_d, wn, zeta, psi_ref, self.r_max, sampleTime ) 186 | 187 | u_control = np.array([delta],float) 188 | 189 | return u_control -------------------------------------------------------------------------------- /vehicles/__init__.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | from .DSRV import * 4 | from .frigate import * 5 | from .otter import * 6 | from .ROVzefakkel import * 7 | from .semisub import * 8 | from .shipClarke83 import * 9 | from .supply import * 10 | from .tanker import * 11 | from .swift import * 12 | -------------------------------------------------------------------------------- /vehicles/frigate.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | frigate.py: 4 | 5 | Class for the frigate (Length 100 m) based on the Norrbin (1963) 6 | nonlinear autopilot model 7 | 8 | psi_dot = r - yaw kinematics 9 | T r_dot + n3 r^3 + n1 r = K delta + d_r - Norrbin (1963) model 10 | dot_delta = f(delta, delta_c) - rudder dynamics 11 | 12 | The ship is controlled by a controllable pitch propeller and a rudder with 13 | rudder dynamics. The model parameters K, T and n3 are speed dependent, 14 | while n = 1 (course stable ship). The parameter sare interpolated for 15 | varying speeds U. The velocity state vector is nu = [ 0 0 0 0 0 r]' 16 | where r is the yaw vwlocity (rad/s). The constructors are: 17 | 18 | frigate() 19 | Step input, rudder angel 20 | frigate('headingAutopilot',psi_d) 21 | Heading autopilot with option: 22 | psi_d: desired heading (m) 23 | 24 | Methods: 25 | 26 | [nu, u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) returns 27 | nu[k+1] and u_actual[k+1] using Euler's method. The control input is: 28 | 29 | u_control = delta (rad): rudder angle 30 | 31 | u = headingAutopilot(eta,nu,sampleTime) 32 | PID controller for automatic heading control based on pole placement. 33 | 34 | u = stepInput(t) generates rudder angle step inputs. 35 | 36 | References: 37 | J. Van Amerongen (1982). Adaptive Steering of Ships – A Model Reference 38 | Approach to Improved Maneuvering and Economical Course Keeping. PhD thesis. 39 | Delft University of Technology, Netherlands. 40 | T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion 41 | Control. 2nd. Edition, Wiley. URL: www.fossen.biz/wiley 42 | 43 | Author: Thor I. Fossen 44 | """ 45 | import numpy as np 46 | import math 47 | import sys 48 | from functions.control import PIDpolePlacement 49 | 50 | # Class Vehicle 51 | class frigate: 52 | """ 53 | frigate() Step input, rudder angle 54 | frigate('headingAutopilot',U,psi_d) Heading autopilot, speed (m/s) 55 | and desired heading(deg) 56 | """ 57 | def __init__(self, controlSystem = 'stepInput', U = 3.0, r = 0.0): 58 | 59 | if (controlSystem == 'headingAutopilot'): 60 | self.controlDescription = 'Heading autopilot, psi_d = ' + str(r) + ' (deg)' 61 | 62 | else: 63 | self.controlDescription = "Step input for delta" 64 | controlSystem = 'stepInput' 65 | 66 | # Check if the speed U is within the valid range 67 | if (U < 5.0 or U > 12.0): 68 | sys.exit('The speed U should be between 5-12 m/s') 69 | 70 | self.ref = r 71 | self.controlMode = controlSystem 72 | 73 | # Initialize the ship model 74 | self.name = "Frigate" 75 | self.L = 100.0 # Length 76 | self.deltaMax = 30 # max rudder angle (deg) 77 | self.DdeltaMax = 10 # max rudder rate (deg/s) 78 | self.nu = np.array([ U, 0, 0, 0, 0, 0],float) # velocity vector 79 | self.u_actual = np.array([0],float) # control input vector 80 | 81 | self.controls = ['Rudder angle (deg)'] 82 | self.dimU = len(self.controls) 83 | 84 | # ROV Zefakkel (Van Amerongen 1982) 85 | self.n1 = 1 86 | self.n3 = 0.4 87 | 88 | # interpolate to find K, T and n3 as a function of U 89 | U_data = np.array([ 6.0, 9.0, 12.0 ],float) 90 | K_data = np.array([ 0.08, 0.18, 0.23 ],float) 91 | T_data = np.array([ 20.0, 27.0, 21.0 ],float) 92 | n3_data = np.array([ 0.4, 0.6, 0.3 ],float) 93 | 94 | 95 | U = self.nu[0] 96 | self.K = np.interp( U, U_data, K_data ) 97 | self.T = np.interp( U, U_data, T_data ) 98 | self.n3 = np.interp( U, U_data, n3_data ) 99 | 100 | # Heading autopilot 101 | self.e_int = 0.0 # integral state, initial value 102 | self.wn = 0.4 # PID pole placement parameters 103 | self.zeta = 1 104 | 105 | # Reference model 106 | self.r_max = 1 * math.pi / 180 # maximum yaw rate 107 | self.psi_d = 0 # position, velocity and acc. states 108 | self.r_d = 0 109 | self.a_d = 0 110 | self.wn_d = self.wn / 5 111 | self.zeta_d = 1 112 | 113 | 114 | def dynamics(self,eta,nu,u_actual,u_control,sampleTime): 115 | """ 116 | [nu, u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) 117 | integrates the ship equations of motion using Euler's method. 118 | """ 119 | 120 | # States and inputs 121 | delta_c = u_control[0] 122 | delta = u_actual[0] 123 | r = nu[5] 124 | 125 | # Rudder angle saturation and dynamics 126 | if ( abs(delta) >= self.deltaMax * math.pi/180 ): 127 | delta = np.sign(delta) * self.deltaMax * math.pi/180 128 | 129 | delta_dot = delta_c - delta 130 | if ( abs(delta_dot) >= self.DdeltaMax * math.pi/180 ): 131 | delta_dot = np.sign(delta_dot) * self.DdeltaMax * math.pi/180 132 | 133 | # Dynamics 134 | r_dot = (1 / self.T) * ( self.K * delta - self.n3 * r**3 - self.n1 * r ) 135 | nu_dot = np.array( [0, 0, 0, 0, 0, r_dot], float) 136 | 137 | # Forward Euler integration [k+1] 138 | nu = nu + sampleTime * nu_dot 139 | delta = delta + sampleTime * delta_dot 140 | 141 | u_actual = np.array([delta],float) 142 | 143 | return nu, u_actual 144 | 145 | 146 | def stepInput(self,t): 147 | """ 148 | delta_c = stepInput(t) generates stern plane step inputs. 149 | """ 150 | delta_c = 20 * (math.pi/180) 151 | if t > 30: 152 | delta_c = 10 * (math.pi/180) 153 | if t > 50: 154 | delta_c = 0 155 | 156 | u_control = np.array([delta_c],float) 157 | 158 | return u_control 159 | 160 | 161 | def headingAutopilot(self,eta,nu,sampleTime): 162 | """ 163 | u = headingAutopilot(eta,nu,sampleTime) is a PID controller 164 | for automatic heading control based on pole placement. 165 | 166 | delta = (T/K) * a_d + (1/K) * rd 167 | - Kp * ( ssa( psi-psi_d ) + Td * (r - r_d) + (1/Ti) * z ) 168 | 169 | """ 170 | psi = eta[5] # yaw angle 171 | r = nu[5] # yaw rate 172 | e_psi = psi - self.psi_d # yaw angle tracking error 173 | e_r = r - self.r_d # yaw rate tracking error 174 | psi_ref = self.ref * math.pi / 180 # yaw angle setpoint 175 | 176 | wn = self.wn # PID natural frequency 177 | zeta = self.zeta # PID natural relative damping factor 178 | wn_d = self.wn_d # reference model natural frequency 179 | zeta_d = self.zeta_d # reference model relative damping factor 180 | 181 | m = self.T / self.K 182 | d = self.n1 / self.K 183 | k = 0 184 | 185 | # PID feedback controller with 3rd-order reference model 186 | [delta, self.e_int, self.psi_d, self.r_d, self.a_d] = \ 187 | PIDpolePlacement( self.e_int, e_psi, e_r, self.psi_d, self.r_d, self.a_d, \ 188 | m, d, k, wn_d, zeta_d, wn, zeta, psi_ref, self.r_max, sampleTime ) 189 | 190 | u_control = np.array([delta],float) 191 | 192 | return u_control -------------------------------------------------------------------------------- /vehicles/otter.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | otter.py: 4 | Class for the Maritime Robotics Otter USV, www.maritimerobotics.com. 5 | The length of the USV is L = 2.0 m. The constructors are: 6 | 7 | otter() 8 | Step inputs for propeller revolutions n1 and n2 9 | otter('headingAutopilot',psi_d,V_current,beta_current,tau_X) 10 | Heading autopilot with options: 11 | psi_d: desired yaw angle (deg) 12 | V_current: current speed (m/s) 13 | beta_c: current direction (deg) 14 | tau_X: surge force, pilot input (N) 15 | 16 | Methods: 17 | 18 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) returns 19 | nu[k+1] and u_actual[k+1] using Euler's method. The control inputs are: 20 | 21 | u_control = [ n1 n2 ]' where 22 | n1: propeller shaft speed, left (rad/s) 23 | n2: propeller shaft speed, right (rad/s) 24 | 25 | u = headingAutopilot(eta,nu,sampleTime) 26 | PID controller for automatic heading control based on pole placement. 27 | 28 | u = stepInput(t) generates propeller step inputs. 29 | 30 | [n1, n2] = controlAllocation(tau_X, tau_N) 31 | Control allocation algorithm. 32 | 33 | References: 34 | T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion 35 | Control. 2nd. Edition, Wiley. 36 | URL: www.fossen.biz/wiley 37 | 38 | Author: Thor I. Fossen 39 | """ 40 | import numpy as np 41 | import math 42 | from functions.control import PIDpolePlacement 43 | from functions.gnc import Smtrx,Hmtrx,m2c,crossFlowDrag,sat 44 | 45 | # Class Vehicle 46 | class otter: 47 | """ 48 | otter() Step inputs for n1 and n2 49 | otter('headingAutopilot',psi_d) Heading autopilot, desired yaw angle (deg) 50 | """ 51 | def __init__(self, controlSystem = 'stepInput', 52 | r = 0, V_current = 0, beta_current = 0, tau_X = 120): 53 | 54 | if (controlSystem == 'headingAutopilot'): 55 | self.controlDescription = 'Heading autopilot, psi_d = ' \ 56 | + str(r) + ' (deg)' 57 | else: 58 | self.controlDescription = 'Step inputs for n1 and n2' 59 | controlSystem = 'stepInput' 60 | 61 | self.ref = r 62 | self.V_c = V_current 63 | self.beta_c = beta_current 64 | self.controlMode = controlSystem 65 | self.tauX = tau_X # surge force (N) 66 | 67 | # Initialize the Otter USV model 68 | self.T_n = 1.0 # propeller time constants (s) 69 | self.L = 2.0 # Length (m) 70 | self.B = 1.08 # beam (m) 71 | self.nu = np.array([0, 0, 0, 0, 0, 0], float) # velocity vector 72 | self.u_actual = np.array([0, 0], float) # propeller revolution states 73 | self.name = "Otter USV" 74 | 75 | self.controls = [ 76 | 'Left propeller shaft speed (rad/s)', 77 | 'Right propeller shaft speed (rad/s)' ] 78 | self.dimU = len(self.controls) 79 | 80 | # Constants 81 | g = 9.81 # acceleration of gravity (m/s^2) 82 | rho = 1025 # density of water 83 | 84 | m = 55.0 # mass (kg) 85 | mp = 25.0 # Payload (kg) 86 | self.m_total = m + mp 87 | rp = np.array([0, 0, -0.35],float) # location of payload (m) 88 | rg = np.array([0.2, 0, -0.2],float) # CG for hull only (m) 89 | rg = (m*rg + mp*rp) / (m + mp) # CG corrected for payload 90 | self.S_rg = Smtrx(rg) 91 | self.H_rg = Hmtrx(rg) 92 | self.S_rp = Smtrx(rp) 93 | 94 | R44 = 0.4 * self.B # radii of gyration (m) 95 | R55 = 0.25 * self.L 96 | R66 = 0.25 * self.L 97 | T_yaw = 1.0 # time constant in yaw (s) 98 | Umax = 6 * 0.5144 # max forward speed (m/s) 99 | 100 | # Data for one pontoon 101 | self.B_pont = 0.25 # beam of one pontoon (m) 102 | y_pont = 0.395 # distance from centerline to waterline centroid (m) 103 | Cw_pont = 0.75 # waterline area coefficient (-) 104 | Cb_pont = 0.4 # block coefficient, computed from m = 55 kg 105 | 106 | # Inertia dyadic, volume displacement and draft 107 | nabla = (m + mp) / rho # volume 108 | self.T = nabla / (2 * Cb_pont * self.B_pont*self.L) # draft 109 | Ig_CG = m * np.diag( np.array( [R44**2, R55**2, R66**2]) ) 110 | self.Ig = Ig_CG - m * self.S_rg @ self.S_rg - mp * self.S_rp @ self.S_rp 111 | 112 | # Experimental propeller data including lever arms 113 | self.l1 = -y_pont # lever arm, left propeller (m) 114 | self.l2 = y_pont # lever arm, right propeller (m) 115 | self.k_pos = 0.02216/2 # Positive Bollard, one propeller 116 | self.k_neg = 0.01289/2 # Negative Bollard, one propeller 117 | self.n_max = math.sqrt((0.5*24.4 * g)/self.k_pos) # max. prop. rev. 118 | self.n_min = -math.sqrt((0.5*13.6 * g)/self.k_neg) # min. prop. rev. 119 | 120 | # MRB_CG = [ (m+mp) * I3 O3 (Fossen 2021, Chapter 3) 121 | # O3 Ig ] 122 | MRB_CG = np.zeros( (6,6) ) 123 | MRB_CG[0:3, 0:3] = (m + mp) * np.identity(3) 124 | MRB_CG[3:6, 3:6] = self.Ig 125 | MRB = self.H_rg.T @ MRB_CG @ self.H_rg 126 | 127 | # Hydrodynamic added mass (best practice) 128 | Xudot = -0.1 * m 129 | Yvdot = -1.5 * m 130 | Zwdot = -1.0 * m 131 | Kpdot = -0.2 * self.Ig[0,0] 132 | Mqdot = -0.8 * self.Ig[1,1] 133 | Nrdot = -1.7 * self.Ig[2,2] 134 | 135 | self.MA = -np.diag([Xudot, Yvdot, Zwdot, Kpdot, Mqdot, Nrdot]) 136 | 137 | # System mass matrix 138 | self.M = MRB + self.MA 139 | self.Minv = np.linalg.inv(self.M) 140 | 141 | # Hydrostatic quantities (Fossen 2021, Chapter 4) 142 | Aw_pont = Cw_pont * self.L * self.B_pont # waterline area, one pontoon 143 | I_T = 2 * (1/12)*self.L * self.B_pont**3 * \ 144 | (6*Cw_pont**3/((1+Cw_pont)*(1+2*Cw_pont))) + 2 * Aw_pont * y_pont**2 145 | I_L = 0.8 * 2 * (1/12) * self.B_pont * self.L**3 146 | KB = (1/3) * ( 5 * self.T / 2 - 0.5 * nabla / (self.L * self.B_pont) ) 147 | BM_T = I_T/nabla # BM values 148 | BM_L = I_L/nabla 149 | KM_T = KB + BM_T # KM values 150 | KM_L = KB + BM_L 151 | KG = self.T - rg[2] 152 | GM_T = KM_T - KG # GM values 153 | GM_L = KM_L - KG 154 | 155 | G33 = rho * g * (2 * Aw_pont) # spring stiffness 156 | G44 = rho * g * nabla * GM_T 157 | G55 = rho * g * nabla * GM_L 158 | G_CF = np.diag([0, 0, G33, G44, G55, 0]) # spring stiff. matrix in CF 159 | LCF = -0.2 160 | H = Hmtrx(np.array([LCF, 0.0, 0.0])) # transform G_CF from CF to CO 161 | self.G = H.T @ G_CF @ H 162 | 163 | # Natural frequencies 164 | w3 = math.sqrt( G33 / self.M[2,2] ) 165 | w4 = math.sqrt( G44 / self.M[3,3] ) 166 | w5 = math.sqrt( G55 / self.M[4,4] ) 167 | 168 | # Linear damping terms (hydrodynamic derivatives) 169 | Xu = -24.4 * g / Umax # specified using the maximum speed 170 | Yv = 0 171 | Zw = -2 * 0.3 * w3 * self.M[2,2] # specified using relative damping 172 | Kp = -2 * 0.2 * w4 * self.M[3,3] 173 | Mq = -2 * 0.4 * w5 * self.M[4,4] 174 | Nr = -self.M[5,5] / T_yaw #specified by the time constant T_yaw 175 | 176 | self.D = -np.diag([Xu, Yv, Zw, Kp, Mq, Nr]) 177 | 178 | # Trim: theta = -7.5 deg corresponds to 13.5 cm less height aft 179 | self.trim_moment = 0 180 | self.trim_setpoint = 280; 181 | 182 | # Propeller configuration/input matrix 183 | B = self.k_pos * \ 184 | np.array([ 185 | [1, 1], 186 | [-self.l1, -self.l2] ]) 187 | self.Binv = np.linalg.inv(B) 188 | 189 | # Heading autopilot 190 | self.e_int = 0 # integral state 191 | self.wn = 1.2 # PID pole placement 192 | self.zeta = 0.8 193 | 194 | # Reference model 195 | self.r_max = 10 * math.pi / 180 # maximum yaw rate 196 | self.psi_d = 0 # angle, angular rate and angular acc. states 197 | self.r_d = 0 198 | self.a_d = 0 199 | self.wn_d = self.wn / 5 # desired natural frequency in yaw 200 | self.zeta_d = 1 # desired relative damping ratio 201 | 202 | 203 | def dynamics(self,eta,nu,u_actual,u_control,sampleTime): 204 | """ 205 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates 206 | the Otter USV equations of motion using Euler's method. 207 | """ 208 | 209 | # Input vector 210 | n = np.array( [u_actual[0], u_actual[1]] ) 211 | 212 | # Current velocities 213 | u_c = self.V_c * math.cos(self.beta_c - eta[5]) # current surge vel. 214 | v_c = self.V_c * math.sin(self.beta_c - eta[5]) # current sway vel. 215 | 216 | nu_c = np.array([u_c,v_c,0,0,0,0],float) # current velocity vector 217 | nu_r = nu - nu_c # relative velocity vector 218 | 219 | # Rigid body and added mass Coriolis and centripetal matrices 220 | # CRB_CG = [ (m+mp) * Smtrx(nu2) O3 (Fossen 2021, Chapter 6) 221 | # O3 -Smtrx(Ig*nu2) ] 222 | CRB_CG = np.zeros( (6,6) ) 223 | CRB_CG[0:3, 0:3] = self.m_total * Smtrx(nu[3:6]) 224 | CRB_CG[3:6, 3:6] = -Smtrx( np.matmul(self.Ig,nu[3:6]) ) 225 | CRB = self.H_rg.T @ CRB_CG @ self.H_rg # transform CRB from CG to CO 226 | 227 | CA = m2c(self.MA, nu_r) 228 | CA[5,0] = 0 # assume that the Munk moment in yaw can be neglected 229 | CA[5,1] = 0 # if nonzero, must be balanced by adding nonlinear damping 230 | 231 | C = CRB + CA 232 | 233 | # Ballast 234 | g_0 = np.array([0.0, 0.0, 0.0, 0.0, self.trim_moment, 0.0]) 235 | 236 | # Control forces and moments - with propeller revolution saturation 237 | thrust = np.zeros(2) 238 | for i in range(0,2): 239 | 240 | n[i] = sat(n[i],self.n_min,self.n_max) # saturation, physical limits 241 | 242 | if n[i] > 0: # positive thrust 243 | thrust[i] = self.k_pos * n[i] * abs(n[i]) 244 | else: # negative thrust 245 | thrust[i] = self.k_neg * n[i] * abs(n[i]) 246 | 247 | # Control forces and moments 248 | tau = np.array( [thrust[0] + thrust[1], 0, 0, 0, 0, 249 | -self.l1 * thrust[0] - self.l2 * thrust[1] ]) 250 | 251 | # Hydrodynamic linear damping + nonlinear yaw damping 252 | tau_damp = -np.matmul(self.D, nu_r) 253 | tau_damp[5] = tau_damp[5] - 10 * self.D[5,5] * abs(nu_r[5]) * nu_r[5] 254 | 255 | # State derivatives (with dimension) 256 | tau_crossflow = crossFlowDrag(self.L,self.B_pont,self.T,nu_r) 257 | sum_tau = tau + tau_damp + tau_crossflow - np.matmul(C,nu_r) \ 258 | - np.matmul(self.G,eta) - g_0 259 | 260 | nu_dot = np.matmul(self.Minv, sum_tau) # USV dynamics 261 | n_dot = (u_control - n) / self.T_n # propeller dynamics 262 | trim_dot = (self.trim_setpoint - self.trim_moment) / 5 # trim dynamics 263 | 264 | # Forward Euler integration [k+1] 265 | nu = nu + sampleTime * nu_dot 266 | n = n + sampleTime * n_dot 267 | self.trim_moment = self.trim_moment + sampleTime * trim_dot 268 | 269 | u_actual = np.array(n,float) 270 | 271 | return nu, u_actual 272 | 273 | 274 | def controlAllocation(self,tau_X, tau_N): 275 | """ 276 | [n1, n2] = controlAllocation(tau_X, tau_N) 277 | """ 278 | tau = np.array([tau_X, tau_N]) # tau = B * u_alloc 279 | u_alloc = np.matmul(self.Binv, tau) # u_alloc = inv(B) * tau 280 | 281 | # u_alloc = abs(n) * n --> n = sign(u_alloc) * sqrt(u_alloc) 282 | n1 = np.sign(u_alloc[0]) * math.sqrt( abs(u_alloc[0]) ) 283 | n2 = np.sign(u_alloc[1]) * math.sqrt( abs(u_alloc[1]) ) 284 | 285 | return n1, n2 286 | 287 | 288 | def headingAutopilot(self,eta,nu,sampleTime): 289 | """ 290 | u = headingAutopilot(eta,nu,sampleTime) is a PID controller 291 | for automatic heading control based on pole placement. 292 | 293 | tau_N = (T/K) * a_d + (1/K) * rd 294 | - Kp * ( ssa( psi-psi_d ) + Td * (r - r_d) + (1/Ti) * z ) 295 | 296 | """ 297 | psi = eta[5] # yaw angle 298 | r = nu[5] # yaw rate 299 | e_psi = psi - self.psi_d # yaw angle tracking error 300 | e_r = r - self.r_d # yaw rate tracking error 301 | psi_ref = self.ref * math.pi / 180 # yaw angle setpoint 302 | 303 | wn = self.wn # PID natural frequency 304 | zeta = self.zeta # PID natural relative damping factor 305 | wn_d = self.wn_d # reference model natural frequency 306 | zeta_d = self.zeta_d # reference model relative damping factor 307 | 308 | m = 41.4 # moment of inertia in yaw including added mass 309 | T = 1 310 | K = T/m 311 | d = 1/K 312 | k = 0 313 | 314 | # PID feedback controller with 3rd-order reference model 315 | tau_X = self.tauX 316 | 317 | [tau_N, self.e_int, self.psi_d, self.r_d, self.a_d] = \ 318 | PIDpolePlacement( self.e_int, e_psi, e_r, self.psi_d, self.r_d, self.a_d, \ 319 | m, d, k, wn_d, zeta_d, wn, zeta, psi_ref, self.r_max, sampleTime ) 320 | 321 | [n1, n2] = self.controlAllocation(tau_X, tau_N) 322 | u_control = np.array([n1, n2],float) 323 | 324 | return u_control 325 | 326 | def stepInput(self,t): 327 | """ 328 | u = stepInput(t) generates propeller step inputs. 329 | """ 330 | n1 = 100 # rad/s 331 | n2 = 80 332 | 333 | if (t > 30 and t < 100): 334 | n1 = 80 335 | n2 = 120 336 | else: 337 | n1 = 0 338 | n2 = 0 339 | 340 | u_control = np.array([n1, n2],float) 341 | 342 | return u_control 343 | 344 | 345 | 346 | -------------------------------------------------------------------------------- /vehicles/semisub.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | semisub.py 4 | Class for a semisubmersible with mass m = 27 162 500 (mass), pontoon 5 | Height H_p = 13.0 m, pontoon width B_p = 12.0 m, pontoon length 6 | L_p = 84.6 m. 7 | 8 | semisub() 9 | Step inputs for propeller revolutions n1, n2, n3 and n4 10 | semisub('DPcontrol',x_d,y_d,psi_d,V_c,beta_c) DP control system 11 | x_d: desired x position (m) 12 | y_d: desired y position (m) 13 | psi_d: desired yaw angle (deg) 14 | V_c: current speed (m/s) 15 | beta_c: current direction (deg) 16 | 17 | Methods: 18 | 19 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) 20 | returns nu[k+1] and u_actual[k+1] using Euler's method. 21 | The control inputs are: 22 | 23 | u_control = n (RPM) 24 | n = [ #1 Bow tunnel thruster (RPM) 25 | #2 Bow tunnel thruster (RPM) 26 | #3 Aft tunnel thruster (RPM) 27 | #4 Aft tunnel thruster (RPM) 28 | #5 Right poontoon main propeller (RPM) 29 | $6 Left pontoon main propeller (RPM) ] 30 | 31 | u_alloc = controlAllocation(tau) 32 | Control allocation based on the pseudoinverse 33 | 34 | n = DPcontrol(eta,nu,sampleTime) 35 | Nonlinear PID controller for DP based on pole placement. 36 | 37 | n = stepInput(t) generates propellers step inputs. 38 | 39 | Reference: 40 | T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion 41 | Control. 2nd. Edition, Wiley. 42 | URL: www.fossen.biz/wiley 43 | 44 | Author: Thor I. Fossen 45 | """ 46 | import numpy as np 47 | import math 48 | from functions.control import DPpolePlacement 49 | from functions.gnc import sat,ssa 50 | 51 | # Class Vehicle 52 | class semisub: 53 | """ 54 | semisub() Step inputs tau_X, tay_Y, tau_N 55 | semisub('DPcontrol',x_d,y_d,psi_d,V_c,beta_c) DP control system 56 | """ 57 | def __init__(self, controlSystem = 'stepInput', 58 | r_x = 0, r_y = 0, r_n = 0, V_current = 0, beta_current = 0): 59 | 60 | if (controlSystem == 'DPcontrol'): 61 | self.controlDescription = 'Nonlinear DP control (x_d, y_d, psi_d) = (' \ 62 | + str(r_x) + ' m, ' + str(r_y) + ' m, ' + str(r_n) + ' deg)' 63 | 64 | else: 65 | self.controlDescription = 'Step inputs for n = [n1, n2, n3, n4]' 66 | controlSystem = 'stepInput' 67 | 68 | self.ref = np.array([ r_x, r_y, (math.pi/180) * r_n ], float) 69 | self.V_c = V_current 70 | self.beta_c = beta_current 71 | self.controlMode = controlSystem 72 | 73 | # Initialize the semisub model 74 | self.L = 84.6 75 | self.T_n = 1.0 # propeller revolution time constants (s) 76 | self.n_max = np.array([ 77 | 250, 250, 160, 160, 160, 160 ],float) # RPM saturation limits (N) # Length (m) 78 | self.nu = np.array([0, 0, 0, 0, 0, 0], float) # velocity vector 79 | self.u_actual = np.array([0, 0, 0, 0, 0, 0], float) # RPM inputs 80 | self.name = "Semisubmersible" 81 | 82 | self.controls = [ 83 | '#1 Bow tunnel thruster (RPM)', 84 | '#2 Bow tunnel thruster (RPM)', 85 | '#3 Aft tunnel thruster (RPM)', 86 | '#4 Aft tunnel thruster (RPM)', 87 | '#5 Right poontoon main propeller (RPM)', 88 | '$6 Left pontoon main propeller (RPM)' ] 89 | self.dimU = len(self.controls) 90 | 91 | 92 | # Semisub model 93 | MRB = 1.0e+10 * np.array([ 94 | [ 0.0027, 0, 0, 0, -0.0530, 0], 95 | [ 0, 0.0027, 0, 0.0530, 0, -0.0014 ], 96 | [ 0, 0, 0.0027, 0, 0.0014, 0 ], 97 | [ 0, 0.0530, 0, 3.4775, 0, -0.0265 ], 98 | [ -0.0530, 0, 0.0014, 0, 3.8150, 0 ], 99 | [0, -0.0014, 0, -0.0265, 0, 3.7192 ] ], float) 100 | 101 | MA = 1.0e+10 * np.array([ 102 | [ 0.0017, 0, 0, 0, -0.0255, 0 ], 103 | [ 0, 0.0042, 0, 0.0365, 0, 0 ], 104 | [ 0, 0, 0.0021, 0, 0, 0 ], 105 | [ 0, 0.0365, 0, 1.3416, 0, 0 ], 106 | [ -0.0255, 0, 0, 0, 2.2267, 0 ], 107 | [0, 0, 0, 0, 0, 3.2049 ] ], float) 108 | 109 | self.D = 1.0e+09 * np.array([ 110 | [ 0.0004, 0, 0, 0, -0.0085, 0 ], 111 | [ 0, 0.0003, 0, 0.0067, 0, -0.0002 ], 112 | [ 0, 0, 0.0034, 0, 0.0017, 0 ], 113 | [ 0, 0.0067, 0, 4.8841, 0, -0.0034 ], 114 | [ -0.0085, 0, 0.0017, 0, 7.1383, 0 ], 115 | [ 0, -0.0002, 0, -0.0034, 0, 0.8656 ] ], float) 116 | 117 | self.G = 1.0e+10 * np.diag([ 0.0, 0.0, 0.0006, 1.4296, 2.6212, 0.0 ]) 118 | 119 | self.M = MRB + MA 120 | 121 | self.Minv = np.linalg.inv(self.M) 122 | 123 | # Thrust coefficient and configuration matrices (Fossen 2021, Ch. 11.2) 124 | K = np.diag( [ 3.5, 3.5, 25.0, 25.0, 25.0, 25.0 ]) 125 | T = np.array([ 126 | [0, 0, 0, 0, 1, 1], 127 | [1, 1, 1, 1, 0, 0], 128 | [30, 20, -20, -30, -self.L/2, self.L/2] ],float) 129 | self.B = T @ K 130 | 131 | # DP control system 132 | self.e_int = np.array([ 0, 0, 0 ],float) # integral states 133 | self.x_d = 0.0 # setpoints 134 | self.y_d = 0.0 135 | self.psi_d = 0.0 136 | self.wn = np.diag([ 0.2, 0.2, 0.1 ]) # PID pole placement 137 | self.zeta = np.diag([ 1.0, 1.0, 1.0 ]) 138 | 139 | 140 | def dynamics(self,eta,nu,u_actual,u_control,sampleTime): 141 | """ 142 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates the 143 | semisub equations of motion using Euler's method. 144 | """ 145 | 146 | # Input vector 147 | n = u_actual 148 | 149 | # Current velocities 150 | u_c = self.V_c * math.cos(self.beta_c - eta[5]) # current surge velocity 151 | v_c = self.V_c * math.sin(self.beta_c - eta[5]) # current sway velocity 152 | 153 | nu_c = np.array([u_c,v_c,0,0,0,0],float) # current velocity vector 154 | nu_r = nu - nu_c # relative velocity vector 155 | 156 | # Control forces and moments with propeller saturation 157 | n_squared = np.zeros(self.dimU) 158 | for i in range(0,self.dimU): 159 | n[i] = sat(n[i],-self.n_max[i],self.n_max[i]) # saturation limits 160 | n_squared[i] = abs( n[i] ) * n[i] 161 | 162 | tau3 = np.matmul(self.B, n_squared) 163 | tau = np.array([ tau3[0], tau3[1], 0, 0, 0, tau3[2]],float) 164 | 165 | # 6-DOF semisub model 166 | nu_dot = np.matmul( self.Minv, tau 167 | - np.matmul(self.D, nu_r) - np.matmul(self.G, eta)) 168 | n_dot = (u_control - u_actual) / self.T_n 169 | 170 | # Forward Euler integration 171 | nu = nu + sampleTime * nu_dot 172 | n = n + sampleTime * n_dot 173 | 174 | u_actual = np.array(n,float) 175 | 176 | return nu, u_actual 177 | 178 | 179 | def controlAllocation(self,tau3): 180 | """ 181 | u_alloc = controlAllocation(tau3), tau3 = [tau_X, tau_Y, tau_N]' 182 | u_alloc = B' * inv( B * B' ) * tau3 183 | """ 184 | B_pseudoInv = self.B.T @ np.linalg.inv( self.B @ self.B.T ) 185 | u_alloc = np.matmul(B_pseudoInv, tau3) 186 | 187 | return u_alloc 188 | 189 | 190 | def DPcontrol(self,eta,nu,sampleTime): 191 | """ 192 | u = DPcontrol(eta,nu,sampleTime) is a nonlinear PID controller 193 | for DP based on pole placement: 194 | 195 | tau = -R' Kp (eta-r) - R' Kd R nu - R' Ki int(eta-r) 196 | u = B_pseudoinverse * tau 197 | """ 198 | # 3-DOF state vectors 199 | eta3 = np.array([ eta[0], eta[1], eta[5] ]) 200 | nu3 = np.array([ nu[0], nu[1], nu[5] ]) 201 | 202 | # 3-DOF diagonal model matrices 203 | M3 = np.diag([ self.M[0][0], self.M[1][1], self.M[5][5] ]) 204 | D3 = np.diag([ self.D[0][0], self.D[1][1], self.D[5][5] ]) 205 | 206 | [tau3,self.e_int,self.x_d,self.y_d,self.psi_d] = DPpolePlacement( 207 | self.e_int,M3,D3,eta3,nu3,self.x_d,self.y_d,self.psi_d, 208 | self.wn,self.zeta,self.ref,sampleTime) 209 | 210 | u_alloc = self.controlAllocation(tau3) 211 | 212 | # u_alloc = abs(n) * n --> n = sign(u_alloc) * sqrt(u_alloc) 213 | n = np.zeros(self.dimU) 214 | for i in range(0, self.dimU): 215 | n[i] = np.sign(u_alloc[i]) * math.sqrt( abs(u_alloc[i]) ) 216 | 217 | u_control = n 218 | 219 | return u_control 220 | 221 | 222 | def stepInput(self,t): 223 | """ 224 | u = stepInput(t) generates force step inputs. 225 | """ 226 | n = np.array([0, 0, 100, 100],float) 227 | 228 | if t > 30: 229 | n = np.array([50, 50, 50, 50],float) 230 | if t > 70: 231 | n = np.array([0, 0, 0, 0],float) 232 | 233 | u_control = n 234 | 235 | return u_control 236 | 237 | 238 | 239 | -------------------------------------------------------------------------------- /vehicles/shipClarke83.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | shipClarke83.py: 4 | 5 | Class for a generic ship parametrized using the main dimensions L, B, and T. 6 | The ship model is based on the linear maneuvering coefficients by 7 | Clarke (1983). 8 | 9 | shipClarke83() 10 | Step input, rudder angle 11 | shipClarke83('headingAutopilot',psi_d,L,B,T,Cb,V_current,beta_c,tau_X) 12 | psi_d: desired yaw angle (deg) 13 | L: ship length (m) 14 | B: ship beam (m) 15 | T: ship draft (m) 16 | Cb: block coefficient (-) 17 | V_current: current speed (m/s) 18 | beta_c: current direction (deg) 19 | tau_X: surge force, pilot input (N) 20 | 21 | Methods: 22 | 23 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime ) returns 24 | nu[k+1] and u_actual[k+1] using Euler's method. The control input is: 25 | 26 | u_control = delta_r (rad) is for the ship rudder. 27 | 28 | u = headingAutopilot(eta,nu,sampleTime) 29 | PID controller for automatic heading control based on pole placement. 30 | 31 | u = stepInput(t) generates rudder step inputs. 32 | 33 | References: 34 | D. Clarke, P. Gedling and G. Hine (1983). The Application of Manoeuvring 35 | Criteria in Hull Design using Linear Theory. Transactions of the Royal 36 | Institution of Naval Architects (RINA), 125, pp. 45-68. 37 | T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion 38 | Control. 2nd. Edition, Wiley. 39 | URL: www.fossen.biz/wiley 40 | 41 | Author: Thor I. Fossen 42 | """ 43 | import numpy as np 44 | import math 45 | from functions.control import PIDpolePlacement 46 | from functions.models import clarke83 47 | 48 | # Class Vehicle 49 | class shipClarke83: 50 | """ 51 | shipClarke83() 52 | Rudder angle step inputs 53 | shipClarke83('headingAutopilot', psi_d, L, B, T, Cb, V_c, beta_c, tau_X) 54 | Heading autopilot 55 | """ 56 | def __init__(self, controlSystem = 'stepInput', r = 0, L = 50.0, B = 7.0, 57 | T = 5.0, Cb = 0.7, V_current = 0, beta_current = 0,tau_X = 1e5): 58 | 59 | if (controlSystem == 'headingAutopilot'): 60 | self.controlDescription = 'Heading autopilot, psi_d = ' \ 61 | + str(r) + ' (deg)' 62 | 63 | else: 64 | self.controlDescription = "Step input for delta_r" 65 | controlSystem = 'stepInput' 66 | 67 | self.ref = r 68 | self.V_c = V_current 69 | self.beta_c = beta_current 70 | self.controlMode = controlSystem 71 | 72 | # Initialize the ship model 73 | self.name = "Clarke (1983) linear ship model" 74 | self.L = L # length (m) 75 | self.B = B # beam (m) 76 | self.T = T # Draft (m) 77 | self.Cb = Cb # block coefficient 78 | self.rho = 1025 # density of water (kg/m^3) 79 | self.Lambda = 0.7 # rudder aspect ratio: Lambda = b**2 / AR 80 | self.tau_X = tau_X # surge force (N), pilot input 81 | self.deltaMax = 30 # max rudder angle (deg) 82 | self.T_delta = 1.0 # rudder time constants (s) 83 | self.nu = np.array([2, 0, 0, 0, 0, 0], float ) # velocity vector 84 | self.u_actual = np.array([0],float) # control input vector 85 | 86 | if self.L > 100: 87 | self.R66 = 0.27 * self.L # approx. radius of gyration in yaw (m) 88 | else: 89 | self.R66 = 0.25 * self.L 90 | 91 | self.controls = ['Rudder angle (deg)'] 92 | self.dimU = len(self.controls) 93 | 94 | # Heading autopilot 95 | self.e_int = 0 # integral state 96 | self.wn = 0.5 # PID pole placement 97 | self.zeta = 1 98 | 99 | # Reference model 100 | self.r_max = 1.0 * math.pi / 180 # maximum yaw rate 101 | self.psi_d = 0 # angle, angular rate and angular acc. states 102 | self.r_d = 0 103 | self.a_d = 0 104 | self.wn_d = self.wn / 5 # desired natural frequency in yaw 105 | self.zeta_d = 1 # desired relative damping ratio in yaw 106 | 107 | # controller parameters m, d and k 108 | U0 = 3 # cruise speed 109 | [M,N] = clarke83(U0,self.L, self.B, self.T,self.Cb,self.R66,0,self.L) 110 | self.m_PID = M[2][2] 111 | self.d_PID = N[2][2] 112 | self.k_PID = 0 113 | 114 | # Rudder yaw moment coefficient (Fossen 2021, Chapter 9.5.1) 115 | b = 0.7 * self.T # rudder height 116 | AR = b**2 / self.Lambda # aspect ratio: Lamdba = b**2/AR 117 | CN = 6.13 * self.Lambda / ( self.Lambda + 2.25 ) # normal coefficient 118 | t_R = 1 - 0.28 * self.Cb - 0.55 119 | a_H = 0.4 120 | x_R = -0.45 * self.L 121 | x_H = -1.0 * self.L 122 | 123 | # tau_N = Yd * delta 124 | self.Nd = -0.25 * ( x_R + a_H * x_H ) * self.rho * U0**2 * AR * CN 125 | 126 | 127 | def dynamics(self,eta,nu,u_actual,u_control,sampleTime): 128 | """ 129 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates 130 | the ship equations of motion using Euler's method. 131 | """ 132 | 133 | # Current velocities 134 | u_c = self.V_c * math.cos(self.beta_c - eta[5]) # current surge velocity 135 | v_c = self.V_c * math.sin(self.beta_c - eta[5]) # current sway velocity 136 | 137 | nu_c = np.array([u_c,v_c,0,0,0,0],float) # current velocity vector 138 | nu_r = nu - nu_c # relative velocity vector 139 | 140 | U_r = math.sqrt( nu_r[0]**2 + nu_r[1]**2 ) # relative speed 141 | 142 | # Rudder command and actual rudder angle 143 | delta_c = u_control[0] 144 | delta = u_actual[0] 145 | 146 | # Rudder forces and moment (Fossen 2021, Chapter 9.5.1) 147 | b = 0.7 * self.T # rudder height 148 | AR = b**2 / self.Lambda # aspect ratio: Lamdba = b**2/AR 149 | CN = 6.13 * self.Lambda / ( self.Lambda + 2.25 ) # normal coefficient 150 | t_R = 1 - 0.28 * self.Cb - 0.55 151 | a_H = 0.4 152 | x_R = -0.45 * self.L 153 | x_H = -1.0 * self.L 154 | 155 | Xdd = -0.5 * ( 1 - t_R ) * self.rho * U_r**2 * AR * CN 156 | Yd = -0.25 * ( 1 + a_H ) * self.rho * U_r**2 * AR * CN 157 | Nd = -0.25 * ( x_R + a_H * x_H ) * self.rho * U_r**2 * AR * CN 158 | 159 | # Control forces and moment 160 | delta_R = -delta # physical rudder angle (rad) 161 | T = self.tau_X # thrust (N) 162 | t_deduction = 0.1 # thrust deduction number 163 | tau1 = ( 1 - t_deduction ) * T - Xdd * math.sin( delta_R )**2 164 | tau2 = -Yd * math.sin( 2 * delta_R ) 165 | tau6 = -Nd * math.sin( 2 * delta_R ) 166 | tau = np.array( [ tau1, tau2, tau6 ],float) 167 | 168 | # Linear maneuvering model 169 | T_surge = self.L # approx. time constant in surge (s) 170 | xg = 0 # approx. x-coordinate, CG (m) 171 | 172 | # 3-DOF ship model 173 | [M,N] = clarke83(U_r,self.L, self.B, self.T,self.Cb,self.R66,xg,T_surge) 174 | Minv = np.linalg.inv(M) 175 | nu3 = np.array( [ nu_r[0], nu_r[1], nu_r[5] ]) 176 | nu3_dot = np.matmul( Minv, tau - np.matmul(N,nu3) ) 177 | 178 | # 6-DOF ship model 179 | nu_dot = np.array( [ nu3_dot[0],nu3_dot[1],0,0,0,nu3_dot[2] ]) 180 | 181 | # Rudder angle saturation 182 | if ( abs(delta) >= self.deltaMax * math.pi / 180 ): 183 | delta = np.sign(delta) * self.deltaMax * math.pi / 180 184 | 185 | # Rudder dynamics 186 | delta_dot = (delta_c - delta) / self.T_delta 187 | 188 | # Forward Euler integration [k+1] 189 | nu = nu + sampleTime * nu_dot 190 | delta = delta + sampleTime * delta_dot 191 | 192 | u_actual = np.array([delta],float) 193 | 194 | return nu, u_actual 195 | 196 | 197 | def stepInput(self,t): 198 | """ 199 | delta_c = stepInput(t) generates rudder step inputs. 200 | """ 201 | delta_c = 10 * (math.pi/180) 202 | if t > 50: 203 | delta_c = 0 204 | 205 | u_control = np.array([delta_c],float) 206 | 207 | return u_control 208 | 209 | 210 | def headingAutopilot(self,eta,nu,sampleTime): 211 | """ 212 | delta_c = headingAutopilot(eta,nu,sampleTime) is a PID controller 213 | for automatic heading control based on pole placement. 214 | 215 | tau_N = m * a_d + d * r_d 216 | - Kp * ( ssa( psi-psi_d ) + Td * (r - r_d) + (1/Ti) * e_int ) 217 | """ 218 | psi = eta[5] # yaw angle 219 | r = nu[5] # yaw rate 220 | e_psi = psi - self.psi_d # yaw angle tracking error 221 | e_r = r - self.r_d # yaw rate tracking error 222 | psi_ref = self.ref * math.pi / 180 # yaw angle setpoint 223 | 224 | wn = self.wn # PID natural frequency 225 | zeta = self.zeta # PID natural relative damping factor 226 | wn_d = self.wn_d # reference model natural frequency 227 | zeta_d = self.zeta_d # reference model relative damping factor 228 | 229 | m = self.m_PID 230 | d = self.d_PID 231 | k = 0 232 | 233 | # PID feedback controller with 3rd-order reference model 234 | [tau_N, self.e_int, self.psi_d, self.r_d, self.a_d] = PIDpolePlacement(\ 235 | self.e_int, e_psi, e_r,self.psi_d, self.r_d, \ 236 | self.a_d, m, d, k, wn_d, zeta_d, wn, zeta, psi_ref, self.r_max, \ 237 | sampleTime ) 238 | 239 | # Control allocation: tau_N = Yd * delta 240 | delta_c = tau_N / self.Nd # rudder command 241 | 242 | u_control = np.array([delta_c],float) 243 | 244 | return u_control 245 | 246 | 247 | 248 | -------------------------------------------------------------------------------- /vehicles/supply.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | supply.py: 4 | Class for an offshore supply vessel length L = 76.2. 5 | The constructors are: 6 | 7 | supply() 8 | Step inputs for propeller revolutions n1, n2, n3 and n4 9 | supply('DPcontrol',x_d,y_d,psi_d,V_c,beta_c) DP control system 10 | x_d: desired x position (m) 11 | y_d: desired y position (m) 12 | psi_d: desired yaw angle (deg) 13 | V_c: current speed (m/s) 14 | beta_c: current direction (deg) 15 | 16 | Methods: 17 | 18 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) 19 | returns nu[k+1] and u_actual[k+1] using Euler's method. 20 | The control inputs are: 21 | 22 | u_control = n (RPM) 23 | n = [ #1 Bow thruster (RPM) 24 | #2 Bow thruster (RPM) 25 | #3 Right main propeller (RPM) 26 | #4 Left main propeller (RPM) ] 27 | 28 | u_alloc = controlAllocation(tau) 29 | Control allocation based on the pseudoinverse 30 | 31 | n = DPcontrol(eta,nu,sampleTime) 32 | Nonlinear PID controller for DP based on pole placement. 33 | 34 | n = stepInput(t) generates propellers step inputs. 35 | 36 | References: 37 | T. I. Fossen, S. I. Sagatun and A. J. Sorensen (1996) 38 | Identification of Dynamically Positioned Ships 39 | Journal of Control Engineering Practice CEP-4(3):369-376 40 | T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion 41 | Control. 2nd. Edition, Wiley. 42 | URL: www.fossen.biz/wiley 43 | 44 | Author: Thor I. Fossen 45 | """ 46 | import numpy as np 47 | import math 48 | from functions.control import DPpolePlacement 49 | from functions.gnc import sat,ssa 50 | 51 | # Class Vehicle 52 | class supply: 53 | """ 54 | supply() Step inputs n1,n2,n3 and n4 55 | supply('DPcontrol',x_d,y_d,psi_d,V_c,beta_c) DP control system 56 | """ 57 | def __init__(self, controlSystem = 'stepInput', 58 | r_x = 0, r_y = 0, r_n = 0, V_current = 0, beta_current = 0): 59 | 60 | if (controlSystem == 'DPcontrol'): 61 | self.controlDescription = 'Nonlinear DP control (x_d, y_d, psi_d) = (' \ 62 | + str(r_x) + ' m, ' + str(r_y) + ' m, ' + str(r_n) + ' deg)' 63 | 64 | else: 65 | self.controlDescription = 'Step inputs for n = [n1, n2, n3, n4]' 66 | controlSystem = 'stepInput' 67 | 68 | self.ref = np.array([ r_x, r_y, (math.pi/180) * r_n ], float) 69 | self.V_c = V_current 70 | self.beta_c = beta_current 71 | self.controlMode = controlSystem 72 | 73 | # Initialize the supply vessel model 74 | self.L = 76.2 # Length (m) 75 | self.T_n = 1.0 # prop. rev. time constant (s) 76 | self.n_max = np.array([ # RPM saturation limits (N) 77 | 250, 250, 160, 160 ],float) 78 | self.nu = np.array([0, 0, 0, 0, 0, 0], float) # velocity vector 79 | self.u_actual = np.array([0, 0, 0, 0], float) # RPM inputs 80 | self.name = 'Offshore supply vessel' 81 | 82 | # Two tunnel thrusters in the bow, no. 1 and 2 83 | # Two main propellers aft, no. 3 and 4 84 | self.controls = [ 85 | '#1 Bow thruster (RPM)', 86 | '#2 Bow thruster (RPM)', 87 | '#3 Right main propeller (RPM)', 88 | '#4 Left main propeller (RPM)' ] 89 | self.dimU = len(self.controls) 90 | 91 | # Constants 92 | g = 9.81 # acceleration of gravity (m/s^2) 93 | rho = 1025 # density of water 94 | m = 6000.0e3 # mass (kg) 95 | 96 | # Thrust coefficient and configuration matrices (Fossen 2021, Ch. 11.2) 97 | K = np.diag( [ 2.4, 2.4, 17.6, 17.6 ]) 98 | T = np.array([ 99 | [0, 0, 1, 1], 100 | [1, 1, 0, 0], 101 | [30, 22, -self.L/2, self.L/2] ],float) 102 | self.B = T @ K 103 | 104 | # Tbis = np.diag( [1, 1, 1 / self.L],float) 105 | Tbis_inv = np.diag( [1.0, 1.0, self.L]) 106 | 107 | # 3-DOF model matrices - bis scaling (Fossen 2021, App. D) 108 | Mbis = np.array([ 109 | [ 1.1274, 0, 0], 110 | [ 0, 1.8902, -0.0744], 111 | [ 0, -0.0744, 0.1278] ], float) 112 | 113 | Dbis = np.array([ 114 | [ 0.0358, 0, 0 ], 115 | [ 0, 0.1183, -0.0124 ], 116 | [ 0, -0.0041, 0.0308 ] ], float) 117 | 118 | self.M3 = m * Tbis_inv @ Mbis @ Tbis_inv 119 | self.M3inv = np.linalg.inv(self.M3) 120 | self.D3 = m * math.sqrt( g / self.L ) * Tbis_inv @ Dbis @ Tbis_inv 121 | 122 | # DP control system 123 | self.e_int = np.array([ 0, 0, 0 ],float) # integral states 124 | self.x_d = 0.0 # setpoints 125 | self.y_d = 0.0 126 | self.psi_d = 0.0 127 | self.wn = np.diag([ 0.3, 0.3, 0.1 ]) # PID pole placement 128 | self.zeta = np.diag([ 1.0, 1.0, 1.0 ]) 129 | 130 | 131 | def dynamics(self,eta,nu,u_actual,u_control,sampleTime): 132 | """ 133 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates the 134 | supply vessel equations of motion using Euler's method. 135 | """ 136 | 137 | # Input vector 138 | n = u_actual 139 | 140 | # Current velocities 141 | u_c = self.V_c * math.cos(self.beta_c - eta[5]) # current surge velocity 142 | v_c = self.V_c * math.sin(self.beta_c - eta[5]) # current sway velocity 143 | 144 | nu_c = np.array([u_c,v_c,0,0,0,0],float) # current velocity vector 145 | nu_r = nu - nu_c # relative velocity vector 146 | 147 | # Control forces and moments with propeller saturation 148 | n_squared = np.zeros(self.dimU) 149 | for i in range(0,self.dimU): 150 | n[i] = sat(n[i],-self.n_max[i],self.n_max[i]) # saturation, physical limits 151 | n_squared[i] = abs( n[i] ) * n[i] 152 | 153 | tau3 = np.matmul(self.B, n_squared) 154 | 155 | # 3-DOF dynamics 156 | nu3_r = np.array([ nu_r[0], nu_r[1], nu_r[5] ]) 157 | nu3_dot = np.matmul( self.M3inv, tau3 - np.matmul(self.D3, nu3_r) ) 158 | 159 | # 6-DOF ship model 160 | nu_dot = np.array([ nu3_dot[0],nu3_dot[1],0,0,0,nu3_dot[2] ]) 161 | n_dot = (u_control - u_actual) / self.T_n 162 | 163 | # Forward Euler integration 164 | nu = nu + sampleTime * nu_dot 165 | n = n + sampleTime * n_dot 166 | 167 | u_actual = np.array(n,float) 168 | 169 | return nu, u_actual 170 | 171 | 172 | def controlAllocation(self,tau3): 173 | """ 174 | u_alloc = controlAllocation(tau3), tau3 = [tau_X, tau_Y, tau_N]' 175 | u_alloc = B' * inv( B * B' ) * tau3 176 | """ 177 | B_pseudoInv = self.B.T @ np.linalg.inv( self.B @ self.B.T ) 178 | u_alloc = np.matmul(B_pseudoInv, tau3) 179 | 180 | return u_alloc 181 | 182 | 183 | def DPcontrol(self,eta,nu,sampleTime): 184 | """ 185 | u = DPcontrol(eta,nu,sampleTime) is a nonlinear PID controller 186 | for DP based on pole placement: 187 | 188 | tau = -R' Kp (eta-r) - R' Kd R nu - R' Ki int(eta-r) 189 | u = B_pseudoinverse * tau 190 | """ 191 | eta3 = np.array([ eta[0], eta[1], eta[5] ]) 192 | nu3 = np.array([ nu[0], nu[1], nu[5] ]) 193 | 194 | [tau3,self.e_int,self.x_d,self.y_d,self.psi_d] = DPpolePlacement( \ 195 | self.e_int,self.M3,self.D3,eta3,nu3, \ 196 | self.x_d,self.y_d,self.psi_d, \ 197 | self.wn,self.zeta,self.ref,sampleTime) 198 | 199 | u_alloc = self.controlAllocation(tau3) 200 | 201 | # u_alloc = abs(n) * n --> n = sign(u_alloc) * sqrt(u_alloc) 202 | n = np.zeros(self.dimU) 203 | for i in range(0, self.dimU): 204 | n[i] = np.sign(u_alloc[i]) * math.sqrt( abs(u_alloc[i]) ) 205 | 206 | u_control = n 207 | 208 | return u_control 209 | 210 | 211 | def stepInput(self,t): 212 | """ 213 | u = stepInput(t) generates propeller step inputs (RPM). 214 | """ 215 | n = np.array([0, 0, 100, 100],float) 216 | 217 | if t > 30: 218 | n = np.array([50, 50, 50, 50],float) 219 | if t > 70: 220 | n = np.array([0, 0, 0, 0],float) 221 | 222 | u_control = n 223 | 224 | return u_control 225 | 226 | 227 | 228 | -------------------------------------------------------------------------------- /vehicles/swift.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sun Dec 5 03:55:21 2021 4 | 5 | @author: mohamedd 6 | """ 7 | """ 8 | swift.py 9 | Class for underwater vehicle swift 10 | 11 | swift() 12 | Step inputs for propeller revolutions n1, n2, n3, n4, n5, n6, n7 and n8 (RPM) 13 | swift('DPcontrol',x_d,y_d,psi_d,V_c,beta_c) DP control system 14 | x_d: desired x position (m) 15 | y_d: desired y position (m) 16 | psi_d: desired yaw angle (deg) 17 | V_c: current speed (m/s) 18 | beta_c: current direction (deg) 19 | 20 | Methods: 21 | 22 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) 23 | returns nu[k+1] and u_actual[k+1] using Euler's method. 24 | The control inputs are: 25 | 26 | u_control = n (RPM) 27 | 28 | Indicies of the vertical thrusters are as follows: 29 | 30 | | +x-axis 31 | | 32 | n2 o | o n1 33 | | 34 | + --------------------> 35 | +y-axis 36 | n4 o o n3 37 | 38 | Indicies of the horizontal thrusters are as follows: 39 | 40 | | +x-axis 41 | | 42 | n6 o | o n5 43 | | 44 | + --------------------> 45 | +y-axis 46 | n8 o o n7 47 | 48 | 49 | 50 | n = [ #1 vertical thruster (RPM) 51 | #2 vertical thruster (RPM) 52 | #3 vertical thruster (RPM) 53 | #4 vertical thruster (RPM) 54 | #5 horizontal thruster (RPM) 55 | #6 horizontal thruster (RPM) 56 | #7 horizontal thruster (RPM) 57 | #8 horizontal thruster (RPM) 58 | 59 | u_alloc = controlAllocation(tau) 60 | Control allocation based on the pseudoinverse 61 | 62 | n = DPcontrol(eta,nu,sampleTime) 63 | Nonlinear PID controller for DP based on pole placement. 64 | 65 | n = stepInput(t) generates propellers step inputs. 66 | 67 | Reference: 68 | T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion 69 | Control. 2nd. Edition, Wiley. 70 | URL: www.fossen.biz/wiley 71 | 72 | Author: Thor I. Fossen 73 | """ 74 | import numpy as np 75 | import math 76 | from functions.control import DPpolePlacement6, LQR 77 | from functions.gnc import sat, m2c, gvect, Dmtx 78 | 79 | # Class Vehicle 80 | class swift: 81 | """ 82 | swift() Step inputs tau_X, tay_Y, tau_N 83 | swift('DPcontrol',x_d,y_d,z_d,phi_d,theta_d,psi_d,V_c,beta_c) DP control system 84 | """ 85 | def __init__(self, controlSystem = 'stepInput', 86 | r_x = 0, r_y = 0, r_z = 0, r_roll = 0, r_pitch = 0, r_yaw = 0, V_current = 0, beta_current = 0): 87 | 88 | if (controlSystem == 'DPcontrol1'): 89 | self.controlDescription = 'Nonlinear PID DP control (x_d, y_d,zd, phi_d, theta_d psi_d) = (' \ 90 | + str(r_x) + ' m, ' + str(r_y) + ' m, ' + str(r_z) + ' m, ' \ 91 | + str(r_roll) + ' deg, ' + str(r_pitch) + ' deg, ' + str(r_yaw) + ' deg)' 92 | 93 | elif (controlSystem == 'DPcontrol2'): 94 | self.controlDescription = 'Optimal DP control (x_d, y_d,zd, phi_d, theta_d psi_d) = (' \ 95 | + str(r_x) + ' m, ' + str(r_y) + ' m, ' + str(r_z) + ' m, ' \ 96 | + str(r_roll) + ' deg, ' + str(r_pitch) + ' deg, ' + str(r_yaw) + ' deg)' 97 | 98 | else: 99 | self.controlDescription = 'Step inputs for n = [n1, n2, n3, n4, n5, n6, n7, n8]' 100 | controlSystem = 'stepInput' 101 | 102 | self.ref = np.array([ r_x, r_y, r_z, (math.pi/180) * r_roll, (math.pi/180) * r_pitch, (math.pi/180) * r_yaw], float) 103 | self.V_c = V_current 104 | self.beta_c = beta_current 105 | self.controlMode = controlSystem 106 | 107 | # Initialize the semisub model 108 | self.L = 0.75 # Length (m) 109 | self.volume = 0.0364 110 | self.mass = 35.5 111 | self.T_n = 0.2 # propeller revolution time constants 112 | self.n_max = np.array([ 113 | 3000, 3000, 3000, 3000, 3000, 3000, 3000, 3000],float) # RPM saturation limits (N) 114 | self.nu = np.array([0, 0, 0, 0, 0, 0], float) # velocity vector 115 | self.u_actual = np.array([0, 0, 0, 0, 0, 0, 0, 0], float) # RPM inputs 116 | self.name = "swift" 117 | 118 | self.controls = [ 119 | '#1 Vertical thruster (RPM)', 120 | '#2 Vertical thruster (RPM)', 121 | '#3 Vertical thruster (RPM)', 122 | '#4 Vertical thruster (RPM)', 123 | '#5 Horizontal thruster (RPM)', 124 | '#6 Horizontal thruster (RPM)', 125 | '#7 Horizontal thruster (RPM)', 126 | '#8 Horizontal thruster (RPM)'] 127 | self.dimU = len(self.controls) 128 | 129 | # Swift model 130 | # Rigid-body mass matrix MRB 131 | # MRB = [Mass*I3 0 132 | # 0 Ib] 133 | # where Ib is the inertia matrix 3x3 134 | MRB = np.array([ 135 | [ 35.5, 0, 0, 0, 0, 0], 136 | [ 0, 35.5, 0, 0, 0, 0], 137 | [ 0, 0, 35.5, 0, 0, 0], 138 | [ 0, 0, 0, 0.8061, -0.0059, 0.0005], 139 | [ 0, 0, 0, -0.0059, 0.8, -0.0113], 140 | [ 0, 0, 0, 0.0005, -0.0113, 1.5599 ] ], float) 141 | 142 | # Added-mass matrix MA 143 | MA = np.diag([13, 30, 45, 12.4, 12.2, 5.5]) 144 | # Linear damping matrix 145 | self.Dlin = np.diag([-22.8, -30.95, -50.26, -16.05, -16.73, -5.13]) 146 | # Quadratic damping matrix 147 | self.Dquad = np.diag([-28.43, -55.98, -137.5, -0.0, -0.0, -0.00]) 148 | # Total damping matrix 149 | self.D = Dmtx(self.Dlin, self.Dquad, self.nu) 150 | # r_bg = [x_g y_g z_g]: location of the CG with respect to the CO 151 | self.r_bg = np.array([0,0,0], float) 152 | # r_bb = [x_b y_b z_b]: location of the CB with respect to th CO 153 | self.r_bb = np.array([0,0,-0.12], float) 154 | # Weight force 155 | self.Weight = self.mass * 9.806 156 | # Buoyancy force 157 | self.Buoyancy = self.volume * 1000 * 9.806 158 | 159 | # System-inertia matrix 160 | self.M = MRB + MA 161 | 162 | self.Minv = np.linalg.inv(self.M) 163 | 164 | # Thrust configuration matrix for swift AUV 165 | self.T = np.array([ 166 | [0.00000, 0.00000, 0.00000, 0.00000, 0.70710, 0.70710,-0.7071,-0.7071], 167 | [0.00000, 0.00000, 0.00000, 0.00000, 0.70710,-0.70710,-0.7071, 0.7071], 168 | [1.00000, 1.00000, 1.00000, 1.00000, 0.00000, 0.00000, 0.0000, 0.0000], 169 | [0.19279,-0.18721, 0.19279,-0.18721,-0.00000, 0.00000,-0.0000, 0.0000], 170 | [0.19936, 0.19858,-0.26364,-0.26442,-0.00000,-0.00000, 0.0000, 0.0000], 171 | [0.00000,-0.00000, 0.00000,-0.00000, 0.29320,-0.28925,-0.3993, 0.39541]], float) 172 | # Thrust coefficient matrix 173 | self.K = 0.000004 * np.identity(8, float) 174 | self.B = self.T @ self.K 175 | 176 | # DP control system 177 | self.e_int = np.array([0, 0, 0, 0, 0, 0],float) # integral states 178 | self.eta_d = np.zeros(6) 179 | self.nu_d = np.zeros(6) 180 | self.wn = np.diag([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) # PID pole placement 181 | self.zeta = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) 182 | self.Q = np.eye(12) # LQR 183 | self.R = 0.001 * np.eye(6) 184 | 185 | 186 | def dynamics(self,eta,nu,u_actual,u_control,sampleTime): 187 | """ 188 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates the 189 | swift equations of motion using Euler's method. 190 | """ 191 | 192 | # Input vector 193 | n = u_actual 194 | 195 | # Current velocities 196 | u_c = self.V_c * math.cos(self.beta_c - eta[5]) # current surge velocity 197 | v_c = self.V_c * math.sin(self.beta_c - eta[5]) # current sway velocity 198 | 199 | nu_c = np.array([u_c,v_c,0,0,0,0],float) # current velocity vector 200 | nu_r = nu - nu_c # relative velocity vector 201 | 202 | # Control forces and moments with propeller saturation 203 | n_squared = np.zeros(self.dimU) 204 | for i in range(0,self.dimU): 205 | n[i] = sat(n[i],-self.n_max[i],self.n_max[i]) # saturation limits 206 | n_squared[i] = abs( n[i] ) * n[i] 207 | 208 | tau = np.matmul(self.B, n_squared) 209 | # Total damping matrix 210 | self.D = Dmtx(self.Dlin, self.Dquad, nu_r) 211 | # Coriolis and centripetal matrix C 212 | self.C = m2c(self.M, nu_r) 213 | # Restoring forces and moments vector 214 | self.g = gvect(self.Weight, self.Weight, eta[4], eta[3], self.r_bg, self.r_bb) 215 | tau_resistive = - np.matmul(self.D, nu_r) - np.matmul(self.C, nu_r) - self.g 216 | # 6-DOF AUV model 217 | nu_dot = np.matmul(self.Minv, tau + tau_resistive) 218 | n_dot = (u_control - u_actual) / self.T_n 219 | 220 | # Forward Euler integration 221 | nu = nu + sampleTime * nu_dot 222 | n = n + sampleTime * n_dot 223 | 224 | u_actual = np.array(n,float) 225 | 226 | return nu, u_actual 227 | 228 | 229 | def controlAllocation(self,tau6): 230 | """ 231 | u_alloc = controlAllocation(tau6), tau6 = [tau_X, tau_Y, tau_Z, tau_Roll, tau_Pitch, tau_Yaw]' 232 | u_alloc = B' * inv( B * B' ) * tau6 233 | """ 234 | # Moore-Penrose pseudo-inverse 235 | B_pseudoInv = self.B.T @ np.linalg.inv( self.B @ self.B.T ) 236 | # Eight thrusters [N] 237 | u_alloc = np.matmul(B_pseudoInv, tau6) 238 | 239 | return u_alloc 240 | 241 | 242 | def DPcontrol(self,eta,nu,sampleTime): 243 | """ 244 | u = DPcontrol(eta,nu,sampleTime) is the dynamics positioning method: 245 | """ 246 | if (self.controlMode == 'DPcontrol1') : 247 | # MIMO nonlinear PID controller for DP based on pole placement 248 | # tau = -R' Kp (eta-r) - R' Kd R nu - R' Ki int(eta-r) 249 | [tau,self.e_int,self.eta_d] = DPpolePlacement6( 250 | self.e_int,self.M,self.D,eta,nu,self.eta_d, 251 | self.wn,self.zeta,self.ref,sampleTime) 252 | 253 | elif (self.controlMode == 'DPcontrol2'): 254 | # Linear Quadratic Regulator for optimal DP 255 | # tau = K e, where K is the optimal feedback gain 256 | # e is the error state vector 257 | [tau,self.eta_d] = LQR(self.M,self.Dlin,self.Dquad,self.r_bb,self.r_bg, 258 | self.Weight,eta,nu,self.eta_d,self.ref,self.nu_d,sampleTime,self.wn, 259 | self.Q,self.R) 260 | 261 | # From generlized forces tau to each thruster squared speed (RPM^2) 262 | u_alloc = self.controlAllocation(tau) 263 | # u_alloc = abs(n) * n --> n = sign(u_alloc) * sqrt(u_alloc) 264 | n = np.zeros(self.dimU) 265 | for i in range(0, self.dimU): 266 | n[i] = np.sign(u_alloc[i]) * math.sqrt( abs(u_alloc[i]) ) 267 | # Commanded 8 propeller speeds (RPM) 268 | u_control = n 269 | 270 | return u_control 271 | 272 | 273 | def stepInput(self,t): 274 | """ 275 | u = stepInput(t) generates force step inputs. 276 | """ 277 | n = np.array([0, 0, 100, 100],float) 278 | 279 | if t > 30: 280 | n = np.array([50, 50, 50, 50],float) 281 | if t > 70: 282 | n = np.array([0, 0, 0, 0],float) 283 | 284 | u_control = n 285 | 286 | return u_control 287 | 288 | 289 | 290 | -------------------------------------------------------------------------------- /vehicles/tanker.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | tanker.py: 4 | 5 | Class for a large tanker, length L = 304.8 m and draft T = 18.46 m. The 6 | input variable 'depth' can be used to simulate shallow water effects. 7 | 8 | tanker() 9 | Step input, rudder angle 10 | tanker('headingAutopilot',psi_d,V_current,beta_c,depth,rpm) 11 | psi_d: desired yaw angle (deg) 12 | V_c: current speed (m/s) 13 | beta_c: current direction (deg) 14 | depth: the water depth must be larger than the draft T = 18.46 m 15 | rpm: shaft speed, nominal propeller rpm = 80 16 | 17 | Methods: 18 | 19 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime ) returns 20 | nu[k+1] and u_actual[k+1] using Euler's method. The control input is: 21 | 22 | u_control = delta_r (rad) is for the ship rudder. 23 | 24 | u = headingAutopilot(eta,nu,sampleTime) 25 | PID controller for automatic heading control based on pole placement. 26 | 27 | u = stepInput(t) generates rudder step inputs. 28 | 29 | References: 30 | W. B. Van Berlekom and T. A. Goddard (1972). Maneuvering of Large Tankers, 31 | Transaction of SNAME, Vol. 80, pp. 264-298. 32 | T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion 33 | Control. 2nd. Edition, Wiley. 34 | URL: www.fossen.biz/wiley 35 | 36 | Author: Thor I. Fossen 37 | """ 38 | import numpy as np 39 | import math 40 | import sys 41 | from functions.control import PIDpolePlacement 42 | from functions.models import clarke83 43 | 44 | # Class Vehicle 45 | class tanker: 46 | """ 47 | tanker() 48 | Rudder angle step inputs 49 | tanker('headingAutopilot',psi_d,V_current,beta_c,depth,rpm) 50 | Heading autopilot 51 | """ 52 | def __init__(self, controlSystem = 'stepInput', r = 0, 53 | V_current = 0, beta_current = 0, depth = 20.0, rpm = 80.0): 54 | 55 | if (controlSystem == 'headingAutopilot'): 56 | self.controlDescription = 'Heading autopilot, psi_d = ' \ 57 | + str(r) + ' (deg)' 58 | else: 59 | self.controlDescription = "Step input for delta_r" 60 | controlSystem = 'stepInput' 61 | 62 | self.ref = r 63 | self.V_c = V_current 64 | self.beta_c = beta_current 65 | self.waterDdepth = depth 66 | self.n_c = rpm 67 | self.controlMode = controlSystem 68 | 69 | # Initialize the ship model 70 | self.name = 'Tanker' 71 | self.L = 304.8 # length (m) 72 | self.T = 18.46 # draft (m) 73 | self.deltaMax = 30 # max rudder angle (deg) 74 | self.DdeltaMax = 5 # max rudder rate (deg/s) 75 | self.nMax = 90.0 # max propeller shaft speed (rpm) 76 | 77 | if (rpm < 10.0 or rpm > self.nMax): 78 | sys.exit('The RPM value should be in the interval 10-90') 79 | 80 | if (depth < self.T): 81 | sys.exit('The water depth must be larger than the draft T = 18.46 m') 82 | 83 | self.nu = np.array([4.8, 0, 0, 0, 0, 0], float) # velocity vector 84 | self.u_actual = np.array([0],float) # control input vector 85 | 86 | self.controls = ['Rudder angle (deg)'] 87 | self.dimU = len(self.controls) 88 | 89 | # Heading autopilot 90 | self.e_int = 0 # integral state 91 | self.wn = 0.2 # PID pole placement 92 | self.zeta = 0.8 93 | 94 | # Reference model 95 | self.r_max = 1.0 * math.pi / 180 # maximum yaw rate 96 | self.psi_d = 0 # angle, angular rate and angular acc. states 97 | self.r_d = 0 98 | self.a_d = 0 99 | self.wn_d = self.wn / 5 # desired natural frequency in yaw 100 | self.zeta_d = 1.0 # desired relative damping ratio in yaw 101 | 102 | 103 | def dynamics(self,eta,nu,u_actual,u_control,sampleTime): 104 | """ 105 | [nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates 106 | the ship equations of motion using Euler's method. 107 | """ 108 | h = self.waterDdepth # water depth (m) 109 | L = self.L # length (m) 110 | T = self.T # draft (m) 111 | 112 | # States and controls 113 | delta_c = u_control[0] # autopilot rudder command 114 | delta = u_actual[0] # actual rudder angle (rad) 115 | n = self.n_c / 60.0 # propeller shaft speed (rps) 116 | u = nu[0] 117 | v = nu[1] 118 | r = nu[5] 119 | 120 | # Current velocities 121 | u_c = self.V_c * math.cos(self.beta_c - eta[5]) # current surge velocity 122 | v_c = self.V_c * math.sin(self.beta_c - eta[5]) # current sway velocity 123 | 124 | nu_c = np.array([u_c,v_c,0,0,0,0],float) # current velocity vector 125 | nu_r = nu - nu_c # relative velocity vector 126 | 127 | u_r = nu_r[0] 128 | v_r = nu_r[1] 129 | 130 | try: 131 | beta = v_r / u_r # sideslip angle (rad) 132 | 133 | except ZeroDivisionError: 134 | print ('The sideslip angle is not defined for u_r = u - u_c = 0') 135 | 136 | # Parameters and hydrodynamic derivatives 137 | t = 0.22 138 | 139 | cun = 0.605 140 | cnn = 38.2 141 | 142 | Tuu = -0.00695 143 | Tun = -0.00063 144 | Tnn = 0.0000354 145 | 146 | m11 = 1.050 # 1 - Xudot 147 | m22 = 2.020 # 1 - Yvdot 148 | m33 = 0.1232 # kz^2 - Nrdot 149 | 150 | d11 = 2.020 # 1 + Xvr 151 | d22 = -0.752 # Yur - 1 152 | d33 = -0.231 # Nur - xG 153 | 154 | Xuuz = -0.0061; YT = 0.04; NT = -0.02 155 | Xuu = -0.0377; Yvv = -2.400; Nvr = -0.300 156 | Xvv = 0.3; Yuv = -1.205; Nuv = -0.451 157 | Xudotz = -0.05; Yvdotz = -0.387; Nrdotz = -0.0045 158 | Xuuz = -0.0061; Yurz = 0.182; Nurz = -0.047 159 | Xvrz = 0.387; Yvvz = -1.5; Nvrz = -0.120 160 | Xccdd = -0.093; Yuvz = 0.0; Nuvz = -0.241 161 | Xccbd = 0.152; Yccd = 0.208; Nccd = -0.098 162 | Xvvzz = 0.0125; Yccbbd = -2.16; Nccbbd = 0.688 163 | Yccbbdz= -0.191; Nccbbdz = 0.344 164 | 165 | # Shallow water effects 166 | z = T / ( h - T ) 167 | if (z >= 0.8): 168 | Yuvz = -0.85 * ( 1.0 - 0.8 / z ) 169 | 170 | # Forces and moment 171 | gT = (1/L * Tuu * u_r**2 + Tun * u_r * n + L * Tnn * abs(n) * n) 172 | c = math.sqrt( cun * u_r * n + cnn * n**2 ) 173 | 174 | gX = 1/L * ( Xuu * u_r**2 + L * d11 * v_r * r + Xvv * v_r**2 \ 175 | + Xccdd * abs(c) * c * delta**2 \ 176 | + Xccbd * abs(c) * c * beta * delta + L * gT * (1 - t) \ 177 | + Xuuz * u_r**2 * z \ 178 | + L * Xvrz * v_r * r * z + Xvvzz * v_r**2 * z**2 ) 179 | 180 | gY = 1/L * ( Yuv * u_r * v_r + Yvv * abs(v_r) * v_r \ 181 | + Yccd * abs(c) * c * delta + L * d22 * u_r * r \ 182 | + Yccbbd * abs(c) * c * abs(beta) * beta * abs(delta) \ 183 | + YT * gT * L + L * Yurz * u_r * r * z + Yuvz * u_r * v_r * z \ 184 | + Yvvz * abs(v_r) * v_r * z \ 185 | + Yccbbdz * abs(c) * c * abs(beta) * beta * abs(delta) *z ) 186 | 187 | gLN = Nuv * u_r * v_r + L * Nvr * abs(v_r) * r \ 188 | + Nccd * abs(c) * c * delta + L * d33 * u_r * r \ 189 | + Nccbbd * abs(c) * c * abs(beta) * beta * abs(delta) \ 190 | + L * NT * gT + L * Nurz * u_r * r * z \ 191 | + Nuvz * u_r * v_r * z + L * Nvrz * abs(v_r) * r * z \ 192 | + Nccbbdz * abs(c) * c * abs(beta) * beta * abs(delta) * z 193 | 194 | # Shallow water effects 195 | m11 = m11 - Xudotz * z 196 | m22 = m22 - Yvdotz * z 197 | m33 = m33 - Nrdotz * z 198 | 199 | # Dimensional state derivatives 200 | nu_dot = np.array([ 201 | gX / m11, 202 | gY / m22, 203 | 0.0, 204 | 0.0, 205 | 0.0, 206 | gLN / (L**2 * m33) ]) 207 | 208 | # Rudder angle saturation and dynamics 209 | if ( abs(delta) >= self.deltaMax * math.pi / 180 ): 210 | delta = np.sign(delta) * self.deltaMax * math.pi / 180 211 | 212 | delta_dot = (delta_c - delta) 213 | if ( abs(delta_dot) >= self.DdeltaMax * math.pi / 180 ): 214 | delta_dot = np.sign(delta_dot) * self.DdeltaMax * math.pi / 180 215 | 216 | # Forward Euler integration [k+1] 217 | nu = nu + sampleTime * nu_dot 218 | delta = delta + sampleTime * delta_dot 219 | 220 | u_actual = np.array([delta],float) 221 | 222 | return nu, u_actual 223 | 224 | 225 | def stepInput(self,t): 226 | """ 227 | delta_c = stepInput(t) generates rudder step inputs. 228 | """ 229 | delta_c = 10 * (math.pi/180) 230 | if t > 50: 231 | delta_c = 0 232 | 233 | u_control = np.array([delta_c],float) 234 | 235 | return u_control 236 | 237 | 238 | def headingAutopilot(self,eta,nu,sampleTime): 239 | """ 240 | delta_c = headingAutopilot(eta,nu,sampleTime) is a PID controller 241 | for automatic heading control based on pole placement. 242 | 243 | tau_N = m * a_d + d * r_d 244 | - Kp * ( ssa( psi-psi_d ) + Td * (r - r_d) + (1/Ti) * e_int ) 245 | """ 246 | psi = eta[5] # yaw angle 247 | r = nu[5] # yaw rate 248 | e_psi = psi - self.psi_d # yaw angle tracking error 249 | e_r = r - self.r_d # yaw rate tracking error 250 | psi_ref = self.ref * math.pi / 180 # yaw angle setpoint 251 | 252 | wn = self.wn # PID natural frequency 253 | zeta = self.zeta # PID natural relative damping factor 254 | wn_d = self.wn_d # reference model natural frequency 255 | zeta_d = self.zeta_d # reference model relative damping factor 256 | 257 | m = 500 258 | d = 0.0 259 | k = 0.0 260 | 261 | # PID feedback controller with 3rd-order reference model 262 | [delta_r, self.e_int, self.psi_d, self.r_d, self.a_d] = PIDpolePlacement(\ 263 | self.e_int, e_psi, e_r,self.psi_d, self.r_d, \ 264 | self.a_d, m, d, k, wn_d, zeta_d, wn, zeta, psi_ref, self.r_max, \ 265 | sampleTime ) 266 | 267 | delta = -delta_r 268 | u_control = np.array([delta],float) 269 | 270 | return u_control 271 | 272 | 273 | 274 | --------------------------------------------------------------------------------