Pasture feeding robot mapping positioning method and device based on multi-sensor fusion

Abstract

The invention provides a pasture feeding robot mapping positioning method and device based on multi-sensor fusion, and solves the technical problem of insufficient positioning precision in an existing navigation signal unstable environment. The method comprises the steps of forming synchronous acquisition data through an integrated sensor, wherein the synchronous acquisition data at least comprises point cloud data; eliminating the distortion of the point cloud data according to the time sequence of the IMU data, and performing feature extraction to form a key frame; and performing graph optimization on the key frame to form a local map, and updating and determining the pose and track of the robot according to the local map. The interference of unstable navigation signals on navigation in the house is effectively overcome by improving an existing point cloud data processing process and utilizing multi-sensor special type data to form graph optimization, and the positioning and navigation requirements of autonomous operation inside and outside the house of the pasture feeding robot can be met.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to a method and device for mapping and positioning a pasture feeding robot based on multi-sensor fusion. Background Art

[0002] Animal husbandry is a labor-intensive industry with a poor working environment and high labor intensity. Large-scale ranches are generally located in remote areas far away from cities, with an open environment outside the houses and a messy environment inside the houses. In the process of using ranch feeding robots to improve labor efficiency and automate feeding, pushing and loading, continuous positioning and navigation are required. Since ranch feeding robots need to frequently switch operations inside and outside the house, they are limited by the instability of GNSS (Global Navigation Satellite System) and RTK (Real Time Kinematic) signals in the house environment. The positioning accuracy inside and outside the house is often quite different, and navigation inside the house cannot be reliable, which is not conducive to avoiding potential obstacles in the environment. Therefore, higher requirements are placed on positioning algorithms and supporting hardware, resulting in higher production and maintenance costs. Summary of the invention

[0003] In view of the above problems, an embodiment of the present invention provides a method and device for mapping and positioning a pasture feeding robot based on multi-sensor fusion, which solves the technical problem of insufficient positioning accuracy in an environment with unstable navigation signals.

[0004] The method for mapping and positioning a pasture feeding robot based on multi-sensor fusion according to an embodiment of the present invention includes:

[0005] Synchronously collect data by integrating sensors, including at least point cloud data;

[0006] Eliminate the distortion of point cloud data according to the temporal nature of IMU data and perform feature extraction to form key frames;

[0007] The key frames are optimized to form a local map, and the robot's position and trajectory are determined based on the local map update.

[0008] In one embodiment of the present invention, the forming of synchronous acquisition data includes:

[0009] During the driving process, mechanical laser radar is used to generate basic point cloud data frame by frame;

[0010] Used to synchronize basic point cloud data and data collected by other sensors through timestamps.

[0011] In one embodiment of the present invention, forming a key frame includes:

[0012] Get IMU data based on the start and end timestamps of each frame of basic point cloud data;

[0013] The angle and displacement integrals at each imaging moment in the frame are formed according to the IMU data, and the LiDAR posture data at the corresponding moment is formed;

[0014] According to the laser radar posture data, the distortion of the corresponding frame point cloud data is eliminated and the features are extracted to form key frame data.

[0015] In one embodiment of the present invention, forming the local map includes:

[0016] Perform graph optimization based on the constraints between key frames to obtain the optimized posture of the robot;

[0017] Process the odometer data according to the interval threshold of the starting key frame to form a local point cloud map and a local trajectory;

[0018] The current key frame is downsampled and matched with the local point cloud map, and the odometer data is updated to form a point cloud map update.

[0019] In one embodiment of the present invention, it further includes:

[0020] Determine the stability of the navigation signal and switch the positioning mode based on the determination result.

[0021] In one embodiment of the present invention, the switching of the positioning mode includes:

[0022] The straightness of the coordinate points solved by GNSS signals is judged at the time intervals of several specified key frames on the straight travel path;

[0023] If no GNSS signal is received or the straightness deviation is greater than the deviation threshold, the GNSS navigation mode is switched to the NDT navigation mode.

