一种基于向量权重的激光雷达室内定位方法

本发明属于激光雷达定位领域，尤其涉及一种基于向量权重的激光雷达室内定位方法。

背景技术：

1、随着机器人技术的飞速发展，越来越多的科技产品如无人车、无人机等开始走进人们的生活，而其中实现各类移动机器人智能化的一项重要技术就是定位。在实际应用中，移动机器人在面对复杂场景，如光照变换、动态障碍物多，多传感器融合误差大时，容易导致跟踪失败，进而影响移动机器人的定位。目前，激光雷达室内定位技术达不到作业标准所规定的定位精度，使得室内的装修作业无法依赖移动机器人，例如墙面打磨等作业的移动设备定位技术无法将垂直精度以及水平精度提高至施工要求，所以这类高精度作业完全依赖人工处理。

2、现有的激光雷达定位技术大多结合相机、毫米波雷达、gps定位系统、蓝牙、地磁、超声波雷达、uwb模块以及惯性导航系统等车载辅助设备来共同定位，但不同设备之间的数据信息不一致，处理数据需要大量时间，且不同车载辅助设备存在各自的局部坐标，移动设备的自身坐标，基于环境的全局坐标之间的转换，这必定会加大定位误差，也就无法把定位误差控制在施工要求误差内。所以，在结合不同车载辅助设备进行时会存在算法复杂、效率低下和定位误差较大等问题。

技术实现思路

1、本发明的目的是提出一种基于向量权重和全局匹配的激光雷达室内定位方法，通采用向量权重补偿角度和位置误差，测量偏移角和位置得到纠正，将度数精确到1.1度以下和位置精确到9mm以下。

2、为了达到上述目的，本发明提供了一种基于向量权重的激光雷达室内定位方法，包括：

3、s1：全站仪对现场进行扫描，获得原始的高精度、静态全站仪原始点云数据，将全站仪原始数据中的冗余信息剔除，对所存在的噪声进行滤除；

4、s2：构建激光雷达点云数据；

5、s3：对激光雷达点云模型和全站仪点云模型进行投影，并筛选以及剔除投影图中的微弱特征；

6、s4：将所述全站仪投影网格图分别与激光雷达投影网格图进行重叠度卷积运算，将运算得出的最大卷积结果作为激光雷达点云与全站仪点云在全局坐标系下的绕图像自转中心的匹配度数，并对全站仪点云投影进行旋转，从而完成全站仪点云与激光雷达点云的初步匹配；

7、s5：在进行重叠度卷积运算获得初步匹配度数后，依据现场的实际分布，选取出特征最稳定，且特征信息最丰富的区域作为待初筛基准区域，对各个待初筛基准区域的点云进行插值滤波处理，依据各个待初筛基准区域的位置边界值来设定网格大小，采用自然领域插值法往点云边界内的网格填充点，待插值处理过后进行卷积平滑处理；

8、s6：将进行过插值和平滑处理后的点进行面拟合处理，得到符合基准要求的全站仪点云点集，然后将符合要求的全站仪点云点集作为全站仪初筛基准区域的点云集合；

9、s7：根据各个全站仪初筛基准区域的初筛结果,再次进行迭代筛选全站仪基准特征，在迭代筛选全站仪基准特征前，设置第t次迭代的筛选特征区间εt，面积阈值sroi，同时计算筛选特征区间的轮廓面积s；

10、所述s7中计算筛选特征区间的轮廓面积s的具体步骤包括：

11、第一步，初始化筛选特征区间εt，令筛选特征区间εt与全站仪初筛基准区域的区间大小一致；

12、第二步，筛选特征区间εt遍历全站仪初筛基准区域，判断是否存在非基准特征；

13、第三步，让i＝i+1；

14、若i/3余1，则将εt的长度区域进行缩减并回到第一步；

15、若i/3余2，则将εt的宽度区域进行缩减并回到第一步；

16、若i/3余0，则t＝t+1,将εt-1的宽度和长度区域都进行缩减并更新为εt，接着跳转到第一步；

17、通过特征精筛提取后，筛选出总共k个全站仪的基准特征，然后利用向量权重法对全站仪的基准特征和激光雷达的扫描信息进行对比调整。

18、进一步地，所述步骤s2包括：

19、s21：定点扫描现场环境并生成多帧不同视角的激光雷达点云数据；

20、s22：将多帧不同视角的激光雷达点云数据进行拼接；

21、s23：将激光雷达原始点云数据中的冗余数据进行剔除，经点云预处理后，构建激光雷达点云数据。

