基于交互多模型Kalman的无人集群跟踪优化算法

高文哲 李 智

(四川大学电子信息学院 四川 成都 610065)

0 引 言

我国领海和领空地域辽阔，为防止非法机动目标闯入我国海域和空域，往往采用无人集群巡逻编队方式[1]实现对海域或空域的巡视。在这种场景下，需要派出无人艇或无人机对可疑目标进行跟踪。常见的跟踪方法有视觉目标跟踪和电磁目标跟踪[2]，由于地域辽阔，调用摄像头从视频中定位跟踪目标的精度较低，并且跟踪范围受限，所以往往利用雷达或者电磁测距传感器实现对电磁目标的跟踪行为。但传感器采集的数据都携带噪声干扰，这会直接导致目标的实际位置与探测器的探测位置有较大的偏差，使得跟踪行为不精确。所以在工程运用中，无论是对雷达数据还是测距传感器的数据都需要通过滤波方法[3]去得到与目标真实位置较为接近的滤波数据。

卡尔曼滤波是噪声处理的有效手段。在由观测数据和噪声形成的线性高斯系统中，常用经典的卡尔曼滤波[4]对目标的状态做最优估计，得到较好的跟踪效果。但为满足实际系统中存在的非线性模型[5]，又提出了EKF[6]和无迹卡尔曼滤波(UKF)[7]等方法。EKF主要是将非线性模型进行泰勒展开略去二阶及以上项，将一个非线性系统近似成线性系统，而UKF是对非线性系统中的一步预测方程使用无迹变换(UT)[8]，避免EKF引入的线性化误差。为解决EKF算法鲁棒性[9]的问题，引入了强跟踪卡尔曼滤波算法(STF)[10]。而集合卡尔曼滤波[11]解决了传统卡尔曼滤波计算复杂度高的问题，是对经典卡尔曼滤波的优化改进。为了进一步减小噪声对估计值的影响，提出神经网络卡尔曼滤波[12]实现对噪声统计特性的自适应估计[13]。而当目标突然转弯或者加、减速时，需要采用自适应能力较强的滤波算法——交互多模型Kalman滤波(IMM)算法。

本文在无人集群协同作业场景下，当出现可疑目标时，集群调度系统需要调度当前距离可疑目标最近并且无其他任务的无人智能单元对目标进行跟踪，同时选择若干距离次之的无人智能单元对目标进行协同跟踪，将每个无人智能单元上的电磁传感器的输出作为优化算法的输入，优化算法通过IMM滤波对每组数据进行滤波处理后得到若干组跟踪目标的状态估计值，将这些估计值作为极大似然估计融合算法的样本值，利用极大似然估计通过样本值求出每个智能单元估计值的似然概率，并以似然概率作为若干智能单元的状态估计权值，最后输出经过数据融合的跟踪目标状态估计值，在电磁传感器的每一个采样周期内都进行一次上述过程的处理，形成最终的优化目标跟踪路线。最后仿真结果展示了在无人艇群的场景下，单艇运用IMM滤波和多艇运用集群优化算法，以及不同数量的多艇采用基于IMM滤波的优化算法进行目标跟踪的这两组跟踪路径对比结果，以及这两种情况下的滤波误差均值和标准差对比结果。

1 基于IMM的无人集协同跟踪算法

1.1 无人集群跟踪模型架构

无可疑目标情况下，无人集群在各自的巡视区域内巡逻。当出现可疑目标时，集群调度系统会派遣一艘暂无作业任务的无人智能单元对目标实行跟踪，但为加强跟踪精度，本文考虑让靠近可疑目标的、未收到跟踪任务的其他智能单元协同参与目标的路线估计。在执行跟踪目标的无人智能单元以及协同估计的无人智能单元数量确定后，在每个采样周期内调度系统依据其他无人智能单元与可疑目标的距离分配协同无人智能单元，与跟踪无人智能单元并行地从各自的传感器搜集对可疑目标的观测数据，然后分别执行IMM滤波算法得到各自对可疑目标的估计值，通过距离坐标转换算法将采样周期内的估计值全部转化成相对于跟踪无人艇初始点的位置，再将转化后的每组状态估计值作为极大似然估计系统的输入求出每组值得出似然估计概率，并将此似然估计概率作为每组IMM滤波输出状态值得到权值，通过这种交互策略求出的权值能使跟踪无人艇的最终跟踪路线有较好的跟踪效果，最后通过带权融合系统得到最优的跟踪目标状态估计值，执行跟踪任务的无人智能单元利用这个最优值对目标进行可靠跟踪，而参与协同跟踪任务的无人智能单元的路径不受影响。

图1为无人集群目标跟踪系统的整体架构。

图1 无人集群目标跟踪系统结构

