Automatic pruning system for fruit trees based on machine vision

Abstract

The application provides an automatic pruning system for fruit trees based on machine vision, which is provided with a mechanical arm device, a positioning navigation device and an edge computing module on a tracked vehicle. In the running process, the system can obtain laser point cloud by scanning the orchard with a laser radar, construct an orchard panoramic point cloud map in cooperation with the orchard image collected by the camera, and segment the point cloud of each fruit tree to guide the tracked vehicle to run to the vicinity of the fruit tree, and then further identify the branches to be pruned on the tree trunk according to the local point cloud and close-range image obtained by scanning the working end of the mechanical arm with a close-range laser radar and a working end depth camera, and drive the executor to prune accordingly. The application integrates laser scanning technology, machine vision technology, autonomous navigation and obstacle avoidance technology and fruit tree branch pruning technology, and truly realizes the automation and intelligentization of fruit tree branch pruning, and provides strong systematic technical support for improving the efficiency of fruit tree cultivation by improving the tree shape.

Description

Technical Field

[0001] This application relates to the field of orchard operation equipment, and more specifically to an automatic fruit tree pruning system based on machine vision. Background Technology

[0002] Fruit tree pruning is an important horticultural practice, crucial for improving light distribution and gas exchange at the canopy and orchard scales, balancing vegetative and reproductive growth, and facilitating cultivation and harvesting practices. However, fruit tree pruning requires specialized agricultural technicians, making it one of the most expensive and labor-intensive tasks. For decades, researchers have focused on mechanizing fruit tree pruning. However, large-scale mechanized pruning systems struggle to guarantee pruning quality, potentially leading to decreased fruit quality and yield.

[0003] In recent years, numerous academic institutions and enterprises both domestically and internationally have been dedicated to the research and development of automated pruning technology for fruit trees. Mechanical pruning and robotic pruning are the two mainstream methods. Mechanical pruning, primarily used for hedgerow pruning, is a non-selective, large-scale pruning system widely applied in grapevine pruning. However, for fruit trees, the quality assurance capability of non-selective pruning systems is limited, potentially leading to excessive branch growth; therefore, it is mostly used for summer pruning. Robotic pruning, on the other hand, is a selective pruning operation that achieves the same quality and level of pruning as manual pruning through precise target recognition and mechanical control. However, due to the complexity of tree canopy structure and environment, high demands are placed on the robot's mobility, the control precision of the robotic arm, and its efficiency.

[0004] With the development of machine vision sensors such as stereo vision and LiDAR, as well as object detection technologies in image analysis, some progress has been made in robotic technology for pruning complex tree canopies (such as apple and cherry trees). However, current research largely faces the challenge of balancing pruning accuracy and efficiency. Therefore, the accuracy, robustness, and efficiency of robots in outdoor orchard environments still need further improvement. Summary of the Invention

[0005] This application addresses the shortcomings of existing technologies by providing a machine vision-based automatic fruit tree pruning system. This system comprehensively utilizes multi-sensor fusion positioning and coordinated 3D RGB point cloud map construction technology to accurately assess the device's posture and the structure of fruit tree branches, enabling precise pruning of branches according to user-defined pruning strategies. The specific technical solution adopted in this application is as follows.

[0006] First, to achieve the above objectives, a machine vision-based automatic fruit tree pruning system is proposed, comprising: a tracked vehicle with a rotating base, on which a robotic arm is mounted, the robotic arm having extension and retraction degrees of freedom, and an end effector, a working depth camera, and a near-field LiDAR on the top of the robotic arm; the tracked vehicle is also equipped with a navigation LiDAR, a navigation depth camera, and an inertial navigation unit, all communicatively connected to an edge computing module; the edge computing module performs the following steps: path planning based on a pre-received and entered orchard digital map; driving the tracked vehicle to run within the orchard according to the planned path based on inertial navigation data fed back by the inertial navigation unit, orchard images fed back by the navigation depth camera, and scanning information detected by the navigation LiDAR; during the operation of the tracked vehicle, triggering the navigation LiDAR to scan the orchard area and simultaneously triggering the navigation depth camera to acquire orchard images, then recording the images scanned by the navigation LiDAR at the same time... The system uses laser point clouds and orchard images acquired by navigation depth cameras to perform real-time localization and simultaneous mapping (SLAM) to output poses. Extended Kalman filtering is then used for data assimilation to update and generate a panoramic cloud map of the orchard. Based on this map, the system filters the tree point clouds within a given area, removing points taller than the tree trunks to identify the tree trunk positions. These trunk positions are then used to determine the robot's stopping point for pruning. The driven tracked vehicle moves to the corresponding stopping point near the tree to be pruned based on the individual tree point cloud positions. Using local point clouds obtained from near-field LiDAR scanning and near-field image segmentation from the working end depth camera, background noise is removed, and the corresponding point cloud clusters for the tree are extracted. The pruning area is then determined based on pre-received and recorded pruning rules. Finally, the rotating base and robotic arm move the end effector to the pruning area, triggering the end effector to perform pruning operations based on the distance between it and the branch to be pruned.

[0007] Optionally, the automatic fruit tree pruning system based on machine vision, as described above, includes the following specific steps for segmenting the local point cloud obtained by near-field LiDAR scanning and the near-field image acquired by the depth camera at the working end, removing background noise, and extracting the point cloud cluster corresponding to the fruit tree: First, by thresholding the depth image of the depth camera at the working end to obtain a foreground image with complex background removed, and masking it onto the near-field color image to obtain a foreground branch image; then, by segmenting the pixels of the main branches and lateral branches in the foreground branch image using the SegNet semantic segmentation network to obtain mask images of the main branches and lateral branches; next, by jointly calibrating the depth camera at the working end and the near-field LiDAR to obtain the camera-to-LiDAR extrinsic parameter matrix, enabling precise matching between the point cloud and the image, projecting the image coordinate system onto the point cloud coordinate system, thereby segmenting the three-dimensional point cloud of the branch based on the pixel position of the branch identified in the image, and extracting the point cloud cluster corresponding to the fruit tree.

[0008] In orchards, trees are planted in multiple rows, making it unavoidable to see multiple rows of fruit trees in the field of view simultaneously. Relying solely on color information is insufficient to remove trees in the back row. A depth camera can simultaneously acquire depth and color images; the depth image can then be used to effectively remove distant trees and other background elements.

