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

2024-08-25 22:36

本文主要是介绍【自动驾驶】控制算法(五)连续方程离散化与离散LQR原理,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

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

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

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

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

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


文章目录

  • 引言
  • 一、连续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主 忠厚老实的老王 的视频,作为博主的学习笔记,分享给大家共同学习。

  在上一节将坐标变换和车辆动力学模型综合起来,得到了关于误差的微分方程:
在这里插入图片描述

  可以写成 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 xL=0,yL=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)=(IAdt)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)=(I2Adt)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+Butt+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 (I2Adt)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)=(I2Adt)1(I+2Adt)X(t)+(I2Adt)1Bu(t)dt  因为 d t dt dt 通常比较小 0.01 、 0.02 0.01、0.02 0.010.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)=(I2Adt)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ˉ=(I2Adt)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=0n1(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+Bu0x1=0Ax1+Bu1x2=0Axn1+Bun1xn=0  约束覆盖了从 x 0 x_0 x0 x n x_n xn 以及从 u 0 u_0 u0 u n − 1 u_{n-1} un1。发现写的代价函数 J J J 正好是 从 x 0 x_0 x0 x n x_n xn 以及从 u 0 u_0 u0 u n − 1 u_{n-1} un1的这样函数,即约束完全覆盖二次型的自变量。

  所以 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=0n1(xkTQxk+ukTRuk)+xnTQxn+k=0n1λk+1T(Axk+Bukxk+1)  这就是带约束的拉格朗日乘子法,注意 A x k + B u k − x k + 1 Ax_k+Bu_k-x_{k+1} Axk+Bukxk+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=0n1[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=0n1(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++Hn1+(λ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=0n1Hk+k=1n1(λkTxk)+(λnTxn)+xnTQxn+(λ0Tx0)(λ0Tx0)=k=0n1Hk+k=0n1(λkTxk)λnTxn+xnTQxn+λ0Tx0=k=0n1(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)=Ax(Ax)=ATx(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 x0J=0x0λ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} x1J=0x1H1x1(λTx1)=0λ1=x1H1 ∂ 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} x2J=0x2H2x2(λTx2)=0λ2=x2H2  发现偏导有相同的规律,可直接写成:
∂ 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} xkJ=0λk=xkHk(k=1,2,3,,n1)ukJ=0ukHk=0(k=1,2,3,,n1)xnJ=0xn(xnTQxnλnTxn)=0λkJ=0xk+1=Axk+Buk(k=1,2,3,...,n1)  上式中的 0 0 0 都是 0 0 0 向量。

  下面计算 ∂ H k ∂ x k \frac{\partial H_k}{\partial x_k} xkHk 以及 ∂ H k ∂ u k \frac{\partial H_k}{\partial u_k} ukHk。首先把 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} xkHk=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} ukHk=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=21R1BTλk+1xk+1=Axk+Bukλn=2QXn(k=1,2,,n1)(k=1,2,,n1)(k=1,2,,n1)(1)(2)(3)(4)  看起来好像是四个式子,但实际上是 3 n − 2 3n- 2 3n2 个式子。

  看一下这 3 n − 2 3n- 2 3n2 个式子,发现 ( 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=21R1BTλ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=n1) 式:
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} un1=21R1BT(2QXn)=R1BTQXn(5)  将 ( 5 ) (5) (5)代入 ( 3 ) (3) (3) ( k = n − 1 ) (k=n-1) (k=n1) 式:
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=Axn1+Bun1=Axn1+(BR1BTQxn)=(I+BR1BTQ)1Axn1(6)  将 ( 4 ) (4) (4)代入 ( 1 ) (1) (1) ( k = n − 1 ) (k=n-1) (k=n1) 式:
λ 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} λn1=2QXn1+ATλn=2QXn1+AT2QXn(7)  将 ( 6 ) (6) (6)代入 ( 7 ) (7) (7) ( k = n − 1 ) (k=n-1) (k=n1) 式:
λ 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} λn1=2QXn1+2ATQ(I+BR1BTQ)1AXn1=2(Q+ATQ(I+BR1BTQ)1)AXn1(8)  将 ( 4 ) (4) (4) ( 8 ) (8) (8)式比较,发现很像。

4、黎卡提方程的引入

  递推性质所决定的,从 λ n \lambda _n λn 推到 λ n − 1 \lambda _{n-1} λn1 λ n − 2 \lambda _{n-2} λn2,一定有相同形式,不妨设:
λ 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} Pk1=Q+ATPk(I+BR1BTPk)1A(9)  该递推式叫做 黎卡提方程(Riccati)

