一种低算力与松耦合的激光雷达和IMU的SLAM方法及系统与流程

本发明涉及同时定位与建图，具体地说是涉及一种低算力与松耦合的激光雷达和imu(inertial measurementunit，惯性测量单元)的slam(simultaneouslocalization and mapping，同时定位与建图)方法及系统。

背景技术：

1、非结构化环境常伴随非铺装路面和极少的规则建筑，这种环境中，移动平台在颠簸路面行驶，激光雷达获取的点云数据会受到严重畸变，而且传感器数据可能会短暂缺失或失效；此外，由于极少的规则建筑缺乏清晰的线面特征，传统的基于特征的slam算法在这种情况下极易失效。

2、imu通常安装在运动系统上，通过测量运动载体的惯性，提供有效的局部运动估计，由于缺少外部观测，仅基于imu的航迹推算容易出现漂移和快速发散的问题；通过ndt(normal distribution transform,正态分布变换)点云匹配方法，可构建直接法激光雷达里程计(lidar odometry)，提供有效的位姿观测，以纠正imu的航迹漂移问题。eskf(errorstate kalman filter，误差状态卡尔曼滤波器)是一种位姿估计方法，基于激光雷达数据，利用imu预测位姿作为匹配初始位姿，利用激光里程计提供位姿观测，由此构成lio(lidar-inertial odometry，松耦合激光惯性里程计)，将激光雷达和imu数据结合起来，以获得更准确的位姿估计，通过综合使用imu的预测和激光雷达的观测来减少航迹漂移。通过增加闭环检测和位姿图优化可有效消除前端里程计累计误差。由此构成完整的slam系统，同时确定车辆(定位)和构建周围环境的地图。

3、由于芯片制程限制，国产化cpu主频较低、性能一般，算力较低。基于优化的位姿求解slam算法计算复杂度高，难以在低算力平台部署或实时运行，极大地限制slam技术的推广和应用。

4、综上，亟需一种可以在低算力平台实时运行的高鲁棒性、高精度slam方法。

技术实现思路

1、本发明的目的是提供一种低算力与松耦合的激光雷达和imu的slam方法及系统，用于解决传统slam方法及系统难以在低算力平台部署，传感器数据丢失则方法失效系统崩溃，难以提供长时间、长距离下的准确定位和精确地图构建的技术问题。

2、为了解决上述技术问题，本发明提供了一种低算力与松耦合的激光雷达和imu的slam方法，包括如下步骤：

3、步骤s1，对所述imu进行静态初始化；

4、步骤s2，基于实时采集的激光雷达数据和imu数据创建一帧联合测量数据，使用eskf和ndt组成的松耦合lio对所述一帧联合测量数据中的imu数据进行预测，获得各个所述imu数据的运动位姿状态；同时通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据；

5、步骤s3，基于所述去畸变点云数据和ndt观测数据选取关键帧，基于所述关键帧对应的所述去畸变点云数据构建ndt表述子进行闭环检测得到潜在闭环对，gicp对所述潜在闭环对确认获得确认闭环对，基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新所述运动位姿状态进行定位并生成点云地图。

6、进一步地，所述步骤s1包括：

7、运行于低算力硬件平台上的ros消息中间件实时采集激光雷达和imu数据；

8、对所述激光雷达和imu数据进行时间软同步后，将移动平台静止一段时间后进行imu静态初始化；

9、所述imu静态初始化包括：计算静止时间段内的所述imu数据中的陀螺仪数据、加速度数据的均值；基于所述陀螺仪数据、加速度数据的均值得到陀螺仪和加速度计的零偏。

10、进一步地，基于所述激光雷达数据中的采样时间戳以及所述imu数据中的采样时间戳，对激光雷达和imu进行时间戳对齐，实现两者时间软同步。

11、进一步地，所述通过匹配采样时间戳将实时采集的所述激光雷达和imu数据创建一帧联合测量数据包括：

12、对实时采集的所述激光雷达数据和所述imu数据中的采样时间戳做比对；

13、通过匹配所述激光雷达数据中的采样时间戳和所述imu数据中的采样时间戳；

14、将一帧激光雷达数据及其采样时间戳对应时间周期内对应的多帧imu数据对齐，创建得到所述一帧联合测量数据。

15、进一步地，所述使用eskf和ndt组成的松耦合lio对所述一帧联合测量数据中的imu数据进行预测，获得各个所述imu数据的运动位姿状态包括：

16、对所述eskf进行初始化状态估计，包括初始化名义状态变量和误差状态变量估计；

17、基于所述一帧联合测量数据，将其中的所述imu数据积分后作为名义状态放入eskf名义状态变量；同时利用ndt观测数据更新所述eskf的误差状态变量；

18、基于所述名义状态变量和所述误差状态变量进行预测计算卡尔曼增益，进行误差状态的更新；

19、经过预测和更新，修正所述eskf的误差状态，把所述eskf的误差状态归入所述名义状态，所述eskf的名义状态为各个所述imu数据的运动位姿状态。

