《自动控制原理》- 第十章 动态系统的最优控制方法

import numpy as np
import matplotlib
# 在导入pyplot之前设置非交互式后端
matplotlib.use('Agg')  # 使用非交互式后端
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
import os

plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False

# 小车系统状态方程
def car_system(t, x, u):
    dx1_dt = x[1]  # 位置导数是速度
    dx2_dt = u     # 速度导数是加速度
    return [dx1_dt, dx2_dt]

# 性能指标积分项
def performance_integrand(x, u):
    return (x[0] - 10)**2 + x[1]**2 + 0.1 * u**2

# 最优控制策略
def optimal_control(x):
    k1, k2 = 2.0, 1.5
    return -k1 * (x[0] - 10) - k2 * x[1]

# 仿真参数
t_span = (0, 10)
t_eval = np.linspace(0, 10, 1000)
x0 = [0, 0]  # 初始状态

# 使用solve_ivp一次性求解整个轨迹
def system_wrapper(t, x):
    u = optimal_control(x)
    return car_system(t, x, u)

sol = solve_ivp(
    system_wrapper,
    t_span,
    x0,
    t_eval=t_eval,
    method='RK45'
)

# 计算控制输入和性能指标
u_history = [optimal_control(sol.y[:, i]) for i in range(len(t_eval))]
J = 0
for i in range(len(t_eval)-1):
    dt = t_eval[i+1] - t_eval[i]
    J += performance_integrand(sol.y[:, i], u_history[i]) * dt

x_history = sol.y.T  # 转置为(N, 2)形状

# 绘制结果
plt.figure(figsize=(12, 8))

plt.subplot(2, 1, 1)
plt.plot(t_eval, x_history[:, 0], 'b-', label='位置')
plt.plot(t_eval, x_history[:, 1], 'r-', label='速度')
plt.axhline(y=10, color='g', linestyle='--', label='目标位置')
plt.xlabel('时间(s)')
plt.ylabel('状态')
plt.legend()
plt.title('小车最优轨迹控制 - 状态响应')

plt.subplot(2, 1, 2)
plt.plot(t_eval, u_history, 'k-')
plt.xlabel('时间(s)')
plt.ylabel('控制输入u')
plt.title('小车最优轨迹控制 - 控制输入')

plt.tight_layout()

# 保存图像而不是直接显示
output_dir = "D:/python project/自动控制原理"
os.makedirs(output_dir, exist_ok=True)
output_path = os.path.join(output_dir, "optimal_control_plot.png")
plt.savefig(output_path)
print(f"图形已保存至: {output_path}")

# 输出最终性能指标
print(f"最终性能指标J = {J:.4f}")

import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import minimize

# 设置中文字体
plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False


# 系统状态方程：x1位置, x2速度, u控制输入(-1到1)
def system_dynamics(x, u):
    dx1_dt = x[1]
    dx2_dt = u
    return np.array([dx1_dt, dx2_dt])


# 模拟系统在控制输入下的响应
def simulate_system(u, x0, tf):
    t = np.linspace(0, tf, 100)
    x = np.zeros((len(t), 2))  # 创建二维数组存储状态
    x[0] = x0  # 设置初始状态

    for i in range(1, len(t)):
        dt = t[i] - t[i - 1]
        # 计算四阶龙格-库塔系数
        k1 = system_dynamics(x[i - 1], u)
        k2 = system_dynamics(x[i - 1] + 0.5 * dt * k1, u)
        k3 = system_dynamics(x[i - 1] + 0.5 * dt * k2, u)
        k4 = system_dynamics(x[i - 1] + dt * k3, u)

        # 更新状态
        x[i] = x[i - 1] + (dt / 6) * (k1 + 2 * k2 + 2 * k3 + k4)

    return x, t


# 定义优化目标：最小化时间和终端误差
def objective(params, x0, xf):
    u = params[0]  # 控制输入
    tf = params[1]  # 时间

    # 模拟系统响应
    x, t = simulate_system(u, x0, tf)

    # 计算终端误差
    terminal_error = np.sum((x[-1] - xf) ** 2)

    # 目标函数：时间 + 终端误差惩罚
    return tf + 100 * terminal_error


# 约束条件：控制输入范围
def constraint(params):
    u = params[0]
    return 1 - abs(u)  # u在[-1,1]之间


# 初始状态和目标状态
x0 = np.array([0, 0])  # 初始位置和速度
xf = np.array([5, 0])  # 目标位置和速度

# 优化求解
# 初始猜测值：[控制输入, 时间]
x_init = [0.5, 10.0]
bounds = [(-1, 1), (1, 20)]  # 控制输入和时间的范围
constraints = [{'type': 'ineq', 'fun': constraint}]

# 寻找最优控制和时间
result = minimize(
    objective, x_init,
    args=(x0, xf),
    method='SLSQP',
    bounds=bounds,
    constraints=constraints,
    options={'disp': True}
)

# 提取优化结果
optimal_u, optimal_tf = result.x

print(f"最优控制输入: u = {optimal_u:.4f}")
print(f"最优时间: tf = {optimal_tf:.4f}s")

# 仿真最优控制结果
x_opt, t_opt = simulate_system(optimal_u, x0, optimal_tf)

# 绘制结果
plt.figure(figsize=(12, 8))

plt.subplot(2, 1, 1)
plt.plot(t_opt, x_opt[:, 0], 'b-', label='位置')
plt.plot(t_opt, x_opt[:, 1], 'r-', label='速度')
plt.axhline(y=xf[0], color='g', linestyle='--', label='目标位置')
plt.axhline(y=xf[1], color='m', linestyle='--', label='目标速度')
plt.xlabel('时间(s)')
plt.ylabel('状态')
plt.legend()
plt.title(f'最短时间控制 - 最优时间: {optimal_tf:.2f}s')

plt.subplot(2, 1, 2)
plt.plot(t_opt[:-1], np.ones_like(t_opt[:-1]) * optimal_u, 'k-')
plt.xlabel('时间(s)')
plt.ylabel('控制输入u')
plt.title('最短时间控制 - 最优控制输入')

plt.tight_layout()
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_bvp
import warnings

warnings.filterwarnings('ignore', category=RuntimeWarning)  # 忽略数值计算警告

plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False

# 火箭系统参数 - 修改参数使问题更合理
g = 9.81  # 重力加速度(m/s²)
c = 3000  # 增大排气速度(m/s) 以提高推力
m0 = 1000  # 初始质量(kg)
fuel_flow_rate = 1.0  # 增大燃料消耗率(kg/s per unit throttle) 使推力更大


# 火箭系统状态方程（向量化处理）
def rocket_system(t, y, u):
    dh_dt = y[1]  # 高度导数是速度
    dv_dt = c * u / y[2] - g  # 速度导数是加速度
    dm_dt = -fuel_flow_rate * u  # 质量导数
    return np.vstack([dh_dt, dv_dt, dm_dt])


# 协态方程（向量化处理）
def co_state_eq(t, y, lambda_vec, u):
    dlambda1_dt = np.zeros_like(u)  # ∂H/∂h = 0
    dlambda2_dt = -lambda_vec[0]  # ∂H/∂v = -λh
    dlambda3_dt = lambda_vec[1] * c * u / (y[2] ** 2)  # ∂H/∂m = λv * c * u / m²
    return np.vstack([dlambda1_dt, dlambda2_dt, dlambda3_dt])


# 根据极小值原理计算最优控制（向量化处理）
def compute_optimal_control(y, lambda_vec):
    # 计算哈密顿函数对u的偏导数
    dH_du = fuel_flow_rate - lambda_vec[1] * c / y[2] - lambda_vec[2] * fuel_flow_rate

    # 边界控制策略（使用向量化操作）
    u = np.where(dH_du > 0, 0.0, 1.0)
    return u


# 定义整个系统的微分方程
def system_ode(t, z):
    # z = [h, v, m, λh, λv, λm]
    n = t.size
    y = z[:3]  # 状态变量 (3, n)
    lambda_vec = z[3:]  # 协态变量 (3, n)

    # 计算最优控制（返回长度为n的数组）
    u = compute_optimal_control(y, lambda_vec)

    # 计算状态导数 (3, n)
    dy_dt = rocket_system(t, y, u)

    # 计算协态导数 (3, n)
    dlambda_dt = co_state_eq(t, y, lambda_vec, u)

    # 合并结果 (6, n)
    return np.vstack([dy_dt, dlambda_dt])


# 边界条件函数 - 放松终端约束
def bc(za, zb):
    # 初始条件 (t=0)
    h0, v0, m0, _, _, _ = za
    # 终端条件 (t=T)
    hT, vT, mT, lambda_hT, lambda_vT, lambda_mT = zb

    # 放松终端速度约束，允许小量误差
    return [
        h0 - 0,  # 初始高度为0
        v0 - 0,  # 初始速度为0
        m0 - 1000,  # 初始质量为1000kg
        vT - 0,  # 终端速度为0 (到达最高点)
        lambda_hT - 0,  # λh(T) = 0 (自由高度)
        lambda_mT - 0  # λm(T) = 0 (自由质量)
    ]


