Full-scene multi-source fusion positioning method, system and device for unmanned operation equipment

Abstract

The invention discloses a full-scene multi-source fusion positioning method, system and device for unmanned operation equipment, and relates to the technical field of full-scene positioning in automatic driving of the unmanned operation equipment. According to the method, positioning sources comprise a GNSS, a laser LiDAR and an IMU, a set threshold value is compared according to an output result of the GNSS in the initialization and operation process of a system, scene recognition is achieved, corresponding positioning sources are fused according to different scenes, and weights of the positioning sources are judged according to confidence coefficients of the positioning sources to obtain a fused positioning result. According to the method, at least two types of positioning sources are fused in indoor, outdoor and indoor and outdoor transition stages, the positioning precision of the unmanned operation equipment is improved, the indoor and outdoor scene switching logic is optimized, and smooth switching of positioning scenes of the unmanned operation equipment and indoor and outdoor seamless positioning are achieved.

Description

Technical Field

[0001] The present invention belongs to the field of navigation and positioning technology, and specifically relates to a full-scene multi-source fusion positioning method, system and device for unmanned operating equipment. Background Art

[0002] Unmanned equipment, when autonomously navigating and positioning in complex environments, faces the challenge of a single positioning source being insufficient. To address this issue, multi-positioning source fusion technology has been proposed to compensate for the shortcomings of single-sensor state estimation. However, blindly increasing the number of sensors requires excessive processor computing power, impacting the real-time performance of positioning data. Furthermore, in certain scenarios, some positioning sources may provide poor positioning results, and fusion without weighting can lead to unexpected problems. Furthermore, when unmanned equipment switches between indoor and outdoor environments, hard switching between different positioning sources can create jumps at the switchover point, causing significant interference. Summary of the Invention

[0003] To solve the above technical problems, the present invention provides a method, system and device for multi-source fusion positioning of unmanned operating equipment in all scenarios. It uses multiple positioning sources to realize scene recognition, performs fusion positioning of corresponding positioning sources in different scenarios, and takes weight distribution into consideration at the same time to realize seamless indoor and outdoor positioning of unmanned operating equipment in all scenarios.

[0004] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0005] A full-scene multi-source fusion positioning method for unmanned equipment, the method comprising:

[0006] Step 1: Determine whether the current scene is an outdoor scene based on whether the positioning result output by the positioning source GNSS is an RTK fixed solution; if it is an outdoor scene, use GNSS RTK and IMU for fusion positioning; if not, proceed to step 2;

[0007] Step 2: Determine whether the LiDAR is in normal working condition based on whether the point cloud density collected by the LiDAR is greater than the first threshold of the LiDAR. If it is in normal working condition, proceed to step 3.

[0008] Step 3: Determine whether the unmanned operation equipment is currently in an indoor scene or in an indoor-outdoor transition phase based on whether the number of satellites in the positioning result output by the positioning source GNSS is greater than the GNSS first threshold. If it is in an indoor scene, use laser LiDAR and IMU fusion positioning. If it is in an indoor-outdoor transition phase, proceed to step 4.

[0009] Step 4: During the indoor-outdoor transition phase, fusion positioning is performed using three types of positioning sources: GNSS, IMU, and laser LiDAR. By calculating the confidence of each positioning source and assigning corresponding weights, the extended Kalman filter algorithm is used to obtain the fusion positioning results of the indoor-outdoor transition phase.

[0010] In another aspect, the present invention provides a full-scene multi-source fusion positioning system for unmanned equipment, comprising:

[0011] A first judgment unit is configured to determine whether the scene is an outdoor scene based on whether the positioning result output by the positioning source GNSS is an RTK fixed solution. If the scene is an outdoor scene, the first execution unit performs fusion positioning using GNSS RTK and IMU. If the scene is not an outdoor scene, the second judgment unit is executed;

[0012] a second judgment unit, configured to determine whether the laser LiDAR is in a normal working state according to whether the point cloud density collected by the laser LiDAR is greater than a first threshold of the LiDAR, and if it is in a normal working state, execute the third judgment unit;

[0013] The third judgment unit is used to determine whether the unmanned operation equipment is currently in an indoor scene or an indoor-outdoor transition stage based on whether the number of satellites in the positioning result output by the positioning source GNSS is greater than the first GNSS threshold. If it is in an indoor scene, the second execution unit uses laser LiDAR and IMU fusion positioning; if it is in the indoor-outdoor transition stage, the third execution unit uses three types of positioning sources, GNSS, IMU and laser LiDAR, for fusion positioning to obtain the indoor-outdoor transition stage positioning result.

[0014] In the third aspect, the present invention provides a full-scene multi-source fusion positioning device for unmanned operating equipment, the device including a processor and a memory, the memory being used to store a computer operation program; when the processor executes the computer operation program, it can implement the aforementioned full-scene multi-source fusion positioning method for unmanned operating equipment.

[0015] The beneficial effects of the present invention are:

[0016] GNSS positioning enables high-precision continuous positioning in outdoor scenarios, and scene recognition based on GNSS output. Fusion with an IMU ensures high-frequency positioning output, ensuring real-time response in dynamic scenarios. LiDAR maintains high-precision positioning indoors through point cloud matching. The integration of GNSS, IMU, and LiDAR positioning sources enables high-precision positioning indoors and outdoors, as well as in all scenarios, meeting the requirements for seamless positioning of unmanned equipment in all scenarios, both indoors and outdoors. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1This is a flow chart of a full-scene multi-source fusion positioning method for unmanned equipment according to the present invention;

[0018] Figure 2 This is a structural diagram of a full-scene multi-source fusion positioning system for unmanned equipment of the present invention;

[0019] Figure 3 This is a structural diagram of a full-scene multi-source fusion positioning device for unmanned operation equipment of the present invention;

[0020] Figure 4 This is a diagram of the actual working trajectory of unmanned equipment. DETAILED DESCRIPTION

[0021] The present invention will be further described below with reference to the accompanying drawings and embodiments, with unmanned agricultural machinery being taken as an example for its specific implementation.

[0022] Figure 1 This is a flow chart of a full-scene multi-source fusion positioning method for unmanned equipment provided by the present invention. Specifically, the positioning sources included in this method include GNSS (Global Navigation Satellite System), laser LiDAR, and IMU (Inertial Measurement Unit). The method includes:

[0023] Step 1: After the positioning source is initialized, determine whether the scene is outdoor based on whether the positioning result output by the positioning source GNSS is an RTK (Real-time kinematic) fixed solution. If the positioning result output by the positioning source GNSS is an RTK fixed solution, it is determined to be an outdoor scene. GNSS RTK and IMU can be used for fusion positioning. The fusion method can be loose coupling, tight coupling, etc., taking advantage of the high precision of RTK positioning technology and the high frequency characteristics of IMU to obtain outdoor high-precision and high-frequency integrated navigation positioning results.

[0024] If the positioning result output by the positioning source GNSS is not an RTK fixed solution, it is determined that the scene is not outdoor and proceed to step 2.

[0025] Step 2: Determine whether the laser LiDAR is in normal working condition based on whether the point cloud density collected by the laser LiDAR is greater than the first LiDAR threshold. If the collected point cloud density is less than or equal to the first LiDAR threshold, it is determined that the laser LiDAR is in an abnormal working condition, and an alarm message is printed to the user, prompting the user to check the system status; if the collected point cloud density is greater than the first LiDAR threshold, it is determined that the laser LiDAR is in normal working condition, and then proceed to step 3.

[0026] Step 3: Determine whether the unmanned equipment is currently in an indoor scene or an indoor-outdoor transition stage based on whether the number of satellites in the positioning result output by the positioning source GNSS is greater than the GNSS first threshold. If the number of satellites is less than or equal to the GNSS first threshold, it is determined to be an indoor scene, and laser LiDAR and IMU fusion positioning are used to obtain indoor positioning results; if the number of satellites is greater than the GNSS first threshold, it is determined to be an indoor-outdoor transition stage, and proceed to step 4. Preferably, the GNSS first threshold can be set to 4.

[0027] Laser LiDAR fusion with IMU can eliminate the distortion caused by carrier motion, and its fusion method adopts tight coupling.

[0028] Step 4: During the indoor-outdoor transition phase, the three types of positioning sources, GNSS, IMU, and LiDAR, are integrated for positioning to obtain the positioning results for the indoor-outdoor transition phase.

[0029] The extended Kalman state equation and observation equation for multi-positioning source information fusion are constructed as follows:

[0030] ,

[0031] Where, for The state vector at the moment, for The state vector at the moment, is the state transition matrix, is the input control matrix, Input control vector, is the process noise vector, For the Positioning sources in The observation vector at time t, For the Positioning sources in The system state observation matrix at time , For the Positioning sources in The observation noise vector at time t.

[0032] Let the state vector ,

[0033] Let the observation vector ,

[0034] Where, To fuse the positioning coordinates, is the position coordinate solved by IMU, Calculate position coordinates for GNSS positioning, Calculate position coordinates for LiDAR positioning.

[0035] The state equation and observation equation are:

[0036]

