利用模糊控制和pid控制法,用python实现单级倒立摆

由于单级倒立摆模型比较复杂，需要涉及到动力学方程等知识，因此建议使用现成的模拟器进行模拟和控制，例如vpython等。

以下是一个简单的示例代码，使用PID控制单级倒立摆的角度：

from vpython import *
import time

# 定义常量
g = 9.8  # 重力加速度
m = 1.0  # 摆的质量
l = 1.0  # 摆的长度
b = 0.1  # 摩擦系数

# 定义初始状态
theta = 0.1  # 初始角度
theta_dot = 0.0  # 初始角速度

# 定义控制器参数
Kp = 100.0  # 比例系数
Ki = 0.1  # 积分系数
Kd = 10.0  # 微分系数

# 定义PID控制器
class PID:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.last_error = 0.0
        self.integral = 0.0

    def update(self, error, dt):
        self.integral += error * dt
        derivative = (error - self.last_error) / dt
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.last_error = error
        return output

# 初始化场景
scene = canvas(title='Inverted Pendulum', width=800, height=600)

# 定义摆的杆和箱子
rod = cylinder(pos=vector(0, 0, 0), axis=vector(l*sin(theta), -l*cos(theta), 0), radius=0.05, color=color.red)
box = box(pos=vector(l*sin(theta), -l*cos(theta), 0), size=vector(0.2, 0.2, 0.2), color=color.green)

# 定义PID控制器
pid = PID(Kp, Ki, Kd)

# 开始模拟
while True:
    # 计算摆的角加速度
    theta_dot_dot = (m*g*l*sin(theta) - b*theta_dot) / (m*l**2)

    # 计算控制量
    error = -theta
    control = pid.update(error, 0.01)

    # 更新状态
    theta_dot += theta_dot_dot * 0.01
    theta += theta_dot * 0.01
    theta += control

    # 更新摆的杆和箱子的位置
    rod.axis = vector(l*sin(theta), -l*cos(theta), 0)
    box.pos = vector(l*sin(theta), -l*cos(theta), 0)

    # 等待一段时间，使模拟器能够正常显示
    time.sleep(0.01)

这段代码使用PID控制器控制单级倒立摆的角度，其中PID控制器的参数可以根据实际情况进行调整。在模拟器中，摆的杆和箱子的位置会随着时间的推移而变化，同时控制器会不断调整摆的角度，保持其在倒立状态。