【自动驾驶】控制算法（五）连续方程离散化与离散LQR原理

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝


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

---

文章目录

• 引言
• 一、连续LQR与离散LQR

• 1、LQR问题概述
• 2、离散LQR的优势
• 3、离散LQR的数学基础：拉格朗日乘子法

• 二、离散LQR问题描述

• 1、离散LQR问题描述
• 2、代价函数和约束条件
• 3、离散LQR问题分解

• 三、离散化方法

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

• (1) 向前欧拉法
• (2) 向后欧拉法
• (3) 中点欧拉法

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

• 四、离散LQR解法

• 1、代价函数
• 2、约束条件
• 3、代价函数和约束的拉格朗日乘子法表示

• 五、向量导数

• 1、向量导数规则
• 2、求解偏导数的规律
• 3、递推关系式的推导
• 4、黎卡提方程的引入
• 5、控制量的表达式

• 六、黎卡提方程

• 1、迭代性质
• 2、LQR控制量的计算
• 3、其他书中的黎卡提方程形式
• 4、矩阵求逆引理的应用

• 七、LQR算法总结

• 1、离散化
• 2、求解黎卡提方程
• 3、求解 K
• 4、求解控制量

• 参考资料

---

引言

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

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

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

---

一、连续LQR与离散LQR

1、LQR问题概述

  下面求 LQR 问题，就是在 $J$ 等于 $Q$ 和 $R$ 二次型里选择合适的 $u$，让 $J$ 最小，并且 $u$ 要满足 $\dot e_{rr}=Ae_{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\xrightarrow{\mathrm{discrete}}X_{k+1}=\bar{A}x_{k}+\bar{B}u_{k}$

2、代价函数和约束条件

  设代价函数为：
$J=\sum_{k=0}^{+\infty}(x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k})$  在约束条件$x_{k+1}=\bar{A}x_{k}+\bar{B}u_{k}$下的极小值。

$J$

3、离散LQR问题分解

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

• 如何实现$\dot{{X}}=AX+B{u}\xrightarrow{\mathrm{discrete}}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}{A}xdt$$x\left( t+dt \right) -x\left( t \right) =Ax\left( \xi \right) dt\quad \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\quad$$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}B u(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}Qx_{k}+u_{k}^{T}Ru_{k})$  希望能达到稳定的控制，即在任何时段， $k$ 等于 $0$

2、约束条件

$X_{k+1}=Ax_{k}+Bu_{k}$ ，$A$ 对应上面的 $\bar A$， $B$ 对应上面的 $\bar B$。

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

  具体做法是：
$J=\sum_{k=0}^{n-1}(x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k})+x_{n}^{T}Qx_{n}$  让 $n$

问题来了，为什么没有 $u_{n}^{T}Qu_{n}$

  先把约束写出来：
$\begin{array}{c} Ax_0+Bu_0-x_1=0\\ Ax_1+Bu_1-x_2=0\\ \vdots\\ Ax_{n-1}+Bu_{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}Qx_k+u_{k}^{T}Ru_k \right)}+x_{n}^{T}Qx_n+\sum_{k=0}^{n-1}{\lambda _{k+1}^{T}\left( Ax_k+Bu_k-x_{k+1} \right)}$  这就是带约束的拉格朗日乘子法，注意 $Ax_k+Bu_k-x_{k+1}$ 可能不是数，而是列向量，所以 $\lambda _{k+1}$

$0$ 到 $n +1$ 相加，可以合并：
$J=\sum_{k=0}^{n-1}{\left[ x_{k}^{T}Qx_k+u_{k}^{_T}Ru_k+\lambda _{k+1}^T\left( Ax_k+Bu_k \right) -\lambda _{k+1}^{T}x_{k+1} \right]}+x_{n}^{T}Qx_n$