[0024] In one embodiment of the present invention, it further includes:

[0025] During the map construction process and / or before and after the map is loaded, map point sparse processing is performed as required.

[0026] In one embodiment of the present invention, the map point sparse processing includes:

[0027] Perform the first sparse operation on the map points in the point cloud map before loading the point cloud map;

[0028] After the point cloud map is loaded, the map points are thinned out a second time according to the positioning quality requirements.

[0029] The mapping and positioning device of a pasture feeding robot based on multi-sensor fusion according to an embodiment of the present invention comprises:

[0030] A memory, used to store program codes in the process of processing the method for mapping and positioning a pasture feeding robot based on multi-sensor fusion as described in any one of claims 1 to 8;

[0031] A processor is used to execute the program code.

[0032] The mapping and positioning device of a pasture feeding robot based on multi-sensor fusion according to an embodiment of the present invention comprises:

[0033] A heterogeneous data synchronization module, used to form synchronously collected data by integrating sensors, including at least point cloud data;

[0034] The key frame formation module is used to eliminate the distortion of point cloud data and perform feature extraction to form key frames according to the temporal nature of IMU data;

[0035] The optimization composition module is used to optimize the key frames to form a local map, and determine the robot's position and trajectory based on the local map update.

[0036] The mapping and positioning method and device for a ranch feeding robot based on multi-sensor fusion according to an embodiment of the present invention provide a new positioning method for the feeding robot. By improving the existing point cloud data processing process and using multi-sensor proprietary type data to form a map optimization, the interference of unstable navigation signals to navigation within the house is effectively overcome, and the positioning and navigation requirements of the ranch feeding robot for autonomous operation inside and outside the house can be met. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 The figure is a flow chart of a method and device for mapping and positioning a pasture feeding robot based on multi-sensor fusion according to an embodiment of the present invention.

[0038] Figure 2 The figure shows a schematic diagram of the architecture of a pasture feeding robot mapping and positioning device based on multi-sensor fusion according to an embodiment of the present invention. DETAILED DESCRIPTION

[0039] In order to make the purpose, technical solution and advantages of the present invention clearer and more understandable, the present invention is further described below in conjunction with the accompanying drawings and specific implementation methods. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0040] A method for mapping and positioning a pasture feeding robot based on multi-sensor fusion according to an embodiment of the present invention is as follows Figure 1 As shown. Figure 1 In this embodiment, the present invention includes:

[0041] Step 100: Synchronously collect data by integrating sensors, including at least point cloud data.

[0042] Those skilled in the art can understand that in SLAM (Simultaneous Localization and Mapping) technology, sensors can be used to collect surrounding spatial information. LiDAR can continuously collect information about the robot's surroundings to form point cloud data. Point cloud data is related to the LiDAR perception mechanism, usually has a collection time, and there is dynamic collection when the robot is moving.

[0043] Step 200: Eliminate distortion of point cloud data according to the temporal nature of IMU data and perform feature extraction to form key frames.

[0044] IMU (Inertial Measurement Unit) sensors usually include:

[0045] An accelerometer is used to measure the acceleration of an object. Acceleration is the rate of change of velocity over time, so by integrating the acceleration we can get the velocity, and integrating it again we get the displacement.

[0046] A gyroscope is used to measure the angular velocity of an object, that is, the rate at which the object rotates around its axis. The angle can be obtained by integrating the angular velocity.

[0047] Magnetometers are used to measure the magnetic field around an object and determine the orientation of the object in the Earth's coordinate system.

[0048] The temporal characteristics of IMU data can be used to eliminate the quantitative distortion caused by dynamic errors in the process of point cloud data formation. The point cloud data after distortion elimination can be used to extract the key state data of temporal sequence for constructing local spatial features.

[0049] Step 300: Optimize the key frames to form a local map, and determine the robot's position and trajectory based on the local map update.

