CN109253726A - 一种水下滑翔器导航定位系统及上浮精度校正方法 - Google Patents

Abstract

本发明公开了一种水下滑翔器导航定位系统及上浮精度校正方法，该水下滑翔器导航定位系统包括微机电系统惯性测量单元，全球卫星定位系统接收模块，三轴磁力计，多普勒测速仪以及组合导航硬件处理系统。针对上浮精度校正过程中，会受到外界恶劣天气、海浪波动、船舰遮挡等因素的影响，使得多传感器数据融合算法的导航精度降低，定位发散甚至失效的问题，本发明建立的一种基于多重自适应渐消因子的H∞卡尔曼滤波算法来保证滑翔器导航定位的鲁棒性和自适应性。克服了极端恶劣条件下的水下滑翔器在上浮校正时，传统组合导航算法所导致的导航定位精度降低、发散以至于滑翔器无法继续正常工作的问题。最终实现水下滑翔器的准确定位。

Description

一种水下滑翔器导航定位系统及上浮精度校正方法

技术领域

本发明属于导航技术领域，涉及了水下滑翔器的导航定位方法，具体是一种水下滑翔器导航系统的上浮精度校正方法，该方法满足在极端恶劣情况下，水下滑翔器的上浮校准精度仍达到一定要求，提高导航系统的鲁棒性和自适应性，最终实现水下滑翔器的准确定位。

背景技术

水下滑翔器是一种用于海洋探索和资源开发的重要工具。水下滑翔器工作过程中本身运动并不剧烈，从而采用低成本、低功耗、长航时的微机电系统惯性测量单元(MEMS-initial measurement unit)足以符合导航定位要求。但滑翔器长时间的水下工作以及在干扰条件下(包括洋流涌动、磁场干扰等)，会产生较大的定位累积误差，因此滑翔器需要上浮至水面，利用高精度的全球定位系统(GPS) 信号以及数据融合算法对其定位进行校正。但是上浮精度校正过程中，会受到外界恶劣天气、海浪波动、船舰遮挡等因素的影响，使得多传感器数据融合算法的导航精度降低，定位发散甚至失效。

因此，如何在极端恶劣条件下，保证水下滑翔器上浮精度校正工作的稳定性和精确性成为了水下滑翔器准确定位的重要问题。

发明内容

发明目的：针对滑翔器处于上浮校正状态时，可能受到外界恶劣天气、海浪波动、船舰遮挡等因素的影响的问题，本发明目的在于提供一种水下滑翔器导航定位系统及上浮精度校正方法，通过多传感器数据融合算法来保证滑翔器导航定位的鲁棒性和自适应性，以克服极端恶劣条件下的水下滑翔器在上浮校正时，传统组合导航算法所导致的导航定位精度降低、发散以至于滑翔器无法继续正常工作的问题，最终实现水下滑翔器的准确定位。

技术方案：为实现上述发明目的，本发明采用如下技术方案：

一种水下滑翔器导航定位系统，包括：微机电系统惯性测量单元 (MEMS-IMU)，全球卫星定位系统接收模块(GPS)，三轴磁力计，多普勒测速仪(DVL)，以及组合导航硬件处理系统。所述MEMS-IMU集成了三轴加速度计和三轴陀螺仪，输出的三轴加速度和角速率信息通过惯性导航算法得到水下滑翔器的姿态、速度和位置导航信息；所述三轴磁力计用于校正滑翔器的航向信息，所述多普勒测速仪用于判断水下滑翔器的运动状态；所述GPS用于滑翔器上浮校正操作中，与MEMS-IMU组成GPS/INS位置速度校正的松组合导航系统，利用组合导航滤波器算法，校正滑翔器的速度、位置误差；所述组合导航硬件处理系统用于三轴磁力计、GPS、MEMS-IMU以及DVL信号的接收处理与时钟同步工作，以及组合导航多传感器信息的算法解算工作。

