一种基于点集权重和二次匹配的激光雷达室内定位方法

1.本公开实施例涉及激光雷达定位领域，尤其涉及一种基于点集权重和二次匹配的激光雷达室内定位方法。


背景技术：

2.随着机器人技术的飞速发展，越来越多的科技产品如无人车、无人机等开始走进人们的生活，而其中实现各类移动机器人智能化的一项重要技术就是定位。在实际应用中，移动机器人在面对复杂场景，如光照变换、动态障碍物多，多传感器融合误差大时，容易导致跟踪失败，进而影响移动机器人的定位。目前，激光雷达室内定位技术达不到4mm以下的定位精度，使得室内的装修作业无法依赖移动机器人，例如墙面打磨等作业的移动设备定位技术无法将垂直精度限制4mm以下以及水平精度限制4mm以下，所以这类高精度作业完全依赖人工处理。
3.现有的激光雷达定位技术大多结合相机、毫米波雷达、gps定位系统、蓝牙、地磁、超声波雷达、uwb模块以及惯性导航系统等车载辅助设备来共同定位，但不同设备之间的数据信息不一致，处理数据需要大量时间，且不同车载辅助设备存在各自的局部坐标，移动设备的自身坐标，基于环境的全局坐标之间的转换，这必定会加大定位误差，也就无法把定位误差控制5mm以内。所以，在结合不同车载辅助设备进行时会存在算法复杂、效率低下和定位误差较大等问题。


技术实现要素：