# 求解火箭最优控制问题
def solve_rocket_optimal():
    # 时间区间 - 缩短到更合理的时间
    t_span = (0, 10)  # 缩短总时间
    t = np.linspace(t_span[0], t_span[1], 100)

    # 改进的初始状态猜测 - 更符合物理规律
    z_guess = np.zeros((6, t.size))

    # 状态变量初始猜测
    # 高度：抛物线上升
    z_guess[0] = 2000 * (1 - (1 - t / t[-1]) ** 2)

    # 速度：先增后减，终点接近0
    z_guess[1] = 300 * np.sin(np.pi * t / t[-1])

    # 质量：线性减少
    z_guess[2] = 1000 - 80 * t  # 消耗约800kg燃料

    # 协态变量初始猜测 - 全部设为小量
    z_guess[3] = -0.01 * np.ones_like(t)  # λh
    z_guess[4] = -0.01 * np.ones_like(t)  # λv
    z_guess[5] = 0.001 * np.ones_like(t)  # λm

    # 尝试求解BVP
    solution = solve_bvp(
        system_ode,
        bc,
        t,
        z_guess,
        max_nodes=20000,  # 增加最大节点数
        tol=1e-2,  # 增大容差
        verbose=2  # 显示求解过程
    )

    if solution.success:
        print("求解成功!")
        t_sol = np.linspace(t_span[0], t_span[1], 500)
        z_sol = solution.sol(t_sol)
        return t_sol, z_sol, solution
    else:
        print("求解失败")
        print(f"原因: {solution.message}")
        print(f"状态: {solution.status}")
        print(f"节点数: {solution.x.size}")

        # 即使失败也返回结果用于诊断
        t_sol = np.linspace(t_span[0], t_span[1], 500)
        z_sol = solution.sol(t_sol)
        return t_sol, z_sol, solution


# 执行求解
t_sol, z_sol, solution = solve_rocket_optimal()

