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

本文讨论了使用PID和LQR控制器对多旋翼飞行器进行控制的设计。LQR控制器通过动态规划方法优化控制输入，涉及状态反馈矩阵的计算和代价函数的选择。文章详细介绍了LQR的步骤、Riccati方程及其在连续和离散时间系统中的应用。此外，还提到为位置控制设计PID控制器，并提出在实现LQR控制器后添加积分器以消除稳态偏移量的建议。

type
status
date
slug
summary
tags
category
icon
password
任务内容参考内容设计方案A：高度和偏航控制——PID控制器B1：x，y位置控制——LQR控制器B2：x，y位置控制——PID控制器C：为x，y位置添加积分器算法逻辑B1：x，y位置控制——LQR控制器代码内容（部分）

任务内容

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

参考内容

LQR控制部分基础参考内容：
LQR控制器——数学推导
参考链接：
Linear Quadratic Regulator (LQR) With Python Code Example
In this tutorial, we will learn about the Linear Quadratic Regulator (LQR). At the end, I'll show you my example implementation of LQR in Python. To get started, let's take a look at what LQR is all about.
Linear Quadratic Regulator (LQR) With Python Code Example
https://automaticaddison.com/linear-quadratic-regulator-lqr-with-python-code-example/
Linear Quadratic Regulator (LQR) With Python Code Example
(LQR)6 DOF Quadcoptor
This project shows how to use LQR linear controller to control a non-linear 6-dof quadcopter. The purpose of LQR controller is to balance between performance of the drone and the energy it consumes. LQR controller can realize it in a systematic way.
https://jordan787878.github.io/firstweb/6dofQuadcopter/6dofQuad_LQR.html

设计方案

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

对于PID的参数整形，可以参考如下三种方案（根据实机情况选择其一，或将多种方案进行相互对比）
  1. 运用齐格勒-尼科尔斯法则（Ziegler-Nichols method）获取初始估测值。
      2. 该方法是基于系统稳定性分析的PID整定方法．在设计过程中无需考虑任何特性要求，整定方法非常简单，但控制效果却比较理想．具体整定方法如下：
      首先，假设只有比例控制，置，然后增加比例系数一直到系统首次出现等幅振荡（闭环系统的极点在ｊω轴上），此时获取系统开始振荡时的临界增益；
      再将该比例系数根据下面的表格乘以对应的参数，这里乘以0.6
      其他参数按照以下公式计算：
      其中为比例控制参数，为积分控制参数，为微分控制参数，为振荡时的频率。
      用上述法则确定PID控制器的参数，使系统的超调量在10%~60%之间，其平均值约为25%。
  1. 使用仿真获取对应的增益值。
  1. 在真实系统中修改对应的增益值。
      2. 方法：缓慢增加 P 直至振荡开始，然后慢慢加入少量D来抑制振荡，然后慢慢添加少量的I来纠正任何稳态误差。

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

对于多旋翼飞行器来说，线性运动方程表明了对于从位置坐标中获取的x坐标和y坐标以及滚转角和俯仰角之间的完全解耦。
因此，我们可以使用俯仰率来控制一个机体坐标 x 位置误差，以及滚转率来控制一个机体坐标 y 位置误差。（内环 → 外环控制）
此任务的目标是分别为每个 x 和 y 设计一个 LQR 控制器。
Linear Quadratic Regulator，首字母缩略词 LQR 代表线性二次调节器。名称本身指定此控制器设计方法适用的设置：
  • 系统的动态是线性的，
  • 要最小化的成本函数 (cost function) 是二次的，
  • 系统化的控制器将状态调节为零。
以下信息总结了合成一个无穷小界LQR控制器的步骤和方程。由此得到的控制器被称为“线性状态反馈控制器”，通常用矩阵“K”表示。状态反馈矩阵K对系统的每个输入有一行，对系统的每个状态有一列。
  • 连续时间和离散时间线性系统动力学分别记为
      • 为了将连续时间系统矩阵A和B转换为离散时间系统矩阵和，使用MATLAB函数c2d，指定零阶保持 (zero order hold) 作为离散化方法。
  • 需要选择Q和R成本函数 (cost function) 矩阵来实现飞行器的期望飞行性能。在无限时间跨度 (infinite-horizon) 的LQR代价函数为：
    • Q是状态成本矩阵，其行数和列数与状态数相同，该矩阵权衡状态向量中每个状态的相对重要性，而R是输入成本矩阵，行数与控制输入的行数相同，列数与控制输入的列数相同，该矩阵惩罚执行器的工作量。
        • 要选择 Q 和 R 成本函数矩阵，应该考虑状态向量的不同元素。例如，也许您希望惩罚10厘米的 x 位置偏差与偏航 5 度偏差的量相同。
    • 使用 MATLAB 函数 care 和 dare 分别计算连续和离散时间的 Riccati 代数方程。可以参考MATLAB阅读帮助文档从而了解这些函数的计算。
    • 连续时间线性时间不变（LTI）的无限时间跨度 LQR 设计方程系统是（直接取自控制系统I讲义）：
        • 其中：
        第一个方程是求解的 Riccati 方程
        第二个是实现的控制率
        第三个是状态反馈增益矩阵
    • 离散时间 LTI 系统的无限时间跨度 LQR 设计方程为：
        • 其中下标表示适用于离散时间系统的变量
    • care 和 dare 函数都可以返回状态反馈增益矩阵，如果使用此矩阵，需要注意符号约定。

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

    如果您还希望为(x, y)位置实现一个PID控制器，这是相关的任务描述。
    基于A方案中实现一个针对高度和偏航的PID控制器的进一步任务：设计、实现和调整一个PID控制器来控制 x 和 y 位置。
    回想一下，在B方案中提供的线性化的运动方程，表明了 x 位置和俯仰角与 y 位置和滚转角之间的完全解耦。
    因此，我们可以使用俯仰率来控制机体坐标 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 将保持最佳反馈增益值。这是一个重要的矩阵，当乘以状态误差时，将生成最佳控制输入（请参阅下一步）。
    • 循环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控制系统的代码设计
     
    Loading...