任务内容
通过调整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_{d}=k_{i}=0
kd=ki=0，然后增加比例系数一直到系统首次出现等幅振荡（闭环系统的极点在ｊω轴上），此时获取系统开始振荡时的临界增益
K
m
K_{m}
Km；
再将该比例系数根据下面的表格乘以对应的参数，这里乘以0.6
调节器的类型
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|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
其他参数按照以下公式计算：
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∗πω
其中
K
p
K_{p}
Kp为比例控制参数，
K
i
K_{i}
Ki为积分控制参数，
K
d
K_{d}
Kd为微分控制参数，
ω
\omega
ω为振荡时的频率。
用上述法则确定PID控制器的参数，使系统的超调量在10%~60%之间，其平均值约为25%。
-
使用仿真获取对应的增益值。
-
在真实系统中修改对应的增益值。
方法：缓慢增加 P 直至振荡开始，然后慢慢加入少量D来抑制振荡，然后慢慢添加少量的I来纠正任何稳态误差。
B1：x，y位置控制——LQR控制器
对于多旋翼飞行器来说，线性运动方程表明了对于从位置坐标中获取的x坐标
p
x
p_{x}
px和y坐标
p
y
p_{y}
py以及滚转角
β
\beta
β和俯仰角
γ
\gamma
γ之间的完全解耦。
[
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]
因此，我们可以使用俯仰率
ω
y
\omega_{y}
ωy来控制一个机体坐标 x 位置误差，以及滚转率
ω
x
\omega_{x}
ωx来控制一个机体坐标 y 位置误差。（内环 → 外环控制）
此任务的目标是分别为每个 x 和 y 设计一个 LQR 控制器。
Linear Quadratic Regulator，首字母缩略词 LQR 代表线性二次调节器器。名称本身指定此控制器设计方法适用的设置：
- 系统的动态是线性的，
- 要最小化的成本函数 (cost function) 是二次的，
- 系统化的控制器将状态调节为零。
以下信息总结了合成一个无穷小界LQR控制器的步骤和方程。由此得到的控制器被称为“线性状态反馈控制器”，通常用矩阵“K”表示。状态反馈矩阵K对系统的每个输入有一行，对系统的每个状态有一列。
-
连续时间和离散时间线性系统动力学分别记为
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
为了将连续时间系统矩阵A和B转换为离散时间系统矩阵
A
D
A_{D}
AD和
B
D
B_{D}
BD，使用MATLAB函数c2d，指定零阶保持 (zero order hold) 作为离散化方法。
-
需要选择Q和R成本函数 (cost function) 矩阵来实现飞行器的期望飞行性能。在无限时间跨度 (infinite-horizon) 的LQR代价函数为：
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是状态成本矩阵，其行数和列数与状态数相同，该矩阵权衡状态向量中每个状态的相对重要性，而R是输入成本矩阵，行数与控制输入的行数相同，列数与控制输入的列数相同，该矩阵惩罚执行器的工作量。
要选择 Q 和 R 成本函数矩阵，应该考虑状态向量的不同元素。例如，也许您希望惩罚10厘米的 x 位置偏差与偏航 5 度偏差的量相同。
-
使用 MATLAB 函数 care 和 dare 分别计算连续和离散时间的 Riccati 代数方程。可以参考MATLAB阅读帮助文档从而了解这些函数的计算。
-
连续时间线性时间不变（LTI）的无限时间跨度 LQR 设计方程系统是（直接取自控制系统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∞
其中：
第一个方程是求解
P
∞
P_{\infty}
P∞的 Riccati 方程
第二个是实现的控制率
u
(
t
)
u(t)
u(t)
第三个是状态反馈增益矩阵
K
∞
K_{\infty}
K∞
-
离散时间 LTI 系统的无限时间跨度 LQR 设计方程为：
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
其中下标
(
.
)
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
np.set_printoptions(precision=3,suppress=True)
max_linear_velocity = 3.0
max_angular_velocity = 1.5708
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]
"""
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]
"""
x_error = actual_state_x - desired_state_xf
N = 50
P = [None] * (N + 1)
Qf = Q
P[N] = Qf
for i in range(N, 0, -1):
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)
K = [None] * N
u = [None] * N
for i in range(N):
K[i] = -np.linalg.pinv(R + B.T @ P[i+1] @ B) @ B.T @ P[i+1] @ A
u[i] = K[i] @ x_error
u_star = u[N-1]
return u_star
def main():
dt = 1.0
actual_state_x = np.array([0,0,0])
desired_state_xf = np.array([2.000,2.000,np.pi/2])
A = np.array([ [1.0, 0, 0],
[ 0,1.0, 0],
[ 0, 0, 1.0]])
R = np.array([[0.01, 0],
[ 0, 0.01]])
Q = np.array([[0.639, 0, 0],
[0, 1.0, 0],
[0, 0, 1.0]])
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)
optimal_control_input = lqr(actual_state_x,
desired_state_xf,
Q, R, A, B, dt)
print(f'Control Input = {optimal_control_input}')
actual_state_x = state_space_model(A, actual_state_x, B,
optimal_control_input)
if state_error_magnitude < 0.01:
print("\nGoal Has Been Reached Successfully!")
break
print()
main()
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
