Multi-source fusion SLAM (Simultaneous Localization and Mapping) positioning method for factory automatic driving

Abstract

The invention discloses a multi-source fusion SLAM positioning method for factory automatic driving, and the method comprises the following steps: S1, calibrating a GNSS antenna, a LiDAR sensor and an IMU inertial measurement unit which are fixedly disposed on a carrier, and obtaining calibration external parameters; s2, correcting motion distortion of laser point cloud data collected by using the LiDAR sensor, and obtaining point cloud data under a global coordinate system through the calibration external parameters; s3, performing map matching; s4, acquiring a global LIO point cloud map; s5, establishing a high-precision point cloud map; and S6, repositioning is carried out. According to the method, data of multiple sensors such as the GNSS antenna, the laser radar and the inertial sensor are fused, so that the accuracy and the robustness of sensing the environment of the factory area are improved, and the automatic driving high-precision navigation positioning requirement of a carrier in a complex and changeable factory area environment is met.

Description

Technical Field

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

[0002] In the complex environment of industrial plants, navigation systems must be able to provide high-precision positioning and real-time map building capabilities. However, traditional global navigation satellite systems (GNSS), such as Beidou and GPS, provide real-time positioning services, but their accuracy can only reach the meter level, which is far from enough to meet the strict requirements for accuracy in industrial applications. It is difficult to achieve efficient and accurate map building and positioning, and provide stable and reliable navigation for the carrier. Existing satellite navigation systems rely on ground-based augmentation technology to achieve real-time centimeter-level high-precision positioning, but this method is difficult to achieve continuous high-precision positioning in an environment such as an industrial plant where signals are easily interfered with and blocked.

[0003] High-precision and high-reliability positioning is the core key technology of robot navigation. The reliability of a single GNSS sensor is low and it is impossible to achieve high-precision positioning in all scenarios. It is necessary to adopt precise positioning technology with multi-source sensor fusion processing. As a high-precision sensor, LiDAR can provide accurate coordinate information and build dense point cloud maps without being restricted by occlusion and lighting conditions. The SLAM (Simultaneously Localization and Mapping) technology based on LiDAR has been relatively mature in theory, technology and product application because of its early development. It is widely used in fields such as autonomous driving and robot navigation, allowing mobile carriers to simultaneously locate themselves and build environmental maps in unknown environments based on sensor observation information. However, the positioning accuracy in complex environments is still insufficient, which is a difficult problem that needs to be solved urgently in the field of mobile carrier navigation and positioning technology. Summary of the invention

[0004] In order to solve the problems existing in the background technology, 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] To this end, the present invention adopts the following technical solutions:

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

[0007] S1, calibrating the GNSS antenna, LiDAR sensor and IMU inertial measurement unit fixedly mounted on the carrier to obtain calibration external parameters;

[0008] 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;

[0009] 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;

[0010] S4, obtain the global LIO point cloud map:

[0011] 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;

[0012] S5, build a high-precision point cloud map:

[0013] 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.

[0014] S6, relocation:

[0015] 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.

[0016] 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.

[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] Among them, t i is the time when the LiDAR sensor collects the i-th laser point, 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; 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;

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

[0023] 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:

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

[0025] 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;

[0026] S23, according to the calibration external parameter obtained in S1, P i ′ The position is transformed into the global coordinate system to obtain the point cloud data in the global coordinate system.

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

[0028] S31, constructing the distance residual of the point surface through the point cloud data in the global coordinate system obtained in S2;

[0029] S32, using iterative Kalman filtering to optimize the distance residual obtained in S31 to obtain a state vector of the carrier when in motion;

[0030] 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;

[0031] 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.

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

[0033] 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.

[0034] Preferably, the carrier is an unmanned 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 with the assistance of point cloud and IMU data when the GNSS position error is large or the position is lost in the complex environment of the factory.

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

[0038] 3. The method of the present invention uses multi-sensor data such as GNSS antenna, lidar, inertial sensor, etc. to fuse, which improves the accuracy and robustness of environmental perception and can meet the requirements of high-precision navigation and positioning of autonomous driving of the carrier in complex and changeable factory environments. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

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

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

[0043] Figure 5 Schematic diagram of the relocation process when performing step S6 in an embodiment of the present invention. DETAILED DESCRIPTION

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

