Automatic driving path planning obstacle avoidance method based on sampling and cost evaluation

Abstract

The invention discloses an automatic driving path planning obstacle avoidance method and system based on sampling and cost evaluation. Comprising the following steps: spline curve fitting: determining control points and curve parameters through specific calculation to obtain a discrete path; a path sampling step: calculating and splicing in three sections to generate a plurality of candidate sampling paths in smooth transition; a sampling path cost calculation step: evaluating path cost from multiple dimensions such as longitudinal, transverse and lane changing and comprehensively calculating the path cost; an optimal path determination step: selecting an optimal path according to a cost evaluation result; and an optimal path speed planning step, wherein the speed is optimized according to the obstacle collision condition. The method solves the problems of low running speed, low planning success rate, unsmooth trajectory and the like of the existing algorithm, improves the efficiency, success rate and stability of automatic driving path planning and obstacle avoidance, is suitable for the technical field of automatic driving, promotes the development of the automatic driving technology, and has wide application prospects. And the driving safety and reliability of the automatic driving vehicle in various complex environments can be effectively improved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving, and particularly focuses on path planning and obstacle avoidance algorithms for autonomous vehicles. Specifically, it is a method for autonomous driving path planning and obstacle avoidance based on sampling and cost evaluation, which is used to solve the problem of how an autonomous vehicle safely and efficiently plans its driving path and avoids obstacles during driving. Background Art

[0002] At present, with the booming development of autonomous driving technology, path planning and obstacle avoidance, as one of the core functions of an autonomous driving system, play a crucial role in ensuring the safety, stability, and overall performance of vehicle driving.

[0003] Currently, a variety of path planning and obstacle avoidance algorithms are widely used in the field of autonomous driving. Among them, path planning algorithms such as Hybrid A*, RRT, etc., and motion planning algorithms such as TEB, DWA, etc. are relatively common. These algorithms usually rely on converting perception data into a grid map, and then combining the vehicle kinematic model to carry out obstacle avoidance operations. For example, in practical applications, the vehicle obtains the surrounding environment information through on-vehicle sensors, processes it into the form of a grid map to represent the position and distribution of obstacles, and then searches for a feasible path on the grid map according to the kinematic limitations of the vehicle, such as the minimum turning radius, maximum speed, etc.

[0004] Problems existing in the prior art: These existing algorithms have many limitations.

[0005] Firstly, in terms of running speed, due to the high computational complexity of processing the grid map and path search based on the map, the algorithm runs slowly, making it difficult to meet the strict real-time requirements of the autonomous driving system. For example, in a complex urban traffic scenario, the vehicle needs to make quick decisions to cope with the ever-changing road conditions, but the existing algorithms may not be able to provide an effective path plan in time due to excessive computational time consumption.

[0006] Secondly, the planning success rate is unstable. In some special cases, such as when encountering complex scenarios like narrow channels, dense obstacles, or the sudden appearance of dynamic obstacles, the algorithm may not be able to successfully plan a feasible path, causing the autonomous vehicle to get into trouble and seriously affecting the safety and smoothness of driving.

[0007] Furthermore, the trajectory smoothness is insufficient. The paths planned by existing algorithms are often not smooth enough, and the vehicle needs to frequently adjust the steering and speed during driving, which not only increases the wear of vehicle mechanical components but also easily leads to unstable lateral control. Especially in high-speed driving scenarios, the vehicle has higher requirements for path smoothness, and this defect of the existing algorithms makes them completely inapplicable, because an uneven path may cause serious safety problems such as vehicle out of control.

[0008] In view of the above problems existing in the prior art in terms of path planning and obstacle avoidance for autonomous driving, there is an urgent need for a brand-new, more efficient and reliable algorithm to improve the performance of autonomous driving systems. Summary of the Invention

[0009] In view of the deficiencies of the current related prior art, the present invention provides an autonomous driving path planning and obstacle avoidance method based on sampling and cost evaluation. The present invention aims to overcome the defects of the prior art and provide a safer, more stable and efficient path planning and obstacle avoidance solution for autonomous vehicles through an innovative method based on sampling and cost evaluation, promoting the further development of autonomous driving technology, enabling it to better adapt to various complex actual driving scenarios, reducing accident risks and improving traffic efficiency.

[0010] Specifically, the present invention expects to quickly generate multiple candidate paths with good smoothness and transitional properties through a unique path sampling strategy, effectively improving the efficiency and success rate of path planning and enhancing the adaptability of autonomous vehicles in complex environments. At the same time, with the help of an accurate cost evaluation mechanism, comprehensively considering various factors such as the lateral, longitudinal and lane-changing costs of the path, a comprehensive and accurate evaluation of the candidate paths is realized, so as to be able to select the optimal path, improving the rationality and scientificity of path planning. In addition, a reasonable speed plan is made for the optimal path based on the obstacle collision situation, ensuring that the vehicle can efficiently avoid obstacles during driving while maintaining driving stability and safety, providing a solid technical support for the reliable operation of autonomous driving technology in practical applications, and promoting the continuous development and progress of the autonomous driving field.

[0011] To achieve the above object, the present invention provides an autonomous driving path planning and obstacle avoidance method based on sampling and cost evaluation, including the following steps:

[0012] S1: Spline curve fitting step, used to obtain a path curve with specific smoothness and controllability, including a control point calculation sub-step and a curve parameter determination sub-step, where

[0013] In the control point calculation sub-step, for two given points with headings, the control points of the spline curve are determined through a specific geometric calculation method; in the curve parameter determination sub-step, the parametric equations, first derivative equations, second derivative equations, arc length equations, heading angle equations at any point and curvature equations of the spline curve are calculated based on the control points, and then the discrete path is obtained;

[0014] S2: Path sampling step, to generate multiple candidate paths with smooth transition characteristics, each sampling path is calculated and spliced in three segments, including a straight line extension segment generation sub-step, a curve fitting segment generation sub-step, a translation segment generation sub-step and a path splicing sub-step; where

[0015] The straight-line extension segment generation sub-step extends along the heading according to the current pose of the vehicle to obtain the path of the straight-line segment part; the curve fitting segment generation sub-step determines the control points by calculating the relative position relationship with the reference path, and generates the lane-changing segment path by using spline curve fitting; the translation segment generation sub-step determines the translation range and direction based on the position of specific points on the reference path to obtain the translation segment path; the path splicing sub-step combines the above three segments of paths into a complete sampling path;

[0016] S3: Sampling path cost calculation step, performing multi-dimensional cost evaluation on each sampling path, including a longitudinal cost calculation sub-step, a lateral cost calculation sub-step, and a comprehensive cost calculation and path evaluation sub-step, where

