电子产品世界 » 论坛首页 » 电子DIY » QuadCopter DIY » 串级PID----代码+调参


共128条 1/13 1 2 3 4 5 6 ›| 跳转至

串级PID----代码+调参

高工
2015-02-17 00:50:42    评分

春节快到了,提前祝EEPW的网友们新年快乐,万事如意。

在这里分享一下串级PID的代码和调参经验。


串级PID,外环是角度环,内环是角速度环。

角度环通过角度误差pid计算,得到角速度的期望值,送到角速度环;角速度环再做一次pid得到电机输出。

这样做的目的是,通过两级pid串联,增大了四轴的阻尼(角度误差(角速度)阻尼、角速度误差(角加速度)阻尼)。参数调节好,四轴在飞行跟随手感会很好,机动性强,指哪打哪,也能很好的做到悬停。


下面是外环代码:

#define angle_max 	 		10.0f	
#define angle_integral_max 	1000.0f	
/*******************************************************
函数原型:	void Control_Angle(struct _out_angle *angle,struct _Rc *rc)
功    能:	PID控制器(外环)
*******************************************************/
void Control_Angle(struct _out_angle *angle,struct _Rc *rc)
{
	static struct _out_angle control_angle;
	static struct _out_angle last_angle;
//////////////////////////////////////////////////////////////////
//			以下为角度环
//////////////////////////////////////////////////////////////////
	if(rc->ROLL>1490 && rc->ROLL<1510)	
		rc->ROLL=1500;
	if(rc->PITCH>1490 && rc->PITCH<1510)	
		rc->PITCH=1500;
//////////////////////////////////////////////////////////////////
	control_angle.roll  = angle->roll  - (rc->ROLL  -1500)/13.0f ;
	control_angle.pitch = angle->pitch - (rc->PITCH -1500)/13.0f;
//////////////////////////////////////////////////////////////////
	if(control_angle.roll >  angle_max)	//ROLL
		roll.integral +=  angle_max;
	else if(control_angle.roll < -angle_max)
		roll.integral += -angle_max;
	else
		roll.integral += control_angle.roll;
	
	if(roll.integral >  angle_integral_max)
	   roll.integral =  angle_integral_max;
	if(roll.integral < -angle_integral_max)
	   roll.integral = -angle_integral_max;
//////////////////////////////////////////////////////////////////
	if(control_angle.pitch >  angle_max)//PITCH
	   pitch.integral +=  angle_max;
	else if(control_angle.pitch < -angle_max)
	   pitch.integral += -angle_max;
	else
		pitch.integral += control_angle.pitch;

	if(pitch.integral >  angle_integral_max)
	   pitch.integral =  angle_integral_max;
	if(pitch.integral < -angle_integral_max)
	   pitch.integral = -angle_integral_max;
//////////////////////////////////////////////////////////////////
	if(rc->THROTTLE<1200)//油门较小时,积分清零
	{
		roll.integral  = 0;
		pitch.integral = 0;
	}
//////////////////////////////////////////////////////////////////
	roll.output  = roll.kp *control_angle.roll  + roll.ki *roll.integral  + roll.kd *(control_angle.roll -last_angle.roll );
	pitch.output = pitch.kp*control_angle.pitch + pitch.ki*pitch.integral + pitch.kd*(control_angle.pitch-last_angle.pitch);
//////////////////////////////////////////////////////////////////
	last_angle.roll =control_angle.roll;
	last_angle.pitch=control_angle.pitch;
}

 


下面是内环代码:

#define gyro_max 	 		50.0f	
#define gyro_integral_max 	5000.0f
/******************************************************************************
函数原型:	void Control_Gyro(struct _SI_float *gyro,struct _Rc *rc,uint8_t Lock)
功    能:	PID控制器(内环)
*******************************************************************************/ 
void Control_Gyro(struct _SI_float *gyro,struct _Rc *rc,uint8_t Lock)
{
	static struct _out_angle control_gyro;
	static struct _out_angle last_gyro;
	int16_t throttle1,throttle2,throttle3,throttle4;
//////////////////////////////////////////////////////////////////
//			以下为角速度环
//////////////////////////////////////////////////////////////////
	if(rc->YAW>1400 && rc->YAW<1600)
		rc->YAW=1500;
//////////////////////////////////////////////////////////////////
	control_gyro.roll  = -roll.output - gyro->y*Radian_to_Angle;
	control_gyro.pitch = pitch.output - gyro->x*Radian_to_Angle;
	control_gyro.yaw   = -(rc->YAW-1500)/2.0f - gyro->z*Radian_to_Angle ;
//////////////////////////////////////////////////////////////////
	if(control_gyro.roll >  gyro_max)	//GYRO_ROLL
		gyro_roll.integral +=  gyro_max;
	else if(control_gyro.roll < -gyro_max)
		gyro_roll.integral += -gyro_max;
	else
		gyro_roll.integral += control_gyro.roll;
	
	if(gyro_roll.integral >  gyro_integral_max)
	   gyro_roll.integral =  gyro_integral_max;
	if(gyro_roll.integral < -gyro_integral_max)
	   gyro_roll.integral = -gyro_integral_max;
//////////////////////////////////////////////////////////////////
	if(control_gyro.pitch >  gyro_max)//GYRO_PITCH
		gyro_pitch.integral +=  gyro_max;
	else if(control_gyro.pitch < -gyro_max)
		gyro_pitch.integral += -gyro_max;
	else
		gyro_pitch.integral += control_gyro.pitch;
	
	if(gyro_pitch.integral >  gyro_integral_max)
	   gyro_pitch.integral =  gyro_integral_max;
	if(gyro_pitch.integral < -gyro_integral_max)
	   gyro_pitch.integral = -gyro_integral_max;
//////////////////////////////////////////////////////////////////
//	if(control_gyro.yaw >  gyro_max)//GYRO_YAW
//		gyro_yaw.integral +=  gyro_max;
//	else if(control_gyro.yaw < -gyro_max)
//		gyro_yaw.integral += -gyro_max;
//	else
		gyro_yaw.integral += control_gyro.yaw;
	
	if(gyro_yaw.integral >  gyro_integral_max)
	   gyro_yaw.integral =  gyro_integral_max;
	if(gyro_yaw.integral < -gyro_integral_max)
	   gyro_yaw.integral = -gyro_integral_max;
//////////////////////////////////////////////////////////////////
	if(rc->THROTTLE<1200)//油门较小时,积分清零
	{
		gyro_yaw.integral  = 0;
	}
//////////////////////////////////////////////////////////////////
	gyro_roll.output  = gyro_roll.kp *control_gyro.roll  + gyro_roll.ki *gyro_roll.integral  + gyro_roll.kd *(control_gyro.roll -last_gyro.roll );
	gyro_pitch.output = gyro_pitch.kp*control_gyro.pitch + gyro_pitch.ki*gyro_pitch.integral + gyro_pitch.kd*(control_gyro.pitch-last_gyro.pitch);
	gyro_yaw.output   = gyro_yaw.kp  *control_gyro.yaw   + gyro_yaw.ki  *gyro_yaw.integral   + gyro_yaw.kd  *(control_gyro.yaw  -last_gyro.yaw  );
//////////////////////////////////////////////////////////////////
	last_gyro.roll =control_gyro.roll;
	last_gyro.pitch=control_gyro.pitch;
	last_gyro.yaw  =control_gyro.yaw;
//////////////////////////////////////////////////////////////////
	if(rc->THROTTLE>1200 && Lock==0)
	{
		throttle1 = rc->THROTTLE - 1050 + gyro_pitch.output + gyro_roll.output - gyro_yaw.output;
		throttle2 = rc->THROTTLE - 1050 + gyro_pitch.output - gyro_roll.output + gyro_yaw.output;
		throttle3 = rc->THROTTLE - 1050 - gyro_pitch.output + gyro_roll.output + gyro_yaw.output;
		throttle4 = rc->THROTTLE - 1050 - gyro_pitch.output - gyro_roll.output - gyro_yaw.output;
	}
	else
	{
		throttle1=0;
		throttle2=0;
		throttle3=0;
		throttle4=0;
	}
	Motor_Out(throttle1,throttle2,throttle3,throttle4);
}

 

注意到,pitch和roll用到了内外环,yaw只用到了内环。

——回复可见内容——





关键词: 串级PID     代码     调参    

菜鸟
2015-02-28 15:56:23    评分
2楼
好东西....

菜鸟
2015-03-10 07:24:16    评分
3楼
 不错,一直搞不懂串级PID啊

高工
2015-03-12 15:05:48    评分
4楼
看看

菜鸟
2015-03-12 20:51:00    评分
5楼
 不错,一直搞不懂串级PID啊

菜鸟
2015-03-14 22:58:55    评分
6楼
学习下                     

菜鸟
2015-03-15 20:01:31    评分
7楼

请问下PID内环中,

control_gyro.roll  = -roll.output gyro->y*Radian_to_Angle;

用的是外环得出的 期望的角度 减去 角速的值,这个单位都不一样,为何能减呢? 


高工
2015-03-16 09:30:20    评分
8楼

roll.output 

是角度环做了PID的输出,得到的是角速度环控制量。不用纠结单位


菜鸟
2015-03-17 17:14:54    评分
9楼
哦。                                                                               

菜鸟
2015-03-17 18:52:34    评分
10楼
楼主给力

共128条 1/13 1 2 3 4 5 6 ›| 跳转至

回复

匿名不能发帖!请先 [ 登陆 注册 ]