A path planning method and device, electronic equipment and storage medium

Abstract

Embodiments of the present application disclose a path planning method and device, electronic equipment and a storage medium. The present application quickly and accurately constructs a three-dimensional grid map with obstacles by obtaining parameter information and obstacle information of a mobile tool, determines a target node according to a starting node, a longest planning distance and the obstacle information, wherein the starting node is a starting point of the mobile tool in the three-dimensional grid map, and the target node is an end point of the mobile tool in the three-dimensional grid map. Based on the obtained starting point and end point, the path trajectory from the starting point to the end point can be quickly and accurately calculated, the path planning of the mobile tool is realized, and the problem that the mobile tool cannot be timely and accurately planned in the related art is avoided.

Description

Technical Field

[0001] This application relates to the field of vehicle control technology, and more specifically, to a path planning method, apparatus, electronic device, and storage medium. Background Technology

[0002] With economic development, vehicles have become an indispensable product for people. However, when a vehicle is in motion, the driver's skill level and judgment have a significant impact on driving safety. Although ultrasonic radar and reversing camera assistance systems can provide warnings and alerts to the driver, accidents are difficult to completely avoid due to the complexity of the driving environment and the limitations of the driver's skill level.

[0003] In related technologies, driver assistance systems (ADAS) have been proposed to assist vehicle driving. To promote the development of ADAS, many countries are investing heavily in research, and the application areas of ADAS are constantly expanding. In everyday driving and parking scenarios, people expect ADAS to be able to autonomously plan routes and drive according to the planned path, which has spurred the development of forward-moving functions. However, current technology cannot plan vehicle routes accurately and in a timely manner. Summary of the Invention

[0004] The embodiments of this application provide a path planning method, apparatus, electronic device, and storage medium that can accurately plan parking trajectories, solving the problem in related technologies where path planning cannot be performed in a timely and accurate manner, thus limiting the assisted driving system.

[0005] Other features and advantages of this application will become apparent from the following detailed description, or may be learned in part from practice of this application.

[0006] According to one aspect of the embodiments of this application, a path planning method is provided, the path planning method comprising: acquiring parameter information of a mobile tool and obstacle information around the mobile tool; constructing a three-dimensional grid map for the mobile tool based on the parameter information, and determining a starting node and a maximum planned distance of the mobile tool on the three-dimensional grid map based on the parameter information; setting obstacles on the three-dimensional grid map based on the obstacle information, and determining a target node based on the obstacles set on the three-dimensional grid map, the starting node, and the maximum planned distance; and determining the path trajectory of the mobile tool based on the starting node and the target node.

[0007] In some examples, determining a target node based on obstacles set on the 3D grid map, the starting node, and the longest planned distance includes: determining multiple steering wheel angles and a fixed driving distance, wherein the fixed driving distance is lower than the longest planned distance; starting from the starting node, calculating the first target position in the 3D grid map after the mobile tool has traveled the fixed driving distance at different steering wheel angles, and using the first target position as an intermediate node, with each steering wheel angle corresponding to one intermediate node; and determining the target node based on the multiple intermediate nodes and the starting node.

[0008] In some examples, determining the target node based on a plurality of intermediate nodes and the starting node includes: determining the cost of each intermediate node and sorting the costs of the plurality of intermediate nodes in order; selecting the intermediate node with the lowest cost from the plurality of intermediate nodes, and determining the target node based on the intermediate node with the lowest cost and the starting node.

[0009] In some examples, determining the target node based on the intermediate node with the lowest cost and the starting node includes: calculating the distance from the intermediate node with the lowest cost to the starting node; if the distance from the intermediate node with the lowest cost to the starting node exceeds the longest planning distance, the intermediate node with the lowest cost is selected as the target node.

[0010] In some examples, determining the target node based on the intermediate node with the lowest cost and the starting node further includes: if the distance from the intermediate node with the lowest cost to the target node does not exceed the longest planned distance, starting from the intermediate node with the lowest cost, calculating the second target position in the three-dimensional grid map after the mobile tool travels the fixed distance at different steering wheel angles on the three-dimensional grid map, and redetermining multiple intermediate nodes based on the second target position; and determining the target node based on the redetermined multiple intermediate nodes and the starting node.

[0011] In some examples, determining the cost of each intermediate node includes: determining the parent node corresponding to each intermediate node, determining the steering wheel angle difference and gear position corresponding to each intermediate node based on the parent node, and determining the cost of each intermediate node based on the steering wheel angle difference and / or the gear position; and / or; determining the steering wheel angle corresponding to each intermediate node, and determining the cost of each intermediate node based on the steering wheel angle.

[0012] In some examples, determining the cost of each intermediate node includes at least one of the following verification methods: determining a reference trajectory, obtaining the lateral distance between each intermediate node and the reference trajectory, and determining the cost of each intermediate node based on the lateral distance; determining the minimum distance between each intermediate node and obstacles in the 3D grid map, and determining the cost of each intermediate node based on the minimum distance; determining the distance difference between each intermediate node and the starting node in the 3D grid map, and determining the cost of each intermediate node based on the distance difference.

[0013] In some examples, after determining the path trajectory of the mobile tool based on the starting node and the target node, the method further includes: smoothing the path trajectory and performing speed planning on the mobile tool to obtain an output trajectory; controlling the mobile tool to travel according to the output trajectory, and during the travel of the mobile tool, determining whether the mobile tool deviates from the output trajectory or whether the travel distance of the mobile tool exceeds a threshold; if the mobile tool deviates from the output trajectory or the travel distance of the mobile tool exceeds the threshold, re-acquiring the parameter information of the mobile tool and the obstacle information around the mobile tool.

[0014] According to one aspect of the embodiments of this application, a path planning device is provided, the path planning device comprising: an acquisition module, the acquisition module being configured to acquire parameter information of a mobile tool and obstacle information surrounding the mobile tool; a construction module, the construction module being configured to construct a three-dimensional grid map for the mobile tool based on the parameter information, and determine a starting node and a maximum planning distance of the mobile tool on the three-dimensional grid map based on the parameter information; a determination module, the determination module being configured to set obstacles on the three-dimensional grid map based on the obstacle information, and determine a target node based on the obstacles set on the three-dimensional grid map, the starting node, and the maximum planning distance; and a planning module, the planning module being configured to determine the path trajectory of the mobile tool based on the starting node and the target node.

[0015] According to one aspect of the embodiments of this application, an electronic device is provided, including one or more processors; and a storage device for storing one or more computer programs, which, when executed by the one or more processors, cause the electronic device to perform the method as described above.

