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

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

An Unmanned Underwater Vehicle Target Tracking Method Based on HuberM-Cubature Kalman Filter

【技术实现步骤摘要】
一种基于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-1WTk(27)以本文档来自技高网...

【技术保护点】
1.一种基于HuberM‑Cubature卡尔曼滤波的无人水下航行器目标跟踪方法，其特征在于，包括以下步骤：1)建立无人水下航行器的运动方程；2)建立无人水下航行器的观测方程；3)采用基于Huber‑Cubature卡尔曼滤波的UUV的目标跟踪定位算法，建立UUV运动模型及观测模型，3.1)令k时刻的状态值xk与估计值

【技术特征摘要】
1.一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法，其特征在于，包括以下步骤：1)建立无人水下航行器的运动方程；2)建立无人水下航行器的观测方程；3)采用基于Huber-Cubature卡尔曼滤波的UUV的目标跟踪定位算法，建立UUV运动模型及观测模型，3.1)令k时刻的状态值xk与估计值之间的关系表达式为其中，δxk为估计偏差，设方差为Pk|k-1,线性化后的观测方程线性化矩阵为：式中：Hk为线性化观测方程的斜率矩阵，表示为Hk＝[(Pk|k-1)-1Px,y]T定义如下表达式：ZK＝Mkxk+xk3.2)引入Huber极大似然估计代价函数：式中，gi为残差向量分量，g＝Mkxk-Zk，ρ(gi)为式中，γ为给定的阈值参数，令则最小化极大似然估计代价函数得:若则得ψ＝diag[ψ(gi)]3.3)对上式采用迭代算...

【专利技术属性】
技术研发人员：范世东，王斌，
申请(专利权)人：武汉理工大学，
类型：发明
国别省市：湖北,42