使用PID和LQR控制器进行多旋翼飞行器控制

Phillweston

已于 2022-02-15 07:56:30 修改

阅读量3.3k

点赞数 2

分类专栏：控制理论无人机程序设计文章标签：无人机控制理论

于 2022-02-15 07:56:09 首次发布

本文链接：https://blog.csdn.net/qq_26550927/article/details/122935543

版权

无人机同时被 3 个专栏收录

22 篇文章 24 订阅

订阅专栏

程序设计

17 篇文章 2 订阅

订阅专栏

控制理论

5 篇文章 1 订阅

订阅专栏

任务内容

通过调整PID和LQR控制器以实现稳定悬停的多旋翼飞行器，运用在无论是在仿真中还是在实际系统中。

参考内容

LQR控制部分基础参考内容：

LQR控制器

参考链接：

Linear Quadratic Regulator (LQR) With Python Code Example

设计方案

A：高度和偏航控制——PID控制器

对于PID的参数整形，可以参考如下三种方案（根据实机情况选择其一，或将多种方案进行相互对比）

运用齐格勒-尼科尔斯法则（Ziegler-Nichols method）获取初始估测值。

该方法是基于系统稳定性分析的PID整定方法．在设计过程中无需考虑任何特性要求，整定方法非常简单，但控制效果却比较理想．具体整定方法如下：

首先，假设只有比例控制，置 $k_{d}=k_{i}=0$ ，然后增加比例系数一直到系统首次出现等幅振荡（闭环系统的极点在ｊω轴上），此时获取系统开始振荡时的临界增益 $K_{m}$ ；

再将该比例系数根据下面的表格乘以对应的参数，这里乘以0.6

$\begin{array}{|c|c|c|c|}\hline \text{ 调节器的类型 } & 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}$

其他参数按照以下公式计算：

$K_{p}=0.6*K_{m}\\K_{d}=K_{p}*\frac{\pi}{4\omega}\\K_{i}=K_{p}*\frac{\omega}{\pi}$

其中 $K_{p}$ 为比例控制参数， $K_{i}$ 为积分控制参数， $K_{d}$ 为微分控制参数， $\omega$ 为振荡时的频率。

用上述法则确定PID控制器的参数，使系统的超调量在10%~60%之间，其平均值约为25%。
使用仿真获取对应的增益值。
在真实系统中修改对应的增益值。

方法：缓慢增加 P 直至振荡开始，然后慢慢加入少量D来抑制振荡，然后慢慢添加少量的I来纠正任何稳态误差。

B1：x，y位置控制——LQR控制器

对于多旋翼飞行器来说，线性运动方程表明了对于从位置坐标中获取的x坐标 $p_{x}$ 和y坐标 $p_{y}$ 以及滚转角 $\beta$ 和俯仰角 $\gamma$ 之间的完全解耦。

$\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]$

$\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]$

因此，我们可以使用俯仰率 $\omega_{y}$ 来控制一个机体坐标 x 位置误差，以及滚转率 $\omega_{x}$ 来控制一个机体坐标 y 位置误差。（内环 → 外环控制）

此任务的目标是分别为每个 x 和 y 设计一个 LQR 控制器。

Linear Quadratic Regulator，首字母缩略词 LQR 代表线性二次调节器器。名称本身指定此控制器设计方法适用的设置：

系统的动态是线性的，
要最小化的成本函数 (cost function) 是二次的，
系统化的控制器将状态调节为零。

以下信息总结了合成一个无穷小界LQR控制器的步骤和方程。由此得到的控制器被称为“线性状态反馈控制器”，通常用矩阵“K”表示。状态反馈矩阵K对系统的每个输入有一行，对系统的每个状态有一列。

连续时间和离散时间线性系统动力学分别记为

$\dot{x}=A x+B u, \quad x_{k+1}=A_{\mathrm{D}} x_{k}+B_{\mathrm{D}} u_{k}$

为了将连续时间系统矩阵A和B转换为离散时间系统矩阵 $A_{D}$ 和 $B_{D}$ ，使用MATLAB函数c2d，指定零阶保持 (zero order hold) 作为离散化方法。
需要选择Q和R成本函数 (cost function) 矩阵来实现飞行器的期望飞行性能。在无限时间跨度 (infinite-horizon) 的LQR代价函数为：

$J_{\infty}=\int_{0}^{\infty}\left(x(t)^{\top} Q x(t)+u(t)^{\top} R u(t)\right) d t$
Q是状态成本矩阵，其行数和列数与状态数相同，该矩阵权衡状态向量中每个状态的相对重要性，而R是输入成本矩阵，行数与控制输入的行数相同，列数与控制输入的列数相同，该矩阵惩罚执行器的工作量。

要选择 Q 和 R 成本函数矩阵，应该考虑状态向量的不同元素。例如，也许您希望惩罚10厘米的 x 位置偏差与偏航 5 度偏差的量相同。
使用 MATLAB 函数 care 和 dare 分别计算连续和离散时间的 Riccati 代数方程。可以参考MATLAB阅读帮助文档从而了解这些函数的计算。
连续时间线性时间不变（LTI）的无限时间跨度 LQR 设计方程系统是（直接取自控制系统I讲义）：

$\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}$

其中：

第一个方程是求解 $P_{\infty}$ 的 Riccati 方程

第二个是实现的控制率 $u (t)$

第三个是状态反馈增益矩阵 $K_{\infty}$
离散时间 LTI 系统的无限时间跨度 LQR 设计方程为：

$\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}$

其中下标 $_{D}$ 表示适用于离散时间系统的变量
care 和 dare 函数都可以返回状态反馈增益矩阵，如果使用此矩阵，需要注意符号约定。

B2：x，y位置控制——PID控制器

如果您还希望为(x, y)位置实现一个PID控制器，这是相关的任务描述。

基于A方案中实现一个针对高度和偏航的PID控制器的进一步任务：设计、实现和调整一个PID控制器来控制 x 和 y 位置。

回想一下，在B方案中提供的线性化的运动方程，表明了 x 位置和俯仰角与 y 位置和滚转角之间的完全解耦。

因此，我们可以使用俯仰率 $\omega_{y}$ 来控制机体坐标 x 位置误差，而滚转率 $\omega_{x}$ 来控制机体坐标 y 位置误差。

由于俯仰（或滚转）角度的动力学比x(或y)位置的动力学足够快，因此我们也可以用一个嵌套的控制器合理地控制位置误差。“外部控制”环取一个x位置误差，并请求一个俯仰角β来纠正误差，然后“内环”取这个请求的俯仰角β的误差，并请求一个关于机体坐标y轴的角速率ωy来纠正误差。

C：为x，y位置添加积分器

一旦您完成了先前B任务中(x, y)位置的LQR控制器的实现，请观察在跟踪(x, y)设定点时的稳态偏移量。尝试调整飞行器中的电池的位置几毫米，并再次观察稳态偏移量。

观察稳态偏移量，并在每个 x 和 y 位置控制器上添加一个积分器，以消除稳态偏移量。

算法逻辑

B1：x，y位置控制——LQR控制器

LQR 使用一种称为动态规划的技术。当飞行器在世界上移动时，在每个时间步长t处，使用一系列 for 循环（其中我们运行每个for循环N次）计算最佳控制输入，这些循环输出对应于最小总成本的 u（即控制输入）。

初始化 LQR 函数：传入 7 个参数。

LQR(Actual State x, Desired State xf, Q, R, A, B, dt);

x_error = Actual State x – Desired State xf
初始化时间步长 N

通常将N设置为某个任意整数，如50。LQR 问题的解决方案以递归方式获得，从最后一个时间步开始，并及时向后工作到第一个时间步。

换句话说，for 循环（我们将在一秒钟内到达这些循环）需要经过大量迭代（在本例中为 50 次）才能达到 P 的稳定值（我们将在下一步中介绍 P）。
初始化 P 为包含 N+1 个元素的空列表

令 P[N]=Q

循环i从N到1，分别用以下公式 (Riccati方程) 计算 P[i-1]

P[i-1] = Q + ATP[i]A – (ATP[i]B)(R + BTP[i]B)-1(BTP[i]A)
初始化 K 和 u 分别为包含 N 个元素的空列表

循环i从0到N-1，分别用以下公式计算状态反馈增益矩阵 K[i]

K[i] = -(R + BTP[i+1]B)-1BTP[i+1]A

K 将保持最佳反馈增益值。这是一个重要的矩阵，当乘以状态误差 $x (t)$ 时，将生成最佳控制输入 $u (t)$ （请参阅下一步）。
循环i从0到N-1，分别用以下公式计算控制输入

u[i] = K[i] @ x_error

我们对for循环进行N次迭代，直到我们得到最优控制输入u的稳定值（例如u = [线速度v，角速度ω]）。我假设当N = 50时达到稳定值。
返回最佳控制量u_star

u_star = u[N-1]

最佳控制输入u_star。这是我们在上面的上一步中计算的最后一个值。它位于u列表的最后。

返回最佳控制输入。这些输入将被发送到我们的飞行器，以便它可以移动到新的状态（即新的x位置，y位置和偏航角γ）。

代码内容（部分）

这里以双轮小车为例，分析LQR控制系统的代码设计

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()