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

Abstract

The application discloses an automatic driving path planning obstacle avoidance method and system based on sampling and cost evaluation. The method comprises a spline curve fitting step, a path sampling step, a sampling path cost calculation step, an optimal path determination step and an optimal path speed planning step. The spline curve fitting step determines control points and curve parameters through specific calculation to obtain a discrete path. The path sampling step calculates and splices three sections to generate multiple candidate sampling paths with smooth transitions. The sampling path cost calculation step evaluates path cost from multiple dimensions such as longitudinal, transverse and lane changing and comprehensively calculates. The optimal path determination step selects an optimal path according to the cost evaluation result. The optimal path speed planning step optimizes the speed according to the obstacle collision situation. The application solves the problems of slow running speed, low planning success rate and non-smooth trajectory of the existing algorithm, improves the efficiency, success rate and stability of automatic driving path planning obstacle avoidance, is suitable for the automatic driving technical field, promotes the development of automatic driving technology, and effectively improves the driving safety and reliability of automatic driving vehicles in various complex environments.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving, and in particular to a path planning and obstacle avoidance method for an automatic driving vehicle based on sampling and cost evaluation. BACKGROUND

[0002] In the current booming development of automatic driving technology, path planning and obstacle avoidance, as one of the core functions of the automatic 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 automatic driving. Among them, path planning algorithms such as hybrid A* and RRT, and motion planning algorithms such as TEB and DWA are relatively common. These algorithms usually rely on converting perception data into a grid map, and then combining a vehicle kinematic model to carry out obstacle avoidance operations. For example, in practical applications, the vehicle's surrounding environment information is obtained through on-board sensors, which is processed into a grid map form to represent the position and distribution of obstacles, and then according to the kinematic constraints of the vehicle such as the minimum turning radius and maximum speed, a feasible path is searched on the grid map.

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

[0005] Firstly, in terms of running speed, due to the high complexity of grid map processing and map-based path search, the algorithm runs slowly, which is difficult to meet the strict real-time requirements of the automatic driving system. For example, in complex urban traffic scenarios, the vehicle needs to make quick decisions to respond to the rapidly changing road conditions, but the existing algorithms may not be able to provide effective path planning in time due to the long calculation time.

[0006] Secondly, the planning success rate is unstable. In some special situations, such as narrow passages, dense obstacles or sudden appearance of dynamic obstacles, the algorithm may fail to successfully plan a feasible path, causing the automatic driving vehicle to be in a difficult situation, seriously affecting the safety and smoothness of driving.

[0007] Thirdly, the trajectory smoothness is insufficient. The path planned by the existing algorithm is often not smooth enough, and the vehicle needs to frequently adjust the steering and speed during driving, which not only increases the wear and tear of the vehicle's mechanical parts, but also easily leads to unstable lateral control. Especially in high-speed driving scenarios, the vehicle requires higher smoothness of the path, and the defect of the existing algorithm makes it completely unsuitable, because the unsmooth path may cause the vehicle to lose control and other serious safety problems.

[0008] Given the aforementioned problems with existing technologies in autonomous driving path planning and obstacle avoidance, there is an urgent need for a new, more efficient, and reliable algorithm to improve the performance of autonomous driving systems. Summary of the Invention

[0009] In view of the shortcomings of existing technologies, this invention provides an autonomous driving path planning and obstacle avoidance method based on sampling and cost evaluation. This invention aims to overcome the deficiencies of existing technologies by providing a safer, more stable, and more efficient path planning and obstacle avoidance solution for autonomous vehicles through an innovative sampling and cost evaluation-based method. This will promote the further development of autonomous driving technology, enabling it to better adapt to various complex real-world driving scenarios, reduce accident risks, and improve traffic efficiency.

[0010] Specifically, this invention aims to rapidly generate multiple candidate paths with good smoothness and transition characteristics 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. Simultaneously, by employing a precise cost evaluation mechanism, comprehensively considering factors such as lateral, longitudinal, and lane-changing costs, a comprehensive and accurate evaluation of candidate paths is achieved, enabling the selection of the optimal path and improving the rationality and scientific rigor of path planning. Furthermore, based on obstacle collision scenarios, reasonable speed planning is performed on the optimal path, ensuring that the vehicle can efficiently avoid obstacles while maintaining stability and safety during operation. This provides solid technical support for the reliable operation of autonomous driving technology in practical applications, promoting the continuous development and progress of the autonomous driving field.

[0011] To achieve the above objectives, this invention provides an autonomous driving path planning and obstacle avoidance method based on sampling and cost evaluation, comprising 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, wherein...

[0013] The control point calculation sub-step determines the control points of the spline curve using a specific geometric calculation method for two given points with headings. The curve parameter determination sub-step calculates 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 based on the control points, thereby obtaining the discrete path.

[0014] S2: Path sampling step. To generate multiple candidate paths with smooth transition characteristics, each sampled path is calculated and concatenated into 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 concatenation sub-step; where...

[0015] The straight line extension sub-step generates a path of the straight line segment part by extending along the heading according to the current pose of the vehicle; the curve fitting sub-step generates a path of the lane changing segment by determining control points through calculation of relative position relationship with the reference path and generating the path by spline curve fitting; the translation sub-step determines the translation range and direction based on the position of a specific point on the reference path to obtain the path of the translation segment; and the path splicing sub-step combines the three paths into a complete sampling path.

[0016] S3: a sampling path cost calculation step, in which a multi-dimensional cost evaluation is performed 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, wherein

[0017] The longitudinal cost calculation sub-step calculates the cost of the degree of obstruction of 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; and the comprehensive cost calculation and path evaluation sub-step comprehensively calculates the comprehensive cost according to the set weight based on the above-mentioned costs to evaluate the pros and cons of the path.

[0018] S4: an optimal path determination step, in which the optimal path is selected according to the sampling path cost calculation result;

[0019] S5: an optimal path speed planning step, in which the optimal path is optimized in speed according to the collision condition of the obstacle, including a collision detection sub-step, a parking point determination sub-step, and a speed adjustment sub-step, wherein

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

[0021] Further, in step S1:

[0022] In the control point calculation sub-step, for two points P1(X1, Y1, Yaw1) and P4(X4, Y4, Yaw4) with headings, the other two control points P2 and P3 are solved as follows:

[0023] a: find the central point coordinates of P1-P4:

[0024] x c =(x1+x4) / 2

[0025] y c =(y1+y4) / 2(0.1)

[0026] b: through the midpoint, P1 and P4 respectively to the positive and negative vertical point to get control points;

[0027] λ2 = sin(Yaw1)(x c -x1) + cos(Yaw1)(y c -y1)

[0028] x2 = x1 + λ2 sin(Yaw1)

[0029] y2 = y1 + λ2 cos(Yaw1)(0.2)

[0030] And:

[0031] λ3 = sin(Yaw4)(x4-x c ) + cos(Yaw4)(y4-y c )

[0032] x3 = x4 - λ3 sin(Yaw4)

[0033] y3 = y4 - λ3 cos(Yaw4) (0.3)

[0034] In the curve parameter determination sub-step,

[0035] Parameter equation, first order, second order parameter equation and path solving:

[0036] 1) 3 order spline curve parameter equation:

[0037] x(t) = x1(1-t) 3 + 3x2t(1-t) 2 + 3x3(1-t)t 2 + x4t 3

[0038] y(t) = y1(1-t) 3 + 3y2t(1-t) 2 + 3y3(1-t)t 2 + y4t 3 (0.4)

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

[0040] 2) 3 order spline curve first derivative equation

[0041] x'(t) = -3x1(1-t) 2 + 3x2(1-t) 2 - 6x2t(1-t) + 6x3t(1-t) - 3x3t 2 + 3x4t 2

[0042] y'(t) = -3y1(1-t) 2 + 3y2(1-t) 2 -6y2t(1-t) + 6y3t(1-t) - 3y3t 2 + 3y4t 2 (0.5)

[0043] 2) 3rd order spline curve 2nd derivative equation

[0044] x"(t) = 6x 1( 1-t) -12x2(1-t) + 6x2t + 6x3(1-t) -12x3t + 6x4t

[0045] y"(t) = 6y 1( 1-t) -12y2(1-t) + 6y2t + 6y3(1-t) -12y3t + 6y4t (0.6)

[0046] 4) Curve arc length

[0047]

[0048] 5) Heading angle at arbitrary point t

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

[0050] 6) Curvature at arbitrary point t

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

[0052] Thus, given P1 and P4, and the spline curve discrete path interval, the solving method is: d) solve the other two control point coordinates according to (0.1);

[0053] e) solve the arc length according to (0.7), and divide by the path interval to get a series of discrete t (0-1)

[0054] f) for t, solve the coordinates x and y, heading yaw, and curvature cur using (0.4) (0.8) (0.9) respectively.

[0055] Further, in step S2:

[0056] In the straight extension segment generation sub-step, the carTipPose and carTip segment path are generated:

[0057] The pose data structure of the defined point is Point(x, y, yaw), that is, the pose data point contains an X coordinate, a Y coordinate, and a heading angle; the pose of the center point of the rear axle of the vehicle is currentPose, and the carTipPose is a point extending a distance CarTip along the heading direction, so by using Green's formula, the coordinates and the heading of the point are as follows:

[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 the segment is a straight line, the calculation of the path of the segment can be obtained by interpolation at a certain interval;

[0062] 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 backward on the reference path until a point has a cumulative distance >= rollIn from refCarTipPose on the reference path, and the point is taken as rollPose on the reference path, recorded as refRollPose;

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

[0064] In the translation segment generation sub-step, horizonPose and rollIn segment path generation: using refRollPose on the reference path, traverse from refRollPose backward on the reference path until a point has a cumulative distance >= horizon from refRollPose on the reference path, and the point is taken as horizonPose on the reference path, recorded as refHorizonPose;

[0065] The index of the reference path where the refRollPose is located is refRollPoseIndex, and the index of the reference path where the refHorizonPose is located is refHorizonPoseIndex, so each sampling path is the segment of the reference path translated upward or downward;

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

[0067] Further, in step S3:

[0068] In the longitudinal cost calculation sub-step, a stop point stopIndex is determined when an obstacle or a radar point cloud is within a threshold detectionRange from a point on the sampling trajectory, and 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 the stop point, dis(0-end) is the total length of the sampling trajectory, and the longitudinal cost lonCost ∈ [0, 1.0];

[0070] In the lateral cost calculation sub-step, the lateral cost LatCost is calculated according to the deviation of the sampling trajectory from the reference trajectory; the calculation formula is:

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

[0072] where i represents the i-th sampling trajectory;

[0073] In the lane change cost calculation sub-step, the lane change cost LaneChangeCost is calculated according to the deviation of the sampling trajectory from the current optimal trajectory; 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, the comprehensive cost is calculated according to the set weight:

[0077] For each sampled trajectory, the comprehensive cost is calculated according to the corresponding weight, and the calculation formula of the comprehensive cost is:

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

[0079] LaneChangeCostWeight * LaneChangeCost

[0080] The sampled trajectory with the minimum cost is selected as the optimal trajectory.

[0081] Further, in step S4, the sampled path with the minimum comprehensive cost in the sampled path cost calculation step is directly selected as the optimal path.

[0082] Further, in step S5:

[0083] In the collision detection sub-step, the optimal trajectory is subjected to collision detection, and when the distance between the obstacle and the point on the optimal trajectory is less than detectionRange, the collision point index collisionIndex is determined, and if there is no collision, the process is exited.

[0084] In the parking point determination sub-step, the points are iterated from the collisionIndex point, and when the distance between the points on the trajectory is greater than the safety distance safetyRange, the point is recorded as a parking point.

[0085] In the speed adjustment sub-step, the maximum allowable speed of each point is calculated from the starting point of the trajectory to the collisionIndex, and 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, and if it is less than the minimum speed (generally 0), the minimum speed is replaced.

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

[0087] The above technical scheme is adopted in the present application, and at least the following beneficial effects are achieved:

[0088] 1. Path planning efficiency improvement: Through innovative path sampling strategies, based on spline curve fitting and three-section path generation methods, multiple candidate paths with good smoothness and transition can be quickly generated. Compared with traditional methods that rely on grid maps and complex search algorithms, the computational complexity is greatly reduced, the efficiency of path planning is improved, and the real-time requirements of autonomous driving systems are better met. In complex traffic scenarios, vehicles can quickly obtain feasible paths and effectively respond to rapidly changing road conditions.

[0089] 2. Path planning success rate improvement: Accurate sampling path cost calculation mechanism considers multiple factors such as longitudinal, lateral, and lane changing costs. This allows for more accurate evaluation of the pros and cons of each candidate path in complex scenarios such as narrow lanes, dense obstacles, or dynamic obstacles, making it more likely to plan a feasible path and significantly improving the success rate of planning, reducing the risk of autonomous vehicles getting into trouble, and ensuring safety and smoothness of travel.

[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. Vehicles do not need to frequently adjust steering and speed during travel, reducing mechanical component wear and improving lateral control stability. In high-speed driving scenarios, smooth paths can effectively avoid safety problems such as vehicle loss of control, making autonomous vehicle travel safer and more reliable.

[0091] 4. Adaptability and intelligence optimization: Speed planning strategy based on obstacle collision conditions can dynamically adjust the speed on the optimal path according to actual road conditions. Vehicles can more reasonably approach and avoid obstacles under safe conditions, 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 more similar to the reasonable decisions of human drivers, improving overall driving performance.

[0092] 5. Promote the development of autonomous driving technology: This invention provides a more efficient and reliable solution for autonomous driving path planning and obstacle avoidance, helping to overcome many limitations of existing technologies. 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 DRAWINGS

[0093] In order to more clearly illustrate the technical solutions of the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or prior art description. Obviously, the drawings in the following description only represent some of the embodiments of the present application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings.

[0094] Figure 1 is a flowchart schematic diagram of the present application;

[0095] Figure 2 is a control point solving schematic diagram of the present application;

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

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

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

[0099] Figure 6 is the obstacle avoidance effect schematic diagram of the present application when the reference trajectory has obstacles and the lane is changed to the right;

[0100] Figure 7 is the obstacle avoidance effect schematic diagram of the present application when the reference trajectory has obstacles and the lane is changed to the left;

[0101] Figure 8 is the obstacle avoidance effect schematic diagram of the present application when the reference trajectory has obstacles and the lane is changed to the left;

[0102] Figure 9 is the obstacle avoidance effect schematic diagram of the present application when the reference trajectory has obstacles and the lane is changed to the left;

[0103] Figure 10 is the obstacle avoidance effect schematic diagram of the present application when the reference trajectory has obstacles and the lane is changed to the left; DETAILED DESCRIPTION

[0104] The exemplary embodiments will be described in detail herein with reference to the attached drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The implementations described in the following exemplary embodiments are not meant to represent all implementations consistent with the present application. Rather, they are merely examples of apparatus and methods consistent with some aspects of the present application as detailed in the appended claims.

[0105] Embodiment One:

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

[0107] S1: a spline curve fitting step, used for obtaining a path curve with specific smoothness and controllability, including a control point calculation substep and a curve parameter determination substep, wherein

[0108] The control point calculation substep determines the control point of the spline curve through a specific geometric calculation method for the given two points with a heading; the curve parameter determination substep calculates the parameter equation, the first derivative equation, the second derivative equation, the arc length equation, the heading angle equation at an arbitrary point and the curvature equation of the spline curve based on the control point, and then obtains the discrete path;

[0109] S2: a path sampling step, for generating multiple candidate paths with smooth transition characteristics, the sampling path is divided into three segments for calculation and splicing, including a straight line extension segment generation substep, a curve fitting segment generation substep, a translation segment generation substep and a path splicing substep; wherein

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

[0111] S3: a sampling path cost calculation step, for multi-dimensional cost evaluation of each sampling path, including a longitudinal cost calculation substep, a lateral cost calculation substep and a comprehensive cost calculation and path evaluation substep, wherein

[0112] The longitudinal cost calculation substep calculates the cost of the degree of obstruction of the path in the longitudinal direction according to the position relationship between the obstacle and the path; the lateral cost calculation substep calculates the lateral cost according to the deviation degree of the sampling path relative to the reference path; the lane changing cost calculation substep 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 substep integrates the above costs and calculates the comprehensive cost according to the set weight to evaluate the path.

[0113] S4: an optimal path determination step, for selecting the optimal path according to the sampling path cost calculation result;

[0114] S5: an optimal path speed planning step, for speed optimization of the optimal path according to the obstacle collision condition, including a collision detection substep, a parking point determination substep and a speed adjustment substep, wherein,

[0115] a collision detection sub-step to detect whether there is an obstacle on the optimal path and determine the collision position; a parking point determination sub-step to determine the parking point based on the collision point position and the safety distance; and a speed adjustment sub-step to calculate and adjust the speed of each point on the path according to the parking point position and the kinematic parameters of the vehicle.

[0116] The present application aims to solve many challenges in the field of automatic driving, such as path planning and obstacle avoidance algorithm, and improve the performance and safety of the automatic driving system.

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

[0118] From the effect, the present application can quickly generate multiple smooth and natural transition candidate paths through unique spline curve fitting and path sampling strategy, effectively improving the efficiency of path planning, enabling the automatic driving vehicle to make decisions more quickly in 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 respond to various obstacle scenarios. Good trajectory smoothness and speed planning based on obstacle collision not only ensure the stability of vehicle driving and reduce mechanical wear, but also make the vehicle more intelligent and reasonable when approaching and avoiding obstacles, further improving the safety and reliability of automatic driving and promoting the development of automatic driving technology.

[0119] Embodiment Two

[0120] This embodiment two is further supplemented based on the above-mentioned embodiment one: 0: spline curve fitting

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

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

[0123] 0.1 Control point solving

[0124] Let P1 (X1, Y1, Yaw1), P4 (X4, Y4, Yaw4), and the other two control points P2 and P3 are solved as follows: a: find the P1-P4 center point coordinates:

[0125] x c = (x1+x4) / 2

[0126] y c = (y1+y4) / 2 (0.1)

[0127] b: through the midpoint, forward to P1 and P4 reverse respectively to find the vertical point, as P2 and P3 as shown: Figure 2

[0128] λ2 = sin(Yaw1)(x c -x1) + cos(Yaw1)(y c -y1)

[0129] x2 = x1+λ2sin(Yaw1)

[0130] y2 = y1+λ2cos(Yaw1) (0.2)

[0131] And:

[0132] λ3 = sin(Yaw4)(x4-x c ) + cos(Yaw4)(y4-y c )

[0133] x3 = x4-λ3sin(Yaw4)

[0134] y3 = y4-λ3cos(Yaw4) (0.3)

[0135] 0.2 Parameter equation, first-order, second-order parameter equation and path solving

[0136] 1) 3-order spline curve parameter equation:

[0137] x(t) = x1(1-t) 3 +3x2t(1-t) 2 +3x3(1-t)t 2 +x4t 3

[0138] y(t) = y1(1-t) 3 +3y2t(1-t) 2 +3y3(1-t)t 2 +y4t 3 (0.4)

[0139] ​where: t e [0, 1].

[0140] 2) First derivative equation of 3rd order spline curve

