Self-adaptive pure tracking control method and storage medium

Abstract

The invention relates to the technical field of intelligent auxiliary driving, and provides an adaptive pure tracking control method and a storage medium, on the basis of a pure tracking algorithm, a path curvature change rate, a residual distance and a vehicle state quantity (vehicle speed) are introduced, and adaptive control of different road curvatures and speed working conditions is realized by dynamically adjusting a foresight distance; and a yaw angle error compensation mechanism is further combined, a multi-preview point optimization mechanism is introduced, and a cost function is constructed according to a compensation item and a plurality of candidate corners to solve an optimal control quantity, so that the tracking precision and stability of the vehicle under a complex path condition are effectively improved.

Description

Technical Field

[0001] This invention relates to the field of intelligent assisted driving technology, and in particular to an adaptive pure tracking control method and storage medium. Background Technology

[0002] Currently, the Pure Pursuit algorithm is widely used in path tracking control methods for autonomous vehicles due to its simple structure and low computational cost. The Pure Pursuit algorithm determines the steering angle of the vehicle's front wheels through geometric relationships to achieve vehicle tracking of the target path. However, under conditions of high speed or significant curvature changes, it is prone to problems such as large path tracking errors and vehicle yaw instability. In practical applications, it still has the following shortcomings: (1) Ignore the dynamic characteristics of yaw angle error Traditional pure tracking algorithms calculate the angle between the target point and the vehicle position based solely on geometric relationships, without considering the yaw angle error variations in vehicle dynamics. Therefore, when the vehicle experiences sideslip, changes in tire lateral force, or vehicle inertial lag, the actual yaw angle often deviates significantly from the target heading angle, leading to control command lag or overshoot, thus affecting path tracking accuracy and stability.

[0003] (2) Fixed control gain, poor adaptability Existing pure tracking algorithms typically employ fixed preview distances or fixed control gains. When vehicle speed or path curvature varies significantly, these fixed parameters cannot accommodate all operating conditions. The results are: sluggish control response at low speeds, and oscillations or even tracking instability at high speeds or sharp bends, lacking the ability to adaptively adjust to complex conditions.

[0004] (3) Insufficient robustness to disturbances and nonlinear operating conditions When a vehicle is subjected to disturbances such as crosswinds, changes in road surface adhesion, or steering system lag, traditional pure tracking controllers lack compensation mechanisms, making them prone to cumulative deviations or continuous oscillations. This is especially true in high-speed or low-adhesion scenarios, where path tracking errors increase significantly.

[0005] (4) Difficult to coordinate with vehicle dynamics model While existing improvement methods (fuzzy control or MPC) have improved accuracy to some extent, they either have high computational complexity and poor real-time performance, or lack reasonable utilization of vehicle dynamics (such as yaw rate and yaw acceleration), making it difficult to balance real-time performance and accuracy. Summary of the Invention

[0006] This invention provides an adaptive pure tracking control method and storage medium, which solves the technical problems of existing pure tracking control methods in complex working conditions, such as large path deviation, lag in yaw angle response, and insufficient control stability, making it difficult to meet the requirements of autonomous driving systems for high-precision and high-robust path tracking.

[0007] To address the above technical problems, this invention provides an adaptive pure tracking control method, comprising: Obtain path information and vehicle status parameters, and then calculate curvature, rate of curvature change, and remaining distance; Construct an adaptive function model to adaptively adjust the forward sight distance based on the vehicle state variables, the rate of curvature change, and the remaining distance; The aiming point is determined based on the vehicle status quantity and the forward sight distance; Obtain the yaw angle error and calculate the rate of change of the error, and then construct a compensation term for error compensation; Based on a pure tracking algorithm, multiple pre-aiming points are introduced to calculate multiple candidate turning angles. Based on the compensation term and multiple candidate steering angles, a cost function is constructed, the optimal control quantity is solved as the target steering wheel angle, and then executed.

[0008] This basic scheme, based on the pure tracking algorithm, introduces the path curvature change rate, remaining distance, and vehicle state quantity (vehicle speed). By dynamically adjusting the forward look-ahead distance, it achieves adaptive control for different road curvature and speed conditions. Furthermore, it combines a yaw angle error compensation mechanism and introduces a multi-preview point optimization mechanism. Based on the compensation term and multiple candidate turning angles, it constructs a cost function to solve for the optimal control quantity, thereby effectively improving the tracking accuracy and stability of the vehicle under complex path conditions.

[0009] In a further implementation, path information and vehicle state variables are obtained, and then curvature, rate of change of curvature, and remaining distance are calculated, including: Obtain the path information of the planned path, and extract the vehicle pose from the obtained vehicle state variables; the vehicle state variables include the vehicle pose and vehicle speed, and the vehicle pose includes the vehicle center of gravity position and actual yaw angle. Calculate the curvature of the path segment corresponding to the current vehicle based on the path information, and calculate the rate of change of curvature; The remaining driving distance is calculated based on the vehicle status and the path information.

