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

Abstract

The invention provides an orchard navigation method, device and equipment based on multi-sensor fusion and a medium. The method comprises the following steps: acquiring three-dimensional point cloud data obtained 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 orchard in the two-dimensional grid map 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 the 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 for solving the problem that an existing orchard navigation method lacks the capability of being matched with orchard operation requirements, the technology of enabling orchard operation equipment to walk autonomously and complete operation tasks can be achieved, and unmanned management of an orchard is achieved by being matched with intelligent orchard operation machines and tools.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and navigation, and in particular to an orchard navigation method based on multi-sensor fusion. Background Art

[0002] The fruit industry is a labor-intensive and seasonally-dependent industry, with labor costs accounting for a high proportion of orchard production costs. In order to reduce labor costs and safety risks in orchard production, orchard operating equipment navigation technology was invented. Orchard operating equipment navigation technology can achieve unmanned management of orchards through map construction, positioning, navigation point coordinate calculation and path planning. In recent years, domestic and foreign researchers have developed some orchard navigation methods, but most of them are adapted to relatively single orchard environments and lack the ability to adapt to complex orchard scenarios. Current orchard navigation methods mostly extract navigation paths in real time, and do not construct high-precision maps of orchards, making it difficult to cope with changes in the orchard environment caused by the growth of fruit trees. Summary of the invention

[0003] The present invention provides an orchard navigation method based on multi-sensor fusion, which is used to solve the problem in the prior art that there is a lack of navigation methods capable of matching the orchard operation requirements, and to achieve high-precision positioning adapted to a variety of orchard environments.

[0004] The present invention provides an orchard navigation method based on multi-sensor fusion, the method comprising: Obtain three-dimensional point cloud data from scanning orchards; Construct a 3D point cloud map of the orchard based on 3D point cloud data; Generate a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, wherein the two-dimensional grid map includes a plurality of grids of the same size; Based on the number of points in the three-dimensional point cloud map vertically projected onto each grid point in the two-dimensional grid map, the position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked to generate a two-dimensional grid map of the orchard; Positioning and navigation are carried out based on the three-dimensional point cloud map and two-dimensional grid map of the orchard.

[0005] 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: Performing interpolation processing on 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 the point cloud depth map, calculate the smoothness of each point in the point cloud depth map and sort the points of the point cloud depth map according to the smoothness, and extract the corner feature points and surface feature points in each frame of the point cloud depth map; The point cloud frames are spliced ​​according to the corner feature points and surface feature points in each frame of the point cloud depth map to obtain a three-dimensional point cloud map of the orchard.

[0006] According to the orchard navigation method based on multi-sensor fusion provided by the present invention, a blank two-dimensional grid map is generated according to the plane of the three-dimensional point cloud map, and the two-dimensional grid map contains a plurality of grids of the same size, including: Traversing each point in the three-dimensional point cloud map, and determining the plane size of the three-dimensional point cloud map according to the coordinate value of each point; Obtain the diameter data of the trunks of the fruit trees in the orchard, and determine the grid size by combining the diameter data of the trunks with the plane size of the three-dimensional point cloud map, wherein the size of the grid does not exceed the cross section of the trunks of the fruit trees; The plane of the 3D point cloud map is divided according to the grid size to generate a blank 2D grid map.

[0007] According to the orchard navigation method based on multi-sensor fusion provided by the present invention, based on the number of points in each grid of the two-dimensional grid map vertically projected by the points in the three-dimensional point cloud map, the position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked, and a two-dimensional grid map of the orchard is generated, including: 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 one-to-one to each subspace in the three-dimensional point cloud map; The position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked according to the number of points contained in each subspace in the three-dimensional point cloud map, and a two-dimensional grid map of the orchard is generated.

[0008] According to the orchard navigation method based on multi-sensor fusion provided by the present invention, the position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked according to the number of points contained in each subspace in the three-dimensional point cloud map, and a two-dimensional grid map of the orchard is generated, including: Get the height data of the trunks of fruit trees in the orchard; Determine 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, it is determined that there are fruit trees in the subspace, and the grid corresponding to the subspace in the two-dimensional grid map is marked as occupied; The grids marked as occupied in the two-dimensional grid map are determined as the positions of fruit trees, and a two-dimensional grid map of the orchard is generated.

[0009] According to the orchard navigation method based on multi-sensor fusion provided by the present invention, positioning and navigation are performed based on a three-dimensional point cloud map and a two-dimensional grid map of the orchard, including: Obtaining orchard work tasks to be performed by the navigation device; Position the navigation device based on the three-dimensional point cloud map to obtain the positioning posture of the navigation device; Determine the navigation point of the navigation equipment when performing the orchard work task according to the orchard work task and the two-dimensional grid map; Plan navigation paths based on positioning posture, navigation points and two-dimensional grid maps; When the positioning posture indicates that the position of the navigation device is at the position of the navigation point, the above steps of determining the navigation point and planning the navigation path are repeated until the orchard work task is completed.

[0010] According to the orchard navigation method based on multi-sensor fusion provided by the present invention, a navigation device is positioned based on a three-dimensional point cloud map to obtain a positioning posture of the navigation device, including: Obtain RTK data and IMU data of the navigation device; The RTK data is converted into odometer data, and the navigation device to be navigated is positioned based on the odometer data, IMU data, 3D point cloud data and 3D point cloud map to obtain the positioning posture of the navigation device to be navigated.

