A three-dimensional navigation method for orchard working robots based on a grid map

Abstract

This invention relates to the field of orchard navigation technology, specifically a 3D method for orchard operations based on a grid map. The method includes: constructing an orchard point cloud map using a mapping algorithm; filtering the acquired 3D point cloud map and constructing a triangular grid map of the orchard, embedding the orchard's GPS information into the triangular grid map; based on the triangular grid map with GPS information, performing ICP registration between the point cloud data scanned by the LiDAR and the points on the triangular grid map using the RMCL 3D positioning algorithm, outputting the 3D pose of the operation robot in the orchard; updating the 3D pose of the operation robot in the orchard using GNSS information; planning the navigation path of the operation robot using grid navigation based on the GPS information of the target point; and controlling the operation robot to dynamically avoid obstacles and navigate to the target point. This invention enables precise positioning and autonomous navigation of the operation robot in hilly orchards, allowing it to complete tasks such as orchard inspection, plant protection, harvesting, and transportation.

Description

Technical Field

[0001] This invention relates to the field of navigation technology for orchard robots, and more specifically to a three-dimensional navigation method for orchard robots based on grid maps. Background Technology

[0002] In recent years, with the decline in the agricultural labor force, agricultural robots have received widespread attention and developed rapidly. Automatic navigation technology is the core technology for solving the autonomous movement of agricultural robots. Currently, in orchard production, robots can move autonomously in relatively flat orchard environments to perform tasks such as inspection, plant protection, harvesting, and transportation, reducing labor costs and improving crop production efficiency. However, in hilly orchards in southern China, the terrain is rugged with significant elevation differences, and the planting slope of fruit trees often exceeds 15 degrees. This results in poor robot positioning and navigation using traditional two-dimensional navigation, sometimes even preventing autonomous positioning and navigation. Using GPS alone for positioning and navigation is also problematic because tree branches and leaves can block GNSS signals, hindering accurate positioning and navigation. Furthermore, the significant elevation differences in hilly orchards mean that using two-dimensional grid maps for positioning and navigation lacks three-dimensional Z-axis information, leading to significant positioning errors. If only three-dimensional point clouds are used as radar odometry, the accumulated errors in point cloud positioning gradually increase during robot operation, resulting in substantial positioning errors. Therefore, a three-dimensional navigation method for orchard operation robots that combines three-dimensional navigation technology is needed to enable the robot to locate and navigate autonomously in orchards in the hilly and mountainous areas of southern China, and to complete tasks such as orchard inspection, plant protection, harvesting and transportation. Summary of the Invention

[0003] To address the technical problems existing in the prior art, this invention provides a three-dimensional navigation method for orchard operation robots based on a grid map. By fusing data from lidar, IMU inertial sensors, and GPS positioning systems, a high-precision three-dimensional point cloud map is constructed. Based on the RMCL algorithm and Mesh_Navigation algorithm, the operation robot can achieve precise positioning and autonomous navigation in hilly orchards, completing tasks such as orchard inspection, plant protection, harvesting, and transportation.

[0004] The objective of this invention can be achieved by adopting the following technical solutions:

[0005] A 3D navigation method for orchard robots based on grid maps includes the following steps:

[0006] S1. Obtain the three-dimensional point cloud data of the orchard and the angular velocity and acceleration of the robot through the operation robot, and generate the orchard point cloud map based on the FAST-LIO2 mapping algorithm or LIO-SAM mapping algorithm.

[0007] S2. The obtained 3D point cloud map is filtered by radius filtering algorithm and statistical filtering algorithm. Based on the filtered 3D point cloud map, an orchard triangular grid map is constructed. The GPS information of the orchard is embedded into the triangular grid map, and a triangular grid map with GPS information is output.

[0008] S3. Based on the triangular mesh map with GPS information, the point cloud data scanned by the LiDAR and the points of the triangular mesh map are registered by ICP based on the RMCL three-dimensional positioning algorithm. The three-dimensional pose of the robot in the orchard is output. The three-dimensional pose of the robot in the orchard is updated by combining the GNSS information of the robot. The navigation path of the robot is planned by Mesh_Navigation according to the GPS information of the target point. The robot is controlled to dynamically avoid obstacles and navigate to the target point.

[0009] Specifically, the feature is that the operation robot includes: a tracked chassis, a lidar, an IMU inertial sensor, a GNSS receiver, a computer controller, and a router. The lidar, IMU inertial sensor, GNSS receiver, computer controller, and router are all mounted on the tracked chassis. The lidar scans the environment around the operation robot in real time to generate three-dimensional point cloud data of the orchard. The IMU inertial sensor acquires the angular velocity and acceleration of each axis during the operation of the operation robot, and the GNSS receiver collects the GNSS signal of the operation robot.

[0010] Specifically, the process of generating orchard point cloud maps based on the FAST-LIO2 or LIO-SAM mapping algorithm includes: selecting either the FAST-LIO2 or LIO-SAM mapping algorithm according to the size of the orchard; using the computer controller to calculate the relative motion changes between adjacent lidar frames through pre-integration based on the acceleration and angular velocity data of the working robot collected by the IMU inertial sensor; predicting the current pose state based on the state of the previous moment; using the matching residual between the lidar point cloud and the local map as the observation value; optimizing the predicted current pose state using iterative Kalman filtering; and outputting the final optimized pose estimate through joint prediction and updating. The generated orchard point cloud map is then displayed in the computer controller based on the optimized pose estimate.

[0011] Specifically, step S2 includes:

[0012] S21. The obtained orchard point cloud map is filtered using the radius filtering algorithm and statistical filtering algorithm based on the PCL library to obtain the filtered orchard point cloud map.

[0013] S22. Based on the filtered point cloud map of the three orchards, a triangular mesh map of the orchards is constructed using the greedy triangulation method of the PCL library. By projecting the three-dimensional point cloud onto a two-dimensional plane, using two-dimensional Delaunay triangulation, and then mapping the result back to three-dimensional space, a mesh map is generated by triangulating the point cloud through greedy triangulation projection.

