AUV路径跟踪滑模控制

一、参考文献

Modelling and Motion Control of an Underactuated Autonomous Underwater Vehicle

Principles of Guidance-Based Path Following in 2D and 3D 

二、仿真效果

 

 

 

 

三、源代码

（1）AUV模型

%position=[x,y,z,Fai,Theta,Psi]';
%velocity=[u,v,w,p,q,r]';
%thruster=[port,starvoard,bow,stern]';
%force=[X,Y,Z,K,M,N]';
function acceleration=AUV_model(position,velocity,thruster)
global M;
global C;
global D;
global B;
global J;
% x=position(1);
% y=position(2);
% z=position(3);
Fai=position(4);
Theta=position(5);
Psi=position(6);

J_1=[cos(Psi)*cos(Theta),-sin(Psi)*cos(Fai)+cos(Psi)*sin(Theta)*sin(Fai),sin(Psi)*cos(Fai)+cos(Psi)*sin(Theta)*cos(Fai);
     sin(Psi)*cos(Theta),cos(Psi)*cos(Fai)+sin(Psi)*sin(Theta)*sin(Fai),sin(Psi)*sin(Theta)*cos(Fai)-cos(Psi)*cos(Fai);
     -sin(Theta),cos(Theta)*sin(Fai),cos(Theta)*cos(Fai)];

J_2=[1,tan(Theta)*sin(Fai),tan(Theta)*cos(Fai);
     0,cos(Fai),-sin(Fai);
     0,cos(Fai)/cos(Theta),cos(Fai)/cos(Theta)];

zero_3=zeros(3,3);

J=[J_1,zero_3;
   zero_3,J_2];

u=velocity(1);
v=velocity(2);
w=velocity(3);
p=velocity(4);
q=velocity(5);
r=velocity(6);

m=60;
I_xx=5.4;
I_yy=15.5;
I_zz=15.5;

X_dot_u=-6.9;
Y_dot_v=-120.2;
Z_dot_w=-120.2;
K_dot_p=0;
M_dot_q=-89.53;
N_dot_r=-89.6;
Y_dot_r=89.83;
Z_dot_q=-89.83;
M_dot_w=-89.83;
N_dot_v=89.83;

%mass matrix
M_RB=diag([m,m,m,I_xx,I_yy,I_zz]);
M_A=-[X_dot_u,0,0,0,0,0;
      0,Y_dot_v,0,0,0,Y_dot_r;
      0,0,Z_dot_w,0,Z_dot_q,0;
      0,0,0,K_dot_p,0,0;
      0,0,M_dot_w,0,M_dot_q,0;
      0,N_dot_v,0,0,0,N_dot_r];
M=M_RB+M_A;
%Coriolis-centripetal matrix

C_RB=[0,0,0,0,m*w,-m*v;
      0,0,0,-m*w,0,m*u;
      0,0,0,m*v,-m*u,0;
      0,m*w,-m*v,0,I_zz*r,-I_yy*q;
      -m*w,0,m*u,-I_zz*r,0,I_xx*p;
      m*v,-m*u,0,I_yy*q,-I_xx*p,0];
a_b=M_A*velocity;
C_A=[0,0,0,0,-a_b(3),a_b(2);
     0,0,0,a_b(3),0,-a_b(1);
     0,0,0,-a_b(2),a_b(1),0;
     0,-a_b(3),a_b(2),0,-a_b(6),a_b(5);
     a_b(3),0,-a_b(1),a_b(6),0,-a_b(1);
     -a_b(2),a_b(1),0,-a_b(5),a_b(4),0];
C=C_RB+C_A;

%drag matrix
X_uu=8.038;
Y_vv=252;
Z_ww=252;
K_pp=0;
M_qq=246.1;
N_rr=246.1;

D=diag([X_uu,Y_vv,Z_ww,K_pp,M_qq,N_rr]);

l_1=0.21;
l_2=0.21;
l_3=0.8;
l_4=0.8;
B=[1,1,0,0;
   0,0,0,0;
   0,0,1,1;
   0,0,0,0;
   0,0,l_3,-l_4;
   l_1,-l_2,0,0];

tao=B*thruster;

acceleration=M\(tao-C*velocity-D*velocity);
end

（2）引导算法

%curve_value=[0];
%ship_position=[x,y,z,Fai,Theta,Psi]';
%ship_velocity=[u,v,w,p,q,r]';
function [curve_update,expect_azimuth_angle,expect_elevation_angle]=spital_los(curve_parameter,curve_value,position,velocity)
%parameter curve--spiral line
a=curve_parameter(1);
b=curve_parameter(2);
h=curve_parameter(3);
%los parameter
delta_e=0.5;
delta_h=0.5;
gamma=0.9;
%algorithm
curve_position=[b*sin(curve_value),a*cos(curve_value),h*curve_value/(2*pi)]';
d_curve_position=[b*cos(curve_value),-a*sin(curve_value),h/(2*pi)]';
curve_azimuth_angle=atan2(d_curve_position(2),d_curve_position(1));
curve_elevation_angle=atan2((-d_curve_position(3)),(d_curve_position(1)^2+d_curve_position(2)^2)^0.5);

rotation_azimuth_matrix=[cos(curve_azimuth_angle),-sin(curve_azimuth_angle),0;
                         sin(curve_azimuth_angle),cos(curve_azimuth_angle),0;
                         0,0,1];
rotation_elevation_matrix=[cos(curve_elevation_angle),0,sin(curve_elevation_angle);
                           0,1,0;
                           -sin(curve_elevation_angle),0,cos(curve_elevation_angle)];
rotation_matrix=rotation_azimuth_matrix*rotation_elevation_matrix;

error=rotation_matrix'*(position(1:3)-curve_position(1:3));
%output
curve_update=(norm(velocity(1:3))*cos(position(5)-curve_azimuth_angle)*cos(position(6)-curve_elevation_angle)+gamma*error(1))/...
              norm(d_curve_position);
expect_azimuth_angle=curve_azimuth_angle+atan2(-error(2),delta_e);
expect_elevation_angle=curve_elevation_angle+atan2(error(3),delta_h);

if expect_azimuth_angle>pi
    expect_azimuth_angle=expect_azimuth_angle-2*pi;
end
if expect_azimuth_angle<-pi
    expect_azimuth_angle=2*pi+expect_azimuth_angle;
end
if expect_elevation_angle>pi
    expect_elevation_angle=expect_elevation_angle-2*pi;
end
if expect_elevation_angle<-pi
    expect_elevation_angle=2*pi+expect_elevation_angle;
end

end

（3）滑模控制

%position=[x,y,z,Fai,Theta,Psi]';
%velocity=[u,v,w,p,q,r]';
function thruster=AUV_slide_model(position,velocity,position_d,velocity_d,d_position_d,d_velocity_d)

global M;
global C;
global D;
global B;
global J;

% x=position(1);
% y=position(2);
% z=position(3);
% Fai=position(4);
Theta=position(5);
Psi=position(6);
 
u=velocity(1);
% v=condition(2);
w=velocity(3);
% p=condition(4);
q=velocity(5);
r=velocity(6);

% x_d=position_d(1);
% y_d=position_d(2);
% z_d=position_d(3);
% Fai_d=position_d(4);
Theta_d=position_d(5);
Psi_d=position_d(6);

if (abs(Theta_d)>pi/2 && Theta_d*Theta<0)
    if Theta_d<Theta
        Theta_d=Theta_d+2*pi;
    else
        Theta_d=Theta_d-2*pi;
    end
end
if (abs(Psi_d)>pi/2 && Psi_d*Psi<0)
    if Psi_d<Psi
        Psi_d=Psi_d+2*pi;
    else
        Psi_d=Psi_d-2*pi;
    end
end

u_d=velocity_d(1);
% v_d=condition_d(2);
w_d=velocity_d(3);
% p_d=condition_d(4);
q_d=velocity_d(5);
r_d=velocity_d(6);

X=[u,w,q,r,Theta,Psi]';
X_d=[u_d,w_d,q_d,r_d,Theta_d,Psi_d]';
d_X_d=[d_velocity_d(1),d_velocity_d(3),d_velocity_d(5),d_velocity_d(6),d_position_d(5),d_position_d(6)]';

F_X_1=-M\(C*velocity+D*velocity);
F_X_2=J*velocity;
F_X_1(4)=[];
F_X_1(2)=[];
F_X_2(1:4)=[];
F_X=[F_X_1;F_X_2];

B_1=M\B;
B_1(4,:)=[];
B_1(2,:)=[];
B_=[B_1;zeros(2,4)];

Lambda_Theta=0.2;
Lambda_Fai=0.2;
Lambda=[1,0,0,0,0,0;
        0,1,0,0,0,0;
        0,0,1,0,Lambda_Theta,0;
        0,0,0,1,0,Lambda_Fai];

K=diag([1.8,1.2,0.8,0.8]);

S=Lambda*(X-X_d);

U_eq=(Lambda*B_)\(Lambda*d_X_d-Lambda*F_X);
U_s=-(Lambda*B_)\(K*sat(S));
U=U_eq+U_s;
thruster=U;

end

%% sat/sgn/tanh
function value=sat(s)
    delta=0.2;
    value=zeros(length(s),1);
    for i=1:length(s)
        if s(i)>delta
            value(i)=1;
        elseif s(i)<-delta
            value(i)=-1;
        else
            value(i)=s(i)/delta;
        end
    end
end

（4） 主函数

clear;clc;close all;
%const parameter
delta_t=0.1;
global J;
%parameter curve--spiral line
a=10;
b=10;
h=-4;
curve_parameter=[a,b,h];
t=0:0.1:10*pi;

