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 = kp
        self.ki = ki
        self.kd = kd
        self.ep = 0.0
        self.ei = 0.0
        self.ed = 0.0
        self.dt = 0.1

    def get_u(self, e):
        self.ed = e - self.ep
        self.ei += e
        self.ep = copy.deepcopy(e)
        u = self.kp*self.ep+self.ki*self.ei+self.kd*self.ed
        if u > np.pi/2: u = np.pi/2        
        if u < -np.pi/2: u = -np.pi/2
        return 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 * dt
        self.x += np.cos(self.angle) * dt
        self.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.x
    y_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

但效果一般。