【算法设计】永磁同步电机（PMSM）FOC算法

Tesla97

已于 2024-11-09 21:55:20 修改

阅读量2.3k

点赞数 17

文章标签：算法

于 2024-11-09 21:52:51 首次发布

本文链接：https://blog.csdn.net/weixin_73999150/article/details/143650180

版权

引言：四足机器人的运动性能很大程度上取决于腿部电机的控制精度。作为底层核心技术，FOC(Field Oriented Control)矢量控制算法在无刷电机控制中发挥着关键作用。FOC是一种先进的电机控制策略，通过Clarke和Park变换将三相电流转换到d-q坐标系下，实现磁链和转矩的解耦控制。这种控制方法可以实现更高的转速范围、提升运行效率、获得更精确的转矩控制以及改善动态响应性能。FOC控制系统包含电流环控制、坐标变换和调制策略三个关键模块。其中电流环控制负责d-q轴控制，坐标变换实现abc-dq转换，调制策略则采用SVPWM空间矢量调制。FOC系统调试主要包括电流环PI控制器调节、波形监控和自动整定三个环节。通过合理的FOC控制算法实现和调试，可以显著提升四足机器人的运动性能和控制精度，为上层运动控制奠定坚实基础。

一、PMSM概述

1、永磁同步电机（PMSM）与无刷直流电机（BLDC）区别

PMSM与BLDC虽然在转矩产生的物理机理上相同，但它们源于不同的应用领域。BLDC是PM BDC(永磁无刷直流电机)的派生词，而PMSM则是一种励磁磁场由永磁体提供的交流同步电机。两者最显著的区别在于控制方法：BLDC采用六步换向控制，PMSM则使用FOC(磁场定向控制)。FOC通过精确控制磁场实现高精度控制，具有转矩平稳、低噪声、高效率等特点。选择哪种电机主要取决于应用需求：高性能伺服系统通常选择PMSM配合FOC控制，而一般应用场合则可以选用成本较低的BLDC。

2、PMSM结构

永磁同步电机主要由以下部分组成：

定子(包含定子铁心和绕组)
转子(包含永磁体、转子铁心和转轴)
轴承等配件

表面式结构

表贴式：永磁体直接贴在转子表面
插入式：永磁体嵌入转子表面

内置式结构

径向式：永磁体埋入转子内部径向排列
切向式：永磁体切向排列
混合式：结合径向和切向布局

• PMSM具有与BLDC类似的结构，但是，PMSM反电势信号为正弦的，而BLDC PMSM梯形波

• 数学模型不同• PMSM采用正弦电流驱动

• 与三相ACIM类似，但气隙磁通由转子上安装的磁钢产生

3、PMSM特性

• 波形形状主要受到定子设计的影响
• 每相每极槽数是一个关键参数
• 分数槽、绕组和磁极电机可实现波形设计
• 制造容差决定波形的质量

瞬时功率

-转矩 x 转速 = 反电势 x 相电流

• $i_se_s=T\omega$ 表示电机的电磁功率与机械功率的关系

• $T\propto i_{s}$ 表示转矩与定子电流成正比

转矩的产生

转矩公式： $T=Fr\sin\theta$

PMSM使用FOC控制

• 保持电流超前于转子位置90°
• 需要一直获取连续的转子位置信息
• 更好的转矩性能
• 无转矩脉动

二、FOC控制算法原理

FOC矢量控制是一种高精度的无刷电机驱动方法，通过精确控制磁场的大小和方向，实现对电机的"像素级"精确控制。这种控制策略不仅能够获得平稳的输出转矩，还具有低噪声、高效率和快速动态响应等优势。

两相电流采样

ADC测量三相电流：第三相通过基尔霍夫计算

//原始电流采集
 controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
 controller.adc1_raw = ADC1->DR;
 controller.adc3_raw = ADC3->DR;

//对原始电流进行滤波补偿等换算，得到三相电流i_a，i_b，i_c 
 controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    //Calculate phase currents from ADC readings
 controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
 controller->i_a = -controller->i_b - controller->i_c;

DQ0变换（clark变换+park变换），代码为

 /*dq变换函数：dq0(float theta, float a, float b, float c, float *d, float *q)
输入的是三相电流abc，dq轴电角度theta
输出的是dq轴上的两个电流分量d,q*/
void dq0(float theta, float a, float b, float c, float *d, float *q){
/// DQ0 Transform ///
///Phase current amplitude = lengh of dq vector///
///i.e. iq = 1, id = 0, peak phase current of 1///
    
    float cf = FastCos(theta);
    float sf = FastSin(theta);
    
    *d = 0.6666667f*(cf*a + (0.86602540378f*sf-.5f*cf)*b + (-0.86602540378f*sf-.5f*cf)*c);   ///Faster DQ0 Transform
    *q = 0.6666667f*(-sf*a - (-0.86602540378f*cf-.5f*sf)*b - (0.86602540378f*cf-.5f*sf)*c);
       
    }

