简易旋转倒立摆及控制装置设计方案

一、系统概述

旋转倒立摆是一个经典的控制系统实验平台，它由一个可自由旋转的摆杆和一个驱动旋转臂的电机组成。控制目标是：通过电机驱动旋转臂，使摆杆从自然下垂的静止状态快速摆动至竖直向上的倒立位置，并保持稳定平衡。

该系统是学习非线性控制、状态反馈、实时控制的理想对象。

二、系统组成与硬件设计

2.1 核心硬件清单

组件	推荐型号/规格	数量	功能说明	预估成本
主控制器	STM32F4系列 (如STM32F407) 或 Arduino Due	1	运行控制算法，处理传感器数据，生成PWM	80-150元
直流电机	带编码器的直流减速电机 (12V, 30:1减速比)	1	驱动旋转臂，需足够扭矩	100-200元
电机驱动器	TB6612FNG 或 DRV8833 双H桥模块	1	接收MCU信号，驱动电机正反转	20-30元
角度传感器	绝对值编码器 (如AS5048A) 或高精度电位器	2	1个测旋转臂角度，1个测摆杆角度	100-200元
摆杆与旋转臂	碳纤维杆/铝杆 (摆杆长20-30cm，臂长15-25cm)	1套	机械结构主体，要求轻质、刚性	50元
轴承与支架	法兰轴承、铝型材、3D打印件	1套	支撑结构，保证转动顺滑	100元
电源	12V/3A 开关电源	1	为电机和控制器供电	50元
其他	杜邦线、螺丝、接插件等	1批	连接与固定	50元

总计成本：约 500-800元。

2.2 机械结构设计要点

旋转臂：
- 材料：铝型材或3D打印（PLA/ABS）
- 长度：15-25cm，一端连接电机轴，另一端连接摆杆铰链。
- 要求：轻量化以减少转动惯量。
摆杆：
- 材料：碳纤维管或细铝管（直径6-10mm）。
- 长度：20-30cm。这是关键参数，影响系统动力学和控制难度。
- 顶端可配重（如螺母）以调节重心。
铰链与轴承：
- 摆杆与旋转臂的连接点使用低摩擦轴承，确保摆杆只能在一个平面内自由摆动。
- 整个旋转臂组件通过法兰轴承固定在底板上，确保旋转平面水平。
底座：
- 使用厚重的铝板或木板作为底座，防止系统运行时整体移动。

三、系统建模与动力学分析

3.1 系统参数定义

符号	物理意义	单位	典型值（示例）
( l )	摆杆质心到铰链点的长度	m	0.2
( m )	摆杆的质量	kg	0.1
( J )	摆杆绕其质心的转动惯量	kg·m²	( \frac{1}{3}ml^2 )
( M )	旋转臂（含电机转子）的等效质量	kg	0.5
( R )	旋转臂的长度	m	0.2
( b_p )	摆杆铰链处的摩擦系数	N·m·s/rad	0.001
( b_r )	旋转臂轴承处的摩擦系数	N·m·s/rad	0.01
( g )	重力加速度	m/s²	9.8

3.2 非线性动力学方程（拉格朗日法推导）

系统有两个自由度：

( $\theta$ )：旋转臂与参考线的夹角。
( $\alpha$ )：摆杆与竖直向下方向的夹角（平衡点时 α=0）。

系统动能T与势能V：
$T = \frac{1}{2}(MR^2)\dot{\theta}^2 + \frac{1}{2}m[ (R\dot{\theta} + l\dot{\alpha}\cos\alpha)^2 + (-l\dot{\alpha}\sin\alpha)^2 ] + \frac{1}{2}J\dot{\alpha}^2$
$V = mgl(1-\cos\alpha)$

拉格朗日方程：
$\frac{d}{dt}(\frac{\partial L}{\partial \dot{q_i}}) - \frac{\partial L}{\partial q_i} = Q_i$
其中 ( $L = T - V$ )， ( $q_1 = \theta, q_2 = \alpha$ )。( $Q_1 = \tau - b_r\dot{\theta}$ )， ( $Q_2 = -b_p\dot{\alpha}$ )。( $\tau$ ) 是电机施加在旋转臂上的扭矩。

最终推导出的非线性方程：

$(MR^2 + mR^2)\ddot{\theta} + mRl\cos\alpha \cdot \ddot{\alpha} - mRl\sin\alpha \cdot \dot{\alpha}^2 + b_r\dot{\theta} = \tau$

