匿名四轴的控制算法的角速度内环的讨论

张衍波

分享：如题，研究了很久的飞控算法，研究来研究去还是PID，这里提出一个角速度环的讨论。关于飞机角度控制，（自稳模式），直接使用角度误差做PID控制，这种控制算法在理论上是没有问题
的。参数调的好，照样可以爽飞。我这里提出的双闭环控制，仅对有兴趣研究控制算法这部分的朋友一起讨论研究。不惜勿喷。关于角度PID控制的，就不多说，反正我们控制的目标最后都是
角度。从理论上讲，我们角度有了误差就要进行调节，角度的变化是由角速度引起的。所以这里对角速度再做一个PID，其实我认为这里做一个PI控制即可，但是由于功底不够深厚，所以采用
PID。先是角度有误差，产生一个对角速度的控制量，这就是我们的角速度的控制输入量，角速度进行反馈作差，产生PID输出到电机上产生角度变化。再反馈到角度环上。这样就形成了一个
双闭环的系统。这里，可以对角速度做一个限幅，防止在角度误差太大的时候，过分调节。 思路就是这样，很简单。有兴趣的出来沟通沟通。 源文件附上，只改了control。c里面的一小部
分。求有机器的大神帮忙调调参数验证一下。
#include "control.h"
#define Gyro_Gr                0.0010653f        
PID PID_ROL,PID_PIT,PID_YAW,PID_ALT,PID_POS,PID_PID_1,PID_PID_2;
//PID PID_ROL_W,PID_PIT_W;   
float get_lastgro_rol=0,get_lastgro_pit=0;
float gro_rol_i=0,gro_pit_i=0;
int16_t getlast_roll=0,geilast_pitch=0;
float rol_i=0,pit_i=0,yaw_p=0;
vs16 Moto_PWM_1=0,Moto_PWM_2=0,Moto_PWM_3=0,Moto_PWM_4=0,Moto_PWM_5=0,Moto_PWM_6=0,Moto_PWM_7=0,Moto_PWM_8=0;
void Control(T_float_angle *att_in,T_int16_xyz *gyr_in, T_RC_Data *rc_in, u8 armed)
{
        T_float_angle angle;
        T_float_angle gyro;
        float KAG=0.001;
        
        angle.rol = att_in->rol - (rc_in->ROLL-1500)/25;
        angle.pit = att_in->pit - (rc_in->ITCH-1500)/25;  
        
        rol_i += angle.rol;  
        if(rol_i>2000)
        rol_i=2000;
        if(rol_i<-2000)
        rol_i=-2000;
        PID_ROL.pout = PID_ROL.P * angle.rol;
        PID_ROL.dout = PID_ROL.D * gyr_in->X;   
        PID_ROL.iout = PID_ROL.I * rol_i;     
        pit_i += angle.pit;
        if(pit_i>2000)
        pit_i=2000;
        if(pit_i<-2000)
        pit_i=-2000;
        PID_PIT.pout = PID_PIT.P * angle.pit;
        PID_PIT.dout = PID_PIT.D * gyr_in->Y;
        PID_PIT.iout = PID_PIT.I * pit_i;
        
        
        if(rc_in->YAW<1480||rc_in->YAW>1520)
        {gyr_in->Z=gyr_in->Z+(rc_in->YAW-1500)*10;}
        yaw_p+=gyr_in->Z*0.0609756f*0.002f;// +(Rc_Get.YAW-1500)*30
        PID_YAW.pout=PID_YAW.P*yaw_p;
        PID_YAW.dout = PID_YAW.D * gyr_in->Z;                                   
        
        if(rc_in->THROTTLE<1200)
        {               
                pit_i=0;
                rol_i=0;
                yaw_p=0;
        }
        PID_ROL.OUT = PID_ROL.pout + PID_ROL.iout +PID_ROL.dout;
        
        PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
        PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;
        
      
      
        PID_ROL.OUT= (PID_ROL.OUT>2500)?2500ID_ROL.OUT;     
        PID_ROL.OUT= (PID_ROL.OUT<(-2500))?(-2500)ID_ROL.OUT;
  PID_PIT.OUT= (PID_PIT.OUT>2500)?2500ID_PIT.OUT;     
        PID_PIT.OUT= (PID_PIT.OUT<(-2500))?(-2500)ID_PIT.OUT;
      
        gyro.rol=PID_ROL.OUT*KAG-(gyr_in->X)*Gyro_Gr;  
        gyro.pit=PID_PIT.OUT*KAG-(gyr_in->Y)*Gyro_Gr;
        
        gro_rol_i +=gyro.rol;   
        gro_pit_i +=gyro.pit;
               
        PID_PID_1.pout=gyro.rol * PID_PID_1.P;
        PID_PID_1.dout=(gyro.rol - get_lastgro_rol)* PID_PID_1.D;
        PID_PID_1.iout=gro_rol_i*PID_PID_1.I;
        PID_PID_1.OUT = PID_PID_1.pout+PID_PID_1.iout +PID_PID_1.dout;
        
        PID_PID_2.pout=gyro.pit * PID_PID_2.P;
        PID_PID_2.dout=(gyro.pit - get_lastgro_pit)* PID_PID_2.D;
        PID_PID_2.iout=gro_pit_i*PID_PID_2.I;
        PID_PID_2.OUT = PID_PID_2.pout+PID_PID_2.iout +PID_PID_2.dout;
        
        if(rc_in->THROTTLE>1200&&armed)
        {
                Moto_PWM_1 = rc_in->THROTTLE - 1000 - PID_PID_1.OUT - PID_PID_2.OUT + PID_YAW.OUT;
                Moto_PWM_2 = rc_in->THROTTLE - 1000 + PID_PID_1.OUT - PID_PID_2.OUT - PID_YAW.OUT;
                Moto_PWM_3 = rc_in->THROTTLE - 1000 + PID_PID_1.OUT + PID_PID_2.OUT + PID_YAW.OUT;
                Moto_PWM_4 = rc_in->THROTTLE - 1000 - PID_PID_1.OUT + PID_PID_2.OUT - PID_YAW.OUT;
        }
        else
        {
                Moto_PWM_1 = 0;
                Moto_PWM_2 = 0;
                Moto_PWM_3 = 0;
                Moto_PWM_4 = 0;
        }
        Moto_PwmRflash(Moto_PWM_1,Moto_PWM_2,Moto_PWM_3,Moto_PWM_4);
                                
}

还你一世界

楼主你带给了我们一片绿色