PID算法

#ifndefDISCRETE_PID_H_#defineDISCRETE_PID_H_#includeincludes.h/*DataStructure*/typedefstructpid_parameterpack{FP32fKp;FP32fKi;FP32fKd;}PID_PP;typedefstructpid_runtimepack{FP32fA;FP32fB;FP32fC;FP32fError[3];}PID_RP;typedefstructpid_module{PID_PPcPP;PID_RPcRP;}PID_MODULE;voidPID_Reset(PID_MODULE*pPID,FP32fTargetVal,FP32fCurrentVal);FP32PID_CalculateControlOutput(PID_MODULE*pPID,FP32fTargetVal,FP32fCurrentVal);#endif/*DISCRETE_PID_H_*/源文件：voidPID_Reset(PID_MODULE*pPID,FP32fTargetVal,FP32fCurrentVal){//calculatefactorspPID-cRP.fA=pPID-cPP.fKp+pPID-cPP.fKi+pPID-cPP.fKd;pPID-cRP.fB=pPID-cPP.fKp+2*pPID-cPP.fKd;pPID-cRP.fC=pPID-cPP.fKd;//set3errorstocurrenterrorpPID-cRP.fError[0]=fCurrentVal-fTargetVal;pPID-cRP.fError[1]=pPID-cRP.fError[0];pPID-cRP.fError[2]=pPID-cRP.fError[0];}FP32PID_CalculateControlOutput(PID_MODULE*pPID,FP32fTargetVal,FP32fCurrentVal){FP32fPIDVal;//updateerrors0-1-2pPID-cRP.fError[2]=pPID-cRP.fError[1];pPID-cRP.fError[1]=pPID-cRP.fError[0];pPID-cRP.fError[0]=fCurrentVal-fTargetVal;//calculatepidvaluefPIDVal=pPID-cRP.fError[0]*pPID-cRP.fA-pPID-cRP.fError[1]*pPID-cRP.fB+pPID-cRP.fError[2]*pPID-cRP.fC;return(fPIDVal);}小车巡线控制算法控制算法电机控制算法的作用是接受指令速度值，通过运算向电机提供适当的驱动电压，尽快地和尽快平稳地使电机转速达到指令速度值，并维持这个速度值。换言之，一旦电机转速达到了指令速度值，即使在各种不利因素(如斜坡、碰撞之类等使电机转速发生变化的因素)的干扰下也应该保持速度值不变。为了提高机器人小车控制系统的控制精度，选用合适的控制算法显得十分必要。控制算法是任何闭环系统控制方案的核心，然而并非越复杂、精度越高的算法越好，因为比赛要求非常高的实时性，机器人必须在非常短的时间内作出灵敏的反应，所以现代的一些先进控制算法，比如模糊控制、神经元网络控制等就不能应用到小车控制系统里。本系统选用了最常规、最经典的PID控制算法，通过实际应用取得了很好的效果。1比例项控制回路中的第一个偏差转换环节就是比例项。这一环节简单地将偏差信号乘以常数K得到新的CV值(值域为-100~100)。基本的比例控制算法如下：loop:PV=ReadMotorSpeed()Error=SP-PVCV=Error*KpropSetpwm(cv)Gotoloop上一段程序中的SetPWM()函数并非将CV值作为绝对的PWM占空比来对待。否则，不断降低的偏差值会使输出值接近零，而且由于电机工作时需要持续的PWM信号，控制系统将会使电机稳定在低速运转状态上，从而导致控制系统策略失败。相反，CV值一般被取作当前PWM占空比的改变量，并被附加到当前的PWM占空比上。这也要求SetPWM()函数必须将相加后得到的PWM占空比限制在0%~100%。正的CV值将使电机两端电压增加。负的CV值将使电机两端电压降低。如果CV值等于0，则无需改变但前占空比。较低的K值会使电机的速度响应缓慢，但是却很平稳。较高的K值会使速度响应更快，但是却可能导致超调，即达到稳定输出前在期望值附近振荡。过高的K值会导致系统的不稳定，即输出不断震荡且不会趋于期望值。2微分项任何变量的微分项被用来描述该变量是如何相对于另一个变量(多位时间)变化的。换句话说，任何变量的微分项就是它随时间的变化率。如位移随时间的变化率是速度。速度相对于时间的微分是加速度。在PID控制器中，值得关心的是偏差信号相对于时间的微分，或称变化率。绝大多数控制器将微分项定义为：Rate=(E-E)/T式中，E为当前偏差，E为前次偏差值，T为两次测量的时间间隔。负的变化率表明偏差信号的改善。当微分项被具体应用于控制器中时，将一个常数乘以该微分项，并将它加到比例项上，就可以得到最终的CV值计算公式：CV=(KE)+(KRate)当偏差信号接近零时，CV值将为负，所以当偏差信号开始改善时，微分项的作用将逐渐减弱校正输出量。在某些场合下，微分项还有利于超调量的消除，并可以允许使用较大的K值，从而可以改善响应的快速性。微分环节还预示了偏差信号的变化趋势。当控制对象对控制器的输出响应迟缓时，微分环节的作用尤为明显。含有微分项的控制算法的伪代码实现如下：loop:PV=ReadMotorSpeed()LastError=ErrorError=SP-PVRate=Error-LastErrorCV=Error*Kprop+Krate*RateSetPWM(CV)Gotoloop3积分项积分正好与微分相对。假如有一个描述变化率(微分)的表达式，那么对该表达式的积分就将得到随时间变化的原物理量。如加速度的积分是速度，速度的积分是位移。在PID控制回路中，偏差的积分代表从控制开始时算起所有偏差积累的总和。该总和被常数K所乘后再添加到回路输出中。在回路中，如果没有积分环节，尽管控制系统也会趋于稳定，但是由于某种原因输出值可能最终也无法达到SP值。一个简单但完全的PID控制器地伪代码实现如下：loop:PV=ReadMotorSpeed()LastError=ErrorIsum=Isum+ErrorError=SP-PVRate=Error-LastErrorCV=Error*Kprop+Krate*Rate+Kint*IsumSetPWM(CV)Gotoloop由于积分项会越来越大，这就会使控制回路在SP值的改变时响应变慢，某些应用场合在CV值达到取值边界(如为：-100~100)时会停止累加Isum。在SP值改变时，也可以除去Isum项。