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

2022-08-30葛泉波王贺彬杨秦敏张兴国刘华平

自动化学报 2022年8期

葛泉波王贺彬杨秦敏张兴国刘华平

状态估计理论被广泛应用于各领域,例如太空监测[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为系统的状态变量,zk∈Rm为系统的量测向量,f(·)和h(·)分别为已知的状态转移函数和量测函数.wk-1∈Rn为非高斯过程噪声,vk∈Rm为非高斯量测噪声.实际上,非高斯噪声如脉冲噪声、闪烁噪声等广泛存在于工程应用中,会严重破坏以二阶统计理论为基础的高斯滤波算法的设计过程[20],因此为了提高滤波器工程应用能力,必须深入研究非高斯滤波方法的设计.

1.2 高斯和容积卡尔曼滤波

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

高斯和容积卡尔曼滤波(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),其中xi为p维向量,由高斯混合模型生成

式中,αk、μk、Σk分别表示高斯混合模型中第k个高斯项的权重、均值和协方差,c表示混合模型分量的个数即最终类簇.

经典EM 算法的流程如图2 所示,其中zki为隐含参量,θ0=,混合模型分量数目c,在EM 算法中需要提前给出.

图2 EM 算法迭代流程Fig.2 EM algorithm process

E-step:给定初始参数θ0,计算隐含参量zki[21]:

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

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

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

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

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

2.2 一种改进鲁棒EM 算法

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

其中

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

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

图3 改进鲁棒EM 算法迭代流程Fig.3 Improved robust EM algorithm process

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

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

3.1 高斯和容积卡尔曼滤波

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

图4 改进高斯和容积卡尔曼滤波Fig.4 Improved Gaussian-sum cubature Kalman filter

1)时间更新:

假设已知k－1 时刻xk-1|k-1统计特性为:

根据贝叶斯滤波递推公式可得k时刻xk的一步预测概率密度估计p(xk|zk-1)为[12]:

式中,各中间变量为:

式中,βr为容积点权重,nx为状态向量维数,χr为球面径向规则确定的容积点.

2)量测更新:当接收到k时刻量测值时,可以得到后验概率密度p(xk|z1:k)为[12]:

式中,各中间变量为:

量测更新后每一项的权重为:

xk|k和Pk|k可表示为:

和

3.2 基于凸组合技术的高斯混合项融合方法

高斯和滤波算法中初始高斯项有I项,经过1次滤波操作后变为I×J×L项,随着时间的推移,k次滤波结束后将增加到I×Jk×Lk项.高斯项数的增加会导致计算量不断增加,从而限制该算法的使用.因此,需要对式(26)和式(27)的高斯混合项进行合并,以减少高斯混合项的个数,降低计算的复杂度.就目前而言,已有的高斯混合项合并方法大多基于马氏距离或者KL 距离,由于机理不同,其产生的效果也不一样,难以从理论比较两种算法的优劣性.本文将对这两种方法合并后的高斯项进行加权融合估计.

3.2.1 基于马氏距离的高斯项合并

Salmond[18]提出一种高斯项合并方法,首先定义一个马氏距离为:

设定最大高斯项数为G,一旦高斯项超过该数值则进行合并.此时,首先删除权重较低的高斯项,然后根据马氏距离依次将距离最小的高斯项进行合并,高斯项合并的计算可表示为:

3.2.2 基于KL 距离的高斯项合并

B(·)准则[19]是一种基于KL 距离的高斯项合并方法,该算法的核心是计算两个高斯项的KL 距离,若KL 距离越小则两个高斯项越相近,此时更趋向于去合并他们.B(·)准则的计算方法定义如下:

依据B(·)准则,依次将最小B(m,n)的两个高斯项进行合并,原则如下:

3.2.3 基于凸组合的高斯合并项融合

