一种基于单轴转停方案的系泊估漂方法

摘要

本发明提供的是一种基于单轴转停方案的系泊估漂方法。通过GPS确定载体的初始位置参数；采集光纤陀螺仪输出和加速度计输出的数据并对数据进行处理；IMU采用8个转停次序为一个旋转周期的转位方案；根据IMU转动状态下陀螺仪的输出对捷联矩阵进行实时更新，同时对采集到的IMU旋转状态下的加速度信息进行坐标转换，转换到数学平台坐标系；根据载体动基座误差模型建立载体系泊状态时的分离位置回路的卡尔曼估漂模型；经过卡尔曼滤波后得到的惯性器件常值偏差采用平均滤波方法进行处理，得到相对稳定的估计平均值。载体处于系泊状态时，利用本发明提供的估漂方法，依据卡尔曼滤波技术可以准确地估计出惯性器件的常值偏差。

说明书

技术领域

本发明涉及的是一种测量方法，尤其涉及的是一种基于单轴转停方案的系泊估漂方法。

背景技术

捷联惯性导航系统SINS是一种完全自主的导航系统，利用陀螺仪和加速度计测量载体相对惯性空间的角运动和线运动参数，在给定初始条件下，由计算机进行积分运算，连续、实时地提供位置、速度和姿态信息。由于SINS完全依靠自身的惯性元件，不依靠任何外界信息测量导航参数，因此具有隐蔽性好，不受气候条件限制、不受干扰等优点，是一种完全自主式、全天候的导航系统，已广泛应用于航空、航天、航海等领域。根据SINS的基本原理，SINS在导航过程中惯性器件常值偏差的存在是导致惯性导航系统定位精度难以提高的主要因素。如何有效地限制惯性导航误差发散、提高惯性导航系统精度是惯性导航领域一项非常重要的课题。

根据SINS的基本原理，SINS在进入导航状态前需要获得初始信息，包括初始位置、速度和姿态。其中初始姿态的精度就是SINS进入导航状态时的初始对准精度，因此在捷联惯导系统进入导航状态前必须首先完成初始姿态的确定。由于“平台”是测量比力的基准，因此“平台”的初始对准就非常重要；在捷联惯性导航系统中捷联矩阵起到数学平台的作用。因此初始对准的关键性问题就是如何在较短的时间内得到较高精度的初始捷联矩阵的初始值。而惯性器件偏差是导致初始捷联矩阵存在偏差的主要因素，因此估漂技术的重要性就得以体现。

旋转调制技术是惯性导航系统的一种自校正方法。它不需要引入外部校正信息，能够自动对系统中惯性器件的常值偏差进行平均，达到抵消漂移对系统精度的影响。因而可以提高惯性导航系统长时间工作的精度，充分发挥惯性导航“自主式”的优点。应用旋转调制技术，还可以应用较低精度的惯性器件，构成较高精度的惯性导航系统，有利于降低惯性导航系统的成本，同时由于引入外界运动可以有效地提高惯导系统部分参数的可观测度。

舰船处于系泊状态时，由于受到海浪因素的影响产生摇摆与荡运动，这些干扰运动产生的干扰加速度和干扰角速度分别被加速度计和陀螺感知，其中干扰加速度经过惯导系统中的积分环节解算出的载体速度中存在着较大的误差，与载体实际线速度进行比较后作为系统滤波模型的观测量。观测量的不准确将导致系统在进行估计的时候会降低参数的收敛速度同时降低参数的估计精度。

在CNKI数据库中与本发明相关的公开报道有：1、《旋转光纤捷联惯性导航系统的误差调制分析》，提出一种带连续稳定旋转平台的新型光纤捷联惯性导航系统，旨在提高系统导航精度的同时，不增加惯导系统的成本。研究该惯导系统的组成结构和工作原理，分析旋转光纤捷联惯性导航系统误差调制过程，最后对整套惯性导航系统进行仿真。2、《利用旋转调制技术实现快速高精度方位对准》，提出了研制旋转调制系统的技术指标。但都没有提出与本发明相关的调制方法及估漂方法。

