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

原创已于 2024-08-04 11:39:57 修改 · 6.3k 阅读

54 ·

CC 4.0 BY-SA版权

文章标签：

#机器人 #自动驾驶

于 2024-07-28 23:12:46 首次发布

PnC 专栏收录该内容

1 篇文章

订阅专栏

本文介绍最优控制中LQR算法（Linear Quadratic Regulator）的离散版本推导求解过程，即推导Riccati方程，而后计算出最优控制量，最后通过一个程序demo展示LQR算法效果。
本文主要内容如下：

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

00 线性二次型调节

线性二次型调节器（Linear Quadratic Regulator，缩写为LQR）是一种经典的最优控制算法。对于线性系统，如果它的代价函数是二次型函数，则最优控制量可以通过LQR求解得到。
下面是《现代控制理论》中对线性二次型和状态调节器的描述：

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

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

01 线性状态空间方程

对于线性系统，都可以通过线性状态空间方程进行描述，离散的线性状态空间方程如下：
$\begin{equation} x_{k+1}=Ax_k+Bu_k \end{equation}$
其中：

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

1.1 举个例子

一个小车在一条直线上做变速直线运动。
在这里插入图片描述
如果我们可以通过控制速度来控制小车的运动，并且速度变化的时间间隔为dt，则可以得到关于位置p和速度v之间的关系，如下：
$\begin{equation} p_{k+1} = p_k + v_kdt \nonumber \end{equation}$
上面的表达式就是此系统的线性状态空间方程，其中：
$n=1;m=1;x_k=[p_k];u_k=[v_k];A=[1];B=[dt]$

02 二次型代价函数

离散型LQR代价函数如下：
$J=\sum_{k=0}^{N-1}{(x_k^TQx_k+u_k^TRu_k)+x_N^TQx_N}$
可以看出该代价函数是一个关于（N+1）个x和N个u的二次型函数。其中：

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

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

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

03 反馈控制

反馈控制是指在某一行动和任务完成之后，将实际结果进行比较，从而对下一步行动的进行产生影响，起到控制的作用。下图是一个线性二次型最优反馈系统。
在这里插入图片描述
在上图的系统中，控制量u是通过对系统的结果状态x与一个最优反馈控制矩阵K相作用得到的，离散化的关系式如下：
$u_k=-K_kx_k$

04 向量求导

这里的自变量都是向量，对于向量求导，有如下规则：
$\frac {\partial (x^TA)}{\partial x}=A \\ \frac {\partial(Ax)}{\partial x}=A^T \\ \frac {\partial(x^TAx)} {\partial x}=(A+A^T)x$
当矩阵A是对称矩阵时：
$\frac {\partial(x^TAx)} {\partial x}=2Ax$

05 带约束的最优化问题

前面给出了LQR有关的基础数学表达式，经过整理会得到一个如下的最优化问题：
$\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}$
以上是一个带约束的最优化问题，以前面的线性状态空间方程为约束条件，以二次代价函数为目标函数，求解目标函数取最小值时的状态量x和控制量u。

在高等数学中，可以通过拉格朗日乘数法求解带等式约束的最优化问题。这里先构造一个拉格朗日函数：
$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=\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关于x, u和λ的一阶偏导为0时的解，即求解出了原代价函数J在约束条件下的极小值点。

06 求极值点

对公式（2）进行求导：
$\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}$
==>
$\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}$
当k=N-1时，将公式（4）带入公式（5），得到：
$\begin{equation} u_{N-1}=-\frac 1 2 R^{-1}B^T2QX_N=-R^{-1}B^TQx_N \end {equation}$
再将公式（7）带入公式（6），得到：
$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}$
再将公式（8）带入公式（4），得到：
$\begin{equation} \lambda_N=2Q(I+BR^{-1}B^TQ)^{-1}Ax_{N-1} \end{equation}$
再将公式（9）带入公式（3），得到：
$\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}$
对比公式（4）和公式（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}$
令：
$\begin{equation} P_{N-1}=Q+A^TQ(I+BR^{-1}B^TQ)^{-1}A \nonumber \end{equation}$
则公式（10）可以写作：
$\begin{equation} \lambda_{N-1} = 2P_{N-1}x_{N-1} \end{equation}$
这样可以明显看出，公式（11）和公式（4）结构一样。如果我们令k=N-2，用公式（11）代替公式（4），然后重复前面的计算过程，可以得到如下表达式：
$P_{N-2}=Q+A^TP_{N-1}(I+BR^{-1}B^TP_{N-1})^{-1}A$
如果令 $P_N=Q$ ，可以得到一个如下的递推关系：
$\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}$
这里的公式（12）就是离散化的Ricatte方程。

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

结合前面的反馈控制关系式，可以得到：
$\begin {equation} K_k=(R+B^TP_{k+1}B)^{-1}B^TP_{k+1}A, \quad k=0,1,...,N-1 \end {equation}$

07 LQR求解步骤

LQR设计和求解步骤如下：

根据系统特性，确定系统矩阵A，和控制矩阵B；根据优化的目标，确定状态权重矩阵Q和控制权重矩阵R，每一时间步对应的矩阵Q和矩阵R是可以不一样的；测量出系统当前状态 $x_0$ 。
将矩阵A,B,Q,R带入公式（12）Ricatte方程，求出矩阵P的序列。
将矩阵P的序列带入公式（14），求出反馈控制矩阵K的序列。
将反馈控制矩阵 $K_0$ 和系统当前状态 $x_0$ ，带入公式 $u_0=-K_0x_0$ ，可以得到当前时刻的最优控制量 $u_0$ 。
如果需要求解剩余时间步的控制量，则需要通过状态空间方程（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 {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 + 1] = A x [k] + B u [k]$
咱们的优化目标是：小车运动的S-T图和预期S-T图尽量贴合。因此，小车的位置偏差x，对应的代价权重应该比较大；相反，小车的速度偏差，对应的代价权重应该比较小。代价函数如下：
$J=\sum_{k=0}^{N-1}(x^T[k]Qx[k]+u^T[k]Ru[k])+x^T[N]Qx[N]$
这里设置代价权重如下：
$\begin {align} Q & =[1] \nonumber \\ R & =[0.01] \nonumber \end {align}$

下面是求解代码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