[0014] S23. Based on the orchard's terrain information, embed GPS information into the orchard's triangular grid map.

[0015] Specifically, step S22 includes:

[0016] The legality of the input point cloud map of the three orchards is verified, and the number of points contained in the point cloud map is calculated. For point cloud maps with more than four million points, voxel filtering is performed on the points to reduce the point cloud sampling for SLAM mapping.

[0017] Radius filtering is applied to the filtered point cloud to remove discrete noise points caused by environmental influences, and statistical filtering is used to remove the influence of off-cluster noise in the point cloud map.

[0018] The filtered point cloud is smoothed using the least squares method to remove the high-frequency vibration effects caused by sensor noise in the LiDAR, while preserving the planar and curved features of the ground and walls. The point cloud normal vectors are calculated using GPU acceleration, and a gridded map is generated by triangulating the point cloud through greedy triangulation.

[0019] Specifically, step S3 includes:

[0020] S31. Based on the RMCL three-dimensional positioning algorithm, the point cloud data collected by the LiDAR in real time is registered with the nearest distance of the points on the triangular mesh surface. The nearest point of intersection between each triangular mesh surface and the laser ray is taken as the predicted pose estimation point, and the three-dimensional pose of the robot at the predicted pose estimation point is output.

[0021] S32. In areas with stable GNSS, the robot corrects the accumulated positioning error due to the lack of GPS positioning in real time by acquiring the obtained GNSS information. At the same time, it uses a Kalman filter to predict and update the positioning error in GNSS-rejected areas, thereby updating the three-dimensional pose of the robot in the orchard.

[0022] S33. Using the triangular mesh map as the global map, the Dijkstra algorithm and CVP algorithm are used to plan the navigation path of the robot based on the GPS information of the target point, and the robot is controlled to dynamically avoid obstacles and navigate to the target point.

[0023] Specifically, step S31 includes:

[0024] Based on the NVIDIA OptiX accelerated ray tracing API, Intel Embree ray tracing library, and Rmagine computation library, the RMCL 3D localization algorithm is used to perform ICP registration between the point cloud data scanned by the LiDAR and the points on the triangular mesh map. The NVIDIA OptiX accelerated ray tracing API is used to track the laser beam emitted by the LiDAR, and the shortest distance between the laser ray and the mesh map is calculated to achieve 3D localization of the robot. The Intel Embree ray tracing library is used to track and calculate the laser beam emitted by the LiDAR, and the result is fed back to the robot to obtain its 3D localization. The Rmagine computation library is used to track the laser beam emitted by the LiDAR and simulate a distance sensor, calculating the distance relationship between the laser beam and the sensor, and transmitting the calculated data to the computer control unit. Based on the calculated data, the computer control unit uses the closest intersection point between each triangular mesh face and the laser ray as the estimated position point for the robot's predicted pose.

[0025] Specifically, step S32 includes:

[0026] The robot's current position coordinates are calculated by the GNSS receiver based on the received real-time GPS information. The difference between the robot's current position coordinates and the position coordinates of the embedded GPS information read from the map positioning is used as the positioning error. The existing positioning path is corrected with the positioning error as the confidence level. The GNSS positioning is updated by directly registering with the current frame and historical frames of the grid map through ICP registration, thus correcting the positioning error accumulated without GPS positioning.

[0027] Specifically, step S33 includes:

[0028] The difference between the received GPS information of the target point and the GPS information of the grid to which the robot is located is calculated. Based on Mesh Navigation, the difference is published to the MoveBase navigation stack node in the form of a topic message. Static obstacle information obtained by the grid map and dynamic obstacle information obtained by LiDAR scanning are combined to construct a suitable planned path for the robot to reach the target point based on the Dijkstra algorithm and the CVP algorithm.

[0029] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0030] 1. This invention provides a 3D navigation method for orchard operation robots based on grid maps. Through multi-sensor fusion, 3D map analysis, and 3D navigation technology, the operation robot achieves autonomous operation and navigation in the orchard. The dual SLAM mapping scheme and environmental perception are integrated with LiDAR, inertial sensors, and GPS positioning system to construct high-precision 3D point cloud maps for different needs. This overcomes the inability of a single SLAM mapping scheme to cope with complex orchard environments and provides stable and reliable mapping results and environmental perception capabilities.

[0031] 2. Rapid point cloud optimization and mesh conversion: Utilizing the PCL library and integrating GPU and CUDA computing acceleration cores, this method converts and generates mesh maps while simultaneously optimizing point cloud map noise and achieving hardware acceleration. It constructs a feature-based 3D mesh map, reducing the time consumed by map loading and analysis.

[0032] 3. Optimization of positioning errors and external guidance in areas with regional GNSS rejection: Addressing the variability of complex orchard environments, in areas with stable GPS positioning, the system continuously updates the positioning error generated during 3D positioning using acquired GNSS information and GPS information embedded in the grid map. Kalman filtering is then used to determine a higher-precision 3D pose based on the positioning error. By acquiring externally input GPS coordinate information and calculating the results using GPS information embedded in the grid, external guidance is achieved for the robot's patrol operations.

[0033] 4. High-precision 3D positioning and navigation: The RMCL algorithm enables high-precision positioning of the robot in the orchard. It can determine the robot's pose in the orchard by the correspondence between laser rays and a grid map, achieving precise navigation. Simultaneously, the user can specify a target point in the computer control unit, and the Mesh_Navigation algorithm will plan the robot's automatic navigation to the target point. Attached Figure Description

[0034] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0035] Figure 1 This is a control flow diagram of the three-dimensional navigation method for orchard operation robots in an embodiment of the present invention;

[0036] Figure 2 This is a structural block diagram of the orchard operation robot in an embodiment of the present invention;

[0037] Figure 3This is a flowchart of the SLAM mapping and point cloud optimization conversion method in an embodiment of the present invention;

[0038] Figure 4 This is a schematic diagram of the three-dimensional positioning method in an embodiment of the present invention and a comparison diagram of positioning errors;

[0039] Figure 5 This is a flowchart illustrating the principle and process of the three-dimensional positioning and navigation method for the work robot in this embodiment of the invention. Detailed Implementation

