Orchard agricultural robot navigation method

Abstract

The invention provides an orchard agricultural robot navigation method, and belongs to the technical field of agricultural robots. The method comprises the following steps: constructing a priori map of an orchard; carrying out odometer initialization and rear end correction on the robot; carrying out ground segmentation and obstacle identification; constructing a cost map: constructing the cost map through static obstacles of a prior map and dynamic obstacles after ground segmentation, and realizing visualization of the obstacles; and path planning: assigning an initial point and an end point on the cost map, firstly realizing global path planning, bypassing an obstacle by using local path planning to realize an obstacle avoidance function when a dynamic obstacle appears in the advancing process, and performing real-time trajectory optimization and smooth control. According to the method, accurate positioning and autonomous navigation of the robot in a complex orchard environment can be realized; meanwhile, through the combination of global path planning and local path planning, the robot can flexibly process static and dynamic obstacles, and the robustness of path planning and the effectiveness of a navigation task are ensured.

Description

Technical Field

[0001] The invention relates to the technical field of agricultural robots, and in particular to a navigation method for an orchard agricultural robot. Background Art

[0002] In orchard automation, agricultural robots have gradually become an important tool for the development of modern agriculture due to their high efficiency and precision. However, the orchard environment is complex and changeable, usually with irregular terrain, dense vegetation, and a variety of dynamic obstacles (such as operating vehicles and personnel), which pose severe challenges to the autonomous navigation of robots. Traditional navigation methods, such as positioning and path planning based on a single sensor, are often difficult to effectively cope with these complex scenarios, especially in semi-structured environments such as orchards where there are dynamic obstacles and the local environment changes frequently. With the rapid development of sensor technologies such as laser radar (LiDAR) and inertial measurement unit (IMU), three-dimensional laser point cloud technology has gradually been applied to the navigation system of agricultural robots. Through three-dimensional point cloud technology, robots can perceive the surrounding environment in real time and accurately identify static and dynamic obstacles in the orchard, thereby providing rich environmental data for path planning and obstacle avoidance. However, how to efficiently fuse these sensor data to achieve global path planning and real-time obstacle avoidance is still a difficult point in the current autonomous robot navigation technology.

[0003] Most existing navigation methods combine simultaneous localization and mapping (SLAM) technology with path planning algorithms to solve the problem of robot positioning and path planning in unknown environments. However, in dynamic and unstructured environments such as orchards, SLAM technology still has problems of cumulative errors and insufficient precision. Especially during long-term operations, the drift problem of IMU will lead to a significant increase in positioning errors, which in turn affects the accuracy and reliability of navigation.

[0004] In addition, existing navigation methods often lack effective obstacle avoidance strategies when facing dynamic obstacles in orchards. The appearance and movement of dynamic obstacles will change the environmental layout of the orchard in real time, which requires the robot to replan the path in a short time, avoid obstacles and continue to perform tasks. However, most of the existing obstacle avoidance algorithms are based on static environment assumptions and are difficult to adapt to the dynamically changing environmental conditions in the orchard.

[0005] Therefore, it is necessary to improve the existing navigation method of the orchard robot to overcome the defects of the prior art. Summary of the invention

[0006] In order to overcome the problems existing in the related technology, one of the purposes of the present invention is to provide an orchard agricultural robot navigation method, which realizes the precise positioning and autonomous navigation of the robot in a complex orchard environment by combining multi-sensor data such as prior maps, lidar and IMU; at the same time, through the combination of global path planning and local path planning, the robot can flexibly handle static and dynamic obstacles, ensuring the robustness of path planning and the effectiveness of navigation tasks.

[0007] A navigation method for an orchard agricultural robot, comprising:

[0008] Construct a priori map of the orchard: collect the original 3D point cloud data of the orchard environment and generate a priori map of the orchard;

[0009] Odometer initialization and backend correction: Use the prior map to initialize the robot's odometer, and correct the odometer's position and posture in real time during the backend optimization stage;

[0010] Ground segmentation and obstacle identification: Use the ground segmentation algorithm to segment the collected original point cloud input into ground point cloud and obstacle point cloud; obstacles include dynamic obstacles and static obstacles;

[0011] Constructing cost map: constructing cost map through static obstacles of prior map and dynamic obstacles after ground segmentation to realize obstacle visualization;

[0012] Path planning: Specify the initial point and the end point on the cost map, implement global path planning first, and use local path planning to bypass the obstacles when dynamic obstacles appear during the movement process to achieve obstacle avoidance function, and perform real-time trajectory optimization and smooth control.

[0013] In a preferred technical solution of the present invention, the construction of a priori map of the orchard includes:

[0014] Determine the origin: select a global initial point in the orchard environment as the origin of the map;

