LQR的理解与运用 第一期——理解篇
目录标题
- 0.本系列目的
-
- 理解
- 运用
-
- 一阶倒立摆
-
- matlab+simscape multibody实现
- urdf模型+drake实现
- 简单的轮足模型（二阶倒立摆）
- 1 理解LQR
-
- Q1：LQR解决了什么问题
- Q2:LQR的适用场景与形式
- Q3:LQR的解决思路
-
- 3.1 LQR控制器设计步骤：
- 3.2 Matlab中应用
- Q4:LQR中做出的...假设？
-
- K的推导
- 怎么理解u=Kx
- 稳定性分析
- 可控性分析
- 参考文档
0.本系列目的
理解与运用LQR
理解
解释什么是LQR，并梳理LQR求解过程。
大家可以思考这么几个问题：
- LQR的适用场景是（使用条件）？
- LQR解决了哪些问题（什么是LQR）?
- LQR的变量是什么，输入是什么，输出是什么？
- LQR是怎么解决这些问题的？
- LQR中有没有为了优化问题设置的假设条件？
- 可能需要一点自控理论知识，不过对聪明人来说，学过矩阵乘法就行^ ^
如果看不懂就看下面的参考文章，保证好懂
本文部分片段搬运自以下文章，并根据学校课件和个人理解进行修改，侵删！
(个人感觉最好✨→) 线性二次型调节器(LQR)原理详解
https://blog.csdn.net/qq_36133747/article/details/123413115
LQR最优控制方法小结
https://zhuanlan.zhihu.com/p/363033191
RoboMaster平衡步兵机器人控制系统设计
https://zhuanlan.zhihu.com/p/563048952
运用
一阶倒立摆
matlab+simscape multibody实现
- 对一阶倒立摆模型进行建模，设计LQR参数
- 在
matlab上，实现模型的框图设计 - 通过
simscape multibody将这一过程可视化
urdf模型+drake实现
待续（很远的坑）
简单的轮足模型（二阶倒立摆）
参考RoboMaster平衡步兵机器人控制系统设计
构造以下模型
1 理解LQR
Q1：LQR解决了什么问题
本文讨论LQR基本原理时，被控对象都是线性定常系统，系统状态不随时间变化。对于一个状态空间：
x ˙ = A x + B u y = C x + D u (1) \begin{array}{l} \dot{x}=A x+B u \\ y=C x+D u \end{array}\tag{1} x˙=Ax+Buy=Cx+Du(1)
我们设定一个线性反馈控制器 u = − K x u=-K x u=−Kx,用以得到输入参数 u u u与状态变量 x x x的关系（求解矩阵 K K K的方法会在后面提到），此时第一行可以写为
x ˙ = A x − B K x = ( A − B K ) ⏟ A c l x (2) \dot{x}=A x-B K x=\underbrace{(A-B K)}_{A_{c l}} x\tag{2} x˙=Ax−BKx=Acl (A−BK)x(2)
让系统稳定的条件是矩阵 A c l A_{cl} Acl 的特征值的实部均为负数（？），我们当然可以手动选择几个满足上述条件的特征值，然后反解 K K K，从而得到控制器。
而LQR的出现，就是为了让几个参数的选择更为合理，从而使得控制器控制效果更好。其实现方式正是通过设计代价函数 J J J实现的。
本文讨论无限时间的LQR问题（有限时间的LQR问题属于状态时变的问题，这里暂时不考虑），无限时间的LQR问题设计的成本代价泛函 J J J为：
J = ∫ 0 ∞ ( x T Q x + u T R u ) d t , Q = Q T , R = R T , Q ≥ 0 , R > 0 (3) J=\int_{0}^{\infty}\left(x^{T} Q x+u^{T} R u\right) d t, Q=Q^{T}, R=R^{T}, Q \geq 0, R>0\tag{3} J=∫0∞(xTQx+uTRu)dt,Q=QT,R=RT,Q≥0,R>0(3)
一般来说，Q阵和R阵为对角阵，分别确定了状态变量 x x x和输入参数 u u u的权重。对角阵上的值越大说明我们设计时对于该量的重视程度越大，即希望这个量在变化过程中保持较小的值，换种说法就是对于该量的“惩罚”越大。我们的设计目标就是得到一系列的控制序列使代价累积的最小。
代价函数的解释，举例：
图片内容及部分文字来自线性二次型调节器(LQR)原理详解
因此，问题转变为了选择合适的 K K K使得代价函数 J J J最小。(见Q3)
Q2:LQR的适用场景与形式
- LQR是一个多输入，多输出（MIMO）的调节器(仅适用于线性系统)；其目的是在给定代价为二次的代价方程中找出最小代价。
- 在多个输入参数中，可分别设置不同权重。
此外，由于LQR为状态方程形式，因此输入参数为n*1的形式，而权重矩阵（下文 Q , R Q,R Q,R）往往是n*n的对角矩阵，即除了主对角线均为0；故为了表现加权、二次、多输入，代价函数往往为以下形式：( x x x, u u u的位置见上文状态空间表达形式)
J = x T ( t ) Q x ( t ) + u T ( t ) R u ( t ) J = x^{T}(t) Q x(t)+u^{T}(t) R u(t) J=xT(t)Qx(t)+uT(t)Ru(t)
Q3:LQR的解决思路
接Q1，下一步是选择合适的 K K K使得代价函数 J J J最小。
根据式 ( 3 ) (3) (3),我们另定义一个辅助常量矩阵 P P P，
d d t x T P x = − ( x T Q x + u T R u ) (4) \frac{\mathrm{d}}{\mathrm{d} t} x^{T} P x=-\left(x^{T} Q x+u^{T} R u\right)\tag{4} dtdxTPx=−(xTQx+uTRu)(4)
(?) P P P是对称矩阵， P = P T > 0 P=P^T>0 P=PT>0
-
P P P的作用
将 ( 4 ) (4) (4)带入 ( 3 ) (3) (3)有
J = − ∫ 0 ∞ d d t x T P x d t = − ( x T P x ∣ ∞ − x T P x ∣ 0 ) = − ( 0 − x T P x ∣ 0 ) = x T ( 0 ) P x ( 0 ) (5) \begin{aligned} J & =-\int_{0}^{\infty} \frac{\mathrm{d}}{\mathrm{d} t} x^{T} P x \mathrm{~d} t \\ & =-\left(\left.x^{T} P x\right|_{\infty}-\left.x^{T} P x\right|_{0}\right) \\ & =-\left(0-\left.x^{T} P x\right|_{0}\right) \\ & =x^{T}(0) P x(0) \end{aligned}\tag{5} J=−∫0∞dtdxTPx dt=−(xTPx ∞−xTPx 0)=−(0−xTPx 0)=xT(0)Px(0)(5)当系统稳定时， t ⟶ ∞ , x ⟶ 0 t \longrightarrow \infty , x \longrightarrow 0 t⟶∞,x⟶0
可见代价函数只跟初始状态和矩阵 P P P有关
带入 u = K x u = Kx u=Kx,结合式 ( 2 ) (2) (2)、 ( 4 ) (4) (4)，有
x ˙ T P x + x T P x ˙ + x T Q x + x T K T R K x = 0 x T A c l T P x + x T P A c l x + x T Q x + x T K T R K x = 0 x T ( A c l T P + P A c l + Q + K T R K ) x = 0 (6) \begin{aligned} \dot{x}^{T} P x+x^{T} P \dot{x}+x^{T} Q x+x^{T} K^{T} R K x & =0 \\ x^{T} A_{c l}^{T} P x+x^{T} P A_{c l} x+x^{T} Q x+x^{T} K^{T} R K x & =0 \\ x^{T}\left(A_{c l}^{T} P+P A_{c l}+Q+K^{T} R K\right) x & =0 \end{aligned}\tag{6} x˙TPx+xTPx˙+xTQx+xTKTRKxxTAclTPx+xTPAclx+xTQx+xTKTRKxxT(AclTP+PAcl+Q+KTRK)x=0=0=0(6)
代入 A c l = A − B K A_{c l}=A-B K Acl=A−BK 可以得到
A c l T P + P A c l + Q + K T R K = 0 ( A − B K ) T P + P ( A − B K ) + Q + K T R K = 0 A T P + P A + Q + K T R K − K T B T P − P B K = 0 (7) \begin{aligned} A_{c l}^{T} P+P A_{c l}+Q+K^{T} R K & =0 \\ (A-B K)^{T} P+P(A-B K)+Q+K^{T} R K & =0 \\ A^{T} P+P A+Q+K^{T} R K-K^{T} B^{T} P-P B K & =0 \end{aligned}\tag{7} AclTP+PAcl+Q+KTRK(A−BK)TP+P(A−BK)+Q+KTRKATP+PA+Q+KTRK−KTBTP−PBK=0=0=0(7)
将 K = R − 1 B T P K=R^{-1}B^TP K=R−1BTP（暂不推导，见Q4）代入，有
A T P + P A + Q + ( R − 1 B T P ) T R ( R − 1 B T P ) − ( R − 1 B T P ) T B T P − P B ( R − 1 B T P ) = 0 A T P + P A + Q − P B R − 1 B T P = 0 (8) \begin{array}{l} A^{T} P+P A+Q+\left(R^{-1} B^{T} P\right)^{T} R\left(R^{-1} B^{T} P\right)-\left(R^{-1} B^{T} P\right)^{T} B^{T} P-P B\left(R^{-1} B^{T} P\right)=0 \\\\ A^{T} P+P A+Q-P B R^{-1} B^{T} P=0 \end{array}\tag{8} ATP+PA+Q+(R−1BTP)TR(R−1BTP)−(R−1BTP)TBTP−PB(R−1BTP)=0ATP+PA+Q−PBR−1BTP=0(8)
上式也被称为微分Riccati方程(Algebraic Riccati Equation ，ARE)
到这一步就可以认为 P P P是solvable的了，现在已经有很完善的方法能求解ARE了
3.1 LQR控制器设计步骤：
步骤一：根据我们想要的期望状态，初步设计好 Q Q Q， R R R（一般凭借经验，可以通过迭代不断调整）
步骤二：根据代数Riccati方程（由系统矩阵组成的等式），情况下求解矩阵P用的是数值解法，很少的情况可以求其解析解。
步骤三：根据 P P P，得到 K K K的表达式，得到最优控制序列：
3.2 Matlab中应用
% 二阶被控系统，一个输入量
clc;clear;close all
A=[0 1 ;10 0];
B=[0 ;-1];
C=[1 0];
D=0;
Q=[100 0;0 1];
R=0.01;
K=lqr(A,B,Q,R);
sys=ss(A-B*K,B,C,D);% 经过状态量反馈后的新状态矩阵
t=0:0.01:2;
x0=[1.5,2.5];
[y,t,x]=initial(sys,x0,t);
plot(t,x(:,1),LineWidth=1.5);hold on
plot(t,x(:,2),LineWidth=1.5);grid on
xlabel('t');
ylabel(x);
legend('x1','x2')
图片及代码来自LQR控制方法小结
Q4:LQR中做出的…假设？
K = R − 1 B T P K=R^{-1} B^{T} P K=R−1BTP并不是假设，而是推导的结果，过程如下
K的推导
如果只是应用LQR方法，那么推导过程可以不用细看，记住下面的表达式就可以，现在用matlab或是python中的一些库就可以直接求解，应用时理解K的含义就可以。
推导过程应用到矩阵求导相关公式，推荐一个在线矩阵求导网站：Matrix Calculus，可以用来验证自己算的对不对。
推导过程：
观察式 ( 7 ) (7) (7)， A , B , Q , R , P A,B,Q,R,P A,B,Q,R,P都是常值矩阵，唯一可变的是 K K K阵，所以问题转换为找到一个 K K K使得代价函数最小，下面用到了一些构造，主要关注带 K K K的部分，求解的想法是要将 K K K包含在一个满足一定约束的式子里面或许可以得到 K K K的计算表达式，一种思路是如果我们可以把含有 K K K的部分转换成类似 ( M + N ) T ( M + N ) (M+N)^T(M+N) (M+N)T(M+N) 的结构，那么要使得代价最小，就会有 M + N = 0 M + N = 0 M+N=0 ，那么 K K K就可以求。
令 R = T T T R=T^TT R=TTT，代入式 ( 7 ) (7) (7)，有
A T P + P A + Q − K T B T P − P B K + K T T T T K = 0 A T P + P A + Q − K T B T P − P B K + ( T K ) T T K = 0 (9) A^{T} P+P A+Q-K^{T} B^{T} P-P B K+K^{T} T^{T} T K=0\\\\ A^{T} P+P A+Q-K^{T} B^{T} P-P B K+(T K)^{T} T K=0\tag{9} ATP+PA+Q−KTBTP−PBK+KTTTTK=0ATP+PA+Q−KTBTP−PBK+(TK)TTK=0(9)
为向 ( M + N ) T ( M + N ) (M+N)^{T}(M+N) (M+N)T(M+N)$ 上面靠，将目标形式展开: M T M + N T N + M T N + N T M M^{T} M+N^{T} N+M^{T} N+N^{T} M MTM+NTN+MTN+NTM ，令 M = T K M=T K M=TK ，刚好可以满足上面其中一项，剩下的用待定系数，带进去解 N N N的表达式:
M T M + N T N + M T N + N T M = − K T B T P − P B K + ( T K ) T T K , M = T K ⇒ N T N + K T T T N + N T T K = − K T B T P − P B K (10) M^{T} M+N^{T} N+M^{T} N+N^{T} M=-K^{T} B^{T} P-P B K+(T K)^{T} T K, M=T K\\\\ \Rightarrow N^{T} N+K^{T} T^{T} N+N^{T} T K=-K^{T} B^{T} P-P B K\tag{10} MTM+NTN+MTN+NTM=−KTBTP−PBK+(TK)TTK,M=TK⇒NTN+KTTTN+NTTK=−KTBTP−PBK(10)
注意 ( 10 ) (10) (10)左右并不一定相等，只是为了让等式右凑出等式左的样子
继续观察，等式两边都有含 K T K^T KT项和含 K K K 项 ，先用含 K K K 项拼凑出 N N N:
N T T K = − P B K ⇒ N = [ − P B ( T − 1 ) ] T ⇒ N = − ( T − 1 ) T B T P T = N = − ( T − 1 ) T B T P (11) \begin{array}{l} N^{T} T K=-P B K \Rightarrow N=\left[-P B\left(T^{-1}\right)\right]^{T} \Rightarrow N=-\left(T^{-1}\right)^{T} B^{T} P^{T}=N= \\ -\left(T^{-1}\right)^{T} B^{T} P \end{array}\tag{11} NTTK=−PBK⇒N=[−PB(T−1)]T⇒N=−(T−1)TBTPT=N=−(T−1)TBTP(11)
将 N N N 代入，求剩下的部分:
N T N + K T T T N + N T T K = − K T B T P − P B K , N = − ( T − 1 ) T B T P (12) N^{T} N+K^{T} T^{T} N+N^{T} T K=-K^{T} B^{T} P-P B K, \quad N=-\left(T^{-1}\right)^{T} B^{T} P\tag{12} NTN+KTTTN+NTTK=−KTBTP−PBK,N=−(T−1)TBTP(12)
发现含 K T K^T KT 项也消掉了，仅剩下不含 K K K 的第一项:
N T N = [ ( T − 1 ) T B T P ] T [ ( T − 1 ) T B T P ] = P B R − 1 B T P (13) N^{T} N=\left[\left(T^{-1}\right)^{T} B^{T} P\right]^{T}\left[\left(T^{-1}\right)^{T} B^{T} P\right]=P B R^{-1} B^{T} P\tag{13} NTN=[(T−1)TBTP]T[(T−1)TBTP]=PBR−1BTP(13)
带到代数Riccati方程中有:
A T P + P A + Q + [ T K − ( T − 1 ) T B T P ] T [ T K − ( T − 1 ) T B T P ] − P B R − 1 B T P = 0 (14) A^{T} P+P A+Q+\left[T K-\left(T^{-1}\right)^{T} B^{T} P\right]^{T}\left[T K-\left(T^{-1}\right)^{T} B^{T} P\right]-P B R^{-1} B^{T} P=0\tag{14} ATP+PA+Q+[TK−(T−1)TBTP]T[TK−(T−1)TBTP]−PBR−1BTP=0(14)
因为:
x T [ T K − ( T − 1 ) T B T P ] T [ T K − ( T − 1 ) T B T P ] x ≥ 0 (15) x^T\left[T K-\left(T^{-1}\right)^{T} B^{T} P\right]^{T}\left[T K-\left(T^{-1}\right)^{T} B^{T} P\right]x \ge 0\tag{15} xT[TK−(T−1)TBTP]T[TK−(T−1)TBTP]x≥0(15)
令 T K − ( T − 1 ) T B T P = 0 T K-\left(T^{-1}\right)^{T} B^{T} P = 0 TK−(T−1)TBTP=0，可以解出 K = R − 1 B T P K=R^{-1} B^{T} P K=R−1BTP
怎么理解u=Kx
u = K x u=Kx u=Kx是将输入向量 u u u用状态变量 x x x的线性表达，不妨把这理解为LQR的特征，或者说LQR这一形式就意味着这一假设
稳定性分析
想要分析系统的稳定性，一般采用李雅普诺夫稳定性理论来证明。
首先，定义李雅普诺夫函数：
V ( x ) = x T P x , P = P T P > 0 (16) V(x)=x^{T} P x, P=P^{T} P>0\tag{16} V(x)=xTPx,P=PTP>0(16)
P P P为正定常数矩阵，所以 V ( x ) V(x) V(x) 是 正定的。(?)
然后，对 V ( x ) V(x) V(x) 求 x x x 的一阶导数：
V ˙ ( x ) = x ˙ T P x + x T P x ˙ (17) \dot{V}(x)=\dot{x}^{T} P x+x^{T} P \dot{x}\tag{17} V˙(x)=x˙TPx+xTPx˙(17)
将 x ˙ = A x + B u , u = − K x = − R − 1 B T P x \dot{x}=A x+B u, u=-K x=-R^{-1} B^{T} P x x˙=Ax+Bu,u=−Kx=−R−1BTPx 带入上式，得到：
V ˙ ( x ) = x T [ A T P − K T B T P + P A − P B K ] x (18) \dot{V}(x)=x^{T}\left[A^{T} P-K^{T} B^{T} P+P A-P B K\right] x\tag{18} V˙(x)=xT[ATP−KTBTP+PA−PBK]x(18)
再代入 K K K，得到:
V ˙ ( x ) = x T [ A T P + P A − 2 P B R − 1 B T P ] x (19) \dot{V}(x)=x^{T}\left[A^{T} P+P A-2 P B R^{-1} B^{T} P\right] x\tag{19} V˙(x)=xT[ATP+PA−2PBR−1BTP]x(19)
因为通过无限时间LQR设计的 P P P 会满足上面的代数Riccati方程 ( 8 ) (8) (8)，带进来，得到:
V ˙ ( x ) = − x T [ Q + P B R − 1 B T P ] x (20) \dot{V}(x)=-x^{T}\left[Q+P B R^{-1} B^{T} P\right] x\tag{20} V˙(x)=−xT[Q+PBR−1BTP]x(20)
因为: Q > 0 , R > 0 , P > 0 Q>0, R>0, P>0 Q>0,R>0,P>0 ，故 V ˙ ( x ) < 0 \dot{V}(x)<0 V˙(x)<0 ， 系统是渐进稳定的。
上面是从别处搬运的，我只是看了一遍公式无误，自己其实也看不太懂
也有一种说法，是说稳定性取决于状态矩阵A的特征值的符号，利用MATLAB语句[V,F] = eig(A)求得状态矩阵A的特征值以及对应的特征向量。
当全部为实根的时候，只有它们都小于零，系统才是稳定的。
可控性分析
只要满足一些基本条件，LQR的设计过程就能保证得到一个让系统稳定的反馈控制器。
- LQR定理
令系统 ( A , B ) (A,B) (A,B) 可控， R R R 和 Q Q Q 都是正定的，则闭环系统 ( A − B K ) ( A − B K ) (A−BK)近稳定。注意，不管系统的开环稳定性如何，这都是成立的。
回顾现控的相关知识：可控性可以通过检查可控性矩阵 U = [ B A B A 2 B ⋯ A n − 1 B ] U=\left[\begin{array}{lllll} B & A B & A^{2} B & \cdots & A^{n-1} B \end{array}\right] U=[BABA2B⋯An−1B]是否满秩来判断
参考文档
(个人感觉最好✨→) 线性二次型调节器(LQR)原理详解
https://blog.csdn.net/qq_36133747/article/details/123413115
(推导K→) LQR最优控制方法小结
https://zhuanlan.zhihu.com/p/363033191
(应用篇)RoboMaster平衡步兵机器人控制系统设计
https://zhuanlan.zhihu.com/p/563048952