A vehicle obstacle avoidance trajectory planning method, system, vehicle and storage medium

Abstract

The application relates to the technical field of automatic driving of vehicles, and provides a vehicle obstacle avoidance trajectory planning method, a vehicle obstacle avoidance trajectory planning system, a vehicle and a storage medium.The method comprises the following steps: obtaining coordinate point information of a lane line and coordinate point information of a lane center line according to vehicle positioning information; taking the lane center line of the lane where the vehicle is located as a reference line, and converting the position, heading, speed and acceleration of the vehicle into a longitudinal position, a lateral offset, a lateral speed, a longitudinal speed, a lateral acceleration and a longitudinal acceleration in a frenet coordinate system; taking obstacle information, if there is an obstacle, converting the obstacle coordinates into S obs ,L obs , if there are multiple obstacles, converting the coordinates of each obstacle into S obsi ,L obsi , and calculating a target point through dynamic programming by means of point sampling; converting the target point into coordinates in a natural coordinate system, smoothing the coordinates through a smoothing algorithm to obtain a trajectory point set, and fitting the trajectory point set to generate a trajectory equation. The application has a simple calculation process and lower cost.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving of vehicles, in particular to a vehicle obstacle avoidance trajectory planning method and system, a vehicle and a storage medium. BACKGROUND

[0002] With the rise of artificial intelligence technology, the motion trajectory planning problem taking the automatic driving vehicle as the research object is paid more and more attention, and the obstacle avoidance trajectory planning is a key part of the automatic driving vehicle, which has great significance for the research of the automatic driving vehicle. In the driving process of the automatic driving vehicle, accurately and safely avoiding obstacles in real time not only can improve the safety of the intelligent vehicle, but also can improve the travel efficiency to a certain extent, so the obstacle avoidance problem of the intelligent vehicle has become a research hotspot.

[0003] In the lane changing planning, the mainstream planning methods currently include the search method and the sampling method. The patent with the application publication number CN114194215A discloses an intelligent vehicle obstacle avoidance lane changing trajectory planning method and system. According to the road environment, obstacle information and current vehicle speed, a feasible trajectory cluster is generated based on the stability limit starting return point and the polynomial curve. Pre-collision detection is performed on the lane changing trajectories in the feasible trajectory cluster, and trajectories that do not meet the pre-collision detection conditions are screened and removed to obtain a collision-free trajectory cluster. On the basis of the collision-free trajectory cluster, a cost function about the three evaluation indexes of comfort, obstacle avoidance efficiency and sideslip is established. Fuzzy reasoning is used to determine the weights of the three evaluation indexes, and the optimal obstacle avoidance lane changing return point is searched by solving the multi-objective optimization problem, that is, the final obstacle avoidance lane changing trajectory is planned.

[0004] The technical solution of the above patent document can maximize the performance indexes of the trajectory such as comfort and obstacle avoidance efficiency while taking into account safety, real-time performance and curvature continuity, and effectively improve the riding experience of the passengers. However, the evaluation indexes focus on comfort, obstacle avoidance efficiency and sideslip, the calculation of the vehicle obstacle avoidance method is complex, the cost is high, and it is difficult to be commercialized. SUMMARY

[0005] Therefore, the purpose of the embodiments of the present application is to provide a vehicle obstacle avoidance trajectory planning method and system, a vehicle and a storage medium, which can more simply plan the vehicle obstacle avoidance trajectory, the whole calculation process is simple, the cost is lower, and it is easy to be commercialized.

[0006] To achieve the above technical purposes, the technical solutions adopted by the present application are as follows:

[0007] In a first aspect, the present application provides a vehicle obstacle avoidance trajectory planning method, comprising the following steps,

[0008] S1, obtaining vehicle positioning information, and obtaining coordinate point information of a lane line natural coordinate system and coordinate point information of a lane center line natural coordinate system in a high-precision map according to the vehicle positioning information;

[0009] S2, taking the lane center line of the lane where the vehicle is located as a reference line, converting the vehicle position, heading, speed, and acceleration into a longitudinal position s, a lateral offset l, a lateral speed a longitudinal speed l', a lateral acceleration and a longitudinal acceleration l" in a frenet coordinate system s, l;