[0011] According to the orchard navigation method based on multi-sensor fusion provided by the present invention, the method further includes: Determine 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; Based on the area occupied by the canopy of the fruit tree and the position of the navigation device, determine whether the navigation device will be scratched when passing through the navigation path; If so, the navigation path is adjusted according to the position of the navigation device and the area occupied by the crown of the fruit tree, so as to avoid the navigation device from scratching the crown of the fruit tree when passing through the navigation path.

[0012] The present invention also provides an orchard navigation device based on multi-sensor fusion, comprising: A data acquisition module, used to obtain three-dimensional point cloud data by scanning the orchard; A three-dimensional map construction module, used to construct a three-dimensional point cloud map of the orchard based on three-dimensional point cloud data; A two-dimensional map construction module is used to generate a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, wherein the two-dimensional grid map includes a plurality of grids of the same size; The two-dimensional map construction module is also used to determine and mark the position of each fruit tree in the orchard in the two-dimensional grid map based on the number of points in each grid of the two-dimensional grid map vertically projected by the points in the three-dimensional point cloud map, so as to generate a two-dimensional grid map of the orchard; The positioning and navigation module is used for positioning and navigation based on the three-dimensional point cloud map and two-dimensional grid map of the orchard.

[0013] 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: Performing interpolation processing on 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 the point cloud depth map, calculate the smoothness of each point in the point cloud depth map and sort the points of the point cloud depth map according to the smoothness, and extract the corner feature points and surface feature points in each frame of the point cloud depth map; The point cloud frames are spliced ​​according to the corner feature points and surface feature points in each frame of the point cloud depth map to obtain a three-dimensional point cloud map of the orchard.

[0014] 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: Traverse each point in the three-dimensional point cloud map, and 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 trunks of the fruit trees in the orchard, and determine the grid size by combining the diameter data of the trunks with 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; The plane of the 3D point cloud map is divided according to the grid size to generate a blank 2D grid map.

[0015] 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: Based on the two-dimensional grid 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, wherein each grid in the two-dimensional grid map corresponds one-to-one to each subspace in the three-dimensional point cloud map; The position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked according to the number of points contained in each subspace in the three-dimensional point cloud map, and a two-dimensional grid map of the orchard is generated.

[0016] 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: Get the height data of the trunks of fruit trees in the orchard; Determine 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, it is determined that there are fruit trees in the subspace, and the grid corresponding to the subspace in the two-dimensional grid map is marked as occupied; The grids marked as occupied in the two-dimensional grid map are determined as the positions of fruit trees, and a two-dimensional grid map of the orchard is generated.

[0017] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the positioning navigation module is specifically used for: Obtaining orchard work tasks to be performed by the navigation device; Position the navigation device based on the three-dimensional point cloud map to obtain the positioning posture of the navigation device; Determine navigation points when performing orchard work tasks based on orchard work tasks and two-dimensional grid maps; Plan navigation paths based on positioning posture, navigation points and two-dimensional grid maps; When the positioning posture indicates that the position of the navigation device is at the position of the navigation point, the above steps of determining the navigation point and planning the navigation path are repeated until the orchard work task is completed.

[0018] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the positioning navigation module is specifically used for: Obtain RTK data and IMU data of the navigation device; The RTK data is converted into odometer data, and the navigation device to be navigated is positioned based on the odometer data, IMU data, 3D point cloud data and 3D point cloud map to obtain the positioning posture of the navigation device to be navigated.

[0019] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the positioning navigation module is specifically used for: Determine 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; Based on the area occupied by the canopy of the fruit tree and the position of the navigation device, determine whether the navigation device will be scratched when passing through the navigation path; If so, the navigation path is adjusted according to the position of the navigation device and the area occupied by the crown of the fruit tree, so as to avoid the navigation device from scratching the crown of the fruit tree when passing through the navigation path.

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

[0021] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the orchard navigation method based on multi-sensor fusion as described in any one of the above is implemented.

[0022] The present invention also provides a computer program product, comprising a computer program, wherein when the computer program is executed by a processor, the computer program implements any of the above-mentioned orchard navigation methods based on multi-sensor fusion.

[0023] The invention provides an orchard navigation method based on multi-sensor fusion, which constructs a three-dimensional point cloud map of the orchard by scanning the orchard, and then generates a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, 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 on each grid point in the two-dimensional grid map, generates a two-dimensional grid map of the orchard, extracts the position information of the fruit tree trunk from the three-dimensional point cloud map of the orchard to construct a two-dimensional grid map, and finally performs positioning and navigation based on the three-dimensional point cloud map and the two-dimensional grid map of the orchard. Compared with the prior art, the three-dimensional point cloud map is constructed in real time and converted according to the three-dimensional point cloud map to obtain a two-dimensional grid map marking the position of the fruit trees in the orchard, so that the operating equipment can be positioned according to the three-dimensional point cloud map, and then the operating equipment is controlled to perform fixed-point operations at the designated fruit tree position according to the trunk position in the two-dimensional grid map, which can adapt to the navigation tasks in the complex environment of the orchard, support the development of smart agriculture, and realize unmanned management of the orchard. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0025] Figure 1 It is a flow chart of the orchard navigation method based on multi-sensor fusion provided by the present invention.

[0026] Figure 2 It is a schematic diagram of a scene in which the three-dimensional point cloud map provided by the present invention extracts the trunk position information to obtain a two-dimensional grid map.

