导航原理_捷联式导航

1、6.6 捷联惯导系统的初始对准捷联惯导系统的初始对准 惯导系统是一种自主式导航系统。它不需惯导系统是一种自主式导航系统。它不需要任何外部信息，只要给定导航的初始条件要任何外部信息，只要给定导航的初始条件（初始速度、位置等），便可根据系统中的惯（初始速度、位置等），便可根据系统中的惯性敏感元件测量的比力和角速率通过计算机实性敏感元件测量的比力和角速率通过计算机实时地计算出各种导航参数。时地计算出各种导航参数。 对于捷联惯导系统，由于捷联矩阵对于捷联惯导系统，由于捷联矩阵T起到起到了平台的作用，因此导航工作一开始就需要获了平台的作用，因此导航工作一开始就需要获得捷联矩阵得捷联矩阵T的初始值，以便完

2、成导航任务。的初始值，以便完成导航任务。捷联惯导系统的初始对准就是确定捷联矩阵的捷联惯导系统的初始对准就是确定捷联矩阵的初始值。初始值。在静基座条件下，捷联惯导系统的加速度计的在静基座条件下，捷联惯导系统的加速度计的输入量为输入量为 ，陀螺的输入量为地球自转角速，陀螺的输入量为地球自转角速度度 。因此。因此 和和 就成为初始对准的基准。就成为初始对准的基准。将陀螺和加速度计的输出采集到计算机中，通将陀螺和加速度计的输出采集到计算机中，通过计算可以求出捷联矩阵过计算可以求出捷联矩阵T的初始值。的初始值。bgbie bgbie 静基座条件下解析式初始对准的过程静基座条件下解析式初始对准的过程初始对

3、准的已知条件：对准地点的经、纬初始对准的已知条件：对准地点的经、纬度度矩阵矩阵 可以通过加速度计与陀螺的测量值可以通过加速度计与陀螺的测量值来计算。在进行初始对准时，当地的经度来计算。在进行初始对准时，当地的经度 和纬度和纬度L是已知的，因此重力加速度是已知的，因此重力加速度 和地和地球自转角速度球自转角速度 在地理坐标系的分量都是确在地理坐标系的分量都是确定的。定的。 tbCbgbie 00txttytzggggg0cossintiexttieieyietiezieLL 由由 和和 构成一个新向量构成一个新向量 ，即，即gie ieg 根据地理坐标系与机体坐标系之间的转换矩根据地理坐标系与机

4、体坐标系之间的转换矩阵阵 可得下列三个公式：可得下列三个公式：btCbbttbbtietiebbttCCCgg（6.6-1） 根据以上三个向量等式可以写出根据以上三个向量等式可以写出9个方程，可个方程，可以求出以求出 的的9个元素。其中个元素。其中 、 分别由加速分别由加速度计和陀螺仪测量得到，而度计和陀螺仪测量得到，而 均可通均可通过计算得到。过计算得到。btCbgbie tttbie、g 具体计算过程具体计算过程将式将式(6.6-1)两边转置，可以得到如下公式：两边转置，可以得到如下公式：()()()()b TtTtbbTtTtieiebb TtTtbCCCgg（6.6-2）1112132

5、12223313233tbTTTTCTTTTTT设设 根据公式（根据公式（6.6-2），可以写出如下计算公），可以写出如下计算公式：式：111213212223313233313233()()00b TtTtbbbbxyzCTTTggggTTTTTTgTgTgT gg通过上式，可以求出通过上式，可以求出 。31T3233、TT同样，通过同样，通过 ，可以求出，可以求出 ，)()bTtTtieiebC 21T2223、TT111213212223313233213122322333)()0cossincossincossincossinbTtTtieiebbbbiexieyiezieieieie

