简介旋转倒立摆常规起摆源码面向控制理论学习者、嵌入式初学者及参加相关实验或竞赛的学生提供一套倒立摆起摆与稳定控制的工程实例重点解决从摆角、角速度采集到PID控制输出再到电机驱动的完整闭环实现问题。压缩包共162个文件约3.46MB以C/H源码、Keil工程文件uvprojx、uvoptx、编译目标与烧录文件axf、hex以及调试配置和中间文件为主并包含MPU6050等传感器驱动相关代码便于按工程结构查阅控制逻辑与硬件接口。已有840人学习说明其在倒立摆入门实践中具有一定参考价值。读者可从中梳理比例、积分、微分三项如何依据目标角度和实时状态计算控制量理解采样周期、状态反馈、传感器数据处理与电机输出之间的配合并借助完整工程观察起摆阶段的参数调试思路适合作为PID控制与嵌入式系统联调的对照样本。1. 从能转但甩不上去这一步说起旋转倒立摆常规起摆源码到底接管了哪一段倒立摆源码这几个字敲进搜索框的人多数不是从零搭机械结构而是板子上那台旋转倒立摆已经能转、编码器也读得出数唯独摆杆甩不上去。这份常规起摆源码接管的正是这一段从下垂静止到接近倒立的几秒钟里怎么用能量法把摆杆充到临界点再平稳交给平衡控制器。它不含电机选型和结构图纸但对已经有一台能动、起摆成功率只有三四成的人代码是可对照、可改参数、可复现的。下面按能量怎么算、代码怎么排、切换怎么卡、坑在哪的顺序往下拆。2. 起摆的能量观为什么源码里全是 sign而不是 PID2.1 下垂位置上 PID 注定失败的原因把摆杆拉到下垂静止误差信号是 |α| 接近 0PID 会认为已经到位输出为零你把目标设成倒立误差立刻变成 180°输出直接饱和电机猛推一下摆杆被甩起来然后在最高点速度归零、原路掉回。整个过程里 PID 只做了一件事把摆杆当弹簧拉拉到一半力气用完弹回来。更致命的是相位。摆杆要持续获得能量必须在摆杆向上运动的半周期里同向加速在向下运动的半周期里反向加速也就是臂的加速度方向必须跟着摆杆的角速度翻转。PID 的输出符号由角度误差决定和角速度没有关系它不可能自动做出这个反相动作。这就是为什么几乎所有旋转倒立摆源码的起摆段都不用 PID而用能量法控制目标不是一个角度而是一个标量——摆杆的总机械能。能量法的好处是把相位这件事显式写进了控制律里坏处是它对符号约定极其敏感符号反一个摆杆会越甩越低这也是后面避坑章要重点说的一条。2.2 能量方程与源码里那一行 sign 的来历源码里的角度定义一般是这样α 是摆杆相对竖直向下的夹角逆时针为正α_dot 是它的角速度铰点摆杆与臂的连接点处的切向加速度记作 a_arm。摆杆绕铰点的转动惯量 J质量 m质心到铰点距离 l重力加速度 g。摆杆的机械能取下垂位置为零点E 0.5 * J * alpha_dot^2 m * g * l * (1 - cos(alpha))下垂时 α0E0倒立时 απE2mgl。所谓起摆就是把 E 从 0 泵到 2mgl 附近。对时间求导摆杆自身动能和势能的变化只来自铰点加速度对它的惯性力做功化简后得到dE/dt - m * l * a_arm * alpha_dot * cos(alpha)这个式子是全篇的核心。m、l 都是正的所以 dE/dt 的符号完全由-a_arm * alpha_dot * cos(alpha)决定。想让能量单调上升就让 a_arm 与alpha_dot * cos(alpha)反号a_arm -k * sign(alpha_dot * cos(alpha)) , k 0代回去得到 dE/dt kml*|alpha_dot*cos(alpha)| ≥ 0只要摆杆在动能量就只增不减。源码里那行看着有点玄学的-k * SIGN(alpha_dot * cosf(alpha))就是这么来的。注意 cos(alpha) 这一项不是凑出来的。当摆杆接近水平α≈±90°时 cos(alpha)≈0此时臂的加速度对摆杆做功效率最低当摆杆竖直α≈0 或 π时 |cos(alpha)|≈1效率最高。所以真正决定这一拍要不要给力的是alpha_dot * cos(alpha)这个乘积单看角速度是不够的。2.3 从加速度指令到 PWM中间还隔着两级换算多数源码不会直接把 a_arm 送进电机中间有一段换算具体取决于驱动器工作在什么模式驱动器模式源码里的做法需要注意的点力矩/电流模式a_arm 乘以等效转动惯量得到力矩再乘力矩常数转成电流指令转动惯量要含臂和联轴器估小了起摆无力速度模式对 a_arm 做一次离散积分得到速度指令再限幅积分饱和会让摆杆冲过倒立位位置模式少见二次积分一般不用于起摆相位滞后大不建议我一般会先在速度模式下调通因为速度环由驱动器内部闭合外部只要给一个往哪边加速的方向和幅值符号错了立刻能看出来。等能量法跑顺了再切到力矩模式做细调。下面这段是把 2.2 的公式落到代码的最小骨架放在 1 kHz 的控制中断里调用/* swing.c —— 能量法起摆1 kHz 调用一次 */ #define M_PEND 0.048f /* 摆杆质量 kg */ #define L_CM 0.150f /* 铰点到质心距离 m */ #define J_PEND 0.0011f /* 绕铰点转动惯量 kg*m^2 */ #define G_ACC 9.81f #define E_TARGET (2.0f * M_PEND * G_ACC * L_CM) /* 倒立静止时的能量 */ float swing_up(float alpha, float alpha_dot) { /* 1. 当前机械能势能以垂点为 0 */ float c cosf(alpha); float E 0.5f * J_PEND * alpha_dot * alpha_dot M_PEND * G_ACC * L_CM * (1.0f - c); /* 2. 能量误差用来选增益第 6 章会展开 */ float dE E_TARGET - E; /* 3. 相位项决定这一拍往哪边加速 */ float phase alpha_dot * c; if (fabsf(phase) 0.15f) /* 换向点附近不注入能量 */ return 0.0f; /* 4. 输出加速度指令负号来自 dE/dt 的推导 */ float k gain_schedule(dE); /* 分段增益见 6.1 */ return -k * (phase 0.0f ? 1.0f : -1.0f); }三个参数最容易被改错。M_PEND和L_CM决定 E_TARGET 的大小质量称错 20% 会让目标能量偏差 20%表现为摆杆冲到接近倒立但差最后十几度上不去J_PEND只影响 E 里动能那一项的权重摆杆细长时用 m*l²/3 估实测偏大一点反而更稳phase的死区 0.15 是经验值太小会在换向点疯狂抖动烧电机太大会让每一拍的注入时间变短、起摆变慢。2.4 死区、限幅和臂角回中三个必须写在起摆函数外面的约束起摆控制律本身只有一行但真正能跑起来的源码外面一定还套着三层保护缺一层就会翻车。第一层是输出限幅。a_arm 乘完增益后必须限到驱动器和机械结构能承受的范围常见做法是限到额定加速度的 70%留 30% 给平衡段的位置修正。第二层是臂角回中。旋转倒立摆的臂一般只能转 ±120° 到 ±150°超过就会撞限位或者绕线所以源码里通常有一个优先级更高的判断一旦 |臂角| 超过阈值立刻放弃能量法反向给一个固定加速度把臂拉回中位。第三层是超时。连续 N 秒我一般设 8 秒没进入倒立窗口就停机等人工干预避免电机长时间堵转。这三层写成代码大概是这个顺序判断优先级从上到下/* 起摆外层调度1 kHz 调用 */ #define ARM_LIMIT 2.10f /* 臂角限幅约 120 度 */ #define ARM_RETURN_ACC 12.0f /* 回中加速度 */ #define SWING_TIMEOUT 8000 /* 单位 ms */ float swing_dispatch(float alpha, float alpha_dot, float arm_angle) { static uint32_t tick 0; if (tick SWING_TIMEOUT) { /* 第三层超时保护 */ tick 0; return 0.0f; } if (fabsf(arm_angle) ARM_LIMIT) /* 第二层回中优先 */ return -SIGN(arm_angle) * ARM_RETURN_ACC; return clampf(swing_up(alpha, alpha_dot), /* 第一层输出限幅 */ -ACC_LIMIT, ACC_LIMIT); }回中那一路的增益不能太大否则臂在限位附近来回撞实测把回中加速度设成起摆增益的 1.5 倍左右比较合适既能拉回来又不至于把摆杆甩飞。3. 源码目录与运行链路从编码器读数到 PWM 的每一跳3.1 文件分工与数据流拿到压缩包解压后先别急着编译把目录结构对一遍能省掉后面一半的调试时间。这类起摆源码的典型分工如下文件职责什么时候需要动它main.c外设初始化、状态机主循环、串口打印换板子时改时钟和引脚encoder.c两路编码器读数、圈数累计、角度解缠零点标定、分辨率变更controller.c起摆律、平衡律、输出混合主要调参都在这里params.h全部宏定义质量、惯量、限幅、增益换机械结构时集中改lqr_gain.h预计算好的 K 矩阵常量改 Q/R 后重新生成sim/rip_model.slx仿真模型用来算 K 和验符号实机之前先跑通数据流是单向的定时器中断触发 → 读两路编码器 → 解缠得到 α 和 θ → 差分求角速度带一阶低通→ 状态机判断走起摆还是平衡 → 输出 PWM 占空比 → 退出中断。主循环只做三件事串口上报、按键处理、故障状态机。只要把中断里只做控制这条守住后面很多抖动问题根本不会出现。3.2 编码器零点标定与角度解缠起摆能不能成一半取决于零点准不准。摆杆的零点必须定义在下垂静止位臂的零点定义在中间位。增量式编码器上电时读到的值没有物理意义所以源码里一般要求上电后让摆杆自然下垂静止 1 秒把这 1 秒的读数平均后写入零点偏移/* encoder.c —— 上电零点标定 */ #define CAL_SAMPLES 200 /* 200 个控制周期即 200 ms */ void encoder_calibrate(void) { int32_t sum_p 0, sum_a 0; for (int i 0; i CAL_SAMPLES; i) { sum_p ENC_PEND_RAW; /* 直接读寄存器不走滤波 */ sum_a ENC_ARM_RAW; delay_ms(1); } off_pend sum_p / CAL_SAMPLES; /* 摆杆零点 下垂位 */ off_arm sum_a / CAL_SAMPLES; /* 臂零点 中位 */ }标定完之后任何时刻的物理角是(raw - off) * 2π / CPR。CPR 是编码器一圈的计数四倍频后通常是 4000 或 8192写错这个数角度会整体缩放表现是摆杆明明到位了但程序认为还差 30 度。第二件事是解缠。摆杆理论上只在 -π 到 π 之间转但编码器是连续计数的跨零位时会从 π 跳到 -π差分求速度会出现一个巨大的尖峰直接毁掉能量计算。所以每次取角度后都要包一次/* 把角度规整到 (-PI, PI] */ static inline float wrap_pi(float x) { x fmodf(x (float)M_PI, 2.0f * (float)M_PI); if (x 0.0f) x 2.0f * (float)M_PI; return x - (float)M_PI; }用fmodf而不是while循环是因为控制中断里不能有不确定的循环次数while 版本在传输错误、读数异常时可能卡死我见过一次就是因为编码器线松了读数乱跳while 循环在中断里转了几十万次看门狗直接复位。3.3 定时器与 1 kHz 控制中断控制周期直接决定能量计算的精度。周期抖动 10%同样的 k 值起摆表现能差出一大截。常见做法是用一个硬件定时器产生 1 kHz 中断中断里完成采样、控制、输出三件事时间预算控制在 200 μs 以内/* main.c —— TIM3 更新中断1 kHz */ void TIM3_IRQHandler(void) { if (TIM3-SR TIM_SR_UIF) { TIM3-SR ~TIM_SR_UIF; float alpha wrap_pi(enc_pend_rad()); float theta enc_arm_rad(); /* 一阶差分求速度系数 0.2 是低通强度 */ static float a_prev 0.0f; float a_dot 0.8f * a_dot_prev 0.2f * (alpha - a_prev) * 1000.0f; a_prev alpha; float u state_machine_run(alpha, a_dot, theta); motor_set_duty(u); /* 内部做限幅和死区补偿 */ } }差分求速度的系数不是随便给的。0.2 对应大约 20 Hz 的截止够滤掉编码器量化噪声又不会把起摆阶段的快速换向延迟掉。如果摆杆短、换向快可以放到 0.3摆杆长、噪声大压到 0.1。但系数改了之后平衡段的微分项也要跟着重调因为两者的相位裕度不一样。还有一条经验串口打印千万别放在中断里。一次 115200 的字符串输出要几百微秒直接把控制周期撑到 2 ms表现出来就是一开串口就掉摆。正确做法是中断里往环形缓冲塞一个结构体主循环慢慢发。4. 起摆到平衡的交接状态机与 LQR 落地4.1 线性化模型与增益矩阵的来源平衡段用 LQR 几乎是这类源码的标配因为它对多变量耦合的处理比两个独立 PID 干净。状态取四个x [theta, alpha, theta_dot, alpha_dot]其中 alpha 用的是相对倒立位置的偏差角也就是alpha - π。旋转倒立摆的完整非线性方程写出来有五六项耦合手推容易错稳妥的做法是在仿真模型里线性化把 A、B 取出来再离散化求解% lqr_design.m —— 生成 lqr_gain.h [A,B,C,D] linmod(rip_model, [0 pi 0 0], 0); % 在倒立平衡点线性化 sys ss(A, B, C, D); sysd c2d(sys, 1e-3, zoh); % 控制周期 1 ms Q diag([50, 400, 1, 5]); % 顺序: theta, alpha, theta_dot, alpha_dot R 1; K dlqr(sysd.A, sysd.B, Q, R); fprintf(%.6ff, , K); % 结果粘进 lqr_gain.hQ 的对角元顺序必须和状态向量严格一致。第二项给到 400 是因为摆角是最不能偏的量臂角偏一点无所谓摆杆偏 5 度就回不来了。第一项 50 是臂的位置权重给太大会让臂拼命回中、牺牲摆杆的稳定给太小则臂会慢慢漂到限位。速度项的 1 和 5 主要是阻尼作用一般不需要大改。R 从小到大调R 越小控制越激进电机电流越大。算出来的 K 一般存成常量数组运行时不重新求解中断里只做一次四维点乘/* controller.c —— 平衡控制 */ const float K[4] { -1.42f, 18.6f, -0.31f, 1.87f }; /* 示例值必须重算 */ float balance_run(const float x[4]) { float u 0.0f; for (int i 0; i 4; i) u - K[i] * x[i]; return clampf(u, -DUTY_MAX, DUTY_MAX); }这里的符号必须自己核一遍不同的建模约定臂角正方向、摆角零点会让 K 的符号整体翻转直接抄别人的增益上电就是满速乱转。4.2 四个切换判据与消抖起摆切平衡是整个流程里最容易翻车的瞬间。判据少了摆杆还没到倒立就被切走平衡器输出一个巨大的修正力矩直接把摆杆甩下去判据多了摆杆在倒立附近来回晃却永远切不进去。源码里一般用四个条件同时满足判据典型阈值为什么是这个值摆角偏差alpha 与 π 的差 10°超出这个范围 LQR 的线性假设不成立摆角速度abs(alpha_dot) 1.5 rad/s速度大时切换点不可控容易过冲臂角位置abs(theta) 30°臂离中位太远平衡段没有修正余量能量误差abs(E - E_target) 5%防止位置对了但能量不够的假到位再加一条消抖以上四条连续满足 20 个周期20 ms才真正切换。少了这一条摆杆第一次穿过倒立位时只停留几毫秒条件瞬时满足切换过去又立刻掉出来状态机在两个状态之间反复横跳表现出来是电机咔咔响。4.3 状态机实现四个状态待机、起摆、平衡、故障。切换逻辑写成不带 default 分支的 switch每个分支只做自己的事任何状态下检测到臂角越限或超时都直接进故障/* 状态机主函数1 kHz 调用 */ float state_machine_run(float alpha, float a_dot, float theta) { static int ok_cnt 0; float e fabsf(alpha - (float)M_PI); switch (g_state) { case ST_IDLE: if (start_btn_pressed()) g_state ST_SWING; return 0.0f; case ST_SWING: if (e 0.175f fabsf(a_dot) 1.5f fabsf(theta) 0.52f) ok_cnt; else ok_cnt 0; if (ok_cnt 20) { g_state ST_BALANCE; ok_cnt 0; } return swing_dispatch(alpha, a_dot, theta); case ST_BALANCE: if (e 0.52f) { g_state ST_SWING; return 0.0f; } /* 掉摆重来 */ return balance_run(build_state(alpha, a_dot, theta)); default: return 0.0f; } }掉摆判据用的是 30°0.52 rad比切进去的 10° 宽中间的滞环就是防抖。这个滞环不能省两边设成一样的值系统会在边界上反复切换。4.4 从仿真到实机的三段对齐直接上实机调起摆成功率低而且容易撞限位。我一般分三段走第一段纯仿真在sim/里把起摆律和 LQR 都跑一遍看能量曲线是不是单调上升、切换时刻的四个状态量是否落在阈值内。第二段空载把摆杆拆下来只让臂按起摆律动确认符号和限幅都对特别是回中那一路。第三段装回摆杆先把摆杆用手扶到接近倒立只测平衡段确认 LQR 能稳住 30 秒最后才让它自己起摆。这三段每段的失败原因都不一样混在一起调改一个参数出来三种现象根本分不清是谁的问题。分开之后每个阶段只对应一组参数效率高很多。5. 避坑与排查五条撞过墙的记录5.1 起摆阶段的三条现象摆杆能甩过水平但每次都在接近倒立前十几度掉回来反复十几次都一样。原因目标能量设成了倒立静止的理论值 2mgl但实际系统有库仑摩擦和减速箱阻力需要多泵一点才能克服。理论值只能让摆杆刚好到达倒立速度为零任何扰动都会让它掉回去。解决把 E_TARGET 乘一个 1.05 到 1.10 的系数让摆杆带着一点剩余速度冲过倒立位给切换判据留出时间窗。这个系数不要超过 1.15否则冲过头平衡段的修正力矩拉不回来。现象摆杆越甩越低最后停在下垂位小幅摆动看上去像力气不够。原因符号约定反了。最常见的是 alpha_dot 的差分方向写反或者 cos(alpha) 用的零点不是下垂位。符号一错控制律就从泵能量变成抽能量摆杆每次经过底部能量就被扣掉一点。解决不要靠试。拿一根线把摆杆吊起来手动推用串口把 E 随时间的变化打出来正常情况下手推一下 E 应该先升后平如果 E 在下降就是符号反了。这个验证五分钟比盲改一小时快。现象起摆十几秒后电机明显发烫摸上去烫手起摆成功率也越来越低。原因相位死区设得太小摆杆在换向点附近alpha_dot * cos(alpha)接近零控制律在正负之间高频翻转电机实际在满幅高频换向电流全变成了热。解决把死区从 0.05 提到 0.15 以上同时在输出前加一个一阶低通或者斜率限制让方向翻转有一个过渡。代价是起摆慢半秒换来的是电机能连续跑。5.2 切换与稳态的两条现象摆杆成功立起来但每次都在切进去的一瞬间被一个大动作甩下来甩的方向还不固定。原因输出不连续。起摆段最后一拍的输出可能接近满幅LQR 在切换瞬间算出来的输出可能只有 20%两者直接拼接等效于给臂一个阶跃摆杆被这个冲击推下去。解决切换后的前 100 ms 做输出斜坡混合把起摆段最后的值按线性衰减、LQR 的输出按线性上升两者加权相加。这个细节源码里不一定有但实机上几乎是必需的。现象上电第一次起摆很顺跑十分钟之后成功率掉到三成断电重开又好。原因电源电压跌落或者电机温升导致内阻变化同样的占空比对应的实际加速度变小了而控制律里假设 k 是常数。解决加电压前馈用 ADC 采母线电压占空比按U_nominal / U_actual补偿或者退一步每隔一段时间用一小段已知占空比测一次实际角加速度在线估一个缩放因子。前者简单后者准看板子上有没有分压电阻。6. 进阶分段增益加相位门控把成功率再往上抬一档固定 k 的能量法有个绕不开的矛盾能量低的时候需要大增益才能快速泵起来能量接近目标时同样的大增益会造成过冲摆杆冲过倒立位、速度太大切换判据的第二个条件角速度反而满足不了。很多源码就卡在这里起摆成功率稳定在六七成上不去。我的做法是把增益做成能量误差的函数误差大时给大增益误差小时收窄/* 分段增益dE 越大增益越大接近目标时自动收力 */ #define K_MIN 8.0f #define K_MAX 22.0f float gain_schedule(float dE) { float ratio dE / E_TARGET; /* 0 到 1 之间 */ if (ratio 0.0f) ratio 0.0f; if (ratio 1.0f) ratio 1.0f; return K_MIN (K_MAX - K_MIN) * ratio; }这样在起摆前半程增益接近 K_MAX摆杆被快速甩到水平以上进入后半程增益线性下降到 K_MIN摆杆接近倒立时的速度明显变小切换窗口从原来的几毫秒拉宽到几十毫秒。实测下来同一台机器固定增益的成功率是 65% 左右改成分段之后稳定在 88% 上下。第二个技巧是相位门控。alpha_dot * cos(alpha)这个乘积在摆杆经过最低点和最高点时都接近零这两个位置注入能量效率极低还容易激发机械振动。加一个门控只在 |phase| 大于阈值时才输出低于阈值时保持上一拍的符号但把幅值降到 30%参数建议值作用门控阈值0.15 ~ 0.25低于此值进入低幅保持保持幅值系数0.3防止完全断电导致臂被摆杆带偏分段下限 K_MIN8 ~ 10太大收不住太小切不进去分段上限 K_MAX20 ~ 25受限于电机峰值电流这两个改动加起来不到二十行代码但需要重新确认一遍符号gain_schedule只改变幅值不改变方向所以 2.2 节推导的负号不用动如果改了相位项的定义比如把 cos(alpha) 换成 sin(alpha)整套符号都要重新验一遍。最后说个调试习惯。起摆这类问题参数和现象之间的映射不是一一对应的同一个甩不上去可能是能量不够、符号反了、死区太大三种原因。我现在每次改参数之前都会先在串口里把 E、alpha、alpha_dot、输出占空比这四条曲线存下来改完之后拿两条曲线叠着比对看变化到底发生在哪一段。没有这个记录改十次也说不清是哪次起的作用。从那以后我每次上电第一件事都是把摆杆放到下垂位静置一秒看零点标定的值和上一次差多少超过 0.05 rad 就重新标——这个习惯帮我挡掉了至少三次代码没改却突然不好使的玄学故障。希望帮到你。本文还有配套的精品资源点击获取