Method for obtaining curvature of control point of planned trajectory of vehicle and smoothing method

Abstract

A method for obtaining a curvature of a control point of a planned trajectory of a vehicle and a smoothing method. On the basis of a control point and a point at which a vehicle is located, a corresponding interval point between the two points on a trajectory may be obtained. Then, whether a curvature of the trajectory from the point at which the vehicle is located to the control point changes may be determined on the basis of whether the three points are collinear. When the curvature changes, the curvature of the circumscribed circle of a corresponding triangle is calculated on the basis of coordinates of the three points, that is, the curvature of the circumscribed circle is used as a real-time curvature of the planned trajectory at the control point when the vehicle moves from a reached point to the control point. As the vehicle travels, curvatures of subsequent control points may be obtained by repeating the foregoing, so that an end point at which the curvature of a previous section of the planned trajectory changes is used as a start point at which the curvature of a next section of the planned trajectory changes. In this way, a plurality of sections of the planned trajectory with continuously changing curvatures can be smoothly connected, thereby finally smoothing the planned trajectory. The processing manner ensures continuity of the curvatures of the control points, has a small calculation amount, and is real-time.

Description

A control point curvature acquisition method and smoothing method for vehicle planning trajectory Technical Field

[0001] The present application relates to the technical field of intelligent vehicle path planning, and in particular to a method for obtaining and smoothing the curvature of control points of a vehicle planning trajectory. Background Art

[0002] Smart cars are attracting increasing attention due to their potential to improve road safety, economy, and travel convenience. Path planning for smart cars is a key area of ​​research in the decision neighborhood, responsible for planning a safe and feasible driving path for smart cars. Under the conditions of satisfying nonholonomic constraints, smart cars can use path planning algorithms to obtain a series of discrete path points in a configuration space containing obstacle information and drivable space. These path points meet the vehicle's kinematic constraints and collision-free requirements. A key issue in path planning is how to use path point fitting methods to curve fit these discrete path points into a continuous path that meets the vehicle's nonholonomic constraints and avoids collisions with obstacles in space.

[0003] In related technologies, control point trajectory smoothing methods mainly include two algorithms: curve interpolation and sampling-based search. Curve interpolation is a mathematical method that creates a smooth curve by calculating the curve between control points. Sampling-based search algorithms mainly include Dijkstra and A* algorithms to solve trajectory planning problems.

[0004] The first type of method is based on curve interpolation: straight lines and circular arcs are simple to implement and require low computational complexity, but the curvature is discontinuous; polynomial curves need to be of order four or above to ensure curvature continuity, but the determination of parameters is more difficult; clothoid curve integration is time-consuming and the curve is continuous but not smooth, and depends on trajectory points; the calculation result of spline curves may not be the optimal solution.

[0005] The second type of method is based on sampling search: the Dijkstra algorithm does not meet the real-time requirements, the calculation results are discontinuous, and the amount of calculation is large; the search step size of the A* algorithm is difficult to determine, and it is inefficient in complex and large-scale planning environments.

[0006] Summary of the Invention

[0007] The embodiments of the present application provide a method for obtaining and smoothing the curvature of the control points of a planned vehicle trajectory, so as to solve the problem in the related art that when the control point trajectory smoothing method adopts curve interpolation, the polynomial curve needs to be of the fourth order or higher to ensure the curvature continuity, resulting in a large amount of calculation.

[0008] In a first aspect, a method for obtaining curvature of control points of a vehicle planning trajectory is provided, comprising:

[0009] Determine whether the vehicle's location, current control point, and interval point are collinear;

[0010] If they are not collinear, the curvature of the circle formed by the vehicle point, the current control point, and the interval point is obtained; the curvature is used as the curvature of the current control point; otherwise, the curvature of the current control point is recorded as zero.

[0011] In some embodiments, determining whether the vehicle location, the current control point, and the interval point are collinear specifically includes the following steps:

[0012] A judgment value is obtained based on the vehicle location, the current control point and the interval point, and the mapping relationship;

[0013] If the judgment value is equal to zero, the three points are collinear;

[0014] If the judgment value is greater than zero, the three points are not collinear and the curvature is negative;