[0017] The longitudinal cost calculation sub-step calculates the cost of the degree of obstruction of the longitudinal driving of the path according to the position relationship between the obstacle and the path; the lateral cost calculation sub-step calculates the lateral cost according to the deviation degree of the sampling path relative to the reference path; the lane-changing cost calculation sub-step calculates the lane-changing cost based on the deviation degree of the sampling path from the current optimal path; the comprehensive cost calculation and path evaluation sub-step synthesizes the above costs and calculates the comprehensive cost according to the set weights to evaluate the quality of the path;

[0018] S4: Optimal path determination step, selecting the optimal path according to the sampling path cost calculation result;

[0019] S5: Optimal path speed planning step, optimizing the speed of the optimal path according to the obstacle collision situation, including a collision detection sub-step, a stop point determination sub-step, and a speed adjustment sub-step, where,

[0020] The collision detection sub-step detects whether there are obstacles on the optimal path and determines the collision position; the stop point determination sub-step determines the stop point based on the collision point position and the safety distance; the speed adjustment sub-step calculates and adjusts the speeds of each point on the path according to the stop point position and the vehicle kinematic parameters.

[0021] Furthermore, in step S1:

[0022] In the control point calculation sub-step, for two points P 1 (X 1 , Y 1 , Yaw 1 ), P 4 (X 4 , Y 4 , Yaw 4 ), the solution methods for the other two control points P 2 and P 3 are as follows:

[0023] a: Calculate the center point coordinates of P1 - P4:

[0024] x c =(x 1 +x 4 ) / 2

[0025] y c =(y 1 +y 4 ) / 2(0.1)

[0026] b: Through the mid - point, find the perpendicular points in the positive direction of P1 and the negative direction of P4 respectively to obtain the control points;

[0027] λ 2 =sin(Yaw 1 )(x c -x 1 )+cos(Yaw 1 )(y c -y 1 )

[0028] x 2 =x 1 +λ 2 sin(Yaw 1 )

[0029] y 2 =y 1 +λ 2 cos(Yaw 1 )(0.2)

[0030] And:

[0031] λ 3 =sin(Yaw 4 )(x 4 -x c )+cos(Yaw 4 )(y 4 -y c )

[0032] x 3 =x 4 -λ 3 sin(Yaw 4 )

[0033] y 3 =y 4 -λ 3 cos(Yaw 4 )(0.3)

[0034] In the sub - step of determining the curve parameters,

[0035] Parametric equations, first - order, second - order parametric equations and path solving:

[0036] 1) Parametric equations of the cubic spline curve:

[0037] x(t) = x 1 (1 - t) 3 + 3x 2 t(1 - t) 2 + 3x 3 (1 - t)t 2 + x 4 t 3

[0038] y(t) = y 1 (1 - t) 3 + 3y 2 t(1 - t) 2 + 3y 3 (1 - t)t 2 + y 4 t 3 (0.4)

[0039] where: t ∈ [0, 1];

[0040] 2) First - order derivative equations of the cubic spline curve

[0041] x'(t) = -3x 1 (1 - t) 2 + 3x 2 (1 - t) 2 - 6x 2 t(1 - t)+ 6x 3 t(1 - t)- 3x 3 t 2 + 3x 4 t 2

[0042] y'(t) = -3y 1 (1 - t) 2 + 3y 2 (1 - t) 2 - 6y 2 t(1 - t)+ 6y 3 t(1 - t)- 3y 3 t 2 + 3y 4 t 2 (0.5)

[0043] 2) Second - order derivative equations of the cubic spline curve

[0044] x”(t) = 6x 1( 1 - t)- 12x 2 (1 - t)+ 6x 2 t+ 6x3 (1 - t) - 12x 3 t + 6x 4 t

[0045] y''(t) = 6y 1( (1 - t) - 12y 2 (1 - t) + 6y 2 t + 6y 3 (1 - t) - 12y 3 t + 6y 4 t(0.6)

[0046] 4) Arc length of the curve

[0047]

[0048] 5) Heading angle at any point t

[0049] yaw(t) = atctan(y'(t) / x'(t)) (0.8)

[0050] 6) Curvature at any point t

[0051] cur(t) = x'(t)y''(t) - y'(t)x''(t) / (x' 2 (t) + y' 2 (t)) 3 / 2 (0.9)

[0052] Therefore, given P1 and P4, and the method for solving the discrete path of the spline curve with the path interval interval is: d) Solve the coordinates of the other two control points according to...1;

[0053] e) Solve the arc length according to Equation (0.7) and divide it by the path interval to obtain a series of discrete t (0 - 1)

[0054] f) For t, use Equations (0.4), (0.8), and (0.9) to solve the coordinates x and y, the heading yaw, and the curvature cur respectively.

[0055] Furthermore, in step S2:

[0056] In the sub - step of generating the straight - line extension segment, generating carTipPose and the path of the carTip segment:

[0057] Define the pose data structure of a point Point(x, y, yaw), that is, the pose data point includes the X coordinate, Y coordinate, and heading angle; the pose of the center point of the rear axle of the vehicle is currentPose, and carTipPose is the point that extends a distance CarTip along the heading. Therefore, using Green's formula, the coordinates and heading of this point are respectively:

[0058] carTipPose_x = currentPose_x + CarTip * cos(currentPose_yaw)

[0059] carTipPose_y = currentPose_y + CarTip * sin(currentPose_yaw)

[0060] carTipPose_yaw = currentPose_yaw

[0061] Since this section is a straight line, the calculation of the path of this section can be obtained by interpolating at a certain interval currently;

[0062] In the sub-step of generating the curve fitting section, the rollInPose and the path generation of the rollIn section: First, calculate the point refCarTipPose on the reference path that is closest to carTipPose, and then traverse backward on the reference path from refCarTipPose until the cumulative distance of a certain point from refCarTipPose on the reference path >= rollIn, then this point is used as the rollPose on the reference path, denoted as refRollPose;

[0063] The number of sampling paths rollOutNumbers is odd, the horizontal sampling interval is horizontalDensity, the coordinates are determined by the relative position and angular relationship with the points on the reference path when calculating rollInPose, and finally the rollInPath is generated by fitting a spline curve with carTipPose and refRollPose as endpoints;

[0064] In the sub-step of generating the translation section, the horizonPose and the path generation of the rollIn section: Use refRollPose on the reference path and traverse backward on the reference path until the cumulative distance of a certain point from refRollPose on the reference path >= horizon, and this point is the horizonPose on the reference path, denoted as refHorizonPose;

