Automatic driving vehicle out-of-trap method combining mixed A* and spline curve algorithm

Abstract

The invention provides an automatic driving vehicle escape method combining a mixed A * and spline curve algorithm. The method comprises the following steps that whether a safe passing space exists or not is judged according to a passing criterion; if yes, obtaining a target node coordinate to obtain a de-trapping path by adopting a mixed A * algorithm; the automatic driving vehicle executes an out-of-trap path and obtains second vehicle information, an optimal path node set and an optimal path are obtained based on the second vehicle information by adopting a dynamic programming algorithm, and a target function of a cubic spline curve is constructed based on QP; solving the objective function to obtain a smooth path; obtaining a node curvature according to the smooth path so as to judge whether the automatic driving vehicle is out of order according to an out-of-order criterion; and if so, executing the smooth path as the driving path by the autonomous vehicle. According to the self-driving vehicle escape method combining the mixed A * and spline curve algorithm, the problem of escape of the self-driving vehicle is effectively solved, the trapped vehicle is prevented from obstructing traffic, and the operation efficiency of the unmanned vehicle is greatly improved.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent networked vehicles, and in particular to a method for automatically driving a vehicle out of trouble by combining a hybrid A* and spline curve algorithm. Background Art

[0002] Path planning is an important research area in autonomous driving. The planning module needs to plan a safe and stable path for the vehicle based on the received perception and map information.

[0003] The most common autonomous driving planning algorithm in driving is the lateral and longitudinal decoupled planning algorithm. Since the goal of the autonomous driving vehicle path planning problem is to obtain a trajectory that includes the vehicle path and the corresponding vehicle motion state, such as the vehicle heading angle, speed, etc., it needs to be solved in the SLT three-dimensional space. Directly solving in three-dimensional space requires high computational efficiency and it is difficult to ensure real-time performance. Therefore, a common method is to use the Frenet coordinate system to decouple the lateral and longitudinal parts of the path planning and solve them, thereby reducing the complexity of the path planning problem.

[0004] The hybrid A* algorithm is a path planning algorithm that combines the traditional A* algorithm and vehicle kinematics. It is suitable for path planning of robots and driverless cars in unstructured scenes. It can ensure that the generated path meets the dynamic constraints by balancing the discrete state space and the continuous state space. However, in the process of autonomous driving in large spaces and structured roads, its search complexity is high and it is difficult to ensure stable decision-making. Therefore, it is mostly used in closed scenes such as parking and parks.

[0005] However, during autonomous driving, if an adjacent vehicle suddenly cuts in or a sensor misses detection, the algorithm based on lateral and longitudinal decoupling will often fail to solve the problem, causing the vehicle to be trapped. Even if a path can be generated, it is difficult to ensure that the vehicle's kinematic constraints are met. Summary of the invention

[0006] In view of the problems existing in the prior art, the present invention provides an autonomous driving vehicle escape method combining a hybrid A* and spline curve algorithm, which effectively solves the problem of autonomous driving vehicles being able to escape from trapped states and avoids trapped vehicles obstructing traffic.

[0007] To achieve the above object, the technical solution adopted by the present invention is as follows: a method for autonomous driving vehicle escape from trouble combining hybrid A* and spline curve algorithm, comprising the following steps:

[0008] Determine whether the autonomous driving vehicle has a safe passage space based on the trapped state; if so, obtain the target node coordinates based on the safe passage space;

[0009] A hybrid A* algorithm is used to obtain an escape path according to the trapped state and the target node coordinates;

[0010] The autonomous driving vehicle executes the escape path, and in the process of executing the escape path, a spline curve algorithm is used to obtain a smooth path;

[0011] Obtaining the node curvature of the smooth path;

[0012] Determining whether the autonomous driving vehicle is out of trouble based on the node curvature;

[0013] If out of trouble, a driving path is obtained according to the smooth path;

[0014] The autonomous driving vehicle executes the driving path.

[0015] Furthermore, the step of determining whether there is the safe passage space is as follows:

[0016] Acquire lane information, obstacle information, and first vehicle information, where the first vehicle information is vehicle information of a trapped state of the autonomous driving vehicle;

[0017] Acquire a body width of the autonomous driving vehicle according to the first vehicle information;

