基于自适应交互多模型算法的三维空间水下动态目标跟踪

秦洪懋，叶宏伟，崔庆佳，徐 彪，胡满江

（1. 湖南大学 机械与运载工程学院 整车先进设计制造技术全国重点实验室，湖南 长沙 410082；2. 湖南大学无锡智能控制研究院，江苏 无锡 214115）

0 引言

自主水下机器人（autonomous underwater vehicle，AUV）作为高科技水下平台，被广泛应用于海洋资源探测、目标探测和目标侦察等民用和军用领域，目前已成为各国竞相发展的重要海洋装备。其中，AUV 动态目标跟踪技术是实现目标探测、目标侦察等任务的核心技术之一。AUV 利用其配置的摄像头、声呐等传感器探测水中的目标，通过算法处理获取目标物观测信息，从而得到目标的运动状态信息，进而针对目标物运动状态执行相应的轨迹跟踪、姿态调整等控制指令。而目标物运动状态的精准预测是动态目标跟踪控制技术的基础，其核心是基于传感器测量数据对目标物的位置、速度等状态信息进行预测。

目标跟踪的传统方法是采用单一的运动模型，如恒定速度（constant velocity， CV）模型、恒定加速度（constant acceleration， CA）模型等，这些模型利用卡尔曼滤波、扩展卡尔曼滤波等算法进行求解。近些年来，一些更加准确的模型开始被引入，并且结合各种优化的滤波算法，从某个方向上也使跟踪精度得到了提高。但是当跟踪的目标发生机动行为时，使用单一模型的算法会出现模型失配情况，从而会造成目标跟踪精度的大幅降低。针对目标发生机动运动或者系统模型不确定的问题，目前较为成熟的解决方法是采用交互式多模型（interacting multiple model， IMM）［1-2］，其最直观的思想就是如果任意一个模型都不能描述出目标的所有运动状态，那么就用多个模型并行对目标状态进行估计，然后融合结果，通过马尔可夫转移概率矩阵实现模型间的转换，因此具有自适应跟踪能力。IMM算法受所使用的滤波方法的影响。无迹卡尔曼滤波器（unscented Kalman filter， UKF）［3］从统计学角度出发，采用无迹变换得到Sigma 点来对非线性系统进行近似。扩展卡尔曼滤波器（extend Kalman filter， EKF） 通过泰勒展开来完成局部线性化，与之相比UKF则拥有更高的精度和稳定性，而所需的计算量又远远比粒子滤波器的小［4］。因此选用UKF 构成的IMM-UKF 算法在目标跟踪领域得到了广泛的应用。但是标准IMMUKF算法的模型间切换存在滞后性，往往在目标发生机动后一段时间算法才能切换为匹配模型进行跟踪［5］。如果可以根据当前系统模型信息自适应地修改转移概率，IMM 算法的性能将得到充分的提高。许多研究人员在该领域进行了卓有成效的探索。在文献［6］中，误差压缩率由基于模型的状态估计与交互步骤之后状态估计的比率定义。该误差压缩率有助于实现转移概率的适应性，从而提高IMM 算法的估计精度。然而，这种方法仅适用于双模型系统。文献［7］概括了文献［6］中得出的结论，所提出的方法不再局限于双模型系统。然而，如果系统模型不跳跃，则会增加局部误差。在文献［8］中，转移概率根据模型概率修正。然而，由于模型概率包含过去模型的信息，该算法在响应模型跳转时存在严重的滞后。在文献［9］中，转移概率根据基于模型的似然函数的比率修正，这种方法可以实现转移概率的自适应。然而，由于比率不受限制，转移概率矩阵（transition probability matrix， TPM） 中的主对角元素将不再占主导地位，因此算法的性能严重下降。文献［10］提出了一种基于自适应转移概率矩阵的并行IMM算法，其将模型跳跃阈值引入转移概率校正函数中，将过去模型信息的影响最小化，减少了模型跳跃过程中的响应时间并减小了估计误差，保持了抗噪声能力。

