捷联式惯性导航积分算法设计-上下完整篇

1、捷联式惯性导航积分算法设计上篇：姿态算法PaulG.SavageStrapdownAssociates,Inc.,MaplePlain,Minnesota55359摘要：本论文分上下两篇,用于给现代捷联惯导系统的主要软件算法设计提供一个严密的综合方法：将角速率积分成姿态角,将加速度变换或积分成速度以及将速度积分成位置.该算法是用两速修正法构成的,而两速修正法是具有一定创新程度的新奇算法,是为姿态修正而开发出来的,在姿态修正中,以中速运用精密解析方程去校正积分参数姿态、速度或位置,其输入是由在参数修正姿态锥化修正、速度摇橹修正以及高分辨率位置螺旋修正时间间隔内计算运动角速度和加速度的高速算法提供

2、的.该设计方法考虑了通过捷联系统惯性传感器对角速度或比力加速度所进行的测量以及用于姿态基准和矢量速度积分的导航系旋转问题.本论文上篇定义了捷联惯导积分函数的总体设计要求,并开发出了用于姿态修正算法的方向余弦法和四元数法；下篇着重讨论速度和位置积分算法的设计.尽管上下两篇讨论中常常涉及到根本的惯性导航概念,然而,本论文是为那些已对根底惯导概念很熟悉的实际工作者而写的.专门用语：A,A,4,A3=任意坐标系CAA2=将矢量从A2坐标系投影到A坐标系的方向余弦矩阵I=单位矩阵qA1=从A2坐标系投影到A坐标系的旋转矢量所构成的姿态变化四元数qA2*=qA1的共轲四元数,它的第1项与q2的首项相同,余

3、下的24项与qR的互为相反数q1=单位四元数,它的第1项为1,其余3项为0V=无具体坐标系定义的矢量VA=列向量,它的各项元素等于矢量V在坐标系A的各轴上的投影AAV=向量V的反对称或交叉积形式,代表如下矩阵：0VzaVyaVza0VxaVYAVXA0AAA其中：Vxa,Vya,Vza是V的分量,V与人系矢量的矩阵乘积等于V与该矢量的叉积V；A-0=与VA等量的四元数矢量,AVA=A2坐标系相对于A坐标系的角速率,当Ai为惯性系I系时,a2是由安装在A2坐标系上的角速率传感器所测到的角速率1 .概论惯性导航是通过对速度积分得到位置并对总加速度积分得到速度的过程.总加速度是指由重力加速度和被施加

4、的非重力产生的加速度亦即比力加速度之和.惯性导航系统INS包括：用于积分的导航计算机；用于给积分运算定时的精密时钟,测量比力加速度用的加速度计组台；用于作为所算位置的一个函数而进行的重力加速度计算而留于导航计算机中的重力模型软件,以及为了定义作为速度计算一局部的加速度计三元组的角度方向所用的姿态基准.在现代INS中,姿态基准是由驻留于INS计算机中的软件积分函数提供的,其输入来自一个有三轴的惯性角速度传感器.角速度传感器和加速度计三元组安装在一个公用的牢固构架上,该构架装在INS的底盘上,以保证每个惯性传感器之间的精确对准,这样的一种布置称之为捷联INS.由于惯性传感器牢固地固定在底盘内,所以

5、也就牢固地固定于安装INS的飞行器上.INS计算机中的根本函数有将角速率变换为姿态的积分函数称之为姿态积分.使用姿态数据将测得的加速度值转换到适当的导航坐标系中,再将它积分成矢量速度的函数称之为矢量速度积分,还有将导航系矢量速度积分成位置的函数称之为位置积分.这样就有了三个积分函数,姿态函数、矢量速度函数及位置函数,每个函数的精度要求很高,以保证函数误差极小,符台惯性传感器精度的要求.回眸历史,由于早在50年代,根底捷联惯导概念就开始形成,所以多年来捷联分析师将精力主要集中于姿态积分函数算法的设计上.而种种算法的设计方法总是受到当时的飞行计算机技术的能力和局限性的影响.50年代后期和60年代,

6、各种研究机构的捷联工作者们采取两种用于姿态积分函数运算的方法,即用一阶数字算法进行高速姿态修正运算,如1020kHz和用高阶算法进行的低速姿态修正运算,如50100HZ.如果要精确考虑高频角速率分量时,就得考虑用高速算法,这样可以调整为系统的三维姿态变化.然而,那个时期的计算机工艺技术只能对姿态修正算法进行一些简化的一阶方程运算,精度有限.相反,高阶算法提倡者竭力吹捧较一阶算法提升了解析精度的高阶算法；但是,由于每个姿态修正循环可执行运算次数的连带增加导致必须使姿态修正速率减缓以满足当时的计算机吞吐量的限制,从而使提升了的精度被降低.由于姿态四元数可以当作对所算姿态参数的解析形,这一优点使之成

