多传感器融合的室内机器人SLAM

徐淑萍，杨定哲，熊小墩

(西安工业大学 计算机科学与工程学院,西安 710021)

随着各种类型的室内移动机器人被广泛应用在人类的生活、生产中,机器人的同步定位与建图(Simultaneous Localization and Mapping,SLAM)技术作为机器人完成服务工作的基础,逐渐成为研究的热点[1]。受室内环境多样性与复杂性的影响,普通单一雷达的同步定位与建图由于能获取的信息较为有限,从而不能构建完整的三维地图信息[2]。因此,多传感器融合成为了解决室内机器人SLAM缺陷的一个新的方法。目前应用于求解SLAM问题的主流传感器为激光雷达和视觉相机,各有不同的优势[3]。其中二维激光雷达具有误差小和可靠性高的优点,但是没有垂直方向的信息,同时也容易造成资源的浪费;而深度相机则是具有视野比较广,可以识别具体物体的优点,但是建图精度和稳定性较差,出现偏差的概率较高[4-5]。目前已经有非常多的学者基于感知传感器的互补性提出了许多的融合SLAM方案。

在视觉和姿数据测量传感器的融合方面,文献[6]利用耦合的方式对相机和惯性测量单元(Inertial Measurement Unit,IMU)以及里程计进行融合,得到了更加准确的位姿数据,但是由于使用的并非紧耦合,视觉传感器容易出现光敏性特点和不稳定性。文献[7]将IMU采集到的数据作为初始位姿传递给视觉传感器,一定程度上减少了深度相机在建图时的数据压力,但是这么做容易出现数据紊乱,若在IMU单元的数据出错,则会导致视觉部分出现问题。在轮式里程计和激光传感器方面,文献[8]则通过扩展卡尔曼滤波(Extended Kalman Filter,EKF)算法,让里程计提供位姿的同时,融合激光雷达的点云数据,使得到的数据更加精确可靠。文献[9]通过激光雷达识别关键帧的数据,通过ICP(Interative Closest Point,ICP)算法将其同里程计数据加权融合,从而得出更加精确的位姿信息。但是这两种算法都会由于实现方式过于传统,容易出现效率低下、数据融合效果差等问题。在激光和姿态测量传感器的融合方面,文献[10]将激光雷达和IMU进行紧耦合融合,为机器人提供了一个更高频率的实时位姿数据,但是却没有办法很好的消除掉无效数据造成数据冗余。文献[11]尝试引入里程计数据辅助IMU进行融合,可仍有部分误差存在。在视觉和激光传感器的融合方面,文献[12]将激光和相机进行融合,实现了闭环检测和完整的全局地图构建,但稳定性却不佳。文献[13]基于静态的场景进行视觉和激光的融合,提升了建图精度,但是却没办法做到动态实时性。在文献[14]里将里程计、IMU、视觉相机和激光雷达整体融合数据进行了分析,验证了多传感器融合SLAM的可行性,却出现了精度下降、部分地图缺失等问题。

综合上述文献可看出,多传感器融合的SLAM可提供更加精确的位姿数据,同时可以提高建图与导航的精度。但同时也有许多急需解决的问题,例如里程计打滑、地图融合缺陷等。文中提出了一种新融合方法,即利用二维激光雷达、深度相机、轮式里程计和IMU单元来进行多传感器的融合,通过改进的PL-ICP(Point-to-Line ICP,PL-ICP)算法进行关键帧的截取,弥补普通ICP算法在截取数据和融合方面的不足。将EFK算法直接作用于IMU以保证数据采集的精简和有效。最后通过贝叶斯方法,融合视觉和激光传感器的数据,进行融合二维栅格地图的构建,弥补单一传感器建图的不足。旨在提高室内机器人位姿信息的精确程度,建立信息要素更加齐全,数据更加完善的全局栅格地图,并在实际的环境中证明了融合的有效性。

1 激光点云的帧间匹配