4.本发明的目的在于一种基于点集权重和二次匹配的激光雷达室内定位方法，采用向量结合点集权重补偿角度误差，测量偏移角得到纠正，将度数精确到0.3度以下。
5.为了达到上述目的，在本发明提供了一种基于点集权重和二次匹配的激光雷达室内定位方法，所述方法包括：
6.s101、获取静态数据；
7.s201、对全站仪采集的原始数据进行下采样，用于提高后续的处理效率，通过确定参数后，再对全站仪采集的数据进行复原；
8.s301、对经过下采样的数据进行特征分割，剔除不必要点云特征，保留需要使用的点云特征；
9.s401、找到最佳的摆正角度并做摆正，为后续的墙面提取提供基准；
10.s501、选取k个基准点云面，并将各个基准点云面提取出来；
11.s601、激光雷达扫描墙面，近似获取对基准面的测量结果；
12.s701、依据姿态修正计算激光雷达当前坐标。
13.进一步地，所述s101为读取全站仪的数据并可视化数据，将全站仪扫描得到的形貌显示出来。
14.进一步地，所述原始数据为全站仪扫描得到的形貌。
15.进一步地，所述不必要点云特征包括天花板，地面，窗台等不需要用来与激光雷达扫描对比的点云特征。
16.进一步地，所述s401的具体方法为：
17.分别沿着x，y，z轴进行投影，获得投影后的轮廓线，生成网格矩阵来统计投影区域的点数；
18.经过对投影区域的点数进行统计并展示，将投影区域通过顺时针转动一定角度；
19.通过判断第几次旋转时，被投影行的点数最多，相邻行点数最少，得出的转动角度即为最佳的摆正角度。
20.进一步地，所述选取k个基准点云面的标准为：
21.墙面面积大于占比阈值，且墙面的平整度高；
22.尽可能分布在不同墙面上，保障始终有3个或3个以上基准面位于激光雷达可视范围之内。
23.进一步地，对所述全站仪的基准点云面分别进行拟合，并在拟合后求出基准点云面的法向量。
24.进一步地，所述拟合的方法为采用奇异值分解最小二乘算法。
25.进一步地，所述s601的具体步骤为：
26.s6011、激光雷达扫描房间获取点云，并根据全站仪的基准点云面{p1，p2，
…
，pk}的位置，从获取的点云中依据匹配算法或者惯性导航数据初步的预估姿态，再筛选出相对应位置的k个激光雷达测量获取的基准面{p
′1，p
′2，
…
，p
′k}，再通过点云匹配方法，动态获取激光雷达的当前倾角姿态；
27.s6012、抽取激光雷达点云中的第i个位移基准面p
′i，且所抽取的激光雷达基准点云面p
′i的位置要与全站仪扫描的第i个基准点云面pi的位置相对应，且第i个激光雷达基准点云面p
′i的区域大小不能超过全站仪的第i个基准点云面pi的区域大小；
28.s6013、计算出k个激光雷达的基准面{p
′1，p
′2，
…
，p
′k}的有效扫描面积为{s1，s2，
…
sk}，提取其中有效面积大于预设阈值s的k个基准面的有效扫描面积并组成新的有效扫描面积集合{s1，s2，
…
sk}，或者筛选出有效面积最大且不为0的前k个基准面的有效扫描面积并组成新的a有效扫描面积集合{s1，s2，
…
sk}，则新构成的激光雷达的基准面集合为{p
″1，p"2，
…
，p"k}，
29.第i个基准点云面的权重系数为wi，第i个激光雷达基准点云面的扫描面积为si，则第i个基准面的权重系数为，
[0030][0031]
其中k为有效集合的数量，s
t
表示第t个激光雷达基准点云面的扫描面积的面积，si表示第i个激光雷达基准点云面的扫描面积的面积，μ
t
为第t个激光雷达基准点云面的调节阈值；
[0032]
则点集权重系数w
′i[0033]w′i＝k*wi；
[0034]
s6013、计算出k个激光雷达的基准面{p
″1，p"2，
…
，p"k}的质心{c1，c2，
…ck
}，并依
据质心将各激光雷达的基准点云面集合平移，使新基准面集合{p
″′1，p
″′2，
…
，p
″′k}的质心位于全局坐标原点，获得各个基准面的点云集合p
″′i＝{l
″′
i1，
l
″′
i2，
…
l
″′
it
}，1≤i≤k；
[0035]
s6014、调用全站仪获取的对应基准面法向量计算旋转至单位向量的倾角αf与βf；
[0036]
s6015、调整各激光雷达扫描的基准点云面集合p
″″i＝{l
″″
i1
，l
″″
i2
，
…
l
″″
it
}的z坐标数据其中1≤i≤k，
[0037][0038]
其中，表示更新后的第i个点的z坐标数值，zj表示第i个点的z坐标数值，将更新z坐标后的所有基准面点集p
″″i＝{l
″″
i1
，l
″″
i2，
…
l
″″
it
}，1≤i≤k，合并获得参考点云面pa，并通过平面拟合获得法向量
[0039]
s6016、非基准点云面的激光雷达测量三维角度偏差为
[0040][0041]
其中，δαd为非基准点云面法向量与x轴的角度偏差，δβd为非基准点云面法向量与y轴的角度偏差，δγd为非基准点云面法向量与z轴的角度偏差，表示全局坐标中x轴正方向的单位向量，表示全局坐标中y轴正方向的单位向量，表示全局坐标中z轴正方向的单位向量，α
δ
，β
δ
，γ
δ
分别为绕x，y，z轴的角度影响系数；
[0042]
则三维偏差为
[0043][0044][0045][0046][0047]
其中，表示非基准点云面法向量与x轴的三维矢量偏差，表示非基准点云面法向量与y轴的三维矢量偏差，表示非基准点云面法向量与z轴的三维矢量偏差；
[0048]
s6017、判断的参数是否大于阈值e，若的参数大于阈值e则将{p
″″1，p
″
″2，
…
，p
″″k}带入更新步骤s6015；
[0049]
若的参数满足要求，则进入步骤s6018；
[0050]
s6018、确定每个轴上的旋转偏差角度为并对激光雷达测量误差进行补偿。
[0051]
进一步地，所述s701的具体步骤为：
[0052]
通过(δαd，δβd，δγd)对激光雷达扫描的基准点云面进行修正，获取姿态修正后的激光雷达的基准面集合{p
″″′1，p
″″′2，
…
，p
″″′k}，并算出修正后的基准面点云{p
″″′1，p
″″′2，
…
，p
″″′k}的质心{c
′1，c
′2，
…c′k}，进而计算各质心到对应的全站仪基准点云面的距离偏差，
[0053]
将第i个激光雷达的基准面p
″″′i的质心c
′i(x
′i，y
′i，z
′i)，1≤i≤k，带入到对应的全站仪基准点云面pi的平面公式，
[0054][0055]
其中，ai是第i个全站仪基准面的平面公式中的第1个参数，bi是第i个全站仪基准面的平面公式中的第2个参数，ci是第i个全站仪基准面的平面公式中的第3个参数，di是第i个全站仪基准面的平面公式第4个参数，
[0056]
求出各个距离偏差δdi[0057][0058]
则三维距离偏差为
[0059][0060]
其中，δx为激光雷达在x轴方向上的位置偏差，δy为激光雷达在y轴方向上的位置偏差，δz为激光雷达在z轴方向上的位置偏差。t的取值范围为1到k，为第t个全站仪基准面的平面公式中的第1个参数、第t个雷达基准面的质心到对应的全站仪基准面的距离偏差δd
t
与第t个激光雷达基准点云面的面积权重系数w
t
相乘之后的累加和；
[0061]
为第t个全站仪基准面的平面公式中的第2个参数、第t个雷达基准面的质心到对应的全站仪基准面的距离偏差δd
t
与第t个激光雷达基准点云面的面积权重系数w
t
相乘之后的累加和；为第t个全站仪基准面的平面公式中的第3个参数、第t个雷达基准面的质心到对应的全站仪基准面的距离偏差δd
t
与第t个激光雷达基准点云面的面积权重系数w
t
相乘之后的累加和，λ
x
、λy、λz分别为x、y、z轴方向上的调整因子；
[0062]
将δx、δy、δz用于补偿激光雷达的坐标，得到总偏差量δx，δy，δz后，将静态估计的位置通过平移补偿(δx，δy，δz)的方法算出激光雷达实际扫描点坐标j(xj，yj，zj)。
[0063]
本发明的有益技术效果至少在于以下几点：
[0064]
(1)本发明的一种基于点集权重和二次匹配的激光雷达室内定位方法，先采用静态粗匹配，然后利用向量权重分析法进行角度位姿调整，再采用动态精匹配的定位方法，实现快速，精确定位，使用静态补偿结合动态补偿方式，使激光雷达的姿态精度更高，也使得墙面垂直度和平整度的测量精度达到
±
0.3度以下；
[0065]
(2)本发明采用向量结合权重补偿角度误差，测量偏移角得到纠正，将度数精确到
±
0.3度以下，将定位误差限制在
±
4mm以内，本发明不需要配合数据复杂的车载设备；
[0066]
(3)本发明对比现有技术不复杂，效率高，精度高，利用静态全站仪数据与动态激光雷达数据的复。
附图说明
[0067]
利用附图对本发明作进一步说明，但附图中的实施例不构成对本发明的任何限制，对于本领域的普通技术人员，在不付出创造性劳动的前提下，还可以根据以下附图获得其它的附图。
[0068]
图1为本发明一种基于点集权重和二次匹配的激光雷达室内定位方法流程图。
[0069]
图2为本发明全站仪原始点云示意图。
[0070]
图3为本发明全站仪数据下采样点云图。
[0071]
图4为本发明特征提取点云图。
[0072]
图5为本发明点云沿z轴投影图。
[0073]
图6为本发明点云角度纠正投影图。
[0074]
图7为发明点云投影点数统计图。
[0075]
图8为本发明特征点云面提取示意图。
[0076]
图9为本发明特征点云面拟合示意图。
[0077]
图10为ndt匹配算法以及pca匹配算法的匹配效果示意图。
[0078]
图11为全站仪拟合面合并示意图。
[0079]
图12为激光雷达扫描面合并示意图。
[0080]
图13为待匹配的点云图。
[0081]
图14为配准后的点云图。
具体实施方式
[0082]
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
[0083]
如图1所示，本发明提供了一种基于点集权重和二次匹配的激光雷达室内定位方法。
[0084]
具体实施方式：一种基于点集权重和二次匹配的激光雷达室内定位方法，方法包
括：
[0085]
s101、获取静态数据：先是读取全站仪的数据并可视化，将全站仪扫描得到的形貌显示出来，如图2所示。
[0086]
s201、对全站仪采集的原始数据进行下采样，用于提高后续的处理效率，通过确定参数后，再对全站仪采集的数据进行复原，如图3所示；
[0087]
s301、对经过下采样的数据进行特征分割，剔除不必要点云特征，保留需要使用的点云特征，如图4所示；
[0088]
s401、找到最佳的摆正角度并做摆正，为后续的墙面提取提供基准，如图5和图6所示，图6是经过自动选择最佳角度的点云投影图；
[0089]
角度摆正方法，分别沿着x，y，z轴进行投影，获得投影后的轮廓线，在投影的基础上，生成网格矩阵来统计投影区域的点数。如图5所示，我们需要将图顺时针进行调整，使投影摆正。所以，我们在投影的基础上，生成网格矩阵用来统计每个区域的投影点数。沿着z轴方向进行投影，每个矩阵元素的长宽根据实际房子的墙面厚度确定，确保每个矩阵元素代表一个长宽的尺寸与墙面厚度相同的点云投影数量。经过对投影点数进行统计并展示，如图6所示，由于大部分点集中在31行，我们通过顺时针转动一定角度，使更多的点进入31行，更少的点进入30行和29行。经过阶梯角度的旋转，判断第几次旋转时，被投影行(31行)的点数最多，相邻行(29行和30行)点数最少，得出的转动角度即为最佳的摆正角度。
[0090]
先是将1
°
作为旋转幅度，在[0
°
，10
°
]的区间内旋转10次，判断在第几次旋转时，被投影网格(投影轮廓线所在的网格)的点数最多，相邻网格(与投影轮廓线所在的网格相邻的网格)点数最少，确定旋转a度为最佳摆正度数。
[0091]
然后将0.1
°
作为旋转幅度，在[a.0
°
，a.9
°
]的区间内旋转9次，判断在第几次旋转时，被投影矩阵(投影轮廓线所在的网格)的点数最多，相邻网格(与投影轮廓线所在的网格相邻的网格)点数最少，确定旋转0.b度为最佳摆正度数。
[0092]
接着将0.01
°
作为旋转幅度，在[a.b0
°
，a.b9
°
]的区间内旋转9次，判断在第几次旋转时，被投影矩阵(投影轮廓线所在的网格)的点数最多，相邻网格(与投影轮廓线所在的网格相邻的网格)点数最少，确定旋转0.0c度为最佳摆正度数。
[0093]
再将0.001
°
作为旋转幅度，在[a.bc0
°
，a.bc9
°
]的区间内旋转9次，判断在第几次旋转时，被投影矩阵(投影轮廓线所在的网格)的点数最多，相邻网格(与投影轮廓线所在的网格相邻的网格)点数最少，确定旋转0.00d度为最佳摆正度数。
[0094]
经过多次判断，并确定旋转角度，从图5中房子沿着z轴的投影轮廓分别已经与x轴、以及y轴相平行。
[0095]
同理，通过对x轴，y轴进行摆正，得出x轴的最佳摆正度数和y轴的最佳摆正度数。进而将房间的三维坐标进行摆正，尽量减少定位时的误差。
[0096]
s501、选取k个基准点云面，并将各个基准点云面提取出来，如图6-7所示。选取的标准是：(1)特征点云面在房间的占比较大，且墙面的平整度高。(2)尽可能分布在不同墙面上，保障始终有3个或3个以上基准面位于激光雷达可视范围之内。
[0097]
对k个全站仪的基准点云面{p1，p2，
…
，pk}分别进行拟合，求出k个基准点云面的法向量并在拟合后对各个全站仪的基准点云面{p1，p2，
…
，pk}进行合并，如图11所示。
[0098]
拟合的方法为采用奇异值分解(svd)最小二乘算法，算法的原理是使所有点到该拟合平面的距离的平方和最小，使得出的拟合平面为最佳平面，函数的具体运行流程是计算点云的质心、去质心化、矩阵的奇异值分解、获取最小特征值相对应的特征向量、计算坐标系原点到拟合平面的距离，最后得出拟合平面的方程参数plane。
[0099]
奇异值分解(svd)最小二乘算法的原理为：
[0100]
对于得到的多个点集数据，可以先设的拟合平面方程为：
[0101]
ax+by+cz+d＝0
ꢀꢀ
(1)
[0102]
其约束条件为：
[0103]
a2+b2+c2＝1
ꢀꢀꢀꢀ
(2)
[0104]
可以得到拟合平面的参数a、b、c、d。这时，要使获得的拟合平面是最佳的，就要使得n个邻近点到该拟合平面的距离的平方和最小，即需要满足：
[0105][0106]
在公式3中，其中i是表示点云中第i个点，di是点云数据中的任一点pi(xi，yi，zi)到这个拟合平面的距离di＝|axi+byi+czi+d|。要使e
→
min，可以利用svd矩阵分解求得拟合平面参数a、b、c，最后再利用拟合平面的质心求得d。
[0107]
根据实际墙面情况设置墙点最大距离阈值d
max
，将点云数据中的任一点pi(xi，yi，zi)依次代入公式(1)中并求出点到面距离di。若di大于墙点最大距离阈值d
max
，则该点会被丢弃。待将不满足墙点最大距离阈值d
max
的所有点丢弃后，重新进行面拟合求出更新后的公式(1)，继续判断点云数据是否满足墙点最大距离阈值d
max
，若满足，则更新后的公式(1)为所求拟合平面方程，否则继续进行迭代更新公式(1)直至求出满足墙点最大距离阈值d
max
的拟合平面方程。
[0108]
s601、激光雷达扫描墙面，近似获取对基准面的测量结果；
[0109]
激光雷达扫描房间获取激光雷达的点云集合ji，并根据全站仪的基准点集qi的位置，从获取的点云中依据惯性导航系统的数据初步地预估静态姿态，依据扫描情况筛选出k个墙面作为激光雷达的基准面，并将激光雷达基准面相对应区间内的点集更新为激光雷达的点集j
t
，再通过以下的点云找准方法，动态获取激光雷达的当前倾角姿态。
[0110]
激光雷达基准面筛选条件：
[0111]
根据激光雷达扫描房间获取的点集，同时依据全站仪基准面选取出相对应的墙面，计算对应墙面的有效扫描面积，提取其中有效扫描面积大于预设阈值s的k个墙面作为激光雷达基准面，激光雷达基准面的有效扫描面积为{s1，s2，
…
sk}，或者筛选出有效面积最大且不为0的前k个墙面作为激光雷达基准面，且激光雷达基准面的有效扫描面积集合为{s1，s2，
…
sk}，则这些筛选出来的激光雷达基准面的点集构成更新后的激光雷达点集ji。
[0112]
根据激光雷达扫描房屋获取的点云数据，筛选出组成激光雷达的基准面点集后，对各个基准面内的点集进行自聚类迭代面拟合，待面拟合操作完成后，将各个激光雷达基准点云面进行合并，如图12所示，然后依据各个基准面的边界计算各个基准面的有效扫描面积{s1，s2，
…
sk}。
[0113]
第i个激光雷达基准点云面的扫描面积为si，则第i个激光雷达基准点云面的面积权重系数为wi为，
[0114][0115]
其中k为激光雷达基准面的数量，s
t
表示第t个激光雷达基准点云面的扫描面积的面积，si表示i个激光雷达基准点云面的扫描面积的面积，μ
t
为第t个激光雷达基准点云面的调节阈值。
[0116]
则点集权重系数w
′i[0117]w′i＝k*wi[0118]
基于点集权重的定位方法：
[0119]
步骤1、计算出k个激光雷达的基准面{p
″1，p"2，
…
，p"k}的质心{c1，c2，
…ck
}，并依据质心将各激光雷达的基准点云面集合平移，使新基准面集合{p
″′1，p
″′2，
…
，p
″′k}的质心位于全局坐标原点，获得各个基准面的点云集合p
″′i＝{l
″′
i1
，l
″′
i2，
…
l"
′
it
}(1≤i≤k)。
[0120]
步骤2、依据激光雷达基准面，调用全站仪获取的对应基准面的法向量计算旋转至单位向量的倾角αi与βi。
[0121]
先是计算出第i个全站仪的基准点云面的法向量投影到全局坐标yoz平面的角度αi，则αi为
[0122][0123]
其中为x轴的单位向量(1，0，0)，将向量绕着与的共同垂直轴旋转αi度，从而获得第一次旋转后的对应向量
[0124][0125]
将对应的向量绕x轴旋转β度之后获得第二次旋转后的第f个全站仪基准点云面的向量
[0126]
将各个激光雷达的基准点云面参照对应全站仪点云基准面依次进行旋转角度αi、角度βi后，旋转更新激光雷达基准面点集j
′
t
。
[0127]
步骤3、调整更新激光雷达扫描的基准面点集的z坐标数据，
[0128][0129]
zi为第i个激光雷达基准点云所对应点集中所有点的z坐标数值，w
′i第i个激光雷达基准点云所对应的点集权重系数；
[0130]
将更新z坐标后的所有激光雷达基准面点集ji合并获得非基准面点集pa，并通过将非基准面点集pa进行自聚类迭代拟合从而获得参考点云面的法向量
[0131]
步骤4、非基准点云面的激光雷达测量三维角度偏差为
[0132][0133]
δαd为非基准点云面法向量与x轴的角度偏差，δβd为非基准点云面法向量与y轴的角度偏差，δγd为非基准点云面法向量与z轴的角度偏差，为参考点云面的法向量，表示全局坐标中x轴正方向的单位向量，表示全局坐标中y轴正方向的单位向量，表示全局坐标中z轴正方向的单位向量，α
δ
，β
δ
，γ
δ
分别为绕x，y，z轴的角度影响系数。
[0134]
则三维偏差为
[0135][0136][0137][0138][0139]
其中，表示全局坐标的单位向量，为非基准点云面法向量与x轴的三维矢量偏差，为非基准点云面法向量与y轴的三维矢量偏差，为非基准点云面法向量与z轴的三维矢量偏差。
[0140]
步骤5、若满足任意条件则停止迭代，继续下一步，否则，重新转到步骤3。
[0141]
步骤6、确定每个轴上的旋转偏差角度为并对激光雷达测量误差进行补偿。
[0142]
s701、依据姿态修正计算激光雷达当前坐标。
[0143]
通过δαd，δβd，δγd的对激光雷达基准面点集ji进行修正，获取姿态修正更新后的激光雷达基准面点集ji，并算出修正后的各个激光雷达基准面的质心{c
′1，c
′2，
…c′k}，进而计算各激光雷达基准面的质心到对应的全站仪基准面的距离偏差δdi，
[0144][0145]
其中，ai是第i个全站仪基准面的平面公式中的第1个参数，bi是第i个全站仪基准面的平面公式中的第2个参数，ci是第i个全站仪基准面的平面公式中的第3个参数，di是第i个全站仪基准面的平面公式第4个参数，(x
′i，y
′i，z
′i)为第i个激光雷达基准面的质心位
置。
[0146]
则三维距离偏差为
[0147][0148]
其中，将δx、δy、δz用于补偿激光雷达的坐标，λ
x
、λy、λz分别为x、y、z轴方向上的调整因子。
[0149]
得到总偏差量δx，δy，δz后，将静态估计的位置通过平移补偿(δx，δy，δz)的方法算出激光雷达实际扫描点坐标j(xj，yj，zj)。
[0150]
其中，步骤5中满足以下任一情况即可以作为停止条件：
[0151]
a)迭代次数大于设定阈值n
max
；
[0152]
b)三维偏差的参数小于设定阈值e；
[0153]
c)迭代误差err
t
》err
t+1
；
[0154]
d)角度误差小于设定角度阈值g。
[0155]
s801、点激光测距传感器
[0156]
在激光雷达的左上角，左下角，右上角，右下角固定布置四个点激光测距传感器，且四个点激光测距传感器的朝向是移动设备的正面朝向，且点激光测距传感器随激光雷达一起旋转移动。
[0157]
根据第i个激光雷达基准点云面p
″′i(1≤i≤k)的拟合平面公式中的向量参数a
′i，b
′i来判断面的朝向，筛选出向量参数a
′i的绝对值或者b
′i的绝对值大于向量阈值g(一般情况下，g≥0.9)的激光雷达基准点云面，然后再依据该基准点云面的拟合平面公式中的平移参数di来判断该激光雷达基准点云面在坐标中的位置。
[0158]
通过四个点激光测距传感器的朝向随设备的移动旋转，通过扫描第i个激光基准点云面所对应的墙面，获得4个点激光测距传感器的数据，并判断出设备本体与墙面的平行度。左上角，左下角，右上角，右下角4个点激光测距传感器的数据分别为d
1i
，d
2i
，d
3i
，d
4i
，d
1i
，d
2i
，d
3i
，d
4i
之间的中心值为d
′i。
[0159]d′i＝(d
i1
+d
i2
+d
i3
+d
i4
)/4
[0160]
若d
1i
，d
2i
，d
3i
，d
4i
两两之间数值差小于距离阈值df，且t-1时刻的中心值d
′i≤t时刻的中心值d
′i≤t+1时刻的中心值d
′i，其中t为每个扫描间隔的时间，若不满足要求，则退出。若满足，则中心值d
′i用来进一步修正激光雷达的位姿。
[0161]
判断是否激光雷达基准点云面p
″′i(1≤i≤k)的拟合平面公式中的向量参数a
′i的绝对值≥向量阈值g，且x
j-di≥0，若是，则在求出中心值d
′i后，仅与激光雷达的x轴数据进行对比
[0162][0163]nx
为符合拟合平面公式中的向量参数a
′i的绝对值≥向量阈值g，且x
j-di≥0的激光雷达基准点云面的总面数。
[0164]
若不是，则继续判断判断是否激光雷达基准点云面p
″′i(1≤i≤k)的拟合平面公式中的向量参数a
′i的绝对值≥向量阈值g，且x
j-di≤0，若是，则在求出中心值d
′i后，仅与激光雷达的x轴数据进行对比
[0165][0166]n′
x
为符合拟合平面公式中的向量参数a
′i的绝对值≥向量阈值g，且x
j-di≤0的激光雷达基准点云面的总面数。
[0167]
判断是否激光雷达基准点云面p
″′i(1≤i≤k)的拟合平面公式中的向量参数b
′i的绝对值≥向量阈值g，且y
j-di≥0，若是，则在求出中心值d
′i后，仅与激光雷达的y轴数据进行对比。
[0168][0169]
其中，di为第i个激光雷达基准点云面p
″′i(1≤i≤k)的拟合平面公式中的平移参数，ny为符合拟合平面公式中的向量参数b
′i的绝对值≥向量阈值g，且y
j-di≥0的激光雷达基准点云面的总面数。
[0170]
若不是，则继续判断判断是否激光雷达基准点云面p
″′i(1≤i≤k)的拟合平面公式中的向量参数b
′i的绝对值≥向量阈值g，且y
j-di≤0，若是，则在求出中心值d
′i后，仅与激光雷达的y轴数据进行对比
[0171][0172]n′y为符合拟合平面公式中的向量参数b
′i的绝对值≥向量阈值g，且y
j-di≤0的激光雷达基准点云面的总面数。
[0173]
从而求出激光雷达的当前坐标j(x
′j，y
′j，zj)。
[0174]
利用j(x
′j，y
′j，zj)与(xj，yj，zj)之间的差值再次对激光雷达点云做补偿，从而获得最终匹配点云，如图14所示。
[0175]
综上所述，本专利提出了一种基于点集权重和二次匹配的激光雷达室内定位方法，通过自聚类迭代拟合点云面原理和基于点集权重的定位方法对室内定位的定位误差限制在
±
4mm以内，而且不需要配合数据复杂的车载设备。静态补偿结合动态补偿方式，使激光雷达的姿态精度更高，也使得墙面垂直度和平整度的测量精度达到
±
0.3度以下。采用向量结合权重补偿角度误差，测量偏移角得到纠正，将度数精确到
±
0.3度以下；先采用静态
粗匹配，然后利用向量权重分析法进行角度位姿调整，再采用动态精匹配的定位方法，实现快速，精确定位。本方法的算法不复杂，效率高，精度高；利用静态全站仪数据与动态激光雷达数据的复用。
[0176]
尽管已经示出和描述了本发明的实施例，本领域技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变形，本发明的范围由权利要求及其等同物限定。

