基于期望最大化与容积卡尔曼平滑器的机载多平台多传感器系统误差配准算法

2020-07-15贺丰收缪礼锋

控制理论与应用 2020年6期

程然，贺丰收，缪礼锋

(中国航空工业集团公司雷华电子技术研究所，江苏无锡 214063)

1 引言

机载多平台多传感器信息融合技术能够充分利用各类数据的特点，获取不同视角、不同传感器、不同特征的互补数据，大大扩展了探测区域的覆盖面积，提高了信息利用效率，加快了数据更新速率，从而能够显著提升飞机编队对目标的跟踪能力，提供更快捷的航迹起始速度，更短的航迹收敛时间，更精确的航迹跟踪精度和更稳定的航迹连续性[1-2].

然而在舰载机编队飞行等复杂的目标跟踪场景中，系统误差的存在会导致航迹关联混乱、融合精度降低等问题[2].导致产生这种现象的原因是不同机载平台上的传感器测量信息都是在各自的测量坐标系下获得的，当飞机编队对目标信息进行融合跟踪时，需要将这些测量信息转换到同一参考坐标系中才能进行关联滤波.由于受到机载平台自身定位误差、传感器测量误差及安装误差等诸多系统误差因素的影响，加之坐标转换又不可避免地会放大上述误差，导致融合中心在公共参考坐标系下难以将源于同一目标的测量值关联起来，严重影响目标航迹的稳定性和跟踪精度[2].因此，系统误差配准是多源信息融合的前提，为保证后继多平台多传感器信息融合的精度，有必要开展多平台系统误差配准技术研究.

随着对系统误差配准研究的不断深入，一些消除系统误差影响的方法逐渐发展起来，主要分为两类:离线估计补偿法和在线联合估计法.离线估计补偿法首先估计出系统误差，然后对目标量测进行修正，并基于修正后的量测估计目标的真实状态，如最小二乘法[3]、广义最小二乘法[4]、精确极大似然算法[5]等.此类算法都是假设系统误差是缓慢变化的，从而对某一时间段内的量测数据进行批处理，但不能对系统误差的变化做出实时地调整，缺乏实时性.在线联合估计法考虑到系统误差和目标状态的相互作用，将系统误差作为目标状态的分量同时对二者进行估计，如扩维高斯滤波算法[6]等.但此类算法需要精确已知系统误差的状态空间模型，然而在实际工程应用中，系统误差的状态空间模型可能是不准确的或者是未知的，从而恶化扩维高斯滤波器的性能，甚至导致滤波器发散.

期望最大化(expectation maximization，EM)算法是一种求解模型中不可观测的未知参数最大似然估计的迭代算法，它能保证被估参数随着迭代次数的増加而逐步收敛到一个局部极大值，在处理不完全数据中有着重要应用[7-8].Henry等人为解决多传感器信息融合中数据关联与误差配准之间相互影响的问题，提出了一种卡尔曼滤波(Kalman filter，KF)和期望最大化算法相结合的算法EM-KF算法，使其能够同时对系统状态和空间误差进行估计，有效提高了信息融合的精度[9].然而该算法仅适用于线性系统的情况，无法推广到非线性系统.刘德浩等人采用EM与扩展卡尔曼滤波(extended Kalman filter，EKF)相结合的方法，解决了量测维数不同的异类传感器系统误差配准及航迹融合问题[10].然而该方法在计算非线性函数的雅可比矩阵时对非线性函数的一阶线性化近似精度偏低，当系统具有强非线性时，算法的数值稳定性不佳，导致对系统状态及误差的估计精度严重下降，甚至引起滤波发散.Stefano等人将组网内各传感器的站址误差、测量误差及姿态误差全部考虑在内，提出了一种基于EM算法的多传感器系统误差配准方法[11].然而该算法仅适用于解决固定平台多传感器的误差配准问题.

为了解决机载多平台同质多传感器的系统误差配准问题，并主要考虑同质传感器量测系统误差对目标融合跟踪精度的影响，本文借鉴最大似然估计算法的思想，提出了一种基于期望最大化与容积卡尔曼平滑器(cubature Kalman smoother，CKS)的机载多平台同质多传感器系统误差配准算法.该算法将传感器的量测系统误差视为系统待估计的未知参数，构建了新的传感器量测方程.同时引入EM算法框架，在期望步骤(expectation step，E-step)中利用容积卡尔曼滤波器(cubature Kalman filter，CKF)和CKS近似计算完整对数似然函数的数学期望，在最大化步(maximization step，M-step)中对该数学期望进行最大化处理，最后通过解析更新反复迭代的方式获得各传感器系统误差的参数估计.最后，数值仿真验证了本文提出算法的有效性.

2 问题陈述

2.1 EM算法

考虑如下状态空间形式的离散非线性系统[12]:

其中:xk∈Rn和zk∈Rm分别表示k时刻系统状态向量和量测向量;n和m分别表示系统状态维数和量测维数;fk-1(·)和hk(·)分别表示系统状态转移函数和量测函数;wk∈Rn和vk∈Rm分别表示系统过程噪声向量和量测噪声向量，二者分别服从均值为零，方差为Qk和Rk的高斯分布，且互不相关.初始系统状态向量x0服从均值为方差为P0|0的高斯分布，且它与wk及vk互不相关.η表示未知的系统误差向量，本文的问题是如何估计出这个未知的系统误差向量η，为此本文采用EM算法来解决这个问题.EM算法是一种基于贝叶斯推断在概率模型中寻找未知参数向量的近似最大似然估计的迭代算法[13-14]，它通过最大化如下的联合对数似然函数，从而得到未知参数向量η的一个近似最大似然解[15-16]，即

根据贝叶斯定理，联合对数似然函数可以被分解如下:

对方程(6)等号两边同时取关于pηj(x0:N|z1:N)的数学期望，并代入到方程(5)中，可以得到

根据方程(7)，可以推出Lη(z1:N)和Q(η，ηj)的关系如下[15-16]:

从方程(9)可以看出，如果存在第j+1步η的一个合适近似ηj+1使得Q(ηj+1，ηj)≥Q(ηj，ηj)，那么就能保证成立.因此，EM算法能保证Lη(z1:N)在每一步的迭代过程中都是非递减的.基于EM算法的未知参数向量η估计的具体实施步骤被总结在算法1中.

算法1基于EM算法的未知参数向量估计.

步骤1选择一个未知参数的初始估计η0;

步骤2E-step:根据方程(5)计算Q(η，ηj);

步骤3M-step:最优化计算:

步骤4重复步骤2-3直到满足收敛要求.

2.2 容积卡尔曼滤波器和平滑器

在高斯近似滤波器和平滑器中涉及到求解带高斯分布的非线性状态函数和量测函数的多维积分，并且这些多维非线性积分全部集中在高斯近似滤波器中，在高斯近似平滑器中则不需计算多余的多维非线性积分.由于状态函数和量测函数的非线性一般会导致计算多维积分难以实现，因此通常情况下只能采用数值近似的方法来获得次优的高斯近似滤波器.

基于不同的数值近似方法，目前已经提出了许多次优的高斯近似滤波器，其中主要包括基于无迹变换准则的无迹卡尔曼滤波器(unscented Kalman filter，UKF)[17]，基于高斯埃尔米特求积准则的高斯埃尔米特滤波器(Gauss Hermite filter，GHF)[18]和基于三阶球径容积准则的CKF[19]等.UKF通过无迹变换产生确定性采样点，并利用这些确定性采样点经非线性变换后的结果来计算系统状态的后验均值和协方差[17].然而在高维非线性系统条件下，基于无迹变换准则所产生的确定性采样点的权值极易出现负值情况，导致其矩积分中引入很大的截断误差，数值稳定性不佳，滤波精度严重下降，甚至引起滤波发散.GHF利用高斯埃尔米特求积公式近似计算预测及预测协方差中的非线性函数多维积分，把多维积分看成是单重积分的嵌套序列，然后依次对每个变量应用单重积分公式[18].然而，GHF受到非常严重的维数灾难的困扰，函数的计算次数随着状态维数的增长呈指数增长.尽管高斯埃尔米特公式的近似精度较高，但维数灾难所引起的高昂的计算代价限制了它的应用推广.而基于三阶球径容积准则的CKF所产生的确定性采样点具有相同的权值，且权值永远为正，在一定程度上缓解了滤波过程中由于舍入误差的累计而导致的滤波发散和数值精度降低的问题.虽然近似精度比GHF略有降低，但因其计算简单，易于实现，逐渐被人们接受，并被广泛使用.因此，本文采用三阶球径容积准则来计算这些高斯加权多维非线性函数积分，具体实施过程如下:

1) 选取容积采样点ξ(i):

2) 计算含高斯权重的非线性函数积分:

其中:f(x)表示任意的非线性函数，m和P 分别表示高斯分布的均值和协方差矩阵，表示协方差矩阵P 的一个Cholesky分解因子.

3 基于EM-CKS的多传感器系统误差配准方法

3.1 E-step

E-Step利用现有未知参数的估计值来计算完整数据对数似然函数的期望:

利用方程(1)-(2)给出的状态空间模型的一阶马尔科夫性质及贝叶斯定理，可以将联合概率密度函数pη(x0:N，z1:N)分解如下:

其中:pη(xk+1|xk)表示系统状态转移概率密度函数，pη(zk+1|xk+1)表示似然概率密度函数.将方程(14)代入到方程(13)中，可以得到

从方程(15)中可以看到，计算Q(η，ηj)需要用到平滑概率密度函数和联合平滑概率密度函数为了解决这一问题，本文利用了CKS来近似计算这两个平滑概率密度函数.可被近似为[16]

根据方程(1)-(2)给出的状态空间模型，状态转移概率密度分布函数pη(xk+1|xk)和似然概率密度分布函数pη(zk+1|xk+1)可以分别表示如下:

将方程(20)代入方程(18)中，Q1(η，ηj)可以被重新表示为

将方程(21)代入方程(19)中，Q2(η，ηj)可以被重新表示为

令关于任意概率密度函数p(x)的数学期望表示为

则方程(22)和方程(24)可以整理为

3.2 M-step

本节将解决方程(10)的最大化问题.定义

其中III表示与状态同维数的单位矩阵，将式(31)带入式(29)中，则I1(η，ηj)可以重新表示如下:

将hk+1(xk+1，η)在xk+1=和η=ηj处分别进行一阶泰勒级数展开，得

其中Hk+1和分别表示了系统非线性量测函数hk+1(xk+1，η)在xk+1=和η=ηj处的雅克比矩阵，即

则方程(30)可以重新表示如下:

将式(32)(38)分别代入式(27)-(28)中，整理得

利用向量求导公式:

将方程(39)等号两边对η=ηj+1求偏导，得

将方程(40)等号两边对η=ηj+1求偏导，得

而根据方程(10)中ηj+1的定义，需要满足如下条件:

即需要满足如下方程:

因此，可以推出

其中

本文提出的基于EM-CKS的多传感器系统误差配准方法被详细的总结在算法2中.

算法2基于EM-CKS的多传感器系统误差配准方法.

步骤1选择一个未知参数的初始估计η0，迭代次数M，并设置初始迭代步数i=0;

步骤2基于当前时刻未知参数向量的估计值ηj运行CKF和CKS，最后得到经过平滑后的状态估计值

步骤3计算方程(47)中的雅克比矩阵H，并将该雅克比矩阵H和步骤2得到的状态平滑均值一同代入到方程(47)中更新ηj+1;

步骤4i+1 →i并检查迭代终止条件i ＞M.如果这个条件满足，那么迭代终止;否则迭代继续，并返回步骤2.

3.3 收敛性讨论

从方程(9)可以看出，若Q(ηj+1，ηj)≥Q(ηj，ηj)，那么

并且等号成立当且仅当满足如下条件[15]:

根据方程(48)-(50)，当Lη(z1:N)是一个关于参数向量η的凸函数时，并满足[15]

那么

非线性系统辨识的收敛性是一个非常重要但非常困难的问题，因为其包含的多维非线性积分一般无法解析求解，从而无法获得Q(η，ηj)和pη(x0:N|z1:N)的解析解.此外在实际应用中，似然函数Lη(z1:N)有可能是一个多峰函数，存在多个局部解，而EM算法无法避开这个问题.所以通常情况下是无法分析EM算法的收敛性或给出EM算法的收敛条件的.为此，我们将定性地讨论参数选择对所提出算法收敛性的影响，并说明所提出算法的适用领域和适用条件.

