室内移动机器人实时定位滤波算法仿真对比

刘 慧，姜雨汐

(北京建筑大学 电气与信息工程学院， 北京 100044)

移动机器人的实时定位与地图构建 (Simultaneous Localization and Mapping，SLAM)研究始于20世纪80年代末，是移动机器人领域最基本、最具挑战性的问题之一[1]，科学家们针对如何提高机器人导航定位精度问题已经进行了40多年的广泛研究[2]。目前提高SLAM中的定位精度有3类方法，分别是扩展卡尔曼滤波算法(Extended Kalman Filter，EKF)、无迹卡尔曼滤波算法(Unscented Kalman Filter，UKF)、以及粒子滤波算法(Particle Filter，PF)。其他的改进方法都是以这3类为基础演化而来[3]1-14。

EKF算法是最先被提出的提高移动机器人定位精度的算法，也是最具影响力的算法[4]。目前，在移动机器人领域针对各种不同的任务和环境，出现了不同形式的基于EKF的定位滤波算法。DAS等[5]提出引入增广状态扩展卡尔曼滤波算法(Augmented State Extended Kalman Filter, AS-EKF)来估计机器人的真实位置，与传统EKF算法相比更加精确。WU等[6]提出一种改进EKF算法用于无人直升机自主登船着陆，该算法可以在短时间内运行并达到一定的精度，但算法的误差会随着操作次数的增加而累积，在长时间范围内缺乏一定的适用性。LU等[7]提出了一种结合路标的改进扩展卡尔曼滤波自定位算法，改进算法路径更接近真实路径，机器人位置估计误差较小，可以达到轮式机器人在球罐中高精度定位的目的。目前EKF算法已经非常成熟[8]，在传感器噪声满足高斯分布且系统为弱非线性系统的情况下，EKF能取得较好的效果[9]。EKF算法的一个关键问题是协方差矩阵的二次型：随着地标点的不断增加，系统的协方差矩阵维度呈二次递增，计算压力很大因此EKF算法不适合大规模环境[3]1-14。EKF算法通常会引入截断误差，并且必须在每个时间步长更新系统雅可比矩阵，这导致了计算量的增加，因此不利于移动机器人的实时定位。

为了克服上述EKF算法的缺点，1997年，JULIEAR和UHKMANN提出了UKF算法。UKF是一种新的非线性系统滤波方法，采用无迹变换(Unscented Transformation，UT)，不需要计算雅可比矩阵，但是在系统模型不确定情况下，UKF的系统鲁棒性欠佳。针对模型不确定性估计问题，有学者将强跟踪滤波和UKF算法相结合以求解决此类问题[10]。符强等[11]提出一种改进的强跟踪UKF算法，有效提高强跟踪UKF滤波实际应用中的稳定性。YAN等[12]提出一种基于自适应约束的强跟踪UKF方法，提高了飞行控制系统非线性突变条件下故障检测的准确性。ZHANG等[13]提出基于特征的UKF算法水下声呐成像，试验表明UKF算法对水下机器人的姿态估计效果较好，定位误差可以保持在较低水平并在较长时间内保持稳定，构建的地图具有较好的一致性。

PF算法是一种基于蒙特卡罗思想的非线性非高斯状态估计滤波方法[14]801-809，具有处理非线性、非高斯分布和多模态问题的能力，适用于移动机器人的定位滤波。1999年CARPENTER等[15]首次提出粒子滤波器的概念。PF算法是通过一组粒子来表示后验概率，但存在粒子退化和多样性丧失问题，严重影响粒子滤波的精度。为解决这一问题，蔡登禹等[16]提出一种结合云自适应遗传算法的改进的粒子滤波算法，既保护了有效粒子不被破坏，又提髙了粒子的多样性，较传统算法跟踪效果更加稳定，跟踪精度更高。白晓波等[17]提出一种改进优化粒子滤波方法，结合多新息理论(Multi Innovation Theory, MIT)提高了滤波精度，但是在保证滤波精度不变的情况下，却大大增加了运算量。ARORA等[18]提出一种蝴蝶优化算法(Butterfly Optimization Algorithm，BOA)，这是一种基于蝴蝶觅食行为的全局优化算法，该算法具有更高的收敛性，优于其他自然启发式算法，如萤火虫算法、蚁群算法、人工蜂群、蝙蝠算法等。

