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

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

背景技术：

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

目前，无人水下航行器(unmannedunderwatervehicle,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时刻的状态值xk与估计值之间的关系表达式为

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

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

定义如下表达式：

zk＝mkxk+xk(8)

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

式中，gi为残差向量分量，g＝mkxk-zk，ρ(gi)为

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

若则得

ψ＝diag[ψ(gi)](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)构造条件性高斯模型：

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

xk＝a(zk)xk-1+b(zk)wk+f(zk)uk(14)

yk＝c(zk)xk+d(zk)vk+g(zk)uk(15)

zk为非线性状态值，xk为线性状态，yk为观测值，uk为控制输入值，wk和vk是均值为0方差分别为qw和rv的高斯白噪声，a为状态矩阵，c为观测矩阵，b、d为噪声矩阵，f、g为输入输出矩阵。

2)ckf时间更新

x^*i,k|k-1＝f(xi,k-1|k-1)(17)

3)ckf量测更新

zi,k|k-1＝h(xi,k|k-1)(21)

wk＝pxz,k|k-1p^-1zz,k|k-1(25)

pk|k＝pk|k-1-wkpzz,k|k-1w^tk(27)

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

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

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

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

定义如下表达式：

zk＝mkxk+xk(35)

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

式中gi为残差向量分量，g＝mkxk-zk，ρ(gi)可表达为

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

若则得

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

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

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

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

仿真实验：

在不同噪声干扰下与cubature卡尔曼滤波(ckf)、extended卡尔曼滤波(ekf)、convertedmeasurement卡尔曼滤波(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卡尔曼滤波的数值稳定性和跟踪精度要更高。

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