CN101419080A - 微型捷联惯性测量系统的零速校正方法 - Google Patents

Abstract

本发明提供的是一种微型捷联惯性测量系统的零速校正方法。是一种改进的曲线拟合零速校正技术，可以在不增加微型捷联惯性测量系统成本的前提下，提高微型捷联惯性测量系统的位置测量精度。微型捷联惯性测量系统利用陀螺和加速度计输出测量载体的速度和位置。在三个或三个以上停车时间点记录微型捷联惯性测量系统的速度测量误差，利用曲线拟合技术方案得到零速校正时间间隔内的速度误差曲线，再积分得到位置误差修正值，最后与微型捷联惯性测量系统的位置测量值求差后得到修正后的位置坐标。

Description

微型捷联惯性测量系统的零速校正方法

(一)技术领域

本发明涉及的是一种惯性测量的误差控制技术，具体地说是微型捷联惯性测量系统进行位置测量时的校正误差的方法。

(二)背景技术

惯性测量系统由惯性测量组件和导航计算机组成，它实时地测量车辆、直升机的位置信息。由于其测量精度高、具有实时性，并且测量过程中不需要任何外部信息，也不向外辐射能量，自主性强，所以它被广泛地应用于炮兵的定位、定向和导弹发射的初始对准，大地测量，地质勘探以及管道、电缆铺设等多个领域，具有重要的国防意义和巨大的经济效益。

微型捷联惯性测量系统采用微型惯性测量组合MIMU(包括微型陀螺和微型加速度计)作为其惯性测量组件，并采用了专门的集成电路和嵌入式微型导航计算机。因此，微型捷联惯性测量系统继承了MIMU成本低、体积小、高可靠、抗振动、抗冲击的优点。同时，由于微型陀螺比其它类型陀螺零位误差要大，因此，微型捷联惯性测量系统存在着测量误差漂移大的棘手的问题。

由于目前微型陀螺制造工艺水平的限制，微型捷联惯性测量系统单纯地采用捷联惯性导航算法时，精度是难以提高的。因此，采用零速校正技术来控制误差增长。零速校正载体停车时捷联惯性测量系统的速度误差作为观测量，进而对位置误差进行校正。零速校正的方法有较多，如二次曲线拟合、实时卡尔曼滤波、平滑估计等。二次曲线拟合方法简单，但精度较差。实时卡尔曼滤波应用于零速校正时，校正的精度较高，但方位误差角在停车修正期间难以估计。并且卡尔曼滤波时状态量较多，矩阵计算量大，这会占用大量的导航计算机资源，对导航计算机的性能要求较高。然而采用高性能、计算主频高的导航计算机会增加整个微型捷联惯性测量系统的成本，给工程中的广泛使用带来问题。

(三)发明内容

本发明的目的在于提供一种能够在不增加微型捷联惯性测量系统成本的前提下，提高微型捷联惯性测量系统的位置测量精度的微型捷联惯性测量系统的零速校正方法。

本发明的目的是这样实现的：

步骤1：通过外部设备确定载体的初始位置参数与初始速度值；

步骤2：微型捷联惯性测量系统进行初始对准，确定载体相对导航坐标系的初始姿态，得到姿态四元数的初始值；

步骤3：载体按照预先设定的路径进行机动，在更新周期H＝t_m-t_m-1内，采集微型陀螺输出的载体相对于惯性坐标系的角速度ω、微型加速度计输出的载体相对于惯性坐标系的比力

步骤4：利用步骤3中微型陀螺输出的载体相对于惯性坐标系的角速度ω进行捷联姿态更新，得到载体坐标系b系相对于导航坐标系n系的捷联矩阵T；

步骤5：利用步骤3获得的载体相对于惯性坐标系的比力步骤4中捷联矩阵T进行速度更新，得到载体的速度值；

步骤6：载体在预先设定的第一个停车时间点t₁停车，此时载体的真实速度为零，微型捷联惯性测量系统的速度输出值即为速度误差δV₁；

步骤7：重复步骤6的过程，在每个停车时间点记录微型捷联惯性测量系统的速度误差，要求停车时间点的个数为三个或三个以上；

步骤8：对量测数据的噪声进行平滑，引入曲线拟合技术，把三个或三个以上停车点的速度误差作为观测量，