$x_k$ 和 $u_k$ 的项，起个新的名字 $H_k$，设
$H_k=x_{k}^{T}Qx_k+u_{k}^{_T}Ru_k+\lambda _{k+1}^T\left( Ax_k+Bu_k \right)$  这样可以写成非常简单的形式：
$J=\sum_{k=0}^{n-1}(H_{k}-\lambda_{k+1}^{T}x_{k+1})+x_{n}^{T}Qx_{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}Qx_n$  最后再加上 $\lambda_0^Tx_0$ 再减去负的 $\lambda_0^Tx_0$，这样和原式相等，但加了负的 $\lambda_0^Tx_0$ 就可以和前面东西合并：
$\begin{aligned} 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}Qx_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}Qx_n+\lambda _{0}^{T}x_0\\ &=\sum_{k=0}^{n-1}{\left( H_k-\lambda _{k}^{T}x_k \right)}+x_{n}^{T}Qx_n-\lambda _{n}^{T}x_n+\lambda _{0}^{T}x_0\\ \end{aligned}$
  写成这样就可以求导。

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

---

五、向量导数

1、向量导数规则

  规则如下：
$\frac{\partial(x^{T}A)}{\partial x}=A\quad\frac{\partial(Ax)}{\partial x}=A^{T}\quad\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\quad \frac{\partial \lambda _{0}^{T}x_0}{\partial x_0}=0\quad \Rightarrow \quad \lambda _0=0$$\frac{\partial J}{\partial x_1}=0\quad \frac{\partial H_1}{\partial x_1}-\frac{\partial \left( \lambda ^Tx_1 \right)}{\partial x_1}=0\quad \Rightarrow \quad \lambda _1=\frac{\partial H_1}{\partial x_1}$$\frac{\partial J}{\partial x_2}=0\quad \frac{\partial H_2}{\partial x_2}-\frac{\partial \left( \lambda ^Tx_2 \right)}{\partial x_2}=0\quad \Rightarrow \quad \lambda _2=\frac{\partial H_2}{\partial x_2}$  发现偏导有相同的规律，可直接写成：
$\begin{aligned} &\frac{\partial J}{\partial x_{k}}=0\quad\Rightarrow \quad \lambda_{k}=\frac{\partial H_{k}}{\partial x_{k}}\quad(k=1,2,3,\ldots ,n-1)\\ &\frac{\partial J}{\partial u_{k}}=0\quad\Rightarrow\quad\frac{\partial H_{k}}{\partial u_{k}}=0\quad(k=1,2,3,\cdots ,n-1)\\ &\frac{\partial J}{\partial x_n}=0\quad \Rightarrow \quad \frac{\partial \left( x_{n}^{T}Qx_n-\lambda _{n}^{T}x_n \right)}{\partial x_n}=0\\ &\frac{\partial J}{\partial \lambda _k}=0\quad \Rightarrow \quad x_{k+1}=Ax_k+Bu_k\quad \left( k=1,2,3,...,n-1 \right)\\ \end{aligned}$  上式中的 $0$ 都是 $0$

$\frac{\partial H_k}{\partial x_k}$ 以及 $\frac{\partial H_k}{\partial u_k}$。首先把 $H_k$ 的定义拿下来：
$H_k=x_{k}^{T}Qx_k+u_{k}^{_T}Ru_k+\lambda _{k+1}^T\left( Ax_k+Bu_k \right)$  根据向量导数得到：
$\frac{\partial H_k}{\partial x_k}=2Qx_k+A^T\lambda _{k+1}$$\frac{\partial H_k}{\partial u_k}=2Ru_k+B^T\lambda _{k+1}$  把式子带到上面四个等于 $0$

