LQR调参实战:如何用Python快速实现倒立摆稳定控制(附完整代码)
如果你正在机器人控制或自动化领域摸索,想找一个既经典又实用的控制器设计方法,那么LQR(线性二次调节器)绝对是你绕不开的一站。它不像某些玄学般的调参方法,LQR提供了一套清晰的数学框架,让你能系统地权衡“状态误差”和“控制代价”。但理论归理论,真正把它用起来,尤其是在像倒立摆这样经典的欠驱动系统上,很多人还是会卡在调参这一步。这篇文章,我就想抛开那些冗长的公式推导,直接带你上手,用Python一步步搭建一个倒立摆的LQR控制器,并聚焦于最关键的环节——如何高效地调整Q和R这两个权重矩阵,让摆杆从晃晃悠悠到稳稳立住。整个过程,我会分享一些从仿真到实际调试中积累的“手感”和经验,希望能帮你少走弯路。
1. 从零搭建:倒立摆的数学模型与线性化
在动手写代码之前,我们必须先理解要控制的对象。一个典型的旋转倒立摆系统,通常包含一个可以绕轴旋转的摆杆和一个驱动摆杆的电机。我们的目标是,无论摆杆初始是下垂还是被扰动,都能通过控制电机转矩,让它快速且稳定地保持在上方的垂直平衡位置。
1.1 定义状态变量与动力学方程
首先,我们需要用数学语言描述这个系统。对于倒立摆,最核心的状态变量通常有四个:
- 摆杆角度(θ):偏离垂直向上位置的角度。这是我们最关心的。
- 摆杆角速度(θ_dot):角度随时间的变化率。
- 电机(或小车)位置(x):在某些倒立摆变体中存在。
- 电机速度(x_dot):位置的变化率。
为了简化,我们这里先考虑一个经典的、忽略电机位置的水平旋转倒立摆模型。其非线性动力学方程可以通过拉格朗日力学推导出来,它描述了摆杆角度、角速度与控制转矩(u)之间的关系。这个方程通常形如:
[ (J + ml^2)\ddot{\theta} + b\dot{\theta} - mgl \sin(\theta) = u ]
其中:
J是摆杆的转动惯量。m和l分别是摆杆的质量和质心到转轴的长度。b是粘滞摩擦系数。g是重力加速度。u是施加的控制转矩。
这个方程是非线性的,主要是因为包含了 sin(θ) 项。而LQR是线性控制器,所以下一步必须进行线性化。
1.2 在平衡点附近线性化
LQR设计基于线性状态空间模型。我们的控制目标是让摆杆稳定在垂直向上位置,即平衡点 θ = 0。我们在这一点附近对系统进行线性化处理。
线性化的核心技巧是使用小角度假设:当 θ 很小时,sin(θ) ≈ θ,cos(θ) ≈ 1。将这个近似代入原非线性方程,并定义状态向量 x = [θ, θ_dot]^T,我们可以得到线性化后的状态空间方程:
[ \dot{x} = A x + B u ]
其中,系统矩阵 A 和输入矩阵 B 的具体形式为:
[ A = \begin{bmatrix} 0 & 1 \ \frac{mgl}{J+ml^2} & -\frac{b}{J+ml^2} \end{bmatrix}, \quad B = \begin{bmatrix} 0 \ \frac{1}{J+ml^2} \end{bmatrix} ]
这个 A, B 矩阵对就是我们设计LQR控制器的基石。它们的准确性直接影响到最终的控制效果。在实际编程中,我们会先定义系统的物理参数,然后由代码计算出 A 和 B。
import numpy as np
# 倒立摆物理参数
m = 0.1 # 摆杆质量 (kg)
l = 0.3 # 质心到转轴长度 (m)
J = 0.006 # 摆杆转动惯量 (kg*m^2)
b = 0.01 # 摩擦系数 (N*m*s)
g = 9.81 # 重力加速度 (m/s^2)
# 计算线性化后的系统矩阵 A 和输入矩阵 B
denominator = J + m * l**2
A = np.array([[0, 1],
[m*g*l / denominator, -b / denominator]])
B = np.array([[0],
[1 / denominator]])
print("系统矩阵 A:")
print(A)
print("\n输入矩阵 B:")
print(B)
注意:这里的参数(m, l, J, b)需要根据你实际使用的倒立摆硬件或高保真仿真模型进行标定。不准确的模型参数会导致基于模型设计的LQR控制器性能下降,甚至不稳定。
2. LQR核心:理解代价函数与权重矩阵的艺术
拿到了线性模型 (A, B),接下来就是LQR的舞台了。LQR的核心思想是定义一个衡量控制效果“好坏”的标尺——二次型代价函数,然后找出使这个“坏处”累计值最小的控制律。
2.1 代价函数的直观解读
连续时间LQR的代价函数 J 定义为:
[ J = \int_0^\infty (x^T Q x + u^T R u) , dt ]
这个公式包含了两部分:
- 状态代价
x^T Q x:惩罚系统状态偏离平衡点(通常是零点)的程度。Q越大,控制器就越“讨厌”状态误差,会不惜一切代价(甚至很大的控制量)快速把误差压到零。 - 控制代价
u^T R u:惩罚控制输入(如电机转矩)的大小。R越大,控制器就越“吝啬”,倾向于使用更小的控制力,但这可能导致响应变慢。
所以,调参的本质,就是在“快速收敛”和“控制能耗/执行器压力”之间做权衡。Q 和 R 就是你这个“裁判”手中的打分牌。
2.2 权重矩阵 Q 和 R 的设置策略
Q 和 R 通常被选为对角矩阵,这非常方便,因为对角线上的元素直接对应着每个状态变量和控制输入的权重。
-
对于状态权重矩阵 Q:
- 它是一个
n x n的矩阵(n为状态数,本例中n=2)。 Q[0,0]对应角度θ的权重。我们希望摆杆直立,所以这个值通常需要设得比较大。Q[1,1]对应角速度θ_dot的权重。设置它可以抑制摆杆的晃动速度,增加系统阻尼。- 非对角线元素一般设为0,除非你明确知道状态之间的耦合需要被特别惩罚。
- 它是一个
-
对于控制权重矩阵 R:
- 它是一个
m x m的矩阵(m为控制输入数,本例中m=1)。 - 在本例中,
R就是一个标量[r]。r越大,控制力就越受限制。
- 它是一个
一个经典的初始设置策略是 Bryson’s Rule:选择 Q 和 R 的对角元素,使得每个状态和输入的允许最大偏差的平方的倒数作为权重。例如,如果你认为角度 θ 的最大可接受偏差是0.1弧度,那么 Q[0,0] 可以设为 1 / (0.1^2) = 100。这为调参提供了一个有理有据的起点。
# 根据 Bryson‘s Rule 设定初始 Q 和 R
theta_max = 0.1 # 允许的最大角度偏差 (rad)
theta_dot_max = 1.0 # 允许的最大角速度 (rad/s)
u_max = 2.0 # 允许的最大控制转矩 (N*m)
Q = np.array([[1.0 / (theta_max**2), 0],
[0, 1.0 / (theta_dot_max**2)]])
R = np.array([[1.0 / (u_max**2)]])
print("根据 Bryson‘s Rule 设定的初始权重矩阵:")
print("Q = ")
print(Q)
print("R = ", R)
3. 实战求解:用Python计算LQR反馈增益K
有了 A, B, Q, R,求解LQR控制器就转化为一个标准的数学计算问题:求解连续时间代数Riccati方程(CARE)。幸运的是,我们完全不需要手动推导,利用 SciPy 库可以一键求解。
3.1 求解Riccati方程与增益计算
scipy.linalg.solve_continuous_are 函数就是干这个的。它返回Riccati方程的解矩阵 P,然后最优反馈增益矩阵 K 由公式 ( K = R^{-1} B^T P ) 计算得出。
from scipy.linalg import solve_continuous_are
# 使用上面定义的 A, B, Q, R
# 求解连续时间代数Riccati方程 (CARE)
P = solve_continuous_are(A, B, Q, R)
# 计算最优反馈增益矩阵 K
K = np.linalg.inv(R) @ B.T @ P # 等价于 np.dot(np.dot(np.linalg.inv(R), B.T), P)
print("Riccati方程的解矩阵 P:")
print(P)
print("\n最优反馈增益矩阵 K:")
print(K)
得到的 K 是一个 1x2 的矩阵(因为1个输入控制2个状态)。控制律就是 u = -K * x。这意味着,控制器会实时测量当前的角度 θ 和角速度 θ_dot,然后按照 K[0]*θ + K[1]*θ_dot 的比例计算出一个控制转矩,但符号为负(负反馈),用于抵消偏差。
3.2 验证闭环系统稳定性
计算出 K 后,一个良好的习惯是立即检查闭环系统 A_cl = A - B@K 的特征值。所有特征值都应具有负实部(位于复平面的左半平面),系统才是稳定的。
# 计算闭环系统矩阵
A_closed = A - B @ K
# 计算闭环系统特征值
eig_vals = np.linalg.eigvals(A_closed)
print("闭环系统矩阵 A_cl:")
print(A_closed)
print("\n闭环系统特征值:")
for eig in eig_vals:
print(f" {eig:.4f}")
# 稳定性判断
if np.all(np.real(eig_vals) < 0):
print("✓ 闭环系统稳定。")
else:
print("✗ 闭环系统不稳定!需要重新调整 Q 或 R。")
4. 调参心法:从仿真到“手感”的迭代策略
理论计算出的 K 保证了线性化模型在平衡点附近的稳定性,但实际性能如何?能否应对较大的初始偏差?这就要靠调参了。调参是一个迭代和权衡的过程。
4.1 建立仿真环境进行测试
我们需要一个仿真循环来模拟倒立摆的实时控制过程。这里使用欧拉积分进行简单的时间步进。注意,仿真中我们使用完整的非线性动力学模型来评估控制器性能,这比单纯的线性仿真更有说服力。
import matplotlib.pyplot as plt
def simulate_inverted_pendulum(A, B, K, initial_angle=0.2, sim_time=5.0, dt=0.01):
"""
模拟倒立摆的LQR控制过程。
使用非线性模型进行仿真,用线性LQR控制律。
"""
# 初始化
t = 0
theta = initial_angle # 初始角度偏移
theta_dot = 0.0 # 初始角速度
time_history = [t]
angle_history = [theta]
control_history = [0.0]
# 物理参数(与之前定义一致)
# ... (m, l, J, b, g 应作为参数传入或使用全局变量)
while t < sim_time:
# 1. 构建当前状态向量 (线性化点附近的状态)
x = np.array([[theta], [theta_dot]])
# 2. 计算LQR控制量 u = -K*x
u = -K @ x
u = u[0, 0] # 转换为标量
# 3. 使用**非线性模型**更新下一个状态 (欧拉积分)
# 非线性动力学方程: (J+ml^2)*theta_ddot + b*theta_dot - mgl*sin(theta) = u
theta_ddot = (u + m*g*l*np.sin(theta) - b*theta_dot) / (J + m*l**2)
theta_dot += theta_ddot * dt
theta += theta_dot * dt
# 4. 记录数据
t += dt
time_history.append(t)
angle_history.append(theta)
control_history.append(u)
return time_history, angle_history, control_history
# 运行仿真
time, angle, ctrl = simulate_inverted_pendulum(A, B, K, initial_angle=0.3, sim_time=3.0)
# 绘制结果
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6))
ax1.plot(time, angle)
ax1.set_ylabel('摆杆角度 (rad)')
ax1.set_title('LQR控制下倒立摆角度响应')
ax1.grid(True)
ax1.axhline(y=0, color='r', linestyle='--', alpha=0.5) # 目标线
ax2.plot(time, ctrl)
ax2.set_xlabel('时间 (s)')
ax2.set_ylabel('控制转矩 (N*m)')
ax2.set_title('控制输入历史')
ax2.grid(True)
plt.tight_layout()
plt.show()
4.2 系统性调参流程与问题诊断
运行第一次仿真后,你可能会看到各种不理想的曲线。别急,我们可以根据响应形状来调整 Q 和 R。
| 观察到的现象 | 可能的原因 | 调整方向 |
|---|---|---|
| 收敛太慢 | 状态惩罚不够,或控制惩罚太重。 | 增大 Q 的对角元(特别是角度权重),或减小 R。 |
| 超调过大或振荡 | 角速度阻尼不足,或者状态惩罚相对于控制惩罚过强。 | 增大 Q 中角速度的权重(Q[1,1]),或适当增大 R 以平滑控制动作。 |
| 控制输入饱和(达到执行器极限) | R 太小,控制器过于“激进”。 | 显著增大 R,或者检查 Q 是否过大,需要整体平衡。 |
| 稳态存在微小振荡 | 可能是离散仿真步长或噪声导致,也可能是增益略高。 | 微调 Q 或 R,或在实际系统中加入低通滤波。 |
一个我常用的迭代流程是:
- 固定R,调Q:先将
R设为一个较小的固定值(如[0.01]),然后主要调整Q。优先调整角度权重Q[0,0],直到响应速度满意。如果出现振荡,再增加角速度权重Q[1,1]来增加阻尼。 - 固定Q,调R:在响应形状基本满意后,观察控制输入
u的大小。如果u峰值远超你的执行器(如电机)能力,就需要增大R来限制控制量。增大R通常会减慢响应,所以可能需要回到步骤1微调Q进行补偿。 - 权衡与折中:反复进行1和2,直到在响应速度、超调/振荡和控制量大小三者之间找到一个可接受的平衡点。
提示:在实际硬件上调试时,务必先从非常保守的参数(大
R, 小Q)开始,逐步增加“攻击性”,并密切关注执行器温度和状态,防止损坏设备。
4.3 进阶技巧:处理输出调节与积分抗扰
基础的LQR是状态调节器,目标是让状态回到零点。但有时我们希望摆杆稳定在一个非零的角度(比如跟踪一个缓慢变化的期望角度),或者消除稳态误差。这时就需要引入参考输入前馈和积分控制。
- 前馈控制:如果期望状态是
x_ref,控制律可修改为u = -K*(x - x_ref)。对于常数x_ref,这需要重新求解一个包含参考点的Riccati方程,或者简单计算稳态控制量u_ff。 - 积分抗扰:在状态向量中增加误差的积分项,构成LQI(Linear-Quadratic-Integral)控制器,可以有效抑制常值干扰。这需要扩展系统状态矩阵
A和B,并重新设计Q。
# 示例:为角度控制增加积分环节 (LQI简化思路)
# 1. 扩展系统状态,增加角度误差的积分 state_int
# 新的状态向量: x_aug = [theta, theta_dot, integral_of(theta_error)]^T
# 2. 构建增广系统矩阵 A_aug, B_aug
# 3. 为增广系统设计新的 Q_aug 和 R,求解新的增益 K_aug = [Kp, Kd, Ki]
# 4. 控制律: u = -Kp*theta - Kd*theta_dot - Ki*state_int
# 注意:这是一个概念示例,具体实现需谨慎处理积分抗饱和等问题。
调参没有银弹,它混合了理论指导、仿真验证和实际经验。最好的学习方法就是动手修改代码中的 Q 和 R,观察仿真曲线的变化,慢慢培养出对这两个矩阵影响的直觉。当你看到摆杆从跌落中迅速甩起并稳稳立住时,那种成就感就是对你调试工作最好的回报。
391



被折叠的 条评论
为什么被折叠?



