位置型PID的实现——基于直线一级倒立摆
倒立摆的实验是在小半年前的课程实验中遇到的，由于比较感兴趣，就多花了些时间研究了一下，实验设备是前海格致便携倒立摆，编写语言是C语言，如果你需要使用其他语言，我相信在理解位置型PID的原理后你将能够非常轻易的编写出来。
学艺不精，只知道一些基础的内容，本文主要用作学习记录，如果有错误，欢迎指正。
目录
一、先介绍下PID的基本原理：
二、接下来看一下PID的基本公式：
三、最后还需要理解串级PID：
四、倒立摆源码分析及代码编写依据
五、程序源代码
一、先介绍下PID的基本原理：
- P（Proportional），比例。通过当前误差进行计算，通过比例快速调节，但无法消除静态误差。就好比你在百米赛道上，把你眼睛蒙着，你控制自己每一步只走剩余赛道的1/100，在绝对理想的情况下，你也只能无限趋近于终点线，更别说在实际系统中的各种误差和变化。
- I（Integral），积分。通过过去误差进行计算，通过积分进行计算，可以消除静态误差，但在实际环境中容易使系统响应过慢。这个其实很好理解，你在实际算数学题的时候就会发现，往往求积分比求微分更快，比起乘法自然更快。
- D(Derivative)，微分。通过对误差的变化趋势进行计算，可以消除振荡，但如果输入中存在高频噪音，则会被放大。
这个的解释稍微复杂一点。众所周知，波形的一般表达式为：
其中
越大，代表其频率越高。
现在在输入中存在一个噪音，表达式为:
，对它求微分，就变成了：
，足足放大了一千倍！所以频率越高，其放大倍数越大，则越容易形成干扰。
我自学自动控制是在B站看的"DR_CAN"的课，他的课可能不能帮你通过考试，但一定可以加深你对于知识的理解。其中有一句话：
做人当如PID一样，“利用现在（P），总结过去（I），预测未来（D）”。
由于单用一种调节方式存在较大的弊端，所以一般会组合进行调节，例如：
PI调节：可以有效改善稳态误差。
PD调节：提高稳定性和响应速度，即改变瞬态。
PID调节：可以达到理想的多项性能指标要求。
二、接下来看一下PID的基本公式：
- PID控制公式：
![u \left ( t \right ) = Kp\left [e\left ( t \right ) +\frac{1}{Ti}\int_{0}^{t}e\left ( t \right )\cdot dt +Td\frac{de\left ( t \right )}{dt}\right ]](http://img.e-com-net.com/image/info8/d82f1f276d714dd48c7cceeef609f01b.gif)
其中
为输出的控制量，
为偏差信号，
为比例增益，
为积分常数，
为微分常数。 - 位置型PID基本公式：
![u\left ( k \right )=Kp\cdot e\left ( k \right )+Ki\cdot \sum_{i=0}^{}e(i)+Kd\cdot \left [ e(k)-e(k-1) \right ]](http://img.e-com-net.com/image/info8/17300ca41f714d57b6163ae0c4a49bb5.gif)
其中
为目标值-当前值，
误差的累积，
为这次误差-上次误差。
由于该PID控制是以系统当前的实际位置和你想达到位置的偏差来进行计算，因此也叫做位置型PID。除此之外还有增量型PID，积分分离型PID，微分先行型PID等等，各种类型之间不能说有绝对的优劣之分，只能说适用条件各不相同。
三、最后还需要理解串级PID：
-
先看单级PID：仅靠一个回路的单回路控制系统，学自动控制绝对会接触的温度控制系统便是一个典型的单回路控制系统，如果采用PID的控制方法则为单级PID。
如图便是一个简单的单级PID控制系统：
- 串级PID：依靠两个回路的双回路控制系统，且回路中的两个PID控制器以类似于电路中“串联”的方式进行连接。
如图便是一个简单的串级PID控制系统：
- 相较于单级PID，串级PID抗干扰能力强，稳定性更好，缺点是运算量增加，两者的效果区别直接看图则更加明显。
蓝色为串级PID，黄色为单级PID，明显可以看到其调节时间更短，超调量更小。
至于何时使用串级PID，在系统中只有一个PID控制器或多个PID控制器均作用于同一个控制器时则可以考虑使用，例如无人机中的电机，汽车的驱动马达，包括此次会提到的倒立摆。
以上图片截取自simulink。 - 在实际应用之中，也会使用并级PID，其实现效果与串级类似，再考虑到此为多输入，输出叠加到一起的单输出系统，使用并级PID其计算也更加简单，之后的程序便是基于并级PID进行编写。（理论上要给速度控制添加低通滤波器以达到更好的效果，我在程序中并没有添加，所得到的倒立摆稳定效果也比较理想）
基于倒立摆的并级PID示意图如图所示：
四、倒立摆源码分析及代码编写依据
前文也提到了，这是前海格致便携倒立摆，底层代码以及通信部分都已经编写好了，只需要考虑其控制代码的编写就可以，及串级PID代码的编写。
代码还是挺多的，首先分析源码，找到有用的数据还是非常重要的，倒立摆的源码也不全部放出来了，就在这里截取有用的源码放出进行分析。
//PID控制器控制参数
double KP_Pos=8.3;//小车位置环
double KD_Pos=6.8;
double KP_Ang=37.8;//摆杆角度环
double KD_Ang=3.4;
//全局变量定义
double CartPos=0;//小车位置，单位:m
double RodAng=0;//摆杆角度，单位:rad
double CartPos_Des=0;//小车期望位置
double RodAng_Des=0;//摆杆期望角度
double Output_Pos=0;//计算位置环输出的控制量
double Output_Ang=0;//计算角度环输出的控制量
//自起摆参数
double Kz=1;//控制量微调
//系统宏定义
#define INTPERIOD 0.01 //控制周期，单位：s
//角度转换，重新定义系统零点，零点由摆杆竖直向下变为竖直向上
if(RodAng>0)
RodAng = RodAng - PI;
else
RodAng = RodAng + PI;
//角度转换完成这其中大部分是一些变量的定义，在源码之中给位置环定义为CartPos,给角度环定义为RodAng，我们在之后会直接将位置环定义为position，角度环定义为angle。下面请试着回忆前面位置型PID的公式，来确认我们需要些什么变量，什么？公式忘了？忘了还不快滑上去看看？
首先我们需要PID的三个基本参数，即那三个增益系数。
源码中只给了P和D增益系数的定义，明显是想使用PD控制，变量命名分别为KP、KD。
接着是误差，我们用后缀err表示，位置环和角度环误差的求法由于在程序中进行了角度转换而变得不同。
先说位置环，当小车位于正中心处时，其坐标为0cm，它可以左右移动各15cm，其坐标便是从-15cm~+15cm,而期望值是0，所以说小车的误差其实就等于它的实际位置。
再说角度环，其倒立摆直立时其角度为0°，所以当实际角度＞0°时，由于角度转换将使其＜0°，反之则使其＞0°。所以虽然其误差也等于它的实际位置，但是由于角度转换，我们需要添加负号。
故err的求法如下：
position_err = CartPos_Des + CartPos;//期望位置加上实际位置
angle_err = RodAng_Des - RodAng;//期望角度减去实际角度还需要给上一环节的误差进行赋值，即表示
，在这里使用last进行表示，我考虑的是使用函数进行程序编写，因此在函数的结尾处直接将赋值给last，利用静态变量传递参数即可。
微分项的计算结果也需要一个变量进行表示，在这里表示为differential。
这里以位置环为例，角度环原理相同。
position_differential=(position_err - position_last)/INTPERIOD; 最后需要一个变量存储总的结果，在这里命名为PWM，因为直流电机的控制采用PWM，其原理在这里不做更多的解释。
这里以位置环为例，角度环原理相同。
position_PWM = position_err * KP_Pos + position_differential * KD_Pos;到这里，前期工作已经全部完成，组装成源码只是分分钟的事情。
五、程序源代码
之前的代码段也做了足够的解释，这里就不再写注释了（懒）。
double position(double CartPos)
{
double position_err=0, position_PWM=0;
static double position_last=0, position_differential=0;
position_err = CartPos_Des + CartPos;
position_differential = (position_err - position_last)/INTPERIOD;
position_last = position_err;
position_PWM = position_err * KP_Pos + position_differential * KD_Pos;
return position_PWM;
}
double angle(double RodAng)
{
double angle_err=0, angle_PWM=0;
static double angle_last=0, angle_differential=0;
angle_err = RodAng_Des - RodAng;
angle_differential = (angle_err - angle_last)/INTPERIOD;
angle_last = angle_err;
angle_PWM = angle_err * KP_Ang + angle_differential * KD_Ang;
return angle_PWM;
}
//最后在对应的电机输出部分，根据并级PID计算方式，求和后直接输出就为PWM
Output_Pos = position(CartPos);
Output_Ang = angle(RodAng);
Output=Kz*(Output_Pos+Output_Ang);有些人或许会问，为什么differential也要用静态变量呢？其实我只是觉得这么好看而言，上下对称，赏心悦目。
其实这是个非常简单的程序，完全可以不用函数，反而显得复杂了，但没办法，我总是痴迷于函数，能装成函数的我一定用函数写，就为了好看以及为了在虚无的未来里调用它，至少在C语言是这样（面向过程）。
其实还写了个不是函数版本的变量命名规则请参照上文。
这段代码没有扔倒立摆里跑过，不过我相信逻辑肯定没问题。
position_err = CartPos_Des + CartPos;
angel_err = RodAng_Des - RodAng;
position_differential = (position_err - position_last) / INTPERIOD;
angel_differential = (angel_err - angel_last) / INTPERIOD;
position_last = position_err;
angel_last = angel_err;
OutPut_Pos = KP_Pos * position_err + KD_Pos * posirion_differential;
OutPut_Ang = KP_Ang * angel_err + KD_Ang * angle_differential;基本的PID功能便已经实现了，里面的涉及的知识点基本也掰开解释过一次，相信能够全部理解的话，即使你没学过自动控制原理也可以编写出最基本的PID程序。
码字不易，都看到这里了，顺便点个赞呗！