基于改进高斯混合模型的机器人运动状态估计

状态估计理论被广泛应用于各领域, 例如太空监测[1]、无线通信、机器人运动跟踪以及金融行业等[2]. 当系统为线性、噪声统计特性服从高斯分布并完全已知时, 卡尔曼滤波器是最优的解决方案[3-4]. 在大多数实际工程应用中, 系统往往存在非线性、非高斯等复杂特征, 此时若采用经典卡尔曼滤波方法来对实际系统进行状态估计, 将出现估计精度下降和收敛性变差等情形, 甚至会出滤波发散现象, 这严重制约了经典卡尔曼滤波理论在实际工程中的应用. 因此, 深入开展非线性非高斯系统下的卡尔曼滤波方法研究具有重要的意义.

近几十年来, 研究人员已提出诸多面向非线性系统的卡尔曼滤波方法, 如扩展卡尔曼滤波、无迹卡尔曼滤波、容积卡尔曼滤波(Cubature Kalman filter, CKF)[5], 以及一些相应的改进算法. 其中, 扩展卡尔曼滤波因结构简单在早期大受欢迎, 但该方法只能处理弱非线性问题, 当系统非线性增强时其估计性能将急剧下降; 与扩展卡尔曼滤波相比, 无迹卡尔曼滤波引入了采样近似技术, 估计精度和稳定性有了很大提升; CKF则是在无迹卡尔曼滤波基础上提出的一种改进型卡尔曼滤波方法, 基于三阶球面径向容积准则来进行采样近似, 是理论上当前最接近贝叶斯滤波的基础性非线性滤波算法. 现有研究表明, CKF的数值稳定性及滤波精度优于传统的扩展卡尔曼滤波和无迹卡尔曼滤波, 尤其是在高维非线性系统状态估计中具有更为明显的算法优势[6-7].

此外, 针对非高斯系统的状态估计问题研究也取得一些进展, 目前已建立了基于高斯和、粒子滤波和极大相关熵等技术在内多种非高斯滤波方法[8]. 文献[9]基于最大相关熵准则提出一种适用于非高斯噪声的卡尔曼滤波算法, 但是其高斯核大小对结果影响较大. 为了克服这一缺陷, 研究者们对极大相关熵准则中的核函数进行一系列的改进[10], 如对高斯核大小进行自适应处理, 但都难以从根本解决对核敏感的问题. 高斯和技术是一种比较直观和受欢迎的非高斯处理方法之一, 其基本原理是采用多个有限高斯项概率密度去逼近非高斯概率密度, 即利用多个高斯分布的和去近似表示一个非高斯分布, 从而可以基于传统的卡尔曼滤波框架实现滤波[11]. 因此, 通过将高斯和技术和CKF结合, 可进一步构建相应的滤波算法来解决非线性非高斯系统的状态估计问题[3, 12]. 事实上, 高斯和滤波器设计的关键问题主要包括高斯混合模型的参数估计和高斯项优化合并[13-14], 其中高斯混合模型参数估计主要采用期望最大化(Expectation-maximum, EM)算法, 该算法是由Dempster等[15]于1977年提出的一种迭代算法, 其通过估计参数的极大似然值来分析不完全数据集, 从而达到获取最佳聚类结果的目的. 然而, EM算法存在对初始值比较敏感、收敛速度慢、容易陷入局部收敛以及需要知道混合成分个数等缺陷, 在工程应用受到极大的限制[16-17], 依然有待进一步深入改进. 此外, 在高斯和滤波过程中, 高斯项的个数会呈指数型递增, 这使得对高斯项进行合理有效地合并就显得非常必要. 目前典型用于高斯项合并的距离度量包括马氏(Mahalanobis)距离和Kullback-Leibler (KL)距离[18-19], 其中马氏距离度量倾向于采用剪枝方式来删除权重较低的高斯项, 而KL距离度量更倾向于合并而不是剪枝方式[19]. 由于这两种高斯混合项处理方式机理不同, 所产生的高斯项约简效果也有差异, 一般很难从理论上验证孰优孰劣.

