基于平方根卡尔曼滤波的粒子滤波算法

基于迭代平方根容积粒子滤波目标跟踪算法陈超波;刘叶楠;高嵩【期刊名称】《测控技术》【年(卷),期】2015(034)007【摘要】针对粒子滤波目标跟踪算法粒子退化及跟踪精度问题,提出了一种基于马尔可夫链-蒙特卡罗(MCMC,Markov Chain Monte Carlo)的迭代平方根容积粒子滤波(ISRCPF,iterated square root cubature Kalman particle filter)算法(ISRCPF-MCMC).在该滤波算法中,利用容积数值积分原则计算非线性随机函数的均值和方差,通过正交矩阵分解代替矩阵开方,在生成的粒子滤波建议分布中融入当前量测值,提高对系统后验概率的逼近程度.然后在此基础上融合MCMC抽样算法(MH,Metropolis Hasting)对所选建议分布进行优化,增加粒子多样性,以提高跟踪精度.仿真试验结果表明,ISRCPF-MCMC算法的估计误差与其他算法相比降低至0.403％.【总页数】5页(P120-124)【作者】陈超波;刘叶楠;高嵩【作者单位】西安工业大学电子信息工程学院,陕西西安710021;西安工业大学电子信息工程学院,陕西西安710021;西安工业大学电子信息工程学院,陕西西安710021【正文语种】中文【中图分类】TP391【相关文献】1.基于噪声补偿的迭代平方根CKF的汽车雷达目标跟踪算法 [J], 熊星;王彩玲;荆晓远2.基于迭代积分粒子滤波的目标跟踪算法∗ [J], 毛少锋;冯新喜;鹿传国;危璋3.基于平方根容积信息滤波的弹道目标跟踪算法 [J], 刘俊;刘瑜;熊伟;孙顺4.基于量化新息的容积粒子滤波融合目标跟踪算法 [J], 徐小良;汤显峰;葛泉波;管冰蕾5.基于迭代容积平方根粒子滤波的城市雨水利用控制模型 [J], 罗义;代学民;马立山;龚建英因版权原因，仅展示原文概要，查看原文内容请购买。

考虑随机非线性离散广义系统(1)((),)((),)()Mx k f x k k g x k k w k +=+ (0-1)(1)((1),1)(1)y k h x k k v k +=++++(0-2)其中：k 为离散时间，状态变量()n x k ∈ℜ，观测变量()m y k ∈ℜ，输入噪声()w k ，观测噪声()v k ，()f ⋅，()g ⋅和()h ⋅对()x k 是可微的，p n M ⨯∈ℜ是奇异常数矩阵，min{,}rank M r p n =<。

假设1 ()w k 和()v k 是零均值的不相关白噪声()()()()()()()()T Tkj T w k Q k S k E w j v j v k S k R k δ⎧⎫⎡⎤⎡⎤⎡⎤=⎨⎬⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦⎩⎭ 其中E 为均值符号，T 为转置符号，1kk δ=，0()kj k j δ=≠。

假设2 非线性广义系统(0-1)~(0-2)是强可控的。

如果在k 时刻，状态变量()x k 的最小方差线性估值ˆ(|)x k k 已知，则可以将非线性系统(0-1)在ˆ()(|)x k x k k =，(0-2)在ˆ()(1|)x k x k k =+附近进行Taylor 级数展开，只保留一次项，近似可得ˆ()(|)((),)ˆˆ(1)((|),)|(()(|))((),)()()x k x k k Tf x k k Mx k f xk k k x k x k k g x k k w k x k =∂+=+-+∂(0-3) ˆ(1|)(1|)((1|),)ˆˆ(1)((1|),)|(()(|))(1)(1|)x k k x k k T h x k k k y k hxx k k k x k xk k v k x k k +=+∂++=++-++∂+(0-4) 其中：离散状态一步预测为ˆˆ(1|)((|),)x k k f x k k k +=，并认为ˆ((),)((|),)g x k k g x k k k =。

粒子滤波算法在图像处理中的应用一、背景图像处理技术的发展已经广泛应用于人们的日常生活中，如视频监控、医学图像诊断、无人驾驶、虚拟现实等领域。

图像处理技术中的超分辨率恢复和目标跟踪等问题，需要解决信号处理领域经典问题：估计和滤波问题。

二、传统滤波和粒子滤波的比较传统滤波技术方法主要包括线性滤波、非线性滤波和尺度空间方法。

这些方法的局限性在于图像中最普遍的问题是非高斯的噪声以及非线性/非高斯分布的像素值。

相比传统滤波技术，粒子滤波(PF)算法具有较强的适用性。

PF 是基于贝叶斯滤波的非线性卡尔曼滤波(Extended Kalman Filter, EKF)的一个扩展，利用非参数方法实现对状态方程非线性函数的逼近，适用于一般的非线性、非高斯问题。

PF算法的基本思想是通过表示概率分布的一组粒子在已知观测数据的情况下进行递推，得到概率分布随时间的演化过程。

与传统的滤波算法不同，PF算法利用粒子对状态量进行数值估计，而不是利用直接的数值估计，避免了高斯和线性假设的限制。

三、粒子滤波算法在图像处理中的应用1、目标跟踪在图像处理中，通过PF算法进行目标跟踪是一种有效的方法。

对于跟踪过程，PF算法可以用于估计目标区域和跟踪目标的运动状态，比如目标的速度和方向等。

在PF算法中，需要选择重采样方法，可以采用系统自适应粒子滤波算法(SIR-PF)来解决这个问题。

这种方法可以通过自适应的方法估计粒子的权重和重采样的样本数量，从而提高重采样抽样和跟踪的性能。

2、图像匹配图像匹配是估计两幅图像之间空间关系的过程。

在图像处理中，PF算法可以应用于偏移估计、匹配关键点、图像对齐等问题。

PF算法的主要优势在于可以接受非线性的变换模型和非高斯噪声，同时可以在最大后验概率(MAP)框架下进行求解。

