三级倒立摆LQR控制：MATLAB建模与Webots仿真实践

爱过河的小马锅

1. 三级倒立摆系统概述

三级倒立摆（Triple Inverted Pendulum System，TIPS）是控制理论中经典的实验平台，由一个移动小车和三个铰接摆杆组成。这个系统之所以具有研究价值，是因为它完美体现了单输入多输出（SIMO）系统的所有典型特征：非线性、强耦合和内在不稳定性。就像杂技演员用一根手指平衡三节叠加的竹竿，任何微小的扰动都会导致系统迅速失稳。

从控制工程角度看，三级倒立摆的状态空间包含7个自由度：小车位置x，三个摆杆角度θ₁、θ₂、θ₃，以及它们各自的一阶导数。但控制输入仅有小车驱动力u这一个变量，这种欠驱动特性使得控制策略设计极具挑战性。实际物理系统中，摆杆长度通常设计为l₁=0.2m、l₂=0.15m、l₃=0.1m，质量分布遵循m₁>m₂>m₃的原则，这种参数配置可以增强系统的可观测性。

注意：在Webots仿真中，建议先采用标准参数进行建模，待控制器调试稳定后再尝试不同参数组合。突然改变质量分布可能导致系统动态特性剧变，使原有控制器失效。

2. 动力学建模与MATLAB验证

2.1 拉格朗日法建模详解

拉格朗日力学为多体系统建模提供了系统化方法。对于三级倒立摆，我们首先需要建立全局坐标系：设x轴为小车运动方向，y轴垂直向上，原点位于小车初始位置。系统动能T包含四部分：

小车平动动能：T₀ = ½Mẋ²
第一摆杆动能（平动+转动）：T₁ = ½m₁[v₁² + (l₁²θ̇₁²)/3]
第二摆杆动能：T₂ = ½m₂[v₂² + (l₂²θ̇₂²)/3]
第三摆杆动能：T₃ = ½m₃[v₃² + (l₃²θ̇₃²)/3]

其中vₙ表示各摆杆质心的合速度，需要通过位置矢量的时间导数求得。势能V则主要考虑重力势能：

V = g[m₁l₁(1-cosθ₁) + m₂(l₁(1-cosθ₁)+l₂(1-cosθ₂)) + m₃(l₁(1-cosθ₁)+l₂(1-cosθ₂)+l₃(1-cosθ₃))]

构建拉格朗日函数L=T-V后，对每个广义坐标qᵢ（包括x,θ₁,θ₂,θ₃）应用拉格朗日方程：

ddt(∂L/∂q̇ᵢ) - ∂L/∂qᵢ = Qᵢ

这将得到4个非线性微分方程，描述系统完整动力学行为。

2.2 MATLAB符号运算实战

在MATLAB中实现自动推导可大幅减少人为错误。以下是关键代码片段：

matlab复制syms m1 m2 m3 M l1 l2 l3 g theta1 theta2 theta3 x u real
syms dtheta1 dtheta2 dtheta3 dx real

% 位置矢量计算
p1 = [x + l1*sin(theta1); l1*cos(theta1)];
p2 = p1 + [l2*sin(theta2); -l2*cos(theta2)];
p3 = p2 + [l3*sin(theta3); -l3*cos(theta3)];

% 速度计算
v1 = jacobian(p1,[x,theta1])*[dx;dtheta1];
v2 = jacobian(p2,[x,theta1,theta2])*[dx;dtheta1;dtheta2];
v3 = jacobian(p3,[x,theta1,theta2,theta3])*[dx;dtheta1;dtheta2;dtheta3];

% 动能计算
T = 0.5*(M*dx^2 + m1*sum(v1.^2) + m2*sum(v2.^2) + m3*sum(v3.^2));

% 势能计算
V = g*(m1*p1(2) + m2*p2(2) + m3*p3(2));

% 拉格朗日方程推导
q = [x; theta1; theta2; theta3];
dq = [dx; dtheta1; dtheta2; dtheta3];
L = T - V;
eqns = LagrangeEquations(L, q, dq);

其中LagrangeEquations是自定义函数，用于自动生成拉格朗日方程。得到的非线性方程还需要在平衡点（θ≈0）附近进行线性化，才能应用LQR控制。

3. LQR控制器设计与实现

3.1 控制算法原理

线性二次调节器(LQR)通过最小化代价函数J来求取最优控制律：

J = ∫(xᵀQx + uᵀRu)dt

其中Q和R是需要设计的权重矩阵。Q矩阵对角元素分别对应状态变量[x,θ₁,θ₂,θ₃,ẋ,θ̇₁,θ̇₂,θ̇₃]的惩罚权重。实践经验表明，角度误差的权重应比位置误差高1-2个数量级，例如：

Q = diag([1, 1000, 800, 600, 0.1, 10, 8, 6]);
R = 0.01;

R值的选择需要折中控制效果和执行器饱和问题。通过求解Riccati方程可获得最优反馈增益矩阵K。

3.2 C语言实现要点

在Webots中实现LQR控制需要注意以下关键点：

状态获取周期必须严格等于仿真步长（通常1-10ms）
角度测量需要处理±π跳变问题
控制输出需考虑执行器饱和

典型控制循环结构如下：

c复制void controller_step() {
    // 获取当前状态
    double x = wb_position_sensor_get_value(cart_position_sensor);
    double theta1 = wb_position_sensor_get_value(pendulum1_sensor);
    double theta2 = wb_position_sensor_get_value(pendulum2_sensor);
    double theta3 = wb_position_sensor_get_value(pendulum3_sensor);
    
    // 角度归一化(-π,π]
    theta1 = fmod(theta1 + M_PI, 2*M_PI) - M_PI;
    
    // 计算速度（一阶差分）
    static double prev_x = 0;
    double dx = (x - prev_x) / TIME_STEP;
    prev_x = x;
    
    // LQR控制计算
    double u = -K[0]*x - K[1]*theta1 - K[2]*theta2 - K[3]*theta3
               - K[4]*dx - K[5]*dtheta1 - K[6]*dtheta2 - K[7]*dtheta3;
    
    // 输出饱和处理
    u = fmax(fmin(u, MAX_FORCE), -MAX_FORCE);
    wb_motor_set_force(cart_motor, u);
}

4. Webots仿真工程搭建

4.1 物理建模规范

在Webots中创建三级倒立摆时，建议采用以下层次结构：

code复制DEF TIPS Group {
  children [
    DEF CART Solid {
      translation 0 0.05 0
      children [
        Shape { geometry Box { size 0.2 0.1 0.1 } }
        HingeJoint {
          jointParameters HingeJointParameters {
            axis 0 1 0
          }
          device [
            RotationalMotor { name "motor1" }
            PositionSensor { name "sensor1" }
          ]
          endPoint DEF PEND1 Solid { ... }
        }
      ]
    }
  ]
}