CN109489690A - 一种适用于高动态翻滚再入的助推器导航定位解算方法 - Google Patents

Abstract

一种适用于高动态翻滚再入的助推器导航定位解算方法，(1)根据优化后的三子样等效旋转矢量法，建立高动态姿态解算方程；(2)根据步骤(1)建立的高动态姿态解算方程，建立助推返回段的速度位置导航解算模型；(3)建立MEMS惯性器件的误差模型，根据步骤(2)的助推返回段的速度位置导航解算模型和助推再入实测数据，对MEMS惯性器件的误差进行辨识，得到修正后的MEMS惯性器件输出结果；(4)将步骤(3)修正后的MEMS惯性器件输出结果代入步骤(2)的助推返回段的速度位置导航解算模型，实现助推再入过程的导航定位解算。该算法具有解算精度高、计算周期短、适应大姿态机动等优点，可为助推器再入段提供准确的导航位置信息。

Description

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

技术领域

本发明涉及一种适用于高动态翻滚再入的助推器导航定位解算方法，属于导航制导与控制技术领域。

背景技术

世界各航天大国及私营航天公司，均在积极开展可重复使用运载火箭的研制及飞行验证。随着火箭回收的结构、动力、控制等关键技术的突破，研制和飞行风险及经济成本已经大幅下降。运载火箭的回收及重复使用不仅可以大大降低发射成本，还能对飞行残骸进行有效的控制，避免造成生命财产损失。传统运载火箭的助推器在工作结束后无控自由坠落地面，重达数吨的残骸从空中坠落地面有巨大破坏力，解决分离体的安全控制问题已经刻不容缓。通过对助推器安全可控回收技术的研究，确认现阶段开展基于伞降技术的助推器落区安全控制及回收是一种快速、经济、有效的方式。

为了实现运载火箭助推器的伞降回收方案，再入飞行段的导航定位解算是影响任务成败的关键技术，需要根据实时解算出的高度位置信息，确定开伞条件。

助推器在下落过程中，姿态变化较为剧烈，最大角速度达到了400°/s，高速旋转条件下，MEMS惯性器件的测量误差进一步被放大，严重影响了导航解算的精度，现有的导航解算算法已不在适用这种高动态再入飞行器。因此亟需开展适用于高动态翻滚再入的助推器导航定位算法研究。

发明内容

本发明解决的技术问题为：克服现有技术不足，提供一种适用于高动态翻滚再入的助推器导航定位解算方法，可以实现高动态翻滚再入助推器的导航解算，具有解算精度高、计算周期短、适应大姿态机动等优点，可为助推器再入段提供准确的导航位置信息。

本发明解决的技术方案为：一种适用于高动态翻滚再入的助推器导航定位解算方法，步骤如下：

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

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

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

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

步骤(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)建立的高动态姿态解算方程，建立助推返回段的速度位置导航解算模型，具体如下：

助推返回段的速度位置导航解算模型，包括：助推器质心速度与位置参数方程、引力计算方程、飞行高度方程和助推器射程计算方程，

优选的助推器质心速度与位置参数方程、引力计算方程、飞行高度方程和助推器射程计算方程建立步骤如下：

(2.1)根据步骤(1)建立的高动态姿态解算方程，建立助推器质心速度与位置参数方程如下：

式中：

为MEMS加表在箭体系下沿X、Y、Z轴的输出值。

V_ax,V_ay,V_az则为助推器质心相对于发射惯性坐标系的速度在发射惯性坐标系下的X轴、Y轴、Z轴的分量；则为助推器质心相对于发射惯性坐标系的加速度在发射惯性坐标系下的X轴、Y轴、Z轴的分量；

为加速度计测量的发惯系下的视加速度在发射惯性坐标系下的X轴、Y轴、Z轴的分量；

g＝[g_ax,g_ay,g_az]^T为沿惯性系的地球引力，g_ax,g_ay,g_az分别为g在发射惯性坐标系下的X轴、Y轴、Z轴的分量；

(2.2)根据助推器质心速度与位置参数方程，建立引力计算方程；

引力计算方程为：