在PF算法中，需要利用连续的状态极其之间的相互联系，利用高斯混合模型(GMM)来代替样本的权重分布。

这样，可以更好地利用概率密度函数对隐藏变量进行建模，提高状态的估计精度。

平方根体积积分卡尔曼滤波郭文艳;姬春艳;周吉瑞【摘要】Cubature quadrature rule is a kind of higher algebra precision integration method to improve nonlinear state estimate problems. To enhance the stability of nonlinear filter, based on a square root decomposition of covariance matrix, a new Square Root Cubature Quadrature Kalman Filter(SRCQKF)is proposed. The spherical radial cubature rule and Gauss-Laguerre quadrature rule are applied to compute the integral points. The square root of covariance matrix is obtained and propagated by matrix QR decomposition. Two typical nonlinear examples show that the SRCQKF improves the accuracy of nonlinear estimation and has more stability.%体积积分是一种新的具有较高代数精度的积分方法。

为了提高非线性滤波算法的精度和数值稳定性，将体积积分规则和平方根分解引入卡尔曼滤波框架中，提出了平方根体积积分卡尔曼滤波算法（SRCQKF）。

新算法采用球半径体积规则和高斯-拉盖尔积分规则计算积分点，利用矩阵的QR分解得到协方差矩阵的平方根并传播平方根。

两个典型的非线性系统的实验结果表明，与体积卡尔曼滤波相比，新算法提高了非线性状态的估计精度，具有较高的数值稳定性。

