时间戳

首页 > 解决方案 > Drake 中输入和输出端口之间的仿射关系

drake - Drake 中输入和输出端口之间的仿射关系

问题描述

假设我有一个Diagram带输入端口和输出端口的 Drake。我碰巧知道，在保持图表状态不变的情况下，这些输入和输出端口具有仿射关系，即 y = A(x) u + b(x) 其中 x 是 的完整状态Diagram。然而，图表不是明确的，AffineSystem因为它对状态的依赖是非线性的。

为图表的给定状态生成相应AffineSystem的或提取矩阵 A 和 b 的最佳方法是什么？目前，我正在使用

# set state
context.get_mutable_continuous_state_vector().SetFromVector(x)

# inputs to zero to get affine term
diagram.get_input_port().FixValue(context, np.zeros(k))
b = diagram.get_output_port().Eval(context)

# get linear term coefficient matrix
linearized = Linearize(
       diagram,
       context,
       input_index,
       output_index,
       equilibrium_check_tolerance=1e20
    )
A = linearized.D()

公差基本上只是迫使德雷克线性化系统，1e20即使它不处于平衡状态。但我想知道是否有更原则的方法来做到这一点。

我想这样做的原因类似于这个用户关于欠驱动系统的输入分配的问题。对于由 a 、 和 a 组成的图表MultibodyPlant，Propeller它将Multiplexer输入端口多路复用到Propeller命令和JointActuator命令，我想计算输入和广义加速度之间的仿射关系，因此我可以在 QP 中使用这些数据进行输入分配。

在此先感谢您的帮助！

标签： drake

解决方案

这可能不是一个好方法，但您可以考虑通过以下过程获得 A(x) 和 b(x)

  1. 首先修复u为 0，然后 xdot 就是 b(x)，正如您已经完成的那样。
  2. 现在固定u为 [1, 0, 0, ..., 0]，则 xdot 为 b(x) + A(x).col(0)。从这个结果中减去从第 1 步获得的 b(x)，剩下的就是 A(x).col(0)。
  3. 通过设置单位向量 eᵢ 重复步骤 2 u，使得 eᵢ(j) = δ(i, j)。然后你得到 A(x) 的第 i 列。

推荐阅读