平衡小车设计_4_PID实现

平衡小车设计_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、注意转向环控制量在两个电机的控制中应反向叠加