Automatic driving vehicle path planning method fusing motion constraints and safety constraints and medium

Abstract

The application discloses a path planning method of an automatic driving vehicle fusing motion constraints and safety constraints and a medium, and the method steps are as follows: 1) initializing path planning parameters; 2) establishing an evaluation function introducing a safety distance and motion constraints; 3) calculating a shortest path of the automatic driving vehicle by using the evaluation function; 4) screening path nodes in the shortest path and removing redundant path nodes; 5) smoothing the path optimization nodes by using a third-order Bezier curve, and generating a planned path of the automatic driving vehicle; and 6) controlling the automatic driving vehicle to travel according to the planned path, and judging whether an obstacle changes every time the automatic driving vehicle passes a node, if the obstacle changes, updating a vehicle actual moving distance k m value and a current position s start , setting the current position as s last , updating a generation value of an affected node, and then returning to step 4). The medium stores a computer program; and the application significantly improves smoothness and continuity of motion of the automatic driving vehicle.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of vehicle path planning, in particular to an automatic driving vehicle path planning method and medium fusing motion constraints and safety constraints. BACKGROUND

[0002] With the rapid progress of science and technology, automatic driving vehicles have developed rapidly and received extensive attention from experts in various fields. Automatic driving vehicles play a crucial role in national defense, military, warehousing logistics, smart home, intelligent manufacturing, and other tasks. As the application scenarios of automatic driving vehicles become more complex, the autonomy of automatic driving vehicles in the environment is increasingly required. Not only static obstacles, but also dynamic obstacles need to be avoided. Therefore, automatic driving vehicle path planning in dynamic environments is a basic and key problem.

[0003] Path planning algorithms mainly include graph search algorithms, random sampling algorithms, intelligent algorithms, etc. Graph search algorithms mainly include A* algorithm and Dijkstra algorithm, etc. Random sampling algorithms mainly include Probabilistic Roadmaps (PRM) algorithm, Rapidly-Exploring Random Tree (RRT) algorithm, etc. Intelligent algorithms mainly include genetic algorithm, ant colony algorithm, etc. These algorithms are widely used in path planning.

[0004] Typical path planning algorithms have good obstacle avoidance effect for static obstacles, but are not suitable for dynamic obstacles. However, in real working environments, the environment has complexity and randomness. Assuming that an automatic driving vehicle is disturbed by random obstacles on the route during travel, the automatic driving vehicle will collide with the obstacles. For dynamic environment path planning, incremental heuristic search algorithms are better than traditional heuristic search algorithms because incremental heuristic search algorithms can reuse previously searched path information, and when encountering an infeasible path, the automatic driving vehicle can quickly re-plan the path. Incremental algorithms designed for dynamic environments mainly include LPA* (Lifelong Planning A-star) algorithm and D*Lite (Dynamic A-star Lite) algorithm. LPA* algorithm is used to solve the shortest path problem with fixed start point and fixed target point in dynamic environment. D*Lite algorithm is improved based on LPA* algorithm and is a path planning algorithm with variable start point and fixed target point. At the same time, D*Lite also uses the reverse search method of D*, which is more suitable for handling dynamic obstacles.

[0005] However, the D*Lite algorithm also has disadvantages. The D*Lite algorithm searches the path by eight-neighborhood, and the planned path is not smooth, and there are many turning points, and the turning angle is a multiple of 45 degrees, which is not practical for autonomous vehicles because of its limited turning performance and does not meet the kinematic constraint model of autonomous vehicles. The path planned by the D*Lite algorithm is very close to the obstacle, and it is extremely easy to collide, and the safety is low. Some scholars have improved the D*Lite algorithm, that is, when planning the path, the obstacle is inflated, and the kinematic model of the vehicle is constructed. Although this method considers safety and kinematic constraints, the key to ensuring path safety is to inflate the obstacle. Inflation is a direct operation on the environment map, which is very dependent on the accuracy of the map, and at the same time, inflating the obstacle in the environment map will reduce the quality of the map, making it difficult to achieve accurate navigation. SUMMARY

[0006] The purpose of the present application is to provide an autonomous vehicle path planning method that combines motion constraints and safety constraints, comprising the following steps:

[0007] 1) Load the grid map containing the current position node s of the autonomous vehicle start and the target node; let the starting position at the last time s last =s start ;

[0008] 2) Initialize the path planning parameters;

[0009] 3) Establish an evaluation function that introduces a safety distance and motion constraint;

[0010] 4) Calculate the shortest path of the autonomous vehicle using the evaluation function; the shortest path includes a plurality of path nodes;

[0011] 5) Screen the path nodes in the shortest path to remove redundant path nodes to obtain path optimization nodes;

[0012] 6) Smooth the path optimization nodes using a third-order Bezier curve to generate an autonomous vehicle planning path;

[0013] 7) Control the autonomous vehicle to travel according to the planned path, and the autonomous vehicle passes through each node, then judge whether the obstacle changes, if the obstacle changes, update the actual moving distance k m value and the current position s start , and set the current position as s last , update the generation value of the affected node, and then return to step 4).

[0014] Further, the step of initializing the path planning parameters comprises:

[0015] Setting the path node priority queue U as an empty queue, and setting the actual moving distance k of the autonomous vehicle to 0. m ;

[0016] Setting the g value and the rhs value of all nodes of the grid map to infinity; the g value represents the distance from the node to the target node; the rhs value represents the minimum value of the distance from the current node s to the target node and the safety distance obtained by the successor node of s, i.e. the cost function;

[0017] Setting the rhs value of the target node to 0, and calculating the evaluation function key value of the target point, and writing the target node into the priority queue U.

[0018] Further, the step of establishing the evaluation function introducing the safety distance and the motion constraint comprises:

[0019] 3.1) Establishing the safety distance matrix S of the current position node n×n , i.e.

[0020]

[0021] In the formula, n x n is the dimension of the safety distance matrix; n is an odd number and n≥3; in the matrix S n×n , "1" represents a sub-node that the autonomous vehicle is likely to reach next, "3" represents a node to be detected; and "0" represents a node that does not need to be searched;

[0022] 3.2) Based on the safety distance matrix S n×n , the cost function introducing the safety distance is established, i.e.

[0023]

[0024] In the formula, Succ(s) represents all successor nodes of the current node s, c(s,s') is the cost value from the current node s to the successor node s', and g(s') represents the distance from the successor node s' to the target node; D S is an increment; when there is no obstacle in the node to be detected in the safety distance matrix S n×n , D S =0, otherwise, D S ≠0.

[0025] 3.3) The evaluation function key introducing the safety distance and the motion constraint is established, i.e.

[0026]

[0027] In the formula, k1 and k2 are intermediate parameters; the parameter k m =k m '+h(s last ,sstart );k m ' represents the actual moving distance of the vehicle at the last time, s last represents the last starting point, s start represents the current position of the autonomous vehicle, h(s last ,s start ) represents the distance between s last and s start . g(s) indicates the distance from the current node s to the target node s goal ; h(s,s start ) indicates the distance between s and s start .

[0028] Further, the step of calculating the shortest path of the autonomous vehicle by using the evaluation function comprises:

[0029] 4.1) Take out the node s with the smallest key value from the path node priority queue U; expand the node s to obtain a plurality of child nodes; update the g value, rhs value and key value of the child nodes, and add the child nodes to the priority queue U or update the corresponding values of the child nodes in the priority queue U;

[0030] 4.2) Determine whether key(s) < key(s start ) or rhs(s start )≠g(s start ) is true, if true, repeat step 4.1), if not, start from the current position node s start of the autonomous vehicle, traverse all parent nodes until the target point s goal is found, thereby generating the shortest path.

[0031] Further, the step of screening the path nodes in the shortest path comprises:

[0032] 5.1) Start from the i-th node and traverse all nodes in turn, if the current node can be connected to the i-th node without obstacles, remove the nodes between the first node and the current node; the initial value of i is 1;

[0033] 5.2) Let i=i+1 and return to step 5.1) until the target point is connected.

[0034] Further, the third-order Bezier curve parameter P(t) is as follows:

[0035] P(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3,t∈[0,1] (4)

[0036] In the formula, P0, P1, P2 and P3 represent control points of a third-order Bezier curve.

[0037] Further, the included angle β between the axis of the autonomous vehicle and the driving direction of the front wheel satisfies the following constraint:

[0038] |β|≤β max (5)

[0039] In the formula, β represents the included angle between the axis of the autonomous vehicle and the driving direction of the front wheel; and β max is the upper limit of the included angle between the axis of the autonomous vehicle and the driving direction of the front wheel.

[0040] The path curvature κ of the motion of the autonomous vehicle satisfies the following constraint:

[0041]

[0042] In the formula, R min is the minimum turning radius of the vehicle; β max is the upper limit of the included angle between the axis of the autonomous vehicle and the driving direction of the front wheel, and l represents the wheelbase.

[0043] A computer readable storage medium having a computer program stored thereon;

[0044] When the computer program is invoked, the steps of the path planning method of the autonomous vehicle fusing the motion constraint and the safety constraint are executed.

[0045] It is worth noting that the present application proposes an Improved D*Lite (ID*Lite) algorithm fusing the motion constraint and the safety constraint. The algorithm first sets the safety distance between the autonomous vehicle and the obstacle in the D*Lite algorithm to improve the safety of the path. Then, based on the kinematic constraint of the autonomous vehicle, the search direction of the path is increased to avoid the turning angle of the path exceeding the steering maneuverability of the autonomous vehicle. Finally, the path is optimized, the optimization algorithm of removing redundant points is used to solve the problem of zigzag search path and redundant nodes, the third-order Bezier curve is used to smooth the path, and a curvature-continuous path is generated, which is convenient for subsequent motion control of the autonomous vehicle.

[0046] The technical effects of the present application are self-evident, and the beneficial effects of the present application are as follows:

[0047] 1) The path planned by the present application can be far away from the obstacle, which ensures the safety and feasibility of the path.

[0048] 2) The kinematic constraint of the autonomous vehicle is introduced in the present application, which solves the problem of limited steering performance of the autonomous vehicle.

[0049] 4) The application can reduce the number of motor start-stop of the autonomous vehicle, which is conducive to prolonging the service life of the autonomous vehicle.

[0050] 5) The application smoothes the vehicle driving path, and the smoothed path is shorter and has better smoothing performance, which is more conducive to the stable control of the autonomous vehicle.

[0051] 6) The application significantly improves the smoothness and continuity of the motion of the autonomous vehicle. BRIEF DESCRIPTION OF DRAWINGS

[0052] Figure 1 is the flow chart of the ID Lite algorithm of the application;

[0053] Figure 2 is the kinematic model of the autonomous vehicle of the application;

[0054] Figure 3 is the principle diagram of the application for eliminating redundant points;

[0055] Figure 4 (a), Figure 4 (b), Figure 4 (c) are the path planning effects based on the no safety distance matrix, the 3-dimensional safety distance matrix, and the 5-dimensional safety distance matrix, respectively;

[0056] Figure 5 (a), Figure 5 (b) are the path planning effects of introducing 8-direction 8-neighborhood kinematic constraints and 16-direction 24-neighborhood kinematic constraints on the 30*30 grid map of the application, respectively;

[0057] Figure 6 (a), Figure 6 (b) are the path planning effects of introducing 8-direction 8-neighborhood kinematic constraints and 16-direction 24-neighborhood kinematic constraints on the 50*50 grid map of the application, respectively;

[0058] Figure 7 (a), Figure 7 (b) are the path planning comparison effects before and after eliminating the redundant points of the application, respectively;

[0059] Figure 8 (a), Figure 8 (b) are the path planning comparison effects before and after smoothing the path of the application, respectively;

[0060] Figure 9 is the path planning effect of different algorithms of the application in the same static environment;

[0061] Figure 10 (a), Figure 10(b) shows the path planning performance of the D*Lite and ID*Lite algorithms in a dynamic environment with random obstacles. Detailed Implementation

[0062] The present invention will be further described below with reference to embodiments, but it should not be construed that the scope of the present invention is limited to the following embodiments. Various substitutions and modifications made based on ordinary technical knowledge and common practices in the art without departing from the above-described technical concept of the present invention should be included within the scope of protection of the present invention.

[0063] Example 1:

[0064] See Figures 1 to 10 An autonomous vehicle path planning method that integrates motion constraints and safety constraints includes the following steps:

[0065] 1) Load the node containing the current location of the autonomous vehicle. start And the grid map of the target node; let the starting position s of the previous time step be... last =s start ;

[0066] 2) Initialize path planning parameters;

[0067] 3) Establish an evaluation function that incorporates safety distance and motion constraints;

[0068] 4) Calculate the shortest path for the autonomous vehicle using the evaluation function; the shortest path includes multiple path nodes;

[0069] 5) Filter the path nodes in the shortest path, remove redundant path nodes, and obtain the path optimization nodes;

[0070] 6) Use third-order Bézier curves to smooth the path optimization nodes and generate the planned path for autonomous vehicles;

[0071] 7) Control the autonomous vehicle to travel along the planned path. Each time the autonomous vehicle passes a node, determine if the obstacle has changed. If it has changed, update the vehicle's actual travel distance k. m Value and current position s start And set the current position to s last Update the value of the affected node, and then return to step 4).

[0072] The steps for initializing path planning parameters include:

[0073] Set the path node priority queue U to an empty queue, and let the autonomous vehicle travel the actual distance k. m Set to 0;

[0074] Set the g and rhs values ​​of all nodes in the raster map to infinity; the g value represents the distance from the node to the target node; the rhs value represents the minimum sum of the distance from the current node s to the target node and the safe distance obtained from the successor nodes of the current node s, i.e., the cost function.

[0075] Set the RHS value of the target node to 0, calculate the evaluation function key value of the target node, and write the target node into the priority queue U.

[0076] The steps to establish an evaluation function that incorporates safety distance and motion constraints include:

[0077] 3.1) Establish the safe distance matrix S of the current location node. n×n ,Right now:

[0078]

[0079] In the formula, n×n is the dimension of the safety distance matrix; n is an odd number and n≥3; matrix S n×n In this context, "1" represents a child node that the autonomous vehicle may reach next, "3" represents a node to be detected, and "0" represents a node that does not need to be searched.

[0080] 3.2) Based on the safety distance matrix S n×n Establish the cost function that introduces a safe distance, namely:

[0081]

[0082] In the formula, Succ(s) represents all successor nodes of the current node s, and c(s,s') is the current node's successor node. s The cost to the successor node s', g(s') represents the distance from the successor node s' to the target node; D S This is an additional term; when the safety distance matrix S n×n If there are no obstacles at the node to be detected, then D S =0, otherwise, D S ≠0.

[0083] 3.3) Establish an evaluation function key that incorporates safety distance and motion constraints, namely:

[0084]

[0085] In the formula, k1 and k2 are intermediate parameters; parameter k m =k m '+h(s last ,s start );k m 'Represents the actual distance the vehicle traveled at the previous moment, s last s represents the previous starting point. starth(s) represents the current position of the autonomous vehicle, last h(s start ) represents the distance between s last and s start . g(s) represents the distance from the current node s to the target node s goal ; h(s, s start ) represents the distance between s and s start . k1 = min(g(s), rhs(s)) + h(s, s start ) + k m . k2 = min(g(s), rhs(s)).

[0086] The step of calculating the shortest path of the autonomous vehicle using the evaluation function comprises:

[0087] 4.1) Take out the node s with the smallest key value from the path node priority queue U; expand the node s to obtain a plurality of child nodes; update the g value, rhs value and key value of the child nodes, and add the child nodes to the priority queue U or update the corresponding values of the child nodes in the priority queue U;

