一阶倒立摆控制：从PID到智能算法的实现与优化

1. 项目概述：一阶倒立摆的控制挑战

倒立摆系统是控制理论中的经典实验平台，它模拟了众多现实世界中的平衡控制问题，比如火箭姿态控制、两轮平衡车等。这个项目我们实现的是一个基础版本——一阶倒立摆，也就是在可移动小车上安装一个自由摆杆的系统。

这个看似简单的系统其实蕴含着丰富的控制理论挑战。摆杆本身是不稳定的，重心在上方，任何微小的扰动都会导致它倒下。我们的目标是通过控制小车的左右移动，使摆杆保持直立状态。这需要实时测量摆杆的角度和角速度，并快速计算出适当的控制力。

提示：一阶倒立摆的动力学方程是非线性的，这增加了控制难度。在实际操作中，我们通常会在平衡点附近对系统进行线性化处理。

2. 系统建模与基础控制

2.1 物理系统建模

首先我们需要建立系统的数学模型。设摆杆长度为l，质量为m，小车质量为M，摆杆与垂直方向的夹角为θ。根据拉格朗日力学，可以得到系统的运动方程：

python复制# 倒立摆动力学方程简化表示
def pendulum_dynamics(theta, theta_dot, u):
    """
    theta: 摆杆角度(rad)
    theta_dot: 角速度(rad/s)
    u: 控制输入(N)
    返回: 角加速度(rad/s^2)
    """
    g = 9.8  # 重力加速度
    l = 0.3  # 摆杆长度(m)
    return (g * np.sin(theta) + u * np.cos(theta)) / l

这个方程告诉我们，角加速度取决于当前角度、控制输入和系统参数。在实际项目中，我们还需要考虑摩擦、电机响应延迟等实际因素。

2.2 PID控制器实现

最基础的控制方案是PID控制。我们设计了角度环和位置环的双环控制结构：

python复制class PIDController:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.prev_error = 0
        self.integral = 0
        
    def co