舵机PID控制

舵机工作原理

舵机PID控制

原理介绍

代码

主代码

赋值PWM寄存器

限制PWM赋值

取绝对值

位置式PID控制

位置式离散PID

舵机工作原理

控制电路接收信号源的控制脉冲，并驱动电机转动：控制电路接收来自遥控器或者单片机的控制信号，然后驱动电机进行转动。

齿轮组将电机的速度成大倍数缩小，并将电机的输出扭矩放大响应倍数，然后输出：齿轮组起到了减速和增加扭矩的作用，让电机的输出更适合舵机的操作。

电位器和齿轮组的末级一起转动，测量舵机轴转动角度：通过电位器和齿轮组，我们可以测量舵机轴的转动角度，也就是舵机当前停留的位置。

电路板检测并根据电位器判断舵机转动角度，然后控制舵机转动到目标角度或保持在目标角度：电路板会根据电位器测量的角度信息来判断舵机的位置，并根据设定的目标角度来控制舵机是否需要继续转动或者停留在目标位置。

模拟舵机需要一个外部控制器（遥控器的接收机或者单片机）产生脉宽调制信号来告诉舵机转动角度，脉冲宽度是舵机控制器所需的编码信息：最后，模拟舵机需要外部的控制器产生特定脉冲宽度的调制信号，来告诉舵机应该转动到什么角度，脉冲宽度对应着舵机应该停留的位置。舵机的控制脉冲周期是20毫秒，脉宽从0.5毫秒到2.5毫秒，分别对应着-90度到+90度的位置（对于180°舵机）。

有关舵机控制的可以阅读这篇文章：stm32f1的一个定时器多路pwm控制舵机_stm32舵机pwm程序-CSDN博客

舵机PID控制

原理介绍

已知舵机的控制只需要给定一个 PWM 信号和合适的电源，就可以控制它转动的角度（实现位置控制）。如果我们要进行 PID 闭环控制，就必须有一个反馈值，但在这种情况下我们实际上是没有的。因此，我们需要调整我们的策略。由于舵机自带位置闭环控制功能，假设：只要给定范围内的目标位置信号，舵机都能到达该位置。也就是说，当前控制周期的输出目标位置会作为下一个控制周期的已知测量位置。

简单点就是：假设舵机具有良好的位置闭环控制能力，能够准确地到达我们所指定的目标位置。因此，在每个控制周期结束时，我们可以将当前周期的目标位置视为下一个周期的起始位置，从而实现连续的位置控制。

注意的是：这种假设的前提是确保每个控制周期的控制幅度不会超出舵机的响应范围，以确保舵机能够按照我们的期望进行位置调节。就是它上一个周期的控制信号确实抵达了，因此我们需要确认我们每个周期的控制幅度在它的响应范围内。

假设当系统的控制周期为10毫秒，每个周期控制位置运动10个单位（先不考虑具体单位）。通过离散化，我们可以计算出舵机的控制速度：v=10/10ms=1/ms。这意味着每毫秒舵机的位置变化了1个单位。

我们知道舵机的控制脉冲周期是50赫兹，对应着脉冲宽度在0.5毫秒到2.5毫秒之间，分别对应-90度到+90度的角度范围。假设输出值A=10000时对应100%占空比，那么脉冲宽度在0.5毫秒到2.5毫秒之间对应的A值分别为：0.5/2010000=250 和 2.5/2010000=1250。即为 Amax -Amin =1000 对应 90-（-90） =180°。对应着90度到-90度的角度范围。这里的10和之前提到的1000是相对应的。所以10对应的角度就是180°/1000*10=1.8°。考虑到单位时间是10毫秒，所以舵机的速度是 V=1.8°/10ms=18°/s。

通过这样的计算，我们可以将舵机的速度与其参数进行比较。假设舵机的速度参数是0.14秒/60°=428°/s，我们可以发现设定的速度远小于舵机本身的参数，因此响应时间是完全足够的。

外部提供一个控制信号，表示系统应该达到的目标位置。通过上一个周期的控制输出值，计算出系统上一个周期已经到达的位置，并将该位置作为位置PID控制器的输入。位置PID控制器根据当前位置和目标位置之间的误差，计算出一个控制输出值，用于调节系统朝目标位置移动。对于控制输出值，根据设定的目标速度进行限幅处理，确保控制信号不会过大。将经过限幅处理后的控制信号进行位置积分，以便更好地追踪目标位置的变化。最后，通过单片机输出PWM信号，控制系统执行相应的动作，实现位置控制。

