A high-precision real-time positioning method for robots in orchards

Abstract

This invention discloses a high-precision real-time positioning method for robots in orchards. It compensates for positioning accuracy issues caused by interference with GNSS positioning signals using a binocular vision camera and a LiDAR sensor. The method includes: acquiring robot positioning information using both the binocular vision camera and the LiDAR sensor; fusing these two sets of information; and then calibrating the fused positioning information using a high-precision positioning signal from a GNSS RTK system when the GNSS positioning is not locked. This ensures the fused positioning information achieves GNSS positioning accuracy. When the system detects interference with the GNSS signal, it replaces the GNSS positioning signal with the calibrated high-precision fused positioning information. This method has low complexity, low computational load, and fast signal calibration speed, providing high-precision real-time positioning information for the robot even when GNSS lock-on is lost.

Description

Technical Field

[0001] This invention relates to the field of robot positioning technology, and in particular to a high-precision real-time positioning method for robots in orchards. Background Technology

[0002] In recent years, the country has attached great importance to the development of agricultural robots, investing considerable personnel and funds in their research and development, and actively promoting the use of automated agricultural machinery throughout society to achieve modern agricultural production. With the implementation of these policies, various intelligent agricultural robots have emerged. Most of these robots use satellite navigation and positioning systems for navigation and positioning. However, due to the special and complex nature of their operating environment, these agricultural robots face various interferences and obstructions when navigating and positioning in the field. This easily leads to the loss or inaccuracy of satellite positioning signals, greatly reducing the reliability and practicality of the robots.

[0003] Taking orchard harvesting robots as an example, when the robot walks among the fruit trees, it is often obscured or covered by dense foliage. In this situation, the GNSS signal is blocked during transmission, reducing the accuracy of the positioning signal. Users will be unable to obtain the robot's accurate location, and the inaccurate positioning signal will also affect its navigation route planning and subsequent harvesting work. Currently, the mainstream methods to address the problem of inaccurate robot positioning signals include GNSS combined with visual positioning, GNSS combined with lidar positioning, and map-based positioning. Although these methods have improved positioning performance, they still cannot guarantee positioning accuracy when GNSS is obstructed. Furthermore, these fusion positioning methods have certain requirements for the application environment and cannot work well in complex environments. Therefore, it is urgent to research a high-precision real-time positioning method for robots in complex outdoor environments such as orchards. Summary of the Invention

[0004] The purpose of this invention is to provide a high-precision real-time positioning method for robots in orchards. Even when the robot's satellite positioning signal is lost or interfered with and becomes inaccurate, it can still use its own sensors to achieve high-precision real-time positioning with an accuracy close to that of the satellite positioning signal, thus solving the problem of satellite positioning signal loss for robots in complex outdoor environments such as orchards.

[0005] The technical solution for achieving the objective of this invention is as follows:

[0006] A high-precision real-time positioning method for robots in orchards calibrates the fused positioning information of various sensors using high-precision GNSS positioning information, enabling the high-precision fused positioning information to compensate for the GNSS positioning signal. The method includes:

[0007] Step 1: Binocular vision localization. Use a binocular vision camera to collect road surface information and calculate the robot's localization information.

[0008] Step 2: LiDAR positioning. Use LiDAR to scan the surrounding environment and determine the robot's positioning information.

[0009] Step 3: Sensor positioning information fusion. The positioning signals from each positioning module are fused to obtain a more accurate positioning result.

[0010] Step 4: Fusion positioning information calibration. When the GNSS RTK positioning status is normal, compare the high-precision positioning signal of GNSS with the fused positioning information of each sensor obtained by the fusion algorithm, and adjust the fusion algorithm parameters according to the comparison results.

[0011] Step 5: Check if the GNSS positioning signal is normal.

[0012] Step 6: Positioning signal switching. When an error is detected in the GNSS positioning signal, the system will no longer use the GNSS positioning results, but will switch to the calibrated high-precision fused positioning information.

[0013] Furthermore, the specific process of step 1 includes:

[0014] Step 1.1: Adjust the binocular vision camera so that it can vertically capture images of the horizontal road surface, and set the shooting frequency according to the robot's moving speed so that there is more overlapping area in adjacent frames.

[0015] Step 1.2: Use Fourier transform to convert the two adjacent frames of the captured image to the frequency domain, and establish logarithmic polar coordinates with the image center as the origin.

[0016] Step 1.3: Identify the overlapping regions in two adjacent frames, and convert the changes in rotation and scaling of the overlapping regions in the two frames into translations of two logarithmic polar coordinate axes, thereby obtaining the sum of the angles and scaling coefficients of the two images.

[0017] Step 1.4: Based on the positional changes of the overlapping region in the two-dimensional plane, the translational amounts of the robot in the horizontal and vertical directions are obtained. Combined with the angle and scaling coefficients of the two adjacent frames, the change in the robot's three-dimensional rectangular coordinates in the time interval between the two adjacent frames can be calculated, thus obtaining the robot's three-dimensional rectangular coordinates relative to the initial moment.

[0018] Step 1.5: Repeat steps 1.2 to 1.4, and continuously use adjacent frame images to calculate the changes in the robot's three-dimensional coordinates within a certain time period.

[0019] Furthermore, the specific process of step 2 includes:

[0020] Step 2.1: Use a 3D lidar sensor to scan the surrounding environment 360° and extract the laser points of the surrounding fruit trees.

[0021] Step 2.2: After the sensor receives the data from the laser points of the surrounding fruit trees, the system will establish a three-dimensional Cartesian coordinate system centered on the lidar, and then convert the data of each laser point into three-dimensional coordinate data.

[0022] Step 2.3: Select enough fruit tree laser points as positioning reference points. As the robot moves, the coordinates of the reference laser points change continuously. Based on the coordinate changes of the reference points at the start and end times, the change in the robot's position can be calculated, thus obtaining the robot's three-dimensional rectangular coordinates relative to the start time.

[0023] Furthermore, the specific process of step 3 includes:

[0024] Step 3.1: Convert the two sets of positioning coordinates obtained from the binocular vision camera and the lidar sensor into geodetic coordinates.

[0025] Step 3.2: Establish a fusion model, initialize fusion parameters, and linearly fuse the two sets of sensor positioning data that have been converted into geodetic coordinates.

[0026] Furthermore, the specific process of step 4 includes:

[0027] Step 4.1: First, ensure that the GNSS RTK positioning is in normal positioning state to ensure the accuracy of the GNSS positioning results.

[0028] Step 4.2: Compare the high-precision GNSS positioning results with the sensor fusion positioning results, and adjust the relevant parameters of the fusion positioning algorithm according to the comparison results.

[0029] Step 4.3: Repeat steps 4.1 and 4.2 to continuously adjust the parameters of the fusion positioning algorithm so that the sensor fusion positioning result gradually approaches the GNSS positioning result, thereby improving the accuracy and reliability of the fusion positioning result.

[0030] Furthermore, the methods for detecting whether the GNSS positioning signal is normal in step 5 include a direct method and an indirect method. The direct method refers to directly utilizing the GGA statement in the GNSS transmission protocol. Taking the BeiDou satellite system as an example, it detects the mode indicator bit data in the BDGGA statement. If it is 0, the positioning is invalid; if it is not 0, the positioning is valid. The indirect method involves calculating and storing the positioning time and the time difference between the received signal of two adjacent frames of positioning data, and comparing it with a pre-set time threshold. If the positioning time and the time difference between two frames exceed the threshold, it is determined that the GNSS positioning signal is interfered with.

[0031] Furthermore, step 5 is continuously activated throughout the robot localization process. When the GNSS signal is detected to be normal, the fused localization data is calibrated. When the localization data accuracy is close to the GNSS accuracy, the calibration is no longer performed. Meanwhile, the GNSS signal continues to be monitored in real time. When interference with the GNSS localization signal is detected, step 6 is continued.

[0032] Furthermore, the specific process of step 6 includes:

[0033] Step 6.1: When interference is detected in the GNSS positioning signal, the system immediately switches the positioning signal to the calibrated sensor fusion positioning result. At this time, the accuracy of the result is consistent with the accuracy when the GNSS is not lost.

