Path Fitting Method, Device, Electronic Device and Storage Medium

Abstract

The present invention provides a path fitting method, apparatus, electronic device, and storage medium. The method includes: determining the curvature of any path point based on the position of any path point in the path to be fitted and the position of the adjacent path point of the any path point; dividing the path to be fitted into multiple local paths based on the curvature of each path point and a path point number separation threshold; performing curve fitting on each local path based on the curve type corresponding to each local path to obtain the curve fitting result of each local path; and determining the curve fitting result of the path to be fitted based on the curve fitting result of each local path. The method, apparatus, electronic device, and storage medium provided by the present invention have good continuity and high accuracy of the obtained curve fitting result, which is beneficial to improving the stability of an autonomous driving vehicle during driving.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving, and particularly to a path fitting method, device, electronic device and storage medium. Background Art

[0002] In the field of autonomous driving, the problem of path fitting often occurs. Reasonable path fitting is beneficial to improving the stability of an autonomous driving vehicle during driving.

[0003] In the prior art, the commonly used method is to use polynomials for fitting, or to use multiple groups of polynomials to separately fit multiple segments of paths, so as to generate the driving path of an autonomous driving vehicle. The generated fitted path has poor continuity and accuracy, and cannot be used to assist the vehicle in autonomous driving. Summary of the Invention

[0004] The present invention provides a path fitting method, device, electronic device and storage medium, so as to solve the technical problems of poor continuity and accuracy of path fitting in the prior art.

[0005] The present invention provides a path fitting method, including:

[0006] Determining the curvature of any path point based on the position of the any path point in the path to be fitted and the position of the adjacent path point of the any path point;

[0007] Dividing the path to be fitted into multiple local paths based on the curvatures of each path point and a threshold of the number of separated path points;

[0008] Performing curve fitting on each local path based on the curve type corresponding to each local path to obtain the curve fitting result of each local path;

[0009] Determining the curve fitting result of the path to be fitted based on the curve fitting result of each local path.

[0010] According to the path fitting method provided by the present invention, the step of dividing the path to be fitted into multiple local paths based on the curvatures of each path point and a threshold of the number of separated path points includes:

[0011] Determining the curvature continuity of each path point based on the curvatures of each path point and a threshold of the number of separated path points;

[0012] Dividing the path to be fitted into multiple local paths based on the curvature continuity of each path point; the local path includes a curvature continuous path and a straight line path.

[0013] According to the path fitting method provided by the present invention, based on the curvature continuity of each path point, the path to be fitted is divided into multiple local paths, and then it includes:

[0014] Determine the path length of the curvature continuous path among the multiple local paths;

[0015] If the path length of the curvature continuous path is less than the preset path length threshold, set the curvature of each path point in the curvature continuous path to zero, and merge the curvature continuous path with the adjacent straight path.

[0016] According to the path fitting method provided by the present invention, based on the curvature continuity of each path point, the path to be fitted is divided into multiple local paths, and then it further includes:

[0017] Determine the number of path points of the straight path among the multiple local paths, and the curvature direction sign of the curvature continuous path adjacent to the straight path;

[0018] If the number of path points of the straight path is less than the preset number of path points and the curvature direction sign is the same as that of the adjacent curvature continuous path, merge the straight path with the adjacent curvature continuous path.

[0019] According to the path fitting method provided by the present invention, based on the position of any path point in the path to be fitted and the positions of the adjacent path points of the any path point, determine the curvature of the any path point, and then it includes:

[0020] If the curvature of the any path point is less than the preset curvature threshold, set the curvature of the any path point to zero.

[0021] According to the path fitting method provided by the present invention, curve fitting is performed on each local path based on the curve type corresponding to each local path to obtain the curve fitting result of each local path, including:

[0022] Based on the central angle of each curvature continuous path in the local path and the preset central angle threshold, determine the curve type of each curvature continuous path; the curve types include curved path curves and small curvature curves;

[0023] If the curve type of the curvature continuous path is a curved path curve, determine the curve fitting result of the curvature continuous path based on a circular curve and a clothoid;

[0024] If the curve type of the curvature continuous path is a small curvature curve, determine the curve fitting result of the curvature continuous path based on a fifth-degree polynomial curve.

[0025] According to the path fitting method provided by the present invention, determining the curve fitting result of the curvature continuous path based on a circular curve and a clothoid includes:

[0026] Based on the path points on the curvature continuous path, determining the incoming straight line, the outgoing straight line, and the circular curve corresponding to the curvature continuous path;

[0027] Based on the incoming straight line, the outgoing straight line, and the circular curve corresponding to the curvature continuous path, determining the clothoid corresponding to the curvature continuous path;

[0028] Based on the circular curve and the clothoid corresponding to the curvature continuous path, determining the curve fitting result of the curvature continuous path.

[0029] The present invention also provides a path fitting device, including:

[0030] A curvature determination unit, configured to determine the curvature of any path point based on the position of any path point in the path to be fitted and the position of the adjacent path point of the any path point;

[0031] A path segmentation unit, configured to divide the path to be fitted into multiple local paths based on the curvature of each path point and a path point number separation threshold;

[0032] A segmented fitting unit, configured to perform curve fitting on each local path based on the curve type corresponding to each local path to obtain the curve fitting result of each local path;

[0033] A path generation unit, configured to determine the curve fitting result of the path to be fitted based on the curve fitting result of each local path.

[0034] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored on the memory and executable on the processor, and when the processor executes the program, the steps of the path fitting method are implemented.

[0035] The present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the path fitting method are implemented.

[0036] The path fitting method, device, electronic device, and storage medium provided by the present invention divide the path to be fitted into multiple local paths according to the curvature of each path point in the path to be fitted, and then perform curve fitting on each local path according to the curve type corresponding to each local path, and finally obtain the curve fitting result of the path to be fitted. The obtained curve fitting result has good continuity and high accuracy, which is beneficial to improving the stability of an autonomous driving vehicle during driving. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0038] Figure 1 It is a schematic flowchart of the path fitting method provided by the present invention;

[0039] Figure 2 It is a schematic diagram of the principle of curvature calculation provided by the present invention;

[0040] Figure 3 It is a schematic diagram of the judgment of curvature continuity provided by the present invention;

[0041] Figure 4 It is a schematic diagram of the curve fitting provided by the present invention;

[0042] Figure 5 It is a schematic structural diagram of the path fitting device provided by the present invention;

[0043] Figure 6 It is a schematic structural diagram of the electronic device provided by the present invention. Specific Embodiments

[0044] To make the objectives, technical solutions, and advantages of the present invention clearer, the following will clearly and completely describe the technical solutions in the present invention in conjunction with the drawings in the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the scope of protection of the present invention.

[0045] Figure 1 It is a schematic flowchart of the path fitting method provided by the present invention, as Figure 1 shown, the method includes:

[0046] Step 110, determine the curvature of any path point based on the position of any path point in the path to be fitted and the position of the adjacent path point of any path point.

[0047] Specifically, the position of any path point on the path to be fitted can be determined by the RTK (Real-time kinematic) data collected during manual driving of the vehicle. Based on the positions of any path point and its adjacent path points, the curvature of this path point can be calculated. Curvature is the rate of rotation of the tangent direction angle of a point on a curve with respect to the arc length, indicating the degree to which the curve deviates from a straight line. Mathematically, it is a value representing the degree of bending of a curve at a certain point. The larger the curvature, the greater the degree of bending of the curve. The reciprocal of the curvature is the radius of curvature.

[0048] Figure 2 This is the schematic diagram of the curvature calculation provided by the present invention. As Figure 2 shown, perform parabolic fitting on three points, and then calculate the curvature of the middle point. The detailed steps are as follows: The coordinates of point A, point B, and point C are (x1, y1), (x2, y2), and (x3, y3) respectively. Generally, point A, point B, and point C are three adjacent points on the path, and the following conditions need to be satisfied: The distance between point A and point B should be greater than or equal to a minimum distance d min , that is, d AB ≥d min . For point B and point C, a similar condition also needs to be satisfied, that is, d BC ≥d min . If the distance between point A and point B does not meet the above conditions, search forward along the path until the first point that meets the above distance condition is found and used as the new point A. If the distance between point B and point C does not meet the above conditions, search backward along the path until the first point that meets the above distance condition is found and used as the new point C. Through point A, point B, and point C, there is a unique parabola, which can be represented by a parametric equation. Solve the coefficients of the parametric equation, and then calculate the curvature of the middle point through the definition of curvature and the coefficients of the solved parametric equation. At the same time, the tangent slope of the middle point can also be calculated.

[0049] Step 120: Divide the path to be fitted into multiple local paths based on the curvatures of each path point and the threshold between the number of path points.

[0050] Specifically, the sign of the curvature direction can be determined according to the concavity and convexity of the curve formed by each path point. For example, the curvature of a convex surface is positive, and the curvature of a concave surface is negative. The sign of the curvature direction can also be determined according to the bending direction of the curve formed by the path points. For example, the curvature is positive when bending counterclockwise and negative when bending clockwise.

[0051] The path to be fitted can be divided into multiple local paths according to the direction signs and magnitudes of the curvatures of each path point, as well as the threshold for the number of path points separated. For example, if the magnitudes of the curvatures of multiple consecutive path points are zero, it indicates that the path where these multiple path points are located may form a straight path. If the direction signs of the curvatures of multiple consecutive path points are the same, it indicates that the path where these multiple path points are located may form a curved path that continuously bends in the same direction.