[0040] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments, and the implementation of the present invention is not limited thereto. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0041] Example 1:

[0042] like Figure 1 The diagram shows the control flow of a three-dimensional navigation method for orchard robots. The three-dimensional navigation method for orchard robots based on a grid map, as described in this invention, includes the following steps:

[0043] S1. Obtain the 3D point cloud data of the orchard and the angular velocity and acceleration of the robot through the operation robot, and generate the orchard point cloud map based on the FAST-LIO2 mapping algorithm or LIO-SAM mapping algorithm.

[0044] Specifically, the operational robot device includes: a tracked chassis, a LiDAR (Light Detection and Ranging) system, an IMU (Inertial Measurement Unit), a GNSS receiver, a computer controller, and a router. The LiDAR, IMU, and GNSS receiver are all mounted on the tracked chassis. The LiDAR, IMU, and GNSS receiver are all connected to the computer controller via signal transmission, and connected to the user terminal via the router.

[0045] Specifically, the tracked chassis is a mobile platform carrying various hardware components. Powered by a built-in 48V DC power supply, it drives two brushless motors on the left and right sides of the chassis upon receiving a motion signal. The motors then output final power via a reducer, enabling autonomous driving of the tracked vehicle in the orchard. Control commands from the computer are sent to the embedded system, where they are converted and output as control signals to regulate wheel speed and headlight brightness. The tracked chassis's status information is fed back to the computer.

[0046] LiDAR (Light Detection and Ranging) is used to scan the environment around a robotic work vehicle in real time, generating 3D point cloud data of the orchard. Specifically, LiDAR acquires point cloud data corresponding to various target objects around the orchard. A 16-line 3D LiDAR can scan the environment in front of the laser beam. The LiDAR obtains the distance information of each target object by emitting laser pulses and measuring their return time. Through multiple measurements, the distance information of the target object in different directions can be obtained. LiDAR can measure the distance of objects with extremely high precision, typically at the millimeter level. Therefore, this distance information can be accurately converted into spatial coordinates, thereby generating point cloud data, representing the 3D position of objects in the environment. The simplest form of point cloud data contains only Cartesian coordinates of x, y, and z.

[0047] Inertial measurement units (IMUs) are used to acquire the angular velocity and acceleration of each axis during the operation of the robot. This allows for path correction, reducing deviations in the generated navigation path and enhancing the robot's navigation accuracy.

[0048] The GNSS receiver is used to collect GPS information from the robot. Based on the collected GPS information from the robot, the GPS information of the orchard is obtained, which can assist the robot in positioning with high precision. By solving the satellite signal and ground reference station data based on carrier phase dynamic differential technology, real-time position acquisition is realized in the SLAM mapping process, and loop closure detection is completed.

[0049] The aforementioned computer, serving as the host computer for the tracked vehicle, needs to be able to perform operations such as SLAM mapping, 3D positioning, integrating IMU inertial sensor data, and issuing navigation commands to the robot. To ensure that the computer has sufficient computing power and a relatively small size, the Thunderobot MIX mini-host is selected as the computer.

[0050] The router is used to connect the sensors and the computing control unit for data exchange. It uses the TCP / IP LAN protocol to facilitate information exchange between LiDAR data and the computing control unit, enabling collaborative operation between the various modules. The user terminal connects to the computing control unit via the router, allowing users to remotely control the computer's virtual desktop.

[0051] like Figure 2The diagram shows the structural block of the orchard operation robot. The robot's main body is a tracked chassis, on which are mounted collaborative components such as a LiDAR, GNSS receiver, IMU inertial sensor, and router. To save space and ensure sufficient performance for the computer control unit, a high-performance, low-power mini-PC, the Thunderobot MIX, is used. The GNSS receiver, IMU inertial sensor, LiDAR, and router are all connected to the Thunderobot MIX mini-PC, which in turn connects to the user terminal via the router. Before using the robot, its devices and algorithms need to be initialized and configured, including joint calibration of the LiDAR and IMU, pairing the CAN card with the robot chassis, and configuring the ROS package and environment.

[0052] In this example, the LiDAR, IMU inertial sensor, and GNSS receiver, along with their corresponding ROS package files, are enabled on the work robot. The robot then performs SLAM mapping of the entire orchard. After mapping, the PCL library is used to filter and optimize the orchard point cloud map, ultimately outputting a triangular mesh map. This triangular mesh map is then transmitted to the computer control unit, where the corresponding positioning and navigation algorithm determines the robot's pose within the orchard. Given a target point on the computer control unit, the tracked vehicle automatically calculates and plans a suitable path and sends motion commands to the chassis via a CAN card. The internal motion control system calculates the speed control and automatically navigates the work robot to the target point.

[0053] Specifically, the process of generating orchard point cloud maps based on the FAST-LIO2F mapping algorithm or the LIO-SAM mapping algorithm includes: selecting either the FAST-LIO2 or LIO-SAM mapping algorithm according to the size of the orchard to generate orchard point cloud maps; the computer controller calculating the relative motion changes between adjacent lidar frames through pre-integration based on the acceleration and angular velocity data of the robot collected by the IMU inertial sensor; predicting the current pose state based on the state of the previous moment as the subsequent pose state update stage; the computer controller using the matching residual between the lidar point cloud and the local map as the observation value; optimizing the predicted current pose state using iterative Kalman filtering; and outputting the final optimized pose estimate through joint prediction and update; and displaying the generated orchard point cloud map in the computer controller based on the optimized pose estimate.

[0054] Specifically, the acceleration and acceleration measurement values ​​of the IMU are defined as follows:

[0055] ;

[0056] in, The angular velocity measured by the IMU at time t. The actual angular velocity, The bias of a gyroscope is a slowly changing systematic error. White noise (measurement noise) of a gyroscope.

[0057] ;

[0058] in, Acceleration caused by non-gravitational external forces, 𝑎 𝑡 : The robot's true linear acceleration in the world coordinate system W, : The gravity vector in the world coordinate system The rotation matrix from the world system W to the local system B. This means converting the actual acceleration (excluding gravity) from the current system to the current system. : Zero bias of the accelerometer White noise from the accelerometer.

