Vehicle trajectory tracking control method and system, unmanned vehicle and readable medium

Abstract

The invention provides a vehicle trajectory tracking control method and system, an unmanned vehicle and a readable medium, and belongs to the field of automatic driving. The vehicle trajectory tracking control method comprises the steps that the longitudinal speed, the steering state and the reference trajectory of a vehicle are obtained; constructing an error model of the vehicle relative to the reference trajectory, wherein the error model at least represents a transverse error and a course error of the vehicle relative to the reference trajectory; performing nonlinear optimization on the error model by using an iterative linear quadratic regulator algorithm, and generating a control input sequence for vehicle transverse control in a prediction time domain; and mapping the control input sequence into a steering control signal of the vehicle, so that the vehicle adjusts a driving track based on the steering control signal. According to the vehicle trajectory tracking control method and system, the unmanned vehicle and the readable medium provided by the invention, the dependence on kinetic parameters difficult to identify can be weakened, the nonlinear precision and the vehicle-mounted real-time performance are considered, and the trajectory tracking precision and convergence are improved.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving technology, specifically relating to a vehicle trajectory tracking and control method, system, unmanned vehicle, and readable medium. Background Technology

[0002] Vehicle trajectory tracking control is a fundamental component of autonomous driving systems. Its goal is to ensure that the vehicle follows a preset reference trajectory as closely as possible under complex and time-varying road and operating conditions, thereby guaranteeing safety and efficiency. Common trajectory tracking methods in existing engineering projects include proportional-integral-derivative (PID) control, linear quadratic regulator (LQR) control, and model predictive control (MPC). PID methods are simple to implement and have intuitive parameter tuning, but when faced with strong nonlinear coupling between the vehicle's lateral and heading directions, and significant changes in tire sideslip characteristics with operating conditions, the static linear gain struggles to maintain performance across the entire range. Furthermore, the integral term is prone to integral saturation and recovery oscillations under saturation and amplitude-limiting conditions, leading to slow convergence of tracking errors or even unstable oscillations.

[0003] In engineering practice, LQR and MPC typically rely on linearized descriptions or approximations of vehicle models. Limited by assumptions such as small angles, low sideslip conditions, and simplified model structures, these methods are prone to model mismatch in scenarios with low adhesion (wet / slippery / gravel), high-curvature curves, rapid speed changes, or sideslip: the vehicle model used within the controller differs significantly from the actual vehicle dynamics, reducing the effectiveness of the control law for the real system. Specifically, this manifests as difficulty in compressing tracking errors, poor algorithm convergence, and the vehicle exhibiting serpentine / dragon-like swaying. Summary of the Invention

[0004] The purpose of this disclosure is to provide a vehicle trajectory tracking control method, system, unmanned vehicle, and readable medium that can improve the accuracy of vehicle trajectory tracking control.

[0005] To achieve the above objectives, the technical solution provided in this disclosure is as follows:

[0006] In a first aspect, this disclosure provides a vehicle trajectory tracking control method, which includes:

[0007] The system acquires the vehicle's longitudinal speed, steering state, and reference trajectory. Based on these parameters, it constructs an error model of the vehicle relative to the reference trajectory. This error model at least represents the vehicle's lateral position error, lateral speed error, heading angle error, and heading angular velocity error relative to the reference trajectory. An iterative linear quadratic regulator algorithm is used to perform nonlinear optimization on the error model, generating a control input sequence for lateral control of the vehicle in the prediction time domain. The control input sequence is then mapped to the vehicle's steering control signal, enabling the vehicle to adjust its driving trajectory based on the steering control signal.

[0008] In one or more embodiments, the lateral position error, lateral velocity error, heading angle error, and heading angular velocity error change in conjunction with the vehicle's longitudinal velocity and the curvature of the reference trajectory.

[0009] In one or more embodiments, the error discrete update of the error model satisfies the following relationship:

[0010]

[0011]

[0012]

[0013] ;

[0014] in, Indicates lateral error. Indicates lateral velocity error. Indicates heading error. Indicates the error in heading angular velocity. Indicates the sampling time step. Indicates the sampling point at the current time. Indicates the sampling point at the next moment. Indicates the longitudinal speed of the vehicle. Indicates the vehicle's turning radius. This represents the curvature of the reference trajectory at the aiming point. Indicates the vehicle's wheelbase. This indicates the steering angle of the vehicle's front wheels.

[0015] In one or more embodiments, the iterative linear quadratic regulator algorithm performs nonlinear optimization on the error model, specifically including: at the nominal points of each sampling time of the current nominal trajectory, performing a first-order Taylor expansion of the error model to obtain a time-varying linear approximation, wherein the nominal trajectory is the internal baseline trajectory used for linearization and cost approximation in the current iteration, and the nominal point is the expansion anchor point of the nominal trajectory at a certain sampling time; at the nominal point, performing a second-order Taylor approximation on the cost function of state deviation and control deviation to obtain a quadratic form; using backward dynamic programming to calculate the state feedback gain and feedforward correction term, and updating the nominal trajectory and control input sequence through forward rolling simulation until the total cost converges.

[0016] In one or more embodiments, the cost function includes at least: a state deviation term for penalizing lateral deviation and heading deviation, and a control cost term for penalizing the change in control input magnitude and adjacent time intervals.

[0017] In one or more embodiments, the nonlinear optimization includes: handling the inequality constraints in a soft constraint manner, and incorporating at least one of the following into the objective function through a continuously differentiable penalty function: steering angle magnitude, steering angle change rate, curvature and curvature change rate, lateral deviation threshold and heading angular velocity threshold.

[0018] In one or more embodiments, the control input sequence is represented by curvature or an equivalent steering amount of curvature.

[0019] Secondly, this disclosure provides a vehicle trajectory tracking and control system, which includes:

[0020] The system includes an acquisition module for acquiring the vehicle's longitudinal speed, steering state, and reference trajectory; an error modeling module for constructing an error model of the vehicle relative to the reference trajectory based on the longitudinal speed, steering state, and reference trajectory, wherein the error model at least characterizes the vehicle's lateral position error, lateral speed error, heading angle error, and heading angular velocity error relative to the reference trajectory; an optimization module for performing nonlinear optimization of the error model using an iterative linear quadratic regulator algorithm to generate a control input sequence for lateral control of the vehicle in the prediction time domain; and a control module for mapping the control input sequence to a steering control signal for the vehicle, enabling the vehicle to adjust its driving trajectory based on the steering control signal.

[0021] Thirdly, this disclosure provides an unmanned vehicle, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the vehicle trajectory tracking control method as described above.

[0022] Fourthly, this disclosure provides a computer-readable medium carrying computer-executable instructions, which, when executed by a processor, are used to implement the vehicle trajectory tracking control method as described above.

[0023] The vehicle trajectory tracking control method, system, unmanned vehicle, and readable medium disclosed herein construct a kinematic error model based on longitudinal velocity, steering state, and reference trajectory, including lateral position, lateral velocity error, heading angle error, and heading angular velocity error, thus reducing the dependence on difficult-to-identify dynamic parameters. Then, iLQR is used to perform iterative linearization and quadratic approximation in the finite prediction time domain to calculate the control input sequence, balancing nonlinear accuracy and on-board real-time performance, thereby improving trajectory tracking accuracy and convergence. Attached Figure Description

[0024] To more clearly illustrate the technical solutions in the embodiments of this disclosure or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments recorded in this disclosure. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0025] Figure 1 This is a flowchart of a vehicle trajectory tracking control method in one embodiment of the present disclosure;

[0026] Figure 2 This is a test image of lateral position error obtained by trajectory tracking control in one embodiment of this disclosure;

[0027] Figure 3 This is a test diagram of heading angle error obtained by trajectory tracking control in one embodiment of this disclosure;

[0028] Figure 4 This is a structural block diagram of a vehicle trajectory tracking control system according to an embodiment of the present disclosure;

[0029] Figure 5 This is a structural block diagram of an unmanned vehicle according to one embodiment of the present disclosure. Detailed Implementation

[0030] To enable those skilled in the art to better understand the technical solutions in this disclosure, the technical solutions in the embodiments of this disclosure will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this disclosure, and not all of them. Based on the embodiments of this disclosure, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this disclosure.

[0031] With the rapid development of autonomous driving technology, the core objective has become ensuring that vehicles travel safely, smoothly, and accurately along a preset trajectory in complex road environments. However, existing trajectory tracking control methods still have significant shortcomings. While PID control is simple in structure, it struggles to balance stability and accuracy under nonlinear conditions, and the integral term may even lead to oscillations and instability. Methods such as LQR and MPC rely on linearized vehicle models, but vehicles exhibit significant nonlinearity and time-varying parameter characteristics under different adhesion conditions, curves with different curvatures, or high-speed conditions. This results in a large difference between the linear model and the real vehicle, leading to insufficient tracking accuracy, difficulty in algorithm convergence, and even trajectory divergence. Furthermore, the vehicle dynamics parameters are difficult to accurately identify due to environmental and load influences, posing challenges to traditional methods relying on precise parameter modeling in engineering applications. On the other hand, the handling of constraints (such as steering amplitude and rate of change, lateral deviation range, etc.) is also inadequate in existing methods. Either constraints are ignored, leading to vehicle operational risks, or explicit constraints result in excessive computational complexity and insufficient real-time performance.

[0032] To address the aforementioned issues, this disclosure proposes a novel approach: instead of relying on traditional dynamic models, it constructs an error description model that reflects the evolution of lateral and heading states, starting from the error relationship between the vehicle and the reference trajectory. This shifts vehicle control from absolute state modeling to relative error modeling, reducing reliance on complex dynamic parameters. Furthermore, iterative optimization is introduced, progressively approximating the nonlinear problem as a solvable linear quadratic problem in the prediction time domain. This allows the optimization to continuously refine the control strategy in each iteration until convergence to a better trajectory tracking result. Simultaneously, during optimization, a soft constraint mechanism transforms safety boundaries such as steering angle, curvature, and lateral deviation into part of the cost function, automatically increasing penalties as the control law approaches the boundaries, effectively balancing the constraints.

[0033] Please refer to Figure 1 The diagram shown is a flowchart of a vehicle trajectory tracking control method according to an embodiment of this disclosure. The vehicle trajectory tracking control method specifically includes the following steps:

[0034] S101: Acquire the vehicle's longitudinal speed, steering status, and reference trajectory.

[0035] It should be noted that the reference trajectory can be a pre-planned path for the target vehicle. This reference trajectory can be obtained through a planning algorithm or pre-specified. Taking trajectory planning as an example, given the vehicle's initial state (including starting position, velocity, and acceleration), target state (including target position, velocity, and acceleration), obstacle positions, and dynamic and comfort constraints, a smooth trajectory can be calculated, allowing the vehicle to reach the target state along this trajectory. Trajectory planning typically includes two parts: path planning and velocity planning. Path planning is responsible for calculating a smooth path from the starting position to the target position, while velocity planning calculates the velocity at each point along this path, thus forming a velocity curve.

[0036] For vehicle longitudinal velocity, multi-source observations can be acquired in parallel and then fused to provide a velocity estimate. Wheel speed sensors have a fast response and low noise, but they can deviate under wet, slippery, braking, or skidding conditions. GNSS / IMU fusion has long-term stability, but it is affected by obstructed environments. One option is to fuse wheel speed, accelerometer readings, and inertial navigation direction at a frequency of 50-100Hz in the vehicle coordinate system to form a longitudinal velocity estimate. In canyons or tunnels, the weight of GNSS can be reduced to ensure that the velocity signal does not jump. Taking high-speed cruising as an example, when a vehicle is traveling at a constant speed of 90 km / h, the wheel speed deviates slightly due to tire wear, while the acceleration integral result given by the IMU is relatively smooth. The fused longitudinal velocity can avoid the systematic errors caused by a single source and provide a reliable input for subsequent error models.

[0037] Steering status can be obtained by measuring the steering wheel angle from the steering angle sensor or the CAN signal from the EPS controller, and then converted into the front wheel real angle by combining the steering transmission ratio; if the hardware allows, the wheel angle can also be read directly (steer-by-wire is more direct). Considering sensor zero bias and slight jitter, dynamic zero-point calibration and low-pass filtering can be set; to offset actuator hysteresis and bus delay, timestamp alignment and simple hysteresis compensation can be performed on the control side.

[0038] The reference trajectory can be derived from upper-level path planning or lane line extraction from high-precision maps and perception fusion. In step S101, it is unified to the same coordinate frame and time reference as the vehicle state, and necessary preprocessing is performed: discrete point densification and interpolation to match the controller sampling rate, smoothing estimation of curvature and heading to reduce the risk of differential amplification noise, and reparameterization by arc length to stably perform nearest point projection and preview point selection. For example, on a curve with a speed limit of 60 km / h on a two-lane road in the suburbs, the planning provides a reference trajectory point series for the next 60m. This can be resampled to a 20ms control cycle, the tangent and curvature of each point can be calculated, and a preview point can be selected 15-20m in front of the vehicle.

[0039] Longitudinal velocity and steering state are derived from the vehicle coordinate system. The reference trajectory is often in the map or world coordinate system. They can be aligned to the same instant through epoch timestamps. Then, the trajectory elements can be mapped to the vehicle coordinate system or the vehicle state can be mapped to the trajectory coordinate system through attitude and position solutions.

[0040] S102: Based on the longitudinal speed, steering state and reference trajectory, construct an error model of the vehicle relative to the reference trajectory. The error model at least characterizes the lateral position error, lateral speed error, heading angle error and heading angular velocity error of the vehicle relative to the reference trajectory.

[0041] The core of step S102 is to convert the motion relationship between the vehicle and the reference trajectory into a set of relative error states that evolve over time. This allows the use of a small number of states that reflect the essence of the trajectory deviation to represent the real-time difference between the vehicle and the reference trajectory, and to provide a linearizable and discretizable controlled object model for subsequent iLQR (iterative linear quadratic regulator) optimization.

[0042] This error model is a dynamic system driven by the vehicle's longitudinal velocity, steering state, and the curvature of the reference trajectory. It can reflect the evolution of lateral position error, lateral velocity error, heading angle error, and heading angular velocity error over time.

[0043] Specifically, the vehicle's deviation from the reference trajectory can be expressed as four basic state quantities: lateral position error, lateral velocity error, heading angle error, and heading angular velocity error. Based on the vehicle's longitudinal velocity, wheelbase, front wheel steering angle, and the curvature of the reference trajectory at the preview point, the error update satisfies the following relationship:

[0044]

[0045]

[0046]

[0047]

[0048] ;

[0049] in, Indicates lateral error. Indicates lateral velocity error. Indicates heading error. Indicates the error in heading angular velocity. Indicates the sampling time step. Indicates the sampling point at the current time. Indicates the sampling point at the next moment. Indicates the longitudinal speed of the vehicle. Indicates the vehicle's turning radius. This represents the curvature of the reference trajectory at the aiming point. Indicates the vehicle's wheelbase. Indicates the steering angle of the vehicle's front wheels. This indicates the turning radius of the vehicle.

[0050] The above relationships reveal the coupling between error, speed, and curvature: when a heading angle error exists, the lateral component of the vehicle's longitudinal velocity directly leads to changes in the lateral velocity error; when there is a difference between the vehicle's turning curvature and the reference curvature, the heading angle error increases proportionally with speed. That is, the dynamic evolution of vehicle error depends not only on the initial state but also closely on the vehicle's speed and trajectory geometry. For example, on a high-speed straight road, even if the initial heading angle error is small, the lateral velocity error may accumulate rapidly, leading to an increase in lateral position deviation; while on a small-radius curve, if the steering angle is insufficient, the heading angle error will increase rapidly, making convergence impossible. Therefore, the error model design realistically captures the relative motion between the vehicle and the reference trajectory.

[0051] S103: The error model is nonlinearly optimized using an iterative linear quadratic regulator algorithm to generate a control input sequence for vehicle lateral control in the prediction time domain.

[0052] In an exemplary embodiment, the iterative linear quadratic regulator algorithm performs nonlinear optimization of the error model in the following ways: at the nominal points of each sampling time of the current nominal trajectory, a first-order Taylor expansion of the error model is performed to obtain a time-varying linear approximation, wherein the nominal trajectory is the internal baseline trajectory used for linearization and cost approximation in the current iteration, and the nominal point is the expansion anchor point of the nominal trajectory at a certain sampling time; at the nominal point, a second-order Taylor approximation is performed on the cost function of state deviation and control deviation to obtain a quadratic form; the state feedback gain and feedforward correction term are calculated using backward dynamic programming, and the nominal trajectory and control input sequence are updated through backward rolling simulation until the total cost converges.

[0053] Specifically, the cost function includes at least: a state deviation term for penalizing lateral and heading deviations, and a control cost term for penalizing the change in control input magnitude with adjacent time intervals. The nonlinear optimization includes: handling inequality constraints using a soft constraint approach, incorporating at least one of the following—steering angle magnitude, steering angle rate of change, curvature and rate of change of curvature, lateral deviation threshold, and heading angular velocity threshold—into the objective function through a continuously differentiable penalty function, thereby significantly increasing the cost as the optimization approaches the boundary to avoid exceeding the limit.

[0054] In this embodiment, an iterative linear quadratic regulator (iLQR) is used. Instead of directly seeking the optimal solution on the original strongly nonlinear system, the nominal trajectory is used as a support point to locally linearize the nonlinear problem at each sampling time and quadratize the cost function. Then, the feedback / feedforward control law is obtained through analytical back substitution from back to front. A new trajectory is then generated by rolling from front to back. This process is repeated until the total cost converges.

[0055] At the implementation level, iLQR first requires a nominal trajectory as the basis for expansion. One option is to use the state-control sequence obtained from the previous control cycle as initial conditions for a warm start. If it's the first run, a coarse trajectory can also be constructed using the control sequence mapped from the reference curvature (e.g., using the reference curvature as the equivalent steering amount). Then, at the nominal point of each sampling time, the error model is expanded using a first-order Taylor expansion, yielding a set of time-varying linear approximations. Simultaneously, the cost functions for state and control deviations are approximated using a second-order Taylor expansion at the nominal point, transforming them into quadratic forms. In this way, the original nonlinear optimization is decomposed into a series of subproblems involving a linear system and quadratic costs, preserving the necessary nonlinear information (through continuous updates to the nominal points) while enabling rapid analytical solutions.

[0056] Next are the two waterlines of iLQR. The first is Backward: moving backward from the end of the prediction time domain, the future loss is fed back to the current decision, analytically obtaining the state feedback gain and feedforward correction term at each time step. The second is Forward: using the calculated gain and feedforward term, starting from the current measured state, a new state-control trajectory is generated using linear feedback and feedforward; this new trajectory is often closer to the optimal response of the real system than the old one. After completing one Forward, it is used as the new nominal trajectory to enter the next iteration.

[0057] In trajectory tracking, lateral position error and heading angle error are typically given high weights to minimize deviations as quickly as possible. Simultaneously, penalties are applied to the control amplitude and control changes at adjacent sampling times to suppress chattering and reduce actuator fatigue. When physical boundaries exist (such as maximum steering angle, maximum steering angular velocity, maximum curvature or rate of change of curvature, lateral error threshold, and heading angular velocity threshold), this embodiment does not introduce a hard constraint solver. Instead, these are softened into cost terms using continuously differentiable barrier / truncation functions: the cost increases sharply as the solution approaches the boundary. This amplifies the quadratic coefficients related to the boundary in the backward substitution, prompting the feedback and feedforward to automatically converge to more conservative control, preventing the solution from jumping out of the feasible region from the outset.

[0058] During the backward process, to avoid ambiguity in the error state variables, the error state variables can be denoted as... The control quantity can be denoted as Therefore, the aforementioned error update formula can be rewritten as:

[0059]

[0060]

[0061]

[0062] ;

[0063] For any reference point on the reference trajectory, denoted by r, the above equation can be rewritten as:

[0064] ;

[0065] in, , As an error model, all expectations are therefore 0.

[0066] To transform the nonlinear problem into an iLQR-solvable time-varying LQ subproblem, a first-order Taylor expansion is performed at each nominal point of the current nominal trajectory, neglecting higher-order terms:

[0067]

[0068]

[0069] ;

[0070] It is evident that the non-zero terms of the Jacobian matrix explicitly depend on the vehicle's longitudinal velocity v and the nominal heading. This ensures that the physical property of the error varying in tandem with the vehicle's longitudinal velocity and the reference curvature is preserved at the linear approximation level. The above equation can be further rewritten as:

[0071] ;

[0072] Change in state quantity error It can be represented as: , .

[0073] The above equation shows that the state error can form a linear state space. Performing forward Euler discretization on the above equation, we obtain:

[0074] ;

[0075] To complement model linearization, a second-order Taylor approximation is applied to the cost function at each nominal point, transforming it into a standard quadratic form. The cost function can be expressed as:

[0076]

[0077] ;

[0078] The second-order Taylor expansion around the nominal point (x0, u0) transforms into an error model with the same control quantity as the model:

[0079]

[0080] .

[0081] Using continuously differentiable barrier / truncation functions, constraints can be equivalent to cost terms. Taking control variables as an example:

[0082] ;

[0083] The above calculations include the error and the first and second derivatives of the Barrier function.

[0084] ;

[0085] After the second-order Taylor expansion, the cost is a quadratic term of the control error term. Therefore, the model is linear, and the cost is a quadratic term, which can be solved using dynamic programming with LQR.

[0086] ;

[0087] As can be seen, the control variables are incorporated into the quadratic cost, thus uniformly handling constraints such as steering angle / curvature magnitude and rate of change, lateral deviation threshold, and heading angular velocity threshold in the LQ subproblem of iLQR. When the solution approaches the boundary u... bound As time goes on, the cost increases exponentially, and optimization naturally moves away from the infeasible region. This avoids the drawback of LQR's inability to directly handle constraints and has a real-time advantage over MPC with explicit constraints.

[0088] The cost function may include nonlinear terms, and the system state equations are also typically nonlinear. This type of nonlinear model can be expressed as: , .

[0089] In this embodiment, the aforementioned nonlinear model can first be linearized using Taylor expansion. The system state equation f after the first-order Taylor expansion is expressed as follows:

[0090] .

[0091] The cost function l, after second-order Taylor expansion, is expressed as follows:

[0092] .

[0093] The system state equation and cost function after the linear expansion are transformed into models of trajectory point state variable errors and cost errors, respectively, as follows:

[0094]

[0095] .

[0096] As can be seen, after the above processing, the trajectory point cost error model becomes a quadratic form again, and the trajectory point state variable error becomes a linear combination of input increments (state variable increments and control variable increments). Therefore, the solution method for correcting the driving trajectory in the above linear model can be used similarly.

[0097] In this embodiment, the first-order gradient vector and Hessian matrix of the cost function of the corresponding trajectory point, as well as the Jacobian matrix of the system state equation, can be determined first based on the state variables of the driving trajectory.

[0098]

[0099]

[0100] ;

[0101] Among them, l k Let l be the first-order gradient vector of the cost function at trajectory point k. x Let l be the partial derivative of the cost function with respect to the state variables. u For the partial derivative of the cost function with respect to the control quantity, l xu and l ux Let l be the second-order mixed partial derivative of the cost function with respect to the state and control variables. xx Let l be the second-order partial derivative of the cost function with respect to the state variables. uu L is the second-order partial derivative of the cost function with respect to the control quantity. k Let F be the Hessian matrix of the cost function at trajectory point k. k Let be the Jacobian matrix of the system state equation at trajectory point k.

[0102] Subsequently, the optimal control law at step T can be determined based on the first-order gradient vector and Hessian matrix of the cost function of the corresponding trajectory point, the Jacobian matrix of the system state equation, and the linearized action value function.

[0103] Similarly, based on the above definitions of state value and action value, state value correction and action value correction are appropriately defined.

[0104] As an example, the state value correction is expressed as follows:

[0105] ;

[0106] The action value correction is expressed as follows:

[0107] ;

[0108] The kinematic equation constraints are expressed as follows:

[0109] ;

[0110] Where δX is the state variable correction, δU is the control variable correction, and f x Given the state input matrix and f u The control input matrix is ​​δQ(k). δQ(k) can be viewed as a linearized action value function.

[0111] Substituting and expanding the above equation, we get:

[0112] ;

[0113] After reorganization, the result is:

[0114]

[0115]

[0116]

[0117]

[0118]

[0119]

[0120] ;

[0121] Similarly, by taking the partial derivative of δQ(k) with respect to δU, we can obtain the optimal control law for the current step (i.e., the T-th step):

[0122]

[0123]

[0124] ;

[0125] Based on the optimal control rate at step T and the linearized action value function, the state value correction at step T is determined, and then the optimal control rate at step T-1 is recursively derived.

[0126] Specifically, the optimal control rate can be substituted into the linearized motion value function to obtain:

[0127]

[0128] .

[0129] It can be seen that in the process of recursively calculating the optimal control rate at each step from the end point of the driving trajectory to the initial point, the control quantity correction δU for each trajectory point (each step) can be continuously solved. In this embodiment, the control quantity of each trajectory point can be corrected based on this control quantity correction, thereby realizing the correction of the driving trajectory. The control quantity U correction process for each trajectory point can be expressed as:

[0130]

[0131] ;

[0132] in, It is the Taylor expansion point of the driving trajectory.

[0133] The corrected driving trajectory can be considered as the current coarse solution. Using this corrected driving trajectory and the linearized motion value function, the above recursive process of correcting the optimal control rate and driving trajectory at each step is repeated until the preset target is reached. At this point, the final corrected driving trajectory can be obtained.

[0134] Since iLQR cannot directly handle inequality constraints, which are essential in trajectory planning, these constraints can be transformed into equivalent costs using a truncation function. Different truncation functions are available, such as exponential, logarithmic, and square functions. The following example uses an exponential function; the transformed truncation function can be expressed as follows:

[0135] ;

[0136] Where q1 and q2 are constants set according to requirements.

[0137] As can be seen, when d(x)≥0, the value of the truncation function tends to infinity. Therefore, adding the truncation function sub-term to the action value function is equivalent to constraining the truncation function sub-term to tend towards a minimum value, thereby satisfying the constraint condition.

[0138] S104: Map the control input sequence to a vehicle steering control signal, so that the vehicle adjusts its driving trajectory based on the steering control signal.

[0139] The control input sequence is output in the form of curvature or equivalent steering angle, which needs to be converted into actual steering angle commands through a vehicle-specific control mapping function. This mapping can take into account the vehicle's steering geometry parameters (such as steering ratio and wheelbase) and the actuator's response characteristics, for example, by determining the relationship between steering angle and curvature through lookup tables or linear interpolation. Signal transmission can be achieved via the Controller Area Network (CAN) bus, sending the calculated control signals to the Electronic Control Unit (ECU), which then executes them via the steering motor or hydraulic system. To accommodate hardware limitations, filters (such as low-pass filters) can be added to smooth the control signals, avoiding mechanical stress or passenger discomfort caused by abrupt changes.

[0140] Taking an autonomous vehicle driving on city roads as an example, assume that the generated control input sequence indicates a curvature of 0.02m. -1 Adjust to 0.025 m -1 After mapping, the corresponding steering angle increases from 4° to 5°. This signal is transmitted to the steering motor via the CAN bus, and the vehicle then adjusts its direction, successfully approaching a curve with changing curvature. Step S104 transforms the abstract optimization result into perceptible vehicle actions, achieving closed-loop control from path deviation to actual trajectory correction, ensuring synchronization between the vehicle and the reference trajectory.

[0141] Figure 2 This is a test image of lateral position error obtained by using the aforementioned vehicle trajectory tracking control method in one embodiment of this disclosure. Figure 3 This is a test diagram of the heading angle error obtained by using the aforementioned vehicle trajectory tracking control method in one embodiment of this disclosure. Figure 2 and Figure 3 It can be seen that the average lateral position error is within 5cm and the heading angle error is within 2°, indicating that the trajectory tracking effect of the vehicle trajectory tracking control method provided in this disclosure is quite good.

[0142] In summary, the vehicle trajectory tracking control method provided in this disclosure constructs a kinematic error model based on longitudinal velocity, steering state, and reference trajectory, which includes lateral position, lateral velocity error, heading angle error, and heading angular velocity error, thus reducing the dependence on difficult-to-identify dynamic parameters. Furthermore, it utilizes iLQR to perform iterative linearization and quadratic approximation within the finite prediction time domain to calculate the control input sequence, balancing nonlinear accuracy and on-board real-time performance, thereby improving trajectory tracking accuracy and convergence.

[0143] Please refer to Figure 4 As shown, based on the same inventive concept as the aforementioned vehicle trajectory tracking control method, this disclosure provides a vehicle trajectory tracking control system 400, which includes: an acquisition module 401, an error modeling module 402, an optimization module 403, and a control module 404.

[0144] The acquisition module 401 is used to acquire the vehicle's longitudinal speed, steering state, and reference trajectory; the error modeling module 402 is used to construct an error model of the vehicle relative to the reference trajectory based on the longitudinal speed, steering state, and reference trajectory, wherein the error model at least characterizes the vehicle's lateral position error, lateral speed error, heading angle error, and heading angular velocity error relative to the reference trajectory; the optimization module 403 uses an iterative linear quadratic regulator algorithm to perform nonlinear optimization on the error model, generating a control input sequence for vehicle lateral control in the prediction time domain; the control module 404 maps the control input sequence to the vehicle's steering control signal, enabling the vehicle to adjust its driving trajectory based on the steering control signal.

[0145] Please refer to Figure 5 As shown, this disclosure also provides an unmanned vehicle 500, which includes at least one processor 501, a memory 502 (e.g., non-volatile memory), a main memory 503, and a communication interface 504. The at least one processor 501, memory 502, main memory 503, and communication interface 504 are connected together via an internal bus 505. The at least one processor 501 is used to invoke at least one program instruction stored or encoded in the memory 502 to cause the at least one processor 501 to perform various operations and functions of the vehicle trajectory tracking control method described in the various embodiments of this specification.

[0146] In the embodiments of this specification, the unmanned vehicle 500 can be configured with a functional terminal to carry the above-mentioned hardware structure. The functional terminal may include, but is not limited to: personal computer, server computer, workstation, desktop computer, laptop computer, notebook computer, mobile electronic device, smartphone, tablet computer, cellular phone, personal digital assistant (PDA), handheld device, messaging device, wearable electronic device, consumer electronic device, etc.

[0147] This disclosure also provides a computer-readable medium carrying computer-executable instructions, which, when executed by a processor, can be used to implement various operations and functions of the vehicle trajectory tracking control method described in the various embodiments of this specification.

[0148] The computer-readable medium in this disclosure can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium can 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 a computer-readable storage medium may include, but are not limited to: 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 disclosure, a computer-readable storage medium can be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

[0149] In this disclosure, a computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. Such propagated data signals may take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. The computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium, which can transmit, propagate, or transfer a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium may be transmitted using any suitable medium, including but not limited to: wireless, wireline, optical fiber, RF, etc., or any suitable combination thereof.

[0150] Those skilled in the art will understand that embodiments of this disclosure can be provided as methods, systems, or computer program products. Therefore, this disclosure can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this disclosure can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0151] This disclosure is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus, systems, and computer program products according to embodiments of this disclosure. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0152] It will be apparent to those skilled in the art that this disclosure is not limited to the details of the exemplary embodiments described above, and that this disclosure can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of this disclosure is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within this disclosure. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0153] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A vehicle trajectory tracking control method, characterized in that, include: Acquire the vehicle's longitudinal velocity, steering status, and reference trajectory; Based on the longitudinal velocity, steering state, and reference trajectory, an error model of the vehicle relative to the reference trajectory is constructed. The error model at least characterizes the lateral position error, lateral velocity error, heading angle error, and heading angular velocity error of the vehicle relative to the reference trajectory. The error model is nonlinearly optimized using an iterative linear quadratic regulator algorithm to generate a control input sequence for vehicle lateral control in the prediction time domain. The control input sequence is mapped to the vehicle's steering control signal, enabling the vehicle to adjust its driving trajectory based on the steering control signal.

2. The vehicle trajectory tracking control method according to claim 1, characterized in that, The lateral position error, lateral velocity error, heading angle error, and heading angular velocity error change in conjunction with the vehicle's longitudinal velocity and the curvature of the reference trajectory.

3. The vehicle trajectory tracking control method according to claim 2, characterized in that, The error discrete update of the error model satisfies the following relationship: ； in, Indicates lateral error. Indicates lateral velocity error. Indicates heading error. Indicates the error in heading angular velocity. Indicates the sampling time step. Indicates the sampling point at the current time. Indicates the sampling point at the next moment. Indicates the longitudinal speed of the vehicle. Indicates the vehicle's turning radius. This represents the curvature of the reference trajectory at the aiming point. Indicates the vehicle's wheelbase. This indicates the steering angle of the vehicle's front wheels.

4. The vehicle trajectory tracking control method according to claim 1, characterized in that, The iterative linear quadratic regulator algorithm performs nonlinear optimization on the error model, specifically including: At the nominal points of each sampling time of the current nominal trajectory, the error model is expanded by Taylor first order to obtain a time-varying linear approximation, where the nominal trajectory is the internal baseline trajectory used for linearization and cost approximation in the current iteration, and the nominal point is the expansion anchor point of the nominal trajectory at a certain sampling time. At the nominal point, a second-order Taylor approximation is performed on the cost function of state deviation and control deviation to obtain a quadratic form; The state feedback gain and feedforward correction term are calculated using backward dynamic programming, and the nominal trajectory and control input sequence are updated through forward rolling simulation until the total cost converges.

5. The vehicle trajectory tracking control method according to claim 4, characterized in that, The cost function includes at least: a state deviation term for penalizing lateral and heading deviations, and a control cost term for penalizing changes in the control input magnitude and adjacent time intervals.

6. The vehicle trajectory tracking control method according to claim 5, characterized in that, The nonlinear optimization includes: handling the inequality constraints with a soft constraint method, and incorporating at least one of the following into the objective function through a continuously differentiable penalty function: steering angle magnitude, steering angle change rate, curvature and curvature change rate, lateral deviation threshold, and heading angular velocity threshold.

7. The vehicle trajectory tracking control method according to claim 1, characterized in that, The control input sequence is represented by curvature or the equivalent turning amount of curvature.

8. A vehicle trajectory tracking control system, characterized in that, include: The acquisition module is used to acquire the vehicle's longitudinal speed, steering status, and reference trajectory; The error modeling module is used to construct an error model of the vehicle relative to the reference trajectory based on the longitudinal velocity, steering state and reference trajectory. The error model at least characterizes the lateral position error, lateral velocity error, heading angle error and heading angular velocity error of the vehicle relative to the reference trajectory. The optimization module uses an iterative linear quadratic regulator algorithm to perform nonlinear optimization on the error model, generating a control input sequence for vehicle lateral control in the prediction time domain. The control module maps the control input sequence to the vehicle's steering control signal, enabling the vehicle to adjust its driving trajectory based on the steering control signal.

9. An unmanned vehicle, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the vehicle trajectory tracking control method as described in any one of claims 1 to 7.

10. A computer-readable medium, characterized in that, The computer-readable medium carries computer-executable instructions, which, when executed by a processor, are used to implement the vehicle trajectory tracking control method as described in any one of claims 1 to 7.

Images