企业官网建设流程全解析

简介：旋转倒立摆常规起摆源码.zip 面向控制理论与嵌入式控制入门者，围绕旋转倒立摆的常规起摆与直立平衡过程给出可编译运行的完整工程，适合用来理解状态反馈、PID 参数整定与实时控制循环的组织方式。压缩包共 162 个文件、约 3.46MB，以 24 个 .c 源文件与 30 个 .h 头文件为主体，配套 Keil 工程文件与 .hex、.axf 可执行镜像，以及 MPU6050 姿态解算等驱动代码，构成从工程配置到硬件驱动的完整链路。已有 840 人学习。读者可从中获得起摆阶段的控制输入计算逻辑、传感器数据读取与滤波、电机驱动输出及采样周期的实现细节，并借助现成工程直接编译烧录、逐项调整比例、积分、微分参数观察响应变化，把 PID 原理与实机调试对应起来。

1. 旋转倒立摆起摆源码：垂摆上电，两秒后自己站到顶部

上电那一刻摆杆还垂在正下方，电机先是一顿，接着左右摆动幅度越来越大，第三下把摆杆甩过水平线，随后在顶部小幅抖两下稳住——这就是旋转倒立摆的常规起摆。这份旋转倒立摆源码做的正是这件事：它不依赖精确模型，用能量法把欠驱动状态下的摆杆“泵”到接近竖直，再交给 LQR 接管稳摆。它适合手上有旋转倒立摆实验台、或者正准备自己搭一台的开发者，也适合做过位置环 PID、想看看欠驱动系统究竟难在哪的人。整个工程烧进去就能自起，不需要上位机在线陪着跑，断电重启也不用人扶。

2. 技术栈与工程结构：压缩包里哪几个文件在真正干活

2.1 硬件与软件栈：一颗 M4 主控、两路编码器、一路 H 桥

这套倒立摆源码不是纯仿真工程，是能直接烧到台子上的裸机程序。主循环几乎不干活，真正的控制全部压在 1kHz 的定时器中断里：读两路编码器 → 算摆角、臂角及其角速度 → 判断当前处于起摆还是稳摆 → 输出一路带方向的 PWM。整条链路里没有 RTOS，没有动态内存分配，中断服务函数只做定长浮点运算，最坏执行时间是可以算出来的。

不用 RTOS 是刻意的。1ms 的控制周期本来就紧，任务调度带来的微秒级抖动会直接变成角速度估计里的噪声，而速度项在 LQR 里权重不低，抖起来整台架子都在响。常见的做法是把控制环放在最高优先级定时器中断，串口波形输出放在主循环里做低优先级搬运，两边用环形缓冲区隔开，谁都不阻塞谁。

两路编码器的分工不一样。电机端那一路测的是旋转臂的角度 α，转速高，一般用增量式 A/B 正交编码器，定时器硬件四倍频；摆杆端那一路测的是摆角 θ，量程只有一圈，用单圈绝对值编码器通过 SPI 读，分辨率 12 位以上起步——摆杆在顶部附近的控制精度全靠它，10 位分辨率基本没法稳住。

注意：摆杆端的编码器一定要装在摆杆的转轴上，不要装在电机轴上再换算。中间任何一级同步带或齿轮的间隙，都会在顶部变成肉眼可见的抖动。

2.2 目录结构与第一次编译烧录

解压之后别急着打开 main.c，先看目录。工程的分层很清楚，控制算法和硬件操作是分开的，换板子的时候只动 bsp 那一层就行。

rotary_pendulum/ ├── app/ │ ├── main.c 初始化、主循环、串口搬运 │ ├── control_task.c 1kHz 中断入口，串起整条控制链 │ ├── swing_up.c 能量法起摆 │ ├── lqr_balance.c LQR 稳摆 │ └── mode_switch.c 起摆/稳摆切换与防抖 ├── bsp/ │ ├── encoder.c 两路编码器读取、方向、零位 │ ├── motor_pwm.c PWM 与方向脚、死区补偿 │ └── uart_scope.c 波形帧打包发送 ├── math/ │ ├── matrix.c 4x4 矩阵乘向量，LQR 用 │ └── filter.c 一阶低通、限幅、符号函数 ├── config/ │ └── params.h 所有可调参数集中在这一个文件 └── tools/ ├── lqr_design.m 求 LQR 增益的脚本 └── scope_plot.py 串口波形上位机

编译和烧录就是标准三步，工具链用工程里自带的 Makefile，第一次跑之前确认交叉编译器在 PATH 里：

