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

捷联式惯性导航积分算法设计 上篇 姿态算法 Paul G Savage Strapdown Associates Inc Maple Plain Minnesota 55359 摘要 本论文分上下两篇 用于给现代捷联惯导系统的主要软件算法设计提供一个严密 的综合方法 将角速率积分成姿态角 将加速度变换或积分成速度以及将速度积分成位置 该算法是用两速修正法构成的 而两速修正法是具有一定创新程度的新颖算法 是为姿态 修正而开发出来的 在姿态修正中 以中速运用精密解析方程去校正积分参数 姿态 速度 或位置 其输入是由在参数修正 姿态锥化修正 速度摇橹修正以及高分辨率位置螺旋修 正 时间间隔内计算运动角速度和加速度的高速算法提供的 该设计方法考虑了通过捷联系 统惯性传感器对角速度或比力加速度所进行的测量以及用于姿态基准和矢量速度积分的导 航系旋转问题 本论文上篇定义了捷联惯导积分函数的总体设计要求 并开发出了用于姿 态修正算法的方向余弦法和四元数法 下篇着重讨论速度和位置积分算法的设计 尽管上 下两篇讨论中常常涉及到基本的惯性导航概念 然而 本论文是为那些已对基础惯导概念 很熟悉的实际工作者而写的 专门用语 任意坐标系 123 A A A A 将矢量从坐标系投影到坐标系的方向余弦矩阵 1 2 A A C 2 A 1 A I 单位矩阵 从坐标系投影到坐标系的旋转矢量所构成的姿态变化四元数 1 2 A A q 2 A 1 A 的共轭四元数 它的第1项与的首项相同 余下的2 4项与 1 2 A A q 1 2 A A q 1 2 A A q 的互为相反数 1 2 A A q 单位四元数 它的第1项为1 其余3项为0 1 q 无具体坐标系定义的矢量V 列向量 它的各项元素等于矢量在坐标系A的各轴上的投影 A VV 向量的反对称 或交叉积 形式 代表如下矩阵 A V A V 0 0 0 ZAYA ZAXA YAXA VV VV VV 其中 是的分量 与A系矢量的矩阵乘积等于 XA V YA V ZA V A V A V 与该矢量的叉积 A V 与等量的四元数矢量 A q V A V 0 A V 坐标系相对于坐标系的角速率 当为惯性系 I系 时 是由安装 2 A A2 A 1 A 1 A 2 A A 在坐标系上的角速率传感器所测到的角速率 2 A 1 概论 惯性导航是通过对速度积分得到位置并对总加速度积分得到速度的过程 总加速度是指由重力加 速度和被施加的非重力产生的加速度 亦即比力加速度 之和 惯性导航系统 INS 包括 用于 积分的导航计算机 用于给积分运算定时的精密时钟 测量比力加速度用的加速度计组台 用 于作为所算位置的一个函数而进行的重力加速度计算而留于导航计算机中的重力模型软件 以及 为了定义作为速度计算一部分的加速度计三元组的角度方向所用的姿态基准 在现代INS中 姿 态基准是由驻留于INS计算机中的软件积分函数提供的 其输入来自一个有三轴的惯性角速度传 感器 角速度传感器和加速度计三元组安装在一个公用的牢固构架上 该构架装在INS的底盘上 以保证每个惯性传感器之间的精确对准 这样的一种布置称之为捷联INS 因为惯性传感器牢固 地固定在底盘内 所以也就牢固地固定于安装INS的飞行器上 INS计算机中的基本函数有将角速率变换为姿态的积分函数 称之为姿态积分 使用姿态数据将测 得的加速度值转换到适当的导航坐标系中 再将它积分成矢量速度的函数 称之为矢量速度积分 还有将导航系矢量速度积分成位置的函数 称之为位置积分 这样就有了三个积分函数 姿态函 数 矢量速度函数及位置函数 每个函数的精度要求很高 以确保函数误差极小 符台惯性传感 器精度的要求 回眸历史 因为早在50年代 基础捷联惯导概念就开始形成 所以多年来捷联分析师将精力主要 集中于姿态积分函数算法的设计上 而种种算法的设计方法总是受到当时的飞行计算机技术的能 力和局限性的影响 50年代后期和60年代 各种研究机构的捷联工作者们采取两种用于姿态积分 函数运算的方法 即用一阶数字算法进行高速姿态修正运算 如10 20kHz和用高阶算法进行的 低速姿态修正运算 如50 100HZ 如果要精确考虑高频角速率分量时 就得考虑用高速算法 这样可以调整为系统的三维姿态变化 然而 那个时期的计算机工艺技术只能对姿态修正算法进 行一些简化的一阶方程运算 精度有限 相反 高阶算法提倡者极力吹捧较一阶算法提高了 解析精度的高阶算法 但是 由于每个姿态修正循环可执行运算次数的连带增加导致必须使 姿态修正速率减缓以满足当时的计算机吞吐量的限制 从而使提高了的精度被降低 由于姿态四 元数可以当作对所算姿态参数的解析形 这一优点使之成为一种更可取的算法 与传统方向余弦矩阵 姿态表示法相比 所以它的出现使上述两种算法的优点黯然失色 就那一时期研究过的算法而言 四 元数法在高频角速率环境下表现出的运算精度最优 1966年 作者提出一个新的两速姿态积分函数运算方法 在该方法中 姿态修正运算被分成两部 分 即把简单高速一阶算法部分与更复杂的中速高阶算法部分结合起来使用 后一部分的输入 由高速算法提供 简化了的高速部分用于考虑姿态修正循环内的高频角振荡 这能调整系统的姿态 建立 传统上称之为锥化 合在一起 两速方法的组合精度等于以高速速率进行的高阶运算 为了提 高精度 然而 由于高速算法简化之故 组合后对计算机的吞吐量的要求丝毫也不比原来的高速一 阶姿态修正算法的吞吐量大 参考文献 6 采用的两速算法设计受到其基本解析公式的限制 该公 式是一个皮卡德类型 Picard type 的连续型的姿态速率微分方程的递归积分 在该方程中 同时 产生中速和高速算法 解析递归积分设计过程的复杂性限制了高阶中速算法的扩展 在参考文献 6 中仅扩展到2阶 这在当时被认为是可以接受的 1969年 焦尔敦 Jordan 在一个不相关的设计活动中提出一个用于捷联姿态修正函数的两速法 在此方法中 解析公式的开头建立在两个单独定义的计算上 一个建立在输入姿态变化基础上的中 速 经典封闭型 准确高阶姿态修正算法和一个测量中速算法姿态变化输入的简化高速二阶积分算 法 1971年 鲍尔兹 Bortz 将焦尔敦的概念扩展到基于微分方程的高速计算 积分时将测量的准 确姿态变化输入给准确的姿态修正算法 准确的中速姿态算法可以通过两个三角系数舍位构建成任 何精度要求的特定阶次 实际上 鲍尔兹的姿态变化微分力程的简化型式现被用于高速函数计算 因此 参考文献 7 和 8 提供了一个更一般的两速姿态修正算法 在此算法中 中速高阶算法 和高速简化算法可以独立地进行改编以满足特定的应用要求 让人感兴趣的是 参考文献 8 提 出了一个简化型高速算法的模拟执行程序 参考文献 7 和 8 提出的两速法 主张在准确中 速姿态修正运算中采用方向余弦法 的第二个好处是中速部分也可以用一个解析的精确的封闭形 式的四元数修正算法来作为公式 该算法采用同用于方向余弦修正算法一样的高速输入 这样 一来 新的两速法无论对于方向余弦法还是对四元数修正法都有同样的精度 而这两种方法都是由 解析的精确封闭式方程推导出来的 这里假定对泰勒级数的三角系数的展开进行到比较准确的量级 大多数飞机使用的现代捷联INS采用以两速法为基础的姿态修正算法 中速算法部分的重复速率一般 是基于最大角速率来设计 如50 200Hz 以尽量减少中 高速算法中幂级数舍位误差的影响 高 速算法的重复速率是在预计的捷联惯性传感器组合振动环境的基础上精确考虑了引起振荡的锥化效 应而进行设计的 如用于1n mile h有50 的径向位置误差速率的飞机的INS的重复速率为 1 4kHz 连续进行的两速姿态算法的开发工作一直集中于高速积分函数的变化上 原来设想的简 单一阶的种种现有高速姿态算法利用现代计算机日益增加的吞吐量 演变为各种高阶算法 而且经 度得到提高 参见参考文献 9 11 和 12 的第7 1节 对于姿态修正函数的演化 截 止目前 有关用于加速度变换或矢量速度积分和位置积分的捷联INS姊妹算法的开发方面的其他并驾 齐驱式著述很少见到发表 本论文上篇定义了捷联惯导积分函数的总体设计要求并描述了开发基于两速法的姿态积分算法的 综合设计过程 文章中所提供的材料是参考文献 12 第7 1节 它是参考文献 9 中材料的扩展 的浓缩型 这些材料着重于提供更严密的解析公式 同时 在可能的情况下 考虑到便于生成计算 机软件的文件及有效性 这与现代飞行计算机技术也是一致的 这些材料使用准确的封闭式方程 在姿态算法没计过程中包括了考虑到姿态修正时间周期内导航坐标系旋转的种种严密处理方法 本论文的第二节提供了有关坐标系和所采用参数的背景材料 第三节以连续微分方程形式提供了 一整套典型的捷联惯导姿态 矢量速度和位置方程 这构成了等量算法设计过程的框架 第四节探 讨以高速部分的普通形式进行两速姿态积分运算的算法 适用于在导航系旋转影响下的方向余弦法和 四元数法的公式 并描述了一个特殊的形式 说明在古典高速二阶锥化计算算法中一种算法的设计 第五节给出有关姿态积分算法的表格参考数据 第六节就选择特殊应用算法和建立运算运行速率之 后的过程进行了一般化讨论 第七节给出结论性评述 最后 认识到下述观点很重要 尽管两速法的初衷是克服早期 l965 1975年 计算机技术的吞 吐量的局限性 然而随着现代高速计算机的快速发展进步 这一局限性正在变得无足轻重 这就促 使工程技术人员想回到一个更简单的单速算法结构上来 采用这种结构时 所有计算以一个重复速 率进行 速率很高 足以准确地将多轴高频角速率和加速度调整影响考虑进去 本论文上 下两篇 提供的两速结构是和压缩成在对算法进行公式化的专门章节里解释的那种单速型是相兼容的 2 坐标系和姿态方向的关系 本节定义了本篇论文中使用的坐标系 并一般性地说明方向余弦矩阵 姿态四元数和 用于代表两坐标系之间的角度关系的旋转矢量姿态参数等等的性质 2 1坐标系定义 坐标系是根据右手法则对彼此互相垂直的三个连续编了号 或标了字符 的单位矢量加 以定义的解析抽象 它可以想像为三个垂直线 轴 穿过一个共同的点 原点 各单位矢 量从原点沿三条垂直线向外发散 在本论文中 坐标系原点的实际位置是任意的 在特定 坐标系中 一个矢量的分量 或投影 等于矢量与该坐标系各个单位矢量的点乘 本论文中 采用的矢量为自由矢量 因此 在对它们进行解析描述的坐标系中 其位置不存在优劣性 坐标系定义如下 1 E系是用于位置定位因而定义的地球固联坐标系 它的典型定义是它有一个轴与地球极轴 平行 其他两轴固连于地球并与赤道平面平行 2 N系是一个导航坐标系 Z轴与当地地球表面参考位置的垂直向上方向平行 N系用于将加 速度积分为速度并定义E系内当地垂直角方向 3 L系是当地水平坐标系 与N系平行 但Z轴垂直向下 而X和Y轴沿N系的Y和X轴 L系用于 描述捷联传感器坐标系方向的基准 4 B系是捷联惯性传感器坐标系 机体系 其轴与标准右手正交传感器输入轴相平行 5 I系是一个非旋转惯性坐标系 用作角旋转测量基准 为I系选择的特殊方向在其方向与 解析运算有关的章节中进行讨论 2 2姿态参数定义 方向余弦矩阵定义为一个方阵 其各列是单位矢量的正交组 每列等于沿系的一个轴方向 2 A 上的单位矢量在系各轴上的投影 1 A 1 1111 2222 123 AAAA AAAA Cuuu 式中是沿系轴i方向上的单位矢量在坐标系各轴上的投影 1 2 A iA u 2 A 1 A 从这一基本定义出发 可以验证出 的i行 j列的元素等于系轴i和系轴j之间的 1 2 A A C 1 A 2 A 夹角的余弦值 的转置矩阵等于其逆矩阵 转置矩阵的各列等于系轴中单位矢 1 2 A A C 1 2 A A C 1 A 量在系各轴上的投影 且与一个投影于系各轴上的向量的积等于此向量在系 2 A 1 2 A A C 2 A 1 A 各轴上的投影分量 而对的转置矩阵则相反 1 2 A A C 2 112 2 AAA A VC V 211 2 AAAT A VCV 2 式可以用于导出方向余弦矩阵链式规律 3 332 221 AAA AAA CC C 旋转矢量定义旋转轴和绕轴的旋转幅度 可以想像系通过绕此旋转矢量旋转而从它的起始姿 1 A 态转到一个新的姿态 绕旋转矢量旋转的角度等于旋转矢量的大小 现在 称系为系的新姿 2 A 1 A 态 通过给系的这一定义 一个任意定义的旋转矢量就可唯一地确定系姿态与原始系姿态 2 A 2 A 1 A 二者的关系 反之 如果给定系和系的姿态关系 就可以定义一个与此姿态一致的旋转矢量 1 A 2 A 这样一来 旋转矢量可以用来定义系与系相关的姿态 从解析式上看 见参考文献 4 2 A 1 A 9 和 12 的第3 2 2 1节 旋转矢量和方向余弦矩阵之间的关系可以由下式给出 4 1 2 2 2 sin 1 cos sin A A CI 式中和是旋转矢量及其幅度 旋转矢量的一个独特特性是在系和系中有相同的分量 1 A 2 A 见参考文献 12 的第3 2 2 1节 因此 4 式中的代表或 1 A 2 A 姿态四元数是一个四矢量 即有四个分量 定义为旋转矢量的一个函数 见参考文献 4 9 和 12 的第3 2 4节和 14 的第73 76页 5 1 2 cos0 5 sin0 5 0 5 0 5 A A q 由 5 式可以看出 各元素的平方和是单位1 坐标变换方程可以用四元数代数方程表示 见参考 1 2 A A q 文献 4 9 和 12 的第3 2 4 1节 6 1121 22 AAAA qAqA Vq Vq 21 11212 2211 AAAAAAA qAqAAqA Vq V qq V q 6 式可以用来推导出姿态四元数的链式规律 7 332 121 AAA AAA qq q 2 3 姿态参数速率方程 第2 2节姿态参数的变化速率 见参考文献 4 8 9 和 12 的第3 3节 由下式给出 8 11211 22212 11 AAAAA AAAAA CCC 9 11211 22212 11 11 22 qq AAAAA AAAAA qqq 10 111 121212 2 11sin 1 22 1 cos AAA A AA AA A 3 连续型捷联惯导方程 定义捷联惯导系统中进行的典型基本运算的微分方程 见参考文献 9 l2 的第4章和 15 的第77 103页以及156 177页 给出如下 姿态速率 11 LLBLL BBIBILB CCC 或代之以 12 11 22 qq LLBLL BBIBILB qqq 当地水平系旋转速率 13 LLNN ILNIEEN C 14 NETE IENIE C 15 NNNN ENCZNZNZN F uvu 加速度变换 16 LLB SFBSF aC a 或代之以 17 qq LLBL SFBSFB aq aq 18 NNL SFLSF aC a 矢量速度速率 19 NNNNN PIEIE ggR 20 2 NNNNNN SFPENIE vagv 位置速率 21 EEN NNEN CC 22 NN ZN huv 式中 R 从地球中心到INS的位置矢量 相对于地球的速度矢量 位置变化速率 解析式中定义为E系中R的时间导数 v h 地面高度 定义为从INS到地球表面的距离 沿从INS到地球参考重力平面上的切线平面的 垂线方向测量 曲率矩阵 3 3 是位置的一个函数 其元素3 i和i 3等于零 而其余元素绕对 C F E N Ch 角线成对称形 对于球形地球模型而言 除对角线上外其余元素是零 且对角线上数值是 从地球中心到INS的径向距离的倒数 对于扁地球模型而言 其余的项代表投影到INS高度 线上的地球表面上的本地曲率 见参考文献 12 第5 3节有关封闭型的表述部分 沿重力平面垂直方向 N系的Z轴 上的单位矢量 ZN u 的垂直分量 选取的值取决于所采用的N系的类型 如 游动方位系统或自由方 ZN N EN ZN 位系统的设计保证对所有地球定位而言是非奇异的 见参考文献 l2 的第4 6节和 N EN 15 的88 99页 比力加速度 定义为由施加的非重力力产生的相对于非旋转惯性空间的加速度 它由 SF a 加速度计测得 质量吸引重力加速度或重力 R的一个函数 g 铅垂重力或重力 对于稳定的INS而言 它沿铅垂线方向 P g 的解析模型可以在参考文献 16 17 的第4 4节和 18 的6 3节找到 关于在N系的gg 分量 请参见参考文献 12 第5 4 1节 在运行捷联惯导函数过程中 捷联INS计算机用适当的积分算法对后面的姿态速率 矢量 速度速率以及位置速率方程进行积分运算 关于后面的导航方程形式 下面几点值得注意 方向余弦和四元数姿态都是以机体姿态速率或加速 度变换运算的形式表示 无论哪一个部可以用于实践 而其结果实质上是一致的 矢量速度是相对 于地球 E系 定义的 矢量速度速率方程是以当地水平定义的N系写出来的 用于将它积分成矢量 速度 对于像飞机的INS之类的许多地形导航用途而言 这很典型 也可以选择其它坐标系用于 矢量速度定义和矢量速度速率方程 如用于战术和战略导弹制导 位置速率方程将位置定义为高度 和N系相对于E系的角度方向 从中可以提取出经度和纬度 并可以计算出R值 见参考文献 12 第4 5 1和4 5 3以及 15 的88 89页 对于位置速率方程而言 位置也可以定义为简单的R 从 中可以计算出和h 见参考文献 12 的第4 5 4节 高度速率方程 22 显得价值不大 但当 E N C 你考虑旋转扁地球模型 地球上的一个旋转N系和固定的高度定义时 这就不一定了 参考文献 12 的4 4节和5 5 节表明对于旋转的扁地球模型而言 方程 22 是准确的 如果决定引入垂直通道重力或发散控制来 防止不稳定垂直通道误差呈指数增加 那么方程 20 和 22 务必包括一个额外增加的垂直控制项 参考文献 12 第4 4 1节 15 第102 103页以及 18 的10 3节 4 姿态即时修正算法 本节探讨适合用数字计算机进行积分的方向余弦矩阵速率方程 11 和姿态四元数速率方程 12的 算法形式 这些方程将用现在的传统两速法来建立 传统两速法中的解析的精确的封闭形式的方程 可以用于基本姿态修正函数的运算 该函数的输入是由一个为了在基本姿态修正周期中测出姿态变 化而设计的高速算法所提供的 4 1 姿态方向余弦矩阵 方向余弦矩阵的修正算法是为实现下述目的而设计的 在姿态即时修正时间内所得的数 L B C 字结果跟在相同时间间隔对方程 11 中表达式的正规连续积分得出的结果相同 该算法的思路 L B C 是以理想的机体B系和当地水平L系在数字即时修正区间内确定的变化 方程11中由和引的 B IB L IL 作为每一即时修正时刻相对非旋转惯性空间 I系 的连续离散点上的当前构造值 考虑到完全 一般化的情形 我们还允许由于L系角运动的修正运算不一定是在当B系运动而对修正的同 L B C L B C 一时刻瞬时运行 例如在一个多速率数字计算回路结构中 以高速率进行修正是由于B系 L B C 旋转而不是由于L系旋转 如果对计算机吞吐量的最小化感兴趣的话 从软件结构上说 可以让L系 修正比B系修正慢5 10倍 我们用来描述坐标系方向随时间变化的专门术语如下 当计算机修正时间为时 机体B系在非旋转惯性空间I中的离散方向 m I B m t 因B系角运动对进行即时修正的计算机循环拍次 m L B C 当地水平L系在计算机修正时间在非旋转惯性空间I中的离散方向 n I L n t 因L系角运动对进行即时修正的计算机循环拍次 n L B C 有了这些定义 采用 3 式方向余弦矩阵递推计算规律便能建立起对即时修正算法如下 L B C 23 1 1 1 1 III nnm III mmm LLB BBB CCC 24 1 1 III nnn III mnm LLL BLB CCC 其中 时刻B系与时刻L系的转换关系矩阵 1 1 In Im L B C 1m t 1n t L B C 时刻B系与时刻L系的转换关系矩阵 In Im L B C m t n t L B C 考虑到B系相对惯性空间从时刻点到时刻点的旋转的方向余弦矩阵 1 Im Im B B C 1m t m t 考虑到L系相对惯性空间从时刻点到时刻点的旋转的方向余弦矩阵 1 In In L L C 1n t n t 在 23 和 24 式描述的算法把各个时间的机体B系和当地水平L系的方向联系起来 并为 B系与L系提供了以不同修正速率对进行的惯性角运动修正 与B系 它能以200 300度 L B C 秒的速度进行动态旋转 相比 当地水平L系的惯性角速率一般来说较小 它等于地球旋转 速率加上L系相对地球的角速率 传递速率 它一般不会大过地球速率的几倍 因此 L系 即时修正一般可以以低于B系修正的速率完成 而且有相当的精度 应当注意 对B系和L系 运动修正速率的要求 在某种程度上 是通过和测量姿态变化的近似高速算法作比较 将 误差降至最小的基础上确立的 见本文4 1 1和4 1 2节 对的B系和L系的运动修正是 L B C 由 23 和 24 式的项和项完成的 其算法分别推导如下 1 Im Im B B C 1 In In L L C 4 1 1 机体系旋转 23 式用来考虑捷联传感器 机体 B系相对于非旋转空间的角速率来修正姿态 1 Im Im B B C B IB 方向余弦矩阵 的正式定义为 L B C 1 Im Im B B C 1 1 1 m II mm It m m t BB BB t CICdt 25 式中B t 是到时间间隔内任意时间的机体B系姿态 1m t m t 矩阵还可以用定义的系姿态相对于系的旋转矢量来表示 对 4 式进行泰勒级 1 Im Im B B C m I B 1 m I B 数展开 可得系数项 1 2 sin1 cos Im Im B mm Bmmm mm CI 26 24 sin 1 3 5 mmm m 24 2 1 cos1 2 4 6 mmm m 式中 定义为在时刻系姿态相对系的旋转矢量 旋转矢量可以通 m m t m I B 1 m I B m 过把当作定义为大于时刻的普通的B系姿态相对于系普通旋转矢量来对待的办 1m t 1 m I B 法计算出来 然后 是通过对普通的方程从时刻进行积分得来 而 26 式中用到 1m t 的就是在时刻的积分结果 把用于定义的系当作非旋转惯性基准系来处理 m t 1 m I B I 一般可以得到下列表达式 其方法是将 10 式中的坐标系由机体系取代 而坐标 2 AB 系由惯性系取代 得到用角速率描述的下式 1 AI 2 11sin 1 22 1 cos BBB IBIBIB 27 式中定义为在大于时刻的系姿态相对于系的旋转矢量 通常称之为鲍尔 1m t B 1 m I B 兹方程的 27 式使机体系的姿态的变化与系相对于惯性系的角速度产生联系 BBI B IB 而可以通过捷联角速率传感器测得 B IB 于是 26 式的姿态旋转矢量便可以通过从时刻到时刻对 27 式进行积分求得 m 1m t m t 1 m t t tdt m mt 28 式中是运行中的积分时间变量 要减少 27 式中求算值的计算次数 应加入简化设计 例如 通过幂级数扩展 可以将 27 式中的项的放大系数近似为 B IB 2 2 1sin111 1 1 2 1 cos 126012 29 因此 27 式的可以由下式给出 30 11 212 BBB IBIBIB 通过仿真和分析 在假定的解析定义角运动条件下进行解析扩展 可以得到保证值的二 阶精度的表达式 31 111 2122 BBB IBIBIB 式中 32 1 m t B IB t td 31 式很重要 因为它能使 27 式简化到二阶精度 即的误差为三阶 只保持 阶项 这样一来 27 式就变成二阶精度 33 1 2 BB IBIB 代入方程 33 28 式则变为下式 34 1 1 2 m m t BB mIBIB t tdt 最后 用 32 式可得 35 mmm 其中 1 m t B IB t td mm t 36 1 1 2 m m t B mIB t tdt 式中是从到的圆锥姿态运动 项被锥化成锥化项目 因为它受到测量 m 1m t m t m 的圆锥运动分量的影响 圆锥运动定义为角速度矢量本身在旋转的状态 由于表现 B IB B IB 为纯圆锥运动 的幅度虽然恒定不变 但其矢量在旋转 则系的一个固定轴近似垂 B IB B 直于矢量的旋转平面 该固定轴将随着角速度的运动而形成一个锥面 因此用术语圆锥 B IB 运动来描述这个运动 在圆锥角运动条件下 与垂直的系轴表现为振荡 这与非圆 B IB B 锥运动或自转角运动相反 在后一种情况下 于垂直的轴绕旋转 B IB B IB 对不发生旋转的情况而言 从 36 式可以容易看出 将与平行 因此 B IB t B IB 中的积分函数的叉乘将是零 也将是零 这种情况下 即当不发生旋转时 34 式 m m B IB 将变成如下简化形式 1 m m t B mIB t dt 37 注意 37 式也适用于当不旋转时 27 和 28 式中的精确求解 即不存在近 B IB m 似问题 这点可以通过观察 27 式很容易地得到验证 因为在 27 式中 在积分开始 t 时就与对齐 并将与保持平行 这是由于在表达式中 与叉乘将保 B IB B IB t t B IB 持为零 在这种条件下 27 式和 28 式也将化简为 37 式 下面讨论积分角速率和圆锥增量算法 关于 36 式中的积分速率和的锥化表述的 m m 离散化数字算法可以通过这样的方法开发出来 即考虑是一般函数在 时刻的 m t t m t 值 由 36 式得 38 1 1 2 m t B IB t td 现在考虑将 38 式的积分分成在到时间间隔内到达时刻的时间和时刻之后 1m t m t 1l t 1l t 的时间两部分 这样一来 38 式等于 1 l tt mm t 39 1 1 2 l l t B lIB t tdt 式中 是时值 而 是时的计算机的循环拍次 需注意 根据上述定 1l 1l tt t l l tt 义 循环拍次快于循环拍次 现在 我们再定义一个在到的时间间隔内 循环时lm 1m t m tl 间点 于是在时间点 包扩初始条件在内的 39 式就变为 l t l t 1lll mllm tt 当 40 1m tt 0 l 1 1 2 l l t B lIB t tdt 通过类似过程 40 式的表达式可以通过 36 式的计算求得 t t 1 l tt 1 l t B IB t td 41 ll t 1lll 当 1m tt 0 l 根据 41 式和 42 式得 1 1 11 22 l l t B lllIB t tdt 42 1lll mllm tt 当 1m tt 0 l 41 和 42 式构成数字递归算法 该算法以 作为计算机循环速率计算和锥化项 l m m 作为从到的整个时间间隔内和的变化之和 42 式中积分项的数字当量仍 1l t l t l 尚待确定 姿态算法开发方面的后续工作集中在估算锥化方程 42 中积分项的数字算法设计上 一 一般而言 所采用的方法都是对从到时间间隔内的角速率模型假定一个一般的解 1l t l t B IB 析式 如一个在时间上截尾的一般多项式 于是 42 式的积分项就可以以解析的形式作为 一般速率模型系数的一个函数 如多项式系数来表示 最后 角速率模型系数是通过匹配 连续积分计算出的角速率增量的测量值来计算出来的 就随后的举例而言 角速率模型被 近似为一个常数加一个实时线性累积值 其常数和梯度是由现有的和以前的值计算出 l 来的 此算法的一个更为复杂的形式可能是在假设的角速率模型中包含有一个时变平方项 用现有的 过去的以及过去之过去的值判定系数 近来在这一领域进行的工作是通过 l 利用到时间间隔内角速率传感器的测量值来计算角速率模型系数 参见文献 19 提 1l t l t 出的一个用于强化单速算法的技术扩展 从而将第三个循环速率加到整个姿态修正过程 的结构中 姿态 修正和锥化 修正 正如至此所作的讨论 以及锥化修正的传感器 L B C l 采样 后一种技术在改进时采用一个一般的角速率模型 该角速率模型是依据其对 42 式 中积分项的影响直接定义为到时间间隔内连续积分角速率增量传感器采样值之间的加 1l t l t 权叉乘之和 类似于文献 19 介绍的到时间间隔内的方法 在后一种情况下的加 1m t m t 权系数于是得到优化并可以获取纯锥化环境下的最佳平均性能 亦即在幅度上为常数 B IB 但在旋转 之后提到的种种设计方法中的每一种都是以根据假定的角速率模型形状使用曲 面拟合技术为基础的 每一种的最终算法在不是为某个速率环境而设计时和存在有角速率 传感器量化噪声的情况下会表现不同 更为可取的算法的选择 务必包括仿真分析以确认 运行速率环境下和传感器噪声特性状态下是否可以接受 现通过算法举倒对这一部分进行总结 这儿举出的是 42 式积分项算法的例子 它的机 理是通过对幂级数进行截尾展开 使机体速率项被近似到一阶 B IB 和 常数 43 1 B IBl tt A A 参考文献 9 11 和 12 的第7 1 1 1 1表明对于时间段到的 43 式运动有 2l t l t 44 1 1 1 12 l l t B IBll t tdt 将 44 式代入 42 式 可得 45 11 11 26 llll 45 式从类别上可归为求的二阶算法 这是因为它在方程中包含有现在的和过去的 m l 循环乘积 从导出 44 式的分析可以看出 中的 和乘积项即项是由l l l1l 1 6 到时间间隔内线性斜坡角速率的近似化得来的 如果角速率近似化成时间的二次曲线 l t 2l t 变化函数 那么 势必导致包含 和的乘积的三阶算法 如果角速率被近l1l 2l 似成到整个时间间隔内的一个常数 那么 45 式中的项势必导致求的一 1l t l t l 1 6 m 阶算法 最后 如果角速率正在慢慢变化 那么 我们可以将近似为等于零 换言之 m 或更准确地说 我们可以让 循环速率等于循环速率 这使等于用 45 式在时lm m m t 刻计算出来的 注意根据 41 式初始状态定义为零 后一种算法在参考文献 l 1l 4 得到了改进 须注意 要设定 和速率相等 还可以通过增加速率使之与 速率lmml 相匹配的办法实现 结果得到一个单一的高速高阶算法 该算法附带有一个比两速算法更 为简单的软件结构 但是 需要更大的吞吐量 现代计算机速度的不断提高使这一算法在 未来成为更为可取的算法 35 式和的全部数字算法是由 41 42 和 45 式合并后得出的结果所决定的 m m 1 l l t l t d 1lll 当时 mllm tt 1m tt 0 l 46 11 11 26 llll 1lll 当时 mllm tt 1m tt 0 l 47 式中 微积分角速率增量 即 捷联角速率传感器给出的脉冲输出的解析表达式 即d B IBdt 由角速率传感器给出的积分角速率输出增量之和 l 4 1 2 当地水平系的旋转 24 式采用式来修正姿态方向余弦矩阵 以考虑本地水平坐标系系相对于 1 In In L L C L B CL 非旋转空间的角速率 的推导与第4 1 1节的推导非常类似 的正式 L IL 1 In In L L C 1 Im Im B B C 1 In In L L C 定义是 1 1 1 n II nt II nn n t LL LL t CICdt 48 式中 是到时间间隔内任意时刻的系姿态 L t 1n t n tL 矩阵也可以用定义的系的姿态相对于系的旋转矢量来表示 对 4 1 In In L L C n I L 1 n I L 式进行泰勒级数展开可得系数项 1 2 sin 1 cos In In L nn Lnnn nn CI 24 sin 1 3 5 nnn n 49 24 2 1 cos 1 2 4 6 nnn n 式中是定义时刻系的姿态相对于时刻系的旋转矢量 注意 在 49 式中 n n t n I L 1n t 1 n I L 项的符号与 26 式表达式的同类项的符号相反 是负的 这是因为 sin n n n 1 Im Im B B C 与 或 4 式 的指向相反 的矢量变换是从到 而 1 In In L L C 1 Im Im B B C 2 1 A A C 1 In In L L C 1 n I L n I L 的矢量变换是从到 这样 49 式中的形式便是 26 式中 1 Im Im B B C m I B 1 m I B 1 In In L L C 表达式的转置 1 Im Im B B C 因为到的修正周期相对较短 在数值上将会很小 又因为小且在整个 1n t n t n L IL 到的典型修正周期内变化缓慢 由于该时间周期内的矢量速度和位置变化小 所以 1n t n t 系速率矢量可以近似为非旋转 结果可以像旋转矢量方程 1O 的简化形式一样计算L L IL 49 这里的叉乘项被忽略不计 1 n n t L nIL t dt 50 顺便指出 鉴于是个小量 如已讨论过的那样 用于的 49 式亦可简化 例如 n 1 In In L L C 可从 49 式导出一个二阶形式 1 1 2 In In L Lnnn CI 51 对于现行的现代计算机技术而言 采用解算的 49 式简化形式 如 51 式 的计算 1 In In L L C 机在内存或吞吐量方面的优点与增加软件有效性或文件复杂性以及精确度上损失相比较 是微不足道的 精确度损失在导航过程中一般来说是很小的 然而 在初始对准运算过程 中 惯性导航启动之前 这种精度损失不可忽视 初始对准运算时矩阵常被用于对 1 In In L L C 进行倾斜修正 参考文献 12 第6 1 2节和第15节l20 121页 对的初始倾斜对 L B C L B C 准修正量可能会很大 如0 1 1度 如果对 49 式采用过于简化的形式 在初始对准过程中 就可能产生不希望的误差 初始对准运算的闭环伺服运动最终会修正由产生的结果 L B C L B C 性姿态误差 然而 它可能会将冗余正交误差留在行 和列 里面 其结果一定是需要 L B C 包括一个正交或归一化修正算法 见第4 1 3节 作为修正过程的外层循环 L B C 50 式积分的离散数字算法可以通过下述方法建立起来 即第一步先将 13 式和 n 15 式合并 求出被积函数 然后进行如下近似计算 L IL 52 1 21 21 2 nnn LLNNNN ILNIEZNZNCZN CuFuv 式中 下标是到的中途 的值 将 52 式代入 5O 式 然后可得 1 2 n 1n t n t 1 21 21 2 nnn j LNNNN nNIEnZNZNnCZNm CTu TFuR 53 用 14 式估算且 N IE 1 m m t NN m t Rv dt 54 式中 是计算机循环修正周期 而是在到的计算机修正循环内循环 n T 1nn tt j 1n t n tnm 的循环次数 53 式下标为的项都是位置的函数 它 见下篇参考文献 13 在以循环速率 1 2 n n 进行的姿态修正之后得到更新 因此 要计算 52 式中的这些项 必须在以前计算的 参数的基础上 采用一个近似外推公式 例如 采用最后两次 的计算值的线性外推公 式 将变为 1 211212 131 222 nnnnnn 55 在下篇 参考文献 13 中 我们发现速度更新紧跟着位置更新 因此 现在的和过去 N v 的循环的数值可用于估算 54 式中的积分 对 54 式进行梯形积分可得 m N m R 1 1 2 NNN mmmm RvvT 56 式中是计算机循环的更新周期 m Tm 1mm tt 下篇 参考文献 13 还开发了一种具有高分辩能力的算法 用于精密位置修 N m R 正 它考虑了从到循环修正时间间隔内的动态角速率和加速度变化 1m m 4 1 3 归一化和正交化 根据第2 2节的基本定义的列 和行 代表正交单位矢量 因此 它们在模值上等于 L B C 1 归一化条件 且彼此相互正交 正交化条件 更新算法中 除了已描述过的基本 L B C 修正算法外 也常常包括一个归一化和正交化算法以保证的各行和各列正交 导致 L B C L B C 正交误差的因素包括正交初始化误差 软件编程误差 由于计算机字长小于用于 L B C L B C 算法更新周期所希望的全部位数而引起的舍入误差 以及 26 和 49 式泰勒级数展开 L B C 中所选项数太少而引起的舍位误差 重要的一点是要注意到 参考文献 12 第3 4 1节 归一化和正交化误差只是由于 23 24 26 及 49 式的软件运行过程中出现的误差所 引起的 而不是由于供这些方程的算法误差或是由惯性传感器输入误差所引起 积分 L B C 算法软件的总体设计或校正过程必须保证 在预想的导航时间周期内和角速率环境条件下 编程无误差并且舍入误差和舍位误差在可以接受的范围内 这是一个用现代计算机技术或 软件开发技术可轻易实现的目标 不过 将正交化或归一化修正算法包括进去是许多 L B C 捷联惯导软件包一直采用的传统作法 目的在于提高精度 并放宽不许运算中出现任何正 交误差这一严格要求 用于归一化正交的算法是以下面这一特性为基础的 即方向余弦矩 阵的转置矩阵等于其逆矩阵 见2 2节 因此 与其转置矩阵之乘积为单位矩阵 这一 L B C 状态的改变 可用来测量正交误差 而该误差在迭代运算中可被控制算法修正 参考文献 9 12 第7 1 1 3节 15 216 218页 4 2 姿态四元数 设计姿态四元数的修正算法的目的是为了使在各姿态修正时刻得到的数字结果跟 12 式 L B q 表达式的正规连续积分在同一时刻瞬间得到的数字结果相同 姿态四元数修正算法 L B q L B q 是严格按照本论文第4 1节用修正算法推导的同样过程开发出来的 因此 采用 7 式 L B C 姿态四元数链式规律可以写出下式 1 1 1 1 III nnm III mmm LLB BBB qqq 57 58 1 1 III nnn III mnm LLL BLB qqq 式中 时刻的系到时刻的系的四元数 1 1 In Im L B q 1m t B 1n t L L B q 时刻的系到时刻的系的四元数 In Im L B q m tB n tL L B q 考虑了系相对惯性空间旋转的从时刻的方向到时刻的方向的 1 Im Im B B q B 1m t m t 姿态四元数 考虑了系相对惯性空间旋转的从时刻的方向到时刻的方向的姿态四元 1 In In L L q L 1n t n t 数 的修正是由 57 式和 58 式中和完成的 两个算法分别推导如下 L B q 1 Im Im B B q 1 In In L L q 4 2 1 机体系旋转 57 式用修正姿态四元数以考虑捷联传感器 机体 系相对于非旋转空间 1 Im Im B B q L B qB 的旋转角速率 的正式定义为 B IB 1 Im Im B B q 59 1 1 1 1 m II mm It m m Bt B BB t qqqdt 式中是到时间间隔中任意时刻系的姿态 B t 1m t m tB 姿态四元数也可以用一个旋转矢量的方式来表达 该旋转矢量定义系与 1 Im Im B B q m I B 系之间的关系 对 5 式进行泰勒级数展开可得如下各系数项 1 m I B 1 cos0 5 sin0 5 0 5 0 5 Im Im m B Bm m m q 60 24 sin0 5 0 5 0 5 1 0 53 5 mmm m 24 0 5 0 5 cos0 51 2 4 mm 60 式用姿态四元数修正旋转矢量与在4 1 1节用方向余弦矩阵修正是一致 m L B C m 的 而且是用由 35 41 和 42 或 35 46 和 47 式提供的相同算法计算出来的 4 2 2 当地水平坐标系 系 旋转L 58 式修正姿态四元数的方法是利用 考虑当地水平坐标系L系相对非旋转空 L B q 1 In In L L q 间的角速率与二者的关系来实现的 的正式定义是 L IL 1 In In L L q 1 In In L L q 1 1 1 1 n In t II nn n t L L LL t qqqdt 61 61 式中的代表到时间间隔内任一时间的系姿态 L t 1n t n tL 姿态四元数也可以用旋转矢量的方式来表达 该旋转矢量定义为系的姿态 1 In In L L q n I L 相对于系的关系 对 5 式进行泰勒级数展开可得如下积分项 1 n I L 1 cos0 5 sin0 5 0 5 0 5 In In n L Ln n n q 62 24 sin0 5 0 5 0 5 1 0 53 5 nnn n 24 0 5 0 5 cos0 51 2 4 nn n 式中的负号考虑了的反相指向 它描述了系姿态相对系的关系 这与旋 n 1 In In L L q 1 n I L n I L 转矢量的指向不同 描述了系姿态相对系的关系 62 式旋转矢量与用于方向 n n n I L 1 n I L n 余弦矩阵修正的是一致的 且是用第4 1 2节中描述的且由 53 55 和 56 式提供的相同 L B C n 的方法计算出来的 在精度上可与 51 式方向余弦修正相媲美的 62 式的近似形式可以很轻易地通过代入 和舍位求得 63 1 2 1 0 5 0 5 0 5 In In L n L n q 第4 1 2节中关于使用简化的 51 式方向余弦当地水平系修正算法的合理性的意见也适用于 63 式姿态四元数修正算法 但不太适用于完整的 62 式那种形式 4 2 3 归一化 为了保持第2 2节中所讨论过的基本姿态四元数归一化的特性 在姿态四元数修正 L B q 过程中往往包含一个归一化算法 作为该修正过程中的一个外环函数 在第4 1 3节中有关 方向余弦矩阵需要归一化或正交化的讨论同样适用于姿态四元数 唯一的例外是正交化在 定义四元数上毫无意义 如同定义姿态方向余弦矩阵那样 因此 在第4 1 3节中的正交 化讨论在此不适用 如果决定要采用四元数正交化算法 那么该算法是基于使用一个控制 算法将的模值与1相比较 而且该模值与1的差值可利用迭代的控制算法进行修正 参考 L B q 文献 9 12 第7 1 1 3节 15 216 218页 5 姿态积分算法总结 表I汇总了介绍过的捷联惯导姿态积分函数的种种算法 它是按照导航计算机中运算执 行次序列出的 表1列出算法函数 输入参数 输出参数及方程序号数 6 算法及运行速率选择 面对可供选择的大量捷联惯导算法 软件设计人员终归要选择一套适于自己使用的算 法 把它掌握在手 本论文上篇和续篇中介绍的算法只是多年来好几位作者发表了的很多 类似算法中的一