[0088] 4.2) Determine whether key(s) < key(s start ) or rhs(s start ) ≠ g(s start ) is true, if true, repeat step 4.1), if not, start from the current position node s start of the autonomous vehicle, traverse all parent nodes until the target node s goal is found, thereby generating the shortest path. key(s start ), key(s) represents the evaluation function of s, s start ; rhs(s start ) represents the cost function of s start ; g(s start ) represents the distance from s start to the target node s goal ;

[0089] The step of screening the path nodes in the shortest path comprises:

[0090] 5.1) Starting from the i-th node, traverse all nodes in turn, if the current node can be connected to the i-th node without obstacles, remove the nodes between the first node and the current node; the initial value of i is 1;

[0091] 5.2) Let i = i + 1, and return to step 5.1) until the target point is connected.

[0092] The third order Bezier curve parameter P(t) is as follows:

[0093] P(t) = (1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3,t∈[0,1] (4)

[0094] wherein P0, P1, P2, P3 represent control points of the cubic Bezier curve.

[0095] The angle β between the axis of the autonomous vehicle and the driving direction of the front wheel satisfies the following constraint:

[0096] |β|≤β max (5)

[0097] wherein β represents the angle between the axis of the autonomous vehicle and the driving direction of the front wheel; and β max is the upper limit of the angle between the axis of the autonomous vehicle and the driving direction of the front wheel.

[0098] The path curvature κ of the motion of the autonomous vehicle satisfies the following constraint:

[0099]

[0100] wherein R min is the minimum turning radius of the vehicle; β max is the upper limit of the angle between the axis of the autonomous vehicle and the driving direction of the front wheel, and l represents the wheelbase.

[0101] A computer readable storage medium having stored thereon a computer program;

[0102] When the computer program is invoked, the steps of the method for planning a path of an autonomous vehicle by fusing motion constraints and safety constraints are executed.

[0103] Embodiment 2:

[0104] The method for planning a path of an autonomous vehicle by fusing motion constraints and safety constraints comprises the following steps:

[0105] 1. Set the current position of the autonomous vehicle as a starting point s start , assign s last =s start , and s last represents the previous starting point;

[0106] 2. Initialization, specifically:

[0107] Set the priority queue U as an empty queue, and set the actual moving distance k m of the autonomous vehicle as 0;

[0108] Set the g value and the rhs value of all nodes s to infinity; the g value represents the distance from the node to the target node; the rhs value represents the minimum value of the sum of the distance from the current node s to the target node and the safety distance obtained by the successor node of s, i.e. the cost function;

[0109] Finally, set the rhs value of the target point to 0, calculate the evaluation function key value thereof, and add it to the priority queue U.

[0110] 3. Calculate the shortest path based on the evaluation function with the added safety distance and motion constraint;

[0111] (1) The specific process of the cost function with the added safety distance is as follows:

[0112] The safety distance matrix is used to describe the safety distance by setting the safety distance between the autonomous vehicle and the obstacle to improve the D*Lite algorithm, as shown in formula (1), which is an n*n matrix, n is an odd number and n≥3, and the dimension of the matrix is flexibly selected according to the actual working safety distance size.

[0113]

[0114] The matrix S n×n The "1" in the matrix represents the position that the autonomous vehicle may reach next, i.e. the child node; the "3" on the outermost periphery of the matrix represents the position to be detected; and the remaining positions in the matrix are all "0", representing the points that do not need to be searched. When searching for the path, the cost of each "1" point is detected, and then it is detected whether there is an obstacle at the position of "3" in the safety distance matrix S n×n of the "1" position. If there is an obstacle, a term D S is added to the rhs(s) function of the D*Lite algorithm. If there is no obstacle, it is not added.

[0115]

[0116] Wherein: Succ(s) represents all successor nodes of node s, c(s,s') is the cost value from the current node s to s', and g(s') represents the distance from node s' to the target point.

[0117] (2) The specific process of introducing kinematic constraint to perform path planning based on D*Lite is as follows:

[0118] The autonomous vehicle with the minimum turning radius constraint is considered as a typical non-holonomic constraint system, and the simplified model is as shown in Figure 2 Due to the mechanical characteristics β of the autonomous vehicle, the following constraint needs to be met:

[0119] |β|≤β max(3)

[0120] where β represents the angle between the center axis of the autonomous vehicle and the driving direction of the front wheel; β max is the upper limit of the angle between the center axis of the autonomous vehicle and the driving direction of the front wheel.

[0121] The path curvature κ of the autonomous vehicle motion needs to satisfy:

[0122]

[0123] where R min is the minimum turning radius of the vehicle; β max is the upper limit of the angle between the center axis of the autonomous vehicle and the driving direction of the front wheel; and l represents the wheelbase.(5)

[0124] It is thus concluded that:

[0125]

[0126]

[0127] where l is 2649 mm, R min is 1466 mm, and β max is 32.2 degrees.

[0128] By increasing the number of search neighborhoods to reduce the steering angle, and by adding 8 more search directions on the original basis, the search range is expanded to 16 search directions, and 24 neighborhoods of the current node can be detected, and the minimum turning angle is refined to 22.5 degrees.

[0129] (3) The specific process of calculating the shortest path is as follows:

[0130] 1) Take the point s with the smallest key value from U for expansion, update the g value and rhs value of the child node of s, calculate the key value of the child node, and add it to the priority queue U or update the value in U;

[0131] 2) If key(s) < key(s start ) or rhs(s start ) ≠ g(s start ), repeat step 1), continuously diffuse and traverse in the direction of the starting point, otherwise traverse all parent nodes from s start until the target point s goal is found, thereby obtaining the initial shortest path.

