Path planning method and device, computer device and storage medium

Abstract

The application provides a path planning method and device, computer equipment and a storage medium. The method comprises the following steps: based on at least two preset extension directions and a spiral extension curve, performing multi-round node extension from an initial position point until a terminal position is searched to obtain each extension node; based on a preset cost calculation rule, calculating the node cost of each extension node; comparing the node cost of each extension node in each round of node extension, and determining the extension node with the minimum node cost as a first path point in each round, and forming a driving path based on each first path point. Through the spiral extension curve, the node extension is more reasonable, the curvature change between nodes is reduced, the discontinuity of the curvature is reduced, and the driving path is smoother, which is beneficial to the driving of the vehicle.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, and in particular to a path planning method, apparatus, computer device, and storage medium. Background Technology

[0002] To promote the integration of intelligent technologies with the coal mining industry and enhance the level of intelligence in coal mines, researching unmanned driving systems that can improve the intelligence, safety, and operational efficiency of open-pit mines has become crucial for realizing the digitalization and intelligentization of mines. Path planning, as one of the key technologies in unmanned driving systems for mines, is tasked with planning a safe and drivable path for the vehicle based on the mine environment information received by the environmental perception module and the collected digital map information. Existing academic literature on path planning methods for intelligent vehicles mainly includes graph search methods, sampling methods, interpolation methods, and numerical optimization methods.

[0003] However, existing technologies have path points with discontinuous curvature on the planned driving paths, which is not conducive to vehicle driving. Summary of the Invention

[0004] Therefore, it is necessary to provide a path planning method, apparatus, computer equipment, and storage medium to address the aforementioned technical problems.

[0005] A path planning method, comprising:

[0006] Based on at least two preset expansion directions and a spiral expansion curve, multiple rounds of node expansion are performed from the initial position until the termination position is found, thus obtaining each expanded node.

[0007] Based on the preset cost calculation rules, calculate the node cost of each of the extended nodes;

[0008] The node costs of each node in each round of node expansion are compared, and the node with the lowest node cost is determined as the first path point in each round. A driving path is then formed based on each of the first path points.

[0009] The aforementioned path planning method, apparatus, computer equipment, and storage medium, based on at least two preset expansion directions and a spiral expansion curve, perform multiple rounds of node expansion from an initial position until a termination position is found, obtaining each expanded node. Based on preset cost calculation rules, the node cost of each expanded node is calculated. The node costs of each expanded node in each round of expansion are compared, and the expanded node with the lowest cost is determined as the first path point. A driving path is then formed based on these first path points. This design, by using a spiral expansion curve for node expansion in each round, makes the curvature changes between nodes more reasonable, reduces curvature discontinuities, and thus makes the driving path smoother, which is beneficial for vehicle movement. Attached Figure Description

[0010] Figure 1 This is a flowchart illustrating a path planning method in one embodiment;

[0011] Figure 2 This is a structural block diagram of a path planning device in one embodiment;

[0012] Figure 3 This is an internal structural diagram of a computer device in one embodiment;

[0013] Figure 4 This is another flowchart illustrating the path planning method in one embodiment;

[0014] Figure 5 This is a schematic diagram of node expansion in one embodiment;

[0015] Figure 6 This is a schematic diagram of the boundary of the envelope region in one embodiment;

[0016] Figure 7 This is a schematic diagram of the vector to be synthesized in one embodiment;

[0017] Figure 8 This is a schematic diagram of a feasible region in one embodiment;

[0018] Figure 9 This is a numerical illustration of the heuristic cost on a path in one embodiment;

[0019] Figure 10 This is a schematic diagram of an eight-neighborhood in one embodiment. Detailed Implementation

[0020] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0021] Example 1

[0022] In this embodiment, as Figure 1 As shown, a path planning method is provided, which includes:

[0023] Step 110: Based on at least two preset expansion directions and a spiral expansion curve, perform multiple rounds of node expansion from the initial position point until the termination position is found, and obtain each expansion node.

[0024] In this embodiment, in each round of node expansion, the current node is taken as the expansion parent node, and expansion child nodes are determined by radiating outward from the expansion parent node. Different outward radiating directions correspond to different preset expansion directions. After determining the preset expansion direction, an expansion path is extended outward with a spiral expansion curve in each preset expansion direction, and expansion child nodes are determined on each expansion path.

[0025] Step 120: Calculate the node cost of each of the extended nodes based on the preset cost calculation rules.

[0026] In this embodiment, a preset cost calculation rule is used to calculate the node cost of each extended node. This preset cost calculation rule can be the node cost calculation rule in the A* algorithm. The node cost quantifies the cost of traveling from the extended parent node to the current extended child node. The higher the cost, the greater the risk factor and / or the longer the travel path from the extended parent node to the current extended child node. It should be understood that both extended child nodes and extended parent nodes belong to extended nodes. Extended child nodes and extended parent nodes are used to represent the extension relationship between two extended child nodes in the current round of node extension, where extended child nodes are extended from the extended parent node.

[0027] Step 130: Compare the node costs of each node in each round of node expansion, determine the node with the lowest node cost as the first path point in each round, and form a driving path based on each first path point.

[0028] In this embodiment, multiple paths are formed between the initial and final positions through connections between the extended child nodes. The vehicle can move from the initial position to the final position via different paths. To reduce the node cost incurred by the vehicle during travel, extended child nodes are selected round by round based on node cost. Since extended child nodes extended from different parent nodes may be located at the same position during the extension process, each extended child node corresponds to a different parent node. Starting from the final position, backtracking is performed round by round to determine the parent node, and the parent node with the lowest node cost among the multiple parent nodes corresponding to the extended child node is selected as the first path point.

[0029] In this embodiment, the path planning method, based on at least two preset expansion directions and a spiral expansion curve, performs multiple rounds of node expansion from the initial position until the termination position is found, obtaining each expansion node. Based on preset cost calculation rules, the node cost of each expansion node is calculated. The node costs of each expansion node in each round of expansion are compared, and the expansion node with the lowest cost is determined as the first path point. A driving path is then formed based on these first path points. This design, using a spiral expansion curve for node expansion in each round, makes the curvature changes between nodes more reasonable, reduces abrupt curvature changes, and thus makes the driving path smoother, which is beneficial for vehicle movement.

[0030] It should be understood that, although Figure 1 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 1 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0031] Example 2

[0032] This embodiment provides a path planning method, which includes:

[0033] Step 1: Based on at least two preset expansion directions and a spiral expansion curve, perform multiple rounds of node expansion from the initial position point until the termination position is found, and obtain each expansion node.

[0034] In this embodiment, since the steering angle range of the vehicle is limited and different vehicles correspond to different steering angle ranges, different preset expansion directions are determined within the steering angle range of the vehicle to flexibly adapt to different steering angle ranges. Moreover, at the same expansion node, the area that the vehicle can turn in varies depending on the orientation of the vehicle.

[0035] Furthermore, node expansion is performed based on the spiral expansion curve and the expansion step size. In this embodiment, in order to make the travel distance between adjacent expansion nodes more reasonable, the expansion step size is unified during the node expansion process, that is, the expansion step size between adjacent expansion nodes in each round is equal, so that the travel distance between two adjacent expansion nodes is the same.

[0036] See Figure 5 Furthermore, multiple discrete extension angles are determined from the steering angle range of the vehicle, with preset extension directions on both sides of the extension angles. In this embodiment, to make the distribution of preset extension directions more reasonable, the steering angle range is divided at each extension node to obtain multiple extension angles. Each extension direction radiates outward from the extension node, and each extension angle is distributed sequentially in a counterclockwise / clockwise direction with the extension node as the center. An extension direction is set between adjacent extension angles. By setting the extension angles, the distribution of the preset extension directions can be flexibly changed. The larger the extension angle, the sparser the multiple spiral extension curves derived from the same extension node; the smaller the extension angle, the denser the multiple spiral extension curves derived from the same extension node. Figure 5 Starting from the right side of the vehicle, nine discrete preset expansion directions are determined sequentially in a counterclockwise direction. Each of the nine preset expansion directions corresponds to a turn number, such as turn number 0, turn number 1, ..., turn number 8.

[0037] Furthermore, all expansion angles are equal. In this embodiment, the equal expansion angles ensure that the preset expansion directions are evenly distributed, which helps to evenly distribute the expansion nodes on the map and facilitates the determination of a more reasonable driving path.

[0038] In one embodiment, in each round of node expansion, an expanded child node is expanded from the expanded parent node based on a preset node expansion formula; wherein, the preset node expansion formula is shown in formula (1).

[0039]

[0040] Equation (1), To expand the x-coordinate of the node; To expand the ordinate of the node; To extend the vehicle's yaw angle at the node; The curvature of the extended node is denoted by n; n-1 is the index of the extended child node; n-1 is the index of the extended parent node. To extend the rate of curvature change between the parent node and the extended child node, L represents the expansion step size; d This indicates the direction of movement of the extended node, taking a value of 1 or -1. Movement directions include forward and backward. When the movement direction is forward... d The value is 1; when the direction of movement is backward, d The value is -1; l Let L be the integral variable, and let L be the total length L. l This represents the path length. In this embodiment, the coordinates of the extended child nodes are... and By expanding the coordinates of the parent node, that is... and The calculated rate of change of curvature is the preset extended curvature. It is calculated in advance. The integral operation of equation (1) can be obtained by Fresnel function. The curvature of the cyclotron extension curve changes linearly with the arc length. The rate of change of the curvature of the cyclotron extension curve is determined by the rate of change of curvature. Control is achieved by setting different curvature change rates for different extended sub-nodes. This results in the extended child nodes being distributed in different locations.

[0041] In one embodiment, the step of performing multiple rounds of node expansion from an initial position point until a termination position is found, based on at least two preset expansion directions and a spiral expansion curve, to obtain each expanded node, includes:

[0042] Based on at least two preset expansion directions and a spiral expansion curve, multiple rounds of node expansion are performed from the initial position until the termination position is found, resulting in nodes to be filtered.

[0043] The node curvature of the nodes to be screened is obtained, and based on a preset curvature threshold, the nodes to be screened are selected to obtain each extended node. In this embodiment, during node expansion, multiple extended child nodes are expanded from an extended parent node based on multiple preset expansion curvatures. The curvature of the spiral expansion curve between different extended child nodes and the extended parent node is different, i.e., the preset expansion curvature is different. That is, the preset expansion curvatures corresponding to different spiral expansion curves of the same extended parent node are different. Therefore, the node curvatures of different extended child nodes are different, and the rate of change of curvature between the extended parent node and the extended child nodes is also different. Different. To ensure the safe operation of the vehicle, the rate of change of curvature... Greater than the preset filtering curve threshold Remove the extended child nodes and retain the rate of curvature change. Less than the preset screening curve threshold The extended child nodes effectively limit the rate of curvature change between the extended parent node and the extended child nodes, ensuring the linear continuity of curvature.

[0044] It's important to understand that "expanding parent node" and "expanding child node" define the expansion relationship between two expanding nodes in a single round of node expansion, with the expanding parent node expanding the expanding child node. The expanding child node from the previous round will become the expanding parent node for the next round of node expansion.