其中：x_a,y_a,z_a则为助推器质心相对于发射惯性坐标系的位置在发射惯性坐标系下的X轴、Y轴、Z轴的分量；R_0x,R_0y,R_0z为发射点地心矢径在发射惯性坐标系下的X轴、Y轴、Z轴的分量；

助推器质心距地心的距离

A₀为发射点方位角，B₀为发射点大地纬度，GM为地球引力系数；φ为箭下点地心纬度；

其中：

R₀为发射点位置的地心矢量R_0x,R_0y,R_0z的模值。

μ₀为发射点地理纬度B₀与地心纬度φ₀之差，即μ₀＝B₀-φ₀。

假设地球为一两轴旋转椭球体，R₀的长度由子午椭球方程得：

其中，

b_e＝a_e(1-α_e)，a_e为地球半长轴，α_e为地球扁率；H₀为发射系下发射点高度初值；

箭下点地心纬度φ计算公式

其中：ω_e为地球自转角速度；ω_ex、ω_ey、ω_ez为地球自转角速度在发射惯性坐标系下的X轴、Y轴、Z轴的分量；

地球自转角速度矢量：

(2.3)根据助推器质心速度与位置参数方程，建立飞行高度方程，如下；

火箭地心矢量在发射惯性坐标系下的投影ra为：

火箭地心矢量在发射坐标系下的投影为：

火箭绝对速度矢量在发射惯性坐标系下的投影为

火箭相对速度矢量在发射坐标系下的投影为

根据已知的r_a，V_a，r，V计算公式由以下矢量计算公式给出：

r＝G_Ar_a

V＝G_AV_a-ω_e×r

G_A为惯性系到发射系转换矩阵；

箭下点地球半径R的计算公式为：

飞行高度计算公式为

H＝r-R

(2.4)根据助推器质心速度与位置参数方程，建立助推器射程计算方程，具体如下：

射程角β的计算公式如下：

射程L＝R₀·β

式中，β为射程角

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

通过对助推器的落地点高度H和落点射程L两个参数来辨识加表的零值误差和陀螺的零值误差Δω_x1,Δω_y1,Δω_z1，采用遗传算法调整MEMS器件的误差和Δω_x1,Δω_y1,Δω_z1，使助推落点时刻的射程和高度与实测值一致。

其中，分别为加表测量误差在箭体坐标系下X轴、Y轴、Z轴的分量，Δω_x1,Δω_y1,Δω_z1分别为陀螺测量误差在箭体坐标系下X轴、Y轴、Z轴的分量；

辨识目标函数J为：

式中，H_c表示助推器落地时刻的海拔高度实测值，L_c为落地时刻的射程实测值，k₁、k₂为加权系数，k₁+k₂＝1

利用更新的值；利用Δω_x1,Δω_y1,Δω_z1更新ω_x1,ω_y1,ω_z1的值，更新方程如下；

式中,为更新后的加表在箭体坐标系下X轴、Y轴、Z轴的分量，为更新后的陀螺在箭体坐标系下X轴、Y轴、Z轴的分量。

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

将更新后的值、更新后的值替换步骤(2)助推返回段的速度位置导航解算模型中值以及步骤(1)中ω_x1,ω_y1,ω_z1值，完成导航解算。

本发明与现有技术相比的优点在于：

(1)本发明提出一种适用于高动态翻滚再入的助推器导航定位解算方法，该算法可以实现高动态翻滚再入助推器的导航解算，具有解算精度高、计算周期短、适应大姿态机动等优点，可为助推器再入段提供准确的导航位置信息。

(2)本发明根据助推器再入角速度运动形式，提出适用于高动态翻滚再入的助推器优化三子样等效旋转矢量法；

(3)本发明根据优化三子样等效旋转矢量法，建立高动态姿态解算方程；

(4)本发明通过建立带约束条件下的误差辨识方程，解决了MEMS惯性器件在高速旋转条件下测量精度低的问题；

(5)本发明可应用于我国各型运载火箭子级回收导航解算，具有解算精度高、计算周期短、适应大姿态机动等优点。

附图说明

图1高动态翻滚再入助推器导航定位算法流程图；

图2助推器发射惯性系下X方向速度图；

