4. 基于模糊PID的倒立摆平衡控制

任务目标

使用模糊PID控制器实现一阶倒立摆的平衡控制。系统状态为摆杆角度θ和角速度θ̇，控制器输出为小车加速度u。通过设计模糊规则在线调整PID增益（Kp、Ki、Kd），使摆杆在受到初始扰动后能够回到并保持在竖直平衡位置（θ=0）。

系统模型与依赖

倒立摆简化模型（忽略摩擦，小车位置不做控制）：

θ'' = (g·sinθ + u·cosθ) / (L·(4/3 - m·cos²θ/(M+m)))

其中：M=1.0kg（小车质量），m=0.1kg（摆杆质量），L=0.5m（摆杆半长），g=9.8m/s²。仿真步长dt=0.01s，总时长10s。初始状态θ=0.1rad，θ̇=0。

依赖：Python 3.8+，NumPy，Matplotlib（仅用于绘图验证）。

模糊PID控制器设计

模糊控制器采用二维输入（角度误差e、角速度ec），输出为PID增益修正量ΔKp、ΔKi、ΔKd。输入输出论域均为[-1,1]，采用三角形隶属函数，5×5规则表。最终PID参数为：

Kp = Kp_base + ΔKp·Kp_scale
Ki = Ki_base + ΔKi·Ki_scale
Kd = Kd_base + ΔKd·Kd_scale

基础增益：Kp_base=120，Ki_base=30，Kd_base=20；缩放因子：Kp_scale=80，Ki_scale=20，Kd_scale=15。

关键完整代码

import numpy as np

# ---------- 倒立摆物理模型 ----------
def pendulum_dynamics(state, u, M=1.0, m=0.1, L=0.5, g=9.8):
    theta, theta_dot = state
    sin_t, cos_t = np.sin(theta), np.cos(theta)
    denom = L * (4/3 - m*cos_t**2/(M+m))
    theta_ddot = (g*sin_t + u*cos_t) / denom
    return np.array([theta_dot, theta_ddot])

# ---------- 模糊隶属函数 ----------
def trimf(x, a, b, c):
    return np.clip(np.minimum((x-a)/(b-a), (c-x)/(c-b)), 0, 1) if a < b < c else np.zeros_like(x)

# 输入隶属函数（5个模糊集：NB, NS, ZO, PS, PB）
def input_mf(e, ec):
    # 论域[-1,1]，三角形顶点位置
    centers = [-1.0, -0.5, 0.0, 0.5, 1.0]
    e_mf = np.array([trimf(e, centers[i]-0.5, centers[i], centers[i]+0.5) for i in range(5)])
    ec_mf = np.array([trimf(ec, centers[i]-0.5, centers[i], centers[i]+0.5) for i in range(5)])
    return e_mf, ec_mf

# ---------- 模糊规则表（输出隶属度索引：0=NB,1=NS,2=ZO,3=PS,4=PB）----------
# 规则表行索引对应e的模糊集，列索引对应ec的模糊集
rule_kp = np.array([[4,4,3,3,2],   # e=NB
                    [4,3,3,2,2],   # e=NS
                    [3,3,2,2,1],   # e=ZO
                    [3,2,2,1,1],   # e=PS
                    [2,2,1,1,0]])  # e=PB

rule_ki = np.array([[0,0,1,2,2],
                    [0,1,1,2,3],
                    [1,1,2,2,3],
                    [1,2,2,3,3],
                    [2,2,3,3,4]])

rule_kd = np.array([[2,1,0,1,2],
                    [1,1,0,1,1],
                    [0,0,0,0,0],
                    [1,1,0,1,1],
                    [2,1,0,1,2]])

# 输出隶属函数中心值（用于重心法解模糊）
out_centers = np.array([-1.0, -0.5, 0.0, 0.5, 1.0])

def fuzzy_infer(e, ec, rule_table):
    e_mf, ec_mf = input_mf(e, ec)
    # 计算每条规则的激活强度（取小）
    activation = np.zeros((5,5))
    for i in range(5):
        for j in range(5):
            activation[i,j] = min(e_mf[i], ec_mf[j])
    # 重心法解模糊
    num = 0.0
    den = 0.0
    for i in range(5):
        for j in range(5):
            out_idx = rule_table[i,j]
            w = activation[i,j]
            num += w * out_centers[out_idx]
            den += w
    return num / den if den > 0 else 0.0

# ---------- 模糊PID控制器 ----------
def fuzzy_pid_controller(state, ref, dt, integral, prev_error,
                          Kp_base=120, Ki_base=30, Kd_base=20,
                          Kp_scale=80, Ki_scale=20, Kd_scale=15):
    theta, theta_dot = state
    e = ref - theta
    ec = -theta_dot  # 角速度即为误差变化率（ref恒定）
    
    # 输入归一化到[-1,1]
    e_norm = np.clip(e / 0.5, -1, 1)   # 角度误差最大0.5rad
    ec_norm = np.clip(ec / 2.0, -1, 1) # 角速度最大2rad/s
    
    # 模糊推理得到增益修正量
    d_kp = fuzzy_infer(e_norm, ec_norm, rule_kp)
    d_ki = fuzzy_infer(e_norm, ec_norm, rule_ki)
    d_kd = fuzzy_infer(e_norm, ec_norm, rule_kd)
    
    # 计算实际PID参数
    Kp = Kp_base + d_kp * Kp_scale
    Ki = Ki_base + d_ki * Ki_scale
    Kd = Kd_base + d_kd * Kd_scale
    
    # PID控制律
    integral += e * dt
    derivative = (e - prev_error) / dt
    u = Kp*e + Ki*integral + Kd*derivative
    return u, integral, e, Kp, Ki, Kd

# ---------- 仿真主程序 ----------
def simulate():
    dt = 0.01
    t_end = 10.0
    n_steps = int(t_end / dt)
    
    state = np.array([0.1, 0.0])  # 初始角度0.1rad
    integral = 0.0
    prev_error = 0.0 - state[0]
    
    theta_history = []
    u_history = []
    t_history = []
    
    for step in range(n_steps):
        u, integral, e, Kp, Ki, Kd = fuzzy_pid_controller(
            state, 0.0, dt, integral, prev_error)
        
        # 限制控制量
        u = np.clip(u, -20, 20)
        
        # 更新状态（RK2中点法）
        k1 = pendulum_dynamics(state, u)
        k2 = pendulum_dynamics(state + dt*k1, u)
        state = state + dt * (k1 + k2) / 2
        
        prev_error = e
        theta_history.append(state[0])
        u_history.append(u)
        t_history.append(step*dt)
    
    return np.array(t_history), np.array(theta_history), np.array(u_history)

# ---------- 运行仿真 ----------
if __name__ == "__main__":
    t, theta, u = simulate()
    
    # 验证指标
    max_theta = np.max(np.abs(theta))
    final_theta = np.abs(theta[-1])
    settle_idx = np.where(np.abs(theta) < 0.01)[0]
    settle_time = t[settle_idx[0]] if len(settle_idx) > 0 else None
    
    print(f"最大摆角偏差: {max_theta:.4f} rad")
    print(f"最终摆角偏差: {final_theta:.6f} rad")
    print(f"稳定时间(进入±0.01rad): {settle_time:.2f}s" if settle_time else "未在10s内稳定")
    
    # 绘图验证（取消注释以显示）
    # import matplotlib.pyplot as plt
    # plt.figure(figsize=(10,4))
    # plt.subplot(1,2,1); plt.plot(t, theta); plt.title("Pendulum Angle (rad)"); plt.xlabel("t(s)")
    # plt.subplot(1,2,2); plt.plot(t, u); plt.title("Control Input (m/s²)"); plt.xlabel("t(s)")
    # plt.tight_layout(); plt.show()

运行与验证

将上述代码保存为fuzzy_pendulum.py，在终端运行：python fuzzy_pendulum.py
程序输出三项指标：最大摆角偏差、最终摆角偏差、稳定时间。
通过条件：最终摆角偏差小于0.001rad（即|θ|<0.001），且稳定时间小于5s。

模糊规则调整方法

若系统出现振荡（稳定时间过长），应减小rule_kp中远离对角线的值（即大误差时降低Kp），或增大rule_kd中对应位置的阻尼值。若响应过慢（最大摆角偏大），应增大rule_kp中对角线附近的值。调整后重新运行，观察指标变化。

预期输出（基于代码逻辑）

最大摆角偏差: 约0.10~0.15 rad
最终摆角偏差: < 0.0001 rad
稳定时间: 约2~4s

以上为基于控制器结构和初始条件的确定性预期，实际数值以本地运行为准。

📚 课程目录