[0027] Figure 3 It is a flow chart of the positioning algorithm for fusing three-source data provided by the present invention.

[0028] Figure 4 It is a flow chart of the random particle set motion measurement fusion iterative positioning algorithm for fusing three kinds of data provided by the present invention.

[0029] Figure 5 It is a schematic diagram of a scenario for calculating navigation points provided by the present invention.

[0030] Figure 6 It is a flow chart of the multi-point navigation algorithm provided by the present invention.

[0031] Figure 7 The present invention provides a schematic diagram of an orchard navigation system.

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

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

[0034] Fig.10 It is a structural schematic diagram of the electronic device provided by the present invention. DETAILED DESCRIPTION

[0035] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0036] Combine the following Figure 1-10 The invention describes an orchard navigation method, device, equipment and medium based on multi-sensor fusion.

[0037] The orchard navigation method based on multi-sensor fusion mainly uses industrial computers, three-dimensional lidar, inertial measurement unit (IMU), and real-time differential positioning (RTK) equipment.

[0038] Figure 1 is a flow chart of the orchard navigation method based on multi-sensor fusion provided by the present invention, such as Figure 1 As shown, the method includes the following: Step 101: Obtain three-dimensional point cloud data by scanning an orchard.

[0039] Step 102: construct a three-dimensional point cloud map of the orchard based on the three-dimensional point cloud data.

[0040] In some embodiments, a three-dimensional point cloud map of an orchard is constructed based on three-dimensional point cloud data, including: interpolating 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 the point cloud depth map, calculating the smoothness of each point in 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 the point cloud depth map; splicing the point cloud frames according to the corner feature points and surface feature points in each frame of the point cloud depth map to obtain a three-dimensional point cloud map of the orchard. In specific implementation, based on the three-dimensional simultaneous localization and mapping (SLAM) method, firstly integrate the inertial measurement unit data, and obtain the speed change and posture change between two moments, so as to realize the odometer function in the map construction. Secondly, process the laser radar point cloud data, and obtain the motion change of each frame of laser radar from the start of scanning to the start of scanning of the next frame of laser radar by integrating the rotation angles around the three coordinate axes in the three-dimensional coordinate system. Combined with the timestamp of the data point in the frame point cloud, proportional interpolation is performed to achieve point cloud motion distortion correction and obtain a depth map based on the three-dimensional point cloud data. Then, according to the formula ( ) Calculate the smoothness of the depth map and sort it to obtain corner feature points and surface feature points, and realize feature extraction of points in the depth map. Among them, C is the smoothness of the data point, is the depth value of the data point; is the depth value of the adjacent data point.

[0041] Finally, based on the feature points in the point cloud frame, a tree data structure (K-dimensional tree, KD-Tree) of the current frame and the historical frame is established, and the K nearest neighbors are searched. If the match is successful, the new point cloud frame is spliced ​​into the historical point cloud, and the construction of the three-dimensional point cloud map of the orchard is gradually realized and saved as a PCD (Point Cloud Data) file. When matching the point cloud frame, the corner feature points and surface feature points extracted from the point cloud depth map corresponding to the point cloud frame are used for matching. If the point cloud depth map of the current point cloud frame and the point cloud depth map of the historical point cloud frame have the same features or the similarity is higher than the similarity threshold, the current point cloud frame is considered to match the corresponding historical point cloud frame, and the current point cloud frame is spliced ​​with the corresponding historical point cloud frame to gradually build a three-dimensional point cloud map of the orchard.

[0042] In the disclosed embodiment, due to the diverse types of orchard environments, single-line lidar detection has great limitations. Therefore, in order to improve the robustness of orchard navigation and increase the perception of orchard environmental information in map construction and positioning, the present invention adopts a multi-line lidar and inertial measurement unit data deep fusion method, and uses a SLAM-based map construction method to realize the construction of a three-dimensional point cloud map of the orchard.

[0043] Step 103: Generate a blank two-dimensional grid map based on the plane of the three-dimensional point cloud map, wherein the two-dimensional grid map includes a plurality of grids of the same size.

[0044] Exemplarily, the size of the horizontal cross section of the three-dimensional point cloud map can be set to the size of the two-dimensional grid map to generate a blank two-dimensional map, and then the blank two-dimensional map is divided into a number of grids of the same size.

[0045] In some embodiments, a blank two-dimensional grid map is generated based on the plane of a three-dimensional point cloud map, wherein the two-dimensional grid map contains a number of grids of the same size, including: traversing each point in the three-dimensional point cloud map, and determining the plane size of the three-dimensional point cloud map according to the coordinate value of each point; obtaining diameter data of the trunks of fruit trees in the orchard, and determining the grid size by combining the diameter data of the trunks with 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; dividing the plane of the three-dimensional point cloud map according to the grid size to generate a blank two-dimensional grid map.

[0046] Process the entire 3D point cloud map. The coordinates of the points in the 3D point cloud are It means that each point in the point cloud is traversed to determine the maximum and minimum values ​​of the point's coordinates on the x-axis, as well as the maximum and minimum values ​​of the point's coordinates on the y-axis. The size of the horizontal cross-section of the orchard's three-dimensional point cloud map is calculated based on the maximum and minimum values ​​of the point cloud coordinates on the x-axis and y-axis.

