本发明涉及激光雷达数据处理，具体涉及一种激光雷达slam多回环检测与迭代优化方法及电子设备。
背景技术：
1、激光slam(simultaneous localization and mapping)是一种广泛使用的激光雷达数据定位与建图技术，无人机等航飞平台和无人车等行走平台可携带激光雷达，利用slam技术对地面现场态势建立三维点云模型，广泛用于地理测绘、行业巡检和应急救援等场景。slam算法利用激光雷达帧间点云匹配得到帧间的变换位姿，通过帧间位姿累加得到全局位姿，再将每帧点云根据全局位姿进行旋转与平移，得到全局坐标系下的三维点云。其中帧间匹配产生的误差，一般通过回环检测的方式，进行全局优化。当前激光雷达slam技术应用中，多以起点与终点建立一个回环检测，导致航线中间匹配三维点云误差大，且最终全局优化节点过多、计算量过大，不适用于应急和快速应用场景。
2、针对现有激光雷达slam的技术与应用局限，本发明公开一种激光雷达slam多回环检测与迭代优化技术，该技术设计无人机/车系统多回环航线和回环检测点，应用icp迭代最近点和gtsam图优化方法，选取航线中关键帧，逐个检测点匹配当前帧与关键帧，迭代优化该回环内所有关键帧位姿，并迭代输出应用优化精度的三维点云，以实现更高精度和时效的应急与快速态势感知。
技术实现思路
1、针对现有技术的局限，本发明公开了一种激光雷达slam多回环检测与迭代优化方法及电子设备，用于解决现有激光雷达slam在应急和快速态势感知场景的应用瓶颈。
2、本发明通过以下技术方案予以实现：
3、第一方面，本发明公开一种激光雷达slam多回环检测与迭代优化方法，所述方法包括以下步骤：
4、s1设计无人机/车系统多回环航线，提取存储回环检测点坐标；
5、s2无人机/车系统按s1中设计的航线行驶，提取当前帧特征点并计算全局位姿，选取关键帧并存储其点云与位姿；
6、s3无人机/车系统进入s1中提取的检测点坐标指定半径内进行回环检测，匹配选取该检测点的当前帧与关键帧；
7、s4求解s3中选取的该检测点当前帧与关键帧点云集合的相对位姿变化关系，并全局优化该回环内所有关键帧位姿及其点云；
8、s5重复s3-s4逐个检测点迭代回环检测和全局优化，迭代输出应用优化精度的三维点云。
9、更进一步的，所述s1中，无人机/车系统包括无人机/车平台、激光雷达、视频相机、控制地面站和态势地面站，无人机可搭载面阵激光雷达正射扫描监测区域，无人车可搭载机械激光雷达360°扫描监测区域；控制地面站控制无人机/车和激光雷达、视频相机按设定的航线和姿态扫描监测区域，态势地面站实时接收处理激光点云和视频图像，自动生成三维点云和实景模型，并配套基于动态视频和三维模型的纹理、温度、几何特征及空间变量的智能识别分析功能模块。
10、更进一步的，所述s1中，设计无人机/车系统多回环航线的方法为，先自近距航点对角贯穿监测区域至对角远距航点，再以弓字形航线折返回近距航点，折返航线与贯穿航线多次交汇构建多个回环和检测点。
11、更进一步的，所述s2中，提取当前帧特征点并计算全局位姿的方法为，当前帧点云数据进行slam算法处理，提取点云中的角点特征和面点特征，进行基于特征点的帧间里程计位姿估计，得到帧间变化的最优位姿估计和当前帧全局位姿。
12、更进一步的，所述s2中，选取关键帧的方法为，计算当前帧与前一关键帧的距离或时间差，选取大于设定阈值的当前帧为新一关键帧。
13、更进一步的，所述s3中，回环检测的方法为，将检测点设定半径内所有的当前帧与关键帧坐标建立kd-tree进行最近邻搜索，匹配选取该检测点的距离小于阈值且时间满足要求的当前帧与关键帧，构建检测回环。
14、更进一步的，所述s4中，采用icp(iterative closest point)迭代最近点方法，求解检测点匹配当前帧与关键帧点云集合的相对位姿变化关系，再使用gtsam(georgia techsmoothing and mapping)图优化方法，得到该回环所有关键帧全局优化后的最佳估计坐标及其点云。
15、第二方面，本发明公开一种电子设备，包括处理器以及存储有执行指令的存储器，当所述处理器执行所述存储器存储的所述执行指令时，所述处理器执行第一方面所述的激光雷达slam多回环检测与迭代优化方法。
16、本发明的有益效果为：
17、本发明设计无人机/车系统自近距航点对角贯穿监测区域至远距航点，再以弓字形航线折返回至近距航点，折返航线与贯穿航线多次交汇构建多个回环和检测点，选取航线中关键帧，应用icp迭代最近点和gtsam图优化方法，逐个检测点匹配当前帧与关键帧，并全局优化该回环内所有关键帧位姿，进而在航线中迭代输出应用优化精度的三维点云，以满足应急和快速态势感知场景的更高精度与时效要求。
1.一种激光雷达slam多回环检测与迭代优化方法，其特征在于，所述方法包括以下步骤：
2.根据权利要求1所述的激光雷达slam多回环检测与迭代优化方法，其特征在于，所述s1中，无人机/车系统包括无人机/车平台、激光雷达、视频相机、控制地面站和态势地面站，无人机可搭载面阵激光雷达正射扫描监测区域，无人车可搭载机械激光雷达360°扫描监测区域；控制地面站控制无人机/车和激光雷达、视频相机按设定的航线和姿态扫描监测区域，态势地面站实时接收处理激光点云和视频图像，自动生成三维点云和实景模型，并配套基于动态视频和三维模型的纹理、温度、几何特征及空间变量的智能识别分析功能模块。
3.根据权利要求1所述的激光雷达slam多回环检测与迭代优化方法，其特征在于，所述s1中，设计无人机/车系统多回环航线的方法为，先自近距航点对角贯穿监测区域至对角远距航点，再以弓字形航线折返回近距航点，折返航线与贯穿航线多次交汇构建多个回环和检测点。
4.根据权利要求1所述的激光雷达slam多回环检测与迭代优化方法，其特征在于，所述s2中，提取当前帧特征点并计算全局位姿的方法为，当前帧点云数据进行slam算法处理，提取点云中的角点特征和面点特征，进行基于特征点的帧间里程计位姿估计，得到帧间变化的最优位姿估计和当前帧全局位姿。
5.根据权利要求1所述的激光雷达slam多回环检测与迭代优化方法，其特征在于，所述s2中，选取关键帧的方法为，计算当前帧与前一关键帧的距离或时间差，选取大于设定阈值的当前帧为新一关键帧。
6.根据权利要求1所述的激光雷达slam多回环检测与迭代优化方法，其特征在于，所述s3中，回环检测的方法为，将检测点设定半径内所有的当前帧与关键帧坐标建立kd-tree进行最近邻搜索，匹配选取该检测点的距离小于阈值且时间满足要求的当前帧与关键帧，构建检测回环。
7.根据权利要求1所述的激光雷达slam多回环检测与迭代优化方法，其特征在于，所述s4中，求解检测点匹配当前帧与关键帧相对位姿变化关系的方法为，采用icp(iterativeclosest point)迭代最近点方法，计算旋转矩阵r和平移向量t，使得下式最小：
8.根据权利要求1所述的激光雷达slam多回环检测与迭代优化方法，其特征在于，所述s4中，全局优化关键帧位姿及其点云的方法为，使用gtsam(georgia tech smoothing andmapping)图优化方法，将当前帧和关键帧点云的相对位姿变换叠加到该关键帧位姿上即为当前帧通过回环检测计算的在地图坐标系下的位姿，该位姿与由里程计叠加的当前帧在地图坐标系下的位姿会有一定偏差，利用该偏差调整该回环内所有关键帧位姿。具体优化使用gtsam开源图优化库进行，将回环检测得到的帧间约束加入到因子图中，回环的置信度就是icp的得分，进行isam的更新，得到该回环所有关键帧全局优化后的最佳估计坐标。再根据全局优化后的最佳估计坐标生成该回环所有关键帧全局优化后的三维点云模型。
9.一种电子设备，包括处理器以及存储有执行指令的存储器，当所述处理器执行所述存储器存储的所述执行指令时，所述处理器执行如权利要求1至8中任一所述的激光雷达slam多回环检测与迭代优化方法。
