简介从经典控制理论中的PID控制谈起在实际工程中许多非线性、多干扰系统的稳定控制都依赖于准确的对象建模与传感器融合。以自由摆平板系统为例其核心难点在于摆杆运动对平板产生复杂的切向与离心干扰单纯依赖位置反馈难以实现小球稳定。通过拉格朗日方程建立动力学模型并借助IMU互补滤波还原摆角信息再结合外环位置、内环角度的双环PID结构可以有效抑制扰动。该系统广泛应用于自动化竞赛、课程设计及机器人平衡控制等场景。本文围绕动力学建模、硬件选型、滤波与控制器实现展开并给出前馈补偿与参数整定方法为类似控制问题提供一套可参考的工程方案。1. 自由摆平板控制系统先看干扰再看算法自由摆末端装一块平板摆杆摆动时平板上的小球要求停在原地或者只允许小幅移动——这是很多自动化课程设计和电子设计竞赛都爱出的题目。真正动手调试时最先让人意外的现象是摆杆摆到最大角度、平板那一瞬间稳定下来时小球反而最容易滚出去摆杆加速回摆时平板还没来得及补偿球已经过了边界。原因是平板的加速度干扰并不和摆角同步它更依赖摆杆的角速度与角加速度。这个系统的本质不是“把平板放平”而是让平板在非惯性坐标系中不断抵消运动产生的切向干扰再把小球位置闭环起来。本文按工程师自己做这套系统的路径展开动力学建模、硬件选型、角度解算与双环PID实现、参数整定与排错最后给前馈补偿等实用技巧。正在做类似毕业设计或竞赛题的同学可以把它当作一份可复现的控制方案参考。2. 自由摆平板控制系统的动力学建模与目标划分2.1 自由摆坐标系与自由度划分常见做法是把系统抽象成三个自由度摆杆绕铰链的转角theta平板相对水平面的倾角phi小球在平板上的位置x。摆杆自由摆动电机只驱动平板转动因此系统里只有phi是直接控制量theta是外部扰动源x是最终被控量。建模时取铰链为原点竖直向下为基准方向。摆杆简化为长度L的均质杆小球简化为平板上的质点。平板的转动惯量与摆杆相比通常很小但平板质量不能忽略因为它会改变摆杆的等效质心位置影响摆动周期。实际系统中摆杆越长摆动周期越长留给控制器的时间越充分摆杆太短时摆动频率高对控制带宽的要求成倍增加。2.2 摆杆运动在平板上形成的两类耦合加速度用拉格朗日方程建立系统动能和势能后把运动方程在摆角不大的条件下线性化可以得到一个在工程上很有指导意义的近似式x_ddot -g * sin(phi) - L * theta_ddot x * theta_dot^2其中x_ddot是小球相对平板的加速度g * sin(phi)是平板倾角提供的恢复加速度-L * theta_ddot是摆杆角加速度产生的切向干扰x * theta_dot^2是摆杆转动带来的法向离心项。-g * sin(phi)表明平板倾角本身就有让小球的稳的能力这一项是控制作用的“执行通道”。-L * theta_ddot是最棘手的部分摆杆回摆时角加速度方向突变干扰瞬间换向如果控制器只按当前角度误差输出就会滞后半拍。x * theta_dot^2在摆速较高时明显它会把小球向外侧推表现为球在平板上来回“蠕动”。从传递函数角度看若暂不考虑干扰项从平板倾角phi到小球位置x的通道近似是两个积分环节系统开环不稳定只靠比例控制必然振荡必须引入速度反馈或者等效阻尼。这个结论直接决定了后面控制器要采用内外双环结构而不是一个简单的单环PID。2.3 两类控制目标的工程取舍自由摆平板控制系统通常会分成两类目标。第一类是“镇定型”不管摆杆怎么摆小球尽量停在固定位置通常停在平板中心。第二类是“跟随型”给小球设定一条随时间变化的轨迹比如正弦运动系统要让小球跟随轨迹。两类目标对传感器和算法的要求差异很大明确自己要做哪一类再动手能避免后期返工。目标类型控制任务验收标准传感器要求实现难度镇定型小球固定在目标点摆幅30°内球位移2cm球位置传感器摆角传感器较低适合入门跟随型小球跟踪时变轨迹跟随延时0.3s误差5cm球位置传感器连续轨迹生成较高需加前馈硬件方案上两类目标几乎相同区别主要在控制算法。镇定型用双环PID加前馈就能达到不错的效果跟随型则额外需要轨迹规划模块并且在控制环里加入目标速度前馈否则纯位置调节会有明显滞后。下面章节的硬件方案按两类通用设计算法部分以镇定型为基线最后补充跟随型的扩展方法。3. 自由摆平板控制系统的硬件方案与参数信息3.1 机械结构设计与频率匹配机械结构设计的核心是让“自由摆”真正自由同时让平板驱动不产生额外扰动。常见的做法是摆杆用铝合金空心管或碳纤维管一端通过轴承安装在支架上另一端固定平板电机。支架底座要足够重避免摆杆摆动时整个系统跟着晃。铰链处选用低摩擦轴承但不要在轴上额外加润滑脂否则自由摆衰减太慢会给调试带来麻烦。摆杆长度的选择要和控制频率匹配。一个L0.4m的自由摆单摆周期约为2 * PI * sqrt(L/g) ≈ 1.27s这意味着控制系统的响应带宽至少要有摆动频率的5到10倍控制周期建议不超过5ms。如果摆杆缩短到0.2m周期降到约0.9s同样的控制周期下相位裕度会明显变差。因此常规做法是把摆杆长度控制在0.3m0.5m之间平板尺寸在25cm×20cm左右这样既能看清运动又给控制器留出裕量。平板与电机之间尽量采用直连或同步带连接。直连刚度高、响应快但电机轴要能承受平板的弯矩同步带可以缓解弯矩但会引入弹性变形在高速摆动时表现为平板末端的微小抖动。对小球控制来说平板末端抖动等于直接给小球叠加干扰因此我一般优先选直连方案。3.2 角度检测与位置检测方案对比摆杆角度测量有两种主流方案编码器和六轴IMU。编码器安装在铰链转轴上直接输出摆杆相对支架的角度精度高、零漂小但安装要求同轴度好且只能测摆杆这一个角度。IMU方案把加速度计和陀螺仪贴在摆杆上不需要精确的机械对准但需要做滤波和零偏校正否则角度积分漂移会让你怀疑控制算法写错了。检测对象方案优点缺点典型器件摆杆角度光电编码器精度高、零漂小安装同轴度要求高2000线增量编码器摆杆角度六轴IMU安装灵活、可测角速度需要滤波与零偏校正MPU6050小球位置线性电位器信号简单、响应快有机械摩擦导电塑料电位器小球位置灰度传感器阵列无接触、成本低分辨率有限线性CCD或反射式传感器小球位置摄像头信息丰富、无机械接触帧率受限OV2640小球位置测量是很多人低估的部分。早期方案常用在平板上铺一层导电橡胶、小球作为触点的办法但摩擦太大小球运动被明显阻滞。更可靠的是用线性电位器或一排灰度传感器贴在平板下方通过小球遮挡位置换算坐标。摄像头方案能同时获得位置和速度但帧率通常只有30到60帧控制周期被限制在16ms以上对自由摆这种快速变化系统来说太低不推荐在主控制环里直接使用更适合作为离线分析工具。3.3 驱动电机与扭矩估算平板驱动选型需要同时满足扭矩和响应速度两个条件。航模舵机是入门阶段最省事的方案自带位置闭环但内部PID响应较慢且无法直接控制扭矩用在自由摆上会感觉平板动作“黏”。更可靠的是带编码器的直流减速电机或总线舵机前者由主控直接做位置环后者可以修改速度环参数。驱动电机的最小扭矩可以按最恶劣工况估算。平板和球的转动惯量J乘以期望角加速度alpha_max再加上重力偏置项T_min J * alpha_max M * g * r其中M是平板和球的总质量r是质心到电机轴的水平距离。工程上取1.52倍的安全余量后额定扭矩在0.30.6 N·m的直流减速电机基本够用。减速比选择上电机输出转速控制在60120 rpm比较合适太高则低速控制不平滑太低则响应跟不上摆动干扰。4. 基于互补滤波的摆角解算与双环PID控制实现4.1 用互补滤波从IMU还原自由摆摆角如果摆角由IMU测量最常见的做法是先用加速度计结合atan2求静态角度再用陀螺仪积分补充动态信息最后用互补滤波融合。互补滤波比卡尔曼滤波简单得多在自由摆这种角度变化范围已知的系统中效果差别很小。#define GYRO_COEF 0.96f #define ACC_COEF 0.04f float pitch_angle 0.0f; float gyro_bias 0.0f; void swing_angle_init(float ax, float ay, float az, int samples) { float sum 0.0f; for (int i 0; i samples; i) { sum read_gyro(); // 静止时采集陀螺仪输出 } gyro_bias sum / samples; } void update_swing_angle(float ax, float ay, float az, float gyro_rate, float dt) { // 加速度计静态估计把摆杆近似看作静止时测得重力分量 float acc_angle atan2f(-ax, az) * 180.0f / PI; // 陀螺仪积分减去零偏后再累计 float gyro_angle pitch_angle (gyro_rate - gyro_bias) * dt; // 一阶互补低频信任加速度计高频信任陀螺仪 pitch_angle ACC_COEF * acc_angle GYRO_COEF * gyro_angle; }代码里acc_angle由加速度计的ax和az求得atan2f能同时处理象限符号避免在摆杆过竖直位置时跳变。gyro_bias在初始化时静止采集若干帧陀螺仪平均值得到这一步不能省否则积分漂移会在几十秒内让角度偏出十几度。GYRO_COEF取0.96意味着动态过程主要依赖陀螺仪只有低频段才由加速度计纠正这个比例在摆杆场景下基本不用改动。4.2 双环PID控制器结构外环控位置、内环控角度自由摆平板控制器的经典结构是双环控制器外环根据小球位置误差计算出期望平板倾角内环让平板快速跟踪这个期望倾角。外环慢、内环快两者控制频率可以相同但参数上内环增益要远高于外环。#define BALL_TARGET 0.0f // 目标位置平板中心 #define PWM_LIMIT 80.0f // 输出限幅防止过驱动 void balance_control(float ball_pos, float ball_vel, float plate_angle, float plate_vel) { float target_angle, err_pos, err_angle, pid_out; // 外环位置误差映射到期望倾角 err_pos BALL_TARGET - ball_pos; target_angle KP_POS * err_pos - KD_POS * ball_vel; // 内环期望倾角和实际倾角的误差 err_angle target_angle - plate_angle; pid_out KP_ANG * err_angle - KD_ANG * plate_vel; // 限幅后输出PWM if (pid_out PWM_LIMIT) pid_out PWM_LIMIT; if (pid_out -PWM_LIMIT) pid_out -PWM_LIMIT; set_plate_pwm(pid_out); }外环输出target_angle表示“平板应该倾斜多少度才能把球拉回中心”KD_POS乘的是小球速度方向为负是因为小球正在远离目标时应减小期望倾角。内环的KD_ANG直接作用于平板角速度这一项相当于系统阻尼能抑制平板本身的振荡。PWM_LIMIT根据电机驱动能力设定工程上先取最大输出的80%避免平板动作过猛。这里有一个关键细节外环的KD_POS不要加在位置误差微分上。小球位置信号通常带有测量噪声直接微分会放大噪声导致平板高频抖动。如果传感器只输出位置没有速度可以用一阶低通近似微分或者用位置差分加滑动平均但不要用纯差分。4.3 控制周期与中断定时器的参数设置控制周期直接决定系统带宽上限。常见的做法是让内环控制频率保持在500Hz到1kHz外环可以降到250Hz左右。若控制频率太低自由摆的切向干扰在每个控制周期内变化过大PID无法有效跟踪太高则主控负担重而且球位置传感器的采样率未必跟得上。// 以72MHz主控为例定时器中断频率500Hz void plate_timer_init(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_TimeBaseStructure.TIM_Prescaler 72 - 1; // 72MHz/72 1MHz TIM_TimeBaseStructure.TIM_Period 2000 - 1; // 1MHz/2000 500Hz TIM_TimeBaseStructure.TIM_ClockDivision 0; TIM_TimeBaseStructure.TIM_CounterMode TIM_CounterMode_Up; TIM_TimeBaseInit(TIM3, TIM_TimeBaseStructure); TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitStructure.NVIC_IRQChannel TIM3_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority 0; NVIC_InitStructure.NVIC_IRQChannelSubPriority 1; NVIC_InitStructure.NVIC_IRQChannelCmd ENABLE; NVIC_Init(NVIC_InitStructure); TIM_Cmd(TIM3, ENABLE); }预分频器和自动重装值共同决定中断频率72MHz / 72 / 2000 500Hz。把控制逻辑放在定时器中断服务函数里主循环只负责显示和参数调整这样控制周期不因主循环任务变化而抖动。如果主控还承担通信任务需要给中断处理函数设定高优先级并且中断服务函数里不要做浮点打印或阻塞式串口输出。5. 自由摆平板控制器参数整定与典型故障排查5.1 先整内环再开外环的调参步骤双环控制器直接从外环调起很容易陷入振荡。正确顺序是先把小球固定住整定内环平板角度环再放开小球整定外环。步骤整定对象操作现象调整方向1内环KP_ANG平板快速到达目标倾角但来回抖减小KP_ANG2内环KD_ANG平板到位慢、有超调增大KD_ANG3内环联合手推平板感到柔顺有阻力定住当前值4外环KP_POS球在中心附近低频振荡先减小KP_POS再加大KD_POS5外环KD_POS球缓慢漂移、不收敛适当增大KP_POS6前馈KFF摆动时球向一侧偏移调整前馈方向和幅值内环整定时把小球用手按住或取下给平板一个方波目标角观察响应。KP_ANG过大时平板抖动明显并伴随电机发热KD_ANG过小时平板会冲过目标位置再折返。内环调好后平板的阶跃响应应该在一个到两个控制周期内到位没有明显振荡。再把手松开从KP_POS0.5附近的较小值开始加观察小球运动。如果球在中心来回荡说明外环阻尼不够优先增大KD_POS不要急着减小KP_POS。5.2 自由摆运动中常见的三个失败场景第一个故障场景是摆杆摆动后小球总是沿摆动方向偏移。这通常是前馈缺失或陀螺零偏不准导致的而不是PID参数问题。处理办法是先让摆杆静止检查小球是否能在平板中心稳定住能稳定后再手动小幅度摆动摆杆看偏移量是否随摆幅增大而增大。第二个故障场景是平板高频高频抖动类似“震颤”。这有两种来源一种来自内环增益过高放大了位置传感器噪声另一种来自机械谐振电机带动平板在高频段出现固有共振。排查时可以让摆杆完全静止只给内环阶跃信号如果静止时平板仍然发烫或声音尖锐优先降低控制频率或对plate_vel增加一阶低通滤波。第三个故障场景是球到达平板边缘后失控。这不是算法问题而是平板边界与传感器量程不匹配。自由摆摆动幅度大时球的运动范围会超过传感器有效测量区控制器读到的是钳位值而不是真实位置。常见做法是在控制代码里判断球位置超出安全阈值后立即执行“平板上电回中”策略防止球掉落后平板还朝一个方向猛转。5.3 用冻结参数法判断控制器带宽“冻结参数法”是我在调试这类系统时比较依赖的技巧。具体操作是把控制器输出固定在一个偏置值上让平板不参与反馈只由外部电机或手动推动摆杆摆动记录球在平板上的实际运动轨迹。这个轨迹反应的是纯干扰下的开环响应。如果开环响应中球位移明显说明干扰能量落在控制带宽之外如果球位移虽然存在但频率较低则说明控制器有希望把干扰抑制掉。这能直观地回答一个常见问题“到底是我PID参数不够好还是响应速度根本不够”冻结参数法还能验证前馈项的符号和比例——让前馈先单独作用看能否抵消部分开环位移再把前馈并入闭环参数调整会顺畅很多。6. 把摆杆角加速度变成前馈量三个工程落点6.1 摆杆角加速度前馈的写法前馈补偿的目标是抵消x_ddot中的-L * theta_ddot项。估算摆杆角加速度最容易的方式是差分用陀螺仪输出的摆杆角速度连续两帧做差再除以控制周期。虽然差分会引入噪声但前馈通常只取差值的部分比例噪声幅度有限。float get_feedforward(float swing_vel, float dt) { static float last_vel 0.0f; // 角速度差分近似角加速度 float swing_accel (swing_vel - last_vel) / dt; last_vel swing_vel; // 方向符号与摆杆长度有关KFF1时表示完全补偿 return KFF * swing_accel; }KFF不是越大越好。大于1时前馈方向相反反而会把球推向边缘小于0.5时补偿不足摆动中仍能看到周期性偏移。标定方法让摆杆以固定幅度来回摆动同时观察球的平均位置偏移。从0开始逐步增大KFF当球的平均偏移量不再变化时说明前馈系数已经收敛到该摆幅下的最佳值。6.2 开机自检要检查的四个量上电后不要急着进入控制循环先做静态自检。第一项检查IMU零偏是否在预期范围若零偏超过阈值说明传感器或供电有问题第二项检查球位置传感器中心值把球放在平板中心确认读数与标定值一致第三项检查平板电机正反向用很小的PWM输出让平板左右各转一次确认控制方向没有反第四项确认定时器中断频率稳定可以用示波器量一个GPIO翻转波形。这四项检查通过后再运行控制算法可以把大部分硬件问题拦截在调参之前。6.3 参数集管理与比赛调参的落地方式调参过程中改动频繁手动改代码里的常量再烧录效率太低。常见做法是把控制器参数定义成结构体数组用拨码开关或按键选择参数组在运行时直接生效。typedef struct { float kp_ang, kd_ang; float kp_pos, kd_pos; float kff; int pwm_limit; } control_params_t; const control_params_t param_sets[3] { { 8.0f, 0.4f, 1.2f, 0.3f, 0.6f, 80 }, { 6.5f, 0.5f, 1.0f, 0.4f, 0.8f, 80 }, { 9.0f, 0.3f, 1.5f, 0.5f, 0.5f, 100 }, };这样每个参数组只对应一组完整配置调参时可以快速对比三组之间的差别。比赛或验收现场时间有限建议现场只验证“小摆幅稳定”“大摆幅缓慢恢复”和“小球位置回中精度”三个动作这三项通过后再考虑跟随型轨迹。若出现球在平板上反复漂移且每种参数组都无改善优先怀疑传感器标定值失效而不是继续改PID参数。本文还有配套的精品资源点击获取 自由摆平板控制系统：动力学建模与双环PID控制实现
简介从经典控制理论中的PID控制谈起在实际工程中许多非线性、多干扰系统的稳定控制都依赖于准确的对象建模与传感器融合。以自由摆平板系统为例其核心难点在于摆杆运动对平板产生复杂的切向与离心干扰单纯依赖位置反馈难以实现小球稳定。通过拉格朗日方程建立动力学模型并借助IMU互补滤波还原摆角信息再结合外环位置、内环角度的双环PID结构可以有效抑制扰动。该系统广泛应用于自动化竞赛、课程设计及机器人平衡控制等场景。本文围绕动力学建模、硬件选型、滤波与控制器实现展开并给出前馈补偿与参数整定方法为类似控制问题提供一套可参考的工程方案。1. 自由摆平板控制系统先看干扰再看算法自由摆末端装一块平板摆杆摆动时平板上的小球要求停在原地或者只允许小幅移动——这是很多自动化课程设计和电子设计竞赛都爱出的题目。真正动手调试时最先让人意外的现象是摆杆摆到最大角度、平板那一瞬间稳定下来时小球反而最容易滚出去摆杆加速回摆时平板还没来得及补偿球已经过了边界。原因是平板的加速度干扰并不和摆角同步它更依赖摆杆的角速度与角加速度。这个系统的本质不是“把平板放平”而是让平板在非惯性坐标系中不断抵消运动产生的切向干扰再把小球位置闭环起来。本文按工程师自己做这套系统的路径展开动力学建模、硬件选型、角度解算与双环PID实现、参数整定与排错最后给前馈补偿等实用技巧。正在做类似毕业设计或竞赛题的同学可以把它当作一份可复现的控制方案参考。2. 自由摆平板控制系统的动力学建模与目标划分2.1 自由摆坐标系与自由度划分常见做法是把系统抽象成三个自由度摆杆绕铰链的转角theta平板相对水平面的倾角phi小球在平板上的位置x。摆杆自由摆动电机只驱动平板转动因此系统里只有phi是直接控制量theta是外部扰动源x是最终被控量。建模时取铰链为原点竖直向下为基准方向。摆杆简化为长度L的均质杆小球简化为平板上的质点。平板的转动惯量与摆杆相比通常很小但平板质量不能忽略因为它会改变摆杆的等效质心位置影响摆动周期。实际系统中摆杆越长摆动周期越长留给控制器的时间越充分摆杆太短时摆动频率高对控制带宽的要求成倍增加。2.2 摆杆运动在平板上形成的两类耦合加速度用拉格朗日方程建立系统动能和势能后把运动方程在摆角不大的条件下线性化可以得到一个在工程上很有指导意义的近似式x_ddot -g * sin(phi) - L * theta_ddot x * theta_dot^2其中x_ddot是小球相对平板的加速度g * sin(phi)是平板倾角提供的恢复加速度-L * theta_ddot是摆杆角加速度产生的切向干扰x * theta_dot^2是摆杆转动带来的法向离心项。-g * sin(phi)表明平板倾角本身就有让小球的稳的能力这一项是控制作用的“执行通道”。-L * theta_ddot是最棘手的部分摆杆回摆时角加速度方向突变干扰瞬间换向如果控制器只按当前角度误差输出就会滞后半拍。x * theta_dot^2在摆速较高时明显它会把小球向外侧推表现为球在平板上来回“蠕动”。从传递函数角度看若暂不考虑干扰项从平板倾角phi到小球位置x的通道近似是两个积分环节系统开环不稳定只靠比例控制必然振荡必须引入速度反馈或者等效阻尼。这个结论直接决定了后面控制器要采用内外双环结构而不是一个简单的单环PID。2.3 两类控制目标的工程取舍自由摆平板控制系统通常会分成两类目标。第一类是“镇定型”不管摆杆怎么摆小球尽量停在固定位置通常停在平板中心。第二类是“跟随型”给小球设定一条随时间变化的轨迹比如正弦运动系统要让小球跟随轨迹。两类目标对传感器和算法的要求差异很大明确自己要做哪一类再动手能避免后期返工。目标类型控制任务验收标准传感器要求实现难度镇定型小球固定在目标点摆幅30°内球位移2cm球位置传感器摆角传感器较低适合入门跟随型小球跟踪时变轨迹跟随延时0.3s误差5cm球位置传感器连续轨迹生成较高需加前馈硬件方案上两类目标几乎相同区别主要在控制算法。镇定型用双环PID加前馈就能达到不错的效果跟随型则额外需要轨迹规划模块并且在控制环里加入目标速度前馈否则纯位置调节会有明显滞后。下面章节的硬件方案按两类通用设计算法部分以镇定型为基线最后补充跟随型的扩展方法。3. 自由摆平板控制系统的硬件方案与参数信息3.1 机械结构设计与频率匹配机械结构设计的核心是让“自由摆”真正自由同时让平板驱动不产生额外扰动。常见的做法是摆杆用铝合金空心管或碳纤维管一端通过轴承安装在支架上另一端固定平板电机。支架底座要足够重避免摆杆摆动时整个系统跟着晃。铰链处选用低摩擦轴承但不要在轴上额外加润滑脂否则自由摆衰减太慢会给调试带来麻烦。摆杆长度的选择要和控制频率匹配。一个L0.4m的自由摆单摆周期约为2 * PI * sqrt(L/g) ≈ 1.27s这意味着控制系统的响应带宽至少要有摆动频率的5到10倍控制周期建议不超过5ms。如果摆杆缩短到0.2m周期降到约0.9s同样的控制周期下相位裕度会明显变差。因此常规做法是把摆杆长度控制在0.3m0.5m之间平板尺寸在25cm×20cm左右这样既能看清运动又给控制器留出裕量。平板与电机之间尽量采用直连或同步带连接。直连刚度高、响应快但电机轴要能承受平板的弯矩同步带可以缓解弯矩但会引入弹性变形在高速摆动时表现为平板末端的微小抖动。对小球控制来说平板末端抖动等于直接给小球叠加干扰因此我一般优先选直连方案。3.2 角度检测与位置检测方案对比摆杆角度测量有两种主流方案编码器和六轴IMU。编码器安装在铰链转轴上直接输出摆杆相对支架的角度精度高、零漂小但安装要求同轴度好且只能测摆杆这一个角度。IMU方案把加速度计和陀螺仪贴在摆杆上不需要精确的机械对准但需要做滤波和零偏校正否则角度积分漂移会让你怀疑控制算法写错了。检测对象方案优点缺点典型器件摆杆角度光电编码器精度高、零漂小安装同轴度要求高2000线增量编码器摆杆角度六轴IMU安装灵活、可测角速度需要滤波与零偏校正MPU6050小球位置线性电位器信号简单、响应快有机械摩擦导电塑料电位器小球位置灰度传感器阵列无接触、成本低分辨率有限线性CCD或反射式传感器小球位置摄像头信息丰富、无机械接触帧率受限OV2640小球位置测量是很多人低估的部分。早期方案常用在平板上铺一层导电橡胶、小球作为触点的办法但摩擦太大小球运动被明显阻滞。更可靠的是用线性电位器或一排灰度传感器贴在平板下方通过小球遮挡位置换算坐标。摄像头方案能同时获得位置和速度但帧率通常只有30到60帧控制周期被限制在16ms以上对自由摆这种快速变化系统来说太低不推荐在主控制环里直接使用更适合作为离线分析工具。3.3 驱动电机与扭矩估算平板驱动选型需要同时满足扭矩和响应速度两个条件。航模舵机是入门阶段最省事的方案自带位置闭环但内部PID响应较慢且无法直接控制扭矩用在自由摆上会感觉平板动作“黏”。更可靠的是带编码器的直流减速电机或总线舵机前者由主控直接做位置环后者可以修改速度环参数。驱动电机的最小扭矩可以按最恶劣工况估算。平板和球的转动惯量J乘以期望角加速度alpha_max再加上重力偏置项T_min J * alpha_max M * g * r其中M是平板和球的总质量r是质心到电机轴的水平距离。工程上取1.52倍的安全余量后额定扭矩在0.30.6 N·m的直流减速电机基本够用。减速比选择上电机输出转速控制在60120 rpm比较合适太高则低速控制不平滑太低则响应跟不上摆动干扰。4. 基于互补滤波的摆角解算与双环PID控制实现4.1 用互补滤波从IMU还原自由摆摆角如果摆角由IMU测量最常见的做法是先用加速度计结合atan2求静态角度再用陀螺仪积分补充动态信息最后用互补滤波融合。互补滤波比卡尔曼滤波简单得多在自由摆这种角度变化范围已知的系统中效果差别很小。#define GYRO_COEF 0.96f #define ACC_COEF 0.04f float pitch_angle 0.0f; float gyro_bias 0.0f; void swing_angle_init(float ax, float ay, float az, int samples) { float sum 0.0f; for (int i 0; i samples; i) { sum read_gyro(); // 静止时采集陀螺仪输出 } gyro_bias sum / samples; } void update_swing_angle(float ax, float ay, float az, float gyro_rate, float dt) { // 加速度计静态估计把摆杆近似看作静止时测得重力分量 float acc_angle atan2f(-ax, az) * 180.0f / PI; // 陀螺仪积分减去零偏后再累计 float gyro_angle pitch_angle (gyro_rate - gyro_bias) * dt; // 一阶互补低频信任加速度计高频信任陀螺仪 pitch_angle ACC_COEF * acc_angle GYRO_COEF * gyro_angle; }代码里acc_angle由加速度计的ax和az求得atan2f能同时处理象限符号避免在摆杆过竖直位置时跳变。gyro_bias在初始化时静止采集若干帧陀螺仪平均值得到这一步不能省否则积分漂移会在几十秒内让角度偏出十几度。GYRO_COEF取0.96意味着动态过程主要依赖陀螺仪只有低频段才由加速度计纠正这个比例在摆杆场景下基本不用改动。4.2 双环PID控制器结构外环控位置、内环控角度自由摆平板控制器的经典结构是双环控制器外环根据小球位置误差计算出期望平板倾角内环让平板快速跟踪这个期望倾角。外环慢、内环快两者控制频率可以相同但参数上内环增益要远高于外环。#define BALL_TARGET 0.0f // 目标位置平板中心 #define PWM_LIMIT 80.0f // 输出限幅防止过驱动 void balance_control(float ball_pos, float ball_vel, float plate_angle, float plate_vel) { float target_angle, err_pos, err_angle, pid_out; // 外环位置误差映射到期望倾角 err_pos BALL_TARGET - ball_pos; target_angle KP_POS * err_pos - KD_POS * ball_vel; // 内环期望倾角和实际倾角的误差 err_angle target_angle - plate_angle; pid_out KP_ANG * err_angle - KD_ANG * plate_vel; // 限幅后输出PWM if (pid_out PWM_LIMIT) pid_out PWM_LIMIT; if (pid_out -PWM_LIMIT) pid_out -PWM_LIMIT; set_plate_pwm(pid_out); }外环输出target_angle表示“平板应该倾斜多少度才能把球拉回中心”KD_POS乘的是小球速度方向为负是因为小球正在远离目标时应减小期望倾角。内环的KD_ANG直接作用于平板角速度这一项相当于系统阻尼能抑制平板本身的振荡。PWM_LIMIT根据电机驱动能力设定工程上先取最大输出的80%避免平板动作过猛。这里有一个关键细节外环的KD_POS不要加在位置误差微分上。小球位置信号通常带有测量噪声直接微分会放大噪声导致平板高频抖动。如果传感器只输出位置没有速度可以用一阶低通近似微分或者用位置差分加滑动平均但不要用纯差分。4.3 控制周期与中断定时器的参数设置控制周期直接决定系统带宽上限。常见的做法是让内环控制频率保持在500Hz到1kHz外环可以降到250Hz左右。若控制频率太低自由摆的切向干扰在每个控制周期内变化过大PID无法有效跟踪太高则主控负担重而且球位置传感器的采样率未必跟得上。// 以72MHz主控为例定时器中断频率500Hz void plate_timer_init(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_TimeBaseStructure.TIM_Prescaler 72 - 1; // 72MHz/72 1MHz TIM_TimeBaseStructure.TIM_Period 2000 - 1; // 1MHz/2000 500Hz TIM_TimeBaseStructure.TIM_ClockDivision 0; TIM_TimeBaseStructure.TIM_CounterMode TIM_CounterMode_Up; TIM_TimeBaseInit(TIM3, TIM_TimeBaseStructure); TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitStructure.NVIC_IRQChannel TIM3_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority 0; NVIC_InitStructure.NVIC_IRQChannelSubPriority 1; NVIC_InitStructure.NVIC_IRQChannelCmd ENABLE; NVIC_Init(NVIC_InitStructure); TIM_Cmd(TIM3, ENABLE); }预分频器和自动重装值共同决定中断频率72MHz / 72 / 2000 500Hz。把控制逻辑放在定时器中断服务函数里主循环只负责显示和参数调整这样控制周期不因主循环任务变化而抖动。如果主控还承担通信任务需要给中断处理函数设定高优先级并且中断服务函数里不要做浮点打印或阻塞式串口输出。5. 自由摆平板控制器参数整定与典型故障排查5.1 先整内环再开外环的调参步骤双环控制器直接从外环调起很容易陷入振荡。正确顺序是先把小球固定住整定内环平板角度环再放开小球整定外环。步骤整定对象操作现象调整方向1内环KP_ANG平板快速到达目标倾角但来回抖减小KP_ANG2内环KD_ANG平板到位慢、有超调增大KD_ANG3内环联合手推平板感到柔顺有阻力定住当前值4外环KP_POS球在中心附近低频振荡先减小KP_POS再加大KD_POS5外环KD_POS球缓慢漂移、不收敛适当增大KP_POS6前馈KFF摆动时球向一侧偏移调整前馈方向和幅值内环整定时把小球用手按住或取下给平板一个方波目标角观察响应。KP_ANG过大时平板抖动明显并伴随电机发热KD_ANG过小时平板会冲过目标位置再折返。内环调好后平板的阶跃响应应该在一个到两个控制周期内到位没有明显振荡。再把手松开从KP_POS0.5附近的较小值开始加观察小球运动。如果球在中心来回荡说明外环阻尼不够优先增大KD_POS不要急着减小KP_POS。5.2 自由摆运动中常见的三个失败场景第一个故障场景是摆杆摆动后小球总是沿摆动方向偏移。这通常是前馈缺失或陀螺零偏不准导致的而不是PID参数问题。处理办法是先让摆杆静止检查小球是否能在平板中心稳定住能稳定后再手动小幅度摆动摆杆看偏移量是否随摆幅增大而增大。第二个故障场景是平板高频高频抖动类似“震颤”。这有两种来源一种来自内环增益过高放大了位置传感器噪声另一种来自机械谐振电机带动平板在高频段出现固有共振。排查时可以让摆杆完全静止只给内环阶跃信号如果静止时平板仍然发烫或声音尖锐优先降低控制频率或对plate_vel增加一阶低通滤波。第三个故障场景是球到达平板边缘后失控。这不是算法问题而是平板边界与传感器量程不匹配。自由摆摆动幅度大时球的运动范围会超过传感器有效测量区控制器读到的是钳位值而不是真实位置。常见做法是在控制代码里判断球位置超出安全阈值后立即执行“平板上电回中”策略防止球掉落后平板还朝一个方向猛转。5.3 用冻结参数法判断控制器带宽“冻结参数法”是我在调试这类系统时比较依赖的技巧。具体操作是把控制器输出固定在一个偏置值上让平板不参与反馈只由外部电机或手动推动摆杆摆动记录球在平板上的实际运动轨迹。这个轨迹反应的是纯干扰下的开环响应。如果开环响应中球位移明显说明干扰能量落在控制带宽之外如果球位移虽然存在但频率较低则说明控制器有希望把干扰抑制掉。这能直观地回答一个常见问题“到底是我PID参数不够好还是响应速度根本不够”冻结参数法还能验证前馈项的符号和比例——让前馈先单独作用看能否抵消部分开环位移再把前馈并入闭环参数调整会顺畅很多。6. 把摆杆角加速度变成前馈量三个工程落点6.1 摆杆角加速度前馈的写法前馈补偿的目标是抵消x_ddot中的-L * theta_ddot项。估算摆杆角加速度最容易的方式是差分用陀螺仪输出的摆杆角速度连续两帧做差再除以控制周期。虽然差分会引入噪声但前馈通常只取差值的部分比例噪声幅度有限。float get_feedforward(float swing_vel, float dt) { static float last_vel 0.0f; // 角速度差分近似角加速度 float swing_accel (swing_vel - last_vel) / dt; last_vel swing_vel; // 方向符号与摆杆长度有关KFF1时表示完全补偿 return KFF * swing_accel; }KFF不是越大越好。大于1时前馈方向相反反而会把球推向边缘小于0.5时补偿不足摆动中仍能看到周期性偏移。标定方法让摆杆以固定幅度来回摆动同时观察球的平均位置偏移。从0开始逐步增大KFF当球的平均偏移量不再变化时说明前馈系数已经收敛到该摆幅下的最佳值。6.2 开机自检要检查的四个量上电后不要急着进入控制循环先做静态自检。第一项检查IMU零偏是否在预期范围若零偏超过阈值说明传感器或供电有问题第二项检查球位置传感器中心值把球放在平板中心确认读数与标定值一致第三项检查平板电机正反向用很小的PWM输出让平板左右各转一次确认控制方向没有反第四项确认定时器中断频率稳定可以用示波器量一个GPIO翻转波形。这四项检查通过后再运行控制算法可以把大部分硬件问题拦截在调参之前。6.3 参数集管理与比赛调参的落地方式调参过程中改动频繁手动改代码里的常量再烧录效率太低。常见做法是把控制器参数定义成结构体数组用拨码开关或按键选择参数组在运行时直接生效。typedef struct { float kp_ang, kd_ang; float kp_pos, kd_pos; float kff; int pwm_limit; } control_params_t; const control_params_t param_sets[3] { { 8.0f, 0.4f, 1.2f, 0.3f, 0.6f, 80 }, { 6.5f, 0.5f, 1.0f, 0.4f, 0.8f, 80 }, { 9.0f, 0.3f, 1.5f, 0.5f, 0.5f, 100 }, };这样每个参数组只对应一组完整配置调参时可以快速对比三组之间的差别。比赛或验收现场时间有限建议现场只验证“小摆幅稳定”“大摆幅缓慢恢复”和“小球位置回中精度”三个动作这三项通过后再考虑跟随型轨迹。若出现球在平板上反复漂移且每种参数组都无改善优先怀疑传感器标定值失效而不是继续改PID参数。本文还有配套的精品资源点击获取