CN107063245B - 一种基于5阶ssrckf的sins/dvl组合导航滤波方法 - Google Patents

Abstract

本发明公开了一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法，具体包括：以水下航行器为研究对象，建立了捷联惯性导航非线性误差模型，并以多普勒测速仪作为辅助建立了组合导航模型；采用SSR规则选取CKF的容积点，并建立5阶SSRCKF的非线性滤波模型；本发明采用SSR规则对标准CKF滤波方法进行改进，在一定程度上减少了CKF滤波方法的容积采样点，在高阶系统中效果显著，解决了传统CKF算法在高阶系统中容积采样点的数目太大导致算法的实时性不好和效率不高的问题。

Description

一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法

技术领域

本发明属于导航领域，特别是涉及一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法。

背景技术

捷联惯性导航系统能实时测量系统所处的位置、当前速度和姿态等导航信息，并能在短时间内保持高精度，具有多输出信号、强隐蔽性、强自主式等优点，其系统噪声较低，但作为一种导航信息具有连续性的系统，系统误差会随时间积累，若不采取外界导航信息校正，其精度将会快速下降而导致无法使用。

发明内容

发明目的：为了提高AUV组合导航的精度及稳定性，并且考虑到实时的计算量，本发明提出了一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法。

技术方案：一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法，包括如下步骤：

1)建立SINS的非线性误差模型：在SINS中选取东北天地理坐标系作为导航坐标系，以水下航行器自身建立载体坐标系，并给定实际计算导航坐标系，建立惯性器件陀螺仪和速度计的误差模型，并求解系统的姿态误差方程、位置误差方程；

2)建立DVL的非线性误差模型：针对DVL误差中的刻度误差以及随机测量误差，以一阶马尔科夫过程来拟合误差方程；

3)根据步骤1和步骤2分别得到的SINS非线性误差模型和DVL非线性误差模型建立组合导航滤波模型：选取SINS的三维姿态角误差、速度误差的前两维、位置误差的前两维，三维陀螺常值漂移和加速度计的水平方向的两维常值误差的12维状态变量，选取DVL的刻度系数误差和随机常值测量误差作为状态量；然后根据步骤1和步骤2分别得到的SINS非线性误差模型和DVL非线性误差模型共同建立组合导航的状态方程；以DVL和SINS的东向、北向速度分量的差值作为观测量,建立SINS/DVL组合导航系统的观测方程；

4)建立5阶SSRCKF滤波模型，输出导航系统参数：利用5阶SSRCKF对SINS和DVL输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度、位置和姿态矩阵，得到精确的导航信息。

进一步的，所述步骤1)中建立SINS非线性误差模型的具体步骤包括：

选取东-北-天坐标系为SINS的导航坐标系并记为n系，载体系选用右-前-上记为b系，SINS解算的导航坐标系记为n′系；系统真实姿态角为

真实速度Vⁿ＝[V_E,V_N,V_U]^T，真实地理坐标P＝[L,λ,H]^T；记陀螺仪和加速度计的随机常值误差分别为ε_b和

陀螺仪和加速度计对应的扰动白噪声分别为w_g和w_f，则陀螺仪误差ε^b＝ε_b+w_g，加速度计误差

陀螺仪和加速度计的测量值为

和

和

分别为地球自转角速度和导航坐标系相对地球坐标系的旋转角速度；V_N和V_E分别表示载体的运动速度的北向分量和东向分量；R_N和R_E分别为卯酉圈半径和子午圈半径；

建立SINS的速度、姿态和位置误差方程，具体如下：

速度方程为：

姿态误差方程为：

其中：

位置误差方程为：

进一步的，所述步骤2)中建立DVL误差模型的具体步骤包括：

设定DVL刻度误差为δK_c，随机测量误差为δV_DVL，DVL的实际测量值为

DVL的理想速度为V_DVL；

DVL的速度方程为：

定义β_V,β_K分别表示DVL随机测量误差和刻度误差的相关时间；w_V,w_K为其对应的高斯白噪声；则一阶马尔科夫过程拟合的误差方程表示为：

进一步的，所述步骤3)中建立组合导航滤波模型的具体步骤包括：

对于SINS，忽略高度通道的状态量，即选取以下12维状态量：

