《惯性导航原理》第二次大作业一、原理分析在利用方向余弦方法对惯性导航系统进行测算时，刚体空间位置用固连于刚体的动坐标系对固定参考系各轴的九个方向余弦来确定，九个方向余弦角存在六个约束条件，计算比较繁琐，模型也比较复杂。

如果在计算过程中引入四元数，则可以通过坐标系的一次转动，实现方向余弦方法中的三次坐标旋转。

原理图如下：λ、L、hVx、Vy、Vz从原理图可以清楚的看出，通过捷联姿态矩阵C b t可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法即可以得到任意时刻载体的位置和速度信息。

关键在于捷联姿态矩阵的求解。

在这里应用四元数知识进行解算。

1. 捷联姿态矩阵的求解因为平台的初始姿态角都是已知的，则可以先求解四元数中各元的初值。

平台坐标系相对于地理坐标系的三次旋转可以由四元数的乘积得到。

用四元数表示这三次转动为（逆时针为正）：Q 1=cos φ2+zsin φ2Q 2=cos θ2+xb1sin θ2Q 3=cos γ2+ybsin γ2对三者进行四元素乘法运算：Q =Q 3∘Q 2∘Q 1结果与四元数Λ=λ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带入四元数姿态矩阵即可得到捷联姿态矩阵：C t b =(λ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)通过此矩阵可以将地理坐标系的参数转移到平台坐标系。

此矩阵的逆矩阵C bt就是将比力信息从平台坐标系转移到地理坐标系的姿态矩阵。

此外此矩阵还要在四元数的迭代计算中使用。

下面进行四元数的迭代计算。

四元数微分方程的矩阵形式为(λ0λ1λ2λ3)=12(ωtbx b−ωtbx b−ωtby b−ωtbz bωtbz b −ωtby bωtby b−ωtbz b0 ωtbx bωtbz bωtby b−ωtbx b 0)(λ0λ1λ2λ3)式中ωtb b=ωib b−C t bωit t其中ωib b即为陀螺仪所测量的角速度值，ωit t为平台的指令角速度，为地球的自转角速率与地理坐标系相对于地球坐标系的角速率之和，即ωit t=ωie t+ωet t地球的自转角速率为：ωie t=[ωiex tωiey tωiez t]=[ωie cosLωie sinL]地理坐标系相对于地球坐标系的角速率为：ωet t=[ωetx tωety tωetz t] =[−V ety tR yt V etxtR xtV etxtR xttanL]上述四元数方程的解和下面矩阵方程的解类似：Q（λ）=12M∗(ωtb b)Q（λ）记q(t)=[λ0λ1λ2λ3]T，由角增量法可得迭代公式（取四阶算法）：q(n+1)={[1−(∆θ0)28+(∆θ0)4384]I+[12−(∆θ0)248][∆θ]}q(n)（n=0,1,2，……）式中[∆θ]=∫M ∗(ωtb b )t 2t 1dt =( 0∆θx −∆θx 0−∆θy −∆θz ∆θz −∆θy ∆θy−∆θz 0 ∆θx ∆θz∆θy−∆θx 0)(∆θ0)2=∆θx 2+∆θy 2+∆θz 2q(0)即为解算出的初值，带入迭代公式即可得到任意时刻的捷联姿态矩阵C b t。

通过捷联姿态矩阵C b t 可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法求解任意时刻载体的位置和速度信息即可。

2、指北方位平台式惯导求解方位和速度载体相对地球运动时，加速度计测得的比力表达式，称为比力方程，方程如下：g V V f ep ep ie ep -⨯++=)2(ϖϖ在指北方案中，平台模拟地理坐标系，将上式中平台坐标系用地理坐标系代入得：t t t et t ie t t g V f V +⨯+-=)2(ϖϖ系统中测量的是比力分量，将上式写成分量形式[ t x V t yV V z t ]= [f x t f y t f z t ] - [0−(2ωiez t +ωetz t )(2ωiey t +ωety t )(2ωiez t +ωetz t )0−(2ωiex t +ωetx t )−(2ωiey t +ωety t )(2ωiex t +ωetx t )0][V x t V y t V z t] + [00g]将ωie t 和ωet t的表达式带入上式，即可得到如下方程组：{t z xt tx ie t y xt t x iet xtxV R V L V L R V L f V )cos 2()tan sin 2(+-++=ωω t zyt t y t x xt tx ie t y t y V R V V L R V L f V ++-=)tan sin 2(ω V z t =f z t +(2ωie cosL +V x t R xt )V x t + V y t R ytV y t – g（6） 作业要求只考虑水平通道，因此只需要计算正东、正北两个方向的速度即可。

理论上计算得到t xV 、t y V 后，再积分一次可得到速度值，即⎪⎩⎪⎨⎧+=+=⎰⎰tt y t y t y t tx t x t x V dt V V V dt V V 0000 但在本次计算过程中，三个方向的速度均是从零开始在各时间节点上的累加，并不是t 的函数，因此速度计算可以由以下方程组实现{V x t (i +1)={f x t (i )+[2ωie sinL (i )+V x t (i )R xt (i )tanL (i )]V y t (i )−[2ωie cos L (i )+V x t (i )R xt (i )]V z t }∗0.01+V x t(i )V y t (i +1)={f y t (i )−[2ωie sinL (i )+V x t (i )R xt (i )tanL (i )]V x t (i )+[V y t (i )R yt (i )]V z t }∗0.01+V y t (i )V z t (i +1)={f z t (i )+[2ωie cosL (i )+V x t (i )R xt (i )]V x t (i )+[V y t (i )R yt(i )]V y t −g}∗0.01+V z t (i) 此方程组表示了从第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 xt t xt yt tyLdt R V L dt R V L 000sec λλ与速度的计算相同，经纬度也不是t 的函数，可以由累加得到：{()()()()ｙyL i+1=0.01V i/R i+L i;()()()()()()()x xJ i10.01V i/R i*cos L i J i;+=+二、程序流程图三、导航结果1.系统位置坐标曲线图2.系统东向速度随时间变化曲线图3.系统北向速度随时间变化曲线图4．系统纬度、经度、东向速度、北向速度的终点值四、小结本次的运行结果太小，觉得可能不对。

检查了几天，把公式都重新推了一遍，也反复检查了程序的写法，但一直找不到问题所在，也有可能是我的想法有问题，希望老师在评阅时可以帮我找出其中的问题所在，予以指点，谢谢老师！五、源程序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(ga m(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:48000Rx(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))+V x(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组数据处理完成循环结束endLzz=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'); %以时间为横轴（单位：秒），北向速度为纵轴作出系统速度随时间变化曲线图纠正第一次大作业中的错误，经纬度的输出图像经度和纬度写反了。