.
|
|
HarbinInstitute of Technology
飞行器制导与控制
实验报告
专 | 业： | 自动化 |
班 | 级： | 1120410333 | |
学 | 号： | ||
姓 | 名： | ||
2015/12/12 | | ||
设计时间： |
| ||
.
.
上机实验1：
使用四阶龙格库塔法求解微分方程
dy | ? | sin(? | ? | b | ) | (1) | ||
dx | | | | | | | ||
先定义参数 | ?b ，初值条件可以自己任取。 | |||||||
1. | 源程序： | |||||||
function[x,y] = M1(fun,x0,xt,y0,PointNum)
ifnargin<4 | PointNum<=0
PointNum=100;
end
ifnargin<3
y0=0;
end
y(1,:)=y0(:)';
h=(xt-x0)/(PointNum-1);
x=x0+[0:(PointNum)]'*h;
fork=1:(PointNum)
f1=f1(:)';f2=h*feval(fun,x(k)+h/2,y(k,:)); f1=h*feval(fun,x(k),y(k,:));
f4=f4(:)';
y(k+1,:)=y(k,:)+(f1+2*(f2+f3)+f4)/6;
end
2、运行文件：
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：
假设飞行器恒速率飞行，飞行器的动力学方程可简化为：
v | ? | 0 | cos? | (2)(2) | |||||||
v??? | a | y | ? | g | (3)(3) | ||||||
? | a | z | (4)(4) | ||||||||
? | v | cos???v | |||||||||
飞行器的运动学方程为：
x?vcos?cos?v (5)(5)
y?v sin? (6)(6)
z??v cos sin?v (7)(7)
.
.
初始条件 | v 0 | ,??v | 0 | , | x 0 | , | y 0 | , | z 自己选取，0 | a a 为控制加速度，y z | (8) | ||||||
a | y | ? | 40 | g | |||||||||||||
a | z | ? | 40 | g | |||||||||||||
选择合适的控制加速度变化规律，画出飞行轨迹。
代码如下：
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;
for(k=2:500) time=time+dt; st=0;
ft=(980/(-vm*cos(st)))*dt+ft;
end
plot3(pmr(1,:),pmr(2,:),pmr(3,:));
grid on;
实验图如下：
.
.
上机实验3：
从升降舵舵偏角到弹体俯仰角速率和法向加速度的传函分别为：
G s 1??? ??? ? ?a s 3 ?a a 2 5?a a 3 4 (9) | |||
C (s ) | I | G d(s ) | G 2(s ) |
G g(s ) | G 1(s ) | ||
Ga(s)
其中，Gg??,Ga??s分别为陀螺与加速度计的传递函数，Cs??,Gd??s 为待设计的控制
器，请设计合适的Cs??,Gd??s，使系统能够跟踪输入指令，具有较好的性能。
系统性能指标及系统模型：
.
.
实际系统模型如下
G s 1 ( ) | ? | ?（）? ? ?（）z | ?66.54 | s | ? | 72.73 | |||||||||||||
| s | 2 | ? | 2.78 | s | ? | 106.6 | ||||||||||||
G s ( ) | ? | n（）y ? ?（）z | 0.129 | s | 2 | ? | 0.129 | s | ? | 72.73 | |||||||||
2 | | s | 2 | ? | 2.78 | s | ? | 106.6 | |||||||||||
系统性能指标
设计外环控制器，使控制系统达到预定的性能指标，上升时间小于0.2s，剪切频率大于3rad/s，幅值大于10dB，相角裕度大于50°。
参数设计:
环部分：
其中认为则环反馈通道中传递函数为G1，前向通道上传递函数为1，
则开环Bode 图如下：
此时相角裕度90.8°，剪切频率116rad/s。满足环设计需求。外环设计
.
.
即设计C(s)的传递函数，根据环设计完成后的传递函数，采用PID控制进行设计，其中
传递函数如下：
开环传递函数波特图如下：
剪切频率大于3rad/s，幅值大于10dB，相角裕度大于50°，满足性能指标需求。
Simulink仿真图如图所示：
仿真结果如下:
.
.
由图可知，实验结果基本满足要求参数。
四、实验结果分析：
实验设计采用比例控制器作为环，通过计算和试凑可知，基本满足参数需求，之后设计
外环设计，采用PID控制器作为外环设计出的双环控制系统可以满足系统的性能指标要求，
最终俯仰轴稳定控制系统剪切频率大于3rad/s，幅值大于10dB，相角裕度大于50°，上升
时间小于0.2s，符合要求。仿真实验达到设计目标。
上机实验4：
导弹的动力学和运动学方程同实验2，如式(2)(3)(4)(5)(6)(7)所示，目标的动力学方程为：
?vtcos??tvt?atz (13)
目标的运动学方程为：
x&t?vtcos?tcos?vt (14)
y&t?vtsin?t (15)
z&t??vtcos?tsin?vt (16)
比例导引律：
其中， | q?? | a | y | ? | KVq &? | z | r | 2 | (17) | ||||||
a | z | ?? | KVq &? | (18) | |||||||||||
arctan | x r | y | r |
| |||||||||||
| (19) | ||||||||||||||
2 | ? | ||||||||||||||
.
.
q?? | arctan | z | r | (20) |
? | | x r | | |
目标相对导弹的运动方程：
x & r | ? | x & t | ? | x & | (21) |
? | ? | y & | (22) | ||
y & r | y & t | ||||
? | ? | z & | (23) | ||
z & r | z & t |
其中，
xr?xt?x (24)
yr?yt?y (25)
zr?zt?z (26)
初始条件v0,??0 v0,x0,y0,zv 0 t0,?t0,?vt0,xt0,yt0,zt0,q?0,q?0自己设定，目标的运动情况自
己假定，选择合适的比例导引系数，利用四阶龙格库塔求解出仿真结果，绘出导弹与目标的
clearall; 运动轨迹。
clc closeall;
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];
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);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);
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));
ifabs(imag(b))>0
b=0.0000001;
end
ifabs(imag(dq))>0
dq=0.0000001;
end
q(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);
dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b));
dq=0.0000001; end
q(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));
ifabs(imag(dq))>0
dq=0.0000001;
.
.
end
q(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));
ifabs(imag(dq))>0
dq=0.0000001;
end
q(k)=q(k-1)+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));
ifabs(imag(dq))>0
dq=0.0000001;
end
q(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)];
ifr(k)<0.06; r(k)=sqrt((ptr(1,k)-pmr(1,k))^2+(ptr(2,k)-pmr(2,k))^2+(ptr(3,k)-pmr(3,k))^2);
end; break;
end
figure(1);
plot3(pmr(1,1:k),pmr(2,1:k),pmr(3,1:k),'k',ptr(1,:),ptr(2,:),ptr(3,:));
axis([02505025]);
gridon
当K=3时仿真图像如下图所示：
.
.
.