看了看PID控制算法的原理,自己用Python的matplotlib简单写了一个控制小球到指定位置的小演示。
如果小球不在指定的高度,就计算误差,再输入一个控制信号——这里是加速度a,以便驱动小球运动,从而改变小球的位置。
不停的计算:计算高度误差,计算输出的信号——加速度a,计算新的速度,计算新高度。
如果只考虑小球当前位置和指定高度的误差,始终会来回摆动,无法进入稳定状态。
如果用到PID算法,可以很好的进入稳定状态。
因为是虚拟环境,没有考虑摩擦力,也就用不上PID算法的积分项,也是偷懒,就相当于PID里的PD两部分考虑了。
# -*- coding: utf-8 -*-
# Author: HarryJD@sina.com
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的交互模式,能很简单的完成演示动画。