PID算法_python实现_详细注释

PID标准实现

需要注意的是每一次 calculate更新传入的是目标位置和当前位置，返回的是一个加速度

import matplotlib.pyplot as plt

class PID():
    def __init__(self, dt, max, min, Kp, Kd, Ki):
        self.dt = dt  # 采样周期
        self.max = max  # 最大调整值
        self.min = min  # 最小调整值

        self.Kp = Kp  # 比例系数
        self.Kd = Kd  # 积分系数
        self.Ki = Ki  # 微分系数

        self.integral = 0  # 积分和
        self.previous_error = 0  # delta_t 产生偏差
        self.error = 0 #当前位置距离目标位置的偏差

    def calculate(self, setpoint, measured_value):
        '''
        :param setpoint: 目标位置
        :param measured_value:  当前位置
        :return: 返回需要调整的速度
        '''
        self.error = setpoint - measured_value  # 计算得到偏差

        P_out = self.Kp * self.error  # 计算出比例值
        self.integral = self.integral + self.error * self.dt  # 计算得到积分累加和

        I_out = self.Ki * self.integral  # 计算出积分值
        derivative = (self.error - self.previous_error) / self.dt

        D_out = self.Kd * derivative  # 计算出微分值

        self.previous_error = self.error  # 保存上一次偏差
        output = (P_out + I_out + D_out)

        if output > self.max:
            output = self.max

        if output < self.min:
            output = self.min

        return output

list_x = list()
list_y = list()

testPID = PID(dt=0.1, max=100, min=-100,
              Kp=0.1, Kd=0.01, Ki=0.01);

val = 20 # 初始化位置
speed = 0 # 初始化速度
for point in range(0, 200):
    inc = testPID.calculate(0, val)

    speed += inc * 1# 速度 + 返回的速度
    val += speed * 1 # 调整位置, 1是一个代码时间片,速度*time==调整的位置

    list_x.append(point)
    list_y.append(speed)

plt.scatter(list_x, list_y, alpha=0.6, edgecolors='white')
plt.plot(list_x,list_y)

plt.legend()
plt.show()

目标检测面积比追踪

面积比映射成距离函数

def caculatorAreaRadioDistance(view_size:	tuple, target_size:tuple, set_point_scale = 0.3, power = 20):
    '''
    # 计算面积比, 并返回面积比于距离的映射公式
    :param view_size: 视图 (w,h)
    :param target_size: 目标框大小 (w,h)
    :param set_point_scale: 目标面积比
    :param power: 倍数
    :return: 面积比至目标比映射出的距离
    '''
    area1, area = target_size[0] * target_size[1] , view_size[0] * view_size[1]
    radio = area1/area
    print(radio)
    # radio = radio if radio < 0.5 else 0.5 # 面积比不会超过0.5
    radio = radio if radio < 1 else 1 # 面积比不会超过1

    # dis = set_point_scale - radio # 离 log1 的距离
    # log_dis = math.log(1 + dis)
    # distance = log_dis * power # 面积比和距离的映射公式

    raido_err = set_point_scale - radio #目标比例 - 当前比例
    # distance = (1/raido_err) * power #比例距离映射

    distance = math.tanh(raido_err * math.pi) * power
    # distance = distance if distance < 100 else 100
    return distance

目标检测框与视图比例映射成距离的方式，可以根据距离映射公式，实现目标检测的小车追踪

import math

import matplotlib.pyplot as plt
import time

class PID():
    def __init__(self, dt, max, min, Kp, Ki, Kd):

        self.dt = dt

        self.max = max  # 最大调整值
        self.min = min  # 最小调整值

        self.Kp = Kp  # 比例系数()
        self.Kd = Kd  # 微分系数(当参数为正时起到阻尼的效果,当参数为负则是助力的效果)
        self.Ki = Ki  # 积分系数(误差的积分,能够最终确保距离)

        self.integral = 0  # 积分和

        self.previous_error = 0  # 上一次偏差

        self.error = 0

    def calculate(self, setpoint, measured_value):

        self.error = setpoint - measured_value  # 计算得到偏差

        P_out = self.Kp * self.error  # 计算出比例值

        self.integral = self.integral + self.error * self.dt  # 计算得到累计误差
        I_out = self.Ki * self.integral  # 计算出积分值

        derivative = (self.error - self.previous_error) / self.dt
        D_out = self.Kd * derivative  # 计算出微分值

        self.previous_error = self.error  # 保存上一次偏差
        output = (P_out + I_out + D_out)

        if output > self.max:
            output = self.max

        if output < self.min:
            output = self.min

        return output

