一种基于激光雷达的室内相对定位方法与流程

本发明属于室内相对定位的方法，涉及一种基于激光雷达的室内相对定位方法，涉及用多线激光雷达实现无人机室内相对定位的方法。

背景技术：

近年来，无人机技术发展迅速，应用领域越来越广，飞行空间也逐渐扩展到室内，应用于室内拍摄、环境建模、室内侦察等领域。而无人机的室内相对定位是无人机技术的非常重要的环节。室内相对定位，即是在室内环境下，获得无人机相对于室内环境的位置信息。

虽然基于gps定位的导航系统广泛应用于无人机的定位，但是在室内环境下，gps信号易被干扰和屏蔽而无法使用。因此如何实现无人机室内相对定位，是一项具有挑战性的问题。目前，主要采用声波信号、电磁信号、视觉信息以及惯性测量单元(imu)测量等多种类型传感器测量来实现无人机室内相对定位，其中，基于声波信号主要通过声呐传感器，通过超声波测量与目标之间的距离信息。基于电磁信号的室内相对定位需要采集足够密度的详细信号的磁场数据并上传到服务器，通过磁场信号的对比来获得位置信息。声波信号和电磁信号均存在易被干扰、精度低的问题。基于视觉信息的室内相对定位主要通过相机采集视觉信号，通过图像匹配、特征提取等方式实现室内定位，其主要缺点是图像信息数据量很大，且无法在昏暗条件下使用。而基于惯性测量单元的定位主要通过数字平台对陀螺仪和加速度计的数据进行解算，从而获得位置信息，它最大的问题是存在误差的积累，无法单独使用。文献《somania,chungs,celikk.mono-visioncornerslamforindoornavigation[j].ieeeinternationalconferenceonelectro/inform...,2008:343-348.》中，美国爱荷华州立大学的celik等人使用单目相机作为主要传感器，提出一种mvcslam算法，通过跟踪地面上的角点特征信息，实现相对定位。但是该方法适应性差，要求室内有明显的角点，难以应对复杂环境，如当角点被家具或人的走动所遮挡时，该方法则显得无能为力。

激光雷达是一种通过发射激光束，接收反射信号，来感知周围环境的传感器，它能获取激光扫描点的三维坐标和反射强度信息，主要应用在无人驾驶领域。随着无人驾驶的发展，激光雷达发展迅速，它精度高、抗干扰、体积小，相比相机的数据量小，且能在昏暗条件下使用。这些优点使激光雷达更加适合无人机的室内相对定位。

技术实现要素：

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于激光雷达的室内相对定位方法，克服现有室内相对定位方法的不足。本发明应用到了随机采样一致性算法，基于随机采样一致性算法提取出室内环境的墙面边界，并得到墙面边界模型的平面方程，从而进一步得出激光雷达相对室内环境的位置信息。

技术方案

一种基于激光雷达的室内相对定位方法，其特征在于步骤如下：

步骤1：在方形室内环境下，记相邻两面墙分别为a、b，将激光雷达随机放置于室内某点，通电后打开激光雷达，采集得到多帧包含三维坐标信息和反射回波强度的激光雷达点云数据；

步骤2：基于随机采样一致性算法，对每一帧激光雷达点云数据进行第一次拟合处理，得到每帧拟合墙面a的平面方程，进而得到激光雷达到墙面a的距离；处理过程为：

1、从激光雷达点云数据中统计反射回波强度值出现的频率，选择频率最高的j组强度值，记为i1,i2,i3,...,ij，对这些回波强度值进行直通滤波，保留与这些回波强度值对应的点云数据，作为采样的总样本集u1；

2、从第一步中获得的样本中随机选择3个点云数据作为子集，根据选择的子集数据，在平面方程一次项系数满足归一化条件的情况下，利用待定系数法得到这三个点所在的平面方程a1x+b1y+c1z+d1＝0；

3、设置误差容限ε1，定义一致集ω1＝φ为空集，依次求取所有样本点到平面a1x+b1y+c1z+d1＝0的距离，记样本点ai(xi,yi,zi)到平面a1x+b1y+c1z+d1＝0的距离值为li＝|a1xi+b1yi+c1zi+d1|，如果距离li满足条件：li＜ε1，则将样本点ai标记为局内点，并将其归入一致集ω1；一致集ω1的元素个数记做k，一致集ω的模型参数即为a1,b1,c1,d1；

