基于激光雷达SLAM的三维点云自适应算法

姜晓勇,吴起威,魏 璇,应凯健,陈奕磊,魏益民,王郑杭,陶慧翔

(1.浙江科技学院 机械与能源工程学院,浙江 杭州 310000;2.浙江大学 机械工程学院,浙江 杭州 310058)

1 引 言

随着自主导航技术的进步、巡检机器人发展前景广阔。SLAM是巡检机器人在巡检自主导航过程中不可或缺的关键技术。但是在定位建图过程中存在易移动物体会大大降低SLAM建图精度,影响巡检机器人的自主导航巡检效率,因此提升SLAM定位建图精度对巡检机器人自主导航有着重要意义。

此外,由于计算机视觉技术的极速发展,三维点云在巡检行业的应用越来越广泛。激光雷达等扫描仪设备获取三维点云,被广泛地应用在自动驾驶[1-2]、地形测绘[3]、文物保护[4-5]、四足机器人定位与导航等领域。现有的三维激光扫面设备可以快速获取大量的点云数据,大量的点云数据在存储、操作、显示上,会占用大量的计算机资源[6],包括计算机内存、CPU等。因此,对点云数据进行精简处理,提高运行效率极其重要。

迄今为止,点云数据最具代表性的精简方法通常包括不顾及点云特征方法和顾及点云特征的方法[7]。FILIP等[8]将点云所在的最小包围盒划分为相等的小长方体,取接近重心的点代表小长方体的全部点云,实现点云精简。Fleishman等[9]提出包围盒法多适用于曲率较小的数据,对于曲率起伏较大的数据,若压缩率较大,则精简后的点云不能保留较多的特征点,会影响数据的细节表达。Pauly等[10]提出随机采样算法没有考虑点云数据的形状和特征,在对点云数据[11]随机采样时,若压缩率[12]较大,可能会导致点云特征信息丢失。李青蒙等[13]提出曲率采样算法适合于曲率较大的数据,对于平坦的数据,若压缩率较大,可能会造成点云数据的大量缺失。

为了尽量保留点云数据的特征区域和轮廓信息的情况下剔除三维点云数据中存在的大量冗余数据点,提出了一种基于曲率的点云数据分类简化框架,该框架使用了FPS算法[14-15]提取分布均匀的质心点,能保证点云数据的均匀分布,使用高斯曲率将数据点分类,能够很好的消除三维点云数据中特征不明显区域的冗余数据点。与现有的一些点云采样方法相比,该框架能在保持相同精简率的情况下更好的保留点云数据特征明显的区域。

2 激光SLAM原理分析

SLAM是指机器人在未知环境下实现对自身定位的同时构建它周围世界的环境地图。SLAM问题的示意图如图1所示,图中坐标系分别为:位置固定的世界坐标系和随机器人移动不断变化的传感器坐标系,SLAM的实质是在机器人移动的同时不断求解当前传感器位置on相对于世界坐标系O的位姿矩阵T,将传感器坐标系下采集的环境数据添加到世界坐标系下,增量式地构建环境地图。激光雷达扫描出的点云信息可以较好地表现出环境中的几何信息,能够体现环境三维空间结构信息,激光雷达传感器的简化模型[16]如图2所示。

图1 SLAM问题示意图

图2 激光雷达传感器简化模型

点P为激光雷达扫描得到的一空间激光点,根据飞行时间差计算P点与激光雷达中心的距离为R;θ为激光点P对应的水平扫描角度;β为激光点对应的竖直扫描角度,每束激光对应的竖直角度β是固定的,由激光雷达结构决定。

在激光雷达模型下,计算在激光雷达坐标系下点P的坐标,其数学表达式如式(1)所示:

(1)

若为非多线激光雷达,则上式中Z方向即为零。

激光SLAM技术已经日渐成熟,目前大多数的SLAM建图是根据点云数据实现的,但是由于点云数据的局限性,如数据量大,占用空间大,以及数据离散等问题,会导致常用的导航算法难以区分栅格地图上的有效空闲区域,占用区域和未知区域[17]。