针对上述问题, 本文以自由度为3 (包括x、y与朝向) 的地面移动机器人的运动状态估计系统为对象, 提出一类改进的非线性非高斯CKF的设计. 该工作主要创新包括建立一种改进的鲁棒EM算法, 并提出了基于信息融合技术的高斯项合并方法. 在鲁棒EM算法改进方面, 本文将引入加权信息量概念来进一步完善EM算法中的目标函数惩罚项, 使得在优化过程中能考虑包括隐含信息量在内的更全面的参数信息. 通过这种方式, 得到的权重更新参数将更具有代表性, 从而减少EM算法的迭代次数并提高其收敛速度. 在高斯项合并方法方面, 综合考虑上述两类高斯项合并方法原理和性能上的差异性和互补性, 基于信息融合技术提出一种新的高斯合并项融合方法: 先单独对马氏距离和KL距离度量进行高斯混合项合并, 以改善高斯项合并距离度量的合理性; 再以两个距离度量独立运行输出的高斯合并项为基础, 提出对两类高斯合并项进行加权融合的高斯混合项融合方法. 基于传统高斯和容积卡尔曼滤波的设计框架, 本文提出新型非线性非高斯滤波方法, 并通过理论分析和机器人转弯运动状态估计仿真场景来验证新算法的有效性.

1.   问题描述

1.1   系统描述

考虑如下一类非线性非高斯估计系统:

式中, xk∈Rn${\mathit{x}}_k\in {\mathbf{R}}^n$为系统的状态变量, zk∈Rm${\mathit{z}}_k\in {\mathbf{R}}^m$为系统的量测向量, f(⋅)$\mathit{f}(\cdot )$和h(⋅)$\mathit{h}(\cdot )$分别为已知的状态转移函数和量测函数. wk−1∈Rn${\mathit{w}}_{k-1}\in {\mathbf{R}}^n$为非高斯过程噪声, vk∈Rm${\mathit{v}}_k\in {\mathbf{R}}^m$为非高斯量测噪声. 实际上, 非高斯噪声如脉冲噪声、闪烁噪声等广泛存在于工程应用中, 会严重破坏以二阶统计理论为基础的高斯滤波算法的设计过程[20], 因此为了提高滤波器工程应用能力, 必须深入研究非高斯滤波方法的设计.

1.2   高斯和容积卡尔曼滤波

根据高斯和定理, 任意分布的概率密度函数都能够用N$N$个高斯项的累加进行近似表示, 即利用多个不同权重的高斯项求和去描述非高斯噪声[11]. 过程和量测噪声可用高斯混合模型表示为:

式中, p(wk−1)$p\left({\mathit{w}}_{k-1}\right)$、p(vk)$p\left({\mathit{v}}_k\right)$分别表示过程噪声和量测噪声的概率密度; αjw,k${\alpha }_{w,k}^j$、w¯jk−1${\overline{\mathit{w}}}_{k-1}^j$和Qjk−1${\mathit{Q}}_{k-1}^j$分别表示第j$j$个过程噪声高斯分量的的权重、均值和方差. αlv,k${\alpha }_{v,k}^l$、v¯lk${\overline{\mathit{v}}}_k^l$和Rlk${\mathit{R}}_k^l$分别表示第l$l$个量测噪声高斯分量的权重、均值和方差, 且∑Jj=1αjw,k=1${\sum }_{j=1}^J{\alpha }_{w,k}^j=1$, ∑Ll=1αlv,k=1${\sum }_{l=1}^L{\alpha }_{v,k}^l=1$.

高斯和容积卡尔曼滤波( Gaussian-sum cubature Kalman filter, GSCKF)的基本框架如图1所示. 主要步骤包括: 近似高斯分布集合估计、并行容积卡尔曼滤波、高斯项合并和信息融合等. 其中, 高斯分布集合估计是确定几个高斯分布的和来近似表示原非高斯分布, 并估计出各个高斯分布的特征参数, 从而可以建立高斯混合模型; 并行容积卡尔曼滤波是以多高斯分布估计为基础, 分别执行单高斯分布下的标准CKF来获得相应的状态估计; 随着估计时间增加, 高斯项个数将呈几何级增长, 因此进行高斯项即多个高斯分布的滤波估计结果合并约简就显得非常必要; 当完成高斯项合并约简后, 将剩余的高斯分布滤波估计进行融合获得最终的高斯和滤波估计.

