【最优控制】LQR求解公式推导

大家好，欢迎来到IT知识分享网。

1. 线性二次型调节
2. 线性状态空间方程
3. 二次型代价函数
4. 反馈控制
5. 向量求导
6. 带约束的最优化问题
7. 求极值点
8. LQR求解步骤
9. 程序demo
10. 总结

00 线性二次型调节

如果系统是线性的，性能泛函是状态变量（或/和）控制变量的二次型函数的积分， 则这样的最优控制问题称为线性二次型最优控制问题。简称线性二次型。 --《现代控制理论》 

状态调节器的任务在于，当系统状态由于任何原因偏离了平衡状态时，能在不消耗 过多能量的情况下，保持系统状态各个分量仍接近于平衡状态。 --《现代控制理论》 

01 线性状态空间方程

• $x_{k+1}$表示第k步的系统状态向量，是一个n维向量；
• $x_k$表示第k+1步的系统状态向量，是一个n维向量；
• $u_k$表示第k步的输入（或控制）向量，是一个m维向量；
• $A$被称作系统矩阵，是一个n*n方阵；
• $B$被称作控制矩阵，是一个n*m的矩阵。

1.1 举个例子

02 二次型代价函数

• Q表示状态权重方阵，大小为n*n，半正定矩阵，一般是对称矩阵。
• R表示控制权重方阵，大小为m*m，正定矩阵，一般是对称矩阵。

需要注意的是，这里关于x的项比关于u的项多一项，这是由于状态空间方程（1）导致的。

根据优化目标的不同，我们可以使用不同的Q矩阵和R矩阵。

03 反馈控制

04 向量求导

05 带约束的最优化问题

06 求极值点

再将公式（6）带入公式（7），并且将其中的N-1看作k，N看作k+1，Q看作$P_{k+1}$可以得到：
$$\begin{array}{rl}{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,\dots ,N-1\end{array}$$
在通过公式（12）求得$P_{k+1}$的情况下，再使用公式（13），就可以求解出每一步的最优控制量u。

07 LQR求解步骤

LQR设计和求解步骤如下：

1. 根据系统特性，确定系统矩阵A，和控制矩阵B；根据优化的目标，确定状态权重矩阵Q和控制权重矩阵R，每一时间步对应的矩阵Q和矩阵R是可以不一样的；测量出系统当前状态$x_0$。
2. 将矩阵A,B,Q,R带入公式（12）Ricatte方程，求出矩阵P的序列。
3. 将矩阵P的序列带入公式（14），求出反馈控制矩阵K的序列。
4. 将反馈控制矩阵$K_0$和系统当前状态$x_0$，带入公式$u_0=-K_0{x}_0$，可以得到当前时刻的最优控制量$u_0$。
5. 如果需要求解剩余时间步的控制量，则需要通过状态空间方程（1），求出下一时刻的系统状态$x_{k+1}$，再将反馈控制矩阵$K_{k+1}$和系统当前状态$x_{k+1}$，带入公式$u_{k+1}=-K_{k+1}{x}_{k+1}$，可以得到下一时刻的最优控制量$u_{k+1}$。这里k分别取0,1,…N-2，依次求解。

08 程序demo

这里用一个demo演示LQR的控制跟踪效果：还是一个小车在一条直线上做变速直线运动，程序上游模块给出了一个预期的小车S-T图和V-T图。但由于外界环境影响（比如风阻、路面打滑等），小车运动与预期有一点偏差，我们的程序需要尽可能给出合适的控制量速度v，使得优化后小车运动的S-T图和预期S-T图尽量贴合。

小车的预期位置$S_{ref}[k]$和预期速度$v_{ref}[k]$，满足如下关系：

$$s_{ref}[k+1]=s_{ref}[k]+v_{ref}[k]dt$$

经过优化后，小车的位置$s_{opt}[k]$和速度$v_{opt}[k]$，满足如下关系：

$$s_{opt}[k+1]=s_{opt}[k]+v_{opt}[k]dt$$

令：

$$\begin{array}{rl}x[k]& =[s_{opt}[k]-s_{ref}[k]]\\ u[k]& =[v_{opt}[k]-v_{ref}[k]]\\ A& =[1]\\ B& =[dt]\end{array}$$

下面是求解代码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., -0.00, -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.00, 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., 2.9999, 9, 2, 2, 5, 1, 6, 2 

从输出结果来看，虽然在当前时刻（k=0）,小车位置偏差达到了1，但是当下一时刻（k=1时），小车位置偏差立即缩小到0.0098，后续时间步的位置偏差也越来越小，最后无限接近于0。

优化前后S-T图和V-T图对比效果：

如果我们希望小车的整体速度偏差更小，可以调整权重：
$$\begin{array}{rl}Q& =[0.01]\\ R& =[1]\end{array}$$
运行程序后的S-T图和V-T图对比效果如下：

从上图可以看出，小车的整体速度跟随更好，位置偏差较大。

09 总结

本文先简要介绍了LQR有关的基础理论知识，然后进行了详细的LQR求解公式推导，最后通过一个简单的demo程序模拟LQR的优化效果。

LQR在自动驾驶领域应用比较多，尤其是在自动驾驶的控制模块用得比较多。另外，LQR和最优控制中的MPC算法较为相似，可以相互结合学习理解。

除了通过拉格朗日乘数法求解LQR，还可以通过哈密尔顿函数求解LQR。

LQR主要用于优化控制问题，需要状态空间方程式线性系统，以及二次型代价函数。对于非线性系统或非二次型代价函数，可以通过iLQR处理。