[0059] If IMU measurements are used to infer the robot's motion, then the robot's time... speed ,Location and rotation It can be represented as:

[0060] ;

[0061] ;

[0062] ;

[0063] Among them, 𝑉 𝑡 : 𝑡 is the velocity in the world system at any given time, 𝑔∆𝑡 is the velocity change caused by gravity, 𝑅𝑡 is the rotation matrix from the local system B to the world system W.

[0064] Here it is assumed that the angular velocity and acceleration of B are constant during the integration process, that is... .

[0065] The relative motion increment between two time steps i and j is calculated by IMU pre-integration. The relative positional motion relationship between the two time steps i and j obtained by IMU pre-integration is as follows:

[0066] Relative velocity increment : ;

[0067] Relative position increment : ;

[0068] Relative rotation increment : ;

[0069] Using the matching residuals between the LiDAR point cloud and the local map as observations, the relationship between the two, when the LiDAR scans the map to obtain scene planar features and edge features, can be expressed as:

[0070] Edge feature point-line distance :

[0071] ;

[0072] Planar feature point-plane distance :

[0073] ;

[0074] For planar features , , and The resulting planar blocks are then used to minimize the optimal transformation using the Gauss-Newton method to find the pose transformation of the current frame that minimizes the sum of the distances from all feature points in the current frame to their corresponding feature geometry in the previous frame.

[0075] ;

[0076] The edge and plane features extracted by the LiDAR scan at time i are represented as follows: and Based on the optimal transformation result, the pose state vector at time i... pose state vector at time i+1 Relative transformation between It can be represented as:

[0077] ;

[0078] Among them, T i T represents the pose at time i. i+1 Represents the pose at time i+1; inverse matrix Used to transfer T i+1 By transforming to the coordinate system at time i, we obtain the relative motion transformation from time i to i+1. The pose of each frame is obtained after optimization, and then subjected to relative transformation. The relative motion between frames is obtained and accumulated to form a complete odometer trajectory.

[0079] In this embodiment, considering a large orchard (generally defined as one larger than 30 mu), a LiDAR scanner continuously scans the environment around the robot to generate 3D point cloud data. Due to the relatively large environment, although the fruit trees are planted in a relatively regular pattern, there is a significant amount of weeds, resulting in a large mapping area for the robot. Using a conventional SLAM mapping scheme would yield an excessively large number of point clouds, leading to substantial processing loads. Traditional LiDAR SLAM often employs a loosely coupled approach, where the LiDAR handles pose estimation, and the IMU (Inertial Measurement Unit) only assists by filling in the inter-frame gaps in the LiDAR. Information fusion is post-processed. This method is highly sensitive to sensor noise and dynamic interference, especially in large outdoor scenarios like wild orchards. The accumulated sensor errors and environmental interference during continuous mapping can easily cause LiDAR inter-frame matching failures, leading to point cloud invalidation. The LIO-SAM algorithm is used for mapping. During the mapping process, the LiDAR calculates the relative pose residuals between adjacent frames through feature point matching, while the IMU processes the acceleration and angular velocity of the IMU inertial sensor through pre-integration and generates relative motion constraints between adjacent LiDAR frames. The front end outputs the residuals of each value of the LiDAR and the IMU inertial sensor. The back end of the computer receives the two residuals and converts them into factors and adds them to the same factor map. The optimization goal is to minimize the sum of the residuals of all factors and output the optimal pose, thus obtaining a point cloud map with a small voxel density.

[0080] For orchards no larger than 30 mu (approximately 2 hectares), the limited space and relatively haphazard planting of fruit trees, coupled with minimal environmental noise interference from weeds, result in a relatively small SLAM mapping range for the robot. Compared to the LIO-SAM mapping scheme, FAST-LIO2 abandons factor graph optimization and adopts a framework that fuses tightly coupled iterative Kalman filtering with point-by-point motion distortion compensation. During the robot's mapping process, it performs closed-loop real-time estimation of the covariance between the acceleration, angular velocity, and IMU (Inertial Measurement Unit) sensor states (such as zero bias) and the mapping state. Therefore, when choosing a scene mapping scheme requiring high contrast, multiple feature points, and strong robustness, FAST-LIO2 is more suitable than LIO-SAM.

[0081] S2. The obtained orchard point cloud map is filtered using radius filtering and statistical filtering algorithms. Based on the filtered orchard point cloud map, an orchard triangular grid map is constructed. The GPS information of the orchard is embedded into the triangular grid map, and a triangular grid map with GPS information is output.

[0082] S21. The radius filtering algorithm and statistical filtering algorithm based on the PCL library are used to filter the acquired orchard point cloud map to obtain the filtered orchard point cloud map.

[0083] The PCL (Point Cloud Library) is a large-scale open-source, cross-platform point cloud processing library. Its radius filtering algorithm is primarily used for downsampling and simplification of point clouds. Within a sphere of a specified radius, if the number of points is below a threshold, they are identified as outliers and removed. It can also be used to homogenize point cloud density. This directly addresses your question about "improving the filtering optimization speed for dense point clouds" by reducing the amount of data processed subsequently. Statistical filtering is mainly used to remove outlier noise points. Based on statistical analysis of the average distance between each point and its K nearest neighbors, it filters out points whose distance to the mean exceeds a certain multiple of the standard deviation. It effectively removes "floating" and isolated noise points, a crucial step in ensuring data purity before mesh reconstruction. It can perform a series of corresponding optimization filtering processes on the point cloud map generated by SLAM mapping and finally output the transformed triangular mesh map. This algorithm can improve the filtering optimization speed for dense point clouds while also reducing the occurrence of defects in triangular mesh reconstruction when the voxel density is low and the point cloud sampling is limited.

[0084] S22. Based on the filtered point cloud map of the three orchards, the greedy triangulation method of the PCL library is used to construct a triangular mesh map of the orchards. This is done by projecting the three-dimensional point cloud onto a two-dimensional plane, using two-dimensional Delaunay triangulation, and then mapping the result back to three-dimensional space. The point cloud is then triangulated by greedy triangulation projection to generate a mesh map.