[0037]

[0038] Where, for Fusion positioning coordinates at all times, for IMU positioning position coordinates at this moment, for GNSS positioning position coordinates at the moment, for LIDAR positioning position coordinates at the moment, and IMU in and The weight of the direction on the fusion positioning result, and GNSS in and The weight of the direction on the fusion positioning result, and LADAR and The weight of the direction on the fusion positioning result, for Always and Observation noise in direction; To fuse the positioning coordinates, for and Direction control input.

[0039] Judging the weight of the positioning source in the fusion system according to its confidence can weaken the impact of undesirable positioning sources on the overall positioning effect. The confidence matrix As follows:

[0040] ,

[0041] Confidence Matrix Elements in ,in 、 、 Respectively Positioning result of the positioning source, The positioning results of the positioning sources and the average positioning results of all three positioning sources are used to calculate the confidence matrix Perform column normalization, then sum each row and normalize again to get the confidence vector , It is the confidence of the first positioning source for the positioning result. and The influence of the direction positioning result is equally important, i.e. , , .

[0042] The weights calculated based on the confidence level of the positioning source have been determined, and the state transfer matrix is for:

[0043] ,

[0044] After determining the covariance matrix of the state vector and the observation vector, substituting them into the extended Kalman filter state equation and observation equation can obtain the optimal estimate, thereby obtaining the final fusion positioning result.

[0045] Thus, step 4 completes the multi-source fusion positioning in the indoor-outdoor transition stage.

[0046] After the fusion positioning results are obtained in steps 1, 3, and 4, the process returns to the judgment in step 1 and executes steps 1 to 4.

[0047] The positioning method performs arm measurement between the GNSS antenna and the IMU, intrinsic calibration of the IMU and laser LiDAR, and extrinsic calibration of the relative centers of the IMU and laser LiDAR during the positioning source installation phase. Initial alignment of multiple positioning sources, i.e., time synchronization and spatial alignment, is completed during the initialization phase.

[0048] On the other hand, Figure 2 As shown, the present invention provides a full-scene multi-source fusion positioning system for unmanned equipment, including:

[0049] A first judgment unit is configured to determine whether the scene is an outdoor scene based on whether the positioning result output by the positioning source GNSS is an RTK fixed solution. If the scene is an outdoor scene, the first execution unit performs fusion positioning using GNSS RTK and IMU. If the scene is not an outdoor scene, the second judgment unit is executed;

[0050] a second judgment unit, configured to determine whether the laser LiDAR is in a normal working state according to whether the point cloud density collected by the laser LiDAR is greater than a first threshold of the LiDAR, and if it is in a normal working state, execute the third judgment unit;

[0051] The third judgment unit is used to determine whether the unmanned operation equipment is currently in an indoor scene or an indoor-outdoor transition stage based on whether the number of satellites in the positioning result output by the positioning source GNSS is greater than the first GNSS threshold. If it is in an indoor scene, the second execution unit uses laser LiDAR and IMU fusion positioning; if it is in the indoor-outdoor transition stage, the third execution unit uses three types of positioning sources, GNSS, IMU and laser LiDAR, for fusion positioning to obtain the indoor-outdoor transition stage positioning result.

[0052] Preferably, a user interaction unit may also be included, for transmitting each positioning result to a user interaction interface for real-time display.

[0053] Thirdly, as Figure 3 As shown, the present invention provides a full-scene multi-source fusion positioning device for unmanned operating equipment, which includes a core processor and a memory, and the memory is used to store a computer operation program; when the processor executes the computer operation program, the aforementioned full-scene multi-source fusion positioning method for unmanned operating equipment is implemented, wherein GNSS data is obtained by a satellite antenna, and laser LiDAR data is directly sent to the processor.

[0054] The actual working trajectory of unmanned equipment is as follows Figure 4 As shown, the black rectangle represents the hangar, the solid black line is the planned trajectory, the dashed gray line is the actual operating trajectory, and the X and Y axes represent the relative position on the plane. It can be seen that the positioning is highly accurate in both indoor and outdoor scenes, as well as in the indoor-outdoor switching phase, with no position jumps during the indoor-outdoor switching phase.

