Agricultural machine path planning method and device, electronic equipment and storage medium

Abstract

The application provides a path planning method and device for agricultural machinery, electronic equipment and a storage medium, wherein the method comprises the following steps: determining a starting pose and a target pose of the agricultural machinery; performing node searching according to the starting pose to obtain an extended node of the agricultural machinery, and determining whether path planning is completed according to a first pose of a first node in the extended node and the target pose, wherein the first node is the last node in the extended node; when it is determined that the path planning is not completed, performing path extension according to the first node to obtain an extended path; obtaining an initial path of the agricultural machinery according to the extended node and the extended path, and performing smoothing and interpolation processing on the initial path to obtain a target path of the agricultural machinery. The number and time of search nodes are reduced, and the accuracy and efficiency of path planning are improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of train dynamics simulation, and particularly relates to a path planning method and device for agricultural machinery, an electronic device and a storage medium. BACKGROUND

[0002] With the development of automatic driving technology, automatic driving technology is also being applied to various fields, such as agricultural operation scenarios. Because of the use of automatic driving technology, the work efficiency of agricultural machinery is improved.

[0003] For the automatic driving of agricultural machinery, accurate and reasonable path planning is required. The operation process of agricultural machinery mainly includes straight or curved operation stage, turning stage, obstacle avoidance stage and material supplement stage (such as adding seeds and fertilizer). Therefore, it is necessary to reasonably plan the operation path of agricultural machinery.

[0004] At present, when planning the path, the agricultural machinery is repeatedly operated to ensure that all operation areas are operated. However, this method is not reasonable for path planning, which reduces the operation efficiency of agricultural machinery. SUMMARY

[0005] The present application provides a path planning method and device for agricultural machinery, an electronic device and a storage medium to solve the problem of low efficiency and unreasonable path planning in the prior art.

[0006] The present application provides a path planning method for agricultural machinery, comprising:

[0007] determining the initial pose and target pose of the agricultural machinery;

[0008] performing node search according to the initial pose to obtain an extended node of the agricultural machinery, and determining whether path planning is completed according to a first pose of a first node in the extended node and the target pose, wherein the first node is the last node in the extended node;

[0009] when it is determined that the path planning is not completed, performing path extension according to the first node to obtain an extended path;

[0010] obtaining an initial path of the agricultural machinery according to the extended node and the extended path, and performing smoothing and interpolation processing on the initial path to obtain a target path of the agricultural machinery.

[0011] According to the path planning method for agricultural machinery provided by the present application, before the initial pose and target pose of the agricultural machinery are determined, the method further comprises:

[0012] Obtain a vector map of a target map in a map database, and obtain a road network topology map, a grid map and a region map corresponding to the target map according to the vector map and operation information of the target map;

[0013] Perform cost calculation according to the grid map and the region map to obtain a cost map corresponding to the target map.

[0014] According to the path planning method of the agricultural machine provided by the application, the starting pose and the target pose of the agricultural machine are determined, which comprises:

[0015] Determine the world coordinates of the starting position and the target position of the agricultural machine;

[0016] According to the target map, a map coordinate system is constructed, and the starting coordinates and the target coordinates of the agricultural machine in the map coordinate system are determined according to the world coordinates;

[0017] Obtain the orientation information of the agricultural machine, and determine the starting pose and the target pose of the agricultural machine according to the orientation information, the starting coordinates and the target coordinates.

[0018] According to the path planning method of the agricultural machine provided by the application, the node search is performed according to the starting pose to obtain the extended node of the agricultural machine, which comprises:

[0019] Calculate the heuristic value of each grid in the target map to the target coordinates, wherein the heuristic value comprises an unobstructed heuristic value and an obstructed heuristic value;

[0020] Determine an initial node expansion step, and determine a node search set corresponding to each node search, wherein the node expansion step is the number of node searches, an intermediate node is determined when each node search is completed, and each intermediate node corresponds to a node search set;

[0021] In the node search set, perform node search according to the cost map and the heuristic value to obtain the node obtained by each node search, and update the node expansion step in a decreasing manner when one node search is completed;

[0022] When the node expansion step is updated to zero, the extended node of the agricultural machine is obtained according to the cumulative cost of node search.

[0023] According to the path planning method of the agricultural machine provided by the application, whether the path planning is completed is determined according to the first pose of the first node in the extended node and the target pose, which comprises:

[0024] Obtain the first pose of the first node;

[0025] determining whether a first coordinate in the first pose and the target coordinate are in a same node, and determining whether an angle difference between a first angle in the first pose and a target angle in the target pose is less than or equal to a preset value;

[0026] if it is determined that the first coordinate and the target coordinate are in the same node and the angle difference is less than or equal to the preset value, it is determined that path planning is completed;

[0027] if it is determined that the first coordinate and the target coordinate are not in the same node or the angle difference is greater than the preset value, it is determined that path planning is not completed.

[0028] According to the path planning method of the agricultural machine provided by the application, after the path expansion is performed according to the first node to obtain an expanded path when it is determined that the path planning is not completed, the method further comprises the following steps:

[0029] determining whether the expanded path passes through an obstacle region, wherein the region map comprises an obstacle region and a non-obstacle region;

[0030] if it is determined that the expanded path does not pass through the obstacle region, an initial path of the agricultural machine is obtained according to the expanded node and the expanded path;

[0031] if it is determined that the expanded path passes through the obstacle region, it is determined that the expanded path is abnormal, and an intermediate node expansion step length of the agricultural machine at the first node is updated, so that node expansion and path expansion are performed according to the intermediate node expansion step length, until the obtained expanded path passes through the obstacle region.

[0032] According to the path planning method of the agricultural machine provided by the application, the initial path is smoothed and interpolated to obtain a target path of the agricultural machine, which comprises the following steps:

[0033] constructing a target function, wherein the target function comprises an obstacle penalty function, a curvature penalty function, a smoothness penalty function and a road network deviation penalty function;