[0085] In this embodiment, the incoming three-orchard point cloud map is first validated for legality. The number of points in the three-orchard point cloud map is calculated, and a threshold for the number of points (e.g., four million points) is used as a dividing point to determine the selection of subsequent filtering algorithms. For point cloud maps with more than four million points, voxel filtering is first applied to reduce point cloud sampling for SLAM (Simultaneous Localization and Mapping) mapping. Then, radius filtering is applied to the filtered point cloud to remove discrete noise points caused by environmental influences. Finally, statistical filtering is used to remove the influence of off-cluster noise in the point cloud map. After filtering, the point cloud is smoothed using the least squares method to remove the high-frequency vibration influence caused by sensor noise of the LiDAR, while retaining the planar and curved features of the ground and walls. Since the smoothing process requires calculating the normal vector for each smoothed surface, a GPU is used to accelerate the calculation of the point cloud normal vector. Finally, a meshed map is generated by greedy triangulation projection of the point cloud, resulting in a triangular mesh map. For large point cloud maps with fewer than four million points, the triangulation algorithm is the same except for omitting the voxel filtering step. The final output triangular mesh map will be used as a global map reference for 3D positioning and navigation.

[0086] S23. Based on the orchard's terrain information, embed the orchard's GPS information into a triangular grid map and output a triangular grid map with GPS information.

[0087] The orchard's GPS information is obtained by a GNSS receiver, primarily acquiring latitude, longitude, and altitude information from the GPS data.

[0088] like Figure 3 The diagram illustrates the process of SLAM mapping and point cloud map optimization conversion for the operational robot. Users manually operate the operational robot to scan and map the entire lychee orchard by running the mapping ROS package on the computer control unit. Different mapping schemes are implemented based on the size of the lychee orchard. The generated point cloud map is then transmitted to the meshing ROS package. The density of the input point cloud is assessed to determine if voxel filtering downsampling is necessary. Finally, the optimized and filtered point cloud map is converted into a triangular mesh map, stored, and output to the computer control unit as a global map reference for subsequent 3D localization and navigation.

[0089] S3. Based on the triangular mesh map with GPS information, the point cloud data scanned by the LiDAR and the points of the triangular mesh map are registered by ICP based on the RMCL three-dimensional positioning algorithm. The three-dimensional pose of the robot in the orchard is output. The three-dimensional pose of the robot in the orchard is updated by combining the GNSS information of the robot. The navigation path of the robot is planned by Mesh_Navigation according to the GPS information of the target point. The robot is controlled to dynamically avoid obstacles and navigate to the target point.

[0090] S31. Based on the RMCL three-dimensional positioning algorithm, the point cloud data collected by the LiDAR in real time is registered with the nearest distance of the points on the triangular mesh surface. The nearest point of intersection between each triangular mesh surface and the laser ray is taken as the predicted pose estimation point, and the three-dimensional pose of the robot at the predicted pose estimation point is output.

[0091] In this embodiment, based on the NVIDIA OptiX accelerated ray tracing API, the Intel Embree ray tracing library, and the Rmagine computing library, the RMCL 3D localization algorithm is used to perform ICP registration between the point cloud data scanned by the LiDAR and the points on the triangular mesh map. The LiDAR-emitted laser beam is tracked by calling the NVIDIA OptiX accelerated ray tracing API, and the 3D localization of the robot is achieved by calculating the shortest distance between the laser beam and the mesh map. The Intel Embree ray tracing library is used to perform tracking calculations on the LiDAR-emitted laser beam, and the results are fed back to the robot to obtain its 3D localization. The Rmagine computing library is used to track the LiDAR-emitted laser beam and simulate a distance sensor, calculating the distance relationship between the laser beam and the sensor, and transmitting the calculated data to the computer control unit. Based on the calculated data, the computer control unit uses the closest intersection point between each triangular mesh face and the laser beam as the robot's predicted pose estimation point.

[0092] The NVIDIA OptiX accelerated ray tracing API is a GPU-accelerated ray tracing API launched by NVIDIA. It can deeply utilize CUDA cores and dedicated ray tracing units of RTX series graphics cards, and its core is to improve ray tracing efficiency by leveraging the parallel computing capabilities of NVIDIA RTX graphics cards. It tracks the laser beam emitted by the LiDAR by calling NVIDIA OptiX, and calculates the shortest distance between the laser ray and the grid map to achieve 3D positioning of the robot. The Intel Embree library is a CPU-accelerated ray tracing library developed by Intel, optimized for the multi-core architecture of various Intel CPUs, fully utilizing the parallel computing capabilities of the CPU, and without GPU dependency. Its core is to achieve efficient ray tracing on Intel CPUs. It tracks and calculates the laser beam emitted by the LiDAR by calling the Embree library, and feeds the result back to the robot to obtain its 3D positioning. The Rmagine computing library is used to quickly and accurately simulate virtual distance sensors in large 3D environments using ray tracing. It tracks the laser beam emitted by the LiDAR and simulates the distance sensor, integrates Embree and OptiX to simulate and calculate the distance relationship between the laser beam and the sensor, and finally transmits the calculated data to the computing control computer.

