捷联惯导作业

一、原理分析：捷联式惯导系统是将惯性器件（陀螺仪和加速度计）直接固连在载体上的系统。

图1为捷联式惯导系统的原理图，陀螺仪和加速度计输出分别送入姿态矩阵计算和由载体坐标系至平台坐标系的方向余弦矩阵的计算。

有了姿态矩阵，其一可以实现把载体坐标系轴向加速度信息变换到导航坐标系轴，进而可以进行所需的导航参数计算，其二利用姿态矩阵的元素，提取方位和姿态信息。

图1. 捷联式惯导系统的原理图 姿态速率微分方程为:12b tbωΛ=Λ (1)其中；()b b b t ttb ib t ie et C ωωωω=-- (2)bib ω为陀螺仪测量经补偿后的值；0cos sin t iex tt ieiey ie t ie iez L L ωωωωωω⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦，为地球自转角速率； tan tety t yt etx t tt etx etety xtt etz tetxxt V R V R V L R ωωωω⎡⎤-⎢⎥⎢⎥⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎢⎥⎢⎥⎣⎦，为地理坐标系相对地球坐标系的转动角速率； 导航坐标系到载体坐标系的姿态矩阵为：cos cos sin sin sin sin cos cos sin sin cos sin sin cos cos cos sin cos sin sin sin cos sin sin cos sin cos cos cos tt C ψϕψθϕψϕψθϕθϕψθψθθψϕψθϕψϕψθϕθϕ-+-⎡⎤⎢⎥=-⎢⎥⎢⎥+-⎣⎦(3)对应的四元素初值为:0123cos cos cos sin sin sin 222222cos sin cos sin cos sin 222222cos cos sin sin sin cos 222222cossinsinsincoscos222222ψθϕψθϕλψθϕψθϕλψθϕψθϕλψθϕψθϕλ⎧=-⎪⎪⎪=-⎪⎨⎪=+⎪⎪⎪=+⎩(4)四元素姿态矩阵为：2222012312031302222212030123230122221302230101232()2()2()2()2()2()bt C λλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλλ⎡⎤+++++⎢⎥=--+-+⎢⎥⎢⎥----+⎣⎦ (5)将姿态速率微分方程展开成矩阵形式：0112233001020bbbtbxtbytbx b bb tbx tbz tby b b btby tbztbx b bbtbz tbytbxλλωωωλλωωωωωωλλωωωλλ⎡⎤⎡⎤⎡⎤---⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦(6)该微分方程的解可用比卡逼近法求解，解得[]000s i n 2()c o s (0)2t I λλλλλλ∆⎧⎫⎪⎪∆=+∆⎨⎬∆⎪⎪⎩⎭ (7)其中：22202((()))b b b tbx tby tbz ωωωλ+∆+=，220I λλ∆=-∆将0cos2λ∆和00sin2λλ∆∆展成级数并提取有限项，得到姿态矩阵的算法为：[]2420001(1)(1)()()8384248n I n λλλλλλ⎧⎫∆∆∆+=-++-∆⎨⎬⎩⎭ (8)根据比力方程比力方程：f a G =-(2)t t tttti e e tV f V gωω=-+⨯+(9)写为分量形式：0(2)(2)0(2)0(2)0(2)(2)0t t tt t t tx xx iez etz iey ety t t t t t tt y y iez etz iex etx y t t t t t t t iey ety iex etx z z z V f V V f V g f V V ωωωωωωωωωωωω⎡⎤⎡⎤⎡⎤⎡⎤ -+ + ⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥=- +-++⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥-+ +-⎣⎦⎣⎦⎣⎦⎣⎦⎣⎦ (10)从而有：(2s i n )(2c o s )(2s i n )(2c o s )ttt tt txx x x ie yiezxtxt tt yt tt txy y ie xzxt ytttyt t ttxz x iex y xtytV V V f L tgL V L V R R V V V f L tgL V V R R V V V f L V V gR R ωωωω⎧=++-+⎪⎪⎪⎪=-++⎨⎪⎪⎪=+++-⎪⎩(11)其中：22Re(1sin ())Re(1(23sin ()))Rxt e L Ryt e L =+=-- (12)天向速度为零，高度通道保持不变，上式可写为：2(s i n )t a n2(s i n )t a nt tx y t t tx x ie y xt t tt t t x xy y ie x ytV V V f LV L R V V V f L V L R ωω⎧=++⎪⎪⎨⎪=--⎪⎩(13)将加速度对时间积分可以得到X 、Y 两个方向的速度：00y 0(2sin tan )(2sin tan )tt t t tx xie x x xttt tttxy ie y xtV V L L V dt V R V V L L V dt V R ωω=++=++⎰⎰(14)X 、Y 两个方向的速度除以对应的地球曲率半径，再次积分得到载体的经纬度位置：(0)(0)c o s tt yyttt xxt V L dt L R V dt R Lλλ=+=+⎰⎰(15)二、程序流程图：三、导航结果：不考虑天向速度及飞行高度的导航结果如下：1.位置坐标曲线：2.系统东向速度曲线：3.系统北向速度曲线：4.系统终点值：四、小结：1、对捷联式惯导系统有了一定的了解。