移动机器人的位姿信息可以通过轮式里程计或惯性里程计的方式得到,但是这两种方式求出的里程计信息误差比较大。实际上,激光里程计算法使用多个子图拼接完成地图。不可能将每一帧识别到的点云全部插入子图中,所以激光里程计只选择关键帧的数据插入,而对于不重要的帧数据会被舍弃。传统的轮式里程计的数据则作为激光里程计进行求解的初始迭代值,辅助激光里程计更好求解机器人的位姿信息。

在激光的帧间匹配求解中,迭代最近点(Interative Closest Point,ICP)算法由于不用对点云集分割和特征提取就可以通过对初始值迭代的方式获得良好的匹配效果,所以ICP算法成为了目前研究最多、应用最成熟的算法之一[15]。

1.1 ICP 算法

ICP算法的基本原理如下。

设激光的两个点云帧空间坐标的集合分别为

(1)

式中:xn和pn表示k组对应点中的某一组对应点。对两帧点云的均值分别用ux和up表示。

(2)

(3)

当W满秩时

R=UVT,

(4)

t=ux-Rup。

(5)

综上所述,ICP算法每一次迭代都会对原点集中的每一个点进行遍历,直至找到与目标点集中最近的点[16]。ICP算法的基本过程:①对于需要匹配的两帧激光点云集,寻找点云的最邻近关联点,通常初始的对应点使用轮式里程计的数据进行求取。②根据对应点,进一步求解R和t。③对点云进行转换,计算匹配的误差并判断误差是否满足设定的阈值。如果满足则输出求解的R、t,如果不满足,则继续迭代,直到误差满足设定的阈值。

1.2 PL-ICP 算法

ICP算法中,在寻找对应点的过程中,认为欧式距离最近的点就是点云对应点,但是在实际的室内环境中,激光点到实际环境曲面的距离才是最好的误差尺度,所以标准的ICP算法会造成一定数量的错误对应点[17]。针对这个问题,ICP算法衍生了出了很多改进版本,其中点到线的迭代最近点改进了标准ICP算法的误差方程,利用激光点到下帧激光点云最近两点连线的距离来近似真实的尺度关系,更适合室内场景[18]。

PL-ICP算法的误差方程为

J(Rk+1,tk+1)=

(6)

PL-ICP算法的基本过程为:① 根据车轮里程计的数据获取初始的旋转矩阵;② 转换当前帧激光数据到参考坐标系;③ 对当前激光帧的每一个点,在参考帧中找到最近的两个点并计算误差,去除误差过大的点,构造最小化误差方程;④ 求解R和t并判断误差。如果不满足设定的阈值,则利用此R和t回到Step1继续迭代。如果满足,则输出R和t用作求取机器人的位姿。

PL-ICP是匹配精度和鲁棒性都较高的算法[19],相比于ICP算法求解精度更高,也更适合室内环境,同样的,PL-ICP方法在大空间环境、激光点稀疏的情况下有更好的匹配效果[20]。所以文中的激光前端也将采用PL-ICP算法对激光的帧间匹配进行求解。但是其对初值也相对更敏感,需要更高精度的初值,虽然轮式里程计有频率高、环境负面影响低等优点,但其同样也有测试精度低、轮子容易出现变形和打滑导致测试出现较大误差等问题。如果只使用车轮里程计的数据作为匹配的初值,由于车轮里程计的累积误差很大,很容易导致算法陷入局部循环。

2 多传感器融合的SLAM

2.1 扩展卡尔曼滤波融合轮式里程计和IMU

由于PL-ICP算法对初值要求相比于ICP算法更高,而轮式里程计会随着机器人的运动和轮胎打滑等情况导致累积误差越来越大,如果仅使用车轮里程计的数据作为PL-ICP算法迭代的初值可能会导致PL-ICP算法陷入局部循环的情况。而IMU虽然存在积分位姿发散的问题,但瞬时姿态准确,所以可以利用扩展卡尔曼滤波将车轮里程计和IMU进行融合,提高位姿的准确性,并将融合后的位姿数据作为PL-ICP算法的初值,避免其陷入局部循环的情况[21]。

扩展卡尔曼滤波通过将非线性函数进行一阶泰勒展开来实现非线性函数线性化,基本过程如下。

状态方程为

xk=f(xk-1,uk-1,wk-1)。

(7)

观测方程为

zk=h(xk,vk)。

(8)

(9)

(10)

(11)

(12)

所以扩展卡尔曼滤波的预测阶段如下。

①先验状态估计为

(13)

②误差的协方差矩阵为

(14)

校正阶段为

① 卡尔曼增益

(15)

② 后验状态估计结果

(16)

③更新误差协方差矩阵

(17)

利用扩展卡尔曼滤波对车轮里程计和IMU的融合过程为:①当第一个传感器的观测方程更新后得到系统的状态量及系统的协方差矩阵将作为第二个传感器校正过程的系统预测状态量和系统预测协方差矩阵进行状态更新;②将更新后的系统状态估计结果和协方差矩阵作为融合后的输出,将二者用于预测过程进行下一时刻的迭代[22]。为了保证后续真实环境的融合实验能够顺利进行,在仿真软件中对算法进行测试,验证算法是否能够正确订阅IMU和里程计节点的信息并输出融合后的里程计数据。算法在ROS下的仿真融合计算图如图1所示。

图1 仿真融合计算图

通过图1中箭头的指向可以看到,融合节点robot_pose_ekf分别订阅了底盘的odom话题和imu_data话题,之后将融合的结果以odom_combined话题发布出来同时发布机器人的tf变换,最后通过odom_ekf节点转换odom_combined的格式以odom_ekf话题发布出来。最终融合的结果如图2所示。

图2 扩展卡尔曼滤波融合结果

如图2所示,融合成功之后在rviz的odometry里就可以进行订阅。其中白色箭头代表的是融合之后的里程计信息,红色箭头代表的是原始的底盘里程计信息。在仿真软件中通过键盘控制节点控制仿真机器人运动,融合节点就可以实时输出融合之后的里程计数据。融合里程计数据odom_ekf的输出结果测试如图3所示。

图3 融合里程计数据

上述实验均是对融合算法的仿真测试,为了进一步验证扩展卡尔曼滤波对IMU和轮式里程计融合的有效性,在如图4所示的真实场景对机器人的定位准确性进行测试。图中起点位置为机器人开机的位置也是世界坐标系的原点,测试场景的瓷砖大小为65 cm×65 cm大小。为了方便记录,让机器人从下图中起点的位置沿红色长方形轨迹运动至图中终点位置。

图4 实验场景

为了更方便对里程计的数据进行对比,测试过程中选取长方形轨迹中除起点外的剩下三个顶点作为目标点,其中里程计的数据保留小数点后三位,单位为m,最终测试结果见表1。

表1 实验结果

通过上表的测试数据可以看出,在1.3 m×3.25 m的长方形区域轨迹内,通过融合轮式里程计和IMU的数据使机器人的定位精度相比于轮式里程计的定位精度至少提升了33%。由于PL-ICP算法对初值要求相比于ICP要求更高,较差的初值可能会使迭代求解的过程陷入局部循环,所以文中利用扩展卡尔曼融合轮式里程计和IMU的数据,将融合后相对更准确的数据作为PL-ICP算法的初值,一定程度上避免了PL-ICP可能陷入局部循环的问题。

2.2 融合二维栅格地图的创建

由于单纯的二维激光SLAM建图只能扫描到安装高度的信息,缺乏垂直方向的信息,对于室内环境中的办公桌、凳子等物品进行扫描建图时可能会存在信息缺失的情况,例如:当桌面的高度高于激光雷达的安装高度,此时激光雷达只能扫描到桌腿的信息,认为桌腿中间没有障碍物。而如果机器人的实际高度高于桌面的高度,后续导航时,由于激光雷达扫描不到桌面,判定桌腿中间没有障碍物可以通过,这种情况就可能导致机器人损坏。当前,基于特征点和直接法的视觉SLAM算法都是建立在静态环境下进行测算的,但是在现实场景中会出现许多变量,导致SLAM系统的定位精度和鲁棒性严重下降,甚至导致建图失败。并且单纯的视觉SLAM,由于传感器本身的限制,精度比较低,受环境光的影响大,缺乏纹理环境的误差非常大等问题,传统的视觉SLAM算法并不具备对目标的实时感知能力,所以也不适合将单纯视觉建图用于导航。但是视觉SLAM可以扫描到垂直方向的信息,所以可以将视觉建立的局部栅格地图和激光建立的局部栅格地图进行融合。一方面可以弥补激光建图缺乏垂直方向信息的问题;另一方面也可以改善视觉建图精度低的问题。激光和视觉扫描范围如图5所示。

图5 激光和视觉扫描范围

上图中红色的虚线表示二维激光传感器安装位置的扫描范围,只能扫描到安装高度平面的物体。紫色圆锥虚线代表视觉传感器的扫描范围,文中使用的Xtion Pro live深度相机的垂直方向视野范围为45°、水平方向为58°。

由于最终建立的是二维的栅格地图,而深度相机的深度点云信息是三维的,所以需要将三维的深度信息投影成二维的伪激光数据,然后构建二维栅格地图。最终激光和视觉建立的局部栅格地图融合过程为:二维激光雷达获取点云信息之后将激光获取的障碍物信息根据求解的机器人位姿转化到栅格地图坐标系下,形成环境障碍物的局部二维栅格地图。同时将深度相机的数据投影为伪二维激光数据,同样转化到栅格地图坐标系下形成局部二维栅格地图。然后对两者的局部二维栅格地图进行融合,补充二维激光雷达未获取到的垂直方向的环境障碍物信息,形成全局栅格地图。

对于局部栅格地图的融合,根据栅格地图的基本原理,继续将贝叶斯方法用于栅格地图的融合,可简化得到融合公式为[23]

(18)

表2 局部地图融合规则

如果得到栅格占据的概率p0大于栅格初始设定阈值T0,则当前栅格被占据的概率值设为1,否则仍为p0,其中T0取0.5。则距离传感器测量之后每个栅格的概率值为

(19)

(20)

3 实验验证与分析

3.1 仿真环境分析

使用的机器人为Handsfree,首先利用Gazebo三维仿真软件创建一个和Handsfree平台相同传感器的仿真机器人模型,创建完成后如图6所示。图6为仿真机器人的整体模型,其中红色方框中标示的分别为仿真的Xtion Pro Live深度相机和RPLidar A1激光雷达模型,同时为了确保更好进行后续实验,分别为仿真机器人的各传感器设置了和真实机器人相同传感器的参数。针对文中算法需要验证的实验效果建立仿真实验环境,在仿真实验环境中设置四脚课桌、T型桌子(桌面高度均高于二维激光雷达安装高度),卡车(卡车车身高度也高于二维激光雷达的安装高度)以及方便对比的实心球体等障碍物。其中卡车模型的设置是为了在最终创建的环境地图上更好的展示出融合效果,最终创建的仿真实验环境如图7所示。

图6 机器人模型

图7 仿真实验环境

通过图7可以看出:由于桌面和卡车车身的高度均高于二维激光雷达的安装高度,所以二维激光雷达的扫描点穿过了桌子和卡车,如果是单一的二维激光雷达SLAM,那么就会认为此处没有障碍物的存在,从而导致后续导航时机器人可能和桌子以及卡车发生碰撞。通过对上图中的仿真实验环境进行单一的二维激光传感器建图和文中的多传感融合方案建图实验,进行融合的有效性验证,实验过程中保持机器人运动的角速度和线速度相同。结果如图8所示。