δV₁＝a+bcos(υt₁)+csin(υt₁)

δV₂＝a+bcos(υt₂)+csin(υt₂)           (1)

 .             .

 .             .

 .             .

δV_n＝a+bcos(υt_n)+csin(υt_n)

其中 $\mathrm{\υ}=\sqrt{\frac{g}{R}}$ 表示舒拉频率，a，b，c为待定系数，将式(1)写成矩阵的形式为

$Z=H\hat{X}---\left(2\right)$

其中

$\hat{X}={\left[\begin{array}{ccc}a& b& c\end{array}\right]}^T,$ Z＝[δV₁ δV₂…δV_n]^T，

$H=\left[\begin{array}{ccc}1& \cos \left({\mathrm{\υt}}_1\right)& \sin \left(\mathrm{\υ}{t}_1\right)\\ 1& \cos \left({\mathrm{\υt}}_2\right)& \sin \left({\mathrm{\υt}}_2\right)\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ 1& \cos \left({\mathrm{\υt}}_n\right)& \sin \left({\mathrm{\υt}}_n\right)\end{array}\right];$

步骤9：利用最小二乘的方法计算出曲线的系数

$\hat{X}={\left[\begin{array}{ccc}a& b& c\end{array}\right]}^T={\left(H^{T}H\right)}^{-1}{H}^{T}Z---\left(3\right)$

从而拟合出速度误差的曲线方程

δV(t)＝a+bcosυt+csinυt；                  (4)

步骤10：在时间段[0，t_n]内积分速度误差的曲线方程式δV(t)＝a+bcosυt+csinυt得到位置误差修正值，最后与微型捷联惯性测量系统的位置输出值求差后得到修正后的位置坐标：纬度L和经度λ，

$L=\tilde{L}-\frac{{\∫}_0^{t_n}\mathrm{\δV}\left(t\right)\mathrm{dt}}{R}---\left(5\right)$

$\mathrm{\λ}=\widetilde{\mathrm{\λ}}-\frac{{\∫}_0^{t_n}\mathrm{\δV}\left(t\right)\mathrm{dt}}{R\cos L}$

其中R为地球半径。

微型捷联惯性测量系统对于系统的成本有较高的要求。因此，微型捷联惯性测量系统的误差控制通常采用二次曲线拟合零速校正技术方案，降低对于导航计算机计算能力的要求，以便使用低成本的导航计算机。但传统的二次曲线拟合零速校正技术方案精度差。改进的曲线拟合零速校正误差技术方案在拟合微型捷联惯性测量系统速度误差曲线时，考虑了系统误差特性的舒拉误差振荡特性，从而提高了速度误差曲线的拟合精度，更为有效地对微型捷联惯性测量系统的位置误差进行控制。

设定仿真参数：陀螺常值漂移和随机游走分别为0.1°/h和0.01°/sqrt(h)，加速度计零位偏差和噪声分别为10^-4*g和10^-5*g。初始位置为：北纬45.7796°，东经126.6705°。

设置载体的运动轨迹：向东行驶3000s，再沿西偏北45°行驶4000s。此过程中，载体每间隔4min停车1min，进行零速校正。ZUPT点之间的运动特性是相同的，包括加速、匀速、减速三个阶段。比较两种捷联惯性测量系统的定位精度。1号微型捷联惯性测量系统system1(采用二次曲线拟合零速校正技术)；2号微型捷联惯性测量系统system2(采用二改进的曲线拟合零速校正误差技术)。由图3与图4可以看出：由于改进的曲线拟合零速校正误差技术方案在拟合微型捷联惯性测量系统速度误差曲线时，考虑了系统误差特性的舒拉误差振荡特性，从而提高了速度误差曲线的拟合精度。system2的东向定位精度与北向定位精度明显高于system1。

(四)附图说明

图1为本发明的适合于微型捷联惯性测量系统的曲线拟合零速校正技术方案的流程图。

图2为本发明中，微型捷联惯性测量系统的速度测量方案的流程图。

图3与图4为1号微型捷联惯性测量系统system1(采用二次曲线拟合零速校正技术)与2号微型捷联惯性测量系统system2(采用二改进的曲线拟合零速校正误差技术)的测试实验结果。其中横轴表示地理东向、地理北向的位置测量误差值。可以看出：采用二改进的曲线拟合零速校正误差技术的system2定位精度高于system1。

