Orchard navigation method, device, equipment and medium based on multi-sensor fusion

Abstract

The application provides an orchard navigation method, device, equipment and medium based on multi-sensor fusion, which comprises the following steps: obtaining three-dimensional point cloud data by scanning an orchard; constructing a three-dimensional point cloud map of the orchard based on the three-dimensional point cloud data; generating a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, wherein the two-dimensional grid map comprises a plurality of grids with the same size; determining and marking the position of each fruit tree in the two-dimensional grid map based on the number of points in the three-dimensional point cloud map that are vertically projected in each grid of the two-dimensional grid map, and generating a two-dimensional grid map of the orchard; and performing positioning navigation based on the two-dimensional grid map of the orchard. The method is used to solve the problem that the existing orchard navigation method lacks the ability to match the needs of orchard operation, and can make the orchard operation equipment walk autonomously and complete the operation task. In combination with the intelligent operation tools of the orchard, the unmanned management of the orchard is realized.

Description

Technical Field

[0001] This invention relates to the field of positioning and navigation technology, and in particular to an orchard navigation method based on multi-sensor fusion. Background Technology

[0002] The fruit industry is labor-intensive and highly seasonal, with labor costs accounting for a significant portion of orchard production costs. To reduce labor costs and minimize safety risks in orchard production, orchard operation equipment navigation technology has been developed. This technology enables unmanned orchard management through map building, positioning, navigation point coordinate calculation, and path planning. In recent years, researchers both domestically and internationally have developed several orchard navigation methods, but most are suitable for relatively simple orchard environments and lack adaptability to complex orchard scenarios. Current orchard navigation methods primarily extract navigation paths in real time without constructing high-precision orchard maps, making it difficult to address the environmental changes caused by the growth of fruit trees. Summary of the Invention

[0003] This invention provides an orchard navigation method based on multi-sensor fusion to solve the problem of the lack of navigation methods in the prior art that can match the needs of orchard operations, and to achieve high-precision positioning that can adapt to various orchard environments.

[0004] This invention provides an orchard navigation method based on multi-sensor fusion, the method comprising:

[0005] Obtain 3D point cloud data from a scanned orchard;

[0006] Constructing a 3D point cloud map of the orchard based on 3D point cloud data;

[0007] A blank two-dimensional raster map is generated from the plane of the three-dimensional point cloud map. The two-dimensional raster map contains several rasters of the same size.

[0008] Based on the number of points vertically projected from the 3D point cloud map into each grid of the 2D raster map, the position of each fruit tree in the orchard in the 2D raster map is determined and marked, and a 2D raster map of the orchard is generated.

[0009] Location and navigation are based on the orchard's 3D point cloud map and 2D raster map.

[0010] According to the orchard navigation method based on multi-sensor fusion provided by the present invention, a three-dimensional point cloud map of the orchard is constructed based on three-dimensional point cloud data, including:

[0011] Interpolation processing is performed on point cloud frames in 3D point cloud data to obtain a point cloud depth map generated based on the point cloud frames;

[0012] For each frame of point cloud depth map, calculate the smoothness of each point in the point cloud depth map and sort the points in the point cloud depth map according to the smoothness. Extract the corner feature points and surface feature points in each frame of point cloud depth map.

[0013] By stitching together the point cloud frames based on the corner feature points and surface feature points in each frame of the point cloud depth map, a three-dimensional point cloud map of the orchard is obtained.

[0014] According to the orchard navigation method based on multi-sensor fusion provided by the present invention, a blank two-dimensional grid map is generated from the plane of a three-dimensional point cloud map. The two-dimensional grid map contains several grids of the same size, including:

[0015] Traverse each point in the 3D point cloud map and determine the planar dimensions of the 3D point cloud map based on the coordinate values ​​of each point;

[0016] Obtain the diameter data of the tree trunks in the orchard, and determine the grid size by combining the trunk diameter data with the planar dimensions of the 3D point cloud map. The size of the grid shall not exceed the cross-section of the tree trunk.

[0017] The plane of the 3D point cloud map is divided according to the grid size, and a blank 2D raster map is generated.

[0018] According to the orchard navigation method based on multi-sensor fusion provided by the present invention, the method determines and marks the position of each fruit tree in the orchard in the two-dimensional grid map based on the number of points in the three-dimensional point cloud map vertically projected onto each grid of the two-dimensional grid map, thereby generating a two-dimensional grid map of the orchard. The method includes: dividing the space of the three-dimensional point cloud map into several subspaces of the same size based on the two-dimensional grid map, and calculating the number of points contained in each subspace, wherein each grid in the two-dimensional grid map corresponds one-to-one with each subspace in the three-dimensional point cloud map.

[0019] Based on the number of points contained in each subspace of the 3D point cloud map, the position of each fruit tree in the orchard in the 2D grid map is determined and marked, thus generating a 2D grid map of the orchard.

[0020] The orchard navigation method based on multi-sensor fusion provided by the present invention determines and marks the position of each fruit tree in the orchard in a two-dimensional grid map according to the number of points contained in each subspace of the three-dimensional point cloud map, and generates a two-dimensional grid map of the orchard, including:

[0021] Obtain data on the height of fruit tree trunks in the orchard;

[0022] Determine the range of fruit tree trunk height based on the height data of the fruit tree trunk;

[0023] For each subspace, if the number of points within the height range in the subspace exceeds the preset number, it is determined that there are fruit trees in the subspace, and the corresponding grid in the two-dimensional grid map of the subspace is marked as occupied.

[0024] The grates marked as occupied in the two-dimensional raster map are identified as the locations of fruit trees, thus generating a two-dimensional raster map of the orchard.

[0025] The orchard navigation method based on multi-sensor fusion provided by the present invention performs positioning and navigation based on a three-dimensional point cloud map and a two-dimensional grid map of the orchard, including:

[0026] Obtain the orchard work tasks to be performed by the device to be navigated;

[0027] The positioning pose of the navigation device is obtained by locating the device based on a 3D point cloud map.

[0028] The navigation point of the device to be navigated when performing orchard tasks is determined based on the orchard work tasks and the two-dimensional grid map.

[0029] Navigation path planning is based on positioning pose, navigation points, and a 2D grid map;

[0030] When the positioning pose indicates that the location of the device to be navigated is at the location of the navigation point, repeat the above steps of determining the navigation point and planning the navigation path until the orchard work task is completed.

