平衡小车设计_4_PID实现
参考平衡之家的算法实现
首先明确三个环的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、注意转向环控制量在两个电机的控制中应反向叠加