基于matlab的捷联惯导算法设计及仿真,基于 Matlab 的捷联惯导算法设计及仿真1.doc...

基于 Matlab 的捷联惯导算法设计及仿真1.doc

精品论文推荐基于 Matlab 的捷联惯导算法设计及仿真1严恭敏 西北工业大学航海学院，西安 (710072) E-摘要根据圆锥误差补偿算法和划船误差补偿算法的研究成果，考虑到实际捷联惯导算法 仿真程序编写的方便性，总结了一些与捷联惯导更新算法有关的函数的计算公式。对圆锥误差补偿算法和捷联惯导算法进行了仿真，仿真结果和理论分析结论吻合。在附录中给出了Matlab 的 m 文件源程序代码，具有一定的参考价值。 关键词捷联惯导；四元数；等效旋转矢量；Matlab；算法；仿真 中图分类号V249.31. 引言在捷联惯导系统中采用数学平台，姿态更新解算是捷联惯导系统算法的核心部分，由于 四元数法算的优良特性，它在工程实际中经常被采用。为了减小姿态计算的不可交换性误差， 前人研究并建立了等效旋转矢量方程，高精度姿态更新解算的研究主要集中在等效旋转矢量 方程的求解上，在圆锥运动环境下，许多研究者提出并完善了圆锥误差补偿算法。基于圆锥 误差补偿算法和划船误差补偿算法的等效原理，可将圆锥误差补偿算法移植到划船误差补偿 算法中去，从而减少了划船误差推导的繁琐过程。上述研究都已经比较成熟1-6，本文根据 这些研究结果，并考虑到实际仿真程序编写的方便性，总结了一些与捷联惯导算法有关的函 数的计算公式或步骤，其中更详细的推导过程可见参考文献7,8。最后，对圆锥误差补偿算 法和捷联惯导算法进行了仿真。附录中给出了Matlab的m文件源程序代码具有一定的参考价 值。2. 捷联惯导算法文中选取东-北-天(E-N-U)地理坐标系为导航坐标系，记为 n 系；捷联惯组坐标系记 为 b 系。2.1 相关函数(1)四元数的共轭与乘积。四元数 q 可表示为 q q0 qv q0 q1i q2 j q3 k 。用 q表示 q 的共轭四元数； q q1q2 表示四元数 q 是四元数 q1 与 q2 的乘积，四元数相乘是不可交换的。b(2)四元数与向量相乘。一般情况下利用矩阵进行坐标变换，如关系式 v n C n v b ，同样利用四元数也可以表示坐标变换。设变换四元数 q n 与变换矩阵 C n 相对应，则定义变换bb四元数 q n 和向量 v b 的乘法，记为 v n q n v b ，它由以下规则实现首先，将向量扩展成四bb元数，令 qb 0 v b ；其次，做四元数乘法，令 q n q n qb q n ；最后，从四元数 q n 中提取bbv向量，有 v n q n 。(3)等效旋转矢量与四元数之间的转换。等效旋转矢量 v 转化为四元数 q 的公式为1 本课题得到水下信息处理与控制国家级重点实验室基金(9140C230206070C2306)资助。- 10 -q Fvqv cos v / 2 v sin v / 2v1其中 Fvq 表示从等效旋转矢量转换到四元数的函数，容易看出有 q Fvq v 成立。假设 v 是小量，则从 q 中可以求出 v ，首先令 vn 2 v / 2 arccosq0 ，再求得v Fqvq 2 vn 2 qsinvn 2 2其中 Fqv 表示从四元数转换到等效旋转矢量的函数。b(4)姿态向量 A 与姿态四元数 q n 之间的转换。记由俯仰角 、横滚角 和航向角 组成的向量 A T 为姿态向量，通过姿态矩阵 C n 作为过渡，容易实现 A 与 q n 之间vbb的转换，此处不作详细分析。2.2 地球模型通常给出的地球椭球模型参数为长半轴 Re 和扁率 f ，由椭球几何学容易得到其它参数，在惯性导航算法中经常用到，它们是偏心率 e 2 f f 2 、短半轴 R 1 f Re 、第二RppMe偏心率 ep 2 R 2 / R 、子午圈半径 R Re 1 e /1 e2sin 2L3 / 2、卯酉圈半径RN Re/1 e2 sin 2 L1/ 2 。另外，重力加速度 g 和地球自转角速率 也是惯性导航解算的必备参数，在 GRS80 椭球模型中，正常重力公式为3g g0 1 5.27094 1022sin 2 L 2.32718 105iepsin4 L 3.086 10-56 h3其中 g0 9.7803267714m/s ，h 为海拔高度，而ie 7.2921151467 10rad/s 。记地球自转角速度矢量在导航坐标系投影为 n ，惯导速度 v n 引起导航坐标系转动速度为 n ，则ien 0ieiecos Liesin LTen4an v n /R hv n /R hv n tan L /R hT4b并且记enNMENENnnn2.3 圆锥误差与划船误差补偿算法in ie en4c假设陀螺和加速度计均为等周期采样(采样周期 h )，圆锥误差与划船误差补偿周期均为T ，且T n h ， n 为子样数。再假设在第 m 个补偿周期中，陀螺角增量和加速度计比力速度增量 采样输出分 别为 m i 和 vm i ( i 1,2,3...n )，并令采样 总角增量 nnm i1 m i 和采样总比力速度增量 vm i 1 vm i ，则考虑圆锥误差补偿后的等效旋转矢量计算公式为m m n1ki m im n5i1上式中第二项为圆锥误差补偿量，系数 ki 的选择见表 1。根据划船误差补偿算法与圆锥误差补偿算法的对偶原理，则考虑划船误差补偿后的比力速度增量计算公式为vsfm vm 1 / 2 m vm i 1ki m i vm n i 1ki vm i m n6上式中第二项为旋转速度增量，第三项为划船误差补偿量。表 1 圆锥误差补偿算法系数子样数 nk1k2k3k422/3---39/2027/20--454/10592/105214/105-5250/504525/504650/5041375/5042.4 捷联惯导更新算法捷联惯导更新的基本思想是，将前一时刻的导航参数(姿态、速度和位置)作为初值， 利用前一时刻至当前时刻的惯性器件采样输出，解算当前时刻的导航参数，作为下一时刻捷 联惯导解算的初值，如此反复不断。惯性器件采样经过 2.3 小节的误差补偿后获得等效旋转 矢量 m 和比力速度增量 vsfm ，再经过以下三步骤便可实现捷联惯导更新，这里直接给出 计算公式。(1)速度更新算法nnnnnnnvm vm1 Fvq inm1Tm / 2qbm1 vsfm gm1 2iem1 enm1 vm1 Tm(2)位置更新算法7L L T v/ Rnmm1m Nm,m1M8a T sec Lv/ Rnmm1mm1 Em,m1N8b(3)姿态更新算法h h T vnmm1m Um,m18cnnn n3. 仿真与分析qbm qbm1Fvq m qbm1 inm1Tm 9根据第 2 节的分析，将各函数和算法编制成 Matlab 的 m 文件，文件名及简要功能如表2 所示，详细的源程序代码参见附录，文中还着重对圆锥误差补偿算法和捷联惯导更新算法 进行了仿真。表 2 Matlab 文件功能说明文件功能文件功能glvs.m全局变量a2quat.m由姿态向量求姿态四元数earth.m有关地球参数计算q2att.m由姿态四元数求姿态向量qconj.m qmul.m四元数共轭四元数相乘cnscl.m sins.m圆锥误差和划船误差补偿捷联惯导更新算法qmulv.m四元数乘向量test1.m圆锥误差补偿算法仿真rv2q.m等效旋转转换为四元数test2.m捷联惯导算法算法仿真q2rv.m四元数转换为等效旋转3.1 圆锥误差补偿算法仿真如图 1，动坐标系 b 系相对参考坐标系 r 系绕 or z r 轴作圆锥运动，半锥角为 ，锥运动 频率为 ，即b 系与 r 系坐标原点重合， ob xb 轴以 or xr 轴为对称中心，在 xr or z r 平面内 作幅值为 的振动； ob yb 轴以 or yr 轴为中心，在 yr or z r 平面内幅值为 的振动； ob zb 轴在以 or z r 轴为中心的圆锥面上运动。动坐标系 b 相对参考坐标系 r 的方位关系还可用姿态四元数描述为bq r t cos / 2 sin / 2 cos tsin / 2 sin t0 T10zrzb yr ybxror obxb图 1 b 系相对 r 系作圆锥运动两个相邻采样时刻之间的角增量输出为k 2 sin sinh / 2 sin tk h / 2k 1 2 sin sinh / 2 cos t h / 2 11 2h sin 2 / 2其中，h t k 1 t k ， k 1 为从 t k 时刻到 t k 1 时刻的角增量，即 t k 1 时刻采样获得的角增量输出。理论上可以证明，圆锥误差补偿算法在一个补偿周期T 内的算法漂移误差为n1 n nn122k1k 12 h2 n112图 2 圆锥误差补偿仿真当 1、 5Hz、 h 10ms、 n 3时，仿真一分钟的姿态误差如图2所示。其中蓝色曲线为圆锥误差补偿姿态与真实姿态之间的误差，可见在 or xr 轴和 or yr 轴有微小振荡误差，而 or zr 轴为圆锥漂移误差。最下面小图中红色曲线为利用公式(12)计算的理论圆锥 漂移误差，它与蓝色曲线非常接近。此外，当 1Hz、 h 10ms，而 和 n 取不同值时，进行了一系列的圆锥漂移误差仿 真和理论漂移误差计算，结果如表 3 所示。从表中可以看出，只有当半锥角 很小时，高 子样数的仿真误差和理论误差才比较接近，而当 较大时，高子样数的仿真误差和理论误差之间相差倍数很大，甚至在仿真误差中出现高子样补偿效果不如低子样的现象。产生这种现象的原因是，由于在圆锥漂移补偿算法推导过程中作出了半锥角 是小角度的假设，还须指 出的是，在推导过程中还假设圆锥频率和采样周期乘积 h 必须为小量。因此，在实际中必须结合应用环境选择合适的圆锥误差补偿算法，而并非子样数越高计算精度就越高，但是，如果考虑到实际陀螺的精度(如陀螺漂移 110-4 /h)，即使 为 10 时，选用 3 子样的计算精度也足够了，或者提高采样频率有利于降低计算误差。1 子样2 子样3 子样4 子样5 子样仿真误差6.013e-74.745e-104.016e-133.522e-169.558e-19理论误差6.012e-74.747e-104.013e-133.523e-163.161e-19仿真误差2.164e-31.708e-61.444e-91.612e-121.623e-12理论误差2.164e-31.709e-61.445e-91.268e-121.138e-15仿真误差7.7906.148e-34.596e-64.480e-62.103e-5理论误差7.7926.152e-35.205e-64.566e-94.097e-12仿真误差771.2420.596-5.455e-34.416e-22.075e-2理论误差779.2720.6155.205e-44.566e-74.097e-10表 3 一分钟圆锥漂移误差仿真和理论计算(单位)111103.2 捷联惯导算法仿真传统上用姿态矩阵表示的计算数学平台 C n 与真实数学平台 C n 之间的关系为bbn n nnCb C n Cb I Cb13其中 为平台误差角，它表示从 n 系到 n 系的小转动角向量(可以看成是等效旋转矢量)。假设变换矩阵 C n 、 C n 、 C n 分别与变换四元数 q n 、 q n 、 q n 相对应，则有 q n q n q n ，bnbbnnbn b并且利用矩阵与四元数之间的关系可以证明 q n F 成立，所以有nvqqvq bbn Fq n14aF q n q n Fq n q n 14bvq b bqv b b在仿真时，通过(14)式能够方便地进行平台误差角处理，与传统矩阵描述平台误差角相比， 四元数描述方法的优点是不必再作正交化处理。对静态导航进行了仿真，初始平台误差角分别设置为0.5、0.5和3，仿真一小时导航参 数结果如图3所示，其中速度误差和位置误差小图中，红色曲线为高度通道的误差，它们是 发散的，而水平通道(包括水平平台误差、水平速度和经纬度误差)呈现振荡趋势，方位平 台误差变化比较小。仿真结果符合惯导系统误差理论分析的结论。4. 小结图 3 捷联惯导算法仿真总结了一些与捷联惯导更新算法有关的函数的计算公式，对圆锥误差补偿算法和捷联惯 导算法进行了仿真。在圆锥误差补偿算法中发现，只有当半锥角很小时，高子样数的仿真误 差和理论误差才比较接近，否则相差倍数很大，因此在高精度应用场合，必须通过提高采样 频率来降低计算误差。捷联惯导静态导航仿真结果与惯导系统误差理论分析结论相吻合，证 明了所设计算法的正确性。参考文献1Bortz J E, A new mathematical ulation for strapdown inertial navigationJ, IEEE Transactions onAerospace and Electronic Systems,1971,7161-66.2Miller R B, A new strapdown attitude algorithmJ, Journal of Guidance, Control and Dynamics,1983,64287-291.3Roscoe K M, Equivalency between strapdown inertial navigation coning and sculling integrals /algorithmsJ Journal of Guidance, Control and Dynamics，2001，242201-2054秦永元. 惯性导航M. 北京科学出版社，2006.5张玲霞，陈明，曹晓青. 基于对偶性原理捷联惯导划船误差补偿优化算法J. 中国惯性技术学报，2003,11533-38.6潘献飞,吴文启,吴美平. 考虑机抖激光陀螺信号滤波特性的圆锥算法修正J. 中国惯性技术学报，2007,153259-264.7严恭敏. 捷联惯导算法及车载组和导航系统研究D. 西安西北工业大学硕士论文，2004,3.8严恭敏. 车载自主定位定向系统研究D. 西安西北工业大学博士论文，2006,5.Algorithm design and simulation for SINS based on MatlabYan GongminCollege of Marine，Northwestern Polytechnical University，Xian (710072)AbstractBased on the research achievements of coning error compensation and sculling error compensation algorithm in strapdown inertial navigation systemSINS, and considering the convenience ofsimulation software program, some calculation ulas about the SINS updating algorithm are summarized in this paper. Coning error compensation algorithm simulation and SINS algorithmsimulation are carried out and the simulation results are consistent with the theory analysis. Simulation source codes written in Matlabs m file are present in the appendix, and provide valuable references forSINS software designer.Keywordsstrapdown inertial navigation system；quaternion；equivalent rotate vector；Matlab；algorithm；simulation作者简介严恭敏(1977-)，男，福建建瓯人，2006 年 9 月毕业于西北工业大学自动化学 院，获导航、制导与控制专业博士学位，现在西北工业大学航海学院兵器科学与技术博士后 流动站工作，主要从事陆用定位定向系统、水下航行器自主导航技术与组合导航系统理论研 究。附录Matlab仿真程序 glvs.m 全局变量全局变量global glvglv.Re 6378160;地球半径glv.f 1/298.3;地球扁率glv.e sqrt2*glv.f-glv.f2; glv.e2 glv.e2; 地球椭圆度等其它几何参数glv.Rp 1-glv.f*glv.Re;glv.ep sqrtglv.Re2glv.Rp2/glv.Rp; glv.ep2 glv.ep2;glv.wie 7.2921151467e-5;地球自转角速率glv.g0 9.7803267714;重力加速度 glv.mg 1.0e-3*glv.g0; 毫重力加速度 glv.ug 1.0e-6*glv.g0;微重力加速度 glv.ppm 1.0e-6;百万分之一 glv.deg pi/180; 角度glv.min glv.deg/60; 角分 glv.sec glv.min/60; 角秒 glv.hur 3600;小时 glv.dph glv.deg/glv.hur;度/小时 glv.mil 2*pi/6000; 密位 glv.nm 1853; 海里 glv.kn glv.nm/glv.hur; 节2./3,0,0,09./20,27./20,0,054./105,92./105,214./105,0250./504,525./504,650./504,1375./504 ;glv.cs 圆锥和划船误差补偿系数 earth.m 有关地球参数计算function wnie, wnen, RMh, RNh, gn earthpos, vnglobal glvslsinpos1; clcospos1; tlsl/cl; sl2sl*sl; sl4sl2*sl2;wnie glv.wie*0; cl; sl;sq 1-glv.e2*sl2; sq2 sqrtsq;RMh glv.Re*1-glv.e2/sq/sq2pos3; RNh glv.Re/sq2pos3;wnen -vn2/RMh; vn1/RNh; vn1/RNh*tl;g glv.g0*15.27094e-3*sl22.32718e-5*sl4-3.086e-6*pos3; grs80 gn 0;0;-g; qconj.m 四元数共轭function qo qconjqiqo qi1; -qi24; qmul.m 四元数相乘function q qmulq1,q2q q11 * q21 - q12 * q22 - q13 * q23 - q14 * q24; q11 * q22 q12 * q21 q13 * q24 - q14 * q23; q11 * q23 q13 * q21 q14 * q22 - q12 * q24; q11 * q24 q14 * q21 q12 * q23 - q13 * q22 ; qmulv.m 四元数乘向量function vo qmulvq, viqi 0;vi;qo qmulqmulq,qi,qconjq;vo qo24,1; rv2q.m 等效旋转转换为四元数function q rv2qrv norm sqrtrv*rv; if norm1.e-20f sinnorm/2/norm/2;elseendf 1;q cosnorm/2; f/2*rv; q2rv.m 四元数转换为等效旋转function v q2rvq n2 acosq1; if n21e-20k 2*n2/sinn2;elseendk 2;v k*q24; a2quat.m 由姿态向量求姿态四元数function q a2quatatt 先求姿态矩阵si sinatt1; ci cosatt1; sj sinatt2; cj cosatt2;sk sinatt3; ck cosatt3;M cj*cksi*sj*sk, ci*sk, sj*ck-si*cj*sk;-cj*sksi*sj*ck, ci*ck, -sj*sk-si*cj*ck;-ci*sj,si,ci*cj ; 再求四元数q sqrtabs1.0 M1,1 M2,2 M3,3/2.0; signM3,2-M2,3 * sqrtabs1.0 M1,1 - M2,2 - M3,3/2.0; signM1,3-M3,1 * sqrtabs1.0 - M1,1 M2,2 - M3,3/2.0; signM2,1-M1,2 * sqrtabs1.0 - M1,1 - M2,2 M3,3/2.0 ; q2att.m 由姿态四元数求姿态向量function att q2attqnb 先求姿态矩阵q11 qnb1*qnb1; q12 qnb1*qnb2; q13 qnb1*qnb3; q14 qnb1*qnb4;q22 qnb2*qnb2; q23 qnb2*qnb3; q24 qnb2*qnb4;q33 qnb3*qnb3; q34 qnb3*qnb4;q44 qnb4*qnb4;M q11q22-q33-q44, 2*q23-q14,2*q24q13;2*q23q14,q11-q22q33-q44, 2*q34-q12;2*q24-q13,2*q34q12,q11-q22-q33q44 ; 再求姿态att asinM3,2; atan-M3,1/M3,3; atanM1,2/M2,2;if M3,3 0ifatt2 0 att2 att2pi; elseatt2 att2-pi; endendif M2,2 0ifatt3 0att3 att3pi*2;endelseatt3 att3pi;end cnscl.m 圆锥误差和划船误差补偿function phim, dvbm cnsclwm, vmglobal glvn sizewm,2;cm 0;0;0; sm 0;0;0;for k1n-1cm cm glv.csn-1,k*wm,k; 准备圆锥误差补偿endwmm sumwm,2;phim wmm crosscm,wm,n;if nargin2for k1n-1sm sm glv.csn-1,k*vm,k; 准备划船误差补偿endendvmm sumvm,2;dvbm vmm 1.0/2*crosswmm,vmm crosscm,vm,ncrosssm,wm,n; sins.m 捷联惯导更新function qnb, vn, pos sinsqnb_1, vn_1, pos_1, wm, vm, tstss ts*sizewm,2; phim,dvbm cnsclwm,vm; wnie,wnen,rmh,rnh,gn earthpos_1,vn_1; wnin wniewnen;vn vn_1 qmulvrv2q-wnin*1.0/2*tss,qmulvqnb_1,dvbm gn-crosswniewnin,vn_1*tss;pos pos_1 tss*vn_12/rmh;vn_11/rnh*cospos_11;vn_13;qnb qmulqnb_1, rv2qphim - qmulvqconjqnb_1,wnin*tss; test1.m 圆锥误差补偿仿真glvsclear err dthetaa 1*glv.deg; 半锥角w 2*pi*5;锥运动频率h 0.01;采样时间n 3;子样数len fix1*60/h; 仿真时间长度t 0;初始姿态四元数q cosa/2; sina/2*cosw*t; sina/2*sinw*t; 0; q1 q;for knnlen 姿态真值t k*h;q cosa/2; sina/2*cosw*t; sina/2*sinw*t; 0; 锥运动解算值for m1nt k-nm-1*h;dtheta,m -2*sina*sinw*h/2*sinw*th/2;2*sina*sinw*h/2*cosw*th/2;-2*w*h*sina/22 ; 角增量endendphi cnscldtheta;圆锥误差补偿q1 qmulq1,rv2qphi;姿态更新errk/n, -q2rvqmulq1,qconjq; 求姿态误差figuresubplot3,1,1, ploterr,1/glv.sec, ylabelitphi_xrm / arcsec; subplot3,1,2, ploterr,2/glv.sec, ylabelitphi_yrm / arcsec; subplot3,1,3, ploterr,3/glv.sec, ylabelitphi_zrm / arcsec; str sprintfittrm / times.0fms, n*h*1000; xlabelstr; 理论圆锥误差k2 1;for k1n1k2 k2*2*k-1;endepsilon a2*w*h2*n1 * n*factorialn / 2n1*k2;hold on, plot0,lengtherr,0,epsilon*lengtherr/glv.sec,r-- test2.m 捷联惯导更新仿真glvsclear errphi errvn errpos ts0.1;采样周期att00;0;0*glv.deg; vn00;0;0; pos00*glv.deg;0;0;初始值设置qnb0 a2quatatt0;vn vn0; pos pos0;wnie glv.wie*0; cospos01; sinpos01;g glv.g0*15.27094e-3*sinpos0122.32718e-5*sinpos014-3.086e-6*pos03;wbib qmulvqconjqnb0,wnie;fb qmulvqconjqnb0,0;0;g;wmwbib*ts; vmfb*ts;静止时角增量和比力增量qnb qmulrv2q-0.5;0.5;3*glv.min,qnb0; 加入姿态误差kk 1;for k13600*10 qnb,vn,possinsqnb,vn,pos,wm,vm,ts; if modk,100errphikk, -q2rvqmulqnb,qconjqnb0; 求姿态误差errvnkk, vn-vn0;errposkk, pos-pos0;kk kk1;endendfigure,subplot3,1,1,ploterrphi/glv.min, ylabelitphirm / arcmin; subplot3,1,2,ploterrvn, ylabelitdelta Vrm / m/s; subplot3,1,3,ploterrpos,12*glv.