惯性导航原理.ppt

第三章第三章 惯性导航原理惯性导航原理主要捷联式3.1常用坐标系常用坐标系 n n 惯性导航中所采用的坐标系可分为惯性导航中所采用的坐标系可分为惯性坐标系惯性坐标系与与非惯性坐非惯性坐标系标系两类，惯性导航区别于其它类型的导航方案（如无线两类，惯性导航区别于其它类型的导航方案（如无线电导航、天文导航等）的根本不同之处就在于其导航原理电导航、天文导航等）的根本不同之处就在于其导航原理是建立在牛顿力学定律是建立在牛顿力学定律又称惯性定律又称惯性定律的基础上的，的基础上的，“惯性导航惯性导航”也因此而得名。而牛顿力学定律是在惯性空也因此而得名。而牛顿力学定律是在惯性空间内成立的，这就有必要首先引入惯性坐标系，作为讨论间内成立的，这就有必要首先引入惯性坐标系，作为讨论惯导基本原理的坐标基准。对飞行器进行导航的主要目的惯导基本原理的坐标基准。对飞行器进行导航的主要目的就是要确定其导航参数，飞行器的导航参数就是通过各个就是要确定其导航参数，飞行器的导航参数就是通过各个坐标系之间的关系来确定的，这些坐标系是区别于惯性坐坐标系之间的关系来确定的，这些坐标系是区别于惯性坐标系、并根据导航的需要来选取的。将它们统称为非惯性标系、并根据导航的需要来选取的。将它们统称为非惯性坐标系，如地球坐标系、地理坐标系、导航坐标系、平台坐标系，如地球坐标系、地理坐标系、导航坐标系、平台坐标系及机体坐标系等。坐标系及机体坐标系等。n n在惯性导航中常用的坐标系有在惯性导航中常用的坐标系有n n1.1.地心惯性坐标系地心惯性坐标系 n n 地心惯性坐标系不考虑地球绕太阳的公转运地心惯性坐标系不考虑地球绕太阳的公转运动，地心惯性坐标系的原点选在地球的中心，它动，地心惯性坐标系的原点选在地球的中心，它不参与地球的自转。惯性坐标系是惯性敏感元件不参与地球的自转。惯性坐标系是惯性敏感元件测量的基准，在导航计算时无需在这个坐标系中测量的基准，在导航计算时无需在这个坐标系中分解任何向量，因此惯性坐标系的坐标轴的定向分解任何向量，因此惯性坐标系的坐标轴的定向无关紧要，但习惯上将无关紧要，但习惯上将z z轴选在沿地轴指向北极的轴选在沿地轴指向北极的方向上，而方向上，而x x、y y轴则在地球的赤道平面内，并指轴则在地球的赤道平面内，并指向空间的两颗恒星。向空间的两颗恒星。n n2.2.地球坐标系地球坐标系 n n地球坐标系是固连在地球上的坐标系，它相对惯地球坐标系是固连在地球上的坐标系，它相对惯性坐标系以地球自转角速率性坐标系以地球自转角速率 旋转，地球坐标系的旋转，地球坐标系的原点在地球中心，原点在地球中心，轴与轴与 轴重合，轴重合，在赤道平面在赤道平面内，内，x x轴指向格林威治经线，轴指向格林威治经线，y y轴指向东经轴指向东经9090度方度方向。向。n n3.地理坐标系 n n 地理坐标系是在飞行器上用来表示飞行器所在位置的东向、北向和垂线方向的坐标系。地理坐标系的原点选在飞行器重心处，x指向东，y指向北，z沿垂线方向指向天（东北天）。n n4.导航坐标系n n 导航坐标系是在导航时根据导航系统工作的需要而选取的作为导航基准的坐标系。指北方位系统：导航坐标系与地理坐标系重合；自由方位系统或游动自由方位系统：轴与 轴重合，而 与 及 与 之间相差一个自由方位角或游动方位角 。n n5.平台坐标系n n 平台坐标系是用惯导系统来复现导航坐标系时所获得的坐标系，平台坐标系的坐标原点位于飞行器的重心处。对于平台惯导系统，平台坐标系是通过平台台体来实现的；对于捷联惯导系统，平台坐标系是通过存储在计算机中的方向余弦矩阵来实现的。n n6.机体坐标系n n机体坐标系是固连在机体上的坐标系。机体坐标系的坐标原点o位于飞行器的重心处，x沿机体横轴指向右，y沿机体纵轴指向前，z垂直于oxy，并沿飞行器的竖轴指向上。3.2四元数理论四元数理论四元数 表示 四元数：描述刚体角运动的数学工具四元数：描述刚体角运动的数学工具 (quaternions)针对捷联惯导系统，可弥补欧拉参数在描述和解算方面的不足。针对捷联惯导系统，可弥补欧拉参数在描述和解算方面的不足。四元数的表示四元数的表示由一个实单位和三个虚数单位由一个实单位和三个虚数单位 i,j,k 组成的数组成的数 或者省略或者省略 1，写成，写成i,j,k 服从如下运算公式：服从如下运算公式：四元数 组成部分i,j,k 服从如下运算公式服从如下运算公式 称作标量部分，称作标量部分，称作矢量部分称作矢量部分 四元数的另一种表示法四元数的另一种表示法 P 泛指矢量部分泛指矢量部分提示：四元数与刚体转动的关系提示：四元数与刚体转动的关系四元数基本性质 加减法1四元数加减法四元数加减法 或简单表示为或简单表示为 四元数基本性质 乘法2四元数乘法四元数乘法 或简单表示为或简单表示为 关于相乘符号关于相乘符号 关于交换律和结合律关于交换律和结合律四元数基本性质 共轭 范数3共轭四元数共轭四元数 仅向量部分符号相反的两个四元数仅向量部分符号相反的两个四元数 和和 互为共轭互为共轭 可证明：可证明：4四元数的范数四元数的范数 定义定义 则称为规范化四元数则称为规范化四元数 四元数基本性质 逆 除法5逆四元数逆四元数 当当 时时 6四元数的除法四元数的除法 若若 则则 若若 则则 不能表示为不能表示为（含义不确切含义不确切）四元数表示转动 约定一个坐标系或矢量相对参考坐标系旋转，一个坐标系或矢量相对参考坐标系旋转，转角为转角为，转轴转轴 n n 与参考系各轴间的方向余弦值为与参考系各轴间的方向余弦值为cos、cos、cos。则表示该旋转的四元数可以写为则表示该旋转的四元数可以写为 为特征四元数为特征四元数（范数为范数为 1）四元数既表示了转轴方向，又表示了转角大小（转动四元数）四元数既表示了转轴方向，又表示了转角大小（转动四元数）四元数表示转动 矢量旋转如果矢量如果矢量 R 相对固定坐标系旋转相对固定坐标系旋转，旋转四元数为，旋转四元数为 q，转动后转动后的矢量为的矢量为 R，则这种转动关系可通过四元数旋转运算来实现则这种转动关系可通过四元数旋转运算来实现含义：矢量含义：矢量 R 相对固定坐标系产生旋转，相对固定坐标系产生旋转，转角和转轴由转角和转轴由 q 决定决定四元数表示转动 坐标系旋转如果如果坐标系坐标系 OXYZ 发生发生 q 旋转，得到新坐标系旋转，得到新坐标系 OXYZ 一个相对原始坐标系一个相对原始坐标系 OXYZ 不发生旋转变换的矢量不发生旋转变换的矢量 V 矢量矢量 V 在新坐标系上在新坐标系上 OXYZ 的投影为的投影为 则不变矢量则不变矢量 V 在两个坐标系上的投影之间存在如下关系：在两个坐标系上的投影之间存在如下关系：式中式中 分别称为矢量分别称为矢量 V 在坐标系在坐标系 OXYZ 和和 OXYZ 上的映像上的映像四元数 映象图解四元数表示转动 方向余弦将该投影变换式展开，也就是把将该投影变换式展开，也就是把代入上述投影变换式代入上述投影变换式进行四元数乘法运算，整理运算结果可得进行四元数乘法运算，整理运算结果可得四元数表示转动 方向余弦其中方向余弦矩阵其中方向余弦矩阵 四元数表示转动 旋转合成多次旋转多次旋转的合成的合成对于一个坐标系经过多次旋转后，新坐标系和原始坐标系之间对于一个坐标系经过多次旋转后，新坐标系和原始坐标系之间的关系等效于一个一次转动的效果，的关系等效于一个一次转动的效果，相应地有合成转动四元数相应地有合成转动四元数 假定假定 q1、q2 分别是第一次转动、第二次转动的四元数分别是第一次转动、第二次转动的四元数 q 是合成转动的四元数，是合成转动的四元数，那么有如下关系成立：那么有如下关系成立：上式中上式中 q1 和和 q2 的转轴方向必须以映象的形式给出。的转轴方向必须以映象的形式给出。如果如果 q1 和和 q2 的转轴方向都以原始坐标系的分量表示，则有的转轴方向都以原始坐标系的分量表示，则有 求方向余弦 非映象方式1用四元数旋转变换的方法求取两个坐标系之间的方向余弦表。用四元数旋转变换的方法求取两个坐标系之间的方向余弦表。坐标系坐标系 OXYZ 相对相对OXYZ 三次旋转，以欧三次旋转，以欧拉角拉角 、的形的形式给出。式给出。第一转，绕第一转，绕 Z 轴转轴转角，瞬时转轴角，瞬时转轴 n 和和 k 轴重合，则转动四元轴重合，则转动四元数为数为 第二转，绕第二转，绕 OX1 轴转轴转角，角，瞬时转轴瞬时转轴 n 的方向表示式为的方向表示式为 其转动四元数为其转动四元数为 求方向余弦 非映象方式2求方向余弦 非映象方式合成由于由于 q1 和和 q2 的瞬时转轴的瞬时转轴都是以同一个坐标系的方向余弦来都是以同一个坐标系的方向余弦来表示，则合成转动四元数表示，则合成转动四元数 q 的计算采用：的计算采用：求方向余弦 映象方式1以瞬时转轴以瞬时转轴映象映象形式给出形式给出转动四元数的表达式并求转动四元数的表达式并求出合成转动四元数出合成转动四元数 第一次转时，映象形式的第一次转时，映象形式的 q1 和非映象形式的和非映象形式的 q1 是是一致的：一致的：求方向余弦 映象方式2第二转绕第二转绕 OX1 轴转轴转 角角瞬时转轴瞬时转轴 n 是由是由 OX 经过经过第一转转换来的第一转转换来的OX 轴对应单位矢量轴对应单位矢量 i，所所以定义以定义 n 的映象为的映象为 i则则 q2 的映象表示式为的映象表示式为 求方向余弦 映象方式3第三转，绕第三转，绕 OZ 轴转动轴转动 角角瞬时转轴瞬时转轴 n 是由是由 OZ 经过经过第一转和第二转转换来的第一转和第二转转换来的OZ 轴对应单位矢量轴对应单位矢量 k，所以定义所以定义 n 的映象为的映象为 k则则 q3 的映象表示式为的映象表示式为求方向余弦 映象合成由于由于 q1、q2 和和 q3 都是映象形式都是映象形式，所以三次转动的合成转动四所以三次转动的合成转动四元数元数 q 为为据此可算出对应的方向余弦表据此可算出对应的方向余弦表 四元数补充 两种转动公式 坐标系旋转时，不变矢量坐标系旋转时，不变矢量 V 在两个坐标系上的投影之间存在在两个坐标系上的投影之间存在如下关系：如下关系：在一些资料中，四元数的转动公式也经常写成如下的形式在一些资料中，四元数的转动公式也经常写成如下的形式 这个公式的意义是说，在一个超复数空间中，或者在一个固这个公式的意义是说，在一个超复数空间中，或者在一个固定坐标系中，矢量定坐标系中，矢量 VE 按着四元数按着四元数 q 所表示的方向和大小转所表示的方向和大小转动了一个角度，得到一个新的矢量动了一个角度，得到一个新的矢量 VE四元数补充 计算上的优点四元数法能得到迅速发展，是由于飞行器控制与导航的发展，要四元数法能得到迅速发展，是由于飞行器控制与导航的发展，要求更合理地描述刚体空间运动，以及便于计算机的应用。求更合理地描述刚体空间运动，以及便于计算机的应用。采用方向余弦矩阵描述飞行器运动时，要积分矩阵微分方程式：采用方向余弦矩阵描述飞行器运动时，要积分矩阵微分方程式：式中式中C为动坐标系转置到定坐标系的方向余弦矩阵，为动坐标系转置到定坐标系的方向余弦矩阵，为动坐为动坐标系相对定坐标系旋转角速度标系相对定坐标系旋转角速度的反对称矩阵：的反对称矩阵：包含包含 9 个一阶微个一阶微分方程式，计算分方程式，计算量比较大量比较大 四元数补充 计算上的优点如果采用四元数法，则是要求解四元数方程式如果采用四元数法，则是要求解四元数方程式 q 为动坐标系的转动四元数，为动坐标系的转动四元数，为动坐标系相对定坐标系为动坐标系相对定坐标系的旋转角速度，也表示为四元数的旋转角速度，也表示为四元数 按四元数乘积展开按四元数乘积展开 只要解四个一阶微分只要解四个一阶微分方程式组方程式组即可即可3.3视加速度和比力视加速度和比力 根据根据质质心运心运动动定理和相定理和相对对运运动动学原理，学原理，飞飞行体行体质质心运心运动动的微分方程（在的微分方程（在惯惯性坐性坐标标系下）系下）为为：式中，式中，-飞飞行体的行体的质质量；量；-推力；推力；-空气阻力；空气阻力；-惯惯性空性空间飞间飞行行时时，导弹质导弹质心加速度；心加速度；，-由推力由推力产产生的加速度；生的加速度；-由阻力引起的阻力加速度。由阻力引起的阻力加速度。由上式可得出由上式可得出 飞飞行体行体质质心运心运动动的微分方程（在的微分方程（在弹弹体坐体坐标标系下）系下）为为：或或 ，式中式中是是动动点的相点的相对对加速度，将（加速度，将（*）代入上式）代入上式 得得 由上式可知，由上式可知，测测得的得的是推力加速度是推力加速度和阻力加速度和阻力加速度的矢量和，称的矢量和，称为视为视加速度，在加速度，在实际实际的的测试测试中由加速度中由加速度传传感器得到的感器得到的值值是是在敏感在敏感轴轴上的分量，上的分量，实际实际的的惯惯性坐性坐标标系下的加速度系下的加速度可通可通过过上式上式变换变换得到，在得到，在弹弹体坐体坐标标系上系上动动点的力点的力为为称称为为比力，加速度比力，加速度计实际计实际是通是通过过比力来比力来测测量加速度的。量加速度的。n n由惯性测量组合测得的视加速度是相对惯由惯性测量组合测得的视加速度是相对惯性空间的加速度，在以上的分析计算中，性空间的加速度，在以上的分析计算中，假设了地球的曲率半径很大，自转速度为假设了地球的曲率半径很大，自转速度为零，在实际的导航中，飞行体是在曲率半零，在实际的导航中，飞行体是在曲率半径不为零且具有引力场的地球表面上，因径不为零且具有引力场的地球表面上，因此，需要对惯性空间加速度相对地球加速此，需要对惯性空间加速度相对地球加速度之差，即有害加速度进行补偿。即飞行度之差，即有害加速度进行补偿。即飞行体速度和地球自转角速度引起的哥氏加速体速度和地球自转角速度引起的哥氏加速度，飞行体沿地球表面飞行而产生的向心度，飞行体沿地球表面飞行而产生的向心加速度。加速度。3.4捷联惯导系统的算捷联惯导系统的算法实现法实现 捷联惯导基本算法与误差捷联惯导系统算法概述捷联惯导系统算法概述算法：从惯性仪表输出到导航与控制信息算法：从惯性仪表输出到导航与控制信息捷联惯导算法的基本内容：捷联惯导算法的基本内容：一、系统初始化一、系统初始化(Initialization)：1、给定飞行器初始位置、速度等、给定飞行器初始位置、速度等2、数学平台的初始对准、数学平台的初始对准3、惯性仪表的校准、惯性仪表的校准二、惯性仪表误差补偿二、惯性仪表误差补偿(Compensation)三、姿态矩阵的计算三、姿态矩阵的计算四、导航计算四、导航计算五、导航控制信息的提取五、导航控制信息的提取姿态计算 欧拉角微分方程1姿态矩阵的计算姿态矩阵的计算假设数学坐标系模拟地理坐标系假设数学坐标系模拟地理坐标系飞行器姿态的描述：飞行器姿态的描述：航向角航向角、俯仰角俯仰角、滚动角滚动角一、欧拉微分方程一、欧拉微分方程从地理坐标系到载体坐标系从地理坐标系到载体坐标系的旋转顺序：的旋转顺序：方向余弦矩阵：方向余弦矩阵：姿态计算 欧拉角微分方程2飞行器相对地理坐标系的角速度：飞行器相对地理坐标系的角速度：姿态计算 欧拉角微分方程3求解欧拉角速率得求解欧拉角速率得注意事项：当注意事项：当=90 度时，方程出现奇点度时，方程出现奇点姿态计算 矩阵方程精确解1二、方向余弦矩阵微分方程及其解二、方向余弦矩阵微分方程及其解其中其中由于陀螺仪直接测得的是载体由于陀螺仪直接测得的是载体相对惯性空间的角速度，所以：相对惯性空间的角速度，所以：导航计算可以得到导航计算可以得到有有因此因此得得姿态计算 矩阵方程精确解2的精确解（毕卡逼近）：的精确解（毕卡逼近）：其中其中方向不变时的精确解方向不变时的精确解九个微分方程求解，计算量大九个微分方程求解，计算量大姿态计算 四元数精确解1三、四元数微分方程式及其解三、四元数微分方程式及其解由第一章，四元数微分方程式：由第一章，四元数微分方程式：对对 的处理类似上一节的处理类似上一节精确解：精确解：其中：其中：姿态计算 四元数精确解2其中：其中：姿态计算 姿态航向角计算1四、姿态和航向角的计算四、姿态和航向角的计算根据载体和地理坐标系之间的方向余弦矩阵可确定姿态、航向角根据载体和地理坐标系之间的方向余弦矩阵可确定姿态、航向角姿态、航向角姿态、航向角真值的判断真值的判断姿态计算 姿态航向角计算2如利用四元数微分方程求解，如利用四元数微分方程求解，则先利用四元数求解结果计算则先利用四元数求解结果计算方向余弦矩阵的元素方向余弦矩阵的元素(1-58)：姿态实时计算 概述姿态矩阵的实时计算姿态矩阵的实时计算因假定因假定“数学平台数学平台”跟踪地理坐标系，因跟踪地理坐标系，因此此所以可得相应的姿态矩阵微分方程（所以可得相应的姿态矩阵微分方程（6-12）：）：或四元数微分方程：或四元数微分方程：注意事项：注意事项：1、上述两个方程中的角速度表达式不一样、上述两个方程中的角速度表达式不一样2、方程第二项较小，计算时速度可以低一些、方程第二项较小，计算时速度可以低一些增量算法 矩阵方程精确解一、角增量算法一、角增量算法(Angular Increment Algorithm)角增量：陀螺仪数字脉冲输出，每个脉冲代表一个角增量角增量：陀螺仪数字脉冲输出，每个脉冲代表一个角增量一个采样周期内，陀螺输出脉冲数对应的角增量为：一个采样周期内，陀螺输出脉冲数对应的角增量为：1、矩阵微分方程、矩阵微分方程(Matrix Differential Equation)计算计算根据矩阵微分方程的精确解（根据矩阵微分方程的精确解（6-20），有：），有：（解（解的第一项）的第一项）增量算法 矩阵方程CS参数展开合并上式，得展开合并上式，得其中其中增量算法 矩阵方程1阶将前式简写为：将前式简写为：或离散形式：或离散形式：C按按 Cn、Sn 取不同的近似值，形成相应的一阶取不同的近似值，形成相应的一阶 四阶算法四阶算法一阶算法：一阶算法：令令可将上述算法解写成可将上述算法解写成矩阵元素的形式：矩阵元素的形式：增量算法 矩阵方程1阶 一阶增量算法一阶增量算法增量算法 矩阵方程2-4阶当当 Cn、Sn 取取 n=2,3,4 时：时：二阶增量算法：二阶增量算法：三阶增量算法：三阶增量算法：四阶增量算法：四阶增量算法：增量算法 四元数2、四元数微分方程的计算：、四元数微分方程的计算：其中，其中，I 为单位四元数，为单位四元数，如如（6-24）所示：）所示：写成迭代形式：写成迭代形式：增量算法 四元数设设一阶算法：一阶算法：增量算法 四元数或展开为元素形式：或展开为元素形式：增量算法 四元数同理，可得二阶算法：同理，可得二阶算法：三阶算法：三阶算法：四阶算法：四阶算法：数值积分 1阶用一阶用一阶 四阶龙格四阶龙格-库塔积分矩阵和四元数微分方程库塔积分矩阵和四元数微分方程1、一阶龙格、一阶龙格-库塔法库塔法(Runge-Kutta)一个矩阵微分方程一个矩阵微分方程当初始条件已知，其一阶龙格当初始条件已知，其一阶龙格-库塔的解为库塔的解为:方程的解为初始值加上以初方程的解为初始值加上以初始点斜率为斜率的一个增量始点斜率为斜率的一个增量斜率斜率K的准确度不同，解的的准确度不同，解的精确度也不同精确度也不同数值积分 1阶 矩阵（1）姿态矩阵微分方程）姿态矩阵微分方程简化为简化为其一阶龙格其一阶龙格-库塔解：库塔解：展开为展开为元素形元素形式：式：与一阶与一阶增量算增量算法一致法一致数值积分 1阶 四元数（2）四元数微分方程）四元数微分方程或或一阶龙格一阶龙格-库塔解库塔解数值积分 2阶 矩阵 2、二阶龙格、二阶龙格-库塔法库塔法对一阶算法适当改进，使平均斜率更准确一些对一阶算法适当改进，使平均斜率更准确一些二阶龙格二阶龙格-库塔算法的解：库塔算法的解：（1）矩阵微分方程）矩阵微分方程数值积分 2阶 矩阵二阶龙格二阶龙格-库塔解：库塔解：设设则则数值积分 2阶 矩阵数值积分 2阶 四元数（2）四元数微分方程）四元数微分方程数值积分 2阶 四元数数值积分 4阶 矩阵3、四阶龙格、四阶龙格-库塔法库塔法则解则解（1）矩阵微分方程）矩阵微分方程则解则解数值积分 4阶 四元数（2）四元数微分方程）四元数微分方程则解则解数值积分 4阶 四元数数值积分 4阶 四元数角速度提取龙格龙格-库塔积分需要用到角速度信息库塔积分需要用到角速度信息而陀螺仪的数字脉冲输出为角增量而陀螺仪的数字脉冲输出为角增量如果采用周期如果采用周期 T 很小，可以近似把角速度看成常值或线性变化很小，可以近似把角速度看成常值或线性变化1、把角速度看成常值，则周期、把角速度看成常值，则周期 T 内内 一阶角速率提取一阶角速率提取2、把角速度看成线性变化、把角速度看成线性变化则角增量则角增量角速度提取从从 ti 到到 ti+T/2 的角增量：的角增量：从从 ti 到到 ti+T 的角增量：的角增量：求解，得求解，得因此因此ti 时刻陀螺输出置零时刻陀螺输出置零ti+T/2 时刻陀螺不置零时刻陀螺不置零ti+T 时刻陀螺置零时刻陀螺置零角速度提取如果如果 ti+T/2 时刻陀螺输出置零时刻陀螺输出置零从从 ti 到到 ti+T/2 的角增量：的角增量：从从 ti+T/2 到到 ti+T 的角增量：的角增量：代入代入得得