drake - 具有依赖于时变 LQR 返回的 K 矩阵的成本函数的非线性系统的直接转录

Question

我正在实现名为 DIRTREL 的轨迹优化算法，它本质上是带有附加成本函数的直接转录。然而，成本函数结合了通过围绕决策变量 (x, u) 对系统进行线性化并采用离散时变 LQR 获得的 K 个矩阵。我的问题是如何在德雷克中最有效和简洁地表达这一点，因为我目前的方法象征性地描述了系统，并由于 Riccati 差分方程的递归性质而导致极长的符号方程(只会随着更多的时间步长而增加长度)，如果这种象征性的方法甚至合适。
更多细节:

将我的系统指定为 LeafSystem

声明一个带有决策变量 x, u

的数学程序

要获得时变线性化动态，请指定一个类，该类在单个时间步接收动态和决策变量，并使用符号返回该时间步的雅可比行列式。Jacobian(args)

添加包含整个轨迹的成本函数，因此所有 x, u

成本函数内部:

通过使用接受决策变量并返回雅可比行列式的类，为每个时间步获取线性化矩阵 A_i、B_i、G_i(G_i 表示噪声)

计算 TVLQR 成本 (S[n]) 和 Riccati 差分方程，使用 A_i 和 B_i 矩阵并求解 Ks

返回数学程序的成本，它本质上是 K 矩阵的大型线性组合

一个旁注是我不确定以符号方式计算逆的最容易处理的方法，但我最关心的是我的方法以及这种符号描述是否合适。

Answer 1

我觉得DIRTREL有几个细节值得讨论:

成本矩阵 S[n]取决于线性化动力学 Ai, Bi .我认为在 DIRTREL 中，您需要解决非线性优化问题，这需要成本的梯度。因此，要计算成本的梯度，您将需要 S[n] 的梯度。 ，这需要梯度Ai, Bi .自 Ai和 Bi是动力学函数的梯度f(x, u) ，您将需要计算动力学的二阶梯度。

我们有一个 paper关于进行轨迹优化和优化与 LQR cost-to-go 相关的成本函数。 DIRTREL 对我们的论文做了一些改进。在我们的实现中，我们处理了 S也作为决策变量，所以我们的决策变量是x, u, S , 约束包括动力学约束 x[n+1] = f(x[n], u[n]) ，以及作为对 S 的约束的 Riccati 方程.我认为 DIRTREL 的方法在决策变量较少的情况下可以更好地扩展，但我没有比较两种方法之间的数值性能。

我不知道为什么你需要象征性地计算逆。首先你需要计算的逆是什么？其次，Drake 支持使用自动微分来计算数值的梯度。我建议进行数值计算而不是符号计算。由于在数值优化中，您只需要成本/约束的值和梯度，因此以数值方式计算这些值通常要高效得多，而不是先推导符号表达式，然后计算符号表达式。