A trajectory tracking control algorithm for unmanned vehicle sensor fusion

Abstract

The application discloses a kind of about unmanned vehicle sensor fusion trajectory tracking control algorithm, it is related to unmanned vehicle driving technical field, including the data acquisition and fusion processing of sensor, based on the environmental perception information after fusion, using advanced trajectory prediction algorithm, the future trajectory of unmanned vehicle is predicted;In the process of trajectory tracking, according to the real-time feedback sensor data and vehicle state information, the parameters of trajectory prediction and tracking controller are dynamically adjusted, to improve the accuracy and robustness of trajectory tracking.The application firstly, through real-time sensor data, the dynamic model of vehicle will be constantly updated, so that the controller can more accurately predict the motion state of vehicle.Secondly, by optimizing the controller parameters, including prediction time domain and control time domain parameters and constraint conditions in prediction model, more accurate control of vehicle trajectory is realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned driving, and particularly relates to a trajectory tracking control algorithm for sensor fusion of an unmanned vehicle. BACKGROUND

[0002] The development of unmanned driving technology relies on accurate perception of the surrounding environment and accurate tracking of vehicle trajectories. However, due to the limitations of a single sensor in terms of accuracy, stability, and applicable scenarios, how to effectively fuse data from multiple sensors to improve the accuracy and robustness of trajectory tracking has become a hot topic in current unmanned driving technology research. In recent years, the intelligent vehicle industry has developed rapidly, and an intelligent unmanned vehicle is a comprehensive system that integrates functions such as environment perception, planning and decision-making, and trajectory tracking control. As one of its core functions, many scholars at home and abroad have conducted extensive research on trajectory tracking control. So far, the more mature trajectory tracking control methods include sliding mode control, LQR control, fuzzy control, and model predictive control. Among them, the model predictive control algorithm can consider the prediction process and solve more constraint problems, and is applied to the trajectory tracking control of intelligent unmanned vehicles. SUMMARY

[0003] The present application relates to the technical field of unmanned driving, and particularly relates to a trajectory tracking control algorithm for sensor fusion of an unmanned vehicle.

[0004] A trajectory tracking control algorithm for sensor fusion of an unmanned vehicle, the algorithm comprising:

[0005] Raw data is obtained from sensors such as radar, cameras, and lidar, and the raw data is preprocessed, including denoising, filtering, time synchronization, and coordinate system conversion, to ensure the accuracy and consistency of the data. A multi-sensor data fusion algorithm, such as Kalman filtering, particle filtering, or deep learning algorithm, is used to fuse data from different sensors to generate fused environment perception information; the fused environment perception information includes the position, speed, acceleration of the vehicle, obstacle position, road information, etc.

[0006] Based on the fused environment perception information, an advanced trajectory prediction algorithm is used to predict the future trajectory of the unmanned vehicle;

[0007] During trajectory tracking, the parameters of the trajectory prediction and tracking controller are dynamically adjusted based on real-time feedback of sensor data and vehicle state information to improve the accuracy and robustness of trajectory tracking.

[0008] Among them, the advanced trajectory prediction algorithm includes an adaptive extended Kalman filter algorithm (AEKF) and an adaptive model predictive control algorithm (AMPC).

[0009] As a further technical solution of the present application, the trajectory prediction algorithm selects an adaptive model prediction control algorithm (AMPC), according to the prediction result, a trajectory tracking controller is designed to generate control instructions of the unmanned vehicle, including steering, acceleration and braking, etc., so as to realize accurate tracking of the vehicle trajectory.

[0010] As a further technical solution of the present application, based on the fused environment perception information, an advanced trajectory prediction algorithm is used to predict the future trajectory of the unmanned vehicle, including the following steps:

[0011] Step one, system modeling and parameter derivation: according to the three-degree-of-freedom dynamics model of the vehicle, the tire side slip angle formula and the coordinate transformation, the vehicle dynamics equation is derived as follows:

[0012] ;

[0013] ;

[0014] ;

[0015] ;

[0016] ;

[0017] In the formula: and are the lateral velocity and lateral acceleration, respectively; and are the longitudinal velocity and longitudinal acceleration, respectively; and are the yaw rate and yaw angular acceleration, respectively; is the mass of the vehicle; is the moment of inertia; and are the distances from the mass center point to the front and rear axles, respectively; and are the longitudinal forces of the front and rear wheels, respectively; and are the lateral forces of the front and rear wheels, respectively; and are the longitudinal and lateral displacements of the vehicle in the inertial coordinate system, respectively; and are the longitudinal stiffness of the front and rear wheels, respectively; and are the slip rates of the front and rear wheels, respectively; and are the side slip angles of the front and rear wheels, respectively; and are the side slip stiffness of the front and rear wheels, respectively.

