An autonomous driving trajectory planning method based on spline curves and polynomial curves

Abstract

The present invention discloses an automatic driving trajectory planning method based on spline curves and polynomial curves, which relates to the field of automatic driving technology, including the generation and processing of global paths, environmental perception and data fusion processing, extraction and processing of local reference paths, local trajectory planning, optimal trajectory sampling and trajectory output, etc. The present invention can achieve a smooth and optimal driving effect for vehicles in complex scenes with multiple vehicles and multiple dynamic and static obstacles.

Description

Technical Field

[0001] The present invention belongs to the technical field of driverless vehicles, and in particular relates to an automatic driving trajectory planning method based on spline curves and polynomial curves. Background Art

[0002] With the continuous development of science and technology, sensor technology is becoming more and more advanced, chip technology has made great progress, communication technology is changing with each passing day, and vehicle intelligent driving technology is gradually becoming mature. The biggest role of the development of intelligent driving technology is to reduce the driving burden of drivers and reduce the occurrence of traffic accidents. At present, many well-known companies at home and abroad have invested a large number of high-tech personnel and technologies to develop high-level intelligent driving technology.

[0003] The driving decision and trajectory planning module is one of the core parts of autonomous driving, especially the trajectory planning algorithm has attracted more and more attention from researchers. A good trajectory planning algorithm can make the vehicle drive more smoothly and safely. Usually, the trajectory planning algorithm of a vehicle can obtain information about the vehicle and its surroundings through sensors and other sensing components, such as the position and speed of dynamic and static obstacles, the position and speed of the vehicle itself, etc., and plan a safe, feasible and reasonable driving trajectory, and send the trajectory information to the control module in real time. The control module makes reasonable control of the vehicle, such as acceleration, deceleration, obstacle avoidance, lane change, etc., to realize the vehicle's autonomous driving function. At present, although the existing autonomous driving vehicles can detect dynamic and static obstacles, due to the ever-changing obstacles, the vehicle's response to obstacles is not flexible and stable enough, there is a risk of collision between the vehicle and the obstacle, and there are great safety hazards.

[0004] In addition, the existing vehicles usually set a stop circle with a radius of R and the stop point as the center near the reference path. As long as the vehicle is in the stop area, the vehicle stops. However, due to vehicle driving errors, vehicle obstacle avoidance, and difficulty in decelerating immediately, the vehicle may run out of the stop circle or even fail to reach the stop circle, resulting in parking failure. Summary of the invention

[0005] The purpose of the present invention is to provide an automatic driving trajectory planning method based on spline curves and polynomial curves to solve the above-mentioned defects caused by the prior art.

[0006] The present invention discloses a trajectory planning method based on spline curves and polynomial curves in a complex and changeable environment, including the generation and processing of global paths, the extraction and processing of local reference paths, environmental perception and data fusion processing, local trajectory planning, optimal trajectory sampling and trajectory output, etc. Among them:

[0007] S1. Generation and processing of global paths. The global path is obtained from high-precision maps or pre-sampled longitude and latitude coordinates.

[0008] Furthermore, if the global path is obtained through a high-precision map, its original data is composed of elements such as roads that intersect horizontally and vertically, traffic lights, etc. According to the starting point and the end point, a reasonable global path is planned through the A* algorithm according to certain rules, and the discrete points or feature points of the global path are obtained as a reference path for trajectory planning.

[0009] Furthermore, if the global path is obtained by pre-sampled longitude and latitude coordinates, the original data is the position data sampled at a certain time frequency, and the data point density is uneven, so the sampled position data needs to be preprocessed. In an equidistant manner, the longitude and latitude coordinates are extracted and converted into world coordinates (x, y) as a reference path for trajectory planning.

[0010] S2. Extraction and processing of local reference paths

[0011] The local path is a section of the global path. Since the global path is a discrete point, if the discrete points are directly interpolated, the path curvature, heading angle and other curve properties will change greatly. If the vehicle uses this as a reference path for local trajectory planning, the candidate trajectory will change dramatically, which is not conducive to the smooth driving of the vehicle. Therefore, it is necessary to perform spline fitting on the local reference path to make the local path smoother.

[0012] S3. Environmental perception and data fusion

[0013] The perception and fusion module, whose fused data is based on the vehicle coordinate system, and the local path or global path is based on the world coordinate system. Due to the difference in coordinate systems, the perception data and the local path or the global path cannot be mapped. It is necessary to convert the fused data based on the vehicle coordinate system into the fused data based on the world coordinate system so that the coordinate systems of the two data are unified to realize the mapping of perception and path.

[0014] S4. Local trajectory planning

[0015] The local path of spline fitting is used as the reference line to establish the Frenet coordinate system. In this coordinate system, trajectory planning is completed from the horizontal and vertical dimensions, with time as the variable, through polynomial curve interpolation, and the trajectory is used as a candidate trajectory.

[0016] S5. Perform kinematic verification, curvature verification, collision detection verification, boundary condition verification, etc. on the candidate trajectories to screen feasible trajectories.

[0017] S6. Calculate the cost of the feasible trajectory. The cost factors include: comfort, reference trajectory deviation, trajectory length, collision risk, traffic rules, etc.

[0018] S7. Select the optimal trajectory based on the principle of minimum cost.

[0019] S8. Optimal trajectory sampling and trajectory output

[0020] The initial discrete sampling of the optimal trajectory is performed at equal time intervals, and the distances between adjacent sampling points are uneven, which is not conducive to vehicle control. Therefore, the discrete points sampled at equal time intervals are sampled by spline quadratic equal spacing interpolation, and the position, curvature, heading angle, arc length and other information of the equal spacing sampling points are calculated and output to the vehicle control module, which selects appropriate preview points according to the actual situation based on the discrete sampling points to perform vehicle control.

[0021] The present invention also discloses a vehicle stopping method, which calculates the projection of the current position of the vehicle and the stop point on the reference path, and compares the relative positions of the projection points of the two. If the vehicle projection point is within the stop interval of the projection point of the stop point, the vehicle should perform a parking action.

[0022] The advantages of the present invention are:

[0023] 1. Perform equidistant interval extraction processing on the global path sampling points, which reduces the number of global path sampling points while ensuring the accuracy and effectiveness of the global path, thereby reducing the calculation amount of the trajectory planning system and improving the stability and robustness of the system.

[0024] 2. Perform spline fitting on the local path to make the local path smoother while ensuring that the error between the fitting path and the expected path is small, thus compensating for the reference path anomaly caused by the sensor and other irreversible errors.

[0025] 3. Considering cost factors that are more in line with driving habits, such as obstacle collision risk and traffic rules, the optimal trajectory planned will be more adaptable to the environment, such as avoiding obstacles / overtaking on the left, slowing down and passing close to obstacles, etc.

[0026] 4. By comparing the relative position of the vehicle and the projection point of the stopping point on the reference line in a projection manner, it can adapt to more parking scenarios, such as parking in the middle of driving, parking when the parking point cannot be reached due to obstacle avoidance, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 This is the overall process of the trajectory planning algorithm.

[0028] Figure 2 Schematic diagram of local path spline fitting processing.

[0029] Figure 3 Schematic diagram of obstacle conversion.

[0030] Figure 4 Schematic diagram of vehicle local path trajectory planning.

[0031] Figure 5 Flowchart for optimal trajectory confirmation.

[0032] Figure 6 This is a schematic diagram of a vehicle stopping. DETAILED DESCRIPTION

[0033] In order to make the technical means, creative features, objectives and effects achieved by the present invention easy to understand, the present invention is further explained below in conjunction with specific implementation methods.

[0034] Reference Figure 1 As shown, the present invention is an automatic driving trajectory planning method based on spline curves and polynomial curves, which features include: global path generation and processing, local reference path extraction and processing, environment perception and data fusion processing, local trajectory planning, optimal trajectory sampling and trajectory output, etc.

