改进的平方根容积模糊自适应卡尔曼滤波SLAM算法

李 俊,舒志兵,王苏洲

(南京工业大学 电气工程与控制科学学院，南京 211816)

改进的平方根容积模糊自适应卡尔曼滤波SLAM算法

李 俊,舒志兵,王苏洲

(南京工业大学 电气工程与控制科学学院，南京 211816)

针对移动机器人SLAM算法存在系统噪声对定位精度影响严重，特征点的增加导致轨迹偏移等现象。文章将迭代思想与时变渐消因子引入平方根容积卡尔曼滤波中，通过动态调节新息均值和协方差的方式，建立模糊自适应模型调整噪声权值，改善系统中存在的运动噪声和观测噪声。该算法相对于以往算法只能解决单一问题而言，具有更好的兼容性与鲁棒性。通过实验仿真结果可以看出，该算法相对于以往算法在X方向、Y方向和位姿偏移角的误差分别减小了21.59%、36.45%、32.97%。将此算法应用于实际中，具有良好的地图重建效果。

SLAM算法；迭代平方根容积；时变渐消因子；模糊自适应

0 引言

同时定位与地图重建是指移动机器人在未知环境下通过激光雷达进行环境地图的重建并在重建后的地图中表示出该机器人位姿状态的过程[1]。SLAM算法在移动机器人中的应用是该领域的研究热点。传统的算法将扩展卡尔曼滤波引入到SLAM领域，其本质就是使用卡尔曼滤波对线性化的模型进行处理[2]。扩展的卡尔曼滤波(EKF)虽然能够解决非线性系统问题，但是不能很好的处理算法中的数据关联且精度不高[3-5]。在此基础之上，近年来许多研究学者提出了各种改进的新型算法。针对地图重建过程中受外界环境影响较大的问题[6]，涂刚毅等人提出了一种超声概率栅格地图环境特征点提取算法[7]。赵琳等人提出了迭代测量更新的UKF算法，有效解决了无迹卡尔曼滤波(UKF)中Sigma点会逐渐偏离真实状态估计值的问题[8]，但机器人在同时定位与地图重建过程中系统往往存在噪声，随着地图特征点的增加系统的观测维度也会随之增加，导致定位效果不佳[9-10]。

在分析了以上研究成果后提出一种改进的平方根容积模糊自适应卡尔曼滤波SLAM算法，该算法将迭代方式与强跟踪相结合，并且针对机器人存在运动噪声和观测噪声这一问题，采用改进的模糊自适应方式对不同的噪声进行自适应动态调整。该算法的优点在于大大降低了算法复杂度，解决了采样点在非线性情况下存在失真的问题，能够很好的校正因特征点增加引起的轨迹偏移现象，提高了位姿精度。

1 融合模糊自适应与平方根容积的SLAM算法

1.1 模糊自适应噪声动态调整算法

(1)

对移动机器人进行建模，其动力学模型和观测模型如下：

(2)

式中：Uk为nu维运动信息向量，Zk为nz维的观测向量。Qk、Rk分别表示运动噪声和观测噪声。

对于式(2)中的运动噪声和观测噪声，对其设置控制权值，则式(2)变换为：

(3)

其中，ak、bk分别表示控制的权值。

模糊自适应算法通过动态调整新息均值和方差的方式对噪声的权值进行了模糊调整。则新息和新息方差定义如下：

(4)

在时间窗M内新息和方差的均值如下所示：

(5)

根据新息和方差的均值得到其置信度为：

(6)

为了提高模糊自适应模型的适应性，本文引入高斯函数来动态的确定ak，bk的值，高斯函数如下所示：

(7)

式中，c表示高斯函数的中心，σ表示高斯函数的宽度。

由式(6)、(7)得到自适应控制ak、bk的值为：

(8)

式中，η为[0,1]之间的常数，表示对运动噪声和观测噪声的调节率，σ的值通过公式(6)可得：

(9)

由于高斯值是有界的，所以ak、bk的波动范围比较小，所以在解决滤波器出现发散问题时具有良好的效果。通过自适应的方式调整高斯函数曲线形状，从而增加了系统的稳定性与精确性。其模糊自适应控制模型结构如图1所示。

