STM32驱动 有刷电机 位置，速度，电流，三环控制

PID算法：

位置式PID：

离散化位置式PID：

增量式PID：

增量式和位置式PID程序：

int32_t Increment_PID_Control(PID_TypeDef *PID,float Feedback_value)
{
    PID->Error = (float)(PID->SetPoint - Feedback_value);//偏差e
    if((PID->Error >= -5)&&(PID->Error<=5))                             /*限定闭环死区*/
    {
        PID->Error = 0;
    }
    
    PID->ActualValue += (PID->Proportion * (PID->Error - PID->LastError))                          /* 比例环节 */
                      + (PID->Integral * PID->Error)                                             /* 积分环节 */
                      + (PID->Derivative * (PID->Error - 2 * PID->LastError + PID->PrevError));  /* 微分环节 */
    
    PID->PrevError = PID->LastError;                                                            /* 存储偏差，用于下次计算 */
    PID->LastError = PID->Error;
    
    if(PID->ActualValue < -6000){PID->ActualValue=-6000;}
    else if(PID->ActualValue > 6000){PID->ActualValue=6000;}//6000 是PWM 占空比上限
    
    return ((int32_t)(PID->ActualValue));
}



int32_t Positional_PID_Control(PID_TypeDef *PID,float Feedback_value)
{
    PID->Error = (float)(PID->SetPoint - Feedback_value);//偏差e
    if((PID->Error >= -5)&&(PID->Error<=5))                             /*限定闭环死区*/
    {
        PID->Error = 0;
    }
    if(PID->Error > -1500 && PID->Error < 1500)                    /*积分分离，误差较大时去掉积分项作用*/
    {
        PID->SumError += PID->Error;                                           /*误差累加（积分）*/
        
        if(PID->SumError > 500)                                                   /*积分限幅，防止积分饱和*/
        {
            PID->SumError = 500;
        }
        else if(PID->SumError < -500)
        {
            PID->SumError = -500;
        }
    }
    
    PID->ActualValue = (PID->Proportion * PID->Error)                       /* 比例环节 */
                     + (PID->Integral * PID->SumError)                    /* 积分环节 */
                     + (PID->Derivative * (PID->Error - PID->LastError)); /* 微分环节 */
    PID->LastError = PID->Error;

    if(PID->ActualValue < -6000){PID->ActualValue=-6000;}
    else if(PID->ActualValue > 6000){PID->ActualValue=6000;}//6000 是PWM 占空比上限
    
    return ((int32_t)(PID->ActualValue));
}

• 位置式需防积分饱和，增量式不会产生积分饱和
• 为了解决积分饱和的问题，引入了抗积分饱和的PID算法。在计算U(k)的时候，先判断上一时刻的控制量U(k-1)是否已经超出了限制范围。若U(k-1)>Umax，则只累加负偏差；若U(k-1)<Umin，则只累加正偏差。从而避免控制量长时间停留在饱和区

• 在饱和的时候将积分器的累计值初始化到一个比较理想的值；

• 将积分累计量限制上下限，避免积分累计量超过限制值；

PID三环串联：

1. 设定位置值 传入位置环，

2. 位置环输出结果，对结果进行限制，作为速度环的设定值，对位置环结果的限制就是对速度的限制

3. 获取速度值，计算速度环，

4. 速度环输出结果，对结果进行限制，作为电流环的设定值，对速度环结果的限制就是对电流的限制

5. 获取电流值，计算电流环

6. 电流环结果传入PWM控制，

7. 最终控制电机 位置，速度，电流

//        /******************************************************三环控制*********************************************************/
        /************获取实时数据****************/
        BDCM_Position = Encoder_Get_Position();             //计算位置
        BDC_speed_r_min = Encoder_Get_Speed();              //计算速度
        BDCM_Current_Now = Get_BDCM_A();                    //获得电流
        if((BDCM_Position_PID.SetPoint < BDCM_Position-500) || (BDCM_Position_PID.SetPoint > BDCM_Position+500))//位置有偏差，进入PID计算
        {
            
        /***************************************位置环*******************************/
            BDCM_PWM_Period = Positional_PID_Control(&BDCM_Position_PID,BDCM_Position);//进行位置PID计算
            /**********限制位置环结果，作为速度设置值*************/
            #define BDCM_SPEED_PID_SET_POINT 100
            if(BDCM_PWM_Period < -BDCM_SPEED_PID_SET_POINT){BDCM_PWM_Period=-BDCM_SPEED_PID_SET_POINT;}
            else if(BDCM_PWM_Period > BDCM_SPEED_PID_SET_POINT){BDCM_PWM_Period=BDCM_SPEED_PID_SET_POINT;}//位置PID结果限制，对应速度值

            BDCM_Speed_PID.SetPoint = BDCM_PWM_Period;//位置PID结果 传入 速度PID设置值
            if(BDCM_Speed_PID.SetPoint != 0)//目标速度不为零，进入速度PID计算
            {
        /****************************************速度环******************************/
                BDCM_PWM_Period = Increment_PID_Control(&BDCM_Speed_PID,BDC_speed_r_min);//速度PID计算
                /***********速度PID结果控制电流设置值，限制速度环结果，作为电流环设置值*************/
                #define BDCM_CURRENT_PID_SET_POINT 30
                if(BDCM_PWM_Period < -BDCM_CURRENT_PID_SET_POINT){BDCM_PWM_Period=-BDCM_CURRENT_PID_SET_POINT;}
                else if(BDCM_PWM_Period > BDCM_CURRENT_PID_SET_POINT){BDCM_PWM_Period=BDCM_CURRENT_PID_SET_POINT;}//速度环PID结果限制，对应电流环值
                    
                BDCM_Current_PID.SetPoint = BDCM_PWM_Period;
        /*****************************************电流环*********************************/
                BDCM_PWM_Period = Increment_PID_Control(&BDCM_Current_PID,BDCM_Current_Now);//电流PID
               if(BDCM_Speed_PID.SetPoint>0)
                {
                    BDCM_Set(1,BDCM_PWM_Period);
                }
                else
                {
                    BDCM_Set(0,-BDCM_PWM_Period);
                }
            
            }
           
        }else
        {
            BDCM_Speed_PID.SetPoint = 0;
            BDCM_Current_PID.SetPoint = 0;
            BDCM_PWM_Period = 0;
        }

控制结果：

速度保持100r/min

电流保持30mA