A following error determination method, device, equipment and storage medium

Abstract

The present invention discloses a following error determination method, apparatus, device and storage medium. The method includes: obtaining a target trajectory and motion parameters of a current vehicle, wherein the motion parameters of the current vehicle include: the current vehicle position, the longitudinal speed of the current vehicle, the heading angle of the current vehicle, and the yaw angular velocity of the current vehicle; determining the trajectory shape of the target trajectory according to the coordinates of points on the target trajectory; determining a following error according to the trajectory shape of the target trajectory, the target trajectory and the motion parameters of the current vehicle. Through the technical solution of the present invention, since it does not rely on an acceleration or angular acceleration sensor for analysis, it can reduce the software and hardware deployment cost, and can filter the sensor noise error to ensure the determination of the following error under certain noise conditions, providing an important reference basis for the design of an in-vehicle embedded system controller.

Description

Technical Field

[0001] Embodiments of the present invention relate to the technical field of vehicles, and in particular, to a method, device, equipment, and storage medium for determining following error. Background Art

[0002] The following problem of autonomous vehicles is a core problem of autonomous driving, and the performance of the controller, that is, the control accuracy of the desired following trajectory, is partly determined by the following error. When the following error accuracy cannot be guaranteed, the performance of the controller cannot be guaranteed. The following error accuracy is usually jointly determined by the desired trajectory shape, sensor signal accuracy, and solution algorithm.

[0003] Existing technologies often identify all types of trajectories as a single shape, rely on acceleration or angular acceleration sensors for analysis, and introduce relevant sensor noise errors for following control. Since sensor noise errors are not considered and filtered in actual operations, it may lead to an unreasonable increase in computing power requirements, an increase in software and hardware deployment costs, and a reduction in computing accuracy. Summary of the Invention

[0004] Embodiments of the present invention provide a method, device, equipment, and storage medium for determining following error to solve the problem that in actual operations, sensor noise errors are not considered and filtered, which may lead to an unreasonable increase in computing power requirements, an increase in software and hardware deployment costs, and a reduction in computing accuracy. By identifying the shape of the target trajectory and selecting the corresponding solution algorithm, the computing power requirements can be reduced, and it does not rely on acceleration or angular acceleration sensors for analysis, which can reduce software and hardware deployment costs and provide an important reference basis for the design of in-vehicle embedded system controllers.

[0005] According to one aspect of the present invention, a method for determining following error is provided, including:

[0006] Obtain a target trajectory and the motion parameters of the current vehicle, where the motion parameters of the current vehicle include: the current vehicle position, the longitudinal speed of the current vehicle, the heading angle of the current vehicle, and the yaw angular velocity of the current vehicle;

[0007] Determine the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory;

[0008] Determine the following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle.

[0009] According to another aspect of the present invention, a device for determining following error is provided, and the device for determining following error includes:

[0010] An acquisition module, configured to acquire a target trajectory and motion parameters of a current vehicle, where the motion parameters of the current vehicle include: the position of the current vehicle, the longitudinal speed of the current vehicle, the heading angle of the current vehicle, and the yaw angular velocity of the current vehicle;

[0011] A first determination module, configured to determine the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory;

[0012] A second determination module, configured to determine a following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle.

[0013] According to another aspect of the present invention, there is provided an electronic device, where the electronic device includes:

[0014] At least one processor; and

[0015] A memory communicatively connected to the at least one processor; where

[0016] The memory stores a computer program executable by the at least one processor, and when the computer program is executed by the at least one processor, the at least one processor is enabled to execute the following error determination method according to any embodiment of the present invention.

[0017] According to another aspect of the present invention, there is provided a computer-readable storage medium storing computer instructions for causing a processor to implement the following error determination method according to any embodiment of the present invention when executed.

[0018] In the embodiments of the present invention, by acquiring a target trajectory and motion parameters of a current vehicle, where the motion parameters of the current vehicle include: the position of the current vehicle, the longitudinal speed of the current vehicle, the heading angle of the current vehicle, and the yaw angular velocity of the current vehicle; determining the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory; and determining a following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle, the problem that in actual operations, sensor noise errors are not considered and filtered, which may lead to an unreasonably increased computing power requirement, an increased cost of software and hardware deployment, and a reduced computing accuracy is solved. Since the analysis does not rely on an acceleration or angular acceleration sensor, the cost of software and hardware deployment can be reduced, and the sensor noise error can be filtered to ensure the determination of the following error under certain noise conditions, providing an important reference basis for the design of an in-vehicle embedded system controller.

[0019] It should be understood that the content described in this part is not intended to identify the key or important features of the embodiments of the present invention, nor is it used to limit the scope of the present invention. Other features of the present invention will become readily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following will briefly introduce the drawings required for the embodiments. It should be understood that the following drawings only show some embodiments of the present invention, and therefore should not be regarded as limiting the scope. For those of ordinary skill in the art, without creative efforts, other related drawings can also be obtained based on these drawings.

[0021] Figure 1 is a flowchart of a following error determination method in an embodiment of the present invention;

[0022] Figure 2 is a diagram showing the calculation of lateral offset in an embodiment of the present invention;

