Unmanned picking robot path planning algorithm for smart greenhouse

Abstract

The invention provides an unmanned picking robot path planning algorithm for a smart greenhouse, and the algorithm comprises the steps: initializing a map space, and obtaining the poses of a starting point and a target point, basic parameters of a robot, algorithm parameters, and obstacle information in the environment; the optimal distance between the robot and the target is calculated according to the obstacle information and the algorithm parameters; based on the optimal distance, the starting point and the target point position poses, an improved A * algorithm is adopted to introduce an obstacle adsorption strategy, path points are generated through cost function calculation and iteration, and an initial path is obtained; screening redundant path points in the initial path, and trimming and optimizing the path through curvature analysis and a neighborhood search method to obtain a final smooth, safe and efficient robot navigation path.

Description

Technical Field

[0001] The present invention relates to the field of information technology, and particularly to a path planning algorithm for an unmanned picking robot used in a smart greenhouse. Background Art

[0002] Implementing efficient and intelligent path planning for an unmanned picking robot in a complex orchard environment faces a series of technical challenges. The primary problem is how to design a safe and efficient picking path for the robot in an orchard with dense obstacles and variable terrain. This involves how the path planning algorithm balances the contradiction between obstacle avoidance and picking efficiency. Traditional methods often struggle to achieve both goals simultaneously. They are either too conservative, resulting in low picking efficiency, or too adventurous, increasing the risk of collisions. Secondly, the three-dimensional structure of fruit trees and the irregular distribution of fruits pose difficulties in precise positioning. Determining the optimal picking distance and position is a key issue. In addition, the dynamic changes in the orchard environment, such as the growth status of fruits and weather factors, also bring uncertainties to path planning. The robot needs to be able to perceive environmental changes in real time and dynamically adjust the path, which poses high requirements for the real-time performance and adaptability of the algorithm. Finally, how to maximize the picking coverage rate while ensuring path safety and efficiency, and avoid missed or repeated picking, is also an issue that needs to be deeply considered. Solving these technical problems is crucial for improving the application effect of unmanned picking robots in actual orchard environments. Summary of the Invention

[0003] The present invention provides a path planning algorithm for an unmanned picking robot used in a smart greenhouse, mainly including:

[0004] Initializing the map space, obtaining the poses of the starting point and the target point, the basic parameters of the robot, the algorithm parameters, and the obstacle information in the environment; calculating the optimal distance between the robot and the target according to the obstacle information and the algorithm parameters; based on the optimal distance and the poses of the starting point and the target point, introducing an obstacle adsorption strategy using an improved A* algorithm, generating path points through cost function calculation and iteration to obtain an initial path; screening redundant path points in the initial path, and pruning and optimizing the path through curvature analysis and neighborhood search methods to obtain a final smooth, safe, and efficient robot navigation path.

[0005] Further, the initialization of the map space includes: calibrating the map boundary, dividing grids, and obtaining the obstacle attribute information of each grid; collecting three-dimensional data of the greenhouse scene to construct map elevation information; extracting the contour of the fruit and vegetable planting area, extracting the position distribution information of plants in the area, and establishing map semantic information.

[0006] Further, obtaining the poses of the starting point and the target point, the basic parameters of the robot, the algorithm parameters, and the information about obstacles in the environment includes: obtaining the spatial positions, sizes, etc. of the obstacles around the robot through lidar scanning and converting them into information in the map coordinate system; determining the target picking area according to the greenhouse operation process and obtaining the coordinates of the target picking points in this area; obtaining the size parameters, kinematic constraints, etc. of the robot itself; setting parameters such as the search step size and the obstacle influence factor of the A* algorithm.

[0007] Further, calculating the optimal distance between the robot and the target includes: obtaining the average crown size of the plants in the target picking area and reserving a certain margin as the safety navigation gap for the robot; determining the working range of the robotic arm and obtaining its optimal working distance; comprehensively considering the plants and the working range of the robotic arm to determine the optimal operation distance between the robot and the obstacles.

[0008] Further, calculating and iteratively generating path points through the cost function includes: radiating and searching with the starting point as the core, constructing Open and Close lists, and screening passable adjacent nodes; introducing a dual cost function of tradition and obstacle influence to evaluate the comprehensive cost of each node; selecting the node with the minimum cost as the next hop, updating the Close list and the current node, and iteratively optimizing until the target point.

[0009] Further, screening the initial path points includes: calculating the curvature of three adjacent points in the initial path, and if the curvature is less than the threshold, it is regarded as a redundant path point; screening the redundant path points, removing the nodes with less influence, and retaining the key inflection points and safety nodes.

