一种基于多层直接优化的激光点云室内建图与合并方法

本发明涉及一种基于多层直接优化的激光点云室内建图与合并方法，属于机器人视觉。

背景技术：

1、激光点云地图是一种广泛应用于机器人导航、三维重建和环境感知等领域的三维数据表示形式。在点云地图建立过程中，准确地估计帧间运动状态对于环境建模、实时分析和定位的准确性至关重要。通常，点云位姿估计可以通过运动平台上的传感器如惯性测量单元(imu)、全球定位系统(gps)、里程计等设备提供的实时数据计算获得。然而，附加传感器的引入也会带来一系列问题：首先，附加传感器增加了系统的复杂性和成本，对于许多固定、单一的应用场景来说并不经济实用；其次，不同传感器之间的数据融合和校准是一个复杂且易出错的过程，可能引入新的误差源；此外，附加传感器的数据处理和解析也需要额外的算法支持，这将进一步增加建图系统的计算负担。由此可知，上述几方面问题对研究工作中的算法开发和并行优化难度均提出了挑战。

2、在室内相对紧凑且不存在过多动态因素的环境中，采用相对易集成的单一激光传感器轻量化建图方案也可能会达到理想效果，一般而言，纯激光点云建图方案多遵循的研究范式为：首先运用如csm、icp等扫描匹配算法进行点云初步配准，以获取初始位姿变换；随后，基于预测点与实际点间的距离误差构建代价函数，进而执行非线性优化。然而，这一思路在室内环境中仍存在若干待完善之处：(1)csm、icp等算法在处理室内环境时，因点云细节丰富，不宜过度降采样，这导致csm算法的对应关系搜索空间显著增大，计算复杂度急剧上升；虽然icp算法相比csm在效率上有所提升，但在室内几何特征复杂的情况下，其准确匹配对应点的难度增加，延长了算法的运行时间。(2)在执行非线性优化时，室内环境具有丰富的纹理、结构、颜色变化，若仅依赖对应点间的距离信息进行优化，则会忽视这些关键的视觉信息，可能导致建图结果精度不足或出现误差。(3)现有方案往往未对地面、天花板等处的冗余点云进行有效滤除。从空间分布角度看，室内环境中的墙体、楼梯、桌椅等垂向特征显著的目标对于地图构建和自主导航的路径规划至关重要，非线性优化过程应过多地倾向于更为关键的垂向特征点方向的误差减小。

技术实现思路

1、为了解决上述激光点云室内建图过程中存在的一些问题，本发明将提出一种基于多层直接优化的激光点云室内建图与合并方法，该方法无需借助附加传感器，而是将长点云序列划分为多个片段，可使积累误差间断性归零；在每个片段内部进行点云配准和非线性优化时，借鉴相机视觉slam中的多层直接法，利用连续帧之间的像素亮度信息直接、快速地完成对状态估计值的非线性优化，从而做到维持高精度状态估计的同时兼顾建图的实时性。

2、为达到上述目的，本发明的技术方案为：

3、一种基于多层直接优化的激光点云室内建图与合并方法，包含以下步骤：

4、s1.地面信息滤除；

5、s2.基于vfh比对的点云序列片段化；

6、s3.基于多层直接法优化的点云运动状态估计；

7、s4.地图建立及片段合并。

8、进一步，步骤s1所述地面信息滤除，具体过程包括：

9、s11.以激光雷达中心为坐标原点o建立空间直角坐标系o-xyz，其中xoy平面平行于激光雷达所在的运动平台，z轴垂直于xoy平面；滤除与o间距小于0.5m的点云，以防止运动平台对建图造成的干扰；

10、s12.对于当前帧点云中的全部点，逐一计算其在xoy平面的投影相对于y轴正方向的方位角α，若α<0，则α＝α+2π，设定角度分辨率δθ，则全部点云将落入以o为中心的2π/δθ个角度区间之中，即实现了点云按照α完成聚类，对于索引号为i的聚类，记为cluster[i]；

11、s13.对cluster[i]中的各点，计算其在xoy平面的投影相对于o的距离r，并按照r由小到大进行排序，排序后的聚类记为cluster[i]′；

12、s14.测量激光雷达距离地面的高度b，将o投影至地面，得到点o′(0,0,-b)；设定两个阈值：一是cluster[i]′中相邻两点俯仰角度差β1，作为优选，β1＝8°；另一为当前点与点o′间俯仰角度差β2，作为优选，β2＝5°；

