学习记录--PID（角度双环）

一，理论理解

参考：从不懂到会用！PID从理论到实践~_哔哩哔哩_bilibili

1. 三个参数

Kp：比例系数：pid->p_out  = pid->kp * pid->err[0];（p项输出为kp*（本次误差））可知Kp可调曲线的斜率，但大了会很跳跃

Ki：积分系数：pid->i_out += pid->ki * pid->err[0]; （i项输出为ki*（所有误差的累积））可知Ki可以使受控目标达到target。由控制无人机悬停高度的例子可知，只由kp是不能使无人悬停到target高度的：始终会有稳态误差（需要一个误差*kp来提供稳态，否则例如若没有误差->力=0->无人机下坠）这时就要通过不断地累计误差增加pid->i_out来达到target，达到后i_out就不变了（而不是变为0，因此能支撑达到稳态）

积分限幅：防止积分项过大（当长时间外部影响导致误差积累过大时（例如人为压着无人机），积分项会过大而很久才稳定）

积分分离：当误差过大时就不让积分项发挥作用（例如需要突然改变target，积分分离可以防止积分项由于误差的突变而产生的突变）

Kd：微分系数：pid->d_out  = pid->kd * (pid->err[0] - pid->err[1]);（d项输出为kd*（本次误差-上次误差）（连续上来说就是斜率））可以用来抵消Kp和Ki的影响，防止曲线的剧烈抖动。

2. 双环控制

注意自动量纲转化的理解：进行量纲转化无非就是对变量进行常数处理，而这些在最终的计算中都可以放进三个参数中。

（上图出自421施工队(见参考视频)）

二，代码部分

1. pid:

#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))//积分限幅

typedef struct _pid_struct_t
{
  float kp;
  float ki;
  float kd;
  float i_max;		//限幅
  float out_max;
	float i_band;		//分离
  
  float target;      
  float feedback;      		
  float err[2];   		// error and last error

  float p_out;
  float i_out;
  float d_out;
  float total_out;
}pid_struct_t;

void pid_init(pid_struct_t *pid, float kp, float ki, float kd, float i_max, float out_max, float i_band)
{
  pid->kp      = kp;
  pid->ki      = ki;
  pid->kd      = kd;
  pid->i_max   = i_max;
  pid->out_max = out_max;
	pid->i_band  = i_band;
}

float pid_calc(pid_struct_t *pid, float target, float feedback)
{
  pid->target = target;
  pid->feedback = feedback;
  pid->err[1] = pid->err[0];										//上一次误差
  pid->err[0] = pid->target - pid->feedback;		//本次的误差
  
  pid->p_out  = pid->kp * pid->err[0];
	pid->d_out  = pid->kd * (pid->err[0] - pid->err[1]);
  if(fabs(pid->err[0]) < pid->i_band)						//积分分离：当误差太大时就不让积分项发挥作用  
	{
		pid->i_out += pid->ki * pid->err[0];
	  LIMIT_MIN_MAX(pid->i_out, -pid->i_max, pid->i_max);	//对积分项限幅
	}
	else
	{
		pid->i_out = 0;
	}
  pid->total_out = pid->p_out + pid->i_out + pid->d_out;
  LIMIT_MIN_MAX(pid->total_out, -pid->out_max, pid->out_max);//对总输出限幅
  return pid->total_out;
}

 2: 角度更新

//角度积累更新函数（注意位置target可以有多圈）
void update_angle(motor_angle* _angle, uint16_t angle_fbk)
{
	_angle->encoder = angle_fbk;		
	if(_angle->encoder_is_init)
	{
		if(_angle->encoder - _angle->last_encoder > 4096)	//当前电机反馈角度-上次反馈角度超过半圈
		{																									//由于角度值为0-8292：想获得角度的积累，要用圈计数来辅助（当前反馈-上次反馈值）才能达到目的
			_angle->round_cnt --;																	
		}
		if(_angle->encoder - _angle->last_encoder < -4096)	
		{
			_angle->round_cnt ++;														//++--正好不同情况下凑整圈
		}
	}
	else		//只执行一次
	{
		_angle->encoder_offset = _angle->encoder;		//第一次得到的角度反馈赋给encoder_offset（零点）
		_angle->encoder_is_init = 1;
	}
	_angle->angle_offset = _angle->encoder_offset/8292.0f * 360.0f;	//机械角度值转化为角度值
	_angle->last_encoder = _angle->encoder;
	_angle->total_encoder = _angle->round_cnt*8192 + _angle->encoder - _angle->encoder_offset;	
	_angle->angle = _angle->total_encoder/8192.0f * 360.0f;				//量纲转换
}

三. 上位机调参

根据所用上位机的协议收发数据包即可