移动小车中的简单PID控制

理论基础

主要是根据下面的位置式PID离散公式进行控制：通过对当前时刻的比例控制与当前时刻与上一时刻的微分控制进行平滑调整。

上式中，e(k)、e(k-1)为当前时刻和上一时刻偏差角，u(k)为输出角速度，由于位置式PID在积分项达到饱和时,误差仍然会在积分作用下继续累积，造成积分饱和问题，所以一般使用PD控制。

代码实现

以移动小车为例，假设当前帧求出的偏角为delta,上一帧的偏角为delta_last，输出角速度为voltage，那么PD控制可以写为：

float PD(float speed)
{    
     Setdelta=0; 
     delta=Setdelta-Actualdelta;
     voltage=Kp*deltal+Kd*(delta-delta_last); 
     delta_last = delta;
     Actualdelta=voltage*1.0; 
     return Actualdelta; 
}

然后不断调参来优化控制。