高精度伺服系统拆开来看并不神秘电机、驱动器、编码器再加一层薄薄的实时控制代码。真正难的是最后那层代码——C 运动控制底层要在一个固定不变的毫秒级节拍里把轨迹规划、三环运算、总线收发、状态判断全部做完中间不能有一次迟到。我前后完整做过几套精密定位平台的控制内核从最早用脚本拼原型到后来把整条链路压进一颗隔离核上的实时线程踩过的坑基本都集中在确定性这三个字上。这篇文章写给正在自己动手写控制内核的工程师、准备把算法从仿真搬到实机的学生以及被跟随误差和末端抖动反复折腾的调试人员。里面的参数、公式、代码结构都来自实际工程里反复试出来的做法能直接抄的部分我会尽量写成能抄的样子。1. 先把高精度拆成能测量的指标1.1 分辨率、精度、重复定位精度、跟随误差是四件事新手最容易犯的错是把编码器位数当成系统精度来宣传。17 位编码器配 5mm 导程的丝杠理论分辨率的算法是 5mm 除以 131072得到大约 0.038 微米。这个数字写进 PPT 很好看可实机测下来重复定位精度往往落在 2 到 5 微米这个区间。差出来的两个数量级来自机械背隙、丝杠螺距累积误差、导轨直线度、热伸长还有联轴器的扭转刚度。分辨率只说明能分辨的最小步距不说明走得到、停得住、回得来。前者是电子学问题后者是机电耦合问题两者之间没有换算关系。精度和重复定位精度的区别值得单独说一句。精度是多次走到同一条指令位置后的平均偏差靠标定和螺距补偿能压下去重复定位精度是多次到达同一目标后彼此之间的一致性靠的是机械刚性和闭环稳定性补偿手段有限。做半导体或光学类设备时客户真正关心的通常是重复定位精度因为设备的加工一致性直接由它决定。而真正最难压的是第四个指标——动态跟随误差也就是运动过程中指令位置与实际位置之间的瞬时偏差。静止时误差可能只有 1 微米一旦以 200mm/s 跑起来跟随误差轻松到几百微米甚至毫米级这个数量级的落差是后面所有算法工作的主战场。1.2 跟随误差的量级要用公式先估一遍跟随误差有个非常朴素的估算式稳态匀速运动时误差约等于速度除以位置环增益即 e v / KvKv 的单位是 1/s。假设你设的位置环比例增益折算下来是 50 每秒走 100mm/s那跟随误差就是 2mm。这个数字第一次算出来的人通常会吓一跳明明编码器能分辨到几十纳米怎么跑起来就落后 2 毫米答案很简单位置环是个比例控制器它必须靠一个足够大的误差才能挤出足够的速度指令误差本身就是控制作用的来源。想让误差变小要么提高 Kv要么在前馈通道上做文章。提高 Kv 的天花板由机械共振和采样相位滞后共同决定。Kv 拉到 150 以上很多中小型平台就会开始听到中高频啸叫那是结构的一阶模态被激发了。所以工程上的常规打法是把 Kv 稳在一个既有裕量又不至于太软的值通常是 40 到 100 之间然后用速度前馈把匀速段的跟随误差基本抹平再用加速度前馈处理加减速段的残余。这三步做完200mm/s 下的跟随误差从 1.8mm 压到 20 微米以内是完全现实的量级跨了两个数量级。后面第 2.3 节会给出具体的实现方式和实测数据。1.3 控制内核为什么选 C 而不是脚本或梯形图选型这件事我试过三条路。最早用脚本语言拼原型好处是改一行立刻能看到效果坏处是解释执行带来的周期抖动轻易就到几百微秒而且垃圾回收会在你完全预料不到的时刻插进来把 1kHz 的循环打出一个 20ms 的空洞。第二个原型走的是 PLC 软逻辑扫描周期确实固定但算法表达力受限做个带加加速度约束的七段式 S 曲线要写上百个网络节点调试时基本靠猜。最后落到 C不是因为语言本身多优雅而是它同时满足了三件事内存布局完全可控、没有运行期垃圾回收、能直接贴着操作系统做实时调度和硬件访问。代价也很清楚。C 不帮你兜底指针越界、整数溢出、浮点异常、数据竞争全得自己防。做运动控制内核时我给自己定了几条硬规矩控制线程内一次动态内存分配都不允许出现所有中间变量用栈上结构体浮点运算统一用 double避免 float 在长距离累积插补时丢精度所有跨线程的数据交换走无锁单生产者单消费者队列。这几条定下来之后内核的确定性才勉强够看。S 曲线的参数推导、PID 的离散化、总线的收发时序全都在这个约束框架里展开。2. 三环结构在代码里到底是什么样子2.1 三个环的周期配比与职责边界伺服系统的经典三层结构是电流环、速度环、位置环从内到外一层比一层慢。电流环负责让电机绕组里的电流跟上指令周期通常在 10 到 20 微秒量级这一层基本都在驱动器固件里跑上位控制器的代码碰不到。速度环周期常见是 250 微秒到 1 毫秒位置环则是 500 微秒到 2 毫秒。这个配比不是随便定的背后的原则是内环带宽必须显著高于外环一般要求 5 到 10 倍否则两个环的动态会互相干扰表现为整定的时候怎么调都不收敛或者刚调好一个工况换个速度就发散。环路典型周期主要任务常见实现位置电流环10~20 μs电流闭环、PWM 输出驱动器固件速度环250~1000 μs速度跟踪、抑制负载扰动驱动器或控制器位置环500~2000 μs位置跟踪、轨迹跟随控制器为主位置环和速度环放在哪一层直接决定了你的 C 代码要写多少。有一种架构是把两个环都下放到驱动器控制器只负责发位置指令流那么上位机的工作量就是轨迹规划和总线时序代码量小但控制自由度低前馈补偿这类手段很难加。另一种是把速度环和位置环都拿到控制器里算驱动器工作在转矩模式这样前馈、观测器、自定义滤波全都可以自己实现代价是控制器必须承担全部的实时压力。我做的几套高精度平台都走第二条路因为跟随误差要压到几十微米以内不下放转矩模式基本没戏。2.2 PID 离散化的四个必须处理的细节教科书上的 PID 是连续域公式搬到代码里就变成了一堆需要小心处理的边界情况。位置式 PID 的离散形式是 u[k] Kp·e[k] Ki·Ts·Σe Kd·(e[k] − e[k−1])/Ts看起来简单实际写起来有四个坑必须处理。第一个是积分饱和。当执行器已经到限幅还继续积分误差会一直累积等到该反向的时候积分项要花很长时间才能吐回来表现为明显的过冲和滞后。常规做法是输出限幅配合反算抗饱和也就是把限幅前后之差乘一个系数反馈回积分器。第二个是微分冲击。如果直接对误差求微分设定值一阶跃微分项就会瞬间打出一个尖峰把电机踹一下。正确做法是对测量值求微分物理意义是阻尼设定值突变时不会产生冲击。第三个是采样周期抖动。理论推导都假设 Ts 恒定实际系统里因为调度延迟每个周期总会差几个微秒。在 1ms 周期下几微秒的抖动影响可以忽略但如果你的周期是 100 微秒5% 的周期误差足以让积分项和微分项都算偏。稳妥做法是每次循环用实际测量的 dt而不是用标称值。第四个是微分噪声。编码器读数本身带量化噪声差分放大后非常难看必须在微分通道后面串一个一阶低通截止频率一般取速度环带宽的 3 到 5 倍既能压住噪声又不引入太多相位滞后。struct PidParam { double kp 0.0, ki 0.0, kd 0.0; double ff_v 0.0, ff_a 0.0; // 速度/加速度前馈系数 double i_limit 0.0; // 积分项限幅 double out_limit 0.0; // 输出限幅 double d_lpf_hz 800.0; // 微分通道低通截止频率 }; class PidAxis { public: double update(double ref, double fb, double v_ref, double a_ref, double dt) { const double err ref - fb; // 微分作用于测量值避免设定值阶跃带来的冲击 const double d_raw (fb - fb_prev_) / dt; fb_prev_ fb; const double alpha 1.0 - std::exp(-2.0 * M_PI * p_.d_lpf_hz * dt); d_state_ alpha * (d_raw - d_state_); // 积分带反算抗饱和 integ_ p_.ki * err * dt; integ_ std::clamp(integ_, -p_.i_limit, p_.i_limit); double u p_.kp * err integ_ - p_.kd * d_state_ p_.ff_v * v_ref p_.ff_a * a_ref; // 输出限幅后把超出量反算回积分器 const double u_sat std::clamp(u, -p_.out_limit, p_.out_limit); if (u ! u_sat p_.ki 1e-12) { integ_ 0.25 * (u_sat - u); } return u_sat; } private: PidParam p_; double integ_ 0.0; double d_state_ 0.0; double fb_prev_ 0.0; };这段代码里有几个细节值得停一下。alpha的算法用的是双线性变换的简化形式周期变化时它跟着变保证截止频率不漂。反算系数取 0.25 是个经验值太大容易在限幅边界来回振荡太小又起不到快退饱和的作用具体数值和你系统的刚度有关可以在这个附近微调。另外d_state_是低通后的微分状态不是原始微分这一点在初始化时要归零否则第一次循环会打出一个假尖峰。2.3 前馈把跟随误差提前预支掉前馈是整个伺服调试里性价比最高的一招。它的思路非常直白既然跟随误差来自控制器要等误差出现才动作那我就把已知的运动信息提前加进去。速度前馈把参考速度乘一个系数加到输出上加速度前馈把参考加速度乘一个系数加上去。理想情况下如果前馈系数完全匹配系统的惯量和阻尼位置环的误差输出几乎为零PID 只在有外部扰动时才工作。实测数据比较有说服力。某套直线电机平台行程 200mm最高速度 200mm/s。不加前馈时匀速段的跟随误差稳定在 1.8mm。只加 100% 速度前馈降到 60 微米左右剩下的主要是加减速段的残余。再把加速度前馈调到 85%全行程最大跟随误差压到 15 微米。这里的关键是加速度前馈系数基本对应总惯量除以电机推力常数的倒数关系所以它有个物理意义不是纯凑数。速度前馈则主要补偿粘滞阻尼和反电动势系数带来的滞后。前馈不是越大越好。速度前馈超过 105% 之后匀速段会出现轻微的超前表现为位置曲线在指令前面跑遇到外部阻力时又猛地掉回来末端定位时能看到一次小幅回弹。加速度前馈过冲更明显会在加减速的拐点上打出一个尖峰实测波形上是一次高频抖动。我的经验是把速度前馈整到 100% 附近加速度前馈从 60% 开始逐步往上加加到末端过冲刚开始露头就退回来 5 个百分点这个位置基本是稳的。3. C 底层的工程骨架线程、内存、时序3.1 把控制线程变成不会迟到的线程实时性的第一件事不是算法是调度。普通优先级的线程在通用操作系统上跑 1kHz 循环抖动轻松到几百微秒偶尔还会被后台任务挤出一个几毫秒的空洞。这种确定性根本没法做高精度控制因为位置环的相位裕度会随抖动一起崩。要压住抖动需要一套组合拳把控制线程的调度策略设为 SCHED_FIFO优先级给到 80 左右把线程绑到一颗隔离出去的 CPU 核上让系统调度器别把别的任务塞进来用 mlockall 锁住进程内存防止运行中被换页循环内部禁止任何可能阻塞的系统调用日志、文件、网络全部挪到别的线程。周期等待也别用普通的 sleep。相对睡眠会累积误差每个周期晚一点点跑一小时就漂出去几十毫秒。正确做法是用 clock_nanosleep 的绝对时间模式配合 CLOCK_MONOTONIC每次在上一周期的目标时刻上直接加周期而不是在当前时刻上加。这样即使某一拍被延迟下一拍也会自动追回来不会形成累积漂移。void controlLoop(int period_ns) { struct timespec next; clock_gettime(CLOCK_MONOTONIC, next); while (running_.load(std::memory_order_relaxed)) { next.tv_nsec period_ns; while (next.tv_nsec 1000000000L) { next.tv_nsec - 1000000000L; next.tv_sec 1; } // 本轮的实际周期用于 PID 计算 const double dt period_ns * 1e-9; cycle(dt); // 采样、规划、闭环、输出 clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, next, nullptr); } }做完这套配置1kHz 循环的周期抖动能从几百微秒压到 10 微秒以内这个量级已经足够支撑位置环的稳定运行。有个细节容易漏不要在这个线程里调用任何会隐式加锁的东西包括某些标准库容器的扩容、字符串拼接、格式化输出。我曾经在一个日志宏上栽过跟头那个宏在异常路径上会分配内存结果每次故障触发时控制周期就被拉长一次故障本身反而查不出来。3.2 无锁数据通路让非实时任务进不来控制线程需要和界面线程、通信线程、日志线程交换数据如果把互斥锁放在控制线程里随时可能被优先级反转拖住——低优先级的界面线程持锁时被调度走控制线程只能干等一个周期就废了。解决办法是把所有跨线程交互都改成单生产者单消费者队列控制线程只做原子的读写指针操作绝不阻塞。环形队列的实现要点是三件事容量必须是 2 的幂这样取模可以用位与代替除法读写序号用std::atomicsize_t写端用 release 语义、读端用 acquire 语义保证数据先写完再更新序号队列满的时候直接丢弃或者覆盖绝不能让控制线程等待。template typename T, size_t N class SpscQueue { static_assert((N (N - 1)) 0, N must be power of two); public: bool push(const T v) { const size_t w w_.load(std::memory_order_relaxed); const size_t r r_.load(std::memory_order_acquire); if (w - r N) return false; buf_[w (N - 1)] v; w_.store(w 1, std::memory_order_release); return true; } bool pop(T v) { const size_t r r_.load(std::memory_order_relaxed); const size_t w w_.load(std::memory_order_acquire); if (r w) return false; v buf_[r (N - 1)]; r_.store(r 1, std::memory_order_release); return true; } private: T buf_[N]; std::atomicsize_t w_{0}, r_{0}; };队列里传的东西也要讲究。不要传指针不要传带堆内存的容器最好是 POD 结构体几十字节以内拷贝成本可以忽略。命令下发队列放目标位置、速度上限、加速度上限这类参数状态回传队列放当前位置、当前速度、跟随误差、状态字。每条数据带一个序号界面线程发现序号跳跃就知道中间丢帧了这个比事后查日志高效得多。3.3 状态机把现场事故挡在代码外面伺服系统最怕的不是算法不准是状态乱掉。上电时驱动器没准备好就发使能回零没完成就下运动指令急停按下后还有残余指令在队列里等着执行——这些在实验室里可能只是报个错在设备上就是撞机。我的做法是把整个生命周期写成显式状态机每个状态定义清楚允许的转换和超时任何非法转换直接拒绝并锁存故障。状态划分大概是这样初始化、上电自检、伺服使能、回零、就绪、运行、暂停、故障、急停。关键点在于运行状态只接受运动指令回零状态只接受回零指令跨界一律拒绝。急停走最高优先级路径不经过队列直接在控制周期里检查一个原子标志位一旦置位立即切断使能输出并把指令位置同步到当前位置避免恢复时出现位置跳变。故障状态必须锁存不能自动恢复必须人工确认后走一遍完整的上电流程。这套东西写起来枯燥但它是把无数次现场事故挡在代码外面的唯一办法。4. 轨迹规划S 曲线到底怎么算4.1 梯形速度曲线在精密平台上的硬伤梯形速度曲线是最容易实现的方案加速、匀速、减速三段加速度是方波。问题出在加速度的跳变上跳变意味着加加速度jerk在数学上是无穷大实际系统中表现为一次冲击激励。这个冲击会激起机械结构的高阶模态在末端定位时表现为一段衰减振荡 settling time 被拉长好几倍。行程越短、速度越高这个问题越明显因为冲击在整个运动时间里占的比重更大。S 曲线把加速度也做成连续的用恒定的加加速度段去衔接加速度曲线变成一个梯形速度曲线变成 S 形。代价是参数计算复杂了不少而且要处理行程太短达不到最高速度的情况。但收益很实在同样一套平台梯形曲线末端振荡要 80 毫秒才稳定换成 S 曲线后降到 15 毫秒以内定位节拍直接提升一大截。4.2 七段式 S 曲线的参数推导标准七段式包含加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。给定最高速度 v_max、最大加速度 a_max、最大加加速度 j_max加加速段的时长是 t1 a_max / j_max这个阶段速度从 0 涨到 a_max²/(2·j_max)。匀加速段的时长 t2 v_max/a_max − a_max/j_max如果算出来是负数说明速度还没到 v_max 加速度就已经到顶了这时候没有匀加速段退化成两段。加速段总时长 t_acc v_max/a_max a_max/j_max加速段走过的位移有个非常干净的结论s_acc v_max · t_acc / 2。这个式子在有匀加速段和没有匀加速段两种情况下都成立推导过程略繁但结论可以直接用。拿一组具体参数算一遍。v_max 500 mm/sa_max 5000 mm/s²j_max 50000 mm/s³。t1 5000/50000 0.1s这一阶段结束时速度是 5000²/(2×50000) 250 mm/s。t2 500/5000 − 0.1 0正好是临界情况没有匀加速段。加速总时长 0.2s加速位移 500×0.2/2 50mm。减速对称也是 50mm。所以这套参数下行程 100mm 刚好跑满全程没有匀速段行程 200mm 时匀速段 (200−100)/500 0.2s总运动时间 0.6s。行程短于 100mm 时达不到 500mm/s需要反解峰值速度。设 S 60mm加速位移只有 30mm代入 s_acc v·(v/a_max a_max/j_max)/2得到 30 v·(v/5000 0.1)/2整理成 v² 500v − 300000 0解得 v ≈ 352 mm/s。检验一下 352 250说明确实有匀加速段假设成立。如果解出来小于 250就要换成纯 S 形公式此时 t_acc 2·√(v/j_max)s_acc v·√(v/j_max)反解 v ((S/2)²·j_max)^(1/3)。这两个分支必须都写上否则短行程时算出来的曲线会超调。4.3 在线插补与浮点累积误差规划层算出七段的边界时刻和对应的速度、加速度控制周期里只需要根据当前时间 t 判断落在哪一段用解析式直接算位置而不是每周期做一次数值积分。这个选择很关键积分会累积浮点误差跑一万个周期之后位置可能偏出几个微米解析式每次从 t 重新算误差不累积。段内位置的公式就是标准运动学公式加加速段用三次多项式匀加速段用二次匀速段用一次写成一个 switch 就够了。多轴插补的关键是统一时间标尺。假设 X 轴要走 200mmY 轴要走 50mm如果各自按自己的最大速度规划两轴到达时间不同走出来的就是折线而不是直线。正确做法是先算出合成运动的路径长度按路径长度做一次规划得到总时间 T然后每个周期用相同的归一化时间参数去查各轴的位置。这样两轴严格同步任意时刻实际位置都落在指令直线上。圆弧插补同理只是位置由圆心、半径和角度参数算出再投影到各轴。5. 反馈通道从编码器读数到闭环收敛5.1 分辨率换算与量化噪声的现实差距23 位绝对值编码器单圈 8388608 个计数配 5mm 导程丝杠一个计数对应 0.6 纳米。这个数字在物理上没有意义因为机械系统的热膨胀在 1 摄氏度下就能带来几微米的位移编码器再准也测不准。实际能用到的分辨率受限于噪声底干净的布线和良好的接地能做到 20 到 50 纳米布线糟糕时噪声能到微米级。噪声来自哪里编码器信号线的屏蔽层单端接地还是双端接地、动力线和信号线是否分开走、驱动器地和控制器地是否共地、开关电源的纹波有多大。这些问题在图纸上看不出来装机之后才一个个暴露。我遇到过一次跟随误差莫名抖动的案例查了半天算法没找到原因最后发现是编码器线和电机动力线绑在同一个线槽里把屏蔽层重新做单端接地、动力线换成屏蔽双绞并拉开 10cm 距离抖动直接消失了。噪声处理不能只靠接线软件侧也要有一道。速度是由位置差分算出来的差分会把噪声放大所以速度通道必须滤波。一阶低通的截止频率取速度环带宽的 3 到 5 倍比较合适比如速度环带宽 200Hz低通取 800Hz。滤波带来的相位滞后要算清楚一阶低通在 100Hz 处的相位滞后约 7 度这个量会吃掉一部分相位裕度整定时要预留出来。5.2 总线周期、时延与相位裕度的关系用实时工业以太网总线EtherCAT 这一类时控制器的位置环周期和总线周期是绑定的。总线周期 1ms位置环就只能 1kHz因为反馈数据要等一个总线周期才能拿回来。整个回路的时延由四部分组成编码器采样、总线传输、控制器计算、指令下发加起来通常是 2 到 3 个总线周期。这个时延在频域上就是纯相位滞后滞后角等于 360° × f × T_d。算一下就知道多要命。假设总时延 2ms在 100Hz 处相位滞后 72 度。如果你的位置环在这个频率附近本来只有 40 度的相位裕度加上这 72 度直接变成负的系统必然振荡。这就是为什么高精度平台的总线周期会往 500 微秒甚至 250 微秒压不是为了数据量是为了把这部分滞后压下去。时延没法完全消除但可以补偿。常用做法是用速度外推一个周期把反馈值往前推 T_d 时间fb_pred fb v·T_d。这个预测在匀速段非常准加减速段会有一点偏差但配合加速度项修正后误差可以控制在很小的范围。我的做法是只补偿一个周期的固定时延剩下的用降低 Kv 来换稳定裕度这样既不会过度依赖模型也不至于把带宽浪费掉。6. 上机调参的真实顺序6.1 调参必须按固定顺序走跳步必翻车调参的合理顺序是空载单轴、电流环自整定、速度环、位置环、带负载、全行程、多轴联动。每一步都有明确的验收标准不达标绝不进入下一步。跳过速度环直接调位置环是最常见的错误结果是位置环怎么调都软绵绵因为底下的速度环根本没跟上。空载阶段先确认方向和使能逻辑用手转电机轴感受阻尼是不是均匀有卡顿说明电流环或换相有问题。电流环一般由驱动器自动辨识需要输入电机电感、电阻、反电动势系数辨识完看阶跃响应的上升时间是否在预期范围。速度环整定时给一个阶跃速度指令看响应有没有明显超调和振荡用示波器或者控制器内部的高速采样抓波形。位置环最后调先只开 P慢慢加到临界振荡再退回来 30%然后加前馈。6.2 阶跃、正弦、频响三种测试各管什么阶跃响应看的是时域指标上升时间、超调量、整定时间。位置阶跃给 1mm采样率足够高的话能清楚看到有没有过冲、过冲后几次收敛。正弦跟随测试看的是频域跟踪能力给不同频率的正弦指令测量幅值衰减和相位滞后画出伯德图就能算出带宽和相位裕度。频响测试最费时间但信息量最大扫频范围一般从 1Hz 到 500Hz每个频点跑几个周期取稳态。测试类型输入信号关键指标目标参考值阶跃响应1mm 位置阶跃超调量、整定时间超调5%整定30ms正弦跟随1~50Hz 正弦幅值衰减、相位滞后20Hz 处衰减3dB频响扫描1~500Hz 扫频带宽、相位裕度带宽80Hz裕度35°匀速跟随恒定速度稳态跟随误差30μm 200mm/s6.3 一组实测参数与调整过程某套直线电机平台的最终参数记录如下供参考但不建议直接照搬因为机械结构差异太大。参数初始值最终值调整理由位置环 Kp25 1/s60 1/s逐步加到 85 出现啸叫后退回位置环 Ki08消稳态误差再大就过冲速度环 Kp0.82.4配合带宽提升速度环 Ki30120负载扰动抑制速度前馈0100%匀速段误差基本抹平加速度前馈085%加减速段残余压到 15μm微分低通500Hz900Hz噪声可控前提下放宽调整过程里最耗时的是位置环 Kp 那一步。从 25 加到 60 很顺60 到 85 之间开始出现 180Hz 附近的持续振荡用加速度计一测正好是平台一阶模态。这时候不能硬往上加要么做陷波滤波把这个频点压下去要么就停在 60。我选了后者因为陷波滤波虽然能压住振荡但会引入额外相位滞后在别的频段埋雷。7. 常见问题排查实录7.1 抖动、异响、超调、跟随误差大的分离思路排查的基本原则是先分清是电气问题、机械问题还是算法问题。有个简单的判别方法断开位置环只跑速度环如果抖动还在问题在速度环以内或者机械如果速度环很干净一闭位置环就抖那就是位置环参数或者反馈通道的问题。现象可能原因验证方法处理手段静止时高频抖动编码器噪声、增益过高断电手推看反馈是否跳变加反馈滤波、降 Kv匀速段跟随误差大速度前馈缺失、阻尼大提高前馈看是否改善速度前馈补到 100%加减速段过冲加速度前馈过大、积分饱和减小前馈看波形前馈退 5%检查抗饱和末端衰减振荡机械模态被激发加速度计测频率降加加速度、加陷波定位后缓慢漂移积分饱和、重力负载看积分项是否顶到限幅加前馈配重、限幅反算偶发丢步或报警总线误码、电源跌落查总线错误计数器检查接地与屏蔽7.2 几条踩出来的经验单位混乱是新人最容易踩的坑而且踩了之后极难发现。位置用毫米、速度用毫米每秒、加速度用毫米每二次方秒这套单位在数值上跨度很大编码器计数转毫米的系数一旦写错一位小数表现是位置差十倍但不报错。我的做法是在代码里给每个物理量定义一个强类型包装编译期就能挡住单位混用运行时再加一个量程检查超出物理可能的范围直接进故障。实时线程里绝对不能有隐式分配。标准库的std::string、std::vector、std::function在特定操作下都会分配内存一旦分配触发缺页周期就废了。我现在的规矩是控制周期函数里只允许出现定长数组、POD 结构体和预先分配的缓冲区任何需要动态构造的东西都在初始化阶段搞定。最后一条是关于日志的。调试期总想在控制线程里打点东西看但格式化输出本身就是几十微秒的开销打完点波形就变样了。正确的做法是控制线程只往环形缓冲里塞裸数据格式化和落盘全放到低优先级线程。这样既能看到真实波形又不影响控制节拍。我个人在实际操作中的体会是伺服系统调到最后拼的不是算法有多花哨而是对每一个误差来源的清楚认知。知道那 2 毫米的跟随误差是被位置环增益挤出来的就知道该往前馈上使劲知道末端那 15 毫秒的振荡是被加速度跳变激出来的就知道该动加加速度而不是动 PID。参数表可以抄这条因果链抄不走只能自己一段段跑波形跑出来。 
长河君 · 2026/10/29 22:09:09 · 阅读 1.2k 高精度伺服C++实时控制内核：确定性、三环与前馈调参
高精度伺服系统拆开来看并不神秘电机、驱动器、编码器再加一层薄薄的实时控制代码。真正难的是最后那层代码——C 运动控制底层要在一个固定不变的毫秒级节拍里把轨迹规划、三环运算、总线收发、状态判断全部做完中间不能有一次迟到。我前后完整做过几套精密定位平台的控制内核从最早用脚本拼原型到后来把整条链路压进一颗隔离核上的实时线程踩过的坑基本都集中在确定性这三个字上。这篇文章写给正在自己动手写控制内核的工程师、准备把算法从仿真搬到实机的学生以及被跟随误差和末端抖动反复折腾的调试人员。里面的参数、公式、代码结构都来自实际工程里反复试出来的做法能直接抄的部分我会尽量写成能抄的样子。1. 先把高精度拆成能测量的指标1.1 分辨率、精度、重复定位精度、跟随误差是四件事新手最容易犯的错是把编码器位数当成系统精度来宣传。17 位编码器配 5mm 导程的丝杠理论分辨率的算法是 5mm 除以 131072得到大约 0.038 微米。这个数字写进 PPT 很好看可实机测下来重复定位精度往往落在 2 到 5 微米这个区间。差出来的两个数量级来自机械背隙、丝杠螺距累积误差、导轨直线度、热伸长还有联轴器的扭转刚度。分辨率只说明能分辨的最小步距不说明走得到、停得住、回得来。前者是电子学问题后者是机电耦合问题两者之间没有换算关系。精度和重复定位精度的区别值得单独说一句。精度是多次走到同一条指令位置后的平均偏差靠标定和螺距补偿能压下去重复定位精度是多次到达同一目标后彼此之间的一致性靠的是机械刚性和闭环稳定性补偿手段有限。做半导体或光学类设备时客户真正关心的通常是重复定位精度因为设备的加工一致性直接由它决定。而真正最难压的是第四个指标——动态跟随误差也就是运动过程中指令位置与实际位置之间的瞬时偏差。静止时误差可能只有 1 微米一旦以 200mm/s 跑起来跟随误差轻松到几百微米甚至毫米级这个数量级的落差是后面所有算法工作的主战场。1.2 跟随误差的量级要用公式先估一遍跟随误差有个非常朴素的估算式稳态匀速运动时误差约等于速度除以位置环增益即 e v / KvKv 的单位是 1/s。假设你设的位置环比例增益折算下来是 50 每秒走 100mm/s那跟随误差就是 2mm。这个数字第一次算出来的人通常会吓一跳明明编码器能分辨到几十纳米怎么跑起来就落后 2 毫米答案很简单位置环是个比例控制器它必须靠一个足够大的误差才能挤出足够的速度指令误差本身就是控制作用的来源。想让误差变小要么提高 Kv要么在前馈通道上做文章。提高 Kv 的天花板由机械共振和采样相位滞后共同决定。Kv 拉到 150 以上很多中小型平台就会开始听到中高频啸叫那是结构的一阶模态被激发了。所以工程上的常规打法是把 Kv 稳在一个既有裕量又不至于太软的值通常是 40 到 100 之间然后用速度前馈把匀速段的跟随误差基本抹平再用加速度前馈处理加减速段的残余。这三步做完200mm/s 下的跟随误差从 1.8mm 压到 20 微米以内是完全现实的量级跨了两个数量级。后面第 2.3 节会给出具体的实现方式和实测数据。1.3 控制内核为什么选 C 而不是脚本或梯形图选型这件事我试过三条路。最早用脚本语言拼原型好处是改一行立刻能看到效果坏处是解释执行带来的周期抖动轻易就到几百微秒而且垃圾回收会在你完全预料不到的时刻插进来把 1kHz 的循环打出一个 20ms 的空洞。第二个原型走的是 PLC 软逻辑扫描周期确实固定但算法表达力受限做个带加加速度约束的七段式 S 曲线要写上百个网络节点调试时基本靠猜。最后落到 C不是因为语言本身多优雅而是它同时满足了三件事内存布局完全可控、没有运行期垃圾回收、能直接贴着操作系统做实时调度和硬件访问。代价也很清楚。C 不帮你兜底指针越界、整数溢出、浮点异常、数据竞争全得自己防。做运动控制内核时我给自己定了几条硬规矩控制线程内一次动态内存分配都不允许出现所有中间变量用栈上结构体浮点运算统一用 double避免 float 在长距离累积插补时丢精度所有跨线程的数据交换走无锁单生产者单消费者队列。这几条定下来之后内核的确定性才勉强够看。S 曲线的参数推导、PID 的离散化、总线的收发时序全都在这个约束框架里展开。2. 三环结构在代码里到底是什么样子2.1 三个环的周期配比与职责边界伺服系统的经典三层结构是电流环、速度环、位置环从内到外一层比一层慢。电流环负责让电机绕组里的电流跟上指令周期通常在 10 到 20 微秒量级这一层基本都在驱动器固件里跑上位控制器的代码碰不到。速度环周期常见是 250 微秒到 1 毫秒位置环则是 500 微秒到 2 毫秒。这个配比不是随便定的背后的原则是内环带宽必须显著高于外环一般要求 5 到 10 倍否则两个环的动态会互相干扰表现为整定的时候怎么调都不收敛或者刚调好一个工况换个速度就发散。环路典型周期主要任务常见实现位置电流环10~20 μs电流闭环、PWM 输出驱动器固件速度环250~1000 μs速度跟踪、抑制负载扰动驱动器或控制器位置环500~2000 μs位置跟踪、轨迹跟随控制器为主位置环和速度环放在哪一层直接决定了你的 C 代码要写多少。有一种架构是把两个环都下放到驱动器控制器只负责发位置指令流那么上位机的工作量就是轨迹规划和总线时序代码量小但控制自由度低前馈补偿这类手段很难加。另一种是把速度环和位置环都拿到控制器里算驱动器工作在转矩模式这样前馈、观测器、自定义滤波全都可以自己实现代价是控制器必须承担全部的实时压力。我做的几套高精度平台都走第二条路因为跟随误差要压到几十微米以内不下放转矩模式基本没戏。2.2 PID 离散化的四个必须处理的细节教科书上的 PID 是连续域公式搬到代码里就变成了一堆需要小心处理的边界情况。位置式 PID 的离散形式是 u[k] Kp·e[k] Ki·Ts·Σe Kd·(e[k] − e[k−1])/Ts看起来简单实际写起来有四个坑必须处理。第一个是积分饱和。当执行器已经到限幅还继续积分误差会一直累积等到该反向的时候积分项要花很长时间才能吐回来表现为明显的过冲和滞后。常规做法是输出限幅配合反算抗饱和也就是把限幅前后之差乘一个系数反馈回积分器。第二个是微分冲击。如果直接对误差求微分设定值一阶跃微分项就会瞬间打出一个尖峰把电机踹一下。正确做法是对测量值求微分物理意义是阻尼设定值突变时不会产生冲击。第三个是采样周期抖动。理论推导都假设 Ts 恒定实际系统里因为调度延迟每个周期总会差几个微秒。在 1ms 周期下几微秒的抖动影响可以忽略但如果你的周期是 100 微秒5% 的周期误差足以让积分项和微分项都算偏。稳妥做法是每次循环用实际测量的 dt而不是用标称值。第四个是微分噪声。编码器读数本身带量化噪声差分放大后非常难看必须在微分通道后面串一个一阶低通截止频率一般取速度环带宽的 3 到 5 倍既能压住噪声又不引入太多相位滞后。struct PidParam { double kp 0.0, ki 0.0, kd 0.0; double ff_v 0.0, ff_a 0.0; // 速度/加速度前馈系数 double i_limit 0.0; // 积分项限幅 double out_limit 0.0; // 输出限幅 double d_lpf_hz 800.0; // 微分通道低通截止频率 }; class PidAxis { public: double update(double ref, double fb, double v_ref, double a_ref, double dt) { const double err ref - fb; // 微分作用于测量值避免设定值阶跃带来的冲击 const double d_raw (fb - fb_prev_) / dt; fb_prev_ fb; const double alpha 1.0 - std::exp(-2.0 * M_PI * p_.d_lpf_hz * dt); d_state_ alpha * (d_raw - d_state_); // 积分带反算抗饱和 integ_ p_.ki * err * dt; integ_ std::clamp(integ_, -p_.i_limit, p_.i_limit); double u p_.kp * err integ_ - p_.kd * d_state_ p_.ff_v * v_ref p_.ff_a * a_ref; // 输出限幅后把超出量反算回积分器 const double u_sat std::clamp(u, -p_.out_limit, p_.out_limit); if (u ! u_sat p_.ki 1e-12) { integ_ 0.25 * (u_sat - u); } return u_sat; } private: PidParam p_; double integ_ 0.0; double d_state_ 0.0; double fb_prev_ 0.0; };这段代码里有几个细节值得停一下。alpha的算法用的是双线性变换的简化形式周期变化时它跟着变保证截止频率不漂。反算系数取 0.25 是个经验值太大容易在限幅边界来回振荡太小又起不到快退饱和的作用具体数值和你系统的刚度有关可以在这个附近微调。另外d_state_是低通后的微分状态不是原始微分这一点在初始化时要归零否则第一次循环会打出一个假尖峰。2.3 前馈把跟随误差提前预支掉前馈是整个伺服调试里性价比最高的一招。它的思路非常直白既然跟随误差来自控制器要等误差出现才动作那我就把已知的运动信息提前加进去。速度前馈把参考速度乘一个系数加到输出上加速度前馈把参考加速度乘一个系数加上去。理想情况下如果前馈系数完全匹配系统的惯量和阻尼位置环的误差输出几乎为零PID 只在有外部扰动时才工作。实测数据比较有说服力。某套直线电机平台行程 200mm最高速度 200mm/s。不加前馈时匀速段的跟随误差稳定在 1.8mm。只加 100% 速度前馈降到 60 微米左右剩下的主要是加减速段的残余。再把加速度前馈调到 85%全行程最大跟随误差压到 15 微米。这里的关键是加速度前馈系数基本对应总惯量除以电机推力常数的倒数关系所以它有个物理意义不是纯凑数。速度前馈则主要补偿粘滞阻尼和反电动势系数带来的滞后。前馈不是越大越好。速度前馈超过 105% 之后匀速段会出现轻微的超前表现为位置曲线在指令前面跑遇到外部阻力时又猛地掉回来末端定位时能看到一次小幅回弹。加速度前馈过冲更明显会在加减速的拐点上打出一个尖峰实测波形上是一次高频抖动。我的经验是把速度前馈整到 100% 附近加速度前馈从 60% 开始逐步往上加加到末端过冲刚开始露头就退回来 5 个百分点这个位置基本是稳的。3. C 底层的工程骨架线程、内存、时序3.1 把控制线程变成不会迟到的线程实时性的第一件事不是算法是调度。普通优先级的线程在通用操作系统上跑 1kHz 循环抖动轻松到几百微秒偶尔还会被后台任务挤出一个几毫秒的空洞。这种确定性根本没法做高精度控制因为位置环的相位裕度会随抖动一起崩。要压住抖动需要一套组合拳把控制线程的调度策略设为 SCHED_FIFO优先级给到 80 左右把线程绑到一颗隔离出去的 CPU 核上让系统调度器别把别的任务塞进来用 mlockall 锁住进程内存防止运行中被换页循环内部禁止任何可能阻塞的系统调用日志、文件、网络全部挪到别的线程。周期等待也别用普通的 sleep。相对睡眠会累积误差每个周期晚一点点跑一小时就漂出去几十毫秒。正确做法是用 clock_nanosleep 的绝对时间模式配合 CLOCK_MONOTONIC每次在上一周期的目标时刻上直接加周期而不是在当前时刻上加。这样即使某一拍被延迟下一拍也会自动追回来不会形成累积漂移。void controlLoop(int period_ns) { struct timespec next; clock_gettime(CLOCK_MONOTONIC, next); while (running_.load(std::memory_order_relaxed)) { next.tv_nsec period_ns; while (next.tv_nsec 1000000000L) { next.tv_nsec - 1000000000L; next.tv_sec 1; } // 本轮的实际周期用于 PID 计算 const double dt period_ns * 1e-9; cycle(dt); // 采样、规划、闭环、输出 clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, next, nullptr); } }做完这套配置1kHz 循环的周期抖动能从几百微秒压到 10 微秒以内这个量级已经足够支撑位置环的稳定运行。有个细节容易漏不要在这个线程里调用任何会隐式加锁的东西包括某些标准库容器的扩容、字符串拼接、格式化输出。我曾经在一个日志宏上栽过跟头那个宏在异常路径上会分配内存结果每次故障触发时控制周期就被拉长一次故障本身反而查不出来。3.2 无锁数据通路让非实时任务进不来控制线程需要和界面线程、通信线程、日志线程交换数据如果把互斥锁放在控制线程里随时可能被优先级反转拖住——低优先级的界面线程持锁时被调度走控制线程只能干等一个周期就废了。解决办法是把所有跨线程交互都改成单生产者单消费者队列控制线程只做原子的读写指针操作绝不阻塞。环形队列的实现要点是三件事容量必须是 2 的幂这样取模可以用位与代替除法读写序号用std::atomicsize_t写端用 release 语义、读端用 acquire 语义保证数据先写完再更新序号队列满的时候直接丢弃或者覆盖绝不能让控制线程等待。template typename T, size_t N class SpscQueue { static_assert((N (N - 1)) 0, N must be power of two); public: bool push(const T v) { const size_t w w_.load(std::memory_order_relaxed); const size_t r r_.load(std::memory_order_acquire); if (w - r N) return false; buf_[w (N - 1)] v; w_.store(w 1, std::memory_order_release); return true; } bool pop(T v) { const size_t r r_.load(std::memory_order_relaxed); const size_t w w_.load(std::memory_order_acquire); if (r w) return false; v buf_[r (N - 1)]; r_.store(r 1, std::memory_order_release); return true; } private: T buf_[N]; std::atomicsize_t w_{0}, r_{0}; };队列里传的东西也要讲究。不要传指针不要传带堆内存的容器最好是 POD 结构体几十字节以内拷贝成本可以忽略。命令下发队列放目标位置、速度上限、加速度上限这类参数状态回传队列放当前位置、当前速度、跟随误差、状态字。每条数据带一个序号界面线程发现序号跳跃就知道中间丢帧了这个比事后查日志高效得多。3.3 状态机把现场事故挡在代码外面伺服系统最怕的不是算法不准是状态乱掉。上电时驱动器没准备好就发使能回零没完成就下运动指令急停按下后还有残余指令在队列里等着执行——这些在实验室里可能只是报个错在设备上就是撞机。我的做法是把整个生命周期写成显式状态机每个状态定义清楚允许的转换和超时任何非法转换直接拒绝并锁存故障。状态划分大概是这样初始化、上电自检、伺服使能、回零、就绪、运行、暂停、故障、急停。关键点在于运行状态只接受运动指令回零状态只接受回零指令跨界一律拒绝。急停走最高优先级路径不经过队列直接在控制周期里检查一个原子标志位一旦置位立即切断使能输出并把指令位置同步到当前位置避免恢复时出现位置跳变。故障状态必须锁存不能自动恢复必须人工确认后走一遍完整的上电流程。这套东西写起来枯燥但它是把无数次现场事故挡在代码外面的唯一办法。4. 轨迹规划S 曲线到底怎么算4.1 梯形速度曲线在精密平台上的硬伤梯形速度曲线是最容易实现的方案加速、匀速、减速三段加速度是方波。问题出在加速度的跳变上跳变意味着加加速度jerk在数学上是无穷大实际系统中表现为一次冲击激励。这个冲击会激起机械结构的高阶模态在末端定位时表现为一段衰减振荡 settling time 被拉长好几倍。行程越短、速度越高这个问题越明显因为冲击在整个运动时间里占的比重更大。S 曲线把加速度也做成连续的用恒定的加加速度段去衔接加速度曲线变成一个梯形速度曲线变成 S 形。代价是参数计算复杂了不少而且要处理行程太短达不到最高速度的情况。但收益很实在同样一套平台梯形曲线末端振荡要 80 毫秒才稳定换成 S 曲线后降到 15 毫秒以内定位节拍直接提升一大截。4.2 七段式 S 曲线的参数推导标准七段式包含加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。给定最高速度 v_max、最大加速度 a_max、最大加加速度 j_max加加速段的时长是 t1 a_max / j_max这个阶段速度从 0 涨到 a_max²/(2·j_max)。匀加速段的时长 t2 v_max/a_max − a_max/j_max如果算出来是负数说明速度还没到 v_max 加速度就已经到顶了这时候没有匀加速段退化成两段。加速段总时长 t_acc v_max/a_max a_max/j_max加速段走过的位移有个非常干净的结论s_acc v_max · t_acc / 2。这个式子在有匀加速段和没有匀加速段两种情况下都成立推导过程略繁但结论可以直接用。拿一组具体参数算一遍。v_max 500 mm/sa_max 5000 mm/s²j_max 50000 mm/s³。t1 5000/50000 0.1s这一阶段结束时速度是 5000²/(2×50000) 250 mm/s。t2 500/5000 − 0.1 0正好是临界情况没有匀加速段。加速总时长 0.2s加速位移 500×0.2/2 50mm。减速对称也是 50mm。所以这套参数下行程 100mm 刚好跑满全程没有匀速段行程 200mm 时匀速段 (200−100)/500 0.2s总运动时间 0.6s。行程短于 100mm 时达不到 500mm/s需要反解峰值速度。设 S 60mm加速位移只有 30mm代入 s_acc v·(v/a_max a_max/j_max)/2得到 30 v·(v/5000 0.1)/2整理成 v² 500v − 300000 0解得 v ≈ 352 mm/s。检验一下 352 250说明确实有匀加速段假设成立。如果解出来小于 250就要换成纯 S 形公式此时 t_acc 2·√(v/j_max)s_acc v·√(v/j_max)反解 v ((S/2)²·j_max)^(1/3)。这两个分支必须都写上否则短行程时算出来的曲线会超调。4.3 在线插补与浮点累积误差规划层算出七段的边界时刻和对应的速度、加速度控制周期里只需要根据当前时间 t 判断落在哪一段用解析式直接算位置而不是每周期做一次数值积分。这个选择很关键积分会累积浮点误差跑一万个周期之后位置可能偏出几个微米解析式每次从 t 重新算误差不累积。段内位置的公式就是标准运动学公式加加速段用三次多项式匀加速段用二次匀速段用一次写成一个 switch 就够了。多轴插补的关键是统一时间标尺。假设 X 轴要走 200mmY 轴要走 50mm如果各自按自己的最大速度规划两轴到达时间不同走出来的就是折线而不是直线。正确做法是先算出合成运动的路径长度按路径长度做一次规划得到总时间 T然后每个周期用相同的归一化时间参数去查各轴的位置。这样两轴严格同步任意时刻实际位置都落在指令直线上。圆弧插补同理只是位置由圆心、半径和角度参数算出再投影到各轴。5. 反馈通道从编码器读数到闭环收敛5.1 分辨率换算与量化噪声的现实差距23 位绝对值编码器单圈 8388608 个计数配 5mm 导程丝杠一个计数对应 0.6 纳米。这个数字在物理上没有意义因为机械系统的热膨胀在 1 摄氏度下就能带来几微米的位移编码器再准也测不准。实际能用到的分辨率受限于噪声底干净的布线和良好的接地能做到 20 到 50 纳米布线糟糕时噪声能到微米级。噪声来自哪里编码器信号线的屏蔽层单端接地还是双端接地、动力线和信号线是否分开走、驱动器地和控制器地是否共地、开关电源的纹波有多大。这些问题在图纸上看不出来装机之后才一个个暴露。我遇到过一次跟随误差莫名抖动的案例查了半天算法没找到原因最后发现是编码器线和电机动力线绑在同一个线槽里把屏蔽层重新做单端接地、动力线换成屏蔽双绞并拉开 10cm 距离抖动直接消失了。噪声处理不能只靠接线软件侧也要有一道。速度是由位置差分算出来的差分会把噪声放大所以速度通道必须滤波。一阶低通的截止频率取速度环带宽的 3 到 5 倍比较合适比如速度环带宽 200Hz低通取 800Hz。滤波带来的相位滞后要算清楚一阶低通在 100Hz 处的相位滞后约 7 度这个量会吃掉一部分相位裕度整定时要预留出来。5.2 总线周期、时延与相位裕度的关系用实时工业以太网总线EtherCAT 这一类时控制器的位置环周期和总线周期是绑定的。总线周期 1ms位置环就只能 1kHz因为反馈数据要等一个总线周期才能拿回来。整个回路的时延由四部分组成编码器采样、总线传输、控制器计算、指令下发加起来通常是 2 到 3 个总线周期。这个时延在频域上就是纯相位滞后滞后角等于 360° × f × T_d。算一下就知道多要命。假设总时延 2ms在 100Hz 处相位滞后 72 度。如果你的位置环在这个频率附近本来只有 40 度的相位裕度加上这 72 度直接变成负的系统必然振荡。这就是为什么高精度平台的总线周期会往 500 微秒甚至 250 微秒压不是为了数据量是为了把这部分滞后压下去。时延没法完全消除但可以补偿。常用做法是用速度外推一个周期把反馈值往前推 T_d 时间fb_pred fb v·T_d。这个预测在匀速段非常准加减速段会有一点偏差但配合加速度项修正后误差可以控制在很小的范围。我的做法是只补偿一个周期的固定时延剩下的用降低 Kv 来换稳定裕度这样既不会过度依赖模型也不至于把带宽浪费掉。6. 上机调参的真实顺序6.1 调参必须按固定顺序走跳步必翻车调参的合理顺序是空载单轴、电流环自整定、速度环、位置环、带负载、全行程、多轴联动。每一步都有明确的验收标准不达标绝不进入下一步。跳过速度环直接调位置环是最常见的错误结果是位置环怎么调都软绵绵因为底下的速度环根本没跟上。空载阶段先确认方向和使能逻辑用手转电机轴感受阻尼是不是均匀有卡顿说明电流环或换相有问题。电流环一般由驱动器自动辨识需要输入电机电感、电阻、反电动势系数辨识完看阶跃响应的上升时间是否在预期范围。速度环整定时给一个阶跃速度指令看响应有没有明显超调和振荡用示波器或者控制器内部的高速采样抓波形。位置环最后调先只开 P慢慢加到临界振荡再退回来 30%然后加前馈。6.2 阶跃、正弦、频响三种测试各管什么阶跃响应看的是时域指标上升时间、超调量、整定时间。位置阶跃给 1mm采样率足够高的话能清楚看到有没有过冲、过冲后几次收敛。正弦跟随测试看的是频域跟踪能力给不同频率的正弦指令测量幅值衰减和相位滞后画出伯德图就能算出带宽和相位裕度。频响测试最费时间但信息量最大扫频范围一般从 1Hz 到 500Hz每个频点跑几个周期取稳态。测试类型输入信号关键指标目标参考值阶跃响应1mm 位置阶跃超调量、整定时间超调5%整定30ms正弦跟随1~50Hz 正弦幅值衰减、相位滞后20Hz 处衰减3dB频响扫描1~500Hz 扫频带宽、相位裕度带宽80Hz裕度35°匀速跟随恒定速度稳态跟随误差30μm 200mm/s6.3 一组实测参数与调整过程某套直线电机平台的最终参数记录如下供参考但不建议直接照搬因为机械结构差异太大。参数初始值最终值调整理由位置环 Kp25 1/s60 1/s逐步加到 85 出现啸叫后退回位置环 Ki08消稳态误差再大就过冲速度环 Kp0.82.4配合带宽提升速度环 Ki30120负载扰动抑制速度前馈0100%匀速段误差基本抹平加速度前馈085%加减速段残余压到 15μm微分低通500Hz900Hz噪声可控前提下放宽调整过程里最耗时的是位置环 Kp 那一步。从 25 加到 60 很顺60 到 85 之间开始出现 180Hz 附近的持续振荡用加速度计一测正好是平台一阶模态。这时候不能硬往上加要么做陷波滤波把这个频点压下去要么就停在 60。我选了后者因为陷波滤波虽然能压住振荡但会引入额外相位滞后在别的频段埋雷。7. 常见问题排查实录7.1 抖动、异响、超调、跟随误差大的分离思路排查的基本原则是先分清是电气问题、机械问题还是算法问题。有个简单的判别方法断开位置环只跑速度环如果抖动还在问题在速度环以内或者机械如果速度环很干净一闭位置环就抖那就是位置环参数或者反馈通道的问题。现象可能原因验证方法处理手段静止时高频抖动编码器噪声、增益过高断电手推看反馈是否跳变加反馈滤波、降 Kv匀速段跟随误差大速度前馈缺失、阻尼大提高前馈看是否改善速度前馈补到 100%加减速段过冲加速度前馈过大、积分饱和减小前馈看波形前馈退 5%检查抗饱和末端衰减振荡机械模态被激发加速度计测频率降加加速度、加陷波定位后缓慢漂移积分饱和、重力负载看积分项是否顶到限幅加前馈配重、限幅反算偶发丢步或报警总线误码、电源跌落查总线错误计数器检查接地与屏蔽7.2 几条踩出来的经验单位混乱是新人最容易踩的坑而且踩了之后极难发现。位置用毫米、速度用毫米每秒、加速度用毫米每二次方秒这套单位在数值上跨度很大编码器计数转毫米的系数一旦写错一位小数表现是位置差十倍但不报错。我的做法是在代码里给每个物理量定义一个强类型包装编译期就能挡住单位混用运行时再加一个量程检查超出物理可能的范围直接进故障。实时线程里绝对不能有隐式分配。标准库的std::string、std::vector、std::function在特定操作下都会分配内存一旦分配触发缺页周期就废了。我现在的规矩是控制周期函数里只允许出现定长数组、POD 结构体和预先分配的缓冲区任何需要动态构造的东西都在初始化阶段搞定。最后一条是关于日志的。调试期总想在控制线程里打点东西看但格式化输出本身就是几十微秒的开销打完点波形就变样了。正确的做法是控制线程只往环形缓冲里塞裸数据格式化和落盘全放到低优先级线程。这样既能看到真实波形又不影响控制节拍。我个人在实际操作中的体会是伺服系统调到最后拼的不是算法有多花哨而是对每一个误差来源的清楚认知。知道那 2 毫米的跟随误差是被位置环增益挤出来的就知道该往前馈上使劲知道末端那 15 毫秒的振荡是被加速度跳变激出来的就知道该动加加速度而不是动 PID。参数表可以抄这条因果链抄不走只能自己一段段跑波形跑出来。