[0031] The orchard navigation method based on multi-sensor fusion provided by the present invention locates the navigation device based on a three-dimensional point cloud map to obtain the positioning pose of the navigation device, including:

[0032] Acquire RTK and IMU data of the device to be navigated;

[0033] RTK data is converted into odometer data, and the positioning of the device to be navigated is obtained based on the odometer data, IMU data, 3D point cloud data and 3D point cloud map.

[0034] The orchard navigation method based on multi-sensor fusion provided by the present invention further includes:

[0035] The area occupied by the canopy of fruit trees around the navigation path is determined based on the navigation path and the 3D point cloud map;

[0036] Determine whether the device will scratch when it travels along the navigation path based on the area occupied by the fruit tree canopy and the position of the device to be navigated.

[0037] If so, the navigation path will be adjusted according to the position of the device to be navigated and the area occupied by the fruit tree canopy, so as to avoid the device to be navigated scraping against the fruit tree canopy when passing through the navigation path.

[0038] The present invention also provides an orchard navigation device based on multi-sensor fusion, comprising:

[0039] The data acquisition module is used to acquire 3D point cloud data obtained from scanning the orchard;

[0040] The 3D map building module is used to build a 3D point cloud map of the orchard based on 3D point cloud data.

[0041] The 2D map building module is used to generate a blank 2D raster map from the plane of the 3D point cloud map. The 2D raster map contains several rasters of the same size.

[0042] The 2D map building module is also used to determine and mark the position of each fruit tree in the orchard in the 2D grid map based on the number of points vertically projected from the 3D point cloud map into each grid of the 2D grid map, and generate a 2D grid map of the orchard.

[0043] The positioning and navigation module is used for positioning and navigation based on the orchard's 3D point cloud map and 2D raster map.

[0044] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the three-dimensional map construction module is specifically used for:

[0045] Interpolation processing is performed on point cloud frames in 3D point cloud data to obtain a point cloud depth map generated based on the point cloud frames;

[0046] For each frame of point cloud depth map, calculate the smoothness of each point in the point cloud depth map and sort the points in the point cloud depth map according to the smoothness. Extract the corner feature points and surface feature points in each frame of point cloud depth map.

[0047] By stitching together the point cloud frames based on the corner feature points and surface feature points in each frame of the point cloud depth map, a three-dimensional point cloud map of the orchard is obtained.

[0048] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the two-dimensional map construction module is specifically used for:

[0049] Traverse each point in the 3D point cloud map and determine the planar size of the 3D point cloud map based on the coordinate value of each point; obtain the diameter data of the trunks of fruit trees in the orchard, and determine the grid size by combining the trunk diameter data with the planar size of the 3D point cloud map, wherein the size of the grid does not exceed the cross-section of the fruit tree trunk.

[0050] The plane of the 3D point cloud map is divided according to the grid size, and a blank 2D raster map is generated.

[0051] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the two-dimensional map construction module is further used for:

[0052] Based on the two-dimensional raster map, the space of the three-dimensional point cloud map is divided into several subspaces of the same size, and the number of points contained in each subspace is calculated. Each grid in the two-dimensional raster map corresponds one-to-one with each subspace in the three-dimensional point cloud map.

[0053] Based on the number of points contained in each subspace of the 3D point cloud map, the position of each fruit tree in the orchard in the 2D grid map is determined and marked, thus generating a 2D grid map of the orchard.

[0054] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the two-dimensional map construction module is further used for:

[0055] Obtain data on the height of fruit tree trunks in the orchard;

[0056] Determine the range of fruit tree trunk height based on the height data of the fruit tree trunk;

[0057] For each subspace, if the number of points within the height range in the subspace exceeds the preset number, it is determined that there are fruit trees in the subspace, and the corresponding grid in the two-dimensional grid map of the subspace is marked as occupied.

[0058] The grates marked as occupied in the two-dimensional raster map are identified as the locations of fruit trees, thus generating a two-dimensional raster map of the orchard.

[0059] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the positioning and navigation module is specifically used for:

[0060] Obtain the orchard work tasks to be performed by the device to be navigated;

[0061] The positioning pose of the navigation device is obtained by locating the device based on a 3D point cloud map.

[0062] The navigation points for performing orchard tasks are determined based on the orchard work tasks and the two-dimensional grid map.

[0063] Navigation path planning is based on positioning pose, navigation points, and a 2D grid map;

[0064] When the positioning pose indicates that the location of the device to be navigated is at the location of the navigation point, repeat the above steps of determining the navigation point and planning the navigation path until the orchard work task is completed.

[0065] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the positioning and navigation module is further used for:

[0066] Acquire RTK and IMU data of the device to be navigated;

[0067] RTK data is converted into odometer data, and the positioning of the device to be navigated is obtained based on the odometer data, IMU data, 3D point cloud data and 3D point cloud map.

[0068] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the positioning and navigation module is further used for:

[0069] The area occupied by the canopy of fruit trees around the navigation path is determined based on the navigation path and the 3D point cloud map;

[0070] Determine whether the device will scratch when it travels along the navigation path based on the area occupied by the fruit tree canopy and the position of the device to be navigated.

[0071] If so, the navigation path will be adjusted according to the position of the device to be navigated and the area occupied by the fruit tree canopy, so as to avoid the device to be navigated scraping against the fruit tree canopy when passing through the navigation path.

[0072] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the orchard navigation method based on multi-sensor fusion as described above.

[0073] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the orchard navigation method based on multi-sensor fusion as described above.

[0074] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements any of the above-described orchard navigation methods based on multi-sensor fusion.

[0075] This invention provides an orchard navigation method based on multi-sensor fusion. It constructs a 3D point cloud map of the orchard by scanning it, then generates a blank 2D grid map based on the plane of the 3D point cloud map. By vertically projecting the points from the 3D point cloud map onto each grid cell of the 2D grid map, the position of each fruit tree in the orchard is determined and marked, generating a 2D grid map of the orchard. This method extracts the tree trunk position information from the 3D point cloud map to construct the 2D grid map, and finally performs positioning and navigation based on both the 3D point cloud map and the 2D grid map. Compared with existing technologies, this method, by constructing a 3D point cloud map in real time and converting it into a 2D grid map marking the positions of fruit trees in the orchard, allows for the positioning of work equipment based on the 3D point cloud map. Then, by controlling the work equipment to perform fixed-point operations at designated fruit tree locations based on the tree trunk positions in the 2D grid map, it can adapt to navigation tasks in complex orchard environments, support the development of smart agriculture, and achieve unmanned orchard management. Attached Figure Description

