用Python实现的PID控制算法的演示

看了看PID控制算法的原理，自己用Python的matplotlib简单写了一个控制小球到指定位置的小演示。

如果小球不在指定的高度，就计算误差，再输入一个控制信号——这里是加速度a，以便驱动小球运动，从而改变小球的位置。

不停的计算：计算高度误差，计算输出的信号——加速度a，计算新的速度，计算新高度。

如果只考虑小球当前位置和指定高度的误差，始终会来回摆动，无法进入稳定状态。

如果用到PID算法，可以很好的进入稳定状态。

因为是虚拟环境，没有考虑摩擦力，也就用不上PID算法的积分项，也是偷懒，就相当于PID里的PD两部分考虑了。

# -*- coding: utf-8 -*-
# Author: [email protected]
import matplotlib as mpl
from matplotlib import pyplot as plt
import numpy as np

# 设置字体
# mpl.rcParams['font.sans-serif'] = ['SimHei']  # 指定默认字体
# mpl.rcParams['axes.unicode_minus'] = False  # 解决保存图像是负号'-'显示为方块的问题

# 定义数据
itor = 1
X1 = 5  # 横坐标固定为5，只改变小球的高度
X2 = 15  # 初始纵坐标
XP = 10  # 预设的纵坐标
v = 0    # 速度
a = 0    # 加速度

Kp = 0.2
Kd = 0.2
Err1 = X2 - XP
Err0 = 0
Max_itor = 50000
#
plt.figure(figsize=(12, 8), dpi=100)
plt.ion()

while itor < Max_itor:
    # 清空旧的画布
    plt.cla()
    plt.xlim((-1, 20))
    plt.ylim((-1, 20))
    plt.plot([-1, 20], [XP, XP], 'r-')

    # rgeion
    a = Kp*(1.0*Err1 + Kd*(Err1-Err0))  # 按PD算法计算要给予的加速度
    # a = Kp*(1.0*Err1)  # 不用PD算法
    v = v - a
    X2 = X2 + v
    # endregion

    Err0 = Err1  # 保留旧的误差
    Err1 = X2 - XP  # 计算新的误差

    plt.scatter(X1, X2,  # 散点的横、纵坐标序列
                s=30,   # 点
                c='blue',
                label="PD Algorithm")
    plt.legend()  # 显示图例说明
    itor = itor + 1
    plt.pause(0.008)

通过这个小演示案例，更熟悉了一下Matplotlib，确实功能强大。

用matplotlib的交互模式，能很简单的完成演示动画。