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

摘要

一种无逆矩阵自适应滤波的SINS/DVL组合定位方法，涉及高精度SINS/DVL组合定位。本发明是为了解决由于传统SINS/DVL组合导航滤波算法计算过程复杂，导致计算效率低和稳定性差的问题。本发明所述的一种无逆矩阵自适应滤波的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＝[0 0 a_E a_N 0 0 0 w_E w_N w_U w_d w_Δ 0]^T  (17)根据式(3)～(15)建立状态传递阵F_SINS/DVL其中：有：对于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组合定位任务。