[0065] Denote the index of refRollPose on the reference path as refRollPoseIndex, and the index of refHorizonPose on the reference path as refHorizonPoseIndex. Therefore, each sampling path is the translation of this section of the reference path upward or downward;

[0066] In the path splicing sub-step, the carTipPath, rollInPath, and horizonPath of all sampled trajectories are spliced in a specific order.

[0067] Furthermore, in step S3:

[0068] In the longitudinal cost calculation sub-step, start searching from the starting point of the sampled trajectory. When the distance between an obstacle or radar point cloud and a point on the sampled trajectory is less than the threshold detectionRange, determine the stop point stopIndex. If the entire path is safe, the longitudinal cost lonCost = 0.0; otherwise, the longitudinal cost is calculated according to the following formula: lonCost = 1.0 - dis(0->stopIndex) / dis(0-end);

[0069] Where dis(0->stopIndex) is the cumulative distance from the starting point to this stop point, dis(0-end) is the total length of the sampled trajectory, and the longitudinal cost lonCost ∈ [0, 1.0];

[0070] In the lateral cost calculation sub-step, calculate the lateral cost LatCost according to the deviation degree between the sampled trajectory and the reference trajectory; the calculation formula is:

[0071] LatCost = fabs(i - (rollOutNumbers - 1) / 2) / (rollOutNumbers - 1) / 2

[0072] Where i represents the i-th sampled trajectory;

[0073] In the lane change cost calculation sub-step, calculate the lane change cost LaneChangeCost according to the deviation degree between the sampled trajectory and the current optimal trajectory, and the calculation formula is:

[0074] LaneChangeCost = fabs(i - currentIndex) / (rollOutNumbers - 1) / 2

[0075] Where currentIndex is the index of the current optimal trajectory; the value range of LaneChangeCost is LaneChangeCost ∈ [0, 1.0];

[0076] In the comprehensive cost calculation and path evaluation sub-step, calculate the comprehensive cost according to the set weights:

[0077] For each sampled trajectory, calculate the comprehensive cost according to the corresponding weight. The calculation formula for the comprehensive cost is:

[0078] Cost = LonCostWeight * lonCost + LatCostWeight * LatCost +

[0079] LaneChangeCostWeight * LaneChangeCost

[0080] Select the sampling trajectory with the minimum cost as the optimal trajectory.

[0081] Furthermore, in step S4, directly select the sampling path with the minimum comprehensive cost in the sampling path cost calculation step as the optimal path.

[0082] Furthermore, in step S5:

[0083] In the collision detection sub-step, perform collision detection on the optimal trajectory. When the distance between the obstacle and the point on the optimal trajectory is less than detectionRange, determine the collision point index collisionIndex. If there is no collision, exit.

[0084] In the parking point determination sub-step, iterate backward from the collisionIndex point. When the distance between this point and the collisionIndex point on the trajectory is greater than the safety distance safetyRange, record this point as the parking point.

[0085] In the speed adjustment sub-step, traverse and calculate the maximum allowable speed of each point from the starting point of the trajectory to the collisionIndex. The calculation formula is v max = 2 * decelerationMax * diatance. If this speed is less than the original speed of the trajectory point, replace it. If it is less than the minimum speed (usually), replace it with the minimum speed.

[0086] From the collisionIndex to the end of the trajectory, set the speed of all trajectory points to 0.

[0087] The present invention adopts the above technical solutions and has at least the following beneficial effects:

[0088] 1. Improvement in path planning efficiency: Through an innovative path sampling strategy, based on spline curve fitting and a three-stage path generation method, it can quickly generate multiple candidate paths with good smoothness and transitional properties. Compared with the traditional method that relies on grid maps and complex search algorithms, it greatly reduces the computational complexity, improves the efficiency of path planning, and better meets the strict real-time requirements of the autonomous driving system. In complex traffic scenarios, the vehicle can obtain a feasible path more quickly and effectively respond to the rapidly changing road conditions.

[0089] 2. Improved path planning success rate: The precise sampling path cost calculation mechanism comprehensively considers various factors such as longitudinal, lateral, and lane-changing costs. This enables more accurate evaluation of the advantages and disadvantages of each candidate path in the face of complex scenarios such as narrow channels, dense obstacles, or dynamic obstacles, thus making it more likely to plan a feasible path, significantly improving the planning success rate, reducing the risk of autonomous vehicles getting stuck, and ensuring driving safety and smoothness.

[0090] 3. Enhanced trajectory smoothness and stability: Spline curve fitting and carefully designed path sampling and splicing methods ensure that the generated path has high smoothness. The vehicle does not need to frequently adjust steering and speed during driving, reducing wear of mechanical components and effectively improving the stability of lateral control. Especially in high-speed driving scenarios, the smooth path can effectively avoid safety problems such as vehicle out-of-control, making the driving of autonomous vehicles safer and more reliable.

[0091] 4. Optimized adaptability and intelligence: The speed planning strategy based on obstacle collision conditions can dynamically adjust the speed on the optimal path according to the actual road conditions. The vehicle can approach and avoid obstacles more reasonably on the premise of safety, further improving the system's adaptability to complex environments. At the same time, this dynamic speed planning reflects the intelligence of the algorithm, making the behavior of autonomous vehicles closer to the reasonable decisions of human drivers and enhancing the overall driving performance.

[0092] 5. Promoting the development of autonomous driving technology: The present invention provides a more efficient and reliable solution for autonomous driving path planning and obstacle avoidance, helping to overcome many limitations of the existing technology. This will promote the further development of autonomous driving technology in practical applications, improve traffic efficiency, reduce traffic accidents, and lay a solid technical foundation for the construction of future intelligent transportation systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0093] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0094] Figure 1 It is a schematic flowchart of the present invention;

[0095] Figure 2 It is a schematic diagram of solving control points of the present invention;

[0096] Figure 3 It is one of the schematic diagrams of path sampling of the present invention;

[0097] Figure 4 It is the second schematic diagram of path sampling of the present invention;

[0098] Figure 5 It is the third schematic diagram of path sampling of the present invention;

[0099] Figure 6 It is the schematic diagram of obstacle avoidance effect when changing lanes to the right with obstacles on the reference trajectory of the present invention;

[0100] Figure 7 It is the schematic diagram of obstacle avoidance effect when returning to the reference trajectory after bypassing obstacles by the present invention;

[0101] Figure 8 It is the schematic diagram of obstacle avoidance effect when changing lanes to the left with obstacles on the reference trajectory of the present invention;

