OpenMV与PID控制

文章目录

  • PID导论
    • 理解PID算法思想
      • 例子
    • Kp ——>P比例控制算法Proportion)
    • Kd ——>D微分控制算法Derivative)
    • Ki——>I积分控制算法Integral)
      • 总结理解
        • 1）比例控制算法
        • 2）积分控制算法
        • 3）微分控制算法
      • 看一段伪代码加深下理解
    • PID 算法公式拆解
    • PID公式离散化处理
    • PID编程

PID导论

PID，就是“比例P（proportional）、I积分（integral）、D微分（derivative）”，是一种很常见的控制算法。在某些需要将某一个物理量“保持稳定”的场合（比如维持平衡，稳定温度、转速等），PID都会派上大用场。

PID 算法因为其实现简单且效果拔群的特性，而被广泛使用，比如温度控制元件、无人机飞行姿势、机器人控制等。

在控制系统理论体系中，会根据系统是否有反馈将系统分为开环系统与闭环系统，而 PID 算法是闭环系统中常用的算法，为了方便理解，这里举个具体的例子来解释开环系统与闭环系统：

我开发了一个机器人，在一开始，机器人身上没有安装任何传感器，此时你给机器人一个目标，让它跑到距离它当前位置 10 米远的前方，因为机器人没有环境传感器，即缺乏反馈信息，它在前进时，很可能就会偏离当前目标，如图 1 所示：
图1

将开环系统的整体结构抽象一下，如图 2 所示：

图2
结合机器人的例子，其中：

  • Input：机器人接收到一个指令，该指令让机器人前进 10 米
  • Controller：控制器通过计算得出要前进的方向与距离
  • Process：机器人执行控制器的计算结果，前进相应的距离

从图 2 可以看出，整个系统是没有反馈的，此时机器人在前进的过程中，很可能就出现图 1 的情况。

我们为机器人添加环境传感器，它现在可以判断自己与目标的方向与距离的，此时，我们将开环系统转变成了闭环系统，其抽象结构为：
图3
在闭环系统中，机器人的例子是这样的：

  • Input：机器人接收到一个指令，该指令让机器人前进 10 米
  • Controller：控制器通过计算得出要前进的方向与距离
  • Process：机器人执行控制器的计算结果，前进相应的距离
  • Feedback：传感器收集环境信息，比如当前是否偏离了目标，如果偏离了，就将偏离的距离作为 error ，然后再作为 Controller 输入的一部分

在闭环系统下，机器人可以比较好的到达目标位置了，但如果要求机器人尽可能快的到达目标位置呢？

尽可能快背后的要求是，完成任务的时间尽可能短且要准确的到达目的地，不能有偏差，此时我们可以利用 PID 算法去实现这个目标。在闭环系统中，主要就是利用 PID 算法来实现了 Controller 这块，如图 4 所示：
图4

理解PID算法思想

例子

比如，我想控制一个“热得快”，让一锅水的温度保持在50℃

这么简单的任务，为啥要用到微积分的理论呢？你一定在想：

这不是so easy嘛~ 小于50度就让它加热，大于50度就断电，不就行了？几行代码用Arduino分分钟写出来

没错~在要求不高的情况下，确实可以这么干！But！如果换一种说法，你就知道问题出在哪里了：

如果我的控制对象是一辆汽车呢？

要是希望汽车的车速保持在50km/h不动，你还敢这样干么

设想一下，假如汽车的定速巡航电脑在某一时间测到车速是45km/h。它立刻命令发动机：加速！
结果，发动机那边突然来了个100%全油门，嗡的一下，汽车急加速到了60km/h。这时电脑又发出命令：刹车！

结果，吱…哇…乘客吐)

所以，在大多数场合中，用“开关量”来控制一个物理量，就显得比较简单粗暴了。有时候，是无法保持稳定的。因为单片机、传感器不是无限快的，采集、控制需要时间。而且，控制对象具有惯性。比如你将一个加热器拔掉，它的“余热”（即热惯性）可能还会使水温继续升高一小会。

这时，就需要一种『算法』：

  • 它可以将需要控制的物理量带到目标附近
  • 它可以“预见”这个量的变化趋势
  • 它也可以消除因为散热、阻力等因素造成的静态误差

于是，当时的数学家们发明了这一历久不衰的算法——这就是PID控制。

你应该已经知道了，P，I，D是三种不同的调节作用，既可以单独使用（P，I，D），也可以两个两个用（PI，PD），也可以三个一起用（PID）。

这三种作用有什么区别呢？客官别急，听我慢慢道来

在这里插入图片描述
我们先只说PID控制器的三个最基本的参数：Kp、Ki、Kd。