[0016] According to one aspect of the embodiments of this application, an embodiment of this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor of an electronic device, causes the electronic device to perform the method described above.

[0017] In the technical solution provided by the embodiments of this application, parameter information of a mobile tool and obstacle information around the mobile tool are obtained; a three-dimensional grid map for the mobile tool is constructed based on the parameter information, and the starting node and the longest planned distance of the mobile tool on the three-dimensional grid map are determined based on the parameter information; obstacles are set on the three-dimensional grid map based on the obstacle information, and a target node is determined based on the obstacles set on the three-dimensional grid map, the starting node, and the longest planned distance; the path trajectory of the mobile tool is determined based on the starting node and the target node. This invention quickly and accurately constructs a three-dimensional grid map with obstacles by obtaining parameter information and obstacle information of the mobile tool, and determines the target node based on the starting node, the longest planned distance, and obstacle information. The starting node is the starting point of the mobile tool in the three-dimensional grid map, and the target node is the ending point of the mobile tool in the three-dimensional grid map. Based on the obtained starting and ending points, the path trajectory from the starting point to the ending point can be calculated quickly and accurately, realizing path planning for the mobile tool and avoiding the problem in related technologies where timely and accurate path planning for the mobile tool is not possible, leading to limitations in assisted driving.

[0018] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and do not limit this application. Attached Figure Description

[0019] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application. It is obvious that the drawings described below are merely some embodiments of this application, and those skilled in the art can obtain other drawings based on these drawings without any inventive effort. In the drawings:

[0020] Figure 1 This is a basic flowchart illustrating a path planning method in an exemplary embodiment of this application;

[0021] Figure 2 This is a basic flowchart illustrating a path planning method in an exemplary embodiment of this application;

[0022] Figure 3 This is a basic flowchart illustrating a path planning method in an exemplary embodiment of this application;

[0023] Figure 4 This is a basic flowchart illustrating a path planning method in an exemplary embodiment of this application;

[0024] Figure 5This is a basic flowchart illustrating a path planning method in an exemplary embodiment of this application;

[0025] Figure 6 This is a basic flowchart illustrating a path planning method in an exemplary embodiment of this application;

[0026] Figure 7 This is a basic flowchart illustrating a path planning method in an exemplary embodiment of this application;

[0027] Figure 8 This is a basic flowchart illustrating a path planning method in an exemplary embodiment of this application;

[0028] Figure 9 This is a basic flowchart illustrating a path planning method in an exemplary embodiment of this application;

[0029] Figure 10 This is a basic schematic diagram illustrating multiple steering wheel angles for path planning, as shown in an exemplary embodiment of this application;

[0030] Figure 11 This is a basic schematic diagram of a path planning device shown in an exemplary embodiment of this application;

[0031] Figure 12 A schematic diagram of the structure of a computer system suitable for implementing the electronic device of the present application is shown. Detailed Implementation

[0032] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application. Rather, they are merely examples of apparatuses and methods consistent with some aspects of this application as detailed in the appended claims.

[0033] The block diagrams shown in the accompanying drawings are merely functional entities and do not necessarily correspond to physically independent entities. That is, these functional entities can be implemented in software, in one or more hardware modules or integrated circuits, or in different network and / or processor devices and / or microcontroller devices.

[0034] The flowcharts shown in the accompanying diagrams are merely illustrative and do not necessarily include all content and operations, nor do they necessarily have to be executed in the described order. For example, some operations may be broken down, while others may be combined or partially combined; therefore, the actual execution order may change depending on the specific circumstances.

[0035] It should also be noted that "multiple" as mentioned in this application refers to two or more. "And / or" describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A alone, A and B simultaneously, or B alone. The character " / " generally indicates that the preceding and following related objects have an "or" relationship.

[0036] To address the aforementioned technical problems, embodiments of this application provide a path planning method, such as... Figure 1 As shown, the path planning method includes:

[0037] S101. Obtain parameter information of the mobile tool and obstacle information around the mobile tool;

[0038] S102. Construct a three-dimensional grid map for the mobile tool based on the parameter information, and determine the starting node and the longest planned distance of the mobile tool on the three-dimensional grid map based on the parameter information;

[0039] S103. Set obstacles on the three-dimensional grid map according to the obstacle information, and determine the target node based on the obstacles set on the three-dimensional grid map, the starting node, and the longest planning distance;

[0040] S104. Determine the path trajectory of the mobile tool based on the starting node and the target node.

[0041] The path planning method provided in this embodiment is applied to mobile vehicles, which can be any equipment with mobility capabilities, including vehicles with autonomous or intelligent driving functions (including passenger vehicles (e.g., cars, buses, coaches, minibuses, etc.), cargo vehicles (e.g., ordinary trucks, box trucks, trailer trucks, enclosed trucks, tank trucks, flatbed trucks, container trucks, dump trucks, special structure trucks), special vehicles (e.g., logistics delivery vehicles, automated guided vehicles (AGVs), patrol vehicles, cranes, excavators, bulldozers, loaders, road rollers, off-road engineering vehicles, armored engineering vehicles, sewage treatment vehicles, sanitation vehicles, vacuum trucks, floor scrubbers, water sprinkler trucks, sweeping robots, food delivery robots, shopping guide robots, lawnmowers, golf carts, etc.), recreational vehicles (e.g., amusement park vehicles, amusement park autonomous driving devices, balance bikes, etc.), rescue vehicles (e.g., fire trucks, ambulances, power repair vehicles, engineering emergency rescue vehicles, etc.)) and robots (e.g., sweeping robots, food delivery robots, etc.).

[0042] It is understood that the parameter information of the aforementioned mobile tool includes, but is not limited to: the coordinates (x, y) of the mobile tool, its own angle θ, its speed v, its steering wheel angle ω, and the configuration of the sensors (it is understood that the longest planning distance L can be determined based on the distance that the configuration sensors can perceive; for example, if the distance that the configuration sensors can perceive is X, then the longest planning distance L is set to ≤X. It should be noted that the longest planning distance is not higher than the distance that the configuration sensors can perceive). The obstacle information around the mobile tool includes, but is not limited to: the position, shape, and predicted trajectory of the obstacles; in this example, the boundaries of the drivable area, road boundaries, and fences are all considered obstacles. Therefore, the acquired obstacle information also includes: the boundaries of the drivable area, road boundaries, and fences. In some examples, to better plan the path of the mobile tool later, this example also acquires road markings such as lane lines when acquiring parameter information and obstacle information, and further performs path planning based on lane lines and other road markings. This will be explained in detail later and will not be repeated here.

