一种高速低特征环境下无人机激光雷达的测图方法和系统与流程

本发明涉及激光雷达，尤其涉及一种高速低特征环境下无人机激光雷达的测图方法和系统。

背景技术：

1、在地形测绘、桥梁、隧道和底下矿洞的侦测领域，经常用到无人机的激光雷达测绘系统。激光雷达测绘系统能够快速精确地采集大量点云数据，有效节约人力物力，达到缩短工期、提高工作效率和经济效益的目的。另外，激光雷达测绘系统在复杂地形和危险地区测绘，能够在不直接接触危险目标的情况下，详细快速地进行外业数据的采集，这样既能够保证人员和设备的安全，又能够满足成图精度的要求，还能够提高工作效率。

2、传统的激光雷达测绘系统包括定姿定位系统pos和激光雷达，其中，定姿定位系统pos由惯性测量单元imu和全球定位系统gps组成。激光雷达扫描得到的点云与高精度的定位定姿系统pos组合，采用后处理方式能够生成高精度点云。但是该激光雷达测绘系统存在以下问题：1.硬件系统昂贵；整个系统对imu及gps接收机的要求较高，惯性导航系统在对于整个硬件成本上占比相当的比例。2.点云存在分层；激光雷达扫描得到的点云存在航带间分层和往返航线分层，需要定期对设备进行重新标定。3.imu存在漂移；惯性测量单元imu自身的漂移对点云数据的准确性存在影响。4.环境依赖度大；点云数据的精度对周围环境的依赖度大，观测条件要求苛刻，对于gps信号较差或者遮挡等环境，点云精度明显变差。5.数据采集困难；对于桥梁测量、地下空间和隧道等无gps环境下无法进行精准的数据采集。

3、为解决上述问题，近年来，激光雷达测绘系统采用激光雷达和低成本惯导的同步定位与地图构建slam技术相结合，实现激光雷达数据的采集。然而，传统地面激光雷达采用的slam算法在无人机领域应用存在困难，因为传统的slam技术难以适应无人机的高速动态运动和低特征物的应用环境，导致slam技术应用于无人机存在以下问题：

4、1.传统的slam算法无法满足无人机平台低特征环境下的里程计计算；

5、2.通过slam对于处理速度上无法满足机载高速动态环境；

6、3.在机载平台上里程计容易出现混乱，无法正常体现真实的飞行轨迹；

7、4.slam成图方面，对于高速动态环境及低特征物环境容易出现错误成图程计乱飞；

8、5.无人机姿态剧烈晃动，高速拐弯飞行，无法在断时间内完成帧间匹配，造成里程计解算失败。

技术实现思路

1、本发明提供一种高速低特征环境下无人机的激光雷达的测图方案，旨在解决现有的slam技术难以适应无人机的高速动态运动和低特征物的应用环境的问题。

2、为实现上述目的，根据本发明的第一方面，本发明提出了一种高速低特征环境下无人机的激光雷达的测图方法，包括：

3、根据惯性测量单元imu的测量参数，计算得到全局坐标系下imu的状态转换模型；

4、根据激光雷达在每一帧激光坐标系下的激光点坐标，计算得到激光雷达的量测模型；

5、使用流形卡尔曼滤波算法对状态转换模型和量测模型进行降维计算，得到imu的最优状态；

6、使用imu的最优状态，更新得到激光雷达的里程计信息；

7、使用贝叶斯图优化算法对里程计信息和gps因子进行图优化处理，得到激光雷达的点云图像。

8、优选的，上述激光雷达的测图方法中，根据惯性测量单元imu的测量参数，计算得到全局坐标系下imu的状态转换模型的步骤包括：

9、将imu的第一帧作为全局帧，使用imu的角速度和加速度，计算得到全局帧下imu的状态转换模型；

10、根据imu的索引，使用imu的采样周期对imu的状态转换模型进行离散化，得到离散化的状态转换模型。

11、优选的，上述激光雷达的测图方法中，根据激光雷达在每一帧激光坐标系下的激光点坐标，计算得到激光雷达的量测模型的步骤包括：

12、将激光雷达在每一帧激光坐标系下的激光点坐标，投影至激光雷达扫描结束时刻的激光坐标系，得到扫描结束时刻的所有激光点坐标；

13、获取激光雷达到imu的外参，按照激光雷达的位姿与外参的坐标对应关系，计算所有激光点坐标在全局坐标系下的量测模型。

14、优选的，上述激光雷达的测图方法中，使用流形卡尔曼滤波算法对状态转换模型和量测模型进行降维计算的步骤包括：

15、将状态转换模型和量测模型输入至流形卡尔曼滤波算法的流形进行降维操作，计算得到imu的残差；

16、使用协方差矩阵对imu的残差进行协方差计算，得到imu的最优状态；

17、上述使用imu的最优状态，更新得到激光雷达的里程计信息的步骤包括：

18、将imu的最优状态反向输入至imu的状态转换模型和激光雷达的量测模型，得到激光雷达的里程计信息。

19、优选的，上述激光雷达的测图方法中，使用贝叶斯图优化算法对里程计信息和gps因子进行图优化处理的步骤包括：

20、使用isam2算法建立贝叶斯因子树；

