地图构建方法、装置与设备与流程

1.本技术涉及机器人技术领域，尤其涉及一种地图构建方法、装置与设备。


背景技术：

2.随着社会的发展和科技的进步，机器人的功能变得多样化，这些各种各样的机器人能够在特定环境下很好地完成工作，因此越来越受到人们的欢迎。
3.大多数情况下机器人的工作环境是未知或者不确定的，机器人自主运动和定位需要借助环境地图来实现，目前机器人构建地图的方式，采用激光或者视觉进行即时定位与地图构建(simultaneous localization and mapping，slam)，这样在特征点不丰富或者场景相似的场地中容易导致slam匹配失败，从而导致构建的地图精确度较低。


技术实现要素：

4.有鉴于此，本技术提供一种地图构建方法、装置与设备，以提高机器人在特征点不丰富或者场景相似的场地中构建的地图的精确性。
5.为了实现上述目的，第一方面，本技术实施例提供一种地图构建方法，包括：
6.获取自移动设备移动过程中的初始位姿和视觉图像；
7.从所述视觉图像中提取目标关键帧；
8.根据所述初始位姿和所述视觉图像，确定局部误差方程；
9.采用所述局部误差方程对所述目标关键帧进行局部非线性优化，以得到所述自移动设备在所述目标关键帧优化后的位姿；
10.将每个所述目标关键帧优化后的位姿，加入至特征地图层，以得到目标特征地图层；
11.根据所述优化后的位姿确定全局误差方程；
12.根据所述全局误差方程对所述目标特征地图层中的位姿进行全局非线性优化，以得到全局地图。
13.作为本技术实施例一种可选的实施方式，所述根据所述初始位姿和所述视觉图像，确定局部误差方程，包括：
14.根据所述初始位姿确定所述自移动设备在所述目标关键帧的预测位姿，并根据所述预测位姿确定位姿测量误差因子；
15.提取所述目标关键帧的视觉图像特征点；
16.根据所述预测位姿和所述视觉图像特征点确定重投影误差因子；
17.根据所述自移动设备的实时位置信息确定实时动态测量误差因子；
18.根据所述视觉图像特征点和所述实时位置信息确定边缘化因子；
19.根据所述位姿测量误差因子、所述重投影误差因子、所述实时动态测量误差因子和所述边缘化因子确定所述局部误差方程。
20.作为本技术实施例一种可选的实施方式，所述根据所述预测位姿和所述视觉图像
特征点确定重投影误差因子，包括：
21.根据所述预测位姿确定所述目标关键帧的视觉图像特征点在所述特征地图层中的坐标；
22.根据所述视觉图像特征点在所述特征地图层中的坐标，确定所述视觉图像特征点在所述特征地图层中匹配的目标特征点；
23.根据所述预测位姿，将所述目标特征点转换到所述视觉图像中，以得到匹配特征点；
24.根据所述匹配特征点和所述视觉图像特征点确定重投影误差因子。
25.作为本技术实施例一种可选的实施方式，根据所述优化后的位姿确定全局误差方程包括：
26.根据每个所述目标关键帧所对应的位姿，以及每个所述目标关键帧所对应的位姿的逆，确定所述目标关键帧优化后的位姿的相对位姿约束因子；
27.根据每个所述目标关键帧所对应的位姿的逆以及每个所述目标关键帧各自对应的闭环检测位姿，得到闭环检测位姿约束因子；
28.根据每个所述目标关键帧所对应的位姿的逆，以及每个所述目标关键帧各自对应的实时动态测量位置，确定实时动态测量位置约束因子；
29.根据所述相对位姿约束因子、所述闭环检测位姿约束因子以及所述实时动态测量位置约束因子，确定全局误差方程。
30.作为本技术实施例一种可选的实施方式，所述全局地图还包括目标轨迹层和边界层中的至少一个，所述目标轨迹层用于确定所述自移动设备移动过程中的作业轨迹范围，所述边界层用于区分作业范围和非作业范围以及用于在所述作业范围内为所述自移动设备规划所述作业轨迹范围。
31.作为本技术实施例一种可选的实施方式，所述全局地图包括所述目标轨迹层时，所述方法还包括：
32.获取初始轨迹层；
33.将每个所述目标关键帧优化后的位姿加入至所述初始轨迹层，以得到所述目标轨迹层。
34.作为本技术实施例一种可选的实施方式，视觉图像包括彩色图像和所述彩色图像对应的深度图像，所述全局地图包括边界层时，所述方法还包括：获取所述彩色图像中的边界信息，以及从所述彩色图像对应的深度图像中确定所述边界信息所对应的深度值；其中，所述边界信息用于确定所述作业范围和非作业范围；
35.根据所述边界信息以及所述边界信息所对应的深度值，获取所述边界信息在所述自移动设备的第一坐标系下的坐标；
36.根据获取到的所述自移动设备移动过程中的位姿，以及所述边界信息在所述第一坐标系下的坐标，确定所述边界信息在第二坐标系下的坐标，以使所述边界信息在所述第二坐标系下的坐标作为所述边界层的坐标信息。
37.作为本技术实施例一种可选的实施方式，在所述获取自移动设备移动过程中的初始位姿和视觉图像之前，所述方法还包括：
38.控制自移动设备从初始位置沿预设方向移动预设距离后停止；
39.将所述自移动设备停止时的翻滚角、俯仰角和位置信息作为所述自移动设备的初始位姿；
40.将所述初始位姿变换到所述自移动设备停止后采集到的初始视觉图像中的特征点所在的第二坐标系下，以使所述初始位姿与所述第二坐标系下的坐标一一对应；
41.根据所述初始视觉图像中的特征点在所述第二坐标系下的坐标，得到初始的特征地图层。
42.第二方面，本技术实施例提供一种地图构建装置，包括：
43.获取模块：用于获取自移动设备移动过程中的初始位姿和视觉图像；
44.提取模块：用于从所述视觉图像中获取若干目标关键帧；
45.局部误差确定模块：用于根据所述初始位姿和所述视觉图像，确定局部误差方程；
46.局部优化模块：用于采用所述局部误差方程对所述目标关键帧进行局部非线性优化，以得到所述自移动设备在所述目标关键帧优化后的位姿；
47.添加模块：用于将每个所述目标关键帧优化后的位姿，加入至特征地图层，以得到目标特征地图层；
48.全局误差确定模块：用于根据所述优化后的位姿确定全局误差方程；
49.全局优化模块：用于根据所述全局误差方程对所述目标特征地图层中的位姿进行全局非线性优化，以得到目标全局地图。
50.第三方面，本技术实施例提供一种电子设备，包括：存储器和处理器，所述存储器用于存储计算机程序；所述处理器用于在调用所述计算机程序时执行上述第一方面或第一方面的任一实施方式所述的方法。
51.第四方面，本技术实施例提供一种可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述第一方面或第一方面的任一实施方式所述的方法。
52.本技术实施例提供的地图构建方案，首先获取自移动设备移动过程中的初始位姿和视觉图像，以及从视觉图像中获取若干目标关键帧，根据初始位姿和视觉图像，确定局部误差方程，采用局部误差方程对目标关键帧进行局部非线性优化，以得到自移动设备在目标关键帧优化后的位姿，进一步将每个目标关键帧优化后的位姿，加入至特征地图层，以得到目标特征地图层，最后根据优化后的位姿确定全局误差方程，根据全局误差方程对目标特征地图层中的位姿进行全局非线性优化，以得到目标全局地图。本技术实施例提供的地图构建方案，结合视觉图像和位姿数据生成的局部误差方程和全局误差方程，实现了自移动设备位姿的双重优化，不仅在自移动设备移动过程中减少了视觉图像关键帧位姿的局部误差，而且对整个目标特征地图层中的位姿进行全局的误差消减，从而提高了地图构建的精确度，使得自移动设备在特征点不丰富、场景相似、实时动态测量仪信号差和轮子打滑等环境使部分传感器获取的数据较差的场景下，依然可以构建出精确度较高的地图。
附图说明
53.图1为本技术实施例提供的地图构建方法的流程示意图；
54.图2为本技术实施例提供的局部误差方程确定过程的流程示意图；
55.图3为本技术实施例提供的重投影误差因子确定过程的流程示意图；
56.图4为本技术实施例提供的全局误差方程确定过程的流程示意图；
57.图5为本技术实施例提供的地图构建方法构建的全局地图的示意图；
58.图6为本技术实施例提供的目标轨迹层生成过程的流程示意图；
59.图7为本技术实施例提供的边界层生成过程的流程示意图；
60.图8为本技术实施例提供的定位初始化过程的流程示意图；
61.图9为本技术实施例提供的地图构建装置；
62.图10为本技术实施例提供的电子设备的结构示意图。
具体实施方式
63.下面结合本技术实施例中的附图对本技术实施例进行描述。本技术实施例的实施方式部分使用的术语仅用于对本技术的具体实施例进行解释，而非旨在限定本技术。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例不再赘述。
64.本技术实施例提供的地图构建方法可以由地图构建设备来实现，该地图构建设备可以是自移动设备，例如机器人，也可以为应用于机器人中的芯片或者电路，或者，该地图构建设备也可以为电子设备或者应用于电子设备中的芯片或者电路，例如，可以在电脑上采用该地图构建方法构建地图。本实施例后续以地图构建方法应用于机器人为例进行描述。在地图构建设备为电子设备时，该地图构建设备可以和机器人交互，比如，机器人可以向电子设备上报机器人的多种传感器数据等，本技术实施例对此不作限定。
65.其中，机器人可以是割草机器人、扫地机器人、排雷机器人、巡航机器人等，本实施例对此不做特别限定。
66.图1为本技术实施例提供的地图构建方法的流程示意图，如图1所示，该方法可以包括如下步骤：
67.s110、获取自移动设备移动过程中的初始位姿和视觉图像。
68.自移动设备配置多个不同类型的传感器和相机，包括但不限于轮速计、惯性传感器和实时动态测量仪(real time kinematic，rtk)。
69.其中，轮速计可以是环形轮速传感器、线性轮速传感器或者霍尔式轮速传感器等。
70.惯性传感器可以是微机电传感器(micro electro mechanical system，mems)，也可以是惯性测量传感器(inertial measurement unit，imu)等。
71.相机可以是单目相机、双目相机、rgb-d相机、事件相机等。
72.初始位姿可以包括自移动设备的轮速计采集到移动过程中的数据、惯性传感器采集到的数据和实时动态测量仪采集到的自移动设备的位置信息，自移动设备可以通过相机采集视觉范围内的视觉图像，进而获取视觉图像中的特征点。
73.本实施例后续以轮速计为环形轮速传感器、惯性传感器为imu、相机为rgb-d相机为例，进行示例性说明。
74.可以理解的是，初始位姿还可以包括其他传感器的数据，本技术实施例对此不作限定。
75.s120、从视觉图像中获取若干目标关键帧。
76.具体地，自移动设备可以从采集的多帧视觉图像中除第一帧视觉图像外的视觉图像中确定一个或多个目标视觉图像，其中，目标视觉图像与目标视觉图像的前一帧视觉图
像之间的视差大于预设角度。
77.自移动设备可以将第一帧视觉图像以及一个或多个目标视觉图像作为目标关键帧，具体可以根据实际情况来设置视觉图像帧数来作为目标关键帧，此处不做限定。
78.自移动设备可以将提取到的目标关键帧加入滑动窗口中，以方便后续对目标关键帧进行局部非线性优化。
79.自移动设备还可以将提取到的目标关键帧存储到视觉字典数据库中，以方便后续闭环检测。
80.s130、根据初始位姿和视觉图像，确定局部误差方程。
81.图2为本技术实施例提供的局部误差方程确定过程的流程示意图，如图2所示，该过程可以包括如下步骤：
82.s131、根据初始位姿获取自移动设备在目标关键帧的预测位姿，并根据预测位姿确定位姿测量误差因子。
83.预测位姿可以包括轮速计的预测结果和imu的预测结果，对应的，位姿测量误差因子可以包括轮速计误差因子和imu的误差因子。
84.目标关键帧可以是相机采集的视觉图像中的第一帧视觉图像以及与前一帧视觉图像的视差大于预设阈值的视觉图像。
85.具体地，自移动设备可以对采集到的每一帧视觉图像对应的轮速计数据和imu数据进行预积分，以获取自移动设备在目标关键帧的轮速计的预测结果和imu的预测结果。
86.自移动设备可以根据如下公式(1)对轮速计采集的数据进行预积分：
[0087][0088]
其中，表示第j个目标关键帧中，自移动设备在世界坐标系中的坐标，表示第i个目标关键帧中，自移动设备在世界坐标系中的坐标，表示第i个目标关键帧，自移动设备在世界坐标系下的旋转姿态参数，q
bo
表示imu坐标系到轮速计坐标系的旋转外参，v
oi
表示第i个目标关键帧轮速计的速度，δt表示第i个目标关键帧与第j个目标关键帧之间的时间间隔，其中，i≥1，j》i。
[0089]
自移动设备可以根据如下公式(2)对imu采集的数据进行预积分：
[0090][0091]
其中，表示第j个目标关键帧中，自移动设备在世界坐标系中的坐标，表示第j个目标关键帧自移动设备的速度，表示第j个目标关键帧中，自移动设备在世界坐标系下的旋转姿态参数，表示第i个目标关键帧imu陀螺仪的偏量，表示第i个目标关键帧imu加速计的偏量，表示第i个目标关键帧中，自移动设备在世界坐标系中的坐
标，表示第i个目标关键帧自移动设备的速度，δt表示第i个目标关键帧与第j个目标关键帧之间的时间间隔，gw表示重力，表示第i个目标关键帧，自移动设备在世界坐标系下的旋转姿态参数，表示第i个目标关键帧到第j个目标关键帧imu平移的预积分结果，表示第i个目标关键帧到第j个目标关键帧imu速度的预积分结果，表示第i个目标关键帧到第j个目标关键帧imu旋转的预积分结果，表示第j个目标关键帧imu陀螺仪的偏量，表示第j个目标关键帧imu加速计的偏量。
[0092]
自移动设备在对轮速计采集的数据和imu采集的数据进行预积分后，可以分别根据轮速计的预积分结果(即轮速计的预测结果)和imu的预积分结果(即imu的预测结果)，确定轮速计和imu的误差因子。
[0093]
具体地，自移动设备可以根据如下公式(3)确定轮速计误差因子：
[0094][0095]
其中，ro(v
oi
,s)表示第i个目标关键帧轮速计误差因子，ro表示轮速计误差，v
oi
表示第i个目标关键帧轮速计的速度，s表示边缘化对应的一个视觉图像特征点的状态量即表示滑动窗口中的优化变化量，表示自移动设备在第i个目标关键帧在世界坐标系下的旋转姿态参数，表示第j个目标关键帧中，自移动设备在世界坐标系中的坐标，表示第i个目标关键帧中，自移动设备在世界坐标系中的坐标，p
bo
表示自移动设备在imu坐标系到轮速计坐标系之间的外参的平移，表示第j个目标关键帧中，自移动设备在世界坐标系下的旋转姿态参数，表示第i个目标关键帧和第j个目标关键帧之间的轮速计预积分结果，∑o表示轮速计的信息矩阵。
[0096]
自移动设备可以根据如下公式(4)确定imu误差因子：
[0097][0098]
其中，bj表示第j个目标关键帧，bi表示第i个目标关键帧，s表示边缘化对应的一个视觉图像特征点的状态量即滑动窗口中的优化变化量，r
p
、rq、rv、r
ba
、r
bg
分别表示imu的平移误差、旋转误差、速度误差、陀螺仪误差和加速计误差，表示第i个目标关键帧自移动设备姿态的逆，(x，y，z)为目标关键帧中自移动设备对应的相机坐标。
[0099]
s132、提取目标关键帧的视觉图像特征点，根据预测位姿和视觉图像特征点确定重投影误差因子。
[0100]
图3为本技术实施例提供的重投影误差因子确定过程的流程示意图，如图3所示，
该过程可以包括以下步骤：
[0101]
s1321、根据预测位姿确定目标关键帧的视觉图像特征点在特征地图层中的坐标。
[0102]
视觉图像特征点为视觉图像中具有代表性的点，例如，可以是一帧图像中的树木、人物或者物体的轮廓上某个点，自移动设备可以获取视觉图像中这些点对应的像素值。
[0103]
视觉图像特征点可以是一个也可以是多个，例如一帧视觉图像中有视觉图像特征点a和视觉图像特征点b，以视觉图像中的视觉图像特征点a为例，自移动设备可以根据该视觉图像对应的轮速计的预测结果和惯性传感器的预测结果，确定该视觉图像的预测位姿，根据预测位姿确定视觉图像特征点a在特征地图层中的坐标。
[0104]
s1322、根据视觉图像特征点在特征地图层中的坐标，确定视觉图像特征点在特征地图层中匹配的目标特征点。
[0105]
具体地，自移动设备可以根据视觉图像中的视觉图像特征点a在特征地图层中的坐标，在特征地图层中找到与视觉图像特征点a代表同一实际位置且对应的像素值相同的目标特征点a1。
[0106]
s1323、根据预测位姿，将目标特征点转换到视觉图像中，以得到匹配特征点。
[0107]
具体地，自移动设备可以根据预测位姿确定目标特征点a1在视觉图像中的坐标，并将a1投影到视觉图像中，得到视觉图像特征点a的匹配特征点a'。
[0108]
s1324、根据匹配特征点和视觉图像特征点确定重投影误差因子。
[0109]
具体地，自移动设备可以根据视觉图像中的视觉图像特征点a和a的匹配特征点a'确定重投影误差因子，自移动设备可以根据如下公式(5)和公式(6)确定重投影误差因子：
[0110][0111][0112]
其中，为第j个目标关键帧中一个特征点在相机坐标系下的坐标，t
bc
表示相机到imu坐标系的外参，表示第i个目标关键帧的位姿，表示第j个目标关键帧的位姿，λ表示该特征点的逆深度，目标关键帧的位姿，λ表示该特征点的逆深度，表示该特征点在像素坐标系下的坐标，rc表示重投影误差因子。
[0113]
s133、根据自移动设备的实时位置信息确定实时动态测量误差因子。
[0114]
自移动设备可以根据rtk采集的自身当前的位置信息和当前视觉图像中自移动设备自身的位置信息确定rtk误差因子(也就是实时动态测量误差因子)，其中，实时位置信息包括自移动设备在rtk基站坐标系下的坐标，以及自移动设备的偏航角。
[0115]
具体地，自移动设备可以根据如下公式(7)确定rtk误差因子：
[0116]
error＝(x
1-x2)*(x
1-x2)+(y
1-y2)*(y
1-y2)+|(θ
1-θ2)|........公式(7)
[0117]
其中，error表示rtk误差因子，(x1，y1)表示rtk采集的自移动设备在rtk基站坐标系下的坐标，(x2，y2)表示当前视觉图像中自移动设备的坐标，θ1表示rtk流动站(也就是自移动设备)当前的航向角，θ2表示当前自移动设备在视觉图像中的航向角。
[0118]
为了使建立出的地图无方向上的角度漂移，可以使用单天线rtk来采集自移动设备的位置信息，当rtk为单天线时，由于rtk无法反馈rtk流动站的航向角，自移动设备可以通过帧间差分的方式计算出rtk流动站当前的航向角。
[0119]
具体地，当移动机器人在检测到自身沿直线移动时，可以根据相邻两帧视觉图像中自移动设备的位置坐标(rtk_x1，rtk_y1)(rtk_x2，rtk_y2)，确定rtk流动站当前的航向角，其中，rtk流动站当前的航向角可以根据如下公式(8)确定：
[0120]
θ1＝atan2(rtk_y
2-rtk_y1，rtk_x
2-rtk_x1).............公式(8)
[0121]
其中，当自移动设备当前的角速度小于设定的角速度时，则自移动设备为直线移动，否则，自移动设备为非直线移动。
[0122]
s134、根据视觉图像特征点和实时位置信息确定边缘化因子。
[0123]
自移动设备可以根据视觉图像特征点和自移动设备自身的实时位置信息确定边缘化因子，具体地，自移动设备可以根据如下公式(9)确定边缘化因子：
[0124]
er＝||r
p-j
p
s||2.........................................公式(9)
[0125]
其中，er表示边缘化因子，j
p
表示边缘化对应的信息矩阵，该信息矩阵包括自移动设备的实时位置信息，s表示边缘化对应的一个视觉图像特征点的状态量即表示滑动窗口中的优化变化量。
[0126]
s135、根据位姿测量误差因子、重投影误差因子、实时动态测量误差因子和边缘化因子确定局部误差方程。
[0127]
具体地，自移动设备可以根据如下公式(10)构建局部误差方程：
[0128][0129]
其中，||r
p-j
p
s||2表示边缘化因子，表示imu的误差因子(即位姿测量误差因子)，表示重投影误差因子，ρ表示核函数，εc表示视觉图像像素投影函数，表示视觉图像的像素点，表示rtk误差因子(即实时动态测量误差因子)，(x
curi
，y
curi
，θ
curi
)表示第i个目标关键帧中自移动设备的坐标，(x
rtki
，y
rtki
，θ
rtki
)表示相机采集第i个目标关键帧时rtk采集的自移动设备的坐标。
[0130]
可以理解的是，当存在闭环检测位姿约束因子时，自移动设备也可以根据如下公式(11)构建局部误差方程：
[0131][0132]
其中，||t
cur-1
·
t
close
||2表示闭环检测位姿约束因子，t
cur
表示自移动设备当前位置，t
close
表示闭环检测到的位置。
[0133]
进一步，当自移动设备在特征点不丰富、场景相似、rtk信号差和轮子打滑等部分传感器获取的数据较差的场景下，依然可以通过其他传感器获取的正常数据确定的误差因子，构建局部误差方程。
[0134]
s140、采用局部误差方程对目标关键帧进行局部非线性优化，以得到目标关键帧优化后的位姿。
[0135]
具体地，滑动窗口中可以维护n个目标关键帧，自移动设备可以根据局部误差方程对滑动窗口中维护的每个目标关键帧的位姿进行非线性优化，得到目标关键帧优化后的位姿，n为大于1的整数。
[0136]
目标关键帧中的位姿可以包括以下变量：
[0137]
s＝[x1,
…
,xn,x
bc
,x
bo
,λ1,
…
,λm]，m∈[0,z]
[0138][0139]
x
bc
＝[p
bc
,q
bc
]
[0140]
x
bo
＝[p
bo
,q
bo
]
[0141]
其中，s表示滑动窗口中的优化变化量也可以表示为滑动窗口目标关键帧优化后的位姿，(x1,
…
,xn)和(λ1,
…
,λm)表示滑动窗口目标关键帧的位姿中可以优化的状态变量，x
bc
表示imu坐标系到相机坐标系的外参，x
bo
表示imu坐标系到轮速计坐标系的外参，p
bc
表示imu坐标系到相机坐标系的平移外参，q
bc
表示imu坐标系到相机坐标系的旋转外参，p
bo
表示imu坐标系到轮速计坐标系的平移外参，q
bo
表示imu坐标系到轮速计坐标系的旋转外参,xk表示第k个目标关键帧中可以优化的状态变量，表示第k个目标关键帧自移动设备的位置，表示第k个目标关键帧自移动设备的速度，表示第k个目标关键帧自移动设备的位姿，表示第k个目标关键帧imu加速计的偏量，表示第k个目标关键帧imu陀螺仪的偏量。
[0142]
s150、将每个目标关键帧优化后的位姿，加入至特征地图层，以得到目标特征地图层。
[0143]
其中，对创建好的特征地图层进行初始化，即将初始位姿加入到特征地图层。初始化之后，自移动设备可以将滑动窗口中的优化后的目标关键帧的位姿，加入到特征地图层中与目标关键帧对应的位置，以得到目标特征地图层，目标特征地图层中包括每个目标关键帧优化后的位姿以及每个目标关键帧优化后的特征点。
[0144]
s160、根据优化后的位姿确定全局误差方程。
[0145]
图4为本技术实施例提供的全局误差方程确定过程的流程示意图，如图4所示，该
过程可以包括如下步骤：
[0146]
s161、根据每个目标关键帧所对应的位姿，以及每个目标关键帧所对应的位姿的逆，确定目标关键帧优化后的位姿的相对位姿约束因子。
[0147]
自移动设备可以将目标关键帧中的n个目标关键帧加入位姿图中，其中，在n个目标关键帧中，每相邻的两个目标关键帧之间的平移差大于设定距离或者角度差大于设定角度。
[0148]
自移动设备可以根据位姿图中每个目标关键帧所对应的优化后的位姿，以及位姿图中每个目标关键帧所对应的优化后的位姿的逆，确定位姿图中的目标关键帧优化后的位姿的相对位姿约束因子。
[0149]
具体地，自移动设备可以根据如下公式(12)确定相对位姿约束因子：
[0150]
error1＝t1.inverse()*t2+t2.inverse()*t3+.....+ti.inverse()*t
i+1
[0151]
+t
n-1
.inverse()*tn.......公式(12)
[0152]
其中，error1表示相对位姿约束因子，ti表示在n个目标关键帧中，第i个目标关键帧对应的优化后的位姿，ti.inverse()表示第i个目标关键帧对应的优化后的位姿的逆。
[0153]
s162、根据每个目标关键帧所对应的位姿的逆以及每个目标关键帧各自对应的闭环检测位姿，得到闭环检测位姿约束因子。
[0154]
自移动设备可以检测位姿图中的每一个目标关键帧是否需要进行闭环检测，例如可以每隔设定值，对位姿图中的目标关键帧进行闭环检测，即当自移动设备检测到当前的目标关键帧与上一个进行闭环检测的目标关键帧之间的间隔数为设定值，则对当前目标关键帧进行闭环检测，否则，对当前目标关键帧不进行闭环检测。
[0155]
在自移动设备确定位姿图中当前的目标关键帧需要进行闭环检测时，可以根据当前目标关键帧中提取的特征点，在视觉字典数据库中搜索相似的目标关键帧，如果没有搜索到，则继续检测下一个目标关键帧是否需要闭环检测；如果搜索到，则将这些相似的目标关键帧组合成局部特征地图。
[0156]
自移动设备可以根据当前目标关键帧中的特征点和局部地图中的特征点利用pnp(perspective-n-point，透视n点)算法进行匹配，得到闭环检测位姿。
[0157]
自移动设备可以根据位姿图中每个目标关键帧所对应的优化后的位姿的逆以及每个目标关键帧各自对应的闭环检测位姿，得到闭环检测位姿约束因子。
[0158]
具体地，自移动设备可以根据如下公式(13)确定闭环检测位姿约束因子：
[0159]
error2＝ti.inverse()*ti_close....................公式(13)
[0160]
其中，error2表示闭环检测位姿约束因子，ti_close表示n个目标关键帧中第i个目标关键帧对应的闭环检测位姿。
[0161]
s163、根据每个目标关键帧所对应的位姿的逆，以及每个目标关键帧各自对应的实时动态测量位置，确定实时动态测量位置约束因子。
[0162]
自移动设备可以根据位姿图中每个目标关键帧所对应的优化后的位姿的逆，以及位姿图中每个目标关键帧各自对应的rtk采集的位置信息，确定实时动态测量位置约束因子。
[0163]
具体地，自移动设备可以根据如下公式(14)确定实时动态测量位置约束因子：
[0164]
error3＝ti.inverse()*ti_rtk...................公式(14)
[0165]
其中，error3表示实时动态测量位置约束因子，ti_rtk表示n个目标关键帧中第i个目标关键帧对应的rtk采集的位置信息。
[0166]
s164、根据相对位姿约束因子、闭环检测位姿约束因子以及实时动态测量位置约束因子，确定全局误差方程。
[0167]
具体地，自移动设备可以根据如下公式(15)确定全局误差方程：
[0168]
error＝error1+error2+error3................公式(15)。
[0169]
自移动设备还可以将闭环检测位姿约束因子加入到下一次滑动窗口中，以便下一次在滑动窗口中构建局部误差方程时，加入该闭环检测位姿约束因子。
[0170]
s170、根据全局误差方程对目标特征地图层中的位姿进行全局非线性优化，以得到目标全局地图。
[0171]
具体地，自移动设备可以根据全局误差方程，对目标特征地图层中的位姿进行全局非线性优化，得到全局地图。
[0172]
全局非线性优化可以是自移动设备根据全局误差方程，对目标特征地图层中的除第一帧关键帧之外的所有目标关键帧的位姿进行非线性优化。
[0173]
自移动设备还可以对目标特征地图层中的地图特征点进行全局非线性优化，使得整地图一致性更合理,更接近真实场景。
[0174]
本领域技术人员可以理解，以上实施例是示例性的，并非用于限定本技术。在可能的情况下，以上步骤中的一个或者几个步骤的执行顺序可以进行调整，也可以进行选择性组合，得到一个或多个其他实施例。本领域技术人员可以根据需要从上述步骤中任意进行选择组合，凡是未脱离本技术方案实质的，都落入本技术的保护范围。
[0175]
图5为本技术实施例提供的地图构建方法构建的全局地图的示意图，如图5所示，该全局地图除了包括目标特征地图层外，还可以包括目标轨迹层和边界层。
[0176]
目标轨迹层用于确定自移动设备移动过程中的作业轨迹范围，边界层用于区分作业范围和非作业范围以及用于在作业范围内为自移动设备规划作业轨迹范围。
[0177]
图6为本技术实施例提供的目标轨迹层生成过程的流程示意图，如图6所示，当全局地图还包括目标轨迹层时，该过程可以包括如下步骤：
[0178]
s210、获取初始轨迹层。
[0179]
具体地，可以新建一个空白层，将自移动设备移动前的初始位姿，投影到该空白层得到初始轨迹层。
[0180]
s220、将每个目标关键帧优化后的位姿加入至初始轨迹层，以得到目标轨迹层。
[0181]
具体地，自移动设备开始移动后，可以将每个目标关键帧优化后的位姿加入至初始轨迹层，得到目标轨迹层，目标轨迹层中包括每个目标关键帧优化后的位姿对应的坐标形成的轨迹，使得自移动设备在作业时，可以只在轨迹范围内作业。
[0182]
图7为本技术实施例提供的边界层生成过程的流程示意图，如图7所示，当全局地图还包括边界层时，该过程可以包括如下步骤：
[0183]
s310、获取彩色图像中的边界信息，以及从彩色图像对应的深度图像中确定边界信息所对应的深度值。
[0184]
具体地，可以在融合定位的同时，获取自移动设备的rgb-d相机采集到的rgb图像(即彩色图像)和深度图像。
[0185]
具体地，可以从rgb图像中提取边界信息，边界信息用于确定作业范围和非作业范围，边界信息可以包括代表自移动设备的最外层和阴影边界的可见点的集合以及遮挡物和背景的交界上的点的集合，自移动设备可以从rgb图像对应的深度图像中确定上述各个点所对应的深度值。
[0186]
s320、根据边界信息以及边界信息所对应的深度值，获取边界信息在自移动设备的第一坐标系下的坐标。
[0187]
自移动设备可以根据边界信息中的各个点所对应的深度值，获得边界信息中的各个点在自移动设备的相机坐标系(即第一坐标系)下的坐标。
[0188]
s330、根据获取到的自移动设备移动过程中的位姿，以及边界信息在第一坐标系下的坐标，确定边界信息在第二坐标系下的坐标，以使边界信息在第二坐标系下的坐标作为边界层的坐标信息。
[0189]
自移动设备可以根据获取到的自身的位姿，以及边界信息中的各个点在相机坐标系下的坐标，确定边界信息中的各个点在rtk基站坐标系(即第二坐标系)下的坐标，将这些点的坐标添加到边界层中，然后将添加到边界层中的点连接形成工作边界，自移动设备在作业时，可以只在工作边界所包含的范围内规划路线，以确保自移动设备不超过工作边界。
[0190]
在本技术的另一个实施例中，在获取自移动设备移动过程中的第一数据之前，自移动设备还可以先进行定位初始化。
[0191]
图8为本技术实施例提供的定位初始化过程的流程示意图，如图8所示，该过程可以包括如下步骤：
[0192]
s410、控制自移动设备从初始位置沿预设方向移动预设距离后停止。
[0193]
具体地，可以控制自移动设备沿直线移动设定的距离，例如0.5米。在自移动设备移动过程中，可以每间隔固定时间采集rtk位置坐标，采用最小二乘法的方式对多组rtk位置坐标进行线性拟合，得到一条直线，将该直线的方位角作为自移动设备的初始偏航角。
[0194]
s420、将自移动设备停止时的翻滚角、俯仰角和位置信息作为自移动设备的初始位姿。
[0195]
具体地，可以将自移动设备停止移动时刻，imu采集的自移动设备的姿态中的翻滚角和俯仰角作为自移动设备的初始翻滚角和俯仰角。
[0196]
将自移动设备停止移动时刻，rtk采集的坐标作为自移动设备的初始坐标，即自移动设备的第一位置信息。
[0197]
根据自移动设备初始翻滚角、俯仰角和初始坐标，确定自移动设备的初始位姿。
[0198]
s430、将初始位姿变换到自移动设备停止后采集到的初始视觉图像中的特征点所在的第二坐标系下，以使所述初始位姿与所述第二坐标系下的坐标一一对应。
[0199]
具体地，可以将自移动设备停止后采集到的视觉图像中的特征点，经过初始位姿的变换，变换为在rtk基站坐标系下的坐标。
[0200]
s440、根据初始视觉图像中的特征点在第二坐标系下的坐标，得到特征地图层。
[0201]
具体地，可以根据变换后的rtk基站坐标系下的坐标，将对应的特征点投影到特征地图层中，得到特征地图层，后续对目标关键帧中的位姿进行优化，都是基于自移动设备的初始位姿。
[0202]
本技术实施例提供的地图构建方案，首先获取自移动设备移动过程中的初始位姿
和视觉图像，根据初始位姿和视觉图像，确定局部误差方程，然后从视觉图像中提取目标关键帧，采用局部误差方程对目标关键帧进行局部非线性优化，以得到目标关键帧优化后的位姿，进一步将每个目标关键帧优化后的位姿，加入至目标特征地图层，最后根据优化后的位姿确定全局误差方程，根据全局误差方程对目标特征地图层中的位姿进行全局非线性优化，以得到全局地图。本技术实施例提供的地图构建方案，结合视觉图像和位姿数据生成的局部误差方程和全局误差方程，实现了自移动设备位姿的双重优化，不仅在自移动设备移动过程中减少了视觉图像关键帧位姿的局部误差，而且对整个目标特征地图层中的位姿进行全局的误差消减，从而提高了地图构建的精确度，使得自移动设备在特征点不丰富、场景相似、实时动态测量仪信号差和轮子打滑等环境使部分传感器获取的数据较差的场景下，依然可以构建出精确度较高的地图。
[0203]
基于同一发明构思，本技术实施例还提供了一种地图构建装置。图9为本技术实施例提供的地图构建装置的结构示意图，如图9所示，本技术实施例提供的地图构建装置可以包括：
[0204]
获取模块11：用于获取自移动设备移动过程中的初始位姿和视觉图像；
[0205]
提取模块12：用于从所述视觉图像中获取若干目标关键帧；
[0206]
局部误差确定模块13：用于根据所述初始位姿和所述视觉图像，确定局部误差方程；
[0207]
局部优化模块14：用于采用所述局部误差方程对所述目标关键帧进行局部非线性优化，以得到所述自移动设备在所述目标关键帧优化后的位姿；
[0208]
添加模块15：用于将每个所述目标关键帧优化后的位姿，加入至特征地图层，以得到目标特征地图层；
[0209]
全局误差确定模块16：用于根据所述优化后的位姿确定全局误差方程；
[0210]
全局优化模块17：用于根据所述全局误差方程对所述目标特征地图层中的位姿进行全局非线性优化，以得到全局地图。
[0211]
作为一种可选的实施方式，所述局部误差确定模块13具体用于：
[0212]
根据所述初始位姿确定所述自移动设备在所述目标关键帧的预测位姿，并根据所述预测位姿确定位姿测量误差因子；
[0213]
提取所述目标关键帧的视觉图像特征点；
[0214]
根据所述预测位姿和所述视觉图像特征点确定重投影误差因子；
[0215]
根据所述自移动设备的实时位置信息确定实时动态测量误差因子；
[0216]
根据所述特征点和所述实时位置信息确定边缘化因子；
[0217]
根据所述位姿测量误差因子、所述重投影误差因子、所述实时动态测量误差因子和所述边缘化因子确定所述局部误差方程。
[0218]
作为一种可选的实施方式，所述局部误差确定模块13还用于：
[0219]
根据所述预测位姿确定所述目标关键帧的视觉图像特征点在所述特征地图层中的坐标；
[0220]
根据所述视觉图像特征点在所述特征地图层中的坐标，确定所述视觉图像特征点在所述特征地图层中匹配的目标特征点；
[0221]
根据所述预测位姿，将所述目标特征点转换到所述视觉图像中，以得到匹配特征
点；
[0222]
根据所述匹配特征点和所述视觉图像特征点确定重投影误差因子。
[0223]
作为一种可选的实施方式，全局误差确定模块16用于：
[0224]
根据每个所述目标关键帧所对应的位姿，以及每个所述目标关键帧所对应的位姿的逆，确定所述目标关键帧优化后的位姿的相对位姿约束因子；
[0225]
根据每个所述目标关键帧所对应的位姿的逆以及每个所述目标关键帧各自对应的闭环检测位姿，得到闭环检测位姿约束因子；
[0226]
根据每个所述目标关键帧所对应的位姿的逆，以及每个所述目标关键帧各自对应的实时动态测量位置，确定实时动态测量位置约束因子；
[0227]
根据所述相对位姿约束因子、所述闭环检测位姿约束因子以及所述实时动态测量位置约束因子，确定全局误差方程。
[0228]
作为一种可选的实施方式，所述全局地图还包括目标轨迹层和边界层中的至少一个，所述目标轨迹层用于确定所述自移动设备移动过程中的作业轨迹范围，所述边界层用于区分作业范围和非作业范围以及用于在所述作业范围内为所述自移动设备规划所述作业轨迹范围。
[0229]
作为一种可选的实施方式，所述全局地图包括所述目标轨迹层时，所述添加模块15还用于：
[0230]
获取初始轨迹层；
[0231]
将每个所述目标关键帧优化后的位姿加入至所述初始轨迹层，以得到所述目标轨迹层。
[0232]
作为一种可选的实施方式，所述全局地图包括边界层时，视觉图像包括彩色图像和所述彩色图像对应的深度图像，所述地图构建装置还用于：
[0233]
获取所述彩色图像中的边界信息，以及从所述彩色图像对应的深度图像中确定所述边界信息所对应的深度值，其中，所述边界信息用于确定所述作业范围和非作业范围；
[0234]
根据所述边界信息以及所述边界信息所对应的深度值，获取所述边界信息在所述自移动设备的第一坐标系下的坐标；
[0235]
根据获取到的所述自移动设备移动过程中的位姿，以及所述边界信息在所述第一坐标系下的坐标，确定所述边界信息在第二坐标系下的坐标，以使所述边界信息在所述第二坐标系下的坐标作为所述边界层的坐标信息。
[0236]
作为一种可选的实施方式，在所述获取自移动设备移动过程中的初始位姿和视觉图像之前，所述地图构建装置还用于：
[0237]
控制自移动设备从初始位置沿预设方向移动预设距离后停止；
[0238]
将所述自移动设备停止时的翻滚角、俯仰角和位置信息作为所述自移动设备的初始位姿；
[0239]
将所述初始位姿变换到所述自移动设备停止后采集到的初始视觉图像中的特征点所在的第二坐标系下，以使所述初始位姿与所述第二坐标系下的坐标一一对应；
[0240]
根据所述初始视觉图像中的特征点在所述第二坐标系下的坐标，得到特征地图层。
[0241]
本实施例提供的地图构建装置可以执行上述方法实施例，其实现原理与技术效果
类似，此处不再赘述。
[0242]
基于同一发明构思，本技术实施例还提供了一种电子设备。图10为本技术实施例提供的电子设备的结构示意图，如图10所示，本实施例提供的电子设备包括：存储器210和处理器220，存储器210用于存储计算机程序；处理器220用于在调用计算机程序时执行上述方法实施例所述的方法。
[0243]
本实施例提供的电子设备可以执行上述方法实施例，其实现原理与技术效果类似，此处不再赘述。
[0244]
本技术实施例还提供一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述方法实施例所述的方法。
[0245]
本技术实施例还提供一种计算机程序产品，当计算机程序产品在电子设备上运行时，使得电子设备执行时实现上述方法实施例所述的方法。
[0246]
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本技术的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
[0247]
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时，全部或部分地产生按照本技术实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者通过所述计算机可读存储介质进行传输。所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线)或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质(例如软盘、硬盘或磁带)、光介质(例如dvd)、或者半导体介质(例如固态硬盘(solid state disk，ssd))等。
[0248]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，该流程可以由计算机程序来指令相关的硬件完成，该程序可存储于计算机可读取存储介质中，该程序在执行时，可包括如上述各方法实施例的流程。而前述的存储介质可以包括：rom或随机存储记忆体ram、磁碟或者光盘等各种可存储程序代码的介质。
[0249]
在本技术中出现的对步骤进行的命名或者编号，并不意味着必须按照命名或者编号所指示的时间/逻辑先后顺序执行方法流程中的步骤，已经命名或者编号的流程步骤可以根据要实现的技术目的变更执行次序，只要能达到相同或者相类似的技术效果即可。
[0250]
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
[0251]
在本技术所提供的实施例中，应该理解到，所揭露的装置/设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征点可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
[0252]
应当理解，当在本技术说明书和所附权利要求书中使用时，术语“包括”指示所描述特征点、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征点、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
[0253]
在本技术的描述中，除非另有说明，“/”表示前后关联的对象是一种“或”的关系，例如，a/b可以表示a或b；本技术中的“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，a和/或b，可以表示：单独存在a，同时存在a和b，单独存在b这三种情况，其中a，b可以是单数或者复数。
[0254]
并且，在本技术的描述中，除非另有说明，“多个”是指两个或多于两个。“以下至少一项”或其类似表达，是指的这些项中的任意组合，包括单项或复数项的任意组合。例如，a，b，或c中的至少一项，可以表示：a，b，c，a-b，a-c，b-c，或a-b-c，其中a，b，c可以是单个，也可以是多个。
[0255]
如在本技术说明书和所附权利要求书中所使用的那样，术语“如果”可以依据上下文被解释为“当...时”或“一旦”或“响应于确定”或“响应于检测到”。类似地，短语“如果确定”或“如果检测到[所描述条件或事件]”可以依据上下文被解释为意指“一旦确定”或“响应于确定”或“一旦检测到[所描述条件或事件]”或“响应于检测到[所描述条件或事件]”。
[0256]
另外，在本技术说明书和所附权利要求书的描述中，术语“第一”、“第二”、“第三”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。
[0257]
在本技术说明书中描述的参在本技术说明书中描述的参考“一个实施例”或“一些实施例”等意味着在本技术的一个或多个实施例中包括结合该实施例描述的特定特征点、结构或特点。由此，在本说明书中的不同之处出现的语句“在一个实施例中”、“在一些实施例中”、“在其他一些实施例中”、“在另外一些实施例中”等不是必然都参考相同的实施例，而是意味着“一个或多个但不是所有的实施例”，除非是以其他方式另外特别强调。
[0258]
最后应说明的是：以上各实施例仅用以说明本技术的技术方案，而非对其限制；尽管参照前述各实施例对本技术进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征点进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本技术各实施例技术方案的范围。