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

权利要求书

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；

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

W2z<αA2x<βA2y<θ]]>

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

5Td2sinα≤K22Td2β≤K22Td2θ≤K]]>

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

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

Roll=arctan(-20Alxj,20Alzj)Pitch=arcsin(20Alyj/gz)]]>

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

Roll=atan2(-A2x,A2z)Pitch=arcsin(A2y/gz)]]>

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

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

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

Q=Q1Q2Q3Q4=cosa0sina1cosa2+sina0cosa1sina2cosa0cosa1sina2-sina0sina1cosa2cosa0sina1sina2-sina0cosa1cosa2cosa0cosa1cosa2+sina0sina1sina2]]>

其中：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：

qK=QKQ12+Q22+Q32+Q42(K=1,2,3,4)]]>

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

C=C11C12C13C21C22C23C31C32C33]]>

其中：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₀₂，并送给组合导航模块：

γ0=W2x-C21cos(Lat)Wie-C31sin(Lat)Wieγ1=W2y-C22cos(Lat)Wie-C32sin(Lat)Wieγ2=W2z-C23cos(Lat)Wie-C33sin(Lat)Wiea02=A2y-C33gz]]>

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

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