舵机这东西玩过机器人或航模的朋友都不陌生。给它一个PWM信号它就转到指定角度简单直接。但如果你真拿MG995或MG996R做过项目就会发现事情没那么简单——空载时转得挺准一装上负载就开始抖要么到位慢吞吞要么在目标角度附近来回晃。很多人第一反应是舵机质量不行换更贵的型号结果问题依旧。其实根子不在舵机本身而在于控制方式大多数教程只教你怎么用Servo.write()却没人告诉你这背后是一个开环系统没有任何反馈修正能力。MG995和MG996R这两款舵机在DIY圈子里保有量极大金属齿轮、扭矩够用、价格便宜做机械臂、云台、小车转向都很合适。但它们有个共同的毛病内部电位器精度一般死区偏大加上齿轮间隙导致重复定位精度只能做到±3°到±5°。如果你只是做个开关门的小玩意这精度够了但要做机械臂抓取、云台稳定跟踪这类任务就必须引入闭环控制。PID就是干这个的——它不关心舵机内部怎么想只盯着目标角度和实际角度差了多少然后不断修正输出直到误差趋近于零。这篇内容面向的是已经会用Arduino或STM32驱动舵机、但被抖动和精度问题困扰的开发者。我会从MG995/MG996R的物理特性讲起拆解为什么标准PWM驱动不够用然后一步步搭建带反馈的PID调速方案包括传感器选型、采样滤波、参数整定、前馈补偿这些实战环节。代码会给出Arduino和STM32 HAL两个版本参数整定过程会附上实测曲线和踩坑记录。看完你至少能明白一件事舵机控制精度的天花板往往不是舵机本身决定的而是你的控制策略决定的。1. 先搞清楚MG995和MG996R到底能做什么、不能做什么1.1 这两款舵机的内部结构决定了控制上限MG995和MG996R外观几乎一样内部都是直流有刷电机金属齿轮减速箱电位器反馈。注意这里的电位器反馈是给舵机内部控制芯片用的不是给你用的。你从外面只能看到三根线电源、地、信号。信号线接收的是50Hz的PWM波脉宽0.5ms到2.5ms对应0°到180°。舵机内部的控制芯片会拿这个脉宽和电位器电压做比较驱动电机转到对应位置。问题就出在这个内部闭环上。它的控制目标是电位器电压等于脉宽对应的参考电压而不是输出轴角度等于你想要的绝对角度。电位器和输出轴之间隔着齿轮组齿轮有间隙电位器本身也有线性度误差。所以当你反复给同一个脉宽信号时输出轴的实际角度每次都会有微小差异。我实测过五个MG996R同一脉宽下最大偏差能到4.2°。更麻烦的是死区。舵机内部芯片为了防止电机在目标位置附近来回振荡设置了一个死区范围大概对应±1.5°到±2°。也就是说误差在这个范围内时电机根本不动作。这对于需要精细定位的场景来说就是硬伤。1.2 标准PWM驱动的三个典型问题用Servo.write()或者直接输出PWM你会遇到三类问题。第一类是到位抖动舵机转到目标附近后电机还在反复微调表现为高频嗡嗡声和轻微抖动。这是因为内部死区设置偏小或者负载力矩让电位器读数波动芯片不断修正。第二类是负载下垂空载时能转到90°装上机械臂后只能到85°因为负载力矩让电机在到达目标前就堵转了。第三类是响应迟滞从0°转到180°需要0.2秒左右但如果你要求它在0.1秒内到位它做不到而且会过冲。这三个问题的根源是一样的你给的是位置指令但舵机内部执行的是位置误差→电机电压的简单比例控制没有积分项消除稳态误差也没有微分项抑制过冲。换句话说舵机内部就是一个低质量的P控制器。你要做的是在外面再加一层更聪明的控制器。1.3 什么场景值得上PID什么场景不值得不是所有项目都需要PID。如果你只是做个舵机测试架、开关窗帘、控制小车转向标准PWM足够了加PID反而增加复杂度和成本。但以下场景值得考虑机械臂多关节联动每个关节的角度误差会累积到末端执行器云台跟踪移动目标需要快速响应且不能过冲仿生机器人关节需要模拟肌肉的柔顺特性高精度定位平台重复定位精度要求小于1°。判断标准很简单如果你发现标准PWM驱动下系统误差超过应用允许范围或者动态响应满足不了要求那就该上PID了。MG995/MG996R在加了外部反馈后重复定位精度可以做到±0.5°以内响应时间也能优化30%以上。代价是需要额外加角度传感器以及一套参数整定流程。2. 外部反馈怎么加传感器选型和安装的实战细节2.1 角度传感器的三种方案对比给舵机加外部反馈核心是找一个能测量输出轴绝对角度的传感器。常见方案有三种电位器、磁性编码器、光学编码器。电位器最便宜但寿命短、有磨损、线性度一般磁性编码器比如AS5600I2C接口12位分辨率非接触式寿命长但需要磁铁和芯片对准光学编码器精度最高但价格贵、体积大、安装复杂。对于MG995/MG996R这个级别的舵机我推荐AS5600。它的有效分辨率是0.088°足够覆盖舵机的机械精度。价格在10元左右I2C接口直接接单片机代码也简单。唯一需要注意的是磁铁安装径向充磁的圆磁铁要固定在输出轴上芯片要正对磁铁中心间距1mm到3mm。间距太大会导致读数跳变太小可能机械干涉。传感器类型分辨率接口寿命成本适用场景电位器10位模拟短低低成本测试AS560012位I2C长中推荐方案光学编码器14位SPI/AB长高高精度需求2.2 机械安装的坑同轴度决定数据质量传感器装不好后面PID调死也白搭。我踩过最坑的一次是磁铁偏心安装导致AS5600读数呈正弦波动PID一开就振荡。正确的做法是先用3D打印一个联轴器把磁铁固定在舵机输出轴的正中心然后把AS5600芯片板固定在舵机外壳上保证芯片中心与磁铁中心对齐。同轴度误差控制在0.5mm以内。如果你用的是舵机云台或机械臂输出轴往往还要连接其他结构件。这时候建议把传感器装在舵机输出轴和负载之间直接测量负载角度而不是舵机轴角度。因为齿轮间隙的存在舵机轴角度和负载角度在换向时会有差异。直接测负载角度PID才能补偿掉齿轮间隙的影响。注意AS5600的磁铁必须用径向充磁的轴向充磁的磁铁装上去读数不会变化。买的时候看清楚规格或者直接用AS5600配套的磁铁。2.3 采样频率和滤波别让噪声骗了PIDAS5600的I2C读取速度取决于时钟频率。标准100kHz下读一次角度大概需要1ms。如果你用400kHz可以做到300μs左右。对于舵机这种响应速度在100ms级别的系统1kHz的采样率绰绰有余。但问题是原始数据有噪声直接喂给PID会导致微分项剧烈跳动。我通常加两级滤波第一级是滑动平均窗口大小5到10消除随机噪声第二级是一阶低通滤波截止频率设在20Hz到50Hz之间根据舵机响应速度调整。滤波会引入相位滞后所以截止频率不能太低否则PID的微分项会失效。实测下来滑动平均窗口取8低通截止频率取30Hz效果比较均衡。// AS5600读取与滤波示例 #include Wire.h #define AS5600_ADDR 0x36 float readAngle() { Wire.beginTransmission(AS5600_ADDR); Wire.write(0x0E); // 角度寄存器高字节 Wire.endTransmission(); Wire.requestFrom(AS5600_ADDR, 2); int raw (Wire.read() 8) | Wire.read(); return raw * 360.0 / 4096.0; } // 滑动平均 一阶低通 float filterAngle(float newAngle) { static float buf[8] {0}; static int idx 0; static float lastFiltered 0; buf[idx] newAngle; idx (idx 1) % 8; float sum 0; for (int i 0; i 8; i) sum buf[i]; float avg sum / 8.0; float alpha 0.3; // 低通系数 lastFiltered alpha * avg (1 - alpha) * lastFiltered; return lastFiltered; }3. PID控制器设计位置式还是增量式前馈怎么加3.1 位置式PID和增量式PID在舵机场景下的选择位置式PID输出的是控制量的绝对值增量式输出的是控制量的增量。对于舵机控制我推荐位置式因为舵机的PWM占空比和角度之间是线性关系位置式直接输出PWM比较直观。增量式更适合步进电机或需要积分抗饱和的场景。位置式PID的公式是u(t) Kp * e(t) Ki * ∫e(t)dt Kd * de(t)/dt。其中e(t)是目标角度减去实际角度。输出u(t)映射到PWM脉宽比如0.5ms到2.5ms对应0°到180°。但这里有个问题舵机内部已经有自己的控制器了你在外面再加PID相当于两个控制器串联。如果参数没调好两个控制器会互相打架表现为低频振荡。解决办法是把舵机当成一个功率放大器来用而不是位置伺服。具体做法是给舵机发送一个固定的PWM脉宽让它内部的控制器失效或者把舵机改成连续旋转模式然后你直接用PID输出控制电机的电压或电流。但MG995/MG996R改连续旋转比较麻烦需要拆开调电位器。更简单的办法是保留舵机内部控制器但把PID的输出叠加到PWM脉宽上相当于给舵机一个修正量。3.2 前馈补偿让PID不用那么累纯PID是反馈控制意思是误差出现了才动作。但舵机在换向或加速时误差会突然变大PID需要时间响应。前馈控制的思想是根据目标角度的变化率提前给出一个控制量让系统更快跟上。对于舵机前馈量可以取目标角速度乘以一个系数直接加到PWM输出上。前馈系数的整定方法是先不加前馈调好PID参数然后给一个斜坡目标信号观察实际角度滞后多少逐步增大前馈系数直到滞后明显减小但不振荡。实测下来前馈系数取0.3到0.5之间效果比较好。前馈的好处是它不依赖反馈所以不会引入稳定性问题只是帮忙推一把。// 带前馈的位置式PID float pidWithFeedforward(float target, float actual, float dt) { static float integral 0; static float lastError 0; static float lastTarget 0; float error target - actual; integral error * dt; float derivative (error - lastError) / dt; // 前馈项目标角速度乘以系数 float targetVel (target - lastTarget) / dt; float feedforward 0.4 * targetVel; float output Kp * error Ki * integral Kd * derivative feedforward; lastError error; lastTarget target; return output; }3.3 积分抗饱和舵机堵转时的保护舵机堵转时误差一直存在积分项会不断累积导致输出饱和。等堵转解除后积分项需要很长时间才能降下来表现为舵机发呆或者猛冲。解决办法是积分抗饱和当输出达到上限或下限时停止积分累积。或者用反计算法把饱和量反馈回去。对于舵机控制我通常设置积分限幅比如积分项最大不超过输出范围的30%。另外当误差符号和积分符号相反时清零积分。这两个措施能有效防止积分饱和。实测中如果不加抗饱和舵机堵转2秒后恢复时间需要5秒以上加了之后恢复时间在0.5秒以内。4. 参数整定从振荡到稳定的完整调试链路4.1 先调Kp找到临界振荡点整定PID参数我习惯从Kp开始。把Ki和Kd设为0给舵机一个阶跃信号比如从0°跳到90°观察响应。Kp太小响应慢到位时间长Kp太大出现振荡甚至等幅振荡。逐步增大Kp直到出现等幅振荡记录此时的Kp为Ku振荡周期为Tu。对于MG996R加AS5600的方案我实测的Ku大概在8到12之间Tu在0.15秒到0.25秒之间。然后根据Ziegler-Nichols方法Kp取0.6KuKi取2Kp/TuKd取Kp*Tu/8。但Z-N方法偏激进实际调试时我会把Kp再降20%Ki降30%Kd保持不变。这样响应稍慢但超调小稳定性好。4.2 再调Kd抑制过冲和振荡Kd的作用是预测误差变化趋势提前刹车。Kd太小过冲大Kd太大对噪声敏感舵机会抖动。调试时先给一个阶跃信号观察过冲量。如果过冲超过10%增大Kd如果舵机出现高频抖动减小Kd。注意Kd对噪声敏感所以滤波一定要做好。如果滤波后还是有抖动可以在微分项上加一个低通滤波截止频率取采样频率的1/10。我踩过的一个坑是Kd调得太大舵机在目标位置附近高频抖动声音像蜜蜂。后来发现是AS5600的噪声没滤干净微分项把噪声放大了。加了滑动平均后Kd可以调得更大响应也更快。4.3 最后调Ki消除稳态误差Ki的作用是消除稳态误差。但舵机内部有死区外部PID的积分项需要足够大才能克服死区。调试时给一个固定目标角度观察实际角度和目标的差值。如果差值不为零缓慢增大Ki直到差值消失。但Ki太大会导致低频振荡表现为舵机在目标位置附近缓慢来回摆动。我的经验是Ki取Kp的1/100到1/50之间。如果舵机负载变化大Ki可以取大一点如果负载稳定Ki取小一点。另外积分项一定要限幅防止饱和。参数作用太小太大推荐范围Kp响应速度响应慢振荡5-10Ki消除稳态误差有静差低频振荡0.05-0.2Kd抑制过冲过冲大高频抖动0.1-0.54.4 实测曲线与问题排查调好参数后用串口把目标角度和实际角度打印出来画成曲线。正常的阶跃响应应该是快速上升轻微过冲然后稳定在目标值。如果曲线出现等幅振荡说明Kp太大或Kd太小如果曲线上升太慢说明Kp太小如果曲线有稳态误差说明Ki太小。我遇到过一个奇怪现象舵机在小角度范围内比如0°到30°响应正常但大角度范围比如0°到180°出现振荡。后来发现是舵机内部电位器的线性度问题在大角度时反馈电压非线性导致内部控制器增益变化。解决办法是分段整定PID参数或者用查表法补偿非线性。5. Arduino和STM32上的代码实现差异5.1 Arduino版本适合快速验证Arduino的Servo库用起来简单但它的PWM更新频率是50Hz分辨率有限。如果你用Servo.write()它直接输出固定脉宽你没法叠加PID输出。所以需要绕过Servo库直接用analogWrite()或者定时器输出PWM。但Arduino Uno的analogWrite()频率是490Hz或980Hz不是50Hz直接驱动舵机会有问题。正确做法是用Timer1库或者手动配置定时器输出50Hz的PWM脉宽可调。下面是一个基于TimerOne库的例子#include TimerOne.h #include Wire.h #define SERVO_PIN 9 #define AS5600_ADDR 0x36 float targetAngle 90.0; float Kp 8.0, Ki 0.1, Kd 0.3; float integral 0, lastError 0; void setup() { Wire.begin(); Timer1.initialize(20000); // 20ms周期50Hz Timer1.pwm(SERVO_PIN, 1500); // 初始1.5ms对应90° } void loop() { float actual filterAngle(readAngle()); float error targetAngle - actual; integral error * 0.02; // dt20ms integral constrain(integral, -50, 50); float derivative (error - lastError) / 0.02; float output Kp * error Ki * integral Kd * derivative; lastError error; int pulseWidth 1500 output; // 1.5ms为中心 pulseWidth constrain(pulseWidth, 500, 2500); Timer1.pwm(SERVO_PIN, pulseWidth); delay(20); }5.2 STM32 HAL版本适合产品化STM32的定时器资源丰富可以输出高精度PWM。用HAL库配置TIM1的通道1输出PWM频率50Hz分辨率1μs。然后开一个定时器中断比如1kHz在中断里做PID计算和PWM更新。注意PID计算和PWM更新要放在同一个中断里保证时序一致。// STM32 HAL PID中断服务函数 void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim htim2) { // 1kHz中断 float actual filterAngle(readAS5600()); float error targetAngle - actual; integral error * 0.001; integral constrain(integral, -50, 50); float derivative (error - lastError) / 0.001; float output Kp * error Ki * integral Kd * derivative; lastError error; int pulse 1500 (int)output; pulse constrain(pulse, 500, 2500); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, pulse); } }STM32版本的优势是中断优先级可配可以把PID中断设为高优先级I2C读取设为低优先级避免I2C阻塞影响控制周期。另外STM32的浮点运算单元可以加速PID计算1kHz中断下CPU占用率不到5%。5.3 两个版本的性能对比Arduino Uno的16MHz主频做浮点PID计算大概需要50μs加上I2C读取1ms控制周期只能做到2ms左右也就是500Hz。STM32F103的72MHz主频浮点计算10μsI2C用DMA可以做到100μs控制周期可以做到1ms甚至更快。对于舵机这种响应速度在100ms级别的系统500Hz和1kHz的控制周期差别不大但STM32的余量更大可以跑更复杂的算法比如模糊PID或级联PID。平台主频控制周期浮点运算适用场景Arduino Uno16MHz2ms软件模拟快速验证STM32F10372MHz1ms硬件FPU产品化ESP32240MHz0.5ms硬件FPU无线控制6. 进阶玩法级联PID和模糊PID在舵机上的应用6.1 级联PID位置环速度环单环PID只控制位置输出直接给PWM。级联PID在位置环外面再加一个速度环位置环的输出作为速度环的目标值速度环的输出给PWM。速度环的反馈来自角度传感器的差分或者直接用电机的反电动势。级联PID的好处是抗负载扰动能力更强因为速度环可以快速响应负载变化。对于舵机速度环的反馈可以用角度差分得到但噪声大需要滤波。我试过在STM32上实现级联PID位置环Kp6速度环Kp2Ki0.5。效果是负载突变时角度波动从5°降到1°以内。但调试难度也翻倍两个环的参数互相影响需要先调内环再调外环。6.2 模糊PID自适应参数调整模糊PID根据误差和误差变化率实时调整Kp、Ki、Kd。对于舵机这种非线性系统模糊PID可以适应不同负载和角度范围。实现方法是定义误差和误差变化率的模糊集比如负大、负小、零、正小、正大然后建一个规则表根据当前误差查表调整PID参数。我实测过模糊PID在MG996R上的效果空载时和普通PID差不多但加载后普通PID需要重新整定模糊PID可以自动适应。缺点是规则表需要调试而且计算量大Arduino跑不动至少需要STM32F4级别。6.3 什么时候该收手别为了PID而PID我见过很多项目明明标准PWM就够了非要上PID结果调试花了一周效果还不如开环。判断标准是如果开环控制下系统误差在应用允许范围内且动态响应满足要求就不要上PID。PID是解决特定问题的工具不是万能药。另外如果舵机本身精度不够比如齿轮间隙太大PID也救不了。这时候换舵机比调PID更有效。提示在决定上PID之前先测量开环控制下的重复定位精度和响应时间。如果这两个指标已经满足要求直接开环控制省时省力。7. 调试工具和实用技巧7.1 用串口绘图工具实时观察响应曲线Arduino IDE自带的串口绘图工具只能看单条曲线不够用。我推荐用SerialPlot或者Better Serial Plotter可以同时画多条曲线支持缩放和暂停。把目标角度、实际角度、PID输出都打印出来一眼就能看出问题。打印格式用target:90.0,actual:88.5,output:120工具会自动解析。STM32的话可以用STM32CubeMonitor支持实时变量监控和绘图。或者简单点用printf重定向到串口配合VOFA上位机效果也很好。7.2 参数整定的自动化尝试手动整定PID参数很耗时我试过用简单的梯度下降自动整定给一个阶跃信号计算超调量和调节时间然后根据规则调整参数重复几次。但舵机系统有噪声和死区自动整定效果不稳定。后来我改用matlab pid tuner把舵机的传递函数辨识出来在Simulink里仿真整定再把参数移植到单片机。辨识传递函数的方法是给舵机一个伪随机信号记录输入输出用系统辨识工具箱拟合。这个方法适合有MATLAB license的团队。7.3 电源和接地的坑MG995/MG996R堵转电流能到2.5A如果电源功率不够电压会跌落导致舵机内部控制器复位或振荡。我遇到过用USB供电舵机一转就重启的情况。解决办法是单独给舵机供电电源至少5V 3A并且和单片机共地。另外PWM信号线要远离电机线避免干扰。如果舵机抖动严重可以在电源端并一个1000μF的电解电容。注意舵机电源和单片机电源一定要共地否则PWM信号没有参考电平舵机会乱转。8. 从单舵机到多舵机机械臂场景的扩展8.1 多舵机控制的资源分配机械臂通常有4到6个舵机如果每个舵机都加PID和角度传感器单片机的资源会紧张。Arduino Uno只有一个硬件I2C多个AS5600需要I2C多路复用器比如TCA9548A。STM32有多个I2C可以分担。另外PWM通道也要够用STM32的每个定时器有4个通道两个定时器就能驱动8个舵机。控制周期方面如果每个舵机都跑1kHz PID6个舵机就是6kHz的计算量。STM32F103跑6个浮点PID大概占用30% CPU还可以接受。如果不够可以降低控制频率到500Hz或者用定点运算。8.2 关节联动和轨迹规划机械臂的多个关节需要联动才能让末端执行器走直线或圆弧。这时候每个关节的目标角度不是固定的而是根据轨迹规划实时计算出来的。PID控制器需要快速跟踪这些变化的目标。前馈控制在这里特别有用因为轨迹规划已经给出了每个时刻的目标角速度直接前馈给PID可以大幅减小跟踪误差。我做过一个三自由度机械臂末端走直线时如果只用PID跟踪误差在2mm左右加了前馈后误差降到0.5mm以内。轨迹规划用五次多项式插值保证加速度连续避免舵机冲击。8.3 总线舵机的对比如果你要做复杂的机械臂可以考虑总线舵机比如飞特舵机。总线舵机内部有更高精度的编码器和更好的控制器支持位置、速度、电流多种控制模式通过串口或CAN总线通信。缺点是价格贵一个总线舵机能买十个MG996R。但对于需要高精度、多关节协同的场景总线舵机的开发效率更高不需要自己加传感器和调PID。我的建议是如果预算有限用MG996R加外部PID适合学习和中小型项目如果预算充足直接上总线舵机省下的调试时间更值钱。9. 我踩过的几个典型坑和解决办法9.1 舵机自锁导致的积分饱和有一次做机械臂抓取舵机堵转后PID积分项累积到上限松开后舵机猛冲差点把机械臂撞坏。后来加了积分抗饱和并且检测电流电流超过阈值时清零积分。电流检测用INA219或者简单的采样电阻成本不高但能有效保护机械结构。9.2 AS5600磁铁脱落导致失控AS5600的磁铁用胶水粘在输出轴上夏天高温后胶水软化磁铁脱落PID失去反馈舵机全速旋转。后来改用机械卡扣固定磁铁并且加了磁铁脱落检测如果角度读数长时间不变而PWM输出在变化就判定传感器故障切换到开环控制并报警。9.3 电源纹波导致的随机抖动用开关电源给舵机供电时PWM频率和开关频率产生拍频导致舵机随机抖动。后来在电源端加了LC滤波并且把PWM频率从50Hz改成300Hz舵机支持宽频率PWM避开了开关频率的谐波。注意不是所有舵机都支持300HzMG996R实测可以但有些舵机只能50Hz。9.4 参数整定时的假稳定有时候PID参数看起来稳定了但用手轻轻拨一下舵机臂它就振荡起来。这是因为参数是在空载下整定的加载后系统增益变化。解决办法是加载整定或者留足够的稳定裕度。我通常会把Kp再降10%牺牲一点响应速度换取更好的鲁棒性。10. 写在最后一些个人体会舵机控制这件事入门容易精通难。MG995和MG996R作为入门级舵机本身精度有限但通过外部PID和合适的传感器完全能满足大多数中小型项目的需求。关键是要理解PID不是魔法它只是把开环系统变成闭环系统用反馈来修正误差。如果机械结构本身有间隙、传感器安装有偏差、电源有噪声PID也救不了。我在实际项目中的体会是先把机械和电气基础做好再调PID。机械同轴度、传感器安装、电源质量这三样做好了PID参数随便调调就能稳定。反过来如果这三样有问题PID调死也白搭。另外不要迷信自动整定工具手动整定虽然慢但能让你更理解系统的特性。最后分享一个小技巧调PID时先用一个固定的目标角度比如90°观察稳态误差。如果稳态误差大先调Ki如果过冲大先调Kd如果响应慢先调Kp。每次只调一个参数调完观察效果再调下一个。这样虽然慢但不会乱。等有经验了可以三个参数一起调但新手千万别这么干。