[0050] Those skilled in the art can understand that graph optimization can eliminate errors between key state data. In the process of constructing a local map of the space around the robot, the robot's posture is continuously quantified, and the robot's posture is converted between adjacent moments. The posture error is eliminated through the position constraint boundary embodied by the relative posture conversion matrix. At the same time, the factor graph formed by the constraints established by each sensor data in the key frame is used to optimize the robot's posture in a targeted manner. The factor graph includes but is not limited to IMU factors, laser odometer factors, GPS factors, loop factors, etc., and shows stronger robustness for posture optimization in special scenarios. For example, the optimization effect of the loop factor graph can make the drift or error at the loop smaller. The local map is incrementally updated as the robot moves, reflecting the robot's posture and trajectory when the GNSS navigation mode is not applicable.

[0051] The mapping and positioning method for a ranch feeding robot based on multi-sensor fusion in an embodiment of the present invention effectively overcomes the interference of unstable navigation signals to navigation within the house by improving the existing point cloud data processing process and using multi-sensor proprietary type data to form a map optimization. It can meet the positioning and navigation requirements of the ranch feeding robot for autonomous operation within the house, and provides a new positioning method for the feeding robot.

[0052] like Figure 1 As shown, in one embodiment of the present invention, it also includes:

[0053] Step 400: Determine the stability of the navigation signal and switch the positioning mode according to the determination result.

[0054] The NDT (Normal Distribution Transform) navigation mode based on the matching of point clouds formed in the above steps has the advantage of being used in the house. The GNSS navigation mode is more suitable for use outside the house. The trigger signal for switching the positioning mode is formed by establishing a stability judgment of the preset positioning reference. The signal stability of the GNSS navigation mode is judged through stability judgment to meet the accurate positioning needs of the ranch feeding robot when it frequently switches operations inside and outside the house.

[0055] like Figure 1 As shown, in one embodiment of the present invention, it also includes:

[0056] Step 500: Perform map point sparse processing as required during the map construction process and / or before and after the map is loaded.

[0057] During the navigation process, the complete map formed by the local map is relatively large, which requires high computing power during the application process and may affect the matching speed during positioning. By extracting and controlling point features and line features during the mapping process, the map sparsity can be used to quantify the degree of map sparsity, which can ensure the efficiency of map loading during navigation applications.

[0058] By adjusting the map sparsity (which can also be considered as density) according to the relationship between map sparsity and positioning quality during navigation application, the map sparsity is adjusted according to navigation requirements. When applying navigation, the upper limit of map sparsity is set to the map sparsity during the mapping process. In this way, both navigation requirements of speeding up loading and adjusting density can be taken into account at the same time.

[0059] like Figure 1 As shown, in one embodiment of the present invention, step 100 includes:

[0060] Step 110: During the traveling process, a mechanical laser radar is used to perform imaging to generate basic point cloud data frame by frame;

[0061] Step 120: Synchronize the basic point cloud data and the data collected by other sensors through timestamps.

[0062] The mechanical LiDAR imaging process is performed by mechanical scanning, which results in the start and end range of the timestamps of the map point data in the same frame of point cloud. At the same time, during the time segment of the mechanical scan, the robot is in a moving state, and the data collected by other sensors, such as IMU data, GNSS data, odometer data, etc., also have timestamps.

[0063] The mapping and positioning method of a pasture feeding robot based on multi-sensor fusion in an embodiment of the present invention establishes time-series synchronization features between various types of collected data through multi-sensor fusion, establishes measurement dimensions related to point cloud data, and provides additional data basis for the correction and optimization of point cloud data.

[0064] like Figure 1 As shown, in one embodiment of the present invention, step 200 includes:

[0065] Step 210: Acquire IMU data according to the start and end timestamps of each frame of basic point cloud data;

[0066] Step 220: Forming angle and displacement integrals at each imaging moment in the frame according to the IMU data to form the LiDAR pose data at the corresponding moment;

[0067] Step 230: Dedistortion and feature extraction of corresponding frame point cloud data are performed according to the laser radar posture data to form key frame data.