[0010] Further, pruning and optimizing the path through curvature analysis and neighborhood search method includes: taking the screened key path nodes as the benchmark, conducting path search within the neighborhood; analyzing the slope change of adjacent path points, and merging and simplifying the path segments with smaller curvature; fitting the path points with a Bezier curve to smooth the trajectory and complete the pruning and optimization of the path.

[0011] The technical solutions provided by the embodiments of the present invention may include the following beneficial effects:

[0012] The present invention discloses an intelligent path planning method for unmanned picking robots. Aiming at the complex obstacle distribution in the orchard environment and the special requirements of picking operations, the present invention first obtains the map space information and robot parameters to determine the optimal picking distance. Then, by combining the Manhattan distance and the Euclidean distance, the cost of reachable points is calculated, and considering the influence of the obstacle expansion layer, the path selection is dynamically adjusted. On this basis, the present invention generates an initial path in an iterative manner and optimizes it by removing linearly related path points, and finally obtains a simplified final path. This method can not only effectively avoid obstacles, but also ensure the efficiency of picking operations, realizing the intelligent path planning of unmanned picking robots in complex orchard environments and improving the picking efficiency and safety. Brief Description of the Drawings

[0013] Figure 1 It is a flowchart of a path planning algorithm for an unmanned picking robot used in a smart greenhouse according to the present invention. Detailed Embodiment

[0014] To further understand the content of the present invention, the present invention will be described in detail in combination with the drawings and embodiments. The following further describes the present application in detail with reference to the drawings and embodiments. It can be understood that the specific embodiments described herein are only used to explain the related invention, rather than limiting the invention. In addition, it should be noted that only the parts related to the invention are shown in the drawings for the convenience of description.

[0015] As Figure 1 , a path planning algorithm for an unmanned picking robot used in a smart greenhouse in this embodiment may specifically include:

[0016] S101. Obtain map space information, where the map space information includes the starting point position, the target point position, and obstacle information.

[0017] Cooperate with multi-modal perception devices such as depth vision sensors and lidar to scan the greenhouse environment and construct a three-dimensional digital map. According to the requirements of the picking task, extract the position coordinate information of the starting point and the target point from the environmental map. Through environmental scanning, obtain the spatial position, shape outline and other attribute information of the obstacles, and mark the obstacles such as fixed facilities, temporary stacking objects, and dense plant areas on the map to form the map spatial information containing key navigation nodes. Based on the map spatial information, perform semantic segmentation on the environment to identify the passable area and the obstacle area. Use image processing algorithms to extract the edge features of the obstacles and obtain accurate obstacle contour information. For the identified obstacles, calculate their spatial dimensions through point cloud data analysis. Combine the robot body parameter information and set a certain width of the inflation layer around the obstacles to avoid collisions between the robot and the obstacles. Integrate the starting point, target point positions and the inflated obstacle information to construct a constraint map spatial model for path planning. Based on this model, divide the path search space and discretize the continuous space by the grid method. With the inflated obstacle contour boundary as the constraint, judge the passability of each grid cell. If the grid is covered by obstacles, mark it as impassable; otherwise, mark it as passable. Thus, a discrete search space reflecting environmental constraints is formed. Explore the feasible path from the starting point to the target point in the discrete space through the graph search algorithm. Take the center point of the grid cell as the node and connect the adjacent passable grids as the edges to construct an undirected graph model for path search. Use the distance of the robot moving between adjacent grids as the edge cost and the Manhattan distance from the starting point to the target point as the heuristic function, and use the A* algorithm for heuristic search to find the path with the minimum cost from the starting point to the target point in the graph. Smooth the optimal path obtained by the A* algorithm search to remove redundant nodes. According to Dubin's PA*th theory and combined with the robot kinematic constraints, generate a navigation path that meets smoothness and executability as the motion instruction output of the greenhouse picking robot.