Kp ——>P比例控制算法Proportion)

P就是比例的意思。它的作用最明显，原理也最简单。我们先说这个：

需要控制的量，比如水温，有它现在的『当前值』，也有我们期望的『目标值』。

  • 当两者差距不大时，就让加热器“轻轻地”加热一下。
  • 要是因为某些原因，温度降低了很多，就让加热器“稍稍用力”加热一下。
  • 要是当前温度比目标温度低得多，就让加热器“开足马力”加热，尽快让水温到达目标附近。

这就是P的作用，跟开关控制方法相比，是不是“温文尔雅”了很多

实际写程序时，就让偏差（目标减去当前）与调节装置的“调节力度”，建立一个一次函数的关系，就可以实现最基本的“比例”控制了~

如控制无人机的升力F = Kp × error = Kp × 目标值 - 当前值)表示了升力F和误差error之间的比例关系

Kp越大，响应速度越快，调节作用越激进，到达平衡位置产生的震荡也越大，Kp调小会让调节作用更保守。

要是你正在制作一个平衡车，有了P的作用，你会发现，平衡车在平衡角度附近来回“狂抖”，比较难稳住。
如果已经到了这一步——恭喜你！离成功只差一小步了~

Kd ——>D微分控制算法Derivative)

D的作用更好理解一些，所以先说说D，最后说I。

刚才我们有了P的作用。你不难发现，只有P好像不能让平衡车站起来，水温也控制得晃晃悠悠，好像整个系统不是特别稳定，总是在“抖动”。

在这里插入图片描述

你心里设想一个弹簧：现在在平衡位置上。拉它一下，然后松手。这时它会震荡起来。因为阻力很小，它可能会震荡很长时间，才会重新停在平衡位置。

请想象一下：要是把上图所示的系统浸没在水里，同样拉它一下 ：这种情况下，重新停在平衡位置的时间就短得多。

我们需要一个控制作用，让被控制的物理量的“变化速度”趋于0，即类似于“阻尼”的作用。

因为，当比较接近目标时，P的控制作用就比较小了。越接近目标，P的作用越温柔。有很多内在的或者外部的因素，使控制量发生小范围的摆动。D的作用就是让物理量的速度趋于0

只要什么时候，这个量具有了速度，D就向相反的方向用力，尽力刹住这个变化。

如控制无人机的速度：derror) / dt = d目标位置 - 当前位置) / dt = 速度

即D算法能够对无人机的速度做出响应 ，当无人机速度过快时，D算法会抵消掉一部分由P算法计算出来的升力使无人机不会冲出平衡位置太远)，从而减缓系统的震荡。

kD参数越大，向速度相反方向刹车的力道就越强。

Ki——>I积分控制算法Integral)

还是以热水为例。假如有个人把我们的加热装置带到了非常冷的地方，开始烧水了。需要烧到50℃。

在P的作用下，水温慢慢升高。直到升高到45℃时，他发现了一个不好的事情：天气太冷，水散热的速度，和P控制的加热的速度相等了。
这可怎么办？

  • P兄这样想：我和目标已经很近了，只需要轻轻加热就可以了。
  • D兄这样想：加热和散热相等，温度没有波动，我好像不用调整什么。

于是，水温永远地停留在45℃，永远到不了50℃。

作为一个人，根据常识，我们知道，应该进一步增加加热的功率。可是增加多少该如何计算呢？

前辈科学家们想到的方法是真的巧妙：

设置一个积分量。只要偏差存在，就不断地对偏差进行积分（累加），并反应在调节力度上。

这样一来，即使45℃和50℃相差不太大，但是随着时间的推移，只要没达到目标温度，这个积分量就不断增加。系统就会慢慢意识到：还没有到达目标温度，该增加功率啦！

到了目标温度后，假设温度没有波动，积分值就不会再变动。这时，加热功率仍然等于散热功率。但是，温度是稳稳的50℃。

再以无人机举例：
只要误差error存在，那么无人机的升力也一定存在，但是在无人机接近目标高度时，这个升力最终会与重力平衡，此时无人机就不会继续上升了——但我们还没有达到目标高度啊！

为了解决这个问题：我们需要累积这个误差值error（目标位置 – 当前位置） ,当误差长时间不变（即长时间得不到修正），我们就需要给无人机提供更大的升力，迫使无人机上升

kI的值越大，积分时乘的系数就越大，积分效果越明显。

所以，I的作用就是，减小静态情况下的误差，让受控物理量尽可能接近目标值。

I在使用时还有个问题：需要设定积分限制。防止在刚开始加热时，就把积分量积得太大，难以控制

