Task content
By adjusting PID and LQR controller to realize stable hovering of multi rotor aircraft, it is applied in both simulation and actual system.
Reference content
Basic reference content of LQR control part:
Reference link:
Linear Quadratic Regulator (LQR) With Python Code Example
design scheme
A: Altitude and yaw control - PID controller
For the parameter shaping of PID, you can refer to the following three schemes (select one according to the actual machine situation, or compare multiple schemes with each other)
-
The Ziegler Nichols method is used to obtain the initial estimate.
This method is a PID tuning method based on system stability analysis. There is no need to consider any characteristic requirements in the design process. The tuning method is very simple, but the control effect is ideal. The specific tuning methods are as follows:
First, assume that there is only proportional control, set k d = k i = 0 k_{d}=k_{i}=0 kd = ki = 0, and then increase the scale coefficient until the system has equal amplitude oscillation for the first time (the pole of the closed-loop system is at J ω On the axis), at this time, the critical gain when the system starts oscillation is obtained K m K_{m} Km；
Then multiply the scale coefficient by the corresponding parameter according to the following table, here multiply by 0.6
Type of regulator K p T i T d P 0.5 K e ∞ 0 P I 0.45 K e 1 1.2 T e 0 P I D 0.6 K e 0.5 T e 0.125 T e \Begin {array} {C | C | C |} \ hline \ text {type of regulator} & K_ {p} & T_ {i} & T_ {d}\\hline P & 0.5K_ {e} & \infty & 0\\hline PI & 0.45K_ {e} & \frac{1}{1.2}T_ {e} & 0 \\hline PID & 0.6K_ {e} & 0.5T_ {e} & 0.125T_ {e} \\hline \end{array} Type of regulator: PPIPID ∞ Kp ∞ 0.5Ke ≈ 0.45Ke ≈ 0.6Ke ∞ Ti ∞ 1.21 ≈ Te ≈ 0.5Te ≈ Td ≈ 000.125Te
Other parameters are calculated according to the following formula:
K p = 0.6 ∗ K m K d = K p ∗ π 4 ω K i = K p ∗ ω π K_{p}=0.6*K_{m}\\K_{d}=K_{p}*\frac{\pi}{4\omega}\\K_{i}=K_{p}*\frac{\omega}{\pi} Kp=0.6∗KmKd=Kp∗4ωπKi=Kp∗πω
among K p K_{p} Kp ＾ is the proportional control parameter, K i K_{i} Ki is the integral control parameter, K d K_{d} Kd = differential control parameter, ω \omega ω Is the frequency of oscillation.
The above rules are used to determine the parameters of the PID controller, so that the overshoot of the system is between 10% ~ 60%, and the average value is about 25%.
-
Use simulation to obtain the corresponding gain value.
-
Modify the corresponding gain value in the real system.
Methods: slowly increase P until the oscillation begins, then slowly add a small amount of D to suppress the oscillation, and then slowly add a small amount of I to correct any steady-state error.
B1: x, y position control - LQR controller
For multi rotor aircraft, the linear equation of motion shows the x coordinate obtained from the position coordinate p x p_{x} px and y coordinates p y p_{y} py# and roll angle β \beta β And pitch angle γ \gamma γ Complete decoupling between.
[ p ˙ x p ¨ x β ˙ ] = [ 0 1 0 0 0 g 0 0 0 ] [ p x p ˙ x β ] + [ 0 0 1 ] [ ω y ] \left[\begin{array}{l}\dot{p}_{x} \\\ddot{p}_{x} \\\dot{\beta}\end{array}\right]=\left[\begin{array}{lll}0 & 1 & 0 \\0 & 0 & g \\0 & 0 & 0\end{array}\right]\left[\begin{array}{l}p_{x} \\\dot{p}_{x} \\\beta\end{array}\right]+\left[\begin{array}{l}0 \\0 \\1\end{array}\right]\left[\omega_{y}\right] ⎣⎡p˙xp¨xβ˙⎦⎤=⎣⎡0001000g0⎦⎤⎣⎡pxp˙xβ⎦⎤+⎣⎡001⎦⎤[ωy]
[ p ˙ y p ¨ y γ ˙ ] = [ 0 1 0 0 0 − g 0 0 0 ] [ p y p ˙ y γ ] + [ 0 0 1 ] [ ω x ] \left[\begin{array}{c}\dot{p}_{y} \\\ddot{p}_{y} \\\dot{\gamma}\end{array}\right]=\left[\begin{array}{ccc}0 & 1 & 0 \\0 & 0 & -g \\0 & 0 & 0\end{array}\right]\left[\begin{array}{c}p_{y} \\\dot{p}_{y} \\\gamma\end{array}\right]+\left[\begin{array}{l}0 \\0 \\1\end{array}\right]\left[\omega_{x}\right] ⎣⎡p˙yp¨yγ˙⎦⎤=⎣⎡0001000−g0⎦⎤⎣⎡pyp˙yγ⎦⎤+⎣⎡001⎦⎤[ωx]
Therefore, we can use the pitch rate ω y \omega_{y} ω y ， to control the position error of a body coordinate x and the rolling rate ω x \omega_{x} ω x ， to control a body coordinate y position error. (inner ring → outer ring control)
The goal of this task is to design an LQR controller for each x and y respectively.
Linear Quadratic Regulator, the acronym LQR stands for Linear Quadratic Regulator. The name itself specifies the settings applicable to this controller design method:
- The dynamics of the system is linear,
- The cost function to be minimized is quadratic,
- The systematic controller adjusts the state to zero.
The following information summarizes the steps and equations for synthesizing an infinitesimal bounded LQR controller. The resulting controller is called "linear state feedback controller", which is usually represented by matrix "K". The state feedback matrix K has one row for each input of the system and one column for each state of the system.
-
The dynamics of continuous time and discrete-time linear systems are recorded as
x ˙ = A x + B u , x k + 1 = A D x k + B D u k \dot{x}=A x+B u, \quad x_{k+1}=A_{\mathrm{D}} x_{k}+B_{\mathrm{D}} u_{k} x˙=Ax+Bu,xk+1=ADxk+BDuk
In order to convert continuous time system matrices A and B into discrete-time system matrices A D A_{D} AD # and B D B_{D} BD, using MATLAB function c2d, specify zero order hold as the discretization method.
-
Q and R cost function matrices need to be selected to achieve the desired flight performance of the aircraft. The LQR cost function in infinite horizon is:
J ∞ = ∫ 0 ∞ ( x ( t ) ⊤ Q x ( t ) + u ( t ) ⊤ R u ( t ) ) d t J_{\infty}=\int_{0}^{\infty}\left(x(t)^{\top} Q x(t)+u(t)^{\top} R u(t)\right) d t J∞=∫0∞(x(t)⊤Qx(t)+u(t)⊤Ru(t))dt
-
Q is the state cost matrix with the same number of rows and columns as the number of states. The matrix weighs the relative importance of each state in the state vector, while R is the input cost matrix with the same number of rows and columns as the control input. The matrix punishes the workload of the actuator.
To select Q and R cost function matrices, different elements of the state vector should be considered. For example, you may want to penalize the x position deviation of 10 cm by the same amount as the yaw deviation of 5 degrees.
-
Use MATLAB functions care and dare to calculate the continuous and discrete-time Riccati algebraic equations respectively. You can refer to MATLAB to read the help document to understand the calculation of these functions.
-
The continuous time linear time invariant (LTI) infinite time span LQR design equation system is (directly taken from the handout of control system I):
0 = − A ⊤ P ∞ − P ∞ A + P ∞ B R − 1 B ⊤ P ∞ − Q u ( t ) = − K ∞ x ( t ) K ∞ = R − 1 B ⊤ P ∞ \begin{aligned}0 &=-A^{\top} P_{\infty}-P_{\infty} A+P_{\infty} B R^{-1} B^{\top} P_{\infty}-Q \\u(t) &=-K_{\infty} x(t) \\K_{\infty} &=R^{-1} B^{\top} P_{\infty}\end{aligned} 0u(t)K∞=−A⊤P∞−P∞A+P∞BR−1B⊤P∞−Q=−K∞x(t)=R−1B⊤P∞
Of which:
The first equation is to solve P ∞ P_{\infty} Riccati equation of P ∞
The second is the achieved control rate u ( t ) u(t) u(t)
The third is the state feedback gain matrix K ∞ K_{\infty} K∞
-
The infinite time span LQR design equation of discrete-time LTI system is:
0 = − P ∞ , D + A D ⊤ P D , ∞ A D − A D ⊤ P D , ∞ B D ( B D ⊤ P D , ∞ B D + R ) − 1 B D ⊤ P D , ∞ A D + Q u D ( k ) = − K D , ∞ x D ( k ) K D , ∞ = ( B D ⊤ P D , ∞ B D + R ) − 1 B D ⊤ P D , ∞ A D \begin{aligned}0 &=-P_{\infty, \mathrm{D}}+A_{\mathrm{D}}^{\top} P_{\mathrm{D}, \infty} A_{\mathrm{D}}-A_{\mathrm{D}}^{\top} P_{\mathrm{D}, \infty} B_{\mathrm{D}}\left(B_{\mathrm{D}}^{\top} P_{\mathrm{D}, \infty} B_{\mathrm{D}}+R\right)^{-1} B_{\mathrm{D}}^{\top} P_{\mathrm{D}, \infty} A_{\mathrm{D}}+Q \\u_{\mathrm{D}}(k) &=-K_{\mathrm{D}, \infty} x_{\mathrm{D}}(k) \\K_{\mathrm{D}, \infty} &=\left(B_{\mathrm{D}}^{\top} P_{\mathrm{D}, \infty} B_{\mathrm{D}}+R\right)^{-1} B_{\mathrm{D}}^{\top} P_{\mathrm{D}, \infty} A_{\mathrm{D}}\end{aligned} 0uD(k)KD,∞=−P∞,D+AD⊤PD,∞AD−AD⊤PD,∞BD(BD⊤PD,∞BD+R)−1BD⊤PD,∞AD+Q=−KD,∞xD(k)=(BD⊤PD,∞BD+R)−1BD⊤PD,∞AD
Where subscript ( . ) D (.)_{D} (.) D ， denotes the variables applicable to discrete-time systems
-
Both care and dare functions can return the state feedback gain matrix. If you use this matrix, you need to pay attention to the sign convention.
Controller - position, y: PID control
If you also want to implement a PID controller for the (x, y) position, this is the relevant task description.
Based on scheme A, the further task of realizing A PID controller for altitude and yaw is to design, implement and adjust A PID controller to control x and y positions.
Recall that the linearized equation of motion provided in scheme B shows the complete decoupling between x position and pitch angle and y position and roll angle.
Therefore, we can use the pitch rate ω y \omega_{y} ω y ， to control the position error of body coordinate x, and the rolling rate ω x \omega_{x} ω x ， to control the body coordinate y position error.
Since the dynamics of pitch (or roll) angle is fast enough than that of X (or y) position, we can also use a nested controller to reasonably control the position error. The "external control" loop takes an x position error and requests a pitch angle β To correct the error, and then the "inner loop" takes the requested pitch angle β And request an angular rate about the Y-axis of the body coordinate ω Y to correct the error.
C: Add integrator for x, y position
Once you have completed the implementation of the LQR controller at the (x, y) position in the previous B task, observe the steady-state offset when tracking the (x, y) setpoint. Try to adjust the position of the battery in the aircraft by a few millimeters and observe the steady-state offset again.
Add x and y offsets to each steady-state controller to eliminate one offset.
Algorithmic logic
B1: x, y position control - LQR controller
LQR uses a technique called dynamic programming. When the aircraft moves around the world, at each time step t, a series of for cycles (in which we run each for cycle N times) are used to calculate the optimal control input. These cycle outputs correspond to u (i.e. control input) of the minimum total cost.
-
Initialize LQR function: pass in 7 parameters.
LQR(Actual State x, Desired State xf, Q, R, A, B, dt);
x_error = Actual State x – Desired State xf
-
Initialization time step N
Usually, N is set to any integer, such as 50. The solution of LQR problem is obtained recursively, starting from the last time step and working backward to the first time step in time.
In other words, the for loop (which we will reach in a second) requires a lot of iterations (50 times in this case) to reach the stable value of P (we will introduce P in the next step).
-
Initialize P to an empty list containing N+1 elements
Let P[N]=Q
Cycle i from N to 1, and calculate P[i-1] with the following formula (Riccati equation) respectively
P[i-1] = Q + ATP[i]A – (ATP[i]B)(R + BTP[i]B)-1(BTP[i]A)
-
Initialize K and u to an empty list containing N elements respectively
Cycle I from 0 to N-1, and calculate the state feedback gain matrix K[i] with the following formula respectively
K[i] = -(R + BTP[i+1]B)-1BTP[i+1]A
K will maintain the optimal feedback gain value. This is an important matrix when multiplied by the state error x ( t ) x(t) When x(t), the best control input is generated u ( t ) u(t) u(t) (see next step).
-
Cycle i from 0 to N-1, and calculate the control input with the following formula respectively
u[i] = K[i] @ x_error
We iterate the for loop N times until we get the stable value of the optimal control input U (e.g. u = [linear velocity v, angular velocity ω]). I assume that it reaches a stable value when N = 50.
-
Return the optimal control quantity u_star
u_star = u[N-1]
Optimal control input u_star. This is the last value we calculated in the previous step above. It is at the end of the U list.
Returns the best control input. These inputs will be sent to our aircraft so that it can move to a new state (i.e. new x position, y position and yaw angle) γ).
Code content (part)
Taking the two wheeled car as an example, this paper analyzes the code design of LQR control system
import numpy as np
# Description: Linear Quadratic Regulator example
# (two-wheeled differential drive robot car)
######################## DEFINE CONSTANTS #####################################
# Supress scientific notation when printing NumPy arrays
np.set_printoptions(precision=3,suppress=True)
# Optional Variables
max_linear_velocity = 3.0 # meters per second
max_angular_velocity = 1.5708 # radians per second
def getB(yaw, deltat):
"""
Calculates and returns the B matrix
3x2 matix ---> number of states x number of control inputs
Expresses how the state of the system [x,y,yaw] changes
from t-1 to t due to the control commands (i.e. control inputs).
:param yaw: The yaw angle (rotation angle around the z axis) in radians
:param deltat: The change in time from timestep t-1 to t in seconds
:return: B matrix ---> 3x2 NumPy array
"""
B = np.array([ [np.cos(yaw)*deltat, 0],
[np.sin(yaw)*deltat, 0],
[0, deltat]])
return B
def state_space_model(A, state_t_minus_1, B, control_input_t_minus_1):
"""
Calculates the state at time t given the state at time t-1 and
the control inputs applied at time t-1
:param: A The A state transition matrix
3x3 NumPy Array
:param: state_t_minus_1 The state at time t-1
3x1 NumPy Array given the state is [x,y,yaw angle] --->
[meters, meters, radians]
:param: B The B state transition matrix
3x2 NumPy Array
:param: control_input_t_minus_1 Optimal control inputs at time t-1
2x1 NumPy Array given the control input vector is
[linear velocity of the car, angular velocity of the car]
[meters per second, radians per second]
:return: State estimate at time t
3x1 NumPy Array given the state is [x,y,yaw angle] --->
[meters, meters, radians]
"""
# These next 6 lines of code which place limits on the angular and linear
# velocities of the robot car can be removed if you desire.
control_input_t_minus_1[0] = np.clip(control_input_t_minus_1[0],
-max_linear_velocity,
max_linear_velocity)
control_input_t_minus_1[1] = np.clip(control_input_t_minus_1[1],
-max_angular_velocity,
max_angular_velocity)
state_estimate_t = (A @ state_t_minus_1) + (B @ control_input_t_minus_1)
return state_estimate_t
def lqr(actual_state_x, desired_state_xf, Q, R, A, B, dt):
"""
Discrete-time linear quadratic regulator for a nonlinear system.
Compute the optimal control inputs given a nonlinear system, cost matrices,
current state, and a final state.
Compute the control variables that minimize the cumulative cost.
Solve for P using the dynamic programming method.
:param actual_state_x: The current state of the system
3x1 NumPy Array given the state is [x,y,yaw angle] --->
[meters, meters, radians]
:param desired_state_xf: The desired state of the system
3x1 NumPy Array given the state is [x,y,yaw angle] --->
[meters, meters, radians]
:param Q: The state cost matrix
3x3 NumPy Array
:param R: The input cost matrix
2x2 NumPy Array
:param dt: The size of the timestep in seconds -> float
:return: u_star: Optimal action u for the current state
2x1 NumPy Array given the control input vector is
[linear velocity of the car, angular velocity of the car]
[meters per second, radians per second]
"""
# We want the system to stabilize at desired_state_xf.
x_error = actual_state_x - desired_state_xf
# Solutions to discrete LQR problems are obtained using the dynamic
# programming method.
# The optimal solution is obtained recursively, starting at the last
# timestep and working backwards.
# You can play with this number
N = 50
# Create a list of N + 1 elements
P = [None] * (N + 1)
Qf = Q
# LQR via Dynamic Programming
P[N] = Qf
# For i = N, ..., 1
for i in range(N, 0, -1):
# Discrete-time Algebraic Riccati equation to calculate the optimal
# state cost matrix
P[i-1] = Q + A.T @ P[i] @ A - (A.T @ P[i] @ B) @ np.linalg.pinv(
R + B.T @ P[i] @ B) @ (B.T @ P[i] @ A)
# Create a list of N elements
K = [None] * N
u = [None] * N
# For i = 0, ..., N - 1
for i in range(N):
# Calculate the optimal feedback gain K
K[i] = -np.linalg.pinv(R + B.T @ P[i+1] @ B) @ B.T @ P[i+1] @ A
u[i] = K[i] @ x_error
# Optimal control input is u_star
u_star = u[N-1]
return u_star
def main():
# Let the time interval be 1.0 seconds
dt = 1.0
# Actual state
# Our robot starts out at the origin (x=0 meters, y=0 meters), and
# the yaw angle is 0 radians.
actual_state_x = np.array([0,0,0])
# Desired state [x,y,yaw angle]
# [meters, meters, radians]
desired_state_xf = np.array([2.000,2.000,np.pi/2])
# A matrix
# 3x3 matrix -> number of states x number of states matrix
# Expresses how the state of the system [x,y,yaw] changes
# from t-1 to t when no control command is executed.
# Typically a robot on wheels only drives when the wheels are told to turn.
# For this case, A is the identity matrix.
# Note: A is sometimes F in the literature.
A = np.array([ [1.0, 0, 0],
[ 0,1.0, 0],
[ 0, 0, 1.0]])
# R matrix
# The control input cost matrix
# Experiment with different R matrices
# This matrix penalizes actuator effort (i.e. rotation of the
# motors on the wheels that drive the linear velocity and angular velocity).
# The R matrix has the same number of rows as the number of control
# inputs and same number of columns as the number of
# control inputs.
# This matrix has positive values along the diagonal and 0s elsewhere.
# We can target control inputs where we want low actuator effort
# by making the corresponding value of R large.
R = np.array([[0.01, 0], # Penalty for linear velocity effort
[ 0, 0.01]]) # Penalty for angular velocity effort
# Q matrix
# The state cost matrix.
# Experiment with different Q matrices.
# Q helps us weigh the relative importance of each state in the
# state vector (X, Y, YAW ANGLE).
# Q is a square matrix that has the same number of rows as
# there are states.
# Q penalizes bad performance.
# Q has positive values along the diagonal and zeros elsewhere.
# Q enables us to target states where we want low error by making the
# corresponding value of Q large.
Q = np.array([[0.639, 0, 0], # Penalize X position error
[0, 1.0, 0], # Penalize Y position error
[0, 0, 1.0]]) # Penalize YAW ANGLE heading error
# Launch the robot, and have it move to the desired goal destination
for i in range(100):
print(f'iteration = {i} seconds')
print(f'Current State = {actual_state_x}')
print(f'Desired State = {desired_state_xf}')
state_error = actual_state_x - desired_state_xf
state_error_magnitude = np.linalg.norm(state_error)
print(f'State Error Magnitude = {state_error_magnitude}')
B = getB(actual_state_x[2], dt)
# LQR returns the optimal control input
optimal_control_input = lqr(actual_state_x,
desired_state_xf,
Q, R, A, B, dt)
print(f'Control Input = {optimal_control_input}')
# We apply the optimal control to the robot
# so we can get a new actual (estimated) state.
actual_state_x = state_space_model(A, actual_state_x, B,
optimal_control_input)
# Stop as soon as we reach the goal
# Feel free to change this threshold value.
if state_error_magnitude < 0.01:
print("\nGoal Has Been Reached Successfully!")
break
print()
# Entry point for the program
main()