2、熟悉了捷联式惯导系统的解算方法。

3、熟悉了四元素法在惯导解算中的应用。

五、源程序：clcclearclose allData=load('C:\Users\hp\Desktop\fw.dat');w=Data(:,2:4); %加载加速度计数据f=Data(:,5:7); %加载陀螺仪数据[M,N]=size(f);Wie=7.292115147e-5; %地球自传角速度re=6378245; %地球椭球长半径h=30; %飞行高度e=1/298.3; %定义常量T=1/80; %采样周期为1/80E=eye(4); %单位矩阵%初始经纬度longitude=zeros(1,M);latitude=zeros(1,M);longitude(1)=116.344695283*pi/180; %初始经度（弧度）latitude(1)=39.975172*pi/180; %初始纬度（弧度）%初始速度velocityE=0.000048637; %东向速度velocityN=0.000206947; %北向速度velocity=zeros(2,M);%初始姿态角Ksai=[91.637207*pi/180 zeros(1,M)]; %航向角（弧度）Seita=[0.120992605*pi/180 zeros(1,M)]; %俯仰角（弧度）Gama=[0.010445947*pi/180 zeros(1,M)]; %横滚角（弧度）%重力加速度计算参数g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;%四元素初始值lamda0=cos(0.5*Ksai(1))*cos(0.5*Seita(1))*cos(0.5*Gama(1))-sin(0.5*Ksai(1))*sin(0.5*Seita(1))*sin(0.5*Gama(1));lamda1=cos(0.5*Ksai(1))*sin(0.5*Seita(1))*cos(0.5*Gama(1))-sin(0.5*Ksai(1))*cos(0.5*Seita(1))*sin(0.5*Gama(1));lamda2=cos(0.5*Ksai(1))*cos(0.5*Seita(1))*sin(0.5*Gama(1))+sin(0.5*Ksai(1))*sin(0.5*Seita(1))*cos(0.5*Gama(1));lamda3=cos(0.5*Ksai(1))*sin(0.5*Seita(1))*sin(0.5*Gama(1))+sin(0.5*Ksai(1))*cos(0.5*Seita(1))*cos(0.5*Gama(1));%用四元素表示得姿态矩阵Ctb=[lamda0^2+lamda1^2-lamda2^2-lamda3^22*(lamda1*lamda2+lamda0*lamda3)2*(lamda1*lamda3-lamda0*lamda2);2*(lamda1*lamda2-lamda0*lamda3)lamda0^2-lamda1^2+lamda2^2-lamda3^22*(lamda2*lamda3+lamda0*lamda1);2*(lamda1*lamda3+lamda0*lamda2)2*(lamda2*lamda3-lamda0*lamda1)lamda0^2-lamda1^2-lamda2^2+lamda3^2];for i=2:Mlamda=[lamda0 lamda1 lamda2 lamda3]'; %四元素的四个元素值f_ins=f(i,:)';w_ins=w(i,:)';Ryt=re*(1-2*e+3*e*(sin(latitude(i)))^2); %计算子午圈主曲率半径Rxt=re*(1+e*(sin(latitude(i)))^2); %计算卯酉圈主曲率半径g=g0*(1+gk1*(sin(latitude(i)))^2)*(1-2*h/re)/sqrt(1-gk2*(sin(latitude(i))) ^2); %重力加速度计算Cbt=Ctb';f_t=Cbt*f_ins; %将体轴系中的比例转化到地理系%加速度计算ax=f_t(1)+2*Wie*sin(latitude(i-1))*velocityN+velocityE*velocityN*tan(lati tude(i-1))/Rxt; %东向加速度分量ay=f_t(2)-2*Wie*sin(latitude(i-1))*velocityE-velocityE*velocityE*tan(latitu de(i-1))/Rxt; %北向加速度分量%速度计算velocityE=velocity(1,i-1)+ax*T;velocityN=velocity(2,i-1)+ay*T;velocity(1,i)=velocityE;velocity(2,i)=velocityN;%经纬度计算longitude(i)=longitude(i-1)+velocity(1,i-1)/Rxt*sec(latitude(i))*T; latitude(i)=latitude(i-1)+velocity(2,i-1)/Ryt*T;%本体系相对地理系的角速度Wetx_t(i)=-velocity(2,i-1)/Ryt;Wety_t(i)= velocity(1,i-1)/Rxt;Wetz_t(i)=velocity(1,i-1)*tan(latitude(i-1))/Rxt; %在地理坐标系的位移角速率Wet_t=[Wetx_t(i-1) Wety_t(i-1) Wetz_t(i-1)]';%陀螺仪测的角速率值Wib_b=[w_ins(1) w_ins(2) w_ins(3)]';%在地理坐标系的地球角速率Wie_t=[0 Wie*cos(latitude(i)) Wie*sin(latitude(i))]';%姿态矩阵角速率Wtb_b=Wib_b-Ctb*(Wie_t+Wet_t);%用角增量法计算四元素姿态矩阵Ma=[0 -Wtb_b(1) -Wtb_b(2) -Wtb_b(3);Wtb_b(1) 0 Wtb_b(3) -Wtb_b(2);Wtb_b(2) -Wtb_b(3) 0 Wtb_b(1);Wtb_b(3) Wtb_b(2) -Wtb_b(1) 0];Ma=Ma*T;dertaQ0=sqrt((Ma(1,2))^2+(Ma(1,3))^2+(Ma(1,4))^2);lamda=(cos(dertaQ0/2)*eye(4)+sin(dertaQ0/2)/dertaQ0*Ma)*lamda; lamda0=lamda(1);lamda1=lamda(2);lamda2=lamda(3);lamda3=lamda(4 );Ctb=[lamda0^2+lamda1^2-lamda2^2-lamda3^22*(lamda1*lamda2+lamda0*lamda3)2*(lamda1*lamda3-lamda0*lamda2);2*(lamda1*lamda2-lamda0*lamda3)lamda0^2-lamda1^2+lamda2^2-lamda3^22*(lamda2*lamda3+lamda0*lamda1);2*(lamda1*lamda3+lamda0*lamda2)2*(lamda2*lamda3-lamda0*lamda1)lamda0^2-lamda1^2-lamda2^2+lamda3^2];end%将弧度换算为角度longitude=longitude*180/pi;latitude =latitude*180/pi;%绘制曲线图%绘制经度变化曲线图figure(1)plot(longitude, latitude, 'b', 'linewidth', 2 );grid onxlabel('经度（°）');ylabel('纬度（°）');title('经维度变化曲线图'); %以时间为横轴，东向速度为纵轴作图t=1/80:1/80:T*(M); %时间矩阵figure(2);plot(t,velocity(1,:),'r');title('东向速度时间坐标曲线图');xlabel('时间(s)');ylabel('速度(m/s)');grid on;%以时间为横轴，北向速度为纵轴作图figure(3);plot(t,velocity(2,:),'r');title('北向速度时间坐标曲线图');xlabel('时间(s)');ylabel('速度(m/s)');grid on;。