An ultra-high precision positioning method for orchard picking robot

Abstract

The application discloses an ultra-high-precision positioning method for an orchard picking robot, first collects laser positioning information, visual positioning information, IMU pose estimation information and differential RTK pose estimation data respectively, and respectively distributes and fuses the estimated positioning poses based on different weights, and outputs final robot pose information, the application fuses laser, visual, IMU and differential RTK signals, makes up for the problems that laser, visual and IMU are not accurate enough, applicable scenes are inconsistent, satellite signals are poor when the tree crown is blocked, and the positioning accuracy of differential RTK is unreliable, fuses objective weight and subjective weight, and improves the positioning accuracy by combining the characteristics of various sensors, performs adaptive dynamic weight evaluation, so that the fusion result is more accurate and the robustness is higher.

Description

Technical Field

[0001] This invention belongs to the field of robot positioning, specifically relating to an ultra-high precision positioning method for orchard harvesting robots. Background Technology

[0002] With the continuous improvement of social development and people's living standards, market demand is booming, leading to large-scale fruit tree planting. In recent years, the country has attached great importance to the development of agricultural robots and actively promoted the use of automated agricultural machinery. While satellite navigation and positioning systems are commonly used for robot positioning, complex operating environments, such as orchards, with their numerous tree canopies obstructing satellite signals, significantly reduce positioning accuracy and make the positioning results unstable. Indoor positioning methods based on 3D LiDAR are widely used, but outdoor positioning accuracy is greatly affected by uneven ground leading to uneven laser scanning, and the limited corner / surface feature point information provided by fixed-shape objects such as walls, floors, and tabletops. Visual sensors are also susceptible to lighting conditions outdoors, resulting in unstable positioning accuracy. Inertial sensors, due to error accumulation, can drift by tens of meters over time, limiting their use to auxiliary positioning tools. Different sensors have their own characteristics, and a single sensor is insufficient to solve the positioning problem in complex environments like orchards. Therefore, researching a high-precision real-time positioning method for orchard robots using multi-sensor fusion is urgently needed.

[0003] Different sensors offer varying positioning accuracy and stability. Commonly used laser, vision, and inertial sensors provide positioning accuracy at the decimeter level. Various fusion positioning algorithms for these sensors can reduce the accuracy to around 10cm, meeting high-precision positioning requirements. Currently, differential RTK (real-time kinematic) positioning relying on global satellite navigation systems achieves an accuracy of 1.5cm. However, its drawback is that when the signal is blocked, the positioning accuracy can drift to over 10cm, making it unreliable for continuous differential RTK positioning. Summary of the Invention

[0004] To address the aforementioned problems, the present invention aims to provide an ultra-high precision positioning method for orchard harvesting robots. This method enables ultra-high precision real-time positioning even under complex conditions such as poor lidar point cloud matching in outdoor orchards, significant light-related influences on visual sensors, and low positioning accuracy due to satellite signal obstruction. It utilizes a multi-sensor complementary fusion approach to achieve this positioning.

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

[0006] A method for ultra-high precision positioning of an orchard harvesting robot includes the following steps:

[0007] Step 1: Based on the LiDAR scanning of the surrounding environment, obtain point cloud information, and correct the LiDAR point cloud distortion caused by motion based on IMU information, and align it with the global point cloud.

[0008] Step 2: The binocular vision camera collects environmental information, extracts feature point information, and combines it with LiDAR and IMU information to perform feature point depth correlation and perform inter-frame motion estimation.

[0009] Step 3: Collect satellite positioning data. Determine whether the data is available based on the number of satellites. If available, use RTK pose estimation data as the initial value. If unavailable, use laser-vision fusion pose estimation data and IMU information fused by piecewise fitting method to perform IMU pose estimation data at the current time.

[0010] Step 4: Input the laser positioning information, visual positioning information, IMU pose estimation information and differential RTK pose estimation data into the tightly coupled data optimization model, output the optimized pose, smooth the optimized pose, and output the laser-visual tightly coupled positioning pose.

[0011] Step 5: Determine the number of satellites found and output the initial pose of the robot based on the number of satellites;

[0012] Step 6: Determine if the satellite signal has been lost, and optimize the robot pose based on the satellite signal loss time;

