A GNSS and SLAM fusion positioning method for unmanned systems

Abstract

The present invention discloses a GNSS and SLAM fusion positioning method for unmanned systems. Through the linkage between the control module and the positioning module of the unmanned system, the positioning performance of the environment is evaluated in real time to instruct the unmanned system to adjust the control, so as to obtain an environment or condition that is more conducive to positioning, reduce the situation in which both GNSS and SLAM cannot provide effective positioning information in some areas under complex environments, improve the environmental adaptability of the fusion algorithm, and ensure the safe operation of the system.

Description

Technical Field

[0001] The present invention relates to the field of multi-source fusion positioning, and in particular to a GNSS and SLAM fusion positioning method for unmanned systems. Background Art

[0002] With the continuous advancement of technology and the gradual maturity of the market, unmanned systems will play an increasingly important role in all walks of life, and market demand will continue to grow, especially in the fields of intelligent driving, logistics, manufacturing, medical care, retail, etc., where unmanned systems are expected to become standard. Many of these application scenarios involve mixed indoor and outdoor scenarios, such as airports, train stations, or large commercial complexes. These scenarios usually involve unmanned systems in transition areas between indoor and outdoor environments, or in complex environments where indoor and outdoor environments are integrated. This places higher demands on the environmental adaptability, safety, and reliability of unmanned systems. In particular, its precise positioning capability requires seamless switching or integration of multiple positioning technologies, among which the combination of GNSS (Global Navigation Satellite System) and SLAM (Simultaneous Positioning and Mapping) technology provides a better solution. GNSS positioning uses satellites and is more suitable for open outdoor areas without obstructions. SLAM technology mainly relies on lidar or visual cameras to collect information about the surrounding environment and then build local maps and accurately estimate the pose. It is more suitable for scenes with rich texture features such as indoors, urban canyons or dense forests. The combination of the two allows GNSS to correct the accumulated errors of SLAM during long-term operation or large-scale movement, while SLAM technology can make up for the lack of GNSS positioning in signal-blocked or weak signal environments. The existing commonly used fusion frameworks, whether probability estimation, Kalman filtering or graph optimization methods, need to ensure that at least one of the lidar / visual images of GNSS or SLAM continues to provide sufficient effective information. However, the diversity and complexity of the actual application environment may still fail to meet the above conditions, which brings certain challenges to the fusion of GNSS and SLAM. Summary of the invention

[0003] The present invention provides a GNSS and SLAM fusion positioning method for unmanned systems. By combining the control information of the unmanned system, it effectively reduces the situation where GNSS and SLAM cannot provide effective positioning information in some areas under complex environments, improves the environmental adaptability of the fusion algorithm, and ensures the safe operation of the system. The method includes the following steps:

[0004] The unmanned system is powered on and the GNSS positioning module and SLAM positioning module are turned on;

[0005] Stay still for N seconds, and determine whether the GNSS positioning module or the SLAM positioning module outputs valid positioning information. If both are negative, it is considered that the environment is not suitable for the operation of the unmanned system, and the work is terminated;

[0006] Otherwise, the unmanned system starts to work and performs joint control positioning according to the output information of the current round of the GNSS positioning module and the SLAM positioning module, including:

[0007] (1) If only the SLAM positioning module outputs valid positioning information, the valid positioning information output by the SLAM positioning module is used for control, and it is determined whether the GNSS positioning module outputs valid satellite information. If so, a stationary / hovering request is sent to the control module, otherwise the unmanned system continues to work according to the pre-set settings;

[0008] (2) If only the GNSS positioning module outputs valid positioning information, a request to release the stationary / hovering state is sent to the control module, and the valid positioning information output by the GNSS positioning module is used for control, and the SLAM positioning module uses the valid positioning information output by the GNSS positioning module to update the position and posture of the local map key frame;

[0009] (3) If both output valid positioning information, a request to release the stationary / hovering state is sent to the control module, and the coordinate conversion parameters of the GNSS positioning module and the SLAM module are updated, and the fused position and attitude are used for control;

[0010] (4) If neither of them outputs valid positioning information, a fallback request is sent to the control module, and it is determined whether the number of consecutive fallback requests exceeds the set threshold. If so, the work is terminated or the work route is replanned according to the task requirements. Otherwise, the unmanned system continues to work according to the pre-set settings.

[0011] Optionally, the determining whether the GNSS positioning module or the SLAM positioning module outputs valid positioning information includes: the valid positioning information of the GNSS positioning module refers to one or more positions, or positions and headings, or positions, headings and speeds that meet the following conditions:

[0012] (1) The DOP (Dilution of Precision) value is less than the set accuracy threshold;

