自动驾驶---Control之LQR控制

1 前言

        在前面的系列博客文章中为读者阐述了很多规划相关的知识（可参考下面专栏），本篇博客带领读者朋友们了解控制相关的知识，后续仍会撰写规控相关文档。

        在控制理论的发展过程中，人们逐渐认识到对于线性动态系统的控制，可以通过优化某种性能指标来达到更好的控制效果。而LQR控制就是基于这种思想发展而来的。它通过将系统状态与控制信号之间的关系表示成一种二次型，然后通过最小化这个二次型来求解最优的控制策略。       

        目前的工业领域三大控制器占据着主要位置：PID，LQR以及MPC，本篇博客为读者介绍LQR控制器。

2 LQR建模求解

        LQR（Linear quadratic regulator，线性二次型调节器）是一种基于状态反馈的最优控制策略，其核心在于设计一个状态反馈控制器，使得系统的性能指标达到最优。下面将对LQR控制器进行详细介绍：

2.1 原理与步骤

1. 确定状态方程模型：首先需要确定一个描述系统状态的动力学模型，该模型通常以状态空间的形式给出，用于描述系统在不同状态下的动态行为。
2. 线性化处理：对状态方程进行线性化处理，将其转化为线性系统模型。这是为了简化问题，使得控制器设计更加便捷。
3. 定义目标函数：目标函数通常是系统状态和控制输入的二次型函数，用于评估控制性能的好坏。这个函数的选择会直接影响到控制器的设计效果。
4. 优化目标函数：通过设计状态反馈控制器，使得目标函数取最小值。这个过程中，需要求解一个最优控制问题，通常涉及到Riccati方程的求解。

2.2 详细内容

2.2.1 一般推导

        对于一个闭环系统，状态方程为：

        其中 为状态量， 为控制量， 分别为状态矩阵，控制矩阵。假设，那么上式可以整理为一个新的状态方程：

        为了能够选出最优化的特征值，引入了一个代价函数，考虑两个方面的内容：

• 希望系统达到稳定状态使得偏差最小：惩罚状态量
• 希望控制量较小的情况下也能达到稳定状态：惩罚控制量

        因此代价函数可以写成如下形式：

        其中， 为状态量， 为控制量， 分别为状态权重矩阵，控制权重矩阵。在量产中的调参也主要集中在矩阵上。

        基于上面两个方面的考虑，这里希望  为半正定矩阵来保证；希望 为正定矩阵来保证，该项表示控制所付出的代价。

        将 代入到代价函数种可得，

        为了得到  矩阵，假设存在一个常量矩阵  使得,

        将上式左边微分展开得到，并代入 ，

        由二次型的性质可得，如果保证上式为0，则括号内的值必须为0，即：

        上式中含有矩阵的二次型，不利于计算，因此取 消掉二次项得，

        从上面的公式中，可以看到形式正好是控制理论中的Riccati方程。

2.2.2 离散域推导

        网上看到很多文章的推导结果不太一致，在此处说明，另一种形式的Riccati方程是基于离散时域的推导结果：

        记  时刻代价函数为 ，价值函数为 ，其中 

        这里同样假设存在一个矩阵 （）使得下式成立，

        那么基于离散状态方程 ， 可以写为：

        对于价值函数  取极小值，即

       这里插一个矩阵求导的知识（注意区分：分子布局还是分母布局）

                  

        整理可得，

        由于 都是对称矩阵，可得，

        由  可得，

        将  代入到  中可得，

        最后得到的离散Riccati方程如下：

        在控制理论中，Riccati方程扮演着至关重要的角色，它通常用于描述线性时不变系统的最优控制问题。通过求解Riccati方程，可以找到使得系统状态达到最优的反馈控制律。对于离散时间系统和连续时间系统，Riccati方程的形式和求解方法会有所不同。

        求解连续时间Riccati方程通常涉及数值方法，因为解析解可能不存在或难以找到。常用的数值方法包括迭代法和直接法。迭代法如牛顿法或不动点迭代法，通过迭代更新矩阵 P的值，直到满足收敛条件。直接法则试图通过矩阵分解或变换直接求解方程。

        此部分的代码主要如下：

#include 
#include 
#include 
 
using Eigen::Matrix;
using Eigen::Dynamic;
 
typedef Matrix<double, Dynamic, Dynamic> Mtx;
 
/*
 * @brief Computes the LQR gain matrix (usually denoted K) for a discrete time
 * infinite horizon problem.
 *
 * @param A State matrix of the underlying system
 * @param B Input matrix of the underlying system
 * @param Q Weight matrix penalizing the state
 * @param R Weight matrix penalizing the controls
 * @param N Weight matrix penalizing state / control pairs
 * @param K Pointer to the generated matrix (has to be a double/dynamic size
 * matrix!)
 * @param eps Delta between iterations that determines when convergence is
 * reached
 */
bool SolveLQRMatrix(const Mtx &A, const Mtx &B, const Mtx &Q, const Mtx &R,
                    const Mtx &N, Mtx *K, double eps = 1e-15) {
  // check if dimensions are compatible
  if (A.rows() != A.cols() || B.rows() != A.rows() || Q.rows() != Q.cols() ||
      Q.rows() != A.rows() || R.rows() != R.cols() || R.rows() != B.cols() ||
      N.rows() != A.rows() || N.cols() != B.cols()) {
    std::cout << "One or more matrices have incompatible dimensions. Aborting."
              << std::endl;
    return false;
  }
 
  // precompute as much as possible
  Mtx B_T = B.transpose();
  Mtx Acal = A - B * R.inverse() * N.transpose();
  Mtx Acal_T = Acal.transpose();
  Mtx Qcal = Q - N * R.inverse() * N.transpose();
 
  // initialize P with Q
  Mtx P = Q;
 
  // iterate until P converges
  unsigned int numIterations = 0;
  Mtx Pold = P;
  while (true) {
    numIterations++;
    // compute new P
    P = Acal_T * P * Acal -
        Acal_T * P * B * (R + B_T * P * B).inverse() * B_T * P * Acal + Qcal;
 
    // update delta
    Mtx delta = P - Pold;
    if (fabs(delta.maxCoeff()) < eps) {
      std::cout << "Number of iterations until convergence: " << numIterations
                << std::endl;
      break;
    }
    Pold = P;
  }
 
  // compute K from P
  *K = (R + B_T * P * B).inverse() * (B_T * P * A + N.transpose());
 
  return true;
}

