CN103994777B - 空投物资组合导航装置用空中快速对准方法 - Google Patents

Abstract

本发明公开了一种空投物资组合导航装置用空中快速对准方法，适用于空投物资空投后的快速对准。该方法利用卫星导航模块提供的位置信息、陀螺提供的角速率信息、加速度计提供的加速度信息以及磁航向计提供的航向信息，计算出空投物资载体的俯仰值、横滚值、航向值，进而求出空投物资载体坐标系到导航坐标系的坐标变化矩阵和陀螺漂移值、加速度计的漂移值，并将其送给导航模块。本发明解决了空投物资载体被投放后，组合导航装置在空中加电启动且空投物资载体出现不同程度晃动时的快速对准问题。

Description

空投物资组合导航装置用空中快速对准方法

技术领域

本发明属于导航技术领域，主要涉及一种适用于空中投放物资上组合导航装置的载体坐标系与导航坐标系的对准方法，尤其涉及一种空投物资被投放后组合导航装置在空中加电启动后的快速对准方法。

背景技术

精确控制的空投技术是一种新型投送物资的手段。在军事上，空投保障是实现物资战略、战役机动保障的重要方式，是空降部队及敌后作战部队后续补给的主要手段。尤其在时间紧迫、跨海作战、水陆交通被阻断的战争中，利用空投向前沿部队、敌后部队或救灾部队实施保障，保持部队的生存能力和战斗力，具有特别重要的意义；在民用方面也是救援物资送达的重要方式之一。

传统的空投技术，采用弹道式圆形伞系统，这种空投方法带来了如下问题：(1)高空空投落点精度很差。为保证空投精度，传统空投的高度一般控制在600m左右，这一高度受敌防空火力和空投区域地形的影响很大，给运输机带来更多的威胁；(2)投送平台不能自主控制，投放精度受空投环境的影响较大，特别是复杂地形、复杂气象环境条件下进行空投，投送精度很难保证；(3)空投时所需的降落区面积较大，载荷的落地点散布较大，集结投送装备物资所需的时间长，在山区等地形复杂区域很难实施可靠的投送保障；(4)空投载荷在空中飞行的距离短，空投运输机不得不飞临距离空投区域很近的地方进行空投，这增加了运输机的危险性。

近年来，随着精确制导(特别是卫星导航技术)、惯性导航及先进的控制技术与空投技术的融合，使精确空投成为可能。但是，由于卫星导航数据的更新率较慢、因地形遮挡会产生数据中断现象、且不能够给出载体的姿态信息。因此，载体上仅仅使用卫星导航装置不能够完全满足对投放载体进行控制的需求，需要在空投载体上使用卫星与惯性导航相结合的组合导航装置，组合导航装置需要进行对准后才能进行工作。

南京航空航天大学公开号为CN101256080的中国专利，“卫星/惯性组合导航系统的空中对准方法”，提出了一种卫星/惯性组合导航系统的空中对准方法，包括卫星导航接收机定位数据采集；卫星导航定位数据缓存；惯性测量单元数据采集；飞行器运动加速度计算；导航坐标系下三维比力计算；俯仰角和横滚角三角方程的快速数据求解；初始化捷联惯性导航系统的导航参数等步骤，可实现对载体动态飞行中的捷联惯性导航系统空中初始对准，解决飞机、导弹等运动载体中捷联惯性导航系统在空中飞行中的快速对准问题。但是该对准方法中，航向参数需要外界提供，且该对准方法中载体运动方向与载体航向坐标需要保持一致。而在空投过程中，由于安全及电磁兼容等方面的要求，投放物资在空中才能加电启动，因此对准期间没有可用的航向信息，投放物资在空中还会出现晃动等情况，因此这种对准方法不能适用于精确空投用组合导航系统的对准需求。

发明内容

本发明要解决的技术问题是，为克服现有技术的不足，为精确空投用组合导航系统提供一种空中快速对准方法。

为解决上述技术问题，本发明提供的空中快速对准方法包括以下操作步骤：

第一步：计数器j清零，计时器Tim清零，将卫星数据接收标志设置为未完成；