[0045] Step 2: Calculate the node cost of each of the extended nodes based on the preset cost calculation rules.

[0046] In this embodiment, the preset cost calculation rule can use the vehicle movement direction on the current expansion node, the change in the running direction between the current expansion node and its parent node, the vehicle yaw angle, the movement distance between the current expansion node and its parent node, and the distance from the road boundary as reference parameters to calculate the node cost.

[0047] In one embodiment, the step of calculating the node cost of each of the extended nodes based on a preset cost calculation rule includes:

[0048] Obtain the current extended node;

[0049] Calculate the actual cost between the initial position and the current expansion node, and calculate the estimated cost between the termination position and the current expansion node;

[0050] The actual cost and the estimated cost are summed to obtain the node cost of the current expansion node. In this embodiment, the node cost of each expansion node includes the accumulated actual cost from the initial position to the expansion node and the accumulated estimated cost from the termination position to the expansion node. The actual cost can be calculated by calculating the actual cost of at least one of the following for each expansion node before the accumulation node: expansion step size, movement direction, and change of movement direction. This measures the cost required for the vehicle to move from the initial position to the expansion node. The larger the actual cost, the greater the distance traveled and / or the driving risk of the vehicle moving from the initial position to the current expansion node. The estimated cost can be the cost of the distance traveled by the vehicle from the termination position to the current expansion node. The larger the estimated cost, the greater the distance traveled by the vehicle from the current expansion node to the termination position. For each expansion node, the node cost is calculated round by round from the termination position, and the expansion node with the smallest node cost is determined as the first path point.

[0051] Furthermore, the calculation formula for node cost is shown in equation (2):

[0052]

[0053] In equation (2), For node cost, For the actual cost, To estimate the cost, n is the index of the expanded node. In this embodiment, the actual cost... The estimated cost is the actual cost accumulated from the initial position to the nth expansion node. This is the estimated cost accumulated from the termination position to the nth extended node. Since different extended parent nodes may extend to the same extended child node, when an extended child node (with the index n+1) is determined as the first path point, it is necessary to calculate the node cost of each extended parent node (with the index n) of that extended child node. Each extended parent node corresponds to a node cost. Determine the cost of each node The extended parent node corresponding to the minimum value in the path is the first path point.

[0054] In one embodiment, the calculation rule for the actual cost of the currently extended node includes:

[0055] Take the parent node of the current extended node as the current parent node, and obtain the actual cost of the current parent node as the actual cost of the parent node;

[0056] The actual transfer cost between the current parent node and the extended node is calculated based on a preset real-price calculation model.

[0057] The actual cost of the parent point and the actual transfer cost are summed to obtain the actual cost of the current extended node. In this embodiment, during node expansion in step one, the current extended node is expanded from the current parent point. When the vehicle travels, it first travels to the current parent point and then to the current extended node. The actual cost of the parent point is the actual cost accumulated from the initial position to the current parent point. A preset actual cost calculation model is used to calculate the actual transfer cost required for the vehicle to move from the extended parent node to the extended child node. The actual transfer cost is used to measure the difference in the actual transfer cost between two extended nodes. Moving from the current parent point to the current extended node requires paying the corresponding actual transfer cost to reach the current extended node.

[0058] In one embodiment, the preset real-price calculation model includes: calculating the transfer price between the current parent point and the extended node according to each of the preset real-price transfer rules; summing the transfer prices to obtain the actual transfer cost; wherein the preset real-price transfer rules include at least one of the following: angle difference cost rule, reversing cost rule, node distance cost rule, gear shifting cost rule, preset side extension cost rule, and preset side collision cost rule. In this embodiment, in order to more comprehensively evaluate the transfer price between the current parent point and the current extended node, different preset real-price transfer rules are used to achieve multi-faceted measurement of the transfer price, making the node cost of the extended node more accurate, thereby facilitating the selection of a reasonable extended node as the first path point.

[0059] Furthermore, the actual cost of current expansion nodes The calculation formula is shown in equation (3):

[0060]

[0061] Where g(n-1) is the actual cost of the parent node, which is the actual cost of the extended parent node n-1 of the current extended node n. The transfer price is calculated for the angle difference cost rule. The transfer fee calculated for the reversing cost rule. The transfer price is calculated for the node distance cost rule. The transfer price calculated for the shift cost rule. The sum of the two transfer costs calculated based on the preset side expansion cost rule and the preset side collision cost rule.

[0062] In this embodiment, equation (3) is the formula for calculating the actual cost of the current expansion node in each iteration. (Transfer pricing) , , , and At least one transfer price can be 0, meaning that the transfer price is not calculated using the corresponding type's preset real price transfer rules. This does not mean that the actual transfer cost must include all the transfer prices calculated using the aforementioned preset real price transfer rules. Similarly, the sum of transfer prices... It can also be one of the transfer prices calculated by the preset side expansion cost rule or the preset side collision cost rule. The transfer price corresponding to the preset side expansion cost rule is 0, or the transfer price corresponding to the preset side collision cost rule is 0. The sum of these transfer prices is... It can represent one type of transfer price.

[0063] In one embodiment, the angle difference cost rule includes: obtaining the current vehicle body sway angle of the current extended node and the parent point vehicle body sway angle of the current parent point; and calculating the angle difference cost between the current parent point and the extended node as the transfer price based on a preset sway angle cost formula. In this embodiment, the current vehicle body sway angle is the vehicle body sway angle of the vehicle at the current extended node, and the parent point vehicle body sway angle is the vehicle body sway angle of the vehicle at the current parent point. To ensure smooth driving of the vehicle, a penalty is imposed on cases where the change in vehicle body sway angle between the current vehicle body sway angle and the parent point vehicle body sway angle is large, with a larger corresponding angle difference cost, thereby increasing the node cost of the extended node and reducing the probability of selecting the extended node.

[0064] Furthermore, the preset swing angle cost formula is shown in equation (4):

[0065]

[0066] In equation (4), For absolute value operations, For the modulo operation, The cost of the angle difference for the current expansion node n, (i) The yaw angle of the vehicle body at the i-th extended node, Let n-1 be the yaw angle of the vehicle body. Let n be the yaw angle of the current extended node n. In this embodiment, equation (4) can be used to penalize the current extended node n when the yaw angle, i.e., the direction of the vehicle's front end, changes abruptly from the current parent point n-1 to the current extended node n. The larger the change in the yaw angle, the greater the angle difference penalty. The larger the value, the lower the probability that the current expansion node n will be selected as the first path point.

[0067] In one embodiment, the reversing cost rule includes: obtaining the current movement direction of the extended node as the current movement direction, wherein the movement direction includes a forward direction and a backward direction; when the current movement direction is a forward direction, determining a preset first reversing cost as the transfer price of the current extended node; when the current movement direction is the backward direction, determining a preset second reversing cost as the transfer price of the current extended node, wherein the preset second reversing cost is greater than the preset first reversing cost. In this embodiment, compared to moving in the forward direction, the vehicle moving in the backward direction is more likely to collide, posing a greater safety risk. Therefore, the vehicle's behavior of moving in the forward direction at the current extended node is rewarded by setting a preset first reversing cost as the transfer price, and the vehicle's behavior of moving in the backward direction at the current extended node is penalized by setting a preset second reversing cost as the transfer price. The preset second reversing cost is greater than the preset first reversing cost, thus biasing the selection of extended nodes moving in the forward direction when selecting the first path point.

[0068] Furthermore, the transfer price calculated by the reversing cost rule is shown in equation (5):

[0069]

[0070] In equation (5), The transition price of the current expansion node n is calculated for the reversing cost rule, where a is a constant and a > 0. This indicates that the current direction of movement of the current extension node n is the forward direction. This indicates that the current direction of movement of the current extension node n is backward. In this embodiment, equation (5) is used to penalize backward paths because the risk of vehicles retreating is greater in the mining area scenario. Therefore, it is necessary to penalize the current extension node n whose current direction of movement is backward. The transfer price of the current extension node is set to a, which is greater than the transfer price of the current extension node n whose current direction of movement is forward, thereby reducing the probability of selecting the current extension node n that has retreated as the first path point.

[0071] In one embodiment, the node distance cost rule includes: obtaining the expansion step size between the current expanding node and the current parent point as the transfer cost. In this embodiment, in order to measure the distance required for the vehicle to travel from the current parent point to the current expanding node, the larger the expansion step size, the larger the required distance, and the larger the transfer cost. Therefore, the current expanding node is penalized, increasing its node cost and reducing the probability of selecting it as the first path point.

[0072] Furthermore, the expansion step size is a fixed constant. In this embodiment, multiple rounds of node expansion are typically performed according to the expansion step size during the node expansion step. The distance between the expanded nodes is the same, and the transfer cost required for the vehicle to move between two expanded nodes is the same. Therefore, the transfer cost corresponding to the node distance cost rule is set to a fixed constant.

[0073] In one embodiment, the shifting cost rule includes: obtaining the current movement direction of the current extended node, obtaining the movement direction of the current parent point as the parent point movement direction; when the current movement direction and the parent point movement direction are different, determining a preset shifting cost as the transfer cost, wherein the preset shifting cost is greater than zero. In this embodiment, since shifting gears during driving can easily lead to safety accidents, the current movement direction of the extended node is obtained, and the current movement direction of the parent point is obtained. The current movement direction and the parent point movement direction are compared. If the current movement direction and the parent point movement direction are different, for example, the parent point movement direction is a forward direction and the current movement direction is a backward direction, it indicates that the vehicle has shifted gears. When moving from the current parent point to the current extended node, a shifting behavior occurs, and the current extended node is penalized by setting a preset shifting cost greater than zero to increase the node cost of the current extended node and reduce the probability of selecting the current extended node as the first path point.

[0074] Furthermore, when the current direction of movement is the same as the direction of movement of the parent point, a preset shift cost is determined as the transfer price, wherein the preset shift cost is less than the preset shift cost. In this embodiment, in order to reward the behavior of not shifting gears during the movement from the current parent point to the current extension node, when the current direction of movement is the same as the direction of movement of the parent point, the current extension node is rewarded, and the preset shift cost is set as the transfer price of the current extension node.

[0075] Furthermore, the transfer price calculated by the shift cost rule is shown in equation (6):

[0076]

[0077] In equation (6), The transition cost is calculated for the current expansion node n according to the shift cost rule. b is a constant and a positive number. d(n) represents the movement direction of the nth expansion node, with a value of 1 or -1. A value of 1 indicates that the nth expansion node moves in a forward direction, and a value of -1 indicates that the nth expansion node moves in a backward direction. This indicates that the current direction of movement of the current extension node n is the forward direction. This indicates that the current direction of movement of the current expanding node n is backward. Indicates the current parent point The direction of movement of the parent point is the forward direction. Indicates the current parent point The parent point moves in a backward direction. In this embodiment, if the current expanding node and the current parent point move in the same direction, and The same, preset gear cost The value is 0; if the current expanding node and the current parent node have different movement directions, and Different, preset shift cost Let 'b' represent the magnitude of the penalty incurred when switching between forward and backward. It's important to understand that the transfer price calculated by the shift cost rule is a preset shift cost or a pre-defined shift cost, determined based on the movement direction of the current extension node and the current parent node.