[0015] If the judgment value is less than zero, the three points are not collinear and the curvature is positive.

[0016] In some embodiments, before determining whether the vehicle location, the current control point, and the interval point are collinear, the following steps are further included:

[0017] Obtain the planned trajectory and convert it into a driving line in the SL coordinate system; in the SL coordinate system, the S axis is the centerline of the road; the L axis and the S axis are in the same plane and perpendicular to the S axis;

[0018] The point where the driving line intersects the S axis is used as the control point;

[0019] Any point between the current control point and the vehicle's location on the vehicle trajectory is taken as an interval point.

[0020] In some embodiments, before determining whether the vehicle location, the current control point, and the interval point are collinear, the following steps are further included:

[0021] Obtain the planned trajectory and convert it into a driving line in the SL coordinate system; in the SL coordinate system, the S axis is the centerline of the road; the L axis and the S axis are in the same plane and perpendicular to the S axis;

[0022] The point where the driving line intersects the S axis is used as the control point;

[0023] The midpoint between the current control point and the vehicle's location on the vehicle trajectory is taken as the interval point.

[0024] In some embodiments, when the road on which the vehicle is located is a one-way road, the S axis is the center line of the road in the longitudinal direction;

[0025] When the road where the vehicle is located is a two-way multi-lane road, the S axis is the center line of the lane where the vehicle is located in the length direction.

[0026] In some embodiments, obtaining the curvature of a circle formed by the vehicle's location, the current control point, and the interval point includes the following steps:

[0027] Get the distance between the vehicle's location and the current control point, and record it as the side length one;

[0028] Get the distance between the vehicle's location and the interval point, and record it as the length of the side 2;

[0029] Get the distance between the current control point and the interval point, and record it as the length of the side three;

[0030] Determine whether side lengths one, two, and three can form a triangle;

[0031] If so, then side lengths 1, 2, and 3 will form a triangle, and the curvature of the circumscribed circle of the triangle can be calculated by combining mapping relationship 2;

[0032] Otherwise, the curvature is recorded as zero.

[0033] In some embodiments, determining whether side length one, side length two, and side length three can form a triangle comprises the following steps:

[0034] If the sum of any two sides of side 1, side 2, and side 3 is greater than the remaining side, then a triangle can be formed;

[0035] If the length of any of the sides 1, 2, and 3 is zero, then a triangle cannot be formed.

[0036] In a second aspect, a vehicle planning trajectory control point curvature acquisition system is provided, which includes:

[0037] The first module is used to determine whether the vehicle location, the current control point and the interval point are collinear;

[0038] The second module is used to obtain the curvature of the circumscribed circle corresponding to the three points based on the vehicle point, the current control point and the interval point when they are not collinear; the curvature is used as the curvature of the current control point; when they are collinear, the curvature of the current control point is recorded as zero.

[0039] In a third aspect, a method for smoothing a vehicle planning trajectory is provided, comprising:

[0040] When the vehicle needs to travel to the current control point, the curvature of the current control point is obtained according to the control point curvature acquisition method of the vehicle planning trajectory;

[0041] When the vehicle reaches the current control point and is about to travel to the next control point, the coordinates of the vehicle's location are replaced with the coordinates of the current control point, and the coordinates of the current control point are replaced with the coordinates of the next control point; then, the curvature of the next control point is obtained according to the control point curvature acquisition method of the vehicle's planned trajectory;

[0042] Repeat the above steps to obtain the curvature of all control points, and smooth the vehicle planning trajectory based on the obtained curvature of all control points.

[0043] In a fourth aspect, a vehicle trajectory smoothing system is provided, comprising:

[0044] The third module is used to obtain the curvature of the current control point according to the control point curvature acquisition method of the vehicle planned trajectory when the vehicle needs to travel to the current control point;

[0045] The fourth module is configured to replace the coordinates of the vehicle's location with the coordinates of the current control point and the coordinates of the current control point with the coordinates of the next control point when the vehicle reaches the current control point and is about to travel to the next control point; and then obtain the curvature of the next control point according to the control point curvature acquisition method of the vehicle's planned trajectory;