%north-east coordinate
% x=r*cos(t);
% y=r*sin(t);
y=a*cos(t);
x=b*sin(t);
z=h*t/(2*pi);

curve_value=0;
%parameter ship
position=[0,5,0,0,0,0]';%[x,y,z,Fai,Theta,Psi]
velocity=[0,0,0,0,0,0]';%[u,v,w,p,q,r]
thruster=[0,0,0,0]';
acceleration=AUV_model(position,velocity,thruster);

%befroe
curve_value_before=curve_value;
expect_elevation_angle_before=position(5);
expect_azimuth_angle_before=position(6);
expect_elevation_angle_before_b=expect_elevation_angle_before;
expect_azimuth_angle_before_b=expect_azimuth_angle_before;

%storage
position_storage=[];
position_d_storage=[];
%circle
iteration=0;
while iteration~=1200
    position_storage=[position_storage,position];
    
    [curve_update,expect_azimuth_angle,expect_elevation_angle]=spital_los(curve_parameter,curve_value,position,velocity);
    curve_value=curve_value+curve_update*delta_t;
    position_d=[b*sin(curve_value),a*cos(curve_value),h*curve_value/(2*pi),...
                0,expect_elevation_angle,expect_azimuth_angle]';
%     position_d_before=[b*sin(curve_value_before),a*cos(curve_value_before),h*curve_value_before/(2*pi),...
%                        0,expect_azimuth_angle_before,expect_elevation_angle_before]';
    %angle change
    if(expect_elevation_angle*expect_elevation_angle_before<0 && abs(expect_elevation_angle)>pi/2)
        if(expect_elevation_angle>expect_elevation_angle_before)
            expect_elevation_angle_before=expect_elevation_angle_before+2*pi;
        else
            expect_elevation_angle_before=expect_elevation_angle_before-2*pi;
        end
    end
    if(expect_azimuth_angle*expect_azimuth_angle_before<0 && abs(expect_azimuth_angle)>pi/2)
        if(expect_azimuth_angle>expect_azimuth_angle_before)
            expect_azimuth_angle_before=expect_azimuth_angle_before+2*pi;
            expect_azimuth_angle_before_b=expect_azimuth_angle_before_b+2*pi;
        else
            expect_azimuth_angle_before=expect_azimuth_angle_before-2*pi;
            expect_azimuth_angle_before_b=expect_azimuth_angle_before_b-2*pi;
        end
    end

    velocity_d=[1,0,0,0,...
                (expect_elevation_angle-expect_elevation_angle_before)/delta_t,...
                (expect_azimuth_angle-expect_azimuth_angle_before)/delta_t]';
%     velocity_d=[1,0,0,0,...
%                 0,...
%                 0]';
    d_position_d=velocity_d;
    d_velocity_d=[0,0,0,0,...
                  (expect_elevation_angle-2*expect_elevation_angle_before+expect_elevation_angle_before_b)/delta_t,...
                  (expect_azimuth_angle-2*expect_azimuth_angle_before+expect_azimuth_angle_before_b)/delta_t]';
%     d_velocity_d=[0,0,0,0,...
%                   0,...
%                   0]';
    thruster=AUV_slide_model(position,velocity,position_d,velocity_d,d_position_d,d_velocity_d);
    
    %position iterate
    acceleration=AUV_model(position,velocity,thruster);
    velocity=velocity+acceleration*delta_t;
    position(5:6)=position(5:6)+velocity(5:6)*delta_t;
    position(1:3)=position(1:3)+J(1:3,1:3)*velocity(1:3)*delta_t;
    
    if position(5)>pi 
        position(5)=position(5)-2*pi;
    end
    if position(5)<-pi 
        position(5)=2*pi+position(5);
    end
    if position(6)>pi 
        position(6)=position(6)-2*pi;
    end
    if position(6)<-pi 
        position(6)=2*pi+position(6);
    end
    position_d_storage=[position_d_storage,position_d];
    %before
    curve_value_before=curve_value;
    expect_azimuth_angle_before_b=expect_azimuth_angle_before;
    expect_elevation_angle_before_b=expect_elevation_angle_before;
    expect_azimuth_angle_before=expect_azimuth_angle;
    expect_elevation_angle_before=expect_elevation_angle;

    iteration=iteration+1;
end
%draw
figure(1)
plot3(y,x,z);
axis equal;grid on;hold on;
plot3(position_storage(2,:),position_storage(1,:),position_storage(3,:),'r-');
figure(2)
axis_x=0:length(position_d_storage)-1;
plot(axis_x,position_d_storage(5,:),'b-');
hold on;
plot(axis_x,position_storage(5,:),'r-');
xlabel("t");
ylabel("error_Theta");
figure(3)
plot(axis_x,position_d_storage(6,:),'b-');
hold on;
plot(axis_x,position_storage(6,:),'r-');
xlabel("t");
ylabel("error_Psi");