# 绘制结果
if t_sol is not None:
    # 提取状态变量
    h = z_sol[0]
    v = z_sol[1]
    m = z_sol[2]

    # 计算控制输入
    u_history = np.zeros_like(t_sol)
    for i in range(len(t_sol)):
        y = z_sol[:3, i]
        lambda_vec = z_sol[3:, i]
        u_history[i] = compute_optimal_control(y, lambda_vec)

    plt.figure(figsize=(14, 12))

    # 高度响应
    plt.subplot(4, 1, 1)
    plt.plot(t_sol, h, 'b-', linewidth=2)
    plt.xlabel('时间(s)')
    plt.ylabel('高度(m)')
    plt.title('火箭最优轨迹控制 - 高度响应')
    plt.grid(True)

    # 速度响应
    plt.subplot(4, 1, 2)
    plt.plot(t_sol, v, 'r-', linewidth=2)
    plt.xlabel('时间(s)')
    plt.ylabel('速度(m/s)')
    plt.title('火箭最优轨迹控制 - 速度响应')
    plt.grid(True)

    # 质量变化
    plt.subplot(4, 1, 3)
    plt.plot(t_sol, m, 'k-', linewidth=2)
    plt.xlabel('时间(s)')
    plt.ylabel('质量(kg)')
    plt.title('火箭最优轨迹控制 - 质量变化')
    plt.grid(True)

    # 控制输入
    plt.subplot(4, 1, 4)
    plt.plot(t_sol, u_history, 'g-', linewidth=2)
    plt.xlabel('时间(s)')
    plt.ylabel('控制输入u')
    plt.title('火箭最优轨迹控制 - 控制输入')
    plt.ylim(-0.1, 1.1)
    plt.grid(True)

    plt.tight_layout()
    plt.savefig('rocket_optimal_control.png')
    plt.show()

    # 输出关键指标
    max_height = np.max(h)
    final_mass = m[-1]
    fuel_used = m0 - final_mass
    print(f"最大高度: {max_height:.2f} m")
    print(f"最终质量: {final_mass:.2f} kg")
    print(f"燃料消耗: {fuel_used:.2f} kg")
    print(f"飞行时间: {t_sol[-1]:.2f} s")

    # 绘制协态变量
    plt.figure(figsize=(12, 8))
    plt.subplot(3, 1, 1)
    plt.plot(t_sol, z_sol[3], 'b-', label='λh')
    plt.xlabel('时间(s)')
    plt.ylabel('λh')
    plt.legend()
    plt.title('高度协态变量')
    plt.grid(True)

    plt.subplot(3, 1, 2)
    plt.plot(t_sol, z_sol[4], 'r-', label='λv')
    plt.xlabel('时间(s)')
    plt.ylabel('λv')
    plt.legend()
    plt.title('速度协态变量')
    plt.grid(True)

    plt.subplot(3, 1, 3)
    plt.plot(t_sol, z_sol[5], 'k-', label='λm')
    plt.xlabel('时间(s)')
    plt.ylabel('λm')
    plt.legend()
    plt.title('质量协态变量')
    plt.grid(True)

    plt.tight_layout()
    plt.savefig('rocket_costates.png')
    plt.show()

    # 检查边界条件满足情况
    print("\n边界条件检查:")
    initial_conditions = [z_sol[0, 0], z_sol[1, 0], z_sol[2, 0]]
    terminal_conditions = [z_sol[1, -1], z_sol[3, -1], z_sol[5, -1]]

    print(f"初始高度: {initial_conditions[0]:.4f} m (应为0)")
    print(f"初始速度: {initial_conditions[1]:.4f} m/s (应为0)")
    print(f"初始质量: {initial_conditions[2]:.4f} kg (应为1000)")
    print(f"终端速度: {terminal_conditions[0]:.4f} m/s (应为0)")
    print(f"终端λh: {terminal_conditions[1]:.4f} (应为0)")
    print(f"终端λm: {terminal_conditions[2]:.4f} (应为0)")

    # 绘制哈密顿函数
    H_values = np.zeros_like(t_sol)
    for i in range(len(t_sol)):
        y = z_sol[:3, i]
        lambda_vec = z_sol[3:, i]
        u = u_history[i]

        # 计算哈密顿函数
        dh_dt = y[1]
        dv_dt = c * u / y[2] - g
        dm_dt = -fuel_flow_rate * u

        # H = λ·f + L
        H = lambda_vec[0] * dh_dt + lambda_vec[1] * dv_dt + lambda_vec[2] * dm_dt
        H_values[i] = H

    plt.figure(figsize=(10, 6))
    plt.plot(t_sol, H_values, 'm-', linewidth=2)
    plt.xlabel('时间(s)')
    plt.ylabel('哈密顿函数值')
    plt.title('哈密顿函数随时间变化')
    plt.grid(True)
    plt.savefig('hamiltonian.png')
    plt.show()

else:
    print("求解失败，无法绘制结果")

import numpy as np
import matplotlib.pyplot as plt
from scipy import linalg

plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False

# 倒立摆系统参数
g = 9.81  # 重力加速度(m/s^2)
m = 0.1  # 摆锤质量(kg)
M = 1.0  # 小车质量(kg)
l = 0.5  # 摆长(m)
b = 0.1  # 小车摩擦系数

# 状态空间矩阵 (线性化后)
A = np.array([
    [0, 1, 0, 0],
    [0, -b / M, m * g / M, 0],
    [0, 0, 0, 1],
    [0, b / (M * l), -(m * g * (M + m)) / (M * l), 0]
])

B = np.array([
    [0],
    [1 / M],
    [0],
    [-1 / (M * l)]
])

# 性能指标矩阵
Q = np.diag([1, 1, 10, 10])  # 状态权重矩阵
R = np.array([[0.1]])  # 控制输入权重矩阵

# 求解代数黎卡提方程
P = linalg.solve_continuous_are(A, B, Q, R)

# 计算最优控制增益
K = np.dot(np.linalg.inv(R), np.dot(B.T, P))
print(f"最优控制增益K: {K}")


# 系统仿真
def simulate_lqr(x0, t_span, K):
    t = np.linspace(t_span[0], t_span[1], 1000)
    x = np.zeros((len(t), len(x0)))
    u_history = np.zeros(len(t))  # 存储所有时间步的控制输入

    x[0] = x0

    for i in range(0, len(t) - 1):
        dt = t[i + 1] - t[i]
        # 确保控制输入是标量
        u_val = -np.dot(K, x[i])[0]  # 提取标量值
        u_history[i] = u_val

        # 使用欧拉法积分
        dx = np.dot(A, x[i]) + np.dot(B, np.array([u_val])).flatten()
        x[i + 1] = x[i] + dt * dx

    # 计算最后一个控制输入
    u_history[-1] = -np.dot(K, x[-1])[0]  # 提取标量值

    return x, t, u_history


