CN110514203A - 一种基于isr-ukf的水下组合导航方法 - Google Patents

Abstract

本发明提供了一种基于ISR‑UKF的水下组合导航方法，包括：分别建立水下潜航阶段SINS/DVL子系统和水面位置修正阶段SINS/GPS子系统的状态方程和量测方程；建立水下潜航阶段SINS/DVL子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，进行水下潜航阶段的导航；在水下潜航较长时间以后，通过控制AUV的潜行深度到达水面附近停留较短时间，获取GPS位置、速度信息辅助，建立水面位置修正阶段SINS/GPS子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，实现对AUV水下潜航阶段偏差的校正，从而实现AUV沿指定路线航行的目的。本发明能够提高SINS/DVL/GPS水下组合导航系统的滤波解算效率，在保证实时性和稳定性的前提下更加易于编程实现。

Description

一种基于ISR-UKF的水下组合导航方法

技术领域

本发明属于惯性导航领域，一种基于迭代平方根无迹卡尔曼滤波算法(IteratedSquare-Root Unscented Kalman Filter，ISR-UKF)的水下组合导航方法。

背景技术

实时高精度的导航定位服务，是AUV(即自主式水下潜器)有效作业和安全回收的关键。但海洋环境与陆地环境有很大的不同，它具有动态性、未知性和复杂性，特殊的水下环境增加了水下导航定位的难度，所以通过某种有效手段来提高水下导航定位的精度一直是人们关注的热点。另外，单一传感器的输出数据具有某种程度的不确定性，具体表现在数据的不完全性、不正确性或不可靠性。比如：惯性导航是一种自主导航，没有与外界的能量交换，隐蔽性好，短时间内惯导可以保持很好的导航精度，但是随着时间的延长，惯导具有非常明显误差积累，导航精度迅速下降；GNSS(即全球卫星导航系统)、雷达等使用电磁波作为能量载体的导航系统在水下难以使用，海水具有导电性，致使电磁波迅速衰减，不能进行远距离传播。相对而言，海水中声波是唯一一种能远距离传播的能量载体。

基于上述原因，需要构建以SINS/DVL(即捷联惯性导航系统/多普勒计程仪)组合导航为主，GPS为辅的组合导航系统以更好地实现AUV的导航定位，该系统为非线性系统。这就需要通过Kalman滤波技术对组合导航的误差状态进行估计，并修正SINS的导航误差。以Kalman滤波为基本结构的EKF(即扩展卡尔曼滤波)易于实现，已被广泛应用于各种工程领域；但EKF也存在着自身无法克服的局限性，如在实际应用中很多系统并不存在雅克比矩阵，此时要对非线性函数进行线性化是行不通的。而UKF(即无迹卡尔曼滤波)采用无迹变换代替了EKF中的局部线性化，不需要计算雅克比矩阵，不需要对雅克比矩阵进行求导，因此不要求状态函数和量测函数必须是连续可微的，不要求系统是近似线性的；但在UKF滤波过程中，需要对每个采样点进行非线性变换，计算量大，且在数值计算中往往存在舍入误差可能会破坏系统估计误差协方差矩阵的非负定性和对称性，影响滤波算法的收敛速度和稳定性。

发明内容

为解决上述问题，本发明公开了一种基于迭代平方根无迹卡尔曼滤波算法(即：Iterated Square-Root Unscented Kalman Filter，简写为ISR-UKF)的水下组合导航方法，可以为AUV提供更高的姿态、速度和位置精度，且比迭代UKF减小了部分计算量，能够提高SINS/DVL/GPS水下组合导航系统的滤波解算效率，在保证实时性和稳定性的前提下更加易于编程实现。

为了达到上述目的，本发明提供如下技术方案：

一种基于ISR-UKF的水下组合导航方法，具体包括以下步骤：

步骤1：完成导航定位系统的初始对准后，根据系统误差方程建立水下潜航阶段SINS/DVL子系统的状态方程，根据SINS解算的速度量和DVL测量的速度量之差作为量测量建立水下潜航阶段SINS/DVL子系统的量测方程；

步骤2：在水面附近，不考虑天向运动速度及误差，根据误差方程建立水面位置修正阶段SINS/GPS子系统的状态方程，以SINS解算出来的位置、速度与GPS输出的位置、速度之差作为量测量建立水面位置修正阶段SINS/GPS子系统的量测方程；

步骤3：根据步骤1中建立的子系统状态方程和量测方程建立水下潜航阶段SINS/DVL子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，进行水下潜航阶段的导航；

步骤4：控制AUV的潜行深度到达水面附近停留，当GPS设备状态为定位有效时，获取GPS位置、速度信息辅助，根据步骤2中建立的子系统状态方程和量测方程，建立水面位置修正阶段SINS/GPS子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，进行对AUV水下潜航阶段偏差的校正。

进一步的，所述步骤1中建立水下潜航阶段SINS/DVL子系统的状态方程和量测方程，具体过程如下：

步骤1.1对需要用到的坐标系进行定义：

i——惯性坐标系：不随地球旋转，原点位于地球中心，z_i轴指向北极，x_i轴指向春分点，y_i轴与x_i、z_i构成右手坐标系；

e——地球坐标系：与地球固联，原点位于地心，x_e轴穿越本初子午线与赤道交点，z_e轴指向北极，y_e轴x_e、z_e构成右手坐标系；

b——载体坐标系：原点位于运载体中心，z_b轴垂直运载体向上，x_b指向运载体前方，y_b与x_b、z_b构成右手坐标系；

p——实际计算得出的平台坐标系；

n——与东-北-天地理坐标系重合的导航坐标系；

步骤1.2建立水下潜航阶段SINS/DVL子系统的状态方程，具体步骤如下：

取姿态误差角(φ_E φ_N φ_U)、速度误差(δv_E δv_N δv_U)位置误差(δL δλ δh)、陀螺常值漂移(ε_bx ε_by ε_bz)和加速度计随机常值误差(▽_bx ▽_by ▽_bz)作为SINS系统的状态量，记为：

X_N(t)＝[φ_E φ_N φ_U δv_E δv_N δv_U δL δλ δh ε_bx ε_by ε_bz ▽_bx ▽_by ▽_bz]^T

其中，φ_E是东向失准角，φ_N是北向失准角，φ_U是天向失准角；δv_E是东向速度误差，δv_N是北向速度误差，δv_U是天向速度误差；δL是纬度误差，δλ是经度误差，δh是高度误差；ε_bx是x向陀螺漂移，ε_by是y向陀螺漂移，ε_bz是z向陀螺漂移；▽_bx是x向加速度计随机常值误差，▽_by是y向加速度计随机常值误差，▽_bz是z向加速度计随机常值误差。

通过理想惯导比力方程的速度微分方程和捷联惯导系统的实际速度微分方程，推导出四元数表示的速度误差方程：

其中，Vⁿ是载体在n系下的理想速度，是n系下的地球自转角速度，为n系相对于e系的角速度在n系下的投影，f^b为b系下的比力；

式中，为p系到n系的转换四元数，为b系到p系的转换四元数，和分别代表p系到n系以及b系到p系的转换矩阵；

式中，“～”表示载体实际测量值，δ表示载体的理想值与实际测量值之间的误差，gⁿ为n系下的重力加速度，▽^b为b系下的加速度计误差向量；

四元数姿态误差方程：

其中，表示n系相对于i系的旋转角速度在n系下的投影，为n系到p系的方向余弦矩阵，ε^b为陀螺仪误差向量在b系下的投影，B为关于四元数的4×3维矩阵；

位置误差方程：

其中，R_M和R_N分别表示地球的子午圈曲率半径和卯酉圈曲率半径，L表示载体的纬度，λ表示载体的经度，h表示载体的高度；

SINS系统的噪声：