[0068] Timestamps provide synchronization identification in the process of synchronous data collection of various types of sensors. Since the mechanical laser radar imaging process is carried out in a scanning manner, the timestamps of the points in the same frame of basic point cloud data are different. In the same frame of basic point cloud data, the position and posture of the laser radar will also change as the robot moves. Therefore, it is necessary to calculate the position and posture of the laser radar at the imaging moment of each point in the frame point cloud. The relative transformation posture of the sensor itself can be obtained by integrating the IMU data within the corresponding time length through the start and end time of the basic point cloud data. By integrating the angle and displacement of each point in a frame at the imaging moment, the rotation and translation corresponding to each imaging moment of the frame point cloud can be obtained, and the laser radar posture of each map point in the frame point cloud at the imaging moment can be obtained. The information is used to correct the coordinates of all points to the starting point to eliminate the coordinate distortion of each frame of basic point cloud data. Then, the corresponding key frame feature data is formed through feature extraction, and the feature extraction includes but is not limited to line and surface feature extraction and the formation of factor graphs. Factors as measurement or constraint conditions include but are not limited to IMU factors, laser odometer factors, GPS factors, loop factors, etc.

[0069] The method for mapping and positioning a pasture feeding robot based on multi-sensor fusion in the embodiment of the present invention overcomes the distortion error in the point cloud data acquisition process by utilizing the synchronization of data collected by other types of sensors fused together, and ensures the error accuracy of key frame point cloud data in the same coordinate space, which is beneficial to improving the optimization quality of the factor graph for the robot's posture.

[0070] like Figure 1 As shown, in one embodiment of the present invention, step 300 includes:

[0071] Step 310: Optimize the graph according to the constraints between the key frames to obtain the optimized posture of the robot;

[0072] Step 320: Process the odometer data according to the interval threshold of the starting key frame to form a local point cloud map and a local trajectory;

[0073] Step 330: downsample the current key frame and match it with the local point cloud map, update the odometer data, and form a point cloud map update.

[0074] In one embodiment of the present invention, graph optimization is used to batch optimize the relative position constraints obtained by IMU pre-integration between two consecutive key frames, as well as the position constraints obtained by point cloud matching and GNSS information, to obtain the optimal robot posture. In one embodiment of the present invention, position initialization is performed when the key frame is the first frame, and when the time interval between the current key frame and the previous key frame reaches the interval threshold, the mileage is calculated based on the laser radar odometer data; the corresponding key frame before the current key frame is selected according to the interval threshold to construct a local map. During the march, the feature point cloud of the current key frame is downsampled and matched with the local map, and the odometer data is updated to adjust the global trajectory and update the point cloud map.

[0075] The mapping and positioning method of a pasture feeding robot based on multi-sensor fusion in an embodiment of the present invention obtains the accurate posture of the robot by optimizing the key frame map, and combines key frame screening, point cloud downsampling and mileage matching to improve the construction efficiency of the point cloud map and the accuracy of the positioning trajectory.

[0076] like Figure 1 As shown, in one embodiment of the present invention, step 400 includes:

[0077] Step 410: performing straightness determination of coordinate points solved by GNSS signals at time intervals corresponding to a number of designated key frames on the straight travel path;

[0078] Step 420: If no GNSS signal is received or the straightness offset is greater than the offset threshold, the GNSS navigation mode is switched to the NDT navigation mode.

[0079] The specified key frames may be continuous key frames or a series of key frames with fixed intervals. The straightness judgment is performed based on the offset between each coordinate point and the fitted straight line after linear fitting of the solved coordinate points. When it is judged that the GNSS signal is not received or the straightness offset is too large, the NDT navigation mode (usually entering the house) formed by the mapping and positioning method based on the above embodiment is adopted.

[0080] like Figure 1 As shown, in one embodiment of the present invention, step 500 includes:

[0081] Step 510: performing a first thinning of map points in the point cloud map before loading the point cloud map;

[0082] Step 520: After the point cloud map is loaded, the map points are thinned out a second time according to the positioning quality requirements.

