一种基于二维激光雷达数据中直线特征的室内未知环境定位方法与流程

本发明涉及机器人定位技术领域，尤其是一种基于二维激光雷达数据中直线特征的室内未知环境定位方法。

背景技术：

定位技术广泛应用了军事、科技、经济以及人们生活的各个领域，室外定位技术普遍采用全球导航卫星系统(cnss，包括美国的gps系统，俄罗斯的glonass系统，欧洲的galileo以及中国的北斗系统)，但是由于室内定位要求更高的定位精度(cm级别)，以及cnss在室内信号不稳定或者直接没有等原因，所以现有的定位系统很难满足室内的定位需求，而且室内定位准确性差。

目前的室内定位技术，大多针对已知环境，即需要提前对室内环境进行传感器的布置或者提前获得室内地图，比如基于指纹的定位技术、基于wifi的定位技术等等。

然而，室内未知环境的定位，不需要提前对室内环境进行传感器的布置或者提前获得室内地图即可完成室内定位，传统基于激光雷达的定位技术利用的是激光数据的点特征，比如icp算法，当两帧数据间由于遮挡、大位移和大旋转导致数据重合率下降时，这类算法精度会急剧下降甚至崩溃，即稳定性不好。并且需要复杂的策略和公式来搜索对应点，此外还有迭代过程，这些均导致算法运行速度较慢。

技术实现要素：

针对现有技术的不足，本发明提供一种基于二维激光雷达数据中直线特征的室内未知环境定位方法，该方法能够改善现有技术的基于激光雷达的点特征的方法稳定性和快速性的问题。

本发明的技术方案为：一种基于二维激光雷达数据中直线特征的室内未知环境定位方法，包括以下步骤：

s1)、获得室内未知环境的激光雷达数据其中，为第i帧激光雷达数据对应的第j个扫描点，分别为笛卡尔坐标系中的横坐标和纵坐标；

s2)、对每帧数据点集{pⁱ}进行自适应阈值分割，标注得每帧连通域点集

s3)、使用非直线特征分割法对连通域集合进行再分割，生成每帧数据的直线段特征描述符

s4)、对于相邻帧的直线段特征描述符使用聚类算法计算相邻两帧数据之间的旋转角度δβi；

s5)、对于利用聚类算法计算出相邻两帧数据的位移矩阵

δt＝[δp，δq]^t，其中，δp为横坐标位移，δq为纵坐标位移。

进一步的，步骤s2)中，使用自适应阈值分割法标注连通域，具体步骤如下：

s201)、根据激光雷达数据pⁱ，计算相邻采样点的距离dⁱ和自适应分割阈值φⁱ，具体如下式：

其中，为和之间的欧式距离，和为分别为激光雷达数据pⁱ的第j-1和j个数据点，为激光雷达数据pⁱ的第j个数据点的自适应阈值，ξ为环境变量，o为采样传感器位置，为计算辅助点，延长线段至点使

s202)、对于数据点集pⁱ，提取分割点集：

该公式表示将满足条件的数据点标记为分割点其中，阈值t1限定点集中的最小特征点数，jk为分割点下标，k＝0，1，2，...，m，m≤m，j0＝2，jm＝m；

s203)、分割其中，p^i，k表示pⁱ分割出来的第k个子集；整理标注值，得到最终的连通域集合

进一步的，步骤s3)中，使用非直线特征分割法对连通域集合进行再分割，具体步骤如下：

s301)、对于每个连通域ax+by+c＝0为点集p^i，k的两个端点所在的直线方程，计算

其中，a、b、c分别为方程的参数；

s302)、对连通域p^i，k进行直线特征判断，如果满足分割p^i，k为lx，并且更新其中，lx为p^i，k分割出来的子集，lx的表达式如下：

如果满足则p^i，k不分割，保持不变；

其中，length(·)表示集合的点个数，t2为直线特征判断阈值，t3为直线特征中包含的数据点数阈值；

s303)、重复步骤s301)和s302)，直到的子集都满足最后对各个子集进行直线特征的提取，得到直线段特征描述符

进一步是，步骤s4)中，使用聚类算法计算两帧数据之间的旋转角度δβi，具体包括以下步骤：

s401)、对于两帧数据的直线特征集合任意两直线间的角度差集合为其中和分别为直线特征和在极坐标系中极角参数；

s402)、以δθ作为样本进行聚类，取样本数最多的簇作为关键簇，最后对关键簇的所有样本加权平均即可得到最终的位移矩阵δβi，即旋转角度δβi。

进一步的，步骤s5)中，对于两帧数据的直线特征集合结合偏转角δβi，利用聚类算法计算位移矩阵δt＝[δx，δy]^t，具体步骤如下：

s501)、将直线特征中的所有直线旋转δβi，得到新的线段组

s502)、如果对应第i帧的第ki条直线段，在第i+1帧中存在一条或多条直线，使得：

则构成一组直线段对；两帧数据的直线段对集合即为

其中，和表示对应直线在极坐标系中的极角，ζ为极角差值的阈值，为直线段对；

s503)、如果两帧数据中的任意两对直线段对：

和满足条件则可求解位移矩阵

其中，η为极角差值阈值，和分别是直线段和的单位法向量，dh和dq分别为对应直线段和的两直线间的距离，任意两直线段对计算得到的组成位移矩阵集合：

s504)、以作为样本进行聚类，取样本数最多的簇作为关键簇，最后对关键簇的所有样本加权平均即可得到最终的位移矩阵δti。

本发明的有益效果为：

1、本发明使用更加稳定的直线特征，即使大部分受到遮挡，直线特征参数任然保持一样，保证了匹配算法的稳健性；