(五)具体实施方式

下面结合附图举例对本发明做更详细地描述：

结合图1，本发明的适合于微型捷联惯性测量系统的一种曲线拟合零速校正误差技术包括如下步骤：

步骤1、通过外部设备确定载体的初始位置参数与初始速度值。

步骤2、微型捷联惯性测量系统进行初始对准，确定载体相对导航坐标系的初始姿态，得到姿态四元数的初始值。

步骤3、载体按照预先设定的路径进行机动。在更新周期H＝t_m-t_m-1内，采集微型陀螺输出的载体相对于惯性坐标系的角速度ω，微型加速度计输出的载体相对于惯性坐标系的比力

步骤4、利用步骤3中微型陀螺输出的载体相对于惯性坐标系的角速度ω进行捷联姿态更新，从而获得载体坐标系b系相对于导航坐标系n系的捷联矩阵T。

步骤5、利用步骤3获得的载体相对于惯性坐标系的比力

步骤4中捷联矩阵T进行速度更新得到载体的速度值。

步骤6、载体在预先设定的第一个停车时间点t₁停车。此时，载体的真实速度为零，微型捷联惯性测量系统的速度输出值即为速度误差δV₁。

步骤7、重复步骤6的过程，在每个停车时间点记录微型捷联惯性测量系统的速度误差，要求停车时间点的个数为三个或三个以上(n≥3)。

步骤8、对量测数据的噪声进行平滑，引入曲线拟合技术，把三个或三个以上停车点的速度误差作为观测量。

δV₁＝a+bcos(υt₁)+csin(υt₁)

δV₂＝a+bcos(υt₂)+csin(υt₂)(1)

 .             .

 .             .

 .             .

δV_n＝a+bcos(υt_n)+csin(υt_n)

其中 $\mathrm{\υ}=\sqrt{\frac{g}{R}}$ 表示舒拉频率，a，b，c为待定系数。将式(1)写成矩阵的形式为

$Z=H\hat{X}---\left(2\right)$

其中

$\hat{X}={\left[\begin{array}{ccc}a& b& c\end{array}\right]}^T,$ Z＝[δV₁ δV₂…δV_n]^T，

$H=\left[\begin{array}{ccc}1& \cos \left({\mathrm{\υt}}_1\right)& \sin \left(\mathrm{\υ}{t}_1\right)\\ 1& \cos \left({\mathrm{\υt}}_2\right)& \sin \left({\mathrm{\υt}}_2\right)\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ 1& \cos \left({\mathrm{\υt}}_n\right)& \sin \left({\mathrm{\υt}}_n\right)\end{array}\right];$

步骤9、利用最小二乘的方法计算出曲线的系数

$\hat{X}={\left[\begin{array}{ccc}a& b& c\end{array}\right]}^T={\left(H^{T}H\right)}^{-1}{H}^{T}Z---\left(3\right)$

从而拟合出速度误差的曲线方程。

δV(t)＝a+bcosυt+csinυt                    (4)

步骤10、在时间段[0，t_n]内积分速度误差的曲线方程式(4)得到位置误差修正值，最后与微型捷联惯性测量系统的位置输出值(纬度输出

和经度输出

)求差后得到修正后的位置坐标(纬度L和经度λ)。

$L=\tilde{L}-\frac{{\∫}_0^{t_n}\mathrm{\δV}\left(t\right)\mathrm{dt}}{R}---\left(5\right)$

$\mathrm{\λ}=\widetilde{\mathrm{\λ}}-\frac{{\∫}_0^{t_n}\mathrm{\δV}\left(t\right)\mathrm{dt}}{R\cos L}$

其中R为地球半径。

所述步骤4中，计算获得载体坐标系b系相对于导航坐标系n系的捷联矩阵T的过程为：

步骤4a、利用步骤3中得到的载体相对于惯性坐标系的角速度ω计算旋转矢量的增量Δφ。

Δφ＝ωH                             (6)

步骤4b、由旋转矢量与四元数的关系，得到姿态更新周期H内姿态更新四元数q(H)。

其中

为旋转矢量增量的模。

步骤4c、由姿态四元数更新方程更新姿态四元数

$Q\left(t_m\right)=Q\left(t_{m-1}\right)\⊕q\left(H\right)---\left(8\right)$

