Ackerman type orchard robot field edge path planning method and system and storage medium

Abstract

The invention relates to an Ackerman type orchard robot edge-of-field path planning method and system and a storage medium. The method comprises the following steps: S1, constructing a grid map containing orchard crop rows and edge-of-field turning areas; s2, constructing an integrated geometric model of the Ackerman vehicle and the mounted machine tool thereof; s3, calculating a starting pose and an ending pose of the to-be-planned turning path in the grid map; s4, U-shaped turning path searching of the integrated geometric model is carried out, if the U-shaped turning path is obtained through searching, path planning is ended, otherwise, the step S5 is executed, and Y-shaped turning path planning is carried out; and S5, determining a backing-up ending pose and a backing-up starting pose of the Y-shaped turning path, obtaining three sections of collision-free paths, and splicing the three sections of collision-free paths to generate the Y-shaped turning path. The optimal turning scheme can be adaptively selected according to the actual field edge width and the machine tool size, the field edge space utilization rate is effectively improved, and meanwhile operation safety and efficiency are guaranteed.

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: 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.

[0004] 2. Poor adaptability. The existing U-shaped path planning cannot perform real-time trajectory optimization 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 interrupted operation.

[0005] 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

[0006] 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.

[0007] To achieve the above purpose, the technical solution adopted by the present application is: An Ackermann orchard robot head path planning method, comprising the following steps: S1: constructing a grid map containing orchard crop rows and head turnable areas; S2: constructing an integrated geometric model of an Ackermann vehicle and a machine mounted thereon; 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 the next work row, the start pose and the end pose of the to-be-planned turning path in the grid map are calculated; S4: collision-free path search of the integrated geometric model is performed based on the grid map using a Hybrid A* algorithm, if a U-shaped turning path meeting the turning radius of the current turnable area and being collision-free is searched, the path planning is ended, otherwise, step S5 is performed to perform Y-shaped turning path planning; S5: based on the grid map and the integrated geometric model, search and collision test are sequentially performed in the head boundary direction and the crop row direction to determine the reverse end pose and the reverse start pose of the Y-shaped turning path, segmented path planning is performed based on the reverse end pose and the reverse start pose to obtain three collision-free paths, and the three collision-free paths are spliced to generate the final Y-shaped turning path.

[0008] In some embodiments of the application, step S5 specifically comprises the following steps: S51: path search parameters are determined based on the grid map and the integrated geometric model, the path search parameters comprising a search length, a vehicle turning angle range and a step length; S52: based on the path search parameters, search and collision test are performed from the end pose to the head boundary direction to determine the reverse end pose of the Y-shaped turning path, and the collision-free path from the reverse end pose to the end pose; S53: based on the path search parameters, search and collision test are performed from the reverse end pose to the crop row direction to determine the reverse start pose of the Y-shaped turning path, and the collision-free path from the reverse start pose to the reverse end pose; S54: based on the grid map, the Hybrid A* algorithm is used to calculate the collision-free path of the integrated geometric model from the start pose to the reverse start pose; S55: the three collision-free paths are spliced, and the driving direction of each path is marked to generate the final Y-shaped turning path.

[0009] In some embodiments of the application, the implementation method of step S52 specifically comprises the following steps: Based on the path search parameters, search is performed from the end pose to the head boundary direction, the curvature of the path trajectory is calculated based on the vehicle turning angle range, the change step length of the angle on the discrete path is calculated based on the curvature, and then the discrete pose points on the path are calculated; Position the integrated geometric model to the discrete pose points on the path, and perform 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, determine the last pose point on the path as the reverse driving end pose, and determine the path from the reverse driving end pose to the end pose as a collision-free path.

[0010] In some embodiments of the present application, the implementation method of step S53 specifically comprises the following steps: 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; Position the integrated geometric model to the discrete pose points on the path, and perform 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, determine the last pose point on the path as the reverse driving end pose, and determine the path from the reverse driving end pose to the end pose as a collision-free path.

[0011] In some embodiments of the present application, step S2 specifically comprises the following steps: 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. 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; Integrate the geometric model of the Ackerman vehicle and the mounted machine tool to construct an integrated geometric model.

