Path tracking method, device and storage medium

Abstract

The application relates to a path tracking method, device and storage medium. The method comprises the following steps: determining a pre-look distance; determining at least two reference points in path points of a planned path according to the pre-look distance; in response to the transverse error and the heading error of the at least two reference points not satisfying a preset condition, generating an Euler spiral trajectory table; determining a target column with the minimum current curvature difference value in the Euler spiral trajectory table; establishing an Euler spiral starting point coordinate system with each trajectory point in the target column as a starting point; determining the sum of tracking errors of the at least two reference points in the Euler spiral starting point coordinate system; when the sum of the tracking errors of the at least two reference points takes the minimum value, determining a target curvature change rate; and determining a front wheel steering angle according to the target curvature change rate, the front wheel steering angle being used for controlling vehicle steering to perform path tracking. The embodiment scheme can improve path tracking precision and control the vehicle to gently follow or approach the planned path.

Description

Technical Field

[0001] This application relates to the field of vehicle control technology, and more specifically to a path tracking method, apparatus, and storage medium. Background Technology

[0002] Pure tracking algorithms are a common type of path tracking algorithm. They control the vehicle to follow the planned path by tracking waypoints along that path. However, pure tracking algorithms often suffer from problems such as large heading errors and poor adaptability to trajectories with high rates of curvature change. Summary of the Invention

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

[0004] In a first aspect, embodiments of the present invention provide a path tracking method, the method comprising:

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

[0006] Based on the pre-aiming distance, at least two reference points are determined among the waypoints of the planned path;

[0007] In response to the fact that the lateral error and heading error of the at least two reference points do not meet the preset conditions, an Euler spiral trajectory table is generated, wherein trajectory points in different rows of the Euler spiral trajectory table correspond to different rates of curvature change, and trajectory points in different columns correspond to different curvatures.

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

[0009] In the Euler spiral trajectory table, determine the target column with the smallest difference from the current curvature;

[0010] Establish an Euler spiral starting coordinate system with each trajectory point in the target column as the starting point;

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

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

[0013] The front wheel steering angle is determined based on the target rate of curvature change, and the front wheel steering angle is used to control the vehicle steering for path tracking.

[0014] In some embodiments, determining the aiming distance based on the vehicle's current position includes:

[0015] In the planned path, determine the two nearest adjacent path points to the vehicle's current position;

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

[0017] The aiming distance is determined based on the current lateral error, the heading error, and the aiming distance limit.

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

[0019] The first reference point, located furthest from the vehicle's current position and determined by the pre-aiming distance, is also included in the planned path; furthermore:

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

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

[0022] Divide the preset curvature change rate interval into m equal parts to obtain m curvature change rate values;

[0023] Divide the curvature interval related to the current vehicle's turning radius into n curvature values.

[0024] Generate a two-dimensional table with m rows and n columns, using the rate of change of curvature as the row and curvature as the column;

[0025] Based on the rate of curvature change 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;

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

[0027] In some embodiments, 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:

[0028] Transform 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 that row;

[0029] The coordinates of the at least two reference points are transformed in the coordinate system of the starting point of the Euler spiral in each row of the Euler spiral trajectory table.

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

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

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

[0033] The tracking error of each reference point on each line is determined based on the nearest trajectory point to each reference point on each line.

[0034] The tracking errors of each reference point in each row are summed to obtain the sum of the tracking errors of the at least two reference points in each row.

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

[0036] In the trajectory points of each row of the Euler spiral trajectory table, determine the nearest trajectory point for each reference point in each row; based on the nearest trajectory point for each reference point in each row, determine the tracking error for each reference point in each row, including:

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

[0038] Determine the first tracking error between the first reference point in each row and the nearest trajectory point in that row;

[0039] Determine the filter rows where the first tracking error is less than the allowable threshold;

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

[0041] In some embodiments, determining the front wheel steering angle based on the target rate of curvature change includes:

[0042] The feedforward control front wheel angle is determined based on the target rate of curvature change.

[0043] Based on the lateral and heading errors present at the vehicle's current position, determine the feedback control front wheel steering angle compensation;

[0044] The front wheel angle is determined based on the feedforward control of the front wheel angle and the feedback control of the front wheel angle compensation.

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

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