但是目前大多数的目标跟踪都只是考虑了对二维平面中的机动目标进行跟踪。本文基于文献［10］中的并行IMM算法进行了改进，且改进算法中的模型集合选择了CV模型、当前统计（current statistical， CS）模型和自适应转弯速率的三维固定中心恒定速率和转向速率（CSCTR）模型，提出了APIMM-UKF 算法，对三维空间中的机动目标进行状态预测；并通过数值仿真证明了本文所提算法兼顾了模型切换速度和预测精度。

1 目标模型

机动目标模型描述了目标状态随着时间变化的过程。基于模型的预测算法，如果模型与目标当前运动状态越是匹配，预测的结果就越是精准。因此，选择目标的运动模型十分重要。

1.1 运动模型

目标运动模型的精度对跟踪器的跟踪精度至关重要，因为一旦假设模型预测的运动与目标的实际运动不匹配，那么跟踪器的性能就会恶化。

在三维空间对目标运动过程进行仿真，k时刻的目标状态为X(k) =[xk，vxk，axk，yk，vyk，ayk，zk，vzk，azk]T，其分别包含了目标在x、y和z三个方向上的位置（xk，yk，zk）、速度（vxk，vyk，vzk）和加速度（axk，ayk，azk）信息。考虑水下运动目标的特点，本文选择CV 模型、CS模型和CSCTR模型对目标运动状态进行建模。

1.1.1 CV模型

CV模型的离散时间状态方程一般式为

式中：Fk，CV——CV 模型的状态转移矩阵；Γk，CV——CV 模型的噪声驱动矩阵；ωk——过程噪声，ωk=为x、y和z方向的过程噪声，其是均值为零、方差为Qk的高斯白噪声。

ωk是一个离散的白噪声序列，其对状态变量产生“最小”的影响，反映了因扰动和其他因素导致不可预测的建模错误。

假设采样周期为T，则每个参数的具体值为

式中：FCV——CV 模型的状态转移矩阵子矩阵；ΓCV—— CV 模型的噪声驱动矩阵子矩阵。

1.1.2 CS模型

CS 模型本质上是一个具有自适应均值（非零均值）的加速度的Singer模型。其对目标运动加速度的变化具有更好的敏感性，同时有更好的实时处理能力［11］。

CS模型的离散状态方程和观测方程如下：

式中：Γk，CS—— CS 模型的状态转移矩阵；Gk，CS——CS 模型的控制转移矩阵；——控制向量平均值。

CS 模型的观测方程与CV 模型的观测方程相同。不同之处在于其将Gk，CS和添加到了状态方程中。与CV模型相比，状态转移矩阵和过程噪声的表达式也不一致。该模型的关键假设是当前加速度均值等于估计加速度向量， 即从观测向量Z(k)到时刻k获得的被视为。目标的机动频率通常被设置为固定值α，目标最大和最小机动加速度通常被设置为固定值amax和a-max，当前加速度的方差为。

假设采样周期为T，则：

式中：FCS—— CS 模型的状态转移矩阵子矩阵；Fk，CS—— CS 模型的状态转移矩阵。

CS 模型的控制转移矩阵子矩阵如下：

CS 模型的过程噪声协方差矩阵如下：

1.1.3 CSCTR模型

CSCTR模型［12］假设目标以一个恒定速度在固定平面上围绕一个固定中心旋转，同时利用目标的加速度和速度向量定义了一个具有恒定转弯速率的圆弧运动。对于恒定速率和转向速率的运动目标，其加速度和速度向量是正交的（即a·v= 0），那么可以得到其转弯速率ω：

式中：a——加速度向量，a=[ax，ay，az]T；v——速度向量，v=[vx，vy，vz]T。

CSCTR模型的离散状态方程和观测方程与CV模型的相同。不同之处在于CSCTR模型的状态转移矩阵和噪声驱动矩阵的表达式与CV 模型的是不同的。假设采样周期为T，则：

