三级倒立摆LQR控制：Webots仿真与C语言实现之旅

三级倒立摆LQR控制——C语言Webots仿真三阶倒立摆（TIPS, Triple Inverted Pendulum System）。 需要请预约时间在线讲解教学 依旧使用Windows Webots自带编译环境及裸C实现控制，所见即所得。 使用拉格朗日法动力学建模，MATLAB符号运算验证数学推导，LQR全状态反馈控制。 （A）建模解析 + MATLAB计算 （B）Webots仿真工程 三级倒立摆是一个单输入四输出的非线性、强耦合、不稳定系统。 此Demo对于初学者掌握拉格朗日法动力学建模、MATLAB符号运算、LQR控制算法及其C语言实现和Webots建模仿真有全面性帮助； LQR控制器即线性二次型调节器 LQR（Linear Quadratic Regulator） #三级倒立摆 #三阶倒立摆 #Webots #LQR #拉格朗日方程 #动力学建模 #C语言 #MATLAB #控制算法

最近捣鼓了下三级倒立摆的LQR控制，用Webots结合C语言做了仿真，过程还挺有意思，来跟大家分享分享。

一、三级倒立摆系统简介

三级倒立摆（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 = 0; i < 4; i++) { *u -= K[i] * x[i]; } }

在这段代码中，calculate_control 函数根据当前状态向量 x 和控制增益矩阵 K 计算出控制输入 u。实际应用中，还需要结合Webots提供的API获取实时状态，并将计算出的控制输入应用到模型上。

这个Demo对于初学者掌握拉格朗日法动力学建模、MATLAB符号运算、LQR控制算法及其C语言实现和Webots建模仿真都有全面性帮助。要是有小伙伴想深入了解，需要请预约时间在线讲解教学，大家一起交流探讨呀。

#三级倒立摆 #三阶倒立摆 #Webots #LQR #拉格朗日方程 #动力学建模 #C语言 #MATLAB #控制算法