[0078] In one embodiment, the sum of the two transfer costs calculated by the preset side extension cost rule and the preset side collision cost rule is... The calculation formula is shown in equation (7):

[0079]

[0080] In equation (7), The cost for left and right movement is the sum of the two transfer costs calculated by the preset side expansion cost rule and the preset side collision cost rule. The transfer price of the current expansion node n is calculated based on the preset side expansion cost rules; The transition cost of the current extended node n is calculated based on the preset side collision cost rules. In this embodiment, the left-movement cost... It measures the cost of a collision when the current extended node n travels along the left or right road boundary and the degree to which the travel path is close to the left or right road boundary.

[0081] In one embodiment, the preset lateral expansion cost rule includes: obtaining the current turning direction of the current expansion node; when the current turning direction is far from the preset lateral direction, determining a preset first turning cost as the transfer price of the current expansion node; when the preset expansion direction is close to the preset lateral direction, determining a preset second turning cost as the transfer price of the current expansion node, wherein the preset second turning cost is less than the preset first turning cost. In this embodiment, the preset lateral expansion cost rule is used to reward the vehicle for traveling along a preset road boundary. The preset lateral direction is the left or right side of the vehicle. When the vehicle travels in the forward direction on the road, the road boundary closer to the left side of the vehicle is the left road boundary, and the road boundary closer to the right side of the vehicle is the right road boundary. The preset road boundary can be either the left road boundary or the right road boundary. The preset lateral direction is determined according to the preset road boundary that needs to be approached during driving. For example, when it is necessary to travel along the left road boundary on the left side of the vehicle, the preset lateral direction is set to the left side of the vehicle. The preset side expansion cost rule is used to reward expansion child nodes that move closer to the preset side direction in the same round of node expansion, and to penalize expansion child nodes that move away from the preset side direction in the same round of node expansion. When evaluating node costs, a preset second turning cost is set for expansion child nodes that move closer to the preset side direction, and a preset first turning cost is set for expansion child nodes that move away from the preset side direction. The preset second turning cost is less than the preset first turning cost, making it more likely to select expansion nodes that move closer to the preset side direction as the first path point.

[0082] Furthermore, the transfer price calculated by the preset side expansion cost rule is shown in equation (8-1):

[0083]

[0084] In equation (8-1), This represents the turning point number of the current expansion node n in the preset expansion direction when the current parent point expands in different preset expansion directions, v0, ..., v kHere, k is the numerical value for the turning point number, k is an integer greater than or equal to 0, and c is a constant and a positive number. In this embodiment, the turning point numbers are v0, ..., v k The corresponding preset expansion direction is away from the preset side direction, that is, v0, ..., v k The value of the turning point number in the preset expansion direction away from the preset side direction. If the turning point number is taken as v0, ..., v k The current expansion node in the preset expansion direction will cause the vehicle to move away from the road boundary on the preset lateral direction, requiring the turning direction numbered v0, ..., v k The current expansion node in the preset expansion direction is penalized, reducing the probability of selecting an expansion node far from the preset side direction as the first path point. See also Figure 5 The left side of the preset lateral direction driving vehicle is defined as the preset road boundary, and penalties are applied to extension nodes extending to the right side of the driving vehicle. Penalties are applied if the current extension node is far from the preset lateral direction. For example, if a right turn is made towards preset extension directions numbered 0, 1, or 2, and the current extension node is turned towards preset extension directions numbered 0, 1, or 2, then a penalty is applied to the current extension node, causing the first path point to quickly return to the left road boundary. It is important to understand that the number of preset extension directions to be penalized is determined by k in equation (8-1), and k can be determined according to actual needs.

[0085] In one embodiment, the preset side collision cost rule includes: obtaining the envelope region of the current extended node, wherein the boundary of the envelope region is outside the vehicle region; when the boundary of the envelope region contacts the preset side boundary of the road, determining a preset reward / penalty threshold as the transfer price of the current extended node. In this embodiment, the vehicle region is the object of the vehicle on the map. Each extended node has a vehicle region and an envelope region. The relative position between the vehicle region and the envelope region is determined, and on the side closer to the preset side boundary, the boundary of the envelope region is closer to the preset side boundary relative to the boundary of the vehicle region. The preset side boundary of the road is the curve of the road boundary displayed on the map. The preset side boundary can be the curve of the left road boundary and / or the right road boundary displayed on the map. There can be one preset side boundary or two preset side boundaries. On one hand, when the boundary of the envelope region contacts the preset side boundary, indicating that the vehicle is close to the road boundary, and the preset side boundary enters the envelope region, it means that the vehicle is approaching the road boundary. When the boundary of the envelope region contacts the preset side boundary on multiple consecutive expansion nodes, it means that the vehicle is traveling along the road boundary corresponding to the preset side boundary on the path of multiple consecutive expansion nodes. A smaller preset reward / penalty threshold can be set to reward the current expansion node. On the other hand, when the preset side boundary extends excessively into the envelope region, it means that the vehicle is too close to the road boundary corresponding to the preset side boundary, which is prone to collision. A larger preset reward / penalty threshold can be set to penalize the current expansion node. It should be understood that the preset reward / penalty threshold can be set to different values ​​under different conditions, depending on the degree to which the preset side boundary extends into the envelope region. When the degree to which the preset side boundary extends into the envelope region is small, a smaller preset reward / penalty threshold is set for reward; when the degree to which the preset side boundary extends into the envelope region is large, a larger preset reward / penalty threshold is set for penalty.

[0086] In one embodiment, the envelope region includes an inner envelope and an outer envelope, the boundary of the inner envelope and the boundary of the outer envelope are respectively outside the vehicle region, and the boundary of the outer envelope is outside the boundary of the inner envelope;

[0087] The calculation rules for the preset side collision cost include: obtaining the inner and outer envelopes of the current extended node; when the boundary of the inner envelope is in contact with the preset side boundary, determining a preset first reward / penalty threshold as the preset reward / penalty threshold; when the boundary of the inner envelope is isolated from the preset side boundary and the boundary of the outer envelope is in contact with the preset side boundary, determining a preset second reward / penalty threshold as the preset reward / penalty threshold, wherein the preset second reward / penalty threshold is less than the preset first reward / penalty threshold. In this embodiment, in order to more accurately evaluate the degree to which the preset side boundary penetrates into the envelope region, two regions, the inner and outer envelopes, are set as envelope regions respectively. On the side closer to the preset side boundary, the boundary of the outer envelope is closer to the preset side boundary than the boundary of the inner envelope. The boundary of the outer envelope can be outside the boundary of the inner envelope in one directional region, or it can be outside the boundary of the inner envelope in at least two directional regions. The boundaries of the outer and inner envelopes can share a common boundary in at least one directional region, meaning that the boundaries of the outer and inner envelopes can coincide in at least one directional region. When the boundary of the outer envelope is outside the boundary of the inner envelope in multiple directional regions, it is possible to detect whether a vehicle is approaching the road boundary from different directional regions, making the driving process safer.

[0088] In this embodiment, the vehicle region is within the inner envelope. When the preset side boundary comes into contact with the inner envelope, it indicates that the vehicle is approaching the road boundary, and a preset first reward / penalty threshold is set to penalize the current expansion node. When the boundary of the inner envelope is isolated from the preset side boundary and the boundary of the outer envelope comes into contact with the preset side boundary, it indicates that, on the one hand, the distance of the vehicle from the preset side boundary is greater than the distance of the vehicle from the preset side boundary when the preset side boundary comes into contact with the inner envelope. At this time, it is relatively safe, and the probability of the vehicle colliding is small. On the other hand, it indicates that the current expansion node is conducive to the vehicle traveling along the preset side boundary, and the driving process is more adaptable to changes in the road boundary. At this time, a preset second reward / penalty threshold is set to reward the current expansion node.

[0089] Furthermore, the calculation formula for the preset reward and punishment threshold is shown in (9):

[0090]

[0091] In equation (9), This indicates the preset reward / penalty threshold for the current expansion node. This indicates that the boundary of the inner envelope is in contact with the preset side boundary. This indicates that the boundary of the outer envelope is in contact with the preset side boundary. This indicates that the boundary of the inner envelope and the preset side boundary are not in contact, and d is a penalty constant. In this embodiment, the penalty constant d is greater than 0. The condition is satisfied when the boundary of the inner envelope corresponding to the current extended node is in contact with the preset side boundary. When a vehicle approaches the road boundary too closely, a collision is likely to occur. A penalty is imposed on the current extended node, with a relatively large preset first reward / penalty threshold d set as the preset reward / penalty threshold. A penalty is imposed if the boundary of the inner envelope corresponding to the current extended node does not touch the preset side boundary. Furthermore, the boundary of the outer envelope is in contact with the preset side boundary, which also satisfies the condition that... When a vehicle travels close to the road boundary and is unlikely to collide, a reward is given for the current expansion node, with a small preset second reward / penalty threshold set as -d. Rewards are applied. For other envelope regions that do not contact the preset side boundaries, such as when neither the outer nor inner envelope boundaries contact the preset side boundaries, neither penalty nor reward is applied; the preset reward / penalty threshold is set to 0. .

[0092] See Figure 6 The larger envelope e2 is the outer envelope, and the smaller envelope e1 is the inner envelope. At this point, the larger envelope e2 is in contact with the preset side boundary, while the smaller envelope e1 is not in contact with the preset side boundary, satisfying the condition... and The current preset reward and penalty threshold for expansion nodes A second reward / penalty threshold -d is preset to reward the current expansion node.

[0093] In one embodiment, an inner envelope and an outer envelope are established with the vehicle region as the base point. The positions of the inner and outer envelopes relative to the vehicle region are fixed. Both the inner and outer envelopes are rectangles, and the aspect ratios of the inner and outer envelopes are equal to the aspect ratios of the vehicle region. In this embodiment, fixing the positions of the inner and outer envelopes relative to the vehicle region ensures that the inner and outer envelopes accurately and uniformly represent the position of the vehicle region at different expansion nodes, thus unifying the collision detection standards across different expansion nodes. Furthermore, the equal aspect ratios of the inner and outer envelopes and the vehicle region facilitate the inner envelope covering the vehicle region and the outer envelope covering the inner envelope.

[0094] In one embodiment, when the preset side boundary corresponds to the left road boundary, the inner envelope, outer envelope, and the side of the vehicle area closest to the right road boundary are aligned; and / or, when the preset side boundary corresponds to the right road boundary, the inner envelope, outer envelope, and the side of the vehicle area closest to the left road boundary are aligned. In this embodiment, in order to enable the vehicle to travel guided by the left or right road boundary, the first path point is set as close as possible to the left or right road boundary, and the path during the driving process is similar to the left road boundary, or the path during the driving process is similar to the right road boundary.

[0095] In one embodiment, the calculation rule for the estimated cost of the current expansion node includes:

[0096] Take the child node of the current extended node as the current child node, and obtain the estimated cost of the current child node as the estimated cost of the child node.