需要说明的是，为了避免当前跟踪任务对协同无人智能单元原任务产生影响，协同跟踪只是协同无人智能单元在原巡逻路线上对目标进行距离坐标数据的采集，协同无人智能单元并不是固定的。在整个跟踪过程中，电磁传感器在每一个采样周期内，都需要依据距离对协同无人智能单元的选择进行一次更新，所以在整个跟踪过程中，除了最开始确定为跟踪任务的无人智能单元不需要改变，协同无人智能单元都是依据距离不断更新的。而不同的协同无人智能单元在采样周期内将所采集的数据送至集群数据转换系统，经过距离转换算法，换算成跟踪无人智能单元与可疑目标的距离，然后再经过IMM滤波处理。

1.2 交互多模型卡尔曼滤波

IMM算法虽然包含不同的模型，但这些模型中都不存在一个完全正确的模型，输出是多个滤波器估计结果的加权平均值。权重即为该时刻模型正确描述目标真实运动的模型概率。

IMM算法每次递推含四个步骤。

预测计算式表示为：

(1)

式中：φj表示被观测系统的状态传递矩阵。

预测误差协方差计算式表示为：

(2)

式中：Gj表示系统扰动矩阵；Qj表示系统误差的协方差矩阵。

Kalman增益计算式表示为：

Kj(k)=Pj(k|k-1)HT[HPj(k|k-1)HT]-1+

Pj(k|k-1)HTR-1

(3)

式中：H表示信道矩阵；R表示测量误差的协方差矩阵。

状态更新计算式表示为：

Kj(k)H(k)Xj(k|k-1)

(4)

式中：Z表示目标观察矩阵。

协方差矩阵更新计算式表示为：

Pj(k|k)=[I-K(k)H(k)]P(k|k-1)

(5)

步骤3模型概率更新。采用似然函数来更新模型概率μj(k)，模型j的似然函数表达式表示为：

(6)

步骤4输出交互。对每个滤波器的估计结果加权合并，得到总的状态估计Xj(k|k)和总的协方差估计P(k|k)。

总的状态估计计算式表示为：

(7)

总的协方差估计计算式表示为：

(8)

1.3 多模型建模

通过分析海域[14]环境、空域环境、可疑目标的运动规律可知，可疑目标一般在运动过程中呈现较大的机动性和随机性[15]，目标在被跟踪的过程，往往存在匀速、突然加减速和突然转弯等情况，所以在线性系统的框架下，利用IMM滤波算法构建的运动模型应当包含目标可能出现的所有状态。在此基础上构建三个模型，包含一个非机动模型描述目标的匀速运动状态，一个机动模型描述目标的加速转弯，另一个机动模型描述目标的减速转弯。

(1) 非机动模型。非机动模型下的物体加速度为0，同时假设非机动模型不受过程噪声的影响，即W(k)的方差为0。可疑目标的状态包含x和y方向上的速度、位移和加速度。由此可以得到在非机动模型下，可疑目标在k时刻的状态矩阵X(k)和状态转移矩阵φ(k)。得到以下推断:

(9)

式中：T表示时间。

(2) 机动模型。机动模型存在加速和减速的过程，同时假设两个机动模型都存在过程噪声，并且噪声方差分别为Q1=q1I2×2、Q2=q2I2×2。

(10)

上述两个机动模型的状态转移矩阵和噪声方差分别相等，即φ2=φ3，G2=G3，则有X1(k)=X2(k)。

滤波误差均值计算式表示为：

(11)

滤波误差的标准差计算式表示为：

(12)

1.4 多模型建模极大似然融合算法

1.4.1极大似然估计求权值

电磁坐标传感器的数据采集可信度α关于跟踪目标与传感器采样时的位置之间的距离的函数，以x方向的可信度为例，距离X服从μ=0的正态分布，即N(0,σ2),其概率密度函数f(x)满足：

(13)

(14)

对式(14)作对数变换，得到：

(15)

对式(15)分别进行对μ、σ2的一次偏导，结果如下：

(16)

(17)

联合解得：

(18)

(19)

(20)

根据似然值可以求得任意一个无人智能单元j对跟踪目标的状态估计值在这一组样本值中所占权值wj如下：

(21)

需要指出的是，由于电磁坐标传感器的数据可信度与自身和跟踪目标的距离有关，所以坐标传感器对于跟踪目标的观测值，在x方向与y方向有不同的可信度，权值的求解方法与上述过程一致。

1.4.2坐标转换

单个无人艇在进行跟踪目标的过程中，主要依靠自载的距离传感器对可疑目标进行数据搜集。同时，由于无人艇可以较为准确地按照既定规划路线行进，在已知跟踪起始点状态(X0,Y0)的前提下，在每一个采样周期内，结合无人智能单元在采样周期内的位移，将坐标传感器的数据转化成为相对于跟踪起始点的相对数据。即将动态观测站转化为定点观测站的过程。转化公式如下：