[0035] Before the trajectory planning task begins, it is necessary to obtain obstacle information output by the environment perception and fusion module. The hardware sensors included in the perception fusion module include cameras, millimeter-wave radars, ultrasonic radars, lidars, etc. The obstacle information detected by each of them is uniformly converted to the same coordinate system by the perception fusion module, usually the vehicle coordinate system, and outputs stable obstacle information.

[0036] Before starting the trajectory planning task, it is also necessary to obtain the vehicle positioning information, including the vehicle's position and posture, which is usually obtained by positioning methods such as GPS or SLAM. In addition, it is also necessary to obtain the global path that the vehicle needs to travel. The sources of the global path are mainly high-precision maps, SLAM maps, etc.

[0037] S1. Generation and processing of global paths

[0038] The paths from the high-precision map are saved in segments, and the vehicle cannot know how to choose the path. At this time, the A* algorithm is used to plan the shortest and most reasonable path based on the vehicle position and the destination position, and the global path is sampled. In addition, for the sampled map, since the sampling is based on equal time intervals, the amount of data is huge. In order to reduce the calculation amount of trajectory planning, sampling points are selected as the global path based on equal distance intervals.

[0039] S2. Extraction and processing of local reference paths

[0040] like Figure 2As shown in the figure, for the global path obtained from the sampling map, due to the noise interference of the sensor, there is a large error between the collected data points and the vehicle's driving position, and the error with the actual road centerline is even greater. If spline interpolation is performed directly on the sampling path, the curvature of the curve changes greatly, which is not conducive to vehicle driving. At this time, it is necessary to select interval segments for the sampled global path according to the vehicle position, and perform spline fitting on the interval sampling points. The fitted local path is closer to the vehicle's driving path and the curve is smoother. At the same time, after spline fitting, the reference line is discretized at equal intervals, and the position (x, y, s) and heading angle of the discrete points are calculated. and curvature

[0041] S3. Environmental perception and data fusion processing

[0042] like Figure 3 As shown in the figure, the obstacle information obtained from the perception fusion module is based on the description of the vehicle coordinate system, while the trajectory planning is based on the description of the world coordinate system. Therefore, the collision detection in trajectory planning requires the two coordinate systems to be combined. The position of the obstacle in the vehicle coordinate system ( v x ob , v y ob , v θ ob ), the position of the vehicle in the world coordinate system ( w x car , w y car , w θ car ), then their relationship is as follows:

[0043]

[0044] S4. Local trajectory planning

[0045] like Figure 4 As shown in FIG. 1 , the Frenet coordinate system of the local reference path is established with the arc length direction of the local reference path as the longitudinal direction and the left vertical direction of the local reference path as the transverse direction, and the position of the vehicle in the Frenet coordinate system (s car ,d car ); perform horizontal and vertical sampling on the target position of the vehicle, where the horizontal sampling interval sequence is (d n d n- 1...d0...d -n+1 d -n),n=1,2,3..., this sequence is a sequence with the local reference line as the symmetry center, the left side of the reference line is positive, and the right side is negative. The longitudinal sampling arc length is related to the prediction time. The time is sampled at equal intervals, and the time series is (T, T-ΔT, T-2ΔT, T-3ΔT,..., T-nΔT),n=0,1,2,3.... Based on this, the quintic polynomial trajectory planning is performed from the horizontal d and vertical s dimensions of the Frenet coordinate system to form candidate trajectories, which are numbered according to the horizontal and vertical sampling characteristics (lat_index, lon_index). The numbering rule is: lat_index is the horizontal number, with the reference line as the symmetry center, the left side is positive, and the right side is negative, such as: (-3, -2, -1, 0, 1, 2, 3); lon_index is the vertical number, and it is numbered from far to near as 0, 1, 2, 3.... The relationship between the longitudinal distance s, the lateral distance d, and the time t is:

[0046]

[0047] The candidate trajectories are sampled discretly at equal time intervals to form discrete points of the candidate trajectories.

[0048] In order to solve the coefficients a0~a5 and b0~b5, it is necessary to determine the initial conditions.

[0049] Furthermore, when solving a0~a5, the initial conditions are determined: the vehicle's current position condition (s0, s'0, s″0), and the target point position condition (s1, s′1, s″1). Among them, s=f(t) represents the longitudinal distance at time t; represents the longitudinal velocity at time t; represents the longitudinal acceleration at time t.

[0050] Furthermore, when solving b0~b5, the initial conditions are determined: the vehicle's current position condition (d0, d'0, d″0), and the target point position condition (d1, d′1, d″1). Among them, d=f(s), which represents the lateral offset of the vehicle at the local reference line s; represents the heading angle θ at the local reference line s; represents the curvature k at the local reference line s.

[0051] S5. Curve verification of candidate trajectories

[0052] Including curvature verification, velocity v verification, acceleration a verification, boundary d verification, collision detection verification, etc., the candidate trajectories that meet the conditions are saved in the feasible trajectory list.

[0053]

[0054] Where i,j represents the jth discrete point of the i-th candidate trajectory.

[0055] S6. Calculate the cost of the feasible trajectory

[0056] Factors to consider include: Comfort C comfort , driving efficiency C efficiency , center deviation, C offset Security C safty , traffic rules cost C left , so the total cost of the feasible trajectory is:

[0057] cost = k1C comfort +k2C efficiency +k3C offset +k4C safty +k5C left

[0058] Among them, k1 is the comfort coefficient, k2 is the driving efficiency, k3 is the center deviation coefficient, k4 is the safety factor, and k5 is the traffic rule cost coefficient;

[0059] Furthermore, the comfort cost is mainly reflected in the vehicle's acceleration, which reflects the rate of change of the vehicle's acceleration. Therefore, considering the lateral and longitudinal comfort from the Frenet coordinate system, it is described as the integral of the square of the vehicle's acceleration from the starting point t0 to the end point t1:

[0060]

[0061]

[0062] C comfort =K d C d +K s C s

[0063] Furthermore, the driving efficiency cost is mainly reflected in the integral of the error between the vehicle speed and the target speed, that is:

[0064]

[0065] Furthermore, the center deviation cost is mainly described as the degree of deviation between the target point of the vehicle and the center reference line.

[0066]

[0067] Furthermore, the safety cost is mainly described as the inverse of the minimum vertical distance between the feasible trajectory and the obstacle. The smaller the distance, the greater the collision risk, and the farther the distance, the smaller the collision risk.

[0068]

[0069] Furthermore, the traffic rule cost is mainly described as: when the vehicle avoids obstacles or changes lanes, it gives priority to the trajectory on the left side of the reference line.

[0070]

[0071] S7. When selecting a feasible trajectory, select the trajectory according to the principle of minimum cost.

[0072] S8. Optimal trajectory sampling and trajectory output

[0073] like Figure 5 As shown in the figure, due to reasons such as the stability of obstacle detection, the optimal trajectory may frequently jump, which will cause the vehicle to swing violently or even run off the road, posing a huge safety hazard. In order to avoid the negative impact caused by factors such as unstable obstacle detection, it is necessary to confirm the optimal trajectory. The method is: store the temporary optimal trajectory of M consecutive cycles, and count the number of times the same sequence number of the optimal trajectory appears in M ​​cycles. When the maximum number N is greater than the set threshold, the trajectory is considered stable and the optimal trajectory needs to be updated; otherwise, the trajectory is considered unstable, and the last optimal trajectory continues to be maintained.

[0074] The present invention also discloses a method for stopping a vehicle when it reaches a destination. Figure 6 As shown in the figure, when the stop point is within the local reference line interval, the arc length of the vehicle projection on the reference line is calculated as s 车 , the arc length of the projection of the stop point on the reference line is s 停 , when the condition is met