[0043] Continuing with the previous example, when executing this path planning method, the parameter information of the mobile tool and the obstacle information around the mobile tool can be obtained in real time; the parameter information of the mobile tool and the obstacle information around the mobile tool can be obtained periodically; or the parameter information of the mobile tool and the obstacle information around the mobile tool can be obtained only when performing path planning.

[0044] It is understood that a 3D grid map for the mobile tool is constructed based on the parameter information. Specifically, the coordinates of the mobile tool can be obtained based on the parameter information. A 3D grid map is constructed with the current coordinates of the mobile tool as the center. The length and width of the 3D grid map are 2*L / λ, and the height is 2*π / σ, where L is the longest planning distance, λ is the grid resolution, and σ is the angular resolution (the angular resolution is a parameter set internally by the algorithm, and its value affects the accuracy of the final trajectory and the execution speed of the algorithm; a balance needs to be struck between the two). It is understood that in some examples, the 3D grid map can be constructed with the current coordinates of the mobile tool translated as the center point; in some examples, the length, width, and height of the 3D grid map can be flexibly set by relevant personnel. This example does not limit the length and width of the 3D grid map to 2*L / λ and the height to 2*π / σ.

[0045] Following the previous example, after determining the three-dimensional grid map, it is also necessary to determine the starting node and the longest planned distance of the moving tool on the three-dimensional grid map based on the parameter information. The longest planned distance is determined based on the configuration sensor information contained in the parameter information, which will not be elaborated here.

[0046] Specifically, when determining the starting node of the moving tool on the 3D grid map based on the parameter information, the position of the moving tool in the 3D grid map is obtained, and the obtained position is used as the starting node of the moving tool; specifically, taking the current coordinates of the moving tool as the center to construct a 3D grid map as an example, the center point of the 3D grid map is the position of the moving tool in the 3D grid map, and the center point of the 3D grid map is used as the starting node of the moving tool in the 3D grid map;

[0047] After acquiring obstacle information, obstacles are set on a 3D grid map based on this information, ensuring that the obstacles on the 3D grid map correspond to the actual obstacles. Specifically, obstacles that overlap with the 3D grid map (e.g., objects, drivable area boundaries, road boundaries, and fences) are identified based on the obstacle information and added to the 3D grid map, thus ensuring that the obstacles on the 3D grid map correspond to the actual obstacles.

[0048] By acquiring the parameter information and obstacle information of the moving tool, a 3D grid map with obstacles can be quickly and accurately constructed. Based on the starting node, the longest planned distance, and obstacle information, the target node can be determined. The starting node is the starting point of the moving tool in the 3D grid map, and the target node is the ending point of the moving tool in the 3D grid map. Based on the acquired starting and ending points, the path trajectory from the starting point to the ending point can be quickly and accurately calculated, realizing path planning for the moving tool and avoiding the problem of not being able to perform timely and accurate path planning for the moving tool in related technologies.

[0049] In some examples, after obtaining the starting node, the starting node can be added to the relevant user-defined open list to store the starting node and facilitate its subsequent use.

[0050] In some examples of this embodiment, such as Figure 2 As shown, the step S103 above, which determines the target node based on the obstacles set on the three-dimensional grid map, the starting node, and the longest planning distance, can be expanded to:

[0051] S201. Determine multiple steering wheel angles and a fixed driving distance, wherein the fixed driving distance is lower than the longest planned distance;

[0052] S202. Taking the starting node as the starting point, calculate the first target position in the three-dimensional grid map after the mobile tool travels the fixed distance at different steering wheel angles, and take the first target position as the intermediate node, with each steering wheel angle corresponding to one intermediate node;

[0053] S203. Determine the target node based on the plurality of intermediate nodes and the starting node.

[0054] In step S201, the steering wheel angle interval is divided into n angles to obtain n steering wheel angles. For example, if the steering wheel angle interval is divided into 10 angles, 10 steering wheel angle data can be obtained.

[0055] The fixed driving distance is a distance set by relevant personnel according to actual needs. This distance is lower than the longest planned distance. In some examples of this embodiment, the size of the fixed driving distance is comparable to the grid resolution λ. For example, the fixed distance can be directly set to λ.

[0056] In step S202, starting from the initial node, the first target position in the 3D grid map after the mobile tool travels a fixed distance at different steering wheel angles is calculated, and this first target position is used as an intermediate node. Specifically, taking N steering wheel angles as an example, let the first steering wheel angle, the second steering wheel angle, ..., the Nth steering wheel angle be a1, a2, ..., aN, respectively. Starting from the initial node, the first target position reached by the mobile tool after traveling a fixed distance at steering wheel angle a1 in the 3D grid map is calculated, and this position is used as the intermediate node corresponding to a1. Similarly, starting from the initial node, the first target position reached by the mobile tool after traveling a fixed distance at steering wheel angle a2 in the 3D grid map is calculated, and this position is used as the intermediate node corresponding to a2. This process continues until the first target position reached by the mobile tool after traveling a fixed distance at steering wheel angle aN in the 3D grid map is calculated, and this position is used as the intermediate node corresponding to aN. In this way, the intermediate node corresponding to each steering wheel angle can be obtained.

[0057] Similarly, after obtaining multiple intermediate nodes, these intermediate nodes can also be added to the Open list. In some examples, the Open list can store nodes and sort the stored nodes, placing the node with the lowest cost at the first position of the list for easy use later.

[0058] Continuing from the previous example, such as Figure 3 As shown, after obtaining the intermediate nodes of each steering wheel angle, the target node is determined based on the multiple intermediate nodes and the starting node, which can be expanded as follows:

[0059] S301. Determine the cost of each intermediate node and sort the costs of multiple intermediate nodes in order.

[0060] S302. Select the intermediate node with the lowest cost from the plurality of intermediate nodes, and determine the target node based on the intermediate node with the lowest cost and the starting node.

[0061] In some examples, after determining the cost of each intermediate node, the intermediate nodes are added to an Open list. The Open list then sorts the costs of all intermediate nodes in descending or ascending order, thus identifying the intermediate node with the lowest cost. In some examples, such as... Figure 4 As shown, the cost of determining each intermediate node in step S301 above can be expanded as follows:

[0062] S401. Determine the parent node corresponding to each intermediate node, determine the steering wheel angle difference and gear state corresponding to each intermediate node based on the parent node, and determine the cost of each intermediate node based on the steering wheel angle difference and / or the gear state; and / or;

[0063] S402. Determine the steering wheel angle corresponding to each intermediate node, and determine the cost of each intermediate node based on the steering wheel angle.

