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

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

0 前言

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

1 LQR算法原理

系统：$\dot{x}=Ax+Bu$
线性反馈控制器：$u=-Kx$

让系统稳定的条件是矩阵$A_{cl}$的特征值实部均为负数。因此我们可以手动选择几个满足上述条件的特征值，然后反解出K，从而得到控制器。

代价函数 $J$

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

1.1 连续时间LQR推导

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

---

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

1.2 离散时间LQR推导

离散情况下的LQR推导有最小二乘法和动态规划算法
详细推导见博客：连续时间LQR和离散时间LQR笔记
离散系统：
$x(K+1)=Ax(k)+Bu(k)$
代价函数：

设计步骤：
① 确定迭代范围N
② 设置迭代初始值$P_N=Q$
③ $t=N,...,1$，从后向前循环迭代求解离散时间的代数Riccati方程
$$P_{t-1}=Q+A^T{P}_{t}A-A^T{P}_{t}B(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t}A$$
④ $t=0,...,N$循环计算反馈系数$K_t=(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}A$ 得到控制量$u_t=-K_t{x}_t$

2 LQR代码

主要步骤：
（1）确定迭代范围N，预设精度EPS
（2）设置迭代初始值P = Qf，Qf = Q
（3）循环迭代，$t=1,...,N$
$$P_{new}=Q+A^{T}PA-A^{T}PB(R+B^{T}PB{)}^{-1}{B}^{T}PA$$
若$||P_{new}-P||<EPS$：跳出循环；否则：$P=P_{new}$
（4）计算反馈系数$K=(R+B^T{P}_{new}B{)}^{-1}{B}^T{P}_{new}A$
（5）最终的优化控制量$u^{\ast }=-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.1415926struct 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, 切线方向, kvector<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 ^TPAfor (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}AMatrixXd 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.2vector<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 COMPONENTSroscppstd_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.cppsrc/KinematicModel.cppsrc/main.cppsrc/pid_controller.cppsrc/Reference_path.cpp)
target_link_libraries(lqr_controller ${PYTHON2.7_LIB})

3 PID vs Pure pursuit vs Stanley vs LQR

横向控制算法
（1）PID：鲁棒性较差，对路径无要求，转弯不会内切，速度增加会有一定超调，速度增加稳态误差变大，适用场景：路径曲率较小及低速的跟踪场景

（2）Pure pursuit：鲁棒性较好，对路径无要求，转弯内切速度增加变得严重，速度增加会有一定超调，速度增加稳态误差变大，适用场景：路径连续或不连续或者低速的跟踪场景

（3）Stanley：鲁棒性好，对路径要求曲率连续，转弯不会内切，速度增加会有一定超调，速度增加稳态误差变大，适用场景：路径平滑的中低速跟踪场景

（4）LQR：鲁棒性较差，对路径要求曲率连续，不会转弯内切，曲率快速变化时超调严重，稳态误差小，除非速度特别大，适用场景：路径平滑的中高速城市驾驶跟踪场景