发明内容

本发明的目的在于提供一种载体处于系泊状态时，可以准确地估计出惯性器件的常值偏差的一种基于单轴转停方案的系泊估漂方法。

本发明的目的是这样实现的：包括以下步骤：

(1)通过GPS确定载体的初始位置参数，并装订至导航计算机中；

(2)捷联惯导系统进行预热准备，采集光纤陀螺仪和石英加速度计输出的数据并对数据进行处理；

(3)惯性测量单元(IMU)采用8个转停次序为一个旋转周期的转位方案；

(4)根据旋转状态下陀螺仪的输出确定出捷联矩阵T_s^p，并将加速度计的输出转换到数学平台坐标系；

(5)根据惯导系统动基座误差方程建立载体系泊状态时的估漂模型，以GPS提供的载体速度v₀为基准，并与惯导系统结算速度的差值作为观测量，利用卡尔曼滤波技术实现惯性器件偏差的估计；

(6)将卡尔曼滤波估计出的惯性器件常值偏差采用平均滤波的方法将其进行平滑，滤除小幅度波动的影响。

本发明还可以包括：

1、所述IMU采用8个转停次序为一个旋转周期的转位方案为：次序1，IMU从A点出发顺时针转动180°到达位置C，停止时间T_t；次序2，IMU从C点出发顺时针转动90°到达位置D，停止时间T_t；次序3，IMU从D点出发逆时针转动180°到达位置B，停止时间T_t；次序4，IMU从B点出发逆时针转动90°到达位置A，停止时间T_t；次序5，IMU从A点出发逆时针转动180°到达位置C，停止时间T_t；次序6，IMU从C点出发逆时针转动90°到达位置B，停止时间T_t；次序7，IMU从B点出发顺时针转动180°到达位置D，停止时间T_t；次序8，IMU从D点出发顺时针转动90°到达位置A，停止时间T_t；IMU按照此转动顺序循环进行；其中水平东向轴上的IMU停顿点p3、p8与p4、p7对称于转轴中心；北向轴上的停顿点p1、p5与p2、p6对称于转轴中心。

2、所述根据旋转状态下陀螺仪的输出确定出捷联矩阵T_s^p，并将加速度计的输出转换到数学平台坐标系的方法为：

根据四阶龙格库塔法，应用四元数对惯导系统的捷联矩阵进行实时更新，其公式表示为：

${\stackrel{·}{T}}_s^p=T_s^p[\times {\omega }_{\mathrm{is}}^s]$

其中ω_is^s表示陀螺仪输出；利用IMU坐标系与数学平台坐标系的转换关系，计算出数学平台坐标系下的加速度值，

3、所述根据惯导系统动基座误差方程建立载体系泊状态时的估漂模型，以GPS提供的载体速度v₀为基准，并与惯导系统结算速度的差值作为观测量，利用卡尔曼滤波技术实现惯性器件偏差的估计的方法为：

采用分离载体位置信息的误差模型作为系统的估漂模型，建立以速度误差为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程；

1)建立卡尔曼滤波的状态方程：

用一阶线性微分方程来描述旋转捷联惯导系统的状态误差：

$\stackrel{·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)$

其中X(t)为t时刻系统的状态向量；A(t)和B(t)分别为系统的状态矩阵和噪声矩阵；W(t)为系统噪声向量；

系统的状态向量为：

系统的白噪声向量为：

W＝[a_x a_y ω_x ω_y ω_z 0 0 0 0 0]^T

其中δV_e、δV_n分别表示东向、北向的速度误差；分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；ω_x、ω_y、ω_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的白噪声误差；

系统的状态转移矩阵为：

$A\left(t\right)=\left(\begin{array}{cc}{F}_{5\times 5}& T_{5\times 5}\\ 0_{5\times 5}& 0_{5\times 5}\end{array}\right)$

其中：

$F_{5\times 5}=\left(\begin{array}{cc}{F}_{2\times 2}& F_{2\times 3}\\ F_{3\times 2}& F_{3\times 3}\end{array}\right)$

