[发明专利]空投物资组合导航装置用空中快速对准方法

摘要

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

主权项

一种空投物资组合导航装置用空中快速对准方法，其特征在于：该方法包括以下步骤：第一步：计数器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{\α}\\ {A2}_x\<\mathrm{\β}\\ {A2}_y\<\mathrm{\θ}\end{array}\right.$若不满足，则转入第十四步；若满足，则转入第十五步。α、β、θ的取值需满足下式：$\left\{\begin{array}{c}{{5T}_d}^2\sin \mathrm{\α}\≤K\\ \frac{\sqrt{2}}{2}{T_d}^2\mathrm{\β}\≤K\\ \frac{\sqrt{2}}{2}{T_d}^2\mathrm{\θ}\≤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{\γ}}_0={W2}_x-C_{21}\cos \left(\mathrm{Lat}\right)W_{\mathrm{ie}}-C_{31}\sin \left(\mathrm{Lat}\right)W_{\mathrm{ie}}\\ {\mathrm{\γ}}_1={W2}_y-C_{22}\cos \left(\mathrm{Lat}\right)W_{\mathrm{ie}}-C_{32}\sin \left(\mathrm{Lat}\right)W_{\mathrm{ie}}\\ {\mathrm{\γ}}_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为地球自转角速率。