2、伺服控制基础：伺服系统组成、三环控制（电流环、速度环、位置环）原理、常用控制算法（PID、前馈、陷波）简介

伺服控制是运动控制领域的核心，也是FPGA硬件加速最典型的应用场景之一。在进入FPGA实现细节之前，必须先建立对伺服系统整体架构和算法原理的清晰认知。本章将系统梳理伺服系统的物理组成、三环控制的结构与整定逻辑，以及PID、前馈、陷波三类常用算法的工程定位。理解这些基础，后续的硬件加速设计才能有的放矢——知道哪些计算需要并行化、哪些数据流需要低延迟、哪些资源需要做定点化处理。

2.1 伺服系统的物理组成与信号链路

伺服系统（Servo System）的本质是一个闭环控制系统，其目标是让机械输出（位置、速度、力矩）精确跟随指令输入。一个完整的伺服系统由控制器、驱动器、电机、反馈传感器和机械负载五大部分组成。在工业自动化场景中，控制器通常为PLC或运动控制卡，驱动器为伺服驱动器，电机为永磁同步电机（PMSM）或直流无刷电机（BLDC），反馈传感器为增量式编码器或值编码器。

从信号流的角度看，伺服系统存在两条关键链路：指令链路和反馈链路。指令链路从控制器发出位置指令开始，经过三环调节后生成PWM信号驱动功率级，最终转化为电机转矩；反馈链路则从编码器/电流传感器采集信号，经过解码和调理后回送到各环的输入端。这两条链路的延迟直接决定了系统的动态响应能力，也是FPGA加速的核心价值所在。

2.2 三环控制原理与整定逻辑

三环控制是伺服系统的核心架构，从内到外依次为电流环、速度环、位置环。每一环都是一个独立的闭环控制器，外环的输出作为内环的指令输入。这种嵌套结构的核心思想是：逐层约束、逐层加速——内环响应最快，处理电气量；外环响应较慢，处理机械量。

2.2.1 电流环（最内环）

电流环控制的是电机三相绕组的电流，直接决定电磁转矩。在PMSM控制中，通常采用FOC（磁场定向控制）策略，将三相静止坐标系下的电流通过Clark变换和Park变换转换到旋转dq坐标系，实现转矩电流（iq）和励磁电流（id）的解耦控制。电流环的PI控制器输出为dq轴电压指令，再经过逆Park变换和SVPWM调制生成三相PWM波。

电流环的带宽通常设计为PWM载波频率的1/10~1/20。例如，当PWM频率为10kHz时，电流环带宽约为500Hz~1kHz。电流环的采样必须与PWM载波同步，在载波谷底或峰顶触发ADC采样，以避免开关噪声干扰。

2.2.2 速度环（中间环）

速度环的输入是位置环输出的速度指令，反馈来自编码器位置信号的微分（或专用测速发电机）。速度环的PI控制器输出为电流指令（即iq指令）。速度环需要处理编码器量化噪声带来的微分抖动，通常需要加入低通滤波或观测器（如龙贝格观测器）来平滑速度估计。

速度环带宽一般为电流环带宽的1/5~1/10，典型值在100Hz~500Hz。速度环的整定直接影响系统的抗负载扰动能力和速度平稳性。

2.2.3 位置环（最外环）

位置环的输入是上位控制器发出的位置指令，反馈来自编码器的位置。位置环通常采用P（比例）控制或PD控制，输出为速度指令。位置环带宽受限于机械系统的固有频率，一般在10Hz~100Hz。位置环增益（Kp）设置过高会导致机械振动和超调。

2.3 常用控制算法：PID、前馈与陷波

在三环控制的基础上，实际工程中还需要引入多种辅助算法来提升性能。PID是最基础的反馈控制器，前馈用于提高跟踪精度，陷波器用于抑制机械谐振。

2.3.1 PID控制器

PID控制器由比例（P）、积分（I）、微分（D）三部分组成。在伺服系统中，电流环和速度环通常使用PI控制器（微分项会放大噪声，较少使用），位置环使用P或PD控制器。数字PID的实现公式为：

// 位置式PID（增量式更常用）
u[k] = Kp * e[k] + Ki * Σe[i] + Kd * (e[k] - e[k-1])

// 增量式PID（FPGA中推荐）
Δu[k] = Kp * (e[k] - e[k-1]) + Ki * e[k] + Kd * (e[k] - 2*e[k-1] + e[k-2])

在FPGA中实现PID时，需要特别注意积分饱和（windup）问题。当输出达到限幅值时，积分项应停止累加，否则会导致系统恢复响应变慢。工程上通常采用条件积分法或反算积分法来处理。

