H a r b i n I n s t i t u t e o f T e c h n o l o g y飞行器制导与控制实验报告专业：自动化班级：学号：1120410333姓名：设计时间：2015/12/12上机实验1：使用四阶龙格库塔法求解微分方程sin()ω=+dyt b dx(1)先定义参数,ωb ，初值条件可以自己任取。

1. 源程序：function [x,y] = M1(fun,x0,xt,y0,PointNum) if nargin<4 | PointNum<=0 PointNum=100; endif nargin<3 y0=0; endy(1,:)=y0(:)';h=(xt-x0)/(PointNum-1); x=x0+[0:(PointNum)]'*h; for k=1:(PointNum)f1=h*feval(fun,x(k),y(k,:)); f1=f1(:)';f2=h*feval(fun,x(k)+h/2,y(k,:)); f2=f2(:)';f3=h*feval(fun,x(k)+h/2,y(k,:)); f3=f3(:)';f4=h*feval(fun,x(k)+h,y(k,:)); f4=f4(:)';y(k+1,:)=y(k,:)+(f1+2*(f2+f3)+f4)/6; end2、运行文件： x0=0; xt=2; Num=100;h=(xt-x0)/(Num-1); x=x0+[0:Num]*h; a=1;yt=1-exp(-a*x); fun=inline('-y+1','x','y'); y0=0;PointNum=100;[xr,yr]=M1(fun,x0,xt,y0,Num); M1_x=xr'M1_y=yr'plot(x,yt,'k',xr,yr,'r-') legend('jiexi','Runge-Kutta',2)3、实验结果：上机实验2：假设飞行器恒速率飞行，飞行器的动力学方程可简化为： 0=v (2) cos θθ=-y v a g(3)cos θψ-=v z v a(4)飞行器的运动学方程为： cos cos θψ=v x v(5) sin θ=y v (6)cos sin θψ=-v z v(7)初始条件000000,,,,,θψv v x y z 自己选取，,y z a a 为控制加速度，4040<<y z a g a g(8)选择合适的控制加速度变化规律，画出飞行轨迹。

代码如下：dt=0.01; %设置微小的时间量 vm=400; %导弹的速度 am=30;ae=pi/180; %角度转换倍数x(1)=0;y(1)=0;z(1)=0; %导弹的初始位置 pmr(:,1)=[x(1);y(1);z(1)]; %导弹位置信息矩阵 time=0; %初始化角度和时间信息 sm=vm*dt; %导弹微小时间内飞行距离 % ft=0.4*ae; % st=0.2*ae; % vm=vm+am*time; ft=0; st=0; for (k=2:500) time=time+dt; vm=vm+am*time;pmr(:,k)=[pmr(1,k-1)+vm*dt*cos(st)*cos(ft);pmr(2,k-1)+vm*dt*sin(st); pmr(3,k-1)-vm*dt*cos(st)*sin(ft)]; %目标位置信息的计算 st=(980-9.8*cos(st))/vm*dt+st; %侧滑角的变化 ft=(980/(-vm*cos(st)))*dt+ft; endplot3(pmr(1,:),pmr(2,:),pmr(3,:)); grid on ;实验图如下：上机实验3：从升降舵舵偏角到弹体俯仰角速率和法向加速度的传函分别为：()()()()325341214142ϑδ-+-==++++z s a s a a a a G s s s a a s a a a (9)()()()()251525342214142δ++-==++++y z n s a s a a s a a a a V G s s g s a a s a a a (10)加速度指令指令跟踪控制系统设计为如下图所示：其中，()(),g a G s G s 分别为陀螺与加速度计的传递函数，()(),d C s G s 为待设计的控制器，请设计合适的()(),d C s G s ，使系统能够跟踪输入指令，具有较好的性能。

系统性能指标及系统模型：实际系统模型如下1266.5472.73() 2.78106.6z s s G s ss s ϑδ--==++（）（） 2220.1290.12972.73() 2.78106.6y zn s s s G s s s s δ+-==++（）（） 系统性能指标设计内外环控制器，使控制系统达到预定的性能指标，上升时间小于0.2s ，剪切频率大于3rad/s ，幅值大于10dB ，相角裕度大于50°。

参数设计: 内环部分：其中认为G g (s )=1,则内环反馈通道中传递函数为G1，前向通道上传递函数为1，G g (s )采用比例控制器，根据计算和试凑可知，有以下结果：比例 G g (s )=1.82则开环Bode 图如下：此时相角裕度90.8°，剪切频率116rad/s 。

满足内环设计需求。

外环设计即设计C(s)的传递函数，根据内环设计完成后的传递函数，采用PID控制进行设计，其中传递函数如下：C(s)=20.039×(1+0.006s)(1+0.53s)s(0.071s+1)开环传递函数波特图如下：剪切频率大于3rad/s，幅值大于10dB，相角裕度大于50°，满足性能指标需求。

Simulink仿真图如图所示：仿真结果如下:由图可知，实验结果基本满足要求参数。

四、实验结果分析：实验设计采用比例控制器作为内环，通过计算和试凑可知，基本满足参数需求，之后设计外环设计，采用PID 控制器作为外环设计出的双环控制系统可以满足系统的性能指标要求，最终俯仰轴稳定控制系统剪切频率大于3rad/s ，幅值大于10dB ，相角裕度大于50°，上升时间小于0.2s ，符合要求。