[0047] The current feedforward curvature is determined based on the vehicle's travel distance and the target curvature change rate.

[0048] The feedforward control front wheel angle is determined based on the current feedforward curvature.

[0049] Secondly, embodiments of the present invention provide a path tracking device, the device comprising:

[0050] The aiming distance determination module is used to determine the aiming distance based on the vehicle's current position.

[0051] The reference point determination module is used to determine at least two reference points among the path points of the planned path based on the pre-aiming distance;

[0052] The Euler spiral module is used to generate an Euler spiral trajectory table in response to the lateral error and heading error of the at least two reference points not meeting preset conditions. The trajectory points in different rows of the Euler spiral trajectory table correspond to different rates of curvature change, and the trajectory points in different columns correspond to different curvatures. The current curvature is calculated based on the vehicle's steering wheel angle. The target column with the smallest difference from the current curvature is determined in the Euler spiral trajectory table. An Euler spiral starting coordinate system is established with each trajectory point in the target column as the starting point.

[0053] The path tracking module is used to 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; and to determine the target curvature change rate corresponding to the at least two reference points in the Euler spiral trajectory table when the sum of 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 based on the target rate of curvature change, and the front wheel steering angle is used to control the vehicle steering for path tracking.

[0055] Thirdly, embodiments of the present invention provide a path tracking device, comprising: 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 executes the method described in the first aspect or any one of the first aspects.

[0056] Fourthly, embodiments of the present invention provide a computer-readable storage medium comprising a stored program, wherein, when the program is executed, it controls the device where the computer-readable storage medium is located to perform the method described in the first aspect or any one of the first aspects.

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

[0058] This invention improves path tracking accuracy by introducing at least two path reference points. Furthermore, the method incorporates an Euler spiral trajectory table, which has a uniform rate of curvature change. By using the Euler spiral trajectory table for steering control, the steering wheel can be rotated uniformly, allowing the vehicle to smoothly track or approach the planned path. Attached Figure Description

[0059] Figure 1 A flowchart of a path tracing method provided in an embodiment of the present invention;

[0060] Figure 2 A flowchart of a path tracing method provided in an embodiment of the present invention;

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

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

[0063] Figure 5 This is a schematic diagram of a path tracking device provided in an embodiment of the present invention. Detailed Implementation

[0064] To better understand the technical solution of this application, the embodiments of this application will be described in detail below with reference to the accompanying drawings.

[0065] It should be understood that the described embodiments are merely some, not all, of the embodiments in this application. All other embodiments obtained by those skilled in the art based on the embodiments in this application without inventive effort are within the scope of protection of this application.

[0066] The terminology used in the embodiments of this application is for the purpose of describing particular embodiments only and is not intended to be limiting of this application. The singular forms “a,” “the,” and “the” used in the embodiments of this application and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise.

[0067] It should be understood that the term "and / or" used in this article is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, or B existing alone. Additionally, the character " / " in this article generally indicates that the preceding and following related objects have an "or" relationship.

[0068] In the fields of automated parking and autonomous driving, path planning refers to the desired path that allows a vehicle to move from one location to its destination. A planned path consists of a series of discrete waypoints, each containing location information and the vehicle's desired heading angle when it reaches that point. The waypoints on the planned path satisfy vehicle kinematics; that is, when the vehicle reaches a waypoint and its actual heading angle matches the desired heading angle of that waypoint, the vehicle can reach the next waypoint with a single forward or backward movement involving steering, and the actual heading angle of the next waypoint will match the desired heading angle of that waypoint.

[0069] In related technologies, path point tracking can be performed by calculating the vehicle's actual steering angle using a pure tracking algorithm. However, using a pure tracking algorithm for path tracking can result in problems such as large heading errors and poor adaptability to trajectories with high curvature rates.

[0070] To address the aforementioned technical problems, this invention provides a path tracking method. This method improves path tracking accuracy by introducing at least two path reference points. Furthermore, the method incorporates an Euler spiral trajectory table, where the rate of curvature change is uniform. By using the Euler spiral trajectory table for steering control, the steering wheel can rotate evenly, allowing the vehicle to smoothly track or approach the planned path.