[0015] Deploy the agricultural robot: place the agricultural robot at a global initial point and calibrate the initial position and orientation of the agricultural robot; the agricultural robot has a laser radar (LiDAR) and an inertial measurement unit (IMU);

[0016] Data collection: The agricultural robot starts from the initial point and scans around the orchard. During the scanning process, LiDAR scans the surrounding environment and collects 3D point cloud data. IMU measures the robot's posture and position changes in real time during the movement.

[0017] Map construction: As the robot moves, LiDAR generates local point cloud maps in real time; the local point cloud maps are stitched together to generate a complete prior map.

[0018] In a preferred technical solution of the present invention, the map construction process further includes:

[0019] Multi-sensor fusion: Tightly couple the 3D point cloud data collected by LiDAR with the attitude information of IMU;

[0020] Among them, IMU provides short-term and high-precision pose estimation, and the three-dimensional point cloud collected by LiDAR is matched with the point cloud in the existing map to correct the accumulated error of IMU.

[0021] In a preferred technical solution of the present invention, the process of initializing the robot's odometer using the prior map includes:

[0022] SLAM initialization: Initialize the robot's odometer through the prior map, specify the robot's initial coordinates and orientation in the prior map, and quickly determine the robot's initial position;

[0023] Among them, the prior map provides reference data, which is aligned with the initial point cloud data collected by the robot's lidar to achieve the positioning of the robot's initial posture.

[0024] In a preferred technical solution of the present invention, the real-time correction of the odometer's position and posture in the back-end optimization stage includes:

[0025] During the subsequent movement of the robot, the IMU calculates the current posture of the robot in real time by integration. It also uses the prior map and the local point cloud generated by the lidar scan to perform precise registration and correct the accumulated error of the IMU.

[0026] As the robot moves, the lidar scans the surrounding environment in real time to generate a local map, and aligns the local map with the prior map to achieve continuous correction of the posture, splice the local point cloud data to the global map, and dynamically update the information of the global map.

[0027] Tightly coupling IMU and 3D LiDAR point cloud data enables accurate navigation pose estimation of the robot in complex and bumpy environments.

[0028] In a preferred technical solution of the present invention, the method of using a ground segmentation algorithm to segment the collected original point cloud input into a ground point cloud and an obstacle point cloud includes:

[0029] Project the 3D point cloud data collected by the laser radar onto a 2D plane. Each point cloud data point corresponds to a position on the 2D plane, generating simplified 2D point cloud data.

[0030] A virtual cloth is set above the 2D point cloud data. The initial height of the cloth is determined by the actual terrain of the orchard environment to ensure that the virtual cloth can cover the entire environment.

[0031] The virtual cloth gradually falls under the action of gravity. When the node of the cloth collides with a point in the point cloud data, the cloth node stops falling and attaches to the point.

[0032] When the cloth covers the point cloud data, the point cloud data covered by the cloth is identified as ground point cloud, representing the area where the robot can drive;

[0033] Outlier processing is performed on the identified ground point cloud and obstacle point cloud to remove abnormal points that are obviously deviated from the ground height, ensuring the robustness and accuracy of the segmentation results.

[0034] In a preferred technical solution of the present invention, the cost map is constructed by using the static obstacles of the prior map and the dynamic obstacles after ground segmentation, including:

[0035] The obstacle point cloud of the prior map after removing the ground points is projected onto a two-dimensional plane as the static layer of the cost map, and the fixed obstacles in the orchard are identified;

[0036] The LiDAR scans the environment in real time, and the generated dynamic obstacle point cloud is extracted and projected onto a two-dimensional plane through a ground segmentation algorithm to form the dynamic layer of the cost map;

[0037] The corresponding cost value is assigned according to the distance between the obstacle and the robot; the cost value of static obstacles remains unchanged, while the cost value of dynamic obstacles is updated in real time (to ensure that the cost map can reflect the dynamic changes of the orchard environment in real time and provide accurate environmental data for path planning). The closer the distance, the higher the value (reflecting the impact of the area on the path);

[0038] The data of the static obstacle layer and the dynamic obstacle layer are merged into the cost map to form a cost map containing complete obstacle information;

[0039] In a preferred technical solution of the present invention, the implementation of global path planning includes:

[0040] Based on the established prior map, the A* algorithm is used to generate the optimal path from the robot's current position to the target position. When searching for a path, the A* algorithm treats static obstacles as non-drivable areas to ensure the global optimality of path planning.

[0041] The A* algorithm is used to search for paths on the prior map. The A* algorithm combines heuristic search and cost function to ensure the global optimality of path planning.

[0042] The cost function formula is shown in formula (1):

[0043] f(n)=g(n)+h(n) (1)

