一种增益补偿自适应滤波的SINS/DVL组合定位方法技术

一种增益补偿自适应滤波的SINS/DVL组合定位方法，涉及高精度SINS/DVL组合定位。本发明专利技术是为了有效解决由于传统SINS/DVL组合导航的滤波算法灵活性不足的影响，导致定位精度较低的问题。本发明专利技术所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法，首先基于捷联惯导系统和多普勒计程仪传感器的信息，获取相应状态初值和观测值；然后建立基于组合导航误差模型对应的系统方程和观测方程，用增益补偿改进自适应滤波算法对误差进行校正，并获取校正后目标的速度和位置误差信息；最后将获得的误差信息和捷联惯导与多普勒计程仪的观测信息进行融合，得出高精度的定位结果。

A SINS/DVL Combined Location Method Based on Gain Compensation Adaptive Filtering

【技术实现步骤摘要】
一种增益补偿自适应滤波的SINS/DVL组合定位方法
本专利技术涉及高精度的水下定位技术
技术介绍
在实际的水下定位过程中，由于水下环境的复杂多变，使得捷联惯导与多普勒计程仪组合定位(SINS/DVL)系统的系统噪声和量测噪声的统计特性往往具有一定的时变性。为了对卡尔曼滤波算法进行合理的改进使其对噪声统计特性的变化具有一定的自适应能力，从而来进一步提高滤波精度，达到高精度的定位。本专利技术采用滤波增益补偿方法对改进自适应滤波算法进行优化，采用优化后的滤波算法对定位系统误差发散的情况进行抑制，从而实现提高定位精度的目的。
技术实现思路
本专利技术的目的是为了解决由于复杂水下环境下SINS/DVL组合定位方法系统噪声时变特性，导致定位精度较低的问题，提出一种增益补偿自适应滤波的SINS/DVL组合定位方法。本专利技术所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法包括以下步骤：步骤一、系统建立SINS/DVL组合定位误差模型状态变量X＝[δvEδvNαβγδLδλεEεNεUδvdδΔδC]T，其中δvE和δvN为东、北向速度误差，α、β、γ为平台失准角，δL和δλ为经度误差和纬度误差，εE、εN、εU为东、北、天方向陀螺漂移，δvd为多普勒测量速度偏移误差，δΔ为偏流角误差，δC为刻度系数误差；步骤二、系统通过惯性导航组件中的陀螺仪敏感载体的三轴角速度信息和加速度计测得三轴(东、北、天)加速度信息aE、aN、aH，由多普勒计程仪获得速度vd、偏流角Δ和姿态等导航信息；步骤三、系统将这三个方向的加速度分量aE、aN、aH带入公式(1)分别进行积分，即可得到载体沿这三个方向的速度分量vE、vN、vH。再将三个速度分量带入公式(2)进行积分得到载体的经度L、纬度λ和深度d，其中R为地球半径，t0为运动初始时刻，tk为运动过程中某时刻；系统将这些信息带入公式(3)～(15)中求出误差模型对应的各状态变量X＝[δvEδvNαβγδLδλεEεNεUδvdδΔδC]T：其中，Ω为陀螺振动频率为常量，g为重力加速度为常量，ΔaN，ΔaE为加速度误差；东向和北向速度误差公式：平台失准角：位置误差：陀螺漂移：εE＝-βEεE+wE(10)εN＝-βNεN+wN(11)εU＝-βUεU+wU(12)为陀螺仪在东、北、天方向的误差相关时间，wE、wN、wU为高斯白噪声；多普勒计程仪的速度、偏流角和刻度误差：δvd＝-βdδvd+wd(13)δΔ＝-βΔδΔ+wΔ(14)δC＝0(15)其中为速度偏移误差和偏流角误差的相关时间，wd、wΔ为高斯白噪声；步骤四、系统建立系统状态方程和系统量测方程，如公式(16)和式(21)所示：状态方程描述为：式中：WSINS＝[00aEaN000wEwNwUwdwΔ0]T(17)根据式(3)～(15)建立状态传递阵FSINS/DVL其中：有：对于F6×6有：系统量测方程为：对于HSINS/DVL和VSINS/DVL有：VSINS/DVL＝[vEvN]T(23)这里系统噪声方差阵：量测噪声方差阵：步骤五、系统建立滤波增益改进自适应滤波算法，并对滤波增益改进自适应卡尔曼滤波算法系统的状态方程和量测方程进行描述，如公式(26)、(27)：状态方程描述为：Xk＝FkXk-1+GWk(26)系统量测方程为：其中，Xk为k时刻的状态变量值；Xk-1为k-1时刻的状态变量；F为作用在Xk-1上的状态变换系数；Wk为k时刻的状态噪声值；G为作用在上Wk的系数；Hk为观测模型系数，把真实状态空间映射成观测空间；Zk为k时刻的观测值，由捷联惯导的东向、北向速度误差和多普勒计程仪东向北向速度误差的差构成；Vk为k时刻的观测噪声值；步骤六、将Fk，Gk，Wk，Hk，Vk，以及初始的状态变量X带入系统的状态方程和量测方程进行一步预测，求出k时刻预测的量测值和为k时刻量测值的误差；步骤七、系统根据稳定性判据，判断若成立则系统发散，执行步骤八，采用增益补偿自适应滤波；若不成立则系统收敛，执行步骤九，采用增益补偿强跟踪卡尔曼滤波；步骤八、系统将对应参数及量代入公式(28)～(37)中进行计算，其中，为k时刻的状态变量值；为由k-1时刻的状态变量所得到的k时刻的预测值；Fk,k-1为作用在上的状态变换系数；Kk为作用在上的滤波增益补偿卡尔曼系数；Hk为观测模型系数，把真实状态空间映射成观测空间；Zk为k时刻的观测值，k时刻预测的量测值为Pk,k-1为先验估计误差协方差值；Pk为后验估计误差协方差值；在(28)～(33)式子中分别由时变噪声估值方程所计算得式(34)～(37)：实现增益补偿自适应滤波，得到下一时刻状态变量估计值执行步骤十。步骤九、系统将对应参数及参量代入公式(38)～(46)中进行计算：λk+1＝diag[λ1(k+1),λ2(k+1),…,λm(k+1)](41)实现增益补偿强跟踪卡尔曼滤波，得到下一时刻状态变量估计值执行步骤十；步骤十、系统更新变换k＝k+1，将得到的状态变量估计值记为新的状态变量值步骤十一、系统判断k是否等于n，如果是，则执行步骤十二，否则执行步骤六；步骤十二、系统得到生成的系统状态变量序列输出的最终结果即为当前时刻经过滤波校正后的状态结果。包含是误差量的校正结果，再结合当前时刻SINS/DVL组合定位系统的观测值Yn＝[vEnvNnLnλnvdn]T，得到校正后的航行器东向、北向速度和经纬度信息以及多普勒计程仪的速度信息其中，步骤十三、系统判断SINS/DVL组合定位任务是否完成，如果是，执行步骤十四，否则执行步骤二；步骤十四、系统结束增益补偿自适应滤波的SINS/DVL组合定位任务。附图说明图1为一种增益补偿自适应滤波的SINS/DVL组合定位方法的流程图。具体实施方式具体实施方式一步骤一、系统建立SINS/DVL组合定位误差模型状态变量X＝[δvEδvNαβγδLδλεEεNεUδvdδΔδC]T，其中δvE和δvN为东、北向速度误差，α、β、γ为平台失准角，δL和δλ为经度误差和纬度误差，εE、εN、εU为东、北、天方向陀螺漂移，δvd为多普勒测量速度偏移误差，δΔ为偏流角误差，δC为刻度系数误差；步骤二、系统通过惯性导航组件中的陀螺仪敏感载体的三轴角速度信息和加速度计测得三轴(东、北、天)加速度信息aE、aN、aH，由多普勒计程仪获得速度vd、偏流角Δ和姿态等导航信息；步骤三、系统将这三个方向的加速度分量aE、aN、aH带入公式(1)分别进行积分，即可得到载体沿这三个方向的速度分量vE、vN、vH。再将三个速度分量带入公式(2)进行积分得到载体的经度L、纬度λ和深度d，其中R为地球半径，t0为运动初始时刻，tk为运动过程中某时刻；系统将这些信息带入公式(3)～(15)中求出误差模型对应的各状态变量X＝[δvEδvNαβγδLδλεEεNεUδvdδΔδC]T：其中，Ω为陀螺振动频率为常量，g为重力加速度为常量，ΔaN，ΔaE为加速度误差；东向和北向速度误差公式：平台失准角：位置误差：陀螺漂移：εE＝-βEεE+wE(10)εN＝-βNεN+wN(11)εU＝-βUεU+wU(12)为陀螺仪在东、北、天方向的误差相关时间，wE、wN、wU为高斯白噪声；多普勒计程仪的速度、偏流角和本文档来自技高网...