[0076] To more clearly illustrate the technical solutions in this 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 some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0077] Figure 1 This is a flowchart illustrating the orchard navigation method based on multi-sensor fusion provided by the present invention.

[0078] Figure 2 This is a schematic diagram of a scene where the tree trunk position information is extracted from the three-dimensional point cloud map provided by the present invention to obtain a two-dimensional grid map.

[0079] Figure 3 This is a flowchart of the localization algorithm that integrates three sources of data provided by the present invention.

[0080] Figure 4 This is a flowchart of the random particle set motion measurement fusion iterative localization algorithm that integrates three types of data, provided by the present invention.

[0081] Figure 5 This is a schematic diagram of a scenario for navigation point calculation provided by the present invention.

[0082] Figure 6 This is a flowchart of the multi-point navigation algorithm provided by the present invention.

[0083] Figure 7 This is a schematic diagram of an orchard navigation system provided by the present invention.

[0084] Figure 8 This is a schematic diagram of another orchard navigation system provided by the present invention.

[0085] Figure 9 This is a schematic diagram of the orchard navigation device based on multi-sensor fusion provided by the present invention.

[0086] Figure 10 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0087] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention 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 invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0088] The following is combined with Figure 1-10 This invention describes the orchard navigation method, apparatus, equipment, and medium based on multi-sensor fusion.

[0089] The orchard navigation method based on multi-sensor fusion mainly uses hardware such as industrial control computer, 3D LiDAR, inertial measurement unit (IMU), and real-time kinematic (RTK) equipment.

[0090] Figure 1 This is a flowchart illustrating the orchard navigation method based on multi-sensor fusion provided by the present invention, as shown below. Figure 1 As shown, the method includes the following:

[0091] Step 101: Obtain 3D point cloud data from the scanned orchard.

[0092] Step 102: Construct a 3D point cloud map of the orchard based on the 3D point cloud data.

[0093] In some embodiments, constructing a 3D point cloud map of an orchard based on 3D point cloud data includes: interpolating point cloud frames in the 3D point cloud data to obtain a point cloud depth map generated based on the point cloud frames; calculating the smoothness of each point in each frame of the point cloud depth map and sorting the points in the point cloud depth map according to the smoothness, and extracting corner feature points and surface feature points from each frame of the point cloud depth map; and stitching the point cloud frames according to the corner feature points and surface feature points from each frame of the point cloud depth map to obtain a 3D point cloud map of the orchard. Specifically, based on the Simultaneous Localization and Mapping (SLAM) method, the inertial measurement unit data is first integrated to obtain the velocity and pose changes between two time points, realizing the odometry function in mapping. Secondly, the lidar point cloud data is processed, and the motion changes of each lidar frame from the start of scanning to the start of the next lidar frame are obtained by integrating the rotation angles around the three coordinate axes in the 3D coordinate system. By combining the timestamps of data points in the point cloud frame with proportional interpolation, motion distortion correction of the point cloud is achieved, resulting in a depth map based on the 3D point cloud data. Then, according to the formula ( The smoothness of the depth map is calculated and sorted to obtain corner feature points and face feature points, thus realizing feature extraction of points in the depth map. Here, C represents the smoothness of the data points. This represents the depth value of the data point. This represents the depth value of adjacent data points.

[0094] Finally, based on the feature points in the point cloud frames, a tree-like data structure (K-dimensional tree, KD-Tree) is established for the current frame and historical frames. The K nearest neighbors are searched, and if a match is found, the new point cloud frame is stitched into the historical point cloud, gradually constructing a 3D point cloud map of the orchard, which is then saved as a PCD (Point Cloud Data) file. During point cloud frame matching, corner and surface feature points extracted from the corresponding point cloud depth map are used for matching. If the current point cloud frame's depth map and the historical point cloud frame's depth map have the same features or a similarity higher than the similarity threshold, then the current point cloud frame is considered a match with the corresponding historical point cloud frame. The current point cloud frame is then stitched together with the corresponding historical point cloud frame, gradually constructing a 3D point cloud map of the orchard.

[0095] In this embodiment of the disclosure, due to the diverse types of orchard environments, the limitations of using single-line lidar detection are significant. Therefore, in order to improve the robustness of orchard navigation, the perception of orchard environmental information is added during map construction and positioning. This invention adopts a deep fusion method of multi-line lidar and inertial measurement unit data, and uses a SLAM-based map construction method to construct a three-dimensional point cloud map of the orchard.

[0096] Step 103: Generate a blank two-dimensional raster map based on the plane of the three-dimensional point cloud map. The two-dimensional raster map contains several rasters of the same size.

[0097] For example, the size of the horizontal cross-section of the 3D point cloud map can be set to the size of the 2D raster map to generate a blank 2D map, and then several rasters of the same size can be divided in the blank 2D map.

[0098] In some embodiments, a blank two-dimensional raster map is generated based on the plane of a three-dimensional point cloud map. The two-dimensional raster map contains several grates of the same size. The process includes: traversing each point in the three-dimensional point cloud map and determining the planar size of the three-dimensional point cloud map based on the coordinate values ​​of each point; obtaining the diameter data of the trunks of fruit trees in the orchard and determining the raster size by combining the trunk diameter data with the planar size of the three-dimensional point cloud map, wherein the size of the raster does not exceed the cross-section of the fruit tree trunk; dividing the plane of the three-dimensional point cloud map according to the raster size to generate a blank two-dimensional raster map.

[0099] The entire 3D point cloud map is processed. The coordinates of points in the 3D point cloud are used... This means that by traversing each point in the point cloud, determining the maximum and minimum values ​​of the point's coordinates on the x-axis and y-axis, the dimensions of the horizontal cross-section of the orchard's 3D point cloud map are calculated based on the maximum and minimum values ​​of the point cloud coordinates on the x-axis and y-axis.

[0100] By combining the diameter data of the orchard tree trunks and the planar dimensions of the 3D point cloud map, the resolution of the 2D raster map is set, ensuring that the raster size of the 2D map is no larger than the cross-section of the tree trunk, according to the formula. , , An initial blank two-dimensional raster map is obtained. Among them, The maximum value of the point cloud coordinates on the x-axis. The minimum value of the point cloud coordinates on the x-axis. The maximum value of the point cloud coordinates on the y-axis. R represents the minimum point cloud coordinates on the y-axis, and R represents the diameter of the fruit tree trunk. and These represent the number of grid cells along the X-axis and Y-axis of the map coordinate system, respectively. It is the total number of grid cells contained in the two-dimensional map of the orchard.

[0101] Step 104: Based on the number of points vertically projected from the 3D point cloud map into each grid of the 2D raster map, determine and mark the position of each fruit tree in the orchard in the 2D raster map, and generate a 2D raster map of the orchard.

[0102] Understandably, in a 3D point cloud map, the distribution of points generated by laser scanning of fruit tree trunks is relatively densest within the orchard. Therefore, using a blank 2D raster map as the base of the 3D point cloud map, when points from the 3D point cloud map are vertically projected onto the 2D raster map, the points from the scanned fruit tree trunks will be densely distributed on the 2D raster map. Thus, the position of the fruit tree trunk in the 2D raster map can be determined based on the number of points in each grid cell of the 3D point cloud vertically projected onto the 2D raster map.

[0103] In some embodiments, the location of each fruit tree in the orchard in the two-dimensional grid map is determined and marked based on the number of points in each grid cell of the two-dimensional grid map vertically projected from points in the three-dimensional point cloud map, thereby generating a two-dimensional grid map of the orchard. This includes: dividing the space of the three-dimensional point cloud map into several subspaces of the same size based on the two-dimensional grid map, and calculating the number of points contained in each subspace, wherein each grid cell in the two-dimensional grid map corresponds one-to-one with each subspace in the three-dimensional point cloud map; determining and marking the location of each fruit tree in the orchard in the two-dimensional grid map based on the number of points contained in each subspace of the three-dimensional point cloud map, thereby generating a two-dimensional grid map of the orchard.

[0104] like Figure 2 As shown in step (1), the orchard's 3D point cloud is divided into sections using the grid of the 2D raster map as the base. Figure 2 The step-by-step (2) diagram shows the Q solid subspaces, where, It is the first The height of each point; It is the first points Coordinate values.

[0105] Then, the coordinates of each point are traversed, and the number of points contained in each 3D subspace is calculated. The number of points contained in each 3D subspace is the number of points vertically projected from the 3D point cloud in the corresponding 2D grid of that 3D subspace. Based on the number of points contained in each 3D subspace, it can be determined whether there is a fruit tree trunk in its corresponding 2D grid, thus marking the position of each fruit tree in the orchard in the 2D grid map, generating a 2D grid map of the orchard.

[0106] In some embodiments, determining and marking the position of each fruit tree in the orchard in a two-dimensional grid map based on the number of points contained in each subspace of the three-dimensional point cloud map, and generating a two-dimensional grid map of the orchard, includes: acquiring the height data of the fruit tree trunks in the orchard; determining the height range of the fruit tree trunks based on the height data of the fruit tree trunks; for each subspace, if the number of points within the height range in the subspace exceeds a preset number, determining that there are fruit trees in the subspace, and marking the corresponding grid in the two-dimensional grid map of the subspace as occupied; determining the grid marked as occupied in the two-dimensional grid map as the location of the fruit tree, and generating a two-dimensional grid map of the orchard.

[0107] In practice, based on the x-axis and y-axis coordinates of points in the 3D point cloud, all points are divided into a 3D space with each grid cell as its base. Points in each 3D space are then sorted in ascending order of their z-axis coordinates, with the smallest z-value serving as the height of the "initial plane" of that 3D space. According to the formula Define the "height" of a point in a point cloud map. The coordinates of the point Relative to the grid The difference. Based on the characteristics of the fruit trees and the growth of weeds in the orchard, such as... Figure 2 As shown in step (3), set the trunk height range ( , This filters out the occlusion of the tree trunk by the canopy and weeds, avoiding interference from the point cloud generated by laser scanning of the canopy and weeds in locating the tree trunk. Then, it calculates whether the height of each point in the 3D space conforms to the range of tree trunk height (…). , The number of points, such as Figure 2 As shown in step (4), if the number of points exceeds a set threshold, the grid marked as the bottom of the three-dimensional space is as follows. Figure 2 The occupancy status is shown in step (5) of the diagram. For example... Figure 2As shown in step (6), the grid coordinates of the occupied state are the positions of the fruit trees on the two-dimensional grid map of the orchard. After traversing all three-dimensional spaces, a two-dimensional grid map is obtained, realizing the conversion of the three-dimensional point cloud map with extracted tree trunk position information into a two-dimensional grid map.

[0108] This embodiment of the disclosure determines the height range of the fruit tree trunk by using the height data of the fruit tree trunk, and then determines whether there is a fruit tree trunk in each subspace by counting the number of points within the height range of the trunk. It further marks the grid position of the fruit tree trunk in the two-dimensional grid map, thereby avoiding the interference of the point cloud generated by laser scanning of the tree crown and weeds on the location of the tree trunk, making the position of the tree trunk in the generated two-dimensional grid map more accurate.

[0109] In this embodiment, due to the complex and undulating road conditions in the orchard, achieving high-precision row centerline navigation and autonomous lane changing requires combining environmental detection information and motion information to improve the accuracy and stability of map construction. Therefore, this embodiment designs a two-dimensional raster map construction method that extracts the position information of all fruit tree trunks from a three-dimensional point cloud map of the orchard. This allows for the automatic calculation of navigation point coordinates located on the row centerline and at turning points based on the fruit tree position information. Furthermore, based on fruit tree characteristics, orchard space, and task requirements, path planning between orchard navigation points and real-time path planning are completed on the two-dimensional map.

[0110] Step 105: Perform positioning and navigation based on the orchard's 3D point cloud map and 2D raster map.

[0111] For example, in this embodiment of the disclosure, the target device is located on a three-dimensional map, and then the location result of the target device is projected onto a two-dimensional map. Based on the real-time location of the target device on the two-dimensional map, path calculation and motion control are performed on the target device to achieve navigation of the target device.

[0112] In some embodiments, positioning and navigation based on a 3D point cloud map and a 2D grid map of an orchard includes: acquiring the orchard work task performed by the device to be navigated; locating the device to be navigated based on the 3D point cloud map to obtain the positioning pose of the device to be navigated; determining the navigation point of the device to be navigated while performing the orchard work task according to the orchard work task and the 2D grid map; planning a navigation path based on the positioning pose, the navigation point, and the 2D grid map; and repeating the steps of determining the navigation point and planning the navigation path when the positioning pose indicates that the position of the device to be navigated is at the location of the navigation point, until the orchard work task is completed. Specifically, locating the device to be navigated based on the 3D point cloud map to obtain the positioning pose of the device to be navigated includes: acquiring RTK data and IMU data of the device to be navigated; converting the RTK data into odometer data, and locating the device to be navigated based on the odometer data, IMU data, 3D point cloud data, and the 3D point cloud map to obtain the positioning pose of the device to be navigated.

[0113] Figure 3 This is a flowchart of the localization algorithm that fuses three data sources provided by this invention. Its core is a random particle set motion measurement fusion iterative localization algorithm, where the three data sources are lidar measurement results, inertial measurement unit (IMU) data, and odometry data converted from RTK data. Figure 3 As shown, the 3D point cloud data, IMU data, and odometry data are first processed to obtain filtered point cloud data, attitude information, and position information, respectively, and an initial particle set is randomly generated. Next, motion information from the odometry data is used to predict the particles. Then, resampling is performed to add more random particles to avoid the situation where global error localization cannot be recovered. Subsequently, it is determined whether the set convergence condition has been met. If yes, the process pose is output. If not, the loop continues, and the particle set is iterated. Measurement updates are performed using LiDAR data, attitude correction is performed using IMU data, and particle weights are calculated. Motion information from the odometry data is then used to predict the particles again. Finally, resampling is performed to add more random particles, and the loop continues until the convergence condition is met. Figure 4 This is a flowchart of the random particle set motion measurement fusion iterative localization algorithm that integrates three types of data, provided by the present invention.

[0114] In practice, the constructed orchard 3D point cloud map (PCD) file is converted into a readable map and published to the random particle set motion measurement fusion iterative localization algorithm that integrates three types of data. During localization, RTK data is converted into odometry data; then, the original LiDAR data, original IMU data, and odometry data are converted into readable data and sent to the random particle set motion measurement fusion iterative localization algorithm. The random particle set motion measurement fusion iterative localization algorithm calculates the process pose based on the initial pose, 3D point cloud map, LiDAR data, IMU data, and odometry data, and combines the odometry data to filter the pose to obtain the final localization result.

[0115] Figure 5 This is a schematic diagram of a scenario for navigation point calculation provided by the present invention.

[0116] like Figure 5 As shown, during navigation, the coordinates of the navigation point are first calculated based on the orchard structure and task requirements. Figure 5 The system uses navigation points within the orchard and, based on the output of the positioning module, determines the completion status of each current navigation point and sequentially releases new navigation points, enabling centerline navigation between rows and autonomous turning at the edge of the field. Navigation points in the orchard are mainly of two types: those between rows and those at the edge of the field. Navigation points between rows are set at the midpoint of the line connecting the trunks on both sides of the row. Navigation points at the edge of the field are set at 1 / 2 of the space L on the extended centerline of the row transition area.

[0117] Figure 6 This is a flowchart of the multi-point navigation algorithm provided by the present invention, such as... Figure 6 As shown, the method includes:

[0118] The tree trunk location information on the orchard 2D raster map obtained from the 2D map building module is read, and the navigation point coordinates are calculated.

[0119] The navigation point located on the center line is calculated to achieve autonomous navigation on the fixed center line.

[0120] The system calculates the coordinates of the navigation point at the edge of the orchard, enabling autonomous turning at the orchard's edge. These navigation point coordinates are input to a multi-point navigation algorithm, which combines them with real-time positioning results to determine the arrival status of the current navigation point. Upon arrival at the current navigation point, the algorithm publishes the coordinates of the next navigation point to the path planning module, thus sequentially publishing navigation points.

[0121] In some embodiments, the method further includes: determining the area occupied by the canopy of fruit trees around the navigation path based on the navigation path and the three-dimensional point cloud map; determining whether the device to be navigated will scrape against the navigation path when it passes through the navigation path based on the area occupied by the canopy of the fruit trees and the pose of the device to be navigated; if so, adjusting the navigation path according to the pose of the device to be navigated and the area occupied by the canopy of the fruit trees to avoid the device to be navigated scraping against the canopy of the fruit trees when it passes through the navigation path. Figure 7This is a flowchart illustrating the orchard navigation method based on multi-sensor fusion provided by the present invention, and a schematic diagram illustrating an orchard navigation system provided by the present invention.

[0122] In specific implementation, such as Figure 7 As shown, Dijkstra's algorithm is used to plan navigation paths between navigation points. Combined with the calculation and distribution of navigation point coordinates, segmented path planning is achieved in orchards. Row centerlines conforming to the tree row direction are obtained between rows, and turning paths located in the center of the field edge are obtained at the field edge. This method can be applied in standard orchards and can also handle curved tree rows in non-standard orchards. Path planning between adjacent navigation points is performed only once.

[0123] The Dynamic Window Approach (DWA) algorithm is used to perform real-time path planning based on the positioning results and the relative positions of the paths between navigation points, combined with real-time detected canopy point cloud information. During navigation, the canopy height range is set according to the characteristics of the fruit trees and the height required for the passage of orchard operation equipment, and the canopy point cloud is extracted from the point cloud collected by radar in real time. The relative position of the canopy point cloud relative to the operation equipment is obtained. The positioning module outputs the relative pose of the operation equipment relative to the origin of the 2D map. Combined with the relative position of the canopy point cloud relative to the operation equipment in real time, the relative position of the canopy point cloud relative to the origin of the 2D map is obtained, realizing the projection of the canopy point cloud onto the 2D map. The orchard 2D map with added canopy information is used for real-time path optimization by the DWA algorithm. Ultimately, this achieves the simultaneous tracking of the planned path between navigation points and optimization of the actual execution path, avoiding collisions with the canopy.

[0124] Figure 8 This is a flowchart illustrating the orchard navigation method based on multi-sensor fusion provided by the present invention. It is also a schematic diagram illustrating another orchard navigation system provided by the present invention. Figure 8 As shown, the method includes the following:

[0125] The 3D LiDAR and RTK equipment are connected to the industrial control computer via Ethernet ports, while the gyroscope communicates with the industrial control computer via a 232 serial port and is used as an inertial measurement unit.

[0126] The entire orchard is scanned using a 3D LiDAR system, which is equipped with an inertial measurement unit. A 3D map building module achieves tight coupling between the LiDAR point cloud and the inertial measurement unit data, and a 3D point cloud map of the orchard is built based on SLAM technology.

[0127] It is saved in the industrial control computer in PCD file format and published to the 2D map building module.

[0128] In practice, based on the characteristics of the fruit trees, the thickness of the trunks, and the growth of weeds, a reasonable range of trunk height and map resolution are set. The two-dimensional map building module can obtain a two-dimensional grid map of the orchard that retains the location information of the trunks, which can be used for navigation point coordinate calculation and path planning.

[0129] The calculation and publishing module calculates the coordinates of the navigation point based on the navigation task requirements and the tree trunk position information in the two-dimensional map.

[0130] Real-time point cloud data, inertial measurement unit data, and odometer data based on RTK information are used to achieve positioning in the orchard environment based on a 3D point cloud map of the orchard.

[0131] Based on the positioning results and navigation point coordinates, determine the arrival status of the current navigation point and publish the calculated navigation points sequentially.

[0132] The Dijkstra algorithm is used to plan the path between adjacent navigation points. Combined with the real-time collected fruit tree canopy point cloud information, and based on the positioning results and the path between adjacent navigation points, a real-time path is planned to complete the tracking and optimization of the navigation path, realizing orchard row centerline navigation and autonomous row changing.

[0133] In practice, the 3D map building module works simultaneously during the navigation process to update the orchard map for the next navigation task, in order to cope with the changes in the orchard environment caused by the growth of fruit trees and realize autonomous navigation of the orchard throughout the entire growth cycle.

[0134] In this embodiment, due to the complex and undulating road conditions in the orchard, high-precision row centerline navigation and autonomous lane changing require the combination of environmental detection information and motion information to improve the accuracy and stability of map construction. A two-dimensional raster map construction method is designed, extracting the position information of all fruit tree trunks from a three-dimensional point cloud map of the orchard. Based on the fruit tree position information, the coordinates of navigation points located on the row centerline and at the turning points are automatically calculated. According to the fruit tree characteristics, orchard space, and task requirements, path planning between orchard navigation points and real-time path planning are completed on the two-dimensional map. The path planning between navigation points is completed using the Dijkstra algorithm, while the real-time path planning is completed using the DWA algorithm based on tree canopy information, positioning results, and the relative positions of the paths between navigation points in the real-time point cloud data. A random particle set motion measurement fusion iterative positioning algorithm is designed, fusing three types of data: lidar measurement results, inertial measurement unit data, and RTK data, to achieve high-precision positioning adaptable to various orchard environments. High-precision overall navigation in the orchard is achieved through map construction, navigation point calculation, positioning, path planning, and sequential deployment of navigation points. During navigation, real-time data from the 3D LiDAR is used for positioning and path optimization between navigation points. Simultaneously, it is fused with real-time data from the inertial measurement unit to reconstruct and update the orchard map for subsequent orchard navigation operations, addressing environmental changes caused by tree growth. Furthermore, the technology enabling orchard work equipment to autonomously navigate and complete tasks within the orchard environment through map building, positioning, navigation point coordinate calculation, and path planning meets the requirements for point-to-point navigation for individual fruit trees.

[0135] This invention provides an orchard navigation method based on multi-sensor fusion. It constructs a 3D point cloud map of the orchard by scanning it, then generates a blank 2D grid map based on the plane of the 3D point cloud map. By vertically projecting the points from the 3D point cloud map onto each grid cell of the 2D grid map, the position of each fruit tree in the orchard is determined and marked, generating a 2D grid map of the orchard. This method extracts the tree trunk position information from the 3D point cloud map to construct the 2D grid map, and finally performs positioning and navigation based on both the 3D point cloud map and the 2D grid map. Compared with existing technologies, this method, by constructing a 3D point cloud map in real time and converting it into a 2D grid map marking the positions of fruit trees in the orchard, allows for the positioning of work equipment based on the 3D point cloud map. Then, the equipment is controlled to perform fixed-point operations at designated fruit tree locations based on the tree trunk positions in the 2D grid map. This adaptable to navigation tasks in complex orchard environments supports the development of smart agriculture and enables unmanned orchard management. Figure 9 As shown, the orchard navigation device based on multi-sensor fusion provided by this invention, and the orchard navigation device based on multi-sensor fusion described below, can be referred to in correspondence with the orchard navigation method based on multi-sensor fusion described above. It includes:

[0136] Data acquisition module 901 is used to acquire three-dimensional point cloud data obtained from scanning the orchard;

[0137] The 3D map building module 902 is used to build a 3D point cloud map of the orchard based on 3D point cloud data.

[0138] The 2D map building module 903 is used to generate a blank 2D raster map based on the plane of the 3D point cloud map. The 2D raster map contains several rasters of the same size.

[0139] The 2D map building module 903 is also used to determine and mark the position of each fruit tree in the orchard in the 2D grid map based on the number of points vertically projected from the 3D point cloud map into each grid of the 2D grid map, and generate a 2D grid map of the orchard.

[0140] The positioning and navigation module 904 is used for positioning and navigation based on a two-dimensional grid map of the orchard.

[0141] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the three-dimensional map construction module 902 is specifically used for:

[0142] Interpolation processing is performed on point cloud frames in 3D point cloud data to obtain a point cloud depth map generated based on the point cloud frames;

[0143] For each frame of point cloud depth map, calculate the smoothness of each point in the point cloud depth map and sort the points in the point cloud depth map according to the smoothness. Extract the corner feature points and surface feature points in each frame of point cloud depth map.

[0144] By stitching together the point cloud frames based on the corner feature points and surface feature points in each frame of the point cloud depth map, a three-dimensional point cloud map of the orchard is obtained.

[0145] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the two-dimensional map construction module 903 is specifically used for:

[0146] Traverse each point in the 3D point cloud map and determine the planar size of the 3D point cloud map based on the coordinate value of each point; obtain the diameter data of the trunks of fruit trees in the orchard, and determine the grid size by combining the trunk diameter data with the planar size of the 3D point cloud map, wherein the size of the grid does not exceed the cross-section of the fruit tree trunk.

[0147] The plane of the 3D point cloud map is divided according to the grid size, and a blank 2D raster map is generated.

[0148] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the two-dimensional map construction module 903 is further used for:

[0149] Based on the two-dimensional raster map, the space of the three-dimensional point cloud map is divided into several subspaces of the same size, and the number of points contained in each subspace is calculated. Each grid in the two-dimensional raster map corresponds one-to-one with each subspace in the three-dimensional point cloud map.

[0150] Based on the number of points contained in each subspace of the 3D point cloud map, the position of each fruit tree in the orchard in the 2D grid map is determined and marked, thus generating a 2D grid map of the orchard.

[0151] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the two-dimensional map construction module 903 is further used for:

[0152] Obtain data on the height of fruit tree trunks in the orchard;

[0153] Determine the range of fruit tree trunk height based on the height data of the fruit tree trunk;

[0154] For each subspace, if the number of points within the height range in the subspace exceeds the preset number, it is determined that there are fruit trees in the subspace, and the corresponding grid in the two-dimensional grid map of the subspace is marked as occupied.

[0155] The grates marked as occupied in the two-dimensional raster map are identified as the locations of fruit trees, thus generating a two-dimensional raster map of the orchard.

[0156] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the positioning and navigation module 904 is specifically used for:

[0157] Obtain the orchard work tasks to be executed and the positioning pose of the equipment to be navigated;

[0158] The navigation points for performing orchard tasks are determined based on the orchard work tasks and the two-dimensional grid map.

[0159] Navigation path planning is based on positioning pose, navigation points, and a 2D grid map;

[0160] When the positioning pose indicates that the location of the device to be navigated is at the location of the navigation point, repeat the above steps of determining the navigation point and planning the navigation path until the orchard work task is completed.

[0161] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the positioning and navigation module 904 is further used for:

[0162] Acquire RTK and IMU data of the device to be navigated;

[0163] RTK data is converted into odometer data, and the positioning of the navigation device is obtained based on the odometer data, IMU data, and 3D point cloud data.

[0164] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the navigation module 904 is further used for:

[0165] The area occupied by the canopy of fruit trees around the navigation path is determined based on the navigation path and the 3D point cloud map;

[0166] Determine whether the device will scratch when it travels along the navigation path based on the area occupied by the fruit tree canopy and the position of the device to be navigated.

[0167] If so, the navigation path will be adjusted according to the position of the device to be navigated and the area occupied by the fruit tree canopy, so as to avoid the device to be navigated scraping against the fruit tree canopy when passing through the navigation path.

[0168] like Figure 10 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 10 As shown, the electronic device may include: a processor 1010, a communications interface 1020, a memory 1030, and a communication bus 1040, wherein the processor 1010, the communications interface 1020, and the memory 1030 communicate with each other via the communication bus 1040. The processor 1010 can call logical instructions in the memory 1030 to execute an orchard navigation method based on multi-sensor fusion, the method including:

[0169] Obtain 3D point cloud data from a scanned orchard;

[0170] Constructing a 3D point cloud map of the orchard based on 3D point cloud data;

[0171] A blank two-dimensional raster map is generated from the plane of the three-dimensional point cloud map. The two-dimensional raster map contains several rasters of the same size.

[0172] Based on the number of points vertically projected from the 3D point cloud map into each grid of the 2D raster map, the position of each fruit tree in the orchard in the 2D raster map is determined and marked, and a 2D raster map of the orchard is generated.

[0173] Location and navigation are based on the orchard's 3D point cloud map and 2D raster map.

[0174] Furthermore, the logical instructions in the aforementioned memory 1030 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0175] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer is able to execute the orchard navigation method based on multi-sensor fusion provided by the above methods, the method comprising:

[0176] Obtain 3D point cloud data from a scanned orchard;

[0177] Constructing a 3D point cloud map of the orchard based on 3D point cloud data;

[0178] A blank two-dimensional raster map is generated from the plane of the three-dimensional point cloud map. The two-dimensional raster map contains several rasters of the same size.

[0179] Based on the number of points vertically projected from the 3D point cloud map into each grid of the 2D raster map, the position of each fruit tree in the orchard in the 2D raster map is determined and marked, and a 2D raster map of the orchard is generated.

[0180] Location and navigation are based on a two-dimensional grid map of the orchard.

[0181] In another aspect, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to perform the orchard navigation method based on multi-sensor fusion provided by the methods described above, the method comprising:

[0182] Obtain 3D point cloud data from a scanned orchard;

[0183] Constructing a 3D point cloud map of the orchard based on 3D point cloud data;

[0184] A blank two-dimensional raster map is generated from the plane of the three-dimensional point cloud map. The two-dimensional raster map contains several rasters of the same size.

[0185] Based on the number of points vertically projected from the 3D point cloud map into each grid of the 2D raster map, the position of each fruit tree in the orchard in the 2D raster map is determined and marked, and a 2D raster map of the orchard is generated.

[0186] Location and navigation are based on a two-dimensional grid map of the orchard.

[0187] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0188] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., including several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods of various embodiments or some parts of embodiments.

[0189] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An orchard navigation method based on multi-sensor fusion, characterized in that, The method comprises: acquiring three-dimensional point cloud data of a scanned orchard; constructing a three-dimensional point cloud map of the orchard based on the three-dimensional point cloud data; generating a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, the two-dimensional grid map containing a plurality of grids of the same size; determining and marking the position of each fruit tree in the two-dimensional grid map of the orchard based on the number of points in the three-dimensional point cloud map vertically projected in each grid of the two-dimensional grid map, and generating a two-dimensional grid map of the orchard; positioning and navigation based on the three-dimensional point cloud map and the two-dimensional grid map of the orchard; the generating of the blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, the two-dimensional grid map containing a plurality of grids of the same size, comprises: traversing each point in the three-dimensional point cloud map, determining the plane size of the three-dimensional point cloud map according to the coordinate value of each point, acquiring the diameter data of the trunk of the fruit tree in the orchard, and combining the diameter data of the trunk with the plane size of the three-dimensional point cloud map to determine the grid size, wherein the size of the grid does not exceed the cross section of the trunk of the fruit tree: Q = Q x x Q y wherein x max is the maximum value of the point cloud coordinates on the x-axis, x min is the minimum value of the point cloud coordinates on the x-axis, y max is the maximum value of the point cloud coordinates on the y-axis, y min is the minimum value of the point cloud coordinates on the y-axis, R represents the diameter of the trunk of the fruit tree, Q x and Q y are the number of grids in the X-axis direction and the Y-axis direction of the coordinate system in the blank two-dimensional grid map, respectively, and Q is the total number of grids contained in the blank two-dimensional grid map.