make clean # 清掉上一次的 obj，改过 params.h 之后必须重新全编 make -j8 # 并行编译，产物在 build/ 下 make flash # 通过调试器下载并复位运行

编译通过不代表能转。第一次上电建议把params.h里的SWING_ENABLE置 0，先用调试器手动给一个很小的占空比，确认电机转向和编码器读数方向一致，再打开起摆。所有可调参数都集中在同一个头文件里，这也是这套工程比较友好的地方：

/* config/params.h —— 只改这里，别在 .c 里埋魔法数字 */ #define CTRL_TS 0.001f /* 控制周期 1ms */ #define PEND_M 0.045f /* 摆杆质量 kg */ #define PEND_L 0.200f /* 摆杆质心到铰点距离 m */ #define PEND_J 0.0006f /* 摆杆绕铰点转动惯量 kg*m^2 */ #define SWING_K 0.35f /* 能量项增益 */ #define SWING_UMAX 0.85f /* 起摆输出限幅，占空比归一化 */ #define ARM_KP 0.80f /* 臂回中比例 */ #define ARM_KD 0.06f /* 臂回中微分 */ #define LQR_K0 -1.20f /* 臂角 */ #define LQR_K1 -0.18f /* 臂角速度 */ #define LQR_K2 18.60f /* 摆角 */ #define LQR_K3 2.10f /* 摆角速度 */

参数说明上有个容易忽略的点：PEND_J不要照抄，细杆绕端点大约是m*L^2/3，如果你的摆杆末端加了配重块，这个值偏差能到一倍以上，直接影响能量项的收敛速度。LQR 的四个增益是从tools/lqr_design.m里算出来再粘过来的，不要手调，手调出来的组合大概率在某个角度上发散。

烧进去之后第一件事不是看它能不能站起来，而是打开tools/scope_plot.py看波形。摆角、臂角、控制量三条线同时打出来，起摆过程中摆角应该是幅度递增的振荡，臂角是围绕零位的小幅往复，控制量在正负限幅之间来回。三条线任何一条不符合这个形态，先别往下调增益，去查硬件。

3. 起摆算法拆解：能量法怎么把摆“喂”到顶部

3.1 能量模型与控制律：一个 sign 项决定电机往哪边推

起摆的本质是给摆杆注入能量。摆杆在正下方静止时能量最低，在正上方静止时能量最高，中间差的就是2mgL。能量法不去规划轨迹，只做一件事：每个控制周期算一次摆杆当前的总机械能，和目标能量比较，缺就补、多就等，补的方式是让旋转臂朝某个方向加速，靠臂端切向加速度把摆杆“带”上去。

/* swing_up.c —— 能量法起摆，1kHz 中断里调用 */ #include "params.h" #include <math.h> /* 摆杆机械能：以竖直向上为势能零点，theta 单位 rad */ static float pendulum_energy(float theta, float theta_dot) { float kinetic = 0.5f * PEND_J * theta_dot * theta_dot; float potential = PEND_M * G * PEND_L * (cosf(theta) - 1.0f); return kinetic + potential; /* 静止下垂时约等于 -2mgL */ } float swing_up_step(float alpha, float alpha_dot, float theta, float theta_dot) { float E = pendulum_energy(theta, theta_dot); float dE = E - E_TARGET; /* 目标能量，取 0 或略小于 0 */ /* 能量注入方向：只在摆杆有角速度且不处于竖直附近时才有效 */ float dir = sgnf(theta_dot * cosf(theta)); /* 能量项 + 臂回中项，回中项防止臂越转越快 */ float u = SWING_K * dE * dir - ARM_KP * alpha - ARM_KD * alpha_dot; return clampf(u, -SWING_UMAX, SWING_UMAX); }

逻辑说明：pendulum_energy里势能写成mgl(cosθ - 1)，是因为 θ 以竖直向上为零点，这样摆杆立在顶部时能量恰好为 0，控制目标就是把这个值推到 0 附近。乘cosf(theta)是关键——摆杆在水平位置附近时臂的加速度对摆杆做功效率最高，接近顶部或底部时力臂趋近于零，这个因子自动把输出压下去，避免在顶部附近瞎推。dir前面的符号取决于你的角度正方向和电机正方向是否一致，这一步必须实测确认，符号反了的表现是摆幅越甩越小，最后绕着某个角度打转。

参数说明：SWING_K决定能量注入的激进程度，太大直接把摆甩过头、撞限位，太小起摆要甩十几下才上去，一般从 0.2 开始往上加。ARM_KP和ARM_KD是臂的回中项，没有这两项，臂会随着起摆一路单向加速，几秒钟就到限位。回中项增益不能太大，否则它会把能量项压掉，起摆彻底没劲。

