Linear-Quadratic-Regulator(LQR) Controller
The theory of optimal control is concerned with operating a dynamic system at minimum cost. The case where the system dynamics are described by a set of linear differential equations and the cost is described by a quadratic function is called the LQ problem. One of the main results in the theory is that the solution is provided by the linear–quadratic regulator (LQR), a feedback controller whose equations are given below.
General Description
From Wikipedia
The settings of a (regulating) controller governing either a machine or process (like an airplane or chemical reactor) are found by using a mathematical algorithm that minimizes a cost function with weighting factors supplied by a human (engineer). The cost function is often defined as a sum of the deviations of key measurements, like altitude or process temperature, from their desired values. The algorithm thus finds those controller settings that minimize undesired deviations. The magnitude of the control action itself may also be included in the cost function.
The LQR algorithm reduces the amount of work done by the control systems engineer to optimize the controller. However, the engineer still needs to specify the cost function parameters, and compare the results with the specified design goals. Often this means that controller construction will be an iterative process in which the engineer judges the “optimal” controllers produced through simulation and then adjusts the parameters to produce a controller more consistent with design goals.
The LQR algorithm is essentially an automated way of finding an appropriate state-feedback controller. As such, it is not uncommon for control engineers to prefer alternative methods, like full state feedback, also known as pole placement, in which there is a clearer relationship between controller parameters and controller behavior. Difficulty in finding the right weighting factors limits the application of the LQR based controller synthesis.
Method
Here only finite-horizon, discrete-time LQR problem will be discussed
Goal
In this problem, the discrete-time linear system described by:
Its performance could be defined by the following cost function:
This equation could be simplified by removing the last Gaussian Coefficient.:
Solution
But how to minimize the cost function ? Algebraic Riccati equation provides a common method to resolve this problem, which is named DARE in discrete-time problem.
From terminal condition is found iteratively backwards in time by the dynamic Riccati equation:
The steady-state characterization of , relevant for the infinite-horizon problem in which goes to infinity, can be found by iterating the dynamic equation repeatedly until it converges; then is characterized by removing the time subscripts from the dynamic equation.
The optimal control sequence minimizing the performance index is given by:
Where:
Conclusion
Therefore, the whole process of LQR algorithm could be concluded as,
- Set equal to
- Iterate through Algebraic Riccati equation
- Terminate iteration of while is small enough and calculate feedback matrix
- Calculate the optimal control signal through
Application
Analysis of Apollo control algorithm
In the field of autonomous driving, how to track the planning path well is a really core problem. Geometry algorithm such as Pure Pursuit Algorithm, Stanley Algorithm is only suitable for low-speed and simple conditions. In city working condition, modern advanced control theories such as Model Predictive Control and Linear-Quadratic Regulator control are more appropriate in these conditions.
Here we will discuss how to use LQR Algorithm to track the planning trajectory of self-driving vehicles.
If the state of the self-driving vehicle and the planned trajectory (black points) is known, the state of the vehicle in the soon future (showed as the red points) could be calculated through a sequence fo given control signals u according to the state model
We can use a quadratic cost function to evaluate the performance of the vehicle as followed,
Where represents the sequences of desired states in planned trajectory and represents the predicted points. We can minimize this objective by linearizing the dynamics around the target trajectory and applying LQR algorithm to the resulting linearized system. This optimization is done in an online fashion, each time a new control command will be calculated and sent to controller of the vehicle.
However, in the condition without considering the rate of vehicle’s heading angle, the lateral deviation and heading deviation are relevant to the curvature of the road. LQR algorithm can leads the system to the stable state, but it cannot eliminate the steady-state error from the curvature of the road. Therefore, a feedforward control signal is introduced to suppress the steady-state error. In simplified cases, , where is wheel base of the vehicle and k is the curvature of the road.
Therefore,
Bonus
But how to define the suitable matrices A, B, Q, R in different systems? The following gives detailed steps to define these matrices as an example. (from Apollo)
Example
In autonomous driving, LQR Controller can be used for lateral steering wheel control. Therefore, dynamic analysis is required to build a vehicle dynamic model.
Dynamic analysis
Firstly, we should analyze the forces of vehicles.
As for lateral steering wheel control, we should pay more attention on lateral forces.
Analyze the forces on the front and read wheels of the vehicle,
According to momenta balance of the vehicle,
In equations (1), (2), the car quality , monenta of inertia , distance from front or rear axle to center , are measurable. To resolve the equations, lateral acceleration and lateral forces , are needed.
Lateral acceleration can be decomposed into acceleration caused by lateral displacement and centripetal acceleration.
Where is lateral displacement, is heading angle and is longitudinal speed.
Lateral forces and can be deduced by the below equations,
Where and are lateral stiffness of front and rear wheels respectively, is front wheel angle and and are velocity deviation angle of front and rear wheels.
From the image above, we can get the below equations,
In assumption of small angle,
Back to formula (1) and (2), we can get,
To get the state space expression of vehicle motion, we introduce lateral error and heading angle error . Hence we get the below equations,
In the assumption of uniform motion, a further deduction is,
Similarly,
And back to the equations (6) and (7), we can get the equations based on and ,
Finally, change it to state space expression and we get the vehicle dynamics model expression.
Set
The final state expression is
In this equation, lateral stiffness of front wheel Caf and rear wheel , car quality , momenta of inertia , distance from front or rear axle to center , are constant.
Lateral error , heading angle error , longitudinal speed and front wheel angle are measurable.
In this way, the target trajectory can be tracked by the corresponding algorithm using this model by updating the value measured by sensors every calculation cycle.
Discretization
However, in practical applications, continuous model is not suitable in most cases. Hence we need a discrete model for calculation.
Where,