[0013] Step 7: Reweight the differential RTK pose estimation data, IMU pose estimation data, and laser-vision tightly coupled positioning pose, and output the final pose as the robot pose.

[0014] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0015] (1) This invention integrates laser, vision, IMU and differential RTK signals to make up for the problems of insufficient accuracy of laser, vision and IMU, inconsistent applicable scenarios, poor satellite signal and unreliable positioning accuracy of differential RTK when the tree canopy is blocked;

[0016] (2) This invention integrates objective weighting and subjective weighting, and combines the characteristics of each sensor to improve positioning accuracy and perform adaptive dynamic weight evaluation, so that the fusion result is more accurate and more robust.

[0017] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. Attached Figure Description

[0018] Figure 1 This is a schematic diagram of the ultra-high precision positioning scenario for the orchard harvesting robot of the present invention.

[0019] Figure 2 This is a schematic diagram of the ultra-high precision positioning method for the orchard harvesting robot of the present invention. Detailed Implementation

[0020] Combination Figure 1 When the robot walks in the orchard, lidar, vision sensors, and inertial sensors continuously collect environmental information and the robot's motion status. However, the signals from the Global Navigation Satellite System and differential RTK are blocked by the tree canopy, and the satellites' pose relative to the robot in space is constantly changing. The number of satellites detected at different times is not consistent, making it impossible to guarantee the continuity and stability of the data.

[0021] Therefore, this invention proposes an ultra-high precision positioning method for orchard harvesting robots, comprising the following steps:

[0022] Step 1: Based on the LiDAR scanning of the surrounding environment, obtain point cloud information. Based on the IMU information, correct the LiDAR point cloud distortion caused by motion and align it with the global point cloud.

[0023] Step 1-1: Determine the intrinsic and extrinsic parameter transformation matrices of the IMU and the lidar. Fix the IMU and lidar together, first perform IMU intrinsic parameter calibration to obtain the IMU's stationary error, i.e., the intrinsic parameter matrix, and then perform IMU-lidar extrinsic parameter joint calibration to obtain the extrinsic parameter transformation matrices of the IMU and lidar, including translation and rotation matrices.

[0024] Steps 1-2: Synchronize the lidar and IMU timestamps. Based on the timestamps of the lidar and IMU, obtain the IMU data that is closest to the timestamp of a certain frame of point cloud. Subtract the timestamps to find the difference. If the time difference is less than the synchronization threshold, it is output as the synchronization data. If the time difference is greater than the synchronization threshold, the data is discarded and the above process is repeated for the next frame of lidar sampling data.

[0025] Steps 1-3: The output of a frame of data from the LiDAR is in the form of a point cloud. Based on the LiDAR parameters, the point cloud data output by the LiDAR is processed into line bundles, and the line bundle to which each point belongs and the relative scanning time of each point in this frame of point cloud are recorded.

[0026] Steps 1-4: Obtain the Euler angles of the three axes in the IMU coordinate system relative to the world coordinate system and the acceleration on the three axes. Remove the influence of gravity to obtain the acceleration of the IMU in the world coordinate system. Obtain the displacement and velocity corresponding to each frame of IMU data in the world coordinate system. Use the obtained motion information for interpolation to calculate the distortion compensation transformation matrix of each laser point relative to the laser point at the starting point scanning time.

[0027] Steps 1-5: For each laser point, multiply the original radar laser point coordinates by the compensation transformation matrix to obtain the corrected laser point coordinates, i.e., the laser positioning information.

[0028] Step 2: The binocular vision camera acquires environmental information, extracts feature point information, and combines it with LiDAR and IMU information to perform feature point depth correlation and inter-frame motion estimation.

[0029] Step 2-1: Fix the binocular vision camera, LiDAR, and IMU, perform joint calibration of the extrinsic parameters of the binocular vision camera-LiDAR-IMU, determine the extrinsic parameter transformation matrix of the camera, LiDAR, and IMU, including the translation matrix and rotation matrix, and perform intrinsic parameter calibration of the binocular vision camera to determine the intrinsic parameter calibration matrix.

[0030] Step 2-2: Combining IMU data and the intrinsic and extrinsic parameter transformation matrix calibrated in Step 2-1, the acquired image is distorted, enhanced by filtering, and visual feature points are extracted based on RGB information.

