1. 项目背景与核心挑战
倒立摆系统作为控制理论中的经典研究对象，一直被视为检验控制算法有效性的"试金石"。而三级倒立摆更是将控制难度提升到了新的高度——需要同时稳定三个相互耦合的摆杆。这个项目选择在Webots仿真环境中，用纯C语言实现LQR（线性二次型调节器）控制算法，可以说是控制理论与工程实践的完美结合点。
我最初接触这个项目是在研究生阶段的控制系统课程设计中。当时用MATLAB仿真二级倒立摆就已经让我焦头烂额，而三级倒立摆带来的挑战更是呈指数级增长。最大的难点在于：三个摆杆的运动相互耦合，任何一个摆杆的状态变化都会影响其他两个；系统对初始条件极其敏感；实时计算LQR控制量对算法效率要求极高。
2. 系统建模与LQR控制原理
2.1 三级倒立摆动力学建模
建立精确的数学模型是控制的基础。三级倒立摆系统可以抽象为四个刚体（小车+三个摆杆）的联动系统。采用拉格朗日方程建模时，需要考虑：
- 小车在x轴方向的平动
- 三个摆杆相对于竖直方向的转动
- 各关节间的相互作用力矩
最终得到的非线性微分方程组包含12个状态变量（位置、角度及其导数）。在平衡点附近线性化后，可以得到状态空间表达式：
code复制ẋ = Ax + Bu
y = Cx + Du
其中状态向量x包含：
- 小车位置和速度
- 三个摆杆的角度和角速度
2.2 LQR控制器设计
LQR控制的核心是求解Riccati方程得到最优反馈矩阵K。设计时需要精心选择Q和R矩阵：
c复制// 典型权重矩阵设置示例
Q = diag([100, 1, 100, 1, 100, 1, 100, 1]); // 强调位置和角度误差
R = 0.1; // 控制量权重
在实际调试中发现，三级摆杆的权重分配需要遵循"上重下轻"原则——顶部摆杆的权重应该最大，因为它的不稳定模态最难控制。
3. Webots仿真环境搭建
3.1 机器人模型构建
Webots提供了强大的物理引擎和机器人建模工具。创建三级倒立摆模型时需要注意：
- 关节类型选择：摆杆间使用旋转关节(rotational joint)
- 物理参数设置：质量、转动惯量等需要与真实系统匹配
- 传感器配置：每个关节需要角度传感器，小车需要位置传感器
proto复制PROTO TripleInvertedPendulum [
field SFVec3f translation 0 0 0
field SFRotation rotation 0 1 0 0
]
{
Robot {
translation IS translation
rotation IS rotation
children [
# 小车和摆杆定义...
]
controller "triple_pendulum_controller"
}
}
3.2 仿真参数调优
物理引擎参数对仿真结果影响巨大。关键参数包括：
- 仿真步长：通常设为1-5ms，步长太大会导致数值不稳定
- 接触参数：摆杆间的碰撞参数需要合理设置
- 重力加速度：默认9.81，但可以调整以测试控制器鲁棒性
经验分享：在初期调试时，可以先将重力设为0，让系统处于"太空"环境，等控制器基本工作后再逐步增加重力，这样可以避免系统立即崩溃。
4. C语言控制器实现
4.1 实时控制架构
Webots控制器采用典型的实时控制循环：
c复制int main() {
webots_init();
lqr_init(); // 初始化LQR控制器
while (webots_step(TIME_STEP) != -1) {
read_sensors();
compute_control();
apply_control();
}
webots_cleanup();
return 0;
}
4.2 LQR核心算法实现
LQR计算的关键是矩阵运算。由于嵌入式环境没有MATLAB的矩阵库，需要自己实现：
c复制void lqr_control(const double x[STATE_DIM], double u[1]) {
static const double K[STATE_DIM] = { /* 离线计算的K矩阵 */ };
// 计算控制量 u = -Kx
*u = 0;
for (int i = 0; i < STATE_DIM; i++) {
*u -= K[i] * x[i];
}
// 控制量限幅
*u = fmax(fmin(*u, MAX_FORCE), -MAX_FORCE);
}
4.3 状态估计与滤波
实际系统中传感器都有噪声，需要采用滤波技术：
- 低通滤波处理角度测量噪声
- 通过差分和滤波估计角速度
- 卡尔曼滤波在多传感器融合中的应用
c复制void estimate_states() {
// 一阶低通滤波示例
filtered_angle1 = ALPHA * raw_angle1 + (1-ALPHA) * filtered_angle1;
// 差分法求角速度
static double last_angle1 = 0;
angular_velocity1 = (filtered_angle1 - last_angle1) / TIME_STEP;
last_angle1 = filtered_angle1;
}
5. 调试与性能优化
5.1 控制器调试技巧
调试三级倒立摆是个需要耐心的过程，我的经验是：
- 先调试单级摆，再逐步增加摆杆数量
- 使用Webots的实时绘图功能监控关键状态
- 保存每次运行的日志数据用于离线分析
- 采用"二分法"调整Q和R矩阵元素
5.2 性能优化手段
实时控制对计算效率要求极高，特别是用C语言实现时：
- 将矩阵运算展开为标量运算，避免循环开销
- 使用查找表替代实时矩阵求逆
- 将LQR增益矩阵K预先计算并固化在代码中
- 合理使用定点数运算提高速度
c复制// 优化后的控制量计算示例
*u = - (K0*x[0] + K1*x[1] + K2*x[2] + K3*x[3]
+ K4*x[4] + K5*x[5] + K6*x[6] + K7*x[7]);
5.3 鲁棒性测试方法
一个好的控制器需要能在各种条件下工作：
- 不同初始角度扰动测试
- 参数不确定性测试（如改变摆杆质量）
- 加入随机脉冲干扰测试
- 传感器噪声测试
6. 常见问题与解决方案
6.1 系统发散问题
现象：摆杆迅速倒下，控制器无法稳定系统
可能原因和解决：
- Q矩阵权重分配不合理 → 调整顶部摆杆权重
- 控制量饱和 → 增大R矩阵元素或机械限位
- 仿真步长过大 → 减小步长到1ms左右
6.2 高频振荡问题
现象：系统能维持不倒但持续高频抖动
解决方法：
- 在Q矩阵中增加速度项权重
- 加入低通滤波环节
- 检查传感器噪声是否过大
6.3 实时性问题
现象：控制周期无法跟上仿真步长
优化方案：
- 简化矩阵运算，使用查表法
- 降低状态维度（如忽略某些耦合项）
- 改用更高效的编译器优化选项
7. 项目扩展与进阶方向
这个基础项目有很多值得深入的方向：
- 切换控制：结合LQR和Bang-Bang控制提高抗干扰能力
- 自适应控制：在线调整Q和R矩阵以适应参数变化
- 机器学习：用强化学习优化控制策略
- 硬件实现：用STM32等嵌入式平台实现实物控制
我在完成基础版本后，尝试加入了摆动起摆功能——先让摆杆自然下垂，然后通过控制小车运动积累能量，最终将摆杆摆动到直立位置。这个过程中最关键的挑战是判断何时从起摆控制切换到LQR平衡控制。