先别急着去翻那些满屏 Legendre 多项式、配点、NLP 转换的论文这篇教程我用一个最经典的“双积分器”问题把高斯伪谱法从数学到代码完整走一遍。全程用 Python CasADi 实现代码可以直接复制跑通跑完你就能拿同一套流程去套你自己的轨迹优化问题。这篇文章适合刚接触最优控制、想搞明白伪谱法到底在干什么、或者只是想把 CasADi 用起来的读者看完你会觉得原来所谓的高斯伪谱法核心就三件事——选点、插值、把微分方程变成代数方程。1. 轨迹优化到底在做什么1.1 从一个最简单的例子说起轨迹优化通俗点说就是在满足物理规律的前提下找一条“最好”的运动轨迹。导航软件告诉你走哪条路那是路径规划轨迹优化则更进一步告诉你每个时刻油门踩多少、刹车什么时候踩怎么开最省油、怎么飞最平稳。我选的这个例子是所有轨迹优化教材里的“hello world”——双积分器问题。你可以理解为一辆在无摩擦直轨道上的小车位置是 (x)速度是 (v)控制量是加速度 (u)。它的动力学只有两个方程[ \dot{x} v, \quad \dot{v} u ]任务很朴素小车从原点出发速度为零要求在 1 秒后精确停在 1 米处也就是位置到 1、速度回零同时让控制能量的积分最小[ \min J \int_0^1 u^2(t),dt ]为什么选这个例子因为它的解析解能直接手推出来(u^(t) 6 - 12t)最优目标值 (J^ 12)。有了这个“标准答案”你就能验证自己写的伪谱法代码到底准不准这是学习数值方法时最最重要的一步——先在有解析解的问题上跑通再拿去处理那些没有标准答案的真实问题。1.2 三种常见解法路线打靶法、直接配点法、伪谱法轨迹优化问题在数学上是一个最优控制问题标准写法是 Bolza 型目标函数包含终端代价和过程积分约束包含微分方程、初始条件、末端条件以及各种路径约束。求解思路大致分两类间接法和直接法。间接法要用庞特里亚金极小值原理推导一阶最优性条件推导过程极其痛苦工程上现在已经很少这么干了。直接法把控制量和状态量离散化把最优控制问题直接当成一个大型非线性规划NLP去求解简单粗暴是目前的主流。直接法里又分三派直接打靶法把控制量参数化从初始状态做数值积分然后用终端误差去修正控制参数。思路直观但对初始猜测极其敏感而且路径约束不好加经常积分着就飞了。直接配点法把整个时间轴切成很多小段每段用低阶多项式比如 Hermite-Simpson 方法逼近状态在节点处强制满足动力学。问题是节点通常需要很多几百上千个很正常。伪谱法和配点法思路类似但用全局高阶多项式去逼近整条轨迹节点选在精心设计的高斯型节点上。节点数量少精度却高得多这是它的核心优势。可以这么理解配点法是“用一堆短线段拼出一条路”伪谱法是“用一条长而光滑的曲线直接贴合整条路”。后者对光滑问题有天然的适配性。1.3 为什么高斯伪谱法值得学高斯伪谱法Gauss Pseudospectral MethodGPM最大的卖点是它的精度来源——Legendre-Gauss 求积公式。对光滑问题误差随节点数 (N) 的增加呈指数级下降也就是所谓的“谱精度”。这意味着一般问题用二三十个点就够了而普通配点法可能要几百个点。另一个实用优势是加约束方便。状态约束、控制约束、路径约束都只是在对应的配点处加上代数不等式不改变整个求解框架。这在航天领域尤其受欢迎像火箭入轨、卫星变轨、再入大气层这类问题GPM 几乎是标配思路。这篇教程虽然用小车举例但代码框架换成三维动力学、加上推力约束和大气阻力就是一枚简化版运载火箭的轨迹优化问题。2. 高斯伪谱法的三个核心步骤2.1 用多项式去“猜”轨迹Lagrange 插值伪谱法的第一个核心思想把整条状态轨迹看成一条光滑曲线用插值多项式去逼近它。假设我们有 (N1) 个离散时间点下面会讲具体是什么点记为 (\tau_1, \tau_2, \ldots, \tau_{N1})对应的状态值是 (x_1, x_2, \ldots, x_{N1})。我们用 Lagrange 插值构造一个 (N) 阶多项式[ x(\tau) \approx \sum_{j1}^{N1} x_j \cdot L_j(\tau) ]其中 (L_j(\tau)) 是第 (j) 个 Lagrange 基函数。这里有个非常舒服的性质基函数 (L_j(\tau)) 在第 (j) 个节点上取值为 1在其他节点上取值 0所以多项式系数就是我们离散的状态值本身。换句话说我们不需要额外求解插值系数直接把点拎出来就是多项式的系数。很多人第一次看到这里会犯晕我们不是要求轨迹吗怎么先假设轨迹是多项式了注意这里的“多项式”不是我们臆造的结果而是我们对未知轨迹的一种参数化描述。我们让状态取一系列未知数值 (x_1, \ldots, x_{N1})然后用这些未知数构造一条多项式曲线最后通过优化把这些未知数解出来。未知的是这些点上的取值而不是多项式本身的形式这和有限元里“形函数”的思想如出一辙。2.2 Legendre-Gauss 点选点讲究在哪那么节点怎么选这是伪谱法区别于普通配点法的关键。高斯伪谱法选的是Legendre-Gauss 点简称 LG 点也就是 Legendre 多项式 (P_N(\tau)) 的 (N) 个根。这些点全部落在区间 ((-1, 1)) 内两边密集、中间稀疏。注意它们不包含端点(-1) 和 (1)这是高斯伪谱法的一个标志性特征。为什么不选等距点因为有著名的 Runge 现象等距节点做高阶多项式插值时区间两端会产生剧烈振荡节点越多振荡越厉害插值精度反而更差。而高斯节点遵循切比雪夫式分布两边密中间疏正好能把插值误差压到最低。选 LG 点还有第二个不可替代的理由——高斯求积。当我们用这 (N) 个点做数值积分时[ \int_{-1}^{1} f(\tau),d\tau \approx \sum_{k1}^{N} w_k f(\tau_k) ]只要选用合适的高斯求积权重 (w_k)这个公式对最高 (2N-1) 次的多项式是精确成立的。这一点极其关键后面计算目标函数的积分项时误差不会因为离散化而额外累积。2.3 把微分方程变成代数方程微分矩阵与初始矩阵有了节点和多项式下一步就是把微分方程约束变成代数方程。高斯伪谱法的状态节点配置是这样的(N) 个 LG 配点 (\tau_1, \ldots, \tau_N)再加上一个终端节点 (\tau_{N1} 1)一共 (N1) 个状态节点。控制量只在 (N) 个配点上定义。然后我们需要一个关键工具——微分矩阵(D)。它的定义是在第 (k) 个配点处Lagrange 基函数的导数值[ D_{k,j} \left.\frac{dL_j}{d\tau}\right|_{\tau\tau_k}, \quad k 1,\ldots,N,; j 1,\ldots,N1 ]有了 (D)多项式在配点处的导数就变成了一个矩阵乘法[ \left.\frac{dx}{d\tau}\right|{\tau\tau_k} \approx \sum{j1}^{N1} D_{k,j} x_j ]这个式子直接把“求导”这个运算变成了“矩阵乘向量”这个代数运算。动力学的离散形式就顺理成章了。以我们的双积分器为例做时间归一化 (\tau \in [-1, 1])物理时间 (t) 和归一化时间的关系是 (t \frac{T}{2}(\tau 1))所以 (\frac{d\tau}{dt} \frac{2}{T})。动力学约束变成[ D X \frac{T}{2} V^{coll}, \quad D V \frac{T}{2} U ]这里 (X) 是 (N1) 维状态向量(V^{coll}) 是速度在配点处的取值(U) 是控制量向量。看微分方程变成了代数方程。还有一个问题LG 配点不包含初始时刻 (\tau -1)那初始条件怎么施加答案是初始矩阵(A)[ A_j L_j(-1) ]它的含义是用多项式在 (\tau -1) 处取值把初始条件“拉”回来。于是初始条件 (x(0) x_0) 就写成[ A \cdot X x_0 ]这是高斯伪谱法最容易踩坑的地方。很多人习惯性地以为所有离散节点都是等距分布的直接拿第一个配点当初始点结果程序怎么调都不对。记住LG 配点不含端点初始条件必须通过初始矩阵施加。2.4 连续最优控制问题到 NLP 的完整映射把上面的东西串起来一个连续的最优控制问题就变成了一个有限维的 NLP。映射关系可以整理成一张表连续问题中的对象离散化之后状态轨迹 (x(t))(N1) 个点配点处的值 终端值控制轨迹 (u(t))(N) 个配点处的值动力学方程 (\dot{x} f)微分矩阵等式 (D X \frac{T}{2} f)积分目标 (\int L,dt)高斯求积 (\frac{T}{2} \sum w_k L_k)初始条件 (x(t_0) x_0)初始矩阵约束 (A X x_0)末端条件 (x(t_f) x_f)终端节点约束 (X_{N1} x_f)决策变量的总数很容易算出来对 (n) 维状态、(m) 维控制的系统大约是 (n(N1) mN) 个变量外加末端时间 (t_f) 如果也自由优化的话再加一个。对双积分器就是 (2(N1) N 3N 2) 个变量配点数取 30 的话总共才 92 个变量这在 NLP 里是小到不能再小的问题几毫秒就能解完。3. 环境准备与 CasADi 快速上手3.1 CasADi 是什么为什么选它CasADi 是一个开源的符号数值计算框架在做最优控制和优化控制领域非常流行。它最核心的能力是自动微分你只需要用符号变量把问题描述出来梯度、雅可比矩阵、海森矩阵这些它都能自动帮你求出来然后直接喂给求解器。这意味着什么意味着你不需要手推任何导数。回想一下经典最优控制的推导过程光是一个伴随方程就能劝退大半人。有了 CasADi你只需要像写数学公式一样“写”问题剩下的交给它和求解器。在 Python 里我们通常用它的Opti 栈接口这是一种高层次的建模方式语法接近人的思维方式定义变量、加约束、设目标、求解、取结果。比直接用ca.sqp或者底层接口写问题要舒服得多。3.2 安装与验证安装很简单pip 一行搞定pip install casadi numpy matplotlib如果你用的是 conda也可以conda install -c conda-forge casadi装完验证一下python -c import casadi as ca; print(ca.__version__)能正常打印版本号就没问题。这里提醒一句CasADi 的 pip 安装包一般自带 IPOPT 求解器的二进制直接用就行。如果某些平台上报找不到 IPOPT建议改用 conda-forge 渠道安装它会自动把配套的求解器依赖一起装上。conda install -c conda-forge casadi3.3 自检用多项式验证离散化矩阵我强烈建议你在建模之前先验证一下自己写的离散化矩阵是否正确。方法很简单拿一个已知导数的函数把它的离散值喂进去看微分矩阵算出来的导数对不对。比如 (f(\tau) \tau^2)导数是 (2\tau)而 (f(-1) 1)import numpy as np from numpy.polynomial import polynomial as P from numpy.polynomial import legendre as Lg def lagrange_coef(nodes, k): coef np.array([1.0]) for j, t in enumerate(nodes): if j k: continue coef P.polymul(coef, np.array([-t, 1.0])) coef coef / (nodes[k] - t) return coef def gpm_discretization(N): tau, w Lg.leggauss(N) # N 个 Legendre-Gauss 节点与求积权重 tau np.sort(tau) nodes np.concatenate([tau, [1.0]]) # 配点 终端节点 M N 1 D np.zeros((N, M)) # 微分矩阵 A np.zeros(M) # 初始矩阵x(-1) A x for j in range(M): lj lagrange_coef(nodes, j) D[:, j] P.polyval(tau, P.polyder(lj)) A[j] P.polyval(-1.0, lj) return tau, D, A, w # 自检 1微分矩阵 tau, D, A, w gpm_discretization(8) x_f tau**2 x_all np.concatenate([x_f, [1.0]]) print(微分矩阵误差, np.max(np.abs(D x_all - 2 * tau))) # 自检 2初始矩阵 print(初始矩阵误差, np.abs(A x_all - 1.0)) # 自检 3高斯求积∫_{-1}^{1} τ^2 dτ 2/3 print(求积误差, np.abs(w x_f - 2 / 3))如果你搭建的高斯伪谱法离散化框架是对的这三个误差都应该在 (10^{-14}) 甚至更小的量级。这一步能挡住你后面 90% 的调试时间因为如果矩阵本身就是错的后面求解出来的结果再奇怪都不奇怪。4. 完整实操双积分器轨迹优化4.1 问题建模与离散化准备现在正式开写。先把问题参数列清楚系统状态位置 (x)、速度 (v)控制量加速度 (u)动力学(\dot{x} v, \quad \dot{v} u)初始条件(x(0) 0, v(0) 0)末端条件(x(1) 1, v(1) 0)目标(\min \int_0^1 u^2 dt)离散化方案我们已经在第二节推导完了。状态变量有两组每组 (N1) 个点控制变量一组(N) 个点。动力学约束用微分矩阵写初始条件用初始矩阵写末端条件直接用终端节点的值写。注意一个细节我们在归一化时间域 (\tau \in [-1, 1]) 上求解物理时间 (t \in [0, T])换算关系是 (t \frac{T}{2}(\tau 1))。所以动力学里 (\frac{d\tau}{dt} \frac{2}{T})方程右侧会出现系数 (\frac{T}{2})别漏了。这个系数我最初写代码时漏过一次结果解出来的轨迹形状不对卡了半天才反应过来。4.2 求解代码30 行跑通核心下面的代码就是完整实现。我只保留了最核心的部分注释写在每步旁边import numpy as np import casadi as ca import matplotlib.pyplot as plt def lagrange_coef(nodes, k): coef np.array([1.0]) for j, t in enumerate(nodes): if j k: continue coef P.polymul(coef, np.array([-t, 1.0])) coef coef / (nodes[k] - t) return coef def gpm_discretization(N): tau, w Lg.leggauss(N) tau np.sort(tau) nodes np.concatenate([tau, [1.0]]) M N 1 D np.zeros((N, M)) A np.zeros(M) for j in range(M): lj lagrange_coef(nodes, j) D[:, j] P.polyval(tau, P.polyder(lj)) A[j] P.polyval(-1.0, lj) return tau, D, A, w N 30 # 配点数 T 1.0 # 末端时间 tau, D, A, w gpm_discretization(N) opti ca.Opti() X opti.variable(N 1) # 位置N 个配点 终端 V opti.variable(N 1) # 速度 U opti.variable(N) # 控制 # 动力学约束dx/dtau (T/2) v, dv/dtau (T/2) u opti.subject_to(D X (T / 2) * V[:N]) opti.subject_to(D V (T / 2) * U) # 初始条件小车从位置 0、速度 0 出发 opti.subject_to(A X 0.0) opti.subject_to(A V 0.0) # 末端条件位置到 1速度回 0 opti.subject_to(X[-1] 1.0) opti.subject_to(V[-1] 0.0) # 目标控制能量的积分 J (T / 2) * ca.dot(w, U**2) opti.minimize(J) opti.solver(ipopt, {print_time: False, ipopt: {print_level: 0}}) sol opti.solve() x_opt sol.value(X) v_opt sol.value(V) u_opt sol.value(U) J_opt sol.value(J) print(最优目标值 J , J_opt)这段代码的关键点在于动力学约束用了V[:N]也就是只取速度在配点处的值因为D X的结果长度是 (N)对应 (N) 个配点。同理D V的右边是控制量U控制量本来就只在配点上定义。初学者最容易错的就是这里状态是 (N1) 个点控制是 (N) 个点两者维度不一样直接相减会报维度错误。4.3 结果验证与解析解对比跑完求出解之后千万别直接收工。把它和解析解画在一起对比tc (tau 1) / 2 * T # 配点对应的物理时间 t_all np.concatenate([tc, [T]]) # 所有状态节点的时间 tt np.linspace(0, T, 1000) u_true 6 - 12 * tt v_true 6 * tt - 6 * tt**2 x_true 3 * tt**2 - 2 * tt**3 fig, axes plt.subplots(3, 1, figsize(8, 8), sharexTrue) axes[0].plot(t_all, x_opt, o, labelGPM) axes[0].plot(tt, x_true, -, labelanalytic) axes[0].set_ylabel(x) axes[0].legend() axes[1].plot(t_all, v_opt, o, labelGPM) axes[1].plot(tt, v_true, -, labelanalytic) axes[1].set_ylabel(v) axes[1].legend() axes[2].plot(tc, u_opt, o, labelGPM) axes[2].plot(tt, u_true, -, labelanalytic) axes[2].set_ylabel(u) axes[2].set_xlabel(t) axes[2].legend() plt.tight_layout() plt.show()你期望看到的输出是这样的最优目标值 (J \approx 12)误差在小数点后好几位。位置、速度、控制三条曲线都跟解析解几乎重合配点处的点正好落在解析曲线上。控制量是从 6 线性下降到 -6 的一条直线。我自己第一次跑通的时候看到那条直线真的有点激动——一个这么“高端”的方法在数学上绕了这么大一圈最后得到的曲线和一阶线性函数完全吻合。这说明离散化做好了能做到非常精确的逼近。4.4 扩展一控制量限幅真实系统里控制量当然不可能无限大所以加上控制量约束更贴近实际。比如限制 (|u| \le 1)在 CasADi 里就一行opti.subject_to(opti.bounded(-1.0, U, 1.0))你什么都不用改重新求解即可。加了约束之后目标值会变大控制曲线在起止阶段会被“砍平”——因为无约束情况下最优控制 (6 - 12t) 在 (t0) 附近超过 6、在 (t1) 附近低于 -6现在被限幅拉回来了。这就是路径约束在起作用GPM 处理起来非常自然因为控制值本来就只是离散变量加边界约束和普通变量加边界没有任何区别。这里有个值得玩的实验不断收窄控制边界从 ±1 收到 ±0.2你会看到轨迹为了在 1 秒内完成移动被迫让控制全程贴着约束边界走。加边界的求解过程仍然稳定这也体现了直接法的优势——约束越多问题反而越“简单”因为可行域变小了初值猜测更容易落在收敛盆地里。4.5 扩展二末端时间自由与 bang-bang 控制再进一步让末端时间 (T) 也变成自由变量问题变成“最快用 1 的加速度上限把车从静止推到 1 米处并停下”。这是一个经典的最小时间控制问题理论解是 bang-bang 控制前一半时间全油门加速(u 1)后一半时间全刹车(u -1)总时间 (T^* 2) 秒。代码只需要在上一版基础上改几处tf opti.variable() opti.set_initial(tf, 2.0) opti.subject_to(D X (tf / 2) * V[:N]) opti.subject_to(D V (tf / 2) * U) opti.subject_to(opti.bounded(-1.0, U, 1.0)) opti.subject_to(tf 0.1) opti.minimize(tf)注意这里的动力学约束系数从固定的T / 2变成了tf / 2因为 (tf) 现在是变量了。还要给状态变量一个合理的初始猜测否则 IPOPT 可能从离谱的初值出发收敛得很痛苦opti.set_initial(X, np.linspace(0.0, 1.0, N 1)) opti.set_initial(V, np.zeros(N 1)) opti.set_initial(U, np.zeros(N))跑完之后查看sol.value(tf)你会发现它稳稳停在 2 附近控制曲线几乎是一根方波先 1 后 -1在中间某个点发生切换。这个切换点还会随着配点数 (N) 的增加而越来越尖锐这是伪谱法在有非光滑最优解时的一个典型表现——它会尽量用多项式去逼近那个拐角。想要精确捕捉切换点需要更精细的网格或混合方法但作为入门演示已经足够说明问题。5. 常见问题与调试经验5.1 常见问题速查表这部分内容都是我自己折腾出来的实战经验几乎每条都踩过坑直接放一张速查表现象可能原因解决办法求解器报NaN或Infeasible初始猜测离最优解太远用opti.set_initial()给合理初值解出来的轨迹像波浪线配点数 (N) 过大且问题不光滑减小 (N)或检查约束是否物理端点附近控制剧烈振荡控制只在配点上定义端点靠外推增大 (N)或者在后处理时重新采样结果和解析解差很多时间归一化系数 (T/2) 漏了检查动力学等式的系数收敛慢或迭代步数不够IPOPT 默认最大迭代次数有限调大ipopt.max_iter求导报错对 CasADi 符号变量用了 numpy 函数统一用ca.sin、ca.dot等 CasADi 函数矩阵维度对不上状态 (N1)、控制 (N) 混淆逐行打印形状核对5.2 几个重要的调试心得第一个心得先从解析解可知的问题开始验证。我在做真实项目时如果遇到一个没有解析解的新问题一定会先构造一个简化版本让它的解可以被手算或理论推导跑通了再上完整版本。这不是浪费时间恰恰是省时间。没有“标准答案”的数值结果你根本没法定性判断它是对了还是错了。第二个心得N 的选择要克制。伪谱法精度很高但并不是 (N) 越大越好。(N) 太大之后微分矩阵的条件数会变差数值误差反而增大而且高阶多项式对非光滑问题会产生振荡。我的经验是先试 (N 10)看结果趋势对不对再逐步加到 20、30。如果你的问题用 50 个点还不满足精度要求大概率不是点数不够而是问题本身比如存在不连续不适合纯伪谱法你需要考虑把问题分段或改用其他方法。第三个心得留意 CasADi 和 numpy 的边界。在定义优化问题时符号变量只能用 CasADi 的函数操作比如ca.sin(x)而不是np.sin(x)ca.dot(w, U**2)而不是w (U**2)。在这篇教程的线性动力学里你可能没机会踩这个坑但一旦上非线性动力学比如摆、倒立摆、再入飞行器一个np.sin就会在代码里藏一个极其隐蔽的 bug——它不会立刻报错而是默默地做错。我的习惯是凡是要放进opti的表达式一律只用ca.前缀的函数。第四个心得求解器选项要会调。IPOPT 有几个参数在轨迹优化里特别常用。print_level控制日志输出调试时设成 5 或 6 能看到迭代详情跑通了再降到 0max_iter默认 3000对大型问题经常不够直接设大一点比如1e5如果遇到收敛困难先调tol放松一点比如从1e-8放到1e-6看看问题是否改善。一个容易忽略的事IPOPT 对问题的尺度极其敏感。如果你的状态是速度量级 0~10位置量级 0~1000时间量级 100那一定要先做无量纲化把变量都拉到 (O(1)) 量级否则收敛速度会非常感人。第五个心得也是我个人最喜欢的一条用免费的午餐检验方法。一旦你的高斯伪谱法代码框架写好把它当作一个通用的离散化工具去套各种你能找到解析解或已知结果的最优控制问题——双积分器、倒立摆、月球软着陆简化模型。每多验证一个你对这个方法的信心就增加一分。到最后你会发现真正难的不是“用伪谱法”而是“把问题建得足够好”状态怎么选变量、约束怎么提才不病态、目标怎么表达才光滑。方法本身反倒是相对固定的。这套工具链还有一个很自然的扩展方向把单段伪谱法改成多段拼接在不同段用不同的配点数处理带有阶段切换的问题比如火箭助推器分离、变轨中间点约束。CasADi 对这类问题支持得也很好只需要把每段的离散化矩阵拼起来再加上段间连续性约束即可。学完这篇教程你已经有了一个可以一步步往上搭的扎实底座。
从双积分器到高斯伪谱法：用CasADi实现高精度轨迹优化
先别急着去翻那些满屏 Legendre 多项式、配点、NLP 转换的论文这篇教程我用一个最经典的“双积分器”问题把高斯伪谱法从数学到代码完整走一遍。全程用 Python CasADi 实现代码可以直接复制跑通跑完你就能拿同一套流程去套你自己的轨迹优化问题。这篇文章适合刚接触最优控制、想搞明白伪谱法到底在干什么、或者只是想把 CasADi 用起来的读者看完你会觉得原来所谓的高斯伪谱法核心就三件事——选点、插值、把微分方程变成代数方程。1. 轨迹优化到底在做什么1.1 从一个最简单的例子说起轨迹优化通俗点说就是在满足物理规律的前提下找一条“最好”的运动轨迹。导航软件告诉你走哪条路那是路径规划轨迹优化则更进一步告诉你每个时刻油门踩多少、刹车什么时候踩怎么开最省油、怎么飞最平稳。我选的这个例子是所有轨迹优化教材里的“hello world”——双积分器问题。你可以理解为一辆在无摩擦直轨道上的小车位置是 (x)速度是 (v)控制量是加速度 (u)。它的动力学只有两个方程[ \dot{x} v, \quad \dot{v} u ]任务很朴素小车从原点出发速度为零要求在 1 秒后精确停在 1 米处也就是位置到 1、速度回零同时让控制能量的积分最小[ \min J \int_0^1 u^2(t),dt ]为什么选这个例子因为它的解析解能直接手推出来(u^(t) 6 - 12t)最优目标值 (J^ 12)。有了这个“标准答案”你就能验证自己写的伪谱法代码到底准不准这是学习数值方法时最最重要的一步——先在有解析解的问题上跑通再拿去处理那些没有标准答案的真实问题。1.2 三种常见解法路线打靶法、直接配点法、伪谱法轨迹优化问题在数学上是一个最优控制问题标准写法是 Bolza 型目标函数包含终端代价和过程积分约束包含微分方程、初始条件、末端条件以及各种路径约束。求解思路大致分两类间接法和直接法。间接法要用庞特里亚金极小值原理推导一阶最优性条件推导过程极其痛苦工程上现在已经很少这么干了。直接法把控制量和状态量离散化把最优控制问题直接当成一个大型非线性规划NLP去求解简单粗暴是目前的主流。直接法里又分三派直接打靶法把控制量参数化从初始状态做数值积分然后用终端误差去修正控制参数。思路直观但对初始猜测极其敏感而且路径约束不好加经常积分着就飞了。直接配点法把整个时间轴切成很多小段每段用低阶多项式比如 Hermite-Simpson 方法逼近状态在节点处强制满足动力学。问题是节点通常需要很多几百上千个很正常。伪谱法和配点法思路类似但用全局高阶多项式去逼近整条轨迹节点选在精心设计的高斯型节点上。节点数量少精度却高得多这是它的核心优势。可以这么理解配点法是“用一堆短线段拼出一条路”伪谱法是“用一条长而光滑的曲线直接贴合整条路”。后者对光滑问题有天然的适配性。1.3 为什么高斯伪谱法值得学高斯伪谱法Gauss Pseudospectral MethodGPM最大的卖点是它的精度来源——Legendre-Gauss 求积公式。对光滑问题误差随节点数 (N) 的增加呈指数级下降也就是所谓的“谱精度”。这意味着一般问题用二三十个点就够了而普通配点法可能要几百个点。另一个实用优势是加约束方便。状态约束、控制约束、路径约束都只是在对应的配点处加上代数不等式不改变整个求解框架。这在航天领域尤其受欢迎像火箭入轨、卫星变轨、再入大气层这类问题GPM 几乎是标配思路。这篇教程虽然用小车举例但代码框架换成三维动力学、加上推力约束和大气阻力就是一枚简化版运载火箭的轨迹优化问题。2. 高斯伪谱法的三个核心步骤2.1 用多项式去“猜”轨迹Lagrange 插值伪谱法的第一个核心思想把整条状态轨迹看成一条光滑曲线用插值多项式去逼近它。假设我们有 (N1) 个离散时间点下面会讲具体是什么点记为 (\tau_1, \tau_2, \ldots, \tau_{N1})对应的状态值是 (x_1, x_2, \ldots, x_{N1})。我们用 Lagrange 插值构造一个 (N) 阶多项式[ x(\tau) \approx \sum_{j1}^{N1} x_j \cdot L_j(\tau) ]其中 (L_j(\tau)) 是第 (j) 个 Lagrange 基函数。这里有个非常舒服的性质基函数 (L_j(\tau)) 在第 (j) 个节点上取值为 1在其他节点上取值 0所以多项式系数就是我们离散的状态值本身。换句话说我们不需要额外求解插值系数直接把点拎出来就是多项式的系数。很多人第一次看到这里会犯晕我们不是要求轨迹吗怎么先假设轨迹是多项式了注意这里的“多项式”不是我们臆造的结果而是我们对未知轨迹的一种参数化描述。我们让状态取一系列未知数值 (x_1, \ldots, x_{N1})然后用这些未知数构造一条多项式曲线最后通过优化把这些未知数解出来。未知的是这些点上的取值而不是多项式本身的形式这和有限元里“形函数”的思想如出一辙。2.2 Legendre-Gauss 点选点讲究在哪那么节点怎么选这是伪谱法区别于普通配点法的关键。高斯伪谱法选的是Legendre-Gauss 点简称 LG 点也就是 Legendre 多项式 (P_N(\tau)) 的 (N) 个根。这些点全部落在区间 ((-1, 1)) 内两边密集、中间稀疏。注意它们不包含端点(-1) 和 (1)这是高斯伪谱法的一个标志性特征。为什么不选等距点因为有著名的 Runge 现象等距节点做高阶多项式插值时区间两端会产生剧烈振荡节点越多振荡越厉害插值精度反而更差。而高斯节点遵循切比雪夫式分布两边密中间疏正好能把插值误差压到最低。选 LG 点还有第二个不可替代的理由——高斯求积。当我们用这 (N) 个点做数值积分时[ \int_{-1}^{1} f(\tau),d\tau \approx \sum_{k1}^{N} w_k f(\tau_k) ]只要选用合适的高斯求积权重 (w_k)这个公式对最高 (2N-1) 次的多项式是精确成立的。这一点极其关键后面计算目标函数的积分项时误差不会因为离散化而额外累积。2.3 把微分方程变成代数方程微分矩阵与初始矩阵有了节点和多项式下一步就是把微分方程约束变成代数方程。高斯伪谱法的状态节点配置是这样的(N) 个 LG 配点 (\tau_1, \ldots, \tau_N)再加上一个终端节点 (\tau_{N1} 1)一共 (N1) 个状态节点。控制量只在 (N) 个配点上定义。然后我们需要一个关键工具——微分矩阵(D)。它的定义是在第 (k) 个配点处Lagrange 基函数的导数值[ D_{k,j} \left.\frac{dL_j}{d\tau}\right|_{\tau\tau_k}, \quad k 1,\ldots,N,; j 1,\ldots,N1 ]有了 (D)多项式在配点处的导数就变成了一个矩阵乘法[ \left.\frac{dx}{d\tau}\right|{\tau\tau_k} \approx \sum{j1}^{N1} D_{k,j} x_j ]这个式子直接把“求导”这个运算变成了“矩阵乘向量”这个代数运算。动力学的离散形式就顺理成章了。以我们的双积分器为例做时间归一化 (\tau \in [-1, 1])物理时间 (t) 和归一化时间的关系是 (t \frac{T}{2}(\tau 1))所以 (\frac{d\tau}{dt} \frac{2}{T})。动力学约束变成[ D X \frac{T}{2} V^{coll}, \quad D V \frac{T}{2} U ]这里 (X) 是 (N1) 维状态向量(V^{coll}) 是速度在配点处的取值(U) 是控制量向量。看微分方程变成了代数方程。还有一个问题LG 配点不包含初始时刻 (\tau -1)那初始条件怎么施加答案是初始矩阵(A)[ A_j L_j(-1) ]它的含义是用多项式在 (\tau -1) 处取值把初始条件“拉”回来。于是初始条件 (x(0) x_0) 就写成[ A \cdot X x_0 ]这是高斯伪谱法最容易踩坑的地方。很多人习惯性地以为所有离散节点都是等距分布的直接拿第一个配点当初始点结果程序怎么调都不对。记住LG 配点不含端点初始条件必须通过初始矩阵施加。2.4 连续最优控制问题到 NLP 的完整映射把上面的东西串起来一个连续的最优控制问题就变成了一个有限维的 NLP。映射关系可以整理成一张表连续问题中的对象离散化之后状态轨迹 (x(t))(N1) 个点配点处的值 终端值控制轨迹 (u(t))(N) 个配点处的值动力学方程 (\dot{x} f)微分矩阵等式 (D X \frac{T}{2} f)积分目标 (\int L,dt)高斯求积 (\frac{T}{2} \sum w_k L_k)初始条件 (x(t_0) x_0)初始矩阵约束 (A X x_0)末端条件 (x(t_f) x_f)终端节点约束 (X_{N1} x_f)决策变量的总数很容易算出来对 (n) 维状态、(m) 维控制的系统大约是 (n(N1) mN) 个变量外加末端时间 (t_f) 如果也自由优化的话再加一个。对双积分器就是 (2(N1) N 3N 2) 个变量配点数取 30 的话总共才 92 个变量这在 NLP 里是小到不能再小的问题几毫秒就能解完。3. 环境准备与 CasADi 快速上手3.1 CasADi 是什么为什么选它CasADi 是一个开源的符号数值计算框架在做最优控制和优化控制领域非常流行。它最核心的能力是自动微分你只需要用符号变量把问题描述出来梯度、雅可比矩阵、海森矩阵这些它都能自动帮你求出来然后直接喂给求解器。这意味着什么意味着你不需要手推任何导数。回想一下经典最优控制的推导过程光是一个伴随方程就能劝退大半人。有了 CasADi你只需要像写数学公式一样“写”问题剩下的交给它和求解器。在 Python 里我们通常用它的Opti 栈接口这是一种高层次的建模方式语法接近人的思维方式定义变量、加约束、设目标、求解、取结果。比直接用ca.sqp或者底层接口写问题要舒服得多。3.2 安装与验证安装很简单pip 一行搞定pip install casadi numpy matplotlib如果你用的是 conda也可以conda install -c conda-forge casadi装完验证一下python -c import casadi as ca; print(ca.__version__)能正常打印版本号就没问题。这里提醒一句CasADi 的 pip 安装包一般自带 IPOPT 求解器的二进制直接用就行。如果某些平台上报找不到 IPOPT建议改用 conda-forge 渠道安装它会自动把配套的求解器依赖一起装上。conda install -c conda-forge casadi3.3 自检用多项式验证离散化矩阵我强烈建议你在建模之前先验证一下自己写的离散化矩阵是否正确。方法很简单拿一个已知导数的函数把它的离散值喂进去看微分矩阵算出来的导数对不对。比如 (f(\tau) \tau^2)导数是 (2\tau)而 (f(-1) 1)import numpy as np from numpy.polynomial import polynomial as P from numpy.polynomial import legendre as Lg def lagrange_coef(nodes, k): coef np.array([1.0]) for j, t in enumerate(nodes): if j k: continue coef P.polymul(coef, np.array([-t, 1.0])) coef coef / (nodes[k] - t) return coef def gpm_discretization(N): tau, w Lg.leggauss(N) # N 个 Legendre-Gauss 节点与求积权重 tau np.sort(tau) nodes np.concatenate([tau, [1.0]]) # 配点 终端节点 M N 1 D np.zeros((N, M)) # 微分矩阵 A np.zeros(M) # 初始矩阵x(-1) A x for j in range(M): lj lagrange_coef(nodes, j) D[:, j] P.polyval(tau, P.polyder(lj)) A[j] P.polyval(-1.0, lj) return tau, D, A, w # 自检 1微分矩阵 tau, D, A, w gpm_discretization(8) x_f tau**2 x_all np.concatenate([x_f, [1.0]]) print(微分矩阵误差, np.max(np.abs(D x_all - 2 * tau))) # 自检 2初始矩阵 print(初始矩阵误差, np.abs(A x_all - 1.0)) # 自检 3高斯求积∫_{-1}^{1} τ^2 dτ 2/3 print(求积误差, np.abs(w x_f - 2 / 3))如果你搭建的高斯伪谱法离散化框架是对的这三个误差都应该在 (10^{-14}) 甚至更小的量级。这一步能挡住你后面 90% 的调试时间因为如果矩阵本身就是错的后面求解出来的结果再奇怪都不奇怪。4. 完整实操双积分器轨迹优化4.1 问题建模与离散化准备现在正式开写。先把问题参数列清楚系统状态位置 (x)、速度 (v)控制量加速度 (u)动力学(\dot{x} v, \quad \dot{v} u)初始条件(x(0) 0, v(0) 0)末端条件(x(1) 1, v(1) 0)目标(\min \int_0^1 u^2 dt)离散化方案我们已经在第二节推导完了。状态变量有两组每组 (N1) 个点控制变量一组(N) 个点。动力学约束用微分矩阵写初始条件用初始矩阵写末端条件直接用终端节点的值写。注意一个细节我们在归一化时间域 (\tau \in [-1, 1]) 上求解物理时间 (t \in [0, T])换算关系是 (t \frac{T}{2}(\tau 1))。所以动力学里 (\frac{d\tau}{dt} \frac{2}{T})方程右侧会出现系数 (\frac{T}{2})别漏了。这个系数我最初写代码时漏过一次结果解出来的轨迹形状不对卡了半天才反应过来。4.2 求解代码30 行跑通核心下面的代码就是完整实现。我只保留了最核心的部分注释写在每步旁边import numpy as np import casadi as ca import matplotlib.pyplot as plt def lagrange_coef(nodes, k): coef np.array([1.0]) for j, t in enumerate(nodes): if j k: continue coef P.polymul(coef, np.array([-t, 1.0])) coef coef / (nodes[k] - t) return coef def gpm_discretization(N): tau, w Lg.leggauss(N) tau np.sort(tau) nodes np.concatenate([tau, [1.0]]) M N 1 D np.zeros((N, M)) A np.zeros(M) for j in range(M): lj lagrange_coef(nodes, j) D[:, j] P.polyval(tau, P.polyder(lj)) A[j] P.polyval(-1.0, lj) return tau, D, A, w N 30 # 配点数 T 1.0 # 末端时间 tau, D, A, w gpm_discretization(N) opti ca.Opti() X opti.variable(N 1) # 位置N 个配点 终端 V opti.variable(N 1) # 速度 U opti.variable(N) # 控制 # 动力学约束dx/dtau (T/2) v, dv/dtau (T/2) u opti.subject_to(D X (T / 2) * V[:N]) opti.subject_to(D V (T / 2) * U) # 初始条件小车从位置 0、速度 0 出发 opti.subject_to(A X 0.0) opti.subject_to(A V 0.0) # 末端条件位置到 1速度回 0 opti.subject_to(X[-1] 1.0) opti.subject_to(V[-1] 0.0) # 目标控制能量的积分 J (T / 2) * ca.dot(w, U**2) opti.minimize(J) opti.solver(ipopt, {print_time: False, ipopt: {print_level: 0}}) sol opti.solve() x_opt sol.value(X) v_opt sol.value(V) u_opt sol.value(U) J_opt sol.value(J) print(最优目标值 J , J_opt)这段代码的关键点在于动力学约束用了V[:N]也就是只取速度在配点处的值因为D X的结果长度是 (N)对应 (N) 个配点。同理D V的右边是控制量U控制量本来就只在配点上定义。初学者最容易错的就是这里状态是 (N1) 个点控制是 (N) 个点两者维度不一样直接相减会报维度错误。4.3 结果验证与解析解对比跑完求出解之后千万别直接收工。把它和解析解画在一起对比tc (tau 1) / 2 * T # 配点对应的物理时间 t_all np.concatenate([tc, [T]]) # 所有状态节点的时间 tt np.linspace(0, T, 1000) u_true 6 - 12 * tt v_true 6 * tt - 6 * tt**2 x_true 3 * tt**2 - 2 * tt**3 fig, axes plt.subplots(3, 1, figsize(8, 8), sharexTrue) axes[0].plot(t_all, x_opt, o, labelGPM) axes[0].plot(tt, x_true, -, labelanalytic) axes[0].set_ylabel(x) axes[0].legend() axes[1].plot(t_all, v_opt, o, labelGPM) axes[1].plot(tt, v_true, -, labelanalytic) axes[1].set_ylabel(v) axes[1].legend() axes[2].plot(tc, u_opt, o, labelGPM) axes[2].plot(tt, u_true, -, labelanalytic) axes[2].set_ylabel(u) axes[2].set_xlabel(t) axes[2].legend() plt.tight_layout() plt.show()你期望看到的输出是这样的最优目标值 (J \approx 12)误差在小数点后好几位。位置、速度、控制三条曲线都跟解析解几乎重合配点处的点正好落在解析曲线上。控制量是从 6 线性下降到 -6 的一条直线。我自己第一次跑通的时候看到那条直线真的有点激动——一个这么“高端”的方法在数学上绕了这么大一圈最后得到的曲线和一阶线性函数完全吻合。这说明离散化做好了能做到非常精确的逼近。4.4 扩展一控制量限幅真实系统里控制量当然不可能无限大所以加上控制量约束更贴近实际。比如限制 (|u| \le 1)在 CasADi 里就一行opti.subject_to(opti.bounded(-1.0, U, 1.0))你什么都不用改重新求解即可。加了约束之后目标值会变大控制曲线在起止阶段会被“砍平”——因为无约束情况下最优控制 (6 - 12t) 在 (t0) 附近超过 6、在 (t1) 附近低于 -6现在被限幅拉回来了。这就是路径约束在起作用GPM 处理起来非常自然因为控制值本来就只是离散变量加边界约束和普通变量加边界没有任何区别。这里有个值得玩的实验不断收窄控制边界从 ±1 收到 ±0.2你会看到轨迹为了在 1 秒内完成移动被迫让控制全程贴着约束边界走。加边界的求解过程仍然稳定这也体现了直接法的优势——约束越多问题反而越“简单”因为可行域变小了初值猜测更容易落在收敛盆地里。4.5 扩展二末端时间自由与 bang-bang 控制再进一步让末端时间 (T) 也变成自由变量问题变成“最快用 1 的加速度上限把车从静止推到 1 米处并停下”。这是一个经典的最小时间控制问题理论解是 bang-bang 控制前一半时间全油门加速(u 1)后一半时间全刹车(u -1)总时间 (T^* 2) 秒。代码只需要在上一版基础上改几处tf opti.variable() opti.set_initial(tf, 2.0) opti.subject_to(D X (tf / 2) * V[:N]) opti.subject_to(D V (tf / 2) * U) opti.subject_to(opti.bounded(-1.0, U, 1.0)) opti.subject_to(tf 0.1) opti.minimize(tf)注意这里的动力学约束系数从固定的T / 2变成了tf / 2因为 (tf) 现在是变量了。还要给状态变量一个合理的初始猜测否则 IPOPT 可能从离谱的初值出发收敛得很痛苦opti.set_initial(X, np.linspace(0.0, 1.0, N 1)) opti.set_initial(V, np.zeros(N 1)) opti.set_initial(U, np.zeros(N))跑完之后查看sol.value(tf)你会发现它稳稳停在 2 附近控制曲线几乎是一根方波先 1 后 -1在中间某个点发生切换。这个切换点还会随着配点数 (N) 的增加而越来越尖锐这是伪谱法在有非光滑最优解时的一个典型表现——它会尽量用多项式去逼近那个拐角。想要精确捕捉切换点需要更精细的网格或混合方法但作为入门演示已经足够说明问题。5. 常见问题与调试经验5.1 常见问题速查表这部分内容都是我自己折腾出来的实战经验几乎每条都踩过坑直接放一张速查表现象可能原因解决办法求解器报NaN或Infeasible初始猜测离最优解太远用opti.set_initial()给合理初值解出来的轨迹像波浪线配点数 (N) 过大且问题不光滑减小 (N)或检查约束是否物理端点附近控制剧烈振荡控制只在配点上定义端点靠外推增大 (N)或者在后处理时重新采样结果和解析解差很多时间归一化系数 (T/2) 漏了检查动力学等式的系数收敛慢或迭代步数不够IPOPT 默认最大迭代次数有限调大ipopt.max_iter求导报错对 CasADi 符号变量用了 numpy 函数统一用ca.sin、ca.dot等 CasADi 函数矩阵维度对不上状态 (N1)、控制 (N) 混淆逐行打印形状核对5.2 几个重要的调试心得第一个心得先从解析解可知的问题开始验证。我在做真实项目时如果遇到一个没有解析解的新问题一定会先构造一个简化版本让它的解可以被手算或理论推导跑通了再上完整版本。这不是浪费时间恰恰是省时间。没有“标准答案”的数值结果你根本没法定性判断它是对了还是错了。第二个心得N 的选择要克制。伪谱法精度很高但并不是 (N) 越大越好。(N) 太大之后微分矩阵的条件数会变差数值误差反而增大而且高阶多项式对非光滑问题会产生振荡。我的经验是先试 (N 10)看结果趋势对不对再逐步加到 20、30。如果你的问题用 50 个点还不满足精度要求大概率不是点数不够而是问题本身比如存在不连续不适合纯伪谱法你需要考虑把问题分段或改用其他方法。第三个心得留意 CasADi 和 numpy 的边界。在定义优化问题时符号变量只能用 CasADi 的函数操作比如ca.sin(x)而不是np.sin(x)ca.dot(w, U**2)而不是w (U**2)。在这篇教程的线性动力学里你可能没机会踩这个坑但一旦上非线性动力学比如摆、倒立摆、再入飞行器一个np.sin就会在代码里藏一个极其隐蔽的 bug——它不会立刻报错而是默默地做错。我的习惯是凡是要放进opti的表达式一律只用ca.前缀的函数。第四个心得求解器选项要会调。IPOPT 有几个参数在轨迹优化里特别常用。print_level控制日志输出调试时设成 5 或 6 能看到迭代详情跑通了再降到 0max_iter默认 3000对大型问题经常不够直接设大一点比如1e5如果遇到收敛困难先调tol放松一点比如从1e-8放到1e-6看看问题是否改善。一个容易忽略的事IPOPT 对问题的尺度极其敏感。如果你的状态是速度量级 0~10位置量级 0~1000时间量级 100那一定要先做无量纲化把变量都拉到 (O(1)) 量级否则收敛速度会非常感人。第五个心得也是我个人最喜欢的一条用免费的午餐检验方法。一旦你的高斯伪谱法代码框架写好把它当作一个通用的离散化工具去套各种你能找到解析解或已知结果的最优控制问题——双积分器、倒立摆、月球软着陆简化模型。每多验证一个你对这个方法的信心就增加一分。到最后你会发现真正难的不是“用伪谱法”而是“把问题建得足够好”状态怎么选变量、约束怎么提才不病态、目标怎么表达才光滑。方法本身反倒是相对固定的。这套工具链还有一个很自然的扩展方向把单段伪谱法改成多段拼接在不同段用不同的配点数处理带有阶段切换的问题比如火箭助推器分离、变轨中间点约束。CasADi 对这类问题支持得也很好只需要把每段的离散化矩阵拼起来再加上段间连续性约束即可。学完这篇教程你已经有了一个可以一步步往上搭的扎实底座。