[0018] Specifically, by means of collaborative perception of the Intel ReA*lSense D435 depth camera and the Velodyne VLP-16 lidar, color images and point cloud data of the greenhouse environment are collected at a frequency of 30 Hz. A three-dimensional grid map with a resolution of 5 cm is constructed through point cloud stitching and the RGB-D SLA*M algorithm, and the coordinates of the starting point at (2.5 m, 3.0 m) and the target point at (18.5 m, 12.8 m) are extracted from it. Eight fixed planting beds, three temporary storage areas, and four densely planted areas are identified through point cloud clustering segmentation and labeled on the grid map. The semantic segmentation of the environmental color map is performed using the image segmentation network MA*sk R-CNN. The edge contours of obstacles are extracted by combining point cloud Euclidean clustering. The three-dimensional dimensions of the obstacles are calculated through bounding boxes. Considering the robot's body parameters of 0.8 m × 1.2 m, an inflation layer of 0.4 m is set around the obstacles. The starting point, target point, and inflation layer information are fused into the grid map to construct a path search constraint space with a resolution of 0.1 m. Each grid cell is marked as passable (0) or impassable (1) according to the coverage of the inflation layer obstacles. An undirected graph is constructed with the center points of the grid cells as nodes and the Euclidean distance as the edge cost. The A** algorithm is used to perform path search with 1.2 times the diagonal length as the heuristic function to obtain the optimal path with the minimum total cost. Finally, the A** solution path is smoothed using the cubic spline interpolation method, and with reference to the Dubin's PA*th model, a control instruction sequence with G1 continuity is generated at a speed of 1 m / s and a turning rate of 0.5 rA*d / s as the navigation control output of the picking robot.

[0019] S102. Obtain the parameter information of the unmanned picking robot, where the parameter information includes the basic vehicle parameters and algorithm parameters.

[0020] Obtain the parameter information of the unmanned picking robot, including basic parameters such as the robot's external dimensions, wheelbase, minimum turning radius, maximum driving speed, maximum acceleration, etc., and algorithm parameters such as the heuristic function weight and search step size of the improved A* algorithm. According to the obtained basic robot parameters, determine the kinematic constraint conditions of the robot, such as the minimum turning radius is 0.6 m, the maximum driving speed is 1.2 m / s, and the maximum acceleration is 0.5 m / s 2Etc. The image data of the greenhouse environment is collected by a binocular stereo vision camera, and the machine vision processing library OpenCV is used to preprocess the images to extract the position information of the fruit and vegetable crops. A lidar is used to achieve 360° omnidirectional obstacle detection, obtain the distribution of obstacles in the greenhouse environment, and construct an environmental map model. The constructed map is edited and optimized in the ROS visualization tool, and the starting point, target point positions, and various obstacle area attribute information are marked to complete the initialization settings of the experimental environment. Considering the robot kinematic constraints and the distribution of environmental obstacles, the heuristic function weight and search step parameters of the improved A* algorithm are dynamically adjusted, and an obstacle adsorption strategy is introduced to optimize the path search process. If the constraints for the robot to safely avoid obstacles are met, a path close to the fruit and vegetable planting area is preferentially planned to improve the picking efficiency and visual recognition accuracy; otherwise, the path is adjusted to ensure the safety of the robot. The generated path is smoothed and optimized, and the redundant path points are removed to improve the smoothness of the robot's movement, obtaining a series of smooth, efficient, and safe path point coordinates and attitude information. The optimized path information is published to the robot navigation control node to control the robot to autonomously navigate along the planned path, realizing intelligent unmanned picking operations.

[0021] Specifically, the parameter information of the unmanned picking robot is read through a configuration file. For example, the external dimensions of the robot are 1.2 m in length, 0.8 m in width, and 1.5 m in height, the wheelbase is 0.9 m, the minimum turning radius is configured as 0.6 m, the maximum driving speed is set to 1.2 m / s, and the maximum acceleration is set to 0.5 m / s 2 , and the heuristic function weight of the improved A** algorithm is set to 1.5, and the search step is set to 0.1 m. Based on these parameters, the kinematic constraints of the robot are determined. For example, the minimum turning radius is restricted to 0.6 m, the maximum driving speed does not exceed 1.2 m / s, and the maximum acceleration does not exceed 0.5 m / s 2。An RGB image with a resolution of 1280x720 is collected by a binocular stereo vision camera. The disparity map is calculated through the StereoSGBM algorithm of the OpenCV library. Combining with the camera calibration parameters, point cloud data is generated, and an apple 2m away from the robot is identified, with its three-dimensional coordinates being (2.0, 0.5, 1.2). The surrounding environment is scanned by a 2D lidar to construct a grid map, with each grid size being 0.05m x 0.05m. A column obstacle is detected 1m away from the robot. The grid map is loaded in rviz of ROS, the starting point coordinates are marked as (0,0), the target point coordinates are marked as (10,5), and the column obstacle area is marked as an inflated area. Considering the turning radius and speed limit of the robot comprehensively, the parameters of the A* algorithm are dynamically adjusted, the obstacle adsorption coefficient is set to 0.8, and on the premise that the safety distance is greater than 0.3m, the path is planned to be close to the crop. If an obstacle is detected on the path, the cost value of this area is increased by 20%, and the path is recalculated; otherwise, the original path is maintained. The generated path is fitted with a Bezier curve, and the path points with a curvature greater than 0.5 on the path are removed to obtain a smooth path. Finally, the path point sequence [(0,0), (1.5,1.0), (3.2,2.5), (5.0,3.0),

[0022] (7.5,4.2), (10,5)] and the corresponding pose information are published to the navigation control node.

