写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
个人主页：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。

👍 点赞、评论、收藏，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货，还有思维的火花！

📚 系列专栏：【运动控制】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！

🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

---

---

引言

  本篇博客是 自动驾驶控制算法 系列的第五节。内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

  在上一节将坐标变换和车辆动力学模型综合起来，得到了关于误差的微分方程：

  可以写成 ${\dot{e}}_{rr}=A{e}_{rr}+Bu+C{\dot{\theta }}_r$。暂时先忽略 $C{\dot{\theta }}_r$，考虑 $C{\dot{\theta }}_r$ 的控制以后再讲，暂时不管它。上面的误差微分方程变成如下形式：
$${\dot{e}}_{rr}=A{e}_{rr}+Bu$$

---

一、连续LQR与离散LQR

1、LQR问题概述

  下面求 LQR 问题，就是在 $J$ 等于 $Q$ 和 $R$ 二次型里选择合适的 $u$，让 $J$ 最小，并且 $u$ 要满足 ${\dot{e}}_{rr}=A{e}_{rr}+Bu$ 的约束，这就是典型的 LQR 问题， LQR 问题已经十分成熟，在 Matlab 里有包，可以调用 $\mathrm{lqr}(A,B,Q,R)$，以及 $\mathrm{dlqr}(A,B,Q,R)$，就是离散的(discrete) LQR。

本篇博客讲解 dLQR 的基本原理，因为我们并不满足于当调包师、调参数侠，而是要知道原理。

2、离散LQR的优势

  LQR 分为连续 LQR 和离散 LQR ，但是就本博客只讲离散 LQR ，因为实际应用都是离散 LQR ，特别是在计算机上，计算机处理的是离散数据，而且控制大多也是离散，所以离散 LQR 对实际应用很有用。

  第二个原因是连续 LQR 的理论非常难，涉及到泛函和变分法，而且一般用不到。

3、离散LQR的数学基础：拉格朗日乘子法

  而离散 LQR 只涉及拉格朗日乘子法。

什么是拉格朗日乘子法？

  就是求 $f(x,y)$ 在约束 $g(x,y)=0$ 的条件下的机制，用拉格朗日乘子法，就是建立
$$L(x,y)=f(x,y)+\lambda g(x,y)$$其中，$\frac{\partial L}{\partial x}=0,\frac{\partial L}{\partial y}=0,\frac{\partial L}{\partial \lambda }=0$。

  在自动控制原理书中，使用离散泛函与变分法解离散 LQR，但这样涉及到了太多数学知识，而使用拉格朗日乘子法，可以绕过泛函与变分法的概念把离散 LQR 讲明白。

---

二、离散LQR问题描述

1、离散LQR问题描述

  离散 LQR 的问题描述：
$$\dot{X}=AX+Bu\stackrel{\mathrm{discrete}}{\to }{X}_{k+1}=\bar{A}{x}_k+\bar{B}{u}_k$$

2、代价函数和约束条件

  设代价函数为：
$$J=\sum _{k=0}^{+\infty }(x_k^{T}Q{x}_k+u_k^{T}R{u}_k)$$  在约束条件$x_{k+1}=\bar{A}{x}_k+\bar{B}{u}_k$下的极小值。

  这样和拉格朗日乘子法很像，它们都是某个函数，在另一个函数的约束下，求函数的极小值，而且极小值一定就是最小值，因为根据 $J$ 是二次型，只有极值点，而且是极小值点，自然极小值就是最小值。

3、离散LQR问题分解

现在离散 LQR 问题分解为两个问题：

• 如何实现$\dot{X}=AX+Bu\stackrel{\mathrm{discrete}}{\to }{X}_{k+1}=\bar{A}{X}_k+\bar{B}{u}_k$？

• 如何求解使得 $J$ 在约束条件 $X_{k+1}=\bar{A}{X}_k+\bar{B}{u}_k$下取极小值的 $u_k$？

---

三、离散化方法

1、连续微分方程的离散化

先讲离散化，有两种方法：

• 向前欧拉法
• 中点欧拉法

百度 Apollo 的代码是两者混合。

  下面简单讲一下向前欧拉法和中点欧拉法。

  针对 $\dot{x}=Ax$ 方程，两边积分下限为 $t$，积分上限为 $t+dt$，得到以下式子：
$${\int }_t^{t+dt}\dot{x}dt={\int }_t^{t+dt}Axdt$$$$x\left(t+dt\right)-x\left(t\right)=Ax\left(\xi \right)dt\xi \in \left(t,t+dt\right)$$  把后面积分化为 $\xi$，用的是积分中值定理，得到：
$$x\left(t+dt\right)=x\left(t\right)+Ax\left(\xi \right)dt\xi \in \left(t,t+dt\right)$$  这是精确的离散化，但是这样离散化没有用，因为不知道 $\xi$ 到底是什么，根据 $\xi$ 到底是什么有各种近似解法：

(1) 向前欧拉法

  认为 $X(\xi )=X(t)$：
$$X(t+dt)=(I+Adt)X(t)$$

(2) 向后欧拉法

  认为 $X(\xi )=X(t+dt)$：
$$X(t+dt)=(I-Adt{)}^{-1}X(t)$$

(3) 中点欧拉法

  认为$X(\xi )=\frac{X(t)+X(t+dt)}{2}$：
$$X(t+dt)=(I-\frac{Adt}{2}{)}^{-1}(I+\frac{Adt}{2})X(t)$$  这就是三种离散化方法。

  一般中点欧拉法的精度比向前欧拉法和向后欧拉法要高，这些欧拉法都是对精确解的 $X(\xi )$ 做不同程度的近似。

2、连续方程离散化的数学推导

  对于 $\dot{x}=Ax+Bu$ 方程，先两边积分，将积分划为中式定理形式，得到以下方程：
$$\dot{x}=Ax+Bu\Rightarrow {\int }_t^{t+dt}\dot{x}dt={\int }_t^{t+dt}\left(Ax+Bu\right)dt$$$$X\left(t+dt\right)-X\left(t\right)=Ax\left(\xi \right)dt+Bu\left(\xi \right)dt$$即：
$$X(t+dt)=AX(\xi )dt+X(t)+Bu(\xi )dt$$  对第一个 $X(\xi )$ 用中点欧拉法，对第二个 $u(\xi )$ 用向前欧拉法。

  第二个 $u(\xi )$ 不用终点欧拉法是因为 $u(t+dt)$ 未知，一般 $dt$ 取采样周期，比如 0.01 秒或 0.001 秒， $u(t)$ 一般是要求的当前控制量，这是 LQR 要算的，$u(t+dt)$ 是下一时刻的 LQR 控制量，当前的控制量 $u(t)$ 都不知道，下一时刻的 $u(t+dt)$ 更不知道，所以第二个 $u(\xi )$不能用中点欧拉法。

那为什么第一个 $X(\xi )$ 用中点欧拉法呢？

  因为 $X(t+dt)$，虽然不知道，但它不需要知道。因为只要把它带进去化简就出来了：
$$X(t+dt)=Adt(\frac{X(t+dt)+X(t)}{2})+X(t)+Bu(t)dt$$  把右边 $X(t+dt)$ 移到左边，得到
$$(I-\frac{Adt}{2})X(t+dt)=(I+\frac{Adt}{2})X(t)+Bu(t)dt$$  在左边求逆：

$$X(t+dt)=(I-\frac{Adt}{2}{)}^{-1}(I+\frac{Adt}{2})X(t)+(I-\frac{Adt}{2}{)}^{-1}Bu(t)dt$$  因为 $dt$ 通常比较小 $0.01、0.02$ 这样的数量级，后面矩阵求逆就直接省掉了，可以少算矩阵求逆的计算量。

  这样得到以下方程：
$$X(t+dt)=(I-\frac{Adt}{2}{)}^{-1}(I+\frac{Adt}{2})X(t)+Bu(t)dt$$  其中，$I$ 是单位矩阵， $dt$ 是采样周期，得到离散化的微分方程：
$$X(k+1)=\bar{A}X(k)+\bar{B}u(k)$$  其中，$\bar{A}=(I-\frac{Adt}{2}{)}^{-1}(I+\frac{Adt}{2})$，$\bar{B}=Bdt$。

  以上是连续方程离散化的全部过程。

---

四、离散LQR解法

  下面看一下离散的 LQR 的解法。

1、代价函数

  代价函数 $J$ 是关于 $x$ 和 $u$ 的二次型：
$$J=\sum _{k=0}^{+\infty }(x_k^{T}Q{x}_k+u_k^{T}R{u}_k)$$  希望能达到稳定的控制，即在任何时段， $k$ 等于 $0$ 到正无穷时间段所有误差的和它要最小。

2、约束条件

  约束条件就是上面所推导的离散微分方程$X_{k+1}=A{x}_k+B{u}_k$ ，$A$ 对应上面的 $\bar{A}$， $B$ 对应上面的 $\bar{B}$。

  $k$ 从 $0$ 到至无穷，这无穷就不太好处理，那就先不让 $k$ 从 $0$ 到正无穷这样加下去，先让 $k$ 从 $0$ 一直加到 $n$，再让 $n$ 取无穷。

  具体做法是：
$$J=\sum _{k=0}^{n-1}(x_k^{T}Q{x}_k+u_k^{T}R{u}_k)+x_n^{T}Q{x}_n$$  让 $n$ 趋无穷就可以了。

问题来了，为什么没有 $u_n^{T}Q{u}_n$ 这一项呢？

  先把约束写出来：
$$\begin{array}{c}A{x}_0+B{u}_0-x_1=0\\ A{x}_1+B{u}_1-x_2=0\\ ⋮\\ A{x}_{n-1}+B{u}_{n-1}-x_n=0\end{array}$$  约束覆盖了从 $x_0$ 到 $x_n$ 以及从 $u_0$ 到 $u_{n-1}$。发现写的代价函数 $J$ 正好是 从$x_0$ 到 $x_n$ 以及从 $u_0$ 到 $u_{n-1}$的这样函数，即约束完全覆盖二次型的自变量。

  所以 $u$ 天生就比 $x$ 少，最后写的二次型，必须也得少，才能把约束全部匹配上。

3、代价函数和约束的拉格朗日乘子法表示

  最后用拉格朗日乘子法写出 $J$ 来：
$$J=\sum _{k=0}^{n-1}\left(x_k^{T}Q{x}_k+u_k^{T}R{u}_k\right)+x_n^{T}Q{x}_n+\sum _{k=0}^{n-1}{\lambda }_{k+1}^T\left(A{x}_k+B{u}_k-x_{k+1}\right)$$  这就是带约束的拉格朗日乘子法，注意 $A{x}_k+B{u}_k-x_{k+1}$ 可能不是数，而是列向量，所以 ${\lambda }_{k+1}$ 要有转置，因为两个列向量不能相乘，把它变成行向量，行向量乘列向量才有意义。

  发现这两个求和符号它一样，都是从 $0$ 到 $n+1$ 相加，可以合并：
$$J=\sum _{k=0}^{n-1}\left[x_k^{T}Q{x}_k+u_k^{{}_T}R{u}_k+{\lambda }_{k+1}^T\left(A{x}_k+B{u}_k\right)-{\lambda }_{k+1}^T{x}_{k+1}\right]+x_n^{T}Q{x}_n$$  但是这么写太长了。

  把关于 $x_k$ 和 $u_k$ 的项，起个新的名字 $H_k$，设