[0047] The resolution of the 2D grid map is set based on the diameter data of the orchard tree trunks and the plane size of the 3D point cloud map to ensure that the grid size of the 2D map is not larger than the cross section of the tree trunk. , , Get the initial blank two-dimensional grid map. Among them, is the maximum value of the point cloud coordinate on the x-axis, is the minimum value of the point cloud coordinate on the x-axis, is the maximum value of the point cloud coordinate on the y-axis, is the minimum value of the point cloud coordinate on the y-axis, and R represents the diameter of the fruit tree trunk. and They are the number of grids in the X-axis and Y-axis directions of the map coordinate system, is the total number of rasters contained in the 2D map of the orchard.

[0048] Step 104: based on the number of points in the three-dimensional point cloud map vertically projected onto each grid of the two-dimensional grid map, determine and mark the position of each fruit tree in the orchard in the two-dimensional grid map to generate a two-dimensional grid map of the orchard.

[0049] It is understandable that in the 3D point cloud map, the distribution of points generated by laser scanning the trunks of fruit trees is relatively the densest in the orchard. Then, when a blank 2D grid map is used as the bottom surface of the 3D point cloud map and the points in the 3D point cloud map are vertically projected onto the 2D grid map, the points of the point cloud generated by scanning the trunks of fruit trees are densely distributed on the 2D grid map. In this way, the position of the trunks of fruit trees in the 2D grid map can be determined based on the number of points in each grid of the 3D point cloud vertically projected onto the 2D grid map.

[0050] In some embodiments, based on the number of points in each grid of the two-dimensional grid map vertically projected from the points in the three-dimensional point cloud map, the position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked, and a two-dimensional grid map of the orchard is generated, including: dividing the space of the three-dimensional point cloud map into a number 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 one-to-one to each subspace in the three-dimensional point cloud map; determining and marking the position of each fruit tree in the orchard in the two-dimensional grid map according to the number of points contained in each subspace in the three-dimensional point cloud map, and generating a two-dimensional grid map of the orchard.

[0051] like Figure 2 As shown in step (1) in the figure, the 3D point cloud of the orchard is segmented into the following parts based on the grid of the 2D grid map: Figure 2 The Q cubic subspaces shown in step (2) in , where It is The height of the point; It is Points Coordinate value.

[0052] Then, the coordinates of each point are traversed to calculate the number of points contained in each stereo subspace. The number of points contained in each stereo subspace is the number of points vertically projected from the three-dimensional point cloud in the two-dimensional grid corresponding to the stereo subspace. According to the number of points contained in each stereo subspace, it can be determined whether there is a fruit tree trunk in the corresponding two-dimensional grid, thereby marking the position of each fruit tree in the orchard in the two-dimensional grid map and generating a two-dimensional grid map of the orchard.

[0053] In some embodiments, the position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked according to the number of points contained in each subspace in the three-dimensional point cloud map, and a two-dimensional grid map of the orchard is generated, including: obtaining height data of the trunks of the fruit trees in the orchard; determining the height range of the trunks of the fruit trees according to the height data of the trunks of the fruit trees; for each subspace, if the number of points within the height range in the subspace exceeds a preset number, it is determined that there are fruit trees in the subspace, and the grid corresponding to the subspace in the two-dimensional grid map is marked as occupied; the grid marked as occupied in the two-dimensional grid map is determined as the position of the fruit tree, and a two-dimensional grid map of the orchard is generated.

[0054] In specific implementation, all points are divided into three-dimensional space with each grid as the base according to the x-axis coordinate value and y-axis coordinate value of the point in the three-dimensional point cloud. The points in each three-dimensional space are sorted from small to large according to the z-axis coordinate value, and the smallest z value is used as the height of the "initial plane" of the three-dimensional space. According to the formula , defines the "height" of a point in the point cloud map The coordinates of the point Relative to the grid According to the characteristics of fruit trees and the growth of orchard weeds, such as Figure 2 As shown in step (3) in the figure, set the trunk height range ( , ), filter out the occlusion of tree trunks by tree crowns and weeds, and avoid the interference of point clouds generated by laser scanning tree crowns and weeds in locating the position of tree trunks. Then count the heights of each point in the three-dimensional space that meet the trunk height range ( , ) of the points, such as Figure 2 As shown in step (4) in , if the number of points exceeds the set threshold, the grid marked as the bottom of the three-dimensional space is as follows Figure 2 The occupied state shown in step (5) in . Figure 2 As shown in step (6) in the figure, 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 for extracting the trunk position information into a two-dimensional grid map.

[0055] The disclosed embodiment determines the height range of the fruit tree trunks through the height data of the fruit tree trunks, and then determines whether there are fruit tree trunks in the subspace by counting the number of points in each subspace within the trunk height range, and further marks the grid position of the fruit tree trunks in the two-dimensional grid map, thereby avoiding interference of the point cloud generated by laser scanning of tree crowns and weeds in locating the trunk position, and making the trunk position in the generated two-dimensional grid map more accurate.

[0056] In the disclosed embodiment, since the road conditions in the orchard are complex and fluctuating, high-precision centerline navigation and autonomous line change in the orchard require the combination of environmental detection information and motion information to improve the accuracy and stability of map construction. To this end, the disclosed embodiment designs a two-dimensional grid map construction method that extracts the position information of all fruit tree trunks from the three-dimensional point cloud map of the orchard, so that the coordinates of the navigation points on the centerline of the row and at the turning point of the field can be automatically calculated based on the fruit tree position information, and the path planning and real-time path planning between the navigation points in the orchard can be completed on the two-dimensional map according to the characteristics of the fruit trees, the orchard space and the task requirements.