7、为一种更可取的算法与传统方向余弦矩阵姿态表示法相比所以它的出现使上述两种算法的优点黯然失色.就那一时期研究过的算法而言,四元数法在高频角速率环境下表现出的运算精度最优.1966年,作者提出一个新的两速姿态积分函数运算方法,在该方法中,姿态修正运算被分成两部分,即把简单高速一阶算法局部与更复杂的中速高阶算法局部结合起来使用,后一局部的输入由高速算法提供.简化了的高速局部用于考虑姿态修正循环内的高频角振荡,这能调整系统的姿态建立传统上称之为锥化.合在一起,两速方法的组合精度等于以高速速率进行的高阶运算为了提升精度；然而由于高速算法简化之故,组合后对计算机的吞吐量的要求丝毫也不比原来的高速一阶姿态修

8、正算法的吞吐量大.参考文献【6】采用的两速算法设计受到其根本解析公式的限制,该公式是一个皮卡德类型Picard-type的连续型的姿态速率微分方程的递归积分,在该方程中,同时产生中速和高速算法.解析递归积分设计过程的复杂性限制了高阶中速算法的扩展在参考文献【6】中仅扩展到2阶,这在当时被认为是可以接受的o196孙,焦尔敦Jordan在一个不相关的设计活动中提出一个用于捷联姿态修正函数的两速法,在此方法中,解析公式的开头建立在两个单独定义的计算上：一个建立在输入姿态变化根底上的中速、经典封闭型、准确高阶姿态修正算法和一个测量中速算法姿态变化输入的简化高速二阶积分算法.1971年,鲍尔兹Bortz

9、将焦尔敦的概念扩展到基于微分方程的高速计算,积分时将测量的准确姿态变化输入给准确的姿态修正算法.准确的中速姿态算法可以通过两个三角系数舍位构建成任何精度要求的特定阶次.实际上,鲍尔兹的姿态变化微分力程的简化型式现被用于高速函数计算.因此,参考文献【7】和【8】提供了一个更一般的两速姿态修正算法,在此算法中,中速高阶算法和高速简化算法可以独立地进行改编以满足特定的应用要求.让人感兴趣的是,参考文献【8】提出了一个简化型高速算法的模拟执行程序.参考文献【7】和【8】提出的两速法主张在准确中速姿态修正运算中采用方向余弦法的第二个好处是中速局部也可以用一个解析的精确的封闭形式的四元数修正算法来作为公式

10、,该算法采用同用于方向余弦修正算法一样的高速输入.这样一来,新的两速法无论对于方向余弦法还是对四元数修正法都有同样的精度,而这两种方法都是由解析的精确封闭式方程推导出来的这里假定对泰勒级数的三角系数的展开进行到比拟准确的量级.大多数飞机使用的现代捷联INS采用以两速法为根底的姿态修正算法.中速算法局部的重复速率一般是基于最大角速率来设计如50200Hz,以尽量减少中、高速算法中募级数舍位误差的影响.高速算法的重复速率是在预计的捷联惯性传感器组合振动环境的根底上精确考虑了引起振荡的锥化效应而进行设计的.如用于1nmile/h有50%的径向位置误差速率的飞机的INS的重复速率为14kHz.连续进行

11、的两速姿态算法的开发工作一直集中于高速积分函数的变化上.原来设想的简单一阶的种种现有高速姿态算法利用现代计算机日益增加的吞吐量,演变为各种高阶算法,而且经度得到提升,参见参考文献【9】【11】和【12】的第7-1节.对于姿态修正函数的演化,截止目前,有关用于加速度变换或矢量速度积分和位置积分的捷联INS姊妹算法的开发方面的其他并驾齐驱式著述很少见到发表.本论文上篇定义了捷联惯导积分函数的总体设计要求并描述了开发基于两速法的姿态积分算法的综合设计过程.文章中所提供的材料是参考文献【12】第7-1节它是参考文献【9】中材料的扩展的浓缩型.这些材料着重于提供更严密的解析公式,同时,在可能的情况下,考

12、虑到便于生成计算机软件的文件及有效性这与现代飞行计算机技术也是一致的,这些材料使用准确的封闭式方程.在姿态算法没计过程中包括了考虑到姿态修正时间周期内导航坐标系旋转的种种严密处理方法.本论文的第二节提供了有关坐标系和所采用参数的背景材料.第三节以连续微分方程形式提供了一整套典型的捷联惯导姿态、矢量速度和位置方程,这构成了等量算法设计过程的框架.第四节探讨以高速局部的普通形式进行两速姿态积分运算的算法适用于在导航系旋转影响下的方向余弦法和四元数法的公式,并描述了一个特殊的形式,说明在古典高速二阶锥化计算算法中一种算法的设计,第五节给出有关姿态积分算法的表格参考数据,第六节就选择特殊应用算法和建立

13、运算运行速率之后的过程进行了一般化讨论,第七节给出结论性评述.最后,熟悉到下述观点很重要：尽管两速法的初衷是克服早期l9651975年计算机技术的吞吐量的局限性,然而随着现代高速计算机的快速开展进步,这一局限性正在变得无足轻重.这就促使工程技术人员想回到一个更简单的单速算法结构上来,采用这种结构时,所有计算以一个重复速率进行,速率很高,足以准确地将多轴高频角速率和加速度调整影响考虑进去.本论文上,下两篇提供的两速结构是和压缩成在对算法进行公式化的专门章节里解释的那种单速型是相兼容的.2 .坐标系和姿态方向的关系本节定义了本篇论文中使用的坐标系,并一般性地说明方向余弦矩阵、姿态四元数和用于代表两