3 方法描述

激光点云畸变会使扫描得到的激光点云与空间中的实际位置不一致,影响激光帧间点云匹配以及激光点云和地图匹配的准确性,为优化由于激光雷达运动造成的点云畸变问题,本节针对高频激光位姿以线性插值的方式优化激光点云畸变。对高频激光位姿进行线性插值,将激光雷达非匀速运动补偿为匀速运动;按照匀速运动模型计算出每个激光点对应的位姿转换矩阵,通过转换矩阵将所有激光点转换至该帧激光扫描结束时刻,计算出激光点在结束时刻的实际位置。对运动畸变优化后的点云数据做精简优化。图3为高频激光点云畸变纠正示意图,t0为高频单帧激光扫描的首激光点的时间,该帧扫描最后一个点的时间为t1,通过线性插值优化将激光雷达在(ta,tb)时间段内的运动近似为匀速运动,即将S0处由激光雷达扫描得到的激光点转换至S1处,依据匀速运动理论模型将S1处的激光点转换到扫描结束时S2处,计算出该帧所有时刻的激光点相对S2处的实际位置。

图3 激光点云畸变纠正示意图

3.1 运动畸变去除:高频激光位姿插值

将激光雷达运动补偿为匀速运动优化处理后,将激光点云畸变按照匀速运动模型进行畸变纠正,为提高算法鲁棒性,将各个高频激光点对应的位姿转换矩阵[18]求出,得到激光点相对于扫描结束位置时的坐标。

在(t0,t1)内根据式(2)对高频激光位姿进行线性插值,计算t时对应的位姿转换矩阵:

(2)

P为(ta,tb)时间段内得到的任意一激光点,通过式(3)计算点P相对于扫描结束位置的坐标:

Pt1=TL(t1,t0)(TLt)-1⊕P′

(3)

点P在扫描结束时相对激光雷达的位置为通过插值法纠正畸变得到的Pt1点坐标,对该帧的所有高频激光点做插值法畸变纠正,纠正后的每帧激光点云就等同于激光雷达在结束位置静止扫描一周后所得到的结果。

为验证优化后算法的提升效果,采用回环实验[19]对优化前后算法定位精度进行测试,其计算公式如式:

(4)

其中,A(x,y,z)为机器人起始点的坐标;B(x,y,z)为机器人回到起点时算法估计的机器人位置;d为算法计算的巡检机器人运动轨迹长度。实验机器人搭载VLP-16多线激光雷达,在模拟变电站环境进行回环实验测得激光SLAM算法畸变纠正前后的定位误差结果如表1所示,从表中结果可以看出,经过插值法点云畸变纠正后的激光SLAM算法定位精度相较于原激光SLAM算法而言定位精度有较大的提高,验证了该畸变纠正方法有效性,可提高巡检机器人定位精度及自主导航[20]效率。

表1 畸变矫正前后误差对比

激光点云的位姿畸变通过激光插值法优化,通过激光位姿插值处理畸变量相对较大的非匀速运动,并对其进行算法的精度测试实验,验证了该畸变纠正方法的对提高SLAM定位误差、减少定位误差的有效性。针对畸变纠正后的高精度SLAM点云,还存在的大量冗余数据点,且占用大量计算机内存。在尽量保留点云数据的特征区域和轮廓信息的情况下剔除三维点云数据中冗余数据点[21]。对点云数据进行精简处理,在降低SLAM定位建图误差的同时,大大提高计算运行效率对巡检机器人自主导航具有重要意义。

3.2 点云处理优化框架

所提出的点云简化框架通常包括两个阶段,算法的流程如图4所示。第一个阶段是通过FPS采样算法和局部点云密度阈值选取质心点,然后用加权最小二乘法估计质心点的高斯曲率。第二阶段根据第一阶段计算得到的高斯曲率对质心点进行分类,然后对质心点邻域进行不同的采样方法简化。以下第3.3和3.4节描述了每个阶段的详细流程。

图4 点云简化框架