[0057] Step 105: Perform positioning navigation based on the three-dimensional point cloud map and the two-dimensional grid map of the orchard.

[0058] Exemplarily, the embodiments of the present disclosure realize navigation of the target device by positioning the target device on a three-dimensional map, then projecting the positioning result of the target device onto a two-dimensional map, and performing path calculation and motion control on the target device based on the real-time position of the target device on the two-dimensional map.

[0059] In some embodiments, positioning and navigation are performed based on a three-dimensional point cloud map and a two-dimensional grid map of an orchard, including: obtaining an orchard work task to be performed by the navigation device; positioning the navigation device based on the three-dimensional point cloud map to obtain the positioning posture of the navigation device; determining the navigation point of the navigation device 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 posture, navigation point and two-dimensional grid map; when the positioning posture indicates that the position of the navigation device is at the position of the navigation point, repeating the above steps of determining the navigation point and planning the navigation path until the orchard work task is completed. Among them, positioning the navigation device based on the three-dimensional point cloud map to obtain the positioning posture of the navigation device includes: obtaining RTK data and IMU data of the navigation device; converting the RTK data into odometer data, and positioning the navigation device based on the odometer data, IMU data, three-dimensional point cloud data and three-dimensional point cloud map to obtain the positioning posture of the navigation device.

[0060] Figure 3 The flowchart of the positioning algorithm for fusing three-source data provided by the present invention is a random particle set motion measurement fusion iterative positioning algorithm, wherein the three-source data are laser radar measurement results, inertial measurement unit (IMU) data and odometer data converted from RTK data. Figure 3As shown in the figure, firstly, the three-dimensional point cloud data, IMU data and odometer data are processed to obtain filtered point cloud data, posture information and position information respectively, and the initial particle set is randomly generated. Secondly, the motion information of the odometer data is used to predict the particles. Then resample to increase the random particle set to avoid falling into the situation where the global wrong positioning cannot be recovered. Then determine whether the set convergence condition is met. If so, output the process posture. If not, enter the loop and iterate the particle set. Use lidar data for measurement update, use IMU data for posture correction, and calculate the particle weight. Then use the motion information of the odometer data to predict the particles, and finally resample to increase the random particle set, and loop until the convergence condition is met. Figure 4 It is a flow chart of the random particle set motion measurement fusion iterative positioning algorithm for fusing three kinds of data provided by the present invention.

[0061] In the specific implementation, 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 positioning algorithm that integrates three types of data. During positioning, the RTK data is converted into odometer data; then the original lidar data, original IMU data and odometer data are converted into readable data and sent to the random particle set motion measurement fusion iterative positioning algorithm. The random particle set motion measurement fusion iterative positioning algorithm calculates the process pose based on the initial pose, 3D point cloud map, lidar data, IMU data and odometer data, and filters the pose with the odometer data to obtain the final positioning result.

[0062] Figure 5 It is a schematic diagram of a scenario for calculating navigation points provided by the present invention.

[0063] like Figure 5 As shown in the figure, when navigating, firstly, the coordinates of the navigation points are calculated according to the orchard structure and task requirements, and then the coordinates of the navigation points are obtained. Figure 5 The navigation points in the orchard are set at the midpoint of the line connecting the tree trunks on both sides of the tree row. The headland steering navigation point is set at 1 / 2 of the headland space L on the extension line of the row centerline in the row break area.

[0064] Figure 6 It is a flow chart of the multi-point navigation algorithm provided by the present invention, such as Figure 6 As shown, the method includes: The tree trunk position information on the two-dimensional grid map of the orchard obtained by the two-dimensional map construction module is read, and the coordinates of the navigation point are calculated.

[0065] The navigation point on the line centerline is calculated to realize autonomous navigation on the fixed line centerline.

[0066] Calculate the coordinates of the navigation point at the end of the field to achieve autonomous steering at the end of the orchard. The coordinates of the navigation point are input to the multi-point navigation algorithm, and the arrival of the current navigation point is determined in combination with the real-time positioning results. After reaching the current navigation point, the coordinates of the next navigation point are released to the path planning module to achieve the sequential release of navigation points.

[0067] In some embodiments, the method also includes: determining the area occupied by the crowns of fruit trees around the navigation path based on the navigation path and the three-dimensional point cloud map; determining whether a scratch will occur when the device to be navigated passes through the navigation path based on the area occupied by the crowns of fruit trees and the posture of the device to be navigated; if so, adjusting the navigation path according to the posture of the device to be navigated and the area occupied by the crowns of fruit trees, so as to avoid scratches between the crowns of fruit trees and the device to be navigated when passing through the navigation path. Figure 7 It is a flow chart of an orchard navigation method based on multi-sensor fusion provided by the present invention, and it is a schematic diagram of an orchard navigation system provided by the present invention.

[0068] When implementing it, Figure 7 As shown in the figure, the Dijkstra algorithm is used to plan the navigation path between navigation points, and the segmented path planning in the orchard is realized by combining the calculation and release of the navigation point coordinates. The row centerline that fits the direction of the tree row is obtained between the rows, and the turning path located in the center of the headland space is obtained at the headland. It can be applied in standard orchards and can also deal with curved tree rows in non-standard orchards. The path planning between adjacent navigation points is done only once.

