Positioning and mapping method and system based on fusion of laser radar and inertial measurement unit

Abstract

The invention discloses a laser radar and inertial measurement unit fused positioning and mapping system and method, and solves the problem of height error accumulation in a pure laser SLAM algorithm. Firstly, joint calibration is carried out on the laser radar and the IMU. The laser radar obtains original point cloud data and sends the original point cloud data to the laser speedometer. And the IMU obtains the laser radar pose and sends the laser radar pose to the laser odometer. The laser speedometers comprise a high-frequency laser speedometer and a low-frequency laser speedometer; the high-frequency laser speedometer carries out motion distortion removal and feature point extraction on original point cloud data to obtain feature point clouds, registration is carried out on the feature point clouds corresponding to two laser frames, and the pose difference value of the laser radar is obtained. And the low-frequency laser speedometer selects a key frame, matches the current key frame with the local point cloud map to obtain a laser speedometer pose of the laser radar, and outputs the laser speedometer pose. And a factor graph model is constructed, optimization solution is carried out to obtain optimized variable nodes, and the pose of the laser radar in the world coordinate system and a 3D point cloud map are obtained.

Description

technical field [0001] The invention relates to the technical field of robot perception and navigation, in particular to a positioning and mapping method and system for fusion of 3D laser radar and inertial measurement unit. Background technique [0002] Simultaneous Localization and Mapping (SLAM) is a promising technical field in the field of robot navigation and positioning. The significance of real-time mapping and positioning technology is that it does not rely on the information given by external measurements to determine its own position, but relies on its own sensors to determine the increment of its own position, thereby determining the position of the body in the environmental map. At the same time, a point cloud map of the environment is established based on the sensor data of the position and the current position. The SLAM field is currently mainly divided into laser SLAM technology, visual SLAM technology, and multi-sensor fusion SLAM technology. Due to its st...

AI Technical Summary

Problems solved by technology

[0008] In view of this, the present invention provides a real-time positioning and mapping system for the fusion of 3D laser radar and inertial measurement unit, which is a real-time positioning and mapping framework based on factor graph optimization of laser radar and inertial navigation unit IMU fusion, which solves the problem of Height Error Accumulation Problem in Pure Laser SLAM Algorithm

Images