写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落 。📝
个人主页 ：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。👍 点赞、评论、收藏 ，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货 ，还有思维的火花！📚 系列专栏：【运动控制】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~
文章目录
- 引言
- 一、连续LQR与离散LQR
- 二、离散LQR问题描述
- 三、离散化方法
-
- 1、连续微分方程的离散化
-
- [(1) 向前欧拉法](#(1) 向前欧拉法)
- [(2) 向后欧拉法](#(2) 向后欧拉法)
- [(3) 中点欧拉法](#(3) 中点欧拉法)
- 2、连续方程离散化的数学推导
- 四、离散LQR解法
- 五、向量导数
- 六、黎卡提方程
- 七、LQR算法总结
- 参考资料
引言
本篇博客是 自动驾驶控制算法 系列的第五节。内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。
在上一节将坐标变换和车辆动力学模型综合起来，得到了关于误差的微分方程：
可以写成 e ˙ r r = A e r r + B u + C θ ˙ r \dot e_{rr}=Ae_{rr}+Bu+C\dot{\theta}_r e˙rr=Aerr+Bu+Cθ˙r。暂时先忽略 C θ ˙ r C\dot{\theta}r Cθ˙r，考虑 C θ ˙ r C\dot{\theta}r Cθ˙r 的控制以后再讲，暂时不管它。上面的误差微分方程变成如下形式：
e ˙ r r = A e r r + B u \dot e{rr}=Ae{rr}+Bu e˙rr=Aerr+Bu
一、连续LQR与离散LQR
1、LQR问题概述
下面求 LQR 问题，就是在 J J J 等于 Q Q Q 和 R R R 二次型里选择合适的 u u u，让 J J J 最小，并且 u u u 要满足 e ˙ r r = A e r r + B u \dot e_{rr}=Ae_{rr}+Bu e˙rr=Aerr+Bu 的约束，这就是典型的 LQR 问题， LQR 问题已经十分成熟，在 Matlab 里有包，可以调用 l q r ( A , B , Q , R ) \mathrm{lqr}(A,B,Q,R) lqr(A,B,Q,R)，以及 d l q r ( A , B , Q , R ) \mathrm{dlqr}(A,B,Q,R) dlqr(A,B,Q,R)，就是离散的(discrete) LQR。
本篇博客讲解 dLQR 的基本原理，因为我们并不满足于当调包师、调参数侠，而是要知道原理。
2、离散LQR的优势
LQR 分为连续 LQR 和离散 LQR ，但是就本博客只讲离散 LQR ，因为实际应用都是离散 LQR ，特别是在计算机上，计算机处理的是离散数据，而且控制大多也是离散，所以离散 LQR 对实际应用很有用。
第二个原因是连续 LQR 的理论非常难，涉及到泛函和变分法，而且一般用不到。
3、离散LQR的数学基础：拉格朗日乘子法
而离散 LQR 只涉及拉格朗日乘子法。
什么是拉格朗日乘子法？
就是求 f ( x , y ) f (x ,y) f(x,y) 在约束 g ( x , y ) = 0 g (x, y) =0 g(x,y)=0 的条件下的机制，用拉格朗日乘子法，就是建立
L ( x , y ) = f ( x , y ) + λ g ( x , y ) L(x,y)=f(x,y)+\lambda g(x,y) L(x,y)=f(x,y)+λg(x,y)其中， ∂ L ∂ x = 0 , ∂ L ∂ y = 0 , ∂ L ∂ λ = 0 \frac{\partial L}{\partial x}=0,\frac{\partial L}{\partial y}=0,\frac{\partial L}{\partial\lambda}=0 ∂x∂L=0,∂y∂L=0,∂λ∂L=0。
在自动控制原理书中，使用离散泛函与变分法解离散 LQR，但这样涉及到了太多数学知识，而使用拉格朗日乘子法，可以绕过泛函与变分法的概念把离散 LQR 讲明白。
二、离散LQR问题描述
1、离散LQR问题描述
离散 LQR 的问题描述：
X ˙ = A X + B u → d i s c r e t e X k + 1 = A ˉ x k + B ˉ u k \dot{X}=AX+Bu\xrightarrow{\mathrm{discrete}}X_{k+1}=\bar{A}x_{k}+\bar{B}u_{k} X˙=AX+Budiscrete Xk+1=Aˉxk+Bˉuk
2、代价函数和约束条件
设代价函数为：
J = ∑ k = 0 + ∞ ( x k T Q x k + u k T R u k ) J=\sum_{k=0}^{+\infty}(x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k}) J=k=0∑+∞(xkTQxk+ukTRuk) 在约束条件 x k + 1 = A ˉ x k + B ˉ u k x_{k+1}=\bar{A}x_{k}+\bar{B}u_{k} xk+1=Aˉxk+Bˉuk下的极小值。
这样和拉格朗日乘子法很像，它们都是某个函数，在另一个函数的约束下，求函数的极小值，而且极小值一定就是最小值，因为根据 J J J 是二次型，只有极值点，而且是极小值点，自然极小值就是最小值。
3、离散LQR问题分解
现在离散 LQR 问题分解为两个问题：
-
如何实现 X ˙ = A X + B u → d i s c r e t e X k + 1 = A ˉ X k + B ˉ u k \dot{{X}}=AX+B{u}\xrightarrow{\mathrm{discrete}}X_{k+1}=\bar{A}X_{k}+\bar{B}u_{k} X˙=AX+Budiscrete Xk+1=AˉXk+Bˉuk？
-
如何求解使得 J J J 在约束条件 X k + 1 = A ˉ X k + B ˉ u k X_{k+1}=\bar{A}X_{k}+\bar{B}u_{k} Xk+1=AˉXk+Bˉuk下取极小值的 u k u_k uk？
三、离散化方法
1、连续微分方程的离散化
先讲离散化，有两种方法：
- 向前欧拉法
- 中点欧拉法
百度 Apollo 的代码是两者混合。
下面简单讲一下向前欧拉法和中点欧拉法。
针对 x ˙ = A x \dot x=Ax x˙=Ax 方程，两边积分下限为 t t t，积分上限为 t + d t t+dt t+dt，得到以下式子：
∫ t t + d t x ˙ d t = ∫ t t + d t A x d t \int_t^{t+dt}{\dot{x}}dt=\int_t^{t+dt}{A}xdt ∫tt+dtx˙dt=∫tt+dtAxdt x ( t + d t ) − x ( t ) = A x ( ξ ) d t ξ ∈ ( t , t + d t ) x\left( t+dt \right) -x\left( t \right) =Ax\left( \xi \right) dt\quad \xi \in \left( t,t+dt \right) x(t+dt)−x(t)=Ax(ξ)dtξ∈(t,t+dt) 把后面积分化为 ξ \xi ξ，用的是积分中值定理，得到：
x ( t + d t ) = x ( t ) + A x ( ξ ) d t ξ ∈ ( t , t + d t ) x\left( t+dt \right) =x\left( t \right) +Ax\left( \xi \right) dt\ \ \ \ \ \xi \in \left( t,t+dt \right) x(t+dt)=x(t)+Ax(ξ)dt ξ∈(t,t+dt) 这是精确的离散化，但是这样离散化没有用，因为不知道 ξ \xi ξ 到底是什么，根据 ξ \xi ξ 到底是什么有各种近似解法：
(1) 向前欧拉法
认为 X ( ξ ) = X ( t ) X(\xi)=X(t) X(ξ)=X(t)：
X ( t + d t ) = ( I + A d t ) X ( t ) X(t+dt)=(I+Adt)X(t) X(t+dt)=(I+Adt)X(t)
(2) 向后欧拉法
认为 X ( ξ ) = X ( t + d t ) X(\xi)=X(t+dt) X(ξ)=X(t+dt)：
X ( t + d t ) = ( I − A d t ) − 1 X ( t ) X(t+dt)=( I-Adt)^{-1}X(t ) X(t+dt)=(I−Adt)−1X(t)
(3) 中点欧拉法
认为 X ( ξ ) = X ( t ) + X ( t + d t ) 2 X(\xi)=\frac{X(t)+X(t+dt)}{2} X(ξ)=2X(t)+X(t+dt)：
X ( t + d t ) = ( I − A d t 2 ) − 1 ( I + A d t 2 ) X ( t ) X(t+dt)=(I-\frac{Adt}{2})^{-1}(I+\frac{Adt}{2})X(t) X(t+dt)=(I−2Adt)−1(I+2Adt)X(t) 这就是三种离散化方法。
一般中点欧拉法的精度比向前欧拉法和向后欧拉法要高，这些欧拉法都是对精确解的 X ( ξ ) X(\xi) X(ξ) 做不同程度的近似。
2、连续方程离散化的数学推导
对于 x ˙ = A x + B u \dot{x}=Ax+Bu x˙=Ax+Bu 方程，先两边积分，将积分划为中式定理形式，得到以下方程：
x ˙ = A x + B u ⇒ ∫ t t + d t x ˙ d t = ∫ t t + d t ( A x + B u ) d t \dot{x}=Ax+Bu\Rightarrow \int_t^{t+dt}{\dot{x}dt}=\int_t^{t+dt}{\left( Ax+Bu \right)}dt\quad x˙=Ax+Bu⇒∫tt+dtx˙dt=∫tt+dt(Ax+Bu)dt X ( t + d t ) − X ( t ) = A x ( ξ ) d t + B u ( ξ ) d t X\left( t+dt \right) -X\left( t \right) =Ax\left( \xi \right) dt+Bu\left( \xi \right) dt X(t+dt)−X(t)=Ax(ξ)dt+Bu(ξ)dt即：
X ( t + d t ) = A X ( ξ ) d t + X ( t ) + B u ( ξ ) d t X(t+dt)=AX(\xi)dt+X(t)+Bu(\xi)dt X(t+dt)=AX(ξ)dt+X(t)+Bu(ξ)dt 对第一个 X ( ξ ) X(\xi) X(ξ) 用中点欧拉法，对第二个 u ( ξ ) u(\xi) u(ξ) 用向前欧拉法。
第二个 u ( ξ ) u(\xi) u(ξ) 不用终点欧拉法是因为 u ( t + d t ) u(t+dt) u(t+dt) 未知，一般 d t dt dt 取采样周期，比如 0.01 秒或 0.001 秒， u ( t ) u(t) u(t) 一般是要求的当前控制量，这是 LQR 要算的， u ( t + d t ) u(t+dt) u(t+dt) 是下一时刻的 LQR 控制量，当前的控制量 u ( t ) u(t) u(t) 都不知道，下一时刻的 u ( t + d t ) u(t+dt) u(t+dt) 更不知道，所以第二个 u ( ξ ) u(\xi) u(ξ)不能用中点欧拉法。
那为什么第一个 X ( ξ ) X(\xi) X(ξ) 用中点欧拉法呢？
因为 X ( t + d t ) X(t+dt) X(t+dt)，虽然不知道，但它不需要知道。因为只要把它带进去化简就出来了：
X ( t + d t ) = A d t ( X ( t + d t ) + X ( t ) 2 ) + X ( t ) + B u ( t ) d t X(t+dt)=Adt(\frac{X(t+dt)+X(t)}{2})+X(t)+Bu(t)dt X(t+dt)=Adt(2X(t+dt)+X(t))+X(t)+Bu(t)dt 把右边 X ( t + d t ) X(t+dt) X(t+dt) 移到左边，得到
( I − A d t 2 ) X ( t + d t ) = ( I + A d t 2 ) X ( t ) + B u ( t ) d t (I-\frac{Adt}{2})X(t+dt)=(I+\frac{Adt}{2})X(t)+Bu(t)dt (I−2Adt)X(t+dt)=(I+2Adt)X(t)+Bu(t)dt 在左边求逆：
X ( t + d t ) = ( I − A d t 2 ) − 1 ( I + A d t 2 ) X ( t ) + ( I − A d t 2 ) − 1 B u ( t ) d t X(t+dt)=(I-\frac{Adt}{2})^{-1}(I+\frac{Adt}{2})X(t)+(I-\frac{Adt}{2})^{-1}B u(t)dt X(t+dt)=(I−2Adt)−1(I+2Adt)X(t)+(I−2Adt)−1Bu(t)dt 因为 d t dt dt 通常比较小 0.01 、 0.02 0.01、0.02 0.01、0.02 这样的数量级，后面矩阵求逆就直接省掉了，可以少算矩阵求逆的计算量。
这样得到以下方程：
X ( t + d t ) = ( I − A d t 2 ) − 1 ( I + A d t 2 ) X ( t ) + B u ( t ) d t X(t+dt)=(I-\frac{Adt}{2})^{-1}(I+\frac{Adt}{2})X(t)+Bu(t)dt X(t+dt)=(I−2Adt)−1(I+2Adt)X(t)+Bu(t)dt 其中， I I I 是单位矩阵， d t dt dt 是采样周期，得到离散化的微分方程：
X ( k + 1 ) = A ˉ X ( k ) + B ˉ u ( k ) X(k+1)=\bar{A}X(k)+\bar{B}u(k) X(k+1)=AˉX(k)+Bˉu(k) 其中， A ˉ = ( I − A d t 2 ) − 1 ( I + A d t 2 ) \bar{A}=(I-\frac{Adt}{2})^{-1}(I+\frac{Adt}{2}) Aˉ=(I−2Adt)−1(I+2Adt)， B ˉ = B d t \bar{B}=Bdt Bˉ=Bdt。
以上是连续方程离散化的全部过程。
四、离散LQR解法
下面看一下离散的 LQR 的解法。
1、代价函数
代价函数 J J J 是关于 x x x 和 u u u 的二次型：
J = ∑ k = 0 + ∞ ( x k T Q x k + u k T R u k ) J=\sum_{k=0}^{+\infty}(x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k}) J=k=0∑+∞(xkTQxk+ukTRuk) 希望能达到稳定的控制，即在任何时段， k k k 等于 0 0 0 到正无穷时间段所有误差的和它要最小。
2、约束条件
约束条件就是上面所推导的离散微分方程 X k + 1 = A x k + B u k X_{k+1}=Ax_{k}+Bu_{k} Xk+1=Axk+Buk ， A A A 对应上面的 A ˉ \bar A Aˉ， B B B 对应上面的 B ˉ \bar B Bˉ。
k k k 从 0 0 0 到至无穷，这无穷就不太好处理，那就先不让 k k k 从 0 0 0 到正无穷这样加下去，先让 k k k 从 0 0 0 一直加到 n n n，再让 n n n 取无穷。
具体做法是：
J = ∑ k = 0 n − 1 ( x k T Q x k + u k T R u k ) + x n T Q x n J=\sum_{k=0}^{n-1}(x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k})+x_{n}^{T}Qx_{n} J=k=0∑n−1(xkTQxk+ukTRuk)+xnTQxn 让 n n n 趋无穷就可以了。
问题来了，为什么没有 u n T Q u n u_{n}^{T}Qu_{n} unTQun 这一项呢？
先把约束写出来：
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 \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} Ax0+Bu0−x1=0Ax1+Bu1−x2=0⋮Axn−1+Bun−1−xn=0 约束覆盖了从 x 0 x_0 x0 到 x n x_n xn 以及从 u 0 u_0 u0 到 u n − 1 u_{n-1} un−1。发现写的代价函数 J J J 正好是 从 x 0 x_0 x0 到 x n x_n xn 以及从 u 0 u_0 u0 到 u n − 1 u_{n-1} un−1的这样函数，即约束完全覆盖二次型的自变量。
所以 u u u 天生就比 x x x 少，最后写的二次型，必须也得少，才能把约束全部匹配上。
3、代价函数和约束的拉格朗日乘子法表示
最后用拉格朗日乘子法写出 J J J 来：
J = ∑ k = 0 n − 1 ( x k T Q x k + u k T R u k ) + x n T Q x n + ∑ k = 0 n − 1 λ k + 1 T ( A x k + B u k − x k + 1 ) 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)} J=k=0∑n−1(xkTQxk+ukTRuk)+xnTQxn+k=0∑n−1λk+1T(Axk+Buk−xk+1) 这就是带约束的拉格朗日乘子法，注意 A x k + B u k − x k + 1 Ax_k+Bu_k-x_{k+1} Axk+Buk−xk+1 可能不是数，而是列向量，所以 λ k + 1 \lambda _{k+1} λk+1 要有转置，因为两个列向量不能相乘，把它变成行向量，行向量乘列向量才有意义。
发现这两个求和符号它一样，都是从 0 0 0 到 n + 1 n +1 n+1 相加，可以合并：
J = ∑ k = 0 n − 1 [ x k T Q x k + u k T R u k + λ k + 1 T ( A x k + B u k ) − λ k + 1 T x k + 1 ] + x n T Q x n 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 J=k=0∑n−1[xkTQxk+ukTRuk+λk+1T(Axk+Buk)−λk+1Txk+1]+xnTQxn 但是这么写太长了。
把关于 x k x_k xk 和 u k u_k uk 的项，起个新的名字 H k H_k Hk，设
H k = x k T Q x k + u k T R u k + λ k + 1 T ( A x k + B u k ) H_k=x_{k}^{T}Qx_k+u_{k}^{T}Ru_k+\lambda {k+1}^T\left( Ax_k+Bu_k \right) Hk=xkTQxk+ukTRuk+λk+1T(Axk+Buk) 这样可以写成非常简单的形式：
J = ∑ k = 0 n − 1 ( H k − λ k + 1 T x k + 1 ) + x n T Q x n J=\sum{k=0}^{n-1}(H{k}-\lambda_{k+1}^{T}x_{k+1})+x_{n}^{T}Qx_{n} J=k=0∑n−1(Hk−λk+1Txk+1)+xnTQxn 把它展开变成：
J = H 0 + H 1 + ⋯ + H n − 1 + ( − λ 1 T x 1 − λ 2 T x 2 − ⋯ − λ n T x n ) + 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}Qx_n J=H0+H1+⋯+Hn−1+(−λ1Tx1−λ2Tx2−⋯−λnTxn)+xnTQxn 最后再加上 λ 0 T x 0 \lambda_0^Tx_0 λ0Tx0 再减去负的 λ 0 T x 0 \lambda_0^Tx_0 λ0Tx0，这样和原式相等，但加了负的 λ 0 T x 0 \lambda_0^Tx_0 λ0Tx0 就可以和前面东西合并：
J = ∑ k = 0 n − 1 H k + ∑ k = 1 n − 1 ( − λ k T x k ) + ( − λ n T x n ) + x n T Q x n + ( − λ 0 T x 0 ) − ( − λ 0 T x 0 ) = ∑ k = 0 n − 1 H k + ∑ k = 0 n − 1 ( − λ k T x k ) − λ n T x n + x n T Q x n + λ 0 T x 0 = ∑ k = 0 n − 1 ( H k − λ k T x k ) + x n T Q x n − λ n T x n + λ 0 T x 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} J=k=0∑n−1Hk+k=1∑n−1(−λkTxk)+(−λnTxn)+xnTQxn+(−λ0Tx0)−(−λ0Tx0)=k=0∑n−1Hk+k=0∑n−1(−λkTxk)−λnTxn+xnTQxn+λ0Tx0=k=0∑n−1(Hk−λkTxk)+xnTQxn−λnTxn+λ0Tx0
写成这样就可以求导。
注意：是向量求导，不是数的求导，下面复习一下向量求导。
五、向量导数
1、向量导数规则
规则如下：
∂ ( x T A ) ∂ x = A ∂ ( A x ) ∂ x = A T ∂ ( x T A x ) ∂ x = ( A + A T ) x \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 ∂x∂(xTA)=A∂x∂(Ax)=AT∂x∂(xTAx)=(A+AT)x 若 A A A 为对称矩阵， ∂ ( x T A x ) ∂ x = ( A + A T ) x = 2 A x \frac{\partial(x^{T}Ax)}{\partial x}=(A+A^{T})x=2Ax ∂x∂(xTAx)=(A+AT)x=2Ax
因为 Q Q Q 和 R R R 都是对称矩阵，所以对 Q Q Q 和 R R R 的二次型求导直接乘以 2 2 2 即可。
2、求解偏导数的规律
先写几个找规律：
∂ J ∂ x 0 = 0 ∂ λ 0 T x 0 ∂ x 0 = 0 ⇒ λ 0 = 0 \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 ∂x0∂J=0∂x0∂λ0Tx0=0⇒λ0=0 ∂ J ∂ x 1 = 0 ∂ H 1 ∂ x 1 − ∂ ( λ T x 1 ) ∂ x 1 = 0 ⇒ λ 1 = ∂ H 1 ∂ x 1 \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} ∂x1∂J=0∂x1∂H1−∂x1∂(λTx1)=0⇒λ1=∂x1∂H1 ∂ J ∂ x 2 = 0 ∂ H 2 ∂ x 2 − ∂ ( λ T x 2 ) ∂ x 2 = 0 ⇒ λ 2 = ∂ H 2 ∂ x 2 \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} ∂x2∂J=0∂x2∂H2−∂x2∂(λTx2)=0⇒λ2=∂x2∂H2 发现偏导有相同的规律，可直接写成：
∂ J ∂ x k = 0 ⇒ λ k = ∂ H k ∂ x k ( k = 1 , 2 , 3 , ... , n − 1 ) ∂ J ∂ u k = 0 ⇒ ∂ H k ∂ u k = 0 ( k = 1 , 2 , 3 , ⋯ , n − 1 ) ∂ J ∂ x n = 0 ⇒ ∂ ( x n T Q x n − λ n T x n ) ∂ x n = 0 ∂ J ∂ λ k = 0 ⇒ x k + 1 = A x k + B u k ( k = 1 , 2 , 3 , . . . , n − 1 ) \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} ∂xk∂J=0⇒λk=∂xk∂Hk(k=1,2,3,...,n−1)∂uk∂J=0⇒∂uk∂Hk=0(k=1,2,3,⋯,n−1)∂xn∂J=0⇒∂xn∂(xnTQxn−λnTxn)=0∂λk∂J=0⇒xk+1=Axk+Buk(k=1,2,3,...,n−1) 上式中的 0 0 0 都是 0 0 0 向量。
下面计算 ∂ H k ∂ x k \frac{\partial H_k}{\partial x_k} ∂xk∂Hk 以及 ∂ H k ∂ u k \frac{\partial H_k}{\partial u_k} ∂uk∂Hk。首先把 H k H_k Hk 的定义拿下来：
H k = x k T Q x k + u k T R u k + λ k + 1 T ( A x k + B u k ) H_k=x_{k}^{T}Qx_k+u_{k}^{_T}Ru_k+\lambda _{k+1}^T\left( Ax_k+Bu_k \right) Hk=xkTQxk+ukTRuk+λk+1T(Axk+Buk) 根据向量导数得到：
∂ H k ∂ x k = 2 Q x k + A T λ k + 1 \frac{\partial H_k}{\partial x_k}=2Qx_k+A^T\lambda _{k+1} ∂xk∂Hk=2Qxk+ATλk+1 ∂ H k ∂ u k = 2 R u k + B T λ k + 1 \frac{\partial H_k}{\partial u_k}=2Ru_k+B^T\lambda _{k+1} ∂uk∂Hk=2Ruk+BTλk+1 把式子带到上面四个等于 0 0 0 的式子中，可推导出以下四个方程：
{ λ k = 2 Q x k + A T λ k + 1 ( k = 1 , 2 , ⋯ , n − 1 ) ( 1 ) u k = − 1 2 R − 1 B T λ k + 1 ( k = 1 , 2 , ⋯ , n − 1 ) ( 2 ) x k + 1 = A x k + B u k ( k = 1 , 2 , ⋯ , n − 1 ) ( 3 ) λ n = 2 Q X n ( 4 ) \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. ⎩ ⎨ ⎧λk=2Qxk+ATλk+1uk=−21R−1BTλk+1xk+1=Axk+Bukλn=2QXn(k=1,2,⋯,n−1)(k=1,2,⋯,n−1)(k=1,2,⋯,n−1)(1)(2)(3)(4) 看起来好像是四个式子，但实际上是 3 n − 2 3n- 2 3n−2 个式子。
看一下这 3 n − 2 3n- 2 3n−2 个式子，发现 ( 2 ) (2) (2)式明确给出了 u k = − 1 2 R − 1 B T λ k + 1 u_k=-\frac{1}{2}R^{-1}B^T\lambda {k+1} uk=−21R−1BTλk+1，其中 R R R 和 B B B 已知，剩下的 λ k + 1 \lambda{k+1} λk+1 未知，如果算出 λ k + 1 \lambda_{k+1} λk+1 等于多少？就可以算出 u k u_k uk，这样 LQR 问题就解决了。
问题是 λ k + 1 \lambda_{k+1} λk+1 该怎么算？
从 ( 4 ) (4) (4)式可以得到 λ n = 2 Q X n \lambda n=2QX_n λn=2QXn，和 Q Q Q 和 X n X_n Xn 有关，而 ( 1 ) (1) (1)式和 ( 3 ) (3) (3)式都表示 λ k + 1 \lambda{k+1} λk+1和 λ k \lambda_k λk 以及 x k + 1 x_{k+1} xk+1和 x x x 之间的关系。
如果知道 λ n \lambda _n λn 等于什么的话，通过 ( 1 ) (1) (1)式和 ( 3 ) (3) (3)式反向逆推，一直推到初始状态。
3、递推关系式的推导
递推式如下：
将 ( 4 ) (4) (4)代入 ( 2 ) (2) (2)的 ( k = n − 1 ) (k=n-1) (k=n−1) 式：
u n − 1 = − 1 2 R − 1 B T ( 2 Q X n ) = − R − 1 B T Q X n \begin{equation} u_{n-1} = -\frac{1}{2}R^{-1}B^{T}(2QX_{n}) = -R^{-1}B^{T}QX_{n} \tag{5} \end{equation} un−1=−21R−1BT(2QXn)=−R−1BTQXn(5) 将 ( 5 ) (5) (5)代入 ( 3 ) (3) (3)的 ( k = n − 1 ) (k=n-1) (k=n−1) 式：
x n = A x n − 1 + B u n − 1 = A x n − 1 + ( − B R − 1 B T Q x n ) = ( I + B R − 1 B T Q ) − 1 A x n − 1 (6) \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} xn=Axn−1+Bun−1=Axn−1+(−BR−1BTQxn)=(I+BR−1BTQ)−1Axn−1(6) 将 ( 4 ) (4) (4)代入 ( 1 ) (1) (1)的 ( k = n − 1 ) (k=n-1) (k=n−1) 式：
λ n − 1 = 2 Q X n − 1 + A T λ n = 2 Q X n − 1 + A T 2 Q X n (7) \begin{aligned}\lambda_{n-1}&=2QX_{n-1}+A^{T}\lambda_{n}\\&=2QX_{n-1}+A^{T}2QX_{n}\tag{7}\end{aligned} λn−1=2QXn−1+ATλn=2QXn−1+AT2QXn(7) 将 ( 6 ) (6) (6)代入 ( 7 ) (7) (7)的 ( k = n − 1 ) (k=n-1) (k=n−1) 式：
λ n − 1 = 2 Q X n − 1 + 2 A 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 (8) \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} λn−1=2QXn−1+2ATQ(I+BR−1BTQ)−1AXn−1=2(Q+ATQ(I+BR−1BTQ)−1)AXn−1(8) 将 ( 4 ) (4) (4)与 ( 8 ) (8) (8)式比较，发现很像。
4、黎卡提方程的引入
递推性质所决定的，从 λ n \lambda n λn 推到 λ n − 1 \lambda {n-1} λn−1， λ n − 2 \lambda {n-2} λn−2，一定有相同形式，不妨设：
λ k = 2 P k x k ( k = 1 , 2 , 3 , ⋯ , n ) \lambda{k}=2P{k}x{k}(k=1,2,3,\cdots ,n) λk=2Pkxk(k=1,2,3,⋯,n) 这样把求 λ k \lambda _k λk 的问题转化为求 P k P_k Pk 的问题。
因为根据 ( 2 ) (2) (2)式，控制量 u k u_k uk 直接和 λ k + 1 \lambda_{k+1} λk+1相关， λ k + 1 \lambda_{k+1} λk+1又和 P k + 1 P_{k+1} Pk+1 相关，因此只要求出 P k + 1 P_{k+1} Pk+1 来， 就能算出来 u k u_k uk，算出来之后 LQR 问题就迎刃而解了。
那么 P k P_k Pk 该怎么求呢？
由 ( 4 ) (4) (4)式直接可得到 P n = Q P_n=Q Pn=Q 。
重复上面的递推过程，经过计算得到这样的递推式：
P k − 1 = Q + A T P k ( I + B R − 1 B T P k ) − 1 A (9) P_{k-1}=Q+A^{T}P_{k}(I+BR^{-1}B^{T}P_{k})^{-1}A \tag{9} Pk−1=Q+ATPk(I+BR−1BTPk)−1A(9) 该递推式叫做 黎卡提方程(Riccati)。
5、控制量的表达式
有了递推式又知道 P n = Q P_n=Q Pn=Q ，就可以通过逆推出 P n − 1 P_{n-1} Pn−1， P n − 2 P_{n-2} Pn−2，一直这样递推下去，算出来 P k P_k Pk 之后就好办了
u k = − 1 2 R − 1 B T λ k + 1 = − 1 2 R − 1 B T ( 2 P k + 1 X k + 1 ) = − R − 1 B T P k + 1 ( A X k + B u 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} uk=−21R−1BTλk+1=−21R−1BT(2Pk+1Xk+1)=−R−1BTPk+1(AXk+Buk) 移项得到
u k = − ( R + B T P k + 1 B ) − 1 B T P k + 1 A X k u_k=-\left( R+B^TP_{k+1}B \right) ^{-1}B^TP_{k+1}AX_k uk=−(R+BTPk+1B)−1BTPk+1AXk
这样得到了控制量 u k u_k uk 的具体表达式，其中， R 、 B 、 A R、B、A R、B、A 都是已知量， P P P 是黎卡提方程算出来的，又和 X k X_k Xk 有关，但 X k X_k Xk 一般也是已知，因为 X k X_k Xk 一般取误差 e r r = X − X r e_{rr}=X-X_r err=X−Xr， X X X 就是当前车辆的状态， 通过定位得到的。 X r X_r Xr 是规划的状态和速度，通过规划得到。如果定位也有，规划也有，两者相减得到误差，误差再乘以矩阵就得到控制量。
当然这是非常笼统的说法，横向控制用到的误差要基于坐标变换变成 ( e d , e ˙ d , e φ , e ˙ φ ) (e_d,\dot e_d, e_\varphi, \dot e_\varphi) (ed,e˙d,eφ,e˙φ)，误差是 e d e_d ed 和 e φ e_\varphi eφ，可以通过规划和定位得到，所以控制的上游就是规划模块以及定位模块。
这两个如果都没有，那就没有办法做控制，所以控制一定是最后做的，就是当定位好了，规划也规划好了之后，才能去做控制。
六、黎卡提方程
还要讲一下黎卡提方程。
1、迭代性质
有人可能会觉得为什么叫方程呢？这不就是迭代的东西，就应该是迭代的递推的这样的式子，和方程有什么关系？为什么要把递推的式子叫做方程？
因为当考虑 ∑ k = 0 ∞ x T Q x + u T R u \sum_{k=0}^{\infty}x^{T}Qx+u^{T}Ru ∑k=0∞xTQx+uTRu ， n n n 趋无穷时，仍然选 P n = Q P_n=Q Pn=Q，要迭代无数次。
一般 ( 9 ) (9) (9)式黎卡提方程，迭代几十次后就收敛不再变化了，即 P k = P k − 1 = P k − 2 P_{k}=P_{k-1}=P_{k-2} Pk=Pk−1=Pk−2。
所以在控制量里的 P k + 1 P_{k+1} Pk+1 实际上是 P = Q + A T P ( I + B R − 1 B T P ) − 1 A P=Q+A^TP\left( I+BR^{-1}B^TP \right) ^{-1}A P=Q+ATP(I+BR−1BTP)−1A 的解。
2、LQR控制量的计算
最终的 LQR 控制首先是 P P P 先取初值 Q Q Q，带到黎卡提方程迭代，当 P P P 收敛后，代到 u u u 的表达式中：
u = − ( R + B T P B ) − 1 B T P A X k u=-(R+B^{T}PB)^{-1}B^{T}PAX_{k} u=−(R+BTPB)−1BTPAXk 令 K = ( R + B T P B ) − 1 B T P A K=(R+B^{T}PB)^{-1}B^{T}PA K=(R+BTPB)−1BTPA，可简化为：
u = − K X k u=-KX_{k} u=−KXk Matlab 里离散 LQR 的用法是
[ K , s , E ] = d l q r ( A , B , Q , R ) \left[ K,s,E \right] =dlqr\left( A,B,Q,R \right) [K,s,E]=dlqr(A,B,Q,R) 其中 K = ( R + B T P B ) − 1 B T P A K=(R+B^{T}PB)^{-1}B^{T}PA K=(R+BTPB)−1BTPA， s s s 是黎卡提方程的解 P P P， E E E 为特征值。
Matlab dlqr函数用法就是输入 A , B , Q , R A,B,Q,R A,B,Q,R，就会计算出 K , s , E K,s,E K,s,E。算出 K K K 以后， u u u 就等于 − K X -KX −KX，这样就可以得到想要的控制量。
3、其他书中的黎卡提方程形式
其他书上的黎卡提方程形式如下：
P = Q + A T P A − A T P B ( R + B T P B ) − 1 B T P A P=Q+A^TPA-A^TPB(R+B^TPB)^{-1}B^TPA P=Q+ATPA−ATPB(R+BTPB)−1BTPA 和推导出来的黎卡提方程是一样的。
4、矩阵求逆引理的应用
通过矩阵求逆引理化简：
( A + B C D ) − 1 = A − 1 − A − 1 B ( C − 1 + P A − 1 B ) − 1 D A − 1 (A+BCD)^{-1}=A^{-1}-A^{-1}B(C^{-1}+PA^{-1}B)^{-1}DA^{-1} (A+BCD)−1=A−1−A−1B(C−1+PA−1B)−1DA−1 把推导出来的黎卡提方程拿下来：
P k − 1 = Q + A T P k ( I + B R − 1 B T P k ) − 1 A P_{k-1}=Q+A^{T}P_{k}(I+BR^{-1}B^{T}P_{k})^{-1}A Pk−1=Q+ATPk(I+BR−1BTPk)−1A 对括号里的部分使用矩阵求逆引理，令
A = I B = B C = R − 1 D = B T P k A=I \quad B=B\quad C=R^{-1}\quad D=B^{T}P_{k} A=IB=BC=R−1D=BTPk 所以
( I + B R − 1 B T P k ) − 1 = I − B ( R + B T P B ) − 1 B T P (I+BR^{-1}B^TP_k)^{-1}=I-B(R+B^TPB)^{-1}B^TP (I+BR−1BTPk)−1=I−B(R+BTPB)−1BTP 代入原方程化简得到：
P = Q + A T P ( I − B ( R + B T P B ) − 1 B T P ) A P=Q+A^TP\left( I-B\left( R+B^TPB \right) ^{-1}B^TP \right) A P=Q+ATP(I−B(R+BTPB)−1BTP)A即 P = Q + A T P A − A T P B ( R + B T P B ) − 1 B T P A P=Q+A^TPA-A^TPB\left( R+B^TPB \right) ^{-1}B^TPA P=Q+ATPA−ATPB(R+BTPB)−1BTPA 推荐使用书上写的黎卡提方程，计算量小。
控制量 u = δ u=\delta u=δ， B B B 为 4 × 1 4\times 1 4×1矩阵， R R R 为 1 × 1 1\times 1 1×1矩阵， P P P 为 4 × 4 4\times 4 4×4矩阵。
注意：矩阵求逆是很容易出错误的运算，特别是在当矩阵性质不太好时，矩阵求逆非常容易发生错误，存在舍入误差，最后求逆会得到错误的逆矩阵。
七、LQR算法总结
首先写出误差导数
e ˙ r r = A e r r + B u \dot e_{rr}=Ae_{rr}+Bu e˙rr=Aerr+Bu
1、离散化
e r r ( k + 1 ) = A ˉ e r r ( k ) + B ˉ u ( k ) e_{rr}(k+1)=\bar{A}e_{rr}(k)+\bar{B}u(k) err(k+1)=Aˉerr(k)+Bˉu(k)
2、求解黎卡提方程
P = Q + A ˉ T P A ˉ − A ˉ T P B ˉ ( R + B ˉ T P B ˉ ) − 1 B ˉ T P A ˉ 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} P=Q+AˉTPAˉ−AˉTPBˉ(R+BˉTPBˉ)−1BˉTPAˉ
3、求解 K
K = ( R + B ˉ T P B ˉ − 1 ) B ˉ T P A ˉ K=\left( R+\bar{B}^TP\bar{B}^{-1} \right) \bar{B}^TP\bar{A} K=(R+BˉTPBˉ−1)BˉTPAˉ
4、求解控制量
u ( k ) = − K e r r ( k ) u(k)=-Ke_{rr}(k) u(k)=−Kerr(k)
本节博客把 LQR 原理基本上讲的很明白了。
但仍然不是特别完美，因为强行忽略了后面的小尾巴 C θ r C\theta_r Cθr，只考虑 e ˙ r r = A e r r + B u \dot e_{rr}=Ae_{rr}+Bu e˙rr=Aerr+Bu 的情况计算出了控制量。
下一节介绍考虑小尾巴 e ˙ r r = A e r r + B u + C θ ˙ r \dot e_{rr}=Ae_{rr}+Bu+C\dot{\theta}_r e˙rr=Aerr+Bu+Cθ˙r 的控制该怎么做，本篇博客就写这里，欢迎关注！
参考资料
【基础】自动驾驶控制算法第五讲 连续方程的离散化与离散LQR原理
后记：
🌟 感谢您耐心阅读这篇关于 连续方程离散化与离散LQR原理 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客 ，以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享，共同探索移动机器人 的无限可能！💡
🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