图8 仿真实验对比

通过图7的建图结果结合仿真实验环境可以看出:由于二维激光雷达只能扫描到安装高度的物体,无法扫描到仿真环境中高于激光雷达安装高度的卡车主体和桌面,如图7中仿真环境中蓝色的范围即为激光雷达的扫描点。由于没有卡车主体和桌面的激光信息,建图的时候就会认为此处没有障碍物。最终建图结果如图8(a)所示,对于仿真环境中的桌子和卡车不能建立完整的障碍物地图信息。而融合建图,由于深度相机可以扫描到卡车主体和桌面的信息,并将这些点云信息投影成伪二维激光数据进行建图,所以能很好将桌子和卡车的完整信息建立到最终的栅格地图上,如图8(b)所示。这样就避免了后续导航时机器人可能和障碍物发生碰撞的风险。

3.2 真实环境实验验证与分析

为了进一步验证融合的效果,在如图9所示约70m2的真实环境中对算法进行测试,验证算法在实际环境中的有效性。

图9 真实实验环境

图9(a)为实验环境的整体情况,图9(b)为实验环境前部。在图9(b)中右侧有一排会议桌,其桌面下方留下的空隙高度高于移动机器人的激光雷达安装高度。

如图10所示,图10中红色圆圈部分为激光雷达和会议桌下方空隙的高度对比,由于会议桌的空隙高度大于激光雷达的安装高度,所以单一二维激光雷达进行建图的时候不会将会议桌上方的信息构建到最终的环境栅格地图中去。

图10 激光雷达安装高度

在实验过程中,保持机器人的线速度为0.2 m·s-1,转向的角速度为30°/s。在如图11所示的位置开始出发,顺时针绕实验环境一圈并回到原点。

图11 机器人出发位置

作为对比,分别利用基于单一二维激光雷达的Gmapping算法和文中的融合算法进行建图测试,实验过程中保持机器人的运动速度不变,结果如图12所示。

图12 真实环境建图结果

图12(a)中为Gmapping的建图结果,可以看到最终的栅格地图上缺失了会议桌的部分信息,只有会议桌的桌腿信息。而图12(b)中则将完整的会议桌信息添加到了最终的栅格地图上,而这部分信息正是通过融合了深度相机投影成伪二维激光数据所建立的局部栅格地图所获取,从而验证融合建图的有效性。

另外在运行过程中,为了方便对机器人的位姿进行记录,通过计时控制向移动机器人底盘发布前进和转向的指令来控制机器人到达指定的位置。选取了两次实验中相同位置的5个点来进行位姿的准确性分析,结果见表3。

表3 定位结果

通过表3的结果结合建图结果可知,基于单一二维激光雷达的Gmapping算法由于对轮式里程计的依赖比较严重,而轮式里程计信息随着机器人的运动累积误差不断增大,所以最终机器人运动回原点时,建立的环境地图已经发生了错位,位置的均方根误差达到了20 cm左右。而文中的融合算法,由于采用激光里程计,且利用扩展卡尔曼滤波融合轮式里程计和IMU的数据作为激光里程计的迭代初值,获取了更加准确的机器人位姿,所以整体的位姿误差明显小于Gmapping,最终回到原点时机器人的位置均方根误差在12 cm左右,有效降低了机器人的定位误差。

4 结 论

文中对基于单一二维激光雷达的SLAM进行分析及优化,利用扩展卡尔曼滤波融合轮式里程计和IMU的数据为激光的帧间匹配算法PL-ICP提供初始迭代值,同时在建图阶段融合视觉投影为伪二维激光数据建立的局部二维栅格地图。在仿真实验环境和真实实验环境进行了测试,结果表明:文中的多传感器融合的SLAM方案相比于单一的二维激光SLAM,提升了机器人的位姿准确性,同时构建了环境细节更完善的全局地图。