无人驾驶LQR控制算法 c++ 实现

参考博客：
（1）LQR的理解与运用第一期——理解篇
（2）线性二次型调节器(LQR)原理详解
（3）LQR控制基本原理（包括Riccati方程具体推导过程）
（4）【基础】自动驾驶控制算法第五讲连续方程的离散化与离散LQR原理

0 前言

LQR：线性二次调节器，设计状态反馈控制器的方法

1 LQR算法原理

系统： $\dot x=Ax+Bu$
线性反馈控制器： $u = - K x$
在这里插入图片描述
让系统稳定的条件是矩阵 $A_{cl}$ 的特征值实部均为负数。因此我们可以手动选择几个满足上述条件的特征值，然后反解出K，从而得到控制器。

代价函数 $J$
在这里插入图片描述
在系统稳定的前提下，通过设计合适的K，让代价函数J最小。
Q大：希望状态变量x更快收敛
R大：希望输入量u收敛更快，以更小的代价实现系统稳定

1.1 连续时间LQR推导

具体推导参见博客：线性二次型调节器(LQR)原理详解

求解连续时间LQR反馈控制器参数K的过程：
（1）设计参数矩阵Q、R
（2）求解Riccati方程 $A^TP+PA-PBR^{-1}B^TP+Q=0$ 得到P
（3）计算 $K=R^{-1}B^TP$ 得到反馈控制量 $u = - k x$

1.2 离散时间LQR推导

离散情况下的LQR推导有最小二乘法和动态规划算法
详细推导见博客：连续时间LQR和离散时间LQR笔记
离散系统：
$x (K + 1) = A x (k) + B u (k)$
代价函数：
在这里插入图片描述
设计步骤：
① 确定迭代范围N
② 设置迭代初始值 $P_N=Q$
③ $t = N, ..., 1$ ，从后向前循环迭代求解离散时间的代数Riccati方程
$P_{t-1}=Q+A^TP_tA-A^TP_tB(R+B^TP_{t+1}B)^{-1}B^TP_tA$
④ $t = 0, ..., N$ 循环计算反馈系数 $K_t=(R+B^TP_{t+1}B)^{-1}B^TP_{t+1}A$ 得到控制量 $u_t=-K_tx_t$

2 LQR代码

主要步骤：
（1）确定迭代范围N，预设精度EPS
（2）设置迭代初始值P = Qf，Qf = Q
（3）循环迭代， $t = 1, ..., N$
$P _{new} =Q+A ^TPA−A ^TPB(R+B ^T PB) ^{−1}B ^TPA$
若 $P_{new}-P||<EPS$ ：跳出循环；否则： $P=P_{new}$
（4）计算反馈系数 $K=(R + B^TP_{new}B)^{-1}B^TP_{new}A$
（5）最终的优化控制量 $u^*=-Kx$

Reference_path.h

#pragma once

#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

#define PI 3.1415926

struct refTraj
{
    MatrixXd xref, dref;
    int ind;
};

struct parameters
{
    int L;
    int NX, NU, T;
    double dt;
};

class ReferencePath
{
public:
    ReferencePath();
    vector<double> calcTrackError(vector<double> robot_state);
    double normalizeAngle(double angle);

    // 计算参考轨迹点，统一化变量数组，便于后面MPC优化使用.
    refTraj calc_ref_trajectory(vector<double> robot_state, parameters param, double dl = 1.0);

public:
    vector<vector<double>> ref_path; // x, y, 切线方向, k
    vector<double> ref_x, ref_y;
};

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};
}

double ReferencePath::normalizeAngle(double angle)
{
    while (angle > PI)
    {
        angle -= 2 * PI;
    }
    while (angle < -PI)
    {
        angle += 2 * PI;
    }
    return angle;
}

