Trajectory point fitting methods, apparatus, electronic devices, and readable storage media

Abstract

This application provides a trajectory point fitting method, apparatus, electronic device, and readable storage medium. Based on the straight-line distance between any adjacent trajectory points in the trajectory point set as the distance increment, multiple sets of cubic spline curves are generated based on the trajectory point set and the distance increment. The endpoints of the cubic spline curves satisfy a specific terminal reciprocal continuity condition, while the cubic spline curves of other trajectory points that are not endpoints are continuous in the first derivative and in the second reciprocal. Under the premise of ensuring the smoothness of the final curve, the amount of data processing is small, thereby improving the vehicle tracking response speed.

Description

Technical Field

[0001] This application relates to the field of intelligent driving, and more specifically, to a trajectory point fitting method, apparatus, electronic device, and readable storage medium. Background Technology

[0002] In the field of intelligent driving, the common approach is to generate multiple cubic spline curves from trajectory points, and then combine these multiple cubic spline curves to obtain the entire curve, which serves as the trajectory path required for vehicle tracking.

[0003] There are many existing methods for generating multiple cubic spline curves based on trajectory points. One method is to use a global solution, setting up a system of equations based on all trajectory points and simultaneously solving for the parameters of each cubic spline curve segment.

[0004] The advantage of the overall solution method is that it can ensure the continuity of the second derivative of all points on the fitted curve. The disadvantage is that when there are many trajectory points, the computational load of the joint solution is very large and the calculation is particularly time-consuming, resulting in untimely vehicle tracking and failing to meet the requirements of intelligent driving control algorithms. Summary of the Invention

[0005] In view of this, this application provides a trajectory point fitting method, as follows:

[0006] A trajectory point fitting method, comprising:

[0007] Obtain the set of trajectory points;

[0008] The distance increment is based on the straight-line distance between any two adjacent trajectory points;

[0009] At least two sets of cubic spline curves are generated based on the set of trajectory points and the distance increment. Any combination of fitted curves contains at least two cubic spline curves. The first and second derivatives of the at least two cubic spline curves contained in the arbitrary set of fitted curves are continuous. The endpoints of any set of cubic spline curves satisfy the agreed endpoint derivative continuity condition.

[0010] A target continuous curve is obtained based on the at least two sets of cubic spline curves, and the target continuous curve is used as a reference path for the vehicle.

[0011] Optionally, in the above method, generating at least two sets of cubic spline curves based on the set of trajectory points and the distance increment includes:

[0012] The set of trajectory points is divided into at least two fitting intervals, and each fitting interval contains at least two trajectory points;

[0013] In each fitting interval, based on the distance increment between any adjacent trajectory points, the functional relationship of non-endpoint trajectory points in the fitting interval, and the conditions for continuity of the first and second derivatives, a first target formula set is determined. The first target formula set is the formula of the cubic spline curve corresponding to the non-endpoint trajectory points in the fitting interval.

[0014] In each fitting interval, based on the endpoints in the fitting interval and the agreed endpoint derivative continuity condition, a second target formula set is determined. The second target formula set is the formula of the cubic spline curve corresponding to the endpoints in the fitting interval, and the endpoints include the beginning and the end.

[0015] Based on the first and second target formula sets in each fitting interval, a full-rank linear equation set for the fitting interval is obtained;

[0016] Based on the set of trajectory points and the full-rank linear equation system, the fitting coefficients of the fitting interval are obtained;

[0017] Based on the fitting coefficients, a set of cubic spline curves corresponding to each fitting interval is obtained.

[0018] Optionally, in the above method, dividing the set of trajectory points into at least two fitting intervals includes:

[0019] Get the preset number of line segments in each group;

[0020] Based on the preset number of line segments in each group and the number of trajectory points in the trajectory point set, the number of fitting intervals is determined, and any two adjacent fitting intervals have the same trajectory points.

[0021] Optionally, the above method, in each fitting interval, determines a first set of target formulas based on the distance increment between any adjacent trajectory points, the functional relationship between non-endpoint trajectory points in the fitting interval, and the conditions for continuity of the first and second derivatives, including:

[0022] The first constant is determined based on the x-coordinate / y-coordinate of the first trajectory point in the first fitting interval, and the first formula is obtained.

[0023] Based on the first constant, the function value conditions of the x-coordinate / y-coordinate of the second trajectory point are obtained, and the second formula is obtained. The first trajectory point and the second trajectory point are adjacent in order, and the first trajectory point is before the second trajectory point.

[0024] Based on the continuity condition of the first derivative of the first trajectory point and the second trajectory point, the third formula is obtained;

[0025] Based on the continuity condition of the second derivatives of the first trajectory point and the second trajectory point, the fourth formula is obtained;

[0026] The first target formula set is obtained based on the first formula, the second formula, the third formula, and the fourth formula.

[0027] Optionally, in the above method, within each fitting interval, based on the endpoints of the fitting interval and a pre-defined endpoint derivative continuity condition, a second set of objective formulas is determined, including:

[0028] Determine the position type of the fitting interval to be processed, including the start position, end position, and middle position;

[0029] If the fitting interval is the initial position fitting interval, the formula for determining the second derivative of the initial endpoint in the fitting interval to be zero and the formula for determining the second derivative of the terminal endpoint in the fitting interval based on the second difference are used to obtain the second target formula set.

[0030] If the fitting interval is a fitting interval at an intermediate position, the second target formula set is obtained based on the first derivative formula of the first derivative continuity condition between the starting endpoint of the fitting interval and the previous fitting interval, and the second derivative formula of the terminal endpoint of the fitting interval determined based on the second difference.

[0031] If the fitting interval is the fitting interval at the end position, determine the first derivative formula for the continuity condition of the first derivative of the starting endpoint in the fitting interval with the previous fitting interval, and the formula for determining that the second derivative of the ending endpoint in the fitting interval is zero based on the second difference, to obtain the second target formula set.

[0032] A trajectory point fitting device, comprising:

[0033] The acquisition module is used to obtain the set of trajectory points;

