控制算法——PID算法及其在乐高EV3小车巡线中的应用

Andy Shen2020/5/15大约 10 分钟

零、作者注

本文成稿于 2020 年 5 月 15 日，当时作者正在读初一，以下内容当中部分措辞不甚恰当的以及不好理解的还请谅解。本文原稿是 doc，md 格式的内容可能会出现部分偏差，并且部分图片暂时没有。本文将在未来某个时候进行重构。

一、引言

本文只讲 PID 算法以及在巡线当中的应用，其中一切符号不做阐述，请自学高中数学或向老师（数学科学都行）提问。
AndyShen2006
2020 年 5 月 15 日

二、PID 算法

一、P 算法

P，是由英文当中的 proportion 的开头字母定义的，意义是比例。比例算法，是 PID 巡线当中较为常用的算法。在 PID 算法当中的表达是 kpe(t),意思就是像本身函数的反方向提供一个修正值，使得本身算法能够保证相对平衡。举个例子，我们画一条正弦函数 f(x)=sin(x)：

假如说这条曲线在一个区间[0,π]之间每次都会变成 1.2 倍，如图：

那么这个函数每次都和预期的函数差 0.2sinx，所以 P 算法得到的结果就是 0.2sinx。会每次在[0,π]区间释放一个-0.2sinx 的修正值，使得最后的结果基本没有偏差。
那么那个 kp 是干什么用的呢。原因主要是这个公式只适用于该方程，可实际应用中的修正不一定确定，所以 kp 可以辅助该算法每次只修正一部分偏差，剩下一部分不影响结果偏差，使得整个算法能够在合适的时候起到合适的作用。

二、I 算法

I 算法名字来源于积分英文 Integrate，公式为，Ki*
就是将累计的误差值加起来，然后乘上一个 ki 后，统一修正。由于 I 算法统计的误差值过小，所以一般不会使用。

三、D 算法

D 算法名字来源于微分英文 Derivative。该算法采用了预先判断，如果这条曲线与之前的成某种趋势，则提前修正，公式为 Kd*[e(k)-e(k-1)]。
我们仍然拿上面的 sinx 举例。我们先画一个 sinx 图像：

假设该函数在[0,π]区间内都为 1.2sinx，则图像如下：

这是，P 算法会对这个进行-0.2sinx0 的修正，由于前一次的修正值为 0（开始是理想的）（e(k-1)），D 算法将这个-0.2sinx0（e(k)）与前一次的修正值作差（e(k)-e(k-1)），然后乘上一个 Kd 保存为 oldKd(e(k)-e(k-1))，接下来下一次 P 会继续修正-0.2sinx1,D 算法就会增加修正 Kd sinx0,如此补充修正过后，后来的 P 修正就会越来越少，同样的 D 修正也会越来越少。最后曲线趋于平滑。

三、PID 算法在巡线当中的使用

一、原始判断巡线

此处将会用类编程语言的数学方式对此进行解释。
判断寻线，最简单的寻线方式，逻辑也很简单：车子靠右了就向左开，车子靠左了就向右开。我们可以将这个表达为如下式子：
If(L) turn right
If(R) turn left
Else go stratight

二、单光感 P 算法巡线

我们先回顾一下 P 算法，P 算法靠的是正确和不正确之间的差来进行修正。我们如果把黑线视作一个长方形，接着可以发现，在单光感 P 巡线的时候，那么应该光感一半在黑线上，一半在白色区域，中心正好是黑白边界，则理论上示数应该是 50（具体情况具体判断，可能会有一定误差），我们把小车放在左边界上，如果小车太靠左了，那么光感区域会有更多的在黑线上，值更大；太靠右了，光感区域会有更多的在空白处，值更小。我们就将其与 50 作差，然后修正按正负方向修正（一般左负右正），小的为负向左偏，大的为正向右偏。那么我们就可以设光感值为 n，然后得出如下式子 Turn(Kp(n-50))(Turn 为转向，方向同上，Kp 属于比例常数，需要根据实际需求调整)

三、双光感 P 算法巡线