3 车辆LQR控制

        本小节以汽车运动学模型为例，车辆的运动模型如下图所示，本小节理论部分主要参考龚建伟老师著作《无人驾驶车辆模型预测控制》，基于汽车动力学模型的推导可参考Rajamani的著作《Vehicle Dynamics And Control》。

3.1 运动学模型        

        可得车辆的运动学模型为：

eq?" role="presentation">eq?$eq?$

        可将整个系统看作一个输入为 和  的控制系统。其一般形式为：

        在自动驾驶轨迹跟踪中，通常以跟踪偏差作为输入，因此可得，

  为了能够将该模型应用于控制器的设计，最终进行离散化处理，最终可以写成如下形式： 

         建立运动学模型的代码如下（只保留重要部分）：

void Model::GetModel(float curvature, Eigen::Matrix<double, 3, 1> state_actual){
    //target delta is variable because of curve path.
    float target_delta = atan(wheel_base*curvature); //dphi/dt = V*tan(delta)/L;  w = V*tan(delta)/L
    /*****************************************************/
    // A
    Matrix_->A(0, 0) = 1;
    Matri>A(1, 0) = 0;
    Matrix_->A(1, 1) = 1;
    Matrix_->Ax_->A(0, 1) = 0;
    Matrix_->A(0, 2) = -target_velocity*sin(state_actual(2,0))*dt_;
    Matrix_-(1, 2) = target_velocity*cos(state_actual(2,0))*dt_;
    Matrix_->A(2, 0) = 0;
    Matrix_->A(2, 1) = 0;
    Matrix_->A(2, 2) = 1;
    /*****************************************************/
    // B
    Matrix_->B(0, 0) = cos(state_actual(2,0))*dt_;
    Matrix_->B(0, 1) = 0;
    Matrix_->B(1, 0) = sin(state_actual(2,0))*dt_;
    Matrix_->B(1, 1) = 0;
    Matrix_->B(2, 0) = tan(target_delta)*dt_/wheel_base;
    Matrix_->B(2, 1) = target_velocity*dt_/(wheel_base*(cos(target_delta)*cos(target_delta)));
}

3.2 求解

        再结合上面一节LQR求解代码如下（非完整）：

void LQRControl::LQRController(){
        //near point
        int index = FindMatchedPoint(actual_pos.x, actual_pos.y);
 
        x_ref_(0, 0) = planning_path[index].position.x;
        x_ref_(1, 0) = planning_path[index].position.y;
        x_ref_(2, 0) = planning_path[index].position.yaw;
        x_act_(0, 0) = actual_pos.x;
        x_act_(1, 0) = actual_pos.y;
        x_act_(2, 0) = actual_pos.yaw;
        /***********compute curvature of target point********************************/
        curvature = ComputeCurvature(index);
        // get model matrix A and B
        model_.GetModel(curvature, x_act_);
        matrix_c.A = model_.Matrix_->A;
        matrix_c.B = model_.Matrix_->B;
        // Q(3x3): positive semi-definite matrix, penalizing the state
        matrix_c.Q(0, 0) = 1;
        matrix_c.Q(0, 0) = 10;
        matrix_c.Q(0, 0) = 1;
        // R(2x2): positive definite matrix,  penalizing the controls
        matrix_c.R(0, 0) = 100;
        matrix_c.R(1, 1) = 100;
        // N(3x2):  penalizing the state/control pairs
        matrix_c.N = Eigen::Matrix<double, 3, 2>::Zero();
        /****************************************************************************/
        // Solve the Riccati Equation
        if (!SolveLQRMatrix(matrix_c.A, matrix_c.B, matrix_c.Q, matrix_c.R, matrix_c.N, &matrix_k)){
            std::cout << "Compute matrix K failed!" << std::endl;
        }else{
            std::cout << matrix_k << std::endl;
        }
}

4 特点与应用

（1） 特点

1. 最优性：LQR控制器通过最小化目标函数，使得系统性能达到最优。这种最优性是基于给定的性能指标进行衡量的，因此可以根据实际需求进行灵活调整。
2. 适应性：LQR控制器可以适应不同的系统模型和性能指标，通过调整目标函数和控制器参数，可以实现对不同系统的有效控制。
3. 稳定性：通过设计合适的控制器参数，LQR控制器可以保证系统的稳定性，避免出现过大的状态波动或控制误差。

（2）应用领域

        LQR控制器广泛应用于控制工程、机器人学、飞行器控制以及其他控制领域。在这些领域中，LQR控制器能够有效地处理线性系统的控制问题，实现精确、稳定的控制效果。

5 总结

        综上所述，LQR控制器是一种基于状态反馈的最优控制策略，具有最优性、适应性和稳定性等特点。它在多个领域都有广泛的应用，并且与其他控制器相比具有优势。在实际应用中，可以根据具体需求选择和设计合适的LQR控制器，以实现精确、稳定的控制效果。