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

Question

我正在努力实施名为 DIRTREL 的轨迹优化算法，它本质上是直接转录并增加了成本函数。但是，成本函数包含通过围绕决策变量 (x, u) 线性化系统并采用离散时变 LQR 获得的 K 矩阵。我的问题是如何在 drake 中最有效和简洁地表达这一点，因为我目前的方法象征性地描述了系统并由于 Riccati 差分方程的递归性质而导致极其冗长的符号方程（其长度只会随着更多的时间步长而增加），以及这种象征性的方法是否合适。

更多详情：

1. 将我的系统指定为 LeafSystem
2. 声明一个带有决策变量 x, u 的 MathematicalProgram
3. 要获得随时间变化的线性化动态，请指定一个 class 在单个时间步接受动态和决策变量，并为该时间步指定 returns 雅可比行列 symbolic.Jacobian(参数）
4. 添加包含整个轨迹的成本函数，所以所有x，u

成本函数内部：

1. 通过使用接受决策变量的class为每个时间步获得线性化矩阵A_i、B_i、G_i（G_i用于噪声）和 returns 雅可比
2. 使用 A_i 和 B_i 矩阵的 Riccati 差分方程计算 TVLQR 成本 (S[n]) 并求解 Ks
3. return 本质上是 K 矩阵的大型线性组合的数学程序的成本

附带说明一下，我不确定用符号计算逆的最容易处理的方法，但我最关心的是我的方法以及这种符号描述是否合适。

Answer 1

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

1. 成本矩阵 S[n] 取决于线性化动态 Ai, Bi。我认为在 DIRTREL 中你需要解决非线性优化问题，这需要成本的梯度。因此，要计算成本的梯度，您将需要 S[n] 的梯度，这需要 Ai, Bi 的梯度。由于 Ai 和 Bi 是动力学函数 f(x, u) 的梯度，您将需要计算动力学的二阶梯度。
2. 我们 paper 进行了轨迹优化和优化与 LQR 成本相关的成本函数。 DIRTREL 对我们的论文进行了多项改进。在我们的实现中，我们也将 S 视为决策变量，因此我们的决策变量为 x, u, S，约束包括动力学约束 x[n+1] = f(x[n], u[n]) 和 Riccati 方程作为对 S。我认为 DIRTREL 的方法在决策变量较少的情况下可以更好地扩展，但我没有比较这两种方法之间的数值性能。
3. 我不确定你为什么需要用符号计算逆。首先你需要计算的逆是什么？其次，Drake 支持使用自动微分来计算数值中的梯度。我建议进行数值计算而不是符号计算。由于在数值优化中，你只需要 cost/constraints 的值和梯度，通常用数值计算这些值比先推导符号表达式，然后计算符号表达式更有效。