基于矢量对偶四元数的水下机动目标自适应跟踪

侯翔昊，乔钢，周建波，杨益新

(1. 哈尔滨工程大学 水声技术重点实验室，黑龙江 哈尔滨 150001；2. 海洋信息获取与安全工业和信息化部重点实验室(哈尔滨工程大学)，黑龙江 哈尔滨 150001；3.哈尔滨工程大学 水声工程学院，黑龙江 哈尔滨 150001；4.西北工业大学 航海学院，陕西 西安 710072)

水下目标跟踪技术是对特定海域进行实时监测的必要前提，对水下目标进行准确可靠的跟踪是对其进行后续操作的基础。然而传统的水下目标跟踪技术常常受制于目标运动模型不准确、水下环境干扰而导致的目标观测信息失效以及跟踪算法缺少容错性等缺陷而不能提供良好的水下目标跟踪精度。因此，研究高精度、高鲁棒性的水下目标跟踪算法已然成为水下目标跟踪领域研究的热点与难点问题[1-2]。

在复杂的水下环境中，水下目标可能进行多种机动，建立准确的水下机动目标运动学与动力学模型是对其进行跟踪算法设计的基础。在水下目标跟踪领域，目前研究多集中在基于质点模型的水下目标运动学与动力学建模，即将水下目标视为质点，从而对其运动学与动力学过程进行数学描述。通常，采用不同的数学模型对水下目标的加速度进行表达，从而建立不同的水下目标运动学与动力学模型。这一类基于质点的模型主要有：匀速模型(CV模型)[3-4]、匀速转弯模型(CT模型)以及变加速模型(Singer模型[5-7]、“当前”统计模型[8]、匀加速模型(CA模型)[9]、自适应高斯模型[10]等)等。对水下机动目标而言，为了将多种运动学与动力学模型统一描述，克服目标模型表述的不确定性，文献[16]提出了交互式多模型对水下目标的运动学与动力学过程进行了统一描述，从而对水下机动目标进行有效跟踪。然而，随着目前水下机动目标运动复杂性的增强以及水下观测精度的提升，将水下目标视为质点而对其进行描述的方法已经不能满足对水下目标运动学与动力学准确建模的需求，将其视为质点进行建模将会忽略其质心与观测位置之间的位移，同时也会忽略由水下环境干扰等因素引起的水下目标平动转动耦合作用，当干扰较大时，可能会引起跟踪发散的情况。因此，为了满足当下对水下机动目标准确可靠跟踪的需求，需要研究将水下目标视为刚体，同时将水下目标的加速度与角加速度作为待定跟踪量的新型水下目标运动学与动力学建模方法。

由于水下环境复杂以及水下目标的运动学动力学模型与观测模型具有较强的非线性，一般设计非线性滤波跟踪算法对水下目标进行跟踪。其中，扩展卡尔曼滤波(EKF)[11]是最为广泛使用的非线性滤波方法。然而，由于EKF采用一阶泰勒级数展开对非线性观测模型进行线性化处理，可能会带来较大线性化误差。针对此不足，诸多采用不同线性化方法的贝叶斯滤波技术被提出并应用于目标跟踪过程中，其中具有代表性的有无迹卡尔曼滤波(UKF)[12]、容积卡尔曼(CKF)滤波[13]和粒子滤波(PF)[14-15]。但不论是UKF、CKF，还是PF，理论上越强的线性化能力带来的后果就是急剧增加的运算量与随之而来的计算不确定性。尤其在水下环境复杂，干扰众多的条件下，滤波算法的稳定性与可靠性需对跟踪结果至关重要。因此，为了对水下机动目标进行准确可靠的跟踪，设计具有自适应抗干扰能力的滤波跟踪算法十分必要。

本文将水下机动目标视为刚体，考虑水下目标的平动转动耦合效应，采用矢量对偶四元数对水下刚体目标进行六自由度一体化建模，避免传统基于质点的模型对水下目标建模的局限性；同时将水下机动目标的加速度与角加速度作为状态估计量进行跟踪估计，从而突破IMM算法要求完备模型集的限制。利用水下三维距离观测信息，建立目标观测点与跟踪平台之间的距离观测模型，并考虑水下复杂环境对观测量的干扰，结合新息信息，提出用于水下刚体机动目标跟踪的基于矢量对偶四元数的扩展卡尔曼滤波算法(DVQ-EKF)以及自适应扩展卡尔曼滤波算法(DVQ-AEKF)，以实现复杂水下环境情况下，对水下机动目标的准确可靠跟踪。