图 1  高斯和容积卡尔曼滤波算法流程

Fig. 1  GSCKF algorithm process

下载: 全尺寸图片 幻灯片

现有的高斯和容积卡尔曼滤波方法研究重点集中在算法设计上, 主要包括如下2个核心问题: 1)对于非高斯噪声特性的参数估计研究. 虽然通过高斯和原理可将非高斯噪声表示为多个高斯分布的混合模型, 但应用传统的EM算法及改进方法对高斯混合模型的参数进行估计时, 往往需要提前确定混合成分个数, 并且初始参数值对算法性能的影响较大, 因此提高EM算法的鲁棒性对于改善高斯和滤波的性能非常重要; 2)对高斯混合项的合并模型降阶方法的研究. 高斯和滤波过程将产生大量的高斯项, 这是非高斯模型高斯化的必然结果. 但过多的高斯项必然会对整个滤波过程和结果性能产生不利影响, 尤其是高斯项合并结果将直接影响非高斯状态估计的性能. 现有研究大都采用单一距离的合并机制, 如基于马氏距离的高斯项合并, 该方法主要倾向于合并均值相近的高斯项. 事实上, 高斯化后产生的众多高斯项类型和特征各有不同, 单一的合并基准或策略往往难以尽可能有效地衡量出所有需要合并或剪枝的高斯项, 同时又需要考虑最大高斯分量数的约束, 使得寻找一个合适的动态阈值来降低算法复杂度的同时又能保证算法精度是一个挑战性的问题. 因此, 如何设计更加高效的高斯项合并策略成为改善高斯和滤波性能的一个重要方向.

1.3   研究动机

由于准确确定高斯混合模型中混合成分的具体类簇和数量是非常困难的, 从而导致传统EM算法的应用和性能受到很大的限制. 因此, 针对第1.1节的问题1), 本文借鉴文献[21]引进信息量概念的思路, 通过对传统EM算法目标函数添加惩罚项, 优化EM算法中权重参数的更新规则, 得到一种中引进信息量概念来改进EM算法输出性能的思想, 通过进一步增加传统EM算法目标函数惩罚项(即加权信息量)的方式来实现EM算法中权重参数更新规则的深度优化, 从而获得一种改进的鲁棒EM算法. 该改进算法将混合成分个数的初值作为采样点数, 从而能有效改善对初始参数值敏感的问题, 随后应用各个类簇竞争性的关系和迭代过程中删除权重低的项来获得混合成分的具体参数, 从而EM算法的鲁棒性能够得到有效改善.

针对问题2), 考虑到基于单一距离指标的高斯项合并方式存在适用性和鲁棒性等受限问题, 加之高斯化得到的高斯混合项具有各自的特性以及每种距离指标拥有天然不同的明确倾向性, 使得采用信息融合技术来实现多种距离指标下高斯项合并方法的融合成为可能, 因此提出基于多距离指标高斯项的二次融合模式来设计一种改进的高斯混合项合并方法. 理论上使用更多的距离指标更够更加符合和逼近高斯混合项的复杂特征, 但为了研究阐述的简便性和直观性, 本文只讨论基于马氏距离和基于KL距离的高斯混合项融合合并方法, 即将基于上述两种距离方法的合并结果进行凸组合加权融合[22]来获得最终的非高斯状态估计. 需要提及的是, 一般意义上更多距离指标的融合参与过程可类推获得, 但鉴于计算复杂性和实时性的要求, 过多的距离指标使用也会产生一些新的问题.

2.   改进鲁棒EM算法

EM算法是一种针对不完全数据进行参数估计值求解的迭代算法, 其在观测数据的基础上引进不可观测的潜在数据, 从而将复杂的不完全数据问题转化为完全数据问题[23], 并通过迭代算法完成不完全数据的参数的估计.

2.1   EM算法

