1、捷联惯导基本算法与误差,捷联惯导系统算法概述,算法：从惯性仪表输出到导航与控制信息,捷联惯导算法的基本内容：一、系统初始化：1、给定飞行器初始位置、速度等2、数学平台的初始对准3、惯性仪表的校准二、惯性仪表的误差补偿三、姿态矩阵的计算四、导航计算五、导航控制信息的提取,惯性器件误差 陀螺,惯性器件误差补偿,惯性器件误差补偿对捷联惯导的重要性,一、陀螺仪误差模型,静态误差模型,动态误差模型,惯性器件误差 加速度计,二、加速度计误差模型,静态误差模型,动态误差模型,三、惯性器件的误差补偿,姿态计算 欧拉角微分方程1,姿态矩阵的计算,假设数学坐标系模拟地理坐标系,飞行器姿态的描述： 航向角、俯仰角、
2、滚动角,一、欧拉微分方程,从地理坐标系到载体坐标系的旋转顺序： ,方向余弦矩阵：,姿态计算 欧拉角微分方程2,飞行器相对地理坐标系的角速度：,姿态计算 欧拉角微分方程3,求解欧拉角速率得,注意事项：当 = 90 度时，方程出现奇点,姿态计算 矩阵方程精确解1,二、方向余弦矩阵微分方程及其解,其中,由于陀螺仪直接测得的是载体相对惯性空间的角速度，所以：,导航计算可以得到,有,因此,得,姿态计算 矩阵方程精确解2,的精确解（毕卡逼近）：,其中,方向不变时的精确解,九个微分方程求解，计算量大,姿态计算 四元数精确解1,三、四元数微分方程式及其解,由第一章，四元数微分方程式：,对 的处理类似上一节,精
3、确解：,其中：,姿态计算 四元数精确解2,其中：,姿态计算 姿态航向角计算1,四、姿态和航向角的计算,根据载体和地理坐标系之间的方向余弦矩阵可确定姿态、航向角,姿态、航向角真值的判断,姿态计算 姿态航向角计算2,如利用四元数微分方程求解，则先利用四元数求解结果计算方向余弦矩阵的元素(1-58)：,姿态实时计算 概述,姿态矩阵的实时计算,因假定“数学平台”跟踪地理坐标系，因此,所以可得相应的姿态矩阵微分方程（6-12）：,或四元数微分方程：,注意事项：1、上述两个方程中的角速度表达式不一样2、方程第二项较小，计算时速度可以低一些,增量算法 矩阵方程精确解,一、角增量算法,角增量：陀螺仪数字脉冲输
4、出，每个脉冲代表一个角增量,一个采样周期内，陀螺输出脉冲数对应的角增量为：,1、矩阵微分方程计算,根据矩阵微分方程的精确解（6-20），有：,增量算法 矩阵方程CS参数,展开合并上式，得,其中,增量算法 矩阵方程1阶,将前式简写为：,或离散形式：,C按 Cn、Sn 取不同的近似值，形成相应的一阶 四阶算法,一阶算法：,令,可将上述算法解写成矩阵元素的形式：,增量算法 矩阵方程1阶, 一阶增量算法,增量算法 矩阵方程2-4阶,当 Cn、Sn 取 n = 2, 3, 4 时：,二阶增量算法：,三阶增量算法：,四阶增量算法：,增量算法 四元数,2、四元数微分方程的计算：,其中，I 为单位四元数， 如
5、 （6-24）所示：,写成迭代形式：,增量算法 四元数,设,一阶算法：,增量算法 四元数,或展开为元素形式：,增量算法 四元数,同理，可得二阶算法：,三阶算法：,四阶算法：,数值积分 1阶,用一阶 四阶龙格-库塔积分矩阵和四元数微分方程,1、一阶龙格-库塔法,一个矩阵微分方程,当初始条件已知，其一阶龙格-库塔的解为:,方程的解为初始值加上以初始点斜率为斜率的一个增量斜率K的准确度不同，解的精确度也不同,数值积分 1阶 矩阵,（1）姿态矩阵微分方程,简化为,其一阶龙格-库塔解：,展开为元素形式：,与一阶增量算法一致,数值积分 1阶 四元数,（2）四元数微分方程,或,一阶龙格-库塔解,数值积分 2
6、阶 矩阵,2、二阶龙格-库塔法,对一阶算法适当改进，使平均斜率更准确一些,二阶龙格-库塔算法的解：,（1）矩阵微分方程,数值积分 2阶 矩阵,二阶龙格-库塔解：,设,则,数值积分 2阶 矩阵,数值积分 2阶 四元数,（2）四元数微分方程,数值积分 2阶 四元数,数值积分 4阶 矩阵,3、四阶龙格-库塔法,则解,（1）矩阵微分方程,则解,数值积分 4阶 四元数,（2）四元数微分方程,则解,数值积分 4阶 四元数,数值积分 4阶 四元数,角速度提取,龙格-库塔积分需要用到角速度信息,而陀螺仪的数字脉冲输出为角增量,如果采用周期 T 很小，可以近似把角速度看成常值或线性变化,1、把角速度看成常值，则
7、周期 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 的角增量：,代入,得,捷联惯导误差传播特性,基本特性与平台式惯导系统一致,误差方程的建立方法,系统误差方程的建立（角度、速度、位置）,1、数学平台的误差方程（四元数法）,四元数微分方程：,假设数学平台
8、模拟地理坐标系，因此,则, 理想情况,实际系统中,实际计算的四元数微分方程,四元数误差,四元数计算误差表达式,设一矢量 R 相对地理坐标系静止，在地理坐标系内表示为 RE，在飞行器坐标系内表示为 Rb，用 q 表示载体坐标系相对地理坐标系的转动。则,或,设 Rb 是准确值，考虑四元数的计算误差，有,则,定义, RE、RE 之间的转动四元数为q,四元数误差,得,四元数误差,考虑到,可得,另有,代入上式，并忽略二阶小量, 数学平台误差角的矢量表达,第一项：,第三项：,第二项：,平台误差角：,四元数误差,四元数误差、速度误差,展开得到,数学平台误差角的分量表达式,2、速度误差方程,因数学平台模拟地理坐标系，由第二章（2-36）可得,速度误差,因加速度计固联于载体，将上式写在载体坐标系内：,定义速度误差,则,考虑,设,则,速度误差,转换到地理坐标系：,由于,则,又有,因此,速度误差,第一项：,速度误差方程的矢量表达式,第二项：,第三项：,速度误差,第四项：,速度误差,展开得, 速度误差方程的分量展开形式,位置误差、系统误差,根据经度、纬度的定义,取小偏差，得,4、系统误差方程,由数学平台误差角方程、速度误差方程、位置误差方程组成经度误差方程仍可单独考虑静基座条件下，捷联惯导系统误差传播特性与平台式一致,End,