13、s15.对于cluster[i]′中任一点c(x,y,z)，计算其与排序中的前一点c0(x0,y0,z0)间的俯仰角若c为cluster[i]′中第一点，则计算相对于o′的俯仰角，及与o′的俯仰角βco′；作为优选，βco′可按下式计算：

14、

15、

16、比较与β1，存在以下两种可能：

17、s151.若c0已判定为地面点，则c仍判定为地面点；若c0为非地面点，但满足|βco′|<|β2|，则c也判定为地面点；

18、s152.设定距离阈值d，作为优选，d＝0.2m；若c和c0之间距离dc>d，此时两者相距较远，若满足|βco′|<|β2|，则c判定为地面点；

19、s16.若建图场景在室内，则cluster[i]′中同时存在地面和房屋顶部点云分布，依然可以按照步骤s15，将两者全部予以滤除；

20、s17.遍历全部聚类，获取单帧非地面点云cloud；对于实时获取或录制的连续多帧点云，可按照步骤s11～s16得到连续的非地面点云序列，记为cloud_rec。

21、进一步，步骤s2所述基于vfh比对的点云序列片段化，具体过程为：

22、s21.在坐标系o-xyz中定义视点v(0,0,h)，h与cloud_rec中点云在z轴正方向的最大值zmax关系为：h＝zmax+10；将cloud_rec中的点云逐帧平移至以观察点v为原点的坐标系v-xyz中，在此坐标系中，点v坐标为(0，0，0)，cloud_rec中任一点坐标由(xi,yi,zi)转换为(xi,yi,zi-h)，转换后的点云序列记为cloud_recv；

23、s22.对于cloud_recv中的每一帧点云，以视点v(0,0,0)计算每一帧的视点特征直方图(vfh)，得到含有308个元素的直方图数组；

24、s23.利用flann函数库中的knnsearch函数计算相邻两帧直方图的误差k，若k大于设定的阈值k0，则认为两帧间特征相差较大，将cloud_recv在此截断；由此可形成q个点云序列片段。作为优选，k0选定不宜过低，否则会造成q值过大，增加合并步骤，一般而言，q≤5。

25、进一步，步骤s3所述基于多层直接法优化的点云运动状态估计，其具体过程包括：

26、s31.对于序列片段中相邻的前一帧点云f1和后一帧点云f2，利用正态分布变换(ndt)由f2向f1配准，即将f2变换至f1所在的坐标系中，得到帧间转换初始矩阵rlt；

27、s32.点云投影建立亮度图像；

28、s33.基于多层直接法的非线性优化。

29、进一步，步骤s32所述点云投影建立亮度图像，具体包括以下步骤：

30、s321.将f1全部点投影至z＝-1的平面上：根据相似三角形特征，对于f1中的索引号为v的点(xv,yv,zv)，投影后的坐标得到的平面化二维点云记为f1_2d，计算其在x方向和y方向坐标的最大值(x′max,y′max)、最小值(x′min,y′min)；

31、s322.将f1_2d进行正方体素化，体素宽度为dd，体素亮度为网格内各点反射强度平均值，将得到一幅二维亮度图像，记为img1，其尺寸为w·h，其中则f1中任一点(xv,yv,zv)在其投影的img1中对应的像素点坐标(uv,vv)为：

32、

33、式中floor为向上取整操作；

34、s323.对于点云f2，按照步骤s321～s322，可得到亮度图像img2。

35、进一步，步骤s33所述基于多层直接法的非线性优化，具体包括以下步骤：

36、s331.对于f2中任一点b2(x2,y2,z2)，其在img2中对应的亮度记为i2(b2)，b2在f1中的估计点坐标为b1＝t·b2，在img1中亮度为i1(b1)；其中t为变量，表示待优化的位姿变换矩阵，其初始值为步骤31所述rlt；则根据直接法的光度一致性假设，执行非线性优化的误差函数e(t)可确定为：

37、e(t)＝i2(b2)-i1(t·b2)    (4)

38、s332.使误差函数最小，需计算误差相对于位姿t的导数，根据李代数求导及扰动模型，可转换为对t的扰动量ξ的求导，即：

39、

40、其中：

41、m1(u,v)是b1投影到i1后对应的像素点，表示图像i1在像素m1(u,v)处的亮度梯度，有：

42、

43、根据式(3)所述投影关系，设点b1坐标为(x,y,z)，则有：

44、

45、根据李代数扰动模型，有：

46、

47、其中b1∧表示b1对应的反对称矩阵，i为单位矩阵；