其中姿态更新四元数q(H)由步骤4b求得。Q(t_m)、Q(t_m-1)分别表示载体在t_m、t_m-1时刻的姿态四元数。

步骤4d、利用步骤6获得的t_m时刻姿态四元数Q(t_m)＝[q₀ q₁ q₂ q₃]^T计算载体坐标系b系相对于导航坐标系n系的捷联矩阵T。

$T=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2+{q_3}^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]$

                                                                (9)

结合附图2说明所述步骤5中，获得载体的速度值的过程为：

步骤5a、利用步骤3获得的载体相对于惯性坐标系的比力

与步骤7获得的捷联矩阵T求得

在导航坐标系n系上的投影

fⁿ＝Tf^b                                                        (10)

步骤5b、通过更新周期的初始时刻速度值V(t_m-1)，纬度位置值L(t_m-1)计算载体的牵连角速度ω_en

${\mathrm{\ω}}_{{\mathrm{en}}_y}=-\frac{V_y\left(t_{m-1}\right)}{R}$

${\mathrm{\ω}}_{{\mathrm{en}}_y}=-\frac{V_x\left(t_{m-1}\right)}{R}---\left(11\right)$

${\mathrm{\ω}}_{{\mathrm{en}}_z}=\frac{V_x\left(t_{m-1}\right)}{R}\tan L\left(t_{m-1}\right)$

与地球自转角速度ω_ie。

${\mathrm{\ω}}_{{\mathrm{ie}}_x}=0$

${\mathrm{\ω}}_{{\mathrm{ie}}_y}=\mathrm{\Ω}\cos L\left(t_{m-1}\right)---\left(12\right)$

${\mathrm{\ω}}_{{\mathrm{ie}}_z}=\mathrm{\Ω}\sin L\left(t_{m-1}\right)$

其中R为地球半径，Ω为地球的转速7.27×10^-5/s。

步骤5c、利用步骤5a获得的载体相对于惯性坐标系的比力在导航坐标系n系上的投影f_n，步骤5b获得的载体的牵连角速度ω_en与地球自转角速度ω_ie得到速度的更新值

$V_x\left(H\right)=f_x^n+({2\mathrm{\ω}}_{{\mathrm{ie}}_z}+{\mathrm{\ω}}_{{\mathrm{en}}_z})V_y\left(t_{m-1}\right)-({2\mathrm{\ω}}_{{\mathrm{ie}}_y}+{\mathrm{\ω}}_{{\mathrm{en}}_y})V_z\left(t_{m-1}\right)$

$V_y\left(H\right)=f_y^n-({2\mathrm{\ω}}_{{\mathrm{ie}}_z}+{\mathrm{\ω}}_{{\mathrm{en}}_z})V_x\left(t_{m-1}\right)+({2\mathrm{\ω}}_{{\mathrm{ie}}_x}+{\mathrm{\ω}}_{{\mathrm{en}}_x})V_z\left(t_{m-1}\right)---\left(13\right)$

$V_z\left(H\right)=f_z^n+({2\mathrm{\ω}}_{{\mathrm{ie}}_y}+{\mathrm{\ω}}_{{\mathrm{en}}_y})V_x\left(t_{m-1}\right)-({2\mathrm{\ω}}_{{\mathrm{ie}}_x}+{\mathrm{\ω}}_{{\mathrm{en}}_x})V_y\left(t_{m-1}\right)-g$

步骤5d、利用步骤5c获得的速度的更新值更新计算t_m时刻的速度。

$V\left(t_m\right)=\left[\begin{array}{c}{V}_x\left(t_m\right)\\ V_y\left(t_m\right)\\ V_z\left(t_m\right)\end{array}\right]=\left[\begin{array}{c}{V}_x\left(t_{m-1}\right)+V_x\left(H\right)\\ V_y\left(t_{m-1}\right)+V_y\left(H\right)\\ V_z\left(t_{m-1}\right)+V_z\left(H\right)\end{array}\right]---\left(14\right)$

更新周期的终止时刻的速度值V(t_m)作为下一个更新周期初始时刻的速度值。重复步骤5a到步骤5d的过程，完成对于载体的速度值的更新。

Claims (3)

1、一种微型捷联惯性测量系统的零速校正方法，其特征是：

