九、PID算法

通过前面的学习，我们已经初步了解了软件是如何进行设计实现的；这一节我们接触的PID算法不再引入C++语言方面的新知识。PID算法是控制领域极其重要的一个算法，已经有很多书中都讲述了PID算法，那么为什么这里又将PID算法单列出来一个章节进行讲述呢？例如，在许多电机控制相关的教材中，PID算法的讲述与电机相关知识结合，增加了学生理解的难度，并且PID算法的实现依赖于硬件设施，没有相关实验条件的学生很难通过实验对所学的知识进行验证。

对于所有知识的掌握，我们都秉持着循循渐进的原则。在这一章，我们通过模拟的方式，尽可能屏蔽其他学科知识的干扰，尽可能简单地通过程序对PID算法进行模拟实现，带领大家对这个算法有一个初步的认识，以便将来遇到真正地需要使用PID算法进行相关工程问题的解决的时候，能够快速入门。

9.1 PID算法思想

PID算法广泛应用于工业控制领域，这里我们以具体的电机控制为例，来慢慢进入到抽象的PID算法的数学公式中。为了让大家能够快速理解PID算法的思想，这里不严谨地以电机控制来打个比方，如果对一个小电机我们施加一个6V的电压，电机对应的转速是1000r/min；突然有电机上面加上了一个负载，电机的速度降低至500r/min，我们想让电机保持1000r/min的转速，这个时候应该怎么办呢？我们将电压提升至12V，此时电机速度转速至1100r/min；这样就超出了我们理想的1000r/min，此时我又要减小电压，让电机速度下降至1000r/min...

对以上过程进行总结：当电机转速低于我们预想的转速的时候，我们调高电压；当电机转速高于预想的转速的时候，我们调低电压；整个过程的调节依赖于实际速度和理想速度的偏差，然后人为地进行控制调节。

我们知道现今有了计算机，这一套流程可以交给计算机进行控制调节。而计算机控制依赖于数字计算，PID算法正是针对此类控制的算法。

顾名思义，PID算法的名字中的“P”、“I”、“D”分别代表“比例(proportion)”、“积分(integral)”、“微分(derivative)”，对应的数学表达式如下所示：

其中，u(t)为控制器(或计算机)输出的控制量；e(t)为偏差信号，它等于给定量与输出量之差；即可以代表我们前面所提到的实际速度和理想速度的偏差；Kp、Ki、Kd三个参数是人为设定的，针对不同的应用场景改变这三个参数来得到不同的输出u(t)。

上述数学公式为连续的，我们知道计算机只能对有限的离散量进行计算，所以将连续的PID算法的离散化，可得公式如下所示：

依旧以电机调速为例，为了模拟整个过程，我们假定电机每增加1V电压，电机转速增加1r/min（这里为了进行PID算法模拟实现，虚构了电机电压与转速的关系）；目标速度TargetSpeed设为200(r/min)，三个参数Kp、Ki、Kd分别设置为0.3、0.02、0.02；我们看一看计算机是如何不断根据目标速度TargetSpeed以及实际速度ActualSpeed的偏差error来调节输出电压输出的。

系统刚启动时，实际速度ActualSpeed=0，e1=TargetSpeed-ActualSpeed=200，则

此时，电压输出为68V，对应电机为68r/min；然后控制器继续进行新一轮的计算调节，此时，新的实际速度ActualSpeed=68，e2=TargetSpeed-ActualSpeed=132，则

电压输出变为44.88V，对应电机为44.88r/min；然后控制器继续新一轮计算，此时，实际速度是ActualSpeed=44.88，e3=TargetSpeed-ActualSpeed=155.12，则

电压输出为56.7408，对应电机转速为56.7408r/min...

通过不断地反馈，计算器得到实际速度与目标速度的差值，然后计算调节最终让实际速度稳定在目标速度附近。我们可以看到通过调节Kp、Ki、Kd三个参数就可以调节实际速度达到目标速度的快慢，以及达到目标速度后是否能够快速稳定在目标速度附近。如何根据不同的工程需要调节合适的参数也是一项很重要的工作，这里受制于篇幅限制，不再进行展开。

