三级倒立摆系统简介
三级倒立摆（Triple Inverted Pendulum System，TIPS）是一个典型的单输入四输出的非线性、强耦合且不稳定系统。想象一下，就像杂技演员头顶着好几根依次叠放的杆子，要保持它们都不倒下，这控制难度可想而知。但正是这种复杂又具挑战性的系统，在控制理论学习和研究中非常有价值。
建模与 MATLAB 计算
（一）拉格朗日法动力学建模
我们采用拉格朗日法来给三级倒立摆建模。拉格朗日方程为 (L = T - V)，其中 (T) 是系统动能，(V) 是系统势能。对于三级倒立摆，需要仔细分析每个摆杆以及小车的动能和势能。
以一个简化的单摆为例（方便理解思路），设摆长为 (l)，质量为 (m)，摆角为 (\theta)，那么动能 (T=\frac{1}{2}ml^{2}\dot{\theta}^{2})，势能 (V = mgl(1 - \cos\theta))，这里 (g) 是重力加速度。拉格朗日函数 (L = T - V=\frac{1}{2}ml^{2}\dot{\theta}^{2}-mgl(1 - \cos\theta))。
根据拉格朗日方程 (\frac{d}{dt}(\frac{\partial L}{\partial \dot{q}{i}})-\frac{\partial L}{\partial q{i}} = Q*{i})（(q*{i}) 是广义坐标，(Q_{i}) 是广义力），对单摆来说，广义坐标 (q=\theta)，广义力 (Q = 0)，代入拉格朗日方程可得运动方程：
[ml^{2}\ddot{\theta}+mgl\sin\theta = 0]
当然，三级倒立摆要复杂得多，需要考虑多个摆杆和小车的相互作用，但思路是类似的。
（二）MATLAB 符号运算验证
用 MATLAB 进行符号运算能帮我们验证数学推导。比如在 MATLAB 中，可以这样定义符号变量：
syms m1 m2 m3 M l1 l2 l3 g theta1 theta2 theta3 x u real
这里 (m1,m2,m3) 是三个摆杆质量，(M) 是小车质量，(l1,l2,l3) 是摆杆长度，(g) 重力加速度，(\theta1,\theta2,\theta3) 是摆角，(x) 是小车位置，(u) 是控制输入。
然后计算动能 (T) 和势能 (V)，构建拉格朗日函数 (L)，再通过符号运算得到运动方程。MATLAB 的符号运算工具箱可以轻松处理这些复杂的代数运算，大大减少人工推导的错误。
Webots 仿真工程
（一）搭建 Webots 环境
使用 Windows 下 Webots 自带编译环境及裸 C 实现控制，这种所见即所得的方式很直观。Webots 是一款机器人仿真软件，在里面搭建三级倒立摆模型，设置好各个部件的物理参数，比如质量、长度、摩擦系数等。
（二）C 语言实现 LQR 控制
LQR（Linear Quadratic Regulator）即线性二次型调节器，它的目标是寻找一个最优控制律 (u = -Kx)，使得性能指标 (J=\int_{0}^{\infty}(x^{T}Qx + u^{T}Ru)dt) 最小化，其中 (Q) 和 (R) 是权重矩阵，(x) 是状态向量，(u) 是控制输入。
下面是一段简单的 C 语言实现 LQR 控制律计算的代码示例：
#include <stdio.h>
#include <math.h>
// 假设状态向量 x[4]，控制增益矩阵 K[4]
void calculate_control(double x[], double K[], double *u) {
*u = 0;
for (int i = ; i < ; i++) {
*u -= K[i] * x[i];
}
}