[0083] Since the map is large, which affects the matching speed during positioning, the map is thinned twice. The first time is before loading (it can also be during the map building process), and the second time is after loading, and it is done again according to the sparsity required by the local map during positioning. The first time can make the positioning module load faster when loading the map, and the second time needs to be based on the relationship between map sparsity and positioning quality. The second time's sparsity parameters are adjustable, which can take into account the requirements of loading speed, data computing power and memory resources at the same time.

[0084] An embodiment of the present invention provides a mapping and positioning device for a pasture feeding robot based on multi-sensor fusion, comprising:

[0085] A memory, used to store program codes in the processing of the method for mapping and positioning a pasture feeding robot based on multi-sensor fusion in the above-mentioned embodiment;

[0086] A processor is used to execute the program code in the processing of the method for mapping and positioning a pasture feeding robot based on multi-sensor fusion in the above embodiment.

[0087] The processor may be a DSP (Digital Signal Processor) digital signal processor, an FPGA (Field-Programmable Gate Array) field programmable gate array, an MCU (Microcontroller Unit) system board, a SoC (system on a chip) system board, or a PLC (Programmable Logic Controller) minimum system including I / O.

[0088] A mapping and positioning device for a pasture feeding robot based on multi-sensor fusion according to an embodiment of the present invention is as follows Figure 2 As shown. Figure 2 In this embodiment, the present invention includes:

[0089] A heterogeneous data synchronization module 10, used to form synchronously collected data by integrating sensors, including at least point cloud data;

[0090] A key frame forming module 20, used to eliminate the distortion of the point cloud data and perform feature extraction to form a key frame according to the temporal sequence of the IMU data;

[0091] The optimization composition module 30 is used to optimize the key frames to form a local map, and to update and determine the robot's position and trajectory based on the local map.

[0092] like Figure 2 As shown, in one embodiment of the present invention, it also includes:

[0093] The positioning mode switching module 40 is used to determine the stability of the navigation signal and switch the positioning mode according to the determination result.

[0094] like Figure 2 As shown, in one embodiment of the present invention, it also includes:

[0095] The application optimization module 50 is used to perform map point sparse processing according to requirements during the map construction process and / or before and after the map is loaded.

[0096] like Figure 2 As shown, in one embodiment of the present invention, the heterogeneous data synchronization module 10 includes:

[0097] A point cloud data generating unit 11 is used to generate basic point cloud data frame by frame by using a mechanical laser radar for imaging during the traveling process;

[0098] The heterogeneous data synchronization unit 12 is used to synchronize the basic point cloud data and the collected data of other sensors through time stamps.

[0099] like Figure 2 As shown, in one embodiment of the present invention, the key frame forming module 20 includes:

[0100] A heterogeneous data matching unit 21 is used to obtain IMU data according to the start and end timestamps of each frame of basic point cloud data;

[0101] A distortion error identification unit 22, used to form angle and displacement integrals at each imaging moment in a frame according to the IMU data, and form the laser radar posture data at the corresponding moment;

[0102] The feature extraction and framing unit 23 is used to perform distortion elimination and feature extraction of corresponding frame point cloud data according to the laser radar posture data to form key frame data.

[0103] like Figure 2 As shown, in one embodiment of the present invention, the optimization composition module 30 includes:

[0104] A graph optimization unit 31 is used to perform graph optimization according to the constraint conditions between key frames to obtain an optimized posture of the robot;

[0105] A local map generation unit 32, used to process the odometer data according to the interval threshold of the starting key frame to form a local point cloud map and a local trajectory;

[0106] The local map updating unit 33 is used to perform downsampling processing on the current key frame to match it with the local point cloud map, update the odometer data, and form a point cloud map update.

[0107] like Figure 2As shown, in one embodiment of the present invention, the positioning mode switching module 40 includes:

[0108] A straightness judgment unit 41 is used to judge the straightness of the coordinate points solved by the GNSS signal at time intervals corresponding to a number of specified key frames on the straight travel path;