[0064] The steps to determine the parent node corresponding to each intermediate node are as follows: First, obtain the starting point corresponding to each intermediate node, and then take the node corresponding to the starting point as the parent node. Specifically, if the starting node is the starting point, then set the parent node corresponding to multiple intermediate nodes as the starting node; if a certain intermediate node is taken as the starting point, then after re-determining multiple intermediate nodes, set the parent node corresponding to these re-determined intermediate nodes as the new intermediate node.

[0065] Continuing with the previous example, after determining the parent node corresponding to each intermediate node, the steering wheel angle difference for each intermediate node can be determined based on the steering wheel angles of the parent node and the intermediate node. Similarly, the gear position for each intermediate node can be determined based on the gear position of the parent node and the intermediate node. Specifically, for example, if the steering wheel angle of the parent node is M and the steering wheel angle of the intermediate node is N, then the steering wheel angle difference for that intermediate node is |NM|. Similarly, if the gear position of the parent node is forward (D gear) and the gear position of the intermediate node is reverse (R gear), then the gear position for that intermediate node is in a shifted state. Conversely, if the gear position of the parent node is forward and the gear position of the intermediate node is forward, then the gear position for that intermediate node is in an unshifted state.

[0066] The larger the steering wheel angle difference, the greater the cost of the intermediate node, and the cost increases rapidly in a non-linear manner. Relevant personnel can set the cost corresponding to the steering wheel angle difference, and after obtaining the steering wheel angle difference, they can obtain the corresponding cost based on the steering wheel angle difference.

[0067] Among these, relevant personnel can flexibly set the costs corresponding to the non-shifted state and the shifted state. For example, the cost corresponding to the non-shifted state can be set to 0, and the cost corresponding to the shifted state can be set to E_g (where the cost corresponding to the shifted state should be large, that is, E_g should be set to a value as high as possible to ensure that the planned path does not shift gears as much as possible; it is understandable that the above gears are planned, starting from the current position, moving forward (D gear) or backward (R gear), if the parent node is moving forward, the corresponding intermediate node should also be moving forward).

[0068] When determining the cost of each intermediate node based on the steering wheel angle difference and the gear position, it is also necessary to determine the weights of the costs corresponding to the steering wheel angle difference and the gear position. For example, let the cost corresponding to the steering wheel angle difference be E_Δω and the cost corresponding to the gear position be E_g. Obtain the weights W_Δω and W_g corresponding to the steering wheel angle difference and the gear position respectively, and perform a weighted sum W_Δω*E_Δω+W_g*E_g based on the determined weights to obtain the cost E corresponding to the intermediate node, which is E=W_Δω*E_Δω+W_g*E_g.

[0069] In some examples of this embodiment, the cost of each intermediate node can also be determined by determining the steering wheel angle corresponding to each intermediate node. Specifically, the larger the absolute value of the steering wheel angle, the greater the cost, and the cost increases rapidly in a non-linear manner. The specific relationship between the steering wheel angle and the cost can be flexibly set by relevant personnel.

[0070] When determining the cost of an intermediate node through steps 401 and 402, it is also necessary to determine the weight of the cost corresponding to the steering wheel angle and weight the cost corresponding to the steering wheel angle based on this weight. For example, if the cost corresponding to the steering wheel angle is E_ω, the weight corresponding to the steering wheel angle is determined to be W_ω, and the cost determined in step 401 is W_Δω*E_Δω+W_g*E_g, then the cost corresponding to the intermediate node is finally determined to be E=W_ω*E_ω+W_Δω*E_Δω+W_g*E_g.

[0071] It's understandable that steering wheel angle refers to the angle at which the steering wheel of a vehicle is positioned, i.e., the angle at which the vehicle is turned. Driving angle, on the other hand, refers to the angle at which the vehicle deviates from its straight-line direction of travel, i.e., the vehicle's offset angle. These two are not the same, and driving angle is related to steering wheel angle.

[0072] In some examples of this embodiment, such as Figure 5 As shown, the cost of determining each intermediate node in step S301 above can also be determined by at least one of the following confirmation methods:

[0073] S501. Determine a reference trajectory, obtain the lateral distance between each intermediate node and the reference trajectory, and determine the cost of each intermediate node based on the lateral distance;

[0074] S502. Determine the minimum distance between each intermediate node and an obstacle in the three-dimensional grid map, and determine the cost of each intermediate node based on the minimum distance;

[0075] S503. Determine the distance difference between each intermediate node and the starting node in the three-dimensional grid map, and determine the cost of each intermediate node based on the distance difference.

[0076] The method for determining the reference trajectory in step S501 includes, but is not limited to: determining the lane based on the perception results of the lane lines and road boundaries, and using the center line of the lane as the reference trajectory; wherein, in step S501, the cost of each center node is determined by determining the lateral distance between each intermediate node and the reference trajectory. The greater the lateral distance between the intermediate node and the reference trajectory, the greater the cost, and the relationship between the lateral distance and the cost can be linear (or non-linear in some examples). The specific linear relationship between the lateral distance and the cost can be flexibly set by relevant personnel.

[0077] In step S502, the nearest obstacle corresponding to each intermediate node is determined. After determining the nearest obstacle, the minimum distance between each intermediate node and the corresponding nearest obstacle is determined. The smaller the minimum distance, the greater the corresponding cost, and it increases rapidly in a non-linear manner. For example, the minimum distance between intermediate node A and the nearest obstacle X is M, and the minimum distance between intermediate node B and the nearest obstacle Y is N, and M>N. Then, when calculating the cost based on the minimum distance to the obstacle, the cost of intermediate node A is lower than the cost of intermediate node B.

[0078] In step S503, the distance difference between each intermediate node and the starting node in the three-dimensional grid map is determined, and the cost of each intermediate node is determined based on the distance difference. The smaller the distance difference, the greater the cost. The relationship between the distance difference and the cost can be linear. The specific linear relationship between the distance difference and the cost can be flexibly set by relevant personnel.

[0079] If the cost of the intermediate node is obtained through at least two of steps S501, S502, and S503, it is also necessary to obtain the weight corresponding to the cost calculated in each step. Taking the cost determined by the lateral distance between the intermediate node and the reference trajectory as E_ref, the cost determined by the minimum distance between the intermediate node and the obstacle as E_obs, and the cost determined by the distance difference as E_L as an example, the weights W_ref, W_obs, and W_L corresponding to E_ref, E_obs, and E_L are determined respectively. Then, they are weighted and summed as W_ref*E_ref + W_obs*E_obs + W_L*E_L to obtain the final cost of the intermediate node E = W_ref*E_ref + W_obs*E_obs + W_L*E_L.

[0080] It is understandable that the cost of the intermediate node can also be determined jointly through steps S401, S402, S501, S502, and S503. In this case, the cost of the intermediate node E = W_ref*E_ref + W_obs*E_obs + W_L*E_L +

