PID控制

PID解释：

位置式：

     可以看出，比例部分只与当前的偏差有关，而积分部分则是系统过去所有偏差的累积。位置式PI调节器的结构清晰，P和I两部分作用分明，参数调整简单明了。
但直观上看，要计算第拍的输出值，需要存储等每一拍的偏差，当很大时，则占用很大的内存空间，并且需要花费很多时间去计算，这是目前书籍及网络上普遍认为的位置式PI的缺点。
然而在具体编程操作中，可在每一拍对积分部分进行累积，再加上当前拍的比例部分，即为当前的输出，根本不需要大量的内存空间；另外由于输出有可能超过允许值，因此需要对输出进行限幅，而当输出限幅的时候，积分累加部分也应同时进行限幅，以防输出不变而积分项继续累加，也即所谓的积分饱和过深。

增量式：

    

上式仅仅为增量，只需要当前的和上一拍的偏差即可得出结果，不需要存储每一拍的偏差，因此占内存空间小，这也是普遍认为的增量式的优点。
然而很多场合下需要的往往不只增量，还有上一拍的输出值，于是可知增量式PI调节器算法为

由于，在具体编程操作中，对每一拍的进行累积，即为PI调节器的输出；同样地，为了避免超过允许值，仅需对输出限幅即可。

PID一般有两种：位置式PID和增量式PID。在小车里一般用增量式，为什么呢？位置式PID的输出与过去的所有状态有关，计算时要对e（每一次的控制误差）进行累加，这个计算量非常大，而明没有必要。而且小车的PID控制器的输出并不是绝对数值，而是一个△，代表增多少，减多少。换句话说，通过增量PID算法，每次输出是PWM要增加多少或者减小多少，而不是PWM的实际值。

 

程序

//计算PI控制器的输出，并将结果显示到窗口
float errorheight = currentheight - beforeheight;
detaheight = KP * (errorheight - beforeheight) + KI * errorheight; //PI 控制器计算高度变化值
//对控制器输出做出必要的防错
if ( detaheight <= 0 )
{
detaheight = 1;
}

//更新PI控制上次误差
beforeheight = errorheight;

pd

while ( (abs(theta1) > epson) || (abs(theta2) > epson) )
{
//利用雅克比矩阵求得电机的运动
gemm( Jaco_rot.inv(cv::DECOMP_SVD), ImageCharacterVar, 1, NULL, 0, MotorPoseVar, 0);

//采用PD控制，控制电机的运动
float deltaThetax(0), deltaThetay(0);
deltaThetax = kpX * MotorPoseVar.at<float>(0,0) + kdX *(MotorPoseVar.at<float>(0,0)- ErrorOrigin.at<float>(0,0) );
deltaThetay = kpY * MotorPoseVar.at<float>(1,0) + kdY *(MotorPoseVar.at<float>(1,0)- ErrorOrigin.at<float>(1,0) );

//电机运动
if ( deltaThetay < 0 )
SurugaController5.RelativeMove(1, 1, int(deltaThetay));
else
SurugaController5.RelativeMove(1, 0, int(deltaThetay));
if ( deltaThetax < 0 )
SurugaController5.RelativeMove(2, 0, int(deltaThetax));
else
SurugaController5.RelativeMove(2, 1, int(deltaThetax));

ErrorOrigin = MotorPoseVar;

上述为典型的增量式

J*△μi=ei=△xi

△x2i=   ui=   kp*(ei-ei-1)+ki*ei

输出为增量，适合电机