【技术保护点】
1.一种增益补偿自适应滤波的SINS/DVL组合定位方法，其特征在于所述方法包括以下步骤：步骤一、系统建立SINS/DVL组合定位误差模型状态变量X＝[δvE δvN α β γ δL δλ εE εN εU δvd δΔ δC]

【技术特征摘要】
1.一种增益补偿自适应滤波的SINS/DVL组合定位方法，其特征在于所述方法包括以下步骤：步骤一、系统建立SINS/DVL组合定位误差模型状态变量X＝[δvEδvNαβγδLδλεEεNεUδvdδΔδC]T，其中δvE和δvN为东、北向速度误差，α、β、γ为平台失准角，δL和δλ为经度误差和纬度误差，εE、εN、εU为东、北、天方向陀螺漂移，δvd为多普勒测量速度偏移误差，δΔ为偏流角误差，δC为刻度系数误差；步骤二、系统通过惯性导航组件中的陀螺仪敏感载体的三轴角速度信息和加速度计测得三轴(东、北、天)加速度信息aE、aN、aH，由多普勒计程仪获得速度vd、偏流角Δ和姿态等导航信息；步骤三、系统将这三个方向的加速度分量aE、aN、aH带入公式(1)分别进行积分，即可得到载体沿这三个方向的速度分量vE、vN、vH。再将三个速度分量带入公式(2)进行积分得到载体的经度L、纬度λ和深度d，其中R为地球半径，t0为运动初始时刻，tk为运动过程中某时刻；系统将这些信息带入公式(3)～(15)中求出误差模型对应的各状态变量X＝[δvEδvNαβγδLδλεEεNεUδvdδΔδC]T：其中，Ω为陀螺振动频率为常量，g为重力加速度为常量，ΔaN，ΔaE为加速度误差；东向和北向速度误差公式：平台失准角：位置误差：陀螺漂移：εE＝-βEεE+wE(10)εN＝-βNεN+wN(11)εU＝-βUεU+wU(12)为陀螺仪在东、北、天方向的误差相关时间，wE、wN、wU为高斯白噪声；多普勒计程仪的速度、偏流角和刻度误差：δvd＝-βdδvd+wd(13)δΔ＝-βΔδΔ+wΔ(14)δC＝0(15)其中为速度偏移误差和偏流角误差的相关时间，wd、wΔ为高斯白噪声；步骤四、系统建立系统状态方程和系统量测方程，如公式(16)和式(21)所示：状态方程描述为：式中：WSINS＝[00aEaN000wEwNwUwdwΔ0]T(17)根据式(3)～(15)建立状态传递阵FSINS/DVL其中：有：对于F6×6有：系统量测方程为：对于HSINS/DVL和VSINS/DVL有：VSINS/DVL＝[vEvN]T(23)这里系统噪声方差阵：量测噪声方差阵：步骤五、系统建立滤波增益改进自适应滤波算法，并对滤波增益改进自适应卡尔曼滤波算法系统的状态方程和量测方程进行描述，如公式(26)、(27)：状态方程描述为：Xk＝FkXk-1+GWk(26)系统量测方程为：其中，Xk为k时刻的状态变量值；Xk-1为k-1时刻的状态变量；F为作用在Xk-1上的状态变换系数；Wk为k时刻的状态噪声值；G为作用在上Wk的系数；Hk为观测模型系数，把真实状态空间映射成观测空间；Zk为k时刻的观测值，由捷联惯导的东向、北向速度误差和多普勒计程仪东向北向速度误差的差构成；Vk为k时刻的观测噪声值；步骤六、将Fk，Gk，Wk，Hk，Vk，以及初始的状态变量X带入系统的状态方程和...

【专利技术属性】
技术研发人员：杨一鹏，闫锋刚，罗清华，焉晓贞，彭宇，彭喜元，
申请(专利权)人：哈尔滨工业大学威海，
类型：发明
国别省市：山东,37