一种三维地图的构建方法、装置、设备及存储介质与流程

1.本发明涉及机器视觉技术领域，尤其涉及一种三维地图的构建方法、装置、设备及存储介质。


背景技术：

2.目前，同时定位与地图构建(simultaneous localization and mapping，slam)在机器人导航领域中是一个重要的环节。机器人搭载多种传感器，对探索过的环境建立一个全局的地图，同时能够在任何时候，利用该地图推测自身的位置。其中，传感器的数据特征和观测数据相关性直接影响着机器人的定位和地图构建的精度。
3.对于在密闭电缆沟的施工过程中，人工放线施工的难度大且存在安全风险，可以通过放线机器人来代替人工完成放线工作。利用激光雷达采集到的点云较为稀疏，用稀疏的点云建立的三维地图中，部分信息会缺失。另外，放线机器人工作环境比较复杂，存在光线暗、对比度低且障碍物在不同的高度等情况，利用双目视觉采集点云时受环境影响较大，环境条件不满足时双目视觉采集的点云偏差较大，建立的三维地图也会相应的存在较大误差。


技术实现要素：

4.本发明提供了一种三维地图的构建方法、装置、设备及存储介质，以解决在激光雷达采集的点云稀疏而双目相机受环境影响大，都难以适应各种环境而建立精准的三维地图的问题。
5.根据本发明的一方面，提供了一种三维地图的构建方法，所述方法包括：
6.控制机器人调用激光雷达对指定的区域采集第一点云、同时调用双目相机对所述区域采集第二点云，所述双目相机为rgb-d相机；
7.计算所述第一点云与所述第二点云之间的偏差；
8.若所述偏差大于预设的第一阈值，则根据所述第一点云对所述区域构建三维地图；
9.若所述偏差小于或等于预设的第一阈值，则将所述第一点云与所述第二点云融合为第三点云，根据所述第三点云对所述区域构建三维地图。
10.根据本发明的另一方面，提供了一种三维地图的构建装置，所述装置包括：
11.点云采集模块，用于控制机器人调用激光雷达对指定的区域采集第一点云、同时调用双目相机对所述区域采集第二点云，所述双目相机为rgb-d相机；
12.偏差计算模块，用于计算所述第一点云与所述第二点云之间的偏差；
13.三维地图构建模块，用于若所述偏差大于预设的第一阈值，则根据所述第一点云对所述区域构建三维地图；若所述偏差小于或等于预设的第一阈值，则将所述第一点云与所述第二点云融合为第三点云，根据所述第三点云对所述区域构建三维地图。
14.根据本发明的另一方面，提供了一种电子设备，所述电子设备包括：
15.至少一个处理器；以及
16.与所述至少一个处理器通信连接的存储器；其中，
17.所述存储器存储有可被所述至少一个处理器执行的计算机程序，所述计算机程序被所述至少一个处理器执行，以使所述至少一个处理器能够执行本发明任一实施例所述的三维地图的构建方法。
18.根据本发明的另一方面，提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使处理器执行时实现本发明任一实施例所述的三维地图的构建方法。
19.本发明实施例的技术方案，在激光雷达采集第一点云的基础上增加了双目相机采集第二点云，由于激光雷达采集的点云具有稳定但是稀疏的特性，因此第一点云的置信度高但信息量不多，当第一点云与第二点云偏差较大时，表示双目相机采集的第二点云误差较大，此时可以只使用第一点云来构建三维地图，当第一点云与第二点云偏差较小时，表示第二点云的置信度高，而对于不同传感器采集的点云所包含的信息具有差异，置信度高的第二点云可以作为稀疏的第一点云的补充，第一点云与第二点云融合得到的融合结果保持信息量丰富且置信度高，利用融合结果构建三维地图可以提高三维地图的精确度，在确保三维地图的精确度同时还能很好地适应不同的环境状况。
20.应当理解，本部分所描述的内容并非旨在标识本发明的实施例的关键或重要特征，也不用于限制本发明的范围。本发明的其它特征将通过以下的说明书而变得容易理解。
附图说明
21.为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
22.图1是根据本发明实施例一提供的一种三维地图的构建方法的流程图；
23.图2是根据本发明实施例一提供的一种相机模型的坐标系示意图；
24.图3是根据本发明实施例一提供的一种双目相机与激光雷达的联合标定示意图；
25.图4是根据本发明实施例一提供的一种点云融合流程示意图；
26.图5是根据本发明实施例二提供的一种三维地图的构建装置的结构示意图；
27.图6是实现本发明实施例的三维地图的构建方法的电子设备的结构示意图。
具体实施方式
28.为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
29.需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用
的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
30.实施例一
31.图1为本发明实施例一提供了一种三维地图的构建方法的流程图，本实施例可适用于放线机器人在电缆沟等光照情况不好的复杂环境情况，该方法可以由三维地图的构建装置来执行，该三维地图的构建装置可以采用硬件和/或软件的形式实现。
32.传统的基于立体视觉的方法是敏感的光照条件下，特征提取不完全，在低对比度的情况下，本发明实施例提出了一种测量系统它将立体视觉信息与三维激光雷达相结合并通过ekf算法估计目标的姿态和速度。当不满足双目视觉匹配条件时，目标的姿态和速度可以根据激光雷达的三维信息进行估计；反之，当条件满足三维重建时，立体视觉重建点云信息与获取的点云信息合并激光雷达，从而获得目标点云数据。进一步，利用ekf算法对姿态进行估计，并对其进行了仿真非合作目标的运动。融合双眼视觉系统和三维激光雷达系统，测量方案能满足空间近距离和远距离的要求测量。此外，该系统还可以适应突发事件空间照明条件和提供姿态测量运动估计方法具有良好的鲁棒性和较高的鲁棒性目标的估计精度。
33.如图1所示，该方法包括：
34.s110、控制机器人调用激光雷达对指定的区域采集第一点云、同时调用双目相机对区域采集第二点云，双目相机为rgb-d相机。
35.该步骤中，视觉系统具有独立性、准确性、可靠性以及信息完整性等优势，可完成目标识别、障碍物回避及路径规划等功能。由于单目视觉无法得到目标物体的深度信息，难以估计三维位置信息，为满足复杂环境下的导航需求，将立体视觉导航方法引入自主导航领域。本发明实施例中所采用的双目相机是rgb-d相机(深度相机)，rgb-d相机是在普通的摄像头功能上添加了一个深度测量，输出的深度图是包含与视点的场景对象的表面距离有关的信息的图像。深度图像是一种普遍的三维场景表达方式。利用2个相对位置固定的摄像机可以获取载体的三维信息，但是通过视觉系统的测量方法容易受光照条件的影响而失效，特别是在目标特征提取不完全、对比度较低的情况下。
36.激光雷达具有良好的稳定性，但点云稀疏，在需要精确识别捕获点时存在一些不足。
37.根据激光雷达和双目相机的尺寸和参数建立激光雷达和立体相机模型。本发明实施例中，激光雷达采用单线激光雷达rplidar a2(一种激光扫描测距雷达)激光雷达系统，系统可支持水平视场16个通道，测量角度范围为360
°
，垂直视野30
°
，上下
±
15
°
，有效范围为100m；深度相机可以采用指定的某种机型，示例性的，指定机型的深度相机可以通过光编码技术获取深度图，其深度测距的有效范围0.6～8米，精度达到3毫米，其深度相机视角可以达到水平58
°
和垂直45.5
°
。
38.其中，二维激光采集水平360
°
范围的数据，里程计提供扫描匹配初始值，可以很好地计算快速旋转时候的位姿，弥补针对深度相机视角不足，造成的无法处理快速旋转的缺
陷。
39.左右摄像机独立地识别出目标的任意一个对应点pi(xi，yi，zi)，该点在两个摄像机图像平面上的像素坐标可以表示为p
l
(u
l
，v
l
)。左摄像机和右摄像机的摄像头坐标系分别表示为o
l-x
lylzl
和o
r-xryrzr，以及激光雷达机身坐标系中任意点的坐标(比如o
vlp-x
vlpyvlpzvlp
可以表示为(x
vlp
，y
vlp
，z
vlp
)。
40.在本发明实施例中，示例性的，针对放线机器人在电缆沟中对于障碍物检测和环境建图的应用需求，由于单一传感器测量方法在精度和稳定性上的缺陷，结合环境的复杂性，传统的基于纯视觉的测量方法容易受光照条件的影响而失效；虽然激光雷达具有良好的稳定性，但点云稀疏，在需要精确识别捕获点时存在一些不足。利用激光雷达与双目相机共同对指定区域进行建图可以很好地对环境的变化有自适应性。
41.在具体实现时，需要建立激光雷达模型，可以采用单线激光雷达在机器人上水平安装，通过云台360
°
转动，向周围发射激光，激光遇到障碍物返回后，被接收器获取，并通过计算发射和接收的时间差来计算障碍物距离。当忽略激光自转中对不同角度的采样时间差，可以采用条件概率分布表示三维激光雷达的观测模型表示为：
42.p(zk|xk，m)
43.其中xk为k时刻机器人的位姿，m为地图信息(地图信息包括环境中物体信息的列表及其位置)，zk是激光观测，激光每自转一周采样多次，即
[0044][0045]
可以认为一次扫描中的所有激光点相互独立，则对于完整的一圈扫描有：
[0046][0047]
另外，需要建立立体相机模型，在建立立体相机模型时，由于双目相机为深度摄像机rgb-d，可以同时获取彩色图像和深度图像。首先对相机进行建模，如图2的相机模型的坐标系示意图所示，由三角相似关系，相机模型的坐标系下的点p[x
c y
c zc]
t
与其在物理平面xoy上的成像点[x
p
，y
p
，f]
t
的关系为：
[0048][0049]
其中f为摄像头的成像焦距。
[0050]
在物理平面xoy上的成像点[x
p
，y
p
，f]
t
与像素坐标[u v]
t
的关系为：
[0051][0052]
其中α、β为x、y方向的放大系数，单位为像素/米，xc、yc为相机光心在图像上的坐标偏移量。
[0053]
最后得到的像素坐标与相机坐标系下的三维坐标之间的关系为：
[0054][0055]
其中为相机的内参矩阵。
[0056]
根据融合校准算法对激光雷达和相机进行标定。参考图3的双目相机与激光雷达的联合标定示意图，首先对激光雷达和相机进行联合标定，联合标定的目的是获得激光雷达坐标系与相机坐标系之间的齐次变换矩阵，使激光雷达数据与摄像机图像相对应。
[0057]
左摄像头采集的图像数据用(u
l
，v
l
)表示，激光雷达采集的三维点云数据用(x
vlp
，y
vlp
，z
vlp
)表示。校准的目标是创建一个转换矩阵m，将三维点(x、y、z)映射到2d点(u
l
，v
l
)，即：
[0058][0059]
其中，是左相机的内参矩阵；f
ul
，f
vl
表示xy轴方向的比例因子(即水平和垂直方向的有效焦距)；u
l0
，v
l0
为图像平面的主坐标系；m
l
＝[r
l
t
l
]，r
l
为3*3的姿态旋转矩阵，t
l
是3*1的平移向量。
[0060]
另一方面，根据左相机的成像原理，可以得到：
[0061][0062]
然后得到公式如下：
[0063][0064]
因此，激光雷达坐标系相对于左摄像机坐标系的旋转矩阵和平移矢量为：
[0065][0066]
为了便于机械安装和更好地识别目标，激光雷达可以安装在双目摄像机的中间。校准板由9行12列组成，每个正方形为25毫米，摄像头距离校准板约2米。详细的接头校准步骤如下：
[0067]
(1)调整摄像机。根据距离、f(焦距)等因素调整摄像机的位置，使定标板置于摄像
机和激光雷达的视场内。为了获得更好的标定结果，标定板在视野中的比例应尽可能高。
[0068]
(2)获取图像。在距标定板约2米处，双目摄像机通过外部触发，同时捕捉标定板不同位置和角度的20幅图像。
[0069]
(3)计算双目摄像机的内外参数。利用matlab标定工具箱计算了双目摄像机的内外参数和畸变系数。
[0070]
(4)获取激光雷达点云数据。提取激光雷达各角点的三维坐标(对应于双目摄像机的像素坐标)。
[0071]
(5)计算测量系统的外部参数。根据双目摄像机的标定结果，由公式得到测量系统的外参数c
lrvlp
和c
l
t
vlp
。
[0072]
通过模型建立和联合标定可以得到以下的标定参数：
[0073]
内参数校正矩阵与畸变双目摄像机的参数分别为：
[0074][0075][0076]
双目摄像机的外部参数矩阵为：
[0077][0078][0079]
基于联合标定算法，确定了外部参数三维激光雷达坐标系的标定结果对于左摄像机帧，可以获得如下结果：
[0080][0081]cl
t
vlp
＝[-202.48 1.0698
ꢀ‑
19.41]
t
(mm)
[0082]
另外，对于右摄像机可以用同样的方式进行坐标变换。
[0083]
s120、计算第一点云与第二点云之间的偏差。
[0084]
该步骤中，当第一点云与第二点云的偏差较大时，可以推测双目相机采集的第二点云有较大概率是不够准确的，即环境情况不足以让双目相机采集准确的点云信息；而当第一点云与第二点云偏差较小时，可以推测双目相机采集的第二点云较准确。
[0085]
在一种实施例中，s120包括如下步骤：
[0086]
s120-1，计算第一点云的第一中心；
[0087]
s120-2，计算第二点云的第二中心；
[0088]
s120-3，计算第一中心与第二中心之间的距离，作为第一点云与第二点云之间的偏差。
[0089]
该步骤中，在同时获取双目相机和激光雷达的点云数据后，可以计算第一中心和第二中心，其中，第一中心可以是第一点云中所有点云坐标的均值，第二中心可以是第二点云中所有点云坐标的均值。第一中心与第二中心的距离可以是通过三维空间公式算出的欧式距离，将该距离作为第一点云与第二点云之间的偏差。
[0090]
s130、若偏差大于预设的第一阈值，则根据第一点云对区域构建三维地图。
[0091]
该步骤中，距离较大就说明第一点云与第二点云之间的偏差就大，推测双目相机采集的数据可能不准确，而激光雷达具有良好的稳定性，不容易受环境的影响导致出现较大的点云误差，此时可以只采用激光雷达采集的第一点云对区域进行三维地图的构建，虽然只用第一点云构建三维地图会存在点云较少的情况，但是总体效果会比结合误差较大的第二点云构建的三维地图更加准确。
[0092]
s140、若偏差小于或等于预设的第一阈值，则将第一点云与第二点云融合为第三点云，根据第三点云对区域构建三维地图。
[0093]
该步骤中，距离较小就说明第一点云与第二点云之间的偏差就小，推测双目相机采集的数据准确度较高，现场环境可以满足双目相机获取精确的点云信息的条件，可以通过将第一点云和第二点云进行融合，将融合后得到的第三点云。也就是可以将双目立体视觉信息与激光雷达信息相结合地进行融合测量，通过双目立体视觉系统和三维激光雷达系统分别对非合作目标进行三维重建，对得到的两个点云集进行数据融合，得到完整的三维点云图像。
[0094]
在一种实施例中，针对s140中将第一点云与第二点云融合为第三点云，包括如下步骤：
[0095]
s141，将第一点云与第二点云投影到同一平面上。
[0096]
该步骤中，在第一点云与第二点云的融合过程中，可以把已经完成标定的双目相机以及激光雷达所分别采集的第一点云和第二点云投影到同一平面上。
[0097]
在一种实施例中，第一点云所在的坐标系为激光坐标系，第二点云所在的坐标系为相机坐标系，第二点云中包含地面点云；s140-1包括如下步骤：
[0098]
将在激光坐标系下的点云转换到相机坐标系下；
[0099]
通过平面拟合构建包含相机坐标系的原点的平面方程；
[0100]
从第二点云中识别出地面点云，并将地面点云从第二点云中清除；
[0101]
将转换坐标系后的第一点云以及清除地面点云后的第二点云投影到平面方程所对应的平面上。
[0102]
该步骤中，由于激光雷达采集的第一点云与双目相机采集的第二点云并不在同一个坐标系上，利用联合标定得到的激光雷达与双目相机这两传感器间的旋转平移矩阵，可通过将两者获取的点云转换到同一坐标系下。首先，将激光坐标系下的二维点云转换到相机坐标系并通过平面拟合构建包含相机坐标系原点的平面方程：ax by cz＝d，即拟合平面方程。由于双目相机采集到的第二点云很大概率包含地面点云，地面点云不属于机器人导航时的障碍物信息，因此要将该部分点云去除。最后，通过忽略双目相机获取的描述空间障
碍物的三维点云的z轴信息，只保留x轴和y轴信息，可将三维点云投影至平面方程ax by cz＝d所对应的平面上。
[0103]
s142，将第一点云进行刚性变换，直至第一点云与第二点云对齐。
[0104]
该步骤中，假设[x
tk y
tk z
tk
]
t
和[x
tk 1 y
tk 1 z
tk 1
]
t
分别表示融合测量系统在tk和t
k 1
时刻获得的三维坐标。因此，相邻时刻匹配点之间的刚性变换满足以下关系，即：
[0105][0106]
其中，δ
krk 1
和δkt
k 1
分别表示目标的旋转矩阵和平移矢量。
[0107]
s143，针对已对齐的第一点云与第二点云，基于激光雷达与双目相机之间的关系计算第一点云与第二点云之间的误差。
[0108]
该步骤中，激光雷达与双目相机之间的关系可以是指联合标定后得到的点云坐标之间的变换关系。第一点云与第二点云之间的误差可以用于判断第一点云与第二点云是否匹配完成。
[0109]
在一种实施例中，s143包括如下步骤：
[0110]
针对已对齐的第一点云与第二点云，计算第一点云的几何特征与第二点云的几何特征，几何特征包括平均距离δ、曲率ρ和法向角φ；
[0111]
通过如下公式计算第一点云与第二点云之间的误差：
[0112][0113]
其中，e为第一点云与第二点云之间的误差，nm为第一点云中的点云个数，nd为第二点云中的点云个数，pk和p
k 1
为一对成对点云，ω
i，j
是成对点云的加权系数，mi为第一点云中的第i个点，dj为第二点云中的第j个点，δr是mi与dj之间的旋转矩阵，δt是mi与dj之间的平移矢量，n为第一点云与第二点云的总点云个数，|| ||与|| ||均为范数运算符号，成对点云为第一点云与第二点云投影到同一平面上，指示同一地方的一对点云。
[0114]
该步骤中，成对点云指的是第一点云与第二点云投影到同一平面上，指示同一地方的一对点云。将第一点云和第二点云的几何特征引入到误差函数中，得到的误差结果是第一点云与第二点云的匹配误差，可以提高点云配准的精度。
[0115]
s144，若误差小于预设的第二阈值，则将第一点云的坐标乘以第一权重得到第一结果，将第二点云的坐标乘以第二权重得到第二结果，计算第一结果与第二结果的和值作为第三点云的坐标，将第三点云的坐标作为融合结果，其中，若激光雷达的精度高于双目相机的精度，则第一权重大于第二权重，若所述激光雷达的精度低于所述双目相机的精度，，则第一权重小于第二权重。
[0116]
该步骤中，若误差小于预设的第二阈值，则意味着第一点云与第二点云匹配完成，可以进行第三点云的求解从而实现第一点云与第二点云的融合。第一权重与第二权重则可以通过用户根据设备的情况进行设置，示例性的，用户可以根据以及精度高低来进行权重的分配。
[0117]
s145，若误差大于或等于预设的第二阈值，则调整激光雷达与双目相机之间的关系，返回执行针对已对齐的第一点云与第二点云，基于激光雷达与双目相机之间的关系计算第一点云与第二点云之间的误差。
[0118]
该步骤中，若误差大于或等于预设的第二阈值，意味着第一点云与第二点云没有匹配完毕，可以通过最小化计算误差的公式重新进行匹配，对计算误差的公式进行优化，可以表达为：
[0119][0120]
也就是在输入几何特征后通过最小化误差得到新的r(旋转矩阵)和t(平移矢量)，达到优化了误差公式的效果。在调整激光雷达与双目相机之间的关系得到优化后的计算误差的公示以后，可以返回针对已对齐的第一点云与第二点云，基于激光雷达与双目相机之间的关系计算第一点云与第二点云之间的误差，再次进行匹配以及误差的计算，直至匹配完成然后实现第一点云与第二点云的融合。
[0121]
在一种实施例中，针对s140中根据第三点云对区域构建三维地图，包括如下步骤：
[0122]
对激光雷达进行运动估计得到第一运动结果，对双目相机进行运动估计得到第二运动结果；
[0123]
对激光雷达进行位姿估计得到第一位姿，对双目相机进行位姿估计得到第二位姿；
[0124]
根据第一位姿以及第一运动结果确定第一目标位姿；
[0125]
根据第二位姿以及第二运动结果确定第二目标位姿；
[0126]
利用卡尔曼滤波器对第一目标位姿与第二目标位姿进行融合，得到融合位姿；
[0127]
基于融合位姿，构建区域的三维地图。
[0128]
该步骤中，在得到融合后的第三点云，可以开始基于第三点云进行姿态测量与运动估计。为了估计姿态和速度，本发明实施例中定义了两个坐标系，即参考坐标系和目标坐标系。参考坐标系设置在左摄像机坐标系，坐标系原点位于左摄像机中心，即参考坐标系可表示为目标坐标系为o
b-xbybzb，其坐标系原点位于目标的旋转中心。运动估计方法可分为以下步骤：
[0129]
(1)根据非合作目标的三维点云模型，计算出目标的几何特征δ，ρ，φ等，并对目标进行识别。
[0130]
(2)根据ekf(扩展卡尔曼滤波器)算法，对相邻的两个点云模型进行icp匹配(iterative closest point，一种基于数据配准法，利用最近点搜索法，从而解决基于自由形态曲面的一种算法)，得到非合作翻滚目标的姿态和速度。其中拓展卡尔曼滤波(ekf)如下所示：
[0131]
卡尔曼滤波器适用于线性高斯系统，用于在最小均方误差下的最优估计情况下，求取系统的最优估计。扩展卡尔曼滤波将卡尔曼滤波必须在线性高斯系统的条件进行扩展，通过在滤波点附近泰勒展开，利用一阶近似值将系统线性化，可以应用于本发明实施例
的非线性系统中。本发明实施例对于视觉与激光定位结果的融合部分，采用扩展卡尔曼滤波方法进行位姿融合。
[0132]
对于非线性系统，
[0133][0134]
其中uk是控制输入，wk和vk分别是过程噪声和观测噪声，二者的都是具有零均值的多变量高斯噪声，协方差分别为qk、rk。公式中，函数f可以从上一时刻的状态计算下一时刻状态的预测，h用于从预测状态计算预测观测，不一定是状态的线性函数，但一定是非线性可微的。
[0135]
在扩展卡尔曼滤波中，在当前时刻对系统线性化后，使用每一时刻的雅克比来预测下一时刻的状态。
[0136]
扩展卡尔曼滤波分为预测和更新两部分。
[0137]
预测部分有：
[0138]
x
k|k-1
＝f(x
k-1|k-1
，uk)
[0139][0140]
更新部分有：
[0141]
yk＝z
k-h(x
k|k-1
)
[0142][0143]
x
k|k
＝x
k|k-1
kkyk[0144]
p
k|k
＝(i-k
khk
)p
k|k-1
[0145]
其中，状态转移矩阵元素fk和观测方程的雅克比矩阵元素hk分别是
[0146][0147][0148]
在预测阶段，由于没有考虑观测的作用，分别对状态与其协方差进行预测：
[0149]
通过上一时刻的后验x
k-1|k-1
和当前控制输入uk预测当前时刻的先验x
k|k-1
；p
k|k-1
为协方差先验的预测。
[0150]
在更新阶段，yk为观测残差sk的过程；计算近似卡尔曼增益kk，即与残差在更新中的权重相关；最后根据观测对预测进行修正，获得k时刻更新的状态估计x
k|k
；p
k|k
为更新协方差后验估计。
[0151]
视觉特征点用于匹配后进行运动估计，激光方法采用相关性匹配进行运动估计，所以当视觉与激光同时定位成功时，系统同时输出两个位姿，对二者位姿结果进行ekf融合；当视觉跟踪不成功时，采用激光的定位结果拼接深度相机的点云数据，获取三维地图，同时，继续在后续帧进行特征探测和匹配，重新初始化视觉slam中的地图点，若成功，则继续采用激光雷达与双目相机的融合模式，否则一直使用激光的定位结果建立三维地图。
[0152]
视觉slam获取的定位信息是一个具有六自由度的三维运动，在与激光雷达获取的二维位姿相融合时，需要分解出其在二维地图平面上的运动，即由表示相机姿态的三维旋转矩阵分解出在世界坐标系中xy平面上的位姿分量。由于本发明实施例中的rgb-d相机与激光雷达皆水平安装，认为相机坐标系中的zx平面上的位姿变化即世界坐标系中的xy位姿。那么问题转换为两个二维运动的扩展卡尔曼滤波融合问题。在应用中需要注意以下问题：
[0153]
(1)随着时间的推移，从视觉和激光slam获取的绝对位姿会出现累计误差，所以使用每个传感器的相对位姿差来更新扩展卡尔曼滤波器。
[0154]
(2)当机器人移动时，其在世界参考中的不确定性越来越大。随着时间的推移，协方差会无限增长。因此发布姿态的协方差是无效的，需要发布速度的协方差。
[0155]
(3)由于传感器数据不是绝对同一时间到达，需要在滤波融合时对传感器数据进行插值。
[0156]
在视觉slam跟踪定位失败，采取重定位策略的同时，可以采用激光slam提供的数据持续获取位姿，再提供给视觉端。同时，如果采集到的视觉场景可以重新初始化，则利用激光slam的定位数据重新开始追踪，获得不间断定位结果。在视觉和激光都定位成功的情况下，采用扩展卡尔曼滤波融合定位结果。可以通过视觉slam建立八叉树地图，避免激光slam中只能针对二维平面建图的缺点，可用于避障。
[0157]
在一种实施例中，在s110之后，还包括：
[0158]
分别对第一点云与第二点云进行去噪处理。
[0159]
该步骤中，在采集第一点云以及第二点云之后，可以对激光雷达获取的第一点云进行去噪处理，根据预设的阈值剔除跳变较大的点，得到有效的第一点云；另外，可以采用滤波算法对双目图像进行去噪处理，对采集到的图像进行特征提取和特征匹配，对匹配的特征点集进行ransac算法消除不匹配的特征点，用最小二乘法重建空间点集的三维坐标，得到第二点云。
[0160]
另外，本发明实施例使用具有开放式交互接口的两轮差速驱动的机器人底盘turtlebot2，通过开放式交互接口可以获取机器人两次采样之间的相对运动；软件运行在ubuntu16.04操作系统，并使用对应的机器人操作系统(robot operating system,ros)版本kinetic，并采用了常用的第三方库如开源计算机视觉库(open source computer vision library,opencv)、通用图优化算法(general graph optimization,g2o)、点云库(pointcloud，pcl)、octomap进行基本的数据处理和显示。
[0161]
为了让本领域技术人员对本发明实施例有更好的理解，通过以下例子对第一点云与第二点云的融合做进一步的解释：
[0162]
参考图4的点云融合流程示意图，首先通过匹配两个点云p_k和p_(k 1)的几何特征，如平均距离δ、曲率ρ和法向角φ，在这里两个点云可以是两个相邻时刻的点云。计算相邻时刻点云中各点之间的几何参数以及对应关系，结合对应关系消除点云中的跳跃点。得到各点之间的几何参数后，通过刚性变化，找到对应的匹配点，并将这些几何特征根据误差函数计算两个点集的匹配误差，如果匹配误差范围低于阈值，则匹配完毕，否则通过最小化误差函数，重新进行匹配，并计算新的刚性变换。最后实现确定目标点云输出目标点云。
[0163]
本发明实施例的技术方案，在激光雷达采集第一点云的基础上增加了双目相机采
集第二点云，由于激光雷达采集的点云具有稳定但是稀疏的特性，因此第一点云的置信度高但信息量不多，当第一点云与第二点云偏差较大时，表示双目相机采集的第二点云误差较大，此时可以只使用第一点云来构建三维地图，当第一点云与第二点云偏差较小时，表示第二点云的置信度高，而对于不同传感器采集的点云所包含的信息具有差异，置信度高的第二点云可以作为稀疏的第一点云的补充，第一点云与第二点云融合得到的融合结果保持信息量丰富且置信度高，利用融合结果构建三维地图可以提高三维地图的精确度，在确保三维地图的精确度同时还能很好地适应不同的环境状况。
[0164]
实施例二
[0165]
图5为本发明实施例二提供的一种三维地图的构建装置的结构示意图。如图5所示，该装置包括：
[0166]
点云采集模块510，用于控制机器人调用激光雷达对指定的区域采集第一点云、同时调用双目相机对所述区域采集第二点云，所述双目相机为rgb-d相机；
[0167]
偏差计算模块520，用于计算所述第一点云与所述第二点云之间的偏差；
[0168]
三维地图构建模块530，用于若所述偏差大于预设的第一阈值，则根据所述第一点云对所述区域构建三维地图；若所述偏差小于或等于预设的第一阈值，则将所述第一点云与所述第二点云融合为第三点云，根据所述第三点云对所述区域构建三维地图。
[0169]
在一种实施例中，所述偏差计算模块520，包括如下子模块：
[0170]
第一中心计算子模块，用于计算所述第一点云的第一中心；
[0171]
第二中心计算子模块，用于计算所述第二点云的第二中心；
[0172]
距离计算子模块，用于计算所述第一中心与所述第二中心之间的距离，作为所述第一点云与所述第二点云之间的偏差。
[0173]
在一种实施例中，所述三维地图构建模块530中针对将所述第一点云与所述第二点云融合为第三点云，包括如下子模块：
[0174]
投影子模块，用于将所述第一点云与所述第二点云投影到同一平面上；
[0175]
刚性变换子模块，用于将所述第一点云进行刚性变换，直至所述第一点云与所述第二点云对齐；
[0176]
误差计算子模块，用于针对已对齐的所述第一点云与所述第二点云，基于所述激光雷达与所述双目相机之间的关系计算所述第一点云与所述第二点云之间的误差；
[0177]
执行子模块，用于当所述误差小于预设的第二阈值，则将所述第一点云的坐标乘以第一权重得到第一结果，将所述第二点云的坐标乘以第二权重得到第二结果，计算所述第一结果与所述第二结果的和值作为第三点云的坐标，将所述第三点云的坐标作为融合结果，其中，若所述激光雷达的精度高于所述双目相机的精度，则所述第一权重大于所述第二权重，若所述激光雷达的精度低于所述双目相机的精度，，则所述第一权重小于所述第二权重；当所述误差大于或等于预设的第二阈值，则调整所述激光雷达与所述双目相机之间的关系，返回执行所述针对已对齐的所述第一点云与所述第二点云，基于所述激光雷达与所述双目相机之间的关系计算所述第一点云与所述第二点云之间的误差。
[0178]
在一种实施例中，所述误差计算子模块，包括如下单元：
[0179]
几何特征计算单元，用于针对已对齐的所述第一点云与所述第二点云，计算所述第一点云的几何特征与所述第二点云的几何特征，所述几何特征包括平均距离δ、曲率ρ和
法向角φ；
[0180]
误差计算单元，用于通过如下公式计算所述第一点云与所述第二点云之间的误差：
[0181][0182]
其中，e为所述第一点云与所述第二点云之间的误差，nm为所述第一点云中的点云个数，nd为所述第二点云中的点云个数，pk和p
k 1
为一对成对点云，ω
i，j
是成对点云的加权系数，mi为所述第一点云中的第i个点，dj为所述第二点云中的第j个点，δr是mi与dj之间的旋转矩阵，δt是mi与dj之间的平移矢量，n为所述第一点云与所述第二点云的总点云个数，|| ||与|| ||均为范数运算符号，所述成对点云为所述第一点云与所述第二点云投影到同一平面上，指示同一地方的一对点云。
[0183]
在一种实施例中，所述第一点云所在的坐标系为激光坐标系，所述第二点云所在的坐标系为相机坐标系，所述第二点云中包含地面点云；
[0184]
所述投影子模块，包括如下单元：
[0185]
坐标转换单元，用于将在所述激光坐标系下的点云转换到所述相机坐标系下；
[0186]
平面方程构建单元，用于通过平面拟合构建包含所述相机坐标系的原点的平面方程；
[0187]
地面点云清除单元，用于从所述第二点云中识别出地面点云，并将所述地面点云从所述第二点云中清除；
[0188]
投影单元，用于将转换坐标系后的所述第一点云以及清除地面点云后的所述第二点云投影到所述平面方程所对应的平面上。
[0189]
在一种实施例中，在三维地图构建模块530中针对所述根据所述第三点云对所述区域构建三维地图，包括如下子模块：
[0190]
运动结果估计子模块，用于对所述激光雷达进行运动估计得到第一运动结果，对所述双目相机进行运动估计得到第二运动结果；
[0191]
位姿估计子模块，用于对所述激光雷达进行位姿估计得到第一位姿，对所述双目相机进行位姿估计得到第二位姿；
[0192]
第一目标位姿确定子模块，用于根据所述第一位姿以及所述第一运动结果确定第一目标位姿；
[0193]
第二目标位姿确定子模块，用于根据所述第二位姿以及所述第二运动结果确定第二目标位姿；
[0194]
融合位姿确定子模块，用于利用卡尔曼滤波器对所述第一目标位姿与所述第二目标位姿进行融合，得到融合位姿；
[0195]
三维地图构建子模块，用于基于所述融合位姿，构建所述区域的三维地图。
[0196]
在一种实施例中，还包括如下模块：
[0197]
去噪模块，用于分别对所述第一点云与所述第二点云进行去噪处理。
[0198]
本发明实施例所提供的三维地图的构建装置可执行本发明实施例一所提供的三维地图的构建方法，具备执行方法相应的功能模块和有益效果。
[0199]
实施例三
[0200]
图6示出了可以用来实施本发明的实施例的电子设备10的结构示意图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备(如头盔、眼镜、手表等)和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本发明的实现。
[0201]
如图6所示，电子设备10包括至少一个处理器11，以及与至少一个处理器11通信连接的存储器，如只读存储器(rom)12、随机访问存储器(ram)13等，其中，存储器存储有可被至少一个处理器执行的计算机程序，处理器11可以根据存储在只读存储器(rom)12中的计算机程序或者从存储单元18加载到随机访问存储器(ram)13中的计算机程序，来执行各种适当的动作和处理。在ram 13中，还可存储电子设备10操作所需的各种程序和数据。处理器11、rom 12以及ram 13通过总线14彼此相连。输入/输出(i/o)接口15也连接至总线14。
[0202]
电子设备10中的多个部件连接至i/o接口15，包括：输入单元16，例如键盘、鼠标等；输出单元17，例如各种类型的显示器、扬声器等；存储单元18，例如磁盘、光盘等；以及通信单元19，例如网卡、调制解调器、无线通信收发机等。通信单元19允许电子设备10通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
[0203]
处理器11可以是各种具有处理和计算能力的通用和/或专用处理组件。处理器11的一些示例包括但不限于中央处理单元(cpu)、图形处理单元(gpu)、各种专用的人工智能(ai)计算芯片、各种运行机器学习模型算法的处理器、数字信号处理器(dsp)、以及任何适当的处理器、控制器、微控制器等。处理器11执行上文所描述的各个方法和处理，例如三维地图的构建方法。
[0204]
在一些实施例中，三维地图的构建方法可被实现为计算机程序，其被有形地包含于计算机可读存储介质，例如存储单元18。在一些实施例中，计算机程序的部分或者全部可以经由rom12和/或通信单元19而被载入和/或安装到电子设备10上。当计算机程序加载到ram 13并由处理器11执行时，可以执行上文描述的三维地图的构建方法的一个或多个步骤。备选地，在其他实施例中，处理器11可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行三维地图的构建方法。
[0205]
本文中以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、场可编程门阵列(fpga)、专用集成电路(asic)、专用标准产品(assp)、芯片上系统的系统(soc)、负载可编程逻辑设备(cpld)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
[0206]
用于实施本发明的方法的计算机程序可以采用一个或多个编程语言的任何组合
来编写。这些计算机程序可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器，使得计算机程序当由处理器执行时使流程图和/或框图中所规定的功能/操作被实施。计算机程序可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
[0207]
在本发明的上下文中，计算机可读存储介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的计算机程序。计算机可读存储介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。备选地，计算机可读存储介质可以是机器可读信号介质。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(ram)、只读存储器(rom)、可擦除可编程只读存储器(eprom或快闪存储器)、光纤、便捷式紧凑盘只读存储器(cd-rom)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
[0208]
为了提供与用户的交互，可以在电子设备上实施此处描述的系统和技术，该电子设备具有：用于向用户显示信息的显示装置(例如，crt(阴极射线管)或者lcd(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给电子设备。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
[0209]
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(lan)、广域网(wan)、区块链网络和互联网。
[0210]
计算系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，又称为云计算服务器或云主机，是云计算服务体系中的一项主机产品，以解决了传统物理主机与vps服务中，存在的管理难度大，业务扩展性弱的缺陷。
[0211]
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本发明中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本发明的技术方案所期望的结果，本文在此不进行限制。
[0212]
上述具体实施方式，并不构成对本发明保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明保护范围之内。