[0031] Steps 2-3: Collect point cloud depth information from multiple adjacent frames of LiDAR;

[0032] Steps 2-4: Project the visual feature points and laser point cloud points onto the same coordinate system, search for the n nearest depth point clouds near each visual feature point, determine the depth value of the visual feature point from these n laser points, and determine the three-dimensional coordinates of the feature point.

[0033] Steps 2-5: Perform inter-frame feature point matching on visual feature points with depth labels. Based on the matching results, construct an objective function using the nonlinear least squares method, perform iterative optimization, and obtain the pose estimate, i.e., visual localization information.

[0034] Step 3: Collect satellite positioning data. Determine the availability of the data based on the number of satellites. If available, use RTK pose estimation data as the initial value. If unavailable, fuse IMU information using a piecewise fitting method based on laser-vision fusion pose to obtain the IMU pose estimation data for the current moment.

[0035] Step 3-1: Collect satellite positioning data. If the number of satellites searched is ≥ M, the satellite signal is considered available. The signal receiver fixed on the robot receives both the satellite signal and the carrier phase differential correction signal transmitted through the base station. Based on these two signals, differential calculation is performed, and the output is the differential RTK pose. At the same time, the differential RTK pose received at the previous moment is used as the initial value, and the process proceeds to step 3-2.

[0036] If the number of satellites searched is less than M, the satellite signal is considered unusable, the data is ignored, the laser-visual fusion pose received at the previous moment is used as the initial value, and the process proceeds to step 3-3.

[0037] Step 3-2: Perform coordinate transformation on the differential RTK pose, transforming it from the world coordinate system to the robot coordinate system:

[0038]

[0039]

[0040]

[0041] in, The coordinates are in the robot's coordinate system. Coordinates in the world coordinate system, i.e., RTK coordinates provided by the global navigation satellite system. This is a three-axis coordinate transformation matrix, corresponding to Pitch, roll, and heading angles provided for global satellite navigation systems;

[0042] The transformation yields the RTK pose estimate for the current moment.

[0043] Step 3-3: Based on the IMU data, calculate the change in attitude angle between the current frame and the previous frame, and add it to the pose at the previous moment to obtain the IMU pose estimate at the current moment.

[0044] Step 4: Input the laser positioning information, visual positioning information, IMU pose estimation information and differential RTK pose estimation data into the tightly coupled data optimization model, perform graph optimization using Bayesian tree mapping, output the optimized pose, smooth the optimized pose, and output the laser-visual tightly coupled positioning pose.

[0045] GTSAM is a C++ library based on factor graphs, which provides the iSAM and iSAM2 algorithms. Using GTSAM tools, we construct a data-tightly coupled optimization model. We input laser positioning information, visual positioning information, IMU pose estimation information, and RTK pose estimation information as custom factors into the model. The model uses Bayesian tree mapping for graph optimization, smooths the optimized pose, and outputs the laser-visual tightly coupled positioning pose.

[0046] Step 5: Determine the number of satellites found and output the initial robot pose based on the number of satellites:

[0047] If the number of satellites searched is ≥ N, that is, the signal is very good, the positioning accuracy of differential RTK can reach 1.5cm. Then, the differential RTK pose estimation data is directly output as the robot pose, and the current positioning information is recorded as the initial value for the next moment.

[0048] If the number of searched satellites satisfies: M satellites < number of satellites < N satellites, assign weights to the differential RTK pose estimation information, the IMU pose estimation information, and the laser-vision tightly coupled localization pose according to the vector similarity, and fuse the three poses based on the weights to obtain a fused pose estimation result:

[0049] Determine the length similarity between every two vectors among the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose respectively :

[0050]

[0051] Determine the direction similarity between every two vectors among the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose respectively :

[0052]

[0053] where, represents the included angle between two vectors;

[0054] Determine the similarity between every two vectors among the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose according to the length similarity and the direction similarity respectively :

[0055]

[0056] Then the total similarity of any vector with other vectors is:

[0057]

[0058]

[0059] Assign weights to the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose according to the total similarity of the above formula, to obtain the fused pose estimation result :

[0060]

[0061]

[0062] where, , , represent the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose respectively, , where k=1, 2, 3 respectively represent the differential RTK pose estimation data, the IMU pose estimation data, and the pose weight of the laser-vision tightly coupled localization.