1 基于矢量对偶四元数的水下机动目标运动学与动力学模型

1.1 矢量对偶四元数(DVQ)

四元数具有如下形式：

q=(q0,q1,q2,q3)=q0+q1i+q2j+q3k

(1)

式中：q0是标量部分；q1i+q2j+q3k是矢量部分。其共轭表示为：(q0,-q1,-q2,-q3)。

同时，四元数与实数域的映射关系表示为：[·]:Q4→R4。在这种映射定义下，四元数叉乘可以定义在实数域内：

[a×b]=[a×][b]

(2)

类似地，四元数内积可以定义为：

[ab]=[a]L[b]

(3)

其中:

并且:

由于四元数仅仅描述了目标的转动过程，而忽略了对目标平动过程的表达，因此，单纯采用四元数对目标的运动进行建模将忽略其平动过程，从而不能完成对目标的转动平动一体化建模。为了解决这一问题，文献[17]提出对偶四元数的概念，文献[18]将对偶四元数用于空间目标的转动平动耦合一体化运动学与动力学建模中。

对偶四元数有：

(4)

式中q、q′均为四元数，并且对偶运算符ε2=0，ε≠0。

矢量对偶四元数有：

(5)

1)矢量对偶四元数加法：

(6)

2)矢量对偶四元数乘法：

(7)

3)矢量对偶四元数共轭：

(8)

4)矢量对偶四元数交换：

(9)

其余有关矢量对偶四元数的运算可以参考文献[18]。

1.2 水下机动目标模型综述

图1展示了水下机动目标的运动模型。{I}为惯性系，{B}为水下目标的本体系，坐标原点为目标质心。{B′}为水下目标观测点坐标系，假设与{B}平行，之间有位移ρ。

图1 水下机动目标模型Fig.1 Model of underwater maneuvering target

{B}相对于{I}的姿态四元数为：qB/I，{B}与{I}相对距离为rB/I，{B′}与{I}的距离为rm。同时，本文假设水下机动目标的线速度为v，角速度为ω。

1.3 基于矢量对偶四元数的水下机动目标运动学与动力学模型

由文献[18]，描述水下机动目标的位置和姿态的矢量对偶四元数为：

(10)

描述水下机动目标的速度和角速度的矢量对偶四元数为：

(11)

基于式(10)、(11)，由文献[18]，得到基于矢量对偶四元数的水下机动目标运动学方程为：

(12)

为了避免奇异，利用误差四元数将式(12)改写为：

(13)

当水下机动目标受控做机动运动时，基于矢量对偶四元数的水下机动目标的动力学方程为：

(14)

(15)

2 基于矢量对偶四元数的水下机动目标跟踪算法：DVQ-EKF与DVQ-AEKF

2.1 观测模型

由图1可知，观测点(比如潜艇壳体)与目标质心具有位移ρ，因此，观测方程为：

(16)

式中：A(qB/I)为水下目标质心与跟踪平台之间的姿态转移矩阵；v为观测噪声，满足：

E(v)=0

(17)

(18)

2.2 DVQ-EKF设计

水下机动目标跟踪系统的状态为：

(19)

由于水下复杂环境所带来的干扰，假设水下机动目标在运动过程中，所受干扰力矩对其加速度产的干扰为w，由水下复杂环境带来的观测干扰为v，并且假设w满足：

E(w)=0

(20)

(21)

Xk+1=ΦkXk+Wk

(22)

式中Φk≐I+FΔt，Δt为采样时间。

F为状态方程的雅可比矩阵，并且：

(23)

(24)

(25)

由式(16)，观测方程的雅可比矩阵为：

(26)

其中：

(27)

结合式(17)、(18)、(20)～(27)，基于扩展卡尔曼滤波(EKF)的框架[19]，得到基于矢量对偶四元数的水下机动目标跟踪算法(DVQ-EKF)。

2.3 水下机动目标跟踪算法设计

考虑水下复杂环境可能对观测信息造成干扰，从而影响DVQ-EKF中的测量更新精度，进一步导致跟踪精度下降甚至发散的问题，需要设计具有自适应调节能力的水下机动目标跟踪算法。

定义新息量为：

nk=Zk-h(Xk/k-1)

(28)

由式(28)，可得新息量的方差为：