[0012] In some embodiments of the present application, step S3 further comprises the following steps: 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.

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

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

[0015] Some embodiments of the present application further provide an Ackerman orchard robot head path planning system, comprising: 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 above-mentioned Akkerman orchard robot head path planning method.

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

[0017] The present application has the advantages of: 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, if it cannot be realized, it automatically switches to a Y-shaped path planning mode, and realizes turning through the combination of segmented paths "forward-reverse-forward", which 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; 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 one failure, and recalculates the whole, which effectively reduces the head waiting time.

[0018] Other features and advantages of the present application will be described in the following description, and some will become apparent from the description, or will be understood from the practice of the present application. The purpose and other advantages of the present application can be achieved and obtained by the structure indicated in the specification, claims and drawings. BRIEF DESCRIPTION OF DRAWINGS

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

[0020] Figure 1 A flowchart of an Akkerman orchard robot head path planning method; Figure 2 A schematic diagram of an integrated geometric model in an orchard head environment provided by an embodiment of the present application; Figure 3 A schematic diagram of U-shaped turning path planning provided by an embodiment of the present application; Figure 4 A schematic diagram of U-shaped turning path planning provided by an embodiment of the present application. DETAILED DESCRIPTION

[0021] In order to make the purposes, technical solutions and advantages of the present application clearer, the present application will be described and explained in detail below in conjunction with the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and not intended to limit the present application. Based on the embodiments provided by the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the scope of the present application.

[0022] It should be noted that the terms used herein are only intended to describe specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, the singular form is intended to include the plural form unless the context clearly indicates otherwise, and it should also 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 device including a series of steps or units does not have to be limited to those steps or units clearly listed, but can include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

[0023] The embodiments in the present application and the features in the embodiments can be combined with each other without conflict.

[0024] The technical solutions of the present application will be described in detail below in conjunction with specific embodiments and the drawings of the specification.

[0025] As shown in the drawings, Figure 1 As shown in the drawings, Figure 4 In one illustrative embodiment of the path planning method for an Ackerman orchard robot at the head of the field, the path planning method comprises the following steps.

[0026] S1: Construct a grid map containing orchard crop rows and head turnable areas based on LiDAR point cloud technology or GNSS measurement technology. The crop rows in the map are represented by two parallel straight line features, and the head turnable areas are represented by a polygon envelope.

[0027] S2: Construct an integrated geometric model of the Ackerman vehicle and the mounted implement. The schematic diagram of the integrated geometric model in the orchard head environment is shown in the drawing Figure 2 .

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

[0029] Configure the geometric parameters of the Ackerman vehicle and the mounted implement; 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; the geometric parameters of the mounted implement include but are not limited to the length, width, height of the surrounding frame of the mounted implement and the connection distance from the vehicle.

[0030] Constructing a kinematic geometry model of the Ackerman vehicle and the machine tool mounted thereon based on the geometric parameters of the Ackerman vehicle and the machine tool mounted thereon.

[0031] Integrating the geometric models of the Ackerman vehicle and the machine tool mounted thereon to construct an integrated geometry model.

[0032] 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, the start pose (x s ,y s ,θ s ) and the end pose (x e ,y e ,θ e ) of the to-be-planned turning path in the grid map are calculated.

[0033] In some embodiments of the present application, step S3 further comprises the following steps: 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.

[0034] S4: Using a Hybrid A* algorithm based on the constructed grid map to perform collision-free path search of the integrated geometry model, and if a U-shaped turning path that meets the turning radius of the current turnable area and has no collision is searched, the path planning is ended. The U-shaped turning path planning obtained by searching is shown in FIG. 8. Figure 3

[0035] Otherwise, step S5 is performed to perform Y-shaped turning path planning.

[0036] 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 perform collision-free path search of the integrated geometry model specifically comprises the following steps.

[0037] S41: Initialization: the start point state is entered into the Open table.

[0038] S42: Node expansion: the node with the minimum total cost f is taken, and a curve primitive is generated based on the bicycle model with a discrete steering angle φ and a step size Δs, wherein f = g + h, g represents the cumulative actual cost from the start 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, which is usually the Euclidean distance or eight-connected distance on the grid map.