[0075]

[0076] It is considered that the vehicle has reached the stopping point area, and the vehicle should perform a predetermined action, such as parking.

[0077] It is known from common technical knowledge that the present invention can be implemented by other embodiments that do not deviate from its spirit or essential features. Therefore, the above disclosed embodiments are only illustrative in all respects and are not exclusive. All changes within the scope of the present invention or within the scope equivalent to the present invention are included in the present invention.

Claims

1. An automatic driving trajectory planning method based on spline curves and polynomial curves, characterized in that: The steps include: S1. Generation and processing of global paths Through the A* algorithm, according to the vehicle position and the destination position, the shortest and most reasonable path is planned, and the global path is sampled, and the sampling points are selected as the global path according to the equal distance interval; S2. Extraction and processing of local reference paths For the sampled global path, select interval segments according to the vehicle position, and perform spline fitting on the interval sampling points. The fitted local path is closer to the vehicle's driving path and the curve is smoother. After spline fitting, the reference line is discretized at equal intervals and the position (x, y, s) and heading angle of the discrete points are calculated. and curvature S3. Environmental perception and data fusion processing The obstacle information obtained from the perception fusion module is based on the vehicle coordinate system, while the trajectory planning is based on the world coordinate system. Therefore, the collision detection in trajectory planning requires the two coordinate systems to be combined. The position of the obstacle in the vehicle coordinate system ( v x ob , v y ob , v θ ob ), the position of the vehicle in the world coordinate system ( w x car , w y car , w θ car ), then their relationship is as follows: S4. Local trajectory planning The Frenet coordinate system of the local reference path is established with the arc length direction of the local reference path as the longitudinal direction and the left vertical direction of the local reference path as the transverse direction, and the position of the vehicle in the Frenet coordinate system (s car ,d car ); Perform horizontal and vertical sampling on the target position of the vehicle: The horizontal sampling interval sequence is (d n d n-1 ...d0...d -n+1 d -n ),n=1,2,3..., this sequence is a sequence with the local reference line as the symmetry center, the left side of the reference line is positive and the right side is negative; The arc length of the longitudinal sampling is related to the prediction time. If the time is sampled at equal intervals, the time series is (T, T-ΔT, T-2ΔT, T-3ΔT, ..., T-nΔT), n = 0, 1, 2, 3 ...; Based on this, quintic polynomial trajectory planning is performed from the horizontal d and vertical s dimensions of the Frenet coordinate system to form candidate trajectories, which are numbered according to the horizontal and vertical sampling characteristics. The labeling rule is: lat_index is the horizontal number, with the reference line as the symmetry center, the left is positive and the right is negative; lon_index is the vertical number, numbered 0, 1, 2, 3... from far to near; The relationship between the longitudinal distance s, the lateral distance d, and the time t is: The candidate trajectories are sampled at equal time intervals to form discrete points of the candidate trajectories. In order to solve the coefficients a0~a5 and b0~b5, the initial conditions need to be determined, as follows: When solving a0~a5, determine the initial conditions: the vehicle's current position condition (s0, s'0, s"0), the target point position condition (s1, s1', s1"), where s = f(t) represents the longitudinal distance at time t; represents the longitudinal velocity at time t; represents the longitudinal acceleration at time t; When solving b0~b5, determine the initial conditions: the vehicle's current position condition (d0, d'0, d'0'), the target point position condition (d1, d1', d1'), where d = f(s), which represents the lateral offset of the vehicle at the local reference line s; represents the heading angle θ at the local reference line s; represents the curvature k at the local reference line s; The candidate trajectories are sampled at equal time intervals to form discrete points of the candidate trajectories; S5. Candidate trajectory verification Perform curve verification on candidate trajectories, including curvature verification, velocity verification, acceleration verification, boundary verification, and collision detection verification, and save the candidate trajectories that meet the conditions into the feasible trajectory list: Among them, i,j represents the jth discrete point of the i-th candidate trajectory; S6. Cost calculation of feasible trajectories Calculate the cost of a feasible trajectory, taking into account the following factors: comfort level C comfort , driving efficiency C efficiency , center deviation C offset 、Safety C safty and the traffic rules cost C left , so the total cost of the feasible trajectory is: cost=k1C comfort +k2C efficiency +k3C offset +k4C safty +k5C left Among them, k1 is the comfort coefficient, k2 is the driving efficiency, k3 is the center deviation coefficient, k4 is the safety factor, and k5 is the traffic rule cost coefficient; S7. Optimal trajectory selection When choosing feasible trajectories, select the optimal trajectory according to the principle of minimum cost; S8. Optimal trajectory sampling and trajectory output The temporary optimal trajectory of M consecutive cycles is stored, and the number of times the same sequence number of the optimal trajectory appears in M ​​cycles is counted. When the maximum number N is greater than the set threshold, the trajectory is considered stable and the optimal trajectory needs to be updated; otherwise, the trajectory is considered unstable and the vehicle continues to drive on the last optimal trajectory.

2. The automatic driving trajectory planning method based on spline curves and polynomial curves according to claim 1, characterized in that: Before the trajectory planning task begins, it is necessary to obtain obstacle information output by the environment perception and fusion module. The hardware sensors included in the perception fusion module include but are not limited to cameras, millimeter-wave radars, ultrasonic radars, and lidars. The obstacle information detected by the above hardware sensors is uniformly converted to the same coordinate system by the perception fusion module, and stable obstacle information is output.

3. The automatic driving trajectory planning method based on spline curves and polynomial curves according to claim 2, characterized in that: The coordinate system is a vehicle coordinate system.

4. The automatic driving trajectory planning method based on spline curves and polynomial curves according to claim 1, characterized in that: Before the trajectory planning task begins, it is also necessary to obtain vehicle positioning information, including but not limited to the vehicle's position and posture. The above vehicle positioning information is obtained by GPS or SLAM positioning. In addition, it is also necessary to obtain the global path that the vehicle needs to travel. The source of the global path includes but is not limited to high-precision maps and SLAM maps.

5. The automatic driving trajectory planning method based on spline curves and polynomial curves according to claim 1, characterized in that: The comfort cost is mainly reflected in the vehicle's acceleration, which reflects the rate of change of the vehicle's acceleration. Therefore, considering the lateral and longitudinal comfort from the Frenet coordinate system, it is described as the integral of the square of the vehicle's acceleration from the starting point t0 to the end point t1:

6. The automatic driving trajectory planning method based on spline curves and polynomial curves according to claim 1, characterized in that: The driving efficiency cost is mainly reflected in the error integral between the vehicle speed and the target speed, that is:

7. The automatic driving trajectory planning method based on spline curves and polynomial curves according to claim 1, characterized in that: The center deviation cost is mainly described as the degree of deviation between the target point of the vehicle and the center reference line:

8. The automatic driving trajectory planning method based on spline curves and polynomial curves according to claim 1, characterized in that: The safety cost is mainly described as the inverse of the minimum vertical distance between the feasible trajectory and the obstacle. The smaller the distance, the greater the collision risk, and the farther the distance, the smaller the collision risk:

9. The automatic driving trajectory planning method based on spline curves and polynomial curves according to claim 1, characterized in that: The traffic rule cost is mainly described as giving priority to the track on the left side of the reference line when the vehicle avoids obstacles or changes lanes:

10. A method for determining whether a vehicle has reached a destination and stopped using the automatic driving trajectory planning method of claim 1, characterized in that: When the stop point is within the local reference line interval, the arc length of the vehicle projection on the reference line is calculated as s. 车 , the arc length of the projection of the stop point on the reference line is s 停 , when the conditions are met: It is considered that the vehicle has reached the stopping point area, and the vehicle should perform a predetermined parking action at this time.

Images