[0097] Using the current sub-point as the starting point for neighborhood expansion, multiple rounds of eight-neighborhood expansion are performed until the current expansion node is found. The estimated transfer cost corresponding to each round of eight-neighborhood expansion is determined. The estimated transfer costs determined in each round of eight-neighborhood expansion are sequentially added to the estimated cost of the sub-point to obtain the estimated cost of the current expansion node. In this embodiment, in the node expansion in step one, the current sub-point is obtained by expanding from the current expansion node. When the vehicle travels, it first travels to the current expansion node and then to the current sub-point. In the grid map, there are multiple grids between the current expansion node and the current sub-point. From the current sub-point, different paths can be followed to reach the current expansion node. Different search paths traverse different grids. The more grids traversed, the higher the estimated transfer cost from the current sub-point to the current expansion node may be. In each round of eight-neighborhood expansion, the next grid is searched from the current grid. The next grid can be an adjacent grid sharing a side edge with the current grid, or a diagonal grid sharing a vertex with the current grid. See also... Figure 10 The current grid can be grid F. Grid F can be searched for as the next grid by either grid S2 in its neighboring grids or grid S1 in its diagonal grids. To determine the next grid, the estimated transfer cost of moving from the current grid to the next grid is calculated. The estimated transfer cost can be the distance between the current grid and the next grid. Since grids are usually square, the estimated transfer cost of searching from the current grid to a neighboring grid is less than the estimated transfer cost of searching from the current grid to a neighboring grid (diagonal grid).

[0098] In this embodiment, the grid determined in the previous round of eight-neighbor expansion is used as the starting point for the next round of eight-neighbor expansion. The search continues, and the estimated transfer costs between the eight grids around the starting point and the starting point are calculated. The estimated transfer costs are added to the estimated costs of the starting point to obtain the estimated cost of the corresponding grid. Each of the eight grids around the starting point corresponds to an estimated cost. The grid with the smallest estimated cost is determined as the grid for the next round.

[0099] In one embodiment, during the current round of eight-neighbor expansion, the endpoint of the previous round's eight-neighbor expansion is used as the starting point for the current round's expansion. The eight neighboring grids surrounding the starting point are searched, and the grid with the lowest estimated cost is selected as the endpoint. The estimated cost of the neighboring grid is the sum of the estimated cost of the starting point and the estimated transfer cost of the neighboring grid. The estimated transfer cost is determined based on the grid distance between the starting and endpoints. In this embodiment, to find a better path from the current sub-point to the current expansion node, in each round of eight-neighbor expansion, the grid with the lowest estimated cost from the eight neighboring grids surrounding the starting point is selected as the endpoint. This minimizes the estimated transfer cost from the current sub-point to the current expansion node, accurately measuring the estimated cost of the current expansion node.

[0100] In one embodiment, in each round of eight-neighbor expansion, an expansion direction vector is established with the starting point of the current round's neighborhood expansion as the vector starting point and one neighboring grid of the starting point as the vector ending point.

[0101] Calculate the detection left distance between the endpoint of the extended direction vector and the nearest point on the left road boundary;

[0102] When the left distance to be detected is within the range of the left safe distance, the left cross product is calculated based on the preset left cross product formula, according to the nearest point of the neighborhood expansion starting point of the current round and the preset side boundary.

[0103] When the left cross product is less than 0, the estimated transfer cost is the bonus left grid distance cost; when the left cross product is greater than or equal to 0, the estimated transfer cost is the original grid distance cost, which is the Euclidean distance between the starting point and ending point of the current round's neighborhood expansion. The bonus left grid distance cost is less than the original grid distance cost. In this embodiment, to ensure that the vehicle travels along the left road boundary on the left side of the vehicle as much as possible, in each round of eight-neighborhood expansion, an expansion direction vector is established based on the starting point and neighboring grids. The left cross product is calculated using a preset left cross product formula. The left cross product is used to determine the relative positional relationship between the nearest point of the left road boundary and the expansion direction vector. When the left cross product is less than 0, it means that the nearest point of the left road boundary is on the right side of the expansion direction vector and the nearest point of the left road boundary is on the left side of the vehicle. A bonus left grid distance cost, less than the original grid distance cost, is set as the estimated transfer cost for the neighboring grids corresponding to this expansion direction vector. When the left cross product is greater than or equal to 0, it means the nearest point on the left road boundary is to the left of the expansion direction vector, and the nearest point on the right of the vehicle. Therefore, a penalty needs to be applied to the neighboring grid cells corresponding to this expansion direction vector, setting the original grid distance cost, which is greater than the penalty for the left grid distance, as the estimated transfer cost. It's important to understand that each expansion direction vector is determined round-by-round from the current child point to the current expansion node. The expansion direction vector and the vehicle's movement direction are opposite; the right side of the expansion direction vector corresponds to the left side of the vehicle. Calculate the child node and the nearest map boundary point. The distance to be detected is calculated based on the sum of the vehicle's half-width and the safety margin. If the distance is within a certain range, a left forklift is considered, because the vehicle needs to be planned along the boundary. For other points, the forklift cost is not calculated; instead, the Euclidean distance cost is used. It's important to understand that the detected left distance is the range between the center of the vehicle and the left road boundary. The left safety distance is greater than 0.5 times the width of the vehicle to avoid collisions with the road boundary. When the detected left distance is within the left safety distance range, it indicates that the vehicle is close to the left road boundary, and path points close to the left road boundary are rewarded.

[0104] Furthermore, the left safety distance range includes the sum of half the vehicle width and the left safety margin, where the left safety margin is the distance between the left side of the vehicle and the left road boundary. In this embodiment, the left safety distance range consists of two parts: one is half the vehicle width, which is 0.5 times the vehicle width, representing the distance between the center of the vehicle and its left side; the vehicle width is the distance between the left and right sides of the vehicle. The left safety margin is used to control the distance between the left side of the vehicle and the left road boundary. A left safety margin greater than 0 indicates a larger left safety margin, and a smaller left safety margin indicates that the left side of the vehicle is closer to the left road boundary during driving.

[0105] Furthermore, the left cross product expression is predefined as shown in equation (10):

[0106]

[0107] In equation (10), cross 左 To determine the left cross product, The location of the starting point for the neighborhood expansion (x) c y c and tangent direction , This is the location of the nearest point on the left road boundary.

[0108] In one embodiment, when the left cross product is less than 0, and the grid where the neighborhood expansion endpoint is located is the diagonal grid of the grid where the neighborhood expansion start point is located, the estimated transfer cost is the left diagonal distance cost; and / or,

[0109] When the left cross product is less than 0, and the grid cell containing the endpoint of the neighborhood expansion is an adjacent grid cell of the grid cell containing the starting point of the neighborhood expansion, the estimated transfer cost is the left adjacent edge cost; and / or,

[0110] When the left cross product is greater than or equal to 0, and the grid cell containing the endpoint of the neighborhood expansion is the diagonal grid cell containing the starting grid cell, the estimated transition cost is the original diagonal distance cost; where the original diagonal distance cost is the Euclidean distance between two diagonally opposite grid cells, and the left diagonal distance cost is less than the original diagonal distance cost; and / or,

[0111] When the left cross product is greater than or equal to 0, and the grid where the neighborhood expansion endpoint is located is an adjacent grid of the grid where the neighborhood expansion start point is located, the estimated transition cost is the original adjacent distance cost. The original adjacent distance cost is the Euclidean distance between two adjacent grids, and the left adjacent distance cost is less than the original adjacent distance cost. In this embodiment, since the original grid distance cost is calculated based on the distance between grids, the original grid distance cost differs for adjacent grids and diagonal grids. Typically, the original grid distance cost for adjacent grids is the side length of one grid, and the original grid distance cost for diagonal grids is the side length of one grid. When the grids are diagonally opposite, a reward is given for the estimated transfer cost when the left cross product is less than 0. A reward left diagonal distance cost, less than the original diagonal distance cost, is set as the estimated transfer cost, ensuring the vehicle travels along the road boundary on the left side of the vehicle. A penalty is given for the estimated transfer cost when the left cross product is greater than or equal to 0. A reward original diagonal distance cost, greater than the reward left diagonal distance cost, is set as the estimated transfer cost, penalizing the vehicle for not traveling along the left side of the vehicle. When the grids are adjacent, a reward left adjacent distance cost, less than the original adjacent distance cost, is set as the estimated transfer cost, ensuring the vehicle travels along the road boundary on the left side of the vehicle. A penalty is given for the estimated transfer cost when the left cross product is greater than or equal to 0. A reward original adjacent distance cost, greater than the reward left adjacent distance cost, is set as the transfer cost, penalizing the vehicle for not traveling along the left side of the vehicle.

[0112] Furthermore, the bonus left-adjacent distance cost is the product of the original adjacent distance cost and the bonus left-adjacent distance reduction factor, and the bonus left diagonal distance cost is the product of the original diagonal distance cost and the bonus left diagonal distance reduction factor. The bonus left-adjacent distance reduction factor is greater than 0 and less than 1, and the bonus left diagonal distance reduction factor is greater than 0 and less than 1. For example, the bonus left-adjacent distance reduction factor can be 0.3, 0.5, or 0.8, and the bonus left diagonal distance reduction factor can be 0.3, 0.5, or 0.8.

[0113] In one embodiment, in each round of eight-neighbor expansion, an expansion direction vector is established with the starting point of the current round's neighborhood expansion as the vector starting point and one neighboring grid of the starting point as the vector ending point.

[0114] Calculate the detectable right distance between the endpoint of the extended direction vector and the nearest point on the right road boundary;

[0115] When the right distance to be detected is within the right safe distance range, the right cross product is calculated based on the preset right cross product formula, according to the nearest point of the neighborhood expansion starting point of the current round and the preset side boundary.

[0116] When the right cross product is greater than 0, the estimated transfer cost is the bonus right grid distance cost; when the right cross product is less than or equal to 0, the estimated transfer cost is the original grid distance cost, which is the Euclidean distance between the starting point and ending point of the current round's neighborhood expansion. The bonus right grid distance cost is less than the original grid distance cost. In this embodiment, to ensure that the vehicle travels along the right road boundary on the right side of the vehicle as much as possible, in each round of eight-neighborhood expansion, an expansion direction vector is established based on the starting point and neighboring grids. The right cross product is calculated using a preset right cross product formula. The right cross product is used to determine the relative positional relationship between the nearest point on the right road boundary and the expansion direction vector. When the right cross product is greater than 0, it means that the nearest point on the right road boundary is to the left of the expansion direction vector and to the right of the vehicle. A bonus right grid distance cost, less than the original grid distance cost, is set as the estimated transfer cost for the neighboring grids corresponding to this expansion direction vector. When the right cross product is less than or equal to 0, it indicates that the nearest point on the right road boundary is to the right of the expanding direction vector, and the nearest point on the right road boundary is to the left of the vehicle. A penalty needs to be applied to the neighboring grid corresponding to this expanding direction vector, setting the original grid distance cost, which is greater than the reward right grid distance cost, as the estimated transfer cost. It's important to understand that each expanding direction vector is determined round-by-round from the current sub-point to the current expanding node. The expanding direction vector and the vehicle's movement direction are opposite; the left side of the expanding direction vector corresponds to the right side of the vehicle. It's also important to understand that the detected right distance is the range between the center of the vehicle and the right road boundary. The safe right distance range is greater than 0.5 times the width of the vehicle to avoid collisions with the road boundary. When the detected right distance is within the safe right distance range, it indicates that the vehicle is close to the right road boundary, and pathpoints close to the right road boundary are rewarded.