[0009] Optionally, in the machine vision-based automatic orchard pruning system described above, the laser point cloud obtained from the navigation lidar scan at the same time and the pose output by the navigation depth camera through real-time localization and simultaneous mapping (SLAM) are assimilated using extended Kalman filtering to update and generate a panoramic orchard cloud map. Specifically, the process of obtaining the localization and 3D point cloud information includes the following steps: Step S1: Based on the timestamps marked during acquisition by the navigation depth camera and navigation lidar, orchard images and laser scan data with timestamp intervals less than 30 milliseconds are registered; Step S2: The navigation depth camera and navigation lidar are jointly calibrated using livox_camera_lidar_calibration. First, the intrinsic parameter matrix K of the camera is obtained. Then, images from both the navigation lidar and the camera are simultaneously captured using a calibration board for calibration, obtaining the extrinsic parameter matrix T from the camera to the lidar. LIDAR_CAMERA According to [x LIDAR ,y LIDAR ,z LIDAR ,1]=T LIDAR_CAMERA -1 ·(Z·K -1 The image coordinate system is projected onto the point cloud coordinate system using the formula [u,v,1], where x... LIDAR The position of a point in the lidar coordinate system in the horizontal direction is represented by y. LIDAR The z-axis represents the position of a point in the left-right direction in the lidar coordinate system. LIDAR Z represents the vertical position of a point in the LiDAR coordinate system, Z represents the depth corresponding to the pixel coordinate, and K represents the vertical position of the point in the LiDAR coordinate system. -1[u,v,1] represents the inverse of the camera intrinsic parameter matrix, and [u,v,1] represents the position of the point in the pixel coordinate system; Step S3: After point cloud preprocessing and feature point extraction, the Loam_livox algorithm is used for real-time localization and synchronous mapping based on the Livox LiDAR, and the VIO-SLAM algorithm is used for real-time localization and synchronous mapping based on the ZED2 depth camera, and their corresponding pose information is output respectively; Step S4: The pose output by the LiDAR SLAM and depth camera SLAM is assimilated using extended Kalman filtering, and the pose error caused by the loss of feature points in the LiDAR SLAM is calibrated by taking advantage of the camera's field of view. After initializing the robot's initial position and map, the extended Kalman filter algorithm predicts the next state using the state model. Then, it updates the robot's position, orientation, and map variables using the sensor model, observation model, and currently detected data. By repeatedly executing the above steps, the robot's position and attitude at any given time are calculated. Step S5: Update the orchard digital map at the corresponding position according to the real-time pose, and generate a panoramic 3D point cloud of the orchard in real time. Step S6: Based on the improved Loam_livox algorithm, the laser point cloud is colored using the mapping relationship between the laser point cloud and the camera point cloud to generate a panoramic 3D colored point cloud of the orchard.

[0010] By using extended Kalman filtering, feature loss and positioning errors caused by changes in viewpoint and target movement can be reduced between different frames of data in motion.

[0011] Optionally, in the machine vision-based automatic fruit tree pruning system described above, the specific steps for dividing and determining the pruning area and adjusting the lateral angle and longitudinal position of the robotic arm according to the pre-received and recorded pruning area density pruning rules are as follows: Step T1: Divide the fruit tree point cloud cluster into different units according to height and orientation, corresponding to different pruning areas, numbered S1, S2 to Sn; Step T2: Adjust the lateral angle of the robotic arm by rotating the base and adjust the longitudinal position of the robotic arm by retracting the rod, and sequentially adjust each area to be pruned to be within the field of view of the depth camera and near-field LiDAR on the end effector.

[0012] Optionally, the machine vision-based automatic fruit tree pruning system described above, specifically, extracts the point cloud clusters corresponding to branches based on the point cloud obtained by near-field LiDAR scanning and the near-field image acquired by the depth camera at the work end, as follows: Step R1: By thresholding the depth image of the depth camera at the work end, a foreground image with complex background removed is obtained, which is then masked onto the near-field color image to obtain the foreground branch image; Step R2: The main branches and lateral branches are segmented using the SegNet semantic segmentation network, and after denoising, masked images of the main branches and lateral branches are obtained; Step R3: Perform precise matching of point cloud and image using the depth camera and near-field LiDAR at the working end, as in step S2, and project the image coordinate system onto the point cloud coordinate system; Step R4: Based on the branch pixel positions identified in the image, segment the 3D point cloud of the main branch and lateral branches; Step R5: Use the K-Means algorithm to cluster the point cloud of a single fruit tree obtained by the near-field LiDAR scanning to obtain multiple point cloud clusters corresponding to different fruit trees; Step R6: Further segment and merge the point cloud cluster of each tree by using cylindrical fitting and calculating the cluster center distance to obtain the point cloud of a single branch of the main branch and lateral branches.

[0013] Optionally, in any of the machine vision-based automatic fruit tree pruning systems described above, the specific steps of driving the end effector to move to the corresponding pruning area, identifying the branches to be pruned according to pre-received and recorded pruning rules, and triggering the end effector to perform pruning operations until completion based on the positional distance between the end effector and the branch to be pruned include: Step P1: Measuring the branch inclination angle and branch length of a single lateral branch within the pruning area, identifying and filtering the branches to be pruned and their lower ends based on the branch inclination angle and branch length range required in the user-specified pruning rules; Step P2: Sort the pruning points within the field of view by distance, and sequentially perform pruning operations on the branches to be pruned closest to the end effector; During the pruning operation, based on the depth... The depth information obtained from the camera image is used to calculate the positional distance and orientation between the end effector and the pruning location in real time. The end effector is adjusted to be in a straight line with the pruning point by extending and retracting the telescopic rod. The end effector is controlled to approach the pruning point by the telescopic arm, and the movement trajectory of the robotic arm is recorded. After the depth camera at the working end detects that the pruning location of the branch to be pruned has reached the executable range between the blades of the end effector, the end effector is triggered to drive the blades to close and perform the pruning operation. Step P3: According to the robotic arm movement trajectory recorded during the pruning operation, the robotic arm is driven to return to the original position. The next pruning point in the pruning area is detected according to step P1, and steps P2 and P3 are executed until there are no more pruning points in the area. The tracked chassis and rotating base are moved to the next pruning area.

[0014] After the fruit tree has reached its final shape, dormant pruning mainly focuses on the pruning of one-year-old branches. Due to the growth characteristics of fruit trees, the growth angle of one-year-old branches follows a specific pattern. By measuring the branch angle, one-year-old branches can be quickly identified, and by measuring the branch length, it can be quickly determined whether pruning is necessary.

[0015] Optionally, in the machine vision-based automatic fruit tree pruning system described above, the robotic arm device includes: a support pole, a telescopic arm, and a retractable rod. The bottom of the support pole is connected to the rotating base, which is used to achieve horizontal rotation. The upper part of the support pole is movably connected to the telescopic arm. The retractable rod is disposed between the support pole and the telescopic arm. An end effector is provided at the end of the telescopic arm. The retractable rod extends and retracts, causing the telescopic arm to perform a longitudinal arc movement with the length of the telescopic arm as the radius. The telescopic arm extends and retracts, causing the end effector to move back and forth.

[0016] Optionally, in the machine vision-based automatic fruit tree pruning system described above, the end effector includes a stepper motor, a transmission mechanism, and pruning shears. The output shaft of the stepper motor is perpendicular to the opening and closing direction of the pruning shears. The end of the output shaft has a frustum structure that maintains surface contact with the transmission mechanism. The frustum structure drives the transmission mechanism in a circular motion. The transmission mechanism also has a gear. One side of the pruning shears has a rack structure that meshes with the gear. The transmission mechanism has an annular structure on its outer periphery, with the thickness of the annular structure gradually decreasing from the inner end to the outer end. The side of the frustum structure maintains surface contact with the annular structure. The stepper motor drives the frustum structure to rotate the annular structure. During rotation, the annular structure synchronously drives the gear to rotate. The gear meshes with the rack structure, causing the moving blade to rotate relative to the fixed blade and open and close.

[0017] Optionally, in any of the above-described machine vision-based automatic fruit tree pruning systems, step S3, which uses the SLAM algorithm to perform pose estimation and panoramic mapping on the data scanned by the LiDAR, specifically includes: first, removing edge points, points with excessively high or low intensity, points with incident angles close to π or 0, and occluded points from the laser point cloud collected by the navigation LiDAR; then, performing feature extraction, classifying the points into two main categories based on the local smoothness and reflectivity of the candidate points: edge points and planar points, where planar points refer to points belonging to a plane; and finally, using an iterative pose optimization program to calculate the pose of the navigation LiDAR, during which... The pose is estimated using the residuals between points: For edge points and planar points, the pose is estimated by first finding the 5 nearest points in the point cloud set, calculating the mean and covariance matrices of these 5 points to ensure that edge points are in a straight line and planar points are in the same plane, and then calculating the distance residuals; after obtaining the residuals of edge points and planar points, the pose is solved iteratively by constructing optimization equations; in this iteration process, two iterations are performed first to remove dynamic objects, and the pose is updated using the optimization results of the previous iteration, until 20% of the maximum residual is removed, and then full pose optimization is performed to obtain the six-degree-of-freedom pose estimate of the navigation lidar and output the orchard panoramic cloud map.

[0018] Due to the limitations of solid-state lidar in terms of laser spot size, laser signal-to-noise ratio, and scanning mechanical characteristics, the original point cloud contains dead pixels and empty pixels. Denoising is used to select high-quality points from the original point cloud as valid points. This makes the system's state estimation more robust when using the point cloud later. By identifying edge points and planar points, reliable feature points can be obtained. These feature points are then matched using different feature points acquired by the lidar in adjacent frames, and the lidar pose is updated under moving conditions through distance iteration optimization.

[0019] Optionally, in any of the above-described machine vision-based automatic fruit tree pruning systems, the real-time pose in step S3 is determined based on the estimated rotation of the robotic arm as determined in the following steps: Let ω t The angular velocity provided by the gyroscope is affected by noise; let n be the white noise included in the gyroscope measurement. ω The bias of the gyroscope is b ωt White noise n ω Modeled as Gaussian white noise, bias b ωt Modeled as a random walk process, the angular velocity formula including error is obtained as follows: Let t be the time length between the k-th frame and the (k+1)-th frame of two consecutive near-field LiDAR frames. k and t k+1 Then the pre-integration of the gyroscope By performing a coordinate transformation on the pre-integration result, the rotation estimate in the world coordinate system is obtained.

[0020] Optionally, the automatic fruit tree pruning system based on machine vision as described above is characterized in that step S6 improves the Loam_livox algorithm and adds a point cloud map coloring function. Q1: Perform joint calibration of the LiDAR and camera, referring to step S2 above. Q2: Perform denoising and feature extraction, iterative pose optimization, feature matching, and odometry output on the point cloud, referring to step S3 above. Q3: Timestamp alignment of the LiDAR point cloud and camera image point cloud. Record the timestamps of each frame of LiDAR point cloud and image respectively. Because the frame rate of the image is higher than that of the point cloud, when registering the image corresponding to each frame of point cloud, images within a 15ms range before and after the timestamp of each frame of point cloud will be selected. If there are multiple images, the image with the closest time distance will be selected. Q4: Project the LiDAR point cloud to the image pixels. Using the external parameters of the LiDAR and the left eye of the binocular camera, transform each point of the current frame of point cloud from the LiDAR coordinate system to the left eye coordinate system of the camera. Subsequently, the point cloud is projected from the camera coordinate system to the pixel coordinate system using the camera's intrinsic parameters. The RGB information contained in the corresponding pixels is added to the point's information to obtain points with RGB color information. Q5: By setting the point cloud color display to the corresponding RGB attribute values ​​of the points, a 3D color point cloud map can be displayed. The point cloud map is continuously updated through map reconstruction to obtain the latest colored point cloud map. The original laser point cloud map and the 3D color point cloud map are registered using planar feature points and edge feature points to compare the point cloud coloring effects.

[0021] Coloring 3D point clouds adds spectral information in addition to location and shape, which helps in evaluating the quality of point cloud reconstruction and subsequently using color information to detect tree trunks and distinguish between branches and leaves.

[0022] Beneficial effects

[0023] The machine vision-based automatic fruit tree pruning system provided in this application features a robotic arm, a positioning and navigation device, and an edge computing module mounted on a tracked vehicle. During the vehicle's operation, it uses LiDAR to scan the orchard, obtaining laser point clouds, and then collaborates with images captured by cameras to construct a panoramic cloud map of the orchard. This system can segment the point clouds of each fruit tree based on the panoramic cloud map to guide the tracked vehicle to the vicinity of the trees. Furthermore, it identifies the branches requiring pruning based on the local point clouds and close-up images obtained from scanning by the near-field LiDAR and depth camera on the robotic arm's working end, and accordingly drives the actuators to perform the pruning. By integrating laser scanning technology, machine vision technology, autonomous navigation and obstacle avoidance technology, and fruit tree pruning technology, this application truly achieves automated and intelligent fruit tree pruning, providing strong systematic technical support for improving fruit tree cultivation efficiency through tree shape modification.

[0024] This application utilizes 3D laser scanning technology for fruit trees to predict pruning areas; and employs depth camera and near-field LiDAR imaging technology to achieve precise prediction of pruning points. The fruit tree pruning robot provided in this application integrates a tracked vehicle, a robotic arm, a positioning and navigation device, a machine vision device, and a control device to perform fruit tree pruning. The positioning and navigation device and machine vision device can collect equipment operation data for user-defined path planning and pruning rule formulation. The positioning and navigation system uses a GNSS antenna, inertial navigation unit, LiDAR, and depth camera for path navigation, orchard map construction, and real-time obstacle avoidance. The pruning rule-execution system can identify the robot's stopping point and determine the pruning points on the fruit tree branches based on the local point cloud and near-field images obtained from scanning by the near-field LiDAR and depth camera on the robotic arm's working end, thus achieving real-time tracking and visualization of pruning. This invention creatively integrates a robot autonomous positioning and navigation system with a fruit tree branch pruning system, achieving automated and intelligent fruit tree branch pruning. Utilizing a tracked mobile platform, it can adapt to complex orchard terrain. Through a GNSS positioning and navigation system and lidar, it enables real-time obstacle avoidance, guiding the robot along a designated path. A 3D scanning system located at the front of the tracked vehicle acquires an initial scan of the orchard and fruit trees during the journey. The robot then returns to the charging point along the designated path. This invention employs a depth camera for target detection, analyzing the diameter and direction of branches in real time to identify pruning points, providing feedback to the robotic arm, and finally executing the pruning. This invention reduces labor costs, improves pruning accuracy and efficiency, and minimizes damage to fruit trees during the pruning process.

[0025] Other features and advantages of this application will be set forth in the following description, and will be apparent in part from the description, or may be learned by practicing this application. Attached Figure Description

[0026] The accompanying drawings are provided to further illustrate the present application and form part of the specification. Together with the embodiments of the present application, they serve to explain the present application but do not constitute a limitation thereof. In the drawings:

[0027] Figure 1 This is a side view of the tracked vehicle used in this application;

[0028] Figure 2 This is a schematic diagram of the overall structure of the tracked vehicle used in this application;

[0029] Figure 3 This is a schematic diagram of the positioning and map construction method used in the system of this application;

[0030] Figure 4 This is a schematic diagram of the system framework of this application;

[0031] Figure 5 This is a schematic diagram of the branch and trunk structure of a fruit tree;

[0032] Figure 6 A schematic diagram of a tree branch image segmentation mask;

[0033] Figure 7 A structural diagram of the branch for which pruning is required in this application, and a diagram showing the measurement of the length and angle of the pruned branch.

[0034] Figure 8 This is a system overview diagram of the Loam_livox technology used in this application;

[0035] Figure 9 This is a schematic diagram illustrating the pose estimation of the camera used in this application.

[0036] Figure 10 This is a schematic diagram illustrating the structural principle of the end effector of this application;

[0037] Figure 11 A system platform for real-time positioning and mapping;

[0038] Figure 12 The absolute pose error and relative pose error of the system for turning trajectories in this application are given.

[0039] Figure 13 The overall framework of a multi-sensor fusion system;

[0040] Figure 14 A framework for camera pose estimation;

[0041] Figure 15 Projecting laser point clouds onto image pixels;

[0042] Figure 16To illustrate the different experimental scenarios for applying this application, (a) is a pond; (b) is a road; (c) is a densely planted orchard; (d) is a partial view of a densely planted orchard; (e) is a sparsely planted orchard; and (f) is a partial view of a sparsely planted orchard.

[0043] Figure 17 The orchard point cloud map is provided, where (a) is a densely planted pear orchard, (d) is a sparsely planted pear orchard, (g) is a pond in the orchard, (j) is a road in the orchard, (b,e,h,k) is a magnified view of the point cloud, and (c,f,i,l) is the camera view corresponding to the magnified point cloud map.

[0044] Figure 18 The scenario is a "mouth" shaped operation within a sparse pear orchard. (a) compares the absolute errors of our method and Loam_livox, (b) compares the APE curves over time, (c) compares the box plots of RES, and (d) compares the histograms of RES.

[0045] Figure 19 The diagram shows an "N"-shaped operating scenario within a sparse pear orchard. (a) compares the absolute errors of our method and Loam_livox; (b) compares the APE curves over time; (c) compares the box plots of RES; and (d) compares the bar plots of RES.

[0046] Figure 20 For the sparsely planted pear orchard scenario: (a) comparison of the absolute error of our method and Loam_livox, (b) comparison of APE change curves over time, (c) comparison of RES box plots, (d) comparison of RES bar plots;

[0047] Figure 21 In the scenario of densely planted pear orchards: (a) comparison of absolute error between our method and Loam_livox, (b) comparison of APE change curves over time, (c) comparison of RES box plots, (d) comparison of RES bar plots;

[0048] Figure 22 For the orchard road scenario: (a) comparison of the absolute error of our method and Loam_livox, (b) comparison of APE versus time curves, (c) comparison of RES box plots, (d) comparison of RES bar plots.

[0049] Figure 23 For the orchard pond scenario: (a) comparison of the absolute error of our method and Loam_livox, (b) comparison of APE change curves over time, (c) comparison of RES box plots, (d) comparison of RES bar plots;

[0050] Figure 24This is an example of classifying points into edge points and planar points based on the local smoothness and reflectivity of candidate points during the feature extraction process. Detailed Implementation

[0051] To make the objectives and technical solutions of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this application. All other embodiments obtained by those skilled in the art based on the described embodiments of this application without creative effort are within the scope of protection of this application.

[0052] Those skilled in the art will understand that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the meaning consistent with their meaning in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless defined as herein.

[0053] The meaning of "and / or" as used in this application includes situations where each exists alone or both exist simultaneously.

[0054] In this application, "inner" and "outer" refer to the direction from the outer shell of the tracked vehicle towards the battery module inside the housing, and vice versa, relative to the tracked vehicle itself; rather than a specific limitation on the device mechanism of this application.

[0055] The terms "left" and "right" as used in this application refer to the user's left side as the left and the user's right side as the right when the user is facing the direction of travel of the tracked vehicle, and do not constitute a specific limitation on the device mechanism of this application.

[0056] The term "connection" as used in this application can mean a direct connection between components or an indirect connection between components through other components.

[0057] The terms "up" and "down" as used in this application refer to the direction from the ground to the top of the telescopic boom when the user is facing the direction of travel of the tracked vehicle, which is up and vice versa, and are not a specific limitation on the device mechanism of this application.

[0058] For decades, the precise localization, mapping, and navigation of agricultural robots in orchard environments has been a hot research topic. With the development and in-depth research of SLAM technology, LiDAR SLAM research results are increasing, and application scenarios are becoming increasingly complex. While LiDAR's scanning range and data acquisition accuracy are suitable for building large-scale scene maps, the agricultural orchard environment is completely different from a school environment. Orchards lack high walls, only have relatively short fruit trees, and the branches and leaves of these trees have irregular angles and gaps. These factors reduce the accuracy of the data acquired by the LiDAR sensor. Therefore, the limitations of a single LiDAR sensor make it difficult to adapt to the complex scenarios in agricultural orchards.

[0059] To address the aforementioned issues, an efficient adaptive sensor fusion odometry framework based on simultaneous localization and mapping can solve the localization problem for some agricultural unmanned ground vehicles without GPS assistance. However, this framework significantly reduces the stable geometric features of LiDAR SLAM due to the unstructured and complex characteristics of orchard scenes. The unstructured characteristics of orchard scenes include irregular and unstable environmental information such as the size of fruit trees, the number of branches, the swaying of leaves, and the high degree of overlap between leaves and branches, leading to uncertainty and indescribability in LiDAR data. When processing, extracting features, and matching LiDAR point clouds, problems arise with insufficient, incomplete, and difficult-to-match geometric features.

[0060] The dynamic nature of the environment and the unevenness of the ground cause lidar to increase outlier values ​​in point cloud features during point cloud acquisition, reducing data validity and the amount of usable point cloud data. During point cloud processing, more defective points are removed, leading to a reduction in the number of point clouds and the extraction of geometric features. This, in turn, reduces the effective data available for pose estimation, lowers the accuracy and precision of the estimation, increases pose error, and ultimately reduces the robustness of the system.

[0061] By applying the multi-sensor fusion method proposed in this application, technicians can increase data diversity and improve pose estimation accuracy by utilizing data acquired from IMU and depth cameras. Furthermore, the loosely coupled method of this application further enhances the system's robustness, enabling it to better adapt to complex and harsh environments. Experimental results show that the proposed multi-sensor fusion method increases data diversity, allowing for the effective use of the loosely coupled fusion framework to estimate highly accurate results even when information acquired by LiDAR is insufficient, thereby increasing the system's stability and robustness.

[0062] Furthermore, this application addresses the issue that point clouds acquired by LiDAR lack color information, and environmental maps built using Loam_livox also lack color features. This application improves the algorithm by utilizing information from multiple sensors. By fusing LiDAR point clouds and depth camera images, the RGB color information from the depth camera images is extracted and applied to each corresponding point cloud point, resulting in an environmental map with rich color features. Experimental results demonstrate the effectiveness of our improvement. This increases the diversity of map information, provides richer point cloud maps for orchard management, and offers reliable data for intelligent management.

[0063] Specifically, such as Figure 4 As shown, the automatic fruit tree pruning system proposed in this invention utilizes machine vision-based positioning and recognition technology. Through a fruit tree pruning robot hardware device, a fruit tree pruning control system, and a tracked chassis, it achieves free movement and precise operation in complex orchard environments. This application uses GNSS, inertial navigation units, and lidar for simultaneous positioning, navigation, and mapping; a machine vision device for branch recognition; and combines pruning rules to execute pruning.

[0064] In the specific operation process, this application is approved Figure 1 , 2 The machine vision-based automated fruit tree pruning system shown realizes the pruning operation of fruit trees. The hardware device of this fruit tree pruning robot includes:

[0065] The tracked mobile platform is used to carry robotic arms, positioning and navigation devices, and other related system devices, and can also meet the operation requirements of the fruit tree pruning system on complex orchard surfaces; the robotic arm is installed on the tracked mobile platform for pruning fruit tree branches; the positioning and navigation device is installed on the tracked mobile platform for the system's path planning and real-time obstacle avoidance.

[0066] The tracked mobile platform consists of a drive wheel 2 at the front, a guide wheel 12 at the rear, a load-bearing wheel 1 in the middle, a tension wheel 10, an annular track 13 surrounding the aforementioned wheels, a chassis drive motor 11, a battery module 3, and a housing 21. The battery module 3 is connected to the chassis drive motor 11 and provides power to the chassis drive motor 11. Both are located inside the housing 21. The two sides of the housing 21 are provided with drive wheels 2, guide wheels 12, load-bearing wheels 1, tension wheels 10, and annular tracks 13. The drive wheels 2, guide wheels 12, load-bearing wheels 1, tension wheels 10, and annular tracks 13 form the walking system of the tracked mobile platform. The chassis drive motor 11 drives the drive wheels 2 to rotate. The drive wheels 2 drive the rear guide wheels 12 to rotate through the annular tracks 13. The load-bearing wheels 1 are located between the drive wheels 2 and the guide wheels 12 and rotate under the drive of the annular tracks 13. The load-bearing wheels 1 are located below and behind the drive wheels 2 and are in contact with the ground along with the guide wheels 12. The tension wheels 10 are located above the load-bearing wheels 1.

[0067] The robotic arm device includes a rotating base 19, which is mounted on the housing 21 of the tracked mobile platform and located at the rear of the housing 21. A support column 8 is connected to the rotating base 19, allowing the robotic arm to rotate omnidirectionally. The front end of the support column 8 is connected to a telescopic arm 7, and a retractable rod 6 connects the support column 8 and the telescopic arm 7. An end effector 17 is located at the end of the telescopic arm 7. The retractable rod 6 extends and retracts, causing the telescopic arm 7 to perform a longitudinal arc motion with its length as the radius. The telescopic arm 7 controls the forward and backward movement of the end effector 17 to adjust the trimming position and distance.

[0068] End effector 17 includes Figure 10 The stepper motor 26, transmission mechanism, and trimmer scissors shown are all housed within the inner cavity of the trimmer scissor housing 23. The stepper motor 26 drives the gear transmission mechanism 24 to rotate, which in turn drives the trimmer scissors to open and close. The output shaft of the stepper motor 26 is perpendicular to the direction of the trimmer scissors' opening and closing motion.

[0069] The trimmer includes a movable blade 22 and a fixed blade 25. The fixed blade 25 is fixedly installed inside the trimmer housing 23. The movable blade 22 is installed above the fixed blade 25 and is movably connected to it via a column. The length of the rack structure 22-1 is designed to allow the movable blade 22 to rotate to contact the trimmer housing 23. At this time, the angle formed by the movable blade 22 and the fixed blade 25 is the maximum opening and closing angle of the trimmer.

[0070] The output shaft end of the stepper motor 26 is provided with a first transmission structure that maintains surface contact with the gear transmission mechanism 24. The first transmission structure is a frustum structure 27, which drives the gear transmission mechanism 24 to perform circular motion. The gear transmission mechanism 24 is also provided with a transmission component, which is a gear 24-1, fixedly positioned in the middle of the gear transmission mechanism 24. The trimmer is provided with a mating component, which is a rack structure 22-1. The rack structure 22-1 is located at the tail of the moving blade 22. The gear 24-1 meshes with the rack structure 22-1, that is, the gear 24-1 drives the rack structure 22-1 to move, thereby causing the moving blade 22 to open and close relative to the fixed blade 25.

[0071] The outer circumference of the gear transmission mechanism 24 is a circular ring structure 24-2. The thickness of the circular ring structure 24-2 gradually decreases from the inner end to the outer end. The circular ring structure 24-2 maintains surface contact with the side of the frustum structure 27. The thinner end of the frustum structure 27 contacts the inner end of the circular ring structure 24-2, and the thicker end of the frustum structure 27 contacts the outer end of the circular ring structure 24-2. The stepper motor 26 drives the frustum structure 27 to rotate, which in turn drives the circular ring structure 24-2 to rotate. The circular ring structure 24-2 then drives the gear 24-1 to rotate, and the gear 24-1 drives the trimmer shears to move.

[0072] When positioning and controlling the movement route of the above-mentioned fruit tree pruning robot, it can rely on a navigation device composed of an inertial navigation unit 20 for positioning and navigation, a GNSS antenna 5 for receiving positioning signals, a navigation lidar 4 for constructing a panoramic map of the orchard, and a depth camera 14 for path planning and point cloud coloring.

[0073] In this system, the GNSS antenna, inertial navigation unit, depth camera, and LiDAR are all directly connected to the edge computing module. Based on a user-specified path diagram, the inertial navigation unit 20 guides the robot to complete an initial scan, and the navigation LiDAR 4 and depth camera 14 construct a panoramic SLAM map of the orchard. After identifying the pruning area, the navigation robot moves to the vicinity of the pruning point. A PID algorithm controls the robotic arm to a suitable pruning position close to the target branch and within reach of the pruning shears, ensuring the shears are close to and perpendicular to the branch to perform the pruning operation.

[0074] During the pruning process, the near-field LiDAR of the robotic arm terminal can be set to be connected to the telescopic arm and communicate with the edge computing module to measure the distance between the end effector and the tree branch;

[0075] A depth camera with a polarizing lens can also be mounted on the end of the telescopic arm and communicate with the edge computing module for the identification of pruning points.

[0076] The actuator 17 at the end of the robotic arm can be implemented by a stepper motor and pruning shears. The stepper motor is connected to an edge computing module to drive the pruning shears to open or close to prune the branches.

[0077] The depth camera used in machine vision can be equipped with a polarizing lens. The working depth camera 18, mounted on the robotic arm, enables further precise identification and positioning of pruning points. When the working depth camera 18 identifies a pruning point and the near-field LiDAR 16 detects branches between the pruning shears, it transmits a signal to the edge computing module. The edge computing module then issues a command, causing a stepper motor to move the pruning shears. These steps are repeated until the entire pruning task is completed.

[0078] The system can be controlled via a PC (9), a wireless communication module, and edge computing modules such as Jetson Orin (15). The pruning robot can then utilize data collected from various sensors to achieve its functions.

[0079] a. Data acquisition, used for user-indicated path planning, pruning rule formulation, and machine vision data acquisition;

[0080] b-positioning and navigation, used for path navigation, orchard map building, and real-time obstacle avoidance;

[0081] c-pruning planning and execution is used for determining robot docking and pruning points, controlling the robotic arm and pruning points, and providing real-time tracking and visualization of pruning.

[0082] The system described above can achieve communication between the PC and the robot controller via a 4G network. The integration of each subsystem can be achieved through the interface defined by the ROS system. The linkage between the subsystems can realize a complete positioning and pruning operation process, that is, accurately control the robot to navigate around obstacles from a location in the orchard to the vicinity of the pruning point of the target fruit tree, and then perform pruning.

[0083] In the specific implementation process, this application can be aided by Figure 3 , 4 The system shown implements automatic fruit tree pruning based on machine vision, automatically pruning the branches and trunks of each fruit tree in the orchard through the following steps:

[0084] Step 1: Users enter a two-dimensional digital map of the orchard based on remote sensing imagery into the basic data to plan the path and enter pruning rules.

[0085] Step 2: Real-time obstacle avoidance is activated via the inertial navigation unit 20, depth camera 14, and navigation lidar 4, and the robot is instructed to follow the indicated path.

[0086] Step 3: Utilizing the navigation LiDAR 4 mounted on the tracked vehicle, an initial scan of the orchard and fruit trees is obtained during the journey. This triggers the depth camera 14 to acquire orchard images. The acquired data is uploaded to edge computing. Then, the laser point cloud obtained by the LiDAR 4 scan and the orchard image acquired by the navigation depth camera 14 are used for real-time localization and simultaneous mapping (SLAM) to output the pose. Extended Kalman filtering is used for data assimilation. On the PC 9, SLAM-based 3D point cloud strips of the orchard are generated, resulting in a panoramic map of the orchard. This yields more stable and accurate positioning and 3D point cloud information. Point cloud density detection is used to segment the point cloud of individual fruit trees. Specifically, this step can be implemented as follows:

[0087] Step 3-1: Based on the timestamp, register the laser point cloud collected by the navigation lidar 4 and the orchard image collected by the depth camera 14. Since the laser point cloud and the depth camera image are collected synchronously and the relative positions of the two sensors are fixed, the robot continuously collects data during its movement. It can be set that the images and laser scanning point clouds with an interval of 30 milliseconds between the collection timestamps belong to the same location, so as to register the two data and combine the two data to obtain a cloud map of the entire orchard through subsequent analysis and processing steps.

[0088] Step 3-2: Jointly calibrate the navigation depth camera 14 and the navigation LiDAR 4 using livox_camera_lidar_calibration. First, obtain the camera's intrinsic parameter matrix K. Then, use a calibration board to simultaneously capture images of both the LiDAR and the camera for calibration, obtaining the camera-to-LiDAR extrinsic parameter matrix T. LIDAR_CAMERA According to [x LIDAR ,y LIDAR ,y LIDAR ,1]=T LIDAR_CAMERA -1 ·(Z·K -1 Projecting the image coordinate system onto the point cloud coordinate system using [u,v,1]);

[0089] Step 3-3: Real-time localization and synchronous mapping based on the Livox LiDAR are performed using the Loam_livox algorithm, and real-time localization and synchronous mapping based on the ZED2 depth camera are performed using the VIO-SLAM algorithm, with the corresponding pose information output respectively. Specifically, after point cloud preprocessing and feature point extraction, real-time localization and synchronous mapping based on the Livox LiDAR are performed using the Loam_livox algorithm, and real-time localization and synchronous mapping based on the ZED2 depth camera are performed using the VIO-SLAM algorithm, with the corresponding pose information output respectively.

[0090] Steps 3-4: The Extended Kalman Filter (EKF) is used to assimilate the pose outputs of the LiDAR SLAM and Depth Camera SLAM, leveraging the camera's field of view to correct pose errors caused by missing feature points in the LiDAR SLAM. After initializing the robot's initial position and map, the EKF algorithm predicts the next state using a state model. Then, it updates the robot's position, orientation, and map variables using sensor and observation models, along with currently detected data. By repeatedly executing these steps, the robot's position and pose at any given time can be calculated. For example, the EKF can be used to assimilate the pose outputs of the LiDAR SLAM and Depth Camera SLAM, leveraging the camera's field of view to correct pose errors caused by missing feature points in the LiDAR SLAM. After initializing the robot's initial position and map, the EKF algorithm predicts the next state using a state model. Then, it updates the robot's position, orientation, and map variables using sensor and observation models, along with currently detected data. By repeatedly executing these steps, the robot's position and pose at any given time can be calculated.

[0091] Steps 3-5: Continuously update the map based on the pose and generate a panoramic 3D point cloud of the orchard in real time;

[0092] Steps 3-6: Based on the improved Loam_livox algorithm, the laser point cloud is colored using the mapping relationship between the laser point cloud and the camera point cloud to generate a panoramic 3D color point cloud of the orchard.

[0093] Step 4: Based on the pre-received and entered pruning rules (pruning area density), divide the pruning area and adjust the lateral angle and longitudinal position of the robotic arm. Step 4 can be implemented in the following ways:

[0094] Step 4-1: Divide the fruit tree point cloud cluster into different units according to height and orientation, corresponding to different pruning areas, and number them S1, S2 to Sn;

[0095] Step 4-2: Adjust the lateral angle of the robotic arm by rotating the base 19 and adjust the longitudinal position of the robotic arm by retracting the rod 6, so as to ensure that the area to be trimmed is within the field of view of the working end depth camera 18 and the close-range lidar 16 on the end effector.

[0096] Step 5: The pruning area is identified using the depth camera 18 and near-field LiDAR 16 mounted on the robotic arm. Based on the local point cloud obtained by the near-field LiDAR 16 and the near-field image segmentation acquired by the depth camera 18, background noise is removed, and the point cloud clusters corresponding to the fruit trees are extracted. First, threshold segmentation is performed on the depth image from the depth camera 18 to obtain a foreground image with complex background removed. This image is then masked onto the near-field color image to obtain the foreground branch image. Then, the main branches and lateral branches are segmented using the SegNet semantic segmentation network to obtain masked images of the main branches and lateral branches. The depth camera 18 and the near-field LiDAR 16 are jointly calibrated to obtain the camera-to-LiDAR extrinsic parameter matrix, enabling accurate matching between the point cloud and the image. The image coordinate system is projected onto the point cloud coordinate system. Based on the branch pixel positions identified in the image, the 3D point cloud of the branches is segmented. Then, the pruning area is determined according to the pre-received and recorded pruning rules. Figure 7 Example a identifies the lateral branches of the fruit tree, according to Figure 7 b. Measure the length and angle of the branches. The specific steps for calculating the length of the branches and the angle are as follows.

[0097] Step 5-1: Downsampling is performed on the branch point cloud to reduce the computational burden of subsequent operations. The downsampled point cloud is first subjected to skeletonization based on the Laplacian compression algorithm, as shown in formula (1), where P is the original point cloud, P' is the shrunk point cloud, and W... L and W H It is a diagonal weight matrix that balances contraction and attraction. During contraction, the point cloud becomes increasingly sparse until the solution to the equation converges.

[0098] Step 5-2: Use the farthest point sampling method to sample the shrunken point cloud (i.e., skeleton).

[0099] Step 5-3: In the obtained skeletonized point cloud, select the lowest point (x) i ,y i ,z i Starting from point (Formula 2), calculate the point (x) that is closest to it. i+1 ,y i+1 ,z i+1 The distance to (x) is calculated and added to the total length. Then, the nearest point (x) is calculated. i+1 ,y i+1 ,z i+1 ) is used as the new lowest point to calculate the distance between it and the third point, and the loop continues until the last two points are reached to end the loop, thus obtaining the total length L of the branch.