[0081] W_ω*E_ω+W_Δω*E_Δω+W_g*E_g.

[0082] After determining the cost of each intermediate node, the intermediate nodes are sorted according to their costs, and then the intermediate node with the lowest cost is selected.

[0083] In some examples of this embodiment, such as Figure 6 As shown, step S302 above, which selects the intermediate node with the lowest cost from multiple intermediate nodes and determines the target node based on the intermediate node with the lowest cost and the starting node, can be extended to:

[0084] S601. Calculate the distance from the intermediate node to the starting node that minimizes the cost;

[0085] S602. If the distance from the intermediate node with the lowest cost to the starting node exceeds the longest planning distance, the intermediate node with the lowest cost shall be taken as the target node.

[0086] The calculation of the distance from the intermediate node to the starting node with the minimum cost includes: obtaining the straight-line distance from the intermediate node to the starting node and using this straight-line distance as the distance from the intermediate node to the starting node with the minimum cost; or, calculating the travel distance from the intermediate node to the starting node with the minimum cost and using the calculated travel distance as the distance from the intermediate node to the starting node with the minimum cost.

[0087] In step S602, if the distance from the intermediate node with the lowest cost to the starting node is M, and the longest planning distance determined based on the configured sensor is L, if M is greater than or equal to L, then the intermediate node with the lowest cost is directly used as the target node.

[0088] In some examples, such as Figure 7 As shown, after calculating the distance from the intermediate node with the lowest cost to the starting node, the above step S302, which determines the target node based on the intermediate node with the lowest cost and the starting node, can be extended to:

[0089] S603. If the distance from the intermediate node with the lowest cost to the target node does not exceed the longest planned distance, starting from the intermediate node with the lowest cost, calculate the second target position in the three-dimensional grid map after the mobile tool travels the fixed distance in the three-dimensional grid map at different steering wheel angles, and redetermine multiple intermediate nodes based on the second target position.

[0090] S604. Determine the target node based on the re-determined plurality of intermediate nodes and the starting node.

[0091] In step S603, if the distance from the intermediate node with the lowest cost to the starting node is M, and the longest planning distance determined based on the configured sensor is L, if M is less than L, the intermediate node is re-determined according to the steps in S603.

[0092] Continuing the previous example, let n_cur be the intermediate node with the lowest cost. Using the position corresponding to n_cur as the starting point, calculate the second target position in the 3D grid map after the vehicle travels a fixed distance at different steering wheel angles, and use this second target position as the intermediate node. Specifically, taking an example with N steering wheel angles, let the first steering wheel angle, the second steering wheel angle, ..., the Nth steering wheel angle be a1, a2, ..., aN, respectively. Starting from n_cur, calculate the second target position reached by the vehicle after traveling a fixed distance in the 3D grid map at steering wheel angle a1, and use this position as the intermediate node corresponding to a1. Similarly, starting from the initial node, calculate the second target position reached by the vehicle after traveling a fixed distance in the 3D grid map at steering wheel angle a2, and use this position as the intermediate node corresponding to a2. Continue in this manner until calculating the second target position reached by the vehicle after traveling a fixed distance in the 3D grid map at steering wheel angle aN, and use this position as the intermediate node corresponding to aN.

[0093] After step S603, the intermediate nodes are updated. It is understood that at this time, the starting point of the newly determined intermediate nodes is n_cur, and therefore, the parent node of the newly determined intermediate nodes is n_cur.

[0094] After redetermining the intermediate nodes, the target node is determined based on the redetermined intermediate nodes and the starting node. See step S203 above for details, which will not be repeated here.

[0095] In some examples of this embodiment, such as Figure 8 As shown, after determining the path trajectory of the mobile tool based on the starting node and the target node in step S104 above, the method further includes:

[0096] S701. Smooth the path trajectory and perform speed planning on the moving tool to obtain the output trajectory;

[0097] S702. Control the movement of the mobile tool according to the output trajectory, and determine whether the mobile tool deviates from the output trajectory or whether the travel distance of the mobile tool exceeds a threshold during the movement of the mobile tool.

[0098] S703. If the mobile tool deviates from the output trajectory or the travel distance of the mobile tool exceeds the threshold, the parameter information of the mobile tool and the obstacle information around the mobile tool are reacquired.

[0099] The final output trajectory is obtained by smoothing the path trajectory and performing speed planning. This embodiment does not limit the method of smoothing the path trajectory, and relevant personnel can choose flexibly.

[0100] The output trajectory is sent to the controller, which controls the mobile tool to travel along the output trajectory. During the journey, the controller determines whether the mobile tool deviates from the output trajectory or obtains the distance traveled along the output trajectory and determines whether the distance exceeds a threshold. If the mobile tool deviates from the output trajectory and / or the distance traveled along the output trajectory exceeds the threshold, the controller re-obtains the parameter information of the mobile tool and the obstacle information around the mobile tool to re-plan the path. For example, if the maximum planned distance is 10m, the threshold can be set to 5m. If the mobile tool travels more than 5m after each planning, re-planning is also required. Specifically, re-obtaining the parameter information of the mobile tool and the obstacle information around the mobile tool when the travel distance exceeds the threshold, and thus re-planning the path, avoids re-planning when the maximum planned distance is reached, where obstacles are found to be too close to the mobile tool, making it impossible to plan an obstacle avoidance trajectory. It is understood that, within the limits of system resources, the lower the threshold, the better, to achieve real-time planning as much as possible. This can avoid collisions caused by trajectory deviations due to dynamic changes in obstacles during the journey.

[0101] In some examples of this embodiment, the method for controlling the movement of the mobile tool according to the output trajectory further includes: determining whether the target distance has been completed; if not, re-acquiring the parameter information of the mobile tool and the obstacle information around the mobile tool; if yes, ending the planning process.

[0102] The path planning method provided in this example includes: acquiring parameter information of a mobile tool and obstacle information around the mobile tool; constructing a three-dimensional grid map for the mobile tool based on the parameter information, and determining the starting node and the longest planned distance of the mobile tool on the three-dimensional grid map based on the parameter information; setting obstacles on the three-dimensional grid map based on the obstacle information, and determining a target node based on the obstacles set on the three-dimensional grid map, the starting node, and the longest planned distance; and determining the path trajectory of the mobile tool based on the starting node and the target node. This invention, by acquiring parameter information and obstacle information of the mobile tool, quickly and accurately constructs a three-dimensional grid map with obstacles, and determines the target node based on the starting node, the longest planned distance, and obstacle information. The starting node is the starting point of the mobile tool in the three-dimensional grid map, and the target node is the ending point of the mobile tool in the three-dimensional grid map. Based on the acquired starting and ending points, the path trajectory from the starting point to the ending point can be calculated quickly and accurately, realizing path planning for the mobile tool and avoiding the problem in related technologies where path planning for mobile tools cannot be performed in a timely and accurate manner.