[0132] The calculation formula of the key value is as follows:

[0133]

[0134] k1 is composed of three items, the minimum value of g(s) value and rhs(s) value of the current point, the estimated value of the current point to the starting point and k m value. And k m m = k last ' + h(s start ,s m ), k last ' represents the actual moving distance of the vehicle at the last time, s start represents the last starting point, s last represents the current position of the autonomous vehicle, h(s start ,s last ) represents the distance between s start and s 3 .

[0135] k2 represents the minimum value of g(s) value and rhs(s).

[0136] 4. Remove the redundant points of the path, the specific process is as follows:

[0137] The principle of removing the redundant points of the path is shown in Figure 3 . The original path is shown by the black dotted line. Starting from the starting point S1, all nodes are traversed in turn, if the current node can be connected to the starting point without obstacles, for example, S1S4, then all the intermediate nodes (S2 and S3) between them are removed, otherwise, return to the previous path point. In this way, until connected to the target point. Therefore, the finally generated path only contains the starting point, the turning point and the target point, as shown by the black solid line in Figure 3 .

[0138] 5. Further smooth the path based on the third order Bezier curve. The parameterized expression of the third order Bezier curve is

[0139] P(t) = (1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3,t∈[0,1] (9)

[0140] P0, P1, P2, P3 represent four control points. The flow of smoothing the path is as follows: first, select the key path points of the ID*Lite algorithm path, and then select the path points that meet the actual situation as the control points of the third order Bezier curve. Secondly, the smooth path of a path segment is derived from the control points. Finally, the smooth path segments are connected in turn to obtain a smooth path that meets the kinematic constraints of the autonomous vehicle.

[0141] 6. The autonomous vehicle moves along the generated path;

[0142] 7. The autonomous vehicle checks whether the obstacle changes every step, and if it changes, updates k m , the value of k and the starting point, and sets the current point as s last , updates the value of rhs of the affected point and adds it to the priority queue U;

[0143] 8. Recalculates the shortest path, removes redundant points on the path, and smoothens the path with a cubic Bezier curve. When the target point is reached, the path planning process ends.

[0144] Example 3:

[0145] The verification experiment of the autonomous vehicle path planning method that combines motion constraints and safety constraints includes the following contents:

[0146] 1. Technical effect of setting safety distance

[0147] To verify the effect of the ID*Lite algorithm for setting safety distance, path search is performed on a 30m*30m grid map. The starting point is at the lower left of the map, represented by black. The target point is at the upper right of the map, represented by a black circle. The path planning results of the D*Lite algorithm based on different dimension safety distance matrices are shown in Figure 4 .

[0148] Figure 4 (a) shows the path planned by the D*Lite algorithm. It can be seen that the planned path without safety distance is very close to the obstacle, and the safety of the path is poor. Figure 4 (b), 4(c) are respectively the planned paths based on safety distance matrix dimension n = 3 and 5. It can be seen that there is a distance of 1m and 2m respectively between the planned path and the obstacle.

[0149] Through simulation experiments, it can be obtained that the planned path of the original D*Lite algorithm is the shortest, but since the safety distance between the path and the obstacle is not considered, when the environment map is inaccurate or the autonomous vehicle control has errors, the autonomous vehicle is prone to collision with the obstacle. By introducing the safety distance matrix S n×n into the node search process of the D*Lite algorithm and improving the node evaluation function of the D*Lite algorithm, the planned path can be away from the obstacle, ensuring the safety and feasibility of the path.

[0150] 2. Effect of introducing kinematic constraints

[0151] To verify the effect of the strategy of increasing search directions based on kinematic constraints on path planning, two simulation experiment maps are defined, which are 30m*30m and 50m*50m. The path is searched using 8 search neighborhood and 24 search neighborhood, and the path planning results on the two maps are shown in Figure 5 ,Figure 6 The specific performance parameters are shown in Table 1.

[0152] From Figure 5 (a) and Figure 6 As can be seen from (a), the path planned by the 8-neighborhood search method has many turning points, and the turning angle is a multiple of 45 degrees. Due to the limitation of the kinematic parameters of the autonomous vehicle, the turning angle at many turning points exceeds the maximum turning angle of the autonomous vehicle. Therefore, the 8-neighborhood search method is not suitable for the autonomous vehicle in actual engineering application.

[0153] As can be seen from Table 1, under the 30*30 map, the path planned by the 16-direction 24-neighborhood has fewer turning points than the 8-direction 8-neighborhood, and the path length is reduced by 5.32%, and the smoothness is reduced by 61.94%. Under the 50*50 map, the path planned by the 16-direction 24-neighborhood has a path length that is 0.78% lower than the 8-direction 8-neighborhood, and the smoothness is reduced by 69.11%. In summary, the turning angle of the path planned by the 24-neighborhood search method is a multiple of 22.5 degrees, and the smoothness is significantly reduced, which to some extent meets the kinematic constraints of the autonomous vehicle and solves the problem of limited steering performance of the autonomous vehicle.

[0154] Table 1 Path planning performance parameters of different search neighborhoods

[0155]

[0156] 3. Effect of path optimization

[0157] (1) Effect of removing redundant points

[0158] In order to verify the influence of removing redundant points on path planning, a simulation experiment was conducted on a 30m*30m map. Figure 7 The path planning results before and after removing redundant points are shown. It can be seen that after removing redundant points, the path turning points are reduced. As can be seen from the data in Table 2, the path length is shortened by 0.93%, and the smoothness is reduced by 43.93%. This can reduce the number of motor starts and stops of the autonomous vehicle, which is beneficial to prolong the service life of the autonomous vehicle.

[0159] Table 2 Path planning performance parameters before and after removing redundant points

[0160]

[0161] (2) Effect of path smoothing

[0162] In order to verify the influence of path smoothing on path planning, a simulation experiment was conducted on a 30m*30m map. Figure 8The planning effect before and after the Bezier curve smoothing path is shown. It can be seen that the path before the Bezier curve smoothing is connected by many line segments, and the autonomous vehicle will suddenly change speed at the connecting inflection point. The smoothed curve is derivable and meets the kinematic constraints of the autonomous vehicle, and the autonomous vehicle can move smoothly along the curve. According to the data in Table 3, the length of the smoothed path is shorter, the smoothing performance is better, and it is more conducive to the smooth control of the autonomous vehicle.

[0163] Table 3 Path planning performance parameters before and after path smoothing

[0164] Map size Before / after smoothing Path length / m Smoothing degree / rad 30*30 Before smoothing 39.50 1.34 After smoothing 39.37 1.32

[0165] 4. Comparative experimental results in static environment

[0166] The performance of the proposed ID*Lite algorithm is compared with A*, RRT and original D*Lite algorithm, and the planning effect is shown in Figure 9 . These planning algorithms are applied to the same static environment, and the specific performance parameters are shown in Table 4.

[0167] Table 4 Performance parameters of different algorithms in the same static environment

[0168] Method Path length / m Smoothing degree / rad A* 39.7696 1.5708 RRT 48.6187 18.1437 D*Lite 39.7696 3.1416 ID*Lite 39.3743 1.3240

[0169] As can be seen from Figure 9 , the A*, RRT and D*Lite algorithms have more turning points in the planned path, the path is not smooth, and the path is close to the obstacle, resulting in low path quality. This is because these algorithms do not consider the safety distance between the autonomous vehicle and the obstacle, and do not consider the kinematic constraints of the autonomous vehicle. Such a path will cause the autonomous vehicle to frequently turn, or even pause to complete the turn, which is not conducive to the tracking of the autonomous vehicle and seriously affects the working efficiency of the autonomous vehicle.

[0170] The RRT algorithm has randomness when searching for a path, and is tested 10 times under the same test conditions, and the average value of each evaluation index is calculated to fully verify the performance of the algorithm. According to the data in Table 4, the path planned by the RRT algorithm is the longest, because RRT randomly samples and does not optimize the planned path, which is usually far from the optimal path. ID*Lite considers the kinematic characteristics of the autonomous vehicle and optimizes the path length. In terms of path length and smoothness, ID*Lite algorithm is obviously superior to the other algorithms listed, and the specific performance is: compared with A*, RRT and D*Lite algorithms, the path length is reduced by 0.99%, 19.01%, 0.99% respectively, and the smoothness is reduced by 15.71%, 92.70%, 57.86% respectively, which meets the kinematic constraints of the autonomous vehicle and is more conducive to subsequent tracking control.

[0171] 5. Comparative experimental results under dynamic environments

[0172] To further verify the algorithm's performance in dynamic environments, simulation experiments were conducted in an environment where random obstacles were added to the autonomous vehicle's path, and the results were compared with the original D*Lite algorithm. The replanning results of different algorithms are shown below. Figure 10 As shown,

[0173] The specific performance parameters for the smoothness of movement of autonomous vehicles are shown in Table 5. A black asterisk (*) represents the starting point, and a black circle represents the target point. Solid lines represent the original path, and dashed lines represent the replanned path after the autonomous vehicle suddenly encounters random dynamic obstacles (small black squares). Figure 10 (a) It can be seen that the original D*Lite algorithm replans paths that are too close to obstacles, resulting in low safety, and the turning angle at the turning point is 45 degrees, which is not conducive to the steering of autonomous vehicles. From Figure 10 As shown in the planning results of (b), the ID*Lite algorithm's replanning can yield a safe and smooth path. Table 5 shows that the smoothness of the path obtained by the ID*Lite algorithm is 4.19. rad Compared to the original D*Lite algorithm, the smoothness was reduced by 33.28%, significantly improving the smoothness and continuity of autonomous vehicle motion.

[0174] Table 5 shows the performance parameters of the D*Lite and ID*Lite algorithms in a dynamic environment with random obstacles.

[0175] Method Smoothing degree / rad D*Lite 6.28 ID*Lite 4.19

[0176] Example 4:

[0177] An autonomous vehicle path planning method that integrates motion constraints and safety constraints includes the following steps:

[0178] 1) Load the node s containing the current location of the autonomous vehicle. start And the grid map of the target node; let the starting position s of the previous time step be... last =s start ;