48、s333.将(6)、(7)、(8)式相乘，可得(5)式结果，即误差对于李代数的雅各比矩阵j的形式：

49、

50、s334.基于g2o优化库，顶点类型为basevertex<6,sophus::se3d>，边的类型为baseunaryedge<1,double,vertexpose>，在linearizeoplus()函数中按照(9)式设定雅各比矩阵j，在computeerror()函数中按(4)式设定误差e(t)，t初始值指定为rlt；

51、作为优选，可按照以下方式配置优化求解器：选择6*1块求解器，即g2o::blocksolver<g2o::blocksolvertraits<6,1>>，表示优化变量维度为6，误差值维度为1；再设定线性求解器类型为位姿矩阵类型，即g2o::linearsolverdense<blocksolvertype::posematrixtype>；优化过程中使用列文伯格-马夸尔特下降方式，即g2o::optimizationalgorithmlevenberg进行求解，并设定最高迭代次数为40次，执行优化算法，即可得优化后得转换矩阵rlt′；

52、s335.采用多层尺度缩放，建立图像金字塔，作为优选，设定层数为4层，由上到下的缩放系数分别为0.25、0.5、0.75、1；在上层图像中利用s331～s334得到的rlt′作为下层优化的初始值，再次进行优化，以逐层提升rlt′精度，得到多层优化结果rlt′m；

53、s336.利用pcl库中的transformpointcloud函数，将f2经rlt′m变换至f1的坐标系中，完成相邻帧的建图。

54、进一步，步骤s4所述地图建立及片段合并，其具体过程包括：

55、s41.按照步骤s3，采用滑动窗口法，得到序列片段内全部相邻帧间优化后的转换矩阵；

56、s42.将片段中的各帧点云均变换到首帧所在坐标系中，将形成单帧建图结果；

57、s43.基于opengl映射完成多地图合并。

58、进一步，步骤s43所述基于opengl映射完成多地图合并，其具体合并步骤为：

59、s431.在opengl中将各片段的建图结果加载为多个图层，各图层所在坐标系不一致，因此需要转换至同一坐标系下，为此需选中一图层为参考图层，记为layer0；

60、s432.利用opengl中的透视投影矩阵和顶点着色器，配合鼠标按键和拖动，实现对当前图层的旋转和平移，直至与layer0部分点云吻合以表示配准，记录在软件窗口坐标系中鼠标移动的起始坐标m1(m1,n1)和终止坐标m2(m2,n2)；余弦定理可求旋转角度θ，其对应的旋转向量(绕z轴旋转)由罗德里格斯公式可得其对应的旋转矩阵r；

61、s433.opengl约定的标准化设备坐标区间x、y∈[-1,1]，设软件窗口尺寸为ws·hs，将窗口映射到opengl坐标系后，在横向、纵向的缩放比例系数分别为ws/2、hs/2；已知窗口坐标系中的平移向量为则根据横、纵缩放比例系数，在opengl中的平移向量为因m1、m2的位置源于屏幕中点云位置选择，而屏幕中呈现的点云位置是经过opengl透视投影所得，因此还需逆透视投影方能得到m1、m2在opengl产生的真实归一化平移向量由于当前透视投影矩阵(一般记为mvp)可由opengl摄像机位置与目标位置决定并提供，因而视为已知量，则有：

62、

63、因此，在opengl坐标系中，我们得到的因鼠标拖到引起的转换矩阵tgl可写为

64、

65、s434.将当前图层通过tgl变换至layer0中，完成当前图层向参考图层的合并；

66、s435.对全部图层实现步骤s432～s434，可得最终合并后的完整地图。

67、本发明的一种基于多层直接优化的激光点云室内建图与合并方法，该方法无需依赖附加传感器与回环检测，通过将接收到三维点云中的地面、屋顶信息滤除，有效减少了噪声干扰，提高了建图的质量；然后基于vfh比对算法，将长序列点云划分为多个片段，从而间歇性地将积累漂移归零，保证了建图的准确性和稳定性；在每个片段内部执行ndt配准及非线性优化时，利用多层直接法基于光度一致性假设，对ndt算法提供的初始运动状态实现微调和修正，由于上述过程无需进行点云特征提取与匹配，因而大幅降低了计算复杂性，可在当前帧存续时间内完成配准-优化工作，展现出良好的实时性能；最后，采用了基于opengl映射的点云合并策略，将序列片段的建图结果进行整合，生成完整、连续的点云地图。