[0063] Step 6: Determine whether the satellite signal is lost, and optimize the robot pose according to the satellite signal loss duration:

[0064] For the IMU, there is a cumulative drift error problem. When the time is too long, the error will accumulate to an intolerable level. Therefore, it is necessary to continuously input highly reliable initial values for pose estimation. The low-frequency output differential RTK signal is the most suitable as the initial value for the high-frequency output IMU data, so it is necessary to judge the satellite signal loss duration:

[0065] When the satellite signal is not lost, that is, the loss duration is 0 seconds, proceed to Step 7;

[0066] When the satellite signal loss duration is less than L seconds, the IMU drift is small, and accurate pose estimation can be performed based on historical data. The differential RTK pose estimation data is excluded, that is, the weight of the differential RTK pose estimation data is set to 0, and the process returns to execute Step 5;

[0067] When the satellite signal loss duration is greater than or equal to L seconds, the IMU drift is too large. The differential RTK pose estimation data and the IMU pose estimation data are excluded, that is, the weights of the differential RTK pose estimation data and the IMU pose estimation data are set to 0, and the process returns to execute Step 5.

[0068] Step 7: Reassign weights to the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose, and output the final pose as the robot pose, which is specifically as follows:

[0069] Step 7-1: Assign subjective weights to the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose to obtain the estimated pose :

[0070]

[0071]

[0072] where, is the subjective weight;

[0073] Step 7-2: Determine the weight set combination, the weight vector , then any combination of the weight vector is denoted as:

[0074]

[0075]

[0076] In the formula, The coefficients are linear combination coefficients. One possible combination of weights;

[0077] Step 7-3: Construct the optimization model:

[0078]

[0079] Obtain the optimal linear combination coefficients gather ;

[0080] Step 7-4: Adjust the weights After normalization, the final weight set is obtained. Determine the final output of the robot's optimized pose. Initial pose set :

[0081]

[0082]

[0083]

[0084] Example

[0085] A method for ultra-high precision positioning of an orchard harvesting robot includes the following steps:

[0086] Step 1: Based on the LiDAR scanning of the surrounding environment, obtain point cloud information. Based on the IMU information, correct the LiDAR point cloud distortion caused by motion and align it with the global point cloud.

[0087] Step 1-1: Determine the intrinsic and extrinsic parameter transformation matrices of the IMU and the lidar. Fix the IMU and lidar together, first perform IMU intrinsic parameter calibration to obtain the IMU's stationary error, i.e., the intrinsic parameter matrix, and then perform IMU-lidar extrinsic parameter joint calibration to obtain the extrinsic parameter transformation matrices of the IMU and lidar, including translation and rotation matrices.

[0088] Steps 1-2: Synchronize the lidar and IMU timestamps. Based on the timestamps of the lidar and IMU, obtain the IMU data that is closest to the timestamp of a certain frame of point cloud. Subtract the timestamps to find the difference. If the time difference is less than the synchronization threshold, it is output as the synchronization data. If the time difference is greater than the synchronization threshold, the data is discarded and the above process is repeated for the next frame of lidar sampling data.

[0089] Steps 1-3: The output of a frame of data from the LiDAR is in the form of a point cloud. Based on the LiDAR parameters, the point cloud data output by the LiDAR is processed into line bundles, and the line bundle to which each point belongs and the relative scanning time of each point in this frame of point cloud are recorded.

[0090] Steps 1-4: Obtain the Euler angles of the three axes in the IMU coordinate system relative to the world coordinate system and the acceleration on the three axes. Remove the influence of gravity to obtain the acceleration of the IMU in the world coordinate system. Obtain the displacement and velocity corresponding to each frame of IMU data in the world coordinate system. Use the obtained motion information for interpolation to calculate the distortion compensation transformation matrix of each laser point relative to the laser point at the starting point scanning time.

[0091] Steps 1-5: For each laser point, multiply the original radar laser point coordinates by the compensation transformation matrix to obtain the corrected laser point coordinates, i.e., the laser positioning information.

[0092] Step 2: The binocular vision camera acquires environmental information, extracts feature point information, and combines it with LiDAR and IMU information to perform feature point depth correlation and inter-frame motion estimation.