图1 模糊自适应控制模型结构

如图1所示，通过新息和方差均值的置信度q1(k)和q2(k)的变化在线监测滤波器的稳定性。当q1(k)和q2(k)偏离稳定值时，则通过模糊自适应的方式动态调整ak和bk的值来改变系统噪声与观测噪声的权重，进而改善滤波器的性能。

1.2 强跟踪迭代平方根容积卡尔曼滤波

移动机器人在同时定位与地图重建过程中随着特征点数的增加，系统的观测维度也会随之增加，这将导致容积点偏离预期的位置，为了避免此情况的发生，在平方根容积卡尔曼滤波的基础上增加迭代的方式。利用贝叶斯滤波求出K时刻的先验概率分布和后验概率分布，其算法分为预测阶段和更新阶段。

1.2.1 预测阶段

(10)

(11)

通过k-1时刻的机器人位姿信息预测k时刻机器人位姿信息。其容积点先验估计和位姿信息公式如下：

(12)

(13)

又可得地图特征误差向量如下所示：

(14)

1.2.2 更新阶段

(15)

经过迭代后的容积点公式变为：

(16)

计算第l次迭代后的卡尔曼增益为：

(17)

计算观测误差向量为：

(18)

根据式(14)、式(18)可以解得状态预测协方差阵、地图特征误差向量与观测误差向量的交互协方差矩阵以及观测新息协方差平方根因子如下所示：

(19)

因此卡尔曼增益矩阵为：

(20)

最后计算迭代l+1次后的机器人位姿矩阵和平方根因子矩阵公式如下所示：

(21)

(22)

当达到预先设定的迭代次数后，数据更新如下：

(23)

位姿更新：

(24)

在预测更新过程中经常会由于系统的不稳定导致滤波器出现发散等问题，因此本文通过引入渐消因子来改变数据权重从而提高系统的稳定性与定位精度。强跟踪滤波时变渐消因子公式如下所示：

(25)

其中,

(26)

(27)

(28)

(29)

式中，Hk和Fk|k-1分别表示测量方程与状态方程对于状态变量的一阶偏导矩阵，β为弱化因子，ρ为遗忘因子。

将式(19)带入式(27)、式(28)得到：

(30)

(31)

通过引入渐消因子后的状态协方差预测平方根因子为：

(32)

最后将此平方根因子重新带入预测更新过程中进行迭代运算。

2 系统建模与实验仿真

将本文提出的算法在MATLAB下进行的仿真，使用250m×200m的环境地图，选择17个确定的路径点和35个地图特征点。具体仿真参数如表1所示。

表1 仿真参数

仿真实验中运动模型：

(33)

输入：xv(k)为机器人的位姿，ΔT为传感器采样时间，v(k)为速度，a(k)为机器人在k时刻的方向角，B为两轴间的轴距。输出：xv(k+1)为机器人的新位姿。

仿真实验中观测模型：

(34)

输入：(xi,yi) 为探测到的第i个特征的位置坐标，xv(k)为位姿。输出：通过激光雷达等传感器测得的特征点与移动机器人的距离x和特征点与移动机器人前进方向的夹角θ。

通过仿真得到传统SR-CKF-SLAM算法与改进的SR-CKF-SLAM算法移动机器人轨迹图，图中“虚线”表示机器人估计轨迹，“实线”表示机器人实际轨迹，“*”表示地图特征点，“+”表示SLAM预估计的特征点位置。

图3 改进的SR-CKF-SLAM算法

由图2、图3所知，本文提出的改进的SR-CKF-SLAM算法有效改善了因为特征点的增加导致的轨迹偏移现象，为了更好的验证本文算法的可靠性，将此算法与STF-SRCKF-SLAM算法和ISR-CKF-SLAM算法进行误差对比，在存在系统噪声情况下得到X轴、Y轴以及角度位姿误差对比图如图4所示。

图4 X轴方向误差对比

图5 Y轴方向误差对比

图6 角度误差对比

由图4～图6可以看出，随着时间的增加，移动机器人的定位误差逐渐减小，由于本文算法引入了迭代与时变渐消因子相结合的方式，再加上针对存在系统噪声这一问题，在算法中动态调节新息均值和方差，使得整个算法具有较好的模糊自适应性。

为了更加直观的看出改进后算法的优点，3种算法性能的定量比较如表2所示。

表2 算法误差统计

相比于性能较好的ISR-CKF算法，改进的SR-CKF算法分别在X方向、Y方向和角度位姿上的误差降低了21.59%、36.45%和32.97%。

将改进后的SR-CKF应用于实际中，其硬件部分和实验重建的地图如图7、图8所示，图8中左边为实际场景的地图，右边是算法重建的地图，可以看出本文算法具有良好的地图重建功能。

图7 移动机器人硬件平台

图8 算法重建的地图

3 结论

本文提出的改进的平方根容积模糊自适应算法融合了STF-SRCKF和ISR-CKF的优点，有效改善了因特征点增加导致的轨迹偏移现象，并且采用动态调节新息和协方差的方式改善了系统中存在的运动噪声和观测噪声带来的定位误差较大等问题。通过实验仿真结果可以看出该算法有效提高了定位精度，具有很好的稳定性与自适应性，在实际应用中具有良好的地图重建功能。

[1] Durrant-Whyte H,Bailey T. Simultaneous localization and mapping:Part I[J].IEEE on Robotics& Automation Magazine, 2006, 13(2): 99- 108.

[2] 陶明，凌有铸，陈孟元，等.迭代的平方根容积卡尔曼滤波SLAM算法[J].计算机工程，2015，41(9):317-321.

[3] 欧阳广帅，周晶.基于卡尔曼滤波的高精度弹道滤波算法研究[J]．电子测量技术，2014，37(11):16-19.

[4] 刘丹，段建民，于宏啸.基于自适应渐消 EKF的FastSLAM算法[J].系统工程与电子技术，2016，38(3):644-651.

[5] 张文玲，朱明清，陈宗海.基于强跟踪 UKF的自适应SLAM算法[J].机器人，2010，32(2):190-195.

[6] 周武，赵春霞，沈亚强，等．基于全局观测地图模型的SLAM研究[J].机器人，2010，32(5):647-653.

[7] 涂刚毅，金世俊，祝雪芬，等.基于粒子滤波的移动机器人SLAM算法[J].东南大学学报(自然科学版)，2010，40(1):117-122.

[8] 赵琳,闫鑫,郝勇.基于迭代平方根UKF的SLAM算法[J].弹箭与制导学报，2011,31(2):157-161.

[9] 戴雪梅，郎朗，陈孟元.强跟踪平方根容积卡尔曼滤波SLAM算法[J].电子测量与仪器学报，2015，29(10):1493-1499.

[10] 周武，赵春霞，沈亚强,等．基于全局观测地图模型的SLAM研究[J]．机器人,2010，32(5)：647-653.

ImprovedSquareRootVolumeFuzzyAdaptiveKalmanFilterSLAMAlgorithm

LI Jun,SHU Zhi-bing,WANG Su-zhou

(College of Electrical Engineering and Control Science ,Nanjing Tech University, Nanjing 211816,China)

SLAM algorithm for mobile robot system is severely affected by the noise on the positioning accuracy, the phenomenon such as the increase of feature point lead to track migration. In this paper, the iterative thoughts and time-varying fading factor is introduced into the square root kalman filtering in the volume, and through dynamic adjust the way of the new interest rates mean and covariance, and fuzzy adaptive adjust the weights of noise model, which improved the system noise and observation noise existing in the movement.The algorithm is compared with the previous algorithm can only solve a single problem, which has better compatibility and robustness. The simulation results show that the proposed algorithm compared with previous algorithms in theXdirection,Ydirection error and position deviation angle decreases by 21.59%, 36.45%, 32.97%. This algorithm is applied in practice, and has a good map reconstruction effect.

SLAM algorithm;iterative square root volume;time-varying fading factor;adaptive fuzzy

TH166;TG506

A

1001-2265(2017)12-0029-04

10.13462/j.cnki.mmtamt.2017.12.007

2017-01-15；

2017-02-17

李俊(1993—)，男，江苏扬州人，南京工业大学硕士研究生，研究方向为地图重建与导航，图像处理，运动控制技术，(E-mail)lj893434@163.com。

(编辑李秀敏)