An ackerman orchard robot path planning method, system and storage medium

Abstract

The application relates to an Ackerman orchard robot field path planning method, a system and a storage medium, and the method comprises the following steps: S1, a grid map containing orchard crop rows and a turnable area of a field head is constructed; S2, an integrated geometric model of an Ackerman vehicle and a mounted machine tool is constructed; S3, a start pose and an end pose of a planned turning path in the grid map are calculated; S4, U-shaped turning path search of the integrated geometric model is carried out, if a U-shaped turning path is searched, the path planning is ended, otherwise, step S5 is executed, Y-shaped turning path planning is carried out; S5, a reverse end pose and a reverse start pose of the Y-shaped turning path are determined, three collision-free paths are obtained and splicing processing is carried out, and a Y-shaped turning path is generated. According to the actual field head width and the machine tool size, the optimal turning scheme can be adaptively selected, the field head space utilization rate is effectively improved, and the operation safety and efficiency are ensured.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of agricultural robot control, and particularly relates to an Ackermann orchard robot head path planning method, system and storage medium. BACKGROUND

[0002] With the development of agricultural mechanization and intelligence, the demand for intelligent operation of large-scale hedgerow orchards is increasing. At present, large-scale hedgerow orchards generally use mobile robots based on Ackermann steering structure to perform tasks such as spraying, pruning and harvesting.

[0003] The Ackermann orchard robot generally uses a U-shaped path planning strategy for head turning. In conventional curve operation, it has good steering stability, but due to the inherent characteristics of the Ackermann steering structure, it requires a large turning radius in the empty state. When the spraying machine and picking platform are connected, the effective turning radius further increases, and in the operation of large-scale hedgerow orchards using intensive planting methods, the following defects exist:

[0004] 1. Low utilization rate of head space. In order to meet the minimum turning radius requirement of the Ackermann orchard robot, a large area of non-productive space is forced to be reserved in the head area, resulting in a decrease in the proportion of effective planting area in the orchard and a decrease in land resource utilization efficiency.

[0005] 2. Poor adaptability. The existing U-shaped path planning cannot optimize the trajectory in real time according to the actual available width of the head and the outer dimensions of the machine. In a narrow head scene, the pre-planned U-shaped turning path often cannot be completely executed due to insufficient space, resulting in interruption of the operation.

[0006] 3. Collision risk is prominent. The existing U-shaped path planning does not fully consider the spatial interference relationship between the geometric envelope boundary of the machine and the boundary of the orchard, and the machine is prone to scratching with the surrounding obstacles during turning, which may cause equipment damage and safety accidents. SUMMARY

[0007] The present application aims to solve one of the above technical problems, and provides an Ackermann orchard robot head path planning method, system and storage medium.

[0008] To achieve the above purpose, the technical solution adopted by the present application is:

[0009] An Ackermann orchard robot head path planning method, comprising the following steps:

[0010] S1: Construct a grid map containing orchard crop rows and head turnable areas;

[0011] S2: Construct an integrated geometric model of the Ackermann vehicle and the mounted machine;

[0012] S3: calculating a start pose and an end pose of the planned turning path in the grid map based on a driving direction of the vehicle in the current work row, a head offset of the current work row, and a head offset of the next work row;

[0013] S4: performing collision-free path search of the integrated geometric model based on the grid map using a Hybrid A* algorithm, if a U-shaped turning path that meets a turning radius of the current turnable area and is collision-free is searched, ending the path planning, otherwise, performing step S5 to plan a Y-shaped turning path;

[0014] S5: sequentially searching and performing collision test in a head boundary direction and a crop row direction based on the grid map and the integrated geometric model to determine a reverse end pose and a reverse start pose of the Y-shaped turning path, performing segmented path planning based on the reverse end pose and the reverse start pose to obtain three collision-free paths, and splicing the three collision-free paths to generate a final Y-shaped turning path.

[0015] In some embodiments of the present application, step S5 specifically comprises the following steps:

[0016] S51: determining path search parameters based on the grid map and the integrated geometric model, the path search parameters including a search length, a vehicle turning angle range, and a step length;

[0017] S52: searching and performing collision test in the head boundary direction from the end pose based on the path search parameters to determine the reverse end pose of the Y-shaped turning path, and a collision-free path from the reverse end pose to the end pose;

[0018] S53: searching and performing collision test in the crop row direction from the reverse end pose based on the path search parameters to determine the reverse start pose of the Y-shaped turning path, and a collision-free path from the reverse start pose to the reverse end pose;

[0019] S54: calculating a collision-free path of the integrated geometric model from the start pose to the reverse start pose based on the grid map using the Hybrid A* algorithm;

[0020] S55: splicing the three collision-free paths and marking a driving direction for each path to generate a final Y-shaped turning path.

[0021] In some embodiments of the present application, the implementation method of step S52 specifically comprises the following steps:

[0022] searching in the head boundary direction from the end pose based on the path search parameters, calculating a curvature of a path trajectory based on the vehicle turning angle range, calculating a change step length of an angle on a discrete path based on the curvature, and further calculating discrete pose points on the path.

[0023] Positioning the integrated geometric model to the discrete pose points on the path, performing collision test of the integrated geometric model in combination with the grid map, if the integrated geometric model does not collide with the environment at all the pose points on the path, determining the last pose point on the path as the reverse driving end pose, and determining the path as the collision-free path from the reverse driving end pose to the end pose.

[0024] In some embodiments of the present application, the implementation method of step S53 specifically comprises the following steps:

[0025] Based on the path search parameters, search from the reverse driving end pose to the crop row direction, calculate the curvature of the path trajectory based on the vehicle steering angle range, calculate the angle change step on the discrete path based on the curvature, and further calculate the discrete pose points on the path;

[0026] Positioning the integrated geometric model to the discrete pose points on the path, performing collision test of the integrated geometric model in combination with the grid map, if the integrated geometric model does not collide with the environment at all the pose points on the path, determining the last pose point on the path as the reverse driving end pose, and determining the path as the collision-free path from the reverse driving end pose to the end pose.

[0027] In some embodiments of the present application, step S2 specifically comprises the following steps:

[0028] Configure the geometric parameters of the Ackerman vehicle and the mounted machine tool; wherein the geometric parameters of the Ackerman vehicle include but are not limited to the length, width, height, wheelbase and track of the Ackerman vehicle; and the geometric parameters of the mounted machine tool include but are not limited to the length, width, height of the bounding box of the mounted machine tool and the connection distance from the vehicle.

[0029] Based on the geometric parameters of the Ackerman vehicle and the mounted machine tool, construct the geometric model of the Ackerman vehicle and the mounted machine tool.

[0030] Integrate the geometric model of the Ackerman vehicle and the mounted machine tool to construct an integrated geometric model.

[0031] In some embodiments of the present application, step S3 further comprises the following steps:

[0032] Use the depth-first search algorithm to search out the traversal sequence of the work row, and determine the row head offset of the current work row and the row head offset of entering the next work row based on the traversal sequence.

[0033] In some embodiments of the present application, in step S1, based on the LiDAR point cloud technology or GNSS measurement technology, a grid map containing the orchard crop row and the head turnable area is constructed.

[0034] In some embodiments of the present application, in step S5, if the final Y-shaped turning path cannot be generated, the path planning is ended.

[0035] Some embodiments of the present application further provide an Ackerman orchard robot head path planning system, comprising:

[0036] at least one processor;

[0037] at least one memory for storing at least one program;

[0038] When the at least one program is executed by the at least one processor, the at least one processor implements the above-mentioned Ackerman orchard robot head path planning method.

[0039] Some embodiments of the present application further provide a storage medium, wherein a processor-executable program is stored, and the processor-executable program is used to implement the above-mentioned Ackerman orchard robot head path planning method when executed by a processor.

[0040] The present application has the following beneficial effects:

[0041] 1. The path planning method provided by the present application firstly attempts to use the Hybrid A* algorithm to combine the vehicle and the tool size to plan a U-shaped turning path, and if it cannot be realized, it automatically switches to a Y-shaped path planning mode, realizes turning through the combination of the segmented paths "forward-reverse-forward", and can adaptively select the optimal turning scheme according to the actual head width and the tool size, effectively improve the head space utilization rate, and at the same time ensure the operation safety and efficiency;

[0042] 2. The present application refines the Y-shaped path into "forward-reverse-forward" three segments for independent search, reduces the dimension of single search, verifies each segment independently, avoids a failure, and recalculates the whole, effectively reducing the head waiting time.

[0043] Other features and advantages of the present application will be illustrated in the following description, and some will become apparent from the description, or will be understood by those skilled in the art through implementation of the present application. The purpose and other advantages of the present application can be achieved and obtained through the structure indicated in the specification, claims and drawings. BRIEF DESCRIPTION OF DRAWINGS

[0044] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, specific embodiments of the present application will be described in detail below with reference to the drawings, and other drawings can also be obtained by those skilled in the art without creative labor on the premise of the drawings.

[0045] Figure 1 It is a flowchart of an Ackerman orchard robot head path planning method.

[0046] Figure 2 This is a schematic diagram of the integrated geometric model provided in an embodiment of the present invention in an orchard environment;

[0047] Figure 3 This is a schematic diagram of U-shaped turn path planning provided in an embodiment of the present invention;

[0048] Figure 4 This is a schematic diagram of U-shaped turn path planning provided in an embodiment of the present invention. Detailed Implementation

[0049] To make the objectives, technical solutions, and advantages of this application clearer, the application is described and illustrated below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application. All other embodiments obtained by those skilled in the art based on the embodiments provided in this application without inventive effort are within the scope of protection of this application.

[0050] It should be noted that the terminology used herein is for the purpose of describing particular implementations only and is not intended to limit the exemplary implementations according to this application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. Furthermore, it should be understood that the terms “comprising” and “having”, and any variations thereof, are intended to cover non-exclusive inclusion, for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such process, method, product, or apparatus.

[0051] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0052] The technical solution of the present invention will be described in detail below with reference to specific embodiments and accompanying drawings.

[0053] As attached Figure 1 -Appendix Figure 4 As shown, in an illustrative embodiment of an Ackerman-style orchard robot path planning method of the present invention, the path planning method includes the following steps.

[0054] S1: Construct a raster map containing orchard crop rows and bendable areas at the edge of the field, based on LiDAR point cloud technology or GNSS measurement technology. In the map, crop rows are represented by two parallel straight lines, and bendable areas at the edge of the field are represented by polygonal envelopes.

[0055] S2: constructing an integrated geometric model of the Ackerman vehicle and the mounted machine tool. A schematic diagram of the integrated geometric model in the orchard head environment is shown in FIG. 2. Figure 2

[0056] In some embodiments of the present application, step S2 specifically comprises the following steps.

[0057] configuring geometric parameters of the Ackerman vehicle and the mounted machine tool; wherein the geometric parameters of the Ackerman vehicle include but are not limited to the length, width, height, wheelbase and track of the Ackerman vehicle; and the geometric parameters of the mounted machine tool include but are not limited to the length, width, height of the bounding box of the mounted machine tool and the connection distance with the vehicle.

[0058] constructing a kinematic geometric model of the Ackerman vehicle and the mounted machine tool based on the geometric parameters of the Ackerman vehicle and the mounted machine tool.

[0059] integrating the geometric model of the Ackerman vehicle and the mounted machine tool to construct an integrated geometric model.

[0060] S3: based on the driving direction of the vehicle in the current work row, the head offset of the current work row and the head offset of entering the next work row, calculating the start pose (x s ,y s ,θ s ) and the end pose (x e ,y e ,θ e ) of the planned turning path in the grid map.