[0044] Among them, f(n) is the total cost of node n, g(n) is the actual cost from the starting node to the current node, and h(n) is the heuristic cost estimate from the current node to the target node.

[0045] In a preferred technical solution of the present invention, when a dynamic obstacle appears during the traveling process, the obstacle avoidance function is realized by using local path planning to bypass the obstacle, including:

[0046] As the robot moves along the global path, it perceives dynamic obstacles and unmodeled static obstacles in the surrounding environment in real time;

[0047] Among them, the local path planning adopts the artificial potential field method to construct a potential field model with the target point as the attraction source and the obstacle as the repulsion source;

[0048] The total force of the potential field model is calculated as formula (2):

[0049] F total =F attract +F repulse (2)

[0050] Among them, F attract is the attraction of the target point, which is formula (3):

[0051] F total =-k att *(q―q goal ) (3)

[0052] (3) In the formula, k att is the attraction coefficient, q is the current position of the robot, and q goal is the target point position;

[0053] Obstacle repulsion force: For detected dynamic obstacles, define the repulsion force F repulse is the repulsive force of the obstacle to avoid collision, defined as formula (4):

[0054]

[0055] In formula (4), k att and k rep are the coefficients of attraction and repulsion respectively, q is the current position of the robot, q goal is the target position, d(q) is the distance from the robot to the obstacle, and d thread is the impact range threshold;

[0056] When the robot is closer to an obstacle than a threshold, a repulsive force is generated to avoid collision; when the distance is greater than or equal to the threshold, the repulsive force is zero.

[0057] In a preferred technical solution of the present invention, the robot, during the orchard navigation process, further comprises:

[0058] Coordination of global and local paths: During the robot's movement, local path planning calculates the resultant force and adjusts the robot's movement trajectory in real time based on the perceived environmental information. When the adjustment of the local path planning causes the robot to deviate from the global path, the robot's control system re-updates the global path based on real-time environmental perception data to ensure the robot's navigation continuity and robustness in the complex orchard environment.

[0059] When the adjustment of the local path planning causes the robot to deviate from the global path, the robot's control system re-updates the global path based on real-time environmental perception data, including:

[0060] When the adjustment of the robot trajectory by local path planning deviates from the global path, the robot's control system calls the A* algorithm again through the environment update mechanism to generate a new optimal path.

[0061] The beneficial effects of the present invention are:

[0062] The invention provides an orchard agricultural robot navigation method, which comprises constructing an a priori map of an orchard: collecting original three-dimensional point cloud data of an orchard environment to generate an a priori map of the orchard; odometer initialization and back-end correction: using the a priori map to initialize the odometer of the robot, and correcting the odometer's posture in real time during the back-end optimization stage; ground segmentation and obstacle recognition: using a ground segmentation algorithm to segment the collected original point cloud input into a ground point cloud and an obstacle point cloud; wherein the obstacles include dynamic obstacles and static obstacles; constructing a cost map: constructing a cost map through the static obstacles of the a priori map and the dynamic obstacles after ground segmentation to realize visualization of the obstacles; and path planning: specifying an initial point and an end point on the cost map, first realizing global path planning, and using local path planning to bypass the obstacles when dynamic obstacles appear during the traveling process to realize obstacle avoidance function, and performing real-time trajectory optimization and smooth control. This method can significantly improve the positioning accuracy of the robot in a complex orchard environment and reduce navigation errors caused by environmental changes by fusing prior maps with multi-sensor data. The combination of ground segmentation and obstacle recognition technology enables the robot to accurately distinguish between the ground and obstacles, especially the instant recognition and response of dynamic obstacles, greatly improving the robot's obstacle avoidance efficiency and safety. During the navigation process, the combination of global and local path planning enables the robot to flexibly adjust the path according to the current environment, which not only ensures the overall operation efficiency, but also can respond quickly when encountering emergencies, ensuring the effective completion of the navigation task. The application of this navigation method realizes the automation and intelligence of orchard operations, helps to reduce the manpower burden, improves the operation accuracy and efficiency, and provides strong support for the precise management of modern orchards. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 is a flow chart of an orchard agricultural robot navigation method provided by an embodiment of the present invention;

[0064] Figure 2 is a flow chart of constructing a priori map of an orchard provided by an embodiment of the present invention;

[0065] Figure 3 is a flow chart of ground segmentation and obstacle identification provided by an embodiment of the present invention;

[0066] Figure 4 is a flowchart of constructing a cost map provided by an embodiment of the present invention;

[0067] Figure 5 It is a logic diagram of the orchard agricultural robot navigation method provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0068] The preferred embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. Although the preferred embodiments of the present invention are shown in the accompanying drawings, it should be understood that the present invention can be implemented in various forms and should not be limited by the embodiments described herein. On the contrary, these embodiments are provided to make the present invention more thorough and complete, and to fully convey the scope of the present invention to those skilled in the art.