[0069] The dynamic window approach (DWA) algorithm is used to perform real-time path planning based on the positioning results and the relative position of the path between navigation points, combined with the real-time detected canopy point cloud information. During the navigation process, the canopy height range is set according to the characteristics of the fruit trees and the height required for the orchard operating equipment to pass, and the canopy point cloud in the point cloud collected by the radar in real time is extracted. The relative position of the canopy point cloud relative to the operating equipment is obtained. The positioning module outputs the relative position of the operating equipment relative to the origin of the two-dimensional map. Combined with the relative position of the canopy point cloud collected in real time relative to the operating equipment, the relative position of the canopy point cloud relative to the origin of the two-dimensional map is obtained, and the projection of the canopy point cloud on the two-dimensional map is realized. The orchard two-dimensional map with canopy information added is used for real-time path optimization of the DWA algorithm. Finally, the planned path between navigation points is tracked while the actual execution path is optimized to avoid scratches with the canopy.

[0070] Figure 8 is a flow chart of an orchard navigation method based on multi-sensor fusion provided by the present invention. is another schematic diagram of an orchard navigation system provided by the present invention. Figure 8 As shown, the method includes the following: The 3D laser radar and RTK equipment are connected to the industrial computer through the network port respectively, and the angular gyroscope establishes communication with the industrial computer through the 232 serial port and is used as an inertial measurement unit.

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

[0072] It is saved in the industrial computer in the format of PCD file and published to the two-dimensional map construction module.

[0073] In specific implementation, a reasonable trunk height range and map resolution are set according to the characteristics of fruit trees, trunk thickness, and weed growth. The two-dimensional map construction module can obtain a two-dimensional grid map of the orchard that retains the trunk position information, which is used for navigation point coordinate calculation and path planning.

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

[0075] The point cloud, inertial measurement unit data and odometer data based on RTK information collected in real time are used to realize positioning in the orchard environment based on the orchard's three-dimensional point cloud map.

[0076] Combine the positioning results and the coordinates of the navigation points to determine the current navigation point's arrival status, and publish the calculated navigation points in sequence.

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

[0078] In the specific implementation, during the navigation process, the three-dimensional map construction module works simultaneously to update the orchard map for the next navigation task 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 growth cycle.

[0079] In the disclosed embodiment, because the road conditions in the orchard are complex and fluctuating, in order to achieve high-precision line navigation and autonomous line change in the orchard, it is necessary to combine environmental detection information and motion information to improve the accuracy and stability of map construction. A two-dimensional grid map construction method is designed to extract the position information of all fruit tree trunks from the three-dimensional point cloud map of the orchard. According to the position information of the fruit trees, the coordinates of the navigation points located on the line centerline and the turning point of the field are automatically calculated. According to the characteristics of the fruit trees, the orchard space and the task requirements, the path planning and real-time path planning between the navigation points in the orchard are completed on the two-dimensional map. Among them, the path planning between the navigation points is completed by the Dijkstra algorithm, and the real-time path planning is completed by the DWA algorithm based on the crown information, positioning results and the relative position of the path between the navigation points in the real-time point cloud data. A random particle set motion measurement fusion iterative positioning algorithm that integrates three types of data is designed to achieve high-precision positioning that adapts to various orchard environments by integrating laser radar measurement results, inertial measurement unit data and RTK data. The overall high-precision navigation in the orchard is achieved through map construction, navigation point calculation, positioning, path planning and navigation point release in sequence. During the navigation process, the real-time data of the 3D laser radar is used for positioning and optimizing the path between navigation points. At the same time, it is integrated with the real-time data of the inertial measurement unit to achieve the reconstruction and update of the orchard map for the next orchard navigation operation to cope with the changes in the orchard environment caused by the growth of fruit trees. At the same time, through map construction, positioning, navigation point coordinate calculation and path planning, the orchard operation equipment can walk autonomously in the orchard environment to complete the operation task, meeting the fixed-point navigation needs for single fruit tree operations.

[0080] The present invention provides an orchard navigation method based on multi-sensor fusion, which constructs a three-dimensional point cloud map of the orchard by scanning the orchard, and then generates a blank two-dimensional grid map based on the plane of the three-dimensional point cloud map. Based on the number of points in the three-dimensional point cloud map vertically projected onto each grid point in the two-dimensional grid map, the position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked, and a two-dimensional grid map of the orchard is generated. The position information of the fruit tree trunks is extracted from the orchard three-dimensional point cloud map to construct a two-dimensional grid map, and finally positioning and navigation is performed based on the three-dimensional point cloud map and the two-dimensional grid map of the orchard. Compared with the prior art, a three-dimensional point cloud map is constructed in real time and a two-dimensional grid map marking the positions of the fruit trees in the orchard is obtained according to the conversion of the three-dimensional point cloud map, so that the operating equipment can be positioned according to the three-dimensional point cloud map, and then the operating equipment can be controlled to perform fixed-point operations at the designated fruit tree positions according to the trunk positions in the two-dimensional grid map. This can adapt to navigation tasks in complex orchard environments, support the development of smart agriculture, and realize unmanned management of orchards. Fig. 9 As shown, the orchard navigation device based on multi-sensor fusion provided by the present invention, the orchard navigation device based on multi-sensor fusion described below and the orchard navigation method based on multi-sensor fusion described above can be referred to each other. It includes: Data acquisition module 901, used to acquire three-dimensional point cloud data by scanning the orchard; A three-dimensional map construction module 902 is used to construct a three-dimensional point cloud map of the orchard based on the three-dimensional point cloud data; A two-dimensional map construction module 903 is used to generate a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, wherein the two-dimensional grid map includes a plurality of grids of the same size; The two-dimensional map construction module 903 is further used to determine and mark the position of each fruit tree in the orchard in the two-dimensional grid map based on the number of points in each grid of the two-dimensional grid map vertically projected by the points in the three-dimensional point cloud map, and generate a two-dimensional grid map of the orchard; The positioning and navigation module 904 is used to perform positioning and navigation based on the two-dimensional grid map of the orchard.