仿真实验达到设计目标。

上机实验4：导弹的动力学和运动学方程同实验2，如式(2)(3)(4)(5)(6)(7)所示，目标的动力学方程为： 0=t v(11) cos θθ=-t t yt t v a g(12)cos θψ-=t t vt tz v a(13)目标的运动学方程为： cos cos θψ=t t t vt x v(14) sin θ=t t t y v (15)cos sin θψ=-t t t vt z v(16)比例导引律： ε=y a KVq (17) β=-z a KVq(18)其中，ε=q(19)arctanβ=-rrz q x (20)目标相对导弹的运动方程： =-r t x x x (21) =-r t y y y (22) =-r t z z z(23)其中， =-r t x x x (24) =-r t y y y (25)=-r t z z z(26)初始条件00000000000000,,,,,,,,,,,,,εβθψθψv t t vt t t t v x y z v x y z q q 自己设定，目标的运动情况自己假定，选择合适的比例导引系数，利用四阶龙格库塔求解出仿真结果，绘出导弹与目标的运动轨迹。

clear all; close all; clc dt=0.1;alpha=pi/6;v_t=0.42;s_t=v_t*dt; v_m=0.60;s_m=v_m*dt;x(1)=0;y(1)=0;z(1)=0; %导弹初始位置 pmr(:,1)=[x(1);y(1);z(1)]; ptr(:,1)=[25;5;7]; K=3; q(1)=0; o(1)=0; a(1)=0;for(k=2:600)ptr(:,k)=[ptr(1,1)-v_t*cos(alpha)*dt*k;ptr(2,1);ptr(3,1)+v_t*sin(alpha)*k*dt]; r(k-1)=sqrt((ptr(1,k-1)-pmr(1,k-1))^2+(ptr(2,k-1)-pmr(2,k-1))^2+(ptr(3,k-1)-pmr (3,k-1))^2);c=sqrt((ptr(1,k)-pmr(1,k-1))^2+(ptr(2,k)-pmr(2,k-1))^2+(ptr(3,k)-pmr(3,k-1))^2); b=acos((r(k-1)^2+s_t^2-c^2)/(2*r(k-1)*s_t)); dq=acos((r(k-1)^2-s_t^2+c^2)/(2*r(k-1)*c)); if abs(imag(b))>0 b=0.0000001; endif abs(imag(dq))>0dq=0.0000001;endq(k)=q(k-1)+dq;o(k)=o(k-1)+K*dq;a(k)=o(k)-q(k);c1=r(k-1)*sin(b)/sin(a(k)+b);c2=r(k-1)*sin(a(k))/sin(a(k)+b);c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b)); dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));if abs(imag(dq))>0dq=0.0000001;endq(k)=q(k-1)+dq;o(k)=o(k-1)+K*dq;a(k)=o(k)-q(k);c1=r(k-1)*sin(b)/sin(a(k)+b);c2=r(k-1)*sin(a(k))/sin(a(k)+b);c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b)); dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));if abs(imag(dq))>0dq=0.0000001;endq(k)=q(k-1)+dq;o(k)=o(k-1)+K*dq;a(k)=o(k)-q(k);c1=r(k-1)*sin(b)/sin(a(k)+b);c2=r(k-1)*sin(a(k))/sin(a(k)+b);c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b)); dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));if abs(imag(dq))>0dq=0.0000001;endq(k)=q(k-1)+dq;o(k)=o(k-1)+K*dq;a(k)=o(k)-q(k);c1=r(k-1)*sin(b)/sin(a(k)+b);c2=r(k-1)*sin(a(k))/sin(a(k)+b);c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b));dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));if abs(imag(dq))>0dq=0.0000001;endq(k)=q(k-1)+dq;o(k)=o(k-1)+K*dq;a(k)=o(k)-q(k);c1=r(k-1)*sin(b)/sin(a(k)+b);c2=r(k-1)*sin(a(k))/sin(a(k)+b);c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b));x1(k)=ptr(1,k-1)+c2/s_t*(ptr(1,k)-ptr(1,k-1));y1(k)=ptr(2,k-1)+c2/s_t*(ptr(2,k)-ptr(2,k-1));z1(k)=ptr(3,k-1)+c2/s_t*(ptr(3,k)-ptr(3,k-1));x(k)=pmr(1,k-1)+s_m/c1*(x1(k)-pmr(1,k-1));y(k)=pmr(2,k-1)+s_m/c1*(y1(k)-pmr(2,k-1));z(k)=pmr(3,k-1)+s_m/c1*(z1(k)-pmr(3,k-1));pmr(:,k)=[x(k);y(k);z(k)];r(k)=sqrt((ptr(1,k)-pmr(1,k))^2+(ptr(2,k)-pmr(2,k))^2+(ptr(3,k)-pmr(3,k))^2); if r(k)<0.06;break;end;endfigure(1);plot3(pmr(1,1:k),pmr(2,1:k),pmr(3,1:k),'k',ptr(1,:),ptr(2,:),ptr(3,:));axis([0 25 0 5 0 25]);grid on当K=3时仿真图像如下图所示：。