第3.2.1 节和第3.2.2 节分别介绍了2 种基于不同准则的高斯项合并方法,但是其均存在高斯项合并失真的问题.因此,为了保证高斯项合并的保真度,本文将对上述两种方法得到的高斯合并项进行凸组合融合[22].融合过程可以表示为:

其中

进而,可以得到每一项的后验概率密度:

每一项的权重为:

对权重归一化后得到:

最终融合得到xk|k和Pk|k如下:

定理1.融合后的子滤波器精度高于基于不同距离高斯项合并方法中的子滤波器的估计精度[24]:

证明.由式 (35)可得:

同理可得:

从而可知式(41)成立. □

3.2.4 简要分析

针对非高斯系统的状态估计问题,通过高斯项混合成分特性参数估计和高斯项合并方式两个角度的改进来开展新型高斯和容积卡尔曼滤波器设计,主要工作包括:1)算法的权重更新方式得到进一步优化,进而可获得改进后的混合模型参数更新规则,最终实现非线性高斯和滤波估计性能的提高.此外,该方法不需要提前设定混合成分数目,能够自适应估计混合成分的具体参数;2)针对单一距离指标的倾向性和基于单一距离指标的高斯项合并方法的局限性,提出基于多模式融合策略的高斯项合并思想,并以马氏距离与KL 距离为代表给出了两种高斯项合并方式下的多模式融合计算过程.需要注意的是,不同距离指标下高斯项融合合并实现包括2 种方式,本文采用第2)种方式:1)两者距离指标计算直接融合形成新距离指标后再进行高斯项合并操作;2)先采用各自距离指标进行高斯项合并后再进行融合操作.同时,从信息论角度采用更多的距离指标能够实现对高斯混合模型特征更好的表征,相应的融合方法也可以从两个距离指标融合模式中类推获得.然而,距离指标类别数增加必然会产生一些新的问题,如类别增加对高斯混合模型特征表达能力改善的量化表示、在状态估计正方向改善目标下的类别增加优化选择以及多距离指标融合下高斯项合并过程的收敛性与稳定性分析等,这些问题的进一步研究有助于推动更多距离指标参与下高性能高斯和容积卡尔曼滤波算法的设计.

4 仿真结果与分析

本文以机器人运动状态估计为仿真场景[5],考虑机器人在进行半圆周运动.定义其运动状态矢量xk=[xk,yk,ψk]T,其中xk和yk为机器人在x,y轴方向的位置,ψk为其方位角,机器人的运动方程表示为:

观测模型可以表示为:

机器人的运动模型如下:

式中,控制量u为:

式中,Δt为采样周期,分别为第l个地标的横坐标与纵坐标.xk和zk分别为系统状态向量和量测向量,wk-1和vk分别表示非高斯过程噪声和非高斯量测噪声.系统初始状态,过程噪声,量测噪声,均由高斯混合模型表示如下:

式中,各权重参数设定为:αx=0.5,αw=0.3,αv=0.4,其余参数设置为:

4.1 仿真实验1

为探究改进鲁棒EM 算法对非高斯噪声参数的估计效果,利用第3.2 节中改进鲁棒EM 算法对非高斯系统噪声和量测噪声各采样n=1 000 个点,然后进行高斯混合模型参数的求解.以非高斯过程噪声p(wk)为例,设置改进鲁棒EM 初始参数为:

式中,Wi为噪声采样数据,为采样数据均值.

以过程噪声为例,本文的改进鲁棒EM 算法求解高斯混合模型参数的迭代过程如图5 所示,修改为其中空心圆为噪声的采样点,实心点为用改进鲁棒 EM 算法估计得到高斯项的均值(用MuE 表示),实线椭圆为其协方差(用SigmaE 表示),三角形与虚线椭圆代表真实的高斯混合模型的均值(用MuR表示)与方差(用SigmaR 表示).图6 为文献[21]的鲁棒 EM 算法仿真,其中三角形为用鲁棒 EM算法估计得到高斯项的均值(用MuE 表示),实线椭圆为其协方差(用SigmaE 表示).