[0023] S103. Based on the obstacle information, determine the optimal picking distance by calculating the distance between the unmanned picking robot and the obstacle.

[0024] The environmental map information inside the greenhouse, including the position and attribute information of obstacles, is obtained through an environmental perception system. According to the obtained environmental map information, the Euclidean distance between the unmanned picking robot and the obstacles is calculated. Based on the calculated Euclidean distance, combined with the physical size of the robot and the parameters of the vision sensor, the optimal picking distance is determined, that is, the distance that allows the robot to get as close to the obstacles as possible while ensuring safe obstacle avoidance. An obstacle adsorption strategy is introduced into the path planning algorithm to dynamically adjust the weights of the heuristic function and the search step size parameters, so that the path search process preferentially selects the path points close to the optimal picking distance. If the distance from a path point to the nearest obstacle is equal to the optimal picking distance, then this path point is marked as a weighted sub-node, and an additional reward is given when calculating the current cost to increase the probability of its being selected. The current costs are calculated for the marked weighted sub-nodes and ordinary sub-nodes respectively, and the node with the minimum cost is selected as the target point for the next movement until the destination is reached. After obtaining the initial path composed of a series of parent nodes, a path point optimization strategy is used to smooth the path and reduce unnecessary turning points. According to the optimized path point coordinates and pose information, the robot chassis motion system and the robotic arm actuator are controlled to achieve autonomous obstacle avoidance and efficient picking operations in a complex greenhouse environment. The picking object is accurately positioned through a visual recognition algorithm, and combined with the current position and pose information of the robot, the robotic arm is controlled to perform the picking action, and the picked fruits are stored in the collection device.

[0025] Specifically, when obtaining the greenhouse environmental map information, a 360° lidar scan can be used in combination with the gmA*pping algorithm to construct a grid map with a resolution set to 0.05 m. Taking the robot itself as the origin, the Euclidean distance from each grid to the nearest obstacle is calculated. Considering that the physical radius of the picking robot is 0.4 m, the field of view angle of the binocular camera is 70°, and the baseline length is 0.2 m, the optimal picking distance is calculated to be 1.2 m. During the A** algorithm search process, an obstacle adsorption factor k = 0.8 is introduced. The g(n) value of the path point 1.2 m away from the obstacle is multiplied by k, and the search step size is adjusted from 0.2 m to 0.1 m to increase the search density. For the node with the minimum current cost f(n), if it is a weighted node, then f(n) = g(n) + h(n) - 1.2 * 3, otherwise f(n) = g(n) + h(n). After obtaining the initial path through iterative search, the included angle is judged point by point at a sampling interval of 0.1 m, and the intermediate nodes of the continuous points with an included angle less than 5° are directly removed. The optimized path points control the chassis movement through the ROS navigation package. The YOLOv3 deep learning object detection algorithm is used to identify and locate the fruits, the detection confidence threshold is set to 0.8, the spatial position error is less than 0.01 m, and then it is converted to the target pose in the robotic arm coordinate system to guide the robotic arm to approach smoothly and complete the grasping using a two-finger claw manipulator.

[0026] S104. In the map space, with the starting point as the center, obtain the reachable points within its neighborhood. For each reachable point, calculate the estimated cost by computing the Manhattan distance between it and the target point, and calculate the movement cost by computing the Euclidean distance between it and the starting point.