(22)

(23)

在完成定点观测站的转化后，需要对执行跟踪任务的无人智能单元和协同跟踪的无人智能单元进行状态估计值的带权融合，由式(21)求出的权值与式(22)、式(23)可得最优的集群状态估计值Xop(k)如下：

(24)

式中：Xop(k)为最优的预测输出，即最优跟踪路线。

整体算法流程如图2所示。

图2 无人集群目标跟踪系统流程

2 仿真结果与分析

利用坐标转换公式、IMM Kalman算法、集群优化算法对目标进行滤波跟踪，仿真首先在单艇的基础上，分别采用了适用于非线性系统的EKF算法和IMM Kalman算法对目标进行跟踪；然后在艇群的基础上，利用基于IMM的集群优化算法对目标进行跟踪，并用蒙特卡洛方法[17]仿真30次。需要指出的是，在无人艇群场景下，由于无人艇群的任务模式是多样的，往往包含对海域的巡视以及对可疑目标的侦察、定位和跟踪等。为保证艇群在任意时刻都能执行上述任务，提出的交互式多模型跟踪优化算法仅限于参与跟踪任务和协同跟踪任务的无人艇，不是执行跟踪和协同跟踪任务的其他无人艇不参与数据融合。

图3展示了扩展卡尔曼滤波对目标的跟踪轨迹，可以明显看出，在目标进行慢转弯时，EKF的跟踪性能开始变差，并且逐渐发散，丢失跟踪目标，由图4的位置估计偏差可以定量地反映出EKF在目标进行转弯时对目标跟踪的不可靠性。这也表明单一的非线性滤波方法无法较好地适应运动状态多变的目标。

图3 EKF跟踪结果与真实值对比

图4 EKF的位置误差估计

图5包含三条轨迹，可以看出，CIMMF算法在慢转弯以及慢转弯以后的匀速状态对目标的跟踪路径与目标真实轨迹更加接近，滤去大量的噪声，而单艇滤波算法虽大致上可以保持对目标的跟踪不丢失，但是两处转弯前后的估计值与真实值之间都有较大的偏差。即利用数据融合算法融合多无人艇的跟踪结果与真实值更为接近。

图5 轨迹对比

由式(11)可得单艇滤波和群艇滤波分别在x方向滤波误差均值，由式(12)可得单艇滤波和群艇滤波分别在x方向滤波误差标准差，仿真结果如图6、图7所示。在x方向上，群艇滤波的输出误差均值比单艇误差均值小很多，群艇滤波的输出误差标准差比单艇误差标准差小很多，艇群滤波效果远远好于单艇。在t=300 s时，由于目标在进行急转弯，所以单艇在此时的误差均值最大，而群艇优化算法大大减小了急转弯时的误差，对于快转弯时的自适应能力更强，艇群优化算法使得普通IMM Kalman滤波算法在转弯处更加稳定，降低了目标跟踪丢失率。

图6 x方向误差均值对比

图7 x方向误差标准差

图8为调用不同数量的无人艇协同跟踪目标的误差标准差对比，可见随着参与协同跟踪任务的无人艇数量的增加，IMM滤波优化算法的状态估计值的误差越来越小。通过分析又可以进一步发现，当参与跟踪和跟踪协同任务的无人艇数量增加到3以后，优化算法对噪声的滤波功能显著降低，这也表明，在实际的无人艇巡视过程中，如果出现可疑目标需要派遣无人艇进行跟踪。为了提高艇群整体的作业效率，在可以大幅度提高对目标的跟踪精度和可靠性的基础上，可以只派遣3艘左右的无人艇对可疑目标进行数据采集，其中的跟踪无人艇对目标进行跟踪。

图8 多艇采用CIMMF误差标准差对比

3 结 语

本文基于交互多模型卡尔曼滤波，利用极大似然估计算法和加权融合算法，既解决了无人集群在目标跟踪过程中，可疑目标突然改变方向而导致无人单元失去跟踪目标的问题，又解决了无人智能单元跟踪精度低的问题。针对集群协同跟踪的任务模式，实现的是数据上的协同搜集，只有一个无人智能单元对可疑目标进行实际的轨迹跟踪，这既保证了各个智能单元的任务独立性，又体现了群体的协同性。搜集的数据通过极大似然估计法积分得到每个样本值的最佳权值，经过数据融合后的结果无论是在跟踪可靠性还是跟踪精度方面都有较大的优化，更加接近目标的真实路径，跟踪效果更好。本文提出的集群滤波优化算法具有更强、更稳定的自适应能力，这种针对运动状态突变的算法在无人艇群、无人集群、无人驾驶等场景都有较大的应用前景。