## **5. PID控制方案** 大名鼎鼎的PID算法来了。原理网上一大堆，在下只做个简单的介绍。 核心公式是肯定要有的：  其中，U是输出，err是误差，kp是比例系数，TI是积分系数，TD是微分系数。<br> 本质上还是 输出 = 误差 为了降低震荡，引入了微分项 为了降低动态误差，引入了积分项<br> <br> ## 微分项对震荡的作用 输出 = 误差 + 微分项 微分项其实就是误差变化率的数学描述。 如果误差在增加，误差的变化率就是+值，误差在降低，误差的变化率就是-值。 考虑微分项的情况下，运动如下图：  红线是 输出 = 误差 算法，蓝线是引入微分项的算法。 在第一次抵达目标之前，由于误差值在不断减小，微分项是-值，它的存在让输出总体上也不断减小，从而更慢抵达目标。 在第一次越过目标后，由于误差在逐渐增加，微分项是+值，它起到了增大输出的作用，从而让飞船更快的在错误的方向上减速。 综合下来，微分项起到了消除震荡的作用。 但它也有缺点，如果输出的最大范围很小（也就是推力很小，飞船质量很大的情况下），就算采用 输出 = 误差 的算法也不会产生震荡，而在这种情况下，引入微分项会让飞船花费更长时间来抵达目标点。 > 说来也有趣，微分项在我们前面提到的运动问题场景下，刚好代表位移对时间的微分，即速度。考虑速度当然比不考虑速度更有效，如果能考虑加速度，就很接近精确求解了。 <br> ## 积分项对震荡的作用 输出 = 误差 + 积分项 积分项其实是对一定时间内误差的累加。 试想在这样一个特殊情况下，自己飞船与目标飞船的距离是10，目标飞船在以10的速度加速前进，此时如果用普通算法（输出 = 误差 ），你会发现刚好自己飞船的加速度也是10，两者加速度相同，永远无法接近或远离，就产生了一个无法消除的动态误差。 定性地分析：引入积分项后，输出 = 10+积分项，由于之前误差一直是正数，积分项一定是一个正数，此时输出绝对会超过10，从而进一步增加自己飞船的速度，接近目标。  红线是目标轨迹，绿线是 输出 = 误差 算法，蓝线是引入积分项的算法 可以看到积分项总能抑制动态误差。 <br> ## 比例系数 比例系数实际上是对算法输出的一个约束而已。 例如误差的取值范围是[-1,1]，我们在取时间周期30的情况下，误差项 + 积分项 + 微分项的取值范围在[-32, 32]。 如果此时你控制的是推进器，而它接受的输出范围是[-10,10]，则比例系数P可以取 10/32。这时候你可以准确的知道输出范围会在[-10,10]之间。 <br> ## 离散化 积分和微分都是连续的，而实际上在用的时候，编程块中的时间不是连续的。 就算以最高频率运行，每秒60次，我们只能获得60个误差，它在时间上本身就是离散的。 我们把PID公式离散化，实际上更简单。  公式中的Kp是比例系数，e(k)是误差，T是离散的时间间隔，Ti是积分系数，Td是微分系数。 要注意，公式中的积分系数是倒数的形式<br> **由于在SE中，物理帧是60帧/秒，我们采用的编程块运算频率也设为60次/秒，则时间间隔T可以取1** 下面直接上代码 ``` void Main(){ double d = ...; //获取误差 double Result = getPID(d); } const int T = 30; //周期，这个周期不是公式中的时间间隔T，而是储存累加结果的周期 const double P = 1; //比例系数 const double I = 10; //积分系数 const double D = 10; //微分系数 List<double> d_data = new List<double>(); //储存误差的数组 double getPID(double d){ d_data.Add(d); if(d_data.Count > T){ d_data.Remove(d_arr[0]); } double sum_d = 0; foreach(double i in d_data){ sum_d += i; } return P * (d + sum_d/I + (d_data[d_data.Count - 1] - d_data[d_data.Count-2])*D); //输出结果 } ``` 没错，这样就成了。在不同的设备上，相关的系数可以自己调整。