离散LQR与iLQR的推导思路

一、最优控制

最优控制定义

首先介绍一下LQR的背景，也就是最优控制，最优控制的背景在之前介绍HJB方程的时候也有介绍：HJB方程的一些简单理解和过程推导。假设现在有一辆自动驾驶汽车，我们要设置控制方法，让其沿着指定路线到达某一个位置。要完成这项工作，首要的任务就是了解这辆车当前的“状态”和关于“控制”的变化规律：比如我们一般最关心当前的位置和车速，通过当前位置和速度去进行控制，比如打方向盘，踩刹车，踩油门等，从而又会引起当前位置和车速的变化。与此同时，我们可能还希望省油，车速平稳，不超速等要求。要满足以上的需求，需要合理的设计控制方法，以上问题我们就可以把它写成一个最优控制问题。
最优控制问题的四个基本元素：

• 状态方程：描述系统（自动驾驶汽车）的运动学或动力学方程

  $$\dot{x}(t)=f(x(t),u(t),t),t\in [t_0,t_f].x(t_0)=x_0$$

• 容许控制：控制与状态满足的约束

  $$u\in U,x\in X.$$

• 目标集：在结束时间$t_f$，被控对象的状态$x(t_f)$应符合的条件

S = { x ( t f ) : m ( x ( t f ) , t f ) = 0 } S= \left\{ x(t_f):m(x(t_f),t_f)=0 \right\} S={x(tf​):m(x(tf​),tf​)=0}

• 性能指标：$h$为终端成本，$g$为运行成本
  $$J(u)=h(x(t_f),t_f)+{\int }_{t_0}^{t_f}g(x(t),u(t),t)dt.$$

最优控制可以理解为求解满足状态方程、容许控制，且能达到目标点的情况下使性能指标最优的控制。

数学基础

这里传统的最优控制方法主要有两个：一个是极小值原理，一个是动态规划。这两种方法的产生可谓是神仙打架。在冷战的最初十年中，美苏双方的空军部门都遇到了同一个问题——如何以最短的时间拦截敌方的飞机。双方各自组件了研究团队，寻找切合实际的通用数学工具。
美国自1948年开始进行研究，苏联1955年才开始正是组建团队。尽管苏联方面着手较晚，但是在1956-1957年间苏联团队提出适用范围极广的Pontryagin极小值原理而暂时胜出，美方在1952-1957年提出的动态规划方法则在20世纪60年代被发现可用于最优控制问题，在特定的情况下还可以得到和苏联相同的结果。

里程碑工作

在这场较量中，诞生出了许许多多的重要成果：

• Hestenes问题：寻求最优飞机攻角和坡度。首次使用了Hamiltonian函数
• Isaacs的问题：微分博弈
• Bellman的问题：Bang-Bang控制
• Bellman的多级决策问题与离散时间最优控制
• 开环控制与闭环控制
• Pontryagin极小值原理
• 连续时间最优控制问题与两点边值问题
• 二次性能指标与卡尔曼调节器问题

二、LQR问题

LQR定义

LQR问题其实是一种最简单的最优控制问题。L代表状态方程或者动力学方程是线性的。Q代表目标函数是二次的。具体形式如下：

对于一个路径规划问题，当我们拥有了状态方程，通过任意给出控制，就可以产生无数条路径，而这些路径的优劣需要根据目标函数去抉择。所以我们希望找到一个控制率，这个控制率起一个向导的作用，当一个机器人在某一个位置的时候，控制率会告诉你在现在所处的位置，根据目标函数的判断标准最优控制应该是什么，这种思想的一个简单的数学表达是：
$$u_k=F_k{x}_k+v_k$$
这样可以理解为：状态方程+控制率=最优路径。所以在状态方程已知的情况下，我们的目标就是求解一个控制率，也就是控制对于位置的函数。
在搜索LQR问题的求解推导时，网上的方法不是很统一，这是因为一部分人使用的极小值原理，一部分人使用的是动态规划，下面分别对这两种方法进行简要说明。

极小值原理方法求解