W_N(t)＝[ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^T

则SINS的系统误差方程可以表示为：

其中F_N[·]为非线连续函数；

取DVL速度偏移误差(δV_dx δV_dy δV_dz)和刻度系数误差(Δk_dx Δk_dy Δk_dz)作为DVL系统状态变量，记为：

X_D(t)＝[δV_dx δV_dy δV_dz δk_dx δk_dy δk_dz]^T

其中，δV_dx是x轴DVL速度偏移误差，δV_dy是y轴DVL速度偏移误差，δV_dz是z轴DVL速度偏移误差；δk_dx是x轴刻度系数误差，δk_dy是y轴刻度系数误差，δk_dz是z轴刻度系数误差。

DVL的误差模型为：

其中，β_d表示速度偏移误差的相关时间倒数，ω_d表示激励白噪声；

相应的误差状态方程为：

其中W_D(t)＝[ω_dx ω_dy ω_dz]^T，其中ω_di(i＝x,y,z)为激励白噪声；G_D(t)＝[I_3×3 O_3×3]；τ_di(i＝x,y,z)表示速度偏移误差的相关时间；

选取SINS和DVL子系统的误差状态变量，则组合导航系统的状态向量为：

系统的噪声向量为系统的状态方程表示为：

式中，状态函数F₁[·]为非线性连续函数，Γ₁(t)为该子系统噪声阵；

步骤1.3建立水下潜航阶段SINS/DVL子系统的量测方程，具体步骤如下：

由SINS和DVL形成的量测量：

其中，v_EI是SINS解算出的东向速度，v_NI是SINS解算出的北向速度，v_UI是SINS解算出的天向速度；v_ED是DVL测出的东向速度，v_ND是DVL测出的北向速度，v_UD是DVL测出的天向速度；v_E是载体在导航坐标系下的东向真实速度，v_N是载体在导航坐标系下的北向真实速度，v_U是载体在导航坐标系下的天向真实速度；v_x是在载体坐标系下的x向真实速度，v_y是在载体坐标系下的y向真实速度，v_z是在载体坐标系下的z向真实速度；δv_DE是DVL测速误差换算到导航坐标系后的东向误差，δv_DN是DVL测速误差换算到导航坐标系后的北向误差，δv_DU是DVL测速误差换算到导航坐标系后的天向误差。

将上式展开，并结合之前选取的系统误差状态得该组合系统的量测方程为：

Z₁＝H₁[X₁(t),t]+V₁(t)

式中：H₁[·]为非线性连续函数；V₁(t)为量测噪声。

进一步的，所述步骤2中建立水面位置修正阶段SINS/GPS子系统的状态方程和量测方程具体包括如下步骤：

步骤2.1建立水面位置修正阶段SINS/GPS子系统的状态方程，具体步骤如下：

取状态变量：

X₂(t)＝[φ_E φ_N φ_U δv_E δv_N δL δλ ε_bx ε_by ε_bz ▽_bx ▽_by]^T

系统的噪声误差为：

W₂(t)＝[0 0 ω_gx ω_gy ω_gz ω_ax ω_ay 0 0 0 0 0]^T

建立基于SINS/GPS的AUV组合导航连续系统状态方程：

式中：F₂[·]为非线性连续函数，Γ₂(t)为该子系统噪声阵；

步骤2.2建立水面位置修正阶段SINS/GPS子系统的量测方程，具体步骤如下：

将SINS解算出来的位置、速度与GPS输出的位置位置、速度之差作为AUV水面位置修正阶段滤波解算的量测方程：

式中L为SINS解算出来的纬度，λ为SINS解算出来的经度；V_E为SINS解算出来的东向速度，V_N为SINS解算出来的北向速度；L_G为GPS输出的纬度，λ_G为GPS输出的经度；V_GE为GPS输出的东向速度，V_GN为GPS输出的北向速度。

将上式展开，并结合之前选取的系统误差状态X₂(t)，可得到量测方程为：

Z₂＝H₂[X₂(t),t]+V₂(t)

式中：H₂[·]为非线性连续函数，V₂为量测噪声。

进一步的，所述步骤3中建立水下潜航阶段的非线性滤波方程并通过ISR-UFK算法进行水下潜航阶段导航的具体步骤如下：

步骤3.1建立水下潜航阶段的非线性滤波方程：

对水下潜航阶段的非线性滤波方程离散化有：

其中，X_k和Z_k分别为系统在t_k时刻的状态向量以及量测向量，W_k和V_k分别为水下潜航阶段子系统的噪声阵和量测噪声阵，且均值均为零，统计特性如下：

Q_k和R_k分别为子系统噪声协方差阵和量测噪声协方差阵；

步骤3.2具体的ISR-UFK算法过程如下：

步骤3.2.1初始化增广状态向量及估计误差方差：

步骤3.2.2对于k∈{1,…,∞}，实现步骤如下：

步骤3.2.2.1计算sigma点：

步骤3.2.2.2时间更新，得到一步预测和一步预测误差协方差矩阵的平方根

ξ_k/k-1＝f(ξ_k-1)

步骤3.2.2.3量测更新，包括迭代过程：

对于j＝0:2

χ_j＝h(ξ_j)

至此迭代结束，共两次，

步骤3.2.3计算权值和参数：

其中n是x的维数；λ＝α²(n+κ)-n是一个复合刻度参数；α为决定先验均值周围Sigma点分布广度的主要刻度因数，该分布通常被设置为一个小的正值(例如10^-3≤α≤1)；κ是次要缩放参数，通常被设置为0；β为用来强调验后协方差计算的零阶Sigma点权值的第二刻度因数，并且对于高斯先验随机向量β＝2是最优的；表示矩阵平方根的第i列；协方差矩阵的平方根用SS^T＝P表示；chol{·}表示Cholesky分解函数，qr{·}表示QR分解函数，cholupdate{·}表示Cholesky分解的更新函数；是扰动变量，Δx_p是Δx的第p个扰动分量，L是一个具有适当大小的整数。

进一步的，所述步骤4中建立水面位置修正阶段的非线性滤波方程并通过ISR-UFK算法对AUV水下潜航阶段偏差的校正的具体步骤如下：

步骤4.1建立水面位置修正阶段的非线性滤波方程：

对水面位置修正阶段非线性滤波方程进行离散化有：

其中，x_k和z_k分别为系统在t_k时刻的状态向量以及量测向量，w_k和v_k分别为水面位置修正阶段子系统的噪声阵和量测噪声阵，且均值均为零，统计特性如下：

q_k和r_k分别为该子系统噪声协方差阵和量测噪声协方差阵；

步骤4.2具体的ISR-UFK算法过程如下：

步骤4.2.1初始化增广状态向量及估计误差方差：

步骤4.2.2对于k∈{1,…,∞}，实现步骤如下：

步骤4.2.2.1计算sigma点

步骤4.2.2.2时间更新，得到一步预测和一步预测误差协方差矩阵的平方根

ξ_k/k-1＝f(ξ_k-1)

步骤4.2.2.3量测更新，包括迭代过程：

对于j＝0:2

χ_j＝h(ξ_j)

至此迭代结束，共两次，

步骤4.3计算权值和参数：

将每次AUV浮至水面的时SINS/GPS子系统的ISR-UKF滤波结果的位置信息作为下一次潜航的SINS/DVL子系统的新的位置信息，即初始值，定时修正位置，实现AUV沿指定路线航行。

与现有技术相比，本发明具有如下优点和有益效果：

(1)本发明提出的基于迭代平方根无迹卡尔曼滤波算法的水下组合导航方法，通过SINS/DVL组合导航的水下潜行阶段和SINS/GPS组合导航的水面修正阶段，利用了SINS在短时间内导航精度较高、GPS定位信息准确的特点，可以为AUV提供更高的姿态、速度和位置精度；

(2)本发明公开的ISR-UKF算法比迭代UKF减小了部分计算量，能够提高SINS/DVL/GPS水下组合导航系统的滤波解算效率，在保证实时性和稳定性的前提下更加易于编程实现。

附图说明

图1为本发明提供的的组合系统导航原理图；

图2是本发明提供的组合导航系统的算法流程图；

图3是本发明提供的迭代平方根无迹卡尔曼滤波方法的流程图。

具体实施方式

以下将结合具体实施例对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

本发明所述的一种迭代平方根无迹卡尔曼滤波算法在水下组合导航系统中的应用方法，其流程如图2所示，包含以下步骤：

步骤1：完成导航定位系统的初始对准后，根据系统误差方程建立水下潜航阶段SINS/DVL子系统的状态方程，根据SINS解算的3维速度量和DVL测量的速度量之差作为量测量建立水下潜航阶段SINS/DVL子系统的量测方程；

步骤1.1：首先对需要用到的坐标系进行定义：

i——惯性坐标系：不随地球旋转，原点位于地球中心，z_i轴指向北极，x_i轴指向春分点，y_i轴与x_i、z_i构成右手坐标系；

e——地球坐标系：与地球固联，原点位于地心，x_e轴穿越本初子午线与赤道交点，z_e轴指向北极，y_e轴x_e、z_e构成右手坐标系；

b——载体坐标系：原点位于运载体中心，z_b轴垂直运载体向上，x_b指向运载体前方，y_b与x_b、z_b构成右手坐标系；

p——实际计算得出的平台坐标系；

n——与东-北-天地理坐标系重合的导航坐标系；

步骤1.2：建立水下潜航阶段SINS/DVL子系统的状态方程的具体步骤：

取姿态误差角(φ_E φ_N φ_U)、速度误差(δv_E δv_N δv_U)位置误差(δL δλ δh)、陀螺常值漂移(ε_bx ε_by ε_bz)和加速度计随机常值误差(▽_bx ▽_by ▽_bz)作为SINS系统的状态量，记为：

X_N(t)＝[φ_E φ_N φ_U δv_E δv_N δv_U δL δλ δh ε_bx ε_by ε_bz ▽_bx ▽_by ▽_bz]^T

其中，φ_E是东向失准角，φ_N是北向失准角，φ_U是天向失准角；δv_E是东向速度误差，δv_N是北向速度误差，δv_U是天向速度误差；δL是纬度误差，δλ是经度误差，δh是高度误差；ε_bx是x向陀螺漂移，ε_by是y向陀螺漂移，ε_bz是z向陀螺漂移；▽_bx是x向加速度计随机常值误差，▽_by是y向加速度计随机常值误差，▽_bz是z向加速度计随机常值误差。

通过理想惯导比力方程的速度微分方程和捷联惯导系统的实际速度微分方程，推导出四元数表示的速度误差方程：

其中，Vⁿ是载体在n系下的理想速度，是n系下的地球自转角速度，为n系相对于e系的角速度在n系下的投影，f^b为b系下的比力；

式中，为p系到n系的转换四元数，为b系到p系的转换四元数，和分别代表p系到n系以及b系到p系的转换矩阵；

式中，“～”表示载体实际测量值，δ表示载体的理想值与实际测量值之间的误差，gⁿ为n系下的重力加速度，▽^b为b系下的加速度计误差向量；

四元数姿态误差方程：

其中，表示n系相对于i系的旋转角速度在n系下的投影，为n系到p系的方向余弦矩阵，ε^b为陀螺仪误差向量在b系下的投影，B为关于四元数的4×3维矩阵；

位置误差方程：

其中，R_M和R_N分别表示地球的子午圈曲率半径和卯酉圈曲率半径，L表示载体的纬度，λ表示载体的经度，h表示载体的高度；

SINS系统的噪声：

W_N(t)＝[ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^T

式中，ω_gi(i＝x,y,z)表示三轴陀螺高斯白噪声，ω_ai(i＝x,y,z)表示三轴加速度计三轴高斯白噪声。

则SINS的系统误差方程可以表示为：

其中F_N[·]为非线连续函数；

取DVL速度偏移误差(δV_dx δV_dy δV_dz)和刻度系数误差(Δk_dx Δk_dy Δk_dz)作为DVL系统状态变量，记为：

X_D(t)＝[δV_dx δV_dy δV_dz δk_dx δk_dy δk_dz]^T

其中，δV_dx是x轴DVL速度偏移误差，δV_dy是y轴DVL速度偏移误差，δV_dz是z轴DVL速度偏移误差；δk_dx是x轴刻度系数误差，δk_dy是y轴刻度系数误差，δk_dz是z轴刻度系数误差。

DVL的误差模型为：

其中，β_d表示速度偏移误差的相关时间倒数，ω_d表示激励白噪声；

相应的误差状态方程为：

其中W_D(t)＝[ω_dx ω_dy ω_dz]^T，其中ω_di(i＝x,y,z)为激励白噪声；G_D(t)＝[I_3×3 O_3×3]；τ_di(i＝x,y,z)表示速度偏移误差的相关时间；

选取SINS和DVL子系统的误差状态变量，则组合导航系统的状态向量为系统的噪声向量为系统的状态方程表示为：

式中，状态函数F₁[·]为非线性连续函数，Γ₁(t)为该子系统噪声阵；

步骤1.3：建立水下潜航阶段SINS/DVL子系统的量测方程的具体步骤：

由于DVL测得地速在载体坐标系内的分量，要使其输出的速度与SINS输出的速度形成量测量，必须将DVL的输出速度变换到导航坐标系中，由SINS提供变化所用的姿态矩阵因此，由SINS和DVL形成的量测量：

其中，v_EI是SINS解算出的东向速度，v_NI是SINS解算出的北向速度，v_UI是SINS解算出的天向速度；v_ED是DVL测出的东向速度，v_ND是DVL测出的北向速度，v_UD是DVL测出的天向速度；v_E是载体在导航坐标系下的东向真实速度，v_N是载体在导航坐标系下的北向真实速度，v_U是载体在导航坐标系下的天向真实速度；v_x是在载体坐标系下的x向真实速度，v_y是在载体坐标系下的y向真实速度，v_z是在载体坐标系下的z向真实速度；δv_DE是DVL测速误差换算到导航坐标系后的东向误差，δv_DN是DVL测速误差换算到导航坐标系后的北向误差，δv_DU是DVL测速误差换算到导航坐标系后的天向误差。

将上式展开，并结合之前选取的系统误差状态得该组合系统的量测方程为：

Z₁＝H₁[X₁(t),t]+V₁(t)

式中：H₁[·]为非线性连续函数；V₁(t)为量测噪声。

据此，得到潜航阶段卡尔曼滤波模型的状态方程和量测方程：

步骤2：在水面附近，不考虑天向运动速度及误差，根据误差方程建立水面位置修正阶段SINS/GPS子系统的状态方程，以SINS解算出来的位置、速度与GPS输出的位置、速度之差作为量测量建立水面位置修正阶段SINS/GPS子系统的量测方程；

步骤2.1：建立水面位置修正阶段SINS/GPS子系统的状态方程的具体步骤：

取状态变量：

X₂(t)＝[φ_E φ_N φ_U δv_E δv_N δL δλ ε_bx ε_by ε_bz ▽_bx ▽_by]^T

系统的噪声误差为：

W₂(t)＝[0 0 ω_gx ω_gy ω_gz ω_ax ω_ay 0 0 0 0 0]^T

建立基于SINS/GPS的AUV组合导航连续系统状态方程：

式中：F₂[·]为非线性连续函数，Γ₂(t)为该子系统噪声阵；

步骤2.2：建立水面位置修正阶段SINS/GPS子系统的量测方程的具体步骤：

将SINS解算出来的位置、速度与GPS输出的位置位置、速度之差作为AUV水面位置修正阶段滤波解算的量测方程：

式中L为SINS解算出来的纬度，λ为SINS解算出来的经度；V_E为SINS解算出来的东向速度，V_N为SINS解算出来的北向速度；L_G为GPS输出的纬度，λ_G为GPS输出的经度；V_GE为GPS输出的东向速度，V_GN为GPS输出的北向速度。

将上式展开，并结合之前选取的系统误差状态X₂(t)，可得到量测方程为：

Z₂＝H₂[X₂(t),t]+V₂(t)

式中：H₂[·]为非线性连续函数，V₂为量测噪声。

据此，得到水面位置修正阶段卡尔曼滤波模型的状态方程和量测方程为：

步骤3：根据步骤1中建立的子系统状态方程和量测方程建立水下潜航阶段SINS/DVL子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，进行水下潜航阶段的导航；

步骤3.1：迭代平方根卡尔曼滤波算法介绍如下：

UKF的测量更新过程使用的是线性最小均方估计方法，该方法假设状态估计是测量的线性函数，在得到新的测量之后，使用二阶矩对状态估计进行更新。对于非线性系统来说，这只是一种近似的更新方式。本发明使用更靠近真实状态的估计值进行非线性变换和更新过程中的参数计算会进一步提高非线性近似的程度。在测量更新过程中，U变换在以预测值为中心、以预测方差为协方差所产生的样点中进行。当k时刻观测值已经取得后，再使用估计值和预测方差来重新产生样点，进而进行U变换并计算滤波参数，然后再一次利用观测值改善对状态的估计，这就是迭代UKF的思想，从而提高算法的近似精度。本发明使用平方根U变换，因此得到的迭代滤波器称为迭代平方根UKF(ISR-UKF)；

在无迹卡尔曼(UKF)推算法中，一般须对系统噪声和量测噪声进行状态增广，但是当系统噪声和量测噪声均为加性噪声时，可不做增广处理，有利于进一步降低滤波计算。本发明研究了一种基于复杂加性噪声的迭代平方根UKF算法。复杂加性噪声非线性离散系统模型可表示为：

式中：f[·]、g[·]、h[·]、j[·]均为非线性函数；x_k、z_k分别为状态向量和观测向量；ω_k和v_k分别为系统状态噪声和量测噪声向量。其统计特性如下：E[W_k]＝0，E[V_k]＝0，

由上式定义的系统模型可知，复杂加性噪声模型的特点是模型关于噪声是线性的，具体的SR-UKF算法流程如下：

(1)初始化增广状态向量及估计误差方差

(2)对于k∈{1,…,∞}，实现步骤如下：

<1>计算sigma点：

<2>时间更新：

ξ_k/k-1＝f(ξ_k-1)

χ_k/k-1＝h(ξ_k/k-1)

<3>量测更新：

(3)计算权值和参数

其中n是x的维数；λ＝α²(n+κ)-n是一个复合刻度参数；α为决定先验均值周围Sigma点分布广度的主要刻度因数，该分布通常被设置为一个小的正值(例如10^-3≤α≤1)；κ是次要缩放参数，通常被设置为0；β为用来强调验后协方差计算的零阶Sigma点权值的第二刻度因数，并且对于高斯先验随机向量β＝2是最优的；表示矩阵平方根的第i列；

在SR-UKF实现中，用协方差矩阵的平方根代替协方差矩阵参加递推运算，避免了协方差矩阵的负定性，同时保证了运行效率和数值稳定性；协方差矩阵的平方根用SS^T＝P表示；

SR-UKF采用了三种线性代数方法，即Cholesky分解法、QR分解法和Cholesky因子更新法。其中chol{·}表示Cholesky分解函数，qr{·}表示QR分解函数，cholupdate{·}表示Cholesky分解的更新函数；

ISR-UKF和SR-UKF的主要区别在于量测更新的步骤。对于ISR-UKF，一旦状态预测及相应的协方差得到，将递归执行下面的迭代：

对于j＝0:2

χ_j＝h(ξ_j)

至此迭代结束，共两次。

其中，其中是扰动变量，Δx_p是Δx的第p个扰动分量，L是一个具有适当大小的整数；

为了节约计算量，协方差平方根不进行迭代更新。当迭代次数为0时，ISR-UKF与SR-UKF一致。与SR-UKF相比，ISR-UKF增加了两次迭代测量更新运算，但其计算复杂度仍然为O(L³)量级。一般情况下，状态方程的形式比观测方程复杂，变量维数更多，因此状态方程的运算将占用较多的运算时间，而测量迭代更新过程中增加的运算量相对较小，因而ISR-UKF与SR-UKF的运算量一般相差并不太大。

步骤3.2：根据上述算法，对上述建立的水下潜航阶段卡尔曼滤波方程进行离散化：

对水下潜航阶段的非线性滤波方程离散化有：

其中，X_k和Z_k分别为系统在t_k时刻的状态向量以及量测向量，W_k和V_k分别为水下潜航阶段子系统的噪声阵和量测噪声阵，且均值均为零，统计特性如下：

Q_k和R_k分别为子系统噪声协方差阵和量测噪声协方差阵；

具体的ISR-UFK算法过程如下：

(1)初始化增广状态向量及估计误差方差

(2)对于k∈{1,…,∞}，实现步骤如下：

<1>计算sigma点

<2>时间更新，得到一步预测和一步预测误差协方差矩阵的平方根

ξ_k/k-1＝f(ξ_k-1)

<3>量测更新，包括迭代过程：

对于j＝0:2

χ_j＝h(ξ_j)

至此迭代结束，共两次。

(3)计算权值和参数

步骤4：控制AUV的潜行深度到达水面附近停留，当GPS设备状态为定位有效时，获取GPS位置、速度信息辅助，根据步骤2中建立的子系统状态方程和量测方程，建立水面位置修正阶段SINS/GPS子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，进行对AUV水下潜航阶段偏差的校正。

步骤4.1：对上述建立的水面位置修正阶段卡尔曼滤波方程进行离散化：

对水面位置修正阶段非线性滤波方程进行离散化有：

其中，x_k和z_k分别为系统在t_k时刻的状态向量以及量测向量，w_k和v_k分别为水面位置修正阶段子系统的噪声阵和量测噪声阵，且均值均为零，统计特性如下：

q_k和r_k分别为该子系统噪声协方差阵和量测噪声协方差阵；

步骤4.2：具体的ISR-UFK算法过程如下：

(1)初始化增广状态向量及估计误差方差

(2)对于k∈{1,…,∞}，实现步骤如下：

<1>计算sigma点

<2>时间更新，得到一步预测和一步预测误差协方差矩阵的平方根

ξ_k/k-1＝f(ξ_k-1)

<3>量测更新，包括迭代过程：

对于j＝0:2

χ_j＝h(ξ_j)

至此迭代结束，共两次。

(3)计算权值和参数

将每次AUV浮至水面的时SINS/GPS子系统的ISR-UKF滤波结果的位置信息作为下一次潜航的SINS/DVL子系统的新的位置信息(初始值)，定时修正位置，实现AUV沿指定路线航行。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (5)

1.一种基于ISR-UKF的水下组合导航方法，其特征在于，该方法包括以下步骤：

步骤1：完成导航定位系统的初始对准后，根据系统误差方程建立水下潜航阶段SINS/DVL子系统的状态方程，根据SINS解算的速度量和DVL测量的速度量之差作为量测量建立水下潜航阶段SINS/DVL子系统的量测方程；

步骤2：在水面附近，不考虑天向运动速度及误差，根据误差方程建立水面位置修正阶段SINS/GPS子系统的状态方程，以SINS解算出来的位置、速度与GPS输出的位置、速度之差作为量测量建立水面位置修正阶段SINS/GPS子系统的量测方程；

步骤3：根据步骤1中建立的子系统状态方程和量测方程建立水下潜航阶段SINS/DVL子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，进行水下潜航阶段的导航；

步骤4：控制AUV的潜行深度到达水面附近停留，当GPS设备状态为定位有效时，获取GPS位置、速度信息辅助，根据步骤2中建立的子系统状态方程和量测方程，建立水面位置修正阶段SINS/GPS子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，进行对AUV水下潜航阶段偏差的校正。

2.根据权利要求1所述的基于ISR-UKF的水下组合导航方法，其特征在于，步骤1中所述建立水下潜航阶段SINS/DVL子系统的状态方程和量测方程，具体过程如下：

步骤1.1对需要用到的坐标系进行定义：

i——惯性坐标系：不随地球旋转，原点位于地球中心，z_i轴指向北极，x_i轴指向春分点，y_i轴与x_i、z_i构成右手坐标系；

e——地球坐标系：与地球固联，原点位于地心，x_e轴穿越本初子午线与赤道交点，z_e轴指向北极，y_e轴x_e、z_e构成右手坐标系；

b——载体坐标系：原点位于运载体中心，z_b轴垂直运载体向上，x_b指向运载体前方，y_b与x_b、z_b构成右手坐标系；

p——实际计算得出的平台坐标系；

n——与东-北-天地理坐标系重合的导航坐标系；

步骤1.2建立水下潜航阶段SINS/DVL子系统的状态方程，具体步骤如下：

取姿态误差角(φ_E φ_N φ_U)、速度误差(δv_E δv_N δv_U)位置误差(δL δλ δh)、陀螺常值漂移(ε_bx ε_by ε_bz)和加速度计随机常值误差作为SINS系统的状态量，记为：

其中，φ_E是东向失准角，φ_N是北向失准角，φ_U是天向失准角；δv_E是东向速度误差，δv_N是北向速度误差，δv_U是天向速度误差；δL是纬度误差，δλ是经度误差，δh是高度误差；ε_bx是x向陀螺漂移，ε_by是y向陀螺漂移，ε_bz是z向陀螺漂移；是x向加速度计随机常值误差，是y向加速度计随机常值误差，是z向加速度计随机常值误差。

通过理想惯导比力方程的速度微分方程和捷联惯导系统的实际速度微分方程，推导出四元数表示的速度误差方程：

其中，Vⁿ是载体在n系下的理想速度，是n系下的地球自转角速度，为n系相对于e系的角速度在n系下的投影，f^b为b系下的比力；

式中，为p系到n系的转换四元数，为b系到p系的转换四元数，和分别代表p系到n系以及b系到p系的转换矩阵；

式中，“～”表示载体实际测量值，δ表示载体的理想值与实际测量值之间的误差，gⁿ为n系下的重力加速度，为b系下的加速度计误差向量；

四元数姿态误差方程：

其中，表示n系相对于i系的旋转角速度在n系下的投影，为n系到p系的方向余弦矩阵，ε^b为陀螺仪误差向量在b系下的投影，B为关于四元数的4×3维矩阵；

位置误差方程：

其中，R_M和R_N分别表示地球的子午圈曲率半径和卯酉圈曲率半径，L表示载体的纬度，λ表示载体的经度，h表示载体的高度；

SINS系统的噪声：

W_N(t)＝[ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^T

则SINS的系统误差方程可以表示为：

其中F_N[·]为非线连续函数；

取DVL速度偏移误差(δV_dx δV_dy δV_dz)和刻度系数误差(Δk_dx Δk_dy Δk_dz)作为DVL系统状态变量，记为：

X_D(t)＝[δV_dx δV_dy δV_dz δk_dx δk_dy δk_dz]^T

其中，δV_dx是x轴DVL速度偏移误差，δV_dy是y轴DVL速度偏移误差，δV_dz是z轴DVL速度偏移误差；δk_dx是x轴刻度系数误差，δk_dy是y轴刻度系数误差，δk_dz是z轴刻度系数误差。

DVL的误差模型为：

其中，β_d表示速度偏移误差的相关时间倒数，ω_d表示激励白噪声；

相应的误差状态方程为：

其中W_D(t)＝[ω_dx ω_dy ω_dz]^T，其中ω_di(i＝x,y,z)为激励白噪声；G_D(t)＝[I_3×3 O_3×3]；τ_di(i＝x,y,z)表示速度偏移误差的相关时间；

选取SINS和DVL子系统的误差状态变量，则组合导航系统的状态向量为系统的噪声向量为系统的状态方程表示为：

式中，状态函数F₁[·]为非线性连续函数，Γ₁(t)为该子系统噪声阵；

步骤1.3建立水下潜航阶段SINS/DVL子系统的量测方程，具体步骤如下：

由SINS和DVL形成的量测量：

其中，v_EI是SINS解算出的东向速度，v_NI是SINS解算出的北向速度，v_UI是SINS解算出的天向速度；v_ED是DVL测出的东向速度，v_ND是DVL测出的北向速度，v_UD是DVL测出的天向速度；v_E是载体在导航坐标系下的东向真实速度，v_N是载体在导航坐标系下的北向真实速度，v_U是载体在导航坐标系下的天向真实速度；v_x是在载体坐标系下的x向真实速度，v_y是在载体坐标系下的y向真实速度，v_z是在载体坐标系下的z向真实速度；δv_DE是DVL测速误差换算到导航坐标系后的东向误差，δv_DN是DVL测速误差换算到导航坐标系后的北向误差，δv_DU是DVL测速误差换算到导航坐标系后的天向误差。

将上式展开，并结合之前选取的系统误差状态得该组合系统的量测方程为：

Z₁＝H₁[X₁(t),t]+V₁(t)

式中：H₁[·]为非线性连续函数；V₁(t)为量测噪声。

3.根据权利要求1所述的基于ISR-UKF的水下组合导航方法，其特征在于，步骤2中所述建立水面位置修正阶段SINS/GPS子系统的状态方程和量测方程具体包括如下步骤：

步骤2.1建立水面位置修正阶段SINS/GPS子系统的状态方程，具体步骤如下：

取状态变量：

系统的噪声误差为：

W₂(t)＝[0 0 ω_gx ω_gy ω_gz ω_ax ω_ay 0 0 0 0 0]^T

建立基于SINS/GPS的AUV组合导航连续系统状态方程：

式中：F₂[·]为非线性连续函数，Γ₂(t)为该子系统噪声阵；

步骤2.2建立水面位置修正阶段SINS/GPS子系统的量测方程，具体步骤如下：

将SINS解算出来的位置、速度与GPS输出的位置位置、速度之差作为AUV水面位置修正阶段滤波解算的量测方程：

式中L为SINS解算出来的纬度，λ为SINS解算出来的经度；V_E为SINS解算出来的东向速度，V_N为SINS解算出来的北向速度；L_G为GPS输出的纬度，λ_G为GPS输出的经度；V_GE为GPS输出的东向速度，V_GN为GPS输出的北向速度；

将上式展开，并结合之前选取的系统误差状态X₂(t)，可得到量测方程为：

Z₂＝H₂[X₂(t),t]+V₂(t)

式中：H₂[·]为非线性连续函数，V₂为量测噪声。

4.根据权利要求1所述的基于ISR-UKF的水下组合导航方法，其特征在于，步骤3中所述建立水下潜航阶段的非线性滤波方程并通过ISR-UFK算法进行水下潜航阶段导航的具体步骤如下：

步骤3.1建立水下潜航阶段的非线性滤波方程：

对水下潜航阶段的非线性滤波方程离散化有：

其中，X_k和Z_k分别为系统在t_k时刻的状态向量以及量测向量，W_k和V_k分别为水下潜航阶段子系统的噪声阵和量测噪声阵，且均值均为零，统计特性如下：

Q_k和R_k分别为子系统噪声协方差阵和量测噪声协方差阵；

步骤3.2具体的ISR-UFK算法过程如下：

步骤3.2.1初始化增广状态向量及估计误差方差：

步骤3.2.2对于k∈{1,…,∞}，实现步骤如下：

步骤3.2.2.1计算sigma点：

步骤3.2.2.2时间更新，得到一步预测和一步预测误差协方差矩阵的平方根

ξ_k/k-1＝f(ξ_k-1)

步骤3.2.2.3量测更新，包括迭代过程：

对于j＝0:2

χ_j＝h(ξ_j)

至此迭代结束，共两次，

步骤3.2.3计算权值和参数：

其中n是x的维数；λ＝α²(n+κ)-n是一个复合刻度参数；α为决定先验均值周围Sigma点分布广度的主要刻度因数，该分布通常被设置为一个小的正值(例如10^-3≤α≤1)；κ是次要缩放参数，通常被设置为0；β为用来强调验后协方差计算的零阶Sigma点权值的第二刻度因数，并且对于高斯先验随机向量β＝2是最优的；表示矩阵平方根的第i列；协方差矩阵的平方根用SS^T＝P表示；chol{·}表示Cholesky分解函数，qr{·}表示QR分解函数，cholupdate{·}表示Cholesky分解的更新函数；是扰动变量，Δx_p是Δx的第p个扰动分量，L是一个具有适当大小的整数。

5.根据权利要求1所述的基于ISR-UKF的水下组合导航方法，其特征在于，步骤4中所述建立水面位置修正阶段的非线性滤波方程并通过ISR-UFK算法对AUV水下潜航阶段偏差的校正的具体步骤如下：

步骤4.1建立水面位置修正阶段的非线性滤波方程：

对水面位置修正阶段非线性滤波方程进行离散化有：

其中，x_k和z_k分别为系统在t_k时刻的状态向量以及量测向量，w_k和v_k分别为水面位置修正阶段子系统的噪声阵和量测噪声阵，且均值均为零，统计特性如下：

q_k和r_k分别为该子系统噪声协方差阵和量测噪声协方差阵；

步骤4.2具体的ISR-UFK算法过程如下：

步骤4.2.1初始化增广状态向量及估计误差方差：

步骤4.2.2对于k∈{1,…,∞}，实现步骤如下：

步骤4.2.2.1计算sigma点

步骤4.2.2.2时间更新，得到一步预测和一步预测误差协方差矩阵的平方根

ξ_k/k-1＝f(ξ_k-1)

步骤4.2.2.3量测更新，包括迭代过程：

对于j＝0:2

χ_j＝h(ξ_j)

至此迭代结束，共两次，

步骤4.3计算权值和参数：

将每次AUV浮至水面的时SINS/GPS子系统的ISR-UKF滤波结果的位置信息作为下一次潜航的SINS/DVL子系统的新的位置信息，即初始值，定时修正位置，实现AUV沿指定路线航行。