导航原理作业

导航原理作业（惯性导航部分）

姓名 班号 学号 成绩 批阅人

1004105 11004005** 天 Zb Yb 北 作业内容需包括：

1. 题目内容（即本页） 2. 程序设计说明及代码 3. 计算或仿真结果及分析 4. 仿真中遇到的问题或体会

Xb 东 一枚导弹采用捷联惯性导航系统，三个速率陀螺仪Gx, Gy, Gz 和三个加速度计 Ax, Ay, Az 的敏感轴分别沿着着弹体坐标系的Xb, Yb, Zb轴。初始时刻该导弹处在北纬45.75度，东经126.63度。

第一种情形：正对导弹进行地面静态测试（导弹质心相对地面静止）。

初始时刻弹体坐标系和地理坐标系重合，如图所示，弹体的Xb轴指东，Yb轴指北，Zb轴指天。此后弹体坐标系 Xb-Yb-Zb 相对地理坐标系的转动如下：

首先，弹体绕 Zb（方位轴）转过 -10 度； 接着，弹体绕 Xb（俯仰轴）转过 15 度； 然后，弹体绕 Yb（滚动轴）转过 20 度； 最后弹体相对地面停止旋转。

请分别用方向余弦矩阵和四元数两种方法计算：弹体经过三次旋转并停止之后，弹体上三个加速度计Ax, Ay, Az的输出。取重力加速度的大小g = 9.8m/s2。

第二种情形：导弹正在飞行中。

初始时刻弹体坐标系仍和地理坐标系重合；且导弹初始高度200m，初始北向速度 1800 m/s，初始东向速度和垂直速度都为零。

陀螺仪和加速度计的输出都为脉冲数形式，陀螺输出的每个脉冲代表 0.00001弧度的角增量。加速度计输出的每个脉冲代表1μg，1g = 9.8m/s2。陀螺仪和加速度计输出的采样频率都为10Hz，在200秒内三个陀螺仪和三个加速度计的输出存在了数据文件gaout.mat中，内含一矩阵变量ga，有2000行，6列。每一行中的数据代表每个采样时刻三个陀螺Gx, Gy, Gz和三个加速度计Ax, Ay, Az的输出的脉冲数。格式如下表（前10行） Gx -3 -2 -2 -1 -2 -3 -2 -2 -1 -2 Gy 0 0 0 0 -1 0 0 0 0 0 Gz -30 -30 -30 -30 -30 -30 -30 -31 -30 -31 Ax 556000 556000 555999 556001 556001 556000 556000 556000 556000 556000 Ay 77000 77000 77000 77000 77001 77000 77001 77000 76999 77000 Az 940977 940977 940977 940977 940978 940977 940978 940976 940976 940977

将地球视为理想的球体，半径 6371.00公里，且不考虑仪表误差，也不考虑弹体高度对重力加速度的影响。选取弹体的姿态计算周期为0.1秒，速度和位置的计算周期为1秒。

（1） 请计算200秒后弹体到达的经纬度和高度，东向和北向速度； （2） 请计算200秒后弹体相对当地地理坐标系的姿态四元数；

（3） 请绘制出200秒内导弹的经、纬度变化曲线（以经度为横轴，纬度为纵轴）； （4） 请绘制出200秒内导弹的高度变化曲线（以时间为横轴，高度为纵轴）。

第一种情形：正对导弹进行地面静态测试（导弹质心相对地面静止） 1.方向余弦

(1)程序设计说明及代码

function fxyx wx=15*pi/180; wy=20*pi/180;

wz=-10*pi/180;/将陀螺仪的角度输出转化为弧度输出/ U=[0,-wz,wy;wz,0,-wx;-wy,wx,0];/U——斜对称矩阵/ si=sqrt(wx^2+wy^2+wz^2);/角增量的模/ I=eye(3); S=1-si^2/6;

C=1/2-si^2/24;/S,C——角增量算法的系数/ c=I+S*U+C*U^2;/应用毕卡算法/

c1=inv(c)；/由于c求出来是“从载体到地理”的方向余弦阵，所以要求逆变成“从地理到载体的”，这样，乘以初始地理信息就是载体最后的信息/ g0=[0;0;9.8]; g=c1*g0 end

（2）计算或仿真结果及分析 fxyx g =

-3.5149/东向惯性加速度/ 2.1789/北向惯性加速度/ 8.8854/天向惯性加速度/

2.四元数

(1)程序设计说明及代码

function sys /用来求出四元数wx=15*pi/180; wy=20*pi/180; wz=-10*pi/180; g0=[0；0;0;9.8]; q0=[1;0;0;0];

w=sqrt(wx^2+wy^2+wz^2);

U=[0,-wx,-wy,-wz;wx,0,-wz,-wy;wy,-wz,0,wx; wz,wy,-wx,0]; I=eye(4); S=1/2-w^2/48; C=1-w^2/8+w^4/384; q=(C*I+S*U)*q0

q/

end

function [q1] = quatinv (q)/四元数求逆方程/ q(1)=q(1); q(2)=-q(2); q(3)=-q(3); q(4)=-q(4); q1=q; end

function [q]= quatmultiply (q1,q2);/四元数相乘运算/ lm=q1(1);p1=q1(2);p2=q1(3);p3=q1(4);

q=[lm -p1 -p2 -p3;p1 lm -p3 p2;p2 p3 lm -p1;p3 -p2 p1 lm]*q2; end

（2）计算或仿真结果及分析