14、坐标系之间的角度关系的旋转矢量姿态参数等等的性质.坐标系是根据右手法那么对彼此互相垂直的三个连续编了号或标了字符的单位矢量加以定义的解析抽象.它可以想像为三个垂直线轴穿过一个共同的点原点,各单位矢量从原点沿三条垂直线向外发散.在本论文中,坐标系原点的实际位置是任意的.在特定坐标系中,一个矢量的分量或投影等于矢量与该坐标系各个单位矢量的点乘.本论文中采用的矢量为自由矢量,因此,在对它们进行解析描述的坐标系中,其位置不存在优劣性.坐标系定义如下：1 .E系是用于位置定位因而定义的地球固联坐标系.它的典型定义是它有一个轴与地球极轴平行,其他两轴固连于地球并与赤道平面平行；2 .N系是一个导航坐标系,

15、Z轴与当地地球外表参考位置白垂直向上方向平行,N系用于将加速度积分为速度并定义E系内当地垂直角方向；3 .L系是当地水平坐标系,与N1平行,但Z轴垂直向下,而期口Y轴沿N系的Y和期由.L系用于描述捷联传感器坐标系方向的基准；4 .B系是捷联惯性传感器坐标系机体系,其轴与标准右手正交传感器输入轴相平行；5.I系是一个非旋转惯性坐标系,用作角旋转测量基准.为I系选择的特殊方向在其方向与解2.2姿态参数定义方向余弦矩阵定义为一个方阵,其各列是单位矢量的正交组,每列等于沿A2系的一个轴方向上的单位矢量在A系各轴上的投影CA2uiA2u2a2u3A2式中uA；是沿A2系轴i方向上的单位矢量在坐标系A1各

16、轴上的投影.从这一根本定义出发,可以验证出：C：的i行、j列的元素等于A系轴i和A2系轴j之间的夹角的余弦值,CA2的转置矩阵等于其逆矩阵,CA2转置矩阵的各列等于系A轴中单位矢量在A系各轴上的投影,且CA2与一个投影于A2系各轴上的向量的积等于此向量在Ai系各轴上的投影分量而对CA1的转置矩阵那么相反.va2(cA2)tvAi(2)2式可以用于导出方向余弦矩阵链式规律:cA3A2A3A2CA2cA旋转矢量定义旋转轴和绕轴的旋转幅度.可以想像A系通过绕此旋转矢量旋转而从它的起始姿态转到一个新的姿态,绕旋转矢量旋转的角度等于旋转矢量的大小.现在,称a2系为A系的新姿态,通过给4系的这一定义,一个

17、任意定义的旋转矢量就可唯一地确定A2系姿态与原始Ai系姿态二者的关系.反之,如果给定Ai系和A2系的姿态关系,就可以定义一个与此姿态一致的旋转矢量.这样来,旋转矢量可以用来定义A2系与A系相关的姿态.从解析式上看见参考文献【4】,【9】和【12】的第3.2.2.1节旋转矢量和方向余弦矩阵之间的关系可以由下式给出:CfA2sin(1cos)2()sin-2一-()(4)式中是旋转矢量及其幅度.旋转矢量的一个独特特性是在A系和A2系中有相同的分量见参考文献:【12】的第3.2.2.1节；因此4式中的代表A1或A2姿态四元数是一个四矢量,即有四个分量,定义为旋转矢量的一个函数和【12】的第3.2.4

18、节和【14】的第7376页见参考文献【4】,【9】cos0.5JqAsin0.50.50.5(5)由5式可以看出,qA1各元素的平方和是单位1.坐标变换方程可以用四元数代数方程表示见参考文献【4】、【9】和【12】的第3.2.4.1节.VqAqA：VqA2qA：*,VqA2qA；VqAq2qA2VqAqA2*(6)6式可以用来推导出姿态四元数的链式规律:AA3A2qAqjq%2.3姿态参数速率方程第2.2节姿态参数的变化速率见参考文献【4】、【8】、【9】和【12】的第3.3节由下式给出:CA1A2cA1(A2、A21A2)(I)CA1(8)qA1A21A2q21qA1(9)AA1A2sin2

19、(1cos)(10)3连续型捷联惯导方程定义捷联惯导系统中进行的典型根本运算的微分方程的第77103页以及156177页给出如下：姿态速率：见参考文献【9】、H2的第4章和【15】CBCb(BIBLIL(11)或代之以:LILqLqB(12)当地水平系旋转速率:加速度变换:LasF或代之以:NasFLILNIENENEN)(13)(Cn)TNFc(UZNLBCbHsfLBL*qBas后qBcLNLasFEIEN、N)ZNUZN(14)(15)(16)(17)(18)矢量速度速率Ngp(iE)(NIE)RN(19)NasFNNgp(encN、N2ie)v(20)位置速率:CncE(NN)(21)

20、NNUznv(22)式中：R=从地球中央到ins的位置矢量；V=相对于地球的速度矢量位置变化速率,解析式中定义为E系中R的时间导数；h=地面高度,定义为从INs到地球外表的距离,沿从INs到地球参考重力平面上的切线平面的垂线方向测量；Fc=曲率矩阵3X3是位置CE,h的一个函数,其元素3、i和i、3等于零,而其余元素绕对角线成对称形.对于球形地球模型而言,除对角线上外其余元素是零,且对角线上数值是从地球中央到INS的径向距离的倒数.对于扁地球模型而言,其余的项代表投影到线上的地球外表上的本地曲率见参考文献【12】第5,3节有关封闭型的表述局部ins高度);UZN=沿重力平面垂直方向N系的Z轴上

