CN113487693A - Path fitting method and device, electronic equipment and storage medium - Google Patents
Path fitting method and device, electronic equipment and storage medium Download PDFInfo
- Publication number
- CN113487693A CN113487693A CN202110691959.4A CN202110691959A CN113487693A CN 113487693 A CN113487693 A CN 113487693A CN 202110691959 A CN202110691959 A CN 202110691959A CN 113487693 A CN113487693 A CN 113487693A
- Authority
- CN
- China
- Prior art keywords
- path
- curvature
- curve
- point
- local
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 238000004590 computer program Methods 0.000 claims description 7
- 230000011218 segmentation Effects 0.000 claims description 7
- 238000010586 diagram Methods 0.000 description 10
- 238000004891 communication Methods 0.000 description 7
- 238000001914 filtration Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 3
- 238000005452 bending Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012805 post-processing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T11/00—2D [Two Dimensional] image generation
- G06T11/20—Drawing from basic elements, e.g. lines or circles
- G06T11/203—Drawing of straight lines or curves
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q50/00—Information and communication technology [ICT] specially adapted for implementation of business processes of specific business sectors, e.g. utilities or tourism
- G06Q50/40—Business processes related to the transportation industry
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Business, Economics & Management (AREA)
- Health & Medical Sciences (AREA)
- Economics (AREA)
- General Health & Medical Sciences (AREA)
- Human Resources & Organizations (AREA)
- Marketing (AREA)
- Primary Health Care (AREA)
- Strategic Management (AREA)
- Tourism & Hospitality (AREA)
- General Business, Economics & Management (AREA)
- Image Analysis (AREA)
Abstract
The invention provides a path fitting method, a path fitting device, electronic equipment and a storage medium, wherein the method comprises the following steps: 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 path point; dividing the path to be fitted into a plurality of local paths based on the curvature of each path point and the distance threshold of the path point; performing curve fitting on each local path based on the curve type corresponding to each local path to obtain a 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. According to the method, the device, the electronic equipment and the storage medium, the obtained curve fitting result is good in continuity and high in accuracy, and the stability of the automatic driving vehicle during running is improved.
Description
Technical Field
The invention relates to the technical field of automatic driving, in particular to a path fitting method and device, electronic equipment and a storage medium.
Background
In the field of automated driving, problems with path fitting are often encountered. And the path fitting is reasonably carried out, so that the stability of the automatic driving vehicle during running is improved.
In the prior art, a method is generally adopted in which a polynomial is used for fitting, or a plurality of sets of polynomials are used for respectively fitting a plurality of sections of paths, so as to generate a driving path of an autonomous vehicle. The path generated by fitting has poor continuity and accuracy, and cannot be used for assisting the vehicle to automatically drive.
Disclosure of Invention
The invention provides a path fitting method, a path fitting device, electronic equipment and a storage medium, which are used for solving the technical problems of poor continuity and accuracy of path fitting in the prior art.
The invention provides a path fitting method, which comprises the following steps:
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 path point;
dividing the path to be fitted into a plurality of local paths based on the curvature of each path point and the distance threshold of the path point;
performing curve fitting on each local path based on the curve type corresponding to each local path to obtain a 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.
According to the path fitting method provided by the invention, the dividing of the path to be fitted into a plurality of local paths based on the curvature of each path point and the distance threshold of the path point comprises the following steps:
determining the curvature continuity of each path point based on the curvature of each path point and the distance threshold of the path point;
dividing the path to be fitted into a plurality of local paths based on the curvature continuity of each path point; the local path includes a curvature continuous path and a straight path.
According to the path fitting method provided by the present invention, the dividing the path to be fitted into a plurality of local paths based on the curvature continuity of each path point, then includes:
determining a path length of a curvature continuous path in the plurality of local paths;
if the path length of the curvature continuous path is smaller than a preset path length threshold value, the curvature of each path point in the curvature continuous path is set to be zero, and the curvature continuous path is combined with the adjacent straight path.
According to the path fitting method provided by the present invention, the path to be fitted is divided into a plurality of local paths based on the curvature continuity of each path point, and then the method further comprises:
determining the number of path points of a straight path in the plurality of local paths and the curvature direction sign of a curvature continuous path adjacent to the straight path;
if the number of the path points of the straight path is less than the preset number of the path points and the curvature direction sign of the adjacent curvature continuous path is the same, combining the straight path and the adjacent curvature continuous path.
According to the path fitting method provided by the present invention, the determining 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, then includes:
and if the curvature of any path point is smaller than a preset curvature threshold value, setting the curvature of any path point to be zero.
According to the path fitting method provided by the invention, the 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, and the method comprises the following steps:
determining the curve type of each curvature continuous path based on the central angle of each curvature continuous path in the local paths and a preset central angle threshold; the curve types comprise a curve and a small curve;
if the curve type of the curvature continuous path is a curve, determining a curve fitting result of the curvature continuous path based on a circular curve and a convolution line;
and if the curve type of the curvature continuous path is a small curvature curve, determining a curve fitting result of the curvature continuous path based on a fifth-order polynomial curve.
According to the path fitting method provided by the invention, the determining of the curve fitting result of the curvature continuous path based on the circular curve and the convolution line comprises the following steps:
determining a driving-in straight line, a driving-out straight line and a circular curve corresponding to the curvature continuous path based on path points on the curvature continuous path;
determining a convolution line corresponding to the curvature continuous path based on the driving-in straight line, the driving-out straight line and the circular curve corresponding to the curvature continuous path;
and determining a curve fitting result of the curvature continuous path based on the circular curve and the convolution line corresponding to the curvature continuous path.
The present invention also provides a path fitting apparatus, comprising:
the curvature determining unit is used for 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 path point;
the path segmentation unit is used for dividing the path to be fitted into a plurality of local paths based on the curvature of each path point and the distance threshold of the number of the path points;
the piecewise fitting unit is used 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;
and the path generating unit is used for determining the curve fitting result of the path to be fitted based on the curve fitting result of each local path.
The invention also provides an electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor implements the steps of the path fitting method when executing the program.
The invention also provides a non-transitory computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps of the path fitting method.
According to the path fitting method, the path fitting device, the electronic equipment and the storage medium, the path to be fitted is divided into the plurality of local paths according to the curvature of each path point in the path to be fitted, then curve fitting is carried out on each local path according to the curve type corresponding to each local path, and finally the curve fitting result of the path to be fitted is obtained.
Drawings
In order to more clearly illustrate the technical solutions of the present invention or the prior art, the drawings needed to be used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
FIG. 1 is a schematic flow chart of a path fitting method provided by the present invention;
FIG. 2 is a schematic diagram of curvature calculation provided by the present invention;
FIG. 3 is a schematic diagram of curvature continuity determination provided by the present invention;
FIG. 4 is a schematic diagram of a curve fit provided by the present invention;
FIG. 5 is a schematic structural diagram of a path fitting apparatus provided in the present invention;
fig. 6 is a schematic structural diagram of an electronic device provided in the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is obvious that the described embodiments are some, but not all embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Fig. 1 is a schematic flow chart of a path fitting method provided by the present invention, as shown in fig. 1, the method includes:
Specifically, the position of any path point in the path to be fitted may be determined by RTK (Real-time kinematic) data acquired when the vehicle is manually driven. According to the position of any path point and its adjacent path points, the curvature of the path point can be calculated. Curvature is the rotation rate of the tangential direction angle to the arc length for a certain point on the curve, indicating the degree to which the curve deviates from a straight line. The numerical value of the degree of curve bending at a certain point is mathematically expressed. The larger the curvature, the more curved the curve is. The inverse of the curvature is the radius of curvature.
Fig. 2 is a schematic diagram of curvature calculation provided by the present invention, and as shown in fig. 2, a parabolic fit is performed on 3 points, and then the curvature of the intermediate points is calculated. The detailed steps are as follows: the coordinates of the a point, the B point, and the C point are (x1, y1), (x2, y2), and (x3, y3), respectively. In general, the points a, B and C are adjacent 3 points on the path, and the following conditions need to be satisfied: the distance between the points A and B should be greater than or equal to a minimum distance dminI.e. dAB≥dminSimilar conditions also need to be satisfied for points B and C, namely dBC≥dminIf the distance between the point A and the point B does not meet the condition, searching forwards along the path until the 1 st point meeting the distance condition is found as a new point A. And if the distance between the point B and the point C does not meet the condition, searching backwards along the path until the 1 st point meeting the distance condition is found as a new point C. A unique parabola exists through the points A, B and C, the parabola can be represented by a parametric equation, the coefficient of the parametric equation is solved, then the curvature of the middle point is calculated through the definition of the curvature and the solved coefficient of the parametric equation, and meanwhile, the tangent slope of the middle point can also be calculated.
And step 120, dividing the path to be fitted into a plurality of local paths based on the curvature of each path point and the distance threshold of the path point.
Specifically, the sign of the direction of curvature may be determined from the concavity and convexity of the curve formed by each waypoint, for example, the curvature of the convex surface is positive and the curvature of the concave surface is negative. The sign of the curvature direction may be determined according to the bending direction of the curve formed by the path points, for example, the curvature is positive when the curve is bent counterclockwise, and the curvature is negative when the curve is bent clockwise.
The path to be fitted can be divided into a plurality of local paths according to the direction sign and the size of the curvature of each path point and the distance threshold of the path point. For example, if the curvature of a plurality of successive waypoints is zero, this indicates that the path in which the plurality of waypoints are located may form a straight path. If the direction signs of the curvatures of the plurality of successive route points are the same, this indicates that the routes where the plurality of route points are located may form a curved route that curves continuously in the same direction.
In a path formed by a plurality of continuous path points, the curvature of a plurality of path points in the middle may be zero, and when the number of points with the curvature of zero is small and the direction signs of the path points at both ends of the path are the same, the curve may be considered to be continuous, and when the number of points with the curvature of zero is large, the direction signs of the path points at both ends of the path are the same, but the middle of the curve may be a straight line and the curve may be interrupted. Therefore, it can be determined by setting a threshold for separating the number of route points, and if the number of points with zero curvature is equal to or less than the threshold for separating the number of route points, it can be considered that these route points do not affect the continuity of the plurality of consecutive route points, and if the number of points with zero curvature is greater than the threshold for separating the number of route points, it can be considered that these route points destroy the continuity of the plurality of consecutive route points. The number of path points separated by the threshold value can be set according to the requirement.
Specifically, the curve form of each local path is different, and may be a straight line or a curve. The curves can be subdivided into different types depending on the degree of curvature. Therefore, according to the curve type corresponding to each local path, a corresponding curve model can be selected, and curve fitting is performed 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 convolution line, a straight line and the like. Here, a straight line is taken as a special curve.
And step 140, determining a curve fitting result of the path to be fitted based on the curve fitting result of each local path.
Specifically, after the curve fitting results of each local path are spliced, the curve fitting result of the path to be fitted is obtained.
According to the path fitting method provided by the embodiment of the invention, the path to be fitted is divided into the plurality of local paths according to the curvature of each path point in the path to be fitted, and then curve fitting is carried out on each local path according to the curve type corresponding to each local path, so that the curve fitting result of the path to be fitted is finally obtained.
Based on the above embodiment, step 120 includes:
determining the curvature continuity of each path point based on the curvature of each path point and the distance threshold of the path point;
dividing the path to be fitted into a plurality of local paths based on the curvature continuity of each path point; the local path includes a curvature continuous path and a straight path.
Specifically, the curvature continuity of each path point can be judged according to the curvature of each path point and the distance threshold of the number of the path points, so that the path to be fitted is divided into a curvature continuous path and a straight path.
Fig. 3 is a schematic diagram of curvature continuity judgment provided by the present invention, and as shown in fig. 3, if there is a vertical line above any waypoint, it indicates that the waypoint has a curvature greater than 0, if there is no vertical line above any waypoint, it indicates that the point has a curvature of 0, and if there is a vertical line below any waypoint, it indicates that the point has a curvature less than 0. The conditions for judging the continuity of the 2-point curvature are as follows: the 2 points have curvatures of the same sign, and the number of points separated between the 2 points does not exceed the number of path points separated by the threshold value. Here, 2 points are not more than 3 points apart as an example. In case (a), 2 adjacent points have curvatures of the same sign, and the 2 points are considered to be curvature-continuous at this time. In case (b), the 1 st and 3 rd dots have curvatures of the same sign, the curvature of the 2 nd dot is 0, and since the 1 st and 3 rd dots are separated by 1 dot, there are no more than 3 dots, and these 3 dots are considered to be curvature-continuous. In case (c), the 1 st and 4 th dots have curvatures of the same sign, the curvatures of the 2 nd and 3 rd dots are 0, and since the 1 st and 4 th dots are separated by 2 dots, there are no more than 3 dots, and the 4 th dots are considered to be continuous in curvature. In case (d), the 1 st and 5 th dots have curvatures of the same sign, the 2 nd, 3 rd, and 4 th dots have curvatures of 0, and since the 1 st and 5 th dots are separated by 3 dots, there are no more than 3 dots, and the 5 th dots are considered to have continuous curvatures.
In case (e), the 1 st and 2 nd points have curvatures of different signs, and the 2 nd point curvatures are considered to be discontinuous. In case (f), the 1 st and 3 rd points have curvatures of different signs, and the 2 points are considered to have discontinuous curvatures. In case (g), the 1 st and 6 th dots have curvatures of the same sign, but since the 1 st and 6 th dots are separated by 4 dots and more than 3 dots, the curvatures are not continuous at this time.
And judging the continuity of the curvature by adopting the conditions, putting all points meeting the continuity into the same curvature continuous interval, and calculating to obtain the starting point, the number of the points, the curvature sign, the average curvature and the path length of the curvature continuous interval. A straight line section is arranged between 2 adjacent curvature continuous sections, and the starting point, the number of points and the path length of the straight line section are calculated. At this time, it can be considered that the entire path is preliminarily segmented: a continuous path of curvature and a straight path. The curvature continuous section forms a curvature continuous path, and the straight section forms a straight path. The adjacent curvature continuous paths are straight-line paths, and the adjacent straight-line paths are curvature continuous paths.
According to the path fitting method provided by the embodiment of the invention, the path to be fitted is divided into a plurality of local paths according to the curvature continuity of each path point, and the obtained curve fitting result has good continuity and high accuracy.
Based on any of the above embodiments, the method for dividing the path to be fitted into a plurality of local paths based on the curvature continuity of each path point, and then includes:
determining a path length of a curvature continuous path in the plurality of local paths;
and if the path length of the curvature continuous path is smaller than a preset path length threshold value, setting the curvature of each path point in the curvature continuous path to be zero, and merging the curvature continuous path with the adjacent straight-line path.
Specifically, when the path length corresponding to a curvature continuous path is shorter, the path is closer to a curved line, and when the path length corresponding to a curvature continuous path is longer, the path is closer to a straight line.
The length determination post-processing may be performed for each curvature continuous path in the following manner. Firstly, determining the path length of each curvature continuous path according to the number of points of each curvature continuous path and the average distance of the points, and expressing the path length by a formula as follows:
di＝Nid
in the formula (d)iIs the path length corresponding to the ith curvature continuous path, i is the index of the curvature continuous path, NiIs the number of path points of the ith curvature continuous path, and d is the average distance between the path points of the ith curvature continuous path.
Then, the path length of each curvature continuous path is determined, if the path length d of any curvature continuous pathiLess than a predetermined path length threshold dcurNamely:
di<dcur
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 path to form a new straight path. If the path length of any curvature continuous path does not satisfy the above condition, the curvature continuous path is maintained without adjustment.
The purpose of the method is to filter out shorter curvature continuous paths and to retain longer curvature continuous paths.
According to the path fitting method provided by the embodiment of the invention, the curvature continuous path with a shorter path can be filtered according to the path length of the curvature continuous path, and the curvature continuous path is used as a section of straight path to be combined with an adjacent straight path, so that the smoothness of a path fitting curve is improved, and the stability and the rapidity of the automatic driving vehicle during running are improved.
Based on any of the above embodiments, the method includes dividing the path to be fitted into a plurality of local paths based on the curvature continuity of each path point, and then:
determining the number of path points of a straight path in the plurality of local paths and the curvature direction symbol of a curvature continuous path adjacent to the straight path;
if the number of the path points of the straight path is less than the preset number of the path points and the curvature direction signs of the curvature continuous paths adjacent to the straight path are the same, merging the straight path and the adjacent curvature continuous paths.
Specifically, after filtering out the curvature continuous path with a shorter path, the curvature continuous path remained on the whole path is longer. It is now necessary to filter out the shorter straight paths and connect adjacent curvature continuous paths.
Each straight path may be processed as follows. The method comprises the following steps: for each straight-line path, judging whether the curvature direction symbols of the 2 adjacent curvature continuous paths are the same, if so, judging whether the number of the straight-line path points is smaller than a preset threshold value, and if so, merging the straight-line paths into the 2 adjacent curvature continuous paths.
The method comprises the following specific steps: first, the number of path points of a straight path among a plurality of partial paths and the curvature of a curvature continuous path adjacent to the straight path are determined. For example, the path of a straight path iThe number of the diameter points is NiThe signs of the curvature directions of the curvature continuous paths adjacent to the straight path i are Si-1And Si+1. Here, the sign of the curvature direction of the curvature continuous path may be determined according to the sign of the curvature direction of each path point in the curvature continuous path. Since each path point in the curvature continuous path satisfies the continuity, the curvature direction sign of any path point may be taken as the curvature direction sign of the curvature continuous path.
Next, if the number of path points N of the straight path iiLess than a predetermined number N of path pointslineAnd the curvature direction signs of the curvature continuous paths adjacent to the straight path are the same, i.e.
Ni<NlineAnd Si-1＝Si+1
The straight path i is merged with the adjacent curvature continuous path to form a new curvature continuous path. The number of preset path points can be set according to needs.
The manner of merging straight paths to adjacent curvature continuous paths is as follows: the curvature of the straight line path is firstly assigned as the curvature of the adjacent curvature continuous path, then the paths with the same curvature direction sign are connected, and the starting point, the number of points, the curvature direction sign, the average curvature and the path length of the paths are updated after the connection.
The path fitting method provided by the embodiment of the invention filters the straight path with a shorter path, improves the smoothness of the path fitting curve, and improves the stability and the rapidity of the automatic driving vehicle during running.
Based on any of the above embodiments, step 110 is followed by:
and if the curvature of any path point is smaller than the preset curvature threshold value, setting the curvature of the path point to be zero.
Specifically, after obtaining the curvature of each point on the path, a preliminary processing is required for the curvature. The curvature is processed by using a threshold operation, namely, the curvature smaller than a preset curvature threshold is filtered. The specific steps are as follows, for a path point, comparing the curvature of the point with the magnitude of a preset curvature threshold, if the curvature of the point is smaller than the preset curvature threshold, setting the curvature of the point to 0, and if the curvature of the point is larger than or equal to the preset curvature threshold, keeping the curvature of the point at the original value. The preset curvature threshold may be set as desired.
In addition, after filtering a straight path with a short path and filtering a curvature continuous path with a short path, filtering processing can be performed on the curvature of each path point again to further improve the smoothness of a path fitting curve.
According to the path fitting method provided by the embodiment of the invention, the curvature of each path point is subjected to filtering processing, the smoothness of a path fitting curve is improved, and the stability and the rapidity of the automatic driving vehicle during running are improved.
Based on any of the above embodiments, step 130 includes:
determining the curve type of each curvature continuous path based on the central angle of each curvature continuous path in the local paths and a preset central angle threshold; the curve types include a curve and a small curve;
if the curve type of the curvature continuous path is a curve, determining a curve fitting result of the curvature continuous path based on the circular curve and the convolution line;
and if the curve type of the curvature continuous path is a small-curvature curve, determining a curve fitting result of the curvature continuous path based on the fifth-order polynomial curve.
Specifically, the local path may be fitted with 4 types of curves, which are a straight line, a clothoid, a circle, and a fifth-order polynomial, respectively.
For all straight paths, a straight line is used for fitting. If the local path is a straight line path, fitting a straight line through the starting point and the end point of the straight line, and obtaining the change 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 to the right of the abscissa of the starting point, it indicates that the straight line increases in the x-axis direction, and if the abscissa of the end point of the straight line is to the left of the abscissa of the starting 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 starting point, it indicates that the straight line increases in the y-axis direction, and if the ordinate of the end point of the straight line is below the ordinate of the starting point, it indicates that the straight line decreases in the y-axis direction.
For all curvature continuous paths, the central angle of the curvature continuous path is calculated from the path length and the average curvature of the curvature continuous path. And distinguishing the local path according to the size of the central angle corresponding to the local path, and determining the curve type of the curvature continuous path, wherein the curve type comprises a curve and a small curvature curve. If the central angle is larger than or equal to the preset central angle threshold value, the curve type of the curvature continuous path is a curve, and if the central angle is smaller than the preset central angle threshold value, the curve type of the curvature continuous path is a small curvature curve. The preset central angle threshold may be set as desired.
If the curve type of the curvature continuous path is a curve, determining a curve fitting result of the curvature continuous path based on the circular curve and the convolution line; and if the curve type of the curvature continuous path is a small curvature curve, determining a curve fitting result of the curvature continuous path based on the fifth-order polynomial curve.
For example, for a certain curvature continuous path, if the curve type is a small curvature curve, for a small curvature interval, 5 th order polynomial is used for fitting, wherein the reason why 5 th order polynomial is used is to ensure that the 5 th order polynomial is continuous with the entering straight line and the leaving straight line of the curvature continuous path in 0 th order, 1 st order and 2 nd order. The polynomial of degree 5 is expressed as:
y＝a0+a1x+a2x2+a3x3+a4x4+a5x5
wherein, a0,a1,a2,a3,a4,a5The undetermined coefficients are 6 undetermined coefficients in total, and 6 equations are needed for solving. The 6 constraint conditions are that the starting point and the end point of the 5 th-order polynomial are respectively connected with a straight line, the slope of the tangent line at the starting point and the end point is equal to that of the tangent line of the straight line, and the curvature of the 5 th-order polynomial at the starting point and the end point is 0. By solving undetermined coefficients through constraint conditions, small curvature can be fitted by using 5-degree polynomialCurve line.
According to the path fitting method provided by the embodiment of the invention, curve fitting is carried out on each local path according to the curve type corresponding to each local path to obtain the curve fitting result of each local path, so that the accuracy of curve fitting is improved, and the running stability of the automatic driving vehicle is improved.
Based on any one of the above embodiments, determining a curve fitting result of the curvature continuous path based on the circular curve and the convolution line includes:
determining a driving-in straight line, a driving-out straight line and a circular curve corresponding to the curvature continuous path based on path points on the curvature continuous path;
determining a convolution line corresponding to the curvature continuous path based on the driving-in straight line, the driving-out straight line and the circular curve corresponding to the curvature continuous path;
and determining a curve fitting result of the curvature continuous path based on the circular curve and the convolution line corresponding to the curvature continuous path.
Specifically, the curve corresponds to a large central angle of the curvature continuous path, and it can be considered that the vehicle has made a turn. For curves, a clothoid and a circular curve are used to fit them.
Fig. 4 is a schematic diagram of curve fitting provided by the present invention, as shown in fig. 4, a single-dot chain line indicates a straight line fitted to a straight line path on the 2 sides thereof, a solid line indicates an original path point, a 2-segment dashed curve indicates an added 2-segment convolution line, and a middle two-dot chain line indicates an added circle. The input conditions for fitting the curve are: the slope and intercept of the driving-in straight line, the slope and intercept of the driving-out straight line, the average curvature of the curve and the curvature direction sign of the curve, and the output of the fitting curve is as follows: the length and position of the 2 sections of the gyroid, the circle center position and the range of the circle center angle. The process of curve fitting is described in detail below.
Step one, calculating the intersection point of a driving-in straight line and a driving-out straight line, an angular bisector and the included angle between the straight line and a coordinate axis
And calculating by simultaneous 2 linear equations to obtain the intersection point of the 2 straight lines. And calculating the included angle between the straight line and the coordinate axis according to the slope of the straight line. Then, the angular bisector of 2 straight lines needs to be calculated, and the calculation is divided into 2 cases according to the sign of the curvature of the curve, wherein one case is anticlockwise, and the other case is clockwise. The slope of the bisector is calculated by the nature of the bisector.
Step two, calculating the circle center position, the length of the convolution line and the initial position
A clothoid is a special curve, the purpose of which is to add a clothoid between a straight line and a circle in order 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 the straight line is directly connected to the circle, its curvature is discontinuous, which is not good for vehicle control algorithm, so in curve fitting, it is necessary to add a convolution between the straight line and the circle. The curvature of the clothoid is proportional to its arc length, at the beginning of the clothoid, its arc length is 0, corresponding to a curvature of 0, at the end of the clothoid, its arc length is ls, corresponding to a curvature of a circle. With the average curvature of the curve known, the arc length of the clothoid is calculated by:
ls＝500*ki
where ki denotes the average curvature of the curve in the i-th segment. When calculating the position of the convolution, the calculation is also divided into a counterclockwise case and a clockwise case. The position of the clothoid is calculated in the clockwise case and the counterclockwise case. The basic idea is as follows: firstly, calculating the position of a basic convolution line, then calculating the coordinate position of a basic circle center according to the average radius of a curve, and then calculating the coordinate of the starting point of the convolution line on a driving-in straight line through the starting point of the convolution line, wherein the slope of a tangent at the starting point of the convolution line is equal to that of an input straight line, and the coordinate of the circle center on an angular bisector of the driving-in straight line and the driving-out straight line.
Step three, calculating the final gyration line position and the range of the central angle
In the second step, the starting point position of the convolution line is obtained, and the included angles between the driving-in straight line and the driving-out straight line and the coordinate axis are combined
And
the final clothoid position, root, can be calculatedAnd C, obtaining the end point coordinates of the driving-in convolution line and the driving-out convolution line according to the position of the final convolution line, and then calculating the range of the central angle according to the position of the center of the circle obtained in the step II. The middle circle can be calculated by combining the center coordinates with the range of the center angle.
According to the path fitting method provided by the embodiment of the invention, when the curve type of the curvature continuous path is a small curvature curve, the curve fitting result of the curvature continuous path is determined according to the circular curve and the convolution line, so that the accuracy of curve fitting is improved, and the stability of the automatic driving vehicle during running is improved.
Based on any of the above embodiments, fig. 5 is a schematic structural diagram of a path fitting apparatus provided by the present invention, as shown in fig. 5, the apparatus includes:
a curvature determining unit 510, configured to determine a curvature of any waypoint based on a position of any waypoint in the path to be fitted and positions of neighboring waypoints of any waypoint;
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 the distance threshold between the path points;
a segment fitting unit 530, configured to perform curve fitting on each local path based on the curve type corresponding to each local path, so as to obtain a curve fitting result of each local path;
a path generating unit 540, configured to determine a curve fitting result of the path to be fitted based on the curve fitting result of each local path.
According to the path fitting device provided by the embodiment of the invention, the path to be fitted is divided into the plurality of local paths according to the curvature of each path point in the path to be fitted, then curve fitting is carried out on each local path according to the curve type corresponding to each local path, and finally the curve fitting result of the path to be fitted is obtained.
Based on any of the above embodiments, the path segmentation unit 520 includes:
the continuity judgment subunit is used for determining the continuity of the curvature of each path point based on the curvature of each path point and the distance threshold of the path point;
the path segmentation subunit is used for dividing the path to be fitted into a plurality of local paths based on the curvature continuity of each path point; the local path includes a curvature continuous path and a straight path.
Based on any of the above embodiments, the path segmentation subunit is configured to:
determining a path length of a curvature continuous path in the plurality of local paths;
and if the path length of the curvature continuous path is smaller than a preset path length threshold value, setting the curvature of each path point in the curvature continuous path to be zero, and merging the curvature continuous path with the adjacent straight path.
Based on any of the above embodiments, the path segmentation subunit is further configured to:
determining the number of path points of a straight path in the plurality of local paths and the curvature of a curvature continuous path adjacent to the straight path;
if the number of the path points of the straight path is less than the preset number of the path points and the curvature direction signs of the curvature continuous paths adjacent to the straight path are the same, merging the straight path and the adjacent curvature continuous paths.
Based on any embodiment above, the apparatus further comprises:
and the filtering unit is used for setting the curvature of any path point to be zero if the curvature of any path point is smaller than a preset curvature threshold value.
Based on any of the above embodiments, the segment fitting unit 530 is configured to:
determining the curve type of each curvature continuous path based on the central angle of each curvature continuous path in the local paths and a preset central angle threshold; the curve types include a curve and a small curve;
if the curve type of the curvature continuous path is a curve, determining a curve fitting result of the curvature continuous path based on the circular curve and the convolution line;
and if the curve type of the curvature continuous path is a small curvature curve, determining a curve fitting result of the curvature continuous path based on the fifth-order polynomial curve.
Based on any of the above embodiments, the segment fitting unit 530 includes:
the small curvature curve fitting subunit is used for determining a driving-in straight line, a driving-out straight line and a circular curve corresponding to the curvature continuous path based on path points on the curvature continuous path; determining a convolution line corresponding to the curvature continuous path based on the driving-in straight line, the driving-out straight line and the circular curve corresponding to the curvature continuous path; and determining a curve fitting result of the curvature continuous path based on the circular curve and the convolution line corresponding to the curvature continuous path.
Based on any of the above embodiments, fig. 6 is a schematic structural diagram of an electronic device provided by the present invention, and as shown in fig. 6, 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 complete communication with each other through the communication Bus 640. The processor 610 may call logical commands in the memory 630 to perform the following method:
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 any path point; dividing the path to be fitted into a plurality of local paths based on the curvature of each path point and the distance threshold of the path point; performing curve fitting on each local path based on the curve type corresponding to each local path to obtain a 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.
In addition, the logic commands in the memory 630 may be implemented in the form of software functional units and stored in a computer readable storage medium when the logic commands are sold or used as independent products. Based on such understanding, the technical solution of the present invention may be embodied in the form of a software product, which is stored in a storage medium and includes a plurality of commands for enabling a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
The processor in the electronic device provided in the embodiment of the present invention may call a logic instruction in the memory to implement the method, and the specific implementation manner of the method is consistent with the implementation manner of the method, and the same beneficial effects may be achieved, which is not described herein again.
Embodiments of the present invention further provide a non-transitory computer-readable storage medium, on which a computer program is stored, where the computer program is implemented to perform the method provided in the foregoing embodiments when executed by a processor, and 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 any path point; dividing the path to be fitted into a plurality of local paths based on the curvature of each path point and the distance threshold of the path point; performing curve fitting on each local path based on the curve type corresponding to each local path to obtain a 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.
When the computer program stored on the non-transitory computer readable storage medium provided in the embodiments of the present invention is executed, the method is implemented, and the specific implementation manner of the method is consistent with the implementation manner of the method, and the same beneficial effects can be achieved, which is not described herein again.
The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment. One of ordinary skill in the art can understand and implement it without inventive effort.
Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware. With this understanding in mind, the above technical solutions may be embodied in the form of a software product, which can be stored in a computer-readable storage medium, such as ROM/RAM, magnetic disk, optical disk, etc., and includes commands for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the method according to the embodiments or some parts of the embodiments.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.
Claims (10)
1. A method of path fitting, 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 path point;
dividing the path to be fitted into a plurality of local paths based on the curvature of each path point and the distance threshold of the path point;
performing curve fitting on each local path based on the curve type corresponding to each local path to obtain a 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.
2. The path fitting method according to claim 1, wherein the dividing the path to be fitted into a plurality of local paths based on the curvatures of the path points and the distance between the path points by a threshold value comprises:
determining the curvature continuity of each path point based on the curvature of each path point and the distance threshold of the path point;
dividing the path to be fitted into a plurality of local paths based on the curvature continuity of each path point; the local path includes a curvature continuous path and a straight path.
3. The path fitting method according to claim 2, wherein the dividing the path to be fitted into a plurality of local paths based on the continuity of curvature of the respective path points, then comprises:
determining a path length of a curvature continuous path in the plurality of local paths;
if the path length of the curvature continuous path is smaller than a preset path length threshold value, the curvature of each path point in the curvature continuous path is set to be zero, and the curvature continuous path is combined with the adjacent straight path.
4. The path fitting method according to claim 2, wherein the dividing the path to be fitted into a plurality of local paths based on the continuity of curvature of each path point, further comprises:
determining the number of path points of a straight path in the plurality of local paths and the curvature direction sign of a curvature continuous path adjacent to the straight path;
if the number of the path points of the straight path is less than the preset number of the path points and the curvature direction sign of the adjacent curvature continuous path is the same, combining the straight path and the adjacent curvature continuous path.
5. The path fitting method according to any one of claims 1 to 4, wherein the determining the curvature of any one of the waypoints based on the position of the any one of the waypoints in the path to be fitted and the positions of neighboring waypoints of the any one of the waypoints, thereafter comprises:
and if the curvature of any path point is smaller than a preset curvature threshold value, setting the curvature of any path point to be zero.
6. The path fitting method according to any one of claims 1 to 4, wherein the curve fitting 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 paths and a preset central angle threshold; the curve types comprise a curve and a small curve;
if the curve type of the curvature continuous path is a curve, determining a curve fitting result of the curvature continuous path based on a circular curve and a convolution line;
and if the curve type of the curvature continuous path is a small curvature curve, determining a curve fitting result of the curvature continuous path based on a fifth-order polynomial curve.
7. The path fitting method according to claim 6, wherein determining the curve fitting result of the curvature continuous path based on the circular curve and the clothoid line comprises:
determining a driving-in straight line, a driving-out straight line and a circular curve corresponding to the curvature continuous path based on path points on the curvature continuous path;
determining a convolution line corresponding to the curvature continuous path based on the driving-in straight line, the driving-out straight line and the circular curve corresponding to the curvature continuous path;
and determining a curve fitting result of the curvature continuous path based on the circular curve and the convolution line corresponding to the curvature continuous path.
8. A path fitting apparatus, comprising:
the curvature determining unit is used for 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 path point;
the path segmentation unit is used for dividing the path to be fitted into a plurality of local paths based on the curvature of each path point and the distance threshold of the number of the path points;
the piecewise fitting unit is used 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;
and the path generating unit is used for determining the curve fitting result of the path to be fitted based on the curve fitting result of each local path.
9. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the steps of the path fitting method according to any of claims 1 to 7 are implemented when the program is executed by the processor.
10. A non-transitory computer readable storage medium having stored thereon a computer program, which when executed by a processor implements the steps of the path fitting method according to any of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110691959.4A CN113487693A (en) | 2021-06-22 | 2021-06-22 | Path fitting method and device, electronic equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110691959.4A CN113487693A (en) | 2021-06-22 | 2021-06-22 | Path fitting method and device, electronic equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113487693A true CN113487693A (en) | 2021-10-08 |
Family
ID=77935805
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110691959.4A Pending CN113487693A (en) | 2021-06-22 | 2021-06-22 | Path fitting method and device, electronic equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113487693A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2023137938A1 (en) * | 2022-01-19 | 2023-07-27 | 网易（杭州）网络有限公司 | Path planning method and apparatus for virtual character, electronic device, and storage medium |
-
2021
- 2021-06-22 CN CN202110691959.4A patent/CN113487693A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2023137938A1 (en) * | 2022-01-19 | 2023-07-27 | 网易（杭州）网络有限公司 | Path planning method and apparatus for virtual character, electronic device, and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR102237559B1 (en) | Method for Generating Road Topology Information Based on Segment Modeling for High Definition Map | |
| CN105277203B (en) | A kind of generation method of navigation action, air navigation aid and equipment | |
| CN112622933B (en) | Method and device for determining vehicle stop point | |
| CN114724108B (en) | Lane line processing method and device | |
| CN110689492B (en) | Image edge smoothing method and device | |
| CN110377041B (en) | Reference line generation method and device, terminal equipment and storage medium | |
| CN113739811B (en) | Method and equipment for training key point detection model and generating high-precision map lane line | |
| CN113487693A (en) | Path fitting method and device, electronic equipment and storage medium | |
| CN111737637A (en) | Path curve generation method, device, equipment and storage medium | |
| CN110796854A (en) | Method and related device for checking road speed limit value | |
| CN110909398A (en) | Graph fitting method and device, electronic equipment and computer readable storage medium | |
| CN114670823A (en) | Method, device and equipment for correcting running track and automatic driving vehicle | |
| CN111319614B (en) | Automatic parking path planning method and device and storage medium | |
| CN110749332B (en) | Curvature optimization method and device of RS curve, computer equipment and storage medium | |
| CN116738579A (en) | Automatic construction method and system for parting line of automobile panel | |
| CN111192216A (en) | Lane line smoothing method and system | |
| CN111295695A (en) | Three-dimensional grid data simplification method and device | |
| CN114179805B (en) | Driving direction determining method, device, equipment and storage medium | |
| CN110598327B (en) | Waveguide drawing method, system, storage medium and terminal | |
| CN112686904A (en) | Lane division method, lane division device, electronic equipment and storage medium | |
| CN111093153B (en) | Method and device for determining region shape, electronic equipment and storage medium | |
| CN113487864A (en) | Method, device, electronic equipment and storage medium for determining intersection state | |
| CN111788604A (en) | Image processing method, system, electronic device, robot and storage device | |
| CN110908338B (en) | Blade profile spline reverse curvature correction method and system for turbine blade | |
| CN116424315B (en) | Collision detection method, collision detection device, electronic equipment, automatic driving vehicle and medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |