Multi-Source Fusion SLAM Localization Method for Autonomous Driving in Factory Areas

Abstract

The present invention discloses a multi-source fusion SLAM positioning method for autonomous driving in a factory area, comprising the following steps: S1, calibrating a GNSS antenna, a LiDAR sensor, and an IMU inertial measurement unit fixedly installed on a carrier to obtain calibrated extrinsic parameters; S2, correcting the motion distortion of the laser point cloud data collected by the LiDAR sensor, and then obtaining the point cloud data in the global coordinate system through the calibrated extrinsic parameters; S3, map matching; S4, obtaining a global LIO point cloud map; S5, establishing a high-precision point cloud map; S6, performing relocalization. The method of the present invention uses multi-sensor data such as a GNSS antenna, a lidar, and an inertial sensor for fusion, improving the accuracy and robustness of environmental perception of the factory area and meeting the requirements of high-precision navigation and positioning for autonomous driving of a carrier in a complex and changeable factory area environment.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile carrier navigation and positioning. In particular, it relates to a multi-source fusion SLAM positioning method for factory area autonomous driving. Background Technique

[0002] In the complex environment of industrial factory areas, the navigation system must be able to provide high-precision positioning and real-time map construction capabilities. However, traditional Global Navigation Satellite Systems (GNSS), such as Beidou and GPS, although providing real-time positioning services, their accuracy can only reach the meter level, which is far from sufficient to meet the strict requirements for precision in industrial applications, and it is difficult to achieve efficient and accurate map construction and positioning, and provide stable and reliable navigation for the carrier. Existing satellite navigation systems can achieve real-time centimeter-level high-precision positioning relying on ground-based augmentation technology, but in an environment where signals are easily interfered and blocked in industrial factory areas, it is difficult to achieve continuous high-precision positioning.

[0003] High-precision and high-reliability positioning is the core key technology for robot navigation. The single GNSS sensor has low reliability and cannot achieve high-precision positioning in all scenarios. Precise positioning technology using multi-source sensor fusion processing is required. As a high-precision sensor, Light Detection and Ranging (LiDAR) can provide accurate coordinate information and construct a dense point cloud map without being restricted by occlusion and lighting conditions. The Simultaneously Localization and Mapping (SLAM) technology based on LiDAR, because its development started earlier, has been relatively mature in terms of theory, technology, and product applications, and is widely used in fields such as autonomous driving and robot navigation, allowing a mobile carrier to simultaneously perform self-positioning and environmental map construction according to the observation information of sensors in an unknown environment. However, the positioning accuracy in a complex environment is still insufficient, which is an urgent problem to be solved in the technical field of mobile carrier navigation and positioning. Summary of the Invention

[0004] To solve the problems in the background technique, the present invention proposes a multi-source fusion SLAM positioning method for factory area autonomous driving with high positioning accuracy and strong environmental perception accuracy.

[0005] For this purpose, the present invention adopts the following technical solutions:

[0006] A multi-source fusion SLAM positioning method for factory area autonomous driving, comprising the following steps:

[0007] S1, calibrate the GNSS antenna, LiDAR sensor, and IMU inertial measurement unit fixedly installed on the carrier to obtain the calibrated external parameters;

[0008] S2. Correct the motion distortion of the lidar point cloud data collected by the lidar sensor, and then obtain the point cloud data in the global coordinate system through the calibrated extrinsic parameters.

[0009] S3. Perform map matching to construct a LIO point cloud map; the LIO represents the radar inertial odometry data obtained by fusing the data of the lidar sensor and the IMU inertial measurement unit through step S3.

[0010] S4. Obtain the global LIO point cloud map:

[0011] Use the data obtained by the IMU inertial measurement unit in the global coordinate system and the data obtained by the GNSS antenna for integrated navigation to obtain the result of the tight coupling of GNSS-RTK and INS, and then add it as an optimization factor to the factor graph. Update the global position of all the LIO point cloud maps obtained in S3 through the constraints of the factor graph to obtain the LIO point cloud map in the global coordinate system.

[0012] S5. Establish a high-precision point cloud map:

[0013] When the vehicle passes through or the lidar sensor scans a repeated path, detect the feature association between the current frame of point cloud and the LIO point cloud map in the global coordinate system in real time, and establish a high-precision point cloud map.

[0014] S6. Perform relocalization:

[0015] When the vehicle runs in the area within the finally established high-precision point cloud map, through the radar inertial odometry result at the current moment after the tight coupling of the lidar sensor and the IMU inertial measurement unit at the current moment, take the Scan near the vehicle in the radar inertial odometry result at the current moment to perform Submap-to-Map matching to obtain the position of the vehicle at the current moment under the high-precision point cloud map, and complete the relocalization of the vehicle.