[0102] Figure 9 It is the schematic diagram of obstacle avoidance effect when returning to the reference trajectory after bypassing obstacles by the present invention;

[0103] Figure 10 It is the schematic diagram of obstacle avoidance effect when bypassing obstacles in a complex environment by the present invention. Detailed implementation manners

[0104] Here, the exemplary embodiments will be described in detail, and the examples are shown in the drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The implementation manners described in the following exemplary embodiments do not represent all implementation manners consistent with the present invention. On the contrary, they are merely examples of devices and methods consistent with some aspects of the present invention as detailed in the appended claims.

[0105] Embodiment 1:

[0106] As Figure 1 shown, this embodiment provides an autonomous driving path planning and obstacle avoidance method based on sampling and cost evaluation, including the following steps:

[0107] S1: The spline curve fitting step, used to obtain a path curve with specific smoothness and controllability, including a control point calculation sub-step and a curve parameter determination sub-step, where

[0108] In the control point calculation sub-step, for two given points with headings, the control points of the spline curve are determined through a specific geometric calculation method; in the curve parameter determination sub-step, the parametric equation, first derivative equation, second derivative equation, arc length equation, heading angle equation at any point, and curvature equation of the spline curve are calculated based on the control points, and then the discrete path is obtained;

[0109] S2: Path sampling step. To generate multiple candidate paths with smooth transition characteristics, each sampled path is calculated and spliced in three segments, including a straight extension segment generation sub-step, a curve fitting segment generation sub-step, a translation segment generation sub-step, and a path splicing sub-step. Among them

[0110] In the straight extension segment generation sub-step, the path of the straight segment part is obtained by extending along the heading according to the current pose of the vehicle. In the curve fitting segment generation sub-step, the control points are determined by calculating the relative position relationship with the reference path, and the lane-changing segment path is generated by spline curve fitting. In the translation segment generation sub-step, the translation range and direction are determined based on the position of specific points on the reference path to obtain the translation segment path. In the path splicing sub-step, the above three segments of paths are combined into a complete sampled path.

[0111] S3: Sampled path cost calculation step. Multidimensional cost evaluation is performed on each sampled path, including a longitudinal cost calculation sub-step, a lateral cost calculation sub-step, and a comprehensive cost calculation and path evaluation sub-step. Among them

[0112] In the longitudinal cost calculation sub-step, the cost of the degree of obstruction of longitudinal driving of the path is calculated based on the position relationship between the obstacle and the path. In the lateral cost calculation sub-step, the lateral cost is calculated according to the deviation degree of the sampled path relative to the reference path. In the lane-changing cost calculation sub-step, the lane-changing cost is calculated based on the deviation degree between the sampled path and the current optimal path. In the comprehensive cost calculation and path evaluation sub-step, the above costs are combined and the comprehensive cost is calculated according to the set weights to evaluate the quality of the path.

[0113] S4: Optimal path determination step. The optimal path is selected based on the result of the sampled path cost calculation.

[0114] S5: Optimal path speed planning step. The speed of the optimal path is optimized according to the obstacle collision situation, including a collision detection sub-step, a stop point determination sub-step, and a speed adjustment sub-step. Among them,

[0115] In the collision detection sub-step, it is detected whether there are obstacles on the optimal path and the collision position is determined. In the stop point determination sub-step, the stop point is determined based on the collision point position and the safety distance. In the speed adjustment sub-step, the speeds of each point on the path are calculated and adjusted according to the stop point position and the vehicle kinematic parameters.

[0116] The present invention aims to solve many challenges faced by path planning and obstacle avoidance algorithms in the field of autonomous driving, and improve the performance and safety of the autonomous driving system.

[0117] In terms of the objective, aiming at the problems of slow running speed, unstable planning success rate, insufficient trajectory smoothness, etc. of the existing algorithms, a new method based on sampling and cost evaluation is proposed. Through innovative algorithm design, it is expected to achieve fast, stable, and efficient path planning and obstacle avoidance functions to adapt to complex and changeable actual driving scenarios, reduce accident risks, and improve traffic efficiency.

[0118] In terms of the effect, through the unique spline curve fitting and path sampling strategy of the present invention, multiple smooth and naturally transitioning candidate paths can be quickly generated, effectively improving the efficiency of path planning and enabling the autonomous driving vehicle to make decisions more quickly under complex road conditions. The precise cost evaluation mechanism and optimal path selection strategy significantly improve the planning success rate and enhance the system's ability to handle various obstacle scenarios. The good trajectory smoothness and speed planning based on obstacle collision not only ensure the stability of vehicle driving, reduce mechanical wear, but also make the vehicle more intelligent and reasonable when approaching and avoiding obstacles, further improving the safety and reliability of autonomous driving and strongly promoting the development of autonomous driving technology.

[0119] Embodiment 2

[0120] This Embodiment 2 makes a further supplementary description on the basis of the above Embodiment 1: 0: Spline curve fitting

[0121] As Figure 2 shown, for the general algorithm - two points P1(X1, Y1, YAW1) and P4(X4, Y4, YAW4) with headings are used to fit a curve by the spline method for the fitting of rollInPath.

[0122] The spline curve in this embodiment refers to the Bezier curve. Fitting a curve by the Bezier method means using the Bezier curve to generate a smooth path. The Bezier curve is a kind of spline curve, commonly used in computer graphics to create smooth curved paths. In this embodiment, it is used to generate a path from point P1 to point P4. "For the fitting of rollInPath" is used to describe fitting the path into the path.

[0123] 0.1 Solving control points

[0124] Let P 1 (X 1 , Y 1 , Yaw 1 ), P 4 (X 4 , Y 4 , Yaw 4 ), and the other two control points P 2 and P 3 are solved as follows: a: Find the center point coordinates of P1 - P4:

[0125] x c =(x 1 +x 4 ) / 2

[0126] y c =(y 1 +y 4 ) / 2 (0.1)

[0127] b: Through the midpoint, find the perpendicular points in the positive direction of P1 and the negative direction of P4 respectively, as P2 and P3 as Figure 2 shown:

[0128] λ 2 =sin(Yaw 1 )(x c -x 1 )+cos(Yaw 1 )(y c -y 1 )

[0129] x 2 =x 1 +λ 2 sin(Yaw 1 )

[0130] y 2 =y 1 +λ 2 cos(Yaw 1 )(0.2)

[0131] And:

[0132] λ 3 =sin(Yaw 4 )(x 4 -x c )+cos(Yaw 4 )(y 4 -y c )

[0133] x 3 =x 4 -λ 3 sin(Yaw 4 )

[0134] y 3 =y 4 -λ 3 cos(Yaw 4 )(0.3)