[0141] x'(t) = -3x1(1 - t) 2 + 3x2(1 - t) 2 - 6x2t(1 - t) + 6x3t(1 - t) - 3x3t 2 + 3x4t 2

[0142] y'(t) = -3y1(1 - t) 2 + 3y2(1 - t) 2 - 6y2t(1 - t) + 6y3t(1 - t) - 3y3t 2 + 3y4t 2 (0.5)

[0143] 2) Second derivative equation of 3rd order spline curve

[0144] x"(t) = 6x 1( 1 - t) - 12x2(1 - t) + 6x2t + 6x3(1 - t) - 12x3t + 6x4t

[0145] y"(t) = 6y 1( 1 - t) - 12y2(1 - t) + 6y2t + 6y3(1 - t) - 12y3t + 6y4t (0.6)

[0146] 4) Arc length of curve

[0147]

[0148] 5) Heading angle at arbitrary point t

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

[0150] 6) Curvature at arbitrary 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 interval of the spline curve discrete path, the solving method is:

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

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

[0155] i) For t, solve the coordinates x and y, heading yaw, and curvature cur using formulas (0.4) (0.8) (0.9), respectively.

[0156] 1: Path sampling

[0157] As shown in Figure 3 To achieve the smoothness and transition of the sampled path, each sampled path is calculated in three segments: carTip, rollIn, and horizon. The carTip has a straight line segment generated by extending the current vehicle pose, the rollIn is a lane change segment generated by spline fitting, and the horizon is a base path translation segment. By splicing the three segments, the smoothness of each sampled path can be achieved, and the stability of the lateral control can be achieved.

