用python编写基于pid控制倒立摆的代码
时间: 2024-05-13 11:19:31 浏览: 25
好的,下面是基于PID控制倒立摆的Python代码。
```
import numpy as np
import matplotlib.pyplot as plt
class InvertedPendulum:
def __init__(self, m, M, L, b, g):
self.m = m # 摆球质量
self.M = M # 小车质量
self.L = L # 杆长
self.b = b # 摩擦力系数
self.g = g # 重力加速度
def dynamics(self, x, u):
"""
系统动力学方程
"""
theta, theta_dot, x, x_dot = x
sin_theta = np.sin(theta)
cos_theta = np.cos(theta)
temp = (u + self.m * self.L * theta_dot ** 2 * sin_theta - self.b * x_dot) / (self.M + self.m)
theta_dot_dot = (self.g * sin_theta - cos_theta * temp) / (self.L * (4 / 3 - self.m * cos_theta ** 2 / (self.M + self.m)))
x_dot_dot = temp - self.m * self.L * theta_dot_dot * cos_theta / (self.M + self.m)
return np.array([theta_dot, theta_dot_dot, x_dot, x_dot_dot])
class PIDController:
def __init__(self, kp, ki, kd):
self.kp = kp # 比例系数
self.ki = ki # 积分系数
self.kd = kd # 微分系数
self.error_sum = 0 # 积分误差和
self.last_error = 0 # 上一次误差
def control(self, error, dt):
"""
PID控制器
"""
self.error_sum += error * dt
error_diff = (error - self.last_error) / dt
self.last_error = error
return self.kp * error + self.ki * self.error_sum + self.kd * error_diff
def simulate(pendulum, controller, x0, dt, T):
"""
模拟函数
"""
t = np.arange(0, T, dt)
x = np.zeros((len(t), len(x0)))
x[0] = x0
for i in range(len(t) - 1):
error = np.pi - x[i, 0]
u = controller.control(error, dt)
x_dot = pendulum.dynamics(x[i], u)
x[i + 1] = x[i] + x_dot * dt
return t, x
if __name__ == '__main__':
m = 0.1
M = 1
L = 0.5
b = 0.1
g = 9.8
pendulum = InvertedPendulum(m, M, L, b, g)
controller = PIDController(10, 0.1, 1)
x0 = np.array([np.pi / 6, 0, 0, 0])
dt = 0.01
T = 10
t, x = simulate(pendulum, controller, x0, dt, T)
plt.plot(t, x[:, 0])
plt.xlabel('Time (s)')
plt.ylabel('Angle (rad)')
plt.show()
```
这里我们使用了PID控制器来控制倒立摆。在`PIDController`类中,我们根据比例系数、积分系数和微分系数来计算控制输出。在`simulate`函数中,我们使用Euler方法进行数值模拟。最后,我们将模拟结果绘制成图形以便观察。