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

Question

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

欲知更多详情：

1. 将我的系统指定为LeafSystem
2. 声明具有决策变量x，u的MathematicalProgram
3. 若要获得时变线性化动力学，请指定一个类，该类在单个时间步长上接受动力学和决策变量，并使用symbolic.Jacobian(args)返回该时间步长的雅可比值。
4. 加上包含整个轨迹的成本函数，所以所有的x，u

在成本职能内：

1. 使用接受决策变量并返回雅可比的类，获得每个时间步长的线性化矩阵A_i、B_i、G_i (G_i表示噪声)。
2. 用A_i矩阵和B_i矩阵解Riccati差分方程计算TVLQR成本(Sn)
3. 返回数学程序的成本，该程序本质上是K矩阵的大线性组合。

一方面，我不确定哪种最容易处理的方法来象征性地计算逆，但我最关心的是我的方法，以及这种象征性的描述是否合适。

Answer 1

我认为有几个关于DIRTREL的细节值得讨论：

1. 成本矩阵S[n]依赖于线性化的动力学Ai, Bi.我认为在DIRTREL中，你需要解决一个非线性优化问题，它需要成本的梯度。因此，要计算成本的梯度，需要S[n]的梯度，这需要Ai, Bi的梯度。因为Ai和Bi是动态函数f(x, u)的梯度，所以需要计算动力学的二阶梯度。
2. 我们有一个纸来进行轨迹优化和优化与LQR成本相关的成本函数。DIRTREL对我们的论文做了一些改进。在我们的实现中，我们还将S作为一个决策变量，因此我们的决策变量是x, u, S，其中约束既包括动力学约束x[n+1] = f(x[n], u[n])，又包括S上的Riccati方程。我认为DIRTREL的方法以较少的决策变量进行了更好的扩展，但我还没有比较这两种方法的数值性能。
3. 我不知道你为什么要象征性地计算逆。首先，你需要计算的逆是什么？第二，Drake支持利用自动微分计算数值梯度。我建议做数值计算而不是符号计算。由于在数值优化中，您只需要代价/约束的值和梯度，通常计算这些值的效率要高得多，而不是首先导出符号表达式，然后计算符号表达式。