[0100] Step 5-4: Use the open-source point cloud processing library Open3D to read the point cloud of the branch and create an o3d.geometry instance. Use the "get_oriented_bounding_box()" function to obtain the OBB (Oriented Bounding Box) of the target branch. Assuming the center point of the bounding box is C, the three principal axis vectors are U1U2U3, and the size of the bounding box is L1L2L3, the directed bounding box can be represented by formula (3). Then, use the rotate(self,R) function in the Open3D library to calculate the rotation matrix of the obtained OBB bounding box. Assuming the rotation matrix is ​​R (formula 4), the vectors of its first two columns are extracted as U1U2U3 ... x and U y After normalization, they are respectively expressed as follows: and Because the first and second columns correspond to the X-axis and Y-axis directions respectively. The angle θ between the main direction of the bounding box and the XoY plane (i.e., the ground) can be obtained using the dot product property of vectors (Equation 5). Since the long side of the bounding box is basically consistent with the extension direction of the branch, the angle information of the branch is also obtained.

[0101]

[0102] OBB={C,U1,U2,U3,L1,L2,L3} (3)

[0103]

[0104] The process involves identifying the branches to be pruned, as well as the position of their lower ends, to perform the pruning operation. Step five can be implemented in the following way.

[0105] Step 5-1: By performing thresholding segmentation on the depth image from the depth camera 18 at the working end, a foreground image with complex background removed is obtained.