当似然函数Lη(z1:N)是一个多峰函数时，初始参数估计η0对所提出算法的收敛性有显著的影响.如果η0与最优解很接近，那么随着迭代的进行，最终有方程(53)成立.如果η0与最优解相差很大，那么随着迭代的进行，ηj可能会偏离最优解或者陷入局部解.因此，通过选取与最优解相近的初值可以改善提出算法的收敛性.在很多实际应用中，用户对模型中的未知参数的大致取值范围都有一定的了解，这种信息可以帮助用户选择合适的初值，以更好地应用本文所提出的算法.

在本文所提出的算法中，状态的滤波概率密度和平滑概率密度都被近似为高斯分布，并且它们的高斯近似精度会随着噪声方差的增大而降低，这是因为系统的非线性度会随着噪声方差Qk和Rk的增大而增大.状态的滤波概率密度和平滑概率密度的高斯近似误差会增大参数η的估计误差.而参数η的估计误差反过来又会增大状态的滤波概率密度和平滑概率密度的高斯近似误差，从而使得参数η的估计误差随着迭代的进行而积累，并且噪声方差越大这种积累误差也越大.因此，本文所提出算法对系统误差的辨识精度会随着噪声方差增大而降低.此外，本文所提出算法只有在具有较低或者适中噪声方差的非线性系统中才会收敛，而对于具有大噪声方差的非线性系统，提出的算法可能会出现发散的情况，这是因为大的噪声方差可能会导致巨大的累积误差.

3.4 计算复杂度分析

算法的计算复杂度是工程应用中必须要考虑的一个问题，因此，本节将对所提出算法的计算复杂度进行分析.分析算法计算复杂度最为有效的方法是对其浮点操作数进行准确统计.一次浮点操作数定义为两个浮点数进行一次加、减、乘或除法运算，下面给出几种常用的基本代数运算所需的浮点运算数[20].

1) 矩阵加减.

若A∈Rn×m，B∈Rn×m，计算A±B所需的浮点运算数为nm次;

2) 矩阵相乘.

若A∈Rn×m，B∈Rm×l，计算AB所需的浮点运算数为2mnl-nl次;

3) 矩阵求逆.

若A∈Rn×n，计算A-1所需的浮点运算数为n3次;

4) Cholesky分解.

若A∈Rn×n，计算A的Cholesky因子所需的浮点运算数为次;

由第3.2节的算法2中可以看出，本文所提出的算法实际上是一个不断迭代更新的过程，且每次迭代都需要依次经过CKF、CKS、雅克比矩阵求解及参数更新这几个步骤.下面，本文结合前面给出的基本代数运算，并参照文献[20]首先给出CKF和CKS所需的浮点运算数，整理如表1所示.

结合表1所给出的相关参数的运算复杂度，单次递推CKF算法的计算复杂度可以表示如下[20]:

单次递推CKS算法的计算复杂度可以表示如下:

由式(54)-(55)可以看出，CKF和CKS的计算复杂度均为O(n3)和O(m3)的量级，其中O(·)表示浮点运算的阶数，即与状态维数和量测维数均成三次幂关系.最后根据方程(36)(46)-(47)，本文所提出算法的浮点运算总数可以表示如下:

其中:M表示迭代次数，Mh表示计算量测函数需要的浮点运算数.由式(56)可以很直观的得出，本文所提出算法的计算复杂度主要取决于迭代次数、状态维数及量测维数.因此，在实际工程应用中，迭代次数的选取需要权衡工程计算量和所需精度.

表1 单次递推过程计算量Table 1 Calculation amount of single recursive process

4 仿真

本节将通过模拟双机共同跟踪同一非合作目标的仿真试验来验证本文所提出的机载多平台多传感器系统误差配准算法的有效性和性能.具体仿真场景设置为由两架舰载直升机平台共同探测跟踪一架战斗机目标，其中每架舰载直升机各携载一部三维传感器.战斗机飞行速度较快，飞行海拔高度为2 km，舰载直升机飞行速度较慢，飞行海拔高度为1 km，所有飞行器均沿经线飞行，三维传感器扫描时间间隔为1 s，大地坐标系下舰载直升机平台与战斗机目标真实运动信息的具体仿真场景如图1所示.