[0034] calculating according to the target function to obtain curvatures and interpolations between nodes in the initial path;

[0035] smoothing and interpolating the initial path according to the curvatures and interpolations to obtain a target path.

[0036] The application further provides a path planning device of an agricultural machine, which comprises:

[0037] an agricultural machine positioning module configured to determine a starting pose and a target pose of the agricultural machine;

[0038] a node expansion module configured to perform node search according to the initial pose to obtain an expanded node of the agricultural machine, and determine whether path planning is completed according to a first pose of a first node in the expanded node and the target pose, wherein the first node is the last node in the expanded node;

[0039] a path expansion module configured to perform path expansion according to the first node to obtain an expanded path when it is determined that path planning is not completed;

[0040] a path generation module configured to obtain an initial path of the agricultural machine according to the expanded node and the expanded path, and perform smoothing and interpolation processing on the initial path to obtain a target path of the agricultural machine.

[0041] The present application also provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the path planning method of the agricultural machine according to any one of the above when executing the program.

[0042] The present application also provides a non-transitory computer readable storage medium having a computer program stored thereon, wherein the computer program is executable on a processor to implement the path planning method of the agricultural machine according to any one of the above.

[0043] The present application provides a path planning method, device, electronic device and storage medium of an agricultural machine, which first determines an initial pose and a target pose of the agricultural machine when planning a path for the agricultural machine, then performs node search according to the initial pose to obtain an expanded node of the agricultural machine, and determines whether path planning is completed according to the expanded node, and further performs path expansion according to the last node in the expanded node to obtain an expanded path when it is determined that path planning is not completed, and further obtains an initial path according to the expanded node and the expanded path when it is determined that the expanded path is feasible, and finally obtains a target path of the agricultural machine through smoothing and interpolation processing on the initial path. The number and time of search nodes are reduced, and the accuracy and efficiency of path planning are improved. BRIEF DESCRIPTION OF DRAWINGS

[0044] In order to more clearly illustrate the technical solutions of the present application or the prior art, the following will briefly introduce the drawings needed in the embodiments or prior art description. Obviously, the drawings in the following description are some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0045] Figure 1 is a flowchart of the path planning method of the agricultural machine provided by the present application;

[0046] Figure 2 is a schematic diagram of a vector map provided by the present application;

[0047] Figure 3 This is a schematic diagram of the regional map provided by the present invention;

[0048] Figure 4 This is a schematic diagram of the road network topology provided by the present invention;

[0049] Figure 5 This is a schematic diagram of the node expansion method provided by the present invention;

[0050] Figure 6 This is a flowchart illustrating the steps for obtaining extended nodes provided by the present invention;

[0051] Figure 7 This is a flowchart illustrating the steps for determining whether path planning is complete, as provided by the present invention.

[0052] Figure 8 This is a schematic diagram of the path planning process for agricultural machinery provided by the present invention;

[0053] Figure 9 This is a schematic diagram of the path planning device for agricultural machinery provided by the present invention;

[0054] Figure 10 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0055] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0056] The following is combined with Figures 1-8 This invention describes a path planning method for agricultural machinery. During agricultural machinery operations, maneuvers such as turning around, obstacle avoidance, and relocation are inherent to agricultural machinery. However, as operations progress, the area of ​​farmland where work has already been completed continuously increases. To avoid the impact of directly traversing already completed areas, reasonable path planning is necessary. This method aims to more rationally and effectively plan the path for agricultural machinery while avoiding direct passage through already completed areas, in order to facilitate subsequent maneuvering operations such as turning around or relocation.

[0057] Reference Figure 1 , Figure 1 This is a flowchart illustrating the path planning method for agricultural machinery provided by the present invention, wherein the method includes:

[0058] Step 101: Determine the starting pose and target pose of the agricultural machinery.

[0059] In the path planning of the agricultural machine, the current position of the agricultural machine and the position to be reached need to be determined, and the size difference of the agricultural machine compared with the traditional car is considered to avoid the path that is not easy to walk. In the path planning, the initial pose and the target pose of the agricultural machine are determined.

[0060] The initial pose includes the actual position information and the orientation information of the agricultural machine, such as the coordinate information and the direction information in the world coordinate. The target pose is the place and the attitude that the agricultural machine needs to reach. Usually, in order to conveniently complete the path planning of the agricultural machine, the position division and positioning of the farmland where the agricultural machine works are carried out. Specifically, in determining the initial pose and the target pose of the agricultural machine, the combination of coordinates and angles can be used for identification. For example, in the farmland map, the position of the agricultural machine is determined by using two-dimensional coordinates, and then the orientation of the agricultural machine is represented by using angles. The angle can be the angle with any one axis in the two-dimensional coordinate.

[0061] In determining the initial pose and the target pose of the agricultural machine, the map of the farmland where the agricultural machine works needs to be loaded in advance, and then the positioning of the agricultural machine in the farmland is completed according to the obtained map. Specifically, the world coordinates of the initial position and the target position of the agricultural machine are determined; the map coordinate system is constructed according to the target map, and the initial coordinates and the target coordinates of the agricultural machine in the map coordinate system are determined according to the world coordinates; the orientation information of the agricultural machine is obtained, and the initial pose and the target pose of the agricultural machine are determined according to the orientation information, the initial coordinates and the target coordinates.

[0062] That is, the world coordinates of the initial position and the target position of the agricultural machine are determined, and the map coordinate system is constructed according to the target map. Then, the positioning in the map coordinate system is realized according to the world coordinates, the initial coordinates and the target coordinates in the map coordinate system are obtained, and finally the orientation information of the agricultural machine at the initial position and the target position is fused with the initial coordinates and the target coordinates to obtain the initial pose and the target pose of the agricultural machine, wherein the initial pose is x s =<x,y,θ> s , and the target position is x g =<x,y,θ> g .

[0063] After obtaining the target map, corresponding processing is carried out. Specifically, the vector map of the target map is obtained in the map database, and the road network topology graph, the grid map and the area map corresponding to the target map are obtained according to the vector map and the working information of the target map; the cost calculation is carried out according to the grid map and the area map to obtain the cost map corresponding to the target map.

