Hovercraft Path Planning
In this case study, we seek to determine an optimal control policy for the trajectory of a hovercraft that travels to a set of dynamic waypoints while trying to minimize the thrust input.
Formulation
The corresponding dynamic optimization problem is expressed:
\[\begin{aligned} &&\underset{x(t), v(t), u(t)}{\text{min}} &&& \int_{t \in T} |u(t)|_2^2 dt \\ &&\text{s.t.} &&& v(0) = v0\\ &&&&& \frac{dx}{dt} = v(t), && t \in T\\ &&&&& \frac{dv}{dt} = u(t), && t \in T\\ &&&&& x(t_i) = xw_i, && i \in I \end{aligned}\]
where $x(t)$ is the Cartesian position, $v(t)$ is the velocity, $u(t)$ is the thrust input, $xw_i, \ i \in I,$ are the waypoints, and $T$ is the time horizon.
Model Definition
Let's implement this in InfiniteOpt
and first import the packages we need:
using InfiniteOpt, Ipopt
Next we'll specify our waypoint data:
xw = [1 4 6 1; 1 3 0 1] # positions
tw = [0, 25, 50, 60]; # times
We initialize the infinite model and opt to use the Ipopt solver:
m = InfiniteModel(optimizer_with_attributes(Ipopt.Optimizer, "print_level" => 0));
Let's specify our infinite parameter which is time $t \in [0, 60]$:
@infinite_parameter(m, t in [0, 60], num_supports = 61)
t
Now let's specify the decision variables:
@variables(m, begin
# state variables
x[1:2], Infinite(t)
v[1:2], Infinite(t)
# control variables
u[1:2], Infinite(t), (start = 0)
end)
(GeneralVariableRef[x[1](t), x[2](t)], GeneralVariableRef[v[1](t), v[2](t)], GeneralVariableRef[u[1](t), u[2](t)])
Specify the objective:
@objective(m, Min, ∫(u[1]^2 + u[2]^2, t))
∫{t ∈ [0, 60]}[u[1](t)² + u[2](t)²]
Set the initial conditions with respect to the velocity:
@constraint(m, [i = 1:2], v[i](0) == 0)
2-element Vector{InfOptConstraintRef}:
v[1](0) = 0
v[2](0) = 0
Define the point physics ODEs which serve as our system model:
@constraint(m, [i = 1:2], ∂(x[i], t) == v[i])
@constraint(m, [i = 1:2], ∂(v[i], t) == u[i])
2-element Vector{InfOptConstraintRef}:
d/dt[v[1](t)] - u[1](t) = 0, ∀ t ∈ [0, 60]
d/dt[v[2](t)] - u[2](t) = 0, ∀ t ∈ [0, 60]
Ensure we hit all the waypoints:
@constraint(m, [i = 1:2, j = eachindex(tw)], x[i](tw[j]) == xw[i, j])
2×4 Matrix{InfOptConstraintRef}:
x[1](0) = 1 x[1](25) = 4 x[1](50) = 6 x[1](60) = 1
x[2](0) = 1 x[2](25) = 3 x[2](50) = 0 x[2](60) = 1
Problem Solution
Optimize the model:
optimize!(m)
Extract the results. The InfiniteInterpolations
extension can be used to get a smooth interpolated function for x, which is invoked when both the Interpolations
and InfiniteOpt
packages are imported. Here, cubic splines are used as the interpolation method for both x1 and x2:
using Interpolations
xFunc = value.(x, cubic_spline_interpolation);
Query our interpolated function for the values of x1 and x2:
tvals = LinRange(0, 60, 100)
x1Vals = xFunc[1](tvals)
x2Vals = xFunc[2](tvals);
Plot the results:
using Plots
scatter(xw[1,:], xw[2,:], label = "Waypoints")
xlabel!("x_1")
ylabel!("x_2")
plot!(x1Vals, x2Vals, label = "Trajectory")
That's it, now we have our optimal trajectory!
Maintenance Tests
These are here to ensure this example stays up to date.
using Test
tol = 1E-6
@test termination_status(m) == MOI.LOCALLY_SOLVED
@test has_values(m)
@test x1Vals isa Vector{<:Real}
@test x2Vals isa Vector{<:Real}
@test isapprox(objective_value(m), 0.043685293177035435, atol=tol)
@test isapprox(value(u[1])[end], -0.010503853944039986, atol=tol)
@test isapprox(value(u[2])[end], 0.005456780217220367, atol=tol)
@test isapprox(x1Vals[15], 1.3837274935883543, atol=tol)
@test isapprox(x2Vals[15], 1.6386021193421085, atol=tol)
Test Passed
This page was generated using Literate.jl.