Localization and Mapping Method with Multi-Sensor Fusion in Desert Environment

Abstract

The present invention provides a positioning and mapping method for multi-sensor fusion in a desert environment, which includes the following steps: S1. When the GNSS signal strength exceeds a certain threshold, filter and fuse the IMU signal and the GNSS signal to obtain the IMU and GNSS odometers; S2. Select to use the odometers of IMU and GNSS or the visual odometer according to the richness and reliability of the target object information in the environment; S3. When the GNSS signal strength is lower than a certain threshold and the number of detected target objects in the environment is less than a certain threshold, use the optical flow method to construct an odometer; S4. When the GNSS signal is weak and the number of target objects is small, determine the final positioning information of the robot; S5. Estimate the three-dimensional pose of the target object; S6. Construct a three-dimensional environmental topological map of the desert. The positioning and mapping method for multi-sensor fusion in the desert environment described in the present invention can not only achieve centimeter-level positioning and mapping in the desert environment, but also reduce the computational burden on the computing unit.

Description

Technical Field

[0001] The present invention belongs to the technical field of positioning and mapping in desert environments, and particularly relates to a positioning and mapping method for multi-sensor fusion in desert environments. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) refers to the ability of a robot to sense the surrounding environment through sensors and complete its own positioning in an unfamiliar environment, enabling the robot to replace humans or cooperate with humans to complete certain specific tasks in various environments.

[0003] Desert landforms are complex and changeable, with boulders, steep slopes, potholes, and soft sand everywhere. The perception and recognition of the desert environment have highly similar and dense detail information in terms of shape, color, and texture, and the terrain scene in the desert environment is large. It is difficult to achieve accurate map construction and positioning of the robot with a single sensor alone. Moreover, many current SLAM combination schemes mainly focus on achieving the positioning function, and the three-dimensional maps constructed simultaneously have redundant invalid information and cannot support the further navigation and planning operations of mobile robots. It is difficult to be competent for map construction and positioning work under such special conditions as the desert. Summary of the Invention

[0004] In view of this, to overcome the above defects, the present invention aims to propose a positioning and mapping method for multi-sensor fusion in desert environments.

[0005] To achieve the above object, the technical solution of the present invention is realized as follows:

[0006] The present invention provides a positioning and mapping method for multi-sensor fusion in desert environments, including the following steps:

[0007] S1. When the GNSS signal strength exceeds a certain threshold, filter and fuse the IMU signal and the GNSS signal to obtain an IMU and GNSS odometer;

[0008] S2. Use the camera to obtain RGB information and depth information in the environment, and detect the target objects in the environment through a target detection algorithm. Select to use the odometer of the IMU and GNSS or the visual odometer according to the richness and reliability of the target object information in the environment. Among them, the construction of the visual odometer is also based on the information obtained by the camera and the target detection algorithm, and then feature matching is performed to construct it;

[0009] S3. When the GNSS signal strength is lower than a certain threshold and the number of detected target objects in the environment is less than a certain threshold, use the optical flow method to construct an odometer;

[0010] S4. Based on the feature matching information extracted during the construction of the visual odometer in step S2 or the optical flow information during the construction of the odometer in step S3, obtain the pose transformation information of the robot through relevant algorithms and perform non-linear optimization to obtain the final positioning information of the robot;

[0011] S5. The depth information in step S2 includes the depth information of the center point of the target object. Based on this information, estimate the three-dimensional pose of the target object;

[0012] S6. Combine the positioning information and the three-dimensional pose of the target object to construct a topological map of the desert three-dimensional environment.

[0013] Furthermore, the specific method of step S1 is as follows:

[0014] S101. Obtain the speed and acceleration data of the robot from the IMU signal, and obtain the pose information of the robot through pre-integration;

[0015] S102. Based on the GNSS signal, use real-time differential positioning technology to obtain centimeter-level outdoor positioning accuracy;

[0016] S103. Use the extended Kalman filter method to filter and fuse the IMU signal and the GNSS signal;

[0017] S104. Combine the advantages of the IMU signal and the GNSS signal to further optimize the pose information and obtain the IMU and GNSS odometers.

[0018] Furthermore, in step S2, the specific method of constructing the visual odometer is as follows:

[0019] Select key frames according to the degree of change in the inter-frame information of the images collected by the camera, extract the feature points of the unique desert vegetation or obstacles in the target object based on the target detection algorithm, and perform feature matching to construct a visual odometer.

