CN117451032A - 一种低算力与松耦合的激光雷达和imu的slam方法及系统 - Google Patents

一种低算力与松耦合的激光雷达和imu的slam方法及系统 Download PDF

Info

Publication number
CN117451032A
CN117451032A CN202311440457.XA CN202311440457A CN117451032A CN 117451032 A CN117451032 A CN 117451032A CN 202311440457 A CN202311440457 A CN 202311440457A CN 117451032 A CN117451032 A CN 117451032A
Authority
CN
China
Prior art keywords
data
point cloud
imu
ndt
laser radar
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202311440457.XA
Other languages
English (en)
Inventor
康晓
靳保
张永申
徐乔
周禹昆
丁秋琳
关瑜
靳璐
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhongbing Intelligent Innovation Research Institute Co ltd
Original Assignee
Zhongbing Intelligent Innovation Research Institute Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhongbing Intelligent Innovation Research Institute Co ltd filed Critical Zhongbing Intelligent Innovation Research Institute Co ltd
Priority to CN202311440457.XA priority Critical patent/CN117451032A/zh
Publication of CN117451032A publication Critical patent/CN117451032A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/77—Determining position or orientation of objects or cameras using statistical methods
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3848—Data obtained from both position sensors and additional sensors
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Probability & Statistics with Applications (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Theoretical Computer Science (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明一种低算力与松耦合的激光雷达和IMU的SLAM方法及系统，属定位建图领域。对IMU静态初始化；基于实时采集的激光雷达数据和IMU数据创建一帧联合测量数据，使用ESKF和NDT组成的松耦合LIO对一帧联合测量数据中的IMU数据进行预测，获得各个IMU数据的运动位姿状态；通过采样时间戳与激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据；基于去畸变点云数据和NDT观测数据选取关键帧，基于关键帧对应的去畸变点云数据构建NDT表述子进行闭环检测得到潜在闭环对，GICP对潜在闭环对确认获得确认闭环对，基于关键帧和确认闭环对构建贝叶斯网络，优化和更新运动位姿状态进行定位并生成点云地图。解决传统SLAM难以低算力平台部署和准确定位建图问题。

Description

一种低算力与松耦合的激光雷达和IMU的SLAM方法及系统
技术领域
本发明涉及同时定位与建图技术领域，具体地说是涉及一种低算力与松耦合的激光雷达和IMU(Inertial MeasurementUnit，惯性测量单元)的SLAM(SimultaneousLocalization and Mapping，同时定位与建图)方法及系统。
背景技术
非结构化环境常伴随非铺装路面和极少的规则建筑，这种环境中，移动平台在颠簸路面行驶，激光雷达获取的点云数据会受到严重畸变，而且传感器数据可能会短暂缺失或失效；此外，由于极少的规则建筑缺乏清晰的线面特征，传统的基于特征的SLAM算法在这种情况下极易失效。
IMU通常安装在运动系统上，通过测量运动载体的惯性，提供有效的局部运动估计，由于缺少外部观测，仅基于IMU的航迹推算容易出现漂移和快速发散的问题；通过NDT(Normal Distribution Transform,正态分布变换)点云匹配方法，可构建直接法激光雷达里程计(Lidar Odometry)，提供有效的位姿观测，以纠正IMU的航迹漂移问题。ESKF(ErrorState Kalman Filter，误差状态卡尔曼滤波器)是一种位姿估计方法，基于激光雷达数据，利用IMU预测位姿作为匹配初始位姿，利用激光里程计提供位姿观测，由此构成LIO(Lidar-inertial Odometry，松耦合激光惯性里程计)，将激光雷达和IMU数据结合起来，以获得更准确的位姿估计，通过综合使用IMU的预测和激光雷达的观测来减少航迹漂移。通过增加闭环检测和位姿图优化可有效消除前端里程计累计误差。由此构成完整的SLAM系统，同时确定车辆(定位)和构建周围环境的地图。
由于芯片制程限制，国产化CPU主频较低、性能一般，算力较低。基于优化的位姿求解SLAM算法计算复杂度高，难以在低算力平台部署或实时运行，极大地限制SLAM技术的推广和应用。
综上，亟需一种可以在低算力平台实时运行的高鲁棒性、高精度SLAM方法。
发明内容
本发明的目的是提供一种低算力与松耦合的激光雷达和IMU的SLAM方法及系统，用于解决传统SLAM方法及系统难以在低算力平台部署，传感器数据丢失则方法失效系统崩溃，难以提供长时间、长距离下的准确定位和精确地图构建的技术问题。
为了解决上述技术问题，本发明提供了一种低算力与松耦合的激光雷达和IMU的SLAM方法，包括如下步骤：
步骤S1，对所述IMU进行静态初始化；
步骤S2，基于实时采集的激光雷达数据和IMU数据创建一帧联合测量数据，使用ESKF和NDT组成的松耦合LIO对所述一帧联合测量数据中的IMU数据进行预测，获得各个所述IMU数据的运动位姿状态；同时通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据；
步骤S3，基于所述去畸变点云数据和NDT观测数据选取关键帧，基于所述关键帧对应的所述去畸变点云数据构建NDT表述子进行闭环检测得到潜在闭环对，GICP对所述潜在闭环对确认获得确认闭环对，基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新所述运动位姿状态进行定位并生成点云地图。
进一步地，所述步骤S1包括：
运行于低算力硬件平台上的ROS消息中间件实时采集激光雷达和IMU数据；
对所述激光雷达和IMU数据进行时间软同步后，将移动平台静止一段时间后进行IMU静态初始化；
所述IMU静态初始化包括：计算静止时间段内的所述IMU数据中的陀螺仪数据、加速度数据的均值；基于所述陀螺仪数据、加速度数据的均值得到陀螺仪和加速度计的零偏。
进一步地，基于所述激光雷达数据中的采样时间戳以及所述IMU数据中的采样时间戳，对激光雷达和IMU进行时间戳对齐，实现两者时间软同步。
进一步地，所述通过匹配采样时间戳将实时采集的所述激光雷达和IMU数据创建一帧联合测量数据包括：
对实时采集的所述激光雷达数据和所述IMU数据中的采样时间戳做比对；
通过匹配所述激光雷达数据中的采样时间戳和所述IMU数据中的采样时间戳；
将一帧激光雷达数据及其采样时间戳对应时间周期内对应的多帧IMU数据对齐，创建得到所述一帧联合测量数据。
进一步地，所述使用ESKF和NDT组成的松耦合LIO对所述一帧联合测量数据中的IMU数据进行预测，获得各个所述IMU数据的运动位姿状态包括：
对所述ESKF进行初始化状态估计，包括初始化名义状态变量和误差状态变量估计；
基于所述一帧联合测量数据，将其中的所述IMU数据积分后作为名义状态放入ESKF名义状态变量；同时利用NDT观测数据更新所述ESKF的误差状态变量；
基于所述名义状态变量和所述误差状态变量进行预测计算卡尔曼增益，进行误差状态的更新；
经过预测和更新，修正所述ESKF的误差状态，把所述ESKF的误差状态归入所述名义状态，所述ESKF的名义状态为各个所述IMU数据的运动位姿状态。
进一步地，所述通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据包括：
对所述一帧联合测量数据中激光雷达数据中的点云数据，基于其中每个采样点云数据中所包含各点在激光雷达空间直角坐标系下的坐标和采样时间戳，通过采样时间戳查找其前后相邻的各IMU数据的运动位姿状态进行匹配对齐；
使用球面线性插值计算所述点云数据中每个点在激光雷达空间直角坐标系下的坐标在所述两帧运动位姿状态之间进行插值，获得去畸变点云数据。
进一步地，所述基于松耦合LIO提供的去畸变点云数据和NDT观测数据选取关键帧，基于所述关键帧对应的所述去畸变点云数据构建NDT表述子进行闭环检测得到潜在闭环对，GICP对所述潜在闭环对确认获得确认闭环对包括：
基于所述去畸变点云数据和所述NDT观测数据检测关键帧；其中，所述NDT观测数据中平移和旋转量超过预定阈值后的所述NDT观测数据和所述去畸变点云数据构成所述关键帧；
使用NDT描述子和几何特征重新表征所述关键帧对应的激光雷达数据中的点云数据；
使用所述NDT描述子和几何特征，基于所述激光雷达数据执行闭环检测，检索到与所述关键帧几何特征最相似的候选关键帧，如果相似性比对表明所述候选关键帧与所述关键帧相似，则检索到一对潜在闭环对；
获得潜在闭环对后，进一步获取两个关键帧采样时间戳对应的临近点云数据，通过GICP匹配算法计算两者的匹配度，基于所述匹配度判断潜在闭环对是否为确认闭环对。
进一步地，所述基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新位姿定位并生成点云地图包括：
将所述关键帧的运动位姿状态作为状态节点，形成初始的状态估计因子图；
对于连续的所述关键帧，根据所述NDT观测数据添加相邻约束因子，以限制相邻帧之间的位姿变化；
对于已确认的所述闭环对，添加闭环对因子，约束闭环帧的位姿；
使用iSAM2算法对所述因子图进行优化，同时考虑NDT和所述闭环对因子，获得新位姿估计结果；
将所述新位姿估计结果用于更新当前所述运动位姿状态进行定位；
利用更新后的当前所述运动位姿状态和所述去畸变点云数据生成点云地图。
进一步地，所述实时采集的激光雷达数据包括采样的点云数据，每个采样包含点在激光雷达空间直角坐标系下的坐标、点的反射强度、点所属的激光线束和采样时间戳；
所述实时采集的IMU数据包括陀螺仪数据、加速度计数据、四元数表征姿态数据和采样时间戳。
另一方面，本发明还提供了一种低算力与松耦合的激光雷达和IMU的SLAM系统，包括：
初始化模块M1，用于对所述IMU进行静态初始化；
松耦合LIO处理模块M2，用于基于实时采集的激光雷达数据和IMU数据创建一帧联合测量数据，使用ESKF和NDT组成的松耦合LIO对所述一帧联合测量数据中的IMU数据进行预测，获得各个所述IMU数据的运动位姿状态；同时通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据；
定位和建图模块M3，用于基于所述去畸变点云数据和NDT观测数据选取关键帧，基于所述关键帧对应的所述去畸变点云数据构建NDT表述子进行闭环检测得到潜在闭环对，GICP对所述潜在闭环对确认获得确认闭环对，基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新所述运动位姿状态进行定位并生成点云地图。
与现有技术相比，本发明至少可实现如下有益效果之一：
1、低算力要求：该方法和系统具有低算力要求，能够在计算资源受限的环境中运行。这使得它适用于嵌入式系统、无人机和移动机器人等设备，提供了更广泛的应用场景；
2、传感器融合：结合激光雷达和IMU传感器，提供更全面的环境感知和位姿估计。这种传感器融合可以提高定位和建图的准确性，特别是在复杂环境中；
3、松耦合架构：采用松耦合架构LIO，使系统更加灵活和可扩展。同时激光雷达和IMU也是松耦合的，有助于简化系统设计和维护，同时提高了系统的稳健性；
4、实时性：通过时间同步和实时数据处理，该系统能够提供实时的定位和地图构建，适用于需要快速反应的应用，如自主导航和避障；
5、去畸变处理：采用球面线性插值来处理畸变，有助于提高激光雷达数据的准确性。这对于在移动平台上获得稳定的点云数据非常重要；
6、闭环检测：通过回环检测和确认，可以提高SLAM系统的鲁棒性，减小累积误差，从而更好地保持定位的一致性；
7、增量NDT匹配：采用增量NDT匹配来进行位姿估计，有助于提高定位的精度。这种方法可用于提高在传感器运动中的点云匹配性能。
综合来看，低算力与松耦合的激光雷达和IMU的SLAM方法和系统在提供更精确的定位和地图构建的同时，降低了计算成本，适用于多种低算力和资源受限的应用场景。
本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1为一种低算力与松耦合的激光雷达和IMU的SLAM方法原理框图；
图2为一种低算力与松耦合的激光雷达和IMU的SLAM系统框图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。
本发明的目的是提供一种低算力与松耦合的激光雷达和IMU的SLAM方法及系统，融合ESKF、增量式NDT匹配、NDT全局描述子、GICP(Generalized Iterative ClosestPoint，均一化迭代最临近点云)匹配和因子图优化方法设计得到在低算力平台实时运行的高鲁棒性、高精度SLAM方法。
前端松耦合激光惯性里程计单元，后端回环检测和位姿优化单元。前端松耦合激光惯性里程计单元使用激光雷达和IMU数据通过误差状态卡尔曼滤波器和增量正态分布变换NDT匹配进行里程推算。
后端回环检测和位姿优化单元通过NDT描述子和GICP匹配可高效且准确的检测确认闭环场景，再通过构建包含里程计因子和闭环对因子的因子图进行优化，最后更新位姿和点云地图，实现同时定位与建图。
实施例一：
一种低算力与松耦合的激光雷达和IMU的SLAM方法，包括如下步骤：
步骤S1，对所述IMU进行静态初始化；
步骤S2，基于实时采集的激光雷达数据和IMU数据创建一帧联合测量数据，使用ESKF和NDT组成的松耦合LIO对所述一帧联合测量数据中的IMU数据进行预测，获得各个所述IMU数据的运动位姿状态；同时通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据；
步骤S3，基于所述去畸变点云数据和NDT观测数据选取关键帧，基于所述关键帧对应的所述去畸变点云数据构建NDT表述子进行闭环检测得到潜在闭环对，GICP对所述潜在闭环对确认获得确认闭环对，基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新所述运动位姿状态进行定位并生成点云地图。
进一步地，所述步骤S1包括：
运行于低算力硬件平台上的ROS消息中间件实时采集激光雷达和IMU数据，实现两种传感器数据的时间软同步。；
对所述激光雷达和IMU数据进行时间软同步后，将移动平台静止一段时间后进行IMU静态初始化；
所述IMU静态初始化包括：计算静止时间段内的所述IMU数据中的陀螺仪数据、加速度数据的均值；基于所述陀螺仪数据、加速度数据的均值得到陀螺仪和加速度计的零偏。
工控机、激光雷达传感器和IMU传感器刚性固定安装在移动平台上，确保工控机、激光雷达和IMU电源供应和数据通信。
工控机用于采集、处理来自激光雷达和传感器的数据，进行定位、地图构建。
本发明中所使用的工控机硬件规格、以及性能相对较低，不适宜处理高度计算密集型任务，属于低算力硬件平台。
使用ROS(Robot Operating System，机器人操作系统)提供的消息中间件，采用通用的传感器数据格式采集传感器数据，有助于传感器数据的标准化和管理。ROS消息中间件采集激光雷达数据和IMU数据。
激光雷达传感器用于测量物体和环境中的距离和形状；IMU用于测量和监测物体的线性加速度和角速度，由陀螺仪和加速度计组成。
实时采集的激光雷达数据包含：采样的点云数据信息，每个采样包含点在激光雷达空间直角坐标系下的坐标、点的反射强度、点所属的激光线束和采样时间戳。
实时采集的IMU数据包含：陀螺仪数据、加速度计数据、四元数表征姿态数据和采样时间戳。
时间软同步是指在数据处理中，根据采样时间戳将不同传感器数据进行同步。进一步地，基于所述激光雷达数据中的采样时间戳以及所述IMU数据中的采样时间戳，对激光雷达和IMU进行时间戳对齐，实现两者时间软同步。
由于激光雷达和IMU数据中包含采样时间戳，本发明为激光雷达(Lidar)和IMU(Inertial Measurement Unit，惯性测量单元)的松耦合框架，可以有效地处理不同传感器数据的时间偏差，激光雷达和IMU数据时间软同步，无需进行时间硬同步。这种灵活性使SLAM系统能够在不同类型的传感器数据上正常运行。
基于所述激光雷达数据和所述IMU数据时间软同步的基础上，对IMU进行静态初始化。所述IMU静态初始化包括采集静止状态下的IMU数据，计算获得所述陀螺仪和加速度计的零偏同时估计重力方向。
(1)采集静止状态下的IMU数据：
激光雷达和IMU时间软同步后，将移动平台静止放置一段时间，确保移动平台及刚性固连的IMU传感器均未发生运动。在静止时间段内实时采集IMU数据。
使用数据队列保存静止状态下的IMU数据，静止时间段持续时间达到需求。
对于静止时间段的具体持续时间，由系统实际需求来确定，取决于多个因素，包括传感器性能、系统要求、噪声水平和误差容忍度等。须保证运动平台静止并尽量避免启动发动机等高频振动源，以确保可以获得准确的零偏估计和重力方向估计。
通常，静止时间段不小于一秒到五秒是一个常见的范围。较长的时间段可以降低噪声对估计的影响，但也会增加系统初始化的时间。在实际应用中，需要进行一些实验和调整，以找到一个适合特定系统和环境的最佳静止时间段。决定静止时间段时，需要考虑实时性的需求，较长的静止时间段会导致系统初始化时间增加，但可提高估计的精度。
示例性的，静止时间段不小于1秒，因IMU数据采样频率较高，几百次的静止采样即可有效获取陀螺仪和加速度计零偏估计。
(2)计算陀螺仪零偏、加速度计零偏及重力方向。
计算IMU数据中的陀螺仪数据和加速度计数据的均值和协方差，计算包含重力的加速度计均值和协方差估计重力方向。
若所述协方差大于设定阈值则认为移动平台未静止或所述IMU传感器测量噪声较大，若所述协方差小于设定阈值则将所述均值作为所述IMU的零偏：
示例性的，陀螺仪数据的协方差阈值设置为0.05，加速度计数据的协方差阈值设置为0.05。
在移动平台静止下进行IMU静态初始化，获得陀螺仪零偏、加速度计零偏及重力方向。
具体的，所述IMU静态初始化就是将所述移动平台静止放置一段时间，在静止时间段内，由于移动平台和与其刚性固连的IMU本身没有任何运动，可以认为所述IMU的陀螺仪测到陀螺仪零偏，而加速度计测到加速度计的零偏和重力之和；
统计静止时间段内的陀螺仪数据的均值，计为
统计静止时间段内的加速度计数据的均值，计为
由于移动平台静止并未发生移动或转动，所述静止时间段的陀螺仪均值视为陀螺仪的零偏其中bg为陀螺仪零偏；
加速度计的测量方程，如下式所示：
其中，为加速度测量值，RT为旋转矩阵，a为加速度计真值，g为重力，ba为加速度计零偏，ηa为加速度计随机游走因子。
其中，加速度计真值a是指在没有受到外部干扰或力的情况下，IMU实际加速度的准确值。在IMU传感器测量中，加速度计通常会受到一些误差和噪声的影响，因此测量值可能与真实值存在一定的差异。
加速度计真值用于表示物体的真实运动状态，不受传感器测量误差的影响，用于研究、仿真、校准传感器、验证算法等应用中。在许多应用中，准确的加速度计真值是非常重要的，因为它们可以用来验证传感器的性能和校准传感器，以确保测量结果的准确性。
由于移动平台持续静止时间段内，加速度真值a为零，RT旋转为单位矩阵，加速度计测量值取方向为/>大小为g的矢量为重力矢量，由此确定重力方向为的反向；
去掉重力g后重新计算加速度数据的均值得加速度计零偏/>
陀螺仪数据的协方差和加速度计数据的协方差，用于后续ESKF的噪声参数。
基于这个步骤得到：
(1)陀螺仪零偏和加速度计零偏：分别通过统计陀螺仪数据和加速度计数据的均值，在零偏校准后，分别计算得到陀螺仪零偏和加速度计零偏；
(2)重力方向估计：通过加速度计数据，在静止时间段内静态状态下估计了重力方向为
(3)协方差：计算得到陀螺仪数据和加速度计数据的协方差估计。协方差数据可以用于后续的ESKF的噪声参数。
在激光雷达和IMU时间软同步的基础上，采集静止时间段内的IMU加速度计数据和陀螺仪数据，进行零偏校准，获得陀螺仪零偏和加速度计零偏，同时估计了重力方向。这一过程利用静止时间段内静止状态下的传感器数据来校准传感器，以提高后续的姿态估计的准确性，并通过计算数据的协方差估计来考虑测量不确定性。
进一步地，所述通过匹配采样时间戳将实时采集的所述激光雷达和IMU数据创建一帧联合测量数据包括：
对实时采集的所述激光雷达数据和所述IMU数据中的采样时间戳做比对；
通过匹配所述激光雷达数据中的采样时间戳和所述IMU数据中的采样时间戳；
将一帧激光雷达数据及其采样时间戳对应时间周期内对应的多帧IMU数据对齐，创建得到所述一帧联合测量数据。
对所述实时获取激光雷达和IMU数据通过时间戳对比，得到一帧联合测量数据。以支持后续的数据融合和SLAM运算。
通过时间戳比对保存一帧激光雷达数据及对应时间周期(激光雷达数据开始时刻至结束时刻)内的IMU数据，由此对应将一帧激光雷达数据和多帧IMU数据作为一帧联合测量数据。通过匹配时间戳来确保每帧点云数据与对应时间周期内的多帧IMU数据对齐。
示例性的，本实施例采用机械重复式扫描激光雷达，一帧激光雷达点云数据是激光雷达机械旋转一周的采样数据，采样周期为0.1s；
本实施例采用高频率IMU，采样间隔为0.01s；故一帧所述点云数据对应10次所述IMU采样；一帧联合测量数据包含1帧激光雷达数据和对应的10帧IMU数据。
将来自激光雷达和IMU的传感器数据通过采样时间戳进行对比，以确保它们在时间上对齐，从而创建一帧联合测量数据。
这个步骤的目标是将激光雷达数据与与之对应的多帧IMU数据在时间上保持一致。这种采样时间戳对齐是确保在后续的处理中，可以将激光雷达数据和IMU数据有效地结合在一起，以获得更准确的定位和姿态估计。
进一步地，所述使用ESKF和NDT组成的松耦合LIO对所述一帧联合测量数据中的IMU数据进行预测，获得各个所述IMU数据的运动位姿状态包括：
对所述ESKF进行初始化状态估计，包括初始化名义状态变量和误差状态变量估计；
基于所述一帧联合测量数据，将其中的所述IMU数据积分后作为名义状态放入ESKF名义状态变量；同时利用NDT观测数据更新所述ESKF的误差状态变量；
基于所述名义状态变量和所述误差状态变量进行预测计算卡尔曼增益，进行误差状态的更新；
经过预测和更新，修正所述ESKF的误差状态，把所述ESKF的误差状态归入所述名义状态，所述ESKF的名义状态为各个所述IMU数据的运动位姿状态。
基于所述一帧联合测量数据，使用ESKF对其中的IMU数据进行预测，获得各个所述IMU数据对应的运动位姿状态；同时通过采样时间戳与激光雷达点云数据匹配，进行球面线性插值以获取去畸变点云数据，为后续SLAM运算提供精确的IMU数据对应的运动位姿状态和去畸变点云数据。
依次遍历所述一帧联合测量数据中激光雷达点云数据中的每个点，通过采样时间戳查找获取所述点前后近临对应的IMU的运动状态，使用球面线性差值获得所述点云数据在所述点云结束时刻激光雷达坐标系下所述点的去畸变坐标，获得去畸变点云数据。基于所述一帧联合测量数据，ESKF流程如下：
所述一帧联合测量数据中的IMU数据到达时，将其积分后作为名义状态放入ESKF的名义状态变量(Nominal State)，并将误差部分作为误差状态放入ESKF的误差变量(Error State)；
ESKF使用名义状态变量和误差状态变量来描述系统状态和估计误差，通过卡尔曼滤波技术，ESKF能够根据传感器测量数据来更新名义状态变量，同时估计误差状态变量，最终提供更准确的系统状态估计。名义状态变量是ESKF的核心组成部分，它代表了系统的当前状态，而ESKF通过不断地预测和更新名义状态变量以及估计误差状态变量，实现了对系统状态的精确估计和跟踪。
名义状态是系统的当前状态，误差状态用于纠正和优化名义状态，以提高状态估计的准确性。这种状态估计方法是卡尔曼滤波的核心，通过不断地预测和更新状态变量，实现对系统状态的跟踪和估计。
在移动平台运动过程中，名义状态随着IMU数据随时间进行递推而不断更新，误差状态受到高斯噪声影响而变大，此时，ESKF的误差状态均值和协方差描述误差状态的增长情况，视为高斯分布；
ESKF的更新过程需要依赖IMU以外的传感器观测数据，对卡尔曼滤波器进行修正，本实施例加入激光里程计作为位姿观测，ESKF更新过程中，利用激光里程计观测数据更新ESKF的误差状态后验均值与协方差，随后将这部分误差状态合入ESKF名义状态变量中，把ESKF置零，这样就完成了一次预测——更新过程的循环。
具体的，所述ESKF根据一帧联合测量数据中的IMU数据建立运动模型，加入激光里程计视为位姿观测，继而推导整个误差状态卡尔曼滤波器；设状态变量为：
x＝[p,v,R,bg,ba,g]T
p为位姿平移，v为位姿速度，R为位姿旋转，bg，ba分别为陀螺仪零偏零偏和加速度计零偏，g为重力加速度。代入一帧联合测量数据中的IMU测量数据，状态变量在连续时间下的运动方程为：
其中，为平移一阶微分，即速度，/>为加速度测量值，/>为角速度测量值，ηbg，ηba分别为陀螺仪和加速度计的随机游走因子。
从离散时间状态方程可推出离散时间的状态方程，名义状态变量的离散时间运动方程为：
bg(t+Δt)＝bg(t)，
ba(t+Δt)＝ba(t)，
g(t+Δt)＝g(t).
误差状态的离散时间运动方程为：
δp(t+Δt)＝δp+δvΔt
δbg(t+Δt)＝δbg+ηbg
δba(t+Δt)＝δba+ηba
δg(t+Δt)＝δg
其中，Δt为离散采样间隔时间，θ为旋转角度，ηv为速度观测噪声，ηθ为角度观测噪声。
上述公式可整体记为如下公式，其中ω为噪声：
Q为该噪声的的协方差矩阵。
Q＝diag(03，Cov(ηv)，Cov(ηθ)，Cov(ηg)，Cov(ηa)，03)
其中，03为三阶零矩阵，ηv为速度观测噪声，ηg为重力观测噪声，ηa为加速度测量噪声。
其线性化形式如下公式所述：
δx(t+Δt)＝Fδx(t)+w
其中，F为线性化后的雅可比矩阵，如下式所示：
在此基础上，执行ESKF的预测过程。
预测过程包括对名义状态的预测(IMU积分)以及对误差状态的预测，如下式所示：
δxpred＝Fδx
Ppred＝FPFT+Q
其中，xpred为名义状态(均值)的预测量，Ppred为误差状态(协方差)的预测量，P为误差状态(协方差)，Q为噪声协方差矩阵。
考虑所述ESKF的更新过程，在传统ESKF中，可以直观地对观测方程线性化，求出观测方程相对于名义状态变量的雅可比矩阵，进而更新卡尔曼滤波器。而在所述ESKF中，当前拥有名义状态的估计及误差状态的估计，且希望更新的是误差状态，因此计算观测方程相对于误差状态的雅可比矩阵：
然后再计算卡尔曼增益，进而计算误差状态的更新过程：
K＝PpredHT(HPpredHT+V)-1
δx＝K(z-h(xpred))
x＝xpred+δx
P＝(I-KH)Ppred
其中，K为卡尔曼增益，Ppred为预测的协方差矩阵，z为观测数据，h为观测方程，I为单位阵，最后的P为修正后的协方差矩阵，通过链式法则生成。
经过预测和更新过程之后，ESKF的误差状态估计被修正。
接下来，把ESKF的误差状态归入名义状态，然后重置ESKF。归入部分如下所示：
pk+1＝pk+δpk
vk+1＝vk+δvk
Rk+1＝RkExp(δvk)
bg，k+1＝bg，k+δbg，k
ba，k+1＝ba，k+δba，k
gk+1＝gk+δgk
上述计算视为广义的状态变量加法。
所述ESKF的重置分为均值部分和协方差部分。均值部分实现为：
σx＝0
重置前后的误差状态相差一个旋转方面的小雅可比矩阵：
其中，为当前旋转变换的反对称矩阵。/>
将其扩充到整个状态变量维度，得到一个完整的雅可比矩阵：
Jk＝diag(I3，I3，Jθ，I3，I3，I3)
其中，I3为三阶单位阵。
因此，在误差状态的均值归零时，其协方差矩阵需进行线性变换：
上述实际做了切空间投影，即把一个切空间中的高斯分布投影到另一个切空间中。
进一步地，所述通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据包括：
对所述一帧联合测量数据中激光雷达数据中的点云数据，基于其中每个采样点云数据中所包含各点在激光雷达空间直角坐标系下的坐标和采样时间戳，通过采样时间戳查找其前后相邻的各IMU数据的运动位姿状态进行匹配对齐；
使用球面线性插值计算所述点云数据中每个点在激光雷达空间直角坐标系下的坐标在所述两帧运动位姿状态之间进行插值，获得去畸变点云数据。
对每个点云中点进行处理，通过时间戳查找其前后近邻的运动状态，然后使用球面线性插值来获取点在点云结束时刻的激光雷达坐标系下的去畸变坐标。
对所述一帧联合测量数据中激光雷达数据中的点云数据，基于其中的每个采样点云数据中所包含各点在激光雷达空间直角坐标系下的坐标和采样时间戳，通过采样时间戳查找其前后相邻的所述各个IMU数据的运动位姿状态进行匹配对齐；
对于所述点云数据中的每个点，通过其采样时间戳，在所述IMU数据的运动位姿状态中查找其前后最接近的两帧运动位姿状态；
使用球面线性插值计算所述点云数据中每个点在激光雷达空间直角坐标系下的坐标在所述两帧运动位姿状态之间进行插值；
对于点云数据中的每个点，重复上述插值过程，获得一帧去畸变点云数据，其中的每个点都经过了所述球面线性插值处理，去除了激光雷达数据中的畸变。
这一步骤获得ESKF预测的运动位姿状态息和去畸变点云数据，它用于后续的SLAM运算。
ESKF当前时刻的运动位姿状态作为匹配初始位姿，基于去畸变点云数据，通过增量NDT点云匹配获得匹配位姿，所述匹配位姿作为位姿感测数据进行ESKF状态更新，实现位姿的优化和更新，组成松耦合的LIO。
(1)匹配初始位姿：ESKF使用IMU数据对当前时刻的运动状态进行预测的运动位姿状态，包括位置、速度和方向。这个预测的运动位姿作为匹配初始位姿。
(2)增量NDT点云匹配：
将待匹配的去畸变的点云数据分成若干体素；
计算每个体素内点云数据的统计信息，包括均值和方差，以表示体素内的点云分布；
通过点云与体素的高斯分布进行匹配计算残差，表示点云与体素之间的差异；
采用迭代方法，采用G-N或L-M方法，更新估计的位姿，从而最小化残差，使点云与体素更一致。
根据当前时刻的ESKF预测的运动位姿状态作为匹配初始位姿，通过待匹配的去畸变的点云数据的统计信息，可增量更新匹配后的整体点云数据的统计信息，继而进行优化求解获得新的匹配位姿，由此完成增量NDT匹配；
(3)匹配位姿融入ESKF：匹配位姿作为位姿观测数据可融入所述ESKF进行状态更新，有助于提高对移动平台位置和姿态的估计精度。；ESKF中的卡尔曼滤波部分和增量NDT点云匹配部分相对解耦，由此组成松耦合LIO。
NDT方法通过得到点云的局部统计信息即均值和方差，然后对其进行匹配。
流程如下：
第一步，体素化：将目标点云(待匹配的去畸变的点云数据)按照一定的分辨率划分成若干体素，每个体素是一个立方体或栅格单元。这个分辨率通常由用户指定，决定了体素的大小；
第二步，计算每个体素中的点云高斯分布：对于每个体素，计算其中包含的点云数据的统计信息，包括均值和方差。这些统计信息用于表示体素内点云的分布特征。体素内的点云数据可以视为来自一个高斯分布；
第三步，点云配准：针对点云配准任务，首先确定每个点云中所属的体素，建立该点的位置映射到相应的体素；
第四步，计算残差：对于每个点，计算它与所属体素内部统计信息之间的残差，残差表示点云中的点与体素内高斯分布的统计信息之间的差异，残差用来评估点云与体素的一致性；
第五步，迭代位姿估计：使用迭代方法，采用G-N(Gauss-Newton，高斯-牛顿)或L-M(Levenberg-Marquardt)方法对估计位姿进行迭代，更新位姿估计。
G-N方法通过迭代来不断改进参数的估计值，以减小目标函数(通常是残差平方和)的值，从而逐步接近最优解。常用于解决非线性最小二乘问题的数值优化算法，用于寻找参数估计值，以使目标函数的残差平方和最小化。适用于位姿估计。
L-M方法是一种数值优化算法，通常用于解决非线性最小二乘问题。该算法是对G-N方法和梯度下降方法的一种改进它具有良好的全局收敛性和数值稳定性，广泛用于参数估计、曲线拟合、位姿估计等领域。
NDT方法通过将点云划分成体素，计算每个体素内的点云统计信息，然后通过匹配和迭代位姿估计来优化位姿，以使点云与体素的高斯分布更加一致，从而实现点云的配准和位姿估计。
设被配准的点云中的某个点为qi，它经过R，t(旋转，平移)变换后，落在某个统计指标为μi，∑i的体素中，这个栅格中的残差为：
ei＝Rqi+t-μi
其中，ei为残差，R为旋转矩阵，qi为点云中的第i个点，t为平移三维向量，μi为统计指标，为点云高斯分布。
最后的R，t由一个加权的最小二乘问题决定：
上述问题相当于最大化每个点落在所在栅格的概率分布，因此是一个最大似然估计：
ESKF的状态更新：在ESKF中，使用IMU以外的传感器观测数据对卡尔曼滤波器进行修正。在这里，加入了激光里程计的位姿观测，通过观测数据更新误差状态的后验均值与协方差。随后，将这部分误差合并到名义状态变量中，并将ESKF重置，以完成一次预测-更新过程的循环。
增量更新NDT体素：传统的NDT里程推算需要重新建立局部地图并重置所有NDT体素。然而，这个实施例中，采用增量方式更新高斯分布，避免大规模重置，减少计算量。如果某个体素已包含历史点云，新增的点云会根据新的均值和方差来更新高斯分布。
设某个体素有m个历史点云，构成的高斯分布为μH，∑H。
新增n个点，均值和方差为μA，∑A。
设合并之后的估计均值和方差分贝为μ，∑。根据定义可得：
其中，x1、x2、……xm为历史点云数据中的点，y1、y2、……yn为新增点云数据中的点。
除了体素内部的高斯分布应该随点云更新以外，由于移动平台往往从某个地点出发一直前行，体素本身也应该随着移动平台运动而增加。然而，如果考虑长时间使用这个里程计算法，应该将体素数量限制在一定范围内。
体素数量控制：在长时间使用的情况下，需要限制体素数量，以防止体素数量无限增长。通过设置一个队列模型，把最近更新的体素放到最前面。当整个队列超出预期容量时，删除最旧的那部分历史体素。
示例性的，体素总量保持在十万个左右，很多之前建立的历史体素定期删除。维护一个近期使用的缓存机制。
主要的输出包括以下内容：
(1)匹配后的位姿：通过增量NDT点云匹配，最终输出匹配后的位姿，即移动平台在当前时刻的位置和姿态；
(2)ESKF的状态更新：ESKF的状态变量(包括位置、速度、方向等)将得到更新，这些状态变量表示了移动平台的运动状态；
(3)体素的增量更新：体素内部的高斯分布将被增量更新，以适应新的点云数据。这有助于在长时间使用中保持高质量的局部地图；
(4)松耦合的LIO：ESKF和NDT两个主要组件是松耦合的。
这些输出用于改善激光雷达和IMU的集成，提高定位的准确性和鲁棒性，以满足SLAM系统的需求。
"松耦合"意味着在系统中的不同组件或模块之间的联系相对较弱，它们能够独立运行和更新，而不会过于依赖其他组件。在步骤五中，松耦合体现在ESKF和NDT两个主要组件之间。具体地，以下是如何体现松耦合LIO的一些要点：
(1)ESKF的独立运行和更新：ESKF用于融合来自惯性测量单元(IMU)和其他传感器的信息，以进行状态估计和预测。ESKF可以独立运行，并且它的更新过程主要依赖于IMU数据，而不是其他传感器。这使得ESKF的运行不会受到其他组件的强烈影响，实现了ESKF的松耦合；
(2)增量NDT点云匹配的独立性：增量NDT点云匹配方法用于根据去畸变点云数据来进行位姿的优化和更新。这一步骤是针对激光雷达点云数据，NDT独立于ESKF的运行。点云匹配不依赖于ESKF的状态估计，而是使用ESKF预测的运动位姿状态作为初始位姿进行匹配。因此，增量NDT点云匹配也是一个独立运行的组件，其运行不受ESKF的强制依赖；
(3)松耦合的信息传递：虽然ESKF和增量NDT点云匹配是相对独立的，它们之间的信息传递是通过匹配后的运动位姿状态实现的。匹配后的运动位姿状态作为位姿观测数据，可以融入ESKF进行状态更新。这种信息传递是比较松散的，因为它是通过位姿的形式进行的，而不需要ESKF和点云匹配方法之间的直接交互。
总之，ESKF和NDT点云匹配方法是在系统中相对独立地运行的组件，它们不会强烈地相互依赖，但通过匹配后的运动位姿状态进行信息传递，实现了松耦合的LIO系统。这种松耦合设计有助于系统的可扩展性和模块化，使不同组件可以相对容易地替换或升级，而不会对整个系统造成严重的影响。
进一步地，基于所述去畸变点云数据和所述NDT观测数据检测关键帧；其中，所述NDT观测数据中平移和旋转量超过预定阈值后的所述NDT观测数据和所述去畸变点云数据构成所述关键帧；
使用NDT描述子和几何特征重新表征关键帧对应激光雷达数据中的点云数据；
使用所述NDT描述子和几何特征，基于所述激光雷达数据执行闭环检测，检索到与所述关键帧几何特征最相似的候选关键帧，如果相似性比对表明所述候选关键帧与所述关键帧最相似，则检索到一对潜在闭环对；
获得潜在闭环对后，进一步获取两个关键帧采样时间戳对应的临近点云数据，通过GICP匹配算法计算两者的匹配度，将匹配度较高的潜在闭环对确定为确认闭环对。
基于所述松耦合LIO提供的去畸变点云数据和NDT观测数据，基于所述NDT观测数据设定平移和旋转阈值选取关键帧，再基于关键帧对应的所述去畸变点云数据构建NDT表述子进行闭环检测获得潜在闭环对，基于GICP匹配可对所述潜在闭环对进行确认并获得确认闭环对。具体包括：
(1)检测关键帧：获取所述前端松耦合LIO提供的去畸变点云和NDT观测数据；检测所述NDT的变换关系，当平移或旋转量超过设定阈值后认定当前所述NDT观测数据和去畸变点云数据构成关键帧，存储每一个所述关键帧形成关键帧队列；
(2)构建NDT描述子和几何特征：使用NDT描述子重新表征关键帧对应的点云信息，提取其中的几何信息并保存几何特征，保存各个所述关键帧对应的所述NDT描述子和所述几何特征成NDT描述子队列和几何特征队列。NDT描述子包括点云的几何特征以及统计信息，如均值和协方差。
(3)闭环检测：系统使用前述队列中的NDT描述子和几何特征，基于激光雷达数据执行闭环检测。检索与最新关键帧几何特征最相似的候选关键帧，通过相似性比对来确定潜在的闭环对。
本实施例使用的NDT描述子从单个NDT体素点云(包含均值和协方差)的协方差矩阵特征值出发，使用下式进行体素几何形状分类：
其中，g为几何特征表征值，e1、e2、e3为协方差矩阵特征值(由大至小排序)。
g能够识别四个不同的形状类别：平面、椭球体、球体和直线。
为了描述这些明确的形状类别，使用以下公式将g的值映射到不同的形状上：
其中，S为几何特征表征值，为整数集，s为设定参数，gmax为其g最大值限制。
将每个NDT单元视为由均值μ、协方差矩阵∑表征的正态分布。因此，利用熵E(entropy)作为与正态分布相关联的统计量来描述NDT单元：
提取几何特征S和熵E后，NDT全局描述子用一个2Nr×Ns矩阵来表示。
(4)潜在闭环对检测：如果相似性比对表明候选关键帧与最新关键帧相似，系统检测到一对潜在闭环对。这意味着系统在检测到的两个关键帧之间形成了一个潜在的闭环。
(5)闭环确认：获取到潜在闭环对后，系统进一步获取这两个关键帧采样时间戳对应的临近点云数据，通过GICP匹配算法计算这两个点云数据之间的匹配度。如果匹配度较高，则确认闭环正确。如果匹配度较低，则可能是误检测。此处匹配度高低基于实际需求精度来确定。
采用基于激光雷达的闭环匹配方法获取精准的闭环关联边。基于激光雷达的闭环匹配方法采用GICP点云配准算法，对当前帧的激光点云数据与历史帧的激光点云数据进行配准。
GICP算法是一种对传统的ICP算法进行改进的方法，它在考虑点云之间的对应关系时引入了额外的权重因子，使得算法更具有鲁棒性。
GICP算法的核心思想是通过最小化点云之间的距离度量(如欧氏距离)来找到最优的旋转和平移变换，从而将两个点云对齐。
GICP算法的步骤如下：
初始对齐，通过自适应约束优化的多平台导航定位增强可以获得当前帧点云与历史帧点云的初始化位姿；
对应点匹配，通过寻找在两个点云中最相似的点对来建立点云之间的对应关系。这可以使用最近邻搜索或KD(K-Dimensional)树等数据结构来实现；
计算权重，对每个点对计算一个权重，用于在后续的优化过程中调整点的重要性。这些权重可以根据点之间的距离、法向量等信息来确定；计算刚体变换，使用加权的最小二乘法，通过最小化点对之间的加权距离度量来计算出最优的旋转和平移变换；
更新对齐结果，将计算得到的变换应用于其中一个点云，使其相对于另一个点云进行变换，从而使两个点云更加对齐；迭代优化，重复执行上述过程直到达到一定的收敛条件，如变换的改变量小于某个阈值，或迭代次数达到一定限制。
本实施例构建几何特征队列的KD-Tree，检索与最新关键帧几何特征最相似的候选几何特征关键帧；遍历候选关键帧的NDT描述子，进行描述子的相似比对，确定相似度最高的关键帧；若存在则检测到一对潜在闭环对，由此完成闭环检测；获取到潜在闭环对后，分别获取对应的临近点云数据，通过GICP匹配获得两点云的匹配度，将匹配度最高的潜在闭环对确定为确认闭环对，若匹配度较低则说明闭环错误，由此完成闭环确认。
进一步地，所述基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新位姿定位并生成点云地图包括：
将所述关键帧的运动位姿状态作为状态节点，形成初始的状态估计因子图；
对于连续的所述关键帧，根据所述NDT观测数据添加相邻约束因子，以限制相邻帧之间的位姿变化；
对于已确认的所述闭环对，添加闭环对因子，约束闭环帧的位姿；
使用iSAM2算法对所述因子图进行优化，同时考虑NDT和所述闭环对因子，获得新位姿估计结果；
将所述新位姿估计结果用于更新当前所述运动位姿状态进行定位；
利用更新后的当前所述运动位姿状态和所述去畸变点云数据生成点云地图。
基于关键帧和确认闭环对，构建iSAM2贝叶斯网络，优化和更新位姿，以减小误差并生成点云地图。
根据所述关键帧和所述确认闭环对，构建以位姿为状态节点包含里程计相邻约束因子和闭环对因子的iSAM2贝叶斯网络，即可优化更新位姿，继而优化定位轨迹。因关键帧对应位姿和相应的去畸变点云数据，基于优化定位轨迹可生成点云地图。
为了消除由于传感器测量误差、卡尔曼滤波器本身的局限性和NDT匹配误差导致的累计状态误差，实现更加准确的状态估计，本实施例在状态估计与因子图优化之间构成反馈关系，将因子图优化的位姿结果替换成当前状态位姿。
(1)根据关键帧和确认闭环对，构建iSAM2贝叶斯网络：将关键帧的运动位姿状态作为状态节点，形成初始的状态估计。添加里程计相邻约束因子：对于连续的关键帧，根据里程计数据添加相邻约束因子，以限制相邻帧之间的位姿变化。
添加闭环对因子：对于已确认的闭环对添加闭环对因子，约束闭环帧的位姿。
(2)通过iSAM2进行因子图优化：使用iSAM2算法对构建的因子图进行优化，以最小化因子图中的约束因子，从而获得更准确的位姿估计。
因子图优化将同时考虑激光里程计相邻约束和闭环对因子，以减小累积误差，提高状态估计的精度。
(3)更新位姿估计：获取通过因子图优化得到的新位姿估计结果。将新位姿估计结果用于更新当前状态位姿。
(4)优化定位轨迹：使用新的位姿估计来优化整个定位轨迹，确保轨迹的一致性和准确性。
(5)生成点云地图：利用更新后的位姿估计和去畸变点云数据，生成点云地图，该地图反映了环境的特征和结构。
这些步骤帮助改善定位准确性，提高SLAM系统的鲁棒性，消除误差和提高状态估计的精度。这也确保了系统能够更好地理解环境并创建一致的点云地图。
本实施例提出了一种应用于非结构化道路工况，部署于低算力移动平台的激光雷达和惯性测量单元松耦合SLAM方法。
首先实时获取激光雷达和IMU传感器数据，通过IMU静态初始化获取状态初始值；
ESKF作为状态估计器以IMU数据建立运动过程，通过估计器状态和曲面线性插值对点云运动畸变进行矫正，增量NDT匹配激光里程计使用去畸变点云并提供位姿观测数据进行状态更新过程，避免滤波器发散。ESKF状态估计其和增量NDT激光里程计相对独立，该松耦合激光惯性里程计可看做前端；
通过NDT描述子和GICP匹配可高效且准确的检测确认闭环场景，再通过构建包含里程计因子和闭环对因子的iSAM2贝叶斯网络进行优化，最后更新位姿和点云地图，由此实现同时定位与建图。
本实施例采用误差状态卡尔曼滤波器ESKF作为状态估计器与增量式NDT匹配激光里程计构成松耦合激光惯性里程计，滤波方法与增量更新相比传统的优化求和与重置再搜索再计算相比显著减少了计算量，松耦合激光惯性里程计允许激光或惯性传感器短暂失效，大大提高了前端的鲁棒性。
NDT全局描述子使用统计信息表征环境几何特性，先使用KD-Tree进行统计几何特征相似查找获取候选项，再通过全局描述子进行精细比对确认候选闭环对，减少了计算量。
GICP匹配引入额外的权重因子，是的匹配更具鲁棒性，避免闭环对错误建立。构建的包含里程计相邻约束因子和闭环对因子的iSAM2因子图可有效消除前端累计误差，提高定位准确度。
进一步地，所述实时采集的激光雷达数据包括采样的点云数据，每个采样包含点在激光雷达空间直角坐标系下的坐标、点的反射强度、点所属的激光线束和采样时间戳；
所述实时采集的IMU数据包括陀螺仪数据、加速度计数据、四元数表征姿态数据和采样时间戳。
实施例二：
本发明的另一个实施例，公开了一种低算力与松耦合的激光雷达和IMU的SLAM系统，从而实现实施例一中的一种低算力与松耦合的激光雷达和IMU的SLAM方法。各模块的具体实现方式参照实施例一中的相应描述。
如图2所示，该系统包括IMU初始化模块M1、松耦合SLAM处理模块M2和定位和建图模块M3。
初始化模块M1，用于对所述IMU进行静态初始化；
松耦合LIO处理模块M2，用于基于实时采集的激光雷达数据和IMU数据创建一帧联合测量数据，使用ESKF和NDT组成的松耦合LIO对所述一帧联合测量数据中的IMU数据进行预测，获得各个所述IMU数据的运动位姿状态；同时通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据；
定位和建图模块M3，用于基于所述去畸变点云数据和NDT观测数据选取关键帧，基于所述关键帧对应的所述去畸变点云数据构建NDT表述子进行闭环检测得到潜在闭环对，GICP对所述潜在闭环对确认获得确认闭环对，基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新所述运动位姿状态进行定位并生成点云地图。
本实施例融合误差状态卡尔曼滤波器、增量式NDT匹配、NDT全局描述子、GICP匹配和因子图优化方法设计得到低算力与松耦合的激光雷达和IMU的SLAM方法，解决在非结构化环境，低算力平台SLAM系统难以实时运行，难以获得空旷野外环境高鲁棒性、长距离高精度位姿的问题。
与现有技术相比，本发明可实现如下有益效果：
1、低算力要求：该方法和系统具有低算力要求，能够在计算资源受限的环境中运行。这使得它适用于嵌入式系统、无人机和移动机器人等设备，提供了更广泛的应用场景；
2、传感器融合：结合激光雷达和IMU传感器，可以提供更全面的环境感知和位姿估计。这种传感器融合可以提高定位和建图的准确性，特别是在复杂环境中；
3、松耦合架构：采用松耦合架构LIO，使系统更加灵活和可扩展。同时激光雷达和IMU也是松耦合的，有助于简化系统设计和维护，同时提高了系统的稳健性；
4、实时性：通过时间同步和实时数据处理，该系统能够提供实时的定位和地图构建，适用于需要快速反应的应用，如自主导航和避障；
5、去畸变处理：采用球面线性插值来处理畸变，有助于提高激光雷达数据的准确性。这对于在移动平台上获得稳定的点云数据非常重要；
6、闭环检测：通过回环检测和确认，可以提高SLAM系统的鲁棒性，减小累积误差，从而更好地保持定位的一致性；
7、增量NDT匹配：采用增量NDT匹配来进行位姿估计，有助于提高定位的精度。这种方法可用于提高在传感器运动中的点云匹配性能。
综合来看，低算力与松耦合的激光雷达和IMU的SLAM方法和系统在提供更精确的定位和地图构建的同时，降低了计算成本，适用于多种低算力和资源受限的应用场景。
本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种低算力与松耦合的激光雷达和IMU的SLAM方法，其特征在于，包括如下步骤：
步骤S1，对所述IMU进行静态初始化；
步骤S2，基于实时采集的激光雷达数据和IMU数据创建一帧联合测量数据，使用ESKF和NDT组成的松耦合LIO对所述一帧联合测量数据中的IMU数据进行预测，获得各个所述IMU数据的运动位姿状态；同时通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据；
步骤S3，基于所述去畸变点云数据和NDT观测数据选取关键帧，基于所述关键帧对应的所述去畸变点云数据构建NDT表述子进行闭环检测得到潜在闭环对，GICP对所述潜在闭环对确认获得确认闭环对，基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新所述运动位姿状态进行定位并生成点云地图。
2.根据权利要求1所述的方法，其特征在于，所述步骤S1包括：
运行于低算力硬件平台上的ROS消息中间件实时采集激光雷达和IMU数据；
对所述激光雷达和IMU数据进行时间软同步后，将移动平台静止一段时间后进行IMU静态初始化；
所述IMU静态初始化包括：计算静止时间段内的所述IMU数据中的陀螺仪数据、加速度数据的均值；基于所述陀螺仪数据、加速度数据的均值得到陀螺仪和加速度计的零偏。
3.根据权利要求2所述的方法，其特征在于，基于所述激光雷达数据中的采样时间戳以及所述IMU数据中的采样时间戳，对激光雷达和IMU进行时间戳对齐，实现两者时间软同步。
4.根据权利要求3所述的方法，其特征在于，所述通过匹配采样时间戳将实时采集的所述激光雷达和IMU数据创建一帧联合测量数据包括：
对实时采集的所述激光雷达数据和所述IMU数据中的采样时间戳做比对；
通过匹配所述激光雷达数据中的采样时间戳和所述IMU数据中的采样时间戳；
将一帧激光雷达数据及其采样时间戳对应时间周期内对应的多帧IMU数据对齐，创建得到所述一帧联合测量数据。
5.根据权利要求4所述的方法，其特征在于，所述使用ESKF和NDT组成的松耦合LIO对所述一帧联合测量数据中的IMU数据进行预测，获得各个所述IMU数据的运动位姿状态包括：
对所述ESKF进行初始化状态估计，包括初始化名义状态变量和误差状态变量估计；
基于所述一帧联合测量数据，将其中的所述IMU数据积分后作为名义状态放入ESKF名义状态变量；同时利用NDT观测数据更新所述ESKF的误差状态变量；
基于所述名义状态变量和所述误差状态变量进行预测计算卡尔曼增益，进行误差状态的更新；
经过预测和更新，修正所述ESKF的误差状态，把所述ESKF的误差状态归入所述名义状态，所述ESKF的名义状态为各个所述IMU数据的运动位姿状态。
6.根据权利要求5所述的方法，其特征在于，所述通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据包括：
对所述一帧联合测量数据中激光雷达数据中的点云数据，基于其中每个采样点云数据中所包含各点在激光雷达空间直角坐标系下的坐标和采样时间戳，通过采样时间戳查找其前后相邻的各IMU数据的运动位姿状态进行匹配对齐；
使用球面线性插值计算所述点云数据中每个点在激光雷达空间直角坐标系下的坐标在所述两帧运动位姿状态之间进行插值，获得去畸变点云数据。
7.根据权利要求6所述的方法，其特征在于，所述基于松耦合LIO提供的去畸变点云数据和NDT观测数据选取关键帧，基于所述关键帧对应的所述去畸变点云数据构建NDT表述子进行闭环检测得到潜在闭环对，GICP对所述潜在闭环对确认获得确认闭环对包括：
基于所述去畸变点云数据和所述NDT观测数据检测关键帧；其中，所述NDT观测数据中平移和旋转量超过预定阈值后的所述NDT观测数据和所述去畸变点云数据构成所述关键帧；
使用NDT描述子和几何特征重新表征所述关键帧对应的激光雷达数据中的点云数据；
使用所述NDT描述子和几何特征，基于所述激光雷达数据执行闭环检测，检索到与所述关键帧几何特征最相似的候选关键帧，如果相似性比对表明所述候选关键帧与所述关键帧相似，则检索到一对潜在闭环对；
获得潜在闭环对后，进一步获取两个关键帧采样时间戳对应的临近点云数据，通过GICP匹配算法计算两者的匹配度，基于所述匹配度判断潜在闭环是否为确认闭环对。
8.根据权利要求7所述的方法，其特征在于，所述基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新位姿定位并生成点云地图包括：
将所述关键帧的运动位姿状态作为状态节点，形成初始的状态估计因子图；
对于连续的所述关键帧，根据所述NDT观测数据添加相邻约束因子，以限制相邻帧之间的位姿变化；
对于已确认的所述闭环对，添加闭环对因子，约束闭环帧的位姿；
使用iSAM2算法对所述因子图进行优化，同时考虑NDT和所述闭环对因子，获得新位姿估计结果；
将所述新位姿估计结果用于更新当前所述运动位姿状态进行定位；
利用更新后的当前所述运动位姿状态和所述去畸变点云数据生成点云地图。
9.根据权利要求1-8任一项所述的方法，其特征在于，
所述实时采集的激光雷达数据包括采样的点云数据，每个采样包含点在激光雷达空间直角坐标系下的坐标、点的反射强度、点所属的激光线束和采样时间戳；
所述实时采集的IMU数据包括陀螺仪数据、加速度计数据、四元数表征姿态数据和采样时间戳。
10.一种低算力与松耦合的激光雷达和IMU的SLAM系统，其特征在于，包括：
初始化模块M1，用于对所述IMU进行静态初始化；
松耦合LIO处理模块M2，用于基于实时采集的激光雷达数据和IMU数据创建一帧联合测量数据，使用ESKF和NDT组成的松耦合LIO对所述一帧联合测量数据中的IMU数据进行预测，获得各个所述IMU数据的运动位姿状态；同时通过所述采样时间戳与所述激光雷达数据中的点云数据匹配，进行球面线性插值处理获得去畸变点云数据；
定位和建图模块M3，用于基于所述去畸变点云数据和NDT观测数据选取关键帧，基于所述关键帧对应的所述去畸变点云数据构建NDT表述子进行闭环检测得到潜在闭环对，GICP对所述潜在闭环对确认获得确认闭环对，基于所述关键帧和所述确认闭环对构建贝叶斯网络，优化和更新所述运动位姿状态进行定位并生成点云地图。
CN202311440457.XA 2023-11-01 2023-11-01 一种低算力与松耦合的激光雷达和imu的slam方法及系统 Pending CN117451032A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311440457.XA CN117451032A (zh) 2023-11-01 2023-11-01 一种低算力与松耦合的激光雷达和imu的slam方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311440457.XA CN117451032A (zh) 2023-11-01 2023-11-01 一种低算力与松耦合的激光雷达和imu的slam方法及系统

Publications (1)

Publication Number Publication Date
CN117451032A true CN117451032A (zh) 2024-01-26

Family

ID=89579619

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311440457.XA Pending CN117451032A (zh) 2023-11-01 2023-11-01 一种低算力与松耦合的激光雷达和imu的slam方法及系统

Country Status (1)

Country Link
CN (1) CN117451032A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117739972A (zh) * 2024-02-18 2024-03-22 中国民用航空飞行学院 一种无全球卫星定位系统的无人机进近阶段定位方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117739972A (zh) * 2024-02-18 2024-03-22 中国民用航空飞行学院 一种无全球卫星定位系统的无人机进近阶段定位方法
CN117739972B (zh) * 2024-02-18 2024-05-24 中国民用航空飞行学院 一种无全球卫星定位系统的无人机进近阶段定位方法

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
Indelman et al. Factor graph based incremental smoothing in inertial navigation systems
CN109522832B (zh) 基于点云片段匹配约束和轨迹漂移优化的回环检测方法
US10027952B2 (en) Mapping and tracking system with features in three-dimensional space
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN112444246B (zh) 高精度的数字孪生场景中的激光融合定位方法
Xu et al. A multi-sensor information fusion method based on factor graph for integrated navigation system
Zhang et al. Vision-aided localization for ground robots
CN117451032A (zh) 一种低算力与松耦合的激光雷达和imu的slam方法及系统
CN113960622A (zh) 融合激光雷达及imu传感器信息的实时定位方法及装置
US20240151537A1 (en) Range image aided inertial navigation system (ins) with map based localization
Wen et al. GNSS/LiDAR integration aided by self-adaptive Gaussian mixture models in urban scenarios: An approach robust to non-Gaussian noise
Xu et al. $ D^ 2$ SLAM: Decentralized and Distributed Collaborative Visual-inertial SLAM System for Aerial Swarm
Pöppl et al. Integrated trajectory estimation for 3D kinematic mapping with GNSS, INS and imaging sensors: A framework and review
US20240003687A1 (en) Range image aided ins
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及系统
CN114047766B (zh) 面向室内外场景长期应用的移动机器人数据采集系统及方法
CN115950414A (zh) 一种不同传感器数据的自适应多重融合slam方法
CN116908777A (zh) 基于显式通信带标签伯努利的多机器人随机组网协同导航方法
Xu et al. Humanoid robot localization based on hybrid map
CN114705223A (zh) 多移动智能体在目标跟踪中的惯导误差补偿方法及系统
Xu et al. Multi-Sensor Fusion Framework Based on GPS State Detection
Arturo et al. Cooperative simultaneous localisation and mapping using independent rao–blackwellised filters
CN118089728A (zh) 一种四足机器人轨迹生成方法、装置、设备及存储介质
Nafouki et al. Spatial navigation algorithms for autonomous robotics

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination