系统无法完成crossing_strait的预期效果
如图为u gamma的预期结果
代码如下
clear
clc
close all
% 定义状态变量 Table1.2参数
params.k11 = 1;
params.k12 = 1;
params.k13 = 1;
params.ke = 50;
params.s1 = -10;
params.s2 = -0.05;
params.C1 = -1;
params.s3 = 10;
params.s4 = -0.05;
params.C2 = 1;
params.t1 = 12.26;
params.tf = 58;
params.th=58;%12.26-58s 对应Y轴不等式约束的第一部分
params.td=260; %暂定的仿真结束时间 58-td 对应Y轴不等式约束的第二部分
params.k21 = 10;
params.k23 = 10;
params.ki = 100;
params.d21 = -1;
params.d22 = -1;
params.c1 = 10;
params.c2 = 1;
params.c3 = 1;
params.m_n = 23.8;
params.Xu_n = -0.72253;
params.Nv_n = 0.0313;
params.Yv1_n = -10;
params.Ngamma1_n = -1;
params.Iz_n = 1.76;
params.Yv_n = -0.88965;
params.Ngamma_n = -1.9;
params.Ygamma1_n = -0.1;
params.xg_n = 0.046;
params.Ygamma_n = -7.25;
params.Xu1_n = -2;
params.Nv1_n = 0;
%定义 假设5中的参数（tau31.32）
params.alpha1 = 1; %暂定
params.alpha2 = 2;
params.alpha3 = 3;
params.alphatemp = [params.alpha1,params.alpha2,params.alpha3];
params.alpha = max(params.alphatemp);
%初始状态
xu = 0; yv = 0; psigamma = 0;
x = 0; y = 0; psi = 0;
u = 5; v = 0; gamma = 1;
%初始状态(body-fixed) 用于ode函数
initial_conditions1 = [xu, yv, psigamma, u, v, gamma];%uvgamma
%仿真时间范围（圆形轨迹侦察阶段）
tspan1 = [0 params.t1]; % t1 = 12.26
tspan2 = [params.t1 params.td];
%使用ode45进行仿真
options = odeset('RelTol',1e-5, 'AbsTol',1e-6, 'MaxStep', 1);
[T1, Y1] = ode45(@(t,state)reconnaissance(t, state, params),tspan1, initial_conditions1);
initial_conditions2 = Y1(end, :);%更新初始条件为侦察阶段结束时的状态
[T2, Y2] = ode45(@(t,state)crossing_strait(t, state, params), tspan2, initial_conditions2);
T = [T1; T2 + T1(end)];
Y = [Y1; Y2]; % 状态变量
xu = Y(:, 1);
yv = Y(:, 2);
psigamma = Y(:, 3);
u = Y(:, 4);
v = Y(:, 5);
gamma = Y(:, 6);
crossing_strait函数
function dstate = crossing_strait (t,state,params)
xu = state(1);
yv = state(2);
psigamma = state(3);
u = state(4);
v = state(5);
gamma = state(6);
theta = [u,v,gamma]';%下文矩阵用使用
%读取参数
m_n = params.m_n;
Xu_n = params.Xu_n;
Nv_n = params.Nv_n;
Yv1_n = params.Yv1_n;
Ngamma1_n = params.Ngamma1_n;
Iz_n = params.Iz_n;
Yv_n = params.Yv_n;
Ngamma_n = params.Ngamma_n;
Ygamma1_n = params.Ygamma1_n;
xg_n = params.xg_n;
Ygamma_n = params.Ygamma_n;
Xu1_n = params.Xu1_n;
Nv1_n = params.Nv1_n;
m = m_n+0.2*m_n*sin(t);
Xu = Xu_n+0.2*Xu_n*sin(t);
Nv = Nv_n+0.2*Nv_n*sin(t);
Yv1 = Yv1_n+0.2*Yv1_n*sin(t);
Ngamma1 = Ngamma1_n+0.2*Ngamma1_n*sin(t);
Iz = Iz_n+0.2*Iz_n*sin(t);
Yv = Yv_n+0.2*Yv_n*sin(t);
Ngamma = Ngamma_n+0.2*Ngamma_n*sin(t);
Ygamma1 = Ygamma1_n+0.2*Ygamma1_n*sin(t);
xg = xg_n+0.2*xg_n*sin(t);
Ygamma = Ygamma_n+0.2*Ygamma_n*sin(t);
Xu1 = Xu1_n+0.2*Xu1_n*sin(t);
Nv1 = Nv1_n+0.2*Nv1_n*sin(t);
%define matrix parameter
C13 = -(m-Yv1)*v-(1/2)*(m*xg-Ygamma1+m*xg-Nv1)*gamma;
C13_n = -(m_n-Yv1_n)*v-(1/2)*(m_n*xg_n-Ygamma1_n+m_n*xg_n-Nv1_n)*gamma;
C23 = -(m-Xu1)*u;
C23_n = -(m_n-Xu1_n)*u;
R=[1 0 0;0 1 0;0 0 1];%此阶段的旋转矩阵为单位阵
M=[m-Xu1 0 0;0 m-Yv1 m*xg-Ygamma1;0 m*xg-Nv1 Iz-Ngamma1];%惯性质量矩阵
M_n=[m_n-Xu1_n 0 0;0 m_n-Yv1_n m_n*xg_n-Ygamma1_n;0 m_n*xg_n-Nv1_n Iz_n-Ngamma1_n];
C=[0 0 C13;0 0 C23;-C13 -C23 0];%Coriolis matrix
C_n = [0 0 C13_n;0 0 C23_n;-1*C13_n -1*C23_n 0];
D=[-Xu 0 0;0 -Yv -Ygamma;0 -Nv -Ngamma];%hydrodynamic damping matrix
D_n = [-Xu_n 0 0;0 -Yv_n -Ygamma_n;0 -Nv_n -Ngamma_n];
%Y轴上的洋流扰动
%disturbance = 200 * sin(t);
dxu = u;
dyv = v;
dpsigamma = gamma;
%约束方程
Ai = [params.k21 0 0;0 0 params.k23]; %2*3
ci = [-(xu-t)/params.k21+1;-psigamma/params.k23]; %2*1
bi = [-(dxu-1)/params.k21;-dpsigamma/params.k23]; %2*1
Bi = Ai*Ai'; %2*2可逆
betai = Ai*theta-ci; %2*1
%Piei = [norm(theta)^2;norm(theta);1 ]; %3*1 未知
Piei = (norm(theta)+1)^2;
Gamma = eye(2); %暂定 此处修改为2*2
tau12_1=sqrtm(M_n);
tau12_2=Ai/sqrtm(M_n);
tau12 = tau12_1*pinv(tau12_2)*(bi+Ai/M_n*(C_n*theta+D_n*theta)); %3*1
tau22 = -params.ki*M_n*Ai'/Bi/Gamma*betai; %3*1
tau32 =-1*(M_n*Ai'/Bi/Gamma)*betai*norm(Piei)^2; %3*1
%定义约束力
Qe2 = tau12+tau22+tau32; %3*1;
dstate_temp2= inv(M)*(-C*theta-D*theta+Qe2); %3*1;
du = dstate_temp2(1);
dv = dstate_temp2(2);
dgamma = dstate_temp2(3);
dstate= zeros(6,1);
dstate(1) = dxu;
dstate(2) = dyv;
dstate(3) = dpsigamma;
dstate(4) = du;
dstate(5) = dv; %涉及不等式约束
dstate(6) = dgamma;
end
