在Drake中为7自由度机械臂非线性MPC整合质量矩阵与偏差项

问题描述 #

我正尝试在Drake中为7自由度机械臂实现非线性模型预测控制（Non Linear MPC）。为此，我需要在约束中引入依赖决策变量q、q_dot的动力学参数，如质量矩阵M(q)和偏差项C(q,q_dot)*q_dot。

我进行了如下尝试：

// finalize plant
// create builder, diagram, context, plant context
...

// formulate optimazation problem
drake::solvers::MathematicalProgram prog;

// create decision variables
...
std::vector<drake::solvers::VectorXDecisionVariable> q_v;
std::vector<drake::solvers::VectorXDecisionVariable> q_ddot;

for (int i = 0; i < H; i++) {
    q_v.push_back(prog.NewContinuousVariables<14>(state_var_name));
    q_ddot.push_back(prog.NewContinuousVariables<7>(input_var_name));
}

// add cost
...

// add constraints
...
for (int i = 0; i < H; i++) {
    plant.SetPositionsAndVelocities(*plant_context, q_v[i]);
    plant.CalcMassMatrix(*plant_context, M);
    plant.CalcBiasTerm(*plant_context, C_q_dot);
}

...

for (int i = 0; i < H; i++) {
    prog.AddConstraint( M * q_ddot[i] + C_q_dot + G >= lb );
    prog.AddConstraint( M * q_ddot[i] + C_q_dot + G <= ub );
}

// solve prog
...

上述代码无法运行，因为plant.SetPositionsAndVelocities(.)不支持符号变量。

请问是否有方法将M、C整合到我的最优控制问题（OCP）约束中？

可行解决方案 #

方法1：利用符号动力学生成约束表达式 #

Drake支持符号计算接口，可直接基于决策变量生成符号形式的M(q)和C(q,q̇)q̇，步骤如下：

• 创建符号上下文（SymbolicPlantContext）替代数值上下文
• 将决策变量q_v[i]（包含位置和速度）赋值给符号上下文的状态
• 通过plant.CalcMassMatrixSymbolic和plant.CalcBiasTermSymbolic获取符号形式的M和C项
• 用这些符号表达式直接构建约束：

for (int i = 0; i < H; i++) {
    // 提取当前时刻的位置、速度和加速度决策变量
    const auto& q = q_v[i].head(7);
    const auto& q_dot = q_v[i].tail(7);
    const auto& q_ddot = q_ddot_v[i];

    // 创建符号上下文
    auto symbolic_context = plant.CreateDefaultContext();
    plant.GetMutablePositions(symbolic_context.get()) = q;
    plant.GetMutableVelocities(symbolic_context.get()) = q_dot;

    // 计算符号形式的质量矩阵、偏差项和重力项
    drake::symbolic::Matrix<M_size, M_size> M_sym = plant.CalcMassMatrixSymbolic(*symbolic_context);
    drake::symbolic::VectorX C_sym = plant.CalcBiasTermSymbolic(*symbolic_context);
    drake::symbolic::VectorX G_sym = plant.CalcGravityGeneralizedForces(*symbolic_context);

    // 构建动力学约束：M*q_ddot + C + G 处于上下界范围内
    auto constraint_expr = M_sym * q_ddot + C_sym + G_sym;
    prog.AddConstraint(constraint_expr >= lb);
    prog.AddConstraint(constraint_expr <= ub);
}

方法2：使用CalcInverseDynamics符号接口 #

如果约束基于关节力矩上下界（即τ ∈ [lb, ub]），而动力学方程为τ = M(q)q̈ + C(q,q̇)q̇ + G(q)，可直接用CalcInverseDynamicsSymbolic生成符号化力矩表达式，再添加约束：

for (int i = 0; i < H; i++) {
    const auto& q = q_v[i].head(7);
    const auto& q_dot = q_v[i].tail(7);
    const auto& q_ddot = q_ddot_v[i];

    auto symbolic_context = plant.CreateDefaultContext();
    plant.GetMutablePositions(symbolic_context.get()) = q;
    plant.GetMutableVelocities(symbolic_context.get()) = q_dot;

    // 生成符号化的关节力矩表达式
    drake::symbolic::VectorX tau_sym = plant.CalcInverseDynamicsSymbolic(
        *symbolic_context, q_ddot, drake::VectorX<double>::Zero(7));

    // 添加力矩上下界约束
    prog.AddConstraint(tau_sym >= lb);
    prog.AddConstraint(tau_sym <= ub);
}

方法3：使用Drake内置非线性MPC工具链（推荐） #

若不想手动构建OCP，可直接使用Drake提供的DirectTranscription、TrajectoryOptimization工具或上层ModelPredictiveController框架，这些工具会自动处理动力学约束的符号化构建：

// 示例：用DirectTranscription构建MPC轨迹优化问题
drake::trajectories::PiecewisePolynomial<double> initial_trajectory = ...;
auto dirtran = drake::planning::DirectTranscription(plant, initial_trajectory);
dirtran.AddDurationCost(1.0);
dirtran.AddPathPositionConstraint(...); // 添加其他自定义约束

// 求解优化问题
drake::solvers::MathematicalProgramResult result = dirtran.Solve();

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