1. 项目概述
倒立摆系统作为控制理论研究的经典对象，常被用来验证各种控制算法的有效性。二级倒立摆相比一级倒立摆具有更高的阶数和更强的非线性特性，对控制算法提出了更大挑战。这个项目通过Matlab/Simulink搭建二级倒立摆的动力学模型，分别采用PID控制和LQR控制两种方法实现系统稳定，并对比分析两者的控制效果。
我在研究生阶段就接触过倒立摆实验，当时被这个看似简单却蕴含深奥控制原理的系统深深吸引。后来在工作中参与过工业机械臂的平衡控制项目，发现很多先进控制算法都能在倒立摆这个"控制界的Hello World"上找到原型。这次仿真不仅适合控制理论初学者理解基础概念，对有一定经验的工程师优化算法参数也有参考价值。
2. 系统建模与参数设定
2.1 二级倒立摆动力学模型
二级倒立摆系统由小车、一级摆杆和二级摆杆组成。采用拉格朗日方程建立系统动力学模型时，需要考虑：
- 小车质量M=1kg
- 一级摆杆质量m1=0.5kg，长度l1=0.3m
- 二级摆杆质量m2=0.2kg，长度l2=0.2m
- 摆杆转动惯量J1=1/12m1l1²，J2=1/12m2l2²
- 摩擦系数b=0.1 N/m/s
推导过程会得到一个4阶非线性微分方程组，包含小车位置x、一级摆杆角度θ1和二级摆杆角度θ2及其导数。在平衡点附近进行线性化处理后，可以得到状态空间表达式：
code复制ẋ = Ax + Bu
y = Cx + Du
其中状态变量x=[x, ẋ, θ1, θ̇1, θ2, θ̇2]ᵀ，控制输入u为小车驱动力。
注意：实际建模时要特别注意各坐标系之间的转换关系，特别是二级摆杆的角度是相对于一级摆杆的相对角度，在计算动能和势能时容易出错。
2.2 Simulink模型搭建
在Simulink中搭建模型时，我推荐采用以下结构：
- 使用S-Function实现非线性动力学方程
- 添加饱和模块限制小车位移在±0.5m范围内
- 加入白噪声模拟传感器测量误差
- 设置采样时间为0.01s
模型验证时，可以先用PID控制器让系统稳定，然后给一个脉冲干扰观察恢复过程。正确的模型应该能在一秒内恢复平衡。
3. PID控制器设计与实现
3.1 控制结构设计
对于二级倒立摆，采用串级PID控制结构：
- 外环控制一级摆杆角度θ1，输出作为内环的设定值
- 内环控制二级摆杆角度θ2，输出小车加速度指令
- 最内环控制小车位置x，输出最终控制力
这种结构比单回路PID更能处理系统的强耦合特性。每个PID控制器的输出限幅需要谨慎设置，我建议：
- 角度环输出限幅：±1 rad/s²
- 位置环输出限幅：±10 N
3.2 参数整定经验
参数整定采用试凑法时，按以下顺序调整：
- 先只启用角度θ1的P控制，增大KP直到出现等幅振荡
- 加入KD抑制振荡，记录临界增益Ku和振荡周期Tu
- 使用Ziegler-Nichols公式计算初始PID参数
- 对θ2环重复上述过程
实测有效的参数范围：
- θ1环：KP=50~80, KI=0~5, KD=5~15
- θ2环：KP=30~50, KI=0~3, KD=3~10
- x环：KP=5~15, KI=0, KD=0.5~2
技巧：在Simulink中使用PID Tuner工具可以加速参数整定过程，但需要手动微调才能达到最佳效果。
4. LQR控制器设计与实现
4.1 状态反馈控制原理
LQR(线性二次型调节器)通过最小化代价函数J=∫(xᵀQx + uᵀRu)dt来设计最优状态反馈矩阵K。对于二级倒立摆：
- 设计Q矩阵时，我给角度误差更高的权重：
Q = diag([1, 0, 100, 0, 50, 0]) - 控制权重R取较小值0.1，允许较大的控制力
- 使用Matlab的lqr()函数求解Riccati方程得到K矩阵
4.2 观测器设计
实际系统可能无法测量所有状态，需要设计观测器。我采用Kalman滤波器：
- 过程噪声协方差Qn = diag([0.01, 0.01, 0.1, 0.1, 0.1, 0.1])
- 测量噪声协方差Rn = diag([0.001, 0.001, 0.001])
- 使用kalman()函数设计滤波器
实测发现，当传感器噪声较大时，观测器性能对系统稳定性影响显著。建议在硬件允许的情况下尽可能直接测量更多状态。
5. 控制效果对比分析
5.1 阶跃响应对比
设置初始条件θ1=5°, θ2=10°，对比两种控制器的稳定时间：
| 指标 | PID控制 | LQR控制 |
|---|---|---|
| 稳定时间(s) | 2.5 | 1.2 |
| 超调量(%) | 15 | 5 |
| 控制力峰值(N) | 8.7 | 6.3 |
LQR在响应速度和能量消耗方面表现更优。
5.2 抗干扰测试
在t=3s时施加0.1N的瞬时力干扰：
- PID控制需要约1.8s恢复稳定
- LQR控制仅需0.9s恢复
- LQR的最大角度偏差比PID小40%
5.3 参数鲁棒性测试
改变摆杆质量±20%后：
- PID控制需要重新调整参数
- LQR控制仍能保持稳定，但性能略有下降
- 当模型误差超过30%时，两种控制都需要重新设计
6. 实现中的常见问题
6.1 仿真发散问题
现象：仿真时角度迅速增大到几百rad
解决方法：
- 检查模型方程的单位是否一致
- 减小仿真步长到0.001s
- 给控制器输出添加饱和限制
6.2 高频振荡问题
现象：系统稳定但存在高频抖动
解决方法：
- 在PID控制器中加入低通滤波器
- 调整LQR的Q矩阵，增加速度项权重
- 检查是否采样时间过长
6.3 实时实现难点
将算法移植到实际硬件时：
- 离散化LQR控制器：使用c2d()函数
- 处理计算延迟：增加预测环节
- 电机死区补偿：在前馈项中加入偏置
7. 进阶改进方向
- 模糊PID控制：根据角度误差自动调整PID参数
- 滑模控制：增强抗干扰能力
- 自适应控制：在线识别模型参数
- 强化学习：训练神经网络控制器
我在实际项目中尝试过模糊PID与LQR的混合控制方案，当一级摆杆角度较小时使用LQR，角度较大时切换到模糊PID，取得了比单一控制器更好的效果。这种多模态控制在处理大范围扰动时特别有效。