4. 转向执行电机位置传感器信号处理

任务定义

在SBW线控转向系统中，方向盘与转向执行器无机械连接，驾驶员的路感完全由路感模拟电机（通常为永磁同步电机PMSM）主动产生。本案例实现路感模拟电机的基础控制策略：采用磁场定向控制（FOC）框架，以目标方向盘转矩为输入，通过电流环PI控制器输出d/q轴电压，并经SVPWM调制生成逆变器开关信号。核心目标是：给定目标转矩指令，电流环能快速跟踪q轴电流指令，确保电机输出转矩与目标一致。

系统架构与信号流

核心代码实现

以下代码实现路感模拟电机的电流环FOC控制核心逻辑，包含PI控制器、坐标变换与SVPWM占空比计算。代码采用C语言编写，面向嵌入式实时控制场景。

// 路感模拟电机FOC控制 - 电流环核心实现
// 依赖：数学库math.h，无其他外部依赖

#include <math.h>
#include <stdint.h>

// ========== 控制参数结构体 ==========
typedef struct {
    float Kp;           // 比例增益
    float Ki;           // 积分增益
    float integral;     // 积分累加值
    float out_max;      // 输出限幅
    float out_min;      // 输出下限
} PI_Controller;

typedef struct {
    float id_ref;       // d轴目标电流 (A)
    float iq_ref;       // q轴目标电流 (A)
    float vd;           // d轴输出电压 (V)
    float vq;           // q轴输出电压 (V)
    float v_alpha;      // α轴电压
    float v_beta;       // β轴电压
    float ta;           // A相占空比
    float tb;           // B相占空比
    float tc;           // C相占空比
    float vdc;          // 母线电压 (V)
    float ts;           // 采样周期 (s)
    PI_Controller pi_d; // d轴PI
    PI_Controller pi_q; // q轴PI
} FOC_Controller;

// ========== PI控制器初始化 ==========
void PI_Init(PI_Controller *pi, float kp, float ki, float out_max) {
    pi->Kp = kp;
    pi->Ki = ki;
    pi->integral = 0.0f;
    pi->out_max = out_max;
    pi->out_min = -out_max;
}

// ========== PI控制器更新 ==========
float PI_Update(PI_Controller *pi, float error, float ts) {
    float output;
    // 积分项（带限幅防饱和）
    pi->integral += pi->Ki * error * ts;
    if (pi->integral > pi->out_max) pi->integral = pi->out_max;
    if (pi->integral < pi->out_min) pi->integral = pi->out_min;
    
    // 比例+积分输出
    output = pi->Kp * error + pi->integral;
    
    // 输出限幅
    if (output > pi->out_max) output = pi->out_max;
    if (output < pi->out_min) output = pi->out_min;
    return output;
}

// ========== Clark变换 (三相→αβ) ==========
void Clark_Transform(float ia, float ib, float ic, float *i_alpha, float *i_beta) {
    *i_alpha = ia;
    *i_beta = (ia + 2.0f * ib) / sqrtf(3.0f);
}

// ========== Park变换 (αβ→dq) ==========
void Park_Transform(float i_alpha, float i_beta, float theta, float *id, float *iq) {
    float sin_t = sinf(theta);
    float cos_t = cosf(theta);
    *id = i_alpha * cos_t + i_beta * sin_t;
    *iq = -i_alpha * sin_t + i_beta * cos_t;
}

// ========== 逆Park变换 (dq→αβ) ==========
void Inverse_Park(float vd, float vq, float theta, float *v_alpha, float *v_beta) {
    float sin_t = sinf(theta);
    float cos_t = cosf(theta);
    *v_alpha = vd * cos_t - vq * sin_t;
    *v_beta = vd * sin_t + vq * cos_t;
}

// ========== SVPWM占空比计算 ==========
void SVPWM_Calc(float v_alpha, float v_beta, float vdc, float *ta, float *tb, float *tc) {
    // 归一化到0~1范围
    float vmax = vdc / sqrtf(3.0f);  // 最大不失真相电压幅值
    float v1 = v_alpha / vmax;
    float v2 = v_beta / vmax;
    
    // 计算基本矢量作用时间（采用对称SVPWM）
    float t1, t2;
    int sector;
    
    // 判断扇区
    if (v1 > 0 && v2 > 0 && v2 < sqrtf(3.0f) * v1) sector = 1;
    else if (v1 > 0 && v2 > sqrtf(3.0f) * v1) sector = 2;
    else if (v1 < 0 && v2 > -sqrtf(3.0f) * v1) sector = 3;
    else if (v1 < 0 && v2 < -sqrtf(3.0f) * v1) sector = 4;
    else if (v1 < 0 && v2 < sqrtf(3.0f) * v1) sector = 5;
    else sector = 6;
    
    // 各扇区作用时间计算（简化实现）
    float ts = 1.0f;  // 归一化周期
    switch (sector) {
        case 1:
            t1 = v1 - v2 / sqrtf(3.0f);
            t2 = 2.0f * v2 / sqrtf(3.0f);
            break;
        case 2:
            t1 = v1 + v2 / sqrtf(3.0f);
            t2 = -v1 + v2 / sqrtf(3.0f);
            break;
        case 3:
            t1 = 2.0f * v1 / sqrtf(3.0f);
            t2 = v1 + v2 / sqrtf(3.0f);
            break;
        case 4:
            t1 = -v1 + v2 / sqrtf(3.0f);
            t2 = -2.0f * v1 / sqrtf(3.0f);
            break;
        case 5:
            t1 = -v1 - v2 / sqrtf(3.0f);
            t2 = v1 - v2 / sqrtf(3.0f);
            break;
        default:
            t1 = -2.0f * v2 / sqrtf(3.0f);
            t2 = -v1 - v2 / sqrtf(3.0f);
            break;
    }
    
    // 零矢量分配
    float t0 = ts - t1 - t2;
    if (t0 < 0) {  // 过调制处理
        float scale = ts / (t1 + t2);
        t1 *= scale;
        t2 *= scale;
        t0 = 0;
    }
    
    // 生成三相占空比（对称PWM）
    float ta_on = t0 / 4.0f;
    float tb_on = ta_on + t1 / 2.0f;
    float tc_on = tb_on + t2 / 2.0f;
    
    // 根据扇区分配
    switch (sector) {
        case 1: *ta = ta_on; *tb = tb_on; *tc = tc_on; break;
        case 2: *ta = tb_on; *tb = ta_on; *tc = tc_on; break;
        case 3: *ta = tc_on; *tb = ta_on; *tc = tb_on; break;
        case 4: *ta = tc_on; *tb = tb_on; *tc = ta_on; break;
        case 5: *ta = tb_on; *tb = tc_on; *tc = ta_on; break;
        default: *ta = ta_on; *tb = tc_on; *tc = tb_on; break;
    }
}