20、进一步地，所述通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据包括：

21、对所述一帧联合测量数据中激光雷达数据中的点云数据，基于其中每个采样点云数据中所包含各点在激光雷达空间直角坐标系下的坐标和采样时间戳，通过采样时间戳查找其前后相邻的各imu数据的运动位姿状态进行匹配对齐；

22、使用球面线性插值计算所述点云数据中每个点在激光雷达空间直角坐标系下的坐标在所述两帧运动位姿状态之间进行插值，获得去畸变点云数据。

23、进一步地，所述基于松耦合lio提供的去畸变点云数据和ndt观测数据选取关键帧，基于所述关键帧对应的所述去畸变点云数据构建ndt表述子进行闭环检测得到潜在闭环对，gicp对所述潜在闭环对确认获得确认闭环对包括：

24、基于所述去畸变点云数据和所述ndt观测数据检测关键帧；其中，所述ndt观测数据中平移和旋转量超过预定阈值后的所述ndt观测数据和所述去畸变点云数据构成所述关键帧；

25、使用ndt描述子和几何特征重新表征所述关键帧对应的激光雷达数据中的点云数据；

26、使用所述ndt描述子和几何特征，基于所述激光雷达数据执行闭环检测，检索到与所述关键帧几何特征最相似的候选关键帧，如果相似性比对表明所述候选关键帧与所述关键帧相似，则检索到一对潜在闭环对；

27、获得潜在闭环对后，进一步获取两个关键帧采样时间戳对应的临近点云数据，通过gicp匹配算法计算两者的匹配度，基于所述匹配度判断潜在闭环对是否为确认闭环对。

28、进一步地，所述基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新位姿定位并生成点云地图包括：

29、将所述关键帧的运动位姿状态作为状态节点，形成初始的状态估计因子图；

30、对于连续的所述关键帧，根据所述ndt观测数据添加相邻约束因子，以限制相邻帧之间的位姿变化；

31、对于已确认的所述闭环对，添加闭环对因子，约束闭环帧的位姿；

32、使用isam2算法对所述因子图进行优化，同时考虑ndt和所述闭环对因子，获得新位姿估计结果；

33、将所述新位姿估计结果用于更新当前所述运动位姿状态进行定位；

34、利用更新后的当前所述运动位姿状态和所述去畸变点云数据生成点云地图。

35、进一步地，所述实时采集的激光雷达数据包括采样的点云数据，每个采样包含点在激光雷达空间直角坐标系下的坐标、点的反射强度、点所属的激光线束和采样时间戳；

36、所述实时采集的imu数据包括陀螺仪数据、加速度计数据、四元数表征姿态数据和采样时间戳。

37、另一方面，本发明还提供了一种低算力与松耦合的激光雷达和imu的slam系统，包括：

38、初始化模块m1，用于对所述imu进行静态初始化；

39、松耦合lio处理模块m2，用于基于实时采集的激光雷达数据和imu数据创建一帧联合测量数据，使用eskf和ndt组成的松耦合lio对所述一帧联合测量数据中的imu数据进行预测，获得各个所述imu数据的运动位姿状态；同时通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据；

40、定位和建图模块m3，用于基于所述去畸变点云数据和ndt观测数据选取关键帧，基于所述关键帧对应的所述去畸变点云数据构建ndt表述子进行闭环检测得到潜在闭环对，gicp对所述潜在闭环对确认获得确认闭环对，基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新所述运动位姿状态进行定位并生成点云地图。

41、与现有技术相比，本发明至少可实现如下有益效果之一：

42、1、低算力要求：该方法和系统具有低算力要求，能够在计算资源受限的环境中运行。这使得它适用于嵌入式系统、无人机和移动机器人等设备，提供了更广泛的应用场景；

43、2、传感器融合：结合激光雷达和imu传感器，提供更全面的环境感知和位姿估计。这种传感器融合可以提高定位和建图的准确性，特别是在复杂环境中；

44、3、松耦合架构：采用松耦合架构lio，使系统更加灵活和可扩展。同时激光雷达和imu也是松耦合的，有助于简化系统设计和维护，同时提高了系统的稳健性；

45、4、实时性：通过时间同步和实时数据处理，该系统能够提供实时的定位和地图构建，适用于需要快速反应的应用，如自主导航和避障；

46、5、去畸变处理：采用球面线性插值来处理畸变，有助于提高激光雷达数据的准确性。这对于在移动平台上获得稳定的点云数据非常重要；

47、6、闭环检测：通过回环检测和确认，可以提高slam系统的鲁棒性，减小累积误差，从而更好地保持定位的一致性；

48、7、增量ndt匹配：采用增量ndt匹配来进行位姿估计，有助于提高定位的精度。这种方法可用于提高在传感器运动中的点云匹配性能。

49、综合来看，低算力与松耦合的激光雷达和imu的slam方法和系统在提供更精确的定位和地图构建的同时，降低了计算成本，适用于多种低算力和资源受限的应用场景。

50、本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。