[0106] Mask the foreground color image to obtain the foreground branch image;

[0107] Step 5-2: Segment the main branch and side branch pixels using the SegNet semantic segmentation network, and obtain the mask images of the main branch and side branches after denoising;

[0108] Step 5-3: Perform joint calibration of the depth camera 18 and the close-range lidar 16 at the working end to obtain the external parameter matrix from the camera to the lidar, so that the point cloud and the image can be accurately matched, and the image coordinate system is projected onto the point cloud coordinate system.

[0109] Step 5-4: Based on the branch pixel positions identified in the image, segment the 3D point cloud of the main branch and lateral branches;

[0110] Step 5-5: Cluster the point cloud of a single fruit tree obtained by the 16 scans of the near-field lidar using the K-Means algorithm to obtain multiple point cloud clusters.

[0111] Steps 5-6: The resulting point cloud clusters are further divided into approximately cylindrical segments using a split-merge method. The segmentation step iterates through each point cloud cluster and decomposes them until all clusters can be approximated by cylinders. If the fit is unsatisfactory, it is further decomposed (dividing the point cloud cluster into two subsets). All clusters are traversed, and the center point of each cluster is calculated. Based on the center distance between clusters, overly segmented clusters are merged to obtain individual branch point clouds of the main branch and lateral branches.

[0112] Step Six: Drive the end effector 17 to move to the pruning area (e.g., S1), identify the branch to be pruned according to the pre-received and recorded pruning rules, and trigger the end effector 17 to perform the pruning operation until the pruning is completed based on the positional distance between the end effector 17 and the branch to be pruned. The specific control process in this step is as follows:

[0113] Step 6-1: Measure the branch inclination angle and branch length of a single lateral branch within the pruning area. Based on the user-specified pruning rules (branch inclination angle and branch length range), identify the branch to be pruned and the position of its lower end.

[0114] Step 6-2: Sort the pruning points in the field of view by distance and select the nearest branch to be pruned to perform the pruning operation;

[0115] Step 6-3: Based on the depth information obtained from the depth camera image, the positional distance and orientation between the end effector 17 and the pruning position are calculated in real time. The end effector and the pruning point are aligned in a straight line by the telescopic rod 6. The end effector is controlled to move closer to the pruning point by the telescopic arm 7. The movement trajectory of the robotic arm is recorded. After the depth camera 18 at the working end detects that the pruning position of the branch to be pruned has reached the executable range of the blade of the end effector 17, the end effector 17 is triggered to close and perform the pruning operation.

[0116] Step 6-4: Based on the robotic arm movement trajectory recorded in step P3, drive the robotic arm back to its original position, detect the next pruning point in the pruning area according to step P1, and execute steps P2 and P3 until there are no more pruning points in the area. Move the tracked chassis (19) and the rotating base (19) to the next pruning area.

[0117] During the above operation, when the intelligent robot for pruning fruit trees encounters an obstacle, the depth camera 14 and the navigation lidar 4 transmit the obstacle information to the edge computing module. At the same time, the inertial navigation unit 20 and the depth camera 14 calculate and process the road surface information. If the obstacle can be avoided, the robot starts the obstacle avoidance program. If the obstacle cannot be avoided, the robot will start the emergency stop system and issue an alarm.

[0118] In more realistic application scenarios, considering the complex environment of orchards that are unstructured, dynamic, and lack GPS, this application can preferably adopt the following technology to construct a cloud map of the entire orchard, and then locate the branches of fruit trees based on the cloud map of the entire orchard, and drive the robotic arm to prune the branches accurately according to the required location of the branches.

[0119] This preferred embodiment integrates multi-sensor fusion localization and 3D RGB point cloud mapping. To achieve accurate and robust pose estimation for tracked robots in complex orchards, this solution integrates measurement results from LiDAR odometry, inertial measurement unit (IMU), and visual-inertial odometry (VIO) into a loosely coupled multi-sensor fusion framework based on an extended Kalman filter (EPF) to compensate for the limitations of individual sensors. Simultaneously, an improved Loam_livox algorithm is used to generate a 3D RGB point cloud map of the surrounding environment. Combining the precise localization results with the 3D RGB point cloud map enables more accurate localization for the tracked robot and provides a foundation for further autonomous navigation.

[0120] In its implementation, the multi-sensor fusion localization part of this solution integrates attitude estimations from LiDAR odometry, IMU, and VIO. By processing the LiDAR scan point cloud, it outputs the LiDAR odometry data. The robot's rotation is estimated using the angular velocity provided by the IMU's three-axis gyroscope. Simultaneously, combining the attitude calculations from the LiDAR odometry and IMU, the robot's 6-DOF attitude is obtained. This attitude serves as the state model for the extended Kalman filter system. To correct the state model's pose estimation, the VIO (gyroscope and tightly coupled visual-inertial odometry data) output from the depth camera is used as the observation model. By weighted fusion of the Kalman gains of the state equation and the observation equation, the optimal solution for the estimated state can be obtained.

[0121] Specifically:

[0122] The localization and mapping strategy used in this embodiment is based on LiDAR SLAM technology, called Loam_livox. This technology is developed for the LiDAR solid-state LiDAR and is suitable for orchard scenarios. This method can provide real-time synchronous 20Hz odometry output and map updates. See details... Figure 8As shown, Loam_livox has five modules: point selection and feature extraction, iterative pose optimization, feature matching, odometry output, and map update. The input to this Loam_livox module is the raw point cloud from the LiDAR sensor, and the output is a robot pose and environment point cloud map. It is mainly used to perform pose estimation and panoramic mapping on the LiDAR scan data using SLAM in the following steps: First, the LiDAR sensor's point cloud undergoes a point selection strategy, discarding edge points, points with excessively high or low intensity, points with incident angles close to π or 0, and occluded points. Next, feature extraction is performed, classifying points into two main categories based on the local smoothness and reflectivity of candidate points: edge points and planar points. Planar points refer to points belonging to a plane. Due to the characteristics of LiDAR, an iterative pose optimization procedure is preferred to calculate the LiDAR pose. During the pose calculation stage, the residuals between points are used to estimate the pose. The processing of edge points and planar points is the same: first, find the 5 nearest points in the point cloud set, calculate the mean and covariance matrices for these 5 points to ensure that edge points are in a straight line and planar points are in the same plane, and then calculate the distance residuals. After obtaining the residuals for edge points and planar points, iterative pose solving is performed by constructing optimization equations. During the iteration process, two iterations are performed first to remove dynamic objects, and the pose is updated using the optimization results of the previous iteration. After removing 20% ​​of the maximum residual, full pose optimization is performed to obtain the six-DOF pose estimate of the LiDAR and output the point cloud map.