第40卷第2期自动化学报Vol.40,No.2 2014年2月ACTA AUTOMATICA SINICA February,2014平方根容积Rao-Blackwillised粒子滤波SLAM算法宋宇1,2李庆玲3康轶非1闫德立1摘要面向大尺度环境中的移动机器人同时定位与地图构建(Simultaneous localization and mapping,SLAM)问题,提出平方根容积Rao-Blackwillised粒子滤波SLAM算法.算法主要特点在于:1)采用容积律计算SLAM中的非线性函数高斯权重积分,达到减小SLAM非线性模型线性化误差、提高SLAM精度的目的;2)在SLAM中直接传播误差协方差矩阵的平方根因子,避免了耗费时间的协方差矩阵分解与重构过程,提高了SLAM计算效率.通过仿真、实验将提出的SLAM算法与FastSLAM2.0、UFastSLAM两种算法进行对比,结果表明本文算法在SLAM性能上优于另两者.关键词移动机器人,同时定位与地图构建,粒子滤波,容积律,高斯权重积分引用格式宋宇,李庆玲,康轶非,闫德立.平方根容积Rao-Blackwillised粒子滤波SLAM算法.自动化学报,2014,40(2): 357−367DOI10.3724/SP.J.1004.2014.00357SLAM with Square-root Cubature Rao-Blackwillised Particle FilterSONG Yu1,2LI Qing-Ling3KANG Yi-Fei1YAN De-Li1Abstract In this paper,we derive a new large-scale environment simultaneous localization and mapping(SLAM)al-gorithm based on square-root cubature Rao-Blackwillised particlefilter.The main contributions are:1)to enhance the SLAM performance,the effective cubature rule is utilized to calculate the Gaussian weighted integral of the nonlinear function;2)the covariance square-root factors are directly propagated in our SLAM process.Hence,the time-expensive decompositions on covariance matrixes are avoided.The performance of the proposed algorithm is compared with Fast-SLAM2.0and UFastSLAM using a serial simulations and experiments.Results show that the proposed SLAM outperforms FastSLAM2.0and UFastSLAM.Key words Mobile robot,simultaneous localization and mapping(SLAM),particlefilter,cubature rule,Gaussian weighted integral(GWI)Citation Song Yu,Li Qing-Ling,Kang Yi-Fei,Yan De-Li.SLAM with square-root cubature Rao-Blackwillised particle filter.Acta Automatica Sinica,2014,40(2):357−367即时定位与地图构建(Simultaneous localiza-tion and mapping,SLAM)是“鸡生蛋、蛋生鸡”的悖论问题,一方面移动机器人利用自载传感器感知未知环境,形成对局部环境的理解,并增量式创建收稿日期2012-09-05录用日期2013-02-04Manuscript received September5,2012;accepted February4, 2013国家自然科学基金(60905055,61005070),哈尔滨工业大学机器人技术与系统国家重点实验室开放基金(SKLRS-2009-ZD-04),中央高校基本科研业务费(2014JBM014)资助Supported by National Natural Science Foundation of China (60905055,61005070),Open Function of State Key Labora-tory of Robotics and System of Harbin Institute of Technology (SKLRS-2009-ZD-04),and Fundamental Research Funds for the Central Universities of China(2014JBM014)本文责任编委胡小平Recommended by Associate Editor HU Xiao-Ping1.北京交通大学电子信息工程学院北京1000442.哈尔滨工业大学机器人技术与系统国家重点实验室哈尔滨1500803.中国矿业大学(北京)机电与信息工程学院北京1000831.School of Electronic and Information Engineering,Beijing Jiaotong University,Beijing1000442.State Key Laboratory of Robotics and System,Harbin Institute of Technology,Harbin 1500803.School of Mechanical Electronic and Information Engineering,China University of Mining and Technology,Bei-jing100083全局环境地图;另一方面,机器人利用对已建立环境地图的重复观测机制来处理不确定信息(包括运动和观测的不确定性),实现对其自身准确定位.近20年来,SLAM问题已经成为国际机器人与自动化领域的研究热点,并被认为是实现移动机器人真正自主的“圣杯”[1−2].基于Rao-Blackwellized particlefilter的Fast-SLAM算法为学者Montemerlo于2003年提出[3].相比于传统的卡尔曼滤波SLAM方法,FastSLAM 具有计算代价小、适用于大尺度环境、对数据关联误差鲁棒等优点,已经成为SLAM研究中流行的方法框架.FastSLAM算法根据SLAM问题的条件独立特性,将高维的移动机器人轨迹和环境地图联合后验概率密度估计解耦为低维状态估计,从而解决了状态空间SLAM算法的维数灾难问题,提高了SLAM的求解效率.FastSLAM算法有两个版本: FastSLAM1.0和FastSLAM2.0.FastSLAM1.0利用粒子滤波器跟踪移动机器人位姿参数,并且每一粒子单独维护一幅环境特征地图、每一个环境路标358自动化学报40卷利用卡尔曼滤波(Extended Kalmanfilter,EKF)来维护.Montemerlo在其学位论文中指出,当移动机器人的环境观测噪声较小时(即环境观测传感器过于精确,如激光传感器),FastSLAM1.0容易发生粒子集退化,使得SLAM发散[3].为克服FastSLAM1.0存在的上述问题,FastSLAM2.0算法利用EKF将当前机器人的环境观测信息融入粒子滤波器的提议分布设计之中,使得粒子集中地分布于高观测似然区域.结果表明,FastSLAM2.0较好地解决了粒子退化问题,使用较少的粒子就可得到满意的SLAM结果.基于类似的学术思想,学者Grisetti等提出栅格地图模型FastSLAM算法[4],该方法联合激光雷达扫描匹配技术和机器人里程计信息来优化粒子滤波器的提议分布,实现了提高有效粒子数和SLAM精度的目的;Sim等提出了立体视觉FastSLAM算法[5],该算法的主要特点在于利用立体视觉里程计来设计粒子滤波器的重要性函数;Kim等提出了UFastSLAM算法[6],其核心在于利用无迹变换[7]来估计SLAM中的转移概率密度,与其类似还有中心差分粒子滤波Fast-SLAM算法[8];在UFastSLAM基础上,Kim等在文献[9]中还对粒子滤波器提议分布设计问题进行了探讨,更正了原始UFastSLAM中的过信估计问题,给出了改进型的UFastSLAM算法.在我们的前期工作中,提出利用容积卡尔曼滤波计算Fast-SLAM的提议分布并维护环境特征地图,初步探讨了CFastSLAM算法[10].值得说明的是,与传统SLAM算法相似,在CFastSLAM算法中会以协方差形式来传播SLAM的不确定性.因而,为利用容积律计算转移概率密度,需要在SLAM滤波的迭代过程中(包括预测和更新)不断执行数值敏感且繁琐的协方差矩阵平方根分解、协方差矩阵重构操作.为此,在继承CFastSLAM优点的基础上,探讨如何在SLAM中实现直接传播协方差平方根因子,避免协方差矩阵平方根分解、协方差重构过程,进一步提高SLAM性能,是本文工作的基本出发点.面向特征地图SLAM问题,提出平方根容积FastSLAM算法,并通过仿真、实验验证了其性能.算法主要特点在于:1)利用容积律计算非线性转移概率密度,继承了CFastSLAM的高阶矩估计精度、无需计算雅可比矩阵等优点;2)粒子结构由SLAM 状态及其不确定性协方差平方根因子构成,基于容积律的数值特性,深入探讨并实现了在SLAM迭代过程中的直接协方差平方根因子传播,避免了SLAM迭代中数值敏感且繁琐的协方差平方根分解与重构过程,并可严格保证SLAM协方差矩阵的正定性与对称性.1背景知识1.1FastSLAM算法框架FastSLAM算法的核心思想在于利用机器人对环境观测的历史信息z k=z1,···,z k和机器人的控制的历史信息u k−1=u1,···,u k−1来估计环境地图Θ=θ1,···,θM(环境路标的数量用M来表示)与机器人运动轨迹s k=s1,···,s k的联合后验概率分布p(s k,Θ|z k,u k−1).基于SLAM问题的条件独立性结论:若移动机器人轨迹s k已知,环境特征路标Θ=θ1,···,θM相互独立.因而,联合后验分布p(s k,Θ|z k,u k−1)可以表示为p(s k,Θ|z k,u k−1)=p(s k|z k,u k−1)Mm=1p(θm|s k,z k,u k−1)(1)基于上式表达的条件独立特性,FastSLAM算法采用不同的估计器来估计机器人轨迹后验概率与环境路标的后验概率:对于轨迹后验p(s k|z k,u k−1),采用N个粒子的粒子滤波器估计,其中,每一个粒子单独维护一幅环境地图Θ和机器人路径s k的假设;在机器人轨迹s k已知前提下,由于路标之间相互独立,则对每个环境路标后验p(θm|s k,z k,u k−1)利用单独的卡尔曼滤波器估计.这样,FastSLAM共由N×M+1个滤波器组成.FastSLAM算法中每一个粒子可以表述为S[i]k= (s[i]k,P[i]k),···,(µ[i][m]k,Σ[i][m]k), (2)式中,S[i]k表示FastSLAM算法中的索引为i的粒子;s[i]k为该粒子对机器人当前位姿的假设,P[i]k为s[i]k的协方差;(µ[i][m]k,Σ[i][m]k)为该粒子所维护的特征地图中第m个路标的全局坐标及其不确定性协方差.1.2高斯权重积分的容积律方法为实现高斯域中的贝叶斯滤波,最为重要的步骤是计算高斯分布的非线性转移密度,即计算高斯权重积分(Gaussian weighted integral,GWI),表示为I=g(x)N(x,P x)d x(3)其中,x∈R n x,n x代表变量x的维数,g(x)表示非线性函数;N(x,P x)代表n x维高斯分布.在文献[11]中,学者Arasaratnam等面向GWI 的求解问题提出了容积律方法,该方法利用2n x个等权重容积点计算积分I:I≈12n x2n xj=1g(P xζj+x)(4)2期宋宇等:平方根容积Rao-Blackwillised粒子滤波SLAM算法359式中,容积点集为√P xζj+x,点集ζj表示为{ζj }=√n x{[1]j},j=1,2,···,2n x(5)式中,{[1]j}代表n x维笛卡尔坐标系的坐标轴与n x 维单位超球的交点坐标.2提出的SLAM算法为研究SLAM问题,不失一般性地定义移动机器人系统的含噪声运动学方程和环境观测方程为s k=f(s k−1,u k−1+δu k−1)z k=h(s k,µ[m]k )+δz k(6)其中,s k∈R n s为机器人位姿参数;z k∈R n z为环境观测;f和h分别为非线性机器人运动学模型和环境观测模型;u k−1∈R n u为在离散时间段[k,k−1]内对机器人的控制作用,δu k−1∈R n u为控制噪声,其协方差Q∈R n u×n u;δz k∈R n z为观测噪声,其协方差为R∈R n z×n z;µ[m]k∈R nµ代表第m个环境路标的全局坐标系位置参数.不同于由式(2)所定义的传统FastSLAM算法粒子结构,提出算法的粒子结构中利用协方差矩阵平方根因子代替传统算法粒子结构中的协方差矩阵,并且在SLAM迭代过程中利用平方根因子传播代替传统算法中的协方差传播.提出算法中的粒子结构为S[i] k = (s[i]k,v[i]k),···,(µ[i][m]k,l[i][m]k), (7)其中,s[i]k 为该粒子对机器人当前位姿的假设,v[i]k为协方差矩阵P[i]k 的平方根因子,满足v[i]k(v[i]k)T=P[i] k ;µ[i][m]k为该粒子中索引为m的环境路标位置,l[i][m] k 为其协方差矩阵Σ[i][m]k的平方根因子,满足l[i][m] k (l[i][m]k)T=Σ[i][m]k.2.1机器人状态估计2.1.1状态预测为预测k时刻机器人位姿状态,需要联合控制输入u k−1及其不确定性协方差的平方根因子来增广k−1时刻机器人状态及其协方差平方根因子a=s[i]k−1u k−1,V a=v[i]k−10S Q(8)式中,a∈R n a、V a∈R n a×n a(n a=n s+n u)分别为增广机器人状态及增广协方差平方根因子;s[i] k−1、v[i]k−1为k−1时刻的机器人状态及协方差因子;S Q为控制噪声的协方差平方根因子,满足S Q S TQ=Q.增广状态a服从多维高斯分布N(a,V a V Ta ).为计算机器人在k−1的状态s[i]k−1经运动学模型f传播后的转移概率密度,需按下式计算多维高斯分布N(a,V a V Ta)的容积点集{C[i]j}:{C[i]j}={V aζj+a},j=1,2,···,2n a(9)式中,j为容积点索引.这里值得注意的是,由于算法以协方差平方根因子传播代替协方差传播不确定性传播过程,上式计算容积点时,无需执行数值敏感且计算复杂度较高的协方差矩阵的平方根分解操作.将容积点C[i]j记为机器人状态s和控制作用u的组合形式,即C[i]j=[C s[i]j,C u[i]j]T,并通过机器人运动学模型f传递容积点集,得到变换容积点集{C∗[i]j}={f(C s[i]j,C u[i]j)}.则机器人在k时刻的状态预测为s[i]k|k−1=f(a)N(a,V a V Ta)d a≈12n a2n aj=1C∗[i]j(10)为预测机器人状态协方差的平方根因子v[i]k|k−1,定义误差矩阵Err∈R n s×2n aErr=1√2n a[···,C∗[i]j−s[i]k|k−1,···](11)通过对Err T进行QR分解,机器人状态协方差平方根因子v[i]k|k−1为上三角方阵的转置,表示为[q,r]=qr(Err T),v[i]k|k−1=r T(12)式中,qr(·)为QR分解操作符;r为n s×n s上三角阵.2.1.2状态更新当索引为m的环境路标被移动机器人重新观测到后,利用机器人对该路标的重复观测信息来更新预测状态s[i]k|k−1及其协方差平方根因子v[i]k|k−1.首先,联合环境路标状态及其协方差平方根因子对s[i]k|k−1和v[i]k|k−1进行增广.b=s[i]k|k−1µ[i][m]k−1,V b=v[i]k|k−10l[i][m]k−1(13)式中,b∈R n b、V b∈R n b×n b(n b=n s+nµ)为增广机器人状态和增广协方差平方根因子.增广状态b服从多维高斯分布N(b,V b V Tb).与式(9)类似,分布N(b,V b V Tb)的容积点集为{X[i][m]j}={V bζj+b},j=1,2,···,2n b(14)将每一个容积点表述为机器人状态和环境路标状态的组合形式X[i][m]j=[X s[i][m]j,Xµ[i][m]j]T,并利用机器人环境观测模型h传播容积点集,得到变换360自动化学报40卷容积点集{X∗[i][m]j }={h(X s[i][m]j,Xµ[i][m]j)},进而机器人对该路标的观测预测为下式的高斯权重积分解:z[i][m] k|k−1=h(b)N(b,V b V Tb)d b≈12n b2n bj=1X∗[i][m]j(15)为利用卡尔曼滤波技术更新机器人状态,需计算移动机器人状态与环境路标观测间的交互协方差与观测新息协方差.为此,定义误差矩阵如下:Err z=1√2n b[···,X∗[i][m]j−z[i][m]k|k−1,···]Err s=1√2n b[···,X s[i][m]j−s[i]k|k−1,···](16)进而,机器人状态与观测间的交互协方差为P[i][m]sz=Err s Err z T(17)观测新息协方差平方根因子d[i][m]zz由误差矩阵[Err z,S R]T的QR分解计算:[q,r]=qr([Err z,S R]T),d[i][m]zz=r T(18)式中,r为n z×n z上三角方阵;S R为观测噪声协方差R的平方根因子,即S R S TR=R.基于卡尔曼滤波算法,机器人状态更新为K s=P[i][m]sz (d[i][m]zzd[i][m]zzT)−1s[i] k =s[i]k|k−1+K s(z[m]k−z[i][m]k|k−1)(19)式中,z[m]k为机器人在当前时刻k对索引为m的环境路标的真实传感器测量.机器人状态的协方差平方根因子v[i]k由误差矩阵[Err s−K s Err z,K s S R]T的QR分解计算,即[q,r]=qr([Err s−K s Err z,K s S R]T),v[i]k=r T(20)式中,r为n s×n s上三角方阵;S R为观测噪声协方差R的平方根因子;K s为式(19)中计算的滤波增益.2.1.3重要性采样如果当前时刻移动机器人同时观测到多个已存储在地图中的环境路标,则依次根据对每一个路标的观测对机器人状态s[i]k 及其协方差平方根因子v[i]k进行更新(依次执行式(13)∼(20)),每次更新均以前次更新结果作为初始值.更新完毕后,从机器人状态分布中采集新一代粒子,即s[i] k ∼N(s[i]k,v[i]kv[i]kT)(21)进而,根据观测新息及新息协方差来计算新粒子的重要性权重ω[i]k:ω[i]k∝m=1exp−(z[m]k−z[i][m]k|k−1)T(z[m]k−z[i][m]k|k−1)d[i][m]zzd[i][m]zzT(22)2.2环境地图估计对于提出算法中的每一个粒子S[i]k,在更新其中代表移动机器人位姿的部分后,进入更新环境路标地图阶段.在该阶段,需对当前时刻的重复观测路标和首次观测路标采用不同的策略分别处理.2.2.1重复观测环境路标的更新在当前时刻k,对于那些已经存储在地图中且被移动机器人重复观测到的环境路标m,其全局坐标的高斯描述为N(µ[i][m]k−1,l[i][m]k−1l[i][m]k−1T).则基于容积律,路标状态分布的容积点集为{V[i][m]j}={l[i][m]k−1ζj+µ[i][m]k−1},j=1,2,···,2nµ.容积点集{V[i][m]j}经非线性环境观测模型h作用后,得到变换容积点集{Z∗[i][m]j}={h(s[i]k,V[i][m]j)}.因而,在机器人位姿参数s[i]k已知的前提下,机器人对索引为m的路标的观测预测计算为z∗[i][m]k|k−1=h(s[i]k,µ)N(µ,ll T)dµ≈12nµ2nµj=1Z∗[i][m]j(23)式中,s[i]k为由式(21)计算.与机器人状态更新类似,定义误差矩阵:Err∗z=12nµ[···,Z∗[i][m]j−z∗[i][m]k|k−1,···]Err∗µ=12nµ[···,V[i][m]j−µ[i][m]k−1,···](24)则路标状态与环境测量间的交互协方差矩阵为P[i][m]µz=Err∗µErr∗zT(25)观测新息协方差平方根因子d∗[i][m]zz由矩阵[Err∗z,S R]T的QR得到,即[q,r]=qr([Err∗z,S R]T),d∗[i][m]zz=r T(26)基于卡尔曼滤波,重复观测环境路标状态更新为Kµ=P[i][m]µz(d∗[i][m]zzd∗[i][m]zzT)−1µ[i][m]k=µ[i][m]k−1+Kµ(z[m]k−z∗[i][m]k|k−1)(27)式中,z[m]k为机器人在当前时刻k对索引为m的环境路标的真实传感器测量.2期宋宇等:平方根容积Rao-Blackwillised粒子滤波SLAM算法361环境路标的协方差平方根因子l[i][m]k由对误差矩阵[Err∗µ−KµErr∗z,KµS R]T的QR分解得到:[q,r]=qr([Err∗µ−KµErr∗z,KµS R]T),l[i][m]k=r T(28)其中,r为nµ×nµ上三角方阵;S R为观测噪声协方差R的平方根因子;Kµ为式(27)计算的滤波增益.2.2.2首次观测环境路标的初始化对于首次观测到的路标(这里记其索引为n),利用观测z[n]k 计算其全局坐标并加入粒子S[i]k维护的全局路标地图.由于环境测量z[n]k不确定性分布的容积点集为{F[i][n]j }={S Rζj+z[n]k},j=1,2,···,2n z,则经逆观测模型µ=h−1(s,z)传播后的变换容积点集为{F∗[i][n]j }={h−1(s[i]k,F[i][n]j)}.进而,索引为n的环境路标状态µ[i][n]k为µ[i][n] k =h−1(s[i]k,z)N(z,R)d z≈12n z2n zj=1F∗[i][n]j(29)为初始化该路标的协方差平方根因子,定义n z×2n z误差矩阵Errµ:Errµ=1√2n z[···,F∗[i][n]j−µ[i][n]k,···](30)通过对误差矩阵ErrµT执行QR分解,路标状态协方差平方根因子l[i][n]k计算为[q,r]=qr(ErrµT),l[i][n]k=r T(31)式中,r为nµ×nµ上三角方阵.2.3算法流程综上,提出的SLAM算法具体执行过程见算法1.算法1.本文提出的SLAM算法给定粒子数N,控制噪声Q,观测噪声R,初始化粒子集S0=∅;For k=1,2,3,···//对于每一离散时刻For i=1,2,3,···//对于每一粒子1)索引粒子S[i]k−1;2)根据式(8)∼(12)来预测移动机器人状态及其协方差平方根因子;3)对当前机器人观测执行数据关联,区分重复观测路标和首次观测路标;4)根据机器人对已存储在地图中路标的重复观测,执行机器人状态更新(式(13)∼(20));5)采样新粒子(式(21))并计算其重要性权重(式(22));6)对已存储在粒子中的重复观测路标,根据式(23)∼(28)更新其状态及其协方差平方根因子;7)对于首次观测路标,根据式(29)∼(31)初始化路标状态及其协方差平方根因子,加入环境地图;End for基于粒子权重对粒子集执行重采样处理;返回粒子集S[i]k;End for2.4提出算法的优势分析非线性模型线性化引入的截断误差将在SLAM 迭代过程中逐步累积.因而,为提高SLAM性能,有效减小误差累积是值得关注的核心问题.本质上,SLAM中非线性模型线性化的目的在于计算高斯分布经非线性函数作用后的转移密度,如式(10)、(15)、(23)、(29),其被积函数均具有非线性函数×高斯密度的基本形式.为说明该问题,不失一般性地定义SLAM问题中涉及的非线性函数为y=g(x),其中,x∈R n x服从高斯分布N(ˆx,P x),为计算转移密度N(ˆy,P y),需要计算如下两个GWI:ˆy=g(x)N(ˆx,P x)d xP y=[g(x)−ˆy][g(x)−ˆy]T N(ˆx,P x)d x(32)在FastSLAM2.0中,以上两个GWI是通过对非线性函数g(x)在ˆx处进行线性化所得到,即g(x)=g(ˆx)+∇g(ˆx)(x−ˆx).其中,∇g(ˆx)为雅可比矩阵.因而,根据高斯分布的性质有ˆy=g(ˆx), P y=∇g(ˆx)P x∇g(ˆx)T.显而易见,当g的非线性程度较高时,转移密度N(ˆy,P y)的计算将不准确.对于UFastSLAM算法,上述积分由Sigma支撑点集的线性回归计算,由于Sigma点方法在理论上可以达到3阶非线性,UFastSLAM的精度要高于FastSLAM2.0.然而,Sigma点方法存在于数值稳定性差、难以保证协方差矩阵的正定性、需设定的自由参数多等问题,限制了其在SLAM中的应用.2.4.1提出算法中利用容积律的优势在矩特性方面,容积律(式(4)和(5))是从数值积分观点提出的一种GWI求解方法[11].已经证明了多变量矩积分x i11x i22···x i nnN(ˆx,P x)d x(其中x j指明向量x的j-th分量)在阶数满足条件i nj=1i j≤3时可由容积律来精确计算.对于非线性函数y=g(x),通过对g(x)在x=ˆx处进行泰勒展开,则积分g(x)N(ˆx,P x)d x可以表示为多个矩积分的线性组合.由于利用容积律可以精确计算展开式中所有前三阶矩积分,故而从泰勒展开的意义上,容积律具有3阶非线性估计精度.从估计的矩特性角度,提出SLAM算法中采用的容积律与UFastSLAM和FastSLAM2.0中分别采用的无迹变换、线性化方法的对比见表1.对于SLAM问题,均为多维高斯情况,因而在SLAM算362自动化学报40卷法中采用容积律可以达到与无迹变换相同的矩估计特性.表1不同高斯权重积分求解方法的矩特性对比Table 1Moment characteristic for GWI solutionsGWI 解1维高斯(n x =1)高维高斯(n x >1)线性化方法1阶精度1阶精度无迹变换5阶精度3阶精度容积律3阶精度3阶精度从数值积分的角度,GWI 数值ωi g (x i )解应满足的基本条件是:所有权重ωi 非负.如式(4),显而易见,容积律方法中的所有容积点权重均为1/(2n x ),满足该条件.进而,基于容积律计算的协方差矩阵P y 可保证其非负定性.与之相比较,UFastSLAM 中采用的无迹变换并不能满足该条件.当高斯维数大于3时(对于SLAM,增广状态一般高于3维,即n x >3),其中心Sigma 点的权重ω0=1−n x /3<0,这将可能导致求解的协方差矩阵P y 负定,进而无法求解其平方根,导致SLAM 无法执行.另外,如文献[11]所述,对于数值积分解,当稳定性因子 ωi /|ωi |大于1时,在有限字长的计算机中实施数值计算会引入圆整误差.对于容积律,由于全部容积点的权重均为1/(2n x ),因而其稳定性因子将始终为1.与之相比较,无迹变换的稳定性因子为n x /3+|1−n x /3|,即当维数n x ≤3时数值稳定性为1,而当n x >3时,其稳定性因子将伴随着维数的增长而线性增长,数值计算稳定性变差.提出SLAM 中采用的容积律和UFastSLAM 中采用的无迹变换的数值特性对比见表2.对于SLAM 实际应用,一般增广状态向量的维数高于3(在式(8)、式(13)中,机器人状态包括其坐标和航向角,则增广后状态维数大于3),因而在SLAM 中应用容积律较无迹变换具有数值计算上的优势:数值精度高、可严格保证协方差的正定性.表2容积律与无迹变换的数值特性对比Table 2Numerical characteristic for GWI solutions数值特性无迹变换容积律数值稳定性因子(n x ≤3)11数值稳定性因子(n x >3)2n x3−1>11协方差P y 正定性(n x ≤3)可保证可保证协方差P y 正定性(n x >3)不确定可保证计算代价方面,提出SLAM 算法较基于无迹变换的UFastSLAM 在计算代价方面也具有优势,其原因在于:1)提出算法采用容积律计算GWI,对于相同的高斯维数n x ,容积点数为2n x ,数量低于UFastSLAM 中无迹变换需要的2n x +1个Sigma 点;2)提出算法中的容积点为等权重点,因而利用容积点计算GWI 仅执行简单的平均操作(1次乘法操作),而无迹变换中的Sigma 点为非等权重点,需要使用线性权重回归方式来计算GWI.2.4.2提出算法中采用平方根因子传播的优势利用容积律来计算高斯权重积分,需要计算“容积点集”,而计算“容积点集”需要利用到协方差矩阵的平方根因子,如式(4)所说明.因而,为计算式(32)的非线性转移概率密度N (ˆy ,P y ),需要首先计算协方差矩阵P x 的平方根√P x ,并根据√P x 计算容积点集.容积点集经非线性函数g 传播后,最终用来重构均值ˆy及协方差P y ,完成转移概率密度的估计.由文献[12],利用Cholesky 分解来计算协方差矩阵P x 平方根的复杂度为O(n 3x 6),且由于容积点的数量为2n x ,则重构协方差P y 的复杂度为O(2n 3x ),即传播协方差矩阵的复杂度为O(2n 3x +n 3x /6).与其相比较,本文算法在SLAM 中直接传播协方差平方根因子√P x ,并利用QR 分解实现协方差平方根因子的更新,其计算复杂度为O(2n 3x ).另外,计算协方差矩阵的平方根√P x 是一种应尽力避免的数值敏感操作.为利用Cholesky 分解计算√P x ,其基本前提是要严格保证协方差矩阵P x 的正定性.如果在SLAM 中传播协方差矩阵P x ,由于在计算过程难以避免引入圆整误差,在卡尔曼滤波的协方差更新方程中并不能保证更新后协方差的正定性(卡尔曼滤波算法的协方差更新方程具有矩阵差的形式,形如P +=P −KQK T ),这将可能导致滤波过程的数值不稳定[11−12],甚至滤波器无法执行.因而,在卡尔曼滤波器协方差更新的具体实施中还需要协方差矩阵正定性的检验过程[13].而提出SLAM 算法采用直接传播协方差平方根因子√P x ,由其重构的协方差矩阵为√P x √P x T,可以始终严格保证正定性与对称性.因而,在SLAM 中直接传播协方差平方根因子比传播协方差矩阵具有数值计算稳定性上的明显优势.再者,本文算法中协方差平方根因子的预测和更新均由QR 分解实现,得到的协方差平方根因子为三角方阵,这不仅严格保证了重构的协方差的对称性与正定性,还可以利用高效的回代算法(Back substitution)求解SLAM 算法中涉及矩阵逆,如求解式(19)、(22)和式(27)中的矩阵逆,实现进一步提高SLAM 的求解效率.3仿真研究在Matlab2010平台下(计算机主频2.6GHz),基于悉尼大学野外机器人中心发布的SLAM 算法仿真器开展仿真研究[13].并通过将提出的算法与2期宋宇等:平方根容积Rao-Blackwillised 粒子滤波SLAM 算法363FastSLAM2.0、UFastSLAM 两种方法的综合对比来验证算法性能.在该部分研究中,机器人运动学模型为前轮驱动并转向的Car-like 运动学模型,环境观测模型为Bearing-Rang 激光雷达扫描模型.相关仿真参数设定为:移动机器人航向运动速度v =4m/s;机器人车体前后轮间距WB =4m;环境观测传感器最大观测距离范围40m,最大前向视角180◦;控制周期25ms,环境路标观测周期200ms.仿真中设计的地图环境为图1所示的大尺度、密集路标环境.其地图尺度为230×190,包含路标302个(图中“*”表示实际路标位置,“.”表示估计路标位置).仿真中设定移动机器人从初始状态s 0=[0,0,0]T开始运动,并按照全局规划的一系列目标点(图中“o”表示)行走,直至运动一周后返回起始点(环形闭合).图1大尺度、密集路标下仿真环境Fig.1Large map with dense landmarks3.1增长测量噪声条件下的SLAM 算法性能由于在粒子滤波器的重要性函数设计中缺少了当前的环境观测信息,经典的FastSLAM 算法的主要缺点在于粒子集退化问题(如FastSLAM1.0).特别当环境观测传感器精确时,观测似然函数的外形尖锐,容易造成绝大多数粒子权重很小甚至为零,导致粒子集退化、SLAM 滤波器失效.为验证提出的SLAM 算法有效地避免了粒子退化问题,在图1所示的特征地图下开展一系列的仿真实验:仿真中设定粒子数为10个,车辆速度控制噪声设定为0.3m/s;驾驶角控制噪声设定为3◦;环境特征测量的角度噪声为0.5◦;环境特征测量的距离噪声为11组增长的值,分别为0.01m,0.03m,0.05m,0.07m,0.09m,0.1m,0.15m,0.2m,0.5m,0.7m, 1.0m.算法性能评估标准为均方根误差(Root mean square error,RMSE)和有效粒子百分比(Number of effective particles,NEFF),定义为RMSE =N sk =0|s k −ˆsk |2N s −112NEFF =100%N N i =1(ω[i ]k )2(33)式中,s k 为机器人在k 时刻的真实状态;ˆsk 为SLAM 算法的估计值;N s 为一次SLAM 仿真中的离散采样点数量;ω[i ]k 为第i 个粒子的重要性权重;N 为粒子数.对于每组选定的距离测量噪声水平,均对Fast-SLAM2.0、UFastSLAM 和提出SLAM 算法执行50次仿真实验.并以50次仿真的机器人路径RMSE 平均值(评估SLAM 估计精度)及标准差(评估SLAM 执行稳定性)和NEFF 平均值(及标准差)作为评价标准,实验结果如图2和图3所示.图2为三种算法的RMSE 均值及标准差对比.可以看出,随着测量噪声增长,三种SLAM 算法对车辆路径的估计误差及标准差都逐步增加.然而,提出SLAM 算法的RMSE 均值要低于另外两种算法,意味着提出方法的估计精度要优于另两者,同时提出SLAM 算法在不同观测噪声下的RSME 标准差增长速度也低于另外两种算法,说明其具有较好的运行稳定性.进一步地,这三种算法都避免了粒子集退化问题,当测量噪声较低时(如0.01m,0.03m,0.05m,0.07m,0.09m),SLAM 算法并没有出现较高的RMSE,其原因在于在这三种算法的重要性函数设计过程中均融入了当前环境观测信息.即便观测噪声较低、观测似然p (z k |x k )外形尖锐,经运动图2增长测量噪声条件下的3种算法RMSE Fig.2Performance of robot path estimation withincreasing measurement noise levels in range。

卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理所有滤波问题其实都是求感兴趣的状态的后验概率分布，只是由于针对特定条件的不同，可通过求解递推贝叶斯公式获得后验概率的解析解（KF、EKF、UKF），也可通过大数统计平均求期望的方法来获得后验概率（PF）。

1 KF、EKF、UKF1.1 定义KF、EKF、UKF 都是一个隐马尔科夫模型与贝叶斯定理的联合实现。

是通过观测信息及状态转移及观测模型对状态进行光滑、滤波及预测的方法。

而KF、EKF及UKF的滤波问题都可以通过贝叶斯估计状态信息的后验概率分布来求解。

Kalman在线性高斯的假设下，可以直接获得后验概率的解析解；EKF是非线性高斯模型，通过泰勒分解将非线性问题转化为线性问题，然后套用KF的方法求解，缺陷是线性化引入了线性误差且雅克比、海塞矩阵计算量大；而UKF也是非线性高斯模型，通过用有限的参数来近似随机量的统计特性，用统计的方法计算递推贝叶斯中各个积分项，从而获得了后验概率的均值和方差。

1.2 原理KF、EKF、UKF滤波问题是一个隐马尔科夫模型与贝叶斯定理的联合实现。

一般的状态模型可分为状态转移方程和观测方程，而状态一般都是无法直接观测到的，所以时隐马尔科夫模型。

