【代码收藏夹】Python实现PID算法

一、代码

使用方法

from pid import PID

# p：比例系数，i：微分系数，d：积分系数，imax：积分限幅
pid1 = PID(p=0.07, i=0.01, d=0.01, imax=90)
while(True):
    # error：误差值, 误差值 = 目标值 - 测量值
    error = 50 
    # optput：PID的输出
    output = pid1.get_pid(error)

1.1 Python3 版本

from math import pi, isnan
import time

def millis():
    return int(time.time() * 1000)
 
class PID:
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 1/(2 * pi * 20)
    def __init__(self, p=0, i=0, d=0, imax=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._imax = abs(imax)
        self._last_derivative = float('nan')
 
    def get_pid(self, error, scaler=1):
        tnow = millis()
        dt = tnow - self._last_t
        output = 0
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I()
        self._last_t = tnow
        delta_time = float(dt) / float(1000)
        output += error * self._kp
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):
                derivative = 0
                self._last_derivative = 0
            else:
                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 abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time
            if self._integrator < -self._imax: self._integrator = -self._imax
            elif self._integrator > self._imax: self._integrator = self._imax
            output += self._integrator
        return output

    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float('nan')

1.2 MicroPython - OpenMV版本

from pyb import millis
from math import pi, isnan

class PID:
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 1/(2 * pi * 20)
    def __init__(self, p=0, i=0, d=0, imax=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._imax = abs(imax)
        self._last_derivative = float('nan')
 
    def get_pid(self, error, scaler=1):
        tnow = millis()
        dt = tnow - self._last_t
        output = 0
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I()
        self._last_t = tnow
        delta_time = float(dt) / float(1000)
        output += error * self._kp
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):
                derivative = 0
                self._last_derivative = 0
            else:
                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 abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time
            if self._integrator < -self._imax: self._integrator = -self._imax
            elif self._integrator > self._imax: self._integrator = self._imax
            output += self._integrator
        return output
    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float('nan')

1.3 MicroPython - K210版本

from math import pi, isnan
import utime

def millis():
    return utime.ticks_ms()
 
class PID:
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 1/(2 * pi * 20)
    def __init__(self, p=0, i=0, d=0, imax=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._imax = abs(imax)
        self._last_derivative = float('nan')
 
    def get_pid(self, error, scaler=1):
        tnow = millis()
        dt = tnow - self._last_t
        output = 0
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I()
        self._last_t = tnow
        delta_time = float(dt) / float(1000)
        output += error * self._kp
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):
                derivative = 0
                self._last_derivative = 0
            else:
                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 abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time
            if self._integrator < -self._imax: self._integrator = -self._imax
            elif self._integrator > self._imax: self._integrator = self._imax
            output += self._integrator
        return output

    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float('nan')

二、说明

原代码来自 OpenMV 官方的 PID 代码。因为不同平台的 MicroPython 的 API 有差异，所以移植需要重写 millis 函数。

millis 函数用来获取系统开机后运行的时间长度，单位是毫秒。

三、参考资料

• openmv/pid.py · GitHub：https://github.com/openmv/openmv/blob/master/scripts/libraries/pid.py

---

致正在学习嵌入式的小伙伴们：
嵌入式的学习是要基础知识 + 动手实践同步进行的，在这里给大家推荐一个可以系统学习嵌入式和刷题的网站：牛客网（传送门：牛客网）。

牛客网原本是一个刷 IT 题库的网站，经过多年的发展，已经成为了一个集 课程+刷题+面经+求职+讨论区分享 的一站式求职学习网站，最最最重要的里面的资源全部免费， 而最近他们正打算将这套模式推广到嵌入式行业，专门为嵌入式工程师开设了相应的板块，正在学习嵌入式、寻找嵌入式专项题库、想要找嵌入式相关工作但又缺乏经验的小伙伴们，千万不要错过（链接在下面哦）！

• 牛客网 - 嵌入式专项题库
• 牛客网 - 嵌入式学习专区