总结理解

总的来说，当得到系统的输出后，将输出经过比例，积分，微分3种运算方式，叠加到输入中，从而控制系统的行为，下面用几个简单的实例来说明。

1）比例控制算法

假设我有一个水缸，最终的控制目的是要保证水缸里的水位永远的维持在1米的高度。假设初试时刻，水缸里的水位是0.2米，那么当前时刻的水位和目标水位之间是存在一个误差的error，且error为0.8.这个时候，假设旁边站着一个人，这个人通过往缸里加水的方式来控制水位。如果单纯的用比例控制算法，就是指加入的水量u和误差error是成正比的。即

u=kp*error

假设kp取0.5，
那么t=1时（表示第1次加水，也就是第一次对系统施加控制），那么u=0.5*0.8=0.4，所以这一次加入的水量会使水位在0.2的基础上上升0.4，达到0.6.
接着，t=2时刻（第2次施加控制），当前水位是0.6，所以error是0.4。u=0.5*0.4=0.2，会使水位再次上升0.2，达到0.8.
如此这么循环下去，就是比例控制算法的运行方法。
可以看到，最终水位会达到我们需要的1米。
但是，单单的比例控制存在着一些不足，其中一点就是 –稳态误差！（我也是看了很多，并且想了好久才想通什么是稳态误差以及为什么有稳态误差）。

像上述的例子，根据kp取值不同，系统最后都会达到1米，不会有稳态误差。但是，考虑另外一种情况，假设这个水缸在加水的过程中，存在漏水的情况：
假设每次加水的过程，都会漏掉0.1米高度的水。仍然假设kp取0.5，那么会存在着某种情况，假设经过几次加水，水缸中的水位到0.8时，水位将不会再变换！！！因为，水位为0.8，则误差error=0.2. 所以每次往水缸中加水的量为u=0.5*0.2=0.1.同时，每次加水缸里又会流出去0.1米的水！！！加入的水和流出的水相抵消，水位将不再变化！！
也就是说，我的目标是1米，但是最后系统达到0.8米的水位就不在变化了，且系统已经达到稳定。由此产生的误差就是稳态误差了。
（在实际情况中，这种类似水缸漏水的情况往往更加常见，比如控制汽车运动，摩擦阻力就相当于是“漏水”，控制机械臂、无人机的飞行，各类阻力和消耗都可以理解为本例中的“漏水”）
所以，单独的比例控制，在很多时候并不能满足要求。

2）积分控制算法

还是用上面水缸加水的例子，如果仅仅用比例，可以发现存在暂态误差，最后的水位就卡在0.8了。于是，在控制中，我们再引入一个分量，该分量和误差的积分是正比关系。所以，比例+积分控制算法为：

u=kp*error+ ki∗∫error

还是用上面的例子来说明，第一次的误差error是0.8，第二次的误差是0.4，至此，误差的积分（离散情况下积分其实就是做累加） 为∫error=0.8+0.4=1.2. 这个时候的控制量，除了比例的那一部分，还有一部分就是一个系数ki乘以这个积分项。由于这个积分项会将前面若干次的误差进行累计，所以可以很好的消除稳态误差（假设在仅有比例项的情况下，系统卡在稳态误差了，即上例中的0.8，由于加入了积分项的存在，会让输入增大，从而使得水缸的水位可以大于0.8，渐渐到达目标的1.0.达到目标高度后，假设没有波动，仍满足加水量=出水量，但此时的高度已经是目标1米了）这就是积分项的作用。

3）微分控制算法

换一个另外的例子，考虑刹车情况。平稳的驾驶车辆，当发现前面有红灯时，为了使得行车平稳，基本上提前几十米就放松油门并踩刹车了。当车辆离停车线非常近的时候，则使劲踩刹车，使车辆停下来。整个过程可以看做一个加入微分的控制策略。
微分，说白了在离散情况下，就是error的差值，就是t时刻和t-1时刻error的差
即u=kd*（error（t）-error（t-1）），其中的kd是一个系数项。可以看到，在刹车过程中，因为error是越来越小的（离目标速度越来越近，因此误差越来越小），所以这个微分控制项一定是负数，在控制中加入一个负数项，他存在的作用就是为了防止汽车由于刹车不及时而闯过了线。
从常识上可以理解，越是靠近停车线，越是应该注意踩刹车，不能让车过线，所以这个微分项的作用，就可以理解为刹车，当车离停车线很近并且车速还很快时，这个微分项的绝对值（实际上是一个负数）就会很大，从而表示应该用力踩刹车才能让车停下来。
切换到上面给水缸加水的例子，就是当发现水缸里的水快要接近1的时候，加入微分项，可以防止给水缸里的水加到超过1米的高度，说白了就是减少控制过程中的震荡。
在这里插入图片描述

