北航惯导第二次大作业.doc

《惯性导航原理》第二次大作业

姓名:袁二凯
学号:SY0917145
联系方式:13488854452
642101136@

2011年11月8日

一、 原理分析
指北方位平台式惯导解算
在利用方向余弦方法对惯性导航系统进行测算时，刚体空间位置用固连于刚体的动坐标系对固定参考系各轴的九个方向余弦来确定，九个方向余弦角存在六个约束条件，计算比较繁琐，模型也比较复杂。如果在计算过程中引入四元数，则可以通过坐标系的一次转动，实现方向余弦方法中的三次坐标旋转。原理图如下：
Cbt
加速度计
fb ft λ、L、h
Vx、Vy、Vz
姿态速率微分方程

Witt
Wtbb

陀螺组件

Ctb

Wibb + — Witb

从原理图可以清楚的看出，通过捷联姿态矩阵Cbt可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法即可以得到任意时刻载体的位置和速度信息。关键在于捷联姿态矩阵的求解。在这里应用四元数知识进行解算。
1. 捷联姿态矩阵的求解
因为平台的初始姿态角都是已知的，则可以先求解四元数中各元的初值。平台坐标系相对于地理坐标系的三次旋转可以由四元数的乘积得到。用四元数表示这三次转动为（逆时针为正）：
Q1=cosφ2+zsinφ2
Q2=cosθ2+xb1sinθ2
Q3=cosγ2+ybsinγ2
对三者进行四元素乘法运算：
Q=Q3∘Q2∘Q1
结果与四元数Λ=λ0+λ1x+λ2y+λ3z中的各元素相对应，就可以从已知的平台初始姿态求解出四元数的各元初值。

λ0=cosφ2cosθ2cosγ2-sinφ2sinθ2sinγ2λ1=cosφ2sinθ2cosγ2-sinφ2cosθ2sinγ2λ2=cosφ2cosθ2sinγ2+sinφ2sinθ2cosγ2λ3=sinφ2cosθ2cosγ2+cosφ2sinθ2sinγ2
带入四元数姿态矩阵即可得到捷联姿态矩阵：
Ctb=λ02+λ12+λ22+λ322(λ1λ2+λ0λ3)2(λ1λ3-λ0λ2)2(λ1λ2-λ0λ3)λ02-λ12+λ22-λ322(λ2λ3+λ0λ1)2(λ1λ3+λ0λ2)2(λ2λ3-λ0λ1)λ02-λ12-λ22+λ32
通过此矩阵可以将地理坐标系的参数转移到平台坐标系。此矩阵的逆矩阵Cbt就是将比力信息从平台坐标系转移到地理坐标系的姿态矩阵。此外此矩阵还要在四元数的迭代计算中使用。下面进行四元数的迭代计算。
四元数微分方程的矩阵形式为
λ0λ1λ2λ3=120ωtbxb-ωtbxb0-ωtbyb-ωtbzbωtbzb -ωtbybωtbyb-ωtbzb 0 ωtbxbωtbzbωtbyb-ωtbxb 0λ0λ1λ2λ3
式中
ωtbb=ωibb-Ctbωitt
其中ωibb即为陀螺仪所测量的角速度值，ωitt为平台的指令角速度，为地球的自转角速率与地理坐标系相对于地球坐标系的角速率之和，即
ωitt=ωiet+ωett
地球的自转角速率为：
ωiet=ωiextωieytωiezt=0ωiecosLωiesinL
地理坐标系相对于地球坐标系的角速率为：
ωett=ωetxtωetytωetzt = -VetytRytVetxtRxtVetxtRxttanL
上述四元数方程的解和下面矩阵方程的解类似：
Q（λ）=12M*ωtbbQ（λ）
记q(t)=λ0λ1λ2λ3T，由角增量法可得迭代公式（取四阶算法）：
q(n+1)=1-∆θ028+∆θ04384I+12-∆θ0248∆θ q(n)
（n=0,1,2，……）
式中
∆θ=t1t2M*(ωtbb)dt=0∆θx-∆θx0-∆θy-∆θz∆θz -∆θy∆θy-∆θz 0 ∆θx∆θz∆θy-∆θx 0
∆θ02=∆θx2+∆θy2+∆θz2
q(0)即为解算出的初值，带入迭代公式即可得到任意时刻的捷联姿态矩阵Cbt。通过捷联姿态矩阵Cbt可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法求解任意时刻载体的位置和速度信息即可。
2、指北方位平台式惯导求解方位和速度
载体相对地球运动时，加速度计测得的比力表达式，称为比力方程，方程如下：

在指北方案中，平台模拟地理坐标系，将上式中平台坐标系用地理坐标系代入得：

系统中测量的是比力分量，将上式写成分量形式
Vzt = fxtfytfzt - 0-(2ωiezt+ωetzt)(2ωieyt+ωetyt)(2ωiezt+ωetzt)0-(2ωiext+ωetxt)-2ωieyt+ωetyt2ωiext+ωetxt0VxtVytVzt
+ 00g
将ωiet和ωett的表达式带入上式，即可得到如下方程组：
Vzt=fzt +(2ωiecosL+VxtRxt)Vxt + VytRytVyt – g （6）
作业要求只考虑水平通道，因此只需要计算正东、正北两个方向的速度即可。理论上计算得到、后，再积分一次可得到速度值，即

但在本次计算过程中，三个方向的速度均是从零开始在各时间节点上的累加，并不是t的函数，因此速度计算可以由以下方程组实现Vxti+1=fxti+2ωiesinLi+VxtiRxtitanLiVyti-2ωiecosLi+VxtiRxtiVzt*0.01+VxtiVyti+1=fyti-2ωiesinLi+VxtiRxtitanLiVxti+VytiRytiVzt*0.01+VytiVzti+1=fzti+2ωiecosLi+VxtiRxtiVxti+VytiRytiVyt-g*0.01+Vzti
此方程组表示了从第i个采集点到第（i+1）个采集点的速度递推公式。
方程中Rx表示卯酉圈的曲率半径，Ry表示子午圈的曲率半径，计算方法如下：
Rx=Re/(1-esin2L);
Ry=Re/(1+2e-3esin2L);
由于平台在运动中纬度L也在不断变化，因此，计算过程中应当追踪两个半径的变化。
另外方程组中g表征平台所处纬度下的重力加速度：

g=g0*(1+gk1*c33^2)*(1-2*h/re)/sqrt(1-gk2*c33^2)；
其中g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;
c33=sin(纬度);
为了尽可能减小计算的累积误差，计算过程中可以对g的变化进行追踪。
3.经纬度计算
载体所在位置的地理纬度L，经度λ可由下列方程求得：

与速度的计算相同，经纬度也不是t的函数，可以由累加得到：

二、程序流程图

开始

根据初始姿态角求解四元数的初值

Ctb

陀螺组件
+
- -

ωtbb

Ctb

四元数微分方程（迭代法）

Cbt

求逆
加速度计

指北方位的平台式惯导解算
ωitt

输出经纬度，东向北向的速度

结束

三、导航结果
1.系统位置坐标曲线图

2.系统东向速度随时间变化曲线图

3. 系统北向速度随时间变化曲线图

4．系统纬度、经度、东向速度、北向速度的终点值
经度（度）
纬度（度）
东向速度（m/s）
北向速度（m/s）
116.3446
39.9751
-0.0608
-0.0655
四、小结
本次的运行结果太小，觉得可能不对。检查了几天，把公式都重新推了一遍，也反复检查了程序的写法，但一直找不到问题所在，也有可能是我的想法有问题，希望老师在评阅时可以帮我找出其中的问题所在，予以指点，谢谢老师！
五、源程序
clear all;
clc;
Vx(1)=0.000048637; %初始化变量
Vy(1)=0.000206947;
Vz(1)=0;
pi=3.141592654;
fai(1)=91.637207*pi/180;
sit(1)=0.120992605*pi/180;
gam(1)=0.010445947*pi/180;
L(1)=39.975172*pi/180; %将初始位置经纬度变换成弧度
J(1)=116.344695283*pi/180;
Wie=7.292115147E-5; %给公式中的常数赋值
re=6378245;
e=1/298.3;
h=30;
g0=9.7803267714;
gk1=0.00193185138639;
gk2=0.00669437999013;
load('C:\Users\yuanerkai\Desktop\2011第二次作业\fw'); %读取文件中的数据
Fbx=f_INSc(1,:); %提取正东方向比力数据并定义
Fby=f_INSc(2,:); %提取正北方向比力数据并定义
Fbz=f_INSc(3,:); %提取天向比力数据并定义
Wx=wib_INSc(1,:); %提取陀螺正东方向角速率并定义
Wy=wib_INSc(2,:); %提取陀螺正北方向角速率并定义
Wz=wib_INSc(3,:); %提取陀螺天向角速率并定义
I=eye(4); %定义四阶矩阵

t=1/80;

q=[cos(fai(1)/2)*cos(sit(1)/2)*cos(gam(1)/2)-sin(fai(1)/2)*sin(sit(1)/2)*sin(gam(1)/2);
cos(fai(1)/2)*sin(sit(1)/2)*cos(gam(1)/2)-sin(fai(1)/2)*cos(sit(1)/2)*sin(gam(1)/2);
cos(fai(1)/2)*cos(sit(1)/2)*sin(gam(1)/2)+sin(fai(1)/2)*sin(sit(1)/2)*cos(gam(1)/2);
sin(fai(1)/2)*cos(sit(1)/2)*cos(gam(1)/2)+cos(fai(1)/2)*sin(sit(1)/2)*sin(gam(1)/2);]; %求取四元数的初值
q0=q(1,1);q1=q(2,1);q2=q(3,1);q3=q(4,1);
Ctb=[q0^2+q1^2-q2^2-q3^2 2*(q1*q2+q0*q3) 2*(q1*q3-q0*q2); %求取状态转移矩阵
2*(q1*q2-q0*q3) q0^2-q1^2+q2^2-q3^2 2*(q2*q3+q0*q1);
2*(q1*q3+q0*q2) 2*(q2*q3-q0*q1) q0^2-q1^2-q2^2+q3^2];
for i=1:48000

Rx(i)=re/(1-e*(sin(L(i)))^2); %求取两个半径
Ry(i)=re/(1+2*e-3*e*(sin(L(i)))^2);

Witt=[-Vy(i)/Ry(i);Wie*cos(L(i))+Vx(i)/Rx(i);Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i)]; %指令角速度计算
Witb=Ctb*Witt; %将指令角速度转换到平台坐标系
Wibb=[Wx(i);Wy(i);Wz(i)];
Wtbb=Wibb-Witb;
x=Wtbb(1,1)*t;y=Wtbb(2,1)*t;z=Wtbb(3,1)*t; %求取迭代矩阵中的各Δθ

A=[0 -x -y -z;x 0 z -y;y -z 0 x;z y -x 0]; %求取迭代矩阵[Δθ]
tamp=x^2+y^2+z^2; % （Δθ）^2的计算
q=((1-tamp/8+tamp^2/384)*I+(0.5-tamp/48)*A)*q; %套用迭代公式
q0=q(1,1);q1=q(2,1);q2=q(3,1);q3=q(4,1);
Ctb=[q0^2+q1^2-q2^2-q3^2 2*(q1*q2+q0*q3) 2*(q1*q3-q0*q2); %求取状态转移矩阵
2*(q1*q2-q0*q3) q0^2-q1^2+q2^2-q3^2 2*(q2*q3+q0*q1);
2*(q1*q3+q0*q2) 2*(q2*q3-q0*q1) q0^2-q1^2-q2^2+q3^2];
Cbt=inv(Ctb); %求逆
Ft=Cbt*[Fbx(i);Fby(i);Fbz(i)]; %将比力数据转移到地理坐标系
Fx(i)=Ft(1,1);Fy(i)=Ft(2,1);Fz(i)=Ft(3,1);

g=g0*(1+gk1*(sin(L(i)))^2)*(1-2*h/re)/sqrt(1-gk2*(sin(L(i)))^2); %在纬度发生变化的情况下追踪重力加速度的变化，减小计算误差
Vx(i+1)=(Fx(i)+(2*Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i))*Vy(i)-(2*Wie*cos(L(i))+Vx(i)/Rx(i))*Vz(i))*t+Vx(i);%正东方向速度计算
Vy(i+1)=(Fy(i)-(2*Wie*sin(L(i))+Vx(i)*tan(L(i))/Rx(i))*Vx(i)+Vy(i)*Vz(i)/Ry(i))*t+Vy(i);%正北方向速度计算
Vz(i+1)=(Fz(i)+(2*Wie*cos(L(i)+Vx(i))/Rx(i))*Vx(i)+Vy(i)*Vy(i)/Ry(i)-g)*t+Vz(i);%天向速度计算
L(i+1)=t*Vy(i)/Ry(i)+L(i);%纬度计算
J(i+1)=t*Vx(i)/(Rx(i)*cos(L(i)))+J(i);%经度计算
i=i+1;%i与1累加，直至将48000组数据处理完成循环结束
end
Lzz=L(48001)*180/pi %输出最终位置的纬度
Jzz=J(48001)*180/pi %输出最终位置的经度
Vxzz=Vx(48001) %正东方向的速度最终值
Vyzz=Vy(48001) %正北方向的速度最终值
figure(1)
plot(J*180/pi,L*180/pi);xlabel('经度'),ylabel('纬度'); %以经度为横轴，纬度为纵轴（单位为：度）作出系统位置坐标曲线图
T=0:1/80:600;
figure(2)
plot(T,Vy);xlabel('时间/秒'),ylabel('北向速度/m/s'); %以时间为横轴（单位：秒），东向速度为纵轴作出系统速度随时间变化曲线图
figure(3)
plot(T,Vx);xlabel('时间/秒'),ylabel('东向速度/m/s'); %以时间为横轴（单位：秒），北向速度为纵轴作出系统速度随时间变化曲线图

纠正第一次大作业中的错误，经纬度的输出图像经度和纬度写反了。纠正如下：
程序：figure(1)
plot(J*180/pi,L*180/pi);xlabel('经度'),ylabel('纬度');
输出图像：