激光雷达与深度相机融合的SLAM技术研究

2020-08-01张恒徐万红张禹

机械工程师 2020年7期

张恒，徐万红，张禹

（沈阳工业大学机械工程学院，沈阳110870）

0 引言

近年来，国内外对无人车技术的研究取得了极大的进展，在很多结构化环境或人员稀少的环境中均已开始投入使用。无人车的结构体系大体上可分为环境感知、定位导航、规划决策和车辆控制4个模块[1]。其中定位导航模块中的SLAM（Simultaneous Localization and Mapping,SLAM）技术采用边定位边建图的方法，即同时定位与建图，它是指搭载特定传感器的主体，在没有环境先验信息的条件下，在运动过程中建立环境地图，利用所创建的局部环境地图来估计自身的位姿[2]。

目前，按搭载传感器的方式分类主要可分为搭载激光雷达的激光SLAM和搭载相机的视觉SLAM[3]。在实际使用情况中，激光雷达和相机等单一的传感器都存在一定的局限性。激光雷达精度高，但多线激光雷达价格昂贵，极大地增加了产品成本；单线激光雷达的扫描范围仅为一个水平面，难以在复杂环境中单独使用；视觉SLAM在无纹理或光照弱的环境中效果较差。针对上述问题，本文提出了一种将单线激光雷达与视觉相机相融合的解决方案。通过将多个传感器采集的信息进行互补、丰富数据，能有效提高定位精度和建图的观测区域。

1 传感器介绍

本文中采用Hokuyo URG-04LX-UG01 2D激光雷达，如图1所示，具有精度高、分辨率高、工作不受光线影响等优点，其具体参数如表1所示。可用于机器人自主定位与导航、路径规划与避障等。

图1 2D 激光雷达

表1 2D激光雷达参数表

Kinect V1是微软公司于2010年6月推出的一款RGB-D视觉传感器，如图2所示，中间位置是可获取彩色信息的RGB 摄像头，左右两侧摄像头分别红外发射器与接收器，用来获取与彩色信息对应的深度信息。Kinect扫描速度快，并可以同时获得一定范围内的3D 信息，信息量丰富，具体参数如表2所示。

图2 Kinect V1

表2 Kinect参数表

激光雷达有较高的精度但仍存在一定噪声，激光雷达常用数学模型有光束模型和似然场模型。

光束模型假设每帧激光xi是由k个激光点构成xi=（x1,x2, ……,xk）; 每个激光点在击中物体过程中会受到4种干扰，如图3所示。

图3 光束模型

光束模型具有直观的物理意义，但求解每个激光点的期望值均需进行一次光线跟踪，一帧激光则需要计算k次，计算量较大；传感器在非结构化的环境中轻微的运动也会导致期望值发生较大变化，严重影响观测结果。所以采用文献[4]中提到的似然场模型对激光束进行改进：

式中:phit为测量误差；prand为随机误差；pmax为最大测量距离；zhit、zrand、zmax分别为对应的权重，似然场模型将图像高斯平滑后使其获得相对于位姿更加平滑的期望值，可用于结构或非结构化环境且计算量低。

本文采用针孔相机模型对将现实世界中的三维坐标点映射到二维平面的过程进行描述，如图4所示。

图4中O-x-y-z为相机坐标系，O为摄像机的光心。实际环境中一点P，经过O投影之后，落在成像平面O′-x′-y′-z′上，成像点为P′。P点坐标为[X,Y,Z]T,P′点坐标为[X′,Y′,Z′]T,根据三角形相似并化简后得：

图4 针孔相机模型[5]

储存图象信息时使用原点位于物理成像平面左上角上计算机图像坐标系O-u-v。将针孔模型坐标系中P′=[X′,Y′,Z′]T转换为计算机图像坐标系P′=[u,v]T得：

式中：α、β分别表示在u轴上放缩了α倍、在v轴放缩了β倍；cx、cy表示原点平移距离。

将αf合并为fx、将βf合并为fy，对上式化简后写为矩阵形式得：

其中，K即为相机的内参矩阵，可通过K实现相机标定。