3.2.1 第一阶段:质心点的选取和曲率估计

3.2.1.1 自适应点云平均距离

在这一阶段,首先使用平均点云距离阈值选出质心点,通过加权最小二乘法拟合质心点及其邻域的曲面,然后,估计质心点的高斯曲率。第一阶段的实现过程如图5所示。

图5 第一阶段实现流程图

3.2.1.2 自适应点云平均距离

点云平均距离通常可分为整体与局部两大类[22],用于衡量点云数据的离散程度。整体点云平均距离是若干个局部点云平均距离的均值。局部点云平均距离有两种计算方法:a)使用最近点的距离代表该点的局部点云平均距离;b)求该点与其k近邻域距离的均值为该点的局部点云平均距离[23]。如图6所示,点P和Q分别为使用方法a和方法b计算点云平均距离,P的点云平均距离偏小,Q的点云平均距离偏大,以上两种方法都存在缺陷。因此,提出了一种自适应点云平均距离计算方法。

图6 点云平均距离

假设原始点云P(p1,p2,p3,…,pn),使用FPS算法从P中采样分布均匀的点云P′(p′1,p′2,p′3,…,p′m)(m

1) 使用FPS算法从原始点云P中得到点云P′。

(5)

其中,dmin,dmax,daver分别点云邻域距离的最小值、最大值和均值。

3) 利用步骤2)可得到P′中m个点的局部点云平均距离,则原始点云P的点云平均距离为:

4) 点云密度阈值λ可根据具体情况设置为d<λ<3d。在每轮采样选取质心点时,计算FPS算法采样得到的每个点的平均点云距离,将平均点云距离小于λ的点作为质心点,记作C。

3.2.1.3 基于加权最小二乘法的曲率估计

加权最小二乘拟合曲面对点云局部拟合效果较好,根据点的距离约束给每个点设置一个权重,减少了离散点和边缘点对曲面拟合的影响,提高了曲率估计的精度。

已知Pi∈R3是原始点云上的一个点,Ni是点Pi的邻域点集。以Pi为坐标原点创建局部坐标系Pi-UVW,并构建四元数Q(x,y,z,w)。

(6)

(7)

(8)

其中,R是旋转矩阵;t是平移矩阵。

设最小二乘拟合曲面的曲面方程为:

Γi=a0+a1ui+a2vi+a3ui2+a4uivi+a5vi2

(9)

其中,ai(i=0,1····5)是曲面方程的系数,用矩阵表示为:

(10)

EIV(errors-in-variable)模型表达式为:

y=(A-EA)x+ey

(11)

E(e)=0,D(e)=σ2Q,e=[ey,eA]T

(12)

其中,ey为n维曲面方程向量y的随机误差;EA为系数矩阵A的随机误差。

(13)

(14)

在点Pi的邻域Ni生成二次曲面时,点Pj∈Ni与Pi的距离越小,Pj对生成曲面的影响越大,反之亦然,因此点Pj的权值随距离减少而变大,Pj的权值的形式如下:

(15)

其中,d(Pi,Pj)表示P,Pj之间的距离;k表示N中点的数量。拟合权值矩阵的形式如下:

(16)

其中,W0是系数矩阵的列权值矩阵;Wx是系数矩阵的行权值矩阵。

求解上述方程即可解得二次曲面方程待求系数ai,代入至式(17)可得到拟合曲面。

高斯曲率的求解公式如下:

(17)

其中,

E=1+u2

G=1+v2

F=uv

将L,M,N,E,G,F代入式(17)可求得点Pi的高斯曲率。

3.2.2 第二阶段:法向量估计和自适应分类采样

首先将第一阶段得到的质心点通过高斯曲率阈值划分成特征明显区域和不明显区域。对于不同的区域分别使用不同简化率和简化算法进行点云简化。第二阶段的实现过程如图7所示。

图7 第二阶段实现流程图

3.2.2.1 点云法向量估计

(18)

(19)