[0061] In some embodiments of the present application, step S3 further comprises the following steps:

[0062] using a depth-first search (DFS) algorithm to search for a traversal sequence of the work row, and determining the head offset of the current work row and the head offset of entering the next work row based on the traversal sequence.

[0063] S4: using a Hybrid A* algorithm based on the constructed grid map to search for a collision-free path of the integrated geometric model, and if a U-shaped turning path that satisfies the turning radius of the current turnable area and has no collision is searched, ending the path planning. The U-shaped turning path planning obtained by the search is shown in FIG. 3. Figure 3

[0064] Otherwise, step S5 is performed to plan a Y-shaped turning path.

[0065] In some embodiments of the present application, in step S4, the method of using a Hybrid A* algorithm based on the constructed grid map to search for a collision-free path of the integrated geometric model specifically comprises the following steps.

[0066] ​​S41: Initialization: Enter the starting state into the Open table.

[0067] S42: Node Expansion: Find the node with the minimum total cost f. Generate curve primitives based on the bicycle model using discrete steering angle φ and step size Δs, where f = g + h, g represents the cumulative actual cost from the starting point to the current node, including path length, curvature penalty, steering smoothness cost, etc., and h represents the estimated cost from the current node to the target, usually using Euclidean distance or octagonal distance on the grid map.

[0068] S43: Grid heuristic: estimate h using the 2D Euclidean or octant distance from a node in the grid to the target.

[0069] S44: Collision Detection: Map primitive trajectories to a grid and remove segments that conflict with obstacles.

[0070] S45: Cost calculation: g consists of path length, curvature penalty, and steering smoothness cost, update f.

[0071] S46: Termination condition: When a node is close enough to the target in the grid, call the Analytic extension (such as Clothoid) to directly connect.

[0072] By adjusting the turning radius and curve smoothness together with the discrete series φ, step size Δs, and curvature penalty weight of the motion primitive, the search trajectory is ensured to be both feasible and smooth.

[0073] S5: Determine the positions of the two key path points for reversing in the Y-shaped turning path planning, namely the starting reversing position and the ending reversing position.

[0074] Based on the grid map and integrated geometric model, searches and collision tests are performed sequentially towards the field boundary and crop row direction to determine the reversing end pose (x) of the Y-shaped turning path. be ,y be ,θ be ) and reversing starting position (x bs ,y bs ,θ bs ).

