简介：这份旋转倒立摆常规起摆源码面向控制理论初学者与嵌入式控制方向的学生，围绕经典倒立摆实验装置，提供一套可运行的起摆与稳定控制实例，帮助读者理解PID控制器在实时系统中的落地方式。压缩包共162个文件，约3.46MB，以c与h源文件、o目标文件、crf与d依赖文件为主，另含uvprojx、uvoptx等Keil工程配置及hex、axf、map等编译产物，并附带MPU6050、inv_mpu等传感器驱动代码，构成完整的嵌入式工程结构。目前已有838人学习下载。源码核心在于根据摆角与角速度计算电机力矩，涵盖比例、积分、微分三项的调节逻辑，以及固定采样时间的控制循环、状态反馈、传感器数据处理与滤波、电机驱动等环节。读者可借此掌握PID参数整定、实时控制流程与硬件交互方法，是提升控制工程与系统集成能力的实用参考。
1. 旋转倒立摆常规起摆源码：从「摆不起来」到「稳得住」的那层窗户纸
很多做控制的朋友第一次拿到旋转倒立摆，都会经历同一个场景：电机转得挺欢，摆杆就是甩不上去，要么在最低点来回晃，要么勉强冲到顶又立刻倒下来。你调 PID 调到手酸，改滤波改到怀疑人生，最后发现真正卡住你的不是控制器参数，而是起摆策略本身——常规起摆（swing-up）和平衡控制（balance）是两套逻辑，混在一起调必然翻车。这份「旋转倒立摆常规起摆源码」解决的正是这个断层：它把能量法起摆、切换判据、LQR 平衡三段拆开写清楚，让你能单独验证每一段，而不是对着一个黑匣子瞎调。适合正在做课程设计、毕设，或者拿旋转倒立摆练手控制算法的从业者，尤其是那些已经能跑通电机、但卡在起摆环节的人。
2. 起摆的物理逻辑：能量法为什么比「硬甩」靠谱
2.1 旋转倒立摆和直线倒立摆的本质差别
直线倒立摆的起摆，小车左右跑，摆杆在竖直平面内摆动，能量注入靠的是小车加速度和摆杆角度的耦合。旋转倒立摆不一样：摆杆装在旋转臂末端，旋转臂绕竖直轴转，摆杆在竖直平面内摆动。这个结构决定了它的能量注入方式更「间接」——你控制的是旋转臂的角度或角速度，摆杆的摆动是被旋转臂的向心加速度和切向加速度带起来的。
常见做法是先把旋转臂稳住，给一个恒定的角加速度，让摆杆在离心效应下逐渐摆起来。但这里有个坑：旋转臂的角加速度不能太大，否则摆杆还没摆到足够角度，旋转臂已经转了好几圈，线缆缠绕或者机械限位直接触发。源码里一般会限制旋转臂的角速度上限，同时监控摆杆角度，一旦超过某个阈值就切换到平衡控制器。
从控制角度看，旋转倒立摆的起摆是一个典型的欠驱动系统问题：两个自由度（旋转臂角度、摆杆角度），只有一个执行器（旋转臂电机）。你不能同时精确控制两个量，只能通过耦合关系把能量「泵」进摆杆。能量法的核心思想就是构造一个以摆杆能量为目标的控制律，让摆杆能量单调增加，直到接近竖直位置。
2.2 能量法起摆的控制律推导
设摆杆角度为 θ（竖直向上为 0，逆时针为正），旋转臂角度为 φ，旋转臂转动惯量为 J，摆杆质量为 m，摆杆质心到转轴距离为 l，重力加速度为 g。摆杆的总能量 E 可以写成动能加势能：
E = 0.5 * m * l^2 * θ_dot^2 + m * g * l * (cos(θ) - 1)
这里势能零点取在竖直向上位置，所以 θ=0 时势能为 0，θ=π 时势能为 -2mgl。起摆的目标就是让 E 从 -2mgl 增加到 0 附近。
对 E 求导，代入摆杆动力学方程，可以得到：
E_dot = m * l * θ_dot * (u - ...)
其中 u 是旋转臂的角加速度相关项。为了让 E_dot 恒为正，控制律通常取：
u = k * θ_dot * cos(θ)
k 是增益，符号根据旋转臂的转动方向确定。这个控制律的物理含义很直白：当摆杆向上运动时（θ_dot 和 cos(θ) 同号），给旋转臂一个加速，把能量灌进去；当摆杆向下运动时，反向操作。实际源码里不会直接写这个连续形式，而是离散化后每毫秒算一次，同时加限幅。
// 能量法起摆控制律（离散形式，伪代码） float theta = get_pendulum_angle(); // 摆杆角度，竖直向上为0 float theta_dot = get_pendulum_velocity(); // 摆杆角速度 float energy = 0.5 * m * l * l * theta_dot * theta_dot + m * g * l * (cosf(theta) - 1.0f); float energy_error = 0.0f - energy; // 目标能量为0（竖直向上） // 控制律：能量误差乘以摆杆角速度与角度的耦合项 float u = k_energy * energy_error * theta_dot * cosf(theta); // 限幅，防止旋转臂过冲 u = constrain(u, -u_max, u_max); set_motor_acceleration(u);这段代码里，k_energy是起摆增益，一般从 0.5 到 2.0 之间试，太大摆杆会甩过头，太小起摆时间太长。u_max是旋转臂角加速度上限，取决于电机扭矩和机械结构，源码里通常给一个保守值，比如电机额定扭矩的 60%。energy_error用目标能量减当前能量，保证能量单调增加。注意cosf(theta)在 θ=π/2 附近为零，这意味着摆杆水平时控制作用最弱，这是正常的——水平位置摆杆角速度最大，能量注入效率反而高。
2.3 起摆和平衡的切换判据
起摆控制器只管把能量泵上去，不管摆杆能不能稳住。所以必须有一个切换判据，在摆杆接近竖直且角速度足够小的时候，切到平衡控制器。常见判据是：
- 摆杆角度 |θ| < θ_threshold，一般取 10° 到 15°
- 摆杆角速度 |θ_dot| < θ_dot_threshold，一般取 1 rad/s 到 2 rad/s
- 连续满足上述条件超过 N 个控制周期，N 通常取 5 到 10
源码里这个切换逻辑一般写成一个状态机，起摆状态和平衡状态互斥。切换瞬间要把平衡控制器的积分项清零，否则积分饱和会导致切换后猛冲。这个坑后面会细说。
3. 源码结构拆解：从 main 到中断，每一层在干什么
3.1 工程目录和模块划分
拿到这份源码，第一件事不是急着编译，而是看目录结构。常规起摆源码一般包含这几个部分：
| 目录/文件 | 作用 | 关键函数 |
|---|---|---|
| main.c | 初始化、主循环、状态机调度 | main_loop() |
| encoder.c | 编码器读取和角度解算 | get_pendulum_angle() |
| motor.c | 电机 PWM 输出和方向控制 | set_motor_acceleration() |
| swing_up.c | 能量法起摆控制律 | swing_up_control() |
| balance.c | LQR 或 PID 平衡控制 | balance_control() |
| filter.c | 角度滤波和速度估计 | kalman_update() |
| config.h | 参数配置，增益、阈值、限幅 | 宏定义 |
这个划分的好处是每部分可以单独测试。比如你可以先把 motor.c 和 encoder.c 跑通，手动给电机开环信号，看编码器读数对不对；再单独跑 swing_up.c，观察摆杆能不能甩起来；最后把 balance.c 接上，调平衡参数。不要一上来就全系统联调，那样出了问题你根本不知道是哪一层。
3.2 编码器读取和角度解算的细节
旋转倒立摆一般用两个编码器：一个测旋转臂角度，一个测摆杆角度。摆杆编码器通常装在旋转臂末端，跟着旋转臂一起转，所以读出来的角度是相对旋转臂的，需要加上旋转臂角度才能得到绝对角度。源码里这一步容易写错，导致摆杆角度在旋转臂转动时跳变。
// 编码器读取和角度解算 float arm_angle = read_encoder_arm() * ARM_ENCODER_SCALE; // 旋转臂绝对角度 float pendulum_relative = read_encoder_pendulum() * PEND_SCALE; // 摆杆相对角度 float pendulum_absolute = arm_angle + pendulum_relative; // 摆杆绝对角度 // 归一化到 [-PI, PI] pendulum_absolute = fmodf(pendulum_absolute + PI, 2*PI) - PI; // 速度估计：差分加低通滤波 static float last_angle = 0.0f; float raw_velocity = (pendulum_absolute - last_angle) / DT; float filtered_velocity = low_pass_filter(raw_velocity, VEL_ALPHA); last_angle = pendulum_absolute;ARM_ENCODER_SCALE和PEND_SCALE是编码器分辨率换算系数，取决于编码器线数和减速比。VEL_ALPHA是低通滤波系数，一般取 0.1 到 0.3，太小速度滞后严重，太大噪声放大。这里有个血泪经验：速度估计不要用纯差分，噪声会让你在平衡点附近疯狂抖动，必须加滤波。但滤波太狠又会引入相位滞后，影响 LQR 的相位裕度。我一般先用 0.2 试，然后看平衡时的电机电流波形，如果高频抖动明显就降到 0.1。
3.3 状态机调度和中断优先级
主循环里跑状态机，但编码器读取和电机输出最好放在定时器中断里，保证控制周期严格。常见做法是 1kHz 控制频率，即每 1ms 执行一次控制律。主循环只负责状态切换和调试输出。
// 定时器中断服务函数，1kHz void control_isr(void) { float theta = get_pendulum_angle(); float theta_dot = get_pendulum_velocity(); float phi = get_arm_angle(); float phi_dot = get_arm_velocity(); switch (current_state) { case STATE_SWING_UP: swing_up_control(theta, theta_dot); if (check_balance_condition(theta, theta_dot)) { current_state = STATE_BALANCE; reset_balance_integrator(); // 关键：清零积分 } break; case STATE_BALANCE: balance_control(theta, theta_dot, phi, phi_dot); if (fabsf(theta) > FALL_THRESHOLD) { current_state = STATE_SWING_UP; } break; default: motor_stop(); } }中断优先级要设置好：编码器读取 > 控制计算 > 通信输出。如果编码器读取被通信打断，角度数据会错位，起摆直接失败。这个坑在 STM32 上尤其常见，因为默认优先级都是 0，谁先配置谁优先，很容易搞反。
4. 参数整定和避坑：那些让你熬夜的细节
4.1 起摆增益和限幅的配合
起摆增益k_energy和旋转臂角加速度限幅u_max是一对矛盾。增益大，起摆快，但容易过冲；限幅小，安全，但起摆慢甚至起不来。我一般这样调：
- 先把
u_max设成电机额定扭矩对应角加速度的 50%，k_energy从 0.5 开始。 - 观察摆杆能不能在 5 秒内摆到 30° 以上。如果不能，加大
k_energy到 1.0。 - 如果摆杆冲过竖直位置后反向甩得厉害，说明能量注入过多，减小
k_energy或降低u_max。 - 最终目标是摆杆在 3 到 5 个摆动周期内达到切换条件，且切换瞬间角速度不超过 2 rad/s。
这里有个玄学现象：同样的参数，电池电压不同起摆效果不一样。因为电机扭矩和电压相关，电压低的时候u_max实际输出达不到设定值。所以源码里如果有电压补偿，一定要打开；没有的话，每次换电池后重新微调k_energy。
4.2 切换瞬间的积分清零和输出限幅
起摆切平衡的瞬间，平衡控制器如果带积分项，积分器里可能已经积累了一个很大的值（因为起摆时平衡控制器虽然没输出，但如果你代码里一直在算，积分就在涨）。切换后这个积分会瞬间释放，电机猛冲，摆杆直接飞出去。
解决方法是：平衡控制器在起摆状态时不要计算积分，或者切换时强制清零。源码里一般写一个reset_balance_integrator()，在状态切换时调用。另外平衡控制器的输出也要限幅，一般不超过电机额定扭矩的 80%，留一点余量给突发扰动。
4.3 常见问题排查
现象一：摆杆在最低点小幅晃动，起摆控制器输出饱和但摆杆不动。原因：能量法控制律在 θ=π 附近cos(θ)接近 -1，θ_dot接近 0，乘积接近 0，控制作用极弱。这是能量法的固有死区。 解决：加一个初始扰动，比如给旋转臂一个短时脉冲，让摆杆偏离最低点。或者改用「能量+角度」混合控制律，在最低点附近加一个与 θ 相关的项。
现象二：摆杆能甩到顶，但每次都在切换后立刻倒下来。原因：切换判据太宽松，摆杆角速度还很大就切了，平衡控制器来不及拉回来。 解决：收紧切换阈值，θ_dot_threshold从 2 rad/s 降到 1 rad/s，同时增加连续满足周期数。代价是起摆时间变长，但成功率提高。
现象三：平衡时摆杆高频抖动，电机发热严重。原因：速度估计噪声大，或者 LQR 的微分增益过大。 解决：加大速度滤波系数，或者降低 LQR 的 Kd 项。用示波器看电机电流波形，如果 1kHz 周期内有明显尖峰，就是微分增益问题。
现象四：旋转臂转了几圈后线缆缠绕，限位触发。原因：起摆时旋转臂单向旋转，没有换向逻辑。 解决：在起摆控制律里加一个旋转臂角度监控，超过 ±180° 就反向注入能量。或者机械上装滑环，但那是硬件改动了。
现象五：换了电机或编码器后，起摆完全失效。原因：编码器方向或电机方向反了，能量法控制律的符号错了，越控能量越低。 解决：手动转摆杆，看角度读数是增加还是减少；手动给电机正电压，看旋转臂转向。两个方向必须匹配，否则控制律要取反。
5. 从起摆到平衡的验证：一套可复现的调试流程
5.1 分阶段验证方法
不要一上来就跑完整状态机。我习惯分四步：
第一步，开环测试。电机给固定 PWM，旋转臂应该匀速转；摆杆自由摆动，编码器读数应该平滑变化。如果编码器跳变，先解决硬件问题。
第二步，单独跑起摆。把平衡控制器注释掉，起摆后摆杆到顶就让它倒下来，观察起摆时间和最大摆角。目标是 3 秒内摆角超过 60°。
第三步，单独跑平衡。手动把摆杆扶到竖直附近，启动平衡控制器，看能不能稳住。如果稳不住，先调 LQR 或 PID，不要怪起摆。
第四步，全状态机联调。起摆切平衡，平衡切起摆，反复测试 20 次，统计成功率。成功率低于 80% 就回去查切换判据。
5.2 用串口数据做离线分析
源码里一般有串口输出，把摆杆角度、角速度、旋转臂角度、控制量打到上位机。用 Python 画个图，一眼就能看出问题。
import serial import matplotlib.pyplot as plt import numpy as np ser = serial.Serial('COM3', 115200) data = [] for _ in range(2000): # 采集2秒，1kHz line = ser.readline().decode().strip() values = [float(x) for x in line.split(',')] data.append(values) data = np.array(data) t = np.arange(len(data)) * 0.001 plt.plot(t, data[:, 0], label='pendulum angle') plt.plot(t, data[:, 1], label='pendulum velocity') plt.plot(t, data[:, 2], label='control output') plt.legend() plt.show()看三样东西：角度曲线有没有在切换点突变，速度曲线有没有高频噪声，控制量有没有饱和。如果控制量在起摆阶段一直饱和，说明u_max太小或者k_energy太大。如果速度曲线噪声峰峰值超过 2 rad/s，滤波不够。
5.3 参数保存和版本管理
调好的参数一定要存到 Flash 或者 EEPROM，不要每次上电重新调。源码里如果有save_config()和load_config()，确认它们真的在写。我见过太多人调了一下午，断电后参数丢了，第二天从头再来。另外，每次改参数前把config.h备份一份，命名带日期和改动内容，比如config_20250101_kenergy1.2.h。这个习惯能让你在参数调乱的时候快速回滚，比后悔药管用。
从那以后我每次调起摆参数，都强制走一遍「开环→起摆→平衡→联调」四步，绝不跳步。希望帮到你。
本文还有配套的精品资源，点击获取