22、进一步地，所述点云预处理包括滤波操作。

23、进一步地，所述步骤s3，具体包括：将全站仪点云投影图信息以图像中心的自转轴进行以步长为1度的359次顺时针旋转，并把0度和旋转了359次的图信息分别保存到360个网格图像中。

24、进一步地，所述s6中拟合处理，具体为：

25、s61：对于三维点云空间中的点集存在n(n＞2)个点{p1(x1，y1，z1)，p2(x2，y2，z2)，…，pn(xn，yn，zn)}，

26、通过对三维点云集合的n个点求均值获得点云集合位置均值，点云集合位置均值为

27、

28、则向量为

29、

30、因此平方分量有

31、

32、

33、

34、

35、

36、

37、s62：令并通过svd对cm进行奇异值分解，

38、其中，v＝[a，b，c]，令n＝v，则

39、cm＝u∑vt，

40、可得出拟合平面的方程为ax+by+cz＝d；

41、s63：将点云集合的每个点代入到方程中，可获得每个点到拟合平面的距离，

42、di＝axi+byi+czi-d

43、若di<df，则该点在点云集合中为符合要求的基准点，否则，该点在点云集合中为不符合要求的点；

44、s64：将点云集合中符合要求的基准点再次进行一次面拟合，进而得到更新后的拟合平面参数a，b，c，d，因此更新后的拟合平面方程为ax+by+cz＝d。

45、进一步地，所述向量权重法的步骤具体包括：

46、(a)根据各个全站仪基准特征的全局坐标位置来提取位于同个全局坐标下的k个激光雷达基准特征，然后将选定的k个激光雷达的基准特征分别进行面拟合，获得相应激光雷达基准特征的面拟合参数以及拟合面法向量为

47、同时计算出第t个激光雷达基准特征的质心cent，

48、

49、其中，为第t个激光雷达基准特征中的第i个点，n*为第t个激光雷达基准特征中的总点数；依据激光雷达基准特征的轮廓，计算各激光雷达基准特征的体积vt，t∈[1，k]；

50、(b)将激光雷达基准特征的拟合面法向量和相应的全站仪基准特征的法向量依次投影到yoz，xoz，xoy平面，以投影到yoz，xoz，xoy平面内的投影夹角的角度差依次作为x，y，z轴的旋转角；令则各轴旋转角的计算公式如下：

51、

52、

53、

54、其中，at，bt，ct为第t个全站仪基准特征的法向量的参数；

55、(c)计算各轴旋转权重：令

56、

57、则其中，表示全局坐标的单位向量；

58、(d)根据选定方式获得的各轴旋转权重计算出激光雷达的全局旋转角度，

59、

60、

61、

62、α*，β*，γ*分别为激光雷达的绕x轴、y轴、z轴的全局旋转角度；

63、(e)将激光雷达扫描的点云依次绕z轴旋转γ*度，然后绕y轴旋转β*，最后绕x轴旋转α*度，并把旋转后的质心cent(xt，yt，zt)代入相应的全站仪基准特征的面拟合方程，求出第k个激光雷达基准特征的质心cent到相应全站仪基准特征的距离偏差

64、

65、x轴、y轴和z轴的平移量计算公式如下：

66、

67、

68、

69、(f)根据各轴平移量rx，ry，rz来平移更新k个激光雷达基准特征的质心ct，然后将更新后的质心ct继续代入相应的全站仪基准特征的面拟合方程，求出第k个激光雷达基准特征的质心ct到相应全站仪基准特征的距离偏差

70、(g)若则平移量rx，ry，rz为激光雷达点云的当前最优平移量，并进入步骤(h)，否则，继续求出各轴的平移量为

71、

72、

73、

74、然后跳转到第六步，直至找出激光雷达点云的最优全局平移量rx，ry，rz；

75、(h)依据rx，ry，rz来平移激光雷达数据，然后更新激光雷达点云数据；

76、(i)每次取最后若干次迭代的α*，β*，γ*结果进行分析，若满足基准特征条件，便对基准特征进行收缩，否则迭代直至收敛或达到最大迭代次数后退出迭代。

77、本发明的有益技术效果至少在于以下几点：

78、本发明提出的向量匹配算法能取得较完美的匹配结果，向量匹配算法具备更高的匹配成功率，向量匹配算法更符合施工标准的要求，向量匹配算法则克服了传统算法的不足，在面对复杂、未知环境具备更好的匹配效果。