>> sys q =

0.9725 0.1297 0.1729 -0.0865

>> q=[0.9725;0.1297;0.1729;-0.0865]/将q存入数组/ q =

0.9725 0.1297 0.1729 -0.0865

>> g0=[0;0;0;9.8]/将g0存入数组/ g0 =

0 0 0 9.8000

>> q1= quatinv (q)/对四元数q求逆，得q1/

q1 =

0.9725 -0.1297 -0.1729 0.0865

>> g1= quatmultiply (q1,g0)/由于Ve1=q1。Ve。q,分两步求，第一步g1=q1。Ve 第二步g=g1。q/ g1 =

-0.8477 -1.6944 1.2711 9.5305

>> g= quatmultiply (g1,q) g =

0

-3.5155/东向惯性加速度/ 2.1791/北向惯性加速度/ 8.8839/天向惯性加速度/

>>

（3）仿真中遇到的问题或体会

运用Matlab的问题，我的那个数组在editor中定义的，然后调用的时候显示没有定义这个数组，我去workspace中也确实没有找到。后来，我用load，将g0和q调入。

第二种情形：导弹正在飞行中

(1)程序设计说明及代码

R=6371000; g=9.8; T=1; h(1)=200;

longitude(1)=126.63; latitude(1)=45.75; q=zeros(4,201); q(:,1)=[1;0;0;0]; Ve(1)=0; Vn(1)=1800; Vh(1)=0;/初值/ load gaout.mat

for N=1:200/姿态矫正开始/ q1=zeros(4,11); q1(:,1)=q(:,N); for n=1:10

wx=0.00001*ga((N-1)*10+n,1); wy=0.00001*ga((N-1)*10+n,2);

wz=0.00001*ga((N-1)*10+n,3);/循环调用陀螺的输出数据/ w=[wx,wy,wz]'; sita0=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-sita0^2/48;

C=1-sita0^2/8;/采用三阶毕卡近似/ q1(:,n+1)=(C*I+S*W)*q1(:,n); end

z=15/180*pi/3600;

q(:,N+1)=q1(:,n+1);/将姿态矫正的结果送给q/ we=-Vn(N)/R;

wn=Ve(N)/R+z*cos(latitude(N)/180*pi);

wh=Ve(N)/R*tan(latitude(N)/180*pi)+z*sin(latitude(N)/180*pi);/地理坐标系的转动角速度分量/

t=[we,wn,wh]'*T; t1=norm(t);

r1=[cos(t1/2);sin(t1/2)*(t/t1)];

q(:,N+1)= quatmultiply (quatinv (r1),q(:,N+1));/用地理坐标系的转动四元数修正载体姿态四元数/

fx=1e-6*9.8*mean(ga((N-1)*10+1:N*10,4)); fy=1e-6*9.8*mean(ga((N-1)*10+1:N*10,5));

fz=1e-6*9.8*mean(ga((N-1)*10+1:N*10,6));/加速度计测得的数据/ fb=[fx fy fz]';

q1= quatinv (q(:,N+1)); q2= quatmultiply ([0;fb],q1);

fg= quatmultiply (q(:,N+1),q2);/从载体到地理系/ fe(N)=fg(2);

fn(N)=fg(3);

fh(N)=fg(4);

d_Ve(N)=fe(N)+Ve(N)*Vn(N)/R*tan(latitude(N)/180*pi)-(Ve(N)/R+2*z*cos(latitude(N)/180*pi))*Vh(N)+2*Vn(N)*z*sin(latitude(N)/180*pi);

d_Vn(N)=fn(N)-2*Ve(N)*z*sin(latitude(N)/180*pi)-Ve(N)*Ve(N)/R*tan(latitude(

N)/180*pi)-Vn(N)*Vh(N)/R;

d_Vh(N)=fh(N)+2*Ve(N)*z*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);

Vh(N+1)=d_Vh(N)*T+Vh(N);/积分计算载体相对速度/

longitude(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)=Vh(N)*T+h(N); end figure(1) hold on

plot(longitude,latitude); figure(2) hold on

plot([0:200],h); longitude=longitude(201) latitude=latitude(201) height=h(201) Ve=Ve(201) Vn=Vn(201) q=q(:,201)

（2）计算或仿真结果及分析

question2

longitude =

128.1494

latitude =

48.8966

height =

84.5044

Ve =

1.1240e+003

Vn =

1.5680e+003 q =

0.9506 0.0106 -0.0048 -0.3101

（3）仿真中遇到的问题或体会

编程中首先遇到的问题是四元数相乘及求逆的运算问题，刚开始我想到自己编辑一个function文件，后来上网查了之后，找到了相应的函数，所以最终我采用了matlab自带的四元数函数。 其次，编程中对角度的换算，也会影响到结果的正确性，比如刚开始我没有把地球自转换算成弧度制，而其他地方都是弧度制，所以最后的结果可想而知。 最后我谈谈感想最大的部分吧，就是对这个”系统的简化航姿解算流程”的总结。理论上讲，陀螺的输出用来求出的矩阵可以看做是载体从初态到末态的变换，记作q1,加速度计的输出，可以用来求地理坐标系从初态到末态的变换，记作q2，那么q=[(q1)-1]q2。那么，加速度计的输出用作映像初态，然后求得变换后的四元数，即变化后的加速度信息，用此来进行新的迭代，算出新的物理量。

本文来源：https://www.bwwdw.com/article/rvbp.html