$mRl\cos\alpha \cdot \ddot{\theta} + (ml^2 + J)\ddot{\alpha} - mgl\sin\alpha + b_p\dot{\alpha} = 0$

3.3 在平衡点附近的线性化

控制目标是在 ( $\alpha = 0$ )（摆杆竖直向上） 附近保持稳定。在此点附近进行线性化：( $\sin\alpha \approx \alpha$ )， ( $\cos\alpha \approx 1$ )， ( $\dot{\alpha}^2 \approx 0$ )。

得到线性化状态空间方程：
定义状态向量 ( $\mathbf{x} = [\theta, \alpha, \dot{\theta}, \dot{\alpha}]^T$ )，控制输入 ( $u = \tau$ )。

$\dot{\mathbf{x}} = A\mathbf{x} + Bu$
其中，

$A = \begin{bmatrix} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & a_{32} & a_{33} & a_{34} \\ 0 & a_{42} & a_{43} & a_{44} \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 0 \\ b_3 \\ b_4 \end{bmatrix}$
矩阵元素 ( $a_{ij}$ ) 和 ( $b_i$ ) 由系统物理参数决定。此线性模型是设计LQR等控制器的基础。

四、控制算法与软件实现

4.1 控制策略选择

策略	原理	优点	缺点	适用场景
PID控制	对角度误差进行比例、积分、微分运算	简单易懂，易于实现和调试	对于非线性、强耦合系统性能有限	入门学习，要求不高的稳定控制
线性二次型调节器 (LQR)	基于线性化模型，求解状态反馈矩阵K，最小化二次型性能指标	多状态变量同时优化，理论性能优，鲁棒性好	依赖精确的线性模型，在较大偏离时可能失效	最常用、效果好的方法
模糊控制	基于专家经验制定模糊规则	不依赖精确数学模型，抗干扰能力强	规则设计依赖经验，稳定性证明困难	模型不确定或非线性强的系统
模型预测控制 (MPC)	在线求解有限时域内的最优控制问题	能处理约束，控制性能优异	计算量大，对控制器要求高	高性能或带约束的先进控制

对于简易装置，推荐采用“能量起摆 + LQR稳摆”的组合策略。

4.2 核心算法步骤与代码框架（基于STM32）

步骤1：系统初始化与参数设定

// system_params.h
#define PENDULUM_LENGTH 0.2f    // 摆杆长度 l (m)
#define PENDULUM_MASS 0.1f      // 摆杆质量 m (kg)
#define ARM_LENGTH 0.2f         // 旋转臂长度 R (m)
#define ARM_INERTIA 0.01f       // 旋转臂转动惯量 (kg*m^2)
#define GRAVITY 9.81f           // 重力加速度

// LQR 增益矩阵 K (需根据模型计算，此处为示例)
float K[4] = {-10.0, 100.0, -2.0, 10.0}; // K = [k1, k2, k3, k4]

步骤2：传感器数据读取与滤波

// sensor.c
float get_arm_angle(void) {
    // 读取旋转臂编码器值，并转换为弧度
    uint16_t encoder_val = read_encoder_arm();
    float angle = (encoder_val - ENCODER_OFFSET) * 2.0 * PI / ENCODER_RESOLUTION;
    return angle;
}

float get_pendulum_angle(void) {
    // 读取摆杆编码器值，并转换为弧度。
    // 注意：摆杆编码器零点应对应竖直向下（α=π）。
    uint16_t encoder_val = read_encoder_pendulum();
    float angle = (encoder_val - PENDULUM_OFFSET) * 2.0 * PI / ENCODER_RESOLUTION;
    // 转换为与模型一致的定义（竖直向上为0）
    angle = angle - PI;
    // 应用一阶低通滤波
    static float filtered_angle = 0;
    float alpha = 0.2; // 滤波系数
    filtered_angle = alpha * angle + (1-alpha) * filtered_angle;
    return filtered_angle;
}

void calculate_velocity(float *arm_vel, float *pend_vel) {
    // 通过角度差分计算角速度（需高采样率）
    static float prev_arm_angle = 0, prev_pend_angle = 0;
    static uint32_t prev_time = 0;
    uint32_t current_time = get_micros();
    float dt = (current_time - prev_time) * 1e-6f;

    if(dt > 0 && dt < 0.1) { // 防止dt过大
        *arm_vel = (get_arm_angle() - prev_arm_angle) / dt;
        *pend_vel = (get_pendulum_angle() - prev_pend_angle) / dt;
    }
    prev_arm_angle = get_arm_angle();
    prev_pend_angle = get_pendulum_angle();
    prev_time = current_time;
}

