EEPW论坛

春节快到了，提前祝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只用到了内环。

——回复可见内容——