ilqr原理及公式推导(更新ing)

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

注：以后内容包含许多个人理解，如有错误，疏漏欢迎讨论指正

前言

LQR（线性二次调节器）和 iLQR（迭代线性二次调节器）都是用于控制系统优化的算法，但它们在适用范围和具体实现上有显著区别。

LQR（Linear Quadratic Regulator）

1. 线性系统：LQR假设系统的动力学是线性的，描述如下:

$$x_{t+1}=A{x}_t+B{u}_t$$
其中 $x$是状态向量，$u$ 是控制输入， $A$ 和 $B$ 是系统矩阵。

2. 二次性能指标:
   $$J={\int }_0^{\infty }\left(x^{T}Qx+u^{T}Ru\right)dt$$
   其中 $Q$ 和 $R$ 是权重矩阵，用于平衡状态偏离和控制输入的权重。

iLQR（Iterative Linear Quadratic Regulator）

1. 非线性系统：iLQR适用于非线性系统，描述如下：
   $$x_{t+1}=f(x_t,u_t)$$
   其中 $f$ 是系统的非线性函数。

总结

• LQR 适用于线性系统，并且可以直接求解获得解析解。
• iLQR 适用于非线性系统，通过迭代的方式不断逼近最优解，每次迭代中需要线性化系统并求解LQR子问题。
• 也就是把非线性的问题通过数学方法变成线性问题在求解，因此后面先介绍如何求解LQR问题。

一、lqr问题求解

1. 问题定义

$$\underset{u_1,\dots ,u_T,x_1,\dots ,x_T}{\min }\sum _{t=1}^{T}c(x_t,u_t)\text{ s.t }{x}_t=f(x_{t-1},u_{t-1})\text{\hspace{1em}(1)}$$
其中$c(x_t,u_t)$为代价函数，$f$为系统函数，通过系统函数公式(1)可以写成下式：
$$\underset{u_1,\dots ,u_T}{\min }c(x_1,u_1)+c(f(x_1,u_1),u_2)+\cdots +c(f(f(\dots )\dots ),u_T)\text{\hspace{1em}(2)}$$
其中lqr是线性二次调节器，其系统方程为线性，其代价函数为二次型，因此：
$$f({\mathbf{x}}_t,{\mathbf{u}}_t)={\mathbf{F}}_t\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]+{\mathbf{f}}_t\text{\hspace{1em}(3)}$$

$$c({\mathbf{x}}_t,{\mathbf{u}}_t)=\frac{1}{2}{\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]}^T{\mathbf{C}}_t\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]+{\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]}^T{\mathbf{c}}_t\text{\hspace{1em}(4)}$$
为了便于后续公式推导，先声明一些概念

1. $Q(x_t,u_t)$是在状态$x_t$和动作$u_t$下的代价加上之后所有时间步长的最优代价之和。假设未来的策略都是最优的。$Q(x_t,u_t)=\ell (x_t,u_t)+V(x_{t+1})$,其中$\ell (x_t,u_t)$为当前阶段的代价
2. $V(x_t)$是在状态$x_t$下最优策略的代价函数，即所有可能动作中最小的$Q(x_t,u_t)
3. 同时

$$V(x_t)=minQ(x_t,u_t)$$

4. 公式推导

从$u_T$开始向后迭代，此时$Q(x_t,u_t)$为：
$$Q({\mathbf{x}}_T,{\mathbf{u}}_T)=\frac{1}{2}{\left[\begin{array}{c}{\mathbf{x}}_T\\ {\mathbf{u}}_T\end{array}\right]}^T{\mathbf{C}}_T\left[\begin{array}{c}{\mathbf{x}}_T\\ {\mathbf{u}}_T\end{array}\right]+{\left[\begin{array}{c}{\mathbf{x}}_T\\ {\mathbf{u}}_T\end{array}\right]}^T{\mathbf{c}}_T\text{\hspace{1em}(5)}$$
此时的代价矩阵${\mathbf{C}}_T=\left[\begin{array}{cc}{\mathbf{C}}_{{\mathbf{x}}_T,{\mathbf{x}}_T}& {\mathbf{C}}_{{\mathbf{x}}_T,{\mathbf{u}}_T}\\ {\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{x}}_T}& {\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{u}}_T}\end{array}\right]$, ${\mathbf{c}}_T=\lfloor \begin{array}{c}{\mathbf{c}}_{{\mathbf{x}}_T}\\ {\mathbf{c}}_{{\mathbf{u}}_T}\end{array}\rfloor$

为了寻找最优控制$u_T，Q(x_T,u_T)$对$u_T$求导并令其等于$0$可得到：
$${\nabla }_{{\mathbf{u}}_T}Q({\mathbf{x}}_T,{\mathbf{u}}_T)={\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{x}}_T}{\mathbf{x}}_T+{\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{u}}_T}{\mathbf{u}}_T+{\mathbf{c}}_{{\mathbf{u}}_T}^T=0\text{\hspace{1em}(6)}$$
此时可以解出${\mathbf{u}}_T=-{\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{u}}_T}^{-1}\left({\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{x}}_T}{\mathbf{x}}_T+{\mathbf{c}}_{{\mathbf{u}}_T}\right)$，此时令${\mathbf{K}}_T=-{\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{u}}_T}^{-1}{\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{x}}_T}$,${\mathbf{k}}_T=-{\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{u}}_T}^{-1}{\mathbf{c}}_{{\mathbf{u}}_T}$,则最优控制$u_T$可以写成:
$${\mathbf{u}}_T={\mathbf{K}}_T{\mathbf{x}}_T+{\mathbf{k}}_T\text{\hspace{1em}(7)}$$
前面定义中:$V(x_t)=minQ(x_t,u_t)$,因此用最优$u_T$替换$Q(x_t,u_t)$中的$u_t$，此时我们可以得到$V_T$为:
$$V\left({\mathbf{x}}_T\right)=\text{ const }+\frac{1}{2}{\left[\begin{array}{c}{\mathbf{x}}_T\\ {\mathbf{K}}_T{\mathbf{x}}_T+{\mathbf{k}}_T\end{array}\right]}^T{\mathbf{C}}_T\left[\begin{array}{c}{\mathbf{x}}_T\\ {\mathbf{K}}_T{\mathbf{x}}_T+{\mathbf{k}}_T\end{array}\right]+{\left[\begin{array}{c}{\mathbf{x}}_T\\ {\mathbf{K}}_T{\mathbf{x}}_T+{\mathbf{k}}_T\end{array}\right]}^T{\mathbf{c}}_T\text{\hspace{1em}(8)}$$
将公式8展开可以得到：
$$\begin{gathered} V({\mathbf{x}}_T)=\frac{1}{2}{\mathbf{x}}_T^T{\mathbf{C}}_{{\mathbf{x}}_T,{\mathbf{x}}_T}{\mathbf{x}}_T+\frac{1}{2}{\mathbf{x}}_T^T{\mathbf{C}}_{{\mathbf{x}}_T,{\mathbf{u}}_T}{\mathbf{K}}_T{\mathbf{x}}_T+\frac{1}{2}{\mathbf{x}}_T^T{\mathbf{K}}_T^T{\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{x}}_T}{\mathbf{x}}_T+\frac{1}{2}{\mathbf{x}}_T^T{\mathbf{K}}_T^T{\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{u}}_T}{\mathbf{K}}_T{\mathbf{x}}_T+ \\ {\mathbf{x}}_T^T{\mathbf{K}}_T^T{\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{u}}_T}{\mathbf{k}}_T+\frac{1}{2}{\mathbf{x}}_T^T{\mathbf{C}}_{{\mathbf{x}}_T,{\mathbf{u}}_T}{\mathbf{k}}_T+{\mathbf{x}}_T^T{\mathbf{c}}_{{\mathbf{x}}_T}+{\mathbf{x}}_T^T{\mathbf{K}}_T^T{\mathbf{c}}_{{\mathbf{u}}_T}+\mathrm{const}\text{\hspace{1em}(9)} \end{gathered}$$
令${\mathbf{V}}_T={\mathbf{C}}_{{\mathbf{x}}_T,{\mathbf{x}}_T}+{\mathbf{C}}_{{\mathbf{x}}_T,{\mathbf{u}}_T}{\mathbf{K}}_T+{\mathbf{K}}_T^T{\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{x}}_T}+{\mathbf{K}}_T^T{\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{u}}_T}{\mathbf{K}}_T$,同时${\mathbf{v}}_T={\mathbf{c}}_{{\mathbf{x}}_T}+{\mathbf{C}}_{{\mathbf{x}}_T,{\mathbf{u}}_T}{\mathbf{k}}_T+{\mathbf{K}}_T^T{\mathbf{C}}_{{\mathbf{u}}_T}+{\mathbf{K}}_T^T{\mathbf{C}}_{{\mathbf{u}}_T,{\mathbf{u}}_T}{\mathbf{k}}_T$，因此公式9可以简化为:
$$V({\mathbf{x}}_T)=\mathrm{const}+\frac{1}{2}{\mathbf{x}}_T^T{\mathbf{V}}_T{\mathbf{x}}_T+{\mathbf{x}}_T^T{\mathbf{v}}_T\text{\hspace{1em}(10)}$$
继续向前迭代到$T-1$步，此时：
$$Q({\mathbf{x}}_{T-1},{\mathbf{u}}_{T-1})=\text{const}+\frac{1}{2}{\left[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}\right]}^T{\mathbf{C}}_{T-1}\left[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}\right]+{\left[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}\right]}^T{\mathbf{c}}_{T-1}+V(f({\mathbf{x}}_{T-1},{\mathbf{u}}_{T-1}))\text{\hspace{1em}(11)}$$
其中第二部分为当前状态代价，第三部分为最佳cost-to-go，即当前到目标状态$x_T$的累计成本，同时根据系统函数$x_T=f(x_{T-1},u_{T-1})$,因此第三部分也就是公式10.
根据前文描述我们可以知道系统函数为：$f({\mathbf{x}}_{T-1},{\mathbf{u}}_{T-1})={\mathbf{x}}_T={\mathbf{F}}_{T-1}[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}]+{\mathbf{f}}_{T-1}$,因此$V(x_T)$可以表示为：
$$\begin{gathered} {V({\mathbf{x}}_T)=\mathrm{const}+\frac{1}{2}[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}]}^T{\mathbf{F}}_{T-1}^T{\mathbf{V}}_T{\mathbf{F}}_{T-1}\left[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}\right]+{\left[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}\right]}^T{\mathbf{F}}_{T-1}^T{\mathbf{V}}_T{\mathbf{f}}_{T-1}+ \\ {\left[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}\right]}^T{\mathbf{F}}_{T-1}^T{\mathbf{v}}_T\text{\hspace{1em}(12)} \end{gathered}$$
因此公式(11)可以写成：
$$Q({\mathbf{x}}_{T-1},{\mathbf{u}}_{T-1})=\mathrm{const}+\frac{1}{2}{\left[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}\right]}^T{\mathbf{Q}}_{T-1}\left[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}\right]+{\left[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}\right]}^T{\mathbf{q}}_{T-1}$$
其中${\mathbf{Q}}_{T-1}={\mathbf{C}}_{T-1}+{\mathbf{F}}_{T-1}^T{\mathbf{V}}_T{\mathbf{F}}_{T-1}$,同时${\mathbf{q}}_{T-1}={\mathbf{c}}_{T-1}+{\mathbf{F}}_{T-1}^T{\mathbf{V}}_T{\mathbf{f}}_{T-1}+{\mathbf{F}}_{T-1}^T{\mathbf{v}}_T$
此时我们已经将$Q(x_{T-1}，u_{T-1})$用$x_{T-1}，u_{T-1}$表示出来，此时$Q(x_{T-1}，u_{T-1})$对$u_{T-1}$求导并令其为0则:
$${\nabla }_{{\mathbf{u}}_{T-1}}Q({\mathbf{x}}_{T-1},{\mathbf{u}}_{T-1})={\mathbf{Q}}_{{\mathbf{u}}_{T-1},{\mathbf{x}}_{T-1}}{\mathbf{x}}_{T-1}+{\mathbf{Q}}_{{\mathbf{u}}_{T-1},{\mathbf{u}}_{T-1}}{\mathbf{u}}_{T-1}+{\mathbf{q}}_{{\mathbf{u}}_{T-1}}^T=0\text{\hspace{1em}(13)}$$
此时可以结出最优${\mathbf{u}}_{T-1}={\mathbf{K}}_{T-1}{\mathbf{x}}_{T-1}+{\mathbf{k}}_{T-1}$,其中${\mathbf{K}}_{T-1}=-{\mathbf{Q}}_{{\mathbf{u}}_{T-1},{\mathbf{u}}_{T-1}}^{-1}{\mathbf{Q}}_{{\mathbf{u}}_{T-1},{\mathbf{x}}_{T-1}}$,同时${\mathbf{k}}_{T-1}=-{\mathbf{Q}}_{{\mathbf{u}}_{T-1},{\mathbf{u}}_{T-1}}^{-1}{\mathbf{q}}_{{\mathbf{u}}_{T-1}}$

不断向前迭代直到$t=1$
$$\begin{array}{rl}\mathrm{for}& t=T\text{ to 1:}\\ & {\mathbf{Q}}_t={\mathbf{C}}_t+{\mathbf{F}}_t^T{\mathbf{V}}_{t+1}{\mathbf{F}}_t\\ & {\mathbf{q}}_t={\mathbf{c}}_t+{\mathbf{F}}_t^T{\mathbf{V}}_{t+1}{\mathbf{f}}_t+{\mathbf{F}}_t^T{\mathbf{v}}_{t+1}\\ & Q({\mathbf{x}}_t,{\mathbf{u}}_t)=\mathrm{const}+\frac{1}{2}{\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]}^T{\mathbf{Q}}_t\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]+{\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]}^T{\mathbf{q}}_t\\ & {\mathbf{u}}_t\leftarrow \arg \underset{{\mathbf{u}}_t}{\min }Q({\mathbf{x}}_t,{\mathbf{u}}_t)={\mathbf{K}}_t{\mathbf{x}}_t+{\mathbf{k}}_t\\ & {\mathbf{K}}_t=-{\mathbf{Q}}_{{\mathbf{u}}_t,{\mathbf{u}}_t}^{-1}{\mathbf{Q}}_{{\mathbf{u}}_t,{\mathbf{x}}_t}\\ & {\mathbf{k}}_t=-{\mathbf{Q}}_{{\mathbf{u}}_t,{\mathbf{u}}_t}^{-1}\mathbf{q}{\mathbf{u}}_t\\ & {\mathbf{V}}_t={\mathbf{Q}}_{{\mathbf{x}}_t,{\mathbf{x}}_t}+{\mathbf{Q}}_{{\mathbf{x}}_t,{\mathbf{u}}_t}{\mathbf{K}}_t+{\mathbf{K}}_t^T{\mathbf{Q}}_{{\mathbf{u}}_t,{\mathbf{x}}_t}+{\mathbf{K}}_t^T{\mathbf{Q}}_{{\mathbf{u}}_t,{\mathbf{u}}_t}{\mathbf{K}}_t\\ & {\mathbf{v}}_t={\mathbf{q}}_{{\mathbf{x}}_t}+{\mathbf{Q}}_{{\mathbf{x}}_t,{\mathbf{u}}_t}{\mathbf{k}}_t+{\mathbf{K}}_t^T{\mathbf{Q}}_{{\mathbf{u}}_t}+{\mathbf{K}}_t^T{\mathbf{Q}}_{{\mathbf{u}}_t,{\mathbf{u}}_t}\mathbf{k}\\ & V({\mathbf{x}}_t)=\mathrm{const}+\frac{1}{2}{\mathbf{x}}_t^T{\mathbf{V}}_t{\mathbf{x}}_t+{\mathbf{x}}_t^T{\mathbf{v}}_t\end{array}\text{\hspace{1em}(14)}$$
此时我们可以求出每一个阶段最优的$u_t$,同时我们知道初值$x_0$,因此我们又可以正向迭代求出每一个状态x
$$\begin{array}{rl}\text{for }t& =1\text{ to }T:\\ {\mathbf{u}}_t& ={\mathbf{K}}_t{\mathbf{x}}_t+{\mathbf{k}}_t\\ {\mathbf{x}}_{t+1}& =f({\mathbf{x}}_t,{\mathbf{u}}_t)\end{array}\text{\hspace{1em}(15)}$$

5.个人理解

写到这里lqr的解法已经写完了，但是当我第一次看公式推到的时候并没有明白$Q_t$的具体形式,同时$Q_{u_t,u_t},Q_{x_t,x_t},Q_{x_t,u_t}$的具体形式是什么，经过我向我司新来的X博士多番询问，最终搞明白了。
首先我们做出如下定义：
$$q_x=\frac{\partial Q(x_t,u_t)}{\partial x_t}$$
$$q_u=\frac{\partial Q(x_t,u_t)}{\partial u_t}$$
$$q_{xx}=\frac{{\partial }^{2}Q(x_t,u_t)}{{\partial }^2{x}_t}$$
$$q_{uu}=\frac{{\partial }^{2}Q(x_t,u_t)}{{\partial }^2{u}_t}$$
$$q_{xu}=\frac{{\partial }^{2}Q(x_t,u_t)}{\partial x_t\partial u_t}$$
下面我仅以$q_{xu}$为例进行详细的推导：
$$\begin{array}{rl}Q(x_t,u_t)& =\ell (x_t,u_t)+V(x_{t+1})\\ & =\ell (x_t,u_t)+V(f(x_t,u_t))\end{array}$$
根据链式法则：
$$\begin{array}{rl}{q}_x& =\frac{\partial Q(x_t,u_t)}{\partial x_t}\\ & =\frac{\partial \ell (x_t,u_t)}{\partial x_t}+\frac{\partial V(f(x_t,u_t))}{\partial x_t}\\ & ={\ell }_x+\frac{\partial V}{\partial f}\cdot \frac{\partial f(x_t,u_t)}{\partial x_t}\end{array}$$
其中$\frac{\partial V}{\partial f}$为$V$对$f$的梯度，即为$v_x$,$\frac{\partial f(x_t,u_t)}{\partial x_t}$为$f$对$x_t$的雅可比矩阵即为$f_x$
因此:
$$q_x={\ell }_x+f_x^T{v}_x$$
然后
$$\begin{array}{rl}{q}_{xu}& =\frac{\partial }{\partial u_t}({\ell }_x+f_x^T{v}_x)\\ & ={\ell }_{xu}+{\mathbf{f}}_u^T{\mathbf{v}}_{xx}{\mathbf{f}}_x\end{array}$$
因此所有的结果为：
$$\begin{array}{rl}& 1.q_x={\ell }_x+{\mathbf{f}}_x^T{\mathbf{v}}_x\\ & 2.q_u={\ell }_u+{\mathbf{f}}_u^T{\mathbf{v}}_x\\ & 3.q_{xx}={\ell }_{xx}+{\mathbf{f}}_x^T{\mathbf{v}}_{xx}{\mathbf{f}}_x\\ & 4.q_{uu}={\ell }_{uu}+{\mathbf{f}}_u^T{\mathbf{v}}_{xx}{\mathbf{f}}_u+\mu \mathbf{I}\\ & 5.q_{xu}={\ell }_{xu}+{\mathbf{f}}_u^T{\mathbf{v}}_{xx}{\mathbf{f}}_x\end{array}$$
其中：
$$\begin{gathered} {\mathbf{v}}_x=q_x+K_t^T{q}_{uu}{k}_t+K_t^T{q}_u+q_{ux}^T{k}_t \\ {\mathbf{v}}_{xx}=q_{xx}+K_t^T{q}_{uu}{K}_t+K_t^T{q}_{ux}+q_{ux}^T{K}_t \end{gathered}$$

---

总结

还没写完，后面会有基于ros，ilqr路径规划代码实现，尽情期待！！！！！！！！！！！！！！！！！！！！！！