[0046] The fifth module is used to obtain the curvatures of all control points according to the third module and the fourth module, and to smooth the vehicle planning trajectory according to the obtained curvatures of all control points.

[0047] The beneficial effects of the technical solution provided by this application include:

[0048] An embodiment of the present application provides a method for obtaining and smoothing the curvature of the control points of a planned vehicle trajectory. Since the interval points between the control points and the vehicle's location can be obtained based on the two points, whether the curvature of the trajectory between the vehicle's location and the control point has changed can be determined based on whether the three points are collinear. When it has changed, the curvature of the circumscribed circle of the corresponding triangle is calculated based on the coordinates of the three points, that is, the curvature of the circumscribed circle is used as the real-time curvature of the planned trajectory at the control point when the vehicle arrives at the control point. As the vehicle travels, the above method is repeated to obtain the curvature of subsequent control points, so that the end point where the curvature of the previous planned trajectory changes is used as the starting point where the curvature of the next planned trajectory changes, so that multiple planned trajectories with continuously changing curvatures can be smoothly connected, thereby ultimately achieving smooth processing of the planned trajectory. This processing method ensures the continuity of the curvature of the control points, has a small amount of calculation, and is real-time. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0050] FIG1 is a general flow chart of a method for obtaining curvature of control points of a vehicle planning trajectory provided by an embodiment of the present application;

[0051] FIG2 is a specific flow chart of a method for obtaining the curvature of control points of a vehicle planning trajectory provided in an embodiment of the present application. DETAILED DESCRIPTION

[0052] To make the purpose, technical solutions, and advantages of the embodiments of this application more clear, the technical solutions in the embodiments of this application will be clearly and completely described below in conjunction with the drawings in the embodiments of this application. Obviously, the described embodiments are part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0053] The embodiments of the present application provide a method for obtaining and smoothing the curvature of the control points of a planned vehicle trajectory to solve the problem in related technologies that when the control point trajectory smoothing method uses curve interpolation, the polynomial curve needs to be of order four or above to ensure curvature continuity, resulting in a large amount of calculation.

[0054] First of all, in this application, we should understand the meaning of control points. Normally, there are the following situations in vehicle trajectory planning: the vehicle changes direction from the current lane to another lane, and there is an obstacle or vehicle in front of the vehicle, which needs to be avoided or overtaken.

[0055] When no action is taken, the vehicle will continue to drive in the current lane. In all of the above transitions, the vehicle must cross the centerline of the lane. This means that the trajectory must pass through this point during the change. If the vehicle does not pass through the control point, it can be considered that the vehicle is still driving in the lane and the curvature has not changed. Therefore, this point is used as a control point or a separation point to optimize the vehicle's planned trajectory.

[0056] Since the curvature of the vehicle's planned trajectory changes continuously with driving time, the curvature may not change within a certain period of time, or it may change multiple times within a certain period of time. This application uses the control point as a dividing point to divide the entire planned trajectory of the vehicle into multiple segments with different curvatures.

[0057] This application is a step performed after the planned trajectory of the vehicle is obtained, that is, the planned trajectory needs to be optimized. Therefore, this application mainly protects how to obtain the curvature of the control points and how to perform smoothing based on the obtained curvature of the control points.

[0058] A method for obtaining the curvature of control points of a vehicle planning trajectory, characterized in that it includes:

[0059] Step 101: Determine whether the vehicle location, the current control point, and the interval point are collinear;

[0060] Step 102: If they are not collinear, obtain the curvature of the circle formed by the vehicle point, the current control point, and the interval point; use the curvature as the curvature of the current control point; otherwise, record the curvature of the current control point as zero.

[0061] Through the above steps, we can obtain the corresponding interval points on the trajectory between the control point and the vehicle's location. Then, based on whether these three points are collinear, we can determine whether the curvature of the trajectory between the vehicle's location and the control point has changed. If it has changed, we calculate the curvature of the corresponding triangle's circumcircle based on the coordinates of the three points. In other words, the curvature of this circumcircle is used as the real-time curvature of the planned trajectory at the control point when the vehicle arrives at the control point. This method of calculating curvature is simple and requires little computation.

