-
Notifications
You must be signed in to change notification settings - Fork 0
/
supply.py
306 lines (245 loc) · 10.2 KB
/
supply.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
supply.py:
Class for an offshore supply vessel length L = 76.2.
The constructors are:
supply()
Step inputs for the propeller speeds n1, n2, n3, n4, n5 and n6
supply('DPcontrol',x_d,y_d,psi_d,V_c,beta_c) DP control system
x_d: desired x position (m)
y_d: desired y position (m)
psi_d: desired yaw angle (deg)
V_c: ocean current speed (m/s)
beta_c: ocean current direction (deg)
Methods:
[nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime)
returns nu[k+1] and u_actual[k+1] using Euler's method.
The control inputs are:
u_control = n
n = [ #1 Bow tunnel thruster (RPM),
#2 Bow tunnel thruster (RPM),
#3 Stern tunnel thruster (RPM),
#4 Stern tunnel thruster (RPM),
#5 Right main propeller (RPM),
#6 Left main propeller (RPM)]
u_alloc = controlAllocation(tau)
Control allocation based on the pseudoinverse
n = DPcontrol(eta,nu,sampleTime)
Nonlinear PID controller for DP based on pole placement.
n = stepInput(t) generates propellers step inputs.
References:
T. I. Fossen, S. I. Sagatun and A. J. Sorensen (1996)
Identification of Dynamically Positioned Ships
Journal of Control Engineering Practice CEP-4(3):369-376
T. I. Fossen (2021). Handbook of Marine Craft Hydrodynamics and Motion
Control. 2nd. Edition, Wiley. URL: www.fossen.biz/wiley
Original Author: Thor I. Fossen
Modified by: Rodrigue de Schaetzen
See git issue https://github.com/cybergalactic/MSS/issues/57
"""
from typing import Union
import numpy as np
import math
from ship_ice_planner.controller.control import DPpolePlacement
from ship_ice_planner.controller.gnc import sat
# Class Vehicle
class Supply:
"""
supply() Propeller step inputs
supply('DPcontrol',x_d,y_d,psi_d,V_c,beta_c) DP control system
Inputs:
x_d: desired x position (m)
y_d: desired y position (m)
psi_d: desired yaw angle (deg)
V_c: current speed (m/s)
beta_c: current direction (deg)
"""
def __init__(
self,
controlSystem="stepInput",
r_x=0,
r_y=0,
r_n=0,
V_current=0,
beta_current=0,
):
# Constants
D2R = math.pi / 180 # deg2rad
g = 9.81 # acceleration of gravity (m/s^2)
if controlSystem == "DPcontrol":
self.controlDescription = (
"Nonlinear DP control (x_d, y_d, psi_d) = ("
+ str(r_x)
+ " m, "
+ str(r_y)
+ " m, "
+ str(r_n)
+ " deg)"
)
else:
self.controlDescription = "Step inputs n = [n1, n2, n3, n4, n5, n6]"
controlSystem = "stepInput"
self.ref = np.array([r_x, r_y, r_n * D2R], float)
self.V_c = V_current
self.beta_c = beta_current * D2R
self.controlMode = controlSystem
# Initialize the supply vessel model
self.mass = 6000.0e3 # mass (kg)
self.L = 76.2 # length (m)
self.T_n = 1.0 # prop. speed time constant (s)
self.n_max = np.array([250, 250, 250, 250,
160, 160], float) # RPM saturation limits
self.nu = np.array([0, 0, 0, 0, 0, 0], float) # initial velocity vector
self.u_actual = np.array([0, 0, 0, 0, 0, 0], float) # RPM inputs
self.name = "Offshore supply vessel (see 'supply.py' for more details)"
# Two tunnel thrusters in the bow, no. 1 and 2
# Two tunnel thrusters in the stern, no. 3 and 4
# Two main propellers aft, no. 3 and 4
self.controls = [
"#1 Bow tunnel thruster (RPM)",
"#2 Bow tunnel thruster (RPM)",
"#3 Stern tunnel thruster (RPM)",
"#4 Stern tunnel thruster (RPM)",
"#5 Right main propeller (RPM)",
"#6 Left main propeller (RPM)"
]
self.dimU = len(self.controls)
# Thrust coefficient and configuration matrices (Fossen 2021, Ch. 11.2)
# Thrust_max(i) = K(i) * n_max(i)^2
# Tunnel thruster: 3.2 * 250^2 = 200 kN
# Main propeller: 31.2 * 160^2 = 799 kN
K = np.diag([3.2, 3.2, 3.2, 3.2, 31.2, 31.2]) # thrust coefficient matrix
K_max = np.diag(K * self.n_max ** 2)
K_max_surge = K_max[4] + K_max[5] # max thrust in the surge direction (N)
T = np.array( # actuator configuration matrix only depends on the location of the actuators
[[0, 0, 0, 0, 1, 1], [1, 1, 1, 1, 0, 0],
[30, 22, -22, -30, -8, 8]], float
)
# force vector is given by tau = T @ K @ (abs(n) * n) where n is the propeller RPM
self.B = T @ K
# Tbis = np.diag( [1, 1, 1 / self.L],float)
Tbis_inv = np.diag([1.0, 1.0, self.L])
# 3-DOF model matrices - bis scaling (Fossen 2021, App. D)
Mbis = np.array(
[[1.1274, 0, 0], [0, 1.8902, -0.0744], [0, -0.0744, 0.1278]], float
)
Dbis = np.array(
[[0.0358, 0, 0], [0, 0.1183, -0.0124], [0, -0.0041, 0.0308]], float
)
self.M3 = self.mass * Tbis_inv @ Mbis @ Tbis_inv
self.M3inv = np.linalg.inv(self.M3)
self.D3 = self.mass * math.sqrt(g / self.L) * Tbis_inv @ Dbis @ Tbis_inv
# DP control system
self.e_int = np.array([0, 0, 0], float) # integral states
self.x_d = 0.0 # setpoints
self.y_d = 0.0
self.psi_d = 0.0
self.wn = np.diag([0.1, 0.1, 0.2]) # PID pole placement
self.zeta = np.diag([1 / np.sqrt(2), 1 / np.sqrt(2), 1 / np.sqrt(2)])
# environment forces and moment
self.tau3_env = np.array([0, 0, 0], float)
def dynamics(self, eta, nu, u_actual, u_control, sampleTime):
"""
[nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates the
supply vessel equations of motion using Euler's method.
"""
# Input vector
n = u_actual # propeller speed (RPM)
# Ocean Current velocities
# see slide 22 in "Chapter 3 Rigid Body Kinetics" or slide 56 in "Chapter 6 Maneuvering Models"
u_c = self.V_c * math.cos(self.beta_c - eta[5]) # current surge velocity
v_c = self.V_c * math.sin(self.beta_c - eta[5]) # current sway velocity
nu_c = np.array([u_c, v_c, 0, 0, 0, 0], float) # current velocity vector
nu_r = nu - nu_c # relative velocity vector
# Control forces and moments with propeller saturation
n_squared = np.zeros(self.dimU)
for i in range(0, self.dimU):
n[i] = sat(n[i], -self.n_max[i], self.n_max[i]) # saturation
n_squared[i] = abs(n[i]) * n[i]
tau3 = np.matmul(self.B, n_squared)
tau3 += self.tau3_env # I think here is where the environment force and moment should be added!!
# 3-DOF dynamics
nu3_r = np.array([nu_r[0], nu_r[1], nu_r[5]])
nu3_dot = np.matmul(self.M3inv, tau3 - np.matmul(self.D3, nu3_r))
# 6-DOF ship model and propeller speed dynamics
nu_dot = np.array([nu3_dot[0], nu3_dot[1], 0, 0, 0, nu3_dot[2]])
n_dot = (u_control - u_actual) / self.T_n
# Forward Euler integration
nu = nu + sampleTime * nu_dot
n = n + sampleTime * n_dot
u_actual = np.array(n, float)
return nu, u_actual
def controlAllocation(self, tau3):
"""
u_alloc = controlAllocation(tau3), tau3 = [tau_X, tau_Y, tau_N]'
u_alloc = B' * inv( B * B' ) * tau3
"""
B_pseudoInv = self.B.T @ np.linalg.inv(self.B @ self.B.T)
u_alloc = np.matmul(B_pseudoInv, tau3) # squared propeller speed
return u_alloc
def DPcontrol(self, eta, nu, sampleTime):
"""
u = DPcontrol(eta,nu,sampleTime) is a nonlinear PID controller
for DP based on pole placement:
tau = -R' Kp (eta-r) - Kd nu - R' Ki int(eta-r)
u = B_pseudoinverse * tau
"""
eta3 = np.array([eta[0], eta[1], eta[5]]) # x, y, psi
nu3 = np.array([nu[0], nu[1], nu[5]]) # u, v, r
[tau3, self.e_int] = DPpolePlacement(
self.e_int,
self.M3,
self.D3,
eta3,
nu3,
self.x_d,
self.y_d,
self.psi_d,
self.wn,
self.zeta,
sampleTime
)
u_alloc = self.controlAllocation(tau3)
# u_alloc = abs(n) * n --> n = sign(u_alloc) * sqrt(u_alloc)
n = np.zeros(self.dimU)
for i in range(0, self.dimU):
n[i] = np.sign(u_alloc[i]) * math.sqrt(abs(u_alloc[i]))
u_control = n
return u_control
def set_setpoint(self, x_d, y_d, psi_d):
self.x_d = x_d # m
self.y_d = y_d # m
self.psi_d = psi_d # rad
def stepInput(self, t):
"""
u = stepInput(t) generates propeller step inputs (RPM).
"""
# n = np.array([-170, -130, 126, 160, 160, -70], float)
n = np.array([0, 0, 0, 0, 90, 90], float) # hardcoded to get vessel to reach 2 m/s
if t > 30:
n = np.array([0, 0, 0, 0, 50, 50], float) # hardcoded to get vessel to reach 2 m/s
u_control = n
return u_control
def set_environment_force(self, force):
"""
Force should be a 3D vector [X, Y, N]
"""
self.tau3_env = force
def compute_force(self, n) -> np.ndarray:
"""
computes the resulting forces and moment [X, Y, N]
X -- force in surge
Y -- force in sway
N -- torque in yaw
See Table 2.1 in Fossen (2021) for the SNAME (1950) notation for marine vessels
"""
# assert len(n) == self.dimU
return self.B @ (np.abs(n) * n)
def compute_energy_use(self, n, nu, sampleTime) -> Union[float, np.ndarray]:
if len(n.shape) == 1:
return np.abs(self.compute_force(n)) @ np.abs(nu) * sampleTime
energy_use = np.zeros(len(n))
for i in range(len(n)):
energy_use[i] = np.abs(self.compute_force(n[i])) @ np.abs(nu[i]) * sampleTime
return energy_use