导航原理大作业 哈工大

1、导航原理作业（惯性导航部分）姓名 班号 学号 成绩批阅人Task descriptionA fighter equipped with SINS is initially at the position of 35o NL and 122o EL, stationary on a motionless carrier. Three gyros, GX, GY, GZ, and three accelerometers, AX, AY, AZ, are installed along the axes Xb, Yb, Zb of its body frame respectively.Case

2、1: stationary onboard testThe body frame of the fighter initially coincides with the geographical frame, as shown in the figure, with its pitching axis Xb pointing to the east, rolling axis Yb to the north, and azimuth axis Zb upward. Then the body of the fighter is made to rotate step by step relat

3、ive to the geographical frame:(1) 10o around Xb(2) 30o around Yb(3) 50o around ZbAfter that, the body of the fighter stops rotating. You are required to compute the final outputs of the three accelerometers on the fighter, using both direction cosine matrix and quaternion respectively, and ignoring

4、the device errors. It is known that the magnitude of gravity acceleration is g = 9.8m/s2.Case 2: flight navigationInitially, the fighter is stationary on the motionless carrier with its board 25m above the sea level. Its pitching and rolling axes are both in the local horizon, and its rolling axis i

5、s 45o north by east(北偏东), parallel with the runway onboard. Then the fighter accelerates along the runway and takes off from the carrier.The outputs of the gyros and accelerometers are both pulse numbers. Each gyro pulse is an angular increment of 0.1 arc-sec, and each accelerometer pulse is 1e-6g,

6、with g = 9.8m/s2. The gyro output frequency is 10Hz, and the accelerometers is 1Hz. The outputs of the gyros and accelerometers within 5400s are stored in MATLAB data files named gout.mat and aout.mat, containing matrices gm of 54000×3 and am of 5400×3 respectively. The format of the data

7、is as shown in the tables, with 10 rows of each matrix selected. Each row represents the outputs of the type of sensors at each sampling time.The Earth can be seen as an ideal sphere, with radius 6368.00km and spinning rate7.292×10-5 rad/s, The errors of the sensors are ignored, so is the effec

8、t of height on the magnitude of gravity. The outputs of the gyros are to be integrated every 0.1s. The rotation of the geographical frame is to be updated every 1s, so are the velocities and positions of the fighter. You are required to:(1) compute the final attitude quaternion, longitude, latitude,

9、 height, and east, north, vertical velocities of the fighter.(2) compute the total horizontal distance traveled by the fighter.(3) draw the latitude-versus-longitude trajectory of the fighter, with horizontal longitude axis.(4) draw the curve of the height of the fighter, with horizontal time axis.A

10、X AY第一种情形：正对导弹进行地面静态测试（导弹质心相对地面静止）。初始时刻弹体坐标系和地理坐标系重合，如图所示，弹体的Xb轴指东，Yb轴指北，Zb轴指天。此后弹体坐标系 Xb-Yb-Zb 相对地理坐标系的转动如下：ZbYbXb北东天首先，弹体绕 Zb（方位轴）转过 10度；接着，弹体绕 Xb（俯仰轴）转过 30 度；然后，弹体绕 Yb（滚动轴）转过 -50 度；最后弹体相对地面停止旋转。请分别用方向余弦矩阵和四元数两种方法计算：弹体经过三次旋转并停止之后，弹体上三个加速度计Ax, Ay, Az的输出。取重力加速度的大小g = 9.8m/s2。解答：（1） 四元数法开始时刻弹体上三个加速度计

11、Ax, Ay, Az的输出为（0,0，-g）=(0,0,-9.8).由四元数的定义结合原题可以得到：对于第一次旋转：，第二次旋转：，第三次旋转：， ， 在四元数 q1, q2 和 q3 中，单位矢量都表示成了映像的形式，所以可按照下式合成:由四元数的乘法规则在Matlab中编写如下四元数乘法计算程序：function q=qmul(q1,q2)lm=q1(1);p1=q1(2);p2=q1(3);p3=q1(4);ML=lm -p1 -p2 -p3 p1 lm -p3 p2 p2 p3 lm -p1 p3 -p2 p1 lm;q=ML*q2;编写新的Matlab程序用于计算四元数的逆：func

12、tion qi=qinv(q)qn=norm(q);q(2:4)=-q(2:4);qi=q/qn2;方向余弦矩阵%方向余弦阵g0=0;0;9.8; %初始加速度c1=1 0 0;0 cos(10/180*pi) sin(10/180*pi);0 -sin(10/180*pi) cos(10/180*pi);%第一次绕xb旋转c2=cos(30/180*pi) 0 -sin(30/180*pi);0 1 0;sin(30/180*pi) 0 cos(30/180*pi);%第二次绕yb旋转c3=cos(-50/180*pi) sin(-50/180*pi) 0;-sin(-50/180*pi)

13、cos(-50/180*pi) 0;0 0 1;%第三次绕zb旋转c=c3*c2*c1;%合并余弦阵g=c*g0写出如下主程序进行整体计算过程：%四元数g0=0 0 0 9.8'%四元数表示初始加速度q1=cos(10/2/180*pi) sin(10/2/180*pi) 0 0'%四元数表示第一次转动q2=cos(30/2/180*pi) 0 sin(30/2/180*pi) 0'%四元数表示第二次转动q3=cos(-50/2/180*pi) 0 0 sin(-50/2/180*pi)'%四元数表示第三次转动q=qmul(qmul(q1,q2),q3);%合并