[0062] As the vehicle moves, the above method is repeated to obtain the curvature of subsequent control points. The entire vehicle planning trajectory is optimized according to the curvature of all the control points so that the end point where the curvature of the previous planned trajectory changes is used as the starting point where the curvature of the next planned trajectory changes. In this way, multiple planned trajectories with continuously changing curvatures can be smoothly connected, thereby ultimately achieving smooth processing of the entire planned trajectory. This processing method ensures the continuity of the curvature of the control points, has a small amount of calculation, and is real-time.

[0063] In some preferred embodiments, the curvature has positive and negative values, that is, the corresponding vehicle is traveling to the left or right side of the road. In addition, how to determine whether the vehicle point, the current control point and the interval point are collinear, and whether it is possible to simultaneously determine the positive and negative values ​​of the difference curvature, are explained below:

[0064] Step 101, determining whether the vehicle location, the current control point, and the interval point are collinear, specifically includes the following steps:

[0065] A judgment value is obtained based on the vehicle location, the current control point and the interval point, and the mapping relationship;

[0066] If the judgment value is equal to zero, the three points are collinear;

[0067] If the judgment value is greater than zero, the three points are not collinear and the curvature is negative;

[0068] If the judgment value is less than zero, the three points are not collinear and the curvature is positive.

[0069] Among them, collinearity indicates that the planned trajectory of the vehicle in this section travels along the current lane and does not change.

[0070] In some preferred embodiments, before determining whether the vehicle location, the current control point, and the interval point are collinear, step 100 is further included. Step 100 specifically includes:

[0071] Obtain the planned trajectory and convert it into a driving line in the SL coordinate system; in the SL coordinate system, the S axis is the centerline of the road; the L axis and the S axis are in the same plane and perpendicular to the S axis;

[0072] The point where the driving line intersects the S axis is used as the control point;

[0073] Any point between the current control point and the vehicle's location on the vehicle trajectory is taken as an interval point.

[0074] In another, more preferred form, step 100 specifically includes:

[0075] Obtain the planned trajectory and convert it into a driving line in the SL coordinate system; in the SL coordinate system, the S axis is the centerline of the road; the L axis and the S axis are in the same plane and perpendicular to the S axis;

[0076] The point where the driving line intersects the S axis is used as the control point;

[0077] The midpoint between the current control point and the vehicle's location on the vehicle trajectory is taken as the interval point.

[0078] This method uses the median point as the interval point, making the calculation more accurate. The SL coordinate system is also called frenetframe.

[0079] In the above description, when the road on which the vehicle is located is a one-way road, the S axis is the center line of the road in the longitudinal direction;

[0080] When the road where the vehicle is located is a two-way multi-lane road, the S axis is the center line of the lane where the vehicle is located in the length direction.

[0081] Such supplementary settings can better ensure that the curvature of the planned trajectory changes when the vehicle is driving on different roads, for example, when changing direction from the current lane to another lane, the curvature also changes.

[0082] In some preferred embodiments, the step of obtaining the curvature of the circle formed by the vehicle location, the current control point, and the interval points in step 102 is described in detail:

[0083] Get the distance between the vehicle's location and the current control point, and record it as the side length one;

[0084] Get the distance between the vehicle's location and the interval point, and record it as the length of the side 2;

[0085] Get the distance between the current control point and the interval point, and record it as the length of the side three;

[0086] Determine whether side lengths one, two, and three can form a triangle;

[0087] If so, then side lengths 1, 2, and 3 will form a triangle, and the curvature of the circumscribed circle of the triangle can be calculated by combining mapping relationship 2;

[0088] Otherwise, the curvature is recorded as zero.

[0089] Determining whether side length one, side length two, and side length three can form a triangle includes the following steps:

[0090] If the sum of any two sides of Side 1, Side 2, and Side 3 is greater than the remaining side, then a triangle can be formed; otherwise, a triangle cannot be formed. If the length of any one of Side 1, Side 2, and Side 3 is zero, then a triangle cannot be formed.

[0091] In this embodiment, the case where the vehicle does not go beyond the current lane but the trajectory is not a curve is excluded because in this case it is considered that the current lane only needs to be maintained and no control point is passed, so the curvature is zero.