技术特征：
1.一种基于点集权重和二次匹配的激光雷达室内定位方法，其特征在于，所述方法包括：s101、获取静态数据；s201、对全站仪采集的原始数据进行下采样，用于提高后续的处理效率，通过确定参数后，再对全站仪采集的数据进行复原；s301、对经过下采样的数据进行特征分割，剔除不必要点云特征，保留需要使用的点云特征；s401、找到最佳的摆正角度并做摆正，为后续的墙面提取提供基准；s501、选取k个基准点云面，并将各个基准点云面提取出来；s601、激光雷达扫描墙面，近似获取对基准面的测量结果；s701、依据姿态修正计算激光雷达当前坐标。2.根据权利要求1所述的一种基于点集权重和二次匹配的激光雷达室内定位方法，其特征在于，所述s101为读取全站仪的数据并可视化数据，将全站仪扫描得到的形貌显示出来。3.根据权利要求1所述的一种基于点集权重和二次匹配的激光雷达室内定位方法，其特征在于，所述原始数据为全站仪扫描得到的形貌。4.根据权利要求1所述的一种基于点集权重和二次匹配的激光雷达室内定位方法，其特征在于，所述不必要点云特征包括天花板，地面和窗台不需要用来与激光雷达扫描对比的点云特征。5.根据权利要求1所述的一种基于点集权重和二次匹配的激光雷达室内定位方法，其特征在于，所述s401的具体方法为：分别沿着x，y，z轴进行投影，获得投影后的轮廓线，生成网格矩阵来统计投影区域的点数；经过对投影区域的点数进行统计并展示，将投影区域通过顺时针转动一定角度；通过判断第几次旋转时，被投影行的点数最多，相邻行点数最少，得出的转动角度即为最佳的摆正角度。6.根据权利要求1所述的一种基于点集权重和二次匹配的激光雷达室内定位方法，其特征在于，所述选取k个基准点云面的标准为：墙面面积大于占比阈值，且墙面的平整度高；分布在不同墙面上，保障始终有3个或3个以上基准面位于激光雷达可视范围之内。7.根据权利要求3所述的一种基于点集权重和二次匹配的激光雷达室内定位方法，其特征在于，对所述全站仪的基准点云面分别进行拟合，并在拟合后求出基准点云面的法向量。8.根据权利要求7所述的一种基于点集权重和二次匹配的激光雷达室内定位方法，其特征在于，所述拟合的方法为采用奇异值分解最小二乘算法。9.根据权利要求1所述的一种基于点集权重和二次匹配的激光雷达室内定位方法，其特征在于，所述s601的具体步骤为：s6011、激光雷达扫描房间获取点云，并根据全站仪的基准点云面{p1,p2,
…
,p
k
}的位置，从获取的点云中依据匹配算法或者惯性导航数据初步的预估姿态，再筛选出相对应位
置的k个激光雷达测量获取的基准面{p'1,p'2,
…
,p'
k
}，再通过点云匹配方法，动态获取激光雷达的当前倾角姿态；s6012、抽取激光雷达点云中的第i个位移基准面p'
i
，且所抽取的激光雷达基准点云面p'
i
的位置要与全站仪扫描的第i个基准点云面p
i
的位置相对应，且第i个激光雷达基准点云面p'
i
的区域大小不能超过全站仪的第i个基准点云面p
i
的区域大小；s6013、计算出k个激光雷达的基准面{p'1,p'2,
…
,p'
k
}的有效扫描面积为{s1,s2,
…
s
k
}，提取其中有效面积大于预设阈值s的k个基准面的有效扫描面积并组成新的有效扫描面积集合{s1,s2,
…
s
k
}，或者筛选出有效面积最大且不为0的前k个基准面的有效扫描面积并组成新的a有效扫描面积集合{s1,s2,
…
s
k
}，则新构成的激光雷达的基准面集合为{p”1
,p”2
,
…
,p”k
}，第i个基准点云面的权重系数为w
i
，第i个激光雷达基准点云面的扫描面积为s
i
，则第i个基准面的权重系数为，其中k为有效集合的数量，s
t
表示第t个激光雷达基准点云面的扫描面积的面积，s
i
表示第i个激光雷达基准点云面的扫描面积的面积,μ
t
为第t个激光雷达基准点云面的调节阈值；则点集权重系数w
′
i
wi
i
＝k*w
i
；s6013、计算出k个激光雷达的基准面{p”1
,p”2
,
…
,p”k
}的质心{c1,c2,
…
c
k
}，并依据质心将各激光雷达的基准点云面集合平移，使新基准面集合{p”'1,p”'2,
…
,p”'
k
}的质心位于全局坐标原点，获得各个基准面的点云集合p”'
i
＝{l”'
i1
,l”'
i2
,
…
l”'
it
}，1≤i≤k；s6014、调用全站仪获取的对应基准面法向量计算旋转至单位向量的倾角α
f
与β
f
；s6015、调整各激光雷达扫描的基准点云面集合p
””
i
＝{l
””
i1
,l
””
i2
,
…
l
””
it
}的z坐标数据其中1≤i≤k，其中，表示更新后的第i个点的z坐标数值，z
i
表示第i个点的z坐标数值，将更新z坐标后的所有基准面点集p
””
i
＝{l
””
i1
,l
””
i2
,
…
l
””
it
}，1≤i≤k，合并获得参考点云面p
a
，并通过平面拟合获得法向量s6016、非基准点云面的激光雷达测量三维角度偏差为
其中，δα
d
为非基准点云面法向量与x轴的角度偏差，δβ
d
为非基准点云面法向量与y轴的角度偏差，δγ
d
为非基准点云面法向量与z轴的角度偏差，表示全局坐标中x轴正方向的单位向量，表示全局坐标中y轴正方向的单位向量，表示全局坐标中z轴正方向的单位向量,α
δ
,β
δ
,γ
δ
分别为绕x，y，z轴的角度影响系数；则三维偏差为为为为其中，表示非基准点云面法向量与x轴的三维矢量偏差，表示非基准点云面法向量与y轴的三维矢量偏差，表示非基准点云面法向量与z轴的三维矢量偏差；s6017、判断的参数是否大于阈值e，若的参数大于阈值e则将{p
””1,p
””2,
…
,p
””
k
}带入更新步骤s6015；若的参数满足要求，则进入步骤s6018；s6018、确定每个轴上的旋转偏差角度为并对激光雷达测量误差进行补偿。10.根据权利要求9所述的一种基于点集权重和二次匹配的激光雷达室内定位方法，其特征在于，所述s701的具体步骤为：通过(δα
d
,δβ
d
,δγ
d
)对激光雷达扫描的基准点云面进行修正，获取姿态修正后的激光雷达的基准面集合{p
””
'1,p
””
'2,
…
,p
””
'
k
},并算出修正后的基准面点云{p
””
'1,p
””
'2,
…
,p
””
'
k
}的质心{c'1,c'2,
…
c'
k
}，进而计算各质心到对应的全站仪基准点云面的距离偏差，将第i个激光雷达的基准面p
””
'
i
的质心c'
i
(x'
i
,y'
i
,z'
i
)，1≤i≤k，带入到对应的全站仪基准点云面p
i
的平面公式，其中，a
i
是第i个全站仪基准面的平面公式中的第1个参数，b
i
是第i个全站仪基准面的平面公式中的第2个参数，c
i
是第i个全站仪基准面的平面公式中的第3个参数，d
i
是第i个全站仪基准面的平面公式第4个参数，求出各个距离偏差δd
i
则三维距离偏差为其中，δx为激光雷达在x轴方向上的位置偏差，δy为激光雷达在y轴方向上的位置偏差，δz为激光雷达在z轴方向上的位置偏差，t的取值范围为1到k，为第t个全站仪基准面的平面公式中的第1个参数、第t个雷达基准面的质心到对应的全站仪基准面的距离偏差δd
t
与第t个激光雷达基准点云面的面积权重系数w
t
相乘之后的累加和；为第t个全站仪基准面的平面公式中的第2个参数、第t个雷达基准面的质心到对应的全站仪基准面的距离偏差δd
t
与第t个激光雷达基准点云面的面积权重系数w
t
相乘之后的累加和；为第t个全站仪基准面的平面公式中的第3个参数、第t个雷达基准面的质心到对应的全站仪基准面的距离偏差δd
t
与第t个激光雷达基准点云面的面积权重系数w
t
相乘之后的累加和，λ
x
、λ
y
、λ
z
分别为x、y、z轴方向上的调整因子；将δx、δy、δz用于补偿激光雷达的坐标，得到总偏差量δx,δy,δz后，将静态估计的位置通过平移补偿(δx,δy,δz)的方法算出激光雷达实际扫描点坐标j(x
j
,y
j
,z
j
)。

技术总结
本发明提出了一种基于点集权重和二次匹配的激光雷达室内定位方法，属于激光雷达定位领域，方法包括：获取静态数据；对全站仪采集的原始数据进行下采样，用于提高后续的处理效率，通过确定参数后，再对全站仪采集的数据进行复原；对经过下采样的数据进行特征分割，剔除不必要点云特征，保留需要使用的点云特征；找到最佳的摆正角度并做摆正，为后续的墙面提取提供基准；选取k个基准点云面，并将各个基准点云面提取出来；激光雷达扫描墙面，近似获取对基准面的测量结果；依据姿态修正计算激光雷达当前坐标。本方法的算法不复杂，效率高，精度高，将定位误差限制在


技术研发人员：张春良 翁润庭 岳夏 李子涵 朱厚耀 郑仲之 王亚东 黄灿荣 钟国昌 陈洋洲
受保护的技术使用者：广州大学
技术研发日：2023.05.05
技术公布日：2023/9/23