[0027] According to the map space information, obtain the list of reachable points within the neighborhood of the starting point, remove non-reachable terrain points such as walls and water, and add the starting point to the visited list. For each reachable point, calculate the Manhattan distance between it and the target point to obtain the estimated cost; calculate the Euclidean distance between it and the starting point to obtain the movement cost. By summing the estimated cost and the movement cost, obtain the current cost of the regular child node, which serves as the basic evaluation for path selection. Determine whether the Euclidean distance from the child node to the nearest obstacle is the same as the optimal picking distance. If so, this node is a weighted child node, and by summing the estimated cost and the movement cost and subtracting three times the optimal picking distance, obtain the current cost of the weighted child node. Compare the current costs of all child nodes, select the point with the minimum cost as the next moving point, and mark it as the new parent node. Add the original parent node to the visited list. If the new parent node is not the target point, with the new parent node as the center, repeat steps 1-5 to continue iteratively expanding the path point chain. If the new parent node is the target point, take the sequence of parent nodes from the starting point to the target point as the initial navigation path and end the path planning. Obtain the position coordinates of each point on the initial navigation path, and control the robot to move along the path according to the position coordinates to achieve navigation from the starting point to the target point. During the navigation process, obtain the surrounding environment information in real time through sensors, determine whether there are obstacles or picking targets, and dynamically adjust the navigation path according to the positions of the obstacles and the picking targets to ensure that the robot safely and efficiently reaches the target point and completes the picking task.

[0028] Specifically, according to the map space information, obtain the list of reachable points within the neighborhood of the starting point (10, 10), remove wall points such as (11, 10) and water points such as (10, 11), and add the starting point to the visited list. For 8 reachable points such as (9, 10) and (11, 9), calculate the Manhattan distance between each of them and the target point (50, 50) respectively. For example, the estimated cost of (9, 10) is |9 - 50| + |10 - 50| = 81; then calculate the Euclidean distance between it and the starting point (10, 10). For example, the movement cost of (9, 10) is √(9 - 10) 2 +(10 - 10) 2 ≈1. The sum of the two gives the current cost of the regular child node (9, 10) as 82. Determine the Euclidean distance √(9 - 8) from (9, 10) to the nearest obstacle (8, 10) 2 +(10 - 10) 2Is ≈1 equal to the optimal picking distance 1? If they are equal, then (9, 10) is a weighted child node, and its current cost is adjusted to 82 - 3×1 = 79. Compare the current costs of the 8 child nodes, select the one with the minimum cost, (11, 9), as the next moving point, and mark it as the new parent node. Add the original parent node (10, 10) to the visited list. Taking (11, 9) as the new center, repeat the above process until reaching the target point (50, 50). Use the sequence of parent nodes from the starting point to the target point as the initial navigation path. Obtain the coordinates of each point on the path and control the robot to move along the path. Real-time obtain obstacle information and fruit positions within 5 meters through sensors such as lidar. When the distance to an obstacle is less than 0.5 meters or a ripe fruit appears, trigger path replanning to guide the robot to bypass the obstacle or stop for picking, dynamically optimize the navigation path, and balance safety and picking efficiency.

[0029] S105. If the reachable point is a point on the obstacle inflation layer, calculate its weighted current cost, which is the sum of the estimated cost and the movement cost minus three times the optimal picking distance. Otherwise, its current cost is the sum of the estimated cost and the movement cost.

[0030] According to the coordinates of the currently calculated point and the target point, use the Manhattan distance formula to calculate the estimated cost of this point. According to the coordinates of the currently calculated point and its parent node, use the Euclidean distance formula to calculate the movement cost of this point. Obtain the Euclidean distance from the currently calculated point to the nearest obstacle and determine whether it is equal to the predefined optimal picking distance. If the currently calculated point is a point on the obstacle inflation layer, that is, its distance to the nearest obstacle is equal to the optimal picking distance, then calculate its weighted current cost, which is the sum of the estimated cost and the movement cost minus three times the optimal picking distance. If the currently calculated point is not a point on the obstacle inflation layer, then its current cost is the sum of the estimated cost and the movement cost. Compare the calculated current cost of the currently calculated point with the current costs of other adjacent points, and select the point with the minimum cost as the next moving point. Add the original parent node to the visited node list, i.e., the Closelist, and upgrade the moving point with the minimum current cost to the new parent node. Taking the new parent node as the center, obtain its adjacent reachable points, and repeat steps 1 - 7 to iteratively expand the path point chain. When iterating to the target point, that is, reaching the destination, the parent nodes recorded in the Close list at this time constitute the initial navigation path from the starting point to the ending point.

