Path tracking method and device and storage medium

Abstract

The invention relates to a path tracking method and device and a storage medium. The method comprises the following steps: determining a preview distance; determining at least two reference points in path points of the planned path according to the preview distance; responding to the situation that the transverse error and the course error of the at least two reference points do not meet a preset condition, and generating an Euler helix trajectory table; a target column with the minimum curvature difference value with the current curvature is determined in the Euler helix trajectory table; establishing an Euler helix starting point coordinate system by taking each track point in the target column as a starting point; determining the sum of tracking errors of the at least two reference points in an Euler helix starting point coordinate system; when the sum of the tracking errors of the at least two reference points is minimum, determining a target curvature change rate; and determining a front wheel steering angle according to the target curvature change rate, wherein the front wheel steering angle is used for controlling vehicle steering for path tracking. According to the scheme, the path tracking precision can be improved, and the vehicle is controlled to smoothly follow or approach the planned path.

Description

Technical Field

[0001] The present application relates to the field of vehicle control technology, and in particular to a path tracking method, device and storage medium. Background Art

[0002] The pure tracking algorithm is a common path tracking algorithm. The pure tracking algorithm controls the vehicle to follow the planned path by tracking the path points on the vehicle's planned path. When using the pure tracking algorithm for path tracking, there are often problems such as large heading error and poor adaptability to trajectories with high curvature change rates. Summary of the invention

[0003] In view of this, the present application provides a path tracking method, device and storage medium, which can improve the path tracking accuracy and control the vehicle to smoothly follow or approach the planned path.

[0004] In a first aspect, an embodiment of the present invention provides a path tracking method, the method comprising:

[0005] Determine the preview distance based on the vehicle's current position;

[0006] Determining at least two reference points among the path points of the planned path according to the preview distance;

[0007] In response to the lateral errors and heading errors of the at least two reference points not satisfying a preset condition, generating an Euler spiral trajectory table, wherein trajectory points in different rows of the Euler spiral trajectory table correspond to different curvature change rates, and trajectory points in different columns correspond to different curvatures;

[0008] Calculate the current curvature based on the vehicle's steering wheel angle;

[0009] Determine in the Euler spiral trajectory table the target column having the smallest curvature difference from the current column;

[0010] Taking each trajectory point in the target column as a starting point, a starting point coordinate system of the Euler spiral is established;

[0011] Determining the sum of tracking errors of the at least two reference points in the coordinate system of the starting point of the Euler spiral;

[0012] Determine the target curvature change rate corresponding to the at least two reference points in the Euler spiral trajectory table when the sum of the tracking errors of the at least two reference points is minimized;

[0013] A front wheel steering angle is determined according to the target curvature change rate, and the front wheel steering angle is used to control the steering of the vehicle to perform path tracking.

[0014] In some embodiments, determining the preview distance according to the current position of the vehicle includes:

[0015] Determining two adjacent path points closest to the current position of the vehicle in the planned path;

[0016] Determining a current lateral error and a heading error based on the two adjacent path points;

[0017] The preview distance is determined according to the current lateral error, the heading error and the preview distance limit value.

[0018] In some embodiments, the at least two reference points include:

[0019] A first reference point in the planned path that is farthest from the current position of the vehicle determined by the preview distance; and further comprising:

[0020] At least one reference point located between the first reference point and the current position of the vehicle.

[0021] In some embodiments, generating an Euler spiral trajectory table includes:

[0022] The preset curvature change rate interval is equally divided to obtain m curvature change rate values;

[0023] The curvature interval related to the turning radius of the current vehicle is equally divided to obtain n curvature values;

[0024] Generate a two-dimensional table with m rows and n columns, with the curvature change rate values ​​as rows and the curvature values ​​as columns;

[0025] Determine the trajectory point at each unit position according to the curvature change rate value and the curvature value at each unit position in the two-dimensional table to obtain the Euler spiral trajectory table;

[0026] Among them, the trajectory points located in the same row in the Euler spiral trajectory table represent an Euler spiral trajectory.

[0027] In some embodiments, determining the sum of tracking errors of the at least two reference points in the Euler spiral starting point coordinate system includes:

[0028] Convert the trajectory points in each row of the Euler spiral trajectory table to the coordinate system of the starting point of the Euler spiral in the row where it is located;

[0029] The at least two reference points are subjected to coordinate transformation in the Euler spiral starting point coordinate system of each row of the Euler spiral trajectory table;

[0030] In the Euler spiral starting point coordinate system of each row of the Euler spiral trajectory table, the sum of the tracking errors of the at least two reference points on each row is determined.

[0031] In some embodiments, determining the sum of the tracking errors of the at least two reference points on each row in the Euler spiral starting point coordinate system of each row of the Euler spiral trajectory table includes:

[0032] Among the trajectory points in each row of the Euler spiral trajectory table, determine the trajectory point closest to each reference point in each row;

[0033] Determine the tracking error of each reference point on each row based on the distance of each reference point on each row to the nearest track point;

[0034] The tracking errors of the reference points on each row are added together to obtain the sum of the tracking errors of the at least two reference points on each row.

