AUV路径跟踪滑模控制

本文主要是介绍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 matrixC_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>piexpect_azimuth_angle=expect_azimuth_angle-2*pi;
end
if expect_azimuth_angle<-piexpect_azimuth_angle=2*pi+expect_azimuth_angle;
end
if expect_elevation_angle>piexpect_elevation_angle=expect_elevation_angle-2*pi;
end
if expect_elevation_angle<-piexpect_elevation_angle=2*pi+expect_elevation_angle;
endend

（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<ThetaTheta_d=Theta_d+2*pi;elseTheta_d=Theta_d-2*pi;end
end
if (abs(Psi_d)>pi/2 && Psi_d*Psi<0)if Psi_d<PsiPsi_d=Psi_d+2*pi;elsePsi_d=Psi_d-2*pi;end
endu_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)>deltavalue(i)=1;elseif s(i)<-deltavalue(i)=-1;elsevalue(i)=s(i)/delta;endend
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~=1200position_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 changeif(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;elseexpect_elevation_angle_before=expect_elevation_angle_before-2*pi;endendif(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;elseexpect_azimuth_angle_before=expect_azimuth_angle_before-2*pi;expect_azimuth_angle_before_b=expect_azimuth_angle_before_b-2*pi;endendvelocity_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 iterateacceleration=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;endif position(5)<-pi position(5)=2*pi+position(5);endif position(6)>pi position(6)=position(6)-2*pi;endif position(6)<-pi position(6)=2*pi+position(6);endposition_d_storage=[position_d_storage,position_d];%beforecurve_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");

这篇关于AUV路径跟踪滑模控制的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！