[0123] After processing the lidar scan point cloud and outputting the lidar odometry as described above, this embodiment further estimates the robot's rotation using the angular velocity provided by the IMU's three-axis gyroscope in the following manner: ω is defined as follows. t The angular velocity provided by the gyroscope. Gyroscope measurements are typically affected by noise, including white noise (n). ω and the bias b of the gyroscope ωt White noise n ω Modeled as Gaussian white noise, bias b ωt It is modeled as a random walk process, as shown in equations (1) and (2):

[0124]

[0125] The formula for angular velocity including error is:

[0126]

[0127] The time length between two consecutive lidar frames, frame k and frame (k+1), is t. k and t k+1 Then the pre-integral of the gyroscope can be calculated using formula (4):

[0128]

[0129] The pre-integral value of the gyroscope is calculated in the inertial coordinate system. Therefore, by performing a coordinate transformation on the pre-integral result, the rotation estimate in the world coordinate system can be obtained.

[0130] To further calibrate the state estimates from the LiDAR odometry and IMU, we employed a depth camera in the Vision-Inertial SLAM (VIO-SLAM) module to acquire the robot's 6DOF pose estimate. To integrate the depth camera data into the existing codebase, we installed the ZED Software Development Kit (SDK) 4.0 and used the ROS package zed-ros-wrapper to enable the depth camera to be used in the ROS environment.

[0131] Similar to other visual-inertial SLAM algorithms, VIO-SLAM provided by a depth camera consists of four main parts: visual front-end, IMU (Inertial Measurement Unit) back-end optimization, and loop closure detection. Like classic SLAM algorithms, the visual front-end extracts and matches features from two adjacent image frames and performs pose estimation. In the IMU calculation, pre-integration is required, calculating the camera's translation and rotation changes by double integration of acceleration and single integration of gyroscope deflection. In the back-end optimization, factor maps of camera and IMU poses are combined to... Figure 9 The steps shown achieve optimal pose estimation. In the loop closure detection step, the estimated trajectory is corrected by checking whether the robot revisits previously visited areas.

[0132] Therefore, this embodiment can improve the robustness and accuracy of attitude estimation by fusing attitude estimates from lidar odometry, IMU and VIO through a loosely coupled multi-sensor fusion framework based on extended Kalman filtering.

[0133] In this step, to effectively handle the nonlinearity of the system, this embodiment can further employ the extended Kalman filter algorithm to divide the system into two steps: prediction and update, and perform prediction and update based on the system's state model and observation model.

[0134] The state model and the observation model are as follows:

[0135]

[0136] In the formula, x k Let u be the state vector at time k. k ω is the input control quantity. k For state noise, f(·) is the system state transition function, which is nonlinear, z k Let v be the observed state vector at time k. kLet h(·) be the observation noise, and h(·) be the observation state function. Assume ω k and v k It is Gaussian white noise, and the amplitude of the noise follows a Gaussian distribution.

[0137] Assume the motion noise is Gaussian white noise with a mean of 0:

[0138] ω k ~N(0,Q) k )#(6)

[0139] Expanding the system's state vector using the first-order Taylor formula, the state model is transformed into linear equations:

[0140]

[0141] In the formula This is the posterior estimate of the state vector at time k-1; Let be the Jacobian matrix of the state model.

[0142] Assume the observation noise is Gaussian white noise with a mean of 0:

[0143] v k ~N(0,R k )#(8)

[0144] Expanding the system's observation vectors using the first-order Taylor series, the observation model is transformed into a linear equation:

[0145]

[0146] In the formula This is the prior estimate of the state vector at time k; The Jacobian matrix of the observation model.

[0147] The formula for the prediction part of the EKF algorithm can be derived as follows:

[0148] Prior estimation of the predicted state vector:

[0149]

[0150] The covariance matrix of the prior estimate of the predicted state vector:

[0151]

[0152] The update formula of the EKF algorithm can be derived as follows:

[0153] Calculate the Kalman gain:

[0154]

[0155] The posterior estimate of the state variable is obtained by updating the state variable using the observed values:

[0156]

[0157] Update the covariance matrix with the covariance matrix of the posterior estimate of the state variables:

[0158]

[0159] Therefore, this embodiment can utilize the workflow of extended Kalman filtering to estimate the state of the system: by fusing prior information (the system's state model and observation model) with actual observation information, the optimal state estimate is obtained, and by updating the state estimate and the estimated error covariance matrix in real time, the state estimate is continuously optimized, thereby improving the accuracy and stability of the estimate.

[0160] In practice, the ROS package robot_location we use employs the state estimates of LiDAR odometry and IMU as the state model and the state estimate of VIO as the observation model when implementing real-time multi-sensor fusion localization.

[0161] In this embodiment, the 3D RGB point cloud map construction part uses the improved Loam_livox algorithm to output RGB point clouds, thereby generating a 3D RGB point cloud map of the surrounding environment in real time.

[0162] Specifically, in the map building module, we can further improve the Loam_livox algorithm and add point cloud map coloring functionality. To ensure accurate matching between 3D point clouds and 2D images in indoor and outdoor scenes, we also added RGB colors to achieve joint calibration of the LiDAR and camera.

[0163] During colorization, we first align the timestamps of each frame of the LiDAR point cloud and the left image from the depth camera to ensure that each point cloud can be matched with the corresponding 2D image in real time. Next, we extract feature points and iteratively optimize the point cloud over three consecutive frames to estimate the robot's pose, and calculate the odometry output at a rate of 30Hz to ensure real-time map updates and real-time navigation and mapping capabilities. Then, using the calibrated coordinate transformation relationship, we project the point cloud set of planar and edge feature points onto the image pixel coordinate system and assign corresponding RGB information to each point cloud. Finally, we align the planar and edge feature points and reconstruct the map to continuously update the point cloud map and obtain the latest colored point cloud map.

[0164] To achieve localization, navigation, and obstacle avoidance functions, a high-precision point cloud map is required. In this embodiment, through a series of processing steps on the point cloud provided by the LiDAR, all parts seen by the robot can be modeled, thereby providing the mobile robot with detailed information about its surrounding environment.

[0165] Therefore, this application can guide the tracked robot to achieve detailed path planning and automatic obstacle avoidance in autonomous navigation tasks by using a three-dimensional RGB point cloud map of the orchard environment and multi-sensor fusion positioning results. It can also use a similar point cloud processing method to identify branches that need to be pruned based on the branch distribution structure of each fruit tree and drive the actuator to perform pruning.

[0166] To verify the effectiveness of the aforementioned integrated multi-sensor fusion positioning scheme, this application also conducted eight outdoor experimental tests. Four of these tests involved different movement trajectories, and the other four involved different scenarios, each test encompassing complex environmental conditions. During the experiments, we used the EVO accuracy evaluation tool to comprehensively assess the system's performance. In all eight outdoor experiments, the trajectory accuracy calculated by this application met the operational requirements of the tracked vehicle. The following details the specific process and results of the eight outdoor experiments conducted at the Baima Teaching and Research Base of Nanjing Agricultural University in Nanjing, Jiangsu Province, China. In all eight outdoor experiments, a modern horticultural pear orchard was selected as the experimental environment for the orchard robot's localization, mapping, and navigation.

[0167] During the trial testing in the orchard, the fruit trees were in the stage of new shoot growth and young fruit development. In the experiment, the tracked robot was controlled by a handle, moving in different scenarios and completing localization and mapping tasks in real time.

[0168] The system framework used in the 8 outdoor experiments:

[0169] In this experiment, the system is mainly divided into two parts, such as Figure 13 The diagram illustrates multi-sensor fusion localization and 3D RGB point cloud map construction. The multi-sensor fusion localization section integrates attitude estimation from LiDAR odometry, IMU, and VIO. It processes the LiDAR scan point cloud, outputs the LiDAR odometry, uses the angular velocity provided by the IMU's three-axis gyroscope to estimate the robot's rotation, and combines the attitude calculations from the LiDAR odometry and IMU to obtain the robot's 6-DOF attitude. This attitude serves as the state model for the extended Kalman filter system. To correct the pose estimation of the state model, the VIO output from the camera is used as the observation model. By weighted fusion of the Kalman gains of the state equation and the observation equation, the optimal solution for the estimated state can be obtained. In the 3D RGB point cloud map construction section, an improved Loam_livox algorithm is used, with point cloud coloring functionality added to the algorithm to output RGB point clouds, thereby generating a real-time 3D RGB point cloud map of the surrounding environment.

[0170] Test platforms and locations in 8 outdoor experiments:

[0171] This experiment used a self-developed orchard tracked robot platform. The robot's chassis employs a tracked structure, allowing it to adapt to the complex terrain of orchards and achieve movement in any direction within a planar space. The sensor system includes a lidar, a depth camera, and an IMU, mounted above the control box of the tracked vehicle to ensure its field of view does not scan the edges of the vehicle. The lidar utilizes a unique non-mechanical solid-state laser scanning technology, employing a Livox MID-70 (Livox, China), with a field of view of 70.4° and an accuracy of 100,000 points / second. The depth camera (ZED2, Stereolabs, America) has a field of view of 110°(H) x 70°(V) x 120°(D), a focal length of 2.12mm, and a baseline of 120mm. To integrate the depth camera data into the existing codebase, we installed the ZED Software Development Kit (SDK) 4.0 and used the ROS package zed-ros-wrapper to enable the depth camera to be used in the ROS environment. The IMU (WHEELTEC N100, WHEELTEC, China) includes a three-axis digital gyroscope with a measurement range of ±250° / s. High-precision point clouds provided by LiDAR are used to construct a visualized 3D point cloud map of a large-scale orchard environment. A high-precision inertial navigation system (INS, INS-D-E1, BDstar Navigation, China) is used for error comparison, supporting RTK positioning with an accuracy of 1cm + 1ppm.

[0172] To run the multi-sensor fusion localization and mapping algorithm, we used a laptop equipped with an AMD Ryzen 7 5800h CPU (3.2GHz), 16GB of RAM, and an NVIDIA GeForce RTX 3070 GPU. The algorithm was implemented on an Ubuntu 18.04LTS operating system running ROS Melodic, developed using C++.

[0173] The localization and mapping strategy models used in the 8 outdoor experiments:

[0174] The localization and mapping strategy used in this experiment is based on LiDAR SLAM technology, called Loam_livox. This technology was developed for the LiDAR solid-state LiDAR and is suitable for orchard scenarios. This method can provide real-time synchronous 20Hz odometry output and map updates. Loam_livox has five modules: point selection and feature extraction, iterative pose optimization, feature matching, odometry output, and map update. The input is the raw point cloud from the LiDAR, and the output is the robot pose and the environmental point cloud map.

[0175] To correct the state estimates from the LiDAR odometry and IMU, we employ a depth camera in the Visual-Inertial SLAM (VIO-SLAM) module to acquire the robot's 6DOF pose estimate. Similar to other visual-inertial SLAM algorithms, the VIO-SLAM provided by the depth camera consists of four main parts: visual front-end, IMU side, back-end optimization, and loop closure detection. Figure 14 This demonstrates the basic process of pose estimation using a depth camera.

[0176] The lidar-IMU-vision fusion technology based on extended Kalman filtering was used in 8 outdoor experiments.

[0177] In the experiment, we used the ROS package robot_location to realize real-time multi-sensor fusion localization. The state estimates of the lidar odometry and IMU were used as the state model, and the state estimate of VIO was used as the observation model. Through the loosely coupled multi-sensor fusion framework based on extended Kalman filtering, we can fuse the attitude estimates from lidar odometry, IMU and VIO to improve the robustness and accuracy of attitude estimation. Extended Kalman filtering is essentially an optimization problem solving method used to estimate the state of the system. By fusing prior information (the state model and observation model of the system) with actual observation information, the optimal state estimate is obtained, and the state estimate is continuously optimized by updating the state estimate and the estimated error covariance matrix in real time, thereby improving the accuracy and stability of the estimate

[23] . In order to effectively handle the nonlinearity of the system, the extended Kalman filtering algorithm divides the system into two steps: prediction and update, and performs prediction and update according to the state model and observation model of the system.

[0178] The state model and the observation model are as follows:

[0179]

[0180] In the formula, x k Let u be the state vector at time k. k ω is the input control quantity. k For state noise, f(·) is the system state transition function, which is nonlinear, z kLet v be the observed state vector at time k. k Let h(·) be the observation noise, and h(·) be the observation state function. Assume ω k and v k It is Gaussian white noise, and the amplitude of the noise follows a Gaussian distribution.

[0181] Assume the motion noise is Gaussian white noise with a mean of 0:

[0182] ω k ~N(0,Q) k )#(2)

