伺服电机控制原理：三环PID与FOC矢量控制

伺服电机区别于步进电机的核心是闭环反馈——它不光发出运动指令，还能精确知道自己转到了哪里、转了多快、用了多大力。

伺服系统架构

code

┌──────────────────────────────────────────────────────┐
│                    伺服驱动器                          │
│                                                       │
│  位置指令 ──→ [位置环] ──→ [速度环] ──→ [电流环] ──→ 电机 │
│                ↑            ↑            ↑           │
│                │            │            │           │
│            编码器       速度观测器    电流传感器       │
│            (位置反馈)     (速度反馈)    (电流反馈)      │
└──────────────────────────────────────────────────────┘

三环从内到外：电流环（最快）→ 速度环 → 位置环（最慢）

环路	执行频率	作用
位置环	1kHz	控制目标位置
速度环	5~10kHz	控制转速，平滑过渡
电流环	20~50kHz	控制扭矩/力矩，最底层

三环 PID 控制

位置环（最外层）

typedef struct {
    float kp, ki, kd;          // PID 参数
    float setpoint;            // 目标位置（编码器计数）
    float integral;
    float prev_error;
    float output_limit;        // 输出限幅（作为速度环的输入，单位 RPM）
} PositionController;

float position_pid(PositionController *ctrl, float current_pos) {
    float error = ctrl->setpoint - current_pos;
    
    // 比例
    float p = ctrl->kp * error;
    
    // 积分（带限幅防饱和）
    ctrl->integral += error;
    ctrl->integral = CLAMP(ctrl->integral, -ctrl->output_limit, ctrl->output_limit);
    float i = ctrl->ki * ctrl->integral;
    
    // 微分
    float d = ctrl->kd * (error - ctrl->prev_error);
    ctrl->prev_error = error;
    
    // 输出 → 成为速度环的 setpoint
    return CLAMP(p + i + d, -ctrl->output_limit, ctrl->output_limit);
}

速度环（中间层）

typedef struct {
    float kp, ki, kd;
    float setpoint;            // 目标速度（RPM），来自位置环输出
    float integral;
    float prev_error;
    float output_limit;        // 输出限幅（作为电流环的输入，单位 A）
} SpeedController;

float speed_pid(SpeedController *ctrl, float current_speed) {
    float error = ctrl->setpoint - current_speed;
    
    float p = ctrl->kp * error;
    ctrl->integral += error;
    float i = ctrl->ki * ctrl->integral;
    float d = ctrl->kd * (error - ctrl->prev_error);
    ctrl->prev_error = error;
    
    // 输出 → 成为电流环的 setpoint（Iq_ref）
    return CLAMP(p + i + d, -ctrl->output_limit, ctrl->output_limit);
}

电流环（最内层，FOC 中的 Iq/Id 控制）

typedef struct {
    float kp, ki;
    float setpoint;            // Iq_ref（扭矩电流）或 Id_ref（励磁电流）
    float integral;
    float output_limit;        // 电压限幅（Duty 或 Vq/Vd）
} CurrentController;

float current_pi(CurrentController *ctrl, float current_fb) {
    float error = ctrl->setpoint - current_fb;
    
    float p = ctrl->kp * error;
    ctrl->integral += error;
    float i = ctrl->ki * ctrl->integral;
    
    return CLAMP(p + i, -ctrl->output_limit, ctrl->output_limit);
}

FOC（磁场定向控制）

FOC 是伺服控制的核心算法，核心思想是把三相交流电机的控制，转换成对直流电机的控制。

Clark 变换（三相 → 两相静止）

将 A/B/C 三相电流转换为 α-β 两相坐标：

code

Ia + Ib + Ic = 0

Iα = Ia
Iβ = (Ia + 2Ib) / √3

void clarke_transform(float ia, float ib, float ic, 
                      float *i_alpha, float *i_beta) {
    *i_alpha = ia;
    *i_beta  = (ia + 2.0f * ib) / 1.7320508f;  // sqrt(3)
}

Park 变换（两相静止 → 两相旋转）

将 α-β 旋转到转子磁场同步的 d-q 坐标系：

code

Id =  Iα * cos(θ) + Iβ * sin(θ)
Iq = -Iα * sin(θ) + Iβ * cos(θ)

void park_transform(float i_alpha, float i_beta, float theta,
                    float *i_d, float *i_q) {
    float c = cosf(theta);
    float s = sinf(theta);
    *i_d =  i_alpha * c + i_beta * s;
    *i_q = -i_alpha * s + i_beta * c;
}

反 Park 变换

将控制量 Vd/Vq 转回 α-β：

void inverse_park(float vd, float vq, float theta,
                  float *v_alpha, float *v_beta) {
    float c = cosf(theta);
    float s = sinf(theta);
    *v_alpha = vd * c - vq * s;
    *v_beta  = vd * s + vq * c;
}

FOC 完整流程

code

电流采样 → Clarke → Park → PI(Id) + PI(Iq) → 反Park → SVPWM → 三相逆变
    ↑                                                  │
    └──────────── 编码器 → 角度 θ ←────────────────────────┘

void foc_current_loop(FOC *foc, float ia, float ib, float ic, 
                      float theta, float iq_ref) {
    // 1. Clarke 变换
    float i_alpha, i_beta;
    clarke_transform(ia, ib, ic, &i_alpha, &i_beta);
    
    // 2. Park 变换
    float id, iq;
    park_transform(i_alpha, i_beta, theta, &id, &iq);
    
    // 3. Id 环（励磁电流，通常设 0）
    float vd = current_pi(&foc->id_ctrl, id);
    
    // 4. Iq 环（扭矩电流 = 速度环输出）
    float vq = current_pi(&foc->iq_ctrl, iq);
    
    // 5. 反 Park 变换
    float v_alpha, v_beta;
    inverse_park(vd, vq, theta, &v_alpha, &v_beta);
    
    // 6. SVPWM → 六路 PWM
    svpwm_update(foc->svpwm, v_alpha, v_beta);
}

SVPWM（空间矢量脉宽调制）

六个开关管组成三相全桥，共有 8 种开关状态（6 个有效矢量 + 2 个零矢量）：

code

状态    上管 (A,B,C)    矢量
U0      000             零矢量
U1      100             V1 (0°)
U2      110             V2 (60°)
U3      010             V3 (120°)
U4      011             V4 (180°)
U5      001             V5 (240°)
U6      101             V6 (300°)
U7      111             零矢量

任意电压矢量 Vref  = V1·T1 + V2·T2（相邻两矢量合成）

void svpwm_calc_duty(SVPWM *svpwm, float v_alpha, float v_beta, 
                     float vdc) {
    // 1. 扇区判断
    float v1 = v_beta;
    float v2 = 0.866025f * v_alpha - 0.5f * v_beta;   // sin60*α - cos60*β
    float v3 = -0.866025f * v_alpha - 0.5f * v_beta;   // -sin60*α - cos60*β
    
    int sector = 0;
    if (v1 > 0) sector |= 1;
    if (v2 > 0) sector |= 2;
    if (v3 > 0) sector |= 4;
    // sector ∈ {1,2,3,4,5,6}
    
    // 2. 矢量作用时间计算（以扇区1为例）
    float x = v_beta;
    float y = 0.866025f * v_alpha + 0.5f * v_beta;
    float z = -0.866025f * v_alpha + 0.5f * v_beta;
    
    float t1, t2;
    switch (sector) {
        case 1: t1 = z; t2 = y; break;
        case 2: t1 = y; t2 = -x; break;
        case 3: t1 = -z; t2 = x; break;
        case 4: t1 = -x; t2 = z; break;
        case 5: t1 = x; t2 = -y; break;
        case 6: t1 = -y; t2 = -z; break;
    }
    
    // 3. 过调制处理：如果 T1+T2 > Ts，等比例缩小
    float ts = 1.0f;  // PWM 周期归一化
    if (t1 + t2 > ts) {
        float scale = ts / (t1 + t2);
        t1 *= scale;
        t2 *= scale;
    }
    float t0 = (ts - t1 - t2) / 2;  // 零矢量时间
    
    // 4. 输出三相占空比到定时器比较寄存器
    svpwm->ta = (int)((t0 + t1 + t2) * PWM_PERIOD >> sector);
    svpwm->tb = (int)((t0 + t2) * PWM_PERIOD >> sector);
    svpwm->tc = (int)(t0 * PWM_PERIOD >> sector);
}

编码器闭环反馈

增量式编码器

输出 A/B/Z 三相脉冲，STM32 定时器编码器模式直接计数：

void encoder_init(void) {
    TIM_EncoderInterfaceConfig(TIM2, 
        TIM_EncoderMode_TI12,     // A+B 双沿计数 = 4倍频
        TIM_ICPolarity_Rising, 
        TIM_ICPolarity_Rising);
    TIM_SetAutoreload(TIM2, 0xFFFF);  // 16位计数器
    TIM_Cmd(TIM2, ENABLE);
}

// 速度计算（M法：单位时间脉冲数 / 编码器分辨率）
float calc_speed_m(int pulses, float dt) {
    return (pulses * 60.0f) / (ENCODER_PPR * 4 * dt);  // RPM
}

// 位置计算
int32_t calc_position(void) {
    return TIM_GetCounter(TIM2);  // 直接读计数值
}

绝对式编码器（SPI/SSI 接口）

