伺服系统这个东西，纸上谈兵和真正让它稳稳停在±1个脉冲以内，中间隔着的不是算法，而是一整套关于时间、噪声和机械的工程妥协。我最早接触这类项目时，也以为难点在控制理论，后来才发现课本上的传递函数早就写好了，真正让人熬夜的是：中断晚到了20微秒、编码器反馈里混进了量化噪声、机械在某个速度点突然开始啸叫、上位机下发的指令和底层插补周期差半个节拍。这篇内容就是把这些年从零搭一套高精度伺服控制系统时，在C++这一层踩过的路重新走一遍——从环路结构怎么划分，到控制器里那段1kHz中断服务程序怎么写，从S曲线怎么算，到整定时怎么从一条阶跃响应曲线里读出系统的“脾气”。适合已经懂一点PID、手上有一台电机和驱动器、想把精度真正做上去的读者，也适合做机器人、数控、精密平台这类需要底层运动控制的朋友对照着看。我不打算复述教科书，重点放在那些文档里不会写、但决定成败的细节上。

1. 伺服系统的三层环路与“底层”这个词的真实含义

很多人一说“底层实现”，脑子里浮现的是直接操作寄存器、写汇编。这在伺服系统里只对了一半。真正决定一套伺服系统精度的“底层”，是三层控制环路的时间结构，以及你在每一层里塞进去的代码能不能在确定的时刻跑完。理解这一点，后面的所有C++代码写法才有依据。

1.1 电流环、速度环、位置环的带宽递降关系

伺服系统的经典结构是三层嵌套：最内层电流环，中间速度环，最外层位置环。这三层的采样频率和带宽不是随便定的，遵循一个朴素的工程原则——外层带宽要比内层低一个数量级左右，通常取1/5到1/10。原因很直白：外层的输出是内层的指令，如果外层变化比内层响应还快，内层根本追不上，系统就会震荡。

按常见配置，电流环采样在10kHz到16kHz，对应带宽1到3kHz；速度环采样在1kHz到4kHz，带宽100到300Hz；位置环采样500Hz到1kHz，带宽20到80Hz。这里有个容易被忽略的点：带宽不等于采样率，采样率只是奈奎斯特下限的保障，实际带宽受滤波器、延迟、机械惯量共同约束。我在一个精密平台上试过把位置环采样从1kHz提到2kHz，理论上响应更快，实测跟随误差反而变大了，原因是速度环的指令更新变密，而速度环自身的滤波延迟没变，两层之间的相位关系被打乱。

所以做C++实现时，第一件事不是写PID，而是把三个环的节拍定下来，画一张时间轴：哪个时刻采样编码器、哪个时刻算位置环、哪个时刻把电流指令写进PWM比较寄存器。这张时间轴一旦确定，代码结构基本就固定了。

1.2 C++能不能进环路：一笔算力预算的账

经常有人问，控制环路里用C++靠不靠谱。答案是：语言本身不决定实时性，运行时行为才决定。C++在伺服控制器里能不能用，取决于你能不能保证每次执行的指令数在一个可预测的上界内。

算一笔账。假设主控是一颗跑在400MHz的处理器，带浮点单元，单精度浮点乘加大约2到3个时钟周期。位置环周期1ms，可用时间假设只有200微秒（留出余量给其他任务），按400MHz算就是8万个时钟周期。一个位置环的运算量大概是多少？一次编码器读数转换、一次差分求速度、一次PID、一次前馈计算、一次限幅、一次指令打包，粗算下来2000到5000个时钟周期的量级。也就是说算力上绰绰有余，真正稀缺的是确定性。

不确定性的来源有三个：动态内存分配、异常处理与虚拟调用的不可预测路径、以及操作系统调度带来的抢占。我在项目里定的规矩是——环路内代码禁止new/delete，禁止std::function，禁止抛异常，禁止任何可能触发锁竞争的操作。这几条一旦立住，C++写环路完全没问题，而且比C更容易做出清晰的结构。

