Drake中MPC推箱控制仿真优化缓慢的原因与解决问询

问题1：提升仿真连续性与优化速度的方法 #

• 变量维度裁剪：当前每个时间步都将作用点作为独立优化变量，可大幅压缩变量空间：比如预设作用点的可选区域（如仅允许在箱子的前/后两个面选点，或限定作用点在某一轴上的坐标），而非全自由维度优化；若作用点的调整对动力学影响有限，可固定作用点仅优化推力，后续再按需引入作用点变量。
• 动力学模型与离散方法优化：
  • 换掉Forward Euler，改用中点法或隐式欧拉法，这类方法稳定性更强，可采用更大的离散步长，减少预测时域内的节点数量，同时保证仿真连续性；
  • 对接触约束做近似简化：将库仑摩擦的非线性约束替换为分段线性近似，或仅在检测到接触状态变化时激活约束，避免每个时间步都计算复杂的互补约束。
• 优化框架调整：
  • 启用warm start：每次MPC优化直接用上一次的优化结果（状态、控制量、作用点）作为初始猜测，而非从零开始迭代，Python求解器如IPOPT、CasADi均支持该参数配置，能大幅减少收敛迭代次数；
  • 若场景允许，将direct collocation替换为single shooting方法，虽然对初值敏感，但当前MPC的初值是实时状态，收敛速度会更快，变量数也更少。
• 求解器与代码层面优化：
  • 调优求解器参数：放宽收敛容限（如将tol从1e-6改为1e-4），设置mu_strategy='adaptive'（IPOPT），更换更快的线性求解器（如MA57）；
  • 用CasADi将优化问题编译为C代码（casadi.Function.compile()），规避Python解释器的性能瓶颈；
• 仿真连续性优化：
  • 给控制量添加平滑约束：限制相邻时间步的推力、作用点的变化率（如|u_k - u_{k-1}| <= delta），或在目标函数中加入控制量变化的惩罚项；
  • 用**一阶保持（FOH）**代替零阶保持（ZOH），让作用点与推力的输出更连续，避免仿真跳变。

问题2：仿真缓慢的核心原因分析 #

• 接触与刚体动力学的高度非线性是主要诱因：接触约束属于非光滑互补约束（法向力≥0、接触间隙≤0、二者乘积为0），这类约束会导致优化问题的雅可比矩阵、海森矩阵不连续，求解器需要更多迭代次数来处理非光滑点；刚体动力学本身是多自由度耦合模型，加上作用点变量的耦合，进一步提升了计算复杂度。
• 此外，direct collocation方法本身会将每个时间步的状态、控制量、作用点都作为优化变量，即使缩短预测时域，变量总数仍会随节点数线性增长，这也是优化耗时的次要原因，但你已尝试缩短时域，因此非线性因素的影响更为突出。

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