本文针对室内移动机器人二维运动系统定位精度不够的问题，开展了上述几种定位滤波算法的仿真比较研究。首先，针对一阶非线性状态方程描述的系统(如机械臂的升降、回转、俯仰、伸缩等)，对其观测结果采用上述算法进行滤波处理，验证上述算法对一阶非线性运动系统的位置估计误差和算法时间成本，以此作为参考，将上述算法应用到室内移动机器人二维运动系统的实时位置估计。室内移动机器人二维运动系统可以用二阶非线性状态方程描述，采用上述算法对其运动轨迹进行滤波处理，获得相应的定位结果，分析定位误差和算法效率。试验结果表明，融合蝴蝶优化的粒子滤波算法对于实际中的二维运动系统能够获得较高的运算效率，并且误差较小。

1 3类算法的基本原理

1.1 扩展卡尔曼滤波算法

EKF算法是非线性系统常用的一种滤波方法，其原理是在t时刻对非线性系统的状态方程和观测方程进行泰勒展开，用线性化模型近似非线性系统。非线性系统的状态模型和观测模型的一般形式如下：

(1)

式中：xt为t时刻的状态值；zt为t时刻的观测值；ft-1和ht分别为系统的状态方程和观测方程;ut为控制量；wt和vt为t时刻的系统噪声，一般情况下，wt和vt是均值为0的高斯白噪声。

将式(1)用一阶泰勒展开近似，可以得到线性系统的状态模型和观测模型，其离散形式为：

(2)

式中：Ak为离散系统的状态矩阵，Bk为输入矩阵，Ck为输出矩阵。

EKF算法可以概括为以下几个步骤：

第一步，预测。即预测先验状态估计和先验状态误差协方差。也就是利用预测模型更新当前状态的先验值，推算下一时刻的状态变量和误差协方差，其预测方程为：

(3)

(4)

第二步，计算卡尔曼滤波增益。根据先验状态误差协方差，可以计算得到当前的卡尔曼滤波增益Kk。

(5)

式中：Rv表示噪声vk的协方差矩阵。

第三步，测量更新。用后验结果更新当前状态及状态误差协方差。

(6)

(7)

1.2 无迹卡尔曼滤波算法

UKF算法是基于卡尔曼滤波框架通过UT变换实现条件预测和测量更新，得到滤波后的系统状态。UT变换的基本思想是用2L+1个高斯分布的采样点去近似一个随机变量的取值(L为系统状态的维数)，对其进行非线性变换，然后通过加权统计结果更新卡尔曼滤波的后验均值和方差。

针对式(1)所表示的移动机器人状态模型和观测模型，具体的UKF算法描述如下：

0=E(x0)

(8)

P0=E[(x0-0)(x0-0)T]

(9)

式中：x0通常取值为0，P0通常取值为1。

(10)

(11)

(12)

式中：β为常数，通常取值为2。

(13)

式中:i=1，2，…，L+1，…，2L

3) 用Sigma点集的状态预测下一时刻的状态。

(14)

式中：i=0，1，…，2L。

4) 将Sigma点集的状态预测值加权求和，得到系统状态量的预测结果xt+1/t和状态误差协方差Pt+1/t。

(15)

(16)

5) 再用UT变换，产生新的Sigma点Xt+1/t。

(17)

(18)

(19)

(20)

(21)

7) 计算卡尔曼增益Kt+1。

(22)

8) 更新得到系统当前的状态向量xt+1/t+1和状态误差协方差Pt+1/t+1。

xt+1/t+1=xt+1/t+Kt+1(zt+1-zt+1/t)

(23)

(24)

1.3 粒子滤波算法

PF算法的基本思想基于蒙特卡罗方法，可用于移动机器人定位滤波。具体的用于移动机器人定位的粒子滤波算法有常规粒子滤波算法[19]以及自然启发式粒子滤波算法[20-21]。自然启发式粒子滤波算法解决了粒子退化和多样性匮乏问题，这里主要针对融合BOA的粒子滤波算法进行研究。

1.3.1 蝴蝶优化算法

BOA算法是一种从自然界获取启发的全局寻优算法。BOA算法从蝴蝶群觅食的行为获得启发，主要思想是每一只蝴蝶不仅会散发一定强度的香味，也可以感受到周围其他蝴蝶的香味，并飞向那些散发更多香味的蝴蝶。蝴蝶的香味取决于3个因素：感知形态、刺激强度以及幂指数[22]。用方程表示为：

F=cIa

(25)

式中：F为香味浓度，c为感知形态，I为刺激强度，a为幂指数。

BOA算法的基本步骤如下：

第一步，初始化具有N只蝴蝶的蝴蝶种群，由目标函数f(xi)决定每一只蝴蝶xi的刺激强度Ii。

第二步，计算蝴蝶种群中每一只蝴蝶的适应值Yi，并找到位置最优的蝴蝶，适应值最大为最优。计算蝴蝶的适应值Yi为：

(26)

式中：R为观测噪声的方差，znew为最新观测值，zpred为预测的观测值。

第三步，根据式(25)计算第i只蝴蝶散发的香味浓度Fi。通过产生随机数r决定蝴蝶是进行局部搜索还是全局搜索，这样做可以减少外界环境的干扰。如果随机数满足全局搜索条件，则全局搜索为：

(27)

(28)

如果随机数满足局部搜索条件，则局部搜索可以表示为：

(29)

为避免蝴蝶移动陷入局部最优，在算法中常常引入随机游走。随机游走是指式(29)中xj和xk从同一子群中随机选取。Lévy飞行是一种常用的随机游走方式，能够提高局部搜索效率，其随机游走计算式如下：

Lévy(λ)=t-λ

(30)

式中：1<λ≤3。

1.3.2 融合蝴蝶优化算法的粒子滤波算法

把BOA算法和PF算法相比较，可以看出2种算法有很多共同点：BOA算法中的每一只蝴蝶，类似PF中的粒子；BOA算法中的蝴蝶不断飞向适应值最大的蝴蝶，并更新自己的位置，类似PF中的粒子不断迭代，不断接近真实的系统状态的后验概率分布；BOA算法中最优的蝴蝶即适应值最大的蝴蝶，类似PF中最接近真实值的粒子即权重最大的粒子。

虽然BOA算法和PF算法有很多相似之处，但是不能把二者直接结合。需要把BOA算法进行如下修改[23]，改进后的全局搜索计算式为：

(31)

为使表述更加简洁，融合蝴蝶优化算法的粒子滤波算法可用BAPF来表示。引入BAPF具体实现如下：

2) 预测。根据式(1)计算t+1时刻粒子状态值。

3) 寻找最优值。把粒子滤波中的每一个粒子看作BOA算法中的一只蝴蝶。通过式(26)计算每一个粒子的适应值，并通过式(28)找到全局最优的粒子g*，即适应值最大的蝴蝶。

4) 利用式(25)计算每一个粒子的香味浓度Fi，产生随机数r用以决定粒子是进行全局搜索(依据式(31))还是局部搜索(依据式(29))。

5) 更新优化后粒子的重要性权重并归一化。

6) 重采样及状态输出。

2 仿真结果与分析

试验硬件环境采用的CPU为Intel Core i7处理器，内存为16 GB，软件环境为MATLAB 2014a。针对EKF、UKF、PF、BAPF进行了仿真，并开展这几种算法的比较研究。

2.1 一维系统仿真研究

通常情况下，一阶非线性非高斯运动系统方程的数学模型可以表示为[14]801-809：