[0183] Expanding the system's state vector using the first-order Taylor formula, the state model is transformed into linear equations:

[0184]

[0185] In the formula This is the posterior estimate of the state vector at time k-1; Let be the Jacobian matrix of the state model.

[0186] Assume the observation noise is Gaussian white noise with a mean of 0:

[0187] v k ~N(0,R k )#(4)

[0188] Expanding the system's observation vectors using the first-order Taylor series, the observation model is transformed into a linear equation:

[0189]

[0190] In the formula This is the prior estimate of the state vector at time k; The Jacobian matrix of the observation model.

[0191] The formula for the prediction part of the extended Kalman filter algorithm can be derived:

[0192] Prior estimation of the predicted state vector:

[0193]

[0194] The covariance matrix of the prior estimate of the predicted state vector:

[0195]

[0196] The update part of the extended Kalman filter algorithm can be derived as follows:

[0197] Calculate the Kalman gain:

[0198]

[0199] The posterior estimate of the state variable is obtained by updating the state variable using the observed values:

[0200]

[0201] Update the covariance matrix with the covariance matrix of the posterior estimate of the state variables:

[0202]

[0203] Point cloud coloring methods used in 8 outdoor experiments:

[0204] In the map building module, we improved the Loam_livox algorithm and added point cloud map coloring functionality. To ensure accurate matching between 3D point clouds and 2D images in outdoor scenes, and to add RGB color information to the point clouds, we performed the following steps in sequence: joint calibration of LiDAR and camera, point cloud filtering, timestamp registration, projection of LiDAR point clouds onto image pixels, and point cloud coloring.

[0205] We used the open-source livox_camera_lidar_calibration software to calibrate the device. This software is specifically designed for the joint calibration of livox LiDAR and camera. The calibration included calibrating the camera's intrinsic parameters, acquiring calibration data, and calculating the extrinsic parameters between the camera and LiDAR. The non-repeating scanning characteristic of the livox LiDAR makes it easier to find the accurate location of corners in high-density point clouds, resulting in better calibration results and improved LiDAR-camera fusion performance.

[0206] During colorization, the number of points in each frame of the point cloud is enormous, and coloring every single point would be extremely time-consuming. To ensure the real-time performance of the algorithm, point selection and feature extraction, iterative pose optimization, feature matching, and odometry output are required first. Filtering of the point cloud is mainly performed during point selection and feature extraction. The LiDAR sensor's point cloud first undergoes a point selection strategy, discarding edge points, points with excessively high or low intensity, points with incident angles close to π or 0, and occluded points. Next, feature extraction is performed, classifying points into two main categories based on the local smoothness and reflectivity of the candidate points: edge points and planar points, such as... Figure 24 As shown in the diagram. Here, a planar point refers to a point belonging to a plane. For planar feature points, to ensure that the five points found in the set of all planar points on the map are on the same plane, their covariance matrix is ​​calculated. If the smallest eigenvalue is less than one-third of the second smallest eigenvalue, then these five points are determined to be on the same plane.

[0207] We first align the timestamps of each frame of the LiDAR point cloud and the left image from the depth camera to ensure that each point cloud can be matched with its corresponding 2D image in real time. Since the frame rate of the LiDAR point cloud is 10Hz, while the frame rate of the left image from the depth camera is 30Hz, we need to select the camera image to match the real-time LiDAR point cloud. We record the timestamps of each frame of the point cloud and the image. After processing the point cloud to output pose estimation, we then perform temporal registration between the point cloud and the image. Because the frame rate of the image is higher than that of the point cloud, when registering the image corresponding to each frame of the point cloud, we select images whose timestamps are within 15ms before or after the timestamp of each frame of the point cloud. If there are multiple images, we select the one with the closest temporal distance.

[0208] After matching the point cloud with the corresponding image, we can use the extrinsic parameters of the LiDAR and the left eye of the depth camera to transform each point in the current frame's point cloud from the LiDAR coordinate system to the camera's left eye coordinate system. Then, we use the camera's intrinsic parameters to project the point cloud from the camera coordinate system to the pixel coordinate system. In the pixel coordinate system, the x and y coordinates of each point are the corresponding pixels. We then add the RGB information contained in the corresponding pixels to the point's information to obtain the point cloud containing RGB color information.

[0209] Finally, the planar feature points and edge feature points are aligned on the map, and the map is reconstructed to continuously update the point cloud map and obtain the latest tinted point cloud map. To achieve localization, navigation, and obstacle avoidance functions, a high-precision point cloud map is required. Through a series of processing steps on the point cloud provided by the LiDAR, all parts seen by the robot can be modeled, thus providing the mobile robot with detailed information about its surrounding environment. Therefore, by using a 3D RGB point cloud map of the orchard environment and multi-sensor fusion localization results, the tracked robot can achieve detailed path planning and automatic obstacle avoidance in autonomous navigation tasks, thus playing an important role in precision agriculture.

[0210] To demonstrate the effectiveness of the proposed multi-sensor fusion localization method, we conducted eight experimental tests in an outdoor orchard. Four tests involved different movement trajectories, and the other four were conducted in different scenarios to verify the feasibility of the proposed method under various conditions. Each test included complex environmental conditions, which posed a challenge to the robot's ability to achieve accurate localization.

[0211] Experiments on movement trajectories

[0212] Robots often need to travel some complex routes in orchards. Therefore, it is very necessary to study the diverse trajectory driving of robots in complex scenarios. We conducted four action trajectory test experiments, namely, the "square" trajectory, the turning trajectory, the curve trajectory, and the "N" trajectory. Among them, the "square" trajectory was carried out in a semi-structured pear orchard with a flat ground, a small slope, and support intervals between each row of pear trees. This trajectory test allows the robot to travel a closed route in an environment with a relatively low complexity, and such a route is very common in the actual application of robots. When the tracked robot turns between fruit trees, compared with the Loam_livox method, the trajectory estimated in this application is more accurate. Figure 12 (f) Analysis shows that when turning and in a sharp turn state, a large error occurred in the solution path of Loam_livox, resulting in a general subsequent trajectory effect. The solution proposed in this paper can appropriately correct the error. In addition, as can be seen from the third and fourth rows of Table 1, the root mean square error (RMSE) and median error of Loam_livox are 0.431939 and 0.357968 respectively, while the root mean square error (RMSE) and median error of the method proposed in this application are 0.118604 and 0.079514 respectively. All these prove that the above measurement method based on a depth camera and IMU can effectively improve the positioning accuracy of lidar, and the method provided in this application can effectively guide the operation of the tracked vehicle in orchards with various complex environmental conditions. In all tests, this method shows a high level of accuracy and robustness in terms of positioning and mapping.

[0213] Table 1. Comparison of APE values between the method in this paper and Loam_livox

[0214]

[0215] The turning trajectory, curved trajectory, and "N"-shaped trajectory were all tested in a sparsely planted pear orchard. In this scenario, the pear trees in each row are spaced far apart and unobstructed, allowing the robot to perform complex trajectory movements. The turning trajectory involves the robot making near-right-angle turns. During these turns, the number of features ahead is significantly reduced, increasing the instability and uncertainty of the sensor data. The curved trajectory involves the robot weaving between pear trees at a large angle. Sometimes the robot needs to alternate between fruit trees, making this trajectory test essential. During curved travel, the sensor data becomes highly unstable, increasing the difficulty of data processing for the algorithm. The unstable features also make feature matching more challenging, posing a significant challenge to the algorithm's robustness. To verify the robustness and accuracy of the proposed scheme in complex and varied trajectory travel, we had the robot perform an "N"-shaped trajectory, a complex and varied movement trajectory that presents a significant challenge.

[0216] Experiments in different scenarios

[0217] Different planting methods for fruit trees in orchards lead to variations in the internal environment, increasing environmental diversity and complexity. Therefore, it is necessary to study the robustness and accuracy of robots in complex and diverse scenarios. We conducted test experiments in four scenarios: a sparsely planted pear orchard, a densely planted pear orchard, a roadside or pondside, and pear trees aged 5-6 years. In the sparsely planted pear orchard, the row spacing was 4 meters and the column spacing was 3 meters; in the densely planted pear orchard, the row spacing was 2.8 meters and the column spacing was 1.5 meters. These scenarios, with their varying degrees of complexity and characteristics, presented significant challenges to the robot's precise localization.

[0218] In sparsely planted pear orchards, the pear trees are spaced roughly evenly, the ground is relatively regular, and each row of pear trees is protected by a support structure, preventing passage. Due to the larger spacing between the pear trees and the special pruning of the branches and leaves, this is the most common environment in orchards. In this scenario, the robot can only move in a straight line because the special planting method restricts its direction of travel. In densely planted pear orchards, the pear trees are planted very closely together, with narrow spacing between rows, resulting in more branches and leaves between rows, increasing obstructions and adding challenges to sensor feature extraction and matching. The road refers to the road within the orchard, characterized by a flat surface and pear trees on both sides. The robot has a wide field of vision in front with few features. Only on the sides of the robot are there numerous feature objects, posing a challenge to the robustness of localization and mapping. The environment next to the pond is characterized by different scenes on either side of the robot. One side is the pond, with a wide field of vision and virtually no feature objects. The refraction of light on the water surface also increases the difficulty of sensor data collection, reducing data reliability. The other side is the outer edge of the orchard, with more feature objects and diverse structures, providing rich data for the sensors. Due to the significant differences between the two scenes, the robot's localization and mapping require greater robustness.

[0219] Test evaluation indicators

[0220] All eight experiments used the EVO accuracy evaluation tool to comprehensively evaluate the system's performance. The evaluation metric for SLAM algorithm accuracy is the absolute pose error (APE) value for rotation and translation, also known as the absolute trajectory error (ATE). APE is a measure used to study the global consistency of SLAM trajectories.

[0221] APE is based on two poses P at timestamp i. est,i P ref,i ∈SE(3), which can be calculated using the formula:

[0222]

[0223] In the formula It is an inverse composition operator that takes two poses and gives the relative poses.

[0224] In APE, translation error is determined by using E i The calculation is based on the translation of the component, and the unit is meters.

[0225] APE trans,i =‖trans(E i )‖#(12)

[0226] The rotational error is determined by utilizing E i Calculated using the rotating part, unitless:

[0227] APE rot,i=‖rot(E i ) - I 3×3 ‖ F #(13)

[0228] By using the full E i to calculate the full error considering rotational and translational errors, unitless:

[0229] APE i =‖E i - I 4×4 ‖ F #(14)

[0230] Its root mean square error can be calculated using the absolute error at all times:

[0231]

[0232] In EVO, evo_res is a tool for comparing the evo_ape results of one or more files. Therefore, RES can be used for APE value comparison. For precision error analysis, we use a high-precision inertial navigation system for comparison.

[0233] Results of 8 outdoor experiments

[0234] For the action trajectory, Figure 12 and Table 1 show the RES between our method and Loam_livox in the action trajectory test.

[0235] Among them, the "mouth" - shaped trajectory experiment in the sparse pear orchard shows that:

[0236] As Figure 18 (a) shows, the tracked robot walked a "mouth" - shaped path in the pear orchard. This area has a "Y" - shaped tree support structure for construction, a relatively large tree spacing, and regular row spacing. There is a ditch in the middle of this area, and the tracked vehicle can pass through it, but it will cause bumps. The trajectory diagram shows that both methods can complete the task, but as can be seen from Figure 18 (b), the positioning error of Loam_livox is larger. The trajectory estimated by our method is more accurate, with a maximum absolute pose error (APE) difference of 0.2. Other calculated error metrics are also smaller than the error values of Loam_livox, as shown in the first and second rows of Table 1, which proves the effectiveness of this method in positioning in the orchard environment.

[0237] For the tracked robot, achieving turns between crop rows in a large orchard environment is a very crucial ability. The accuracy of turning directly affects the effect of autonomous precision agriculture work. Therefore, we also designed a turning trajectory experiment in the sparse - planted pear orchard.