[0158] 1.1 carTipPose and carTip segment path generation

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

[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 path can be obtained by interpolating at a certain interval. The pseudo code is as follows:

[0164] [Solution is the path interval]

[0165] Foreach 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 rollInPose and rollIn segment path generation

[0171] First calculate the closest point on the reference path to carTipPose, refCarTipPose, then traverse the reference path from refCarTipPose, until a point is found that is >=rollIn from refCarTipPose on the reference path, then this point is the rollPose on the reference path, recorded as refRollPose.

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

[0173] [denotes the index of all sample paths where the reference path is located]

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

[0175] [denotes the lateral distance of this sample path from the reference path]

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

[0177] [denotes the angle of this point from the reference point, positive above the reference line, negative below the reference line]

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

[0179] [Green's formula]

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

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

[0182] yaw = refRollPose_yaw;

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

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

[0185] So the path generation method of rollIn segment is as follows:

[0186] Foreach i in [0, rollOutNumbers - 1]

[0187] [Start point is carTipPose]

[0188] startPose = carTipPose;

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

[0190] endPose = rollPosesVector[i];

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

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

[0193] [Store rollPath of each sampling line to the array]

[0194] rollInPathVector.push_back(rollInPath);

[0195] Figure 4 The rollInPath diagram is shown.

[0196] 1.3 horizonPose and rollIn segment path generation

[0197] Using the refRollPose obtained in 1.2, traverse the reference path backward until a point is found where the cumulative distance from the refRollPose on the reference path is greater than or equal to the horizon. This point is the horizonPose on the reference path, denoted as refHorizonPose.

[0198] Let refRollPose's index on the reference path be refRollPoseIndex, and refHorizonPose's index on the reference path be refHorizonPoseIndex. Therefore, each sampling path is simply a shift of that segment of the reference path upwards or downwards. The pseudocode for solving this is: Foreachiin[0,rollOutNumbers-1]

[0199] [Indicates the index of all sampling paths containing the reference path]

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

[0201] Foreachjin[refRollPoseIndex,refHorizonPoseIndex]

[0202] refPose = refPath[j];

[0203] [This indicates the lateral distance of the sampling path from the reference path.]

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

[0205] [This indicates the angle of offset from the reference point; values ​​above the reference line are positive, and values ​​below the reference line are negative.]

[0206] doubletransDelta=PI / 2.0WHILE(center-i)>=0ELSE-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 joins the horizonPath of this track]

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

[0213] [horizonPath joins horizonPathVector]

[0214] horizonPathVector.push_back(horizonPath);

[0215] Figure 5 is the schematic diagram of horizonPath of the present application;

[0216] 1.4 Path stitching

[0217] All the sampled tracks share one carTipPath, and their own rollInPath and horizonPath. So the pseudo code of path stitching is:

[0218] Foreachiin[0,rollOutNumbers-1]

[0219] foreachPincatTipPath

[0220] trajectorPath.push_back(p);

[0221] foreachPinrollInPathVector[i]

[0222] trajectorPath.push_back(p);

[0223] foreachPinhorizonPathVector[i]

[0224] trajectorPath.push_back(p);

[0225] trajectorPathVector.push_back(trajectorPath);

[0226] 2: Sampling path cost calculation

[0227] The lateral, longitudinal and lane change cost of each path is calculated, wherein the lateral cost represents the degree of deviation of the path from the reference path, the longitudinal cost represents the degree of how far the path can be driven to encounter an obstacle, and the lane change cost represents the degree of instability of steering caused by path switching.

[0228] 2.1: Path longitudinal cost calculation

[0229] For each sampled trajectory, search from the starting point until a point on the path, if there is an obstacle or the distance between the point and the radar point cloud is less than a certain threshold (detectionRange), the point is recorded as a 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:

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

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

[0232] 2.2: Path lateral cost calculation

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

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

[0235] LatCost represents the degree of deviation of the sampled trajectory from the reference trajectory, wherein i represents the ith sampled trajectory, the uppermost and lowermost trajectories have a cost of 1.0, and the middle trajectories have a cost of 0.0.

[0236] 2.2: Path lane change cost calculation

[0237] The lane change cost represents the degree of deviation of the sampled trajectory from the current optimal trajectory, and the addition of the lane change cost can prevent frequent lane changes caused by the system, leading to instability of lateral control.

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

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

[0240] 2.3: Path synthesis cost calculation and path evaluation

[0241] According to the corresponding weight, the synthesis cost of each sampled trajectory is calculated, and the calculation formula of the synthesis 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, the collision situation of the obstacle on the trajectory still needs to be considered for speed re-planning.

[0247] (1) Collision detection is performed on the trajectory. When there is an obstacle at a certain point and the distance between the obstacle and the point is less than detectionRange, the index value collisionIndex of the point is obtained. If there is no collision point on the path, the current loop is directly exited.

[0248] (2) Iteration is performed from the collisionIndex point to the rear. When the distance between the point and the collisionIndex point on the trajectory is greater than the safety distance (safetyRange, generally 1.5*carLength), the point is recorded as a parking point to ensure a certain safety distance between the parking point and the collision point.

[0249] (3) From the start point of the trajectory to the collisionIndex, the cumulative distance distance from the point to the parking point is calculated, and the maximum speed allowed at the point is calculated according to the following formula:

[0250] v = 2 * decelerationMax * diatance

[0251] v max = 2 * decelerationMax * diatance

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

[0253] (4) Set the velocity of all trajectory points to 0 from the collisionIndex to the end of the trajectory.

[0254] The application effect of the algorithm in the automatic driving path planning and obstacle avoidance is shown by examples under different working conditions, including left and right lane changing when the reference trajectory has obstacles, returning to the reference trajectory after obstacle avoidance, speed re-planning and obstacle avoidance in complex environment, and parking before obstacles in narrow environment, which embodies the ability of the algorithm to effectively plan paths and reasonably avoid obstacles in various scenarios, ensuring the safe and smooth driving of the vehicle.

[0255] Figure 6 : Obstacle avoidance effect schematic diagram of right lane changing when the reference trajectory has obstacles

[0256] The figure shows that when the vehicle detects an obstacle in front of the reference trajectory during driving, the algorithm can select the right lane changing strategy according to the sampling and cost evaluation mechanism. By generating multiple sampling paths, the cost of each path is calculated to determine the optimal path (i.e., the right lane changing path), which enables the vehicle to successfully avoid the obstacle and continue driving. It can be seen that the vehicle's driving trajectory smoothly deviates to the right before encountering the obstacle, achieving safe lane changing.

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

[0258] This figure is Figure 6 a subsequent driving situation of the vehicle after right lane changing to avoid the obstacle. After successfully avoiding the obstacle, the algorithm continues to guide the vehicle to gradually adjust the driving trajectory based on real-time road conditions and path planning strategies, so that it can smoothly return to the original reference trajectory or a reasonable path close to the original reference trajectory for continuous driving, which embodies the ability of the algorithm to continuously optimize path planning in dynamic environments and ensures the coherence and stability of vehicle driving.

[0259] Figure 8 : Obstacle avoidance effect schematic diagram of left lane changing when the reference trajectory has obstacles

[0260] Similar to Figure 6 , when an obstacle appears on the reference trajectory, this figure shows another possible obstacle avoidance decision of the algorithm - left lane changing. The vehicle selects the optimal left lane changing path from multiple sampling paths according to the algorithm, 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, providing multiple feasible obstacle avoidance solutions for the vehicle.

[0261] Figure 9 : Obstacle avoidance effect schematic diagram of returning to the reference trajectory after obstacle avoidance (corresponding to left lane changing situation)

[0262] This is Figure 8The driving track of the middle vehicle continues after the lane change to the left and the obstacle avoidance. After avoiding the obstacle, the vehicle gradually corrects the driving direction through the continuous effect of the algorithm, and finally returns to the appropriate reference track or similar path to continue driving, further proving that the algorithm can not only effectively avoid obstacles in complex road conditions, but also restore to the normal or optimized driving path in subsequent driving, ensuring the efficiency and safety of the overall driving.

[0263] Figure 10 : Schematic diagram of obstacle avoidance effect of obstacle avoidance in complex environment

[0264] The driving conditions of the vehicle in a complex environment are presented in the figure, such as the presence of multiple obstacles, irregular road shapes, and other complex situations. The algorithm of the present application can guide the vehicle to flexibly shuttle between obstacles through multiple path sampling, cost evaluation and optimal path selection, and constantly adjust the speed and direction to achieve safe obstacle avoidance. It can be seen that the vehicle speed gradually decreases when approaching the obstacle, successfully avoids the obstacle at the appropriate position, and the speed change and path planning are closely coordinated to adapt to the complex and variable environment.

[0265] Although the embodiments of the present application have been shown and described above, it can be understood that the above-mentioned embodiments are exemplary and cannot be understood as limiting the present application, and those skilled in the art can make changes, modifications, replacements and variations to the above-mentioned embodiments within the scope of the present application.

Claims

1. An automatic driving path planning obstacle avoidance method based on sampling and cost evaluation, characterized in that: The method comprises the following steps: S1: a spline curve fitting step for obtaining a path curve with specific smoothness and controllability, comprising a control point calculation sub-step and a curve parameter determination sub-step, wherein In the control point calculation sub-step, control points of the spline curve are determined through specific geometric calculation methods for given two points with headings; in the curve parameter determination sub-step, parameter equations, first derivative equations, second derivative equations, arc length equations, heading angle equations at arbitrary points and curvature equations of the spline curve are calculated based on the control points, and then a discrete path is obtained; S2: a path sampling step for generating a plurality of candidate paths with smooth transition characteristics, each sampling path being divided into three segments for calculation and splicing, comprising 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 In the straight line extension segment generation sub-step, a straight line segment path is obtained by extending along the heading direction according to the current pose of the vehicle; in the curve fitting segment generation sub-step, control points are determined by calculating the relative position relationship with the reference path, and a 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 a specific point on the reference path, and a translation segment path is obtained; in the path splicing sub-step, the three paths are combined into a complete sampling path; S3: a sampling path cost calculation step for multi-dimensional cost evaluation of each sampling path, comprising a longitudinal cost calculation sub-step, a lateral cost calculation sub-step, a lane changing cost calculation sub-step and a comprehensive cost calculation and path evaluation sub-step, wherein In the longitudinal cost calculation sub-step, the cost of the degree of obstruction of path longitudinal driving is calculated according to 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 sampling 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 of the sampling path from the current optimal path; in the comprehensive cost calculation and path evaluation sub-step, the comprehensive cost is calculated according to the set weight by comprehensively calculating the above costs, so as to evaluate the path advantages and disadvantages; S4: an optimal path determination step for selecting the optimal path according to the sampling path cost calculation result; S5: an optimal path speed planning step for optimizing the speed of the optimal path according to the obstacle collision condition, comprising 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 is an obstacle on the optimal path and determines the collision position; The parking point determination sub-step determines the parking point based on the collision point position and the safety distance; the speed adjustment sub-step calculates and adjusts the speed of each point on the path according to the parking point position and the kinematic parameters of the vehicle.

2. The method of claim 1, wherein: In step S1: In the control point calculation sub-step, for two points with headings, Two other control points And The solution method is as follows: a: find the center point coordinates of P1-P4: (0.1) b: the control points are obtained by finding the perpendicular points of the center point to P1 and P4 in the forward and reverse directions respectively; (0.2) and (0.3) In the curve parameter determination sub-step, Parameter equations, first derivative equations, second derivative equations and path solving: 1) 3rd order spline curve parametric equation: (0.4) wherein: ; 2) First derivative equation of 3-order spline curve (0.5) 3) Second derivative equation of 3-order spline curve (0.6) 4) Curve arc length (0.7) 5) heading angle at any point t (0.8) 6) curvature at any point t (0.9) Therefore, given P1 and P4, and the spline curve discrete path solving method for the path interval interval is: a) Solve the coordinates of the other two control points according to (0.1), (0.2), (0.3); b) Solve the arc length according to formula (0.7), and divide it by the path interval to obtain a series of discrete t (0-1) c) For t, solve the coordinates x and y, heading yaw, and curvature cur using formulas (0.4), (0.8), and (0.9), respectively.

