如何用线性约束限制KinematicTrajectoryOptimization的q̇(t)？公式及实现是否可行？

问题背景 #

我需要在指定位置约束轨迹的关节速度（比如将起始、结束位置的速度设为0），KinematicTrajectoryOptimization提供了两个相关API，但都不符合需求：

• AddPathVelocityConstraint：原实现不直接约束关节速度q̇(t)
• AddVelocityConstraintAtNormalizedTime：原实现采用非凸通用约束，无法满足线性约束的要求

公式可行性验证 #

你给出的推导是完全可行的，核心逻辑如下：

q̇(t) = drds * dsdt = ṙ(s) / duration,
duration * lb ≤ ṙ(s) ≤ duration * ub.

其中：

• q̇(t)是关节速度，ṙ(s)是路径速度（s为归一化路径参数，范围[0,1]）
• duration是轨迹总时长，dsdt = 1/duration（因为s随时间从0线性变化到1）

通过对ṙ(s)施加duration*lb ≤ ṙ(s) ≤ duration*ub的线性约束，等价于直接对q̇(t)施加lb ≤ q̇(t) ≤ ub的线性约束，完全满足线性约束的要求。

对应实现代码分析 #

你提供的代码逻辑正确，能实现目标：

KinematicTrajectoryOptimization::AddPathVelocityConstraint(
    const Eigen::Ref<const Eigen::VectorXd>& lb,
    const Eigen::Ref<const Eigen::VectorXd>& ub, double s) {
  const VectorX<symbolic::Expression> sym_rdot_value = sym_rdot_->value(s);
  auto binding =
      prog_.AddLinearConstraint(lb * duration_ <= sym_rdot_value && sym_rdot_value <= ub * duration_);
  binding.evaluator()->set_description("path real velocity constraint");
  return binding;
}

代码说明：

1. 获取归一化路径参数s对应的路径速度符号表达式sym_rdot_value
2. 调用AddLinearConstraint添加线性约束，将ṙ(s)限制在duration*lb到duration*ub区间，间接实现对q̇(t)的线性约束
3. 为约束添加描述，方便后续调试和维护

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