Visual studio C++：LQR轨迹跟踪仿真
2024-05-20 08:01:56
前言：
因为工作需要开始学习车辆横纵向控制，然后学到了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>
#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]^Tclass 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();
};4.LQR.cpp
#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), 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;//反馈率Kreturn 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;
}5.trajectory.h
#include <iostream>
#include <vector>
#include "tool.h"
using namespace std;class trajectory {
private:vector<waypoint> waypoints;public://set reference trajectoryvoid refer_path();vector<waypoint> get_path();};
6.trajectory.cpp
#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;//xPP.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;
}7.matplotlibcpp.h
这个配置matplot库的时候会有这个头文件，代码里面直接调用就可以画图啦。
8.LQR_node.cpp
// _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，后面有减速处理，可以看到效果还是挺不错的。
重要的说明！！
1、解黎卡提方程的问题
在大佬python版本中发现了一个问题，经过解黎卡提方程后，得到的矩阵K只迭代了一次(如下图)，这个地方我想了挺久的，最后还是按照迭代法求黎卡提（Riccati）方程的解这篇博客来解的，判定收敛的条件是无穷范数。
2.初始误差的选择(小车与轨迹起点的距离)
在调试过程中发现了一个很有意思的现象，采样时间与允许初始误差范围有关系，太大的初始误差可能会导致跟踪失败，仿真的期望速度为0.5m/s：
| 采样时间(s) | 允许初始误差范围(m) |
| 0.5 | ±5.0 |
| 0.1 | ±10.0 |
| 0.05 | ±11.0 |
测试了三种调试频率，2Hz，10Hz，20Hz，在实验平台上一般使用20Hz的，不过取极限初始误差意义不是很大，自动驾驶里面不可能在离车辆11m的地方开始规划路径。。。但还是可以测试控制器极限性能。
3.权重的选择
这个问题我调了很久，最后发现Q权重一定要比R小，不然速度就会提前计算到0，或者前轮转角值异常。我也不知道是怎么回事，相同的控制率算法写到python里面没有问题，C++有问题，就令我很费解。
4.完整代码
完整代码请见：LQR_cpp
5.后续工作
过几天会把这个项目弄到实验平台上进行仿真和实验。
Visual studio C++：LQR轨迹跟踪仿真相关推荐
- LQR轨迹跟踪算法Python/Matlab算法实现_代码(2)
本文根据LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导(2)使用代码实现,进行仿真: clc clear allKp = 1.0 ; dt =0.1 ;% [s] L = ...
- LQR轨迹跟踪算法Python算法实现3
根据LQR轨迹跟踪算法Python/Matlab算法实现2的代码,我们转化成Python,后续上车使用.代码仅开源到这,可以进行仿真,函数都可以直接使用.工程代码就不开源了. from numpy i ...
- LQR轨迹跟踪算法Python/Matlab算法实现2
这里对上一篇LQR轨迹跟踪算法Python/Matlab算法实现进行勘误: clc clear allKp = 1.0 ; dt = 0.1 ;% [s] L = 2.9 ;% [m] wheel b ...
- LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导
对于文章 LQR轨迹跟踪算法Python/Matlab算法实现中的LQR推导的问题,我简单写了一下手稿,不高兴做成公式了:
- 四旋翼飞行器轨迹跟踪仿真MATLAB simulink/simscape
四旋翼飞行器轨迹跟踪仿真MATLAB simulink/simscape ID:6929669117089277
- 差速机器人的纯轨迹跟踪仿真（Matlab）
差速机器人的纯轨迹跟踪仿真(Matlab) 刚入门,有的地方不对,烦请大家指正. 目录 差速机器人的纯轨迹跟踪仿真(Matlab) 1 差速机器人运动模型 1.1 运动学分析建模 1.2 差速机器人的 ...
- 四个翅膀的飞机Simulink与轨迹跟踪仿真
数学建模 这一部分强烈建议看一篇知乎文章:zinghd的回答 其最终模型结果: 其中部分符号的定义可以参考上面给的知乎链接.下面给出角度的定义和飞行器的外观. 对于上图给出的四旋翼飞行器,其x轴为向量 ...
- LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导(2)
本文提供了另一种LQR状态空间矩阵的推导方式:
- MPC实现自动驾驶轨迹跟踪
本文继MPC运动学方法实现轨迹跟踪推导进行matlab代码实现,虽然你们找到的参考书都是simulink carsim联仿,我却坚持使用纯代码仿真,因为牛逼. 代码模板沿用了LQR轨迹跟踪算法Pyth ...
- [翻译] Visual Studio 2019 RC版发布
今天,我们将分享 Visual Studio 2019 的发布候选版(RC 版) - 这是在 4 月 2 日的虚拟发布活动上正式发布之前的最后步骤之一. 您可以在 visualstudio.com/d ...
最新文章
- A*算法 javascript模拟
- 一分钟！不写代码！给网站添加【统计分析】
- HTML5 新特性
- mysql触发器的简单入门(二)
- 单体内置对象_单体内置对象
- hbase 学习（十二）非mapreduce生成Hfile，然后导入hbase当中
- sitemap.xml文件生成工具
- linux拷贝文件后几百行,我 的 一 些 练 习 题
- ADB: unknown command ?
- java进行微信公众号开发
- android horizontalscrollview属性,Android中HorizontalScrollView使用方法详解
- Android手机视频监控系统
- Anaconda =＞ PyCharm =＞ CUDA =＞ cudnn =＞ PyTorch 环境配置
- VS 无法启动程序（系统找不到指定路径）的解决方法
- 黄哥python培训骗局
- python 函数报错TypeError: object of type 'int' has no len()
- 想象力的再突破！无人机后还有大招？！
- 2019年9月计算机一级试题,2019年9月计算机一级Ms Office试题（4）
- python写入文件没反应_python写入文本 如何用python将变量及其值写入文本文件？...
- pydicom 安装