尧图网站建设 
简介面向电子设计竞赛的倒立摆3.0完整程序包基于STM32F10x标准外设库与Keil MDK工程开发适合电赛参赛队伍、自动化控制方向学生以及倒立摆平衡算法学习者参考使用。压缩包共一百八十个文件整体4.93MB以三十九个C源码和四十一个头文件为主体并配有uvprojx/uvoptx工程配置、axf/hex烧录文件、map映射文件以及调试日志等打开工程即可编译下载二次开发路径清晰。目前已有四百八十二人学习浏览是电赛倒立摆方向较受关注的资料。源码覆盖PWM电机驱动、定时器捕获、ADC传感器采集、I2C/CAN通信等常用外设模块同时包含PID调节、倾角数据处理与系统调试经验便于对照学习倒立摆的建模与控制流程封装良好的代码结构也为后续移植到平衡小车、云台稳定等类似控制场景提供了便利。1. 倒立摆电赛选题为什么 3.0 版本还在卷这个题目倒立摆几乎是电赛控制类题目里出镜率最高的被控对象。从 2015 年的多种波形发生器到近几届的 E 题、H 题控制类赛题反复在“稳、快、准”三个字上做文章而直线一级倒立摆恰恰把这三点全部压缩进了一个不到半米长的滑轨里。所谓 3.0 版本通常是参赛队在同一套机械结构上迭代到第三轮的程序架构第一版能站稳第二版能抗扰动第三版要在规定时间内完成摆起、稳摆、定点移动和抗风干扰。这个标题真正想问的是——倒立摆程序怎么写才能既满足电赛评分点又能在调试现场快速收敛。这篇文章的思路是:先把倒立摆写进程序之前必须建立的数学模型讲清楚,再给出一套从 MATLAB 仿真到 STM32 实物的代码迁移路径,然后落到电赛现场最关键的调参顺序和故障判读,最后聊几个能帮你从“能站”跨到“站得好”的进阶玩法。新人可以照着代码一步步搭,有经验的人可以直接跳到第 4 章对照自己踩过的坑。2. 把倒立摆写进程序前先建好状态空间模型倒立摆程序写不好九成原因不是代码能力而是脑子里没有状态方程。PID 参数调不出来、摆动发散、稳态抖动这些表面现象的背后都是模型参数和实际机械结构的偏差。所以第一步不是打开 Keil而是先在 MATLAB 里把对象的行为描述准确。2.1 直线一级倒立摆的运动方程与状态空间表达常见的倒立摆电赛装置由滑轨、小车、摆杆和直流电机构成。小车在导轨上水平运动通过皮带或齿轮传动摆杆铰接在小车上。建模时通常忽略摩擦力、皮带弹性把系统抽象成一个刚体系统。取摆杆与竖直方向的夹角为 θ小车位移为 x作用在小车上的水平控制力为 F。先用牛顿第二定律分别对小车和摆杆列写方程再联立消去摆杆与小车之间的相互作用力得到非线性微分方程组( M m ) x m l θ cos θ - m l θ² sin θ Fm l x cos θ ( J m l² ) θ - m g l sin θ 0其中 M 是小车质量m 是摆杆质量l 是摆杆质心到铰点的距离J 是摆杆绕质心的转动惯量g 取 9.8。这组方程是所有后续工作的起点。在电赛现场直接用非线性模型做控制不太现实常见做法是在 θ 0 附近做线性化即认为 sin θ ≈ θcos θ ≈ 1并忽略 θ² 项。线性化之后定义状态变量 X [ x, x, θ, θ ]控制量为力 F实际程序中会转换为电机 PWM 占空比得到标准状态空间方程 X A X B u。这里 A 矩阵和 B 矩阵的具体数值取决于你手头机械结构的物理参数所以拿到赛题第一件事不是写代码而是称重、量长度。2.1.1 状态空间方程在程序里的存在形式在代码里状态空间模型通常不会以矩阵形式出现——STM32 上不跑矩阵运算也没有问题。它的作用体现在两个地方一是在 MATLAB 里做仿真验证控制器可行性二是用来计算 LQR 反馈增益 K。当你决定用 LQR 而非 PID 时才需要把 A、B 矩阵写进 MATLAB 脚本。M 1.096; % 小车质量 kg m 0.109; % 摆杆质量 kg l 0.25; % 摆杆质心到转轴距离 m J 0.0034; % 摆杆转动惯量 kg*m^2 g 9.8; A [0 1 0 0; 0 0 -m*g/M 0; 0 0 0 1; 0 0 (Mm)*g/(M*l) 0]; B [0; 1/M; 0; -1/(M*l)]; C eye(4); D zeros(4,1); sys ss(A,B,C,D); rank(ctrb(A,B)) % 能控性判据结果为4说明系统能控这段代码里的 A、B 矩阵是从线性化方程直接整理得到的。第 11 行的ctrb用于计算能控性矩阵的秩倒立摆系统在竖直位置附近理论上完全能控所以秩应为 4。如果在你的参数下秩不满说明机械结构有问题比如摆杆铰点摩擦力过大导致模型失效。2.2 用 MATLAB 仿真先验证 LQR 控制器参数LQR 的设计目标是通过最小化二次型性能指标 J ∫( x Q x u R u ) dt 来求反馈增益 K。Q 矩阵和 R 矩阵的选取是调参的核心。在仿真阶段不需要调得太精细只要能在 2 秒内稳定即可真实参数留给实物调。Q diag([100, 10, 200, 20]); % 对位移、速度、角度、角速度的惩罚 R 1; K lqr(A, B, Q, R); % 闭环仿真加入初始角度偏差5度 t 0:0.001:3; x0 [0; 0; 5*pi/180; 0]; sys_cl ss(A-B*K, B, C, D, 0.001); [y, t] lsim(sys_cl, zeros(size(t)), t, x0); plot(t, y(:,3)*180/pi); % 观察角度响应Q 矩阵里的数值含义很直接Q(3,3) 是角度的权重越大代表越不允许摆杆偏离竖直位置Q(1,1) 是位移权重越大代表小车越不愿意跑远。倒立摆能站稳的底线是角度环权重远大于位移环但也不能大得离谱否则控制力饱和小车高速撞向限位开关。仿真阶段建议重点观察三个指标超调量、调节时间、稳态误差。如果角度响应在 1 秒内回到零且有明显阻尼说明 K 值方向正确。把这组 K 值记录下来后面移植到 STM32 时作为初始值但要有心理准备——实物上的极限增益通常只有仿真值的 50% 到 70%原因是执行机构饱和、延迟和摩擦力。3. 从仿真到实物STM32 上的倒立摆程序骨架仿真跑通只能说明数学模型没问题实物程序才是电赛真正的分水岭。控制周期、传感器融合、执行机构限幅每一个环节都可能让理论模型失效。这一章给出一个可运行的 STM32 程序骨架覆盖从 MPU6050 读取到 PWM 输出的完整链路。3.1 控制周期与传感器读取路径倒立摆对控制周期很敏感。常见做法是把控制中断设为 1ms 到 5ms推荐从 2ms 起步。周期太长会让摆杆在两次控制之间跑出线性化范围周期太短则传感器噪声被放大微分项抖动剧烈。// 定时器中断服务函数2ms 一次 void TIM3_IRQHandler(void) { if (TIM_GetITStatus(TIM3, TIM_IT_Update) ! RESET) { TIM_ClearITPendingBit(TIM3, TIM_IT_Update); // 读取 MPU6050 原始数据 MPU6050_ReadAccel(ax, ay, az); MPU6050_ReadGyro(gx, gy, gz); // 姿态解算得到摆杆角度 float angle GetPendulumAngle(ax, az, gx, gz); float enc_pos ReadEncoderPos(); // 读取小车位置 float enc_vel GetEncoderSpeed(); // 由编码器差分得到速度 // 串级PID计算 float output CascadePID(angle, enc_pos, enc_vel); // 限幅并输出PWM output Constrain(output, -PWM_MAX, PWM_MAX); SetMotorPWM(output); } }这段中断函数的关键在最后三行限幅是必须的因为 PID 输出一旦超过 PWM 最大值执行器饱和会让积分项一路累积最后出现积分饱和现象。常见处理方式是积分限幅和输出限幅同时做。另外MPU6050 的读取放在中断里要注意 I2C 通信时间如果 I2C 时钟配置过慢2ms 中断会被占掉一大半此时应使用 DMA 或硬件 I2C。3.1.1 摆杆角度解算互补滤波还是 Mahony倒立摆对角度实时性要求高不建议直接使用 DMP 输出四元数——DMP 内部有较长的数据融合周期会产生 5ms 到 10ms 的延迟。常见做法是加速度计计算静态角度陀螺仪积分动态角度再用互补滤波融合。float angle 0.98f * (angle gyro_y * dt) 0.02f * acc_angle;这个式子是互补滤波最简形式。0.98 是陀螺仪权重0.02 是加速度计权重。加速度计长期稳定但短期噪声大陀螺仪短期精准但长期漂移互补滤波就是把两者的频域优势结合起来。dt 是控制周期秒gyro_y 是摆杆旋转轴方向的角速度。需要特别注意的是加速度计算角度时用哪个轴。倒立摆摆杆转轴如果平行于 MPU6050 的 X 轴则角度由atan2(ax, az)得到而不是 atan2(ay, az)。接错轴的表现很有意思程序跑起来摆杆在竖直位置疯狂抖稍微偏移就往一个方向加速倒下这时先检查角度计算是否反了。3.2 串级 PID角度环内环、位置环外环单环 PID 只能让摆杆在初始位置附近稳定无法让小车回到原点。电赛评分中“自动回到中点并保持”是很常见的加分项所以程序必须采用串级结构。typedef struct { float kp, ki, kd; float integral; float last_error; } PID_t; float CascadePID(float angle, float pos, float vel) { // 外环位置环输出目标角度偏置 float pos_err 0.0f - pos; float angle_target pos_pid.kp * pos_err pos_pid.kd * (0.0f - vel); angle_target Constrain(angle_target, -MAX_TILT, MAX_TILT); // 内环角度环输出控制力 float angle_err angle_target - angle; angle_pid.integral angle_err * dt; angle_pid.integral Constrain(angle_pid.integral, -I_LIMIT, I_LIMIT); float output angle_pid.kp * angle_err angle_pid.kd * (angle_err - angle_pid.last_error) / dt angle_pid.integral * angle_pid.ki; angle_pid.last_error angle_err; return output; }这段代码体现了倒立摆控制的核心思想位置环并不直接输出力而是输出一个目标倾角——比如小车偏左就让摆杆向右倾斜一个角度靠重力分量牵引小车回中点。这比直接用位置误差去调电机力要自然得多因为倒立摆唯一能控制的是加速度而加速度来自摆杆倾斜产生的水平分力。MAX_TILT通常在 5 度到 10 度之间。如果目标倾角限制太小位置环恢复能力弱限制太大摆杆可能超出线性化范围导致失稳。从 8 度开始调是比较稳妥的起点。3.3 电机驱动与 PWM 限幅策略电赛常见驱动方案是 TB6612 或 DRV8701配合带霍尔编码器的直流减速电机。PWM 频率建议 10kHz 到 20kHz低于 5kHz 电机会有明显的啸叫高于 50kHz 驱动芯片的开关损耗变大。#define PWM_MAX 7200 // 定时器ARR值对应100%占空比 #define PWM_LIMIT 5800 // 限幅到80%防止齿轮冲击 void SetMotorPWM(float output) { float duty output; if (duty PWM_LIMIT) duty PWM_LIMIT; if (duty -PWM_LIMIT) duty -PWM_LIMIT; if (duty 0) { __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, duty); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_2, 0); } else { __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, 0); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_2, -duty); } }限幅到 80% 看起来是浪费动力实际是保护机械结构。倒立摆的电机在极限情况下会瞬间反转全占空比输出会给齿轮和皮带带来很大的冲击应力。竞赛现场更常见的问题是电机线因为反复弯折导致接触不良程序表现为摆杆突然失去控制力直接倒下——排除故障时先检查接线再怀疑代码。4. 倒立摆电赛调参路径三个必调参数与波形判读仿真和程序骨架都到位之后真正花时间的是现场调参。倒立摆的控制器常见有 3 到 4 个待调项角度环 Kp、角度环 Kd、位置环 Kp、位置环 Kd外加角度环 Ki。调参的顺序错了会陷入“调了一天还是站不住”的泥潭。这一章给出经过验证的调参顺序和每步的判据。4.1 角度环 Kp 与 Kd先站稳再说第一步只调角度环把位置环完全禁用目标倾角强制为 0。从一个小 Kp 开始比如 100Kd 设为 20Ki 设 0。观察摆杆是否出现高频抖动。判断标准如下表现象原因处理摆杆缓慢倒下无恢复力Kp 过小按 1.5 倍增大 Kp摆杆在竖直位置高频抖动Kp 过大或 Kd 过小先增大 Kd再微调 Kp摆杆向一侧加速倒下角度符号反了或控制方向反检查电机正反转和传感器方向摆杆可以站稳但来回慢摆Kd 不足增大 Kd每次增加 20%这个阶段的目标是让摆杆用手轻推后能自行回到竖直位置且回正过程没有明显振荡。用串口把角度数据实时发到上位机采样率 100Hz 就够用。波形上理想的回正响应是衰减振荡超调量小于 30%在 1 到 2 秒内收敛。4.2 位置环 Kp 和 Kd让小车回到中点角度环站稳后依次放开位置环。位置环的调试比角度环温和Kp 从 0.5 起调每次增加 0.3。观察小车是否以摆杆竖直为前提缓慢移向中点。注意位置环的响应必须慢于角度环否则内外环时间尺度耦合会造成低频振荡——表现为小车来回大幅移动摆杆始终在竖直附近但整个系统呈周期性的“点头”状态。// 位置环参数建议范围 float pos_kp 1.2f; // 0.5 ~ 2.0 float pos_kd 0.8f; // 0.3 ~ 1.5 // 角度环参数建议范围 float angle_kp 600; // 仿真K值换算后的50%~70% float angle_kd 80; // 与kpd比例约1:8~1:12位置环的 Kd 不是必需的有些方案只靠位置 Kp 就能完成回中点但响应会慢。加了 Kd 后会引入速度反馈系统等效阻尼增大响应更快且不易过冲。速度值由编码器差分得到注意差分运算会放大噪声可以在计算速度时做一个 10 点滑动平均延迟约 10ms对控制影响很小。4.3 积分项的启用时机角度环 Ki 在所有环节之前都建议保持为 0。倒立摆是自不稳定的积分项带来的相位滞后会显著减小稳定裕度。只有在以下场景才考虑加 Ki小车停在倾斜导轨上时摆杆有一个恒定的重力偏置项导致角度环存在稳态误差。此时 Ki 从 0.5 起调配合积分限幅在最大值输出的 20% 以内。常见的错误是看到摆杆回不到精确竖直就加 Ki结果把系统调出了极限环振荡。先检查机械水平——用水平尺量一下导轨是否放平很多时候摆杆回不到竖直不是控制器问题而是导轨本身就倾斜了几毫米。这个检查只需要两分钟却能在现场避免一小时的无效调参。4.4 调参过程中的常见故障与快速定位倒立摆在调试中表现出的故障现象往往是多因一果的以下是电赛现场最高频的三类故障现象排查顺序关键检查点上电后摆杆直接倒下1. 角度值方向 2. 电机方向 3. PWM 输出用手轻推摆杆看 PWM 是否给力能站稳但抗扰太差1. 控制周期 2. 角度滤波延迟 3. 机械间隙摆杆用手弹一下观察恢复时间小车限位撞击1. 位置环增益 2. 目标倾角限幅 3. 编码器方向单独测位置环输出是否限幅第一类是最常见的摆杆倒下瞬间电机反而朝倒下方向加速。这是方向接反的经典表现。把电机线对调或者把角度误差取反二选一解决。第二类问题在搬动装置到赛场后尤其容易出现——赛场的桌面和新环境下摩擦力发生变化原来调好的 K 值不再适配就需要用下面的方法快速恢复。5. 倒立摆程序 3.0 的进阶玩法观测器、自适应与加分项前面的内容解决了“能站稳”的问题3.0 版本要解决的是“站得漂亮”的问题。电赛中评分差距往往在最后的附加题里拉开——抗风扰动、指定位置停车、摆起自动切换。这三个场景分别对应状态观测、参数在线整定和模式切换是值得在赛前投入时间的三个方向。5.1 用卡尔曼滤波替代互补滤波互补滤波的优势是轻量但在电机 PWM 占空比变化剧烈的加减速阶段加速度计会感受到额外的惯性力导致互补滤波中的加速度权重被污染。改进方案是卡尔曼滤波它把角度和陀螺仪零漂作为状态变量利用过程模型预测观测修正得到更干净的倾角估计。// 卡尔曼滤波一维模型状态[角度, 陀螺仪零漂] float angle 0, bias 0; float P[2][2] {{1,0},{0,1}}; float Q_angle 0.001f, Q_bias 0.003f, R_measure 0.03f; float KalmanFilter(float acc_angle, float gyro_rate, float dt) { // 预测 angle dt * (gyro_rate - bias); P[0][0] dt * (dt*P[1][1] - P[0][1] - P[1][0] Q_angle); P[0][1] - dt * P[1][1]; P[1][0] - dt * P[1][1]; P[1][1] Q_bias * dt; // 修正 float S P[0][0] R_measure; float K[2]; K[0] P[0][0] / S; K[1] P[1][0] / S; angle K[0] * (acc_angle - angle); bias K[1] * (acc_angle - angle); P[0][0] - K[0] * P[0][0]; P[0][1] - K[0] * P[0][1]; P[1][0] - K[1] * P[0][0]; P[1][1] - K[1] * P[0][1]; return angle; }卡尔曼滤波的参数调整比互补滤波直观很多Q_angle 是角度过程噪声方差调大表示更信任加速度计Q_bias 是零漂过程噪声方差调大表示认为零漂变化快R_measure 是测量噪声方差调大表示更信任陀螺仪。实测中 R_measure 对滤波效果的影响最大在 0.01 到 0.1 之间调。注意卡尔曼滤波的运算量比互补滤波大 5 倍以上放在中断里要量一下执行时间。5.2 同一套代码应对参数变化在线调整 LQR 增益电赛现场换桌子、换电池电压、甚至换一辆小车物理参数都会发生偏移。与其在现场重新调 PID不如在程序里内置一张 LQR 增益表按电池电压或者摆杆质量估算值自动切换。做法是在 MATLAB 离线算好几组 K 值烧录时按序号存入数组。// 按电压范围自动选择K值 float K_table[3][4] { { 8.2f, 12.1f, 45.3f, 6.4f }, // 高电压如12.0V以上 { 7.5f, 11.0f, 42.8f, 5.9f }, // 中电压 { 6.8f, 10.2f, 39.5f, 5.3f } // 低电压 }; float battery_voltage GetBatteryVoltage(); int idx (battery_voltage 11.5f) ? 0 : (battery_voltage 10.5f) ? 1 : 2; SetLQRGain(K_table[idx]);这个技巧的实用性在于电赛现场电池电压从满电 12.6V 掉到欠压 10V 只需要连续运行不到一小时电机力矩下降会让同一组 K 值从“稳定”变成“临界振荡”。按电压分段切换是最低成本的鲁棒化手段。如果时间充裕可以用递归最小二乘在线辨识系统参数然后通过查表实时更新 K 值这种方案更接近真正的自适应控制但本质是同样的思路——把离线算好的几种工况映射到当前状态。5.3 加分项摆起控制与二次开发倒立摆的初始状态是垂直向下要让它在电赛现场自动完成“上摆稳摆”的姿态切换程序里需要加入能量控制策略。能量法是最常见的方案计算当前摆杆的能量与竖直位置能量之差用一个比例控制器输出来回鞭打让摆杆逐步获得能量并接近倒立位置。float EnergyControl(float angle, float omega) { float E 0.5f * J * omega * omega m * g * l * (cos(angle) - 1.0f); float E_target m * g * l * (1.0f - 1.0f); // 竖直位置能量为0 float output k_e * (E - E_target) * sign(omega * cos(angle)); return output; }当摆杆角速度方向与目标方向匹配时这个控制器会输出与运动方向同向的力给系统注入能量。当摆杆越过竖直位置或者角度进入 ±5 度范围内时切换到 LQR/PID 稳摆模式。这个切换不能直接跳变——需要让输出在几十毫秒内从能量控制器的值过渡到稳摆控制器的值否则切换瞬间会给摆杆一个很大的冲击扰动直接打翻系统。常见做法是加权过渡float mix SmoothStep(angle_range); // 角度越小稳摆占比越高 output (1 - mix) * energy_output mix * balance_output;最后回到 3.0 版本的定位倒立摆程序不是写完就结束的静态工程它是你在电赛控制类赛场上最值得打磨的磨刀石。同一套代码结构里装着的状态方程、传感器融合和反馈控制推到比赛结束它依然可以继续用在后来的智能车、双轮自平衡和无人机悬停项目里。站得稳只是开始——能在 3 分钟调试窗口内完成参数迁移、在未知台面上跑出稳定性能那才是这套倒立摆程序真正值钱的 3.0 版本所要验证的能力。本文还有配套的精品资源点击获取 倒立摆电赛3.0：从状态空间模型到STM32串级PID调参实战
简介面向电子设计竞赛的倒立摆3.0完整程序包基于STM32F10x标准外设库与Keil MDK工程开发适合电赛参赛队伍、自动化控制方向学生以及倒立摆平衡算法学习者参考使用。压缩包共一百八十个文件整体4.93MB以三十九个C源码和四十一个头文件为主体并配有uvprojx/uvoptx工程配置、axf/hex烧录文件、map映射文件以及调试日志等打开工程即可编译下载二次开发路径清晰。目前已有四百八十二人学习浏览是电赛倒立摆方向较受关注的资料。源码覆盖PWM电机驱动、定时器捕获、ADC传感器采集、I2C/CAN通信等常用外设模块同时包含PID调节、倾角数据处理与系统调试经验便于对照学习倒立摆的建模与控制流程封装良好的代码结构也为后续移植到平衡小车、云台稳定等类似控制场景提供了便利。1. 倒立摆电赛选题为什么 3.0 版本还在卷这个题目倒立摆几乎是电赛控制类题目里出镜率最高的被控对象。从 2015 年的多种波形发生器到近几届的 E 题、H 题控制类赛题反复在“稳、快、准”三个字上做文章而直线一级倒立摆恰恰把这三点全部压缩进了一个不到半米长的滑轨里。所谓 3.0 版本通常是参赛队在同一套机械结构上迭代到第三轮的程序架构第一版能站稳第二版能抗扰动第三版要在规定时间内完成摆起、稳摆、定点移动和抗风干扰。这个标题真正想问的是——倒立摆程序怎么写才能既满足电赛评分点又能在调试现场快速收敛。这篇文章的思路是:先把倒立摆写进程序之前必须建立的数学模型讲清楚,再给出一套从 MATLAB 仿真到 STM32 实物的代码迁移路径,然后落到电赛现场最关键的调参顺序和故障判读,最后聊几个能帮你从“能站”跨到“站得好”的进阶玩法。新人可以照着代码一步步搭,有经验的人可以直接跳到第 4 章对照自己踩过的坑。2. 把倒立摆写进程序前先建好状态空间模型倒立摆程序写不好九成原因不是代码能力而是脑子里没有状态方程。PID 参数调不出来、摆动发散、稳态抖动这些表面现象的背后都是模型参数和实际机械结构的偏差。所以第一步不是打开 Keil而是先在 MATLAB 里把对象的行为描述准确。2.1 直线一级倒立摆的运动方程与状态空间表达常见的倒立摆电赛装置由滑轨、小车、摆杆和直流电机构成。小车在导轨上水平运动通过皮带或齿轮传动摆杆铰接在小车上。建模时通常忽略摩擦力、皮带弹性把系统抽象成一个刚体系统。取摆杆与竖直方向的夹角为 θ小车位移为 x作用在小车上的水平控制力为 F。先用牛顿第二定律分别对小车和摆杆列写方程再联立消去摆杆与小车之间的相互作用力得到非线性微分方程组( M m ) x m l θ cos θ - m l θ² sin θ Fm l x cos θ ( J m l² ) θ - m g l sin θ 0其中 M 是小车质量m 是摆杆质量l 是摆杆质心到铰点的距离J 是摆杆绕质心的转动惯量g 取 9.8。这组方程是所有后续工作的起点。在电赛现场直接用非线性模型做控制不太现实常见做法是在 θ 0 附近做线性化即认为 sin θ ≈ θcos θ ≈ 1并忽略 θ² 项。线性化之后定义状态变量 X [ x, x, θ, θ ]控制量为力 F实际程序中会转换为电机 PWM 占空比得到标准状态空间方程 X A X B u。这里 A 矩阵和 B 矩阵的具体数值取决于你手头机械结构的物理参数所以拿到赛题第一件事不是写代码而是称重、量长度。2.1.1 状态空间方程在程序里的存在形式在代码里状态空间模型通常不会以矩阵形式出现——STM32 上不跑矩阵运算也没有问题。它的作用体现在两个地方一是在 MATLAB 里做仿真验证控制器可行性二是用来计算 LQR 反馈增益 K。当你决定用 LQR 而非 PID 时才需要把 A、B 矩阵写进 MATLAB 脚本。M 1.096; % 小车质量 kg m 0.109; % 摆杆质量 kg l 0.25; % 摆杆质心到转轴距离 m J 0.0034; % 摆杆转动惯量 kg*m^2 g 9.8; A [0 1 0 0; 0 0 -m*g/M 0; 0 0 0 1; 0 0 (Mm)*g/(M*l) 0]; B [0; 1/M; 0; -1/(M*l)]; C eye(4); D zeros(4,1); sys ss(A,B,C,D); rank(ctrb(A,B)) % 能控性判据结果为4说明系统能控这段代码里的 A、B 矩阵是从线性化方程直接整理得到的。第 11 行的ctrb用于计算能控性矩阵的秩倒立摆系统在竖直位置附近理论上完全能控所以秩应为 4。如果在你的参数下秩不满说明机械结构有问题比如摆杆铰点摩擦力过大导致模型失效。2.2 用 MATLAB 仿真先验证 LQR 控制器参数LQR 的设计目标是通过最小化二次型性能指标 J ∫( x Q x u R u ) dt 来求反馈增益 K。Q 矩阵和 R 矩阵的选取是调参的核心。在仿真阶段不需要调得太精细只要能在 2 秒内稳定即可真实参数留给实物调。Q diag([100, 10, 200, 20]); % 对位移、速度、角度、角速度的惩罚 R 1; K lqr(A, B, Q, R); % 闭环仿真加入初始角度偏差5度 t 0:0.001:3; x0 [0; 0; 5*pi/180; 0]; sys_cl ss(A-B*K, B, C, D, 0.001); [y, t] lsim(sys_cl, zeros(size(t)), t, x0); plot(t, y(:,3)*180/pi); % 观察角度响应Q 矩阵里的数值含义很直接Q(3,3) 是角度的权重越大代表越不允许摆杆偏离竖直位置Q(1,1) 是位移权重越大代表小车越不愿意跑远。倒立摆能站稳的底线是角度环权重远大于位移环但也不能大得离谱否则控制力饱和小车高速撞向限位开关。仿真阶段建议重点观察三个指标超调量、调节时间、稳态误差。如果角度响应在 1 秒内回到零且有明显阻尼说明 K 值方向正确。把这组 K 值记录下来后面移植到 STM32 时作为初始值但要有心理准备——实物上的极限增益通常只有仿真值的 50% 到 70%原因是执行机构饱和、延迟和摩擦力。3. 从仿真到实物STM32 上的倒立摆程序骨架仿真跑通只能说明数学模型没问题实物程序才是电赛真正的分水岭。控制周期、传感器融合、执行机构限幅每一个环节都可能让理论模型失效。这一章给出一个可运行的 STM32 程序骨架覆盖从 MPU6050 读取到 PWM 输出的完整链路。3.1 控制周期与传感器读取路径倒立摆对控制周期很敏感。常见做法是把控制中断设为 1ms 到 5ms推荐从 2ms 起步。周期太长会让摆杆在两次控制之间跑出线性化范围周期太短则传感器噪声被放大微分项抖动剧烈。// 定时器中断服务函数2ms 一次 void TIM3_IRQHandler(void) { if (TIM_GetITStatus(TIM3, TIM_IT_Update) ! RESET) { TIM_ClearITPendingBit(TIM3, TIM_IT_Update); // 读取 MPU6050 原始数据 MPU6050_ReadAccel(ax, ay, az); MPU6050_ReadGyro(gx, gy, gz); // 姿态解算得到摆杆角度 float angle GetPendulumAngle(ax, az, gx, gz); float enc_pos ReadEncoderPos(); // 读取小车位置 float enc_vel GetEncoderSpeed(); // 由编码器差分得到速度 // 串级PID计算 float output CascadePID(angle, enc_pos, enc_vel); // 限幅并输出PWM output Constrain(output, -PWM_MAX, PWM_MAX); SetMotorPWM(output); } }这段中断函数的关键在最后三行限幅是必须的因为 PID 输出一旦超过 PWM 最大值执行器饱和会让积分项一路累积最后出现积分饱和现象。常见处理方式是积分限幅和输出限幅同时做。另外MPU6050 的读取放在中断里要注意 I2C 通信时间如果 I2C 时钟配置过慢2ms 中断会被占掉一大半此时应使用 DMA 或硬件 I2C。3.1.1 摆杆角度解算互补滤波还是 Mahony倒立摆对角度实时性要求高不建议直接使用 DMP 输出四元数——DMP 内部有较长的数据融合周期会产生 5ms 到 10ms 的延迟。常见做法是加速度计计算静态角度陀螺仪积分动态角度再用互补滤波融合。float angle 0.98f * (angle gyro_y * dt) 0.02f * acc_angle;这个式子是互补滤波最简形式。0.98 是陀螺仪权重0.02 是加速度计权重。加速度计长期稳定但短期噪声大陀螺仪短期精准但长期漂移互补滤波就是把两者的频域优势结合起来。dt 是控制周期秒gyro_y 是摆杆旋转轴方向的角速度。需要特别注意的是加速度计算角度时用哪个轴。倒立摆摆杆转轴如果平行于 MPU6050 的 X 轴则角度由atan2(ax, az)得到而不是 atan2(ay, az)。接错轴的表现很有意思程序跑起来摆杆在竖直位置疯狂抖稍微偏移就往一个方向加速倒下这时先检查角度计算是否反了。3.2 串级 PID角度环内环、位置环外环单环 PID 只能让摆杆在初始位置附近稳定无法让小车回到原点。电赛评分中“自动回到中点并保持”是很常见的加分项所以程序必须采用串级结构。typedef struct { float kp, ki, kd; float integral; float last_error; } PID_t; float CascadePID(float angle, float pos, float vel) { // 外环位置环输出目标角度偏置 float pos_err 0.0f - pos; float angle_target pos_pid.kp * pos_err pos_pid.kd * (0.0f - vel); angle_target Constrain(angle_target, -MAX_TILT, MAX_TILT); // 内环角度环输出控制力 float angle_err angle_target - angle; angle_pid.integral angle_err * dt; angle_pid.integral Constrain(angle_pid.integral, -I_LIMIT, I_LIMIT); float output angle_pid.kp * angle_err angle_pid.kd * (angle_err - angle_pid.last_error) / dt angle_pid.integral * angle_pid.ki; angle_pid.last_error angle_err; return output; }这段代码体现了倒立摆控制的核心思想位置环并不直接输出力而是输出一个目标倾角——比如小车偏左就让摆杆向右倾斜一个角度靠重力分量牵引小车回中点。这比直接用位置误差去调电机力要自然得多因为倒立摆唯一能控制的是加速度而加速度来自摆杆倾斜产生的水平分力。MAX_TILT通常在 5 度到 10 度之间。如果目标倾角限制太小位置环恢复能力弱限制太大摆杆可能超出线性化范围导致失稳。从 8 度开始调是比较稳妥的起点。3.3 电机驱动与 PWM 限幅策略电赛常见驱动方案是 TB6612 或 DRV8701配合带霍尔编码器的直流减速电机。PWM 频率建议 10kHz 到 20kHz低于 5kHz 电机会有明显的啸叫高于 50kHz 驱动芯片的开关损耗变大。#define PWM_MAX 7200 // 定时器ARR值对应100%占空比 #define PWM_LIMIT 5800 // 限幅到80%防止齿轮冲击 void SetMotorPWM(float output) { float duty output; if (duty PWM_LIMIT) duty PWM_LIMIT; if (duty -PWM_LIMIT) duty -PWM_LIMIT; if (duty 0) { __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, duty); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_2, 0); } else { __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, 0); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_2, -duty); } }限幅到 80% 看起来是浪费动力实际是保护机械结构。倒立摆的电机在极限情况下会瞬间反转全占空比输出会给齿轮和皮带带来很大的冲击应力。竞赛现场更常见的问题是电机线因为反复弯折导致接触不良程序表现为摆杆突然失去控制力直接倒下——排除故障时先检查接线再怀疑代码。4. 倒立摆电赛调参路径三个必调参数与波形判读仿真和程序骨架都到位之后真正花时间的是现场调参。倒立摆的控制器常见有 3 到 4 个待调项角度环 Kp、角度环 Kd、位置环 Kp、位置环 Kd外加角度环 Ki。调参的顺序错了会陷入“调了一天还是站不住”的泥潭。这一章给出经过验证的调参顺序和每步的判据。4.1 角度环 Kp 与 Kd先站稳再说第一步只调角度环把位置环完全禁用目标倾角强制为 0。从一个小 Kp 开始比如 100Kd 设为 20Ki 设 0。观察摆杆是否出现高频抖动。判断标准如下表现象原因处理摆杆缓慢倒下无恢复力Kp 过小按 1.5 倍增大 Kp摆杆在竖直位置高频抖动Kp 过大或 Kd 过小先增大 Kd再微调 Kp摆杆向一侧加速倒下角度符号反了或控制方向反检查电机正反转和传感器方向摆杆可以站稳但来回慢摆Kd 不足增大 Kd每次增加 20%这个阶段的目标是让摆杆用手轻推后能自行回到竖直位置且回正过程没有明显振荡。用串口把角度数据实时发到上位机采样率 100Hz 就够用。波形上理想的回正响应是衰减振荡超调量小于 30%在 1 到 2 秒内收敛。4.2 位置环 Kp 和 Kd让小车回到中点角度环站稳后依次放开位置环。位置环的调试比角度环温和Kp 从 0.5 起调每次增加 0.3。观察小车是否以摆杆竖直为前提缓慢移向中点。注意位置环的响应必须慢于角度环否则内外环时间尺度耦合会造成低频振荡——表现为小车来回大幅移动摆杆始终在竖直附近但整个系统呈周期性的“点头”状态。// 位置环参数建议范围 float pos_kp 1.2f; // 0.5 ~ 2.0 float pos_kd 0.8f; // 0.3 ~ 1.5 // 角度环参数建议范围 float angle_kp 600; // 仿真K值换算后的50%~70% float angle_kd 80; // 与kpd比例约1:8~1:12位置环的 Kd 不是必需的有些方案只靠位置 Kp 就能完成回中点但响应会慢。加了 Kd 后会引入速度反馈系统等效阻尼增大响应更快且不易过冲。速度值由编码器差分得到注意差分运算会放大噪声可以在计算速度时做一个 10 点滑动平均延迟约 10ms对控制影响很小。4.3 积分项的启用时机角度环 Ki 在所有环节之前都建议保持为 0。倒立摆是自不稳定的积分项带来的相位滞后会显著减小稳定裕度。只有在以下场景才考虑加 Ki小车停在倾斜导轨上时摆杆有一个恒定的重力偏置项导致角度环存在稳态误差。此时 Ki 从 0.5 起调配合积分限幅在最大值输出的 20% 以内。常见的错误是看到摆杆回不到精确竖直就加 Ki结果把系统调出了极限环振荡。先检查机械水平——用水平尺量一下导轨是否放平很多时候摆杆回不到竖直不是控制器问题而是导轨本身就倾斜了几毫米。这个检查只需要两分钟却能在现场避免一小时的无效调参。4.4 调参过程中的常见故障与快速定位倒立摆在调试中表现出的故障现象往往是多因一果的以下是电赛现场最高频的三类故障现象排查顺序关键检查点上电后摆杆直接倒下1. 角度值方向 2. 电机方向 3. PWM 输出用手轻推摆杆看 PWM 是否给力能站稳但抗扰太差1. 控制周期 2. 角度滤波延迟 3. 机械间隙摆杆用手弹一下观察恢复时间小车限位撞击1. 位置环增益 2. 目标倾角限幅 3. 编码器方向单独测位置环输出是否限幅第一类是最常见的摆杆倒下瞬间电机反而朝倒下方向加速。这是方向接反的经典表现。把电机线对调或者把角度误差取反二选一解决。第二类问题在搬动装置到赛场后尤其容易出现——赛场的桌面和新环境下摩擦力发生变化原来调好的 K 值不再适配就需要用下面的方法快速恢复。5. 倒立摆程序 3.0 的进阶玩法观测器、自适应与加分项前面的内容解决了“能站稳”的问题3.0 版本要解决的是“站得漂亮”的问题。电赛中评分差距往往在最后的附加题里拉开——抗风扰动、指定位置停车、摆起自动切换。这三个场景分别对应状态观测、参数在线整定和模式切换是值得在赛前投入时间的三个方向。5.1 用卡尔曼滤波替代互补滤波互补滤波的优势是轻量但在电机 PWM 占空比变化剧烈的加减速阶段加速度计会感受到额外的惯性力导致互补滤波中的加速度权重被污染。改进方案是卡尔曼滤波它把角度和陀螺仪零漂作为状态变量利用过程模型预测观测修正得到更干净的倾角估计。// 卡尔曼滤波一维模型状态[角度, 陀螺仪零漂] float angle 0, bias 0; float P[2][2] {{1,0},{0,1}}; float Q_angle 0.001f, Q_bias 0.003f, R_measure 0.03f; float KalmanFilter(float acc_angle, float gyro_rate, float dt) { // 预测 angle dt * (gyro_rate - bias); P[0][0] dt * (dt*P[1][1] - P[0][1] - P[1][0] Q_angle); P[0][1] - dt * P[1][1]; P[1][0] - dt * P[1][1]; P[1][1] Q_bias * dt; // 修正 float S P[0][0] R_measure; float K[2]; K[0] P[0][0] / S; K[1] P[1][0] / S; angle K[0] * (acc_angle - angle); bias K[1] * (acc_angle - angle); P[0][0] - K[0] * P[0][0]; P[0][1] - K[0] * P[0][1]; P[1][0] - K[1] * P[0][0]; P[1][1] - K[1] * P[0][1]; return angle; }卡尔曼滤波的参数调整比互补滤波直观很多Q_angle 是角度过程噪声方差调大表示更信任加速度计Q_bias 是零漂过程噪声方差调大表示认为零漂变化快R_measure 是测量噪声方差调大表示更信任陀螺仪。实测中 R_measure 对滤波效果的影响最大在 0.01 到 0.1 之间调。注意卡尔曼滤波的运算量比互补滤波大 5 倍以上放在中断里要量一下执行时间。5.2 同一套代码应对参数变化在线调整 LQR 增益电赛现场换桌子、换电池电压、甚至换一辆小车物理参数都会发生偏移。与其在现场重新调 PID不如在程序里内置一张 LQR 增益表按电池电压或者摆杆质量估算值自动切换。做法是在 MATLAB 离线算好几组 K 值烧录时按序号存入数组。// 按电压范围自动选择K值 float K_table[3][4] { { 8.2f, 12.1f, 45.3f, 6.4f }, // 高电压如12.0V以上 { 7.5f, 11.0f, 42.8f, 5.9f }, // 中电压 { 6.8f, 10.2f, 39.5f, 5.3f } // 低电压 }; float battery_voltage GetBatteryVoltage(); int idx (battery_voltage 11.5f) ? 0 : (battery_voltage 10.5f) ? 1 : 2; SetLQRGain(K_table[idx]);这个技巧的实用性在于电赛现场电池电压从满电 12.6V 掉到欠压 10V 只需要连续运行不到一小时电机力矩下降会让同一组 K 值从“稳定”变成“临界振荡”。按电压分段切换是最低成本的鲁棒化手段。如果时间充裕可以用递归最小二乘在线辨识系统参数然后通过查表实时更新 K 值这种方案更接近真正的自适应控制但本质是同样的思路——把离线算好的几种工况映射到当前状态。5.3 加分项摆起控制与二次开发倒立摆的初始状态是垂直向下要让它在电赛现场自动完成“上摆稳摆”的姿态切换程序里需要加入能量控制策略。能量法是最常见的方案计算当前摆杆的能量与竖直位置能量之差用一个比例控制器输出来回鞭打让摆杆逐步获得能量并接近倒立位置。float EnergyControl(float angle, float omega) { float E 0.5f * J * omega * omega m * g * l * (cos(angle) - 1.0f); float E_target m * g * l * (1.0f - 1.0f); // 竖直位置能量为0 float output k_e * (E - E_target) * sign(omega * cos(angle)); return output; }当摆杆角速度方向与目标方向匹配时这个控制器会输出与运动方向同向的力给系统注入能量。当摆杆越过竖直位置或者角度进入 ±5 度范围内时切换到 LQR/PID 稳摆模式。这个切换不能直接跳变——需要让输出在几十毫秒内从能量控制器的值过渡到稳摆控制器的值否则切换瞬间会给摆杆一个很大的冲击扰动直接打翻系统。常见做法是加权过渡float mix SmoothStep(angle_range); // 角度越小稳摆占比越高 output (1 - mix) * energy_output mix * balance_output;最后回到 3.0 版本的定位倒立摆程序不是写完就结束的静态工程它是你在电赛控制类赛场上最值得打磨的磨刀石。同一套代码结构里装着的状态方程、传感器融合和反馈控制推到比赛结束它依然可以继续用在后来的智能车、双轮自平衡和无人机悬停项目里。站得稳只是开始——能在 3 分钟调试窗口内完成参数迁移、在未知台面上跑出稳定性能那才是这套倒立摆程序真正值钱的 3.0 版本所要验证的能力。本文还有配套的精品资源点击获取