[0069] Most of the existing navigation methods used by orchard robots combine simultaneous localization and mapping (SLAM) technology with path planning algorithms to solve the positioning and path planning problems of robots in unknown environments. However, in dynamic and unstructured environments such as orchards, SLAM technology still has problems of cumulative errors and insufficient precision. Especially during long-term operations, the drift problem of IMU will lead to a significant increase in positioning errors, which in turn affects the accuracy and reliability of navigation. In addition, existing navigation methods often lack effective obstacle avoidance strategies when facing dynamic obstacles in orchards. The appearance and movement of dynamic obstacles will change the environmental layout of the orchard in real time, which requires the robot to be able to replan the path in a short time, avoid obstacles and continue to perform tasks. However, most of the existing obstacle avoidance algorithms are based on static environment assumptions and are difficult to adapt to the dynamically changing environmental conditions in orchards.

[0070] Based on this, the present application provides an orchard agricultural robot navigation method.

[0071] See also Figure 1-Figure 5The method for navigating an orchard agricultural robot comprises: S100, constructing a priori map of the orchard: collecting original three-dimensional point cloud data of the orchard environment to generate a priori map of the orchard; specifically, a drone equipped with a high-precision laser radar and an optical camera can be used to perform an all-round scan of the orchard to collect three-dimensional point cloud data of the orchard environment. The collected data is processed using SLAM (Simultaneous Localization and Mapping) technology to generate a priori map of the orchard containing detailed information such as the location of fruit trees, row spacing, and terrain undulations.

[0072] S200, odometer initialization and back-end correction: use the prior map to initialize the robot's odometer, and correct the odometer's position and posture in real time during the back-end optimization stage;

[0073] S300, ground segmentation and obstacle identification: using a ground segmentation algorithm to segment the collected original point cloud input into a ground point cloud and an obstacle point cloud; wherein the obstacles include dynamic obstacles and static obstacles;

[0074] S400, constructing a cost map: constructing a cost map through static obstacles in the prior map and dynamic obstacles after ground segmentation to achieve visualization of obstacles;

[0075] S500, path planning: specify the initial point and the end point on the cost map, first implement global path planning, and use local path planning to bypass the obstacles when dynamic obstacles appear during the movement to achieve obstacle avoidance function, and perform real-time trajectory optimization and smooth control.

[0076] The above-mentioned navigation method for an orchard agricultural robot can significantly improve the positioning accuracy of the robot in a complex orchard environment and reduce navigation errors caused by environmental changes by integrating prior maps with multi-sensor data. The combination of ground segmentation and obstacle recognition technology enables the robot to accurately distinguish between the ground and obstacles, especially the instant recognition and response of dynamic obstacles, greatly improving the robot's obstacle avoidance efficiency and safety. During the navigation process, through the combination of global and local path planning, the robot can flexibly adjust the path according to the current environment, which not only ensures the overall operation efficiency, but also can respond quickly when encountering emergencies, ensuring the effective completion of the navigation task. The application of this navigation method realizes the automation and intelligence of orchard operations, helps to reduce the manpower burden, improve operation accuracy and efficiency, and provides strong support for the precise management of modern orchards.

[0077] Specifically, constructing a priori map of the orchard includes:

[0078] S101, determining the origin: selecting a global initial point in the orchard environment as the origin of the map; the origin of the map may be a global initial point that is easy to identify and has a fixed position.

[0079] S102, deploying an agricultural robot: placing the agricultural robot at a global initial point, and calibrating the initial position and orientation of the agricultural robot; the agricultural robot has a laser radar (LiDAR) and an inertial measurement unit (IMU); specifically, GPS and manual calibration can be used to ensure that the initial position and orientation of the robot are accurate.

[0080] S103, Data Collection: The agricultural robot starts from the initial point and scans around the orchard. During the scanning process, LiDAR scans the surrounding environment and collects three-dimensional point cloud data. IMU measures the robot's posture and position changes during movement in real time. During the scanning process, the robot starts from the initial point and scans around the orchard according to the preset path or autonomous exploration mode. IMU measures the robot's acceleration, angular velocity, etc. during movement in real time, and obtains the robot's posture and position change information through algorithm calculation.

[0081] During this process, the LiDAR continuously emits laser pulses and receives reflected signals, recording in detail three-dimensional point cloud data such as the position of trees in the orchard, ground undulations, and obstacle contours.

[0082] S104, map construction: As the robot moves, the LiDAR generates local point cloud maps in real time; the local point cloud maps are stitched together to generate a complete prior map. Specifically, through the point cloud stitching algorithm, these local maps are seamlessly stitched together to form a complete orchard prior map.