代码

主代码

int Target_Velocity=20,Position=250,Target_Position=250; //Position为当前值，Target_Position为目标值
float Position_KP=120,Position_KI=0.1,Position_KD=500; //PID参数
int TIM1_UP_IRQHandler(void)  
{    
	if(TIM_GetFlagStatus(TIM1,TIM_FLAG_Update)==SET)//10ms定时器中断
	{   
		TIM_ClearITPendingBit(TIM1,TIM_IT_Update);     //清除定时器中断标志位	                                        
		Moto=Position_PID(Position,Target_Position);    //位置PID
		Moto=Xianfu(Moto,Target_Velocity);              //PWM限幅
		Set_Pwm(Moto);
		
	}       	
	 return 0;	  
}

首先，定义了三个变量：Target_Velocity表示目标转速，Position表示当前位置，Target_Position表示目标位置。

然后，定义了三个PID参数：Position_KP、Position_KI和Position_KD，用于控制PID算法的比例、积分和微分项的权重。

在定时器中断中，通过调用Position_PID函数实现位置式离散PID控制算法，计算出需要输出的PWM值，然后通过Xianfu函数对PWM进行限幅，最后通过Set_Pwm函数将PWM值输出到舵机上，控制舵机。

赋值PWM寄存器

void Set_Pwm(int moto)
{
	Position+=moto;
	if(Position>1250)Position=1250;
	if(Position<250)Position=250;
	TIM_SetCompare1(TIM2,Position);
		
}

设置PWM信号的占空比，控制舵机的位置。代码中的变量Position代表当前PWM信号的占空比值，moto是控制舵机转动的参数，表示需要增加或减少的占空比值。

在函数中，我们先将moto的值加到Position上，然后进行范围限制。如果Position大于1250，说明舵机已经转到最大角度位置，就将Position设置为1250；如果Position小于250，说明舵机已经转到最小角度位置，就将Position设置为250。

最后，通过调用STM32的TIM_SetCompare1函数，将Position的值设置为PWM信号的占空比，从而控制舵机的位置。

限制PWM赋值

int Xianfu(int value,int Amplitude)
{	
	int temp;
	if(value>Amplitude) temp = Amplitude;
	else if(value<-Amplitude) temp = -Amplitude;
	else temp = value;
	return temp;		
}

用于对给定的数值value进行幅值限制，限制的幅值为Amplitude。函数返回限制后的数值。

函数逻辑如下：

如果value大于幅值Amplitude，则将temp赋值为Amplitude。
否则，如果value小于负幅值-Amplitude，则将temp赋值为-Amplitude。
否则，即value在-Amplitude和Amplitude之间，则将temp赋值为value。

最后，函数返回temp，即经过幅值限制处理后的数值。

取绝对值

int myabs(int a)
{ 		   
	  int temp;
		if(a<0)  temp=-a;  
	  else temp=a;
	  return temp;
}

位置式PID控制

int Position_PID (int position,int target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=target-position;                                 
	 Integral_bias+=Bias;	                                 
	 Pwm=Position_KP*Bias/100+Position_KI*Integral_bias/100+Position_KD*(Bias-Last_Bias)/100;       
	 Last_Bias=Bias;                                       
	 return Pwm;                                          
}

根据当前位置和目标位置计算输出PWM值来控制某个系统。下面是代码中各变量和计算步骤的解释：

static float Bias, Pwm, Integral_bias, Last_Bias; 这些变量在函数调用之间保持其值不变，并且仅在定义它们的代码块内可见。

Bias = target - position; 计算当前位置与目标位置之间的偏差（误差）。

Integral_bias += Bias; 累积偏差，用于计算积分项。

Pwm = Position_KP * Bias / 100 + Position_KI * Integral_bias / 100 + Position_KD * (Bias - Last_Bias) / 100; 根据比例、积分和微分项计算PID输出值。其中Position_KP、Position_KI和Position_KD分别代表位置式PID控制器的比例增益、积分增益和微分增益。

Last_Bias = Bias; 将当前偏差保存为上一次的偏差，用于计算微分项。

return Pwm; 返回计算得到的PWM输出值。