2、抛弃复杂的搜索对应点或者对应直线的过程，直接转化为聚类问题，一次性求解旋转角度和位移矩阵，同时不存在迭代，保证了算法的快速性，可以在更加广泛的对计算资源要求苛刻的硬件平台上运行，适合工程实用。

附图说明

图1为本发明的流程示意图；

图2为自适应阈值δi计算示意图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明：

如图1所示，一种基于二维激光雷达数据中直线特征的室内未知环境定位方法，包括以下步骤：

s1)、获得室内未知环境的激光雷达数据其中，为第i帧激光雷达数据对应的第j个扫描点，分别为笛卡尔坐标系中的横坐标和纵坐标；

其中，传感器器采用日本北阳hokuyo公司的urg-04lx-ug012d激光扫描仪，将其安装在移动机器人上，机器人在未知环境中移动，按照一定的时间间隔激光雷达采集相邻两帧数据点集pⁱ和pⁱ⁺¹，并使用中值滤波对数据进行去噪处理。

s2)、对每帧数据点集{pⁱ}进行自适应阈值分割，标注得每帧连通域点集

具体步骤如下：

s201)、根据激光雷达数据pⁱ，计算相邻采样点的距离dⁱ和自适应分割阈值φⁱ，具体如下式：

其中，为和之间的欧式距离，和为激光雷达数据pⁱ的第j-1和j个数据点，为激光雷达数据pⁱ的第j个数据点的自适应阈值，ξ为环境变量，o为采样传感器位置，为计算辅助点，延长线段至点使

s202)、对于数据点集pⁱ，提取分割点集：

该公式表示将满足条件的数据点标记为分割点其中，阈值t1限定点集中的最小特征点数，jk为分割点下标，k＝0，1，2，...，m，m≤m，j0＝2，jm＝m；

s203)、分割其中，p^i，k表示pⁱ分割出来的第k个子集；整理标注值，得到最终的连通域集合

具体的，本实施例中，t1取为5，自适应阈值的原理如图1所示，其中，θ为激光雷达相邻采样的角度间隔，根据不同的型号而不同，本实施例采用的是日本北阳hokuyo公司的urg-04lx-ug012d激光扫描仪，θ＝0.36°。

相邻采样点的距离采样欧氏距离系数ξ取值为3，和分别为采样点和到激光测距仪的距离，所以根据这些已知量和余弦定理，具体的计算公式为：

由此可知，对于同一帧数据与采样点到激光测距仪的距离动态相关。

s3)、使用非直线特征分割法对连通域集合进行再分割，生成每帧数据的直线段特征描述符具体步骤如下：

s301)、对于每个连通域ax+by+c＝0为点集p^i，k的两个端点所在的直线方程，计算

其中，a、b、c为方程的参数；

s302)、对连通域p^i，k进行直线特征判断，如果满足分割p^i，k为lx，并且更新其中，lx为p^i，k分割出来的子集，lx的表达式如下：

如果满足则p^i，k不分割，保持不变；

其中，length(·)表示集合的点个数，t2为直线特征判断阈值，t3为直线特征中包含的数据点数阈值，并且本实施例中，t2取为1.5cm，t3取为5cm。直线特征参数使用最小二乘法提取；

s303)、重复步骤s301)和s302)，直到的子集都满足最后对各个子集进行直线特征的提取，得到直线段特征描述符

s4)、对于相邻帧的直线段特征描述符使用聚类算法计算相邻两帧数据之间的旋转角度δβi，聚类算法选为密度聚类算法；具体步骤如下：

s401)、对于两帧数据的直线特征集合任意两直线间的角度差集合为其中和分别为直线特征和在极坐标系中极角参数；

s402)、以δθ作为样本进行聚类，取样本数最多的簇作为关键簇，最后对关键簇的所有样本加权平均即可得到最终的位移矩阵δβi，即旋转角度δβi。

s5)、对于利用聚类算法计算出相邻两帧数据的位移矩阵δt＝[δp，δq]^t，其中，δp为，δq为，t为，聚类算法选为密度聚类算，ζ是为了保证两直线近乎平行，与直线特征参数的提取精度有关，本实施例取为1.5度。η是为了保证两对直线段对不平行，与直线特征参数的提取精度有关，本实施例取为3度；

具体步骤如下：

s501)、将直线特征中的所有直线旋转δβi，得到新的线段组

s502)、如果对应第i帧的第ki条直线段，在第i+1帧中存在一条或多条直线，使得：

则构成一组直线段对；两帧数据的直线段对集合即为

其中，和表示对应直线在极坐标系中的极角，ζ为极角差值的阈值，为直线段对；

s503)、如果两帧数据中的任意两对直线段对：

和满足条件则可求解位移矩阵

其中，η为极角差值阈值，和分别是直线段和的单位法向量，dh和dq分别为对应直线段和的两直线间的距离，任意两直线段对计算得到的组成位移矩阵集合：

s504)、以作为样本进行聚类，取样本数最多的簇作为关键簇，最后对关键簇的所有样本加权平均即可得到最终的位移矩阵δti。

从实施例中可以看出，本发明使用更加稳定的直线特征(即使大部分受到遮挡，直线特征参数任然保持一样)保证了匹配算法的稳健性。抛弃复杂的搜索对应点或者对应直线的过程，直接转化为聚类问题，一次性求解旋转角度和位移矩阵，同时不存在迭代，保证了算法的快速性，可以在更加广泛的对计算资源要求苛刻的硬件平台上运行，适合工程实用。

上述实施例和说明书中描述的只是说明本发明的原理和最佳实施例，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。