作为优选，所述水下滑翔器导航定位系统具有两种运行状态：水下工作状态和上浮校正状态；若导航定位误差过大，则滑翔器停止实时工作，由水下工作状态切换为上浮校正状态；当GPS/INS组合导航系统的速度、位置误差小于设定阈值时，则滑翔器由上浮校正状态切换为水下工作状态，继续工作。

作为优选，水下工作利用MEMS-IMU和三轴磁力计输出的信号，采用基于互补滤波的四元数算法完成滑翔器的导航定位。

作为优选，上浮校正利用GPS和MEMS-IMU输出的信号，采用GPS/INS位置速度校正的松组合导航系统，完成滑翔器的导航定位；其中组合导航滤波器算法采用基于多重自适应渐消因子的H∞卡尔曼滤波算法，该算法的状态预测协方差阵H∞滤波增益阵H∞滤波状态最优协方差阵式中k和k-1分别表示当前时刻和前一时刻，Φ_k|k-1为状态转移矩阵，H_k为观测矩阵，Q_k-1为系统噪声协方差，S_k为多重自适应渐消因子阵，L_k为对系统状态量的线性组合的估计，I为单位阵，γ为自适应阈值。

作为优选，所述多重自适应渐消因子阵S_k＝diag(s₁,s₂,s₃...,s_n)由下式计算：

式中，λ_i为观测阵H_k第i个观测元素，j_ii(k)为矩阵的第i个对角线元素,ε_i为卡方检测的阈值，υ_i(k)为新息阵V_k第i个观测元素， b_ii(k)为矩阵的第i个对角线元素。

作为优选，所述自适应阈值γ＝η·γ_a；其中，Trace() 表示矩阵求迹运算，ρ()表示矩阵的谱半径。

一种用于水下滑翔器导航定位系统的上浮精度校正方法，包含如下步骤：

(1)判断水下滑翔器是否需要进行上浮精度校正操作，当航向变化过大、速度变化过大，或人为判定滑翔器导航定位误差过大时，滑翔器上浮将至水面，进行精度校正操作；。

(2)滑翔器上浮至水面后，接收GPS的经纬度、高度、三轴速度信号，IMU 的三轴加速度，三轴角速率信号，将基于多重自适应渐消因子的H∞卡尔曼滤波算法应用于GPS/INS位置速度校正的松组合导航系统，完成数据融合；

(3)通过步骤(2)使得组合导航系统的速度、位置误差逐渐减小、趋于零，若误差小于一设定的阈值，则认为上浮校正工作完成，滑翔器切换至水下工作状态。

有益效果：本发明的水下滑翔器导航定位系统在滑翔器处于水下工作状态时，利用低成本的惯性MEMS-IMU以及三轴磁力计完成水下滑翔器的姿态、速度、位置解算，在滑翔器处于上浮校正状态时，利用高精度的GPS模块与惯性 MEMS-IMU元件进行多传感器的数据融合，减小滑翔器的导航定位误差。水下滑翔器这两种状态的切换条件与要求可根据具体工作和任务通过软硬件进行合适的设定。由于，滑翔器上浮精度校正过程中，外界不确定性干扰对其的影响是未知的，同时噪声统计特征和系统模型是时变的。本发明采用的基于多重自适应渐消因子的H∞卡尔曼滤波算法，是能自适应于时变条件下的多传感器数据融合算法，能保证上浮校正的稳定性和精确性满足要求。本发明克服了极端恶劣条件下的水下滑翔器在上浮校正时，传统组合导航算法所导致的导航定位精度降低、发散以至于滑翔器无法继续正常工作的问题。最终实现水下滑翔器的准确定位。

附图说明

图1是本发明实施例的水下滑翔器导航定位系统结构及水下工作与上浮校正状态转换示意图。

图2是本发明实施例中GPS/INS位置速度校正的松组合导航系统框图。

图3是本发明实施例中水下滑翔器上浮校正状态的基于多重自适应渐消因子的H∞卡尔曼滤波算法流程图。

具体实施方式