[0013] (2) GNSS positioning status is fixed solution or floating point solution;

[0014] (3) The change in position coordinates within a continuous time window does not exceed a set distance threshold, where the set distance threshold for a stationary / hovering state is less than or equal to the set distance threshold for a moving state;

[0015] The effective positioning information of the SLAM positioning module refers to the position and posture, or the position, posture and speed, at which the optimization error converges to the set error threshold within a set number of times.

[0016] Optionally, the valid satellite information means that the number of visible satellites of the GNSS positioning module is greater than a set number threshold, or the number of visible satellites whose carrier-to-noise ratio of the GNSS positioning module is greater than a set ratio threshold is greater than a set number threshold.

[0017] Optionally, sending a rollback request to the control module includes:

[0018] The unmanned system is returned to the position where at least one of the GNSS positioning module and the SLAM positioning module outputs valid positioning information in the previous round; or, the unmanned system is returned to the position where at least one of the GNSS positioning module and the SLAM positioning module outputs valid positioning information in the previous round, and the viewing angle of the SLAM positioning module observation sensor is adjusted.

[0019] The above technical solution of the present invention has at least the following beneficial effects:

[0020] (1) Flexibility: The present invention designs a positioning-control linkage strategy that instructs the unmanned system to adjust control by evaluating the positioning performance in real time to obtain an environment or condition that is more conducive to positioning;

[0021] (2) Adaptability: The present invention combines the positioning advantages of GNSS and SLAM and is applicable to more complex environments, such as indoors, forests, urban canyons and other areas where GNSS signals are weak or cannot be covered, thus improving the applicability of the positioning system;

[0022] (3) Safety: The present invention adopts a joint control strategy to maximize the use of effective GNSS and SLAM data during the operation of the unmanned system, avoiding loss of control caused by environmental deterioration. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 A flowchart of a GNSS and SLAM fusion positioning method for an unmanned system provided in the first embodiment of the present invention;

[0024] Figure 2 A schematic diagram of a method for converting GNSS latitude and longitude coordinates into geodetic rectangular coordinates provided in Example 1 of the present invention. DETAILED DESCRIPTION

[0025] In order to make the technical problems, technical solutions and advantages to be solved by the present invention more clear, a detailed description will be given below with reference to the accompanying drawings and specific embodiments.

[0026] Figure 1 FIG. 1 is a flow chart of a GNSS and SLAM fusion positioning method for unmanned systems provided in the first embodiment of the present invention. Figure 1 As shown, the following steps are included:

[0027] S101: The unmanned system is powered on and the GNSS positioning module and SLAM positioning module are turned on;

[0028] S102: Stay still for N seconds, waiting for the GNSS positioning module or the SLAM positioning module to output valid positioning information;

[0029] S103: If both are negative, go to step S104, otherwise go to step S105;

[0030] S104: The environment is deemed unsuitable for unmanned system operation, and this work is terminated;

[0031] S105: the unmanned system starts to work and performs joint control positioning according to the output information of the GNSS positioning module and the SLAM positioning module in the current round;

[0032] S106: Determine whether only the SLAM positioning module outputs valid positioning information, if so, proceed to step S107, otherwise proceed to step S110;

[0033] S107: The control module uses the valid positioning information output by the SLAM positioning module to perform control;

[0034] S108: Determine whether the GNSS positioning module outputs valid satellite information, if so, proceed to step S109, otherwise continue to work according to the preset settings;

[0035] S109: Sending a stationary / hovering request to the control module;

[0036] S110: Determine whether only the GNSS positioning module outputs valid positioning information, if so, proceed to step S111, otherwise proceed to step S114;

[0037] S111: Sending a request to release the stationary / hovering state to the control module;

[0038] S112: the control module uses the valid positioning information output by the GNSS positioning module to perform control;

[0039] S113: the SLAM positioning module updates the position and posture of the local map key frame using the valid positioning information output by the GNSS positioning module;

[0040] S114: Determine whether both the GNSS positioning module and the SLAM positioning module output valid positioning information, if so, proceed to step S115, otherwise proceed to step S118;

[0041] S115: Sending a request to release the stationary / hovering state to the control module;

[0042] S116: Updating coordinate conversion parameters of the GNSS positioning module and the SLAM module;

[0043] S117: The control module uses the fused position and posture for control;

[0044] S118: The GNSS positioning module and the SLAM module both fail to output valid positioning information, and a rollback request is sent to the control module;

[0045] S119: Determine whether the consecutive number of rollback requests exceeds a set number threshold, if so, proceed to step S120, otherwise continue to work according to the preset setting;

[0046] S120: Terminate this work or re-plan the work route according to the task requirements.