[0035] In some embodiments, the at least two reference points include a first reference point;

[0036] Among the trajectory points in each row of the Euler spiral trajectory table, determining the closest trajectory point of each reference point on each row; and determining the tracking error of each reference point on each row according to the closest trajectory point of each reference point on each row, including:

[0037] Among the trajectory points in each row of the Euler spiral trajectory table, determine the trajectory point in each row that is closest to the first reference point;

[0038] Determine a first tracking error between the first reference point in each row and the closest track point in the row;

[0039] determining a filter row in which the first tracking error is less than a tolerable threshold;

[0040] Calculate the tracking error of the remaining reference points on each filtered row.

[0041] In some embodiments, determining the front wheel turning angle according to the target curvature change rate includes:

[0042] Determining a feedforward controlled front wheel steering angle according to the target curvature change rate;

[0043] Determine the feedback control front wheel steering angle compensation according to the lateral error and heading error existing in the current position of the vehicle;

[0044] The front wheel steering angle is determined according to the feedforward controlled front wheel steering angle and the feedback controlled front wheel steering angle compensation.

[0045] In some embodiments, determining the feedforward control front wheel steering angle according to the target curvature change rate includes:

[0046] Determine the distance traveled by the vehicle from the last output of the front wheel steering angle for path tracking to the current position of the vehicle;

[0047] Determining a current feedforward curvature according to the vehicle travel distance and the target curvature change rate;

[0048] The feedforward controlled front wheel steering angle is determined according to the current feedforward curvature.

[0049] In a second aspect, an embodiment of the present invention provides a path tracking device, the device comprising:

[0050] A preview distance determination module is used to determine the preview distance according to the current position of the vehicle;

[0051] A reference point determination module, configured to determine at least two reference points among the path points of the planned path according to the preview distance;

[0052] The Euler spiral module is used for generating an Euler spiral trajectory table in response to the lateral error and the heading error of the at least two reference points not satisfying a preset condition, wherein trajectory points in different rows of the Euler spiral trajectory table correspond to different curvature change rates, and trajectory points in different columns correspond to different curvatures; calculating a current curvature according to a steering wheel angle of the vehicle; determining a target column with a minimum difference from the current curvature in the Euler spiral trajectory table; and establishing an Euler spiral starting point coordinate system with each trajectory point in the target column as a starting point;

[0053] A path tracking module, used to determine the sum of the tracking errors of the at least two reference points in the starting coordinate system of the Euler spiral; determine the target curvature change rate corresponding to the at least two reference points in the Euler spiral trajectory table when the sum of the tracking errors of the at least two reference points is minimized;

[0054] The front wheel steering angle output module is used to determine the front wheel steering angle according to the target curvature change rate, and the front wheel steering angle is used to control the vehicle steering to perform path tracking.

[0055] In a third aspect, an embodiment of the present invention provides a path tracking device, comprising: a memory for storing computer program instructions and a processor for executing program instructions, wherein, when the computer program instructions are executed by the processor, the server executes the method described in the first aspect or any one of the first aspects.

[0056] In a fourth aspect, an embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium includes a stored program, wherein when the program is running, the device where the computer-readable storage medium is located is controlled to execute the method described in the first aspect or any one of the first aspects.

[0057] The path tracking method, device and storage medium provided by the embodiments of the present invention have at least the following beneficial effects:

[0058] The embodiment of the present invention improves the path tracking accuracy by introducing no less than two path reference points, and the method introduces an Euler spiral trajectory table. The curvature change rate in the Euler spiral trajectory table is uniform. Steering control is performed through the Euler spiral trajectory table, so that the steering wheel can be rotated evenly, and the vehicle can be controlled to smoothly track or approach the planned path. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 A flow chart of a path tracking method provided by an embodiment of the present invention;

[0060] Figure 2 A flow chart of a path tracking method provided by an embodiment of the present invention;

[0061] Figure 3 A schematic diagram of a planning path provided by an embodiment of the present invention;

[0062] Figure 4 A schematic diagram of the structure of a path tracking device provided by an embodiment of the present invention;

[0063] Figure 5 A schematic diagram of the structure of a path tracking device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0064] In order to better understand the technical solution of the present application, the embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0065] It should be clear that the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present application.

[0066] The terms used in the embodiments of the present application are only for the purpose of describing specific embodiments, and are not intended to limit the present application. The singular forms "a", "said" and "the" used in the embodiments of the present application and the appended claims are also intended to include plural forms, unless the context clearly indicates other meanings.

[0067] It should be understood that the term "and / or" used in this article is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this article generally indicates that the associated objects before and after are in an "or" relationship.

[0068] In the field of automatic parking or unmanned driving, the planned path refers to the expected path that allows the vehicle to move from one location to the destination. The planned path includes a series of discrete path points, which contain location information and the expected heading angle information when the vehicle moves to the point. The path points on the planned path satisfy the vehicle kinematics, that is, when the vehicle reaches a path point and the actual heading angle is consistent with the expected heading angle of the current path point, the vehicle can reach the next path point through a forward or backward movement with steering, and the actual heading angle is consistent with the expected heading angle of the next path point.