[0081] 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: Performing interpolation processing on 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 the point cloud depth map, calculate the smoothness of each point in the point cloud depth map and sort the points of the point cloud depth map according to the smoothness, and extract the corner feature points and surface feature points in each frame of the point cloud depth map; The point cloud frames are spliced ​​according to the corner feature points and surface feature points in each frame of the point cloud depth map to obtain a three-dimensional point cloud map of the orchard.

[0082] 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: Traverse each point in the three-dimensional point cloud map, and 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 trunks of the fruit trees in the orchard, and determine the grid size by combining the diameter data of the trunks with 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; The plane of the 3D point cloud map is divided according to the grid size to generate a blank 2D grid map.

[0083] 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 further used for: Based on the two-dimensional grid 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, wherein each grid in the two-dimensional grid map corresponds one-to-one to each subspace in the three-dimensional point cloud map; The position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked according to the number of points contained in each subspace in the three-dimensional point cloud map, and a two-dimensional grid map of the orchard is generated.

[0084] 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: Get the height data of the trunks of fruit trees in the orchard; Determine 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, it is determined that there are fruit trees in the subspace, and the grid corresponding to the subspace in the two-dimensional grid map is marked as occupied; The grids marked as occupied in the two-dimensional grid map are determined as the positions of fruit trees, and a two-dimensional grid map of the orchard is generated.

[0085] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the positioning navigation module 904 is specifically used for: Obtain the orchard work tasks to be performed and the positioning posture of the navigation equipment to be performed; Determine navigation points when performing orchard work tasks based on orchard work tasks and two-dimensional grid maps; Plan navigation paths based on positioning posture, navigation points and two-dimensional grid maps; When the positioning posture indicates that the position of the navigation device is at the position of the navigation point, the above steps of determining the navigation point and planning the navigation path are repeated until the orchard work task is completed.

[0086] 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: Obtain RTK data and IMU data of the navigation device; The RTK data is converted into odometer data, and the navigation device to be navigated is positioned according to the odometer data, IMU data and 3D point cloud data to obtain the positioning posture of the navigation device to be navigated.

[0087] According to the orchard navigation device based on multi-sensor fusion provided by the present invention, the navigation module 904 is further used for: Determine 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; Based on the area occupied by the canopy of the fruit tree and the position of the navigation device, determine whether the navigation device will be scratched when passing through the navigation path; If so, the navigation path is adjusted according to the position of the navigation device and the area occupied by the crown of the fruit tree, so as to avoid the navigation device from scratching the crown of the fruit tree when passing through the navigation path.

[0088] like Fig.10 An example of a physical structure diagram of an electronic device is shown in FIG. Fig.10As 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 through the communication bus 1040. The processor 1010 may call the logic instructions in the memory 1030 to execute the orchard navigation method based on multi-sensor fusion, and the method includes: Obtain three-dimensional point cloud data from scanning orchards; Construct a 3D point cloud map of the orchard based on 3D point cloud data; Generate a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, wherein the two-dimensional grid map includes a plurality of grids of the same size; Based on the number of points in the three-dimensional point cloud map vertically projected onto each grid point in the two-dimensional grid map, the position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked to generate a two-dimensional grid map of the orchard; Positioning and navigation are carried out based on the three-dimensional point cloud map and two-dimensional grid map of the orchard.

[0089] In addition, the logic instructions in the above-mentioned memory 1030 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods of various embodiments of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc. Various media that can store program codes.

[0090] On the other hand, the present invention further provides a computer program product, the computer program product includes a computer program, the computer program can be stored in a non-transitory computer-readable storage medium, when the computer program is executed by a processor, the computer can execute the orchard navigation method based on multi-sensor fusion provided by the above methods, the method includes: Obtain three-dimensional point cloud data from scanning orchards; Construct a 3D point cloud map of the orchard based on 3D point cloud data; Generate a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, wherein the two-dimensional grid map includes a plurality of grids of the same size; Based on the number of points in the three-dimensional point cloud map vertically projected onto each grid point in the two-dimensional grid map, the position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked to generate a two-dimensional grid map of the orchard; Positioning and navigation are carried out based on the two-dimensional grid map of the orchard.

[0091] In another aspect, the present invention further provides a non-transitory computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the orchard navigation method based on multi-sensor fusion provided by the above methods is implemented, and the method includes: Obtain three-dimensional point cloud data from scanning orchards; Construct a 3D point cloud map of the orchard based on 3D point cloud data; Generate a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, wherein the two-dimensional grid map includes a plurality of grids of the same size; Based on the number of points in the three-dimensional point cloud map vertically projected onto each grid point in the two-dimensional grid map, the position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked to generate a two-dimensional grid map of the orchard; Positioning and navigation are carried out based on the two-dimensional grid map of the orchard.

[0092] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, i.e., they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Those of ordinary skill in the art may understand and implement it without creative effort.