以下结合具体实施例和附图对本发明进行进一步说明：

如图1所示，本发明公开的一种水下滑翔器导航系统，主要包括：微机电系统惯性测量单元(MEMS-IMU)，全球卫星定位系统接收模块(GPS)，三轴磁力计，多普勒测速仪(DVL)，数字信号处理模块(DSP)，高级精简指令集微处理器(ARM)。

MEMS-IMU集成了三轴加速度计和三轴陀螺仪，输出的三轴加速度和角速率信息通过惯性导航算法得到水下滑翔器的姿态、速度、位置导航信息。同时在上浮精度校正时，与GPS信号进行数据融合，以校正导航定位误差。

三轴磁力计用于互补滤波算法中，校正滑翔器的航向信息。同时若航向变化过大，则反映磁场干扰较强，需要进行上浮精度校正操作。

多普勒测速仪用于判断水下滑翔器的运动状态，若速度变化过大，则反映滑翔器受洋流涌动影响较大，也需要进行上浮精度校正操作。

GPS用于滑翔器上浮校正操作中，利用设计的基于多重自适应渐消因子的H ∞卡尔曼滤波算法，同IMU数据进行融合，校正滑翔器的速度、位置误差。

ARM和DSP构成了水下滑翔器的组合导航硬件处理系统。水下工作状态， ARM用于磁力计、IMU以及DVL信号的接收处理与时钟同步工作。DSP用于惯性导航四元数算法的解算工作。上浮校正状态，ARM用于GPS、IMU信号的接收处理与时钟同步工作。DSP用于基于多重自适应渐消因子的H∞卡尔曼滤波算法解算工作。

本发明公开的一种用于水下滑翔器导航定位系统的上浮精度校正方法，包含以下步骤：

(1)当水下滑翔器导航系统处于水下工作状态时，若系统启用MEMS-IMU 和磁力计模块，采用基于互补滤波的四元数算法计算滑翔器的姿态、速率和位置。四元数基本算法可以参见秦永元教授所编《惯性导航》一书，互补滤波理论可以参见陈熙源教授所写的发明专利《一种基于互补滤波的水下滑翔器节能算法》。

(2)判断水下滑翔器是否需要进行上浮精度校正操作。水下滑翔器工作过程中本身运动并不剧烈，若根据上述四元数算法计算得到的航向变化过大(磁场干扰较强)、速度变化过大(洋流涌动较大)，或人为判定滑翔器导航定位误差过大时，滑翔器上浮将至水面，进行精度校正操作。

(3)滑翔器上浮将至水面后，通过ARM+DSP组合导航硬件处理系统接收 GPS的经纬度、高度、三轴速度信号，IMU的三轴加速度，三轴角速率信号。DSP 对接收到的多传感器信号进行数据融合。在保证MEMS-IMU和GPS可靠性和精度的基础上，采用设计的一种基于多重自适应渐消因子的H∞卡尔曼滤波算法。将该算法应用于如图2所示的GPS/INS位置速度校正的松组合导航系统，以完成数据融合。在滑翔器上浮校正过程中，IMU输出三轴加速度和角速率信息，通过惯性导航系统解算得到滑翔器的位置、速率、姿态角。GPS系统直接输出滑翔器的位置、速率。将GPS系统与INS系统单独输出的信息(位置、速率)作差作为外部观测量输入组合导航滤波器。通过滤波器不断的迭代，输出组合导航系统的最优状态量(位置、速率误差)用于修正包含各种噪声误差的导航位置和速率(姿态角无法在松组合模式下，进行修正)。

上浮精度校正中，采用的一种基于多重自适应渐消因子的H∞卡尔曼滤波算法流程图如图3所示，具体步骤如下：

①自适应渐消因子建立

将组合导航系统考虑为一个线性动力学系统

x_k＝Φ_k|k-1x_k-1+w_k-1

z_k＝H_kx_k+v_k-1