6、ieieieieCTTTLLTTTTTTTLTLTLTLTLTL 由于由于 已经计算出来了，所以根据已经计算出来了，所以根据上式可以计算出上式可以计算出 31T3233、TT21T2223、TT通过通过 ，可以求出，可以求出)()bTtTtbC 11T1213、TT先求先求 和和b t 由于由于 ，所以，所以ieg 000bbbzyiexbbbbbbiezxieybbbyxiezbbbbiezyieyzbbbbiexziezxbbbbieyxiexyggggggggggggg同理同理00000cos000sincos00tttieieieieggLLgL g根据根据 ，可以写出，可以写出 11

7、1213212223313233111213cos00coscoscosbbbbbbbbbbbbiezyieyziexziezxieyxiexyieieieieggggggTTTgLTTTTTTT gLT gLT gL)()bTtTtbC 注意事项：注意事项：1， 可用加速度计的输出可用加速度计的输出 来近似代替，来近似代替， 可由陀螺的可由陀螺的输出输出 来近似代替。公式中对准点来近似代替。公式中对准点的纬度的纬度L和重力加速度和重力加速度g的精确值可作为已知的精确值可作为已知量，量， 也是已知量。也是已知量。由惯导基本方程和静基座条件由惯导基本方程和静基座条件,可得可得 f=-g因此有因此

8、有bbbxyzgggbbbxyzfffbbbiexieyiezbbbibxibyibziebzbzbybybxbxfgfgfg通过上面的计算，可以计算出初始捷联矩阵的通过上面的计算，可以计算出初始捷联矩阵的近似值，也就完成了解析式粗对准。近似的原近似值，也就完成了解析式粗对准。近似的原因主要是陀螺和加速度计有测量误差和漂移、因主要是陀螺和加速度计有测量误差和漂移、载体有扰动运动。载体有扰动运动。经过解析式初对准，实际上我们得到的矩阵是经过解析式初对准，实际上我们得到的矩阵是坐标系坐标系 与理想的坐标系与理想的坐标系 存在着小的误差存在着小的误差角。如果能够求得误差角，那么就可以通过一角。如果能