图3助推器发射惯性系下Y方向速度图；

图4助推器发射惯性系下Y方向速度图；

图5助推器速度高度曲线图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步详细描述。

本发明一种适用于高动态翻滚再入的助推器导航定位解算方法，(1)根据优化后的三子样等效旋转矢量法，建立高动态姿态解算方程；(2)根据步骤(1)建立的高动态姿态解算方程，建立助推返回段的速度位置导航解算模型；(3)建立MEMS惯性器件的误差模型，根据步骤(2)的助推返回段的速度位置导航解算模型和助推再入实测数据，对MEMS惯性器件的误差进行辨识，得到修正后的MEMS惯性器件输出结果；(4)将步骤(3)修正后的MEMS惯性器件输出结果代入步骤(2)的助推返回段的速度位置导航解算模型，实现助推再入过程的导航定位解算。该算法具有解算精度高、计算周期短、适应大姿态机动等优点，可为助推器再入段提供准确的导航位置信息。

导航解算是根据装在助推器上的MEMS测量元器件，获取助推器飞行过程中的角速度和加速度信息，MEMS测量元器件安装坐标系与箭体系重合。

导航解算所用到的坐标系定义包括发射坐标系O-XYZ、发射惯性坐标系O-X_AY_AZ_A和箭体坐标系O₁-X₁Y₁Z₁，具体定义如下所述：

(1)发射坐标系O-XYZ

坐标系原点与发射点O固连；OX轴在发射点水平面内，指向发射瞄准方向；OY轴垂直于发射点水平面指向上方；OZ轴与OXY平面相垂直并构成右手坐标系。由于发射点O随地球一起自转，所以发射坐标系为一动坐标系。

(2)发射惯性坐标系O-X_AY_AZ_A

火箭起飞瞬时，发射惯性坐标系原点和各轴与发射坐标系原点及各轴相应重合，起飞后在惯性空间定位定向。不考虑地球自转时，发射惯性坐标系和发射坐标系重合。

(3)箭体坐标系O₁-X₁Y₁Z₁

箭体坐标系原点O₁位于整体火箭瞬时质心上；O₁X₁为芯级箭体外壳对称轴，指向箭体头部；O₁Y₁位于火箭纵向对称面内且与O₁X₁轴垂直，指向上为正；O₁Z₁轴垂直于O₁X₁Y₁平面，方向按右手直角坐标系确定。

本发明一种适用于高动态翻滚再入的助推器导航定位解算方法，步骤如下：

