捷联惯性导航系统初始对准原理

1 第二章 捷联惯导系统的初试对准 2 1 引言 惯导系统是一种自主式导航系统 它不需要任何人为的外部信息 只要给 定导航的初始条件 例如初始速度 位置等 便可根据系统中的惯性敏感元件 测量的比力和角速率通过计算机实时地计算出各种导航参数 由于 平台 是 测量比力的基准 因此 平台 的初始对准就非常重要 对于平台惯导系统 初试对准的任务就是要将平台调整在给定的导航坐标系的方向上 若采用游动 方位系统 则需要将平台调水平 称为水平对准 并将平台的方位角调至某个 方位角处 称为方位对准 对于捷联惯导系统 由于捷联矩阵起到了平台的 T 作用 因此导航工作一开始就需要获得捷联矩阵的初始值 以便完成导航的 T 任务 显然捷联惯导系统的初始对准就是确定捷联矩阵的初始值 在静基座条 件下 捷联惯导系统的加速度计的输入量为 陀螺的输入量为地球自转 b g 角速率 因此与就成为初始对准的基准 将陀螺与加速度计的输入引 b ie b g b ie 出计算机 通过计算机就可以计算出捷联矩阵的初始值 T 由以上的分析可以看出 陀螺与加速度计的误差会导致对准误差 对准飞 行器的干扰运动也是产生对准误差的重要因素 因此滤波技术对捷联系统尤其 重要 由于初始对准的误差将会对捷联惯导系统的工作造成难以消除的影响 因此研究初始对准的误差传播方程也是非常必要的 2 2 捷联惯导系统的基本工作原理 捷联式惯性导航系统 陀螺仪和加速度计直接与载体固联 加速度计测量 是载体坐标系轴向比力 只要把这个比力转换到导航坐标系上 则其它计算就 与平台式惯性导航系统一样 而比力转换的关键就是要实时地进行姿态基准计 算来提供数学平台 即实时更新姿态矩阵 姿态矩阵也称为捷联矩阵 一般 n b C 选择地理坐标系为导航坐标系 那么捷联矩阵也可表示为 其导航原理 n b C t b C 图如图 2 1 所示 由惯导系统的工作原理可以看出 捷联式惯性导航系统有以下几个主要优 2 点 1 惯性敏感器便于安装 维修和更换 2 惯性敏感器可以直接给出舰船坐 标系轴向的线加速度 线速度 供给舰船稳定控制系统和武备控制系统 角速 度以提供给舰船稳定控制系统和武 备控制系统 3 便于将惯性敏感器重复布 置 从而易在惯性敏感器的级别上实现冗余 技术 这对提高系统的性能和可靠 性十分有利 4 由于去掉了具有常平架的平台 一则消除了稳定平台稳定过程 中的各 种误差 二则由于不存在机电结合的常平架装置 使整个系统可以做得 小而 轻 并易于维护 当然 由于惯性敏感器直接固接于船体上也带来新的 问题 即导致惯性 敏感器的工作环境恶化了 由于惯性敏感器直接承受舰船的 振动 冲击及温 度波动等环境条件 惯性敏感器的输出信息将会产生严重的动 态误差 为保 证惯性敏感器的参数和性能有很高的稳定性 则要求在系统中必 须对惯性敏 感器采取误差补偿措施 2 3 常用坐标系与捷联矩阵 2 3 1 常用坐标系 惯性导航中常用的坐标系有以下几种 1 地心惯性坐标系 下标为 i eiii O x y z 惯性坐标系是符合牛顿力学定律的坐标系 即是绝对静止或只做匀速直线 运动的坐标系 由于宇宙空间中的万物都处于运动之中 因此想寻找绝对的惯 性坐标系是不可能的 我们只能根据导航的需要来选取惯性坐标系 对于在地 球附近运动的飞行器选取地心惯性坐标系是合适的 地心惯性坐标系不考虑地 球绕太阳的公转运动 当然更略去了太阳相对于宇宙空间的运动 地心惯性坐 标系的原点选在地球的中心 它不参与地球的自转 惯性坐标系是惯性敏感 e O 元件测量的基准 由于在进行导航计算时无需再这个坐标系中分解任何向量 因此惯性坐标系的坐标轴的定向本无关紧要 但习惯上我们可以将轴选在沿 i z 地轴指向北极的方向上 而 轴则在地球的赤道平面内 并指向空间的两 i x i y 颗恒星 3 2 地球坐标系 下标为 e eeee O x y z 地球坐标系是固连在在地球上的坐标系 它相对于惯性坐标系以地球自转 角速率旋转 地球坐标系的原点在地球中心 轴 e 15 04107 e h e O ee O z 与轴重合 在赤道平面内 轴指向格林威治经线 轴指向东经 ei O z eee O x y e x e y 方向 90 3 地理坐标系 下标为 t ttt Ox y z 地理坐标系是在飞行器上用来表示飞行器所在位置的东向 北向和垂线方 向的坐标系 地理坐标系的原点选在飞行器的重心处 指向东 指向北 O t x t y 沿垂线方向指向天 东北天 对于地理坐标系 在不同的惯导文献中往往有 t z 不同的取法 所不同之处仅在于坐标轴的正向的指向不同 如还有北西天 北 东地等取法 坐标轴指向不同于仅使向量在坐标系中取投影分量时的正负号有 所不同 并不影响导航基本原理的阐述及导航参数计算结果的正确性 4 导航坐标系 下标为 n nnn Ox y z 导航坐标系是在导航时根据导航系统工作的需要而选取的作为导航基准的 坐标系 当把导航坐标系选得与地理坐标系相重合时 可将这种导航坐标系称 为指北方位系统 为了适应在极区附近导航的需要往往将导航坐标系的轴仍 n z 选得与重合 而使与及与之间相差一个自由方位角或游动方位角 t z n x t x n y t y 这种导航坐标系可称为自由方位系统或游动自由方位系统 5 平台坐标系 下标为 p ppp Ox y z 平台坐标系是用惯导系统来复现导航坐标系时所获得的坐标系 平台坐标 系的坐标原点位于飞行器的重心处 当惯导系统不存在误差时 平台坐标系 O 与导航坐标系相重合 当惯导系统出现误差时 平台坐标系就要相对导航坐标 系出现误差角 对于平台惯导系统 平台坐标系是通过平台台体来实现的 对 于捷联惯导系统 平台坐标系则是通过存储在计算机中的方向余弦矩阵来实现 4 的 因此又叫做 数学平台 对于平台惯导系统 平台坐标系与导航坐标系之 间的误差是由平台的加工 装配工艺不完善 敏感元件误差以及初始对准误差 等因素造成的 而对于捷联惯导系统 该误差则是由算法误差 敏感元件误差 以及初始对准误差造成的 6 机体坐标系 下标为 b bbb Ox y z 机体坐标系是固连在机体上的坐标系 机体坐标系的坐标原点位于飞行 O 器的重心处 沿机体横轴指向右 沿机体纵轴指向前 垂直于 b x b y b z bb Ox y 并沿飞行器的数轴指向上 2 3 2 捷联矩阵的定义 对于捷联惯导系统 加速度计时沿机体坐标系安装的 它只能测量 bbb Ox y z 沿机体坐标系的比力分量 因此需要将 转换为 b x f b y f b z f b x f b y f b z f p x f 实现由机体坐标系到平台坐标系的坐标转换的方向余弦矩阵又叫 p y f p z f p b C 做捷联矩阵 本章用来表示 由于根据捷联矩阵的元素可以单值地确定飞行 T 器的姿态角 因此又可以叫做飞行器状态矩阵 由于捷联矩阵起到了平台的作 用 借助于它可以获得 所以又可以叫做 数学平台 p x f p y f p z f 设机体坐标系固连在机体上 其 轴分别沿飞机的 bbb Ox y z b Ox b Oy b Oz 横轴 纵轴与竖轴 实现由机体坐标系到平台坐标系的坐标转换的捷联矩阵 应满足如下的矩阵方程 T 3 p b pb b p x x yT y z z 1 式中 5 3 2 111213 212223 313233 TTT TTTT TTT 当矩阵求得后 沿机体坐标系测量的比力就可以转换到平台坐标系上 T b f 得到 p f 由平台坐标系至机体坐标系的转换关系 可以通过下述顺序的三种旋转来 表示 ppp zxy pppppppppbbb x y zx y zx y zx y z 绕轴绕轴绕轴 其中 分别为机体的航向角 俯仰角和倾斜角 根据上述的旋转顺序 可以得到由平台坐标系到机体坐标系的转换关系 即 cos0sin100cossin0 0100cossinsincos0 sin0cos0sincos001 cos cossinsinsincos sinsinsincossincos cos sincoscossi p b bp b p x x yy z z n sincoscos sinsinsinsincos sincoscos cos p p p x y z 3 3 由式 3 1 可得 1 p b bp b p x x yTy z z 3 4 考虑到捷联矩阵为正交矩阵 表示转置 于是 T 1t TT t cos cossinsinsincos sinsincoscos sinsin cos sinsinsincoscoscossinsincos sincos sincossincos cos T 6 3 5 2 3 姿态的描述及更新算法 物体 刚体 的姿态是物体相对观察者的几何角度关系的统称 姿态的数 学描述是运动体进行姿态运动建模的基础 一般来讲 刚体的运动要用刚体位 置移动和绕自身重心的转动来描述 其位置移动可用船体上下起伏 heave 前后涌动 surge 左右晃动 sway 来描述 而转动则要用纵摇 pitch 横摇 roll 和偏航 yaw 来描述 纵摇是用来描述船在穿过连续的浪头和 波谷时 船头的船尾忽上忽下而引起的船体绕横摇轴转动的振荡运动 横摇则 是用来描述船体绕纵轴转动的振荡运动 因此 舰船和飞机一类的运载体的姿 态参数就是其纵摇角 横摇角和航向角 为了描述载体的姿态 至少需要建立两个坐标系 即空间参考坐标系 Reference Coordinate 和固连于载体的本体坐标系 Body Reference 载 体坐标系的三个坐标轴在参考坐标系中的方向确定了载体的姿态 描述这些方 向的物理量就称为姿态参数 姿态参数有多种形式 最常用的如欧拉角 四元 数等 采用不同的姿态参数 可以构成不同的坐标转换矩阵 称为姿态矩阵 因为载体姿态是唯一确定的 因此各种姿态参数之间可以互相转换 2 3 1 欧拉角 欧拉角 Euler Angles 发源于欧拉定理 即刚体绕固定点的角位移可以 由绕该点的若干次有限转动合成 因此 可将参考坐标系绕不同坐标轴连续旋 转三次得到载体坐标系 每次的旋转轴取为被旋转坐标系的某一轴 每次的旋 转角就称为欧拉角 姿态矩阵与三次转动的顺序有关 但描述三轴控制的飞机 导弹 舰船等方面 一般采用非对称旋转 由坐标系的定义三个欧拉角的几何意义为 纵摇角 载体绕纵摇轴转动的角度 p b X 横摇角 载体绕横摇轴转动的角度 b Y 7 航向角 载体绕航向轴转动的角度 b Z 根据转动规律得到姿态矩阵 coscossinsinsincossinsinsincossincos cossincoscossin sincoscossinsinsinsincossincoscoscos ppp n bppp ppp C 5 14 111213 212223 313233 TTT TTT TTT 由 5 14 式可得 5 15 1 23 sin T 1 13 33 T tg T 1 21 22 T tg T 纵摇角定义在区间 和反正弦函数主值一样 不存在多值问 00 90 90 题 横摇角定义在区间 航向角定义在区间 都 00 180 180 00 0 360 存在多值问题 求姿态角时应设法判断和的真值 以确定载体落在哪一个 象限 欧拉角是经典的 应用最为广泛的一种姿态参数 具有直接 明显的几何 意义 维数最小 三维 并且往往可以由姿态敏感器直接测量 在某些应用方 面 欧拉角描述法具有明显的优势 但是 由于方向余弦矩阵具有九个元素 所以 解算矩阵微分方程时 实际上解算九个联立微分方程 一般说来 计算 量比较大 此外 它还有一个固有缺陷 即对于某一个角度存在奇异问题 不 同的旋转顺序 奇异角度也不同 因此 要准确描述任意姿态 至少需要两组 欧拉角 为了解决这些问题 可以采用四元数法 下面介绍四元数法 2 3 2 四元数 1 四元数定义及其姿态矩阵 设坐标系绕某轴 ON 转动某个角度 就与坐标系 aaaa S Ox y z 重合 如图 5 2 所示 bbbb S Ox y z 8 图 5 2 四元数示意图 定义如下四个元素 5 16 0 11 22 33 cos 2 sin 2 sin 2 sin 2 q qC qC qC 其中 分别为旋转欧拉轴 ON 在坐标系中的方向余弦 123 C C C aaaa S Ox y z 显然 用这四个元素就可以完全描述这两个坐标系之间的关系 则 03 qq 定义四元数为 5 17 01230 qqqqqq qijk 四元数满足以下的约束条件 a z N n b y a y b x a x b z 9 5 18 2222 0123 1qqqq 所以 四元数四个变量之中只有三个是独立的 根据欧拉旋转公式 5 14 及四元数定义 四元数姿态矩阵可表示为 2 00 T b Rqqq IqR 5 19 22 0112031302 22 1203022301000 22 1302230103 2 12 2 2 2 12 2 2 2 1 b qqq qq qq qq q q qq qqqq qq qRTR q qq qq qq qqq q 为导航坐标系与载体坐标系之间的坐标转换矩阵 0 b Tq 注意到四元数姿态矩阵是由四元数的二项式组成 因此对于四元数 q 也可以得到 5 19 所示的姿态矩阵 四元数的这种非唯一性 0 TT qq q 是与欧拉角的非唯一性相一致的 物理意义上这两者实际代表同一种姿态 同 一种旋转 并无区别 一般我们取四元数正值进行计算 上述分析说明 如果表征 n 系到 b 系的旋转四元数 Q 已经确定 那么就可 以确定出运载体的航向角 俯仰角和横滚角 因此 四元数 Q 包含了所有的姿 态信息 捷联惯导中的姿态更新实质上是如何计算四元数 Q 2 3 3 四元数与欧拉角的关系 四元数和欧拉角的关系可由四元数的乘积公式求出 欧拉角载体坐标系按 三次旋转到导航坐标系 则每次旋转所对应的四元数分别为 1 cossin 22 qk 5 20 2 cossin 22 qi 3 cossin 22 qj 根据四元数的定义及乘法公式 可得 5 21 0123b qqqq 10 因此可解出 5 22 0 1 2 3 coscoscossinsinsin 222222 cossincossincossin 222222 coscossinsinsincos 222222 cossinsinsincoscos 222222 q q q q 或 5 23 1 3210 1 1320 22 03 1 1230 22 02 sin 2 2 2 1 2 2 1 q qq q q qq q tg qq q qq q tg qq 与欧拉角相比 四元数避免了奇异问题 没有复杂的三角运算 更有利于 存储及计算 但由于计算误差等 较难保证四元数的规范化条件 因此往往需 要重新规范化或降阶 2 3 4 姿态更新的四元数算法 设由运载体的机体轴确定的坐标系为 b 惯导系统所采用的导航坐标系为 n 则由 b 系到 n 系的坐标变换矩阵称为运载体的姿态矩阵 姿态更新是指 n b C 根据惯性器件的输出实时计算出矩阵 由于 n 系和 b 系均为直角坐标系 各 n b C 轴之间始终保持直角 所以可将坐标系理解成刚体 当只研究两个坐标系间的 角位置关系时 可对一个坐标系作平移 使其原点与另一个坐标系的原点重合 因此 两坐标系间的空间角位置可理解成刚体的定点转动 从这一基本思想出 发 可获得姿态更新的四元数算法及旋转矢量算法 11 一 四元数微分方程 假设表征 n 系至 b 系的旋转四元数为 1 cos sin 22 R Qu 对 1 式求导可得 2 1 2 R Rb d dt Q Q 刚体绕旋转 u RR Rb u 在捷联惯导系统中 角速度信息是捷联陀螺提供的 而捷联陀螺是在机体 坐标系内测量的 所以 2 式中的还需换成 根据四元数向量之间的 R Rb b Rb 变换关系 可得 Rb rQrQ 3 Rb RbRb QQ 将 3 式代入 2 式可得 4 11 22 bb RbRb d dt Q QQQ Q 记 5 x b Rby z 即得到 6 00 11 22 33 0 0 1 02 0 xyz xzy yzx zyx qq qq qq qq 12 其中 的获取按照下式进行 b nb 7 bbbnn nbibnieen C 式中 是捷联陀螺的输出 由姿态更新的最新值确定 和分别是未知 b ib b n C n en n ie 速率和地球自转速率 对于导航坐标系取地理坐标系的情况有 8 cos sintan N M nn E ieenie N E ie N V R V L R V LL R 式中 为导航计算所得的最新值 分别是北向速度 东向速度 纬度 N V E V L 为地球沿卯酉圈的曲率半径 为地球子午圈的曲率半径 在不精确计算的情况下 N R M R 也可以视地球为圆球形状 则 8 式中 可用地球半径代替 N R M R e R 二 四元数的初值确定和规范化处理 四元数的初值由捷联惯导的初始对准确定 设初始对准确定的姿态阵 0 Q 为 根据式 5 19 及描述刚体旋转的四元数为规范化四元数的结论 有如下 n bij CT 方程成立 9 2222 012311 2222 012322 2222 012333 2222 0123 120312 130213 120321 230123 130231 231032 1 2 2 2 2 2 2 qqqqT qqqqT qqqqT qqqq q qq qT q qq qT q qq qT q qq qT q qq qT q qq qT 13 从上述方程解得 10 0112233 103223 201331 302112 1 1 2 4 4 4 qTTT q qTT q qTT q qTT 的符号可以任选 0 q 由于表征旋转的四元数是规范化四元数 即 但是由于计算误差等 1 Q 因素 计算过程中四元数会逐渐失去规范化特性 因此若干次更新后 必须对 四元数做规范化处理 2222 0123 i i q q qqqq 0 1 2 3i 其中 是四元数更新所得的值 0 q 1 q 2 q 3 q 2 4 捷联惯导系统的误差模型 对于工作在非极区的捷联惯导系统 为了简化计算 导航坐标系一般选取地理坐标系 这样 捷联惯导完全等效于指北方位系统 由于陀螺在捷联系统中起测量器件作用 陀螺 漂移引起的数学漂移与陀螺漂移的方向相反 刻度系数误差引起对运载体角速度的测量误 差 经姿态更新计算引入系统 2 4 1 速度误差方程和位置误差方程 根据比力方程 当不考虑任何误差时 速度的理想值由下式确定 3 6 1 2 nbnnn nbieenn C VfVg 而实际系统中总存在各种误差 所以实际的速度计算值应由下述方程确定 3 6 2 2 nbcccc cbieen C VfVg 14 式中 0 0 0 0 0 0 cnn cnn ieieie cnn enenen cn nnnnn bnbb bbb A UN n UE NE AAxAyAz zy zx yx CC CC KA KdiagKKK AA AAA AA VVV ggg I fIIf 其中 为姿态误差角 和分别为加速度计的刻度 E N U Ai K zi A ix y z 系数和安装误差角 用式 3 6 1 减去式 3 6 2 忽略的影响 并略去二阶小量 得 g 3 6 3 2 2 nnnnb bA nnnnnnn ieenieen CKA Vff VV 当取地理坐标系为导航坐标系时 0 cos sin n ieie ie L L 0 sin cos n ieie ie LL LL 15 tan N M n E en N E N V Rh V Rh V L Rh 2 2 2 2 tan tansec NN MM n EE en NN EEE NNN VV h RhRh VV h RhRh VVVL LLLh RhRhRh 记 111213 212223 313233 n b TTT CTTT TTT 将上述式子代入式 3 6 3 可以得到速度误差方程 16 111213 212223 313233 0 0 0 0 0 0 b xE EUNAxzy b NUENzAyxy b NEyxAz U z UN UE NE fV fTTTKAA VfTTTAKAf TTTAAKf V f VV VV VV U 2 2 2 2cos 2sintan 0 02sin 0 2costansec N M E E ieN N U E ie N NN MM UN EE UEie NN NE EE ie NN V Rh V L Rh V LL Rh VV h RhRh VV VV VVLLh RhRh VV VV LLLLLh RhRh 2 tan E N VL Rh 式 3 6 4 其中 111213 212223 313233 bbb Exyz bbb Nxyz bbb Uxyz TTT TTT TTT 位置误差方程为 3 6 5 2 2 sec sectansec NN MM EEE NNN U VV Lh RhRh VVVL LLLLh RhRhRh hV 2 4 2 姿态误差方程 根据式 2 姿态四元数满足如下微分方程 17 1 2 b nb QQ 其中 姿态速率视为零标量四元数 b nb 如果求取的姿态速率不含任何误差 即 bbb nbibin 则无误差的理想姿态四元数由下式确定 3 6 6 1 2 bb ibin Q Q 但实际系统中 姿态速率由陀螺的输出角速度和对数学平台的指令角速度确定 b ib b in bbb nbibin 其中 指令角速度根据系统解算出的导航解确定 带有一定的误差 所以实际解 b in 算的四元数由下式确定 3 6 7 1 2 bb ibin Q Q 设与相对应的姿态阵为 根据姿态阵与姿态四元数之间的等价