$F_{2\times 2}=\left(\begin{array}{cc}\frac{V_n\tan L}{R_n}& {2\omega }_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}\\ -2({\omega }_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0\end{array}\right)$

$F_{2\times 3}=\left(\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\end{array}\right)$

$F_{3\times 2}=\left(\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{R_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right)$

$F_{3\times 3}=\left(\begin{array}{ccc}0& {\omega }_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}& -({\omega }_{\mathrm{ie}}\cos L+\frac{V_e}{R_n})\\ -({\omega }_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0& -\frac{V_n}{R_m}\\ {\omega }_{\mathrm{ie}}\cos L+\frac{V_e}{R_n}& \frac{V_n}{R_m}& 0\end{array}\right)$

V_e、V_n分别表示东向、北向的速度；ω_ie表示地球自转角速度；R_m、R_n分别表示地球子午、卯酉曲率半径；L表示当地纬度；f_e、f_n、f_u分别表示为导航坐标系下东向、北向、天向的比力；

捷联矩阵T_s^p为：

$T_s^p=\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)$

则

$T_{5\times 5}=\left(\begin{array}{ccccc}{T}_{11}& T_{12}& 0& 0& 0\\ T_{21}& T_{22}& 0& 0& 0\\ 0& 0& -T_{11}& -T_{12}& -T_{13}\\ 0& 0& -T_{21}& {-T}_{22}& -T_{23}\\ 0& 0& -T_{31}& -T_{32}& -T_{33}\end{array}\right);$

2)建立卡尔曼滤波的量测方程：

用一阶线性微分方程来描述旋转捷联惯导系统的量测方程如下：

Z(t)＝H(t)X(t)+v(t)

其中：Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；v(t)表示系统的量测噪声；

系统量测矩阵为：

$H\left(t\right)=\left(\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right)$

量测量为水平速度误差：

v(t)＝[v_e v_n]^T。

4、所述将卡尔曼滤波估计出的惯性器件常值偏差采用平均滤波的方法将其进行平滑，滤除小幅度波动的影响的方法为：已知等距采样点x₀＜x₁＜…＜x_n-1＜…上的观测数据为y₀，y₁，…，y_n-1，y_i表示y_i的平滑值；取加权系数为1，取低通滤波后的400秒到600秒数据做一次平滑，则：

${\bar{y}}_i=\frac{y_{i-n}+y_{i-n+1}+...+y_0+y_1+...+y_{i+n-2}+y_{i+n-1}}{n}.$

本发明利用惯性测量单元(IMU)间歇性转动提高惯导系统中部分参数的可观测度；根据系泊状态下载体位置变化范围小的特点，应用载体位置回路与系统分离的卡尔曼滤波技术估计出三个方向上陀螺仪常值漂移和水平方向上加速度计零偏。考虑到载体系泊状态下估计出的惯性器件常值偏差中存在小范围的波动，将估计出的数据采用平均滤波的方法将其进行平滑。

本发明与现有技术相比的优点在于：本发明打破了舰船系泊状态下，由于摇摆和荡运动的干扰导致导航系下估漂技术难以适用这一问题，利用IMU的转动可以有效地提高系统部分参数的可观测度，同时考虑到舰船系泊状态下位置信息变化幅度小导致忽略位置回路与系统的耦合关系，使载体在系泊状态下达到较好的估计精度。

对本发明有益的效果说明如下：

在Visual C++仿真条件下，对该方法进行仿真实验：

设定陀螺仪常值漂移为0.01°/h，加速度计零位偏差为10^-4g；系统初始对准误差为0.1°、0.1°、0.5°；载体以正弦规律绕纵摇轴、横摇轴和航向轴作三轴摇摆运动，其数学模型为：

$\left(\begin{array}{c}\theta ={\theta }_m\sin ({\omega }_{\theta }t+{\phi }_{\theta })\\ \gamma ={\gamma }_m\sin ({\omega }_{\gamma }t+{\phi }_{\gamma })\\ \psi ={\psi }_m\sin ({\omega }_{\psi }t+{\phi }_{\psi })+k\end{array}\right)$