[0117] Furthermore, the right safety distance range includes the sum of half the vehicle width and the right safety margin, where the right safety margin is the distance between the right side of the vehicle and the right road boundary. In this embodiment, the right safety distance range consists of two parts: one is half the vehicle width, which is 0.5 times the vehicle width, representing the distance between the center of the vehicle and its right side; the vehicle width is the distance between the right sides of the vehicle. The right safety margin is used to control the distance between the right side of the vehicle and the right road boundary. A right safety margin greater than 0 indicates a larger right safety margin, and a smaller right safety margin indicates that the right side of the vehicle is closer to the right road boundary during driving.

[0118] Furthermore, the presupposed right cross product expression is shown in equation (10):

[0119]

[0120] In equation (10), cross 右 To determine the right cross product, The location of the starting point for the neighborhood expansion (x) c y c and tangent direction , This is the location of the nearest point on the right road boundary.

[0121] In one embodiment, when the right cross product is greater than 0, and the grid where the neighborhood expansion endpoint is located is the diagonal grid of the grid where the neighborhood expansion start point is located, the estimated transfer cost is the right diagonal distance cost; and / or,

[0122] When the right cross product is greater than 0, and the grid cell containing the endpoint of the neighborhood expansion is an adjacent grid cell of the grid cell containing the starting point of the neighborhood expansion, the estimated transition cost is the right neighbor distance cost; and / or,

[0123] When the right cross product is less than or equal to 0, and the grid cell containing the endpoint of the neighborhood expansion is the diagonal grid cell containing the starting grid cell, the estimated transition cost is the original diagonal distance cost; where the original diagonal distance cost is the Euclidean distance between two diagonally opposite grid cells, and the right diagonal distance cost is less than the original diagonal distance cost; and / or,

[0124] When the right cross product is less than or equal to 0, and the grid where the neighborhood expansion endpoint is located is an adjacent grid of the grid where the neighborhood expansion start point is located, the estimated transition cost is the original adjacent distance cost. The original adjacent distance cost is the Euclidean distance between two adjacent grids, and the right adjacent distance cost is less than the original adjacent distance cost. In this embodiment, since the original grid distance cost is calculated based on the distance between grids, the original grid distance cost differs for adjacent grids and diagonal grids. Typically, the original grid distance cost for adjacent grids is the side length of one grid, and the original grid distance cost for diagonal grids is the side length of one grid. When the grids are diagonally opposite, a reward is given for the estimated transfer cost when the right cross product is greater than 0. A reward right diagonal distance cost, less than the original diagonal distance cost, is set as the estimated transfer cost, ensuring the vehicle travels along the road boundary to the right of the vehicle. A penalty is given for the estimated transfer cost when the right cross product is less than or equal to 0. A reward right diagonal distance cost, greater than the reward right diagonal distance cost, is set as the estimated transfer cost, penalizing the vehicle for not traveling along the right side of the vehicle. When the grids are adjacent, a reward right adjacent distance cost, less than the original adjacent distance cost, is set as the estimated transfer cost, ensuring the vehicle travels along the road boundary to the right of the vehicle. A penalty is given for the estimated transfer cost when the right cross product is less than or equal to 0. A reward right adjacent distance cost, greater than the reward right adjacent distance cost, is set as the estimated transfer cost, penalizing the vehicle for not traveling along the right side of the vehicle.

[0125] Furthermore, the bonus right neighbor distance cost is the product of the original neighbor distance cost and the bonus right neighbor reduction factor, and the bonus right diagonal distance cost is the product of the original diagonal distance cost and the bonus right diagonal reduction factor. The bonus right neighbor distance reduction factor is greater than 0 and less than 1, and the bonus right diagonal reduction factor is greater than 0 and less than 1. For example, the bonus right neighbor distance reduction factor can be 0.3, 0.5, or 0.8, and the bonus right diagonal reduction factor can be 0.3, 0.5, or 0.8.

[0126] Furthermore, the estimated cost of the neighborhood expansion endpoint is shown in equations (11) and (12):

[0127]

[0128] In equations (11) and (12), h' is the estimated transfer cost between the starting point and the ending point of the neighborhood expansion, h(n-1) is the estimated cost of the starting point of the neighborhood expansion, and s is the side length of a grid. The side length of the grid represents the accuracy of the map and can be adjusted as needed. The smaller the side length, the more refined the map, but the time consumed by the expansion heuristic will increase.

[0129] Step 3: Compare the node costs of each node in each round of node expansion, and determine the node with the lowest node cost as the first path point in each round. Then, form a driving path based on each of the first path points.

[0130] In this embodiment, the termination point is used as the starting point for backtracking, and the termination point is used as an extended child node, which may correspond to different extended parent nodes. When corresponding extended parent nodes are used, the node costs of the extended child nodes are different. The extended parent node corresponding to the minimum node cost is selected as the first path point. Then, the extended parent node is used as the starting point for the next round of backtracking. In the next round of backtracking, the extended parent node is used as an extended child node, and the extended parent node corresponding to the minimum node cost of the extended child node is selected as the first path point. The extended parent node and the first path point are determined round by round of backtracking.

[0131] In one embodiment, the method further includes:

[0132] Using at least three first path points as a group to be smoothed, a vector to be synthesized is established for every two adjacent first path points in each group to be smoothed.

[0133] Subtract the vectors to be synthesized from each group to obtain the diagonal vectors;

[0134] Based on the sum of the magnitudes of the diagonal vectors, an objective function is constructed, and path point constraints are applied based on preset path point constraint rules.

[0135] The planning model constructed using the objective function and the preset path point constraint rules is used as a preset smoothing model. Solving the preset smoothing model generates multiple second path points, and the driving path is formed based on each second path point. In this embodiment, although the first path points are connected by spiral expansion curves, and the paths between the first path points are relatively smooth, the expansion nodes are connected to spiral expansion curves on both sides. The rate of curvature change at the intersection of two spiral expansion curves may change abruptly. To ensure smooth driving, see [reference needed]. Figure 7 Three first path points, p0, p1, and p2, are selected as a group to be smoothed. Between adjacent first path points p0 and p1, a vector to be synthesized, p0p1, is created with p0 as the starting point and p1 as the ending point. Similarly, between adjacent first path points p1 and p2, a vector to be synthesized, p1p2, is created with p1 as the starting point and p2 as the ending point. Subtracting p0p1 from p1p2 yields a diagonal vector p1v. The modulus of p1v quantifies the distance between first path points p1 and v; a larger modulus indicates a larger distance between p1 and v. The greater the curvature of the curve containing the three first path points p0, p1, and p2, the greater the distance between p1 and v, and consequently, the larger the modulus of p1v. Therefore, an objective function can be established based on the modulus of the diagonal vectors. Alternatively, an objective function can be established based on the sum of the moduli of the diagonal vectors determined for each group to be smoothed. The smaller the value of the objective function, the smaller the sum of the magnitudes of the diagonal vectors determined by each group to be smoothed, reducing the selection of path points with abrupt changes in the rate of curvature change, thus smoothing the curve. Preset path point constraint rules are applied to the generated second path points to ensure that they satisfy these rules. The objective function and the preset path point constraint rules construct a planning model as a preset smoothing model. The preset path point constraint rules constrain the range of variation of the second path points, while the objective function acts as a guide. Different second path points may have different values ​​for the corresponding objective function. The second path points determined when the objective function reaches its minimum value are considered solutions to the preset smoothing model.

[0136] In one embodiment, before the step of establishing a synthesized vector for every two adjacent first path points in each group of at least three first path points as a group to be smoothed, the method includes: performing equidistant interpolation on the first path points so that the spacing between the interpolated first path points is equal. In this embodiment, the spacing between the first path points selected based on node costs may be different. In order to unify the magnitude of the synthesized vectors of each group to be smoothed and balance the scales of different groups to be smoothed, equidistant interpolation is performed on the first path points so that the spacing between the interpolated first path points is the same, thereby unifying the magnitude of the synthesized vectors and reasonably evaluating the diagonal vectors of each group to be smoothed.

[0137] In one embodiment, each first path point is traversed, and each first path point serves as the starting point for constructing a vector group to be smoothed. Three adjacent first path points are used to form a group to be smoothed. In this embodiment, to make the driving path smoother, each first path point is traversed, and each first path point serves as the starting point for constructing a vector group to be smoothed. The starting point for vector construction is the starting point for constructing the vectors to be synthesized within a group to be smoothed. Starting from this first path point, each vector to be synthesized is constructed sequentially, with the endpoint of the previous vector to be synthesized serving as the starting point for the next vector to be synthesized. For example, see... Figure 7 Starting with the first path point p0 as the vector construction starting point, three consecutive first path points p0, p1, and p2 are selected as a group to be smoothed, and vectors p0p1 and p1p2 to be synthesized are constructed sequentially. By traversing each first path point in this way, the curvature of the curve formed by combining the spiral extension curves on both sides of each first path point can be quantified. When the curvature of two spiral extension curves changes abruptly at the first path point where they intersect, the magnitude of the diagonal vector of the group to be smoothed is larger, thus reducing the probability of selecting the corresponding second path points in this case.

[0138] In one embodiment, the preset path point constraint rule includes at least one of feasible region constraints and curvature variation constraints, wherein,

[0139] The feasible domain constraint includes: obtaining each path point to be determined, and constraining the coordinate values ​​of each path point to be determined based on a preset feasible range;