[0052] In the path formed by multiple consecutive path points, it is possible that the curvatures of several intermediate path points are zero. From the perspective of the curve shape, if the number of path points with zero curvature is small and the direction signs of the path points at both ends of the path are the same, it can be considered that the curve remains continuous. If the number of path points with zero curvature is large, although the direction signs of the path points at both ends of the path are the same, it can be considered that there is a straight line in the middle of the curve and the curve is interrupted. Therefore, it can be judged by setting the threshold for the number of path points separated. If the number of path points with zero curvature is less than or equal to the threshold for the number of path points separated, it can be considered that these path points do not affect the continuity of multiple consecutive path points. If the number of path points with zero curvature is greater than the threshold for the number of path points separated, it can be considered that these path points disrupt the continuity of multiple consecutive path points. The threshold for the number of path points separated can be set as needed.

[0053] Step 130: Perform curve fitting on each local path based on the curve type corresponding to each local path to obtain the curve fitting result of each local path.

[0054] Specifically, the curve shapes of each local path are different, and can be a straight line or a curve. Curves can be further divided into different types according to the degree of bending. Therefore, according to the curve type corresponding to each local path, the corresponding curve model can be selected to perform curve fitting on each local path to obtain the curve fitting result of each local path. The curve model can be a circular curve, a polynomial curve, a clothoid, a straight line, etc. Here, a straight line is regarded as a special curve.

[0055] Step 140: Determine the curve fitting result of the path to be fitted based on the curve fitting results of each local path.

[0056] Specifically, after splicing the curve fitting results of each local path, the curve fitting result of the path to be fitted is obtained.

[0057] The path fitting method provided by the embodiments of the present invention divides the path to be fitted into multiple local paths according to the curvatures of each path point in the path to be fitted, then performs curve fitting on each local path according to the curve type corresponding to each local path, and finally obtains the curve fitting result of the path to be fitted. The curve fitting result obtained by the fitting method of the present invention has good continuity and high accuracy, which is beneficial to improving the stability of an autonomous driving vehicle during driving.

[0058] Based on the above embodiment, step 120 includes:

[0059] Based on the curvature of each path point and the threshold value of the number of path points, the curvature continuity of each path point is determined;

[0060] Based on the curvature continuity of each path point, the path to be fitted is divided into multiple local paths; the local paths include curvature continuous paths and straight line paths.

[0061] Specifically, the curvature continuity of each path point can be determined according to the curvature of each path point and the threshold value of the number of path points, and the path to be fitted can be divided into a curvature continuous path and a straight line path.

[0062] Figure 3 A schematic diagram of curvature continuity judgment provided by the present invention, such as Figure 3 As shown in , if there is a vertical line on any path point, it indicates that the point has a curvature greater than 0. If there is no vertical line on any path point, it indicates that the curvature of the point is 0. If there is a vertical line below any path point, it indicates that the point has a curvature less than 0. The condition for judging the curvature continuity of two points is: the two points have the same curvature sign, and the number of points between the two points does not exceed the threshold of the number of path points. Here, the example of two points being no more than 3 points apart is used for explanation. In case (a), two adjacent points have the same curvature sign, and the two points are considered to be curvature continuous. In case (b), the first point and the third point have the same curvature sign, and the curvature of the second point is 0. Since the first point and the third point are separated by 1 point, which is not more than 3 points, the three points are considered to be curvature continuous. In case (c), the first and fourth points have the same curvature sign, and the curvature of the second and third points is 0. Since the first and fourth points are separated by 2 points and no more than 3 points, these four points are considered to be curvature continuous. In case (d), the first and fifth points have the same curvature sign, and the curvature of the second, third, and fourth points is 0. Since the first and fifth points are separated by 3 points and no more than 3 points, these five points are considered to be curvature continuous.

[0063] In case (e), the first and second points have curvatures with different signs, and the curvatures of these two points are considered discontinuous. In case (f), the first and third points have curvatures with different signs, and the curvatures of these two points are considered discontinuous. In case (g), the first and sixth points have curvatures with the same sign, but since the first and sixth points are separated by 4 points, which is more than 3 points, the curvature is discontinuous.

[0064] Using the above conditions to judge the continuity of curvature, all points that satisfy the continuity are placed in the same curvature continuous interval, and the starting point, the number of points, the curvature sign, the average curvature, and the path length of the curvature continuous interval are calculated. The straight line intervals are between two adjacent curvature continuous intervals, and the starting point, the number of points, and the path length of the straight line interval are calculated. At this time, it can be considered that the entire path has been initially segmented into: curvature continuous paths and straight line paths. The curvature continuous paths are composed of curvature continuous intervals, and the straight line paths are composed of straight line intervals. The straight line paths are between adjacent curvature continuous paths, and the curvature continuous paths are between adjacent straight line paths.

[0065] The path fitting method provided by the embodiment of the present invention divides the path to be fitted into multiple local paths according to the curvature continuity of each path point, and the obtained curve fitting result has good continuity and high accuracy.