然后，它将上一时刻获得的状态信息的后验分布作为新的先验分布，利用贝叶斯定理，建立一个贝叶斯递推过程，从而得到了贝叶斯递推公式，像常用的卡尔曼滤波、扩展卡尔曼滤波、不敏卡尔曼滤波以及粒子滤波都是通过不同模型假设来近似最优贝叶斯滤波得到的。

这也是滤波问题的基本思路。

所有贝叶斯估计问题的目的都是求解感兴趣参数的后验概率密度。

并且后验概率的求解是通过递推计算目标状态后验概率密度的方法获得的。

在贝叶斯框架下，通过状态参数的先验概率密度和观测似然函数来求解估计问题；在目标跟踪背景下（隐马尔科夫模型），目标动态方差决定状态转移概率，观测方程决定释然函数。

一般化的整个计算过程可以分为3步：01. 一步状态预测：通过状态转移概率及上一时刻的后验概率算出一步预测概率分布。

平方根容积卡尔曼滤波平方根容积卡尔曼滤波（Square Root Cubature Kalman Filter，SRCKF）是一种经典的非线性滤波算法。

本文将以生动有趣的方式，全面介绍SRCKF的概念、原理、应用以及其在实际工程中的指导意义。

首先，让我们来了解一下什么是卡尔曼滤波（Kalman Filter）。

