一种多激光雷达协同的地图构建方法及其系统

1.本发明属于地图构建技术领域，更具体地，涉及一种多激光雷达协同的地图构建方法及其系统。


背景技术：

2.即时定位与地图构建(slam)技术一般指机器人或自主运动设备在未知环境下运动，利用其搭载的传感器对周围环境进行观测，得到自身的位姿和周围环境信息并构建出其周围环境的地图。随着无人驾驶以及机器人行业的高速发展，slam的应用频率越来越高，应用场景也越来越丰富。
3.当前，slam技术最常用的传感器为激光雷达，激光雷达可以获取环境的点云信息，点云信息包含激光点的三维空间坐标、反射强度和激光获取时间；传统的slam一般只使用一个激光雷达，其只能通过增加激光雷达的线数以提高点云的密度和建图精度，这样单个的激光雷达容易受到天气环境的影响或发生其他故障，造成无法建图，因此其鲁棒性不够高、建图效果并不稳定，且存在视野盲区，另外高线数的激光雷达成本较高。
4.为解决上述问题，现有技术方案中也有采用多个小型激光雷达协同建图以提高系统对环境的感知能力以及系统的稳定性，但是多个激光雷达需要进行有效的外参标定才能完成有效的数据融合，从而提高建图的精准度。然而，当前多个激光雷达的外参标定不够准确，且无法在建图过程中实时对外参进行优化。


技术实现要素：

5.本发明为克服上述现有技术中的至少一个缺陷，提供一种多激光雷达协同的地图构建方法及其系统，其可以提高多激光雷达外参标定的精准度。
6.为解决上述技术问题，本发明采用的技术方案是：
7.提供一种多激光雷达协同的地图构建方法，包括以下步骤：
8.s1：多个激光雷达分别获取同一建图区域的原始点云数据，对原始点云数据进行预处理，基于预处理后的点云数据求解得到多个激光雷达之间的外参初始值；
9.s2：对预处理后的点云数据进行特征提取，得到边缘特征、平面特征，并对平面特征进行二次提取得到点面特征g
′
；
10.s3：基于关键帧和滑动窗口，利用点面特征g
′
对预处理后的点云数据进行特征匹配，得到初始化的前端位姿；
11.s4：基于集束调整对前端位姿进行局部优化，得到局部优化位姿；对全局位姿进行优化得到全局优化位姿及全局优化地图；根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定，输出优化外参，并将优化外参返回至步骤s1中更新外参初始值；
12.s5：重复执行步骤s1至步骤s4，持续输出多个全局优化地图，根据多个全局优化地图构建slam地图。
13.本方案中先通过原始点云数据求解出外参初始值，然后再对前端位姿优化完成
后，再利用外参初始值进行优化标定，从而输出优化外参，以实现在线外参标定，输出稳定精准的外参结果，提高多激光雷达融合建图的精准度。
14.优选地，上述的步骤s1中具体包括以下步骤：
15.s11：对原始点云数据进行预处理；
16.s12：利用手眼标定法对经步骤s11预处理后的点云数据求解，得到多个激光雷达之间的外参初始值。
17.优选地，上述的步骤s2中对原始点云数据进行特征提取具体包括以下步骤：
18.s21：通过计算预处理后的点云数据内的平面光滑度，提取出边缘特征与平面特征g；
19.s22：对平面特征g进行二次平面提取，得到点面特征g
′
。
20.优选地，上述的步骤s22中对平面特征g进行二次平面提取，得到点面特征g
′
具体包括以下步骤：
21.s221：计算得到平面特征g的点云法向量，记为n；
22.s222：应用区域生长算法对点云法向量n进行计算，得到多个平面聚类的点集d，其中d＝{d1，...，dn}；
23.s223：遍历点集d中的所有元素；
24.s224：设当前平面聚类的法向量为拟合的平面方程为其中，p为原始点云，d为平面聚类中的点，t表示转置；
25.s225：求解平面方程与聚类中的每个点构建的超定方程得到n与d，其中||n||＝1；
26.s225：遍历平面聚类d中的所有元素，并代入平面方程计算得到
27.s226：判断y是否小于y’，若是，则令g
′
＝g
′
∪pi，并输出点面特征g
′
，若否则返回步骤s221。
28.优选地，上述的步骤s3中基于关键帧和滑动窗口，利用点面特征g
′
对预处理后的点云数据进行特征匹配，得到初始化的前端位姿具体包括以下步骤：
29.s31：提取预处理后的点云数据的当前帧并标记为下一帧ti+1；
30.s32：遍历预处理后的点云数据的所有关键帧；
31.s33：利用下一帧ti+1将边缘特征与平面特征gi+1转换至世界坐标系下，得到与g
′
i+1；
32.s34：利用计算边缘特征中的有效特征点pl到滑动窗口最近直线的残差redge；利用g’i+1计算平面特征中有效特征点pl到滑动窗口最近平面的残差redge；
33.s35：求解最小化目标函数t
*
i+1＝argminti+1{∑redge+∑rplane}；
34.s36：输出匹配后的关键帧t
*
i+1，即得到前端位姿。
35.优选地，上述的步骤s4具体包括以下步骤：
36.s41：利用集束调整对前端位姿进行局部优化，并利用滑动窗口进行调整，得到局部优化位姿；
37.s42：采用isam2方法，基于因子图对全局位姿进行优化，得到全局优化位姿以及全局优化地图；
38.s43：利用全局优化位姿对初始外参进行进一步优化标定，得到优化外参，若标定结果没有收敛，则保持外参初始值不变，若标定结果收敛，则将优化外参替换外参初始值。
39.优选地，上述的步骤s43具体包括以下步骤：
40.s431：获取多个激光雷达的关键帧集合；
41.s432：利用关键帧集合构建当前时刻下的局部地图；
42.s433：计算统计关键帧集合中所有特征点的残差；
43.s434：计算并统计关键帧集合中所有特征点的有效平面残差；
44.s435：计算子目标函数fi＝∑redge+∑rplane首次线性化时的特征值；
45.s436：根据特征值更新子目标函数为ft；
46.s437：根据更新后的子目标函数ft对目标函数t
*
ab＝arg mintabft进行优化，最终输出优化后的t*ab，即为优化外参。
47.优选地，上述的步骤s41中利用集束调整对前端位姿进行局部优化，并利用滑动窗口进行调整，得到局部优化位姿具体包括以下步骤：
48.s411：通过体素划分对预处理后的点云数据的特征进行结构化；
49.s412：设置滑动窗口大小为m，k＝3；
50.s413：将经步骤s411结构化的特征放入体素中；
51.s414：计算体素内特征的协方差；
52.s415：判断k是否大于m，若是，则进入步骤42，若否，则待滑动窗口容量饱满后返回步骤s413。
53.优选地，上述的步骤s42采用isam2方法，基于因子图对全局位姿进行优化，得到全局优化位姿以及全局优化地图具体包括以下步骤：
54.s421：求解滑动窗口内的最优位姿，获取关键帧集合；
55.s422：遍历滑动窗口内的位姿并更新至关键帧集合；
56.s423：构建因子图；
57.s424：将步骤422中的关键帧集合中部分关键帧边缘化并发布至因子图中，构建里程计因子以及进行回环检测；
58.s425：利用剩余的关键帧构建当前滑动窗口副本；
59.s426：在体素中边缘化关键帧对应的体素与特征，得到全局优化位姿以及全局优化地图。
60.本方案中还提供一种多激光雷达协同的地图构建系统，包括数据采集系统、前端里程计以及后端优化系统、在线外参标定系统；
61.数据采集系统用于获取同一建图区域的原始点云数据并发送至前端里程计；
62.前端里程计用于利用原始点云数据求解得到多个激光雷达之间的外参初始值，并对原始电源数据进行特征提取，得到点面特征g
′
，根据点面特征g
′
对原始点云数据进行特征匹配，得到初始化的前端位姿并输入至后端优化系统；
63.后端优化系统用于对前端位姿进行优化，得到全局优化位姿以及全局优化地图，并输出slam地图；
64.在线外参标定系统用于根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定，输出优化外参，并将优化外参返回至前端里程计中。
65.与现有技术相比，有益效果是：
66.本发明一方面通过多激光雷达协同进行地图构建，可以增强建图的鲁棒性，且避免了单个稠密激光雷达的视野盲区问题，提高了建图效果，且多个稀疏激光雷达相较于单个稠密激光雷达成本更低；另一方面，通过原始点云数据求解出外参初始值，然后再对前端位姿优化完成后，再利用外参初始值进行优化标定，从而输出优化外参，以实现在线外参标定，输出稳定精准的外参结果，提高多激光雷达融合建图的精准度。
附图说明
67.图1是本发明实施例1多激光雷达协同的地图构建方法的整体流程示意框图；
68.图2是本发明实施例2多激光雷达协同的地图构建方法的在线外参标定优化流程示意框图；
69.图3是本发明实施例3多激光雷达协同的地图构建系统的整体连接示意框图。
具体实施方式
70.附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。
71.本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”“长”“短”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
72.下面通过具体实施例，并结合附图，对本发明的技术方案作进一步的具体描述：
73.实施例1：
74.如图1所示为一种多激光雷达协同的地图构建方法的第一实施例，包括以下步骤：
75.s1：两个激光雷达分别获取同一建图区域的原始点云数据，对原始点云数据进行预处理，基于预处理后的点云数据求解得到两个激光雷达之间的外参初始值；
76.s2：对预处理后的点云数据进行特征提取，得到边缘特征、平面特征，并对平面特征进行二次提取得到点面特征g
′
；
77.s3：基于关键帧和滑动窗口，利用点面特征g
′
对预处理后的点云数据进行特征匹配，得到初始化的前端位姿；
78.s4：基于集束调整对前端位姿进行局部优化，得到局部优化位姿；对全局位姿进行优化得到全局优化位姿及全局优化地图；根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定，输出优化外参，并将优化外参返回至步骤s1中更新外参初始值；
79.s5：重复执行步骤s1至步骤s4，持续输出多个全局优化地图，根据多个全局优化地图构建slam地图。
80.本实施例中的两个激光雷达包括一个主激光雷达以及一个辅助激光雷达，这样便
于后续的计算过程中对两个激光雷达的数据进行判别。
81.本实施例中的步骤s1中具体包括以下步骤：
82.s11：对原始点云数据进行预处理；具体地，预处理包括对原始点云数据进行过滤以去除部分噪声点，以避免噪声点对特征提取等操作的影响；同时对原始点云数据进行降采样，减少后续处理过程中的计算量，从而提高地图构建的速度；
83.s12：利用手眼标定法对经步骤s11预处理后的点云数据求解，得到两个激光雷达之间的外参初始值。本实施例中采用基于欧式变换的线性插值方法对两个激光雷达进行运动对齐，得到运动对齐后的多组位姿序列及式，使得两个激光雷达的关键帧位姿上时间戳连贯。
84.本实施例中的步骤s2中对原始点云数据进行特征提取具体包括以下步骤：
85.s21：通过计算预处理后的点云数据内的平面光滑度，提取出边缘特征与平面特征g；
86.s22：对平面特征g进行二次平面提取，得到点面特征g
′
。进行二次平面提取可以挑选出底面、曲率较低的曲面或者平面等强平面特征以进行汇聚得到同样为强平面特征的特征g
′
，其记录了有效的平面约束，降低平面特征点云数量，利用其进行关键帧间匹配可以有效提高关键帧匹配的精度以及匹配效率；而对于树叶等不平整的弱平面特征，其会用于后续的滑动窗口的特征匹配。
87.本实施例中的步骤s22中对平面特征g进行二次平面提取，得到特征g
′
具体包括以下步骤：
88.s221：计算得到平面特征g的点云法向量，记为n；
89.s222：应用区域生长算法对点云法向量n进行计算，得到两个平面聚类的点集d，其中d＝{d1，...，dn}；
90.s223：遍历点集d中的所有元素；
91.s224：设当前平面聚类的法向量为拟合的平面方程为其中，p为原始点云，d为平面聚类中的点，t表示转置；
92.s225：求解平面方程与聚类中的每个点构建的超定方程得到n与d，其中||n||＝1；
93.s225：遍历平面聚类d中的所有元素，并代入平面方程计算得到s226：判断y是否小于y’，若是，则令g
′
＝g
′
upi，并输出点面特征g
′
，若否则返回步骤s221。
94.本实施例中的步骤s3中基于关键帧和滑动窗口，利用点面特征g
′
对原始点云数据进行特征匹配，得到初始化的前端位姿具体包括以下步骤：
95.s31：提取预处理后的点云数据的当前帧并标记为下一帧ti+1；
96.s32：遍历预处理后的点云数据的所有关键帧；
97.s33：利用下一帧ti+1将边缘特征i+1与平面特征gi+1转换至世界坐标系下，得到’i+1与g’i+1；
98.s34：利用’i+1计算边缘特征i+1中的有效特征点pl到滑动窗口最近直线的残差redge；利用g’i+1计算平面特征中有效特征点pl到滑动窗口最近平面的残差redge；
99.s35：求解最小化目标函数t
*
i+1＝argminti+1{∑redge+∑rplane}；
100.s36：输出匹配后的关键帧t
*
i+1，即得到前端位姿。
101.本实施例中的步骤s4具体包括以下步骤：
102.s41：利用集束调整对前端位姿进行局部优化，并利用滑动窗口进行调整，得到局部优化位姿；
103.s42：采用isam2方法，基于因子图对全局位姿进行优化，得到全局优化位姿以及全局优化地图；
104.s43：利用全局优化位姿以及局部地图对初始外参进行进一步优化标定，得到优化外参，若标定结果没有收敛，则保持外参初始值不变，若标定结果收敛，则将优化外参替换外参初始值。
105.本实施例中的步骤s43具体包括以下步骤：
106.s431：获取两个激光雷达的关键帧集合；
107.s432：利用关键帧集合构建当前时刻下的局部地图；
108.s433：计算统计关键帧集合中所有特征点的残差；
109.s434：计算并统计关键帧集合中所有特征点的有效平面残差；
110.s435：计算子目标函数fi＝∑redge+∑rplane首次线性化时的特征值；
111.s436：根据特征值更新子目标函数为ft；
112.s437：根据更新后的子目标函数ft对目标函数t
*
ab＝arg mintabft进行优化，最终输出优化后的t*ab，即为优化外参。
113.其中fi=∑redge+∑rplane，为辅助激光雷达的每一帧特征点云到局部地图的子目标函数。
114.这样是由于初始化的外参标定过程中的运动估计可能会存在漂移误差，两个激光雷达之间可能会出现刚体变化而影响采集结果。本实施例中的在线外参标定方法无需借助其他辅助传感器，直接通过多组固定的激光雷达，利用得到的局部地图对外参初值进行优化，多次迭代后得到一个可靠稳定且准确的外参标定结果，其能够精准地表示多个雷达之间的位置关系，同时，准确的外参标定结果也可以反馈至数据采集以及建图过程中，以缩小数据采集时可能产生的误差，进而提高建图的准确性。
115.本实施例中的步骤s41中利用集束调整对前端位姿进行局部优化，并利用滑动窗口进行调整，得到局部优化位姿具体包括以下步骤：
116.s411：通过体素划分对预处理后的点云数据的特征进行结构化；
117.s412：设置滑动窗口大小为m，k＝3；
118.s413：将经步骤s411结构化的特征放入体素中；
119.s414：计算体素内特征的协方差；
120.s415：判断k是否大于m，若是，则进入步骤42，若否，则待滑动窗口容量饱满后返回步骤s413。
121.本实施例中的步骤s42采用isam2方法，基于因子图对全局位姿进行优化，得到全局优化位姿以及全局优化地图具体包括以下步骤：
122.s421：求解滑动窗口内的最优位姿，获取关键帧集合；
123.s422：遍历滑动窗口内的位姿并更新至关键帧集合；
124.s423：构建因子图；
125.s424：将步骤422中的关键帧集合中部分关键帧边缘化并发布至因子图中，构建里程计因子以及进行回环检测；
126.s425：利用剩余的关键帧构建当前滑动窗口副本；
127.s426：在体素中边缘化关键帧对应的体素与特征，得到全局优化位姿以及全局优化地图。
128.需要说明的是，本实施例中激光雷达的数量为两个仅为参考，不能理解为对本方案的限定，具体实施过程中，当然可以根据实际情况增加激光雷达的数量，其建图过程与本实施例相同，这里不再详述。
129.本实施例中一方面通过多激光雷达协同进行地图构建，可以增强建图的鲁棒性，且避免了单个稠密激光雷达的视野盲区问题，提高了建图效果，且两个稀疏激光雷达相较于单个稠密激光雷达成本更低；另一方面，通过原始点云数据求解出外参初始值，然后再对前端位姿优化完成后，再利用外参初始值进行优化标定，从而输出优化外参，以实现在线外参标定，输出稳定精准的外参结果，提高多激光雷达融合建图的精准度，避免了多个激光雷达融合建图出现的外参标定结果不够准确、后续无法进行优化的问题。
130.实施例2：
131.如图2所示为一种多激光雷达协同的地图构建方法的第二实施例，本实施例与实施例1的区别仅在于，本实施例步骤s2中对原始点云数据进行特征提取还会根据原始点云数据内的平面光滑度的大小将特征点云分为强特征与弱特征，本实施例中先提取得到四种强弱特征，即强边缘特征、弱边缘特征、强平面特征与弱平面特征，然后将强边缘特征与弱边缘特征合成为边缘特征，将强平面特征与弱平面特征合成为平面特征，后续对平面特征g进行二次提取，得到特征g
′
。本实施例中的弱平面特征会加入到滑动窗口中进行特征匹配，以增加特征匹配的精准度。
132.实施例3：
133.如图3所示为用于实现上述实施例1或实施例2的一种多激光雷达协同的地图构建系统的实施例，包括数据采集系统、前端里程计以及后端优化系统、在线外参标定系统；数据采集系统与前端里程计通信电连接，前端里程计与后端优化系统通信电连接，后端优化系统与在线外参标定系统通信连接，在线外参标定系统还与数据采集系统通信连接。
134.数据采集系统用于获取同一建图区域的原始点云数据并发送至前端里程计；
135.前端里程计用于利用原始点云数据求解得到多个激光雷达之间的外参初始值，并对原始电源数据进行特征提取，得到点面特征g
′
，根据点面特征g
′
对原始点云数据进行特征匹配，得到初始化的前端位姿并输入至后端优化系统；
136.后端优化系统用于对前端位姿进行优化，得到全局优化位姿以及全局优化地图，并输出slam地图；
137.在线外参标定系统用于根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定，输出优化外参，并将优化外参返回至前端里程计中；本实施例中优化外参同时返回至数据采集系统中，以提高数据采集的精准度。
138.本发明是参照本技术实施例的方法、设备(系统)、和计算机程序产品的流程图或方框图来描述的，应理解可由计算机程序指令实现流程图或方框图中的每一流程或方框、以及流程图或方框图中的流程或方框的结合。可提供这些计算机程序指令到通用计算机、
专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
139.尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
140.显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。