[0069] In the related art, a pure tracking algorithm can be used to calculate the actual steering angle of the vehicle to track the path point. However, when a pure tracking algorithm is used for path tracking, there are problems such as large heading error and poor adaptability to trajectories with high curvature change rates.

[0070] To solve the above technical problems, an embodiment of the present invention provides a path tracking method, which improves the path tracking accuracy by introducing no less than two path reference points, and introduces an Euler spiral trajectory table. The curvature change rate in the Euler spiral trajectory table is uniform. Steering control is performed through the Euler spiral trajectory table, so that the steering wheel can be rotated evenly, and the vehicle can be controlled to smoothly track or approach the planned path.

[0071] See also Figure 1 , is a flow chart of a path tracking method provided by an embodiment of the present invention. Figure 1 As shown, the processing steps of the method include:

[0072] 101, determine the preview distance according to the current position of the vehicle.

[0073] 102. Determine at least two reference points among the path points of the planned path according to the preview distance.

[0074] 103, determining the lateral error and the heading error of the at least two reference points.

[0075] 104. In response to the lateral error and the heading error of the at least two reference points not satisfying a preset condition, an Euler spiral trajectory table is generated, wherein trajectory points in different rows of the Euler spiral trajectory table correspond to different curvature change rates, and trajectory points in different columns correspond to different curvatures.

[0076] 105, calculating the current curvature according to the steering wheel angle of the vehicle.

[0077] 106, determining a target column having the smallest curvature difference from the current one in the Euler spiral trajectory table.

[0078] 107, taking each trajectory point in the target column as a starting point, a starting point coordinate system of the Euler spiral is established.

[0079] 108. Determine the sum of tracking errors of the at least two reference points in the coordinate system of the starting point of the Euler spiral.

[0080] 109. Determine the target curvature change rate corresponding to the at least two reference points in the Euler spiral trajectory table when the sum of the tracking errors of the at least two reference points is minimized.

[0081] 110 , determining a front wheel steering angle according to the target curvature change rate, and the front wheel steering angle is used to control the vehicle steering for path tracking.

[0082] The preview distance refers to the distance set by the vehicle in order to smoothly track the path during driving. The preview distance is used to determine the steering angle of the vehicle. By controlling the vehicle steering, the vehicle is moved from the current position to the path point indicated by the preview distance, achieving the effect of path tracking.

[0083] In an embodiment of the present invention, the at least two reference points determined according to the preview distance may include: a first reference point farthest from the current position of the vehicle in the planned path determined by the preview distance; in addition to the first reference point, also include: at least one reference point located between the first reference point and the current position of the vehicle.

[0084] When determining the above-mentioned at least two reference points, the current position of the vehicle may be the center position of the rear axle of the vehicle in the vehicle body coordinate system, or the current position of the vehicle may be the path point closest to the center position of the rear axle of the vehicle in the planned path.

[0085] The first reference point may be a path point in the planned path that is a preview distance from the current position of the vehicle. Alternatively, the first reference point may be the first path point in the planned path from the current position of the vehicle to a path beyond the preview distance. Alternatively, the first reference point may also be the farthest path point in the planned path within the preview distance.

[0086] In the planned path, the path between the current position of the vehicle and the first reference point is regarded as the reference path. Other reference points among the at least two reference points can be determined from the reference path, such as the bisection point, the trisection point, etc. of the reference path.

[0087] After determining the above-mentioned at least two reference points based on the preview distance, the lateral error and heading error of the above-mentioned at least two reference points can be calculated based on the pure tracking algorithm. If the lateral error and heading error of the at least two reference points meet the preset conditions, it means that the tracking path error calculated by the pure tracking algorithm is within an acceptable range, and the steering wheel angle can be calculated based on the trajectory. If the lateral error and heading error of the above-mentioned at least two reference points do not meet the set conditions, it means that the tracking path error calculated by the pure tracking algorithm is large and does not meet the path tracking requirements, and an Euler spiral trajectory table can be established for path tracking.

[0088] Different rows of the Euler spiral trajectory table established in the embodiment of the present invention correspond to different curvature change rates, and different columns represent different curvatures. The current curvature can be calculated based on the current steering wheel angle of the vehicle. The Euler spiral starting point coordinate system can be established based on the column where the current curvature is located. In the Euler spiral starting point coordinate system, the target curvature change rate is determined by finding the minimum value of the sum of the tracking errors of the above-mentioned at least two reference points. The target curvature change rate is used to determine the front wheel turning angle of the vehicle. The front wheel turning angle is the front wheel turning angle corresponding to the time when the sum of the tracking errors of each reference point is the smallest, thereby controlling the vehicle to smoothly track or approach the path points on the planned path, thereby improving the path tracking accuracy.

[0089] See also Figure 2 , is a flow chart of a path tracking method provided by an embodiment of the present invention. Figure 2 As shown, the processing steps of the method include:

[0090] 201, determine two adjacent path points closest to the current position of the vehicle in the planned path. The current position of the vehicle may be the center position of the rear axle of the vehicle in the vehicle body coordinate system.

[0091] See also Figure 3 , is a schematic diagram of a planning path provided by an embodiment of the present invention. Figure 3 As shown, the planned path includes path points t0, t1, t2, t3, ... Figure 3 In the planned path shown, the two adjacent path points closest to the center position of the rear axle are path points t1 = (x1, y1, θ1) T and path point t2 = (x2, y2, θ2) T .

[0092] 202, determining a current lateral error and a heading error according to two adjacent path points.

[0093] exist Figure 3 In the example given, the current lateral error and heading error are estimated by linearly interpolating the positions and heading angles of two adjacent path points.

[0094] like Figure 3 As shown, path point t1 = (x1, y1, θ1) T and path point t2 = (x2, y2, θ2) T The two adjacent path points closest to the center of the rear axle in the vehicle body coordinate system. The adjacent path point t1=(x1,y1,θ1) is calculated by the following formula: T and path point t2 = (x2, y2, θ2) T Perform linear interpolation to estimate the current lateral error e l and heading error eθ .in:

[0095]

[0096]

[0097] 203, determine the preview distance l according to the current lateral error and heading error and the preview distance limit value d .

[0098] After calculating the current lateral error and heading error, multiply the two errors by the two proportional coefficients k1 and k2 respectively to obtain two distances, take the larger value, and use the preview distance limit value l m Limit its value and get the preview distance l d .

[0099] l d =max(l m ,max(k1e l ,k2e θ ).

[0100] Calculating the preview distance l d In the formula, k1 and k2 are determined according to engineering requirements or engineering experience. For example, if the engineering requirement is that the lateral error of the tracking accuracy is less than 0.1m, the heading error is less than 0.05rad, and it is expected to significantly reduce the error within a distance of 10 times the lateral error, then k1 can be set to 10 and k2 to 20m / rad. In this way, when the lateral error reaches 0.1m or the heading error reaches 0.05rad, a preview distance of 1m will be output.

[0101] Calculating the preview distance l d In the formula, the preview distance limit value l m The value range is 0.1~0.5m. If the value is too small, it will easily cause control system oscillation.

[0102] 204, determine the distance from the current position of the vehicle on the planned path as the preview distance l d The path point of is taken as the first reference point r0, the path between the first reference point r0 and the path point t1 is taken as the reference path, and the midpoint on the reference path is taken as the second reference point r1.

[0103] In some embodiments, in addition to using the midpoint of the reference path as the second reference point, a point dividing the reference path into three equal parts, a point dividing the reference path into four equal parts, etc. may also be used as a reference point.

[0104] by Figure 3 For example, path point t1 = (x1, y1, θ1) T and path point t2 = (x2, y2, θ2)T are the two adjacent path points closest to the center of the rear axle of the vehicle. Then the path point t1=(x1,y1,θ1) T As the current position of the vehicle. r0 is the starting point of path point t1 and is equal to or exceeds the preview distance l on the planned path. d The first path point of the planned path at the preview distance l d The waypoint farthest from waypoint t1.

[0105] 205 , determine the lateral error and the heading error of the first reference point r0 and the second reference point r1 .

[0106] According to the pure tracking algorithm, the arc trajectory passing through the reference point r0 can be calculated, that is, the center and radius of the arc can be obtained. Assume that the coordinates and expected heading of the reference point r0 in the current vehicle body coordinate system are (x r0 ,y r0 ,θ r0 ).

[0107]

[0108] The coordinates of the center of the arc trajectory passing through the reference point r0 are (r0, 0).

[0109] According to the geometric properties of the pure tracking algorithm, the lateral error of the first reference point r0 is 0 and the heading error e θr0 for:

[0110]

[0111] The lateral error e of the second reference point r1 lr1 for:

[0112]

[0113] The heading error e of the second reference point r1 θr1 for:

[0114]

[0115] 206, determine whether the lateral error and heading error of the first reference point and the second reference point are respectively less than the corresponding lateral error threshold and heading error threshold; if so, calculate the steering wheel angle according to the trajectory. Optionally, the calculation can be performed according to the front wheel angle calculation formula of Ackerman steering. If not, execute step 207.

[0116] 207. Establish the Euler spiral trajectory table.

[0117] Among them, the step of establishing the Euler spiral trajectory table includes: dividing the preset curvature change rate interval equally to obtain m curvature change rate values. Dividing the curvature interval related to the turning radius of the current vehicle equally to obtain n curvature values. As shown in Table 1, a two-dimensional table with m rows and n columns is generated with curvature change rate values ​​as rows and curvature values ​​as columns. According to the curvature change rate value and curvature value at each unit position in the two-dimensional table, the trajectory point at each unit position is determined to obtain the Euler spiral trajectory table. Among them, the trajectory points located in the same row in the Euler spiral trajectory table represent an Euler spiral trajectory.

[0118] In one example, a maximum curvature change rate σ is artificially preset. max , according to a certain resolution, the curvature change rate interval [-σ max , σ max ] is divided equally, and a set of σ values ​​is obtained. The 0 value is discarded. Assuming that m σ values ​​are obtained, the m σ values ​​constitute a σ value table. The maximum curvature c can be calculated based on the minimum turning radius of the car. max . The curvature interval -c is divided into max , c max ] is divided equally, and a set of c values ​​is obtained. Assuming that n c values ​​are obtained, the n c values ​​constitute a c value table. As shown in Table 1, a two-dimensional table with m rows and n columns is generated according to the σ value table and the c value table.

[0119] <![CDATA[σ1c1]]> <![CDATA[σ1c2]]> <![CDATA[σ1c3]]> <![CDATA[σ1c4]]> …… <![CDATA[σ1c n ]]> <![CDATA[σ2c1]]> <![CDATA[σ2c m ]]> …… …… <![CDATA[σ m c1]]> <![CDATA[σ m c2]]> <![CDATA[σ m c3]]> <![CDATA[σ m c4]]> …… <![CDATA[σ m c n ]]>

[0120] Table 1 Two-dimensional table generated with curvature change rate values ​​as rows and curvature values ​​as columns

[0121]

[0122] Table 2 Euler spiral trajectory table

[0123] According to the curvature change rate and curvature value at each unit position in Table 1, the trajectory point T(σ m c n ), as shown in Table 2. Trajectory point T(σ m c n ) can be expressed as (x, y, θ) T ,(x,y,θ) T The calculation formula is:

[0124] s = c / σ;

[0125]

[0126]

[0127] The calculated coordinates of each trajectory point are placed at the corresponding cell position to obtain the Euler spiral trajectory table shown in Table 2. In the Euler spiral trajectory table shown in Table 2, each row is an Euler spiral trajectory. The subsequent part of this method compares the actual planned path with the Euler spiral trajectory represented by each row, and selects the path with the best fit to correct the current error.

[0128] 208, determine the starting point of the Euler spiral and establish a coordinate system of the starting point of the Euler spiral.

[0129] The current curvature c0 is calculated based on the steering wheel angle fed back by the vehicle chassis data. The column whose c value in the two-dimensional table is closest to the current curvature c0 is determined as the target column, and the starting point coordinate system of the Euler spiral is established with each trajectory point in the target column as the starting point.

[0130] Taking Table 1 and Table 2 as examples, assuming that the value closest to the current curvature c0 in the two-dimensional table is c3, then the column σ where c3 is located in Table 1 is t c3、σ2c3……σ m c3 is the target column, and the target columns are σ1c3, σ2c3, … m c3 in Table 2 corresponds to the trajectory points T(σ1c3), T(σ2c3)...T(σ m c3) are the starting points of each row.

[0131] According to the pose of each row starting point (x s ,y s ,θ s ) T Establish a coordinate system with the origin of the coordinate system as (x s ,y s ) T , the x-axis is θ s The positive direction of the angle, this coordinate system is also a body coordinate system, and this coordinate system is also called the starting point coordinate system of the Euler spiral.

[0132] 209 , determining the sum of the tracking errors of the first reference point r0 and the second reference point r1 in the coordinate system of the starting point of the Euler spiral.

[0133] Specifically, the trajectory points in each row of the Euler spiral trajectory table are converted to the Euler spiral starting point coordinate system of the row; the first reference point r0 and the second reference point r1 are converted in the Euler spiral starting point coordinate system of each row of the Euler spiral trajectory table; in the Euler spiral starting point coordinate system of each row of the Euler spiral trajectory table, the sum of the tracking errors of the first reference point r0 and the second reference point r1 on each row is determined.

[0134] Optionally, determining the sum of the tracking errors of the first reference point r0 and the second reference point r1 on each row includes: determining, among the trajectory points in each row of the Euler spiral trajectory table, the trajectory point that is closest to the first reference point r0 and the second reference point r1 on each row; determining the tracking errors of the first reference point r0 and the second reference point r1 on each row based on the trajectory point that is closest to the first reference point r0 and the second reference point r1 on each row; adding the tracking errors of the first reference point r0 and the second reference point r1 on each row to obtain the sum of the tracking errors of the first reference point r0 and the second reference point r1 on each row.

[0135] In other embodiments, determining the sum of the tracking errors of the first reference point r0 and the second reference point r1 on each row includes: determining, among the trajectory points in each row of the Euler spiral trajectory table, the first reference point r0 in each row that is closest to the trajectory point; determining a first tracking error between the first reference point r0 and the closest trajectory point in each row; determining filter rows whose first tracking errors are less than an allowable threshold; and calculating the tracking errors of the second reference point r1 on each filter row.

[0136] In some embodiments, in a row of the Euler spiral trajectory table, the first reference point r0 is closest to the trajectory point in the row (x n ,y n ,θ n ) T .

[0137] The coordinates of the first reference point r0 before transformation are (x, y, θ) T , at the starting point coordinate (x s ,y s ,θ s ) T The coordinates after the transformation are (x′, y′, θ′), and the transformation formula is:

[0138]

[0139] θ′=θ+θ s .

[0140] In a row, the first reference point r0 is closest to the trajectory point (x n ,y n ,θ n ) T The tracking error is:

[0141]

[0142] e y =‖e l ‖+k e ‖θ r ′0-θ n ‖.

[0143] where k e It is an artificially set ratio parameter.

[0144] In this way, the tracking error of the first reference point r0 in each row can be calculated. Similarly, the tracking error of the second reference point r1 in each row can be calculated. Thus, the sum of the tracking errors of the first reference point r0 and the second reference point r1 in each row can be calculated. Of course, if there are other reference points in addition to the first reference point r0 and the second reference point r1, the sum of the tracking errors of all reference points is calculated in each row.

[0145] In some embodiments, the tracking error of the first reference point r0 in each row may be calculated first, and the filtering rows where the tracking error of the first reference point r0 is less than the allowable threshold may be determined. Then, the tracking error of the second reference point r1 in each row may be calculated on the filtering rows.

[0146] 210, determine the target curvature change rate under the most recent tracking path.

[0147] After calculating the sum of the tracking errors of the first reference point r0 and the second reference point r1 on each row, the row with the smallest sum of the tracking errors is determined as the corrected tracking path, and the curvature change rate of the row with the smallest sum of the tracking errors is determined as the target curvature change rate.

[0148] 211, determining a feedforward control front wheel steering angle according to the target curvature change rate.

[0149] Among them, determining the feedforward control front wheel steering angle includes: determining the vehicle travel distance from the last output front wheel steering angle for path tracking to the vehicle's current position; determining the current feedforward curvature based on the vehicle travel distance and the target curvature change rate; determining the feedforward control front wheel steering angle based on the current feedforward curvature.

[0150] Specifically, the vehicle travel distance L from the last output of the front wheel steering angle to the current position of the vehicle after the corrected path is estimated based on the wheel odometer data, and the current feedforward curvature k is calculated f And the corresponding feedforward control front wheel steering angle δ.

[0151] k f =σL+k0;

[0152] δ=atan(k f l).

[0153] Where l is the wheelbase of the vehicle's front and rear axles.

[0154] The only control quantity for vehicle steering control is the steering wheel angle. The steering wheel angle is generally proportional to the front wheel angle. Knowing one of them is equivalent to knowing both. As long as you know when or under what conditions the steering wheel is turned to what degree, you can control the vehicle to the desired position. In the embodiment of the present invention, the vehicle driving distance L is the condition. Under this condition, the steering wheel is turned to a certain angle so that the front wheel angle is δ, and the vehicle can drive according to the planned trajectory.

[0155] 212, determine the feedback control front wheel steering angle compensation according to the lateral error and heading error of the current position of the vehicle. The lateral error and heading error of the current position of the vehicle can be the error calculated in step 202. l and e θ .

[0156] 213, determining a front wheel steering angle based on the feedforward control front wheel steering angle and the feedback control front wheel steering angle compensation, the front wheel steering angle being used to control the vehicle steering for path tracking.

[0157] Among them, the feedforward control front wheel angle and the feedback control front wheel angle compensation can be summed to obtain the final output front wheel angle δ out .

[0158] δ out = atan(k f l)+p1e l +i1∑e l +p2e θ +i2∑e θ .

[0159] Among them, p1, i1, p2 and i2 are parameters set through experiments, and the sum sign represents the summation of historical errors.

[0160] The front wheel steering angle is the only steering control parameter, and the feedforward controlled front wheel steering angle is the main part of the parameter. The final output is to control the steering of the vehicle so that the vehicle can travel along a predetermined trajectory. The embodiment of the present invention selects the best tracking path according to the Euler spiral trajectory table in order to use the curvature change rate σ of the row where the best tracking path is located to calculate the feedforward controlled front wheel steering angle. The feedforward controlled front wheel steering angle obtained according to this curvature change rate σ can enable the vehicle to smoothly follow or approach the planned path.

[0161] Corresponding to the above method, an embodiment of the present invention provides a path tracking device, such as Figure 4 As shown, the device comprises:

[0162] The preview distance determination module 401 is used to determine the preview distance according to the current position of the vehicle.

[0163] The reference point determination module 402 is used to determine at least two reference points among the path points of the planned path according to the preview distance.

[0164] The Euler spiral module 403 is used to generate an Euler spiral trajectory table in response to the lateral error and the heading error of the at least two reference points not satisfying the preset conditions, wherein the trajectory points in different rows of the Euler spiral trajectory table correspond to different curvature change rates, and the trajectory points in different columns correspond to different curvatures; calculate the current curvature according to the steering wheel angle of the vehicle; determine the target column with the smallest difference from the current curvature in the Euler spiral trajectory table; and establish the Euler spiral starting point coordinate system with each trajectory point in the target column as a starting point.

[0165] The path tracking module 404 is used to determine the sum of the tracking errors of the at least two reference points in the starting point coordinate system of the Euler spiral; determine the target curvature change rate corresponding to the at least two reference points in the Euler spiral trajectory table when the sum of the tracking errors of the at least two reference points is minimized.

[0166] The front wheel steering angle output module 405 is used to determine the front wheel steering angle according to the target curvature change rate, and the front wheel steering angle is used to control the vehicle steering to perform path tracking.

[0167] The path tracking device of the embodiment of the present invention can execute the path tracking method of the embodiment shown above. For the part not described in detail in the embodiment of the present invention, reference can be made to the relevant description of the method embodiment. The execution process and technical effect of the technical solution can be referred to the description in the method embodiment, which will not be repeated here.

[0168] See also Figure 5 , is a schematic diagram of the structure of a path tracking device provided by an embodiment of the present invention. The path tracking device may be, for example, a vehicle-mounted device. Figure 5 As shown, the path tracking device 500 may include: a processor 501, a memory 502 and a communication unit 503. These components communicate through one or more buses. Those skilled in the art can understand that the structure of the path tracking device shown in the figure does not constitute a limitation on the embodiments of the present application. It can be a bus structure or a star structure, and can also include more or fewer components than shown in the figure, or combine certain components, or arrange the components differently.

[0169] The communication unit 503 is used to establish a communication channel so that the path tracking device can communicate with other devices, receive user data sent by other devices or send user data to other devices.

[0170] The processor 501 is the control center of the path tracking device. It uses various interfaces and lines to connect various parts of the entire path tracking device. It runs or executes software programs, instructions, and / or modules stored in the memory 502, and calls data stored in the memory to perform various functions of the path tracking device and / or process data. The processor can be composed of an integrated circuit (IC), for example, it can be composed of a single packaged IC, or it can be composed of a plurality of packaged ICs with the same or different functions. For example, the processor 501 may include a central processing unit (CPU), a microcontroller unit (MCU), etc.

[0171] The memory 502 is used to store the execution instructions of the processor 501. The memory 502 can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic memory, flash memory, magnetic disk or optical disk.

[0172] When the execution instructions in the memory 502 are executed by the processor 501 , the path tracking device 500 is enabled to execute the path tracking method in the embodiment of the present invention.

[0173] In a specific implementation, the present application further provides a computer storage medium, wherein the computer storage medium may store a program, and when the program is executed, the program may include some or all of the steps in each embodiment of the path tracking method provided in the present application. The storage medium may be a disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM), etc.

[0174] In a specific implementation, the present application also provides a computer program product, wherein the computer program product includes executable instructions, and when the executable instructions are executed on a computer, the computer executes part or all of the steps in each embodiment of the path tracking method provided in the present application.

[0175] The embodiment of the present application also provides a non-temporary computer-readable storage medium, which stores computer instructions, and the computer instructions enable the computer to execute the path tracking method provided by the embodiment of the present application.

[0176] The above-mentioned non-temporary computer-readable storage medium can adopt any combination of one or more computer-readable media. The computer-readable medium can be a computer-readable signal medium or a computer-readable storage medium. The computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples (non-exhaustive list) of computer-readable storage media include: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (Read Only Memory; hereinafter referred to as: ROM), an erasable programmable read-only memory (ErasableProgrammable Read Only Memory; hereinafter referred to as: EPROM) or flash memory, optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In this document, a computer-readable storage medium can be any tangible medium containing or storing a program, which can be used by an instruction execution system, device or device or used in combination with it.

[0177] Those skilled in the art can clearly understand that the technology in the embodiments of the present application can be implemented by means of software plus a necessary general hardware platform. Based on this understanding, the technical solution in the embodiments of the present application is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which can be stored in a storage medium such as ROM / RAM, a disk, an optical disk, etc., including several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment of the present application or some parts of the embodiments.

[0178] In this specification, the same or similar parts between the various embodiments can be referred to each other. In particular, for the device embodiment and the terminal embodiment, since they are basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the description in the method embodiment.

Claims

1. A path tracking method, characterized in that: The method comprises: Determine the preview distance based on the vehicle's current position; Determining at least two reference points among the path points of the planned path according to the preview distance; In response to the lateral errors and heading errors of the at least two reference points not satisfying a preset condition, generating an Euler spiral trajectory table, wherein trajectory points in different rows of the Euler spiral trajectory table correspond to different curvature change rates, and trajectory points in different columns correspond to different curvatures; Calculate the current curvature based on the vehicle's steering wheel angle; Determine in the Euler spiral trajectory table the target column having the smallest curvature difference from the current column; Taking each trajectory point in the target column as a starting point, a starting point coordinate system of the Euler spiral is established; Determining the sum of tracking errors of the at least two reference points in the coordinate system of the starting point of the Euler spiral; Determine the target curvature change rate corresponding to the at least two reference points in the Euler spiral trajectory table when the sum of the tracking errors of the at least two reference points is minimized; A front wheel steering angle is determined according to the target curvature change rate, and the front wheel steering angle is used to control the steering of the vehicle to perform path tracking.

2. The method according to claim 1, characterized in that Determining the preview distance according to the current position of the vehicle includes: Determining two adjacent path points closest to the current position of the vehicle in the planned path; Determining a current lateral error and a heading error based on the two adjacent path points; The preview distance is determined according to the current lateral error, the heading error and the preview distance limit value.

3. The method according to claim 1 or 2, characterized in that: The at least two reference points include: A first reference point in the planned path that is farthest from the current position of the vehicle determined by the preview distance; and further comprising: At least one reference point located between the first reference point and the current position of the vehicle.

4. The method according to claim 1, characterized in that: The generating of the Euler spiral trajectory table comprises: The preset curvature change rate interval is equally divided to obtain m curvature change rate values; The curvature interval related to the turning radius of the current vehicle is equally divided to obtain n curvature values; Generate a two-dimensional table with m rows and n columns, with the curvature change rate values ​​as rows and the curvature values ​​as columns; Determine the trajectory point at each unit position according to the curvature change rate value and the curvature value at each unit position in the two-dimensional table to obtain the Euler spiral trajectory table; Among them, the trajectory points located in the same row in the Euler spiral trajectory table represent an Euler spiral trajectory.

5. The method according to claim 1 or 4, characterized in that: Determining the sum of the tracking errors of the at least two reference points in the coordinate system of the starting point of the Euler spiral includes: Convert the trajectory points in each row of the Euler spiral trajectory table to the coordinate system of the starting point of the Euler spiral in the row where it is located; The at least two reference points are subjected to coordinate transformation in the Euler spiral starting point coordinate system of each row of the Euler spiral trajectory table; In the Euler spiral starting point coordinate system of each row of the Euler spiral trajectory table, the sum of the tracking errors of the at least two reference points on each row is determined.

6. The method according to claim 5, characterized in that The step of determining the sum of the tracking errors of the at least two reference points on each row in the Euler spiral starting point coordinate system of each row of the Euler spiral trajectory table comprises: Among the trajectory points in each row of the Euler spiral trajectory table, determine the trajectory point closest to each reference point in each row; Determine the tracking error of each reference point on each row based on the distance of each reference point on each row to the nearest track point; The tracking errors of the reference points on each row are added together to obtain the sum of the tracking errors of the at least two reference points on each row.

7. The method according to claim 5, characterized in that The at least two reference points include a first reference point; Among the trajectory points in each row of the Euler spiral trajectory table, determining the closest trajectory point of each reference point on each row; and determining the tracking error of each reference point on each row according to the closest trajectory point of each reference point on each row, including: Among the trajectory points in each row of the Euler spiral trajectory table, determine the trajectory point in each row that is closest to the first reference point; Determine a first tracking error between the first reference point in each row and the closest track point in the row; determining a filter row in which the first tracking error is less than a tolerable threshold; Calculate the tracking error of the remaining reference points on each filtered row.

8. The method according to claim 1, characterized in that The determining the front wheel turning angle according to the target curvature change rate comprises: Determining a feedforward controlled front wheel steering angle according to the target curvature change rate; Determine the feedback control front wheel steering angle compensation according to the lateral error and heading error existing in the current position of the vehicle; The front wheel steering angle is determined according to the feedforward controlled front wheel steering angle and the feedback controlled front wheel steering angle compensation.

9. The method according to claim 8, characterized in that The step of determining the feedforward control front wheel steering angle according to the target curvature change rate comprises: Determine the distance traveled by the vehicle from the last output of the front wheel steering angle for path tracking to the current position of the vehicle; Determining a current feedforward curvature according to the vehicle travel distance and the target curvature change rate; The feedforward controlled front wheel steering angle is determined according to the current feedforward curvature.

10. A path tracking device, characterized in that: The device comprises: A preview distance determination module is used to determine the preview distance according to the current position of the vehicle; A reference point determination module, configured to determine at least two reference points among the path points of the planned path according to the preview distance; The Euler spiral module is used for generating an Euler spiral trajectory table in response to the lateral error and the heading error of the at least two reference points not satisfying a preset condition, wherein trajectory points in different rows of the Euler spiral trajectory table correspond to different curvature change rates, and trajectory points in different columns correspond to different curvatures; calculating a current curvature according to a steering wheel angle of the vehicle; determining a target column with a minimum difference from the current curvature in the Euler spiral trajectory table; and establishing an Euler spiral starting point coordinate system with each trajectory point in the target column as a starting point; A path tracking module, used to determine the sum of the tracking errors of the at least two reference points in the starting coordinate system of the Euler spiral; determine the target curvature change rate corresponding to the at least two reference points in the Euler spiral trajectory table when the sum of the tracking errors of the at least two reference points is minimized; The front wheel steering angle output module is used to determine the front wheel steering angle according to the target curvature change rate, and the front wheel steering angle is used to control the vehicle steering to perform path tracking.

11. A path tracking device, characterized in that: include: A memory for storing computer program instructions and a processor for executing the program instructions, wherein when the computer program instructions are executed by the processor, the server is caused to execute the method according to any one of claims 1 to 9.

12. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes a stored program, wherein when the program is executed, the device where the computer-readable storage medium is located is controlled to execute the method according to any one of claims 1 to 9.

Images