[0103] To better understand the present invention, this embodiment provides a more specific example to illustrate the invention, such as... Figure 9 As shown, this example provides a route planning method. Taking a vehicle as the means of transportation, the method specifically includes the following steps:

[0104] 1. Real-time acquisition of perception information of obstacles around the vehicle, including ① the position, shape, and trajectory prediction of objects, ② the boundary of the drivable area, ③ road markings such as lane lines, ④ road boundaries, fences, etc.;

[0105] 2. Read the vehicle's real-time information, including vehicle coordinates (x, y), vehicle body angle θ, vehicle speed v, steering wheel angle ω, and determine the longest planning distance L for each plan based on the distance that the configured sensors can perceive;

[0106] 3. Confirm the existence of lanes based on the perception results of lane lines and road boundaries. If they exist, set the center line of the lane as the reference trajectory.

[0107] 4. The improved hybrid A* algorithm is used for trajectory planning. The specific steps are as follows:

[0108] 1) Construct a three-dimensional grid map centered on the vehicle's current position. The grid map has a length and width of 2*L / λ and a height of 2*π / σ, where λ is the resolution of the length and width (grid resolution) and σ is the angular resolution.

[0109] 2) Obtain obstacles such as objects that overlap with the raster map, drivable area boundaries, and road boundaries;

[0110] 3) Calculate the starting position on the grid map, set it as the starting node, its parent node is empty, its cost is 0, and add it to the Open list (understandably, the Open list will sort the nodes according to their cost).

[0111] 4) Take the node n_cur with the lowest cost from the Open list, put node n_cur into the Close list, and determine whether the current node's travel distance from the starting point has exceeded the longest planned distance. If yes, skip to step 8); otherwise, continue to step 5.

[0112] 5) Divide the steering wheel angle interval into n angles. Starting from the position of node n_cur, calculate the position of the vehicle in the grid map after traveling a fixed distance at different steering wheel angles. Set the parent node of the newly obtained position node as n_cur and add the newly obtained position node to the Open list.

[0113] 6) Calculate the cost of the newly acquired node. The cost components include: steering wheel angle E_ω (the larger the absolute value of the steering wheel angle, the greater the cost, and it increases rapidly in a non-linear manner), and the difference in steering wheel angle between the node and the parent node E_Δω (the larger the absolute value of the angle difference, the greater the cost, and it increases rapidly in a non-linear manner).

[0114] Is the gear position consistent with the parent node? E_g (where the cost of shifting gears is greater, that is, set E_g to a value as high as possible to ensure that the planned path does not shift gears as much as possible; it is understandable that the above gear position is planned, starting from the current position, moving forward (D gear) or backward (R gear), if the parent node is moving forward, the corresponding intermediate node should also be moving forward).

[0115] The minimum distance to the obstacle, E_obs (the smaller the distance, the greater the cost, and it increases rapidly and non-linearly);

[0116] The lateral distance E_ref from the reference trajectory (if a reference trajectory exists, the greater the distance, the greater the cost; the relationship between the two can be linear).

[0117] Let E_L be the distance traveled from the starting point (the smaller the distance traveled, the greater the cost; the relationship between the two can be linear). Therefore, the total cost function is E = W_ω*E_ω + W_Δω*E_Δω + W_L*E_L + W_g*E_g + W_ref*E_ref, where W_* represents the weight of each factor, such as... Figure 10 As shown, Figure 10 In the diagram, 100 represents the obstacle, 200 represents the perceived road boundary, 301-305 represent different steering wheel angles, each angle corresponds to an intermediate node, and 400 represents the path trajectory obtained under path planning. Among these, 301 and 302 are too close to the obstacle, with 301 being even closer, therefore E301>E302. The steering wheel angle of 304 is much larger than that of 305, therefore E304>E305. Although the steering wheel angle of 303 is larger than that of 305, it is farther away from the obstacle, therefore E305>E303. Finally, the cost relationship of the five nodes is E301>E302>E304>E305>E303.

[0118] 7) Repeat step 4);

[0119] 8) Starting from the position of the current node n_cur, find the parent nodes of the nodes in sequence and obtain the travel trajectory from the starting point to the current node;

[0120] 5. Smooth the trajectory and perform velocity planning to obtain the final planned trajectory;

[0121] 6. Output the trajectory to the controller to control the vehicle to travel along the trajectory;

[0122] 7. Determine whether the vehicle has deviated from the trajectory or has traveled more than the threshold distance from the trajectory start point. If so, reacquire the vehicle's position and re-execute method 3-6 for trajectory planning.

[0123] 8. Determine whether the target distance has been explored. If not, repeat method 7. If yes, end the planning.

[0124] The path planning method provided in this example has the following advantages:

[0125] 1. An improved hybrid A* method is used for planning, and the trajectory conforms to the vehicle kinematics model;

[0126] 2. The cost of intermediate nodes is determined by the distance already traveled from the starting point. The cost determined by the above method can enable vehicles to travel the predetermined distance as quickly as possible, speeding up the planning process and achieving the goal of real-time planning.

[0127] 3. It can adapt to most scenarios; as long as information about surrounding obstacles is provided, it can quickly plan a drivable trajectory.

[0128] Based on the same technical concept, this embodiment also provides a path planning device, such as... Figure 11 As shown, the device includes:

[0129] Acquisition module 1 is used to acquire parameter information of the mobile tool and obstacle information around the mobile tool;

[0130] Construction module 2 is used to construct a three-dimensional grid map for the mobile tool according to the parameter information, and to determine the starting node and the longest planned distance of the mobile tool on the three-dimensional grid map according to the parameter information.

[0131] The determining module 3 is used to set obstacles on the three-dimensional grid map according to the obstacle information, and to determine the target node based on the obstacles set on the three-dimensional grid map, the starting node, and the longest planning distance;

[0132] Planning module 4 is used to determine the path trajectory of the mobile tool based on the starting node and the target node.

[0133] In some examples, determining a target node based on obstacles set on the 3D grid map, the starting node, and the longest planned distance includes: determining multiple steering wheel angles and a fixed driving distance, wherein the fixed driving distance is lower than the longest planned distance; starting from the starting node, calculating the first target position in the 3D grid map after the mobile tool has traveled the fixed driving distance at different steering wheel angles, and using the first target position as an intermediate node, with each steering wheel angle corresponding to one intermediate node; and determining the target node based on the multiple intermediate nodes and the starting node.

[0134] In some examples, determining the target node based on a plurality of intermediate nodes and the starting node includes: determining the cost of each intermediate node and sorting the costs of the plurality of intermediate nodes in order; selecting the intermediate node with the lowest cost from the plurality of intermediate nodes, and determining the target node based on the intermediate node with the lowest cost and the starting node.

[0135] In some examples, determining the target node based on the intermediate node with the lowest cost and the starting node includes: calculating the distance from the intermediate node with the lowest cost to the starting node; if the distance from the intermediate node with the lowest cost to the starting node exceeds the longest planning distance, the intermediate node with the lowest cost is selected as the target node.

[0136] In some examples, determining the target node based on the intermediate node with the lowest cost and the starting node further includes: if the distance from the intermediate node with the lowest cost to the target node does not exceed the longest planned distance, starting from the intermediate node with the lowest cost, calculating the second target position in the three-dimensional grid map after the mobile tool travels the fixed distance at different steering wheel angles on the three-dimensional grid map, and redetermining multiple intermediate nodes based on the second target position; and determining the target node based on the redetermined multiple intermediate nodes and the starting node.

[0137] In some examples, determining the cost of each intermediate node includes: determining the parent node corresponding to each intermediate node, determining the steering wheel angle difference and gear position corresponding to each intermediate node based on the parent node, and determining the cost of each intermediate node based on the steering wheel angle difference and / or the gear position; and / or; determining the steering wheel angle corresponding to each intermediate node, and determining the cost of each intermediate node based on the steering wheel angle.

[0138] In some examples, determining the cost of each intermediate node includes at least one of the following verification methods: determining a reference trajectory, obtaining the lateral distance between each intermediate node and the reference trajectory, and determining the cost of each intermediate node based on the lateral distance; determining the minimum distance between each intermediate node and obstacles in the 3D grid map, and determining the cost of each intermediate node based on the minimum distance; determining the distance difference between each intermediate node and the starting node in the 3D grid map, and determining the cost of each intermediate node based on the distance difference.

[0139] In some examples, after determining the path trajectory of the mobile tool based on the starting node and the target node, the method further includes: smoothing the path trajectory and performing speed planning on the mobile tool to obtain an output trajectory; controlling the mobile tool to travel according to the output trajectory, and during the travel of the mobile tool, determining whether the mobile tool deviates from the output trajectory or whether the travel distance of the mobile tool exceeds a threshold; if the mobile tool deviates from the output trajectory or the travel distance of the mobile tool exceeds the threshold, re-acquiring the parameter information of the mobile tool and the obstacle information around the mobile tool.

[0140] It should be understood that the combination of various modules of the path planning device provided in this embodiment can realize the various steps of the above-mentioned path planning method and achieve the same technical effect as the various steps of the path planning method, which will not be elaborated here.

[0141] Embodiments of this application also provide an electronic device, including one or more processors and a storage device, wherein the storage device is used to store one or more computer programs, which, when executed by one or more processors, cause the electronic device to implement the path planning method described above.

[0142] Figure 12 A schematic diagram of the structure of a computer system suitable for implementing the electronic device of the present application is shown.

[0143] It should be noted that, Figure 12 The computer system 1800 of the electronic device shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of this application.

[0144] like Figure 12As shown, the computer system 1800 includes a central processing unit (CPU) 1801, which can perform various appropriate actions and processes, such as executing the methods described in the above embodiments, based on a program stored in read-only memory (ROM) 1802 or a program loaded from storage portion 1808 into random access memory (RAM) 1803. The RAM 1803 also stores various programs and data required for system operation. The CPU 1801, ROM 1802, and RAM 1803 are interconnected via a bus 1804. An input / output (I / O) interface 1805 is also connected to the bus 1804.

[0145] In some embodiments, the following components are connected to the I / O interface 1805: an input section 1806 including a keyboard, mouse, etc.; an output section 1807 including a cathode ray tube (CRT), liquid crystal display (LCD), etc., and a speaker, etc.; a storage section 1808 including a hard disk, etc.; and a communication section 1809 including a network interface card such as a LAN (Local Area Network) card, modem, etc. The communication section 1809 performs communication processing via a network such as the Internet. A drive 1810 is also connected to the I / O interface 1805 as needed. A removable medium 1811, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., is installed on the drive 1810 as needed so that computer programs read from it can be installed into the storage section 1808 as needed.

[0146] Specifically, according to embodiments of this application, the processes described above with reference to the flowcharts can be implemented as computer programs. For example, embodiments of this application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program including a computer program for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 1809, and / or installed from removable medium 1811. When the computer program is executed by processor (CPU) 1801, it performs various functions defined in the system of this application.

[0147] It should be noted that the computer-readable medium shown in the embodiments of this application can be a computer-readable signal medium or a computer-readable storage medium, or any combination of the two. A computer-readable storage medium can be, for example, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory, flash memory, optical fiber, portable compact disc read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this application, a computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, carrying a computer-readable computer program. Such propagated data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. Computer-readable signal media can also be any computer-readable medium other than computer-readable storage media, which can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The computer program contained on the computer-readable medium can be transmitted using any suitable medium, including but not limited to wireless, wired, etc., or any suitable combination thereof.

[0148] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods, and computer program products according to various embodiments of this application. Each block in a flowchart or block diagram may represent a module, segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram or flowchart, and combinations of blocks in a block diagram or flowchart, may be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and a computer program.

[0149] The units or modules described in the embodiments of this application can be implemented in software or hardware, and can also be located in a processor. The names of these units or modules do not necessarily limit the specific unit or module itself.

[0150] Another aspect of this application provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the path planning method as described above. This computer-readable storage medium may be included in the electronic device described in the above embodiments, or it may exist independently and not incorporated into the electronic device.

[0151] Another aspect of this application provides a computer program product comprising a computer program stored in a computer-readable storage medium. A processor of an electronic device reads the computer program from the computer-readable storage medium and executes the computer program, causing the electronic device to perform the path planning method as described above in the various embodiments.

[0152] It should be noted that although several modules or units for the device used to perform actions have been mentioned in the detailed description above, this division is not mandatory. In fact, according to the embodiments of this application, the features and functions of two or more modules or units described above can be embodied in one module or unit. Conversely, the features and functions of one module or unit described above can be further divided and embodied by multiple modules or units.

[0153] Other embodiments of this application will readily conceive of by considering the specification and practicing the embodiments disclosed herein. This application is intended to cover any variations, uses, or adaptations of this application that follow the general principles of this application and include common knowledge or customary techniques in the art not disclosed herein.

