一种协同三维建图方法及系统与流程

1.本发明涉及协同三维建图领域，具体是涉及一种协同三维建图方法及系统。


背景技术：

2.现有技术中存在采用路标与单目相机传感器技术实现多机器人的三维平面建图的技术，但现有技术系统实时性差；
3.还存在采用路标与云架构实现单机器人的二维平面建图，但这种系统不适合大规模环境应用。


技术实现要素：

4.为了克服现有技术的不足，本发明提供了一种协同三维建图方法及系统，具体技术方案如下所示：
5.一种协同三维建图方法，包括：
6.通过云端检测视觉定位标记；
7.通过所述视觉定位标记优化无人机视觉里程计的位姿估计；
8.通过所述视觉定位标记优化无人车视觉里程计的位姿估计；
9.通过所述云端完成orb-slam框架的局部地图构建线程和闭环检测线程。
10.在一个具体的实施例中，还包括：
11.采集环境信息，采用docker作为云端容器，采用kubernetes作为容器的调度服务，采用brpc和beego作为网络构架搭建云平台，使多智能体端与所述云端通讯；
12.多智能体包括一台所述无人机和一台所述无人车，所述无人机和所述无人车构成集中式体系结构，所述无人机前方位置装备第一单目相机且所述第一单目相机的镜头朝下，所述无人车前方位置装备第二单目相机且所述第二单目相机的镜头朝前；
13.选至少2处环境点，打上所述视觉定位标记。
14.在一个具体的实施例中，还包括：
15.所述环境信息包括图像信息，对所述图像信息采用orb-slam算法提取特征点和描述子；
16.通过pnp算法求得深度，得到点云信息；
17.利用所述云平台进行地图初始化，若所述云平台上有地图，则将所述图像信息与所述云端的所述关键帧进行匹配确定初始位置，若所述云平台上没有地图，则将所述图像信息和所述地图等信息作为云平台系统地图的起始；
18.通过匹配特征点对或者重定位方法估计相机位姿；
19.建立图像特征点和局部点云地图间的关系；
20.根据所述关键帧的判断条件，提取所述关键帧上传给所述云端。
21.在一个具体的实施例中，所述“建立图像特征点和局部点云地图间的关系”具体包括：
22.当局部地图由于环境上的遮挡或纹理缺失等原理导致跟踪失败时，系统采取下列方式进行重定位：
23.在所述无人机或所述无人车上的局部地图里去重新定位和匹配参考帧；
24.通过当前帧的信息在所述云平台上进行重定位。
25.在一个具体的实施例中，所述“通过云端检测视觉定位标记”具体包括：
26.进行图像边缘检测；
27.筛选出四边形的轮廓边缘；
28.对所述四边形的轮廓边缘进行解码，识别所述视觉定位标记。
29.在一个具体的实施例中，所述“通过视觉定位标记优化无人机视觉里程计的位姿估计”具体包括：
30.定义坐标系，定义无人机装载相机坐标系pc、无人机坐标系pa、视觉定位标记坐标系pb以及世界坐标系pw，所述世界坐标系pw定义为所述无人机第一帧；
31.所述无人机装载相机坐标系pc的yoz平面与所述无人机坐标系pa的yoz平面平行，并设置所述无人机坐标系pa的原点在所述无人机中心；
32.计算出所述无人机装载相机坐标系pc到所述世界坐标系pw的关系；
33.计算出所述无人机装载相机坐标系pc与所述视觉定位标记坐标系pb的相对位姿和
34.通过所述视觉定位标记得到的相对位姿和视觉里程计得到的相对位姿，求出轨迹误差，并将所述轨迹误差平分在所述无人机的每个关键帧上，使得闭环关键帧与实际误差减小。
35.在一个具体的实施例中，所述“计算出所述无人机装载相机坐标系pc到所述世界坐标系pw的关系”具体包括：
36.所述无人机坐标系pa与所述无人机装载相机坐标系pc是平行关系，既有：
[0037][0038]
其中，pa表示所述无人机坐标系的坐标，pc表示所述无人机装载相机坐标系的坐标，为所述无人机坐标系pa与所述无人机装载相机坐标系pc之间的平移向量，表示所述相机距离所述无人机中心的距离；
[0039]
所述视觉定位标记坐标系pb与所述世界坐标系pw之间的关系满足：
[0040][0041]
其中，pw为所述世界坐标系的坐标，pb为所述视觉定位标记坐标系的坐标，为所述世界坐标系pw与所述视觉定位标记坐标系pb之间的平移向量；
[0042]
角φ、θ和ψ分别是欧拉角，设所述世界坐标系pw到所述无人机坐标系pa的旋转矩阵为所述视觉定位标记坐标系pb到所述无人机装载相机坐标系pc的旋转矩阵为则：
[0043]
[0044][0045]
上述c代表cos，s代表sin，根据上式可得所述视觉定位标记坐标系pb和所述无人机装载相机坐标系pc旋转关系包括：
[0046][0047]
而所述无人机装载相机坐标系pc到所述视觉定位标记坐标系pb的关系表示是：
[0048][0049]
其中，为所述无人机装载相机坐标系pc到所述视觉定位标记坐标系pb的旋转矩阵，为所述无人机装载相机坐标系pc到所述视觉定位标记坐标系pb的平移向量；
[0050]
则得到所述无人机装载相机坐标系pc到所述世界坐标系pw的关系包括：
[0051][0052]
其中，为所述无人机坐标系pa到所述世界坐标系pw的旋转矩阵，为所述无人机坐标系pa到所述世界坐标系pw的平移向量，为所述无人机坐标系pa到所述无人机装载相机坐标系pc的平移向量。
[0053]
在一个具体的实施例中，所述“计算出所述无人机装载相机坐标系pc与所述视觉定位标记坐标系pb的相对位姿和”具体包括：
[0054]
使用相机模型将所述视觉定位标记投影到相机的2d像素平面，得到：
[0055][0056]
其中m代表相机内参矩阵，[u,v,1]代表所述视觉定位标记投影到归一化平面的坐标，[xb,yb,zb]代表视觉定位标记在所述视觉定位标记坐标系pb中的坐标，代表所述视觉定位标记坐标系pb到所述无人机装载相机坐标系pc的平移向量，代表所述视觉定位标记坐标系pb到所述无人机装载相机坐标系pc的旋转矩阵，s＝1/zc代表未知的尺度因子，zc代表所述视觉定位标记在相机坐标系下的z轴坐标，采用直接线性变换算法计算得到和
[0057]
在一个具体的实施例中，所述“通过视觉定位标记优化无人车视觉里程计的位姿估计”具体包括：
[0058]
定义坐标系，定义无人车装载相机坐标系pc、视觉定位标记坐标系pb以及世界坐标系pw，所述世界坐标系pw定义为所述无人机第一帧，所述无人车装载相机坐标系pc与所述无人车坐标系pa的关系确定；
[0059]
得到所述无人车装载相机坐标系pc与所述世界坐标系pw相对位姿t
cw
、所述视觉定
位标记坐标系pb与所述无人车装载相机坐标系pc相对位姿t
bc
、以及所述视觉定位标记坐标系pb与所述世界坐标系pw相对位姿t
bw
；
[0060]
优化无人车位姿与点云坐标；
[0061]
定义所述视觉定位标记坐标系pb与所述无人车装载相机坐标系pc相互间的相对误差是：
[0062][0063]
构建优化目标函数：
[0064][0065]
其中：
[0066]
t
cw
∈{(r
cw
,t
cw
)|r
cw
∈so3,t
cw
∈r3}t
bc
∈{(r
bc
,t
bc
)|r
bc
∈so3,t
bc
∈r3}
[0067]
其中，so3表示三维特殊正交群，t
cw
表示从所述无人车装载相机坐标系pc到所述世界坐标系pw的平移误差，t
bc
表示从所述视觉定位标记坐标系pb到所述无人车装载相机坐标系pc的平移误差，r3表示维数为3的一组基，r
cw
表示从所述无人车装载相机坐标系pc到所述世界坐标系pw的平移误差，r
bc
表示从所述视觉定位标记坐标系pb到所述无人车装载相机坐标系pc的旋转误差；
[0068]
相机运动不止造成旋转误差r
cw
、r
bc
以及平移误差t
cw
、t
bc
，还伴随尺度上的漂移，故进行针对尺度的变换，并采用sim3变换算法，因此：
[0069]scw
＝(r
cw
,t
cw
,s＝1),(r
cw
,t
cw
)＝t
cw
[0070]sbc
＝(r
bc
,t
bc
,s＝1),(r
bc
,t
bc
)＝t
bc
[0071]
其中，s
cw
代表视觉定位标记点从所述世界坐标系pw到所述无人车装载相机坐标系pc的相似变换，s
bc
代表所述视觉定位标记点从所述视觉定位标记坐标系pb到所述无人车装载相机坐标系pc的相似变换，s表示未知到尺度因子；
[0072]
假设优化后的sim3姿态为那么纠正完成的姿态是：
[0073][0074]
其中，r
bw
表示所述视觉定位标记点从所述世界坐标系pw到所述视觉定位标记坐标系pb的旋转矩阵，t
bw
表示所述视觉定位标记点从所述世界坐标系pw到所述视觉定位标记坐标系pb的平移，s表示未知到尺度因子，代表优化后的旋转矩阵、平移向量和尺度因子，代表优化后的相似变换；
[0075]
设定无人车在优化发生前的3d位置为则可以得到变换后的坐标：
[0076][0077]
其中代表所述无人车优化后的位姿。
[0078]
一种协同三维建图系统，用于实现上述所述的协同三维建图方法，包括：
[0079]
环境准备模块，用于采集环境信息；
[0080]
信息处理模块，用于从获取的所述环境信息中，采用orb-slam算法框架中的
tracking线程设计思想，提取关键帧；
[0081]
检测模块，用于通过云端检测视觉定位标记；
[0082]
第一优化模块，用于通过所述视觉定位标记优化无人机视觉里程计的位姿估计；
[0083]
第二优化模块，用于通过所述视觉定位标记优化无人车视觉里程计的位姿估计；执行模块，用于通过所述云端完成orb-slam框架的局部地图构建线程和闭环检测线程。
[0084]
相对于现有技术，本发明具有以下有益效果：
[0085]
本发明提供的一种协同三维建图方法及系统，能够解决协同slam系统实时性难以满足以及解决协同slam系统定位不准的问题，能够实现鲁棒性好、精度高和实时性强的协同三维建图系统。
[0086]
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
[0087]
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
[0088]
图1是实施例中相机的成像模型示意图；
[0089]
图2是实施例中协同三维建图方法的流程步骤图；
[0090]
图3是实施例中协同三维建图系统的模块图。
具体实施方式
[0091]
实施例
[0092]
如图1-图2所示，本实施例提供了一种协同三维建图方法，包括：
[0093]
环境准备，采集环境信息；
[0094]
信息处理，从获取的环境信息中，采用orb-slam算法框架中的tracking线程设计思想，提取关键帧；
[0095]
通过云端检测视觉定位标记，视觉定位标记即路标；
[0096]
通过视觉定位标记优化无人机视觉里程计的位姿估计；
[0097]
通过视觉定位标记优化无人车视觉里程计的位姿估计；
[0098]
通过云端完成orb-slam框架的局部地图构建线程和闭环检测线程。
[0099]
具体地，云端执行orb-slam中的局部地图构建线程(local mapping线程)和闭环检测线程(loop closing线程)。协同slam(cooperative simultaneous localization and mapping，cslam)在容错性、鲁棒性和执行效率上比单机器人更有优势，在未知环境下的灾难救援、资源探测和空间探测等任务中具有重要影响力。cslam系统中的数据计算存储量大，大多数机器人个体无法满足实时性要求。cslam系统通常在大规模环境中执行任务，大量的计算所累计的系统误差(位姿估计误差等)在一定程度上不能完全消除。况且当环境中存在大量重复性地貌时，特征点匹配或重叠区域匹配算法可能出现一定程度的误匹配。累计的系统误差和误匹配都会影响cslam系统建图精度，因此在环境中布置少量路标，使机器
人能够根据路标优化自身位姿，对建图精度提高具有重要意义。三维地图与二维地图相比，信息量更丰富，更能反映真实世界的客观存在形式。
[0100]
具体地，视觉定位标记技术，即路标技术能够辅助相机激光雷达传感器实现更精准的定位与建图，云架构技术能够将多机器人slam技术中的复杂运算转移到云端实现，解决多机器人计算存储资源有限的问题，三维平面的地图环境信息更丰富，更有利于无人机实现导航避障等功能。
[0101]
优选地，本实施例在大规模未知环境中选择较为宽敞的地方打上路标(apriltag码)，无人机和无人车装载上单目相机，多智能体行进过程中利用单目相机实时采集环境信息，利用orb-slam框架进行协同三维建图，并利用apriltag码对orb-slam位姿估计进行优化，利用docker+kubernetes+brpc+beego技术搭建云平台，将计算量大、存储要求高的任务部署在云端，多智能体端用于跟踪和重定位。
[0102]
优选地，本实施例结合路标apriltag+云架构+多机器人+slam三维建图技术，实现无人协同三维建图，能够解决协同slam系统实时性难以满足以及解决协同slam系统定位不准的问题，能够实现鲁棒性好、精度高和实时性强的无人协同三维建图系统。
[0103]
本实施例中，“采集环境信息”具体包括：
[0104]
采用docker+kubernetes+brpc+beego技术搭建云平台，使多智能体端与云端通讯，具体地，采用docker作为云端容器，采用kubernetes作为容器的调度服务，采用brpc和beego作为网络构架搭建云平台，使多智能体端与云端通讯；
[0105]
多智能体包括一台无人机和一台无人车，无人机和无人车构成集中式体系结构；
[0106]
选至少2处环境点，打上视觉定位标记，即打上apriltag码。
[0107]
本实施例中，“无人机和无人车构成集中式体系结构”具体包括：
[0108]
无人机前方位置装备第一单目相机且第一单目相机的镜头朝下，无人车前方位置装备第二单目相机且第二单目相机的镜头朝前。
[0109]
本实施例中，“信息处理”具体包括：
[0110]
环境信息包括图像信息，对图像信息采用orb-slam算法提取特征点和描述子；
[0111]
通过pnp算法求得深度，得到点云信息；
[0112]
利用云平台进行地图初始化，若云平台上有地图，则将图像信息与云端的关键帧进行匹配确定初始位置，若云平台上没有地图，则将图像信息和地图等信息作为云平台系统地图的起始；
[0113]
通过匹配特征点对或者重定位方法估计相机位姿；
[0114]
建立图像特征点和局部点云地图间的关系；
[0115]
根据关键帧的判断条件，提取关键帧上传给云端。
[0116]
本实施例中，“建立图像特征点和局部点云地图间的关系”具体包括：
[0117]
当局部地图由于环境上的遮挡或纹理缺失等原理导致跟踪失败时，系统采取下列方式进行重定位：
[0118]
在无人机或无人车上的局部地图里去重新定位和匹配参考帧；
[0119]
通过当前帧的信息在云平台上进行重定位。
[0120]
本实施例中，“通过云端检测视觉定位标记”具体包括：
[0121]
进行图像边缘检测；
[0122]
筛选出四边形的轮廓边缘；
[0123]
对四边形的轮廓边缘进行解码，识别视觉定位标记，即识别路标(apriltag)。
[0124]
本实施例中，“通过视觉定位标记优化无人机视觉里程计的位姿估计”具体包括：
[0125]
定义坐标系，定义无人机装载相机坐标系pc、无人机坐标系pa、视觉定位标记坐标系pb以及世界坐标系pw，世界坐标系pw定义为无人机第一帧；
[0126]
无人机装载相机坐标系pc的yoz平面与无人机坐标系pa的yoz平面平行，并设置无人机坐标系pa的原点在无人机中心；
[0127]
计算出无人机装载相机坐标系pc到世界坐标系pw的关系；
[0128]
计算出无人机装载相机坐标系pc与视觉定位标记坐标系pb的相对位姿和
[0129]
通过视觉定位标记即通过路标(apriltag)得到的相对位姿和视觉里程计得到的相对位姿，求出轨迹误差，并将轨迹误差平分在无人机的每个关键帧上，使得闭环关键帧与实际误差减小。
[0130]
本实施例中，“计算出无人机装载相机坐标系pc到世界坐标系pw的关系”具体包括：
[0131]
无人机坐标系pa与无人机装载相机坐标系pc是平行关系，既有：
[0132][0133]
其中，pa表示无人机坐标系的坐标，pc表示无人机装载相机坐标系的坐标，为无人机坐标系pa与无人机装载相机坐标系pc之间的平移向量，表示相机距离无人机中心的距离；
[0134]
视觉定位标记坐标系pb与世界坐标系pw之间的关系满足：
[0135][0136]
其中，pw为世界坐标系的坐标，pb为视觉定位标记坐标系的坐标，为世界坐标系pw与视觉定位标记坐标系pb之间的平移向量；
[0137]
角φ、θ和ψ分别是欧拉角，设世界坐标系pw到无人机坐标系pa的旋转矩阵为视觉定位标记坐标系pb到无人机装载相机坐标系pc的旋转矩阵为则：
[0138][0139][0140]
上述c代表cos，s代表sin，根据上式可得视觉定位标记坐标系pb和无人机装载相机坐标系pc旋转关系包括：
[0141][0142]
而无人机装载相机坐标系pc到视觉定位标记坐标系pb的关系表示是：
[0143]
[0144]
其中，为无人机装载相机坐标系pc到视觉定位标记坐标系pb的旋转矩阵，为无人机装载相机坐标系pc到视觉定位标记坐标系pb的平移向量；
[0145]
则得到无人机装载相机坐标系pc到世界坐标系pw的关系包括：
[0146][0147]
其中，为无人机坐标系pa到世界坐标系pw的旋转矩阵，为无人机坐标系pa到世界坐标系pw的平移向量，为无人机坐标系pa到无人机装载相机坐标系pc的平移向量。其中和未知。
[0148]
本实施例中，“计算出无人机装载相机坐标系pc与视觉定位标记坐标系pb的相对位姿和”具体包括：
[0149]
使用相机模型将视觉定位标记投影到相机的2d像素平面，得到：
[0150][0151]
其中m代表相机内参矩阵，[u,v,1]代表视觉定位标记投影到归一化平面的坐标，[xb,yb,zb]代表视觉定位标记在视觉定位标记坐标系pb中的坐标，代表视觉定位标记坐标系pb到无人机装载相机坐标系pc的平移向量，代表视觉定位标记坐标系pb到无人机装载相机坐标系pc的旋转矩阵，s＝1/zc代表未知的尺度因子，zc代表视觉定位标记在相机坐标系下的z轴坐标，采用dlt(direct linear transform,直接线性变换)算法计算得到和
[0152]
本实施例中，“通过视觉定位标记优化无人车视觉里程计的位姿估计”具体包括：
[0153]
定义坐标系，定义无人车装载相机坐标系pc、视觉定位标记坐标系pb以及世界坐标系pw，世界坐标系pw定义为无人机第一帧，无人车装载相机坐标系pc与无人车坐标系pa的关系确定；
[0154]
得到无人车装载相机坐标系pc与世界坐标系pw相对位姿t
cw
、视觉定位标记坐标系pb与无人车装载相机坐标系pc相对位姿t
bc
、以及视觉定位标记坐标系pb与世界坐标系pw相对位姿t
bw
；
[0155]
优化无人车位姿与点云坐标；
[0156]
定义视觉定位标记坐标系pb与无人车装载相机坐标系pc相互间的相对误差是：
[0157][0158]
构建优化目标函数：
[0159][0160]
其中：
[0161]
t
cw
∈{(r
cw
,t
cw
)|r
cw
∈so3,t
cw
∈r3}t
bc
∈{(r
bc
,t
bc
)|r
bc
∈so3,t
bc
∈r3}
[0162]
其中，so3表示三维特殊正交群，t
cw
表示从无人车装载相机坐标系pc到世界坐标系pw的平移误差，t
bc
表示从视觉定位标记坐标系pb到无人车装载相机坐标系pc的平移误差，r3表示维数为3的一组基，r
cw
表示从无人车装载相机坐标系pc到世界坐标系pw的平移误差，r
bc
表示从视觉定位标记坐标系pb到无人车装载相机坐标系pc的旋转误差；
[0163]
相机运动不止造成旋转误差r
cw
、r
bc
以及平移误差t
cw
、t
bc
，还伴随尺度上的漂移，故进行针对尺度的变换，并采用sim3变换算法，因此：
[0164]scw
＝(r
cw
,t
cw
,s＝1),(r
cw
,t
cw
)＝t
cw
[0165]sbc
＝(r
bc
,t
bc
,s＝1),(r
bc
,t
bc
)＝t
bc
[0166]
其中，sim3变换算法就是使用3对匹配点来进行相似变换的求解，进而解出两个坐标系之间的旋转矩阵、平移向量和尺度；s
cw
代表视觉定位标记点从世界坐标系pw到无人车装载相机坐标系pc的相似变换，s
bc
代表视觉定位标记点从视觉定位标记坐标系pb到无人车装载相机坐标系pc的相似变换，s表示未知到尺度因子；
[0167]
假设优化后的sim3姿态为那么纠正完成的姿态是：
[0168][0169]
其中，r
bw
表示视觉定位标记点从世界坐标系pw到视觉定位标记坐标系pb的旋转矩阵，t
bw
表示视觉定位标记点从世界坐标系pw到视觉定位标记坐标系pb的平移，s表示未知到尺度因子，代表优化后的旋转矩阵、平移向量和尺度因子，代表优化后的相似变换；
[0170]
设定无人车在优化发生前的3d位置为则可以得到变换后的坐标：
[0171][0172]
其中代表无人车优化后的位姿。
[0173]
如图3所示，一种协同三维建图系统，用于实现上述的协同三维建图方法，包括：
[0174]
环境准备模块，用于采集环境信息；
[0175]
信息处理模块，用于从获取的环境信息中，采用orb-slam算法框架中的tracking线程设计思想，提取关键帧；
[0176]
检测模块，用于通过云端检测视觉定位标记，即检测路标(apriltag)；
[0177]
第一优化模块，用于通过视觉定位标记优化无人机视觉里程计的位姿估计；
[0178]
第二优化模块，用于通过视觉定位标记优化无人车视觉里程计的位姿估计；
[0179]
执行模块，用于通过云端完成orb-slam框架的局部地图构建线程和闭环检测线程。
[0180]
与现有技术相比，本实施例提供的一种协同三维建图方法及系统，结合路标apriltag+云架构+多机器人+slam三维建图技术，实现无人协同三维建图，能够解决协同slam系统实时性难以满足以及解决协同slam系统定位不准的问题，能够实现鲁棒性好、精度高和实时性强的协同三维建图系统。
[0181]
本领域技术人员可以理解附图只是一个优选实施场景的示意图，附图中的模块或流程并不一定是实施本发明所必须的。
[0182]
本领域技术人员可以理解实施场景中的装置中的模块可以按照实施场景描述进
行分布于实施场景的装置中，也可以进行相应变化位于不同于本实施场景的一个或多个装置中。上述实施场景的模块可以合并为一个模块，也可以进一步拆分成多个子模块。
[0183]
上述本发明序号仅仅为了描述，不代表实施场景的优劣。
[0184]
以上公开的仅为本发明的几个具体实施场景，但是，本发明并非局限于此，任何本领域的技术人员能思之的变化都应落入本发明的保护范围。