[0034] Step 6.2: After the positioning signal is switched, the GNSS positioning signal continues to be detected. When the interference disappears and the GNSS positioning accuracy is restored, the system switches the positioning signal back to the GNSS positioning result.

[0035] Compared with existing technologies, the beneficial effects of this invention are as follows: This invention compensates for the positioning accuracy when GNSS positioning signals are interfered with by using a binocular vision camera and a lidar sensor. A fusion algorithm is designed to fuse the robot positioning information acquired by the binocular vision camera and lidar. Then, the high-precision positioning signal when the GNSS RTK positioning state is not locked is used to calibrate the fused positioning information, so that the sensor fused positioning information can achieve the positioning accuracy of GNSS. When the system detects that the GNSS signal is interfered with, it will use the calibrated high-precision fused positioning information to replace the GNSS positioning signal. When the robot is in a complex outdoor environment such as an orchard, where satellite positioning signals are frequently interfered with, this method can effectively solve the problem of decreased positioning accuracy. Moreover, this method has low complexity and low computational load, and can quickly and frequently perform signal calibration and switching, thereby providing the robot with high-precision real-time positioning information. Attached Figure Description

[0036] Figure 1 This is a schematic diagram of a scenario illustrating a high-precision real-time positioning method for robots in orchards according to the present invention.

[0037] Figure 2 This is a flowchart illustrating a high-precision real-time positioning method for robots in orchards according to the present invention. Detailed Implementation

[0038] This invention improves the accuracy of robot positioning signals when GNSS positioning signals are interfered with by using a binocular vision camera and a lidar sensor. This method can enable robots to have high-precision real-time positioning signals in complex environments such as orchards.

[0039] Combination Figure 1As shown, when the robot walks through rows of trees in an orchard, its GNSS positioning signal is frequently blocked by the trees. This method uses a binocular vision camera to capture high-frequency ground images and continuously calculates the robot's changes in lateral, longitudinal, and height based on the changes in adjacent frames. Simultaneously, a lidar scanner is used to scan the surrounding environment, selecting the trees as reference points. Figure 1 The system uses seven example reference laser points to calculate the robot's coordinate changes based on the changes in distance from the fruit trees. It also fuses the positioning signals from the two sensors and calibrates the fused signal during the time periods when GNSS signals are good in the gaps between the fruit trees, so that the accuracy reaches that of GNSS positioning signals. When the satellite signal is blocked again, the system will switch to the fused positioning information to ensure the robot's high-precision real-time positioning.

[0040] Combination Figure 2 As shown, a high-precision real-time positioning method for robots in orchards includes the following steps:

[0041] Step 1: Binocular vision localization. Using a binocular vision camera to collect road surface information, the robot's localization information is calculated. The specific steps are as follows:

[0042] Step 1.1: Adjust the binocular vision camera so that it can vertically capture images of the horizontal road surface, and set the shooting frequency according to the robot's moving speed so that there is more overlapping area in adjacent frames.

[0043] Step 1.2: Use Fourier transform to convert the two adjacent frames of the captured image to the frequency domain, and establish logarithmic polar coordinates with the image center as the origin respectively;

[0044] Step 1.3: Identify the overlapping regions in two adjacent frames, and convert the changes in rotation and scaling of the overlapping regions in the two frames into translations of the two logarithmic polar coordinate axes, thereby obtaining the sum of the angles and scaling coefficients of the two images.

[0045] Step 1.4: Based on the positional changes of the overlapping area in the two-dimensional plane, the translational amounts of the robot in the horizontal and vertical directions are obtained. Combined with the angle and scaling coefficients of the two adjacent frames, the change in the robot's three-dimensional rectangular coordinates in the time interval between the two adjacent frames can be calculated, thus obtaining the robot's three-dimensional rectangular coordinates relative to the initial moment.

[0046] Step 1.5: Repeat steps 1.2 to 1.4, and continuously use adjacent frame images to calculate the robot's three-dimensional coordinates (X1, Y1, Z1) relative to the initial position within a certain time period.

[0047] Step 2: LiDAR positioning. The robot's location information is determined by scanning the surrounding environment using LiDAR. The specific steps are as follows:

[0048] Step 2.1: Use a 3D LiDAR sensor to scan the surrounding environment and extract the laser points of the surrounding fruit trees.

[0049] Step 2.2: After the sensor receives the data from the laser points of the surrounding fruit trees, the system will establish a three-dimensional Cartesian coordinate system centered on the lidar, and then convert the data of each laser point into three-dimensional coordinate data.

[0050] Step 2.3: Select enough fruit tree laser points as positioning reference points. As the robot moves, the coordinates of the reference laser points change continuously. Based on the coordinate changes of the reference points at the start and end times, the change in the robot's position can be calculated, thus obtaining the robot's three-dimensional rectangular coordinates (X2, Y2, Z2) relative to the start time.

[0051] Step 3: Sensor positioning information fusion. The positioning signals from each positioning module are fused to obtain a more accurate positioning result. The specific steps are as follows:

[0052] Step 3.1: Convert the two sets of positioning coordinates obtained from the binocular vision camera and the LiDAR sensor into geodetic coordinates:

[0053]

[0054] Where (B1,L1,H1) represents the geodetic coordinates after the binocular vision positioning coordinate transformation, and (B2,L2,H2) represents the geodetic coordinates after the lidar positioning coordinate transformation.

[0055] Step 3.2: Establish the fusion model as follows:

[0056]

[0057] Where (B, L, H) represents the fused positioning result, α1 and α2 are the fusion coefficients for latitude coordinates, β1 and β2 are the fusion coefficients for longitude coordinates, γ1 and γ2 are the fusion coefficients for altitude coordinates, and a1, a2, b1, b2, c1, and c2 are the fusion coefficient adjustment speed parameters. and These are the parameters for fine-tuning the coordinates.

[0058] Initialize the fusion parameters. The fusion coefficients α1, α2, β1, β2, γ1, and γ2 for latitude, longitude, and altitude are set to 0.5 by default. The fusion coefficient adjustment speed parameters a1, a2, b1, b2, c1, and c2 are set to 1 by default. Coordinate fine-tuning parameters... and The default value is 0. Then, the two sets of sensor positioning data, which have already been converted to geodetic coordinates, are fused together.

[0059] Step 4: Merge positioning information for calibration. When the GNSS RTK positioning status is normal, merge the high-precision positioning signal (B) of the GNSS. GNSS ,L GNSS H GNSS The first step of parameter tuning involves comparing the positioning results from each sensor with the actual position. Taking latitude coordinate B as an example:

[0060]

[0061]

[0062] If the changes in parameters α1 and α2 during adjustment are too small or too large, it will lead to excessive parameter adjustments. In such cases, a1 and a2 can be adjusted. When a1 and a2 are greater than 1, the adjustment range of latitude parameters α1 and α2 increases; when a1 and a2 are less than 1, the adjustment range of latitude parameters α1 and α2 decreases. If the latitude coordinates of the GNSS are approximately the same as the latitude coordinates of a certain sensor, the corresponding latitude parameter α is 0, indicating that the latitude of that sensor does not need adjustment. The latitude parameter α speeds up latitude fusion. Similarly, the parameter adjustments during the fusion of longitude coordinates L and altitude coordinates H are also handled in this way.

[0063] After the first step of parameter tuning, the high-precision positioning signal (B) of GNSS is used. GNSS ,L GNSS H GNSS The second step of parameter tuning involves comparing the fused localization information (B, L, H) from each sensor obtained by the fusion algorithm with the fused localization information from each sensor.

[0064]

[0065] Repeat the first and second parameter tuning steps as described above until the positioning information (B, L, H) is fused with the high-precision GNSS positioning signal (B). GNSS ,L GNSS H GNSS If the difference is less than the set threshold, then the accuracy of the fused positioning information is approximately the accuracy of the GNSS positioning signal.