4、重新随机选择3个点云数据作为新的子集，同样在满足归一化条件的情况下，用待定系数法求取出新的子集的平面方程a1'x+b1'y+c1'z+d1'＝0，根据选择的新子集平面方程a1'x+b1'y+c1'z+d1'＝0，在同样误差容限ε1的限定下，再次依次求取所有样本点到平面a1'x+b1'y+c1'z+d1'＝0的距离li'，取出满足条件li'＜ε1的点，并将这些点归入新的一致集ω1'，该一致集的元素个数记做k'，判断k'是否大于k，如果是，则将平面方程的系数a1,b1,c1,d1更新为a1',b1',c1',d1'，同时将一致集ω1更新为ω1'，一致集大小k更新为k'；如果不是，则平面方程系数a1,b1,c1,d1、一致集ω1和一致集的大小k均保持不变；

5、设置迭代次数上限n1，重复第4步，随机采样n1次，输出n1次采样后的一致集ω1，以及该一致集的模型参数a1,b1,c1,d1，则激光雷达距离墙面a的距离即为|d1|；

步骤3：从总样本集u1中的数据点中，剔除掉步骤2输出的一致集ω1中的数据点，得到的数据点作为新的样本集，记做u2，对样本集u2重复步骤2的随机采样一致性算法，设置新的误差容限ε2和迭代次数上限n2，输出此时的一致集ω2，以及该一致集的模型参数a2,b2,c2,d2，得到激光雷达距离墙面b的距离为|d2|；

步骤4：根据步骤2和步骤3得到的结果，以相邻墙面法向量方向建立坐标系，使得室内环境保持在坐标系第一象限；得到每一帧激光雷达点云数据处理得到的二维位置坐标信息(xj,yj)，其中xj表示第j帧点云数据输出的距离|d1|，yj表示第j帧点云数据输出的距离|d2|。

有益效果

本发明提出的一种基于激光雷达的室内相对定位方法，基于随机采样一致性算法，对激光雷达点云数据进行处理，从而得到墙面a的平面方程以及到墙面a的距离|d1|，并输出墙面a所包含数据点组成的一致集ω；剔除步骤2输出的一致集ω中的元素组成新样本，再次基于随机采样一致性算法，对新样本点云数据进行处理，从而得到相邻墙面b的平面方程以及到墙面b的距离|d2|；针对步骤2和步骤3得到两个距离，以相邻墙面的法向方向为坐标轴，建立适当坐标系，使得室内环境在第一象限内，则可以表示激光雷达的位置坐标信息。

通过通过地面静态实验验证，可以得出本发明的效果如下：

1、对于一般的室内环境，该方法能够实现对无人机的室内相对定位，且精度很高，能达到2cm(1σ)；

2、该方法对遮挡不敏感，即便存在大书柜等约占墙面面积50％的遮挡，只要存在一片平整的墙面，合理选择阈值，即可将墙面拟合出来，从而获得无人机位置信息；

3、该方法不要求室内环境静止，如果室内存在随机运动的人或物，该方法依然能提供高精度的定位。

由于随机采样一致性算法在采样时的随机性，可能存在丢帧现象，即存在某帧点云数据无法完成拟合，但由于激光雷达采样频率高，该局限性对定位结果影响较小；

根据图可以得出，位置的纵坐标在1cm范围内变化，横坐标在3cm范围变化，因此该方法对无人机进行室内定位能够获得非常好的精度。

附图说明

图1是随机采样一致性算法与最小二乘法对比图；

图2是随机采样一致性算法流程图(以平面模型为例)

图3是基于随机采样一致性算法的室内相对定位方法流程图

图4是室内实验场景示意图

图5是墙面a拟合结果示意图

图6是墙面b拟合结果示意图

图7是在室内遮挡物随机运动情况下多帧数据采集示意图

图8是每帧数据的定位一维坐标

图9是一维定位误差的频率分布直方图

图10是二维定位坐标散点图

