使用DurationCost与AccelerationBounds时KinematicTrajectoryOptimization求解失败

问题复现 #

当使用Drake的KinematicTrajectoryOptimization时，同时添加DurationCost和AccelerationBounds（即使将加速度边界设为无穷大），会出现求解失败的情况，但移除其中任意一项即可成功求解。复现代码如下：

import numpy as np
from pydrake.planning import KinematicTrajectoryOptimization
from pydrake.solvers import Solve

trajopt = KinematicTrajectoryOptimization(num_positions=6, num_control_points=10)
prog = trajopt.get_mutable_prog()

positions = np.array([np.ones(6) * 0.9, 
                      np.ones(6) * 0.8, 
                      np.ones(6) * 0.5, 
                      np.zeros(6)])

for i,joint in enumerate(positions):
    trajopt.AddPathPositionConstraint(
        joint, joint , i / (len(positions) - 1)
    )

trajopt.AddDurationCost(0.5)
trajopt.AddAccelerationBounds(-np.inf * np.ones(6), np.inf * np.ones(6))

result = Solve(prog)
if not result.is_success():
    raise RuntimeError("Drake trajectory failed")

原因解析 #

1. 退化约束的数值干扰
   即便将AccelerationBounds设为无穷大，Drake底层也不会直接忽略这些约束，而是仍会生成对应的约束表达式框架。这类“退化”约束会引入冗余的变量或形式化约束，导致优化问题的数值条件变差——求解器迭代时可能因初始点与冗余约束的交互，出现收敛困难。

2. 路径速度变量的耦合冲突
   KinematicTrajectoryOptimization基于路径参数s∈[0,1]参数化轨迹，总时间（DurationCost的优化目标）和加速度约束都与路径速度v(s)=ds/dt直接耦合：

   • DurationCost本质是最小化总时间∫₀¹ 1/v(s) ds，属于关于v(s)的凸成本函数；
   • 加速度约束会被转化为关于v(s)及其导数dv/ds的凸约束。
     两者同时存在时，即便加速度约束是无穷大，求解器默认的迭代策略（如初始点选择、收敛容忍度）也可能无法适配这种看似无约束但实际有变量耦合的退化场景，最终导致迭代终止于失败状态。

3. 求解器默认参数的适配问题
   Drake默认使用的凸求解器（如OSQP、SCS）对问题数值稳定性有一定要求。添加无穷大边界的加速度约束后，问题的约束矩阵可能出现条件数异常，或对偶变量迭代出现振荡，超出求解器默认容忍度范围，进而导致求解失败。

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