[0039] S43: Grid heuristic: the 2D Euclidean or eight-connected distance of the node in the grid to the target is used to estimate h.

[0040] ​S44: Collision Detection: Map primitive trajectories to a grid and remove segments that collide with obstacles.

[0041] S45: Cost calculation: g is composed of path length, curvature penalty, and steering smoothness cost, and f is updated.

[0042] S46: Termination condition: When the node is close enough to the target in the grid, call the Analytic extension (such as Clothoid) for direct connection.

[0043] The turning radius and curve smoothness are adjusted by the discrete series φ of the motion primitive, the step size Δs, and the curvature penalty weight to ensure that the search trajectory is both feasible and smooth.

[0044] S5: Determine the two key path point positions of the reversing in the Y-turn path planning, namely the starting reversing position and the ending reversing position.

[0045] Based on the grid map and the integrated geometric model, the search and collision test are carried out in the direction of the field boundary and the crop row in turn to determine the reversing end position (x be ,y be ,θ be ) and the reversing start position (x bs ,y bs ,θ bs ).

[0046] Based on the reversing end posture (x be ,y be ,θ be ) and the reversing start position (x bs ,y bs ,θ bs ) performs segmented path planning to obtain three collision-free paths.

[0047] The three collision-free paths are spliced ​​to generate the final Y-shaped turning path. The final Y-shaped turning path is shown in the attached figure. Figure 4 shown.

[0048] In some embodiments of the present invention, step S5 specifically includes the following steps.

[0049] S51: Determine path search parameters based on the grid map and the integrated geometric model. The path search parameters include but are not limited to search length, vehicle steering angle range, and step length.

[0050] S52: Based on the path search parameters, search and collision test are performed from the end position to the edge of the land to determine the end position (x be ,y be ,θ be ), and the end position of the reverse vehicle (xbe ,y be ,θ be ) to the final pose (x e ,y e ,θ e ) collision-free path.

[0051] In some embodiments of the present invention, the implementation method of step S52 specifically includes the following steps: Based on the path search parameters by the end pose (x e ,y e ,θ e ) searches toward the boundary of the land, calculates the curvature of the path trajectory based on the vehicle steering angle range, calculates the step size of the angle change on the discrete path based on the curvature, and then calculates the discrete pose points on the path.

[0052] The integrated geometric model is positioned at discrete pose points on the path, and a collision test of the integrated geometric model is performed in combination with the grid map. If the integrated geometric model does not collide with the environment at all pose points on the path, the last pose point on the path is determined as the reversing end pose, and the path is determined as the reversing end pose (x be ,y be ,θ be ) to the final pose (x e ,y e ,θ e ) collision-free path.