[0093] Through the description of the above implementation modes, those skilled in the art can clearly understand that each implementation mode can be implemented by means of software plus a necessary general hardware platform, or of course by hardware. Based on such an understanding, the above technical solution can essentially or in other words be embodied in the form of a software product that contributes to the prior art. The computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods of each embodiment or some parts of the embodiment.

[0094] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions 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: Obtain three-dimensional point cloud data from scanning orchards; 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 includes a plurality of grids of the same size; Determine and mark 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 in the two-dimensional grid map, and generate a two-dimensional grid map of the orchard; Positioning and navigation are performed based on the three-dimensional point cloud map and the two-dimensional grid map of the orchard.

2. The orchard navigation method based on multi-sensor fusion according to claim 1, characterized in that: The constructing of the three-dimensional point cloud map of the orchard based on the three-dimensional point cloud data comprises: Performing interpolation processing on 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 the point cloud depth map, calculate the smoothness of each point in the point cloud depth map and sort the points of the point cloud depth map according to the smoothness, and extract the corner feature points and surface feature points in each frame of the point cloud depth map; The point cloud frames are spliced ​​according to the corner feature points and surface feature points in each frame of the point cloud depth map to obtain a 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 generating of a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, wherein the two-dimensional grid map contains a plurality of grids of the same size, including: Traversing each point in the three-dimensional point cloud map, and determining the plane size of the three-dimensional point cloud map according to the coordinate value of each point; obtaining the diameter data of the trunks of the fruit trees in the orchard, and determining the grid size by combining the diameter data of the trunks with 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; The plane of the three-dimensional point cloud map is divided according to the grid size to generate the blank two-dimensional grid map.

4. The orchard navigation method based on multi-sensor fusion according to claim 1, characterized in that: The method of determining and marking the position of each fruit tree in the orchard in the two-dimensional grid map based on the number of points in each grid of the two-dimensional grid map vertically projected from the points in the three-dimensional point cloud map to generate the 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 one-to-one to each subspace in the three-dimensional point cloud map; The position of each fruit tree in the orchard in the two-dimensional grid map is determined and marked according to the number of points contained in each subspace in the three-dimensional point cloud map, so as to generate a two-dimensional grid map of the orchard.

5. The orchard navigation method based on multi-sensor fusion according to claim 4, characterized in that: The step of determining and marking the position of each fruit tree in the orchard in the two-dimensional grid map according to the number of points contained in each subspace in the three-dimensional point cloud map, and generating the two-dimensional grid map of the orchard, comprises: Acquiring height data of trunks of fruit trees in the orchard; Determine the height range of the fruit tree trunk according to the height data of the fruit tree trunk; For each subspace, if the number of points in the subspace within the height range exceeds a preset number, it is determined that there are fruit trees in the subspace, and the grid corresponding to the subspace in the two-dimensional grid map is marked as occupied; The grids marked as occupied in the two-dimensional grid map are determined as fruit tree positions, and a two-dimensional grid map of the orchard is generated.

6. The orchard navigation method based on multi-sensor fusion according to claim 1, characterized in that: The positioning and navigation based on the three-dimensional point cloud map and the two-dimensional grid map of the orchard includes: Obtaining orchard work tasks to be performed by the navigation device; Positioning the device to be navigated based on the three-dimensional point cloud map to obtain a positioning posture of the device to be navigated; Determine the navigation point of the navigation device 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 posture, the navigation point and the two-dimensional grid map; When the positioning posture indicates that the position of the navigation device is at the position of the navigation point, the above steps of determining the navigation point and planning the navigation path are repeated until the orchard work task is completed.

7. The orchard navigation method based on multi-sensor fusion according to claim 6, characterized in that: The method of positioning the device to be navigated based on the three-dimensional point cloud map to obtain the positioning posture of the device to be navigated includes: Acquire RTK data and IMU data of the navigation device; The RTK data is converted into odometer data, and the device to be navigated is positioned according to the odometer data, the IMU data, the three-dimensional point cloud data and the three-dimensional point cloud map to obtain the positioning posture of the device to be navigated.

8. The orchard navigation method based on multi-sensor fusion according to claim 6, characterized in that: Also includes: Determine the area occupied by the crowns of fruit trees around the navigation path according to the navigation path and the three-dimensional point cloud map; Determine whether the device to be navigated will be scratched when passing through the navigation path based on the area occupied by the crown of the fruit tree and the position of the device to be navigated; If so, the navigation path is adjusted according to the position of the device to be navigated and the area occupied by the crown of the fruit tree, so as to avoid the device to be navigated from scratching the crown of the fruit tree when passing through the navigation path.

9. An orchard navigation device based on multi-sensor fusion, characterized in that: include: A data acquisition module, used to obtain three-dimensional point cloud data by scanning the orchard; A three-dimensional map construction module, used to construct a three-dimensional point cloud map of the orchard based on the three-dimensional point cloud data; A two-dimensional map construction module, used to generate a blank two-dimensional grid map according to the plane of the three-dimensional point cloud map, wherein the two-dimensional grid map includes a plurality of grids of the same size; The two-dimensional map construction module is further used to determine and mark 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 in the two-dimensional grid map, so as to generate a two-dimensional grid map of the orchard; The positioning and navigation module is used for performing positioning and navigation based on the three-dimensional map and the two-dimensional grid map of the orchard.

10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the orchard navigation method based on multi-sensor fusion as described in any one of claims 1-8 is implemented.

Images