[0045] First, the GNSS antenna, LiDAR sensor and IMU inertial measurement unit are fixedly installed on the carrier running in the unmapped factory area, and their relative positions must not change when the carrier moves, so as to avoid the relative position relationship of each sensor changing during the movement of the carrier and affecting the accuracy of subsequent data processing, mapping and positioning. The carrier can be an unmanned vehicle or a logistics robot.

[0046] like Figure 1 As 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 the carrier running in the unmapped factory area, the GNSS antenna and LiDAR sensor are calibrated to the center of the IMU inertial measurement unit to obtain the calibration external parameters; the calibration external parameters are used to unify the data of each sensor into the global coordinate system after acquiring them. The global coordinate system is the coordinate system with the center of the IMU inertial measurement unit as the origin.

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

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

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

[0052]

[0053] Among them, t iis the time when the LiDAR sensor collects the i-th laser point, 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; 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.

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

[0055] The result obtained by 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:

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

[0057] 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.

[0058] S23, according to the calibration external parameters obtained in S1, the laser point P i ′ The position is transformed into the global coordinate system to obtain the point cloud data in the global coordinate system.

[0059] S3, map matching:

[0060] S31, constructing the distance residual of the point surface through the point cloud data in the global coordinate system obtained in S2;

[0061] S32, using iterative Kalman filtering to optimize the distance residual obtained in S31, to obtain a state vector of the carrier when in motion; the state vector includes position, velocity, attitude, deviation and gravity state;

[0062] 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;

[0063] S34, storing the backend-optimized point cloud obtained in S33 in a kd-tree data format to obtain a 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 in step S3.

[0065] S4, obtain the global LIO point cloud map:

[0066] S41, using the IMU data and GNSS data in the global coordinate system for combined navigation to obtain the result of GNSS-RTK and INS tight coupling. Among them, INS represents the inertial navigation system, and RTK represents real-time dynamic difference; the GNSS data in the global coordinate system is obtained by calibrating the data obtained by the GNSS antenna using the calibration external parameters in S1.

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

[0068] S5, build a high-precision point cloud map:

[0069] The LIO point cloud map in the global coordinate system obtained by S4 is published. When the carrier runs in the factory, it will often pass through repeated sections. When the carrier passes 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, the single or multiple scans scanned by the LiDAR sensor at the current time are accumulated into Submaps, and loop detection is adaptively performed based on the Submap-to-Map matching. Then, by constructing a stable triangle descriptor to optimize the loop detection and the LIO point cloud in the global coordinate system, the final high-precision point cloud map is established. For the detailed process of the loop 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, relocation:

[0071] Within the factory, when the carrier is operating in an area where the final high-precision point cloud map has been established, the radar inertial odometer results of the tightly coupled LiDAR sensor and IMU inertial measurement unit are used to take a single or multiple Scans near the carrier for Submap-to-Map matching to obtain the current position of the carrier under the high-precision point cloud map, thereby completing the repositioning of the carrier.

[0072] Example

[0073] A carrier equipped with a GNSS antenna, a LiDAR sensor, and an IMU inertial measurement unit is used to operate in a factory area. After positioning using the method of the present invention, the carrier generates multiple positioning tracks after multiple operations in the area, such as Figure 2 As shown in the upper left part, specifically:

[0074] The two trajectories generated by running two circles along the factory route are used as the primary positioning trajectory (i.e. location_trajectory 1 to location_trajectory_5); Figure 2The trajectory comparison below is an enlarged view of the marked position in the factory positioning trajectory. Through the trajectory comparison, it can be found that when running for the fifth time, the difference between the two positioning trajectories is the smallest, indicating that as the carrier runs longer, the positioning accuracy will become more and more accurate.

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

[0076] APE is the absolute trajectory error, that is, the absolute error between the trajectory calculated by 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 a low error, and most of the time it can be within the error of 0.05m and can remain stable. The error curve is basically stable, with only a small amount of local fluctuations, and the amplitude is small. This shows that the method proposed in this application has strong stability in the positioning process, can continuously maintain a low error level, and will not have large fluctuations or sudden positioning errors.

[0082] Experiments have shown that, compared with the traditional GNSS+RTK positioning method, the present invention has achieved better navigation performance in a factory environment.

[0083] The final high-precision point cloud map obtained by step S5 is shown in the attached figure. Figure 4 As shown in the figure, by analyzing laser sensor data and fusing GNSS absolute position information, an accurate 3D environment model is constructed to support navigation decisions.

[0084] Through the relocation process of step S6, Figure 5 As shown, the position of the carrier at the current moment under the high-precision point cloud map is obtained through the continuous operation of the carrier.

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, 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