openmv的飞控pid模块分析

参考链接：OpenMV与PID控制
在原文的基础上，加了一些自己的理解

一.如何使用这个模块？

---

（1）模块实例化：

my_pid = PID(p=0.07, i=0, imax=90)//pi控制，积分限幅90

首先这个模块示例化的时候可以显式的传入四个参数
p，i,，d， imax，它们的默认值都是0，其中imax是最大积分限幅量，当积分积累的误差乘上scaler系数的之后（这个系数下文会介绍）
如果大于imax则直接将其赋值为imax，负数同理

---

（2）pid控制：

def get_pid(self, error, scaler)

参数一：略，实际调用也要省略（python语法，不需要过多解释…）
参数二：pid的误差输入
参数三：pid的输出结果往往会很大或者很小，这个时候我们可以让输出结果整体乘上一个系数，这个系数就是参数三（内部源码经乘法分配律转换后也是这么实现的）
返回值：返回本次pid计算结果

需要注意的是这个get_pid方法调用间隔必须小于1s（这已经很长了…）,再长的话，函数内部会把一个叫做dt的变量置0，这个操作将会导致积分项和微分无效（即积分项，微分项输出为0），导致只有比例项是有效的

另外，这个方法有三行的代码我不是很理解如下：

derivative = self._last_derivative + \ 
                                     ((delta_time / (self._RC + delta_time)) * \
                                        (derivative - self._last_derivative))

它的上一行是传统的微分pid输出量，输出结果为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_()函数的第一个形参必须是self
        self._kp = float(p)
        self._ki = float(i)		# 强制类型转换
        self._kd = float(d)
        
        self._imax = abs(imax)	# abs为取绝对值函数-->对imax取绝对值
        
        self._last_derivative = float('nan')	#第一次循环会判断如果为nan，就把self._last_derivative置0，此后它就一直用于指示微分量

	# 为外部访问pid的值提供接口
    def get_pid(self, error, scaler):
##############################进入函数之后首先记录一下当前时间和上一次时间，算出时间差###############################
#因为微分量和积分量的计算需要时间差

#根据差值error、Kp、Ki、Kd获取pid(output)
        tnow = 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 = float(dt) / float(1000)	#获取Δt——>Δt = dt / t

####################################################比例pid计算###################################################
        output += error * self._kp	# 加入比例控制	error为传入的参数，表示未加入控制时的初始误差
####################################################微分pid计算###################################################
        if abs(self._kd) > 0 and dt > 0:	# 若微分参数和时间差>0
            if isnan(self._last_derivative):	#若微分是NaN
            									# isnan(x)函数：若"x"非数字，则返回 True 
                derivative = 0	# 微分值归0 
                
                self._last_derivative = 0	# 微分值为0                
            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	# 乘以总系数
####################################################积分pid计算###################################################
        if abs(self._ki) > 0 and dt > 0:# 如果ki系数大于0，且不是第一个循环（第一个循环不存在积分），且不超时（如果超时，前文会把dt置0）
            self._integrator += (error * self._ki) * scaler * delta_time#计算积分控制
            if self._integrator < -self._imax:#如果积分总量是负数且小于负数阈值（-self._imax）
            	self._integrator = -self._imax#积分负数方向限幅
            
            elif self._integrator > self._imax:#如果积分总量是正数且大于正数阈值（self._imax）
            	self._integrator = self._imax#积分正数方向限幅
            
            output += self._integrator	# 加上积分算法的值
        return output
        
    def reset_I(self):	# 重置I
        self._integrator = 0
        self._last_derivative = float('nan')

三.总结：

这个pid模块是严格按照积分，微分的定义写的，体现在它的输出量与时间相联系。还很细节的加入了积分限幅。可以适用于一般的pid控制了，我们可以选择通过串口把控制输出值发送给下位机执行控制