CN111649744A - 一种基于动力学模型辅助的组合导航定位方法 - Google Patents

Abstract

本发明提供了一种基于动力学模型辅助的组合导航定位方法，首先构建AUV动力学模型并采用惯性导航信息对AUV动力学模型角速度进行修正，然后建立SINS、GPS、DVL、AUV动力学模型的组合导航定位系统，建立组合导航定位系统的联邦滤波器，子滤波器进行单状态卡方检验，检验正常后开展最小结构联邦滤波数据融合，最后将数据融合结果对各主子滤波器公共状态进行重置，非公共状态采用误差平方根分析的方法进行补偿。本发明可以大幅提高以水下自主潜器为平台的组合导航性能，实现高精度可靠的深远海航行。

Description

一种基于动力学模型辅助的组合导航定位方法

技术领域

本发明属于组合导航技术领域，涉及一种基于动力学模型辅助的组合导航定位方法，适用于长航时的自主水下潜器(AUV)水下导航定位系统。

背景技术

由于海水介质对光波和电磁波能量的大幅度衰减，使得水面无线电、卫星等常规导航方式在水下均不可用，惯性导航系统(SINS)因具备自主性、隐蔽性和能获取运载体完备运动信息的优点，成为水下隐蔽导航的主要方式。但是SINS定位精度随时间积累不断下降，而为了满足军事领域水下布雷、民用领域深海探测科考的需求，水下自主潜器的航程有所增加，工作时间长达十几小时甚至数天，因此必须开展高精度、深远海条件下的组合导航技术研究。

声波是水下唯一能够进行远距离传播的信号，且声学多普勒测速系统(DVL)能够提供不随时间发散的高精度测速信息。SINS/DVL组合导航，既可以抑制惯导系统的误差增长率，也解决了DVL所需的高精度航向姿态问题，是解决水下导航最常用的组合方式。目前国内“百米级”底跟踪距离的SINS/DVL组合导航精度较国外相比存在一定的差距，“千米级”底跟踪距离的SINS/DVL组合导航技术鲜有研究，无法满足水下自主潜器高精度导航的需求。

目前水面导航以SINS/GPS为主要方式，水下导航以SINS/DVL为主要方式，如何将其进行信息融合是组合导航系统的关键。工程中多以集中式滤波为主要融合手段，该方法为最优估计，通常需增大算法的运算量，且容错性较低。而以联邦式结构的分散式滤波降低了运算量，且可以根据冗余量进行故障诊断，但在主、子滤波器维度不同时为次优估计，因此有一定局限。另外在深远海等恶劣条件下，若失去外界参考信息，导航系统将工作在纯惯性导航模式，误差积累严重。

因此，开展高精度可靠的深远海组合导航系统、深远海条件下的水下多源信息组合导航技术等方面研究，对于大幅提高以水下自主潜器为平台的组合导航性能，进而增强海军水下作战力量、海洋勘探能力具有重要作用。

发明内容

本发明针对现有技术中水下组合导航存在误差、信息融合精度低等技术问题，提供了一种基于动力学模型辅助的组合导航定位方法，通过构建AUV动力学模型，结合SINS、GPS、DVL信息构建联邦滤波组合导航系统，通过多源信息有效融合，实时抑制惯导系统误差的发散，提高组合导航系统定位精度。

本发明解决上述技术问题采用的技术方案如下：

一种基于动力学模型辅助的组合导航定位方法，包括如下步骤：

S1、根据实际使用的AUV构建动力学模型，利用惯导系统解算的角速度信息对动力学模型中的角速度项进行实时修正；

S2、构建组合导航系统及其联邦滤波模型，建立SINS误差状态方程以及与GPS、DVL、Model三者间卡尔曼滤波的状态方程和观测方程；

S3、在数据融合之前，各子滤波器单独进行滤波，到达融合时间之后先对子滤波器进行一维状态卡方检验，检验正常之后对多源信息进行融合；

