使用模型预测控制对USV进行自主控制（Matlab代码实现）-Toy模板网

这篇具有很好参考价值的文章主要介绍了使用模型预测控制对USV进行自主控制（Matlab代码实现）。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

💥1 概述

📚2 运行结果

🎉3 参考文献

👨‍💻4 Matlab代码

💥1 概述

无人船(unmanned surface vehicles,USV)是一种船端无人操控的水面船舶,近年来受到了广泛关注。如何实现自主航行是USV面临的核心问题,而设计一种具有精确航迹控制能力的运动控制器是解决该问题的基础。

本文介绍了一种模型预控制（MPC）算法，旨在自动驾驶无人水面车辆（USV）驶向一组航路点。该算法的设计被认为对海洋环境中遇到的环境扰动具有鲁棒性。USV和扰动的建模已经简化，因为这项工作旨在证明概念：对于现实世界的实施，应该考虑更准确的建模。

📚2 运行结果

使用模型预测控制对USV进行自主控制（Matlab代码实现）

主函数部分代码：

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)
Tfinal = 150;   %Total simulation time
Te = 0.1;       %Sampling period

%Vectors used to hold simulation data
x = zeros(7, 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
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]';         %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;...    %Linearized state matrix
      0 1 0 0 0 Te*sin(x(3,i)) 0;...
      0 0 1 Te 0 0 0;...
      0 0 0 1 Te 0 0;...
      0 0 0 0 1  0 0;...
      0 0 0 0 0  1 Te;...
      0 0 0 0 0 -k/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];

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
    
    %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