$$H_k=x_k^{T}Q{x}_k+u_k^{{}_T}R{u}_k+{\lambda }_{k+1}^T\left(A{x}_k+B{u}_k\right)$$  这样可以写成非常简单的形式：
$$J=\sum _{k=0}^{n-1}(H_k-{\lambda }_{k+1}^T{x}_{k+1})+x_n^{T}Q{x}_n$$  把它展开变成：
$$J=H_0+H_1+\cdots +H_{n-1}+\left(-{\lambda }_1^T{x}_1-{\lambda }_2^T{x}_2-\cdots -{\lambda }_n^T{x}_n\right)+x_n^{T}Q{x}_n$$  最后再加上 ${\lambda }_0^T{x}_0$ 再减去负的 ${\lambda }_0^T{x}_0$，这样和原式相等，但加了负的 ${\lambda }_0^T{x}_0$ 就可以和前面东西合并：
$$\begin{array}{rl}J& =\sum _{k=0}^{n-1}{H}_k+\sum _{k=1}^{n-1}\left(-{\lambda }_k^T{x}_k\right)+\left(-{\lambda }_n^T{x}_n\right)+x_n^{T}Q{x}_n+\left(-{\lambda }_0^T{x}_0\right)-\left(-{\lambda }_0^T{x}_0\right)\\ & =\sum _{k=0}^{n-1}{H}_k+\sum _{k=0}^{n-1}\left(-{\lambda }_k^T{x}_k\right)-{\lambda }_n^T{x}_n+x_n^{T}Q{x}_n+{\lambda }_0^T{x}_0\\ & =\sum _{k=0}^{n-1}\left(H_k-{\lambda }_k^T{x}_k\right)+x_n^{T}Q{x}_n-{\lambda }_n^T{x}_n+{\lambda }_0^T{x}_0\end{array}$$
  写成这样就可以求导。

  注意：是向量求导，不是数的求导，下面复习一下向量求导。

---

五、向量导数

1、向量导数规则

  规则如下：
$$\frac{\partial (x^{T}A)}{\partial x}=A\frac{\partial (Ax)}{\partial x}=A^T\frac{\partial (x^{T}Ax)}{\partial x}=(A+A^T)x$$  若 $A$ 为对称矩阵，$\frac{\partial (x^{T}Ax)}{\partial x}=(A+A^T)x=2Ax$

  因为 $Q$ 和 $R$ 都是对称矩阵，所以对 $Q$ 和 $R$ 的二次型求导直接乘以 $2$ 即可。

2、求解偏导数的规律

  先写几个找规律：
$$\frac{\partial J}{\partial x_0}=0\frac{\partial {\lambda }_0^T{x}_0}{\partial x_0}=0\Rightarrow {\lambda }_0=0$$$$\frac{\partial J}{\partial x_1}=0\frac{\partial H_1}{\partial x_1}-\frac{\partial \left({\lambda }^T{x}_1\right)}{\partial x_1}=0\Rightarrow {\lambda }_1=\frac{\partial H_1}{\partial x_1}$$$$\frac{\partial J}{\partial x_2}=0\frac{\partial H_2}{\partial x_2}-\frac{\partial \left({\lambda }^T{x}_2\right)}{\partial x_2}=0\Rightarrow {\lambda }_2=\frac{\partial H_2}{\partial x_2}$$  发现偏导有相同的规律，可直接写成：
$$\begin{array}{rl}& \frac{\partial J}{\partial x_k}=0\Rightarrow {\lambda }_k=\frac{\partial H_k}{\partial x_k}(k=1,2,3,\dots ,n-1)\\ & \frac{\partial J}{\partial u_k}=0\Rightarrow \frac{\partial H_k}{\partial u_k}=0(k=1,2,3,\cdots ,n-1)\\ & \frac{\partial J}{\partial x_n}=0\Rightarrow \frac{\partial \left(x_n^{T}Q{x}_n-{\lambda }_n^T{x}_n\right)}{\partial x_n}=0\\ & \frac{\partial J}{\partial {\lambda }_k}=0\Rightarrow x_{k+1}=A{x}_k+B{u}_k\left(k=1,2,3,...,n-1\right)\end{array}$$  上式中的 $0$ 都是 $0$ 向量。

  下面计算 $\frac{\partial H_k}{\partial x_k}$ 以及 $\frac{\partial H_k}{\partial u_k}$。首先把 $H_k$ 的定义拿下来：