2. The orchard navigation method based on multi-sensor fusion according to claim 1, characterized in that, the construction of the three-dimensional point cloud map of the orchard based on the three-dimensional point cloud data comprises: interpolation processing of the point cloud frames in the three-dimensional point cloud data to obtain a point cloud depth map generated based on the point cloud frames; for each frame of point cloud depth map, calculating the smoothness of each point of the point cloud depth map and sorting the points of the point cloud depth map according to the smoothness, extracting the corner feature points and surface feature points in each frame of point cloud depth map; splicing the point cloud frames according to the corner feature points and surface feature points in each frame of point cloud depth map to obtain the three-dimensional point cloud map of the orchard.

3. The orchard navigation method based on multi-sensor fusion according to claim 1, characterized in that, the determination and marking of the position of each fruit tree in the two-dimensional grid map of the orchard based on the number of points in the three-dimensional point cloud map vertically projected in each grid of the two-dimensional grid map to generate a two-dimensional grid map of the orchard, comprises: dividing the space of the three-dimensional point cloud map into a plurality of subspaces of the same size based on the two-dimensional grid map, and calculating the number of points contained in each subspace, wherein each grid in the two-dimensional grid map corresponds to each subspace in the three-dimensional point cloud map one by one; determining and marking the position of each fruit tree in the two-dimensional grid map of the orchard according to the number of points contained in each subspace in the three-dimensional point cloud map to generate a two-dimensional grid map of the orchard.