[0238] As Figure 18As shown in (a), we evaluated the effectiveness of the multi-sensor fusion localization method in testing the turning trajectory by having the robot actively weave through the fruit trees to achieve the turning. Figure 18 (a) It is evident that the proposed method estimates a more accurate trajectory compared to Loam_livox's method. During sharp turns, Loam_livox's path calculation exhibits significant errors, resulting in suboptimal subsequent trajectory performance. The proposed method effectively corrects these errors. Furthermore, as shown in the third and fourth rows of Table 1, Loam_livox's root mean square error (RMSE) and median error are 0.510759 and 0.446691, respectively, while the proposed method's RMSE and median errors are 0.285338 and 0.275739, respectively. Therefore, when a tracked robot turns between fruit trees, the proposed method provides more accurate positioning, which is beneficial for the autonomous navigation of the orchard robot.

[0239] In large orchard environments, especially unstructured orchards, where there are no barriers between fruit trees, tracked robots sometimes need to perform curved movements. During curved movements, the tracking of feature points decreases, leading to unstable tracking and increased trajectory estimation errors. Therefore, we also designed a curved trajectory experiment in a sparsely planted pear orchard.

[0240] In the test action trajectory curve, such as Figure 19 As shown in (a), the effectiveness of the multi-sensor fusion localization method was evaluated. From Figure 19 (a) It is evident that the Loam_livox method exhibits trajectory deviation in the middle of large-angle curves, leading to significant errors in subsequent trajectories. In contrast, the proposed solution relies on depth camera and IMU sensor data to correct errors in the middle of large-angle curves, resulting in smaller subsequent trajectory errors. Furthermore, as shown in rows 5 and 6 of Table 1, all error metrics of Loam_livox are superior to those of the proposed solution. Therefore, when a tracked robot traverses large-angle curves in forest terrain, the proposed solution can provide more accurate positioning for the robot.

[0241] In large orchard environments, tracked robots need to perform many complex navigation tasks. To verify the robustness and accuracy of the proposed solution in complex trajectory navigation, this study also designed an experiment to enable the tracked robot to perform "N"-shaped trajectory movement in a sparsely planted pear orchard.

[0242] from Figure 19(a) It can be seen that the Loam_livox method has a large error at corner tracking points because the LiDAR's field of view is small, which makes it impossible to match and identify feature points when making large turns. This reduces the number of features between frames, resulting in poor convergence of iterative error data and increasing pose calculation errors. The proposed scheme can utilize the advantages of multi-sensor fusion to correct trajectory errors at corners. Figure 19 The box plot analysis in (c) shows that the estimation error of the proposed scheme is within a small range. Furthermore, rows 7 and 8 of Table 1 show that the error indices obtained by other algorithms are also smaller than the error value of Loam_livox. Therefore, the proposed scheme can reduce the pose error of tracked robots to a small range in complex environments such as forests, demonstrating its effectiveness.

[0243] Table 2 shows the relative performance (RES) of our method and Loam_livox in different scenarios. Quantitative results are compared in Table 2.

[0244] Table 2. Comparison of APE values ​​between the method presented in this paper and Loam_livox

[0245]

[0246] For sparsely planted pear orchards:

[0247] Tracked robots encounter various complex environments in large orchards. This complexity and diversity pose significant challenges to the robot's navigation and localization, directly impacting the effectiveness of autonomous precision agriculture. To verify the accuracy of the proposed solution in straight-line movement within a structured pear orchard, such as... Figure 20 As shown in (a), this study enables the tracked robot to move in a straight line.

[0248] from Figure 20 (a) It can be intuitively seen that both methods have good error values ​​in this scenario. This is because in a structured pear orchard with supports, the pear trees are spaced roughly evenly, and the ground is relatively regular, facilitating the movement of the tracked robot. Furthermore, there are no large turns during movement, the tracking and matching of feature points are good, and the pose calculation error is small. Meanwhile, as can be seen from the first and second rows of Table 2, the error indices obtained by both methods are also small. However, the proposed scheme is superior to the Loam_livox method because multi-sensor fusion technology provides more data information for pose calculation, resulting in higher accuracy. Therefore, both schemes are relatively accurate in trajectory estimation in the structured pear orchard scenario, but the proposed scheme has a smaller error.

[0249] For densely planted pear orchards:

[0250] The orchard also features densely planted fruit trees. This scenario tests the accuracy and robustness of the tracked robot navigating through dense orchards. Figure 21 (a) Due to the dense planting, the tracked robot can only move in a straight line. Both solutions have very small error ranges, such as... Figure 21 As shown in (a), the numerous objects between rows of dense fruit trees provide excellent feature points for the sensor. The increased data volume reduces the pose error in the computation, improving accuracy and robustness. For example... Figure 21 As shown in the box plot in (c), both schemes perform well and have good accuracy. As can be seen from the third and fourth rows of Table 2, the error indices of both methods are very small. The advantage of multi-sensor fusion is that it utilizes diverse data to provide more accurate information. Table 2 also shows that multi-sensor fusion is slightly superior to the Loam_livox method.

[0251] Regarding the road to the orchard:

[0252] To evaluate the tracked robot's localization capabilities on flat surfaces, this scenario involved testing the robot by driving it along an orchard road. Figure 22 As shown in (a), the Loam_livox method exhibits a larger error, primarily due to two reasons. First, the small field of view of the lidar prevents accurate feature point matching at turns, increasing the error and affecting subsequent accuracy. Second, the significant vibration of the tracked vehicle on the road reduces the accuracy of data acquired by various sensors, further increasing the error. The comparison of errors in rows 5 and 6 of Table 2 with other scenarios also shows that the multi-sensor fusion scheme experiences a significant increase in error in this scenario. Therefore, both schemes show increased trajectory estimation errors, but the proposed scheme exhibits smaller errors in this environment.

[0253] Regarding the pond in the orchard:

[0254] Orchards can sometimes present relatively complex environments. Therefore, this scenario test was conducted around a pond next to the orchard, such as... Figure 23 As shown in (a), the overall positioning effect of this scheme is good, and the estimated trajectory matches the actual ground conditions. Loam_livox's trajectory becomes uneven in the later stages, as shown in (a). Figure 23(b) Increased error. In this scenario, the proposed method achieves high trajectory accuracy with an RMSE of 0.105891, indicating overall reliability. Therefore, the measurements from the depth camera and IMU effectively improve the positioning accuracy of the lidar, and the proposed method is also applicable to such semi-open complex scenarios. In open environments, there are fewer objects, and the reflection from the pond surface poses a significant challenge to sensor data acquisition. As can be seen from rows 7 and 8 of Table 2, the depth camera and IMU, to some extent, eliminate the scale error of the lidar, thereby improving positioning accuracy. Therefore, the multi-sensor fusion method can compensate for the deficiencies of each sensor, achieving relatively more accurate positioning in complex environments.

[0255] In addition, considering that localization and mapping are both key capabilities for robots to achieve autonomous navigation, the mapping quality was also evaluated in this experiment as follows:

[0256] This application utilizes 3D RGB point cloud maps to provide tracked robots with more detailed information about their surrounding environment and to offer more map information for intelligent orchard management. Therefore, the robot can achieve more accurate and efficient autonomous navigation. To verify the feasibility of the 3D RGB point cloud map method, a series of experiments were conducted in a complex orchard environment. Regarding the processing time of one frame of LiDAR point cloud, the improved Loam_livox algorithm achieves 75.01ms, an increase of 2.79ms compared to the original Loam_livox algorithm, fully meeting the requirements for real-time localization and mapping. Figure 17 As shown:

[0257] Experiments 1 and 2 mentioned above in this application were both conducted in orchards and orchards. Various fruit tree planting methods were employed in the orchards; we selected structured "Y"-shaped orchards and densely planted unstructured orchards to generate 3D RGB point cloud maps. Figure 17 In the diagram, 'a' presents a 3D RGB point cloud map of a densely planted, unstructured orchard; 'd' shows a 3D RGB point cloud map of a Y-shaped, structured orchard. These two different planting structures each have their own characteristics, thus posing a challenge in constructing 3D RGB point cloud maps. Experimental results show that the 3D RGB point cloud maps display well, and the real-time RGB map algorithm is effective. Unstructured crops, structured supports, and the netting above the orchard can all be well-constructed into RGB point cloud maps, and the various colors are clearly represented. However, these maps still have room for improvement. Due to the limitations of LiDAR scanning characteristics and frame rate, point cloud acquisition cannot fully depict the shape of the entire tree, and the spacing between fruit trees and leaves also causes LiDAR light to pass through the orchard without generating effective point clouds. Therefore, the tree shape seen on the map is not complete.

[0258] Experiments 3 and 4 mentioned above in this application were both conducted in orchard environments with spatial constraints and structural characteristics. Experiment 3 was conducted between a pond and fruit trees within an orchard, such as... Figure 17 As shown in (g) above, the weeds beside the orchard and the pond were successfully constructed. Therefore, the 3D RGB point cloud map construction method is suitable for such environments. In Experiment 4, we constructed the road in the orchard and the adjacent orchard, as shown in (g). Figure 17 As shown in (j) of the diagram. In this scenario, the road and the outer trellis of the orchard were successfully constructed, the map coloring was clear and accurate, and the spatial structure of the environment was clearly displayed on the map. However, these maps still have room for improvement. Due to the similarity of the environments, which are relatively open spatial structures, the reflection and reception of laser point clouds are reduced, and the density of the point clouds decreases, thus affecting the clarity of the fruit tree structure. Through map construction experiments, our results demonstrate the robustness of the 3D RGB point cloud map construction method in unstructured, dynamic, and large-scale orchard environments.

[0259] In summary, this application addresses the challenges posed by the dynamic and unstructured nature of orchard environments to accurate and stable localization and mapping. To create richer point cloud maps and provide reliable data for intelligent management, a real-time localization and environmental map construction system for an automatic fruit tree pruning system is proposed. This system utilizes laser SLAM and multi-source information fusion technology to achieve real-time localization and 3D RGB point cloud map construction for a mobile robot in an orchard environment. First, data from lidar odometry, gyroscopes, and tightly coupled visual-inertial odometry (VIO) are integrated into a loosely coupled framework based on an extended Kalman filter to obtain more accurate robot pose estimation. In the multi-sensor fusion algorithm, lidar odometry and gyroscope pose estimates are used as predictions, while VIO localization results are used as observations to update the pose estimation. Finally, the Loam_livox algorithm is improved by adding camera image color information to the lidar point cloud to construct a real-time 3D RGB point cloud map of the orchard environment. The performance of the proposed system is evaluated in the complex environment of a modern standard orchard. Our method integrates multi-sensor data, enabling it to address various challenges in orchard environments and achieve precise localization even under diverse conditions. It exhibits high robustness and accuracy, even tolerating significant estimation errors inherent in LiDAR algorithms. Furthermore, our framework improves LiDAR odometry localization accuracy. The improved 3D RGB point cloud mapping provided in this application is suitable for unstructured, dynamic orchards, generating clear point cloud maps that intuitively present map colors and adapt to diverse orchard environmental challenges. This validates the advantages of the proposed method and highlights its potential application in autonomous navigation for agricultural robots.

[0260] This application proposes a novel real-time localization and mapping method for unstructured, dynamic, and GPS-deficient orchard environments. To achieve accurate and robust pose estimation for tracked robots in complex orchards, this method integrates measurement results from LiDAR odometry, inertial measurement unit (IMU), and visual-inertial odometry (VIO) into a loosely coupled multi-sensor fusion framework based on an extended Kalman filter (EPF) to compensate for the limitations of individual sensors. Simultaneously, an improved Loam_livox algorithm is used to generate a 3D RGB point cloud map of the surrounding environment. By combining the precise localization results with the 3D RGB point cloud map, this application enables more accurate localization for tracked robots and provides a foundation for further autonomous navigation.

[0261] This application was also evaluated in orchards with various complex environmental conditions. In all tests, this application demonstrated high levels of accuracy and robustness in both localization and mapping. However, during testing, the reliability of feature point tracking decreased when turning due to the small field of view of the lidar, thus reducing accuracy in the multi-sensor fusion localization and mapping system. Furthermore, this study did not deeply integrate localization and mapping results for autonomous navigation. Therefore, future work may require the multi-sensor fusion localization framework to incorporate other sensors to adapt to more complex environments, while better integrating localization and mapping results to achieve autonomous navigation in complex agricultural environments.

[0262] The above are merely embodiments of this application, and their descriptions are quite specific and detailed, but they should not be construed as limiting the scope of this patent application. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application.

Claims