对于DVL选取刻度误差和随机测量误差为状态量为：X_DVL＝[δK_cx,δK_cy,δV_DVLx,δV_DVLy]，共同构成SINS/DVL组合导航系统的非线性状态方程，并将非线性状态方程简记为：

选取导航坐标系下捷联解算速度和DVL速度的前两维的差值为SINS/DVL组合导航系统的观测量；

记SINS解算的速度在n系下的投影为

即速度真值Vⁿ与速度误差值

的和，方程如下：

记DVL在载体系b下的直接输出的速度为

为实际测量值在载体系下的表示；SINS解算的姿态变换矩阵为

实际计算坐标系偏离理想坐标系的误差矩阵记为

将DVL的输出速度转换到导航坐标系上为

则根据DVL的速度方程得：

式中：Vⁿ为导航坐标系下速度的真实值，δK_c为DVL的刻度误差，δV_DVL为DVL的随机测量误差，因此观测方程为：

将此观测方程简记为：

z＝h(x,t)+u(t)

式中u(t)为系统的观测噪声，并假设为零均值高斯白噪声；

以采样周期T_s作为滤波周期，并以T_s为步长，对状态方程和观测方程进行离散化，得到系统的非线性滤波模型，并简记为：

式中：w_k为随机系统噪声并且w_k～N(0,Q_k)，Q_k为噪声矩阵；u_k为随机观测噪声并且u_k～N(0,R_k)，R_k为观测噪声矩阵。

进一步的，所述步骤4)中建立5阶SSRCKF滤波模型的具体步骤包括：

41)基于SSR容积采样规则：高斯滤波积分归纳成

其中

采用基于SSR容积采样规则，求解球面积分S(r)，对于拥有n维状态量的系统，求解过程如下：取一组n维向量a_i＝[a_i1,a_i2,a_i3,…,a_in]^T,i＝1,2,…，n+1，并将向量a_i的中点投影到球面U_n得到序列

则有

选用[ξ_i,ω_i]表示滤波器的采样容积点，其中

则得到基于SSR规则下5阶非线性多维积分为：

其中：

F(ξ_i)＝f(ξ_i)+f(-ξ_i)

F(ω_i)＝f(ω_i)+f(-ω_i)；

42)时间更新过程：估计方差P_k-1/k-1通过乔列斯基(Cholesky)分解得：

根据基于SSR容积规则选取容积点集合[ξ_i,ω_i]，计算容积点：

通过状态方程来传导容积点：

因此k时刻的一步状态预测和一步预测方差为：

43)观测过程更新流程：通过观测方程传导容积点：Z_i,k/k-1＝H(X_i,k/k-1)，k时刻的观测量Z的预测以及滤波增益矩阵为：

因此K时刻的状态估计和均方差估计为：

有益效果：本发明采用非线性SINS和DVL的组合导航系统。SINS不需外部输入信息、又不向外发射能量的自主式、隐蔽性强的导航系统，能够提供位置、航向等导航信息，但其导航误差随时间累积。DVL能在波束打到水底的情况下，为载体提供高精度的对地速度，使用便捷。因此将SINS和DVL两种导航系统组合起来，并针对具有非线性特性的SINS不同运动状态设计不同的组合导航方法。当SINS系统误差模型为非线性时，扩展卡尔曼滤波(Extended Kalman Filtering，EKF)中采用泰勒展开及一阶截断会导致很大的线性化误差，从而造成EKF滤波精度低以及必须计算雅克比矩阵的不足，采用CKF，可克服提高滤波算法的精度、稳定性。针对高阶系统，提出了五阶SSRCKF在CKF的基础上可以显著的减少计算量，提高系统的实时性。

附图说明

图1是本发明流程图；

图2是本发明中建立5阶SSRCKF的流程图。

具体实施方式

下面将结合附图，对本发明的实施案例进行详细的描述；

如图1所示，本发明所述的基于5阶SSRCKF(Spherical Simplex-Radial CubatureKalman Filter,球面最简相径容积卡尔曼滤波)的SINS/DVL组合导航滤波方法，包含以下步骤：

步骤1：建立SINS(Strap-down Inertial Navigation System，捷联惯导系统)非线性误差模型：在SINS中选取东北天地理坐标系作为导航坐标系(n)，以水下航行器(Autonomous Underwater Vehicle，AUV)自身建立载体坐标系(b)，并给定实际计算导航坐标系(n′)，建立惯性器件陀螺仪和速度计的误差模型，并求解系统的姿态误差方程、位置误差方程；

选取东-北-天坐标系为SINS的导航坐标系并记为n系，载体系选用右-前-上记为b系，SINS解算的导航坐标系记为n′系；系统真实姿态角为φ＝[φ_E,φ_N,φ_U]^T，真实速度Vⁿ＝[V_E,V_N,V_U]^T，真实地理坐标P＝[L,λ,H]^T；记陀螺仪和加速度计的随机常值误差分别为ε_b和

陀螺仪和加速度计对应的扰动白噪声分别为w_g和w_f，则陀螺仪误差ε^b＝ε_b+w_g，加速度计误差

陀螺仪和加速度计的测量值为

和

和

分别为地球自转角速度和导航坐标系相对地球坐标系的旋转角速度；V_N和V_E分别表示载体的运动速度的北向分量和东向分量；R_N和R_E分别为卯酉圈半径和子午圈半径。建立SINS的速度、姿态和位置误差方程，具体如下：

速度方程为：

姿态误差方程为：

其中：

位置误差方程为：

步骤2：DVL的非线性误差模型的建立：DVL(Doppler Velocity Log，多普勒计程仪)根据多普勒效应，利用超生换能器发射的超声波来测量载体速度的仪器，多普勒计程仪测量的速度误差不随时间而累积，具有一定的实时性和自主性。当载体相对水底的距离在多普勒计程仪有效观测范围内时，多普勒计程仪测得的为载体相对水底的速度，但当多普勒计程仪工作在有效测量范围外时，多普勒计程仪测得的为载体相对于水流的速度。多普勒测速仪利用多普勒效应，由安装在载体上的发射器发射并返回的超声波频移来计算载体运动速度。其误差主要包括刻度误差以及随机测量误差，以一阶马尔科夫过程来拟合误差方程；具体如下：

假设DVL的刻度误差为δK_c，随机测量误差为δV_DVL，DVL实际测量值为

DVL的理想速度为V_DVL。

因此其速度方程可以表示为：

定义β_V,β_K分别表示DVL随机测量误差和刻度误差的相关时间；w_V,w_K为其对应的高斯白噪声；则一阶马尔科夫过程拟合的误差方程表示为：

步骤3：组合导航滤波模型的建立：根据步骤1和步骤2分别得到的SINS非线性误差模型和DVL非线性误差模型建立组合导航滤波模型，针对SINS系统，选取三维姿态角误差、速度误差的前两维、位置误差的前两维，三维陀螺常值漂移和加速度计的水平方向的两维常值误差构成12维状态变量；对于DVL系统选取刻度系数误差和随机常值测量误差作为状态量，建立组合导航的状态方程。以DVL东向、北向的速度作为观测量建立系统的观测方程；具体如下：

对于SINS，忽略高度通道的状态量，即选取以下12维状态量：

对于DVL选取刻度误差和随机测量误差为状态量为：X_DVL＝[δK_cx,δK_cy,δV_DVLx,δV_DVLy]，共同构成SINS/DVL组合导航系统的非线性状态方程，并将非线性状态方程简记为：

选取导航坐标系下捷联解算速度和多普勒测速仪速度的前两维的差值为SINS/DVL组合导航系统的观测量。

记SINS解算的速度在n系下的投影为

即速度真值Vⁿ与速度误差值

的和，方程如下：

记DVL在载体系b下的直接输出的速度为

为实际测量值在载体系下的表示；SINS解算的姿态变换矩阵为

实际计算坐标系偏离理想坐标系的误差矩阵记为

将DVL的输出速度转换到导航坐标系上为

则根据DVL的速度方程得：

式中：Vⁿ为导航坐标系下速度的真实值，δK_c为DVL的刻度误差，δV_DVL为DVL的随机测量误差，因此观测方程为：

将此观测方程简记为：

z＝h(x,t)+u(t)

式中u(t)为系统的观测噪声，并假设为零均值高斯白噪声。

以采样周期T_s作为滤波周期，并以T_s为步长，对系统的状态方程和观测方程进行离散化，得到系统的非线性滤波模型，并简记为：

式中：w_k为随机系统噪声并且w_k～N(0,Q_k)，Q_k为系统的噪声矩阵；u_k为系统的随机观测噪声并且u_k～N(0,R_k)，R_k为系统的观测噪声矩阵。

步骤4：5阶球SSRCKF滤波模型的建立，输出导航系统参数：SSR(SphericalSimplex-Radial，基于球面最短相径)采样规则的CKF(Cubature Kalman Filter，容积卡尔曼滤波)相比于同阶标准容积卡尔曼滤波采样点数目显著降低，滤波效率更高，精度也高于标准CKF。利用5阶SSRCKF对SINS和DVL输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度、位置和姿态矩阵，得到精确的导航信息。其步骤包括时间更新过程及观测过程更新过程。如图2所示，具体如下：

1)SSR容积采样规则：高斯滤波积分归纳成

其中

采用基于SSR容积采样规则，求解球面积分S(r)，对于拥有n维状态量的系统，求解过程如下：取一组n维向量a_i＝[a_i1,a_i2,a_i3,…,a_in]^T,i＝1,2,…，n+1，并将向量a_i的中点投影到球面U_n得到序列

则有

选用[ξ_i,ω_i]表示滤波器的采样容积点，其中

则得到基于SSR规则下5阶非线性多维积分为：

其中：

F(ξ_i)＝f(ξ_i)+f(-ξ_i)

F(ω_i)＝f(ω_i)+f(-ω_i)

2)时间更新过程：估计方差P_k-1/k-1通过乔列斯基(Cholesky)分解得：

根据基于SSR容积规则选取容积点集合[ξ_i,ω_i]，计算容积点：

通过状态方程来传导容积点：

因此k时刻的一步状态预测和一步预测方差为：

3)观测过程更新流程：通过观测方程传导容积点：Z_i,k/k-1＝H(X_i,k/k-1)，k时刻的观测量Z的预测以及滤波增益矩阵为：

因此k时刻的状态估计和均方差估计为：

本发明以水下航行器为研究对象，建立了捷联惯性导航非线性误差模型，并以多普勒测速仪作为辅助建立了组合导航模型；采用SSR规则选取CKF的容积点，并建立5阶SSRCKF的非线性滤波模型；本发明采用SSR规则对标准CKF滤波方法进行改进，在一定程度上减少了CKF滤波方法的容积采样点，在高阶系统中效果显著，解决了传统CKF算法在高阶系统中容积采样点的数目太大导致算法的实时性不好和效率不高的问题。

Claims (4)

1.一种基于5阶SSRCKF的SINS/DVL组合导航滤波方法，其特征在于，包括如下步骤：

1)建立SINS非线性误差模型：在SINS中选取东北天地理坐标系作为导航坐标系，以水下航行器自身建立载体坐标系，并给定实际计算导航坐标系，建立惯性器件陀螺仪和加速度计的误差模型，并求解系统的姿态误差方程、位置误差方程；

2)建立DVL非线性误差模型：针对DVL误差中的刻度误差以及随机测量误差，以一阶马尔科夫过程来拟合误差方程；

3)根据步骤1)和步骤2)分别得到的SINS非线性误差模型和DVL非线性误差模型建立组合导航滤波模型：选取SINS的三维姿态角误差、速度误差的前两维、位置误差的前两维，三维陀螺常值漂移和加速度计的水平方向的两维常值误差的12维状态变量，选取DVL的刻度误差和随机测量误差作为状态量；然后根据步骤1)和步骤2)分别得到的SINS非线性误差模型和DVL非线性误差模型共同建立组合导航的状态方程；以DVL和SINS的东向、北向速度分量的差值作为观测量，建立SINS/DVL组合导航系统的观测方程；

4)建立5阶SSRCKF滤波模型，输出导航系统参数：利用5阶SSRCKF对SINS和DVL输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度、位置和姿态矩阵，得到精确的导航信息，具体包括以下步骤：

41)基于SSR容积采样规则：高斯滤波积分归纳成

其中

采用基于SSR容积采样规则，求解球面积分S(r)，对于拥有n维状态量的系统，求解过程如下：取一组n维向量a_i＝[a_i1，a_i2，a_i3，…，a_in]^T，i＝1，2，…，n+1，并将向量a_i的中点投影到球面U_n得到序列

则有

选用[ξ_i，ω_i]表示滤波器的采样容积点，其中

则得到基于SSR规则下5阶非线性多维积分为：

其中：

F(ξ_i)＝f(ξ_i)+f(-ξ_i)

F(ω_i)＝f(ω_i)+f(-ω_i)；

42)时间更新过程：估计方差P_k-1/k-1通过乔列斯基(Cholesky)分解得：

根据基于SSR容积规则选取容积点集合[ξ_i，ω_i]，计算容积点：

通过状态方程来传导容积点：

因此k时刻的一步状态预测和一步预测方差为：

43)观测过程更新流程：通过观测方程传导容积点：Z_i，k/k-1＝H(X_i，k/k-1)，k时刻的观测量Z的预测以及滤波增益矩阵为：

因此k时刻的状态估计和均方差估计为：

2.如权利要求1所述的基于5阶SSRCKF的SINS/DVL组合导航滤波方法，其特征在于，所述步骤1)中建立SINS非线性误差模型的具体步骤包括：

选取东-北-天坐标系为SINS的导航坐标系并记为n系，载体系选用右-前-上记为b系，SINS解算的导航坐标系记为n′系；系统真实姿态角为

真实速度Vⁿ＝[V_E，V_N，V_U]^T，真实地理坐标P＝[L，λ，H]^T；记陀螺仪和加速度计的随机常值误差分别为ε_b和

陀螺仪和加速度计对应的扰动白噪声分别为w_g和w_f，则陀螺仪误差ε^b＝ε_b+w_g，加速度计误差

陀螺仪和加速度计的测量值为

和

和

分别为地球自转角速度和导航坐标系相对地球坐标系的旋转角速度；V_N和V_E分别表示载体的运动速度的北向分量和东向分量；R_N和R_E分别为卯酉圈半径和子午圈半径；

建立SINS的速度、姿态和位置误差方程，具体如下：

速度方程为：

姿态误差方程为：

其中：

位置误差方程为：

3.如权利要求2所述的基于5阶SSRCKF的SINS/DVL组合导航滤波方法，其特征在于，所述步骤2)中建立DVL误差模型的具体步骤包括：

设定DVL刻度误差为δK_c，随机测量误差为δV_DVL，DVL的实际测量值为

DVL的理想速度为V_DVL；DVL的速度方程为：

定义β_V，β_K分别表示DVL随机测量误差和刻度误差的相关时间；w_V，w_K为其对应的高斯白噪声；则一阶马尔科夫过程拟合的误差方程表示为：

4.如权利要求3所述的基于5阶SSRCKF的SINS/DVL组合导航滤波方法，其特征在于，所述步骤3)中建立组合导航滤波模型的具体步骤包括：

对于SINS，忽略高度通道的状态量，即选取以下12维状态量：

对于DVL选取刻度误差和随机测量误差为状态量为：X_DVL＝[δK_cx，δK_cy，δV_DVLx，δV_DVLy]，共同构成SINS/DVL组合导航系统的非线性状态方程，并将非线性状态方程简记为：

选取导航坐标系下捷联解算速度和DVL速度的前两维的差值为SINS/DVL组合导航系统的观测量；

记SINS解算的速度在n系下的投影为

即速度真值Vⁿ与速度误差值

的和，方程如下：

记DVL在载体系b下的直接输出的速度为

为实际测量值在载体系下的表示；SINS解算的姿态变换矩阵为

实际计算坐标系偏离理想坐标系的误差矩阵记为

将DVL的输出速度转换到导航坐标系上为

则根据DVL的速度方程得：

式中：Vⁿ为导航坐标系下速度的真实值，δK_c为DVL的刻度误差，δV_DVL为DVL的随机测量误差，因此观测方程为：

将此观测方程简记为：

z＝h(x，t)+u(t)

式中u(t)为系统的观测噪声，并假设为零均值高斯白噪声；

以采样周期T_s作为滤波周期，并以T_s为步长，对状态方程和观测方程进行离散化，得到系统的非线性滤波模型，并简记为：

式中：w_k为随机系统噪声并且w_k～N(0，Q_k)，Q_k为噪声矩阵；u_k为随机观测噪声并且u_k～N(0，R_k)，R_k为观测噪声矩阵。

Images