卡尔曼滤波是一种常用的线性时变系统状态估计算法，其基本思想是通过最小化估计值与真实值之间的误差来获得对系统状态的最优估计。

然而，在很多实际应用中，系统常常呈现非线性特征。

为了处理这些非线性问题，研究者们提出了各种改进的非线性滤波算法，而SRCKF便是其中一种。

SRCKF是基于卡尔曼滤波的算法改进，它主要是针对非线性系统进行状态估计。

与传统的扩展卡尔曼滤波（Extended Kalman Filter，EKF）相比，SRCKF具有较好的收敛性和鲁棒性。

SRCKF的核心思想是将系统状态用均值和协方差矩阵表示，通过采用均方根信息滤波的方法来等效地更新状态。

与EKF不同的是，SRCKF采用了容积取样技术，通过选取合适的样本点，可以更好地描述系统的非线性特征。

SRCKF的工作原理可以简要地概括为以下几个步骤：首先，通过容积取样方法构造一组样本点，这些样本点将充分覆盖整个状态空间。

然后，在每个样本点上，利用非线性系统模型进行状态预测，并计算相应的权重。

接下来，通过对各个样本点进行权重加权平均，估计出系统的状态均值和协方差矩阵。

最后，根据测量值与估计值之间的差异，更新状态的估计。