21、的单位矢量;ZN=EN的垂直分量；ZN选取的值取决于所采用的N系的类型.如：游动方位系统或自由方位系统的设计保证NN对所有地球定位而言是非奇异的见参考文献【12】的第4,6节和【15】的88-99页;asF=比力加速度,定义为由施加的非重力力产生的相对于非旋转惯性空间的加速度,它由加速度计测得;g=质量吸引重力加速度或重力R的一个函数；gp=铅垂重力或重力,对于稳定的ins而言,它沿铅垂线方向；g的解析模型可以在参考文献【16】；【17的第4.4节和【18】的6.3节找到.关于g在NK的分量,请参见参考文献【12】第5.4.1节.在运行捷联惯导函数过程中,捷联INS计算机用适当的积分算法对后面

22、的姿态速率、矢量速度速率以及位置速率方程进行积分运算.关于后面的导航方程形式,下面几点值得注意.方向余弦和四元数姿态都是以机体姿态速率或加速度变换运算的形式表示.无论哪一个部可以用于实践,而其结果实质上是一致的.矢量速度是相对于地球E系定义的,矢量速度速率方程是以当地水平定义的N系写出来的用于将它积分成矢量速度.对于像飞机的INS之类的许多地形导航用途而言,这很典型.也可以选择其它坐标系用于矢量速度定义和矢量速度速率方程,如用于战术和战略导弹制导.位置速率方程将位置定义为高度和NK相对于E系的角度方向.从中可以提取出经度和纬度,并可以计算出R直见参考文献【12】第4.5.1和4.5.3以及【1

23、5】的88,89页.对于位置速率方程而言,位置也可以定义为简单的R从中可以计算出CNE和h,见参考文献【12】的第4.5.4节.高度速率方程22显得价值不大,但当你考虑旋转扁地球模型、地球上的一个旋转NK和固定的高度定义时,这就不一定了.参考文献【12】的4.4节和5.5节说明对于旋转的扁地球模型而言,方程22是准确的.如果断定引入垂直通道重力或发散限制来防止不稳定垂直通道误差呈指数增加,那么方程20和22务必包括一个额外增加的垂直限制项参考文献【12】第4.4.1节,【15】第102-103页以及【18】的10.3节.4姿态即时修正算法本节探讨适合用数字计算机进行积分的方向余弦矩阵速率方程1

24、1和姿态四元数速率方程12的算法形式,这些方程将用现在的传统两速法来建立,传统两速法中的解析的精确的封闭形式的方程可以用于根本姿态修正函数的运算.该函数的输入是由一个为了在根本姿态修正周期中测出姿态变化而设计的高速算法所提供的.4.1姿态方向余弦矩阵CL方向余弦矩阵的修正算法是为实现下述目的而设计的：在姿态即时修正时间内所得的数字结果跟在相同时间间隔对方程11中CL表达式的正规连续积分得出的结果相同.该算法的思路是以理想的机体B系和当地水平L系在数字即时修正区间内确定的变化方程11中由?和；引的,作为每一即时修正时刻相对非旋转惯性空间I系的连续离散点上的当前构造值.考虑到完全一般化的情形,我们

25、还允许由于L系角运动的CL修正运算不一定是在当B系运动而对CL修正的同一时刻瞬时运行,例如在一个多速率数字计算回路结构中,CL以高速率进行修正是由于B系旋转而不是由于L系旋转.如果对计算机吞吐量的最小化感兴趣的话,从软件结构上说,可以让L系修正比B系修正慢510倍.我们用来描述坐标系方向随时间变化的专门术语如下：B|m=当计算机修正时间为tm时,机体B系在非旋转惯性空间I中的离散方向；m=因8系角运动对CB进行即时修正的计算机循环拍次；L(当地水平L系在计算机修正时间tn在非旋转惯性空间1中的离散方向;n=因1系角运动对CtL进行即时修正的计算机循环拍次;有了这些定义,采用(3)式方向余弦矩阵

26、递推计算规律便能建立起对cB即时修正算法如下n1)CL(n1)C(m1)(m)BI(m1)BI(m)CL(n)(n1)(23)(24)其中：CL(n1)=tm1时刻B系与tn1时刻L系的转换关系矩阵Cb；(m1)C：(n)=tm时刻B系与tn时刻L系的转换关系矩阵Cb；I(m)BCB1(m1)=考虑到B系相对惯T空间从tm1时刻点到tm时刻点的旋转的方向余弦矩阵；1(m)Li,.、CL1(n)=考虑到L系相对惯性空间从tn1时刻点到tn时刻点的旋转的方向余弦矩阵；I(n1)在(23)和(24)式描述的算法把各个时间的机体晾和当地水平L系的方向联系起来,并为B系与L系提供了以不同修正速率对CL进