图11是二维定位坐标频率分布直方图

图12是定位误差的频率分布直方图

图13是室内相对定位结果图

图14是室内相对定位结果的频率分布直方图

具体实施方式

现结合实施例、附图对本发明作进一步描述：

本发明的技术方案主要包括下述步骤：随机采样一致性算法研究、基于随机采样一致性算法完成室内相对定位。首先，对经典随机采样一致性算法进行简要介绍。

随机采样一致性算法是一种模型参数估计方法，最早由fischler和bolles于1981年提出，它是一种重采样技术，通过估从随机采样得到的样本点中解算模型参数，得到模型，再次计算符合解算模型的样本点数量，如果数量达到阈值，则计算结束。

随机采样一致性算法的基本假设是：

1、数据由“局内点”和“局外点”组成；

2、局内点的数据分布可以用一些模型参数来解释；

3、除局内点外，其他数据均视为局外点；

4、给定一组(通常很小的)局内点，存在一个可以估计模型参数的过程，而该估计模型能够解释或者适用于局内点。

为便于理解，这里将随机采样一致性算法与最小二乘法做对比，两者均是拟合模型的方法，但是最小二乘法将所有的数据点都考虑进去，构建指标(通常为距离值的平方和)，当指标达到最值时的模型参数即为拟合结果。随机采样一致性算法则并为考虑所有点，而是将数据点分为局内点和局外点，只对局内点进行拟合处理，它首先通过对数据随机采样，计算模型参数，然后验证在一定误差范围内符合此模型参数的数据点(即局内点)个数。如果局内点个数超过阈值，则将此模型参数作为拟合结果输出，否则重新采样，直到局内点数据量超过阈值，或者是采样次数超过设定上限为止。

两者的区别如图1，图1中，样本点为所有数据点，黑线为最小二乘法拟合的结果，所有点到该直线的距离的平方和最小。蓝线为随机采样一致性算法拟合的结果，它通过随机采样，将样本点分为局内点(图中的绿色点)和局外点(图中的红色点)两部分，并只对局内点进行拟合。

通过两者的对比，可以得到随机采样一致性算法的适用条件是：

1、输入数据是一组带有噪声数据的观测数据，其中可信数据占了绝大多数；

2、存在可以解释可信数据的模型。

随机采样一致性算法的基本算法流程如下：

1、从观测数据中随机选择一个子集，子集数据点个数不小于模型计算所需的最小数量(一般等于模型计算所需的最小数量)；

2、根据选取的子集数据点，估计出适合子集数据点的模型参数；

3、定义误差允许范围，在误差允许范围内，得到在全部观测数据中符合该模型参数的数据，这些数据组成一致性集合；

4、判断一致性集合的数量是否超过设定的阈值，如果超过阈值，则说明此估计模型正确，将此一致性集合内的点视为局内点，其他点视为局外点；如果不超过阈值，返回第一步，重新随机选取子集；

5、利用最小二乘法或其他拟合方法重新对局内点进行拟合。

以平面模型为例，其基本算法流程图如图2。

以上为经典的随机采样一致性算法，经典的随机采样一致性算法有两个重要的参数：一是局内点数量阈值n,二是误差容限函数ε，即可允许的数据点与拟合模型之间的误差范围ε。显然，经典的随机采样一致性算法无法满足定位需求。这主要是因为：

1、完整的随机采样一致性算法每次只能获得一个结果，对于二维室内相对定位，至少需要获得无人机到两个相邻墙面的距离，才能根据距离得到无人机的相对位置坐标。即需要拟合出多个平面，也就是需要进行多次随机采样一致性算法，这种情况下，局内点数量阈值n难以选取。

2、随着无人机的运动，激光雷达扫描到的墙面点的数量会发生变化，这对局内点数量阈值n的选取进一步增加了困难，从而导致当无人机运动到墙边、或者有比较大的姿态变化时，无法拟合出平面模型，从而无法对无人机进行室内定位。

因此需要对随机采样一致性算法进行改进。