[0066] Step 5: Check if the GNSS positioning signal is normal. The methods for checking the GNSS positioning signal include a direct method and an indirect method. The direct method involves directly utilizing the GGA statement in the GNSS transmission protocol. Taking the BeiDou satellite system as an example, the mode indicator bit data in the BDGGA statement is checked. If it is 0, the positioning is invalid; if it is not 0, the positioning is valid. The indirect method involves calculating and saving the positioning time and the time difference between the received signal of two adjacent frames of positioning data, and comparing it with a preset time threshold. If the positioning time and the time difference between two frames exceed the threshold, the GNSS positioning signal is determined to be interfered with.

[0067] Specifically, the GNSS positioning signal interference detection is continuously enabled throughout the robot positioning process. When the GNSS signal is detected to be normal, the fused positioning data is calibrated. When the positioning data accuracy is close to the GNSS accuracy, the calibration is no longer performed. At the same time, the GNSS signal continues to be in real-time detection. When interference is detected in the GNSS positioning signal, step 6 is continued.

[0068] Step 6: Positioning signal switching. Continuously update the GNSS signal measurement results. If the GNSS is not interfered with, the fusion positioning information / GNSS positioning signal selection switch selects to output the GNSS positioning signal. When interference is detected in the GNSS positioning signal, further determine whether the accuracy of the corrected fusion positioning information reaches the positioning accuracy when the GNSS is working normally. If the accuracy meets the requirements, the fusion positioning information / GNSS positioning signal selection switch selects to output the fusion positioning information. If the accuracy does not meet the requirements, continue adjusting the fusion parameters until the accuracy of the fusion positioning information meets the requirements, or the GNSS returns to normal.

[0069] The positioning described in steps 1 and 2 is not limited to binocular vision and lidar sensors; other sensors can also be selected as positioning signal sources for fusion positioning. Furthermore, the method of the present invention is not limited to the order of the method steps.

[0070] For illustrative purposes, the specific embodiments described above are merely exemplary and are intended to enable those skilled in the art to better understand this patent. They should not be construed as limiting the scope of this patent. All technical solutions obtained by means of equivalent substitution or equivalent transformation fall within the protection scope of this invention.

Claims