[0064] For example, when performing route planning, the vector map of the target map where the agricultural machinery will operate is first obtained from the map database. Then, based on the operation information of the vector map and the target map, the road network topology map, grid map and regional map of the target map are obtained. At the same time, the crossing cost of the agricultural machinery crossing the farmland is introduced, and the cost corresponding to the agricultural machinery crossing each area in the target map is calculated based on the grid map and regional map, so as to obtain the cost map of the target map.

[0065] The vector map is a regional display of the target map, allowing selection of the entire target area. Typically, the vector map defines the entire area of ​​the target map, marking boundaries such as farmland, work zones, non-work zones, and obstacles. The resulting vector map can be displayed as follows: Figure 2 As shown, in Figure 2 The vector map shown has marked the boundaries and obstacles of the target farmland, which allows for accurate division when obtaining the area map of the target farmland.

[0066] After obtaining the vector map of the target map, the corresponding road network topology map, grid map, and regional map will be generated based on the operational information of the vector map and the target map. The operational information of the target map includes the operational status of agricultural machinery on the farmland corresponding to the target map, such as which areas are being worked on, which areas have been completed, and which areas require work. Simultaneously, based on the vector map, the target map can be divided into regions to facilitate subsequent route planning. Typically, farmland areas can be divided into drivable and non-drivable areas. Drivable areas include worked areas, U-turn areas, waiting areas, and non-work areas, while non-drivable areas include areas with fixed obstacles, manually designated areas, and temporary obstacle areas.

[0067] And based on Figure 2 When the vector map shown is used to obtain a regional map, the resulting regional map can be as follows: Figure 3 As shown, in Figure 3 In the area map shown, the areas where obstacles are located are divided into non-driving areas. At the same time, according to the actual operation, the areas in the obtained area map are: the completed area (sowing), the non-operational area, the turning area, the waiting area, the completed area (harrowing), and the non-driving area. Of course, as the agricultural machinery operation proceeds, the area map will also be updated and changed.

[0068] In addition, when processing based on the vector map and the target map, the road network topology map and the grid map of the target map can also be obtained. In actual application, considering the actual operation of the agricultural machine and the actual parameters of the agricultural machine, including the outer dimensions of the implement, the working width, the turning radius of the agricultural machine, the wheelbase, the outer dimensions of the agricultural machine, and the working speed, in order to more reasonably plan the path of the agricultural machine to control the reasonable operation of the agricultural machine, the road network topology map of the target map is planned, as shown in Figure 4

[0069] When generating the cost map of the target map, considering that the weights of different regions are different, the traversal costs of different regions are different, and different grids in the same region have different traversal costs due to the difference in position. When calculating the traversal cost, first, the grid processing is performed on the combination of the vector map and the region map, and then the traversal cost of each grid is calculated.

[0070] When calculating the traversal cost of each grid, the specific process is as follows: assuming that the perpendicular distance of the line segment AB of the point P(x p ,y p ) is h, then:

[0071]

[0072] wherein A(x a ,y a ), B(x b ,y b ) are two end points of the line segment AB.

[0073] Since the weights of different regions are different, such as the weight of the sown region being greater than that of the plowed region, and the weight of the region after the crop emergence being greater than that of the region without crop emergence, the different regions are assigned with weights w z The final weight of each grid is:

[0074] d(p i ) = h i w z ;

[0075] For example, the farmland boundary region often has facilities such as power poles or fences, in order to prevent the planned path from being too close to the farmland boundary and causing safety hazards. The cost of the region near the farmland boundary is inversely proportional to the distance from the farmland boundary, such as the non-working area. The cost function is as follows:

[0076]

[0077] wherein k is a normal number, and a is a value greater than 1, which can be adjusted as needed. When a is larger, the curve becomes steeper near the farmland boundary, that is, the cost becomes higher at a shorter distance.​

[0078] In addition, the grid traversal cost in the U-turn area and the area to be worked is the same, that is, the traversal cost of the agricultural machine is accumulated linearly with the distance when the agricultural machine travels in the area.

[0079] By completing the calculation of the traversal cost of each grid in all areas, the cost map corresponding to the target map can be obtained by summarizing and normalizing.

[0080] Further, after completing the processing based on the vector map and the target map, the obtained maps will be used for subsequent map planning and path smoothing and the like.

[0081] In step 102, node search is performed according to the starting pose to obtain an extended node of the agricultural machine, and whether path planning is completed is determined according to the first pose of the first node in the extended node and the target pose, wherein the first node is the last node in the extended node.

[0082] Specifically, after the starting pose and the target pose of the agricultural machine are determined, path planning is performed to obtain a path from the starting position to the target position. When planning the path, node search is performed according to the starting pose to obtain an extended node of the agricultural machine, and whether path planning is completed is determined according to the first pose of the first node in the extended node and the target pose, and the first node is the last node when the node is extended.

[0083] Exemplarily, when performing node search, the node that can be currently extended can be obtained according to the current node, and when determining the node that can be extended, three steering actions can be applied to obtain, and the three steering actions include: maximum left turn, no turn, maximum right turn and reverse, and then the node search of the current time is completed in the obtained node. For node expansion, the corresponding expansion effect can be as shown in Figure 5 , Figure 5 It can be seen that for the current node, when performing node search, the position of the next node has six, such as when planning the path, the most suitable node is selected from the six nodes as the next node for search and expansion. When determining the next node, the node search is performed by calculating the traversal cost to obtain a node with lower cost as the next node.

[0084] In the actual path planning process, multiple node searches and expansions are usually required to complete the path planning, that is, after completing a node search, node search needs to be continued, and the finally planned path is obtained by continuously searching and expanding. Therefore, when performing node search according to the starting pose to obtain an extended node, the following Figure 5 , Figure 6is a flowchart of steps for obtaining an extended node, wherein the steps include steps 601 to 604.

