《北惯导第二次大作业.doc》由会员分享，可在线阅读，更多相关《北惯导第二次大作业.doc（12页珍藏版）》请在金锄头文库上搜索。
1、惯性导航原理第二次大作业姓名:袁二凯 学号:SY0917145 联系方式:13488854452 2011年11月8日一、 原理分析指北方位平台式惯导解算在利用方向余弦方法对惯性导航系统进行测算时，刚体空间位置用固连于刚体的动坐标系对固定参考系各轴的九个方向余弦来确定，九个方向余弦角存在六个约束条件，计算比较繁琐，模型也比较复杂。如果在计算过程中引入四元数，则可以通过坐标系的一次转动，实现方向余弦方法中的三次坐标旋转。原理图如下：Cbt加速度计fbft、L、hVx、Vy、Vz姿态速率微分方程Witt Wtbb陀螺组件Ctb Wibb + Witb 从原理图可以清楚的看出，通过捷联姿态矩阵Cbt
2、可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法即可以得到任意时刻载体的位置和速度信息。关键在于捷联姿态矩阵的求解。在这里应用四元数知识进行解算。1. 捷联姿态矩阵的求解因为平台的初始姿态角都是已知的，则可以先求解四元数中各元的初值。平台坐标系相对于地理坐标系的三次旋转可以由四元数的乘积得到。用四元数表示这三次转动为（逆时针为正）：Q1=cos2+zsin2Q2=cos2+xb1sin2Q3=cos2+ybsin2 对三者进行四元素乘法运算：Q=Q3Q2Q1 结果与四元数=0+1x+2y+3z中的各元素相对应，就可以从已知的平台初始姿态求解出四元数
3、的各元初值。0=cos2cos2cos2-sin2sin2sin21=cos2sin2cos2-sin2cos2sin22=cos2cos2sin2+sin2sin2cos23=sin2cos2cos2+cos2sin2sin2带入四元数姿态矩阵即可得到捷联姿态矩阵：Ctb=02+12+22+322(12+03)2(13-02)2(12-03)02-12+22-322(23+01)2(13+02)2(23-01)02-12-22+32通过此矩阵可以将地理坐标系的参数转移到平台坐标系。此矩阵的逆矩阵Cbt就是将比力信息从平台坐标系转移到地理坐标系的姿态矩阵。此外此矩阵还要在四元数的迭代计算中使用
4、。下面进行四元数的迭代计算。四元数微分方程的矩阵形式为0123=120tbxb-tbxb0-tbyb-tbzbtbzb -tbybtbyb-tbzb 0 tbxbtbzbtbyb-tbxb 00123式中tbb=ibb-Ctbitt 其中ibb即为陀螺仪所测量的角速度值，itt为平台的指令角速度，为地球的自转角速率与地理坐标系相对于地球坐标系的角速率之和，即itt=iet+ett地球的自转角速率为： iet=iextieytiezt=0iecosLiesinL 地理坐标系相对于地球坐标系的角速率为： ett=etxtetytetzt = -VetytRytVetxtRxtVetxtRxttan
5、L 上述四元数方程的解和下面矩阵方程的解类似：Q（）=12M*tbbQ（）记q(t)=0123T，由角增量法可得迭代公式（取四阶算法）：q(n+1)=1-028+04384I+12-0248 q(n)（n=0,1,2，）式中=t1t2M*(tbb)dt=0x-x0-y-zz -yy-z 0 xzy-x 002=x2+y2+z2q(0)即为解算出的初值，带入迭代公式即可得到任意时刻的捷联姿态矩阵Cbt。通过捷联姿态矩阵Cbt可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法求解任意时刻载体的位置和速度信息即可。2、指北方位平台式惯导求解方位和速度载体相
6、对地球运动时，加速度计测得的比力表达式，称为比力方程，方程如下： 在指北方案中，平台模拟地理坐标系，将上式中平台坐标系用地理坐标系代入得： 系统中测量的是比力分量，将上式写成分量形式Vzt = fxtfytfzt - 0-(2iezt+etzt)(2ieyt+etyt)(2iezt+etzt)0-(2iext+etxt)-2ieyt+etyt2iext+etxt0VxtVytVzt + 00g 将iet和ett的表达式带入上式，即可得到如下方程组： Vzt=fzt +(2iecosL+VxtRxt)Vxt + VytRytVyt g （6）作业要求只考虑水平通道，因此只需要计算正东、正北两个方
7、向的速度即可。理论上计算得到、后，再积分一次可得到速度值，即但在本次计算过程中，三个方向的速度均是从零开始在各时间节点上的累加，并不是t的函数，因此速度计算可以由以下方程组实现Vxti+1=fxti+2iesinLi+VxtiRxtitanLiVyti-2iecosLi+VxtiRxtiVzt*0.01+VxtiVyti+1=fyti-2iesinLi+VxtiRxtitanLiVxti+VytiRytiVzt*0.01+VytiVzti+1=fzti+2iecosLi+VxtiRxtiVxti+VytiRytiVyt-g*0.01+Vzti此方程组表示了从第i个采集点到第（i+1）个采集点的
8、速度递推公式。方程中Rx表示卯酉圈的曲率半径，Ry表示子午圈的曲率半径，计算方法如下：Rx=Re/(1-esin2L);Ry=Re/(1+2e-3esin2L); 由于平台在运动中纬度L也在不断变化，因此，计算过程中应当追踪两个半径的变化。另外方程组中g表征平台所处纬度下的重力加速度： g=g0*(1+gk1*c332)*(1-2*h/re)/sqrt(1-gk2*c332)； 其中g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin(纬度); 为了尽可能减小计算的累积误差，计算过程中可以对g的变化进行追踪。 3.经
9、纬度计算 载体所在位置的地理纬度L，经度可由下列方程求得： 与速度的计算相同，经纬度也不是t的函数，可以由累加得到：二、程序流程图 开始根据初始姿态角求解四元数的初值Ctb 陀螺组件+- -tbbCtb四元数微分方程（迭代法）Cbt 求逆 加速度计指北方位的平台式惯导解算itt 输出经纬度，东向北向的速度结束三、导航结果1.系统位置坐标曲线图2.系统东向速度随时间变化曲线图3. 系统北向速度随时间变化曲线图4系统纬度、经度、东向速度、北向速度的终点值经度（度）纬度（度）东向速度（m/s）北向速度（m/s）116.344639.9751-0.0608-0.0655四、小结 本次的运行结果太小，觉
10、得可能不对。检查了几天，把公式都重新推了一遍，也反复检查了程序的写法，但一直找不到问题所在，也有可能是我的想法有问题，希望老师在评阅时可以帮我找出其中的问题所在，予以指点，谢谢老师！五、源程序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.34
11、4695283*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:UsersyuanerkaiDesktop2011第二次作业fw); %读取文件中的数据Fbx=f_INSc(1,:); %提取正东方向比力数据并定义Fby=f_INSc(2,:); %提取正北方向比力数据并定义Fbz=f_INSc(3,:); %提取天向比力数据并定义Wx=wib_INSc(1,:); %提取陀螺正东方向角速
12、率并定义 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)
