Optimal control of surge and heading

[Olarm]

Hi,

First of all, thank you very much for a great package!

Im in the early stages of developing an optimal control system for surge and yaw control of a ship, currently ive got this set of equations:

$\dot{v} = \frac{b}{m}v + \frac{1}{m}\tau$
$\dot{r} = -\frac{1}{T}r + \frac{K}{T}\delta$
$\dot{x} = v\sin{(r)}$
$\dot{y} = v\cos{(r)}$

And i want to optimize for hitting waypoints and avoiding obstacles by adjusting the inputs: rudder angle $\delta$ and engine torque $\tau$

I started from the hovercraft example and this is my current code:

# Surge / speed model
M = 10000*1000 # mass of ship
B = 200000  # damping coefficient

# KT / heading model parameters 
K = 0.155
T = 100.5
δ_max = pi/4
τ_max = 3000000

# Waypoints
xw = [1 400 600 1; 1 300 0 1] # positions
tw = [0, 150, 300, 450];    # times

m = InfiniteModel(optimizer_with_attributes(Ipopt.Optimizer, "print_level" => 0));

@infinite_parameter(m, t in [0, 450], num_supports = 451)

@variable(m, v, Infinite(t))    # speed
@variable(m, 0 ≤ τ ≤ τ_max, Infinite(t), (start=0)) # Torque, constrained to avoid reversing
@variable(m, -δ_max ≤ δ ≤ δ_max, Infinite(t), (start=0)) # Rudder angle
@variable(m, r, Infinite(t))    # course
@variable(m, x, Infinite(t))    # East position
@variable(m, y, Infinite(t))    # North position

@objective(m, Min, ∫(τ^2 + δ^2, t)) # penalize torque and rudder angle

@constraint(m, v(0) == 0)
@constraint(m, r(0) == 0)
@constraint(m, ∂(v, t) == -B/M*v + 1/M*τ)
@constraint(m, ∂(r, t) == -1/T*r + K/T*δ)
@constraint(m, ∂(x, t) == v * sin(r))
@constraint(m, ∂(y, t) == v * cos(r))
@constraint(m, [j = eachindex(tw)], x(tw[j]) == xw[1,j])
@constraint(m, [j = eachindex(tw)], y(tw[j]) == xw[2,j])

optimize!(m)
x_opt = value.(x);
y_opt = value.(y);
δ_opt = value.(δ);
τ_opt = value.(τ);
v_opt = value.(v);
ts = value.(t);

l = @layout [
    a{0.6h}; [b; c; d]
]

p1 = scatter(xw[1,:], xw[2,:], label = "Waypoints", background_color = :transparent)
plot!(x_opt, y_opt, label = "Trajectory")
xlabel!("x")
ylabel!("y")

p2 = plot(ts, rad2deg.(δ_opt), label="rudder")
p3 = plot(ts, v_opt, label="speed")
p4 = plot(ts, τ_opt, label="torque")

plot(p1, p2, p3, p4, layout=l, size=(800,900))

Im getting some strange results in that the variables dont seem to be constrained as i expect them to be:

As you can see from the plots im getting "jumps" in heading and speed which no input into the equations should be able to produce (at least as far as i can see...).

Any help here would be greatly appreciated!

[pulsipher]

Hi @Olarm, welcome to the forum!

Before using the "optimal values" it is important to check the termination status to make sure the solver achieved a a feasible optimal solution. In this case, I get:

julia> termination_status(m)
LOCALLY_INFEASIBLE::TerminationStatusCode = 5

so the solver was unable to find a feasible solution which explains why the plot doesn't make sense. This can be because either the model is in fact infeasible (i.e., the ship is not capable of hitting the desired waypoints) or a better initial guess is needed (a feasible guess is critical for an interior point solver like Ipopt).

Initial guesses are very helpful for both your control and state variables. You will want to specify this guess as a function of time with the start keyword argument: see the docs. Obtaining such an initial guess can be tricky, but you can use simulations or a relaxed problem (i.e., relax the waypoint constraints to be objective penalties).

You should also scale the problem a little better, torque is several orders of magnitude larger than the other values which is problematic for numerical solvers. You should scale it to make on the same order of magnitude.

After relaxing your problem to only aim for the 1st waypoint with a scaled torque, this is what I get (excluding good guesses):

using InfiniteOpt, Ipopt, Plots

# Surge / speed model
M = 10000*1000 # mass of ship
B = 200000  # damping coefficient

# KT / heading model parameters 
K = 0.155
T = 100.5
δ_max = pi/2
τ_max = 3000000
τ_scale_max = τ_max / M

m = InfiniteModel(Ipopt.Optimizer)

@infinite_parameter(m, t in [0, 150], num_supports = 151)

@variable(m, v, Infinite(t))    # speed
@variable(m, 0 ≤ τ ≤ τ_scale_max, Infinite(t)) # Torque, constrained to avoid reversing
@variable(m, -δ_max ≤ δ ≤ δ_max, Infinite(t)) # Rudder angle
@variable(m, r, Infinite(t))    # course
@variable(m, x, Infinite(t))    # East position
@variable(m, y, Infinite(t))    # North position

@objective(m, Min,  (x(150) - 400)^2 + (y(150) - 300)^2)

@constraint(m, v(0) == 0)
@constraint(m, r(0) == 0)
@constraint(m, ∂(v, t) == -(B/M)*v + τ)
@constraint(m, ∂(r, t) == -(1/T)*r + (K/T)*δ)
@constraint(m, ∂(x, t) == v * sin(r))
@constraint(m, ∂(y, t) == v * cos(r))
@constraint(m, x(0) == 1)
@constraint(m, y(0) == 1)

optimize!(m)

x_opt = value(x)
y_opt = value(y)
δ_opt = value(δ)
τ_opt = value(τ) * M
v_opt = value(v)
ts = value(t)

where the ship is unable to make the waypoint even though it maxes out the rudder.

If we solve it again, but let the initial heading be r(0) = pi/2, we get:

So, it might well be that the original model is not feasible as formulated.

[Olarm]

Thanks alot for the feedback, I will try to simplify things and also play around with a smaller boat.