(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，将动坐标系在起始位置依次绕三个轴转动等效为通过绕某一瞬时轴(瞬时轴为一空间矢量轴)转过一定的角度一次到达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时刻h＝0，即旋转矢量增量为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为三子样旋转矢量的系数；(三子样旋转矢量的系数是根据角速度为二次抛物线的假设得出的，而实际火箭助推器再入角速度并非真正如此，所以所得系数并不能保证算法漂移最小。三子样优化算法的实质是把助推器再入角速度运动作为检验算法优劣的输入条件，以使助推器再入角速度运动误差达到最小为准则，重新确定三子样旋转矢量的系数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)建立的高动态姿态解算方程，建立助推返回段的速度位置导航解算模型，具体如下：

助推返回段的速度位置导航解算模型，包括：助推器质心速度与位置参数方程、引力计算方程、飞行高度方程和助推器射程计算方程；

(2.1)根据步骤(1)建立的高动态姿态解算方程，建立助推器质心速度与位置参数方程如下：

式中：

为MEMS加表在箭体系下沿X、Y、Z轴的输出值。

V_ax,V_ay,V_az则为助推器质心相对于发射惯性坐标系的速度在发射惯性坐标系下的X轴、Y轴、Z轴的分量；则为助推器质心相对于发射惯性坐标系的加速度在发射惯性坐标系下的X轴、Y轴、Z轴的分量；为加速度计测量的发惯系下的视加速度在发射惯性坐标系下的X轴、Y轴、Z轴的分量；

g＝[g_ax,g_ay,g_az]^T为沿惯性系的地球引力，g_ax,g_ay,g_az分别为g在发射惯性坐标系下的X轴、Y轴、Z轴的分量。

(2.2)根据助推器质心速度与位置参数方程，建立引力计算方程；

引力计算方程为：

其中：x_a,y_a,z_a则为助推器质心相对于发射惯性坐标系的位置在发射惯性坐标系下的X轴、Y轴、Z轴的分量；R_0x,R_0y,R_0z为发射点地心矢径在发射惯性坐标系下的X轴、Y轴、Z轴的分量；

助推器质心距地心的距离

A0为发射点方位角，B0为发射点大地纬度。GM为地球引力系数；φ为箭下点地心纬度；

其中：

R₀为发射点位置的地心矢量R_0x,R_0y,R_0z的模值。

μ₀为发射点地理纬度B₀与地心纬度φ₀之差，即μ₀＝B₀-φ₀。

假设地球为一两轴旋转椭球体，R₀的长度由子午椭球方程得：

其中，

b_e＝a_e(1-α_e)，a_e为地球半长轴，α_e为地球扁率；H₀为发射系下发射点高度初值。

箭下点地心纬度φ计算公式

其中：ω_e为地球自转角速度；ω_ex、ω_ey、ω_ez为地球自转角速度在发射惯性坐标系下的X轴、Y轴、Z轴的分量；

地球自转角速度矢量

(2.3)根据助推器质心速度与位置参数方程，建立飞行高度方程，如下；

火箭地心矢量在发射惯性坐标系下的投影r_a为：

火箭地心矢量在发射坐标系下的投影为：

火箭绝对速度矢量在发射惯性坐标系下的投影为

火箭相对速度矢量在发射坐标系下的投影为

根据已知的r_a，V_a，r，V计算公式由以下矢量计算公式给出：

r＝G_Ar_a

V＝G_AV_a-ω_e×r

G_A为惯性系到发射系转换矩阵；

箭下点地球半径R的计算公式为：

飞行高度计算公式为

H＝r-R

(2.4)根据助推器质心速度与位置参数方程，建立助推器射程计算方程，具体如下：

射程角β的计算公式如下：

射程L＝R₀·β

式中，β为射程角

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

通过对助推器的落地点高度H和落点射程L两个参数来辨识加表的零值误差和陀螺的零值误差Δω_x1,Δω_y1,Δω_z1，采用遗传算法调整MEMS器件的误差和Δω_x1,Δω_y1,Δω_z1，使助推落点时刻的射程和高度与实测值一致。

其中，分别为加表测量误差在箭体坐标系下X轴、Y轴、Z轴的分量，Δω_x1,Δω_y1,Δω_z1分别为陀螺测量误差在箭体坐标系下X轴、Y轴、Z轴的分量；

辨识目标函数J为：

式中，H_c表示助推器落地时刻的海拔高度实测值，L_c为落地时刻的射程实测值，k₁、k₂为加权系数，k₁+k₂＝1

利用更新的值；利用Δω_x1,Δω_y1,Δω_z1更新ω_x1,ω_y1,ω_z1的值，更新方程如下；

式中,为更新后的加表在箭体坐标系下X轴、Y轴、Z轴的分量，为更新后的陀螺在箭体坐标系下X轴、Y轴、Z轴的分量。

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

将更新后的值、更新后的值替换步骤(2)助推返回段的速度位置导航解算模型中值以及步骤(1)中ω_x1,ω_y1,ω_z1值，完成导航解算。

具体实施步骤如下：

根据附图1所示的高动态翻滚再入助推器导航算法流程图，本发明的实施步骤为：

步骤1，建立MEMS误差模型和待辨识参数动态方程；

步骤2，建立优化后的三子样旋转矢量姿态解算方程和助推返回段导航解算模型；

步骤3，将步骤1中的误差模型带入步骤2的导航解算模型中，得到助推返回段的高度H和射程L曲线；

步骤4，根据实测的高度Hc和射程Lc，计算目标函数J；

步骤5，判断J是否是极小值，若是则结束，否则采用遗传算法修正MEMS误差模型，重新开始步骤1-5。

根据上述算法原理，以某型火箭助推返回段为例，将上述优化后的三子样等效旋转矢量方法带入导航解算模型，将辨识出的MEMS误差模型带入，可得到助推器返回段的导航信息，具体导航解算结果见附录图2-图5。图2为助推器在发射惯性系X轴速度测量值与理论值的比对曲线，图3为助推器在发射惯性系Y轴速度测量值与理论值的比对曲线，图4为助推器在发射惯性系Z轴速度测量值与理论值的比对曲线，图5为助推器在返回段的高度和速度与理论值的比对曲线。

由图2、图3、图4和图5所示可以看出，本发明所提出的方法可以适应实现高动态翻滚再入助推器再入过程的导航定位解算，导航定位解算误差在10％左右，可以满足设计任务需求。

本发明提出一种适用于高动态翻滚再入的助推器导航定位解算方法，该算法可以实现高动态翻滚再入助推器的导航解算，具有解算精度高、计算周期短、适应大姿态机动等优点，可为助推器再入段提供准确的导航位置信息。根据助推器再入角速度运动形式，提出适用于高动态翻滚再入的助推器优化三子样等效旋转矢量法；根据优化三子样等效旋转矢量法，建立高动态姿态解算方程；

本发明通过建立带约束条件下的误差辨识方程，解决了MEMS惯性器件在高速旋转条件下测量精度低的问题；可应用于我国各型运载火箭子级回收导航解算，具有解算精度高、计算周期短、适应大姿态机动等优点。

Claims (10)

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

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

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

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

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

2.根据权利要求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)由四元数微分方程和四元数与等效旋转矢量的转换关系，推导出等效旋转矢量Φ的微分方程；

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

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

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