在离散情况下的，极小值原理其实可以不必非要写成哈密尔顿方程的形式，更好理解的方式就是使用拉格朗日法，求解KKT条件。

我们将动力学约束放入目标函数中，分别对变量和拉格朗日乘子进行求导，得出以下的四个方程，其中最后一个是终值条件。

这里我们还需要根据这个终值条件做一个假设，在任意时刻的${\lambda }_k$都具有类似终值条件那样的形式，这里绿色是表示已知量，红色代表未知量。如果我们将红色的$P$求解出来，那么将其带入到第二个式子中，不就把控制率求解出来了吗。我们将求解$P$的这个方程成为：$Riccati$方程。一般来说迭代几步就会收敛。
通过这几个式子的消除变量，可以将$P_k$的迭代式求解出来，而我们又已知了最后一个时刻的$P$,这样就可以迭代的求出任意时刻的$P$值，顺理成章的得出控制率：

具体的推导过程可以详见：https://zhuanlan.zhihu.com/p/465054562

动态规划方法求解

使用动态规划之前需要了解什么是最优性原理，可以参考：HJB方程的一些简单理解和过程推导

blog.csdnimg.cn/8bc06d608206439fa084555e331a12a5.png#pic_center)
我们的推导思路其实很简单，举个例子，已知从上海到北京的最短路径是上海-南京-北京，那么根据最优性原理，从南京到北京的最短路径是南京-北京，那么我们可以先求出南京-北京这一段的控制率，然后这段有了之后，再去求上海-南京-北京的最后路径，即从后往前求。
我们这里重新定义一下目标函数，因为要一个阶段一个阶段的求解，所以我们不妨设相邻两个状态的目标函数为:

动力学方程为：

同样我们希望根据上面两个式子找到一个控制率：

我们希望先求出南京-北京这段路的最优控制率，那么这一段的评判标准就是$c(x_T,u_T)$,这里写成了他的展开形式：

希望对于控制最优，所以直接对控制进行求导等于零，得出：

这样我们就轻而易举的求出了这一阶段的控制率，尝到了甜头，那么我们希望继续用这种方法求上海-南京-北京的最优控制率，也即找到$Q(x_{T-1},u_{T-1})$，然后求导等于零，得出最优控制率。很自然的想法就是$Q(x_{T-1},u_{T-1})$的构建是：

但是这里我们需要将$x_T$和$u_T$用$x_{T-1}$和$u_{T-1}$替换掉。如何替换呢？
其实就是简单的变量消除。首先我们可以使用得到的最优控制，将$u_T$用$x_T$替换掉：

其中：

之后使用状态方程或者动力学防尘将：$x_T$替换成$x_{T-1}$和$u_{T-1}$：

其中：

这样我们就可以对其进行求导，如此向前迭代，求出最优控制率，整理成算法就呈现出了如下的反向迭代结构：

两种方法的比较

对于极小值原理，求解的一个开环问题。通过使用变分法，基于哈密尔顿方程组，一次性将最优曲线求解出来。
对于动态规划，他求解的是一个闭环问题。通过将问题变成一个多级决策问题，使用最优性原理，通过迭代的方式求出曲线。
两者虽然出发点不同，但是在求解问题的时候都有一个反馈-前馈结构。反馈主要体现在控制率的求解，前馈体现是使用控制率和状态方程计算轨迹。

三、iLQR框架

对于LQR问题，可以根据求解出来的控制率和状态方程，直接算出最优轨迹，如左边的示意图。
对于iLQR，其实就是目标函数和状态方程出现了非线性，这个时候控制率不是那么好求解。解决思路就是使用迭代的思想进行求解。首先给定一个初始的轨迹，如蓝色的直线，那么为了让其能够求解，可以将目标函数泰勒二次展开，状态方程泰勒一阶展开，之后使用LQR算法生成一条新的轨迹，如红色的虚线，之后反复迭代，直到轨迹的变化在一个规定的极小值内，就认为收敛，达到了最优。

具体的算法可以参考文献：Iterative Linear Quadratic Regulator Design for Nonlinear Biological Movement Systems。
推导思路是使用极小值原理。