27、行的惯性角运动修正.与B系(它能以200-300度/秒的速度进行动态旋转、相比,当地水平L系的惯性角速率一般来说较小,它等于地球旋转速率加上L系相对地球的角速率(传递速率,它一般不会大过地球速率的几倍).因此,L系即时修正一般可以以低于晾修正的速率完成,而且有相当的精度.应当注意,对晾和L系运动修正速率的要求,在某种程度上,是通过和测量姿态变化的近似高速算法作比拟,将误差降至最小的根底上确立的(见本文4.1.1和4.1.2节).对CL的B系和L系的运动修正是由(23)和BL,(24)式的CBI(m1)项和C?n)项完成的,其算法分别推导如下:(m)1(n1)4.1.1机体系旋转(23)式用CB

28、(m1)来考虑捷联传感器(机体汨系相对于非旋转空间的角速率B来修正姿态1(m)、.、.一IB|.、方向余弦矩阵CB,Cb(、的正式定义为:(m)BI(m1)BI(m)mCB(m1)tm1cBdt(25)式中B(t)是tm1到tm时间间隔内任意时间的机体B系姿态BiCb矩阵还可以用定义的BI系姿态相对于B1系的旋转矢量来表示.对(4)式进行泰勒级(m)I(m)I(m1)数展开,可得系数项：CB(m1)(m)sn(m)sinm1m2m4m3!5!1cosm12m4m2m2!4!6!(26)式中,m定义为在tm时刻B1系姿态相对B1系的旋转矢量.m旋转矢量可以通过把当作定义为大于tm1时刻的普通的B

29、系姿态相对于R系普通旋转矢量来对待的办I(m1)法计算出来.然后,是通过对普通的方程从tm1时刻进行积分得来,而(26)式中用到的就是在tm时刻的积分结果.把用于定义的BI系当作非旋转惯性基准系(m1)I来处理,般可以得到以下表达式,其方法是将(10)式中的坐标系庆2由机体系B取代,而坐标系A由惯性系I取代,得到用角速率描述的下式:B1IB2B2?)(iB)(27)式中定义为在大于tm1时刻的B系姿态相对于BI系的旋转矢量.通常称之为鲍尔1(m1)(m)(m1)B兹万程的(27)式使机体系B的姿态的变化与B系相对于惯性系I的角速度旧产生联系,而BIB可以通过捷联角速率传感器测得.于是(26)式

30、的姿态旋转矢量m便可以通过从tm1时刻到tm时刻对(27)式进行积分求得：t-(t)()dt,(m)(tm)(28)tm1式中是运行中的积分时间变量.要减少(27)式中求算值的计算次数,应参加简化设计.B、例如,通过帚级数扩展,可以将(27)式中的(ib)项的放大系数近似为：1Msin1T(1T7；)二(12(1cos)1260112因此,(27)式的可以由下式给出：B1B工(B)IB2IB12(IB)通过仿真和分析(在假定的解析定义角运动条件下进行解析扩展阶精度的表达式：(29)(30),可以得到保证值的二1 B1,B、1B2 IB12IB2IB式中：tBttiBdtm131式很重要,由于它

31、能使27式简化到二阶精度,即(31)(32)的误差为三阶,只保持-阶项.这样一来,27式就变成二阶精度:(33)B1BIB2IB代入方程33,28式那么变为下式:tm1tmBIB(t)b)dt(34)(35)(36)最后,用32式可得:mmm其中：,、t(t)tIBd,m(tm)tm11tmBm1t(IB)dtBIB2tm1式中m是从31到tm的圆锥姿态运动.m项被锥化成锥化工程,由于它受到测量B的圆锥运动分量的影响.圆锥运动定义为角速度矢量本身在旋转的状态.由于旧表现为纯B圆锥运动旧的幅度虽然恒定不变,但其矢量在旋转,那么B系的一个固定轴近似垂直于因此用术语圆锥运动B.IB矢量的旋转平面,该

32、固定轴将随着角速度的运动而形成一个锥面来描述这个运动.在圆锥角运动条件下,与iB垂直的B系轴表现为振荡这与非圆锥运动或自转角运动相反,在后一种情况下,于垂直的轴绕上旋转.,BB对旧不发生旋转的情况而言,从36式可以容易看出,t将与旧平行,因此m中B的积分函数的叉乘将是零,m也将是零.这种情况下,即当旧不发生旋转时,34式将变成如下简化形式：tmBmtIBdt37tm1B注意：37式也适用于当旧不旋转时,27和28式中的m精确求解,即不存在近似问题.这点可以通过观察27式很容易地得到验证,由于在27式中,t在积分开始B.BB时就与旧对齐,并将与旧保持平行,这是由于在(t)表达式中,(t)与旧叉乘

33、将保持为零.在这种条件下,(27)式和(28)式也将化简为(37)式.下面讨论积分角速率和圆锥增量算法.关于(36)式中m的积分速率和m的锥化表述的离散化数字算法可以通过这样的方法开发出来,即考虑m是一般函数(t)在t=tm时刻的值,由(36)式得：1 tB(t)1t()iB)d(38)2 tm1现在考虑将(38)式的积分分成在tm1到口时间间隔内到达t时刻的时间和11时刻之后的时间两局部,这样一来,(38)式等于：(t)|1(t),m(tm)1tlBi1t(t)iB)dt(39)2t1式中：11是ttl1时(t)值,而l是ttl时的计算机的循环拍次.需注意,根据上述定义,l循环拍次快于m循环

