1、惯性导航原理*iiiizyOx*eeezyOxeweOziOzeeyOx*tttzyOx*nnnzyOxnztznxtxnyty *pppzyOx*bbbzyOx*3.2四元数理论*四元数：描述刚体角运动的数学工具四元数：描述刚体角运动的数学工具(quaternions)针对捷联惯导系统，可弥补欧拉参数在描述和解算方面的不足。针对捷联惯导系统，可弥补欧拉参数在描述和解算方面的不足。四元数的表示四元数的表示由一个实单位和三个虚数单位由一个实单位和三个虚数单位 i,j,k 组成的数组成的数 kPjPiPq3211或者省略或者省略 1，写成，写成kPjPiPq321i,j,k 服从如下运算公式：服从
2、如下运算公式：*i,j,k 服从如下运算公式服从如下运算公式 1kkjjiikijjiijkkjjkiikkPjPiPq321 称作标量部分，称作标量部分，kPjPiP321称作矢量部分称作矢量部分 四元数的另一种表示法四元数的另一种表示法 Pq,P 泛指矢量部分泛指矢量部分提示：四元数与刚体转动的关系提示：四元数与刚体转动的关系*kPjPiPq321kjivM3211四元数加减法四元数加减法 MqkPjPiPv)()()()(332211或简单表示为或简单表示为 PvMq,*2四元数乘法四元数乘法)(321321kjivkPjPiPMq)(332211PPPviPPvP)(233211jPP
3、vP)(311322kPPvP)(122133或简单表示为或简单表示为 PvPPvMq 关于相乘符号关于相乘符号 关于交换律和结合律关于交换律和结合律*3共轭四元数共轭四元数 仅向量部分符号相反的两个四元数仅向量部分符号相反的两个四元数),(Pq和和),(*Pq互为共轭互为共轭 可证明：可证明：*)*(qhqh4四元数的范数四元数的范数 q定义定义 2322212*PPPqqq1q则称为规范化四元数则称为规范化四元数*5逆四元数逆四元数 qq11qq*当当 1q时时*1qq6四元数的除法四元数的除法 若若 Mqh 则则 1 Mhq若若 Mhq 则则 Mhq1不能表示为不能表示为 hMq（含义不
4、确切含义不确切）*一个坐标系或矢量相对参考坐标系旋转，一个坐标系或矢量相对参考坐标系旋转，转角为转角为，转轴转轴 n n 与参考系各轴间的方向余弦值为与参考系各轴间的方向余弦值为cos、cos、cos。则表示该旋转的四元数可以写为则表示该旋转的四元数可以写为 nkjiq2sin2coscos2sincos2sincos2sin2cos为特征四元数为特征四元数（范数为范数为 1）四元数既表示了转轴方向，又表示了转角大小（转动四元数）四元数既表示了转轴方向，又表示了转角大小（转动四元数）*如果矢量如果矢量 R 相对固定坐标系旋转相对固定坐标系旋转，旋转四元数为，旋转四元数为 q，转动后，转动后的矢
5、量为的矢量为 R，则这种转动关系可通过四元数旋转运算来实现则这种转动关系可通过四元数旋转运算来实现1 qRqR含义：矢量含义：矢量 R 相对固定坐标系产生旋转，相对固定坐标系产生旋转，转角和转轴由转角和转轴由 q 决定决定*如果如果坐标系坐标系 OXYZ 发生发生 q 旋转，得到新坐标系旋转，得到新坐标系 OXYZ 一个相对原始坐标系一个相对原始坐标系 OXYZ 不发生旋转变换的矢量不发生旋转变换的矢量 V zkyjxiV矢量矢量 V 在新坐标系上在新坐标系上 OXYZ 的投影为的投影为 kzjyixV则不变矢量则不变矢量 V 在两个坐标系上的投影之间存在如下关系：在两个坐标系上的投影之间存在
6、如下关系：qVqVee1式中式中 zkyjxiVekzjyixVe分别称为矢量分别称为矢量 V 在坐标系在坐标系 OXYZ 和和 OXYZ 上的映像上的映像*zkyjxiV kzjyixVkzjyixVezkyjxiVe*qVqVee1将该投影变换式展开，也就是把将该投影变换式展开，也就是把kzjyixVezkyjxiVekPjPiPq321kPjPiPq3211代入上述投影变换式代入上述投影变换式kzjyix)(321kPjPiP)(zkyjxi)(321kPjPiP进行四元数乘法运算，整理运算结果可得进行四元数乘法运算，整理运算结果可得*zyxCzyx其中方向余弦矩阵其中方向余弦矩阵 C2
7、22123213223113223212223212313212322212)(2)(2)(2)(2)(2)(2PPPPPPPPPPPPPPPPPPPPPPPPPPP*多次旋转多次旋转的合成的合成对于一个坐标系经过多次旋转后，新坐标系和原始坐标系之间对于一个坐标系经过多次旋转后，新坐标系和原始坐标系之间的关系等效于一个一次转动的效果，的关系等效于一个一次转动的效果，相应地有合成转动四元数相应地有合成转动四元数 假定假定 q1、q2 分别是第一次转动、第二次转动的四元数分别是第一次转动、第二次转动的四元数 q 是合成转动的四元数，是合成转动的四元数，那么有如下关系成立：那么有如下关系成立：21q
8、qq上式中上式中 q1 和和 q2 的转轴方向必须以映象的形式给出。的转轴方向必须以映象的形式给出。如果如果 q1 和和 q2 的转轴方向都以原始坐标系的分量表示，则有的转轴方向都以原始坐标系的分量表示，则有 12qqq*用四元数旋转变换的方法求取两个坐标系之间的方向余弦表。用四元数旋转变换的方法求取两个坐标系之间的方向余弦表。坐标系坐标系 OXYZ 相对相对OXYZ 三次旋转，三次旋转，以欧拉角以欧拉角 、的形式给出。的形式给出。第一转，绕第一转，绕 Z 轴转轴转角，瞬时转轴角，瞬时转轴 n 和和 k 轴重合，则转动四元轴重合，则转动四元数为数为 kq2sin2cos1*第二转，绕第二转，绕
9、 OX1 轴转轴转角，角，瞬时转轴瞬时转轴 n 的方向表示式为的方向表示式为)sin(cosji其转动四元数为其转动四元数为 nq2sin2cos2)sin(cos2sin2cosji*由于由于 q1 和和 q2 的瞬时转轴的瞬时转轴都是以同一个坐标系的方向余弦来都是以同一个坐标系的方向余弦来表示，则合成转动四元数表示，则合成转动四元数 q 的计算采用：的计算采用：12qqqkji2sin2cos)sin(cos2sin2coskji2sin2cos2sin2sin2cos2sin2cos2cos*以瞬时转轴以瞬时转轴映象映象形式给出形式给出转动四元数的表达式并求转动四元数的表达式并求出合成转
10、动四元数出合成转动四元数 第一次转时，映象形式的第一次转时，映象形式的 q1 和非映象形式的和非映象形式的 q1 是是一致的：一致的：kq2sin2cos1*第二转绕第二转绕 OX1 轴转轴转 角角瞬时转轴瞬时转轴 n 是由是由 OX 经过经过第一转转换来的第一转转换来的OX 轴对应单位矢量轴对应单位矢量 i，所，所以定义以定义 n 的映象为的映象为 i则则 q2 的映象表示式为的映象表示式为 iq2sin2cos2*第三转，绕第三转，绕 OZ 轴转动轴转动 角角瞬时转轴瞬时转轴 n 是由是由 OZ 经过经过第一转和第二转转换来的第一转和第二转转换来的OZ 轴对应单位矢量轴对应单位矢量 k，所
11、以定义所以定义 n 的映象为的映象为 k则则 q3 的映象表示式为的映象表示式为kq2sin2cos3*由于由于 q1、q2 和和 q3 都是映象形式都是映象形式，所以三次转动的合成转动所以三次转动的合成转动四元数四元数 q 为为321qqqqkik2sin2cos2sin2cos2sin2cosji2sin2sin2cos2sin2cos2cosk2sin2cos据此可算出对应的方向余弦表据此可算出对应的方向余弦表*坐标系旋转时，不变矢量坐标系旋转时，不变矢量 V 在两个坐标系上的投影之间存在在两个坐标系上的投影之间存在如下关系：如下关系：qVqVee1在一些资料中，四元数的转动公式也经常写
12、成如下的形式在一些资料中，四元数的转动公式也经常写成如下的形式 1qqVVEE这个公式的意义是说，在一个超复数空间中，或者在一个固这个公式的意义是说，在一个超复数空间中，或者在一个固定坐标系中，矢量定坐标系中，矢量 VE 按着四元数按着四元数 q 所表示的方向和大小转所表示的方向和大小转动了一个角度，得到一个新的矢量动了一个角度，得到一个新的矢量 VE*四元数法能得到迅速发展，是由于飞行器控制与导航的发展，要四元数法能得到迅速发展，是由于飞行器控制与导航的发展，要求更合理地描述刚体空间运动，以及便于计算机的应用。求更合理地描述刚体空间运动，以及便于计算机的应用。采用方向余弦矩阵描述飞行器运动时
13、，要积分矩阵微分方程式：采用方向余弦矩阵描述飞行器运动时，要积分矩阵微分方程式：CC式中式中C为动坐标系转置到定坐标系的方向余弦矩阵，为动坐标系转置到定坐标系的方向余弦矩阵，为动坐为动坐标系相对定坐标系旋转角速度标系相对定坐标系旋转角速度的反对称矩阵：的反对称矩阵：000 xyxzyz包含包含 9 个一阶微个一阶微分方程式，计算分方程式，计算量比较大量比较大*如果采用四元数法，则是要求解四元数方程式如果采用四元数法，则是要求解四元数方程式 qq21q 为动坐标系的转动四元数，为动坐标系的转动四元数，为动坐标系相对定坐标系为动坐标系相对定坐标系的旋转角速度，也表示为四元数的旋转角速度，也表示为四
14、元数 kjizyx 0按四元数乘积展开按四元数乘积展开 xyzyzyyzxzyxPPPPPPPPPPPP2131323213212222只要解四个一阶微分只要解四个一阶微分方程式组方程式组即可即可*3.3视加速度和比力*RrMaMgMaRMgTMaMTRamaraRa 根据质心运动定理和相对运动学原理，根据质心运动定理和相对运动学原理，飞行体质心运动的微分方程（在惯性坐标系下）为：飞行体质心运动的微分方程（在惯性坐标系下）为：式中，式中，-飞行体的质量；飞行体的质量；-推力；推力；-空气阻力；空气阻力；-惯性空间飞行时，导弹质心加速度；惯性空间飞行时，导弹质心加速度；，-由推力产生的加速度；由
15、推力产生的加速度；-由阻力引起的阻力加速度。由阻力引起的阻力加速度。*RTagaa)(mamgmar)(gaarra)(RTraaa由上式可得出由上式可得出 飞行体质心运动的微分方程（在弹体坐标系下）为：飞行体质心运动的微分方程（在弹体坐标系下）为：或或 ，式中式中是动点的相对加速度，将（是动点的相对加速度，将（*）代入上式）代入上式 得得 *)(gaasTaRasaa)()(agmmamgf由上式可知，测得的由上式可知，测得的是推力加速度是推力加速度和阻力加速度和阻力加速度的矢量和，称为视加速度，在实际的测试中由加速度传感器得到的值是的矢量和，称为视加速度，在实际的测试中由加速度传感器得到的
16、值是在敏感轴上的分量，实际的惯性坐标系下的加速度在敏感轴上的分量，实际的惯性坐标系下的加速度可通过上式变换得到，在弹体坐标系上动点的力为可通过上式变换得到，在弹体坐标系上动点的力为称为比力，加速度计实际是通过比力来测量加速度的。称为比力，加速度计实际是通过比力来测量加速度的。*3.4捷联惯导系统的算法实现*捷联惯导系统算法概述捷联惯导系统算法概述算法：从惯性仪表输出到导航与控制信息算法：从惯性仪表输出到导航与控制信息捷联惯导算法的基本内容：捷联惯导算法的基本内容：一、系统初始化一、系统初始化(Initialization)：1、给定飞行器初始位置、速度等、给定飞行器初始位置、速度等2、数学平台
17、的初始对准、数学平台的初始对准3、惯性仪表的校准、惯性仪表的校准二、惯性仪表误差补偿二、惯性仪表误差补偿(Compensation)三、姿态矩阵的计算三、姿态矩阵的计算四、导航计算四、导航计算五、导航控制信息的提取五、导航控制信息的提取*姿态矩阵的计算姿态矩阵的计算假设数学坐标系模拟地理坐标系假设数学坐标系模拟地理坐标系飞行器姿态的描述：飞行器姿态的描述：航向角航向角、俯仰角、俯仰角、滚动角、滚动角一、欧拉微分方程一、欧拉微分方程从地理坐标系到载体坐标系从地理坐标系到载体坐标系的旋转顺序：的旋转顺序：方向余弦矩阵：方向余弦矩阵：CCCCbE*coscossincoscossinsinsinsi
18、ncossincossincoscoscossinsinsincossinsinsincossincossincoscos飞行器相对地理坐标系的角速度：飞行器相对地理坐标系的角速度：TbEbzbEbybEbxbEb000000CCC*000000CCCcoscossin0cossincos0sin01求解欧拉角速率得求解欧拉角速率得bEbzbEbybEbx1coscossin0cossincos0sin01bEbzbEbybEbxcossin0cossincoscos0sincossinsincoscos1注意事项：当注意事项：当=90 度时，方程出现奇点度时，方程出现奇点*二、方向余弦矩阵微
19、分方程及其解二、方向余弦矩阵微分方程及其解 CCbEbEbEbCC其中其中000 xyxzyzbEb由于陀螺仪直接测得的是载体由于陀螺仪直接测得的是载体相对惯性空间的角速度，所以：相对惯性空间的角速度，所以：biEbibbEb导航计算可以得到导航计算可以得到EIE有有EbEiEbEbiECC 因此因此EbEiEbEbibbEbCC 得得EbEiEbibEbEbCCC*bEbEbEbCC的精确解（毕卡逼近）：的精确解（毕卡逼近）：)()(tCttCEbEb220000)(cos1sinbEbbEbI1nnttbEbbEbdt其中其中000bEbXbEbYbEbXbEbZbEbYbEbZbEb22
20、220)()()(bEbZbEbYbEbXbEb方向不变时的精确解方向不变时的精确解九个微分方程求解，计算量大九个微分方程求解，计算量大*三、四元数微分方程式及其解三、四元数微分方程式及其解由第一章，四元数微分方程式：由第一章，四元数微分方程式：qqb对对 的处理类似上一节的处理类似上一节b32132102/2/2/2/02/2/2/2/02/2/2/2/0PPPPPPxyzxzyyzxzyx精确解：精确解：)0(2sin2cos)(000qItq其中：其中：21ttbdt*)0(2sin2cos)(000qItq其中：其中：21ttbdt0000 xyzxzyyzxzyx22220ZYX*四
21、、姿态和航向角的计算四、姿态和航向角的计算根据载体和地理坐标系之间的方向余弦矩阵可确定姿态、航向角根据载体和地理坐标系之间的方向余弦矩阵可确定姿态、航向角coscossincoscossinsinsinsincossincossincoscoscossinsinsincossinsinsincossincossincoscosbEC333231232221131211TTTTTTTTTCbE)(sin131T33231TTtg11121TTtg姿态、航向角姿态、航向角真值的判断真值的判断*)270,180(180)180,90(180)90,0()90,0(2/02/0000000000023
22、33主主主主真象限TT)270,180(180)180,90(180)0,90(360)90,0(2/302/0000000000001211主主主主真象限TT如利用四元数微分方程求解，如利用四元数微分方程求解，则先利用四元数求解结果计算则先利用四元数求解结果计算方向余弦矩阵的元素方向余弦矩阵的元素(1-58)：232221211PPPT)(232112PPPT)(213223PPPT222123233PPPT)(223113PPPT*姿态矩阵的实时计算姿态矩阵的实时计算因假定因假定“数学平台数学平台”跟踪地理坐标系，因跟踪地理坐标系，因此此biEbibbEb所以可得相应的姿态矩阵微分方程（所
23、以可得相应的姿态矩阵微分方程（6-12）：）：EbEiEbibEbEbCCC或四元数微分方程：或四元数微分方程：)()()(tqtqbiEbib注意事项：注意事项：1、上述两个方程中的角速度表达式不一样、上述两个方程中的角速度表达式不一样2、方程第二项较小，计算时速度可以低一些、方程第二项较小，计算时速度可以低一些*一、角增量算法一、角增量算法(Angular Increment Algorithm)角增量：陀螺仪数字脉冲输出，每个脉冲代表一个角增量角增量：陀螺仪数字脉冲输出，每个脉冲代表一个角增量一个采样周期内，陀螺输出脉冲数对应的角增量为：一个采样周期内，陀螺输出脉冲数对应的角增量为：tt
24、tibdt1、矩阵微分方程、矩阵微分方程(Matrix Differential Equation)计算计算根据矩阵微分方程的精确解（根据矩阵微分方程的精确解（6-20），有：），有：220000)(cos1sin)()(bibbibEbEbItCttCEbEiEbibEbEbCCC（解（解的第一项）的第一项）*220000)(cos1sin)()(bibbibEbEbItCttC展开合并上式，得展开合并上式，得)()(tCttCEbEbCSCSCSCCSCSCSCCYXXZYYXZXZYXZZYXYXZZYXZY)(1)(1)(1222222其中其中200cos1C00sinS*将前式简写为
25、：将前式简写为：CtCttCEbEb)()(或离散形式：或离散形式：CnCnCEbEb)()1(C按按 Cn、Sn 取不同的近似值，形成相应的一阶取不同的近似值，形成相应的一阶 四阶算法四阶算法一阶算法：一阶算法：bibEbEbInCnC)()1(111)(XYXZYZEbnC333231232221131211TTTTTTTTTCbE令令可将上述算法解写成可将上述算法解写成矩阵元素的形式：矩阵元素的形式：*XYZXYZXYZXYZXYZXYZnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnTnT)()(
26、)()1()()()()1()()()()1()()()()1()()()()1()()()()1()()()()1()()()()1()()()()1(231333331333232333231313221232321232222232221212211131311131212131211111 一阶增量算法一阶增量算法*当当 Cn、Sn 取取 n=2,3,4 时：时：二阶增量算法：二阶增量算法：2)(21)()1(bibbibEbEbInCnC三阶增量算法：三阶增量算法：220)(21)61()()1(bibbibEbEbInCnC四阶增量算法：四阶增量算法：22020)(2421()61
27、()()1(bibbibEbEbInCnC*2、四元数微分方程的计算：、四元数微分方程的计算：)0(2sin2cos)(0000qItq其中，其中，I 为单位四元数，为单位四元数，如如（6-24）所示：）所示：21ttbdt0000 xyzxzyyzxzyx写成迭代形式：写成迭代形式：)(2sin2cos)1(0000nqInq*设设2cos0C002sinS一阶算法：一阶算法：)(21)1(nqInq*)(21)1(nqInq)(1212121211212121211212121211nqXYZXZYYZXZYX或展开为元素形式：或展开为元素形式：)(21)(21)(21)()1(321nP
28、nPnPnnZYX)(21)(21)(21)()1(3211nPnPnnPnPYZX)(21)(21)(21)()1(3122nPnPnnPnPXZY)(21)(21)(21)()1(2133nPnPnnPnPXYZ*同理，可得二阶算法：同理，可得二阶算法：)(21)81()1(20nqInq三阶算法：三阶算法：)()4821()81()1(2020nqInq四阶算法：四阶算法：)()4821()38481()1(204020nqInq*用一阶用一阶 四阶龙格四阶龙格-库塔积分矩阵和四元数微分方程库塔积分矩阵和四元数微分方程1、一阶龙格、一阶龙格-库塔法库塔法(Runge-Kutta)一个矩阵
29、微分方程一个矩阵微分方程)(),()(ttXftX当初始条件已知，其一阶龙格当初始条件已知，其一阶龙格-库塔的解为库塔的解为:)(),()()(ttXTftXTtX方程的解为初始值加上以初方程的解为初始值加上以初始点斜率为斜率的一个增量始点斜率为斜率的一个增量斜率斜率K的准确度不同，解的的准确度不同，解的精确度也不同精确度也不同*（1）姿态矩阵微分方程）姿态矩阵微分方程EbEiEbibEbEbCCC简化为简化为)()()(ttCtC其一阶龙格其一阶龙格-库塔解：库塔解：)()()()(ttTCtCTtC展开为展开为元素形元素形式：式：)()()()()()()()()()()()()()()(
30、)()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()(231333331333232333231313221232321232222232221212211131311132212131211111ttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTttTttTTtTTtTXYZXYZXYZXYZXYZXYZ与一阶与一阶增量算增量算法一致法一致*（2）四元
31、数微分方程）四元数微分方程qqb32132102/2/2/2/02/2/2/2/02/2/2/2/0PPPPPPxyzxzyyzxzyx或或一阶龙格一阶龙格-库塔解库塔解)()()()(tqtTtqTtqb)()()()()()(2)()(321tPttPttPtTtTtZYX)()()()()()(2)()(3211tPttPtttTtPTtPYZX)()()()()()(2)()(3122tPttPtttTtPTtPXZY)()()()()()(2)()(2133tPttPtttTtPTtPXYZ*2、二阶龙格、二阶龙格-库塔法库塔法对一阶算法适当改进，使平均斜率更准确一些对一阶算法适当改
32、进，使平均斜率更准确一些)(),(1ttXfK1)(TKtXY)(,2TtYfK二阶龙格二阶龙格-库塔算法的解：库塔算法的解：2)()(21KKTtXTtX（1）矩阵微分方程）矩阵微分方程)()()(ttCtC)()(1ttCK1)(TKtCY)(2TtYK*二阶龙格二阶龙格-库塔解：库塔解：2)()(21KKTtCTtC设设333231232221131211YYYYYYYYYY)()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()()(
33、231333331333232333231313221232321232222232221212211131311132212131211111ttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYttTttTTtTTtYXYZXYZXYZXYZXYZXYZ则则*)()()()()()(2)()(312131211111TtYTtYttTttTTtTTtTYZYZ)()()()()()(2)()(113111312121TtYTtYttTttTTtTTtTZ
34、XZX)()()()()()(2)()(211121113131TtYTtYttTttTTtTTtTXYXY)()()()()()(2)()(322232221212TtYTtYttTttTTtTTtTYZYZ)()()()()()(2)()(123212322222TtYTtYttTttTTtTTtTZXZX)()()()()()(2)()(221222123232TtYTtYttTttTTtTTtTXYXY)()()()()()(2)()(332333231313TtYTtYttTttTTtTTtTYZYZ)()()()()()(2)()(133313332323TtYTtYttTttTT
35、tTTtTZXzX)()()()()()(2)()(231323133333TtYTtYttTttTTtTTtTXYXY*（2）四元数微分方程）四元数微分方程)()(1tqtKb1)(TKtqYYTtKb)(2)(2)()(21KKTtqTtq)()()()()()(21)()()()()()(21)()()()()()(21)()()()()()(2121133112321132110tPttPtttKtPttPtttKtPttPtttKtPttPttPtKXYZXZYYZXZYX133312221111100)()()()(TKtPYTKtPYTKtPYTKtY*)()()(21)()()
36、(21)()()(21)()()(2121023310223202132120YTtYTtYTtKYTtYTtYTtKYTtYTtYTtKYTtYTtYTtKXYZXZYYZXZYX)(2)()()(2)()()(2)()()(2)()(2313332212222111112010KKTtPTtPKKTtPTtPKKTtPTtPKKTtTt*3、四阶龙格、四阶龙格-库塔法库塔法)(),()(ttXftX)(),(1ttXfK)2(,2)(12TtTKtXfK)2(,2)(23TtTKtXfK)2(,)(34TtTKtXfK则解则解226)()(4321KKKKTtXTtX（1）矩阵微分方程）矩
37、阵微分方程)()()(ttCtC)()(1ttCK)2(2)(12TtTKtCK)2(2)(23TtTKtCK)()(34TtTKtCK则解则解226)()(4321KKKKTtCTtC*（2）四元数微分方程）四元数微分方程qq)()(1tqtKb2)()2(12TKtqTtKb2)()2(23TKtqTtKb)()(34TKtqTtKb则解则解226)()(4321KKKKTtqTtq)()()()()()(21)()()()()()(21)()()()()()(21)()()()()()(2121133112321132110tPttPtttKtPttPtttKtPttPtttKtPttP
38、ttPtKXYZXZYYZXZYXTKtPATKtPATKtPATKtA2)(2)(2)(2)(133312221111100*)2()2()2(21)2()2()2(21)2()2()2(21)2()2()2(2121023310223202132120ATtATtATtKATtATtATtKATtATtATtKATtATtATtKXYZXZYYZXZYXTKtPBTKtPBTKtPBTKtB2)(2)(2)(2)(233322222111200)2()2()2(21)2()2()2(21)2()2()2(21)2()2()2(2121033310323203132130BTtBTtBTtK
39、BTtBTtBTtKBTtBTtBTtKBTtBTtBTtKXYZXZYYZXZYXTKtPCTKtPCTKtPCTKtC2)(2)(2)(2)(333332223111300*)2()2()2(21)2()2()2(21)2()2()2(21)2()2()2(2121043310423204132140CTtCTtCTtKCTtCTtCTtKCTtCTtCTtKCTtCTtCTtKXYZXZYYZXZYX226)()(226)()(226)()(226)()(43332313334232221222413121111140302010KKKKTtPTtPKKKKTtPTtPKKKKTtPTt
40、PKKKKTtTt*龙格龙格-库塔积分需要用到角速度信息库塔积分需要用到角速度信息)(t)2(Tt)(Tt 而陀螺仪的数字脉冲输出为角增量而陀螺仪的数字脉冲输出为角增量如果采用周期如果采用周期 T 很小，可以近似把角速度看成常值或线性变化很小，可以近似把角速度看成常值或线性变化1、把角速度看成常值，则周期、把角速度看成常值，则周期 T 内内TTttt),()(一阶角速率提取一阶角速率提取2、把角速度看成线性变化、把角速度看成线性变化)(1t则角增量则角增量iittiiiidttt)(),(iittd)(221*221从从 ti 到到 ti+T/2 的角增量：的角增量：TTi81211从从 ti
41、 到到 ti+T 的角增量：的角增量：2221TTi求解，得求解，得)4(121iiT)84(1122iiT因此因此)4(1)(21iiiTt21)2(iiTTt)43(1)(12iiiTTtti 时刻陀螺输出置零时刻陀螺输出置零ti+T/2 时刻陀螺不置零时刻陀螺不置零ti+T 时刻陀螺置零时刻陀螺置零*如果如果 ti+T/2 时刻陀螺输出置零时刻陀螺输出置零从从 ti 到到 ti+T/2 的角增量：的角增量：11ii从从 ti+T/2 到到 ti+T 的角增量：的角增量：212iii代入代入)4(1)(21iiiTt21)2(iiTTt)43(1)(12iiiTTt得得)3(1)(21iiiTt)(1)2(21iiiTTt)3(1)(12iiiTTt*
