FastSLAM for mobile robot based on box particle filter with intelligence optimization
-
摘要:
针对传统FastSLAM算法需要大量粒子构建地图导致计算复杂度高、难以提高估计精度等问题,研究构建了一种基于智能优化箱粒子滤波(IOBPF)的移动机器人FastSLAM算法。首先,将萤火虫算法(FA)的动态寻优机制引入箱粒子滤波(BPF),建立了箱粒子的荧光亮度更新公式、吸引度计算公式和位置更新公式,使箱粒子集智能化地向高似然区域移动,避免了箱粒子的退化现象。然后,以改进的智能优化箱粒子滤波进行机器人位姿估计,并采用扩展区间卡尔曼滤波(EIKF)完成地图的构建和更新。移动机器人的模型仿真和实体实验结果表明:所提智能化FastSLAM算法可有效提升箱粒子的性能,并降低地图构建所需粒子数,从而显著提高FastSLAM的定位精度和地图构建的鲁棒性。
-
关键词:
- 同步定位与地图构建 /
- 移动机器人 /
- 箱粒子滤波(BPF) /
- 萤火虫算法(FA) /
- 扩展区间卡尔曼滤波(EIKF)
Abstract:The traditional FastSLAM algorithm requires a large number of particles to build the map, thus resulting in high computational complexity and difficulty in improving the estimation accuracy. In view of these problems, an algorithm of FastSLAM for mobile robot is presented based on box particle filter with intelligence optimization (IOBPF). First, the dynamic optimization mechanism of firefly algorithm (FA) is applied to the box particle filter (BPF), and the formulas of fluorescence brightness updating, attraction calculation and position updating of box particle are constructed, which makes the box particles move toward the high-likelihood region intelligently and avoid the phenomenon of box particle degeneracy. Then, the improved BPF with intelligence optimization is utilized to estimate the pose of robot, and the extended interval Kalman filter (EIKF) is employed to complete the map building and updating. The results of model simulation and entity experiment of mobile robot show that the intelligent FastSLAM algorithm in this paper can effectively improve the performance of box particles and reduce the number of particles required for map construction, thus significantly improving the positioning accuracy and robustness of map construction.
-
随着人工智能和机器人技术的高速发展及其在各行各业的推广应用,人们对于拥有自主作业能力的智能机器人的需求日益增加,而同步定位与地图构建(simultaneous localization and mapping, SLAM)是实现机器人真正智能化的关键支撑技术。目前,针对SLAM的求解途径大致可归纳为基于最优滤波的方法和基于图优化的方法[1]。其中,基于最优滤波的方法主要依据递归贝叶斯状态估计理论,主要包括基于扩展卡尔曼滤波(extended Kalman filter, EKF)的方法和基于粒子滤波(particle filter, PF)的方法。对于在EKF架构下解算的SLAM算法,环境特征的感知信息会随着机器人的运动持续地加入到联合状态特征向量及其协方差矩阵中,其主要问题在于对特征点深度和协方差矩阵的初始值依赖较大,如果设置欠妥,则会使得估计结果不一致,甚至导致算法不收敛[2-3]。为此,Doucet等[4]根据Rao-Blackwellized粒子滤波(Rao-Blackwellized particle filter, RBPF)的分解思想提出了RBPF-SLAM,该方法可以有效解决地图构建过程中的数据关联问题,并且可以根据实际应用需要构建出不同类型的地图。随后,Montemerlo等[5]又提出了FastSLAM算法,并通过进一步改进和优化提出了更加完善的版本FastSLAM 2.0[6],但该算法需要采样足够数量的粒子才能正确完成地图构建,由于每个粒子都存储了一张周围环境的完整地图,同时又包含了机器人轨迹估计的结果,需要占用大量的计算资源和存储空间。因此,FastSLAM类算法面临的主要挑战是如何尽可能减少采样所需的粒子数。为了解决算法存储量过大的问题,Eliazar和Parr[7]研究并提出了一种基于分布式粒子的SLAM算法,大幅度降低了算法占据的存储空间。为了减少地图构建过程中需要采样的粒子数,通常采取将当前时刻的观测与已构建的地图进行扫描匹配,再将匹配结果替代误差较大的里程计读数,进而改善粒子采样的提议分布[8-10]。
然而,这类基于概率的SLAM算法中都存在一个共同的缺陷,即滤波一致性问题。Bailey等[11]研究发现,RBPF-SLAM在足够数量粒子条件下可以构建出较为准确的地图,但其滤波结果的一致性仅在较短时间内满足要求。而此问题如果采用区间分析(interval analysis, IA)技术则可以得到有效地解决[12]。这是由于区间分析所针对的运算对象是区间量,当浮点数受到四舍五入运算的影响产生误差时,利用区间运算却能得到包含精确解的严格区间界限,因此可以提供有界的结果,保证了滤波的一致性。Gning等[13]将区间运算的特点与传统PF不受系统模型约束的性能优势相结合,提出了一种新颖的非线性滤波算法,称为箱粒子滤波(box particle filter, BPF),其核心思想是: 将传统的点粒子和系统误差统计模型分别取代为区间箱粒子和系统误差界限模型,再进行贝叶斯滤波。相比于传统PF,其最大的性能优势在于所需采样的箱粒子数少且运行速度快,因而算法的运算复杂度低、实时性好。在目标跟踪领域的一些应用中[14-15],BPF只需采样数十个箱粒子就可以达到与传统PF采样数千粒子所获得的滤波精度和可靠性。
基于以上分析,本文在FastSLAM算法架构基础上,充分利用BPF所需粒子数少、运行速度快的性能优势,针对BPF中的箱粒子退化现象,综合运用萤火虫算法(firefly algorithm, FA)[16]的寻优机制和IA技术,创立高性能的智能优化BPF算法,使得箱粒子集朝着机器人真实轨迹靠近,进而提高移动机器人位姿估计和同步定位的精度。结合扩展区间卡尔曼滤波(extended interval Kalman filter, EIKF)[17]完成环境地图的构建和更新,进而增加路标协方差矩阵预测和更新的精度和鲁棒性。
1. 区间分析
在区间分析[18]中,一维闭合区间定义了实数域中一个连续、封闭的子集:
(1) 式中: x和x分别为区间[x]的下界和上界。
如果将上述区间的定义推广至Rn维实数空间,则n维区间向量,或称n维箱粒子[X]定义了n个一维闭合区间的笛卡儿积。
(2) 图 1展示了一个二维实数域中的箱粒子,其基本数学定义如表 1所示。
表 1 二维箱粒子的基本数学定义Table 1. Basic mathematical definition of two-dimensional box particle参数 数学表示 宽度 中心 范数 体积 一般情况下,Rn空间中的箱粒子[X]经过非线性函数f映射后所得到的f([X])为非规则的箱体形状。为了便于计算和分析,使得[X]在非线性函数的映射作用后得到规则的箱体形状,区间分析定义了包含函数(inclusion functions),即已知函数f: Rn→Rm,则其对应的区间函数[f]: IRn→IRm是包含函数,如果满足:
(3) 式中: IRn为n维实区间的集合。
对于各种类型的函数f,区间分析的一个主要目的就是提供合理有效的包含函数[f],使得[f]([X])在包含所有可能解的同时其区间所含范围不太大,这样就可以很大程度上减少算法的计算量,提高收敛速度。为此,区间分析引入了区间约束(interval constraint)的概念,即利用某种特定算法对原区间进行约束,也就是解决约束满足问题(constraint satisfaction problem, CSP),其定义为:在n维实数域上,实值约束函数g(x)=g(x1, …, xn)=0的约束集为
(4) 式中: g(x)=(g1(x), g2(x), …, gm(x))T,x=(x1, x2, …, xn)。则CSP的解集为
(5) 在式(4)中,H的本质就是求解出一个包含[X]中所有x,并满足约束函数g(x)的具有最小冗余的箱粒子[X]′,进而利用[X]′代替[X],即S⊆[X]′⊆[X]。在BPF中,箱粒子的区间收缩一般采用前向后向传播算法,也称约束传播(constraints propagation, CP)算法或CP收缩子,该算法的特点是实现起来简单,而且在高冗余数据和约束情况下,具有较高的执行效率[13]。
2. 智能优化箱粒子滤波
2.1 箱粒子滤波算法
BPF充分结合了序列蒙特卡罗(sequential Monte Carlo, SMC)方法和IA技术,其核心思想是:利用在状态空间中随机采样的具有一定体积的箱粒子代替常规的点粒子,再利用系统误差界限模型代替系统误差统计模型对加权箱粒子集进行序贯迭代。在区间分析的框架中,k时刻系统状态向量xk和观测向量zk分别表示为区间状态向量[Xk]∈IRnx和区间观测向量[Zk]∈IRny。同理,状态转移函数f和观测函数h分别表示为包含函数[f]和[h],则系统状态方程为
(6) 式中: [Vk]和[Wk+1]分别为状态转移噪声和观测噪声箱粒子;[Uk]为控制输入箱粒子。
Gning等[19]在贝叶斯滤波框架下将BPF视为采用混合均匀概率密度函数(probability density function, PDF)的滤波算法,即将每个箱粒子内部都拟合为一个以该箱粒子为支撑集的均匀PDF,每个PDF很好地描述了对应箱粒子的状态特性。因此,若令U[X]表示以箱粒子[X]为支撑集的PDF,则随机变量x可表示为一组均匀PDF的加权和。
(7) 式中: N为混合均匀PDF的数目;[Xi]为第i个箱粒子;
。基于上述分析,根据贝叶斯滤波估计原理,BPF的迭代步骤如下:
1) 预测。在k时刻,假设状态xk的PDF可表示为
(8) 则时间更新步骤如下:
(9) 假设k+1时刻的状态转移噪声对应的箱粒子为[Vk],考虑包含函数[f],对于∀i=1, 2,…, N,如果xk∈[Xki],则有xk+1∈[f]([Xk]+[Uk])+[Vk],因此存在如下关系:
(10) 由式(10)可知,每一个概率密度积分项
都可以近似为一个以 为支撑集的均匀PDF分量进行模拟,即(11) 因此,预测概率密度可近似为
(12) 式(12)表明,k+1时刻预测状态PDF可用N个权值为ωki,支撑集箱粒子为[Xk+1|ki]的均匀PDF加权和近似。
2) 更新。假设状态的似然函数p(zk+1xk+1)可用一组以箱粒子为支撑集的均匀PDF加权和近似。为了简单考虑,此处可认为是单个均匀PDF,即
(13) 令k+1时刻观测噪声为wk,则k+1时刻实际观测箱粒子[Zk+1]包含了所有h(xk+1)+wk。因此,观测更新为
(14) 式中:
为归一化系数,且每一项Ψi都是常数函数,其支撑集为(15) 事实上,式(15)定义了一个CSP,即预测箱粒子[Xk+1|ki]可通过观测函数h与实际观测箱粒子[Zk+1]的关系进行约束,以消除原箱粒子中多余的部分。因此,利用收缩子对[Xk+1|ki]进行收缩,并将约束后得到的新的箱粒子记为
,这样就可以利用这组箱粒子来拟合k+1时刻状态的后验概率p(zk+1|xk+1)。根据式(15)定义的支撑集集合,定义
为包含SSΨi的最小箱粒子,即 ,根据均匀PDF的性质及收缩子具有的收缩性和完备性,可得(16) 结合式(14)和式(16),进一步得到
(17) 因此,状态后验PDF为
(18) 根据式(18),箱粒子的似然为
(19) 式中: |[X]|为箱粒子的体积;b为状态观测维数。
因此,箱粒子的权重更新为
,进而状态的后验概率p(xk+1|z1, k+1)可用一组加权箱粒子 拟合,i=1, 2, …, N。3) 重采样。针对BPF中的箱粒子退化现象,通常采取计算有效样本
的大小,并选择一个阈值Nth,如果Neff < Nth,则执行随机子划分重采样,即根据需要重采样的次数,将当前时刻得到的箱粒子随机地选取其某一维状态子区间进行均匀的划分,使得箱粒子能够保持一个合适的尺寸。该方法不但保证了当前时刻的箱粒子集能够顺利进入下一时刻的迭代,而且也有效去除了箱粒子内部的冗余区域。然而,该方法对箱粒子的子区间进行选取是随机的,这会导致每次实验的结果都不同,因此反复执行随机子划分重采样对滤波精度影响较大。2.2 萤火虫算法智能优化箱粒子滤波
尽管BPF在许多实际的应用中被证明是有效的,但对于高非线性系统和观测误差较大的情况下,其滤波精度不高[20]。作为一种新的非线性滤波算法,BPF的发展时间较短,理论体系还需进一步完善,算法的性能还有进一步提高的空间,其应用方面则主要集中在目标跟踪领域。针对箱粒子的退化现象,考虑结合FA的寻优机制对箱粒子集的空间分布进行智能优化,以改善BPF的滤波性能。
FA[16]是受自然界中的萤火虫通过荧光进行信息交流的群体行为启发而演变得来的。萤火虫的荧光亮度和萤火虫间的吸引度是FA的2个主要因素。其中,荧光亮度用于表示萤火虫所处当前位置的优劣程度,并决定了其下一时刻的运动方向;吸引度表示萤火虫之间的相互作用,并决定了萤火虫移动的距离。FA就是通过对荧光亮度和吸引度的不断迭代,使得萤火虫群体智能地向状态空间内更好的区域运动,进而实现目标的优化。
FA作为一种新型仿生群智能优化算法,其最大的优势在于在工程上容易实现,需要调整的参数少,具有较好的收敛速度和收敛精度。文献[21]将FA的优化机制应用于PF,提高了PF的估计精度,降低了状态估计所需的粒子数。因此,本文考虑结合BPF的运行机制和FA的寻优机制,根据最新观测利用区间分析定义箱粒子的荧光亮度计算公式及空间位置更新公式,进而模拟萤火虫进行觅食或交流的个体行为,使得箱粒子智能地向全局范围内更优的位置移动,这样就可以提高箱粒子集的整体质量,克服箱粒子的退化问题,避免了反复执行随机子划分重采样。
1) 箱粒子的荧光亮度更新公式。在标准FA中,当前时刻的每个萤火虫都需要和其他位置的萤火虫对比荧光亮度值大小,萤光亮度值低的萤火虫会被萤光亮度值高的萤火虫吸引,并朝着萤光亮度值高的萤火虫所处的空间位置移动。因此,萤火虫的相对荧光亮度更新公式定义为
(20) 式中: Im和γ分别为萤火虫的最大萤光亮度和光强吸收系数;dij为萤火虫i与j的空间欧氏距离。
如果直接将式(20)的更新机制应用于BPF,则将会进一步增加算法的运算复杂度。为了确保滤波精度,萤火虫在自适应迭代寻优过程中需要考虑最新的观测。而在区间分析的框架下,其关键思想就是求出当前时刻的预测观测和实际观测在多维状态空间中的“交”。因此,构造箱粒子荧光亮度的计算公式为
(21) 式中: I([Xk+1i])和[Zk+1i]分别为k+1时刻第i个箱粒子的荧光亮度和预测观测;[Zk+1]为实际观测。
根据式(21),由于每个时刻的实际观测值仅有一个,利用实际观测值和每个箱粒子的预测观测值进行对比,代替每个箱粒子之间荧光亮度值的对比,就可有效减小算法的运算复杂度。
2) 箱粒子的吸引度计算公式。在标准FA中,萤火虫的吸引度公式为
(22) 式中: βm为最大吸引度,一般设置为[0.8, 1]内的常数。
如果萤火虫的吸引度越大,则其越容易吸引附近的萤火虫向自身所处的位置运动,这样一来,就会使得所有箱粒子都集中在状态的真实值附近,造成箱粒子的多样性丧失。基于上述分析,根据区间分析中2个中心区间距离的定义,给出k+1时刻箱粒子[Xk+1i]与全局最优箱粒子[gk+1]之间的空间欧氏距离为
(23) 式中: ||·||2为2-范数。
在箱粒子荧光亮度更新公式基础上,进一步考虑在吸引度公式中加入一个随机权值项,这样就能改变箱粒子的移动信息,既能让箱粒子集在高似然区域合理的分布,又可以在一定程度上增强箱粒子的多样性,确保了滤波过程的持续运行。因此,构造箱粒子间的吸引度为
(24) 式中: N(0, 1)为均值为0、方差为1的高斯分布随机向量;(1-ωki)·N(0, 1)为随机权值项。
当完成位置更新之后,需要计算并对比箱粒子的荧光亮度值,更新全局最优箱粒子:
(25) 3) 箱粒子的位置更新公式。在标准FA中,萤火虫i被萤火虫j吸引后移动的位置更新公式为
(26) 式中: xi和xj分别为萤火虫i和j的空间位置;rand表示某个随机数,且服从均匀分布U(0, 1);μ∈[0, 1]为步长因子。
如果将式(26)的更新策略直接应用于BPF,则需要用箱粒子i和其余的箱粒子j进行交互运算,这将对算法的实时性带来不利影响。为此,考虑利用每个箱粒子与全局最优箱粒子间的对比来替代箱粒子间的相互对比,利用每个箱粒子中心位置的更新使每个箱粒子的位置更新,这会提高FA的全局寻优能力,且该阶段的运算复杂度将由O(N2)减少至O(N)。基于上述分析,构造箱粒子的位置更新公式为
(27) 由式(27)可知,箱粒子的随机步长随di变化,且2di(rand-1/2)∈[-di, di]。采用上述位置更新策略后,当箱粒子间的空间位置相距较远时,箱粒子位置更新过程中吸引度所发挥的引导作用相对较弱,而箱粒子自身可以在区间[-di, di]内随机自主的运动,这样就使得算法能够搜索较大的状态空间;当箱粒子之间的空间欧氏距离较近时,箱粒子在状态空间中的自主探索能力和移动范围随着空间欧氏距离di的减小而减弱,而此时箱粒子在位置更新过程中,吸引度起到的主导作用将逐渐增大,进而智能地引导箱粒子朝着全局最优的箱粒子所在位置移动。
通过以上优化策略,利用FA智能优化引导箱粒子集朝着高似然区域移动,可以有效克服箱粒子的退化现象,并且没有增加额外的操作和参数,保持了算法的简单性。另外,考虑到FA本身具有较强的收敛能力,如果迭代次数过多,则会造成算法的计算复杂度增高,而且利用FA智能优化的目的是为了使得箱粒子集朝着似然度高的区域移动,不需要收敛到最优值,否则会造成箱粒子的多样性丧失。因此,通过设定最大迭代次数,保证算法的实时性和箱粒子多样性,即当荧光亮度函数值大于设置的迭代终止阈值时,算法停止迭代,否则继续执行迭代,直至到达最大迭代次数。当算法迭代至设定阈值时,表明箱粒子已经较好地分布在状态的真实值周围。
3. 移动机器人FastSLAM方案设计
FastSLAM算法的核心在于运用RBPF思想对SLAM问题后验概率进行分解,其实现了地图规模和状态估计之间的有效解耦。具体说来,就是将SLAM问题分解成为机器人路径x1:k估计和基于该路径估计的地图M创建2个子问题,即
(28) 式中: z1:k={z1, z2, …, zk}和u1:k={u1, u2, …, uk}分别为观测序列和控制输入序列;mi为第i个环境路标的位置估计;n1:k={n1, n2, …, nk}为数据关联的相关一致性。
根据SLAM的动态贝叶斯图模型,当已知机器人的运动轨迹x1:k时,对其周围环境地图M中存在的N个特征进行估计是相互独立的。因此,FastSLAM采用PF估算机器人所有可能的轨迹后验,其中的每个粒子都包含一份机器人完整的潜在地图。然而,为了提高估计精度,就需要增加描述后验概率分布所需的粒子数,这对于FastSLAM算法来说代价巨大,会增加算法的复杂度。此外,反复的重采样会造成粒子有效性和多样性的损失,导致出现粒子退化现象,使得算法的性能下降,甚至失效;另一方面,在所得路径估计基础上,利用常规EKF对地图特征估计存在计算量过大、精度不高甚至发散等问题。
因此,考虑将改进的智能优化箱粒子滤波(IOBPF)用于实现移动机器人位姿估计,并在此基础上利用EIKF完成地图构建与更新,为了表示方便,简记为IOBPF-SLAM算法。其中,k时刻第i个箱粒子的结构形式如下:
(29) 式中: ([x], [y], [θ])ki为k时刻对机器人的位姿估计;
为第i个箱粒子对第j个路标的位置估计,其不确定性协方差为 ;[Zk]为k时刻的实际观测;第j个路标描述为mj=(mxj, myj)。3.1 移动机器人同步定位
在区间分析框架下,移动机器人系统的运动模型表示为
(30) 式中: k时刻的输入区间向量[Uk]由机器人的基本位移[Δdk]和基本旋转角[Δθk]组成,即[Uk]=([Δdk],[Δθk])T,([x]×[y])T为机器人在全局坐标系下的位置,[θ]为机器人运动方向。
机器人系统的距离-角度观测模型为
(31) 式中: 观测区间向量[Zk]由距离观测区间量[rk]和角度观测区间量[φk]组成,即[Zk]=([rk],[φk])T。
假设k时刻的箱粒子集为
,i=1, 2, …, N,控制输入为[Uk],则k+1时刻的预测箱粒子为(32) 预测观测箱粒子为
(33) 对于环境中设定的路标,根据k+1时刻的实际观测[Zk+1],每个箱粒子对应的荧光亮度计算如下:
(34) 进而,利用式(24)计算箱粒子的吸引度,并利用式(27)和式(25)分别更新箱粒子的位置和全局最优箱粒子。如果预测观测箱粒子与实际观测箱粒子的交集为空,即I([Xk+1i])=Ø,则[Xk+1i]保持不变;如果I([Xk+1i])≠Ø,则利用CP收缩子对[Xk+1i]进行收缩,得到新的箱粒子
。根据系统观测模型,每个箱粒子的似然计算如下:(35) (36) 则每个箱粒子的权重更新如下:
(37) 归一化权重为
(38) 因此,k+1时刻的状态估计值为
(39) 在具体应用中,可将权值最大的箱粒子的中心近似为状态估计值。另外,针对估计值
,其对应的状态估计协方差为(40) 式中:
为将 作为状态估计时的置信度大小。3.2 环境地图构建
Chen等[17]将区间分析引入卡尔曼滤波,将噪声模型和系统模型的误差用区间范围来表示,提出了一种新的区间卡尔曼滤波(interval Kalman filter, IKF)算法,其迭代公式与标准卡尔曼滤波的结构形式完全相同,只是运算变量为区间向量或区间矩阵,估计结果为2条边界曲线,最终结果将2个边界值取平均或加权处理获得。由于区间运算保证了结果的完备性,在一些实际应用中[22-23],IKF相比于卡尔曼滤波展示出了更好的鲁棒性和精度。
对于区间非线性系统,结合IKF和EKF便可进一步得到EIKF的迭代公式。为了下文表示方便,利用上标“I”表示区间量,即[Xk]表示为XkI,状态转移包含函数[f]和观测包含函数[h]分别表示为fI和hI,ΣkI为状态估计的区间协方差矩阵,QkI和RkI分别为系统噪声区间协方差矩阵和观测噪声区间协方差矩阵,则EIKF算法的递归过程如下。
1) 状态初始值。
(41) 2) 计算区间雅可比矩阵。
(42) 3) 预测。
(43) (44) 4) 计算新息和区间卡尔曼增益。
(45) (46) 5) 更新。
(47) (48) 值得说明的是,EIKF中的实际观测zkI在被观测之前是一个不确定的区间矢量,在被观测之后就是一个普通的矢量[24]。
常规FastSLAM算法利用EKF更新地图,但需要对非线性的模型进行线性化。由于机器人系统具有强非线性,线性化只能精确到泰勒级数的一阶,造成估计精度下降甚至导致滤波发散。因此,本文利用EIKF取代EKF完成环境特征估计。根据FastSLAM的解算过程,如果机器人的运动轨迹已知,则对各个特征进行估计彼此是相互独立的,而k+1时刻是否对第j个环境特征进行更新取决于k时刻第j个特征是否被传感器观测到。从而,根据传感器获得的最新观测完成数据关联后,需考虑以下情况:
1) 如果某个路标是第一次被机器人观测到,则其均值
和协方差 初始化为(49) (50) (51) 2) 如果机器人最新观测中包含环境地图中的第j个路标,则其均值
和协方差 利用EIKF更新如下:(52) (53) (54) (55) (56) (57) 3) 如果机器人最新的观测没有包含环境地图中的第j个路标,则其均值和协方差不变。
(58) 4) 如果机器人再也没有观测到周围环境的任何特征,则将前一时刻的地图信息作为当前时刻的地图信息。
在上述EIKF的递推式中,增益矩阵的计算涉及区间矩阵的求逆,而这类求逆问题往往采用迭代的方式求解,算法比较复杂,且耗时长,不利于实时性。因此,为了减少计算量, 进一步提高算法的实时性,本文采用结合Hansen算法[25]和上边界矩阵的区间矩阵求逆方法,具体见算法1。
算法1 区间矩阵的求逆算法。
输入:区间矩阵
。输出:区间矩阵的逆(BI)-1。
步骤1 计算区间矩阵BI的中心矩阵Bmid=[(bij+bij)/2]及其逆矩阵(Bmid)-1。
步骤2 计算
及其范数 。步骤3 如果
,计算上边界矩阵BuI及其逆矩阵(BuI)-1,则(BI)-1≈(BuI)-1;否则,定义区间矩阵ΓI,并且ΓI中的每一个元素都属于[-c, c],满足 ,则 。综合上述,IOBPF-SLAM算法的整体结构如图 2所示。
4. 仿真实验与结果分析
4.1 仿真实验对比分析
为了验证本文算法的性能,根据Bailey[26]开发的SLAM算法通用模拟器设计了尺寸为180 m×80 m,并具有60个路标特征的环境地图及机器人运行参考轨迹,分别对FastSLAM 2.0、UFastSLAM[27]、STSRCDKF-SLAM[28]、基于标准BPF算法的BPF-SLAM及IOBPF-SLAM开展一系列仿真实验并对相关性能进行对比分析。仿真实验在MATLAB 2014a环境下运行,区间运算利用INTLAB 9.0工具箱。计算机的配置型号为:CPU Intel®CoreTMi5-5200U,主频2.20 GHz,内存8 GB。
仿真中,Np和Nb分别表示粒子数和箱粒子数。对于FA群智能优化步骤,光强吸收系数γ=1,最大吸引度βm=0.85,迭代终止阈值设置为0.02。采用独立兼容最近邻(individual compatibility nearest neighbor, ICNN)算法完成地图构建过程中的数据关联。机器人的初始位置设定为全局坐标系的(0, 0)点,并以1.5 m/s的速度运行,其余仿真参数设置如表 2所示。根据区间观测的不确定性,传感器观测表示为
(59) 表 2 仿真参数设置Table 2. Simulation parameter setting参数 设定值 机器人最大转向角 ±30° 机器人最大转向角速度 ±2°/s 机器人轴间距 2 m 传感器的最大观测范围 20 m 传感器的观测频率 50 Hz 控制时间间隔 0.02 s 控制噪声 (0.35 m/s, 2°) 观测噪声 (0.3 m/s, 3°) 式中: 区间长度Δ=[2.5, 2.5]T。
图 3展示了5种SLAM算法在模拟环境中的估计结果。可以看出,UFastSLAM利用无迹粒子滤波(unscented particle filter, UPF)计算机器人状态的不确定性,获得了更加精确的方差,从而改善了估计精度。但随着机器人的持续运动,新的环境特征将不断加入状态向量中,由此引起尺度无迹变换(scaled unscented transformation, SUT)的维数逐渐增加,导致机器人在运动一段距离后,其轨迹估计便开始出现了明显的误差。而STSRCDKF-SLAM利用强跟踪平方根容积卡尔曼滤波,一方面融合了平方根策略,使得其协方差矩阵具备对称性,有效减小了由于系统发生突变导致滤波发散而带来的误差;另一方面,在滤波增益矩阵中加入渐消因子对其进行实时调整,动态自适应地调节数据的权值,进而改善了滤波精度。BPF-SLAM由于采用IA技术,相比于FastSLAM 2.0在一定程度上提高了估计精度,但由于常规BPF面对箱粒子退化现象需要执行反复地进行随机子划分重采样,也出现了一定的误差,而IOBPF-SLAM估计路径与参考轨迹吻合度更高,相对应的路标估计也更为准确。
为了验证算法在增长观测噪声条件下的误差性能,设置环境特征的角度观测噪声为2°,距离观测噪声分别为0.1, 0.3, 0.4, 0.55, 0.7, 0.8, 1.0, 1.2, 1.35, 1.5 m进行仿真实验。图 4展示了上述5种SLAM算法在开展了30次仿真实验后的均方根误差(RMSE)均值及标准差的比较情况。可以看出,5种SLAM算法的轨迹估计误差及标准差随着观测噪声的增长逐渐增加。然而,IOBPF-SLAM算法由于采用FA智能优化,提高了箱粒子集的整体估计效能,并提供了全局一致的结果,因此其RMSE均值和标准差增长速度要低于其他算法。这意味着IOBPF-SLAM的估计精度相较于其他几种算法更好,并具有较好的运行稳定性。
为了在上述增长观测噪声条件下验证IOBPF-SLAM能有效避免粒子退化,对于N次仿真实验,利用有效粒子百分比进行评估。
(60) 在开展了30次仿真实验后,从图 5可以看出,IOBPF-SLAM的有效箱粒子百分比均高于82%,这意味着在算法执行过程中,最多仅有18%的箱粒子没有发挥作用,然而这部分箱粒子主要是为了保持箱粒子的多样性,这说明了FA智能优化的有效性。
Gning等[19]指出,如果BPF的滤波结果为状态的最优估计,则需要满足:①真实状态向量x必须包含于后验PDF的所有支撑集区域内;②状态后验PDF的支撑集区域体积应该为最小。针对条件①,Gning等[15]进一步指出了包含准则ρk,并说明了当不满足此条件时,该滤波器不收敛。为了解释ρk,需要利用置信集的概念。在BPF中,一般采用所有支撑集箱粒子的“并集”来表示置信集Ck(1),即
(61) 因此,包含准则ρk可由如下计算得到:
(62) 如果满足ρk=1,则表明在所有以[Xki]为支撑集的后验PDF内已经包含了状态的真实值。从图 6可以看出,相比于标准的BPF,IOBPF运用FA优化机制引导箱粒子集智能地向高似然区域移动,使该箱粒子集更好地满足了ρk=1,因此提高了滤波性能。
对于一个滤波算法而言,如果其估计误差满足:①均值等于零;②小于或等于其计算所得的协方差,则称该滤波算法是一致的。如果一个滤波算法不一致,则其滤波精度就具有不确定性,这会导致算法估计结果不可靠。Bailey等[11]分析并阐述了粒子退化现象是造成FastSLAM算法不一致的主要原因,并进一步提出了利用归一化估计方差(normalized estimation error squared, NEES)准则来验证算法是否满足一致性。具体而言,假设xk*为k时刻机器人的真实状态,且该状态的维数为d,并假设
为其估计值和方差,则NEES计算如下:(63) NEES本质上描述的是一种加权距离,一般需要开展N次蒙特卡罗实验,然后采用平均NEES进行检验,即
(64) 显然,
服从自由度为Nd的χ2分布,则判断算法的一致性就可转化为统计学中的检验问题,因此算法符合一致性需要满足如下关系:(65) 式中:给定显著性水平α=0.05,并将双边概率为95%的区域作为一致性区域时,
的界定区间为[2.19, 3.93]。因此,为了评估上述5种SLAM算法的一致性,分别开展30次仿真实验,结果如图 7所示,其中红色的实线和红色的虚线分别表示一致性测试的下限和上限。可以看出,UFastSLAM采用无迹变换,避免了由于线性化带来的误差,其一致性优于FastSLAM 2.0;而STSRCDKF-SLAM改进了提议分布,并采用自适应重采样有效保持了粒子的多样性,因此其一致性保持更为持久。相比起来,IOBPF-SLAM在区间分析基础上结合FA的寻优机制,构造了箱粒子集的智能优化策略,因此提供了有界的、更为一致性的结果。
通过对不同SLAM算法的计算机CPU运行时间来对比不同粒子数条件下的SLAM算法计算复杂度。对于每组设定的粒子数,分别开展30次仿真实验,其平均计算复杂度如图 8所示。可以看出,上述5种SLAM算法的时间复杂度随着粒子数的增长逐步增加,FastSLAM 2.0的运算复杂度与粒子数呈近似线性关系。在使用相同粒子数的条件下,由于涉及区间运算和群智能优化过程,提出算法的复杂度要高于其他算法,但由上述图 3的运行结果可知,IOBPF-SLAM利用10个箱粒子所获得的估计精度就已经超过前3种基于PF的SLAM算法。因此,为获得较高的估计精度,IOBPF-SLAM的计算代价相对最低,算法执行所需时间更短。
4.2 移动机器人测试结果与性能评价
为测试IOBPF-SLAM的实际应用效果,利用装备Raspberrypi-3b控制主板和SLAMTEC激光雷达的差分驱动轮式移动机器人(RPLIDAR-A1)分别针对GridSLAM[8]、GMapping[9]、BPF-SLAM和IOBPF-SLAM开展对比实验。实验利用一台上位机笔记本电脑在Ubuntu14.04下安装机器人操作系统(ROS),利用teleop_twist_keyboard软件包控制机器人运动,并结合占用概率地图构建方法[29],采用栅格地图来建立环境模型,通过加载图形可视化工具Rviz显示地图的构建过程。实验场景如图 9所示。
实验中,移动机器人运动速度为0.4 m/s,最大转向速度为0.3 rad/s,控制频率为40 Hz,激光雷达最大测距范围为6 m,并具有360°全方位扫描能力。环境中设置了30个路标点及2个纸箱作为障碍物,生成栅格地图分辨率为25 cm2/cell,粒子数目设置为Np=10,Nb=10。由图 10的地图构建过程可以看出,当采用10个粒子时,GridSLAM无法正常完成地图构建,而其余3种算法均能完成二维栅格地图构建。
为了定量分析实验结果,表 3比较了在正确构建出环境地图条件下4种算法的性能参数。可以看出,GridSLAM需要100个粒子才能正确完成地图构建,而GMapping在RBPF的基础上改进了采样提议分布并采用选择性重采样策略,有效减少了所需粒子数,同时防止了粒子退化现象,是目前最有效的栅格地图构建方法,但仍然需要30个粒子才能完成地图构建。相比起来,BPF-SLAM和IOBPF-SLAM使用更少的粒子数就能完成地图构建,从而提高了SLAM系统的实时性。
表 3 不同SLAM算法的性能比较Table 3. Comparison of performance among different SLAM algorithms算法 粒子数 CPU时间/s 运行时间/s GridSLAM 100 0.873 318.72 GMapping 30 0.401 261.37 BPF-SLAM 15 0.325 223.41 本文算法 10 0.337 247.69 另外,2种基于区间箱粒子的SLAM算法主要的时间消耗在于区间运算,且每个箱粒子都存储并保持自己的栅格地图,对每个箱粒子进行FA智能优化就意味着涉及整个地图的处理,因此,IOBPF-SLAM的运行时间相对于BPF-SLAM要长一些,但其所需粒子数更少,精度更好。图 11进一步展示了IOBPF-SLAM的轨迹估计结果和设定路标的估计结果。移动机器人从“起点”位置(0, 0)开始沿着黑色参考路径以蓝色箭头方向顺时针运动一个回环,基于改进的FA智能优化箱粒子所估计得到的运动轨迹能正确跟随由参考点定义的轨迹,并且所有的参考点都包含在定位箱粒子中,因此提供了一致的结果。
图 12比较了实验中不同SLAM算法对于设定路标的位置估计误差。可以看出,移动机器人的整个运动过程中,在改进的智能优化BPF-SLAM估计所得的潜在路径基础上,IOBPF-SLAM采用EIKF对环境特征进行估计,虽然有少数几个路标点的位置估计误差大于GMapping,但IOBPF-SLAM对整个环境中设定路标的估计精度更高。
图 13的地图构建结果进一步展示了IOBPF-SLAM生成的二维栅格地图与真实环境一致,其边缘清晰,没有重叠或扭曲,并且在整个过程中程序运行平稳,因此是可行有效的,可为工程应用提供参考依据。
5. 结论
1) 本文算法将区间分析引入移动机器人FastSLAM算法架构中,利用BPF取代传统PF,并通过利用FA的动态寻优机制智能地引导箱粒子集朝着移动机器人的真实轨迹靠近,该策略没有增加额外的操作和参数,保持了算法的简单性。
2) IOBPF-SLAM可有效解决传统FastSLAM算法需要大量粒子构建地图的问题, 同时克服了箱粒子的退化,避免了反复执行随机子划分重采样,因此在粒子数较少的情况下,能够获得良好的定位精度和地图构建精度,且计算量适中,满足实时性的要求。
3) 仿真和实验结果表明,IOBPF-SLAM相较于其他几种SLAM算法,为移动机器人的同步定位与位姿估计提供了更为精确、可靠的结果。此外,通过对设定路标的位置估计发现,EIKF在计算量方面与传统EKF算法相当,但其滤波效果和鲁棒性相对于EKF有明显的优势。
考虑到BPF具有的性能优势,在后续工作中,将在区间分析基础上进一步结合其他智能优化与学习进化方法改善BPF的性能,并将其应用于复杂环境或其他类型传感器的移动机器人SLAM问题。
-
表 1 二维箱粒子的基本数学定义
Table 1. Basic mathematical definition of two-dimensional box particle
参数 数学表示 宽度 中心 范数 体积 表 2 仿真参数设置
Table 2. Simulation parameter setting
参数 设定值 机器人最大转向角 ±30° 机器人最大转向角速度 ±2°/s 机器人轴间距 2 m 传感器的最大观测范围 20 m 传感器的观测频率 50 Hz 控制时间间隔 0.02 s 控制噪声 (0.35 m/s, 2°) 观测噪声 (0.3 m/s, 3°) 表 3 不同SLAM算法的性能比较
Table 3. Comparison of performance among different SLAM algorithms
算法 粒子数 CPU时间/s 运行时间/s GridSLAM 100 0.873 318.72 GMapping 30 0.401 261.37 BPF-SLAM 15 0.325 223.41 本文算法 10 0.337 247.69 -
[1] 王忠立, 赵杰, 蔡鹤皋. 大规模环境下基于图优化SLAM的图构建方法[J]. 哈尔滨工业大学学报, 2015, 47(1): 75-85. doi: 10.3969/j.issn.1009-1971.2015.01.011WANG Z L, ZHAO J, CAI H G. A survey of front-end method for graph-based SLAM under large-scale environment[J]. Journal of Harbin Institute of Technology, 2015, 47(1): 75-85(in Chinese). doi: 10.3969/j.issn.1009-1971.2015.01.011 [2] SMITH R C, CHEESEMAN P. On the representation and estimation of spatial uncertainty[J]. International Journal of Robotics Research, 1986, 5(4): 56-68. doi: 10.1177/027836498600500404 [3] HOLMES S A, KLEIN G, MURRAY D W. An O(N2) square root unscented Kalman filter for visual simultaneous localization and mapping[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2009, 31(7): 1251-1263. doi: 10.1109/TPAMI.2008.189 [4] DOUCET A, DE FREITAS N, MURPHY K, et al. Rao-Blackwellised particle filtering for dynamic Bayesian networks[M]//DOUCET A, DE FREITAS N, GORDON N. Sequential Monte Carlo method in practice. Berlin: Springer, 2001: 499-515. [5] MONTEMERLO M, THRUN S, KOLLER D, et al. FastSLAM: A factored solution to the simultaneous localization and mapping problem[C]//Proceedings of the National Conference on Artificial Intelligence, 2002: 593-598. [6] MONTEMERLO M, THRUN S, ROLLER D, et al. FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges[C]//Proceeding of the International Joint Conference on Artificial Intelligence, 2003: 1151-1156. [7] ELIAZAR A, PARR R. DP-SLAM: Fast, robust simultaneous localization and mapping without predetermined landmarks[C]//Proceedings of the International Joint Conference on Artificial Intelligence, 2003: 1135-1142. [8] HAHNEL D, BURGARD W, FOX D, et al. An efficient fastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements[C]//International Conference on Intelligent Robots and Systems. Piscataway: IEEE Press, 2003, 1: 206-211. [9] GRISETTI G, STACHNISS C, BURGARD W. Improved techniques for grid mapping with Rao-Blackwellized particle filters[J]. IEEE Transactions on Robotics, 2007, 23(1): 34-46. doi: 10.1109/TRO.2006.889486 [10] 祝继华, 郑南宁, 袁泽剑, 等. 基于ICP算法和粒子滤波的未知环境地图创建[J]. 自动化学报, 2009, 35(8): 1107-1113.ZHU J H, ZHENG N N, YUAN Z J, et al. A SLAM approach by combining ICP algorithm and particle filter[J]. Acta Automatica Sinica, 2009, 35(8): 1107-1113(in Chinese). [11] BAILEY T, NIETO J, NEBOT E. Consistency of the FastSLAM algorithm[C]//IEEE International Conference on Robotics and Automation (ICRA). Piscataway: IEEE Press, 2006: 424-429. [12] VINCKE B, LAMBERT A, ELOUARDI A. Guaranteed simultaneous localization and mapping algorithm using interval analysis[C] //International Conference on Control Automation Robotics and Vision. Piscataway: IEEE Press, 2018: 1409-1414. [13] ABDALLAH F, GNING A, BONNIFAIT P. Box particle filtering for nonlinear state estimation using interval analysis[J]. Automatica, 2008, 44(3): 807-815. doi: 10.1016/j.automatica.2007.07.024 [14] 李振兴, 刘进忙, 李松, 等. 基于箱式粒子滤波的群目标跟踪算法[J]. 自动化学报, 2015, 41(4): 785-798.LI Z X, LIU J M, LI S, et al. Group targets tracking algorithm based on box particle filter[J]. Acta Automatica Sinica, 2015, 41(4): 785-798(in Chinese). [15] GNING A, RISTIC B, MIHAYLOVA L. Bernoulli particle/box-particle filters for detection and tracking in the presence of triple measurement uncertainty[J]. IEEE Transactions on Signal Processing, 2012, 60(5): 2138-2151. doi: 10.1109/TSP.2012.2184538 [16] YANG X S. Firefly algorithm, stochastic test functions and design optimization[J]. International Journal of Bio-Inspired Computation, 2010, 2(2): 78-84. doi: 10.1504/IJBIC.2010.032124 [17] CHEN G, WANG J, SHIEH L S. Interval Kalman filtering[J]. IEEE Transactions on Aerospace and Electronic Systems, 1997, 33(1): 250-259. doi: 10.1109/7.570759 [18] JAULIN L, KIEFFER M, DIDRIT O, et al. Applied interval analysis[M]. Berlin: Springer, 2001: 1-116. [19] GNING A, MIHAYLOVA L, ABDALLAH F. Mixture of uniform probability density functions for non-linear state estimation using interval analysis[C]//The 13th Conference on Information Fusion. Piscataway: IEEE Press, 2010: 1-8. [20] GNING A, RISTIC B, MIHAYLOVA L, et al. An introduction to box particle filtering[J]. IEEE Signal Processing Magazine, 2013, 30(4): 166-171. doi: 10.1109/MSP.2013.2254601 [21] 田梦楚, 薄煜明, 陈志敏, 等. 萤火虫算法智能优化粒子滤波[J]. 自动化学报, 2016, 42(1): 89-97.TIAN M M, BO Y M, CHEN Z M, et al. Firefly algorithm intelligence optimized particle filter[J]. Acta Automatica Sinica, 2016, 42(1): 89-97(in Chinese). [22] LI N, MA H, YANG C. Interval Kalman filter based RFID indoor positioning[C]//Control and Decision Conference. Piscataway: IEEE Press, 2016: 6958-6963. [23] LE Y, HE X F, XIAO R Y. MEMS IMU and GPS/Beidou integration navigation system using interval Kalman filter[J]. Applied Mechanics and Materials, 2014, 568-570: 970-975. doi: 10.4028/www.scientific.net/AMM.568-570.970 [24] HE X F, VIK B. Use of extended interval Kalman filter on integrated GPS/INS system[C]//Proceedings of International Technical Meeting of the Satellite Division of the Institute of Navigation, 1999: 1907-1914. [25] 万振刚. MINS/GPS组合导航系统若干关键技术研究[D]. 南京: 东南大学, 2005: 55-56.WAN Z G. Research on some key technologies of MINS/GPS integrated navigation system[D]. Nanjing: Southeast University, 2005: 55-56(in Chinese). [26] BAILEY T. SLAM simulations[DS/OL]. (2008-06-10)[2020-08-20]. [27] KIM C, SAKTHIVEL R, WAN K C. Unscented fastSLAM: A robust and efficient solution to the SLAM problem[J]. IEEE Transactions on Robotics, 2008, 24(4): 808-820. doi: 10.1109/TRO.2008.924946 [28] DUAN J M, LIU D, YU H X, et al. An improved FastSLAM algorithm for autonomous vehicle based on the strong tracking square root central difference Kalman filter[C]//2015 IEEE 18th International Conference on Intelligent Transportation Systems (ITSC 2015). Piscataway: IEEE Press, 2015: 693-698. [29] STACHNISS C. Robotic mapping and exploration[M]. Berlin: Springer, 2009: 10-16. 期刊类型引用(9)
1. 蔡艳,杨光永,陈旭东,徐天奇. SLAM精度的向量加权平均自适应调节研究. 组合机床与自动化加工技术. 2024(01): 109-113 . 百度学术
2. 李强. 基于多模态数据和粒子滤波器的移动机器人目标跟踪方法. 计算机测量与控制. 2024(02): 325-331 . 百度学术
3. 赵文金,朱子恒,吴云雁. 基于SLAM导航的煤矿井下机器人设计. 山东煤炭科技. 2024(02): 161-165 . 百度学术
4. 蔡艳,杨光永,樊康生,徐天奇. 基于多策略人工蜂鸟优化PF的SLAM研究. 组合机床与自动化加工技术. 2024(04): 92-97 . 百度学术
5. 蔡艳,杨光永,黄训爱,徐天奇. 多策略鲸鱼算法优化粒子滤波的SLAM精度研究. 重庆理工大学学报(自然科学). 2023(06): 136-145 . 百度学术
6. 蔡艳,杨光永,樊康生,徐天奇. 基于IINFO的粒子重组FastSLAM在SLAM中的研究. 组合机床与自动化加工技术. 2023(09): 31-34+38 . 百度学术
7. 张江桥,范平清,陈勇. 基于主观贝叶斯多传感器数据融合的AGV精确定位研究. 云南大学学报(自然科学版). 2023(05): 1015-1021 . 百度学术
8. 杨光永,蔡艳,陈旭东,徐天奇. 多策略人工兔算法优化粒子滤波的SLAM精度研究. 重庆理工大学学报(自然科学). 2023(11): 257-268 . 百度学术
9. 吴君钦,刘小兰,窦蕾萍,刘雯雯,曾亮. 基于块稀疏的低复杂度宽带MIMO-OFDM稀疏信道估计方案. 科学技术与工程. 2022(26): 11444-11451 . 百度学术
其他类型引用(18)
-