[0066] Based on any of the above embodiments, based on the curvature continuity of each path point, the path to be fitted is divided into multiple local paths, and then includes:

[0067] Determine the path length of the curvature continuous path in the multiple local paths;

[0068] If the path length of the curvature continuous path is less than the preset path length threshold, the curvature of each path point in the curvature continuous path is set to zero, and the curvature continuous path is merged with the adjacent straight line path.

[0069] Specifically, the shorter the path length corresponding to a curvature continuous path, the closer the path is to a curve, and the longer the path length corresponding to a curvature continuous path, the closer the path is to a straight line.

[0070] The length judgment post-processing of each curvature continuous path can be performed according to the following method. First, according to the number of points of each curvature continuous path and its average distance, the path length of each curvature continuous path is determined, which is expressed by the formula:

[0071] d i =N i d

[0072] In the formula, d i is the path length corresponding to the i-th curvature continuous path, i is the label of the curvature continuous path, N i is the number of path points of the i-th curvature continuous path, and d is the average distance between the path points of the i-th curvature continuous path.

[0073] Secondly, judge the path length of each curvature continuous path. If the path length d i of any curvature continuous path is less than the preset path length threshold d cur , that is:

[0074] d i <d cur

[0075] Then, set the curvature of each path point in the curvature - continuous path to zero, and merge the curvature - continuous path with the adjacent straight - line path to form a new straight - line path. If the path length of any curvature - continuous path does not meet the above conditions, then keep the curvature - continuous path without adjustment.

[0076] The purpose of the above method is to filter out the shorter curvature - continuous paths and retain the longer curvature - continuous paths.

[0077] The path fitting method provided by the embodiment of the present invention can filter out the curvature - continuous paths with shorter paths according to the path length of the curvature - continuous paths, merge them as a straight - line path with the adjacent straight - line paths, improve the smoothness of the path - fitting curve, and improve the stability and rapidity when an autonomous vehicle is driving.

[0078] Based on any of the above embodiments, based on the curvature continuity of each path point, divide the path to be fitted into multiple local paths, and then further include:

[0079] Determine the number of path points of the straight - line paths in the multiple local paths, and the curvature - direction signs of the curvature - continuous paths adjacent to the straight - line paths;

[0080] If the number of path points of the straight - line path is less than the preset number of path points and the curvature - direction signs of the curvature - continuous paths adjacent to the straight - line path are the same, then merge the straight - line path with the adjacent curvature - continuous paths.

[0081] Specifically, after filtering out the curvature - continuous paths with shorter paths, the remaining curvature - continuous paths on the entire path are all longer. At this time, it is necessary to filter out the shorter straight - line paths and connect the adjacent curvature - continuous paths.

[0082] Each straight - line path can be processed according to the following method. The method is: for each straight - line path, judge whether the curvature - direction signs of the 2 adjacent curvature - continuous paths are the same. If they are the same, judge whether the number of points of this straight - line path is less than the preset threshold. If it is less than the preset threshold, then merge the straight - line path into the 2 adjacent curvature - continuous paths.

[0083] The specific steps are as follows: First, determine the number of path points of the straight - line paths in the multiple local paths, and the curvature of the curvature - continuous paths adjacent to the straight - line paths. For example, the number of path points of the straight - line path i is N i , and the curvature - direction signs of the curvature - continuous paths adjacent to the straight - line path i are S i-1 and S i+1Here, the curvature direction sign of the curvature continuous path can be determined according to the curvature direction signs of the respective path points in the curvature continuous path. Since the respective path points in the curvature continuous path satisfy continuity, the curvature direction sign of any path point can be used as the curvature direction sign of the curvature continuous path.

[0084] Secondly, if the number of path points N of the straight path i i is less than the preset number of path points N line and the curvature direction signs of the curvature continuous paths adjacent to the straight path are the same, that is

[0085] N i < N line and S i-1 = S i+1

[0086] then the straight path i and the adjacent curvature continuous path are merged to form a new curvature continuous path. Among them, the preset number of path points can be set as needed.

[0087] The method of merging the straight path into the adjacent curvature continuous path is as follows: First, assign the curvature of the straight path to the curvature of the adjacent curvature continuous path, and then connect the paths with the same curvature direction sign. After connection, update the starting point, the number of points, the curvature direction sign, the average curvature, and the path length of the path.

[0088] The path fitting method provided by the embodiments of the present invention filters out the straight paths with shorter paths, improves the smoothness of the path fitting curve, and improves the stability and rapidity when the autonomous driving vehicle travels.

[0089] Based on any of the above embodiments, after step 110, it includes:

[0090] If the curvature of any path point is less than the preset curvature threshold, the curvature of the path point is set to zero.

[0091] Specifically, after obtaining the curvature of each point on the path, a preliminary processing of the curvature is required. Here, a threshold operation is used to process the curvature, that is, filtering out the curvature less than the preset curvature threshold. The specific steps are as follows: For a path point, compare the curvature of the point with the preset curvature threshold. If the curvature of the point is less than this preset curvature threshold, the curvature of the point is set to 0. If the curvature of the point is greater than or equal to this preset curvature threshold, the curvature of the point retains its original value. The preset curvature threshold can be set as needed.

[0092] In addition, after filtering out the straight paths with shorter paths and the curvature continuous paths with shorter paths, a further filtering process can be performed on the curvature of each path point to further improve the smoothness of the path fitting curve.

[0093] The path fitting method provided by the embodiments of the present invention filters the curvature of each path point to improve the smoothness of the path fitting curve, and improve the stability and rapidity when the autonomous vehicle is driving.

[0094] Based on any of the above embodiments, step 130 includes:

[0095] Based on the central angle of each curvature continuous path in the local path and a preset central angle threshold, determine the curve type of each curvature continuous path; the curve types include curved curves and small curvature curves;

[0096] If the curve type of the curvature continuous path is a curved curve, then based on the circular curve and the clothoid, determine the curve fitting result of the curvature continuous path;

[0097] If the curve type of the curvature continuous path is a small curvature curve, then based on the quintic polynomial curve, determine the curve fitting result of the curvature continuous path.

[0098] Specifically, 4 types of curves can be used to fit the local path, and the 4 types of curves are straight line, clothoid, circle and quintic polynomial respectively.

[0099] For all straight paths, they are fitted with straight lines. If the local path is a straight path, then fit a straight line through the start point and the end point of the straight line, and obtain the changing directions of the straight line in the x-axis direction and the y-axis direction. If the abscissa of the end point of the straight line is on the right side of the abscissa of the start point, it indicates that the straight line increases in the x-axis direction. If the abscissa of the end point of the straight line is on the left side of the abscissa of the start point, it indicates that the straight line decreases in the x-axis direction. If the ordinate of the end point of the straight line is above the ordinate of the start point, it indicates that the straight line increases in the y-axis direction. If the ordinate of the end point of the straight line is below the ordinate of the start point, it indicates that the straight line decreases in the y-axis direction.

[0100] For all curvature continuous paths, calculate the central angle of the curvature continuous path according to the path length and the average curvature of the curvature continuous path. Distinguish it according to the size of the central angle corresponding to the local path, and determine the curve type of the curvature continuous path. The curve types include curved curves and small curvature curves. If the central angle is greater than or equal to the preset central angle threshold, then the curve type of the curvature continuous path is a curved curve. If the central angle is less than the preset central angle threshold, then the curve type of the curvature continuous path is a small curvature curve. The preset central angle threshold can be set as needed.

[0101] If the curve type of the curvature continuous path is a curved curve, then based on the circular curve and the clothoid, determine the curve fitting result of the curvature continuous path; if the curve type of the curvature continuous path is a small curvature curve, then based on the quintic polynomial curve, determine the curve fitting result of the curvature continuous path.

[0102] For example, for a certain curvature - continuous path, if its curve type is a small - curvature curve, for the small - curvature interval, a fifth - degree polynomial is used for fitting. The reason for using a fifth - degree polynomial here is that it is necessary to ensure that the fifth - degree polynomial is 0 - order continuous, 1 - order continuous, and 2 - order continuous with the incoming straight line and the outgoing straight line of the curvature - continuous path. The fifth - degree polynomial is expressed as:

[0103] y = a 0 + a 1 x + a 2 x 2 + a 3 x 3 + a 4 x 4 + a 5 x 5

[0104] where a 0 , a 1 , a 2 , a 3 , a 4 , a 5 are undetermined coefficients. There are a total of 6 undetermined coefficients, and 6 equations are needed to solve them. The 6 constraint conditions are that the fifth - degree polynomial is connected to the straight line at the starting point and the ending point, the tangent slopes at the starting point and the ending point are equal to the tangent slopes of the straight line, and the curvature of the fifth - degree polynomial is 0 at the starting point and the ending point. By solving the undetermined coefficients through the constraint conditions, the fifth - degree polynomial can be used to fit the small - curvature curve.

[0105] The path - fitting method provided by the embodiments of the present invention performs curve fitting on each local path according to the curve type corresponding to each local path, obtains the curve - fitting result of each local path, improves the accuracy of curve fitting, and improves the stability of an autonomous driving vehicle during driving.

[0106] Based on any of the above - mentioned embodiments, determining the curve - fitting result of a curvature - continuous path based on a circular curve and a clothoid includes:

[0107] Determining the incoming straight line, the outgoing straight line, and the circular curve corresponding to the curvature - continuous path based on the path points on the curvature - continuous path;

[0108] Determining the clothoid corresponding to the curvature - continuous path based on the incoming straight line, the outgoing straight line, and the circular curve corresponding to the curvature - continuous path;

[0109] Determining the curve - fitting result of the curvature - continuous path based on the circular curve and the clothoid corresponding to the curvature - continuous path.

[0110] Specifically, a curve corresponds to a path with a continuous curvature whose central angle is large, and at this time, it can be considered that the vehicle has turned. For a curve, a clothoid curve and a circular curve are used to fit it.

[0111] Figure 4 A schematic diagram of curve fitting provided by the present invention, such as Figure 4 As shown in the figure, the single-point dash line represents the straight line fitted to the straight path on both sides, the solid line represents the original path point, the two dashed curves represent the two added clothoids, and the double-point dash line in the middle represents the added circle. The input conditions for fitting the curve are: the slope and intercept of the straight line, the slope and intercept of the straight line leaving, the average curvature of the curve, and the sign of the direction of the curvature of the curve. The output of the fitting curve is: the length and position of the two clothoids, the position of the center of the circle, and the range of the center angle of the circle. The following is a detailed introduction to the curve fitting process.

[0112] Step 1: Calculate the intersection of the approaching and departing lines, the angle bisector, and the angle between the line and the coordinate axis.

[0113] The intersection of the two lines is calculated by combining the equations of the two lines. The angle between the line and the coordinate axis is calculated by the slope of the line. Then the angle bisector of the two lines needs to be calculated. According to the sign of the curvature of the curve, there are two cases for calculation, one is counterclockwise and the other is clockwise. The slope of the angle bisector is calculated by the properties of the angle bisector.

[0114] Step 2: Calculate the center position, the length of the clothoid and the starting position

[0115] A clothoid is a special curve. The purpose of adding a clothoid between a straight line and a circle is to make the curvature continuous. For a straight line, its curvature is 0. For a circle, its curvature is a value greater than 0. If a straight line is directly connected to a circle, its curvature is discontinuous, which is not conducive to the vehicle control algorithm. Therefore, in curve fitting, it is necessary to add a clothoid between a straight line and a circle. The curvature of the clothoid is proportional to its arc length. At the starting point of the clothoid, its arc length is 0, corresponding to a curvature of 0. At the end point of the clothoid, its arc length is ls, and the corresponding curvature is the curvature of the circle. When the average curvature of the curve is known, the arc length of the clothoid is calculated by the following formula:

[0116] ls=500*ki

[0117] Among them, ki represents the average curvature of the i-th curve. When calculating the position of the clothoid, it is also divided into counterclockwise and clockwise cases. The position of the clothoid is calculated in clockwise and counterclockwise cases. The basic idea is as follows: first calculate the position of the basic clothoid, and then calculate the basic center coordinate position according to the average radius of the curve, and then calculate the starting point coordinate of the clothoid by the starting point of the clothoid on the approaching straight line, the slope of the tangent at the starting point of the clothoid is equal to the slope of the input straight line, and the center coordinate is on the angular bisector of the approaching straight line and the exiting straight line.

[0118] Step 3: Calculate the final clothoid position and center angle range

[0119] In step 2, the starting point of the clothoid is obtained, combined with the angles between the approaching and departing lines and the coordinate axis. and The final clothoid position can be calculated, and the coordinates of the end points of the entering and leaving clothoids can be obtained according to the final clothoid position, and then the range of the center angle can be calculated according to the center position obtained in step 2. The middle circle can be calculated by combining the center coordinates with the range of the center angle.

[0120] The path fitting method provided by the embodiment of the present invention determines the curve fitting result of the curvature continuous path based on the circular curve and the clothoid curve when the curve type of the curvature continuous path is a small curvature curve, thereby improving the accuracy of the curve fitting and improving the stability of the autonomous driving vehicle during driving.

[0121] Based on any of the above embodiments, Figure 5 A schematic diagram of the structure of the path fitting device provided by the present invention is shown in FIG. Figure 5 As shown, the device comprises:

[0122] The curvature determination unit 510 is used to determine the curvature of any path point based on the position of any path point in the path to be fitted and the positions of the adjacent path points of any path point;

[0123] A path segmentation unit 520, configured to divide the path to be fitted into a plurality of local paths based on the curvature of each path point and a threshold value between the number of path points;

[0124] A segmented fitting unit 530, configured to perform curve fitting on each local path based on the curve type corresponding to each local path, to obtain a curve fitting result for each local path;

[0125] The path generating unit 540 is used to determine the curve fitting result of the path to be fitted based on the curve fitting result of each local path.

[0126] The path fitting device provided by the embodiment of the present invention divides the path to be fitted into multiple local paths according to the curvature of each path point in the path to be fitted, and then performs curve fitting on each local path according to the curve type corresponding to each local path, and finally obtains the curve fitting result of the path to be fitted. The obtained curve fitting result has good continuity and high accuracy, which is conducive to improving the stability of the autonomous driving vehicle during driving.

[0127] Based on any of the above embodiments, the path segmentation unit 520 includes:

[0128] A continuity determination subunit, used to determine the curvature continuity of each path point based on the curvature of each path point and a threshold value of the number of path points;

[0129] The path segmentation subunit is used to divide the path to be fitted into multiple local paths based on the curvature continuity of each path point; the local paths include curvature continuous paths and straight line paths.

[0130] Based on any of the above embodiments, the path segmentation subunit is used for:

[0131] determining a path length of a curvature continuous path among a plurality of local paths;

[0132] If the path length of the curvature continuous path is less than a preset path length threshold, the curvature of each path point in the curvature continuous path is set to zero, and the curvature continuous path is merged with the adjacent straight line path.

[0133] Based on any of the above embodiments, the path segmentation subunit is further used for:

[0134] Determine the number of path points of the straight path in the plurality of local paths and the curvature of the curvature continuous path adjacent to the straight path;

[0135] If the number of path points of the straight path is less than the preset number of path points and the curvature direction sign of the curvature continuous path adjacent to the straight path is the same, the straight path is merged with the adjacent curvature continuous path.

[0136] Based on any of the above embodiments, the device further includes:

[0137] The filtering unit is used to set the curvature of any path point to zero if the curvature of any path point is less than a preset curvature threshold.

[0138] Based on any of the above embodiments, the segmented fitting unit 530 is used to:

[0139] Based on the central angle of each curvature continuous path in the local path and a preset central angle threshold, the curve type of each curvature continuous path is determined; the curve type includes a curved curve and a small curvature curve;

[0140] If the curve type of the curvature continuous path is a curved curve, a curve fitting result of the curvature continuous path is determined based on a circular curve and a clothoid;

[0141] If the curve type of the curvature continuous path is a small curvature curve, a curve fitting result of the curvature continuous path is determined based on a quintic polynomial curve.

[0142] Based on any of the above embodiments, the segmented fitting unit 530 includes:

[0143] The small curvature curve fitting subunit is used to determine the approaching straight line, the departing straight line and the circular curve corresponding to the curvature continuous path based on the path points on the curvature continuous path; determine the clothoid corresponding to the curvature continuous path based on the approaching straight line, the departing straight line and the circular curve corresponding to the curvature continuous path; determine the curve fitting result of the curvature continuous path based on the circular curve and the clothoid corresponding to the curvature continuous path.

[0144] Based on any of the above embodiments, Figure 6 A schematic diagram of the structure of an electronic device provided by the present invention, such as Figure 6 As shown, the electronic device may include: a processor (Processor) 610, a communication interface (Communications Interface) 620, a memory (Memory) 630 and a communication bus (Communications Bus) 640, wherein the processor 610, the communication interface 620, and the memory 630 communicate with each other through the communication bus 640. The processor 610 may call the logic command in the memory 630 to execute the following method:

[0145] Based on the position of any path point in the path to be fitted and the positions of the adjacent path points of any path point, the curvature of any path point is determined; based on the curvature of each path point and the threshold value between the number of path points, the path to be fitted is divided into multiple local paths; based on the curve type corresponding to each local path, curve fitting is performed on each local path to obtain the curve fitting result of each local path; based on the curve fitting result of each local path, the curve fitting result of the path to be fitted is determined.

[0146] In addition, the logic commands in the above-mentioned memory 630 can be implemented in the form of software functional units and can be stored in a computer-readable storage medium when sold or used as an independent product. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, which is stored in a storage medium and includes several commands to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a disk or an optical disk.

[0147] The processor in the electronic device provided in the embodiment of the present invention can call the logic instructions in the memory to implement the above method. Its specific implementation method is consistent with the implementation method of the aforementioned method and can achieve the same beneficial effects, which will not be repeated here.

[0148] An embodiment of the present invention further provides a non-transitory computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the method provided in each of the above embodiments is implemented, for example, including:

[0149] Based on the position of any path point in the path to be fitted and the positions of the adjacent path points of any path point, the curvature of any path point is determined; based on the curvature of each path point and the threshold value between the number of path points, the path to be fitted is divided into multiple local paths; based on the curve type corresponding to each local path, curve fitting is performed on each local path to obtain the curve fitting result of each local path; based on the curve fitting result of each local path, the curve fitting result of the path to be fitted is determined.

[0150] When the computer program stored on the non-transitory computer-readable storage medium provided by the embodiment of the present invention is executed, the above method is implemented. Its specific implementation method is consistent with the implementation method of the aforementioned method and can achieve the same beneficial effects, which will not be repeated here.

[0151] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0152] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of commands to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0153] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A path fitting method, characterized in that, comprising: determining the curvature of any path point based on the position of any path point in the path to be fitted and the position of the adjacent path point of the any path point; dividing the path to be fitted into multiple local paths based on the curvature of each path point and a path point number separation threshold; performing curve fitting on each local path based on the curve type corresponding to each local path to obtain the curve fitting result of each local path; determining the curve fitting result of the path to be fitted based on the curve fitting result of each local path; the position of any path point in the path to be fitted is determined based on real-time dynamic data collected during manual driving of a vehicle; the path to be fitted is used to assist the vehicle in autonomous driving; the dividing the path to be fitted into multiple local paths based on the curvature of each path point and a path point number separation threshold includes: determining the curvature continuity of each path point based on the curvature of each path point and a path point number separation threshold; dividing the path to be fitted into multiple local paths based on the curvature continuity of each path point; the local path includes a curvature continuous path and a straight path; the performing curve fitting on each local path based on the curve type corresponding to each local path to obtain the curve fitting result of each local path includes: determining the curve type of each curvature continuous path based on the central angle of each curvature continuous path in the local path and a preset central angle threshold; the curve type includes a bend curve and a small curvature curve; if the curve type of the curvature continuous path is a bend curve, then determining the curve fitting result of the curvature continuous path based on a circular curve and a clothoid; if the curve type of the curvature continuous path is a small curvature curve, then determining the curve fitting result of the curvature continuous path based on a fifth-degree polynomial curve.

2. The path fitting method according to claim 1, characterized in that, after the dividing the path to be fitted into multiple local paths based on the curvature continuity of each path point, it includes: determining the path length of the curvature continuous path in the multiple local paths; if the path length of the curvature continuous path is less than a preset path length threshold, then setting the curvature of each path point in the curvature continuous path to zero and merging the curvature continuous path with the adjacent straight path.

3. The path fitting method according to claim 1, characterized in that, after the dividing the path to be fitted into multiple local paths based on the curvature continuity of each path point, it further includes: determining the number of path points of the straight path in the multiple local paths and the curvature direction symbol of the curvature continuous path adjacent to the straight path; if the number of path points of the straight path is less than a preset number of path points and the curvature direction symbol of the straight path is the same as that of the adjacent curvature continuous path, then merging the straight path with the adjacent curvature continuous path.

4. The path fitting method according to any one of claims 1 to 3, characterized in that, Determining the curvature of any path point based on the position of the path point in the path to be fitted and the positions of the adjacent path points of the path point, and then includes: If the curvature of the path point is less than a preset curvature threshold, set the curvature of the path point to zero.

5. The path fitting method according to claim 1, characterized in that, the determining the curve fitting result of the curvature continuous path based on the circular curve and the clothoid includes: determining the incoming straight line, the outgoing straight line and the circular curve corresponding to the curvature continuous path based on the path points on the curvature continuous path; determining the clothoid corresponding to the curvature continuous path based on the incoming straight line, the outgoing straight line and the circular curve corresponding to the curvature continuous path; determining the curve fitting result of the curvature continuous path based on the circular curve and the clothoid corresponding to the curvature continuous path.

6. A path fitting device, characterized in that, comprises: a curvature determination unit for determining the curvature of any path point based on the position of the path point in the path to be fitted and the positions of the adjacent path points of the path point; a path segmentation unit for dividing the path to be fitted into multiple local paths based on the curvatures of the respective path points and a path point number separation threshold; a segmentation fitting unit for performing curve fitting on each local path based on the curve type corresponding to each local path to obtain the curve fitting result of each local path; a path generation unit for determining the curve fitting result of the path to be fitted based on the curve fitting results of each local path; the position of any path point in the path to be fitted is determined based on real-time dynamic data collected during manual driving of a vehicle; the path to be fitted is used to assist the vehicle in autonomous driving; the dividing the path to be fitted into multiple local paths based on the curvatures of the respective path points and a path point number separation threshold includes: determining the curvature continuity of each path point based on the curvatures of the respective path points and a path point number separation threshold; dividing the path to be fitted into multiple local paths based on the curvature continuity of each path point; the local paths include curvature continuous paths and straight paths; the performing curve fitting on each local path based on the curve type corresponding to each local path to obtain the curve fitting result of each local path includes: determining the curve type of each curvature continuous path based on the central angle of each curvature continuous path in the local path and a preset central angle threshold; the curve type includes a curved curve and a small curvature curve; if the curve type of the curvature continuous path is a curved curve, determining the curve fitting result of the curvature continuous path based on the circular curve and the clothoid; if the curve type of the curvature continuous path is a small curvature curve, determining the curve fitting result of the curvature continuous path based on a fifth-degree polynomial curve.

7. An electronic device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, when the processor executes the program, the steps of the path fitting method according to any one of claims 1 to 5 are implemented.

8. A non-transitory computer-readable storage medium, on which a computer program is stored, characterized in that, when the computer program is executed by a processor, the steps of the path fitting method according to any one of claims 1 to 5 are implemented.

Images