给定 IDref 和 IQref 值，通过PI控制器得到输出Vq/Vd，并对进行线性化

 //PI控制器
       /// PI Controller ///
       float i_d_error = controller->i_d_ref - controller->i_d;
       float i_q_error = controller->i_q_ref - controller->i_q;//  + cogging_current;
       
       // Calculate feed-forward voltages //
       float v_d_ff = SQRT3*(1.0f*controller->i_d_ref*R_PHASE  - controller->dtheta_elec*L_Q*controller->i_q);   //feed-forward voltages
       float v_q_ff =  SQRT3*(1.0f*controller->i_q_ref*R_PHASE +  controller->dtheta_elec*(L_D*controller->i_d + 1.0f*WB));    
       // Integrate Error //
       controller->d_int += controller->k_d*controller->ki_d*i_d_error;   
       controller->q_int += controller->k_q*controller->ki_q*i_q_error;
       
       controller->d_int = fmaxf(fminf(controller->d_int, OVERMODULATION*controller->v_bus), - OVERMODULATION*controller->v_bus);
       controller->q_int = fmaxf(fminf(controller->q_int, OVERMODULATION*controller->v_bus), - OVERMODULATION*controller->v_bus); 
       
       //limit_norm(&controller->d_int, &controller->q_int, OVERMODULATION*controller->v_bus);     
       controller->v_d = controller->k_d*i_d_error + controller->d_int ;//+ v_d_ff;  
       controller->v_q = controller->k_q*i_q_error + controller->q_int ;//+ v_q_ff; 
       
       controller->v_ref = sqrt(controller->v_d*controller->v_d + controller->v_q*controller->v_q);
       
       limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus);       // Normalize voltage vector to lie within curcle of radius v_bus
       float dtc_d = controller->v_d/controller->v_bus;
       float dtc_q = controller->v_q/controller->v_bus;
       linearize_dtc(&dtc_d);//线性化
       linearize_dtc(&dtc_q);//线性化
       controller->v_d = dtc_d*controller->v_bus;//得到v_d 
       controller->v_q = dtc_q*controller->v_bus;//得到v_q

反DQ0变换（反clark变换+反park变换），有代码

