CN110209180A - 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法 - Google Patents

Abstract

本发明公开了一种基于HuberM‑Cubature卡尔曼滤波的无人水下航行器目标跟踪方法，包括以下步骤：1)建立无人水下航行器的运动方程；2)建立无人水下航行器的观测方程；3)采用基于Huber‑Cubature卡尔曼滤波的UUV的目标跟踪定位算法，建立UUV运动模型及观测模型。本发明将一种HuberM极大似然估计代价函数引入Huber‑Cubature卡尔曼滤波并应用于UUV的目标跟踪定位算法中，建立了UUV运动模型及观测模型，通过对CKF测量信息进行了重建，之后再通过CKF算法对非线性测量信息进行滤波，无需利用传统线性化方法，避免了噪声对信息的干扰，实现了非线性鲁棒滤波。

Description

一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目 标跟踪方法

技术领域

本发明涉及无人水下航行器技术，尤其涉及一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法。

背景技术

水下无人航行器作为近年来海洋领域的热点研究方向之一，其在军事领域、渔业、水下探测等诸多方面扮演了越来越重要的角色。水下无人航行器作为水下探测、水下维修、水下特定区域样本采集的重要辅助工具，为人类科学探索、合理开发海洋资源，提升我国有效利用国际海域资源、顺利开展相关科学研究提供了可靠保障。

目前，无人水下航行器(Unmanned Underwater Vehicle,UUV)的目标跟踪主要通过检测目标的有效辐射(如声辐射和电磁辐射)以获取目标相对于UUV的连续方位角。然而，由于水下恶劣环境致被动声呐的测量精度低，海洋水文环境(海水温度、密度和盐度变化等)复杂，噪声统计特性(平均值和协方差)未知，估计的准确性未知。且非线性的测量量存在于UUV的目标跟踪中，给滤波算法带来了极大的困扰。因此，基于精算滤波算法的UUV目标跟踪算法对于工程实际应用有着重要意义。

边信黔等在“EKF的无人潜航器航位推算算法”中，将EKF滤波算法应用到UUV的航迹推算上，取得较好效果，但EKF对高非线性系统的预测准确性较差，而且需要计算雅可比矩阵，这导致了计算系统的复杂，应用性不强。

发明内容

本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法。

本发明解决其技术问题所采用的技术方案是：一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法，包括以下步骤：

1)建立无人水下航行器的运动方程，具体如下：

其中，x(k)，y(k)，z(k)为k时刻目标的位置，为k时刻目标的速度，ΔT为获取目标状态的观测时间间隔，为过程噪声；

2)建立无人水下航行器的观测方程，具体如下：

Z(k)＝TX(k)+W(k)，

其中，Z(k)为传感器在k时刻的观测值，T为观测转换矩阵，

W(k)＝w(k)+B(k)b，

其中，w(k)为零均值、白色高斯量测噪声向量，w(k)＝[w(x),w(y),w(z)]^T，b为系统偏差，B(k)为系统转移矩阵，表示为：

3)采用基于Huber-Cubature卡尔曼滤波的UUV的目标跟踪定位算法，建立UUV运动模型及观测模型，

令k时刻的状态值x_k与估计值之间的关系表达式为

其中，δx_k为估计偏差，设方差为P_k|k-1,线性化后的观测方程线性化矩阵为：

式中：H_k为线性化观测方程的斜率矩阵，表示为H_k＝[(P_k|k-1)^-1P_x,y]^T (3)

定义如下表达式：

Z_K＝M_kx_k+x_k (8)

引入Huber极大似然估计代价函数：

式中，g_i为残差向量分量，g＝M_kx_k-Z_k，ρ(g_i)为

式中，γ为给定的阈值参数，γ＝0.1345时，在符合高斯分布条件下，HuberM-CKF滤波算法是L₂范数估计效率的95％左右。令则最小化式(9)代价函数得

若则得

ψ＝diag[ψ(g_i)] (13)

对上式采用迭代算法进行预测求解：

迭代初值选取为则迭代终止后估计值与真实值偏差表达式为

本发明产生的有益效果是：本发明将一种HuberM极大似然估计代价函数引入Huber-Cubature卡尔曼滤波(H-CKF)并应用于UUV的目标跟踪定位算法中，建立了UUV运动模型及观测模型，通过对CKF测量信息进行了重建，之后再通过CKF算法对非线性测量信息进行滤波，无需利用传统线性化方法，避免了噪声对信息的干扰，实现了非线性鲁棒滤波。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例的方法流程图；