[0023] Figure 3 is a schematic diagram of a straight line corresponding to a target trajectory in an embodiment of the present invention;

[0024] Figure 4 is a schematic diagram of an arc with a constant curvature radius corresponding to a target trajectory in an embodiment of the present invention;

[0025] Figure 5 is a diagram showing the calculation of following error for a straight line trajectory in an embodiment of the present invention;

[0026] Figure 6 is a diagram showing the calculation of following error for an arc with a constant curvature radius in an embodiment of the present invention;

[0027] Figure 7 is a schematic structural diagram of a following error determination device in an embodiment of the present invention;

[0028] Figure 8 is a schematic structural diagram of an electronic device in an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0029] In order to enable those skilled in the art to better understand the solutions of the present invention, the following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only some of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the scope of protection of the present invention.

[0030] It should be noted that the terms "first", "second", etc. in the description, claims and above-mentioned drawings of the present invention are used to distinguish similar objects, and do not necessarily describe a specific order or sequence. It should be understood that the data used in this way can be interchanged under appropriate circumstances, so that the embodiments of the present invention described here can be implemented in an order other than those illustrated or described here. In addition, the terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device comprising a series of steps or units does not necessarily limit to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

[0031] Embodiment 1

[0032] Figure 1 FIG. is a flowchart of a following error determination method provided by an embodiment of the present invention. This embodiment is applicable to the situation of following error determination. This method can be executed by the following error determination device in the embodiment of the present invention, and the device can be implemented in a software and / or hardware manner, such as Figure 1 shown, the method specifically includes the following steps:

[0033] S110, obtain a target trajectory and motion parameters of the current vehicle, where the motion parameters of the current vehicle include: current vehicle position, longitudinal speed of the current vehicle, heading angle of the current vehicle, and yaw angular velocity of the current vehicle.

[0034] Among them, the way to obtain the target trajectory can be: obtain an initial trajectory, select a section of the trajectory from the initial trajectory according to the current vehicle position and the longitudinal speed of the current vehicle, and determine it as the target trajectory. The way to obtain the target trajectory can also be: obtain an initial trajectory; determine the target trajectory according to the current vehicle position, the longitudinal speed of the current vehicle, and the initial trajectory. For example, the target trajectory is the distance traveled forward in 1 second. Determine the point on the initial trajectory closest to the current vehicle according to the current vehicle position, and determine the point closest to the current vehicle as the starting point of the target trajectory. Determine the trajectory traveled by the current vehicle within 1 second according to the longitudinal speed of the current vehicle, and determine the target trajectory according to the starting point of the target trajectory and the trajectory traveled by the current vehicle within 1 second. It should be noted that the target trajectory is a sufficiently small section of the trajectory selected from the initial trajectory, and the shape of the target trajectory is a straight line or an arc with a fixed radius of curvature.

[0035] S120, determine the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory.

[0036] Among them, the trajectory shape of the target trajectory can be a straight line or an arc with a fixed radius of curvature.

[0037] Specifically, the method for determining the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory may be: obtaining the lateral offset of the points on the target trajectory. If the maximum lateral offset corresponding to the target trajectory is greater than the lateral offset threshold, it is determined that the target trajectory is a circular arc with a constant curvature radius; if the maximum lateral offset corresponding to the target trajectory is less than or equal to the lateral offset threshold, it is determined that the target trajectory is a straight line.

[0038] S130. Determine the following error according to the trajectory shape of the target trajectory, the target trajectory and the motion parameters of the current vehicle.

[0039] Wherein, the following error includes at least one of a lateral following error, a heading angle following error, and a yaw rate following error.

[0040] Specifically, the method for determining the following error based on the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle may be as follows: If the target trajectory is a straight line, the lateral following error is determined based on the current vehicle position, the slope of the target trajectory, and the intercept of the target trajectory. The method for determining the following error based on the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle may also be as follows: If the target trajectory is a straight line, the heading angle following error is determined based on the heading angle of the current vehicle, the coordinates of the first target point on the target trajectory, and the coordinates of the second target point on the target trajectory, where the first target point is the point closest to the current vehicle, and the second target point is the point with the second closest distance to the current vehicle. The method for determining the following error based on the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle may also be as follows: If the target trajectory is a straight line, the yaw rate of the current vehicle is determined as the yaw rate following error. The method for determining the following error based on the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle may also be as follows: If the target trajectory is a circular arc with a constant radius of curvature, the lateral following error is determined based on the center of the target trajectory, the radius of the target trajectory, and the current vehicle position. The method for determining the following error based on the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle may also be as follows: If the target trajectory is a circular arc with a constant radius of curvature, the heading angle following error is determined based on the heading angle of the current vehicle, the coordinates of the first target point on the target trajectory, and the coordinates of the second target point on the target trajectory, where the first target point is the point closest to the current vehicle, and the second target point is the point with the second closest distance to the current vehicle. The method for determining the following error based on the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle may also be as follows: If the target trajectory is a circular arc with a constant radius of curvature, the yaw rate following error is determined based on the longitudinal speed of the current vehicle, the radius of the target trajectory, and the yaw rate of the current vehicle. The embodiments of the present invention do not limit this.

