基于EKF 算法的小地块SLAM 技术研究与分析*

曹鹏铖 ， 陆 静 ， 高巧明

（1.广西科技大学机械与交通工程学院，广西 柳州 545616；2.广西科技大学广西汽车零部件与整车技术重点实验室，广西 柳州 545616）

0 引言

随着《中国制造2025》的提出和机器人技术的不断发展，自主机器人广泛应用于家庭生活、医学、农业和军事等众多领域，农用智能机械的自主作业也逐渐成为研究热点[1-2]。其中，精确的地图与定位是自主机器人工作的关键技术之一。在GPS 和高精度地图技术的支持下，在大面积的农田内实现农业机械的智能化作业已不再是问题，但是，在一些偏远地区的零碎、大小不一的小地块中，GPS 信号往往不稳定，且民用卫星的精度无法保证小地块建图的精度，严重影响农用智能机械的自主作业。在这种情况下，同步定位与建图（Simultaneous Localization and Mapping，SLAM）技术就显得尤为重要[3]。

SLAM 是指机器人处于未知环境中时，通过自身携带的传感器获得位姿信息和环境信息，实现作业过程的自身定位和地图创建[4-7]，它是自主机器人在未知环境中实现自主作业的关键技术。目前，关于机器人的SLAM 技术学术界提出了多种解决方案，有基于扩展卡尔曼滤波（EKF）的SLAM 方法[8-10]，基于粒子滤波的Fast SLAM方法[11-12]，基于全局优化的最大似然算法[13]，基于RGB-D 的视觉SLAM 方法[14]，以及基于深度学习的SLAM 方法[15-16]。基于扩展卡尔曼滤波的SLAM 算法直接提供了导航问题的递归解，以及计算机器人和路标不确定性的估计方法，因其数学的严谨性和结构的适用性，在SLAM 问题中得到了最广泛的应用。Smith 等人最早提出了基于EKF 的SLAM 解决方案，随后许多学者对其进行深入研究，并进行了延伸，在不同的场景中提出了基于EKF 的SLAM 算法[17-18]。但是，在计算资源受限，路标点单一，待估计量比较简单的小地块场景中的SLAM技术尚未有人涉及。因此，课题组提出将基于EKF 算法的SLAM技术应用于小地块环境。

1 SLAM问题的提出

SLAM 是机器人在一个未知的环境里，搭载可获取环境信息的传感器，从某一个位置出发，在行进的过程中估计自身位置，同时建立环境地图，其实现模型图如图1 所示。其中，x、y、z、u分别表示位姿、路标、观测数据和运动传感器数据。SLAM 过程可以由运动方程和观测方程两个方程来解释，假设在t=0 到t=N的时间内，机器人的位姿由x0到xN，路标由y1到yM，则运动方程和观测方程分别为：

图1 SLAM模型图

其中，uk是运动传感器的输入，zk, j是产生的观测数据，wk、vk, j是两个噪声项，并假设满足零均值的高斯分布。

通过数据z和u来推断位姿x和路标y，由此可见，SLAM 问题是一个运动过程的状态估计问题。但是，在噪声的影响下，这时的位姿和地图是有误差的。而且，随着时间的移动，误差逐渐累积，对位置方差的估计将越来越大。图2 中的左图是只有运动数据的情况，右图是考虑了观测数据的情况，图中的虚线圆表示协方差矩阵。由图2 左图可以看出，由于只有运动数据，虚线圆随着物体运动越来越大，机器人不确定自身的位置，越走越“迷路”；在图2 右图中，利用合理的位姿和观测数据，虚线圆就会缩小到一定的大小，保持稳定。因此，需要综合利用运动方程和观测方程的结果以减小状态估计的不确定性，提高SLAM的精度。

图2 状态估计的不确定性

2 基于EKF的SLAM算法

目前，关于SLAM 问题的研究都是采用概率密度的方法。SLAM 问题通过将机器人的每个位姿和地图位置表示为概率密度函数，按照贝叶斯法则，k时刻的状态分布为：

这时，k时刻的状态是根据0 到k时刻的数据估计来的。但是，若假设该过程满足一阶马尔可夫性，也就是k时刻的状态只与k－1 时刻的状态相关，式（2）中的先验部分以k－1 时刻为条件展开：

由以上方程可以看出，技术的关键在于如何精确地将机器人k－1时刻的状态分布推导至k时刻。

为了提高计算的精度，EKF-SLAM 算法通过预测和更新两个阶段来求解机器人的状态估计。在观测到新的路标点时，将其添加到系统状态向量中继续进行更新，示意图如图3所示。

图3 EKF-SLAM算法示意图

2.1 预测阶段

在基于EKF 的SLAM 算法中，采用联合状态向量表示机器人的位姿与路标点，k时刻系统的联合状态向量为：

为了估计k时刻系统的状态，首先要根据式（1）中的运动方程对机器人的位姿进行预测，在预测阶段状态向量的坐标值不变，则有：