SRCKF在实际工程中有广泛的应用。

例如，在无人驾驶汽车的导航系统中，SRCKF可以用于车辆位置和姿态的估计，提高导航的精度和稳定性。

另外，SRCKF还可以应用于航天器的姿态控制、机器人定位与导航、生物医学工程中的生理信号分析等领域。

总结一下，平方根容积卡尔曼滤波是一种非线性滤波算法，通过容积取样和均方根信息滤波等技术，能够较好地处理非线性系统状态估计问题。

自适应平方根球型无迹卡尔曼滤波算法
自适应平方根球型无迹卡尔曼滤波算法（Adaptive Square-Root Cubature Kalman Filtering Algorithm）在近年来受到了互联网行业的广泛关注。

这种算法归结为：一种利用滤波理论从一系列混合系统动态中估计和提取信息的空间序列分析技术。

它有效解决了复杂系统动态状态参数估计问题，可以有效应用在大数据环境中，进而提升了信息提取和处理的准确性以及效率。

算法的优势体现在数据处理的准确性、可靠性与时效性的发挥上。

由于算法的
自适应特性，估计目标状态参数的准确性得到了很大提升，可以更有效的发挥数据的价值，从而更好的满足用户的需求。

此外，无迹的特性也使得整体数据处理过程具备更强的鲁棒性，对于复杂系统可以获得更为稳定的参数估计，能够很好地抵抗外部噪声干扰。

此外，算法的弹性也得到了极大加强，它可以很好地应对复杂系统动态的变化，而且具有更强的自适应性，能够有效的去除虚假的数据，并根据新的条件快速调节状态参数，从而大幅提高数据处理的速度与准确度。

通过上述分析，自适应平方根球型无迹卡尔曼滤波算法在互联网行业中有非常
重要的应用价值。

在大数据环境下，借助该算法，能够更加快速且准确地处理复杂系统动态状态参数变化，从而有效地促进了信息提取与处理的发展，有助于提高用户体验，实现互联网行业的健康发展。