基于Drake的最优控制器仿真：如何建模真实控制延迟？

我正在开发不同的最优控制器，并使用Drake求解器与仿真器进行测试。我将控制器实现为LeafSystem，通过PeriodicDiscreteUpdateEvent触发实际控制步骤：

self.DeclarePeriodicDiscreteUpdateEvent(period_sec=0.1, offset_sec=0.0, update=self._discrete_update)

def _discrete_update(self, context : Context, discrete_state : DiscreteValues):
    system_state = self._plant_output_port.Eval(context)
    control_input, time = self._control(system_state)
    discrete_state.get_mutable_vector(0).SetFromVector(control_input)

控制器输出定义如下：

self._plant_input_port = self.DeclareStateOutputPort(name="plant_input", state_index=self._out_state)

上述实现运行良好，但我希望尽可能贴近真实场景测试反馈系统，因此不确定如何建模实际控制延迟。由于是最优控制器，self._control(system_state)方法调用最长可能耗时0.1秒。

据我对Drake仿真的理解，当UpdateEvent触发时，整个仿真会等待控制器计算输入、更新离散状态（控制输入），之后才基于新控制输入积分连续系统。但我期望的是，如同真实场景一样，系统在控制器计算输入的同时继续推进。

我曾尝试在_discrete_update方法中将控制输入写入另一个中间离散状态向量，并添加一个时间偏移为0.1秒的PeriodicDiscreteUpdateEvent，将中间状态同步至实际输出状态，但不确定是否会引发竞态条件。此外，我还发现可使用DiscreteTimeDelay模块，但这些方法均未利用实际求解时间，仅将输出固定延迟一段时间。我想了解是否存在规范方法实现该需求。

要在Drake中建模控制器计算耗时带来的真实延迟，可以采用以下两种规范方案：

1. 异步事件+延迟状态同步 #

这是最贴近真实硬件行为的方式，核心思路是让控制器计算在后台异步执行，同时仿真继续推进，待计算完成后再将控制输入应用到系统：

• 拆分控制器逻辑：在_discrete_update中仅触发异步计算（比如用Python的threading或concurrent.futures），而非直接阻塞仿真等待结果。
• 新增一个离散状态存储"待应用的控制输入"，同时添加触发式事件（而非周期性事件），当异步计算完成后触发该事件，将计算结果写入输出状态。
• 注意：需要给Drake仿真线程和计算线程加线程安全的同步机制（比如threading.Lock），避免竞态条件。

示例伪代码：

import threading

def __init__(self):
    # 声明中间状态和输出状态
    self._pending_control = self.DeclareDiscreteState(control_dim)
    self._out_state = self.DeclareDiscreteState(control_dim)
    self._plant_input_port = self.DeclareStateOutputPort("plant_input", self._out_state)
    
    # 周期性触发计算请求
    self.DeclarePeriodicDiscreteUpdateEvent(0.1, 0.0, self._trigger_control_compute)
    
    # 声明触发式事件用于更新输出
    self._update_output_event = self.DeclareDiscreteUpdateEvent(
        trigger=EventTrigger(never_trigger=True),
        update=self._apply_pending_control
    )
    self._lock = threading.Lock()
    self._new_control_ready = False

def _trigger_control_compute(self, context, discrete_state):
    system_state = self._plant_output_port.Eval(context)
    # 异步启动控制计算
    threading.Thread(target=self._async_control_compute, args=(system_state,)).start()

def _async_control_compute(self, system_state):
    control_input, _ = self._control(system_state)
    with self._lock:
        # 将结果写入中间状态
        self._pending_control.SetFromVector(control_input)
        self._new_control_ready = True
    # 触发输出更新事件
    self._update_output_event.TriggerAt(context.get_time())  # 立即触发

def _apply_pending_control(self, context, discrete_state):
    with self._lock:
        if self._new_control_ready:
            # 将中间状态同步到输出状态
            discrete_state.get_mutable_vector(self._out_state).SetFromVector(
                self._pending_control.GetVector().CopyToVector()
            )
            self._new_control_ready = False

2. 实时模式仿真+实际耗时补偿 #

如果目标是让仿真严格匹配真实时间推进，可以开启Drake仿真器的实时模式：

• 调用simulator.set_target_realtime_rate(1.0)，让仿真速度和真实时间同步。
• 在_discrete_update中直接执行self._control(system_state)，此时仿真会等待计算完成（就像真实控制器占用CPU时间一样），自然产生与实际计算耗时一致的延迟。
• 这种方式无需额外建模延迟，完全依赖真实计算时间模拟控制延迟，适合高保真度的硬件在环测试场景。

关于你尝试方案的说明 #

• 固定偏移的周期性事件：若控制器计算耗时稳定在0.1秒，该方案可行且不会有竞态条件——因为Drake的事件执行是串行的，同一时间只会处理一个离散更新事件。但它无法匹配_control方法的实际波动耗时，会引入不必要的额外延迟。
• DiscreteTimeDelay模块：它仅适合建模固定时间的传输延迟，而非计算延迟，因此无法满足你的需求。

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