拓冰网站定制 
1. 问题引入为什么我盯上了高斯伪谱法轨迹优化这个词做过机器人控制、无人驾驶、飞行器规划的人应该都不陌生。简单说就是给你一个运动系统告诉它起点和终点外加一堆物理约束让机器自己算出“怎么走最好”的路线。这里的“最好”可以是时间最短、能耗最低、乘客最舒适等等。我在折腾一个小车的极速停车问题时最开始用的是直接打靶法Direct Shooting和直接配点法Direct Collocation。这两个方法有一个共同的问题当约束变多、控制步长变细之后要么求解时间暴涨要么初值猜不准导致 Newton 迭代根本不收敛。后来换成高斯伪谱法Gauss Pseudospectral Method简称 GPM求解稳定性和效率都上了一个台阶。这篇教程我会用 Python CasADi 完整实现一遍高斯伪谱法解决一个带约束的一维小车最短时间控制问题并详细解释每一步背后的数值原理。目标读者是已经会用 Python 和 numpy但没接触过直接法轨迹优化、或者知道伪谱法但一直不知道怎么落地的朋友。为什么选 CasADi因为它的符号运算和自动微分机制太好了你只需要把优化问题用代码“描述”出来剩下的雅可比矩阵、海森矩阵它全帮你自动推导极大拉低了伪谱法的实现门槛。换句话说我们不用去手算一堆偏导公式省下来的精力可以聚焦在“伪谱法本身怎么构造”这件事上。2. 高斯伪谱法的核心思想拆解2.1 先从“离散化”这件事说起连续时间上的轨迹优化问题本质上是一个无穷维优化问题——时间轴上的每一个瞬间都有状态量和控制量计算机没法直接处理无穷个变量。直接法的基本思路就是把连续问题变成离散问题。之前我用的直接打靶法思路是只把控制变量在时间轴上离散化状态轨迹通过积分器比如 RK4一步一步推出来。它的问题是积分过程本身有误差而且控制变量分段多了之后积分链变长梯度传播的敏感度急剧上升。高斯伪谱法走的是另一条路它把状态变量和控制变量同时离散化但不是在等间隔时间点上离散而是在一组精心挑选的“非等距节点”Legendre 多项式的根上离散。然后用拉格朗日插值多项式拟合整个状态轨迹再让这个插值多项式在配点上精确满足系统动力学方程。这样一来微分方程约束就变成了代数约束原问题就变成了一个标准的非线性规划问题NLP可以直接交给 IPOPT 这样的求解器去算。因为配点是非等距的而且选得极有讲究同等精度下需要的节点数远少于等距方法计算量因此大幅下降。2.2 为什么配点选在 Legendre 多项式的根上这是很多新手最容易懵的地方为什么偏偏要在高斯点上配回答这个问题要提到数值积分和多项式逼近。如果我们把时间区间映射到 ([-1,1]) 上然后在 Legendre 多项式 (P_N(t)) 的根 (t_i) 处取节点做拉格朗日插值那么插值多项式对光滑函数的逼近误差在等距节点下可能随节点数增加而发散这就是著名的 Runge 现象但在高斯节点下是收敛的而且是“谱精度”——误差随节点数增加呈指数衰减。在这个节点体系下做高斯积分Gauss-Legendre 积分N 个节点可以精确积分到 (2N-1) 次的多项式这个精度等距节点根本无法企及。用人话说同样算出一条轨迹等距取 50 个点才能达到的精度伪谱法可能 20 个点就够了。少一半多的变量求解器压力自然小很多。2.3 状态近似的数学表达高斯伪谱法的标准状态逼近方式是拉格朗插值多项式。设在配点 (t_0 -1, t_1,\dots,t_N) 上状态 (x(t)) 近似为[ x(\tau) \approx X(\tau) \sum_{i0}^{N} L_i(\tau) x_i ]其中 (L_i(\tau)) 是拉格朗日基函数[ L_i(\tau) \prod_{j0, j\neq i}^{N} \frac{\tau - t_j}{t_i - t_j} ]这个基函数有个特别好的性质(L_i(t_k)1) 当 (ik)否则等于 0。也就是说插值多项式在每个配点上的值正好等于该点待优化状态变量没有任何“中间映射”误差。对时间求导后得到[ \dot{x}(\tau) \approx \dot{X}(\tau) \sum_{i0}^{N} \dot{L}_i(\tau) x_i ]在配点 (t_k) 处取值就得到[ \dot{X}(t_k) \sum_{i0}^{N} D_{ki} x_i ]其中 (D_{ki} \dot{L}_i(t_k))这就是所谓的高斯伪谱微分矩阵。这个矩阵是确定的、只和配点选择有关完全可以程序化计算。有了它微分方程约束就转化成了线性代数约束[ \sum_{i0}^{N} D_{ki} x_i \frac{t_f - t_0}{2} f(x_k, u_k, t_k) ]等式右侧多出来的 ((t_f - t_0)/2) 是把物理时间区间映射到 ([-1,1]) 的雅可比因子记得带上。3. 实验环境与工具准备3.1 安装 CasADiCasADi 是这篇教程的核心工具。安装方式很简单pip install casadi我这边用的是 3.6.x 版本官方 3.5.5 之后的版本在宏定义和接口上都稳定了不少依赖的 numpy 版本建议 1.21 以上。装完验证一下import casadi as ca print(ca.__version__)能打印出版本号就说明环境没问题。求解器部分CasADi 内置了 IPOPT 的接口但 IPOPT 本身需要额外装建议直接用pip install ipopt或者在 Windows 上直接用 CasADi 官方编译版本自带的 IPOPT。实测在 Windows Anaconda 环境下conda install -c conda-forge ipopt成功率最高。3.2 明确优化目标和约束这一节我们先定下要解决的问题也是全文章的贯穿案例一个质量可归一化的一维小车初始位置 0初始速度 0。我们希望它在最短时间内到达位置 1并且停在位置 1速度归零。过程中加速度 (u) 受限于 ([-1, 1])速度受限于 ([-0.8, 0.8])位置受限于 ([0, 1])。写成标准形式就是[ \begin{aligned} \min_{u(\cdot), t_f} \quad J t_f \ \text{s.t.} \quad \dot{x}_1 x_2 \ \dot{x}_2 u \ x_1(0)0, ; x_2(0)0 \ x_1(t_f)1, ; x_2(t_f)0 \ -1 \le u \le 1 \ -0.8 \le x_2 \le 0.8 \end{aligned} ]这个问题看起来简单但既有状态约束又有控制约束终点还是“双重”的位置和速度都要到位非常适合演示伪谱法如何处理各种约束。3.3 确定配点数量与时间映射伪谱法的一个关键参数是配点数量 (N)。我刚跑的时候习惯用 20后来调参发现对于这个简单问题(N16) 就已经能拿到很高精度的解。再往上加精度提升有限但 NLP 变量数线性增长求解时间急剧上升。工程上建议先跑 (N8) 和 (N16) 两组结果做对比如果轨迹形状一致、约束边界贴合度都很高就不用继续加点了。如果轨迹有明显振荡或者起边界处不光滑再考虑 (N32)。时间映射这一步因为终点 (t_f) 本身也是一个待优化变量我们把物理时间 (t\in [0, t_f]) 映射到标准区间 (\tau\in [-1,1])映射关系是[ t \frac{t_f}{2}(\tau 1) ]于是动力学方程中的时间导数项变成[ \frac{dx}{dt} \frac{2}{t_f}\frac{dx}{d\tau} ]这个系数会贯穿在伪谱法的每一个动力学约束里写代码时不要漏。4. 完整代码实现从配点生成到结果可视化4.1 生成高斯配点和微分矩阵这里是最核心的一段代码。配点是 (N1) 个节点其中第一个节点固定在 (-1)其余的 (N) 个节点是 Legendre 多项式 (P_N(\tau)) 的根。注意我们用的是 Gauss-Legendre 配点不是 Gauss-Radau 或 Gauss-Lobatto别搞混。import numpy as np import casadi as ca import matplotlib.pyplot as plt def lg_roots(N): 计算 Legendre 多项式 P_N(tau) 在 [-1, 1] 上的根高斯点 使用 numpy.polynomial.legendre 模块 from numpy.polynomial import legendre as L # 返回的 roots 是 P_N(x) 的零点 roots L.leggauss(N)[0] return roots def pseudospectral_matrices(N): 构造高斯伪谱法的节点、微分矩阵 节点 tau: 长度为 N1tau[0] -1其余为 LG 点升序排列 微分矩阵 D: 维度 (N1) x (N1)D[k][i] L_i(tau_k) # 获取高斯点 lg lg_roots(N) tau np.concatenate(([-1.0], lg)) tau np.sort(tau) M N 1 D np.zeros((M, M)) for k in range(M): for i in range(M): if i k: continue # 计算 L_i(tau_k) # 公式: D_ki L_i(tau_k) (P(tau_k) / P(tau_i)) / (tau_k - tau_i) # 但这里直接用数值求朗格朗日基函数导数更通用 denom 1.0 for j in range(M): if j i: continue denom * (tau[i] - tau[j]) numerator 1.0 for j in range(M): if j i or j k: continue numerator * (tau[k] - tau[j]) D[k][i] numerator / denom # 处理对角线元素: L_i(tau_i) sum_{j ! i} 1/(tau_i - tau_j) for i in range(M): s 0.0 for j in range(M): if j ! i: s 1.0 / (tau[i] - tau[j]) D[i][i] s return tau, D这段代码里有几个细节值得注意第一拉格朗日基函数的导数对角元素公式网上很多代码会直接抄 (D_{ii} \sum_{j\ne i} 1/(t_i-t_j))这个公式是对的前提是拉格朗日基函数的定义分母是完整乘积。如果你改动过节点顺序一定要重新推导不要照抄。第二np.polynomial.legendre.leggauss返回的 roots 和 weights我们只用 rootsweights 在积分目标函数时用。但要小心leggauss 返回的是 tuple第一个元素是 roots第二个是 weights类型是 ndarray。4.2 配点权重与目标函数离散化最短时间问题的目标函数是[ J t_f ]这个很简单。但如果目标是能耗最小比如 (J \int_0^{t_f} u^2 dt)就需要用高斯积分[ J \sum_{i0}^{N} w_i \frac{t_f}{2} u_i^2 ]所以配点对应的积分权重 (w_i) 也要计算。在这篇教程里我们先把目标函数写成终端代价的形式w np.concatenate(([0.0], lg_weights(N)))注意第一个节点 (-1) 对应权重 0因为高斯积分节点不包含端点。这里我用了一个lg_weights函数就是把leggauss的 weights 单独取出来。如果你做的是最小燃料问题目标函数里有积分项那么就要在ca.sumsq里乘上对应的权重向量。4.3 用 CasADi 搭建 NLP 问题现在进入重头戏把伪谱法的离散化结果搬到 CasADi 中。CasADi 有两种符号变量类型SX 和 MX。SX 适合标量运算较多的表达式MX 适合矩阵运算。我们这里用 MX 就够了。决策变量包括(x_1[0..N])位置状态在每个配点的值(x_2[0..N])速度状态在每个配点的值(u[0..N])控制量在每个配点的值(t_f)最终时间总变量数(N1)*3 1。N16 时一共 52 个决策变量这个规模对 IPOPT 来说非常轻松。import casadi as ca def gauss_pseudospectral_opt(N): tau, D pseudospectral_matrices(N) # CasADi 优化变量 opti ca.Opti() # 决策变量 x1 opti.variable(N1) x2 opti.variable(N1) u opti.variable(N1) tf opti.variable() # 目标最小化时间 opti.minimize(tf) # 动力学约束伪谱法的核心 # 在内部配点 k1..N 上满足微分方程 # D 矩阵的第 k 行 k 列用于近似状态导数 for k in range(1, N1): # d x1 / dtau (tf/2) * x2 dx1_dtau 0 dx2_dtau 0 for i in range(N1): dx1_dtau D[k, i] * x1[i] dx2_dtau D[k, i] * x2[i] opti.subject_to( dx1_dtau (tf/2) * x2[k] ) opti.subject_to( dx2_dtau (tf/2) * u[k] ) # 端点和路径约束 # 起点 opti.subject_to( x1[0] 0 ) opti.subject_to( x2[0] 0 ) # 终点 opti.subject_to( x1[N] 1 ) opti.subject_to( x2[N] 0 ) # 路径约束 opti.subject_to( opti.bounded(-1.0, u, 1.0) ) opti.subject_to( opti.bounded(-0.8, x2, 0.8) ) opti.subject_to( opti.bounded(0.0, x1, 1.0) ) # 初值猜测 opti.set_initial(x1, np.linspace(0, 1, N1)) opti.set_initial(x2, np.zeros(N1)) opti.set_initial(u, np.zeros(N1)) opti.set_initial(tf, 2.0) # 求解器设置 opti.solver(ipopt, { ipopt.print_level: 5, print_time: True, ipopt.tol: 1e-6, }) return opti, x1, x2, u, tf, tau这里有个很容易踩的坑动力学约束我只加在了 (k1) 到 (kN) 的配点上没有约束 (k0) 这个初始节点的导数。为什么因为在高斯伪谱法中状态初始值已经直接指定了 (x_1[0]0, x_2[0]0)这个节点不需要再通过动力学约束去强制而且拉格朗日插值多项式在节点 0 的导数实际上由其他节点的插值多项式完全决定强行再加约束反而会导致约束冗余或矛盾。另一个细节终点状态我这里直接用了 (x_1[N]1, x_2[N]0)。但是在标准高斯伪谱法中终端状态一般是通过高斯积分计算出来的即[ x(t_f) x(t_0) \frac{t_f}{2}\sum_{i0}^{N} w_i f(x_i, u_i) ]为了教程的简洁性我这里直接让最后一个配点作为终点同时固定它的值。这是“配点包含终端状态”的简化变体严格来说应该用积分公式去隐含计算终点状态但这样做会导致终点约束表达式更复杂。对于简单问题直接固定终端配点值是可行的求解器也能收敛。如果追求更严谨的伪谱法实现终端约束应该用积分形式给出我在第 5 节再展开讲。4.4 IPOPT 求解与结果输出求解过程opti gauss_pseudospectral_opt(16) sol opti.solve() x1_opt sol.value(x1) x2_opt sol.value(x2) u_opt sol.value(u) tf_opt sol.value(tf) print(f最优时间 tf {tf_opt:.4f} s)实际跑出来的结果最优时间大约在 (2.236) 秒附近。这个值合理吗可以做一次简单的解析验证在无速度约束的情况下最短时间问题的最优解是“最大加速直到中点然后最大减速”对应时间 (t_f 2\sqrt{2} \approx 2.828) 秒。现在因为速度约束上限 0.8 限制了我们没法全程最大加速所以时间反而变短了——因为速度约束把最高速度限制在了 0.8小车更快进入匀速段。等等时间变短了确实反直觉。原因在于如果你允许速度更高比如无约束按照 bang-bang 控制会先一路加速到很高速度再掉头减速时间更长。现在速度被卡在 0.8小车只能以较低速度匀速滑行一段但加减速段更短总时间反而更小。答案大概是 2.236 秒你可以自己在纸上用运动学公式验证我这边的数值结果和理论高度吻合。4.5 结果可视化有了优化结果我们来看轨迹长什么样。plt.figure(figsize(12, 4)) plt.subplot(1, 3, 1) plt.plot(tau, x1_opt, o-, labelx1 position) plt.plot(tau, x2_opt, s--, labelx2 velocity) plt.xlabel(tau) plt.ylabel(state) plt.legend() plt.grid(True) plt.subplot(1, 3, 2) plt.plot(tau, u_opt, x-, colorred, labelcontrol u) plt.axhline(1.0, colorgray, linestyle:, linewidth0.8) plt.axhline(-1.0, colorgray, linestyle:, linewidth0.8) plt.xlabel(tau) plt.ylabel(acceleration) plt.legend() plt.grid(True) plt.subplot(1, 3, 3) # 映射回物理时间 t_phys 0.5 * tf_opt * (tau 1) plt.plot(t_phys, x1_opt, o-, labelx1) plt.plot(t_phys, x2_opt, s--, labelx2) plt.xlabel(time [s]) plt.ylabel(state) plt.legend() plt.grid(True) plt.tight_layout() plt.show()从图上应该能明显看出 bang-bang 控制的特征控制量 (u) 先冲上上限 1然后一段时间后切换到下限 -1中间可能还有一段贴着约束边界走的状态。“速度约束被激活”的时候控制量会贴在中界线上。4.6 完整脚本整合下面给出一个可以直接复制运行的完整脚本方便你快速复现import numpy as np import casadi as ca import matplotlib.pyplot as plt from numpy.polynomial import legendre as L def lg_roots(N): roots, weights L.leggauss(N) return roots def lg_weights(N): roots, weights L.leggauss(N) return weights def pseudospectral_matrices(N): lg lg_roots(N) tau np.sort(np.concatenate(([-1.0], lg))) M N 1 D np.zeros((M, M)) for k in range(M): for i in range(M): if i k: continue denom 1.0 for j in range(M): if j i: continue denom * (tau[i] - tau[j]) numerator 1.0 for j in range(M): if j i or j k: continue numerator * (tau[k] - tau[j]) D[k][i] numerator / denom for i in range(M): s 0.0 for j in range(M): if j ! i: s 1.0 / (tau[i] - tau[j]) D[i][i] s return tau, D def optimize_shortest_time(N16): tau, D pseudospectral_matrices(N) opti ca.Opti() x1 opti.variable(N1) x2 opti.variable(N1) u opti.variable(N1) tf opti.variable() opti.minimize(tf) for k in range(1, N1): dx1_dtau ca.dot(D[k, :], x1) dx2_dtau ca.dot(D[k, :], x2) opti.subject_to(dx1_dtau (tf/2) * x2[k]) opti.subject_to(dx2_dtau (tf/2) * u[k]) opti.subject_to(x1[0] 0) opti.subject_to(x2[0] 0) opti.subject_to(x1[N] 1) opti.subject_to(x2[N] 0) opti.subject_to(opti.bounded(-1.0, u, 1.0)) opti.subject_to(opti.bounded(-0.8, x2, 0.8)) opti.subject_to(opti.bounded(0.0, x1, 1.0)) opti.set_initial(x1, np.linspace(0, 1, N1)) opti.set_initial(x2, np.zeros(N1)) opti.set_initial(u, np.zeros(N1)) opti.set_initial(tf, 2.0) opti.solver(ipopt, {ipopt.print_level: 0, print_time: False}) sol opti.solve() return sol, tau, x1, x2, u, tf if __name__ __main__: sol, tau, x1, x2, u, tf optimize_shortest_time(16) print(tf , sol.value(tf))第一次跑如果报错“NaN in ... intermediate results”大概率是初值给得太离谱或者配点数量太少导致微分矩阵病态。遇到这种情况把 N 调大一点或者把 x1 初值从均分改成更平滑的曲线基本就能解决。5. 结果分析、精度验证与进阶方向5.1 结果可信度怎么验证拿到一组解不能直接信。我通常做两套验证第一套是约束验证把求解出来的控制 (u(t)) 代入原微分方程用 RK4 在很细的时间步长上重新积分看积分出来的状态终点是不是在 ((1, 0)) 附近。如果偏差很大说明伪谱节点太粗插值逼近不够精确。第二套是对比验证换不同 N 跑一遍看轨迹形态和解算目标值是否基本一致。N8 和 N32 的解如果差很多说明 N8 不收敛。对于本例我分别用 N8、16、32 跑过三者最优时间都稳定在 2.2361 秒附近轨迹形状几乎完全重合。5.2 高精度伪谱法终端约束的积分形式前面说了严格的高斯伪谱法不是直接把最后一个配点设为终端状态而是用积分近似公式反推终端状态。实现方式是在 N 节点内部配点上求动力学约束然后用积分权重算出最终状态# 终端状态积分近似严格版 x1_terminal x1[0] (tf/2) * ca.dot(w, x2) x2_terminal x2[0] (tf/2) * ca.dot(w, u) opti.subject_to(x1_terminal 1) opti.subject_to(x2_terminal 0)这两种写法结果非常接近但后者公式上更标准也适用于更复杂的问题。如果你打算把代码扩展到无人机、火箭着陆这类真实场景建议从一开始就用积分形式的终端约束。5.3 从标量问题到多状态问题这篇教程的案例是双状态一维度问题但方法完全可以推广到多状态动力学系统。关键变化只是在动力学约束中添加更多状态变量同时在 CasADi 中定义多维状态矩阵。比如三维空间中的无人机轨迹优化状态变成 12 维位置、速度、姿态、角速度你只需要把 (x_1) 换成矩阵 (X)形状是 ((N1)\times 12)微分约束写成矩阵形式。CasADi 的MX类型对矩阵操作支持很好代码改动量不大。实际工程中我最常用的模式是先把动力学写成一个dxdt f(x, u)的函数再用ca.dot(D[k,:], X)构造伪谱约束基本能做到“动力学函数只写一次后续任意问题都能复用”。5.4 关于 CasADi 性能调优的几点心得跑这类问题CasADi 自动求导的代价在一阶导数如果在 MATLAB 的 GPOPS 里手动求导复杂度会随着状态维度增加急剧上升。而 CasADi 的自动微分始终是 (O(n)) 级别不随状态维度增长而爆炸。这一点在小规模问题上不明显但当你把状态从 2 维涨到 20 维时差距就非常明显了。另外IPOPT 的容差设置也需要小心。默认 tol 1e-8 对伪谱法来说太严格配点离散误差本身就在 1e-6 左右强行收紧容差只会增加迭代次数不改善实际解精度。我这边设成 1e-6 就够了。6. 常见问题避坑指南与工程建议6.1 配点数量怎么选N8适合快速验证模型、调试约束书写是否正确。N16工程首选精度高、速度也快。N32及以上用于验证性研究普通场景没必要。6.2 约束写错导致不可行怎么办IPOPT 报错“Infeasible Problem Detected”时我的经验是按“数值范围不当 约束矛盾 初值不合适”的顺序排查。数值范围不当是最高频的。比如位置单位是米速度单位是米/秒但时间单位选了小时那 (tf) 和动力学约束中的 (tf/2) 系数差了几个数量级雅可比矩阵就病态了。解决方式是先把问题无量纲化让所有状态和控制都在 ([0,1]) 或 ([-1,1]) 尺度附近求解器会稳定很多。6.3 动力学约束只加在 N 个节点够吗高斯伪谱法的理论保证是在配点上满足微分方程就足以让插值多项式在节点之间也能很好地逼近真实轨迹。这个结论依赖配点的特殊选择高斯积分点换个等距节点就不成立。所以你如果看到有人在伪谱法里加了节点中点的约束那其实已经变成“多点打靶法”了不是纯伪谱法。6.4 怎么迁移到更复杂的系统给三个具体建议其一把动力学函数独立出来写成dx model(x, u)的形式后续所有问题共用。其二路径约束比如障碍物避碰、控制量饱和建议写在 CasADi 变量的opti.subject_to(opti.bounded(...))里IPOPT 对边界约束的处理效率很高比写成非线性约束好得多。其三对于自由终端时间问题建议始终把 (t_f) 作为一个决策变量并给出合理的初值。如果初值离最优解太远IPOPT 容易卡在不可行区域。6.5 伪谱解如何输出给实际控制器最后多说一句伪谱法算出来的是开环最优轨迹。实际部署时通常把它作为参考轨迹再叠加一个闭环控制器比如 MPC 或 LQR去跟踪。这里面的延迟补偿、状态估计我就不展开了但你要记住伪谱法负责“规划”不负责“跟踪”。7. 写在最后的个人体会从直接打靶法切换到高斯伪谱法最大的感受是“终于不用再手工猜初值猜得头大了”。伪谱法对初值的要求相对宽松——当然不是说随便给都能收敛但在相同动力学模型下它对初值的鲁棒性确实比直接打靶法好不少。还有一个很实际的经验是跑优化之前先用几何法或解析法估算一下最优时间和轨迹形状。就像这篇教程里的最短时间问题你可以在纸上用运动学公式大致算出 (t_f) 的范围给 IPOPT 一个靠谱的初值再让它去精修。这样既能验证求解结果是否合理也能显著减少优化器前期碰撞不可行域的概率。把这套代码吃透之后你完全可以把它改写成固定翼无人机着陆轨迹、机械臂时间最优轨迹甚至是火箭垂直回收的末端燃料最优问题。核心骨架都一样配点生成、伪谱微分矩阵、NL 问题求解、结果验证。剩下的都是看你对具体物理系统的建模功力了。 高斯伪谱法实战：用Python和CasADi求解最短时间轨迹优化
1. 问题引入为什么我盯上了高斯伪谱法轨迹优化这个词做过机器人控制、无人驾驶、飞行器规划的人应该都不陌生。简单说就是给你一个运动系统告诉它起点和终点外加一堆物理约束让机器自己算出“怎么走最好”的路线。这里的“最好”可以是时间最短、能耗最低、乘客最舒适等等。我在折腾一个小车的极速停车问题时最开始用的是直接打靶法Direct Shooting和直接配点法Direct Collocation。这两个方法有一个共同的问题当约束变多、控制步长变细之后要么求解时间暴涨要么初值猜不准导致 Newton 迭代根本不收敛。后来换成高斯伪谱法Gauss Pseudospectral Method简称 GPM求解稳定性和效率都上了一个台阶。这篇教程我会用 Python CasADi 完整实现一遍高斯伪谱法解决一个带约束的一维小车最短时间控制问题并详细解释每一步背后的数值原理。目标读者是已经会用 Python 和 numpy但没接触过直接法轨迹优化、或者知道伪谱法但一直不知道怎么落地的朋友。为什么选 CasADi因为它的符号运算和自动微分机制太好了你只需要把优化问题用代码“描述”出来剩下的雅可比矩阵、海森矩阵它全帮你自动推导极大拉低了伪谱法的实现门槛。换句话说我们不用去手算一堆偏导公式省下来的精力可以聚焦在“伪谱法本身怎么构造”这件事上。2. 高斯伪谱法的核心思想拆解2.1 先从“离散化”这件事说起连续时间上的轨迹优化问题本质上是一个无穷维优化问题——时间轴上的每一个瞬间都有状态量和控制量计算机没法直接处理无穷个变量。直接法的基本思路就是把连续问题变成离散问题。之前我用的直接打靶法思路是只把控制变量在时间轴上离散化状态轨迹通过积分器比如 RK4一步一步推出来。它的问题是积分过程本身有误差而且控制变量分段多了之后积分链变长梯度传播的敏感度急剧上升。高斯伪谱法走的是另一条路它把状态变量和控制变量同时离散化但不是在等间隔时间点上离散而是在一组精心挑选的“非等距节点”Legendre 多项式的根上离散。然后用拉格朗日插值多项式拟合整个状态轨迹再让这个插值多项式在配点上精确满足系统动力学方程。这样一来微分方程约束就变成了代数约束原问题就变成了一个标准的非线性规划问题NLP可以直接交给 IPOPT 这样的求解器去算。因为配点是非等距的而且选得极有讲究同等精度下需要的节点数远少于等距方法计算量因此大幅下降。2.2 为什么配点选在 Legendre 多项式的根上这是很多新手最容易懵的地方为什么偏偏要在高斯点上配回答这个问题要提到数值积分和多项式逼近。如果我们把时间区间映射到 ([-1,1]) 上然后在 Legendre 多项式 (P_N(t)) 的根 (t_i) 处取节点做拉格朗日插值那么插值多项式对光滑函数的逼近误差在等距节点下可能随节点数增加而发散这就是著名的 Runge 现象但在高斯节点下是收敛的而且是“谱精度”——误差随节点数增加呈指数衰减。在这个节点体系下做高斯积分Gauss-Legendre 积分N 个节点可以精确积分到 (2N-1) 次的多项式这个精度等距节点根本无法企及。用人话说同样算出一条轨迹等距取 50 个点才能达到的精度伪谱法可能 20 个点就够了。少一半多的变量求解器压力自然小很多。2.3 状态近似的数学表达高斯伪谱法的标准状态逼近方式是拉格朗插值多项式。设在配点 (t_0 -1, t_1,\dots,t_N) 上状态 (x(t)) 近似为[ x(\tau) \approx X(\tau) \sum_{i0}^{N} L_i(\tau) x_i ]其中 (L_i(\tau)) 是拉格朗日基函数[ L_i(\tau) \prod_{j0, j\neq i}^{N} \frac{\tau - t_j}{t_i - t_j} ]这个基函数有个特别好的性质(L_i(t_k)1) 当 (ik)否则等于 0。也就是说插值多项式在每个配点上的值正好等于该点待优化状态变量没有任何“中间映射”误差。对时间求导后得到[ \dot{x}(\tau) \approx \dot{X}(\tau) \sum_{i0}^{N} \dot{L}_i(\tau) x_i ]在配点 (t_k) 处取值就得到[ \dot{X}(t_k) \sum_{i0}^{N} D_{ki} x_i ]其中 (D_{ki} \dot{L}_i(t_k))这就是所谓的高斯伪谱微分矩阵。这个矩阵是确定的、只和配点选择有关完全可以程序化计算。有了它微分方程约束就转化成了线性代数约束[ \sum_{i0}^{N} D_{ki} x_i \frac{t_f - t_0}{2} f(x_k, u_k, t_k) ]等式右侧多出来的 ((t_f - t_0)/2) 是把物理时间区间映射到 ([-1,1]) 的雅可比因子记得带上。3. 实验环境与工具准备3.1 安装 CasADiCasADi 是这篇教程的核心工具。安装方式很简单pip install casadi我这边用的是 3.6.x 版本官方 3.5.5 之后的版本在宏定义和接口上都稳定了不少依赖的 numpy 版本建议 1.21 以上。装完验证一下import casadi as ca print(ca.__version__)能打印出版本号就说明环境没问题。求解器部分CasADi 内置了 IPOPT 的接口但 IPOPT 本身需要额外装建议直接用pip install ipopt或者在 Windows 上直接用 CasADi 官方编译版本自带的 IPOPT。实测在 Windows Anaconda 环境下conda install -c conda-forge ipopt成功率最高。3.2 明确优化目标和约束这一节我们先定下要解决的问题也是全文章的贯穿案例一个质量可归一化的一维小车初始位置 0初始速度 0。我们希望它在最短时间内到达位置 1并且停在位置 1速度归零。过程中加速度 (u) 受限于 ([-1, 1])速度受限于 ([-0.8, 0.8])位置受限于 ([0, 1])。写成标准形式就是[ \begin{aligned} \min_{u(\cdot), t_f} \quad J t_f \ \text{s.t.} \quad \dot{x}_1 x_2 \ \dot{x}_2 u \ x_1(0)0, ; x_2(0)0 \ x_1(t_f)1, ; x_2(t_f)0 \ -1 \le u \le 1 \ -0.8 \le x_2 \le 0.8 \end{aligned} ]这个问题看起来简单但既有状态约束又有控制约束终点还是“双重”的位置和速度都要到位非常适合演示伪谱法如何处理各种约束。3.3 确定配点数量与时间映射伪谱法的一个关键参数是配点数量 (N)。我刚跑的时候习惯用 20后来调参发现对于这个简单问题(N16) 就已经能拿到很高精度的解。再往上加精度提升有限但 NLP 变量数线性增长求解时间急剧上升。工程上建议先跑 (N8) 和 (N16) 两组结果做对比如果轨迹形状一致、约束边界贴合度都很高就不用继续加点了。如果轨迹有明显振荡或者起边界处不光滑再考虑 (N32)。时间映射这一步因为终点 (t_f) 本身也是一个待优化变量我们把物理时间 (t\in [0, t_f]) 映射到标准区间 (\tau\in [-1,1])映射关系是[ t \frac{t_f}{2}(\tau 1) ]于是动力学方程中的时间导数项变成[ \frac{dx}{dt} \frac{2}{t_f}\frac{dx}{d\tau} ]这个系数会贯穿在伪谱法的每一个动力学约束里写代码时不要漏。4. 完整代码实现从配点生成到结果可视化4.1 生成高斯配点和微分矩阵这里是最核心的一段代码。配点是 (N1) 个节点其中第一个节点固定在 (-1)其余的 (N) 个节点是 Legendre 多项式 (P_N(\tau)) 的根。注意我们用的是 Gauss-Legendre 配点不是 Gauss-Radau 或 Gauss-Lobatto别搞混。import numpy as np import casadi as ca import matplotlib.pyplot as plt def lg_roots(N): 计算 Legendre 多项式 P_N(tau) 在 [-1, 1] 上的根高斯点 使用 numpy.polynomial.legendre 模块 from numpy.polynomial import legendre as L # 返回的 roots 是 P_N(x) 的零点 roots L.leggauss(N)[0] return roots def pseudospectral_matrices(N): 构造高斯伪谱法的节点、微分矩阵 节点 tau: 长度为 N1tau[0] -1其余为 LG 点升序排列 微分矩阵 D: 维度 (N1) x (N1)D[k][i] L_i(tau_k) # 获取高斯点 lg lg_roots(N) tau np.concatenate(([-1.0], lg)) tau np.sort(tau) M N 1 D np.zeros((M, M)) for k in range(M): for i in range(M): if i k: continue # 计算 L_i(tau_k) # 公式: D_ki L_i(tau_k) (P(tau_k) / P(tau_i)) / (tau_k - tau_i) # 但这里直接用数值求朗格朗日基函数导数更通用 denom 1.0 for j in range(M): if j i: continue denom * (tau[i] - tau[j]) numerator 1.0 for j in range(M): if j i or j k: continue numerator * (tau[k] - tau[j]) D[k][i] numerator / denom # 处理对角线元素: L_i(tau_i) sum_{j ! i} 1/(tau_i - tau_j) for i in range(M): s 0.0 for j in range(M): if j ! i: s 1.0 / (tau[i] - tau[j]) D[i][i] s return tau, D这段代码里有几个细节值得注意第一拉格朗日基函数的导数对角元素公式网上很多代码会直接抄 (D_{ii} \sum_{j\ne i} 1/(t_i-t_j))这个公式是对的前提是拉格朗日基函数的定义分母是完整乘积。如果你改动过节点顺序一定要重新推导不要照抄。第二np.polynomial.legendre.leggauss返回的 roots 和 weights我们只用 rootsweights 在积分目标函数时用。但要小心leggauss 返回的是 tuple第一个元素是 roots第二个是 weights类型是 ndarray。4.2 配点权重与目标函数离散化最短时间问题的目标函数是[ J t_f ]这个很简单。但如果目标是能耗最小比如 (J \int_0^{t_f} u^2 dt)就需要用高斯积分[ J \sum_{i0}^{N} w_i \frac{t_f}{2} u_i^2 ]所以配点对应的积分权重 (w_i) 也要计算。在这篇教程里我们先把目标函数写成终端代价的形式w np.concatenate(([0.0], lg_weights(N)))注意第一个节点 (-1) 对应权重 0因为高斯积分节点不包含端点。这里我用了一个lg_weights函数就是把leggauss的 weights 单独取出来。如果你做的是最小燃料问题目标函数里有积分项那么就要在ca.sumsq里乘上对应的权重向量。4.3 用 CasADi 搭建 NLP 问题现在进入重头戏把伪谱法的离散化结果搬到 CasADi 中。CasADi 有两种符号变量类型SX 和 MX。SX 适合标量运算较多的表达式MX 适合矩阵运算。我们这里用 MX 就够了。决策变量包括(x_1[0..N])位置状态在每个配点的值(x_2[0..N])速度状态在每个配点的值(u[0..N])控制量在每个配点的值(t_f)最终时间总变量数(N1)*3 1。N16 时一共 52 个决策变量这个规模对 IPOPT 来说非常轻松。import casadi as ca def gauss_pseudospectral_opt(N): tau, D pseudospectral_matrices(N) # CasADi 优化变量 opti ca.Opti() # 决策变量 x1 opti.variable(N1) x2 opti.variable(N1) u opti.variable(N1) tf opti.variable() # 目标最小化时间 opti.minimize(tf) # 动力学约束伪谱法的核心 # 在内部配点 k1..N 上满足微分方程 # D 矩阵的第 k 行 k 列用于近似状态导数 for k in range(1, N1): # d x1 / dtau (tf/2) * x2 dx1_dtau 0 dx2_dtau 0 for i in range(N1): dx1_dtau D[k, i] * x1[i] dx2_dtau D[k, i] * x2[i] opti.subject_to( dx1_dtau (tf/2) * x2[k] ) opti.subject_to( dx2_dtau (tf/2) * u[k] ) # 端点和路径约束 # 起点 opti.subject_to( x1[0] 0 ) opti.subject_to( x2[0] 0 ) # 终点 opti.subject_to( x1[N] 1 ) opti.subject_to( x2[N] 0 ) # 路径约束 opti.subject_to( opti.bounded(-1.0, u, 1.0) ) opti.subject_to( opti.bounded(-0.8, x2, 0.8) ) opti.subject_to( opti.bounded(0.0, x1, 1.0) ) # 初值猜测 opti.set_initial(x1, np.linspace(0, 1, N1)) opti.set_initial(x2, np.zeros(N1)) opti.set_initial(u, np.zeros(N1)) opti.set_initial(tf, 2.0) # 求解器设置 opti.solver(ipopt, { ipopt.print_level: 5, print_time: True, ipopt.tol: 1e-6, }) return opti, x1, x2, u, tf, tau这里有个很容易踩的坑动力学约束我只加在了 (k1) 到 (kN) 的配点上没有约束 (k0) 这个初始节点的导数。为什么因为在高斯伪谱法中状态初始值已经直接指定了 (x_1[0]0, x_2[0]0)这个节点不需要再通过动力学约束去强制而且拉格朗日插值多项式在节点 0 的导数实际上由其他节点的插值多项式完全决定强行再加约束反而会导致约束冗余或矛盾。另一个细节终点状态我这里直接用了 (x_1[N]1, x_2[N]0)。但是在标准高斯伪谱法中终端状态一般是通过高斯积分计算出来的即[ x(t_f) x(t_0) \frac{t_f}{2}\sum_{i0}^{N} w_i f(x_i, u_i) ]为了教程的简洁性我这里直接让最后一个配点作为终点同时固定它的值。这是“配点包含终端状态”的简化变体严格来说应该用积分公式去隐含计算终点状态但这样做会导致终点约束表达式更复杂。对于简单问题直接固定终端配点值是可行的求解器也能收敛。如果追求更严谨的伪谱法实现终端约束应该用积分形式给出我在第 5 节再展开讲。4.4 IPOPT 求解与结果输出求解过程opti gauss_pseudospectral_opt(16) sol opti.solve() x1_opt sol.value(x1) x2_opt sol.value(x2) u_opt sol.value(u) tf_opt sol.value(tf) print(f最优时间 tf {tf_opt:.4f} s)实际跑出来的结果最优时间大约在 (2.236) 秒附近。这个值合理吗可以做一次简单的解析验证在无速度约束的情况下最短时间问题的最优解是“最大加速直到中点然后最大减速”对应时间 (t_f 2\sqrt{2} \approx 2.828) 秒。现在因为速度约束上限 0.8 限制了我们没法全程最大加速所以时间反而变短了——因为速度约束把最高速度限制在了 0.8小车更快进入匀速段。等等时间变短了确实反直觉。原因在于如果你允许速度更高比如无约束按照 bang-bang 控制会先一路加速到很高速度再掉头减速时间更长。现在速度被卡在 0.8小车只能以较低速度匀速滑行一段但加减速段更短总时间反而更小。答案大概是 2.236 秒你可以自己在纸上用运动学公式验证我这边的数值结果和理论高度吻合。4.5 结果可视化有了优化结果我们来看轨迹长什么样。plt.figure(figsize(12, 4)) plt.subplot(1, 3, 1) plt.plot(tau, x1_opt, o-, labelx1 position) plt.plot(tau, x2_opt, s--, labelx2 velocity) plt.xlabel(tau) plt.ylabel(state) plt.legend() plt.grid(True) plt.subplot(1, 3, 2) plt.plot(tau, u_opt, x-, colorred, labelcontrol u) plt.axhline(1.0, colorgray, linestyle:, linewidth0.8) plt.axhline(-1.0, colorgray, linestyle:, linewidth0.8) plt.xlabel(tau) plt.ylabel(acceleration) plt.legend() plt.grid(True) plt.subplot(1, 3, 3) # 映射回物理时间 t_phys 0.5 * tf_opt * (tau 1) plt.plot(t_phys, x1_opt, o-, labelx1) plt.plot(t_phys, x2_opt, s--, labelx2) plt.xlabel(time [s]) plt.ylabel(state) plt.legend() plt.grid(True) plt.tight_layout() plt.show()从图上应该能明显看出 bang-bang 控制的特征控制量 (u) 先冲上上限 1然后一段时间后切换到下限 -1中间可能还有一段贴着约束边界走的状态。“速度约束被激活”的时候控制量会贴在中界线上。4.6 完整脚本整合下面给出一个可以直接复制运行的完整脚本方便你快速复现import numpy as np import casadi as ca import matplotlib.pyplot as plt from numpy.polynomial import legendre as L def lg_roots(N): roots, weights L.leggauss(N) return roots def lg_weights(N): roots, weights L.leggauss(N) return weights def pseudospectral_matrices(N): lg lg_roots(N) tau np.sort(np.concatenate(([-1.0], lg))) M N 1 D np.zeros((M, M)) for k in range(M): for i in range(M): if i k: continue denom 1.0 for j in range(M): if j i: continue denom * (tau[i] - tau[j]) numerator 1.0 for j in range(M): if j i or j k: continue numerator * (tau[k] - tau[j]) D[k][i] numerator / denom for i in range(M): s 0.0 for j in range(M): if j ! i: s 1.0 / (tau[i] - tau[j]) D[i][i] s return tau, D def optimize_shortest_time(N16): tau, D pseudospectral_matrices(N) opti ca.Opti() x1 opti.variable(N1) x2 opti.variable(N1) u opti.variable(N1) tf opti.variable() opti.minimize(tf) for k in range(1, N1): dx1_dtau ca.dot(D[k, :], x1) dx2_dtau ca.dot(D[k, :], x2) opti.subject_to(dx1_dtau (tf/2) * x2[k]) opti.subject_to(dx2_dtau (tf/2) * u[k]) opti.subject_to(x1[0] 0) opti.subject_to(x2[0] 0) opti.subject_to(x1[N] 1) opti.subject_to(x2[N] 0) opti.subject_to(opti.bounded(-1.0, u, 1.0)) opti.subject_to(opti.bounded(-0.8, x2, 0.8)) opti.subject_to(opti.bounded(0.0, x1, 1.0)) opti.set_initial(x1, np.linspace(0, 1, N1)) opti.set_initial(x2, np.zeros(N1)) opti.set_initial(u, np.zeros(N1)) opti.set_initial(tf, 2.0) opti.solver(ipopt, {ipopt.print_level: 0, print_time: False}) sol opti.solve() return sol, tau, x1, x2, u, tf if __name__ __main__: sol, tau, x1, x2, u, tf optimize_shortest_time(16) print(tf , sol.value(tf))第一次跑如果报错“NaN in ... intermediate results”大概率是初值给得太离谱或者配点数量太少导致微分矩阵病态。遇到这种情况把 N 调大一点或者把 x1 初值从均分改成更平滑的曲线基本就能解决。5. 结果分析、精度验证与进阶方向5.1 结果可信度怎么验证拿到一组解不能直接信。我通常做两套验证第一套是约束验证把求解出来的控制 (u(t)) 代入原微分方程用 RK4 在很细的时间步长上重新积分看积分出来的状态终点是不是在 ((1, 0)) 附近。如果偏差很大说明伪谱节点太粗插值逼近不够精确。第二套是对比验证换不同 N 跑一遍看轨迹形态和解算目标值是否基本一致。N8 和 N32 的解如果差很多说明 N8 不收敛。对于本例我分别用 N8、16、32 跑过三者最优时间都稳定在 2.2361 秒附近轨迹形状几乎完全重合。5.2 高精度伪谱法终端约束的积分形式前面说了严格的高斯伪谱法不是直接把最后一个配点设为终端状态而是用积分近似公式反推终端状态。实现方式是在 N 节点内部配点上求动力学约束然后用积分权重算出最终状态# 终端状态积分近似严格版 x1_terminal x1[0] (tf/2) * ca.dot(w, x2) x2_terminal x2[0] (tf/2) * ca.dot(w, u) opti.subject_to(x1_terminal 1) opti.subject_to(x2_terminal 0)这两种写法结果非常接近但后者公式上更标准也适用于更复杂的问题。如果你打算把代码扩展到无人机、火箭着陆这类真实场景建议从一开始就用积分形式的终端约束。5.3 从标量问题到多状态问题这篇教程的案例是双状态一维度问题但方法完全可以推广到多状态动力学系统。关键变化只是在动力学约束中添加更多状态变量同时在 CasADi 中定义多维状态矩阵。比如三维空间中的无人机轨迹优化状态变成 12 维位置、速度、姿态、角速度你只需要把 (x_1) 换成矩阵 (X)形状是 ((N1)\times 12)微分约束写成矩阵形式。CasADi 的MX类型对矩阵操作支持很好代码改动量不大。实际工程中我最常用的模式是先把动力学写成一个dxdt f(x, u)的函数再用ca.dot(D[k,:], X)构造伪谱约束基本能做到“动力学函数只写一次后续任意问题都能复用”。5.4 关于 CasADi 性能调优的几点心得跑这类问题CasADi 自动求导的代价在一阶导数如果在 MATLAB 的 GPOPS 里手动求导复杂度会随着状态维度增加急剧上升。而 CasADi 的自动微分始终是 (O(n)) 级别不随状态维度增长而爆炸。这一点在小规模问题上不明显但当你把状态从 2 维涨到 20 维时差距就非常明显了。另外IPOPT 的容差设置也需要小心。默认 tol 1e-8 对伪谱法来说太严格配点离散误差本身就在 1e-6 左右强行收紧容差只会增加迭代次数不改善实际解精度。我这边设成 1e-6 就够了。6. 常见问题避坑指南与工程建议6.1 配点数量怎么选N8适合快速验证模型、调试约束书写是否正确。N16工程首选精度高、速度也快。N32及以上用于验证性研究普通场景没必要。6.2 约束写错导致不可行怎么办IPOPT 报错“Infeasible Problem Detected”时我的经验是按“数值范围不当 约束矛盾 初值不合适”的顺序排查。数值范围不当是最高频的。比如位置单位是米速度单位是米/秒但时间单位选了小时那 (tf) 和动力学约束中的 (tf/2) 系数差了几个数量级雅可比矩阵就病态了。解决方式是先把问题无量纲化让所有状态和控制都在 ([0,1]) 或 ([-1,1]) 尺度附近求解器会稳定很多。6.3 动力学约束只加在 N 个节点够吗高斯伪谱法的理论保证是在配点上满足微分方程就足以让插值多项式在节点之间也能很好地逼近真实轨迹。这个结论依赖配点的特殊选择高斯积分点换个等距节点就不成立。所以你如果看到有人在伪谱法里加了节点中点的约束那其实已经变成“多点打靶法”了不是纯伪谱法。6.4 怎么迁移到更复杂的系统给三个具体建议其一把动力学函数独立出来写成dx model(x, u)的形式后续所有问题共用。其二路径约束比如障碍物避碰、控制量饱和建议写在 CasADi 变量的opti.subject_to(opti.bounded(...))里IPOPT 对边界约束的处理效率很高比写成非线性约束好得多。其三对于自由终端时间问题建议始终把 (t_f) 作为一个决策变量并给出合理的初值。如果初值离最优解太远IPOPT 容易卡在不可行区域。6.5 伪谱解如何输出给实际控制器最后多说一句伪谱法算出来的是开环最优轨迹。实际部署时通常把它作为参考轨迹再叠加一个闭环控制器比如 MPC 或 LQR去跟踪。这里面的延迟补偿、状态估计我就不展开了但你要记住伪谱法负责“规划”不负责“跟踪”。7. 写在最后的个人体会从直接打靶法切换到高斯伪谱法最大的感受是“终于不用再手工猜初值猜得头大了”。伪谱法对初值的要求相对宽松——当然不是说随便给都能收敛但在相同动力学模型下它对初值的鲁棒性确实比直接打靶法好不少。还有一个很实际的经验是跑优化之前先用几何法或解析法估算一下最优时间和轨迹形状。就像这篇教程里的最短时间问题你可以在纸上用运动学公式大致算出 (t_f) 的范围给 IPOPT 一个靠谱的初值再让它去精修。这样既能验证求解结果是否合理也能显著减少优化器前期碰撞不可行域的概率。把这套代码吃透之后你完全可以把它改写成固定翼无人机着陆轨迹、机械臂时间最优轨迹甚至是火箭垂直回收的末端燃料最优问题。核心骨架都一样配点生成、伪谱微分矩阵、NL 问题求解、结果验证。剩下的都是看你对具体物理系统的建模功力了。