[0071] See Figure 1 This is a flowchart of a path tracking method provided in an embodiment of the present invention. Figure 1 As shown, the processing steps of this method include:

[0072] 101. Determine the aiming distance based on the vehicle's current position.

[0073] 102. Based on the pre-aiming distance, determine at least two reference points among the waypoints of the planned path.

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

[0075] 104. In response to the fact that the lateral error and heading error of at least two reference points do not meet the preset conditions, an Euler spiral trajectory table is generated. The trajectory points in different rows of the Euler spiral trajectory table correspond to different rates of curvature change, and the trajectory points in different columns correspond to different curvatures.

[0076] 105. Calculate the current curvature based on the steering wheel angle of the vehicle.

[0077] 106. Determine the target column with the smallest difference from the current curvature in the Euler spiral trajectory table.

[0078] 107. Using each trajectory point in the target column as the starting point, establish the starting coordinate system of the Euler spiral.

[0079] 108. In the coordinate system of the starting point of the Euler spiral, determine the sum of the tracking errors of at least two of the above reference points.

[0080] 109. Determine the rate of change of the target curvature of 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. The front wheel steering angle is determined based on the target rate of curvature change. The front wheel steering angle is used to control the vehicle steering for path tracking.

[0082] The preview distance is a distance set by the vehicle during operation to ensure smooth path tracking. It determines the vehicle's steering angle, and by controlling the vehicle's steering, it moves from its current position to the path point indicated by the preview distance, thus achieving path tracking.

[0083] In this embodiment of the invention, the at least two reference points determined based on the pre-aiming distance may include: a first reference point that is furthest from the vehicle's current position in the planned path, determined by the pre-aiming distance; and in addition to the first reference point, at least one reference point located between the first reference point and the vehicle's current position.

[0084] When determining at least two reference points, the vehicle's current position can be the center of the rear axle in the vehicle coordinate system, or it can be the path point in the planned path that is closest to the center of the rear axle.

[0085] The first reference point mentioned above can be a path point in the planned path that is a pre-aiming distance from the vehicle's current position. Optionally, the first reference point can be the first path point in the planned path after the vehicle's current position exceeds the pre-aiming distance. Optionally, the first reference point can also be the farthest path point in the planned path within the pre-aiming distance.

[0086] In the planned path, the path from the vehicle's current position to the first reference point is considered the reference path. Other reference points among the at least two mentioned above can be determined from the reference path; these other reference points are, for example, the bisection points, trisection points, etc., of the reference path.

[0087] After determining at least two reference points based on the pre-aiming distance, the lateral and heading errors of these reference points can be calculated using a pure tracking algorithm. If the lateral and heading errors of these reference points meet preset conditions, it indicates 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 this trajectory. If the lateral and heading errors of these reference points do not meet the preset conditions, it indicates 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] In this embodiment of the invention, different rows of the Euler spiral trajectory table correspond to different rates of curvature change, and different columns represent different curvatures. The current curvature can be calculated based on the vehicle's current steering wheel angle. An Euler spiral starting point coordinate system can be established based on the column containing the current curvature. In this coordinate system, the target rate of curvature change is determined by finding the minimum sum of the tracking errors of at least two reference points. This target rate of curvature change is used to determine the vehicle's front wheel angle. This front wheel angle is the angle corresponding to the minimum sum of the tracking errors of all reference points, thereby controlling the vehicle to smoothly track or approach path points on the planned path, improving path tracking accuracy.

[0089] See Figure 2 This is a flowchart of a path tracking method provided in an embodiment of the present invention. Figure 2 As shown, the processing steps of this method include:

[0090] 201. Determine the two nearest adjacent path points to the vehicle's current position in the planned path. The vehicle's current position can be the center of the rear axle in the vehicle's coordinate system.

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

[0092] 202. Determine the current lateral error and heading error based on two adjacent path points.

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

[0094] like Figure 3 As shown, the path point t1 = (x1, y1, θ1) T And the path point t2 = (x2, y2, θ2) T These are the two adjacent path points closest to the center of the rear axle in the vehicle's coordinate system. The adjacent path points t1 = (x1, y1, θ1) are defined using the following formula. T And the 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 aiming distance l based on the current lateral and heading errors and the aiming distance limit. d .