步骤1：通过外部设备确定载体的初始位置参数与初始速度值；

步骤2：微型捷联惯性测量系统进行初始对准，确定载体相对导航坐标系的初始姿态，得到姿态四元数的初始值；

步骤3：载体按照预先设定的路径进行机动，在更新周期H＝t_m-t_m-1内，采集微型陀螺输出的载体相对于惯性坐标系的角速度ω、微型加速度计输出的载体相对于惯性坐标系的比力f^b；

步骤4：利用步骤3中微型陀螺输出的载体相对于惯性坐标系的角速度ω进行捷联姿态更新，得到载体坐标系b系相对于导航坐标系n系的捷联矩阵T；

步骤5：利用步骤3获得的载体相对于惯性坐标系的比力f^b、步骤4中捷联矩阵T进行速度更新，得到载体的速度值；

步骤6：载体在预先设定的第一个停车时间点t₁停车，此时载体的真实速度为零，微型捷联惯性测量系统的速度输出值即为速度误差δV₁；

步骤7：重复步骤6的过程，在每个停车时间点记录微型捷联惯性测量系统的速度误差，要求停车时间点的个数为三个或三个以上；

步骤8：对量测数据的噪声进行平滑，引入曲线拟合技术，把三个或三个以上停车点的速度误差作为观测量，

δV₁＝a+bcos(υt₁)+csin(υt₁)

δV₂＝a+bcos(υt₂)+csin(υt₂) (1)

 .            .

 .            .

 .            .

δV_n＝a+bcos(υt_n)+csin(υt_n)

其中 $\mathrm{\υ}=\sqrt{\frac{g}{R}}$ 表示舒拉频率，a，b，c为待定系数，将式(1)写成矩阵的形式为

$Z=H\hat{X}---\left(2\right)$

其中

$\hat{X}={\left[\begin{array}{ccc}a& b& c\end{array}\right]}^T,Z={\left[\begin{array}{cccc}{\mathrm{\δV}}_1& {\mathrm{\δV}}_2& \·\·\·& {\mathrm{\δV}}_n\end{array}\right]}^T,$

$H=\left[\begin{array}{ccc}1& \cos \left({\mathrm{\υt}}_1\right)& \sin \left({\mathrm{\υt}}_1\right)\\ 1& \cos \left({\mathrm{\υt}}_2\right)& \sin \left({\mathrm{\υt}}_2\right)\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ 1& \cos \left({\mathrm{\υt}}_n\right)& \sin \left({\mathrm{\υt}}_n\right)\end{array}\right];$

步骤9：利用最小二乘的方法计算出曲线的系数

$\hat{X}={\left[\begin{array}{ccc}a& b& c\end{array}\right]}^T={\left(H^{T}H\right)}^{-1}{H}^{T}Z---\left(3\right)$

从而拟合出速度误差的曲线方程

δV(t)＝a+bcosυt+c sin υt；         (4)

步骤10：在时间段[0，t_n]内积分速度误差的曲线方程式δV(t)＝a+bcosυt+c sin υt得到位置误差修正值，最后与微型捷联惯性测量系统的位置输出值求差后得到修正后的位置坐标：纬度L和经度λ，

$L=\tilde{L}-\frac{{\∫}_0^{t_n}\mathrm{\δV}\left(t\right)\mathrm{dt}}{R}$                         (5)

$\mathrm{\λ}=\widetilde{\mathrm{\λ}}-\frac{{\∫}_0^{t_n}\mathrm{\δV}\left(t\right)\mathrm{dt}}{R\cos L}$

其中R为地球半径。

2、根据权利要求1所述的微型捷联惯性测量系统的零速校正方法，其特征是：

所述步骤4中，计算获得载体坐标系b系相对于导航坐标系n系的捷联矩阵T的过程为：

步骤4a：利用步骤3中得到的载体相对于惯性坐标系的角速度ω计算旋转矢量的增量Δφ，

Δφ＝ωH；                           (6)

步骤4b：由旋转矢量与四元数的关系，得到姿态更新周期H内姿态更新四元数q(H)，

其中

为旋转矢量增量的模，

步骤4c：由姿态四元数更新方程更新姿态四元数

$Q\left(t_m\right)=Q\left(t_{m-1}\right)\⊗q\left(H\right)---\left(8\right)$

