基于Matlab实现牛顿迭代计算制导和滑模控制制导仿真（源码）.rar

%% 滑模控制2用于制导 % 原模型是二体相对运动，这道题简化为目标是静止的，导弹从高处往下投 % 为简化计算过程，预先将目标物相关量置0 % 假设控制系统无滞后am = ac % ac只用于改变导弹速度方向，不改变导弹速度大小 % TODO: 加速度图与PPT不同 clear all; clc; %% 参数初始化 % dX = AX + Bc*ac + Bt*at % 普通滑膜s=x2+c(x1-lambdaD), ds=-k*s-epsilon*sgn(s) % 自适应滑膜s=x2+k1*(x1-lambdaD)/tgo, ds=[-k2*s-kappa*sgn(s)]/tgo gainc = 0.2; gaink = 0.2; epsilon = 0.002; gaink1 = 1; gaink2 = 1; kappa = 0.01; disx = 6000; %导弹与目标水平距离(m) disy = 3000; %导弹与目标高度距离(m) disr = norm([disx, disy]); %导弹与目标的直线距离(m) velM = 260; %导弹初始速度(m/s) thetaM = 0; %导弹速度与水平夹角(rad) etaM = -atan(disy/disx); %导弹速度与MT连线夹角(rad) accelM = 0; %导弹加速度(m/s^2) lambda = -atan(disy/disx); %视线角(rad) lambdaDesire = -30/180*pi; %期望视线角(rad) dLambda = velM*sin(etaM)/disr; X = [lambda; dLambda]; %初始状态 dt = 0.01; %仿真步长(s) T = 30; %仿真周期(s) %% 画图记录量 time = 0:dt:T; xyLog1 = zeros(2, T/dt+1); xyLog2 = zeros(2, T/dt+1); accelLog = zeros(2, T/dt+1); lambdaLog = zeros(2, T/dt+1); sLog = zeros(2, T/dt+1); %% 普通滑模控制 for i = 1:1:T/dt+1 % 滑模控制量s和加速度accelC dDisr = -velM*cos(etaM); matA = [0, 1; 0, -2*dDisr/disr]; matBc = [0; -cos(etaM)/disr]; matC = [gainc, 1]; s = matC*X - matC(1)*lambdaDesire; accelC = (-matC*matA*X -epsilon*sat(s) -gaink*s) / (matC*matBc); % 状态方程 dX = matA*X + matBc*accelC; % 状态递推 X = X + dX*dt; lambda = X(1); dLambda = X(2); accelM = accelC; dThetaM = accelM / velM; thetaM = thetaM + dThetaM*dt; etaM = lambda - thetaM; disr = disr -velM*cos(etaM)*dt; if disr <= 0 disr = 0; % 写入剩下的数 for j = i:1:T/dt+1 xyLog1(:, j) = [disr*cos(lambda); disr*sin(lambda)]; accelLog(1, j) = accelM; lambdaLog(1, j) = lambda*180/pi; sLog(1, j) = s; end break; end % 记录 xyLog1(:, i) = [-disr*cos(lambda); -disr*sin(lambda)]; accelLog(1, i) = accelM; lambdaLog(1, i) = lambda*180/pi; sLog(1, i) = s; end %% 自适应滑模控制 disr = norm([disx, disy]); %导弹与目标的直线距离(m) thetaM = 0; %导弹速度与水平夹角(rad) etaM = -atan(disy/disx); %导弹速度与MT连线夹角(rad) lambda = -atan(disy/disx); %视线角(rad) dLambda = velM*sin(etaM)/disr; X = [lambda; dLambda]; %初始状态 for i = 1:1:T/dt+1 % 滑模控制量s和加速度accelC dDisr = -velM*cos(etaM); tgo = -disr/dDisr; matA = [0, 1; 0, -2*dDisr/disr]; matBc = [0; -cos(etaM)/disr]; matC = [gaink1/tgo, 1]; s = matC*X - matC(1)*lambdaDesire; accelC = disr*( dLambda*(-2*dDisr/disr + (gaink1+gaink2)/tgo) +... gaink1*(gaink2+1)*(lambda-lambdaDesire)/tgo^2 +... kappa*sat(s)/tgo ); accelC = accelC / cos(etaM); % 状态方程 dX = matA*X + matBc*accelC; % 状态递推 X = X + dX*dt; lambda = X(1); dLambda = X(2); accelM = accelC; dThetaM = accelM / velM; thetaM = thetaM + dThetaM*dt; etaM = lambda - thetaM; %disr = velM*sin(etaM)/dLambda; %不能用这条进行递推 disr = disr -velM*cos(etaM)*dt; if disr<=0 disr = 0; % 写入剩下的数 for j = i:1:T/dt+1 xyLog2(:, j) = [disr*cos(lambda); disr*sin(lambda)]; accelLog(2, j) = accelM; lambdaLog(2, j) = lambda*180/pi; sLog(2, j) = s; end break; end % 记录 xyLog2(:, i) = [-disr*cos(lambda); -disr*sin(lambda)]; accelLog(2, i) = accelM; lambdaLog(2, i) = lambda*180/pi; sLog(2, i) = s; end %% 画图 figure; plot(xyLog1(1,:), xyLog1(2,:),'linewidth', 1.5); hold on; plot(xyLog2(1,:), xyLog2(2,:), 'linewidth', 1.5); grid on; title('导弹飞行轨迹图'); xlabel('x(m)'); ylabel('h(m)'); legend('普通', '自适应'); figure; plot(time, accelLog, 'linewidth', 1.5); hold on; grid on; title('导弹加速度变化图'); xlabel('time(s)'); ylabel('accelM(m/s^2)'); legend('普通', '自适应'); figure; plot(time, lambdaLog, 'linewidth', 1.5); hold on; grid on; title('视线角变化图'); xlabel('time(s)'); ylabel('lambda(°)'); legend('普通', '自适应'); figure; plot(time, sLog, 'linewidth', 1.5); hold on; grid on; title('s函数变化图'); xlabel('time(s)'); ylabel('s'); legend('普通', '自适应'); %% 小函数 function y = sat(x) if x >= 0.1 y = 1; elseif x <= -0.1 y = -1; else y = x/0.1; end end