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

任务内容

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

参考内容

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

LQR控制器

参考链接：

Linear Quadratic Regulator (LQR) With Python Code Example

设计方案

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

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

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

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

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

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

调节器的类型 KpTiTdP0.5Ke∞0PI0.45Ke11.2Te0PID0.6Ke0.5Te0.125Te\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} 调节器的类型 PPIPIDKp0.5Ke0.45Ke0.6KeTi∞1.21Te0.5TeTd000.125Te

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

Kp=0.6∗KmKd=Kp∗π4ωKi=Kp∗ωπ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∗πω

其中KpK_{p}Kp为比例控制参数，KiK_{i}Ki为积分控制参数，KdK_{d}Kd为微分控制参数，ω\omegaω为振荡时的频率。

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

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

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

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

[p˙xp¨xβ˙]=[01000g000][pxp˙xβ]+[001][ω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˙yp¨yγ˙]=[01000−g000][pyp˙yγ]+[001][ω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]

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

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

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

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

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

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

x˙=Ax+Bu,xk+1=ADxk+BDuk\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

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

J∞=∫0∞(x(t)⊤Qx(t)+u(t)⊤Ru(t))dtJ_{\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是状态成本矩阵，其行数和列数与状态数相同，该矩阵权衡状态向量中每个状态的相对重要性，而R是输入成本矩阵，行数与控制输入的行数相同，列数与控制输入的列数相同，该矩阵惩罚执行器的工作量。

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

0=−A⊤P∞−P∞A+P∞BR−1B⊤P∞−Qu(t)=−K∞x(t)K∞=R−1B⊤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∞

其中：

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

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

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

0=−P∞,D+AD⊤PD,∞AD−AD⊤PD,∞BD(BD⊤PD,∞BD+R)−1BD⊤PD,∞AD+QuD(k)=−KD,∞xD(k)KD,∞=(BD⊤PD,∞BD+R)−1BD⊤PD,∞AD\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

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

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

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

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

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

因此，我们可以使用俯仰率ωy\omega_{y}ωy来控制机体坐标 x 位置误差，而滚转率ωx\omega_{x}ω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)x(t)x(t)时，将生成最佳控制输入u(t)u(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 seconddef getB(yaw, deltat):"""Calculates and returns the B matrix3x2 matix ---> number of states x number of control inputsExpresses how the state of the system [x,y,yaw] changesfrom 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 Bdef 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 andthe control inputs applied at time t-1:param: A   The A state transition matrix3x3 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 matrix3x2 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 t3x1 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_tdef 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 system3x1 NumPy Array given the state is [x,y,yaw angle] --->[meters, meters, radians]   :param Q: The state cost matrix3x3 NumPy Array:param R: The input cost matrix2x2 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 numberN = 50# Create a list of N + 1 elementsP = [None] * (N + 1)Qf = Q# LQR via Dynamic ProgrammingP[N] = Qf# For i = N, ..., 1for i in range(N, 0, -1):# Discrete-time Algebraic Riccati equation to calculate the optimal # state cost matrixP[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 elementsK = [None] * Nu = [None] * N# For i = 0, ..., N - 1for i in range(N):# Calculate the optimal feedback gain KK[i] = -np.linalg.pinv(R + B.T @ P[i+1] @ B) @ B.T @ P[i+1] @ Au[i] = K[i] @ x_error# Optimal control input is u_staru_star = u[N-1]return u_stardef main():# Let the time interval be 1.0 secondsdt = 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 destinationfor 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_xfstate_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 inputoptimal_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!")breakprint()# Entry point for the program
main()