当前位置：首页 > news >正文

伪谱法求解最优控制问题（附Python代码）

news 来源：原创 2025/4/23 12:54:09

一、伪谱法

伪谱法作为最优控制领域的经典算法，其基本原理这里不做基本介绍，主要讲解代码实现部分，由于CasADi库本身不自带生成LGL节点和微分矩阵的函数，因此我们首先定义PseudoSpectral对象，包括生成LGL节点和微分矩阵函数的方法。

import numpy as np
from numpy.polynomial import legendre as L
from scipy.special import legendre
class PseudoSpectral:"""伪谱法类，基于Legendre-Gauss-Lobatto配点"""def __init__(self, N, tau_interval=[0, 1]):"""初始化伪谱法对象参数:N: int, 多项式阶数tau_interval: list, 时间区间 [t0, tf]"""self.N = N  # 多项式阶数self.tau_interval = tau_interval# 计算LGL节点和权重（标准区间[-1,1]上）self.xi = self._find_LGL_points()  # 标准区间上的配点self.omega = self._get_LGL_weights()  # 标准区间上的权重# 变换到实际时间区间self.tau = self._transform_to_tau(self.xi)  # 实际时间区间上的配点self.weights = self._transform_weights(self.omega)  # 实际时间区间上的权重# 计算微分矩阵self.D = self._get_differentiation_matrix()# 计算基函数self.basis = self._compute_basis_functions()def _get_legendre_coeffs(self, n):"""获取n阶Legendre多项式的系数"""if n == 0:return np.array([1])elif n == 1:return np.array([1, 0])else:p_prev = np.poly1d([1])p_curr = np.poly1d([1, 0])for k in range(2, n + 1):p_next = ((2 * k - 1) * np.polymul([1, 0], p_curr) - (k - 1) * p_prev) / kp_prev = p_currp_curr = p_nextreturn p_curr.coefficientsdef _find_LGL_points(self):"""计算LGL节点"""if self.N < 2:return np.array([-1, 1])coefs = self._get_legendre_coeffs(self.N)P = np.poly1d(coefs)dP = P.deriv()roots = np.roots(dP)roots = np.real(roots[np.abs(np.imag(roots)) < 1e-15])roots = roots[np.abs(roots) <= 1]nodes = np.concatenate(([-1], roots, [1]))return np.sort(nodes)def _get_LGL_weights(self):"""计算LGL权重"""if self.N < 2:return np.array([1, 1])x = self.xiP = np.poly1d(self._get_legendre_coeffs(self.N))w = np.zeros_like(x)for i in range(len(x)):w[i] = 2 / (self.N * (self.N + 1) * (P(x[i])) ** 2)return wdef _transform_to_tau(self, xi):"""将标准区间[-1,1]的点变换到实际时间区间"""t0, tf = self.tau_intervalreturn ((tf - t0) * xi + (tf + t0)) / 2def _transform_weights(self, omega):"""变换权重到实际时间区间"""t0, tf = self.tau_intervalreturn omega * (tf - t0) / 2def _get_differentiation_matrix(self):N = self.ND = np.zeros((N + 1, N + 1))xi = self.xi  # LGL节点# 获取N阶Legendre多项式P_N = np.poly1d(self._get_legendre_coeffs(N))# 计算每个节点上的Legendre多项式值D[0, 0] = -N * (N + 1) / 4D[N, N] = N * (N + 1) / 4# 对角元素 (除端点外) - 按照MATLAB版本处理for i in range(1, N):  # 对应MATLAB的2:ND[i, i] = 0# 非对角元素for i in range(N + 1):for j in range(N + 1):if i != j:D[i, j] = legendre(N)(xi[i]) / (legendre(N)(xi[j]) * (xi[i] - xi[j]))return Ddef _compute_basis_functions(self):"""计算基函数"""n = self.Nbasis = np.zeros((n + 1, n + 1))x = self.xifor i in range(n + 1):# 构造拉格朗日基函数p = 1for j in range(n + 1):if i != j:p = np.polymul(p, [1 / (x[i] - x[j]), -x[j] / (x[i] - x[j])])basis[:, i] = np.polyval(p, x)return basisdef interpolate(self, y, tau_new):"""在给定点上进行插值参数:y: 节点上的函数值tau_new: 需要插值的点"""# 将tau_new变换回标准区间t0, tf = self.tau_intervalxi_new = 2 * (tau_new - t0) / (tf - t0) - 1# 计算插值结果result = np.zeros_like(xi_new)for i in range(self.N + 1):# 构造拉格朗日基函数p = 1for j in range(self.N + 1):if i != j:p = np.polymul(p, [1 / (self.xi[i] - self.xi[j]),-self.xi[j] / (self.xi[i] - self.xi[j])])result += y[i] * np.polyval(p, xi_new)return resultdef integrate(self, f):"""使用LGL求积计算函数的积分参数:f: 节点上的函数值"""return np.sum(self.weights * f)def differentiate(self, f):"""计算函数在节点上的导数值参数:f: 节点上的函数值"""return np.dot(self.D, f)def verify_sine_derivative(self):t = self.tau# 2. 计算原函数在LGL节点上的值y1 = np.sin(t)*2/(self.tau_interval[1]-self.tau_interval[0])# 3. 用微分矩阵计算导数 (需要考虑时间变换)dy1_numerical = self.D @ y1dy1_analytical = np.cos(t)        # 5. 打印比较结果print('s')# 运行测试
if __name__ == "__main__":N = 5t0, tf =0, 1ps = PseudoSpectral(N, [t0, tf])# 验证导数ps.verify_sine_derivative()