[0092] To illustrate the above calculation process in detail, a specific practical implementation is given below. Please refer to the practical flow chart shown in Figure 2:

[0093] I. Plan a trajectory based on sensor data and chassis data, and convert the trajectory into an SL coordinate system. The trajectory length s is between 0m and 200m. Sensor data and chassis data are the data for generating the trajectory, and are not specifically explained in detail for existing technologies.

[0094] Let s = 0, L = 0, and the starting point (x, y) of the trajectory be the vehicle coordinate point A;

[0095] Take the point (x1, y1) on the trajectory where s = 100 and L = 0 as the control point B;

[0096] Take the midpoint of A and B on the trajectory, that is, the point (x2, y2) at s = 50, L = 0 on the trajectory as the midpoint C;

[0097] II. Determine whether ABC is collinear, and calculate M = (x-x1)*(y2-y1)-(y-y1)*(x2-x1).

[0098] The judgment value is M, and the mapping relationship is M = (x-x1)*(y2-y1)-(y-y1)*(x2-x1);

[0099] If M=0, the three points are collinear, the curvature of control point B is 0, and the control point is output.

[0100] If M>0, C is on the left side of line AB, the curvature of point B is negative, and sign=-1. III. If M<0, C is on the right side of line AB, the curvature of point B is positive, and sign=1. IV. The lengths of sides AB, AC, and BC are a, b, and c.

[0101] If the sum of the two sides is not less than the third side, that is, b+ca<0 or c+ab<0 or a+bc<0, the curvature of the control point B is 0;

[0102] If a=0 or b=0 or c=0, the curvature of control point B is 0;

[0103] In other cases, let p = (a + b + c) / 2, and calculate the curvature of the control point B; the second mapping relationship is:

[0104] Regulations stipulate that the minimum curve radius for highways in plain or hilly areas is 650 meters, and the minimum curve radius for highways in mountainous areas is 250 meters. This solution is based on highway scenarios, so the maximum curvature for highways in plain or hilly areas is no greater than 0.00153, and the curvature for highways in mountainous areas is no greater than 0.004.

[0105] This application also proposes a vehicle planning trajectory control point curvature acquisition system, which includes:

[0106] The first module is used to determine whether the vehicle location, the current control point and the interval point are collinear;

[0107] The second module is used to obtain the curvature of the circumscribed circle corresponding to the three points based on the vehicle point, the current control point and the interval point when they are not collinear; the curvature is used as the curvature of the current control point; when they are collinear, the curvature of the current control point is recorded as zero.

[0108] A method for smoothing a vehicle planning trajectory, comprising:

[0109] When the vehicle needs to travel to the current control point, the curvature of the current control point is obtained according to the control point curvature acquisition method of the vehicle planning trajectory;

[0110] When the vehicle reaches the current control point and is about to travel to the next control point, the coordinates of the vehicle's location are replaced with the coordinates of the current control point, and the coordinates of the current control point are replaced with the coordinates of the next control point; then, the curvature of the next control point is obtained according to the control point curvature acquisition method of the vehicle's planned trajectory;

[0111] Repeat the above steps to obtain the curvature of all control points, and smooth the vehicle planning trajectory based on the obtained curvature of all control points.

[0112] Through the above steps, we can obtain the corresponding interval points on the trajectory between the control point and the vehicle's location. Then, based on whether these three points are collinear, we can determine whether the curvature of the trajectory between the vehicle's location and the control point has changed. If it has changed, we calculate the curvature of the corresponding triangle's circumcircle based on the coordinates of the three points. In other words, the curvature of this circumcircle is used as the real-time curvature of the planned trajectory at the control point when the vehicle arrives at the control point. This method of calculating curvature is simple and requires little computation.

[0113] As the vehicle moves, the above method is repeated to obtain the curvature of subsequent control points. The entire vehicle planning trajectory is optimized according to the curvature of all the control points so that the end point where the curvature of the previous planned trajectory changes is used as the starting point where the curvature of the next planned trajectory changes. In this way, multiple planned trajectories with continuously changing curvatures can be smoothly connected, thereby ultimately achieving smooth processing of the entire planned trajectory. This processing method ensures the continuity of the curvature of the control points, has a small amount of calculation, and is real-time.