[0018] Obtaining a safe passing width according to the vehicle body width;

[0019] The obstacle information includes obtaining the horizontal coordinate of the left side of the obstacle border and the horizontal coordinate of the right side of the obstacle border along the driving direction of the autonomous driving vehicle;

[0020] The lane information includes the horizontal coordinate of the left lane line and the horizontal coordinate of the right lane line;

[0021] Obtaining the left width according to the left horizontal coordinate of the obstacle frame and the left horizontal coordinate of the lane line;

[0022] Obtaining the right width according to the right horizontal coordinate of the obstacle frame and the horizontal coordinate of the right lane line;

[0023] Comparing the safe passage width with the left width, or comparing the safe passage width with the right width;

[0024] If the left width is greater than the safe passage width, or the right width is greater than the safe passage width, then there is the safe passage space.

[0025] Furthermore, if the left side width is greater than the safe passage width, the left side of the obstacle is the safe passage side;

[0026] If the right side width is greater than the safe passage width, the right side of the obstacle is the safe passage side.

[0027] Further, when the left width and the right width are both greater than the safe passage width, the left width is compared with the right width;

[0028] If the left side width is greater than the right side width, the left side of the obstacle is a safe passage side;

[0029] If the left side width is not greater than the right side width, the right side of the obstacle is a safe passage side.

[0030] Furthermore, the method for obtaining the target node coordinates is:

[0031] When the left side of the obstacle is the safe passage side, the target node's horizontal coordinate is half of the sum of the horizontal coordinate of the left side of the obstacle frame and the horizontal coordinate of the left lane line;

[0032] When the right side of the obstacle is the safe passage side, the target node's horizontal coordinate is half of the sum of the horizontal coordinate of the right side of the obstacle frame and the horizontal coordinate of the right lane line;

[0033] Obtaining a set value; obtaining a longitudinal coordinate of the front side of the obstacle border along the driving direction of the autonomous driving vehicle;

[0034] The target node ordinate is the sum of the ordinate of the front side of the obstacle frame and the set value.

[0035] Furthermore, the hybrid A* algorithm is:

[0036]

[0037] Where n is the trapped state, f1(n) is the total cost from the initial state to the trapped state, g1(n) is the cost from the initial state to the trapped state, h1(n) is the heuristic cost from the trapped state to the target state, distance(n) is the distance cost of the trapped state, steer(n) is the steering cost of the trapped state, direction(n) is the forward and backward direction switching cost of the trapped state, and turn_round(n) is the left and right direction switching cost of the trapped state.

[0038] Further, the front-to-back direction switching cost of the trapped state is 0, and the left-to-right direction switching cost of the trapped state is greater than an initial value of the left-to-right direction switching cost.

[0039] Furthermore, the steps of using the spline curve algorithm to obtain the smooth path are:

[0040] The autonomous driving vehicle executes the escape path and obtains second vehicle information, lane information, and obstacle information, wherein the second vehicle information is real-time vehicle information during the process of the autonomous driving vehicle executing the escape path;

[0041] Using a dynamic programming algorithm, obtaining an optimal path node set and an optimal path according to the second vehicle information, the lane information and the obstacle information;

[0042] According to the optimal path node set, the optimal path and the second vehicle information, an objective function of a cubic spline curve is constructed based on QP; the constraints of the objective function include boundary constraints and dynamic feasibility constraints;

[0043] Solve the objective function to obtain a smooth path; the objective function is:

[0044]

[0045] In the formula, Cs(f2) is the objective function, f2(s) is the smooth path, s is the independent variable value at the optimal path node, ω1 is the first weight, ω2 is the second weight, ω3 is the third weight, and ω4 is the fourth weight. is the orientation of the optimal path node, is the node curvature at the optimal path node, is the derivative of the node curvature at the optimal path node, and g2(s) is the optimal path.

[0046] Furthermore, the method for judging whether the autonomous driving vehicle is out of trouble based on the node curvature is as follows: the node curvature at each optimal path node is smaller than the maximum curvature of the autonomous driving vehicle.

[0047] A method for getting out of trouble for an autonomous driving vehicle combining a hybrid A* and a spline curve algorithm, comprising a preparation phase, an escape phase and an exit phase; the autonomous driving vehicle comprises a driving planning module and an escape module, the driving planning module comprises a route planning unit and a decision unit;

[0048] When the lateral planning algorithm of the route planning unit fails, the preparation phase is entered; the preparation phase includes the following steps:

[0049] The decision-making unit:

[0050] Based on the trapped state, it is determined whether the autonomous driving vehicle has a safe passage space; if so, the escape phase is entered; the escape phase includes the following steps:

[0051] The escape module:

[0052] Obtaining target node coordinates based on the safe passage space;

[0053] A hybrid A* algorithm is used to obtain an escape path according to the trapped state and the target node coordinates;

[0054] The autonomous driving vehicle executes the escape path;

[0055] The driving planning module:

[0056] The autonomous driving vehicle executes the escape path, and in the process of executing the escape path, a spline curve algorithm is used to obtain a smooth path;

[0057] Obtaining the node curvature of the smooth path;

[0058] Determining whether the autonomous driving vehicle is out of trouble based on the node curvature;

[0059] If out of trouble, the exit phase will be entered; the exit phase includes the following steps:

[0060] Obtaining a driving path according to the smooth path;

[0061] The autonomous driving vehicle executes the driving path.

[0062] Compared with the prior art, the present invention has the following beneficial effects:

[0063] The present invention effectively solves the problem of autonomous driving vehicles getting out of trouble by calling the hybrid A* algorithm in stages to generate an escape path when the autonomous driving vehicle is trapped, and by combining the dynamic programming algorithm with the QP driving lateral algorithm to determine whether the escape is successful and generate a driving path, thereby avoiding trapped vehicles from obstructing traffic and greatly improving the operating efficiency of unmanned autonomous driving vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 It is a flow chart of the method for getting out of trouble of an autonomous driving vehicle combining a hybrid A* and a spline curve algorithm of the present invention;

[0065] Figure 2 A schematic diagram of a trapped state of an autonomous driving vehicle in an embodiment of the present invention;

[0066] Figure 3 Schematic diagram of the search process of Hybrid A* in an embodiment of the present invention;

[0067] Figure 4 A schematic diagram of obtaining an optimal path node set and an optimal path using a dynamic programming algorithm in an embodiment of the present invention;

[0068] Figure 5 A schematic diagram of the curvature between two points. DETAILED DESCRIPTION

[0069] In order to clearly illustrate the technical features of this solution, this solution is described below through a specific implementation method.

[0070] When an autonomous vehicle is driving close to a static obstacle, the lateral algorithm of the vehicle will fail to plan or generate a trajectory that the vehicle cannot execute, causing the vehicle to be trapped. At the current stage, if an autonomous vehicle is trapped by an obstacle in front of it, it will stop in the lane and wait, blocking traffic and affecting the efficiency of the unmanned vehicle operation.

[0071] See also Figure 1 The embodiment of the present invention provides a method for autonomous driving vehicle escape from distress by combining a hybrid A* and a spline curve algorithm, comprising the following steps:

[0072] Acquire lane information, obstacle information, and first vehicle information, where the first vehicle information is vehicle information of a trapped state of the autonomous driving vehicle;

[0073] Based on the first vehicle information, lane information and obstacle information, determine whether there is safe passage space according to the passage criterion; in frenet coordinates, see Figure 2 In the figure, the box marked with abc is the autonomous driving vehicle. The circular object in front of the autonomous driving vehicle along the driving direction is a static obstacle. The steps to determine whether there is a safe passage space are:

[0074] Get the body width of the autonomous driving vehicle;

[0075] Get safe passage width according to vehicle width _pass ,width _pass =width+buffer, where buffer is the remaining width, usually 0.5m, to ensure the safe passage of the autonomous vehicle;

[0076] Get the maximum horizontal coordinate of the left side of the obstacle border along the driving direction of the autonomous vehicle _l and the horizontal coordinate min on the right side of the obstacle border _l ; Get the horizontal coordinate lanemax of the left lane line _bound_l and the horizontal coordinate lanemin of the right lane line _bound_l ;

[0077] According to the max horizontal coordinate on the left side of the obstacle border _l and the horizontal coordinate lanemax of the left lane line _bound_l Get the left width _leftspac ; e Right now:

[0078] width _leftspace =lanemax _bound_l -max_l ;

[0079] According to the horizontal coordinate min on the right side of the obstacle border _l and the horizontal coordinate lanemin of the right lane line _bound_l Get the right width _rightspace ;Right now:

[0080] width _rightspace =min _l -lanemin _bound_l ;

[0081] Compare the safe passage width to the left width, or compare the safe passage width to the right width;

[0082] If the left width is greater than the safe passing width, or the right width is greater than the safe passing width, there is safe passing space, that is, the passing criterion is:

[0083] width _rightspace >width _pass ∨width _leftspace >width _pass ;

[0084] If there is safe space to pass, then:

[0085] The target node coordinates are obtained according to the lane information and obstacle information; the target node coordinates include the target node horizontal coordinate and the target node vertical coordinate. The method for obtaining the target node coordinates is:

[0086] When the left side of the obstacle is the safe passage side, the horizontal coordinate of the target node is half of the sum of the horizontal coordinates of the left side of the obstacle frame and the horizontal coordinates of the left lane line;

[0087] When the right side of the obstacle is the safe passage side, the horizontal coordinate of the target node is half of the sum of the horizontal coordinate of the right side of the obstacle frame and the horizontal coordinate of the right lane line;

[0088] Get the set value; along the driving direction of the autonomous driving vehicle, get the vertical coordinate of the front side of the obstacle border;

[0089] The vertical coordinate of the target node is the sum of the vertical coordinate of the front side of the obstacle frame and the set value;

[0090] The horizontal coordinate of the target node is:

[0091] target _l =(obs _l +lane _bound_l ) / 2;

[0092] In the formula, target _l is the horizontal coordinate of the target node, obs _lLane is the horizontal coordinate of the obstacle frame on the same side as the safe passage side, _bound_l is the horizontal coordinate of the lane line on the same side as the safe passage side;

[0093] If the left side width is greater than the safe passage width, the left side of the obstacle is the safe passage side;

[0094] If the right side width is greater than the safe passage width, the right side of the obstacle is the safe passage side;

[0095] When both the left and right widths are greater than the safe passage width, compare the left and right widths;

[0096] If the width on the left is greater than the width on the right, the left side of the obstacle is the safe passage side;

[0097] If the left width is not greater than the right width, the right side of the obstacle is the safe passage side;

[0098] The vertical coordinate of the target node is:

[0099] target _s =obs _max_s +s set ;

[0100] In the formula, target _s is the ordinate of the target node, obs _max_s is the front ordinate of the obstacle frame along the driving direction of the autonomous vehicle, s set is the set value.

[0101] A hybrid A* algorithm is used to obtain an escape path according to the first vehicle information and the target node coordinates; the hybrid A* algorithm is:

[0102]

[0103] Where n is the trapped state, f1(n) is the total cost from the initial state to the trapped state, g1(n) is the cost from the initial state to the trapped state, h1(n) is the heuristic cost from the trapped state to the target state, distance(n) is the distance cost of the trapped state, steer(n) is the steering cost of the trapped state, direction(n) is the forward and backward direction switching cost of the trapped state, turn_round(n) is the left and right direction switching cost of the trapped state;

[0104] After determining the coordinates of the target node, the hybrid A* algorithm is used to search for a path from the current position of the trapped state to the target node that satisfies the kinematic constraints. The difference between the Hybrid A* algorithm and the ordinary A* search algorithm is that the Hybrid A* algorithm generates child nodes based on the vehicle kinematics to ensure that the generated path can satisfy the vehicle dynamics constraints. This introduces the state quantity vehicle posture θ, which is converted into a search problem in three-dimensional space (x, y, θ). The search process of Hybrid A* is as follows: Figure 3 As shown, the boxes marked abc in the figure are autonomous driving vehicles, and F, FL, FR, B, BL and BR represent the forward direction, the left turn limit in the forward direction, the right turn limit in the forward direction, the backward direction, the left turn limit in the backward direction and the right turn limit in the backward direction, respectively.