其中，x_k为k时刻系统状态量，Φ_k|k-1为状态转移矩阵，w_k-1为系统噪声；z_k为k时刻系统观测量，H_k为观测矩阵，v_k-1为观测噪声。同时，噪声的期望和协方差为：

E(w_k-1)＝0,var(w_k-1)＝Q_k-1

E(v_k-1)＝0,var(v_k-1)＝R_k-1

自适应渐消因子卡尔曼滤波算法参见Yanrui Geng的《Adaptive estimation ofmultiple fading factors in Kalman filter for navigation applications》一作。写出具体的自适应渐消因子卡尔曼滤波方程：

x_k＝Φ_k|k-1x_k-1

V_k＝z_k-H_kx_k|k-1

x_k＝x_k|k-1+K_kV_k

P_k＝(I-K_kH_k)P_k|k-1

式中，P_k|k-1为状态预测协方差矩阵，K_k为增益阵，为综合状态空间和观测空间的不确定度得到的一个增益系数，该增益系数决定了外部观测量对整个系统的校正能力。V_k为新息矩阵，代表外部观测量z_k与系统预测的状态量x_k|k-1的差异大小。与标准卡尔曼滤波方程相比，仅多了S_k渐消因子矩阵。用于解决对研究对象所建立的数学模型不能真实反映实际的物理过程以及缺乏对系统噪声统计特性的了解时，建模与获得的量测值存在不匹配情况，容易导致滤波发散的问题。自适应渐消因子阵S_k由以下算法得到：

S_k＝diag(s₁,s₂,s₃...,s_n)

式中，λ_i为观测阵H_k第i个观测元素，j_ii(k)为矩阵的第i个对角线元素,ε_i为卡方检测的阈值，υ_i(k)为残差阵V_k第i个观测元素， b_ii(k)为矩阵的第i个对角线元素。值得注意的是，只有观测方程可观测的状态量才进行渐消因子计算，否则退化为标准卡尔曼滤波方程。

②自适应H∞卡尔曼滤波算法

考虑一个线性离散系统

其中，L_k为对系统状态量的线性组合的估计，常取为I，则y_k表示对状态量x_k的直接估计。

因此构造的造价函数J为：

式中，N为整体滤波时间限定，x₀为系统初始状态量，其余已作介绍。H∞滤波中心思想是保证造价函数J的H∞范数最小，也就保证了从干扰信号到估计误差的最大能量增益最小。从而使得外界扰动和模型不确定对系统输出的影响最小。

min||J||_∞＝γ₀

但由于H∞最优估计问题的封闭解仅在某些特定情况下可以得到，因此通常考虑的是设计H∞次优滤波。即给定一个足够接近γ₀的阈值γ，使构造的代数函数J的H∞范数小于γ即可。

如果Φ_k|k-1满秩，则H∞次优问题存在解的充分必要条件可由黎卡提不等式(Riccati inequality)给出:

式中，P_k为系统状态量x_k的最优协方差估计。

值得注意的是，滤波器的鲁棒性能与选择的阈值γ有关。阈值γ越小越接近γ₀，滤波器鲁棒性越强。但γ不能小于γ₀，否则H∞次优问题不存在解，将导致滤波器发散。同时，若γ趋近于无穷时，H无穷滤波器退化为传统kalman滤波器。常规阈值γ一般根据实际滑翔器工作经验确定且无法改变，从而使滤波的效果具有较大的保守性。不能实时保证系统的估计误差较小的同时系统又具有较高的鲁棒性。因此如果阈值γ的取值能够自适应于水下滑翔器不同的上浮校正环境，则能够在保证鲁棒性的基础上进一步提高上浮校正的导航定位精度。因此根据黎卡提不等式建立阈值γ的自适应算法如下：

式中，ρ()表示矩阵的谱半径。令由于最优状态估计协方差P_k不断在滤波迭代中更新，因此γ_a是一个时变因子。