看一段伪代码加深下理解

previous_error = 0
integral = 0while1):# PID会使用到一个循环，循环会不停地根据“误差”来调整输出信号error = set_value - measured_value #第一步计算误差值：设定的目标值 - 当前的测量值integral = integral + error * dt # 计算积分的部分，这行代码会不停地将当前的误差累加起来，并保存在循环外的变量integral里derivative =  error - previous_error ) / dt # 计算微分部分output =  Kp * error ) +  Ki * integral ) +  Kd * derivatve ) # 最后我们将比例、积分、微分加权求和，权重就是Kp、Ki、Kd参数 previous_error = error # 更新误差waitdt)# 等待t秒进入下一次循环goto loop

PID 算法公式拆解

PID调节是有方法、有规律可循的，不过在此之前先深入理解其公式。
别怕，先看认真看PID本体：

在这里插入图片描述
其中：

ut) ————-输出曲线，PID输出值随时间的变化曲线

Kp ————–比例系数

et)————- 偏差曲线，设定值与实际值的偏差error随时间的变化曲线

Ti————— 积分时间

Td————–微分时间

我们以文章一开始提及的，机器人移动到 10 米前的位置为例子

基于 P 算法，机器人从初始位置移动到目标位置时，其距离变化如图 5 所示：
图5
————————————————————–图5——————————————————————-

可以发现，机器人为了实现尽快到达目标的目的，在一开始距离比较远时，速度在快速加快，从图 5 来看，就是斜率较大的部分，此时机器人快速到达 10M 处，但是因为速度问题，冲过了 10 米，冲过后，基于 Feedback 的误差，机器人将会往回走，经过几次波动后，到达 10 米的目标位置。

我们将机器人移动过程中，误差变化所对应的函数error)=set – current 也绘制出来，如下图 6 所示：

图6
————————————————————–图6——————————————————————-

从图 6 可以看出，P 算法 会基于误差控制机器人移动速度，在第一个虚线处，error 较大且是正数，P 算法会让机器人快速移动，第二虚线处，error 是负数，P 算法会让机器人往回移动。

I 算法表示误差在一定时间内的积分

通过图画出来，积分就是error)函数曲线与 Time 轴之间的面积，如图 7 所示：

图7
————————————————————–图7——————————————————————-

I 算法通过积分算法，不断积累误差（面积一直在变大），最终乘以 Ki，便是 I 算法的部分。

D 算法（）是误差的微分处理，它表示 函数在某一点的斜率，如图 8 所示：

图8
————————————————————–图8——————————————————————-

从图 8 可知，当斜率变化过快时，D 算法便会获得较大的负数，以抑制输出的上升速度，从而避免机器人在到达目标时，多次震荡。

PID 算法中3 个不同部分相互配合，其直观形式如下 gif 图：

在这里插入图片描述

PID公式离散化处理

因为实际编程应用中，由于机器的采样周期不能无限小，因此我们用到的都是离散的数据，所以对PID进行编程之前我们得先把模拟公式转换成离散公式。

离散转换后：
在这里插入图片描述
其中：

ut) ————-输出曲线，PID输出值随时间的变化曲线

Kp ————–比例系数

et)————- 偏差曲线，设定值与实际值的偏差随时间的变化曲线

Ti————— 积分时间

Td————–微分时间

T—————-调节周期

转换后我们发现我们需要整定的三个参数变为：
在这里插入图片描述
多了个调节周期T，

因此需要注意的是：必须使T为定值，或者变化小到可以忽略
这样P、I、D才是固定常数，才可能调节

PID编程

直接copy了飞控的代码pid.py

