捷联式惯性导航积分算法设计-速度位置计算.doc

捷联惯导积分算法设计下篇：速度和位置算法Paul G. Savage Strapdown Associates, Inc., Maple Plain, Minnesota 55359摘要：本论文分上下两篇，用于给现代捷联惯导系统的主要软件算法设计提供一个严密的综合方法：将角速率积分成姿态角，将加速度变换或积分成速度以及将速度积分成位置。该算法是用两速修正法构成的，而两速修正法是具有一定创新程度的新颖算法，是为姿态修正而开发出来的，在姿态修正中，以中速运用精密解析方程去校正积分参数(姿态、速度或位置)，其输入是由在参数修正(姿态锥化修正、速度划桨修正以及高分辨率位置螺旋修正)时间间隔内计算运动角速度和加速度的高速算法提供的。该设计方法考虑了通过捷联系统惯性传感器对角速度或比力加速度所进行的测量以及用于姿态基准和矢量速度积分的导航系旋转问题。本论文上篇定义了捷联惯导积分函数的总体设计要求，并开发出了用于姿态修正算法的方向余弦法和四元数法；下篇着重讨论速度和位置积分算法的设计。尽管上下两篇讨论中常常涉及到基本的惯性导航概念，然而本论文提供的材料都假定是为那些熟悉惯性导航的人使用的。专门用语： =任意坐标系； =定义为由施加的非重力产生的相对于非旋转惯性空间的加速度比力，用加速度计测得； =将矢量从坐标系投影到坐标系的方向余弦矩阵；I =单位矩阵； =列向量，它的各项元素等于矢量在坐标系A的各轴上的投影 =向量的反对称（或交叉积）形式，代表如下矩阵： 其中：，是的分量，与A系矢量的矩阵乘积等于与该矢量的叉积； =坐标系相对于坐标系的角速率，当为惯性系（I系）时，是由安装在坐标系上的角速率传感器所测到的角速率。1 导论捷联惯导系统(INS)一般是由惯性角速率传感器和给INS计算机提供数据的加速度计构成的一个正交三轴系统组成的。惯性传感器直接安装(捷联)到INS底盘构架上，这与原来采用主动多轴方向架隔离安装组合防止传感器旋转的INS技术形成对比。INS计算机中进行的主要软件函数运算是将敏感的角速率积分成姿态，将加速度计敏感到的比力加速度变换成导航座标系，将软件模型重力与变换比力相加，计算出整个加速度，并将整个加速度双积分成矢量速度和位置。INS软件设计过程中的关键因素是开发出能在有动态角速率，比力加速度输入的情况下完美无缺地进行姿态、矢量速度以及位置数字积分函数运算的重复数字算法。如上篇(参考文献【1】)中讨论的那样，大多数现代捷联INS采用一个基于双速算法的姿态修正算法：高阶修正算法是用来自高速算法的输入以中等重复速率进行的。中速例行程序可以由精确的封闭型姿态修正运算表示。高速算法的设计目的在于准确计算出在能校正成系统姿态变化(传统上称为锥化)的各个中速算法修正之间的多轴高频角运动。原来设想成为简单一阶算法的现今高速姿态算法利用了现代计算机日益增加的吞吐能力，已变成以改善精度为目的的高阶算法(参见参考文献【1】第57页和文献【8】的第7节)。在姿态修正函数演变成现有形式这期间，虽然人们在用于比力加速度转换，矢量速度积分以及位置积分函数的配套捷联INS算法方面同时进行了开发研究，但有关此类研究成果的发表的论文极为少见。本论文的论题恰是这一主题。比力变换算法的关键是处理初始传感器数据，进而计算出整个矢量算法修正时间间隔内导航座标中的积分比力增量。矢量速度的修正是在以前的矢量速度值的基础上增加导航系比力增量(加上用于重力和座标系旋转影响的增量)来实现的。变换算法的一个关键作用是准确计算出矢量速度更新时间周期内的姿态旋转(也就是捷联加速度计的旋转)。在一些应用方面，这一目的已经通过使用取中心算法得到实现。在取中心算法中，比力变换用的姿态数据是在矢量速度修正时间间隔的中心点得到修正的(因此，引入了交错姿态修正/矢量速度修正软件结构)。变换运算由对矢量速度修正间隔内加速度计比力输入进行积分运算和用矢量修正时间间隔中心点的姿态数据将积分过的比力增量变换成导航系的运算这两种运算组成。后一 种算法的变种是以两倍矢量修正速度对姿态进行修正，以便得出比力增量变化所需速度修正之间的姿态解。另一个变种是计算用于比力变换的姿态(作为速度修正时间间隔的起始和末尾姿态解算均值)。两速算法还可用于动态环境下的比力变换，速度积分(与两速姿态积分算法相类似。参见参考文献【5】和【8】第7.2节)。设计高速算法的目的是计算高频角振荡和线性振荡(这些震荡能纠正传统上称之为划桨的系统性速度生成)；而中速算法的目的是进行建立在高速算法输入基础上的比力变换。一般来讲，比力变换/速度积分算法一直没有姿态积分算法在分析上那么复杂，一般只局限于在机动条件下的初阶精度。实际上，迄今为止尚未见到过有关惯性导航位置积分函数方面的研究专著。据本文作者所知，现代捷联INS通常产生的位置是一个简单的速度不等边四角形(梯形)积分，其积分速率等于或小于速度修正频率。对于需要精确位置数据动态环境下的应用而言，如此粗略的位置积分算法是远远不够的。本文为捷联惯性导航比力变换、速度积分以及位置积分算法等的设计提供了一个综合方法。所提供的资料是由参考文献【8】第7.2节和第7.3节浓缩而成的(参考文献【5】材料的扩展)，着重介绍一套严密的分析方程式和便于计算机文件处理和验证的准确的封闭形式的方程组。论文中提供的速度和位置算法是采用两速计算格式构建起来的；设计中速算法如50-200HZ的目的是为了使在中速修正时间间隔内，在恒定角速率/比力加速度条件下的运算更为精确；中速算法是由如14kHz这样的高速运算提供数据的，这类高速运算在恒定角速率/比力加速度条件下会产生动态偏差速度算法产生划桨和位置算法产生漩涡(作者术语)。本文中还介绍了对积分修正时间间隔内导航座标系旋转进行的严密处理的方法。本论文的层次结构如下：第2节定义所用座标系。第3节将上篇姿态算法推导式用作公式化描述两速比力加速度变换/速度积分算法的模式。第3节把第2节用作以下面两种形式推导位置修正算法的框架，两种形式是建立在不等边四角形基础上的传统形式和两速高分辨率形式。第5节给出可以参考的推导出的算法一览表。第6节先围绕本文主题进行一般性讨论，随后介绍如何选择使用于特定用途的算法和如何建立这些算法的运算速率。第7节是结束语。最后，认识到下面这一点是十分重要的，即：两速方法的初衷是克服早期计算机技术(196575)的吞吐量的局限性，这一局限性随着现代高速计算机技术的不断快速发展而迅速消失。这就促使业内人士最终回到简单的单速算法结构问题上，正由于单速算法结构较为简单才使所有算法能以重复速率进行运算，而重复速率又十分之高，能精确求算出多轴高频角速率和比力加速度校正的效果。本论文所介绍的两速结构和上篇是兼容的，可以压缩成一个单速格式，这在下面算法公式推导的一节中有专门介绍。2 坐标系 座标系是一个由连续编号(或连续字母编序)的三个单元矢量定义的分析抽象阵，三个矢量单元按右手法则彼此正交。该座标系可以想象为一组三条垂直线(轴)通过一个公共点(原点)，单元矢量由该原点沿各自轴线向外发散。本论文中的座标系原点的实际位置是任意设定的。某一特定座标系中某矢量的分量(或投影)等于该矢量与座标系各单元矢量的点积。本文中采用的矢量归类为自由矢量，因而，在对它们进行分析描述时座标系中所取位置上没有孰优孰劣的问题。坐标系定义如下：1.E系是用于位置定位因而定义的地球固联座标系。它的典型定义是它有一个轴与地球极轴平行，其他两轴固连于地球并与赤道平面平行；2.N系是一个导航座标系，Z轴与当地地球表面参考位置的垂直向上方向平行，N系用于将加速度积分为速度并定义E系内当地垂直角方向；3.L系是当地水平座标系，与N系平行，但Z轴垂直向下，而X和Y轴沿N系的Y和X轴。L系用于描述捷联传感器座标系方向的基准；4.B系是捷联惯性传感器座标系(机体系)，其轴与标准右手正交传感器输入轴相平行；5.I系是一个非旋转惯性座标系，用作角旋转测量基准。为I系选择的特殊方向在其方向与解析运算有关的章节中进行讨论。3 速度修正算法本节着重推导用参考文献【1】的方程(16)和(18)积分求解参考文献【1】方程(20)的算法，比力变换项的算法以及将参考文献1方程(14)和(15)得出的角速率用于互补(复合向心)加速度项(角速率与速度的乘积)： (1) (2) (3)式中v是相对于大地的速度分析定义为E座标系中从大地中心到INS的位置矢量的时间导数；是铅垂重力(或重力)，对于固定的INS而言,该重力沿铅垂线沿线。是一个曲线矩阵(33)，是一个其元素（3，i）和(i,3)等于零且其余元素绕对角线对称的位置函数。对于球形大地模型而言，的其余元素是偏离对角线的零且等于对角线从大地到INS径向距离的倒数。对于扁球状大地模型而言，其余的项代表投影到INS高度的大地表面的本地曲面(参见参考文献【8】中5.3节有关闭环形的表述)。是的垂直分量。的值的大小取决于所采用的N系的类型，如漂移方位或自由方位。设计来确保对于所有大地定位来说是非奇异的(见参考文献【8】4.6节和文献【10】第8889页)。是一个向上沿地垂线(N系的Z轴)发散的单元矢量。 方程(1)采用的是方向余弦矩阵变换比力，而不是它的替代形式参考文献1方程(17)，四元数变换法，如适用于B系高度以高度四元数法形式计算那种情况下的四元数变换法。基于四元数比力变换的速度积分算法可以通过扩展这里提供的结果推导出来。 数字速度积分算法可直接的由方程(1)导出： （4） （5） （6）式中m是数字速度积分算法修正速率计算机周期指数。如果要把垂直信道重力/离散稳定性结合在一起，那就须给方程(4)增加修正运算.方程(4)代表垂直速度控制函数(见参考文献【8】4.4.1节和【10】第102103页)。方程(5)用公式表示用于求算重力/互补速度增量的数字算法，而方程(6)用公式表示用于求算积分变换比力增量的数字算法。3.1 重力/互补速度增量 方程(5) 项是位置定位函数，其水平分量很小。因为在整个数字算法m周期内位置变化很平缓，幅度变化有限(尤其在高度上)，所以，方程(5)的可以由m周期内的均值近似表示。因为方程(5)的互补项小(由于其角速率小的缘故)且因为在m周期内速度变化平缓，所以，互补量大小也可近似为m周期各互补量的均值。后者顺理成章地构成下述算法的基础下述算法是用求算的方程(3)去求算方程(5)的： （7）式中m-1/2是指代和之间的参数值，而是速度积分算法修正时间段。 方程(7)的项是用方程(2)估算的。而是由参考文献【1】方程(19)计算出来的。因为在方程(4)中被用来去修正，使之从m一1周期的值修正到m周期值，所以未在方程(7)中清晰显现，必须在从过去的值出发进行推断的基础上近似估算。举一个线性外推算法的例子如下式： （8）方程(7)中，和参数是位置的函数，这些函数随速度修正而得到修正，其修正速率可能是较慢的n倍周期重复速率，如比原速率慢五倍 。因此，为这些参数规定的m-1/2未清晰显现出来，必须在从以往值进行推算的基础上近似估算。例如，线性推算可表示如下： （9）式中：n=位置修正计算机周期指数；j=每个n周内m周期数；r=自从上个n周期(即从)以来m周期个数。3.2 积分变换比力增量积分变换比力增量方程(6)的数字算法必须考虑在到计算机循环周期内当地水平L系的旋转和捷联惯传感器机体B系的旋转。采用参考文献【1】第4.1节描述L系和B系在计算机修正时刻相对于惯性空间I的离散方向时，方程(6)可以通过使用参考文献【1】方程(3)链式规则加以扩展，见下式： （10）或进行下一步扩展： （11） （12） （13）方程(11)-(13)容许用于下述一般情况，即由于L系旋转需要对矩阵进行修正，修正的循环速率(系数n)可以不同于(慢于)由于B系旋转引起的修正速率(系数m)，例如：为了尽量减少计算机吞吐量的要求，软件结构中，n周期L系修正速率的设定比m周期B系修正速率慢四倍。然而，如果我们选择以用于B和L系运动的恒等速率即：n=m对进行修正，方程（11）-（13）仍然有效。必须指出，当nm时，方程(13)仍需要在B系m循环修正时间上对L系的方向进行估算(求算矩阵的)。还必须指出方程(11)是建立于在上述B系m循环中采用的基础上，即矩阵中的。这就意味着当B系紧随方程(11)变换运算之后进行旋转，将得到修正。该值用于定义求解方程(13)的项的算法，从而求解比力变换过程中的本地水平系旋转以及求解方程(12)的机体系积分比力增量项。3.2.1 比力变换过程中当地水平系旋转校正 因为相对于惯性空间来说L系的角速率慢，所以，方程(13)的很接近于单位矩阵I。因此，对于许多应用场合来说，方程(13)的完全可被忽略，因为与其他加速度误差源相比较，它可以忽略不计。对于包含有的高精度应用场合来说，参考文献【1】方程(49)和(50)的一阶形式通常就够了，由此可以得出： （14） （15）然后用参考文献【1】中的方程(3)、方程(13)以及第3节A段慢速改变互补项的假设，求方程(15) 的近似值： （16）式中下标n一1、m表示从到中途的参数值。 将方程（16）代入方程（15）得： （17） （18）方程(17) 项是由方程(2)估算的。如第三节A所述，方程(17) 须在以往值推断的基础上近似求出，例如： （19）因为方程(17)被用于修正方程(4)、(13)和(14)的，所以的现值尚不能用于估算方程(18) 。因此，必须采用以往值推断，如象第三节A所述： 当 当 （20）3.2.2 机体系积分比力增量 方程(11)和(12)的积分项是用类似于参考文献【1】方程(35)和(36)姿态修正求解所采用的那种形式计算出来的。该算法的推导开始是建立在对进行一阶近似求算的基础上的一阶解被分成两部分，应用于两速算法：其中：一部分可以以m循环速率计算出来( 循环速率测量恒定B系角速度和比力的影响)；而高速部分在m循环范围内，它可测量B系角速率/比力的动态变化。之后在恒定的角速率/比力条件下，一阶m循环部分被扩展开来进行准确分析。紧随参考文献【1】第4.1.1节的推导，方程(12) 被推函数项被表示为下式： （21）式中是一个旋转矢量，定义t大于时刻相对于系的B系的一般指向。参考文献【1】方程(32)和(33)表明方程(21)的可以近似地表示为： （22） （23）式中是一个积分时间参数。与方程(22)相一致的方程(21)的一阶近似值忽略并将正弦项近似表示为1(假设所选的周期速度十分之快)，足以使维持在一一个小得合情合理的值上，如小于0.05度。借用方程(22)，可将方程(21)简化为： （24）将方程（24）代入方程（12），然后得一阶： （25）或者，包括方程（23）： ， ， （26）方程(26)定义了计算方程(11) 的一种方法。建议在恒定B系角速率和比力情况下分析这些方程，对于它们而言：， （27）将方程(27) 代入方程(26) 表示式可得到恒定B系角速率和比力： （28）或用方程(26)和(27)求恒定B系角速率和比力： ， ， （29）将用于一般求算的方程(26)与用于恒定角速率/比力计算的方程(29)进行比较，便可以发现二者的差别是用取代了整数项。 对于在到时间间隔内恒定角速率/比力是一个合乎情理的近似值的那种情况，方程(29)更为适用，因为整数项(及其伴生高速算法)被所取代，该值在每一个m周期估算一次。 方程(26)或(29)的带根本性的局限性在于其一阶是近似值，这阻碍了它们的发展应用，即方程(24)用于求算曾用于方程(12) 表示的。方程（24）的估算值仅仅应用于求解的高频内容，而低频内容仍保持满足方程(21)的形式，这被认为是可取的。此算法可以通过下述方法合并而成，首先注意到： （30）式中和的定义与方程(26)中的定义相同。一旦重新组合，方程(30)即变成： （31）略作变形得： （32）现在用方程(31)代替方程(32)等式右边一个项可得： （33）从方程（26）可知： ， （34）由方程（33）式推导出下述形式： （35）方程(35)是方程(26) 表达式中被积式的替换式。用方程(35)代替方程(26)求被积函数可得到下述等量式： （36）如果现在在恒定角速率/比力条件下对方程(36)和(29)的项进行比较，我们会发现，除了方程(36)的积分项外，它们是等量值。通过置换方程(27)，会很容易地验证这一点， 即对于恒定B系角速率/比力而言，方程(36)的积分项变为零。可以得出结论：方程（36）的积分项代表了方程(12) 被积函数中高频内容的积分基值。剩余项项则代表了低频内容。前面定名为划桨的方程(36)的积分项，测量并入纯恒基值的动态角速率/比力对的校正。该校正在经典划桨运动条件下是最大值，经典划桨运动定义为角速率/比力为正弦项，在此状态下，绕B系某一轴的角运动处于与另一轴同频同相的状态 (然后沿第三个轴方向产生得到校准的恒定比力)。这跟水平用单桨通过起伏摇动划船前进的原理是一样的(诚如前面定义的“划桨”这一原始术语所表述的)。必须指出，即使方程(26)的积分项在恒定角速率/比力亦即非划桨状态下含有大基值，该积分项亦被称之为划桨。方程(36)的项在这里被当作速度旋转补偿项。速度旋转被用来表示将一旋转补偿项反馈给速度速率方程(这与第四节所讨论的反馈给位置速率方程的位置旋转补偿项相反)。根据这些说明，通过对方程(26)和(36)进行比较，识别出方程(26)的积分项是代表划桨效应补偿和速度旋转补偿这二者的合成，用后一术语可将方程(36)重新表示为： （37） ， （38） ， （39）式中是速度旋转补偿项，而是划桨效应补偿项。换一种方式，从方程(26)出发： （40） （41）式中和来自方程(38)的划桨效应项，而式中是复合划桨效应和速度旋转补偿项。 方程(37-39)与方程(40)和(41)完全等价；两个方程组只展示出一阶精度。然而，现在方程(32)处于这样一种形式，它能使我们用一个扩展表示式代替方程(39)速度旋转补偿项(该项能使方程(37)正好处于恒定速率/比力状态下)。这是一个重要的扩展，因为一般的运动通常都受到低频角速率和比力补偿韵支配(角速率和比力补偿在极端机动条件下亦即二阶算法误差不得忽略的条件下幅度很大)。要将方程(40)和(41)扩展到精度很高是不可能的，因为旋转补偿效应内嵌有积分项，该积分项包含一阶划桨项。下面的各个代入法除了能为方程(38)推导出数字积分算法外，还可以为方程(37)推导出精确的速度补偿算法。用同样的过程，还能为方程(40)和(41)的推导出数字积分算法，如参考文献【8】第7.2.2.2.2节所示。精确的速度补偿 精确速度旋转补偿算法定义为这样一种算法，即当该算法计算方程(37) 时，该算法在恒定B系角速率/比力条件下,可以为方程(12)的提供一个精确的解。精确速度补偿算法则是用在恒定角速率/比力条件下求算(21)式的，并从方程(12)中推导出来。我们首先考虑更为一般的条件，在此条件下，只有角速度矢量的方向是恒定的，亦即非锥化环境，在此环境下角速率矢量不发生旋转。从方程(23)可以看出，对于非锥化角速率而言：， ， （42）式中是的模值，而是沿着方向的单位矢量，它在B系中被当作是恒定的。 如象参考文献【1】第4.1.1节中所讨论的，对于不发生旋转的情况而言，等于（的积分）。在这样的约束条件下，用于求解方程(12)的项的方程(21)与方程(42)给出适合于非锥化角速度条件下的解算式： （43）对于非锥化角速率和恒定B系比力而言，方程(43)可以扩展成： （44）第3.2.2节的专门术语现在可用于非锥化速率恒定比力推导与适用的方程(42)的关系：， ， （45）而是的模值。将方程(45)代入(44)式可以得到非锥化角速率和恒定比力解如下式： （46）要估算(46)式的积分项，现采用恒定角速率条件，在此条件下方程(42) 为恒定值。然后：， （47）用求算的方程(45)，将方程(47)代入(46)，可以得到在恒定B系角速率情况下的积分项估算为： （48）代入方程(46)得到恒定B系角速率和比力条件下精确表述的理想形式： （49）方程(49)构成恒定角速率比力条件下的精确解。现在我们可以在同样的条件下对方程(49)与(37)进行比较，以识别精确速度旋转补偿项。在恒定角速率比力条件下，方程(37)的划桨项变为零(见第2.2.2节讨论)，且可由下式给出： （50）如果对方程(49)和(50)进行比较，根据其定义，务必明白这一点，即精确速度旋转补偿项可以表示为如下式： （51）方程(51)的三角系数可以从泰勒系列公式中计算出来： （52）方程(51)与(52)构成(39)式速度旋转补偿项求解的一个替代算法，它在恒定B系角速率/比力条件下为(37)式的产生一个精确解。反之，(39)式的算法只准确到一阶。须注意，当采用一阶形式时，求解时，用(52)式和(51)式求得的会降到(39)式 的形式(即像它理应呈现的形式那样)。积分比力和划桨增量。这一部分我们着重推导（37)和(38)式中用于计算和积分的数字算法(37)和(38)式的项由本文上篇(41)式姿态算法求得。亦可以用类似的过程推导出(40)和(41)式的算法。依照本文上篇第4.1.1节用于推导锥化算法的相同过程，我们可以通过假定是在一般函数在时刻的值的办法，推导出划桨算法(如像(38)式那样)。试设(38)式积分在到时间间隔内被分成多个部分，直到时刻或其后，于是得出： （53） 现在，我们定义到时间间隔内的下一个循环时间点，以便使(53)式中源自(38)式的和，包括初始条件在内，变成下式： ， ， （54）当时， ， （55）当时， （56）， 当 式中是高速计算机循环次数。(54)-(56)式构成以为计算机循环速率计算划桨效应项和 (作为整个到时间间隔内和的变化累加求和)的数字递归算法的结构框架。它仍然起决定(56)式积分项的数字等量的作用。我们开始通过把和(54)式和的定义代入可得到下式： （57）划桨方程(57)式积分项的数字算法是在对到时间间隔内B系角速率/比力曲线关系变化过程进行假设的基础上推导出来的。目前已发表的关于选择角速率/比力时间曲线关系应用于划桨算法设计方面的论文很少，不像锥化算法方面的论文那么多。从原则上讲，用于锥化算法的种种方法也可以用于划桨，包括划桨式运动的优化法(参见本文上篇第4.1.1节)。就此论文而言，我们提供了一个基于到时间间隔内一般线性变化角速率/比力的例子：, (58)式中：A，B，C，D是常数矢量。 (57)式积分项算法可以通过下述方法推导出来，即先用(58)式取代(57)式和，然后分析计算整个到时间间隔内(57)式的积分项。中间结果是作为A、B、C和D各常数矢量的一个函数的(57)式积分项公式。然后通过对来自各惯性传感器的积分角速率和比力增量进行连续测量，计算出每个到时间间隔的A、B、C和D常数矢量。这步务必需要进行两次连续测量，方能极为不易地为(58)式线性斜坡式模型测定出四个常数矢量A、B、C和D(抛物线式模型的特点是它有六个矢量，而且需要进行三次连续的传感器测量才能确定，等等)。其结果然后代替前面已定义过的中间结果中的A、B、C和D，导出与在整个到时间间隔内与(57)式积分项等量的积分算法。如果以循环速率对连续传感器增量进行取样，测量须在和时间点处进行，与跨接，与跨接(或与整体相接)。另一种可替代的方法是，传感器取样可以在到时间间隔内进行，在每个循环为(58)式线性斜坡模型取两个样，为抛物线模型取三个样等等。对于以循环速率采的传感器的样而言，后一过程(详见参考文献【8】第7.2.2.2.2节)的结果表明，对于(58)式线性斜坡模型而言，与(5457)式等量的算法可以由下式给出： 本文上篇(46)式的积分角速率传感器输出 （59）， ， 当 （60），当 （61）式中：来自加速度仪的积分比力输出增量累加求和；微分积分比力增量，即来自捷联惯加速度计的脉冲输出的分析表述，表示。求解的(61)式被划归为二阶算法，这是因为它包括现有的和过去的循环，的乘积。中的，循环，乘积项，亦即项源于到时间间隔内线性斜波角速率和比力的近似值。如果角速率和比力项被近似为抛物线变化的时间函数，定会得出一个含，和循环，乘积的三阶算法。如果角速率和比力被近似为到区间的一个常数，则(61)式项定会消失，导致一个求的一阶算法。如果角速率和比力缓慢地变化，我们可以将近似为等于零。另一个可供选择(且更为精确)的方法是我们可以设循环速率等于m循环速率，这使与曾在时间点计算出的(61)式相等从(60)式初始条件定义和本文上篇(46)式可以看出和必定是零。须指出，通过增加m速率使它与速率相匹配的办法也能达到使和m速率相等的目的。其结果势必是一个简单、高速、高阶算法，其软件结构比两速算法更简便，但要求的吞吐量更大。现代计算机不断增强的运算速率会使这一算法更有前途。4 位置修正算法在这一节中，着重就以地球表面上方高度h的形式求算相对于地球的位置和定义本地水平N系与地球E系之间角方向的方向余弦矩阵(从中可以提取出经度与纬度)的数字积分算法。文中写出两种算法形式：一个是基于梯形速率积分的典型算法，另一个是描述位置修正期间动态姿态和速度变化情况的高解算法。高解算法的数学模型建立在第3节两速矢量速度修正算法后面。上述两种算法即典型算法和高解算法都可以用本文上篇(21)和(22)式连续微分方程形式表示，重抄如下： （62） （63）式中：h表示地球表面上方高度。典型算法和高解算法是从h和的一般修正公式中推导出来的。下面几节推导出一般位置修正过程公式，然后推导出典型的、高解的位置修正计算方法。4.1 一般位置修正一般的高度h的修正算法与(62)式积分一样，是在位置修正n循环内被公式化的： （64） （65）考虑到更高速的数字算法回路即高度和速度积分的m回路，(65)式可以写成： （66） （67）如果要将垂直信道重力/离散稳定也包括进去，则(64)式须包括表示高度控制函数的附加运算(见参考文献【8】第441节和参考文献【10】第102103页)。求解方向余弦矩阵的一般修正算法是为了在修正时间里获得与(63)式表示式在同样的时间间隔内得到的正规连续积分一样的数字结果而设计的。该算法是这样开发出来的，即把数字修正领域的本地水平导航N系方向关系函数(63)式乘以想像为它是由每个修正时刻相对于地球(E系)的连续独立方向构成的。然后，用本文上篇(3)式方向余弦矩阵积链式法则建立求解的一般性修正算法，如下式： （68）式中：= 在计算机修正时刻，N系在旋转地球座标系空间(E)中的离散方向；= 在时刻，N系相对E系的方向余弦矩阵；=在时刻，N系相对E系的方向余弦矩阵；= 方向余弦矩阵，描述N系相对于地球（E），在时刻的指向朝时刻的指向发生的旋转。（68）式矩阵的正式定义如下： （69）（69）式中的代表在到时间间隔内的任意时刻，N系的姿态。 按照本文上篇第4.1.1节关于求解算法的同样开发过程，也可以利用定义的系的姿态相对于系的旋转矢量形式表示矩阵。利用本文上篇(4)式，并进行泰勒展开可得： （70） 式中是定义的 时刻系姿态相对于时刻系姿态的旋转矢量。N系相对于地球的角速率是比较小的，在典型情况下绝不会大于一个或两个地球速率。之所以如此，是因为从到的修正循环相对而言较短，的幅度很小。因为小，而且在典型的到修正循环范围内在缓慢慢改变（由于在这一时间段内矢量速度和位置发生的变化小)，所以N系速率矢量值可以近似为非旋转。其结果是(70)式可以被求出来，作为本文上篇(10)式旋转矢量速率表达式的积分简化形式，该式中各叉积项被忽略不计： （71）(71)式积分的离散数字算法可以这样建立起来，即先将(3)式求解的近似成表示成下式： （72）式中是在和中间的值。将(72)式代入（71)式并应用(67)式定义，然后得出下式： （73）式中，是计算机n循环修正周期 。(71)式项是所有的位置函数，这些位置函数尚未得到修正。因此，要算出各项的值，必须使用一个基于前述参数计算值的近似推算公式。例如，使用线性推算公式算出的最后两个值将会是： （74）计算源自(67)式积分的(66)式和(73)式的项的方法取决于在位置修正中是否采用典型的梯形积分法还是采用更为精确的高解积分法。两种方法都将在下面章节中进行介绍。4.2 典型位置修正应用典型梯形求积分法解算h和修正过程势必会用到(64)、(66)、(68)、(70)、( 73)和(74)式以及求解的(67)式中的梯形求积分算法： （75）4.3 高解位置修正用于进行和修正过程运算的高解算法采用(64)、(66)、(68)、(70)、(73)、(74)式以及求解 的(67)式中的高速数字积分算法。求解的数字算法是这样开发出来的，即首先扩展(67)式的被积函数。将(4)式的表达式代入(6)式。可以定义为自最后的修正时刻起在一般时间点上的连续时间函数： （76）方程（76）是基于这样的假设，即重力互补项可以近似为时间到的一个常数积分。用(76)式可以将(67)式表示为： ， （77）式中是由比力产生的在L系的投影。 (11)、(13)和(36)式表明，(77)式中的可由下式近似到一阶(机体旋转角中)； （78）， 式中： =为时刻B系运动和时刻L系运动得到的修正矩阵，=在时刻与时刻L系与由(14)式算出的结果关联起来的方向余弦矩阵的现有的和过去的m循环值。在(78)式中，为了简便起见，I的下标和上标符号为清晰表示(11)和(13)式被略去。此外，表示式的第一部分中的和已被近似为从到期间的线性斜坡形式。还须指出，如第3.2.1节那样，(78)式项可以由单位矩阵加以近似求值，并应用于精度要求特别高的工程运算中。基于(78)式包括(14)式，(77)式的项可以由下述等量形式加以定义： （79）式中，和来自划桨方程（38）。 按照第3.2.2节中所使用的在机体系积分比力增量的相同方法，求解的(79)式的项可以修改成非积分项加一个在常数角速率/比力下消失的积分项，两个项均为一阶精度。然后，非积分项将被扩展成在常数角速率/比力条件下没有误差的更为精确的形式。我们开始用传统部分求积代入法如第3.2.2节求解(35)式那样表明，(79)式项有下列等量形式： （80）， ， ， 式中，和是和随时间的积分。因为，和是相等的，我们可以写出下式： （81）用(81)式和取代，把它们分别代入(81)式，台并同类项后可得下式： （82） 现在用(80)式取代(82)式，并将它代入(77)和(79)式，获得计算的理想形式： （83） （84） （85）式中，、和来自划桨方程（38），并且： ， ， （86）式中，是类似于(37)和(39)式矢量速度旋转补偿项的位置旋转补偿项，而则是类似于(37)和(38)式划桨项的旋涡项。“旋涡”这一术语是作者专门为该项杜撰的一个名词，同时使它听起来像划桨但表示的是位置求积(在强调这一字母发音的位置矢量上有变化)。伴随涡卷的复杂数学推导和相关算法也许是作者杜撰“旋涡”这一提法的更为重要的原因。 (84)式的关键特性是(85)式的旋涡项在机体轴角速率和比力是常值的条件下其值是零。这可以轻易地得到验证，方法是将常值角速率和比力矢量代入(85)式，替代和项，然后分析进行的积分运算。这样一来，在机体轴角速率和比力是动态值的情况下只产生一个输出。这是一个很重要的特性，因为对于大多数真实的动态环境而言，高频角速率/比力的模值都很小，应用一阶近似值就可以达到精度要求 (所谓一阶是指在到时间间隔内积分机体角速率比力中的第一阶项)。我们的结论是在低频机体角速率和比力分量较大的情况下，应用的分析形式同样也是在合乎情理的精度范围内。 (86)式位置旋转补偿的一阶形式在极端机动条件下有一个显著的二阶误差。（84）式能够使与其它项分离，该形式使我们能够将扩大成更为精确的形式，这一形式在角速率比力为常值的情况下很准确(如第3.2.2节第一子小节关于矢量速度旋转补偿项求算那样)。下面几节探讨(84)式中准确位置旋转补偿项和(85)式旋涡项及其它积分项的算法。4.3.1 准确位置旋转补偿 (84)式的精度改进形式通过下述方式导出，即规定其解在机体角速度比力是常值的情况下是精确的，但是在机体角速率比力为一般情况时，其解的一阶项等于(86)式的值。此推导开始先回到(79)式的基本定义： （87）如（42）式和（44）式那样，现在我们在基于B系比力为常值和非锥化角速率的条件下，利用（87）式的精确定义： （88）对于B系角速度(矢量方向和模值)是常值而言，如在(47)式那样， (88)式积分项可以得到评估： （89）从（47）式我们可以得出： （90）于是在B系角速率为常值时，（89）式变为： （91）（88）式利用（91）式的结果，得到在B系角速率和比力为常值的情况下，结果为： （92）(92)式可以进一步改进，方法是按照求双积分的(85)式那样代入，应用(45)式适当项的定义，并进行因式分解：（93）(93)式项可以通过下面的推演用替代方式表示：采用(85)式的适当定义，我们发现，在B系角速率和比力为常值的情况下，有下述情况： （94）从(94)式可以看出，在(93)式B系角速率比力为常值条件下的与下式等量： （95）然后将(95)式代入(93)式，可以得到B系角速率和比力为常值条件下，（93）式变为： （96）现在(96)