理想情况卡尔曼滤波的新息阵但系统不确定性影响会导致观测量的异常，从而引起滤波器的失常。这样的情况会引起新息序列平方和的的变化。较大时，反映不确定性干扰对系统造成影响较大，此时应增加系统鲁棒性(阈值γ应减小)，反之较小时，反映干扰较小，此时应更关注于系统导航定位精度(阈值γ应增大)。则自适应阈值γ为：

γ＝η·γ_a其中，

式中，为求迹运算，即求矩阵对角线元素之和。

与标准卡尔曼滤波器方程不同之处为：

式中，

③基于多重自适应渐消因子的H∞卡尔曼滤波算法

将步骤①②的算法合并，形成整体的水下滑翔器上浮精度校正的组合导航算法(基于多重自适应渐消因子的H∞卡尔曼滤波算法)。算法流程图如图3所示，组合导航滤波器分为三个回路(滤波回路、增益回路、渐消回路)进行解算，具体流程如下：

i)首先进行时间更新。根据上一时刻滤波输出的最优状态量估计x_k-1一步预测当前时刻的状态量x_k|k-1，根据上一时刻滤波输出的最优协方差估计阵P_k-1和渐消回路输出的渐消因子阵S_k一同预测当前时刻的状态预测协方差P_k|k-1。

ii)其次进行量测更新。根据当前时刻的状态预测协方差P_k|k-1计算增益阵K_k，在接收新的外部观测量Z_k后利用增益阵K_k和一步预测状态量x_k|k-1更新系统最优状态量估计x_k，再根据P_k|k-1和时变因子γ_a计算H∞阈值γ后，计算得到H∞滤波状态最优协方差P_k。

iii)最后进行渐消因子更新。根据H∞滤波状态最优协方差P_k更新渐消回路的过渡阵后，计算得到多重渐消因子阵S_k。

(4)通过采用的一种基于多重自适应渐消因子的H∞卡尔曼滤波算法使得组合导航系统的速度、位置误差逐渐减小、趋于零。若误差小于一设定的阈值，则可认为上浮校正工作完成，滑翔器可切换至水下工作状态。

Claims (9)

1.一种水下滑翔器导航定位系统，其特征在于，包括微机电系统惯性测量单元MEMS-IMU，全球卫星定位系统接收模块GPS，三轴磁力计，多普勒测速仪DVL以及组合导航硬件处理系统；所述MEMS-IMU集成了三轴加速度计和三轴陀螺仪，输出的三轴加速度和角速率信息通过惯性导航算法得到水下滑翔器的姿态、速度和位置导航信息；所述三轴磁力计用于校正滑翔器的航向信息，所述多普勒测速仪用于判断水下滑翔器的运动状态；所述GPS用于滑翔器上浮校正操作中，与MEMS-IMU组成GPS/INS位置速度校正的松组合导航系统，利用组合导航滤波器算法，校正滑翔器的速度、位置误差；所述组合导航硬件处理系统用于三轴磁力计、GPS、MEMS-IMU以及DVL信号的接收处理与时钟同步工作，以及组合导航多传感器信息的算法解算工作。

2.根据权利要求1所述的水下滑翔器导航定位系统，其特征在于，所述水下滑翔器导航定位系统具有两种运行状态：水下工作状态和上浮校正状态；若导航定位误差过大，则滑翔器停止实时工作，由水下工作状态切换为上浮校正状态；当GPS/INS组合导航系统的速度、位置误差小于设定阈值时，则滑翔器由上浮校正状态切换为水下工作状态，继续工作。

3.根据权利要求1所述的水下滑翔器导航定位系统，其特征在于，水下工作利用MEMS-IMU和三轴磁力计输出的信号，采用基于互补滤波的四元数算法完成滑翔器的导航定位。

4.根据权利要求1所述的水下滑翔器导航定位系统，其特征在于，上浮校正利用GPS和MEMS-IMU输出的信号，采用GPS/INS位置速度校正的松组合导航系统，完成滑翔器的导航定位；其中组合导航滤波器算法采用基于多重自适应渐消因子的H∞卡尔曼滤波算法；