2.3.2 前馈控制

前馈控制是一种开环补偿策略，它直接根据指令的变化率（速度前馈）或变化加速度（加速度前馈）提前施加控制量，以减小反馈控制的滞后。在位置环中，典型的前馈公式为：

速度前馈：Vff = Kff_v * dP_ref/dt
加速度前馈：Aff = Kff_a * d²P_ref/dt²
最终速度指令 = 位置环输出 + Vff + Aff

前馈系数通常通过实验整定：先关闭前馈，记录跟踪误差；然后逐步增大前馈系数，观察误差减小的效果。前馈不会影响系统稳定性，但能显著提高轨迹跟踪精度。

2.3.3 陷波滤波器

机械系统（如丝杠、皮带、减速机）存在固有谐振频率，当控制带宽接近该频率时，会激发机械振荡。陷波滤波器（Notch Filter）在谐振频率处产生一个深度衰减的凹口，从而抑制谐振。数字陷波器的传递函数为：

H(z) = (1 - 2*cos(ω0)*z⁻¹ + z⁻²) / (1 - 2*r*cos(ω0)*z⁻¹ + r²*z⁻²)

其中 ω0 = 2π*f0/fs（f0为谐振频率，fs为采样率）
r 为陷波深度系数（0 < r < 1，越小陷波越深）

2.4 案例拆解：三环整定与FPGA加速效果

下面通过一个实际的伺服驱动器开发案例，展示三环控制从理论到工程落地的完整过程。

项目背景：某机器人关节模组需要开发一款高动态响应伺服驱动器，电机为400W PMSM（极对数4），编码器为17bit值编码器（BISS-C协议），PWM频率10kHz，要求电流环带宽≥1kHz，速度环带宽≥200Hz，位置环带宽≥30Hz。

初始现象：在MCU（STM32H750，主频480MHz）上实现三环控制，电流环实际执行周期为80μs（受限于ADC读取和FOC计算），速度环周期为400μs，位置环周期为2ms。实测电流环带宽仅约600Hz，速度环带宽约120Hz，无法满足设计要求。同时，在高速运行时出现明显的机械谐振（约2.3kHz），导致噪声和振动。

分析判断过程：

1. 电流环延迟主要来自ADC采样（SPI读取约3μs）、FOC变换（约15μs）和PI运算（约5μs），加上任务调度开销，总延迟约80μs。根据带宽公式 BW ≈ 1/(2π*T_delay)，80μs延迟对应约2kHz理论带宽，但实际受限于采样保持和PWM更新延迟，仅达到600Hz。
2. 速度环微分噪声明显，编码器在低速时量化误差导致速度波动，需要增加低通滤波，但滤波又引入了额外相位延迟。
3. 2.3kHz的机械谐振频率接近速度环带宽的10倍，但电流环的PWM谐波（10kHz的边带）可能激励该谐振。

FPGA改造方案：

1. 将三环全部迁移到FPGA（Xilinx Artix-7 XC7A35T）中实现，MCU仅负责通信和参数管理。
2. 电流环采用全流水线设计：ADC采样（并行SPI接口，1μs）→ Clark/Park变换（组合逻辑+流水线，2μs）→ PI运算（3个时钟周期，0.3μs）→ 逆Park+SVPWM（2μs）。总延迟约5μs，电流环周期稳定在50μs（与PWM载波同步）。
3. 速度环使用M/T测速法，编码器计数在FPGA中并行处理，速度更新周期100μs，并加入一阶低通滤波（截止频率500Hz）。
4. 在电流环输出前加入2.3kHz数字陷波器，衰减深度-20dB，品质因数Q=5。

结果对比：

风险与代价：FPGA开发周期比MCU长（约3周 vs 1周），调试难度更高，需要掌握硬件描述语言和时序分析。此外，FPGA的定点化运算需要仔细处理数据位宽，避免溢出和精度损失。在本案例中，电流环PI采用Q15格式（16位定点），速度环采用Q12格式，位置环采用Q20格式，经过仿真验证精度满足要求。

复盘经验：三环控制的性能瓶颈往往不在算法本身，而在数据通路延迟。FPGA的核心优势在于将ADC读取、坐标变换、PI运算、PWM更新等环节通过流水线并行化，将总延迟从微秒级压缩到纳秒级。同时，FPGA的确定性时序（无中断延迟、无任务调度抖动）对于伺服控制至关重要。

2.5 常见误区与设计要点总结