[发明专利]一种多激光雷达协同的地图构建方法及其系统

权利要求书

1.一种多激光雷达协同的地图构建方法，其特征在于，包括以下步骤：

S1：多个激光雷达分别获取同一建图区域的原始点云数据，对原始点云数据进行预处理，基于预处理后的点云数据求解得到多个激光雷达之间的外参初始值；

S2：对预处理后的点云数据进行特征提取，得到边缘特征、平面特征，并对平面特征进行二次提取得到点面特征

S3：基于关键帧和滑动窗口，利用所述点面特征对预处理后的点云数据进行特征匹配，得到初始化的前端位姿；

S4：基于集束调整对前端位姿进行局部优化，得到局部优化位姿；对全局位姿进行优化得到全局优化位姿及全局优化地图；根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定，输出优化外参，并将优化外参返回至步骤S1中更新外参初始值；

S5：重复执行步骤S1至步骤S4，持续输出多个全局优化地图，根据多个全局优化地图构建SLAM地图。

2.根据权利要求1所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S1中具体包括以下步骤：

S11：对原始点云数据进行预处理；

S12：利用手眼标定法对经步骤S11预处理后的点云数据求解，得到多个激光雷达之间的外参初始值。

3.根据权利要求2所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S2中所述对原始点云数据进行特征提取具体包括以下步骤：

S21：通过计算预处理后的点云数据的平面光滑度，提取出边缘特征与平面特征

S22：对平面特征进行二次平面提取，得到点面特征

4.根据权利要求3所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S22中所述对平面特征进行二次平面提取，得到点面特征具体包括以下步骤：

S221：计算得到平面特征的点云法向量，记为N；

S222：应用区域生长算法对点云法向量N进行计算，得到多个平面聚类的点集D，其中D＝{d1,...,dn}；

S223：遍历点集D中的所有元素；

S224：设当前平面聚类的法向量为拟合的平面方程为其中，p为原始点云，d为平面聚类中的点，T表示转置；

S225：求解平面方程与聚类中的每个点构建的超定方程得到n与d，其中‖n‖＝1；

S225：遍历平面聚类D中的所有元素，并代入平面方程计算得到

S226：判断y是否小于y’，若是，则令并输出点面特征若否则返回步骤S221。

5.根据权利要求4所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S3中所述基于关键帧和滑动窗口，利用所述点面特征对预处理后的点云数据进行特征匹配，得到初始化的前端位姿具体包括以下步骤：

S31：提取预处理后的点云数据的当前帧并标记为下一帧Ti+1；

S32：遍历预处理后的点云数据的所有关键帧；

S33：利用下一帧Ti+1将边缘特征与平面特征转换至世界坐标系下，得到与

S34：利用计算边缘特征中的有效特征点pl到滑动窗口最近直线的残差redge；利用计算平面特征中有效特征点pl到滑动窗口最近平面的残差redge；

S35：求解最小化目标函数T^*i+1＝argminTi+1{∑redge+∑rplane}；

S36：输出匹配后的关键帧T^*i+1，即得到前端位姿。

6.根据权利要求1至5任一项所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S4具体包括以下步骤：

S41：利用集束调整对前端位姿进行局部优化，并利用滑动窗口进行调整，得到局部优化位姿；

S42：采用ISAM2方法，基于因子图对全局位姿进行优化，得到全局优化位姿以及全局优化地图；

S43：利用全局优化位姿对初始外参进行进一步优化标定，得到优化外参，若标定结果没有收敛，则保持外参初始值不变，若标定结果收敛，则将优化外参替换外参初始值。