上述分析可以看出，问题出在阈值n的选取，因此这里考虑取消此阈值，而转化为对随机采样次数进行限制。基本思想是：不设局内点个数阈值n，即不考虑每次迭代的局内点个数与阈值n的比较，而是直接设置最大迭代次数k，k次迭代后得到k组局内点数值，每组数据有ni(i＝1,2,...,k)个点，找到局内点最多的数据nj，该组的计算结果视为最优解，该组局内点模型视为最终拟合结果。再完成该论拟合后丢弃该组数据，再次基于随机采样一致性算法对新样本进行再次拟合计算，依据此方法即可依次获得多个拟合平面，从而得到激光雷达所在的坐标信息。

此外，考虑到激光雷达的回波强度与材料有关，而一般在室内环境下，墙面材料所占面积最大，回波强度值稳定，因此可以考虑在随机采样时，优先对出现频率高的回波强度值进行采样，以此来提高计算效率。

综合以上分析，本文提出基于激光雷达的室内相对定位方法步骤如下：

步骤1：从激光雷达点云数据中统计反射回波强度值出现的频率，选择频率最高的j组强度值，记为i1,i2,i3,...,ij，根据这些回波强度值设计直通滤波器，保留与这些回波强度值对应的点云数据，作为采样的总样本集u1；

步骤2：从第一步中获得的样本中随机选择3个点云数据作为子集，根据选择的子集数据，得到这三个点所在的平面方程a1x+b1y+c1z+d1＝0，其中a1,b1,c1满足归一化条件：a1²+b1²+c1²＝1；

步骤3：设置误差容限ε1，定义一致集ω1＝φ(空集)，对所有样本点进行遍历，如果样本点ai(xi,yi,zi)满足条件：|a1xi+b1yi+c1zi+d1|＜ε1，则将样本点ai标记为局内点，并将其归入一致集ω1。一致集ω1的元素个数记做k，一致集ω的模型参数即为a1,b1,c1,d1；

步骤4：重新选择3个点云数据作为新的子集，估计出新子集的平面模型a1'x+b1'y+c1'z+d1'＝0，根据选择的新子集模型a1'x+b1'y+c1'z+d1'＝0，在同样误差容限ε1的限定下，对所有样本点进行再次遍历，获得新的一致集ω1'，该一致集的元素个数记做k'，判断k'是否大于k，如果是，则将模型参数a1,b1,c1,d1更新为a1',b1',c1',d1'，同时将一致集ω1更新为ω1'，一致集大小k更新为k'；如果不是，则模型参数a1,b1,c1,d1、一致集ω1和一致集的大小k均保持不变；

步骤5：设置迭代次数上限n1，重复第五步，随机采样n1次，输出n1次采样后的一致集ω1，以及该一致集的模型参数a1,b1,c1,d1，则激光雷达距离墙面a的距离即为|d1|。

步骤6：取新的样本集为u2＝u1-ω1，即把总样本集u1中的数据点中，剔除掉步骤2输出的一致集ω1中的数据点，得到的数据点作为新的样本集，记做u2，对新的样本集重复步骤2的随机采样一致性算法，设置新的误差容限ε2和迭代次数上限n2，输出此时的一致集ω2，以及该一致集的模型参数a2,b2,c2,d2，进而可以得到激光雷达距离墙面b的距离为|d2|。

步骤7：根据步骤5和步骤6输出的结果，以相邻墙面法向量方向建立坐标系，使得室内环境保持在坐标系第一象限。得到每一帧激光雷达点云数据处理得到的二维位置坐标信息(xj，yj)，其中xj表示第j帧点云数据输出的距离|d1|，yj表示第j帧点云数据输出的距离|d2|。

该方法中提取墙面模型和距墙面距离的算法流程图如图3。

实施例1：静止室内环境下对一帧数据的采集与处理

本实验选择在某间办公室进行，实验场景如图4。激光雷达随机放置于桌面上，通过测量激光雷达到墙面a和墙面b的距离，来确定激光雷达的坐标信息。考虑到室内环境的特点，本实验对墙面a设置了人的遮挡，其特点是遮挡可变；而墙面b选择了有书柜遮挡的墙面，其特点是遮挡面积大，但遮挡不可变。