[0034] The incremental determination module is used to determine the distance increment based on the straight-line distance between any two adjacent trajectory points.

[0035] The spline curve determination module is used to generate at least two sets of cubic spline curves based on the set of trajectory points and the distance increment. Any fitting curve combination contains at least two cubic spline curves. The first and second derivatives of the at least two cubic spline curves contained in the arbitrary fitting curve group are continuous. The endpoints of any set of cubic spline curves satisfy the agreed endpoint derivative continuity condition.

[0036] A continuous curve determination module is used to obtain a target continuous curve based on the at least two sets of cubic spline curves, the target continuous curve being used as a reference path for the vehicle.

[0037] Optionally, in the above-described apparatus, the spline curve determination module includes:

[0038] A partitioning unit is used to divide the set of trajectory points into at least two fitting intervals, each fitting interval containing at least two trajectory points;

[0039] The first determining unit is used to determine a first target formula set in each fitting interval based on the distance increment between any adjacent trajectory points, the functional relationship of non-endpoint trajectory points in the fitting interval, and the conditions of continuity of the first and second derivatives. The first target formula set is the formula of the cubic spline curve corresponding to the non-endpoint trajectory points in the fitting interval.

[0040] The second determining unit is used to determine a second target formula set in each fitting interval based on the endpoints in the fitting interval and the agreed endpoint derivative continuity condition. The second target formula set is the formula of the cubic spline curve corresponding to the endpoints in the fitting interval, and the endpoints include the beginning and the end.

[0041] A combination unit is used to obtain a full-rank linear equation system for the fitting interval based on the first target formula group and the second target formula group in each fitting interval;

[0042] The third determining unit is used to obtain the fitting coefficients of the fitting interval based on the set of trajectory points and the full-rank linear equation system.

[0043] The spline curve unit is used to obtain a set of cubic spline curves corresponding to each fitting interval based on the fitting coefficients.

[0044] Optionally, in the above-described apparatus, the dividing unit includes:

[0045] Get sub-units, used to obtain the preset number of line segments in each group;

[0046] The sub-unit is determined based on the preset number of line segments in each group and the number of trajectory points in the trajectory point set, to determine the number of fitting intervals, wherein any two adjacent fitting intervals have the same trajectory points.

[0047] An electronic device includes: a memory and a processor;

[0048] The memory stores the processing program;

[0049] The processor is used to load and execute the processing program stored in the memory to implement the steps of the trajectory point fitting method as described in any of the preceding claims.

[0050] A readable storage medium having a computer program stored thereon, the computer program being invoked and executed by a processor to implement the steps of the trajectory point fitting method as described in any one of the preceding claims.

[0051] As can be seen from the above technical solution, this application provides a trajectory point fitting method, including: obtaining a trajectory point set; using the straight-line distance between any adjacent trajectory points as the distance increment; generating at least two sets of cubic spline curves based on the trajectory point set and the distance increment, wherein any fitting curve combination contains at least two cubic spline curves, and the first and second derivatives of the at least two cubic spline curves contained in the arbitrary fitting curve set are continuous, and the endpoints of any set of cubic spline curves satisfy a predetermined endpoint derivative continuity condition; obtaining a target continuous curve based on the at least two sets of cubic spline curves, wherein the target continuous curve is used as a reference path for the vehicle. In this solution, the straight-line distance between any adjacent trajectory points in the trajectory point set is used as the distance increment, and multiple sets of cubic spline curves are generated based on the trajectory point set and the distance increment. The endpoints of the cubic spline curves satisfy a specific terminal reciprocal continuity condition, while the cubic spline curves of other trajectory points that are not endpoints are continuous in terms of first and second derivatives. Compared with the overall solution in the prior art, this method reduces the amount of data processing while ensuring the smoothness of the final curve, thus improving the vehicle tracking response speed. Attached Figure Description

[0052] To more clearly illustrate the technical solutions of the embodiments of this application, the drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only embodiments of this application. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0053] Figure 1 This is a flowchart of Embodiment 1 of a trajectory point fitting method provided in this application;

[0054] Figure 2 This is a flowchart of Embodiment 2 of a trajectory point fitting method provided in this application;

[0055] Figure 3 This is a flowchart of Embodiment 3 of a trajectory point fitting method provided in this application;

[0056] Figure 4 This is a flowchart of Embodiment 4 of a trajectory point fitting method provided in this application;

[0057] Figure 5 This is a flowchart of Embodiment 5 of a trajectory point fitting method provided in this application;

[0058] Figure 6 This is a schematic diagram of the target continuous curve obtained in the scenario of the application trajectory point fitting method provided in this application;

[0059] Figure 7 This is a schematic diagram of an embodiment of a trajectory point fitting device provided in this application. Detailed Implementation

[0060] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0061] like Figure 1 The diagram shown is a flowchart of an embodiment 1 of a trajectory point fitting method provided in this application. This method is applied to an electronic device that can process the trajectory of a vehicle being tracked. The method includes the following steps:

[0062] Step S101: Obtain the set of trajectory points;

[0063] The trajectory point fitting method provided in this embodiment can be applied to in-vehicle processors or application servers that provide navigation services for vehicles. This application does not limit the electronic devices that execute this trajectory point fitting method.

[0064] Among them, trajectory points are points in the trajectory of the vehicle to be tracked.

[0065] Specifically, the set of trajectory points contains several trajectory points, which are identified by two-dimensional coordinates.

[0066] For example, the sequence of trajectory points involved in the path is (X i ,Y i ), i = 1, 2, 3, ..., N

[0067] Where N≥1, and N is the number of trajectory points.

[0068] In practice, the coordinates and number of trajectory points in the set of trajectory points can be set according to the actual situation. This application does not impose any restrictions on the specific coordinates and number of trajectory points.

[0069] Step S102: Use the straight-line distance between any two adjacent trajectory points as the distance increment;

[0070] Among them, the straight-line distance between trajectory points serves as the sequence of incremental and absolute distance traveled to create a reference path.

[0071] The specific process is as follows:

[0072]

