如何提升pydrake中SNOPT求解器的MPC轨迹优化速度

问题背景 #

我用PyDrake的SNOPT求解器解决基于线性倒立摆模型的多步行走轨迹优化问题，当前求解2步轨迹耗时0.6秒，需要提速以满足实时MPC需求。

已尝试的提速方法及疑问 #

1. 放宽容差无效果，代码如下：

self.mp.SetSolverOption(SnoptSolver().solver_id(), "Major feasibility tolerance", 1e-4)
self.mp.SetSolverOption(SnoptSolver().solver_id(), "Minor feasibility tolerance", 1e-4)

2. solve()耗时0.2秒，查看snopt.out发现首次主迭代前完成了600次主/次迭代及300次QP次迭代，疑问：这是否意味着暖启动可将耗时减半？Drake是否支持SNOPT的暖启动？（参考Stack Overflow帖子称不支持）
3. 某一非线性Python约束被评估500次，占总求解时间的60%，疑问：通过Cython或纯C实现能否提升效率？是否有相关资源参考，Py转C能带来多大性能提升？

附snopt.out部分内容：

SNMEMA EXIT 100 -- finished successfully
 SNMEMA INFO 104 -- memory requirements estimated

针对性解答 #

1. 容差调整无效的原因 #

SNOPT的主/次可行性容差对迭代次数的影响取决于问题约束的紧密度。如果你的问题原本约束就宽松，或者SNOPT在默认容差下已经快速收敛，放宽容差自然不会有明显变化。可以尝试同时调整Major optimality tolerance和Minor optimality tolerance，或者查看snopt.out中迭代终止的具体原因，确认是否是容差触发的收敛。

2. 暖启动的可行性与Drake支持情况 #

从snopt.out的迭代统计来看，首次主迭代前的大量迭代可能是SNOPT初始化阶段的可行性恢复计算。暖启动确实可以复用前一次求解的最优解、对偶变量等信息，减少初始化迭代次数，理论上能显著降耗时。但目前Drake的SNOPT接口不支持显式暖启动，这和你看到的Stack Overflow结论一致。不过可以退而求其次：将前一次求解的最优解作为当前问题的初始猜测值（通过MathematicalProgram.SetInitialGuess()设置），虽不是严格的SNOPT暖启动，但能帮SNOPT更快定位可行域，减少迭代次数。

3. 非线性约束的性能优化 #

Python实现的非线性约束因为解释器开销，高频评估时必然是瓶颈。用Cython或纯C++重写约束是非常有效的优化手段，性能提升通常能达到10-100倍（取决于约束的计算复杂度）：

• 纯C实现：基于Drake的C接口，继承Constraint类实现约束逻辑，再通过PyDrake导出到Python使用，Drake官方文档有自定义C++约束并绑定Python的教程；
• Cython实现：将核心计算逻辑用Cython编写，减少Python解释器开销，适合不想完全切换到C++的场景。

遗漏的优化点 #

1. 问题结构检查：线性倒立摆模型的大部分约束应该是线性的，确认是否误将线性约束用非线性接口实现——线性约束的求解效率远高于非线性约束；
2. SNOPT参数调优：除容差外，还可调整这些参数：
   • 设置Major iterations limit和Minor iterations limit，避免不必要的迭代；
   • 将Hessian approximation设为BFGS或Limited-memory BFGS，减少二阶信息的计算开销；
   • 若问题结构稳定，开启Cold start option，跳过部分初始化步骤；
3. 缩减问题规模：检查轨迹优化的时间步长是否可减少，或状态/控制变量是否有冗余，尽可能降低问题维度；
4. 并行计算：若有多个独立的约束评估，可尝试并行化约束计算，但需注意PyDrake的线程安全性。

内容的提问来源于stack exchange，提问作者Zion