本文介绍最优控制中LQR算法(Linear Quadratic Regulator)的离散版本推导求解过程,即推导Riccati方程,而后计算出最优控制量,最后通过一个程序demo展示LQR算法效果。
本文主要内容如下:
- 线性二次型调节
- 线性状态空间方程
- 二次型代价函数
- 反馈控制
- 向量求导
- 带约束的最优化问题
- 求极值点
- LQR求解步骤
- 程序demo
- 总结
00 线性二次型调节
线性二次型调节器(Linear Quadratic Regulator,缩写为LQR)是一种经典的最优控制算法。对于线性系统,如果它的代价函数是二次型函数,则最优控制量可以通过LQR求解得到。
下面是《现代控制理论》中对线性二次型和状态调节器的描述:
如果系统是线性的,性能泛函是状态变量(或/和)控制变量的二次型函数的积分,
则这样的最优控制问题称为线性二次型最优控制问题。简称线性二次型。
--《现代控制理论》
状态调节器的任务在于,当系统状态由于任何原因偏离了平衡状态时,能在不消耗
过多能量的情况下,保持系统状态各个分量仍接近于平衡状态。
--《现代控制理论》
01 线性状态空间方程
对于线性系统,都可以通过线性状态空间方程进行描述,离散的线性状态空间方程如下:
x
k
+
1
=
A
x
k
+
B
u
k
\begin{equation} x_{k+1}=Ax_k+Bu_k \end{equation}
xk+1=Axk+Buk
其中:
- x k + 1 x_{k+1} xk+1表示第k步的系统状态向量,是一个n维向量;
- x k x_{k} xk表示第k+1步的系统状态向量,是一个n维向量;
- u k u_{k} uk表示第k步的输入(或控制)向量,是一个m维向量;
- A A A被称作系统矩阵,是一个n*n方阵;
- B B B被称作控制矩阵,是一个n*m的矩阵。
1.1 举个例子
一个小车在一条直线上做变速直线运动。
如果我们可以通过控制速度来控制小车的运动,并且速度变化的时间间隔为dt,则可以得到关于位置p和速度v之间的关系,如下:
p
k
+
1
=
p
k
+
v
k
d
t
\begin{equation} p_{k+1} = p_k + v_kdt \nonumber \end{equation}
pk+1=pk+vkdt
上面的表达式就是此系统的线性状态空间方程,其中:
n
=
1
;
m
=
1
;
x
k
=
[
p
k
]
;
u
k
=
[
v
k
]
;
A
=
[
1
]
;
B
=
[
d
t
]
n=1;m=1;x_k=[p_k];u_k=[v_k];A=[1];B=[dt]
n=1;m=1;xk=[pk];uk=[vk];A=[1];B=[dt]
02 二次型代价函数
离散型LQR代价函数如下:
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^TQx_k+u_k^TRu_k)+x_N^TQx_N}
J=k=0∑N−1(xkTQxk+ukTRuk)+xNTQxN
可以看出该代价函数是一个关于(N+1)个x和N个u的二次型函数。其中:
- Q表示状态权重方阵,大小为n*n,半正定矩阵,一般是对称矩阵。
- R表示控制权重方阵,大小为m*m,正定矩阵,一般是对称矩阵。
需要注意的是,这里关于x的项比关于u的项多一项,这是由于状态空间方程(1)导致的。
根据优化目标的不同,我们可以使用不同的Q矩阵和R矩阵。
03 反馈控制
反馈控制是指在某一行动和任务完成之后,将实际结果进行比较,从而对下一步行动的进行产生影响,起到控制的作用。下图是一个线性二次型最优反馈系统。
在上图的系统中,控制量u是通过对系统的结果状态x与一个最优反馈控制矩阵K相作用得到的,离散化的关系式如下:
u
k
=
−
K
k
x
k
u_k=-K_kx_k
uk=−Kkxk
04 向量求导
这里的自变量都是向量,对于向量求导,有如下规则:
∂
(
x
T
A
)
∂
x
=
A
∂
(
A
x
)
∂
x
=
A
T
∂
(
x
T
A
x
)
∂
x
=
(
A
+
A
T
)
x
\frac {\partial (x^TA)}{\partial x}=A \\ \frac {\partial(Ax)}{\partial x}=A^T \\ \frac {\partial(x^TAx)} {\partial x}=(A+A^T)x
∂x∂(xTA)=A∂x∂(Ax)=AT∂x∂(xTAx)=(A+AT)x
当矩阵A是对称矩阵时:
∂
(
x
T
A
x
)
∂
x
=
2
A
x
\frac {\partial(x^TAx)} {\partial x}=2Ax
∂x∂(xTAx)=2Ax
05 带约束的最优化问题
前面给出了LQR有关的基础数学表达式,经过整理会得到一个如下的最优化问题:
minimize
J
=
∑
k
=
0
N
−
1
(
x
k
T
Q
x
k
+
u
k
T
R
u
k
)
+
x
N
T
Q
x
N
subject to
x
k
+
1
=
A
x
k
+
B
u
k
,
k
=
0
,
.
.
.
N
−
1
\begin{align} \text{minimize} & \quad J = \sum_{k=0}^{N-1}(x_k^TQx_k+u_k^TRu_k)+x_N^TQx_N \nonumber \\ \text{subject to} & \quad x_{k+1}=Ax_k+Bu_k, \quad k=0,...N-1 \nonumber \end{align}
minimizesubject toJ=k=0∑N−1(xkTQxk+ukTRuk)+xNTQxNxk+1=Axk+Buk,k=0,...N−1
以上是一个带约束的最优化问题,以前面的线性状态空间方程为约束条件,以二次代价函数为目标函数,求解目标函数取最小值时的状态量x和控制量u。
在高等数学中,可以通过拉格朗日乘数法求解带等式约束的最优化问题。这里先构造一个拉格朗日函数:
L
=
∑
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
)
L=\sum_{k=0}^{N-1}(x_k^TQx_k+u_k^TRu_k)+x_N^TQx_N+\sum_{k=0}^{N-1}\lambda_{k+1}^T(Ax_k+Bu_k-x_{k+1})
L=k=0∑N−1(xkTQxk+ukTRuk)+xNTQxN+k=0∑N−1λk+1T(Axk+Buk−xk+1)
变形:
L
=
∑
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
−
x
k
+
1
)
]
+
x
N
T
Q
x
N
L
=
∑
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
L=\sum_{k=0}^{N-1}[x_k^TQx_k+u_k^TRu_k+\lambda_{k+1}^T(Ax_k+Bu_k-x_{k+1})]+x_N^TQx_N \\ \begin{equation} L=\sum_{k=0}^{N-1}[x_k^TQx_k+u_k^TRu_k+\lambda_{k+1}^T(Ax_k+Bu_k)-\lambda_{k+1}^Tx_{k+1}]+x_N^TQx_N \end{equation}
L=k=0∑N−1[xkTQxk+ukTRuk+λk+1T(Axk+Buk−xk+1)]+xNTQxNL=k=0∑N−1[xkTQxk+ukTRuk+λk+1T(Axk+Buk)−λk+1Txk+1]+xNTQxN
求解出函数L关于x, u和λ的一阶偏导为0时的解,即求解出了原代价函数J在约束条件下的极小值点。
06 求极值点
对公式(2)进行求导:
∂
L
∂
x
0
=
2
Q
x
0
+
A
T
λ
1
=
0
∂
L
∂
x
k
=
2
Q
x
k
+
A
T
λ
k
+
1
−
λ
k
=
0
,
k
=
1
,
2
,
.
.
,
N
−
1
∂
L
∂
x
N
=
2
Q
x
N
−
λ
N
=
0
∂
L
∂
u
k
=
2
R
u
k
+
B
T
λ
k
+
1
=
0
,
k
=
0
,
1
,
.
.
.
,
N
−
1
∂
L
∂
λ
k
=
A
x
k
−
1
+
B
u
k
−
1
−
x
k
=
0
,
k
=
1
,
2
,
.
.
.
,
N
\begin {alignat}{2} \frac {\partial L} {\partial x_0} & =2Qx_0+A^T\lambda_1 && =0 \nonumber \\ \frac {\partial L}{\partial x_k} & =2Qx_k+A^T\lambda_{k+1}-\lambda_k && =0,\quad k=1,2,..,N-1 \nonumber \\ \frac {\partial L}{\partial x_N} & =2Qx_N-\lambda_N && =0 \nonumber \\ \frac {\partial L}{\partial u_k} & =2Ru_k+B^T\lambda_{k+1} && =0, \quad k=0,1,...,N-1 \nonumber \\ \frac {\partial L}{\partial \lambda_k} & =Ax_{k-1}+Bu_{k-1}-x_k && =0, \quad k=1,2,...,N \nonumber \end {alignat}
∂x0∂L∂xk∂L∂xN∂L∂uk∂L∂λk∂L=2Qx0+ATλ1=2Qxk+ATλk+1−λk=2QxN−λN=2Ruk+BTλk+1=Axk−1+Buk−1−xk=0=0,k=1,2,..,N−1=0=0,k=0,1,...,N−1=0,k=1,2,...,N
==>
0
=
2
Q
x
0
+
A
T
λ
1
λ
k
=
2
Q
x
k
+
A
T
λ
k
+
1
,
k
=
1
,
2
,
.
.
,
N
−
1
λ
N
=
2
Q
x
N
u
k
=
−
1
2
R
−
1
B
T
λ
k
+
1
,
k
=
0
,
1
,
.
.
.
,
N
−
1
x
k
+
1
=
A
x
k
+
B
u
k
,
k
=
0
,
1
,
.
.
.
,
N
−
1
\begin {align} 0 & = 2Qx_0+A^T\lambda_1 \nonumber \\ \lambda_k & =2Qx_k+A^T\lambda_{k+1},\quad k=1,2,..,N-1 \\ \lambda_N & = 2Qx_N \\ u_k & =-\frac 1 2 R^{-1}B^T\lambda_{k+1}, \quad k=0,1,...,N-1 \\ x_{k+1} & =Ax_{k}+Bu_{k}, \quad k=0,1,...,N-1 \end {align}
0λkλNukxk+1=2Qx0+ATλ1=2Qxk+ATλk+1,k=1,2,..,N−1=2QxN=−21R−1BTλk+1,k=0,1,...,N−1=Axk+Buk,k=0,1,...,N−1
当k=N-1时,将公式(4)带入公式(5),得到:
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^T2QX_N=-R^{-1}B^TQx_N \end {equation}
uN−1=−21R−1BT2QXN=−R−1BTQxN
再将公式(7)带入公式(6),得到:
x
N
=
A
x
N
−
1
+
B
(
−
R
−
1
B
T
Q
x
N
)
=
A
x
N
−
1
−
B
R
−
1
B
T
Q
x
N
(
I
+
B
R
−
1
B
T
Q
)
x
N
=
A
x
N
−
1
x
N
=
(
I
+
B
R
−
1
B
T
Q
)
−
1
A
x
N
−
1
x_N =Ax_{N-1}+B(-R^{-1}B^TQx_N) =Ax_{N-1}-BR^{-1}B^TQx_N \\ (I+BR^{-1}B^TQ)x_N=Ax_{N-1} \\ \begin{equation} x_N =(I+BR^{-1}B^TQ)^{-1}Ax_{N-1} \end{equation}
xN=AxN−1+B(−R−1BTQxN)=AxN−1−BR−1BTQxN(I+BR−1BTQ)xN=AxN−1xN=(I+BR−1BTQ)−1AxN−1
再将公式(8)带入公式(4),得到:
λ
N
=
2
Q
(
I
+
B
R
−
1
B
T
Q
)
−
1
A
x
N
−
1
\begin{equation} \lambda_N=2Q(I+BR^{-1}B^TQ)^{-1}Ax_{N-1} \end{equation}
λN=2Q(I+BR−1BTQ)−1AxN−1
再将公式(9)带入公式(3),得到:
λ
N
−
1
=
2
Q
x
N
−
1
+
A
T
2
Q
(
I
+
B
R
−
1
B
T
Q
)
−
1
A
x
N
−
1
λ
N
−
1
=
2
[
Q
+
A
T
Q
(
I
+
B
R
−
1
B
T
Q
)
−
1
A
]
x
N
−
1
\begin {align} \lambda_{N-1} & =2Qx_{N-1}+A^T2Q(I+BR^{-1}B^TQ)^{-1}Ax_{N-1} \nonumber \\ \lambda_{N-1} & =2[Q+A^TQ(I+BR^{-1}B^TQ)^{-1}A]x_{N-1} \end{align}
λN−1λN−1=2QxN−1+AT2Q(I+BR−1BTQ)−1AxN−1=2[Q+ATQ(I+BR−1BTQ)−1A]xN−1
对比公式(4)和公式(10),发现它们形状相似:
λ
N
=
2
Q
x
N
,
(
4
)
λ
N
−
1
=
2
[
Q
+
A
T
Q
(
I
+
B
R
−
1
B
T
Q
)
−
1
A
]
x
N
−
1
(
10
)
\begin {align} \lambda_N & = 2Qx_N, \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad \quad (4) \nonumber \\ \lambda_{N-1} & =2[Q+A^TQ(I+BR^{-1}B^TQ)^{-1}A]x_{N-1} \quad (10) \nonumber \end {align}
λNλN−1=2QxN,(4)=2[Q+ATQ(I+BR−1BTQ)−1A]xN−1(10)
令:
P
N
−
1
=
Q
+
A
T
Q
(
I
+
B
R
−
1
B
T
Q
)
−
1
A
\begin{equation} P_{N-1}=Q+A^TQ(I+BR^{-1}B^TQ)^{-1}A \nonumber \end{equation}
PN−1=Q+ATQ(I+BR−1BTQ)−1A
则公式(10)可以写作:
λ
N
−
1
=
2
P
N
−
1
x
N
−
1
\begin{equation} \lambda_{N-1} = 2P_{N-1}x_{N-1} \end{equation}
λN−1=2PN−1xN−1
这样可以明显看出,公式(11)和公式(4)结构一样。如果我们令k=N-2,用公式(11)代替公式(4),然后重复前面的计算过程,可以得到如下表达式:
P
N
−
2
=
Q
+
A
T
P
N
−
1
(
I
+
B
R
−
1
B
T
P
N
−
1
)
−
1
A
P_{N-2}=Q+A^TP_{N-1}(I+BR^{-1}B^TP_{N-1})^{-1}A
PN−2=Q+ATPN−1(I+BR−1BTPN−1)−1A
如果令
P
N
=
Q
P_N=Q
PN=Q,可以得到一个如下的递推关系:
P
k
=
Q
+
A
T
P
k
+
1
(
I
+
B
R
−
1
B
T
P
k
+
1
)
−
1
A
,
k
=
1
,
2
,
.
.
,
N
−
1
\begin{equation} P_k=Q+A^TP_{k+1}(I+BR^{-1}B^TP_{k+1})^{-1}A,\quad k=1,2,..,N-1 \end{equation}
Pk=Q+ATPk+1(I+BR−1BTPk+1)−1A,k=1,2,..,N−1
这里的公式(12)就是离散化的Ricatte方程。
再将公式(6)带入公式(7),并且将其中的N-1看作k,N看作k+1,Q看作
P
k
+
1
P_{k+1}
Pk+1可以得到:
u
k
=
−
R
−
1
B
T
P
k
+
1
(
A
x
k
+
B
u
k
)
u
k
=
−
R
−
1
B
T
P
k
+
1
A
x
k
−
R
−
1
B
T
P
k
+
1
B
u
k
(
I
+
R
−
1
B
T
P
k
+
1
B
)
u
k
=
−
R
−
1
B
T
P
k
+
1
A
x
k
(
R
+
B
T
P
k
+
1
B
)
u
k
=
−
B
T
P
k
+
1
A
x
k
u
k
=
−
(
R
+
B
T
P
k
+
1
B
)
−
1
B
T
P
k
+
1
A
x
k
,
k
=
0
,
1
,
.
.
.
,
N
−
1
\begin{align} u_k & =-R^{-1}B^TP_{k+1}(Ax_k+Bu_k) \nonumber \\ u_k &=-R^{-1}B^TP_{k+1}Ax_k-R^{-1}B^TP_{k+1}Bu_k \nonumber \\ (I+R^{-1}B^TP_{k+1}B)u_k &=-R^{-1}B^TP_{k+1}Ax_k \nonumber \\ (R+B^TP_{k+1}B)u_k &=-B^TP_{k+1}Ax_k \nonumber \\ u_k &=-(R+B^TP_{k+1}B)^{-1}B^TP_{k+1}Ax_k, \quad k=0,1,...,N-1 \end{align}
ukuk(I+R−1BTPk+1B)uk(R+BTPk+1B)ukuk=−R−1BTPk+1(Axk+Buk)=−R−1BTPk+1Axk−R−1BTPk+1Buk=−R−1BTPk+1Axk=−BTPk+1Axk=−(R+BTPk+1B)−1BTPk+1Axk,k=0,1,...,N−1
在通过公式(12)求得
P
k
+
1
P_{k+1}
Pk+1的情况下,再使用公式(13),就可以求解出每一步的最优控制量u。
结合前面的反馈控制关系式,可以得到:
K
k
=
(
R
+
B
T
P
k
+
1
B
)
−
1
B
T
P
k
+
1
A
,
k
=
0
,
1
,
.
.
.
,
N
−
1
\begin {equation} K_k=(R+B^TP_{k+1}B)^{-1}B^TP_{k+1}A, \quad k=0,1,...,N-1 \end {equation}
Kk=(R+BTPk+1B)−1BTPk+1A,k=0,1,...,N−1
07 LQR求解步骤
LQR设计和求解步骤如下:
- 根据系统特性,确定系统矩阵A,和控制矩阵B;根据优化的目标,确定状态权重矩阵Q和控制权重矩阵R,每一时间步对应的矩阵Q和矩阵R是可以不一样的;测量出系统当前状态 x 0 x_0 x0。
- 将矩阵A,B,Q,R带入公式(12)Ricatte方程,求出矩阵P的序列。
- 将矩阵P的序列带入公式(14),求出反馈控制矩阵K的序列。
- 将反馈控制矩阵 K 0 K_0 K0和系统当前状态 x 0 x_0 x0,带入公式 u 0 = − K 0 x 0 u_0=-K_0x_0 u0=−K0x0,可以得到当前时刻的最优控制量 u 0 u_0 u0。
- 如果需要求解剩余时间步的控制量,则需要通过状态空间方程(1),求出下一时刻的系统状态 x k + 1 x_{k+1} xk+1,再将反馈控制矩阵 K k + 1 K_{k+1} Kk+1和系统当前状态 x k + 1 x_{k+1} xk+1,带入公式 u k + 1 = − K k + 1 x k + 1 u_{k+1}=-K_{k+1}x_{k+1} uk+1=−Kk+1xk+1,可以得到下一时刻的最优控制量 u k + 1 u_{k+1} uk+1。这里k分别取0,1,…N-2,依次求解。
08 程序demo
这里用一个demo演示LQR的控制跟踪效果:还是一个小车在一条直线上做变速直线运动,程序上游模块给出了一个预期的小车S-T图和V-T图。但由于外界环境影响(比如风阻、路面打滑等),小车运动与预期有一点偏差,我们的程序需要尽可能给出合适的控制量速度v,使得优化后小车运动的S-T图和预期S-T图尽量贴合。
小车的预期位置 S r e f [ k ] S_{ref}[k] Sref[k]和预期速度 v r e f [ k ] v_{ref}[k] vref[k],满足如下关系:
s r e f [ k + 1 ] = s r e f [ k ] + v r e f [ k ] d t s_{ref}[k+1]=s_{ref}[k]+v_{ref}[k]dt sref[k+1]=sref[k]+vref[k]dt
经过优化后,小车的位置 s o p t [ k ] s_{opt}[k] sopt[k]和速度 v o p t [ k ] v_{opt}[k] vopt[k],满足如下关系:
s o p t [ k + 1 ] = s o p t [ k ] + v o p t [ k ] d t s_{opt}[k+1]=s_{opt}[k]+v_{opt}[k]dt sopt[k+1]=sopt[k]+vopt[k]dt
令:
x [ k ] = [ s o p t [ k ] − s r e f [ k ] ] u [ k ] = [ v o p t [ k ] − v r e f [ k ] ] A = [ 1 ] B = [ d t ] \begin {align} x[k] & = [s_{opt}[k]-s_{ref}[k]] \nonumber \\ u[k] &= [v_{opt}[k]-v_{ref}[k]] \nonumber \\ A & =[1] \nonumber \\ B &=[dt] \nonumber \end{align} x[k]u[k]AB=[sopt[k]−sref[k]]=[vopt[k]−vref[k]]=[1]=[dt]
则:
x
[
k
+
1
]
=
A
x
[
k
]
+
B
u
[
k
]
x[k+1]=Ax[k]+Bu[k]
x[k+1]=Ax[k]+Bu[k]
咱们的优化目标是:小车运动的S-T图和预期S-T图尽量贴合。因此,小车的位置偏差x,对应的代价权重应该比较大;相反,小车的速度偏差,对应的代价权重应该比较小。代价函数如下:
J
=
∑
k
=
0
N
−
1
(
x
T
[
k
]
Q
x
[
k
]
+
u
T
[
k
]
R
u
[
k
]
)
+
x
T
[
N
]
Q
x
[
N
]
J=\sum_{k=0}^{N-1}(x^T[k]Qx[k]+u^T[k]Ru[k])+x^T[N]Qx[N]
J=k=0∑N−1(xT[k]Qx[k]+uT[k]Ru[k])+xT[N]Qx[N]
这里设置代价权重如下:
Q
=
[
1
]
R
=
[
0.01
]
\begin {align} Q & =[1] \nonumber \\ R & =[0.01] \nonumber \end {align}
QR=[1]=[0.01]
下面是求解代码demo:
#include<iostream>
#include <Eigen/Dense>
template <class T>
void PrintVector(const std::string& prefix, std::vector<T>& vec) {
std::cout << prefix << ": ";
for (int i = 0; i < vec.size(); ++i) {
if (i == 0) {
std::cout << vec[i];
} else {
std::cout << ", " << vec[i];
}
}
std::cout << std::endl;
}
// 状态空间方程
void Dynamic(double dt, int N, const std::vector<Eigen::VectorXd>& u, const Eigen::VectorXd& x_0,
const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, std::vector<Eigen::VectorXd>& x) {
x.clear();
x.push_back(x_0);
for (int i = 0; i < N; ++i) {
x.emplace_back(A*x[i] + B*u[i]);
}
}
// LQR求解P矩阵序列
void LQR(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B,
const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R,
const int N, std::vector<Eigen::MatrixXd>& P) {
P.clear();
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(Q.rows(), Q.cols());
P.push_back(Q);
for (int i = 0; i < N-1; ++i) {
P.push_back(Q+A.transpose()*P[i]*(I+B*R.inverse()*B.transpose()*P[i]).inverse()*A);
}
std::reverse(P.begin(), P.end());
}
int main() {
const int n = 1;
const int m = 1;
const int N = 10;
double dt = 1;
Eigen::VectorXd s_0(n);
s_0 << 0.0; // 预期的当前位置
// 预期速度序列
std::vector<double> v = {5, 1, 3, 9, 2, 2, 5, 1, 6, 2};
std::vector<Eigen::VectorXd> v_ref;
for (int i = 0; i < N; ++i) {
Eigen::VectorXd u(n);
u << v[i];
v_ref.push_back(u);
}
// 确定A、B矩阵,通过状态空间方程,计算预期位置序列
Eigen::MatrixXd A(1, n);
A(0,0) = 1;
Eigen::MatrixXd B(1, m);
B(0,0) = dt;
std::vector<Eigen::VectorXd> s_ref;
Dynamic(dt, N, v_ref, s_0, A, B, s_ref);
PrintVector("s_ref", s_ref);
PrintVector("v_ref", v_ref);
//确定Q、R矩阵,并求解LQR
Eigen::MatrixXd Q(n, n);
Eigen::MatrixXd R(m, m);
Q << 1;
R << 0.01;
std::vector<Eigen::MatrixXd> P;
LQR(A, B, Q, R, N, P);
PrintVector("P", P);
// 计算出优化后的u和x序列
Eigen::VectorXd x_0(n);
x_0 << 1.0; //假设k=0时,当前时刻的小车位置误差为1.0
std::vector<Eigen::VectorXd> u;
std::vector<Eigen::VectorXd> x;
x.push_back(x_0);
for (int i = 0; i < N; ++i) {
u.push_back(-(R+B.transpose()*P[i]*B).inverse()*B.transpose()*P[i]*A*x[i]);
x.emplace_back(A*x[i] + B*u[i]);
}
PrintVector("u", u);
PrintVector("x", x);
// 将优化后的x和u,转化为小车的优化后位置和速度
std::vector<Eigen::VectorXd> s_opt;
std::vector<Eigen::VectorXd> v_opt;
for (int i = 0; i < s_ref.size(); ++i) {
s_opt.push_back(s_ref[i]+x[i]);
}
for (int i = 0; i < v_ref.size(); ++i) {
v_opt.push_back(v_ref[i]+u[i]);
}
PrintVector("s_opt", s_opt);
PrintVector("v_opt", v_opt);
return 0;
}
程序输出结果:
s_ref: 0, 5, 6, 9, 18, 20, 22, 27, 28, 34, 36
v_ref: 5, 1, 3, 9, 2, 2, 5, 1, 6, 2
P: 1.0099, 1.0099, 1.0099, 1.0099, 1.0099, 1.0099, 1.0099, 1.0099, 1.0099, 1
u: -0.990195, -0.00970873, -9.51928e-05, -9.33352e-07, -9.15139e-09, -8.97281e-11, -8.79772e-13, -8.62605e-15, -8.45772e-17, -8.29188e-19
x: 1, 0.00980486, 9.61354e-05, 9.42594e-07, 9.24201e-09, 9.06166e-11, 8.88484e-13, 8.71146e-15, 8.54147e-17, 8.3748e-19, 8.29188e-21
s_opt: 1, 5.0098, 6.0001, 9, 18, 20, 22, 27, 28, 34, 36
v_opt: 4.0098, 0.990291, 2.9999, 9, 2, 2, 5, 1, 6, 2
从输出结果来看,虽然在当前时刻(k=0),小车位置偏差达到了1,但是当下一时刻(k=1时),小车位置偏差立即缩小到0.0098,后续时间步的位置偏差也越来越小,最后无限接近于0。
优化前后S-T图和V-T图对比效果:
如果我们希望小车的整体速度偏差更小,可以调整权重:
Q
=
[
0.01
]
R
=
[
1
]
\begin {align} Q & =[0.01] \nonumber \\ R & =[1] \nonumber \end {align}
QR=[0.01]=[1]
运行程序后的S-T图和V-T图对比效果如下:
从上图可以看出,小车的整体速度跟随更好,位置偏差较大。
09 总结
本文先简要介绍了LQR有关的基础理论知识,然后进行了详细的LQR求解公式推导,最后通过一个简单的demo程序模拟LQR的优化效果。
LQR在自动驾驶领域应用比较多,尤其是在自动驾驶的控制模块用得比较多。另外,LQR和最优控制中的MPC算法较为相似,可以相互结合学习理解。
除了通过拉格朗日乘数法求解LQR,还可以通过哈密尔顿函数求解LQR。
LQR主要用于优化控制问题,需要状态空间方程式线性系统,以及二次型代价函数。对于非线性系统或非二次型代价函数,可以通过iLQR处理。
文章目的在于,记录个人最近的收获,如果有读者感兴趣,那刚好可以共同学习与成长。欢迎留言讨论。
关注微信公众号“程序员小阳”,相互交流更多软件开发技术。