// 计算参考轨迹点，统一化变量数组，只针对MPC优化使用
// robot_state 车辆的状态(x,y,yaw,v)
refTraj ReferencePath::calc_ref_trajectory(vector<double> robot_state, parameters param, double dl)
{
    vector<double> track_error = calcTrackError(robot_state);
    double e = track_error[0], k = track_error[1], ref_yaw = track_error[2], ind = track_error[3];
    refTraj ref_traj;
    ref_traj.xref = MatrixXd(param.NX, param.T + 1);
    ref_traj.dref = MatrixXd (param.NU,param.T);
    int ncourse = ref_path.size();
    ref_traj.xref(0,0)=ref_path[ind][0];
    ref_traj.xref(1,0)=ref_path[ind][1];
    ref_traj.xref(2,0)=ref_path[ind][2];
    //参考控制量[v,delta]
    double ref_delta = atan2(k * param.L, 1);
    for(int i=0;i<param.T;i++){
        ref_traj.dref(0,i)=robot_state[3];
        ref_traj.dref(1,i)=ref_delta;
    }
    double travel = 0.0;
    for(int i = 0; i < param.T + 1; i++){
        travel += abs(robot_state[3]) * param.dt;

        double dind = (int)round(travel / dl);

        if(ind + dind < ncourse){
            ref_traj.xref(0,i)=ref_path[ind + dind][0];
            ref_traj.xref(1,i)=ref_path[ind + dind][1];
            ref_traj.xref(2,i)=ref_path[ind + dind][2];
        }else{
            ref_traj.xref(0,i)=ref_path[ncourse-1][0];
            ref_traj.xref(1,i)=ref_path[ncourse-1][1];
            ref_traj.xref(2,i)=ref_path[ncourse-1][2];
        }
    }
    return ref_traj;
}

LQR.h

#pragma once

#define EPS 1.0e-4
#include <Eigen/Dense>
#include <vector>
#include <iostream>
using namespace std;
using namespace Eigen;

class LQR {
private:
    int N;
public:
    LQR(int n);

    MatrixXd calRicatti(MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R);
    double LQRControl(vector<double> robot_state, vector<vector<double>> ref_path, double s0, MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R);
};

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);
}

main.cpp

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

namespace plt = matplotlibcpp;

int main()
{
    int N = 500; // 迭代范围
    double Target_speed = 7.2 / 3.6;
    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_;
    ReferencePath referencePath;
    KinematicModel model(0, 1.0, 0, 0, 2.2, 0.1);
    PID_controller PID(3, 0.001, 30, Target_speed, 15.0 / 3.6, 0.0);
    LQR lqr(N);
    vector<double> robot_state;

    for (int i = 0; i < 700; i++)
    {
        plt::clf();
        robot_state = model.getState();
        vector<double> one_trial = referencePath.calcTrackError(robot_state);
        double k = one_trial[1], ref_yaw = one_trial[2], s0 = one_trial[3];

        double ref_delta = atan2(k * model.L, 1); // L = 2.2
        vector<MatrixXd> state_space = model.stateSpace(ref_delta, ref_yaw);

        double delta = lqr.LQRControl(robot_state, referencePath.ref_path, s0, state_space[0], state_space[1], Q, R);
        delta = delta + ref_delta;
        double a = PID.calOutput(model.v);
        model.updateState(a, delta);
        cout << "Speed: " << model.v << " m/s" << endl;

        x_.push_back(model.x);
        y_.push_back(model.y);
        //画参考轨迹
        plt::plot(referencePath.ref_x, referencePath.ref_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;
}

CMakeList.txt

cmake_minimum_required(VERSION 3.0.2)
project(LQR)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
)

set(CMAKE_CXX_STANDARD 11)

file(GLOB_RECURSE PYTHON2.7_LIB "/usr/lib/python2.7/config-x86_64-linux-gnu/*.so")
set(PYTHON2.7_INLCUDE_DIRS "/usr/include/python2.7")

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES huatu
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
)

include_directories(include
        ${PYTHON2.7_INLCUDE_DIRS}
)

add_executable(lqr_controller src/LQR.cpp
                              src/KinematicModel.cpp
                              src/main.cpp
                              src/pid_controller.cpp
                              src/Reference_path.cpp)
target_link_libraries(lqr_controller ${PYTHON2.7_LIB})