22捷联惯航系统算法

捷联惯导系统算法1捷联惯导基本算法与误差捷联惯导系统算法概述算法：从惯性仪表输出到导航与控制信息捷联惯导算法的基本内容：一、系统初始化(Initialization)：1、给定飞行器初始位置、速度等2、数学平台的初始对准3、惯性仪表的校准二、惯性仪表误差补偿(Compensation)三、姿态矩阵的计算四、导航计算五、导航控制信息的提取23姿态计算四元数方法cossin22qn对于四元数用四元数q描述刚体的定点转动，即当只关心b系相对于E系的角位置时，可认为b系是有E系经过无中间过程的一次等效旋转形成。而四元数q包含了这种等效旋转的全部信息：矢量n为旋转轴和旋转方向，θ为转过的角度。2222123123132222212321323122221322313122()2()2()2()2()2()EbPPPPPPPPPCPPPPPPPPPPPPPPPPPP姿态计算姿态角方法四、姿态和航向角的计算根据载体和地理坐标系的方向余弦矩阵可确定姿态、航向角coscossincoscossinsinsinsincossincossincoscoscossinsinsincossinsinsincossincossincoscosbEC333231232221131211TTTTTTTTTCbE)(sin131T33231TTtg11121TTtg姿态、航向角真值的判断4姿态计算姿态角方法)270,180(180)180,90(180)90,0()90,0(2/02/000000000002333主主主主真象限TT)270,180(180)180,90(180)0,90(360)90,0(2/302/0000000000001211主主主主真象限TT如利用四元数微分方程求解，则先利用四元数求解结果计算方向余弦矩阵的元素(1-58)：232221211PPPT)(232112PPPT)(213223PPPT222123233PPPT)(223113PPPT56cossin22qn对上式两边求导sincossin22222dddtdtqnn其中EbbEbEbdddCdtdtdtnnnn7由于刚体绕n转动，与刚体固联的b坐标系各轴在旋转过程中分别位于三个不同的圆锥面上，圆锥顶点即为坐标原点，n位于圆锥的对称轴上，即n在b坐标系的三个坐标轴上的投影不变。所以n在b系内观察时始终不变，则有0bddtn又有如下条件存在nnbn0Eddtnnn8sincos2222ddtqn则四元数的微分方程变为求解如下的四元数方程cossin2222nqnncossin2222nnn9根据四元数乘法可知，两个相同的单位四元数相乘，其结果为-1，即sinsin2222nn从而可以求出cossin22222nqnsincos2222ddtqn四元数微分为10从而可以得到如下公式2ddtqnq又有如下条件存在nnbn则可以建立四元数的微分方程12nnbddtqq捷联惯导系统中，陀螺仪测量的角速度是载体坐标系下的表示，需要转化到地理坐标系下11如果两个向量nn和nb是零标量的四元数，则nE和nb间的变换关系可采用四元数乘法表示：nbnqnq则陀螺仪测量的载体坐标系下角速度转换到地理坐标系可以表示为bnnbnbqq由于q是表征刚体旋转的规范化四元数，应用四元数乘法结合律，得1122bbnbnbddtqqqqq12如果将角速度表示为向量形式xbnbyz则可以将四元数微分写成矩阵形式12bnbddtqMq姿态计算四元数精确解由四元数微分方程式：32132102/2/2/2/02/2/2/2/02/2/2/2/0PPPPPPxyzxzyyzxzyx13bbbnnnbibninenC姿态计算四元数精确解)0(2sin2cos)(000qItq其中：21ttbdt0000xyzxzyyzxzyx22220ZYX14姿态计算四元数精确解•由于计算误差、不可交换性误差等影响，计算的四元数会失去规范性，即其范数不再等于1，需对计算的四元数周期地进行规范化处理，其处理公式如下：153210ˆˆˆˆˆqqqqqqii捷联惯导系统误差传播特性16捷联惯导误差传播特性基本特性与平台式惯导系统一致系统误差方程的建立（角度、速度、位置）1、数学平台的误差方程（四元数法）四元数微分方程：bbqq21假设数学平台模拟地理坐标系，因此biEbibbb则)(21biEbibqq——理想情况实际系统中bibmbiEC实际计算的四元数微分方程)(21biECbibmCCqq17四元数误差四元数计算误差表达式设一矢量R相对地理坐标系静止，在地理坐标系内表示为RE，在飞行器坐标系内表示为Rb，用q表示载体坐标系相对地理坐标系的转动。则1qqRRbE或qRqREb1设Rb是准确值，考虑四元数的计算误差，有1'CbCEqRqRCECbqRqR11则111qqRqqRCECE定义1Cqqq——RE、RE’之间的转动四元数为δq18四元数误差02sin2cosΦq21Φ1Cqqq11CCqqqqq)(21biEbibqq)(21biECbibmCCqq11)(21)(21CbiECbibmCbiEbibqqqqq得1112121)(21CbiECCbiECbibmbibqqqqqq19四元数误差1112121)(21CbiECCbiECbibmbibqqqqqqq考虑到bibmbibbibqqEIEbiE1CEiECCbIECqq1qqEibbib1可得1112121CEiECbibqqqqqqq1121CCEiECCqqqqEiECEiEEibqqq212121另有21Φq2Φq代入上式，并忽略二阶小量EiECEiEEib)21()21()21(ΦΦΦΦ20EiECEiEEib)21()21()21(ΦΦΦΦEiECEiEEiEEiECEib22)(ΦΦΦEiEEiEEib——数学平台误差角的矢量表达第一项：TNEEib][第三项：sincoseEeENEiEtgRVRVRV第二项：cossecsin2eEEeENEIERVtgRVRVRVT平台误差角：四元数误差21四元数误差、速度误差展开得到EeEeENRVtgRVRV)cos()sin(NNeEeERVtgRVRV)sin(sinRVRVRVtgRVNeEeEE)cos()cossec(2——数学平台误差角的分量表达式2、速度误差方程因数学平台模拟地理坐标系2eTieeTeTTdVfVgdt22速度误差gVAAriEIr)(因加速度计固联于载体，将上式写在载体坐标系内：brCbrCVAbCbrCbCbiECbgVA)(Im定义速度误差rrCrVVV则])()[(ImbrbbiEbrCbCbiECbIbbrVVAAVbbCgg考虑biEbiEbiECbbbCbrbrbiCVVV设0bbCgg则)()[(ImbrbrbbiEbbiEbIbbrVVAAV])(brbbiEV])()[(ImbrbbiEbrbbiEbIbVVAA23速度误差])()[(ImbrbbiEbrbbiEbIbbrVVAAV转换到地理坐标系：brbbiEbICbCErVqqqAqAqV)[(11Im1])(qVbrbbiE由于bIbIbAAAIm则111ImCbICCbICCbCqAqqAqqAq又有qAqAEIbI1qAqAEIbI1因此11111ImCEICCEICCbCqqAqqqqAqqqAqqAqqAqEIEI11将21Φq代入上式，展开并忽略二阶小量：ErEEiEEIEICbCVAAqAq)(1ImΦErEEiEV)(24速度误差第一项：TNEEIAAAAErEEiEEIEICbCVAAqAq)(1ImΦErEEiEV)(——速度误差方程的矢量表达式TNEEIAAAA第二项：TΦ第三项：sincoseEeENEiEtgRVRVRVsincos0eeETNEEIVVVV25速度误差ErEEiEEIEICbCVAAqAq)(1ImΦErEEiEV)(第四项：cossecsin2eEEeENEIERVtgRVRVRVcossin0eeE26位置误差、系统误差根据经度、纬度的定义RVNsecRVE取小偏差，得RVNtgRVRVEEsecsec4、系统误差方程由数学平台误差角方程、速度误差方程、位置误差方程组成经度误差方程仍可单独考虑静基座条件下，捷联惯导系统误差传播特性与平台式一致27捷联惯导系统模拟仿真28陀螺仪数据发生器陀螺仪的输出bibbib~TzyxHRPRPRPRPRbnbcoscos0sinsin10sincos0cosbenbiebibbnb)(nenniebnbibC29加速度计数据发生器加速度计的输出bbff~Tzyx][nnnennienngVVf)2(nbnbfCf惯导的基本方程为因此，综合以上各式即可得到加速度计的模拟输出bf~30捷联算法仿真研究载体运动过程中，由于受到路面、风浪、气流等条件的影响时常经历不规则的摇摆运动，使用单一的正弦