/*（1）反dq变换（反clark变换）函数： abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w)
输入的是dq轴上的角度、d轴的电压值v_d、q轴的电压值v_q
输出的是三相电流振幅v_u、v_v、v_w*/
void abc( float theta, float d, float q, float *a, float *b, float *c){
    /// Inverse DQ0 Transform ///
    ///Phase current amplitude = lengh of dq vector///
    ///i.e. iq = 1, id = 0, peak phase current of 1///
    float cf = FastCos(theta);
    float sf = FastSin(theta);
    
    *a = cf*d - sf*q;                // Faster Inverse DQ0 transform
    *b = (0.86602540378f*sf-.5f*cf)*d - (-0.86602540378f*cf-.5f*sf)*q;
    *c = (-0.86602540378f*sf-.5f*cf)*d - (0.86602540378f*cf-.5f*sf)*q;
    }

 /*（2）空间矢量控制函数：svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w)
输入的是电源电压v_bus、三相电压振幅uvw
输出的三相电压矢量dtc_u、dtc_v、dtc_w
实质也是理论公式转换而已*/
void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
    /// Space Vector Modulation ///
    /// u,v,w amplitude = v_bus for full modulation depth ///
    
    float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))*0.5f;
    
    *dtc_u = fminf(fmaxf(((u -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);
    *dtc_v = fminf(fmaxf(((v -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);
    *dtc_w = fminf(fmaxf(((w -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX); 
    }
//最终得到v_u、v_v、v_w
abc(controller->theta_elec + 0.0f*DT*controller->dtheta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation

如果没有位置传感器，则需要使用观测器对速度位置进行估计，四足机器人电机安装有磁编码器，因此不需要使用观测器，直接读取结果就行。但是在读取编码器值之前还要对编码器的值进行处理，原始的值含有很多误差和噪声，下面就来分析编码器的原始值是怎么处理的。

1、FOC算法中的PI控制器设计

（1）温度，内阻观测器估计

（2）根据克拉克，帕克变换得到Id和Iq

（3）弱磁控制器设计，通过给Id负电流，削弱磁链，弱磁的好处在于即使低电压条件下，也能保证电机额定转速额定力矩的稳定输出。

/// Field Weakening ///
       //电流参考值，弱磁场控制器
       //0.5f*OVERMODULATION*controller->v_bus是弱磁启用条件的电压门槛值
       controller->fw_int += .001f*(0.5f*OVERMODULATION*controller->v_bus - controller->v_ref);
       //当在额定速度以下时，0.5f*OVERMODULATION*controller->v_bus大于controller->v_ref  ，差值是正值，积分后还是正值，但由于限幅最大值是0，那么此时fw_int为0  ;
       controller->fw_int = fmaxf(fminf(controller->fw_int, 0.0f), -I_FW_MAX);
       controller->i_d_ref = controller->fw_int;
       //float i_cmd_mag_sq = controller->i_d_ref*controller->i_d_ref + controller->i_q_ref*controller->i_q_ref;
       limit_norm(&controller->i_d_ref, &controller->i_q_ref, I_MAX);

（4）PI控制，代码如下

void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta){
        
        /// Update observer estimates ///刷新观测预估
        // Resistance observer //内阻观测器 没用到
        // Temperature Observer //温度观测器 没用到

        //温度观测器，貌似没有用的到
        float t_rise = (float)observer->temperature - 25.0f;
        float q_th_in = (1.0f + .00393f*t_rise)*(controller->i_d*controller->i_d*R_PHASE*SQRT3 + controller->i_q*controller->i_q*R_PHASE*SQRT3);
        float q_th_out = t_rise*R_TH;
        observer->temperature += INV_M_TH*DT*(q_th_in-q_th_out);
        
         //内阻观测器，貌似没有用到，q的内阻对温度的影响，q内阻受q的电压和反电动势的综合影响
        observer->resistance = (controller->v_q - SQRT3*controller->dtheta_elec*(WB))/controller->i_q;
        //observer->resistance = controller->v_q/controller->i_q;
        if(isnan(observer->resistance)){observer->resistance = R_PHASE;}
        observer->temperature2 = (double)(25.0f + ((observer->resistance*6.0606f)-1.0f)*275.5f);
        double e = observer->temperature - observer->temperature2;
        observer->temperature -= .001*e;
        //printf("%.3f\n\r", e);
        

       /// Commutation Loop ///电流环
       controller->loop_count ++;   
       if(PHASE_ORDER){                                                                          // Check current sensor ordering
           controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    // Calculate phase currents from ADC readings
           controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
           }
        else{
            controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);    
           controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
           }
       controller->i_a = -controller->i_b - controller->i_c;       
       
       float s = FastSin(theta); 
       float c = FastCos(theta);                            
       dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
       //controller->i_d = 0.6666667f*(c*controller->i_a + (0.86602540378f*s-.5f*c)*controller->i_b + (-0.86602540378f*s-.5f*c)*controller->i_c);   ///Faster DQ0 Transform
       //controller->i_q = 0.6666667f*(-s*controller->i_a - (-0.86602540378f*c-.5f*s)*controller->i_b - (0.86602540378f*c-.5f*s)*controller->i_c);
        
        //没用到
        controller->i_q_filt = 0.95f*controller->i_q_filt + 0.05f*controller->i_q;
        controller->i_d_filt = 0.95f*controller->i_d_filt + 0.05f*controller->i_d;
        
        
        // Filter the current references to the desired closed-loop bandwidth
        //没用到
        controller->i_d_ref_filt = (1.0f-controller->alpha)*controller->i_d_ref_filt + controller->alpha*controller->i_d_ref;
        controller->i_q_ref_filt = (1.0f-controller->alpha)*controller->i_q_ref_filt + controller->alpha*controller->i_q_ref;

       
       /// Field Weakening ///
       //电流参考值，弱磁场控制器
       controller->fw_int += .001f*(0.5f*OVERMODULATION*controller->v_bus - controller->v_ref);
       controller->fw_int = fmaxf(fminf(controller->fw_int, 0.0f), -I_FW_MAX);
       controller->i_d_ref = controller->fw_int;
       //float i_cmd_mag_sq = controller->i_d_ref*controller->i_d_ref + controller->i_q_ref*controller->i_q_ref;
       limit_norm(&controller->i_d_ref, &controller->i_q_ref, I_MAX);
       
       
       
       /// PI Controller ///PI控制器
       float i_d_error = controller->i_d_ref - controller->i_d;
       float i_q_error = controller->i_q_ref - controller->i_q;//  + cogging_current;
       
       // Calculate feed-forward voltages //
       //计算反馈电压
       float v_d_ff = SQRT3*(1.0f*controller->i_d_ref*R_PHASE  - controller->dtheta_elec*L_Q*controller->i_q);   //feed-forward voltages
       float v_q_ff =  SQRT3*(1.0f*controller->i_q_ref*R_PHASE +  controller->dtheta_elec*(L_D*controller->i_d + 1.0f*WB));
       
       // Integrate Error //积分
       controller->d_int += controller->k_d*controller->ki_d*i_d_error;   
       controller->q_int += controller->k_q*controller->ki_q*i_q_error;
       
       controller->d_int = fmaxf(fminf(controller->d_int, OVERMODULATION*controller->v_bus), - OVERMODULATION*controller->v_bus);
       controller->q_int = fmaxf(fminf(controller->q_int, OVERMODULATION*controller->v_bus), - OVERMODULATION*controller->v_bus); 
       
       //limit_norm(&controller->d_int, &controller->q_int, OVERMODULATION*controller->v_bus);     
       controller->v_d = controller->k_d*i_d_error + controller->d_int ;//+ v_d_ff;  
       controller->v_q = controller->k_q*i_q_error + controller->q_int ;//+ v_q_ff; 
       
       controller->v_ref = sqrt(controller->v_d*controller->v_d + controller->v_q*controller->v_q);
       
       limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus);       // Normalize voltage vector to lie within curcle of radius v_bus
       float dtc_d = controller->v_d/controller->v_bus;
       float dtc_q = controller->v_q/controller->v_bus;
       linearize_dtc(&dtc_d);
       linearize_dtc(&dtc_q);
       controller->v_d = dtc_d*controller->v_bus;
       controller->v_q = dtc_q*controller->v_bus;
       abc(controller->theta_elec + 0.0f*DT*controller->dtheta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
       svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation

       if(PHASE_ORDER){                                                         // Check which phase order to use, 
            TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);                        // Write duty cycles，设置占空比
            TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_v);
            TIM1->CCR1 = (PWM_ARR)*(1.0f-controller->dtc_w);
        }
        else{
            TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);
            TIM1->CCR1 = (PWM_ARR)*(1.0f-controller->dtc_v);
            TIM1->CCR2 =  (PWM_ARR)*(1.0f-controller->dtc_w);
        }

       controller->theta_elec = theta;                                          
       
    }

2、磁编码器位置角度校正及线性化

磁编码器位置校正及线性化的目的就是为了得到一个去除了噪声，干净的的电气角度，因为前面FOC算法需要用到。

电气角度=机械角度×极对数% 2π

注意：在使用过程中，由于安装、焊接等误差的影响，多数情况下，编码器反馈值与实际值存在周期性偏差的现象，除此之外，各种噪音也会影响编码器值，要实现高精度位置控制，消除掉此类偏差是必须的。

（1）编码器位置角度校正

1）UVW三相相序判断

电机相序校正是确保多关节控制系统正常运行的重要步骤。在实际应用中，每个电机定子的三相线（W、U、V）的连接顺序可能存在差异，这是由于制造过程中的随机接线所致。不过，通过简单的校正方法，可以确保系统的正常运行。

测试步骤
给电机q轴施加正向电流指令，观察编码器反馈值的变化趋势：

当编码器值呈现单调递增时，表明相序连接正确
当编码器值呈现单调递减时，表明相序需要调整

（1）为了确定电机相位的顺序（UWV）
做法：
定子电压矢量沿正方向缓慢旋转。如果位置传感器的输出增加，相位排序是正确的。如果位置传感器的输出减小，则在程序中交换两个电机相位
确定电机相位的函数：
//order_phases(&Encoder_spi, &gpio, &controller, &prefs);*/

void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){   
    
    ///Checks phase order, to ensure that positive Q current produces
    ///torque in the positive direction wrt the position sensor.
    printf("\n\r Checking phase ordering\n\r");
    float theta_ref = 0;
    float theta_actual = 0;
    float v_d = V_CAL;         //Put all volts on the D-Axis锁住d轴
    float v_q = 0.0f;
    float v_u, v_v, v_w = 0;
    float dtc_u, dtc_v, dtc_w = .5f;
    int sample_counter = 0;
    
    ///Set voltage angle to zero, wait for rotor position to settle
    abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                                 //inverse dq0 transform on voltages
    svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                            //space vector modulation
//电机开始转动
    for(int i = 0; i<20000; i++){
        TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);                                        // Set duty cycles
        TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
        TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
        wait_us(100);
        }
    //ps->ZeroPosition();
    ps->Sample(DT); //得到编码器值
    wait_us(1000);
    //float theta_start = ps->GetMechPositionFixed();                                  //get initial rotor position
    float theta_start;
    controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    //Calculate phase currents from ADC readings
    controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
    controller->i_a = -controller->i_b - controller->i_c;
    dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
    float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
    printf("\n\rCurrent\n\r");
    printf("%f    %f   %f\n\r\n\r", controller->i_d, controller->i_q, current);
    /// Rotate voltage angle
    while(theta_ref < 4*PI){                                                    //rotate for 2 electrical cycles
        abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                             //inverse dq0 transform on voltages
        svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                        //space vector modulation
        wait_us(100);
        TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);                                        //Set duty cycles
        TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
        TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
       ps->Sample(DT);                                                            //sample position sensor
       theta_actual = ps->GetMechPositionFixed();
       if(theta_ref==0){theta_start = theta_actual;}
       if(sample_counter > 200){
           sample_counter = 0 ;
        printf("%.4f   %.4f\n\r", theta_ref/(NPP), theta_actual);
        }
        sample_counter++;
       theta_ref += 0.001f;
        }
    float theta_end = ps->GetMechPositionFixed();
    int direction = (theta_end - theta_start)>0;
    printf("Theta Start:   %f    Theta End:  %f\n\r", theta_start, theta_end);
    printf("Direction:  %d\n\r", direction);
    if(direction){printf("Phasing correct\n\r");}//相序正确
    else if(!direction){printf("Phasing incorrect.  Swapping phases V and W\n\r");}//相序错误，需要调换顺序
    PHASE_ORDER = direction;
    }

通常情况下，给一个较大的电压到d轴上，缓慢的让角度递增，转子会在磁场力的作用下，随着旋转矢量的方向缓慢转动。此时，判断编码器的值是递增还是递减，如果递增，则一切正常。如果是递减，则在程序里，调换V相和W相的输出。

2）编码器的零位置与转子的d轴不对齐的问题

理想情况下，编码器的零位置将与转子的d轴对齐，但是因为转子上的永磁体和位置传感器安装使手动的，不可能那么精确，所以编码器的零位置不完全于转子的d轴对齐，所以需要校正编码器（对编码器做标定）

给U相接电源正，V相和W相接电源负，此时会锁定转子到d轴，锁定后，多次读取编码器的反馈值，取其平均值，作为编码器的偏移offset。可以把该值写到Flash里，每次上电读取编码器反馈值，并与偏移量offset求差，获取转子当前位置。

3）纹波消除

我们给定电机一个角度值，让其正转一圈，反转一圈，对给定值和磁编码器反馈值进行对比分析。

波形放大之后所看到高频纹波，来自齿槽转矩。从上面偏差图（右上角）中可以看到，中间有一个向下的跳变，这就是电机切换方向的地方。

由于电机摩擦和惯量的存在，实际运行时，它总是稍微滞后于参考角度。当电机切换方向时，这个滞后又切换到另一侧。

在信号顶部有一些低频的较大的纹波，这是由于位置传感器的安装偏心引起。这个偏差看起来很小，但是将它转换成电气偏差时，将会变得很大，比如MIT的关节电机，极对数是21，这就意味着，1°的机械误差被放大后变成21°的电气误差，这个误差显然非常大。因此进行编码器的偏心校正是必须的。

（2）编码器位置角度线性化

偏心率校正首先需要将齿槽效应和摩擦转矩的影响从偏心误差中分离出来。摩擦转矩的影响很容易分离，只需要将电机向前旋转和向后旋转的采样值进行平均即可。如上图滤波后的图3。

消除齿槽转矩的纹波可以通过设计FIR滤波器，使其在某些频率和谐波下具有零增益。

滤除高频齿槽转矩纹波后，剩下的就是传感器偏心引起的非线性误差了。使用这个滤波后的信号图，我们可以构建一个查找表，用来线性化编码器的输出。由于信号变化的缓慢，因此并不需要太高的分辨率。使用128个点或256个点就可以工作的很好。通过从偏差中减去平均值生成查找表，程序使用时，再将偏差转换成编码器的原始计数值。如上图滤波后的图4。

/*编码器线性化过程
做法：【因为这个非线性测量一圈就能确定，并且不会改变的性质】
误差信号用作查找表，使位置传感器的输出线性化*/
void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){
    /// Measures the electrical angle offset of the position sensor
    /// and (in the future) corrects nonlinearity due to position sensor eccentricity
    printf("Starting calibration procedure\n\r");
    float * error_f;
    float * error_b;
    int * lut;
    int * raw_f;
    int * raw_b;
    float * error;
    float * error_filt;
    
    const int n = 128*NPP;     //NPP=21 // number of positions to be sampled per mechanical rotation.  Multiple of NPP for filtering reasons (see later)
    const int n2 = 40;          // increments between saved samples (for smoothing motion)
    float delta = 2*PI*NPP/(n*n2);    // change in angle between samples
    error_f = new float[n]();     // error vector rotating forwards
    error_b = new float[n]();     // error vector rotating backwards
    const int  n_lut = 128;
    lut = new int[n_lut]();       // clear any old lookup table before starting.
    
    error = new float[n]();
    const int window = 128;
    error_filt = new float[n]();
    float cogging_current[window] = {0};
    
    ps->WriteLUT(lut); 
    raw_f = new int[n]();
    raw_b = new int[n]();
    float theta_ref = 0;
    float theta_actual = 0;
    float v_d = V_CAL;     // Put volts on the D-Axis
    float v_q = 0.0f;
    float v_u, v_v, v_w = 0;
    float dtc_u, dtc_v, dtc_w = .5f;
    
        
    ///Set voltage angle to zero, wait for rotor position to settle
//编码器的零位置将与转子的d轴对齐
    abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);  // inverse dq0 transform on voltages
    svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
    for(int i = 0; i<40000; i++){
        TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u); // Set duty cycles占空比
        if(PHASE_ORDER){                                   
            TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
            TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
            }
        else{
            TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
            TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
            }
        wait_us(100);
        }
    ps->Sample(DT);   
    controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    //Calculate phase currents from ADC readings
    controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
    controller->i_a = -controller->i_b - controller->i_c;
    dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
    float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
    printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
    for(int i = 0; i<n; i++){           //正转一圈 // rotate forwards
       for(int j = 0; j<n2; j++){   
        theta_ref += delta;
       abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);    // inverse dq0 transform on voltages
       svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);    // space vector modulation
        TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
        if(PHASE_ORDER){                          // Check phase ordering
            TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);    // Set duty cycles占空比
            TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
            }
        else{
            TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
            TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
            }
            wait_us(100);
            ps->Sample(DT);
        }
       ps->Sample(DT);
       theta_actual = ps->GetMechPositionFixed();
       error_f[i] = theta_ref/NPP - theta_actual;
       raw_f[i] = ps->GetRawPosition();
        printf("%.4f   %.4f    %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]);
       //theta_ref += delta;
        }
    
    for(int i = 0; i<n; i++){             //反转一圈// rotate backwards
       for(int j = 0; j<n2; j++){
       theta_ref -= delta;
       abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);      // inverse dq0 transform on voltages
       svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation
        TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
        if(PHASE_ORDER){
            TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
            TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
            }
        else{
            TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
            TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
            }
            wait_us(100);
            ps->Sample(DT);
        }
       ps->Sample(DT);                // sample position sensor
       theta_actual = ps->GetMechPositionFixed();   // get mechanical position
       error_b[i] = theta_ref/NPP - theta_actual;
       raw_b[i] = ps->GetRawPosition();
       printf("%.4f   %.4f    %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
       //theta_ref -= delta;
        }    
        
        float offset = 0;                                  
        for(int i = 0; i<n; i++){
            offset += (error_f[i] + error_b[n-1-i])/(2.0f*n);    // calclate average position sensor offset
            }
        offset = fmod(offset*NPP, 2*PI);   // convert mechanical angle to electrical angle
        
            
        ps->SetElecOffset(offset);    // Set position sensor offset
        __float_reg[0] = offset;
        E_OFFSET = offset;
        
        /// Perform filtering to linearize position sensor eccentricity
        /// FIR n-sample average, where n = number of samples in one electrical cycle
        /// This filter has zero gain at electrical frequency and all integer multiples
        /// So cogging effects should be completely filtered out.
        
        
        float mean = 0;
        for (int i = 0; i<n; i++){    //Average the forward and back directions
            error[i] = 0.5f*(error_f[i] + error_b[n-i-1]);
            }
        for (int i = 0; i<n; i++){
            for(int j = 0; j<window; j++){
                int ind = -window/2 + j + i;   // Indexes from -window/2 to + window/2
                if(ind<0){
                    ind += n;}             // Moving average wraps around
                else if(ind > n-1) {
                    ind -= n;}
                error_filt[i] += error[ind]/(float)window;
                }
            if(i<window){
                cogging_current[i] = current*sinf((error[i] - error_filt[i])*NPP);
                }
            //printf("%.4f   %4f    %.4f   %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
            mean += error_filt[i]/n;
            }
        int raw_offset = (raw_f[0] + raw_b[n-1])/2;  //Insensitive to errors in this direction, so 2 points is plenty
        
        
        printf("\n\r Encoder non-linearity compensation table\n\r");
        printf(" Sample Number : Lookup Index : Lookup Value\n\r\n\r");
        for (int i = 0; i<n_lut; i++){     // build lookup table
            int ind = (raw_offset>>7) + i;
            if(ind > (n_lut-1)){ 
                ind -= n_lut;
                }
            lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI));
            printf("%d   %d   %d \n\r", i, ind, lut[ind]);
            wait(.001);
            }
            
        ps->WriteLUT(lut);   // write lookup table to position sensor object
        //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging));  //compensation doesn't actually work yet....
        memcpy(&ENCODER_LUT, lut, sizeof(lut));    // copy the lookup table to the flash array
        printf("\n\rEncoder Electrical Offset (rad) %f\n\r",  offset);
        
        if (!prefs->ready()) prefs->open();
        prefs->flush();             // write offset and lookup table to flash
        prefs->close();
        
        delete[] error_f;       //gotta free up that ram
        delete[] error_b;
        delete[] lut;
        delete[] raw_f;
        delete[] raw_b;

    }

