参考轨迹系统t0输出零值t1输出首状态的问题排查咨询

问题现象 #

在带控制器的闭环仿真中实现参考轨迹跟踪时，发现t0时刻出现极大的tau_cmd。排查后确认，t0时刻期望状态为零值而非参考轨迹的首个状态，直到t1时刻才输出参考轨迹的首个状态，这是力矩过大的直接原因。

已做尝试 #

已在构建diagram并连接所有端口后，显式设置参考轨迹子系统的初始状态为参考轨迹首个状态，代码如下：

diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()

s_ref_traj_sys_context = diagram.GetMutableSubsystemContext(s_ref_sys, diagram_context)
s_ref_traj_sys_context.get_mutable_discrete_state_vector().SetFromVector(state_traj_ref[0, :])

疑问 #

1. 是否遗漏了关键步骤？
2. 有没有推荐的参考轨迹跟踪实现方式？

补充：参考轨迹类代码 #

使用LeafSystem定义的参考轨迹类如下：

class JointStateReferenceTrajectory(LeafSystem):
    def __init__(self, 
                 t_offset, 
                 query_rate,
                 s_traj,
                 times,
                 hs,
                 a_init,
                 print_to_console=False):
        super().__init__()
        self._print_to_console = print_to_console
        self.N, self.nx = s_traj.shape
        self._s_traj_raw = s_traj
        self._state_port_index = self.DeclareDiscreteState(self.nx)
        self.times = times
        self.hs = hs
        self.acceleration_init = a_init # 7x1

        # construct the piecewise trajectory here
        self._refTraj = PiecewisePolynomial.FirstOrderHold(self.times, self._s_traj_raw.T)

        # period_sec: The period (in seconds) at which the event should recur. 
        # offset_sec: The time offset (in seconds) before the first occurrence of the event. 
        # update: callback function
        self.DeclarePeriodicUnrestrictedUpdateEvent(period_sec=query_rate, offset_sec=t_offset, update=self.Update)

        # declare output port 
        self.DeclareStateOutputPort("ref joint state", self._state_port_index)
        self.DeclareVectorOutputPort("ref joint acceleration", int(self.nx/2), self.CalcReferenceAcceleration)
        # self.DeclareVectorOutputPort("joint states", self.nx, self.CalcOutput) # this is to use with a callback function

        # declare publishing port
        self.DeclareForcedPublishEvent(self.PrintToConsole)
    
    def Update(self, context, discrete_state):
        t = context.get_time()
        traj_i = self._refTraj.value(t)
        # discrete_state.get_mutable_discrete_state().SetAtIndex(0, traj_i)
        discrete_state.get_mutable_discrete_state().set_value(traj_i)
        
        if self._print_to_console:
            self.PrintToConsole(context)

问题原因分析 #

你设置初始状态的逻辑没问题，但问题出在周期更新事件的触发时机：

• 使用DeclarePeriodicUnrestrictedUpdateEvent时指定了offset_sec=t_offset，如果t_offset不为0，第一个更新事件要等到t_offset时刻才会触发，而t0时刻（仿真启动时）离散状态还没被更新，保持默认的零值。
• 另外，DeclareStateOutputPort直接输出离散状态，所以仿真初始时刻输出的是未更新的零值，直到第一个更新事件触发后才会切换到参考轨迹值。

解决方法 #

1. 调整更新事件的偏移量
   将offset_sec设为0，让第一个更新事件在t=0时刻就触发：

   self.DeclarePeriodicUnrestrictedUpdateEvent(period_sec=query_rate, offset_sec=0.0, update=self.Update)
   

   这样仿真启动时就会立即更新离散状态为参考轨迹的t=0值，避免初始零值误差。

2. 在系统内部初始化离散状态
   在LeafSystem的__init__中直接给离散状态设置初始值，不需要在外部手动修改上下文：

   def __init__(self, ...):
       super().__init__()
       # ... 其他初始化代码 ...
       self._state_port_index = self.DeclareDiscreteState(self.nx)
       # 直接设置初始离散状态为参考轨迹首个状态
       self.SetDefaultDiscreteState(self._s_traj_raw[0, :])
   

   这种方式更简洁，也能确保初始状态正确。

3. 改用实时查询轨迹的输出端口
   如果不需要把轨迹状态存在离散状态里，可以直接通过输出端口实时查询轨迹值，避免离散状态更新延迟：

   self.DeclareVectorOutputPort("ref joint state", self.nx, self.CalcReferenceState)
   
   def CalcReferenceState(self, context, output):
       t = context.get_time()
       traj_i = self._refTraj.value(t)
       output.set_value(traj_i)
   

   这种方式输出的是实时轨迹值，没有初始状态延迟问题，适合简单的轨迹跟踪场景。

推荐的参考轨迹跟踪实现方式 #

• 离散更新场景：确保周期更新事件的偏移量为0，同时在系统初始化时设置正确的初始离散状态，避免初始误差。
• 高精度实时跟踪：优先选择直接通过输出端口查询轨迹的方式，减少离散状态带来的延迟和同步问题。
• 轨迹平滑处理：如果参考轨迹是分段的，建议使用更高阶的插值（比如PiecewisePolynomial.Cubic），避免速度/加速度突变导致的力矩冲击。

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