$\left\{ \begin{aligned} & \lambda _k = 2Qx_k + A^T\lambda _{k+1} & \quad (k=1,2,\cdots ,n-1) & \quad (1) \\ & u_k = -\frac{1}{2}R^{-1}B^T\lambda _{k+1} & \quad (k=1,2,\cdots ,n-1) & \quad (2) \\ & x_{k+1} = Ax_k + Bu_k & \quad (k=1,2,\cdots ,n-1) & \quad (3) \\ & \lambda _n = 2QX_n & & \quad (4) \end{aligned} \right.$  看起来好像是四个式子，但实际上是 $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=2QX_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{equation} u_{n-1} = -\frac{1}{2}R^{-1}B^{T}(2QX_{n}) = -R^{-1}B^{T}QX_{n} \tag{5} \end{equation}$  将$(5)$代入$(3)$的 $(k=n-1)$ 式：
$\begin{aligned} x_n&=Ax_{n-1}+Bu_{n-1}\\ &=Ax_{n-1}+\left( -BR^{-1}B^TQx_n \right)\\ &=\left( I+BR^{-1}B^TQ \right) ^{-1}Ax_{n-1} \tag{6}\\ \end{aligned}$  将$(4)$代入$(1)$的 $(k=n-1)$ 式：
$\begin{aligned}\lambda_{n-1}&=2QX_{n-1}+A^{T}\lambda_{n}\\&=2QX_{n-1}+A^{T}2QX_{n}\tag{7}\end{aligned}$  将$(6)$代入$(7)$的 $(k=n-1)$ 式：
$\lambda_{n-1}=2QX_{n-1}+2A^{T}Q(I+BR^{-1}B^{T}Q)^{-1}AX_{n-1}\\=2(Q+A^{T}Q(I+BR^{-1}B^{T}Q)^{-1})AX_{n-1}\tag{8}$  将$(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+BR^{-1}B^{T}P_{k})^{-1}A \tag{9}$  该递推式叫做 黎卡提方程(Riccati)。

5、控制量的表达式

$P_n=Q$ ，就可以通过逆推出$P_{n-1}$，$P_{n-2}$，一直这样递推下去，算出来 $P_k$ 之后就好办了
$\begin{aligned} 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^TP_{k+1}\left( AX_k+Bu_k \right)\\ \end{aligned}$  移项得到
$u_k=-\left( R+B^TP_{k+1}B \right) ^{-1}B^TP_{k+1}AX_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_\varphi, \dot e_\varphi)$，误差是$e_d$ 和 $e_\varphi$，可以通过规划和定位得到，所以控制的上游就是规划模块以及定位模块。

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

---

六、黎卡提方程

  还要讲一下黎卡提方程。

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^TP\left( I+BR^{-1}B^TP \right) ^{-1}A$

2、LQR控制量的计算

$P$ 先取初值 $Q$，带到黎卡提方程迭代，当 $P$ 收敛后，代到 $u$ 的表达式中：
$u=-(R+B^{T}PB)^{-1}B^{T}PAX_{k}$  令 $K=(R+B^{T}PB)^{-1}B^{T}PA$，可简化为：
$u=-KX_{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^TPA-A^TPB(R+B^TPB)^{-1}B^TPA$

4、矩阵求逆引理的应用

  通过矩阵求逆引理化简：
$(A+BCD)^{-1}=A^{-1}-A^{-1}B(C^{-1}+PA^{-1}B)^{-1}DA^{-1}$  把推导出来的黎卡提方程拿下来：
$P_{k-1}=Q+A^{T}P_{k}(I+BR^{-1}B^{T}P_{k})^{-1}A$  对括号里的部分使用矩阵求逆引理，令
$A=I \quad B=B\quad C=R^{-1}\quad D=B^{T}P_{k}$  所以
$(I+BR^{-1}B^TP_k)^{-1}=I-B(R+B^TPB)^{-1}B^TP$  代入原方程化简得到：
$P=Q+A^TP\left( I-B\left( R+B^TPB \right) ^{-1}B^TP \right) A$即$P=Q+A^TPA-A^TPB\left( R+B^TPB \right) ^{-1}B^TPA$

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

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

---

七、LQR算法总结

  首先写出误差导数
$\dot e_{rr}=Ae_{rr}+Bu$

1、离散化

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

2、求解黎卡提方程

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

3、求解 K

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

4、求解控制量

$u(k)=-Ke_{rr}(k)$

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

$C\theta_r$，只考虑 $\dot e_{rr}=Ae_{rr}+Bu$

$\dot e_{rr}=Ae_{rr}+Bu+C\dot{\theta}_r$

---

参考资料

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

---

后记：

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

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

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

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

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