如何消除 Direct Collocation 重建状态轨迹中的振荡

2026-11-04排障赵师傅

结论

应先排查离散化误差、求解容差和模型尺度。这里的振荡很可能是 Direct Collocation 在稀疏网格上产生的伪解。轨迹只需在 knot points 和 collocation points 满足离散后的动力学约束，而 ReconstructStateTrajectory() 返回的三次多项式无法保证区间内每个时刻都严格满足连续动力学。

建议按以下顺序处理：

将时间步长基准改为 T / (N - 1)。
明确调用 SnoptSolver().Solve(prog)，并收紧 feasibility tolerance。
密集采样重建轨迹，检查动力学残差。
用粗网格解 warm start 更细的网格，避免增加 sample points 后直接冷启动。
添加速度、输入变化率或加速度正则项，并限制输入。
检查末端 link 的惯量、joint damping 和执行器等效惯量是否合理。

仅增加 knot 数量，或只在 knot points 添加 velocity limits，通常无法消除区间内部的振荡。

为什么 knot points 正常，区间内部仍会振荡

Direct Collocation 优化的是有限个 knot points 上的状态和输入。对于相邻节点 (i) 和 (i+1)，Drake 根据状态及其动力学导数构造分段多项式，并在 collocation point 施加动力学缺陷约束。

因此，离散问题满足的是有限组方程，而非：

[ \dot{x}(t)=f(x(t),u(t)),\quad \forall t ]

对于机械系统：

[ x = \begin{bmatrix}q\v\end{bmatrix} ]

重建的 (q(t)) 和 (v(t)) 都来自分段多项式。在约束点之外，数值上可能出现：

[ \frac{d q(t)}{dt} \neq N(q(t))v(t) ]

或者：

[ \dot{v}(t) \neq f_v(q(t),v(t),u(t)) ]

所以，knot points 上的状态平滑，不代表重建轨迹在区间内部也合理。

如果增加 sample points 后振荡频率反而升高，通常是优化器找到了另一组网格尺度上的伪解，或者问题的数值尺度较差，不能据此断定 collocation points 的实现有误。

先修正时间步长设置

num_time_samples=N 对应 N - 1 个时间区间。固定总时长为 (T)，且所有区间等长时，每个区间应为：

[ h=\frac{T}{N-1} ]

当前代码使用了 time_constraint / num_samples。当 N=11、T=1 时，实际时间步长为 0.1，代码设置的最大值也正好是 0.1。这样一来，每个时间步都会落在构造函数允许范围的上边界，平白增加一个活动约束，影响数值求解。

可以改成：

num_samples = 11
duration = 1.0
nominal_h = duration / (num_samples - 1)

direct_collocation = DirectCollocation(
    plant,
    plant_context,
    num_time_samples=num_samples,
    input_port_index=plant.get_actuation_input_port().get_index(),
    minimum_time_step=0.5 * nominal_h,
    maximum_time_step=1.5 * nominal_h,
)

direct_collocation.AddEqualTimeIntervalsConstraints()
direct_collocation.AddDurationBounds(duration, duration)

总时长固定、区间等长时，最终仍会得到 h = nominal_h。放宽构造时的取值范围，可以避免时间步长同时卡在边界上。

确认真正使用了 SNOPT，并收紧容差

代码虽然创建了 SnoptSolver，最后调用的却是通用入口：

result = Solve(prog)

Solve() 会自行选择求解器。即便当前环境最终选中了 SNOPT，也建议明确调用，以免安装环境变化后使用了其他求解器：

solver = SnoptSolver()
solver_id = solver.solver_id()

prog.SetSolverOption(
    solver_id, "Major feasibility tolerance", 1e-7)
prog.SetSolverOption(
    solver_id, "Major optimality tolerance", 1e-6)
prog.SetSolverOption(
    solver_id, "Minor feasibility tolerance", 1e-7)
prog.SetSolverOption(
    solver_id, "Minor optimality tolerance", 1e-6)

result = solver.Solve(prog)

对于轨迹优化中的动力学等式约束，1e-3 可能过松，尤其是在状态量、力矩和惯量相差多个尺度时。上面的数值可作为排查起点，不是所有模型通用的固定配置。如果收紧容差后求解变得困难，应先检查变量尺度和模型惯量，不要继续盲目缩小容差。