[0016] Specifically, S1 is as follows: For the GNSS antenna, lidar sensor, and IMU inertial measurement unit on the vehicle running in the factory area where no map has been built, calibrate the GNSS antenna and the lidar sensor to the center of the IMU inertial measurement unit to obtain the calibrated extrinsic parameters; the calibrated extrinsic parameters are used to unify the data obtained by each sensor into the global coordinate system after the data is obtained by each sensor.

[0017] Preferably, the global coordinate system is a coordinate system with the center of the IMU inertial measurement unit as the origin.

[0018] Step S2 includes the following sub-steps:

[0019] S21. Obtain the rotation matrix and translation vector as shown in the following formula:

[0020]

[0021] where t i is the acquisition time when the LiDAR sensor acquires the i-th laser point, and i is the number of the laser point; R(t i ) is the rotation matrix of the LiDAR sensor relative to the LiDAR sensor coordinate system at time t i ; T(t i ) is the translation vector of the LiDAR sensor relative to the LiDAR sensor coordinate system at time t i ; is the motion speed of the carrier at the start time of the point cloud frame where the i-th laser point is acquired; a and ω are the observed data of the acceleration and angular acceleration of the IMU inertial measurement unit; Δt is the time interval from the start to the end of the point cloud frame where the i-th laser point is acquired; and exp are both operators of Lie group and Lie algebra;

[0022] S22, correct the motion distortion of the laser point cloud data:

[0023] Propagate the result obtained by S21 back to the laser point cloud data of the corresponding frame to obtain the undistorted laser point cloud data. The following formula holds:

[0024] P i ′ = R(t i ) -1 ·(P i - T(t i ));

[0025] where P i is the position of the i-th laser point obtained by the LiDAR sensor, and P i ′ is the position of the i-th laser point in the LiDAR sensor coordinate system;

[0026] S23, convert the position of P i ′ to the global coordinate system according to the calibrated external parameters obtained by S1 to obtain the point cloud data in the global coordinate system.

[0027] Step S3 includes the following sub-steps:

[0028] S31, construct the distance residual between the point and the plane based on the point cloud data in the global coordinate system obtained by S2;

[0029] S32, use iterative Kalman filtering to optimize the distance residual obtained by S31 to obtain the state vector of the carrier during motion;

[0030] S33. Transform the point cloud data in the global coordinate system obtained in S2 using the state vector obtained in S32 to obtain the point cloud after backend optimization in the local coordinate system;

[0031] S34. Store the point cloud after backend optimization obtained in S33 in the kd-tree data format to obtain the LIO point cloud map; the LIO represents the radar inertial odometry data obtained by fusing the data of the LiDAR sensor and the IMU inertial measurement unit through step S3.

[0032] The state vector in step S32 includes position, velocity, attitude, deviation, and gravity state variables.

[0033] Specifically, S5 uses the single or multiple Scans scanned by the LiDAR sensor to accumulate into a Submap, and adaptively performs loop closure detection according to the Submap-to-Map matching; then optimizes the loop closure detection and the LIO point cloud in the global coordinate system by constructing a stable triangle descriptor to establish a high-precision point cloud map.

[0034] Preferably, the carrier is an autonomous vehicle or a logistics robot.

[0035] Compared with the prior art, the present invention has the following beneficial effects:

[0036] 1. The method of the present invention can still achieve high-precision positioning of the carrier through the assistance of point cloud and IMU data when the GNSS position error is large or the position is locked in the complex environment of the factory area.

[0037] 2. The method of the present invention utilizes the high-precision and long-distance measurement capabilities of the LiDAR sensor, and can successfully solve the problem of discontinuous positioning caused by GNSS signal interference in industrial factory areas.

[0038] 3. The method of the present invention uses multi-sensor data such as GNSS antennas, lidar, and inertial sensors for fusion, improving the accuracy and robustness of environmental perception, and can meet the high-precision navigation and positioning requirements of the carrier in the complex and changeable factory area environment. Description of the Drawings

[0039] Figure 1 is a flowchart of the present invention;

[0040] Figure 2 is a schematic diagram of multiple positioning trajectories in a complex factory area environment in an embodiment of the present invention;

[0041] Figure 3 is the error estimation of the positioning in a complex factory area environment in an embodiment of the present invention;

[0042] Figure 4 It is a partial schematic diagram of the high-precision map finally constructed in the embodiment of the present invention;