uint16_t read_absolute_position(void) {
    uint8_t tx[2] = {0x00, 0x00};
    uint8_t rx[2];
    
    SPI_CS_LOW();
    SPI_TransmitReceive(tx, rx, 2);
    SPI_CS_HIGH();
    
    return ((rx[0] & 0x3F) << 8) | rx[1];  // 12~14位绝对值
}

伺服 vs 步进电机的本质区别

特性	伺服电机	步进电机
控制方式	闭环（编码器反馈）	开环（脉冲计数）
过载能力	3~5 倍额定扭矩	过载即失步
高速性能	3000~6000 RPM	通常 < 1000 RPM
低速性能	平稳，无抖动	低频振动明显
丢步	不会丢（闭环纠偏）	过载/高速会丢步
位置精度	编码器分辨率决定	步距角决定
成本	较高（控制器+编码器）	较低
控制器	FOC + SVPWM	脉冲序列 + 微步细分

整定技巧

三环 PID 调参顺序

code

1. 电流环（Id / Iq）      先调，保证电流跟随
2. 速度环                  在电流环稳定后调
3. 位置环                  最后调，响应要快但不过冲

整定口诀

code

Kp 调大 → 响应快，但过大会震荡
Ki 调大 → 消除静差，但过大会超调
Kd 调大 → 抑制过冲，但过大会放大噪声

实操步骤：
1. 只加 Kp，调到临界震荡
2. Kp × 0.6，开始加 Ki
3. Ki 调到稳态误差消除
4. Kd = 0（大多数伺服不需要微分项）

// 通用 PI 控制器（工业伺服常用 PI，不用 D）
typedef struct {
    float kp, ki;
    float integral;
    float integral_limit;  // 积分限幅防饱和
    float output_limit;
} PI_Controller;

float pi_update(PI_Controller *pi, float error) {
    float p = pi->kp * error;
    pi->integral += error;
    pi->integral = CLAMP(pi->integral, -pi->integral_limit, pi->integral_limit);
    float i = pi->ki * pi->integral;
    return CLAMP(p + i, -pi->output_limit, pi->output_limit);
}

总结

伺服电机控制五要素：

三环级联：位置环 → 速度环 → 电流环，从外到内、从慢到快
FOC 矢量控制：Clark → Park → PI → 反Park → SVPWM
编码器反馈：增量式（A/B计数）或绝对式（SPI读取）
PI 调参：先电流、后速度、最后位置，增量减小防震荡
防积分饱和：每环积分必须限幅（clamping）

伺服电机控制原理：三环PID与FOC矢量控制

伺服电机控制原理：三环PID与FOC矢量控制

伺服系统架构

三环 PID 控制

位置环（最外层）

速度环（中间层）

电流环（最内层，FOC 中的 Iq/Id 控制）

FOC（磁场定向控制）

Clark 变换（三相 → 两相静止）

Park 变换（两相静止 → 两相旋转）

反 Park 变换

FOC 完整流程

SVPWM（空间矢量脉宽调制）

编码器闭环反馈

增量式编码器

绝对式编码器（SPI/SSI 接口）

伺服 vs 步进电机的本质区别

整定技巧

三环 PID 调参顺序

整定口诀

总结

相关推荐

ESP32物联网全栈方案：MQTT多传感器采集 + FastAPI + Tauri桌面应用

I2C与SPI总线详解：从协议差异到传感器手册实战接入

Android 系统驱动开发流程详解

评论 (0)

伺服电机控制原理：三环PID与FOC矢量控制

伺服系统架构#

三环 PID 控制#

位置环（最外层）#

速度环（中间层）#

电流环（最内层，FOC 中的 Iq/Id 控制）#

FOC（磁场定向控制）#

Clark 变换（三相 → 两相静止）#

Park 变换（两相静止 → 两相旋转）#

反 Park 变换#

FOC 完整流程#

SVPWM（空间矢量脉宽调制）#

编码器闭环反馈#

增量式编码器#

绝对式编码器（SPI/SSI 接口）#

伺服 vs 步进电机的本质区别#

整定技巧#

三环 PID 调参顺序#

整定口诀#

总结#

相关推荐

ESP32物联网全栈方案：MQTT多传感器采集 + FastAPI + Tauri桌面应用

I2C与SPI总线详解：从协议差异到传感器手册实战接入

Android 系统驱动开发流程详解

评论 (0)

伺服系统架构

三环 PID 控制

位置环（最外层）

速度环（中间层）

电流环（最内层，FOC 中的 Iq/Id 控制）

FOC（磁场定向控制）

Clark 变换（三相 → 两相静止）

Park 变换（两相静止 → 两相旋转）

反 Park 变换

FOC 完整流程

SVPWM（空间矢量脉宽调制）

编码器闭环反馈

增量式编码器

绝对式编码器（SPI/SSI 接口）

伺服 vs 步进电机的本质区别

整定技巧

三环 PID 调参顺序

整定口诀

总结