[0140] The curvature variation constraint includes: obtaining the vehicle body sway angle of each of the path points to be determined; calculating the difference between the vehicle body sway angles of two path points to be determined between preset angle difference intervals as the angle difference to be detected; comparing the angle difference to be detected with a preset angle difference threshold, and when the angle difference to be detected is greater than the preset angle difference threshold, constraining the coordinate value of the path point to be determined based on a preset limit range. In this embodiment, the selection range of the second path point is constrained by feasible region constraints and / or curvature variation constraints. The path point to be determined is a second path point with an undetermined value, which is a variable. When the objective function reaches its minimum value, the value corresponding to the path point to be determined is the solution of the planning model, which is the second path point. The feasible region constraint is applied to each path point to be determined, so that the path point to be determined has a determined value within a preset feasible range. See also Figure 8 Each path point to be determined corresponds to a feasible region, and the two sides of the path point on the x-axis are respectively within the preset feasible range []. It can move within a preset feasible range on both sides of the y-axis. [Move within], preset feasible range [ [This refers to the distance that can be moved left and / or right along the x-axis, with a preset feasible range.] [This refers to the distance that can be moved upwards and / or downwards along the y-axis, with a preset feasible range.] ] and preset feasible range[ The range of variation constituting the feasible region of the path point to be found Curvature variation constraints constrain the curvature variations between different path points, reducing the feasible region for path points with large curvature. The vehicle body sway angle of each path point is obtained, and the sway angles of two path points separated by a preset flight angle difference are subtracted to obtain the detected angle difference, which measures the degree of curvature variation between the two path points. To facilitate vehicle movement, the feasible region of path points with large curvature variations is reduced by constraining their coordinate values, thus decreasing their movable range.

[0141] In one embodiment, the formula for calculating the sum of the moduli of each of the diagonal vectors is shown in equation (13):

[0142]

[0143] In equation (13), , represents the vector to be synthesized; , represents the vector to be synthesized; Represents a diagonal vector. Indicates the first path point. This indicates the first path point.

[0144] In one embodiment, the objective function is shown in equation (14):

[0145]

[0146] In equation (14), For sparse parameters, a regularization term is used to prevent overfitting. It is the sum of the magnitudes of each diagonal vector. The coefficients of the real symmetric matrix. Sparse parameters. This is determined automatically by the solver. In this embodiment, Let f(x) be the path point to be found. When the objective function f(x) reaches its minimum value, the path point to be found is... The solution is the location of the second path point.

[0147] In one embodiment, the preset path point constraint rules are shown in equations (15) to (19):

[0148]

[0149] In equations (15) to (19), , This is the coefficient matrix of the equation; , This is the coefficient matrix of the inequality; Let be the path points to be found. , This represents a preset feasible range along the x-axis; , This represents the preset feasible range along the y-axis. For the body sway angle, For the nth path point to be determined The body sway angle; For the (n-1)th path point to be determined The body sway angle; The preset angle difference threshold; Preset limit range, where cof is the angle coefficient. , indicating that the nth path point to be found is traversed. To the One path point to be determined The subscript variable, Path points to be found scope.

[0150] In this embodiment, equations (17) and (18) are feasible region constraints. Equation (17) applies to each path point to be determined along the x-axis. Constrain the feasible region, equation (18) on the y-axis for each path point to be determined. The feasible region is constrained. Equation (19) represents the curvature variation constraint. To predetermine the flight angle difference interval, starting from the nth path point to be determined... To the One path point to be determined There is at least one path point to be determined between them. When the nth path point to be determined and the One path point to be determined Difference in body sway angle That is, the angle difference to be detected is greater than or equal to the preset angle difference threshold. When, for the nth path point to be determined To the One path point to be determined Perform feasible region constraints on the path points to be found between the nth path points, such that the path point to be found is... To the One path point to be determined The movable distance of the path points to be determined along the x-axis is... The movable distance along the y-axis is also .because Equation (19) iterates through each path point to be determined. Apply feasible region constraints.

[0151] In one embodiment, linear interpolation is performed on the second path points to calculate the heading angle and determine the driving path. In this embodiment, since the distance between the solved second path points is uneven, and the driving vehicle requires equidistant path points, linear interpolation is also required to make the interpolated second path points equidistant. The heading angle is obtained by linearly interpolating between the two points. The distance from the interpolation point to the second path point at the starting point is divided by the total distance, and then multiplied by the angle difference between the two points.

[0152] Example 3

[0153] In this embodiment, as Figure 4 As shown, a path planning method is provided, including: given a global map and the starting and ending points of the path, a node expansion unit of a clothoid curve is used to expand nodes; a node evaluation unit of a left-moving rule evaluates the expanded nodes, and takes the node with the minimum cost for the next round of expansion, until the end point is reached, and all the nodes with the minimum cost can form a coarse path; a discrete point smoothing unit of a diagonal vector smooths the coarse path to obtain the final drivable path.

[0154] In this embodiment, a path planning method and system for unmanned dump trucks in open-pit mines is proposed to address the requirements for left-hand driving in open-pit mine environments and the issue of smoothing efficiency for long-distance transportation paths. This method can generate a global path that meets the production requirements of the mining area and provide a safe and reliable left-hand driving path for unmanned mining trucks from the starting point to the end point.

[0155] The method comprises: a node expansion unit for the clothoid curve, a node evaluation unit for the left-row rule, and a discrete point smoothing unit for the diagonal vector.

[0156] The first step involves using the Clothoid curve node expansion unit to generate expansion nodes with a certain step size. These expansion nodes are connected via Clothoid curves, ensuring that the curvature and rate of change of curvature between nodes remain within a reasonable range. The length of the Clothoid curve between each child node and its parent node is fixed. The vehicle turning angle of a child node is one of nine directions, corresponding to the curvature of the vehicle wheel. Expanding from a parent node to a child node means the wheel angle continuously changes to the child node. Since the wheel angle of a child node is fixed, it is expanded forward by a fixed length using the Clothoid curve. Nodes are expanded at each of the nine angles, meaning each parent node expands into nine child nodes, with the parent node connected to each of the nine child nodes via Clothoid curves. Each node has its own parent node pointer, allowing the path from the starting point to the destination to be retrieved by tracing back to the parent node after reaching the destination.

[0157] The second step involves the node evaluation unit of the left-moving rule, which is primarily responsible for evaluating the cost of nodes generated by the node expansion unit of the Clothoid curve. This cost mainly consists of accumulated cost and heuristic cost. After expansion, the point with the lowest cost is selected to form a coarse path.

[0158] The third step involves the discrete point smoothing unit of the diagonal vector, which is mainly responsible for further smoothing the coarse path output by the node evaluation unit of the left-hand rule, thereby improving the smoothness and drivability of the path.

[0159] In this embodiment, based on the global map of the mine, after determining the start and end points of the planned path, a node expansion unit using a clothoid curve is employed to expand nodes from the start point towards the end point. The expanded nodes are evaluated using a node evaluation unit with a left-leaning rule, and the node with the lowest cost is selected and re-input into the clothoid curve node expansion unit to begin a new round of node expansion until the nodes have expanded to a certain range from the end point. Then, a series of nodes with the lowest cost are extracted to form a coarse path from the start point to the end point. Finally, a discrete point smoothing unit using diagonal vectors further smooths the coarse path.

[0160] In one embodiment, such as Figure 5 As shown, the node extension unit of the Clothoid curve discretizes the vehicle's front wheel steering angle into 9 fixed angles, with the front and rear nodes connected by Clothoid curves. The fixed angles of the Clothoid curve's node extension unit refer to the vehicle's wheel steering angles, which change linearly with movement. From the current wheel steering angle to... Figure 2 The target turning angle. The wheel turning angle corresponds to the curvature of the route.

[0161] In one embodiment, the curvature of the clothoid curve changes linearly with the arc length, and the rate of change is determined by the rate of change of curvature. To implement control, assume Parent node, child node Calculated using equation (1):

[0162]

[0163] In equation (1), These represent the axis center position, the vehicle body yaw angle, and the corresponding curvature, respectively. The integral can be calculated using Fresnel functions. =[1,-1] represents the vehicle moving forward or backward. d is either -1 or 1, where 1 represents moving forward and -1 represents moving backward. L represents the rate of curvature change between adjacent parent and child nodes, and L is the step size for expansion. It is pre-calculated, if Greater than the maximum rate of change of curvature If the extended child node is abandoned, this method of extension can effectively limit the rate of curvature change between parent and child nodes and ensure the linear continuity of curvature. l The integral variable represents the path length, which is the integral from 0 to the total length L.

[0164] Equation (1) extends from the parent node to the child node, representing how to calculate the extension from the parent node to the child node, based on the parent node's... Determine child nodes Nine fixed child nodes pass To illustrate this, different child nodes correspond to different curvatures, so the rate of change of curvature... It's different. , represents the rate of change of wheel angle between parent and child nodes, which is also the rate of change of curvature. It is pre-calculated, if Greater than the maximum rate of change of curvature If the child node is not found, then the extended child node is discarded. The child nodes calculated by equation (1) are all those that have undergone... The filtered nodes, in equation (1) All less than the maximum rate of change of curvature A parent node corresponds to multiple child nodes. The maximum rate of change of curvature is used to initially filter the child nodes, retaining those with a rate of change of curvature less than the maximum rate of change of curvature.

[0165] In one embodiment, the node evaluation unit of the left-row rule performs cost calculation on the newly generated node after each node expansion step. The cost calculation is mainly completed by the node evaluation unit of the left-row rule. The clothoid curve is the form expressed by equation (1), which is used to expand the next node. It is a widely used curve, and the child node needs to be calculated using equation (1).

[0166] In one embodiment, the node evaluation unit of the left-row rule calculates the cost by occupying the grid map, and the cost of any node can be expressed as:

[0167]

[0168] In equation (2), the cost function Cost based on estimated distance from the target point and the cumulative cost from the starting point Composition, cumulative cost from the starting point It is calculated in real time during the node expansion process and consists of five costs.

[0169] In one embodiment, the cumulative cost The calculation is shown in equation (3):

[0170]

[0171] Where g(n-1) is the cumulative cost of the parent node of the current node. The cost is the difference in angle between the preceding and following nodes. For the price of reversing, For the cost of distance, For the cost of shifting gears, The cost of moving to the left.

[0172] In one embodiment, the cost of the angle difference between the front and rear nodes The calculation formula is shown in equation (4):

[0173]

[0174] In equation (4), To take the absolute value, For the remainder operation, this formula can be used to calculate the angle between any two points in the direction of the smaller angle. The angle refers to the yaw angle of the vehicle, that is, the direction of the vehicle's front end. This avoids the problem of abrupt changes in angles of 0 and 360 degrees. This cost focuses on the smoothness of the path. (i) The yaw angle of the vehicle body at the i-th node.

[0175] In one embodiment, the cost of reversing The calculation formula is shown in formula (5):

[0176]

[0177] In equation (5), a is a constant. This cost penalizes the backward path because the risk of vehicles reversing is greater in the mining scenario. d(n) represents the nth node, indicating whether the path is forward or backward.

[0178] In one embodiment, the distance cost between preceding and following nodes It is a fixed constant, which corresponds to the step size of the A* algorithm extension.

[0179] In one embodiment, shift cost The calculation formula is shown in equation (6):

[0180]

[0181] In equation (6), b is a constant and a positive number, representing the magnitude of the cost penalty when switching between forward and backward.

[0182] In one embodiment, the cost of left-movement The calculation formula is shown in equation (7):

[0183]

[0184] In equation (7), the cost of left-hand movement Cost of expanding from left-hand nodes and boundary collision cost composition.

[0185] In one embodiment, the cost of expanding the left-row node The calculation formula is shown in equation (8-2):

[0186]

[0187] In equation (8-2), The node number that the current node is extended from by its parent node (e.g., Figure 5 (In the middle number), Equation (8-2) applies appropriate penalties to the extended nodes that turn right. That is, the current node is penalized by the right side of the Clothoid curve corresponding to the front wheel turning angle of the vehicle where the parent node is located. c is the cost constant, and c is a positive number. This cost makes the path prioritize the left-hand node, so that the path can quickly return to the left boundary of the map. Figure 5 Numbers 0, 1, and 2 indicate right turns.

[0188] In one embodiment, collision cost The calculation formula is shown in equation (9):

[0189]

[0190] In equation (9), , , These are the functions for determining whether a vehicle collides with the map boundary (the boundary of roads in the map) after being added to the cost envelope, where d is the penalty constant. For example... Figure 6 As shown, envelopes with distances of e1 and e2 are added to the front, rear, and left sides of the vehicle, respectively, forming two rectangles to determine whether the vehicle collides with the boundary. When the map boundary collides with the vehicle's smaller envelope rectangle ( A penalty is imposed if the path is too close to the boundary; the map boundary collides with the large envelope rectangle. And it did not collide with the small envelope rectangle. A reward is given when the path is within a suitable range from the boundary; otherwise, no reward or penalty is given. This cost allows the path to be planned close to the left boundary of the map. Equation (9) is mainly used to generate attraction after the hybrid A* search reaches the boundary, allowing it to expand nodes along the road boundary. The collision cost is the cost of the child node, and all nodes except the starting point have a parent node. That is, it is also a child node. A rectangular envelope is established with the rear axle center of the vehicle. Based on the vehicle's length, width, wheelbase, and other parameters, rectangular envelopes e1 and e2 are established, where e2>e1. The parameters of these envelopes e1 and e2 are manually configured.

[0191] In one embodiment, the estimated cost per grid distance target point The calculation is performed through steps 3-1 to 3-4.

[0192] Step 3-1, assuming the child node coordinates are In the raster map, an eight-neighbor expansion method is used for child node expansion. The positional relationship between parent and child nodes is determined by taking the counterclockwise x-axis direction as 0° and the child node direction... These are the corresponding eight extended direction angles. One of them Eight-neighbor extension is a costly extension based on raster maps. See also... Figure 10 Set the current child node as the parent node F of the eight-neighbor expansion. Expand the child nodes of its eight neighboring domains. Child node direction It represents the tangent direction of the child node on the Clothoid curve. The eight extended direction angles are used to determine the left and right sides of the map boundary points.

[0193] Step 3-2: Calculate child nodes and the nearest map boundary point. The distance, where, when moving left, the map boundary points It is the left boundary point of the path, the nearest map boundary point. Determined by Euclidean distance. If the distance is within a certain range of the sum of the vehicle's half-width and the safety margin, then the cross product is calculated using equation (10) to determine that the nearest map boundary point is located to the left or right of the expansion direction vector. The expansion direction vector is determined by the positional relationship between the parent and child nodes of the expansion, which for a raster map is [0, 90, 180, 270]; where equation (10) is as follows:

[0194]

[0195] Step 3-3, because it expands from the end point to the starting point, if If the nearest map boundary point is to the right of the child node's expansion direction, the expansion distance cost decreases; otherwise, the original Euclidean distance expansion cost is maintained. The expansion distance cost is a positive constant, less than the Euclidean distance cost.

[0196] It is necessary to understand that The estimated distance cost is calculated from the endpoint towards the starting point, so the direction is opposite to the search direction. The left side of the search direction is the right side of the expansion direction in the estimated distance cost calculation, and the original left side becomes the right side.

[0197] This method allows the extended heuristic cost, i.e., the estimated distance cost h(n), to form a lower cost along the left boundary of the map; the heuristic cost of the nth node is calculated as shown in equations (11) and (12):

[0198]

[0199] In equations (11) and (12), h' represents the transition cost between parent and child nodes. Heuristic costs are not used as path points but only as nodes used for cost calculation. The heuristic cost of a child node equals the heuristic cost of its parent node plus the heuristic cost between parent and child nodes. Each grid cell in each map has a heuristic cost. Estimated distance cost. It calculates the cost of each grid cell on the map; it's a cost map. During pathfinding, a Clothoid curve is used to expand from the starting point. Each expanded point is rounded to an integer to find its corresponding grid cell, which then has an estimated distance cost. This is the same as The total cost is obtained by adding the costs together. The cumulative cost g(n) is calculated during the pathfinding process from the starting point to the destination. The heuristic cost h(n) is pre-calculated, obtained by expanding the calculation from the destination back to the starting point. The point on the path is selected based on... This is the total cost. Nodes with lower total costs will be prioritized for the next round of child node expansion.

[0200] In this embodiment, the estimated distance cost h(n) is used to reward path nodes where the boundary point is on the left, reducing the Euclidean distance expansion cost of these path nodes to achieve the reward objective. For path nodes where the boundary point is on the right, the Euclidean distance expansion cost of these path nodes is penalized, keeping it unchanged. See also... Figure 9 The left side forms a region with low cost, such as Figure 9 The boundary shown is shorter on one side, and the height represents the value of h.

[0201] In this embodiment, see Figure 10 The Euclidean distance expansion cost is calculated by the distance between parent and child nodes. For a raster map with a side length of 1, the diagonal expansion is √2, such as the distance between parent node F and child node S1. The expansion in the x and y directions is 1, such as the distance between parent node F and child node S2. That is, the center is parent node F. When the parent and child nodes are on the x / y axis, the Euclidean distance expansion cost is 1 for the grid side length. When the parent and child nodes are on opposite sides, the Euclidean distance expansion cost is √2 for the grid side length.

[0202] In this embodiment, the eight expansion directions of the eight neighborhoods are [0°, 45°, 90°, 135°, 180°, 225°, 270°, 315°], with the x-axis starting at 0° and increasing by 45° counterclockwise. The expansion directions are determined one by one, starting at 0° and increasing by 45° counterclockwise. The expansion direction vector is between parent and child nodes, determined by the parent node... It is a vector that starts at the point of origin, points to its child nodes, and terminates at the child nodes.

[0203] Steps 3-4 repeat the expansion process from steps 1-3 until all map grids have been expanded. The sum of the above-mentioned costs and the estimated heuristic costs is then calculated. Each node is evaluated, and the node with the lowest cost is selected to proceed to the next round of node expansion. This cost evaluation is then repeated until the starting point of the path is reached. Finally, a series of path points with the lowest costs are selected to obtain a coarse path from the starting point to the ending point. Expanding from the ending point to the starting point refers to calculating the heuristic cost h(n), and the path expansion utilizes a clothoid curve to extend the path from the starting point to the ending point. Path points represent the path from the starting point to the ending point; a coarse path means it is not yet smooth.

[0204] In this embodiment, each node has multiple values ​​of F(n). Different parent nodes may expand to the same child node, thus changing the cost of the child node. The node with the lowest cost is selected. In each round of expansion, a path point is determined based on the minimum cost. The expansion in different rounds is independent of each other. The node with the lowest cost is selected for expansion first, and the rest are placed in a queue to await expansion. The nodes are independent of each other, but their costs are interrelated. The node with the lowest cost selected will not be expanded again.

[0205] In one embodiment, the discrete-point smoothing unit of the diagonal vector mainly smooths the coarse path generated by the node evaluation unit of the left-row rule. For example... Figure 7 As shown, after using a Clothoid curve with linear curvature to perform equidistant interpolation on the coarse path, the length of the Clothoid curve is the same, ensuring that the distance between points is equal, so that the Clothoid curve lengths between two adjacent path points after interpolation are equal. Three consecutive adjacent points p0, p1, and p2 on the coarse path are selected. p0, p1, and p2 may be coarse path points or interpolation points, forming... , The sum of two vectors gives the diagonal vector. , | | is the magnitude of the diagonal vector of node p1. Obviously, the smaller the magnitude of the diagonal vector, the straighter the line connecting points p0, p1, and p2, and the smoother the path. It can be represented as A vector is composed of two nodes.

[0206] In this embodiment, the input to the discrete point smoothing unit of the diagonal vector is the coarse path. The clothoid curve can be used for interpolation because it was previously expanded using a clothoid curve. Uneven interpolation spacing causes uneven magnitude of the smoothed vector; densely spaced points result in a smaller magnitude, leading to inconsistent cost scales during optimization. The step size of the node evaluation unit for the left-hand rule expansion process, after generating the coarse path, requires the use of information from the coarse path points for interpolation. Linear interpolation is performed directly, calculating points between two points as a straight line. The heading angle is interpolated by linearly varying the distance between the two points. The distance from the interpolation point to the starting point is divided by the total distance multiplied by the angle difference between the two points. The heading angle controls the direction of the path at the path points, representing the direction of the vehicle's heading.

[0207] In one embodiment, the sum of the magnitudes of the diagonal vectors of all path points is shown in equation (13):

[0208]

[0209] In equation (13), Equation (13) represents the sum of the magnitudes of the diagonal vectors of all path points. The smoothness of the path diagonal vectors can be represented by the magnitudes of the diagonal vectors. Equation (13) is minimized. Combining the feasible region tunnel, a numerical optimization quadratic programming model is established. The quadratic programming model can be expressed as equations (14) to (17). Among them, the matrix dot product is equivalent to the following: As shown, , yes , , yes , , yes Xi represents the vector consisting of path points. This represents the magnitude of a diagonal vector.

[0210] In one embodiment, the objective function of the quadratic programming model is shown in equation (14):

[0211]

[0212] In equation (14), For sparse parameters, a regularization term is used to prevent overfitting. The coefficients are the real symmetric matrix of the sum of the magnitudes of the diagonal vectors of the path points. Sparse parameters. It is determined automatically by the solver.

[0213] In one embodiment, the preset path point constraint rules of the quadratic programming model include equations (15) and (16), wherein equations (15) and (16) are as follows:

[0214]

[0215] In equations (15) and (16), , The coefficient matrix of the equation. , This is the coefficient matrix of the inequality; Let be the path points to be found. and Classes can be merged into one, with coefficients similar to... This is a format specified by the solver. The open-source solver OSQP is used to solve the planning model; OSQP has standardized inputs and outputs. , , , The numerical values ​​are matrix relationships. A series of constraints on the x and y coordinates are combined to form a matrix, which constrains the displacement of each path point during the smoothing process.

[0216] In one embodiment, the preset path point constraint rules of the quadratic programming model include equations (17) and (18), wherein equations (17) and (18) are as follows:

[0217]

[0218] In equations (17) and (18), the feasible region tunnel is used to construct maximum and minimum value constraints on the x and y coordinates of discrete path points. The feasible region tunnel consists of a local rectangular feasible region for each path point. Composed of the maximum distance that each path point can move in the x and y coordinate directions. Local rectangular feasible region of the path point. These represent the minimum and maximum movable amounts in the positive and negative directions of the x and y axes, respectively. Xmin, Ymin, Xmax, and Ymax are determined using collision detection, which involves moving the point in small steps in each of the x and y directions until it can no longer be moved, thus obtaining the corresponding values.

[0219] In one embodiment, the preset path point constraint rules of the quadratic programming model include equation (19).

[0220]

[0221] In equation (19), To calculate the interval of the heading angle difference, cof is the angle coefficient. This means that the rectangular feasible region representing the path points within this interval is adjusted, where `bound` is the size of the rectangular feasible region. This indicates a large curvature check performed on all path points. When... When this occurs, it indicates that the curvature between the (n+num)th node and the nth node is relatively large. For these nodes with relatively large curvature (n+ind, where...), (Implement local rectangular feasible region constraints separately) This is a restriction applied to all nodes. When The large difference in yaw angles between these points indicates that their curvature is high. Therefore, feasible region reduction is needed; that is, after satisfying the conditions, the size of the feasible region is directly set to the bound. This is a set of constraints applied to all nodes.

[0222] In this embodiment, see Figure 8 Calculate the rectangular constraints in the positive and negative x and y directions for each path point. All the rectangular constraints of the path points constitute the feasible region tunnel. Dashed and solid lines represent different path segments, and the three points in the middle are interpolated points. Figure 8 This is a schematic diagram and does not necessarily represent the interpolation of three points. The points to be interpolated are approximate path points and may be replaced by new points after interpolation. The bound value is an empirical value and can be 0.1. This means setting the bound value of the rectangular area to the bound value, and performing the same operation on each node with large curvature.

[0223] After solving the quadratic programming model, the smoothed point positions change and the point spacing is no longer uniform. Therefore, linear interpolation and recalculation of the point heading angles are performed to obtain the final path. The number of nodes after interpolation is the same as the number of path points after solving. This specific number depends on the number of path points generated during the A* planning module. After solving the quadratic programming model, the optimized point spacing may not be uniform, and control usually requires equidistant points as input, so interpolation is needed to adjust the point spacing to be consistent.

[0224] It is important to understand that the left-row rule is transformed into the right-row rule, and the right-row rule can achieve the same effect.

[0225] This embodiment proposes a path planning method that uses a left-hand hybrid A* algorithm to satisfy the left-hand driving rules in mining areas. It also proposes using a clothoid curve instead of the traditional method of fixed front wheel angle for node expansion, improving the quality of the initial path. Finally, it utilizes a diagonal vector smoothing quadratic programming model with large curvature constraint points to smooth the path, improving efficiency while ensuring curvature constraints are met. The proposed method significantly improves the applicability of the traditional hybrid A* algorithm to mining scenarios, while ensuring the safety, smoothness, and drivability of the final generated path.

[0226] In this embodiment, the functions of the clothoid curve include: (1) ensuring linear continuity of curvature between parent and child nodes; and (2) removing extended nodes with excessively large rates of curvature change between parent and child nodes. Clothoid curve interpolation is mainly used during the search process. Typically, to speed up the search, the search step size is increased, for example, a step size of 4 meters. After the search is complete, the clothoid curve is used for interpolation to increase the path point density.

[0227] The technical innovations in this embodiment are summarized in Table 1 below:

[0228]

[0229] Example 4

[0230] In this embodiment, as Figure 2 As shown, a path planning device is provided, comprising:

[0231] Node expansion module: used to expand nodes from the initial position point in multiple rounds based on at least two preset expansion directions and a spiral expansion curve until the termination position is found, and obtain each expanded node;

[0232] Cost calculation module: used to calculate the node cost of each of the extended nodes based on preset cost calculation rules;

[0233] Path generation module: used to compare the node costs of each of the expanded nodes in each round of node expansion, determine the expanded node with the lowest node cost in each round as the first path point, and form a driving path based on each of the first path points.

[0234] For specific limitations regarding the path planning device, please refer to the limitations of the path planning method above, which will not be repeated here. Each unit in the aforementioned path planning device can be implemented entirely or partially through software, hardware, or a combination thereof. These units can be embedded in or independent of the processor in a computer device in hardware form, or stored in the memory of a computer device in software form, so that the processor can call and execute the operations corresponding to each unit.

[0235] Example 5

[0236] In this embodiment, a computer device is provided. Its internal structure diagram can be shown as follows: Figure 3As shown, the computer device includes a processor, memory, network interface, display screen, and input devices connected via a system bus. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system and computer programs, and also contains a database storing preset cost calculation rules. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The network interface communicates with other computer devices that have deployed application software. When executed by the processor, the computer program implements a path planning method. The display screen can be an LCD screen or an e-ink screen. The input devices can be a touch layer covering the display screen, buttons, a trackball, or a touchpad on the computer device's casing, or an external keyboard, touchpad, or mouse.

[0237] Those skilled in the art will understand that Figure 3 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0238] In one embodiment, a computer device is provided, including a memory and a processor, the memory storing a computer program, the processor executing the computer program to implement the steps of the path planning method described in any of the above embodiments.

[0239] Example 6

[0240] In this embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, it implements the steps of the path planning method described in any of the above embodiments.

[0241] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0242] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0243] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A path planning method characterized by, include: Based on at least two preset expansion directions and a spiral expansion curve, multiple rounds of node expansion are performed from the initial position until the termination position is found, thus obtaining each expanded node. Based on the preset cost calculation rules, calculate the node cost of each of the extended nodes; The node costs of each node in each round of node expansion are compared, and the node with the lowest node cost is determined as the first path point in each round. Using at least three first path points as a group to be smoothed, a vector to be synthesized is established for every two adjacent first path points in each group to be smoothed; the vectors to be synthesized in the same group are subtracted to obtain each diagonal vector; an objective function is constructed based on the sum of the magnitudes of each diagonal vector, and path point constraints are performed based on preset path point constraint rules; the planning model constructed by the objective function and the preset path point constraint rules is used as a preset smoothing processing model, and the preset smoothing processing model is solved to generate multiple second path points, and a driving path is formed based on each second path point; The preset path point constraint rules include curvature variation constraints; the curvature variation constraints include: obtaining the vehicle body sway angle of each path point to be determined; calculating the difference between the vehicle body sway angles of two path points to be determined between preset flight angle difference intervals as the angle difference to be detected; comparing the angle difference to be detected with a preset angle difference threshold, and when the angle difference to be detected is greater than the preset angle difference threshold, constraining the coordinate values ​​of the path point to be determined based on a preset limit range.

