2.2捷联惯航系统算法程序.ppt

捷联算法仿真研究 载体运动过程中，由于受到路面、风浪、气流等条件的影响时常经历不规则的摇摆运动，使用单一的正弦运动模型显然不能逼近载体航行的实际情况。因此，这里采用多个正弦波叠加组合作为载体运动模型 ： ? 振幅 频率 相位 纵摇角P 0.30～ 1.5° 0.11～0.22 Hz 0 ～ 360° 横摇角R 0.42～ 2.1° 0.07～0.18 Hz 0 ～ 360° 航向角H 0.50～ 1.7° 0.19～0.30 Hz 0 ～ 360° * 捷联算法仿真研究 初始经度：118°，32° 加速度计的常值偏置误差 ：400μg， 随机偏置误差： 50μg 陀螺仪的常值漂移误差：0.2°/h， 随机漂移误差： 0.05°/h 初始误差： 仿真时间：60 h 仿真条件及参数如下： * 捷联算法仿真研究 * 捷联算法仿真研究 * * * * * * * * * 捷联惯导系统算法 * 捷联惯导基本算法与误差 捷联惯导系统算法概述 算法：从惯性仪表输出到导航与控制信息 捷联惯导算法的基本内容： 一、系统初始化(Initialization)： 1、给定飞行器初始位置、速度等 2、数学平台的初始对准 3、惯性仪表的校准 二、惯性仪表误差补偿(Compensation) 三、姿态矩阵的计算 四、导航计算 五、导航控制信息的提取 * * 姿态计算 四元数方法 对于四元数 用四元数q描述刚体的定点转动，即当只关心b系相对于E系的角位置时，可认为b系是有E系经过无中间过程的一次等效旋转形成。而四元数q包含了这种等效旋转的全部信息：矢量n为旋转轴和旋转方向，θ为转过的角度。 姿态计算 姿态角方法 四、姿态和航向角的计算 根据载体和地理坐标系的方向余弦矩阵可确定姿态、航向角 姿态、航向角真值的判断 * 姿态计算 姿态角方法 如利用四元数微分方程求解，则先利用四元数求解结果计算方向余弦矩阵的元素(1-58)： * * 对上式两边求导 其中 * 由于刚体绕n转动，与刚体固联的b坐标系各轴在旋转过程中分别位于三个不同的圆锥面上，圆锥顶点即为坐标原点，n位于圆锥的对称轴上，即n在b坐标系的三个坐标轴上的投影不变。所以n在b系内观察时始终不变，则有 又有如下条件存在 * 则四元数的微分方程变为 求解如下的四元数方程 * 根据四元数乘法可知，两个相同的单位四元数相乘，其结果为-1，即 从而可以求出 四元数微分为 * 从而可以得到如下公式 又有如下条件存在 则可以建立四元数的微分方程 捷联惯导系统中，陀螺仪测量的角速度是载体坐标系下的表示，需要转化到地理坐标系下 * 如果两个向量 nn 和 nb 是零标量的四元数，则nE和nb间的变换关系可采用四元数乘法表示： 则陀螺仪测量的载体坐标系下角速度转换到地理坐标系可以表示为 由于q是表征刚体旋转的规范化四元数，应用四元数乘法结合律，得 * 如果将角速度表示为向量形式 则可以将四元数微分写成矩阵形式 姿态计算 四元数精确解 由四元数微分方程式： * 姿态计算 四元数精确解 其中： * 姿态计算 四元数精确解 由于计算误差、不可交换性误差等影响，计算的四元数会失去规范性，即其范数不再等于1，需对计算的四元数周期地进行规范化处理，其处理公式如下： * 捷联惯导系统误差传播特性 * 捷联惯导误差传播特性 基本特性与平台式惯导系统一致 系统误差方程的建立（角度、速度、位置） 1、数学平台的误差方程（四元数法） 四元数微分方程： 假设数学平台模拟地理坐标系，因此 则 —— 理想情况 实际系统中 实际计算的四元数微分方程 * 四元数误差 四元数计算误差表达式 设一矢量 R 相对地理坐标系静止，在地理坐标系内表示为 RE，在飞行器坐标系内表示为 Rb，用 q 表示载体坐标系相对地理坐标系的转动。则 或 设 Rb 是准确值，考虑四元数的计算误差，有 则 定义 —— RE、RE’ 之间的转动四元数为δq * 四元数误差 得 * 四元数误差 考虑到 可得 另有 代入上式，并忽略二阶小量 * —— 数学平台误差角的矢量表达 第一项： 第三项： 第二项： 平台误差角： 四元数误差 * 四元数误差、速度误差 展开得到 ——数学平台误差角的分量表达式 2、速度误差方程 因数学平台模拟地理坐标系 * 速度误差 因加速度计固联于载体，将上式写在载体坐标系内： 定义速度误差 则 考虑 设 则 * 速度误差 转换到地理坐标系： 由于 则 又有 因此 将 代入上式，展开并忽略二阶小量： * 速度误差 第一项： ——速度误差方程的矢量表达式 第二项： 第三项： * 速度误差 第四项： * 位置误差、系统误差 根据经度、纬度的定义 取小偏差，得 4、系统误差方程 由数学平台误差角方程