[0093] In one application embodiment, the computational controller receives the incoming triangular mesh map file, opens the ROS package of RMCL in the system, and the triangular mesh map sent by the system will be displayed in rviz. At this time, the computational controller will simultaneously receive the point cloud data sent by the LiDAR scan and perform registration calculations on the distance between the laser ray and the triangular mesh face using the RMCL algorithm. The RMCL algorithm first finds the nearest point corresponding to the ray projection point on the triangular mesh face, then uses P2P and P2L as error measures, estimates the optimal transformation parameters through covariance reduction and SVD, and finally applies the optimal transformation parameters to pose guessing. Simultaneously, it searches again for the corresponding points of the ray projection point and the triangular mesh vertices to optimize the 3D pose of the final output predicted point. For the registration of the nearest point cloud, locating the feature point closest to the point cloud dataset usually requires accelerating the point cloud search by pre-constructing a kd-tree or hash grid on the dataset. In the mesh map, this is usually different; the main difference is that the nearest point is searched on each constructed triangular mesh face, rather than using the vertices on the mesh surface as the ICP registration feature points of the point cloud. The RMCL algorithm utilizes the BVH algorithm in Embree to accelerate the nearest-distance registration of point cloud data scanned by LiDAR with points on triangular mesh surfaces. For triangular mesh processing, the RMCL algorithm uses the OptiX API on NVIDIA GPUs to efficiently calculate the intersection points of laser rays with the mesh. OptiX helps the computational controller accelerate the search for the required laser ray projection correspondence points. When calculating the laser projection correspondence points, the computational controller converts the LiDAR measurements into ray representations and, using an initially given pose estimate, conditionally tracks the virtual ray along the path the LiDAR will scan. Simultaneously, the intersection points with the mesh map are used as the actual corresponding points to be measured by the robot during pose estimation. Then, the computational controller projects the Cartesian points measured by the LiDAR onto the intersection plane to determine the map correspondence, matching the surface closest to the robot to the projection correspondence point. For all ray projection cases, the computational controller uses the Rmagine library to flexibly construct sensor simulations based on the final ray projections. Finally, the nearest intersection point between each triangular mesh surface and the laser ray is used as the final output predicted pose estimation point.

[0094] like Figure 4The diagram illustrates the process of the 3D localization method for the robot and explains the 2D localization error. The robot constructs a SLAM point cloud map of the scene using a LiDAR and an IMU inertial sensor. After filtering and optimization, the point cloud map is converted into a grid map and uploaded to the terminal computer. The RMCL 3D localization algorithm, based on the first input pose prediction point, uses OptiX in the virtual distance sensor Rmagine to track the LiDAR ray. It calculates the nearest neighbor of the intersection of the LiDAR ray and the grid using Embree, and accelerates ICP registration of the point cloud using HBV. ICP registration is performed between the current frame point cloud actually acquired by the robot and the simulated point cloud obtained in the previous step. Through iterative optimization, ICP calculates the pose transformation that best aligns the two point clouds. The pose corrected by ICP is output as the localization result at the current moment, ultimately outputting the predicted 3D pose of the point. Simultaneously, the 3D pose estimation of the localized point is predicted and optimized again.

[0095] S32. In areas with stable GNSS, the positioning error accumulated due to the lack of GPS positioning is corrected in real time by acquiring the obtained GNSS information. At the same time, a Kalman filter is used to predict and update the positioning error in GNSS-rejected areas, thereby updating the three-dimensional pose of the robot in the orchard.

[0096] Specifically, in orchards located in the steep hilly and mountainous areas of southern China, where the slope β typically exceeds 15 degrees and the slope distance is long, significant cumulative errors may occur, causing the robot to lose accurate positioning. Therefore, it is necessary to correct the accumulated positioning errors caused by the lack of GPS positioning in real time using acquired GNSS information. The GNSS receiver calculates the robot's current position coordinates based on the received real-time GPS information. The difference between the robot's current position coordinates and the position coordinates from the embedded GPS information read from the map positioning is used as the positioning error. After obtaining the positioning error, the existing positioning path is corrected using the positioning error as the confidence level. ICP registration is used to directly register with the current and historical frames of the grid map, achieving GNSS positioning updates and correcting the positioning errors accumulated without GPS positioning. The embedded GPS information read from the map positioning is the precise position coordinates of the orchard's "baseline point" or "reference trajectory," obtained beforehand through high-precision surveying, and stored in the map used by the robot.

[0097] In a steep mountain orchard environment, during the conversion of the LiDAR point cloud into a 2D grid map, the robot obtains distance information from the map that is the projected distance of its actual position on the 2D grid map due to the lack of Z-axis information. This distance is represented by the robot's positioning distance Fx based on the 2D map, as shown in the figure. The actual distance obtained by the LiDAR and the distance information read from the 3D grid are represented by the true distance measured in 3D space, Rx (hypotenuse distance), as shown in the figure. The difference between these two distances is Δx. Therefore, the relative error RE is:

[0098] ;

[0099] Let the slope of the ramp be β. The larger this value is, the larger the relative error value RE will be, and the greater the error generated by the robot in two-dimensional positioning.

[0100] In areas with good GNSS signal, the robot relies on GNSS for absolute positioning. However, in "GNSS-denied areas" caused by tree canopy obstruction, terrain occlusion, etc., the robot's inertial measurement unit (IMU) calculates the trajectory. The gyroscope integration of the IMU gives the robot's attitude increment, and the accelerometer integration after attitude compensation gives the robot's velocity and position increment. The IMU pre-integration is used as a priori through a Kalman filter, and LiDAR is used to correct the robot's three-dimensional pose in the orchard in real time.

[0101] The formula for a Kalman filter can be expressed as:

[0102] Algorithm Kalman_filter( , , , );

[0103] Among them, 𝜇 t ₋1 represents the best estimate (posterior estimate) of the system state vector at time t-1, 𝛴 t ₋1 represents the state estimation covariance matrix at time t-1. t Let represent the control input vector at time t, 𝑧 t This represents the observation at time t.

[0104] Mean of predicted GPS confidence With variance It can be represented as:

[0105] (1)

[0106] (2)

[0107] Kalman gain matrix Represented as:

[0108] (3)

[0109] in, The observation noise covariance matrix represents the uncertainty of the observation.

[0110] (4)

[0111] (5)

[0112] return (6)

[0113] Among them, the predicted confidence mean is converted into the desired expected confidence mean through formulas (3) and (4), and the observation is used. With expected measurement The Kalman gain adjustment of the difference between the values ​​is used to update the confidence level, and finally the variance of the current confidence level is obtained through formula (5). After passing through the Kalman filter, the mean and variance data of the updated GPS confidence level are finally obtained.

[0114] S33. Using the triangular mesh map as the global map, the Dijkstra algorithm and CVP algorithm are used to plan the navigation path of the robot based on the GPS information of the target point, and the robot is controlled to dynamically avoid obstacles and navigate to the target point.

