[发明专利]一种无逆矩阵自适应滤波的SINS/DVL组合定位方法

权利要求书

1.一种无逆矩阵自适应滤波的SINS/DVL组合定位方法，其特征在于，所述方法包括以下步骤：

步骤一、系统建立SINS/DVL组合定位误差模型状态变量X＝[δv_Eδv_NαβγδLδλε_Eε_Nε_Uδv_dδΔδC]^T，其中δv_E和δv_N为东、北向速度误差，α、β、γ为平台失准角，δL和δλ为经度误差和纬度误差，ε_E、ε_N、ε_U为东、北、天方向陀螺漂移，δv_d为多普勒测量速度偏移误差，δΔ为偏流角误差，δC为刻度系数误差；

步骤二、系统通过惯性导航组件中的陀螺仪敏感载体的三轴角速度信息和加速度计测得三轴即东、北、天的加速度信息a_E、a_N、a_H，由多普勒计程仪获得速度v_d、偏流角Δ和姿态的导航信息；

步骤三、系统将这三个方向的加速度分量a_E、a_N、a_H代入公式(1)分别进行积分，即可得到载体沿这三个方向的速度分量v_E、v_N、v_H；再将三个速度分量代入公式(2)进行积分得到载体的经度L、纬度λ和深度d，

其中R为地球半径，t₀为运动初始时刻，t_k为运动过程中某时刻；

系统将这些信息代入公式(3)～(15)中求出误差模型对应的各状态变量X＝[δv_Eδv_NαβγδLδλε_Eε_Nε_Uδv_dδΔδC]^T：

东向和北向速度误差公式：

 

其中，Ω为陀螺振动频率为常量，g为重力加速度为常量，Δa_N，Δa_E为加速度误差；

平台失准角：

位置误差：

陀螺漂移：

ε_E＝-β_Eε_E+w_E                      (10)

ε_N＝-β_Nε_N+w_N                     (11)

ε_U＝-β_Uε_U+w_U                     (12)

为陀螺仪在东、北、天方向的误差相关时间，w_E，w_N，w_U为高斯白噪声；

多普勒计程仪的速度、偏流角和刻度误差：

δv_d＝-β_dδv_d+w_d                        (13)

δΔ＝-β_ΔδΔ+w_Δ                         (14)

δC＝0                              (15)

其中为速度偏移误差和偏流角误差的相关时间，w_d、w_Δ为高斯白噪声；

步骤四、系统建立系统状态方程和系统量测方程，如公式(16)和式(21)所示：

状态方程描述为：

式中：

W_SINS＝[00 a_E a_N 000 w_E w_N w_U w_d w_Δ 0]^T  (17)

根据式(3)～(15)建立状态传递阵F_SINS/DVL

其中：

有：

F₃₆＝-g，

F₄₅＝g，

F₆₁＝-ΩsinL，

对于F_6×6有：

系统量测方程为：

对于H_SINS/DVL和V_SINS/DVL有：

V_SINS/DVL＝[v_E v_N]^T                      (23)

这里系统噪声方差阵：

量测噪声方差阵：

步骤五、系统建立无逆矩阵改进自适应滤波算法，并对无逆矩阵改进自适应卡尔曼滤波算法系统的状态方程和量测方程进行描述，如公式(26)，(27)，状态方程描述为：

X_k＝F_kX_k-1+GW_k         (26)

系统量测方程为：

其中，X_k为k时刻的状态变量值；X_k-1为k-1时刻的状态变量；F为作用在X_k-1上的状态变换系数；W_k为k时刻的状态噪声值；G为作用在上W_k的系数；H_k为观测模型系数，把真实状态空间映射成观测空间；Z_k为k时刻的观测值，由捷联惯导的东向、北向速度误差和多普勒计程仪东向北向速度误差的差构成；V_k为k时刻的观测噪声值；

步骤六、将F_k，G_k，W_k，H_k，V_k以及初始的状态变量X代入系统的状态方程和量测方程进一步预测，求出k时刻预测的量测值和为k时刻量测值的误差；

步骤七、系统根据稳定性判据，判断若成立则系统发散，应采用无逆矩阵自适应滤波执行步骤八；若不成立则系统收敛，应采用无逆矩阵强跟踪卡尔曼滤波，执行步骤九；

步骤八、系统将对应参数及参量代入公式(28)～(40)中进行计算

λ_k(1)＝[H_k(1)L_k(1)+λ_k(1)]^-1                  (34)

λ_k(2)＝[H_k(2)L_k(2)+λ_k(2)]^-1                 (38)

其中，为k时刻的状态变量值；和为k时刻求状态变量的过渡值；为由k-1时刻的状态变量所得到的k时刻的预测值；F_k,k-1为作用在上的状态变换系数；K_k为作用在上的无逆矩阵卡尔曼系数；λ_k为过渡常数和L_k L_k∈R^n×1；H_k为观测模型系数，把真实状态空间映射成观测空间Z_k为k时刻的观测值，k时刻预测的量测值为其中Z_k＝[Z_k(1)Z_k(2)]^T，P_k,k-1为先验估计误差协方差值；为k时刻误差协方差过渡值；P_k为后验估计误差协方差值

在(28)～(40)式子中分别由时变噪声估值方程所计算得式(41)～(44)：

实现无逆矩阵自适应滤波操作，得到下一时刻状态变量估计值执行步骤十；

步骤九、系统将对应参数及量代入公式(45)～(60)中进行计算：

λ_k+1＝diag[λ_1(k+1),λ_2(k+1),…,λ_m(k+1)](48)

λ_k(1)＝[H_k(1)L_k(1)+λ_k(1)]^-1(54)

λ_k(2)＝[H_k(2)L_k(2)+λ_k(2)]^-1(58)

实现无逆矩阵强跟踪卡尔曼滤波，得到下一时刻状态变量估计值执行步骤十；

步骤十、系统更新变换k＝k+1，将得到的状态变量估计值记为新的状态变量值

步骤十一、系统判断k是否等于n，如果是，则执行步骤十二，否则执行步骤六；

步骤十二、系统得到生成的系统状态变量序列输出的最终结果即为当前时刻经过滤波校正后的状态结果，包含是误差量的校正结果；再结合当前时刻SINS/DVL组合定位系统的观测值Y_n＝[v_En v_Nn L_nλ_n v_dn]^T，得到校正后的航行器东向、北向速度和经纬度信息以及多普勒计程仪的速度信息其中

步骤十三、系统判断SINS/DVL组合定位任务是否完成，如果是，执行步骤十四，否则执行步骤二；

步骤十四、系统结束无逆矩阵自适应滤波的SINS/DVL组合定位任务。

2.根据权利要求1所述的一种无逆矩阵自适应滤波的SINS/DVL组合定位方法，其特征在于，步骤七中判断滤波是否发散的判据是：根据滤波估计的误差与期望误差的大小关系来判断。