图2是本发明实施例的水下无人航行器轨迹图；

图3是本发明实施例的各方向滤波前后误差图；

图4是本发明实施例的H-CKF算法与CKF、EKF、CMKF滤波算法误差对比图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

如图1所示，一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法，包括以下步骤：

(1)建立运动方程，模拟水下机器人在水下的运动，首先对目标的运动规律进行建模，假设目标在三维空间内以匀速直线运动，在离散时间系统下k+1时刻目标运动方程可以表示为：

式中，x(k)，y(k)，z(k)为k时刻目标的位置，ΔT为获取目标状态的时间间隔。在实际目标运行过程中，目标不可避免的会受到环境因素的影响，速度会发生小范围的波动，不可能做严格的匀速直线运动，这些影响因素可以视为过程噪声来进行建模，所以加入过程噪声对运动方程修正为：

目标的速度可以表示为：

将运动方程用矩阵的形式表示如下：

在匀速直线运动模型中，目标的状态方程反映了在不同时刻之间目标状态的关联，运动目标的状态向量表示为因此目标状态方程表示为：X(k+1)＝FX(k)+ΓV(k)(5)，其中，F为状态转移矩阵，V(k)为过程噪声。

公式(1)-(5)模拟产生水下机器人的真实轨迹，所得数据称之为真值，由于在此模型中所假设水下机器人在Z方向上速度为零，在X-Y平面上做匀速直线运动；但是实际应用中的声呐无法直接得到真实值，只能得到测量值，也就是本发明数学模型中添加噪声的测量值，由于噪声误差的存在，声呐得到的数据可能与真实值差别很大，图2为真实轨迹和测量轨迹。

(2)建立量测方程，量测方程是对传感器量测过程的模型假设，量测数据往往是在极坐标系下的。因此设传感器在k时刻的量测值为

其中，和分别为斜距离量测、方位角量测和高低角量测；将其转换到以测量点为坐标原点、以大地为基准平面的直角坐标系(即参考坐标系)中，则有：

应用泰勒展开式，并忽略高阶项，不难得到：

Z(k)＝TX(k)+W(k) (8)

其中，T＝[I3,03]，为测量转换矩阵。

在理想状态没有系统偏差的条件下：

W(k)＝w(k) (9)，

在有系统偏差条件下：W(k)＝w(k)+B(k)b (10)，

其中，w(k)为零均值、白色高斯量测噪声向量，

w(k)＝[w(x),w(y),w(z)]^T (11)，

b为系统偏差，B(k)为系统转移矩阵，可以表示为：

(3)将一种HuberM极大似然估计代价函数引入Huber-Cubature卡尔曼滤波并应用于UUV的目标跟踪定位算法中，建立UUV运动模型及观测模型产生运动数据并模拟声呐完成信息采集。

CKF介绍：

1)构造条件性高斯模型：

z_k～p(z_k/z_k-1) (13)

x_k＝A(z_k)x_k-1+B(z_k)w_k+F(z_k)u_k (14)

y_k＝C(z_k)x_k+D(z_k)v_k+G(z_k)u_k (15)

z_k为非线性状态值，x_k为线性状态，y_k为观测值，u_k为控制输入值，w_k和v_k是均值为0方差分别为Q_w和R_v的高斯白噪声，A为状态矩阵，C为观测矩阵，B、D为噪声矩阵，F、G为输入输出矩阵。

2)CKF时间更新

X^* _i,k|k-1＝f(X_i,k-1|k-1) (17)

3)CKF量测更新

Z_i,k|k-1＝h(X_i,k|k-1) (21)

W_k＝P_xz,k|k-1P^-1 _zz,k|k-1 (25)

P_k|k＝P_k|k-1-W_kP_zz,k|k-1W^T _k (27)

以上为CKF算法的表述，本发明为解决以往CKF中存在的非高斯白噪声问题，采用极大似然HuberM方法对其优化。

具体做法：令k时刻的状态值x_k与估计值之间的关系表达式为

估计偏差δx_k，方差P_k|k-1,线性化后的观测方程线性化矩阵为

式中：H_k为线性化观测方程的斜率矩阵，可表示为H_k＝[(P_k|k-1)^-1P_x,y]^T (30)

定义如下表达式：