[0055] The specific embodiments described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above are only specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A full-scene multi-source fusion positioning method for unmanned equipment, characterized by: The method comprises: Step 1: Determine whether the current scene is an outdoor scene based on whether the positioning result output by the positioning source GNSS is an RTK fixed solution; if it is an outdoor scene, use GNSS RTK and IMU for fusion positioning; if not, proceed to step 2; Step 2: Determine whether the LiDAR is in normal working condition based on whether the point cloud density collected by the LiDAR is greater than the first threshold of the LiDAR. If it is in normal working condition, proceed to step 3. Step 3: Determine whether the unmanned operation equipment is currently in an indoor scene or in an indoor-outdoor transition phase based on whether the number of satellites in the positioning result output by the positioning source GNSS is greater than the GNSS first threshold. If it is in an indoor scene, use laser LiDAR and IMU fusion positioning. If it is in an indoor-outdoor transition phase, proceed to step 4. Step 4: During the indoor-outdoor transition phase, fusion positioning is performed using three types of positioning sources: GNSS, IMU, and laser LiDAR. By calculating the confidence of each positioning source and assigning corresponding weights, the extended Kalman filter algorithm is used to obtain the fusion positioning results of the indoor-outdoor transition phase.

2. The method for full-scene multi-source fusion positioning of unmanned equipment according to claim 1 is characterized in that: In step 1, if the positioning result output by the positioning source GNSS is an RTK fixed solution, it is determined to be an outdoor scene, and GNSS RTK and IMU are used for fusion positioning; If the positioning result output by the positioning source GNSS is not an RTK fixed solution, it is determined that the scene is not an outdoor scene.

3. The full-scene multi-source fusion positioning method for unmanned equipment according to claim 1 is characterized in that: In step 2, if the collected point cloud density is less than or equal to the first LiDAR threshold, it is determined that the laser LiDAR is in an abnormal working state; if the collected point cloud density is greater than the first LiDAR threshold, it is determined that the laser LiDAR is in a normal working state.

4. The method for full-scene multi-source fusion positioning of unmanned equipment according to claim 1 is characterized in that: In step 3, if the number of satellites is less than or equal to the first GNSS threshold, it is determined to be an indoor scene and laser LiDAR and IMU fusion positioning is used; if the number of satellites is greater than the first GNSS threshold, it is determined to be an indoor-outdoor transition stage.

5. The full-scene multi-source fusion positioning method for unmanned equipment according to claim 1 is characterized in that: The step 4 comprises: Step 4.1: Calculate the weight of the positioning source in the fusion system based on its confidence level; Step 4.2: Substitute the weights of each positioning source into the state transfer matrix in the extended Kalman filter equation to obtain the fused positioning result.

6. The method for full-scene multi-source fusion positioning of unmanned equipment according to claim 5 is characterized in that: The step 4.1 includes calculating the confidence matrix of each positioning source, performing column normalization on the confidence matrix, and then summing and normalizing each row to obtain a confidence vector. The confidence vector is combined with the positioning source and the positioning target to obtain a weight in the fusion system.

7. The method for full-scene multi-source fusion positioning of unmanned equipment according to claim 6 is characterized in that: The step 4.2 includes constructing an extended Kalman filter equation, bringing the weights obtained by the positioning source according to the confidence into the state transfer equation, and obtaining the optimal fusion positioning result through an iterative prediction-update process.

8. The method for full-scene multi-source fusion positioning of unmanned equipment according to claim 1 is characterized in that: After the fusion positioning results are obtained in steps 1, 3, and 4, the process returns to the judgment in step 1 and executes steps 1 to 4 again.

9. A full-scene multi-source fusion positioning system for unmanned equipment, characterized by: include: A first judgment unit is configured to determine whether the scene is an outdoor scene based on whether the positioning result output by the positioning source GNSS is an RTK fixed solution. If the scene is an outdoor scene, the first execution unit performs fusion positioning using GNSS RTK and IMU. If the scene is not an outdoor scene, the second judgment unit is executed; a second judgment unit, configured to determine whether the laser LiDAR is in a normal working state according to whether the point cloud density collected by the laser LiDAR is greater than a first threshold of the LiDAR, and if it is in a normal working state, execute the third judgment unit; The third judgment unit is used to determine whether the unmanned operation equipment is currently in an indoor scene or an indoor-outdoor transition stage based on whether the number of satellites in the positioning result output by the positioning source GNSS is greater than the first GNSS threshold. If it is in an indoor scene, the second execution unit uses laser LiDAR and IMU fusion positioning; if it is in the indoor-outdoor transition stage, the third execution unit uses three types of positioning sources, GNSS, IMU and laser LiDAR, for fusion positioning to obtain the indoor-outdoor transition stage positioning result.

10. A full-scene multi-source fusion positioning device for unmanned equipment, characterized by: The positioning device includes a processor and a memory, and the memory is used to store a computer operation program; when the processor executes the computer operation program, it can implement the full-scene multi-source fusion positioning method for unmanned operation equipment described in any one of claims 1-8.

Images