34、拍次.现在,我们再定义一个在tm1到tm的时间间隔内l循环时间点tl,于是在tl时间点,包扩初始条件在内的(39)式就变为:ml(tltm)当ttm1,l0(40)1 tlbi1t(t)iB)dt2 ll1通过类似过程,(40)式的(t)表达式可以通过(36)式(t)的计算求得：八、tB.(t)i1(t),(t)t1tBd(41)(42)tl1l(tl),ll1l当ttm1,l0根据(41)式和(42)式得：iB)dt11tli2(i1J2tn(t)ml(tltm)(41)和(42)式构成数字递归算法,该算法以l作为计算机循环速率计算m和锥化项m,作为从ti1到ti的整个时间间隔内和的变化之和

35、,(42)式中1积分项的数字当量仍尚待确定.姿态算法开发方面的后续工作集中在估算锥化方程(42)中积分项的数字算法设计上,一一B般而百,所米用的万法都是对从tl1到tl时间间隔内的角速率模型旧假定一个一般的解析式,如一个在时间上截尾的一般多项式.于是(42)式的积分项就可以以解析的形式作为一般速率模型系数的一个函数,如多项式系数来表示.最后,角速率模型系数是通过匹配连续积分计算出的角速率增量的测量值来计算出来的.就随后的举例而言,角速率模型被近似为一个常数加一个实时线性累积值,其常数和梯度是由现有的和以前的l值计算出来的.此算法的一个更为复杂的形式可能是在假设的角速率模型中包含有一个时变平方项

36、,用现有的、过去的以及过去之过去的l值判定系数.近来在这一领域进行的工作是通过利用tl1到ti时间间隔内角速率传感器的测量值来计算角速率模型系数(参见文献【19】提出的一个用于强化单速算法的技术扩展),从而将第三个循环速率加到整个姿态修正过程的结构中：姿态(C(L)修正和锥化(i)修正(正如至此所作的讨论)以及锥化修正的传感器采样.后一种技术在改良时采用一个一般的角速率模型,该角速率模型是依据其对(42)式中积分项的影响直接定义为ti1到ti时间间隔内连续积分角速率增量传感器采样值之间的加权叉乘之和(类似于文献【19】介绍的tm1到口时间间隔内的方法).在后一种情况下的加权系数于是得到R优化并

37、可以获取纯锥化环境下的最正确平均性能,亦即旧在幅度上为常数,但在旋转.之后提到的种种设计方法中的每一种都是以根据假定的角速率模型形状使用曲面拟合技术为基础的.每一种的最终算法在不是为某个速率环境而设计时和存在有角速率传感器量化噪声的情况下会表现不同.更为可取的算法的选择,务必包括仿真分析以确认运行速率环境下和传感器噪声特性状态下是否可以接受.现通过算法举倒对这一局部进行总结.这儿举出的是(42)式积分项算法的例子,它的机B理是通过对塞级数进行截尾展开,使机体速率旧项被近似到一阶：Bib(tt1),和二吊数(43)参考文献9-【11】和【12】的第7.1.1.1.1说明对于时间段ti2到L的(4

38、3)式运动有:titi1B1(iB)dt而(i1i)(44)将(44)式代入(42)式,可得：1z1、i(i1i1)i26(45)式从类别上可归为求m的二阶算法,这是由于它在l方程中包含有现在的和过去的.一.一1,l循环乘积.从导出(44)式的分析可以看出,l中的l和l1乘积项即一项为哪一项由tl6到tl2时间间隔内线性斜坡角速率的近似化得来的.如果角速率近似化成时间的二次曲线变化函数,那么,势必导致包含l,l1和l2的乘积的三阶算法,如果角速率被近似成.1t到tl整个时间间隔内的一个常数,那么,(45)式中l的一项势必导致求m的一阶算法.6最后,如果角速率正在慢慢变化,那么,我们可以将m近似

39、为等于零.换言之(或更准确地说),我们可以让l循环速率等于m循环速率.这使m等于用(45)式在时刻计算出来的注意根据(41)式初始状态定义l1为零,后一种算法在参考文献【4】得到了改良.须注意.要设定l和m速率相等,还可以通过增加m速率使之与l速率相匹配的方法实现.结果得到一个单一的高速高阶算法,该算法附带有一个比两速算法更为简单的软件结构,但是,需要更大的吞吐量,现代计算机速度的不断提升使这一算法在未来成为更为可取的算法.(35)式m和m的全部数字算法是由(41)、(42)和(45)式合并后得出的结果所决定的:tldtl1(46)ml(tl露),当ttm1时l01)(47)l(tltm),当

40、ttm1时l0式中：d微积分角速率增量,即：捷联角速率传感器给出的脉冲输出的解析表达式,即iBdt=由角速率传感器给出的积分角速率输出增量之和.4.1.2当地水平系的旋转(24)式采用C：I(n)式来修正姿态方向余弦矩阵C：,以考虑本地水平坐标系L系相对于Li,、,cLi(n)的正式i(n1)L|(n1)非旋转空间的角速率11cLi的推导与第4.1.1节C：I(m1)的推导非常类似.1(n1)km)定义是：c：i(n)Itncjdt|(n1)tn1i(n1)式中,L(t)是tn1到tn时间间隔内任意时刻的L系姿态.CLI(n)矩阵也可以用定义的L,系的姿态相对于L系的旋转矢量来表示.对(4)式