[0098] After calculating the current lateral and heading errors, multiply the two errors by two scaling factors k1 and k2 respectively to obtain two distances. Take the larger of the two distances and apply it to the pre-aiming distance limit value l. m By restricting its value, the aiming distance l is obtained. d .

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

[0100] In calculating the pre-aiming distance l d In the formula, k1 and k2 are determined based on engineering requirements or experience. For example, if engineering requirements dictate that the lateral error of tracking accuracy is less than 0.1m and the heading angle error is less than 0.05rad, and the goal is to significantly reduce the error within a distance equal to 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 1m pre-aiming distance will be output.

[0101] In calculating the pre-aiming distance l d In the formula, the pre-aiming distance limit value l m The value range is 0.1 to 0.5m. If the value is too small, it is easy to cause oscillation in the control system.

[0102] 204. Determine the distance from the vehicle's current position on the planned path as the aiming distance l. d The path point is taken as the first reference point r0, the path between the first reference point r0 and the path point t1 is regarded 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, the trisection points, quartersection points, etc., of the reference path can also be used as reference points.

[0104] by Figure 3 For example, the path point t1 = (x1, y1, θ1) T And the path point t2 = (x2, y2, θ2)T These are the two adjacent path points closest to the center of the rear axle of the vehicle. Therefore, path point t1 = (x1, y1, θ1). T The vehicle's current position is r0, which starts from path point t1 and extends along the planned path at a distance equal to or greater than the pre-aimed distance l. d The first waypoint, or the path within the pre-aimed distance l in the planned path. d The path point that is furthest from path point t1.

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

[0106] The circular trajectory passing through reference point r0 can be calculated using a pure tracking algorithm, thus determining the center and radius of the arc. Let the coordinates of reference point r0 in the current vehicle coordinate system and its desired heading be (x...). r0 y r0 θ r0 ).

[0107]

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

[0109] Based on 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] Lateral error e at the second reference point r1 lr1 for:

[0112]

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

[0114]

[0115] 206. Determine whether the lateral error and heading error of the first and second reference points are less than their respective lateral error thresholds and heading error thresholds. If so, calculate the steering wheel angle based on the trajectory. Optionally, the calculation can be performed using the front wheel steering angle calculation formula of Ackermann steering. If not, proceed to step 207.

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

[0117] The steps for establishing the Euler spiral trajectory table include: dividing a preset curvature change rate interval into m equal parts to obtain m curvature change rate values; dividing the curvature interval related to the current vehicle's turning radius into n curvature values; generating a two-dimensional table of m rows and n columns, with the curvature change rate values ​​as rows and the curvature values ​​as columns; and determining the trajectory point at each unit position based on the curvature change rate value and the curvature value in the two-dimensional table to obtain the Euler spiral trajectory table. Trajectory points in the same row of the Euler spiral trajectory table represent one Euler spiral trajectory.

[0118] In one example, a maximum rate of change of curvature σ is pre-defined. max The curvature change rate range [-σ] is divided at a certain resolution. max , σ max Divide the vehicle into equal parts to obtain a set of σ values. Discard the 0 values. Assume you get m σ values, which form a σ value table. The maximum curvature c can be calculated based on the vehicle's minimum turning radius. max The curvature interval -c is divided at a certain resolution Δc. max c max Divide the σ value into equal parts to obtain a set of c values. Assuming there are n c values, these n c values ​​form a c value table. As shown in Table 1, generate a two-dimensional table with m rows and n columns based on 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 is a two-dimensional table generated with the rate of change of curvature as the row and the curvature value as the column.

[0121]

[0122] Table 2. Euler Spiral Trajectory Table

[0123] Based on the rate of curvature change and curvature values ​​at each cell location in Table 1, the trajectory point T(σ) at the corresponding cell location can be obtained. m c n As shown in Table 2. Trajectory point T(σ) m c n The trajectory coordinates of (x, y, θ) can be represented 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 in the corresponding cell positions to obtain the Euler spiral trajectory table shown in Table 2. In the Euler spiral trajectory table shown in Table 2, each row represents 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 the coordinate system of the starting point of the Euler spiral.

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

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

[0131] Based on the pose (x) of the starting point of each row s ,y s ,θ s ) T Establish a coordinate system with its origin at (x) s ,y s ) T The x-axis is θ s The positive direction of the angle; this coordinate system is also a vehicle body coordinate system, also known as the Euler spiral starting point coordinate system.

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

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

[0134] Optionally, determining the sum of the tracking errors of the first reference point r0 and the second reference point r1 in each row includes: determining the closest trajectory point of the first reference point r0 and the second reference point r1 in each row of the Euler spiral trajectory table; determining the tracking error of the first reference point r0 and the second reference point r1 in each row based on the closest trajectory point of the first reference point r0 and the second reference point r1 in each row; and adding the tracking errors of the first reference point r0 and the second reference point r1 in each row to obtain the sum of the tracking errors of the first reference point r0 and the second reference point r1 in 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 in each row includes: determining the closest trajectory point of the first reference point r0 in each row of the trajectory points in the Euler spiral trajectory table; determining a first tracking error between the first reference point r0 and the closest trajectory point in each row; determining filter rows where the first tracking error is less than an allowable threshold; and calculating the tracking error of the second reference point r1 in each filter row.

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

[0137] The coordinates of the first reference point r0 before the transformation are (x, y, θ). T The starting coordinates (x, y) of a certain row in the Euler spiral trajectory table s ,y s ,θ s ) T The transformed coordinates are (x′, y′, θ′), and the transformation formula is:

[0138]

[0139] θ′=θ+θ s .

[0140] In a certain 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 a proportional parameter set by humans.

[0144] Following this method, 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. Therefore, 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 besides the first reference point r0 and the second reference point r1, then the sum of the tracking errors of all reference points in each row should be calculated.

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

[0146] 210, Determine the rate of change of the target curvature under the most recently tracked 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 tracking errors is determined as the corrected tracking path. The curvature change rate of the row with the smallest sum of tracking errors is then determined as the target curvature change rate.

[0148] 211. Determine the front wheel steering angle for feedforward control based on the target rate of curvature change.

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

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

[0151] k f =σL+k0;

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

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

[0154] The only controllable variable for vehicle steering control is the steering wheel angle. The steering wheel angle is generally proportional to the front wheel angle; knowing one is equivalent to knowing both. Knowing when or under what conditions to turn the steering wheel by a certain angle allows control of the vehicle's steering to the desired position. In this embodiment, the vehicle's travel distance L is the condition. Under these conditions, turning the steering wheel to a certain angle so that the front wheel angle is δ will cause the vehicle to travel along the planned trajectory.

[0155] 212. Based on the lateral and heading errors present at the vehicle's current position, determine the feedback control front wheel steering angle compensation. The lateral and heading errors present at the vehicle's current position can be the values ​​of e calculated in step 202. l and e θ .

[0156] 213. Based on the feedforward control of the front wheel angle and the feedback control of the front wheel angle compensation, the front wheel angle is determined. The front wheel angle is used to control the vehicle steering for path tracking.

[0157] The final output front wheel steering angle δ can be obtained by summing the feedforward control front wheel steering angle and the feedback control front wheel steering angle compensation. out .

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

[0159] Where p1, i1, p2, and i2 are parameters set through experiments, and the summation sign represents summing the historical errors.

[0160] The front wheel angle is the only steering control parameter. Feedforward control of the front wheel angle is the main part of this parameter. The final output is to control the vehicle's steering so that the vehicle travels along a predetermined trajectory. In this embodiment of the invention, the optimal tracking path is selected based on the Euler spiral trajectory table in order to use the rate of curvature change σ of the row where the optimal tracking path is located to calculate the feedforward control of the front wheel angle. The feedforward control of the front wheel angle obtained by this rate of curvature change σ can make the vehicle smoothly follow or approach the planned path.

[0161] Corresponding to the above method, embodiments of the present invention provide a path tracking device, such as... Figure 4 As shown, the device includes:

[0162] The aiming distance determination module 401 is used to determine the aiming distance based on 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 based on the pre-aiming distance.

[0164] The Euler spiral module 403 is used to generate an Euler spiral trajectory table in response to the fact that the lateral error and heading error of the at least two reference points do not meet the preset conditions. The trajectory points in different rows of the Euler spiral trajectory table correspond to different rates of curvature change, and the trajectory points in different columns correspond to different curvatures. The current curvature is calculated based on the steering wheel angle of the vehicle. The target column with the smallest difference from the current curvature is determined in the Euler spiral trajectory table. The starting point coordinate system of the Euler spiral is established with each trajectory point in the target column as the 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 coordinate system of the starting point of the Euler spiral; and to 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 based on the target rate of curvature change, and the front wheel steering angle is used to control the vehicle steering for path tracking.

[0167] The path tracking device of this invention can execute the path tracking method of the embodiments shown above. For parts not described in detail in the embodiments of this invention, please refer to the relevant descriptions of the method embodiments. The execution process and technical effects of this technical solution can be found in the descriptions of the method embodiments, and will not be repeated here.

[0168] See Figure 5 This is a schematic diagram of a path tracking device provided in 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 via one or more buses. Those skilled in the art will understand that the structure of the path tracking device shown in the figure does not constitute a limitation on the embodiments of this application. It may be a bus topology or a star topology, and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

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

[0170] The processor 501 serves as the control center of the path tracking device. It connects various parts of the path tracking device via various interfaces and lines. By running or executing software programs, instructions, and / or modules stored in the memory 502, and by calling data stored in the memory, it performs various functions of the path tracking device and / or processes data. The processor can be composed of integrated circuits (ICs), such as a single packaged IC or multiple packaged ICs with the same or different functions connected together. 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 storage, flash memory, magnetic disk or optical disk.

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

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

[0174] In a specific implementation, this application also provides a computer program product, wherein the computer program product includes executable instructions, which, when executed on a computer, cause the computer to perform some or all of the steps in various embodiments of the path tracing method provided in this application.

[0175] This application also provides a non-transitory computer-readable storage medium that stores computer instructions that cause the computer to execute the path tracing method provided in this application.

[0176] The aforementioned non-transitory computer-readable storage medium may be any combination of one or more computer-readable media. A computer-readable medium may be a computer-readable signal medium or a computer-readable storage medium. A computer-readable storage medium may be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of computer-readable storage media (a non-exhaustive list) include: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), or flash memory, optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this document, a computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in connection with an instruction execution system, apparatus, or device.

[0177] Those skilled in the art will clearly understand that the techniques in the embodiments of this application can be implemented using software plus necessary general-purpose hardware platforms. Based on this understanding, the technical solutions in the embodiments of this application, or the parts that contribute to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments of this application or some parts of the embodiments.

[0178] The same or similar parts between the various embodiments in this specification can be referred to mutually. In particular, the device embodiments and terminal embodiments are basically similar to the method embodiments, so the description is relatively simple, and the relevant parts can be referred to the description in the method embodiments.

Claims

1. A path tracking method characterized by, The method comprises: determining a preview distance according to the current position of the vehicle; determining at least two reference points in the path points of the planned path according to the preview distance; 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 different rows of trajectory points in the Euler spiral trajectory table correspond to different curvature change rates, and different columns of trajectory points correspond to different curvatures; calculating a current curvature according to the steering wheel angle of the vehicle; determining a target column in the Euler spiral trajectory table with which the difference value of the current curvature is the smallest; establishing an Euler spiral origin coordinate system with each trajectory point in the target column as the origin, respectively; determining the sum of tracking errors of the at least two reference points in the Euler spiral origin coordinate system; determining a target curvature change rate corresponding to the at least two reference points in the Euler spiral trajectory table when the sum of tracking errors of the at least two reference points takes the minimum value; determining a front wheel steering angle according to the target curvature change rate, wherein the front wheel steering angle is used to control the steering of the vehicle to track the path.

2. The method of claim 1, wherein: determining a preview distance according to the current position of the vehicle comprises: determining two adjacent path points closest to the current position of the vehicle in the planned path; determining the current lateral error and the heading error according to the two adjacent path points; determining the preview distance according to the current lateral error, the heading error, and a preview distance limit value.