[0105] In order to generate a smoother reversing path, the forward and backward switching cost of the trapped state is 0, and the left and right switching cost of the trapped state is greater than the initial value of the left and right switching cost, that is, the left and right switching cost of the trapped state is increased;

[0106] The autonomous driving vehicle executes the escape path and obtains second vehicle information, where the second vehicle information is real-time vehicle information during the autonomous driving vehicle's execution of the escape path, and is dynamic information;

[0107] Using a dynamic programming (DP) algorithm, the optimal path node set and the optimal path are obtained according to the second vehicle information, lane information and obstacle information;

[0108] See also Figure 4 First, slice and scatter the lanes in the Frenet coordinates, turn the trajectory problem into a piecewise optimal problem, and use the dynamic programming (DP) algorithm to solve the optimal substructure. The main idea of ​​the DP algorithm is to use the current position of the vehicle as the starting point and calculate the cost for each sampling point. The cost includes the distance from the road boundary, the distance from the obstacle, the offset relative to the reference line, etc. After generating such a sampling node graph, the total cost of each sampling point is the sum of the costs from the starting point to the current point. In this way, using the DP algorithm, starting from the last row, continuously select the points with the smallest total cost value in the last row, record them as the optimal previous node of the point, and reversely find all the previous nodes in sequence according to the recorded previous node number until the starting point of the vehicle, that is, the optimal path node set is obtained. All optimal path node sequences represent the optimal path obtained by the DP algorithm according to the cost;

[0109] According to the optimal path node set, the optimal path and the second vehicle information, the objective function of the cubic spline curve is constructed based on QP (Quadratic Programming). The constraints of the objective function include boundary constraints and dynamic feasibility constraints. The boundary constraints and dynamic feasibility constraints are imposed on each optimal path node to restrict the autonomous driving vehicle within the lane by limiting the horizontal coordinate. The objective function is:

[0110]

[0111] In the formula, Cs(f2) is the objective function, f2(s) is the smooth path, s is the independent variable value at the optimal path node, ω1 is the first weight, ω2 is the second weight, ω3 is the third weight, and ω4 is the fourth weight. is the orientation at the optimal path node, is the first-order derivative of f2(s), is the node curvature at the optimal path node, is the second-order derivative of f2(s), is the derivative of the node curvature at the optimal path node, is the third-order derivative of f2(s), and g2(s) is the optimal path;

[0112] The second-order derivative of f2(s) and the third-order derivative of f2(s) are related to the dynamic feasibility constraints. Since all constraints are linear, the objective function can be solved very quickly using the QP solver;

[0113] Solve the objective function to obtain a smooth path;

[0114] Get the node curvature based on the smoothed path; see Figure 5 , the node curvature is obtained according to the curvature formula between two points on the curve. The node curvature is:

[0115]

[0116] Where k is the node curvature at x, x is the optimal path node, lim is the minimization function, α is the first tangent angle, the first tangent is the tangent of the smooth path at x, Δα is the angle between the first tangent and the second tangent, the second tangent is the tangent of the smooth path at x', x' is the point on the smooth path close to the optimal path node, Δs is the arc length between x and x';

[0117] Based on the node curvature, whether the autonomous driving vehicle is out of trouble is judged according to the escape criterion; the escape criterion is: the node curvature at each optimal path node is less than the maximum curvature of the autonomous driving vehicle.

[0118] If out of trouble, the driving path is obtained according to the smooth path;

[0119] If the vehicle is not out of trouble, re-acquire the second vehicle information until it is out of trouble;

[0120] The autonomous vehicle executes the driving path.

[0121] A method for getting out of trouble for an autonomous driving vehicle combining a hybrid A* and a spline curve algorithm, comprising a preparation phase, an escape phase and an exit phase; the autonomous driving vehicle comprises a driving planning module and an escape module, the driving planning module comprises a route planning unit and a decision unit;

[0122] When the lateral planning algorithm of the route planning unit fails, it enters the preparation phase; the preparation phase includes the following steps:

[0123] Decision making unit:

[0124] Acquire lane information, obstacle information, and first vehicle information, where the first vehicle information is vehicle information of a trapped state of the autonomous driving vehicle;

[0125] Based on the first vehicle information, lane information and obstacle information, judging whether there is safe passage space according to a passage criterion;

[0126] If there is safe passage, the escape phase will begin. The escape phase includes the following steps:

[0127] Escape module:

[0128] Obtain the target node coordinates based on lane information and obstacle information;

[0129] A hybrid A* algorithm is used to obtain an escape path based on the first vehicle information and the target node coordinates;

[0130] The autonomous vehicle executes an escape path;

[0131] Driving planning module:

[0132] Obtaining second vehicle information, where the second vehicle information is real-time vehicle information during the process of the autonomous driving vehicle executing an escape path;

[0133] Using a dynamic programming algorithm, an optimal path node set and an optimal path are obtained according to the second vehicle information, lane information and obstacle information;

[0134] According to the optimal path node set, the optimal path and the second vehicle information, the objective function of the cubic spline curve is constructed based on QP; the constraints of the objective function include boundary constraints and dynamic feasibility constraints;

[0135] Solve the objective function to obtain a smooth path;

[0136] Obtain node curvature based on the smoothed path;

[0137] Based on the node curvature, it is determined whether the autonomous driving vehicle is out of trouble according to the escape criterion;

[0138] If the situation is resolved, the exit phase will begin. The exit phase includes the following steps:

[0139] Obtaining a driving path according to the smooth path;

[0140] The autonomous vehicle executes the driving path.

[0141] Finally, it should be noted that the above content is only used to illustrate the technical solution of the present invention, rather than to limit the scope of protection of the present invention. Simple modifications or equivalent substitutions of the technical solution of the present invention by ordinary technicians in this field do not deviate from the essence and scope of the technical solution of the present invention.

Claims

1. A method for autonomous driving vehicle escape from distress by combining hybrid A* and spline curve algorithms, characterized in that: The following steps are involved: Determine whether the autonomous vehicle has safe passage space based on the trapped state; If yes, obtaining the target node coordinates based on the safe passage space; A hybrid A* algorithm is used to obtain an escape path according to the trapped state and the target node coordinates; The autonomous driving vehicle executes the escape path, and in the process of executing the escape path, a spline curve algorithm is used to obtain a smooth path; Obtaining the node curvature of the smooth path; Determining whether the autonomous driving vehicle is out of trouble based on the node curvature; If out of trouble, a driving path is obtained according to the smooth path; The autonomous driving vehicle executes the driving path.

2. The method for automatically driving a vehicle out of trouble by combining a hybrid A* and spline curve algorithm according to claim 1, characterized in that: The steps for determining whether there is the safe passage space are: Acquire lane information, obstacle information, and first vehicle information, where the first vehicle information is vehicle information of a trapped state of the autonomous driving vehicle; Acquire a body width of the autonomous driving vehicle according to the first vehicle information; Obtaining a safe passing width according to the vehicle body width; The obstacle information includes obtaining the horizontal coordinate of the left side of the obstacle border and the horizontal coordinate of the right side of the obstacle border along the driving direction of the autonomous driving vehicle; The lane information includes the horizontal coordinate of the left lane line and the horizontal coordinate of the right lane line; Obtaining the left width according to the left horizontal coordinate of the obstacle frame and the left horizontal coordinate of the lane line; Obtaining the right width according to the right horizontal coordinate of the obstacle frame and the horizontal coordinate of the right lane line; Comparing the safe passage width with the left width, or comparing the safe passage width with the right width; If the left width is greater than the safe passage width, or the right width is greater than the safe passage width, then there is the safe passage space.

3. The method for automatically driving a vehicle out of trouble by combining a hybrid A* and spline curve algorithm according to claim 2, characterized in that: If the left side width is greater than the safe passage width, the left side of the obstacle is the safe passage side; If the right side width is greater than the safe passage width, the right side of the obstacle is the safe passage side.

4. The method for automatically driving a vehicle out of trouble by combining a hybrid A* and spline curve algorithm according to claim 3, characterized in that: When the left width and the right width are both greater than the safe passage width, comparing the left width with the right width; If the left side width is greater than the right side width, the left side of the obstacle is a safe passage side; If the left side width is not greater than the right side width, the right side of the obstacle is a safe passage side.