所述滤波算法的状态预测协方差阵

H∞滤波增益阵

H∞滤波状态最优协方差阵

式中k和k-1分别表示当前时刻和前一时刻，Φ_k|k-1为状态转移矩阵，H_k为观测矩阵，Q_k-1为系统噪声协方差，S_k为多重自适应渐消因子阵，L_k为对系统状态量的线性组合的估计，I为单位阵，γ为自适应阈值。

5.根据权利要求4所述的水下滑翔器导航定位系统，其特征在于，所述多重自适应渐消因子阵S_k＝diag(s₁,s₂,s₃...,s_n)由下式计算：

式中，λ_i为观测阵H_k第i个观测元素，j_ii(k)为矩阵的第i个对角线元素,ε_i为卡方检测的阈值，υ_i(k)为新息阵V_k第i个观测元素，b_ii(k)为矩阵的第i个对角线元素。

6.根据权利要求5所述的水下滑翔器导航定位系统，其特征在于，所述自适应阈值γ＝η·γ_a；其中， Trace()表示矩阵求迹运算，ρ()表示矩阵的谱半径。

7.一种用于水下滑翔器导航定位系统的上浮精度校正方法，其特征在于，包括如下步骤：

(1)判断水下滑翔器是否需要进行上浮精度校正操作，当航向变化过大、速度变化过大，或人为判定滑翔器导航定位误差过大时，滑翔器上浮将至水面，进行精度校正操作；

(2)滑翔器上浮至水面后，接收GPS的经纬度、高度、三轴速度信号，IMU的三轴加速度，三轴角速率信号，将基于多重自适应渐消因子的H∞卡尔曼滤波算法应用于GPS/INS位置速度校正的松组合导航系统，完成数据融合；

(3)通过步骤(2)使得组合导航系统的速度、位置误差逐渐减小、趋于零，若误差小于一设定的阈值，则认为上浮校正工作完成，滑翔器切换至水下工作状态。

8.根据权利要求7所述的用于水下滑翔器导航定位系统的上浮精度校正方法，其特征在于，所述基于多重自适应渐消因子的H∞卡尔曼滤波算法的状态预测协方差阵

H∞滤波增益阵H∞滤波状态最优协方差阵式中k和k-1分别表示当前时刻和前一时刻，Φ_k|k-1为状态转移矩阵，H_k为观测矩阵，Q_k-1为系统噪声协方差，S_k为多重自适应渐消因子阵，L_k为对系统状态量的线性组合的估计，I为单位阵，γ为自适应阈值。

9.根据权利要求8所述的用于水下滑翔器导航定位系统的上浮精度校正方法，其特征在于，所述基于多重自适应渐消因子的H∞卡尔曼滤波算法的解算流程包括：

首先进行时间更新，具体包括：根据上一时刻滤波输出的最优状态量估计x_k-1一步预测当前时刻的状态量x_k|k-1＝Φ_k|k-1x_k-1，根据上一时刻滤波输出的最优协方差估计阵P_k-1和渐消回路输出的渐消因子阵S_k一同预测当前时刻的状态预测协方差

其次进行量测更新，具体包括根据当前时刻的状态预测协方差P_k|k-1计算增益阵在接收新的外部观测量Z_k后利用增益阵K_k和一步预测状态量x_k|k-1更新系统最优状态量估计x_k，再根据P_k|k-1和时变因子γ_a计算H∞阈值γ后，计算得到H∞滤波状态最优协方差

最后进行渐消因子更新，具体包括：根据H∞滤波状态最优协方差P_k更新渐消回路的过渡阵后，计算得到多重渐消因子阵S_k＝diag(s₁,s₂,s₃...,s_n)，其中

式中，λ_i为观测阵H_k第i个观测元素，j_ii(k)为矩阵的第i个对角线元素,ε_i为卡方检测的阈值，υ_i(k)为新息阵V_k第i个观测元素，b_ii(k)为矩阵的第i个对角线元素。