[0010] This solution calculates curvature, rate of change of curvature, and remaining travel distance based on path information and vehicle state variables to support dynamic adjustment of forward sight distance. By analyzing the dynamic influence of vehicle dynamics, it can improve tracking accuracy in curves and enhance stability in straight roads, achieving smoother path adaptive tracking control.

[0011] In a further implementation scheme, an adaptive function model is constructed to adaptively adjust the forward look-ahead distance based on the vehicle state variables, the rate of curvature change, and the remaining distance, including: Obtain the vehicle speed from the vehicle state variables, and calculate the first mapping function based on the vehicle speed; Calculate the second mapping function based on the rate of change of curvature; Calculate the third mapping function based on the remaining distance; An adaptive function model is constructed based on the first, second, and third mapping functions, and the forward look distance is adaptively adjusted based on the minimum forward look distance.

[0012] This scheme calculates the first, second, and third mapping functions based on vehicle speed, rate of curvature change, and remaining distance, and then constructs an adaptive function model. Combined with the minimum forward sight distance, the forward sight distance is adaptively adjusted. While ensuring the basic control stability of the vehicle at low speeds or small curvatures, the scheme achieves online dynamic optimization of control parameters, effectively balancing response speed and control smoothness, and avoiding path deviation and oscillation.

[0013] In a further implementation, determining the aiming point based on the vehicle state quantity and the forward look distance includes: Extract the vehicle centroid position from the vehicle state variables and determine the current vehicle position; Based on the vehicle position and the forward distance, the coordinates of the aiming point are determined from the path information; Based on the coordinates of the aiming point, the geometric relationship between the vehicle and the aiming point is analyzed to determine the geometric deflection angle of the aiming point relative to the vehicle.

[0014] This solution, based on the vehicle's real-time pose and dynamically adjusted forward sight distance, clearly defines the coordinates of each pre-aiming point and its geometric deflection relative to the vehicle, making path tracking more closely match the actual trajectory and improving control accuracy. Combined with the dynamic adjustment of forward sight distance to adapt to changes in vehicle speed and curvature, it helps to enhance vehicle control stability. At the same time, the pre-calculation of geometric deflection provides direct input for lateral control, effectively reducing calculation delay and improving system real-time performance and robustness.

[0015] In a further implementation, the yaw angle error is obtained and the rate of change of the error is calculated, and then a compensation term is constructed for error compensation, including: Obtain the expected heading angle from the path information; Obtain the actual yaw angle of the vehicle from the vehicle state parameters; Calculate the yaw angle error based on the expected heading angle of the path and the actual yaw angle of the vehicle. Taking into account the instantaneous rate of change and the continuous accumulation of the yaw angle error, a comprehensive analysis is performed to obtain a comprehensive error value, which is used as a compensation term for error compensation.

[0016] In a further implementation, considering the instantaneous rate of change and the continuous accumulation of the yaw angle error, a comprehensive analysis is performed to obtain a comprehensive error value, including: The vehicle speed is obtained from the vehicle state variables, and the compensation gain is calibrated by combining the yaw angle error. The corresponding cumulative amount and instantaneous rate of change are calculated based on the yaw angle error. The comprehensive error value is calculated based on the yaw angle error, instantaneous rate of change, continuous cumulative amount, and corresponding compensation gain. The formula for calculating the comprehensive error value is as follows:

[0017] In the formula, Indicates yaw angle error. Indicates the instantaneous rate of change. Indicates the cumulative amount. , , These represent the compensation gains for yaw angle error, instantaneous rate of change, and continuous cumulative amount, respectively.

[0018] This solution obtains vehicle speed from vehicle state variables and combines it with yaw angle error for compensation gain calibration, ensuring stability and rapid convergence under different operating conditions (such as high-speed straight driving or low-speed turning), significantly improving the dynamic response and stability of path tracking. Considering the instantaneous rate of change and continuous accumulation of the yaw angle error, a comprehensive analysis is performed to obtain a comprehensive error value, which is used as a compensation term for error compensation, enhancing the predictability and anti-interference capability of tracking control. The comprehensive analysis also considers offsetting dynamic lag of yaw angle and suppressing long-term deviations or low-speed drift, making the compensation more accurate and smooth.

[0019] In a further implementation, based on a pure tracking algorithm, multiple pre-aiming points are introduced, and multiple corresponding candidate turning angles are calculated, including: A set of pre-aiming points is obtained by acquiring multiple pre-aiming points; Traverse the set of preview points to obtain the geometric deflection angle and forward distance of the corresponding preview point relative to the vehicle; The candidate turning angle is calculated based on the geometric deflection angle, forward sight distance, and vehicle wheelbase.

[0020] This solution is based on a pure tracking algorithm and introduces a multi-preview point optimization mechanism. By integrating information from multiple preview points, it takes into account both real-time tracking accuracy and long-term path trends, making steering actions smoother and more predictive. This improves the stability of the vehicle when cornering at high speeds and the robustness of overall tracking, significantly enhancing the adaptability of traditional pure tracking algorithms to different environments and paths.

[0021] In a further implementation, a cost function is constructed based on the compensation term and the plurality of candidate steering angles, and the optimal control quantity is solved as the target steering wheel angle and executed, including: A cost function is constructed based on multiple candidate steering angles, and the compensation term is incorporated into the cost function in a weighted form. The cost function is differentiated with respect to the front wheel steering angle and its value is set to 0. The obtained front wheel steering angle is used as the optimal control quantity. Then, the target steering wheel angle is set and executed based on the optimal control quantity. The cost function after incorporating the compensation term is as follows:

[0022] In the formula, This represents the error weighting coefficient. This represents the optimal control quantity. When representing small angles The value, This represents the candidate turning angle for the i-th aiming point. Indicates compensation item, This represents the compensation coefficient.

[0023] This scheme introduces a multi-preview point optimization mechanism and incorporates the compensation terms into the cost function as weights to calculate the optimal control quantity. The multi-preview point mechanism provides path trend prediction, making the control forward-looking. By incorporating the dynamic error compensation term as a weight into the cost function, deviations caused by model mismatch or disturbances can be corrected in real time. Collaborative optimization, while taking into account global smoothness, provides precise and flexible compensation for local errors, thereby improving the adaptive capability under complex operating conditions.

[0024] Further implementation methods also include: Obtain the vehicle speed, rate of curvature change, and remaining distance corresponding to each pre-aiming point, and calculate the error weight coefficient corresponding to each pre-aiming point based on one or more of the vehicle speed, rate of curvature change, and remaining distance.

[0025] This scheme calculates error weighting coefficients based on the path characteristic information represented by vehicle speed, rate of curvature change, and remaining distance at each prediction point, determines the contribution of each prediction point to the final control, and automatically increases the weight of prediction points in key areas such as curves and sudden curvature changes, highlighting path difficulties to improve control sensitivity.

[0026] The present invention also provides a storage medium storing computer-readable instructions thereon, which, when executed by a computer's processor, cause the computer to perform the adaptive pure tracking control method for yaw angle error compensation as described above. Attached Figure Description

[0027] Figure 1This is the workflow of an adaptive pure tracking control method provided in an embodiment of the present invention; Figure 2 This is a reference diagram for parameter settings provided in an embodiment of the present invention. Detailed Implementation

[0028] The embodiments of the present invention are described in detail below with reference to the accompanying drawings. The embodiments are given for illustrative purposes only and should not be construed as limiting the present invention. The accompanying drawings are for reference and illustration only and do not constitute a limitation on the scope of patent protection of the present invention, because many changes can be made to the present invention without departing from the spirit and scope of the present invention.

[0029] Example 1 An adaptive pure tracking control method provided in this embodiment of the invention, such as... Figure 1 , Figure 2 As shown, in this embodiment, the steps include: S1. Obtain path information and vehicle state variables, and then calculate curvature, rate of change of curvature, and remaining distance, including: S11. Obtain the path information of the planned path, and extract the vehicle pose from the obtained vehicle state variables; the vehicle state variables include the vehicle pose and vehicle speed, and the vehicle pose includes the vehicle center of gravity position and the actual yaw angle. Specifically: (1) Obtain the target path consisting of a series of discrete points output by the vehicle planning system. The path point sequence is denoted as:

[0030] In the formula, ( () represents the coordinates of the path point. This is the expected heading angle for the path. Path information includes the coordinates of path points and the expected heading angle.

[0031] (2) Obtain the current vehicle status data provided by the vehicle real-time positioning system:

[0032] In the formula, The location of the vehicle's center of gravity. v is the vehicle's actual yaw angle and v is the vehicle speed.

[0033] The vehicle real-time positioning system includes a GNSS / IMU fusion module.

[0034] S12. Calculate the curvature of the path segment corresponding to the current vehicle based on the path information, and calculate the rate of change of curvature as follows; (1) Calculate the curvature of the path segment corresponding to the current vehicle based on the path point sequence. :

[0035] Where s is the path arc length.

[0036] Detailed calculation steps: Calculate the tangent direction (heading angle) at each point:

[0037] Note: For the last point, you can use the heading angle of the previous point or handle it specially.

[0038] Calculate the cumulative arc length from the starting point to each point:

[0039]

[0040] For point

[0041]

[0042] For the finish line

[0043]

[0044] (2) Further calculate the rate of change of curvature:

[0045] The rate of change of curvature reflects the trend of the curvature of the path curve and is used for subsequent adaptive adjustment.

[0046] S13. Calculate the remaining driving distance based on the vehicle status quantity and the path information.

[0047] Specifically, the vehicle's current position (i.e., the vehicle's centroid position) is obtained from the vehicle state variables and matched with the path information to drive the vehicle to the nearest projection point on the planned path. Calculate the remaining driving distance:

[0048] The remaining driving distance describes the path length from the current destination and is used to adjust the forward sight distance.

[0049] This embodiment calculates curvature, rate of curvature change, and remaining travel distance based on path information and vehicle state variables to support dynamic adjustment of forward sight distance. By analyzing the dynamic influence of vehicle dynamics, it can improve tracking accuracy in curves and enhance stability in straight roads, achieving smoother path adaptive tracking control.

[0050] S2. Construct an adaptive function model to adaptively adjust the forward sight distance based on the vehicle speed, rate of change of curvature, and remaining distance, including: S21. Calculate the mapping function: (1) Calculate the first mapping function based on the vehicle speed, and the calculation formula is as follows;

[0051] (2) Calculate the second mapping function based on the curvature change rate, and the calculation formula is as follows;

[0052] (3) Calculate the third mapping function based on the remaining distance, and the calculation formula is as follows;

[0053] In the formula, the parameters , , , It is a sensitivity adjustment factor used to balance response speed and control smoothness.

[0054] When the vehicle speed When it increases, To improve ride comfort, the forward sight distance is made to increase linearly or exponentially with speed; when the path curvature changes at a certain rate... When it increases, Reduce the forward sight distance so that the controller can respond to curve changes in advance; when the remaining distance When decreasing, Reducing the forward sight distance facilitates precise docking or stopping at the end point.

[0055] Ultimately, the dynamic range of the forward look distance can be limited to:

[0056] The typical calibration range for the maximum forward sight distance is as follows: Low-speed scenarios (v<2m / s): =1.5 2.5m Medium speed scenario (v=2–5m / s): =2.5 4.0m High-speed scenarios (v>5m / s): =4.0 5.5m S22. Construct an adaptive function model based on the first mapping function, the second mapping function, and the third mapping function. Adaptively adjust the forward look distance based on the minimum forward look distance. The calculation formula of the adaptive function model is as follows:

[0057] In the formula: The minimum forward sight distance is used to ensure the basic control stability of the vehicle at low speeds or with small curvatures. This is the first mapping function, reflecting the effect of vehicle speed on forward sight distance; The second mapping function describes the effect of road curvature changes on control sensitivity; This is the third mapping function, used to shorten the predicted distance during the terminal deceleration phase; , , These are the adjustment coefficients for the first, second, and third mapping functions, respectively, obtained through simulation or real vehicle calibration.

[0058] S23. Perform periodic updates of the forward look distance based on historical data.

[0059] Specifically, to ensure the real-time performance and stability of the algorithm, the look-ahead distance is updated in real time every control cycle (e.g., 20ms) based on the current state. The update strategy is as follows:

[0060] in, Indicates the forward sight distance at the next moment. Indicates the forward sight distance at the current moment. This indicates that the forward sight distance at the next moment is calculated based on the adaptive function model. It is a smoothing factor used to prevent frequent oscillations.

[0061] This embodiment calculates the first, second, and third mapping functions based on vehicle speed, rate of curvature change, and remaining distance, and then constructs an adaptive function model. Combined with the minimum forward sight distance, the forward sight distance is adaptively adjusted. While ensuring the basic control stability of the vehicle at low speeds or small curvatures, the control parameters are dynamically optimized online, effectively balancing response speed and control smoothness, avoiding path deviation and oscillation, and achieving adaptive adjustment capability for complex working conditions.

[0062] S3. Determine the aiming point based on the vehicle status quantity and the forward sight distance, including: Extract the vehicle centroid position from the vehicle state variables and determine the current vehicle position; Based on the vehicle position and the forward distance, the coordinates of the aiming point are determined from the path information; Based on the coordinates of the aiming point, the geometric relationship between the vehicle and the aiming point is analyzed to determine the geometric deflection angle of the aiming point relative to the vehicle.

[0063] Specifically, based on the current vehicle position and the adaptively obtained forward-looking distance... Determine the distance from the vehicle on the path. Pre-aiming point:

[0064] Calculate the geometric deflection angle between the vehicle and the aiming point:

[0065] in, It is the angle (i.e., geometric deflection angle) between the direction of the vehicle's front wheels and the line connecting the aiming point. This is the vehicle's actual yaw angle.

[0066] This embodiment, based on the vehicle's real-time pose and dynamically adjusted forward sight distance, clarifies the coordinates of each pre-aiming point and its geometric deflection relative to the vehicle, making path tracking more closely match the actual trajectory and improving control accuracy. Combined with the dynamic adjustment of forward sight distance to adapt to changes in vehicle speed and curvature, it helps to enhance vehicle control stability. At the same time, the pre-calculated geometric deflection provides direct input for lateral control, effectively reducing calculation delay and balancing real-time performance and accuracy.

[0067] S4. Obtain the yaw angle error and calculate the rate of change of the error, and then construct a compensation term for error compensation, including: S41. Obtain the expected heading angle of the path from the path information; S42. Obtain the actual yaw angle of the vehicle from the vehicle status variables; S43. Calculate the yaw angle error based on the expected heading angle of the described path and the actual yaw angle of the vehicle. The calculation formula is as follows:

[0068] In the formula, Actual vehicle yaw angle , This represents the expected heading angle for the path.

[0069] S44. Considering the instantaneous rate of change and the continuous accumulation of the yaw angle error, a comprehensive analysis is performed to obtain a comprehensive error value, which is used as a compensation item for error compensation.

[0070] In this embodiment, considering both the instantaneous rate of change and the cumulative amount of the yaw angle error, a comprehensive analysis is performed to obtain the comprehensive error value, including: The vehicle speed is obtained from the vehicle state variables, and the compensation gain is calibrated by combining the yaw angle error. The corresponding cumulative amount and instantaneous rate of change are calculated based on the yaw angle error. The comprehensive error value is calculated based on the yaw angle error, instantaneous rate of change, continuous cumulative amount, and corresponding compensation gain. The formula for calculating the comprehensive error value is as follows:

[0071] In the formula, Indicates yaw angle error. Indicates the instantaneous rate of change. Indicates the cumulative amount. , , These represent the compensation gains for yaw angle error, instantaneous rate of change, and continuous cumulative amount, respectively.

[0072] The instantaneous rate of change is obtained by calculating the first derivative of the yaw angle error. ;

[0073] instantaneous rate of change This reflects the dynamic trend of vehicle yaw angle response being delayed or advanced.

[0074] Compensation gain Used to correct instantaneous yaw error; compensation gain Used to compensate for dynamic hysteresis of yaw angle; compensate for gain. Used to suppress long-term deviations or low-speed drift. The compensation logic is as follows: when >0: This indicates that the vehicle's heading is lagging behind the target path, and the controller increases the steering input; when <0: indicates that the vehicle's heading has overshooted, and the controller reduces steering commands; When | | Maintaining non-zero values ​​for extended periods: The integral term gradually increases, achieving slow drift correction.

[0075] To cope with different speed and curvature conditions, compensation gain is required. , , Adaptive adjustment:

[0076]

[0077]

[0078] in, , , The preset initial adjustment value, , The speed sensitivity parameter is represented by the 'sat' function, which indicates amplitude limiting control.

[0079] The SAT function is a piecewise linear function designed to limit the amplitude of an input signal within a certain range. Its definition is as follows:

[0080] in, Indicating yaw angle error Threshold.

[0081] This mechanism allows the system to increase the proportional response (improving sensitivity) at low speeds and decrease the differential gain (avoiding oscillations) at high speeds.

[0082] This embodiment obtains vehicle speed from vehicle state variables and combines it with yaw angle error for compensation gain calibration, ensuring stability and rapid convergence under different operating conditions (such as high-speed straight driving or low-speed turning), significantly improving the dynamic response and stability of path tracking. Considering the instantaneous rate of change and continuous accumulation of the yaw angle error, a comprehensive analysis is performed to obtain a comprehensive error value, which is used as a compensation term for error compensation, enhancing the predictability and anti-interference capability of tracking control. The comprehensive analysis also considers offsetting dynamic lag of yaw angle and suppressing long-term deviation or low-speed drift, making the compensation more accurate and smooth.

[0083] S5. Based on a pure tracking algorithm, multiple pre-aiming points are introduced, and multiple candidate turning angles are calculated accordingly, including: A set of pre-aiming points is obtained by acquiring multiple pre-aiming points; Traverse the set of preview points to obtain the geometric deflection angle and forward distance of the corresponding preview point relative to the vehicle; The candidate turning angle is calculated based on the geometric deflection angle, forward sight distance, and vehicle wheelbase.

[0084] Specifically, traditional pure tracking algorithms calculate control inputs based on only a single look-ahead point, failing to fully reflect the geometric changes of the path over a wider range. This can easily lead to control lag or oversteering under conditions of significant curvature variations or complex paths. Therefore, this embodiment introduces a multi-look-ahead point optimization mechanism, continuously utilizing the aforementioned look-ahead distance formula along the path. Multiple aiming points are calculated (the next aiming point is calculated based on the previous aiming point), or multiple aiming points are selected at equal intervals. The optimal control quantity is calculated by combining the geometric information of multiple aiming points, thus balancing local accuracy and overall smoothness, as follows: Assuming the planned path is uniformly distributed with points at arc lengths / distances from the start to the end (or the path is discretized), a series of pre-aiming points are taken along the path from the vehicle's current position at regular intervals (here, 2m) until the end point, resulting in a set of pre-aiming points (in vector form):

[0085] in The nearest aiming point (approximately 2 meters from the vehicle). It could be the endpoint or the furthest forward aiming point; N is determined by the path length and the 2m interval.

[0086] The wheelbase L is known, and the vehicle status (from positioning / IMU) is as follows:

[0087] Define each aiming point Relative vehicle geometric deflection (the difference between the line connecting the vehicle's current position and the aiming point and the vehicle's heading):

[0088] Each pre-aiming point The corresponding forward sight distance (geometric distance):

[0089] The "ideal turning angle" given by traditional single-point pure tracking is (purely geometric) as follows:

[0090] This embodiment provides a "candidate turning angle" for each pre-aiming point. Thus, a single instruction can be found. (Optimal control value, the actual front wheel steering angle issued by the vehicle), making it optimal in the sense of multiple aiming points (combining local accuracy and long-term trend).

[0091] To facilitate analysis and engineering implementation, we first perform common small-angle linearization:

[0092] but,

[0093] In the formula, When representing small angles The value of .

[0094] This embodiment is based on a pure tracking algorithm and introduces a multi-preview point optimization mechanism. By integrating information from multiple preview points, it takes into account both real-time tracking accuracy and long-term path trends, making steering actions smoother and more predictive. This improves the stability of the vehicle when cornering at high speeds and the robustness of overall tracking, significantly enhancing the adaptability of traditional pure tracking algorithms to different environments and paths.

[0095] S6. Construct a cost function based on the compensation term and multiple candidate steering angles, solve for the optimal control quantity as the target steering wheel angle, and execute it, including: S61. Obtain the vehicle state quantity, curvature change rate and remaining distance corresponding to each of the pre-aiming points, and calculate the weight based on one or more of the vehicle state quantity, curvature change rate and remaining distance to obtain the error weight coefficient corresponding to each of the pre-aiming points.

[0096] Weight The contribution of each prediction point to the final control can be determined as follows: 1. Calculate the original weights: Based on the design objectives, calculate the original weights ui for each target point Pi.

[0097] (1) Distance attenuation type (emphasizing near point):

[0098] (2) Curvature / rate of change sensitive type (the curvature or rate of change of curvature of the current aiming point is large, the near point weight should be increased to improve sensitivity):

[0099] (3) Speed / Scene Adaptive Type (At high speed, focus on the far point; at low speed, focus on the near point). :

[0100] when As ω increases, the weight decays more slowly with distance (more emphasis is placed on distant points). Indicates the distance decay weighting index; This represents the curvature / rate of change sensitivity coefficient.

[0101] Adaptive selection based on actual road conditions , , Any one of the terms in the equation is used as the error weighting coefficient.

[0102] 2. Combine using weighted summation, the formula is:

[0103] in .

[0104] Weight normalization yields the error weight coefficients:

[0105] This embodiment calculates the error weighting coefficient based on the path feature information represented by the vehicle speed, rate of curvature change, and remaining distance at each prediction point, determines the contribution of each prediction point to the final control, automatically increases the weight of prediction points in key areas such as curves and sudden curvature changes, highlights path difficulties, and improves control sensitivity.

[0106] S62. Construct a cost function based on the multiple candidate steering angles, and incorporate the compensation term into the cost function in the form of weights. Take the derivative of the cost function with respect to the front wheel steering angle and set its value to 0. Use the obtained front wheel steering angle as the optimal control quantity, and then set the target steering wheel angle based on the optimal control quantity and execute it.

[0107] Specifically, a cost function is constructed based on the multiple candidate corners, as follows:

[0108] In the formula, It is the weight of each pre-aiming point.

[0109] Constructing the cost function right Taking the derivative and setting it to 0, we get The solution is the optimal control quantity. .

[0110]

[0111] Solving for:

[0112] visible, yes The weighted average has a very small computational load and can be easily implemented in real time on automotive-grade MCUs / ECUs.

[0113] Will Substituting, we get:

[0114] At this point, considering the yaw angle compensation... By incorporating the weights into the cost function, we obtain:

[0115] In the formula, This represents the error weighting coefficient. This represents the optimal control quantity. When representing small angles The value, This represents the candidate turning angle for the i-th aiming point. Indicates compensation item, This represents the compensation coefficient.

[0116] Solve the problem, same analysis as above:

[0117] get:

[0118] visible, The closed-form solution is a weighted fusion of "multi-preview point weighted average" and "yaw compensation," which is easy to implement and can be used to... As a design and adjustment parameter.

[0119] This embodiment introduces a multi-preview point optimization mechanism and incorporates the compensation terms into the cost function as weights to calculate the optimal control quantity. The multi-preview point mechanism provides path trend prediction, making the control forward-looking. By incorporating the dynamic error compensation term as a weight into the cost function, deviations caused by model mismatch or disturbances can be corrected in real time. Collaborative optimization, while taking into account global smoothness, provides precise and flexible compensation for local errors, thereby improving the adaptive capability under complex operating conditions.

[0120] S7. Real-time detection of error change trends, and execution of closed-loop feedback and adaptive correction.

[0121] A real-time monitoring mechanism is established based on lateral error, yaw angle error, and error change rate. When an abnormal error trend is detected (such as oscillation or overshoot), the compensation coefficient is automatically adjusted. and In addition, adaptive mapping function parameters are used to form a closed-loop self-learning control mechanism, enabling the algorithm to continuously optimize under different road curvature, speed and disturbance conditions.

[0122] See Figure 2 After setting the parameters, the calculation logic for the optimal control quantity of the control method in this embodiment is as follows: In the current cycle, a circular arc path is first given, with the circle point at (0,10) and a radius of 10m. Starting from point (0,0), there are 30 path points at counterclockwise intervals of 10cm, and the path length is 3m. The vehicle state variables are {0,0,0,1}, with curvatures of 0.1 and a curvature change rate of 0. The remaining distance decreases sequentially from 2.9m to 0.

[0123] 1. Calculate the forward sight distance set up It is 0.5. , Both are 1. =0, =1, , , If both are 1, then,

[0124]

[0125]

[0126]

[0127] At this point, the forward sight distance is calculated to be 1.85m.

[0128] The pre-aiming points are obtained as {1.7993, 0.0324, 0.1800}, and the pre-aiming points... The corresponding forward sight distance (geometric distance) =1.85.

[0129] II. Determine the aiming point Again Starting from point A, calculate the second aiming point. and Same as above.

[0130]

[0131] Get the aiming point The values ​​{2.8971, 0.0841, 0.2900} correspond to the forward sight distance (geometric distance). =1.1.

[0132] III. Calculation of Error Weighting Coefficients Assume the wheelbase L = 2.78. =1, =1, =0, =0, , ,but, Calculate the error weighting coefficient for the first aiming point:

[0133]

[0134] set up , ,but,

[0135]

[0136] The results of integration are as follows:

[0137] Calculate the error weighting coefficient for the second aiming point:

[0138]

[0139]

[0140] The results of integration are as follows:

[0141] Finally, normalization yields:

[0142]

[0143] IV. Calculate the compensation gain and compensation term set up , , ,

[0144]

[0145]

[0146]

[0147] set up If it is 1.57, then,

[0148]

[0149]

[0150] V. Calculate the optimal control quantity

[0151]

[0152]

[0153]

[0154] set up =1, then,

[0155] In summary, the optimal control quantity can be obtained. = .

[0156] Thus, a comparison is made in a real-world scenario: an S-curve road with gradually increasing curvature, with a speed range of 3-5 km / h.

[0157]

[0158] Compared with traditional pure tracking algorithms, the maximum lateral error, average lateral error, maximum yaw angle error, and steering overshoot of this embodiment are significantly reduced, and the number of closed-loop oscillations and settling time are also greatly reduced, thereby effectively improving the tracking accuracy and stability of the tracking control.

[0159] This invention, based on a pure tracking algorithm, introduces path curvature change rate, remaining distance, and vehicle speed. By dynamically adjusting the forward look-ahead distance, it achieves adaptive control for different road curvature and speed conditions. Furthermore, it combines a yaw angle error compensation mechanism and introduces a multi-preview point optimization mechanism. Based on the compensation term and multiple candidate turning angles, it constructs a cost function to solve for the optimal control quantity, thereby effectively improving the tracking accuracy and stability of the vehicle under complex path conditions.

[0160] Example 2 This invention also provides a storage medium storing computer-readable instructions. When these instructions are executed by a computer's processor, the computer performs the adaptive pure tracking control method for yaw angle error compensation as described in Embodiment 1 above. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0161] The embodiments of the present invention have the following beneficial effects: 1) Dynamic compensation for yaw angle error: effectively suppresses heading deviation caused by vehicle steering lag; 2) Improved path tracking accuracy: Lateral and heading errors are significantly reduced; 3) Adaptive gain adjustment mechanism: The control sensitivity can be automatically adjusted according to the curvature change.

[0162] 4) Simple calculation and high real-time performance: facilitates real-time operation of embedded controllers.

[0163] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. An adaptive pure tracking control method, characterized in that, include: Obtain path information and vehicle status parameters, and then calculate curvature, rate of curvature change, and remaining distance; Construct an adaptive function model to adaptively adjust the forward sight distance based on the vehicle state variables, the rate of curvature change, and the remaining distance; The aiming point is determined based on the vehicle status quantity and the forward sight distance; Obtain the yaw angle error and calculate the rate of change of the error, and then construct a compensation term for error compensation; Based on a pure tracking algorithm, multiple pre-aiming points are introduced to calculate multiple candidate turning angles. Based on the compensation term and multiple candidate steering angles, a cost function is constructed, the optimal control quantity is solved as the target steering wheel angle, and then executed.

2. The adaptive pure tracking control method as described in claim 1, characterized in that, Obtain path information and vehicle state variables, and then calculate curvature, rate of change of curvature, and remaining distance, including: Obtain the path information of the planned path, and extract the vehicle pose from the obtained vehicle state variables; the vehicle state variables include the vehicle pose and vehicle speed, and the vehicle pose includes the vehicle center of gravity position and actual yaw angle. Calculate the curvature of the path segment corresponding to the current vehicle based on the path information, and calculate the rate of change of curvature; The remaining driving distance is calculated based on the vehicle's position and the path information.

3. The adaptive pure tracking control method as described in claim 2, characterized in that, Constructing an adaptive function model to adaptively adjust the forward look-ahead distance based on the vehicle state variables, the rate of curvature change, and the remaining distance includes: Obtain the vehicle speed from the vehicle state variables, and calculate the first mapping function based on the vehicle speed; Calculate the second mapping function based on the rate of change of curvature; Calculate the third mapping function based on the remaining distance; An adaptive function model is constructed based on the first, second, and third mapping functions, and the forward look distance is adaptively adjusted based on the minimum forward look distance.

4. The adaptive pure tracking control method as described in claim 1, characterized in that, Determining the aiming point based on the vehicle status parameters and the forward sight distance includes: Extract the vehicle centroid position from the vehicle state variables and determine the current vehicle position; Based on the vehicle position and the forward distance, the coordinates of the aiming point are determined from the path information; Based on the coordinates of the aiming point, the geometric relationship between the vehicle and the aiming point is analyzed to determine the geometric deflection angle of the aiming point relative to the vehicle.

5. The adaptive pure tracking control method as described in claim 1, characterized in that, Obtain the yaw angle error and calculate the rate of change of the error, and then construct a compensation term for error compensation, including: Obtain the expected heading angle from the path information; Obtain the actual yaw angle of the vehicle from the vehicle state parameters; Calculate the yaw angle error based on the expected heading angle of the path and the actual yaw angle of the vehicle. Taking into account the instantaneous rate of change and the continuous accumulation of the yaw angle error, a comprehensive analysis is performed to obtain a comprehensive error value, which is used as a compensation term for error compensation.

6. The adaptive pure tracking control method as described in claim 5, characterized in that, Taking into account the instantaneous rate of change and the continuous accumulation of the yaw angle error, a comprehensive analysis is performed to obtain the comprehensive error value, including: The vehicle speed is obtained from the vehicle state variables, and the compensation gain is calibrated by combining the yaw angle error. The corresponding cumulative amount and instantaneous rate of change are calculated based on the yaw angle error. The comprehensive error value is calculated based on the yaw angle error, instantaneous rate of change, continuous cumulative amount, and corresponding compensation gain. The formula for calculating the comprehensive error value is as follows: In the formula, Indicates yaw angle error. Indicates the instantaneous rate of change. Indicates the cumulative amount. , , These represent the compensation gains for yaw angle error, instantaneous rate of change, and continuous cumulative amount, respectively.

7. The adaptive pure tracking control method as described in claim 1, characterized in that, Based on a pure tracking algorithm, multiple pre-aiming points are introduced, and multiple candidate turning angles are calculated, including: A set of pre-aiming points is obtained by acquiring multiple pre-aiming points; Traverse the set of preview points to obtain the geometric deflection angle and forward distance of the corresponding preview point relative to the vehicle; The candidate turning angle is calculated based on the geometric deflection angle, forward sight distance, and vehicle wheelbase.

8. The adaptive pure tracking control method as described in claim 2, characterized in that, Based on the compensation term and multiple candidate steering angles, a cost function is constructed, the optimal control quantity is solved as the target steering wheel angle, and the process is executed, including: A cost function is constructed based on multiple candidate steering angles, and the compensation term is incorporated into the cost function in a weighted form. The cost function is differentiated with respect to the front wheel steering angle and its value is set to 0. The obtained front wheel steering angle is used as the optimal control quantity. Then, the target steering wheel angle is set and executed based on the optimal control quantity. The cost function after incorporating the compensation term is as follows: In the formula, This represents the error weighting coefficient. This represents the optimal control quantity. When representing small angles The value, This represents the candidate turning angle for the i-th aiming point. Indicates compensation item, This represents the compensation coefficient.

9. The adaptive pure tracking control method as described in claim 8, characterized in that, Also includes: Obtain the vehicle speed, rate of curvature change, and remaining distance corresponding to each pre-aiming point, and calculate the error weight coefficient corresponding to each pre-aiming point based on one or more of the vehicle speed, rate of curvature change, and remaining distance.

10. A storage medium, characterized in that, It stores computer-readable instructions, which, when executed by the computer's processor, cause the computer to perform an adaptive pure tracking control method for yaw angle error compensation as described in any one of claims 1 to 9.

Images