捷联惯导系统算法.ppt

1、捷联惯导基本算法,捷联惯导系统算法概述,算法：从惯性仪表输出到导航与控制信息,捷联惯导算法的基本内容： 一、系统初始化： 1、给定飞行器初始位置、速度等 2、数学平台的初始对准 3、惯性仪表的校准 二、惯性仪表的误差补偿 三、姿态矩阵的计算 四、导航计算 五、导航控制信息的提取,惯性器件误差 加速度计,惯性器件的误差补偿,姿态计算 欧拉角微分方程1,姿态矩阵的计算,假设数学坐标系模拟地理坐标系,飞行器姿态的描述： 航向角、俯仰角、滚动角,一、欧拉微分方程,从地理坐标系到载体坐标系的旋转顺序： ,方向余弦矩阵：,姿态计算 欧拉角微分方程2,飞行器相对地理坐标系的角速度：,姿态计算 欧拉角微分方程

2、3,求解欧拉角速率得,注意事项：当 = 90 度时，方程出现奇点,姿态计算 矩阵方程精确解1,二、方向余弦矩阵微分方程及其解,其中,由于陀螺仪直接测得的是载体相对惯性空间的角速度，所以：,导航计算可以得到,有,因此,得,姿态计算 矩阵方程精确解2,的精确解（毕卡逼近）：,其中,方向不变时的精确解,九个微分方程求解，计算量大,姿态计算 四元数精确解1,三、四元数微分方程式及其解,由第一章，四元数微分方程式：,对 的处理类似上一节,精确解：,其中：,姿态计算 四元数精确解2,其中：,姿态计算 姿态航向角计算1,四、姿态和航向角的计算,根据载体和地理坐标系之间的方向余弦矩阵可确定姿态、航向角,姿态、

3、航向角真值的判断,姿态计算 姿态航向角计算2,如利用四元数微分方程求解，则先利用四元数求解结果计算方向余弦矩阵的元素(1-58)：,姿态实时计算 概述,姿态矩阵的实时计算,因假定“数学平台”跟踪地理坐标系，因此,所以可得相应的姿态矩阵微分方程（6-12）：,或四元数微分方程：,注意事项： 1、上述两个方程中的角速度表达式不一样 2、方程第二项较小，计算时速度可以低一些,增量算法 矩阵方程精确解,一、角增量算法,角增量：陀螺仪数字脉冲输出，每个脉冲代表一个角增量,一个采样周期内，陀螺输出脉冲数对应的角增量为：,1、矩阵微分方程计算,根据矩阵微分方程的精确解（6-20），有：,增量算法 矩阵方程C

4、S参数,展开合并上式，得,其中,增量算法 矩阵方程1阶,将前式简写为：,或离散形式：,C按 Cn、Sn 取不同的近似值，形成相应的一阶 四阶算法,一阶算法：,令,可将上述算法解写成矩阵元素的形式：,增量算法 矩阵方程1阶, 一阶增量算法,增量算法 矩阵方程2-4阶,当 Cn、Sn 取 n = 2, 3, 4 时：,二阶增量算法：,三阶增量算法：,四阶增量算法：,增量算法 四元数,2、四元数微分方程的计算：,其中，I 为单位四元数， 如 （6-24）所示：,写成迭代形式：,增量算法 四元数,设,一阶算法：,增量算法 四元数,或展开为元素形式：,增量算法 四元数,同理，可得二阶算法：,三阶算法：,

5、四阶算法：,数值积分 1阶,用一阶 四阶龙格-库塔积分矩阵和四元数微分方程,1、一阶龙格-库塔法,一个矩阵微分方程,当初始条件已知，其一阶龙格-库塔的解为:,方程的解为初始值加上以初始点斜率为斜率的一个增量 斜率K的准确度不同，解的精确度也不同,数值积分 1阶 矩阵,（1）姿态矩阵微分方程,简化为,其一阶龙格-库塔解：,展开为元素形式：,与一阶增量算法一致,数值积分 1阶 四元数,（2）四元数微分方程,或,一阶龙格-库塔解,数值积分 2阶 矩阵,2、二阶龙格-库塔法,对一阶算法适当改进，使平均斜率更准确一些,二阶龙格-库塔算法的解：,（1）矩阵微分方程,数值积分 2阶 矩阵,二阶龙格-库塔解：

6、,设,则,数值积分 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 时刻陀螺不置