其中：θ、γ、ψ分别表示纵摇角、横摇角和航向角的摇摆角度变量；θ_m、γ_m、ψ_m分别表示相应的摇摆角度幅值；ω_θ、ω_γ、ω_ψ分别表示相应的摇摆角频率；φ_θ、φ_γ、φ_ψ分别表示相应的初始相位；ω_i＝2π/T_i，i＝θ、γ、ψ，T_i表示相应的摇摆周期，k为初始航向角。仿真时取：θ_m＝6°，γ_m＝12°，ψ_m＝10°，T_θ＝8s，T_γ＝10s，T_ψ＝6s，k＝0°。

载体的横荡、纵荡和垂荡引起的线速度为：

式中，i＝x，y，z为地理坐标系的东向、北向、天向。为[0，2π]上服从均匀分布的随机相位。

载体初始位置：北纬45.7796°，东经126.6705°；

初始姿态误差角：三个初始姿态误差角均为零；

赤道半径：R_e＝6378393.0m；

椭球度：e＝3.367e-3；

由万有引力可得的地球表面重力加速度：g₀＝9.78049；

地球自转角速度(弧度/秒)：7.2921158e-5；

陀螺仪常值漂移：0.01度/小时；

陀螺仪随机游走：

加速度计零偏：10^-4g₀；

加速度计噪声：10^-6g₀；

常数：π＝3.1415926。

附图说明

图1为本发明的一种基于单轴转停方案的系泊估漂方法；

图2为本发明的IMU单轴四位置转停；

图3为本发明的估计的陀螺仪常值漂移；

图4为本发明的估计的水平方向上的加速度计零偏。

具体实施方式

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

(1)通过GPS确定载体的初始位置参数，并装订至导航计算机中；

(2)捷联惯导系统进行预热准备，采集光纤陀螺仪和石英加速度计输出的数据并对数据进行处理；

(3)IMU采用8个转停次序为一个旋转周期的转位方案(如附图2)；

次序1，IMU从A点出发顺时针转动180°到达位置C，停止时间T_t；次序2，IMU从C点出发顺时针转动90°到达位置D，停止时间T_t；次序3，IMU从D点出发逆时针转动180°到达位置B，停止时间T_t；次序4，IMU从B点出发逆时针转动90°到达位置A，停止时间T_t；次序5，IMU从A点出发逆时针转动180°到达位置C，停止时间T_t；次序6，IMU从C点出发逆时针转动90°到达位置B，停止时间T_t；次序7，IMU从B点出发顺时针转动180°到达位置D，停止时间T_t；次序8，IMU从D点出发顺时针转动90°到达位置A，停止时间T_t；IMU按照此转动顺序循环进行。为了有效地对水平方向上的惯性器件偏差在对称位置上进行正负平均，定义水平东向轴上的IMU停顿点p3、p8与p4、p7对称于转轴中心；北向轴上的停顿点p1、p5与p2、p6对称于转轴中心。四位置转停方案仍然是转动角度为180°或90°间隔进行。

(4)根据旋转状态下陀螺仪的输出确定出捷联矩阵T_s^p，并将加速度计的输出转换到数学平台坐标系；

根据四阶龙格库塔法，应用四元数对惯导系统的捷联矩阵进行实时更新，其公式表示为：

${\stackrel{·}{T}}_s^p=T_s^p[\times {\omega }_{\mathrm{is}}^s]---\left(1\right)$

其中ω_is^s表示陀螺仪输出。利用IMU坐标系与数学平台坐标系的转换关系，计算出数学平台坐标系下的加速度值。

$f^p=T_s^p{f}^s---\left(2\right)$

(5)根据惯导系统动基座误差方程建立载体系泊状态时的估漂模型，以GPS提供的载体速度v₀为基准，并与惯导系统结算速度的差值作为观测量。利用卡尔曼滤波技术实现惯性器件偏差的估计；

由于采用IMU的转动可以有效地提高惯导系统中部分参数的可观测度，同时考虑到系泊状态时载体的位置变化幅度小，因此可以采用分离载体位置信息的误差模型作为系统的估漂模型。建立以速度误差为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程；