一个实用的自检方法：把环路函数编译出来，用反汇编看最内层循环的指令数，同时检查是否有对malloc、__cxa_throw、pthread_mutex_lock的调用。有就说明你的约束没落实。

1.3 控制系统里最贵的资源不是算力而是抖动

同一套代码，跑在裸机上和跑在带调度的系统上，平均执行时间可能只差5%，但最坏执行时间的差别可能是10倍。伺服系统对抖动的敏感度远超对平均性能的敏感度——因为位置环的输出直接变成速度指令，周期抖动会转化成速度指令的相位噪声，最终体现在跟随误差上。

我做过一次对比测试：同样的环路代码，一次在中断里直接跑，一次在用户态高优先级线程里跑。平均耗时都在30微秒左右，但用户态那版的最大抖动到了180微秒。表现出来就是电机在低速时能听到轻微的“沙沙”声，位置反馈的频谱上在几百赫兹处多出一片底噪。

处理办法有几个层次：能用硬件定时器中断就别用软件定时器；必须用线程的话，把优先级提到实时区间，锁定内存防止换页，把栈预分配好；再往上一层，如果主控支持协处理器或者独立的运动控制单元，把环路整个下沉过去。这几条不是可选项，是精度做上去的前提。

2. C++控制器的工程骨架：中断、线程与内存的三重约束

定完节拍就该落地代码了。这一层我见过两种极端写法：一种是把所有东西塞进一个巨大的中断函数里，三千行，改一行全靠运气；另一种是过度设计，接口层套了四层抽象，结果一次调用要穿五个虚函数表。真正好用的骨架通常在中间——分层清晰，但每层的调用开销可控。

2.1 一个1kHz周期里到底能塞下多少活

先把一个完整周期拆开看。以1kHz位置环为例，一个周期内要做的事情大致是：

1. 读取编码器原始值，做多圈拼接和零位补偿
2. 计算位置差分得到速度，必要时做滤波
3. 位置环PID运算，输出速度指令
4. 速度前馈与加速度前馈叠加
5. 速度限幅、加速度限幅、转矩限幅
6. 指令下发到驱动器（总线写或模拟量输出）
7. 状态机更新、故障检测、数据记录

这里第1步和第6步是I/O，耗时取决于接口。如果走的是实时现场总线，一次帧收发可能有几十微秒的固定开销，这部分必须放在周期的最开始和最末尾，中间的计算时间就成了净预算。我的习惯是把周期切成三段：前段I/O、中段计算、后段下发，中间用一个明确的时间戳记录每段的实际耗时，运行中超标就置标志位。

