说实话我真正把根轨迹法从课本里变成自己手里的工具不是考试前背公式那会儿而是有一次在舵机位置控制项目里被震荡折腾了整整两天之后。当时我用纯比例控制写了一个位置环Kp从小往大试输出要么到位慢、跟不上目标要么刚到位就开始来回抖眼看就是一个停不下来的极限环。凭经验凑参数也能把程序跑起来但我总觉得不踏实为什么这个增益是2.6而不是4.0换一台舵机同样的参数为什么就不及格后来我冷静下来把舵机系统拆成传递函数画根轨迹算稳定性边界和阻尼比再回头去写PD控制代码整个过程突然就顺了跟摸黑调参完全不是一回事。这篇想围绕“从控制理论的根轨迹法和稳定性分析谈到舵机PD控制代码实现”这条线把我当时的建模、分析、设计和代码落地整个链条捋一遍。我不打算复述教科书而是聊一个真实的舵机系统是怎么被“翻译”成数学对象又是怎么从根轨迹的结论里得出Kp、Kd的取值最后落到一段可以直接跑的C/C代码里。适合正在调伺服电机、对PID只有“公式级理解”、想弄明白参数背后物理意义的开发者。1. 先把“控制对象”翻译成传递函数1.1 舵机的物理结构是怎么简化成数学模型的很多人一提到舵机想到的是航模上那种1ms到2ms的PWM舵机。要先把一个口径对齐那种舵机内部已经做完了位置闭环你给PWM宽度本质是给了一个角度目标并不是直接给电机输入电压。如果我们自己想写PD控制代码更常见的背景是另一类系统——电机加减速器再加一个电位器或编码器没有内置完整位置环或者你拿到了一个舵机总成但想绕过原控制板自己来做位置环。这两种情况的模型基本通用只是参数不同。无论哪种情况控制对象的核心都是一台直流电机。电枢回路的电压方程是u(t) L·di/dt R·i(t) Ke·ω(t)机械侧的转动方程是J·dω/dt Km·i(t) - B·ω(t) - τL这里u是电枢电压i是电枢电流ω是电机转速J是折算到电机轴上的总转动惯量B是粘性阻尼系数Ke是反电动势系数Km是电磁转矩系数τL是外部负载转矩。如果把这个方程组拉普拉斯变换、消去电流会得到一个从电压到转速的二阶动态模型。实际工程里绝大多数小型直流电机的电气时间常数L/R只有毫秒甚至更低而机械时间常数J/B往往在几十到几百毫秒量级。电气动态比机械动态快得多可以在控制带宽范围内把L忽略掉于是“电压→转速”降成一阶惯性环节转速再积分一次就得到角度G(s) Θ(s) / U(s) Km / [s·(τm·s 1)]这里的τm是机械时间常数Km是等效增益。这个模型看起来简单但足够用来做根轨迹分析和PD参数设计。它不是完整真理但它抓住了一个位置伺服系统最主要的两个特征惯性滞后特性和积分特性。1.2 一组能直接拿来分析的真实参数我在一个具体项目里拿到过这样一组等效参数机械时间常数τm 0.2s从控制电压到输出轴角度的等效增益Km ≈ 50。这里的单位口径是控制电压归一化为0~1输出角度用弧度表示。于是被控对象写成P(s) 50 / [s·(0.2s 1)]开环极点是 s 0 和 s -5。s 0来自角度到位置的积分表示系统本身是I型系统对阶跃位置输入理论上没有稳态误差s -5代表电机和负载的惯性滞后。参数的来源完全来自实验给电机一个固定阶跃电压记录输出轴的速度上升曲线找到速度到达稳态值63.2%的时间点就得到了τm稳态速度除以输入电压就得到等效增益Km。这个过程最好在开环状态做也就是不接任何反馈控制直接驱动输出。如果做不了开环实验用扫频或系统辨识也能凑合但阶跃法最简单现场十分钟就能拿到能用的近似值。这里有个特别容易踩的坑传感器和输出的量纲如果不统一后续所有计算都会乱掉。比如角度用弧度算但编码器输出的是原始计数值电压用PWM百分比算但代码里写的是0~255的整数值。这些单位不一致会让理论计算出来的Kp和Kd跟实际运行完全对不上。我习惯在开始分析前先把所有信号统一到一个口径里角度换算成弧度控制量归一化到0~1再写进传递函数。2. 根轨迹法不猜参数先让根“走”给你看2.1 根轨迹的基本规则和手工绘制要点根轨迹法解决的核心问题是当某个参数通常是增益K从零变到无穷大的时候闭环极点是怎么移动的。闭环极点的位置直接决定了系统动态行为实部决定衰减速度虚部决定振荡频率。所以只要在复平面上画出极点的轨迹增益变化带来的趋势就一目了然。对于单位反馈系统闭环特征方程是1 K·C(s)·P(s) 0根轨迹上的点既满足幅值条件也满足相角条件。手工画图时只需要记住几条规则根轨迹从开环极点出发终止于开环零点或无穷远处实轴上的区段右侧开环极点和零点总数为奇数时这个区段就是根轨迹分支数等于开环极点数对称于实轴分支数减去零点数后多余的分支沿着渐近线趋向无穷远渐近线与实轴的交点和夹角都有公式。对于实际项目我不太建议每次都纯手工精确绘制但规则务必理解。因为用计算机画图很容易难的是画完之后你能读懂它。看根轨迹时我主要看三个东西极点往左还是往右移阻尼比够不够增益再往上加会不会穿过虚轴。2.2 纯比例控制下的根轨迹暴露了什么先考虑最朴素的控制方案只有比例项 C(s) Kp。被控对象是 P(s) 50/[s(0.2s1)]开环传递函数就是L(s) 50Kp / [s·(0.2s 1)]闭环特征方程0.2s² s 50Kp 0归一化后s² 5s 250Kp 0开环极点两个0和-5。根轨迹从这两个极点出发随着Kp增大两个实根先沿着实轴相向而行在s -2.5处相遇然后离开实轴变成一对共轭复根。我算了一下临界阻尼点判别式等于零时 Kp 0.025。超过这个值系统就会产生超调。如果给系统提出阻尼比约束比如要求ζ不低于0.7那么根必须落在等阻尼比射线约束的扇形区域内。对纯比例控制这个约束直接换算成增益上限250Kp ≤ (5/1.4)² ≈ 12.75 Kp ≤ 0.051增益一旦超过0.051阻尼比就会跌破0.7输出出现明显超调。而在这个增益下闭环自然频率只有大约3.57 rad/s响应速度很慢。想通过加大Kp提高响应速度阻尼比又直线下降。这就是根轨迹法最直观的价值它把“快”和“稳”这两个指标之间的矛盾摆在明面上让人知道单靠比例项根本不可能同时满足。纯比例不是完全不稳定无论Kp多大两个闭环根的实部始终是-2.5不会越过虚轴。这也就是说从稳定性边界这个角度看系统在连续域里是稳定的。但工程上真正的威胁不是数学上的“不稳定”而是设计指标达不到速度想要更高增益必须更大增益一大超调率就飙到50%以上这在位置控制里根本不能用。3. PD控制的本质用微分给系统补“阻尼”3.1 比例项和微分项各干了什么比例项的工作是误差越大输出越大本质上是一个位置弹簧把系统往目标位置拉。但一个没有阻尼的弹簧会来回震荡这就是为什么纯比例增益调大之后舵机抖得厉害。微分项在PD里工作方式是输出的一部分正比于误差的变化速率相当于系统里多了一个粘性阻尼器。用开车的类比来理解比例项就像你盯着前车距离距离越近刹车踩得越大力微分项则是你感受到自己在靠近的速度只要还有趋近速度就会提前收油门甚至轻带刹车。比例项处理“现在差多少”微分项处理“现在正在以多快趋势逼近”天然提供预测性。从频域看微分项在高频处提升幅值同时提供相位超前这正好弥补了被控对象积分环节带来的相位滞后。相位裕度提高了增益就可以加得更高而不至于震荡。3.2 PD控制器如何把根轨迹“掰”回左半平面PD控制器的传递函数是C(s) Kp Kd·s Kp·(1 Td·s)其中Td Kd/Kp并联一个微分项等价于在开环传递函数中增加了一个零点。被控对象原本两个极点(0和-5)只有一条到无穷远的根轨迹分支现在因为多了一个零点根轨迹走向被重新分配闭环极点就不再受“增益越大阻尼越小”的锁死限制。把PD代入前面的模型闭环特征方程变成0.2s² (1 50Kd)s 50Kp 0对比标准二阶系统特征方程s² 2ζωn·s ωn² 0归一化后可以得到ζ (5 250Kd) / (2√(250Kp)) ωn √(250Kp)可以看到Kd进入了阻尼项但基本不影响自然频率。这意味着PD可以让“快”和“稳”解耦Kp主要定响应速度Kd定阻尼程度。这一步是整个设计的核心思想你可以先指定想要的ωn和ζ然后反解Kp和Kd而不是靠运气盲调。比如我这套系统设计目标取 ωn 12 rad/s、ζ 0.8代入公式Kp ωn² / 250 144 / 250 0.576 Kd (2ζωn - 5) / 250 (19.2 - 5) / 250 ≈ 0.0568如果只用比例控制去满足同样的自然频率12阻尼比只有大约0.21超调会超过50%。加上微分项之后同样的响应速度下超调被压到1%出头收敛时间也短得多。这就是“PD解决纯P死结”的完整逻辑。3.3 从理论值到代码前的最后一次单位换算上面算出的Kp和Kd默认的控制量是归一化电压(0~1)反馈量是弧度。如果你的代码里角度来自ADC原始值比如电位器采样输出0~4095或者PWM输出写的是0~255的整数那么Kp、Kd必须按比例缩放。具体做法是先确定满量程输出对应的物理量。假设ADC满量程4096对应角度π弧度PWM满量程255对应归一化电压1那么代码里的Kp_code Kp × (255/1) ÷ (4096/π)Kd_code也是类似换算。这个“单位标定”在纯手工调试中经常被忽略结果就是理论值算出来非常漂亮一上电却完全不匹配浪费半天时间才发现只是scale问题。4. 舵机PD控制代码实现离散域里的关键细节4.1 误差微分还是测量微分这里有个大坑理论上PD可以写作u Kp·e Kd·de/dt离散化以后de/dt最简单的写法是de/dt ≈ (e[k] - e[k-1]) / Ts但这里的细节很关键。如果目标位置本身是个阶跃量e[k]会产生一个瞬间跳变直接用误差差分计算导数会得到很大的尖峰脉冲也就是常见的“微分冲击”。实际效果是舵机一收到新目标就猛冲一下很容易触发PWM饱和甚至把机械结构撞出异响。更稳妥的做法是让微分项作用于测量值而不是误差也就是测量位置差分得到角速度再乘上负号。我习惯写成u Kp·e - Kd·dθ/dt原因是目标位置通常变化不频繁而传感器测量值的连续性更好这样既避免了微分冲击实际产生的阻尼效果也完全一致只是实现上稍微拐了一个弯。4.2 一套可以直接落地的C/C代码下面这段代码是我在嵌入式环境里常用的位置环PD控制器封装。它包含了测量微分、一阶低通滤波、输出限幅基本就是前面模型分析和参数换算的代码化。// servo_pd.h #pragma once #include stdint.h class ServoPD { public: ServoPD() {} // 初始化传入Kp、Kd、采样周期Ts(秒) void init(float kp, float kd, float ts) { Kp_ kp; Kd_ kd; Ts_ ts; prev_pos_ 0.0f; filt_vel_ 0.0f; out_max_ 1.0f; // 默认输出限幅为归一化0~1可自行修改 output_ 0.0f; } void setOutputLimit(float limit) { out_max_ limit; } // 每Ts调用一次输入目标角度和当前角度返回控制量 float compute(float target, float measured) { float err target - measured; // 角度误差 // 用测量位置差分近似角速度并做一阶低通滤波 float raw_vel (measured - prev_pos_) / Ts_; filt_vel_ 0.8f * filt_vel_ 0.2f * raw_vel; prev_pos_ measured; // PD控制律比例项作用在误差上微分项作用在测量速度上 float out Kp_ * err - Kd_ * filt_vel_; // 输出限幅防止PWM越界 if (out out_max_) out out_max_; if (out -out_max_) out -out_max_; output_ out; return output_; } private: float Kp_; float Kd_; float Ts_; float prev_pos_; float filt_vel_; float out_max_; float output_; };调用方式在固定周期的定时中断里执行。示例// 假设1kHz定时中断 void timerISR() { float target 0.5f; // 目标角度单位弧度 float measured encoder_to_rad(); // 编码器读数换算成弧度 float u servo_pd.compute(target, measured); set_pwm_duty(u); // u归一化到-1.0~1.0 }有一点必须强调这个compute必须在固定采样周期里调用。如果你放在主循环里循环时间随代码路径变化微分项的分母Ts就不准速度估计会出现周期性波动系统稳定性大打折扣。硬件定时器中断是首选。4.3 参数整定的实际操作顺序拿到理论Kp和Kd之后我一般不会直接按计算值上电而是分四步第一步先用最小的Kp验证方向和反馈极性。正反馈会让舵机越来越偏遇到舵机往一个方向猛打甚至堵转先检查误差符号别急着调参。第二步Kp从理论值的一半开始Kd先给理论值。观察阶跃响应如果超调量大优先增加Kd而不是急着降Kp。因为降Kp虽然也能减少超调但会牺牲响应速度。第三步如果响应慢、到位拖泥带水在保持阻尼比不劣化的前提下增加Kp同时按比例微调Kd。这个过程中理论公式的价值就体现出来了你知道目标ωn在哪里也知道当前ζ大概是多少而不是纯粹靠试。第四步跑到目标附近还残留小幅抖动时检查输出和反馈的量化噪声不要盲目调参。噪声引起的抖动和真震荡往往长得一样处理方式完全相反。整个过程的核心原则是理论参数只是起点但它把起点放在了一个离最优解很近的地方现场微调的量很小。5. 实战调试现场踩过的坑和排查清单5.1 微分噪声数字PD最容易翻车的地方量化反馈碰到微分项噪声问题会被放大很多倍。电位器或编码器的ADC读数最低位跳几个数本身对位置影响可以忽略但除以Ts之后变成了速度估计的高频毛刺再乘上Kd输出扭矩就跟着抖。位置看起来稳电机却在微微发抖时间长了还会发热。我的做法是给速度估计加一阶低通滤波。上面的代码里已经写了filt_vel_ 0.8f * filt_vel_ 0.2f * raw_vel;滤波系数0.8对应的时间常数大约是0.8×Ts/(0.2) 4个采样周期这个滤波极点最好放在控制带宽的4倍以上否则会引入过多相位滞后降低系统的稳定性裕度。如果发现滤波后阻尼效果变差就把系数往0.9调同时稍微加大Kd让有效阻尼补回来。另一个更隐蔽的问题如果微分项直接作用于误差目标位置每次变化都产生一个阶跃导数尖峰会让执行器瞬间饱和。把微分作用在测量位置上是更干净的方案这也是我上面代码采用测量微分的根本原因。5.2 输出饱和、死区与静差PD输出限幅之后如果误差很大执行器会长期处于饱和状态。饱和本身不是大事它会自然形成“先全速跑、接近目标再减速”的Bang-Bang式控制实际效果反而更快。但如果Kp不够、Kd过大系统可能陷入死区边缘的极限环表现为到位后偶尔抖一下、停一下、再抖一下。摩擦和齿轮死区是另一个隐藏凶手。舵机减速机构里不可避免有间隙小误差产生的输出扭矩可能推不动输出轴形成明显的静差。PD控制里没有积分项对恒定静差天生无能为力。想解决静差可以考虑加前馈补偿或者引入积分项。加积分时要注意抗积分饱和尤其是输出限幅为±1时积分值该限幅也限幅不然下一次换向会有一大段滞后。单纯为了消静差盲目加大Kp和Kd会带来硬碰硬的效果系统变成刚性极强任何一点外部扰动都会被放大成尖锐抖动机械寿命也会缩短。这种情况我宁可使用前馈也不愿意把增益推到极限。5.3 采样间隔抖动看得到却最容易被忽略前面说了固定采样周期的重要性实际项目中我第一次调试时就在主循环里调了ServoPD结果现象很奇怪Kp、Kd按理论设好看波形却一直有随机的高频抖动。Waves让我看了半天后来意识到主循环里还有其他任务循环时间在5ms到15ms之间波动微分项的Ts代入了固定采样值实际速度估计误差被拉大了好几倍。解决办法是回到定时器中断把控制律放在固定1kHz或2kHz的中断里执行中断里只做最轻量的计算和传感器读取不做日志打印、不做动态内存分配。日志通过DMA或者环形缓冲区异步输出避免阻塞控制周期。这一点听上去不像控制问题但它造成的现象足以让你误判整套PD参数。5.4 故障速查表现象可能原因排查方向舵机高频颤抖位置勉强稳住测量噪声被微分放大检查速度估计滤波系数降低Kd到位后大幅来回振荡Kp过高或Kd不足按ζ公式补算Kd优先加Kd响应慢到达目标半天Kp不足或Kd偏大适当加Kp确认Kd没有过度抑制目标阶跃后瞬间猛冲微分作用于误差产生微分冲击改为测量位置微分静止时有小幅度间歇抖动死区极限环、摩擦或齿轮间隙检查输出是否处于临界饱和考虑死区补偿曲线看起来OK但是角度对不准传动间隙或输出量纲不一致检查编码器换算和单位标定这些现象在项目里几乎一个个都遇到过。有一个经验很值得分享遇到任何奇怪的振荡先不要急着动Kp、Kd第一件事是打印采样周期Ts的实际值并且记录一段时间看看它是否稳定。如果Ts抖动所有基于Ts的微分项都不可信调参就变成碰运气。最后聊两句从头到尾走完一遍我最大的感受是根轨迹法不是课堂上的古董它和PD代码之间有一条非常短的逻辑线。写一个PD控制器核心是选两个参数但这两个参数背后牵涉到被控对象的极点和零点、自然频率、阻尼比、稳定性裕度。不建模、不画根轨迹当然也能调出一版能用的参数但那是靠试错凑出来的建模、画根轨迹之后调参是有方向感的你知道参数边界在哪里也知道改Kp和Kd分别会改变哪个闭环指标。最后分享一个小技巧调试PD之前先把采样时间和数据单位这两种信息打印出来看一遍。我踩过好几次坑之后发现大多数问题根本不是PID本身不好而是采样周期不稳定、单位没统一这两种最不起眼的原因。把这两个口径校准了理论计算出来的Kp、Kd离最终能用值之间往往只差一次很小的现场微调。
