用python写一段建立一阶直线倒立摆模型的代码

可以用以下的 Python 代码建立一阶直线倒立摆模型：

import numpy as np
import matplotlib.pyplot as plt
import control

# 初始状态
x0 = [0.1, 0, 0, 0]  # [theta, theta_dot, x, x_dot]

# 系统参数
m_cart = 1.0     # 小车质量
m_pend = 0.1     # 摆杆质量
l = 0.5          # 摆杆长度
g = 9.8          # 重力加速度

# 状态空间模型
A = np.array([
    [0, 1, 0, 0],
    [0, 0, (m_cart+m_pend)*g/m_cart/l, 0],
    [0, 0, 0, 1],
    [0, 0, -m_pend*g/m_cart, 0]
])
B = np.array([0, 1/m_cart, 0, -1/m_cart/l]).reshape((4,1))
C = np.array([1, 0, 0, 0]).reshape((1,4))
D = np.array([0])

# 状态反馈控制器设计
Q = np.diag([1, 1, 10, 1])
R = 0.01
K, S, E = control.lqr(A, B, Q, R)
A_cl = A - B @ K

# 线性系统建模
sys = control.ss(A_cl, B, C, D)

# 相应参数计算
y,t = control.step(sys, X0=x0)
theta = y[:,0]
cart_x = y[:,2]

# 绘图
plt.plot(t, theta*180/np.pi, 'b', label='theta')
plt.plot(t, cart_x, 'r', label='cart_x')
plt.legend(loc='upper right')
plt.xlabel('Time (s)')
plt.ylabel('Angle (deg) / Position (m)')
plt.title('Inverted Pendulum System')
plt.grid()
plt.show()

这个代码建立了一个状态空间模型，并使用 LQR 控制设计了一个状态反馈控制器。我们可以通过运行代码，得到直线倒立摆的角度和小车位置随时间的变化曲线图。