步骤3：能量起摆控制器（Swing-up）

目标：给系统注入能量，使摆杆从下垂状态摆动到倒立位置附近。

// swing_up.c
#define ENERGY_TARGET (2 * PENDULUM_MASS * GRAVITY * PENDULUM_LENGTH) // 目标能量 E = 2mgl

float swing_up_control(float pendulum_angle, float pendulum_velocity) {
    // 计算当前系统的总机械能
    float kinetic = 0.5 * PENDULUM_MASS * PENDULUM_LENGTH * PENDULUM_LENGTH * pendulum_velocity * pendulum_velocity;
    float potential = PENDULUM_MASS * GRAVITY * PENDULUM_LENGTH * (1 - cosf(pendulum_angle));
    float total_energy = kinetic + potential;

    // 能量差
    float energy_error = total_energy - ENERGY_TARGET;

    // 基于能量的简单Bang-Bang控制
    float control_gain = 5.0; // 需要调试
    float torque = 0;
    if(fabs(energy_error) > 0.1) { // 能量误差死区
        torque = control_gain * (pendulum_velocity * cosf(pendulum_angle) > 0 ? 1 : -1);
        // 限制最大扭矩
        torque = constrain(torque, -MAX_TORQUE_SWING, MAX_TORQUE_SWING);
    }
    return torque;
}

步骤4：LQR稳摆控制器（Balance）

当摆杆角度接近倒立位置（例如 |α| < 20°）时，切换至LQR控制。

// lqr_balance.c
float lqr_balance_control(float arm_angle, float pendulum_angle, float arm_vel, float pend_vel) {
    // 定义状态向量 x = [θ, α, θ_dot, α_dot]
    float x[4] = {arm_angle, pendulum_angle, arm_vel, pend_vel};

    // 计算控制量 u = -Kx
    float torque = 0;
    for(int i=0; i<4; i++) {
        torque -= K[i] * x[i];
    }

    // 限制输出扭矩，保护电机和硬件
    torque = constrain(torque, -MAX_TORQUE_BALANCE, MAX_TORQUE_BALANCE);
    return torque;
}

步骤5：主控制循环与模式切换

// main_control.c
typedef enum {SWING_UP, BALANCE, IDLE} ControlMode;
ControlMode current_mode = SWING_UP;

void control_loop(void) {
    // 1. 读取所有传感器数据
    float theta = get_arm_angle();
    float alpha = get_pendulum_angle();
    float theta_dot, alpha_dot;
    calculate_velocity(&theta_dot, &alpha_dot);

    // 2. 模式切换逻辑
    if(current_mode == SWING_UP && fabs(alpha) < 0.35) { // 约20度
        // 进入稳摆区域，切换模式
        current_mode = BALANCE;
        // 可在此重置积分器等
    } else if(current_mode == BALANCE && fabs(alpha) > 0.7) { // 约40度
        // 偏离太大，切换回起摆
        current_mode = SWING_UP;
    }

    // 3. 根据模式计算控制量
    float motor_torque = 0;
    switch(current_mode) {
        case SWING_UP:
            motor_torque = swing_up_control(alpha, alpha_dot);
            break;
        case BALANCE:
            motor_torque = lqr_balance_control(theta, alpha, theta_dot, alpha_dot);
            break;
        case IDLE:
            motor_torque = 0;
            break;
    }

    // 4. 将扭矩指令转换为电机PWM占空比并输出
    // 假设电机扭矩常数 Kt，则电压 V = (R/Kt)*torque + Ke*omega
    // 简化：PWM_duty = motor_torque / MAX_TORQUE * MAX_PWM
    int pwm_output = (int)((motor_torque / MAX_TORQUE) * MAX_PWM);
    set_motor_pwm(pwm_output);

    // 5. 数据记录或发送（用于调试）
    send_to_serial(theta, alpha, theta_dot, alpha_dot, motor_torque, current_mode);
}

参考代码简易旋转倒立摆及控制装置 www.youwenfan.com/contentcnt/160775.html

4.3 上位机调试界面（Python示例）

使用 pySerial 和 matplotlib 实时显示状态。

# monitor.py
import serial
import matplotlib.pyplot as plt
import matplotlib.animation as animation

