4.基于STM32C8T6的四旋翼无人机的飞控制作----理论准备4，四旋翼姿态控制理论

二部分 四旋翼PID控制理论

       此部分晦涩难懂，而且每个人采取的方式也不一样，笔者是两年之前做的,现在写的也只能作为参考，具体哪种控制方式好可以多方面参考资料，针对自己的实际情况进行调试。笔者当年做也是困在了这个环节，将一般的控制理论搞了个差不多，但是参数和算法的调试没有到位，因为有其他项目后期就再也没有继续调试，但是感觉已经学习到了大部分的知识，再调试有点耽误时间，看当时其他网友说可能一个月也调不好参数，因为涉及的参数众多关系复杂，而且调试过程复杂有一定的风险（说不定会被螺旋桨削去，，，注意安全）

        四旋翼采用的是内环外环PID，有两层。不考虑气压传感器等其他传感器，只考虑将遥控器数据和MPU6050数据作为系统输入变量，将四个PWM值作为输出变量。遥控器采用四通道遥控器，除了油门外，另外三个控制三个角度，三个角度摇杆所处的位置就代表四旋翼的目标角度。油门作为电机转速的基础量，控制电机转速时候要在油门给的电机转速的基础上进行加减，但是要注意加减的比例和对pwm占空比的限幅，防止电机疯掉。一开始做无人机可能就会被这几个变量如何融合的问题所困扰，当时经过一段时间的查找资料最终搞明白了。

这是目标角度的计算公式，pwmout的几个变量是单片机捕获的遥控器接收机的信号，范围是1000-2000

target_X=(pwmout1-1502)/20;
target_Y=(pwmout2-1502)/20;
target_Z=(pwmout4-1502)/20;     
 

这是内外环pid计算公式，其中外环是角度环，内环是角速度环，先有的目标角度，后有的目标角速度，外环的输出作为内环的输入。内环的输出就是电机转速的差值，依靠此差值分两个电机一组进行控制前后左右移动

///////////////////////////Íâ»·½Ç¶È»·PID/////////////////////////////////////////////////////////
err_OX=target_X-Q_ANGLE.X;
Pitch_out_i+=err_OX;
//------------------------Pitch»ý·ÖÏÞ·ù-------------------------------------------------//
if(Pitch_out_i>300) Pitch_out_i=300;
else if(Pitch_out_i<-300) Pitch_out_i=-300;
//------------------------Pitch»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
Pitch_shell_OUT=Pitch_shell_P*err_OX+Pitch_shell_I*Pitch_out_i+Pitch_shell_D*(err_OX-err_LAST_OX);
err_LAST_OX=err_OX;    //±£´æÉÏÒ»´ÎµÄÖµ
    
     
err_OY=target_Y-Q_ANGLE.Y;
Roll_out_i+=err_OY;
//------------------------Roll»ý·ÖÏÞ·ù-------------------------------------------------//
if(Roll_out_i>300) Roll_out_i=300;
else if(Roll_out_i<-300) Roll_out_i=-300;
//------------------------Roll»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
Roll_shell_OUT=Roll_shell_P*err_OY+Roll_shell_I*Roll_out_i+Roll_shell_D*(err_OY-err_LAST_OY);    
err_LAST_OY=err_OY;    //±£´æÉÏÒ»´ÎµÄÖµ
    
Q_ANGLE.Z=0;
err_OZ=target_Z-Q_ANGLE.Z;
YAW_out_i+=err_OZ;
//------------------------YAW»ý·ÖÏÞ·ù-------------------------------------------------//
if(YAW_out_i>300) YAW_out_i=300;
else if(YAW_out_i<-300) YAW_out_i=-300;
//------------------------YAW»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
YAW_shell_OUT=YAW_shell_P*err_OZ+YAW_shell_I*YAW_out_i+YAW_shell_D*(err_OZ-err_LAST_OZ);        
err_LAST_OZ=err_OZ;    //±£´æÉÏÒ»´ÎµÄÖµ    
    
    
//////+++++++++++++++//////ÄÚ»·½ÇËٶȻ·PID////+++++++++++++++++++++++////////////////////////////
    
err_IX=Pitch_shell_OUT-MPU6050_ACC_LAST.X;
Pitch_in_i+=err_IX;    
//------------------------Pitch»ý·ÖÏÞ·ù-------------------------------------------------//
if(Pitch_in_i>300) Pitch_in_i=300;
else if(Pitch_in_i<-300) Pitch_in_i=-300;
//------------------------Pitch»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
Pitch_core_OUT=Pitch_core_P*err_IX+Pitch_core_I*Pitch_in_i+Pitch_core_D*(err_IX-err_LAST_IX)-30;
err_LAST_IX=err_IX;    //±£´æÉÏÒ»´ÎµÄÖµ    
    
    
err_IY=Roll_shell_OUT-MPU6050_ACC_LAST.Y;
Roll_in_i+=err_IY;    
//------------------------Roll»ý·ÖÏÞ·ù-------------------------------------------------//
if(Roll_in_i>300) Roll_in_i=300;
else if(Roll_in_i<-300) Roll_in_i=-300;
//------------------------Roll»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
Roll_core_OUT=Roll_core_P*err_IY+Roll_core_I*Roll_in_i+Roll_core_D*(err_IY-err_LAST_IY);
err_LAST_IY=err_IY;    //±£´æÉÏÒ»´ÎµÄÖµ        
    
    
err_IZ=YAW_shell_OUT-MPU6050_ACC_LAST.Z;
YAW_in_i+=err_IZ;    
//------------------------YAW»ý·ÖÏÞ·ù-------------------------------------------------//
if(YAW_in_i>300) YAW_in_i=300;
else if(YAW_in_i<-300) YAW_in_i=-300;
//------------------------YAW»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
YAW_core_OUT=YAW_core_P*err_IZ+YAW_core_I*YAW_in_i+YAW_core_D*(err_IZ-err_LAST_IZ);
err_LAST_IZ=err_IZ;    //±£´æÉÏÒ»´ÎµÄÖµ            
 

电机PWM输出的重要公式，其中pwmout3是油门的占空比采集值

    if(pwmout3<1010)
    {
    c=0;
    PWM1=c;
    PWM2=c;
    PWM3=c;
    PWM4=c;
    }
    if(pwmout3>=1010)
    {
    PWM1=pwmout3+Pitch_shell_OUT-Roll_shell_OUT+YAW_shell_OUT;
    PWM2=pwmout3+Pitch_shell_OUT+Roll_shell_OUT-YAW_shell_OUT;
    PWM3=pwmout3-Pitch_shell_OUT+Roll_shell_OUT+YAW_shell_OUT;
    PWM4=pwmout3-Pitch_shell_OUT-Roll_shell_OUT-YAW_shell_OUT;
    }