# Hamilton函数可视化：用Python动态演示极小值原理控制过程 如果你曾经在最优控制理论中挣扎过，看着那些抽象的数学公式和符号推导感到无从下手，那么这篇文章就是为你准备的。我最初接触极小值原理时，也常常困惑于如何将那些协态方程、边界条件和极值条件与实际系统的动态行为联系起来。直到我开始用Python将这些过程可视化，一切才豁然开朗。今天，我想分享如何利用SymPy进行符号计算，结合Matplotlib的动画功能，将状态变量、协态变量和控制量的动态变化关系直观地呈现出来。这不仅是一个教学工具，更是一种强大的调试和理解手段，尤其适合在Jupyter Notebook环境中进行交互式学习和探索。 我们将通过两个经典实例——火箭推力调节和倒立摆控制——来贯穿整个讲解。你会发现，当抽象的数学变成屏幕上跳动的曲线和动画时，那些深奥的理论瞬间变得触手可及。无论你是正在学习控制理论的学生，还是希望将理论应用于实践的工程师，这种“理论直观化”的方法都能为你打开一扇新的大门。 ## 1. 环境准备与核心工具链搭建 在开始动态演示之前，我们需要一个稳定且功能齐全的Python环境。我推荐使用Anaconda来管理你的Python发行版和虚拟环境，它能避免很多依赖冲突的麻烦。对于本文涉及的科学计算和可视化任务，以下几个库是必不可少的： - **SymPy**： 用于符号计算，可以轻松地推导Hamilton函数、协态方程，并进行符号微分和积分。 - **NumPy**： 提供高效的数值数组操作，是后续数值积分和矩阵运算的基础。 - **Matplotlib**： 核心绘图库，我们将用它的`animation`模块来创建动态图。 - **SciPy**： 特别是`scipy.integrate`模块，用于求解微分方程。 - **Jupyter Notebook/Lab**： 提供交互式环境，非常适合逐步执行代码、即时查看图表和动画。 你可以通过以下命令一次性安装所有依赖： ```bash pip install sympy numpy matplotlib scipy jupyter ``` 为了确保动画能在Notebook中正确显示，我们还需要进行一些简单的初始化设置。下面的代码块通常放在Notebook的第一个单元格中。 ```python # 导入必要的库并设置绘图样式 import sympy as sp import numpy as np import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation from scipy.integrate import solve_ivp from IPython.display import HTML # 设置SymPy的打印格式，让数学公式更美观 sp.init_printing(use_latex=True) # 设置Matplotlib的全局样式，让图表更专业 plt.style.use('seaborn-v0_8-darkgrid') plt.rcParams['figure.figsize'] = [10, 6] plt.rcParams['font.size'] = 12 ``` > 提示：如果你在本地运行Jupyter时遇到动画无法显示的问题，可以尝试在导入后添加`%matplotlib notebook`或`%matplotlib widget`魔术命令，以获得更丰富的交互功能。 准备工作就绪后，我们首先来回顾一下极小值原理的核心思想，并看看如何用SymPy将其“翻译”成可操作的代码。极小值原理本质上是Pontryagin对经典变分法的推广，用于处理控制量有约束的情况。其核心是构造一个Hamilton函数，然后通过一组正则方程（状态方程和协态方程）、边界条件以及极值条件来求解最优控制轨迹。 ## 2. 火箭推力调节：一个完整的符号推导与数值求解案例 让我们从一个相对简单但物理意义明确的例子开始：火箭的垂直软着陆问题。这个问题可以简化为一个一维高度控制模型。我们的目标是让火箭从某个初始高度和速度，在燃料消耗最小的约束下，平稳地降落到地面（速度为零）。 ### 2.1 问题建模与符号推导 首先，我们定义系统的状态变量和控制变量。状态变量包括高度`h`、垂直速度`v`；控制变量是推力`u`，它被限制在一个范围内，例如 `0 <= u <= u_max`。性能指标通常选择为最小化燃料消耗，这等价于最小化推力对时间的积分（或未段质量最大）。 我们用SymPy来声明这些符号变量，并构造系统的动力学方程和Hamilton函数。 ```python # 定义符号变量 t = sp.symbols('t', real=True) # 时间变量 h, v = sp.Function('h')(t), sp.Function('v')(t) # 状态：高度和速度 u = sp.Function('u')(t) # 控制：推力 lam_h, lam_v = sp.symbols('lambda_h lambda_v', cls=sp.Function) # 协态变量 g, m = sp.symbols('g m', positive=True, constant=True) # 常量：重力加速度，质量 # 状态方程 (动力学) h_dot = v v_dot = -g + u / m # 性能指标：最小化燃料消耗，即最小化 ∫ u dt。为构造Hamilton函数，我们取被积函数L = u。 L = u # 构造Hamilton函数: H = L + λ^T * f H = L + lam_h(t) * h_dot + lam_v(t) * v_dot sp.simplify(H) ``` 运行这段代码后，SymPy会输出Hamilton函数的符号表达式：`u(t) + lambda_h(t)*v(t) + lambda_v(t)*(-g + u(t)/m)`。接下来，我们需要根据极小值原理推导协态方程和极值条件。协态方程是Hamilton函数对状态变量的负偏导。 ```python # 协态方程: dλ/dt = -∂H/∂x lam_h_eq = sp.Eq(sp.diff(lam_h(t), t), -sp.diff(H, h)) lam_v_eq = sp.Eq(sp.diff(lam_v(t), t), -sp.diff(H, v)) # 显示协态方程 lam_h_eq, lam_v_eq ``` 由于高度`h`没有直接出现在H中（只通过其导数`v`间接关联），所以`∂H/∂h = 0`，这意味着`λ_h`是常数。而`∂H/∂v = λ_h`，所以`λ_v`的微分方程是 `dλ_v/dt = -λ_h`。这是一个关键的简化。 极值条件要求我们选择控制量`u(t)`，使得Hamilton函数H在每一时刻都取最小值。由于H关于`u`是线性的：`H = [1 + λ_v(t)/m] * u(t) + ...`（省略了不含u的项），而`u`有界，最优控制必然取边界值。这就是所谓的**Bang-Bang控制**或**开关控制**。 ```python # 分析极值条件：H关于u是线性的，系数为 (1 + lambda_v(t)/m) coeff_u = sp.diff(H, u) coeff_u_simplified = sp.simplify(coeff_u) coeff_u_simplified ``` 系数 `1 + lambda_v(t)/m` 被称为**开关函数**。最优控制`u*(t)`的规则是： - 如果开关函数 > 0，则选择最小的`u`（即`u = 0`）以使H最小。 - 如果开关函数 < 0，则选择最大的`u`（即`u = u_max`）。 - 如果开关函数 = 0，则处于奇异弧，控制量由其他条件决定（本例中可能不会出现）。 这个简单的分析已经揭示了最优控制的结构：它要么是最大推力，要么是零推力（发动机关闭）。接下来的挑战是求解协态变量`λ_v(t)`的轨迹，以确定具体的切换时间。 ### 2.2 数值求解与轨迹生成 为了看到具体的动态过程，我们需要给系统赋予具体的数值，并通过数值积分来求解两点边值问题。我们假设一个简化的场景：火箭质量`m=1`（归一化），重力`g=9.8`，最大推力`u_max = 20`，初始高度`h0=100`，初始速度`v0=-10`（向下），末端要求`h(tf)=0`, `v(tf)=0`，终端时间`tf`自由。 这是一个典型的**两点边值问题**：我们知道初始状态和终端状态，但不知道协态变量的初值。常用的求解方法是“打靶法”。下面，我们编写一个函数，对于猜测的协态初值`lam_h0`和`lam_v0`，积分状态和协态方程，并计算终端状态误差。 ```python import numpy as np from scipy.integrate import solve_ivp from scipy.optimize import fsolve # 系统参数 g_val = 9.8 m_val = 1.0 u_max = 20.0 # 动力学和协态方程（数值版本） def dynamics(t, y, u_max): """y = [h, v, lam_h, lam_v]""" h, v, lam_h, lam_v = y # 根据开关函数决定控制量 switching_func = 1 + lam_v / m_val if switching_func < 0: u = u_max else: u = 0.0 # 状态方程 dhdt = v dvdt = -g_val + u / m_val # 协态方程 (已推导: dlam_h/dt = 0, dlam_v/dt = -lam_h) dlam_hdt = 0.0 dlam_vdt = -lam_h return [dhdt, dvdt, dlam_hdt, dlam_vdt] # 打靶函数：给定协态初值，积分到终端时间，返回终端状态误差 def shooting_error(lam_init, h0, v0, hf_desired=0.0, vf_desired=0.0): """lam_init = [lam_h0, lam_v0]""" lam_h0, lam_v0 = lam_init y0 = [h0, v0, lam_h0, lam_v0] # 积分直到火箭触地（高度<=0）或速度变为正（开始上升，不符合软着陆） # 定义一个事件来精确检测高度为零的时刻 def hit_ground(t, y): return y[0] # 高度为零时返回0 hit_ground.terminal = True # 积分到此事件终止 hit_ground.direction = -1 # 只检测从正到负的穿越 sol = solve_ivp(dynamics, [0, 50], y0, args=(u_max,), events=[hit_ground], max_step=0.1) if sol.t_events[0].size > 0: t_final = sol.t_events[0][0] # 获取终端状态 hf, vf = sol.y[0, -1], sol.y[1, -1] # 返回高度和速度的误差 return [hf - hf_desired, vf - vf_desired] else: # 如果没有触发事件（比如火箭飞走了），返回一个很大的误差 return [1e3, 1e3] # 初始猜测和求解 h0, v0 = 100.0, -10.0 initial_guess = [0.5, -5.0] # 对lam_h0, lam_v0的猜测 # 使用fsolve求解使终端误差为零的协态初值 lam_opt, info, ier, msg = fsolve(shooting_error, initial_guess, args=(h0, v0), full_output=True) print(f"求解状态: {ier}, 消息: {msg}") print(f"最优协态初值: lam_h0 = {lam_opt[0]:.4f}, lam_v0 = {lam_opt[1]:.4f}") ``` 求解出最优的协态初值后，我们进行一次完整的积分，获取整个最优轨迹，包括状态、协态和控制量的时间序列。 ```python # 用最优协态初值进行最终积分 y0_opt = [h0, v0, lam_opt[0], lam_opt[1]] sol_opt = solve_ivp(dynamics, [0, 30], y0_opt, args=(u_max,), dense_output=True, max_step=0.05) # 从解中提取数据 t_span = np.linspace(0, sol_opt.t[-1], 500) h_opt, v_opt, lam_h_opt, lam_v_opt = sol_opt.sol(t_span) # 根据协态变量计算最优控制序列 switching_func_opt = 1 + lam_v_opt / m_val u_opt = np.where(switching_func_opt < 0, u_max, 0.0) # 计算Hamilton函数值（沿最优轨迹应为常数） H_opt = u_opt + lam_h_opt * v_opt + lam_v_opt * (-g_val + u_opt / m_val) ``` 现在，我们已经有了所有数据，可以开始制作我们的第一个动态演示了。 ### 2.3 创建Matplotlib动画：观察动态演变 静态图表很难展现系统随时间演变的过程。我们将创建四个子图，分别展示高度/速度、协态变量、控制推力以及Hamilton函数值随时间的变化。动画会有一个移动的垂直线（时间指针），清晰地指示当前时刻各变量的取值。 ```python # 创建图形和坐标轴 fig, axs = plt.subplots(2, 2, figsize=(12, 8)) fig.suptitle('火箭软着陆最优控制过程可视化', fontsize=16) # 子图1：状态变量 (高度和速度) ax1 = axs[0, 0] line_h, = ax1.plot([], [], 'b-', lw=2, label='高度 h(t)') line_v, = ax1.plot([], [], 'r-', lw=2, label='速度 v(t)') ax1.axhline(y=0, color='k', linestyle='--', alpha=0.5) # 地面线 ax1.set_xlabel('时间 (s)') ax1.set_ylabel('状态值') ax1.set_title('状态变量轨迹') ax1.legend() ax1.grid(True) # 子图2：协态变量 ax2 = axs[0, 1] line_lam_h, = ax2.plot([], [], 'g-', lw=2, label=r'$\lambda_h$') line_lam_v, = ax2.plot([], [], 'm-', lw=2, label=r'$\lambda_v$') ax2.set_xlabel('时间 (s)') ax2.set_ylabel('协态值') ax2.set_title('协态变量轨迹') ax2.legend() ax2.grid(True) # 子图3：控制推力 ax3 = axs[1, 0] line_u, = ax3.plot([], [], 'k-', lw=2, drawstyle='steps-post', label='推力 u(t)') ax3.set_xlabel('时间 (s)') ax3.set_ylabel('推力') ax3.set_title('最优控制序列 (Bang-Bang)') ax3.set_ylim([-1, u_max+2]) ax3.legend() ax3.grid(True) # 子图4：Hamilton函数值 (应为常数) ax4 = axs[1, 1] line_H, = ax4.plot([], [], 'c-', lw=2, label='Hamiltonian H') ax4.set_xlabel('时间 (s)') ax4.set_ylabel('H 值') ax4.set_title('Hamilton函数沿最优轨线') ax4.legend() ax4.grid(True) # 在所有子图上添加一条移动的垂直线（时间指针） vlines = [] for ax in [ax1, ax2, ax3, ax4]: vline = ax.axvline(x=0, color='orange', linestyle='--', alpha=0.7) vlines.append(vline) # 初始化函数：设置绘图数据为空 def init(): for line in [line_h, line_v, line_lam_h, line_lam_v, line_u, line_H]: line.set_data([], []) return [line_h, line_v, line_lam_h, line_lam_v, line_u, line_H] + vlines # 动画更新函数 def update(frame): # frame是当前帧索引，我们将其映射到时间 current_time_index = int(frame * len(t_span) / 200) # 总共200帧 current_time_index = min(current_time_index, len(t_span)-1) t_current = t_span[:current_time_index+1] # 更新状态变量曲线 line_h.set_data(t_span[:current_time_index+1], h_opt[:current_time_index+1]) line_v.set_data(t_span[:current_time_index+1], v_opt[:current_time_index+1]) # 更新协态变量曲线 line_lam_h.set_data(t_span[:current_time_index+1], lam_h_opt[:current_time_index+1]) line_lam_v.set_data(t_span[:current_time_index+1], lam_v_opt[:current_time_index+1]) # 更新控制曲线 (注意阶梯图的绘制方式) line_u.set_data(t_span[:current_time_index+1], u_opt[:current_time_index+1]) # 更新Hamilton函数曲线 line_H.set_data(t_span[:current_time_index+1], H_opt[:current_time_index+1]) # 更新所有子图的时间指针位置 current_t = t_span[current_time_index] for vline in vlines: vline.set_xdata([current_t, current_t]) return [line_h, line_v, line_lam_h, line_lam_v, line_u, line_H] + vlines # 创建动画对象 ani = FuncAnimation(fig, update, frames=200, init_func=init, blit=True, interval=50, repeat=False) # 在Jupyter Notebook中显示动画 plt.tight_layout() # 如果你在脚本中运行，可以使用 ani.save('rocket_landing.mp4') 保存为视频 HTML(ani.to_jshtml()) # 在Notebook中内嵌显示 ``` 运行这段代码后，你会看到一个四幅子图的动画。时间线从左向右移动，你可以观察到： 1. **状态图**：火箭高度从100米平滑下降至0米，速度从-10 m/s（向下）逐渐减小，并在着陆时刻精确变为0。 2. **协态图**：`λ_h`保持为常数（正如我们符号推导所预测的），`λ_v`则线性变化（因为其导数是`-λ_h`）。 3. **控制图**：推力`u(t)`在0和`u_max`之间切换，清晰地展示了Bang-Bang控制。切换时刻由开关函数`1+λ_v(t)/m`的过零点决定。 4. **Hamilton函数图**：其值在整个轨迹上基本保持常数，这是极小值原理的一个重要性质（对于自治系统、终端时间自由的问题，H在末端应为0；对于时间固定问题，H为常数）。图中微小的波动可能源于数值积分误差，但整体趋势验证了理论。 这个动画生动地展示了理论（协态方程决定开关函数）如何直接决定了控制器（推力）的行为，而控制器行为又驱动了系统状态（高度和速度）达到期望的终端条件。你可以尝试修改初始条件（如`h0`, `v0`）或系统参数（如`u_max`），重新运行打靶法和动画，观察最优轨迹如何变化。 ## 3. 倒立摆控制：非线性系统的可视化挑战与技巧 倒立摆是一个经典的非线性、不稳定系统，其控制问题比火箭着陆更具挑战性，也更能体现可视化工具的价值。我们将考虑一个简单的车杆式倒立摆模型，目标是通过施加在小车上的水平力`u`，使摆杆从自然下垂的静止状态（不稳定平衡点）摆动并稳定在竖直向上的位置。 ### 3.1 非线性动力学与符号线性化 倒立摆的状态通常包括小车位置`x`、小车速度`v`、摆杆角度`θ`和角速度`ω`。其动力学方程是非线性的。为了应用极小值原理（通常用于求解最优控制律），一个常见的方法是先在其目标平衡点（`θ=0`）附近进行线性化，然后对线性化系统设计线性二次型调节器（LQR），这本质上是极小值原理在二次型性能指标下的特例。 首先，我们用SymPy推导非线性方程，然后在`θ=0, ω=0`附近进行线性化。 ```python # 定义倒立摆系统的符号变量 # 状态变量 x, v, theta, omega = sp.Function('x')(t), sp.Function('v')(t), sp.Function('theta')(t), sp.Function('omega')(t) # 控制输入 F = sp.Function('F')(t) # 系统参数 M, m, l, g = sp.symbols('M m l g', positive=True, constant=True) # 小车质量，摆杆质量，摆长一半，重力加速度 # 非线性动力学方程 (来自拉格朗日方程或牛顿-欧拉方程) # 这里直接给出标准形式： # (M+m)*x'' + m*l*cos(theta)*theta'' - m*l*sin(theta)*(theta')^2 = F # m*l*cos(theta)*x'' + (m*l^2)*theta'' - m*g*l*sin(theta) = 0 # 为了用SymPy处理，我们将其写成一阶形式： # 令 x1=x, x2=v=x', x3=theta, x4=omega=theta' # 则方程组为： # x1' = x2 # (M+m)*x2' + m*l*cos(x3)*x4' - m*l*sin(x3)*x4^2 = F # x3' = x4 # m*l*cos(x3)*x2' + m*l^2*x4' - m*g*l*sin(x3) = 0 # 这是一个微分代数方程组(DAE)。更简单的方法是直接使用已知的线性化模型。 # 在平衡点(theta=0, omega=0)附近线性化后，状态空间方程为： # dx/dt = A * x + B * u # 其中 x = [x, v, theta, omega]^T, u = F # 定义符号矩阵 A = sp.Matrix([[0, 1, 0, 0], [0, 0, -m*g/M, 0], [0, 0, 0, 1], [0, 0, (M+m)*g/(M*l), 0]]) B = sp.Matrix([[0], [1/M], [0], [-1/(M*l)]]) A, B ``` 线性化后的系统矩阵`A`和`B`是常数矩阵，这使得我们可以使用LQR理论来求解最优状态反馈控制律 `u = -K * x`，其中增益矩阵`K`通过求解代数Riccati方程得到。这个`K`实际上就是极小值原理应用于线性二次型问题所得到的最优解。 ### 3.2 设计LQR控制器并模拟闭环响应 我们将使用`scipy.linalg`中的`solve_continuous_are`函数来求解Riccati方程，从而得到最优反馈增益`K`。然后，我们模拟在初始角度有一个小扰动（例如`θ(0)=0.1 rad`）下，闭环系统的响应。 ```python import numpy as np from scipy.linalg import solve_continuous_are from scipy.integrate import solve_ivp # 赋予参数具体数值 M_val = 1.0 # 小车质量 (kg) m_val = 0.3 # 摆杆质量 (kg) l_val = 0.5 # 摆杆质心到转轴的长度 (m) g_val = 9.8 # 重力加速度 (m/s^2) # 构建数值的A, B矩阵 A_num = np.array([[0, 1, 0, 0], [0, 0, -m_val*g_val/M_val, 0], [0, 0, 0, 1], [0, 0, (M_val+m_val)*g_val/(M_val*l_val), 0]]) B_num = np.array([[0], [1/M_val], [0], [-1/(M_val*l_val)]]).reshape(-1, 1) # 确保是列向量 # 定义LQR权重矩阵 Q = np.diag([1.0, 0.1, 10.0, 0.1]) # 状态权重，对角阵。给角度(theta)较大的惩罚，使其快速稳定。 R = np.array([[0.1]]) # 控制输入权重，标量。 # 求解连续时间代数Riccati方程 (CARE): A^T P + P A - P B R^{-1} B^T P + Q = 0 P = solve_continuous_are(A_num, B_num, Q, R) # 计算最优反馈增益矩阵 K = R^{-1} B^T P K = np.linalg.inv(R) @ B_num.T @ P print(f"最优反馈增益矩阵 K:\n{K}") # 闭环系统动力学: dx/dt = (A - B*K) * x A_cl = A_num - B_num @ K def closed_loop_dynamics(t, x_vec): """闭环系统微分方程""" return A_cl @ x_vec # 初始状态：小车在原点静止，摆杆有一个小的初始角度偏移 x0 = np.array([0.0, 0.0, 0.1, 0.0]) # [x, v, theta, omega] # 模拟时间 t_span_sim = (0, 5) t_eval = np.linspace(0, 5, 500) # 数值积分 sol = solve_ivp(closed_loop_dynamics, t_span_sim, x0, t_eval=t_eval, method='RK45') t_sim = sol.t x_sim, v_sim, theta_sim, omega_sim = sol.y # 计算控制输入序列 u_sim = - (K @ sol.y).flatten() ``` 现在，我们有了闭环系统的时间响应数据。接下来，我们创建一个更丰富的动画，不仅显示状态和控制的曲线，还用一个简单的图形来示意倒立摆本身的运动。 ### 3.3 创建倒立摆运动动画 这个动画将包含两个部分：左侧是一个动态的倒立摆示意图，右侧是状态和控制量的曲线图。时间指针将同步移动。 ```python # 创建图形和坐标轴布局 fig2 = plt.figure(figsize=(14, 6)) # 左侧：倒立摆示意图 ax_pend = plt.subplot2grid((2, 3), (0, 0), rowspan=2) ax_pend.set_xlim(-2, 2) ax_pend.set_ylim(-0.5, 1.5) ax_pend.set_aspect('equal') ax_pend.set_title('倒立摆实时状态') ax_pend.grid(True) # 绘制轨道和小车 ax_pend.plot([-1.5, 1.5], [0, 0], 'k-', lw=3) # 轨道 cart_width, cart_height = 0.3, 0.15 cart = plt.Rectangle((-cart_width/2, -cart_height/2), cart_width, cart_height, fc='skyblue', ec='black') ax_pend.add_patch(cart) # 摆杆 pendulum, = ax_pend.plot([], [], 'r-', lw=3) # 摆锤 bob, = ax_pend.plot([], [], 'ko', markersize=10) # 右侧：状态和控制曲线 (分四个小图) ax_x = plt.subplot2grid((2, 3), (0, 1)) ax_v = plt.subplot2grid((2, 3), (0, 2)) ax_theta = plt.subplot2grid((2, 3), (1, 1)) ax_u = plt.subplot2grid((2, 3), (1, 2)) axes_curves = [ax_x, ax_v, ax_theta, ax_u] curve_titles = ['小车位置 x(t)', '小车速度 v(t)', '摆杆角度 θ(t)', '控制力 u(t)'] curve_data = [x_sim, v_sim, theta_sim, u_sim] curve_lines = [] curve_vlines = [] for i, (ax, title, data) in enumerate(zip(axes_curves, curve_titles, curve_data)): line, = ax.plot([], [], 'b-', lw=2) curve_lines.append(line) ax.set_xlabel('时间 (s)') ax.set_ylabel(title.split(' ')[0]) ax.set_title(title) ax.grid(True) ax.set_xlim(0, t_span_sim[1]) # 为每个曲线图添加时间指针 vline = ax.axvline(x=0, color='orange', linestyle='--', alpha=0.7) curve_vlines.append(vline) # 初始化函数 def init_anim(): cart.set_xy((-cart_width/2, -cart_height/2)) pendulum.set_data([], []) bob.set_data([], []) for line in curve_lines: line.set_data([], []) return [cart, pendulum, bob] + curve_lines + curve_vlines # 动画更新函数 def update_anim(frame): # 计算当前帧对应的时间索引 idx = int(frame * len(t_sim) / 200) idx = min(idx, len(t_sim)-1) t_current = t_sim[idx] # 更新倒立摆图示 x_cart = x_sim[idx] theta_current = theta_sim[idx] # 摆杆端点坐标 pend_x = x_cart + l_val * np.sin(theta_current) pend_y = l_val * np.cos(theta_current) # 假设摆杆铰链在小车中心上方 pendulum.set_data([x_cart, pend_x], [0, pend_y]) bob.set_data([pend_x], [pend_y]) # 更新小车位置 cart.set_xy((x_cart - cart_width/2, -cart_height/2)) # 更新曲线图 for i, (line, data) in enumerate(zip(curve_lines, curve_data)): line.set_data(t_sim[:idx+1], data[:idx+1]) # 更新所有曲线图的时间指针 for vline in curve_vlines: vline.set_xdata([t_current, t_current]) return [cart, pendulum, bob] + curve_lines + curve_vlines # 创建动画 ani2 = FuncAnimation(fig2, update_anim, frames=200, init_func=init_anim, blit=True, interval=50, repeat=False) plt.tight_layout() HTML(ani2.to_jshtml()) ``` 运行这个动画，你会看到左侧的倒立摆从倾斜状态开始，在小车来回移动的控制作用下，逐渐摆动并最终稳定在竖直向上的位置。右侧的四个图表分别展示了： - **位置x(t)**：小车为了平衡摆杆而进行的移动轨迹。 - **速度v(t)**：小车的速度变化。 - **角度θ(t)**：摆杆角度从初始的0.1弧度衰减到0的过程，可能伴有超调。 - **控制力u(t)**：施加在小车上的水平力，它是由状态反馈 `u = -Kx` 计算得出的连续信号。 这个动画清晰地展示了LQR控制器如何通过全状态反馈，将不稳定的倒立摆系统镇定下来。你可以通过调整LQR的权重矩阵`Q`和`R`来观察控制器性能的变化。例如，增大`Q`中对应角度`θ`的权重，会使控制器更激进地纠正角度偏差，可能导致控制力更大或小车移动更剧烈。 ## 4. 高级话题：从Bang-Bang到奇异控制与数值求解技巧 在前面的火箭例子中，我们得到了Bang-Bang控制。但在某些问题中，当Hamilton函数对控制量的导数（即开关函数）在某个时间区间内恒为零时，就会进入**奇异弧**。在奇异弧上，仅凭极值条件无法唯一确定最优控制，需要用到高阶条件。虽然我们无法在简短的文章中深入推导奇异控制，但我们可以通过一个数值例子来展示如何检测和处理这种情况。 考虑一个简单的双积分器系统（例如，一个在无摩擦平面上受外力控制的质点），其性能指标是时间与控制能量加权和的最小化。在某些权重下，最优解可能包含奇异弧。 ```python # 示例：双积分器系统，性能指标 J = ∫ (1 + ρ*u^2) dt，寻找可能存在的奇异控制 rho = 0.1 # 控制能量权重 # 状态: [位置, 速度], 控制: 加速度 # 状态方程: dx = v, dv = u # Hamilton函数: H = 1 + ρ*u^2 + λ1*v + λ2*u # 开关函数: ∂H/∂u = 2*ρ*u + λ2 # 常规情况: u = -λ2/(2ρ) (无约束时) # 但若λ2恒为零一段时间，则进入奇异弧，此时控制由d(∂H/∂u)/dt = 0等条件决定。 # 这是一个更复杂的边值问题。我们可以使用数值优化库（如GEKKO）或直接转录法来求解。 # 以下代码展示如何使用scipy.optimize直接求解一个离散化的问题。 from scipy.optimize import minimize import matplotlib.pyplot as plt N = 100 # 时间离散点数 T = 5.0 # 固定终端时间 dt = T / (N-1) # 决策变量：每个时间点的状态x, v和控制u # 变量顺序: [x0, x1, ..., x_{N-1}, v0, v1, ..., v_{N-1}, u0, u1, ..., u_{N-2}] # 注意：控制变量比状态变量少一个，因为末端控制通常不影响性能指标（对于本问题）。 def objective(z): """目标函数: 离散近似 J ≈ Σ (1 + ρ*u_k^2) * dt """ u = z[2*N : 2*N + (N-1)] # 控制序列 J = np.sum(1 + rho * u**2) * dt return J def constraint_dynamics(z): """动力学约束: x_{k+1} = x_k + v_k*dt, v_{k+1} = v_k + u_k*dt """ x = z[:N] v = z[N:2*N] u = z[2*N: 2*N + (N-1)] cons = [] for k in range(N-1): cons.append(x[k+1] - (x[k] + v[k]*dt)) # 位置更新约束 cons.append(v[k+1] - (v[k] + u[k]*dt)) # 速度更新约束 return np.array(cons) def constraint_boundary(z): """边界条件约束: x0=0, v0=0, x_{N-1}=1, v_{N-1}=0 """ x = z[:N] v = z[N:2*N] return np.array([x[0], v[0], x[-1]-1.0, v[-1]]) # 初始猜测：线性插值 x_guess = np.linspace(0, 1, N) v_guess = np.ones(N) * 0.2 u_guess = np.zeros(N-1) z0 = np.concatenate([x_guess, v_guess, u_guess]) # 设置约束 cons = [{'type': 'eq', 'fun': constraint_dynamics}, {'type': 'eq', 'fun': constraint_boundary}] # 调用优化器求解 result = minimize(objective, z0, constraints=cons, method='SLSQP', options={'maxiter': 1000, 'disp': True}) if result.success: z_opt = result.x x_opt = z_opt[:N] v_opt = z_opt[N:2*N] u_opt = z_opt[2*N: 2*N + (N-1)] t_opt = np.linspace(0, T, N) # 绘制结果 fig3, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6)) ax1.plot(t_opt, x_opt, 'b-', label='位置 x(t)') ax1.plot(t_opt, v_opt, 'r-', label='速度 v(t)') ax1.set_ylabel('状态') ax1.legend() ax1.grid(True) # 控制信号，注意时间点对齐 t_u = np.linspace(0, T-dt, N-1) ax2.step(t_u, u_opt, where='post', color='k', label='控制 u(t)') ax2.set_xlabel('时间 (s)') ax2.set_ylabel('控制输入') ax2.legend() ax2.grid(True) plt.suptitle('双积分器系统时间-能量最优控制 (数值优化解)') plt.tight_layout() plt.show() # 检查开关函数 (λ2) 的行为 (通过协态方程近似) # 对于这个问题，协态方程: dλ1/dt = 0, dλ2/dt = -λ1 # 边界条件: λ1(T)自由, λ2(T)=0 (因为末端状态固定，φ=0) # 我们可以反向积分协态方程。但这里我们采用一个更简单的方法：检查数值解中u的形态。 # 如果存在一段区间u为常数（且不是边界值），则可能对应奇异弧。 print("控制序列的前10个值:", u_opt[:10]) print("控制序列的统计:", f"均值={np.mean(u_opt):.3f}, 标准差={np.std(u_opt):.3f}") else: print("优化失败:", result.message) ``` 这段代码使用了直接转录法，将连续时间最优控制问题离散化为一个非线性规划问题，然后用`scipy.optimize.minimize`求解。虽然这个方法对于简单问题有效，但对于更复杂或高维问题，可能需要更专业的工具，如GEKKO、CasADi或Pyomo。 运行结果可能会显示一个平滑的控制曲线，而不是Bang-Bang控制。通过分析控制曲线的形状（例如，是否存在一段平台期），并结合协态变量的近似计算，可以初步判断是否存在奇异弧。要严格证明，则需要计算开关函数及其高阶导数。 在可视化方面，你可以将数值优化得到的状态、协态（如果通过伴随方程计算得到）和控制量，用类似前面的动画方法展示出来。这能帮助你直观地理解奇异控制与Bang-Bang控制在轨迹形态上的区别。 通过这两个实例——火箭的Bang-Bang控制和倒立摆的连续状态反馈控制——我们看到了Python在将最优控制理论可视化方面的强大能力。从符号推导到数值求解，再到动态图形展示，这一完整的工作流程不仅加深了对极小值原理的理解，也为我们设计、分析和调试实际控制器提供了直观的工具。