# 初始状态 (小角度扰动)
x0 = np.array([0, 0, 0.1, 0])  # [x, dx/dt, theta, dtheta/dt]

# 仿真
x, t, u_history = simulate_lqr(x0, (0, 10), K)

# 绘制结果
plt.figure(figsize=(14, 10))

plt.subplot(3, 1, 1)
plt.plot(t, x[:, 0], 'b-', label='小车位置')
plt.xlabel('时间(s)')
plt.ylabel('位置(m)')
plt.legend()
plt.title('倒立摆LQR控制 - 小车位置响应')

plt.subplot(3, 1, 2)
plt.plot(t, x[:, 2], 'r-', label='摆角(rad)')
plt.xlabel('时间(s)')
plt.ylabel('角度(rad)')
plt.legend()
plt.title('倒立摆LQR控制 - 摆角响应')

plt.subplot(3, 1, 3)
plt.plot(t, u_history, 'k-')
plt.xlabel('时间(s)')
plt.ylabel('控制力(N)')
plt.title('倒立摆LQR控制 - 控制输入')

plt.tight_layout()
plt.show()

# 计算性能指标 - 向量化计算
dt = t[1] - t[0]  # 时间步长

# 状态代价: x^T Q x
state_cost = np.sum(x @ Q * x, axis=1)  # 逐点计算x^T Q x

# 控制代价: u^T R u
control_cost = R[0, 0] * u_history ** 2

# 总性能指标
J = np.sum((state_cost + control_cost) * dt)

print(f"性能指标J = {J:.4f}")

import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import differential_evolution

plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False
# 被控对象传递函数：G(s) = 1/(s^2 + 2*zeta*omega_n*s + omega_n^2)
zeta = 0.5  # 阻尼比
omega_n = 1.0  # 自然频率


# 系统闭环传递函数在PID控制下的响应
def system_response(Kp, Ki, Kd, t_end=10, t_step=0.01):
    t = np.arange(0, t_end, t_step)
    # 初始化 y 为二维数组
    y = np.zeros((len(t), 2))
    e = 1.0  # 初始误差
    u_prev = 0
    e_prev = e
    e_sum = 0

    for i in range(1, len(t)):
        # PID控制律
        u = Kp * e + Ki * e_sum + Kd * (e - e_prev) / t_step

        # 模拟被控对象响应 (二阶系统)
        y_dot = np.zeros(2)
        y_dot[0] = y[i - 1, 1]
        y_dot[1] = -2 * zeta * omega_n * y[i - 1, 1] - omega_n ** 2 * y[i - 1, 0] + omega_n ** 2 * u

        # 欧拉法积分
        y[i] = y[i - 1] + t_step * y_dot

        # 更新误差
        e = 1 - y[i, 0]
        e_sum += e * t_step
        e_prev = e

    return t, y[:, 0]


# 性能指标函数 (ITAE: 时间乘绝对误差积分)
def objective_function(params):
    Kp, Ki, Kd = params
    t, y = system_response(Kp, Ki, Kd)
    error = 1 - y
    itae = np.sum(t * np.abs(error) * (t[1] - t[0]))
    return itae


# 优化参数范围
bounds = [(0, 10), (0, 5), (0, 5)]  # Kp, Ki, Kd的范围

# 使用差分进化算法优化PID参数
result = differential_evolution(objective_function, bounds, popsize=20, maxiter=100)

# 最优PID参数
Kp_opt, Ki_opt, Kd_opt = result.x
print(f"最优PID参数: Kp={Kp_opt:.4f}, Ki={Ki_opt:.4f}, Kd={Kd_opt:.4f}")
print(f"最小ITAE值: {result.fun:.4f}")

# 仿真最优PID控制器响应
t_opt, y_opt = system_response(Kp_opt, Ki_opt, Kd_opt)

# 绘制优化结果
plt.figure(figsize=(10, 6))
plt.plot(t_opt, y_opt, 'b-', linewidth=2, label='优化PID响应')
plt.axhline(y=1, color='r', linestyle='--', label='参考输入')
plt.xlabel('时间(s)')
plt.ylabel('系统输出')
plt.title('PID控制器参数优化设计结果')
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()

# 绘制误差曲线
error_opt = 1 - y_opt
plt.figure(figsize=(10, 6))
plt.plot(t_opt, error_opt, 'k-', linewidth=2)
plt.xlabel('时间(s)')
plt.ylabel('误差')
plt.title('优化PID控制器的误差响应')
plt.grid(True)
plt.tight_layout()
plt.show()