[0047] In this embodiment, the unmanned system refers to a platform and its supporting devices that can complete designated tasks autonomously or through remote control. Common ones include unmanned aerial vehicles, unmanned vehicles, unmanned ships, unmanned submersibles, quadruped robots, etc. The key technologies of unmanned systems are intelligent perception and autonomous control. The GNSS positioning module and the SLAM positioning module belong to intelligent perception technology. The position, posture, speed and other information they output will be used as input to the control module for autonomous control. According to the positioning principle, the GNSS positioning module can be ordinary single-point positioning using pseudo-range observation, or real-time differential positioning using pseudo-range observation or phase observation; the SLAM positioning module can be L-SLAM based on laser radar, or V-SLAM based on vision, or a combination of the two, depending on the sensor type. No specific limitation is made in this embodiment.

[0048] In this embodiment, the determining whether the GNSS positioning module or the SLAM positioning module outputs valid positioning information includes:

[0049] The effective positioning information of the GNSS positioning module refers to one or more positions, or positions and headings, or positions, headings and speeds that meet the following conditions:

[0050] (1) The DOP (Dilution of Precision) value is less than the set accuracy threshold;

[0051] (2) GNSS positioning status is fixed solution or floating point solution;

[0052] (3) The change in position coordinates within a continuous time window does not exceed a set distance threshold, where the set distance threshold for a stationary / hovering state is less than or equal to the set distance threshold for a moving state;

[0053] The effective positioning information of the SLAM positioning module refers to the position and posture, or position, posture and speed, where the optimization error converges to the set error threshold within a set number of times. Here, several typical SLAM algorithm frameworks are used as examples to illustrate their specific implementation methods:

[0054] (1) The SLAM positioning module uses the LOAM framework that is tightly coupled with a mechanical multi-line laser radar and an IMU. The laser point cloud frame extracts corner point and surface point features, performs point-line matching and point-surface matching with the local map through an initial estimated pose, and takes the minimum average distance error from the feature point to the line or surface as the optimization goal. The pose result of the laser radar is iterated using the Levenberg-Marquardt method. If the iteration number threshold is N_Thr and the iteration error threshold is E_Thr, the effective positioning information at this time refers to the pose result obtained when the average distance is less than the threshold E_Thr within the iteration number less than the threshold N_Thr.

[0055] (2) The SLAM positioning module uses the FAST-LIO framework with a solid-state multi-line laser radar and an IMU tightly coupled. The raw data of the laser point cloud frame is directly used as the observation quantity, and the residual observation equation is constructed based on the distance to the nearest neighbor plane or line. The IESKF (Iterative Error-State Kalman Filter) is used to iteratively update the Kalman gain to estimate the pose error state. Similarly, FAST-LIO sets an iteration number threshold N_Thr and an iteration error threshold E_Thr. At this time, the effective positioning information refers to the pose result obtained when the difference between the pose error states estimated twice in a row within the iteration number less than the threshold N_Thr is less than the threshold E_Thr.

[0056] (3) The SLAM positioning module uses the ORB-SLAM framework that tightly couples the visual camera and the IMU. It first constructs a pyramid from the captured image frame to extract the FAST corner points, and calculates the rotated BRIEF descriptor. It then estimates the initial pose of the camera through the previous frame image, finds the matching feature point pairs between the current frame and the local map, and uses the BA (Bundle Adjustment) optimization that minimizes the reprojection error between the projection of all feature points and the observed values ​​to estimate the current camera pose and the spatial coordinates of the feature points. At this time, the effective positioning information refers to the pose result obtained when the reprojection error of the feature points is less than the threshold E_Thr within the number of iterations less than the threshold N_Thr.

[0057] In this embodiment, the valid satellite information means that the number of visible satellites of the GNSS positioning module is greater than a set number threshold, or the number of visible satellites whose carrier-to-noise ratio of the GNSS positioning module is greater than a set ratio threshold is greater than a set number threshold.

[0058] In this embodiment, sending a rollback request to the control module includes:

[0059] The unmanned system is returned to the position where at least one of the GNSS positioning module and the SLAM positioning module outputs valid positioning information in the previous round; or, the unmanned system is returned to the position where at least one of the GNSS positioning module and the SLAM positioning module outputs valid positioning information in the previous round, and the viewing angle of the SLAM positioning module observation sensor is adjusted.

[0060] In this embodiment, the method for updating the coordinate conversion parameters of the GNSS positioning module and the SLAM module described in step S116 includes:

[0061] S1161: Convert the GNSS latitude and longitude coordinates to the geodetic rectangular coordinate system;