[0115] Specifically, the difference between the received GPS information of the target point and the GPS information of the grid to which the robot is located is calculated. Based on Mesh_Navigation, the difference is published to the MoveBase navigation stack node in the form of a topic message. Static obstacle information obtained by the grid map and dynamic obstacle information obtained by LiDAR scanning are combined to construct a suitable planned path for the robot to reach the target point based on the Dijkstra algorithm and the CVP algorithm.

[0116] Specifically, Mesh Navigation employs Dijkstra's algorithm and the CVP algorithm for path planning. Dijkstra's algorithm handles global path planning, while CVP optimizes local trajectories. Using Dijkstra's algorithm, the robot plans a globally optimal path from its current position to the target fruit tree (GPS point). This path follows the rows of fruit trees, avoiding all known trunks. The CVP algorithm calculates the clearance distance from the robot's current position to the nearest obstacle. A larger clearance indicates greater safety, allowing the robot to run faster; a smaller clearance indicates proximity to the obstacle, requiring the robot to slow down. Path searching can be performed directly on the triangular mesh map. Given a target point (from the fruit tree GPS database), the planner searches the mesh surface for a walkable and cost-optimal path from the current position to the target point. This allows the robot to follow the global path and avoid dynamic obstacles in real time, ensuring the robot can dynamically avoid obstacles and autonomously navigate to the target point to complete its tasks in the orchard. In an orchard environment, GPS data from the fruit trees is integrated with GNSS information to update and correct the robot's actual position. This allows for real-time acquisition of the robot's global position within the orchard. By comparing the ground truth GNSS value with the grid positioning data, the ground truth GNSS value is ultimately used as the true position. The 3D positioning and navigation technology based on triangular mesh maps enables the robot to achieve 3D positioning and autonomous navigation within the orchard.

[0117] In this embodiment, after achieving 3D positioning, the computer controller uses the target point data transmitted via remote connection from the user and employs either CVP (Clothoid Velocity Profile) or Dijkstra's algorithm for path planning. A triangular mesh map is used as the global map to guide the robot to avoid static obstacles, planning a safe path around them. Simultaneously, based on real-time scanning information from the LiDAR, the robot is guided to avoid dynamic obstacles, making local path adjustments to achieve real-time obstacle avoidance. This enables 3D navigation of the robot in the orchard, reducing the manpower required for harvesting and transportation.