[0075] Based on the reversing end pose (x) be ,y be ,θ be ) and reversing starting position (x bs ,y bs ,θ bs Segmented path planning is performed to obtain three collision-free paths.

[0076] The three collision-free path segments are spliced ​​together to generate the final Y-shaped turning path, as shown in the attached figure. Figure 4 As shown.

[0077] In some embodiments of the present application, step S5 specifically comprises the following steps.

[0078] S51: determining path search parameters based on the grid map and the integrated geometric model, the path search parameters including but not limited to search length, vehicle steering angle range and step length.

[0079] S52: searching and collision testing from the end pose to the headland boundary direction based on the path search parameters to determine the reverse end pose (x be , y be , θ be ) of the Y-shaped turning path, and the collision-free path from the reverse end pose (x be , y be , θ be ) to the end pose (x e , y e , θ e ).

[0080] In some embodiments of the present application, the implementation method of step S52 specifically comprises the following steps:

[0081] searching from the end pose (x e , y e , θ e ) to the headland boundary direction based on the path search parameters, calculating the curvature of the path trajectory based on the vehicle steering angle range, calculating the angle change step length on the discrete path based on the curvature, and further calculating the discrete pose points on the path.

[0082] positioning the integrated geometric model to the discrete pose points on the path, performing collision testing of the integrated geometric model in combination with the grid map, if the integrated geometric model does not collide with the environment at all pose points on the path, determining the last pose point on the path as the reverse end pose, and determining the path as the collision-free path from the reverse end pose (x be , y be , θ be ) to the end pose (x e , y e , θ e ).

[0083] S53: searching and collision testing from the reverse end pose (x be , y be , θ be ) to the crop row direction based on the path search parameters to determine the reverse start pose (x bs , y bs , θ bs ) of the Y-shaped turning path, and the collision-free path from the reverse start pose (x bs , ybs ,θ bs ) to the end pose (x be ,y be ,θ be ) of the reversing;

[0084] In some embodiments of the present application, the implementation method of step S53 specifically comprises the following steps:

[0085] searching from the end pose (x be ,y be ,θ be ) to the crop row direction based on the path search parameters, calculating the curvature of the path trajectory based on the vehicle steering angle range, calculating the angle change step on the discrete path based on the curvature, and further calculating the discrete pose points on the path;

[0086] positioning the integrated geometric model to the discrete pose points on the path, performing collision test of the integrated geometric model in combination with the grid map, if the integrated geometric model does not collide with the environment at all the pose points on the path, determining the last pose point on the path as the start pose (x bs ,y bs ,θ bs ) of the reversing, and determining the path as the collision-free path from the start pose (x bs ,y bs ,θ bs ) of the reversing to the end pose (x be ,y be ,θ be ).

[0087] S54: calculating the collision-free path of the integrated geometric model from the start pose (x s ,y s ,θ s ) to the start pose (x bs ,y bs ,θ bs ) of the reversing based on the grid map using the Hybrid A* algorithm.

[0088] S55: splicing the three collision-free paths and marking the driving direction for each path to generate the final Y-shaped turning path. It should be noted that the directions of the three collision-free paths are respectively the direction from the start pose (x s ,y s ,θ s ) to the start pose (x bs ,y bs ,θ bs ) of the reversing, the direction from the start pose (x bs ,y bs ,θ bs) to a reverse end pose (x be ,y be ,θ be ) and a direction from the reverse end pose (x be ,y be ,θ be ) to an end pose (x e ,y e ,θ e ).

[0089] In some embodiments of the present application, in step S5, if the final Y-shaped turning path cannot be generated, the path planning is ended.

[0090] Some embodiments of the present application further provide an Akkerman orchard robot head path planning system, comprising at least one processor.

[0091] At least one memory for storing at least one program.

[0092] When the at least one program is executed by the at least one processor, the at least one processor implements the above-mentioned Akkerman orchard robot head path planning method.

[0093] Some embodiments of the present application further provide a storage medium, wherein a processor-executable program is stored, and the processor-executable program, when executed by a processor, is used to implement the above-mentioned Akkerman orchard robot head path planning method.

[0094] Finally, it should be noted that: in the specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts of each embodiment can be referred to.

[0095] The above embodiments are only used to illustrate the technical solutions of the present application but not to limit it; although the present application has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that the specific embodiments of the present application can be modified or some technical features can be replaced by equivalent ones; without departing from the spirit of the technical solutions of the present application, they should be covered in the technical solution range of the present application.

Claims

1. A method for path planning in an Ackerman-style orchard robot, characterized in that, Includes the following steps: S1: Construct a grid map that includes rows of orchard crops and the bendable areas at the edge of the field; S2: Construct an integrated geometric model of the Ackerman vehicle and its mounting equipment; S3: Based on the vehicle's driving direction in the current work row, the head offset of the current work row, and the head offset when entering the next work row, calculate the start and end poses of the planned turning path in the grid map. S4: Based on the grid map, use the Hybrid A* algorithm to search for a collision-free path for the integrated geometric model. If a U-shaped turning path that satisfies the turning radius of the current turning area and is collision-free is found, then the path planning ends. Otherwise, proceed to step S5 to plan the Y-shaped turning path; S5: Based on the grid map and the integrated geometric model, perform search and collision tests sequentially towards the field boundary and crop row direction to determine the reversing end pose and reversing start pose of the Y-shaped turning path; based on the reversing end pose and reversing start pose, perform segmented path planning to obtain three collision-free paths; and stitch the three collision-free paths together to generate the final Y-shaped turning path. Step S5 specifically includes the following steps: S51: Determine path search parameters based on the grid map and the integrated geometric model. The path search parameters include search length, vehicle steering angle range, and step size. S52: Based on the path search parameters, perform a search and collision test from the end pose towards the edge of the ground to determine the reversing end pose of the Y-shaped turning path, and the collision-free path from the reversing end pose to the end pose. S53: Based on the path search parameters, search and collision test from the reversing end pose to the cropping direction to determine the reversing start pose of the Y-shaped turning path, and the collision-free path from the reversing start pose path to the reversing end pose. S54: Based on the grid map, use the Hybrid A* algorithm to calculate the collision-free path of the integrated geometric model from the starting pose to the reversing starting pose; S55: The three collision-free paths are spliced ​​together, and the driving direction is marked for each path to generate the final Y-shaped turning path. Specifically, step S52 includes the following steps: The path search is performed from the end pose toward the edge of the ground based on the path search parameters. The curvature of the path trajectory is calculated based on the vehicle steering angle range. The step size of the angle change on the discrete path is calculated based on the curvature, and then the discrete pose points on the path are calculated. The integrated geometric model is located at discrete pose points on the path. Collision tests of the integrated geometric model are performed using the grid map. If the integrated geometric model does not collide with the environment at any pose point on the path, the last pose point on the path is determined as the reversing end pose, and the path is determined as a collision-free path from the reversing end pose to the end pose.

2. The method for path planning in an Ackerman-style orchard robot according to claim 1, characterized in that, The implementation method of step S53 specifically includes the following steps: Based on the path search parameters, the vehicle searches from the reversing end pose towards the cropping direction. The curvature of the path trajectory is calculated based on the vehicle steering angle range. The step size of the angle change on the discrete path is calculated based on the curvature, and then the discrete pose points on the path are calculated. The integrated geometric model is located at discrete pose points on the path. Collision tests of the integrated geometric model are performed using the grid map. If the integrated geometric model does not collide with the environment at any pose point on the path, the last pose point on the path is determined as the reversing start pose, and the path is determined as a collision-free path from the reversing start pose to the reversing end pose.

3. The method for path planning in an Ackerman-style orchard robot according to claim 1, characterized in that, Step S2 specifically includes the following steps: Configure the geometric parameters of the Ackerman vehicle and its mounts; wherein, the geometric parameters of the Ackerman vehicle include, but are not limited to, the length, width, height, wheelbase and track width of the Ackerman vehicle; the geometric parameters of the mounts include, but are not limited to, the length, width, height of the mount's enclosure frame and the distance to the vehicle; A geometric model of the Ackerman vehicle and its trailer is constructed based on the geometric parameters of the Ackerman vehicle and its trailer. The geometric models of the Ackerman vehicle and its attachments are integrated to construct a unified geometric model.

4. The method for path planning in an Ackerman-style orchard robot according to claim 1, characterized in that, Step S3 further includes the following steps: The depth-first search algorithm is used to find the traversal sequence of the job rows, and the row head offset of the current job row and the row head offset for entering the next job row are determined based on the traversal sequence.

5. The method for path planning in an Ackerman-style orchard robot according to claim 1, characterized in that, In step S1, a raster map containing orchard crop rows and bendable areas at the edge of the field is constructed based on LiDAR point cloud technology or GNSS measurement technology.

6. The method for path planning in an Ackerman-style orchard robot according to claim 1, characterized in that, If the final Y-shaped turning path cannot be generated in step S5, the path planning ends.

7. An Ackerman-style orchard robot path planning system, characterized in that, include: At least one processor; At least one memory for storing at least one program; When the at least one program is executed by the at least one processor, the at least one processor implements the Ackerman orchard robot field path planning method as described in any one of claims 1-6.

8. A storage medium storing a processor-executable program, characterized in that, The processor-executable program, when executed by the processor, is used to implement the Ackerman orchard robot path planning method as described in any one of claims 1-6.

Images