[0073] Among them, (X) i ,Y i ) is the i-th trajectory point, (X)i+1 ,Y i+1 ) is the (i+1)th trajectory point, ΔS i It is the distance increment between the i-th trajectory point and the (i+1)-th trajectory point.

[0074] S0 is the first trajectory point, and its distance increment is 0.

[0075] Among them, the above three formulas can obtain the distance increment between trajectory points and the absolute sequence of the distance increment.

[0076] Step S103: Generate at least two sets of cubic spline curves based on the set of trajectory points and the distance increment;

[0077] Among them, any set of fitted curves contains at least two cubic spline curves, and the first and second derivatives of the at least two cubic spline curves in the set of fitted curves are continuous. The endpoints of any set of cubic spline curves satisfy the agreed endpoint derivative continuity condition.

[0078] Based on this set of trajectory points and the aforementioned distance increment, multiple sets of cubic spline curves can be obtained.

[0079] Moreover, the derivative continuity conditions at the non-endpoints and endpoints of the cubic spline curve are different.

[0080] Specifically, at non-endpoints, the first and second derivatives must be continuous, while at endpoints, the condition for continuous terminal derivatives must be met.

[0081] Among them, any two adjacent points (X) i ,Y i ) to (X i+1 ,Y i+1 The parametric equations of the cubic spline curves between () are as follows:

[0082]

[0083] Among them, a xi b xi c xi d xi and a yi b yi c yi d yi These are the fitting parameters to be determined.

[0084] It should be noted that the first and second derivatives are continuous at non-endpoints, ensuring a smooth curve at non-endpoints, while the terminal derivative is continuous at endpoints, ensuring a smooth curve at the endpoints.

[0085] It should be noted that the specific process of generating the cubic spline curve will be described in detail in the subsequent instructions, and will not be described in detail in this embodiment.

[0086] Step S104: Obtain a target continuous curve based on the at least two sets of cubic spline curves, the target continuous curve being used as a reference path for the vehicle.

[0087] Among them, by connecting the multiple sets of cubic spline curves obtained in the above steps, the target continuous curve can be obtained.

[0088] The target continuous curve is a continuous and smooth trajectory curve. The vehicle path is controlled based on the target continuous curve, so that the driver's lateral control algorithm can output a smooth steering wheel angle and achieve stable path tracking.

[0089] In summary, this embodiment provides a trajectory point fitting method, comprising: obtaining a set of trajectory points; using the straight-line distance between any two adjacent trajectory points as the distance increment; generating at least two sets of cubic spline curves based on the set of trajectory points and the distance increment, wherein each set of fitted curves contains at least two cubic spline curves, and the first and second derivatives of the at least two cubic spline curves within each set of fitted curves are continuous, and the endpoints of any set of cubic spline curves satisfy a predetermined endpoint derivative continuity condition; and obtaining a target continuous curve based on the at least two sets of cubic spline curves. In this scheme, the straight-line distance between any two adjacent trajectory points in the set of trajectory points is used as the distance increment, and multiple sets of cubic spline curves are generated based on the set of trajectory points and the distance increment. The endpoints of these cubic spline curves satisfy a specific terminal reciprocal continuity condition, while the cubic spline curves of other trajectory points (not endpoints) are continuous in both their first and second derivatives. This approach ensures a smooth final curve while minimizing data processing and improving vehicle tracking response speed.

[0090] like Figure 2 The flowchart shown is a second embodiment of a trajectory point fitting method provided in this application. The method includes the following steps:

[0091] Step S201: Obtain the set of trajectory points;

[0092] Step S202: Use the straight-line distance between any two adjacent trajectory points as the distance increment;

[0093] Steps S201-202 are the same as steps S101-102 in Example 1, and will not be described again in this example.

[0094] Step S203: Divide the set of trajectory points into at least two fitting intervals, each fitting interval containing at least two trajectory points;

[0095] Specifically, several trajectory points in the set of trajectory points are divided into multiple fitting intervals, and each fitting interval contains multiple trajectory points.

[0096] Specifically, the set of trajectory points is a number of trajectory points sorted by coordinates. When dividing the points into fitting intervals, the intervals can be divided according to the order of the coordinates.

[0097] Specifically, the set of trajectory points can be divided into multiple fitting intervals according to the set division rules.

[0098] It should be noted that the division rule can be a fixed rule or a rule set according to the actual situation. This application does not restrict the form of the division rule.

[0099] Step S204: In each fitting interval, based on the distance increment between any adjacent trajectory points, the functional relationship of non-endpoint trajectory points in the fitting interval, and the conditions for continuity of the first and second derivatives, determine the first target formula set;

[0100] The first target formula group is the formula for the cubic spline curve corresponding to the non-endpoint trajectory points within the fitting interval.

[0101] Each fitting interval contains multiple adjacent trajectory points, and the distance increment between any two adjacent trajectory points is determined sequentially.

[0102] By defining the functional relationship and the conditions for continuity of the first and second derivatives for the non-endpoint trajectory points within the fitting interval, the first target formula set corresponding to the non-endpoint trajectory point can be determined.

[0103] The process of generating the first target formula set will be described in detail in the subsequent instructions, and will not be described in detail in this embodiment.

[0104] Step S205: In each fitting interval, based on the endpoints of the fitting interval and the agreed endpoint derivative continuity condition, determine the second target formula set;

[0105] The second target formula set is the formula for the cubic spline curve corresponding to the endpoints within the fitting interval, and the endpoints include the beginning and the end.

[0106] In each fitting interval, the endpoints of multiple trajectory points are used to determine a second set of objective formulas for the endpoints based on the agreed endpoint derivative continuity condition.

[0107] The second objective set of formulas is the formula for the cubic spline curve corresponding to that endpoint.

[0108] The process of generating the second objective formula set will be described in detail in the subsequent instructions, and will not be described in detail in this embodiment.

[0109] Step S206: Based on the first target formula group and the second target formula group in each fitting interval, obtain the full-rank linear equation group of the fitting interval;

[0110] By combining the first objective formula set and the second objective formula set determined for each fitting interval, a full-rank linear equation system for that fitting interval is obtained.

[0111] It should be noted that the coefficients in the second objective formula set and the first objective formula set are undetermined, that is, the coefficients of the full-rank linear equation system of the fitting interval are undetermined. The fitting coefficients determined in subsequent steps are the coefficients of the full-rank linear equation system.

[0112] Step S207: Based on the set of trajectory points and the full-rank linear equation system, obtain the fitting coefficients of the fitting interval;

[0113] Specifically, by combining the coordinates of each trajectory point in the fitting interval with the full-rank linear equation system, the coefficients of the full-rank linear equation system can be solved, which yields the fitting coefficients of the fitting interval.

[0114] Step S208: Based on the fitting coefficients, obtain a set of cubic spline curves corresponding to each fitting interval;

[0115] Substituting the fitting coefficient into the full-rank linear equation system yields a definite set of equations, from which the cubic spline curve for the corresponding fitting interval can be obtained.

[0116] Step S209: Obtain the target continuous curve based on the at least two sets of cubic spline curves.

[0117] Step S209 is the same as step S104 in Example 1, and will not be described again in this example.

[0118] In summary, the trajectory point fitting method provided in this embodiment includes: firstly, dividing the trajectory point set into multiple fitting intervals; determining a first target formula set based on the functional relationship of the non-endpoint trajectory points in each fitting interval and the conditions for continuity of the first and second derivatives; and determining a second target formula set based on the derivative continuity conditions of the endpoints in each fitting interval. This achieves the formulas for the cubic spline curves corresponding to the non-endpoints and endpoints within the fitting interval. Based on the first and second target formula sets and the trajectory point set, the fitting coefficients of the fitting interval are obtained. Then, based on the fitting coefficients, the cubic spline curves corresponding to the fitting interval are determined. Similarly, the cubic spline curves corresponding to each fitting interval are obtained. In this scheme, the method for obtaining the cubic spline curves corresponding to each fitting interval is explained in detail.

[0119] like Figure 3 The flowchart shown is a third embodiment of a trajectory point fitting method provided in this application. The method includes the following steps:

[0120] Step S301: Obtain the set of trajectory points;

[0121] Step S302: Use the straight-line distance between any two adjacent trajectory points as the distance increment;

[0122] Steps S301-302 are the same as steps S201-202 in Example 2, and will not be described again in this example.

[0123] Step S303: Obtain the preset number of line segments for each group;

[0124] The number of line segments in each group is a pre-set value.

[0125] For example, the number of line segments in each group is 5, 2, etc. This application does not impose any restrictions on the value of the number of line segments in each group.

[0126] Step S304: Determine the number of fitting intervals based on the preset number of line segments in each group and the number of trajectory points in the trajectory point set;

[0127] In this context, any two adjacent fitting intervals have the same trajectory points.

[0128] Specifically, the number of fitting intervals can be obtained by taking the quotient of the number of trajectory points in the trajectory point set and the number of line segments in each group.

[0129] In practice, in order to ensure that each adjacent fitting interval overlaps, that is, has the same trajectory points, it is necessary to use the method of rounding down to determine the number of fitting intervals.

[0130] Where the number of trajectory points is N (N≥1) and the number of line segments in each group is p (p≥1), then the total number of groups (M) is:

[0131]

[0132] The function floor() is a rounding operation that discards the decimal part.

[0133] As an example, if there are 11 trajectory points in the set, and each group has 4 line segments, then the number of groups is 2. Divide the 11 trajectory points into two groups: the first group is trajectory points 1-5 (five points forming 4 curves), and the second group is trajectory points 5-11 (7 points forming 6 curves).

[0134] Generally, except for the last group, it is necessary to ensure that the number of line segments in each of the first few groups corresponds to the preset value of the number of line segments in each group. The number of line segments in the last group may be greater than the preset value of the number of line segments in each group.

[0135] Where, let n i This represents the number of fitted curves contained in the i-th group, as detailed below:

[0136]

[0137] Step S305: In each fitting interval, based on the distance increment between any adjacent trajectory points, the functional relationship of non-endpoint trajectory points in the fitting interval, and the conditions for continuity of the first and second derivatives, determine the first target formula set;

[0138] Step S306: In each fitting interval, determine the second target formula set based on the derivative continuity condition of the endpoints of the fitting interval;

[0139] Step S307: Based on the first target formula group and the second target formula group in each fitting interval, obtain the full-rank linear equation group of the fitting interval;

[0140] Step S308: Based on the set of trajectory points and the full-rank linear equation system, obtain the fitting coefficients of the fitting interval;

[0141] Step S309: Based on the fitting coefficients, obtain a set of cubic spline curves corresponding to each fitting interval;

[0142] Step S310: Obtain the target continuous curve based on the at least two sets of cubic spline curves.

[0143] Steps S305-310 are the same as steps S204-209 in Example 2, and will not be described again in this example.

[0144] In summary, the trajectory point fitting method provided in this embodiment includes: grouping the trajectory points in the trajectory point set based on a preset number of line segments in each group, obtaining the number of fitting intervals and the trajectory points contained in each fitting interval, thus providing a basis for subsequently determining the cubic spline curves of each fitting interval.

[0145] like Figure 4 The flowchart shown is a fourth embodiment of a trajectory point fitting method provided in this application. The method includes the following steps:

[0146] Step S401: Obtain the set of trajectory points;

[0147] Step S402: Use the straight-line distance between any two adjacent trajectory points as the distance increment;

[0148] Step S403: Divide the set of trajectory points into at least two fitting intervals, each fitting interval containing at least two trajectory points;

[0149] Steps S401-402 are the same as steps S401-403 in Example 2, and will not be described again in this example.

[0150] Step S404: Determine the first constant based on the x-coordinate / y-coordinate of the first trajectory point in the first fitting interval to obtain the first formula;

[0151] In this embodiment, the first fitting interval is used as an example for illustration. The method for determining the first target formula group for each fitting interval is the same as the method for determining the first fitting interval.

[0152] The determination is based on two adjacent first trajectory points and second trajectory points.

[0153] The coordinates of the first trajectory point are (X... i ,Y i The coordinates of the second trajectory point are (X... i+1 ,Y i+1 ).

[0154] In this embodiment, the explanation is based on the horizontal axis. The calculation process for the vertical axis is the same as that for the horizontal axis and will not be described in detail here.

[0155] Among them, the formula for generating the functional relationship of each trajectory point in the first fitting interval is provided.

[0156] This functional relationship can generate 2n. i Equations.

[0157] First, determine the constants for the function conditions used to calculate adjacent trajectory points.

[0158] Specifically as follows:

[0159] x i (0)=d xi =X i i = 1, 2, ..., n i (4)

[0160] Step S405: Based on the first constant, obtain the function value conditions of the x-coordinate / y-coordinate of the second trajectory point, and obtain the second formula;

[0161] The first trajectory point and the second trajectory point are adjacent in order, and the first trajectory point precedes the second trajectory point.

[0162] Then, based on the constant of the function condition for calculating adjacent trajectory points, the function value condition between the first trajectory point and the second trajectory point is determined.

[0163] Specifically as follows:

[0164]

[0165] The first and second formulas together form the function value conditions between two adjacent trajectory points.

[0166] Step S406: Based on the continuity condition of the first derivative of the first trajectory point and the second trajectory point, the third formula is obtained;

[0167] Among them, the first derivative and the second derivative are continuous between two adjacent trajectory points.

[0168] In step S406, the third formula is generated based on the continuity condition of the first derivative.

[0169] Specifically as follows:

[0170]

[0171] Step S407: Based on the continuity condition of the second derivatives of the first trajectory point and the second trajectory point, the fourth formula is obtained;

[0172] In step S407, the third formula is generated based on the continuity condition of the second derivative.

[0173] Specifically as follows:

[0174] x i (ΔS) i )-x i+1 (0) = 6a xi ΔS i +2b xi -6a x,i+1 ΔS i+1 -2b x,i+1 =0, i=1,2,...,n i -1(7)

[0175] Step S408: Obtain the first target formula group based on the first formula, the second formula, the third formula, and the fourth formula;

[0176] Among them, based on the four types of formulas determined in steps S404-407 above, a first target formula set is obtained for each trajectory point that is not an endpoint in the first fitting interval.

[0177] This first set of objective formulas includes the first and second formulas for the function condition, and the third and fourth formulas for the derivative continuity condition. Specifically:

[0178] Function condition:

[0179]

[0180] Condition for continuity of derivative:

[0181]

[0182] The first set of objective formulas, combined with the subsequently determined second set of objective formulas, can determine the fitting coefficients of the fitting interval, thereby obtaining the cubic spline curve corresponding to the fitting interval.

[0183] Step S409: In the first fitting interval, based on the derivative continuity condition of the endpoints in the first fitting interval, determine the second target formula set;

[0184] Step S410: Based on the first target formula group and the second target formula group in each fitting interval, obtain the full-rank linear equation group of the fitting interval;

[0185] Step S411: Based on the set of trajectory points and the full-rank linear equation system, obtain the fitting coefficients of the fitting interval;

[0186] Step S412: Based on the fitting coefficients, obtain a set of cubic spline curves corresponding to each fitting interval;

[0187] Step S413: Obtain the target continuous curve based on the at least two sets of cubic spline curves.

[0188] Steps S409-413 are the same as steps S205-209 in Example 2, and will not be described again in this example.

[0189] In summary, the trajectory point fitting method provided in this embodiment includes: based on adjacent first and second trajectory points in a first fitting interval, forming a first target formula group based on function value conditions, first derivative continuity conditions, and second derivative continuity conditions, respectively, to determine the target formula group for generating cubic spline curves corresponding to each trajectory point that is not an endpoint in the first fitting interval, thus providing a basis for subsequently determining the fitting coefficients of the cubic spline curves.

[0190] like Figure 5 The flowchart shown is a sample of embodiment 5 of a trajectory point fitting method provided in this application. The method includes the following steps:

[0191] Step S501: Obtain the set of trajectory points;

[0192] Step S502: Use the straight-line distance between any two adjacent trajectory points as the distance increment;

[0193] Step S503: Divide the set of trajectory points into at least two fitting intervals, each fitting interval containing at least two trajectory points;

[0194] Step S504: In each fitting interval, based on the distance increment between any adjacent trajectory points, the functional relationship of non-endpoint trajectory points in the fitting interval, and the conditions for continuity of the first and second derivatives, determine the first target formula set;

[0195] Steps S501-504 are the same as steps S201-204 in Example 2, and will not be described again in this example.

[0196] Step S505: Determine the location type of the fitting interval to be processed;

[0197] The position types include start position, end position, and intermediate position.

[0198] In order to ensure that the final curve is smooth at the connection points of each fitting interval, it is necessary to process the endpoint positions of each fitting interval to ensure that the endpoint satisfies the agreed endpoint derivative continuity condition.

[0199] Among them, different derivative continuity conditions are applied to the starting endpoint and the terminal endpoint.

[0200] Specifically, if the fitting interval to be processed is in different positions, different derivative continuity conditions are used for its starting endpoint and ending endpoint.

[0201] In this embodiment, the explanation is based on the horizontal axis. The calculation process for the vertical axis is the same as that for the horizontal axis and will not be described in detail here.

[0202] Step S506: If the fitting interval is the initial position fitting interval, determine the formula for the second derivative of the initial endpoint in the fitting interval being zero and the formula for determining the second derivative of the terminal endpoint in the fitting interval based on the second difference, to obtain the second target formula set;

[0203] If the fitting interval is the initial position fitting interval, based on the natural condition of the second derivative, the second derivative of the initial endpoint is 0, and the second derivative of the terminal is estimated by second-order difference.

[0204] Specifically as follows:

[0205]

[0206] Where j represents the order of the fitting interval, and when j=1, the endpoint condition of the above formula (8) is satisfied.

[0207] Where p represents the p-th trajectory point in the fitting interval of the starting position.

[0208] Step S507: If the fitting interval is a fitting interval at the middle position, the second target formula group is obtained based on the first derivative formula of the first derivative continuity condition between the starting endpoint of the fitting interval and the previous fitting interval, and the second derivative formula of the terminal endpoint of the fitting interval determined based on the second difference.

[0209] If the fitting interval is the middle fitting interval, in order to ensure that the trajectory corresponding to any two consecutive fitting intervals in the generated target continuous curve is continuous and smooth, the starting endpoint is continuous with the first derivative of the previous fitting interval, and the second derivative of the terminal is estimated by second-order difference.

[0210] Specifically as follows:

[0211]

[0212] Where j represents the order of the fitting interval, and when the value of j is from 2 to M-1, the endpoint condition of the above formula (9) is satisfied.

[0213] Where p represents the p-th trajectory point in the fitting interval of the starting position.

[0214] Where, x pj-p ′ is the first derivative of the last trajectory point in the previous fitted interval.

[0215] Step S508: If the fitting interval is the fitting interval of the end position, determine the first derivative formula of the first derivative continuity condition between the starting endpoint and the previous fitting interval in the fitting interval, and the formula for determining that the second derivative of the ending endpoint in the fitting interval is zero based on the second difference, to obtain the second target formula set.

[0216] If the fitting interval is the fitting interval at the end position, in order to ensure that the trajectories corresponding to any two consecutive fitting intervals in the generated target continuous curve are continuous and smooth, the first derivative of the starting endpoint is continuous with the first derivative of the previous fitting interval. Based on the natural condition of the second derivative, the second derivative of the ending endpoint is 0.

[0217]

[0218] Where j represents the order of the fitting interval, and when the value of j is M, the endpoint condition of the above formula (10) is satisfied.

[0219] Where p represents the p-th trajectory point in the fitting interval of the starting position.

[0220] The second objective formula set, combined with the previously determined first objective formula set, can determine the fitting coefficients of the fitting interval, thereby obtaining the cubic spline curve corresponding to the fitting interval.

[0221] Step S509: Based on the first target formula group and the second target formula group in each fitting interval, obtain the full-rank linear equation group of the fitting interval;

[0222] Step S510: Based on the set of trajectory points and the full-rank linear equation system, obtain the fitting coefficients of the fitting interval;

[0223] Step S511: Based on the fitting coefficients, obtain a set of cubic spline curves corresponding to each fitting interval;

[0224] Step S512: Obtain the target continuous curve based on the at least two sets of cubic spline curves.

[0225] Steps S509-512 are the same as steps S206-209 in Example 2, and will not be described again in this example.

[0226] In summary, the trajectory point fitting method provided in this embodiment includes: determining the target formula set for generating cubic spline curves corresponding to each trajectory point of the endpoints in the fitting interval based on different position types of the fitting interval and different endpoint derivative continuity conditions of the starting endpoints and ending endpoints in each fitting interval, so as to provide a basis for subsequently determining the fitting coefficients of the cubic spline curves.

[0227] Corresponding to the above embodiment of the trajectory point fitting method provided in this application, this application also provides scenario embodiments for applying the trajectory point fitting method.

[0228] In this implementation scenario, assuming there are 10 trajectory points, and assuming the number of line segments p in each group is equal to 3, then the number of groups M is equal to 3. Solve for the three cubic spline curves of each fitting interval.

[0229] The three sets of trajectory points are as follows: The first set includes point 1 (X1,Y1), point 2 (X2,Y2), point 3 (X3,Y3), and point 4 (X4,Y4); the second set includes point 4 (X4,Y4), point 5 (X5,Y5), point 6 (X6,Y6), and point 7 (X7,Y7); the third set includes point 7 (X7,Y7), point 8 (X8,Y8), point 9 (X9,Y9), and point 10 (X4,Y4). 10 ,Y 10 ).