4. The orchard navigation method based on multi-sensor fusion according to claim 3, characterized in that, the determination and marking of the position of each fruit tree in the two-dimensional grid map of the orchard according to the number of points contained in each subspace in the three-dimensional point cloud map to generate a two-dimensional grid map of the orchard, comprises: acquiring the height data of the trunk of the fruit tree in the orchard; determining the height range of the trunk of the fruit tree according to the height data of the trunk of the fruit tree; for each subspace, if the number of points in the subspace within the height range exceeds a preset number, it is determined that there is a fruit tree in the subspace, and the corresponding grid of the subspace in the two-dimensional grid map is marked as an occupied state; Determine the grid marked as an occupancy state in the two-dimensional grid map as the position of the fruit tree, and generate a two-dimensional grid map of the orchard.

5. The multi-sensor fusion based orchard navigation method according to claim 1, wherein, The positioning navigation based on the three-dimensional point cloud map and the two-dimensional grid map of the orchard comprises: Obtaining an orchard work task to be performed by a device to be navigated; Positioning the device to be navigated based on the three-dimensional point cloud map to obtain a positioning pose of the device to be navigated; Determining a navigation point of the device to be navigated when performing the orchard work task according to the orchard work task and the two-dimensional grid map; Planning a navigation path based on the positioning pose, the navigation point, and the two-dimensional grid map; When the positioning pose represents that the position of the device to be navigated is at the position where the navigation point is located, repeating the steps of determining the navigation point and planning the navigation path until the orchard work task is completed.

6. The orchard navigation method based on multi-sensor fusion according to claim 5, characterized in that, The positioning of the device to be navigated based on the three-dimensional point cloud map to obtain a positioning pose of the device to be navigated comprises: Obtaining RTK data and IMU data of the device to be navigated; Converting the RTK data into odometer data, and positioning the device to be navigated according to the odometer data, the IMU data, the three-dimensional point cloud data, and the three-dimensional point cloud map to obtain a positioning pose of the device to be navigated.

7. The orchard navigation method based on multi-sensor fusion according to claim 5, characterized in that, Further comprising: Determining the occupied range of the fruit tree crown around the navigation path according to the navigation path and the three-dimensional point cloud map; Determining whether the device to be navigated will be scratched when passing through the navigation path based on the occupied range of the fruit tree crown and the pose of the device to be navigated; If so, adjusting the navigation path according to the pose of the device to be navigated and the occupied range of the fruit tree crown to avoid the device to be navigated from being scratched by the fruit tree crown when passing through the navigation path.

8. An orchard navigation device based on multi-sensor fusion, characterized by, Comprise: A data acquisition module for acquiring three-dimensional point cloud data obtained by scanning an orchard; A three-dimensional map construction module for constructing a three-dimensional point cloud map of the orchard based on the three-dimensional point cloud data; A two-dimensional map construction module for generating a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, the two-dimensional grid map comprising a plurality of grids of the same size; The two-dimensional map construction module is further configured to determine and mark the position of each fruit tree in the two-dimensional grid map based on the number of points in the three-dimensional point cloud map that are vertically projected in each grid of the two-dimensional grid map, and generate a two-dimensional grid map of the orchard; A positioning and navigation module for positioning and navigation based on the three-dimensional map and the two-dimensional grid map of the orchard; The two-dimensional grid map generated according to the plane of the three-dimensional point cloud map comprises a plurality of grids of the same size, which comprises: Traverse each point in the three-dimensional point cloud map, determine the plane size of the three-dimensional point cloud map according to the coordinate value of each point, obtain the diameter data of the trunk of the fruit tree in the orchard, and determine the grid size by combining the diameter data of the trunk and the plane size of the three-dimensional point cloud map, wherein the size of the grid does not exceed the cross section of the trunk of the fruit tree: Q = Q x x Q y wherein x max is the maximum value of the point cloud coordinates on the x-axis, x min is the minimum value of the point cloud coordinates on the x-axis, y max is the maximum value of the point cloud coordinates on the y-axis, y min is the minimum value of the point cloud coordinates on the y-axis, R represents the diameter of the trunk of the fruit tree, Q x and Q y are the number of grids in the X-axis direction and the Y-axis direction of the coordinate system in the blank two-dimensional grid map, respectively, and Q is the total number of grids contained in the blank two-dimensional grid map.

9. An electronic device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor implements the multi-sensor fusion based orchard navigation method according to any one of claims 1-7 when executing the computer program.

Images