(29)

当水下机动目标跟踪系统正常工作，即跟踪量满足最优估计时，DVQ-EKF的滤波增益为：

(30)

为了对受扰观测进行处理，需要通过计算nk，从而调节Kk对水下机动目标进行准确跟踪。将系统观测受扰的影响反映在观测噪声Rk上，由式(29)可得：

(31)

其中γk为观测噪声的比例因子矩阵，可表示为：

(32)

在[0,k]时刻中，有：

(33)

将式(33)代入式(32)可得：

(34)

假设观测干扰均由水下环境波动导致，为了使γk的对角元素均大于等于1，定义修正的比例因子矩阵的第ii个对角元素为：

(35)

由式(35)可得，修正的观测噪声矩阵为：

(36)

修正后的卡尔曼滤波增益为：

(37)

可见，通过式(37)对DVQ-EKF的滤波增益进行修正，可以得到在干扰下较为准确的跟踪量。基于扩展卡尔曼滤波(EKF)的框架[19]，将其中的卡尔曼增益与观测噪声矩阵替换为式(36)与式(37)，即可得到基于矢量对偶四元数的水下机动目标自适应跟踪算法(DVQ-AEKF)。

2.4 观测干扰分析

为了节约计算资源，提升跟踪系统的跟踪效率，对水下机动目标进行跟踪时，需要在DVQ-EKF以及DVQ-AEKF两者间进行合理切换。

定义系统的观测失效判别量为：

采用假设检验的思想，设定2个假设条件如下：

H0：跟踪系统不存在观测受强干扰扰情况，跟踪过程正常；

H1：跟踪系统存在观测受强干扰扰情况，跟踪过程异常；

基于上述2个条件，观测干扰分析步骤为：

1)给定临界概率ξ(0<ξ<1)，即满足：

为了更直观的说明本文所设计的水下机动目标跟踪算法的工作过程，其算法流程图如图2所示。

图2 水下机动目标跟踪系统算法流程Fig.2 Flow chart of the proposed tracking algorithm

3 实验仿真与分析

并假设在[200,225] s，水下复杂环境对距离观测量有随机附加扰动，其强度为：

基于上述条件，对DVQ-EKF于DVQ-AEKF的性能进行仿真测试，对水下机动目标的质心位移，质心速度和质心位置的跟踪结果如图3～8所示。

表1 水下机动目标初始参数Table 1 Initial values of the target

图3 DVQ-EKF与DVQ-AEKF跟踪下的质心位移误差Fig.3 Error of tracking distance by DVQ-EKF and DVQ-AEKF

图4 DVQ-EKF和DVQ-AEKF跟踪下的线速度误差Fig.4 Error of tracking velocity by DVQ-AEKF and DVQ-EKF

通过图3～8，可以看到采用DVQ-EKF或DVQ-AEKF均能在设定仿真环境下对水下机动目标进行跟踪。在水下目标进行机动，并在观测收到一定强度的扰动时，DVQ-EKF的质心位移的跟踪精度小于25 m，DVQ-AEKF的质心位移的跟踪精度小于10 m；DVQ-EKF的线速度估计误差小于0.7 m/s，DVQ-AEKF的线速度估计误差小于0.4 m/s。并且，通过本文提出的基于DVQ的建模方法，能够对水下目标的相关尺寸(即目标质心与观测点之间的位移，本文称为质心位置)做出估计，从而弥补了传统质点建模方法对该参数估计的盲区。通过仿真发现，DVQ-EKF的目标质心位置估计误差在3 m左右;DVQ-AEKF的目标质心位置估计误差在1 m左右。

图5 DVQ-EKF和DVQ-AEKF跟踪下的质心位置误差Fig.5 Error of tracking location of the mass point by DVQ-EKF and DVQ-AEKF

4 结论

1)本文将用于航空航天领域的矢量对偶四元数建模方法用于水下机动目标建模过程，克服了传统水下目标跟踪过程中采用质点模型描述目标运动学动力学过程的误差，并将CV与CA模型进行了一体化描述。

2)结合了自适应滤波的技术手段，通过对受扰观测进行更新权重调整，从而对水下机动目标进行了有效跟踪。

3)由于DVQ-AEKF采用了自适应调整跟踪滤波算法增益的技术手段，使其能够应对距离观测量受扰较大的情况，从而具有更高的跟踪精度与跟踪稳定性。