从倒立摆到无人机：拆解LQR算法在机器人控制中的实战应用与调优
从倒立摆到无人机：拆解LQR算法在机器人控制中的实战应用与调优
在机器人控制领域，工程师们常常面临一个核心挑战：如何让系统在保持稳定的同时，还能高效地完成既定任务。无论是实验室里摇晃的倒立摆，还是空中自主飞行的无人机，都需要精妙的控制算法来平衡响应速度与能耗。LQR（线性二次调节器）作为一种经典的最优控制方法，正因其数学优雅和工程实用性，成为解决这类问题的利器。
不同于传统PID控制的经验调参，LQR通过量化系统状态与控制输入的代价，自动计算出最优反馈矩阵。但真正让LQR发挥威力的，在于工程师如何根据具体场景设计合理的代价函数。本文将从一个经典案例——倒立摆控制出发，逐步拆解LQR的实战应用技巧，并延伸到无人机姿态控制等复杂场景。
1. 倒立摆：理解LQR控制的理想试验场
倒立摆系统由可移动小车和顶端自由旋转的摆杆组成，看似简单却蕴含着丰富的控制原理。其非线性动力学方程经过工作点线性化后，可表示为状态空间模型：
ẋ = Ax + Bu y = Cx + Du其中状态向量x通常包含小车位置、速度、摆杆角度和角速度。对于典型的实验室倒立摆，A和B矩阵可能如下：
# 倒立摆线性化模型示例（参数需根据实际系统确定） A = [[0, 1, 0, 0], [0, -0.1, 3, 0], [0, 0, 0, 1], [0, -0.5, 30, 0]] B = [[0], [0.5], [0], [1.5]]设计Q矩阵的物理考量需要明确控制目标优先级：
- 若主要防止摆杆倒下，应加大角度θ和角速度θ̇的权重
- 若需限制小车移动范围，则需提高位置x和速度ẋ的代价系数
例如，针对摆杆稳定优先的场景：
Q = np.diag([1, 0.1, 10, 2]) # 角度权重最大 R = [0.1] # 控制输入权重通过Riccati方程求解得到的K矩阵，将实现状态变量的最优加权反馈。实际调试时，可先用仿真验证控制器性能：
% MATLAB中LQR设计示例 [K, S, e] = lqr(A, B, Q, R); sys_cl = ss(A-B*K, B, C, D); step(sys_cl); % 查看阶跃响应2. Q与R矩阵：平衡性能与能耗的艺术
LQR的核心哲学体现在代价函数J的设计上：
J = ∫(xᵀQx + uᵀRu)dtQ矩阵的工程实践需要关注：
- 对角元素比值决定状态变量的收敛优先级
- 非对角项可处理耦合状态的关系（但通常从对角矩阵开始）
- 过大的Q值可能导致控制输入饱和
R矩阵的调节技巧：
- 增大R值可降低执行器能耗，但会减慢响应速度
- 实际系统中需考虑电机扭矩限制
- 建议从R=1开始，按数量级调整
下表展示了不同Q/R设置对倒立摆控制的影响对比：
| 参数组合 | 摆杆稳定时间(s) | 最大控制输入 | 能量消耗指数 |
|---|---|---|---|
| Q=diag([1,1,10,2]), R=0.1 | 1.2 | 3.5N | 1.8 |
| Q=diag([5,1,5,1]), R=1 | 2.1 | 2.1N | 1.2 |
| Q=diag([10,5,20,5]), R=0.01 | 0.8 | 8.7N | 4.5 |
提示：实际调试时可先固定R=1，调整Q观察系统响应，再微调R值优化能耗。记录每次参数变更的性能指标，建立参数-性能对应关系。
3. 从仿真到实机：LQR的工程实现细节
当仿真结果满意后，将LQR部署到真实系统还需考虑：
离散化处理： 大多数数字控制系统需要离散时间LQR设计：
from scipy import signal dt = 0.01 # 采样周期 A_d, B_d, _, _, _ = signal.cont2discrete((A, B, C, D), dt)抗饱和策略： 当控制输入超出执行器限幅时，可引入：
- 积分抗饱和（LQI）
- 输入限幅+积分补偿
- 参考轨迹平滑
状态估计： 实际系统可能无法测量所有状态，需设计观测器：
// 卡尔曼滤波器示例（嵌入式C实现） void kalman_update(float z) { pred_x = A * x; pred_P = A * P * A_T + Q_kalman; K = pred_P * H_T / (H * pred_P * H_T + R_kalman); x = pred_x + K * (z - H * pred_x); P = (I - K * H) * pred_P; }4. 进阶应用：LQR在无人机姿态控制中的变体
四旋翼无人机姿态控制面临比倒立摆更复杂的动力学特性。典型改进方案包括：
LQR+积分控制（LQI）： 为消除稳态误差，增加误差积分状态：
A_aug = [A, zeros(4,1); -C, 0] B_aug = [B; 0]时变LQR： 针对轨迹跟踪任务，设计随时间变化的Q(t)矩阵：
def time_varying_Q(t): if t < 2: # 起飞阶段注重角度稳定 return np.diag([1, 0.5, 10, 2, 0.1]) else: # 巡航阶段平衡位置与能耗 return np.diag([5, 1, 5, 1, 0.5])鲁棒LQR设计： 考虑模型不确定性，采用H∞方法设计鲁棒控制器：
[Krob, ~, ~] = lqr(A, B, Q, R, N);在实机调试中，建议采用渐进式验证：
- 先在姿态锁定模式下测试基础LQR
- 逐步增加积分项权重
- 最后测试轨迹跟踪性能
调试过程中，记录以下关键数据有助于分析：
- 状态变量的收敛曲线
- 控制输入的时间序列
- 能量消耗累计值
- 外部干扰下的恢复时间