// ========== FOC主控制函数（每个PWM周期调用一次） ==========
void FOC_CurrentLoop(FOC_Controller *foc, float ia, float ib, float ic, 
                     float theta_elec, float torque_ref, float kt) {
    float i_alpha, i_beta, id_fb, iq_fb;
    float err_d, err_q;
    
    // 1. 转矩指令→q轴电流指令
    foc->iq_ref = torque_ref / kt;   // Kt为转矩常数(Nm/A)
    foc->id_ref = 0.0f;              // 表贴式PMSM采用id=0控制
    
    // 2. 三相电流采样→Clark变换
    Clark_Transform(ia, ib, ic, &i_alpha, &i_beta);
    
    // 3. Park变换→dq轴反馈电流
    Park_Transform(i_alpha, i_beta, theta_elec, &id_fb, &iq_fb);
    
    // 4. 电流误差计算
    err_d = foc->id_ref - id_fb;
    err_q = foc->iq_ref - iq_fb;
    
    // 5. PI控制器输出d/q轴电压
    foc->vd = PI_Update(&foc->pi_d, err_d, foc->ts);
    foc->vq = PI_Update(&foc->pi_q, err_q, foc->ts);
    
    // 6. 逆Park变换→αβ轴电压
    Inverse_Park(foc->vd, foc->vq, theta_elec, &foc->v_alpha, &foc->v_beta);
    
    // 7. SVPWM生成占空比
    SVPWM_Calc(foc->v_alpha, foc->v_beta, foc->vdc, 
               &foc->ta, &foc->tb, &foc->tc);
}

// ========== 初始化示例 ==========
void FOC_Init(FOC_Controller *foc, float vdc, float ts) {
    foc->vdc = vdc;
    foc->ts = ts;
    foc->id_ref = 0.0f;
    foc->iq_ref = 0.0f;
    
    // PI参数：Kp=0.5V/A, Ki=50V/(A·s)，输出限幅±10V
    PI_Init(&foc->pi_d, 0.5f, 50.0f, 10.0f);
    PI_Init(&foc->pi_q, 0.5f, 50.0f, 10.0f);
}

参数配置与运行方式

本案例代码为嵌入式控制算法模块，需在实时控制环境中运行。典型配置如下：

• 硬件平台：STM32F407或类似带浮点单元的MCU，PWM频率10kHz
• 电机参数：路感模拟PMSM，极对数4，转矩常数Kt=0.8Nm/A，母线电压Vdc=12V
• 采样：三相电流通过双采样电阻或LEM传感器获取，转子位置由旋变/编码器提供

调用方式：在PWM中断服务函数中，每次触发时读取电流采样值ia/ib/ic和电角度theta_elec，调用FOC_CurrentLoop()，将输出的占空比ta/tb/tc写入定时器比较寄存器即可。

验证方法与判定标准

仿真验证步骤：

1. 将上述代码编译为静态库，在MATLAB/Simulink中通过S-Function或C-MEX方式接入PMSM电机模型
2. 设置目标转矩阶跃：0→2Nm（对应iq_ref=2.5A），持续0.5s后阶跃至-1Nm
3. 观测q轴电流响应曲线，记录上升时间与稳态误差

判定标准：

• q轴电流稳态误差<2%目标值（即稳态电流在2.45~2.55A范围内）
• 电流环上升时间（10%~90%）<2ms
• d轴电流维持在0A附近（波动<±0.1A），验证id=0控制有效
• 输出占空比始终在0~1范围内，无过调制饱和

预期输出示例（基于给定PI参数与2Nm阶跃输入的确定性计算结果）：稳态时iq_fb=2.50A，vd≈0V（忽略反电动势时），vq≈Kp×0+Ki×∫0dt=0V（稳态误差为0时PI输出保持恒定）。实际vq需克服电机反电动势，其值取决于转速。