[0018] Step two, prediction model and objective function: According to the vehicle dynamics equation in step one, the continuous state equation of the vehicle dynamics model can be obtained as follows:

[0019] ;

[0020] ;

[0021] In the formula, the state variable , the control variable , the output variable , The front wheel steering angle is denoted by , the state matrix is denoted by , the control matrix is denoted by , and the output matrix is denoted by

[0022] The system is discretized by using the Euler method:

[0023] ;

[0024] ;

[0025] In the formula: , , is the sampling time of the system;

[0026] To prevent the occurrence of no feasible solution, the following transformation is made:

[0027] ;

[0028] A new state space expression is obtained as follows:

[0029] ;

[0030] ;

[0031] Among them, , , , ;

[0032] The output expression of the system in the prediction time domain is as follows:

[0033] ;

[0034] Among them: ;

[0035] ;

[0036] ;

[0037] ;

[0038] To ensure that the vehicle can quickly and stably track the target path, the target function is designed as:

[0039] ;

[0040] In the formula: represents the prediction time domain; represents the control time domain; and are weight matrices, respectively; represents the desired trajectory; represents the weight coefficient; represents the relaxation factor; In the formula reflects the difference between the actual output and the ideal output, which is used to evaluate the control accuracy of the controlled object; represents the degree of output increment, mainly reflecting the reliability of the controlled object. Setting the relaxation factor is to ensure that the quadratic programming problem has an optimal solution. At the same time, considering the kinematics and dynamics characteristics of the actual car, it is necessary to impose some constraints on the state variables and control variables of the system, including:

[0041] ;

[0042] ;

[0043] ;

[0044] ;

[0045] The target function is converted into a standard quadratic programming form with constraints convenient for Matlab software to solve:

[0046] ;

[0047] In the formula, , , represents the tracking error. The increment of the control variable in the control time domain obtained in each optimization period is:

[0048] ;

[0049] The first control variable calculated at the current time in the control increment sequence is taken as the actual control variable output, and the controlled vehicle, that is, the front wheel steering angle, is taken as the controlled vehicle.

[0050] ;

[0051] The selection of model predictive controller parameters will directly affect the tracking effect of the automobile, and the prediction time domain and control time domain parameters are one of the important parameters of the model predictive controller, a time domain parameter adaptive model predictive controller is designed, and the performance thereof is verified.

[0052] Step three, design the optimal prediction time domain and control time domain: since the prediction time domain and control time domain have a greater influence on the control effect of the model predictive controller, in order to find the optimal prediction time domain and control time domain parameters under different constant speeds, the controller time domain parameters are screened, and effective simulation experiments are obtained, in order to improve the tracking precision and driving stability of the automobile trajectory tracking controller, the root mean square value of the automobile lateral displacement error and the root mean square value of the automobile yaw angle and the mass center side slip angle velocity are selected as the indexes for measuring the pros and cons of the trajectory tracking control parameters, and the expression is as follows:

[0053] ;

[0054] In the formula: is the optimal evaluation index; is the actual path; is the reference path; is the automobile yaw rate; is the automobile mass center side slip angle; a, b, b1 and b2 are four different weight factors, and the weight size depends on the attention degree to the tracking precision and driving stability of the automobile.

[0055] The optimal evaluation index value is calculated, and finally the optimal prediction time domain and control time domain corresponding to the model predictive controller under different speeds are obtained, the selected data is fitted by a 4th order polynomial in the Matlab toolbox, and the formula is obtained and put into the controller; the vehicle driving stability mainly includes two parts: the vehicle stability weight and the vehicle steering performance weight, and b2 represents the weight of the vehicle steering performance in the vehicle driving stability.

[0056] The beneficial effects realized by the present application are as follows:

[0057] Model predictive controller plays an important role in trajectory tracking of autonomous vehicles. The present invention adopts an adaptive model predictive control (AMPC) algorithm, aiming to achieve accurate prediction and effective control of vehicle trajectory through real-time updating of vehicle dynamic model and multi-sensor data. The core idea of MPC algorithm is to predict the state of the vehicle in the future based on the current state, and to realize the tracking of the trajectory through the optimization of the controller parameters. The key of the controller design lies in the establishment of the dynamic model and the optimization of the parameters. First of all, through real-time sensor data, the dynamic model of the vehicle will be constantly updated, so that the controller can more accurately predict the motion state of the vehicle. Secondly, through the optimization of the controller parameters, including the prediction horizon and the control horizon parameters and the constraint conditions in the prediction model, more accurate control of the vehicle trajectory is realized. BRIEF DESCRIPTION OF DRAWINGS

[0058] Fig. 1 Flowchart for calculating optimal evaluation index module.

[0059] Fig. 2 Overall flowchart of the method of the present invention;

[0060] Fig. 3 Overall architecture diagram of data transmission of the method of the present invention. DETAILED DESCRIPTION

[0061] The technical solutions of the present invention will be described in detail below in combination with specific drawings.

[0062] Please refer to Figs. 1 to 3 The present invention provides a trajectory tracking control algorithm for unmanned vehicle sensor fusion, which includes:

[0063] 1. Data acquisition and fusion processing of sensors:

[0064] Raw data is obtained from radar, camera and lidar sensors, and preprocessed, including denoising, filtering, time synchronization and coordinate system conversion, to ensure the accuracy and consistency of the data. Multi-sensor data fusion algorithms such as Kalman filter, particle filter or deep learning algorithm are used to fuse the data from different sensors to generate fused environment perception information; the fused environment perception information includes the position, speed, acceleration of the vehicle, obstacle position, road information, etc.

[0065] In the process of trajectory tracking and control of autonomous vehicles, the collection and processing of sensor data are the core elements to maintain the accuracy and safety of the system. This invention adopts a variety of sensor technologies, including high-precision radar, high-definition cameras, and laser radar, to monitor the vehicle's surroundings in all directions. Radar sensors can accurately measure the distance and speed of target objects, laser radar can depict detailed environmental structure information in the form of point clouds, and high-definition cameras provide ample visual information. These sensors capture information in unique ways and are integrated through data fusion technology to form a comprehensive perception of the vehicle's surroundings, laying a solid foundation for subsequent trajectory tracking and control. The fusion of sensor data is a key measure to ensure that the system accurately understands the surrounding environment. By using complex algorithms and models, the data collected by each sensor is integrated and calibrated to eliminate possible errors and inconsistencies. This fusion capability enables the system to more comprehensively grasp the environment, allowing for more accurate planning of vehicle routes and precise control.

[0066] 1. Trajectory prediction and tracking:

[0067] Based on the fused environmental perception information, advanced trajectory prediction algorithms are used to predict the future trajectory of the unmanned vehicle.

[0068] In the trajectory tracking process, according to the real-time feedback of sensor data and vehicle state information, the parameters of the trajectory prediction and tracking controller are dynamically adjusted to improve the accuracy and robustness of trajectory tracking. Sensor data mainly includes high-precision radar, high-resolution cameras, and laser radar to comprehensively perceive the data obtained from the vehicle's surroundings. Radar sensors can provide accurate measurements of target object distance and speed, while laser radar presents detailed environmental structure information in the form of point clouds, and high-resolution cameras provide rich visual data. These sensors capture information in unique ways and are integrated through data fusion technology to form a comprehensive perception of the vehicle's surroundings, providing a solid foundation for subsequent trajectory tracking control. Vehicle state information mainly includes position information, speed information, acceleration and deceleration information, and direction information, etc.

[0069] Among them, advanced trajectory prediction algorithms include adaptive extended Kalman filter algorithm (AEKF) and adaptive model predictive control algorithm (AMPC). In this embodiment, the trajectory prediction algorithm is selected as the adaptive model predictive control algorithm (AMPC). According to the prediction results, a trajectory tracking controller is designed to generate control instructions for the unmanned vehicle, including steering, acceleration, and braking, to achieve accurate tracking of the vehicle's trajectory.

[0070] Step one, system modeling and parameter derivation: according to the three degrees of freedom dynamics model of the vehicle, tire side slip angle formula and coordinate transformation, the vehicle dynamics equation is derived as follows:

[0071] ;

[0072] ;

[0073] ;

[0074] ;

[0075] ;

[0076] In the formula: and are the lateral velocity and lateral acceleration, respectively; and are the longitudinal velocity and longitudinal acceleration, respectively; and are the yaw angular velocity and yaw angular acceleration, respectively; is the mass of the vehicle; is the moment of inertia; and are the distances from the mass center point to the front and rear axles, respectively; and are the longitudinal forces of the front and rear wheels, respectively; and are the lateral forces of the front and rear wheels, respectively; and are the longitudinal displacements of the vehicle in the inertial coordinate system; and are the longitudinal stiffness of the front and rear wheels, respectively; and are the slip ratios of the front and rear wheels, respectively; and are the side slip angles of the front and rear wheels, respectively; and are the side slip stiffness of the front and rear wheels, respectively;

[0077] Step two, prediction model and objective function: according to the vehicle dynamics equation in step one, the continuous state equation form of the vehicle dynamics model is obtained as follows:

[0078] ;

[0079] ;

[0080] In the formula, the state variable , the control variable , and the output variable θf represents a front wheel steering angle, C represents a state matrix, B represents a control matrix, D represents an output matrix;

[0081] The system is discretized using the Euler method:

[0082]

[0083]

[0084] wherein: Ts represents a system sampling time;

[0085] To prevent the occurrence of no feasible solution, the following transformation is made:

[0086]

[0087] A new state space expression is obtained:

[0088]

[0089]

[0090] wherein:

[0091] The output quantity expression of the system in the prediction horizon is:

[0092]

[0093] wherein:

[0094]

[0095]

[0096]

[0097] To ensure that the vehicle can quickly and stably track the target path, the target function is designed as:

[0098]

[0099] wherein: Tf represents a prediction horizon; Tc represents a control horizon;​​​​​​​​​​​​​​​​​​ and are weight matrices respectively; represents the desired trajectory; represents the weight coefficient; represents the relaxation factor; the first term reflects the difference between the actual output and the ideal output, which is used to evaluate the control accuracy of the controlled object, and the first term is , including longitudinal speed, lateral speed, yaw angle, yaw angle acceleration, longitudinal displacement and lateral displacement; the second term represents the degree of output increment, and the second term is , i.e. front wheel angle, mainly reflecting the reliability of the controlled object, setting the relaxation factor is to ensure that the quadratic programming problem has an optimal solution, at the same time, considering the kinematics and dynamics characteristics of the actual vehicle, it is necessary to impose some constraints on the state variables and control variables of the system, including:

[0100] ;

[0101] ;

[0102] ;

[0103] ;

[0104] The objective function is converted into a standard quadratic programming form with constraints convenient for Matlab software to solve:

[0105] ;

[0106] wherein, , , represents the tracking error, and the increment of the control variable in the control time domain obtained in each optimization period is:

[0107] ;

[0108] The first control variable calculated at the current time in the control increment sequence is output as the actual control variable, and the controlled vehicle, i.e. the front wheel angle;

[0109] ;

[0110] The selection of model predictive controller parameters will directly affect the tracking effect of the vehicle, so it is particularly important to find suitable controller parameters. As one of the important parameters of the model predictive controller, the prediction time domain and control time domain parameters have a great influence on the effect of the controller. Therefore, a time domain parameter adaptive model predictive controller is designed, and its performance is verified;

[0111] Step three, design the optimal prediction horizon and control horizon: Since the prediction horizon and control horizon have a greater impact on the model predictive controller control effect, in order to find the optimal prediction horizon and control horizon parameters under different constant speeds, according to the relevant literature and data, etc. Set the vehicle speed, prediction horizon and control horizon as shown in Table 1, and the total simulation experiment group is 600 groups.

[0112] Table 1 Time domain simulation test design

[0113] Parameter Numerical range Step size Vehicle speed (km / h) 36~108 18 Prediction horizon 8~28 1 Control horizon 1~6 1

[0114] In order to ensure the accuracy of trajectory tracking and avoid the occurrence of automobile stability and other problems, the following screening is carried out for the controller time domain parameters. This mainly includes three aspects:

[0115] 1) Select the required prediction horizon and control horizon parameters to prevent the controller from reporting errors;

[0116] 2) Select the appropriate value from the parameters that meet the set constraints to ensure driving safety;

[0117] 3) Select parameters to ensure that the controller can respond in real time, and the required calculation time is less than the data acquisition time.

[0118] According to the above filtering conditions, 442 groups of effective simulation experiments are obtained. In order to improve the tracking accuracy and driving stability of the vehicle trajectory tracking controller, the root mean square value of the vehicle lateral displacement error and the root mean square value of the vehicle yaw angle and mass center side slip angle velocity are selected as the indicators to measure the pros and cons of the trajectory tracking control parameters, and the expression is as follows:

[0119]

[0120] In the formula: is the optimal evaluation index; is the actual path; is the reference path; is the vehicle yaw rate; is the vehicle mass center side slip angle; a, b, b1 and b2 are weight factors, and the weight size depends on the attention degree to the vehicle tracking accuracy and driving stability. After multiple verification simulations, the coefficient weights are finally determined as 0.5, 0.5, 70 and 3. a: represents the weight factor of the root mean square value of the lateral displacement error. It reflects the importance of lateral deviation in the trajectory tracking process. A larger a value means that the controller will pay more attention to reducing the lateral deviation. The formula mainly includes two parts: tracking accuracy weight and vehicle driving stability weight, and a represents the weight of vehicle tracking accuracy in the total.

[0121] ​b: represents a weight factor of another relevant parameter root mean square value, which reflects the importance of vehicle stability in the trajectory tracking process. A larger b value means that the controller will pay more attention to the stability of the vehicle. The formula mainly includes two parts: tracking accuracy weight and vehicle driving stability weight, and b represents the weight of vehicle driving stability in the overall.

[0122] b1: represents a weight factor of the root mean square value of the centroid side slip angle velocity. It reflects the importance of vehicle stability in the trajectory tracking process. A larger b1 value means that the controller will pay more attention to maintaining the stability of the vehicle. Vehicle driving stability mainly includes two parts: vehicle stability weight and vehicle steering performance weight, and b1 represents the weight of vehicle stability in vehicle driving stability.

[0123] b2: represents a weight factor of the root mean square value of the yaw rate. It reflects the importance of vehicle steering performance in the trajectory tracking process. A larger b2 value means that the controller will pay more attention to improving the steering accuracy and response speed of the vehicle. Vehicle driving stability mainly includes two parts: vehicle stability weight and vehicle steering performance weight, and b2 represents the weight of vehicle steering performance in vehicle driving stability.

[0124] The controller simulation test of the present application mainly focuses on considering the trajectory tracking accuracy, and considering that these parameters have different dimensions, an optimal evaluation index calculation module as shown in Fig. 1 is designed, with lateral displacement, longitudinal displacement, yaw rate and centroid side slip angle as input, through lateral error calculation and root mean square value calculation modules, the optimal evaluation index value is calculated, and finally the optimal prediction time domain and control time domain corresponding to the model predictive controller at different speeds are obtained. After screening, the selected data is fitted by 4th order polynomial in Matlab toolbox, and the formula is put into the controller.

[0125] 3. Dynamic adjustment and optimization:

[0126] During the trajectory tracking process, the parameters of the trajectory prediction and tracking controller are dynamically adjusted according to the real-time feedback sensor data and vehicle state information, so as to improve the accuracy and robustness of trajectory tracking.

[0127] In order to better understand the technical scheme of the present application by those skilled in the art, in the specific implementation, the present application can be carried out according to the following steps:

[0128] Data acquisition and preprocessing: The invention focuses on verifying the effectiveness of the proposed sensor fusion-based autonomous vehicle trajectory tracking and control scheme through experimental simulation and road testing. A common autonomous driving simulation platform is selected to simulate diverse road conditions and traffic scenarios, covering urban roads, highways, and complex intersections. The data generated by these simulation experiments, as shown in Table 2,

[0129] Table 2 Simulation data

[0130]

[0131] Sensor data fusion results: The invention successfully obtains rich information from different sensors through the integration of multi-source sensor data fusion technology, and further constructs a comprehensive and accurate environmental perception system. In this system, the data captured by various sensors complement each other, jointly constructing a multi-dimensional, multi-perspective environmental information panoramic map. Specifically, radar sensors provide accurate position and speed information of static and dynamic targets around the vehicle; cameras capture intuitive visual content, including road signs, pedestrians, and vehicle image information; and laser radars describe the structural features of the surrounding environment in the form of high-precision point cloud data. After fine data fusion processing, the invention effectively eliminates the limitations that may exist when using a single sensor, thereby constructing a more comprehensive and accurate environmental perception framework. This fusion result not only demonstrates the system's comprehensive perception ability of environmental information, but also provides a solid foundation for subsequent autonomous vehicle trajectory tracking and control.

[0132] The application of sensor data fusion technology significantly improves the system's comprehensive recognition and in-depth understanding of various target objects in the environment, while achieving accurate tracking of key parameters such as target position and speed. In complex traffic scenarios, the system can accurately identify multiple vehicles, pedestrians, and other obstacles, and capture their position information and movement trajectories in real time. This powerful comprehensive perception ability gives the autonomous driving system more accurate environmental analysis capabilities, enabling it to quickly make decisions and plan reasonable driving trajectories. For subsequent trajectory tracking and vehicle control, this fusion result provides crucial information support. Table 3 details the specific results of sensor data fusion.

[0133] Table 3 Sensor data fusion result display

[0134]

[0135] Model predictive controller parameter optimization analysis: The optimization of the parameters of the model predictive controller is crucial for the trajectory tracking and control of autonomous vehicles. By carefully adjusting and optimizing the parameters of the controller, the invention aims to improve the system's ability to accurately control and predict the vehicle's motion trajectory. The process of optimizing parameters takes into account the dynamic characteristics of the vehicle and environmental changes by continuously adjusting the parameters of the controller. By adjusting the prediction horizon and control horizon parameters of the controller and combining the constraints in model predictive control, the system can significantly improve its ability to adapt to different road conditions and environments, thereby enhancing the accuracy of vehicle trajectory tracking and the stability of control. This optimization strategy not only enables the system to more accurately predict the motion state of the vehicle, but also improves its adaptability to dynamic environmental changes, providing a more solid guarantee for the actual operation of the system.

[0136] However, the key to the parameter optimization process is to find the right balance between the performance and complexity of the controller. Due to the differences in the dynamic characteristics of the vehicle and external interference factors under different environments and road conditions, the optimized parameters may not be universally applicable. In addition, during the parameter optimization process, the impact of controller adjustment on the real-time performance of the system must also be fully considered to ensure that the controller can respond quickly and be practical. Table 4 lists the results of the parameter optimization of the model predictive controller in detail.

[0137] Table 4 Model predictive controller parameter optimization analysis

[0138]

[0139] Road test result analysis: Through the research and verification of the invention, a sensor fusion-based trajectory tracking and control scheme for autonomous vehicles is proposed. In various environments such as urban roads, highways, and complex road intersections, the system has shown excellent trajectory tracking performance. Through comparative analysis of test data, it is found that the trajectory deviation of the system under various test conditions is controlled within a reasonable range, and the control stability is effectively maintained. Especially in the face of complex traffic conditions, the system can still accurately track the expected trajectory, showing high stability and robustness. The above result analysis fully proves that the trajectory tracking and control scheme proposed by the invention based on sensor fusion has achieved remarkable results in practical application and testing, effectively solving the problem of trajectory control of autonomous vehicles in different environments, and providing important technical support for the further promotion and application of autonomous driving technology.

[0140] This invention contains numerous theoretical derivations of Model Predictive Control (MPC) formulas. Some formulas include new letters without specific meaning, used merely to better explain the derivation process. The focus is on helping readers understand the derivation process (for example, to illustrate the increasing order from k-1 to k, from k to k+Np-1, and k+Nc-1; and u(k) represents the value of the control variable at time k, where the control variable is merely a pronoun referring to the steering angle of the car's front wheels). "The symbols represent the change in the control variable from time k-1 to k, sometimes called the increment, and so on; the matrices appearing in the formulas only represent the set of all data for a certain variable from one time to another, and have different meanings in different formulas (they are just pronouns). The pronouns appearing throughout the text are explained at the beginning; for example, the letter 'u' represents the control variable, which is the front wheel angle of a car, but its form may be u(k-1), u(k)..." They are all essentially control quantities, just with different names; These refer to lateral velocity, longitudinal velocity, yaw angle, yaw acceleration, longitudinal displacement, and lateral displacement, respectively, and so on.

[0141] It should be noted that, in this document, the term "comprising" or any other variation thereof is intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element.

[0142] The above are merely preferred embodiments of the present invention and do not limit the scope of the patent. Any equivalent structural or procedural transformations made based on the description and drawings of the present invention, or direct or indirect applications in other related technical fields, are similarly included within the scope of patent protection of the present invention.

Claims

1. A trajectory tracking control algorithm based on sensor fusion for unmanned vehicles, characterized in that, The algorithm includes: Raw data is acquired from sensors, preprocessed, and then a multi-sensor data fusion algorithm is used to fuse data from different sensors to generate fused environmental perception information. The fused environmental perception information includes the vehicle's position, speed, acceleration, obstacle positions, and road information. Based on the fused environmental perception information, a trajectory prediction algorithm is used to predict the future trajectory of the unmanned vehicle; During trajectory tracking, the parameters of the trajectory prediction and tracking controller are dynamically adjusted based on real-time feedback sensor data and vehicle status information to improve the accuracy and robustness of trajectory tracking. The trajectory prediction algorithms employed include adaptive extended Kalman filtering and adaptive model predictive control. Sensor data includes high-precision radar, high-resolution cameras, and lidar. Vehicle state information includes position, speed, acceleration, deceleration, and orientation. Based on the fused environmental perception information, the steps for predicting the future trajectory of the autonomous vehicle using trajectory prediction algorithms include: Step 1: System Modeling and Parameter Derivation: Based on the vehicle's three-degree-of-freedom dynamics model, the tire slip angle formula, and coordinate transformation, the vehicle dynamics equations are derived and simplified as follows: ； ； In the formula: and These are lateral velocity and lateral acceleration, respectively. and These are longitudinal velocity and longitudinal acceleration, respectively. and These are the yaw rate and yaw acceleration, respectively. For car quality; It is the moment of inertia; and These are the distances from the centroid o to the front and rear axes, respectively. and These represent the longitudinal and lateral displacements of the vehicle in the inertial coordinate system, respectively. and These are the longitudinal stiffnesses of the front and rear wheels, respectively. and These are the slip ratios of the front and rear wheels, respectively. and These are the front and rear wheel slip angles, respectively. and These are the lateral stiffness of the front and rear wheels, respectively. Step 2, Prediction Model and Objective Function: Based on the vehicle dynamics equations from Step 1, the continuous state equations of the vehicle dynamics model can be obtained as follows: ； In the formula, the state variable Control quantity Output , Show the front wheel steering angle. Represents the state matrix, Represents the control matrix. Indicates the output matrix; The system is discretized using the Euler method: In the formula: , , This refers to the system sampling time. To prevent situations where no feasible solution exists, the following transformation is made: ； We obtain a new state-space expression: in, ; The system's output in the prediction time domain is expressed as follows: in: ; ； ； The objective function is designed as follows: In the formula: Indicates the prediction time domain; Indicates control over the time domain; and These are the weight matrices; Represents the desired trajectory; Indicates the weighting coefficient; Represents the relaxation factor; in the formula The first item reflects the difference between the actual output and the ideal output, and is used to evaluate the control accuracy of the controlled object. This indicates the degree of output increment. Setting a relaxation factor ensures that the quadratic programming problem has an optimal solution. Simultaneously, some constraints need to be imposed on the system's state and control variables, including: Transform the objective function into a standard quadratic programming form with constraints, which is easier to solve with Matlab software: ； In the formula, , , Representing the tracking error, the increment of the control variable in the control time domain obtained in each optimization cycle is: ； The first control variable calculated at the current moment in the control increment sequence is output as the actual control variable. ； Step 3: Design the optimal prediction and control time domains: Select the root mean square value of the vehicle's lateral displacement error, the root mean square values ​​of the vehicle's yaw angle and the sideslip angular velocity as indicators to evaluate the quality of the trajectory tracking control parameters. The expressions are as follows: ； In the formula: The optimal evaluation index; The actual path; The path for reference; This refers to the yaw rate of the car. The sideslip angle is the angle between the vehicle's center of gravity and its body. , , and These are weighting factors; The optimal evaluation index value is calculated, and the optimal prediction time domain and control time domain corresponding to the model predictive controller at different speeds are finally obtained. After screening, the selected data is fitted and rounded using a fourth-order polynomial in the Matlab toolbox to obtain the formula, which is then put into the controller. This represents the weight of vehicle tracking accuracy in the overall picture. This represents the weight of vehicle driving stability in the overall picture. Vehicle driving stability mainly includes two parts: vehicle stability weight and vehicle steering performance weight. This represents the weight of vehicle stability in vehicle driving stability. This represents the weight of vehicle steering performance in vehicle driving stability.

2. The trajectory tracking control algorithm for unmanned vehicles based on sensor fusion according to claim 1, characterized in that, The trajectory prediction algorithm uses an adaptive model predictive control algorithm. Based on the prediction results, a trajectory tracking controller is designed to generate control commands for the unmanned vehicle.

Images