[0109] The navigation mode switching unit 42 is used to switch the GNSS navigation mode to the NDT navigation mode if no GNSS signal is received or the straightness offset is greater than an offset threshold.

[0110] like Figure 2 As shown, in one embodiment of the present invention, the application optimization module 50 includes:

[0111] A loading sparse unit 51 is used to perform a first sparse operation on map points in the point cloud map before loading the point cloud map;

[0112] The balanced sparse unit 52 is used to perform a second sparse operation on the map points according to the positioning quality requirement after the point cloud map is loaded.

[0113] The above is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A method for mapping and positioning a pasture feeding robot based on multi-sensor fusion, characterized in that: include: Synchronously collect data by integrating sensors, including at least point cloud data; Eliminate the distortion of point cloud data according to the temporal nature of IMU data and perform feature extraction to form key frames; The key frames are optimized to form a local map, and the robot's position and trajectory are determined based on the local map update.

2. The method for mapping and positioning a pasture feeding robot based on multi-sensor fusion according to claim 1, characterized in that: The forming of synchronous acquisition data comprises: During the driving process, mechanical laser radar is used to generate basic point cloud data frame by frame; Used to synchronize basic point cloud data and data collected by other sensors through timestamps.

3. The method for mapping and positioning a pasture feeding robot based on multi-sensor fusion according to claim 1, characterized in that: The forming of key frames comprises: Get IMU data based on the start and end timestamps of each frame of basic point cloud data; The angle and displacement integrals at each imaging moment in the frame are formed according to the IMU data, and the LiDAR posture data at the corresponding moment is formed; According to the laser radar posture data, the distortion of the corresponding frame point cloud data is eliminated and features are extracted to form key frame data.

4. The method for mapping and positioning a pasture feeding robot based on multi-sensor fusion according to claim 1, characterized in that: The forming of the local map comprises: Perform graph optimization based on the constraints between key frames to obtain the optimized posture of the robot; Process the odometer data according to the interval threshold of the starting key frame to form a local point cloud map and a local trajectory; The current key frame is downsampled and matched with the local point cloud map, and the odometer data is updated to form a point cloud map update.

5. The method for mapping and positioning a pasture feeding robot based on multi-sensor fusion according to claim 1, characterized in that: Also includes: Determine the stability of the navigation signal and switch the positioning mode based on the determination result.

6. The method for mapping and positioning a pasture feeding robot based on multi-sensor fusion as claimed in claim 5, characterized in that: The switching of the positioning mode comprises: The straightness of the coordinate points solved by GNSS signals is judged at the time intervals of several specified key frames on the straight travel path; If no GNSS signal is received or the straightness deviation is greater than the deviation threshold, the GNSS navigation mode is switched to the NDT navigation mode.

7. The method for mapping and positioning a pasture feeding robot based on multi-sensor fusion according to claim 1, characterized in that: Also includes: During the map construction process and / or before and after the map is loaded, map point sparse processing is performed as required.

8. The method for mapping and positioning a pasture feeding robot based on multi-sensor fusion according to claim 7, characterized in that: The map point sparse processing comprises: Perform the first sparse operation on the map points in the point cloud map before loading the point cloud map; After the point cloud map is loaded, the map points are thinned out a second time according to the positioning quality requirements.

9. A mapping and positioning device for a pasture feeding robot based on multi-sensor fusion, characterized in that: include: A memory, used to store program codes in the process of processing the method for mapping and positioning a pasture feeding robot based on multi-sensor fusion as described in any one of claims 1 to 8; A processor is used to execute the program code.

10. A mapping and positioning device for a pasture feeding robot based on multi-sensor fusion, characterized in that: include: A heterogeneous data synchronization module, used to form synchronously collected data by integrating sensors, including at least point cloud data; The key frame formation module is used to eliminate the distortion of point cloud data and perform feature extraction to form key frames according to the temporal nature of IMU data; The optimization composition module is used to optimize the key frames to form a local map, and determine the robot's position and trajectory based on the local map update.

Images