3.2 从控制量到 PWM：死区补偿和输出限幅

算出来的u是归一化到 [-1, 1] 的控制量，真正写进定时器比较寄存器之前还有一道工序。H 桥有死区，电机有静摩擦，占空比低于某个阈值时电机根本不转，只是在那儿嗡嗡响发烫。这个阈值不能查手册，必须实测。

/* motor_pwm.c —— 把 [-1,1] 的控制量变成 PWM 与方向 */ void motor_set(float u) { const float dead = 0.06f; /* 实测死区，不是手册值 */ float duty = fabsf(u); if (duty < dead) { /* 死区内直接断电，避免堵转发热 */ pwm_set(0); dir_set(0); return; } dir_set(u > 0.0f ? 1 : 0); /* 把 (dead,1] 线性映射到 (0,1]，保证低速段也有分辨率 */ pwm_set((uint16_t)((duty - dead) / (1.0f - dead) * PWM_FULL)); }

逻辑说明：如果只是简单地把|u| * PWM_FULL写进去，那么 0 到 6% 这一段全是无效输出，起摆初期能量项很小的那些周期电机毫无反应，表现出来就是“启动迟钝、第三下才甩起来”。做了死区补偿之后，小控制量也能推动摆杆，起摆过程明显更平顺。

参数说明：dead的测法是从 0 开始每 50ms 加 1% 占空比，记录电机轴开始转动的那一刻，取那个值再往上留 1% 余量。注意这个值随电池电压变化，电压低的时候死区会变大，所以低电量状态下起摆失败率高，不是算法的问题。

3.3 三个必须现场标定的数

SWING_K、SWING_UMAX、E_TARGET这三个数决定了起摆是干脆利落还是磨磨蹭蹭。

SWING_UMAX先从 0.5 开始，能起摆就行。限幅拉满会有一个隐患：摆杆快到位时能量项还很大，电机来不及收，直接甩过顶把摆杆拍到另一边，表现就是“每次都在顶部附近翻车”。E_TARGET一般取 0 或者 -0.1 左右的小负值，取 0 意味着要求摆杆以零速停在正上方，实际不可达，能量项永远有残差，输出会一直顶在限幅上，取略小一点反而更容易平稳进入切换窗口。

标定顺序建议是：先固定ARM_KP/ARM_KD让臂能回中，再把SWING_K从 0.2 往上加到“三到五下能起”，最后调E_TARGET让摆杆在顶部附近的速度尽量小。顺序颠倒的话，你会在三个参数之间来回改，改到最后自己都忘了哪次是对的——这是血泪经验。

4. 稳摆与切换：LQR 增益计算、状态滤波和“接棒”时机

4.1 四状态线性化模型与 LQR 增益

稳摆用 LQR，状态量取四个：臂角 α、臂角速度 α̇、摆角 θ、摆角速度 θ̇。不用手推雅可比矩阵，容易在符号上翻车，直接在平衡点附近做数值线性化，仿真和求增益共用同一份非线性动力学函数，省得两套模型对不上。

% tools/lqr_design.m —— 在竖直向上平衡点附近数值线性化后求增益 x0 = [0; 0; 0; 0]; % [alpha; alpha_dot; theta; theta_dot] eps = 1e-6; n = 4; A = zeros(n); B = zeros(n, 1); for i = 1:n dx = zeros(n, 1); dx(i) = eps; % f(x, u) 是非线性动力学函数，和仿真用的是同一份 A(:, i) = (f(x0 + dx, 0) - f(x0 - dx, 0)) / (2 * eps); end B = (f(x0, eps) - f(x0, -eps)) / (2 * eps); Q = diag([800, 10, 1200, 15]); % 摆角权重最高，其次是臂角 R = 1; K = lqr(A, B, Q, R); % 1x4 增益，直接粘进 params.h

逻辑说明：中心差分的好处是把“符号到底该是正还是负”这件事交给了代码，你只要保证f本身是对的。Q的对角线调法有个经验顺序：先加摆角权重，让摆杆立得住；再加臂角权重，把臂拉回中间，否则摆杆立住了但臂一直在慢慢漂，几十秒后撞限位。R加大意味着省力，电机声音会变小，但抗扰能力下降，台子轻微一晃就倒。

参数说明：Q里摆角那一项从 500 调到 2000，最直观的变化是顶部抖动幅度变小、但电机电流声变尖。如果调完发现电机一直在高频颤，那不是增益不够，是速度估计噪声太大，回到 4.3 去看滤波。

