基于多传感器点云的鲁棒配准方法及系统

1.本发明属于测量技术领域，具体涉及一种基于多传感器点云的鲁棒配准方法及系统。


背景技术：

2.点云配准是计算机视觉(computer vision)领域一项重要的研究内容。一直以来，点云配准工作局限在采用单一三维传感器对场景进行点云采集，并将不同视角的点云进行配准得到场景的完整三维表示，但是在面向大场景三维重建时，由于大场景空间范围广、点云数据量大，采用单一三维传感器难以获得场景的全方位信息。
3.因此，有必要建立一种多传感器点云采集系统来解决单一传感器获得场景信息少的难题，并结合多传感器点云的鲁棒配准方法解决多传感器采集的点云配准误差大，鲁棒性差的问题。


技术实现要素：

4.本发明提供基于多传感器点云的鲁棒配准方法及系统，旨在至少解决现有技术中存在的技术问题之一。本发明利用折反射全景相机视场大的优点并结合激光传感器分辨率高的特点，形成了多传感器点云采集系统，提出了多传感器点云的鲁棒配准方法，解决了单一传感器获得场景信息少的难题。解决了多传感器采集的点云配准误差大，鲁棒性差，最后应用本发明提出的点云配准方法进行三维重建、机器人定位等。
5.本发明的技术方案涉及一种基于多传感器点云的鲁棒配准方法，用于移动式多传感器点云采集系统，所述移动式多传感器点云采集系统包括折反射全景相机、激光传感器、移动机器人底盘和可伸缩支架，所述可伸缩支架一端固定连接所述折反射全景相机和激光传感器，另一端固定连接所述移动机器人底盘；所述方法包括以下步骤：
6.a、通过所述移动机器人底盘实时获取平台的位置和姿态，并生成规划路径，根据所述规划控制机器人运动，使得所述折反射全景相机和所述激光传感器在扫描点进行点云采集，得到点云信息；
7.b、通过非线性优化和深度学习的点云配准算法对所述点云信息进行点云配准，得到配准后的点云；
8.c、对所述配准后的点云进行后处理，包括进行三维点云分割和点云三角面片化，得到经过所述后处理的点云。
9.进一步，所述方法还包括：
10.在执行所述步骤a前，对所述折反射全景相机采用泰勒图像模型进行标定；
11.在进行所述点云采集时，将标定球设置在所述激光传感器的视野内，且使所有的所述标定球不在同一平面内。
12.进一步，所述折反射全景相机和所述激光传感器在扫描点进行点云采集，得到点云信息包括：
13.a1、通过编码光发射子系统向观演空间投射编码光，使得所述编码光在被测物体上产生畸变；
14.a2、通过全景相机子系统采集畸变的编码光，并通过基于主动视觉的复杂场景三维全景感知方法，生成深度信息；
15.a3、通过信号处理与控制子系统，将所述深度信息转换为点云信息。
16.进一步，所述通过所述移动机器人底盘实时获取平台的位置和姿态，并生成规划路径包括：
17.通过所述移动机器人底盘的激光雷达、imu和轮式编码器获取的传感器数据，经时间戳同步后进行定位与建图，得到全局地图；
18.基于所述全局地图生成全局轨迹，并基于所述全局轨迹得到一个以当前机器人位置为中心的滑动窗口；
19.当检测到未知障碍物和所述滑动窗口内的轨迹发生碰撞，则生成一条重规划的局部轨迹，并将所述局部轨迹连接到所述全局轨迹上的某个点，得到规划路径。
20.进一步，所述步骤b包括：
21.计算得到刚性变换参数g，计算公式为：
[0022][0023]
其中，p∈rm×3，q∈rn×3，r∈so(3)为旋转矩阵，t∈r3为平移向量， r(f(p)，f(rq+t))＝||f(p)-f(rq+t)||2为激光传感器点云p与变换后的全景相机点云q之间的特征度量投影误差，f是编码器模块学习到的特征提取函数。
[0024]
进一步，所述步骤b包括：
[0025]
b1、基于编码器模块，通过权值共享的多层感知机和最大池化层提取两片输入点云的特征，其中所述输入点云包括激光传感器点云p和全景相机点云q；
[0026]
b2、通过解码器以非监督的方式训练所述编码器模块，将所述特征恢复到点云里对应的部分；
[0027]
b3、计算基于所述特征度量投影误差，公式为：
[0028][0029]
其中f(.)∈rk是提取到点云的全局特征，g是变换矩阵，其中包括旋转矩阵和平移矩阵，k是特征维数，f是所述编码器模块学习到的特征提取函数；
[0030]
并根据所述特征计算雅克比矩阵，公式为：
[0031][0032]
其中，j是f(p)关于变换参数θ的雅可比矩阵，ξ＝(ri，ti)是迭代过程中变换参数的无穷小扰动；
[0033]
将得到的特征误差输入到非线性优化算法中来估计变换增量δθ，更新变换参数，其中
[0034]
δθ＝(j
t
j)-1jtr[0035]
把所述变换参数作用到所述全景相机点云q上，得到的新的全景相机点云q；
[0036]
确定源点云和目标点云是否完整对齐，若不是则返回所述步骤b1。
[0037]
进一步，所述步骤b2包括：
[0038]
基于倒角损失函数和几何损失函数训练所述编码器模块；
[0039]
其中，所述倒角损失函数计算公式如下；
[0040][0041]
其中，p∈a是一组从单位平方内采样的点，x是点云的特征，是多层感知机的第i个神经元的参数，s
*
是原始输入的三维点云；
[0042]
所述几何损失函数的计算公式如下：
[0043][0044]
其中，p是点云，m是它的总点数。
[0045]
进一步，所述步骤c包括：
[0046]
通过cloudcompare对所述点云进行裁剪，得到pts点云文件；
[0047]
通过geomagic将所述pts点云文件封装成三角面片。
[0048]
本发明的技术方案还涉及一种计算机可读存储介质，其上储存有计算机程序指令，所述计算机程序指令被处理器执行时实施上述的方法。
[0049]
本发明的技术方案还涉及一种移动式多传感器点云采集系统，包括扫描仪、移动机器人底盘、可伸缩支架及所述的计算机可读存储介质；其中，所述扫描仪包括折反射全景相机和激光传感器；所述可伸缩支架一端固定连接所述扫描仪，另一端固定连接所述移动机器人底盘。
[0050]
与现有的技术相比，本发明具有以下的特点。
[0051]
1、本发明提供了一种移动式多传感器点云采集系统。设计并搭建了移动机器人作为传感器的底盘，可以实现二维平面的建图与导航，点云采集设备分别采用折反射全景相机和激光传感器，利用折反射全景相机视场大的优点并结合激光传感器分辨率高的特点，形成了多传感器点云采集系统，结合多传感器点云的鲁棒配准方法，解决了单一传感器获得场景信息少的难题。
[0052]
2、本发明提供了一种结合非线性优化和深度学习的点云配准方法。将深度学习方法作为前端提取点云特征，并以特征图的形式表示，将非线性优化方法作为后端估计变换增量，更新变换参数，具有速度快、鲁棒性强等特点，提高了点云配准的精确性。
[0053]
3、本发明提供的鲁棒配准方法在点云处理方面，对配准后的点云进行离群点去除、点云分割、三角面片化、拼接、渲染等操作使得获得的点云更具有真实感。
附图说明
[0054]
图1是根据本发明的移动式多传感器点云采集系统的结构示意图。
[0055]
图2是基于本发明的多传感器点云的鲁棒配准方法的流程示意图。
[0056]
图3是根据本发明的采集路径的规划阶段的流程示意图图。
[0057]
图4是根据本发明的非线性优化和深度学习的点云配准算法的框架示意图。。
具体实施方式
[0058]
以下将结合实施例和附图对本发明的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本发明的目的、方案和效果。
[0059]
需要说明的是，如无特殊说明，当某一特征被称为“固定”、“连接”在另一个特征，它可以直接固定、连接在另一个特征上，也可以间接地固定、连接在另一个特征上。本文所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。此外，除非另有定义，本文所使用的所有的技术和科学术语与本技术领域的技术人员通常理解的含义相同。本文说明书中所使用的术语只是为了描述具体的实施例，而不是为了限制本发明。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的组合。
[0060]
应当理解，尽管在本公开可能采用术语第一、第二、第三等来描述各种元件，但这些元件不应限于这些术语。这些术语仅用来将同一类型的元件彼此区分开。例如，在不脱离本公开范围的情况下，第一元件也可以被称为第二元件，类似地，第二元件也可以被称为第一元件。本文所提供的任何以及所有实例或示例性语言(“例如”、“如”等)的使用仅意图更好地说明本发明的实施例，并且除非另外要求，否则不会对本发明的范围施加限制。
[0061]
参照图1，根据本发明的移动式多传感器点云采集系统，包括扫描仪、移动机器人底盘、可伸缩支架及计算机可读存储介质(图中未示出)。扫描仪包括折反射全景相机和激光传感器；所述可伸缩支架一端固定连接扫描仪，另一端固定连接移动机器人底盘。本发明实施例的折反射全景相机，由于双曲面镜具有单视点的优点，能够一次采集水平方向360
°
、垂直方向240
°
视野范围内场景的高清晰点云；激光传感器，由于激光传播距离远，可以生成视野较长的点云，并且具有分辨率高的成像机制，可以一次采集较长视野的高分辨率点云。移动机器人底盘可以分为离线和在线方式，不仅可以实现机器人的遥控，还可以实现对场景的二维建图，避障与导航，并将建立的二维地图与机器人导航路径显示在pc机上。本实施例的可伸缩支架下方固定在移动机器人平台上，上方固定着采集点云的传感器，根据场景的不同，可以调节伸缩支架的高度。
[0062]
参照图2，本发明实施例的基于多传感器点云的鲁棒配准方法分为采集阶段，配准阶段和后处理阶段，具体步骤如下：
[0063]
a、通过移动机器人底盘实时获取平台的位置和姿态，并生成规划路径，根据规划控制机器人运动，使得折反射全景相机和激光传感器在扫描点进行点云采集，得到点云信息；
[0064]
b、通过非线性优化和深度学习的点云配准算法对点云信息进行点云配准，得到配准后的点云；
[0065]
c、对配准后的点云进行后处理，包括进行三维点云分割和点云三角面片化，得到经过后处理的点云。
[0066]
在一些实施例中，在采集前，首先要对折反射全景相机进行标定，标定时采用泰勒图像模型。
[0067]
在一些实施例中，采用折反射全景相机采集系统，可以分为编码光发射子系统、全景相机子系统、信号处理与控制子系统。编码光发射子系统用于向观演空间投射编码光，并在被测物体上产生畸变。使用全景相机子系统采集畸变的编码光，并通过基于主动视觉的
复杂场景三维全景感知方法，生成深度信息。经过信号处理与控制子系统，将深度信息转换为点云信息。
[0068]
在一些实施例中，折反射全景相机和激光传感器在扫描点进行点云采集，得到点云信息包括：
[0069]
a1、通过编码光发射子系统向观演空间投射编码光，使得编码光在被测物体上产生畸变；
[0070]
a2、通过全景相机子系统采集畸变的编码光，并通过基于主动视觉的复杂场景三维全景感知方法，生成深度信息；
[0071]
a3、通过信号处理与控制子系统，将深度信息转换为点云信息。
[0072]
激光传感器无需提前标定，在采集时将标定球放在激光传感器的视野内，并保证标定球不在同一平面内，即可实现标定。
[0073]
在一些实施例中，在采集路径的规划阶段，通过移动机器人底盘建图导航系统实现。该子系统包括底盘嵌入式控制系统和激光slam建图定位导航系统。底盘嵌入式控制系统包括正交轮定位模块，万向轮和电路控制模块，主要用于移动指挥平台的移动以及实时获取平台的位置和姿态。激光slam建图定位导航系统包括激光雷达，微处理器，显示器，主要用于对大场景进行建图，并能实时定位平台在地图中的位置信息以及对起始点和目标点之间的全局路径规划和局部路径规划。
[0074]
在一些实施例中，参照图3，本发明使用的移动机器人底盘包含激光雷达、imu 和轮式编码器三种传感器，将各传感器数据时间戳同步后送入同时定位与建图模块 (simultaneous localization and mapping，slam)，将slam输出的地图作为全局地图供定位和规划模块使用。这里将运动规划模块分为两部分：全局规划和局部规划。一般传统方法(例如ros中的navigation功能包)中，全局规划的结果为一条路径，即不考虑时间维度，其作用只是为局部规划提供“指示”作用，控制指令是通过局部规划实时生成的。而本发明的思路是将全局规划直接构建成一个轨迹生成器，即在全局地图上直接生成一条可交给控制模块执行的高质量轨迹，但由于生成的全局轨迹只能保证不和静态地图发生碰撞，而现实中往往会出现地图中不存在的动态障碍物或者地图轻微改变的情况，因此在后端加入一个局部规划来实现避障功能。由于传感器感知范围和计算资源的限制，每次进行全局的重规划是不合理的，因此本发明实施例在全局轨迹上维护一个以当前机器人位置为中心的滑动窗口，当检测到未知障碍物和滑动窗口内的轨迹发成碰撞，则生成一条重规划的局部轨迹实现避障，并重新连接到全局轨迹上前方的某个点。在获得了可靠的轨迹后，将轨迹交给控制模块去跟踪，最终输出速度指令控制机器人进行运动。
[0075]
通过全局和局部路径规划，底盘建图导航系统可以在空间场景内自主规划出最佳的路径和扫描点，将三维扫描与建模系统以最优路径传送到扫描点。这样不仅节省操作人员时间和精力，而且实现了更加精确的三维点云采集，使大场景扫描更加全面、完整。
[0076]
在一些实施例中，在配准阶段，参照图4，结合非线性优化和深度学习的点云配准算法，这是算法的主体框架，第一部分是编码器，通过权值共享的多层感知机和最大池化层提取两片输入点云的特征。第二部分是个多任务的神经网络解决配准问题，共两个分支，第一个分支是解码器对特征进行解码，解码器用的是四层的全连接，通过解码器将特征恢复到点云里这些特征对应的部分。第二块是首先计算基于特征的投影误差，同时根据特征计
算雅克比矩阵，然后将特征误差输入到非线性优化算法中来估计变换增量，更新变换参数，最后把变换参数作用到输入点云q上，这样再开始新一轮迭代，直到源点云和目标点云完整的对齐。
[0077]
给定两个点云p∈rm×3，q∈rn×3，配准的目标是找到使点云q与p最佳对齐的刚性变换参数g(旋转矩阵r∈so(3)，平移向量t∈r3)，
[0078][0079]
在公式(1)里，r(f(p)，f(rq+t))＝||f(p)-f(rq+t)||2是点云p与变换后的点云q之间的特征度量投影误差。f(p)∈rk是点云p的特征，k是特征维数，f是编码器模块学习到的特征提取函数。为了求解上式(1)，提出了一个特征配准框架，该框架综合了经典非线性优化算法和无监督学习技术的优点，可以以半监督或无监督的方式训练框架。首先，对两片输入点云提取关注旋转的特征；其次，将特征输入到多任务模块中；在第一个分支中，解码器被设计成以非监督的方式训练编码器模块。在第二分支中，计算投影误差r来表示两个输入特征之间的差值，通过最小化特征差值来估计最优变换。对变换估计进行迭代，并采用逆合成算法估计每一步的变换增量(δθ)：
[0080]
δθ＝(j
t
j)-1jt
r#(2)
[0081]
其中r是特征度量投影误差，j是f(p)关于变换参数θ的雅可比矩阵：
[0082][0083]
其中，ξ＝(ri，ti)是迭代过程中变换参数的无穷小扰动。
[0084]
编码器模块的目的是学习一个特征提取函数f，可以提取出输入点云独一无二的特征，编码器网络设计的主要原则是所生成的特征应该是注意旋转的，以便在变换估计时能够反映关于旋转的差异。该特征由两层多层感知机和一个最大池化层提取。
[0085]
特征提取完成后，下一步是特征学习和点云变换估计，该估计是通过直接最小化特征度量投影误差而最终确定的，并且不需要搜索对应点。在编码器模块生成独特的特征后，使用解码器模块将特征恢复到3d点云中，编码器-解码器网络可以以无监督的方式进行训练，有助于编码器模块学习生成具有旋转差异的显著特征，该分支的原理是编码器模块为两片点云生成不同的特征，解码器可以将不同的特征恢复到相应的旋转后的点云。解码器块由四层全连接层组成，由leakyrelu作为激活函数，解码器模块的输出与输入点云具有相同维数。
[0086]
为了解决配准问题，本发明通过使用非线性优化算法来估计变换参数，以最小化特征度量投影误差。定义特征度量投影误差为：
[0087][0088]
其中f(.)∈rk是提取到点云的全局特征，g是变换矩阵，其中包括旋转矩阵和平移矩阵。
[0089]
训练的关键任务是编码器模块，用于提取基于旋转的特征，本发明为弱监督框架提出两个损失函数，通过忽略有监督几何损失，可以很容易地推广到无监督框架。两个损失函数分别为倒角损失和几何损失，倒角损失常作为点云配准网络的损失函数，数值越小说明配准结果越好，倒角损失函数如下：
[0090][0091]
其中，p∈a是一组从单位平方内采样的点，x是点云的特征，是多层感知机的第i个神经元的参数，s
*
是原始输入的三维点云。
[0092]
几何损失函数的目标是使变换矩阵的估计值与变换矩阵的真值之间的差值最小，几何损失函数如下：
[0093][0094]
其中，p是点云，m是它的总点数，弱监督训练的最终损失函数为：
[0095]
loss＝loss
cf
+loss
pe
#(7)
[0096]
在弱监督训练的情况下，损失函数包括倒角损失和几何损失，而在无监督训练时，损失函数直接用倒角损失函数计算。
[0097]
在一些实施例中，本发明对配准后的点云进行后处理，使得场景最终的三维表示更具有真实感，具体需要采用cloudcompare，geomagic，3dmax这三种软件，下面将具体操作步骤展开描述。
[0098]
c.1三维点云分割。
[0099]
首先在cloudcompare中打开配准后的点云，首先将一些离群点和噪点去除掉，选中该点云文件，使用上方菜单栏中剪刀样式的裁剪功能，点击鼠标左键是开始裁剪并固定一个个点，点击右键完成裁剪。通过操作在裁剪选项菜单保留裁剪框以外的地方，或保留绿色裁剪框以内的地方，完成裁剪。裁剪后在左侧文件栏会有两个文件，选中你想要继续裁剪的点云继续完成后续裁剪工作，以上操作视点云实际情况而定，若无离群点可直接对目标点云进行裁剪分割。
[0100]
接着要对点云的每个空间几何面进行手动裁剪，将目标点云摆正，使用裁剪功能将房间的一个侧面框住，并保留裁剪的内部。选中该文件，点击左上角的保存，并保存为pts格式，这样就完成了一个面的裁剪。接下来选中被裁剪后的点云文件，再对每个面像上面说的一样进行裁剪，并每个都保存成pts文件，这样就完成了三维点云分割。
[0101]
c.2点云三角面片化。
[0102]
这一部分需要在geomagic软件中完成，geomagic的主要功能就是将之前裁剪好的一个个pts点云文件封装成三角面片，并转换成obj格式。首先在geomagic界面中将该pts文件打开，然后在上面菜单栏中选择“点
”‑“
封装”,然后点击确定，完成封装，再点击左上角的保存，将该文件保存成obj格式，之后将每个pts文件如上述操作一遍。
[0103]
最后，将三角面片化的文件导入3dmax中，由于上述的处理并不改变点云文件中每个点的空间位置，因此，导入后的obj文件在空间几何位置上也是相连的，全部导入后，然后选择保存为max文件，至此，所有后处理操作完毕，经过后处理的点云从三维重建角度来讲更具有真实感。
[0104]
应当认识到，本发明实施例中的方法步骤可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机系
统通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。
[0105]
此外，可按任何合适的顺序来执行本文描述的过程的操作，除非本文另外指示或以其他方式明显地与上下文矛盾。本文描述的过程(或变型和/或其组合)可在配置有可执行指令的一个或多个计算机系统的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。
[0106]
进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、ram、rom等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本文所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还可以包括计算机本身。
[0107]
计算机程序能够应用于输入数据以执行本文所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。
[0108]
以上所述，只是本发明的较佳实施例而已，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。在本发明的保护范围内其技术方案和/或实施方式可以有各种不同的修改和变化。