[0093] Step 2-1: Fix the binocular vision camera, LiDAR, and IMU, perform joint calibration of the extrinsic parameters of the binocular vision camera-LiDAR-IMU, determine the extrinsic parameter transformation matrix of the camera, LiDAR, and IMU, including the translation matrix and rotation matrix, and perform intrinsic parameter calibration of the binocular vision camera to determine the intrinsic parameter calibration matrix.

[0094] Step 2-2: Combining IMU data and the intrinsic and extrinsic parameter transformation matrix calibrated in Step 2-1, the acquired image is distorted, enhanced by filtering, and visual feature points are extracted based on RGB information.

[0095] Steps 2-3: Collect point cloud depth information from multiple adjacent frames of LiDAR;

[0096] Steps 2-4: Project the visual feature points and laser point cloud points onto the same coordinate system, search for the n nearest depth point clouds near each visual feature point, determine the depth value of the visual feature point from these n laser points, and determine the three-dimensional coordinates of the feature point.

[0097] Steps 2-5: Perform inter-frame feature point matching on visual feature points with depth labels. Based on the matching results, construct an objective function using the nonlinear least squares method, perform iterative optimization, and obtain the pose estimate, i.e., visual localization information.

[0098] Step 3: Collect satellite positioning data. Determine the availability of the data based on the number of satellites. If available, use RTK pose estimation data as the initial value. If unavailable, fuse IMU information using a piecewise fitting method based on laser-vision fusion pose to obtain the IMU pose estimation data for the current moment.

[0099] Step 3-1: Collect satellite positioning data. If the number of satellites searched is ≥ M, the satellite signal is considered available. The signal receiver fixed on the robot receives both the satellite signal and the carrier phase differential correction signal transmitted through the base station. Based on these two signals, differential calculation is performed, and the output is the differential RTK pose. At the same time, the differential RTK pose received at the previous moment is used as the initial value, and the process proceeds to step 3-2.

[0100] If the number of satellites searched is less than M, the satellite signal is considered unusable, the data is ignored, the laser-visual fusion pose received at the previous moment is used as the initial value, and the process proceeds to step 3-3.

[0101] Step 3-2: Perform coordinate transformation on the differential RTK pose, transforming it from the world coordinate system to the robot coordinate system:

[0102]

[0103]

[0104]

[0105] in, The coordinates are in the robot's coordinate system. Coordinates in the world coordinate system, i.e., RTK coordinates provided by the global navigation satellite system. This is a three-axis coordinate transformation matrix, corresponding to Pitch, roll, and heading angles provided for global satellite navigation systems;

[0106] The transformation yields the RTK pose estimate for the current moment.

[0107] Step 3-3: Based on the IMU data, calculate the change in attitude angle between the current frame and the previous frame, and add it to the pose at the previous moment to obtain the IMU pose estimate at the current moment.

[0108] Step 4: Input the laser positioning information, visual positioning information, IMU pose estimation information, and differential RTK pose estimation data into the tightly coupled data optimization model. Perform graph optimization using Bayesian tree mapping, output the optimized pose, and smooth the optimized pose. The output is the laser-vision tightly coupled positioning pose.

[0109] GTSAM is a C++ library based on factor graphs, which provides the iSAM and iSAM2 algorithms. With the GTSAM tool, a data tight coupling optimization model is constructed, into which laser positioning information, visual positioning information, IMU pose estimation information and RTK pose estimation information are input as custom factors. This model adopts Bayesian tree mapping for graph optimization, smoothes the optimized pose, and outputs the laser-vision tight coupling positioning pose.

[0110] Step 5: Determine the number of searched satellites, and determine the initial pose of the robot to be output according to the number of satellites:

[0111] If the number of searched satellites ≥ N, that is, in the case of very good signal, the positioning accuracy of differential RTK can reach 1.5cm, then the differential RTK pose estimation data is directly output as the robot pose, and the current positioning information is recorded as the initial value for the next moment;

[0112] If the number of searched satellites satisfies: M < the number of satellites < N, then weights are assigned to the differential RTK pose estimation information, the IMU pose estimation information, and the laser-vision tight coupling positioning pose according to the vector similarity, and the three poses are fused based on the weights to obtain a fused pose estimation result:

[0113] Respectively determine the length similarity between every two vectors in the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tight coupling positioning pose :

[0114]

[0115] Respectively determine the direction similarity between every two vectors in the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tight coupling positioning pose :

[0116]

[0117] Herein, represents the included angle between two vectors;

[0118] Determine the similarity between every two vectors in the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tight coupling positioning pose according to the length similarity and the direction similarity respectively :

[0119]

[0120] Then the total similarity of any vector with other vectors is:

[0121]

[0122]

[0123] According to the total similarity of the above formula, weights are assigned to the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose, so as to obtain the fused pose estimation result :

[0124]

[0125]

[0126] wherein, , , respectively denote the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose, , k=1,2,3, respectively denote the weights of the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose.

[0127] Step 6: Determine whether the satellite signal is lost, and perform robot pose optimization according to the satellite signal loss duration:

[0128] For IMU, there is a cumulative drift error problem. When the time is too long, the error will accumulate to an intolerable level. Therefore, highly reliable initial values must be continuously input for pose estimation, and the low-frequency output differential RTK signal is the most suitable as the initial value for the high-frequency output IMU data, so it is necessary to determine the satellite signal loss duration:

[0129] When the satellite signal is not lost, that is, the loss duration is 0 second, proceed to Step 7;

[0130] When the satellite signal loss duration is less than L seconds, the IMU drift amount is small, and accurate pose estimation can be performed based on historical data, the differential RTK pose estimation data is excluded, that is, the weight of the differential RTK pose estimation data is set to 0, and the process returns to execute Step 5;

[0131] When the satellite signal loss duration is greater than or equal to L seconds, the IMU drift amount is too large, the differential RTK pose estimation data and the IMU pose estimation data are excluded, that is, the weights of the differential RTK pose estimation data and the IMU pose estimation data are set to 0, and the process returns to execute Step 5.

[0132] Step 7: Perform weight redistribution for the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose, and output the final pose as the robot pose, which is specifically as follows:

[0133] Step 7-1: Assign subjective weights to the differential RTK pose estimation data, IMU pose estimation data, and laser-vision tightly coupled positioning pose to obtain the estimated pose. :

[0134]

[0135]

[0136] in, It is a subjective weight;

[0137] Step 7-2: Determine the combination of weight sets and weight vectors. Then, any combination of weight vectors is denoted as:

[0138]

[0139]

[0140] In the formula, For linear combination coefficients, One possible combination of weights;

[0141] Step 7-3: Construct the optimization model:

[0142]

[0143] Obtain the optimal linear combination coefficients gather ;

[0144] Step 7-4: Adjust the weights After normalization, the final weight set is obtained. Determine the final output of the robot's optimized pose. Initial pose set :

[0145]

[0146]

[0147]

[0148] The method described in this patent was tested on a robotic vehicle equipped with an Ouster 32-line LiDAR, a MicroStrain 3dm-gx5-25 inertial sensor, a Realsense D455 binocular camera, and a Huace CGI-610 integrated navigation system. The experimental vehicle measured positioning accuracy in a tea garden (open and unobstructed) and an orchard (with tree obstruction). The results showed that in open areas, the differential RTK positioning accuracy could reach 1.66 cm. However, with tree obstruction, the accuracy drifted. In severe obstruction, the differential RTK signal was lost, and the satellite signal positioning accuracy drifted to 13.86 cm, significantly lower than the accuracy in unobstructed areas. The IMU's one-hour static zero-bias error was 0.066 cm. The absolute attitude error of the laser-vision tightly coupled positioning pose (without real-time RTK correction) was 12.13 cm, while the absolute attitude error of the laser-vision tightly coupled positioning pose (with real-time RTK correction) was reduced to 5.06 cm.

[0149] This patented method intelligently fuses differential RTK signals, IMU data, and laser-vision tightly coupled positioning pose data, reducing the average positioning accuracy to 1.98cm.

[0150] The above embodiments illustrate and describe the basic principles and main features of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the present invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed.

Claims

