捷联惯导系统算法

《捷联惯导系统算法》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。

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

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

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

4、-20），有：（解的第一项）增量算法矩阵方程CS参数展开合并上式，得其中增量算法矩阵方程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时刻陀螺输出置

7、零ti+T/2时刻陀螺不置零ti+T时刻陀螺置零角速度提取如果ti+T/2时刻陀螺输出置零从ti到ti+T/2的角增量：从ti+T/2到ti+T的角增量：代入得