ser = serial.Serial('COM3', 115200)
fig, axes = plt.subplots(2, 2)
lines = [ax.plot([], [])[0] for ax in axes.flatten()]
data_buffers = [[] for _ in range(4)] # 存储theta, alpha, theta_dot, alpha_dot

def update(frame):
    if ser.in_waiting:
        line = ser.readline().decode().strip()
        try:
            values = list(map(float, line.split(',')))
            if len(values) >= 4:
                for i in range(4):
                    data_buffers[i].append(values[i])
                    if len(data_buffers[i]) > 200:
                        data_buffers[i].pop(0)
                    lines[i].set_data(range(len(data_buffers[i])), data_buffers[i])
                axes[0,0].relim(); axes[0,0].autoscale_view()
                # ... 其他axes同理
        except:
            pass
    return lines

ani = animation.FuncAnimation(fig, update, interval=50)
plt.show()

五、系统搭建与调试步骤

5.1 硬件组装步骤

制作底座与支架：确保底座稳固，垂直支架能牢固支撑电机和轴承。
安装旋转臂：将旋转臂牢固安装在电机输出轴上，确保其旋转平面水平。
安装摆杆铰链：在旋转臂末端安装低摩擦轴承，作为摆杆的旋转轴。
安装传感器：
- 将旋转臂编码器与电机同轴安装。
- 将摆杆编码器安装在摆杆铰链处。
电路连接：
- 将电机连接至电机驱动板的输出端。
- 将电机驱动板的控制引脚（PWM，IN1，IN2）连接至MCU。
- 将两个编码器连接至MCU的定时器编码器接口或SPI/I2C接口。
- 为MCU、电机驱动、编码器提供合适的电源（注意共地）。

5.2 软件调试流程

基础通信测试：确保MCU能通过串口向上位机发送数据。
传感器校准：
- 旋转臂：定义机械零点（如指向正前方时为0度）。
- 摆杆：将摆杆自然下垂，记录此时编码器值，对应 α = π。
电机开环测试：编写程序让电机按固定速度正反转，检查连接是否正确，机械转动是否顺畅。
单独调试能量起摆：
- 屏蔽LQR控制，只运行 swing_up_control。
- 调整 control_gain 和 MAX_TORQUE_SWING，观察摆杆是否能摆动幅度越来越大。
单独调试LQR稳摆（关键步骤）：
- 用手将摆杆扶到接近竖直向上的位置。
- 运行 lqr_balance_control。
- 调整LQR增益K：这是最核心的调试。可以先在MATLAB/Python中根据线性化模型求解理论K值，然后在实际系统中微调。通常先调 k2（摆杆角度反馈）和 k4（摆杆角速度反馈），再调 k1 和 k3。
联调与模式切换：将两部分控制器结合，测试从起摆到稳摆的自动切换。微调切换阈值（如 0.35 弧度）。

5.3 常见问题与解决

问题现象	可能原因	解决方案
摆杆完全无法起摆	电机扭矩不足；起摆控制增益太小；摩擦力太大	检查电机电源；增大 `control_gain`；润滑轴承。
起摆过程震荡，无法到达顶部	能量控制律参数不合适	调整 `control_gain` 或尝试更复杂的能量控制律。
在倒立点附近剧烈振荡	LQR增益过大（尤其是微分增益k3, k4）	减小LQR增益，特别是角速度反馈增益。
能稳住但缓慢漂移	旋转臂角度反馈增益 `k1` 太小；存在稳态误差	适当增大 `k1`；或在LQR中引入旋转臂位置的积分项。
切换时产生抖动	切换阈值设置不当；两种控制律输出不连续	优化切换逻辑，如增加滞回区间；在切换点进行控制量平滑过渡。

六、总结与拓展

6.1 项目总结

通过本项目，你可以：

掌握经典控制对象的机械设计与系统建模方法。
实践传感器数据处理、实时控制程序编写。
深入理解能量控制、状态反馈（LQR）等核心控制算法。
锻炼硬件调试和系统集成的工程能力。

6.2 进阶挑战

改用无刷电机（FOC控制）：获得更平滑的扭矩控制。
实现双级倒立摆：控制难度呈指数级增加。
改用MPC控制：体验更先进的优化控制方法。
增加视觉反馈：用摄像头识别摆杆角度，替代编码器。
实现移动平衡：将底座改为小车，构成“旋转倒立摆小车”系统。