[0083] More preferably, the map construction process further includes:

[0084] Multi-sensor fusion: Tightly couple the 3D point cloud data collected by LiDAR with the attitude information of IMU;

[0085] Among them, IMU provides short-term and high-precision pose estimation, and the three-dimensional point cloud collected by LiDAR is matched with the point cloud in the existing map to correct the accumulated error of IMU.

[0086] The IMU provides short-term, high-precision pose estimation as the initial value for LiDAR data registration; the three-dimensional point cloud collected by the LiDAR performs feature matching with the point cloud in the existing map to correct the accumulated error caused by the IMU's long-term integration, thereby ensuring the global consistency and accuracy of the map.

[0087] Furthermore, the method of initializing the robot's odometer using the prior map includes:

[0088] SLAM initialization: Initialize the robot's odometer through the prior map, specify the robot's initial coordinates and orientation in the prior map, and quickly determine the robot's initial position; in actual applications, by specifying the robot's initial position and orientation, the prior map can be used to calibrate the absolute position of the radar system in the orchard.

[0089] Among them, the prior map provides reference data, which is aligned with the initial point cloud data collected by the robot's lidar to achieve the positioning of the robot's initial posture. The prior map provides reference data of the orchard environment, which is aligned with the initial point cloud data collected by the robot's lidar to achieve accurate positioning of the robot's initial posture. This process quickly determines the robot's absolute position in the orchard and provides a reliable starting point for subsequent navigation.

[0090] The real-time correction of the odometer's position and posture during the back-end optimization stage includes:

[0091] During the subsequent movement of the robot, the IMU calculates the current posture of the robot in real time by integration. It also uses the prior map and the local point cloud generated by the lidar scan to perform precise registration and correct the accumulated error of the IMU.

[0092] During the robot's movement, the LiDAR scans the surrounding environment in real time to generate a local map, and aligns the local map with the prior map to achieve continuous correction of the posture, splice the local point cloud data to the global map, and dynamically update the information of the global map. The LiDAR scans the surrounding environment in real time to generate a local point cloud map. These local point cloud maps are precisely aligned with the prior map, and the transformation matrix of the local point cloud relative to the prior map is calculated through algorithms such as feature matching and iterative closest point (ICP), and the accumulated error caused by the IMU integration is corrected.

[0093] During the robot's movement, the above-mentioned local point cloud generation and registration process is repeated continuously to achieve continuous correction of the posture. The corrected local point cloud data is spliced ​​into the global map, and the information of the global map is dynamically updated to ensure the real-time and accuracy of the map.

[0094] By tightly coupling the IMU and 3D LiDAR point cloud data, the robot can accurately estimate its navigation pose in a complex and bumpy orchard environment. This tightly coupled method effectively suppresses environmental noise and interference, and improves the stability and accuracy of the navigation system.

[0095] More specifically, the method of using a ground segmentation algorithm to segment the collected original point cloud input into a ground point cloud and an obstacle point cloud includes:

[0096] S301, projecting the three-dimensional point cloud data collected by the laser radar onto a two-dimensional plane, with each point cloud data point corresponding to a position on the two-dimensional plane, to generate simplified two-dimensional point cloud data; this process reduces the complexity of the data and facilitates subsequent ground segmentation processing.

[0097] S302, setting a piece of virtual cloth above the two-dimensional point cloud data. The initial height of the cloth is determined by the actual terrain of the orchard environment to ensure that the virtual cloth can cover the entire environment. This step provides a basis for the subsequent falling of the cloth and ground point cloud recognition.

[0098] S303, the virtual cloth gradually falls under the action of gravity. When the node of the cloth collides with a point in the point cloud data, the node of the cloth stops falling and attaches to the point. Through the falling of the virtual cloth, the cloth can gradually fit the ground point cloud, and the ground and obstacles are initially separated. Through the method of virtual cloth falling and collision detection, the ground point cloud and obstacle point cloud can be accurately identified, which improves the precision and accuracy of ground identification.

[0099] S304, when the cloth covers the point cloud data, the point cloud data covered by the cloth is identified as ground point cloud, representing the area where the robot can drive; the point cloud data provides important environmental information for subsequent navigation and path planning.

[0100] S305: Perform outlier processing on the identified ground point cloud and obstacle point cloud, remove outlier points that are obviously deviated from the ground height, and ensure the robustness and accuracy of the segmentation results. This can improve the accuracy and reliability of ground segmentation and provide a more accurate environment model for the robot's navigation and operation.

[0101] Furthermore, the cost map is constructed by using the static obstacles of the prior map and the dynamic obstacles after ground segmentation, including:

[0102] S401, projecting the obstacle point cloud of the prior map after removing the ground points onto a two-dimensional plane as the static layer of the cost map, and identifying the fixed obstacles in the orchard; fixed obstacles include trees, buildings, etc., and the positions and shapes of these obstacles remain unchanged for a long time.

[0103] S402, the laser radar scans the environment in real time, and the generated dynamic obstacle point cloud is extracted and projected onto a two-dimensional plane through a ground segmentation algorithm to form a dynamic layer of the cost map; the dynamic layer can reflect the moving or newly appearing obstacles in the orchard environment, such as pedestrians, animals, etc., in real time.

[0104] S403. Assign corresponding cost values ​​according to the distance between the obstacle and the robot; the cost value of static obstacles remains unchanged to reflect their long-term existence; the cost value of dynamic obstacles will be updated in real time (to ensure that the cost map can reflect the dynamic changes of the orchard environment in real time and provide accurate environmental data for path planning), and the closer the distance, the higher the value (reflecting the impact of the area on the path); the cost value of dynamic obstacles is updated in real time to ensure that the cost map can reflect the dynamic changes of the orchard environment in real time. The closer the obstacle, the higher its cost value, to reflect the greater impact of the area on path planning.

[0105] S404: Merge the data of the static obstacle layer and the dynamic obstacle layer into the cost map to form a cost map containing complete obstacle information.

[0106] The cost map provides comprehensive environmental data support for subsequent path planning, ensuring that the robot can accurately avoid obstacles during navigation.

[0107] Specifically, the implementation of global path planning includes:

[0108] Based on the established prior map, the A* algorithm is used to generate the optimal path from the robot's current position to the target position. The prior map contains detailed information about the orchard environment, including the position and shape of static obstacles. When searching for a path, the A* algorithm treats static obstacles as non-drivable areas to avoid collisions between the robot and obstacles, thus ensuring the global optimality of path planning.

[0109] The A* algorithm is used to search for paths on the prior map. The A* algorithm combines heuristic search and cost function to ensure the global optimality of path planning.

[0110] The cost function formula is shown in formula (1):

[0111] f(n)=g(n)+h(n) (1)

[0112] Among them, f(n) is the total cost of node n, g(n) is the actual cost from the starting node to the current node, and h(n) is the heuristic cost estimate from the current node to the target node.

[0113] In actual applications, by comparing the total costs of different nodes, the A* algorithm can select the path with the lowest cost, that is, the optimal path. The A* algorithm searches and generates the optimal path from the robot's current position to the target position on the prior map. The generated optimal path is output to the robot to guide the robot to navigate and operate in the orchard environment.

[0114] Furthermore, when a dynamic obstacle appears during the traveling process, the obstacle avoidance function is realized by using local path planning to bypass the obstacle, including:

[0115] As the robot moves along the global path, it perceives dynamic obstacles and unmodeled static obstacles in the surrounding environment in real time;

[0116] When the robot moves along the global path, it uses sensors to perceive dynamic obstacles and unmodeled static obstacles in the surrounding environment in real time. Through data processing and analysis, it obtains information such as the position, speed, and direction of movement of obstacles, providing accurate environmental data support for subsequent local path planning.

[0117] Among them, the local path planning adopts the artificial potential field method to construct a potential field model with the target point as the attraction source and the obstacle as the repulsion source;

[0118] The total force of the potential field model is calculated as formula (2):

[0119] F total =F attract +F repulse (2)

[0120] Among them, F attract is the attraction of the target point, which is formula (3):

[0121] F total =-k att *(q―q goal ) (3)

[0122] (3) In the formula, k att is the attraction coefficient, q is the current position of the robot, and q goal is the target point position;

[0123] Obstacle repulsion force: For detected dynamic obstacles, define the repulsion force F repulse is the repulsive force of the obstacle to avoid collision, defined as formula (4):

[0124]

[0125] In formula (4), k att and k rep are the coefficients of attraction and repulsion respectively, q is the current position of the robot, q goal is the target position, d(q) is the distance from the robot to the obstacle, and d thread is the impact range threshold; the repulsive force is inversely proportional to the distance from the robot to the obstacle, and the repulsive force coefficient k rep Determines the strength of the repulsive force.

[0126] Calculate the attraction and repulsion of the robot's current position to determine the robot's direction and speed. When the robot is less than the threshold distance from the obstacle, a repulsion force will be generated to avoid collision; when the distance is greater than or equal to the threshold, the repulsion force is zero;