求解完成后，还要查看 infeasible constraints 和动力学约束的实际残差，不能只检查 result.is_success()。

密集检查连续动力学残差

要区分振荡来自轨迹重建误差还是动力学本身，可以在每个区间内取多个点并计算：

[ r(t)=\dot{x}{\text{recon}}(t) -f(x{\text{recon}}(t),u_{\text{recon}}(t)) ]

示例：

x_traj = direct_collocation.ReconstructStateTrajectory(result)
u_traj = direct_collocation.ReconstructInputTrajectory(result)
xdot_traj = x_traj.derivative(1)

check_context = plant.CreateDefaultContext()

sample_times = np.linspace(
    x_traj.start_time(),
    x_traj.end_time(),
    1001,
)

residual_norms = []

for t in sample_times:
    x_value = x_traj.value(t).reshape(-1)
    u_value = u_traj.value(t).reshape(-1)
    xdot_reconstructed = xdot_traj.value(t).reshape(-1)

    plant.SetPositionsAndVelocities(check_context, x_value)
    plant.get_actuation_input_port().FixValue(check_context, u_value)

    xdot_dynamics = (
        plant.EvalTimeDerivatives(check_context).CopyToVector()
    )

    residual_norms.append(
        np.linalg.norm(xdot_reconstructed - xdot_dynamics)
    )

print("maximum dense dynamics residual:", max(residual_norms))

还可以分别检查 position kinematics 和 velocity dynamics：

nq = plant.num_positions()
nv = plant.num_velocities()

position_residuals = []
velocity_residuals = []

for t in sample_times:
    x_value = x_traj.value(t).reshape(-1)
    u_value = u_traj.value(t).reshape(-1)
    xdot_reconstructed = xdot_traj.value(t).reshape(-1)

    plant.SetPositionsAndVelocities(check_context, x_value)
    plant.get_actuation_input_port().FixValue(check_context, u_value)

    xdot_dynamics = (
        plant.EvalTimeDerivatives(check_context).CopyToVector()
    )

    residual = xdot_reconstructed - xdot_dynamics
    position_residuals.append(np.linalg.norm(residual[:nq]))
    velocity_residuals.append(np.linalg.norm(residual[nq:nq + nv]))

print("maximum position residual:", max(position_residuals))
print("maximum velocity residual:", max(velocity_residuals))

如果残差主要在 knot points 之间增大，说明网格上的离散轨迹无法准确表示连续动力学。此时需要做 mesh refinement、调整正则项或改善模型尺度。

也可以将重建的输入轨迹送入连续 plant，从初始状态重新仿真。如果仿真轨迹与 ReconstructStateTrajectory() 明显分离，说明当前 collocation 解的连续动力学一致性不足。

用上一层网格的解初始化更细网格

直接把 num_samples 从 11 改成较大的数值并冷启动，优化器可能落入新的局部解，振荡也可能随着网格加密而变快。

可以逐级细化网格：

coarse_x = coarse_dircol.ReconstructStateTrajectory(coarse_result)
coarse_u = coarse_dircol.ReconstructInputTrajectory(coarse_result)

fine_dircol.SetInitialTrajectory(coarse_u, coarse_x)
fine_result = fine_solver.Solve(fine_dircol.prog())

例如依次使用 11、21、41 个 knot points，每一层都用上一层的重建轨迹初始化，并在求解后重新计算密集动力学残差。如果网格细化后残差和振幅同时下降，说明这一处理方向有效。

如果加密网格后，振荡始终集中在一两个区间的尺度上，还需要检查求解容差、缩放和代价函数。

给优化器加入平滑偏好

当前目标函数只有输入能量：

direct_collocation.AddRunningCost(u.T @ R @ u)

在固定时间内，这项代价未必能抑制状态或输入的快速变化。可以加入适度的 velocity cost：

nq = plant.num_positions()
nv = plant.num_velocities()

x = direct_collocation.state()
v = x[nq:nq + nv]

Qv = 1e-3 * np.eye(nv)
direct_collocation.AddRunningCost(v.T @ Qv @ v)

也可以惩罚相邻 knot points 的输入变化。在固定等时间间隔下，下面的形式近似惩罚 (\int |\dot{u}|^2dt)：

weight_du = 1e-3
h = duration / (num_samples - 1)

for i in range(num_samples - 1):
    du = (
        direct_collocation.input(i + 1)
        - direct_collocation.input(i)
    )
    prog.AddQuadraticCost((weight_du / h) * du.dot(du))

如果所用 Drake 版本没有公开 input(i) 重载，可以通过该版本提供的 input decision-variable accessor 添加同样的代价。

velocity cost 和 input-rate cost 的权重应逐步增加，同时观察任务轨迹、控制量和动力学残差。权重过大可能改变原问题的最优解，甚至使固定时间的边界条件难以满足。

添加真实的输入和状态限制

最小示例没有设置 actuator effort limits。对于惯量很小的关节，优化器可能利用很大的瞬时输入和加速度，得到数值上可行但物理上没有意义的轨迹。

输入限制可以直接加到所有 knot points：

u = direct_collocation.input()

u_lower = np.array([-20.0, -20.0, -20.0])
u_upper = np.array([ 20.0,  20.0,  20.0])

direct_collocation.AddConstraintToAllKnotPoints(u >= u_lower)
direct_collocation.AddConstraintToAllKnotPoints(u <= u_upper)

这里的 20.0 仅用于展示代码结构，实际数值必须取自执行器和传动系统。

joint position limits 和 velocity limits 也应采用机器人的真实范围。施加在 knot points 上的 bound 无法保证三次重建曲线在区间内部不越界，因此仍需密集采样检查。如果存在越界，应继续细化网格，或者为边界预留一定裕量。

任务允许时，还可以限制平均加速度：

a_max = np.array([5.0, 5.0, 5.0])
h = duration / (num_samples - 1)

for i in range(num_samples - 1):
    v0 = direct_collocation.state(i)[nq:nq + nv]
    v1 = direct_collocation.state(i + 1)[nq:nq + nv]
    dv = v1 - v0

    prog.AddBoundingBoxConstraint(
        -a_max * h,
         a_max * h,
         dv,
    )

该约束控制相邻节点间的平均加速度，无法单独约束区间内部的瞬时加速度。它可以抑制节点间的交替跳变，但不能替代密集残差检查。

检查 damping 与惯量的相对尺度

为 revolute joint 设置：

<dynamics damping="0.1"/>

通常表示粘性阻尼系数，其力矩与关节速度成正比。这个数值是否过大，要结合关节的等效惯量、齿轮传动和单位设置判断。对于惯量很小的末端 link，近似时间尺度为：

[ \tau \sim \frac{J}{d} ]

其中 (J) 是关节看到的等效惯量，(d) 是 damping。较小的 (\tau) 会形成快速动力学模式，使固定阶数的 collocation 难以在粗网格上准确表示。

增加末端 link 的 mass 或 length 后振荡有所改善，说明需要检查：

URDF 中 mass、length、inertia tensor 的单位是否一致。
inertia tensor 是否符合 link 的实际几何和质量。
末端 link 是否被建模成近乎无惯量的刚体。
damping="0.1" 对该关节是否过大。
是否遗漏了真实执行器的电机转子惯量和减速器反射惯量。
是否缺少 actuator effort limits。

修改 link mass 不应成为消除优化问题的手段。如果真实机器人包含电机和减速器，应将它们对关节等效惯量的贡献纳入动力学模型。具体建模方式取决于当前 Drake 版本和机器人模型格式。

修正状态维度的写法

示例假设：

NUM_JOINTS = plant.get_actuation_input_port().size()
start_state = np.zeros(NUM_JOINTS * 2)

这种写法只适用于 num_positions == num_velocities == num_actuators 的完全驱动简单关节系统。更稳妥的写法是：

nq = plant.num_positions()
nv = plant.num_velocities()

start_state = np.zeros(nq + nv)
end_state = np.concatenate([
    np.ones(nq),
    np.zeros(nv),
])

如果模型中有 quaternion、floating base 或未驱动关节，就不能根据 actuation input 的维度推断 state 维度。