基于四足机器人的强度信息匹配快速激光SLAM算法

技术领域

本发明涉及四足机器人控制领域，具体为基于四足机器人的强度信息匹配快速激光SLAM算法。

背景技术

当前激光SLAM(Simultaneous LoCalizationand Mapping,同时定位与地图构建技术)算法主要以LOAM为主要框架，再在其基础上进行改进。LOAM算法在传感器获取数据过程中，地面情况会导致传感器噪声一直存在，例如，传感器接收到草返回的可能是很粗糙的数据，提取边缘特征会不可靠，因此在进行特征点提取之前需要剔除不可靠数据；在特征点提取过程中，只利用了几何信息，而激光发出和返回的强度信息没有利用；在帧图匹配过程中，需要消耗大量时间计算匹配信息，计算效率低，当前场景下，机器人平台上的计算资源是有限的，因此优化算法对帧图匹配的计算方法尤为重要。基于以上的分析，LOAM的激光SLAM算法已不能满足建图的需要，而基于强度信息的激光SLAM算法，在提取特征点之前进行了剔除噪声，提高对特征点提取的准确性，在帧图匹配，运动估计过程中，在位姿估计处理后会再次进行畸变计算，计算后未畸变的特征更新的地图中，提高相同地点的定位精度，同时减小计算量。

对于基于强度信息的激光SLAM算法而言，目前激光SLAM算法中，实时处理就涉及到点云的匹配，现有的匹配可分为两种，一种是原始点云的匹配，另一种是特征点的匹配，对于原始点云匹配常见的算法有ICP(Iterative Closest Points，迭代最近点)，通过查找欧式空间中最近的点计算对应点，再迭代对应点之间的距离残差，使其最小化，在两个点云中的姿态变换收敛到最终位置。另外一种点云匹配算法是IMLS-SLAM，其通过为每个点赋予权重，每个权重值通过基于目标点的局部曲面法线的IMLS(Implicit Least Square，隐式最小二乘法)计算得到。然而对原始点云匹配的计算消耗太高。LOAM中引入特征点匹配来进行点云的匹配，主要使用的点云匹配方法是提取平面点和边缘点当作特征点，通过对局部点云的平滑度计算区分边缘点和平面点。但是在激光发射与返回的过程中，激光的强度信息并没有考虑在内，只是在几何层面做了分析，信息利用率不高。

LOAM算法的特征点提取只考虑了几何特征，例如边缘点和平面点，而在激光发射和返回的过程中，功率同样是有变化的，在帧-地图匹配过程中，计算时间长，建图效果不佳。本发明在提取特征值的同时加入激光强度的特征，这样更有利于特征点提取的准确性。同时，由于四足机器人平台上的资源有限，无法进行复杂的算法运算，建图时间并不能保证实时的效果，因此在尽量保证精度的基础上简化算法复杂度尤为重要。本发明考虑到现有的激光雷达都在10Hz以上运行，并且每帧扫描时间很短，因此可以假设短时间内的角速度和线速度是固定的，从而预测运动并校正畸变。位姿估计之后，再次计算畸变，并将未畸变的特征更新到地图中，这样可以提高相同地点的定位精度并减小计算量。

发明内容

为解决上述技术问题，本发明提出了基于四足机器人的强度信息匹配快速激光SLAM算法，本发明涉及的激光SLAM算法在特征点提取的信息利用与计算消耗大做了改进，涉及到的特征提取方法中，除了利用几何信息之外，提出将激光的强度作为特征匹配中的重要信息；同时将运动估计模块做了改进，在得到位姿估计后，再次计算畸变，计算后未畸变的特征更新到地图中，提高相同地点的定位精度。

基于四足机器人的强度信息匹配快速激光SLAM算法，包括：

多线激光雷达扫描区域后，生成点云数据后将点云数据作为输入项，利用点云数据进行构建三维地图，实现实时定位；

利用所述的基于强度信息的激光SLAM算法，其中包括以下步骤：

S1.对点云数据滤除噪声并区分地面点和非地面点；

所述的步骤S1中，包括以下步骤：

S11.采用BFS聚类方法滤除点云数据噪声；

S12.对点云进行分割；

S13.对点云数据进行降维处理；

S14.对降维处理后的点云数据拟合直线

S15.筛选地面点和非地面点；

S2.强度标定及点云数据进行提取特征点；

所述的步骤S2中，包括以下步骤：

S21.计算点云的强度信息；

S22.提取平面点和边缘点；

S3.运动估计和畸变补偿；

所述的步骤S3中，包括以下步骤：

S31.对姿态变换矩阵进行求解，激光雷达每旋转一周为一帧，构建帧与帧之间的约束关系；

S32.校正点云；

S4.位姿估计；

所述的步骤S4中，包括以下步骤：

S41.构建点到边及点到面的关系；

S42.计算点到边及点到面的最小权重及位姿估计；

S5.畸变第二次补偿及建立全局地图；

所述的步骤S5中，包括以下步骤：

S51.对优化后的姿态变换矩阵重新计算；

S52.将未畸变的点云更新到由每帧点云形成的地图中。

