因为工作需要开始学习车辆横纵向控制，然后学到了LQR，正好写一个博客把程序保存下来。为了加强C++代码能力，本次仿真的所有文件均用C++完成。
开始之前非常感谢这位大佬给出的参考：【自动驾驶】LQR实现轨迹跟踪，这次项目大部分都是将该博客从python翻译成C++，当然其中也发现了一些问题，后续再谈。
该项目用多个模块组成，分别为LQR、LQR_node、tool、trajectory、matplot5个模块。
1.LQR_node为主函数节点，负责调用轨迹生成模块、LQR控制器模块和画图；
2.LQR为LQR控制器模块，控制器中构造了模型参数A、B，计算黎卡提方程等功能；
3.trajectory为轨迹生成模块，并且计算出坐标点对应的曲率值；
4.tool为工具模块，定义了项目中需要的数据类型和一些角度处理函数（虽然没用到）；
5.matplot为画图模块，调用了python的matplot功能进行作图；
该项目用到的库有Eigen、python、matplotlibcpp，其中最为重要的是Eigen库，建议提前看一下该库的基本命令。
安装和使用C++线性代数库eigen（Windows下minGW+VS code, VS2019配置方式）
windows下配置C++版本的matplotlib绘图工具matplotlibcpp
别忘了把解决方案配置换成Release，我在这里卡了好久
#pragma once
#include <iostream>
using namespace std;
#define pi acos(-1)
//定义路径点
typedef struct waypoint {
int ID;
double x, y, yaw, K;//x,y,yaw,曲率K
}waypoint;
//定义小车状态
typedef struct vehicleState {
double x, y, yaw, v, kesi;//x,y,yaw,前轮偏角kesi
}vehicleState;
//定义控制量
typedef struct U {
double v;
double kesi;//速度v,前轮偏角kesi
}U;
double normalize_angle(double angle);//角度归一化 [-pi,pi];
double limit_kesi(double kesi);//前轮转角限幅 [-pi/2,pi/2];
#include<iostream>
#include<tool.h>
double normalize_angle(double angle)//角度归一化 [-pi,pi];
{
if (angle > pi) {
angle -= 2.0 * pi;
}
if (angle <= -pi) {
angle += 2.0 * pi;
}
return angle;
}
double limit_kesi(double kesi) {
if (kesi > pi / 2) {
kesi = pi / 2;
}
if (kesi < -pi / 2) {
kesi = -pi / 2;
}
return kesi;
}#include <iostream>
#include <Eigen/Dense>
#include "tool.h"
using namespace std;
typedef Eigen::Matrix<double, 3, 3> Matrix3x3;
typedef Eigen::Matrix<double, 3, 1> Matrix3x1;
typedef Eigen::Matrix<double, 2, 1> Matrix2x1;
typedef Eigen::Matrix<double, 2, 2> Matrix2x2;
typedef Eigen::Matrix<double, 3, 2> Matrix3x2;
typedef Eigen::Matrix<double, 2, 3> Matrix2x3;
//状态方程变量: X = [x_e y_e yaw_e]^T
//状态方程控制输入: U = [v_e kesi_e]^T
class LQR
{
private:
Matrix3x3 A_d;
Matrix3x2 B_d;
Matrix3x3 Q;
Matrix2x2 R;
Matrix3x1 X_e;
Matrix2x1 U_e;
double L;//车辆轴距
double T;//采样间隔
double x_car, y_car, yaw_car, x_d, y_d, yaw_d;//车辆位姿和目标点位姿
double v_d, kesi_d;//期望速度和前轮偏角
double Q3[3];//Q权重，三项
double R2[2];//R权重，两项
int temp = 0;
public:
void initial(double L_, double T_, vehicleState car, waypoint waypoint, U U_r, double* Q_, double* R_);//初始化
void param_struct();//构造状态方程参数
Matrix2x3 cal_Riccati();//黎卡提方程求解
U cal_vel();//LQR控制器计算速度
void test();
};
#include <iostream>
#include <LQR.h>
using namespace std;
void LQR::initial(double L_, double T_, vehicleState car, waypoint waypoint, U U_r, double *Q_, double *R_) {
L = L_;
T = T_;
x_car = car.x; y_car = car.y; yaw_car = car.yaw;
x_d = waypoint.x; y_d = waypoint.y; yaw_d = waypoint.yaw;
v_d = U_r.v;kesi_d = U_r.kesi;
for (int i = 0; i < 3; i++) {
Q3[i] = Q_[i];
}
for (int j = 0; j < 2; j++) {
R2[j] = R_[j];
}
}
void LQR::param_struct() {
Q << Q3[0], 0.0, 0.0,
0.0, Q3[1], 0.0,
0.0, 0.0, Q3[2];
//cout << "Q矩阵为：\n" << Q << endl;
R << R2[0], 0.0,
0.0, R2[1];
//cout << "R矩阵为：\n" << R << endl;
A_d << 1.0, 0.0, -v_d * T * sin(yaw_d),
0.0, 1.0, v_d* T* cos(yaw_d),
0.0, 0.0, 1.0;
//cout << "A_d矩阵为:\n" << A_d << endl;
B_d << T * cos(yaw_d), 0.0,
T* sin(yaw_d), 0.0,
T* tan(kesi_d)/L, v_d* T / (L * cos(kesi_d) * cos(kesi_d));
//cout << "B_d矩阵为:\n" << B_d << endl;
X_e << x_car - x_d, y_car - y_d, yaw_car - yaw_d;
cout << "X_e矩阵为:\n" << X_e << endl;
}
Matrix2x3 LQR::cal_Riccati() {
int N = 150;//迭代终止次数
double err = 100;//误差值
double err_tolerance = 0.01;//误差收敛阈值
Matrix3x3 Qf = Q;
Matrix3x3 P = Qf;//迭代初始值
//cout << "P初始矩阵为\n" << P << endl;
Matrix3x3 Pn;//计算的最新P矩阵
for (int iter_num = 0; iter_num < N; iter_num++) {
Pn = Q + A_d.transpose() * P * A_d - A_d.transpose() * P * B_d * (R + B_d.transpose() * P * B_d).inverse() * B_d.transpose() * P * A_d;//迭代公式
//cout << "收敛误差为" << (Pn - P).array().abs().maxCoeff() << endl;
//err = (Pn - P).array().abs().maxCoeff();//
err = (Pn - P).lpNorm<Eigen::Infinity>();
if(err < err_tolerance)//
{
P = Pn;
cout << "迭代次数" << iter_num << endl;
break;
}
P = Pn;
}
//cout << "P矩阵为\n" << P << endl;
//P = Q;
Matrix2x3 K = -(R + B_d.transpose() * P * B_d).inverse() * B_d.transpose() * P * A_d;//反馈率K
return K;
}
U LQR::cal_vel() {
U output;
param_struct();
Matrix2x3 K = cal_Riccati();
Matrix2x1 U = K * X_e;
//cout << "反馈增益K为：\n" << K << endl;
//cout << "控制输入U为：\n" << U << endl;
output.v = U[0] + v_d;
output.kesi = U[1] + kesi_d;
return output;
}
void LQR::test() //控制器效果测试
{
/*param_struct();
while (temp < 1000) {
Matrix2x3 K = cal_Riccati();
Matrix2x1 U = K * X_e;
//cout <<"state variable is:\n" <<X_e << endl;
//cout <<"control input is:\n"<< U << endl;
Matrix3x1 X_e_ = A_d * X_e + B_d * U;
X_e = X_e_;
temp++;
}*/
Matrix3x3 C,D,F;
C << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
F << 1.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 7.0;
D = (C - F);
double BBBB = D.lpNorm<Eigen::Infinity>();
cout << BBBB << endl;
}#include <iostream>
#include <vector>
#include "tool.h"
using namespace std;
class trajectory {
private:
vector<waypoint> waypoints;
public:
//set reference trajectory
void refer_path();
vector<waypoint> get_path();
};
#include <iostream>
#include <vector>
#include <trajectory.h>
#include <math.h>
using namespace std;
void trajectory::refer_path() {
waypoint PP;
for (int i = 0; i < 1000; i++)
{
PP.ID = i;
PP.x = 0.1 * i;//x
PP.y = 2.0 * sin(PP.x / 5.0) + 2.0 * cos(PP.x / 2.5);//y
//直线
//PP.y = 5.5;
PP.yaw = PP.K = 0.0;
waypoints.push_back(PP);
}
for (int j = 0; j < waypoints.size(); j++) {
//差分法求一阶导和二阶导
double dx, dy, ddx, ddy;
if (j == 0) {
dx = waypoints[1].x - waypoints[0].x;
dy = waypoints[1].y - waypoints[0].y;
ddx = waypoints[2].x + waypoints[0].x - 2 * waypoints[1].x;
ddy = waypoints[2].y + waypoints[0].y - 2 * waypoints[1].y;
}
else if (j == (waypoints.size() - 1)) {
dx = waypoints[j].x - waypoints[j - 1].x;
dy = waypoints[j].y - waypoints[j - 1].y;
ddx = waypoints[j].x + waypoints[j - 2].x - 2 * waypoints[j].x;
ddy = waypoints[j].y + waypoints[j - 2].y - 2 * waypoints[j].y;
}
else {
dx = waypoints[j + 1].x - waypoints[j].x;
dy = waypoints[j + 1].y - waypoints[j].y;
ddx = waypoints[j + 1].x + waypoints[j - 1].x - 2 * waypoints[j].x;
ddy = waypoints[j + 1].y + waypoints[j - 1].y - 2 * waypoints[j].y;
}
waypoints[j].yaw = atan2(dy, dx);//yaw
//计算曲率：设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2).
double AAA = sqrt(pow((pow(dx, 2) + pow(dy, 2)), 3));
waypoints[j].K = (ddy * dx - ddx * dy) / (sqrt(pow((pow(dx, 2) + pow(dy, 2)), 3)));
}
}
vector<waypoint> trajectory::get_path() {
return waypoints;
}这个配置matplot库的时候会有这个头文件，代码里面直接调用就可以画图啦。
// _ooOoo_
// o8888888o
// 88" . "88
// (| -_- |)
// O\ = /O
// ____/`---'\____
// . ' \\| |// `.
// / \\||| : |||// \
// / _||||| -:- |||||- \
// | | \\\ - /// | |
// | \_| ''\---/'' | |
// \ .-\__ `-` ___/-. /
// ___`. .' /--.--\ `. . __
// ."" '< `.___\_<|>_/___.' >'"".
// | | : `- \`.;`\ _ /`;.`/ - ` : | |
// \ \ `-. \_ __\ /__ _/ .-` / /
// ======`-.____`-.___\_____/___.-`____.-'======
// `=---='
//
// .............................................
// 佛祖保佑 永无BUG
// 佛曰:
// 写字楼里写字间，写字间里程序员；
// 程序人员写程序，又拿程序换酒钱。
// 酒醒只在网上坐，酒醉还来网下眠；
// 酒醉酒醒日复日，网上网下年复年。
// 但愿老死电脑间，不愿鞠躬老板前；
// 奔驰宝马贵者趣，公交自行程序员。
// 别人笑我忒疯癫，我笑自己命太贱；
// 不见满街漂亮妹，哪个归得程序员？
#include <iostream>
#include <LQR.h>
#include <vector>
#include <trajectory.h>
#include <stdlib.h>
#include "matplotlibcpp.h"
using namespace std;
namespace plt = matplotlibcpp;
#define pi acos(-1)
#define T 0.05//采样时间 很有意思的测试数据：T=0.5s，允许误差范围为±5.0m；T=0.1s，允许误差范围为±10.0m；T=0.05s；允许误差范围为±11m
#define L 0.5//车辆轴距
#define V_DESIRED 0.5//期望速度
vehicleState update_state(U control, vehicleState car) {
car.v = control.v;
car.kesi = control.kesi;
car.x += car.v * cos(car.yaw) * T;
car.y += car.v * sin(car.yaw) * T;
car.yaw += car.v / L * tan(car.kesi) * T;
//car.yaw = normalize_angle(car.yaw);
return car;
}
class Path {
private:
vector<waypoint> path;
public:
//添加新的路径点
void Add_new_point(waypoint& p)
{
path.push_back(p);
}
void Add_new_point(vector<waypoint>& p)
{
path = p;
}
//路径点个数
unsigned int Size()
{
return path.size();
}
//获取路径点
waypoint Get_waypoint(int index)
{
waypoint p;
p.ID = path[index].ID;
p.x = path[index].x;
p.y = path[index].y;
p.yaw = path[index].yaw;
p.K = path[index].K;
return p;
}
// 搜索路径点, 将小车到起始点的距离与小车到每一个点的距离对比，找出最近的目标点索引值
int Find_target_index(vehicleState state)
{
double min = abs(sqrt(pow(state.x - path[0].x, 2) + pow(state.y - path[0].y, 2)));
int index = 0;
for (int i = 0; i < path.size(); i++)
{
double d = abs(sqrt(pow(state.x - path[i].x, 2) + pow(state.y - path[i].y, 2)));
if (d < min)
{
min = d;
index = i;
}
}
//索引到终点前，当（机器人与下一个目标点的距离Lf）小于（当前目标点到下一个目标点距离L)时，索引下一个目标点
if ((index + 1) < path.size())
{
double current_x = path[index].x; double current_y = path[index].y;
double next_x = path[index + 1].x; double next_y = path[index + 1].y;
double L_ = abs(sqrt(pow(next_x - current_x, 2) + pow(next_y - current_y, 2)));
double L_1 = abs(sqrt(pow(state.x - next_x, 2) + pow(state.y - next_y, 2)));
//ROS_INFO("L is %f,Lf is %f",L,Lf);
if (L_1 < L_)
{
index += 1;
}
}
return index;
}
};
class LQR_node {
private:
vehicleState car;//小车状态
double Q[3];
double R[2];
int lastIndex;//最后一个点索引值
waypoint lastPoint;//最后一个点信息
vector<double> x,y,x_p,y_p,v_a,v_d,kesi_a,kesi_d;
public:
LQR* controller = new LQR();
Path* path = new Path();
trajectory* trajec = new trajectory();
LQR_node()//初始化中添加轨迹、小车初始位姿
{
//ROS:
addpointcallback();
//robot:
car.x = -1.325;
car.y = 2.562;
car.yaw = 0.964;
car.v = 0.0;
car.kesi = 0.1;
}
~LQR_node() {
free(controller);
free(path);
free(trajec);
}
void addpointcallback(){
trajec->refer_path();
vector<waypoint> waypoints = trajec->get_path();
path->Add_new_point(waypoints);
cout << "path size is:" << path->Size() << endl;
lastIndex = path->Size() - 1;
lastPoint = path->Get_waypoint(lastIndex);
}
double slow_judge(double distance) {
if (distance>=5.0&&distance <= 15.0) {
return 0.35;
}
else if (distance>=0.1&&distance < 5.0) {
return 0.15;
}
else if (distance < 0.1) {
printf("reach goal!\n");
plot_();
}
else
{
return V_DESIRED;
}
}
//控制器流程
void LQR_track() {
U U_r;
waypoint Point;
//搜索路径点
int target_index = path->Find_target_index(car);
printf("target index is : %d\n", target_index);
//获取路径点信息，构造期望控制量
Point = path->Get_waypoint(target_index);
//printf("waypoint information is x:%f,y:%f,yaw:%f,K:%f\n", Point.x, Point.y, Point.yaw, Point.K);
//减速判断
double kesi = atan2(L * Point.K, 1);
double v_distance = abs(sqrt(pow(car.x - lastPoint.x, 2) + pow(car.y - lastPoint.y, 2)));
printf("the distance is %f\n", v_distance);
U_r.v = slow_judge(v_distance);U_r.kesi = kesi;
printf("the desired v is: %f,the desired kesi is: %f\n", U_r.v,U_r.kesi);
//权重矩阵
Q[0] = 1.0; Q[1] = 1.0; Q[2] = 1.0;
R[0] = 4.0; R[1] = 4.0;
//使用LQR控制器
controller->initial(L, T, car, Point, U_r, Q, R);//初始化控制器
U control = controller->cal_vel();//计算输入[v, kesi]
printf("the speed is: %f,the kesi is: %f\n", control.v, control.kesi);
printf("the car position is x: %f, y: %f\n", car.x, car.y);
//储存小车位姿用来画图
x.push_back(car.x);
y.push_back(car.y);
v_a.push_back(car.v);
v_d.push_back(U_r.v);
kesi_a.push_back(car.kesi);
kesi_d.push_back(U_r.kesi);
//小车位姿状态更新
car = update_state(control, car);
}
//控制启停函数
void control() {
int i = 0;
while (i < 10000) {
LQR_track();
i++;
}
}
//画图程序
void plot_() {
vector<double> time;
for (int i = 0; i < path->Size(); i++) {
x_p.push_back(path->Get_waypoint(i).x);
y_p.push_back(path->Get_waypoint(i).y);
}
for (int j = 0; j < v_a.size(); j++) {
time.push_back(double(j));
}
plt::subplot(3, 1, 1);
plt::title("Car position");
plt::plot(x_p, y_p, "-k", x, y, "-.r");
plt::subplot(3, 1, 2);
plt::title("Car speed");
plt::plot(time, v_d, "-k", time, v_a, "-.r");
plt::subplot(3, 1, 3);
plt::title("Car kesi");
plt::plot(time, kesi_d, "-k", time, kesi_a, "-.r");
plt::show();
exit(0);
}
};
int main(char argc, char* argv) {
LQR_node* node = new LQR_node();
node->control();
return 0;
}
三幅图分别为车辆位置、车辆速度、车辆前轮转角。黑色实线为期望值，红色虚线为实际值。速度为0.5，后面有减速处理，可以看到效果还是挺不错的。
在大佬python版本中发现了一个问题，经过解黎卡提方程后，得到的矩阵K只迭代了一次(如下图)，这个地方我想了挺久的，最后还是按照迭代法求黎卡提（Riccati）方程的解这篇博客来解的，判定收敛的条件是无穷范数。
在调试过程中发现了一个很有意思的现象，采样时间与允许初始误差范围有关系，太大的初始误差可能会导致跟踪失败，仿真的期望速度为0.5m/s：
| 采样时间(s) | 允许初始误差范围(m) |
| 0.5 | ±5.0 |
| 0.1 | ±10.0 |
| 0.05 | ±11.0 |
测试了三种调试频率，2Hz，10Hz，20Hz，在实验平台上一般使用20Hz的，不过取极限初始误差意义不是很大，自动驾驶里面不可能在离车辆11m的地方开始规划路径。。。但还是可以测试控制器极限性能。
这个问题我调了很久，最后发现Q权重一定要比R小，不然速度就会提前计算到0，或者前轮转角值异常。我也不知道是怎么回事，相同的控制率算法写到python里面没有问题，C++有问题，就令我很费解。
完整代码请见：LQR_cpp
过几天会把这个项目弄到实验平台上进行仿真和实验。