基于Matlab-的捷联惯导算法设计及仿真

严恭敏西北工业大学航海学院，西安（710072）E-mail：[e-mail]摘要：根据圆锥误差补偿算法和划船误差补偿算法的研究成果，考虑到实际捷联惯导算法仿真程序编写的方便性，总结了一些与捷联惯导更新算法有关的函数的计算公式。对圆锥误差补偿算法和捷联惯导算法进行了仿真，仿真结果和理论分析结论吻合。在附录中给出了Matlab的m文件源程序代码，具有一定的参考价值。关键词：捷联惯导；四元数；等效旋转矢量；Matlab；算法；仿真中图分类号：V249.31.引言在捷联惯导系统中采用数学平台，姿态更新解算是捷联惯导系统算法的核心部分，由于四元数法算的优良特性，它在工程实际中经常被采用。为了减小姿态计算的不可交换性误差，前人研究并建立了等效旋转矢量方程，高精度姿态更新解算的研究主要集中在等效旋转矢量方程的求解上，在圆锥运动环境下，许多研究者提出并完善了圆锥误差补偿算法。基于圆锥误差补偿算法和划船误差补偿算法的等效原理，可将圆锥误差补偿算法移植到划船误差补偿算法中去，从而减少了划船误差推导的繁琐过程。上述研究都已经比较成熟[1-6]，本文根据这些研究结果，并考虑到实际仿真程序编写的方便性，总结了一些与捷联惯导算法有关的函数的计算公式或步骤，其中更详细的推导过程可见参考文献[7,8]。最后，对圆锥误差补偿算法和捷联惯导算法进行了仿真。附录中给出了Matlab的m文件源程序代码具有一定的参考价值。2.捷联惯导算法文中选取东-北-天（E-N-U）地理坐标系为导航坐标系，记为n系；捷联惯组坐标系记为b系。2.1相关函数（1）四元数的共轭与乘积。四元数q可表示为kqjqiqqqv32100+++=+=qq。用∗q表示q的共轭四元数；21qqq=表示四元数q是四元数1q与2q的乘积，四元数相乘是不可交换的。（2）四元数与向量相乘。一般情况下利用矩阵进行坐标变换，如关系式bnbnvCv=，同样利用四元数也可以表示坐标变换。设变换四元数nbq与变换矩阵nbC相对应，则定义变换四元数nbq和向量bv的乘法，记为bnbnvqv=，它由以下规则实现：首先，将向量扩展成四元数，令bbvq+=0；其次，做四元数乘法，令∗=nbbnbnqqqq；最后，从四元数nq中提取向量，有nvnqv=。（3）等效旋转矢量与四元数之间的转换。等效旋转矢量v转化为四元数q的公式为1本课题得到水下信息处理与控制国家级重点实验室基金（9140C230206070C2306）资助。)2/sin()2/cos()(vvvvvFq+==→qv(1)其中)(•→qvF表示从等效旋转矢量转换到四元数的函数，容易看出有)(vFq−=→∗qv成立。假设v是小量，则从q中可以求出v，首先令)arccos(2/02qvn==v，再求得vnnvqvvqqFv)sin(2)(22⋅==→(2)其中)(•→vqF表示从四元数转换到等效旋转矢量的函数。（4）姿态向量A与姿态四元数nbq之间的转换。记由俯仰角θ、横滚角γ和航向角ψ组成的向量[]Tψγθ=A为姿态向量，通过姿态矩阵nbC作为过渡，容易实现A与nbq之间的转换，此处不作详细分析。2.2地球模型通常给出的地球椭球模型参数为长半轴eR和扁率f，由椭球几何学容易得到其它参数，在惯性导航算法中经常用到，它们是偏心率22ffe−=、短半轴epRfR)1(−=、第二偏心率ppeRRRep/22+=、子午圈半径2/3222)sin1/()1(LeeRReM−−=、卯酉圈半径2/122)sin1/(LeRReN−=。另外，重力加速度g和地球自转角速率ieω也是惯性导航解算的必备参数，在GRS80椭球模型中，正常重力公式为hLLgg64523010086.3)sin1032718.2sin1027094.51(−−−×−×+×+=(3)其中7803267714.90=gm/s2，h为海拔高度，而rad/s10677.29211514-5×=ieω。记地球自转角速度矢量在导航坐标系投影为nieω，惯导速度nv引起导航坐标系转动速度为nenω，则[]Tsincos0LLieienieωω=ω(4a)[]T)/(tan)/()/(hRLvhRvhRvNnENnEMnNnen+++−=ω(4b)并且记nennieninωωω+=(4c)2.3圆锥误差与划船误差补偿算法假设陀螺和加速度计均为等周期采样（采样周期h），圆锥误差与划船误差补偿周期均为T，且hnT⋅=，n为子样数。再假设在第m个补偿周期中，陀螺角增量和加速度计比力速度增量采样输出分别为)(im∆θ和)(im∆v（ni...3,2,1=），并令采样总角增量∑==nimmi1)(∆θ∆θ和采样总比力速度增量∑==nimmi1)(∆v∆v，则考虑圆锥误差补偿后的等效旋转矢量计算公式为)(])([11nikmnimimm∆θ∆θ∆θΦ×+=∑−=(5)上式中第二项为圆锥误差补偿量，系数ik的选择见表1。根据划船误差补偿算法与圆锥误差补偿算法的对偶原理，则考虑划船误差补偿后的比力速度增量计算公式为)(2/1mmmsfm∆v∆θ∆v∆v×+=)}(])([)(])({[1111niknikmnimimnimi∆θ∆v∆v∆θ×+×+∑∑−=−=(6)上式中第二项为旋转速度增量，第三项为划船误差补偿量。表1圆锥误差补偿算法系数子样数nk1k2k3k422/3---39/2027/20--454/10592/105214/105-5250/504525/504650/5041375/5042.4捷联惯导更新算法捷联惯导更新的基本思想是，将前一时刻的导航参数（姿态、速度和位置）作为初值，利用前一时刻至当前时刻的惯性器件采样输出，解算当前时刻的导航参数，作为下一时刻捷联惯导解算的初值，如此反复不断。惯性器件采样经过2.3小节的误差补偿后获得等效旋转矢量mΦ和比力速度增量sfm∆v，再经过以下三步骤便可实现捷联惯导更新，这里直接给出计算公式。（1）速度更新算法mnmnenmniemmsfmnbmmninmqvnmnmTT])2([)2/(1111111−−−−−−→−×+−+−+=vωωg∆vqωFvv(7)（2）位置更新算法MnmNmmmmRvTLL/1,1−−+=(8a)NnmEmmmmmRvLT/sec1,11−−−+=λλ(8b)nmUmmmmvThh1,1−−+=(8c)（3）姿态更新算法)(111mninmnbmmqvnbmnbmT−∗−→−−=ωqΦFqq(9)3.仿真与分析根据第2节的分析，将各函数和算法编制成Matlab的m文件，文件名及简要功能如表2所示，详细的源程序代码参见附录，文中还着重对圆锥误差补偿算法和捷联惯导更新算法进行了仿真。表2Matlab文件功能说明文件功能文件功能glvs.m全局变量a2quat.m由姿态向量求姿态四元数earth.m有关地球参数计算q2att.m由姿态四元数求姿态向量qconj.m四元数共轭cnscl.m圆锥误差和划船误差补偿qmul.m四元数相乘sins.m捷联惯导更新算法qmulv.m四元数乘向量test1.m圆锥误差补偿算法仿真rv2q.m等效旋转转换为四元数test2.m捷联惯导算法算法仿真q2rv.m四元数转换为等效旋转，动坐标系b系相对参考坐标系r系绕rrzo轴作圆锥运动，半锥角为α，锥运动频率为ω，即：b系与r系坐标原点重合，bbxo轴以rrxo轴为对称中心，在rrrzox平面内作幅值为α的振动；bbyo轴以rryo轴为中心，在rrrzoy平面内幅值为α的振动；bbzo轴在以rrzo轴为中心的圆锥面上运动。动坐标系b相对参考坐标系r的方位关系还可用姿态四元数描述为[]T0sin)2/sin(cos)2/sin()2/cos()(tttrbωαωαα=q(10)zrxryrαxbybzbor(ob)图1b系相对r系作圆锥运动两个相邻采样时刻之间的角增量输出为⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−++−=+)2/(sin)(2)]2/(cos[)2/sin(sin2])2/(sin[)2/sin(sin221αωωωαωωαhhthhthkkk∆θ(11)其中，kktth−=+1，1+k∆θ为从kt时刻到1+kt时刻的角增量，即1+kt时刻采样获得的角增量输出。理论上可以证明，圆锥误差补偿算法在一个补偿周期T内的算法漂移误差为122111)()12(2!++=+∏−×=nnknhknnωαε(12)图2圆锥误差补偿仿真当α=1º、ω=5Hz、h=10ms、n=3时，仿真一分钟的姿态误差如图2所示。其中蓝色曲线为圆锥误差补偿姿态与真实姿态之间的误差，可见在rrxo轴和rryo轴有微小振荡误差，而rrzo轴为圆锥漂移误差。最下面小图中红色曲线为利用公式（12）计算的理论圆锥漂移误差，它与蓝色曲线非常接近。此外，当ω=1Hz、h=10ms，而α和n取不同值时，进行了一系列的圆锥漂移误差仿真和理论漂移误差计算，结果如表3所示。从表中可以看出，只有当半锥角α很小时，高子样数的仿真误差和理论误差才比较接近，而当α较大时，高子样数的仿真误差和理论误差之间相差倍数很大，甚至在仿真误差中出现高子样补偿效果不如低子样的现象。产生这种现象的原因是，由于在圆锥漂移补偿算法推导过程中作出了半锥角α是小角度的假设，还须指出的是，在推导过程中还假设圆锥频率和采样周期乘积h⋅ω必须为小量。因此，在实际中必须结合应用环境选择合适的圆锥误差补偿算法，而并非子样数越高计算精度就越高，但是，如果考虑到实际陀螺的精度（如陀螺漂移1×10-4º/h），即使α为10º时，选用3子样的计算精度也足够了，或者提高采样频率有利于降低计算误差。表3一分钟圆锥漂移误差仿真和理论计算（单位：）α1子样2子样3子样4子样5子样仿真误差6.013e-74.745e-104.016e-133.522e-169.558e-191理论误差6.012e-74.747e-104.013e-133.523e-163.161e-19仿真误差2.164e-31.708e-61.444e-91.612e-121.623e-121′理论误差2.164e-31.709e-61.445e-91.268e-121.138e-15仿真误差7.7906.148e-34.596e-64.480e-62.103e-51º理论误差7.7926.152e-35.205e-64.566e-94.097e-12仿真误差771.2420.596-5.455e-34.416e-22.075e-210º理论误差779.2720.6155.205e-44.566e-74.097e-103.2捷联惯导算法仿真传统上用姿态矩阵表示的计算数学平台'nbC与真实数学平台nbC之间的关系为nbnbnnnbCφICCC)(''×−≈=(13)其中φ为平台误差角，它表示从'n系到n系的小转动角向量（可以看成是等效旋转矢量）。假设变换矩阵'nbC、'nnC、nbC分别与变换四元数'nbq、'nnq、nnq相对应，则有nbnnnbqqq''=，并且利用矩阵与四元数之间的关系可以证明)('φFq−=→qvnn成立，所以有nbqvnbqφFq)('−=→(14a))()(''∗→∗→−=⇔=−nbnbvqnbnbqvqqFφqqφF(14b)在仿真时，通过（14）式能够方便地进行平台误差角处理，与传统矩阵描述平台误差角相比，四元数描述方法的优点是不必再作正交化处理。对静态导航进行了仿真，初始平台误差角分别设置为0.5′、0.5′和3′，仿真一小时导航参数结果如图3所示，其中速度误差和位置误差小图中，红色曲线为高度通道的误差，它们是发散的，而水平通道（包括