[0041] Optionally, obtaining the target trajectory includes:

[0042] Obtaining an initial trajectory;

[0043] Determining the target trajectory based on the current vehicle position, the longitudinal speed of the current vehicle, and the initial trajectory.

[0044] Wherein, the initial trajectory is a preset following trajectory, and the embodiments of the present invention do not limit the method for obtaining the initial trajectory.

[0045] Specifically, the method for determining the target trajectory based on the current vehicle position, the longitudinal speed of the current vehicle, and the initial trajectory may be as follows: Obtain the initial trajectory, and select a section of the trajectory from the initial trajectory according to the current vehicle position and the longitudinal speed of the current vehicle, and determine it as the target trajectory.

[0046] Optionally, determining the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory includes:

[0047] If the maximum lateral offset corresponding to the target trajectory is greater than the lateral offset threshold, then determine that the target trajectory is a circular arc with a constant curvature radius;

[0048] If the maximum lateral offset corresponding to the target trajectory is less than or equal to the lateral offset threshold, then determine that the target trajectory is a straight line.

[0049] Specifically, if the maximum lateral offset corresponding to the target trajectory is greater than the lateral offset threshold, the method for determining that the target trajectory is a circular arc with a constant curvature radius may be: Obtain the lateral offset of each point on the target trajectory, compare the lateral offset of each point to obtain the maximum lateral offset, and if the maximum lateral offset is greater than the lateral offset threshold, then determine that the target trajectory is a circular arc with a constant curvature radius.

[0050] Specifically, if the maximum lateral offset corresponding to the target trajectory is less than or equal to the lateral offset threshold, the method for determining that the target trajectory is a straight line may be: Obtain the lateral offset of each point on the target trajectory, compare the lateral offset of each point to obtain the maximum lateral offset, and if the maximum lateral offset is less than or equal to the lateral offset threshold, then determine that the target trajectory is a straight line.

[0051] Optionally, determining the following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle includes:

[0052] If the target trajectory is a straight line, then determine the lateral following error according to the current vehicle position, the slope of the target trajectory, and the intercept of the target trajectory;

[0053] If the target trajectory is a straight line, then determine the heading angle following error according to the heading angle of the current vehicle, the coordinates of the first target point on the target trajectory, and the coordinates of the second target point on the target trajectory, where the first target point is the point closest to the current vehicle, and the second target point is the point with the second closest distance to the current vehicle;

[0054] If the target trajectory is a straight line, then determine the yaw rate of the current vehicle as the yaw rate following error.

[0055] Specifically, if the target trajectory is a straight line, the method for determining the lateral following error based on the current vehicle position, the slope of the target trajectory, and the intercept of the target trajectory can be as follows: The straight line equation corresponding to the target trajectory is y = mx + c, and the lateral following error is calculated based on the following formula:

[0056]

[0057] where e lat is the lateral following error, m is the slope corresponding to the target trajectory, c is the intercept corresponding to the target trajectory, and (X v , Y v ) is the current vehicle position coordinates.

[0058] Specifically, if the target trajectory is a straight line, the method for determining the heading angle following error based on the current vehicle's heading angle, the coordinates of the first target point on the target trajectory, and the coordinates of the second target point on the target trajectory can be as follows:

[0059] The heading angle following error is calculated based on the following formula:

[0060] θ e = θ - θ R

[0061] where θ is the vehicle's current heading angle, θ e is the heading angle following error, θ R is the desired heading angle, which can be obtained through the vehicle's nearest point (X m , Y m ) and the second nearest point (X k , Y k ) to the target trajectory. For example, the desired heading angle can be calculated based on the following formula:

[0062]

[0063] Specifically, if the target trajectory is a straight line, the method for determining the yaw rate following error by using the current vehicle's yaw rate can be as follows: Since the radius of curvature of a straight line can be considered infinite, the desired yaw rate is 0, and the yaw rate following error where is the current vehicle's yaw rate.

[0064] Optionally, determining the following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle includes:

[0065] If the target trajectory is an arc with a constant radius of curvature, determine the lateral following error according to the center of the target trajectory, the radius of the target trajectory, and the current vehicle position;

[0066] If the target trajectory is a circular arc with a constant radius of curvature, the heading angle following error is determined according to the heading angle of the current vehicle, the coordinates of the first target point on the target trajectory, and the coordinates of the second target point on the target trajectory, where the first target point is the point closest to the current vehicle, and the second target point is the point with the second closest distance to the current vehicle;

[0067] If the target trajectory is a circular arc with a constant radius of curvature, the yaw rate following error is determined according to the longitudinal speed of the current vehicle, the radius of the target trajectory, and the yaw rate of the current vehicle.

[0068] Specifically, if the target trajectory is a circular arc with a constant radius of curvature, the method for determining the lateral following error according to the center of the target trajectory, the radius of the target trajectory, and the position of the current vehicle may be as follows:

[0069] The lateral trajectory error e is calculated based on the following formula lat :

[0070]

[0071] where (X c , Y c ) are the coordinates of the center of the target trajectory, (X v , Y v ) are the coordinates of the current vehicle position, and R is the radius of the target trajectory.