对式(18)进行特征分解,得到特征值λ0、λ1、λ2,其中λ0<λ1<λ2,对应的特征向量为v0、v1、v2,则点Pi的法向量为协方差矩阵M的最小特征值对应的特征向量v0。

3.2.2.2 自适应分类采样

设高斯曲率的阈值为ε,搜索质心点集C(c1,c2,c3,…,cs)的邻域,邻域内点的数量为N(本文N=20),使用加权最小二乘法估计质心点C(c1,c2,c3,…,cs)的高斯曲率,记作K(k1,k2,k3,…,ks)。根据曲率阈值ε将质心点分成特征明显的点集,记作C1,特征不明显的点集,记作C2。对于点ci∈C,若ki>ε,将点ci加入点集C1,反之将点ci加入点集C2。

对于C1中特征明显区域的点,使用FPS算法保留其邻域内较多的点,一般为邻域内点的数量的80 %,可根据精简要求具体设置邻域内的采样率;对于C2中特征不明显区域的点,首先使用PCA算法估计该点及其邻域的法向量,计算该点与其邻域内点的法向量夹角,记作θ,然后根据法向量夹角阈值α,统计出法向量夹角小于α的邻域点的数量,记作n,防止采样后的点云出现空洞,特征不明显的邻域内的采样率一般不高于80 %,若n>0.8N,删除法向量夹角小于α的前0.8N个邻域点,反之删除所有法向量夹角小于α的邻域点。

4 实 验

本文算法基于3D激光SLAM算法Lio_sam进行SLAM建图。使用所提出的点云采样算法进行点云数据简化实验,并与其他三种点云简化方法(包括均匀采样法、体素采样法、曲率采样法)进行了比较。

4.1 激光SLAM运动畸变去除实验平台

本文实验平台采用巡检机器人外接Velodyne VLP-16激光雷达与电源进行SLAM建图。图8为本文实验平台图。图9为优化后的激光SLAM三维点云图。

图8 实验平台图

图9 激光SLAM三维点云图

4.2 单个刚体点云简化实验

点云实验实验使用斯坦福大学建立的3D点云数据库中的dragon点云数据作为实验样本,分别使用均匀采样法、体素采样法、曲率采样法及本文提出的采样法进行点云精简。原始点云精简后的精简率见表2,原始点云如图10所示,精简后的点云如图11所示,精简后点云局部结果如图12所示。可以看出,本文提出的方法相比其他3种采样算法在精简度相似的情况下,能够更有针对性的保留点云数据的轮廓特征和大量的特征点。

表2 点云数据精简率

图10 原始点云

图11 精简的点云图

图12 精简后点云局部结果

4.3 大型场景点云简化实验

本实验对视觉激光信息融合纠正后扫描获得大场景的三维点云数据进行简化,点云数据来源于巡检机器人SLAM运动畸变优化后获得。原始点云精简后的精简率见表3,原始点云如图13所示,精简后点云如图14所示,精简后点云局部结果如图15所示。可以看出,本文提出的方法相比其他3种采样算法在精简度相似的情况下,能够更有针对性的保留点云数据的轮廓特征和大量的特征点。

表3 点云数据精简率

图13 原始点云

图14 精简的点云图

5 总 结

针对激光SLAM运动畸变的问题,提出了一种针对高频激光插值优化方法。有效减少激光雷达运动产生的畸变,大大提高SLAM建图精度的同时,针对激光雷达运动等三维扫描设备获得的三维点云数据存在着大量的冗余三维数据点的问题,提出了一种基于曲率的点云数据自适应简化框架。该框架能够很好的消除三维点云数据中特征不明显区域的冗余数据点。该框架由两个阶段组成:第一阶段,选取分布均匀的质心点,搜素质心点k近邻域,估计质心点的高斯曲率;第二阶段,使用高斯曲率将质心点分为特征点和非特征点,采取不同的简化方法进行精简。实验结果表明,该框架相比传统点云简化方法能更好的保留点云的特征区域和轮廓,对激光雷达扫描的点云数据用于巡检机器人的自主导航起到了促进作用。