简介旋转倒立摆常规起摆源码包是一份面向控制理论与嵌入式初学者的完整工程实例基于经典旋转倒立摆装置实现常规起摆与平衡控制重点演示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°。机械限位必须作为最后一道保险。程序里一定要加软限位当摆臂角度超过设定阈值时控制量直接置零并报警避免摆臂撞坏或电机过流。这一行代码比任何控制算法都值得优先加上。写在最后的一点体会旋转倒立摆这个项目网上框架代码一抓一大把但真正能跑稳定、不出事故的版本往往不是算法最花哨的而是把方向定义、限位保护、滤波和切换时机这些“脏活”做到位的版本。这份常规起摆源码的价值也正在于此——它给你一条被反复验证过的主路径剩下的细节调试才是真正长本事的地方。能把起摆调顺再去碰自平衡机器人或者其他欠驱动系统心里就有底了。本文还有配套的精品资源点击获取 旋转倒立摆能量法起摆源码解析：调参技巧与避坑指南
简介旋转倒立摆常规起摆源码包是一份面向控制理论与嵌入式初学者的完整工程实例基于经典旋转倒立摆装置实现常规起摆与平衡控制重点演示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°。机械限位必须作为最后一道保险。程序里一定要加软限位当摆臂角度超过设定阈值时控制量直接置零并报警避免摆臂撞坏或电机过流。这一行代码比任何控制算法都值得优先加上。写在最后的一点体会旋转倒立摆这个项目网上框架代码一抓一大把但真正能跑稳定、不出事故的版本往往不是算法最花哨的而是把方向定义、限位保护、滤波和切换时机这些“脏活”做到位的版本。这份常规起摆源码的价值也正在于此——它给你一条被反复验证过的主路径剩下的细节调试才是真正长本事的地方。能把起摆调顺再去碰自平衡机器人或者其他欠驱动系统心里就有底了。本文还有配套的精品资源点击获取