[0230] Among them, the three cubic spline curves are solved using the four trajectory points of the first group.

[0231]

[0232] Among them, a x1 b x1 c x1 d x1 ax2 b x2 c x2 d x2 a x3 b x3 c x3 d x3 These are the coefficients of three cubic spline curves, totaling 12 unknown coefficients. The values ​​of these 12 unknown coefficients can be obtained by solving a system of the aforementioned 12 equations. Using the same method, the coefficients of the three cubic spline curves where y varies with ΔS can be calculated, from which the parametric expression of the three curve segments between the four points can be obtained.

[0233] Among them, the three cubic spline curves are solved by using the four trajectory points of the second group.

[0234]

[0235] Among them, a x4 b x4 c x4 d x4 a x5 b x5 c x5 d x5 a x6 b x6 c x6 d x6 These are the coefficients of three cubic spline curves, totaling 12 unknown coefficients. The values ​​of these 12 unknown coefficients can be obtained by solving a system of the aforementioned 12 equations. Using the same method, the coefficients of the three cubic spline curves where y varies with ΔS can be calculated, from which the parametric expression of the three curve segments between the four points can be obtained.

[0236] Among them, the three cubic spline curves are solved by using the four trajectory points of the third group.

[0237]

[0238] Among them, a x7 b x7 c x7 d x7 a x8 b x8 c x8 d x8 a x9 b x9 c x9 d x9These are the coefficients of three cubic spline curves, totaling 12 unknown coefficients. The values ​​of these 12 unknown coefficients can be obtained by solving a system of the aforementioned 12 equations. Using the same method, the coefficients of the three cubic spline curves where y varies with ΔS can be calculated, from which the parametric expression of the three curve segments between the four points can be obtained.

[0239] Through the above process, multiple sets of three-segment spline curves are obtained, and finally the target continuous curve is obtained.

[0240] like Figure 6 The diagram shows a target continuous curve obtained in a scenario where the trajectory point fitting method is applied. In this diagram, 9 cubic spline curves are fitted based on 10 trajectory points to finally obtain the target continuous curve. In the diagram, the dots represent trajectory points and the dashed lines represent the target continuous curve.

[0241] Corresponding to the trajectory point fitting method embodiment provided in this application above, this application also provides an apparatus embodiment for applying the trajectory point fitting method.

[0242] like Figure 7 The diagram shown is a structural schematic of an embodiment of a trajectory point fitting device provided in this application. The device includes the following components: an acquisition module 701, an increment determination module 702, a spline curve determination module 403, and a continuous curve determination module 704.

[0243] The acquisition module 701 is used to acquire a set of trajectory points;

[0244] The incremental determination module 702 is used to determine the distance increment based on the straight-line distance between any two adjacent trajectory points.

[0245] The spline curve determination module 703 is used to generate at least two sets of cubic spline curves based on the set of trajectory points and the distance increment. Any fitting curve combination contains at least two cubic spline curves. The first and second derivatives of the at least two cubic spline curves contained in the arbitrary fitting curve group are continuous. The endpoints of any set of cubic spline curves satisfy the agreed endpoint derivative continuity condition.

[0246] The continuous curve determination module 704 is used to obtain a target continuous curve based on the at least two sets of cubic spline curves, and the target continuous curve is used as a reference path for the vehicle.

[0247] Optionally, the spline curve determination module includes:

[0248] A partitioning unit is used to divide the set of trajectory points into at least two fitting intervals, each fitting interval containing at least two trajectory points;

[0249] The first determining unit is used to determine a first target formula set in each fitting interval based on the distance increment between any adjacent trajectory points, the functional relationship of non-endpoint trajectory points in the fitting interval, and the conditions of continuity of the first and second derivatives. The first target formula set is the formula of the cubic spline curve corresponding to the non-endpoint trajectory points in the fitting interval.

[0250] The second determining unit is used to determine a second target formula set in each fitting interval based on the endpoints in the fitting interval and the agreed endpoint derivative continuity condition. The second target formula set is the formula of the cubic spline curve corresponding to the endpoints in the fitting interval, and the endpoints include the beginning and the end.

[0251] A combination unit is used to obtain a full-rank linear equation system for the fitting interval based on the first target formula group and the second target formula group in each fitting interval;

[0252] The third determining unit is used to obtain the fitting coefficients of the fitting interval based on the set of trajectory points and the full-rank linear equation system.

[0253] The spline curve unit is used to obtain a set of cubic spline curves corresponding to each fitting interval based on the fitting coefficients.

[0254] Optionally, the partitioning unit includes:

[0255] Get sub-units, used to obtain the preset number of line segments in each group;

[0256] The sub-unit is determined based on the preset number of line segments in each group and the number of trajectory points in the trajectory point set, to determine the number of fitting intervals, wherein any two adjacent fitting intervals have the same trajectory points.

[0257] Optionally, the first determining unit is specifically used for:

[0258] The first constant is determined based on the x-coordinate / y-coordinate of the first trajectory point in the first fitting interval, and the first formula is obtained.

[0259] Based on the first constant, the function value conditions of the x-coordinate / y-coordinate of the second trajectory point are obtained, and the second formula is obtained. The first trajectory point and the second trajectory point are adjacent in order, and the first trajectory point is before the second trajectory point.

[0260] Based on the continuity condition of the first derivative of the first trajectory point and the second trajectory point, the third formula is obtained;

[0261] Based on the continuity condition of the second derivatives of the first trajectory point and the second trajectory point, the fourth formula is obtained;

[0262] The first target formula set is obtained based on the first formula, the second formula, the third formula, and the fourth formula.

[0263] Optionally, the second determining unit is specifically used for:

[0264] Determine the position type of the fitting interval to be processed, including the start position, end position, and middle position;

[0265] If the fitting interval is the initial position fitting interval, the formula for determining the second derivative of the initial endpoint in the fitting interval to be zero and the formula for determining the second derivative of the terminal endpoint in the fitting interval based on the second difference are used to obtain the second target formula set.

[0266] If the fitting interval is a fitting interval at an intermediate position, the second target formula set is obtained based on the first derivative formula of the first derivative continuity condition between the starting endpoint of the fitting interval and the previous fitting interval, and the second derivative formula of the terminal endpoint of the fitting interval determined based on the second difference.

[0267] If the fitting interval is the fitting interval at the end position, determine the first derivative formula for the continuity condition of the first derivative of the starting endpoint in the fitting interval with the previous fitting interval, and the formula for determining that the second derivative of the ending endpoint in the fitting interval is zero based on the second difference, to obtain the second target formula set.

[0268] It should be noted that the trajectory point fitting device provided in this embodiment can be applied to an in-vehicle processor or an application server that provides navigation services for vehicles. This application does not limit the device that performs this trajectory point fitting method.

[0269] Furthermore, the functions of each component structure of the trajectory point fitting device provided in this embodiment are explained in the method embodiment and will not be repeated in this embodiment.

[0270] In summary, this application provides a trajectory point fitting device that uses the straight-line distance between any adjacent trajectory points in a trajectory point set as the distance increment. Based on this trajectory point set and the distance increment, multiple sets of cubic spline curves are generated. The endpoints of these cubic spline curves satisfy a specific terminal reciprocal continuity condition, while the cubic spline curves of other trajectory points that are not endpoints are continuous in both the first derivative and the second reciprocal. Compared with the overall solution in the prior art, this device reduces the amount of data processing while ensuring the smoothness of the final curve, thereby improving the vehicle tracking response speed.

[0271] Corresponding to the above embodiment of the trajectory point fitting method provided in this application, this application also provides an electronic device and a readable storage medium corresponding to the trajectory point fitting method.

[0272] The electronic device includes: a memory and a processor;

[0273] The memory stores the processing program;

[0274] The processor is used to load and execute the processing program stored in the memory to implement the steps of the trajectory point fitting method as described in any of the preceding claims.

[0275] For details on the specific trajectory point fitting method for this electronic device, please refer to the aforementioned trajectory point fitting method embodiment.

[0276] The readable storage medium stores a computer program that is invoked and executed by a processor to implement the steps of the trajectory point fitting method as described in any of the preceding claims.

[0277] Specifically, the computer program stored on the readable storage medium executes the trajectory point fitting method, as described in the aforementioned trajectory point fitting method embodiments.

[0278] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. The apparatus provided in the embodiments is described simply because it corresponds to the method provided in the embodiments; relevant parts can be found in the method section.

[0279] The above description of the provided embodiments enables those skilled in the art to make or use this application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of this application. Therefore, this application is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features provided herein.

Claims

1. A trajectory point fitting method, characterized in that, The method comprises the following steps: obtaining a set of trajectory points; determining a distance increment between any adjacent trajectory points as a straight line distance between the adjacent trajectory points; dividing the set of trajectory points into at least two fitting intervals, each fitting interval containing at least two trajectory points; in each fitting interval, determining a first target formula group based on the distance increment between any adjacent trajectory points, a function relationship of non-end point trajectory points in the fitting interval, and a first derivative continuity and a second derivative continuity condition, the first target formula group being a formula of a cubic spline curve corresponding to the non-end point trajectory points in the fitting interval; in each fitting interval, determining a second target formula group based on end points in the fitting interval and a predetermined end point derivative continuity condition, the second target formula group being a formula of a cubic spline curve corresponding to the end points in the fitting interval, the end points including a start end and an end end; based on the first target formula group and the second target formula group in each fitting interval, obtaining a full-rank linear equation group of the fitting interval; based on the set of trajectory points and the full-rank linear equation group, obtaining a fitting coefficient of the fitting interval; based on the fitting coefficient, obtaining a set of cubic spline curves corresponding to each fitting interval; based on the at least two sets of cubic spline curves, obtaining a target continuous curve, the target continuous curve being used as a reference path of a vehicle.

2. The method of claim 1, wherein, The dividing the set of trajectory points into at least two fitting intervals comprises: obtaining a preset number of line segments in each group; based on the preset number of line segments in each group and a number of trajectory points in the set of trajectory points, determining a number of fitting intervals, any two adjacent fitting intervals having the same trajectory points.

3. The method of claim 1, wherein, In each fitting interval, determining the first target formula group based on the distance increment between any adjacent trajectory points, the function relationship of non-end point trajectory points in the fitting interval, and the first derivative continuity and the second derivative continuity condition comprises: determining a first constant based on an abscissa / ordinate of a first trajectory point in a first fitting interval, to obtain a first formula; obtaining a function value condition of an abscissa / ordinate of a second trajectory point based on the first constant, to obtain a second formula, the first trajectory point and the second trajectory point being adjacent in order, and the first trajectory point being before the second trajectory point; obtaining a third formula based on a first derivative continuity condition of the first trajectory point and the second trajectory point; obtaining a fourth formula based on a second derivative continuity condition of the first trajectory point and the second trajectory point; obtaining the first target formula group based on the first formula, the second formula, the third formula, and the fourth formula.

4. The method of claim 1, wherein, In each fitting interval, determining the second target formula group based on the end points in the fitting interval and the predetermined end point derivative continuity condition comprises: determining a position type of a fitting interval to be processed, the position type including a start position, an end position, and an intermediate position; if the fitting interval is a start position fitting interval, determining a formula of a second derivative of a start end point in the fitting interval being zero and a formula of a second derivative of an end end point in the fitting interval based on a second difference, to obtain the second target formula group. If the fitting interval is a middle position fitting interval, a first target formula group is determined based on a first derivative formula of a first derivative continuous condition of a start end point in the fitting interval and a last fitting interval, and a second derivative formula of a second derivative of a terminal end point in the fitting interval is determined based on a second difference, to obtain the second target formula group; If the fitting interval is an end position fitting interval, a first derivative formula of a first derivative continuous condition of a start end point in the fitting interval and a last fitting interval is determined, and a formula that a second derivative of a terminal end point in the fitting interval is zero is determined based on a second difference, to obtain the second target formula group.

5. A trajectory point fitting device, comprising: an obtaining module configured to obtain a trajectory point set; an increment determining module configured to determine a distance between any adjacent trajectory points as a distance increment; a spline curve determining module configured to generate at least two groups of cubic spline curves based on the trajectory point set and the distance increment, any fitting curve combination containing at least two cubic spline curves, the first and second derivatives of the at least two cubic spline curves contained in any fitting curve combination being continuous, and the end points of any group of cubic spline curves satisfying a prescribed end point derivative continuous condition; a continuous curve determining module configured to obtain a target continuous curve based on the at least two groups of cubic spline curves, the target continuous curve being used as a reference path of a vehicle; wherein the spline curve determining module comprises: a division unit configured to divide the trajectory point set into at least two fitting intervals, each fitting interval containing at least two trajectory points; a first determining unit configured to determine, in each fitting interval, a first target formula group based on a distance increment between any adjacent trajectory points, a function relationship of non-end point trajectory points in the fitting interval, and a first and second derivative continuous condition, the first target formula group being a formula of a cubic spline curve corresponding to the non-end point trajectory points in the fitting interval; a second determining unit configured to determine, in each fitting interval, a second target formula group based on end points in the fitting interval and a prescribed end point derivative continuous condition, the second target formula group being a formula of a cubic spline curve corresponding to the end points in the fitting interval, the end points including a start end and a terminal end; a combination unit configured to obtain a full rank linear equation group of the fitting interval based on the first target formula group and the second target formula group in each fitting interval; a third determining unit configured to obtain fitting coefficients of the fitting interval based on the trajectory point set and the full rank linear equation group; a spline curve unit configured to obtain a group of cubic spline curves corresponding to each fitting interval based on the fitting coefficients.

6. The apparatus of claim 5, wherein, The division unit comprises: an acquisition subunit configured to acquire a preset number of line segments per group; a determination subunit configured to determine a number of fitting intervals based on the preset number of line segments per group and a number of trajectory points in the trajectory point set, any two adjacent fitting intervals having the same trajectory points.

7. An electronic device, comprising: comprising: a memory and a processor; wherein the memory stores a processing program; The processor is configured to load and execute the processing program stored in the memory to implement each step of the trajectory point fitting method according to any one of claims 1-4.

8. A readable storage medium, characterized in that, The computer program is stored on the readable storage medium and is invoked and executed by the processor to implement each step of the trajectory point fitting method according to any one of claims 1-4.

Images