[0085] In step 601, a heuristic value of each grid in the target map to the target coordinate is calculated, wherein the heuristic value includes an unobstructed heuristic value and an obstructed heuristic value.

[0086] In step 602, an initial node extension step is determined, and a node search set corresponding to each node search is determined, wherein the node extension step is the number of node searches, an intermediate node is determined when each node search is completed, and each intermediate node corresponds to a node search set.

[0087] In step 603, node search is performed according to the cost map and the heuristic value in the node search set, a node obtained by each node search is obtained, and the node extension step is updated in a decreasing manner when each node search is completed.

[0088] In step 604, when the node extension step is updated to zero, an extended node of the agricultural machine is obtained according to the cumulative cost of the node search.

[0089] In the node search and extension, since the planned path is uncertain, an initial node extension step can be set in advance, that is, the number of node searches and extensions is preset, and then the subsequent path planning method is adjusted according to the actual search situation in the process of node search and extension.

[0090] As described above, in path planning, in addition to considering the distance factor, the cost generated by the agricultural machine in the traversal process is also considered, and in order to more reasonably and accurately plan the path, the influence of obstacles on path planning is also considered when planning. Specifically, when planning, a heuristic value of each grid in the target map to the target coordinate is calculated, wherein the heuristic value includes an unobstructed heuristic value and an obstructed heuristic value, and then the heuristic value is introduced into the node determination process when the node search and extension are performed.

[0091] In the calculation of the heuristic value, a hybrid A* algorithm can be used for calculation. For the obtained heuristic value, the obstacle-free heuristic value is the under-actuated obstacle-free heuristic value, which ignores obstacles but considers the kinematic constraints of the nonholonomic under-actuated vehicle, that is, assuming that the environment is obstacle-free, the shortest path from the grid to the target point is calculated using the Reeds-Shepp curve. The obstacle heuristic value is the full-actuated obstacle heuristic value, which, contrary to the first heuristic function, ignores the nonholonomic nature of the vehicle but utilizes the obstacle grid map to calculate the shortest Euclidean distance from the target point to the grid in two-dimensional space by using the traditional A* or dynamic programming method. Finally, in the determination of the generation value, the larger one is selected as the cost of passing through the grid.

[0092] In addition, in the node search, the initial node expansion step is determined, and the node search set corresponding to each node search is determined, which can be obtained in the manner shown in Figure 5 After the corresponding intermediate node is determined by completing a search, the next search is entered, and the initial node expansion step is updated in a decreasing manner when a search is completed. For example, if the initial node expansion step is N, the updated node expansion step is N-1 after completing a node search, that is, the node expansion step is the number of times of node search. Finally, when the node expansion step is updated to zero, it is determined that the node search is completed, and the node obtained by each search is the expansion node of the agricultural machine.

[0093] In actual planning, since the initial node expansion step is randomly set, the expansion node of the agricultural machine obtained by completing the node search may not complete the path planning. If there is a large difference between the last node obtained at this time and the node to which the target pose belongs, further processing, including path expansion and node expansion, is required. Therefore, after the node search is completed to obtain the expansion node, it is necessary to determine whether the path planning is completed. At this time, the method shown in Figure 7 , Figure 7 is a flowchart of the step of determining whether the path planning is completed, provided by the application. The step includes steps 701 to 704.

[0094] Step 701: obtaining a first pose of a first node;

[0095] Step 702: determining whether the first coordinate in the first pose and the target coordinate are in the same grid, and determining whether the angle difference between the first angle in the first pose and the target angle in the target pose is less than or equal to a preset value;

[0096] Step 703: if it is determined that the first coordinate and the target coordinate are in the same grid, and the angle difference is less than or equal to the preset value, it is determined that the path planning is completed.

[0097] In step 704, if it is determined that the first coordinate and the target coordinate are not in the same grid or the angle difference is greater than the preset value, it is determined that the path planning is not completed.

[0098] Specifically, when determining whether the path planning is completed, the first pose of the first node is compared with the target pose, and when comparing, the comparison of the position and the comparison of the pose are included.

[0099] Illustratively, in the process of movement of the agricultural machine, in addition to the change of the position, the change of the pose, such as the change of the movement direction, is also included, wherein the pose is represented by the angle. In the actual planning process, it is not necessarily guaranteed that the pose of the agricultural machine at the last node is completely the same as the target pose, but it is necessary to guarantee that the two are approximately the same.

[0100] Therefore, when comparing, it is necessary to determine whether the first position of the first node and the target position in the target pose are in the same grid, and it is also necessary to determine the relationship between the first angle included in the first pose and the target angle included in the target pose. When determining the angle relationship, the angle difference between the first angle and the target angle can be calculated, and then the comparison of the pose can be completed according to the size of the angle difference.

[0101] Further, when determining whether the path planning is completed, it is necessary to guarantee that the first position of the first node and the target position are in the same grid, and it is also necessary to guarantee that the obtained angle difference is less than or equal to the set threshold value. The angle difference less than or equal to the set threshold value can effectively guarantee that the orientation of the agricultural machine at the first node is approximately the same as the orientation in the target pose. Otherwise, it is determined that the path planning is not completed, and further planning processing is needed.

[0102] In step 103, when it is determined that the path planning is not completed, the first node is used to expand the path to obtain an expanded path.

[0103] Specifically, after the node search and the node expansion are performed, when it is determined that the path planning is not completed, the first node obtained is used to expand the path to obtain an expanded path.

[0104] Illustratively, when the expanded node is obtained by the node search, a planned path can be obtained according to the obtained expanded node. However, since the first node does not reach the node corresponding to the target pose, it is necessary to plan the path for the subsequent part to complete the path planning between the first node and the target pose.

[0105] When planning the subsequent path according to the first node, path expansion between the first node and the target node (the node corresponding to the target pose) is performed based on the hybrid A* algorithm. Since the expanded path obtained when performing path expansion does not take into account the actual situation of the target map, including whether there are obstacles or whether the area is drivable.

[0106] Therefore, after obtaining the expanded path by performing path expansion, it is necessary to determine whether the expanded path is drivable, specifically including: determining whether the expanded path passes through an obstacle area, wherein the area map includes an obstacle area and a non-obstacle area; if it is determined that the expanded path does not pass through the obstacle area, obtaining an initial path of the agricultural machine according to the expanded node and the expanded path; if it is determined that the expanded path passes through the obstacle area, determining that the expanded path is abnormal, and updating the intermediate node expansion step of the agricultural machine at the first node to perform node expansion and path expansion according to the intermediate node expansion step until the obtained expanded path passes through the obstacle area.

[0107] That is, since the target map has an un-drivable area in the obtained area map when performing area division, and the expanded path obtained by performing path expansion based on the hybrid A* algorithm may pass through the obstacle area, at this time, the expanded path is determined whether it will pass through the obstacle area, and when it is determined that it does not pass through the obstacle area, it means that the agricultural machine can drive on the expanded path, and when it is determined that it passes through the obstacle area, it means that the agricultural machine cannot drive on the expanded path, then at this time, the node expansion needs to be performed again, and after completing the node expansion, the path expansion is performed again until the last node after completing the node expansion is approximately the target node, or the expanded path after completing the path expansion does not pass through the obstacle area.

[0108] It should be noted that when determining whether path planning is complete according to the expanded node, there may be a case where the first node is approximately the target node, that is, the path expansion is ended, at this time, the path expansion will not be performed.

[0109] Step 104, obtaining an initial path of the agricultural machine according to the expanded node and the expanded path, and performing smoothing and interpolation processing on the initial path to obtain a target path of the agricultural machine.

[0110] Specifically, after obtaining the expanded node and the expanded path, the initial path of the agricultural machine can be obtained by connecting the nodes and connecting the nodes and the expanded path. Since the initial path has several nodes connected, in order to ensure the normal and reasonable operation of the agricultural machine, smoothing and interpolation processing are performed on the initial path to obtain the target path of the agricultural machine.

[0111] Exemplarily, when the initial path is obtained, the initial path is obtained by connecting nodes, resulting in that the initial path is not smooth enough at the nodes and there is an inharmonious turning situation, therefore, the initial path is processed in combination with the road network topology map of the target map, so that the agricultural machine can be more stable during operation.

[0112] In the processing of the initial path, the target function is constructed, wherein the target function includes an obstacle penalty function, a curvature penalty function, a smoothness penalty function and a road network deviation penalty function; the curvature and the interpolation between the nodes in the initial path are obtained by calculation according to the target function; and the initial path is processed by smoothing and interpolation according to the curvature and the interpolation to obtain the target path.

[0113] That is, the related target function is constructed, and the initial path is constrained by the target function to realize the smoothing of the path at the nodes and between the nodes. For the constructed target function, the obstacle penalty function, the curvature penalty function, the smoothness penalty function and the road network deviation penalty function are included, wherein the road network deviation penalty function is obtained based on the road network topology map. In actual application, by limiting the operation mode of the agricultural machine, the operation of the agricultural machine is more reasonable and natural.

[0114] For the constructed target function, the following can be performed:

[0115]

[0116]

[0117]

[0118]

[0119]