[0043] Figure 5 It is a schematic diagram of the relocalization process when performing step S6 in the embodiment of the present invention. Specific embodiments

[0044] The technical solution of the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments.

[0045] First, a GNSS antenna, a LiDAR sensor, and an IMU inertial measurement unit are fixedly installed on a carrier running in a factory area without a built map, and it is necessary to ensure that their relative positions do not change during the movement of the carrier to avoid the change of the relative position relationship of each sensor during the movement of the carrier, which affects the accuracy of subsequent data processing, mapping, and positioning. The carrier can be an autonomous vehicle or a logistics robot.

[0046] As Figure 1 shown, the multi-source fusion SLAM positioning method for factory area autonomous driving of the present invention can be used for the above-mentioned carrier, including the following steps:

[0047] S1, calibrate multiple sensors:

[0048] For the GNSS antenna, LiDAR sensor, and IMU inertial measurement unit on a carrier running in a factory area without a built map, calibrate the GNSS antenna and LiDAR sensor to the center of the IMU inertial measurement unit to obtain calibration extrinsic parameters; the calibration extrinsic parameters are used to conveniently unify them to the global coordinate system after each sensor obtains data. The global coordinate system is a coordinate system with the center of the IMU inertial measurement unit as the origin.

[0049] S2, correct the motion distortion of the laser point cloud data:

[0050] For each frame of laser point cloud data and IMU data obtained by the LiDAR sensor, since the carrier is in a moving state when the LiDAR sensor collects data, the collected laser point cloud data has motion distortion, and it is necessary to forward-propagate according to the corresponding data of the IMU to obtain R(t i ) and T(t i ), and correct the motion distortion of the laser points at different times, specifically including the following sub-steps:

[0051] S21, obtain the rotation matrix and translation vector, as shown in the following formula:

[0052]

[0053] Among them, t iis the acquisition time of the i-th laser point collected by the LiDAR sensor, where i is the laser point number; R(t i ) is the rotation matrix of the LiDAR sensor relative to the LiDAR sensor coordinate system at time t i ; T(t i ) is the translation vector of the LiDAR sensor relative to the LiDAR sensor coordinate system at time t i ; is the motion speed of the carrier at the start time of the point cloud frame where the i-th laser point is collected; a and ω are the observed data of the acceleration and angular acceleration of the IMU inertial measurement unit; Δt is the time interval from the start to the end of the point cloud frame where the i-th laser point is collected; and exp are both operators of Lie groups and Lie algebras.

[0054] S22, correct the motion distortion of the laser point cloud data:

[0055] Backpropagate the result obtained by S21 to the laser point cloud data of the corresponding frame to obtain the undistorted laser point cloud data, and there is the following formula:

[0056] P i ′ = R(t i ) -1 ·(P i - T(t i ));

[0057] where P i is the position of the i-th laser point obtained by the LiDAR sensor, and P i ′ is the position of the i-th laser point in the LiDAR sensor coordinate system.

[0058] S23, convert the position of the laser point P i ′ to the global coordinate system according to the calibrated external parameters obtained by S1 to obtain the point cloud data in the global coordinate system.

[0059] S3, map matching:

[0060] S31, construct the distance residual between points and planes from the point cloud data in the global coordinate system obtained by S2;

[0061] S32, use iterative Kalman filtering to optimize the distance residual obtained by S31 to obtain the state vector of the carrier during motion; the state vector includes position, speed, attitude, deviation, and gravity state quantities;

[0062] S33. Transform the coordinate system of the point cloud data in the global coordinate system obtained in S2 using the state vector obtained in S32 to obtain the point cloud optimized at the backend in the local coordinate system.

[0063] S34. Store the point cloud optimized at the backend obtained in S33 in the kd-tree data format to obtain the LIO point cloud map.

[0064] The LIO represents the radar inertial odometer data obtained by fusing the data of the LiDAR sensor and the IMU inertial measurement unit through step S3.

[0065] S4. Obtain the global LIO point cloud map:

[0066] S41. Use the IMU data and GNSS data in the global coordinate system for integrated navigation to obtain the result of the tight coupling of GNSS-RTK and INS. Among them, INS represents the inertial navigation system, and RTK represents real-time kinematic differential; the GNSS data in the global coordinate system is calibrated by using the calibration extrinsic parameters in S1 for the data obtained by the GNSS antenna.

[0067] S42. Add the result of the tight coupling obtained in S41 as an optimization factor to the factor graph, and update the global positions of all the LIO point cloud maps obtained in S3 through the constraints of the factor graph to obtain the LIO point cloud map in the global coordinate system.