2. The path planning method of claim 1, wherein, The preset path point constraint rules also include feasible region constraints; wherein, the feasible region constraints include: obtaining each path point to be determined, and constraining the coordinate values ​​of each path point to be determined based on a preset feasible range.

3. The path planning method according to claim 1, characterized in that, The steps for calculating the node cost of each of the extended nodes based on preset cost calculation rules include: Obtain the current extended node; Calculate the actual cost between the initial position and the current expansion node, and calculate the estimated cost between the termination position and the current expansion node; The actual cost and the estimated cost are summed to obtain the current node cost of the expanded node.

4. The path planning method according to claim 3, characterized in that, The calculation rules for the actual cost of the currently described expansion node include: Take the parent node of the current extended node as the current parent node, and obtain the actual cost of the current parent node as the actual cost of the parent node; The actual transfer cost between the current parent node and the extended node is calculated based on a preset real-price calculation model. The actual cost of the parent node and the actual transfer cost are summed to obtain the actual cost of the current extended node. And / or, The calculation rules for the estimated cost of the currently described expansion node include: Take the child node of the current extended node as the current child node, and obtain the estimated cost of the current child node as the estimated cost of the child node. Using the current sub-point as the starting point for neighborhood expansion, perform multiple rounds of eight-neighborhood expansion until the current expansion node is found. Determine the estimated transfer cost corresponding to each round of eight-neighborhood expansion. Add the estimated transfer costs determined by each round of eight-neighborhood expansion to the estimated cost of the sub-point to obtain the estimated cost of the current expansion node.