$$H_k=x_k^{T}Q{x}_k+u_k^{{}_T}R{u}_k+{\lambda }_{k+1}^T\left(A{x}_k+B{u}_k\right)$$  根据向量导数得到：
$$\frac{\partial H_k}{\partial x_k}=2Q{x}_k+A^T{\lambda }_{k+1}$$$$\frac{\partial H_k}{\partial u_k}=2R{u}_k+B^T{\lambda }_{k+1}$$  把式子带到上面四个等于 $0$ 的式子中，可推导出以下四个方程：

$$\{\begin{array}{rlrl}& {\lambda }_k=2Q{x}_k+A^T{\lambda }_{k+1}& (k=1,2,\cdots ,n-1)& (1)\\ & u_k=-\frac{1}{2}{R}^{-1}{B}^T{\lambda }_{k+1}& (k=1,2,\cdots ,n-1)& (2)\\ & x_{k+1}=A{x}_k+B{u}_k& (k=1,2,\cdots ,n-1)& (3)\\ & {\lambda }_n=2Q{X}_n& & (4)\end{array}$$  看起来好像是四个式子，但实际上是 $3n-2$ 个式子。

  看一下这 $3n-2$ 个式子，发现 $(2)$式明确给出了 $u_k=-\frac{1}{2}{R}^{-1}{B}^T{\lambda }_{k+1}$，其中 $R$ 和 $B$ 已知，剩下的 ${\lambda }_{k+1}$ 未知，如果算出 ${\lambda }_{k+1}$ 等于多少？就可以算出 $u_k$，这样 LQR 问题就解决了。

问题是 ${\lambda }_{k+1}$ 该怎么算？

  从 $(4)$式可以得到 ${\lambda }_n=2Q{X}_n$，和 $Q$ 和 $X_n$ 有关，而 $(1)$式和 $(3)$式都表示 ${\lambda }_{k+1}$和 ${\lambda }_k$ 以及$x_{k+1}$和 $x$ 之间的关系。

  如果知道 ${\lambda }_n$ 等于什么的话，通过 $(1)$式和 $(3)$式反向逆推，一直推到初始状态。

3、递推关系式的推导

  递推式如下：

  将$(4)$代入$(2)$的 $(k=n-1)$ 式：
$$\begin{array}{c}{u}_{n-1}=-\frac{1}{2}{R}^{-1}{B}^T(2Q{X}_n)=-R^{-1}{B}^{T}Q{X}_n\end{array}\text{\hspace{1em}(5)}$$  将$(5)$代入$(3)$的 $(k=n-1)$ 式：
$$\begin{array}{rl}{x}_n& =A{x}_{n-1}+B{u}_{n-1}\\ & =A{x}_{n-1}+\left(-B{R}^{-1}{B}^{T}Q{x}_n\right)\\ & ={\left(I+B{R}^{-1}{B}^{T}Q\right)}^{-1}A{x}_{n-1}\end{array}\text{\hspace{1em}(6)}$$  将$(4)$代入$(1)$的 $(k=n-1)$ 式：
$$\begin{array}{rl}{\lambda }_{n-1}& =2Q{X}_{n-1}+A^T{\lambda }_n\\ & =2Q{X}_{n-1}+A^{T}2Q{X}_n\end{array}\text{\hspace{1em}(7)}$$  将$(6)$代入$(7)$的 $(k=n-1)$ 式：
$$\begin{gathered} {\lambda }_{n-1}=2Q{X}_{n-1}+2A^{T}Q(I+B{R}^{-1}{B}^{T}Q{)}^{-1}A{X}_{n-1} \\ =2(Q+A^{T}Q(I+B{R}^{-1}{B}^{T}Q{)}^{-1})A{X}_{n-1}\text{\hspace{1em}(8)} \end{gathered}$$  将$(4)$与$(8)$式比较，发现很像。

4、黎卡提方程的引入

  递推性质所决定的，从${\lambda }_n$ 推到${\lambda }_{n-1}$，${\lambda }_{n-2}$，一定有相同形式，不妨设：
$${\lambda }_k=2P_k{x}_k(k=1,2,3,\cdots ,n)$$  这样把求${\lambda }_k$ 的问题转化为求 $P_k$ 的问题。

  因为根据$(2)$式，控制量 $u_k$ 直接和 ${\lambda }_{k+1}$相关，${\lambda }_{k+1}$又和 $P_{k+1}$ 相关，因此只要求出 $P_{k+1}$ 来， 就能算出来$u_k$，算出来之后 LQR 问题就迎刃而解了。

那么 $P_k$ 该怎么求呢？

  由$(4)$式直接可得到 $P_n=Q$ 。

  重复上面的递推过程，经过计算得到这样的递推式：
$$P_{k-1}=Q+A^T{P}_k(I+B{R}^{-1}{B}^T{P}_k{)}^{-1}A\text{\hspace{1em}(9)}$$  该递推式叫做 黎卡提方程(Riccati)。

5、控制量的表达式

  有了递推式又知道$P_n=Q$ ，就可以通过逆推出$P_{n-1}$，$P_{n-2}$，一直这样递推下去，算出来 $P_k$ 之后就好办了
$$\begin{array}{rl}{u}_k& =-\frac{1}{2}{R}^{-1}{B}^T{\lambda }_{k+1}\\ & =-\frac{1}{2}{R}^{-1}{B}^T\left(2P_{k+1}{X}_{k+1}\right)\\ & =-R^{-1}{B}^T{P}_{k+1}\left(A{X}_k+B{u}_k\right)\end{array}$$  移项得到
$$u_k=-{\left(R+B^T{P}_{k+1}B\right)}^{-1}{B}^T{P}_{k+1}A{X}_k$$

  这样得到了控制量 $u_k$ 的具体表达式，其中，$R、B、A$ 都是已知量， $P$ 是黎卡提方程算出来的，又和 $X_k$ 有关，但 $X_k$ 一般也是已知，因为 $X_k$ 一般取误差 $e_{rr}=X-X_r$，$X$ 就是当前车辆的状态， 通过定位得到的。$X_r$ 是规划的状态和速度，通过规划得到。如果定位也有，规划也有，两者相减得到误差，误差再乘以矩阵就得到控制量。

  当然这是非常笼统的说法，横向控制用到的误差要基于坐标变换变成 $(e_d,{\dot{e}}_d,e_{\phi },{\dot{e}}_{\phi })$，误差是$e_d$ 和 $e_{\phi }$，可以通过规划和定位得到，所以控制的上游就是规划模块以及定位模块。

  这两个如果都没有，那就没有办法做控制，所以控制一定是最后做的，就是当定位好了，规划也规划好了之后，才能去做控制。

---

六、黎卡提方程

  还要讲一下黎卡提方程。

1、迭代性质

  有人可能会觉得为什么叫方程呢？这不就是迭代的东西，就应该是迭代的递推的这样的式子，和方程有什么关系？为什么要把递推的式子叫做方程？

  因为当考虑 ${\sum }_{k=0}^{\infty }{x}^{T}Qx+u^{T}Ru$ ，$n$ 趋无穷时，仍然选 $P_n=Q$，要迭代无数次。

  一般 $(9)$式黎卡提方程，迭代几十次后就收敛不再变化了，即 $P_k=P_{k-1}=P_{k-2}$。

  所以在控制量里的 $P_{k+1}$ 实际上是 $P=Q+A^{T}P{\left(I+B{R}^{-1}{B}^{T}P\right)}^{-1}A$ 的解。

2、LQR控制量的计算

  最终的 LQR 控制首先是 $P$ 先取初值 $Q$，带到黎卡提方程迭代，当 $P$ 收敛后，代到 $u$ 的表达式中：
$$u=-(R+B^{T}PB{)}^{-1}{B}^{T}PA{X}_k$$  令 $K=(R+B^{T}PB{)}^{-1}{B}^{T}PA$，可简化为：
$$u=-K{X}_k$$   Matlab 里离散 LQR 的用法是
$$\left[K,s,E\right]=dlqr\left(A,B,Q,R\right)$$  其中 $K=(R+B^{T}PB{)}^{-1}{B}^{T}PA$， $s$ 是黎卡提方程的解 $P$， $E$ 为特征值。

  Matlab dlqr函数用法就是输入$A,B,Q,R$，就会计算出 $K,s,E$。算出 $K$ 以后， $u$ 就等于$-KX$，这样就可以得到想要的控制量。

3、其他书中的黎卡提方程形式

  其他书上的黎卡提方程形式如下：

$$P=Q+A^{T}PA-A^{T}PB(R+B^{T}PB{)}^{-1}{B}^{T}PA$$  和推导出来的黎卡提方程是一样的。

4、矩阵求逆引理的应用

  通过矩阵求逆引理化简：
$$(A+BCD{)}^{-1}=A^{-1}-A^{-1}B(C^{-1}+P{A}^{-1}B{)}^{-1}D{A}^{-1}$$  把推导出来的黎卡提方程拿下来：
$$P_{k-1}=Q+A^T{P}_k(I+B{R}^{-1}{B}^T{P}_k{)}^{-1}A$$  对括号里的部分使用矩阵求逆引理，令
$$A=IB=BC=R^{-1}D=B^T{P}_k$$  所以
$$(I+B{R}^{-1}{B}^T{P}_k{)}^{-1}=I-B(R+B^{T}PB{)}^{-1}{B}^{T}P$$  代入原方程化简得到：
$$P=Q+A^{T}P\left(I-B{\left(R+B^{T}PB\right)}^{-1}{B}^{T}P\right)A$$即$$P=Q+A^{T}PA-A^{T}PB{\left(R+B^{T}PB\right)}^{-1}{B}^{T}PA$$  推荐使用书上写的黎卡提方程，计算量小。

  控制量 $u=\delta$，$B$ 为 $4\times 1$矩阵，$R$ 为 $1\times 1$矩阵，$P$ 为 $4\times 4$矩阵。

  注意：矩阵求逆是很容易出错误的运算，特别是在当矩阵性质不太好时，矩阵求逆非常容易发生错误，存在舍入误差，最后求逆会得到错误的逆矩阵。

---

七、LQR算法总结

  首先写出误差导数
$${\dot{e}}_{rr}=A{e}_{rr}+Bu$$

1、离散化

$$e_{rr}(k+1)=\bar{A}{e}_{rr}(k)+\bar{B}u(k)$$

2、求解黎卡提方程

$$P=Q+{\bar{A}}^{T}P\bar{A}-{\bar{A}}^{T}P\bar{B}{\left(R+{\bar{B}}^{T}P\bar{B}\right)}^{-1}{\bar{B}}^{T}P\bar{A}$$

3、求解 K

$$K=\left(R+{\bar{B}}^{T}P{\bar{B}}^{-1}\right){\bar{B}}^{T}P\bar{A}$$

4、求解控制量

$$u(k)=-K{e}_{rr}(k)$$

  本节博客把 LQR 原理基本上讲的很明白了。

  但仍然不是特别完美，因为强行忽略了后面的小尾巴 $C{\theta }_r$，只考虑 ${\dot{e}}_{rr}=A{e}_{rr}+Bu$ 的情况计算出了控制量。

  下一节介绍考虑小尾巴 ${\dot{e}}_{rr}=A{e}_{rr}+Bu+C{\dot{\theta }}_r$ 的控制该怎么做，本篇博客就写这里，欢迎关注！

---

参考资料

  【基础】自动驾驶控制算法第五讲 连续方程的离散化与离散LQR原理

---

后记：

🌟 感谢您耐心阅读这篇关于 连续方程离散化与离散LQR原理 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡

🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