Drake轨迹优化中决策变量与浮点值比较及区间依赖问题解决方案咨询

我目前正在处理一个涉及二进制执行器的轨迹优化问题。为了避免求解MINLP（混合整数非线性规划），我没有直接对状态和控制输入做优化，而是假设每个二进制执行器在“开启”和“关闭”状态间交替，仅对两类状态的区间时长做优化，存储该决策变量的数组记为h，下文示例中为N×2矩阵。

以下是采用双积分器的最小示例，该系统有两个控制输入，分别对系统施加正向和负向力：

此处我将状态轨迹建模为三阶多项式序列，我不希望将这些执行器合并为一个状态为-1、0、1的执行器，因为我要适配的通用系统还包含更多二进制执行器。我添加了额外约束：多项式连接的连续性与可微性约束、所有区间时长之和等于期望终态时间约束、初态与终态约束，以及系统动力学约束。我最初的想法是在固定间隔上施加动力学约束，即：

但问题在于，任意时间t下每个执行器可能处于任意区间，且区间时长可能收缩到0，可能出现一个执行器处于最后一个区间、另一个仍处于第一个区间的情况，也就是决策变量（时长）的取值会改变决策变量之间的依赖关系。该问题在Drake中的表现是：当Tau是Drake表达式、t是数值时，我无法执行Tau < t这类比较操作。代码片段如下：

# Enforce dynamics at the end of each control interval
for t in np.arange(0, Tf, dt_dyn):
    # Find the index of the interval that is active for each actuator
    t_ctrl = np.cumsum(h, axis=0)
    intervals = (t_ctrl < t)
    idxs = np.sum(intervals, axis=0)
    # If the idx is even the actuator is off, otherwise its on
    prog.AddConstraint(eq(qdd(q_a, t, dt_state),
                          continuous_dynamics(q(q_a, t, dt_state),
                                              qd(q_a, t, dt_state),
                                              [idxs[0] % 2, idxs[1] % 2])))

对应的报错信息如下：

Traceback (most recent call last):
  File "test.py", line 92, in <module>
    intervals = (t_ctrl < t)
RuntimeError: You should not call `__bool__` / `__nonzero__` on `Formula`. If you are trying to make a map with `Variable`, `Expression`, or `Polynomial` as keys (and then access the map in Python), please use pydrake.common.containers.EqualToDict`.

我的问题偏向概念层面而非技术实现：Drake是否有其他方式支持这种“决策变量依赖于自身取值”的特性？或者是否有其他问题转录方式，可以避免给定时间下执行器所属区间的歧义问题？

这类区间时长为决策变量的轨迹优化属于多阶段优化范畴，Drake不支持直接对符号表达式做分支判断，你可以通过两种无分支的转录方案解决该问题：

• 时间缩放法（首选）：给每个控制区间单独定义局部时间域，将区间总时长h作为决策变量，直接在每个区间的局部时间内定义状态多项式、施加动力学约束，不需要在全局固定时间点做区间归属判断。相邻区间的端点处额外施加连续性、可微性约束即可，所有约束均为符号友好的代数约束，不需要引入额外二进制变量，Drake的MathematicalProgram完全支持求解。
• 大M约束重构法：如果必须在全局固定时间点施加约束，可引入二进制辅助变量重写区间归属逻辑：对每个执行器的第k个区间、第t个采样点，引入0-1变量b_{k,t}表示t时刻该执行器处于区间k，通过大M法将sum_{i=1}^{k-1}h_i ≤ t ≤ sum_{i=1}^k h_i的逻辑判断转化为线性不等式约束，最终每个时刻的控制输入可表示为sum(b_{k,t} * u_k)，其中u_k是第k个区间对应的执行器开关状态。Drake提供AddLogicalConstraint接口可直接实现这类逻辑约束的自动转录，不需要手动编写大M不等式。

这两种方案都可以完全规避“用符号表达式做大小比较产生分支”的问题，适配多二进制执行器的通用场景。

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