[0135] 0.2 Parametric Equations, First-Order, Second-Order Parametric Equations and Path Solving

[0136] 1) Parametric Equation of 3rd-Order Spline Curve:

[0137] x(t) = x 1 (1 - t) 3 + 3x 2 t(1 - t) 2 + 3x 3 (1 - t)t 2 + x 4 t 3

[0138] y(t) = y 1 (1 - t) 3 + 3y 2 t(1 - t) 2 + 3y 3 (1 - t)t 2 + y 4 t 3 (0.4)

[0139] where: t ∈ [0, 1].

[0140] 2) First - order derivative equation of the cubic spline curve

[0141] x'(t) = - 3x 1 (1 - t) 2 + 3x 2 (1 - t) 2 - 6x 2 t(1 - t)+ 6x 3 t(1 - t)- 3x 3 t 2 + 3x 4 t 2

[0142] y'(t) = - 3y 1 (1 - t) 2 + 3y 2 (1 - t) 2 - 6y 2 t(1 - t)+ 6y 3 t(1 - t)- 3y 3 t 2 + 3y 4 t 2 (0.5)

[0143] 2) Second - order derivative equation of the cubic spline curve

[0144] x”(t) = 6x 1( 1 - t)- 12x 2 (1 - t)+ 6x 2 t+ 6x 3 (1 - t)- 12x 3 t+ 6x4 t

[0145] y”(t) = 6y 1( 1 - t) - 12y 2 (1 - t) + 6y 2 t + 6y 3 (1 - t) - 12y 3 t + 6y 4 t (0.6)

[0146] 4) Curve arc length

[0147]

[0148] 5) Course angle at any point t

[0149] yaw(t) = atctan(y'(t) / x'(t)) (0.8)

[0150] 6) Curvature at any point t

[0151] cur(t) = x'(t)y”(t) - y'(t)x”(t) / (x' 2 (t) + y' 2 (t)) 3 / 2 (0.9)

[0152] Therefore, given P1 and P4, and the method for solving the discrete path of the spline curve with the path interval interval is as follows:

[0153] g) Solve the coordinates of the other two control points according to.1;

[0154] h) Solve the arc length according to equation (0.7), and divide it by the path interval to obtain a series of discrete t (0 - 1)

[0155] i) For t, use equations (0.4), (0.8), and (0.9) to solve the coordinates x and y, the course yaw, and the curvature cur respectively.

[0156] 1: Path sampling

[0157] As Figure 3 shown, to achieve the smoothness and transitional property of the sampling path, each sampling path is calculated in three segments: carTip, rollIn, and horizon. Among them, carTip is the straight line segment generated by the linear extension of the current vehicle pose, rollIn is the lane-changing segment generated by spline curve fitting; horizon is the basic path translation segment. By splicing the paths of the three segments, the smoothness of each sampling path can be achieved, and the stability of lateral control can be realized.

[0158] 1.1 Generation of carTipPose and carTip segment path

[0159] Define the pose data structure of a point Point(x, y, yaw), that is, the pose data point includes the X coordinate, Y coordinate, and heading angle (unit: rad). The pose of the center point of the vehicle's rear axle is currentPose, and carTipPose is the point that extends a distance CarTip along the heading from this point. Therefore, using Green's formula, the coordinates and heading of this point are respectively:

[0160] carTipPose_x = currentPose_x + CarTip * cos(currentPose_yaw)

[0161] carTipPose_y = currentPose_y + CarTip * sin(currentPose_yaw)

[0162] carTipPose_yaw = currentPose_yaw

[0163] Since this segment is a straight line, the calculation of this segment of the path can be obtained by linearly interpolating at a certain interval. Its pseudocode is:

[0164] [Solution is the path interval]

[0165] For each i in [0, 1 + CarTip / solution]

[0166] x = currentPose_x + carTipPose_x * i / (1 + CarTip / solution)

[0167] y = currentPose_y + carTipPose_y * i / (1 + CarTip / solution)

[0168] yaw = currentPose_yaw