[0031] Specifically, according to the coordinates of the currently calculated point (3, 4) and the target point (7, 8), the Manhattan distance formula |3 - 7| + |4 - 8| = 8 is used to calculate that the estimated cost of this point is 8. According to the coordinates of the currently calculated point (3, 4) and its parent node (2, 3), the Euclidean distance formula √((3 - 2)² + (4 - 3)²) ≈ 1.414 is used to calculate that the movement cost of this point is approximately 1.414. The Euclidean distance from the currently calculated point to the nearest obstacle is obtained as 1, which is equal to the predefined optimal picking distance of 1. It is determined that this point is on the obstacle expansion layer, so its weighted current cost is calculated as 8 + 1.414 - 3 * 1 = 6.414. The adjacent point (4, 5) is not on the obstacle expansion layer, and its current cost is the sum of the estimated cost of 6 and the movement cost of 1.414, which is 7.414. Comparing the cost of the current point (3, 4) of 6.414, which is less than the cost of the adjacent point (4, 5) of 7.414, the point with the minimum cost (3, 4) is selected as the next moving point. The original parent node (2, 3) is added to the Close list, and (3, 4) is upgraded to the new parent node. Taking (3, 4) as the center, its adjacent reachable points such as (2, 5), (4, 5), etc. are obtained, and the above steps are repeated to iteratively expand the path point chain until (7, 8) is iterated. Then, the points (2, 3), (3, 4), ……, (7, 8) recorded in the Close list form the initial navigation path from the starting point to the ending point.

[0032] S106. Use the reachable point with the minimum current cost as the new starting point, move the original starting point into the closed list, and the new starting point and the original starting point form the first path segment.

[0033] According to the information of the starting point and its adjacent eight points, remove the unreachable terrain points and add them to the closed list, and add the remaining reachable points to the open list. The estimated cost is obtained by calculating the Manhattan distance between the starting point and each point in the open list, and the movement cost is calculated by calculating the Euclidean distance between the starting point and each point in the open list. The sum of the two gives the current cost of each point. Determine whether the Euclidean distance from each point in the open list to the nearest obstacle is equal to the optimal picking distance. If they are equal, the current cost of this point is weighted. Compare the current costs of each point in the open list, use the point with the minimum current cost as the new starting point, and move the original starting point into the closed list. Obtain the coordinate information of the new starting point and the original starting point, construct the first path segment and store it. Taking the new starting point as the center, obtain the information of its adjacent eight points, remove the points in the closed list and the unreachable points, and add the remaining points to the open list. Repeat steps 2 - 6 until the point with the minimum current cost is the target point, then stop the loop. Connect all the stored path segments in sequence to obtain the initial path from the starting point to the target point. Optimize the initial path to remove redundant path points to obtain the final optimal path.

[0034] Specifically, among the starting point and its adjacent eight points, 3 points are located on inaccessible terrains such as walls or waters, and they are added to the closed list, while the remaining 5 points are added to the open list. The Manhattan distances between the starting point and each point in the open list are 2, 3, 4, 5, and 6 respectively, and the Euclidean distances are 1.4, 2.2, 2.8, 3.6, and 4.2 respectively. The sum of the two distances gives the current cost of each point as 3.4, 5.2, 6.8, 8.6, and 10.2. Judge the Euclidean distance from each point in the open list to the nearest obstacle. Assuming the optimal picking distance is 2.5, the distances are 2.1, 2.5, 3.2, 3.8, and 4.5 respectively. Among them, the second point has a distance equal to the optimal picking distance. Perform a weighted process on its current cost of 5.2, subtract 3 times the optimal picking distance to get a new cost of -2.3. Compare the current costs of each point in the open list. The cost of the second point is the smallest, so it is used as the new starting point, and the original starting point is moved to the closed list. Obtain the coordinates of the new starting point (3, 4) and the coordinates of the original starting point (1, 2), and construct the first path segment [(1, 2), (3, 4)] and store it. Taking the new starting point as the center, obtain its adjacent eight points, remove 2 points in the closed list and 1 inaccessible point, and add the remaining 5 points to the open list. Repeat the above steps until the point with the minimum current cost is the target point (9, 8), and then stop the loop. Connect the stored path segments in order to obtain the initial path [(1, 2), (3, 4), (5, 6), (7, 8), (9, 8)], and perform an optimization process on it to remove the redundant path point (5, 6) to obtain the final optimal path [(1, 2), (3, 4), (7, 8), (9, 8)].

[0035] S107. Repeat the above steps until reaching the target point to obtain the initial path.