式中：FCSCTR——CSCTR 模型的状态转移矩阵的子矩阵；Fk，CSCTR——CSCTR模型的状态转移矩阵。

式中：ΓCSCTR——CSCTR 模型的噪声驱动矩阵的子矩阵；Γk，CSCTR——CSCTR模型的噪声驱动矩阵。

1.2 量测模型

目标的量测模型为

式中：Z(k)——k时刻目标状态的观测向量，Z(k) =其中，分别为方位角、俯仰角、距离和距离变化率；νk——观测噪声，其是均值为零、方差为Rk的高斯白噪声；h()——状态向量X(k)和观测值Z(k)之间的转换方程。

h()转换关系为

2 自适应并行IMM-UKF算法

2.1 标准IMM-UKF算法

标准IMM-UKF算法由输入交互、模型条件滤波、模型概率更新和估计融合4 个部分组成，算法流程如图1 所示。下文详细描述包含N个子运动模型的标准IMM-UKF 算法从（k-1）时刻到k时刻4 个步骤的递推过程。

图1 IMM-UKF 算法流程Fig. 1 Flow of IMM-UKF algorithm

图2 APIMM-UKF 算法流程Fig. 2 Flow of APIMM-UKF algorithm

2.1.1 输入交互

对模型条件初始化，或者根据前一时刻系统状态估计和协方差估计来推测当前时刻测量值并对模型重新初始化，得出模型当前时刻各个UKF输入的状态向量和协方差矩阵。

模型概率预测计算公式如下：

式中：μk|k-1，i——（k-1）时刻预测k时刻模型i为匹配模型的概率；kji——转移概率矩阵的元素，表示从模型j切换到模型i的概率；μ(k-1)，j——（k-1）时刻模型j为匹配模型的概率。

混合权重计算公式如下：

式中：μ(k-1)，j|i——（k-1）时刻模型j转移到模型i的混合转移权重。

混合状态估计计算公式如下：

混合协方差计算公式如下：

式中：-1|k-1，i——（k-1）时刻的混合协方差矩阵；Pk-1|k-1，i——（k-1）时刻协方差矩阵的值；-1|k-1，j——（k-1）时刻模型j的混合状态向量估计值。

2.1.2 模型并行滤波

利用交互作用器得出的混合估计和混合协方差，分别采用不同运动模型的UKF滤波器进行滤波，得到k时刻状态估计值和协方差估计值Pk|k，i。

2.1.3 模型概率更新

通过似然函数更新各个模型的概率，计算公式如下：

式中：μk，i——k时刻模型i为匹配模型的概率；Λk，i——模型i的似然概率；z͂k，i——量测残差；Sk，i——残差协方差，其可通过UKF滤波获得。

2.1.4 估计融合

将模型并行滤波得出的k时刻目标状态向量的估计值和协方差矩阵与更新后的模型概率结合，得出总的融合估计。具体过程为

式中：——k时刻目标状态向量估计值。

式中：Pk|k——k时刻协方差估计值。

2.2 APIMM-UKF算法

模型交互是IMM 算法的标志性步骤，其中，根据TPM的指导，将基于模型的状态估计及其协方差进行重组。这种重组使得IMM算法在不增加并行滤波器数量的情况下考虑了更多的模型跳转信息，有效地提高了算法的估计精度。然而标准IMM 算法的TPM 是固定的，这使得算法的性能难以满足需求。因此，利用系统当前信息来实现转移概率的自适应调整，就能在目标发生机动后快速切换到相应的模型，从而提高算法精度［13］。

文献［10］提出了一种自适应转移概率矩阵的并行交互多模型（ATPM-PIMM，APIMM）算法，其由一个标准IMM（common IMM， CIMM）算法和一个自适应转移概率的IMM并行组成，模型跳跃阈值Th用于识别模型是否跳跃，然后使用不同形式的校正函数对转移概率进行校正。因此，APIMM-UKF 算法既能平滑噪声，又能快速响应模型跳跃，最大限度地减小了状态估计的误差。APIMM-UKF算法流程如图 2所示。

