一种基于结构化环境的线性距离特征提取算法

匡 兵，田春月，陈凤冉，孙毛毛

(桂林电子科技大学机械工程学院，桂林 541000)

在传统的里程计导航中，通常采用航迹推算法进行地图构建与定位，但是此方法会随时间增长产生累积误差问题，难以满足机器人长时间运动的需求。激光雷达是一种通过点云数据确定环境信息的主动传感器，具有较高的距离和角度分辨率，因此增加激光雷达传感器可增强建图时对环境的探测和感知。同时使用特征提取算法，可以提取环境特征信息，并在已知机器人地标动态模型的基础上，获取位姿信息用于地图构建[1]。线性距离特征提取算法通过对环境形状的简单假设，估计所需线条特征，只提取结构化环境中可用于建图的特征线，便可以描述所需的环境结构[2]，可较有效地应用于即时定位与地图构建(simultaneous localization and mapping，SLAM)中，同时相对于传统的特征提取算法，此算法与地图的匹配精度更高。

激光雷达建图通常采用基于点云的匹配和基于特征的匹配两种匹配技术[3]。传统的ICP(iterative closest point)算法是基于点云的匹配技术，首先构建两个点云集合中当前帧和之前帧的误差均方根，通过迭代使得到误差的非线性方程组的解达到最小，从而达到建图的目的。但是由于加入速度信息后会产生严重的运动畸变，因此ICP算法不适用于机器人在动态环境中的运动。基于特征的匹配技术需要将二维特征地图的原始扫描点转换为几何特征。而传统的特征匹配算法如Hough变换通常是对匹配的图像进行灰度处理后直接从扫描的图像中提取直线，但是这种方法在估计直线参数时没有考虑噪声和拟合的不确定性[4]。在此基础上发展起来的线性回归算法，采用的是直接在空间中搜索参数域，然后在相邻线段中分布式的分析集群信息，其过程十分复杂，不利于建图的实时性。随机样本一致性(random sample consensus,RANSAC)算法，是一种在存在异常值的情况下对模型进行稳定拟合的算法，可用于多种特征类型的分割，在图像特征提取中应用广泛[4]，但是在真实环境中，这种概率算法无法最大限度地利用点云数据，同时过度迭代可能会陷入局部极值问题。

采用线性距离特征提取算法，对扫描得到的原始激光点云数据进行初步的聚类分割，通过选定合适的阈值，对不同区域的点云数据分别进行最小二乘拟合。构建建图所需的正交线类别，并使用最近邻算法(KNN)对正交线进行筛选，通过线段合并和偏差的矫正完善特征提取算法，生成环境地图。以期较为有效地降低建图的时间复杂度同时提高地图匹配精度。

1 机器人运动建模

1.1 速度运动模型

图1 机器人运动模型

在平面上运动的机器人，可通过构建基于条件概率的运动模型，表示其在环境地图中的状态变化[5]。其中速度运动模型是通过构建旋转和平移的速度矩阵，用于控制机器人的位姿变化。其运动状态用位姿表示时，通常使用x、y、θ三个变量进行描述。其中x、y为机器人所在的二维平面的坐标；θ为机器人航向角；w为运动角速度；v为机器人运动的切线速度。

如图1所示，令xt-1=(x,y,θ)T为机器人在t-1 时刻的状态估计，假设机器人随时间的运动是恒定的，则有机器人在t-1时刻的位姿xc、yc分别为

(1)

(2)

可以推导出机器人特征点经过Δt时间后的位姿(x′,y′,θ′)，其中x′,y′为机器人变换后的位姿坐标，θ′为变换后的航向角。

(3)

(4)

(5)

(6)

1.2 地标测量模型

在机器人速度模型的基础上，建立地标测量模型并与环境地图相关联，为地图的聚类分割和特征提取提供测量模型。地标测量是将机器人自身局部坐标系的方向和距离与环境特征点的位姿相匹配，从而为机器人的导航奠定基础。

一般用三维列向量表征环境特征，如式(7)所示：

(7)

式(7)中：f(zt)为建立的环境特征向量组；ft为表征环境特征的一组列向量；rt为特征点相对于原点的距离；Φ表示方位；st表征环境地标的类型。

假设特征之间是相互独立的。可以建立在环境地图m={m1，m2，…}下的条件概率模型p[f(zt)|xt,m]，其公式如式(8)所示：

(8)

式(8)中：xt为当前的状态估计值和环境信息，其值可估计对应的特征函数。

由1.1节可知，机器人的位姿可以用xt=(x′,y′,θ′)T表示，由此可得一致状态量在t-1时刻的概率分布。通过给定t时刻的观测数据可以估计出状态量在t时刻的特征向量：