3.根据权利要求1所述的一种适用于高动态翻滚再入的助推器导航定位解算方法，其特征在于：瞬时轴为一空间矢量轴。

4.根据权利要求2所述的一种适用于高动态翻滚再入的助推器导航定位解算方法，其特征在于：(1.2)由四元数微分方程和四元数与等效旋转矢量的转换关系，推导出等效旋转矢量Φ的微分方程，如下：

四元数微分方程为：

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

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

5.根据权利要求2所述的一种适用于高动态翻滚再入的助推器导航定位解算方法，其特征在于：(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时刻绕瞬时轴旋转的角速度的二阶导数。

6.根据权利要求2所述的一种适用于高动态翻滚再入的助推器导航定位解算方法，其特征在于：(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为三子样旋转矢量的系数.

7.根据权利要求2所述的一种适用于高动态翻滚再入的助推器导航定位解算方法，其特征在于：(1.5)根据步骤(1.4)T到T+h时刻的旋转矢量增量Φ(T+h)，确定姿态四元数，具体如下：

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

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

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

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

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

8.根据权利要求1所述的一种适用于高动态翻滚再入的助推器导航定位解算方法，其特征在于：步骤(2)根据步骤(1)建立的高动态姿态解算方程，建立助推返回段的速度位置导航解算模型，助推返回段的速度位置导航解算模型，包括：助推器质心速度与位置参数方程、引力计算方程、飞行高度方程和助推器射程计算方程。

9.根据权利要求1所述的一种适用于高动态翻滚再入的助推器导航定位解算方法，其特征在于：步骤(2)根据步骤(1)建立的高动态姿态解算方程，建立助推返回段的速度位置导航解算模型，具体如下：

(2.1)根据步骤(1)建立的高动态姿态解算方程，建立助推器质心速度与位置参数方程；

(2.2)根据助推器质心速度与位置参数方程，建立引力计算方程；

(2.3)根据助推器质心速度与位置参数方程，建立飞行高度方程；

(2.4)根据助推器质心速度与位置参数方程，建立助推器射程计算方程。

10.根据权利要求1所述的一种适用于高动态翻滚再入的助推器导航定位解算方法，其特征在于：步骤(4)将步骤(3)修正后的MEMS惯性器件输出结果代入步骤(2)的助推返回段的速度位置导航解算模型，实现助推再入过程的导航定位解算，具体如下：

将更新后的值、更新后的值替换步骤(2)助推返回段的速度位置导航解算模型中值以及步骤(1)中ω_x1,ω_y1,ω_z1值，完成导航解算。