Automatic driving path tracking control method and system based on linear quadratic regulator
By adopting a path tracking control method based on a linear secondary regulator in the autonomous driving system, the problems of insufficient control accuracy and poor stability in complex road conditions and dynamic environments in the prior art are solved, and the path tracking effect with high accuracy, low computational complexity and good stability are achieved.
Patent Information
- Application Number
- CN202510292595.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-03-12
- Publication Date
- 2025-06-13
AI Technical Summary
The existing autonomous driving path tracking method has insufficient control accuracy and poor stability under complex road conditions and dynamic environments.
The automatic driving path tracking control method based on a linear secondary regulator is adopted. By obtaining the vehicle's posture information, speed information and preset path point information, the position and direction of the target point are calculated, the vehicle's kinematic model is established and converted into a linear discrete state error space model, and the optimal control input of the linear secondary regulator is calculated to realize the vehicle's driving control and path tracking.
It realizes high-precision path tracking, especially in complex paths and dynamic environments, has low computational complexity, meets the real-time requirements of autonomous vehicles, and maintains good stability and robustness.
Smart Images
Figure CN120143613A_ABST
Abstract
Description
Technical Field
[0001] This application belongs to the technical field of autonomous driving, and particularly relates to an autonomous driving path tracking control method and system based on a linear quadratic regulator. Background Art
[0002] Autonomous driving technology is one of the important research directions in the automotive industry and the field of artificial intelligence in recent years. With the rapid development of sensor technology, computing power, and artificial intelligence algorithms, autonomous driving vehicles are gradually moving from the laboratory to practical applications. The core technologies of autonomous driving vehicles include environmental perception, path planning, and vehicle control. Among them, path tracking control is the key technology to ensure that the vehicle travels along a preset path.
[0003] The goal of path tracking control is to enable an autonomous driving vehicle to travel along a preset path while maintaining the stability and safety of the vehicle. Existing path tracking methods have problems of insufficient control accuracy and poor stability in complex road conditions and dynamic environments. Summary of the Invention
[0004] The purpose of this application is to provide an autonomous driving path tracking control method and system based on a linear quadratic regulator to solve the problems of insufficient control accuracy and poor stability of existing path tracking methods in complex road conditions and dynamic environments.
[0005] According to the first aspect of the embodiments of this application, an autonomous driving path tracking control method based on a linear quadratic regulator is provided. The method may include:
[0006] Obtain the pose information, speed information, and preset path point information of the vehicle;
[0007] Calculate the position and direction of the target point according to the pose information, speed information, and preset path point information;
[0008] Establish a kinematic model of the vehicle according to the position and direction, and transform it into a linearized discrete state error space model of the vehicle;
[0009] Calculate the optimal control input of the linear quadratic regulator according to the linearized discrete state error space model;
[0010] Perform driving control and path tracking on the vehicle according to the optimal control input.
[0011] In some alternative embodiments of this application, the pose information includes position coordinates and orientation angles;
[0012] The speed information includes linear velocity and angular velocity;
[0013] The preset path point information includes the position coordinates and orientation angles of the path points.
[0014] In some alternative embodiments of the present application, a kinematic model of the vehicle is established according to the position and orientation and transformed into a linearized discrete state error space model of the vehicle, including:
[0015] Establish a kinematic model of the vehicle according to the position and orientation;
[0016] Linearize and discretize the kinematic model to obtain a linearized discrete state error space model.
[0017] In some alternative embodiments of the present application, the kinematic model is represented by the following formula:
[0018]
[0019] Where L is the wheelbase of the vehicle; Is the first derivative of the state variable; Is the state variable And the control input variable The functional relationship between them, that is, the state transition equation; Is the state variable [x r , y r , ψ r corresponding to any reference point r among the preset path points of the vehicle; T ; Is the input variable [v r , δ r corresponding to any reference point r among the preset path points of the vehicle; T .
[0020] In some alternative embodiments of the present application, after linearization, the vehicle kinematic model is:
[0021]
[0022]
[0023]
[0024]
[0025] Where, Is the first derivative of the state error quantity, Is the state error quantity, Is the control input error quantity, A is the state matrix in the state error transfer equation after linearization of the vehicle kinematic model, and B is the input matrix in the state error transfer equation after linearization of the vehicle kinematic model.
[0026] In some alternative embodiments of the present application, the discrete state error space model is represented by the following formula:
[0027]
[0028]
[0029] Among them, is the state error quantity corresponding to the (k + 1)-th moment, is the state error quantity corresponding to the k-th moment, T is the sampling step, A is the state matrix in the state error transfer equation of the linearized vehicle kinematic model, and B is the input matrix in the state error transfer equation of the linearized vehicle kinematic model. is the state matrix in the state error transfer equation after discretization of the linearized vehicle kinematic model, is the input matrix in the state error transfer equation after discretization of the linearized vehicle kinematic model.
[0030] In some alternative embodiments of the present application, calculating the optimal control input of the linear quadratic regulator according to the linearized discrete state error space model includes:
[0031] Determining the linear quadratic regulator parameters according to the linearized discrete state error space model;
[0032] Determining the optimal control input according to the linear quadratic regulator parameters.
[0033] In some alternative embodiments of the present application, the linear quadratic regulator parameters are determined by a performance index function, and the performance index function is:
[0034]
[0035] where Q and R are weight matrices.
[0036] In some alternative embodiments of the present application, the optimal control input is obtained by the following formula:
[0037]
[0038] where K is the feedback gain matrix, which is obtained by solving the Riccati equation.
[0039] According to the second aspect of the embodiments of the present application, there is provided an automatic driving path tracking control system based on a linear quadratic regulator, including: a processor, a memory, and a program or instruction stored on the memory and executable on the processor. When the program or instruction is executed by the processor, the steps of the automatic driving path tracking control method based on the linear quadratic regulator according to any one of the embodiments of the first aspect are implemented.
[0040] The above technical solutions of the present application have the following beneficial technical effects:
[0041] The method of the embodiment of the present application can achieve high-precision path tracking, especially performing excellently in complex paths and dynamic environments; it has low computational complexity and meets the real-time requirements of autonomous vehicles; and the linear quadratic regulator has good stability and robustness, enabling the method to keep the vehicle driving stably under complex road conditions. BRIEF DESCRIPTION OF THE DRAWINGS
[0042] Figure 1 is a schematic flowchart of an autonomous driving path tracking control method based on a linear quadratic regulator in an exemplary embodiment of the present application;
[0043] Figure 2 is a schematic diagram of a vehicle dynamics model in an exemplary embodiment of the present application;
[0044] Figure 3 is a control flowchart of a linear quadratic regulator in an exemplary embodiment of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS
[0045] To make the objectives, technical solutions and advantages of the present application more clear and understandable, the present application will be further described in detail below in conjunction with the specific embodiments and with reference to the accompanying drawings. It should be understood that these descriptions are merely exemplary and are not intended to limit the scope of the present application. In addition, in the following description, descriptions of well-known structures and technologies are omitted to avoid unnecessarily confusing the concepts of the present application.
[0046] The schematic diagram of the layer structure according to the embodiment of the present application is shown in the drawings. These figures are not drawn to scale, where for the purpose of clarity, some details are enlarged and some details may be omitted. The shapes of various regions and layers shown in the figures, as well as their relative sizes and positional relationships, are merely exemplary. In practice, there may be deviations due to manufacturing tolerances or technical limitations, and those skilled in the art can design regions / layers with different shapes, sizes, and relative positions according to actual needs.
[0047] Obviously, the described embodiments are part of the embodiments of the present application, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present application without creative efforts shall fall within the scope of protection of the present application.
[0048] In the description of the present application, it should be noted that the terms "first", "second", and "third" are only used for descriptive purposes and cannot be construed as indicating or implying relative importance.
[0049] In addition, the technical features involved in different embodiments of the present application described below can be combined with each other as long as they do not conflict with each other.
[0050] The following will combine with the accompanying drawings to elaborate in detail on the method and system for autonomous driving path tracking control based on the linear quadratic regulator provided by the embodiments of the present application through specific embodiments and their application scenarios.
[0051] As Figure 1 shown, in the first aspect of the embodiments of the present application, a method for autonomous driving path tracking control based on the linear quadratic regulator is provided. The method may include:
[0052] S110: Obtain the pose information, speed information, and preset path point information of the vehicle;
[0053] S120: Calculate the position and direction of the target point according to the pose information, speed information, and preset path point information;
[0054] S130: Establish a kinematic model of the vehicle according to the position and direction, and transform it into a linearized discrete state error space model of the vehicle;
[0055] S140: Calculate the optimal control input of the linear quadratic regulator according to the linearized discrete state error space model;
[0056] S150: Perform driving control and path tracking on the vehicle according to the optimal control input.
[0057] The method of this embodiment can achieve high-precision path tracking, especially perform excellently in complex paths and dynamic environments; has low computational complexity and meets the real-time requirements of autonomous driving vehicles; and the linear quadratic regulator has good stability and robustness, enabling this method to maintain the stable driving of the vehicle under complex road conditions.
[0058] In some embodiments, the pose information includes position coordinates and orientation angles;
[0059] The speed information includes linear velocity and angular velocity;
[0060] The preset path point information includes the position coordinates and orientation angles of the path points.
[0061] This embodiment can calculate the position and direction of the target point according to the current pose information and speed information of the vehicle, and the preset path point information. The target point is the point that the vehicle needs to track during path tracking, and its position and direction can be obtained by methods such as interpolation or fitting of the path points.
[0062] In some embodiments, establishing a kinematic model of the vehicle according to the position and direction, and transforming it into a linearized discrete state error space model of the vehicle includes:
[0063] Establish a kinematic model of the vehicle according to the position and direction;
[0064] Linearize and discretize the kinematic model to obtain a linearized discrete state error space model.
[0065] Based on the kinematic model of the vehicle, this embodiment establishes a linearized state space model of the vehicle, further obtains a state error space model, and discretizes it to obtain the corresponding discrete state error space model. The linearized state space model includes the state variables and control input variables of the vehicle. The state variables include the position coordinates and orientation angle of the vehicle, etc., and the control input variables can include the speed and steering angle of the vehicle, etc.
[0066] In some embodiments, the kinematic model is represented by the following formula:
[0067]
[0068] where L is the wheelbase of the vehicle; is the first derivative of the state variable [x, y, ψ] T The state variable [x, y, ψ] corresponds to the position of the vehicle in the x and y directions, and the yaw angle ψ respectively; the first derivative T is the speed in the corresponding direction; is the function relationship between the state variable and the control input variable , that is, the state transition equation; the input variable includes the longitudinal speed v and the vehicle steering angle δ of the vehicle; is the state variable [x when the vehicle is at any reference point r among the preset path points r , y r , ψ r ; T ; is the input variable [v r , δ r when the vehicle is at any reference point r among the preset path points T .
[0069] In some embodiments, after linearization, the vehicle kinematic model is:
[0070]
[0071]
[0072]
[0073]
[0074] where is the first derivative of the state error quantity, is the state error quantity, is the control input error quantity, A is the state matrix in the state error transfer equation after linearizing the vehicle kinematic model, and B is the input matrix in the state error transfer equation after linearizing the vehicle kinematic model.
[0075] In some embodiments, the discrete state error space model is represented by the following formula:
[0076]
[0077] where, is the state error quantity corresponding to the (k + 1)-th moment, is the state error quantity corresponding to the k-th moment, T is the sampling step, A is the state matrix in the state error transfer equation of the linearized vehicle kinematic model, B is the input matrix in the state error transfer equation of the linearized vehicle kinematic model, is the state matrix in the state error transfer equation after discretizing the linearized vehicle kinematic model, is the input matrix in the state error transfer equation after discretizing the linearized vehicle kinematic model.
[0078] In some embodiments, calculating the optimal control input of the linear quadratic regulator according to the linearized discrete state error space model includes:
[0079] Determining the linear quadratic regulator parameters according to the linearized discrete state error space model;
[0080] Determining the optimal control input according to the linear quadratic regulator parameters.
[0081] In this embodiment, a linear quadratic regulator (i.e., an LQR controller) is designed according to the state space model of the vehicle. The design of the LQR controller includes defining a performance index function and solving the optimal control law. The performance index function usually includes the weighted sum of the state error variable and the control input error variable of the vehicle. By adjusting the weight coefficients, the optimization of different performance indexes of the vehicle can be achieved. The optimal control law can be obtained by solving the Riccati equation. The optimal control law can calculate the optimal control input according to the difference between the current state and the target state of the vehicle.
[0082] Furthermore, the driving of the vehicle can be controlled according to the optimal control input calculated by the LQR controller, so that the vehicle can drive along the preset path and achieve path tracking.
[0083] In some embodiments, the linear quadratic regulator parameters are determined by the performance index function, and the performance index function is:
[0084]
[0085] Among them, Q and R are weight matrices.
[0086] In some embodiments, the optimal control input is obtained by the following formula:
[0087]
[0088] Among them, K is the feedback gain matrix, which is obtained by solving the Riccati equation.
[0089] The method of the above embodiments uses an LQR controller to control the path tracking of an autonomous vehicle. It can calculate the optimal control input according to the difference between the current state and the target state of the vehicle, achieve precise control of the vehicle, and improve the path tracking accuracy of the vehicle. The LQR controller has advantages such as good stability and strong robustness, and can keep the vehicle driving stably under complex road conditions and dynamic environments, improving the driving safety of the vehicle. By adjusting the weight coefficients in the performance index function of the LQR controller, the optimization of different performance indexes of the vehicle can be realized, such as speed tracking performance, steering performance, etc., so that the vehicle can better adapt to different driving scenarios and task requirements. In addition, the method of the above embodiments has a low computational complexity, can meet the real-time requirements of autonomous vehicles, and is applicable to path tracking control in practical applications.
[0090] In a specific embodiment, a first-order kinematic model is used to describe the motion characteristics of the vehicle, as Figure 2 shown. The state variables of the vehicle include the position coordinates [x, y] T and the orientation angle ψ. The control input variables include the vehicle speed ν and the steering angle δ. The kinematic model of the vehicle can be expressed as:
[0091]
[0092] Among them, L is the wheelbase of the vehicle, is the first derivative of the state variable, is the state variable and the control input variable The functional relationship between them, that is, the state transition equation, is the state variable [x r , y r , ψ r T corresponding to any reference point r among the preset path points of the vehicle and the input variables [v r , δ r T .
[0093] After linearization, the kinematic model of the vehicle is:
[0094]
[0095]
[0096]
[0097]
[0098] wherein, is the first derivative of the state error quantity, is the state error quantity, is the control input error quantity, A is the state matrix in the state error transfer equation after linearization of the vehicle kinematic model, and B is the input matrix in the state error transfer equation after linearization of the vehicle kinematic model.
[0099] Linearize and discretize the above kinematic model to convert it into a discrete state error space model:
[0100]
[0101] wherein, is the state error quantity corresponding to the (k + 1)-th moment, is the state error quantity corresponding to the k-th moment, T is the sampling step, A is the state matrix in the state error transfer equation of the linearized vehicle kinematic model, B is the input matrix in the state error transfer equation of the linearized vehicle kinematic model, is the state matrix in the state error transfer equation after discretization of the linearized vehicle kinematic model, is the input matrix in the state error transfer equation after discretization of the linearized vehicle kinematic model.
[0102] The design of the LQR controller includes defining the performance index function and solving the optimal control law. As Figure 3 shown, Figure 3 shows the structure of the LQR controller, including the state space model, the performance index function, the Riccati equation solver, and the calculation process of the feedback gain matrix K. The input (vehicle state ) and output (optimal control input ) of the controller are also marked in the figure. The performance index function is defined as:
[0103]
[0104] wherein, Q and R are weight matrices, and by adjusting the values of their elements, the optimization of different performance indexes of the vehicle can be achieved. For example, increasing the weight of the corresponding position error in Q can improve the path tracking accuracy, and increasing the weight of the corresponding control input in R can reduce the amplitude of the control input, thereby improving the robustness of the system.
[0105] The optimal control law can be obtained by multiplying the feedback gain matrix K with the state error at time k and its form is as follows:
[0106]
[0107] where K is the feedback gain matrix and can be obtained by solving the Riccati equation.
[0108] The driving of the vehicle is controlled according to the optimal control input calculated by the LQR controller. For example, according to the difference between the calculated actual speed, steering angle and the desired speed, steering angle, the throttle, brake and steering systems of the vehicle are controlled so that the vehicle can drive along the preset path to achieve path tracking.
[0109] Exemplarily, assume that the wheelbase L of the vehicle is 2.7 m and the preset path is a complex path including a straight line and a curve. The current pose and speed information of the vehicle are obtained through sensors, and combined with the preset path point information, the position and direction of the target point are calculated. Based on the kinematic model of the vehicle, linearization and discretization are performed, a discrete state error space model is established, and an LQR controller is designed. By adjusting the weight matrices Q and R, the path tracking performance of the vehicle is optimized.
[0110] In practical applications, the kinematic model of the vehicle is non-linear. In order to apply the LQR controller, the model needs to be linearized. The accuracy of linearization directly affects the control performance. The selection of the weight matrices Q and R in the performance index function is crucial for the control performance. The design of the weight matrices needs to be adjusted according to the specific application scenario to balance the path tracking accuracy, control input amplitude and system stability. In order to meet the real-time requirements of autonomous vehicles, the method and system of this embodiment adopt an efficient numerical solution method to calculate the Riccati equation to ensure that the controller can output the optimal control input in a short time.
[0111] The method of the above embodiment can improve the accuracy and stability of path tracking, enabling the vehicle to drive precisely along the preset path. It improves the real-time performance and adaptability, enabling it to maintain good control performance in a dynamic environment. By optimizing the performance index, the balance of different performance indexes of the vehicle can be achieved, such as speed tracking performance, steering performance, etc. This method is applicable to the path tracking control of autonomous vehicles and can effectively improve the driving safety and comfort of the vehicle in complex road conditions. In the intelligent transportation system, this method can be used to optimize the path planning and control of vehicles and improve the traffic flow efficiency. It can also be applied to the path tracking control of mobile robots to help the robots achieve precise navigation in complex environments.
[0112] It should be noted that for the automatic driving path tracking control method based on the linear quadratic regulator provided in the embodiments of the present application, the execution subject may be an automatic driving path tracking control device based on the linear quadratic regulator, or a control module in the automatic driving path tracking control device based on the linear quadratic regulator for executing the method of automatic driving path tracking control based on the linear quadratic regulator. In the embodiments of the present application, taking the automatic driving path tracking control device based on the linear quadratic regulator as an example to execute the method of automatic driving path tracking control based on the linear quadratic regulator, the device for automatic driving path tracking control based on the linear quadratic regulator provided in the embodiments of the present application is described.
[0113] In the second aspect of the embodiments of the present application, an automatic driving path tracking control system based on a linear quadratic regulator is provided, including: a processor, a memory, and a program or instruction stored on the memory and executable on the processor, and when the program or instruction is executed by the processor, the steps of the automatic driving path tracking control method according to any one of the embodiments in the first aspect are implemented.
[0114] The automatic driving path tracking control system based on the linear quadratic regulator in the embodiments of the present application may be a device, or a component, an integrated circuit, or a chip in a terminal. The device may be a mobile electronic device or a non-mobile electronic device. Exemplarily, the mobile electronic device may be a mobile phone, a tablet computer, a laptop computer, a handheld computer, an in-vehicle electronic device, a wearable device, an ultra-mobile personal computer (UMPC), a netbook, or a personal digital assistant (PDA), etc., and the non-mobile electronic device may be a server, a Network Attached Storage (NAS), a personal computer (PC), a television (TV), a teller machine, or a self-service machine, etc. The embodiments of the present application do not make specific limitations.
[0115] The automatic driving path tracking control system based on the linear quadratic regulator in the embodiments of the present application may be a device with an operating system. The operating system may be an Android operating system, an iOS operating system, or other possible operating systems. The embodiments of the present application do not make specific limitations.
[0116] The automatic driving path tracking control system based on the linear quadratic regulator provided in the embodiments of the present application can achieve Figure 1 each process implemented by the method embodiments, and for the sake of avoiding repetition, it will not be elaborated here.
[0117] The embodiments of the present application have been described above in conjunction with the accompanying drawings. However, the present application is not limited to the above specific implementation manners. The above specific implementation manners are merely illustrative rather than restrictive. Under the inspiration of the present application, those of ordinary skill in the art can also make many forms without departing from the purpose of the present application and the scope protected by the claims, and all of them fall within the protection scope of the present application.
Claims
1. An automatic driving path tracking control method based on a linear quadratic regulator, characterized in that: include: Obtain vehicle position information, speed information, and preset path point information; Calculate the position and direction of the target point according to the posture information, the speed information and the preset path point information; Establishing a kinematic model of the vehicle according to the position and the direction, and converting it into a linearized discrete state error space model of the vehicle; Calculating an optimal control input of a linear quadratic regulator according to the linearized discrete state error space model; The vehicle is controlled to travel and track a path according to the optimal control input.
2. The automatic driving path tracking control method based on a linear quadratic regulator according to claim 1, characterized in that: The posture information includes position coordinates and orientation angle; The speed information includes linear speed and angular speed; The preset path point information includes the location coordinates and orientation angle of the path point.
3. The automatic driving path tracking control method based on a linear quadratic regulator according to claim 1, characterized in that: The kinematic model of the vehicle is established according to the position and the direction, and converted into a linearized discrete state error space model of the vehicle, including: establishing a kinematic model of the vehicle based on the position and the orientation; The kinematic model is linearized and discretized to obtain a linearized discrete state error space model.
4. The automatic driving path tracking control method based on a linear quadratic regulator according to claim 3 is characterized in that: The kinematic model is expressed by the following formula: Wherein, L is the wheelbase of the vehicle; is the first-order derivative of the state variable, wherein the state variable includes the speed information in the x and y directions and the angular velocity information of the vehicle heading angle ψ; is a state variable and control input variables The functional relationship between them, that is, the state transfer equation; is the state variable [x r ,y r ,ψ r ] T ; is the input variable [v r ,δ r ] T .
5. The automatic driving path tracking control method based on a linear quadratic regulator according to claim 4, characterized in that: The vehicle kinematic model is linearized as follows: in, is the first-order derivative of the state error, is the state error, To control the input error amount, A is the state matrix in the state error transfer equation after the vehicle kinematics model is linearized, and B is the input matrix in the state error transfer equation after the vehicle kinematics model is linearized.
6. The automatic driving path tracking control method based on a linear quadratic regulator according to claim 5, characterized in that: The discrete state error space model is expressed by the following formula: in, is the state error corresponding to the k+1 moment, is the state error corresponding to time k, T is the sampling step, A is the state matrix in the state error transfer equation of the linearized vehicle kinematics model, B is the input matrix in the state error transfer equation of the linearized vehicle kinematics model, is the state matrix in the state error transfer equation after the linearized vehicle kinematic model is discretized, is the input matrix in the state error transfer equation after the discretization of the linearized vehicle kinematic model.
7. The automatic driving path tracking control method based on a linear quadratic regulator according to claim 1, characterized in that: The step of calculating the optimal control input of the linear quadratic regulator according to the linearized discrete state error space model comprises: Determine the linear quadratic regulator parameters according to the linearized discrete state error space model; Based on the linear quadratic regulator parameters, an optimal control input is determined.
8. The automatic driving path tracking control method based on a linear quadratic regulator according to claim 7, characterized in that: The linear quadratic regulator parameters are determined by a performance index function, which is: Among them, Q and R are weight matrices.
9. The automatic driving path tracking control method based on a linear quadratic regulator according to claim 7, characterized in that: The optimal control input is obtained by the following formula: Where K is the feedback gain matrix, which is obtained by solving the Riccati equation.
10. An automatic driving path tracking control system based on a linear quadratic regulator, characterized in that: include: A processor, a memory, and a program or instruction stored in the memory and executable on the processor, wherein the program or instruction, when executed by the processor, implements the steps of the automatic driving path tracking control method based on a linear quadratic regulator as described in any one of claims 1 to 9.