1、捷联惯导数学模型及惯导解算原理捷联惯导数学模型及惯导解算原理坐标系的定义1.地理坐标系（下标为t）：取载体质心，轴指向东，轴指向北，轴沿垂线指向天。2.导航坐标系（下标为n）：取载体质心，与 重合，与 ，与 相差一个游动方位角 。3.平台坐标系（小标为p）：取载体质心，当惯导系统不存在误差时，平台坐标系与导航坐标系重合；存在误差时平台坐标系相对导航坐标系存在三个姿态失准角 ，。4.机体坐标系（下标为b）：取载体质心，沿载体横轴指向右，沿载体纵轴向前，垂至于 ，并沿载体指向上。5.地心惯性坐标系 ：坐标原点取在地心，6.地球坐标系 ：地球坐标系固连在地球的坐标系,它相对于地心惯性坐标系以地球自传
2、角速度以上坐标系间关系见下图t ttOX YZn nnOX Y ZpppOX Y ZbbbOX Y ZOtXtYtZOnZtZnYtYnXtXxyzOObXbYbZbbOX YiiiiO X YZiiiiXYXZ轴和 轴位于地球赤道面内，轴指向春分点，轴与地球自转轴重合eeeeO X Y Z15.04107/ie。小时90eeeeeeOXYXYZ取在地心，轴位于地球赤道面内轴指向格林威治经线，轴指向东经，轴与地球自转轴重合 捷联惯导系统程序编排示意图捷联式惯导系统数学模型的推导1.系统的速率地球自转引起地理坐标系转动的三个方向的角速度分量为：其中 为地球自转角速度，L为纬度。E，N，U分别表示
3、东北天三个方向 飞行器运动引起地理坐标系相对于地球坐标系的三个角速度分量为：0cossintieEtieNietieUieLLietanttNetEmttEetNnttEetUnVRhVRhVLRh 接上文 将前两式求和得：上式为指北方位系统跟踪角速率。其中：cossintanttNitEmttEitNienttEitUienVRhVLRhVLLRh 22123sin1 298.2571sinemenRReLeRReLmenttENRReRhVV地球参考椭球子午圈上各点曲率半径地球赤道半径地球椭球度地球参考椭球卯酉圈上各点曲率半径载体飞行高度、地理坐标系下的东向和北向速度方向余弦矩阵（位置矩阵
4、）平台坐标系与地球坐标系转动关系为：其中导航坐标系到平台坐标系的转换矩阵 由于姿态角 是小角度,则 可近似表示为:peCpeppeeepXXYCYZZ111213212223313233sinsincoscossinsinsin sincoscossincoscossincossinsincossin sinsincoscoscoscos coscos sinsinpeCCCLLLCCCCLLLCCCLLL pnC,xyz pnC111zypnzxyxC 确定纬度L和经度利用方向余弦矩阵可确定纬度和经度的真值。先求其主值：又因定义域：1.求纬度的真值L反正弦函数的主值域与L的定义域一致，因此：
5、2.求经度的真值反正切函数的主值域是 与 的定义域不一致，因此需要在 的定义域内确定经度的真值。由：由于在L的定义域内cosL永远为正，所以利用 的正负值可确定真值 ：13313231sintanLCCC主主90,90180,180L LL主90,90113231cossintantancoscosCLCL主31cosC与同号31C主，31313101800,01800,0CCC主主主主主=方向余弦矩阵 的即时修正方向余弦矩阵的变化是由平台坐标系相对地球坐标系运动的角速率（又称位置速率）引起的peCpep.CC.111213111213.212223212223.313233313233000
6、ppepUepNppepUepEppepNepECCCCCCCCCCCCCCCCCC 位置速率位置速率是由飞行器地速的水平分量引起的，由于平台坐标系与地理坐标系相差一个游动方位角，可得：可写成综合上两式得：pepcossinsincostpEEtpNNVVVVpepcossinsincosptepEepEptepNepN222211cossinsincossincos11sincosppepEmnmnEppNepNmnmnRhRhRhRhVVRhRhRhRh 指令角速率pip 将上式写成投影分量形式，并结合位置矩阵的形式可得：pppipieep132333cossincoscossinpppi
7、pEieepEieepEpppipNieepNieepNpipUieieLCLCLC速度方程惯导基本方程：其中：整理上式可得：.2epieepepVfVg.2epieepepVfVg平台系相对地球的加速度向量加速度计测量的比力向量无明显物理意义，又称有害加速度重力加速度向量.02200202220pppppppEieUepUieNepNEEpppppppNNieUepUieEepENpppppppUUieNepNieEepEUVfVVfVgfVV 捷联矩阵（姿态矩阵）实现由机体坐标系到平台坐标系的坐标转换的方向余弦矩阵称为捷联矩阵，用T表示；根据捷联矩阵的元素可以确定飞行器姿态角，因此捷联矩阵
8、又称姿态矩阵。满足方程：得到矩阵T后，沿机体坐标系测量的比力就可以转换到平台坐标系上，得到：pbCpbpbbpXXYT YZZ111213212223313233cos cossin sin sincos sincos coscos sin sincos sinsin sin coscos cossin sincos sin cossin cossincos cosGGGGGGGGGGTTTTTTTTTT,GGG格网航向角，俯仰角，倾斜角取 0,所以 pbfT f由姿态矩阵T确定飞行器姿态角根据矩阵T中的元素可以确定各角的主值：再根据定义域与T中元素的符号得到各角真值：132131331122
9、2sintantanGTTTTT主主主333333222222,0180,00180,00,0,0360,0,0180,0GGGGGGTTTTTT主主主主主主主主主主主时且且高度及重力加速度g的计算1.捷联式惯导系统的高度通道是发散的，需要引入外部高度信息进行阻尼。2.重力加速度g的计算其中h为载体飞行高度26339.78030.0517990.94114 10gCh用四元数法对捷联矩阵进行即时修正机体坐标系相对平台坐标系的转动可用转动四元数Q来表示：从而四元数的微分方程为将上式写成矩阵形式，得：对它求解便可实时地求出0123bbbQqqiq jq k,bbbbbbij kijk与机体坐标系的
10、基相一致.120E bNbUbQQijk.00.11.22.33001020ENUEUNNUEUNEqqqqqqqq0123,q q q q用四元数法对捷联矩阵进行即时修正简称四元数的即时修正32103210000021qqqqqqqqbpbxbpbybpbzbpbxbpbzbpbybpbybpbzbpbxbpbzbpbybpbx姿态速率计算)(piepepbnbibbipbibbpbC四元数的最佳正交化转动四元数变换定理可知：由于计算误差的存在使得：从而归一化条件不再存在，应进行归一化处理，设即时修正获得的四元数为：其模为除以它的模可得：222201231qqqq222201231qqqq0
11、123bbbQqq iq jq k22220123QNqqqq012322220123,bbbiiQQqq iq jq kqQQqNNqqqq四元数最佳正交化由上式四元数的模方可得：显然满足四元数的归一条件，同时也完成了捷联矩阵T的的正交化222201231qqqq222201231203130222221203012323012222130223010123222222qqqqq qq qq qq qTq qq qqqqqq qq qq qq qq qq qqqqq32102322212032101qqqqqqqqqqqq简略的公式四元数初始值的确定 00000000010000200003
12、0coscoscossinsinsin2222220sincoscoscossinsin2222220coscossinsinsincos2222220cossincossincossin222222qqqq00000000)(21)(2)(2)(2)(21)(2)(2)(2)(21222110322031103223213021203130212322qqqqqqqqqqqqqqqqqqqqqqqqqqqqqqCpb捷联矩阵也可表示为pbC捷联惯导系统程序流程图轨迹发生器的生成 导航解算算法是建立在惯性仪表的输出的基础上,为了验证导航解算的正确与否,及后续的组合导航研究,需要设计模拟载体运动
13、的轨迹发生器.轨迹发生器的设计思路是根据假定载体运动轨迹,在特定的坐标系下,采用适当的姿态矩阵算法推导出惯性仪表的理论输出值,同时模拟出载体运动的位置,速度,姿态以及陀螺仪的输出值,加速度计的输出值,再加上相应的随机误差值.捷联惯导初值的给定1.给定初始位置其中:2.给定初始速度其中:3.给定初始姿态角其中:上述初始误差 由惯导系统初始对准条件决定.000,L h*000000000*000000,LLL hhhL hLh为载体真实初始位置，为初始位置误差000,ENUVVV*000000000*000000,EEENNNUUUENUENUVVVVVVVVVVVVVVV为载体真实初始速度为初始
14、速度误差000，*0000000*00000000，为载体真实初始姿态角，为初始姿态角误差00000000,ENULhVVV0，比力的模拟计算 载体在导航坐标系中的真实速度变化矢量 及载体姿态角的真实变化 按一定规律给出；载体的真实位置初值：真实速度初值：真实姿态初值：均给定，则有：.*,TxyzV V V.*,*000,L h*000,xyzVVV*000,*.*.*.*2sin2cos2sin2cosnxzxxieyiezNNyznxyyiexNMyynzzziexNMVVfVLtgL VLVRhRhV VVfVLtgL VRhRhV VVfVLVgRhRh比力模拟计算上式中：加速度计测量
15、值的模拟计算公式为：是含有加速度计常值测量偏差及服从正态分布的随机噪声。*2*2*ReRe,1sin123 sinMNRReLeeL*,bnbxxxbbnbynyybnbzzzffffCfffff,bbbxyzfff姿态角速度的模拟计算根据给定的 可求出载体相对于导航坐标系的角速度。则陀螺仪测得的载体相对于惯性空间角速度可用下式模拟：.*,*.*.*.*.*cos0sin10000100cossin0sin0cos0sincos0cossincos0sin0100sin0cos00bnbxbnbybnbz.*.*.*cossincoscossin*,cossin,ybbMibxnbxxbbbxibynbynieyNbbzibznbzxieNxyzVRhVCLRhVLtgLRh包含了陀螺常值漂移和服从正太分布的随机漂移
