PID控制示例

PID控制简单示例

import numpy as np
import matplotlib.pyplot as plt
import copy# 定义曲线函数 y = sin(x)
def target_curve(x):return np.sin(x)class PID:def __init__(self, kp, ki, kd):self.kp = kpself.ki = kiself.kd = kdself.ep = 0.0self.ei = 0.0self.ed = 0.0self.dt = 0.1def get_u(self, e):self.ed = e - self.epself.ei += eself.ep = copy.deepcopy(e)u = self.kp*self.ep+self.ki*self.ei+self.kd*self.edif u > np.pi/2: u = np.pi/2        if u < -np.pi/2: u = -np.pi/2return u# 定义小车模型
class Car:def __init__(self):self.x = 0.0  # 当前位置self.y = 0.0  # 当前位置self.angle = 0.0  # 当前转角def update(self, angle_control, dt):self.angle += angle_control * dtself.x += np.cos(self.angle) * dtself.y += np.sin(self.angle) * dt# 定义控制参数
Kp = 10  # 转角比例增益
Ki = 0.0  # 转角积分增益
Kd = 10  # 转角微分增益# 定义时间参数
dt = 0.1
t = np.arange(0, 100, dt)# 创建小车对象和PID控制器对象
car = Car()
pid = PID(Kp, Ki, Kd)# 进行控制循环
x_ref = t  # 设定x值为时间
# y_ref = target_curve(x_ref)  # 计算对应的y值
y_ref = np.array([1]*len(t))
x_actual = np.zeros_like(t)
y_actual = np.zeros_like(t)
for i in range(len(t)):x_actual[i] = car.xy_actual[i] = car.y# 转角控制angle_error = y_ref[i] - y_actual[i]angle_output = pid.get_u(angle_error)# 更新小车状态car.update(angle_output, dt)# 绘制结果
plt.figure(figsize=(20, 5))
plt.plot(x_ref, y_ref, label='Reference')
plt.plot(x_actual, y_actual, label='Actual')
plt.xlabel('x')
plt.ylabel('y')
plt.legend()
plt.show()

尝试了一下跟踪正弦波的参数

Kp = 0.5
Ki = 0.01
Kd = 12

但效果一般。