武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 概述概述姿态矩阵的实时计算姿态矩阵的实时计算因假定因假定“数学平台数学平台”跟踪地理坐标系，因跟踪地理坐标系，因此此所以可得相应的姿态矩阵微分方程（所以可得相应的姿态矩阵微分方程（6-12）：）：或四元数微分方程：或四元数微分方程：注意事项：注意事项：1、上述两个方程中的角速度表达式不一样、上述两个方程中的角速度表达式不一样2、方程第二项较小，计算时速度可以低一些、方程第二项较小，计算时速度可以低一些 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 矩阵方程精确解矩阵方程精确解一、角增量算法一、角增量算法角增量：陀螺仪数字脉冲输出，每个脉冲代表一个角增量角增量：陀螺仪数字脉冲输出，每个脉冲代表一个角增量一个采样周期内，陀螺输出脉冲数对应的角增量为：一个采样周期内，陀螺输出脉冲数对应的角增量为：1、矩阵微分方程计算、矩阵微分方程计算根据矩阵微分方程的精确解（根据矩阵微分方程的精确解（6-20），有：），有：（解（解的第一项）的第一项） 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 矩阵方程矩阵方程CS参数参数展开合并上式，得展开合并上式，得其中其中 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 矩阵方程矩阵方程1阶阶将前式简写为：将前式简写为：或离散形式：或离散形式：ΔC按按 Cn、、Sn 取不同的近似值，形成相应的一阶取不同的近似值，形成相应的一阶 ~ 四阶算法四阶算法一阶算法：一阶算法：令令可将上述算法解写成可将上述算法解写成矩阵元素的形式：矩阵元素的形式： 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 矩阵方程矩阵方程1阶阶—— 一阶增量算法一阶增量算法 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 矩阵方程矩阵方程2-4阶阶当当 Cn、、Sn 取取 n = 2, 3, 4 时：时：二阶增量算法：二阶增量算法：三阶增量算法：三阶增量算法：四阶增量算法：四阶增量算法： 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 四元数四元数2、四元数微分方程的计算：、四元数微分方程的计算：其中，其中，I 为单位四元数，为单位四元数，[Δθ] 如如 （（6-24）所示：）所示：写成迭代形式：写成迭代形式： 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 四元数四元数设设一阶算法：一阶算法： 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 四元数四元数或展开为元素形式：或展开为元素形式： 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 四元数四元数同理，可得二阶算法：同理，可得二阶算法：三阶算法：三阶算法：四阶算法：四阶算法： 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 1阶阶用一阶用一阶 ~ 四阶龙格四阶龙格-库塔积分矩阵和四元数微分方程库塔积分矩阵和四元数微分方程1、一阶龙格、一阶龙格-库塔法库塔法一个矩阵微分方程一个矩阵微分方程当初始条件已知，其一阶龙格当初始条件已知，其一阶龙格-库塔的解为库塔的解为:方程的解为初始值加上以初方程的解为初始值加上以初始点斜率为斜率的一个增量始点斜率为斜率的一个增量斜率斜率K的准确度不同，解的的准确度不同，解的精确度也不同精确度也不同 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 1阶阶 矩阵矩阵（（1）姿态矩阵微分方程）姿态矩阵微分方程简化为简化为其一阶龙格其一阶龙格-库塔解：库塔解：展开为展开为元素形元素形式：式：与一阶与一阶增量算增量算法一致法一致 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 1阶阶 四元数四元数（（2）四元数微分方程）四元数微分方程或或一阶龙格一阶龙格-库塔解库塔解 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 2阶阶 矩阵矩阵 2、二阶龙格、二阶龙格-库塔法库塔法对一阶算法适当改进，使平均斜率更准确一些对一阶算法适当改进，使平均斜率更准确一些二阶龙格二阶龙格-库塔算法的解：库塔算法的解：（（1）矩阵微分方程）矩阵微分方程 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 2阶阶 矩阵矩阵二阶龙格二阶龙格-库塔解：库塔解：设设则则 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 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 时刻陀螺不置零时刻陀螺不置零ti + T 时刻陀螺置零时刻陀螺置零 武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所武汉大学测绘学院卫星应用工程研究所 ti + T/2 时刻陀螺输出置零时刻陀螺输出置零从从 ti 到到 ti + T/2 的角增量：的角增量：从从 ti + T/2 到到 ti + T 的角增量：的角增量：代入代入得得 。