LQR控制算法推导以及简单分析
已于 2022-04-29 16:14:43 修改
阅读量6.2w
收藏
754
点赞数
134
首先，这篇文章是看了几个大神的博客后，自己抄录以及整理的内容，其中有些自己的想法，但是原理部分基本都是学习大神们的，在此先说明一下。
1 全状态反馈控制系统
在介绍LQR之前，首先，先回顾一下现在控制理论中的基本的控制器 —————— 全状态反馈控制，上图
假设有一个线性系统用状态向量表示：
{
x
˙
=
A
x
+
B
u
y
=
C
x
+
D
u
(1)
\begin{cases} \dot{x} = Ax + Bu & \\ y = Cx + Du & \end{cases} \tag{1}
{x˙=Ax+Buy=Cx+Du(1)
其中，
x
(
t
)
∈
R
n
x(t) \in R^n
x(t)∈Rn，
u
(
t
)
∈
R
m
u(t) \in R^m
u(t)∈Rm，初始条件是
x
(
0
)
x(0)
x(0)。
在此，我们需要设计一个状态反馈控制器
u
=
−
K
x
(2)
u = -Kx \tag{2}
u=−Kx(2)
使上述控制器能达到期望的稳定性能，将式(2)带入系统状态方程(1)中，有
x
˙
=
(
A
−
B
K
)
x
=
A
c
x
(3)
\dot{x} = (A - BK)x = A_{c}x \tag{3}
x˙=(A−BK)x=Acx(3)
设定系统中的各个状态量都可知，式(1)所示的开环系统，传递函数的极点就是系统矩阵A的特征值。现在变换成了式(2)的闭环形式，通过配置反馈矩阵
K
K
K，可以使得闭环系统达到所期望的系统状态。(注意，这种控制器设计与矩阵C、D没什么关系)
SO，来了一个新的问题，极点在什么样的位置会使得系统的性能较好呢？并且，当系统变量很多的时候，就算设计极点已经达到了最优，矩阵K的计算如何计算呢？
因此，LQR提供了如下思路。
2 LQR
LQR的目标就是找到一组控制量
u
0
,
u
1
,
.
.
.
u_0,u_1,...
u0,u1,...，使得同时有
x
0
,
x
1
,
.
.
.
x_0,x_1,...
x0,x1,...足够小(系统达到稳定状态)，
u
0
,
u
1
,
.
.
.
u_0,u_1,...
u0,u1,...足够小(控制量尽量小的变化)，选取代价函数为
J
=
1
2
∫
0
∞
x
T
Q
x
+
u
T
R
u
d
t
(4)
J = {1 \over 2}\int_0^\infty {x^TQx + u^TRu} \space dt \tag{4}
J=21∫0∞xTQx+uTRu dt(4)
其中，Q、R就是需要设计的半正定矩阵和正定矩阵。
看上式(4)是不是线代中的二次型(线代真的是一门重要的学科，工程中大量都是线代的运算)，而且还是那种只有平方项的二次型，这样就成了最小二乘法的问题。代价函数
J
J
J需要达到最小值，那么在
t
t
t趋近于无穷时，状态向量
x
(
t
)
x(t)
x(t)肯定趋近于0，即是达到了系统稳态；同理，
t
t
t趋近于无穷时，控制向量
u
(
t
)
u(t)
u(t)也会趋近于0，意味着，随着时间的推移，需要对系统施加的控制量会越来越小，意味着使用最小的控制量使得系统达到了最终控制目标。
下面来聊聊Q、R值的选取，一般来说，选取Q、R矩阵的时候，为了方便观察各个系统状态量而选取对角阵，增加Q的一个值，意味着这个值作用的系统状态量，将以更快的速度衰减到0，这时候，举个栗子还是很必要的，比如， Q 11 Q_{11} Q11选取较大的值，会让 x 1 x_1 x1很快的衰减到0；另外一方面，加大 R R R的值，会使得对应的控制量减小，控制器执行更少的动作，意味着系统的状态衰减将变慢。所以， Q 、 R Q、R Q、R的选取，要综合看具体的实际应用场景来调节，俗话说，鱼和熊掌不可兼得，这个矛盾就像做的轨迹跟踪MPC中的设置不同的权重一样，期望车辆后轴心cte值越小，那么到了转弯处必须要很大幅度的刹车，以及打方向盘，速度以及yaw都会大幅度变化，而如果期望执行器动作越小，那么就无法保证cte的准确度一样。
好了，上面介绍了一些参数的意义问题，下面重点就是公式的推导了，在不同大神那看到了不同的推导方式，下面就把自己比较欣赏的一种写出来吧，虽说过程也很重要，但是工程嘛，只要结果对，效果好，选个自己看起来舒服的方式就好了，开始推导过程
- 将
u
=
−
K
x
u = -Kx
u=−Kx代入代价函数后，有
J = 1 2 ∫ 0 ∞ x T ( Q + K T R K ) x d t (5) J = {1 \over 2}\int_0^\infty x^T(Q + K^TRK)x \space dt \tag{5} J=21∫0∞xT(Q+KTRK)x dt(5) - 假设纯在一个常量矩阵
P
P
P使得，
d d t ( x T P x ) = − x T ( Q + K T R K ) x (6) {d \over dt}(x^TPx) = - x^T(Q + K^TRK)x \tag{6} dtd(xTPx)=−xT(Q+KTRK)x(6) - 把式(6)代入(5)后，有
J = − 1 2 ∫ 0 ∞ d d t x T ( P ) x = 1 2 x T ( 0 ) P x ( 0 ) (7) J = -{1 \over 2}\int_0^\infty \frac{d}{dt}x^T(P)x \space = {1 \over 2}x^T(0)Px(0) \tag{7} J=−21∫0∞dtdxT(P)x =21xT(0)Px(0)(7)
式(7)的意思就是，t趋近于无穷时，系统状态向量x(t)趋近于0，这样就直接结算出了积分方程。 - 把式(6)左边微分展开，
x ˙ T P x + x T P x ˙ + x T Q x + x T K T R K x = 0 \dot{x}^{T}Px + x^{T}P\dot{x}+x^TQx+x^TK^TRKx = 0 x˙TPx+xTPx˙+xTQx+xTKTRKx=0
状态变量x的微分用式(3)表示，
x T A c T P x + x T P A c x + x T Q x + x T K T R K x = 0 x^{T}A^{T}_cPx+x^{T}PA_{c}x+x^TQx+x^TK^TRKx = 0 xTAcTPx+xTPAcx+xTQx+xTKTRKx=0
整理后，有
x T ( A c T P + P A c + Q + K T R K ) x = 0 (8) x^{T}(A^{T}_{c}P+PA_{c}+Q+K^{T}RK)x=0 \tag{8} xT(AcTP+PAc+Q+KTRK)x=0(8)
这样，就又回到了二次型的问题，如果式(8)要有解，那么括号里面的部分必须等于0.
A c T P + P A c + Q + K T R K = 0 (9) A^{T}_{c}P+PA_{c}+Q+K^{T}RK = 0 \tag{9} AcTP+PAc+Q+KTRK=0(9)
把 A c = A − B K A_c = A-BK Ac=A−BK代入式(9)
( A − B K ) T P + P ( A − B K ) + Q + K T R K = 0 (10) (A-BK)^{T}P+P(A-BK)+Q+K^{T}RK = 0 \tag{10} (A−BK)TP+P(A−BK)+Q+KTRK=0(10)
A T P + P A + Q + K T R K − K T B T P − P B K = 0 (11) A^{T}P+PA+Q+K^{T}RK-K^{T}B^{T}P-PBK=0 \tag{11} ATP+PA+Q+KTRK−KTBTP−PBK=0(11) - 式(11)还是一个关于
K
K
K的二次型，这样会导致计算量太复杂，so只要这个等式成立就好，那么这里令
K
=
R
−
1
B
T
P
K = R^{-1}B^TP
K=R−1BTP，然后式(11)，可以化为
A T P + P A + Q + K T R ( R − 1 B T P ) − K T B T P − P B ( R − 1 B T P ) = 0 A^{T}P+PA+Q+K^{T}R(R^{-1}B^TP)-K^{T}B^{T}P-PB(R^{-1}B^TP)=0 ATP+PA+Q+KTR(R−1BTP)−KTBTP−PB(R−1BTP)=0
A T P + P A + Q − P B R − 1 B T P = 0 (12) A^{T}P+PA+Q-PBR^{-1}B^TP=0 \tag{12} ATP+PA+Q−PBR−1BTP=0(12)
式(12)中， A , B , Q , R A,B,Q,R A,B,Q,R都是已知量，那么通过式(12)可以求解出 P P P，式(12)就是著名的Riccati方程。
总结
上面，从理论以及公式推导两个方面，介绍了LQR，现在从头看一下LQR的思路：
- 选择参数矩阵Q,R
- 求解Riccati方程得到矩阵P
- 根据P计算 K = R − 1 B T P K=R^{-1}B^{T}P K=R−1BTP
- 计算控制量 u = − K x u=-Kx u=−Kx
MPC与LQR比较
MPC和LQR两种控制方式有很多的相似之处，但是也有很多不相同的地方，
- 首先，LQR的研究对象是现代控制理论中的状态空间方程给出的线性系统，而MPC的研究对象可以是线性系统，也可以是非线性系统。不过现在很多的做法都是将非线性系统线性化，然后进行相关计算，具体要根据自己的工程情况来确定哪种方式比较好，比如之前做MPC的时候，线控车底层速度控制接口就是加速度，那就没必要根据IMU再套嵌个一层PID。
- 其次，既然是优化问题，那就离不开目标函数的设计，LQR的目标函数在上面已经有描述，MPC的目标函数，多数都是多个优化目标乘以不同权重然后求和的方式。虽然方式不同，不过都是对达到控制目标的代价累计。
- 最后，工作时域上的不同，LQR的计算针对同一工作时域，在一个控制周期内，LQR只计算一次，并将此次计算出的最优解下发给控制器即可；而MPC是滚动优化的，计算未来一段时间内，每个采样周期都会经过计算，得出一组控制序列，但是只将第一个控制值下发给控制器。
Reference：
- F.L. Lewis .<< Linear Quadratic Regulator (LQR) State Feedback Design >>
- https://blog.csdn.net/u013914471/article/details/84324754?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task
- https://zhuanlan.zhihu.com/p/72605138
- https://blog.csdn.net/heyijia0327/article/details/39270597
909
暂无认证
到【灌水乐园】发言
CSDN-Ada助手: 你要暗自努力然后惊艳所有人。
qq_26344349: 公式7的积分结果，应该是 无穷时刻 - 0时刻，无穷时刻x趋于0，因为系统稳定， 公式6 加上负号，J的转化结果就没有负号了
我自是年少，韶华倾负944: 公式6那里为什么要加个负号呢？
疯狂的头铁石头: 是不是可以有一条全局路径，或者你设定的纵向规划距离长一点，让它可以看到后续的参考线
不会是要长脑子了吧: a和b是不是对应矩形长宽的一半？