Destiny小型四旋翼飞行器——PID串级调试

destiny、 · 发表于 2016-8-27 19:21:25

《Destiny小型四旋翼飞行器》是一个连载帖，连载本帖有几个目的，其一是想和大家相互学习、相互交流，得到更快的提升；其二是帮助比本人更新的新手，本人遇到的一些常见的问题都会在这里说明，让大家少走弯路；其三，给自己一点坚持的动力，把发帖当成一种习惯而引导自己学习。不矫情了，进入正题。
         今天的内容是平衡PID的调试，其实这些内容是上个礼拜抽时间搞的，但是这个礼拜一直很忙，进度就停了。首先说明，本人水平有限，能用来搞小四轴的时间也很少，因此没有去深入研究捷联惯导算法，直接用MPU6050的DMP输出欧拉角。

一开始尝试的是单级角度环PID，如果不去管什么死区，平衡的时候期望角度（roll和pitch）应该是0度，偏差即实际测量角度的负数，微分是两次偏差的差值（没有用陀螺仪的值当微分部分）。刚开始只给了P，从小到大，寻找各种资料里面所说的等幅震荡，然而并没有毛用，P太小了几乎没有效果，P稍微大一点就会越荡越嗨，感觉形成了正反馈。随意的加入D，也是从小到大，所谓的抑制和预测的效果也没有体现出来，然后翻了很多资料，看到有人说只有角度环时，角度为期望，系统调节至角度为0的时候角速度的存在会导致机体继续偏转，产生震荡。然后才有所顿悟，上了串级PID，下文是串级PID的介绍。
PID公式使用的是位置式的，代码也完全按照公式写的，只是加了角度/角速度限幅和积分限幅（参考了烈火狂龙的PID调节部分）。以下是位置式PID的数学公式：
               u(k) = Kp * e(k) + Ki * ∑e(i) + Kd * [e(k) - e(k-1)]
以下是PID代码部分：
[mw_shl_code=c,true]void Angle_PID()
{
      //Roll
      ANGLE_ERROR.Roll = 0 - Angle_Euler.Roll;//
      if(ANGLE_ERROR.Roll >  Angle_Max)       //ROLL
            ANGLE_ERROR_INTE.Roll +=  Angle_Max;
      if(ANGLE_ERROR.Roll < -Angle_Max)
            ANGLE_ERROR_INTE.Roll += -Angle_Max;
      else
            ANGLE_ERROR_INTE.Roll += ANGLE_ERROR.Roll;
      if(ANGLE_ERROR_INTE.Roll >  Angle_Inte_Max)//积分限幅
         ANGLE_ERROR_INTE.Roll =  Angle_Inte_Max;
      if(ANGLE_ERROR_INTE.Roll < -Angle_Inte_Max)
         ANGLE_ERROR_INTE.Roll = -Angle_Inte_Max;
      ANGLE_ERROR_DIFF.Roll = ANGLE_ERROR.Roll - ANGLE_ERROR_LAST.Roll;//e(k-1)
      Roll_PID.Pout = Roll_PID.P * ANGLE_ERROR.Roll;    //roll PID
      Roll_PID.Iout = Roll_PID.I * ANGLE_ERROR_INTE.Roll;
      Roll_PID.Dout = Roll_PID.D * ANGLE_ERROR_DIFF.Roll;
      Roll_PID.ALLout = Roll_PID.Pout + Roll_PID.Iout + Roll_PID.Dout;    //roll OUT
      ANGLE_ERROR_LAST.Roll = ANGLE_ERROR.Roll;//

      //Pitch
      ANGLE_ERROR.Pitch = 0 - Angle_Euler.Pitch;//
      if(ANGLE_ERROR.Pitch >  Angle_Max)       //Pitch
            ANGLE_ERROR_INTE.Pitch +=  Angle_Max;
      if(ANGLE_ERROR.Pitch < -Angle_Max)
            ANGLE_ERROR_INTE.Pitch += -Angle_Max;
      else
            ANGLE_ERROR_INTE.Pitch += ANGLE_ERROR.Pitch;
      if(ANGLE_ERROR_INTE.Pitch >  Angle_Inte_Max)//积分限幅
         ANGLE_ERROR_INTE.Pitch =  Angle_Inte_Max;
      if(ANGLE_ERROR_INTE.Pitch < -Angle_Inte_Max)
         ANGLE_ERROR_INTE.Pitch = -Angle_Inte_Max;
      ANGLE_ERROR_DIFF.Pitch = ANGLE_ERROR.Pitch - ANGLE_ERROR_LAST.Pitch;//e(k-1)
      Pitch_PID.Pout = Pitch_PID.P * ANGLE_ERROR.Pitch;    //pitch PID
      Pitch_PID.Iout = Pitch_PID.I * ANGLE_ERROR_INTE.Pitch;
      Pitch_PID.Dout = Pitch_PID.D * ANGLE_ERROR_DIFF.Pitch;
      Pitch_PID.ALLout = Pitch_PID.Pout + Pitch_PID.Iout + Pitch_PID.Dout;    //pitch OUT
      ANGLE_ERROR_LAST.Pitch = ANGLE_ERROR.Pitch;//
}[/mw_shl_code]
[mw_shl_code=c,true]void Gyro_PID()
{
      //Roll
      GYRO_ERROR.X = Roll_PID.ALLout - GYRO_XYZ.X * Radian_to_Angle;
      if(GYRO_ERROR.X >  Gyro_Max)       //ROLL
            GYRO_ERROR_INTE.X +=  Gyro_Max;
      if(GYRO_ERROR.X < -Gyro_Max)
            GYRO_ERROR_INTE.X += -Gyro_Max;
      else
            GYRO_ERROR_INTE.X += GYRO_ERROR.X;
      if(GYRO_ERROR_INTE.X >  Gyro_Inte_Max)//积分限幅
         GYRO_ERROR_INTE.X =  Gyro_Inte_Max;
      if(GYRO_ERROR_INTE.X < -Gyro_Inte_Max)
         GYRO_ERROR_INTE.X = -Gyro_Inte_Max;
      GYRO_ERROR_DIFF.X = GYRO_ERROR.X - GYRO_ERROR_LAST.X;//e(k-1)
      Gyro_X_PID.Pout = Gyro_X_PID.P * GYRO_ERROR.X;    //PID
      Gyro_X_PID.Iout = Gyro_X_PID.I * GYRO_ERROR_INTE.X;
      Gyro_X_PID.Dout = Gyro_X_PID.D * GYRO_ERROR_DIFF.X;
      Gyro_X_PID.ALLout = Gyro_X_PID.Pout + Gyro_X_PID.Iout + Gyro_X_PID.Dout;    //OUT
      GYRO_ERROR_LAST.X = GYRO_ERROR.X;//

      //Pitch
      GYRO_ERROR.Y = Pitch_PID.ALLout - GYRO_XYZ.Y * Radian_to_Angle;
      if(GYRO_ERROR.Y >  Gyro_Max)       //ROLL
            GYRO_ERROR_INTE.Y +=  Gyro_Max;
      if(GYRO_ERROR.Y < -Gyro_Max)
            GYRO_ERROR_INTE.Y += -Gyro_Max;
      else
            GYRO_ERROR_INTE.Y += GYRO_ERROR.Y;
      if(GYRO_ERROR_INTE.Y >  Gyro_Inte_Max)//积分限幅
         GYRO_ERROR_INTE.Y =  Gyro_Inte_Max;
      if(GYRO_ERROR_INTE.Y < -Gyro_Inte_Max)
         GYRO_ERROR_INTE.Y = -Gyro_Inte_Max;
      GYRO_ERROR_DIFF.Y = GYRO_ERROR.Y - GYRO_ERROR_LAST.Y;//e(k-1)
      Gyro_Y_PID.Pout = Gyro_Y_PID.P * GYRO_ERROR.Y;    //PID
      Gyro_Y_PID.Iout = Gyro_Y_PID.I * GYRO_ERROR_INTE.Y;
      Gyro_Y_PID.Dout = Gyro_Y_PID.D * GYRO_ERROR_DIFF.Y;
      Gyro_Y_PID.ALLout = Gyro_Y_PID.Pout + Gyro_Y_PID.Iout + Gyro_Y_PID.Dout;    //OUT
      GYRO_ERROR_LAST.Y = GYRO_ERROR.Y;//
}[/mw_shl_code]
以下是GYRO滑动滤波：
[mw_shl_code=c,true]void Gyro_Filter(S16_XYZ *Gyro_in,S16_XYZ *Gyro_out)
{
      static int16_t Filter_x[Filter_Num],Filter_y[Filter_Num],Filter_z[Filter_Num];
      static uint8_t Filter_count;
      int32_t Filter_sum_x=0,Filter_sum_y=0,Filter_sum_z=0;
      uint8_t i=0;

      Filter_x[Filter_count] = Gyro_in->X;
      Filter_y[Filter_count] = Gyro_in->Y;
      Filter_z[Filter_count] = Gyro_in->Z;

      for(i=0;i<Filter_Num;i++)
      {
            Filter_sum_x += Filter_x;
            Filter_sum_y += Filter_y;
            Filter_sum_z += Filter_z;
      }

      Gyro_out->X = Filter_sum_x / Filter_Num;
      Gyro_out->Y = Filter_sum_y / Filter_Num;
      Gyro_out->Z = Filter_sum_z / Filter_Num;

      Filter_count++;
      if(Filter_count == Filter_Num)
            Filter_count=0;
}[/mw_shl_code]

看了很多帖子，按照别人成功的思路开始，先调角速度内环，给了P参数之后，晃动机体有抑制力，不过不强，然后逐渐增大到一定值，感觉抑制力足够强的时候，发现角速度环并不能调整角度在某个值，此时角度外环参数均为0，由于重力的原因，机体会不断倾斜。于是开始加入内环的D，从小开始加，并没有看到别人调试的时候遇到的波形，但是机体是有一些变化，可是看不出规律。于是继续增大。。。在增加到某个值的时候，机体瞬间就平稳多了，这种突然来临的效果还是不由得有些小兴奋。继续增大D，发现效果会变差，又换回了状态最好的那组参数，然后去晃动机体，发现每次都能回中，可惜回复速度很慢，感觉应该受限于外环的P，于是开始调整角度环的P，发现这个参数的确能加快回中的速度，调整了几个之后感觉恢复速度够快了，就没有继续增大，怕超调了再次引起震荡。但是机体总感觉不想别人调试的那么连续，感觉是陀螺仪数据不稳的结果，因此加入了滑动滤波，对陀螺仪的输出值进行平滑处理，发现略有效果。目前暂时就干了这些，遥控器的代码还没有开始码，最近很忙，估计会拖到9月10号以后了。以下是“烤四轴”的效果，调试杆是用竹筷子削的，没有用万向节，因为生锈了（阻力有些大）！

在测试的过程中出现了一个奇怪的问题，机体加入roll和pitch之后相对比较稳定，但是会突然180°翻转，出现这种问题这是加速自杀啊！DMP输出的角度（roll和Pitch）范围是±180°，暂时没有找到突然侧翻的原因，也没去监测DMP输出的角度是否有异常，如果有调过的小伙伴遇到同样的问题请给个解决方法，也请路过的大神指点一下。
如果有进度，会在论坛里更新或发帖，但是由于个人原因，能用来做这个的时间很少，所以更新时间不定！有问题大家可以联系我，会在晚上（一般会很晚）回复，以下是个人的联系方式。
QQ：1170513392
邮箱：hhx_destiny@sina.com