Solve Fuel-Optimal Navigation Problem using C/GMRES

In this example, you address a specialized variant of the classic Zermelo's navigation problem using Multistage Nonlinear Model Predictive control.

Traditionally, Zermelo's navigation problem deals with determining the minimum time path for a boat moving from an initial location to a target destination across a watery expanse. The boat is capable of a certain maximum speed and is usually presented as a time-optimal control problem. The goal is to derive the best possible control to reach the destination B in the least possible time.

Use Multistage Nonlinear Model Predictive Control (NMPC) to determine the optimal path for a boat to reach a destination by minimizing the fuel expenditure, while being affected by a drift.

Problem Formulation

Let's consider a two-dimensional case where the boat is moving in the xy-plane. The state of the boat is given by its position $(\mathit{x},y)$ and its velocity $\mathit{v}$. The control inputs are the velocity $\mathit{v}$ and the direction $\alpha$ of the boat. The drift is represented by a velocity vector $d(x,y)$, which depends on the position of the boat.

The dynamics of the boat can be represented by the following differential equations:

$$\dot{\mathit{x}}=\mathit{v}\cdot \cos \left(\alpha \right)+{\mathit{d}}_{\mathit{x}}\left(\mathit{x},\mathit{y}\right)$$

$$\dot{\mathit{y}}=\mathit{v}\cdot \sin \left(\alpha \right)+{\mathit{d}}_{\mathit{y}}\left(\mathit{x},\mathit{y}\right)$$

where ${\mathit{d}}_{\mathit{x}}$ and ${\mathit{d}}_{\mathit{y}}$ are the components of the drift vector $d(x,y)$.

For this example, the drift function is defined as $\mathit{d}\left(\mathit{x},\mathit{y}\right)=\left[\begin{array}{c}3+0.2\mathit{y}\cdot \left(1-\mathit{y}\right)\\ 0\end{array}\right]$.

% Define the destination
x_d = 10;
y_d = 10;

% Define the initial state
x_0 = 0;
y_0 = 0;

Setting Up Nonlinear MPC Controller

Create a nonlinear multistage MPC object with two states and two inputs.

Ts = 1e-1;% Sampling time
p = 20;   % Prediction horizon
nx = 2;   % Number of states
nmv = 2;  % Number of manipulated (control) variables

msobj = nlmpcMultistage(p, nx, nmv);
msobj.Ts = Ts; 

For this example, the model and stage functions are encapsulated within the Navigation class (which is defined in the suppporting file Navigation.m) as static methods. To call these static methods, use the class name followed by a dot (.), and then the method name. These methods provide the MPC controller with the necessary system dynamics and optimization criteria evaluations at each step within the prediction horizon.

% Define the State and State Jacobian functions
msobj.Model.StateFcn = 'Navigation.StateFcn';
msobj.Model.StateJacFcn = 'Navigation.StateJacFcn';
msobj.Model.ParameterLength = 1;

% Define the Cost and Cost Jacobian functions
[msobj.Stages.CostFcn] = deal('Navigation.CostFcn');
[msobj.Stages.CostJacFcn] = deal('Navigation.CostJacFcn');
[msobj.Stages.ParameterLength] = deal(3);

Specify control input constraints for realistic and safe operation of the boat. The direction angle constraint ($0$ to $2\pi$ radians) ensures full navigational freedom within the boat's capability. The velocity constraint ($0$ to $1$ m/s) matches the boat propulsion limits, preventing commands exceeding its power.

% Set constraints for the direction angle control input
msobj.ManipulatedVariables(1).Min = 0;
msobj.ManipulatedVariables(1).Max = 6.28;

% Set constraints for the velocity control input
msobj.ManipulatedVariables(2).Min = 0;
msobj.ManipulatedVariables(2).Max = 1;

For this example, use the Continuation/Generalized Minimum Residual (C/GMRES) solver to handle the optimization problem within the Multistage Model Predictive Control framework.

To configure the CGMRES solver for our MPC controller, define its properties as follows:

By setting these properties, you ensure that the MPC controller is equipped with a solver that is tuned for the specific requirements and dynamics of the application, including the ability to navigate with drift and avoid obstacles.

% msobj.Optimization.Solver = 'cgmres';
% msobj.Optimization.SolverOptions.StabilizationParameter = 1/msobj.Ts;
% msobj.Optimization.SolverOptions.Restart = 1;
% msobj.Optimization.SolverOptions.MaxIterations = 20;

The state and stage functions require state and stage parameters. Use getSimulationData to initialize data structure. For more information on simulation data structure for multistage MPC, see getSimulationData.