[0179] 2) Initialize path planning parameters;

[0180] 3) Establish an evaluation function that incorporates safety distance and motion constraints;

[0181] 4) Calculate the shortest path for the autonomous vehicle using the evaluation function; the shortest path includes multiple path nodes;

[0182] 5) Filter the path nodes in the shortest path, remove redundant path nodes, and obtain the path optimization nodes;

[0183] 6) Smooth the path optimization node by using the third order Bezier curve, and generate the automatic driving vehicle planning path;

[0184] 7) Control the automatic driving vehicle to travel according to the planning path, and the automatic driving vehicle passes through each node, then judge whether the obstacle changes, if the obstacle changes, update the actual moving distance k of the vehicle m Value and the current position s start , and set the current position as s last , update the value of the affected node, and then return to step 4).

[0185] Example 5:

[0186] The automatic driving vehicle path planning method fusing motion constraint and safety constraint, the technical content is the same as that of example 4, further, the step of initializing the path planning parameter includes:

[0187] Set the path node priority queue U as an empty queue, and set the actual moving distance k of the automatic driving vehicle as 0; m

[0188] Set the g value and the rhs value of all nodes of the grid map as infinite; the g value represents the distance from the node to the target node; the rhs value represents the minimum value of the distance from the current node s to the target node and the safety distance obtained by the successor node of s, that is, the cost function;