[0068] S5. Establish a high-precision point cloud map:

[0069] The LIO point cloud map in the global coordinate system obtained by publishing S4. When the vehicle runs in the factory area, it often passes through repeated sections. When the vehicle passes through or the sensor scans a repeated path, the feature association between the current frame point cloud and the global point cloud map is detected in real time to establish a high-precision point cloud map. Specifically: Use a single or multiple Scans scanned by the LiDAR sensor at the current time, accumulate them into a Submap, and perform loop closure detection adaptively according to the Submap-to-Map matching. Then, optimize the loop closure detection and the LIO point cloud in the global coordinate system by constructing a stable triangle descriptor to complete the establishment of the final high-precision point cloud map. For the detailed process of the loop closure detection, see C. Yuan, J. Lin, Z. Zou, X. Hong and F. Zhang, "STD: Stable Triangle Descriptor for 3D place recognition," 2023 IEEE International Conference on Robotics and Automation (ICRA), London, United Kingdom, 2023, pp. 1897-1903, doi: 10.1109 / ICRA48891.2023.10160413.

[0070] S6, perform relocalization:

[0071] In the factory area, when the vehicle runs in the area where the final high-precision point cloud map has been established, the position of the vehicle at the current moment under the high-precision point cloud map is obtained by taking the result of the radar inertial odometer after the tight coupling of the LiDAR sensor and the IMU inertial measurement unit, and taking a single or multiple Scans near the vehicle for Submap-to-Map matching to complete the relocalization of the vehicle.

[0072] Embodiment

[0073] Use a vehicle equipped with a GNSS antenna, a LiDAR sensor, and an IMU inertial measurement unit to run in the factory area. After positioning using the method of the present invention, multiple positioning trajectories are generated after the vehicle runs in the area multiple times, as shown in Figure 2 the upper left part, specifically:

[0074] Two trajectories generated by running two laps along the factory area route are used as one positioning trajectory (i.e., location_trajectory 1 to location_trajectory_5); Figure 2The following trajectory comparison is an enlarged view of the marked position in the factory area positioning trajectory; through the trajectory comparison, it can be found that the gap between the two positioning trajectories is the smallest when running for the 5th time, indicating that with the passage of the carrier running time, the positioning accuracy will become more and more accurate.

[0075] The error evaluation of the above factory area positioning is as Figure 3 shown, where:

[0076] APE is the absolute trajectory error, that is, the absolute error between the calculated trajectory of this method and the true trajectory;

[0077] Rmse is the root mean square error, which represents the square root of the average of the squares of the errors between the estimated trajectory and the true trajectory;

[0078] median is the median of the trajectory error;

[0079] mean is the mean of the absolute trajectory error;

[0080] std is the standard deviation of the absolute trajectory error;

[0081] From the error evaluation, the method of the present invention has low error, and can be within an error of 0.05m most of the time and maintain stability. The error curve is basically stable, with only a small amount of local fluctuations and small amplitudes. This shows that the method proposed in this application has strong stability during the positioning process, can continuously maintain a low error level, and will not have large fluctuations or sudden positioning errors.

[0082] It is proved by experiments that the present invention has superior navigation performance in the environment facing the factory area compared with the traditional GNSS+RTK positioning method.

[0083] The final high-precision point cloud map obtained through step S5 is as attached Figure 4 shown. By analyzing the laser sensor data and fusing the GNSS absolute position information, an accurate three-dimensional environment model is constructed to support navigation decisions.

[0084] The relocalization process through step S6 is as Figure 5 shown. By continuously running the carrier, the position of the carrier at the current moment under the high-precision point cloud map is obtained.

Claims