[0114] A vehicle trajectory smoothing system comprising:

[0115] The third module is used to obtain the curvature of the current control point according to the control point curvature acquisition method of the vehicle planned trajectory when the vehicle needs to travel to the current control point;

[0116] The fourth module is configured to replace the coordinates of the vehicle's location with the coordinates of the current control point and the coordinates of the current control point with the coordinates of the next control point when the vehicle reaches the current control point and is about to travel to the next control point; and then obtain the curvature of the next control point according to the control point curvature acquisition method of the vehicle's planned trajectory;

[0117] The fifth module is used to obtain the curvatures of all control points according to the third module and the fourth module, and to smooth the vehicle planning trajectory according to the obtained curvatures of all control points.

[0118] A storage medium of computer-readable instructions, which, when executed by one or more processors, enables the one or more processors to execute the steps of a method for obtaining the curvature of control points of a planned vehicle trajectory.

[0119] A vehicle comprising: the aforementioned system for acquiring curvature of control points of a planned vehicle trajectory. The vehicle can be a fuel-powered vehicle or a new energy vehicle, such as an electric vehicle. The system can implement the various processes of the engine characteristic map-based autonomous driving speed planning method described in any of the aforementioned embodiments, achieving the same technical effects. To avoid repetition, these processes are not described here.

[0120] The present application is described with reference to the flow chart and / or block diagram of the method, device (system), and computer program product according to the embodiment of the present application. It should be understood that each flow process and / or box in the flow chart and / or block diagram and the combination of the flow process and / or box in the flow chart and / or block diagram can be realized by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processing machine or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce a device for realizing the function specified in one flow chart flow or multiple flows and / or one box or multiple boxes of the block diagram.

[0121] These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce a product including an instruction device that implements the functions specified in one or more processes in the flowchart and / or one or more boxes in the block diagram.

[0122] These computer program instructions may also be loaded onto a computer or other programmable data processing device so that a series of operational steps are performed on the computer or other programmable device to produce a computer-implemented process, whereby the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more processes of the flowchart and / or one or more blocks of the block diagram. In a typical configuration, a computing device includes one or more processors (CPUs), input / output interfaces, network interfaces, and memory. Memory may include non-permanent memory in a computer-readable medium, random access memory (RAM), and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. Memory is an example of a computer-readable medium.

[0123] In the description of this application, it should be noted that the terms "upper" and "lower" and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing this application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on this application. Unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or an indirect connection through an intermediate medium, or it can be internal communication between two elements. For those of ordinary skill in the art, the specific meanings of the above terms in this application can be understood according to the specific circumstances.

[0124] It should be noted that, in this application, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device comprising a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprising a ..." does not exclude the presence of other identical elements in the process, method, article or device comprising the element.

[0125] The foregoing is merely a list of specific embodiments of the present application, intended to enable those skilled in the art to understand or implement the present application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present application. Therefore, the present application is not limited to the embodiments shown herein, but is intended to conform to the broadest scope consistent with the principles and novel features of the present application.

Claims

1. A method for obtaining the curvature of control points of a vehicle planning trajectory, characterized in that: It includes: Determine whether the vehicle location, current control point and interval point are collinear; If they are not collinear, the curvature of the circle formed by the vehicle point, the current control point and the interval point is obtained; the curvature is used as the curvature of the current control point; otherwise, the curvature of the current control point is recorded as zero.

2. The method for obtaining the curvature of the control points of the vehicle planning trajectory according to claim 1, characterized in that: Determining whether the vehicle location, the current control point and the interval point are collinear specifically includes the following steps: A judgment value is obtained based on the vehicle location, the current control point and the interval point, and the mapping relationship; If the judgment value is equal to zero, the three points are collinear; If the judgment value is greater than zero, the three points are not collinear and the curvature is negative; If the judgment value is less than zero, the three points are not collinear and the curvature is positive.