1. A method for ultra-high precision positioning of an orchard harvesting robot, characterized in that, Includes the following steps: Step 1: Based on the LiDAR scanning of the surrounding environment, obtain point cloud information, and correct the LiDAR point cloud distortion caused by motion based on IMU information, and align it with the global point cloud. Step 2: The binocular vision camera collects environmental information, extracts feature point information, and combines it with LiDAR and IMU information to perform feature point depth correlation and perform inter-frame motion estimation. Step 3: Collect satellite positioning data. Determine whether the data is available based on the number of satellites. If available, use RTK pose estimation data as the initial value. If unavailable, use laser-vision fusion pose estimation data and IMU information fused by piecewise fitting method to perform IMU pose estimation data at the current time. Step 4: Input the laser positioning information, visual positioning information, IMU pose estimation information and differential RTK pose estimation data into the tightly coupled data optimization model, output the optimized pose, smooth the optimized pose, and output the laser-visual tightly coupled positioning pose. Step 5: Determine the number of satellites found and output the initial pose of the robot based on the number of satellites; Step 6: Determine if the satellite signal has been lost, and optimize the robot pose based on the satellite signal loss time; Step 7: Reweight the differential RTK pose estimation data, IMU pose estimation data, and laser-vision tightly coupled localization pose, and output the final pose as the robot pose: Step 7-1: Assign subjective weights to the differential RTK pose estimation data, IMU pose estimation data, and laser-vision tightly coupled positioning pose to obtain the estimated pose. : ； ； in, It is a subjective weight; Step 7-2: Determine the combination of weight sets and weight vectors. Then, any combination of weight vectors is denoted as: ； ； In the formula, For linear combination coefficients, One possible combination of weights; Step 7-3: Construct the optimization model: ； Obtain the optimal linear combination coefficients gather ; Step 7-4: Adjust the weights After normalization, the final weight set is obtained. Determine the final output of the robot's optimized pose. Initial pose set : ； ； ； wherein, , , respectively represent differential RTK pose estimation data, IMU pose estimation data, and laser-vision tightly coupled localization pose, , j=1, 2, 3, respectively represent the weights of the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled localization pose when the number of searched satellites satisfies: M < number of satellites < N.

2. The ultra-high precision positioning method for orchard harvesting robots according to claim 1, characterized in that, The step 1, correcting the laser point cloud information based on IMU information, specifically involves: Step 1-1: Determine the intrinsic and extrinsic parameter transformation matrices for the IMU and lidar; Steps 1-2: Synchronize the lidar and IMU timestamps; Steps 1-3: Based on the lidar parameters, the point cloud data output by the lidar is processed into line bundles, and the line bundle to which each point belongs and the relative scanning time of each point in this frame of point cloud are recorded. Steps 1-4: Obtain the Euler angles of the three axes in the IMU coordinate system relative to the world coordinate system and the acceleration on the three axes. Remove the influence of gravity to obtain the acceleration of the IMU in the world coordinate system. Obtain the displacement and velocity corresponding to each frame of IMU data in the world coordinate system. Use the obtained motion information for interpolation to calculate the distortion compensation transformation matrix of each laser point relative to the laser point at the starting point scanning time. Steps 1-5: For each laser point, multiply the original radar laser point coordinates by the compensation transformation matrix to obtain the corrected laser point coordinates.

3. The ultra-high precision positioning method for orchard harvesting robots according to claim 1, characterized in that, The inter-frame motion estimation in step 2 specifically includes: Step 2-1: Fix the binocular vision camera, LiDAR, and IMU, perform joint calibration of the extrinsic parameters of the binocular vision camera-LiDAR-IMU, determine the extrinsic parameter transformation matrix of the camera, LiDAR, and IMU, including the translation matrix and rotation matrix, and perform intrinsic parameter calibration of the binocular vision camera to determine the intrinsic parameter calibration matrix. Step 2-2: Combining IMU data and the intrinsic and extrinsic parameter transformation matrix calibrated in Step 2-1, the acquired image is distorted, enhanced by filtering, and visual feature points are extracted based on RGB information. Steps 2-3: Collect point cloud depth information from multiple adjacent frames of LiDAR; Step 2-4. Project the visual feature points and laser point cloud points into the same coordinate system, search for n nearest depth point clouds in the vicinity of each visual feature point, determine the depth value of the visual feature point from the n laser point cloud points, and determine the 3D coordinates of the feature point; Step 2-5. Perform inter-frame feature point matching on the visual feature points with depth labels, construct an objective function through the nonlinear least squares method according to the matching result, perform iterative optimization, and obtain the pose estimation.

4. The ultra-high precision positioning method for orchard harvesting robots according to claim 1, characterized in that, The RTK pose estimation or IMU pose estimation at the current moment in Step 3 is specifically as follows: Step 3-1. Collect satellite positioning data. If the number of searched satellites is ≥ M, the satellite signal is considered available. The signal receiver fixed on the robot receives the satellite signal and the carrier phase differential correction signal sent by the reference station at the same time. On the basis of these two signals, differential calculation is implemented, and the differential RTK pose is output. Meanwhile, the differential RTK pose received at the previous moment is taken as the initial value, and the process proceeds to Step 3-2; If the number of searched satellites is < M, the satellite signal is considered unavailable, the data is ignored, the laser-vision fusion pose received at the previous moment is taken as the initial value, and the process proceeds to Step 3-3; Step 3-2. Perform coordinate conversion on the differential RTK pose, converting it from the world coordinate system to the robot coordinate system: ； in, The coordinates are in the robot's coordinate system. These are coordinates in the world coordinate system, specifically RTK coordinates provided by the Global Navigation Satellite System. This is a three-axis coordinate transformation matrix, corresponding to Pitch, roll, and heading angles provided for global satellite navigation systems; The RTK pose estimation at the current moment is obtained after conversion; Step 3-3. According to the IMU data, calculate the variation of the attitude angle between the current frame and the previous frame, add it to the pose at the previous moment, and obtain the IMU pose estimation at the current moment.

5. The ultra-high precision positioning method for orchard harvesting robots according to claim 1, characterized in that, Determining the output initial pose of the robot according to the number of searched satellites in Step 5 is specifically as follows: If the number of searched satellites is ≥ N, the differential RTK pose estimation data is directly output as the robot pose, and the current positioning information is recorded as the initial value for the next moment; If the number of searched satellites satisfies: M < the number of satellites < N, assign weights to the differential RTK pose estimation information, the IMU pose estimation information, and the laser-vision tightly coupled positioning pose according to the vector similarity, and fuse the three poses based on the weights to obtain the fused pose estimation result.

6. The ultra-high precision positioning method for orchard harvesting robots according to claim 5, characterized in that, Assigning weights to the differential RTK pose estimation data, the IMU pose estimation data, and the laser-vision tightly coupled positioning pose according to the vector similarity is specifically as follows: Determine the length similarity between pairwise vectors in differential RTK pose estimation data, IMU pose estimation data, and laser-vision tightly coupled localization pose estimation data, respectively. : ； Determine the directional similarity between pairwise vectors in differential RTK pose estimation data, IMU pose estimation data, and laser-vision tightly coupled localization pose estimation data, respectively. : ； in, This represents the angle between two vectors; The similarity between pairwise vectors in differential RTK pose estimation data, IMU pose estimation data, and laser-vision tightly coupled positioning pose is determined based on length similarity and orientation similarity, respectively. : ； The total similarity of any vector with other vectors is: ； ； Based on the total similarity in the above formula, the weighted values ​​of vector difference RTK pose estimation data, IMU pose estimation data, and laser-vision tightly coupled positioning pose are used to obtain the fused pose estimation result. : ； ； in, , , These represent differential RTK pose estimation data, IMU pose estimation data, and laser-vision tightly coupled positioning pose, respectively. , j=1,2,3, representing differential RTK pose estimation data, IMU pose estimation data, and laser-vision tightly coupled positioning pose weights, respectively.

7. The ultra-high precision positioning method for orchard harvesting robots according to claim 6, characterized in that, Performing pose optimization according to the satellite signal loss time in Step 6 is specifically as follows: Determine the satellite signal loss time: When the satellite signal is not lost, that is, the loss time is 0 second, the process proceeds to Step 7; When the satellite signal loss time is < L seconds, the differential RTK pose estimation data is excluded, that is, the weight of the differential RTK pose estimation data is set to 0, and the process returns to execute Step 5; When the satellite signal loss time is > L seconds, the differential RTK pose estimation data and the IMU pose estimation data are excluded, that is, the weights of the differential RTK pose estimation data and the IMU pose estimation data are set to 0, and the process returns to execute Step 5.

Images