上面程序大意是就是：

1）对齐转子到零位，锁定d轴，

2）正转一圈得到设定角度值和编码器反馈角度值的误差值erro_f（共21×128组）

3）反转一圈得到设定角度值和编码器反馈角度值的误差值erro_b（共21×128组）

4）offset +=erro_f+erro_b/2n,消除摩擦转矩的影响

5)error[i] = 0.5f*(error_f[i] + error_b[n-i-1]);正反相加求平均值

6）error[i] 使用FIR滤波滤除齿槽高频部分，得到编码器偏心误差

7）以21个点为一组，建立一个表，表里有128个元素，涵盖电机一圈，供程序运行时使用

三、FOC控制算法调试

1、验证三相逆变模块的正确输出

断开电机连接，依次将U、V、W三相的占空比设置为0、100%、50%，使用万用表测量对应端口的电压，0占空比时输出电压应接近0V，100%占空比时接近母线电压，50%占空比时为母线电压的一半。若测量结果符合预期，说明配置正确。

2、测试相电流采样电路的功能

(1）无论使用哪种方法，最后都是使用AD进行采样的，根据采样电路的结构可以推导出电流采样值与AD结果寄存器值之间的关系。所以先要配置好AD模块，一般情况下，可以使用PWM信号来触发AD采样，具体触发时机取决于使用的采样方案。配置好寄存器后，需要验证电流采样的正确性。

(2)先断开电机连接，使用仿真器连续读取AD采样结果寄存器的值，此时的采样值即为电流零点。观察电流零点的稳定性，一般来说，如果电流采样的稳定性较好，AD结果寄存器中只会有最后一两位在波动。若电流零点波动得较严重，说明采样稳定性很差，此时需要在程序中增加滤波算法。

(3)接上电机，给U相输出一个很小的占空比，V、W两相占空比设置为0。具体占空比的值取决于母线电压及绕组电阻，可以预先估计一下，保证电流在安全范围内，一般可以取为1A左右。此时再用仿真器读取计算出的U、V、W三相电流，根据正电压产生正电流的电动机原则，U相电流应该是正的，V、W两相电流应该是负的，且V、W两相电流应基本相同。若正负号不对，需要进行调整。

如有条件的话，可以使用电流探头或钳型电流表等仪器测量真实的电流值，将真实值与采样值进行对比，以此评估采样的正确性及精度。如果无法测量电流的真实值，可以改变PWM占空比，保证增大占空比电流也会增大，减小占空比电流也随之减小即可。

3、测试变换程序的正确性

矢量控制的核心其实就在Clark与Park变换上，通过这两个变换实现了直轴与交轴的解耦。库提供了现成的变换模块，可直接使用。如果是自行编写的程序，需要预先通过仿真等方法确定程序的正确性。

4、调试SVPWM模块

其实矢量控制也并不一定要使用SVPWM（空间矢量调制）方法，也可使用其他方法（如滞环控制等）进行电流控制，不过SVPWM是最优策略，也是主流做法。输出是PWM占空比，上述步骤中已经确定了PWM输出的正确性，现在再加上SVPWM算法进行验证。

（1）把FOC其他部分注释掉，只保留反park变换和svpwm函数。

（2）反park变换的输入参数有3个：Vq、Vd、Angle，将Vq=0，Vd设置为一个较小的值，Angle=0，然后接电机上电，此时svpwm会有输出，电机有力，转子被锁定在当前电角度位置。如果没有力，说明Vd值太小了。同时可以查看CH1、CH2、CH3的波形和svpwm的标准波形做对比。比如，处于svpwm的扇区1，那么CH1、CH2、CH3上的波形关系如下：

你可以在仿真模式下，把CH1、CH2、CH3通道的比较值加入watch窗口，然后手动改变Vd的值。改变Vd的值，3个通道的比较值也会跟着改变。

（3）将Angle由0开始，每次增加30°左右，此时电机会跟着旋转，且每次旋转的角度应该是相同的，记录下这个旋转方向，这就是此系统固有的正方向。此时还可以验证电机的极对数，若Angle重复增加N个周期后电机回到起始点（可用记号笔进行标注），电机的极对数即为N。

5、整定d轴、q轴PID参数

（1）把电流采样模块、clark变换、park变换、PID控制器加入。

（2）此时系统的输入参数有3个：PID控制器的参考量IQREF、IDREF和Angle。设置Angle=0，IQREF=0，IDREF设置为一个较小值（可以与上面的Vd一样）。

（3）d轴和q轴都有PID控制器，将两个PID的参数都设置为0。之后保持q轴的PID为0，然后整定d轴的参数（整定的过程中，可以给电机上电观察效果，目的是使电流采样值经过clark、park变换后的Iq、Id值接近于IQREF、IDREF）。d轴整定完后，把q轴的PID参数设置为相同即可。

6、使用编码器输出角度替代Angle值

改变Angle值，电机旋转起来。旋转状态的Iq、Id值与停止状态一样仍然是接近于IQREF、IDREF的。在程序让Angle自加，使用j-scope观察4个变量的变化：Angle值、编码器返回的角度值、A相电流采样值、B相电流采样值。

Angle值、编码器返回的角度值的波形如左边的两个图。两个波形应该都是幅值相同的三角波，如果编码器值的变化方向与Angle不同，那么编码器的输出角度值需要反向。他们两者的波形并不重合，在水平轴上存在偏差值。而这步的目的就是要让他们重合，这样就能用编码器输出值替代Angle值了。

因为Angle值代表的是转子的真实电角度。因此要把编码器输出角度矫正为Angle值。如果是绝对式编码器，就要把编码器返回的角度值减去偏差，使之与Angle值的波形重合。如果是相对式编码器，就需要当电机启动阶段给定一个Angle值进行预定位时，把编码器的输出角度矫正为Angle值。

右边的两个图是相电流的采样值。调试PI控制器参数时也可参考相电流曲线，若曲线发生畸变，不是图中那样的正弦波形，需要降低比例及积分作用。

修改程序，使用编码器的输出角度值替代Angle值。绝对式编码器并不需要特殊的启动过程，因为矫正过后，一上电编码器的输出角度就是与转子的电角度是重合的。使用相对式编码器，在上电时，并不知道转子的位置，因为需要先设定Angle、IqRef为0，IdRef设置为合适值，此时电机转子会自动旋转到零点位置处，然后再把编码器的输出角度值矫正为0，完成编码器值与转子电角度的重合。

7、验证电流闭环

上面几步调试过程中使用的Angle值均是由程序计算生成的，现在需要加入实际角度采样值，完成完整的电流闭环部分。

上电开始运行时，如果没有霍尔信号或绝对值编码器，是很难获取转子绝对位置的，故一般的做法是先进行预定位操作。具体方法是：将Angle及IqRef设置为0，IdRef设置为合适值，此时电机会自动旋转到零点位置处，此过程中将角度累加寄存器持续置零即可，这样即完成了初始的预定位操作。

之后按上一步调试电流PI控制器中的方法，自动改变Angle让电机旋转起来，再观察Angle与角度采样值，二者应该是严格相同的。确定了角度采样值的正确性后，修改程序，使用角度采样值替代Angle，这样就完成了完整的电流闭环程序。此时已经不再需要使用Angle了，初始预定位时，只要在程序中一直将角度值清零即可。

最后验证电流闭环的正确性，在完成预定位后，依次验证Id与Iq。验证Id时将IqRef设置为0，IdRef设置为一个合适的正值，此时电机是不会旋转的，用手转动电机也是可以转动的，只是不同于自由转动状态，此时旋转电机时会感到阻力较大，有一个力始终在维持电机处于当前位置。验证Iq时同理将IdRef设置为0，IqRef设置为一个合适的值即可。需要注意的是，因为启动电流明显大于稳态电流，如果IqRef的值设置得过小，电机无法旋转起来，而增大IqRef，使电机可以旋转起来后，电机会一直加速到最高转速。为保证安全，需要对输出电压占空比进行限幅。