【技术实现步骤摘要】
一种无逆矩阵自适应滤波的SINS/DVL组合定位方法
本专利技术涉及高精度的水下定位技术。
技术介绍
在实际的水下定位过程中,由于水下环境的复杂多变,使得捷联惯导与多普勒计程仪组合定位(SINS/DVL)系统的系统噪声和量测噪声的统计特性往往具有一定的时变性。为了对噪声统计特性的变化具有一定的自适应能力,提高滤波精度,常采用自适应卡尔曼滤波算法对噪声进行处理。然而在Kalman滤波过程中,大部分运算量集中在求逆运算的过程。而且求逆运算对于计算机来说,处理复杂,可靠性差。因此本专利技术采用快速计算的无求逆矩阵算法对自适应卡尔曼滤波算法进行改进,采用改进后的滤波算法优化矩阵求逆运算过程,在保证滤波精度和可靠性的同时,还能够减少计算量,提升运算效率,确保运行过程相对可靠稳定。
技术实现思路
本专利技术的目的是为了解决复杂水下环境下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为刻度系数误差;步骤二、系统通过惯性导航组件中的陀螺仪敏感载体的三轴角速度信息和加速度计测得三轴(东 ...
【技术保护点】
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带入系统的状态方程和量测方程进行一步预测,求出k时刻预测的量测值和为k时刻量测值的误差;步骤七、系统根据稳定性判据,判断若成立则系统发散,应采用无逆矩阵自适应滤波执行步骤八;若不成立则系统收敛,应采用无逆矩阵强跟踪卡尔曼滤波,执行步骤九;步骤八、系统将对应参数及参量代入公式(28)~(40)中进...
【专利技术属性】
技术研发人员:罗清华,杨一鹏,闫锋刚,焉晓贞,彭宇,彭喜元,
申请(专利权)人:哈尔滨工业大学威海,
类型:发明
国别省市:山东,37
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。