1. A machine vision based automatic pruning system for fruit trees, characterized in that, The application relates to a track vehicle provided with a rotating base (19) and a mechanical arm device having telescopic freedom, and the top of the mechanical arm device is provided with an end effector (17), a working end depth camera (18) and a short-range laser radar (16). The track vehicle is also provided with a navigation laser radar (4), a navigation depth camera (14) and an inertial navigation unit (20) in communication connection with an edge computing module (15). The edge computing module (15) is used for performing the following steps: According to a pre-received and recorded orchard digital map, the track vehicle is driven to run in the orchard according to a planned path based on inertial navigation data fed back by the inertial navigation unit (20), orchard images fed back by the navigation depth camera (14) and scanning information detected by the navigation laser radar (4). During the running of the track vehicle, the navigation laser radar (4) is triggered to scan the orchard region, and the navigation depth camera (14) is synchronously triggered to collect orchard images, then the laser point cloud obtained by the navigation laser radar (4) and the orchard images obtained by the navigation depth camera (14) at the same time are subjected to real-time positioning and synchronous mapping, a pose is output, data assimilation is carried out by using an extended Kalman filter, and an orchard panoramic point cloud map is updated and generated. According to the orchard panoramic point cloud map, the point cloud of a fruit tree in a given region is filtered to remove point clouds with a height higher than a trunk, the trunk position of the fruit tree is recognized, and the corresponding parking point of the robot during pruning is determined according to the trunk position. After the track vehicle is driven to run to the corresponding parking point near the fruit tree to be pruned according to the position of the single fruit tree point cloud, the local point cloud obtained by the short-range laser radar (16) and the near-range image collected by the working end depth camera (18) are segmented, background noise is removed, the point cloud cluster corresponding to the fruit tree is extracted, and then the pruning area is determined according to the pre-received and recorded pruning rules. The rotating base (19) and the mechanical arm device are driven to move the end effector (17) to the pruning area, and the end effector (17) is triggered to perform pruning work according to the position distance between the end effector (17) and the tree branch to be pruned. The specific steps of segmenting the local point cloud obtained by the short-range laser radar (16) and the near-range image collected by the working end depth camera (18), removing background noise and extracting the point cloud cluster corresponding to the fruit tree include the following steps:

2. The automatic machine vision-based pruning system for fruit trees as claimed in claim 1, wherein, Firstly, the depth image of the working end depth camera (18) is subjected to threshold segmentation to obtain a foreground image with complex background removed, and the foreground image is masked to a near-range color image to obtain a foreground branch image; Then, the pixels of the main branch and the side branch in the foreground branch image are segmented by a SegNet semantic segmentation network to obtain a mask image of the main branch and the side branch. ​ The work end depth camera (18) and the short-range laser radar (16) are jointly calibrated again to obtain a camera-to-radar external parameter matrix, so that the point cloud and the image can be accurately matched, the image coordinate system is projected to the point cloud coordinate system, and based on the branch pixel position identified by the image, the three-dimensional point cloud of the branch is segmented, and the point cloud cluster corresponding to the fruit tree is extracted.

3. The automatic machine vision-based pruning system for fruit trees as claimed in claim 1, wherein, The laser point cloud obtained by scanning the navigation laser radar (4) and the navigation depth camera (14) at the same time are real-time positioned and synchronized mapping, and the pose is output, the data assimilation is performed by using the extended Kalman filter, the panoramic point cloud map of the orchard is updated, and the process of positioning and three-dimensional point cloud information includes the following steps: Step S1: based on the time stamp marked when the navigation depth camera (14) and the navigation laser radar (4) are collected, the orchard image and the laser scanning data with a time stamp interval less than 30 milliseconds are registered; Step S2: joint calibration of the navigation depth camera (14) and the navigation lidar (4) through livox camera lidar calibration, first, the intrinsic matrix K of the camera is obtained, then the calibration board is used to simultaneously shoot the images of the navigation lidar and the camera to calibrate, and the extrinsic matrix T of the camera to the radar is obtained LIDAR_CAMERA , according to [X LIDAR ,y LIDAR ,Z LIDAR ,1] = T LIDAR_CAMERA -1 ·(Z·K -1 ·[u,v,1]) project the image coordinate system to the point cloud coordinate system, wherein x LIDAR represents the position of the point in the horizontal direction under the laser radar coordinate, y LIDAR represents the position of the point in the left-right direction under the laser radar coordinate, z LIDAR represents the position of the point in the vertical direction under the laser radar coordinate, Z represents the depth corresponding to the pixel coordinate, K -1 represents the inverse matrix of the camera intrinsic matrix, and [u,v,1] represents the position of the point in the pixel coordinate system; Step S3: after the point cloud preprocessing and feature point extraction steps, the Loam_livox algorithm is used for real-time positioning and synchronous mapping based on the livox laser radar, and the VIO-SLAM algorithm is used for real-time positioning and synchronous mapping based on the ZED2 depth camera, and the corresponding pose information is output respectively; Step S4: the extended Kalman filter is used to assimilate the data of the pose output by the laser radar SLAM and the depth camera SLAM, the camera field of view is used to calibrate the pose error caused by the loss of laser SLAM feature points; after initializing the initial position and map of the robot, the extended Kalman filter algorithm predicts the next state through the state model, and then updates the position, direction and map variables of the robot through the sensor model and observation model and the currently detected data, and through repeated execution of the above steps, the position and attitude of the robot at any time are calculated; Step S5: according to the real-time pose, the corresponding position of the orchard digital map is updated, and the panoramic three-dimensional point cloud of the orchard is updated in real time; Step S6: based on the improved Loam_livox algorithm, the mapping relationship between the laser point cloud and the camera point cloud is used to color the laser point cloud, and the panoramic three-dimensional color point cloud of the orchard is generated.

4. The automatic machine vision-based pruning system for fruit trees as claimed in claim 1, wherein, According to the pruning area density pruning rules received and input in advance, the specific steps of segmenting and adjusting the horizontal angle and longitudinal position of the mechanical arm of the pruning area are as follows: Step T1: according to the height and direction, the fruit tree point cloud cluster is segmented into different units corresponding to different pruning areas, numbered as S1, S2 to Sn; Step T2: rotate the base (19) to adjust the horizontal angle of the mechanical arm, and retract the rod (6) to adjust the longitudinal position of the mechanical arm, and sequentially adjust each pruning area to be maintained within the field of view of the work end depth camera (18) and the short-range laser radar (16) on the end effector.

5. The machine vision-based automatic pruning system of fruit trees as claimed in claim 4, wherein, According to the point cloud obtained by scanning the short-range laser radar (16) and the short-range image collected by the work end depth camera (18), the point cloud cluster corresponding to the branch is segmented and extracted in the following manner: Step R1: Obtain the foreground image by threshold segmentation on the depth image of the working end depth camera (18), mask to the close-range color image, and obtain the foreground branch image; Step R2: Segment the main branch and side branch pixels by the SegNet semantic segmentation network, and obtain the mask image of the main branch and side branch after denoising; Step R3: Accurately match the point cloud and image according to step S2 by the working end depth camera (18) and the close-range laser radar (16), and project the image coordinate system to the point cloud coordinate system; Step R4: Segment the three-dimensional point cloud of the main branch and side branch based on the branch pixel position recognized by the image; Step R5: Cluster the single-fruit-tree point cloud obtained by the close-range laser radar (16) scanning to obtain multiple point cloud clusters corresponding to different fruit trees by the K-Means algorithm; Step R6: Further segment and merge the point cloud cluster of each tree by cylindrical fitting and calculating the distance between cluster centers, and obtain the single branch point cloud of the main branch and side branch.

6. The automatic machine vision-based pruning system for fruit trees as claimed in claims 1-4, wherein, The specific steps of driving the end effector (17) to move to the corresponding pruning area, identifying the branch to be pruned according to the pre-received and entered pruning rules, and triggering the end effector (17) to perform pruning operation according to the position distance between the end effector (17) and the branch to be pruned until the pruning is completed include: Step P1: Measure the branch angle and length of the single side branch in the pruning area, and identify and select the branch to be pruned and the lower end position of the branch based on the required branch angle range and branch length range in the user-specified pruning rule; Step P2: Sort the pruning points in the field of view by distance, and perform pruning operation on the closest pruning branch of the end effector; During the pruning operation, the position distance and orientation between the end effector (17) and the pruning position are calculated in real time based on the depth information obtained from the depth camera image, the end effector is adjusted to be on the same straight line with the pruning point through the extension of the telescopic rod (6), the end effector is controlled to approach the pruning point through the telescopic arm (7), the motion trajectory of the robot arm is recorded, and after the working end depth camera (18) detects that the pruning position of the branch to be pruned reaches the executable range between the blades of the end effector (17), the end effector (17) is triggered to drive the blades to close for pruning operation; Step P3: Drive the robot arm to return to the original position according to the recorded motion trajectory of the robot arm during the pruning operation, detect the next pruning point in the pruning area according to step P1, and perform steps P2 and P3 until there is no pruning point in the area, and move the crawler chassis (19) and the rotating base (19) to the next pruning area.

7. The machine vision-based automatic pruning system of fruit trees as claimed in claim 1, wherein, The robot arm device comprises: Supporting vertical pole (8), telescopic arm (7) and contraction pole (6), wherein the bottom of the supporting vertical pole (8) is connected with the rotating base (19) for realizing horizontal rotation, the upper part of the supporting vertical pole (8) is movably connected with the telescopic arm (7), the contraction pole (6) is arranged between the supporting vertical pole (8) and the telescopic arm (7), and the end of the telescopic arm (7) is provided with an end effector (17); the contraction pole (6) is telescopic, drives the telescopic arm (7) to make longitudinal circular motion with the length of the telescopic arm (7) as a radius; the telescopic arm (7) is telescopic, drives the end effector (17) to move forward and backward.

8. The machine vision based automatic pruning system of fruit trees as claimed in claim 6 wherein, The end effector (17) comprises a stepping motor (26), a transmission mechanism and a trimming knife; The output shaft direction of the stepping motor (26) is perpendicular to the opening and closing movement direction of the trimming knife, the end of the output shaft is provided with a circular table structure (27) in surface contact with the transmission mechanism, the circular table structure (27) drives the transmission mechanism to make circular motion, and a gear (24-1) is further arranged on the transmission mechanism; one side of the trimming knife is provided with a rack structure (22-1) matched with the gear (24-1); The transmission mechanism is provided with a circular ring structure (24-2) on the outer periphery, the thickness of the circular ring structure (24-2) gradually decreases from the inner circular end to the outer circular end; the side surface of the circular table structure (27) is in surface contact with the circular ring structure (24-2); The stepping motor (26) drives the circular table structure (27) to rotate, the circular ring structure (24-2) rotates synchronously in the rotating process, the gear (24-1) rotates synchronously, and the gear (24-1) meshes with the rack structure (22-1) to drive the moving blade (22) to rotate relative to the fixed blade (25) to open and close.

9. The automatic machine vision-based pruning system for fruit trees as claimed in claim 3, wherein, In the step S3, the steps of pose estimation and panoramic mapping of the data scanned by the laser radar by using the SLAM algorithm specifically include: First, the edge points, points with too large or too small intensity, points with incident angle close to π or 0 and blocked points in the laser point cloud collected by the navigation laser radar (4) are removed; Then, feature extraction is performed, and based on the local smoothness of the candidate points and the reflectivity of the points, the points are divided into two main categories: edge points and plane points, wherein the plane points refer to the points belonging to a plane; Then, an iterative pose optimization program is used to calculate the pose of the navigation laser radar, and in the calculation process, the residual error between the points is used to estimate the pose: For the edge points and the plane points, the nearest five points in the point cloud set are found first when estimating the pose, the mean and the covariance matrix of the five points are calculated to ensure that the edge points are on a straight line and the plane points are on the same plane, and then the distance residual error is calculated; after obtaining the edge point residual error and the plane point residual error, the iterative pose is solved by constructing an optimization equation; in this iteration process, two iterations are first performed to remove dynamic objects, and the pose is updated using the optimization result of the last iteration, and after removing 20% of the maximum residual error, full pose optimization is performed to obtain the six-degree-of-freedom pose estimation of the navigation laser radar and output the orchard panoramic point cloud map.

10. The machine vision-based automatic pruning system of fruit trees as claimed in claim 9, wherein, The real-time pose in the step S3 is determined according to a rotation estimation of the mechanical arm determined according to the following steps: Let ω t The angular velocity provided to the gyroscope is affected by noise, let n ω be the white noise contained in the gyroscope measurement, and let b ωt be the bias of the gyroscope ω be modeled as a Gaussian white noise, and let b ωt be modeled as a random walk process, the resulting angular velocity formula containing errors is Let the length of time between two consecutive near-range lidar frames, the kth frame and the k+1th frame, be t k and t k+1 then the pre-integration of the gyroscope The pre-integration result is subjected to coordinate transformation, i.e. a rotation estimation in the world coordinate system is obtained.

Images