Self-adaptive vehicle trajectory tracking control method and system suitable for curved surface terrain

Abstract

The invention discloses a self-adaptive vehicle trajectory tracking control method and system suitable for a curved surface terrain. The method comprises the following steps: S1, obtaining a point cloud map and a reference trajectory of a current environment; s2, selecting key points on the reference trajectory, and fitting a local ground equation around the key points; and S3, establishing a manifold model prediction controller by taking the position, the speed and the yaw angle of the vehicle as state variables and taking the longitudinal acceleration and the front wheel rotation angle of the vehicle as control variables. And S4, adjusting the target function weight of the manifold model prediction controller in combination with the curvature of the current position of the reference trajectory. And S5, in combination with the current speed of the vehicle and the transverse error between the vehicle and the reference trajectory, the PID controller is used for compensating the acceleration output by the controller and the front wheel turning angle. Compared with an existing trajectory tracking method, the method provided by the invention has the advantage that the trajectory tracking precision and control robustness of the vehicle on the complex curved surface terrain can be remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of vehicle trajectory tracking and control, and in particular relates to a vehicle trajectory tracking control method and system suitable for adaptive curved terrain. Background Art

[0002] With the rapid development of autonomous driving and unmanned driving technologies, the ability of vehicles to drive autonomously in various complex environments has become particularly important. Especially on non-flat curved terrain, such as mountains, rural roads, forest rescue, and mining transportation, the vehicle's trajectory tracking accuracy and the robustness of the control system directly affect the safety and stability of driving. However, most of the existing trajectory tracking methods are designed based on flat terrain, ignoring key factors such as the slope, roll, and height changes of the curved terrain. This results in insufficient trajectory tracking accuracy when the vehicle is driving on uneven terrain, and it is easy to deviate from the predetermined path, increasing dynamic instability and safety risks.

[0003] In addition, the unpredictability of complex terrain places higher demands on the robustness of the control system. Existing model predictive control (MPC) methods often have difficulty coping with the challenges brought about by drastic changes in terrain when dealing with complex curved terrain. In particular, when the vehicle encounters sharp turns, steep slopes, or highly irregular terrain, traditional control algorithms cannot effectively adapt to these changes, which may lead to control failure or vehicle deviation from the trajectory. In order to solve these problems, a robust trajectory tracking control algorithm that can adapt to complex curved terrain is urgently needed to improve the performance and safety of autonomous vehicles in these environments and expand the scope of application of autonomous driving technology. Summary of the invention

[0004] In order to solve the technical problems existing in the background technology, the present invention aims to provide a vehicle trajectory tracking control method and system suitable for curved terrain adaptation. By accurately fitting the terrain of the vehicle's driving path and combining the manifold model predictive control (MPC) method, a vehicle trajectory tracking solution suitable for complex curved terrain is provided.

[0005] In order to solve the technical problem, the technical solution of the present invention is:

[0006] A vehicle trajectory tracking control method suitable for curved terrain adaptation, the method comprising:

[0007] S1: Obtain the point cloud information of the current environment to build a three-dimensional map and obtain the reference trajectory that the vehicle needs to track;

[0008] S2: Select key points of the reference trajectory and fit local ground equations near the key points;

[0009] S3: Determine the state and control quantity of the vehicle, and construct a manifold model predictive controller based on the ground equation obtained in step S2;

[0010] S4: adjusting the objective function weight of the manifold model predictive controller in combination with the curvature of the current position of the reference trajectory;

[0011] S5: Calculate the current speed error of the vehicle and the lateral error between the vehicle and the reference trajectory, and use the PID controller to compensate the acceleration and front wheel steering angle output by the optimized manifold model prediction controller.

[0012] Further, the step S2 comprises:

[0013] uniformly selecting a plurality of key points on the reference trajectory, including an initial point, an end point, and each key point on the path of the trajectory;

[0014] At each key point, the distance between the key point and the adjacent key point is taken as the radius R, and the point cloud data within the radius is extracted;

[0015] Use polynomial fitting or spline interpolation surface fitting algorithm to perform surface fitting on the extracted point cloud data and establish the local ground equation;

[0016] Evaluate the fitted ground equations to ensure that the fitted surface accurately describes the terrain features around the key points, and adjust the fitting parameters as needed;

[0017] The fitted ground equations are used as the input of the local terrain model and fed into the manifold model predictive controller.

[0018] Further, the step S2 specifically includes:

[0019] S201: uniformly select multiple key points on the reference trajectory, including the initial point, the end point of the trajectory, and each key point on the path:

[0020] The reference trajectory is represented as a set of discrete trajectory points {P 1 ,P 2 ,…,P n}, where each trajectory P i Contains its two-dimensional coordinates (x i ,y i ), the key points are selected by evenly spaced selections or dynamically adjusted according to the complexity of the terrain;

[0021] S202: At each key point, the distance between the key point and the adjacent key point is used as the radius R, and the point cloud data within the radius is extracted:

[0022] For each selected key point P i=(x i ,y i ), define a neighborhood range of radius R:

[0023]

[0024] Among them, ∥P i -P i+1 ∥ is the key point P i Its adjacent key point P i+1 The Euclidean distance between them is used to extract the corresponding point cloud data {Q j}Perform surface fitting;

[0025] S203: Use polynomial fitting, spline interpolation or other surface fitting algorithms to perform surface fitting on the extracted point cloud data and establish a local ground equation, including:

[0026] Terrain parameter fitting:

[0027] For the extracted point cloud data {Q j =(x j ,y j ,z j )} to perform surface fitting, assuming that the ground can be represented by a quadratic polynomial:

[0028] z=F(x,y)=a 0 +a 1 x+a 2 y+a 3 x 2 +a 4 xy+a 5 y 2

[0029] The fitting problem is transformed into minimizing the sum of squares of the following residuals:

[0030]

[0031] The optimization problem is solved by using optimization libraries such as Ceres to obtain the fitting parameter p = [a 0 ,a 1 ,a 2 ,a 3 ,a 4 ,a 5 ] T ;

[0032] Definition of terrain gradient:

[0033] Terrain Gradient Represents the partial derivative of terrain height z with respect to x and y on the plane (x, y), that is

[0034]

[0035] Compute terrain gradients by fitting parameters:

[0036] According to the fitted surface equation, the terrain gradient g is expressed as a linear combination of the fitting parameters:

[0037]

[0038] Therefore, the terrain gradient g is expressed as:

[0039]

[0040] The derivative of the terrain gradient g with respect to the fitting parameter p is expressed as:

[0041]

[0042] S204: Evaluate the fitted ground equations to ensure that the fitted surface can accurately describe the terrain features around the key points, and adjust the fitting parameters as needed:

[0043] The quality of the fitted surface is evaluated by calculating the mean square error (MSE):

[0044]

[0045] in, is the estimated value of the fitted surface, N is the number of point clouds, if MSE exceeds the predetermined threshold, the fitting parameters or model need to be adjusted;

[0046] S205: The fitted ground equation is used as the input of the local terrain model and input to the manifold model prediction controller:

[0047] The fitted parameter p = [a 0 ,a 1 ,a 2 ,a 3 ,a 4 ,a 5 ] T The input is sent to the vehicle's manifold model predictive controller to update the local terrain model. The manifold model predictive controller will use the fitted parameters to perform trajectory prediction and control in complex terrain to solve for the optimal control input.

[0048] Further, the step S3 comprises:

[0049] S301: determining the position, speed and plane rotation matrix of the vehicle as state quantities of the manifold model predictive controller; determining the longitudinal acceleration and front wheel steering angle of the vehicle as control quantities of the manifold model predictive controller;

[0050] S302: Establishing a state transfer equation of a manifold model prediction controller based on a dynamic model of the vehicle and the local ground equation;

[0051] S303: constructing an objective function by comprehensively considering the path tracking error, the change of the control input and the dynamic constraints of the vehicle;

[0052] S304: Solve the objective function using a numerical optimization method to obtain the optimal longitudinal acceleration and front wheel turning angle of the vehicle at the current moment.

[0053] Further, the step S3 specifically includes:

[0054] S301: Determine the position, speed and plane rotation matrix of the vehicle as the state variables of the manifold model prediction controller; determine the longitudinal acceleration and front wheel steering angle of the vehicle as the control variables of the manifold model prediction controller:

[0055] Definition of state quantity:

[0056] The position coordinates of the vehicle on the plane are defined as

[0057] The speed of the vehicle on the plane is defined as

[0058] The vehicle’s plane rotation matrix is ​​defined as R∈SO(2);

[0059] The vehicle state x is expressed as:

[0060]

[0061] Definition of control volume:

[0062] The longitudinal acceleration of the vehicle along the body axis is defined as

[0063] The front wheel turning angle of the vehicle is defined as

[0064] The control quantity u of the vehicle is expressed as:

[0065]

[0066] Defining a composite manifold Its dimensions Vehicle status Vehicle control volume

[0067] S302: Based on the vehicle's dynamics model and local ground equations, a state transfer equation of the manifold model prediction controller is established to ensure that the state quantity can accurately predict the movement of the vehicle on the curved terrain:

[0068] Define x b (P,R) is the x-coordinate axis of the vehicle body, is the vehicle speed along the body axis, is the yaw angular velocity of the vehicle around the z-axis;

[0069] Consider a n-manifold The robot system on , which is discretized with a sampling period Δt, is as follows:

[0070]

[0071] Considering the movement of the vehicle on the curved terrain, the state transfer equation is established as follows:

[0072]

[0073] Among them, α and β are based on the terrain gradient The calculated coefficients are:

[0074]

[0075] Among them, e 1 =[1,0] T is the unit direction vector;

[0076] The linearization error equation is derived:

[0077]

[0078] The coefficient and The calculation is as follows:

[0079]

[0080] After derivation, the partial derivative of the state transfer function and and The calculation is as follows:

[0081]

[0082] The calculation methods of various parameters are as follows:

[0083]

[0084] where e 2 =[0,1] T ;

[0085] S303: Considering the path tracking error, the change of control input and the dynamic constraints of the vehicle, an objective function is constructed:

[0086] When constructing the objective function of the manifold model predictive controller, the path tracking error, the change of control input and the constraints of vehicle dynamics are comprehensively considered;

[0087] Construction of the objective function:

[0088] The objective function J is defined as the weighted sum of path tracking error, control input change, and terminal state error:

[0089]

[0090] in:

[0091] N is the controller prediction step size;

[0092] x k is the state of the vehicle at time k;

[0093] is the target state (the state on the reference trajectory);

[0094] u k is the control input of the vehicle at time k;

[0095] is the target control input (the control input corresponding to the reference trajectory);

[0096] Q k , R k and P N are the weight matrices of state error, control input error and terminal state error; state transfer equation and constraints:

[0097] The state transition equation of the vehicle is based on its dynamic model:

[0098]

[0099] The initial state is:

[0100] x 0 =x init

[0101] The constraints controlling the input are:

[0102] u k ∈U,k=0,…,N-1

[0103] Where U represents the allowable range of control input;

[0104] The error dynamics are linearized and transformed into a QP problem:

[0105] Using the linearized single-step error state equation derived in step S302:

[0106]

[0107] The error state equation of step N is further derived:

[0108] δX=MδU+Hδx 0

[0109] in:

[0110]

[0111]

[0112] The optimization of the objective function is transformed into a standard quadratic programming problem QP:

[0113]

[0114] δU∈[δU min ,δU max ]

[0115] δx 0 =δx init

[0116] in:

[0117]

[0118]

[0119] S304: Solve the objective function using numerical optimization methods: Use the QP solver to effectively solve and obtain the optimal control input increment in each control cycle This enables rolling optimization control.

[0120] Further, the step S4 comprises:

[0121] S401: Calculate the curvature of the reference track at the current position of the vehicle on the reference track to obtain curvature information of the current driving path of the vehicle;

[0122] S402: dynamically adjusting the weight coefficient in the objective function of the manifold model predictive controller according to the calculated curvature information, increasing the weight of the path tracking error term when the curvature is large; and balancing the weight of the control input change term when the curvature is small;

[0123] S403: Input the adjusted objective function into the optimization process of the manifold model predictive controller, and update the objective function weight in real time in subsequent control cycles to adapt to dynamically changing terrain and trajectory characteristics.

[0124] Further, the step S5 comprises:

[0125] S501: obtaining the current speed of the vehicle in real time, calculating the deviation from the target speed, obtaining the current position of the vehicle in real time, and calculating the distance between the current position of the vehicle and the nearest point of the reference trajectory as the lateral error;

[0126] S502: Design a PID controller based on the lateral error and speed error of the vehicle, and adjust the proportional gain and integral gain of the controller;

[0127] S503: In each control cycle, the output of the PID controller is calculated, and the longitudinal acceleration and the front wheel steering angle generated by the manifold model prediction controller are compensated in real time according to the output value to correct the driving direction and acceleration / deceleration behavior of the vehicle;

[0128] S504: Based on the vehicle's dynamics model, limiting the compensated acceleration and front wheel steering angle is performed.

[0129] A vehicle trajectory tracking control system suitable for curved terrain adaptation, the system comprising:

[0130] The environment perception module is used to obtain the point cloud information of the current environment to build a three-dimensional map and obtain the reference trajectory that the vehicle needs to track;

[0131] A key point extraction module, used to select key points of the reference trajectory and fit local ground equations near the key points;

[0132] A state estimation and control building module, used to determine the state quantity and control quantity of the vehicle, and to build a manifold model predictive controller based on the ground equation obtained in step S2;

[0133] A curvature adjustment module, used to adjust the objective function weight of the manifold model predictive controller in combination with the curvature of the current position of the reference trajectory;

[0134] The error compensation and output optimization module is used to calculate the vehicle's current speed error and the lateral error between the vehicle and the reference trajectory, and use a PID controller to compensate for the acceleration and front wheel steering angle output by the manifold model prediction controller.

[0135] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, a vehicle trajectory tracking control method suitable for adaptive vehicle trajectory tracking on curved terrain as claimed in any one of claims 1 to 7 is implemented.

[0136] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements a vehicle trajectory tracking control method suitable for curved terrain adaptation as claimed in any one of claims 1 to 7.

[0137] Compared with the prior art, the advantages of the present invention are:

[0138] The present invention can accurately describe the slope, roll and height changes of complex curved terrain by selecting key points on the reference trajectory and fitting local ground equations around these key points. Compared with the traditional flat terrain trajectory tracking algorithm, the present invention improves the trajectory tracking accuracy of the vehicle on uneven terrain and effectively reduces the tracking error caused by terrain changes;

[0139] The present invention dynamically adjusts the objective function weight in the manifold model predictive control (OMMPC) to adapt to the curvature change of the vehicle's current trajectory position. This design enhances the robustness of the control system under different terrain conditions, especially in sharp bends or drastically changing terrain, ensuring that the vehicle can track the reference trajectory more accurately;

[0140] The present invention combines the vehicle's current speed error and lateral error, and uses a PID controller to perform real-time compensation for the acceleration and front wheel angle output by the manifold model prediction controller. Through this integrated compensation mechanism, the vehicle's control accuracy and driving stability are further improved, especially effectively avoiding the risk of skidding and trajectory deviation on complex terrain. BRIEF DESCRIPTION OF THE DRAWINGS

[0141] Figure 1 , flow chart of the robust manifold model predictive control method proposed in the present invention;

[0142] Figure 2 , Schematic diagram of vehicle driving on curved surface;

[0143] Figure 3 ,Schematic diagram of fitting local ground equations around the reference trajectory;

[0144] Figure 4 , the robust manifold model predictive control framework proposed in this invention. DETAILED DESCRIPTION

[0145] The specific implementation mode of the present invention is described below in conjunction with embodiments:

[0146] It should be noted that the structures, proportions, sizes, etc. shown in this specification are only used to match the contents disclosed in the specification so that people familiar with this technology can understand and read them, and are not used to limit the conditions under which the present invention can be implemented. Any structural modification, change in proportional relationship or adjustment of size should still fall within the scope of the technical content disclosed in the present invention without affecting the effects and purposes that can be achieved by the present invention.

[0147] At the same time, the terms such as "upper", "lower", "left", "right", "middle" and "one" cited in this specification are only for the convenience of description and are not used to limit the scope of implementation of the present invention. Changes or adjustments to their relative relationships should be regarded as the scope of implementation of the present invention without substantially changing the technical content.

[0148] Embodiment 1:

[0149] like Figure 1 and Figure 4 As shown, a method and system for adaptive vehicle trajectory tracking control on curved terrain includes the following steps:

[0150] S1: Get the current environment point cloud map and reference trajectory;

[0151] When the vehicle is driving, the SLAM (Simultaneous Localization and Mapping) system or other sensors such as LiDAR and cameras are used to collect point cloud data of the vehicle's surroundings. After pre-processing steps such as filtering and downsampling, the point cloud data generates a three-dimensional point cloud map of the current environment.

[0152] The system receives a reference trajectory from the upper-level planner or a target path pre-set by the operator. The reference trajectory provides information about the path the vehicle should travel for subsequent steps and is the basis of the manifold model predictive controller.

[0153] S2: Select key points on the reference trajectory and fit local ground equations around these key points. The schematic diagram of this process is shown in Figure 3 As shown;

[0154] S201: uniformly select a number of key points on the reference trajectory, including the initial point, the end point of the trajectory, and each key point on the path:

[0155] In practical applications, the reference trajectory can be represented as a set of discrete trajectory points {P 1 ,P 2 ,...,P n}, where each trajectory P i Contains its two-dimensional coordinates (x i ,y i ). The selection of key points can be done by evenly spacing them or dynamically adjusting them according to the complexity of the terrain.

[0156] S202: At each key point, the distance between the key point and the adjacent key point is used as the radius R, and the point cloud data within the radius is extracted:

[0157] For each selected key point P i =(x i ,yi ), define a neighborhood range of radius R:

[0158]

[0159] Among them, ∥P i -P i+1 ∥ is the key point P i Its adjacent key point P i+1 The Euclidean distance between them is used to extract the corresponding point cloud data {Q j}Perform surface fitting.

[0160] S203: Use polynomial fitting, spline interpolation or other surface fitting algorithms to perform surface fitting on the extracted point cloud data and establish a local ground equation:

[0161] 1. Terrain parameter fitting

[0162] For the extracted point cloud data {Q j =(x j ,y j ,z j )} to perform surface fitting, assuming that the ground can be represented by a quadratic polynomial:

[0163] z=F(x,y)=a 0 +a 1 x+a 2 y+a 3 x 2 +a 4 xy+a 5 y 2

[0164] The fitting problem can be transformed into minimizing the sum of squares of the following residuals:

[0165]

[0166] This optimization problem can be solved by using optimization libraries such as Ceres to obtain the fitting parameter p = [a 0 ,a 1 ,a 2 ,a 3 ,a 4 ,a 5 ] T .

[0167] 2. Definition of terrain gradient

[0168] Terrain Gradient Represents the partial derivative of terrain height z with respect to x and y on the plane (x, y), that is

[0169]

[0170] 3. Calculate terrain gradient by fitting parameters

[0171] According to the fitted surface equation, the terrain gradient g can be expressed as a linear combination of fitting parameters:

[0172]

[0173] Therefore, the terrain gradient g can be expressed as:

[0174]

[0175] Similarly, the derivative of the terrain gradient g with respect to the fitting parameter p can be expressed as:

[0176]

[0177] S204: Evaluate the fitted ground equations to ensure that the fitted surface can accurately describe the terrain features around the key points, and adjust the fitting parameters as needed:

[0178] The quality of the fitted surface can be evaluated by calculating the mean square error (MSE):

[0179]

[0180] in, is the estimated value of the fitted surface, and N is the number of point clouds. If the MSE exceeds the predetermined threshold, the fit may be inaccurate and the fitting parameters or model need to be adjusted.

[0181] S205: The fitted ground equation is used as the input of the local terrain model and input to the manifold model prediction controller:

[0182] The fitted parameter p = [a 0 ,a 1 ,a 2 ,a 3 ,a 4 ,a 5 ] T The parameters are input into the manifold model prediction controller of the vehicle to update the local terrain model. The manifold model prediction controller will use these parameters to predict and control the trajectory in complex terrain and solve the optimal control input (such as acceleration and steering angle). The specific process is shown in S303.

[0183] S3: A manifold model predictive controller is established with the vehicle's position, velocity and yaw angle as state variables and the vehicle's longitudinal acceleration and front wheel steering angle as control variables.

[0184] S301: Figure 2As shown, the vehicle position, velocity and vehicle plane rotation matrix are defined as the state quantities of the manifold model prediction controller; the vehicle longitudinal acceleration and front wheel angle are defined as the control quantities of the manifold model prediction controller:

[0185] Definition of state quantity:

[0186] The position coordinates of the vehicle on the plane are defined as

[0187] The speed of the vehicle on the plane is defined as

[0188] The vehicle's plane rotation matrix is ​​defined as R∈SO(2),

[0189] The vehicle's state x can be expressed as:

[0190]

[0191] 2. Definition of control quantity:

[0192] The longitudinal acceleration of the vehicle along the body axis is defined as

[0193] The front wheel turning angle of the vehicle is defined as

[0194] The control quantity u of the vehicle can be expressed as:

[0195]

[0196] Defining a composite manifold Its dimensions Vehicle status Vehicle control volume

[0197] S302: Based on the vehicle's dynamic model and local ground equations, the state transfer equation of the manifold model prediction controller is established to ensure that the state quantity can accurately predict the movement of the vehicle on the curved terrain:

[0198] Define x b (P,R) is the x-coordinate axis of the vehicle body, is the vehicle speed along the body axis, is the yaw angular velocity of the vehicle around the z-axis.

[0199] Consider a n-manifold The robot system on , which is discretized with a sampling period Δt, can be written in a compact form, called the canonical form, as follows:

[0200]

[0201] Considering the movement of the vehicle on the curved terrain, the state transfer equation is established as follows:

[0202]

[0203] Among them, α and β are based on the terrain gradient The calculated coefficients are:

[0204]

[0205] Among them, e 1 =[1,0] T is the unit direction vector.

[0206] The linearization error equation is derived:

[0207]

[0208] The coefficient and The calculation is as follows:

[0209]

[0210] In the present invention, after derivation, the partial derivative of the state transfer function and G fk and G fk The calculation is as follows:

[0211]

[0212]

[0213] The calculation methods of various parameters are as follows:

[0214]

[0215] where e 2 =[0,1] T

[0216] S303: Considering the path tracking error, the change of control input and the dynamic constraints of the vehicle, an objective function is constructed:

[0217] When constructing the objective function of the manifold model predictive controller, the path tracking error, the change of the control input, and the constraints of the vehicle dynamics must be considered comprehensively. The objective function is designed to minimize these errors and the change of the control input, ensuring that the vehicle can accurately track the reference trajectory on complex terrain and maintain a smooth and stable state of motion.

[0218] 1. Construction of objective function:

[0219] Define the objective function J as the weighted sum of path tracking error, control input change, and terminal state error

[0220]

[0221] in:

[0222] N is the controller prediction step size,

[0223] x k is the state of the vehicle at time k,

[0224] is the target state (the state on the reference trajectory),

[0225] u k is the control input of the vehicle at time k,

[0226] is the target control input (the control input corresponding to the reference trajectory),

[0227] Q k , R k and P N are the weight matrices of state error, control input error and terminal state error respectively.

[0228] 2. State transfer equations and constraints:

[0229] The state transition equation of the vehicle is based on its dynamic model:

[0230]

[0231] The initial state is:

[0232] x 0 =x init

[0233] The constraints controlling the input are:

[0234] u k ∈U,k=0,…,N-1

[0235] Where U represents the allowable range of the control input.

[0236] 3. Linearize the error dynamics and transform it into a QP problem:

[0237] In order to solve the optimization problem more effectively, the present invention uses the linearized single-step error state equation derived in S302:

[0238]

[0239] The error state equation of step N is further derived:

[0240] δX=MδU+Hδx 0

[0241] in:

[0242]

[0243] The optimization of the objective function is transformed into a standard quadratic programming problem (QP):

[0244]

[0245] stδU∈[δU min ,δU max ]

[0246] δx 0 =δx init

[0247] in:

[0248]

[0249]

[0250] S304: Solve the objective function using numerical optimization methods: Use the existing QP solver to effectively solve and obtain the optimal control input increment in each control cycle This enables rolling optimization control.

[0251] S4: Adjust the objective function weight of the manifold model predictive controller based on the curvature of the current position of the reference trajectory.

[0252] S401: According to the current position of the vehicle on the reference track, the curvature of the reference track at the position is calculated to obtain the curvature information of the current driving path of the vehicle:

[0253] Curvature is an important geometric quantity that describes the curvature of a path. When a vehicle is driving, in order to calculate the curvature of the current position, it is necessary to first calculate based on the trajectory point data. Assume that the set of trajectory points is {P i =(x i ,y i ,θ i )}, where (x i ,y i ) is the coordinate of the trajectory point, θ i is the angle of the trajectory point.

[0254] The curvature is calculated by the following steps:

[0255] 1. For the current trajectory point P k, select a window range including its left and right adjacent points (the default window size is 5 points).

[0256] 2. Calculate the statistics of the coordinates and angles of each trajectory point:

[0257]

[0258] Where N is the size of the window.

[0259] 3. Calculate the second-order statistics of the trajectory points:

[0260]

[0261] 4. Curvature C k The calculation formula is:

[0262]

[0263] The curvature value indicates the curvature of the path at the current position. If the calculated curvature is an invalid value (such as NaN or infinity), the last valid curvature value is used as the current curvature.

[0264] S402: Dynamically adjust the weight R in the objective function of the manifold model prediction controller according to the calculated curvature value:

[0265] R=R 0 ·(1+0.05·C k )

[0266] Among them, R 0 is the initial weight matrix. The larger the curvature, the larger the amplitude of weight adjustment, thus paying more attention to minimizing the path tracking error; when the curvature is small, the controller pays more attention to smoothness and stability.

[0267] S403: Input the adjusted objective function into the optimization process of the manifold model predictive controller, i.e. step S303.

[0268] S5: Calculate the current speed error of the vehicle and the lateral error between the vehicle and the reference trajectory, and use a PID controller to compensate the acceleration and front wheel steering angle output by the controller.

[0269] S501: Obtain the current speed of the vehicle in real time, calculate the deviation from the target speed, obtain the current position of the vehicle in real time, and calculate the distance between the current position of the vehicle and the nearest point of the reference trajectory as the lateral error:

[0270] 1. Speed ​​error calculation:

[0271] The vehicle's current speed v x It is perceived by the actual speed. The target speed v targetis the value extracted from the reference trajectory by the trajectory analyzer. The velocity error e v It is expressed as:

[0272] e v =v target -v x

[0273] Among them, v target is the reference speed, v x is the current actual speed.

[0274] 2. Calculation of lateral error:

[0275] Lateral error e y is the vertical distance between the current position of the vehicle and the nearest point on the reference trajectory. Assume that the current vehicle position is p current =(x,y), the nearest reference point is p ref =(x ref ,y ref ), then the lateral error can be calculated as:

[0276] e y =∥p current -p ref ∥ ⊥

[0277] Among them, ∥·∥ ⊥ Indicates vertical distance calculation.

[0278] S502: Based on the lateral error and speed error of the vehicle, a PID controller is designed, and the proportional gain and integral gain of the controller are adjusted:

[0279] The PI controller is used to compensate the speed and lateral position in real time and adjust the controller ratio (K p ) and integral (K i ) gain to ensure the stable driving of the vehicle.

[0280] 1. For speed control, the output u of the PID controller v for:

[0281]

[0282] in, is the proportional gain of the velocity error, is the integral gain of the speed error, is the differential gain of the speed error.

[0283] 2. For lateral error control, the output u of the PID controller y for:

[0284]

[0285] in, is the proportional gain of the lateral error, is the integral gain of the lateral error, is the differential gain of the speed error.

[0286] S503: In each control cycle, the output of the PID controller is calculated, and the longitudinal acceleration and front wheel steering angle generated by the manifold model prediction controller are compensated in real time according to the output value to correct the vehicle's driving direction and acceleration / deceleration behavior:

[0287] In S304, the acceleration output a(t) of the manifold model predictive controller is obtained, and the velocity v is calculated using the discrete time integration method. x , the specific calculation formula is:

[0288] v x (t+Δt)=v x (t)+a(t)·Δt

[0289] Among them, v x (t+Δt) is the linear velocity at the next time step, v x where a(t) is the linear velocity at the current time step, a(t) is the acceleration at the current time step, and Δt is the time step length (i.e., “dt”).

[0290] The output u of the PlD controller v Applied to the current linear velocity v x Compensate for speed:

[0291] v′ x =v x +u v

[0292] Among them, v′ x is the line speed after compensation.

[0293] 1. Apply lateral error compensation to the front wheel steering angle δ:

[0294] δ′=δ+u y

[0295] Among them, u θ is the compensation amount calculated according to the yaw angle error.

[0296] S504: Combined with the vehicle's dynamics model, the compensated acceleration and front wheel steering angle are limited to ensure that the control amount is within the controllable range of the vehicle and to avoid unstable behavior caused by overcompensation:

[0297] To avoid unstable behavior due to overcompensation, the velocity and angular velocity need to be limited:

[0298] v′ x ′=clamp(v′ x ,0,v max )

[0299] δ″=clamp(δ′,-δ max ,δ max )

[0300] Among them, v max and δ max are the maximum limits of vehicle speed and steering angle respectively, and the clamp function is:

[0301]

[0302] The final speed control command v x ″ and steering angle control command δ″ are output to the vehicle controller to complete vehicle control.

[0303] S505: In summary, the algorithm flow of the entire ROMMPC controller is as follows:

[0304]

[0305] Embodiment 2:

[0306] The present invention provides a vehicle trajectory tracking control system suitable for curved terrain adaptation, which can be used to implement the above-mentioned vehicle trajectory tracking control method suitable for curved terrain adaptation, specifically comprising:

[0307] The environment perception module is used to obtain the point cloud information of the current environment to build a three-dimensional map and obtain the reference trajectory that the vehicle needs to track;

[0308] A key point extraction module, used to select key points of the reference trajectory and fit local ground equations near the key points;

[0309] A state estimation and control building module, used to determine the state quantity and control quantity of the vehicle, and to build a manifold model predictive controller based on the ground equation obtained in step S2;

[0310] A curvature adjustment module, used to adjust the objective function weight of the manifold model predictive controller in combination with the curvature of the current position of the reference trajectory;

[0311] The error compensation and output optimization module is used to calculate the vehicle's current speed error and the lateral error between the vehicle and the reference trajectory, and use a PID controller to compensate for the acceleration and front wheel steering angle output by the manifold model prediction controller.

[0312] Embodiment 3:

[0313] This embodiment provides a terminal device, which includes a processor and a memory, wherein the memory is used to store a computer program, the computer program includes program instructions, and the processor is used to execute the program instructions stored in the computer storage medium. The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. It is the computing core and control core of the terminal, which is suitable for implementing one or more instructions, and is specifically suitable for loading and executing one or more instructions to implement the corresponding method flow or corresponding function; the processor described in the embodiment of the present invention can be used for an operation applicable to a curved terrain adaptive vehicle trajectory tracking control method, including the following steps:

[0314] S1: Obtain the point cloud information of the current environment to build a three-dimensional map and obtain the reference trajectory that the vehicle needs to track;

[0315] S2: Select key points of the reference trajectory and fit local ground equations near the key points;

[0316] S3: Determine the state and control quantity of the vehicle, and construct a manifold model predictive controller based on the ground equation obtained in step S2;

[0317] S4: Adjust the objective function weight of the manifold model predictive controller in combination with the curvature of the current position of the reference trajectory;

[0318] S5: Calculate the current speed error of the vehicle and the lateral error between the vehicle and the reference trajectory, and use the PID controller to compensate the acceleration and front wheel steering angle output by the manifold model prediction controller.

[0319] Embodiment 4:

[0320] This embodiment provides a storage medium, specifically a computer-readable storage medium (Memory), which is a memory device in a terminal device for storing programs and data. It is understandable that the computer-readable storage medium here can include both the built-in storage medium in the terminal device and the extended storage medium supported by the terminal device. The computer-readable storage medium provides a storage space, which stores the operating system of the terminal. In addition, one or more instructions suitable for being loaded and executed by the processor are also stored in the storage space, and these instructions can be one or more computer programs (including program codes). It should be noted that the computer-readable storage medium here can be a high-speed RAM memory or a non-volatile memory, such as at least one disk memory.

[0321] The processor may load and execute one or more instructions stored in a computer-readable storage medium to implement the corresponding steps of the above-mentioned embodiment related to a vehicle trajectory tracking control method applicable to curved terrain adaptiveness; the processor may load and execute the following steps of one or more instructions in the computer-readable storage medium:

[0322] S1: Obtain the point cloud information of the current environment to build a three-dimensional map and obtain the reference trajectory that the vehicle needs to track;

[0323] S2: Select key points of the reference trajectory and fit local ground equations near the key points;

[0324] S3: Determine the state and control quantity of the vehicle, and construct a manifold model predictive controller based on the ground equation obtained in step S2;

[0325] S4: Adjust the objective function weight of the manifold model predictive controller in combination with the curvature of the current position of the reference trajectory;

[0326] S5: Calculate the current speed error of the vehicle and the lateral error between the vehicle and the reference trajectory, and use the PID controller to compensate the acceleration and front wheel steering angle output by the manifold model prediction controller.

[0327] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented 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.

[0328] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0329] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0330] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0331] The preferred embodiments of the present invention are described in detail above, but the present invention is not limited to the above embodiments, and various changes can be made within the knowledge scope of ordinary technicians in this field without departing from the purpose of the present invention.

[0332] Many other changes and modifications may be made without departing from the concept and scope of the present invention.It should be understood that the present invention is not limited to the specific embodiments, and the scope of the present invention is defined by the appended claims.

Claims

1. A method for adaptive vehicle trajectory tracking control applicable to curved terrain, characterized in that: The method comprises: S1: Obtain the point cloud information of the current environment to build a three-dimensional map and obtain the reference trajectory that the vehicle needs to track; S2: Select key points of the reference trajectory and fit local ground equations near the key points; S3: Determine the state and control quantity of the vehicle, and construct a manifold model predictive controller based on the ground equation obtained in step S2; S4: adjusting the objective function weight of the manifold model predictive controller in combination with the curvature of the current position of the reference trajectory; S5: Calculate the current speed error of the vehicle and the lateral error between the vehicle and the reference trajectory, and use the PID controller to compensate the acceleration and front wheel steering angle output by the optimized manifold model prediction controller.

2. The method for adaptive vehicle trajectory tracking control applicable to curved terrain according to claim 1, characterized in that: The step S2 comprises: uniformly selecting a plurality of key points on the reference trajectory, including an initial point, an end point, and each key point on the path of the trajectory; At each key point, the distance between the key point and the adjacent key point is taken as the radius R, and the point cloud data within the radius is extracted; Use polynomial fitting or spline interpolation surface fitting algorithm to perform surface fitting on the extracted point cloud data and establish the local ground equation; Evaluate the fitted ground equations to ensure that the fitted surface accurately describes the terrain features around the key points, and adjust the fitting parameters as needed; The fitted ground equations are used as the input of the local terrain model and fed into the manifold model predictive controller.

3. The method for adaptive vehicle trajectory tracking control applicable to curved terrain according to claim 1, characterized in that: The step S2 specifically includes: S201: uniformly select multiple key points on the reference trajectory, including the initial point, the end point of the trajectory, and each key point on the path: The reference trajectory is represented as a set of discrete trajectory points {P1,P2,…,P n }, where each trajectory P i Contains its two-dimensional coordinates (x i ,y i ), the key points are selected by evenly spaced selections or dynamically adjusted according to the complexity of the terrain; S202: At each key point, the distance between the key point and the adjacent key point is used as the radius R, and the point cloud data within the radius is extracted: For each selected key point P i =(x i ,y i ), define a neighborhood range of radius R: Among them, ∥P i -P i+1 ∥ is the key point P i Its adjacent key point P i+1 The Euclidean distance between them is used to extract the corresponding point cloud data {Q j }Perform surface fitting; S203: Use polynomial fitting, spline interpolation or other surface fitting algorithms to perform surface fitting on the extracted point cloud data and establish a local ground equation, including: Terrain parameter fitting: For the extracted point cloud data {Q j =(x j ,y j ,z j )} to perform surface fitting, assuming that the ground can be represented by a quadratic polynomial: z=F(x,y)=a0+a1x+a2y+a3x 2 +a4xy+a5y 2 The fitting problem is transformed into minimizing the sum of squares of the following residuals: The optimization problem is solved by using optimization libraries such as Ceres to obtain the fitting parameters p = [a0, a1, a2, a3, a4, a5] T ; Definition of terrain gradient: Terrain Gradient Represents the partial derivative of terrain height z with respect to x and y on the plane (x, y), that is Compute terrain gradients by fitting parameters: According to the fitted surface equation, the terrain gradient g is expressed as a linear combination of the fitting parameters: Therefore, the terrain gradient g is expressed as: The derivative of the terrain gradient g with respect to the fitting parameter p is expressed as: S204: Evaluate the fitted ground equations to ensure that the fitted surface can accurately describe the terrain features around the key points, and adjust the fitting parameters as needed: The quality of the fitted surface is evaluated by calculating the mean square error (MSE): in, is the estimated value of the fitted surface, N is the number of point clouds, if MSE exceeds the predetermined threshold, the fitting parameters or model need to be adjusted; S205: The fitted ground equation is used as the input of the local terrain model and input to the manifold model prediction controller: The fitted parameters p = [a0, a1, a2, a3, a4, a5] T The input is sent to the vehicle's manifold model predictive controller to update the local terrain model. The manifold model predictive controller will use the fitted parameters to perform trajectory prediction and control in complex terrain to solve for the optimal control input.

4. The method for adaptive vehicle trajectory tracking control applicable to curved terrain according to claim 1, characterized in that: The step S3 comprises: S301: determining the position, speed and plane rotation matrix of the vehicle as state quantities of the manifold model predictive controller; determining the longitudinal acceleration and front wheel steering angle of the vehicle as control quantities of the manifold model predictive controller; S302: Establishing a state transfer equation of a manifold model prediction controller based on a dynamic model of the vehicle and the local ground equation; S303: constructing an objective function by comprehensively considering the path tracking error, the change of the control input and the dynamic constraints of the vehicle; S304: Solve the objective function using a numerical optimization method to obtain the optimal longitudinal acceleration and front wheel turning angle of the vehicle at the current moment.

5. The method for adaptive vehicle trajectory tracking control applicable to curved terrain according to claim 1, characterized in that: The step S3 specifically includes: S301: Determine the position, speed and plane rotation matrix of the vehicle as the state variables of the manifold model prediction controller; determine the longitudinal acceleration and front wheel steering angle of the vehicle as the control variables of the manifold model prediction controller: Definition of state quantity: The position coordinates of the vehicle on the plane are defined as The speed of the vehicle on the plane is defined as The vehicle’s plane rotation matrix is ​​defined as R∈SO(2); The vehicle state x is expressed as: Definition of control volume: The longitudinal acceleration of the vehicle along the body axis is defined as The front wheel turning angle of the vehicle is defined as The control quantity u of the vehicle is expressed as: Defining a composite manifold Its dimensions Vehicle status Vehicle control volume S302: Based on the vehicle's dynamic model and local ground equations, a state transfer equation of the manifold model prediction controller is established to ensure that the state quantity can accurately predict the movement of the vehicle on the curved terrain: Define x b (P,R) is the x-coordinate axis of the vehicle body, is the vehicle speed along the body axis, is the yaw angular velocity of the vehicle around the z-axis; Consider a n-manifold The robot system on , which is discretized with a sampling period Δt, is as follows: Considering the movement of the vehicle on the curved terrain, the state transfer equation is established as follows: Among them, α and β are based on the terrain gradient The calculated coefficients are: Where, e1=[1,0] T is the unit direction vector; The linearization error equation is derived: The coefficient and The calculation is as follows: After derivation, the partial derivative of the state transfer function and and The calculation is as follows: The calculation methods of various parameters are as follows: where e2 = [0,1] T ; S303: Considering the path tracking error, the change of control input and the dynamic constraints of the vehicle, an objective function is constructed: When constructing the objective function of the manifold model predictive controller, the path tracking error, the change of control input and the constraints of vehicle dynamics are comprehensively considered; Construction of the objective function: The objective function J is defined as the weighted sum of path tracking error, control input change, and terminal state error: in: N is the controller prediction step size; x k is the state of the vehicle at time k; is the target state (the state on the reference trajectory); u k is the control input of the vehicle at time k; is the target control input (the control input corresponding to the reference trajectory); Q k , R k and P N are the weight matrices of state error, control input error, and terminal state error respectively; State transition equations and constraints: The state transition equation of the vehicle is based on its dynamic model: The initial state is: x0=x init The constraints controlling the input are: u k ∈U,k=0,…,N-1 Where U represents the allowable range of control input; The error dynamics are linearized and transformed into a QP problem: Using the linearized single-step error state equation derived in step S302: The error state equation of step N is further derived: δX=MδU+Mδx0 in: The optimization of the objective function is transformed into a standard quadratic programming problem QP: δU∈[δU min ,δU max ] δx0=δx init in: S304: Solve the objective function using numerical optimization methods: Use the QP solver to effectively solve and obtain the optimal control input increment in each control cycle This enables rolling optimization control.

6. The method for adaptive vehicle trajectory tracking control applicable to curved terrain according to claim 1, characterized in that: The step S4 comprises: S401: Calculate the curvature of the reference track at the current position of the vehicle on the reference track to obtain curvature information of the current driving path of the vehicle; S402: dynamically adjusting the weight coefficient in the objective function of the manifold model predictive controller according to the calculated curvature information, increasing the weight of the path tracking error term when the curvature is large; and balancing the weight of the control input change term when the curvature is small; S403: Input the adjusted objective function into the optimization process of the manifold model predictive controller, and update the objective function weight in real time in subsequent control cycles to adapt to dynamically changing terrain and trajectory characteristics.

7. The method for adaptive vehicle trajectory tracking control applicable to curved terrain according to claim 1, characterized in that: The step S5 comprises: S501: obtaining the current speed of the vehicle in real time, calculating the deviation from the target speed, obtaining the current position of the vehicle in real time, and calculating the distance between the current position of the vehicle and the nearest point of the reference trajectory as the lateral error; S502: Design a PID controller based on the lateral error and speed error of the vehicle, and adjust the proportional gain and integral gain of the controller; S503: In each control cycle, the output of the PID controller is calculated, and the longitudinal acceleration and the front wheel steering angle generated by the manifold model prediction controller are compensated in real time according to the output value to correct the driving direction and acceleration / deceleration behavior of the vehicle; S504: Based on the vehicle's dynamics model, limiting the compensated acceleration and front wheel steering angle is performed.

8. A vehicle trajectory tracking control system suitable for curved terrain adaptation, characterized in that: The system comprises: The environment perception module is used to obtain the point cloud information of the current environment to build a three-dimensional map and obtain the reference trajectory that the vehicle needs to track; A key point extraction module, used to select key points of the reference trajectory and fit local ground equations near the key points; A state estimation and control building module, used to determine the state quantity and control quantity of the vehicle, and to build a manifold model predictive controller based on the ground equation obtained in step S2; A curvature adjustment module, used to adjust the objective function weight of the manifold model predictive controller in combination with the curvature of the current position of the reference trajectory; The error compensation and output optimization module is used to calculate the vehicle's current speed error and the lateral error between the vehicle and the reference trajectory, and use a PID controller to compensate for the acceleration and front wheel steering angle output by the manifold model prediction controller.

9. A computer device, characterized in that: The invention comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, a vehicle trajectory tracking control method applicable to curved terrain adaptive vehicle according to any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, which, when executed by a processor, implements a vehicle trajectory tracking control method suitable for curved terrain adaptation according to any one of claims 1 to 7.

Images