xref = [10; 5];
obs = [2; 5; 2];
pvcost = [xref; p];
pvstate = 1;
simdata = getSimulationData(msobj);
simdata.StateFcnParameter = pvstate;
simdata.StageParameter = repmat(pvcost, p+1, 1);

Validate the model functions.

validateFcns(msobj,rand(nx,1),rand(nmv,1),simdata)

Model.StateFcn is OK.
Model.StateJacFcn is OK.
"CostFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK.
"CostJacFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK.
Analysis of user-provided model, cost, and constraint functions complete.

Closed-Loop Simulation in MATLAB

Simulate the system for 2 seconds with a target trajectory to follow.

% Define the simulation duration in seconds.
Duration = 15;

% Initial state and control vectors.
x0 = zeros(nx,1);
u0 = zeros(nmv,1);

% Preallocate history arrays  states and control inputs,
% for later plotting.
xHistory1 = x0.'; % State history
uHistory1 = u0.'; % Control input history

% Initialize the control input for the first step.
uk = u0;

% Simulation loop to propagate the system dynamics
% over the specified duration.
for k = 1:(Duration/Ts)
    % Current state at iteration k.
    xk = xHistory1(k,:).';
    
    % Compute the optimal control action using nlmpcmove.
    [uk, simdata, info] = nlmpcmove(msobj, xk, uk, simdata);
    
    % Simulate the system for the next control interval using ode45.
    odefun = @(t,xk) Navigation.StateFcn(xk,uk);
    [TOUT, XOUT] = ode45(odefun, [0 Ts], xk);
    
    % Log the states and control inputs for analysis and plotting.
    xHistory1(k+1,:) = XOUT(end,:);
    uHistory1(k+1,:) = uk;
end

% Time vector for plotting.
time = 0:Ts:Duration;

Plot the state and control input histories.

figure;
tiledlayout(2,2);
ax = gobjects(4,1);
ax(1) = nexttile; plot(time, xHistory1(:,1));
ax(2) = nexttile; plot(time, xHistory1(:,2));
ax(3) = nexttile; plot(time, uHistory1(:,1));
ax(4) = nexttile; plot(time, uHistory1(:,2));

title(ax(1), 'State 1: Position x');
title(ax(2), 'State 2: Position y');
title(ax(3), 'Control Input: Direction Angle');
title(ax(4), 'Control Input: Velocity');

xlabel(ax,'Time (s)');
ylabel(ax(1), 'Position x');
ylabel(ax(2), 'Position y');
ylabel(ax(3), 'Direction Angle (rad)');
ylabel(ax(4), 'Velocity (m/s)');

Adjust the state dynamics of the boat to account for drift.

msobj.Model.StateFcn = 'Navigation.StateFcnWithDrift';
msobj.Model.StateJacFcn = 'Navigation.StateJacFcnWithDrift';
simdata.InitialGuess = [];

Simulate the system for 2 seconds with a target trajectory to follow.

% Preallocate history arrays for storing states and control inputs for plotting.
xHistory2 = x0.'; % State history
uHistory2 = u0.'; % Control input history

% Initialize the control input for the first step.
uk = u0;

% Simulation loop to propagate the system dynamics over the specified duration.
for k = 1:(Duration/Ts)
    % Current state at iteration k.
    xk = xHistory2(k,:).';
    
    % Compute the optimal control action using nlmpcmove.
    [uk, simdata, info] = nlmpcmove(msobj, xk, uk, simdata);
    
    % Simulate the system for the next control interval using ode45.
    odefun = @(t,xk) Navigation.StateFcnWithDrift(xk,uk);
    [TOUT, XOUT] = ode45(odefun, [0 Ts], xk);
    
    % Log the states and control inputs for analysis and plotting.
    xHistory2(k+1,:) = XOUT(end,:);
    uHistory2(k+1,:) = uk;
end

Plot the state and control input histories.

figure;
tiledlayout(2,2);
ax = gobjects(4,1);
ax(1) = nexttile; plot(time, xHistory2(:,1));
ax(2) = nexttile; plot(time, xHistory2(:,2));
ax(3) = nexttile; plot(time, uHistory2(:,1));
ax(4) = nexttile; plot(time, uHistory2(:,2));

title(ax(1), 'State 1: Position x');
title(ax(2), 'State 2: Position y');
title(ax(3), 'Control Input: Direction Angle');
title(ax(4), 'Control Input: Velocity');