[0036] According to the starting point and its adjacent eight points, remove the unwA*lkA*ble points (such as walls and water, which are terrains that cannot be reached), add the starting point to the Close list, add the remaining adjacent points to the Open list, set the starting point as the parent node, and the remaining points as child nodes. Calculate the H value (estimated cost) using the Manhattan distance, which is obtained by the sum of the absolute values of the coordinate differences between the calculated point and the target point. Calculate the G value (movement cost) using the Euclidean distance, which is obtained by the square root of the sum of the squares of the coordinate differences between the calculated point and the parent node. Calculate the F value (the current cost of the child node), which is obtained by adding the H value and the G value. Compare the F values of each child node in the Open list, mark the point with the smallest F value as the current moving point, remove it from the Open list and add it to the Close list, and at the same time set it as the new parent node. Taking the current moving point as the center, obtain its adjacent eight points, remove the unwA*lkA*ble points and the points in the Close list, add the remaining points to the Open list, and calculate their H, G, and F values. Determine whether the current moving point is the target point. If so, obtain the initial path; if not, jump to step 5 and continue to execute. Optimize according to the initial path points, remove the redundant inflection points, and obtain the simplified path. According to the simplified path, generate the robot's motion trajectory, determine the motion direction and speed of the robot along the path points, and obtain the final navigation path.

[0037] Specifically, taking the starting point (Sxy) and its adjacent eight points as an example, obstacle points such as water and walls are removed as unwA*lkA*ble points, the starting point is added to the Close list, and the remaining obstacle-free adjacent points are added to the Open list. Taking (Sxy) as the parent node and the points in the Open list as child nodes. Calculate the H value using the Manhattan distance |x1 - x2| + |y1 - y2|. For example, the H value of the child node (3, 4) to the target point (7, 8) is |(3 - 7)| + |(4 - 8)| = 8. Calculate the G value using the Euclidean distance √((x1 - x2) 2 +(y1 - y2) 2 ) For example, the G value from (Sxy)(1, 1) to the child node (3, 4) is √((1 - 3) 2 +(1 - 4) 2) = 3.6. F = G + H. For example, for the child node above, F = 3.6 + 8 = 11.6. Compare the F values in the Open list. The current point with the minimum F value (e.g., F = 7.2) is used as the moving point, which is moved from the Open list to the Close list and serves as the new parent node. Taking this point as the center, obtain the adjacent eight points, remove the obstacle points and the points in the Close list, add the new points to the Open list and calculate the F values. If the moving point is (7, 8), then the path is completed; otherwise, return to the previous step. Optimize the initial path (1, 1)(3, 4)(4, 6)(7, 8), remove the redundant inflection points, such as (3, 4), to obtain (1, 1)(4, 6)(7, 8). Then refine the movement trajectory of the robot along this simplified path, determine the movement direction and speed of each segment, and generate the final navigation path.

[0038] S108. Determine whether there are three or more linearly dependent path points in the initial path. If so, remove the middle linearly dependent path points to obtain the simplified final path.

[0039] Initialize the greenhouse map space, the positions and heading angles of the starting point and the target point, the basic parameters of the picking robot, the algorithm parameters, and the obstacle information in the environment, and use the improved A* algorithm for path planning. During the path planning process, introduce an obstacle adsorption strategy, dynamically adjust the weights of the heuristic function and the search step size parameters. On the premise of ensuring the robot's safe obstacle avoidance, prioritize planning paths close to the fruit and vegetable planting areas. The completed initial path contains a series of path point coordinates and attitude information. Obtain the coordinates of three adjacent path points, and by calculating the included angle between the two vectors formed by the three points, determine whether the three points are on the same straight line. If three adjacent path points are linearly dependent, that is, the included angle is equal to 0 or 180 degrees, then mark the middle path point as a redundant path point. Traverse all the path points in the initial path, and according to the redundant path point markings, remove the continuous redundant path points to obtain the simplified path. To further smooth the simplified path, according to the minimum turning radius constraint of the picking robot, perform circular arc interpolation on the path points at the turning points to make the turning path smoother. By calculating the distance and included angle between adjacent points on the simplified path, determine whether it meets the kinematic constraints of the picking robot, such as the maximum driving speed and maximum acceleration limits. If the local section of the simplified path does not meet the kinematic constraints, then insert auxiliary path points on this section, and by optimizing the positions of the interpolation points, make the interpolated path meet the constraint conditions. Publish the finally optimized simplified path to the picking robot navigation control node to guide the robot to move along the planned path and reach the target picking point efficiently and smoothly to complete the picking operation task.