现在结合一个具体的例子，使用伪谱法的应用过程。假设初始状态为 $[x_1=0,x_2=1]$ ，状态转移方程为

$\dot{x}_1=(1-x_2^{2})x_1-x_2+u,\dot{x}_2=x_1$

状态约束：

$x_1 \geq -0.25$

控制约束：

$-1\leq u\leq 1$

指标函数为：

$L=\int _{t_0}^{t_f}(x_{1}^{2}+x_{2}^{2}+u^{2})dt$

根据伪谱法，假设我们取20阶LGL节点，那么对应42个状态变量，21个控制变量，根据重要的动力学约束，构建等式约束。

$\frac{dx}{dt}=\frac{dx}{d\tau}\frac{d\tau}{dt}=\frac{2}{t_f-t_0}\frac{dx}{d\tau}\rightarrow \frac{dx}{d\tau}=\frac{t_f-t_0}{2}f(x,u,t)$

$\frac{dx_1{(\tau_k)}}{d\tau}=\sum_{j=1}^{N}D_{kj}x(\tau_k)$

因此得到等式约束 $\frac{t_f-t_0}{2}f(x,u,t)=\sum _{j=1}^{N}D_{kj}x(\tau_j)$

整个问题的优化变量 $[x_1(t_0),x_2(t_0),u(t_0)...,x_1(t_f),x_2(t_f),u(t_f)]$ ，得到微分矩阵 $\boldsymbol{D}_{21\times21}$

第一个动力学约束为

$(1-x_2^{2}(t_0))x_1(t_0)-x_2(t_0)+u(t_0)=\sum_{j=1}^{21}\boldsymbol{D}_{1j}x_1(t_j)$

$x_1(t_0)=\sum_{j=1}^{21}\boldsymbol{D}_{1j}x_2(t_j)$

依次类推得到42个等式的动力学约束，加上初始的2个边界约束，一共44个等式约束

现在根据指标的计算

$L=\frac{t_f-t_0}{2}\sum _{i=0}^{20}w(t_i)(x_1^2(t_i)+x_2^2(t_i)+u^2(t_i))$

结合Python的CasADi库来求解，代码如下图

# optimal_control.py
from PseudoSpectral import PseudoSpectral
import casadi as ca
import numpy as np
import matplotlib.pyplot as pltclass OptimalControl:def __init__(self, N=6):"""初始化最优控制问题求解器参数:N: int, LGL配点阶数"""self.ps = PseudoSpectral(N, [-1, 1])  # 使用标准区间self.N = N# 时间区间self.T = 10.0  # 终端时间# 获取LGL节点和权重self.tau = self.ps.xi  # 配点(标准区间)self.weights = self.ps.omega  # 权重self.D = self.ps.D  # 微分矩阵def solve(self):"""求解最优控制问题"""# 声明状态和控制变量x1 = ca.SX.sym('x1')x2 = ca.SX.sym('x2')x = ca.vertcat(x1, x2)u = ca.SX.sym('u')# 系统动力学xdot = ca.vertcat((1 - x2 ** 2) * x1 - x2 + u, x1)f = ca.Function('f', [x, u], [xdot], ['x', 'u'], ['xdot'])# 目标函数L = x1 ** 2 + x2 ** 2 + u ** 2L_fun = ca.Function('L', [x1, x2, u], [L])# 创建连续时间动力学函数f = ca.Function('f', [x, u], [xdot, L], ['x', 'u'], ['xdot', 'L'])# 构建NLP问题# 决策变量X = ca.SX.sym('X', 2, self.N + 1)  # 状态变量在所有节点上的值U = ca.SX.sym('U', self.N + 1)  # 控制变量在所有节点上的值# 将决策变量转换为向量w = ca.vertcat(ca.reshape(X, -1, 1), U)# 目标函数J = 0for i in range(self.N + 1):J += self.weights[i] * L_fun(X[0,i], X[1,i], U[i]) * self.T/2# 约束g = []# 动力学约束for i in range(2):  # 对每个状态分量# 使用微分矩阵计算数值导数: dx/dtau = D*xdx_numerical = ca.mtimes(self.D, X[i, :].T)# 在每个节点上比较数值导数和模型导数for k in range(self.N + 1):x_current = [X[0, k], X[1, k]]u_current = U[k]# 计算模型动力学 (注意时间变换)dx_model = f(x=ca.vertcat(x_current[0], x_current[1]),u=u_current)['xdot'][i] * self.T / 2  # 时间变换系数# 动力学约束：数值导数 = 模型导数g.append(dx_numerical[k] - dx_model)# 边界条件g.append(X[0, 0] - 0)  # x1(0) = 0g.append(X[1, 0] - 1)  # x2(0) = 1# 构建NLPnlp = {'x': w, 'f': J, 'g': ca.vertcat(*g)}# 设置求解器选项opts = {'ipopt.print_level': 5, 'print_time': 1}solver = ca.nlpsol('solver', 'ipopt', nlp, opts)# 设置初值和约束边界w0 = self._generate_initial_guess()lbw = -np.inf * np.ones(w.shape[0])  # 决策变量下界ubw = np.inf * np.ones(w.shape[0])  # 决策变量上界# 控制约束for i in range(self.N + 1):idx = 2 * (self.N + 1) + ilbw[idx] = -1ubw[idx] = 1# 状态约束for i in range(self.N + 1):lbw[2 * i] = -0.25  # x1 下界lbw[0] = 0  # X_0 = 0ubw[0] = 0lbw[1] = 1  # X_1 = 1ubw[1] = 1# 约束边界lbg = np.zeros(len(g))ubg = np.zeros(len(g))# 求解NLPsol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)# 提取结果w_opt = sol['x'].full().flatten()# 正确提取状态和控制变量x1_opt = w_opt[::2][:(self.N + 1)]  # 隔一个取一个提取x1x2_opt = w_opt[1::2][:(self.N + 1)]  # 隔一个取一个提取x2x_opt = np.vstack((x1_opt, x2_opt))  # 组装状态向量u_opt = w_opt[2 * (self.N + 1):]  # 提取控制量 # 提取x2return x_opt, u_optdef plot_results(self, x_opt, u_opt):"""使用插值多项式绘制连续轨迹"""import matplotlib.pyplot as plttgrid = np.linspace(0, self.T, self.N + 1)plt.figure(1)plt.clf()plt.plot(tgrid, x_opt[0], '--')plt.plot(tgrid, x_opt[1], '-')plt.plot(tgrid, u_opt, '-.')plt.xlabel('t')plt.legend(['x1', 'x2', 'u'])plt.grid()plt.show()def interpolate(self, y, t_new):"""使用拉格朗日插值重构连续函数参数:y: 在LGL节点上的函数值t_new: 需要插值的新时间点返回:y_new: 插值后的函数值"""# 将时间点映射到标准区间 [-1,1]xi_new = 2 * t_new / self.T - 1# 初始化插值结果y_new = np.zeros_like(xi_new)# 计算拉格朗日基函数并插值for i in range(self.N + 1):# 构造第i个拉格朗日基函数li = np.ones_like(xi_new)for j in range(self.N + 1):if j != i:li *= (xi_new - self.tau[j]) / (self.tau[i] - self.tau[j])# 累加插值结果y_new += y[i] * lireturn y_newdef _generate_initial_guess(self):"""生成更好的初始猜测"""w0 = []# 1. 状态变量初值x1_init = np.zeros(self.N + 1)  # x1 满足边界条件x2_init = np.linspace(1, 0, self.N + 1)  # x2 满足边界条件# 2. 使用动力学方程估计控制初值u_init = np.zeros(self.N + 1)for i in range(self.N + 1):# 计算状态导数if i > 0 and i < self.N:dx1_dt = np.dot(self.D, x1_init)[i] * 2 / self.Tx1_i = x1_init[i]x2_i = x2_init[i]# 从动力学方程: dx1/dt = (1-x2^2)x1 - x2 + uu_init[i] = dx1_dt - (1 - x2_i ** 2) * x1_i + x2_ielse:# 端点处使用平滑过渡u_init[i] = 0# 限制控制在约束范围内u_init = np.clip(u_init, -1, 1)# 组合所有初值w0.extend(x1_init)w0.extend(x2_init)w0.extend(u_init)return np.array(w0)# 测试代码
if __name__ == "__main__":# 创建求解器ocp = OptimalControl(N=20)# 求解问题x_opt, u_opt = ocp.solve()# 绘制结果ocp.plot_results(x_opt, u_opt)

最终得到