[0127] This application calculates the attractive and repulsive forces in real time, and adjusts the robot's movement direction and speed according to the total force, so that the robot can bypass dynamic obstacles and unmodeled static obstacles to achieve obstacle avoidance. When the robot encounters multiple obstacles, it can dynamically adjust according to the total force of the potential field model and select the optimal obstacle avoidance path. This method can adapt to the ever-changing situation in the orchard environment, such as the appearance of dynamic obstacles and the existence of unmodeled static obstacles, so that the robot can complete navigation and operation tasks safely and efficiently in a complex environment. In addition, through local path planning, the robot can dynamically adjust the movement direction and speed according to real-time environmental information, avoid unnecessary collisions and stagnation, and improve navigation efficiency.

[0128] Furthermore, the robot's navigation in the orchard also includes:

[0129] Coordination of global and local paths: During the robot's movement, local path planning calculates the resultant force and adjusts the robot's movement trajectory in real time based on the perceived environmental information. When the adjustment of the local path planning causes the robot to deviate from the global path, the robot's control system re-updates the global path based on real-time environmental perception data to ensure the robot's navigation continuity and robustness in the complex orchard environment.

[0130] When the adjustment of the local path planning causes the robot to deviate from the global path, the robot's control system re-updates the global path based on real-time environmental perception data, including:

[0131] When the adjustment of the robot trajectory by local path planning deviates from the global path, the robot's control system calls the A* algorithm again through the environment update mechanism to generate a new optimal path. The robot's control system controls the robot's moving direction and speed based on the planning results of the global path and local path, achieving accurate navigation and efficient operation of the robot in the orchard environment.

[0132] Through the coordination of global and local paths, this method enables the robot to flexibly respond to environmental changes during navigation, ensure the continuity of navigation, and avoid global path interruption or deviation due to local obstacle avoidance.

[0133] In addition, it should be noted that the use of terms such as "first" and "second" to limit components is only for the convenience of distinguishing the corresponding components. Unless otherwise stated, the above terms have no special meaning and therefore cannot be understood as limiting the scope of protection of this application.

[0134] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A navigation method for an orchard agricultural robot, characterized in that: include: Construct a priori map of the orchard: collect the original 3D point cloud data of the orchard environment and generate a priori map of the orchard; Odometer initialization and backend correction: Use the prior map to initialize the robot's odometer, and correct the odometer's position and posture in real time during the backend optimization stage; Ground segmentation and obstacle recognition: Use the ground segmentation algorithm to segment the collected original point cloud input into ground point cloud and obstacle point cloud; The obstacles include dynamic obstacles and static obstacles; Constructing cost map: constructing cost map through static obstacles of prior map and dynamic obstacles after ground segmentation to realize obstacle visualization; Path planning: Specify the initial point and the end point on the cost map, implement global path planning first, and use local path planning to bypass the obstacles when dynamic obstacles appear during the movement process to achieve obstacle avoidance function, and perform real-time trajectory optimization and smooth control.

2. The orchard agricultural robot navigation method according to claim 1, characterized in that: The step of constructing a priori map of an orchard comprises: Determine the origin: select a global initial point in the orchard environment as the origin of the map; Deploy the agricultural robot: place the agricultural robot at a global initial point and calibrate the initial position and orientation of the agricultural robot; the agricultural robot has a laser radar and an inertial measurement unit; Data collection: The agricultural robot starts from the initial point and scans around the orchard. During the scanning process, Li DAR scans the surrounding environment and collects 3D point cloud data. IMU measures the robot's posture and position changes in real time during the movement. Map construction: As the robot moves, LiDAR generates local point cloud maps in real time; the local point cloud maps are stitched together to generate a complete prior map.

3. The orchard agricultural robot navigation method according to claim 2, characterized in that: The map construction process also includes: Multi-sensor fusion: Tightly couple the 3D point cloud data collected by LiDAR with the attitude information of IMU; Among them, IMU provides short-term and high-precision pose estimation, and the three-dimensional point cloud collected by LiDAR is matched with the point cloud in the existing map to correct the accumulated error of IMU.

4. The orchard agricultural robot navigation method according to claim 1, characterized in that: The method of initializing the robot's odometer using the prior map includes: SLAM initialization: Initialize the robot's odometer through the prior map, specify the robot's initial coordinates and orientation in the prior map, and quickly determine the robot's initial position; Among them, the prior map provides reference data, which is aligned with the initial point cloud data collected by the robot's lidar to achieve the positioning of the robot's initial posture.

5. The orchard agricultural robot navigation method according to claim 1, characterized in that: The real-time correction of the odometer's position and posture during the back-end optimization stage includes: During the subsequent movement of the robot, the IMU calculates the current posture of the robot in real time by integration. It also uses the prior map and the local point cloud generated by the lidar scan to perform precise registration and correct the accumulated error of the IMU. When the robot is moving, the laser radar scans the surrounding environment in real time to generate a local map, and aligns the local map with the prior map to achieve continuous correction of the posture, splice the local point cloud data to the global map, and dynamically update the information of the global map; Tightly coupling IMU and 3D LiDAR point cloud data enables accurate navigation pose estimation of the robot in complex and bumpy environments.