[0020] Furthermore, the relevant algorithms for constructing the visual odometer are as follows:

[0021] Let a set of matched 3D feature points be P and P' respectively;

[0022] P = {p1, …, p n}, P' = {p'1, …, p' n}

[0023] Find a Euclidean transformation R, t such that

[0024]

[0025] Then the motion between two frames is obtained.

[0026] Further, in step S3, before constructing the odometer, key frames of the image need to be selected according to the frequency change of the GNSS signal. The selection method is as follows: Set a threshold. After calculating the similarity of the input image signal, when it exceeds the threshold, it is a key frame. If staying on the same screen for a long time, reduce or skip setting key frames.

[0027] Further, in step S3, the method of constructing the odometer using the optical flow method is as follows:

[0028] Generate an image pyramid for the target detection area, from layer 0 to layer n. The size of the previous image is 4 times that of the next image, and use a low-pass filter for smoothing processing;

[0029] Calculate the optical flow and affine transformation matrix. Use the result of the upper layer as the initial value of the lower layer, and on this basis, calculate the optical flow and affine transformation matrix of the lower layer until the initial layer 0;

[0030] Iteratively solve to construct the odometer.

[0031] Further, in step S4, obtain the pose transformation information of the robot through the bundle adjustment method or the sliding window method.

[0032] Further, in step S5, the calculation method of the depth information of the center point of the target object is as follows:

[0033] After training the lightweight target detection model through the vegetation dataset collected and made locally, real-time identify the target objects in the desert environment, and obtain the distance of the center point of the target object according to the camera.

[0034] Further, in step S5, combine the two-dimensional target detection result in the target detection algorithm with the depth information of the center point of the target object, deduce the bounding box algorithm, and estimate the three-dimensional pose of the target object.

[0035] Compared with the prior art, the positioning and mapping method of multi-sensor fusion in the desert environment described in the present invention has the following beneficial effects:

[0036] (1) Through the information fusion of multi-sensors such as RTK technology supplemented by machine vision, real-time target detection is carried out, and a desert topological map is constructed. Different odometers can be selected according to the richness of features in the environment to realize the route, which can not only achieve centimeter-level positioning and mapping in the desert environment, but also reduce the computing burden of the computing unit.

[0037] (2) It can also realize the real-time precise positioning of the robot and record the planting position at any time, making equidistant planting and precise watering and maintenance possible, greatly improving the rationality and survival rate of desert planting, and can be migrated to other robots working in open environments such as agricultural planting robots. Brief Description of the Drawings

[0038] The accompanying drawings that form a part of the present invention are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0039] Figure 1 It is a flowchart of the method for positioning and mapping with multi-sensor fusion in the desert environment described in the present invention. Detailed Description of the Invention

[0040] It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0041] The present invention will be described in detail below with reference to the accompanying drawings and in conjunction with the embodiments.

[0042] As Figure 1 shown, a method for positioning and mapping with multi-sensor fusion in the desert environment is as follows:

[0043] 1. Obtain the speed and acceleration observation data of the robot from the IMU signal, and obtain the pose information of the robot through, but not limited to, pre-integration. Specifically:

[0044] Generally, the update frequency of the satellite positioning system is 10HZ, while the update frequency of the IMU sensor can reach up to 1kHz at most. By integrating the data such as speed and acceleration in the IMU, we can obtain the pose of the robot. However, the data in the IMU are all based on the initial moment. In order to avoid re-integrating the IMU measurement values every time for optimization iteration to obtain the speed, acceleration and other information of the robot, which increases the burden on the computing system. The integration term for calculating the pose each time is separated from the calculation formula to form a pre-integration term, and then the pose information of the robot is obtained only according to the observation values of the sensors.

[0045] 2. From the GNSS signal, obtain centimeter-level outdoor positioning accuracy by, but not limited to, real-time kinematic (RTK) technology. Specifically:

[0046] Based on GNSS, a base station is set up on the ground. The ground base station obtains satellite positioning and compares it with the actual position of the base station to calculate the positioning error of GNSS. The error is fed back to the mobile base station of the vehicle through GNSS, and then the positioning is corrected to make the positioning error reach the centimeter level.

[0047] 3. Filter and fuse the IMU signal and the GNSS signal by, but not limited to, the method of extended Kalman filter (EKF). Specifically:

[0048] Assume that the noise of the IMU and GNSS signals follows Gaussian noise. Update the observation equation of the IMU sensor to obtain the state variables and covariance matrix of the system, and use the two as the system predicted state variables and system predicted covariance matrix of the GNSS signal for state update. Repeat this process for each cycle to complete the filtering and fusion work of the sensor.

[0049] 4. Further optimize the pose information by combining the advantages of the IMU under high-speed and short-time conditions with the low drift advantage of the GNSS under large-scene and long-time conditions to obtain the odometer of the robot, specifically as follows:

[0050] When the GNSS signal is strong, the motion trajectory of the robot relative to the starting position, that is, the odometer, can be obtained by combining the advantages of the IMU under high-speed and short-time conditions with the low drift advantage of the GNSS under large-scene and long-time conditions in the above manner.

[0051] 5. Obtain the RGB information and depth information in the environment through, but not limited to, a passive binocular depth camera.

[0052] 6. Detect whether there are target objects such as trees and people in the environment through the target detection algorithm. According to the richness and reliability of the target object information, the odometer of the IMU and GNSS or the visual odometer can be selected.

[0053] Among them, after training the lightweight target detection model with the vegetation dataset collected and made locally, the target objects in the desert environment are recognized in real time, and the center point distance of the target objects is obtained according to the depth camera.

[0054] 7. Select key frames according to the degree of change in the information between image frames.

[0055] 8. According to the results of target detection, extract the feature points of desert-specific vegetation such as Hedysarum scoparium or obstacles, and perform feature matching to construct a visual odometer. The specific algorithm is as follows:

[0056] Suppose we have a set of matched 3D feature points P and P';

[0057] P = {p1,..., p n}, P' = {p'1,..., p' n}

[0058] Find a Euclidean transformation R, t such that

[0059]

[0060] Then the motion between two frames can be obtained.

[0061] 9. In the case where there are fewer target objects that can be detected in the environment with weak GNSS signals, select key frames according to the change of the GNSS signal;

[0062] By setting a threshold value, after calculating the similarity of the input image signal, when it exceeds the threshold value, it is a key frame. If staying on the same screen for a long time, the setting of key frames is reduced or skipped;

[0063] 10. Furthermore, based on the theory of invariant gray scale, the L-K optical flow method is used to construct an odometer, specifically as follows:

[0064] Generate an image pyramid for the target detection area, from layer 0 to layer n. The previous image is 4 times the size of the next image, and a low-pass filter is used for smoothing;

[0065] Track based on the pyramid; that is, calculate the optical flow and the affine transformation matrix, use the result of the previous layer as the initial value of the next layer, and then calculate the optical flow and the affine transformation matrix of the next layer on this basis until the initial layer 0;

[0066] Iteratively solve; that is, for each layer, calculate the optical flow and the radiation transformation matrix to minimize the error.

[0067] 11. According to the extracted feature matching information or optical flow information, obtain the pose transformation of the robot through, but not limited to, the bundle adjustment method (BA) or the sliding window method.

[0068] 12. When the number of key frames reaches a certain level, use, but not limited to, the g2o algorithm to perform non-linear optimization on the robot pose again.

[0069] 13. Obtain the final positioning information of the robot in the desert environment according to the non-linear optimization algorithm of the previous step.

[0070] By comprehensively selecting the above series of sensors and algorithm routes, the movement path of the robot relative to the starting point can be finally obtained, and its position in the path can be known. Furthermore, the robot can move and operate autonomously in the desert terrain.

[0071] 14. By combining the two-dimensional target detection result with the depth information of the center point of the target object, deduce the bounding box algorithm and estimate the three-dimensional pose of the target object.

[0072] Use a simple geometric body with characteristics (called an AABB bounding box) to approximately replace the complex desert detection target object.

[0073] 15. Combine the positioning information and the three-dimensional information of the target to construct a topological map of the desert three-dimensional environment.

[0074] Combine the self-positioning information of the mobile robot, the three-dimensional coordinate information of the detection target relative to the mobile robot, and the size and pose of the bounding box to construct a topological map of the desert three-dimensional environment.

[0075] Those of ordinary skill in the art will realize that the units and method steps of the examples described in combination with the embodiments disclosed herein can be implemented by electronic hardware, computer software, or a combination of the two. To clearly illustrate the interchangeability of hardware and software, the components and steps of the examples have been generally described according to their functions in the above description. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Professional technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of the present invention.

