当前位置：首页 > 行业知识 > 浅谈线性二次型调节器（LQR）算法（二）—— 离散系统及仿真代码

行业知识

浅谈线性二次型调节器（LQR）算法（二）—— 离散系统及仿真代码

2026-11-02 07:02:31 来源：互联网转载

传送门

前言
线性二次型控制器
动手实践
- 系统模型
- 仿真代码
- 运行结果
- 带输入的系统模型
- LQR控制器设计
- 控制器仿真
- 运行结果
- 结论
- - 优势
  - 劣势
  - 后��
    前言
    
    之前已经写过一篇关于LQR控制器的理解，但是在看了一些资料，重新思考复盘过，认为原先关于LQR的理解有一定的投机取巧成分。并且只阐述了连续系统下的相关公式推导，并没有对离散系统进行推导叙述。
    但为了能够体现出两次思考的差别，便没有将原有博客删除或直接修改，而是新起一篇博客进行重新描述整个理解和公式推导过程，最后会给出仿真过程，并将仿真文件上传分享。
    为了不做一些重复性赘述，默认读者掌握相关的基础知识
    
    线性二次型控制器
    - 状态空间方程
      X ⃗ ′ = A X ⃗ + B U ⃗ Y = C X ⃗ \vec{X}' = A\vec{X} + B\vec{U}\\ Y=C\vec{X} X ′=AX +BU Y=CX
      对于LQR来说，只需要讨论其状态转移方程，并且将连续系统转换成离散系统。对应的状态转移方程为
      X ⃗ k + 1 = A D X ⃗ k + B D U ⃗ k \vec{X}_{k+1} = A_{D}\vec{X}_{k}+B_{D}\vec{U}_{k} X k+1=ADX k+BDU k
      这里简单讨论一下 A A A和 A D A_{D} AD， B B B和 B D B_{D} BD之间的关系，由于离散系统还与离散周期有关系，因此需要假定离散周期为 Δ T \Delta T ΔT，根据连续状态转移方程
      X ⃗ ′ = X ⃗ k + 1 − X ⃗ k Δ T = A X ⃗ k + B U ⃗ k X ⃗ k + 1 − X ⃗ k = Δ T A X ⃗ k + Δ T B U ⃗ k X ⃗ k + 1 = ( E + Δ T A ) X ⃗ k + Δ T B U ⃗ k \vec{X}' = \frac{\vec{X}_{k+1} - \vec{X}_{k}}{\Delta T}=A\vec{X}_{k} + B\vec{U}_{k}\\ \vec{X}_{k+1} - \vec{X}_{k}=\Delta T A\vec{X}_{k}+\Delta T B\vec{U}_{k}\\ \vec{X}_{k+1} = (E+\Delta TA)\vec{X}_{k}+\Delta TB\vec{U}_{k} X ′=ΔTX k+1−X k=AX k+BU kX k+1−X k=ΔTAX k+ΔTBU kX k+1=(E+ΔTA)X k+ΔTBU k
      即可以得到
      A D = E + Δ T A B D = Δ T B A_{D} = E+\Delta T A \\ B_{D}=\Delta T B AD=E+ΔTABD=ΔTB
      - 代价函数（Cost Function）
        代价函数是用来描述LQR控制倾向的函数，通过调节权重矩阵来修改控制特性。另外，代价函数用于计算控制律，其中的权重矩阵最终都会影响控制律，进而影响控制效果。
        代价函数的形式为
        J = 1 2 X ⃗ ( t f ) T F X ⃗ ( t f ) + 1 2 ∫ t 0 t f ( X ⃗ T Q X ⃗ + U ⃗ T R U ⃗ ) d t J = \frac{1}{2}\vec{X}(t_f)^{T}F\vec{X}(t_f)+\frac{1}{2}\int_{t_0}^{t_f}{(\vec{X}^{T}Q\vec{X} + \vec{U}^{T}R\vec{U})dt} J=21X (tf)TFX (tf)+21∫t0tf(X TQX +U TRU )dt
        其中， t f t_f tf为目标时刻， t 0 t_0 t0为初始时刻。矩阵 F F F为末端代价权重矩阵，用于描述末端状态向量的权重大小。矩阵 Q Q Q和矩阵 R R R分别状态向量 X ⃗ \vec{X} X 和系统输入 U ⃗ \vec{U} U 在过程代价中的权重。
        由于是对离散系统进行LQR控制，因此也需要对代价函数进行离散化描述，可以得到
        J = 1 2 X ⃗ ( N ) T F X ⃗ ( N ) + 1 2 ∑ k = 0 N − 1 ( X ⃗ ( k ) T Q X ⃗ ( k ) + U ⃗ ( k ) T R U ⃗ ( k ) ) J = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N)+\frac{1}{2}\sum_{k=0}^{N-1}{ (\vec{X}(k)^{T}Q\vec{X}(k) + \vec{U}(k)^{T}R\vec{U}(k)) } J=21X (N)TFX (N)+21k=0∑N−1(X (k)TQX (k)+U (k)TRU (k))
        这里我想详细阐述一些关于LQR特性的内容。根据这个离散代价函数，需要思考几个问题
        
        J J J的含义
        代价函数 J J J代表了从 0 0 0时刻到 N N N时刻，这个过程中状态向量以及输入的累积值。
        J J J的最优化代表什么
        从代价函数的形式可以看出，其都是半正定矩阵，即 J ≥ 0 J \geq 0 J≥0。因此对 J J J求最优化，会使得状态向量向零向量收敛。
        J J J的最优化结果是什么
        用 J J J对输入 U ⃗ \vec{U} U 进行求导，可以得到从一个输入向量的时间序列 { U 0 ⃗ , U 1 ⃗ , U 2 ⃗ . . . , U N − 1 ⃗ } \{\vec{U_0},\vec{U_1},\vec{U_2}...,\vec{U_N-1} \} {U0 ,U1 ,U2 ...,UN−1 }。但是这显然不好求，太多自变量了，求偏导都得求到头大，因此需要使用一些数学方法来巧妙地实现最优求解。
        递归特性
        当 k = N k=N k=N时
        J k = N = 1 2 X ⃗ ( N ) T F X ⃗ ( N ) J_{k=N} = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N) Jk=N=21X (N)TFX (N)
        很明显，最后一个时刻的代价函数值就是末端代价，末端代价即为N时刻代价函数的最优值 J k = N ∗ J^{*}_{k=N} Jk=N∗。
        当 k = N − 1 k=N-1 k=N−1时
        J k = N − 1 = 1 2 X ⃗ ( N ) T F X ⃗ ( N ) + 1 2 ( X ⃗ ( N − 1 ) T Q X ⃗ ( N − 1 ) + U ⃗ ( N − 1 ) T R U ⃗ ( N − 1 ) ) = J k = N + 1 2 ( X ⃗ ( N − 1 ) T Q X ⃗ ( N − 1 ) + U ⃗ ( N − 1 ) T R U ⃗ ( N − 1 ) ) J_{k=N-1} = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N) + \ \frac{1}{2}(\vec{X}(N-1)^{T}Q\vec{X}(N-1) + \vec{U}(N-1)^{T}R\vec{U}(N-1)) \\ =J_{k=N}+ \frac{1}{2}(\vec{X}(N-1)^{T}Q\vec{X}(N-1) + \vec{U}(N-1)^{T}R\vec{U}(N-1)) Jk=N−1=21X (N)TFX (N)+ 21(X (N−1)TQX (N−1)+U (N−1)TRU (N−1))=Jk=N+21(X (N−1)TQX (N−1)+U (N−1)TRU (N−1))
        当 k = N − 2 k=N-2 k=N−2时
        J k = N − 2 = 1 2 X ⃗ ( N ) T F X ⃗ ( N ) + 1 2 ( X ⃗ ( N − 1 ) T Q X ⃗ ( N − 1 ) + U ⃗ ( N − 1 ) T R U ⃗ ( N − 1 ) ) + 1 2 ( X ⃗ ( N − 2 ) T Q X ⃗ ( N − 2 ) + U ⃗ ( N − 2 ) T R U ⃗ ( N − 2 ) ) = J k = N − 1 + 1 2 ( X ⃗ ( N − 2 ) T Q X ⃗ ( N − 2 ) + U ⃗ ( N − 2 ) T R U ⃗ ( N − 2 ) ) J_{k=N-2} = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N) + \ \frac{1}{2}(\vec{X}(N-1)^{T}Q\vec{X}(N-1) + \vec{U}(N-1)^{T}R\vec{U}(N-1)) \\ +\frac{1}{2}(\vec{X}(N-2)^{T}Q\vec{X}(N-2) + \vec{U}(N-2)^{T}R\vec{U}(N-2))\\ =J_{k=N-1}+\frac{1}{2}(\vec{X}(N-2)^{T}Q\vec{X}(N-2) + \vec{U}(N-2)^{T}R\vec{U}(N-2)) Jk=N−2=21X (N)TFX (N)+ 21(X (N−1)TQX (N−1)+U (N−1)TRU (N−1))+21(X (N−2)TQX (N−2)+U (N−2)TRU (N−2))=Jk=N−1+21(X (N−2)TQX (N−2)+U (N−2)TRU (N−2))
        很明显，不同时刻间的代价函数包含了下一时刻的代价函数，体现了非常强烈的递归特性。
        根据贝尔曼最优理论，如果 k k k时刻的代价函数 J k J_{k} Jk是最优的，那么他包含的 J k + 1 J_{k+1} Jk+1一定是最优的。
        从 k = N k=N k=N时刻开始，计算 N N N时刻的最优代价函数
        J k = N ∗ = J k = N = 1 2 X ⃗ ( N ) T F X ⃗ ( N ) J^{*}_{k=N} = J_{k=N} = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N) Jk=N∗=Jk=N=21X (N)TFX (N)
        当 k = N − 1 k=N-1 k=N−1时，
        J k = N − 1 = J k = N + 1 2 ( X ⃗ ( N − 1 ) T Q X ⃗ ( N − 1 ) + U ⃗ ( N − 1 ) T R U ⃗ ( N − 1 ) ) = 1 2 X ⃗ ( N ) T F X ⃗ ( N ) + 1 2 ( X ⃗ ( N − 1 ) T Q X ⃗ ( N − 1 ) + U ⃗ ( N − 1 ) T R U ⃗ ( N − 1 ) ) J_{k=N-1} =J_{k=N}+\frac{1}{2} { (\vec{X}(N-1)^{T}Q\vec{X}(N-1) + \vec{U}(N-1)^{T}R\vec{U}(N-1)) }\\ =\frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N)+\frac{1}{2} { (\vec{X}(N-1)^{T}Q\vec{X}(N-1) + \vec{U}(N-1)^{T}R\vec{U}(N-1)) } Jk=N−1=Jk=N+21(X (N−1)TQX (N−1)+U (N−1)TRU (N−1))=21X (N)TFX (N)+21(X (N−1)TQX (N−1)+U (N−1)TRU (N−1))
        又有
        X ⃗ ( N ) = A X ⃗ ( N − 1 ) + B U ⃗ ( N − 1 ) \vec{X}(N)=A\vec{X}(N-1)+B\vec{U}(N-1) X (N)=AX (N−1)+BU (N−1)
        对 U ⃗ ( N − 1 ) \vec{U}(N-1) U (N−1)求导
        ∂ J k = N − 1 ∂ U ⃗ ( N − 1 ) = ∂ X ⃗ ( N ) ∂ U ⃗ ( N − 1 ) ⋅ ∂ 1 2 X ⃗ ( N ) T F X ⃗ ( N ) ∂ X ⃗ ( N ) + ∂ 1 2 U ⃗ ( N − 1 ) T R U ⃗ ( N − 1 ) ) ∂ U ⃗ ( N − 1 ) \frac{\partial J_{k=N-1}}{\partial \vec{U}(N-1)} \ =\frac{\partial \vec{X}(N)}{\partial \vec{U}(N-1)} \cdot \frac{\partial \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N)}{\partial \vec{X}(N)}\ +\frac{\partial \frac{1}{2} \vec{U}(N-1)^{T}R\vec{U}(N-1)) }{\partial \vec{U}(N-1)} ∂U (N−1)∂Jk=N−1 =∂U (N−1)∂X (N)⋅∂X (N)∂21X (N)TFX (N) +∂U (N−1)∂21U (N−1)TRU (N−1))
        经过矩阵求导得到
        ∂ J k = N − 1 ∂ U ⃗ ( N − 1 ) = B T ⋅ F ⋅ ( A X ⃗ ( N − 1 ) + B U ⃗ ( N − 1 ) ) + R U ⃗ ( N − 1 ) \frac{\partial J_{k=N-1}}{\partial \vec{U}(N-1)} =\ B^{T}\cdot F \cdot (A\vec{X}(N-1)+B\vec{U}(N-1))+R\vec{U}(N-1) ∂U (N−1)∂Jk=N−1= BT⋅F⋅(AX (N−1)+BU (N−1))+RU (N−1)
        令其求导为0，求极值
        B T ⋅ F ⋅ ( A X ⃗ ( N − 1 ) + B U ⃗ ( N − 1 ) ) + R U ⃗ ( N − 1 ) = 0 B^{T}\cdot F \cdot (A\vec{X}(N-1)+B\vec{U}(N-1))+R\vec{U}(N-1)=0 BT⋅F⋅(AX (N−1)+BU (N−1))+RU (N−1)=0
        可以得到
        − B T F A ⋅ X ⃗ ( N − 1 ) = ( B T F B + R ) ⋅ U ⃗ ( N − 1 ) -B^{T}FA \cdot \vec{X}(N-1)=(B^{T}FB+R) \cdot \vec{U}(N-1) −BTFA⋅X (N−1)=(BTFB+R)⋅U (N−1)
        解得
        U ⃗ ( N − 1 ) = − ( B T F B + R ) − 1 B T F A ⋅ X ⃗ ( N − 1 ) = − K ( N − 1 ) ⋅ X ⃗ ( N − 1 ) \vec{U}(N-1) = -(B^{T}FB+R) ^{-1} B^{T}FA \cdot \vec{X}(N-1) \ =-K(N-1) \cdot \vec{X}(N-1) U (N−1)=−(BTFB+R)−1BTFA⋅X (N−1) =−K(N−1)⋅X (N−1)
        这里令 K = ( B T F B + R ) − 1 B T F A K = (B^{T}FB+R) ^{-1} B^{T}FA K=(BTFB+R)−1BTFA，可以得到全状态反馈的控制律，即 U ⃗ = − K X ⃗ \vec{U} = -K \vec{X} U =−KX
        矩阵运算之前在卡尔曼滤波器里讲过，有兴趣可以翻看，不重复赘述。卡尔曼滤波器
        为了验证其是否为极小值点，二次求导。
        ∂ 2 J k = N − 1 ∂ U ⃗ 2 ( N − 1 ) = B T F B + R \frac{\partial^{2} J_{k=N-1}}{\partial \vec{U}^{2}(N-1)} =\ B^{T} FB +R ∂U 2(N−1)∂2Jk=N−1= BTFB+R
        显然，由于矩阵 R R R正定， B T F B B^{T}FB BTFB为半正定矩阵，因此
        ∂ 2 J k = N − 1 ∂ U ⃗ 2 ( N − 1 ) > 0 \frac{\partial^{2} J_{k=N-1}}{\partial \vec{U}^{2}(N-1)} >0 ∂U 2(N−1)∂2Jk=N−1>0
        因此该极值点为极小值点。
        将极值点 U ⃗ ( N − 1 ) = − ( B T F B + R ) − 1 B T F A ⋅ X ⃗ ( N − 1 ) \vec{U}(N-1)=-(B^{T}FB+R) ^{-1} B^{T}FA \cdot \vec{X}(N-1) U (N−1)=−(BTFB+R)−1BTFA⋅X (N−1)代入，得到 N − 1 N-1 N−1时刻的最优代价函数
        J ∗ ( N − 1 ) = X ⃗ T ( N − 1 ) ( [ A − B K ( N − 1 ) ] T ⋅ F ⋅ [ A − B K ( N − 1 ) ] + K ( N − 1 ) T R K ( N − 1 ) + Q ) X ⃗ ( N − 1 ) J^{*}(N-1) = \vec{X}^{T}(N-1) \ ( [A-BK(N-1)]^{T} \cdot F \cdot [A-BK(N-1)] + K(N-1)^{T} R K(N-1) + Q) \ \vec{X}(N-1) J∗(N−1)=X T(N−1) ([A−BK(N−1)]T⋅F⋅[A−BK(N−1)]+K(N−1)TRK(N−1)+Q) X (N−1)
        可以看出无论是 J ∗ ( N ) J^{*}(N) J∗(N) 还是 J ∗ ( N − 1 ) J^{*}(N-1) J∗(N−1) 都是以 X ⃗ T P X ⃗ \vec{X}^{T}P\vec{X} X TPX 的形式成立。
        因此总结其规律，令
        P ( 0 ) = F P ( 1 ) = ( [ A − B K ( N − 1 ) ] T ⋅ P ( 0 ) ⋅ [ A − B K ( N − 1 ) ] + K ( N − 1 ) T R K ( N − 1 ) + Q ) P(0) = F\\ P(1) = ( [A-BK(N-1)]^{T} \cdot P(0) \cdot [A-BK(N-1)] + K(N-1)^{T} R K(N-1) + Q) P(0)=FP(1)=([A−BK(N−1)]T⋅P(0)⋅[A−BK(N−1)]+K(N−1)TRK(N−1)+Q)
        则可以得到增益矩阵( N − 1 N-1 N−1时刻)
        K ( N − 1 ) = ( B T P ( 0 ) B + R ) − 1 B T P ( 0 ) A K(N-1) = (B^{T}P(0)B+R) ^{-1} B^{T}P(0)A K(N−1)=(BTP(0)B+R)−1BTP(0)A
        
        总结一下
        当 k = N k=N k=N时刻
        J ∗ ( N ) = X ⃗ T ( N ) P ( 0 ) X ⃗ ( N ) J^{*}(N) = \vec{X}^{T}(N) P(0) \vec{X}(N) J∗(N)=X T(N)P(0)X (N)
        当 k = N − 1 k=N-1 k=N−1时刻
        K ( N − 1 ) = ( B T P ( 0 ) B + R ) − 1 B T P ( 0 ) A P ( 1 ) = ( [ A − B K ( N − 1 ) ] T ⋅ P ( 0 ) ⋅ [ A − B K ( N − 1 ) ] + K ( N − 1 ) T R K ( N − 1 ) + Q ) J ∗ ( N − 1 ) = X ⃗ T ( N − 1 ) P ( 1 ) X ⃗ ( N − 1 ) K(N-1) = (B^{T}P(0)B+R) ^{-1} B^{T}P(0)A\\ P(1) = ( [A-BK(N-1)]^{T} \cdot P(0) \cdot [A-BK(N-1)] + K(N-1)^{T} R K(N-1) + Q) \\ J^{*}(N-1) = \vec{X}^{T}(N-1) P(1) \vec{X}(N-1) K(N−1)=(BTP(0)B+R)−1BTP(0)AP(1)=([A−BK(N−1)]T⋅P(0)⋅[A−BK(N−1)]+K(N−1)TRK(N−1)+Q)J∗(N−1)=X T(N−1)P(1)X (N−1)
        根据其递归特性，可以得到通用一般形式(当 k ≥ 1 k\ge1 k≥1)
        K ( N − k ) = ( B T P ( k − 1 ) B + R ) − 1 B T P ( k − 1 ) A P ( k ) = ( [ A − B K ( N − k ) ] T ⋅ P ( k − 1 ) ⋅ [ A − B K ( N − k ) ] + K ( N − k ) T R K ( N − k ) + Q ) J ∗ ( N − k ) = X ⃗ T ( N − k ) P ( k ) X ⃗ ( N − k ) K(N-k) = (B^{T}P(k-1)B+R) ^{-1} B^{T}P(k-1)A\\ P(k) = ( [A-BK(N-k)]^{T} \cdot P(k-1) \cdot [A-BK(N-k)] + K(N-k)^{T} R K(N-k) + Q) \\ J^{*}(N-k) = \vec{X}^{T}(N-k) P(k) \vec{X}(N-k) K(N−k)=(BTP(k−1)B+R)−1BTP(k−1)AP(k)=([A−BK(N−k)]T⋅P(k−1)⋅[A−BK(N−k)]+K(N−k)TRK(N−k)+Q)J∗(N−k)=X T(N−k)P(k)X (N−k)
        动手实践
        
        系统模型
        
        以弹簧阻尼系统为例，设质量块质量为m，以静力平衡点为位移0点，其受力情况为：
        
        这里列出受力情况，主要是向一些新入门的同学展示一些状态空间方程是如何来的，已知可跳过。
        m a = − k x − c v ma = -kx-cv ma=−kx−cv
        其中， a a a为质量块的加速度， v v v为质量块的速度， x x x为质量块的位移， k k k为弹簧系数， c c c为阻尼系数。
        接下来，建立状态空间方程，设状态向量为 X ⃗ \vec{X} X 为
        X ⃗ = [ x 1 x 2 ] = [ x v ] \vec{X} =\begin{bmatrix} x_1\\x_2\end{bmatrix} = \begin{bmatrix} x\\v\end{bmatrix} X =[x1x2]=[xv]
        则状态转移方程为
        X ′ ⃗ = A X ⃗ → [ x 1 ′ x 2 ′ ] = [ x ′ v ′ ] = [ v a ] = [ v − k m x − c m v ] = [ x 2 − k m x 1 − c m x 2 ] = [ 0 1 − k m − c m ] [ x 1 x 2 ] \vec{X'}=A\vec{X}\\ \rightarrow \begin{bmatrix} x_{1}'\\x_{2}'\end{bmatrix}=\begin{bmatrix} x'\\v'\end{bmatrix}=\begin{bmatrix} v\\a\end{bmatrix} =\begin{bmatrix} v\\-\frac{k}{m}x-\frac{c}{m}v\end{bmatrix}=\begin{bmatrix} x_2\\-\frac{k}{m}x_{1}-\frac{c}{m}x_{2}\end{bmatrix}=\begin{bmatrix} 0 & 1\\-\frac{k}{m}&-\frac{c}{m}\end{bmatrix}\begin{bmatrix} x_1\\x_2\end{bmatrix} X′ =AX →[x1′x2′]=[x′v′]=[va]=[v−mkx−mcv]=[x2−mkx1−mcx2]=[0−mk1−mc][x1x2]
        为了在matlab中将其运动过程仿真出来，将其转换成离散方程，设采样时间为 T T T，则可以得到
        [ x 1 ( k + 1 ) x 2 ( k + 1 ) ] = [ 1 T − k T m 1 − c T m ] [ x 1 ( k ) x 2 ( k ) ] \begin{bmatrix} x_{1}(k+1)\\x_{2}(k+1)\end{bmatrix}=\begin{bmatrix} 1&T\\-\frac{kT}{m}&1-\frac{cT}{m}\end{bmatrix}\begin{bmatrix} x_{1}(k)\\x_{2}(k)\end{bmatrix} [x1(k+1)x2(k+1)]=[1−mkTT1−mcT][x1(k)x2(k)]
        
        仿真代码
        
        T = 0.001;%离散周期位1msm = 1;%重量块质量为1kgc = 0.2;k = 0.5;%阻尼系数和弹簧系数A = [1 T;-k*T/m 1-c*T/m];B = [0;1/m];%系统状态空间方程x0 = [4;0];%系统初始状态向量n = 100000; %仿真的步数x = zeros(n,1);v = zeros(n,1);time = zeros(n,1);%记录状态数据，用来绘图的xk_1 = [0;0];xk = x0;%x(k)以及x(k+1)for i = 1:n xk_1 = A*xk; x(i) = xk(1); v(i) = xk(2); time(i) = i*T; xk = xk_1;endplot(time, x,'r-',time,v,'b-') % 绘制曲线xlabel('x') % 添加x轴标签ylabel('y') % 添加y轴标签title('lqr') % 添加标题legend('x','v');grid on % 添加网格线
        
        运行结果
        
        可以看出，这是一个无论初始状态如何，最终都能够振荡收敛的系统。此时在这个系统里加入一个力 F ⃗ \vec{F} F ，作为系统输入 u u u，根据 L Q R LQR LQR计算输入序列，来使得系统状态更快速收敛为0。
        
        带输入的系统模型
        
        带输入的系统模型的状态空间方程为
        [ x 1 ( k + 1 ) x 2 ( k + 1 ) ] = [ 1 T − k T m 1 − c T m ] [ x 1 ( k ) x 2 ( k ) ] + [ 0 T m ] u \begin{bmatrix} x_{1}(k+1)\\x_{2}(k+1)\end{bmatrix}=\begin{bmatrix} 1&T\\-\frac{kT}{m}&1-\frac{cT}{m}\end{bmatrix}\begin{bmatrix} x_{1}(k)\\x_{2}(k)\end{bmatrix}+\begin{bmatrix} 0\\\frac{T}{m}\end{bmatrix}u [x1(k+1)x2(k+1)]=[1−mkTT1−mcT][x1(k)x2(k)]+[0mT]u
        
        LQR控制器设计
        
        定义代价函数 J J J
        J = 1 2 X ⃗ ( N ) T P ( 0 ) X ⃗ ( N ) + 1 2 ∑ k = 0 N − 1 ( X ⃗ ( k ) T Q X ⃗ ( k ) + U ⃗ ( k ) T R U ⃗ ( k ) ) J=\frac{1}{2} \vec{X}(N)^{T}P(0)\vec{X}(N)+\frac{1}{2}\sum_{k=0}^{N-1}{ (\vec{X}(k)^{T}Q\vec{X}(k) + \vec{U}(k)^{T}R\vec{U}(k)) } J=21X (N)TP(0)X (N)+21k=0∑N−1(X (k)TQX (k)+U (k)TRU (k))
        其中，矩阵 P ( 0 ) , Q , R P(0),Q, R P(0),Q,R分别是根据控制需求定义的末端代价权重矩阵，过程状态代价矩阵，过程输入代价矩阵。
        计算全状态矩阵 K K K
        根据上面的结论可以得到，递推求解公式为
        K ( N − k ) = ( B T P ( k − 1 ) B + R ) − 1 B T P ( k − 1 ) A P ( k ) = ( [ A − B K ( N − k ) ] T ⋅ P ( k − 1 ) ⋅ [ A − B K ( N − k ) ] + K ( N − k ) T R K ( N − k ) + Q ) K(N-k) = (B^{T}P(k-1)B+R) ^{-1} B^{T}P(k-1)A\\ P(k) = ( [A-BK(N-k)]^{T} \cdot P(k-1) \cdot [A-BK(N-k)] + K(N-k)^{T} R K(N-k) + Q) K(N−k)=(BTP(k−1)B+R)−1BTP(k−1)AP(k)=([A−BK(N−k)]T⋅P(k−1)⋅[A−BK(N−k)]+K(N−k)TRK(N−k)+Q)
        其中，
        A = [ 1 T − k T m 1 − c T m ] , B = [ 0 T m ] A=\begin{bmatrix} 1&T\\-\frac{kT}{m}&1-\frac{cT}{m}\end{bmatrix},B=\begin{bmatrix} 0\\\frac{T}{m}\end{bmatrix} A=[1−mkTT1−mcT],B=[0mT]
        控制器仿真
        
        clear all;lqr_flag = 1;T = 0.001;%离散周期位1msm = 1;%重量块质量为1kgc = 0.2;k = 0.5;%阻尼系数和弹簧系数A = [1 T;-k*T/m 1-c*T/m];B = [0;T/m];%系统状态空间方程x0 = [4;0];%系统初始状态向量n = 100000;x = zeros(n,1);v = zeros(n,1);time = zeros(n,1);u = zeros(n,1);%记录状态数据，用来绘图的xk_1 = [0;0];xk = x0;%状态向量xk和xk+1P=zeros(n,4);%P迭代矩阵，用于计算KP0 = [1 0;0 1];%末端状态代价矩阵2x2Q = [1 0;0 1];%过程状态代价矩阵2x2R = 1;%过程输入代价矩阵1x1K = zeros(n,2);%全状态反馈矩阵 1x2P(1,:) = P0(:)';%初始化for i = 2:n tmpP = reshape(P(i-1,:),2,2); tmpK = reshape(K(n-i+1,:),1,2); K(n-i+1,:) = reshape( (B'*tmpP*B+R)\B'*tmpP*A,1,2); P(i,:)= reshape( (A-B * tmpK)'* tmpP *(A-B * tmpK) + tmpK'*R*tmpK+Q ,1,4);end%从最后一个往前算P(k)if lqr_flag == 0 %无输入系统 for i = 1:n xk_1 = A*xk; x(i) = xk(1); v(i) = xk(2); time(i) = i*T; xk = xk_1; endelse % lqr控制输入下的系统 for i = 1:n uk = - reshape(K(i,:),1,2)*xk; xk_1 = A*xk + B*uk; x(i) = xk_1(1); v(i) = xk_1(2); time(i) = i*T; u(i) = uk; xk = xk_1; end endplot(time, x,'r-',time,v,'g-',time,u,'b-') % 绘制曲线xlabel('x') % 添加x轴标签ylabel('y') % 添加y轴标签title('lqr') % 添加标题legend('x','v','u');grid on % 添加网格线
        
        运行结果
        
        结论
        
        L Q R LQR LQR控制器是一种通过代价函数最优求解控制序列的算法，以全状态反馈的控制律形式实现控制。并且其反馈矩阵的计算只与状态方程矩阵，代价函数的三个矩阵有关系。
        
        参数调试的几种基本原则
        若想快速收敛就将矩阵 Q Q Q中对应的状态权重调大；
        若想减小系统冲激则提高矩阵 R R R的值；
        建议多动手尝试，调试不同位置参数来观察控制效果。
        优势
        
        计算简单，并且可离线计算，即已经事先规划好了，无论你的初始状态如何。
        可以根据控制需求调节参数，以达到不同的控制效果，使用较为灵活。
        劣势
        
        只适用于线性系统。
        不引入当前过去状态来修正控制律增益，抗扰动能力较差。
        即当前系统受扰动出现较大的状态偏移后无法做出及时应对和修正
        后续
        
        当你看到这里，不知道你有没有想过，使用 L Q R LQR LQR就只能让你的系统状态量归0吗，如果我不希望他不归0，保持为其他的某个常数，亦或者是以某种轨迹进行变化，又应该如何使用 L Q R LQR LQR来实现呢。这类问题就是轨迹跟踪的问题。这个内容就是我下一篇博客要讨论的东西，敬请期待。