4.2 起摆到稳摆的交接：一次切换失败能让你调一整天

整套代码里最容易翻车的不是起摆也不是 LQR，是中间那一瞬间的交接。切早了摆杆还没到能量窗口，LQR 拉不回来；切晚了摆杆已经过了顶开始下落，LQR 只能眼睁睁看着。判据要同时看角度、角速度、臂速三个量，再加连续计数防抖。

/* mode_switch.c —— 起摆与稳摆的切换判据 */ #define SW_THETA 0.22f /* rad，约 12.6 度 */ #define SW_THETA_DOT 2.0f /* rad/s */ #define SW_ARM_DOT 10.0f /* rad/s，臂转太快时不切 */ #define SW_CNT 20 /* 连续 20ms 满足才切，约 1 个摆动周期 */ static int cnt = 0; void mode_update(float alpha_dot, float theta, float theta_dot) { if (mode == MODE_SWING) { int ok = (fabsf(theta) < SW_THETA) && (fabsf(theta_dot) < SW_THETA_DOT) && (fabsf(alpha_dot) < SW_ARM_DOT); cnt = ok ? cnt + 1 : 0; if (cnt >= SW_CNT) { mode = MODE_BALANCE; cnt = 0; } } else { /* 回差：掉出两倍阈值才退回起摆，防止边界反复横跳 */ if (fabsf(theta) > 2.0f * SW_THETA) { mode = MODE_SWING; cnt = 0; } } }

逻辑说明：三个条件里SW_ARM_DOT最容易被忽略。臂在起摆末期往往还在高速旋转，这时候即使摆角进了窗口，切过去 LQR 第一拍输出会把臂硬拽住，反作用力矩直接把摆杆甩飞。加了臂速约束之后，切换会晚半个周期，但成功率高得多。

回差是第二个必须有的东西。没有回差，摆杆在阈值附近来回穿越，模式每几十毫秒翻一次，电机会发出明显的“哒哒”声，几十秒内驱动器必过流。

参数说明：SW_THETA从 15° 往 10° 收，收得太紧会导致起摆永远进不了窗口（能量项精度不够），太松则是切过去接不住。SW_CNT的作用是确认摆杆真的在减速通过，而不是高速掠过，20 个周期在 1kHz 下是 20ms，正好盖住顶部附近的一小段。

4.3 滤波与测速：差分放大噪声的血泪经验

摆角速度 θ̇ 只能靠差分得到。12 位编码器在 1kHz 下差分，量化噪声被放大 1000 倍，直接喂给 LQR 的结果是电机持续高频颤振。必须滤波，但滤波带来的相位滞后又会让 LQR 的阻尼效果打折，这是一对矛盾。

/* filter.c —— 一阶低通，截止频率由系数决定 */ typedef struct { float y; float a; } lpf_t; static inline float lpf_step(lpf_t *f, float x) { f->y += f->a * (x - f->y); /* a = 1 - exp(-2*pi*fc*Ts) */ return f->y; } /* 初始化：Ts = 1ms 时，a=0.2 对应截止频率约 32Hz */ lpf_t theta_filt = { .y = 0.0f, .a = 0.20f };

逻辑说明：截止频率的选取要跟 LQR 的闭环带宽匹配。摆杆的摆动频率通常在 1~3Hz，把截止频率放在 30Hz 左右，既能把差分噪声压下去，相位滞后在 3Hz 处也只有几度，LQR 完全吃得下。有人图省事直接把系数调到 0.02，截止频率掉到 3Hz 以下，结果就是摆杆在顶部慢慢晃、越晃越大，这不是增益不够，是滤波器把有效信号也吃掉了。

参数说明：α 和 θ 的位置信号本身干净，不用滤波；α̇ 和 θ̇ 必须滤。另外，差分用的前后两帧角度要先做连续性处理，跨过 ±π 边界时那一帧的差分会突然变成2π量级，滤波器来不及压，表现为顶部偶尔抽一下——这个坑在第 5 章还会再提一次。

5. 避坑与常见问题排查：起摆翻车最多的五个地方

5.1 机械与电气层面的三个坑

坑一：上电后电机只响不转，手推一下才动。

现象是 PWM 波形正常、控制量也有输出，但摆杆纹丝不动，十来秒后电机外壳发烫。原因通常是两个：一是占空比落在 H 桥死区加静摩擦的范围内，实际出力为零；二是驱动器的使能脚没有在初始化里显式拉高，或者拉高后没留建立时间。解决办法是把死区实测出来写进motor_set，同时在初始化末尾加 10ms 延时再使能定时器输出，别让第一拍 PWM 撞在驱动器还没准备好的时候。