[0076] In several embodiments provided in the present application, it should be understood that the disclosed methods and systems can be implemented in other ways. For example, the above-mentioned division of units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. The above-mentioned units may or may not be physically separated. The components shown as units may or may not be physical units, that is, they can be located in one place or distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiments of the present invention.

[0077] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some or all of the technical features. These modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention, and they should all be covered by the scope of the claims and the description of the present invention.

[0078] The above is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A positioning and mapping method based on multi-sensor fusion in desert environment, characterized in that, The steps are as follows: S1. When the GNSS signal strength exceeds a certain threshold, filter and fuse the IMU signal and the GNSS signal to obtain the IMU and GNSS odometers; S2. Use the camera to obtain the RGB information and depth information in the environment, and detect the objects in the environment through the object detection algorithm. Select to use the odometer of the IMU and GNSS or the visual odometer according to the richness and reliability of the object information in the environment. Among them, the construction of the visual odometer is also based on the information obtained by the camera and the object detection algorithm, and then feature matching is carried out to construct it; S3. When the GNSS signal strength is lower than a certain threshold and the number of detected objects in the environment is less than a certain threshold, use the optical flow method to construct the odometer; S4. According to the feature matching information extracted when constructing the visual odometer in step S2 or the optical flow information when constructing the odometer in step S3, obtain the pose transformation information of the robot through relevant algorithms, and perform nonlinear optimization to obtain the final positioning information of the robot; S5. The depth information in step S2 includes the depth information of the center point of the object, and the three-dimensional pose of the object is estimated based on this information; S6. Combine the positioning information and the three-dimensional pose of the object to construct a topological map of the desert three-dimensional environment; The specific method of step S1 is as follows: S101. Obtain the speed and acceleration data of the robot from the IMU signal, and obtain the pose information of the robot through pre-integration; S102. Based on the GNSS signal, use the real-time differential positioning technology to obtain centimeter-level outdoor positioning accuracy; S103. Use the extended Kalman filter method to filter and fuse the IMU signal and the GNSS signal; S104. Combine the advantages of the IMU signal and the GNSS signal to further optimize the pose information to obtain the IMU and GNSS odometers; In step S2, the specific method of constructing the visual odometer is as follows: Select key frames according to the degree of change in the inter-frame information of the images collected by the camera, extract the feature points of the specific vegetation or obstacles in the desert in the object according to the object detection algorithm, and perform feature matching to construct the visual odometer; The relevant algorithms for constructing the visual odometer are as follows: Let a set of well-matched 3D feature points be P and P' respectively; P = {p1, …, p n}, P′ = {p′1, …, p′ n} Find a Euclidean transformation R, t such that Then the motion between two frames is obtained.

2. The positioning and mapping method with multi-sensor fusion in a desert environment according to claim 1, characterized in that In step S3, before constructing the odometer, it is necessary to select the key frames of the images according to the frequency change of the GNSS signal. The selection method is: set a threshold, and after calculating the similarity of the input image signal, when it exceeds the threshold, it is a key frame. If staying on the same screen for a long time, that is, reduce or skip setting key frames.

3. The positioning and mapping method based on multi-sensor fusion in a desert environment according to claim 1 or 2, characterized in that, In step S3, the method of using the optical flow method to construct the odometer is as follows: Generate an image pyramid for the object detection area, from the 0th layer to the nth layer. The size of the previous image is 4 times that of the next image, and a low-pass filter is used for smoothing processing; Calculate the optical flow and the affine transformation matrix, use the result of the previous layer as the initial value of the next layer, and calculate the optical flow and the affine transformation matrix of the next layer on this basis until the initial 0th layer; Iteratively solve to construct the odometer.

4. The positioning and mapping method based on multi-sensor fusion in the desert environment according to claim 1, characterized in that: In step S4, the pose transformation information of the robot is obtained by the bundle adjustment method or the sliding window method.

5. The positioning and mapping method with multi-sensor fusion in a desert environment according to claim 1, characterized in that In step S5, the calculation method of the depth information of the center point of the target object is as follows: After training the lightweight target detection model with the vegetation dataset collected and made locally, the target object in the desert environment is recognized in real time, and the distance of the center point of the target object is obtained according to the camera.

6. The positioning and mapping method with multi-sensor fusion in a desert environment according to claim 1, characterized in that In step S5, based on the two-dimensional target detection result in the target detection algorithm and the depth information of the center point of the target object, the bounding box algorithm is deduced to estimate the three-dimensional pose of the target object.

Images