P15捷联惯导系统算法.ppt

* * 捷联惯导基本算法与误差 捷联惯导系统算法概述 算法：从惯性仪表输出到导航与控制信息 捷联惯导算法的基本内容： 一、系统初始化(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阶 矩阵 二阶龙格-库塔解： 设 则