[0169] carTipPath.push_back(P(x, y, yaw)

[0170] 1.2 Generation of rollInPose and rollIn segment path

[0171] First, calculate the point refCarTipPose on the reference path that is closest to carTipPose. Then, traverse backward from refCarTipPose on the reference path until a point is reached where the cumulative distance from refCarTipPose on the reference path is >= rollIn. This point is taken as the rollPose on the reference path, denoted as refRollPose.

[0172] Assume the number of sampling paths is rollOutNumbers (odd), and the horizontal sampling interval is horizontalDensity. Then, for any sampling path, its rollInPose is calculated as follows (pseudo-code): For each i in [0, rollOutNumbers - 1]

[0173] [Indicates the indices of all sampling paths where the reference path is located]

[0174] int center = (rollOutNumbers - 1) / 2;

[0175] [Indicates the horizontal distance of this sampling path from the reference path]

[0176] double transDis = fabs(center - i) * horizontalDensity;

[0177] [Indicates the angle by which this point is offset from the reference point, positive above the reference line and negative below the reference line]

[0178] double transDelta = PI / 2.0 WHILE(center - i) >= 0 ELSE -PI / 2.0;

[0179] [Green's theorem]

[0180] x = refRollPose_x + transDis * cos(refRollPose_yaw + transDelta);

[0181] y = refRollPose_y + transDis * sin(refRollPose_yaw + transDelta);

[0182] yaw = refRollPose_yaw;

[0183] [Add the rollPose of each sampling line to the array]

[0184] rollPosesVector.push_back(rollPose(x,y,yaw));

[0185] Therefore, the path generation method for the rollIn segment is as follows:

[0186] For each i in [0, rollOutNumbers - 1]

[0187] [Starting point is carTipPose]

[0188] startPose = carTipPose;

[0189] [End point is the rollPose of each sampling line]

[0190] endPose = rollPosesVector[i];

[0191] [Use the spline method described in Chapter 0 for curve fitting]

[0192] rollInPath = Bezier(startPose, endPose);

[0193] [Store the rollPath of each sampling line into an array]

[0194] rollInPathVector.push_back(rollInPath);

[0195] Figure 4 This is the schematic diagram of rollInPath.

[0196] 1.3 horizonPose and rollIn segment path generation

[0197] Using the refRollPose on the reference path obtained in 1.2, traverse backward from the reference path until the cumulative distance from a certain point to refRollPose on the reference path >= horizon. This point is the horizonPose on the reference path, denoted as refHorizonPose.

[0198] Denote the index of refRollPose on the reference path as refRollPoseIndex, and the index of refHorizonPose on the reference path as refHorizonPoseIndex. Therefore, each sampling path is the upward or downward translation of this segment of the reference path. Its pseudo - code for solution is: For each i in [0, rollOutNumbers - 1]

[0199] [Indicates the index of all sampling paths where the reference path is located]

[0200] int center = (rollOutNumbers - 1) / 2;

[0201] For each j in [refRollPoseIndex, refHorizonPoseIndex]

[0202] refPose = refPath[j];

[0203] [Indicates the lateral distance between this sampling path and the reference path]

[0204] double transDis = fabs(center - i) * horizontalDensity;

[0205] [Indicates the angle by which this point is offset from the reference point, positive above the reference line and negative below the reference line]

[0206] double transDelta = PI / 2.0 WHILE (center - i) >= 0 ELSE -PI / 2.0;

[0207] [Green's theorem]

[0208] x = refPose_x + transDis * cos(refPose_yaw + transDelta);

[0209] y = refPose_y + transDis * sin(refPose_yaw + transDelta);

[0210] yaw = refPose_yaw;

[0211] [This point is added to the horizonPath of this sampling trajectory]

[0212] horizonPath.push_back(P(x, y, yaw));

[0213] [horizonPath is added to the horizonPathVector]

[0214] horizonPathVector.push_back(horizonPath);

[0215] Figure 5 Is the schematic diagram of the horizonPath of the present invention;

[0216] 1.4 Path stitching

[0217] For all sampling trajectories, a common carTipPath, as well as their own rollInPath and horizonPath are used. Therefore, the pseudocode for path stitching is as follows:

[0218] For each i in [0, rollOutNumbers - 1]

[0219] For each p in catTipPath

[0220] trajectorPath.push_back(p);

[0221] For each p in rollInPathVector[i]

[0222] trajectorPath.push_back(p);

[0223] For each p in horizonPathVector[i]

[0224] trajectorPath.push_back(p);

[0225] trajectorPathVector.push_back(trajectorPath);

[0226] 2: Sampling path cost calculation

[0227] Calculate the costs of lateral, longitudinal, and lane-changing for each path. Among them, the lateral cost represents the degree to which the path deviates from the reference path, the longitudinal cost represents the degree to which it can still travel before hitting an obstacle, and the lane-changing cost represents the degree of steering instability caused by path switching. And normalize each cost.

[0228] 2.1: Path longitudinal cost calculation

[0229] Search each sampling trajectory from the starting point until a point on the path where there is an obstacle or the radar point cloud distance from this point is less than a certain threshold (detectionRange), then this point is recorded as the stop point stopIndex. If the entire path is safe, then the longitudinal cost lonCost = 0.0, otherwise the longitudinal cost is calculated according to the following formula:

[0230] lonCost = 1.0 - dis(0 -> stopIndex) / dis(0 - end);

[0231] Among them, dis(0->stopIndex) is the cumulative distance from the starting point to this parking point, and dis(0-end) is the total length of the sampled trajectory. Therefore, the longitudinal cost lonCost ∈ [0, 1.0].

[0232] 2.2: Calculation of the lateral cost of the path

[0233] The calculation formula for the lateral cost is:

[0234] LatCost = fabs(i - (rollOutNumbers - 1) / 2) / ((rollOutNumbers - 1) / 2)

[0235] LatCost represents the degree of deviation of the sampled estimate from the reference trajectory, where i represents the i-th sampled trajectory. The cost of the topmost and bottommost trajectories is 1.0, and the cost of the middle trajectory is 0.0.

[0236] 2.2: Calculation of the lane change cost of the path

[0237] The lane change cost represents the degree of deviation of the sampled trajectory from the current optimal trajectory. Adding the lane change cost can prevent the system from frequently changing lanes, resulting in lateral control instability.

[0238] LaneChangeCost = fabs(i - currentIndex) / ((rollOutNumbers - 1) / 2)

[0239] Among them, currentIndex is the index of the current optimal trajectory. The value range of LaneChangeCost is LaneChangeCost ∈ [0, 1.0].

[0240] 2.3: Calculation of the comprehensive cost of the path and path evaluation

[0241] For each sampled trajectory, calculate the comprehensive cost according to the corresponding weight. The calculation formula for the comprehensive cost is:

[0242] Cost = LonCostWeight * lonCost + LatCostWeight * LatCost +

[0243] LaneChangeCostWeight * LaneChangeCost

[0244] Therefore, the sampled trajectory with the minimum cost is the optimal trajectory of the system.

[0245] 3: Optimal path speed planning

[0246] The sampled trajectory inherits the speed information from the reference trajectory. After obtaining the optimal trajectory, it is still necessary to consider the collision situation with obstacles on the trajectory and perform speed replanning.

[0247] (1) Perform collision detection on the trajectory. When the distance between an obstacle and a certain point is less than the detectionRange at a certain point, obtain the index value collisionIndex of that point; if there is no collision point on the path, directly exit the current loop;

[0248] (2) Iterate backward from the collisionIndex point. When the distance between this point and the collisionIndex point on this trajectory is greater than the safety distance (safetyRange, generally ensuring 1.5 * carLength), record this point as a parking point to ensure that there is a certain safety distance between the parking point and the collision point.

[0249] (3) Traverse from the starting point of the trajectory to the collisionIndex, calculate the cumulative distance distance from this point to the parking point, and calculate the maximum allowable speed of this point according to the following formula:

[0250] v = 2 * decelerationMax * diatance

[0251] v max = 2 * decelerationMax * diatance

[0252] If this speed is less than the original speed at the trajectory point, replace the speed of this point with this speed. If this speed is less than the minimum speed (generally set to 2.0 km / h), replace the speed of this point with the minimum speed.

[0253] (4) From the collisionIndex to the end of the trajectory, set the speed of all trajectory points to 0.

[0254] Examples under different working conditions demonstrate the application effect of the algorithm of the present invention in obstacle avoidance for autonomous driving path planning, including left and right lane changes and returning to the reference trajectory after bypassing obstacles when there are obstacles on the reference trajectory, obstacle bypassing in complex environments, and speed replanning and parking in front of obstacles in narrow environments, etc., reflecting the ability of the algorithm to effectively plan paths and reasonably avoid obstacles in multiple scenarios, ensuring the safe and stable driving of the vehicle.

[0255] Figure 6 : Schematic diagram of the obstacle avoidance effect of changing lanes to the right when there are obstacles on the reference trajectory

[0256] The figure shows that during the driving process of a vehicle, when an obstacle is detected in front of the reference trajectory, the algorithm can, based on the sampling and cost evaluation mechanism, select a strategy of changing lanes to the right. By generating multiple sampling paths, calculating the costs of each path, and determining the optimal path (i.e., the path of changing lanes to the right), the vehicle can successfully avoid the obstacle and continue to drive. It can be seen that the driving trajectory of the vehicle smoothly shifts to the right before encountering the obstacle, achieving a safe lane change.

[0257] Figure 7 : Schematic diagram of the obstacle avoidance effect of returning to the reference trajectory after bypassing the obstacle

[0258] This figure is Figure 6 a display of the subsequent driving situation of the vehicle after changing lanes to the right to bypass the obstacle in []. After successfully avoiding the obstacle, the algorithm continues to guide the vehicle to gradually adjust its driving trajectory according to the real-time road conditions and path planning strategy, enabling it to smoothly return to the original reference trajectory or a reasonable path close to the original reference trajectory and continue driving, demonstrating the algorithm's ability to continuously optimize path planning in a dynamic environment and ensuring the coherence and stability of the vehicle's driving.

[0259] Figure 8 : Schematic diagram of the obstacle avoidance effect of changing lanes to the left when there is an obstacle on the reference trajectory

[0260] Similar to Figure 6 this, when an obstacle appears on the reference trajectory, this figure shows another possible obstacle avoidance decision of the algorithm - changing lanes to the left. The vehicle selects the optimal path of changing lanes to the left from multiple sampling paths according to the algorithm's planning, and its driving trajectory smoothly transitions to the left to bypass the obstacle, demonstrating the flexibility and effectiveness of the algorithm in the face of obstacles in different directions and providing multiple feasible obstacle avoidance solutions for the vehicle.

[0261] Figure 9 : Schematic diagram of the obstacle avoidance effect of returning to the reference trajectory after bypassing the obstacle (corresponding to the case of changing lanes to the left)

[0262] This is Figure 8 the continuation of the driving trajectory of the vehicle after changing lanes to the left to bypass the obstacle in []. After the vehicle avoids the obstacle, through the continuous action of the algorithm, it gradually corrects its driving direction and finally returns to a suitable reference trajectory or a similar path to continue moving forward, further proving that the algorithm can not only effectively avoid obstacles in complex road conditions but also restore to a normal or optimized driving path during subsequent driving, ensuring the overall driving efficiency and safety.

[0263] Figure 10 : Schematic diagram of the obstacle avoidance effect of bypassing obstacles in a complex environment

[0264] The figure presents the driving conditions of a vehicle in a complex environment, such as multiple obstacles and irregular road shapes. The algorithm of the present invention can, in this scenario, generate paths through multiple samplings, evaluate costs, and select the optimal path, guiding the vehicle to flexibly shuttle between obstacles, continuously adjusting its speed and direction to achieve safe obstacle avoidance. It can be seen that the vehicle speed gradually decreases when approaching an obstacle and successfully bypasses the obstacle at an appropriate position. The speed change and path planning are closely coordinated to adapt to the complex and changeable environment.

[0265] Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

Claims

1. An autonomous driving path planning and obstacle avoidance method based on sampling and cost evaluation, characterized by: The following steps are involved: S1: Spline curve fitting step, used to obtain a path curve with specific smoothness and controllability, including a control point calculation sub-step and a curve parameter determination sub-step, where The control point calculation substep determines the control points of the spline curve by a specific geometric calculation method for two given points with headings; the curve parameter determination substep calculates the parameter equation, first-order derivative equation, second-order derivative equation, arc length equation, heading angle equation at any point and curvature equation of the spline curve based on the control points, thereby obtaining a discrete path; S2: Path sampling step, in order to generate multiple candidate paths with smooth transition characteristics, each sampling path is divided into three segments for calculation and splicing, including a straight line extension segment generation sub-step, a curve fitting segment generation sub-step, a translation segment generation sub-step and a path splicing sub-step; wherein The straight extension segment generation substep is to obtain the straight segment path by extending the vehicle along the heading according to the current position of the vehicle; the curve fitting segment generation substep is to determine the control point by calculating the relative position relationship with the reference path, and generate the lane change segment path by spline curve fitting; The translation segment generation sub-step determines the translation range and direction based on the position of a specific point on the reference path to obtain a translation segment path; the path splicing sub-step combines the above three path segments into a complete sampling path; S3: Sampling path cost calculation step, which performs multi-dimensional cost evaluation on each sampling path, including a longitudinal cost calculation sub-step, a lateral cost calculation sub-step, and a comprehensive cost calculation and path evaluation sub-step. The longitudinal cost calculation sub-step calculates the cost of the degree of obstruction to the longitudinal travel of the path based on the positional relationship between the obstacle and the path; The lateral cost calculation sub-step calculates the lateral cost according to the deviation degree of the sampling path relative to the reference path; The lane change cost calculation substep calculates the lane change cost based on the deviation degree between the sampled path and the current optimal path; the comprehensive cost calculation and path evaluation substep combines the above costs and calculates the comprehensive cost according to the set weights to evaluate the quality of the path; S4: optimal path determination step, selecting the optimal path based on the sampled path cost calculation result; S5: Optimal path speed planning step, optimizing the speed of the optimal path according to the obstacle collision situation, including a collision detection sub-step, a parking point determination sub-step and a speed adjustment sub-step, wherein: The collision detection sub-step detects whether there are obstacles on the optimal path and determines the collision position; The parking point determination substep determines the parking point based on the collision point position and the safety distance; the speed adjustment substep calculates and adjusts the speed of each point on the path according to the parking point position and the vehicle kinematic parameters.

2. The method according to claim 1, characterized in that: In step S1: In the control point calculation sub-step, for the two heading points P1 (X1, Y1, Yaw1), P4 (X4, Y4, Yaw4), the solution methods for the other two control points P2 and P3 are as follows: a: Find the coordinates of the center point of P1-P4: x c =(x1+x4) / 2 and c (y1+y4) / 2(0.1) b: Through the midpoint, find the perpendicular points in the positive direction of P1 and the reverse direction of P4 to obtain the control point; λ2=sin(Yaw1)(x c -x1)+cos(Yaw1)(y c -y1) x2=x1+λ2sin(Yaw1) y2=y1+λ2cos(Yaw1)(0.2) as well as: λ3=sin(Yaw4)(x4-x c )+cos(Yaw4)(y4-y c ) x3=x4-λ3sin(Yaw4) y3=y4-λ3cos(Yaw4)(0.3) In the curve parameter determination sub-step, Parametric equations, first-order and second-order parametric equations and path solutions: 1) 3rd order spline curve parametric equation: x(t)=x1(1-t) 3 +3x2t(1-t) 2 +3x3(1-t)t 2 +x4t 3 y(t)=y1(1-t) 3 +3y2t(1-t) 2 +3y3(1-t)t 2 +y4t 3 (0.4) Where: t∈[0,1]; 2) The first-order derivative equation of the third-order spline curve x'(t)=-3x1(1-t) 2 +3x2(1-t) 2 -6x2t(1-t)+6x3t(1-t)-3x3t 2 +3x4t 2 y'(t)=-3y1(1-t) 2 +3y2(1-t) 2 -6y2t(1-t)+6y3t(1-t)-3y3t 2 +3y4t 2 (0.5) 2) The second-order derivative equation of the third-order spline curve x”(t)=6x 1( 1-t)-12x2(1-t)+6x2t+6x3(1-t)-12x3t+6x4t y”(t)=6y 1( 1-t)-12y2(1-t)+6y2t+6y3(1-t)-12y3t+6y4t(0.6) 4) Curve arc length 5) Heading angle at any point t yaw(t)=atctan(y'(t) / x'(t))(0.8) 6) Curvature at any point t cur(t)=x'(t)y”(t)-y'(t)x”(t) / (x' 2 (t)+y' 2 (t)) 3 / 2 (0.9) Therefore, given P1 and P4, and the spline curve discrete path solution method with path interval interval is: a) Solve the coordinates of the other two control points according to .1; b) Solve the arc length according to equation (0.7) and divide it by the path interval to obtain a series of discrete t(0-1) c) For t, use equations (0.4), (0.8), and (0.9) to solve the coordinates x and y, heading yaw, and curvature cur respectively.

