捷联惯导

捷联惯导数学模型及惯导解算原理坐标系的定义1.地理坐标系（下标为t）——：取载体质心，轴指向东，轴指向北，轴沿垂线指向天。2.导航坐标系（下标为n）——：取载体质心，与重合，与，与相差一个游动方位角。3.平台坐标系（小标为p）——：取载体质心，当惯导系统不存在误差时，平台坐标系与导航坐标系重合；存在误差时平台坐标系相对导航坐标系存在三个姿态失准角，，。4.机体坐标系（下标为b）——：取载体质心，沿载体横轴指向右，沿载体纵轴向前，垂至于，并沿载体指向上。5.地心惯性坐标系——：坐标原点取在地心，6.地球坐标系——：地球坐标系固连在地球的坐标系,它相对于地心惯性坐标系以地球自传角速度以上坐标系间关系见下图tttOXYZnnnOXYZpppOXYZbbbOXYZOtXtYtZOnZtZnYtYnXtXxyzOObXbYbZbbOXYiiiiOXYZiiiiXYXZ轴和轴位于地球赤道面内，轴指向春分点，轴与地球自转轴重合eeeeOXYZ15.04107/ie。小时90eeeeeeOXYXYZ取在地心，，轴位于地球赤道面内轴指向格林威治经线，轴指向东经，轴与地球自转轴重合加速度计陀螺仪姿态矩阵姿态矩阵计算姿态角计算导航计算机控制显示位置速度姿态修正数学平台bibabibpibapib捷联惯导系统程序编排示意图捷联式惯导系统数学模型的推导1.系统的速率地球自转引起地理坐标系转动的三个方向的角速度分量为：其中为地球自转角速度，L为纬度。E，N，U分别表示东北天三个方向飞行器运动引起地理坐标系相对于地球坐标系的三个角速度分量为：0cossintieEtieNietieUieLLietanttNetEmttEetNnttEetUnVRhVRhVLRh接上文将前两式求和得：上式为指北方位系统跟踪角速率。其中：cossintanttNitEmttEitNienttEitUienVRhVLRhVLLRh22123sin1298.2571sinemenRReLeRReLmenttENRReRhVV地球参考椭球子午圈上各点曲率半径地球赤道半径地球椭球度地球参考椭球卯酉圈上各点曲率半径载体飞行高度、地理坐标系下的东向和北向速度方向余弦矩阵（位置矩阵）平台坐标系与地球坐标系转动关系为：其中导航坐标系到平台坐标系的转换矩阵由于姿态角是小角度,则可近似表示为:peCpeppeeepXXYCYZZ111213212223313233sinsincoscossinsinsinsincoscossincoscossincossinsincossinsinsincoscoscoscoscoscossinsinpeCCCLLLCCCCLLLCCCLLLpnC,,xyzpnC111zypnzxyxC确定纬度L和经度利用方向余弦矩阵可确定纬度和经度的真值。先求其主值：又因定义域：1.求纬度的真值L反正弦函数的主值域与L的定义域一致，因此：2.求经度的真值反正切函数的主值域是与的定义域不一致，因此需要在的定义域内确定经度的真值。由：由于在L的定义域内cosL永远为正，所以利用的正负值可确定真值：13313231sintanLCCC主主90,90180,180LLL主90,90113231cossintantancoscosCLCL主31cosC与同号31C主，31313101800,01800,0CCC主主主主主=方向余弦矩阵的即时修正方向余弦矩阵的变化是由平台坐标系相对地球坐标系运动的角速率（又称位置速率）引起的peCpep.CC...111213111213...212223212223...313233313233000ppepUepNppepUepEppepNepECCCCCCCCCCCCCCCCCC位置速率位置速率是由飞行器地速的水平分量引起的，由于平台坐标系与地理坐标系相差一个游动方位角，可得：可写成综合上两式得：pepcossinsincostpEEtpNNVVVVpepcossinsincosptepEepEptepNepN222211cossinsincossincos11sincosppepEmnmnEppNepNmnmnRhRhRhRhVVRhRhRhRh指令角速率pip将上式写成投影分量形式，并结合位置矩阵的形式可得：pppipieep132333cossincoscossinpppipEieepEieepEpppipNieepNieepNpipUieieLCLCLC速度方程惯导基本方程：其中：整理上式可得：.2epieepepVfVg.2epieepepVfVg平台系相对地球的加速度向量加速度计测量的比力向量无明显物理意义，又称有害加速度重力加速度向量...02200202220pppppppEieUepUieNepNEEpppppppNNieUepUieEepENpppppppUUieNepNieEepEUVfVVfVgfVV捷联矩阵（姿态矩阵）实现由机体坐标系到平台坐标系的坐标转换的方向余弦矩阵称为捷联矩阵，用T表示；根据捷联矩阵的元素可以确定飞行器姿态角，因此捷联矩阵又称姿态矩阵。满足方程：得到矩阵T后，沿机体坐标系测量的比力就可以转换到平台坐标系上，得到：pbCpbpbbpXXYTYZZ111213212223313233coscossinsinsincossincoscoscossinsincossinsinsincoscoscossinsincossincossincossincoscosGGGGGGGGGGTTTTTTTTTT,GGG格网航向角，俯仰角，倾斜角＝取＝0,所以＝pbfTf由姿态矩阵T确定飞行器姿态角根据矩阵T中的元素可以确定各角的主值：再根据定义域与T中元素的符号得到各角真值：1321313311222sintantanGTTTTT主主主＝＝333333222222,0180,00180,00,0,0360,0,0180,0GGGGGGTTTTTT主主主主主主主主主主主时且且高度及重力加速度g的计算1.捷联式惯导系统的高度通道是发散的，需要引入外部高度信息进行阻尼。2.重力加速度g的计算其中h为载体飞行高度26339.78030.0517990.9411410gCh用四元数法对捷联矩阵进行即时修正机体坐标系相对平台坐标系的转动可用转动四元数Q来表示：从而四元数的微分方程为将上式写成矩阵形式，得：对它求解便可实时地求出0123bbbQqqiqjqk,,,,,bbbbbbijkijk与机体坐标系的基相一致.120EbNbUbQQijk.00.11.22.33001020ENUEUNNUEUNEqqqqqqqq0123,,,qqqq用四元数法对捷联矩阵进行即时修正简称四元数的即时修正32103210000021qqqqqqqqbpbxbpbybpbzbpbxbpbzbpbybpbybpbzbpbxbpbzbpbybpbx姿态速率计算)(piepepbnbibbipbibbpbC四元数的最佳正交化转动四元数变换定理可知：由于计算误差的存在使得：从而归一化条件不再存在，应进行归一化处理，设即时修正获得的四元数为：其模为除以它的模可得：222201231qqqq222201231qqqq^^^^^0123bbbQqqiqjqk^2222^^^^0123QNqqqq^^^^^^^^01232222^^^^0123,bbbiiQQqqiqjqkqQQqNNqqqq四元数最佳正交化由上式四元数的模方可得：显然满足四元数的归一条件，同时也完成了捷联矩阵T的的正交化222201231qqqq222201231203130222221203012323012222130223010123222222qqqqqqqqqqqqTqqqqqqqqqqqqqqqqqqqqqqqq32102322212032101qqqqqqqqqqqq简略的公式四元数初始值的确定000000000100002000030coscoscossinsinsin2222220sincoscoscossinsin2222220coscossinsinsincos2222220cossincossincossin222222qqqq00000000)(21)(2)(2)(2)(21)(2)(2)(2)(21222110322031103223213021203130212322qqqqqqqqqqqqqqqqqqqqqqqqqqqqqqCpb捷联矩阵也可表示为pbC捷联惯导系统程序流程图读取飞行器的初值和加速度计与陀螺的采样值设定计算初值t是否在导航解算周期内给出前一时刻的陀螺与加速度计的值位置速率，地球速率和姿态速率的计算四元数的即时修正Q的最佳归一化矩阵T的计算姿态角计算开始位置矩阵C的计算AA位置矩阵的即时修正位置计算比力的坐标变换速度V的即时修正位置速度的计算g的计算t=t＋导航解算周期输出导航解算的结果结束yesno轨迹发生器的生成导航解算算法是建立在惯性仪表的输出的基础上,为了验证导航解算的正确与否,及后续的组合导航研究,需要设计模拟载体运动的轨迹发生器.轨迹发生器的设计思路是根据假定载体运动轨迹,在特定的坐标系下,采用适当的姿态矩阵算法推导出惯性仪表的理论输出值,同时模拟出载体运动的位置,速度,姿态以及陀螺仪的输出值,加速度计的输出值,再加上相应的随机误差值.捷联惯导初值的给定1.给定初始位置其中:2.给定初始速度其中:3.给定初始姿态角其中:上述初始误差由惯导系统初始对准条件决定.000,,Lh***000000000***000000,,,,LLLhhhLhLh为载体真实初始位置，，为初始