1 Star 1 Fork 0

JeffreyChan / mpc

Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
Clone or download
test_disturbances.m 11.16 KB
Copy Edit Web IDE Raw Blame History
Colin Calas authored 2017-08-25 14:23 . first commit
%%%%%%%%%%%%% - USV autonomous control algorithm using MPC - %%%%%%%%%%%%%%
%%% This code use and MPC algorithm to track an angle reference while %%%
%%% keeping a constant cruise speed. %%%
%%% Written by Colin Calas at Cardiff University on August 2017 %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% TODO :
%%% - Add the cruise velocity to the MPC
%%% - Test with a testbench path
%Clean the workspace and close the open figures
clear
close all
%Boat and simulation parameters
m = 37; %Mass of the boat
D = 0.7; %Distance between the motors and the center of mass
I = 0.1; %Moment of inertia (arbitrary value, should be identified)
k = 0.1; %Viscosity coefficient (arbitrary value, should be identified)
ky = 0.1; %Viscosity coefficient on the y axis (arbitrary value, should be identified)
Tfinal = 150; %Total simulation time
Te = 0.1; %Sampling period
%Vectors used to hold simulation data
x = zeros(9, ceil(Tfinal/Te)); %State vector
u = zeros(2, ceil(Tfinal/Te)); %Input vector
delta_u = zeros(2, ceil(Tfinal/Te)); %Input increment vector
a = zeros(3, ceil(Tfinal/Te)); %State vector
a_ref = zeros(1, ceil(Tfinal/Te));
i = 1; %Loop index
%Ordered list of waypoints
x_list = [2 4 32 40 25 10 2]'; %X coordinates of the waypoints
y_list = [2 15 17 7 0 -5 2]'; %Y coordinates of the waypoints
a_list = zeros(7,1); %Angle of the boat between two successive waypoints
current_obj = 2; %As the boat starts in the first waypoint, the current objective is the next
%ie. the second waypoint
%Compute all the angles between two successive waypoints
%The angles returned are between -pi and pi
for j=1:7
a_list(mod(j,7)+1,1) = angle(complex(x_list(mod(j,7)+1)-x_list(j), y_list(mod(j,7)+1)-y_list(j)));
if a_list(j,1) < 0
a_list(j,1) = a_list(j,1);
end
end
%Objectives list containing X,Y and Theta coordinates
r_list = [x_list y_list a_list];
nb_obj = size(r_list,1); %Number of objectives
%MPC horizons
nu = 2; %Control horizon (dof of the optimization problem)
ny = 30; %Prediction horizon
%State-space system used for the MPC
a(:,i) = [0 0 1.4181]'; %Initial conditions : the boat is in the correct orientation
%And has null angular speed and acceleration
u(:,i) = [k/2 k/2]'; %Initial condition on the command : to maintain a speed of 1m/s
delta_u(:,i) = [0 0]'; %Initial condition on the input increments
%System matrices
A = [1 0 0;... %State matrix
Te 1 0;...
0 Te 1];
B = [D/(2*I) -D/(2*I); 0 0; 0 0]; %Input matrix
%Augmented system matrices
A_tilde = [A B; zeros(2,3), eye(2)]; %State matrix
B_tilde = [zeros(3,2); eye(2)]; %Input matrix
C_tilde = [0 0 1 0 0]; %Output matrix
n_output = size(C_tilde,1); %Dimension of the output
n_input = size(B,2); %Number of inputs
n_state = size(B,1); %Number of state variables
%State-space system used to provide an angle reference vector
x(:,i) = [2 2 1.4181 0 0 1 0 0 0]'; %State initial condition : the boat is in the first waypoint
%and correctly oriented
Fx = [1 0 0 0 0 Te*cos(x(3,i)) 0 -Te*sin(x(3,i)) 0;... %Linearized state matrix
0 1 0 0 0 Te*sin(x(3,i)) 0 Te*cos(x(3,i)) 0;...
0 0 1 Te 0 0 0 0 0;...
0 0 0 1 Te 0 0 0 0;...
0 0 0 0 1 0 0 0 0;...
0 0 0 0 0 1 Te 0 0;...
0 0 0 0 0 -k/m 1 0 0;...
0 0 0 0 0 0 0 1 Te;...
0 0 0 0 0 0 0 -ky/m 1];
Fu = [0 0; 0 0; 0 0; 0 0;... %Linearized input matrix
D/(2*I) -D/(2*I); 0 0; 1/m 1/m; 0 0; 0 0];
Dx = -0.02;
Dy = 0.01;
alpha = angle(complex(Dx,Dy));
Dk = [0 0 0 0 0 0 (1/m)*Dx*cos(alpha-x(3,i)) 0 (1/m)*Dy*sin(alpha-x(3,i))]';
r = zeros(n_output*ny,1); %Angle reference vector
%Matrices used for optimization
gainS = computeGainS(n_output, 1); %Steady-state gain
gainQ = computeGainQ(n_output, 1); %Running cost gain on the output
gainR = computeGainR(2, 1); %Running cost gain on the input
Qbar = computeQbar(C_tilde, gainQ, gainS, ny);
Tbar = computeTbar(gainQ, C_tilde, gainS, ny);
Rbar = computeRbar(gainR, nu);
Cbar = computeCbar(A_tilde, B_tilde, ny, nu);
Abar = computeAbar(A_tilde, ny);
Hbar = computeHbar(Cbar, Qbar, Rbar);
Fbar = computeFbar(Abar, Qbar, Cbar, Tbar);
%Quadprog solver options
%Keep the solver quiet
options = optimoptions('quadprog');
options = optimoptions(options, 'Display', 'off');
for t=0:Te:(Tfinal-1)
a_tilde = [a(:,i); u(:,i)]; %Build the augmented state space vector for MPC
%Compute the ny next angle objectives
tmp = x(:,i); %Get the current state
obj = current_obj; %Get the current objective
j = 1;
while(j <= ny)
%Compute the angle of the line between the boat and the next waypoint
goal_angle = angle(complex(r_list(obj,1)-tmp(1,1), r_list(obj,2)-tmp(2,1)));
%If the distance between the goal angle and the current angle is
%greater than pi, it means that there is a shorter path to this angle
%than getting all the way around the unit circle
dist = abs(goal_angle - tmp(3,1));
if dist > pi
if tmp(3,1) < 0
goal_angle = tmp(3,1) - (2*pi - dist);
else
goal_angle = tmp(3,1) + (2*pi - dist);
end
end
%Considering an optimal orientation of the boat and equals inputs
%compute the Ny next boat position.
%If the boat is not yet arrived to the waypoint put the goal_angle into
%the reference vector, else recompute the goal angle.
tmp(3,1) = goal_angle;
Fx = [1 0 0 0 0 Te*cos(tmp(3,1)) 0 -Te*sin(tmp(3,1)) 0;... %Linearized state matrix
0 1 0 0 0 Te*sin(tmp(3,1)) 0 Te*cos(tmp(3,1)) 0;...
0 0 1 Te 0 0 0 0 0;...
0 0 0 1 Te 0 0 0 0;...
0 0 0 0 1 0 0 0 0;...
0 0 0 0 0 1 Te 0 0;...
0 0 0 0 0 -k/m 1 0 0;...
0 0 0 0 0 0 0 1 Te;...
0 0 0 0 0 0 0 -ky/m 1];
Dk = [0 0 0 0 0 0 (1/m)*Dx*cos(alpha-tmp(3,1)) 0 (1/m)*Dy*sin(alpha-tmp(3,1))]';
tmp = Fx*tmp + Fu*[k/2;k/2] + Dk; %Make a step forward
%Have we reached the next waypoint ?
if(tmp(1) > r_list(obj,1)-0.5 && tmp(1) < r_list(obj,1)+0.5 && ...
tmp(2) > r_list(obj,2) - 0.5 && tmp(2) < r_list(obj,2) + 0.5)
%If yes, recompute the goal angle and update the jacobian
obj = mod(obj,7) + 1;
goal_angle = angle(complex(r_list(obj,1)-tmp(1,1), r_list(obj,2)-tmp(2,1)));
dist = abs(goal_angle - tmp(3,1));
if dist > pi
if tmp(3,1) < 0
goal_angle = tmp(3,1) - (2*pi - dist);
else
goal_angle = tmp(3,1) + (2*pi - dist);
end
end
tmp(3,1) = goal_angle;
Fx = [1 0 0 0 0 Te*cos(tmp(3,1)) 0 -Te*sin(tmp(3,1)) 0;... %Linearized state matrix
0 1 0 0 0 Te*sin(tmp(3,1)) 0 Te*cos(tmp(3,1)) 0;...
0 0 1 Te 0 0 0 0 0;...
0 0 0 1 Te 0 0 0 0;...
0 0 0 0 1 0 0 0 0;...
0 0 0 0 0 1 Te 0 0;...
0 0 0 0 0 -k/m 1 0 0;...
0 0 0 0 0 0 0 1 Te;...
0 0 0 0 0 0 0 -ky/m 1];
Dk = [0 0 0 0 0 0 (1/m)*Dx*cos(alpha-tmp(3,1)) 0 (1/m)*Dy*sin(alpha-tmp(3,1))]';
end
r(j,1) = goal_angle;
j = j + 1;
end
a_ref(1,i) = r(1,1);
%Constraints related matrices
Acons = computeCy(ny,n_state+n_input)*Cbar;
Bcons = computeDy([-100 -100 -2*pi 1*k/2 1*k/2]', [100 100 2*pi 1.2*k/2 1.2*k/2]', ny) - ...
computeCy(ny,n_state+n_input)*Abar*a_tilde;
%Solve the optimization problem, if impossible to solve : break and
%display the results
[delta_uFut, fval, exitflag] = quadprog(Hbar/2+Hbar'/2,([a_tilde; r]'*Fbar)',Acons,Bcons,[],[],[],[],[],options);
if(exitflag == -2)
break;
end
delta_u(:,i) = delta_uFut([1 2],1); %Get the input increment to apply
u(:,i) = u(:,i) + delta_u(:,i); %Compute the input
a(:,i+1) = A*a(:,i) + B*u(:,i); %Compute the next state for the MPC state-space
x(3,i) = a(3,i); %Make the angles equal between both systems
Fx = [1 0 0 0 0 Te*cos(x(3,i)) 0 -Te*sin(x(3,i)) 0;... %Linearized state matrix
0 1 0 0 0 Te*sin(x(3,i)) 0 Te*cos(x(3,i)) 0;...
0 0 1 Te 0 0 0 0 0;...
0 0 0 1 Te 0 0 0 0;...
0 0 0 0 1 0 0 0 0;...
0 0 0 0 0 1 Te 0 0;...
0 0 0 0 0 -k/m 1 0 0;...
0 0 0 0 0 0 0 1 Te;...
0 0 0 0 0 0 0 -ky/m 1];
Dk = [0 0 0 0 0 0 (1/m)*Dx*cos(alpha-x(3,i)) 0 (1/m)*Dy*sin(alpha-x(3,i))]';
x(:,i+1) = Fx*x(:,i)+ Fu*u(:,i) + Dk; %Compute the next state for the position system
i = i + 1;
%Had we past the current waypoint
if (x(1,i) < r_list(current_obj,1) + 1 && x(1,i) > r_list(current_obj,1) - 1) && ...
(x(2,i) < r_list(current_obj,2) + 1 && x(2,i) > r_list(current_obj,2) - 1)
%If the current objective was the last : godd job, we're done
%Break and display the results
if current_obj == nb_obj
break;
end
%Else increment the current_obj
current_obj = current_obj + 1;
end
end
%Plot the boat trajectory
subplot(4,3,[1 4]);
plot(x(1,1:i), x(2,1:i), 'k');
l = 1;
h = 1.5;
for j=1:i
if mod(j,20) == 0
line([x(1,j)-(h/2)*cos(x(3,j))+(l/2)*sin(x(3,j)) x(1,j)-(h/2)*cos(x(3,j))-(l/2)*sin(x(3,j))],...
[x(2,j)-(h/2)*sin(x(3,j))-(l/2)*cos(x(3,j)) x(2,j)-(h/2)*sin(x(3,j))+(l/2)*cos(x(3,j))]);
line([x(1,j)-(h/2)*cos(x(3,j))+(l/2)*sin(x(3,j)) x(1,j)+(h/2)*cos(x(3,j))],...
[x(2,j)-(h/2)*sin(x(3,j))-(l/2)*cos(x(3,j)) x(2,j)+(h/2)*sin(x(3,j))]);
line([x(1,j)-(h/2)*cos(x(3,j))-(l/2)*sin(x(3,j)) x(1,j)+(h/2)*cos(x(3,j))],...
[x(2,j)-(h/2)*sin(x(3,j))+(l/2)*cos(x(3,j)) x(2,j)+(h/2)*sin(x(3,j))]);
end
end
%Plot the waypoints circles of acceptance
hold on
theta = 0:0.01:2*pi;
rayon = 1;
X = rayon*cos(theta);
Y = rayon*sin(theta);
for j=1:7
plot(X + r_list(j,1),Y + r_list(j,2), 'r')
end
quiver(20,5,Dx,Dy,800, 'm-', 'LineWidth',1);
hold off
axis([-1 43 -8 20]);
%Plot the first input
subplot(4,3,2);
plot(1:i, u(1,1:i), 'b');
axis([0 round(i,-3) 0 0.08]);
%Plot the second input
subplot(4,3,5);
plot(1:i, u(2,1:i), 'r');
axis([0 round(i,-3) 0 0.08]);
%Plot the angle from the "x" state vector
subplot(4,3,[7 10]);
plot(1:i, x(3,1:i));
%Plot the velocity
subplot(4,3,[3 6]);
plot(1:i, x(6,1:i));
axis([0 round(i,-3) 0 1.5]);
%Plot the angle from the "a" state vector
subplot(4,3,[8 11]);
plot(1:i, a(3,1:i));
hold on
plot(1:i, a_ref(1,1:i));
hold off
subplot(4,3,[9 12]);
plot(1:i, x(8,1:i));

Comment ( 0 )

Sign in for post a comment

Matlab
1
https://gitee.com/jeffreychan/mpc.git
git@gitee.com:jeffreychan/mpc.git
jeffreychan
mpc
mpc
master

Search