运动方程中的f由所采用的机器人模型决定，在不同情况下具有不同的形式，确定f的过程称为参数化。在一般情况下，f往往不是线性的，传递协方差时，需要对f进行线性化，以保证经传递后的协方差依然服从高斯分布。其计算式如下：

其中，Q是观测方程中噪声的协方差矩阵，F是状态转移函数，该函数是对整体状态量进行操作，而不是仅考虑机器人的位姿变换，是对k－1 时刻状态量求导所得的雅可比矩阵。使用下式对协方差的先验估计进行计算：

其中，

2.2 更新阶段

机器人运动之后，可使用自身携带的传感器对环境进行测量，并通过式（1）中的观测方程计算观测数据，并在更新阶段结合观测数据和预测阶段的计算数据，优化SLAM算法中状态估计的不确定性。

记已观测路标点在当前时刻的测量值为Zk, j，由于当前观测到的路标点数量可能不唯一，故用下标i来区分不同的路标点。另外，由于这些路标在之前的过程中已经被观测到，所以其系统世界坐标下的坐标值（系统中的绝对坐标）已知。

根据当前机器人位姿的预测值和上述路标点的坐标值，可用观测模型计算出机器人与路标点间的预计测量值：

对于同一路标点，传感器测量值与预测值之间存在一定的误差，用新息γ来表示该误差：

计算新息与新息协方差之后，即可对系统状态量进行更新，完成EKF-SLAM的算法流程。

3 仿真实验及分析

本研究中的模拟环境以某地果园为环境模型。结构性的田垄整齐划分，每道垄上按一定距离间隔种有果树，果树可看为路标，激光雷达放置在小车上层，用于扫描、匹配、预测位置和建图，如图4所示。

图4 某果园地形模型及实验小车

3.1 室外果园环境模拟

为了验证基于EKF 算法的SLAM 适用于室外果园场景，并具有良好的操作性能，在MATLAB R2018b 平台上进行仿真实验，建立仿真环境。模拟的硬件环境是英特尔酷睿i7-8700CPU，3.2GHz，Ubuntu 18.04 操作系统。设定机器人的主要参数为：轮距L=1 m，行驶速度V=1.5 m/s，最大舵角30º，线速度的误差是0.02 m/s，角速度误差为1 rad/s，传感器测得的距离噪声为0.03 m，角度噪声为1º。俯视来看，小车的作业环境如图5 所示，是一个宽8 m、长16 m 的小地块，三角形表示小车。实验中的路标点为田垄上每间隔2 m 设置一个路标点（果树）。

图5 作业环境示意图

3.2 模拟结果及分析

在模拟环境中，小车从设定位置（果园田头）出发，在田垄间作业经过整个果园，最终的地图模型和作业轨迹如图6 所示，地图模型中白色表示田垄边界，黑色表示小车的可通行域，作业轨迹中虚线表示实际位置，实线表示估计位置。

图6 EKF-SLAM地图和位置估计

模拟结果显示，EKF-SLAM 算法基本实现了小地块的地图建立和定位，证明了算法的可行性。为了进一步分析对比算法的性能和精度，设置转弯路标点稠密实验组，分析路标点的数量对EKF-SLAM 算法结果的影响，并根据激光雷达扫描匹配的估计位置和姿态存在偏差，定义以下误差为：

通过比较位置的估计和实际偏差，位置偏差的标准偏差曲线和x、y的状态量，如图7 所示。从图中可以看出，小车的纵向误差稳定，横向误差有较大波动，EKF-SLAM 算法的整体位置偏差小于0.5 m，但是在150 s 和480 s 左右有较大的偏差，这个时间是小车转弯的前后阶段。由此可见，该算法在转弯处的仿真结果具有较大的误差。小地块的地形通常转弯较多，因误差累积的原因，小地块转弯处的计算偏差对整体建模精度的影响远大于常规的大块平整地块，因此，在基于EKF-SLAM 算法的小地块地图建立和定位中，重点考虑转弯处的建模精度。

图7 EKF-SLAM位置误差和标准偏差

在转弯处加密路标点，模拟结果如图8 所示，由图可知，地图效果和小车位置估计有了明显的改进。小车位置偏差曲线和x、y标准差如图9 所示，对比图7 可以看出，转弯稠密路标点实验组的位置偏差的数值小于原路标点实验组，并且变化趋势较平缓；同时，转弯稠密路标点实验组的标准差也小于原实验组。但是，在150 s、300 s 和480 s 左右位置偏差还是波动异常，表明在小车转弯时的偏差控制还需要进一步的研究。

图8 转弯路标点稠密实验地图和位置估计

图9 转弯稠密路标点实验EKF-SLAM位置偏差和标准差

4 结论

课题组基于EKF-SLAM 原理研究分析了在小地块果园中，小车作业过程的同步定位与建图技术，这将有助于解决小地块果园GPS 信号弱且精度不足时导致难以实现自主作业的问题。通过模拟实验证明了该算法在小地块果园场景中的适用性及有效性。分析仿真结果发现，由于地形的特点，转弯处的模拟精度是影响小地块EKF-SLAM 技术的关键性因素，通过在转弯处设置稠密路标点（果树），有效提高了算法的位置估计精度。