[发明专利]一种适用于高动态翻滚再入的助推器导航定位解算方法

权利要求书

1.一种适用于高动态翻滚再入的助推器导航定位解算方法，其特征在于步骤如下：

(1)根据优化后的三子样等效旋转矢量法，建立高动态姿态解算方程，步骤如下：

(1.1)设动坐标系Ox_ty_tz_t在起始时刻t₀与参考坐标系Ox₀y₀z₀重合，记为Ox₀y₀z₀，动坐标系在起始位置0时刻依次绕三个轴转动历经时间t而到达Ox_ty_tz_t的位置；历经时间t转动的角度为一组欧拉角；将动坐标系在起始位置依次绕三个轴转动等效为通过绕某一瞬时轴转过一定的角度一次到达t时刻位置Ox_ty_tz_t，记瞬时轴转过的角度为α，沿瞬时轴方向的单位矢量为记等效描述刚体转动的旋转矢量Φ简称为旋转矢量；

(1.2)由四元数微分方程和四元数与等效旋转矢量的转换关系，推导出等效旋转矢量Φ的微分方程，如下：

四元数微分方程为：

四元数与等效旋转矢量的转换关系为：

则等效旋转矢量Φ的微分方程，

式中，Q为姿态四元数；为等效旋转矢量的一阶导数；ω为绕瞬时轴旋转的角速度，在箭体系X、Y、Z轴的三分量分别为ω_x1,ω_y1,ω_z1；

(1.3)根据泰勒展开公式，对步骤(1.2)的等效旋转矢量Φ的微分方程进行泰勒展开，求解T+h时刻的旋转矢量增量Φ(T+h)，如下：

以T时刻为0时刻，以T+h时刻为t时刻，从T时刻到T+h时刻转过的角α，则有Φ(T)＝0，对等效旋转矢量微分方程重复求导得到：

式中，为T时刻的等效旋转矢量Φ的一阶导数，为T时刻的等效旋转矢量Φ的二阶导数，Φ⁽³⁾为T时刻的等效旋转矢量Φ的三阶导数；ω(T)为T时刻绕瞬时轴旋转的角速度，为T时刻绕瞬时轴旋转的角速度的一阶导数，为T时刻绕瞬时轴旋转的角速度的二阶导数；

(1.4)时间间隔(T,T+h)内拟合瞬时轴旋转的角速度ω的输出，根据火箭助推器再入角速度运动形式，确定瞬时轴旋转的角速度ω的拟合系数，即确定三子样旋转矢量的系数X和Y，根据三子样旋转矢量的系数X和Y确定T到T+h时刻的旋转矢量增量Φ(T+h)，步骤如下：

在时间间隔(T,T+h)内用二次曲线，拟合瞬时轴旋转的角速度ω的输出，即

根据步骤(1.3)的泰勒展开有：

式中，a、b、c为展开参数，ω⁽ⁱ⁾(T)为T时刻绕瞬时轴旋转的角速度的高阶导数；

进而有：

根据t＝T+h/3，t＝T+2h/3和t＝T+h时刻的陀螺输出角增量Θ₁,Θ₂,Θ₃，求解a，b和c，X、Y、Z方向三个陀螺的输出角增量分别为Θ₁,Θ₂,Θ₃，记Θ＝Θ₁+Θ₂+Θ₃；

其中，分别为箭体系下X、Y、Z三个轴上陀螺在t时刻输出值的平方；

将求解得到的和赋给a，b和c；

将a，b和c带入中，得到：

Φ(T+h)＝Θ+X(Θ₁×Θ₃)+YΘ₂×(Θ₃-Θ₁)

其中，X、Y为三子样旋转矢量的系数；

(1.5)根据步骤(1.4)T到T+h时刻的旋转矢量增量Φ(T+h)，确定姿态四元数，具体如下：

将旋转矢量增量Φ(T+h)转化为四元数增量q(h)，进而得到姿态四元数增量q(h)，如下：

式中，|Φ(T+h)|为Φ(T+h)矢量的模值；

利用姿态四元数增量q(h)，采用四元数迭代算法，求解姿态四元数；

姿态四元数求取公式，即高动态姿态解算方程，如下：

式中，n为当前解算周期，n-1为前一解算周期；

(2)根据步骤(1)建立的高动态姿态解算方程，建立助推返回段的速度位置导航解算模型；

(3)建立MEMS惯性器件的误差模型，根据步骤(2)的助推返回段的速度位置导航解算模型和助推再入实测数据，对MEMS惯性器件的误差进行辨识，得到修正后的MEMS惯性器件输出结果；

(4)将步骤(3)修正后的MEMS惯性器件输出结果代入步骤(2)的助推返回段的速度位置导航解算模型，实现助推再入过程的导航定位解算。