1. A multi-source fusion SLAM positioning method for factory area autonomous driving, characterized in that: The following steps are involved: S1, calibrating the GNSS antenna, LiDAR sensor and IMU inertial measurement unit fixedly mounted on the carrier to obtain calibration external parameters; S2, correcting the motion distortion of the laser point cloud data collected by the LiDAR sensor, and then obtaining the point cloud data in the global coordinate system by calibrating the external parameters; S3, map matching, constructing a LIO point cloud map; the LIO represents the radar inertial odometer data obtained by fusing the data of the LiDAR sensor and the IMU inertial measurement unit in step S3; S4, obtain the global LIO point cloud map: The data acquired by the IMU inertial measurement unit in the global coordinate system and the data acquired by the GNSS antenna are used for combined navigation to obtain the result of tight coupling between GNSS-RTK and INS, which is then added as an optimization factor to the factor graph, and the global positions of all LIO point cloud maps obtained by S3 are updated through the constraints of the factor graph to obtain the LIO point cloud map in the global coordinate system; S5, build a high-precision point cloud map: When the carrier passes by or the LiDAR sensor scans a repeated path, the feature association between the current frame point cloud and the LIO point cloud map in the global coordinate system is detected in real time to establish a high-precision point cloud map. S6, relocation: When the carrier is running in the area where the final high-precision point cloud map has been established, the radar inertial odometer result at the current moment after the LiDAR sensor and the IMU inertial measurement unit are tightly coupled at the current moment, and the Scan near the carrier in the radar inertial odometer result at the current moment is taken for Submap-to-Map matching to obtain the current position of the carrier under the high-precision point cloud map, thereby completing the relocation of the carrier.

2. The multi-source fusion SLAM positioning method for factory area automatic driving according to claim 1 is characterized in that: S1 is specifically as follows: for the GNSS antenna, LiDAR sensor and IMU inertial measurement unit on the carrier running in the unmapped factory area, the GNSS antenna and the LiDAR sensor are calibrated to the center of the IMU inertial measurement unit to obtain the calibration extrinsic parameters; the calibration extrinsic parameters are used to unify the data of each sensor into the global coordinate system after the data are acquired.

3. The multi-source fusion SLAM positioning method for factory area automatic driving according to claim 2 is characterized in that: The global coordinate system is a coordinate system with the center of the IMU inertial measurement unit as the origin.

4. The multi-source fusion SLAM positioning method for factory area automatic driving according to claim 1 is characterized in that: Step S2 includes the following sub-steps: S21, obtain the rotation matrix and translation vector, as shown in the following formula: Among them, t i is the time when the LiDAR sensor collects the i-th laser point, where i is the number of the laser point; R(t i ) is t i The rotation matrix of the LiDAR sensor at time relative to the LiDAR sensor coordinate system; T(t i ) is t i The translation vector of the LiDAR sensor at the moment relative to the LiDAR sensor coordinate system; v ti-1 is the velocity of the carrier at the start time of collecting the point cloud frame where the i-th laser point is located; a and ω are the observation data of the acceleration and angular acceleration of the IMU inertial measurement unit; Δt is the time interval from the start to the end of collecting the point cloud frame where the i-th laser point is located; and exp are operators of Lie groups and Lie algebras; S22, correct the motion distortion of laser point cloud data: The result obtained in S21 is then back-propagated to the laser point cloud data of the corresponding frame to obtain the dedistorted laser point cloud data, which is as follows: P i ′=R(t i ) -1 ·(P i -T(t i )); Among them, P i is the position of the i-th laser point acquired by the LiDAR sensor, P i ′ is the position of the i-th laser point in the LiDAR sensor coordinate system; S23, according to the calibration external parameter obtained in S1, P i The position of ′ is transformed into the global coordinate system to obtain the point cloud data in the global coordinate system.

5. The multi-source fusion SLAM positioning method for factory area automatic driving according to claim 1 is characterized in that: Step S3 includes the following sub-steps: S31, constructing the distance residual of the point surface through the point cloud data in the global coordinate system obtained in S2; S32, using iterative Kalman filtering to optimize the distance residual obtained in S31 to obtain a state vector of the carrier when in motion; S33, performing coordinate system transformation on the point cloud data in the global coordinate system obtained in S2 by using the state vector obtained in S32, so as to obtain a point cloud after back-end optimization in the local coordinate system; S34, storing the backend optimized point cloud obtained in S33 in a kd-tree data format to obtain a LIO point cloud map; the LIO represents the radar inertial odometer data obtained by fusing the data of the LiDAR sensor and the IMU inertial measurement unit in step S3.

6. The multi-source fusion SLAM positioning method for factory area automatic driving according to claim 5 is characterized in that: The state vector in step S32 includes position, velocity, attitude, deviation and gravity state quantity.

7. The multi-source fusion SLAM positioning method for factory area autonomous driving according to claim 5 is characterized in that: S5 specifically uses a single or multiple scans scanned by the LiDAR sensor to accumulate the submaps, and adaptively performs loop detection based on the submap-to-map matching; then optimizes the loop detection and the LIO point cloud in the global coordinate system by constructing a stable triangle descriptor to establish a high-precision point cloud map.

8. The multi-source fusion SLAM positioning method for factory area automatic driving according to claim 1 is characterized in that: The carrier is an unmanned vehicle or a logistics robot.

Images