S4、开展最小结构联邦滤波数据融合，将数据融合结果对各子滤波器公共状态进行重置，非公共状态采用误差平方根分析的方法进行补偿；重复步骤S3、S4至航程结束。

进一步的，AUV动力学模型三个方向的角速度对应为惯导输出的角速度分量。

进一步的，所述步骤S1中AUV动力学模型按照x(k+1)＝x(k)+ΔT·M·F(k)的方式递推，其中，x(k+1)为AUV运动信息，ΔT为模型递推的时间周期，M为驱动矩阵，F(k)为AUV所受外力。

进一步的，所述步骤S2具体包括如下步骤：

S2.1、主滤波器的状态为惯导系统的状态量X_M，

X_M＝[δv_N δv_E θ_N θ_U θ_E ε_x ε_y ε_z Δ_x Δ_y Δ_z]^T

其中，δv_N、δv_E为速度误差，θ_N、θ_U、θ_E为姿态误差，ε_x、ε_y、ε_z为陀螺漂移，Δ_x、Δ_y、Δ_z为加表零位；

S2.2、动力学模型辅助的子滤波器：

状态方程为

观测方程为

Z_Model＝HX_Model+V

动力学模型辅助的子滤波器状态为

X_Model＝X_M

观测信息为

S2.3、设计GPS子滤波器：

状态方程为

观测方程为

Z_GPS＝HX_GPS+V

GPS子滤波器状态为

其中，dλ、

为惯导纬度、经度位置误差；

GPS子滤波器观测信息为

S2.4、设计DVL子滤波器：

状态方程为

观测方程为

Z_DVL＝HX_DVL+V

DVL子滤波器的状态为

X_DVL＝[δv_N δv_E θ_N θ_U θ_E ε_x ε_y ε_z Δ_x Δ_y Δ_z γ ψ θ k_dvl]^T

其中，[γ ψ θ]为惯导与DVL的安装误差角，k_dvl为DVL速度的标度因数；

观测信息为

进一步的，所述步骤S3具体包括如下步骤：

S3.1、以主滤波器作为状态递推器，判断是否到达融合时间，若是，则对子滤波器进行速度、姿态信息估计值的一维状态卡方检验；若不是，则由主滤波器进行时间更新；

S3.2、判断卡方检验数据是否正常，若是，则进行数据融合，否则，返回子滤波器进行数据采集。

进一步的，所述卡方检验方法如下：

计算子滤波器有故障和无故障的统计检验值差值

式中e_k,0(k)为子滤波器无故障时的状态估计值，Δe_k(k)为子滤波器在无故障和有故障时的状态估计差值，λ_β,0(k)、λ_β(k)分别为子滤波器无故障和有故障的统计检验值，P_s(k)、P(k)分别为子滤波器无故障和有故障时的状态估计协方差，e_s(k)为状态递推器即主滤波器的状态估计结果；

判断子滤波器无故障和有故障的统计检验值差值与检验阈值的大小，若统计检验值差值小于检验阈值则正常，否则不正常。

进一步的，所述步骤S4具体包括如下步骤：

计算状态估计的融合结果

状态协方差的融合结果P_g：

其中，

为各主子滤波器的状态估计值，P_i为各主子滤波器的协方差，i＝1,2...N,N为滤波器个数；

计算补偿后的各主子滤波器状态估计值

为

式中P_bci为公共状态与非公共状态间的协方差，P_ci为非公共状态协方差；

计算补偿后的主子滤波器的协方差矩阵P_i ⁺为

式中P_cbi为非公共状态与公共状态间的协方差，P_bi为公共状态协方差，β_i为信息分配系数，满足

本发明的有益效果：

(1)本发明提出了SINS、GPS、DVL、AUV动力学模型组合导航定位方法。针对AUV动力学模型在导航过程中存在精度损失的问题，通过分解出AUV动力学模型中的主要误差源，提出了利用惯导信息进行修正的方法，对其进行补偿，提高了动力学模型输出AUV速度信息的准确度。当AUV在深海环境中，没有其他参考信息时(即没有SINS、GPS、DVL导航信息)，可以利用AUV动力学模型输出的速度信息进行自主导航，减轻了系统对外界参考信息的过度依赖。

(2)本发明提出了基于联邦滤波和状态卡方检验的组合导航多信息融合方法。在构建联邦滤波组合导航系统的过程中，针对所设计的主滤波器维数最小的结构特点，采用对非公共项滤波估计值和协方差进行补偿的方法，提升非公共状态估计的准确性与快速性。在应用状态卡方检验进行故障诊断过程中，采用主滤波器作为状态递推器，避免状态递推器随时间发散。针对状态卡方整体性检验灵敏度下降的问题，通过对每一个状态单独检验，提高对故障的灵敏度。

附图说明

所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例中组合导航定位方法原理框图；

图2为本发明实施例中AUV动力学模型修正方法原理框图；

图3为本发明实施例中组合导航定位联邦滤波原理框图；

图4为本发明实施例中状态卡方检验方法流程图。

具体实施方式

下面对本发明的具体实施例进行详细说明。在下面的描述中，出于解释而非限制性的目的，阐述了具体细节，以帮助全面地理解本发明。然而，对本领域技术人员来说显而易见的是，也可以在脱离了这些具体细节的其它实施例中实践本发明。

在此需要说明的是，为了避免因不必要的细节而模糊了本发明，在附图中仅仅示出了与根据本发明的方案密切相关的设备结构和/或处理步骤，而省略了与本发明关系不大的其他细节。

本发明提供了一种基于动力学模型辅助的组合导航定位方法，如图1所示，包括如下步骤：

S1、构建基于惯性信息辅助的AUV动力学模型

S1.1首先根据AUV在水下运动特点，构建动力学模型(动力学模型简称Model)。由于AUV垂直面和水平面运动参数的耦合很小，可忽略不计。将AUV动力学方程和AUV运动学方程结合即得到水平面三自由度和垂直面三自由度动力学模型。

(1)水平面动力学模型

AUV在水平面上运动时，存在二维水平面线运动和一维绕y_b轴的转动，即w＝0,p＝0,q＝0，则

(2)垂直面动力学模型

AUV在垂直面上运动时，存在二维垂直面线运动和一维绕z_b轴的转动，即v＝0,p＝0,r＝0，则

以上式中u、v、w为AUV前上右三个方向的速度，ψ为航向角，θ为俯仰角，γ为滚转角，p、q、r为前上右三个方向的角速度，m、L、I_z为AUV自身参数。G为重力，B为浮力，ρ为海水密度。X为AUV推力，M、N为外力矩。其他参数为AUV的水动力系数，以X_u′_u为例，X′为前向水动力，u为前向速度，X_u′_u即为前向水动力关于前向速度的二阶导，X_u′为前向水动力关于前向速度的一阶导。其他水动力参数均通过角标方式进行描述，这里不一一赘述，使用时均可通过查表与测量进行获取。

S1.2、利用惯导系统解算的角速度信息对动力学模型中的角速度项进行实时修正，如图2所示。

AUV动力学模型可以根据输入的操纵力(即推力和推力矩)输出得到AUV运动的速度信息，而在实际应用中，AUV的推力是由螺旋桨转速按照对应的参数计算公式得到，操纵力矩是根据舵偏角计算得到的，都需要先对其转换系数进行辨识。一个准确的动力学模型是确保输出速度信息准确的关键，而在实际过程中，很难得到一个完全准确的动力学模型，因此在递推过程中可将AUV的速度信息稳定到带有误差的数值上，比如在转弯过程中，如果忽略洋流的自旋，动力学模型输出的转动角速度应该是绝对角速度，而由于水动力系数的不准确，会对递推结果造成影响，而惯导输出的角速度信息相对准确，因此可以利用惯导的角速度信息对动力学模型进行修正，使动力学模型在水动力系数不完全准确的条件下仍能输出相对洋流的速度信息。

首先对公式(1)(2)进行变形，可以得到k时刻的水动力计算方程：

上式中，X_f，Y_f，Z_f为AUV所受合外力分量。

当模型输入的操纵力矩与实际值不相符时，转速计算必然会有误差，根据方程(3)，载体角速度的误差会传递到水动力计算方程当中。因此可以利用惯导输出的角速度

对模型进行修正，即：

将修正之后的角速度作用于水动力模型，进而对AUV受力进行修正，之后继续按x(k+1)＝x(k)+ΔT·M·F(k)的方式进行递推。其中，x(k+1)为AUV运动信息，即速度位置姿态；ΔT为模型递推的时间周期；M为驱动矩阵，即为式(1)(2)中所描述AUV所受外力与运动信息的关系；F(k)为AUV所受外力。

S2、构建组合导航系统模型，构建基于SINS/GPS/DVL/Model多源信息融合的联邦滤波器，如图3所示，建立SINS误差状态方程以及与GPS、DVL、Model三者间卡尔曼滤波的状态方程和观测方程。

S2.1、设计主滤波器

主滤波器的状态为惯导系统的状态量X_M，

X_M＝[δv_N δv_E θ_N θ_U θ_E ε_x ε_y ε_z Δ_x Δ_y Δ_z]^T

其中，δv_N、δv_E为速度误差，θ_N、θ_U、θ_E为姿态误差，ε_x、ε_y、ε_z为陀螺漂移，Δ_x、Δ_y、Δ_z为加表零位。

S2.2、设计动力学模型辅助的子滤波器：

状态方程：

观测方程：

Z_Model＝HX_Model+V (6)

动力学模型辅助的子滤波器状态与主滤波器相同，均为11维

X_Model＝X_M (7)

由于AUV模型动力学可以输出载体的体坐标系速度，因此观测信息为

上式中的V_Model即为动力学模型输出的u，v，w三维速度信息。

S2.3、设计GPS子滤波器：

状态方程：

观测方程：

Z_GPS＝HX_GPS+V (10)

GPS子滤波器状态在惯导系统公共状态的基础上增加了位置误差：

其中，dλ、

为惯导纬度、经度位置误差。

GPS子滤波器观测信息为：

S2.4、设计DVL子滤波器：

状态方程：

观测方程：

Z_DVL＝HX_DVL+V (13)

DVL子滤波器的状态量有15个：

X_DVL＝[δv_N δv_E θ_N θ_U θ_E ε_x ε_y ε_z Δ_x Δ_y Δ_z γ ψ θ k_dvl]^T

其中，[γ ψ θ]为惯导与DVL的安装误差角，k_dvl为DVL速度的标度因数。

DVL子滤波器观测信息为：

S3、在数据融合之前，各子滤波器单独进行滤波，主滤波器进行时间更新，到达融合时间之后先对子滤波器进行一维状态卡方检验，检验正常之后对多源信息进行融合。

卡方检验过程如图4所示：

S3.1、以主滤波器作为状态递推器，判断是否到达融合时间，若是，则对子滤波器进行速度、姿态信息估计值的一维状态卡方检验；若不是，则由主滤波器进行时间更新；

S3.2、判断卡方检验数据是否正常，若是，则进行数据融合，否则，返回子滤波器进行数据采集。

上述融合时间根据实际需要进行设定，本实施例中，融合时间为5秒一次。

本实施例中，一维状态即单状态卡方检验方法方法如下：

对于单输入单输出系统，子滤波器在无故障和有故障时的状态估计差值Δe_k(k)反映了出现故障对滤波估值的影响：

Δe_k(k)＝e_k(k)-e_k,0(k) (15)

式中e_k,0(k)，e_k(k)分别为子滤波器在k时刻无故障和有故障时的状态估计值。