1. A high-precision real-time positioning method for robots in orchards, characterized in that, include: Multiple positioning information of the robot is determined through various methods; Multiple location information sets are fused using a fusion algorithm to obtain fused location information. To calibrate the fused positioning information, when the GNSS RTK positioning is in normal condition, compare the GNSS positioning signal with the fused positioning information, and adjust the fusion algorithm parameters according to the comparison results until the fused positioning information meets the accuracy requirements. Check if the GNSS positioning signal is normal. If the GNSS positioning signal is normal, repeat the calibration and fusion of positioning information; otherwise, proceed to the following steps. The positioning signal is switched to the calibrated fused positioning information, and the GNSS positioning signal is monitored. If the GNSS positioning signal is normal, the positioning signal is switched to GNSS positioning. The method of determining multiple positioning information of the robot includes obtaining corresponding binocular vision positioning information and lidar positioning information through binocular vision camera and lidar sensor respectively; The process of fusing multiple location information entries using a fusion algorithm to obtain fused location information specifically includes: Step 3.1: Convert the binocular visual positioning information and lidar positioning information coordinates into global geodetic coordinates; Step 3.2: Establish a fusion model, initialize fusion parameters, and linearly fuse the two sets of positioning data that have been converted to geodetic coordinates; the fusion model is as follows: Where (B,L,H) represents the fused positioning information, α1 and α2 are the fusion coefficients for latitude coordinates, β1 and β2 are the fusion coefficients for longitude coordinates, γ1 and γ2 are the fusion coefficients for altitude coordinates, and a1, a2, b1, b2, c1, and c2 are the fusion coefficient adjustment speed parameters. and For fine-tuning parameters of the coordinates; The initial fusion parameters are as follows: the fusion coefficients α1, α2, β1, β2, γ1, and γ2 for latitude, longitude, and altitude are initialized to 0.5; the fusion coefficient adjustment speed parameters a1, a2, b1, b2, c1, and c2 are initialized to 1; and the coordinate fine-tuning parameters are... and =0; The adjustment fusion algorithm parameters include: GNSS positioning signal (B GNSS ,L GNSS H GNSS The parameters are compared one by one with multiple positioning information to perform the first step of parameter tuning; for latitude coordinate B, then: B1 represents the latitude coordinates after the binocular vision positioning coordinates are converted, and B2 represents the latitude coordinates after the lidar positioning coordinates are converted. GNSS positioning signal (B GNSS ,L GNSS H GNSS The parameters are compared with the fused positioning information (B, L, H) to perform the second step of parameter tuning: GNSS high-precision positioning signal (B GNSS ,L GNSS H GNSS The second step of parameter tuning involves comparing the fused localization information (B, L, H) from each sensor obtained by the fusion algorithm with the fused localization information from each sensor.

2. The high-precision real-time positioning method for robotic orchards according to claim 1, characterized in that, The calibration fusion positioning information specifically includes: Step 4.1: First, confirm that the GNSS RTK positioning is in normal positioning state and ensure that the accuracy of the GNSS positioning results meets the set accuracy requirements; Step 4.2: Compare the GNSS positioning result with multiple positioning information and fused positioning information respectively, and adjust the parameters of the fusion algorithm according to the comparison results; Step 4.3: Repeat steps 4.1 and 4.2, continuously adjusting the parameters of the fusion algorithm to make the fused positioning information increasingly approach the GNSS positioning result, until the fused positioning information meets the accuracy requirements.

3. The high-precision real-time positioning method for robotic orchards according to claim 1, characterized in that, The acquisition of binocular visual positioning information via a binocular vision camera specifically includes: Step 1.1: Adjust the binocular vision camera to capture images of the horizontal road surface vertically, and set the shooting frequency according to the robot's moving speed so that there are overlapping areas in adjacent frames. Step 1.2: Use Fourier transform to convert the two adjacent frames of the captured image to the frequency domain, and establish logarithmic polar coordinates with the image center as the origin respectively; Step 1.3: Identify the overlapping regions in two adjacent frames, and convert the changes in rotation and scaling of the overlapping regions in the two frames into translations of the two logarithmic polar coordinate axes to obtain the sum of the angles and scaling coefficients of the two images. Step 1.4: Based on the positional changes of the overlapping area in the two-dimensional plane, obtain the translational amounts of the robot in the horizontal and vertical directions. Combine the angle and scaling coefficients of the two adjacent frames to calculate the change in the robot's three-dimensional rectangular coordinates within the time interval of the two adjacent frames, and obtain the robot's three-dimensional rectangular coordinates relative to the initial time. Step 1.5: Repeat steps 1.2 to 1.4 to continuously obtain the robot's three-dimensional coordinate changes within a certain time period using adjacent frame images.

4. The high-precision real-time positioning method for robotic orchards according to claim 3, characterized in that, The acquisition of lidar positioning information through a lidar sensor specifically includes: Step 2.1: Use a 3D LiDAR sensor to scan the surrounding environment 360° and extract the laser points of the surrounding fruit trees; Step 2.2: Based on the laser point data, establish a three-dimensional Cartesian coordinate system centered on the lidar, and convert the data of each laser point into three-dimensional coordinate data; Step 2.3: Select multiple fruit tree laser points as positioning reference points, calculate the robot's position change based on the coordinate changes of the reference points at the start and end times, and obtain the robot's three-dimensional rectangular coordinates relative to the start time.

5. The high-precision real-time positioning method for robotic orchards according to claim 1, characterized in that, The detection of whether the GNSS positioning signal is normal includes direct and indirect methods.

6. The high-precision real-time positioning method for robotic orchards according to claim 5, characterized in that, The direct method refers to directly using the GGA statement in the GNSS transmission protocol. For the BeiDou satellite system, the mode indicator bit data in the BDGGA statement is detected. If the mode indicator bit data is 0, it indicates that the positioning is invalid; if it is not 0, it indicates that the positioning is valid.

7. The high-precision real-time positioning method for robotic orchards according to claim 5, characterized in that, The indirect method refers to calculating and storing the positioning time and the time difference of the received signal between two adjacent frames of positioning data, and comparing them with a preset time threshold. If the positioning time and the time difference of the received signal between two frames exceed the threshold, it is determined that the GNSS positioning signal is interfered with.

Images