9、够求得误差角，那么就可以通过一次修正得到精确的初始捷联矩阵。次修正得到精确的初始捷联矩阵。tbCttv设坐标系设坐标系 偏离坐标系偏离坐标系t 的角度为的角度为 ，则，则v修正：修正：tTzyxTtxtytxtztytzttIC111tbTtbTtttbtttbCICCCCC)()(捷联惯导系统初始对准误差传播v与平台式惯导系统的分析方法类似，列出误与平台式惯导系统的分析方法类似，列出误差状态方程，求解特征根，根据特征根分析差状态方程，求解特征根，根据特征根分析误差传播的规律。误差传播的规律。6.7 捷联惯导系统的数学模型捷联惯导系统的数学模型 通过对比欧拉角法、四元数法和方向余通过对比欧拉角

10、法、四元数法和方向余弦法，选择四元数法作为捷联矩阵即时修正弦法，选择四元数法作为捷联矩阵即时修正算法。算法。一、四元数一、四元数Q的即时修正的即时修正0123bbbQqq iq jq k机体坐标系相对平台坐标系的转动四元数为机体坐标系相对平台坐标系的转动四元数为 Q的即时修正可通过解下面的四元数微分方程的即时修正可通过解下面的四元数微分方程来实现：来实现： 00112233001020bbbpbxpbypbzbbbpbxpbzpbybbbpbypbzpbxbbbpbzpbypbxqqqqqqqq二、捷联矩阵的计算二、捷联矩阵的计算根据四元数即时修正求出的根据四元数即时修正求出的 ，根据下式计算

11、捷联矩阵根据下式计算捷联矩阵T：013qqqq、 、 、2222012312031302222212030123230122221302230101232()2()2()2()2()2()qqqqq qq qq qq qTq qq qqqqqq qq qq qq qq qq qqqqq三、四元数三、四元数Q的最佳归一化的最佳归一化Q0123bbbQqqiq jq kQ22220123qqqq012301232222222201230123ooooobbbbbbqq iq jq kQQqq iqjq kqqqqqqqq设即时修正获得的四元数设即时修正获得的四元数为为将四元数将四元数除以它的模除以

12、它的模 得到以漂移误差最小为指标的最佳归一化得到以漂移误差最小为指标的最佳归一化四元数：四元数：四、比力的坐标转换四、比力的坐标转换bfpf 加速度计测量的比力加速度计测量的比力 通过捷联矩通过捷联矩阵阵T可转换为可转换为 ，即，即111213212223313233pbxxpbyypbzzfTTTffTTTffTTTf五、速度五、速度V（即（即 ）的即时修正）的即时修正pV根据惯导基本方程来进行速度的即时修正根据惯导基本方程来进行速度的即时修正00(2)2020(2)(2)20ppppppxxiezepzieyepyppppppyyiezepziexepxppppppzzieyepyiexe

13、pxVfVfVfg对于游移方位系统，对于游移方位系统， ，上式可以简化。，上式可以简化。0pepzpzpypxVVV六、地速计算六、地速计算 通常将飞行器相对地球的运动速度在通常将飞行器相对地球的运动速度在水平面的投影称为地速。可以通过下式计水平面的投影称为地速。可以通过下式计算：算：22()()ppxyVVV七、位置矩阵的即时修正七、位置矩阵的即时修正位置矩阵位置矩阵 可以通过求解下列矩可以通过求解下列矩阵微分方程而得：阵微分方程而得：peCC11121311121321222321222331323331323300000pepypepxppepyepxCCCCCCCCCCCCCCCCCC

14、八、位置速率的计算八、位置速率的计算对于游动方位系统，由于对于游动方位系统，由于 ，因此，因此0pepz1111pPypepxxppepyyxpRVVR式中：式中： )21 (11213233eCeCRRexp)21 (11223233eCeCRReyp132312eeC CR九、地球速率的计算九、地球速率的计算地球速率地球速率 经矩阵经矩阵 转换为转换为 eie peCpie 11121313212223233132333300piexiepieyiepiezieieCCCCCCCCCCCC十、姿态速率的计算十、姿态速率的计算姿态速率姿态速率 可以通过下式计算：可以通过下式计算：bpb 1(

15、)bbbbpppbibipibieepT 十一、姿态角的计算十一、姿态角的计算捷联矩阵又叫做姿态矩阵，它是姿态角捷联矩阵又叫做姿态矩阵，它是姿态角 的函数，所以根据捷联矩阵的元的函数，所以根据捷联矩阵的元素，可以求出姿态角。计算过程如下素，可以求出姿态角。计算过程如下G、 、coscossinsinsincossinsincoscos sinsincos sinsinsincoscoscossinsincos sincossincossincoscosGGGGGGGGGGT根据式捷联矩阵的元素，可以确定根据式捷联矩阵的元素，可以确定 的主值。的主值。G、 、1321313311222sinta

16、ntanTTTTT主主G主G、 、求出主值后，可用如下公式求出求出主值后，可用如下公式求出 的真值：的真值：主3303303301800,01800,0TTT主主主主主 当时当时当时220220220360001800GGTTTG主G主G主主G主 当且0时当且时当时G当确定当确定 后，可以确定飞行器的航向角后，可以确定飞行器的航向角G00360360360当当十二、位置计算十二、位置计算 位置矩阵位置矩阵 可以表示为经纬度和可以表示为经纬度和游移方位角的函数，即游移方位角的函数，即peCLLLLLLLLLCpesinsincoscoscoscoscossinsincoscossincossin

17、cossinsincossinsinsinsincoscoscossinsinsincos2313313233arctanarctanarcsinCCCCCLf主主31C主0180主0180主主23C主主0360主0180主的真值确定的真值确定 -+-+的真值确定的真值确定+-+-纬度的真值就等纬度的真值就等于纬度的主值。于纬度的主值。十三、高度计算十三、高度计算 纯惯性系统的高度通道是不稳定的，纯惯性系统的高度通道是不稳定的，通过引入外部高度信息设计反馈回路使高通过引入外部高度信息设计反馈回路使高度通道稳定。捷联系统的高度通道也是由度通道稳定。捷联系统的高度通道也是由计算机计算出来的。计算机

18、计算出来的。十四、重力加速度十四、重力加速度g的计算的计算 重力加速度不是常数，随高度的变化而变重力加速度不是常数，随高度的变化而变化。计算公式为化。计算公式为02(1)hggR当考虑到地球的椭球度时，重力加速度还与当考虑到地球的椭球度时，重力加速度还与纬度有关。计算公式为纬度有关。计算公式为2629.78030.051799sin0.94114 10(/)gLh m s6.8 初始条件的给定与初始数据的计算初始条件的给定与初始数据的计算 为了进行捷联惯性导航的计算，需为了进行捷联惯性导航的计算，需要事先知道两类数据：一类是开始计算要事先知道两类数据：一类是开始计算时给定的初始条件，另一类是通

19、过计算时给定的初始条件，另一类是通过计算获得的初始数据。获得的初始数据。 一、初始条件的给定一、初始条件的给定飞行器在从静止状态进入飞行状态飞行器在从静止状态进入飞行状态前，需要给定起飞前的初始条件。初始条前，需要给定起飞前的初始条件。初始条件包括：件包括：1，初始位置，初始位置 分别是起飞地点分别是起飞地点的经度和纬度和高度的经度和纬度和高度2，初始速度，对于从静止状态开始，初始速度，对于从静止状态开始起飞的情况可取起飞的情况可取3，初始游移方位角，初始游移方位角通常将初始游移方位角选为通常将初始游移方位角选为 。000Lh、 、0000 xyzVVV00二、初始数据的计算二、初始数据的计算

20、1，捷联矩阵初始值，捷联矩阵初始值 的确定的确定0pbTC当初始游移方位角当初始游移方位角 时，时， 。 可在初始对准中确定。可在初始对准中确定。 00ptbbCCtbC2，初始姿态角的计算，初始姿态角的计算飞行器的初始姿态角可根据初始对准时飞行器的初始姿态角可根据初始对准时获得的获得的 的矩阵元素来进行计算。的矩阵元素来进行计算。由于初始游移方位角选为由于初始游移方位角选为 ，所以，所以 ptbbTCC00G3，初始四元数，初始四元数Q的计算的计算 根据捷联矩阵的初值的对角线元素和四元数根据捷联矩阵的初值的对角线元素和四元数约束方程可得约束方程可得2222012311222201232222

21、22012333222201231qqqqTqqqqTqqqqTqqqq求解上述方程可得如下的绝对值方程求解上述方程可得如下的绝对值方程1112233112qTTT2112233112qTTT3112233112qTTT2220123112qqqq为了确定为了确定 的符号，可先选的符号，可先选 为正，为正，根据捷联矩阵与四元数的关系，可得根据捷联矩阵与四元数的关系，可得0123qqqq、 、 、0q2222012312031302222212030123230122221302230101232()2()2()2()2()2()qqqqq qq qq qq qTq qq qqqqqq qq qq qq qq qq qqqqq0signq 13223()signqsign TT23012301012()2()4q qq qq qq qq q 21331()signqsign T)2()4q qq qq qq qq q 32112()signqsign TT12031203032()2()4q qq qq qq qq q 当当 为负时，可以看出为负时，可以看出 均改变符号。均改变符号。 0q123qqq、 、而当而当 全改变符号时，矩阵全改变符号时，矩阵T保持保持不变不变(对角线上的元素，非对角线上的元素对角线上的元素，非对角线上的元素中各项的乘积都不变