5. The method for automatically driving a vehicle out of trouble by combining a hybrid A* and spline curve algorithm according to claim 3 or 4, characterized in that: The method for obtaining the target node coordinates is: When the left side of the obstacle is the safe passage side, the target node's horizontal coordinate is half of the sum of the horizontal coordinate of the left side of the obstacle frame and the horizontal coordinate of the left lane line; When the right side of the obstacle is the safe passage side, the target node's horizontal coordinate is half of the sum of the horizontal coordinate of the right side of the obstacle frame and the horizontal coordinate of the right lane line; Obtaining a set value; obtaining a longitudinal coordinate of the front side of the obstacle border along the driving direction of the autonomous driving vehicle; The target node ordinate is the sum of the ordinate of the front side of the obstacle frame and the set value.

6. The method for automatically driving a vehicle out of trouble by combining a hybrid A* and spline curve algorithm according to claim 1, characterized in that: The hybrid A* algorithm is: Where n is the trapped state, f1(n) is the total cost from the initial state to the trapped state, g1(n) is the cost from the initial state to the trapped state, h1(n) is the heuristic cost from the trapped state to the target state, distance(n) is the distance cost of the trapped state, steer(n) is the steering cost of the trapped state, direction(n) is the forward and backward direction switching cost of the trapped state, and turn_round(n) is the left and right direction switching cost of the trapped state.

7. The method for automatically driving a vehicle out of trouble by combining a hybrid A* and spline curve algorithm according to claim 6, characterized in that: The front-to-back direction switching cost of the trapped state is 0, and the left-to-right direction switching cost of the trapped state is greater than an initial value of the left-to-right direction switching cost.

8. The method for automatically driving a vehicle out of trouble by combining a hybrid A* and spline curve algorithm according to claim 1, characterized in that: The steps of using the spline curve algorithm to obtain the smooth path are: The autonomous driving vehicle executes the escape path and obtains second vehicle information, lane information, and obstacle information, wherein the second vehicle information is real-time vehicle information during the process of the autonomous driving vehicle executing the escape path; Using a dynamic programming algorithm, obtaining an optimal path node set and an optimal path according to the second vehicle information, the lane information and the obstacle information; According to the optimal path node set, the optimal path and the second vehicle information, an objective function of a cubic spline curve is constructed based on QP; the constraints of the objective function include boundary constraints and dynamic feasibility constraints; Solve the objective function to obtain a smooth path; the objective function is: In the formula, Cs(f2) is the objective function, f2(s) is the smooth path, s is the independent variable value at the optimal path node, ω1 is the first weight, ω2 is the second weight, ω3 is the third weight, and ω4 is the fourth weight. is the orientation of the optimal path node, is the node curvature at the optimal path node, is the derivative of the node curvature at the optimal path node, and g2(s) is the optimal path.

9. The method for automatically driving a vehicle out of trouble by combining a hybrid A* and spline curve algorithm according to claim 8, characterized in that: The method for judging whether the autonomous driving vehicle is out of trouble based on the node curvature is as follows: the node curvature at each optimal path node is smaller than the maximum curvature of the autonomous driving vehicle.

10. A method for autonomous driving vehicle escape from distress by combining hybrid A* and spline curve algorithms, characterized in that: It includes a preparation stage, an escape stage and an exit stage; the autonomous driving vehicle includes a driving planning module and an escape module, and the driving planning module includes a route planning unit and a decision-making unit; When the lateral planning algorithm of the route planning unit fails, the preparation phase is entered; the preparation phase includes the following steps: The decision-making unit: Based on the trapped state, it is determined whether the autonomous driving vehicle has a safe passage space; if so, the escape phase is entered; the escape phase includes the following steps: The escape module: Obtaining target node coordinates based on the safe passage space; A hybrid A* algorithm is used to obtain an escape path according to the trapped state and the target node coordinates; The autonomous driving vehicle executes the escape path; The driving planning module: The autonomous driving vehicle executes the escape path, and in the process of executing the escape path, a spline curve algorithm is used to obtain a smooth path; Obtaining the node curvature of the smooth path; Determining whether the autonomous driving vehicle is out of trouble based on the node curvature; If out of trouble, the exit phase will be entered; the exit phase includes the following steps: Obtaining a driving path according to the smooth path; The autonomous driving vehicle executes the driving path.

Images