[0154] The above content is merely a preferred exemplary embodiment of this application and is not intended to limit the implementation of this application. Those skilled in the art can easily make corresponding modifications or alterations based on the main concept and spirit of this application. Therefore, the scope of protection of this application should be determined by the scope of protection claimed in the claims.

Claims

1. A path planning method, characterized in that, The path planning method includes: Obtain parameter information of the mobile tool and information about obstacles around the mobile tool; A three-dimensional grid map for the mobile tool is constructed based on the parameter information, and the starting node and the longest planned distance of the mobile tool on the three-dimensional grid map are determined based on the parameter information. Obstacles are set on the 3D grid map based on the obstacle information, and the target node is determined based on the obstacles set on the 3D grid map, the starting node, and the longest planning distance; the 3D grid map has a length and width of 2L / λ and a height of 2π / σ, where L is the longest planning distance, λ is the grid resolution, and σ is the angular resolution; The path trajectory of the mobile tool is determined based on the starting node and the target node; Determining the target node based on the obstacles set on the 3D raster map, the starting node, and the longest planning distance includes: Determine multiple steering wheel angles and a fixed driving distance, wherein the fixed driving distance is lower than the longest planned distance; Starting from the starting node, calculate the first target position in the three-dimensional grid map after the mobile tool travels the fixed distance at different steering wheel angles, and use the first target position as an intermediate node, with each steering wheel angle corresponding to one intermediate node; The target node is determined based on the plurality of intermediate nodes and the starting node; Determining the target node based on the plurality of intermediate nodes and the starting node includes: The intermediate node with the lowest cost is selected from the plurality of intermediate nodes, and the target node is determined based on the intermediate node with the lowest cost and the starting node; Determining the target node based on the intermediate node and the starting node with the lowest cost further includes: If the distance from the intermediate node with the lowest cost to the target node does not exceed the longest planned distance, starting from the intermediate node with the lowest cost, calculate the second target position in the three-dimensional grid map after the mobile tool travels the fixed distance in the three-dimensional grid map at different steering wheel angles, and redetermine multiple intermediate nodes based on the second target position. The target node is determined based on the redefined plurality of intermediate nodes and the starting node.

2. The method according to claim 1, characterized in that, Determining the target node based on the plurality of intermediate nodes and the starting node includes: The cost of each intermediate node is determined, and the costs of multiple intermediate nodes are ordered sequentially.

3. The method according to claim 2, characterized in that, Determining the target node based on the intermediate node and the starting node with the lowest cost includes: Calculate the distance from the intermediate node to the starting node that minimizes the cost; If the distance from the intermediate node with the lowest cost to the starting node exceeds the longest planning distance, the intermediate node with the lowest cost will be selected as the target node.

4. The method according to claim 2, characterized in that, Determining the cost of each of the intermediate nodes includes: Determine the parent node corresponding to each intermediate node, determine the steering wheel angle difference and gear state corresponding to each intermediate node based on the parent node, and determine the cost of each intermediate node based on the steering wheel angle difference and / or the gear state; and / or; Determine the steering wheel angle corresponding to each intermediate node, and determine the cost of each intermediate node based on the steering wheel angle.

5. The method according to claim 2, characterized in that, Determine the cost of each intermediate node, including at least one of the following confirmation methods: Determine a reference trajectory, obtain the lateral distance between each intermediate node and the reference trajectory, and determine the cost of each intermediate node based on the lateral distance; Determine the minimum distance between each intermediate node and an obstacle in the 3D grid map, and determine the cost of each intermediate node based on the minimum distance; Determine the distance difference between each intermediate node and the starting node in the three-dimensional grid map, and determine the cost of each intermediate node based on the distance difference.

6. The method according to claim 1, characterized in that, After determining the path trajectory of the mobile tool based on the starting node and the target node, the method further includes: The path trajectory is smoothed, and the movement tool is speed-planned to obtain the output trajectory; The vehicle is controlled to move according to the output trajectory, and during the movement of the vehicle, it is determined whether the vehicle deviates from the output trajectory or whether the distance traveled by the vehicle exceeds a threshold. If the mobile tool deviates from the output trajectory or the travel distance of the mobile tool exceeds the threshold, the parameter information of the mobile tool and the obstacle information around the mobile tool are reacquired.

7. A path planning device, characterized in that, The path planning device includes: An acquisition module is used to acquire parameter information of the mobile tool and obstacle information around the mobile tool; A construction module is configured to construct a three-dimensional grid map for the mobile tool based on the parameter information, and to determine the starting node and the longest planned distance of the mobile tool on the three-dimensional grid map based on the parameter information. The determination module is used to set obstacles on the three-dimensional grid map according to the obstacle information, and to determine the target node based on the obstacles set on the three-dimensional grid map, the starting node, and the longest planning distance; A planning module, which is used to determine the path trajectory of the mobile tool based on the starting node and the target node; The 3D raster map has a length and width of 2L / λ and a height of 2π / σ, where L is the longest planning distance, λ is the raster resolution, and σ is the angular resolution. Determining the target node based on the obstacles set on the 3D raster map, the starting node, and the longest planning distance includes: Determine multiple steering wheel angles and a fixed driving distance, wherein the fixed driving distance is lower than the longest planned distance; Starting from the starting node, calculate the first target position in the three-dimensional grid map after the mobile tool travels the fixed distance at different steering wheel angles, and use the first target position as an intermediate node, with each steering wheel angle corresponding to one intermediate node; The target node is determined based on the plurality of intermediate nodes and the starting node; Determining the target node based on the plurality of intermediate nodes and the starting node includes: The intermediate node with the lowest cost is selected from the plurality of intermediate nodes, and the target node is determined based on the intermediate node with the lowest cost and the starting node; Determining the target node based on the intermediate node and the starting node with the lowest cost further includes: If the distance from the intermediate node with the lowest cost to the target node does not exceed the longest planned distance, starting from the intermediate node with the lowest cost, calculate the second target position in the three-dimensional grid map after the mobile tool travels the fixed distance in the three-dimensional grid map at different steering wheel angles, and redetermine multiple intermediate nodes based on the second target position. The target node is determined based on the redefined plurality of intermediate nodes and the starting node.

8. An electronic device, characterized in that, include: One or more processors; A storage device for storing one or more programs, which, when executed by the one or more processors, cause the electronic device to perform the method of any one of claims 1 to 6.

9. A computer-readable storage medium, characterized in that, It stores a computer program that, when executed by the processor of the electronic device, causes the electronic device to perform the method of any one of claims 1 to 6.

Images