[0062] S1162: Select the position coordinates of the latest M pairs of time-aligned GNSS rectangular coordinate systems And the position coordinates of the SLAM rectangular coordinate system Calculate the conversion relationship between the GNSS rectangular coordinate system and the SLAM rectangular coordinate system, including the translation T and rotation R.

[0063] Figure 2 Schematic diagram of a method for converting GNSS latitude and longitude coordinates into geodetic rectangular coordinates provided in this embodiment. Figure 2 As shown, taking the WGS84 coordinate system as an example, assuming that the latitude and longitude of point P output by the GNSS positioning module are expressed as L, B, and H. First, calculate the relationship between the coordinates (x, y) of point P in its meridian plane coordinate system O-xy and the latitude B:

[0064]

[0065] Then, according to the longitude L, the coordinates (X, Y, Z) of point P in the rectangular coordinate system of geodetic space are obtained:

[0066]

[0067] Among them, e is the first eccentricity of the ellipsoid, N is the radius of curvature of the ellipse, and the calculation method is as follows:

[0068]

[0069]

[0070] Here, a is the long semi-axis in the WGS84 earth model, which is 6378137m; b is the short semi-axis in the WGS84 earth model, which is 6356752.3142451793m. If other polar coordinate systems are calculated, the main difference is that the long / short semi-axis values ​​in the earth model are different. The conversion relationship between the GNSS rectangular coordinate system and the SLAM rectangular coordinate system can be calculated based on the least squares method, and the calculation method is as follows:

[0071]

[0072]

[0073]

[0074] [U, S, V] = SVD(H)

[0075]

[0076] T=centroid s -R centroid G

[0077] Here, it is assumed that the position coordinates of the GNSS positioning module and the SLAM positioning module have been time-aligned. If not, the linear difference method can be used to time-align the position coordinates of the two positioning modules first.

[0078] The above is a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A GNSS and SLAM fusion positioning method for unmanned systems, comprising: The unmanned system is powered on and the GNSS positioning module and SLAM positioning module are turned on; Stay still for N seconds, and determine whether the GNSS positioning module or the SLAM positioning module outputs valid positioning information. If both are negative, it is considered that the environment is not suitable for the operation of the unmanned system, and the work is terminated; Otherwise, the unmanned system starts to work and performs joint control positioning according to the output information of the current round of the GNSS positioning module and the SLAM positioning module, including: If only the SLAM positioning module outputs valid positioning information, the valid positioning information output by the SLAM positioning module is used for control, and it is determined whether the GNSS positioning module outputs valid satellite information. If so, a stationary / hovering request is sent to the control module, otherwise the unmanned system continues to work according to the preset settings; If only the GNSS positioning module outputs valid positioning information, a request to release the stationary / hovering state is sent to the control module, and the valid positioning information output by the GNSS positioning module is used for control, and the SLAM positioning module uses the valid positioning information output by the GNSS positioning module to update the position and posture of the local map key frame; If both output valid positioning information, a request to release the stationary / hovering state is sent to the control module, and the coordinate conversion parameters of the GNSS positioning module and the SLAM module are updated, and the fused position and attitude are used for control; If neither of them outputs valid positioning information, a fallback request is sent to the control module, and it is determined whether the number of consecutive fallback requests exceeds the set threshold. If so, the work is terminated or the work route is replanned according to the task requirements, otherwise the unmanned system continues to work according to the pre-set settings.

2. The method according to claim 1, characterized in that The determining whether the GNSS positioning module or the SLAM positioning module outputs valid positioning information includes: The effective positioning information of the GNSS positioning module refers to one or more positions, or positions and headings, or positions, headings and speeds that meet the following conditions: (1) The DOP (Dilution of Precision) value is less than the set accuracy threshold; (2) GNSS positioning status is fixed solution or floating point solution; (3) The change in position coordinates within a continuous time window does not exceed a set distance threshold, where the set distance threshold for a stationary / hovering state is less than or equal to the set distance threshold for a moving state; The effective positioning information of the SLAM positioning module refers to the position and posture, or the position, posture and speed, at which the optimization error converges to the set error threshold within a set number of times.

3. The method according to claim 1, characterized in that The valid satellite information means that the number of visible satellites of the GNSS positioning module is greater than a set number threshold, or the number of visible satellites whose carrier-to-noise ratio of the GNSS positioning module is greater than a set ratio threshold is greater than a set number threshold.

4. The method according to claim 1, characterized in that The sending a rollback request to the control module includes: The unmanned system is made to return to the position where at least one of the GNSS positioning module and the SLAM positioning module outputs valid positioning information in the previous round; or, The unmanned system is made to return to the position where at least one of the GNSS positioning module and the SLAM positioning module outputs valid positioning information in the previous round, and the viewing angle of the observation sensor of the SLAM positioning module is adjusted.

Images