3. The method of claim 1, wherein: In step S2: In the straight extension segment generation sub-step, the carTipPose and the carTip segment path are generated: Define the pose data structure of the point Point (x, y, yaw), that is, the pose data point contains the X coordinate, the Y coordinate, and the heading angle; the pose of the rear axle center point of the vehicle is currentPose, and the carTipPose is a point extending a distance CarTip along the heading, so using Green's formula, the coordinates and heading of the point are: ; Since this segment is a straight line, the path of this segment can be obtained by linear interpolation at a certain interval; In the curve fitting segment generation sub-step, the rollInPose and the rollIn segment path are generated: first, calculate the point refCarTipPose on the reference path closest to carTipPose, then traverse the reference path from refCarTipPose backward until a point on the reference path accumulates a distance >= rollIn from refCarTipPose, then this point is taken as rollPose on the reference path, and is recorded as refRollPose; The number of sampled paths rollOutNumbers is odd, and the horizontal sampling interval is horizontalDensity. When calculating rollInPose, the coordinates are determined by the relative position and angle relationship with the points on the reference path, and finally the rollInPath is generated by fitting a spline curve with carTipPose and refRollPose as endpoints; In the translation segment generation sub-step, the horizonPose and the rollIn segment path are generated: using refRollPose on the reference path, traverse the reference path backward until a point on the reference path accumulates a distance >= horizon from refRollPose, then this point is taken as horizonPose on the reference path, and is recorded as refHorizonPose; Record the index of refRollPose on the reference path as refRollPoseIndex, and the index of refHorizonPose on the reference path as refHorizonPoseIndex, so each sampled path is a segment of the reference path translated upward or downward; In the path splicing sub-step, splice the carTipPath, rollInPath, and horizonPath of all sampled paths in a specific order.