采集数据，共得到55640个点云数据点。使用改进的用于室内相对定位的随机采样一致性算法，对实验数据进行处理，考虑到激光雷达的测量误差在1cm(1σ)以内，雷达参数表给出的测量精度为2cm，而墙面相对比较平整，因此这里为了提高精度，设置误差容限为ε＝3cm，迭代次数设置为n＝600，执行前文步骤1-步骤5，成功将墙面a提取出来，墙面a拟合结果如图5所示。

其中，黑色点为算法判断的局内点数据，即墙面点，共计12288个数据点。而绿色点为局外点数据。

墙面a拟合的标准化平面方程为：

0.8206x-0.5709y-0.0275z-2.7075＝0

因此激光雷达距离该墙面的距离为2.7075m，墙面的法向量为

将初次拟合后的局内点丢弃，执行步骤6，再次进行随机采样，可以得到第二个墙面的拟合结果，如图6所示。

墙面b的数据点共4791个，拟合得到的标准化平面方程为：

-0.5690x-0.8219y+0.0358z-4.7583＝0

根据方程，可以得到激光雷达到该平面的距离为4.7583m，墙面法向量为

至此，已经得到了激光雷达距离两个相互垂直的墙面的距离，执行步骤7，以墙角为原点建立空间直角坐标系，不妨定义方向为x轴方向，方向为y轴方向，则此时激光雷达的位置坐标为(2.7075m，4.7583m)。

而激光雷达的实际坐标为(2.72m，4.77m)，可以看到该方法的精度是非常高的，可以达到2cm以内。

根据以上结果也可以看出，在存在人的遮挡的条件下，依然可以通过随机采样一致性算法来获得墙面模型的信息。在存在大面积遮挡的条件下，依然能够成功提取出墙面模型的特征信息。

实施例2：运动室内环境下从连续多帧激光雷达点云数据中提取位置坐标

当室内存在运动的人或物时，会对墙面造成随机遮挡，因此有必要对连续数据进行处理，以验证当室内存在运动的人或物时，基于随机采样一致性的室内相对定位方法的适用性，并对该方法的精度进行评估。

本实验场景如图7。

如图，实验有人在墙面前自由行走，连续对激光雷达采集的40帧数据进行处理，获取了激光雷达距离墙面a的距离，将每帧数据获得的距离信息与真实距离信息进行对比，可以得到该室内定位方法的精度。

激光雷达距离墙面a的距离d的变化如图8所示。

计算可以得到d的均值为3.0560m，均方差为σ＝0.0029m。而y的真值为3.04m，因此可以绘制出误差的频率分布直方图，如图9。

由图可以直观看出，该方法的定位精度可以达到2cm以内。

由于随机采样一致性算法在采样时的随机性，对墙面的拟合存在一定概率。墙面b数据点少，因此只有32帧数据得到了墙面b的参数模型，将这32帧数据得到的无人机的位置信息绘制出来，得到室内相对定位结果图，如图10。

根据图不难得出，定位的纵坐标在1cm范围内变化，横坐标在3cm附近变化，因此该方法对室内定位能够获得非常好的精度。这里横坐标定位精度略低于纵坐标定位精度，这是因为横坐标等于距离墙面b的距离，而墙面b存在大面积遮挡，虽然依然能拟合出墙面模型，但是墙面b的数据点大约等于墙面a的一半，这导致对墙面b的拟合结果稍差。

作出定位结果的频率分布直方图如图11所示。

通过频率分布直方图可以看出，该方法得到的室内相对定位结果大致服从正态分布。

同时，本实验在采集数据过程中存在人在室内的随机走动干扰，这也证明了该定位方法对运动物体的不敏感。

本发明将随机采样一致性算法应用于室内相对定位。通过将经典的随机采样一致性算法进行改进，利用改进后的随机采样一致性算法去处理室内多线激光雷达点云数据，从多线激光雷达点云数据中拟合得到室内环境的墙面方程，进而解算出雷达相对于室内的位置。通过地面静态验证实验，基于随机采样一致性的室内相对定位方法定位精度可以达到2cm，符合预期指标要求(0.1m)。

该方法解决了室内定位的干扰性强的问题，能够应对室内复杂环境，且对室内环境变化敏感性小，通过合理选择参数，能够提高定位效率，适合于应用于无人机的室内定位工程中。