LQR横向控制及融合PID纵向控制C++实现

简介
一、现代控制理论
- 1.1 经典控制理论和现代控制理论的区别
- 1.2 全状态反馈控制系统
二、LQR控制器
- 2.1 连续时间
- - 2.1.1 Q、R矩阵的选取
  - 2.1.2 推导过程
  - 2.1.3 连续时间下的LQR算法步骤
- 2.2 离散时间
- - 2.2.1 连续LQR和离散LQR的区别
  - 2.2.2离散时间下的LQR算法步骤
三、LQR实现自动驾驶轨迹跟踪（C++实现）
- 3.1 车辆运动学模型
- 3.2 C++代码实现(恒速、横向控制，控制量只有前轮转角）
- - 3.2.1 车辆运动学模型（离散状态空间方程）
  - 3.2.2 参考轨迹
  - 3.2.3 LQR求解
  - 3.2.4 主函数
  - 3.2.4 结果
- 3.3 LQR+PID（初始速度为0 ，目标速度为4m/s）
- - 3.3.1 PID控制器
  - 3.3.2 主函数（LQR+PID）
四、总结

简介

本篇文章主要介绍了自动驾驶或者机器人的底盘运动控制算法LQR（线性二次调节器）的理论推导，以及在ubuntu系统下，基于C++、Eigen库、matplotlib库，实现了对给定轨迹的LQR跟踪控制，分别展示了在恒速下（控制量为前轮转角）通过LQR对前轮转角的横向控制，以及在给定初始速度下，通过LQR对前轮转角的横向控制，P控制器施加油门的纵向控制（调节加速度）。

本人Git仓库地址：LQR

一、现代控制理论

1.1 经典控制理论和现代控制理论的区别

经典控制理论：只能将系统的可测量输出作为反馈信号，是输出反馈。
现代控制理论：将系统的可测量输出和系统内部的状态量作为反馈信号，是输出反馈+状态反馈。

1.2 全状态反馈控制系统

线性二次问题： 当一个系统是线性系统，并且系统的性能指标（目标函数）是状态量和控制变量的二次型函数，这样的最优控制问题是线性二次问题。

根据线性系统定常连续系统的状态描述可以得到其一般性的状态空间描述框图，如下图所示：
在这里插入图片描述
控制系统的状态空间方程和输出表达式为：

其中x是系统的状态量，u是系统的控制量（输入量），实际算法中，一般都由离散误差状态空间方程来表示，x和u都表示为偏差量。
其中A是系统矩阵，B是控制矩阵，两者均为常数矩阵且只由系统本身的参数决定，C是输出矩阵，D大多数情况下为0。

最优控制：配置全状态反馈控制器u = -Kx。

在添加了线性状态反馈控制器K之后，系统由开环系统变为了闭环系统，K是最优控制规律，是状态量的线性表示，构成闭环控制。

反馈系统稳定性的充要条件是系统闭环传递函数的所有极点均有负实部，即均在复频域S平面的左侧。

二、LQR控制器

在自动驾驶车辆控制中，期望的系统响应特性有2点：

（1）车辆对参考轨迹的跟踪误差尽可能小，尽量去贴合参考轨迹，使误差稳定接近于0。
（2）控制输入量（前轮转角、加速度）尽可能小，使系统的能量耗散尽可能小。

最优控制就是在保证控制系统顺利执行的前提下，尽可能的去达到我们所期待的目标，比如综合考虑上述的状态量偏差和控制量输入；如果系统能够用线性微分方程来表示，并且其代价函数是二次型的形式，那么这类问题就是线性二次问题（LQ问题）；此类问题的解就是线性二次调节器，简称LQR。

2.1 连续时间

LQR的目标就是找到一组控制量u，使得状态量能够快速稳定地去贴合参考量，即偏差为0，而且希望不要有太大的能耗，故使控制量u尽可能小。

这是一个典型的多目标优化最优控制问题，选取代价函数为：
在这里插入图片描述
这里的Q和R分别是状态加权矩阵和控制加权矩阵，Q是半正定矩阵，R是正定矩阵，均是对角矩阵。

半正定矩阵：对于所有的非零向量𝑥，都有x TAx≥0。
正定矩阵：对于所有的非零向量𝑥，都有x TAx>0。

正定矩阵通常用于优化问题，作为Hessian矩阵（二阶导数矩阵）来确保函数的局部最小值。
半正定矩阵在优化问题中也很重要，但它们可能表示鞍点或局部最大值的情况。

随着时间的推移，代价函数逐渐达到最小值，状态量x和控制量u都趋近于0（这里的状态量和控制量都由偏差量来表示），系统达到稳态。

2.1.1 Q、R矩阵的选取

Q为半正定的状态加权矩阵，R为正定的控制加权矩阵，通常均为对角阵。Q越大，表示希望系统的状态量能够尽快地贴合参考量，R越大，说明希望控制输入尽可能小，意味这系统的状态衰减变慢。比如， Q 11 选取较大的值，会让 x 1 很快的衰减到0；所以，Q 、 R 的选取，要综合看具体的实际应用场景来调节。

2.1.2 推导过程

LQR的求解目的是为了得到状态反馈增益K，从而得到控制量与状态量的关系u = -Kx.
在这里插入图片描述
得到Riccati方程：

求解得到P：

求解得到状态反馈矩阵K：

最终求解得到最优控制u：

2.1.3 连续时间下的LQR算法步骤

选取状态加权矩阵Q和控制加权矩阵R（分别满足半正定和正定的对角阵）。
求解Riccati方程得到矩阵P。
根据P计算状态反馈矩阵K。
最终得到最优控制u = -Kx。

2.2 离散时间

2.2.1 连续LQR和离散LQR的区别

连续LQR: 在连续时间域内定义，适用于系统状态随时间连续变化的情况。
离散LQR:在离散时间域内定义，适用于系统状态在离散时间点上更新的情况。

离散LQR使用的是离散时间状态空间方程：
在这里插入图片描述
其中k是离散时间步。

离散LQR的代价函数是关于离散时间步的求和：
在这里插入图片描述
求解离散LQR的方法有最小二乘法和动态规划算法。详情见文章链接

从后往前推导可以得到每一个时间步的最优控制律：
在这里插入图片描述
其中矩阵P会依据下式并且配合初始值 PN = Q 进行迭代求解：

上式就是离散时间的代数Riccati方程。P的稳态解和N趋近无限大时的无限时间问题有关，可以将上式反复迭代求解直到收敛，来求得P的稳态解，一般迭代几十步就可以得到P的稳态解。

2.2.2离散时间下的LQR算法步骤

确定迭代范围N
设置迭代初始值PN=Qf，其中Qf = Q
循环迭代，从后往前t = N，…，1
从t = 0，…,N-1，循坏计算反馈状态反馈矩阵K：
最终得到最优控制量 ut = -Kt * Xt。

三、LQR实现自动驾驶轨迹跟踪（C++实现）

3.1 车辆运动学模型

在此参考以车辆后轴中心为车辆转弯中心的单车运动学模型：
在这里插入图片描述
状态空间方程：

状态误差量也可以构成线性状态空间方程：

对上面的线性状态误差空间方程进行离散化，对上面等式进行向前欧拉离散化：

得到下一个时间步的状态量为：

其中T为采样步长，I为单位矩阵；X和u均为误差量。

3.2 C++代码实现(恒速、横向控制，控制量只有前轮转角）

环境：ubuntu20.04
依赖库：Python、Eigen、matplotlib

   sudo apt-get install libeigen3-dev    //Eigen库安装

// cmakelists
 find_package(Eigen3 REQUIRED)
 include_directories(${EIGEN3_INCLUDE_DIR})

3.2.1 车辆运动学模型（离散状态空间方程）

包括状态量更新函数、状态空间方程A（系统矩阵）、B（控制矩阵）计算函数。
KinematicModel.cpp：

#include "KinematicModel.h"

KinematicModel::KinematicModel(double x, double y, double psi, double v, double L, double dt) : x(x), y(y), psi(psi),
                                                                                                v(v), L(L), dt(dt) {}
void KinematicModel::updateState(double a, double delta)
{
    x = x + v * cos(psi) * dt;
    y = y + v * sin(psi) * dt;
    psi = psi + v / L * tan(delta) * dt;
    v = v + a * dt;
}

vector<double> KinematicModel::getState()
{
    return {x, y, psi, v};
}
// 将模型离散化后的状态误差空间表达
vector<MatrixXd> KinematicModel::stateSpace(double ref_delta, double ref_yaw)
{
    MatrixXd A(3, 3), B(3, 2);
    A << 1.0, 0.0, -v * sin(ref_yaw) * dt,
         0.0, 1.0, v * cos(ref_yaw) * dt,
         0.0, 0.0, 1.0;
    B << cos(ref_yaw) * dt, 0,
         sin(ref_yaw) * dt, 0,
         tan(ref_delta) * dt / L, v * dt / (L * cos(ref_delta) * cos(ref_delta));
    return {A, B};
}

3.2.2 参考轨迹

包括参考轨迹的曲线表达、跟踪误差计算函数和角度归一化函数。
Reference_path.cpp:

#include "Reference_path.h"

ReferencePath::ReferencePath()
{
    ref_path = vector<vector<double>>(1000, vector<double>(4));
    // 生成参考轨迹
    for (int i = 0; i < 1000; i++)
    {
        ref_path[i][0] = 0.1 * i;
        ref_path[i][1] = 2 * sin(ref_path[i][0] / 3.0);

        ref_x.push_back(ref_path[i][0]);
        ref_y.push_back(ref_path[i][1]);
    }
    double dx, dy, ddx, ddy;
    for (int i = 0; i < ref_path.size(); i++)
    {
        if (i == 0) {
            dx = ref_path[i + 1][0] - ref_path[i][0];
            dy = ref_path[i + 1][1] - ref_path[i][1];
            ddx = ref_path[2][0] + ref_path[0][0] - 2 * ref_path[1][0];
            ddy = ref_path[2][1] + ref_path[0][1] - 2 * ref_path[1][1];
        } else if (i == ref_path.size() - 1) {
            dx = ref_path[i][0] - ref_path[i- 1][0];
            dy = ref_path[i][1] - ref_path[i- 1][1];
            ddx = ref_path[i][0] + ref_path[i- 2][0] - 2 * ref_path[i - 1][0];
            ddy = ref_path[i][1] + ref_path[i - 2][1] - 2 * ref_path[i - 1][1];
        } else {
            dx = ref_path[i + 1][0] - ref_path[i][0];
            dy = ref_path[i + 1][1] - ref_path[i][1];
            ddx = ref_path[i + 1][0] + ref_path[i - 1][0] - 2 * ref_path[i][0];
            ddy = ref_path[i + 1][1] + ref_path[i - 1][1] - 2 * ref_path[i][1];
        }
        ref_path[i][2] = atan2(dy, dx);
        //计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2).
        ref_path[i][3] = (ddy * dx - ddx * dy) / pow((dx * dx + dy * dy), 3.0 / 2); // k计算
    }
}
// 计算跟踪误差
vector<double> ReferencePath::calcTrackError(vector<double> robot_state)
{
    double x = robot_state[0], y = robot_state[1];
    vector<double> d_x(ref_path.size()), d_y(ref_path.size()), d(ref_path.size());
    for (int i = 0; i < ref_path.size(); i++)
    {
        d_x[i]=ref_path[i][0]-x;
        d_y[i]=ref_path[i][1]-y;
        d[i] = sqrt(d_x[i]*d_x[i]+d_y[i]*d_y[i]);
    }
    double min_index = min_element(d.begin(), d.end()) - d.begin();
    double yaw = ref_path[min_index][2];
    double k = ref_path[min_index][3];
    double angle = normalizeAngle(yaw - atan2(d_y[min_index], d_x[min_index]));
    double error = d[min_index];
    if (angle < 0) error *= -1;
    return {error, k, yaw, min_index};
}

// 角度归一化到 -PI到PI
double ReferencePath::normalizeAngle(double angle)
{
    while (angle > PI)
    {
        angle -= 2 * PI;
    }
    while (angle < -PI)
    {
        angle += 2 * PI;
    }
    return angle;
}

3.2.3 LQR求解

包括黎卡提方程迭代求解P函数和控制量u = -Kx计算函数。
LQR.cpp:

#include "LQR.h"

LQR::LQR(int n) : N(n) {}
// 解代数里卡提方程
MatrixXd LQR::calRicatti(MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R)
{
    MatrixXd Qf = Q;
    MatrixXd P_old = Qf;
    MatrixXd P_new;
    // P _{new} =Q+A ^TPA−A ^TPB(R+B ^T PB) ^{−1}B ^TPA
    for (int i = 0; i < N; i++)
    {
        P_new = Q + A.transpose() * P_old * A - A.transpose() * P_old * B * (R + B.transpose() * P_old * B).inverse() * B.transpose() * P_old * A;
        if ((P_new - P_old).maxCoeff() < EPS && (P_old - P_new).maxCoeff() < EPS) break;
        P_old = P_new;
    }
    return P_new;
}

double LQR::LQRControl(vector<double> robot_state, vector<vector<double>> ref_path, double s0, MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R)
{
    MatrixXd X(3, 1);
    X << robot_state[0] - ref_path[s0][0],
         robot_state[1] - ref_path[s0][1],
         robot_state[2] - ref_path[s0][2];
    MatrixXd P = calRicatti(A, B, Q, R);
    // K=(R + B^TP_{new}B)^{-1}B^TP_{new}A
    MatrixXd K = (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;
    MatrixXd u = -K * X; // [v - ref_v, delta - ref_delta]
    return u(1, 0);
}

3.2.4 主函数

main.cpp:

#include "LQR.h"
#include "Reference_path.h"
#include "KinematicModel.h"
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;

int main(){
    double dt = 0.1;
    double L = 2.0;
    double v = 2.0;
    double x_0 = 0.0;
    double y_0 = 1.0;
    double psi_0 = 0.0;

    int N = 500;
    MatrixXd Q(3,3);
    Q<<3,0,0,
       0,3,0,
       0,0,3;

    MatrixXd R(2,2);
    R<<2.0,0.0,
        0.0,2.0;
    

    // 保存机器人移动过程中的轨迹
    vector<double>x_, y_;
    MyReference_path referencePath;
    KinematicModel robot(x_0,y_0,psi_0,v,L,dt);

    LQRControl robot_motion_LQR(N);//求解Riccati矩阵 P 时 迭代N次

    vector<double> robot_state;


    for(int i = 0;i<700;i++){
        plt::clf();
        robot_state = robot.getState();// {x , y , psi , v};
        vector<double>one_trial = referencePath.calcTrackError(robot_state);
        double k = one_trial[1];
        double ref_yaw = one_trial[2];// 预瞄点曲率
        double s0 = one_trial[3];  // min_distance_index

        double ref_delta = atan2(L*k,1);  // 求出参考轨迹上的预瞄点的航向角
        vector<MatrixXd>state_space = robot.stateSpace(ref_delta,ref_yaw);   //{A,B} 矩阵


     // 传入机器人状态、参考轨迹、min_index, A , B , Q, R     求解得到前轮转角的增量
        double delta = robot_motion_LQR.lqrControl(robot_state, referencePath.refer_path, s0, state_space[0], state_space[1], Q, R);// 前轮转角
        delta += ref_delta;

        robot.updateState(0,delta);   // 加速度设为0，恒速

        cout<<" speed :"<< robot.v<<" m/s "<<endl;

        x_.push_back(robot.x);
        y_.push_back(robot.y);
        
        // 参考轨迹
        plt::plot(referencePath.refer_x,referencePath.refer_y,"b");
        plt::grid(true);
        plt::ylim(-5,5);

        //机器人轨迹
        plt::plot(x_, y_,"r");
        plt::pause(0.01);
    }


     const char* filename = "./LQR.png";
     plt::save(filename);
     plt::show();
     return 0;
  
}

3.2.4 结果

在这里插入图片描述
恒速设置为2m/s，可以观察到在此低速恒速状态下的跟踪误差效果比较好，但是车辆速度较慢。于是可以在此基础上使用PID控制器来调节加速度，如下文所示。

3.3 LQR+PID（初始速度为0 ，目标速度为4m/s）

3.3.1 PID控制器

包括了PID三个参数的设置、目标速度的设置、速度上下限的设置函数，PID求解控制量加速度函数。

pid_control.cpp:

#include "pid_control.h"

PID_controller::PID_controller(double Kp, double Ki, double Kd, double target,
                               double upper, double lower)
    : Kp(Kp), Ki(Ki), Kd(Kd), target(target), upper(upper), lower(lower) {}

void PID_controller::setTarget(double target) { this->target = target; }

void PID_controller::setK(double Kp, double Ki, double Kd) {
  this->Kp = Kp;
  this->Ki = Ki;
  this->Kd = Kd;
}

void PID_controller::setBound(double upper, double lower) {
  this->upper = upper;
  this->lower = lower;
}

double PID_controller::calOutput(
    double state) { // 控制量加速度输出 ， 传入机器人当前速度 ， 输出加速度
                    // （根据当前速度和目标速度的差值）
  this->error = target - state;
  double u = error * Kp + sum_error * Ki + (error - pre_error) * Kd;
  if (u < lower)
    u = lower;
  else if (u > upper)
    u = upper;
  this->pre_error = this->error;
  this->sum_error = sum_error + error;
  return u;
}

void PID_controller::reset() {
  error = 0;
  pre_error = 0;
  sum_error = 0;
}

void PID_controller::setSumError(double sum_error) {
  this->sum_error = sum_error;
}

3.3.2 主函数（LQR+PID）

main_pid.cpp:

#include "LQR.h"
#include "Reference_path.h"
#include "KinematicModel.h"
#include "matplotlibcpp.h"
#include "pid_control.h"


namespace plt = matplotlibcpp;

int main(){
    double dt = 0.1;
    double L = 2.0;
    double v = 0;
    double x_0 = 0.0;
    double y_0 = 1.0;
    double psi_0 = 0.0;
    double target_speed = 4.0;
    double upper_speed = 15.0/3.6;

    int N = 500;
    MatrixXd Q(3,3);
    Q<<10,0,0,
       0,10,0,
       0,0,10;

    MatrixXd R(2,2);
    R<<3.0,0.0,
        0.0,3.0;
    

    // 保存机器人移动过程中的轨迹
    vector<double>x_, y_;
    MyReference_path referencePath;
    KinematicModel robot(x_0,y_0,psi_0,v,L,dt);

    LQRControl robot_motion_LQR(N);//求解Riccati矩阵 P 时 迭代N次
    PID_controller PID(3,0.001,30 ,target_speed , upper_speed , 0.0 );

    vector<double> robot_state;


    for(int i = 0;i<350;i++){
        plt::clf();
        robot_state = robot.getState();// {x , y , psi , v};
        vector<double>one_trial = referencePath.calcTrackError(robot_state);
        double k = one_trial[1];
        double ref_yaw = one_trial[2];// 预瞄点曲率
        double s0 = one_trial[3];  // min_distance_index

        double ref_delta = atan2(L*k,1);  // 求出参考轨迹上的预瞄点的航向角
        vector<MatrixXd>state_space = robot.stateSpace(ref_delta,ref_yaw);   //{A,B} 矩阵


     // 传入机器人状态、参考轨迹、min_index, A , B , Q, R     求解得到前轮转角的增量
        double delta = robot_motion_LQR.lqrControl(robot_state, referencePath.refer_path, s0, state_space[0], state_space[1], Q, R);// 前轮转角
        delta += ref_delta;
        
        double a = PID.calOutput(robot.v);
        robot.updateState(a,delta);   // 加速度由pid控制器调节

        cout<<" speed :"<< robot.v<<" m/s "<<endl;

        x_.push_back(robot.x);
        y_.push_back(robot.y);
        
        // 参考轨迹
        plt::plot(referencePath.refer_x,referencePath.refer_y,"b");
        plt::grid(true);
        plt::ylim(-5,5);

        //机器人轨迹
        plt::plot(x_, y_,"r");
        plt::pause(0.01);
    }


     const char* filename = "./LQR_PID.png";
     plt::save(filename);
     plt::show();
     return 0;
}

在这里插入图片描述
初速度为0，目标速度为4，最终速度稳定在4左右，可以观察到在此低速恒速状态下的跟踪误差效果比较差，但是车辆速度较快，能够更加迅速的到达目标位置，不过要牺牲跟踪效果为代价。

四、总结

本文从现代控制系统的理论作为LQR算法的引入，然后介绍了连续时间的LQR和离散步LQR的推导过程及求解步骤，最后用C++实现了自动驾驶车辆的恒速横向控制LQR来控制前轮转角，以及更进一步的增加了PID控制器用来纵向控制调节加速度，即控制油门刹车。

需要注意的是 ：LQR也可以实现对前轮转角和加速度的横纵向控制，由上面的状态空间方程可以看出，控制量有两个，分别是前轮转角和加速度。但是在量产ADAS或者自动驾驶算法中，横纵向控制往往都是分开控制的，将使用LQR算法进行横向控制，同时使用PID算法进行纵向控制。这种方法在很多自动驾驶科技公司比较常见，百度apollo的控制节点control也是使用同样的思路。