子滤波器无故障和有故障的统计检验值λ_β,0(k)和λ_β(k)分别为：

式中，P_s(k)，P(k)分别为子滤波器无故障和有故障时的状态估计协方差，e_s(k)为状态递推器即主滤波器的状态估计结果。

子滤波器有故障和无故障的统计检验值差值为：

检验阈值Δλ_β可通过查表进行获取，若统计检验值差值小于检验阈值则正常，否则不正常。

从式(17)可以看出，当状态递推器的精度变低，导致上式分母变大，因此当发生故障时，本来通过使Δe_k变大检验出故障，但由于e_k,0(k)-e_s(k)很大，这样就降低了故障检验的敏感度。本发明针对联邦滤波器的特点，设计采用主滤波器作为状态递推器的方法，在融合时间到来之前，主滤波器只进行时间更新，保证不受子系统故障的影响，到达融合时间之后，先进行卡方检验，无故障再去融合，最后用融合结果重置主滤波器，保证其不随时间发散。另一方面，针对状态卡方整体性检验灵敏度下降的问题，采用对速度估计值和姿态估计值等每一个状态单独检验的方法，这样λ服从自由度为一的卡方分布，以提高对故障的灵敏度。

S4、开展最小结构联邦滤波器数据融合，将数据融合结果对各子滤波器进行重置，由于在数据融合过程中，只能对公共状态进行融合，因此重置时只能对公共状态进行重置，非公共状态采用误差平方根分析的方法进行补偿，使重置之后各子滤波器的状态估计值保持最优。

将X_c表示为主子滤波器公共状态，X_bi表示为子滤波器独有状态，整体状态估计值

为：

在用联邦滤波器信息融合的结果对主子滤波器进行重置的过程中，只能对公共状态进行重置，即

为状态估计的融合结果，P_g为状态协方差的融合结果，

为各主子滤波器的状态估计值，P_i为各主子滤波器的协方差，i＝1,2...N,N为滤波器个数。本实施例中主子滤波器共四个，i＝4代表主滤波器，对于本系统

分别为

P_i分别为其对应协方差P_GPS、P_DVL、P_Model、P_M。

再考虑主子滤波器的协方差矩阵P_i，可以写为如下形式

P_ci为由P_g重置之后的子滤波器公共状态协方差阵，对于非公共项的协方差阵P_bi以及公共项与非公共项的协方差阵P_bci和P_cbi无法重置，只能保留原始状态，这样就会导致P_i阵的信息丢失，因此不是最优的。基于误差平方根分析的方法，可以对系统的非公共状态估计值进行补偿，即补偿后的主子滤波器的状态估计值

为

式中P_bci为公共状态与非公共状态间的协方差，P_ci为非公共状态协方差，

为非公共状态估计值，

为公共状态估计值。

补偿后的主子滤波器的协方差矩阵P_i ⁺为，

式中，P_cbi为非公共状态与公共状态间的协方差；P_bi为公共状态协方差；β_i为信息分配系数，满足

可根据对子系统的信任度进行合理分配；P_g为状态协方差的融合结果。

通过对重置之后的各主子滤波器状态估计和状态估计协方差进行补偿，用

P_i ⁺取代传统的

P_i可提高状态估计值的精确度。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

本发明未详细说明部分为本领域技术人员公知技术。

Claims (7)

1.一种基于动力学模型辅助的组合导航定位方法，其特征在于，包括如下步骤：

S1、根据实际使用的AUV构建动力学模型，利用惯导系统解算的角速度信息对动力学模型中的角速度项进行实时修正；

S2、构建组合导航系统及其联邦滤波模型，建立SINS误差状态方程以及与GPS、DVL、Model三者间卡尔曼滤波的状态方程和观测方程；

S3、在数据融合之前，各子滤波器单独进行滤波，到达融合时间之后先对子滤波器进行一维状态卡方检验，检验正常之后对多源信息进行融合；

