Visual studio C++：LQR轨迹跟踪仿真

前言：

        因为工作需要开始学习车辆横纵向控制，然后学到了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库，建议提前看一下该库的基本命令。

 

准备工作

1.项目配置Eigen库：

安装和使用C++线性代数库eigen（Windows下minGW+VS code, VS2019配置方式）

2.项目配置matplot库：
VS C++调用python进行画图matplotlib

 windows下配置C++版本的matplotlib绘图工具matplotlibcpp

 

别忘了把解决方案配置换成Release，我在这里卡了好久

 

代码

1.tool.h

#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];

2.tool.cpp

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

3.LQR.h

#include <iostream>
#include <Eigen/Dense>
#incl