3. The method according to claim 1, characterized in that: In step S2: In the straight line extension segment generation substep, carTipPose and carTip segment path are generated: The pose data structure of the defined point is Point(x,y,yaw), that is, the pose data point contains the X coordinate, Y coordinate and heading angle; the pose of the center point of the rear axle of the vehicle is currentPose, and carTipPose is the point extending a distance CarTip along the heading. Therefore, using Green's formula, the coordinates and heading of the point are: carTipPose_x=currentPose_x+CarTip*cos(currentPose_yaw) carTipPose_y=currentPose_y+CarTip*sin(currentPose_yaw) carTipPose_yaw=currentPose_yaw Since this segment is a straight line, the calculation of this segment of the path can be obtained by interpolating at certain intervals; In the curve fitting segment generation sub-step, rollInPose and rollIn segment path generation: first calculate the point refCarTipPose closest to carTipPose on the reference path, then traverse from refCarTipPose on the reference path, until a point on the reference path has a cumulative distance from refCarTipPose >= rollIn, then the point is used as the rollPose on the reference path, recorded as refRollPose; The number of sampling paths rollOutNumbers is an odd number, the horizontal sampling interval is horizontalDensity, and the coordinates are determined by the relative position and angle relationship with the points on the reference path when calculating rollInPose. Finally, the rollInPath is generated by spline curve fitting with carTipPose and refRollPose as endpoints; In the translation segment generation sub-step, the horizonPose and rollIn segment paths are generated: using the refRollPose on the reference path, traverse backward from the reference path until the cumulative distance from a certain point to refRollPose on the reference path is greater than or equal to horizon. This point is the horizonPose on the reference path, recorded as refHorizonPose; The index of refRollPose on the reference path is refRollPoseIndex, and the index of refHorizonPose on the reference path is refHorizonPoseIndex, so each sampling path is the upward or downward translation of that section of the reference path; In the path stitching substep, carTipPath, rollInPath and horizonPath of all sampled tracks are stitched in a specific order.