各传感器分别测量得到目标的距离、方位和俯仰角三维信息，且它们的测量噪声均为零均值高斯白噪声，对应的各传感器量测噪声协方差矩阵可以表示为

各传感器系统误差均设置为η=[1000 m，0.5°，0.5°]T.

图1 仿真环境Fig.1 Diagrams of simulation environment

为了实施本文所提出的算法，在接下来的双机目标跟踪仿真试验中，本文将使用CKF和CKS来近似计算滤波概率密度和平滑概率密度.此外，为了对比说明本文所提出的算法相比于现有的解决多传感器系统误差配准问题的算法所具有的优越性，本文在仿真中重点比较文献[10]所提出的基于EM-EKF的系统误差配准方法和本文所提出的基于EM-CKS的系统误差配准方法.为了公平比较，所有滤波器都设置为相同的初始条件，即都从以真实参数为中心的对称区间中随机选取未知参数的初值η0.且都执行50次独立的蒙特卡洛仿真试验，每次蒙特卡洛仿真试验的迭代次数设置为100次，使用系统误差在每次迭代的均方根误差(root mean square error，RMSE)作为性能评价指标

首先来看本文提出算法的有效性.图2-4给出了各传感器系统误差的估计结果.此外，为了进一步展示本文所提出算法的精度，表2中给出了各传感器系统误差的最终估计.从图2-4中，可以看到本文所提出的算法在每一次蒙特卡洛运行中都是收敛的，并且这种收敛性独立于初始的传感器系统误差参数值和系统量测输出.从图2-4中，也可以看到所有传感器系统误差的参数估计经20步迭代后都收敛了，因而本文所提出的算法具有较快的收敛速度.从表2中还可以看到本文所提出的算法具有良好的估计精度.以上这些仿真结果说明了本文所提出算法可以用于解决机载多平台多传感器系统误差的非线性系统辨识问题.

图2 各传感器径向距离系统误差估计结果Fig.2 Estimated results of range system errors

图3 各传感器方位角系统误差估计结果Fig.3 Estimated results of azimuth system errors

图4 各传感器俯仰角系统误差估计结果Fig.4 Estimated results of elevation system errors

表2 真实的参数值和估计的参数值(均值±标准差)Table 2 True parameter values and estimated parameter values(mean values±standard deviations)

然后再来看本文提出算法与现有方法的仿真对比.图5-6分别展示了提出方法和现有方法对传感器1的方位角系统误差RMSE和传感器2的距离系统误差估计结果的RMSE，表3给出了两种方法对传感器1方位角系统误差和传感器2距离系统误差估计RMSE均值的统计结果以及单次蒙特卡洛仿真的运行时间.

图5 方位角系统误差RMSE估计结果Fig.5 Estimated results of azimuth system errors RMSE

图6 距离系统误差RMSE估计结果Fig.6 Estimated results of distance system errors RMSE

从图5-6及表3中可以看出，本文提出的基于EMCKS的系统误差配准方法比现有的基于EM-EKF的系统误差配准方法具有更小的RMSE，但EM-CKS的运行时间要慢于EM-EKF.仿真结果分析如下:本文提出的基于EM-CKS的系统误差配准方法采用的是3阶球径容积准则的确定性采样方法来近似计算含高斯分布的多维非线性函数积分，理论上，它能至少以二阶泰勒精度逼近任何非线性系统状态的后验均值和协方差.而EM-EKF采用的是一阶泰勒展开线性化的方法，在计算雅克比矩阵时存在截断误差，无法保证较高的估计精度.在计算量方面，由于EM-CKS在每一次迭代过程都需要产生一定数量的采样点，并经过非线性函数传播这些采样点，这一计算过程会随着系统状态及量测维数的增加而逐渐变慢，因此，EMCKS的单步运行时间会多于EM-EKF.虽然本文提出方法在精度上得到了保证，但其却以牺牲计算量为代价.在实际工程应用中，往往需要权衡工程计算量和所需精度，因此，本文所提出方法适用于对计算量要求不高但对估计精度要求较高的场合.