5、控制量的表达式

  有了递推式又知道 P n = Q P_n=Q Pn=Q ,就可以通过逆推出 P n − 1 P_{n-1} Pn1 P n − 2 P_{n-2} Pn2,一直这样递推下去,算出来 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=21R1BTλk+1=21R1BT(2Pk+1Xk+1)=R1BTPk+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 RBA 都是已知量, 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=XXr 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=0xTQx+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=Pk1=Pk2

  所以在控制量里的 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+BR1BTP)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+ATPAATPB(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=A1A1B(C1+PA1B)1DA1  把推导出来的黎卡提方程拿下来:
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 Pk1=Q+ATPk(I+BR1BTPk)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=R1D=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+BR1BTPk)1=IB(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(IB(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+ATPAATPB(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原理 的技术博客。 📚

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

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

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

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

这篇关于【自动驾驶】控制算法(五)连续方程离散化与离散LQR原理的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/1106844

相关文章

深入探索协同过滤:从原理到推荐模块案例

文章目录 前言一、协同过滤1. 基于用户的协同过滤(UserCF)2. 基于物品的协同过滤(ItemCF)3. 相似度计算方法 二、相似度计算方法1. 欧氏距离2. 皮尔逊相关系数3. 杰卡德相似系数4. 余弦相似度 三、推荐模块案例1.基于文章的协同过滤推荐功能2.基于用户的协同过滤推荐功能 前言     在信息过载的时代,推荐系统成为连接用户与内容的桥梁。本文聚焦于

poj2406(连续重复子串)

题意:判断串s是不是str^n,求str的最大长度。 解题思路:kmp可解,后缀数组的倍增算法超时。next[i]表示在第i位匹配失败后,自动跳转到next[i],所以1到next[n]这个串 等于 n-next[n]+1到n这个串。 代码如下; #include<iostream>#include<algorithm>#include<stdio.h>#include<math.

hdu4407(容斥原理)

题意:给一串数字1,2,......n,两个操作:1、修改第k个数字,2、查询区间[l,r]中与n互质的数之和。 解题思路:咱一看,像线段树,但是如果用线段树做,那么每个区间一定要记录所有的素因子,这样会超内存。然后我就做不来了。后来看了题解,原来是用容斥原理来做的。还记得这道题目吗?求区间[1,r]中与p互质的数的个数,如果不会的话就先去做那题吧。现在这题是求区间[l,r]中与n互质的数的和

【生成模型系列(初级)】嵌入(Embedding)方程——自然语言处理的数学灵魂【通俗理解】

【通俗理解】嵌入(Embedding)方程——自然语言处理的数学灵魂 关键词提炼 #嵌入方程 #自然语言处理 #词向量 #机器学习 #神经网络 #向量空间模型 #Siri #Google翻译 #AlexNet 第一节:嵌入方程的类比与核心概念【尽可能通俗】 嵌入方程可以被看作是自然语言处理中的“翻译机”,它将文本中的单词或短语转换成计算机能够理解的数学形式,即向量。 正如翻译机将一种语言

XTU 1233 n个硬币连续m个正面个数(dp)

题面: Coins Problem Description: Duoxida buys a bottle of MaiDong from a vending machine and the machine give her n coins back. She places them in a line randomly showing head face or tail face o

基于51单片机的自动转向修复系统的设计与实现

文章目录 前言资料获取设计介绍功能介绍设计清单具体实现截图参考文献设计获取 前言 💗博主介绍:✌全网粉丝10W+,CSDN特邀作者、博客专家、CSDN新星计划导师,一名热衷于单片机技术探索与分享的博主、专注于 精通51/STM32/MSP430/AVR等单片机设计 主要对象是咱们电子相关专业的大学生,希望您们都共创辉煌!✌💗 👇🏻 精彩专栏 推荐订阅👇🏻 单片机

hdu4407容斥原理

题意: 有一个元素为 1~n 的数列{An},有2种操作(1000次): 1、求某段区间 [a,b] 中与 p 互质的数的和。 2、将数列中某个位置元素的值改变。 import java.io.BufferedInputStream;import java.io.BufferedReader;import java.io.IOException;import java.io.Inpu

hdu4059容斥原理

求1-n中与n互质的数的4次方之和 import java.io.BufferedInputStream;import java.io.BufferedReader;import java.io.IOException;import java.io.InputStream;import java.io.InputStreamReader;import java.io.PrintWrit

Python3 BeautifulSoup爬虫 POJ自动提交

POJ 提交代码采用Base64加密方式 import http.cookiejarimport loggingimport urllib.parseimport urllib.requestimport base64from bs4 import BeautifulSoupfrom submitcode import SubmitCodeclass SubmitPoj():de

三相直流无刷电机(BLDC)控制算法实现:BLDC有感启动算法思路分析

一枚从事路径规划算法、运动控制算法、BLDC/FOC电机控制算法、工控、物联网工程师,爱吃土豆。如有需要技术交流或者需要方案帮助、需求:以下为联系方式—V 方案1:通过霍尔传感器IO中断触发换相 1.1 整体执行思路 霍尔传感器U、V、W三相通过IO+EXIT中断的方式进行霍尔传感器数据的读取。将IO口配置为上升沿+下降沿中断触发的方式。当霍尔传感器信号发生发生信号的变化就会触发中断在中断