参考博客:
(1)LQR 的理解与运用 第一期——理解篇
(2)线性二次型调节器 (LQR) 原理详解
(3)LQR 控制基本原理(包括 Riccati 方程具体推导过程)
(4)【基础】自动驾驶控制算法第五讲 连续方程的离散化与离散 LQR 原理
0 前言
LQR:线性二次调节器,设计状态反馈控制器的方法
1 LQR 算法原理
系统: x ˙ = A x + B u \dot x=Ax+Bu x˙=Ax+Bu
线性反馈控制器: u = − K x u=-Kx u=−Kx
让系统稳定的条件是矩阵 A c l A_{cl} Acl的特征值实部均为负数。因此我们可以手动选择几个满足上述条件的特征值,然后反解出 K,从而得到控制器。
代价函数 J J J
在系统稳定的前提下,通过设计合适的 K,让代价函数 J 最小。
Q 大:希望状态变量 x 更快收敛
R 大:希望输入量 u 收敛更快,以更小的代价实现系统稳定
1.1 连续时间 LQR 推导
具体推导参见博客:线性二次型调节器 (LQR) 原理详解
求解连续时间 LQR 反馈控制器参数 K 的过程:
(1)设计参数矩阵 Q、R
(2)求解 Riccati 方程 A T P + P A − P B R − 1 B T P + Q = 0 ATP+PA-PBR{-1}B^TP+Q=0 ATP+PA−PBR−1BTP+Q=0 得到 P
(3)计算 K = R − 1 B T P K=R{-1}BTP K=R−1BTP 得到反馈控制量 u = − k x u=-kx u=−kx
1.2 离散时间 LQR 推导
离散情况下的 LQR 推导有最小二乘法和动态规划算法
详细推导见博客:连续时间 LQR 和离散时间 LQR 笔记
离散系统:
x ( K + 1 ) = A x (k) + B u ( k ) x(K+1)=Ax(k)+Bu(k) x(K+1)=Ax(k)+Bu(k)
代价函数:
设计步骤:
① 确定迭代范围 N
② 设置迭代初始值 P N = Q P_N=Q PN=Q
③ t = N , . . . , 1 t=N,…,1 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 P_{t-1}=Q+ATP_tA-ATP_tB(R+BTP_{t+1}B){-1}B^TP_tA Pt−1=Q+ATPtA−ATPtB(R+BTPt+1B)−1BTPtA
④ t = 0 , . . . , N t=0,…,N t=0,…,N 循环计算反馈系数 K t = ( R + B T P t + 1 B ) − 1 B T P t + 1 A K_t=(R+BTP_{t+1}B){-1}B^TP_{t+1}A Kt=(R+BTPt+1B)−1BTPt+1A 得到控制量 u t = − K t x t u_t=-K_tx_t ut=−Ktxt
2 LQR 代码
主要步骤:
(1)确定迭代范围 N,预设精度 EPS
(2)设置迭代初始值 P = Qf,Qf = Q
(3)循环迭代, t = 1 , . . . , N t=1,…,N t=1,…,N
P n e w = Q + A T P A − A T P B ( R + B T P B ) − 1 B T P A P {new} =Q+A ^TPA−A ^TPB(R+B ^T PB) ^{−1}B ^TPA Pnew=Q+ATPA−ATPB(R+BTPB)−1BTPA
若 ∣ ∣ P n e w − P ∣ ∣ < E P S ||P{new}-P||<EPS ∣∣Pnew−P∣∣<EPS:跳出循环;否则: P = P n e w P=P_{new} P=Pnew
(4)计算反馈系数 K = ( R + B T P n e w B ) − 1 B T P n e w A K=(R + BTP_{new}B){-1}B^TP_{new}A K=(R+BTPnewB)−1BTPnewA
(5)最终的优化控制量 u ∗ = − K x u^*=-Kx 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})
3 PID vs Pure pursuit vs Stanley vs LQR
横向控制算法
(1)PID:鲁棒性较差,对路径无要求,转弯不会内切,速度增加会有一定超调,速度增加稳态误差变大,适用场景:路径曲率较小及低速的跟踪场景
(2)Pure pursuit:鲁棒性较好,对路径无要求,转弯内切速度增加变得严重,速度增加会有一定超调,速度增加稳态误差变大,适用场景:路径连续或不连续或者低速的跟踪场景
(3)Stanley:鲁棒性好,对路径要求曲率连续,转弯不会内切,速度增加会有一定超调,速度增加稳态误差变大,适用场景:路径平滑的中低速跟踪场景
(4)LQR:鲁棒性较差,对路径要求曲率连续,不会转弯内切,曲率快速变化时超调严重,稳态误差小,除非速度特别大,适用场景:路径平滑的中高速城市驾驶跟踪场景