2 多传感器数据融合

2.1 点云匹配方法

SLAM在进行前端匹配时常用迭代最近点（Iterative Closest Point，ICP）算法来进行帧间匹配[6]，ICP方法是通过在不同位姿下观察同一物体，得到同一物体的两个不同点云即目标点云和源点云，按照一定约束条件找到最邻近点（pi，qi）,然后计算出旋转矩阵R和平移矩阵t，使得误差函数最小，如图5所示。误差函数E（R，t）为

式中：n为最邻近点对的个数；pi为目标点云P 中的一点；qi为源点云Q 中与pi对应的最近点；R为旋转矩阵；t为平移向量。

由于传统的ICP算法进行匹配时采用点到点的距离作为误差方程往往会造成较大的随机误差，本文采用PLICP算法进行匹配[7]，PL-ICP采用点到线的距离作为误差方程，如图5所示，误差方程为

其中，ni表示曲面对应点的法向量。

PL-ICP算法更加符合实际情况，降低了ICP算法中的误差，提高了匹配精度。

2.2 多传感器数据融合方法

单一传感器具有一定局限性，将两种传感器数据融合来保证测量精度并增加测量范围。根据融合阶段可将数据融合分为像素级融合、特征级融合和决策级融合[8]。

图5 ICP 与PL-ICP 误差函数区别

卡尔曼滤波是一种递归的估计，它的估计过程只与上一个相关状态的估计值和当前状态的实际值有关。卡尔曼滤波数据融合算法信息丢失较少，所以卡尔曼滤波常用来对原始数据进行融合[9]。

使用卡尔曼滤波进行数据处理前应先设定好过程噪声Q与协方差矩阵R。式中A为状态矩阵，因为是一阶，所以取A=1，得:

将贝叶斯估计用于融合多传感器的栅格地图时，可得到改进的融合公式：

为增加数据可靠性、丰富地图细节、提高数据准确性，本文在数据级采用卡尔曼滤波融合激光数据与深度数据，在决策级采用贝叶斯估计融合激光雷达与深度数据生成的局部二维栅格地图，融合流程如图6所示。

图6 激光雷达与Kinect数据融合流程图

3 实验

本文中实验硬件设备如图7所示，采用Turtlebot2作为移动平台，搭载Hokuyo URG-04LX-UG01 激光雷达、Microsoft Kinect V1 传感器、联想拯救者Y-7000笔记本电脑。软件平台为Ubuntu16.04上安装的ROS Kinetic操作系统。

图7 turtlebot2移动机器人

使用Kinect对环境进行扫描建图可以获得更丰富的地图信息，能够将较小的障碍物也在栅格地图中体现出来，但栅格地图精度较差，如图8所示。

图8 Kinect数据栅格地图

仅使用激光雷达作为传感器对环境进行扫描建图，如图9所示，使用激光雷达所建地图精度较高，但激光雷达扫描平面以外的信息没有被采集到，对后续导航工作有一定的影响。

图9 激光数据栅格地图

将两种传感器数据在数据级与决策级进行两次融合后生成栅格地图，如图10所示。

图10 激光和Kinect数据融合栅格地图

可以看出，数据融合后生成的地图既保留了激光雷达建图精度高的特点，同时也通过三维投影采集到多个水平面的环境信息，得到信息更加丰富的栅格环境地图，降低了机器人与其他障碍物发生碰撞的风险。为后续的导航、避障工作提供了更加准确的环境地图。

4 结语

为弥补2D激光雷达或Kinect单一传感器进行SLAM存在的缺陷，本文采用将激光雷达数据与Kinect数据进行两次融合的方法构建环境地图。在数据级使用卡尔曼滤波融合激光数据与Kinect的深度数据，在决策级使用贝叶斯估计融合栅格地图。经过两次融合后得到更加接近真实环境的栅格地图，可使移动机器人在复杂环境中更有效地工作，同时还提高了地图精度，也为后续的定位、导航、避障工作提供了更完善的地图支持。进一步的研究方向可进行多机的SLAM栅格地图融合，以便在广阔的环境中提高建图效率。