[0040] Specifically, when initializing the greenhouse environment map, with a grid resolution of 1 cm, the starting point coordinates of the robot are set to (1.2, 0.8), the heading angle is 45°, the target point coordinates are (6.5, 7.2), and the heading angle is -90°. The minimum turning radius of the picking robot is 0.6 m, the maximum traveling speed is 1.2 m / s, and the maximum acceleration is 0.5 m / s 2 The obstacle information is obtained by lidar scanning, and the obstacle avoidance path planning is carried out with an obstacle expansion radius of 0.1 m. During the path planning process, the obstacle distance factor and the path length factor are comprehensively considered, the heuristic function weight of the A** algorithm is dynamically adjusted, the search step size is set to 1.5 times the radius of the robot, and within the range of 0.2 m close to the obstacle, the obstacle attraction is introduced to make the planned path as close as possible to the crop area. For the initially planned path, with an angle threshold of 15°, the adjacent three path points are traversed, and their linear correlation is judged by the vector angle formula, and the redundant path points are removed to obtain a simplified path. To make the turning smoother, 5 circular arc interpolation points are inserted in the turning section, and it is checked whether the curvature radius of the interpolation path meets the minimum turning radius constraint. Finally, with an acceleration change of 0.2 m / s 2 , the speed between adjacent points of the simplified path is adjusted to meet the maximum traveling speed limit. The optimized path is published to the navigation control node at a time interval of 0.1 s, guiding the robot to start from the starting point and move along the sequence of path points to reach the target picking point smoothly and efficiently.

[0041] The above-disclosed are only the preferred embodiments of the present invention. Of course, the scope of the rights of the present invention cannot be limited thereby. Those of ordinary skill in the art can understand all or part of the processes of implementing the above embodiments, and the equivalent changes made according to the claims of the present invention still fall within the scope covered by the invention.

Claims

1. An unmanned picking robot path planning algorithm for an intelligent greenhouse, characterized in that, Including: Initialize the map space, and obtain the poses of the starting point and the target point, the basic parameters of the robot, the algorithm parameters, and the obstacle information in the environment; According to the obstacle information and the algorithm parameters, calculate the optimal distance between the robot and the target; Based on the optimal distance and the poses of the starting point and the target point, adopt an improved A* algorithm to introduce an obstacle adsorption strategy, calculate through the cost function and iteratively generate path points to obtain an initial path; Screen the redundant path points in the initial path, and prune and optimize the path through curvature analysis and neighborhood search methods to obtain the final smooth, safe and efficient robot navigation path.

2. The path planning algorithm according to claim 1, characterized in that, The initialization of the map space includes: Calibrate the map boundary, divide the grid, and obtain the obstacle attribute information of each grid; Collect three-dimensional data of the greenhouse scene and construct the map elevation information; Extract the contour of the fruit and vegetable planting area, extract the position distribution information of the plants in the area, and establish the map semantic information.

3. The path planning algorithm according to claim 1, characterized in that, The obtaining of the poses of the starting point and the target point, the basic parameters of the robot, the algorithm parameters, and the obstacle information in the environment includes: Obtain the spatial positions, sizes and other information of the obstacles around the robot through lidar scanning, and convert them into information in the map coordinate system; Determine the target picking area according to the greenhouse operation process, and obtain the coordinates of the target picking points in this area; Obtain the size parameters, kinematic constraints and other information of the robot itself; Set parameters such as the search step size and obstacle influence factor of the A* algorithm.

4. The path planning algorithm according to claim 1, characterized in that, The calculation of the optimal distance between the robot and the target includes: Obtain the average crown size of the plants in the target picking area, and reserve a certain margin as the safe navigation gap for the robot; Determine the working range of the robotic arm and obtain its optimal working distance; Comprehensively consider the plants and the working range of the robotic arm to determine the optimal working distance between the robot and the obstacles.

5. The path planning algorithm according to claim 1, wherein The calculation through the cost function and the iterative generation of path points includes: Radiate and search with the starting point as the core, construct Open and Close lists, and screen the passable adjacent nodes; Introduce a dual cost function of tradition and obstacle influence to evaluate the comprehensive cost of each node; Select the node with the minimum cost as the next hop, update the Close list and the current node, and iteratively optimize until the target point.

6. The path planning algorithm according to claim 1, characterized in that, The screening of the initial path points includes: Calculate the curvature of three adjacent points in the initial path. If the curvature is less than the threshold, it is regarded as a redundant path point; Screen the redundant path points, remove the nodes with less influence, and retain the key inflection points and safety nodes.

7. The path planning algorithm according to claim 1, wherein The pruning and optimization of the path through curvature analysis and neighborhood search methods includes: Based on the screened key path nodes, conduct path search within the neighborhood; Analyze the slope change of adjacent path points, and merge and simplify the path segments with smaller curvature; Fit the path points with a Bezier curve to smooth the trajectory and complete the pruning and optimization of the path.

Images