[0189] Set the rhs value of the target node as 0, calculate the evaluation function key value of the target point, and write the target node into the priority queue U.

[0190] Example 6:

[0191] The automatic driving vehicle path planning method fusing motion constraint and safety constraint, the technical content is the same as that of any one of examples 4-5, further, the step of establishing the evaluation function introducing the safety distance and the motion constraint includes:

[0192] 3.1) Establish the safety distance matrix S of the current position node n×n , that is:

[0193]

[0194] In the formula, n*n is the dimension of the safety distance matrix; n is an odd number and n≥3; in the matrix S n×n , "1" represents the sub node that the automatic driving vehicle may reach next, "3" represents the node to be detected; "0" represents the node that does not need to be searched;

[0195] 3.2) Based on the safety distance matrix S n×n , establish the cost function introducing the safety distance, that is:​

[0196]

[0197] where Succ(s) represents all successors of the current node s, c(s,s') is the cost value from the current node s to the successor s', g(s') represents the distance from the successor s' to the target node s goal ; D S is the increment; when there is no obstacle in the safety distance matrix S n×n to be detected, D S = 0, otherwise, D S ≠ 0.

[0198] 3.3) Establish the evaluation function key introducing the safety distance and the motion constraint, that is:

[0199]

[0200] where k1, k2 are intermediate parameters; the parameter k m = k m '+ h(s last ,s start ); k m 'represents the actual moving distance of the vehicle at the last time, s last represents the last starting point, s start represents the current position of the autonomous vehicle, h(s last ,s start ) represents the distance between s last and s start . g(s) represents the distance from the current node s to the target node s goal ; h(s,s start ) represents the distance between s and s start .

[0201] Embodiment 7:

[0202] The autonomous vehicle path planning method fusing the motion constraint and the safety constraint, the technical content is the same as any one of embodiments 4-6, further, the step of calculating the shortest path of the autonomous vehicle by using the evaluation function comprises:

[0203] 4.1) Take out the node s with the minimum key value from the path node priority queue U; expand the node s to obtain a plurality of child nodes; update the g value, the rhs value and the key value of the child nodes, and add the child nodes to the priority queue U or update the corresponding values of the child nodes in the priority queue U;

[0204] 4.2) Judge key(s) < key(s start ) or rhs(s start ) ≠ g(sstart ) is true, then repeat step 4.1), if not, then from the current position node s start of the autonomous vehicle, traverse all parent nodes until the target node s goal is found, thereby generating the shortest path.

[0205] Embodiment 8:

[0206] The method for autonomous vehicle path planning integrating motion constraints and safety constraints, the technical content of which is the same as any one of embodiments 4-7, further, the step of screening the path nodes in the shortest path comprises:

[0207] 5.1) Starting from the i-th node, traverse all nodes in turn, if the current node can be connected to the i-th node without obstacles, then eliminate the nodes between the first node and the current node; i is initially 1;

[0208] 5.2) Let i = i + 1, and return to step 5.1) until the target node is connected.

[0209] Embodiment 9:

[0210] The method for autonomous vehicle path planning integrating motion constraints and safety constraints, the technical content of which is the same as any one of embodiments 4-8, further, the third-order Bezier curve parameter P(t) is as follows:

[0211] P(t) = (1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3 (4)

[0212] Wherein, P0, P1, P2, P3 represent the control points of the third-order Bezier curve. Real number t ∈ [0, 1].

[0213] Embodiment 10:

[0214] The method for autonomous vehicle path planning integrating motion constraints and safety constraints, the technical content of which is the same as any one of embodiments 4-9, further, the angle β between the center axis of the autonomous vehicle and the driving direction of the front wheel satisfies the following constraint:

[0215] |β|≤β max (5)

[0216] Wherein, β represents the angle between the center axis of the autonomous vehicle and the driving direction of the front wheel; β max is the upper limit of the angle between the center axis of the autonomous vehicle and the driving direction of the front wheel.

[0217] The path curvature κ of the autonomous vehicle motion satisfies the following constraint:

[0218]

[0219] in the formula, R min is the minimum turning radius of the vehicle; β max is the upper limit of the included angle between the center axis of the autonomous vehicle and the driving direction of the front wheels, and l represents the wheelbase.

[0220] Embodiment 11:

[0221] A computer readable storage medium having stored thereon a computer program;

[0222] The computer program, when invoked, performs the steps of the method for planning a path for an autonomous vehicle that fuses motion constraints and safety constraints according to any one of embodiments 4-10.

Claims

1. A path planning method for autonomous vehicles that integrates motion constraints and safety constraints, characterized in that, Includes the following steps: 1) Load the node containing the current location of the autonomous vehicle. start And the grid map of the target node; Let the starting position s of the previous time step be... last =s start ; 2) Initialize path planning parameters; 3) Establish an evaluation function that incorporates safety distance and motion constraints; 4) Calculate the shortest path for the autonomous vehicle using the evaluation function; the shortest path includes multiple path nodes; 5) Filter the path nodes in the shortest path, remove redundant path nodes, and obtain the path optimization nodes; 6) Use third-order Bézier curves to smooth the path optimization nodes and generate the planned path for autonomous vehicles; 7) Control the autonomous vehicle to travel along the planned path. Each time the autonomous vehicle passes a node, determine if the obstacle has changed. If it has changed, update the vehicle's actual travel distance k. m Value and current position s start And set the current position to s last Update the value of the affected node, and then return to step 4); The steps to establish an evaluation function that incorporates safety distance and motion constraints include: 3.1) Establish the safe distance matrix S of the current location node. n×n ,Right now: In the formula, n×n is the dimension of the safety distance matrix; n is an odd number and n≥3; matrix S n×n In the diagram, "1" represents a child node that the autonomous vehicle may reach next, "3" represents a node to be detected, and "0" represents a node that does not need to be searched. 3.2) Based on the safety distance matrix S n×n Establish the cost function that introduces a safe distance, namely: In the formula, Succ(s) represents all successor nodes of the current node s, c(s,s') is the cost from the current node s to the successor node s', and g(s') represents the cost from the successor node s' to the target node s. goal Distance; D S This is an additional term; when the safety distance matrix S n×n If there are no obstacles at the node to be detected, then D S =0, otherwise, D S ≠0; 3.3) Establish an evaluation function key that incorporates safety distance and motion constraints, namely: In the formula, k1 and k2 are intermediate parameters; parameter k m =k m '+h(s last ,s start );k m 'Represents the actual distance the vehicle traveled at the previous moment, s last s represents the previous starting point. start h(s) represents the current position of the autonomous vehicle. last ,s start ) represents s last and s start The distance between them; g(s) represents the distance from the current node s to the target node s. goal The distance; h(s,s) start ) represents s and s start The distance between them.

2. The autonomous vehicle path planning method integrating motion constraints and safety constraints according to claim 1, characterized in that, The steps for initializing path planning parameters include: Set the path node priority queue U to an empty queue, and let the autonomous vehicle travel the actual distance k. m Set to 0; Set the g and rhs values ​​of all nodes in the raster map to infinity; the g value represents the distance from the node to the target node; the rhs value represents the cost function. Set the RHS value of the target node to 0, calculate the evaluation function key value of the target node, and write the target node into the priority queue U.

3. The autonomous vehicle path planning method integrating motion constraints and safety constraints according to claim 1, characterized in that, The steps for calculating the shortest path for an autonomous vehicle using the evaluation function include: 4.1) Take the node s with the smallest key value from the priority queue U of the path nodes; expand node s to obtain several child nodes; update the g value, rhs value, and key value of the child nodes, and add the child nodes to the priority queue U or update the corresponding values ​​of the child nodes in the priority queue U. 4.2) Determine key(s) <key(s start ) or rhs(s start )≠g(s start If the condition is met, repeat step 4.1). If the condition is not met, start from the current location node s of the autonomous vehicle. start Start by traversing all parent nodes until the target node s is found. goal This generates the shortest path.

4. The autonomous vehicle path planning method integrating motion constraints and safety constraints according to claim 1, characterized in that, The steps for filtering path nodes in the shortest path include: 5.1) Starting from the i-th node, traverse all nodes in sequence. If the current node can be connected to the i-th node without obstacles, then remove the node between the first node and the current node; i is initially set to 1. 5.2) Let i = i + 1, and return to step 5.1) until the target point is connected.

5. The autonomous vehicle path planning method integrating motion constraints and safety constraints according to claim 1, characterized in that, The parameters P(t) of the third-order Bézier curve are shown below: P(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3,t∈[0,1] (4) In the formula, P0, P1, P2, and P3 represent the control points of the third-order Bézier curve.

6. The autonomous vehicle path planning method integrating motion constraints and safety constraints according to claim 1, characterized in that, The angle β between the central axis of the autonomous vehicle and the direction of travel of the front wheels satisfies the following constraint: |β|≤β max (5) In the formula, β represents the angle between the central axis of the autonomous vehicle and the direction of travel of the front wheels; β max This represents the upper limit of the angle between the centerline of an autonomous vehicle and the direction of travel of the front wheels; The path curvature κ of the autonomous vehicle's motion satisfies the following constraint: In the formula, R min β is the minimum turning radius of the vehicle. max The angle between the centerline of the autonomous vehicle and the direction of travel of the front wheels is the upper limit, where l represents the wheelbase.

7. A computer-readable storage medium, characterized in that, It contains computer programs; When the computer program is invoked, it executes the steps of the autonomous vehicle path planning method that integrates motion constraints and safety constraints as described in any one of claims 1 to 6.

Images