当前位置:   article > 正文

平衡小车设计_4_PID实现_模糊pid角度环怎么写

模糊pid角度环怎么写

平衡小车设计_4_PID实现

 

参考平衡之家的算法实现

 

首先明确三个环的PID都是位置式PID

 

1、角度环:PD

  1. g_tPidA.actual=roll;
  2. g_tPidA.err=g_tPidA.actual-g_tPidA.set;
  3. g_tPidA.delta=gyrox;
  4. g_tPidA.out=g_tPidA.kp*g_tPidA.err+g_tPidA.kd*g_tPidA.delta;

第一行:取当前角度

第二行:求角度偏差

第三行:取当前角速度

第四行:求角度环输出

 

2、转速环:PI

  1. g_tPidS.actual=(g_lSpeed1-g_lSpeed2);
  2. g_tPidS.err=g_tPidS.actual-g_tPidS.set;
  3. g_tPidS.delta=0.7*g_tPidS.delta+0.3*g_tPidS.err;
  4. g_tPidS.integral += g_tPidS.delta;
  5. if(g_ucDir1 == FORWARD)
  6. g_tPidS.integral += g_fSpeed;
  7. else if(g_ucDir1 == BACKWARD)
  8. g_tPidS.integral -= g_fSpeed;
  9. if(g_tPidS.integral>g_fSpeedMax){
  10. g_tPidS.integral=g_fSpeedMax;
  11. }else if(g_tPidS.integral<-g_fSpeedMax){
  12. g_tPidS.integral=-g_fSpeedMax;
  13. }
  14. g_tPidS.out = (g_tPidS.ki*g_tPidS.integral)+(g_tPidS.kp*g_tPidS.delta);
  15. g_tPidS.out = -g_tPidS.out;

第一行:取速度之和作为速度表征量(这里因为方向相反所以取 “-” ,实际计算是数值大小之和)

第二行:求速度偏差

第三行:对速度偏差低通滤波

第四行:求近似积分

第一组if else:根据先后行进方向给定速度(通过给定位移来产生给定速度)

第二组if else:积分限幅(其大小决定速度的最大值)

最后两行:求转速环输出(最后一行可有可无,根据实际极性调整PID参数极性即可)

 

3、转向环:模糊PD

  1. if(ucFlag == 0)
  2. {
  3. g_tPidT.err = 0;
  4. fActuralKd = g_tPidT.kd;
  5. if(g_ucDir2 != STOP)
  6. {
  7. ucFlag = 1;
  8. fTmp = g_lSpeed1 + g_lSpeed2;
  9. fTmp = 150 / fTmp;
  10. if(fTmp < 8)
  11. g_fAdd = 8;
  12. else if(fTmp > 80)
  13. g_fAdd = 80;
  14. else
  15. g_fAdd = fTmp;
  16. }
  17. }
  18. else
  19. {
  20. fActuralKd = 0;
  21. if(g_ucDir2 == STOP)
  22. {
  23. ucFlag = 0;
  24. }
  25. else
  26. {
  27. if(g_ucDir2 == LEFT)
  28. g_tPidT.err += g_fAdd;
  29. else if(g_ucDir2 == RIGHT)
  30. g_tPidT.err -= g_fAdd;
  31. if(g_tPidT.err > g_fMax)
  32. g_tPidT.err = g_fMax;
  33. else if(g_tPidT.err < -g_fMax
  34. g_tPidT.err = -g_fMax;
  35. }
  36. }
  37. g_tPidT.out = -g_tPidT.kp*g_tPidT.err + fActuralKd*gyroz;

思路:根据左右移动标志计算转向环输出

1、非转向状态下:D控制,抑制转向偏差

采集当前转向角速度

根据转向角速度大小输出控制量抑制转向

 

2、转向状态下:P控制,人为给定转向偏差

根据当前速度给定一个转向偏差的递增值

每次计算,转向环输出按递增值进行线性递增

转向环输出限幅

 

3、注意转向环控制量在两个电机的控制中应反向叠加

 

 

 

 

 

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/喵喵爱编程/article/detail/956613
推荐阅读
相关标签
  

闽ICP备14008679号