转移概率校正函数fj为

其中，

式中：μj，C——模型j在CIMM 算法中的概率；μj，A——自适应IMM 算法中模型j的模型概率，这是由模型的过去信息纠正的；Λr——匹配模型的可能性函数；Λl——其他模型的可能性函数。

2.3 改进的APIMM-UKF算法

为了更好地利用前时刻的目标状态信息，采用当前时刻与上一时刻该模型的概率之比对算法中模型的转移概率进行实时修正，这样转移概率校正函数如下：

当k时刻模型j的概率比上一时刻大，校正函数fj就会大于1，从而k时刻模型j的转移概率就随之增大；反之，fj小于1，k时刻模型j的转移概率就随之减小。

3 仿真实验

本文选用CV、CS和CSCTR这3个模型对在三维空间中运动的单目标跟踪进行仿真：第一阶段（0～20 s），做匀加速直线运动；第二阶段（20～40 s），做匀速直线运动；第三阶段（40～85 s），做角速度为的匀速右转弯运动；第四阶段（85～130 s），做匀速直线运动；第五阶段（130～145 s），做垂直面向上匀速转弯运动；第六阶段（145～210 s），做匀速直线运动。

3 个模型初始概率均为1/3，CIMM 算法初始转移概率矩阵为

APIMM 算法的模型跳跃阈值选择0.8，初始转移概率矩阵为

测量噪声标准差为[1 1 8 0.1]T，采样时间为1 s，声呐平台位置为（0，0），目标初始状态向量为[30 0 0 -60 0 0 0 0 0]T。在CS 模型中机动频率最大和最小机动加速度为a±max=±0.5 m/s2。分别以本文改进的APIMM-UKF 算法和标准IMM-UKF 算法进行200次蒙特卡罗仿真，得到图 3～图6。

图3为不同时刻2种算法的距离均方根误差，几乎所有时刻本文的算法的误差都比标准IMM算法的小。图 4是2种算法预测的目标在不同时刻的位置和真实值的比较，可以看出，本文算法的预测精度远远高于标准算法的。图 5 和图 6 分别是基于标准算法和本文算法的每个运动模型的概率变化曲线，对比这两个图可以看出，标准算法各模型概率都稳定平缓，对目标机动反应变化不够明显，这是由于标准算法没有利用状态估计的后验信息对概率转移矩阵进行修正。图5 中，CV 模型的概率一直较高，在90%左右，而另外2 个模型只有5%左右，基本没有实现很好的模型切换。而从图6 中可以看出，本文算法在跟踪目标发生机动运动状态改变后，可以比较快地作出反应，相应地提高了与目标运动状态匹配的模型概率，同时抑制了其他不匹配模型的概率；并且当目标稳定在一个运动状态时，匹配模型的概率达到90%以上。

图3 距离均方根误差Fig. 3 Distance root mean square error

图4 跟踪轨迹Fig. 4 Tracking trajectory

图5 IMM-UKF 算法下各模型概率Fig. 5 Probability of models under the IMM-UKF algorithm

图6 APIMM-UKF 算法下各模型概率Fig. 6 Probability of models under the APIMM-UKF algorithm

4 结束语

本文首先将现有的用于二维平面目标跟踪的APIMM算法进行了改进；然后，结合三维的CV、CS和自适应转弯速率CSCTR模型，用于三维空间中的机动目标跟踪；最后，对水下三维空间运动目标进行跟踪仿真实验。结果显示，本文改进的跟踪算法能有效地提高与目标运动状态相匹配的运动模型的概率，抑制了其他非匹配运动模型对跟踪结果的影响，既能平滑噪声，又能快速响应模型跳跃，减小了对目标状态预测的误差，在水下机动目标的跟踪上具有很好的应用价值。在未来工作中，还可以对使用的目标运动模型进行改进，自适应参数使其更加匹配目标的运动状态。