41、(n1)I(n)(n1)进行泰勒级数展开可得系数项：CL(n)(n1)sinn(1cosn)(n)(n)sinn2n3!4n5!(49)(1cosn)2n12!2n4!6!注意：在(49)式中,式中n是定义tn时刻L1系的姿态相对于tn1时刻L1系的旋转矢量.(n)1(n1)空二(n)项的符号与(26)式C：；1)表达式的同类项的符号相反,是负的.这是由于nL,、BL,、Bi,八“与Cb(或(4)式C：)的指向相反,t的矢量变换是从L、到L,而CbI(n1)I(m)1I(n1)(n1)'(n)I(m),一一、一一一、-.一,Ll、,.一一.,BI的矢量变换是从BI到BI.这样,(49)

42、式中的CL形式便是(26)式中CB表达式的I(m)I(m1)I(n1)(m)转置.由于tn1到tn的修正周期相对较短,n在数值上将会很小.又由于；小且在整个tn1到tn的典型修正周期内变化缓慢(由于该时间周期内的矢量速度和位置变化小),所以L系速率矢量；可以近似为非旋转.结果可以像旋转矢量方程(1O)的简化形式一样计算(49),这里的叉乘项被忽略不计：tnLntILdt(50)tn1顺便指出,鉴于n是个小量,如已讨论过的那样,用于CL的(49)式亦可简化.例如,nI(n1)可从(49)式导出一个二阶形式：Jig1CliI(n)1(n)(n)I(n1)2L,对于现行的现代计算机技术而言,采用解算

43、Cl的(49)式简化形式如(51)式的计算(n1)机在内存或吞吐量方面的优点与增加软件有效性或文件复杂性以及精确度上损失相比拟,是微缺乏道的.精确度损失在导航过程中一般来说是很小的；然而,在初始对准运算过程中(惯性导航启动之前)这种精度损失不可无视,初始对准运算时C：矩阵常被用于对CeL进行倾(n1)斜修正(参考文献【12】第6,1.2节和第15节120121页).对Cb的初始倾斜对准修正量可能会很大,如0.1-1度,如果对49式采用过于简化的形式,在初始对准过程中CL就可能产生不希望的误差.初始对准运算的闭环伺服运动最终会修正由CEL产生的结果性姿态误差；然而,它可能会将冗余正交误差留在C.

44、行和列里面.其结果一定是需要包括一个正交或归一化修正算法见第4.1.3节作为C：修正过程的外层循环.50式n积分的离散数字算法可以通过下述方法建立起来,即第一步先将13式和15式合并,求出被积函数；,然后进行如下近似计算：(52)LLNNNN、ILCn耳1/27*1/2mFCn1/2(UzNV)一一1一式中,下标n一是tn1到tn的中途的值.将52式代入5O式,然后可得:2CLNTnCN|En1/2'nZNn1/2UZNTnFCn1/2(UZNRm)(53)用14式估算ie且(54)NtmN,RmtVdttm1式中,是计算机循环修正周期tn31,而j是在tn1到tn的计算机修正n循环内

45、m循环的循环次数.153式下标为n3的项都是位置的函数,它见下篇参考文献【13】在以n循环速率进行的姿态修正之后得到更新.因此,要计算52式中的这些项,必须在以前计算的参数的根底上,采用一个近似外推公式.例如,采用最后两次的计算值的线性外推公式,将变为：(55)1n1/2n12n1n2在下篇参考文献【13中,我们发现速度vN更新紧跟着位置更新.因此,现在的和过去的m循环的数值可用于估算54式中RN的积分.对54式进行梯形积分可得:(56)式中Tm是计算机m循环的更新周期tmt下篇参考文献【13】还开发了一种具有高分辩水平的RN算法,用于精密位置修正,它考虑了从m1到m循环修正时间间隔内的动态角

46、速率和加速度变化.4.1.3归一化和正交化根据第2.2节的根本定义C：的列(和行)代表正交单位矢量,因此,它们在模值上等于1(归一化条件)且彼此相互正交(正交化条件).C：更新算法中,除了已描述过的根本CL修正算法外,也常常包括一个归一化和正交化算法以保证C：的各行和各列正交.导致C：正交误差的因素包括CfL正交初始化误差、软件编程误差、由于计算机字长小于用于CfL算法更新周期所希望的全部位数而引起的舍入误差、以及(26)和(49)式泰勒级数展开中所选项数太少而引起的舍位误差.重要的一点是要注意到(参考文献【12】第3.4.1节)：归一化和正交化误差只是由于(23)、(24)、(26)及(49

47、)式的软件运行过程中出现的误差所引起的,而不是由于供这些方程的算法误差或是由惯性传感器输入误差所引起.Cb积分算法软件的总体设计或校正过程必须保证：在预想的导航时间周期内和角速率环境条件下,编程无误差并且舍入误差和舍位误差在可以接受的范围内.这是一个用现代计算机技术或软件开发技术可轻易实现的目标.不过,将C.正交化或归一化修正算法包括进去是许多捷联惯导软件包一直采用的传统作法,目的在于提升精度,并放宽不许运算中出现任何正交误差这一严格要求.用于归一化正交的算法是以下面这一特性为根底的；即方向余弦矩阵的转置矩阵等于其逆矩阵(见2.2节)；因此,CB与其转置矩阵之乘积为单位矩阵.这一状态的改变,可