上面只给出了三次手动计算，后面大家最好可以自己手动算一下，这样能够更好理解PID算法；接下来我们通过编写程序，让计算机帮我们进行以上计算。

9.2 PID算法程序模拟

我们这一节通过编写一个程序来模拟上述过程，我们依旧使用输入输出流来观察计算机执行PID算法的结果是否与我们手动的相一致。程序如表9-1所示。（程序解释在后面）

#include<iostream>
using namespace std;

struct PID{
	double TargetSpeed;
	double ActualSpeed;
	double Error;
	double ErrorLast;
	double Kp,Ki,Kd;
	double Voltage;
	double Integral;
}PIDtest;

double PIDRealize(double SetSpeed)
{
	PIDtest.TargetSpeed=SetSpeed;      
	PIDtest.Error=PIDtest.TargetSpeed-PIDtest.ActualSpeed;     
	PIDtest.Integral=PIDtest.Integral+PIDtest.Error;      
	PIDtest.Voltage=PIDtest.Kp*PIDtest.Error+PIDtest.Ki*PIDtest.Integral+PIDtest.Kd*(PIDtest.Error-PIDtest.ErrorLast);	//PID算法公式
	PIDtest.ErrorLast=PIDtest.Error;      
	PIDtest.ActualSpeed=PIDtest.Voltage*1.0;     
	return PIDtest.ActualSpeed;
}

int main()
{	
	PIDtest.TargetSpeed=0.0;     
	PIDtest.ActualSpeed=0.0;     
	PIDtest.Error=0.0;      
	PIDtest.ErrorLast=0.0;     
	PIDtest.Voltage=0.0;     
	PIDtest.Integral=0.0;     
	PIDtest.Kp=0.3;     
	PIDtest.Ki=0.02;     
	PIDtest.Kd=0.02;   

	double Speed=0.0;
	for(int i=0;i<300;i++)     
	{          
		Speed=PIDRealize(200.0);         
		cout<<Speed<<endl;  
	}
	system("pause");
	return 0;
}

表9-1 PID算法模拟程序表

我们定义了一个名为PID的结构体，成员包括目标速度TargetSpeed，实际速度ActualSpeed，当前的偏差Error，上一次的偏差ErrorLast（由于微分环节要计算需要记录上一次偏差），三个参数——Kp、Ki、Kd，当前的输出电压Voltage，偏差之和Integral（积分环节计算需要所有的偏差之和）；最后我们创建了名为PIDtest的结构体变量。

 

接下来我们要实现PID算法，定义一个名为PIDRealize的函数，返回值为double型，形式参数只有一个——SetSpeed，我们将形式参数赋值给TargetSpeed，也就是说我们后面调用PIDRealize函数时，放入的实际参数即我们要最终想要通过计算机自动计算达到的目标速度；其后我们通过用目标速度PIDtest.TargetSpeed减去实际速度PIDtest.ActualSpeed，得到当前这一轮计算的偏差PIDtest.Error；第18行代码将每次计算得到的PIDtest.Error累加起来，其结果为PIDtest.Integral；第19行为PID算法公式；第20行我们将每次的偏差结果赋值给PIDtest.ErrorLast，这样下一次进行PID算法公式计算的时候，就可以使用；为了模拟电压和电机的关系，我们将输出电压的值乘以1.0赋值给实际速度；第22行代码我们将最终的实际速度结果进行返回。

 

进入主函数后，我们先对PIDtest的成员进行初始化，对三个参数Kp、Ki、Kd分别赋值为0.3、0.02、0.02，其他成员赋值为0；第37行至42行我们通过一个for循环语句使计算机进行300次PID算法计算，目标速度设定为200.0，并且每次计算后将结果赋值给double型变量Speed，并将Speed的值输出先出来。

完成上述程序后，我们可以按下F5键进行执行，我们能够对比前三次结果和我们手动计算的一致。大家还可以更改一下Kp、Ki、Kd三个参数，观察一下相应的输出结果变化。