表1 为本文利用估计分布与原始分布的马氏距离衡量改进鲁棒EM 算法前后的性能对比,马氏距离越小表明估计分布越接近于原始分布,其估计精度越高.从图5、图6 和表1 可以看出,改进的鲁棒EM算法能有效地解决了EM 算法鲁棒性差的缺点,相对于鲁棒EM 算法其参数性能、收敛速度都表现优越.以马氏距离作为衡量标准改进后的鲁棒EM 算法性能相比改进前提升83.56%.

图5 改进鲁棒EM 算法迭代过程Fig.5 Improved robust EM algorithm iterative process

图6 鲁棒EM 算法迭代过程Fig.6 Robust EM algorithm iterative process

表1 改进前后鲁棒EM 算法对比Table 1 Comparison of robust EM algorithms before and after improvement

同时,由迭代结果可以看出,改进鲁棒EM 算法可以解决高斯混合模型的参数求解问题,获得了较好的高斯混合模型参数,且收敛速度较快.同理,量测噪声的求解与过程噪声相同,二者用改进鲁棒EM 算法迭代出来的结果为:

4.2 仿真实验2

仿真实验2 是为了比较基于马氏距离的Salmond 方法[18]、基于KL 距离的B(·)准则方法和本文建立的凸组合融合估计方法(GSCKF 和IGSCKF)在跟踪精度上的优劣关系.在仿真场景中,采用仿真一中改进鲁棒EM 算法估计所得到的非高斯噪声的高斯混合模型参数,作为高斯和容积卡尔曼滤波状态估计的非高斯噪声参数,分别对其进行蒙特卡洛仿真,以各算法的均方根误差(Root mean square error,RMSE)作为目标跟踪精度的指标.

对机器人进行运动状态估计的仿真结果如图7所示,其对应的机器人位置的均方根误差计算结果如图8 所示,方位角度误差如图9 所示.

图7 3 种算法对于机器人状态估计Fig.7 Three algorithms for robot state estimation

图8 3 种算法的RMSEFig.8 RMSE of three algorithms

图9 3 种算法的方位角误差Fig.9 The azimuth error of three algorithms

由图7～9 可以看出,基于凸组合融合的高斯混合项合并方法的滤波精度要高于常规的Salmond 方法,以及B(·)准则的高斯混合项合并方法.由表2 可以看出,相比于其余两种方法,本文基于凸组合融合的滤波方法的计算复杂度并没有明显增加.仿真结果表明:1)改进的鲁棒EM 算法可以很好地解决高斯混合模型的参数估计问题;2)采用凸组合来融合两种高斯混合项的合并方法,有效提升了滤波算法的精度与可靠性.

表2 3 种算法RMSE 及运行时间Table 2 RMSE and running time of 3 algorithms

5 结束语

针对非高斯非线性环境下的机器人状态估计问题,本文首先对于非高斯噪声环境建立高斯混合模型,并利用改进的鲁棒EM 算法进行高斯混合模型参数的求解,相比传统EM 算法,改进鲁棒EM 算法对于初始参数不敏感,不需要提前设定混合成分个数.其次,利用由改进鲁棒EM 算法得到的高斯混合模型参数,进行高斯和容积卡尔曼滤波的设计.考虑到高斯和滤波过程中会出现高斯项冗余的情况,本文利用凸组合融合,得到了Salmond 方法与B(·)准则方法两种方法合并后的高斯混合项,并给出相应的理论分析和对比仿真实验.结果表明,经过融合后滤波算法的状态估计精度相比于传统的两种方法有明显的提升. 同时,通用场景下的飞行测试数据验证分析也表明了新方法的有效性.未来进一步究工作包括有效高斯项数目动态优化、非高斯噪声统计特性动态估计、更多合并距离方法的融合策略以及复杂场景的数据非高斯动态建模和基于可信度高斯和滤波器设计等.

附录A