struct CycleTiming { uint32_t t_start; uint32_t t_after_io_in; uint32_t t_after_calc; uint32_t t_end; }; void control_isr() { CycleTiming tm{}; tm.t_start = read_hw_timer(); feedback_.update(read_encoder_raw()); // 前段 I/O tm.t_after_io_in = read_hw_timer(); position_loop_.step(feedback_, command_, output_); // 中段计算 tm.t_after_calc = read_hw_timer(); write_drive_command(output_); // 后段下发 tm.t_end = read_hw_timer(); if (tm.t_end - tm.t_start > CYCLE_BUDGET_TICKS) { diag_.mark_overrun(tm); } }

这段代码看着平淡，但那个耗时记录器救过我很多次。有一次现场偶发丢步，就是靠它发现每17个周期会有一次计算段耗时翻倍，最后定位到是某个诊断日志函数在特定条件下走了格式化输出的慢路径。

2.2 无锁数据通路：双缓冲与序号校验的取舍

环路线程和非实时线程（比如上位机通信、参数修改、日志上传）之间必然要交换数据。这里最容易犯的错是随手加个互斥锁。锁本身不慢，慢的是等待——非实时线程持有锁的时候如果被调度走了，环路线程就得原地等，这是灾难。

常见的替代方案有两种。第一种是双缓冲加原子指针交换，写线程写备用缓冲，写完切换指针。问题是指针切换不是原子的“读-改-写”语义，读线程可能读到正在切换的中间状态。第二种是序号校验，也就是常说的seqlock思路：写线程在写之前把序号加一（变奇数），写完再加一（变偶数），读线程读前后各取一次序号，两次一致且为偶数才认为数据有效。

template <typename T> class SharedBlock { public: void write(const T& v) { seq_.fetch_add(1, std::memory_order_release); // 奇数：写入中 data_ = v; seq_.fetch_add(1, std::memory_order_release); // 偶数：完成 } bool read(T& out) const { uint32_t s1 = seq_.load(std::memory_order_acquire); if (s1 & 1u) return false; // 正在写，本次放弃 out = data_; std::atomic_thread_fence(std::memory_order_acquire); return seq_.load(std::memory_order_relaxed) == s1; } private: mutable std::atomic<uint32_t> seq_{0}; T data_{}; };

读失败就沿用上一拍的值，对控制环路来说完全可接受——参数更新晚一个周期没有任何影响。这里有个细节要注意：T必须是可平凡复制的类型，如果里面含有指针或者std::string，拷贝过程本身就不是原子的，序号校验也救不了你。这是我在一个参数结构体里塞了std::vector之后才明白的教训。

2.3 关掉异常和RTTI之后，寄存器封装该怎么写

环路代码里我一般编译选项加-fno-exceptions -fno-rtti，一是减小代码体积，二是让调用路径可预测。代价是不能用标准库的很多设施，寄存器封装得自己来。

最直接的写法是用volatile指针加位域结构体，但位域的可移植性差而且编译器生成的读改写序列可能不符合时序要求。我现在的习惯是给每个寄存器定义一组内联函数，读用一次volatile读，写用一次volatile写，需要读改写的场合显式写成三步，中间该加的屏障自己加。

class PwmTimer { public: explicit PwmTimer(uintptr_t base) : base_(base) {} inline void set_period(uint16_t p) const { reg(0x10) = p; } inline void enable(bool on) const { uint32_t v = reg(0x00); v = on ? (v | 0x01u) : (v & ~0x01u); reg(0x00) = v; // 显式读-改-写，不让编译器合并 } private: inline volatile uint32_t& reg(uint32_t off) const { return *reinterpret_cast<volatile uint32_t*>(base_ + off); } uintptr_t base_; };

这段代码没什么技术含量，但有个好处：每一处硬件访问在源码里都是可见的，review的时候一眼能看出有没有多余的读改写。用位域结构体的话，timer.en = 1这种写法背后到底是字节写还是字写，得翻汇编才知道。

3. 位置指令整形：S曲线、插补与跟随误差的因果关系

环路搭好之后，下一个决定精度的大头是指令怎么给。同样一段位移，你用阶跃给、用梯形速度给、用S曲线给，最终的跟随误差能差一个数量级。这一节讲清楚为什么，以及代码怎么写。

3.1 梯形速度曲线为什么在高速段必然抖

梯形速度曲线结构简单：加速段斜率恒定、匀速段、减速段斜率恒定。问题出在加速度的突变点上——从0直接跳到a，从a直接跳回0。加速度的突变意味着加加速度（jerk）是冲激函数，这个冲激会激励机械结构的固有频率。

机械系统不是刚体，联轴器、皮带、丝杠、机架都有弹性。把加速度阶跃打进去，等于给这个弹性系统一脚，它会以固有频率振一下，衰减时间取决于阻尼。轻阻尼的传动链，这一下能振几十毫秒。表现出来就是：高速运动到匀速段开始的瞬间，位置反馈会有一个小幅过冲然后回摆，如果这时候正好在做一个需要停顿的动作，精度就没了。

用频域的话说，加速度阶跃的频谱在低频段衰减很慢，能量会漏到机械共振频率上。S曲线的作用就是把加速度的变化率限制住，让加加速度变成一个有限值，频谱在高频段快速滚降。

3.2 七段S曲线的参数求解与代码实现

标准的S曲线分七段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。给定总位移、最大速度、最大加速度、最大加加速度四个约束，需要先判断能走到哪一段——有时候位移太短，压根到不了最大速度，甚至到不了最大加速度。

判断逻辑是经典的分类讨论，我把它写成一个纯函数，输入参数、输出七个时间段的时长，这样环路里每次只需要按时间查表求值，不做任何分支判断。

struct SCurveParam { double v_max, a_max, j_max, distance; }; struct SCurvePlan { double t[7]; // 七段时长，0 表示该段不存在 double v_peak, a_peak; bool valid; }; SCurvePlan plan_s_curve(const SCurveParam& p) { SCurvePlan r{}; double d = std::fabs(p.distance); double ta = p.a_max / p.j_max; // 达到最大加速度所需时间 double da = p.a_max * ta; // 加加速段走的距离 // 情况一：能不能到最大加速度 if (d < 2.0 * da) { // 到不了 a_max，重新求实际可达加速度 double a = std::sqrt(d * p.j_max); r.a_peak = a; double t = a / p.j_max; r.t[0] = r.t[2] = r.t[4] = r.t[6] = t; r.t[1] = r.t[3] = r.t[5] = 0.0; r.v_peak = a * t; // 峰值速度 r.valid = true; return r; } // 情况二：能到 a_max，但可能到不了 v_max double v_at_a = p.a_max * ta; // 纯加速到 a_max 时的速度增量 double d_acc = p.a_max * ta * ta; // 完整加加速+匀加速+减加速的距离估算 if (d < 2.0 * d_acc) { // 到不了 v_max double ta2 = ...; // 由 d 反解匀加速时长 r.t[0] = r.t[2] = r.t[4] = r.t[6] = ta; r.t[1] = r.t[5] = ta2; r.t[3] = 0.0; r.v_peak = p.a_max * (ta + ta2); r.a_peak = p.a_max; r.valid = true; return r; } // 情况三：完整七段 double v_at_full = p.a_max * (ta + p.a_max / p.j_max); r.t[0] = r.t[2] = r.t[4] = r.t[6] = ta; r.t[1] = r.t[5] = p.v_max / p.a_max - ta; r.t[3] = (d - 2.0 * d_acc) / p.v_max; r.v_peak = p.v_max; r.a_peak = p.a_max; r.valid = r.t[1] >= 0.0 && r.t[3] >= 0.0; return r; }

实际工程里我不会每次运动都重新规划——参数一般不变，规划结果可以缓存。真正每拍的运算是根据当前时间t落在哪一段，用闭式公式算出位置、速度、加速度三个量。这比数值积分稳定得多，尤其是长时间运行不会累积误差。

这里有个坑值得单独说：如果位移是负的，所有段的符号要跟着翻转，但时间参数不变。我早期版本忘了处理这个，结果反向运动时曲线完全乱掉，电机一启动就冲出去。

3.3 插补周期和环路周期对不齐会怎样

位置指令往往来自插补器，插补器的周期和位置环的周期常常不一致，比如插补跑1ms，位置环跑500us。直观做法是插补器每1ms给一个目标点，位置环每500us读同一个点两次。这会造成什么？位置指令变成阶梯状，每个阶梯持续两个环路周期，速度前馈算出来的值也跟着台阶跳。

正确的做法是在位置环里对插补点做线性插值，用当前时刻在插补周期内的相位做比例分配。别小看这一步，我在一个圆弧插补的项目里，就是因为漏了这层插值，圆度误差始终压不到5微米以内，加上插值之后直接降到1.2微米。

// 在位置环周期里做指令插值 double interp_position(double t_now) { double phase = (t_now - interp_t0_) / interp_dt_; // 0..1 if (phase > 1.0) phase = 1.0; return prev_target_ + (next_target_ - prev_target_) * phase; }

相位计算用的时间戳必须和插补器同源，最好都来自同一个硬件计数器，否则时钟漂移会慢慢积累，跑几小时后偏移就明显了。

4. 环路整定：从一条阶跃响应曲线读出系统的性格

参数调不出来是新手最头疼的环节。我的经验是别急着上自动整定算法，先学会看阶跃响应曲线。一条曲线里藏着几乎所有你需要的信息，学会读它比记住任何公式都管用。

4.1 阶跃响应的四个特征量与它们对应的参数

给位置环一个小的阶跃指令（比如满量程的1%到2%，太小会被量化噪声淹没，太大可能触发限幅），记录位置反馈。曲线上看四样东西：

这里有个反直觉的点：位置环里我通常不建议加大积分。因为位置环外面往往已经有速度环在兜底，速度环的积分已经把稳态误差吃掉大部分了，位置环再加积分容易引起低频振荡，表现为电机在原地轻微地来回“呼吸”。我的习惯是位置环只做纯P加前馈，积分留给速度环。

4.2 前馈不是万能药：速度前馈与加速度前馈的边界

前馈的作用是把“反馈修正”变成“提前补偿”。理论上前馈给得足够准，跟随误差可以接近零，因为反馈项几乎不用出力。但实际系统里前馈有三个天花板。

第一，速度前馈的增益等于速度环闭环传递函数的倒数，你估不准这个值。惯量会变（负载不同）、摩擦会变（温度、润滑），估偏了前馈就会过补偿或欠补偿。我一般先把前馈增益设到理论值的80%，然后看跟随误差随速度变化的曲线：如果误差随速度线性增长，说明前馈不足；如果高速段误差反向，说明过补偿。

第二，加速度前馈对噪声极其敏感。加速度是位置的两阶差分，量化噪声被放大得厉害。所以加速度前馈必须配一个低通滤波，而滤波又引入相位滞后，这个矛盾没法完全消除，只能折中。我的经验是滤波截止频率取速度环带宽的3到5倍。

第三，前馈不改善稳定性。很多人以为前馈能让系统“更稳”，其实前馈是开环的，它只是在反馈之外加了一条通路，系统的稳定性裕度完全由反馈决定。前馈加过头，反而会在指令突变的瞬间把执行机构推到饱和。

4.3 共振抑制：陷波滤波器参数到底怎么定

机械共振是精度杀手。定位完成后的残余振动、运动过程中的高频啸叫，八成都是共振。定参数的第一步是找到共振频率，方法有两条：一是给一个冲击指令，对位置反馈做FFT看峰值；二是用扫频激励，从低到高慢慢扫，记录幅频响应。

找到频率之后配陷波滤波器。陷波器本质是一个二阶IIR，参数是中心频率、深度、品质因数。中心频率就是共振频率，深度一般先设-20dB，品质因数Q取2到5——Q越大陷波越窄，对频率估计的准确性要求越高。

struct NotchFilter { double b0, b1, b2, a1, a2; double x1 = 0, x2 = 0, y1 = 0, y2 = 0; // f0: 中心频率, fs: 采样率, depth_db: 深度, q: 品质因数 void design(double f0, double fs, double depth_db, double q) { double w0 = 2.0 * M_PI * f0 / fs; double alpha = std::sin(w0) / (2.0 * q); double A = std::pow(10.0, depth_db / 40.0); double a0 = 1.0 + alpha * A; b0 = (1.0 + alpha / A) / a0; b1 = (-2.0 * std::cos(w0)) / a0; b2 = (1.0 - alpha / A) / a0; a1 = (-2.0 * std::cos(w0)) / a0; a2 = (1.0 - alpha * A) / a0; } double process(double x) { double y = b0 * x + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2; x2 = x1; x1 = x; y2 = y1; y1 = y; return y; } };

陷波器有个副作用：它在中心频率附近会引入相位滞后，如果共振频率离速度环带宽不远，加陷波之后系统可能变得不稳定。我遇到过一例，共振在420Hz，而速度环带宽设到了180Hz，加完陷波后整定出来的增益只能降到原来的一半。后来把速度环带宽压到120Hz，陷波深度也放宽到-12dB，反而整体跟随误差更小。这就是取舍——不是把共振压得越死越好。

5. 反馈链路精度：编码器量化、延迟与噪声的处理

控制器算得再准，反馈不准全白搭。这一层的问题往往最隐蔽，因为它在时域上表现得很随机，不做针对性测试很难定位。

5.1 量化误差被微分项放大的倍数

编码器分辨率是有限的。假设一圈131072个计数（17位），那么每个计数对应约0.00275度。位置本身误差不大，问题出在求速度上——速度是位置的差分，除以采样周期。

差分运算对量化噪声的放大倍数是√2/Ts。取Ts=1ms，量化步长q，速度噪声的标准差大约是1.41×q/0.001=1414q。如果q对应的角度换算成直线位移是0.1微米，那速度噪声就是0.14毫米每秒的量级。而低速运行时真实速度可能只有1毫米每秒，信噪比只有7倍，速度环的微分项会被这个噪声搅得没法用。

对策有三条。第一，速度不用位置差分算，直接读驱动器内部的观测器速度，它是基于电流和反电动势估出来的，噪声小得多。第二，非要用差分的话，加一阶低通，截止频率取速度环带宽的2到3倍。第三，做多周期平均，但平均会引入延迟，取4到8个周期比较合适。

5.2 反馈延迟与时间戳对齐

总线式伺服有个绕不开的问题：反馈值带延迟。驱动器采样编码器、打包、发送、控制器接收、解析，这一圈下来可能有一到两个控制周期。如果你拿这个延迟的反馈当当前值用，等于在环路里串了一个纯延迟环节，相位裕度被吃掉一大块。

处理办法是给每个反馈值打时间戳，在环路里做延迟补偿——用上一拍的速度外推一个周期的位置。

double compensate_delay(const Feedback& fb, double t_now) { double dt = (t_now - fb.timestamp) / TICK_SEC; if (dt <= 0.0) return fb.position; if (dt > 3.0) dt = 3.0; // 延迟过大，放弃补偿 return fb.position + fb.velocity * dt * TICK_SEC; }

这个补偿在匀速段效果很好，加减速段会有残差，因为外推用的是线性模型。我的做法是在加减速段把补偿系数降下来，用dt * (1 - |accel| / a_max * 0.5)这类经验公式做折中。不完美，但比不补强。

5.3 低速爬行：摩擦力与刚度的双重作用

低速爬行是精密平台的经典问题。表现是：给定一个很慢的匀速指令，实际位置不是平滑前进，而是一顿一顿地走，每顿一下几十微米。

成因有两层。机械层是静摩擦和动摩擦的差值，静摩擦大，要先积累足够的力才能推动，一动起来摩擦力骤降，就冲出去一截，然后再停住。控制层的诱因是位置环增益不足，误差积累到一定程度才产生足够的指令，本身就在制造“走走停停”。

对策组合起来用：一是在速度环里加摩擦补偿，根据速度方向和大小给一个前馈力矩，静摩擦区给得多，动摩擦区给得少，中间用平滑过渡避免跳变；二是提高位置环增益，让误差一出现就立刻响应；三是适当加入微小的抖动信号，把静摩擦“激活”成动摩擦。第三条听着别扭，但在某些平台上确实有效，抖动的幅度要控制在量化步长以内，频率取几百赫兹。

摩擦补偿的模型参数一定要现场辨识。我见过直接抄别人参数的，结果补偿量给反了方向，电机在零速附近自己来回撞限位。

6. 现场排查实录：三个典型故障的完整链路

理论讲完，说点真刀真枪的。下面三个故障都是我在实际项目里遇到的，排查过程完整写出来，因为思路比结论更有参考价值。

6.1 定位完成信号抖动：从地线一路查到状态机时序

现象：一套两轴平台，单轴定位完成后，“到位”信号在示波器上有几十微秒的抖动，偶尔会让上位机误判超时。

排查链路是这样走的。第一步怀疑阈值设置，把到位窗口从5个计数放宽到20个，抖动依旧，排除。第二步怀疑位置反馈本身在抖，把编码器原始值打出来看，发现计数稳定，但位置值有±1的变化——原来是零位补偿用了浮点运算，在临界点上反复跨越。第三步改补偿算法，改成整数域处理，位置值稳了，但信号还在抖。第四步看状态机，发现到位判断和“运动中”标志的清除不在同一个周期里，中间隔了一拍，这一拍里两个信号同时有效，上位机据此判定异常。最后把状态迁移全部收进同一个周期内完成，问题消失。

这件事让我养成了一个习惯：所有状态迁移放在同一个中断里、同一个时刻完成，不跨周期。

6.2 周期性位置偏差：传动比与脉冲当量的隐形错位

现象：一个带减速机的旋转台，每转一圈回到零点时，位置总会差几十个计数，而且误差大小固定。

第一反应是零点开关的重复精度，测了三次都是微米级，排除。第二反应是编码器多圈计数溢出，检查了拼接逻辑，正常。第三开始怀疑脉冲当量——理论值按减速比1:50算，丝杠导程5毫米，一圈编码器131072计数，那么每个计数应该是5/(50×131072)=0.763微米。实际测量下来，走100毫米指令，实际走了100.023毫米。

反算实际减速比：100.023/100=1.00023，也就是减速机的实际减速比是50.0115而不是50。这种偏差在减速机行业里是正常的，标称值只是名义值。修正办法是现场标定：让轴走一段长距离（越长越好，减少测量误差），用高精度量具测实际位移，反算当量系数，写进参数里。标定完误差降到2微米以内。

这个坑的教训是：脉冲当量绝对不能只按名牌参数算，必须实测标定。

6.3 高频啸叫：增益过高与采样混叠的叠加

现象：某轴在某个速度区间会发出尖锐啸叫，速度一过这个区间就消失。

先怀疑共振，做扫频测试，发现在1180Hz处有一个明显的幅值峰。但按常规加陷波（中心1180Hz，深度-20dB）之后，啸叫没消失，反而多了个新的低频振荡。

停一下想，1180Hz这个频率很可疑——位置环采样1kHz，奈奎斯特频率是500Hz，1180Hz已经超出范围了。这说明我看到的峰值是混叠后的假象，真实的共振可能在2kHz以上。回头看驱动器的电流环采样是16kHz，把陷波挪到电流环去加，中心频率设在真实共振点（通过电流波形测出来的2.3kHz），问题解决。

这件事说明两件事：采样率不够的时候，频谱分析会骗人；陷波滤波器要加在采样率足够高的那一层，位置环加高频陷波基本没意义。

7. 写在这套系统跑稳之后

这套东西我从第一版跑通到现在，最深的体会是——精度不是靠某一个漂亮的算法拿到的，是靠把每一处不确定性都揪出来、量化、然后想办法压下去。控制理论给出的是骨架，真正填肉的是对时间、噪声、机械特性的理解。

如果让我给刚上手的人一句建议，那就是先把测量做扎实。一个能精确记录每个周期耗时、能同步抓取指令与反馈、能对反馈做频谱分析的最小工具集，价值超过任何高级算法。我现在的项目里，这套诊断代码是第一个写的，不是最后一个补的。

另外提一句，参数整定做完之后一定要做温度漂移和长时间运行的复测——冷机调到完美的参数，跑两小时热了之后可能就变了。留一点相位裕度，别把系统调到临界状态，这是我在好几个项目里用返工换来的经验。