[0120] P o The obstacle penalty term penalizes the distance between the trajectory point and the obstacle, and the closer the distance is, the higher the cost is; P c The curvature penalty term imposes an upper limit on the instantaneous curvature of each node, and the curvature of the path is constrained to make the path meet the nonholonomic constraint of the vehicle; P s The smoothness penalty term is used to measure the smoothness of the path, and this term is used to evaluate the direction change rate and distance of the displacement vector between the nodes, and the uneven distance and direction change between the nodes will cause the cost of this term to increase; w represents the road network deviation penalty term, and this term is used to evaluate the degree of deviation of the planned path from the road network topology map. o c c 、 σ represents the smoothness weight, and σ o ​​σ c Represents the quadratic penalty function, x i =[x i ,y i [ ] represents the trajectory point to be optimized, Δx i =x i+1 -x i κ is the displacement vector. max The maximum curvature allowed for agricultural machinery, Δφ i The change in the tangential angle of the displacement vector of the trajectory point is given by the following formula, which can be used to calculate the angle between the two vectors:

[0121]

[0122] therefore d m The maximum distance threshold to the obstacle. This is a road network topology graph, consisting of nodes and edges. Represents the trajectory point to The nearest neighbor distance is calculated. The gradients of each term are calculated and iterated; trajectory interpolation is performed. Curvature sampling is used to generate more path points, thus making the path smoother. Curvature sampling is a method of sampling based on the curvature distribution of the path, which can generate more path points, thus making the path smoother. Assume the initial path point set is... The curvatures are κ0, κ1, ..., κ n If the sampling interval is Δs, then at two adjacent path points and N is generated between i A new path point, the position of which is given by the following formula:

[0123]

[0124] in Is it along the initial path at point The unit tangent vector at κ i It is at point The curvature at the point. The location of the new path point consists of two parts: one is a linear interpolation part between two adjacent initial path points, and the other is a quadratic interpolation part, taking into account the curvature between two adjacent points, and more new points are generated closer to the curvature peak.

[0125] Furthermore, referring to Figure 8 , Figure 8 This is a schematic diagram of the path planning process for agricultural machinery provided by the present invention.

[0126] Reference Figure 8 When performing route planning, the following are included:

[0127] 1. Obtain the farmland vector map, which is obtained by artificial surveying or other methods, and the data is composed of points, lines and surfaces, including the boundary information of farmland, obstacle boundary information and non-operation area boundary information. Then generate a grid map based on the farmland vector map, which is used for node expansion of hybrid A* algorithm and calculation of grid traversal cost.

[0128] 2. Generate a regional map according to the agricultural machinery and operation task parameters and the generated farmland vector map.

[0129] 3. Generate the traversal cost of each grid by calculating the distance from each grid in the region to the nearest side of the region to obtain the cost map of the target map.

[0130] 4. Generate the farmland road network topology map based on the agricultural machinery and operation task parameters and the farmland vector map, wherein the main parameters include the outer dimensions of the machinery, the working width, the turning radius of the agricultural machinery, the wheelbase, the outer dimensions of the agricultural machinery and the working speed.

[0131] 5. When performing path planning, determine the initial pose and target pose, and then perform node search.

[0132] First, initialize the set and where O is the set of unsearched nodes, and the initial pose is placed in O, and the initial analysis expansion step N is set. At the same time, the map module calculates the two heuristic values h1(p i ), h2(p i ) corresponding to each grid, the former is the obstacle-free heuristic value, and the latter is the obstacle heuristic value.

[0133] 6. Determine whether O is empty, if yes, output that no feasible path is found, and end the search. If not, sort the elements in O according to the cumulative cost in the priority queue, push out the node x with the lowest cost, and place the node x with the lowest cost in C.

[0134] 7. Perform analysis expansion.

[0135] Including: determine whether N is 0, if N is 0, perform analysis expansion using the Reeds-Shepp curve, and update N to make N = int(h1(x) / k)+1, k is a constant used to adjust the frequency of analysis expansion. If not, update N to make N = N-1.

[0136] And determine whether the expanded path collides with the obstacle, if the expanded path collides with the obstacle, discard the expansion, and update N to make N = N-1. If there is no collision, add the expanded path to the initial path.

[0137] 8. Determine whether the node x is approximately equal to the target pose x g, if yes, the search ends and the initial path is generated, if no, the node expansion is started: the candidate node x is expanded to generate a child node x next .

[0138] When the candidate node x is expanded, first, the pose state of the current vehicle is associated with the initial search node, three steering actions are applied to the node, including maximum left turn, no turn, maximum right turn and reverse, to obtain six child nodes (as shown in Figure 4 ), and the kinematic model of the vehicle is used to generate the new child node pose state.

[0139] Then for each of these sub-states x next , the grid cell in which it is located is calculated. If there already exists a node with the same grid cell in the O list, and the cost g new of the new node is lower than the cost g(x next ) recorded by the grid before, the node predecessor, cumulative cost and heuristic value are updated, and it is re-placed in O for re-ordering.

[0140] In addition, if x next is not in O, the node is added to O, and its predecessor, cumulative cost and heuristic value are recorded.

[0141] And when g new is greater than or equal to g(x next ), the node attribute values are not updated.

[0142] 9、When x is approximately equal to x g , the node expansion ends and the initial trajectory is generated.

[0143] 10、The target function is constructed, the initial trajectory is smoothed and interpolated to obtain the final trajectory.

[0144] In the path planning method of the agricultural machine in the above embodiment, when the path of the agricultural machine is planned, first, the initial pose and the target pose of the agricultural machine are determined, then the node search is performed according to the initial pose to obtain the expanded nodes of the agricultural machine, and whether the path planning is completed is determined according to the expanded nodes, and then when it is determined that the path planning is not completed, the path is expanded according to the last node in the obtained expanded nodes to obtain the expanded path, and then when it is determined that the expanded path is feasible, the initial path is obtained according to the expanded nodes and the expanded path, and finally the target path of the agricultural machine is obtained by smoothing and interpolating the initial path. By reducing the number and time of search nodes, the accuracy and efficiency of path planning are improved.

[0145] The path planning device of the agricultural machine provided by the present application is described below, and the path planning device of the agricultural machine described below can be correspondingly referred to the path planning method of the agricultural machine described above.

[0146] Figure 9 is a structural schematic diagram of a path planning device of an agricultural machine provided by the present application, as shown in the figure, the path planning device 900 of the agricultural machine comprises: Figure 9

[0147] an agricultural machine positioning module 901, configured to determine a starting pose and a target pose of the agricultural machine;

[0148] a node expansion module 902, configured to perform node searching according to the starting pose to obtain an expanded node of the agricultural machine, and determine whether path planning is completed according to a first pose of a first node in the expanded node and the target pose, wherein the first node is the last node in the expanded node;

[0149] a path expansion module 903, configured to perform path expansion according to the first node to obtain an expanded path when it is determined that the path planning is not completed;

[0150] a path generation module 904, configured to obtain an initial path of the agricultural machine according to the expanded node and the expanded path, and perform smoothing and interpolation processing on the initial path to obtain a target path of the agricultural machine.

[0151] Based on the above embodiment, the agricultural machine positioning module 901 is further configured to:

[0152] obtain a vector map of a target map in a map database, and obtain a road network topology map, a grid map and a region map corresponding to the target map according to the vector map and operation information of the target map;

[0153] perform cost calculation according to the grid map and the region map to obtain a cost map corresponding to the target map.

[0154] Based on the above embodiment, the agricultural machine positioning module 901 is further configured to:

[0155] determine world coordinates of a starting position and a target position of the agricultural machine;

[0156] construct a map coordinate system according to the target map, and determine starting coordinates and target coordinates of the agricultural machine in the map coordinate system according to the world coordinates;

[0157] obtain orientation information of the agricultural machine, and determine the starting pose and the target pose of the agricultural machine according to the orientation information, the starting coordinates and the target coordinates.

[0158] Based on the above embodiment, the node expansion module 902 is further configured to:

[0159] calculate a heuristic value of each grid in the target map to the target coordinates, wherein the heuristic value comprises an unobstructed heuristic value and an obstructed heuristic value;

[0160] ​determining an initial node expansion step, and determining a node search set corresponding to each node search, wherein the node expansion step is the number of node searches, an intermediate node is determined when each node search is completed, and each intermediate node corresponds to a node search set;

[0161] In the node search set, node search is performed according to the cost map and the heuristic value, to obtain a node obtained by each node search, and the node expansion step is updated in a decreasing manner when each node search is completed;

[0162] When the node expansion step is updated to zero, the expansion node of the agricultural machine is obtained according to the cumulative cost of the node search.

[0163] Based on the above embodiment, the node expansion module 902 is further configured to:

[0164] obtain a first pose of the first node;

[0165] determine whether the first coordinate in the first pose and the target coordinate are in the same node, and determine whether the angle difference between the first angle in the first pose and the target angle in the target pose is less than or equal to a preset value;

[0166] If it is determined that the first coordinate and the target coordinate are in the same node, and the angle difference is less than or equal to the preset value, it is determined that the path planning is completed;

[0167] If it is determined that the first coordinate and the target coordinate are not in the same node, or the angle difference is greater than the preset value, it is determined that the path planning is not completed.

[0168] Based on the above embodiment, the path expansion module 902 is further configured to:

[0169] determine whether the expanded path passes through an obstacle region, wherein the region map includes an obstacle region and a non-obstacle region;

[0170] If it is determined that the expanded path does not pass through the obstacle region, an initial path of the agricultural machine is obtained according to the expansion node and the expanded path;

[0171] If it is determined that the expanded path passes through the obstacle region, it is determined that the expanded path is abnormal, and the intermediate node expansion step of the agricultural machine at the first node is updated, so as to perform node expansion and path expansion according to the intermediate node expansion step, until the obtained expanded path passes through the obstacle region.

[0172] Based on the above embodiment, the path generation module 904 is further configured to:

[0173] constructing an objective function, wherein the objective function includes an obstacle penalty function, a curvature penalty function, a smoothness penalty function, and a road network deviation penalty function;

[0174] According to the target function, the curvature and the interpolation between the nodes in the initial path are calculated;

[0175] According to the curvature and the interpolation, the initial path is smoothed and interpolated to obtain the target path.

[0176] In the path planning device of the agricultural machine in the above embodiment, when planning the path of the agricultural machine, the initial pose and the target pose of the agricultural machine are first determined, then the nodes are searched according to the initial pose to obtain the extended nodes of the agricultural machine, and it is determined whether the path planning is completed according to the extended nodes, and then when it is determined that the path planning is not completed, the path is extended according to the last node in the obtained extended nodes to obtain the extended path, and then when it is determined that the extended path is feasible, the initial path is obtained according to the extended nodes and the extended path, and finally the target path of the agricultural machine is obtained by smoothing and interpolating the initial path. By reducing the number and time of searching nodes, the accuracy and efficiency of path planning are improved.

[0177] Figure 10 An example of a schematic diagram of the physical structure of an electronic device is shown in Figure 10 As shown, the electronic device can include a processor 1010, a communication interface 1020, a memory 1030, and a communication bus 1040, wherein the processor 1010, the communication interface 1020, and the memory 1030 communicate with each other through the communication bus 1040. The processor 1010 can call the logical instructions in the memory 1030 to execute the path planning method of the agricultural machine, which includes determining the initial pose and the target pose of the agricultural machine, searching the nodes according to the initial pose to obtain the extended nodes of the agricultural machine, and determining whether the path planning is completed according to the first pose of the first node in the extended nodes and the target pose, wherein the first node is the last node in the extended nodes; when it is determined that the path planning is not completed, the path is extended according to the first node to obtain the extended path; according to the extended nodes and the extended path, the initial path of the agricultural machine is obtained, and the initial path is smoothed and interpolated to obtain the target path of the agricultural machine.

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

[0179] In another aspect, the present application also provides a computer program product, which comprises a computer program, the computer program can be stored on a non-transitory computer readable storage medium, and the computer program can be executed by a processor to enable a computer to execute the path planning method of the agricultural machine provided by the above-mentioned methods. The method comprises: determining the initial pose and the target pose of the agricultural machine; performing node search according to the initial pose to obtain an extended node of the agricultural machine, and determining whether the path planning is completed according to the first pose of the first node in the extended node and the target pose, wherein the first node is the last node in the extended node; when it is determined that the path planning is not completed, performing path extension according to the first node to obtain an extended path; obtaining an initial path of the agricultural machine according to the extended node and the extended path, and performing smoothing and interpolation processing on the initial path to obtain a target path of the agricultural machine.

[0180] In another aspect, the present application also provides a computer program product, which comprises a computer program, the computer program can be stored on a non-transitory computer readable storage medium, and the computer program can be executed by a processor to enable a computer to execute the path planning method of the agricultural machine provided by the above-mentioned methods. The method comprises: determining the initial pose and the target pose of the agricultural machine; performing node search according to the initial pose to obtain an extended node of the agricultural machine, and determining whether the path planning is completed according to the first pose of the first node in the extended node and the target pose, wherein the first node is the last node in the extended node; when it is determined that the path planning is not completed, performing path extension according to the first node to obtain an extended path; obtaining an initial path of the agricultural machine according to the extended node and the extended path, and performing smoothing and interpolation processing on the initial path to obtain a target path of the agricultural machine.

[0181] The device embodiments described above are merely illustrative, wherein the units described as separate components can or can not be physically separate, and the components displayed as units can or can not be physical units, i.e., can be located in one place, or can be distributed to multiple network units. Part or all of the modules can be selected to achieve the purposes of the embodiments according to actual needs. Those skilled in the art can understand and implement without creative labor.

[0182] Through the description of the above embodiments, those skilled in the art can clearly understand that the embodiments can be realized by means of software and the necessary general hardware platform, and of course can also be realized by hardware. Based on such understanding, the above technical solutions can be embodied in the form of a software product, which can be stored in a computer readable storage medium, such as a ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions to make a computer device (which can be a personal computer, a server, or a network device, etc.) execute the methods described in each embodiment or some parts of the embodiments.

[0183] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, and not to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement for part of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A path planning method for agricultural machinery, characterized in that, The method includes: Determine the initial and target positions of the agricultural machinery; Node search is performed based on the initial pose to obtain the extended nodes of the agricultural machinery, and the path planning is determined to be completed based on the first pose of the first node in the extended nodes and the target pose, wherein the first node is the last node in the extended nodes. If the path planning is not completed, the extended path is obtained by expanding the path based on the first node. Based on the extended nodes and the extended paths, the initial path of the agricultural machinery is obtained, and the initial path is smoothed and interpolated to obtain the target path of the agricultural machinery. The step of performing node search based on the initial pose to obtain the extended nodes of the agricultural machinery includes: Calculate the target coordinate heuristic value for each grid in the target map, where the heuristic value includes an accessibility heuristic value and an obstacle-prone heuristic value; Determine the initial node expansion step size and the node search set corresponding to each node search, wherein the node expansion step size is the number of node searches, and an intermediate node is determined when each node search is completed, and each intermediate node corresponds to a node search set. In the node search set, node search is performed based on the cost map and the heuristic value to obtain the node obtained in each node search, and the node expansion step size is updated in a decreasing manner when a node search is completed. When the node expansion step size is updated to zero, the expanded node of the agricultural machinery is obtained based on the cumulative cost of node search; Determining whether path planning is complete based on the first pose of the first node in the extended nodes and the target pose includes: Obtain the first pose of the first node; Determine whether the first coordinate in the first pose and the target coordinate are in the same grid, and determine whether the angle difference between the first angle in the first pose and the target angle in the target pose is less than or equal to a preset value; If it is determined that the first coordinate and the target coordinate are in the same grid, and the angle difference is less than or equal to the preset value, then the path planning is determined to be complete. If it is determined that the first coordinate and the target coordinate are not on the same grid, or the angle difference is greater than the preset value, then the path planning is determined to be incomplete. The process of smoothing and interpolating the initial path to obtain the target path of the agricultural machinery includes: The objective function is constructed, wherein the objective function includes an obstacle penalty function, a curvature penalty function, a smoothness penalty function, and a road network deviation penalty function; The curvature and interpolation between each node in the initial path are calculated based on the objective function. The initial path is smoothed and interpolated based on the curvature and interpolation to obtain the target path.

2. The path planning method for agricultural machinery according to claim 1, characterized in that, Before determining the initial and target poses of the agricultural machinery, the process also includes: Obtain the vector map of the target map from the map database, and obtain the road network topology map, grid map and regional map corresponding to the target map based on the vector map and the operation information of the target map; Cost calculations are performed based on the grid map and the area map to obtain the cost map corresponding to the target map.

3. The path planning method for agricultural machinery according to claim 2, characterized in that, Determining the initial pose and target pose of the agricultural machinery includes: Determine the world coordinates of the starting and target positions of the agricultural machinery; A map coordinate system is constructed based on the target map, and the starting and target coordinates of the agricultural machinery in the map coordinate system are determined based on the world coordinates. The orientation information of the agricultural machinery is obtained, and the initial pose and target pose of the agricultural machinery are determined based on the orientation information, the starting coordinates, and the target coordinates.

4. The path planning method for agricultural machinery according to claim 1, characterized in that, When it is determined that the path planning is not completed, after obtaining the extended path by expanding the path based on the first node, the method further includes: Determine whether the extended path passes through an obstacle area, wherein the area map includes obstacle areas and non-obstacle areas; If it is determined that the extended path does not pass through the obstacle area, then the initial path of the agricultural machinery is obtained based on the extended node and the extended path; If it is determined that the extended path passes through the obstacle area, then the extended path is determined to be abnormal, and the intermediate node extension step size of the agricultural machinery at the first node is updated, so as to perform node extension and path extension according to the intermediate node extension step size, until the obtained extended path passes through the obstacle area.

5. A path planning device for agricultural machinery, characterized in that, include: The agricultural machinery positioning module is used to determine the initial and target positions of the agricultural machinery. The node expansion module is used to perform node search based on the initial pose to obtain the expanded nodes of the agricultural machinery, and to determine whether path planning is completed based on the first pose of the first node in the expanded nodes and the target pose, wherein the first node is the last node in the expanded nodes. The process of searching for nodes based on the initial pose to obtain the extended nodes of the agricultural machinery includes: calculating the heuristic value for reaching the target coordinates of each grid in the target map, wherein the heuristic value includes an obstacle-free heuristic value and an obstacle-prone heuristic value; determining the initial node expansion step size and determining the node search set corresponding to each node search, wherein the node expansion step size is the number of node searches, and an intermediate node is determined when each node search is completed, and each intermediate node corresponds to a node search set; in the node search set, performing node searches based on the cost map and the heuristic values ​​to obtain the nodes obtained in each node search, and updating the node expansion step size in a decreasing manner when a node search is completed; when the node expansion step size is updated to zero, obtaining the extended nodes of the agricultural machinery based on the cumulative cost of the node searches; Determining whether path planning is complete based on the first pose of the first node in the extended nodes and the target pose includes: obtaining the first pose of the first node; determining whether the first coordinate in the first pose and the target coordinate are in the same grid, and determining whether the angle difference between the first angle in the first pose and the target angle in the target pose is less than or equal to a preset value; if it is determined that the first coordinate and the target coordinate are in the same grid, and the angle difference is less than or equal to the preset value, then path planning is determined to be complete; if it is determined that the first coordinate and the target coordinate are not in the same grid, or the angle difference is greater than the preset value, then path planning is determined to be incomplete. The path expansion module is used to expand the path based on the first node to obtain an expanded path when it is determined that the path planning is not completed. The path generation module is used to obtain the initial path of the agricultural machinery based on the extended nodes and the extended path, and to smooth and interpolate the initial path to obtain the target path of the agricultural machinery. This includes: constructing a target function, wherein the target function includes an obstacle penalty function, a curvature penalty function, a smoothness penalty function, and a road network deviation penalty function; calculating the curvature and interpolation between nodes in the initial path based on the target function; and smoothing and interpolating the initial path based on the curvature and interpolation to obtain the target path.

6. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the path planning method for agricultural machinery as described in any one of claims 1 to 4.

7. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the path planning method for agricultural machinery as described in any one of claims 1 to 4.

Images