PID与LQR在二级倒立摆控制中的对比与实践

1. 项目概述：当PID遇上LQR的倒立摆对决

去年在实验室调试双足机器人时，我遇到了一个棘手的问题——机器人在快速转向时总会出现明显的姿态振荡。这个问题让我想起了研究生时期做过的一个经典实验：二级倒立摆控制。作为控制理论中的"Hello World"，倒立摆系统完美诠释了什么是"不稳定系统"：就像试图用指尖平衡一根长杆，稍有不慎就会失去控制。

二级倒立摆比普通单级倒立摆难度更高，它由两个串联的摆杆组成，就像杂技演员同时用下巴和额头顶起两根竹竿。这种系统有六个状态变量（小车位置、两个摆杆角度及其导数），表现出强烈的非线性和耦合特性。本文将通过MATLAB仿真，对比经典PID控制和现代LQR控制在二级倒立摆稳定中的表现，分享我在参数整定和算法实现中的实战经验。

2. 系统建模：从物理模型到状态方程

2.1 物理假设与参数设定

在开始推导方程前，我们需要明确几个合理假设：

• 所有连杆均为刚体（实际工程中需考虑柔性变形带来的振动）
• 忽略传动机构的齿隙（实际伺服系统约0.1-0.5°的间隙会影响控制精度）
• 不考虑库伦摩擦和空气阻力（但实际小车轨道存在约0.2N·s/m的粘滞摩擦）

以实验室常用的倒立摆设备为例，典型参数为：

matlab复制M = 0.5;   % 小车质量(kg)
m1 = 0.2;  % 下摆杆质量(kg) 
m2 = 0.1;  % 上摆杆质量(kg)
l1 = 0.3;  % 下摆杆长度(m)
l2 = 0.25; % 上摆杆长度(m)
g = 9.81;  % 重力加速度

2.2 拉格朗日动力学推导

使用拉格朗日法比牛顿-欧拉法更适合多体系统建模。首先建立系统动能T和势能V：

matlab复制% 动能表达式示例
T = 0.5*M*x_dot^2 + 0.5*m1*(x_dot^2 + l1^2*theta1_dot^2 + 2*l1*x_dot*theta1_dot*cos(theta1)) + ...
    0.5*m2*(x_dot^2 + l1^2*theta1_dot^2 + l2^2*theta2_dot^2 + ...
    2*l1*x_dot*theta1_dot*cos(theta1) + 2*l2*x_dot*theta2_dot*cos(theta2) + ...
    2*l1*l2*theta1_dot*theta2_dot*cos(theta1-theta2));

经过符号运算（建议使用MATLAB的Symbolic Toolbox），最终得到非线性状态方程。在平衡点附近线性化后，可表示为标准状态空间形式：

code复制ẋ = Ax + Bu
y = Cx + Du

关键技巧：线性化时建议采用Jacobian矩阵法，比手工求导更可靠。MATLAB代码片段：

matlab复制[A,B] = jacobianize(@nonlinear_model, x_eq, u_eq);

3. 控制器设计与实现

3.1 PID控制：经典方法的挑战

对于二级倒立摆，我尝试了三种PID结构：

1. 单回路PID：仅反馈下摆杆角度
2. 串级PID：外环角度-内环角速度
3. 多回路PID：独立控制两个摆杆

实际调试发现第三种方案效果最好，但参数整定非常困难。经过两天的手动调节，最终采用的参数为：

matlab复制% 下摆杆PID
Kp1 = 35; Ki1 = 0; Kd1 = 2.5; 

% 上摆杆PID 
Kp2 = 28; Ki2 = 0; Kd2 = 1.8;

% 小车位置PID
Kp_x = 0.8; Ki_x = 0.01; Kd_x = 0.5;

避坑指南：二级倒立摆的PID控制必须注意：

1. 积分项Ki要非常小或为零，否则极易导致饱和
2. 微分项需配合低通滤波（截止频率约10Hz）
3. 采样周期建议≤1ms，否则数字微分会引入显著延迟

3.2 LQR控制：最优控制的魅力

LQR的核心在于设计Q和R矩阵。经过多次尝试，我总结出以下权重选择经验：

1. 状态权重Q的对角元素取值为：

   • 位置相关状态：1-10
   • 角度相关状态：100-1000
   • 速度相关状态：5-50

2. 控制权重R一般取1-0.001，平衡响应速度与控制量大小

具体实现代码：

matlab复制Q = diag([10, 1000, 500, 1000, 300, 50]); % [x,θ1,θ1_dot,θ2,θ2_dot,x_dot]
R = 0.01;
[K,S,e] = lqr(A,B,Q,R);

实测发现LQR对初始条件非常敏感。当初始角度>15°时，线性模型失效导致控制发散。这时需要配合非线性前馈补偿：

matlab复制u_ff = (m1*l1 + m2*l2)*g*sin(θ1) + m2*l2*g*sin(θ2); 
u = -K*x + u_ff;

4. 仿真对比与结果分析

4.1 性能指标对比

在相同初始条件（θ1=5°, θ2=3°）下，两种控制器的表现：

4.2 典型响应曲线

（仿真曲线应包含：两摆杆角度、小车位置、控制输入随时间变化）

从曲线可见：

1. PID控制的下摆杆出现明显振荡（约3次衰减振荡）
2. LQR的上摆杆能在0.5s内快速稳定
3. 当t=3s施加脉冲干扰时，PID需要1.2s恢复，LQR仅需0.4s

5. 工程实践中的进阶技巧

5.1 硬件在环(HIL)测试

当将算法部署到实际设备前，建议进行HIL测试：

matlab复制% 在Simulink中连接真实控制器
set_param('pendulum_model/Solver','FixedStep','0.001');
xPC Target或Arduino均可作为实时目标机

5.2 参数敏感性分析

通过蒙特卡洛仿真评估鲁棒性：

matlab复制for i = 1:100
    m1_var = m1*(0.9 + 0.2*rand); % ±10%质量变化
    % 重新计算控制器并测试性能
end

结果显示LQR在±15%参数变化时仍能稳定，而PID超过±8%就会失稳。

5.3 混合控制策略

结合两者优势的方案：

• 大角度范围：PD控制快速收敛
• 小角度范围：切换至LQR提高精度
  实现时需要设计平滑的过渡逻辑：

matlab复制if max(abs(theta1),abs(theta2)) > 10*pi/180
    u = PD_control;
else
    u = LQR_control;
end

6. 常见问题排查手册

问题1：LQR仿真完美但实际设备振荡

• 检查状态观测器设计（卡尔曼滤波参数是否合理）
• 验证传感器噪声特性（IMU的噪声密度通常为0.01°/√Hz）

问题2：PID控制总是发散

• 确认执行机构饱和情况（电机最大出力是否足够）
• 检查微分项是否引入噪声（尝试增加一阶低通滤波）

问题3：切换控制时产生冲击

• 采用过渡区间（如5°-10°之间线性混合两种控制输出）
• 加入输出速率限制（du/dt ≤ 100N/s）

这个项目让我深刻体会到，控制算法没有绝对的优劣之分。在后续的机器人项目中，我采用了LQR为主、PID为辅的混合架构，既保证了响应速度，又增强了鲁棒性。对于实时性要求极高的场景，还可以考虑模型预测控制(MPC)，当然那又是另一个精彩的故事了。