我们回顾一下单光感 P 巡线，我们会发现一个问题，我们一定要把小车放在边界上，而且有的时候，我们的修正值可能不尽满意，如果该弯太弯了，那么修正不够，然后就直接整懵掉了。
所以我们就要引入一个新的巡线方式：双光感 P 巡线
双光感，就是一左一右，理论上应该两个正好夹着线，返回光值应该都是 100（理想状态，实际要考虑误差）。那么我们用单光感的思路去思考，如果左边的光感值变小了，则说明机器太靠右偏了，应该向左修正；反过来也一样，如果右边的光感值变小了，说明机器太靠左偏了，应该向右修正（这个和判断寻线有异曲同工之妙）。那么 P 巡线之所以是 P 巡线，自然有他的道理。我们对此进行一重抽象：如果两边光感值不同了，那么就会有一个差，这就好比一瓶水，要是你给它翘起一角，则左右不一样高了，那么他会因为地心引力而恢复原来的位置。我们设左边光感值为 l，右边光感值为 r。如果两边平衡，那么理想状态应该是 l-r=0，如果车子靠左偏了，那么 r 就会变小,l 不变，从而打破平衡，那么 l-r≠0 我们就要给予一个 l-r（l-r>0，因为左负右正，所以会向右偏）的修正，反过来也是一样，车子靠右偏了，那么 l 就会变小，r 不变，那么我们仍然要给予一个 l-r（l-r<0，同上，会向左偏）的修正。综上所述，公式就是 Turn(Kp(l-r))

四、多光感 P 算法巡线

同上，多光感一般都是偶数个，除非是用于特殊目的的某些光感（判断是否抵达等）。那么我们可以想一件事情，是不是如果角度较大，那么靠外的光感才会检测到有值。如果只是一点点的偏差，那么都是内部的光感遇到。所以我们有一点，如果外部检测到了，那么修正时要修正的更多。所以我们假设左边由内到外的都是 lk 右边由内到外的都是 rk，共有 n 个,那么不难得出，公式是 Turn(Kp1(l1-r1)+Kp2(l2-r2)+Kp3(l3-r3)+...+Kpn(ln-rn))(Kp1<Kp2<Kp3<...<Kpn)（此处亦可写作求和符号形式，形式为 Σnk=1Kpk(lk-rk)）

五、PD 算法巡线

P 算法由于在绝大部分情况下，它的实用性已经足够强了，但是对于某些想要精益求精，力求稳定的组长来说，这还不够。PID 算法是一种先进的算法，同样的，我们用不到 I，只要用 PD 就足矣了。
PD，关键在于预先判断，俗语有言道：“先发制人，后发者制于人”，P 算法有的时候就太慢了，经常后发，导致有些急弯他就来不及修正，然后就飞线出局了。PD 巡线，就是先发制人。
D 巡线的要求预先判断，就是 Kd(e(t)-e(t-1))，我们把 e 视作一个函数/表格，但是我们不用那么多 e 的信息，所以我们只要一个 old 变量存放信息就够了。
我们将前一次的 P 算法算出来的信息先保存至 old 变量。然后我们每次修正的时候既要修正本身 P 算法的修正量，也要修正预先量，也就是 Kd(p-old)（p 为本次 p 算法修正量，Kd 为一个调整的变量，实测 3-4 左右最佳），然后我们把本次的总修正量算出来，就是 Turn(Kp(l-r)+Kd(p-old))

六、对 PD 算法进行加权*

Info：本章重在提出一种思路并给出试解决方案，在有充分的实验结果证明该章中加权的正确性外，不建议使用该方式作为正式使用。
我们应该都知道，图纸中的弯有很多类型，一种是呈圆形的圆弯（难度较低），一种是呈抛物线/椭圆部分的急弯（在圆锥曲线的理论当中，抛物线就是张得无限开的椭圆）。一般情况下，我们巡线比赛使用的大多都是以急弯为主的图纸，难度相对较高，因为急弯容易修正过多或过少导致最后飞线出局。
现在的 PD 巡线，这个 e(t)，也就是目标曲线的不同切线夹角，我们是认为是理想的一次函数，那么目标函数就是圆的标准方程。可是正如前面所说，急弯一般都不是圆而是抛物线，那么我们现在的 D 算法就会修正不足。
对此，我们可以对 PD 巡线进行加权。
抛物线的函数我们不考虑别的，就是 f(x)=ax2。那么他在 x0 点上的切线斜率就是 2ax0，从而我们可以得出切线方程就是 g(x)=2ax0x。
接下来，我们引入求直线夹角的公式：arctan((k2-k1)/(1-k1k2))(k1,k2 为斜率)，我们把 x0 和 x0+ε（ε 指极小量）代入原式（a 取 0），然后用 Mathematica 画出图像为：

通过这张图像我们可以知道，对于抛物线来说，x 越大，前后两条直线的夹角越小（事实也是这样，抛物线的曲线是下面平，上面陡）
对于此曲线有两种凑近方法，第一种是使用指数函数，思考难度低，易于操控；第二种是使用 EV3 本身自带的 ATan 函数，但是思考难度就稍大；第三种就是通过抛物线去接近这个，因为对于抛物线 f(x)=-x2 来说，它的图像也接近于这个。
上述内容含有大量未实验的内容，思维深度极大，要求掌握高中数学知识，本章可以不读。