1)建立卡尔曼滤波的状态方程：

用一阶线性微分方程来描述旋转捷联惯导系统的状态误差：

$\stackrel{·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)---\left(3\right)$

其中X(t)为t时刻系统的状态向量；A(t)和B(t)分别为系统的状态矩阵和噪声矩阵；W(t)为系统噪声向量；

系统的状态向量为：

系统的白噪声向量为：

W＝[a_x a_y ω_x ω_y ω_z 0 0 0 0 0]^T    (5)

其中δV_e、δV_n分别表示东向、北向的速度误差；分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；ω_x、ω_y、ω_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的白噪声误差；

系统的状态转移矩阵为：

$A\left(t\right)=\left(\begin{array}{cc}{F}_{5\times 5}& T_{5\times 5}\\ 0_{5\times 5}& 0_{5\times 5}\end{array}\right)---\left(6\right)$

其中：

$F_{5\times 5}=\left(\begin{array}{cc}{F}_{2\times 2}& F_{2\times 3}\\ F_{3\times 2}& F_{3\times 3}\end{array}\right)---\left(7\right)$

$F_{2\times 2}=\left(\begin{array}{cc}\frac{V_n\tan L}{R_n}& {2\omega }_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}\\ -2({\omega }_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0\end{array}\right)---\left(8\right)$

$F_{2\times 3}=\left(\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\end{array}\right)---\left(9\right)$

$F_{3\times 2}=\left(\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{R_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right)---\left(10\right)$

$F_{3\times 3}=\left(\begin{array}{ccc}0& {\omega }_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}& -({\omega }_{\mathrm{ie}}\cos L+\frac{V_e}{R_n})\\ -({\omega }_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0& -\frac{V_n}{R_m}\\ {\omega }_{\mathrm{ie}}\cos L+\frac{V_e}{R_n}& \frac{V_n}{R_m}& 0\end{array}\right)---\left(11\right)$

V_e、V_n分别表示东向、北向的速度；ω_ie表示地球自转角速度；R_m、R_n分别表示地球子午、卯酉曲率半径；L表示当地纬度；f_e、f_n、f_u分别表示为导航坐标系下东向、北向、天向的比力。

令捷联矩阵T_s^p为：

$T_s^p=\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(12\right)$

则

$T_{5\times 5}=\left(\begin{array}{ccccc}{T}_{11}& T_{12}& 0& 0& 0\\ T_{21}& T_{22}& 0& 0& 0\\ 0& 0& -T_{11}& -T_{12}& -T_{13}\\ 0& 0& -T_{21}& {-T}_{22}& -T_{23}\\ 0& 0& -T_{31}& -T_{32}& -T_{33}\end{array}\right)---\left(13\right)$

2)建立卡尔曼滤波的量测方程：

用一阶线性微分方程来描述旋转捷联惯导系统的量测方程如下：

Z(t)＝H(t)X(t)+v(t)          (14)

其中：Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；v(t)表示系统的量测噪声；

系统量测矩阵为：

$H\left(t\right)=\left(\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right)---\left(15\right)$

量测量为水平速度误差：

v(t)＝[v_e v_n]^T              (16)

(6)将卡尔曼滤波估计出的惯性器件常值偏差采用平均滤波的方法将其进行平滑，滤除小幅度波动的影响；

考虑到载体系泊状态下估计出的惯性器件常值偏差中存在小范围的波动，这样的信号不利于惯性器件常值偏差的修正，因此采用加权求平均滤波算法对卡尔曼滤波后的惯性器件常值偏差信息进行再次消噪处理。这种方法是最简单的一种信号处理方式，算法公式如下：设已知等距采样点x₀＜x₁＜…＜x_n-1＜…上的观测数据为y₀，y₁，…，y_n-1，y_i表示y_i的平滑值。我们取加权系数为1，取低通滤波后的400秒到600秒数据做一次平滑，则有：

${\bar{y}}_i=\frac{y_{i-n}+y_{i-n+1}+...+y_0+y_1+...+y_{i+n-2}+y_{i+n-1}}{n}---\left(17\right).$