[0118] like Figure 5The diagram shows the principle and flowchart of the 3D localization and navigation method for the robot. A LiDAR and IMU (Inertial Measurement Unit) system constitute the radar-inertial navigation system. Based on the input mesh map file, the computer controller uses OptiX (NVIDIA's real-time ray tracing engine) to track the laser rays emitted by the LiDAR. Then, the correspondence between the laser rays and the mesh map is simulated in the Rmagine library, a library specifically designed for robot simulation. Rmagine is responsible for establishing the correspondence between the LiDAR model and the mesh map. Rmagine calls the BVH (Bounding Volume Hierarchy) data structure in Embree to manage the mesh map. BVH acts like an index for the map, quickly skipping empty areas and directly finding the intersection of the laser rays and triangular faces, achieving rapid registration between the laser and the mesh. The computer controller determines the 3D pose (X, Y, Z, pitch, roll, yaw) of the robot in the lychee orchard based on the information output by the pose calculation RMCL (Realm Proof-of-Loop Collision Calculation), while the filtered data from the IMU is used as the prior odometry output for prediction adjustments.

[0119] In this example, after receiving the incoming triangular mesh map, the map file is analyzed to extract the height difference layer, roughness layer, steepness layer, edge layer, and dilation layer. Each layer is loaded and displayed on RViz. Based on the dilation layer of the mesh map, the safety distance to the robot is calculated to ensure compliance. After analyzing the mesh map file, the 3D navigation algorithm plans a path to a given target point using either Dijkstra's path planning algorithm or the CVP path planning algorithm. Dijkstra's path planning algorithm plans based on the edges of the mesh map, propagating backward from the target point's pose to calculate all reachable vertices between the starting point and the target point. However, Dijkstra's algorithm generates a secondary dominance field during path planning, which is highly dependent on the structure of the incoming mesh map. The CVP path planning algorithm can plan paths above curved surfaces and is not limited by the mesh's edges and topology, thus generating relatively shorter paths. The computer controller acquires the GPS coordinates of the target point from an external source and performs UTM distance calculations with the embedded regional GPS information in the grid map. It then publishes the obtained relative pose information and, combined with the analysis of the grid map using a 3D navigation algorithm, plans a path to the target point using two path planning algorithms. This constructs a path with a shorter relative distance to the target point and a more compliant safety distance. The controller also publishes real-time motion plans to the robot. The robot transmits linear and angular velocity data to the chassis via a CAN card, which is then converted into motor speeds. Ultimately, this enables the robot to autonomously navigate and move to the target point, performing tasks such as inspection, plant protection, harvesting, and transportation.

[0120] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A three-dimensional navigation method for an orchard operation robot based on a grid map, characterized in that, Includes the following steps: S1. Obtain the three-dimensional point cloud data of the orchard and the angular velocity and acceleration of the robot through the operation robot, and generate the orchard point cloud map based on the FAST-LIO2 mapping algorithm or LIO-SAM mapping algorithm. S2. The obtained 3D point cloud map is filtered by radius filtering algorithm and statistical filtering algorithm. Based on the filtered 3D point cloud map, an orchard triangular grid map is constructed. The GPS information of the orchard is embedded into the triangular grid map, and a triangular grid map with GPS information is output. S3. Based on the triangular mesh map with GPS information, the point cloud data scanned by the LiDAR and the points of the triangular mesh map are registered by ICP based on the RMCL three-dimensional positioning algorithm. The three-dimensional pose of the robot in the orchard is output. The three-dimensional pose of the robot in the orchard is updated by combining the GNSS information of the robot. The navigation path of the robot is planned by Mesh_Navigation according to the GPS information of the target point. The robot is controlled to dynamically avoid obstacles and navigate to the target point.

2. The three-dimensional navigation method for orchard operation robots based on grid maps according to claim 1, characterized in that, The operation robot includes: a tracked chassis, a lidar, an IMU inertial sensor, a GNSS receiver, a computer, and a router. The lidar, IMU inertial sensor, GNSS receiver, computer, and router are all mounted on the tracked chassis. The lidar scans the environment around the operation robot in real time to generate three-dimensional point cloud data of the orchard. The IMU inertial sensor acquires the angular velocity and acceleration of each axis during the operation of the operation robot, and the GNSS receiver collects the GNSS signals of the operation robot.

3. The three-dimensional navigation method for orchard operation robots based on grid maps according to claim 2, characterized in that, The process of generating orchard point cloud maps based on the FAST-LIO2 or LIO-SAM mapping algorithm includes: selecting either the FAST-LIO2 or LIO-SAM mapping algorithm according to the size of the orchard; using the computer controller to calculate the relative motion changes between adjacent lidar frames through pre-integration based on the acceleration and angular velocity data of the robot collected by the IMU inertial sensor; predicting the current pose state based on the state of the previous moment; using the matching residual between the lidar point cloud and the local map as the observation value; optimizing the predicted current pose state using iterative Kalman filtering; and outputting the final optimized pose estimate through joint prediction and updating. The generated orchard point cloud map is then displayed in the computer controller based on the optimized pose estimate.

4. The three-dimensional navigation method for orchard operation robots based on grid maps according to claim 1, characterized in that, Step S2 includes: S21. The obtained orchard point cloud map is filtered using the radius filtering algorithm and statistical filtering algorithm based on the PCL library to obtain the filtered orchard point cloud map. S22. Based on the filtered point cloud map of the three orchards, a triangular mesh map of the orchards is constructed using the greedy triangulation method of the PCL library. By projecting the three-dimensional point cloud onto a two-dimensional plane, using two-dimensional Delaunay triangulation, and then mapping the result back to three-dimensional space, a mesh map is generated by triangulating the point cloud through greedy triangulation projection. S23. Based on the orchard's terrain information, embed GPS information into the orchard's triangular grid map.

5. A three-dimensional navigation method for an orchard operation robot based on a grid map according to claim 4, characterized in that, Step S22 includes: The legality of the input point cloud map of the three orchards is verified, and the number of points contained in the point cloud map is calculated. For point cloud maps with more than four million points, voxel filtering is performed on the points to reduce the point cloud sampling for SLAM mapping. Radius filtering is applied to the filtered point cloud to remove discrete noise points caused by environmental influences, and statistical filtering is used to remove the influence of off-cluster noise in the point cloud map. The filtered point cloud is smoothed using the least squares method to remove the high-frequency vibration effects caused by sensor noise in the LiDAR, while preserving the planar and curved features of the ground and walls. The point cloud normal vectors are calculated using GPU acceleration, and a gridded map is generated by triangulating the point cloud through greedy triangulation.

6. A three-dimensional navigation method for an orchard operation robot based on a grid map according to claim 4, characterized in that, Step S3 includes: S31. Based on the RMCL three-dimensional positioning algorithm, the point cloud data collected by the LiDAR in real time is registered with the nearest distance of the points on the triangular mesh surface. The nearest point of intersection between each triangular mesh surface and the laser ray is taken as the predicted pose estimation point, and the three-dimensional pose of the robot at the predicted pose estimation point is output. S32. In areas with stable GNSS, the robot corrects the accumulated positioning error due to the lack of GPS positioning in real time by acquiring the obtained GNSS information. At the same time, it uses a Kalman filter to predict and update the positioning error in GNSS-rejected areas, thereby updating the three-dimensional pose of the robot in the orchard. S33. Using the triangular mesh map as the global map, the Dijkstra algorithm and CVP algorithm are used to plan the navigation path of the robot based on the GPS information of the target point, and the robot is controlled to dynamically avoid obstacles and navigate to the target point.

7. A three-dimensional navigation method for an orchard operation robot based on a grid map according to claim 6, characterized in that, Step S31 includes: Based on the NVIDIA OptiX accelerated ray tracing API, Intel Embree ray tracing library, and Rmagine computation library, the RMCL 3D localization algorithm is used to perform ICP registration between the point cloud data scanned by the LiDAR and the points on the triangular mesh map. The NVIDIA OptiX accelerated ray tracing API is used to track the laser beam emitted by the LiDAR, and the shortest distance between the laser ray and the mesh map is calculated to achieve 3D localization of the robot. The Intel Embree ray tracing library is used to track and calculate the laser beam emitted by the LiDAR, and the result is fed back to the robot to obtain its 3D localization. The Rmagine computation library is used to track the laser beam emitted by the LiDAR and simulate a distance sensor, calculating the distance relationship between the laser beam and the sensor, and transmitting the calculated data to the computer control unit. Based on the calculated data, the computer control unit uses the closest intersection point between each triangular mesh face and the laser ray as the estimated position point for the robot's predicted pose.

8. A three-dimensional navigation method for an orchard operation robot based on a grid map according to claim 6, characterized in that, Step S32 includes: The robot's current position coordinates are calculated by the GNSS receiver based on the received real-time GPS information. The difference between the robot's current position coordinates and the position coordinates of the embedded GPS information read from the map positioning is used as the positioning error. The existing positioning path is corrected with the positioning error as the confidence level. The GNSS positioning is updated by directly registering with the current frame and historical frames of the grid map through ICP registration, thus correcting the positioning error accumulated without GPS positioning.

9. A three-dimensional navigation method for an orchard operation robot based on a grid map according to claim 6, characterized in that, Step S33 includes: The difference between the received GPS information of the target point and the GPS information of the grid to which the robot is located is calculated. Based on Mesh Navigation, the difference is published to the MoveBase navigation stack node in the form of a topic message. Static obstacle information obtained by the grid map and dynamic obstacle information obtained by LiDAR scanning are combined to construct a suitable planned path for the robot to reach the target point based on the Dijkstra algorithm and the CVP algorithm.

Images