# 计算PID输出的公式
# output = self._kd * derivative + self._integrator + self._kd * derivative
#from pyb import millis	# 返回代码执行到当前的时间
from math import pi, isnan	# pi-->Π，isnan-->用于检查给定数字是否为“ NaN” 不是数字)，它接受一个数字，如果给定数字为“ NaN” ，则返回True ，否则返回False 。class PID:# 成员变量的初始化_kp = _ki = _kd = _integrator = _imax = 0    #初始化三个系数，积分，imax限制i的最大值，即对PID的I进行积分限幅为0 ) _last_error = _last_derivative = _last_t = 0	   #  最新差值      最新导数	    上个循环的时间_RC = 1/2 * pi * 20)# 在类中：通俗地说，_init_方法适合给对象输入/赋值。普通的方法适合运算/输出# 对类中的属性成员变量)进行定义def __init__self, p=0, i=0, d=0, imax=0):# 相当于C中的构造函数，如果不传入值，则按照默认值进行构造# _init_)函数的第一个形参必须是selfself._kp = floatp)self._ki = floati)		# 强制类型转换self._kd = floatd)self._imax = absimax)	# abs为取绝对值函数-->对imax取绝对值self._last_derivative = float'nan')	# 设置微分为nan# 为外部访问pid的值提供接口def get_pidself, error, scaler):
#根据差值error、Kp、Ki、Kd获取pidtnow = millis)# tnow表示现在的时间# millis函数可以用来获取Arduino开机后运行的时间长度，该时间长度单位是毫秒# 最长可记录接近50天左右的时间。 如果超出记录时间上限，记录将从0重新开始。# millis函数的返回值为 无符号长整型 数据, 如果将该数值与整型数据或其它数据类型进行运算，运行结果将产生错误。dt = tnow - self._last_t	# 和上次循环的时间差output = 0	# 总输出（你调节的量）if self._last_t == 0 or dt > 1000:# 如果是第一个循环（初始值为0）或时间差>1s（大于规定可微积分的阈值）dt = 0	# 时间差归零self.reset_I)# 重置积分self._last_t = tnow	# 记录结束时间delta_time = floatdt) / float1000)	#获取Δt——>Δt = dt / toutput += error * self._kp	# 加入比例控制	error为传入的参数，表示未加入控制时的初始误差if absself._kd) > 0 and dt > 0:	# 若微分参数和时间差>0if isnanself._last_derivative):	#若微分是NaN# isnanx)函数：若"x"非数字，则返回 True derivative = 0	# 微分值归0 self._last_derivative = 0	# 微分值为0
#PS:前面已经声明为nan，所以openmv没有用微分控制                else:	# 如果self._last_derivative有数值derivative = error - self._last_error) / delta_time	#微分为：（这次的差距-上次的差距）/时间差derivative = self._last_derivative + \ # "\"为续行符，表示下一行是上一行的延续delta_time / self._RC + delta_time)) * \derivative - self._last_derivative))self._last_error = error	# 更新差值self._last_derivative = derivative	# 更新微分值output += self._kd * derivative	# 加入微分控制output *= scaler	# 乘以总系数（不是指针！）if absself._ki) > 0 and dt > 0:# #若I参数>0：如果积分大于0->说明误差可能仍存在！self._integrator += error * self._ki) * scaler * delta_time#计算积分控制if self._integrator < -self._imax:self._integrator = -self._imaxelif self._integrator > self._imax:self._integrator = self._imaxoutput += self._integrator	# 加上积分算法的值return outputdef reset_Iself):	# 重置Iself._integrator = 0self._last_derivative = float'nan')

Python知识补充：
面向对象最重要的概念就是类（class）和实例（instance）

类是抽象的模板，比如学生这个抽象的事物，可以用一个Student类来表示。而实例是根据类创建出来的一个个具体的“对象”，每一个对象都从类中继承有相同的方法，但各自的数据可能不同。

1、以Student类为例，在Python中，定义类如下：

class Studentobject):pass

（Object）表示该类从哪个类继承下来的，Object类是所有类都会继承的类。

2、实例：定义好了类，就可以通过Student类创建出Student的实例，创建实例是通过**类名+)**实现

student = Student)

3、由于类起到模板的作用，因此，可以在创建实例的时候，把我们认为必须绑定的属性强制填写进去。这里就用到Python当中的一个内置方法__init__方法，例如在Student类时，把name、score等属性绑上去:

class Studentobject):def __init__self, name, score):self.name = nameself.score = score	#"self."就相当于C中的"my->"指针

这里注意：（1）__init__方法的第一参数永远是self，表示创建的类实例本身，因此，在__init__方法内部，就可以把各种属性绑定到self，因为self就指向创建的实例本身。（2）有了__init__方法，在创建实例的时候，就不能传入空的参数了，必须传入与__init__方法匹配的参数，但self不需要传，Python解释器会自己把实例变量传进去：

文章参考：PID算法原理 一图看懂PID的三个参数
文章参考：PID 控制算法原理与 Python 实现
文章参考：PID公式通俗理解
文章参考：一文读懂PID控制算法（抛弃公式，从原理上真正理解PID控制）
文章参考：openmv+PID算法详解
感谢几位大神的解读！让初入门OpenMV又想读懂代码的我大为受益！以后我也会潜心学习想更优秀的人靠近，并用自己的能力帮助更多的人🌹

Published by

风君子

独自遨游何稽首 揭天掀地慰生平

发表回复

您的电子邮箱地址不会被公开。 必填项已用 * 标注