[0053] S53: Based on the path search parameters, the reversing end posture (x be ,y be ,θ be ) to search and perform collision tests in the direction of the crop row to determine the reversing starting position (x bs ,y bs ,θ bs ), and the pose path starting from reverse (x bs ,y bs ,θ bs ) to the reversing end position (x be ,y be ,θ be )’s collision-free path; In some embodiments of the present invention, the implementation method of step S53 specifically includes the following steps: Based on the path search parameters, the reversing end pose (x be ,y be ,θ besearching in the direction of crop row, calculating the curvature of path trajectory based on the range of vehicle steering angle, calculating the change step of angle on the discrete path based on the curvature, and further calculating the discrete pose points on the path; 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 start pose (x bs ,y bs ,θ bs ) and determining the path as the collision-free path from the reverse start pose (x bs ,y bs ,θ bs ) to the reverse end pose (x be ,y be ,θ be ).

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

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

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

[0057] Some embodiments of the present application further provide an Akerman orchard robot field path planning system, comprising at least one processor.

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

[0059] When the at least one program is executed by the at least one processor, the at least one processor implements the above-mentioned Akerman orchard robot field path planning method.

[0060] 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 Akerman orchard robot field path planning method when executed by a processor.

[0061] Finally, it should be noted that: the various embodiments in the specification are 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.

[0062] 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 scheme of the present application, they should be covered in the technical scheme range of the present application.

Claims

1. An Ackerman-type orchard robot path planning method, characterized in that: The following steps are involved: S1: Construct a grid map containing orchard crop rows and turning areas at the headlands; S2: Construct an integrated geometric model of the Ackerman vehicle and its mounted equipment; S3: Calculating the starting position and ending position of the turning path to be planned in the grid map based on the driving direction of the vehicle in the current operation row, the head offset of the current operation row, and the head offset of the next operation row; S4: performing a collision-free path search for the integrated geometric model using the Hybrid A* algorithm based on the grid map. If a collision-free U-turn path that satisfies the turning radius of the current turning area is obtained, the path planning is terminated. Otherwise, execute step S5 to perform Y-turn path planning; S5: Based on the grid map and the integrated geometric model, searches and collision tests are performed in the direction of the field boundary and the direction of the crop row in turn to determine the reversing end posture and the reversing start posture of the Y-shaped turning path; segmented path planning is performed based on the reversing end posture and the reversing start posture to obtain three collision-free paths; the three collision-free paths are spliced ​​to generate the final Y-shaped turning path.

2. The Ackerman-type orchard robot path planning method according to claim 1, characterized in that: Step S5 specifically includes the following steps: 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 steering angle range, and a step length; S52: performing a search and collision test from the end position toward the end of the road based on the path search parameters to determine the end position of the reverse vehicle for the Y-turn path, and a collision-free path from the end position to the end position; S53: performing a search and collision test from the reversing end position toward the crop row direction based on the path search parameters to determine a reversing start position of the Y-shaped turning path and a collision-free path from the reversing start position to the reversing end position; S54: Calculating a collision-free path of the integrated geometric model from the starting position to the reversing starting position using a Hybrid A* algorithm based on the grid map; S55: splice the three collision-free paths, mark the driving direction for each path, and generate a final Y-shaped turning path.

3. The Ackerman-type orchard robot path planning method according to claim 2, characterized in that: The implementation method of step S52 specifically includes the following steps: Searching from the end position toward the end of the road based on the path search parameters, calculating the curvature of the path trajectory based on the vehicle steering angle range, calculating the step size of the angle change on the discrete path based on the curvature, and then calculating the discrete position points on the path; The integrated geometric model is positioned at discrete pose points on the path, and a collision test of the integrated geometric model is performed in combination with the grid map. If the integrated geometric model does not collide with the environment at all pose points 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.

4. The Ackerman-type orchard robot path planning method according to claim 2, characterized in that: The implementation method of step S53 specifically includes the following steps: Based on the path search parameters, searching is performed from the reversing end posture toward the crop row direction, the curvature of the path trajectory is calculated based on the vehicle steering angle range, the step length of the angle change on the discrete path is calculated based on the curvature, and the discrete posture points on the path are further calculated; The integrated geometric model is positioned at discrete pose points on the path, and a collision test of the integrated geometric model is performed in combination with the grid map. If the integrated geometric model does not collide with the environment at all pose points 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.

5. The Ackerman-type orchard robot path planning method according to claim 1, characterized in that: Step S2 specifically includes the following steps: Configure the geometric parameters of the Ackerman vehicle and its mounted equipment; the geometric parameters of the Ackerman vehicle include but are not limited to the length, width, height, wheelbase and track of the Ackerman vehicle; the geometric parameters of the mounted equipment include but are not limited to the length, width, height of the bounding box of the mounted equipment and the distance from the vehicle; Constructing geometric models of the Ackermann vehicle and its mounted equipment based on the geometric parameters of the Ackermann vehicle and its mounted equipment; The geometric models of the Ackerman vehicle and its mounted equipment are integrated to construct an integrated geometric model.

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

7. The Ackerman-type orchard robot path planning method according to claim 1, characterized in that: In step S1, a grid map including orchard crop rows and turnable areas at the end of the field is constructed based on LiDAR point cloud technology or GNSS measurement technology.

8. The Ackerman-type orchard robot path planning method according to claim 1, characterized in that: In step S5, if the final Y-shaped turning path cannot be generated, the path planning is terminated.

9. An Ackerman-type orchard robot field 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 Ackermann-type orchard robot field path planning method as described in any one of claims 1 to 8.

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

Images