4. The method according to claim 1, characterized in that: In step S3: In the longitudinal cost calculation sub-step, the search starts from the starting point of the sampling trajectory. When the distance between the obstacle or radar point cloud and the point on the sampling trajectory is less than the threshold detectionRange, the stopping point stopIndex is determined. If the entire path is safe, the longitudinal cost lonCost = 0.0, otherwise the longitudinal cost is calculated according to the following formula: lonCost = 1.0-dis(0->stopIndex) / dis(0-end); Where dis(0->stopIndex) is the cumulative distance from the starting point to the parking point, dis(0-end) is the total length of the sampled trajectory, and the longitudinal cost lonCost∈[0,1.0]; In the lateral cost calculation sub-step, the lateral cost LatCost is calculated according to the deviation between the sampling trajectory and the reference trajectory; the calculation formula is: LatCost=fabs(i-(rollOutNumbers-1) / 2) / (rollOutNumbers-1) / 2) Where i represents the i-th sampling trajectory; In the lane change cost calculation sub-step, the lane change cost LaneChangeCost is calculated according to the deviation between the sampled trajectory and the current optimal trajectory. The calculation formula is: LaneChangeCost=fabs(i-currentIndex) / (rollOutNumbers-1) / 2) Among them, currentIndex is the current optimal trajectory index; LaneChangeCost value range is LaneChangeCost∈[0,1.0]; In the comprehensive cost calculation and path evaluation sub-step, the comprehensive cost is calculated according to the set weights: For each sampled trajectory, the comprehensive cost is calculated according to the corresponding weight. The calculation formula of the comprehensive cost is: Cost=LonCostWeight*lonCost+LatCostWeight*LatCost+ LaneChangeCostWeight*LaneChangeCost The sampling trajectory with the minimum cost is selected as the optimal trajectory.

5. The method for autonomous driving path planning and obstacle avoidance based on sampling and cost evaluation according to claim 1, characterized in that: In step S4, the sampling path with the smallest comprehensive cost in the sampling path cost calculation step is directly selected as the optimal path.

6. The method for autonomous driving path planning and obstacle avoidance based on sampling and cost evaluation according to claim 1, characterized in that: In step S5: In the collision detection sub-step, the optimal trajectory is subjected to collision detection. When the distance between the obstacle and the point on the optimal trajectory is less than detectionRange, the collision point index collisionIndex is determined. If there is no collision, the program exits. In the parking point determination sub-step, iterate from the collisionIndex point backwards, and when the distance between this point and the collisionIndex point on the trajectory is greater than the safety distance safetyRange, record this point as the parking point; In the speed adjustment sub-step, the maximum speed allowed for each point is calculated from the starting point of the trajectory to collisionIndex. The calculation formula is v max =2*decelerationMax*diatance, if the speed is less than the original speed of the trajectory point, it is replaced; if it is less than the minimum speed (usually ), it is replaced by the minimum speed; Set the velocity of all trajectory points from collisionIndex to the end of the trajectory to 0.

Images