6. The orchard agricultural robot navigation method according to any one of claims 1 to 5, characterized in that: The method of using a ground segmentation algorithm to segment the collected original point cloud input into a ground point cloud and an obstacle point cloud includes: Project the 3D point cloud data collected by the LiDAR onto a 2D plane. Each point cloud data point corresponds to a position on the 2D plane, generating simplified 2D point cloud data. A virtual cloth is set above the 2D point cloud data. The initial height of the cloth is determined by the actual terrain of the orchard environment to ensure that the virtual cloth can cover the entire environment. The virtual cloth gradually falls under the action of gravity. When the node of the cloth collides with a point in the point cloud data, the cloth node stops falling and attaches to the point. When the cloth covers the point cloud data, the point cloud data covered by the cloth is identified as ground point cloud, representing the area where the robot can drive; Outlier processing is performed on the identified ground point cloud and obstacle point cloud to remove abnormal points that are obviously deviated from the ground height, ensuring the robustness and accuracy of the segmentation results.

7. The orchard agricultural robot navigation method according to any one of claims 1 to 5, characterized in that: The cost map is constructed by using the static obstacles of the prior map and the dynamic obstacles after ground segmentation, including: The obstacle point cloud of the prior map after removing the ground points is projected onto a two-dimensional plane as the static layer of the cost map, and the fixed obstacles in the orchard are identified; The LiDAR scans the environment in real time, and the generated dynamic obstacle point cloud is extracted and projected onto a two-dimensional plane through a ground segmentation algorithm to form the dynamic layer of the cost map; The corresponding cost value is assigned according to the distance between the obstacle and the robot. The cost value of static obstacles remains unchanged, while the cost value of dynamic obstacles is updated in real time. The closer the distance, the higher the value. The data of the static obstacle layer and the dynamic obstacle layer are fused into the cost map to form a cost map containing complete obstacle information.

8. The orchard agricultural robot navigation method according to claim 1, characterized in that: The implementation of global path planning includes: Based on the established prior map, the A* algorithm is used to generate the optimal path from the robot's current position to the target position. When searching for a path, the A* algorithm treats static obstacles as non-drivable areas to ensure the global optimality of path planning. The A* algorithm is used to search for paths on the prior map. The A* algorithm combines heuristic search and cost function to ensure the global optimality of path planning. The cost function formula is shown in formula (1): f(n)=g(n)+h(n) (1) Among them, f(n) is the total cost of node n, g(n) is the actual cost from the starting node to the current node, and h(n) is the heuristic cost estimate from the current node to the target node.

9. The orchard agricultural robot navigation method according to claim 1, characterized in that: When a dynamic obstacle appears during the moving process, the obstacle avoidance function is realized by using local path planning to bypass the obstacle, including: As the robot moves along the global path, it perceives dynamic obstacles and unmodeled static obstacles in the surrounding environment in real time; Among them, the local path planning adopts the artificial potential field method to construct a potential field model with the target point as the attraction source and the obstacle as the repulsion source; The total force of the potential field model is calculated as formula (2): F total =F attract +F repulse (2) Among them, F attract is the attraction of the target point, which is formula (3): F total =―k att *(q―q goal ) (3) (3) In the formula, k att is the attraction coefficient, q is the current position of the robot, and q goal is the target point position; Obstacle repulsion force: For detected dynamic obstacles, define the repulsion force F repulse is the repulsive force of the obstacle to avoid collision, defined as formula (4): In formula (4), k att and k rep are the coefficients of attraction and repulsion respectively, q is the current position of the robot, q goal is the target position, d(q) is the distance from the robot to the obstacle, and d thread is the impact range threshold; When the robot is closer to an obstacle than a threshold, a repulsive force is generated to avoid collision; when the distance is greater than or equal to the threshold, the repulsive force is zero.

10. The orchard agricultural robot navigation method according to claim 9, characterized in that: The robot's navigation process in the orchard also includes: Coordination of global and local paths: During the robot's movement, local path planning calculates the resultant force and adjusts the robot's movement trajectory in real time based on the perceived environmental information. When the adjustment of the local path planning causes the robot to deviate from the global path, the robot's control system re-updates the global path based on real-time environmental perception data to ensure the robot's navigation continuity and robustness in the complex orchard environment. When the adjustment of the local path planning causes the robot to deviate from the global path, the robot's control system re-updates the global path based on real-time environmental perception data, including: When the adjustment of the robot trajectory by local path planning deviates from the global path, the robot's control system calls the A* algorithm again through the environment update mechanism to generate a new optimal path.

Images