其中姿态更新四元数q(H)由步骤4b求得，Q(t_m)、Q(t_m-1)分别表示载体在t_m、t_m-1时刻的姿态四元数；

步骤4d：利用步骤6获得的t_m时刻姿态四元数Q(t_m)＝[q₀ q₁ q₂ q₃]^T计算载体坐标系b系相对于导航坐标系n系的捷联矩阵T，

$T=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2+{q_3}^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]$

                                 (9)。

3、根据权利要求1或2所述的微型捷联惯性测量系统的零速校正方法，其特征是：所述步骤5中，获得载体的速度值的过程为：

步骤5a：利用步骤3获得的载体相对于惯性坐标系的比力f^b与步骤7获得的捷联矩阵T求得f^b在导航坐标系n系上的投影fⁿ

fⁿ＝Tf^b；                        (10)

步骤5b：通过更新周期的初始时刻速度值V(t_m-1)，纬度位置值L(t_m-1)计算载体的牵连角速度ω_en

${\mathrm{\ω}}_{{\mathrm{en}}_x}=-\frac{V_y\left(t_{m-1}\right)}{R}$

${\mathrm{\ω}}_{{\mathrm{en}}_y}=\frac{V_x\left(t_{m-1}\right)}{R}---\left(11\right)$

${\mathrm{\ω}}_{{\mathrm{en}}_z}=\frac{V_x\left(t_{m-1}\right)}{R}\tan L\left(t_{m-1}\right)$

与地球自转角速度ω_ie

${\mathrm{\ω}}_{{\mathrm{ie}}_x}=0$

${\mathrm{\ω}}_{{\mathrm{ie}}_y}=\mathrm{\Ω}\cos L\left(t_{m-1}\right)---\left(12\right)$

${\mathrm{\ω}}_{{\mathrm{ie}}_z}=\mathrm{\Ω}\sin L\left(t_{m-1}\right)$

其中R为地球半径，Ω为地球的转速7.27×10^-5/s；

步骤5c：利用步骤5a获得的载体相对于惯性坐标系的比力在导航坐标系n系上的投影fⁿ，步骤5b获得的载体的牵连角速度ω_en与地球自转角速度ω_ie得到速度的更新值

$V_x\left(H\right)=f_x^n+({2\mathrm{\ω}}_{{\mathrm{ie}}_z}+{\mathrm{\ω}}_{{\mathrm{en}}_z})V_y\left(t_{m-1}\right)-({2\mathrm{\ω}}_{{\mathrm{ie}}_y}+{\mathrm{\ω}}_{{\mathrm{en}}_y})V_z\left(t_{m-1}\right)$

$V_y\left(H\right)=f_y^n+({2\mathrm{\ω}}_{{\mathrm{ie}}_z}+{\mathrm{\ω}}_{{\mathrm{en}}_z})V_x\left(t_{m-1}\right)+({2\mathrm{\ω}}_{{\mathrm{ie}}_x}+{\mathrm{\ω}}_{{\mathrm{en}}_x})V_z\left(t_{m-1}\right);---\left(13\right)$

$V_z\left(H\right)=f_z^n+({2\mathrm{\ω}}_{{\mathrm{ie}}_y}+{\mathrm{\ω}}_{{\mathrm{en}}_y})V_x\left(t_{m-1}\right)-({2\mathrm{\ω}}_{{\mathrm{ie}}_x}+{\mathrm{\ω}}_{{\mathrm{en}}_x})V_y\left(t_{m-1}\right)-g$

步骤5d：利用步骤5c获得的速度的更新值更新计算t_m时刻的速度

$V\left(t_m\right)=\left[\begin{array}{c}{V}_x\left(t_m\right)\\ V_y\left(t_m\right)\\ V_z\left(t_m\right)\end{array}\right]=\left[\begin{array}{c}{V}_x\left(t_{m-1}\right)+V_x\left(H\right)\\ V_y\left(t_{m-1}\right)+V_y\left(H\right)\\ V_z\left(t_{m-1}\right)+V_z\left(H\right)\end{array}\right]---\left(14\right)$

更新周期的终止时刻的速度值V(t_m)作为下一个更新周期初始时刻的速度值，重复步骤5a到步骤5d的过程，完成对于载体的速度值的更新。

Images