简介：旋转倒立摆常规起摆源码包是一份面向控制理论与嵌入式初学者的完整工程实例，基于经典旋转倒立摆装置实现常规起摆与平衡控制，重点演示PID控制器的设计方法与实际效果。压缩包共162个文件，其中C源码与头文件构成核心控制逻辑，o、axf、hex及map等文件可直接查看编译与烧录产物，uvprojx、uvopt等Keil工程配置便于直接打开调试，包体总大小约3.46MB，结构简洁，适合课程设计、竞赛备赛和自学入门。该资源已有838人学习下载，并沉淀了实践者反馈。研读源码可以深入理解比例项消除偏差、积分项抑制稳态误差、微分项抑制超调的调参思路，同时结合MPU6050传感器驱动与数据滤波代码，能掌握状态反馈、采样周期控制以及实时控制系统的软件实现，是一份兼顾理论与工程落地的宝贵参考资料。
旋转倒立摆常规起摆源码解析：能量法起摆的完整调参与避坑记录
旋转倒立摆是控制入门里最经典的“玩具”之一：一根摆臂在水平面转，末端挂一根摆杆，任务就是让摆杆从自然下垂的状态自己甩起来、越过水平，最后在竖直向上的倒立点稳住。说来简单，真上手才知道，起摆这个动作反而是整套系统里最考验控制功底的部分。最近整理资料又翻到这份旋转倒立摆常规起摆源码，我干脆把它完整拆一遍，把起摆的原理、代码结构、参数整定和踩坑记录都写在下面，给后面做课程设计或者搞欠驱动控制的朋友一个能直接参照的模板。
这份源码采用的“常规起摆”，指的就是控制领域最常见的能量注入法。它不依赖精确的动力学模型，也不用非线性规划那些复杂工具，核心就是让摆臂在合适时机给摆杆补充机械能，等摆杆接近倒立点后再交给LQR稳定控制器。整个过程逻辑非常清晰，适合作为初学者理解欠驱动系统控制的入门项目，也适合作为实验室里旋转倒立摆平台的标准底包——不管你是毕设要用，还是自己搭台子做实验，或者单纯想研究一下起摆和稳定的交接逻辑，这份源码都值得花时间看透。
1. 项目概述：这套起摆源码到底做了什么
1.1 核心问题与解决思路
旋转倒立摆的物理结构其实很简单：底座固定，摆臂由电机驱动在水平面内旋转，摆杆通过旋转关节连接在摆臂末端，只能被动跟随。整个系统有两个自由度，但只有一个输入（摆臂的加速度），属于典型的欠驱动系统。要让摆杆稳稳停在竖直向上，单靠把摆臂转到某个固定位置是做不到的，必须反复利用摆臂的加减速，给摆杆持续传递惯性力，也就是物理上常说的“荡秋千原理”。
起摆是整套系统里最微妙的环节。摆杆从自然下垂状态开始，初始能量最低，如果直接给摆臂一个大量加速度，摆杆只会被甩到一侧，根本不会往竖直方向走。常规做法是用能量反馈项来判断当前该不该注入能量：摆杆在往上走的时候加一把劲，往下走的时候适当抽劲，让摆幅一圈圈变大。当摆杆摆到接近竖直点时，切换到LQR控制器，用线性化模型把它“钉”在倒立位置。起摆部分解决“怎么上去”，稳定部分解决“怎么待住”，两者缺一不可。
1.2 适合谁看、能复用到哪里
这套源码包的目标受众很明确：正在做旋转倒立摆课程设计或毕业设计的同学、想从源码层面理解能量法起摆的控制工程师、以及在做自平衡机器人或摆式系统时需要迁移起摆思路的人。源码里的核心模块并不绑定特定硬件，只要你的平台有摆臂角度传感器、摆杆角度传感器和一个能输出力矩的电机，这套框架就能移植。
我在实际使用中，通常会在拿到源码后做三件事：第一步跑通仿真，验证控制逻辑；第二步对照自己的机械平台校核坐标定义和电机方向；第三步按下文提到的调参顺序，把能量增益和LQR权重整定到适配自己结构的状态。这也是我下面要展开讲解的主线。
2. 原理先行：为什么常规起摆首选能量法
2.1 为什么直接“转向倒立点”不可行
刚接触这个项目的人通常会有个疑问：既然目标是把摆杆转到竖直向上，那把摆臂快速甩到某个位置不就行了吗？实际的物理过程会打破这个幻想。摆杆和摆臂之间是自由铰接，摆臂的运动只能通过惯性力间接作用于摆杆。如果你直接给摆臂一个最大加速度，摆杆会被惯性力甩到与目标点完全相反的方向；等摆臂到位减速停止时，摆杆已经荡过头甚至绕了好几圈，根本不会正好停在竖直位置。
更麻烦的是，倒立点本身是不稳定的平衡点。任何一个微小扰动——比如编码器噪声、电机力矩波动、桌面震动——都会让摆杆倒向一侧。所以起摆过程不能“直接瞄准”，只能“逐步荡上去”：先让摆杆摆幅逐渐增大，然后在摆杆接近倒立点且角速度较小的短暂窗口内，由稳定控制器迅速接管。这也是为什么几乎所有成熟方案都把起摆和稳定拆成两个独立的控制模块来处理。
2.2 能量法的控制律与直觉理解
把摆杆简化为质量集中在末端的单摆模型，它的机械能可以写成：
E = (1/2) * m * L² * θdot² - m * g * L * cosθ
这里 θ 是摆杆与竖直方向的夹角，θ=0 定义为竖直向上，θdot 是角速度，L 是摆杆等效长度。竖直倒立点对应的参考能量是 E_ref = -m * g * L。当摆杆能量低于 E_ref 时，需要向外补充能量；高于时，要适当抽取能量避免过冲太远。
能量法起摆的控制律在代码里通常长这样：
float energy_swingup_ctrl(float theta, float theta_dot, float alpha_dot, float dt) { /* 摆杆能量估算（质量集中在末端近似） */ float E = 0.5f * L_POLE * L_POLE * theta_dot * theta_dot - G * L_POLE * cosf(theta); float E_ref = -G * L_POLE; /* 倒立点势能 */ float dE = E_ref - E; /* 能量误差 */ /* 目标摆臂加速度：能量注入 + 摆杆阻尼 + 摆臂阻尼 */ float acc = K_E * dE * signf(theta_dot * cosf(theta)) + K_D * theta_dot * cosf(theta) - K_A * alpha_dot; return clampf(acc, -ACC_MAX, ACC_MAX); }这个控制律一共有三项，每一项都有明确的物理含义：
- 第一项是能量反馈项，也是起摆的核心。它只在“值得给能量”的时机动作：摆杆正在往上运动（θdot 为正）且摆杆在水平线以下时（cosθ 为正），给摆臂一个正向加速度，让惯性力帮摆杆继续向上走。符号函数 sign(θdot * cosθ) 保证了注入方向始终正确。
- 第二项相当于对摆杆角速度做一个与运动相位相关的激励，让摆动更有节奏感，避免能量项单独作用导致的抖动，同时也能在摆杆越过水平线后提供制动效果。
- 第三项是摆臂自身的阻尼，防止起摆过程中摆臂不断往一个方向偏转。真机上如果少了这一项，摆臂大概率会逐渐转飞，最后撞上限位。
必须说明，这里的控制律是一个工程简化版。真机上电机能输出的加速度有限，所以 ACC_MAX 要按你的电机能力设置，一般取 20~30 rad/s² 起步；同时摆臂加速度要先积分成目标速度，再变成PWM电压输出，还要额外处理死区补偿。但控制核心就是这几行，读懂了它，整个起摆过程就通了一半。
2.3 切换逻辑：从起摆到稳定的交接
起摆结束时，摆杆不一定是静止在倒立点的，而是带着一个较小的角速度冲上去。如果LQR控制器接手太早，摆杆角度还比较大，线性化模型误差会大到控制不住；接手太晚，摆杆已经穿过倒立点往下落，机会窗口就错过了。
我常用的一组切换条件是这样的：
bool need_switch(const PendulumState *st) { /* 摆杆角度接近竖直，角速度足够小，才进入平衡控制 */ return (fabsf(angle_wrap(st->theta)) < 20.f * DEG2RAD) && (fabsf(st->theta_dot) < 1.5f); }但有经验之后，我会在这个基础上再加两个保险：一是要求摆臂角速度也比较小，二是加一个“切换锁定”——连续满足条件超过20个控制周期才真正切过去。否则单次噪声触发切换，LQR在非平衡点附近发力，很容易直接把摆杆打飞。切换瞬间，起摆控制量要立即归零，同时给LQR的输出做一个斜坡限制，避免控制指令跳变。
3. 源码结构与核心模块拆解
3.1 拿到zip后先看目录
绝大多数的旋转倒立摆源码包，都会包含仿真模型和嵌入式代码两部分。打开压缩包后，我建议按下面这个顺序看文件结构：
rotation_inverted_pendulum/ ├── README.md # 搭建说明和坐标定义 ├── model/ # 动力学模型与参数表 ├── sim/ # 仿真（Simulink或Python） ├── src/ │ ├── main.c # 主循环与中断 │ ├── pendulum.c # 角度解算、滤波 │ ├── swingup.c # 起摆控制 │ ├── balance.c # LQR平衡控制 │ └── estimator.c # 状态估计 └── docs/ # 参数标定记录拿到源码不要急着编译烧录，先把 README 里的坐标定义看清楚。这个问题太关键了，不同平台的 θ=0 有的定义为竖直向上，有的定义为竖直向下；有的顺时针为正，有的逆时针为正；电机极性的定义也可能完全相反。方向定义一旦反了，能量项里的 sign 全部反号，起摆必然失败，而且失败得很离谱。我现在的习惯是，先把角度定义、正方向、限位位置和电机极性这四件事画在一张纸上，再碰代码。
3.2 主循环与控制节拍
真机控制率通常跑 1kHz，也就是主循环 1ms 执行一次。每个周期要做的事很固定：读两个编码器的最新值、换算成弧度并做绕回处理、差分得到角速度、经过低通滤波、判断当前处于起摆还是平衡阶段、计算控制量、输出PWM。角度换算最容易踩坑的是绕回（wrap），比如角度从 +179° 跳到 -179°，如果不避免这个跳变直接差分，角速度会算出一个接近每秒几万度的假值。
角速度滤波我常写成一阶低通，惯性时间常数取 5~10ms 左右就够了。编码器分辨率如果只有几百线，差分噪声会很大，摆杆看似静止时角速度曲线却是毛刺状的，这种噪声会让能量项反复乱动作。解决思路要么提高编码器分辨率，要么把低通时间常数加大一点，但滤波太强又会引入相位延迟，这个尺度需要现场试。
3.3 LQR平衡控制参数配置
平衡控制器几乎都用LQR，因为旋转倒立摆在倒立点附近线性化效果很好，LQR调起来直观。状态变量取 [α, θ, αdot, θdot]，其中 α 是摆臂角度。离线用 python-control 就能算出反馈矩阵：
import numpy as np import control # A, B 由平台参数线性化得到，这里假设已在变量中定义 Q = np.diag([3.0, 8.0, 0.05, 0.08]) R = np.array([[0.01]]) K, S, E = control.lqr(A, B, Q, R) print(K)调Q矩阵我有一套固定的经验顺序：先把摆杆角度 θ 的权重拉高，让摆杆优先站稳，我一般从 8~10 开始；然后加摆臂角度 α 的权重，让摆臂能归中，一般 2~5；两个速度项的权重给 0.01~0.1，太大容易抖，太小又会在平衡点附近来回蹭。R 一般从 0.01 附近开始调，R 太大控制量太软，R 太小则控制量剧烈振荡。按这个顺序调出来的K，真机上基本能做到摆杆起摆后在倒立点附近 ±5° 内稳定，摆臂缓慢归中不偏移。
4. 从仿真到真机的实操流程
4.1 先用仿真把参数空间走一遍
我的习惯是，任何控制代码在接触真机前，先在建好的仿真环境里完整跑一遍。旋转倒立摆的仿真模型并不难构建，用欧拉积分就能得到足够真实的响应。关键是仿真里要主动加入工程噪声源：编码器量化误差、控制周期抖动、电机死区、摆臂限位。真机上的很多问题，本质都是这些“理想模型里不存在”的细节引起的。
仿真里我最关注三条曲线：摆杆角度 θ 随时间的变化、能量误差 dE 的收敛过程、控制量的跳变情况。如果能量误差始终不往零走，说明控制律符号或者增益有问题；如果控制量在切换瞬间出现很大的阶跃，说明切换条件或限幅要调整。这些在仿真里看明白，真机调试就能省掉一大半时间。
4.2 真机参数整定的顺序
上真机后，我严格按五步走，每步都有明确的验证通过标准：
- 锁死摆杆，只跑摆臂位置环。确认电机极性、编码器方向、PWM响应都正确。摆臂能稳定停在任意给定角度，这是基础。
- 放开摆杆，从很小的能量增益开始。比如 K_E 从 0.3 起步，手动扶住摆杆，观察起摆逻辑是否让摆幅缓慢增大。如果摆幅停滞，先查符号定义，再慢慢加大K_E。
- 逐步加大 K_E，直到摆杆能越过水平线。这一步是纯手感活，每次加 0.1~0.2，观察摆动是否越来越有力但不过冲。增益太大时摆杆会“爆震”，整机噪音明显变大。
- 调切换条件，让LQR顺利接管。先用宽松条件（比如角度阈值 25°、角速度阈值 2rad/s），确认能接住后，再逐步收紧到更稳定的窗口。
- 回调LQR的Q/R，优化稳定段的跟随表现。
这五步里，第二步和第三步最容易让人反复纠结。经验是：增益太小，摆幅在 10°~20° 就上不去了；增益适中，摆幅会每两个周期肉眼可见地增大一圈；增益过大，摆动会变得毛躁、发涩，甚至摆杆在最高点附近乱颤。
4.3 编码器标定、滤波与摩擦补偿
增量式编码器上电后没有绝对零点，所以首次上电必须做零点标定。简单的做法是让摆杆自然下垂，记录编码器读数，把它对应的角度标记为 θ=π（如果坐标定义竖直向上为0）。这个偏置值可以写死在参数表里，也可以做成上电自检流程。机械装配和温度变化会导致零点漂移，运行一段时间后如果明显感觉平衡位置偏了，就重新标定一次。
关于摩擦补偿，我再补充一句。直流电机或者带减速箱的电机在零速附近都有死区，PWM小到一定程度时电机根本不动。起摆过程中摆臂经常要做小幅换向，死区会让能量注入变得“断续”，表现为摆幅上不去。处理办法是在控制输出上叠加一个随目标方向变化的死区补偿电压，补偿值的大小靠实验测：手动给PWM，记录刚能克服静摩擦的最小占空比。这一项对起摆成功率的影响，很多时候比调K_E还大。
5. 常见问题与排查技巧实录
5.1 摆幅怎么也上不去
这是起摆调参时遇到最多的现象。排查思路按下面这个顺序来：
- 检查符号是否正确。把摆杆用手拨到90°水平位置，观察能量误差和控制量的方向，反号就改过来。
- 检查能量增益是否太小。从 0.3 到 2.0 之间分段尝试，找到摆幅稳步增长的区间。
- 检查编码器噪声和滤波。如果角速度信号毛刺严重，滤波后再试试。
- 检查死区补偿。低速段会不会出现电机“卡住不动”的情况，用手摸摆臂感受有没有迟滞。
我在实际项目中遇到过一种很隐蔽的情况：能量增益改到很大还是摆不上去，最后发现是摆杆质量太轻，能量估算里的 L_POLE 参数和实际不匹配，导致能量误差算得不准。把等效摆长按实际重心位置修正后，问题立刻解决。
5.2 摆上去之后马上掉下来
这说明切换环节出了问题。最常见的是切换太早——摆杆离竖直位置还比较远，LQR按线性模型算出来的控制量根本压不住。解决办法是先放宽角度阈值到 25° 左右，确保LQR能接住，再逐步收紧。另外，切换瞬间如果控制量跳变太大，也会把摆杆“踢”下去，所以一定给切换后的控制量加一个斜坡限制，让LQR的输出从零开始逐渐增大。
还有一个细节容易忽略：LQR的 θ 符号定义必须和起摆模块保持一致。我见过有人在仿真里调好了一组K，搬到真机上却来回振荡，最后发现是符号定义相反，控制输出方向反了。
5.3 起摆时摆臂越走越偏
起摆过程中摆臂产生一个单向位移是很正常的，因为能量注入项本身就会造成净加速度。如果偏得越来越快，说明第三项摆臂阻尼太弱或者符号反了。我的处理方法是：把 -K_A * alpha_dot 这一项单独拿出来看，K_A 从 0.5 开始往上加，直到摆臂在起摆全程的偏移量不超过一定范围，比如 ±30°。
机械限位必须作为最后一道保险。程序里一定要加软限位：当摆臂角度超过设定阈值时，控制量直接置零并报警，避免摆臂撞坏或电机过流。这一行代码，比任何控制算法都值得优先加上。
写在最后的一点体会
旋转倒立摆这个项目，网上框架代码一抓一大把，但真正能跑稳定、不出事故的版本，往往不是算法最花哨的，而是把方向定义、限位保护、滤波和切换时机这些“脏活”做到位的版本。这份常规起摆源码的价值也正在于此——它给你一条被反复验证过的主路径，剩下的细节调试，才是真正长本事的地方。能把起摆调顺，再去碰自平衡机器人或者其他欠驱动系统，心里就有底了。
本文还有配套的精品资源，点击获取