第二步：向三个陀螺、三个加速度计、卫星导航模块以及磁航向计发送启动指令，并启动计时器Tim计时；

第三步：并行处理第四步至第八步、第九步至第十一步；

第四步：连续采集三个陀螺的n个周期的输出数据w_xi、w_yi、w_zi和三个加速度计的n个周期的输出数据a_xi、a_yi、a_zi，i＝1,2,…,n，并令计数器j加1，其中w_xi、w_yi、w_zi分别为空投物资载体的X轴、Y轴、Z轴在空投物资载体坐标系下的角速率，a_xi、a_yi、a_zi分别为空投物资载体的X轴、Y轴、Z轴在空投物资载体坐标系下的加速度，n为采样的周期数量，每个周期的时长为T；

第五步：计算n个周期的空投物资载体的X、Y、Z轴角速率累加值W1_xj、W1_yj、W1_zj、以及上述三轴的空投物资载体的加速度累加值A1_xj、A1_yj、A1_zj；

第六步：判断j×n×T是否大于第一时间参数M。若判断结果为假，则返回第四步；若判断结果为真，则转入第七步；

第七步：对j组空投物资载体的角速率累加值、加速度累加值进行求和运算，分别获得X、Y、Z轴的角速率累加求和值W2_x、W2_y、W2_z以及X、Y、Z轴的加速度累加求和值A2_x、A2_y、A2_z；

第八步：判断卫星数据标志位是否有效。若判断结果为假，则返回第四步，若判断结果为真，则转入第十二步；

第九步：接收卫星导航模块输出的导航数据；

第十步：判断卫星导航模块是否输出导航数据有效标志，若判断结果为假，则返回第九步；若判断结果为真，则转入第十一步；

第十一步：判断计时器Tim是否大于第二时间参数T_d，若判断结果为假，则返回第九步，若判断结果为真，置卫星数据接收标志为完成；

第十二步：根据接收到的导航数据计算空投物资载体当前位置的重力加速度g_z；

第十三步：判断下式是否同时满足：