Z_K＝M_kx_k+x_k (35)

引入Huber极大似然估计代价函数：

式中g_i为残差向量分量，g＝M_kx_k-Z_k，ρ(g_i)可表达为

式中，γ为给定的阈值参数，γ＝0.1345时，在符合高斯分布条件下，HuberM-CKF滤波算法是L₂范数估计效率的95％左右。令则最小化式(36)代价函数得

若则得

ψ＝diag[ψ(g_i)] (40)

对上式可采用迭代算法进行预测求解：

迭代初值一般选取为则迭代终止后估计值与真实值偏差表达式为

根据上述结果，得到状态估计输出和P_k|k，完成对目标的跟踪。

仿真实验：

在不同噪声干扰下与Cubature卡尔曼滤波(CKF)、Extended卡尔曼滤波(EKF)、Converted Measurement卡尔曼滤波(CMKF)进行了仿真对比实验，充分的验证了Huber-Cubature卡尔曼滤波方法的优越性。

本仿真实验设定UUV模型为深海水下机器人“海狮”号,携带高度计、X/Y倾角传感器、DVL和前视声呐，参数参见下表:

如图3，在线性系统内四种卡尔曼滤波表现基本相同。

如图4可知，在高非线性系统内，CMKF已经发散，误差值偏大，在工程应用上表现不佳。相比较于CMKF，EKF和CKF收敛速度明显加快，数值也比较稳定，在低噪声条件下误差值稳定在50米内，在高噪声条件下误差值稳定在1000米以内，工程上表现良好；但EKF和CKF还是存在很大不足，两种滤波算法在高噪声条件下的误差超过500米，无法再减小，性能不如HuberM-CKF。

从收敛速度方面分析HuberM-CKF大概从20秒左右已经开始收敛，EKF和CKF大约从30秒左右才开始收敛，收敛速度明显加快；从误差值方面分析，HuberM-CKF最后收敛的误差值明显小于EKF、CKF的收敛误差值。

通过理论分析和仿真案列，可得到如下结论：

1)卡尔曼滤波在处理线性系统误差方面性能优异，但在非线性系统内性能不佳。

2)在低噪声状况下，CKF无论在收敛速度或是误差值方面都优于EKF，但在高噪声条件下二者性能差别不明显。

3)HuberM-Cubature卡尔曼滤波在水下目标的信号收集处理方面表现良好，极大地减小了误差且比CKF、EKF收敛速度更快，误差数值更小，另外HuberM-Cubature卡尔曼滤波的数值稳定性和跟踪精度要更高。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (3)

1.一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法，其特征在于，包括以下步骤：

1)建立无人水下航行器的运动方程；

2)建立无人水下航行器的观测方程；

3)采用基于Huber-Cubature卡尔曼滤波的UUV的目标跟踪定位算法，建立UUV运动模型及观测模型，

3.1)令k时刻的状态值x_k与估计值之间的关系表达式为

其中，δx_k为估计偏差，设方差为P_k|k-1,线性化后的观测方程线性化矩阵为：

式中：H_k为线性化观测方程的斜率矩阵，表示为H_k＝[(P_k|k-1)^-1P_x,y]^T

定义如下表达式：

Z_K＝M_kx_k+x_k

3.2)引入Huber极大似然估计代价函数：

式中，g_i为残差向量分量，g＝M_kx_k-Z_k，ρ(g_i)为

式中，γ为给定的阈值参数，令则最小化极大似然估计代价函数得:

若则得

ψ＝diag[ψ(g_i)]

3.3)对上式采用迭代算法进行预测求解：

迭代初值选取为则迭代终止后估计值与真实值偏差表达式为

2.根据权利要求1所述的基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法，其特征在于，所述步骤1)中无人水下航行器的运动方程，具体如下：

其中，x(k)，y(k)，z(k)为k时刻目标的位置，为k时刻目标的速度，ΔT为获取目标状态的观测时间间隔，为过程噪声。

3.根据权利要求1所述的基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法，其特征在于，无人水下航行器的观测方程，具体如下：

Z(k)＝TX(k)+W(k)，

其中，Z(k)为传感器在k时刻的观测值，T为观测转换矩阵，

W(k)＝w(k)+B(k)b，

其中，w(k)为零均值、白色高斯量测噪声向量，w(k)＝[w(x),w(y),w(z)]^T，b为系统偏差，B(k)为系统转移矩阵，表示为：