假定存在一组观测数据X′=(x1,x2,⋯,xn)${\mathit{X}}^{\mathbf{\prime }}=({\mathit{x}}_1,{\mathit{x}}_2,\cdots ,{\mathit{x}}_n)$, 其中xi${\mathit{x}}_i$为p$p$维向量, 由高斯混合模型生成

式中, αk${\alpha }_k$、μk${\mathit{\mu }}_k$、Σk${\mathbf{\Sigma }}_k$分别表示高斯混合模型中第k$k$个高斯项的权重、均值和协方差, c$c$表示混合模型分量的个数即最终类簇.

经典EM算法的流程如图2所示, 其中zki${\mathit{z}}_{ki}$为隐含参量, θ0={α01,⋯,α0c,μ01,⋯,μ0c,Σ01,⋯,Σ0c},${\theta }^0=\{{\alpha }_1^0,\cdots ,{\alpha }_c^0,{\mathit{\mu }}_1^0,\cdots {,\mathit{\mu }}_c^0,{\mathbf{\Sigma }}_1^0,\cdots ,{\mathbf{\Sigma }}_c^0\},$ 混合模型分量数目c$c$, 在EM算法中需要提前给出.

图 2  EM算法迭代流程

Fig. 2  EM algorithm process

下载: 全尺寸图片 幻灯片

E-step: 给定初始参数θ0${\theta }^0$, 计算隐含参量zki${\mathit{z}}_{ki}$[21]:

M-step: 计算新一轮迭代模型参数:

1)混合高斯项的权重为:

2)高斯混合模型分量的均值为:

3)高斯混合模型分量的协方差为:

对于上述传统EM算法, 一般需要提前给定混合成分的具体个数, 且对于初始参数比较敏感, 容易陷入局部最优, 这些都限制了EM算法应用.

2.2   一种改进鲁棒EM算法

Yang等[21]提出一种鲁棒EM算法, 通过对传统EM算法目标函数添加惩罚项, 以优化EM算法当中高斯混合项的权重更新方式, 进而改变混合模型的具体参数更新规则. 但添加的惩罚项仅考虑优化权重参数αk${\alpha }_k$, 因此只包含混合高斯项的权重信息. 本文引入信息量的概念来进行惩罚项的添加, 除考虑权重参数外, 进一步考虑了数据中的隐含信息量. 其中, 隐含参量zki${\mathit{z}}_{ki}$代表 xi${\mathit{x}}_i$属于第k$k$个高斯分量的概率, 其包含了混合高斯项的权重、均值和协方差. 通过在EM算法的基础上引进加权信息量的思想, 样本总体加权隐含信息量可表示为−∑ni=1∑ck=1αklnzki$-{\sum }_{i=1}^n{\sum }_{k=1}^c{\alpha }_k\ln {\mathit{z}}_{ki}$, 其中−∑ck=1lnzki$-{\sum }_{k=1}^c\ln {\mathit{z}}_{ki}$代表第i$i$个样本的含信息量, αk${\alpha }_k$代表每个高斯项的权重. 若要使总体的加权信息量最小, 则意味着最大化∑ni=1∑ck=1αklnzki${\sum }_{i=1}^n{\sum }_{k=1}^c{\alpha }_k\ln {\mathit{z}}_{ki}$. 将其作为惩罚项加入到目标函数中可以得到:

其中

进一步考虑约束条件∑ck=1zki=1${\sum }_{k=1}^c{\mathit{z}}_{ki}=1$ 和∑ck=1αk=1${\sum }_{k=1}^c{\alpha }_k=1$下, 通过拉格朗日乘数法最大化J$J$可以求解得到各个高斯项的权重更新公式αk${\alpha }_k$为(详见附录A):

每次迭代时, 若αk<1/n${\alpha }_k<1/n$或αk<0${\alpha }_k<0$, 则认为该高斯项对混合成分的贡献忽略不计, 并移除此高斯项[21], 然后重新调整高斯混合项数目c$c$, 具体为:

同时, 为了保证约束条件∑ck=1αk=1${\sum }_{k=1}^c{\alpha }_k=1$, 需进行以下更新:

流程如图3所示, 本文改进鲁棒EM算法具体步骤如下:

图 3  改进鲁棒EM算法迭代流程

Fig. 3  Improved robust EM algorithm process

下载: 全尺寸图片 幻灯片

步骤 1. 初始化参数c=n$c=n$, αk=1/n${\alpha }_k=1/n$, β=1$\beta =1$. μk${\mathit{\mu }}_k$, Σk${\mathbf{\Sigma }}_k$取值任意, 即θ$\theta$值, 并设定一个足够小的数ε>0$\epsilon >0$作为判断循环结束的条件;

步骤 2. E-step: 根据式(6)计算zki${\mathit{z}}_{ki}$;

步骤 3. M-step: 由式(12)更新权重, 由式(8)计算μk${\mathit{\mu }}_k$, 由式(11)计算β$\beta$, 第1次迭代令β=1$\beta =1$;

步骤 4. 当αk<1/n${\alpha }_k<1/n$时, 可令第k$k$个高斯分量无效, 移除此时的混合系数αk${\alpha }_k$, 同时高斯分量参数c(new)=c(old)−|αk|$c^{(new)}=c^{(old)}-\left|{\alpha }_k\right|$, 当迭代次数大于60次时令β=0$\beta =0$;

步骤 5. 根据式(14)和式(15)更新得到αk′${\alpha }_{k^{\mathrm{\prime }}}$和 zk′i${\mathit{z}}_{k^{\mathrm{\prime }}i}$, 并根据式(8)和式(9)来更新μ′k${\mathit{\mu }}_k^{\mathrm{\prime }}$和Σk${\mathbf{\Sigma }}_k$;

步骤 6. 根据式(6)更新zki${\mathit{z}}_{ki}$;

步骤 7. 若∥∥μ′k−μk∥∥<ε$\Vert {\mathit{\mu }}_k^{\mathrm{\prime }}-{\mathit{\mu }}_k^{}\Vert <\epsilon$则迭代停止, 否则转到步骤3继续执行.

本节提出改进鲁棒EM算法, 对初始的参数值不敏感, 迭代过程中各个高斯项之间是竞争关系, 每次迭代删除权重较小的高斯项, 通过不断迭代可得到混合成分具体参数估计.

3.   改进高斯和容积卡尔曼滤波

3.1   高斯和容积卡尔曼滤波

本文将给出非高斯噪声环境下的高斯和滤波算法(Improved GSCKF, IGSCKF), 算法流程如图4所示. 在非高斯噪声环境下, 用改进鲁棒EM算法进行高斯混合模型的求解, 通过高斯和原理, 使用多个并行的CKF进行状态预测与更新, 求解得到目标的状态估计值. 然而, 随着时间推移, 高斯项的个数会越来越多从而限制算法的使用. 为了有效地对高斯项进行合并, 本文将利用现有两类方法来进行高斯项的合并, 并将合并后的高斯混合项进行凸组合融合. 结合图4具体阐述改进后的高斯和容积卡尔曼滤波算法的滤波过程如下.

图 4  改进高斯和容积卡尔曼滤波

Fig. 4  Improved Gaussian-sum cubature Kalman filter

下载: 全尺寸图片 幻灯片

1)时间更新:

假设已知k−1$k-1$时刻xk−1|k−1${\mathit{x}}_{k-1|k-1}$统计特性为:

根据贝叶斯滤波递推公式可得k$k$时刻xk${\mathit{x}}_k$的一步预测概率密度估计p(xk|zk−1)$p\left({\mathit{x}}_k|{\mathit{z}}_{k-1}\right)$为[12]:

式中, x^i,jk|k−1${\hat{\mathit{x}}}_{k|k-1}^{i,j}$和Pi,jk|k−1${\mathit{P}}_{k|k-1}^{i,j}$可以表示为:

和

式中, 各中间变量为:

式中, βr${\beta }^r$为容积点权重, nx$n_x$为状态向量维数, χr${\mathit{\chi }}^r$为球面径向规则确定的容积点.

2)量测更新: 当接收到k$k$时刻量测值时, 可以得到后验概率密度p(xk|z1:k)$p\left({\mathit{x}}_k|{\mathit{z}}_{1:k}\right)$为[12]: