A curved surface terrain adaptive vehicle trajectory tracking control method and system

Abstract

The application discloses a kind of suitable for curved surface terrain adaptive vehicle trajectory tracking control method and system, comprising the following steps: S1.current environment point cloud map and reference trajectory are obtained.S2.select key point on reference trajectory, and fit local ground equation around these key points.S3.with the position, speed and yaw angle of vehicle as state quantity, with the longitudinal acceleration and front wheel angle of vehicle as control quantity, establish manifold model predictive controller.S4.combining the curvature of reference trajectory current position, adjust the objective function weight of manifold model predictive controller.S5.combining vehicle current speed and vehicle and reference trajectory lateral error, using PID controller to compensate the acceleration and front wheel angle of controller output.The method proposed in the application, compared with existing trajectory tracking method, can significantly improve the trajectory tracking accuracy and control robustness of vehicle on complex curved surface terrain.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of vehicle trajectory tracking and control, and particularly relates to a self-adaptive vehicle trajectory tracking control method and system suitable for curved terrain. BACKGROUND

[0002] With the rapid development of automatic driving and unmanned driving technologies, the ability of vehicles to autonomously drive in various complex environments becomes particularly important. Especially on non-flat curved terrains such as mountainous areas, rural roads, forest rescue, and mining transportation, the trajectory tracking accuracy and robustness of the control system directly affect the safety and stability of driving. However, existing trajectory tracking methods are mostly designed based on flat terrain, ignoring key factors such as slope, lateral inclination, and height changes of curved terrain, which leads to insufficient trajectory tracking accuracy and easy deviation from the predetermined path when the vehicle drives on uneven terrain, increasing dynamic instability and safety risks.

[0003] In addition, the unpredictability of complex terrain poses higher requirements for the robustness of the control system. Existing model predictive control (MPC) methods often struggle to cope with the challenges brought by dramatic changes in terrain when dealing with complex curved terrain. In particular, when the vehicle encounters sharp turns, steep slopes, or irregular terrain heights, traditional control algorithms cannot effectively adapt to these changes, which may lead to control failure or deviation of the vehicle 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 application range of autonomous driving technology. SUMMARY

[0004] The present application aims to provide a self-adaptive vehicle trajectory tracking control method and system suitable for curved terrain, which accurately fits the terrain of the vehicle's driving path and combines with manifold model predictive control (MPC) method to provide a vehicle trajectory tracking solution suitable for complex curved terrain.

[0005] To solve the technical problems, the technical solution of the present application is:

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

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

[0008] S2: Select key points on the reference trajectory and fit a local ground equation near the key points;

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

[0010] S4: Adjust the target 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 a PID controller to compensate for the acceleration and front wheel steering angle output by the optimized manifold model predictive controller.

[0012] Further, the step S2 comprises:

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

[0014] At each key point, taking the distance between the key point and the adjacent key point as the radius R, and extracting the point cloud data within the radius range;

[0015] Using a polynomial fitting or spline interpolation surface fitting algorithm to perform surface fitting on the extracted point cloud data, and establishing a local ground equation;

[0016] Evaluating the fitted ground equation to ensure that the fitted surface can accurately describe the terrain features around the key point, and adjusting the fitting parameters as needed;

[0017] Taking the fitted ground equation as the input of the local terrain model and inputting it into the manifold model predictive controller.

[0018] Further, the step S2 specifically comprises:

[0019] S201: Uniformly selecting a plurality of key points on the reference trajectory, including the initial point, the end point and each key point on the path:

[0020] The reference trajectory is represented as a series of discrete trajectory point sets {P1, P2, …, P n}, wherein each trajectory P i contains its two-dimensional coordinates (x i , y i ), and the key points are selected by uniform spacing or dynamically adjusted according to the complexity of the terrain;

[0021] S202: At each key point, taking the distance between the key point and the adjacent key point as the radius R, and extracting the point cloud data within the radius range:

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

[0023]

[0024] where, ||P i -P i+1 || is the Euclidean distance between the key point P i and its adjacent key point P i+1 , in this neighborhood range, the corresponding point cloud data {Q j} is extracted to perform surface fitting;