3. The method for obtaining the curvature of the control points of the vehicle planning trajectory according to claim 1, characterized in that: Before determining whether the vehicle location, the current control point and the interval point are collinear, the following steps are also included: Get the planned trajectory and convert it into a driving line in the SL coordinate system; the S axis in the SL coordinate system is the center line of the road; the L axis and the S axis are in the same plane and are perpendicular to the S axis; The point where the travel line intersects the S axis is taken as the control point; Any point between the current control point and the vehicle's location on the vehicle trajectory is taken as an interval point.

4. The method for obtaining the curvature of the control points of the vehicle planning trajectory according to claim 1, characterized in that: Before determining whether the vehicle location, the current control point and the interval point are collinear, the following steps are also included: Get the planned trajectory and convert it into a driving line in the SL coordinate system; SL coordinate system The middle S axis is the centerline of the road; the L axis is in the same plane as the S axis and is perpendicular to the S axis; The point where the travel line intersects the S axis is taken as the control point; The midpoint between the current control point and the vehicle's location on the vehicle trajectory is taken as the interval point.

5. The method for obtaining the curvature of the control points of the vehicle planning trajectory according to claim 3 or 4, characterized in that: When the road where the vehicle is located is a one-way road, the S axis is the center line of the road in the length direction; When the road where the vehicle is located is a two-way multi-lane road, the S axis is the center line of the lane where the vehicle is located in the length direction.

6. The method for obtaining the curvature of the control points of the vehicle planning trajectory according to claim 1, characterized in that: Obtaining the curvature of a circle formed by the vehicle point, the current control point, and the interval point includes the following steps: Get the distance between the vehicle's location and the current control point, and record it as the side length one; Get the distance between the vehicle point and the interval point, and record it as the side length 2; Get the distance between the current control point and the interval point, and record it as the length of the side three; Determine whether side lengths one, two, and three can form a triangle; If so, side lengths 1, 2, and 3 will form a triangle, and the curvature of the circumscribed circle of the triangle can be calculated in combination with mapping relationship 2; Otherwise, the curvature is taken as zero.

7. The method for obtaining the curvature of the control points of the vehicle planning trajectory according to claim 6, characterized in that: Determining whether side length one, side length two, and side length three can form a triangle includes the following steps: If the sum of any two sides of side 1, side 2, and side 3 is greater than the remaining side, a triangle can be formed; If the length of any of side one, side two, and side three is zero, a triangle cannot be formed.

8. A vehicle planning trajectory control point curvature acquisition system, characterized in that: It includes: The first module is used to determine whether the vehicle location, the current control point and the interval point are collinear; The second module is used to obtain the curvature of the circumscribed circle corresponding to the three points based on the vehicle point, the current control point and the interval point when they are not collinear; the curvature is used as the curvature of the current control point; when they are collinear, the curvature of the current control point is recorded as zero.

9. A method for smoothing a vehicle planning trajectory, characterized in that: It includes: When the vehicle needs to travel to the current control point, the curvature of the current control point is obtained according to the method for obtaining the curvature of the control point of the vehicle planning trajectory as described in any one of claims 1 to 7; When the vehicle reaches the current control point and is about to travel to the next control point, the coordinates of the point where the vehicle is located are replaced by the coordinates of the current control point, and the coordinates of the current control point are replaced by the coordinates of the next control point; then the curvature of the next control point is obtained according to the control point curvature acquisition method of the vehicle planning trajectory as described in any one of claims 1 to 7; Repeat the above steps to obtain the curvature of all control points, and smooth the vehicle planning trajectory according to the obtained curvature of all control points.

10. A vehicle trajectory smoothing system, characterized in that: It includes: A third module is used for obtaining the curvature of the current control point according to the control point curvature acquisition method of the vehicle planning trajectory according to any one of claims 1 to 7 when the vehicle needs to travel to the current control point; A fourth module is used for replacing the coordinates of the point where the vehicle is located with the coordinates of the current control point, and replacing the coordinates of the current control point with the coordinates of the next control point when the vehicle arrives at the current control point and is about to travel to the next control point; and then obtaining the curvature of the next control point according to the control point curvature acquisition method of the vehicle planning trajectory according to any one of claims 1 to 7; The fifth module is used to obtain the curvatures of all control points according to the third module and the fourth module, and to smooth the vehicle planning trajectory according to the obtained curvatures of all control points.

Images