作为本发明进一步改进，所述的步骤S1具体如下；

首先，对激光雷达获取的点云数据进行聚类分析，采用BFS聚类方法，剔除少于30个点的聚类，进而滤除噪声点云；

之后，将滤除噪声后的点云数据投影到平面x-y坐标系上，将x-y平面视为被Δα划分为M个扇形的圆面，因此将平面分割为M块：

为了识别将投影到x-y平面的点定位到哪一块，采用下式进行处理：

其中，p

之后，对投影后的坐标进行降维处理，由于投影后损失了垂直方向的信息，因此将三维坐标点变为二维坐标点，即(x，y，z)＝(d，z)，其中

在得到降维后的点云数据后，构建d-z平面离散点，针对这些离散点在每一个分割块中采用y＝mx+b进行线性拟合，拟合的直线均方根误差应小于阈值TRMSE,通过设定两个阈值来区分地面点：

进行地面拟合斜率要保证在阈值内，不在阈值内将会呈现垂直结构，同时当斜率m趋近于0时，截距b的绝对值也不应超过阈值Tb，斜率很小时说明地面平整，截距代表了该点距离地面的高度，设定阈值Tb，就能保证不会把高出地面的点云认为是地面，最后根据拟合数据进行区分地面点和非地面点。

作为本发明进一步改进，所述的步骤S2具体如下；

对接收的强度信息进行标定，标定结果应用于特征点提取；

1)强度标定；

强度值为反射信号接收的能量与发射的激光功率之间的比率，其中，接收功率P

P

强度测量I则为：

η

激光扫描测量距离，因此，分析局部法线来估计入射角，找两个最近点p

入射角则为：

2)特征提取；

在提取特征点时，将几何信息与强度信息一起加入,在点云P中，对于每个点p

其中，|N|是附近点的总数，因此特征根据下式的权重计算而出：

其中，w

作为本发明进一步改进，所述的步骤S3具体如下；

从k-1帧到k帧的特征点变换是由旋转和平移变换得到的，获取到相邻两帧的特征点后，得到由k-1帧到k帧的变换矩阵，而激光雷达旋转一周所需的时间段内可以通过线性插值计算每个时刻的变换矩阵；

认为激光雷达的角速度和线速度在很短的时间内是不变的，当激光雷达在k帧时，机器人的变换矩阵为T

其中ξ∈se(3)两帧之间间隔为δ

其中，exp(ξ)是李代数到李群的变换；

当前扫描的第k帧点云P

作为本发明进一步改进，所述的步骤S4具体如下；

各帧进行畸变补偿之后；

运动估计通过排列未畸变的边缘特征

同样，通过位姿得到地图中的面和表面的法向量，而面的法向量的特征向量有一个最小的特征值。得到优化后相一致的边和面后，就从投影的点云中的每个特征点找到最终地图的边和面，通过最小化特征点和地图中的边和面的距离来估计优化后位姿的一致性，边缘点和地图中的边的距离为：

其中，p

面特征和地图中的面之间的距离为：

其中，p

在连续扫描中，由于提取的特征点都是具有较高σ的边缘点和较低σ的平面点，因此引进权重来平衡匹配处理过程，定义局部平滑度来用于确定权重函数，对于边缘点p

其中，

新的位姿由点到边的和点到面的最小权重和估计：

min∑W(p

优化后的位姿可以通过高斯牛顿方法求解非线性等式得到，雅可比矩阵通过左扰动模型求解：

其中，δξ∈se(3)为扰动量，[T

通过求解非线性优化问题，可以得到基于上述对应关系的里程估计，当前位姿矩阵为T

作为本发明进一步改进，所述的步骤S5具体如下；

最终地图由边缘地图和平面地图构成，并且基于关键帧更新，当平移变化好于预定义的平移阈值或旋转变化好于预定义的旋转阈值时将此帧作为关键帧，基于关键帧地图更新会减少计算损耗，畸变补偿时基于匀速运动的机器人，然而这种方法相比于迭代计算精度会降低，因此，对于优化后的位姿矩阵T

Δξ

最后将重计算未畸变的边缘特征和平面特征分别更新到全局地图中的边缘和平面上，形成全局地图。

有益效果：

本发明公开了基于四足机器人的强度信息匹配快速激光SLAM算法。由于当前的激光SLAM算法，以LOAM为代表，在特征点提取过程中，只考虑了几何信息，而在激光发射和返回的过程中，激光强度信息经常被忽略；同时，LOAM算法在帧-地图匹配过程中需要大量的迭代操作以完成帧-地图匹配操作，因此在四足机器人平台上需要丰富硬件资源来完成迭代运算，然而由于机器人平台上的硬件资源有限，算法运算量大，耗时长，实时建图效果不佳。本发明涉及的激光SLAM算法在特征点提取方面和优化算法复杂度方面做了改进，涉及到的特征提取方法中，除了利用几何信息之外，提出将激光的强度作为特征匹配中的重要信息，强度信息中包含环境结构的反射率，该环境结构显示了不同对象的分布，因此强度信息有助于识别多个帧中的相同特征；同时将算法运动估计模块做了改进，在得到位姿估计后，再次计算畸变，计算后未畸变的特征更新到地图中，提高相同地点的定位精度。

附图说明

图1是本发明具体流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

本发明提供一种基于强度信息的快速激光SLAM算法，基于强度信息，是基于激光雷达发射和返回的功率变化信息，考虑到多数算法在特征匹配过程中只考虑了点云的几何信息而忽略了激光的强度信息，提出将强度信息加入其中；帧-地图匹配和运动估计模块，由于四足机器人平台资源有限，且帧-地图匹配过程计算量大，运行时间长，实时性较差，考虑将帧-地图匹配过程中的匹配算法加以改进，在得到位姿估计后，再次计算畸变，将计算后未畸变的特征更新到地图中，提高相同地点的定位精度。具体流程如图1示。

本发明中，激光雷达获取周围环境的激光点云，将点云信息转为欧式距离，每个点都有唯一的坐标点，之后点云数据进行分割，区别地面点和非地面点，之后提取特征，其中包括平面点和边缘点以及计算点云的强度信息。再进行特征匹配，匹配好后继续得到位姿，再次计算畸变，将未畸变的特征更新到地图中。

(一)点云处理。

首先，对激光雷达获取的点云数据进行聚类分析，采用BFS聚类方法，剔除少于30个点的聚类，进而滤除噪声点云。

之后，将滤除噪声后的点云数据投影到平面x-y坐标系上，将x-y平面视为被Δα划分为M个扇形的圆面。因此将平面分割为M块：

为了识别将投影到x-y平面的点定位到哪一块，采用下式进行处理：

其中，p

之后，对投影后的坐标进行降维处理，由于投影后损失了垂直方向的信息，因此将三维坐标点变为二维坐标点，即(x，y，z)＝(d，z)，其中

在得到降维后的点云数据后，构建d-z平面离散点，针对这些离散点在每一个分割块中采用y＝mx+b进行线性拟合，拟合的直线均方根误差应小于阈值T

进行地面拟合斜率要保证在阈值内，不在阈值内将会呈现垂直结构，同时当斜率m趋近于0时，截距b的绝对值也不应超过阈值T

(二)强度标定及提取特征点

对接收的强度信息进行标定，标定结果应用于特征点提取。

1.强度标定。

强度值为反射信号接收的能量与发射的激光功率之间的比率。其中，接收功率P

P

强度测量I则为：

η

激光扫描可以很容易测量距离，因此，可以分析局部法线来估计入射角，可以找两个最近点p

入射角则为：

2.特征提取。

在提取特征点时，将几何信息与强度信息一起加入。在点云P中，对于每个点p

其中，|N|是附近点的总数，因此特征可以根据下式的权重计算而出：

σ

其中，w

(三)运动估计和畸变补偿

从k-1帧到k帧的特征点变换是由旋转和平移变换得到的，获取到相邻两帧的特征点后，可以得到由k-1帧到k帧的变换矩阵，而激光雷达旋转一周所需的时间段内可以通过线性插值计算每个时刻的变换矩阵。

由于现在大多数的激光雷达能在10Hz以上运行，并且连续两帧的扫描时间间隔很短，可以认为激光雷达的角速度和线速度在很短的时间内是不变的。当激光雷达在k帧时，机器人的变换矩阵为T

其中ξ∈se(3)两帧之间间隔为δ

其中，exp(ξ)是李代数到李群的变换。

当前扫描的第k帧点云P

(四)位姿估计

各帧进行畸变补偿之后

运动估计通过排列未畸变的边缘特征

同样，可以通过位姿得到地图中的面和表面的法向量，而面的法向量的特征向量有一个最小的特征值。得到优化后相一致的边和面后，就可以从投影的点云中的每个特征点找到最终地图的边和面。通过最小化特征点和地图中的边和面的距离来估计优化后位姿的一致性。边缘点和地图中的边的距离为：

其中，p

面特征和地图中的面之间的距离为：

其中，p

在连续扫描中，由于提取的特征点都是具有较高σ的边缘点和较低σ的平面点，因此引进权重来平衡匹配处理过程。定义局部平滑度来用于确定权重函数，对于边缘点p

其中，

新的位姿可以由点到边的和点到面的最小权重和估计：

minΣW(p

优化后的位姿可以通过高斯牛顿方法求解非线性等式得到。雅可比矩阵通过左扰动模型求解：

其中，δξ∈se(3)为扰动量，[T

通过求解非线性优化问题，可以得到基于上述对应关系的里程估计。当前位姿矩阵为T

(五)畸变第二次补偿及建立全局地图

最终地图由边缘地图和平面地图构成，并且基于关键帧更新，当平移变化好于预定义的平移阈值或旋转变化好于预定义的旋转阈值时将此帧作为关键帧，基于关键帧地图更新会减少计算损耗，畸变补偿时基于匀速运动的机器人，然而这种方法相比于迭代计算精度会降低，因此，对于优化后的位姿矩阵T

Δξ

最后将重计算未畸变的边缘特征和平面特征分别更新到全局地图中的边缘和平面上，形成全局地图。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。