[0025] S203: using polynomial fitting, spline interpolation or other surface fitting algorithms, the extracted point cloud data is fitted to establish a local ground equation, including:

[0026] Terrain parameter fitting:

[0027] The extracted point cloud data {Q j = (x j , y j , z j )} is fitted, and the ground can be represented by a quadratic polynomial:

[0028] z = F(x, y) = a0 + a1x + a2y + a3x 2 +a4xy + a5y 2

[0029] The fitting problem is converted into minimizing the square sum of the following residual:

[0030]

[0031] This optimization problem is solved by optimization libraries such as Ceres, and the fitting parameters p = [a0, a1, a2, a3, a4, a5] T are obtained.

[0032] Definition of terrain gradient:

[0033] The terrain gradient represents the partial derivative of the terrain height z with respect to x and y in the plane (x, y), that is,

[0034]

[0035] The terrain gradient is calculated by the fitting parameters:

[0036] According to the fitting surface equation, the terrain gradient g is represented 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 equation to ensure that the fitted surface accurately describes 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] wherein, is the estimated value of the fitted surface, N is the number of point clouds, and if the MSE exceeds a predetermined threshold, the fitting parameters or model need to be adjusted;

[0046] S205: Input the fitted ground equation as the input of the local terrain model into the manifold model predictive controller:

[0047] The fitted parameters p = [a0, a1, a2, a3, a4, a5] T are input into the manifold model predictive controller of the vehicle to update the local terrain model, and the manifold model predictive controller will use the fitted parameters to perform trajectory prediction and control in complex terrain to solve the optimal control input.

[0048] Further, the step S3 comprises:

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

[0050] S302: Based on the vehicle dynamics model and the local ground equation, establish the state transition equation of the manifold model predictive controller;

[0051] S303: Consider the path tracking error, change of control input and vehicle dynamics constraints to construct the objective function;

[0052] S304: Use numerical optimization method to solve the objective function to obtain the optimal longitudinal acceleration and front wheel steering angle of the vehicle at the current time.

[0053] Further, the step S3 specifically comprises:

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

[0055] Definition of state quantity:

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

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

[0058] The plane rotation matrix of the vehicle is defined as R∈SO(2);

[0059] The state quantity x of the vehicle is represented as:

[0060]

[0061] Definition of control quantity:

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

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

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

[0065]

[0066] Define the composite manifold Its dimension Vehicle state Vehicle control quantity

[0067] S302: Based on the dynamics model of the vehicle and the local ground equation, establish the state transition equation of the manifold model predictive controller to ensure that the state quantity can accurately predict the motion of the vehicle on the curved terrain:

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

[0069] Consider a robot system located on an n-manifold , which is discretized with a sampling period Δt, 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] Where, e1=[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 e2 = [0, 1] T ;

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

[0086] 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 are comprehensively considered;

[0087] Construction of the objective function:

[0088] The objective function J is defined as the weighted sum of the path tracking error, the control input change, and the 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 (state on the reference trajectory);

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

[0095] is the target control input (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, respectively; state transition equation and constraints:

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

[0098]

[0099] The initial state is:

[0100] x0= x init

[0101] The constraint of the control input is:

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

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

[0104] The linearized error dynamics is converted into a QP problem:

[0105] The linearized one-step error state equation derived in step S302 is used:

[0106]

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

[0108] δX = MδU + Hδx0

[0109] where:

[0110]

[0111]

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

[0113]

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

[0115] δx0=δx init

[0116] wherein:

[0117]

[0118]

[0119] S304: solving the objective function by using a numerical optimization method: effectively solved by a QP solver, obtaining the optimal control input increment in each control period Thus, the rolling optimization control is realized.

[0120] Further, the step S4 comprises:

[0121] S401: according to the current position of the vehicle on the reference trajectory, calculating the curvature of the reference trajectory at this position, obtaining the curvature information of the current driving path of the vehicle;

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

[0123] S403: inputting the adjusted objective function into the optimization process of the manifold model predictive controller, and updating the objective function weight in real time in the subsequent control period to adapt to the dynamic changes of the terrain and the trajectory characteristics.

[0124] Further, the step S5 comprises:

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

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

[0127] S503: in each control period, calculating the output of the PID controller, and according to the output value, real-time compensation is performed on the longitudinal acceleration and front wheel steering angle generated by the manifold model predictive controller, to correct the driving direction and acceleration / deceleration behavior of the vehicle;

[0128] S504: Limit the compensation acceleration and the front wheel steering angle based on the vehicle dynamics model.

[0129] A self-adaptive vehicle trajectory tracking control system for curved surface terrain, the system comprising:

[0130] An environment perception module for obtaining point cloud information of the current environment to establish a three-dimensional map and obtaining a reference trajectory to be tracked by the vehicle;

[0131] A key point extraction module for selecting key points on the reference trajectory and fitting a local ground equation around the key points;

[0132] A state estimation and control construction module for determining state variables and control variables of the vehicle, and constructing a manifold model predictive controller based on the ground equation obtained in step S2;

[0133] A curvature adjustment module for adjusting the target function weight of the manifold model predictive controller in combination with the curvature of the current position of the reference trajectory;

[0134] An error compensation and output optimization module for calculating the current speed error of the vehicle and the lateral error of the vehicle and the reference trajectory, and using a PID controller to compensate the acceleration and the front wheel steering angle output by the manifold model predictive controller.

[0135] A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor executes the program to implement the self-adaptive vehicle trajectory tracking control method for curved surface terrain according to any one of claims 1 to 7.

[0136] A computer readable storage medium having a computer program stored thereon, wherein the program is executable on a processor to implement the self-adaptive vehicle trajectory tracking control method for curved surface terrain according to any one of claims 1 to 7.

[0137] Compared with the prior art, the self-adaptive vehicle trajectory tracking control method for curved surface terrain has the following advantages:

[0138] By selecting key points on the reference trajectory and fitting a local ground equation around the key points, the self-adaptive vehicle trajectory tracking control method for curved surface terrain can accurately describe the slope, roll and height changes of complex curved surface terrain. Compared with the traditional planar terrain trajectory tracking algorithm, the self-adaptive vehicle trajectory tracking control method for curved surface terrain improves the trajectory tracking accuracy of the vehicle on uneven terrain and effectively reduces the tracking error caused by terrain changes.

[0139] The application dynamically adjusts the target function weight in the manifold model predictive control (OMMPC) to adapt to the curvature change of the track position where the vehicle is currently located. This design enhances the robustness of the control system under different terrain conditions, especially on sharp curves or terrain with severe changes, ensuring that the vehicle can more accurately track the reference trajectory.

[0140] The application combines the current speed error and lateral error of the vehicle, and uses a PID controller to compensate the acceleration and front wheel steering angle output by the manifold model predictive controller in real time. Through this integrated compensation mechanism, the control accuracy and driving stability of the vehicle are further improved, especially in complex terrain, effectively avoiding the risk of side slip and trajectory deviation. BRIEF DESCRIPTION OF DRAWINGS

[0141] Figure 1 The robust manifold model predictive control method flowchart proposed by the application;

[0142] Figure 2 The vehicle driving on a curved surface is shown in the schematic diagram.

[0143] Figure 3 The local ground equation is fitted around the reference trajectory, as shown in the schematic diagram.

[0144] Figure 4 The robust manifold model predictive control framework proposed by the application. DETAILED DESCRIPTION

[0145] The specific embodiments of the application will be described below in conjunction with the embodiments:

[0146] It should be noted that the structures, proportions, sizes, etc. shown in the specification are only used to cooperate with the content disclosed in the specification, so that those skilled in the art can understand and read, and are not used to limit the implementation conditions of the application. Any modification of structure, change of proportion relationship or adjustment of size, which does not affect the effect and purpose that can be achieved by the application, should still fall within the scope of the technical content disclosed by the application.

[0147] At the same time, the terms such as "up", "down", "left", "right", "middle" and "one" in the specification are only for the convenience of clear description, and are not used to limit the scope of the application. The change or adjustment of the relative relationship, without substantially changing the technical content, is also considered as the implementation scope of the application.

[0148] Example 1:

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

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

[0151] During the driving of the vehicle, first, the point cloud data of the environment around the vehicle is collected by a SLAM (simultaneous localization and mapping) system or other sensors such as laser radars and cameras. After filtering, downsampling and other preprocessing steps, a three-dimensional point cloud map of the current environment is generated.

[0152] The system receives the reference trajectory provided by the upper planner or the target path of the vehicle driving preset by the operator. The reference trajectory provides path information for the subsequent steps and is the basis for the manifold model predictive controller.

[0153] S2: Select key points on the reference trajectory and fit a local ground equation around the key points, as shown in the process schematic Figure 3 .

[0154] S201: Select several key points on the reference trajectory uniformly, including the initial point, the end point and each key point on the path:

[0155] In actual application, the reference trajectory can be represented as a series of discrete trajectory point sets {P1, P2,..., P n}, where each trajectory P i contains its two-dimensional coordinates (x i , y i ). The selection of key points can be made by uniform spacing or dynamically adjusted 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 taken as the radius R, and the point cloud data within the radius range is extracted:

[0157] For each selected key point P i = (x i , y i ), a neighborhood range with a radius R is defined:

[0158]

[0159] where ||P i -P i+1 || is the Euclidean distance between the key point P i and its adjacent key point P i+1 , and within this neighborhood range, the corresponding point cloud data {Q j} is extracted for surface fitting.

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

[0161] 1. Terrain parameter fitting

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

[0163] z = F(x, y) = a0 + a1x + a2y + a3x 2 +a4xy + a5y 2

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

[0165]

[0166] This optimization problem can be solved by optimization libraries such as Ceres, etc., to obtain the fitting parameters p = [a0, a1, a2, a3, a4, a5] T .

[0167] 2. Definition of terrain gradient

[0168] The terrain gradient represents the partial derivative of the terrain height z with respect to x and y in the plane (x, y), that is,

[0169]

[0170] 3. Calculate the terrain gradient through the fitting parameters

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

[0172]

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

[0174]

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

[0176]

[0177] S204: Evaluate the fitted ground equation 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] wherein, is the estimated value of the fitted surface, and N is the number of points in the point cloud. If the MSE exceeds a predetermined threshold, the fit can be inaccurate, and the parameters or model of the fit need to be adjusted.

[0181] S205: The fitted ground equation is taken as an input of the local terrain model, and is input into the manifold model predictive controller:

[0182] The fitted parameters p = [a0, a1, a2, a3, a4, a5] T are input into the manifold model predictive controller of the vehicle to update the local terrain model. The manifold model predictive controller will use these parameters to perform trajectory prediction and control 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 position, velocity, and yaw angle of the vehicle as state variables, and the longitudinal acceleration and front wheel steering angle of the vehicle as control variables.

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

[0185] Definition of state variables:

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

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

[0188] The plane rotation matrix of the vehicle is defined as R ∈ SO(2),

[0189] The state variable x of the vehicle can be expressed as:

[0190]

[0191] 2. Definition of control variables:

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

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

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

[0195]

[0196] Define the complex manifold The dimension of which is The state of the vehicle The control amount of the vehicle

[0197] S302: Based on the dynamics model of the vehicle and the local ground equation, the state transition equation of the manifold model predictive controller is established to ensure that the state amount can accurately predict the motion of the vehicle on the curved surface terrain:

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

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

[0200]

[0201] Considering the motion of the vehicle on the curved surface terrain, the state transition equation is established as follows:

[0202]

[0203] Where α and β are coefficients calculated based on the terrain gradient :

[0204]

[0205] Where e1 = [1, 0] T is the unit direction vector.

[0206] The linearized error equation is derived as:

[0207]

[0208] Where the coefficients and are calculated as follows:

[0209]

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

[0211]

[0212]

[0213] Where each parameter is calculated as follows:

[0214]

[0215] Where e2 = [0, 1] T

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

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

[0218] 1. Construction of the target function:

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

[0220]

[0221] Where:

[0222] N is the controller prediction step,

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

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

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

[0226] is the target control input (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 transition equation and constraints:

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

[0230]

[0231] The initial state is:

[0232] x0= x init

[0233] The constraints of the control input are:

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

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

[0236] 3. Linearized error dynamics and converted into a QP problem:

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

[0238]

[0239] Further derivation of the N-step error state equation:

[0240] δX = MδU + Hδx0

[0241] where:

[0242]

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

[0244]

[0245] s.t. δU ∈ [δU min , δU max ]

[0246] δx0= δx init

[0247] where:

[0248]

[0249]

[0250] S304: Solve the objective function using numerical optimization methods: effectively solved by existing QP solvers, obtaining the optimal control input increment in each control period Thus realizing the rolling optimization control.

[0251] S4: Adjust the weight of the target function of the manifold model predictive controller according to the curvature of the current position of the reference trajectory.

[0252] S401: Calculate the curvature of the reference trajectory at the current position of the vehicle on the reference trajectory according to the current position of the vehicle on the reference trajectory, and obtain the curvature information of the current driving path of the vehicle:

[0253] Curvature is an important geometric quantity that describes the degree of bending of a path. In the process of driving a vehicle, it is necessary to calculate the curvature at the current position 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 and θ 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 containing 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. The formula for calculating the curvature C k is:

[0262]

[0263] This curvature value represents the degree of bending of the path at the current position. If the calculated curvature is an invalid value (such as NaN or infinity), use the last valid curvature value as the current curvature.

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

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

[0266] Where R0 is the initial weight matrix. The greater the curvature, the greater the weight adjustment, thus paying more attention to the minimization of path tracking error; when the curvature is small, the controller pays more attention to smoothness and stability.

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

[0268] S5: calculate the vehicle current speed error 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: acquire the current speed of the vehicle in real time, calculate the deviation from the target speed, acquire the current position of the vehicle in real time, and calculate the distance between the current position of the vehicle and the nearest point on the reference trajectory as the lateral error:

[0270] 1. Speed error calculation:

[0271] The current speed of the vehicle v x is perceived by the actual speed. The target speed v target is a value extracted from the reference trajectory by the trajectory analyzer. The speed error e v is expressed as:

[0272] e v = v target - v x

[0273] Where v target is the reference speed and v x is the current actual speed.

[0274] 2. Lateral error calculation:

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

[0276] e y = ||p current - p ref ||. ⊥

[0277] Where ||·||. ⊥ represents the perpendicular distance calculation.

[0278] S502: Based on the vehicle's lateral error and speed error, design a PID controller and adjust the controller's proportional gain and integral gain:

[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 angle generated by the manifold model predictive controller are compensated in real time based on 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 of 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 vx The speed is compensated as follows:

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

[0292] where v′ x is the compensated linear velocity.

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

[0294] δ′ = δ + u y

[0295] where u θ is the compensation calculated from the yaw angle error.

[0296] S504: Combine the vehicle's dynamics model to limit the compensated acceleration and front wheel steering angle, ensure that the control quantity is within the controllable range of the vehicle, avoid unstable behavior caused by excessive compensation:

[0297] In order to avoid unstable behavior caused by excessive compensation, the speed and angular velocity need to be limited:

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

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

[0300] where v max and δ max are the maximum limit values 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: Based on the above, the algorithm flow of the entire ROMMPC controller is as follows

[0304]

[0305] Example 2:

[0306] The application provides a curved surface terrain adaptive vehicle trajectory tracking control system which can be used for implementing the curved surface terrain adaptive vehicle trajectory tracking control method.

[0307] An environment perception module is configured to acquire point cloud information of a current environment to establish a three-dimensional map and acquire a reference trajectory to be tracked by the vehicle.

[0308] A key point extraction module is configured to select key points of the reference trajectory and fit a local ground equation near the key points.

[0309] A state estimation and control construction module is configured to determine state quantities and control quantities of the vehicle, and construct a manifold model predictive controller based on the ground equation obtained in step S2.

[0310] A curvature adjustment module is configured to adjust a target function weight of the manifold model predictive controller in combination with a curvature of a current position of the reference trajectory.

[0311] An error compensation and output optimization module is configured to calculate a current speed error of the vehicle and a lateral error of the vehicle relative to the reference trajectory, and use a PID controller to compensate for an acceleration and a front wheel steering angle output by the manifold model predictive controller.

[0312] Embodiment 3

[0313] The embodiment provides a terminal device, which comprises a processor and a memory. The memory is configured to store a computer program, and the computer program comprises program instructions. The processor is configured to execute the program instructions stored in the computer storage medium. The processor can be a central processing unit (CPU), and can also be other general-purpose processors, digital signal processors (DSPs), application specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. The processor is the computing core and control core of the terminal, and is suitable for implementing one or more instructions, and is specifically suitable for loading and executing one or more instructions to implement a corresponding method process or a corresponding function. The processor in the embodiment of the application can be used for the operation of the curved surface terrain adaptive vehicle trajectory tracking control method, which comprises the following steps:

[0314] S1: Acquire point cloud information of a current environment to establish a three-dimensional map, and acquire a reference trajectory to be tracked by the vehicle.

[0315] S2: key point selection is performed on the reference trajectory, and a local ground equation is fitted near the key point;

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

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

[0318] S5: the current speed error of the vehicle and the lateral error of the vehicle and the reference trajectory are calculated, and the acceleration and the front wheel steering angle output by the manifold model predictive controller are compensated using a PID controller.

[0319] Embodiment 4:

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

[0321] The one or more instructions stored in the computer readable storage medium can be loaded and executed by the processor to realize the corresponding steps of the adaptive vehicle trajectory tracking control method for curved terrain in the above embodiment; the one or more instructions in the computer readable storage medium are loaded and executed by the processor as follows:

[0322] S1: point cloud information of a current environment is obtained to establish a three-dimensional map, and a reference trajectory to be tracked by the vehicle is obtained;

[0323] S2: key point selection is performed on the reference trajectory, and a local ground equation is fitted near the key point;

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

[0325] S4: adjust the weight of the target function of the manifold model predictive controller according to the curvature of the current position of the reference trajectory;

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

[0327] Those skilled in the art will understand that embodiments of the present application can be provided as methods, systems, or computer program products. Accordingly, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present application can take the form of a computer program product on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage devices, etc.) embodying computer readable program code.

[0328] The present application is described in reference to the flowcharts and / or block diagrams of the methods, apparatus (systems) and computer program products according to embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, as well as combinations of flows and / or blocks in the flowcharts 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, a special purpose computer, an 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, create means for implementing the functions specified in the flowcharts and / or block diagrams block or blocks. Figure 1 one or more flows and / or blocks Figure 1 means for carrying out the function specified in the flowchart block or blocks.

[0329] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including an instruction means that implements the function specified in the flowcharts and / or block diagrams block or blocks. Figure 1 one or more flows and / or blocks Figure 1 means for carrying out the function specified in the flowchart block or blocks.

[0330] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions executed on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowcharts and / or block diagrams block or blocks. Figure 1 one or more flows and / or blocks Figure 1 means for carrying out the function specified in the flowchart block or blocks.

[0331] The preferred embodiments of the present application have been described in detail above, but the present application is not limited to the above-described embodiments, and various changes can be made within the knowledge of those skilled in the art without departing from the spirit of the present application.

[0332] Many other changes and modifications can be made within the knowledge of those skilled in the art without departing from the concept and scope of the present application. It should be understood that the present application is not limited to a particular embodiment, and the scope of the present application is defined by the appended claims.

Claims

1. A method for adaptive vehicle trajectory tracking control suitable for curved terrain, characterized in that, The method comprises: S1: acquiring point cloud information of a current environment to establish a three-dimensional map, and acquiring a reference trajectory that needs to be tracked by a vehicle; S2: selecting key points on the reference trajectory, and fitting a local ground equation near the key points; 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; at each key point, taking the distance between the key point and the adjacent key point as the radius R, and extracting the point cloud data within the radius range; using a polynomial fitting or spline interpolation surface fitting algorithm to perform surface fitting on the extracted point cloud data to establish a local ground equation; evaluating the fitted ground equation to ensure that the fitted surface can accurately describe the terrain features around the key points, and adjusting the fitting parameters as needed; inputting the fitted ground equation as an input of a local terrain model into a manifold model predictive controller; S3: determining the state quantity and the control quantity of the vehicle, and constructing a manifold model predictive controller based on the ground equation obtained in step S2; S4: adjusting the target function weight of the manifold model predictive controller in combination with the curvature of the current position of the reference trajectory; S5: calculating the current speed error of the vehicle and the lateral error of the vehicle and the reference trajectory, and using a PID controller to compensate the acceleration and the front wheel steering angle output by the optimized manifold model predictive controller.

2. The adaptive vehicle trajectory tracking control method for curved terrain according to claim 1, wherein, The step S2 specifically comprises: S201: 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; 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 ), and the selection of key points is either uniformly spaced or dynamically adjusted according to the complexity of the terrain; S202: at each key point, taking the distance between the key point and the adjacent key point as the radius R, and extracting the point cloud data within the radius range; For each selected key point P i = (x i , y i ), a neighborhood range of radius R is defined: wherein ||P i -P i+1 || is the Euclidean distance between the key point P i and its adjacent key points P i+1 Within this neighborhood range, the corresponding point cloud data {Q j} is extracted for surface fitting; S203: using a surface fitting algorithm to perform surface fitting on the extracted point cloud data to establish a local ground equation, comprising: terrain parameter fitting: The extracted point cloud data {Q j = (x j , y j , z j )} is subjected to surface fitting, and the ground is expressed 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 minimization problem is solved by the Ceres optimization library to obtain the fitting parameters p = [a0, a1, a2, a3, a4, a5] T ; definition of terrain gradient: terrain gradient denotes the partial derivative of the terrain height z with respect to x and y on the plane (x, y), i.e. terrain gradient g is expressed as a linear combination of 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: evaluating the fitted ground equation to ensure that the fitted surface can accurately describe the terrain features around the key points, and adjusting the fitting parameters as needed: the quality of the fitted surface is evaluated by calculating the mean square error MSE: S205: inputting the fitted ground equation as an input of a local terrain model into a manifold model predictive controller. wherein, is an estimated value of the fitting surface, N is the number of point clouds, and if the MSE exceeds a predetermined threshold, the parameters or model of the fitting need to be adjusted; The step S3 comprises: The fitted parameters p = [a0, a1, a2, a3, a4, a5] T are input into the vehicle's manifold model predictive controller to update the local terrain model, which will use the fitted parameters to perform trajectory prediction and control in complex terrain, solving for the optimal control inputs.

3. The adaptive vehicle trajectory tracking control method for curved terrain according to claim 1, wherein, S301: determining the position, speed and vehicle plane rotation matrix of the vehicle as the state quantity of the manifold model predictive controller, and determining the longitudinal acceleration and front wheel steering angle of the vehicle as the control quantity of the manifold model predictive controller; S302: establishing the state transition equation of the manifold model predictive controller based on the dynamics model of the vehicle and the local ground equation; S303: comprehensively considering the path tracking error, the change of the control input and the dynamics constraint of the vehicle to construct a target function; ​ S304: Solve the objective function by using a numerical optimization method to obtain the optimal longitudinal acceleration and front wheel steering angle of the vehicle at the current time.

4. The adaptive vehicle trajectory tracking control method for curved terrain according to claim 1, wherein, The step S3 specifically comprises: S301: Determine the position, speed and vehicle plane rotation matrix of the vehicle as the state quantity of the manifold model predictive controller; determine the longitudinal acceleration and front wheel steering angle of the vehicle as the control quantity of the manifold model predictive controller: Definition of the state quantity: The position coordinates of the vehicle on the plane are defined as The speed of the vehicle in the plane is defined as The plane rotation matrix of the vehicle is defined as R∈SO(2); The state quantity x of the vehicle is represented as: Definition of the control quantity: The longitudinal acceleration of the vehicle in the direction of the vehicle body axis is defined as The front wheel turning angle of the vehicle is defined as The control quantity u of the vehicle is represented as: Defining a complex manifold dimension of which vehicle state vehicle control quantity S302: Based on the vehicle dynamics model and the local ground equation, establish the state transition equation of the manifold model predictive controller to ensure that the state quantity can accurately predict the motion of the vehicle on the curved terrain: Definition of x b (P, R) is the body x coordinate axis, is the vehicle speed along the body axis, is the vehicle yaw rate around the z axis; Consider a robotic system located on an n-manifold which is discretized with a sampling period Δt as follows: Considering the motion of the vehicle on the curved terrain, the state transition equation is established as follows: where a and β are based on terrain gradient Computed coefficients: where e1= [1, 0] T is a unit direction vector; The linearized error equation is derived as follows: where the coefficients and are calculated as follows: By derivation, the partial derivatives of the state transition function and and are computed as follows: Wherein each parameter is calculated as follows: where e2= [0, 1] T ; S303: Considering the path tracking error, the change of the control input and the dynamic constraint of the vehicle, a target function is constructed: When constructing the objective function of the manifold model predictive controller, the path tracking error, the change of the control input and the dynamic constraint of the vehicle are comprehensively considered; Construction of the objective function: Define the objective function J as the weighted sum of the path tracking error, the change of the control input and the terminal state error: Wherein: N is the prediction step of the controller; x k is the state of the vehicle at time k; is the target state, i.e. the state on the reference trajectory; u k is the control input of the vehicle at time k; is the target control input, i.e. the control input corresponding to the reference trajectory; Q k , R k , and P N are weight matrices for state error, control input error, and terminal state error, respectively. State transition equation and constraint: The state transition equation of the vehicle is based on its dynamics model: The initial state is: x0 = x init The constraint of the control input is: u k ∈ U, k = 0,..., N - 1 Wherein, U represents the allowable range of the control input; The linearized error dynamic is converted into a QP problem: Use the linearized single-step error state equation derived in step S302: Further derivation of the N-step error state equation: δX=MδU+Hδx0 Wherein: Convert the optimization of the objective function into a standard quadratic programming problem QP: δU∈[δU min ,δU max ] δx0= δx init Wherein: S304: solving the objective function by numerical optimization method: effectively solved by QP solver, the optimal control input increment in each control period is obtained Thus, the rolling optimization control is realized.

5. The adaptive vehicle trajectory tracking control method for curved terrain according to claim 1, wherein, The step S4 comprises: S401: According to the current position of the vehicle on the reference trajectory, calculate the curvature of the reference trajectory at this position to obtain the curvature information of the current driving path of the vehicle; S402: According to the calculated curvature information, dynamically adjust the weight coefficient in the objective function of the manifold model predictive controller; when the curvature is large, increase the weight of the path tracking error term; when the curvature is small, balance the weight of the control input change term; S403: Input the adjusted objective function into the optimization process of the manifold model predictive controller, and update the weight of the objective function in real time in the subsequent control period to adapt to the dynamically changing terrain and trajectory characteristics.

6. The adaptive vehicle trajectory tracking control method for curved terrain according to claim 1, wherein, The step S5 comprises: S501: Real-time acquisition of the current speed of the vehicle, calculation of the deviation from the target speed, real-time acquisition of the current position of the vehicle, and calculation of the distance between the current position of the vehicle and the nearest point of the reference trajectory as the lateral error; S502: Based on the lateral error and speed error of the vehicle, design a PID controller and adjust the proportional gain and integral gain of the controller; S503: In each control cycle, the output of the PID controller is calculated, the longitudinal acceleration and front wheel steering angle generated by the manifold model predictive controller are compensated in real time according to the output value, and the driving direction and acceleration / deceleration behavior of the vehicle are corrected; S504: In combination with the vehicle dynamics model, the compensated acceleration and front wheel steering angle are limited.

7. A self-adaptive vehicle trajectory tracking control system suitable for curved terrain, characterized in that, The system is used to execute the method of any one of claims 1-6, and the system comprises: An environment perception module is configured to obtain point cloud information of a current environment to establish a three-dimensional map, and obtain a reference trajectory that needs to be tracked by the vehicle; A key point extraction module is configured to select key points of the reference trajectory, and fit a local ground equation near the key points; A state estimation and control construction module is configured to determine state variables and control variables of the vehicle, and construct a manifold model predictive controller based on the ground equation obtained in step S2; A curvature adjustment module is configured to adjust weights of an objective function of the manifold model predictive controller in combination with a curvature of a current position of the reference trajectory; An error compensation and output optimization module is configured to calculate a current speed error of the vehicle and a lateral error of the vehicle relative to the reference trajectory, and use a PID controller to compensate an acceleration and a front wheel steering angle output by the manifold model predictive controller.

8. A computer device, comprising: A computer readable storage medium stores a computer program, and a processor executes the program to implement the method of any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, and a processor executes the program to implement the method of any one of claims 1-6.

Images