PID算法

所谓PID就是比例-积分-微分的英文缩写，但并不是必须同时具备这三种算法，也可以是 PD, PI,甚至只有 P算法控制，下面分别介绍每个参数的含义：

首先需要明确一个事实就是，要实现PID算法，必须在硬件上具有闭环控制，就是得有反馈。比如控制一个电机的转速，就得有一个测量转速的传感器，并将结果反馈到控制器中，而在自平衡系统中，常用的有三个控制环 — 角度环、速度环、转向环

大家可以想象出每个闭环的反馈元件分别是什么吗，对就是上面元件清单里面包含的 IMU（陀螺仪+加速度计）、编码器、摄像头（或者其他可以确定方位的元件比如陀螺仪，磁场计等）

P（比例）：以小车巡线为例，现在需要让小车跟随一条轨迹前进，用PID算法控制方向环，反馈传感器就假设为摄像头。那么小车行进中有这么几种情况：
1.车通过摄像头发现自己处在轨迹的左边，位置误差值为正，那么就需要向右转向，转向值为正
2.车通过摄像头发现自己处在轨迹的右边，位置误差值为负，那么就需要向左转向，转向值为负
3.车通过摄像头发现自己处在轨迹的正中间，位置误差值为0，很欢快地笔直前行，转向值为0 

于是我们发现，小车转向值的输出可以简单地通过把位置误差乘以一个系数就得到了，而且显然，误差越大，得到的转向值也越大，符合需求。这里面这个系数，就是P了，而系数具体的大小，需要根据实际情况调试确定。
我们有了第一个公式：
1
P_term = kP * error

D（微分）：还是以小车巡线为例，依然是那车那线那比例。那么小车行进中有这么几种情况：
在P参数的作用下：
1.小车从左边向中间逐渐靠拢，终于它到达了中点……然而，由于惯性，它根本停不下来！于是小车又到了线的右边
2.小车从右边向中间逐渐靠拢，终于它到达了中点……然而，由于惯性，它根本停不下来！于是小车又到了线的左边
3. …

这跟说好的不一样！于是这个时候D出场了，想想我们期待的效果是啥，我们希望小车到达中点，此时不光位置误差为0，还要转向速度误差也为0。

那么我们设定期望的转向速度为0，此时如果小车转向速度向右的话误差为+，向左为-，再看前面的情况1，小车的转向速度误差为+，我们应该在P之外再给它一个向左的转向力，才能保证它在到达中点时速度不会那么快；情况2类似，此时需要向右的转向力

也就是说，D相当于给了小车一个转向的阻力，而这个力，又恰好可以通过简单地把转向速度误差乘以一个系数得到，显然，转向速度误差越大，得到的阻力越大，符合需求（值得注意的是这里的转向速度是相对中点的，并不是指小车输出的转向速度，可以理解为“位置变化的速度”）
我们有了第二个公式：
1
D_term = kD* (error- last_error)

如果上面的例子还是不好理解的话，考虑前面的单摆模型：
P相当于重力的作用，让摆左右往复运动，而D则相当于空气阻力，让摆慢慢停在中点。D的大小很理想的情况下，应该是大概摆动左右各一下之后就停在中点，想象把摆放在水中摆动的情况。

I（积分）：有的时候我们会发现，系统中存在一些固定的阻力，例如，我们用PID控制一个电机的转速，当给定的目标速度很小的时候，就会出现这样的情况：
根据P_term = kP * error，由于error很小，P的输出也很小，而由于摩擦力的存在，此时并不能让电机转动起来；又由D_term = kD* (error- last_error)，由于电机没有转动，显然(error- last_error)始终为0于是D输出也为0，那么问题来了，除非改变目标值，否则电机就永远转不起来了…
I的作用就是消除这样的静态误差，它会将每次的误差都积累起来，然后同样也是乘以一个系数之后作为输出。比如上面的情况，虽然误差很小，但却不是0，于是在每一轮的计算中，I项把error逐渐累积，直到超过临界值让电机转起来；而在误差为0的情况下，I项却又不会帮倒忙。
第三个公式：I_term = kI*（I_term + error）

以上就是PID的全部计算了，最后三者加起来就得到了：
1
PID_output = P_term + I_term + D_term

每隔一段固定时间把它运行一遍，就是PID算法了。