21、将gps因子、里程计信息对应的里程计因子以及回环因子输入至贝叶斯因子树；

22、使用贝叶斯因子树，逐帧对里程计信息进行因子图优化；

23、将成图后的公共特征物作为回环因子加入贝叶斯因子树进行函数循环；

24、使用gtsam优化函数更新isam算法，得到激光雷达的点云图像。

25、优选的，在得到激光雷达的点云图像的步骤之后，激光雷达的测图方法还包括：

26、对点云图像中点云数据进行降采样，得到激光雷达的特征点数据集；

27、对特征点数据集中每一特征点，使用k近邻算法从预设ivox体素结构中匹配得到最近邻激光点；

28、计算特征点到最近邻激光点的残差，判断残差是否小于或等于预设残差阈值；

29、若判定残差小于或等于预设残差阈值，则将特征点添加至预设ivox体素结构。

30、优选的，上述使用k近邻算法从预设ivox体素结构中匹配得到最近邻激光点的步骤，包括：

31、将包含有激光点的伪希尔伯特空间曲线划分多个体素单元，使用多个体素单元组成预设ivox体素结构，其中，每个体素单元包括多个激光点；

32、使用哈希函数将每个体素单元的网格坐标映射为哈希键值，使用哈希键值组成哈希表；

33、按照体素单元的网格坐标，查找特征点所在的体素单元；

34、按照体素单元的网格坐标对应的哈希键值，查找特征点对应的k个近邻激光点；

35、对近邻激光点与特征点的距离进行计算，得到最近邻激光点。

36、优选的，上述激光雷达的测图方法，在判定残差小于或等于预设残差阈值的步骤之后还包括：

37、将残差输入至协方差矩阵进行计算，得到残差对应的协方差；

38、将协方差输入至激光雷达的量测模型中进行更新，更新激光雷达的里程计信息。

39、根据本发明的第二方面，本发明还提供了一种高速低特征环境下无人机的激光雷达的测图系统，包括：

40、状态转换模型计算模块，用于根据惯性测量单元imu的测量参数，计算得到全局坐标系下imu的状态转换模型；

41、量测模型计算模块，用于根据激光雷达在每一帧激光坐标系下的激光点坐标，计算得到激光雷达的量测模型；

42、最优状态计算模块，用于使用流形卡尔曼滤波算法对状态转换模型和量测模型进行降维计算，得到imu的最优状态；

43、里程计信息更新模块，用于使用imu的最优状态，更新得到激光雷达的里程计信息；

44、图优化模块，用于使用贝叶斯图优化算法对里程计信息和gps因子进行图优化处理，得到激光雷达的点云图像。

45、优选的，上述激光雷达的测图系统中，状态转换模型计算模块包括：

46、模型计算子模块，用于将imu的第一帧作为全局帧，使用imu的角速度和加速度，计算得到全局帧下imu的状态转换模型；

47、离散化处理子模块，用于根据imu的索引，使用imu的采样周期对imu的状态转换模型进行离散化，得到离散化的状态转换模型。

48、优选的，上述激光雷达的测图系统中，量测模型计算模块包括：

49、激光点坐标投影子模块，用于将激光雷达在每一帧激光坐标系下的激光点坐标，投影至激光雷达扫描结束时刻的激光坐标系，得到扫描结束时刻的所有激光点坐标；

50、量测模型计算子模块，用于获取激光雷达到imu的外参，按照激光雷达的位姿与外参的坐标对应关系，计算所有激光点坐标在全局坐标系下的量测模型。

51、优选的，上述激光雷达的测图系统还包括：

52、点云降采样模块，用于对所述点云图像中点云数据进行降采样，得到所述激光雷达的特征点数据集；

53、激光点匹配模块，用于对所述特征点数据集中每一特征点，使用k近邻算法从预设ivox体素结构中匹配得到最近邻激光点；

54、残差计算模块，用于计算所述特征点到所述最近邻激光点的残差；

55、残差判断模块，用于判断所述残差是否小于或等于预设残差阈值；

56、特征点添加模块，用于若所述残差判断模块判定所述残差小于或等于预设残差阈值时，将所述特征点添加至所述预设ivox体素结构。

57、综上，本发明上述技术方案提供的激光雷达的测图方案，通过根据惯性测量单元imu的测量参数，计算得到全局坐标系下imu的状态转换模型；然后根据激光雷达在每一帧激光坐标系下的激光点坐标计算得到激光雷达的量测模型；再使用流形卡尔曼滤波算法对状态转换模型和量测模型，然后使用流形卡尔曼滤波算法对状态转换模型和量测模型进行降维计算，得到imu的最优状态，更新得到激光雷达的里程计信息，最后石永红贝叶斯图优化算法对里程计信息和gps因子进行图优化处理，得到激光雷达的点云图像。本发明提供的激光雷达的测图方案，针对高速动态及低特征物环境，通过采用流形卡尔曼滤波算法在底层数据结构(即上述状态转换模型和量测模型)上进行优化，同时采用贝叶斯图优化方式加入gps因子，更加适合无人机高速动态及低特征环境下的应用。该方案能够利于无人机的机载高动态平台环境下激光雷达slam的高鲁棒性实现及应用。