赞
踩
参考平衡之家的算法实现
首先明确三个环的PID都是位置式PID
1、角度环:PD
- g_tPidA.actual=roll;
- g_tPidA.err=g_tPidA.actual-g_tPidA.set;
- g_tPidA.delta=gyrox;
- g_tPidA.out=g_tPidA.kp*g_tPidA.err+g_tPidA.kd*g_tPidA.delta;
第一行:取当前角度
第二行:求角度偏差
第三行:取当前角速度
第四行:求角度环输出
2、转速环:PI
- g_tPidS.actual=(g_lSpeed1-g_lSpeed2);
- g_tPidS.err=g_tPidS.actual-g_tPidS.set;
- g_tPidS.delta=0.7*g_tPidS.delta+0.3*g_tPidS.err;
- g_tPidS.integral += g_tPidS.delta;
-
- if(g_ucDir1 == FORWARD)
- g_tPidS.integral += g_fSpeed;
- else if(g_ucDir1 == BACKWARD)
- g_tPidS.integral -= g_fSpeed;
-
- if(g_tPidS.integral>g_fSpeedMax){
- g_tPidS.integral=g_fSpeedMax;
- }else if(g_tPidS.integral<-g_fSpeedMax){
- g_tPidS.integral=-g_fSpeedMax;
- }
- g_tPidS.out = (g_tPidS.ki*g_tPidS.integral)+(g_tPidS.kp*g_tPidS.delta);
- g_tPidS.out = -g_tPidS.out;

第一行:取速度之和作为速度表征量(这里因为方向相反所以取 “-” ,实际计算是数值大小之和)
第二行:求速度偏差
第三行:对速度偏差低通滤波
第四行:求近似积分
第一组if else:根据先后行进方向给定速度(通过给定位移来产生给定速度)
第二组if else:积分限幅(其大小决定速度的最大值)
最后两行:求转速环输出(最后一行可有可无,根据实际极性调整PID参数极性即可)
3、转向环:模糊PD
- if(ucFlag == 0)
- {
- g_tPidT.err = 0;
- fActuralKd = g_tPidT.kd;
- if(g_ucDir2 != STOP)
- {
- ucFlag = 1;
- fTmp = g_lSpeed1 + g_lSpeed2;
- fTmp = 150 / fTmp;
- if(fTmp < 8)
- g_fAdd = 8;
- else if(fTmp > 80)
- g_fAdd = 80;
- else
- g_fAdd = fTmp;
- }
- }
- else
- {
- fActuralKd = 0;
- if(g_ucDir2 == STOP)
- {
- ucFlag = 0;
- }
- else
- {
- if(g_ucDir2 == LEFT)
- g_tPidT.err += g_fAdd;
- else if(g_ucDir2 == RIGHT)
- g_tPidT.err -= g_fAdd;
-
- if(g_tPidT.err > g_fMax)
- g_tPidT.err = g_fMax;
- else if(g_tPidT.err < -g_fMax
- g_tPidT.err = -g_fMax;
- }
- }
- g_tPidT.out = -g_tPidT.kp*g_tPidT.err + fActuralKd*gyroz;

思路:根据左右移动标志计算转向环输出
1、非转向状态下:D控制,抑制转向偏差
采集当前转向角速度
根据转向角速度大小输出控制量抑制转向
2、转向状态下:P控制,人为给定转向偏差
根据当前速度给定一个转向偏差的递增值
每次计算,转向环输出按递增值进行线性递增
转向环输出限幅
3、注意转向环控制量在两个电机的控制中应反向叠加
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。