$\left\{\begin{array}{c}{W2}_z<\mathrm{\&alpha;}\\ {A2}_x<\mathrm{\&beta;}\\ {A2}_y<\mathrm{\&theta;}\end{array}\right.$

若不满足，则转入第十四步；若满足，则转入第十五步，α、β、θ的取值需满足下式：

$\left\{\begin{array}{c}{{5T}_d}^2\sin \mathrm{\&alpha;}\&le;K\\ \frac{\sqrt{2}}{2}{T_d}^2\mathrm{\&beta;}\&le;K\\ \frac{\sqrt{2}}{2}{T_d}^2\mathrm{\&theta;}\&le;K\end{array}\right.$

其中K为空投位置误差，且为空投系统要求的技术指标；

第十四步：根据下式计算空投物资载体的横滚角Roll和俯仰角Pitch：

$\left\{\begin{array}{c}\mathrm{Roll}=\arctan (-{20\mathrm{Al}}_{\mathrm{xj}},20{\mathrm{Al}}_{\mathrm{zj}})\\ \mathrm{Pitch}=\arcsin ({20\mathrm{Al}}_{\mathrm{yj}}/g_z)\end{array}\right.$

第十五步：根据下式计算空投物资载体的横滚角Roll和俯仰角Pitch：

$\left\{\begin{array}{c}\mathrm{Roll}=a\tan 2(-{A2}_x,{A2}_z)\\ \mathrm{Pitch}=\arcsin ({A2}_y/g_z)\end{array}\right.$

第十六步：接收磁传感器输出的空投物资载体的航向角Hdg；

第十七步：采用四元数法解算空投物资载体坐标系OX_bY_bZ_b到导航坐标系OX_gY_gZ_g的坐标变换矩阵，其解算具体步骤如下：

17.1步，对四元数矩阵Q初始化：

$Q=\left[\begin{array}{c}{Q}_1\\ Q_2\\ Q_3\\ Q_4\end{array}\right]=\left[\begin{array}{c}\cos a_0\sin a_1\cos a_2+\sin a_0\cos a_1\sin a_2\\ \cos a_0\cos a_1\sin a_2-\sin a_0\sin a_1\cos a_2\\ {\cos a}_0\sin a_1\sin a_2-{\sin a}_0\cos a_1\cos a_2\\ {\cos a}_0\cos a_1\cos a_2+\sin a_0\sin a_1\sin a_2\end{array}\right]$

其中：cosa₀＝cos(Hdg/2)，cosa₁＝cos(Pitch/2)，cosa₂＝cos(Roll/2)

sina₀＝sin(Hdg/2)，sina₁＝sin(Pitch/2)，sina₂＝sin(Roll/2)

17.2步，对四元数矩阵Q进行规范化处理，得到规范化四元数q_K：

$q_K=\frac{Q_K}{\sqrt{Q_1^2+Q_2^2+Q_3^2+Q_4^2}}(K=\mathrm{1,2,3,4})$

17.3步，根据规范化四元数q_K计算空投物资载体坐标系OX_bY_bZ_b到导航坐标系OX_gY_gZ_g的坐标变换矩阵C：

$C=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

其中：C₁₁＝q₁ ²-q₂ ²-q₃ ²+q₄ ²，C₁₂＝2(q₁q₂-q₃q₄)，C₁₃＝2(q₁q₃+q₂q₄)，

C₂₁＝2(q₁q₂+q₃q₄)，C₂₂＝q₂ ²-q₁ ²-q₃ ²+q₄ ²，C₂₃＝2(q₂q₃-q₁q₄)，

C₃₁＝2(q₁q₃-q₂q₄)，C₃₂＝2(q₂q₃+q₁q₄)，C₃₃＝q₃ ²-q₁ ²-q₂ ²+q₄ ²

17.4步，将空投物资载体坐标系OX_bY_bZ_b到导航坐标系OX_gY_gZ_g的坐标变换矩阵C送给组合导航模块；

第十八步：根据下述公式计算X、Y、Z轴陀螺漂移值γ₀、γ₁、γ₂和Z轴加速度计的漂移值a₀₂，并送给组合导航模块：

$\left\{\begin{array}{c}{\mathrm{\&gamma;}}_0={W2}_x-C_{21}\cos \left(\mathrm{Lat}\right)W_{\mathrm{ie}}-C_{31}\sin \left(\mathrm{Lat}\right)W_{\mathrm{ie}}\\ {\mathrm{\&gamma;}}_1={W2}_y-C_{22}\cos \left(\mathrm{Lat}\right)W_{\mathrm{ie}}-C_{32}\sin \left(\mathrm{Lat}\right)W_{\mathrm{ie}}\\ {\mathrm{\&gamma;}}_2={W2}_z-C_{23}\cos \left(\mathrm{Lat}\right)W_{\mathrm{ie}}-C_{33}\sin \left(\mathrm{Lat}\right)W_{\mathrm{ie}}\\ a_{02}={A2}_y-C_{33}{g}_z\end{array}\right.$

其中，W_ie为地球自转角速率。

本发明的有益效果体现在以下几个方面：

(一)本发明采用组合导航系统在空中加电后，利用卫星导航模块提供的位置信息，以及磁航向计提供的航向信息，给惯性导航单元提供初始对准信息，解决了精确空中投放物资的组合导航系统在投放后加电启动，没有预先可用信息，且空投物资的航向坐标可能与运动方向不一致等情况下，完成导航对准的问题。

(二)本发明通过设定陀螺角速率累加求和值和加速度计加速度累加求和值的判据，将空投物资载体的晃动大小程度进行区分，并针对两种不同的晃动程度分别采用两种不同的计算公式计算出空投载体坐标系到导航坐标系的坐标变换矩阵以及陀螺和加速度计漂移值，从而解决了空投物资载体在投放后出现晃动时，在短时间内完成快速对准的问题。

附图说明

图1是本发明快速对准方法的工作流程图。

图2是导航坐标系与空投物资载体坐标系之间的关系示意图。

具体实施方式

下面结合附图及优选实施例对本发明作进一步的详述。

本发明优选实施例提供的快速对准方法用于装有组合导航装置的空投物资载体。

组合导航装置包括电源控制模块、磁航向计、三个陀螺和三个加速度计、卫星导航模块、对准模块、组合导航模块。电源控制模块的功能是在空投物资载体与机舱脱离后给组合导航装置上电；磁航向计的航向轴与空投物资载体前向轴保持一致，其功能是向组合导航模块和对准模块提供空投物资载体的航向信息；三个陀螺的敏感轴X、Y、Z相互垂直且分别与空投物资载体的右、前、上三个方向平行，其功能是实时向对准模块和组合导航模块提供空投物资载体降落过程中的右、前、上三个方向的角加速度；三个加速度计的敏感轴保持正交并分别与空投物资载体的右、前、上三个方向保持一致，其功能是实时向对准模块和组合导航模块提供空投物资载体降落过程中的右、前、上三个方向的线加速度信息；卫星导航模块的功能是向对准模块和组合导航模块提供空投物资载体当前的经度、纬度、高度及东向、北向速度值；对准模块的功能是，根据卫星导航模块及三个陀螺、三个加速度计的输出信息获得空投物资载体坐标系到导航坐标系的坐标变换矩阵以及三个陀螺和Z轴加速度计的漂移量；组合导航模块的功能是，根据三个陀螺、三个加速度计、卫星导航模块、对准模块的输出信息，进行组合导航计算。不难看出，对准模块是完成本发明的核心，在本实施例中对准模块采用DSP处理电路板。

当空投物资载体被投放出机舱且电源模块给组合导航装置各单体供电后，对准模块按如图1所示的流程执行以下操作步骤。

第一步：进行初始化。初始化的内容包括计数器j清零，计时器Tim清零，将卫星数据接收标志设置为未完成。

第二步：向三个陀螺、三个加速度计、卫星导航模块以及磁航向计发送启动指令，并启动计时器Tim计时。

第三步：并行处理第四步至第八步、第九步至第十一步。

第四步：连续采集三个陀螺的n个周期的输出数据w_xi、w_yi、w_zi和三个加速度计n个周期的输出数据a_xi、a_yi、a_zi，i＝1,2,…,n，并令计数器j加1。其中w_xi、w_yi、w_zi分别为空投物资载体的X轴、Y轴、Z轴在空投物资载体坐标系下的角速率，a_xi、a_yi、a_zi分别为空投物资载体的X轴、Y轴、Z轴在空投物资载体坐标系下的加速度，n为采样的周期数量，每个周期的时长为T。

根据图2所示，空投物资载体坐标系OX_bY_bZ_b规定如下：坐标系的原点O在空投物资载体的几何中心，OX_b为空投物资载体的横轴，并指向空投物资载体右侧，OY_b为空投物资载体的纵轴，并指向空投物资载体的前方，OZ_b与OX_b、OY_b满足右手定则，且沿空投物资载体垂直向上。

陀螺、加速度计的采样周期T与对准模块采用的DSP处理电路的频率有关，为了完成每个周期内的相关运算，DSP处理电路的频率要大于陀螺、加速度计的采样频率，在本实施例中，DSP处理电路的频率为150Mhz，采样周期T取10ms，则能够保证DSP处理电路完成每个周期内的相关运算；采样周期数量n与空投载体要求的最大旋转速度S和姿态误差Δσ有关，其关系是S×n×T≤Δσ。在本实施例中Δσ＝2°，S＝30°/s，n的取值为：n＝5。

第五步：根据下式计算n个周期的空投物资载体角速率的累加值、n个周期的空投物资载体加速度的累加值：

$\begin{array}{c}{W1}_{\mathrm{xj}}=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_{\mathrm{xi}},{W1}_{\mathrm{yj}}=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_{\mathrm{yi}},{W1}_{\mathrm{zj}}=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_{\mathrm{zi}}\\ {A1}_{\mathrm{xj}}=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{a}_{\mathrm{xi}},{A1}_{\mathrm{yj}}=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{a}_{\mathrm{yi}},{A1}_{\mathrm{zj}}=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{a}_{\mathrm{zi}}\end{array}$

其中：W1_xj、W1_yj、W1_zj分别为n个周期空投物资载体的X、Y、Z轴角速率的累加值，A1_xj、A1_yj、A1_zj分别为n个周期空投物资载体的X、Y、Z轴加速度的累加值。

第六步：判断j×n×T是否大于第一时间参数M。若判断结果为假，则返回第四步；若判断结果为真，则转入第七步。

第一时间参数M与卫星导航模块的采样周期一致。在本优选实施例中，卫星导航模块的采样周期为1s，即：M＝1s。

第七步：计算j组的空投物资载体角速率累加求和值、j组空投物资载体的加速度累加求和值，计算公式如下：

$\begin{array}{c}{W2}_x=\underset{l=1}{\overset{j}{\mathrm{\&Sigma;}}}W{1}_{\mathrm{xl}},{W2}_y=\underset{l=1}{\overset{j}{\mathrm{\&Sigma;}}}{W1}_{\mathrm{yl}},{W2}_z=\underset{l=1}{\overset{j}{\mathrm{\&Sigma;}}}{W1}_{\mathrm{zl}}\\ {A2}_x=\underset{l=1}{\overset{j}{\mathrm{\&Sigma;}}}{\mathrm{Al}}_{\mathrm{xl}},{A2}_y=\underset{l=1}{\overset{j}{\mathrm{\&Sigma;}}}{\mathrm{Al}}_{y1},{A2}_z=\underset{l=1}{\overset{j}{\mathrm{\&Sigma;}}}{\mathrm{Al}}_{\mathrm{zl}}\end{array}$

其中：W2_x、W2_y、W2_z分别为j组的空投物资载体X、Y、Z轴角速率累加求和值，A2_x、A2_y、A2_z分别为j组的空投物资载体X、Y、Z轴加速度累加求和值。

第八步：判断卫星数据标志位是否有效。若判断结果为假，则返回第四步；若判断结果为真，则转入第十二步。

第九步：接收卫星导航模块输出的导航数据。导航数据包括当前空投物资载体位置的经度Lon、纬度Lat、高度Alt、运动方向Hdg、东向速度Ve及北向速度Vn；

第十步：判断卫星导航模块是否输出导航数据有效标志，若判断结果为假，则返回第九步；若判断结果为真，则转入第十一步。

第十一步：判断计时器Tim是否大于第二时间参数T_d。若判断结果为假，则返回第九步；若判断结果为真，置卫星数据接收标志为完成。

第二时间参数T_d与空投系统要求的对准时间一致。在本优选实施例中，空投系统要求的对准时间为20s。即：T_d＝20s。

第十二步：计算空投物资载体当前位置的重力加速度g_z，计算公式为：

$g_z=\frac{9.780318[1+0.0053024{\sin }^2\left(\mathrm{Lat}\right)][R_e(1-2e)+3e{R}_e{\sin }^2\left(\mathrm{Lat}\right){]}^2}{{[R_e(1-2e)+3e{R}_e{\sin }^2\left(\mathrm{Lat}\right)+\mathrm{Alt}]}^2}$

其中：R_e为地球的半长轴且R_e＝6378.254km。

第十三步：判断下式是否同时满足：

$\left\{\begin{array}{c}{W2}_z<\mathrm{\&alpha;}\\ {A2}_x<\mathrm{\&beta;}\\ {A2}_y<\mathrm{\&theta;}\end{array}\right.$

若不满足，则转入第十四步，此时表明空投物资载体的晃动程度明显；若满足，则转入第十五步，此时表明空投物资载体的晃动程度不明显。α,β,θ的取值与空投位置误差K以及第二时间参数T_d有关。α、β、θ的取值需满足下式：

$\left\{\begin{array}{c}{{5T}_d}^2\sin \mathrm{\&alpha;}\&le;K\\ \frac{\sqrt{2}}{2}{T_d}^2\mathrm{\&beta;}\&le;K\\ \frac{\sqrt{2}}{2}{T_d}^2\mathrm{\&theta;}\&le;K\end{array}\right.$

本实施例中K＝200m,α、β、θ取：α＝3°/s,β＝0.05m/s²,θ＝0.05m/s²。

第十四步：采用n个周期的空投物资载体角速率累加数据、n个周期加速度累加数据计算空投物资载体的横滚角Roll和俯仰角Pitch：

$\left\{\begin{array}{c}\mathrm{Roll}=\arctan (-{20\mathrm{Al}}_{\mathrm{xj}},20{\mathrm{Al}}_{\mathrm{zj}})\\ \mathrm{Pitch}=\arcsin ({20\mathrm{Al}}_{\mathrm{yj}}/g_z)\end{array}\right.$

第十五步：采用j组空投物资载体角速率累加求和值、j组加速度累加求和值计算空投物资载体的横滚角Roll和俯仰角Pitch：

$\left\{\begin{array}{c}\mathrm{Roll}=a\tan 2(-{A2}_x,{A2}_z)\\ \mathrm{Pitch}=\arcsin ({A2}_y/g_z)\end{array}\right.$

第十六步：接收磁传感器输出的空投物资载体的航向角Hdg。

第十七步：根据已经确定的空投物资载体的航向角Hdg、横滚角Roll、俯仰角Pitch，使用四元数法解算空投物资载体坐标系OX_bY_bZ_b到导航坐标系OX_gY_gZ_g的坐标变换矩阵，其解算具体步骤如下：

17.1步，对四元数矩阵Q初始化：

$Q=\left[\begin{array}{c}{Q}_1\\ Q_2\\ Q_3\\ Q_4\end{array}\right]=\left[\begin{array}{c}\cos a_0\sin a_1\cos a_2+\sin a_0\cos a_1\sin a_2\\ \cos a_0\cos a_1\sin a_2-\sin a_0\sin a_1\cos a_2\\ {\cos a}_0\sin a_1\sin a_2-{\sin a}_0\cos a_1\cos a_2\\ {\cos a}_0\cos a_1\cos a_2+\sin a_0\sin a_1\sin a_2\end{array}\right]$

其中：cosa₀＝cos(Hdg/2)，cosa₁＝cos(Pitch/2)，cosa₂＝cos(Roll/2)

sina₀＝sin(Hdg/2)，sina₁＝sin(Pitch/2)，sina₂＝sin(Roll/2)

17.2步，对四元数矩阵Q进行规范化处理，得到规范化四元数q_K：

$q_K=\frac{Q_K}{\sqrt{Q_1^2+Q_2^2+Q_3^2+Q_4^2}}(K=\mathrm{1,2,3,4})$

17.3步，根据规范化四元数q_K计算空投物资载体坐标系OX_bY_bZ_b到导航坐标系OX_gY_gZ_g的坐标变换矩阵C：

$C=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

其中：C₁₁＝q₁ ²-q₂ ²-q₃ ²+q₄ ²，C₁₂＝2(q₁q₂-q₃q₄)，C₁₃＝2(q₁q₃+q₂q₄)，

C₂₁＝2(q₁q₂+q₃q₄)，C₂₂＝q₂ ²-q₁ ²-q₃ ²+q₄ ²，C₂₃＝2(q₂q₃-q₁q₄)，

C₃₁＝2(q₁q₃-q₂q₄)，C₃₂＝2(q₂q₃+q₁q₄)，C₃₃＝q₃ ²-q₁ ²-q₂ ²+q₄ ²

17.4步，将空投物资载体坐标系OX_bY_bZ_b到导航坐标系OX_gY_gZ_g的坐标变换矩阵C送给组合导航模块。

如图2所示，导航坐标系OX_gY_gZ_g的定义为：坐标系的原点O在空投物资载体的几何中心，OX_g轴在当地水平面内指向东，OY_g轴在当地水平面内指向北，OZ_g轴沿当地地垂线方向并且指向天顶，且与OX_g和OY_g轴组成右手坐标系。

第十八步：根据空投物资载体坐标系OX_bY_bZ_b到导航坐标系OX_gY_gZ_g的坐标变换矩阵C，计算X、Y、Z轴陀螺漂移值γ₀、γ₁、γ₂和Z轴加速度计的漂移值a₀₂，并送给组合导航模块，计算公式如下：

$\left\{\begin{array}{c}{\mathrm{\&gamma;}}_0={W2}_x-C_{21}\cos \left(\mathrm{Lat}\right)W_{\mathrm{ie}}-C_{31}\sin \left(\mathrm{Lat}\right)W_{\mathrm{ie}}\\ {\mathrm{\&gamma;}}_1={W2}_y-C_{22}\cos \left(\mathrm{Lat}\right)W_{\mathrm{ie}}-C_{32}\sin \left(\mathrm{Lat}\right)W_{\mathrm{ie}}\\ {\mathrm{\&gamma;}}_2={W2}_z-C_{23}\cos \left(\mathrm{Lat}\right)W_{\mathrm{ie}}-C_{33}\sin \left(\mathrm{Lat}\right)W_{\mathrm{ie}}\\ a_{02}={A2}_y-C_{33}{g}_z\end{array}\right.$

其中W_ie为地球自转角速率且W_ie＝7.29211e^-5rad/s。

Claims (2)

1.一种空投物资组合导航装置用空中快速对准方法，其特征在于：该方法包括以下步骤：

第一步：计数器j清零，计时器Tim清零，将卫星数据接收标志设置为未完成；

第二步：向三个陀螺、三个加速度计、卫星导航模块以及磁航向计发送启动指令，并启动计时器Tim计时；

第三步：并行处理第四步至第八步、第九步至第十一步；

第四步：连续采集三个陀螺的n个周期的输出数据w_xi、w_yi、w_zi和三个加速度计的n个周期的输出数据a_xi、a_yi、a_zi，i＝1,2,…,n，并令计数器j加1；其中w_xi、w_yi、w_zi分别为空投物资载体的X轴、Y轴、Z轴在空投物资载体坐标系下的角速率，a_xi、a_yi、a_zi分别为空投物资载体的X轴、Y轴、Z轴在空投物资载体坐标系下的加速度，n为采样的周期数量，每个周期的时长为T；

第五步：计算n个周期的空投物资载体的X、Y、Z轴角速率累加值W1_xj、W1_yj、W1_zj、以及上述三轴的空投物资载体加速度累加值A1_xj、A1_yj、A1_zj；

第六步：判断j×n×T是否大于第一时间参数M，若判断结果为假，则返回第四步；若判断结果为真，则转入第七步；

第七步：对j组空投物资载体的角速率累加值、加速度累加值进行求和运算，分别获得X、Y、Z轴的角速率累加求和值W2_x、W2_y、W2_z以及X、Y、Z轴的加速度累加求和值A2_x、A2_y、A2_z；

第八步：判断卫星数据标志位是否有效，若判断结果为假，则返回第四步；若判断结果为真，则转入第十二步；

第九步：接收卫星导航模块输出的导航数据；

第十步：判断卫星导航模块是否输出导航数据有效标志，若判断结果为假，则返回第九步；若判断结果为真，则转入第十一步；

第十一步：判断计时器Tim是否大于第二时间参数T_d，若判断结果为假，则返回第九步，若判断结果为真，置卫星数据接收标志为完成；

第十二步：根据接收到的导航数据计算空投物资载体当前位置的重力加速度g_z；

第十三步：判断下式是否同时满足：

$\left\{\begin{array}{c}W{2}_z<\mathrm{\&alpha;}\\ A{2}_x<\mathrm{\&beta;}\\ A{2}_y<\mathrm{\&theta;}\end{array}\right.$

若不满足，则转入第十四步；若满足，则转入第十五步，α、β、θ的取值需满足下式：

$\left\{\begin{array}{c}5{T_d}^{2}sin\mathrm{\&alpha;}\&le;K\\ \frac{\sqrt{2}}{2}{T_d}^2\mathrm{\&beta;}\&le;K\\ \frac{\sqrt{2}}{2}{T_d}^2\mathrm{\&theta;}\&le;K\end{array}\right.$

其中K为空投位置误差，且为空投系统要求的技术指标；

第十四步：根据下式计算空投物资载体的横滚角Roll和俯仰角Pitch：

$\left\{\begin{array}{c}Roll=arctan(-20A{1}_{xj},20A{1}_{zj})\\ Pitch=arcsin(20A{1}_{yj}/g_z)\end{array}\right.$

第十五步：根据下式计算空投物资载体的横滚角Roll和俯仰角Pitch：

$\left\{\begin{array}{c}Roll=atan2(-A{2}_x,A{2}_z)\\ Pitch=\arcsin (A{2}_y/g_z)\end{array}\right.$

第十六步：接收磁航向计输出的空投物资载体的航向角Hdg；

第十七步：采用四元数法解算空投物资载体坐标系OX_bY_bZ_b到导航坐标系OX_gY_gZ_g的坐标变换矩阵，其解算具体步骤如下：

17.1步，对四元数矩阵Q初始化：

$Q=\left[\begin{array}{c}{Q}_1\\ Q_2\\ Q_3\\ Q_4\end{array}\right]=\left[\begin{array}{c}{\mathrm{cosa}}_0{\mathrm{sina}}_1{\mathrm{cosa}}_2+{\mathrm{sina}}_0{\mathrm{cosa}}_1{\mathrm{sina}}_2\\ {\mathrm{cosa}}_0{\mathrm{cosa}}_1{\mathrm{sina}}_2-{\mathrm{sina}}_0{\mathrm{sina}}_1{\mathrm{cosa}}_2\\ {\mathrm{cosa}}_0{\mathrm{sina}}_1{\mathrm{sina}}_2-{\mathrm{sina}}_0{\mathrm{cosa}}_1{\mathrm{cosa}}_2\\ {\mathrm{cosa}}_0{\mathrm{cosa}}_1{\mathrm{cosa}}_2+{\mathrm{sina}}_0{\mathrm{sina}}_1{\mathrm{sina}}_2\end{array}\right]$

其中：cosa₀＝cos(Hdg/2)，cosa₁＝cos(Pitch/2)，cosa₂＝cos(Roll/2)

sina₀＝sin(Hdg/2)，sina₁＝sin(Pitch/2)，sina₂＝sin(Roll/2)

17.2步，对四元数矩阵Q进行规范化处理，得到规范化四元数q_K：

$q_K=\frac{Q_K}{\sqrt{Q_1^2+Q_2^2+Q_3^2+Q_4^2}},(K=1,2,3,4)$

17.3步，根据规范化四元数q_K计算空投物资载体坐标系OX_bY_bZ_b到导航坐标系OX_gY_gZ_g的坐标变换矩阵C：

$C=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

其中：C₁₁＝q₁ ²-q₂ ²-q₃ ²+q₄ ²，C₁₂＝2(q₁q₂-q₃q₄)，C₁₃＝2(q₁q₃+q₂q₄)，

C₂₁＝2(q₁q₂+q₃q₄)，C₂₂＝q₂ ²-q₁ ²-q₃ ²+q₄ ²，C₂₃＝2(q₂q₃-q₁q₄)，

C₃₁＝2(q₁q₃-q₂q₄)，C₃₂＝2(q₂q₃+q₁q₄)，C₃₃＝q₃ ²-q₁ ²-q₂ ²+q₄ ²

17.4步，将空投物资载体坐标系OX_bY_bZ_b到导航坐标系OX_gY_gZ_g的坐标变换矩阵C送给组合导航模块；

第十八步：根据下述公式计算X、Y、Z轴陀螺漂移值γ₀、γ₁、γ₂和Z轴加速度计的漂移值a₀₂，并送给组合导航模块：

$\left\{\begin{array}{c}{\mathrm{\&gamma;}}_0=W{2}_x-C_{21}\cos \left(Lat\right)W_{ie}-C_{31}\sin \left(Lat\right)W_{ie}\\ {\mathrm{\&gamma;}}_1=W{2}_y-C_{22}\cos \left(Lat\right)W_{ie}-C_{32}\sin \left(Lat\right)W_{ie}\\ {\mathrm{\&gamma;}}_2=W{2}_z-C_{23}\cos \left(Lat\right)W_{ie}-C_{33}\sin \left(Lat\right)W_{ie}\\ a_{02}=A{2}_y-C_{33}{g}_z\end{array}\right.$

其中，W_ie为地球自转角速率。

2.根据权利要求1所述的空投物资组合导航装置用空中快速对准方法，其特征在于：采样周期T＝10ms；采样周期数量n＝5；第一时间参数M＝1s；空投系统对准时间T_d＝20s；空投位置误差K＝200m；α＝3°/s,β＝0.05m/s²,θ＝0.05m/s²。