14、三次转动qni=qinv(q);%求四元数q的逆g=qmul(qmul(qni,g0),q)得到新的坐标系中重力的四元数表示：g= -0.0000 -4.4054 -2.6027 8.3581因此，在经过三次旋转后，弹体上三个加速度计Ax, Ay, Az的输出分别为，。（2）方向余弦矩阵方法由题可知，开始时刻弹体上三个加速度计Ax, Ay, Az的输出为（0,0，-g）=(0,0,-9.8).第一次旋转时弹体坐标系的改变得到首次旋转的方向余弦矩阵第二次旋转时弹体坐标系的改变得到首次旋转的方向余弦矩阵第三次旋转时弹体坐标系的改变得到首次旋转的方向余弦矩阵因此，在经过三次旋转后，弹体上三个加速度计

15、Ax, Ay, Az的输出，记作可以表示为：在经过三次旋转后，弹体上三个加速度计Ax, Ay, Az的输出分别为。分析：对两种计算方法得到的结果进行对比，发现两组数据结果相同。说明用这两种方法都可以进行正确的计算。第二种情形：导弹导航程序设计%初始化we=7.292e-5;%地球自转角速度Q=zeros(4,5401);%初始化四元数矩阵Q(:,1)=cos(45/2/180*pi);0;0;sin(45/2/180*pi);%初始四元数R=6368000;%地球半径Longitude=zeros(1,5401);Longitude(1)=122;%初始经度Latitude=zeros(1,5

16、401);Latitude(1)=35;%初始纬度h=zeros(1,5401);h(1)=25;%初始高度g=9.8;%重力加速度VE=zeros(1,5401);VE(1)=0;%初始东向速度VN=zeros(1,5401);VN(1)=0;%初始北向速度VT=zeros(1,5401);VT(1)=0;%初始垂直速度t=0.1;%姿态计算周期k=10;T=k*t;%位、速计算周期K=5400;load gout.mat;%数据读入load aout.mat; %数据读入for N=1:K %姿态子迭代 Q1=zeros(4,11);%用于1秒内四元数更新 Q1(:,1)=Q(:,N);%

17、赋初值 for n=1:k w=0.1/3600/180*pi*gm(N-1)*10+n,:); %角度增量 w=w' normw=norm(w); %取二范数 %角度增量矩阵 W=0,-w(1),-w(2),-w(3); w(1),0,w(3),-w(2); w(2),-w(3),0,w(1); w(3),w(2),-w(1),0; %四阶近似 I=eye(4); S=1/2-normw2/48; C=1-normw2/8+normw4/384; Q1(:,n+1)=(C*I+S*W)*Q1(:,n); %四元数迭代 end Q(:,N+1)=Q1(:,n+1); %地理坐标系的转动

18、角速度分量 WE=-VN(N)/R; WN=VE(N)/R+we*cos(Latitude(N)/180*pi); WT=VE(N)/R*tan(Latitude(N)/180*pi)+we*sin(Latitude(N)/180*pi); %用地理坐标系的转动四元数修正载体姿态四元数 gama=WE,WN,WT'*T; normgama=norm(gama); n=gama/normgama; Qg=cos(normgama/2);sin(normgama/2)*n;%地理坐标系四元数 Q(:,N+1)=qmul(qinv(Qg),Q(:,N+1);%姿态四元数修正 %加速度计测得的

19、比力 fx=1e-6*9.8*am(N,1); fy=1e-6*9.8*am(N,2); fz=1e-6*9.8*am(N,3); fb=fx fy fz'%加速度计比力向量 %加速度计比力从载体坐标系到地里坐标系 f=qmul(Q(:,N+1),qmul(0;fb,qinv(Q(:,N+1); fe(N)=f(2);fn(N)=f(3);ft(N)=f(4); %计算载体的相对加速度 d_VE(N)=fe(N)+VE(N)*VN(N)/R*tan(Latitude(N)/180*pi)-(VE(N)/R+2*we*cos(Latitude(N)/180*pi)*VT(N)+2*VN(

20、N)*we*sin(Latitude(N)/180*pi); d_VN(N)=fn(N)-2*VE(N)*we*sin(Latitude(N)/180*pi)-VE(N)*VE(N)/R*tan(Latitude(N)/180*pi)-VN(N)*VT(N)/R; d_VT(N)=ft(N)+2*VE(N)*we*cos(Latitude(N)/180*pi)+(VE(N)2+VN(N)2)/R-g; %积分计算载体的相对速度 VE(N+1)=d_VE(N)*T+VE(N); VN(N+1)=d_VN(N)*T+VN(N); VT(N+1)=d_VT(N)*T+VT(N); %位置更新 Lon

21、gitude(N+1)=VE(N)/(R*cos(Latitude(N)/180*pi)*T/pi*180+Longitude(N); Latitude(N+1)=VN(N)/R*T/pi*180+Latitude(N); h(N+1)=VT(N)*T+h(N);end%画经纬度曲线figure(1)xlabel('经度/度')ylabel('纬度/度')hold onplot(Longitude,Latitude);%画高度曲线figure(2)xlabel('时间/秒')ylabel('高度/米')hold onplot(0:

22、5400,h); %显示200秒弹体到达的经纬度和高度Longitude=Longitude(5401)Latitude=Latitude(5401)Height=h(5401) %显示200秒后弹体到达的东向速度和北向速度VE=VE(5401)VN=VN(5401)%显示200秒后弹体相对当地地理坐标系的姿态四元数Quaternion=Q(:,5401)运行结果：Longitude =122.8785Latitude =35.0691Height =-604.1142Distance =2.3820e+006VE =-393.4743VN =268.0711VT =-5.4673Quaternion = -0.8750 -0.0054 -0.0023 -0.4841解答：（1） 结