(32)

式中：c1，c2和c3为常数。x(t)为t时刻粒子的状态值，y(t)为t时刻粒子的观测值,w(t)和v(t)为均匀分布的高斯白噪声。

研究常用的机械臂的升降动作，其状态方程和观测方程可以用一阶非线性非高斯运动系统方程表示为：

(33)

在比较研究中采用均方根误差ERMSE作为衡量各滤波算法的性能指标。均方根误差ERMSE为：

(34)

图1为采用上述算法的机械臂升降运动状态估计结果，图2为上述算法所对应的误差，表1为上述算法的估计误差均方值和运算时间的比较。

图1 机械臂升降运动状态估计(N=500)Fig.1 Estimation of robot arm lifting motion state (N=500)

图2 机械臂升降运动估计误差(N=500)Fig.2 Estimation error of robot arm lifting motion (N=500)

表1 估计误差均方值和运算时间比较

通过表1可以发现，在相同条件下，粒子数N=500，BAPF算法的误差值最小。虽然EKF算法的运算时间最短但是误差最大，不适用于实际应用；PF算法的运算时间虽低于BAPF算法，但误差较高。

2.2 二维系统仿真研究

根据上述机械臂的一维运动系统，本文将上述算法应用于室内移动机器人二维运动系统的实时定位滤波。假设室内机器人二维运动系统为二阶非线性模型，模型模拟了机器人在二维空间的一段弧线运动的过程，对机器人运动轨迹进行定位跟踪。其状态方程和观测方程为：

(35)

图3为EKF、UKF、PF以及BAPF算法的室内机器人二维运动轨迹状态估计结果，图4为这几种算法所对应的误差，表2为上述算法的估计误差均方值和运算时间的比较。

图3 室内机器人二维运动轨迹状态估计(N=20)Fig.3 Two-dimensional motion trajectory state estimation for indoor robots (N=20)

图4 室内机器人二维运动轨迹估计误差(N=20)Fig.4 Estimation error of two-dimensional motion trajectory of indoor robot (N=20)

从图3、图4、表2可以看出，UKF算法和PF算法运算时间较长，误差较大；EKF算法虽然运算时间最短但是误差较大，不适合实际应用；BAPF算法的估计结果相对最接近真实轨迹，误差小，运算时间短，效率高。

上述结果表明，BAPF算法是一种高效且高精度的算法，适合实际推广应用。EKF算法不适用于非线性非高斯系统，UKF算法需要花费大量时间处理Sigma点，而BAPF算法在PF算法的基础上引入BOA算法，不对粒子进行舍弃，它可以从根本上改善粒子匮乏问题，使粒子分布更加合理，理论分析和仿真结果一致。

表2 估计误差均方值和运算时间比较Tab.2 Comparison of root mean square error and running time

3 结论

本文采用EKF、UKF、PF、BAPF4种算法，分别在机械臂一维运动系统和室内移动机器人二维非线性运动系统中验证方法的有效性和实用性，为实现移动机器人实时位置的估计奠定理论基础。

通过仿真试验对比可知，BAPF算法的精度和运算效率均适用于实际的机器人定位。仿真结果说明BAPF算法对移动机器人的实时定位系统改进具有实用价值。

究其原因，EKF算法采用泰勒展开把非线性模型线性化，适用性较广且计算效率比较高，但只适用于弱非线性模型，对于强非线性模型误差较大，不适用于实际应用。另一个缺陷在于没有考虑状态方程线性化过程中出现不确定性的情况。而UKF算法适用于非线性高斯系统，该算法没有线性化过程，没有忽略模型的高次项，因而得到的均值和方差的估计要比EKF算法准确。PF算法是基于蒙特卡罗的思想，适用于非高斯非线性系统，不用模型的线性化，而是利用一些样本来获得状态估计。BAPF算法在PF算法基础上引入BOA算法，使粒子分布更合理，并引入Lévy飞行，提高搜索效率，避免局部最优。