4. The method of claim 3, wherein: In step S3: In the longitudinal cost calculation sub-step, the search starts from the beginning of the sampling path, and the stop point stopIndex is determined when the distance between the obstacle or radar point cloud and the point on the sampling path is less than the threshold detectionRange. 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 accumulated distance from the start point to the stop point, dis(0-end) is the total length of the sampled path, 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 of the sampling path from the reference path; the calculation formula is: LatCost = fabs(i-(rollOutNumbers-1) / 2) / (rollOutNumbers-1) / 2) Where i represents the ith sampling path; In the lane change cost calculation sub-step, the lane change cost LaneChangeCost is calculated according to the deviation of the sampling path from the current optimal path; the calculation formula is: LaneChangeCost = fabs(i-currentIndex) / (rollOutNumbers-1) / 2) wherein, the currentIndex is the current optimal path index; the LaneChangeCost takes a value in the range of LaneChangeCost [0, 1.0]; In the comprehensive cost calculation and path evaluation sub-step, the comprehensive cost is calculated according to the set weight: According to the corresponding weight, the comprehensive cost of each sampled path is calculated, and the calculation formula of the comprehensive cost is: The sampling path with the minimum cost is selected as the optimal path.

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

6. The sampling and cost evaluation based automatic driving path planning obstacle avoidance method according to claim 1, wherein, In step S5: In the collision detection sub-step, the optimal path is subjected to collision detection, and when the distance between the obstacle and the point on the optimal path is less than detectionRange, the collision point index collisionIndex is determined. If there is no collision, exit; In the stop point determination sub-step, iteration is performed from the collisionIndex point, and when the distance between the point and the collisionIndex point on the path is greater than the safety distance safetyRange, the point is recorded as the stop point; In the speed adjustment sub-step, the maximum allowable speed of each point is calculated from the path start point to the collisionIndex, and the calculation formula is If the speed is less than the original speed of the path point, it is replaced; if it is less than the minimum speed, it is replaced with the minimum speed. From the collisionIndex to the end of the path, the speed of all path points is set to 0.

Images