def caculatorAreaRadioDistance(view_size:	tuple, target_size:tuple, set_point_scale = 0.3, power = 20):
    '''
    # 计算面积比, 并返回面积比于距离的映射公式
    :param view_size: 视图 (w,h)
    :param target_size: 目标框大小 (w,h)
    :param set_point_scale: 目标面积比
    :param power: 倍数
    :return: 面积比至目标比映射出的距离
    '''
    area1, area = target_size[0] * target_size[1] , view_size[0] * view_size[1]
    radio = area1/area
    print(radio)
    # radio = radio if radio < 0.5 else 0.5 # 面积比不会超过0.5
    radio = radio if radio < 1 else 1 # 面积比不会超过1

    # dis = set_point_scale - radio # 离 log1 的距离
    # log_dis = math.log(1 + dis)
    # distance = log_dis * power # 面积比和距离的映射公式

    raido_err = set_point_scale - radio #目标比例 - 当前比例
    # distance = (1/raido_err) * power #比例距离映射

    distance = math.tanh(raido_err * math.pi) * power
    # distance = distance if distance < 100 else 100
    return distance

# if __name__ == '__main__':
#     w, h = 10,10
#     w1, h1 = 0.1, 0.1
#     # w1, h1 = 0.01, 0.
#     distance = caculatorAreaRadioDistance((w,h), (w1, h1),set_point_scale=0.3,  power=200)
#     print(distance)
#     exit()


# 当面积比达到 0.5的时候小车停止前进


list_x = list()
list_y = list()

testPID = PID(dt=0.1, max=100, min=-100,
              Kp=0.2, Kd=0.01, Ki=0.01);

val = 20 #初始化位置
speed = 0 # 初始化速度
for point in range(0, 200):
    inc = testPID.calculate(0, val)

    speed += inc * 1 #速度 + 返回的速度
    val += speed * 1 #调整位置, 1是一个代码时间片,速度*time==调整的位置

    list_x.append(point)
    list_y.append(speed)

plt.scatter(list_x, list_y, alpha=0.6, edgecolors='white')

plt.plot(list_x,list_y)

plt.legend()

plt.show()

最后模拟真实的世界的时间场景,我们通过time函数来获取deltatime来获取真实pid控制效果

import time

import matplotlib.pyplot as plt

class PID():
    def __init__(self, dt, max, min, Kp, Kd, Ki):
        # self.dt = dt  # 采样周期
        self.max = max  # 最大调整值
        self.min = min  # 最小调整值

        self.Kp = Kp  # 比例系数
        self.Kd = Kd  # 积分系数
        self.Ki = Ki  # 微分系数

        self.integral = 0  # 积分和
        self.previous_error = 0  # delta_t 产生偏差
        self.error = 0 #当前位置距离目标位置的偏差


    def calculate(self, setpoint, measured_value, delta_time):
        '''
        :param setpoint: 目标位置
        :param measured_value:  当前位置
        :param 传入一次函数真实时间
        :return: 返回需要调整的速度
        '''

        self.error = setpoint - measured_value  # 计算得到偏差

        P_out = self.Kp * self.error  # 计算出比例值
        self.integral = self.integral + self.error * delta_time  # 计算得到积分累加和

        I_out = self.Ki * self.integral  # 计算出积分值
        derivative = (self.error - self.previous_error) / delta_time

        D_out = self.Kd * derivative  # 计算出微分值

        self.previous_error = self.error  # 保存上一次偏差
        output = (P_out + I_out + D_out)

        if output > self.max:
            output = self.max

        if output < self.min:
            output = self.min

        return output

plt.ion()
list_x = list()
list_y = list()

testPID = PID(dt=0.1, max=100, min=-100,
              Kp=0.2, Kd=0.5, Ki=0.01);

val = 20 # 初始化位置
speed = 0 # 初始化速度
lasttime = time.time()
plt.pause(0.05)
for point in range(0, 200):
    curtime = time.time()
    deltatime = curtime - lasttime # 求得间距的系统真实时间
    lasttime = time.time()

    inc = testPID.calculate(0, val, deltatime) # 返回一个加速度
    speed += inc * deltatime # 速度 + 加速度*time
    val += speed * deltatime # 调整位置, 1是一个代码时间片,速度*time==调整的位置

    list_x.append(point)
    list_y.append(speed)

    plt.pause(0.05)

    plt.scatter(list_x, list_y, alpha=0.6, edgecolors='white')
    plt.plot(list_x,list_y)

代码运行可以看到动态调整的速度,开始的震荡是因为初始化的原因。