S4、开展最小结构联邦滤波数据融合，将数据融合结果对各子滤波器公共状态进行重置，非公共状态采用误差平方根分析的方法进行补偿；重复步骤S3、S4至航程结束。

2.根据权利要求1所述的组合导航定位方法，其特征在于，所述步骤S1中，AUV动力学模型三个方向的角速度对应为惯导输出的角速度分量。

3.根据权利要求2所述的组合导航定位方法，其特征在于，所述步骤S1中AUV动力学模型按照x(k+1)＝x(k)+ΔT·M·F(k)的方式递推，其中，x(k+1)为AUV运动信息，ΔT为模型递推的时间周期，M为驱动矩阵，F(k)为AUV所受外力。

4.根据权利要求1所述的组合导航定位方法，其特征在于，所述步骤S2具体包括如下步骤：

S2.1、主滤波器的状态为惯导系统的状态量X_M，

X_M＝[δv_N δv_E θ_N θ_U θ_E ε_x ε_y ε_z Δ_x Δ_y Δ_z]^T

其中，δv_N、δv_E为速度误差，θ_N、θ_U、θ_E为姿态误差，ε_x、ε_y、ε_z为陀螺漂移，Δ_x、Δ_y、Δ_z为加表零位；

S2.2、动力学模型辅助的子滤波器：

状态方程为

观测方程为

Z_Model＝HX_Model+V

动力学模型辅助的子滤波器状态为

X_Model＝X_M

观测信息为

S2.3、设计GPS子滤波器：

状态方程为

观测方程为

Z_GPS＝HX_GPS+V

GPS子滤波器状态为

其中，dλ、

为惯导纬度、经度位置误差；

GPS子滤波器观测信息为

S2.4、设计DVL子滤波器：

状态方程为

观测方程为

Z_DVL＝HX_DVL+V

DVL子滤波器的状态为

X_DVL＝[δv_N δv_E θ_N θ_U θ_E ε_x ε_y ε_z Δ_x Δ_y Δ_z γ ψ θ k_dvl]^T

其中，[γ ψ θ]为惯导与DVL的安装误差角，k_dvl为DVL速度的标度因数；

观测信息为

5.根据权利要求1所述的组合导航定位方法，其特征在于，所述步骤S3具体包括如下步骤：

S3.1、以主滤波器作为状态递推器，判断是否到达融合时间，若是，则对子滤波器进行速度、姿态信息估计值的一维状态卡方检验；若不是，则由主滤波器进行时间更新；

S3.2、判断卡方检验数据是否正常，若是，则进行数据融合，否则，返回子滤波器进行数据采集。

6.根据权利要求5所述的组合导航定位方法，其特征在于，所述卡方检验方法如下：

计算子滤波器有故障和无故障的统计检验值差值

式中e_k,0(k)为子滤波器无故障时的状态估计值，Δe_k(k)为子滤波器在无故障和有故障时的状态估计差值，λ_β,0(k)、λ_β(k)分别为子滤波器无故障和有故障的统计检验值，P_s(k)、P(k)分别为子滤波器无故障和有故障时的状态估计协方差，e_s(k)为状态递推器即主滤波器的状态估计结果；

判断子滤波器无故障和有故障的统计检验值差值与检验阈值的大小，若统计检验值差值小于检验阈值则正常，否则不正常。

7.根据权利要求1所述的组合导航定位方法，其特征在于，所述步骤S4具体包括如下步骤：

计算状态估计的融合结果

状态协方差的融合结果P_g：

其中，

为各主子滤波器的状态估计值，P_i为各主子滤波器的协方差，i＝1,2...N,N为滤波器个数；

计算补偿后的各主子滤波器状态估计值

为

式中P_bci为公共状态与非公共状态间的协方差，P_ci为非公共状态协方差；

计算补偿后的主子滤波器的协方差矩阵P_i ⁺为

式中P_cbi为非公共状态与公共状态间的协方差，P_bi为公共状态协方差，β_i为信息分配系数，满足

Images