坑二：起摆到第三下主控复位，串口断流。

现象很吓人，像是程序跑飞，其实是母线电压跌落触发了低压复位。加速瞬间电流能冲到几安，电源内阻加上长导线的压降，母线从 12V 掉到 7V 以下，逻辑电源跟着塌。解决办法是母线并 470μF 以上的电解电容加 0.1μF 陶瓷电容，电源线加粗缩短，逻辑供电和功率供电分成两路；代码上再补一道，限制控制量的变化率，别让占空比在一两个周期内从 0 跳到 85%。

坑三：起摆越甩越低，最后绕着某个角度打转。

这是符号问题，也是最常见的一个。摆角或臂角的计数方向与电机正方向不一致，导致能量项的符号整体反了，控制器在“抽走”能量而不是注入。解决的办法是写一个方向自检流程：手动让电机正向转 30 度，打印 α 的增量；手推摆杆朝正方向偏 30 度，打印 θ 的增量，两个增量都为正才继续往下走。这一步花五分钟，能省掉半天。

5.2 控制与代码层面的两个坑

坑四：快立起来时角度突然跳变，LQR 输出瞬间打满。

现象是摆杆接近顶部时电机猛地抽一下，然后整台架子震一下。原因是绝对值编码器在 0/180 度边界回绕，某一帧的角度从 3.13 跳到 -3.13，差分得到的速度是 -6000 rad/s，LQR 直接饱和。解决办法是上电时把摆角零位标在竖直向下，控制里统一做角度连续化，差分之前先判跳变：两帧差值绝对值超过 π 就认为跨了边界，把当前帧加上或减去 2π 再算。

坑五：切换后 200ms 内剧烈振荡然后倒下。

切过去就抖，说明两件事之一：切换阈值太松，摆杆带着明显的角速度进了窗口；或者速度估计的相位滞后太大，LQR 的阻尼项实际上在帮倒忙。先把SW_THETA从 15° 收到 10~12°，SW_THETA_DOT收到 1.5 rad/s 以内，看是否改善；没改善就去查滤波系数，把截止频率从 10Hz 提到 30Hz 再试。还有一个隐蔽原因：切换瞬间两个控制器的输出不连续，起摆末期u可能还在 0.6，LQR 第一拍算出来是 -0.2，这个阶跃会激起机械结构的弹性振动。加一段 20ms 的线性过渡就能压下去。

提示：调参期间把串口波形一直开着，出问题时先看三条线的形态再动手改参数。凭感觉改是这类项目最大的时间黑洞。

6. 二次开发：换执行机构、加轨迹规划、接上位机

这套倒立摆源码的结构留了改造余量，几个常见的二次开发方向改动量都不大，但每个都有对应的注意点。

最省事的是接上位机。现在工程里发的是 ASCII 行，一行三个浮点，1kHz 下串口根本扛不住，实际是降频到 100Hz 发的。改成二进制帧之后，帧头两个字节、三路数据各四字节、一字节校验，一帧 16 字节，115200 波特率下能跑到 500Hz 以上，起摆过程中那些几十毫秒的细节才看得见。改的时候记住一件事：打包和发送放在主循环，中断里只往环形缓冲区写指针，一旦在中断里等串口发送完成，控制周期立刻就不稳了。

换成直线倒立摆的改动最大，但思路是通的：把臂角的动力学换成小车的平动方程，摆杆部分几乎不用动，能量法的形式也完全一致，只是注入能量的方式从“臂的切向加速度”变成“小车的水平加速度”。真正的坑在摩擦项，直线导轨的静摩擦远比旋转铰链大，起摆时会有一段“推不动”的死区，能量项要在那里额外加一个补偿，否则摆杆永远差一口气上不去。

加轨迹规划是另一个有意思的方向。纯能量法起摆的摆动幅度不可控，摆杆在中途可能甩得很高撞到限位。加一层幅度约束，让臂按固定频率做正弦往复、频率贴着摆杆的固有频率扫，靠共振把摆杆“摇”上去，过程会温和很多，代价是起摆时间长一倍。

最后说个习惯。从那以后我每次改完控制参数，都强制先在仿真里把同一组参数跑一遍，看着状态量收敛了再上电，哪怕只改了一个增益。因为实机上你只有一次机会看到失败瞬间的波形，而仿真里可以倒回去看一百遍。这套源码里tools/那两个脚本就是为这个准备的，别嫌麻烦跳过。希望帮到你。

本文还有配套的精品资源，点击获取