[0010] S3, obtaining obstacle information, if there is no obstacle, the vehicle travels according to the reference line; if there is an obstacle, converting the obstacle coordinates into S obs ,L obs in the frenet coordinate system, if there are multiple obstacles, converting each obstacle coordinate into S obsi ,L obsi in the frenet coordinate system, and calculating a target point through dynamic programming by sampling;

[0011] S4, converting the calculated target point into coordinates in the natural coordinate system through coordinate transformation, and then smoothing through a smoothing algorithm to obtain a trajectory point set;

[0012] S5, fitting the trajectory point set to generate a trajectory equation.

[0013] Further, the taking the lane center line of the lane where the vehicle is located as a reference line comprises: determining the lane where the vehicle is located according to the coordinates and heading of the vehicle, then smoothing the center line of the lane where the vehicle is located to serve as the reference line; and converting reference line point coordinates (x i ,y i ) in the natural coordinate system into (s i ,l i ) in the frenet coordinate system.

[0014] Further, the method of converting the vehicle position, heading, speed, and acceleration into a longitudinal position s, a lateral offset l, a lateral speed a longitudinal speed l', a lateral acceleration and a longitudinal acceleration l" in the frenet coordinate system s, l comprises:

[0015] s=s r

[0016]

[0017]

[0018]

[0019] l' = (1 - k r l) tan (θ x - θ r )

[0020]

[0021] where subscript x represents the ego vehicle, subscript r represents the projection of the ego vehicle onto the reference line, k represents the curvature, θ represents the heading angle, v represents the speed, and a represents the acceleration.

[0022] Further, the target point calculated by the dynamic programming through the sampling by scattering points comprises: obtaining the five polynomial coefficients connecting the two sampling points through the longitudinal position, lateral offset, longitudinal and lateral velocity, longitudinal and lateral acceleration of the two sampling points, and then obtaining the cost function of the five polynomial curve between the two sampling points; the cost function is the distance cost from the reference line, or the lateral velocity cost, or the lateral acceleration cost, or the lateral jerk cost, or the sum of any one of the distance cost from the reference line, the lateral velocity cost, the lateral acceleration cost, and the lateral jerk cost and the danger cost of the static obstacle.

[0023] Further, the danger cost of the static obstacle is negatively correlated with the distance of the static obstacle, when the distance from the ego vehicle to the static obstacle is less than limitdown, it is considered that a collision will occur, and the cost function is set to infinity; when the distance from the ego vehicle to the static obstacle is greater than limitup, it is considered that no collision will occur, and the cost function is 0, when the distance from the ego vehicle to the static obstacle is greater than limitdown and less than limitup, the cost function is negatively correlated with the distance from the ego vehicle to the static obstacle; wherein limitdown represents the lower limit threshold of the judgment of considering danger, and limitup represents the upper limit threshold of the judgment of considering danger.

[0024] Further, the target point calculated is converted into coordinates in the natural coordinate system through coordinate transformation, and then the trajectory point set is obtained through a smoothing algorithm, comprising: converting the target point calculated in the frenet coordinate system into coordinates in the natural coordinate system through coordinate transformation:

[0025] x x = x r - l sin (θ r )

[0026] y x = y r + l cos (θ r )

[0027] Wherein, subscript x represents a sampling point, subscript r represents a projection point of the sampling point on a reference line, and theta represents a heading angle.

[0028] Then, the trajectory point set is smoothed by a smoothing algorithm.

[0029] Further, the fitting of the trajectory point set to generate a trajectory equation comprises: fitting the smoothed trajectory point set by a least square method to generate a 3rd order polynomial trajectory equation.

[0030] In a second aspect, the application further discloses an automatic driving system, which uses the vehicle obstacle avoidance trajectory planning method.

[0031] In a third aspect, the application further discloses a vehicle, which comprises a vehicle body and the automatic driving system.

[0032] In a fourth aspect, the application further discloses a computer readable storage medium, which stores a computer program, and when the computer program runs on a computer, the computer is caused to execute the method.

[0033] The application with the above technical solution has the following advantages:

[0034] The application establishes a frenet coordinate system by taking the smoothed center line as a reference line, projects the static obstacle into the frenet coordinate system, calculates appropriate sampling points after sampling, converts the sampling point coordinates selected in the frenet coordinate system to a natural coordinate system and performs smoothing processing, and finally generates a reference trajectory equation by fitting the sampling points after smoothing processing by a least square method. BRIEF DESCRIPTION OF DRAWINGS

[0035] The application can be further illustrated by the non-limiting embodiments shown in the drawings. It should be understood that the following drawings only show some embodiments of the application, and therefore should not be considered as limiting the scope, and other related drawings can be obtained by those skilled in the art without creative labor.

[0036] Figure 1 The application is a vehicle obstacle avoidance trajectory planning method.

[0037] Figure 2 The application is a vehicle obstacle avoidance trajectory planning method. DETAILED DESCRIPTION

[0038] The present application will be described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that similar or identical parts are denoted by the same reference numerals in the accompanying drawings or description, and the implementation not shown or described in the accompanying drawings is known to those skilled in the art. In the description of the present application, the terms "first", "second", etc. are only used to distinguish the description and cannot be understood as indicating or implying relative importance.

[0039] Embodiment 1,

[0040] The present embodiment is a vehicle obstacle avoidance trajectory planning method, as shown in the following steps: Figure 1

[0041] S1, obtaining vehicle positioning information, and obtaining coordinate point information of a lane line natural coordinate system and coordinate point information of a lane center line natural coordinate system in a high-precision map according to the vehicle positioning information.

[0042] S2, determining the lane where the vehicle is located according to the coordinates and heading of the vehicle, and then smoothing the center line of the lane as a reference line. Convert the reference line point coordinates (x i ,y i ) in the natural coordinate system into (s i ,l i ) in the frenet coordinate system.

[0043] s1=0;l i =0,i=1,2,…

[0044]

[0045] Since the frenet coordinate system is established with reference to the reference line, the lateral offset l i of the reference line point is 0.

[0046] Convert the vehicle position, heading, speed, and acceleration into longitudinal position (s), lateral offset (l), lateral and longitudinal speed (l'), and lateral and longitudinal acceleration (l") in the frenet coordinate system (S, L).

[0047] s=s r

[0048]

[0049]

[0050]

[0051] l'=(1-k r l)tan(θ​​​x -θ r )

[0052]

[0053] where subscript x represents the ego vehicle, subscript r represents the projection of the ego vehicle on the reference line, k represents the curvature, θ represents the heading angle, v represents the speed, and a represents the acceleration.

[0054] S3, obtain the obstacle information according to the sensor data. When there is no obstacle, the vehicle travels according to the reference line; when there is an obstacle, the obstacle coordinates are converted into S obs ,L obs . If there are multiple obstacles, the coordinates of each obstacle are converted into S obsi ,L obsi . According to the reference line, the sampling points are scattered, and the target point is calculated via dynamic programming, as shown in Figure 2 . The curve in the figure represents the reference line, and the A points are sampling points. The star-shaped point pointed to by B represents the obstacle position, and the A point selected by the circle pointed to by C is the final calculated target sampling point. During dynamic programming calculation, the longitudinal position, lateral offset, longitudinal and lateral velocities, longitudinal and lateral accelerations of the two sampling points can be used to obtain the quintic polynomial coefficients connecting the two sampling points, and then the cost function of the quintic polynomial curve between the two sampling points can be obtained. The cost function can be represented as the distance cost (l) from the reference line, the lateral velocity cost (l'), the lateral acceleration cost (l”), the lateral jerk cost (l”'), and the danger cost and with the static obstacle. The danger cost with the static obstacle is negatively correlated with the distance from the static obstacle. For example:

[0055]

[0056] When the distance from the obstacle is less than limitdown, it is considered to collide, and the cost function can be set to infinity, which can be set to a very large number in actual operation. When the distance from the obstacle is greater than limitup, it is considered not to collide, and the cost function is 0. When the distance is between the two, the cost is negatively correlated with the distance, where limitdown represents the lower threshold for judging the existence of danger, and limitup represents the upper threshold for judging the existence of danger.

[0057] S4, the target point calculated in the frenet coordinate system (as shown in Figure 2 ) is converted into the coordinates in the natural coordinate system through coordinate transformation.

[0058] x x =x r -lsin(θ r)

[0059] y x =y r +lcos(θ r )

[0060] The subscript x in the formula represents a sampling point, and the subscript r represents a projection point of the sampling point on a reference line. Then, the trajectory point set is smoothed by a smoothing algorithm. In actual calculation, the smoothing algorithm can use a quadratic programming, a B-spline method, etc.

[0061] S5, finally, the least square method is used to fit the smoothed trajectory point set to generate a cubic polynomial trajectory equation.

[0062] Embodiment 2,

[0063] The embodiment is an automatic driving system, which uses the vehicle obstacle avoidance trajectory planning method of embodiment 1. The automatic driving system of the embodiment establishes a frenet coordinate system by using the lane information and the lane center line information of a high-precision map, and taking the smoothed center line as a reference line. Meanwhile, the static obstacles are projected into the frenet coordinate system, and the appropriate sampling points are calculated by dynamic programming after the sampling. The sampling point coordinates selected in the frenet coordinate system are converted into the natural coordinate system and smoothed. Finally, the least square method is used to fit the smoothed sampling points to generate a reference trajectory equation. The whole calculation process is simple, the cost is lower, and it is easy to be applied in commercialization.

[0064] Embodiment 3,

[0065] The embodiment is a vehicle, which comprises a vehicle body and the automatic driving system of embodiment 2, and the automatic driving system is mounted on the vehicle.

[0066] Embodiment 4,

[0067] The embodiment is a computer readable storage medium, which stores a computer program. When the computer program runs on a computer, the computer executes the above method. Through the description of the above embodiments, those skilled in the art can clearly understand that the present application can be implemented by hardware, or by means of software and necessary general hardware platform. Based on this understanding, the technical solution of the present application can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (such as CD-ROM, U disk, mobile hard disk, etc.), and includes a plurality of instructions for making a computer device (such as a personal computer, a braking device, or a network device, etc.) execute the method described in each embodiment of the present application.

[0068] In the embodiments of the present disclosure, it should be understood that the disclosed apparatus, system and method can also be implemented in other ways. The apparatus, system and method embodiments described above are only illustrative, for example, the flowcharts and block diagrams in the drawings show the possible implementation architectures, functions and operations of the system, method and computer program product according to the embodiments of the present disclosure. In this regard, each block in the flowchart or block diagram can represent a module, a program segment or a part of code, which contains one or more executable instructions for implementing the specified logical function. It should also be noted that each block in the block diagram and / or flowchart, and the combination of blocks in the block diagram and / or flowchart, can be implemented by a special-purpose hardware-based system for implementing the specified functions or actions, or can be implemented by a combination of special-purpose hardware and computer instructions. In addition, the functional modules in the various embodiments of the present disclosure can be integrated together to form a separate part, or each module can exist independently, or two or more modules can be integrated to form a separate part.

[0069] The above only describes the embodiments of the present disclosure and is not used to limit the protection scope of the present disclosure. For those skilled in the art, the present disclosure can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present disclosure shall be included in the protection scope of the present disclosure.

Claims

1. A method for planning vehicle obstacle avoidance trajectory, characterized in that: Includes the following steps, S1. Obtain vehicle positioning information, and based on the vehicle positioning information, obtain the coordinate point information of the lane line natural coordinate system and the coordinate point information of the lane center line natural coordinate system in the high-precision map. S2. Using the center line of the lane where the vehicle is located as a reference line, convert the vehicle's position, heading, speed, and acceleration into the longitudinal position s, lateral offset l, and lateral velocity in the Frenet coordinate system s,l. Longitudinal velocity l', lateral acceleration and longitudinal acceleration l”; S3. Obtain obstacle information. If there are no obstacles, the vehicle travels according to the reference line; if there are obstacles, convert the obstacle coordinates into S in the Frenet coordinate system. obs ,L obs If multiple obstacles exist, the coordinates of each obstacle are converted to S in the Frenet coordinate system. obsi ,L obsi The target sampling points are calculated through dynamic programming by scattering sampling points; The process of calculating the target sampling point through dynamic programming by sampling points includes: obtaining the coefficients of the fifth-order polynomial connecting the two sampling points by the longitudinal position, lateral offset, longitudinal and lateral velocity, and longitudinal and lateral acceleration of the two sampling points, and then obtaining the cost function of the fifth-order polynomial curve between the two sampling points. S4. The calculated target sampling points are transformed into coordinates in the natural coordinate system through coordinate transformation, and then smoothed using a smoothing algorithm to obtain the trajectory point set; S5. Fit the set of trajectory points to generate a trajectory equation.

2. The vehicle obstacle avoidance trajectory planning method according to claim 1, characterized in that: Using the center line of the lane where the vehicle is located as a reference line includes: determining the lane where the vehicle is located based on the vehicle's coordinates and heading, and then smoothing the center line of the lane where the vehicle is located to use as the reference line; and then using the coordinates (x, y, z) of the reference line point in the natural coordinate system. i ,y i Convert ) to Frenet coordinate system (s) i ,l i ).

3. The vehicle obstacle avoidance trajectory planning method according to claim 2, characterized in that: The vehicle's position, heading, speed, and acceleration are converted into the longitudinal position s, lateral offset l, and lateral velocity in the Frenet coordinate system s, l. Longitudinal velocity l', lateral acceleration "and longitudinal acceleration l", including: s=s r l'=(1-k r l)tan(θ x -θ r ) In this context, the subscript x represents the vehicle itself, the subscript r represents the projection point of the vehicle on the reference line, k represents curvature, θ represents heading angle, v represents velocity, and a represents acceleration.

4. The vehicle obstacle avoidance trajectory planning method according to claim 3, characterized in that: The cost function is the sum of the distance cost from the reference line, or the lateral velocity cost, or the lateral acceleration cost, or the lateral jerk cost, or any one of the distance cost from the reference line, the lateral velocity cost, the lateral acceleration cost, and the lateral jerk cost, and the danger cost of the static obstacle.

5. The vehicle obstacle avoidance trajectory planning method according to claim 4, characterized in that: The danger cost of a static obstacle is negatively correlated with the distance to the obstacle. When the distance from the vehicle to the static obstacle is less than limitdown, a collision is considered to occur, and the cost function is set to infinity. When the distance from the vehicle to the static obstacle is greater than limitup, a collision will not occur, and the cost function is 0. When the distance from the vehicle to the static obstacle is greater than limitdown but less than limitup, the cost function is negatively correlated with the distance from the vehicle to the static obstacle. Here, limitdown represents the lower threshold for judging whether there is a danger, and limitup represents the upper threshold for judging whether there is a danger.

6. The vehicle obstacle avoidance trajectory planning method according to claim 5, characterized in that: The process of converting the calculated target sampling points into coordinates in the natural coordinate system through coordinate transformation, and then smoothing them using a smoothing algorithm to obtain a trajectory point set, includes: The target sampling points calculated in the Frenet coordinate system will be transformed into coordinates in the natural coordinate system through coordinate transformation: x x =x r -lsin(θ r ) y x =y r +lcos(θ r ) Where the subscript x represents the sampling point, the subscript r represents the projection of the sampling point onto the reference line, and θ represents the heading angle; Then, a smoothing algorithm is used to obtain the trajectory point set.

7. The vehicle obstacle avoidance trajectory planning method according to claim 5, characterized in that: The process of fitting the trajectory point set to generate a trajectory equation includes: The least squares method is used to fit the smoothed trajectory point set to generate a cubic polynomial trajectory equation.

8. An autonomous driving system, characterized in that: The autonomous driving system uses a vehicle obstacle avoidance trajectory planning method according to any one of claims 1-7.

9. A vehicle, characterized in that: The vehicle includes a vehicle body and the autonomous driving system of claim 8, wherein the autonomous driving system is mounted on the vehicle body.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program that, when run on a computer, causes the computer to execute a vehicle obstacle avoidance trajectory planning method according to any one of claims 1-7.

Images