(9)

式(9)中:mj,x、mj,y表示特征点在世界地图下的坐标值;εσr、εΦr、εsr为在存在噪声条件下的高斯误差变量。

2 激光点云数据聚类与拟合

2.1 点云聚类与分割

设采集到的激光点云数据集合为Ni，其中每一个激光点坐标为(x1,y1),(x2,y2),…,(xn,yn)。根据选定的距离参数对样本间的距离D的大小进行分类[6]。

首先采用欧式距离计算任一时刻两个激光样本的间距：

(10)

其中设具有同一点云特征的样本集合为z1=(xi,yi)，为了对点云数据进行合理的划分，需要人为的确定点云间的距离参数Tmax。若Di>Tmax，则需重新确定一个特征点集区域z2=(xi+1,yi+1)，将样本点划分为z2区域。若Di

2.2 迭代端点匹配及最小二乘拟合

经过聚类分割后的点集不能直接生成适用于线性拟合的最小集合，因此需要将各个区域内的样本点进行迭代端点匹配，将样本集达到分割最大化，从而进行最小二乘拟合[8]。

图2 点云分割模型

如图2所示，首先选取分割好的样本点区域，在此基础上进行迭代端点拟合(IEPF)。选定集合中的首点ds和末点dm并确定一条线段，人为选定线段的距离参数Lmax，在距离线段最远处选取一点dn，若存在Ln>Lmax则对线段进行分割。之后选取分割点与起始终止点组成的线段重新进行分割，重复此过程，直到该点到直线的距离L

在同一区域中选定较长的基线，并以其为基准对区域内的线段进行基于线性回归的最小二乘拟合，对其他线段相对基线的距离求解误差的平方和，从而将激光数据的点云信息拟合成所需的特征线段[9]。

3 基于竖直和水平特征线提取算法

在特征提取法中，需要对前文得到特征线段进行分类和筛选，可将样本区域中拟合的线段分成三类，分别是竖直和水平的特征线，轻微偏移的非正交直线以及其他类型的特征线。提取水平竖直线段，作为维护重建地图所需的主要线，同时对轻微偏移的非正交直线进行矫正，并将其他类型舍弃[10]，最后通过仿真实验评估其性能。

如图3所示，首先通过一次扫描确定世界坐标(xe,ye)的方向，并以此为基准，在各区域拟合好的线段中定义出相对于世界坐标方向的角度参数，选定正交线允许的最大角度公差Δφ，作为筛选特征线的最大范围。若拟合线段相对于基准坐标的角度参数大于最大角度公差Δφ，则剔除该拟合线段。通过剔除角度偏差过大的线段，最终筛选出用于构造结构化环境地图的正交或轻微正交的直线。

图3 水平竖直线提取

在特征线搜索过程中，为了简化搜索样本，采用KNN算法将搜索简化。若两个样本特征足够相似，则有更高的概率为同一类别。根据上述筛选后的环境特征，判断样本的角度公差，作为其类型标签，继而判断待筛选的线段是否为水平或竖直线段[11]。

选择样本最近的k个邻近值，记作l1，l2，…，lk，计算所有训练样本的距离，其对应的类标签为u1，u2，…，uk，则样本整体可记为{li,ui}。此时k所对应的训练样本lk返回的类标签为最大角度公差，即u=Δφ。当搜索到正交线的邻近值线段时，可以基于邻近值线段的类标签来判断此线段的类别。若其邻近值线段与样本值线段的角度公差相同，均小于最大角度公差，则划分为同一类。最终，筛选出具有相同角度公差的类标签线段，完成特征线段的筛选。

4 正交线矫正与线段合并

4.1 正交线矫正

为避免提取的线段受到噪声或动态环境变化的影响，导致定位参考坐标系和全局坐标系方向不一致造成线段微偏，如图4所示，需要矫正所提取的线段中轻微非正交部分，以得到正交线[12]。

图4 正交线矫正

如式(11)所示，选取样本线段l1、l2，通过计算水平直线的角度加权平均值w1、w2、w3确定水平参考方向xz，减去π/2即可得到垂直的参考方向yz。由于线段的中点处特征提取较完整，受噪声干扰较小，因此，需要将观察的线段围绕其中点旋转后与参考的方向(xh,yh)对齐，直到xh和xz之间的相对角正交。

(11)

4.2 正交线段合并

在实现正交特征线段矫正后，为了简化图形的复杂程度，避免线条过于杂乱，需将观测到的水平和竖直线中最长的一段作为基线输入[13]。计算周围线段相对基线的距离，并求出其距离的线性方程组的解，最终将求得的解分别估计水平和竖直线。