48、用来测量正交误差,而该误差在迭代运算中可被限制算法修正(参考文献【9】,【12】第7.1.1.3节,【15】216-218页)4.2姿态四元数设计qB姿态四元数的修正算法的目的是为了使在各姿态修正时刻得到的数字结果跟(12)LL式qB表达式的正规连续积分在同一时刻瞬间得到的数字结果相同.qB姿态四兀数修正算法式(57)(58)是严格根据本论文第4.1节用CB修正算法推导的同样过程开发出来的.因此,采用姿态四元数链式规律可以写出下式：LL,B,1(n1)1(n1)I(m1)qBgqB'm)L|(n)L|(n)L|(n1)QB.1"1)靠15)式中：q：(n1)=tm1时刻的B系

49、到tn1时刻的L系的四元数qB；(m1)qB?*=tm时刻的B系到tn时刻的L系的四元数qB;I(m)qB；/)=考虑了B系相对惯性空间旋转的从幻1时刻的方向到tm时刻的方向的姿态四元数；qL?)=考虑了L系相对惯性空间旋转的从tn1时刻的方向到tn时刻的方向的姿态四元I(n1)qB的修正是由(57)式和(58)式中q：I(m1)和q：I(n)完成的,两个算法分别推导如下:I(m)I(n1)Bi,八(57)式用qB；的旋转角速率4.2.1机体系旋转修正姿态四元数q：以考虑捷联传感器(机体)B系相对于非旋转空间Bl“、qBI(m1)的正式定义为:I(m)BI(m1)qBI(m)tBtm"

50、;m1)J/dtLm1(59)式中B(t)是加1到幻时间间隔中任意时刻B系的姿态.B一qB：；姿态四元数也可以用一个旋转矢量的万式来表达.该旋转矢量定义B,、系与(m)BII(m1)系之间的关系.对(5)式进行泰勒级数展开可得如下各系数项:cos0.5mBI(m1)朝0.50.5msin0.50.5m1.5m)23!(0.5m)45!(60)cos0.512(0.5m)2!4(0.5m)4!(60)式用姿态四元数修正旋转矢量m与在4.1.1节用方向余弦矩阵CB修正m是的,而且是用由(35)、(41)和(42)或(35)、(46)和(47)式提供的相同算法计算出来的.4.2.2当地水平坐标系(L

51、系)旋转(58)式修正姿态四元数qB的方法是利用qL.考虑当地水平坐标系L系相对非旋转空I(n1)间的角速率LL与qLI(n)二者的关系来实现的.qLI(n)的正式定义是:1I(n1)1I(n1)(61)L(n)tnL(t)qL(n1)q1tn1qL(n1)dt(61)式中的L(t)代表tni到tn时间间隔内任一时间的L系姿态.姿态四元数qLI(n)也可以用旋转矢量的方式来表达,该旋转矢量定义为L系的姿态相I(n1)In)对于LI系的关系.对(5)式进行泰勒级数展开可得如下积分项:I(n1)cos0.5nqLI(n)(n1)sin0-5n0.50.5n一一,一一、2,一一、4sin0.5n1(

52、0.5n)(0.5n)0.5n3!5!(62)(0.5n)2(0.5n)4COS0.5n12!4!.L,式中n的负号考虑了qL的反相指向,它描述了(n1)Li系姿态相对Li系的关系,这与旋(n1)(n),转矢量n的指向不同,描述了Li.系姿态相对L|(m)系的关系.(62)式n旋转矢量与用于方向余弦矩阵C：修正的n是一致的,且是用第4.1.2节中描述的且由(53)、(55)和(56)式提供的相同的方法计算出来的.在精度上可与(51)式方向余弦修正相媲美的(62)式的近似形式可以很轻易地通过代入和舍位求得：(n1)210.5(0.5n)20.5n(63)第4.1.2节中关于使用简化的(51)式方

53、向余弦当地水平系修正算法的合理性的意见也适用于(63)式姿态四元数修正算法,但不太适用于完整的(62)式那种形式.4.2.3归一化为了保持第2.2节中所讨论过的根本姿态四元数归一化的特性,在姿态四元数q：修正过程中往往包含一个归一化算法,作为该修正过程中的一个外环函数.在第4.1.3节中有关方向余弦矩阵需要归一化或正交化的讨论同样适用于姿态四元数,唯一的例外是正交化在定义四元数上毫无意义(如同定义姿态方向余弦矩阵那样)；因此.在第4.1.3节中的正交化讨论在此不适用.如果断定要采用四元数正交化算法,那么该算法是基于使用一个限制算法将qB的模值与1相比拟,而且该模值与1的差值可利用迭代的限制算法进行修正(参考文献9,【12】第7.1.1.3节,【15】216-218页).5姿态积分算法总结表I汇总了介绍过的捷联惯导姿态积分函数的种种算法,它是根据导航计算机中运算执行次序列出的.表1列出算法函数、输入参数、输出参数及方程序号数.6算法及运行速率选择面对可供选择的大量捷