基于Drake Direct Collocation的关联关节角度约束轨迹规划咨询

要实现「当theta1处于34°-36°范围时，theta2禁止处于116°-118°范围」的约束，本质是排除theta1在目标区间且theta2也在目标区间的情况。以下是两种直接可行的实现方案：

方法1：乘积约束（最直接，无需额外变量） #

利用数学乘积的符号特性：当theta1和theta2同时处于各自目标区间时，以下表达式的结果为正；我们通过约束该表达式≤0，即可排除这种违规情况。

代码实现 #

import numpy as np
from pydrake.symbolic import Expression

# 定义角度阈值（转换为弧度）
theta1_low = np.deg2rad(34)
theta1_high = np.deg2rad(36)
theta2_low = np.deg2rad(116)
theta2_high = np.deg2rad(118)

# 获取状态变量（注意：根据你的机器人模型调整state索引，确保对应theta1和theta2）
state = dircol.state()
theta1 = state[0]  # 示例：假设theta1是第一个状态变量
theta2 = state[1]  # 示例：假设theta2是第二个状态变量

# 构造约束表达式：排除theta1和theta2同时在目标区间的情况
constraint_expr = (theta1 - theta1_low) * (theta1_high - theta1) * (theta2 - theta2_low) * (theta2_high - theta2)

# 将约束添加到所有轨迹节点
_ = dircol.AddConstraintToAllKnotPoints(constraint_expr <= 0)

注意事项 #

• 该约束属于非凸约束，优化器可能会陷入局部最优，求解时间也可能更长，但胜在实现简单、无需引入额外变量。
• 务必确认state的索引对应正确的关节角度，可通过打印机器人模型的状态变量列表验证。

方法2：大M法（凸约束，需引入二进制变量） #

如果需要凸约束以提升求解稳定性，可通过引入二进制变量实现逻辑「或」约束，但要求求解器支持混合整数非线性规划（MINLP）（如Gurobi）。

代码实现 #

import numpy as np

# 定义角度阈值（转换为弧度）
theta1_low = np.deg2rad(34)
theta1_high = np.deg2rad(36)
theta2_low = np.deg2rad(116)
theta2_high = np.deg2rad(118)
M = np.deg2rad(100)  # 足够大的松弛系数，确保约束在不生效时不影响结果

# 遍历所有轨迹节点添加约束
for t in range(dircol.num_time_samples()):
    state_t = dircol.state(t)
    theta1_t = state_t[0]
    theta2_t = state_t[1]
    
    # 为当前节点添加4个二进制变量，分别对应4种合法情况
    z1 = dircol.prog().AddBinaryVariable(f"z1_{t}")  # z1=1时，theta1 ≤ theta1_low
    z2 = dircol.prog().AddBinaryVariable(f"z2_{t}")  # z2=1时，theta1 ≥ theta1_high
    z3 = dircol.prog().AddBinaryVariable(f"z3_{t}")  # z3=1时，theta2 ≤ theta2_low
    z4 = dircol.prog().AddBinaryVariable(f"z4_{t}")  # z4=1时，theta2 ≥ theta2_high
    
    # 添加对应约束
    dircol.prog().AddConstraint(theta1_t <= theta1_low + M * (1 - z1))
    dircol.prog().AddConstraint(theta1_t >= theta1_high - M * (1 - z2))
    dircol.prog().AddConstraint(theta2_t <= theta2_low + M * (1 - z3))
    dircol.prog().AddConstraint(theta2_t >= theta2_high - M * (1 - z4))
    
    # 约束至少满足一种合法情况
    dircol.prog().AddConstraint(z1 + z2 + z3 + z4 >= 1)

注意事项 #

• 该方法通过引入二进制变量将逻辑约束转化为凸约束，求解更稳定，但会增加问题复杂度，且需要支持MINLP的求解器。

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