5. The path planning method according to claim 4, characterized in that, The preset real price calculation model includes: calculating the transfer price between the current parent point and the extended node according to each preset real price transfer rule; summing the transfer prices to obtain the actual transfer cost; wherein the preset real price transfer rules include at least one of the following: angle difference cost rule, reversing cost rule, node distance cost rule, gear shifting cost rule, preset side extension cost rule, and preset side collision cost rule. The angle difference cost rule includes: obtaining the current vehicle body sway angle of the current extended node and the parent point vehicle body sway angle of the current parent point, and calculating the angle difference cost between the current parent point and the extended node as the transfer price based on the preset sway angle cost formula; The reversing cost rule includes: obtaining the current movement direction of the extended node as the current movement direction, wherein the movement direction includes a forward direction and a backward direction; when the current movement direction is a forward direction, determining a preset first reversing cost as the transfer price of the current extended node; when the current movement direction is the backward direction, determining a preset second reversing cost as the transfer price of the current extended node, wherein the preset second reversing cost is greater than the preset first reversing cost; The node distance cost rule includes: obtaining the expansion step size between the current expansion node and the current parent node as the transfer cost; The shift cost rule includes: obtaining the current movement direction of the current extended node, obtaining the movement direction of the current parent point as the parent point movement direction; when the current movement direction and the parent point movement direction are different, determining a preset shift cost as the transfer cost, wherein the preset shift cost is greater than zero; The preset side expansion cost rule includes: obtaining the current turning direction of the current expansion node; when the current turning direction is far away from the preset side direction, determining the preset first turning cost as the transfer cost of the current expansion node; when the preset expansion direction is close to the preset side direction, determining the preset second turning cost as the transfer cost of the current expansion node, wherein the preset second turning cost is less than the preset first turning cost. The preset side collision cost rule includes: obtaining the envelope region of the current extended node, wherein the boundary of the envelope region is outside the vehicle area; when the boundary of the envelope region contacts the preset side boundary of the road, determining a preset reward / penalty threshold as the transfer cost of the current extended node.

6. The path planning method according to claim 5, characterized in that, The envelope region includes an inner envelope and an outer envelope. The boundary of the inner envelope and the boundary of the outer envelope are respectively outside the vehicle region, and the boundary of the outer envelope is outside the boundary of the inner envelope. The calculation rules for the preset side collision cost include: obtaining the inner envelope and outer envelope of the current extended node; when the boundary of the inner envelope is in contact with the preset side boundary, determining a preset first reward / penalty threshold as the preset reward / penalty threshold; when the boundary of the inner envelope is isolated from the preset side boundary and the boundary of the outer envelope is in contact with the preset side boundary, determining a preset second reward / penalty threshold as the preset reward / penalty threshold, wherein the preset second reward / penalty threshold is less than the preset first reward / penalty threshold.

7. A path planning device, characterized in that, include: Node expansion module: used to expand nodes from the initial position point in multiple rounds based on at least two preset expansion directions and a spiral expansion curve until the termination position is found, and obtain each expanded node; Cost calculation module: used to calculate the node cost of each of the extended nodes based on preset cost calculation rules; Path generation module: used to compare the node costs of each expansion node in each round of node expansion, and determine the expansion node with the lowest node cost as the first path point in each round; Taking at least three first path points as a smoothing group, a composite vector is established for every two adjacent first path points in each smoothing group; the composite vectors in the same group are subtracted to obtain diagonal vectors; an objective function is constructed based on the sum of the magnitudes of the diagonal vectors, and path point constraints are applied based on preset path point constraint rules; the planning model constructed by the objective function and the preset path point constraint rules is used as a preset smoothing processing model, and the preset smoothing processing model is solved to generate multiple second path points, and a driving path is formed based on each second path point; wherein, the preset path point constraint rules include curvature change constraints; the curvature change constraints include: obtaining the vehicle body sway angle of each path point to be determined; calculating the difference between the vehicle body sway angles of two path points to be determined between preset angle difference intervals as the angle difference to be detected; comparing the angle difference to be detected with a preset angle difference threshold, and when the angle difference to be detected is greater than the preset angle difference threshold, the coordinate values ​​of the path points to be determined are constrained based on a preset limit range.

8. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1 to 6.

Images