四轴PID控制不收敛

leixiaofeng

现在是通过PID单独调试相对的两个马达，采用增量式PID，输入是角度，输出是对应的马达PWM Duty的改变量。
测试架构是这样的：将相对的1、3马达关掉，在将1、3马达拖起来，让2、4马达悬空，控制2、4马达PWM可以左右晃动，单独加入P后感觉回收力很小，加大P后就一直左右震荡。加入I后拿在手上倾斜一下才感觉回收力度在慢慢增大，但是还是会调过，出现震荡！加入D后没有改善。下面是我的PID算法：
int32_t        PID_Cal(float Kp,float Ki,float Kd)
{
        float PID_Value,Delta_Ek,Delta_Ek_B;
        int32_t PID_Temp;
        
        Delta_Ek = AngleMotor - AngleMotor_B1;                                //相邻两次的角度差
        Delta_Ek_B = AngleMotor_B1 - AngleMotor_B2;                //上一次的角度差
        
        PID_Value = Kp*Delta_Ek;                                                                                        //比例项
        PID_Value += (Ki*AngleMotor);                                                                        //积分项
        PID_Value += (Kd*(Delta_Ek - Delta_Ek_B));                //微分项
        
        PID_Value += PID_Deci;                                                                                                //获取整数部分
        PID_Temp = (int32_t)PID_Value;
        PID_Deci = PID_Value - PID_Temp;
        return PID_Temp;
}
由于我是4ms采集一次数据并计算一次角度来改变电机PWM Duty，所以很多时候PID出来的都是比较小的数，所以每次都保留了小数部分，将整数部分去改变PWM Duty。比如：马达2高、马达4低算出角度是正的，马达2低、马达4高算出角度是负的，我会将这个角度带到PID中，如果得到的值是正的就会去减小马达2的Duty，增大马达4的Duty；如果结果为负就增加马达2的Duty，减小马达4的Duty。