xlabel(ax,'Time (s)');
ylabel(ax(1), 'Position x');
ylabel(ax(2), 'Position y');
ylabel(ax(3), 'Direction Angle (rad)');
ylabel(ax(4), 'Velocity (m/s)');

Obstacle Avoidance

To enhance the capabilities of our MPC controller and ensure safe navigation, you can introduce obstacle avoidance feature.

For this example, the obstacle is modeled as a circle, and its presence is accounted for within the MPC framework using inequality constraints. These constraints are defined by the Navigation.IneqConFcn function, which calculates the distance between the boat's current position and the center of the obstacle; the boat is required to maintain a distance greater than the obstacle's radius to avoid collision.

Update the msobj object to incorporate obstacle avoidance into each stage of the MPC controller:

[msobj.Stages.IneqConFcn] = deal('Navigation.IneqConFcn');
[msobj.Stages.IneqConJacFcn] = deal('Navigation.IneqConJacFcn');
[msobj.Stages.ParameterLength] = deal(6);

By assigning IneqConFcn and IneqConJacFcn to all stages of the MPC controller, you ensure that the obstacle avoidance behavior is considered at every prediction horizon step. Here, ParameterLength is set to 6 to account for the additional parameters required by these functions, which include the radius and center of the obstacle. This update equips the MPC controller with the necessary tools to navigate safely and efficiently in environments with static obstacles.

pvcost = [xref; p; obs];
pvstate = 1;
simdata = getSimulationData(msobj);
simdata.StateFcnParameter = pvstate;
simdata.StageParameter = repmat(pvcost, p+1, 1);
validateFcns(msobj,rand(nx,1),rand(nmv,1),simdata)

Model.StateFcn is OK.
Model.StateJacFcn is OK.
"CostFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK.
"CostJacFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK.
"IneqConFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK.
"IneqConJacFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21] are OK.
Analysis of user-provided model, cost, and constraint functions complete.

Perform a closed-loop simulation with obstacle avoidance.

% Preallocate history arrays for storing states and control inputs
% for later plotting.
xHistory3 = x0.'; % State history
uHistory3 = u0.'; % Control input history

% Initialize the control input for the first step.
uk = u0;

% Simulation loop to propagate the system dynamics 
% over the specified duration.
for k = 1:(Duration/Ts)
    % Current state at iteration k.
    xk = xHistory3(k,:).';
    
    % Compute the optimal control action using nlmpcmove.
    [uk, simdata, info] = nlmpcmove(msobj, xk, uk, simdata);
    
    % Simulate the system for the next control interval using ode45.
    odefun = @(t,xk) Navigation.StateFcnWithDrift(xk,uk);
    [TOUT, XOUT] = ode45(odefun, [0 Ts], xk);
    
    % Log the states and control inputs for analysis and plotting.
    xHistory3(k+1,:) = XOUT(end,:);
    uHistory3(k+1,:) = uk;
end

Plot the state and control input histories.

figure;
tiledlayout(2,2);
ax = gobjects(4,1);
ax(1) = nexttile; plot(time, xHistory3(:,1));
ax(2) = nexttile; plot(time, xHistory3(:,2));
ax(3) = nexttile; plot(time, uHistory3(:,1));
ax(4) = nexttile; plot(time, uHistory3(:,2));

title(ax(1), 'State 1: Position x');
title(ax(2), 'State 2: Position y');
title(ax(3), 'Control Input: Direction Angle');
title(ax(4), 'Control Input: Velocity');

xlabel(ax,'Time (s)');
ylabel(ax(1), 'Position x');
ylabel(ax(2), 'Position y');
ylabel(ax(3), 'Direction Angle (rad)');
ylabel(ax(4), 'Velocity (m/s)');

Plot the trajectories with (red) and without (blue) obstacle avoidance. Use the helper function plotDriftVelocityMagnitude to create a contour plot of velocity magnitude with drift. Here, the magnitude of the velocity is represented using a color scale from blue to yellow.

figure; 
plotDriftVelocityMagnitude(0,15,-2,7);

hold on; axis equal
plot(xHistory2(:,1), xHistory2(:,2), 'LineWidth', 2)
plot(xHistory3(:,1), xHistory3(:,2), 'LineWidth', 2)

theta = linspace(0, 2*pi);
circle_x = obs(2) + obs(1)*cos(theta);
circle_y = obs(3) + obs(1)*sin(theta);
plot(circle_x, circle_y);
rectangle( ...
    'Position', [obs(2)-obs(1) obs(3)-obs(1) obs(1)*2 obs(1)*2], ...
    'Curvature', 1, ...
    'FaceColor','w')