企业官网建设流程全解析

先别急着去翻那些满屏 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 Method，GPM）最大的卖点是它的精度来源——Legendre-Gauss 求积公式。对光滑问题，误差随节点数 (N) 的增加呈指数级下降，也就是所谓的“谱精度”。这意味着一般问题用二三十个点就够了，而普通配点法可能要几百个点。

另一个实用优势是加约束方便。状态约束、控制约束、路径约束，都只是在对应的配点处加上代数不等式，不改变整个求解框架。这在航天领域尤其受欢迎，像火箭入轨、卫星变轨、再入大气层这类问题，GPM 几乎是标配思路。这篇教程虽然用小车举例，但代码框架换成三维动力学、加上推力约束和大气阻力，就是一枚简化版运载火箭的轨迹优化问题。

2. 高斯伪谱法的三个核心步骤

2.1 用多项式去“猜”轨迹：Lagrange 插值

伪谱法的第一个核心思想：把整条状态轨迹看成一条光滑曲线，用插值多项式去逼近它。

假设我们有 (N+1) 个离散时间点（下面会讲具体是什么点），记为 (\tau_1, \tau_2, \ldots, \tau_{N+1})，对应的状态值是 (x_1, x_2, \ldots, x_{N+1})。我们用 Lagrange 插值构造一个 (N) 阶多项式：

[ x(\tau) \approx \sum_{j=1}^{N+1} x_j \cdot L_j(\tau) ]

其中 (L_j(\tau)) 是第 (j) 个 Lagrange 基函数。这里有个非常舒服的性质：基函数 (L_j(\tau)) 在第 (j) 个节点上取值为 1，在其他节点上取值 0，所以多项式系数就是我们离散的状态值本身。换句话说，我们不需要额外求解插值系数，直接把点拎出来就是多项式的系数。

很多人第一次看到这里会犯晕：我们不是要求轨迹吗？怎么先假设轨迹是多项式了？注意，这里的“多项式”不是我们臆造的结果，而是我们对未知轨迹的一种参数化描述。我们让状态取一系列未知数值 (x_1, \ldots, x_{N+1})，然后用这些未知数构造一条多项式曲线，最后通过优化把这些未知数解出来。未知的是这些点上的取值，而不是多项式本身的形式，这和有限元里“形函数”的思想如出一辙。

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_{k=1}^{N} w_k f(\tau_k) ]

只要选用合适的高斯求积权重 (w_k)，这个公式对最高 (2N-1) 次的多项式是精确成立的。这一点极其关键：后面计算目标函数的积分项时，误差不会因为离散化而额外累积。

2.3 把微分方程变成代数方程：微分矩阵与初始矩阵

有了节点和多项式，下一步就是把微分方程约束变成代数方程。

高斯伪谱法的状态节点配置是这样的：(N) 个 LG 配点 (\tau_1, \ldots, \tau_N)，再加上一个终端节点 (\tau_{N+1} = 1)，一共 (N+1) 个状态节点。控制量只在 (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,N+1 ]

有了 (D)，多项式在配点处的导数就变成了一个矩阵乘法：

[ \left.\frac{dx}{d\tau}\right|{\tau=\tau_k} \approx \sum{j=1}^{N+1} 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) 是 (N+1) 维状态向量，(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。映射关系可以整理成一张表：

决策变量的总数很容易算出来：对 (n) 维状态、(m) 维控制的系统，大约是 (n(N+1) + mN) 个变量，外加末端时间 (t_f) 如果也自由优化的话再加一个。对双积分器，就是 (2(N+1) + 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 casadi

3.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)

离散化方案我们已经在第二节推导完了。状态变量有两组，每组 (N+1) 个点；控制变量一组，(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，控制量本来就只在配点上定义。初学者最容易错的就是这里：状态是 (N+1) 个点，控制是 (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), sharex=True) axes[0].plot(t_all, x_opt, 'o', label='GPM') axes[0].plot(tt, x_true, '-', label='analytic') axes[0].set_ylabel('x') axes[0].legend() axes[1].plot(t_all, v_opt, 'o', label='GPM') axes[1].plot(tt, v_true, '-', label='analytic') axes[1].set_ylabel('v') axes[1].legend() axes[2].plot(tc, u_opt, 'o', label='GPM') axes[2].plot(tt, u_true, '-', label='analytic') 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) 在 (t=0) 附近超过 6、在 (t=1) 附近低于 -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 常见问题速查表

这部分内容都是我自己折腾出来的实战经验，几乎每条都踩过坑，直接放一张速查表：

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 能看到迭代详情，跑通了再降到 0；max_iter默认 3000，对大型问题经常不够，直接设大一点比如1e5；如果遇到收敛困难，先调tol放松一点，比如从1e-8放到1e-6，看看问题是否改善。一个容易忽略的事：IPOPT 对问题的尺度极其敏感。如果你的状态是速度（量级 0~10），位置（量级 0~1000），时间（量级 100），那一定要先做无量纲化，把变量都拉到 (O(1)) 量级，否则收敛速度会非常感人。

第五个心得，也是我个人最喜欢的一条：用免费的午餐检验方法。一旦你的高斯伪谱法代码框架写好，把它当作一个通用的离散化工具，去套各种你能找到解析解或已知结果的最优控制问题——双积分器、倒立摆、月球软着陆简化模型。每多验证一个，你对这个方法的信心就增加一分。到最后你会发现，真正难的不是“用伪谱法”，而是“把问题建得足够好”：状态怎么选变量、约束怎么提才不病态、目标怎么表达才光滑。方法本身反倒是相对固定的。

这套工具链还有一个很自然的扩展方向：把单段伪谱法改成多段拼接，在不同段用不同的配点数，处理带有阶段切换的问题（比如火箭助推器分离、变轨中间点约束）。CasADi 对这类问题支持得也很好，只需要把每段的离散化矩阵拼起来，再加上段间连续性约束即可。学完这篇教程，你已经有了一个可以一步步往上搭的扎实底座。