[0072] Specifically, if the target trajectory is a circular arc with a constant radius of curvature, the method for determining the heading angle following error according to the heading angle of the current vehicle, the coordinates of the first target point on the target trajectory, and the coordinates of the second target point on the target trajectory may be as follows: The heading angle following error is calculated based on the following formula:

[0073] θ e = θ - θ R

[0074] where θ is the heading angle of the vehicle at this time, θ e is the heading angle following error, and θ R is the desired heading angle, which can be obtained through the closest point (X m , Y m ) and the second closest point (X k , Y k ) of the vehicle to the target trajectory. For example, the desired heading angle can be calculated based on the following formula:

[0075]

[0076] It should be noted that the calculation methods of the heading angle following error are the same when the target trajectory is an arc with a constant radius of curvature and when the target trajectory is a straight line.

[0077] Specifically, if the target trajectory is an arc with a constant radius of curvature, the method for determining the yaw rate following error based on the longitudinal speed of the current vehicle, the radius of the target trajectory, and the yaw rate of the current vehicle can be as follows:

[0078] Calculate the yaw rate following error based on the following formula:

[0079]

[0080] where, is the yaw rate of the current vehicle, and v x is the longitudinal speed of the current vehicle.

[0081] Optionally, it further includes:

[0082] Calculate the lateral offset of each point on the target trajectory based on the following formula:

[0083]

[0084] where, (X i , Y i ) is the coordinate of the i-th point on the target trajectory, (X 1 , Y 1 ) is the coordinate of the starting point of the target trajectory, (X N , Y N ) is the coordinate of the ending point of the target trajectory, and dis is the lateral offset of the i-th point on the target trajectory.

[0085] In a specific example, as Figure 2 shown, Figure 2 the curve in it is the target trajectory, (X 1 , Y 1 ) and (X N , Y N ) respectively represent the first point and the last point of the target trajectory. Based on the connection line of the first point and the last point, a straight line is obtained. The angle between this straight line and the positive semi-axis of the X-axis is ψ. The distance from the i-th point (X i , Y i ) on the target trajectory to the straight line is defined as the lateral offset of the i-th point on the target trajectory to the straight line. From the 2nd point to the i-1-th point, the corresponding lateral offset amounts dis 2 , …, dis i-1 can be obtained, find the maximum value dis max among them, and compare it with the threshold dis threshold If dismax >dis threshold Then the target trajectory is an arc with a constant radius of curvature; otherwise, the target trajectory is a straight line.

[0086] Optionally, it further includes:

[0087] Calculating the center of the target trajectory based on the following formula:

[0088]

[0089]

[0090] Where (X c , Y c ) are the coordinates of the center of the target trajectory, N is the number of points on the target trajectory, and (X i , Y i ) are the coordinates of the i-th point on the target trajectory;

[0091] Calculating the radius of the target trajectory based on the following formula:

[0092]

[0093] In a specific example, if the target trajectory is a straight line, the least squares method is used for trajectory fitting; if the target trajectory is an arc curve, trajectory fitting is performed through convex optimization. The following explains the two fitting methods separately:

[0094] Least squares method: As Figure 3 shown, (X 1 , Y 1 ) and (X N , Y N ) represent the first point and the last point of the target trajectory respectively, and y = mx + c represents the straight line with the minimum sum of the distances from all points on the trajectory to this line, which is the fitted straight-line trajectory. The equations from the first point to the N-th point can be expressed as:

[0095]

[0096] The above matrix can be expressed as:

[0097] T = Ax

[0098] To find x, perform the following transformation on it:

[0099] A T T = A T Ax

[0100] (AT A) -1 A T T = x

[0101] The corresponding straight-line slope m and intercept c can be obtained. Since it is a straight line, the radius of curvature is infinite.

[0102] Convex optimization fitting: When the shape of the target trajectory is an arc curve, the convex optimization method is used for fitting. As Figure 4 shown, (X 1 , Y 1 ), …, (X N , Y N ) are the points on the target trajectory. The "preview section" in the figure is the target trajectory. To find an arc trajectory with the minimum sum of distances from all points on the target trajectory to the fitted arc trajectory, the convex optimization fitting method is introduced. That is, using this method, the corresponding radius of curvature R and the center (X c , Y c ) are calculated. The specific algorithm is as follows:

[0103] First, define the difference between the square of the distance from the i-th point on the trajectory to the fitted center (X c , Y c ) and the square of the fitted radius R as e i , that is:

[0104] e i (x) = (X i - X c ) 2 + (Y i - Y c ) 2 - R 2

[0105] Define e i > 0, that is, the calculated value of R is on the small and conservative side (this makes the centripetal acceleration calculated larger, and thus ensures that it will not exceed the centripetal acceleration threshold at the same speed).

[0106] Define the square of the error of the i-th point, that is: The target trajectory fitting loss function can be defined as:

[0107]

[0108] To find the extreme value of the trajectory fitting loss function, the gradient method is introduced, that is:

[0109]

[0110]

[0111]

[0112] Based on the above formula, the center (X c , Y c ) and radius R of the trajectory fitting circle can be obtained.

[0113] To prove that the optimization method will find the minimum value of the trajectory fitting function E(x), it is necessary to prove that E(x) is a convex function. To prove this property, the following two theorems are introduced:

[0114] 1. If h(x) = f(x)g(x) (abbreviated as h = fg), and it satisfies:

[0115] Then h(x) is a convex function.

[0116] 2. Let H(x) = h 1 (x) + … + h n (x), H(x) is a convex function if h 1 (x) … h n (x) are all convex functions.

[0117] The following proves the above two theorems:

[0118] Proof of Theorem 1:

[0119] h = fg

[0120] Then

[0121] h′ = f′g + fg′

[0122] h″ = f″g + fg″ + 2f′g′

[0123] Since f and g are both convex functions, then f″ ≥ 0 and g″ ≥ 0. Also, since: f ≥ 0 and g ≥ 0, so f″g ≥ 0 and g″f ≥ 0. And we know that f′ and g′ have the same sign; then f′g′ ≥ 0. Therefore, h″ = f″g + fg″ + 2f′g′ ≥ 0. It can be proved that h(x) is a convex function

[0124] Proof of Theorem 2:

[0125] Since h 1 (x) … h n (x) are all convex functions, then: h 1 ″(x) … ph″ n (x) are all greater than 0. And we know that H″(x) = h″ 1 (x) + … + h″ n (x), then H″(x) ≥ 0. It can be proved that H(x) is a convex function

[0126] Proof that E(x) is a convex function:

[0127] Step 1: First, prove that \(e(x)\) is a convex function. i (x) is a convex function.

[0128] Given

[0129] e i (x) = (X i - X c ) 2 + (Y i - Y c ) 2 - R 2

[0130] x = (X c , Y c )

[0131]

[0132] Obviously, there is So, \(e(x)\) is a convex function. i (x) is a convex function.

[0133] Step 2: Prove that is a convex function. Here, Theorem 1 is cited:

[0134]

[0135] Among them, f and g are equal and both are convex functions. From the definition of \(e(x)\), it can be known that \(e(x) \geq 0\), and obviously, the signs of f' and g' are the same. Therefore, it can be proved that \(E(x)\) is a convex function. i (x) is defined as \(e(x) \geq 0\), and obviously, the signs of f' and g' are the same. So, it can be proved that \(E(x)\) is a convex function. i (x) \geq 0, and obviously, the signs of f' and g' are the same. So, it can be proved that \(E(x)\) is a convex function. i (x) is a convex function.

[0136] Step 3: Prove that \(E(x)\) is a convex function. Here, Theorem 2 is cited:

[0137]

[0138] Because \(E(x)\) is a convex function, then \(E(x)\) is a convex function. i (x) is a convex function, then \(E(x)\) is a convex function.

[0139] Therefore, it can be concluded that \(E(x)\) is a convex function. So, the extreme value obtained by the gradient method is the minimum value. Next, the gradient method is used to solve the minimum value:

[0140]

[0141]

[0142] Assume that R is greater than 0.

[0143]

[0144] It can be solved that

[0145]

[0146]

[0147]

[0148]

[0149] Because

[0150]

[0151] So

[0152]

[0153]

[0154]

[0155]

[0156] Similar to the above, the derivation formula is omitted:

[0157]

[0158] Rearranging the above equation, (X c , Y c ) can be obtained from the following matrix:

[0159]

[0160]

[0161]

[0162] Among them,

[0163]

[0164]

[0165]

[0166]

[0167]

[0168] When the center position (X c , Y c ) is determined by the above equation, the radius R is determined based on the following formula:

[0169]

[0170] Since then, the radius of curvature R of the trajectory has been solved both in the straight line and curve states.

[0171] As Figure 5 shown, the distance from the current vehicle position (X v , Y v ) to the fitted straight line is the lateral trajectory error. The equation of the straight line is y = mx + c, then the lateral following error e lat is:

[0172]

[0173] The heading angle following error θ e of the straight line is:

[0174] θ e = θ - θ R

[0175] where θ is the vehicle's heading angle at this time, and θ R is the desired heading angle. It is obtained through the closest point (X 1 , Y 1 ) and the second-closest point (X 2 , Y 2 ) of the vehicle to the target trajectory. Among them,

[0176]

[0177] Because the radius of curvature of the straight line can be considered infinite, its desired yaw rate is 0, and the yaw rate following error where is the vehicle's angular velocity at this time.

[0178] When the trajectory is a circular arc, as Figure 6 shown, the lateral following error e lat can be expressed as:

[0179]

[0180] The heading angle following error θ e of the straight line is:

[0181] θ e = θ - θ R

[0182] where θ is the vehicle's heading angle at this time, and θ R is the desired heading angle, which can be obtained through the closest point (X 1 , Y 1 ) and the second-closest point (X 2 , Y 2 ) of the vehicle to the target trajectory.

[0183]

[0184] Since the curvature radius of the circular arc can be obtained by the 200 trajectory fitting and curvature radius calculation module, the yaw rate following error is:

[0185]

[0186] Wherein, is the angular velocity of the vehicle at this time, and v x is the longitudinal vehicle speed at this time. Thus, when the target trajectory is a straight line and a circular arc, the circular arc curvature radius, the lateral following error, the heading angle following error, and the yaw rate following error have all been obtained. The above parameters can be further transmitted to the control module of the next step, providing an important basis for the design of the control system.

[0187] The technical solution of this embodiment, by obtaining the target trajectory and the motion parameters of the current vehicle, wherein the motion parameters of the current vehicle include: the current vehicle position, the longitudinal speed of the current vehicle, the heading angle of the current vehicle, and the yaw rate of the current vehicle; determining the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory; determining the following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle, to solve the problem that in actual operations, sensor noise errors are not considered and filtered, which may lead to an unreasonable increase in the computing power requirement, an increase in the software and hardware deployment cost, and a reduction in the calculation accuracy. Since it does not rely on an acceleration or angular acceleration sensor for analysis, it can reduce the software and hardware deployment cost, and can filter the sensor noise error to ensure that the following error is determined under certain noise conditions, providing an important reference basis for the design of the in-vehicle embedded system controller.

[0188] Embodiment 2

[0189] Figure 7 This is a schematic structural diagram of a following error determination device provided by an embodiment of the present invention. This embodiment is applicable to the situation of following error determination. The device can be implemented in software and / or hardware, and the device can be integrated in any device that provides the function of following error determination, such as Figure 7 As shown, the following error determination device specifically includes: an acquisition module 210, a first determination module 220, and a second determination module 230.

[0190] Among them, the acquisition module is used to acquire the target trajectory and the motion parameters of the current vehicle, wherein the motion parameters of the current vehicle include: the current vehicle position, the longitudinal speed of the current vehicle, the heading angle of the current vehicle, and the yaw rate of the current vehicle;

[0191] A first determination module, configured to determine the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory;

[0192] A second determination module, configured to determine a following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle.

[0193] Optionally, the acquisition module is specifically configured to:

[0194] Acquire an initial trajectory;

[0195] Determine a target trajectory according to the current vehicle position, the longitudinal speed of the current vehicle, and the initial trajectory.

[0196] Optionally, the first determination module is specifically configured to:

[0197] If the maximum lateral offset corresponding to the target trajectory is greater than the lateral offset threshold, determine that the target trajectory is an arc with a constant radius of curvature;

[0198] If the maximum lateral offset corresponding to the target trajectory is less than or equal to the lateral offset threshold, determine that the target trajectory is a straight line.

[0199] Optionally, the second determination module is specifically configured to:

[0200] If the target trajectory is a straight line, determine a lateral following error according to the current vehicle position, the slope of the target trajectory, and the intercept of the target trajectory;

[0201] If the target trajectory is a straight line, determine a heading angle following error according to the heading angle of the current vehicle, the coordinates of the first target point on the target trajectory, and the coordinates of the second target point on the target trajectory, where the first target point is the point closest to the current vehicle, and the second target point is the point next closest to the current vehicle;

[0202] If the target trajectory is a straight line, determine the yaw rate of the current vehicle as the yaw rate following error.

[0203] Optionally, the second determination module is specifically configured to:

[0204] If the target trajectory is an arc with a constant radius of curvature, determine a lateral following error according to the center of the target trajectory, the radius of the target trajectory, and the current vehicle position;

[0205] If the target trajectory is a circular arc with a constant radius of curvature, a heading angle following error is determined based on the heading angle of the current vehicle, the coordinates of a first target point on the target trajectory, and the coordinates of a second target point on the target trajectory, where the first target point is the point closest to the current vehicle, and the second target point is the point with the second-closest distance to the current vehicle;

[0206] If the target trajectory is a circular arc with a constant radius of curvature, a yaw rate following error is determined based on the longitudinal speed of the current vehicle, the radius of the target trajectory, and the yaw rate of the current vehicle.

[0207] Optionally, the first determination module is specifically configured to:

[0208] Calculate the lateral offset of each point on the target trajectory based on the following formula:

[0209]

[0210] where (X i , Y i ) are the coordinates of the i-th point on the target trajectory, (X 1 , Y 1 ) are the coordinates of the starting point of the target trajectory, (X N , Y N ) are the coordinates of the ending point of the target trajectory, and dis is the lateral offset of the i-th point on the target trajectory.

[0211] Optionally, the second determination module is specifically configured to:

[0212] Calculate the center of the target trajectory based on the following formula:

[0213]

[0214]

[0215] where (X c , Y c ) are the coordinates of the center of the target trajectory, N is the number of points on the target trajectory, and (X i , Y i ) are the coordinates of the i-th point on the target trajectory;

[0216] Calculate the radius of the target trajectory based on the following formula:

[0217]

[0218] The above product can execute the method provided by any embodiment of the present invention, and has the corresponding functional modules and beneficial effects for executing the method.

[0219] In the technical solution of this embodiment, by obtaining the target trajectory and the motion parameters of the current vehicle, where the motion parameters of the current vehicle include: the current vehicle position, the longitudinal speed of the current vehicle, the heading angle of the current vehicle, and the yaw angular velocity of the current vehicle; determining the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory; and determining the following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle, so as to solve the problem that in actual operations, sensor noise errors are not considered and filtered, which may lead to an unreasonable increase in computing power requirements, an increase in software and hardware deployment costs, and a reduction in computing accuracy. Since it does not rely on acceleration or angular acceleration sensors for analysis, it can reduce software and hardware deployment costs, and can filter sensor noise errors to ensure that the following error is determined under certain noise conditions, providing an important reference basis for the design of the in-vehicle embedded system controller.

[0220] Embodiment III

[0221] Figure 8 FIG. shows a schematic structural diagram of an electronic device 10 that can be used to implement the embodiments of the present invention. The electronic device is intended to represent various forms of digital computers, such as, laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as, personal digital processors, cellular phones, smart phones, wearable devices (such as helmets, glasses, watches, etc.) and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the present invention described and / or claimed herein.

[0222] As Figure 8 shown, the electronic device 10 includes at least one processor 11, and a memory communicatively connected to at least one processor 11, such as a read-only memory (ROM) 12, a random access memory (RAM) 13, etc. Among them, the memory stores a computer program executable by at least one processor. The processor 11 can execute various appropriate actions and processes according to the computer program stored in the read-only memory (ROM) 12 or the computer program loaded from the storage unit 18 into the random access memory (RAM) 13. In the RAM 13, various programs and data required for the operation of the electronic device 10 can also be stored. The processor 11, the ROM 12, and the RAM 13 are connected to each other through a bus 14. The input / output (I / O) interface 15 is also connected to the bus 14.

[0223] Multiple components in the electronic device 10 are connected to the I / O interface 15, including: an input unit 16, such as a keyboard, a mouse, etc.; an output unit 17, such as various types of displays, speakers, etc.; a storage unit 18, such as a disk, an optical disc, etc.; and a communication unit 19, such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 19 allows the electronic device 10 to exchange information / data with other devices through a computer network such as the Internet and / or various telecommunication networks.

[0224] The processor 11 can be various general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of the processor 11 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. The processor 11 executes the various methods and processes described above, such as the following error determination method.

[0225] In some embodiments, the following error determination method can be implemented as a computer program, which is tangibly contained in a computer-readable storage medium, such as the storage unit 18. In some embodiments, part or all of the computer program can be loaded and / or installed onto the electronic device 10 via the ROM 12 and / or the communication unit 19. When the computer program is loaded into the RAM 13 and executed by the processor 11, one or more steps of the following error determination method described above can be executed. Alternatively, in other embodiments, the processor 11 can be configured to execute the following error determination method in any other suitable manner (e.g., by means of firmware):

[0226] Obtain a target trajectory and the motion parameters of the current vehicle, where the motion parameters of the current vehicle include: the current vehicle position, the longitudinal speed of the current vehicle, the heading angle of the current vehicle, and the yaw angular velocity of the current vehicle;

[0227] Determine the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory;

[0228] Determine the following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle.

[0229] The various embodiments of the systems and techniques described above in this specification can be implemented in digital electronic circuitry, integrated circuit systems, field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems-on-chip (SOCs), complex programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments can include: being implemented in one or more computer programs that are executable and / or interpretable on a programmable system including at least one programmable processor, which can be a special-purpose or general-purpose programmable processor that receives data and instructions from, and transmits data and instructions to, a storage system, at least one input device, and at least one output device.

[0230] The computer programs for implementing the methods of the present invention can be written in any combination of one or more programming languages. These computer programs can be provided to a processor of a general purpose computer, special purpose computer, or other programmable data processing apparatus, such that the computer programs, when executed by the processor, cause the functions / operations specified in the flowchart and / or block diagram to be implemented. The computer programs can be executed entirely on the machine, partly on the machine, as a stand-alone software package partly on the machine and partly on a remote machine, or entirely on the remote machine or server.

[0231] In the context of the present invention, a computer-readable storage medium can be a tangible medium that can contain or store a computer program for use by or in connection with an instruction execution system, apparatus, or device. The computer-readable storage medium can include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. Alternatively, the computer-readable storage medium can be a machine-readable signal medium. More specific examples of the machine-readable storage medium would include an electrical connection based on one or more wires, a portable computer diskette, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or Flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0232] To provide interaction with a user, the systems and techniques described herein can be implemented on an electronic device having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and a pointing device (e.g., a mouse or a trackball) through which the user can provide input to the electronic device. Other kinds of devices can also be used to provide interaction with the user; for example, the feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including acoustic input, voice input, or tactile input).

[0233] The systems and techniques described herein can be implemented in a computing system including backend components (e.g., as a data server), or a computing system including middleware components (e.g., an application server), or a computing system including frontend components (e.g., a user computer having a graphical user interface or a web browser through which the user can interact with an implementation of the systems and techniques described herein), or a computing system including any combination of such backend components, middleware components, or frontend components. The components of the system can be interconnected to each other by digital data communication in any form or medium (e.g., a communication network). Examples of communication networks include: local area network (LAN), wide area network (WAN), blockchain network, and the Internet.

[0234] The computing system can include a client and a server. The client and the server are generally far from each other and usually interact through a communication network. The client-server relationship is created by computer programs running on the respective computers and having a client-server relationship with each other. The server can be a cloud server, also known as a cloud computing server or a cloud host, which is a host product in the cloud computing service system and solves the defects of difficult management and weak business scalability existing in traditional physical hosts and VPS services.

[0235] It should be understood that the various forms of processes shown above can be used, with steps reordered, added, or deleted. For example, the steps recited in the present invention can be executed in parallel, sequentially, or in a different order, as long as the desired results of the technical solution of the present invention can be achieved, and no limitation is imposed herein.

[0236] The above specific embodiments do not constitute a limitation on the protection scope of the present invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A method for determining following error, characterized in that, it includes: Obtain the target trajectory and the motion parameters of the current vehicle, wherein the motion parameters of the current vehicle include: the current vehicle position, the longitudinal speed of the current vehicle, the heading angle of the current vehicle, and the yaw angular velocity of the current vehicle; Determine the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory; Determine the following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle; The determining the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory includes: If the maximum lateral offset corresponding to the target trajectory is less than or equal to the lateral offset threshold, determine that the target trajectory is a straight line; The determining the following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle includes: If the target trajectory is a straight line, determine the lateral following error according to the current vehicle position, the slope of the target trajectory, and the intercept of the target trajectory; If the target trajectory is a straight line, determine the heading angle following error according to the heading angle of the current vehicle, the coordinates of the first target point on the target trajectory, and the coordinates of the second target point on the target trajectory, wherein the first target point is the point closest to the current vehicle, and the second target point is the point next closest to the current vehicle; If the target trajectory is a straight line, determine the yaw angular velocity of the current vehicle as the yaw angular velocity following error.

2. The method according to claim 1, characterized in that, Obtaining the target trajectory includes: Obtain the initial trajectory; Determine the target trajectory according to the current vehicle position, the longitudinal speed of the current vehicle, and the initial trajectory.

3. The method according to claim 1, characterized in that, Determining the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory further includes: If the maximum lateral offset corresponding to the target trajectory is greater than the lateral offset threshold, determine that the target trajectory is a circular arc with a constant radius of curvature.

4. The method according to claim 3, characterized in that, Determining the following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle includes: If the target trajectory is a circular arc with a constant radius of curvature, determine the lateral following error according to the center of the target trajectory, the radius of the target trajectory, and the current vehicle position; If the target trajectory is a circular arc with a constant radius of curvature, determine the heading angle following error according to the heading angle of the current vehicle, the coordinates of the first target point on the target trajectory, and the coordinates of the second target point on the target trajectory, wherein the first target point is the point closest to the current vehicle, and the second target point is the point next closest to the current vehicle; If the target trajectory is a circular arc with a constant radius of curvature, determine the yaw angular velocity following error according to the longitudinal speed of the current vehicle, the radius of the target trajectory, and the yaw angular velocity of the current vehicle.

5. The method according to claim 3, characterized in that, It further includes: The lateral offset of each point on the target trajectory is calculated based on the following formula: where (X i , Y i ) are the coordinates of the i-th point on the target trajectory, (X 1 , Y 1 ) are the coordinates of the starting point of the target trajectory, (X N , Y N ) are the coordinates of the ending point of the target trajectory, and dis is the lateral offset of the i-th point on the target trajectory.

6. The method according to claim 4, wherein, it further includes: calculating the center of the target trajectory based on the following formula: Among them, (X c , Y c ) is the center coordinate of the target trajectory, N is the number of points on the target trajectory, and (X i , Y i ) is the coordinate of the i-th point on the target trajectory; calculating the radius of the target trajectory based on the following formula:

7. A following error determination device, wherein, it includes: an acquisition module, configured to acquire the target trajectory and the motion parameters of the current vehicle, wherein the motion parameters of the current vehicle include: the current vehicle position, the longitudinal speed of the current vehicle, the heading angle of the current vehicle, and the yaw angular velocity of the current vehicle; a first determination module, configured to determine the trajectory shape of the target trajectory according to the coordinates of the points on the target trajectory; a second determination module, configured to determine the following error according to the trajectory shape of the target trajectory, the target trajectory, and the motion parameters of the current vehicle; the first determination module is further configured to determine that the target trajectory is a straight line if the maximum lateral offset corresponding to the target trajectory is less than or equal to the lateral offset threshold; the second determination module is further configured to determine the lateral following error according to the current vehicle position, the slope of the target trajectory, and the intercept of the target trajectory if the target trajectory is a straight line; if the target trajectory is a straight line, determine the heading angle following error according to the heading angle of the current vehicle, the coordinates of the first target point on the target trajectory, and the coordinates of the second target point on the target trajectory, where the first target point is the point closest to the current vehicle, and the second target point is the point with the second closest distance to the current vehicle; if the target trajectory is a straight line, determine the yaw angular velocity of the current vehicle as the yaw angular velocity following error.

8. An electronic device, wherein, the electronic device includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein, the memory stores a computer program executable by the at least one processor, and the computer program is executed by the at least one processor so that the at least one processor can execute the following error determination method according to any one of claims 1-6.

9. A computer-readable storage medium, wherein, the computer-readable storage medium stores computer instructions, and the computer instructions are used to enable the processor to execute the following error determination method according to any one of claims 1-6 when executed.

Images