如图5所示，观察到的垂直线的集合为xd={l12,l13,l14,l15,l12,l35,l45,l56,l57}。假设xd中的所有元素并不相关，则特征线之间的距离关系可以用连续时间内的线性方程组l13+l35-l15=0,l14+l45-l15=0描述，其中u1,u2,…,u7为特征线之间的距离。使用卡尔曼滤波对此进行状态估计，求出其最小解的集合即为线段合并后的最小距离。

图5 特征线合并

最终得到整体算法流程图如图6所示。

图6 线性距离特征提取算法流程图

5 实验评估

5.1 环境地图对比

为验证算法的有效性，使用2D思岚激光雷达进行试验，其帧率为10 Hz，测量半径为10～12 m，将算法在MATLAB平台中分析。首先选择可用来实验的室内结构化环境。之后使用2D思岚激光雷达对环境进行扫描并采集数据，采用特征提取算法构建环境地图。同时在相同的实验环境下，与传统的ICP点云匹配算法对比，分析两种算法在建图完整性上的优劣。首先获取来自激光雷达的原始点云数据，并求出相邻点云距离，使用聚类分割算法选定合适的阈值Tmax进行特征区域分割。如图7所示，紫色部分为原始点云信息，绿色部分为采用特征提取算法聚类分割后的部分点云拟合线段。

图7 聚类分割算法

如图8所示，为了避免区域分割后点云数据较为离散，使线段更加完整，需在此基础上进行最小二乘拟合，得到连续的地图边界，如图8中红色部分所示。

图8 线段拟合

对已提取的水平竖直特征线，采用KNN算法进行竖直和水平特征线段的分类，得到与标签类型相似的正交线段。同时，通过滤波可滤除与建图无关的非正交线段。对杂乱的线段进行合并处理，使离散的线段与基线合并，得到如图9所示的较为完整的地图边界。

图9 正交线合并

如图10所示，在同一环境下，采用点云匹配算法(ICP)进行对比实验。相比特征提取算法，ICP点云匹配算法所建立的地图在边界处较为模糊，同时实验中噪声点过多，因此需要进一步进行高斯滤波处理，此方法计算负荷过大，会导致内存消耗增大，同时对算法进行进一步优化，加大了计算复杂度。

图10 ICP-SLAM点云匹配算法

5.2 地图匹配精度分析

衡量地图构建精度的常规方式有两种：第一是对比实时建图的质量，第二是通过算法生成的地图边界计算平均定位误差的大小[14]。

将竖直和水平线的特征提取算法和目前ICP点云匹配算法进行对比。如图11所示，试验使用KITTI数据集，调用两种算法并建图。对比运动过程中机器人实时建图的质量。由图11可知，所提出的特征提取算法(蓝色)与地图(虚线)的匹配程度更好。

如图12可知，两种算法分别在x、y两个位姿方向上相对于真实轨迹坐标的定位误差。特征线提取算法(蓝色线段)明显更接近于真实轨迹部分(虚线)。因此，特征点匹配法在相对结构化的环境中地图的匹配度较好。

虚线为数据集的真值；蓝色线段为ICP-SLAM算法生成的轨迹；绿色线段为特征线提取算法(ORTH-SLAM)生成的轨迹

图12 算法位姿偏差比较

如表1所示，为了对比平均定位误差的大小，选取相同的数据集，测试得到线性距离特征提取算法的占用率较低，为39.57%，表明此算法在一定程度上可降低计算的复杂度。相同情况下，平均定位误差为2.7 m，可满足工业生产中的定位精度要求，同时与地图的匹配程度较高。因此本文算法可较为有效地减少内存的占用率，适用环境范围更广。

表1 地图匹配结果分析

6 结论

提出了一种用于机器人在结构化环境中建图的特征提取算法，并得到以下结论。

(1)该算法首先建立了机器人的动态地标模型，得到机器人的实时位姿。

(2)通过聚类分割算法，对原始的激光点云数据进行预处理，并将区域分割后的点云数据进行最小二乘拟合。

(3)建立线段相对于世界坐标的方向基准，通过对比最大角度公差，筛选出竖直和水平的直线用于地图的创建。

(4)最后使用最近邻(KNN)滤波法对拟合的线段进行处理，以快速筛选出具有竖直和水平特征的线段。

该算法通过与传统的(ICP)点云匹配算法比较，其优点在于在动态的建图环境中较少受到运动畸变的影响，同时建图较完整。同时与传统点云(ICP)匹配算法相比，该算法一定程度上降低了运算的复杂度，使地图轮廓清晰度提高，同时与地图匹配度较好，可用于解决优化激光点云在边缘处点云稀疏的问题。