3. The method of claim 1 or 2, wherein: the at least two reference points comprise: a first reference point farthest from the current position of the vehicle in the planned path through the preview distance; and at least one reference point between the first reference point and the current position of the vehicle.

4. The method of claim 1, wherein: generating an Euler spiral trajectory table comprises: equally dividing a preset curvature change rate interval to obtain m curvature change rate values; equally dividing a curvature interval related to the turning radius of the current vehicle to obtain n curvature values; generating a two-dimensional table of m rows and n columns with the curvature change rate values as the rows and the curvature values as the columns; determining the trajectory points at each cell position in the two-dimensional table according to the curvature change rate value and the curvature value at the cell position to obtain the Euler spiral trajectory table; wherein the trajectory points in the same row in the Euler spiral trajectory table represent an Euler spiral trajectory.

5. The method of claim 1 or 4, wherein: determining the sum of tracking errors of the at least two reference points in the Euler spiral origin coordinate system comprises: converting the trajectory points in each row of the Euler spiral trajectory table to the Euler spiral origin coordinate system of the row; performing coordinate conversion of the at least two reference points in the Euler spiral origin coordinate system of each row of the Euler spiral trajectory table; determining the sum of tracking errors of the at least two reference points in each row in the Euler spiral origin coordinate system of each row of the Euler spiral trajectory table.

6. The method of claim 5, wherein the determining the sum of tracking errors of the at least two reference points on 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 comprises: determining a nearest trajectory point of each reference point on each row among the trajectory points of each row of the Euler spiral trajectory table; determining a tracking error of each reference point on each row according to the nearest trajectory point of each reference point on each row; and summing up the tracking errors of the reference points on each row to obtain the sum of tracking errors of the at least two reference points on each row.

7. The method of claim 5, wherein the at least two reference points include a first reference point; and the determining a nearest trajectory point of each reference point on each row among the trajectory points of each row of the Euler spiral trajectory table and determining a tracking error of each reference point on each row according to the nearest trajectory point of each reference point on each row comprises: determining a nearest trajectory point of the first reference point in each row among the trajectory points of each row of the Euler spiral trajectory table; determining a first tracking error of the first reference point on each row and the nearest trajectory point of the row; determining a screening row in which the first tracking error is less than a threshold value; and calculating tracking errors of the remaining reference points on the screening rows.

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

9. The method of claim 8, wherein the determining the feedforward control front wheel steering angle according to the target curvature rate of change comprises: determining a vehicle travel distance from a last output front wheel steering angle for path tracking to a current position of the vehicle; determining a current feedforward curvature according to the vehicle travel distance and the target curvature rate of change; and determining the feedforward control front wheel steering angle according to the current feedforward curvature.

10. The device of claim 9, wherein the device comprises: a pre-look distance determination module configured to determine a pre-look distance according to a current position of the vehicle; a reference point determination module configured to determine at least two reference points among path points of a planned path according to the pre-look distance; an Euler spiral module configured to, in response to a lateral error and a heading error of the at least two reference points not satisfying a preset condition, generate an Euler spiral trajectory table, wherein trajectory points of different rows of the Euler spiral trajectory table correspond to different curvature rates of change, trajectory points of different columns correspond to different curvatures, a current curvature is calculated according to a steering wheel steering angle of the vehicle, a target column with a minimum difference value from the current curvature is determined in the Euler spiral trajectory table, and an Euler spiral starting point coordinate system is established with each trajectory point in the target column as a starting point. ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ 10. A path tracking device, characterized by, ​ ​ ​ ​ a path tracking module, configured to determine a sum of tracking errors of the at least two reference points in the coordinate system of the starting point of the Euler spiral, and determine a corresponding target curvature rate of the at least two reference points in the Euler spiral trajectory table when the sum of tracking errors of the at least two reference points takes a minimum value; a front wheel steering angle output module, configured to determine a front wheel steering angle according to the target curvature rate, and the front wheel steering angle is used to control vehicle steering to track a path.

11. A path tracking device, characterized by, comprising: 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 performs the method of any one of claims 1 to 9.

12. A computer-readable storage medium, characterized in that, The computer readable storage medium comprises a stored program, wherein the program controls the device where the computer readable storage medium is located to perform the method of any one of claims 1 to 9 when the program is running.

Images