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);
0.3838 0.3214
0.4040 0.3351
0.4242 0.3486
0.4444 0.3617
0.4646 0.3746
0.4848 0.3873
0.5051 0.3996
0.5253 0.4118
0.5455 0.4236
0.5657 0.4353
0.5859 0.4467
0.6061 0.4579
if abs(imag(b))>0
b=0.0000001;
end
if abs(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);
0.8687 0.5842
0.8889 0.5926
0.9091 0.6008
0.9293 0.6089
0.9495 0.6168
0.9697 0.6245
0.9899 0.6321
1.0101 0.6396
1.0303 0.6468
1.0505 0.6540
1.0707 0.6610
1.0909 0.6678
飞行器的运动学方程为：
初始条件 自己选取， 为控制加速度，
选择合适的控制加速度变化规律，画出飞行轨迹。
代码如下：
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)];%导弹位置信息矩阵
dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));
if abs(imag(dq))>0
0.6263 0.4688
0.6465 0.4796
0.6667 0.4901
0.6869 0.5004
0.7071 0.5105
0.7273 0.5204
0.7475 0.5300
0.7677 0.5395
0.7879 0.5488
0.8081 0.5580
0.8283 0.5669
0.8485 0.5756
1.5960 0.8006
1.6162 0.8046
1.6364 0.8085
1.6566 0.8124
1.6768 0.8162
1.6970 0.8199
1.7172 0.8236
1.7374 0.8271
1.7576 0.8306
1.7778 0.8340
1.7980 0.8374
1.8182 0.8407
0.1414 0.1331
0.1616 0.1506
0.1818 0.1678
0.2020 0.1846
0.2222 0.2011
0.2424 0.2172
0.2626 0.2330
0.2828 0.2485
0.3030 0.2637
0.3232 0.2786
0.3434 0.2932
0.3636 0.3074
end
q(k)=q(k-1)+dq;
o(k)=o(k-1)+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));
Harbin Institute of Technology
飞行器制导与控制
实验报告
专业：自动化
班级：
学号：**********
姓名：
设计时间：2015/12/12
上机实验1：
使用四阶龙格库塔法求解微分方程
先定义参数 ，初值条件可以自己任取。
1.源程序：
function [x,y] = M1(fun,x0,xt,y0,PointNum)
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);
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));
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);
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;
1.8384 0.8439
1.8586 0.8470
1.8788 0.8501
1.8990 0.8532
1.9192 0.8561
1.9394 0.8590
1.9596 0.8619
1.9798 0.8647
2.0000 0.8674
2.0202 0.8701
上机实验2：
假设飞行器恒速率飞行，飞行器的动力学方程可简化为：
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))>0
dq=0.0000001;
if nargin<4 | PointNum<=0
PointNum=100;
end
if nargin<3
y0=0;
end
y(1,:)=y0(:)';
h=(xt-x0)/(PointNum-1);
x=x0+[0:(PointNum)]'*h;
for k=1:(PointNum)
f1=h*feval(fun,x(k),y(k,:));
上机实验4：
导弹的动力学和运动学方程同实验2，如式所示，目标的动力学方程为：
目标的运动学方程为：
比例导引律：
其中，
目标相对导弹的运动方程：
其中，
初始条件 自己设定，目标的运动情况自己假定，选择合适的比例导引系数，利用四阶龙格库塔求解出仿真结果，绘出导弹与目标的运动轨迹。
clear all;
close all;
参数设计:
内环部分：
其中认为 则内环反馈通道中传递函数为G1，前向通道上传递函数为1， 采用比例控制器，根据计算和试凑可知，有以下结果：
比例 =1.82
则开环Bode图如下：
此时相角裕度90.8°，剪切频率116rad/s。满足内环设计需求。
外环设计
即设计C(s)的传递函数，根据内环设计完成后的传递函数，采用PID控制进行设计，其中传递函数如下：
M1_x=xr'
M1_y=yr'
plot(x,yt,'k',xr,yr,'r-')
legend('jiexi','Runge-Kutta',2)
3、实验结果：
0 0
0.0202 0.0202
0.0404 0.0400
0.0606 0.0594
0.0808 0.0784
0.1010 0.0970
0.1212 0.1153
1.1111 0.6745
1.1313 0.6811