前言
上一篇博客用纯C++程序写完了LQR仿真过程，效果还行，然后这一篇博客用来把这个程序部署到实验平台的ROS系统上进行仿真及实验验证。
建议简单看一下上一篇的一个程序思路：Visual studio C++：LQR轨迹跟踪仿真
思路
其实思路非常简单，就是path_planning向LQR_node发送目标点就可以了，主要看红圈里面的就行，其他的都是可视化要用的。
path_planning生成路径通过话题/path传给LQR_node中，LQR_node进行跟踪。
代码部署
一、自定义库函数(LQR、tool、trajectory)
基于上一篇博客，已经写了自定义的相关库函数，有LQR、tool、trajectory、matplot，但是在ROS里面可以不用matplot，可以在rviz里面看，所以丢弃。
1.Tool优化
为了让路径生成过程更加条理清晰，对库函数进行了一些优化，Tool里面路径点参数只设置x、y、yaw，而曲率K选择在控制器里面针对获取的路径去算。添加了一个计算曲率K的函数。
Tool.h
#pragma once
#include <iostream>
#include <math.h>
#include <vector>
using namespace std;
#define pi acos(-1)
//定义路径点
typedef struct waypoint {
int ID;
double x, y, yaw;//x,y
}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 cal_K(vector<waypoint> waypoints, int index);//计算曲率K
Tool.cpp
#include<iostream>
#include<LQR_track/Tool.h>
double cal_K(vector<waypoint> waypoints, int index){
double res;
//差分法求一阶导和二阶导
double dx, dy, ddx, ddy;
if (index == 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 (index == (waypoints.size() - 1)) {
dx = waypoints[index].x - waypoints[index - 1].x;
dy = waypoints[index].y - waypoints[index - 1].y;
ddx = waypoints[index].x + waypoints[index - 2].x - 2 * waypoints[index].x;
ddy = waypoints[index].y + waypoints[index - 2].y - 2 * waypoints[index].y;
}
else {
dx = waypoints[index + 1].x - waypoints[index].x;
dy = waypoints[index + 1].y - waypoints[index].y;
ddx = waypoints[index + 1].x + waypoints[index - 1].x - 2 * waypoints[index].x;
ddy = waypoints[index + 1].y + waypoints[index - 1].y - 2 * waypoints[index].y;
}
//res.yaw = atan2(dy, dx);//yaw
//计算曲率：设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2).
res = (ddy * dx - ddx * dy) / (sqrt(pow((pow(dx, 2) + pow(dy, 2)), 3)));
return res;
}
2.trajectory优化
设置了可以从launch文件导入的参数接口，用于设计轨迹起始点，长度限制等信息，添加了用户自定义轨迹custom_path，按照trajectory.h里面的备注慢慢操作就好了。
trajectory.h
#include <iostream>
#include <vector>
#include "LQR_track/Tool.h"
#include <string>
using namespace std;
class trajectory {
private:
vector<waypoint> waypoints;
double x_start,y_start,limit_x,limit_y;
string trajectory_type;
public:
trajectory(double initial_x_,double initial_y_,string type_,double limit_x_,double limit_y_):
x_start(initial_x_),y_start(initial_y_),trajectory_type(type_),limit_x(limit_x_),limit_y(limit_y_){};
//set reference trajectory
void refer_path();
vector<waypoint> get_path();
//If you need to add a custom path:1.please add the function; 2.Overwrite trajectory.cpp synchronously
//void custom_path();
void line();
void wave1();
void wave2();
};trajectory.cpp
#include <iostream>
#include <vector>
#include "LQR_track/trajectory.h"
#include <math.h>
using namespace std;
double dt = 0.01;//轨迹计算频率
void trajectory::line(){
waypoint PP;
for (int i = 0; i < limit_x/dt; i++)
{
PP.ID = i;
PP.x = x_start + dt * i;//x:10米
PP.y = y_start ;//y
waypoints.push_back(PP);
}
}
void trajectory::wave1(){
waypoint PP;
for (int i = 0; i < limit_x/dt; i++)
{
PP.ID = i;
PP.x = x_start + dt * i;//x
PP.y = y_start + 1.0 * sin(dt*i / 1.5) + 0.5 * cos(dt*i / 1.0);//y
waypoints.push_back(PP);
}
}
void trajectory::wave2(){
waypoint PP;
for (int i = 0; i < limit_x/dt; i++)
{
PP.ID = i;
PP.x = x_start + dt * i;//x
PP.y = y_start - 0.2 * sin(dt*i / 0.4);//y
waypoints.push_back(PP);
}
}
//write the path you design
/*void trajectory::custom_path(){
waypoint PP;
for (int i = 0; i < limit_x/dt; i++)
{
PP.ID = i;
PP.x = ...;//x
PP.y = ...;//y
waypoints.push_back(PP);
}
}*/
void trajectory::refer_path() {
if(trajectory_type == "wave1")wave1();
else if(trajectory_type == "line")line();
else if(trajectory_type == "wave2")wave2();
//else if(trajectory_type == "custom_path")custom_path();//set the index
//计算切线方向并储存
for (int j=0;j<waypoints.size();j++){
double dx, dy, yaw;
if (j == 0) {
dx = waypoints[1].x - waypoints[0].x;
dy = waypoints[1].y - waypoints[0].y;
}
else if (j == (waypoints.size() - 1)) {
dx = waypoints[j].x - waypoints[j - 1].x;
dy = waypoints[j].y - waypoints[j - 1].y;
}
else {
dx = waypoints[j + 1].x - waypoints[j].x;
dy = waypoints[j + 1].y - waypoints[j].y;
}
yaw = atan2(dy, dx);//yaw
waypoints[j].yaw = yaw;
}
}
vector<waypoint> trajectory::get_path() {
return waypoints;
}
3.LQR优化
这个好像没做优化，跟原来的一样吧，还是附上代码：
LQR.h
#include <iostream>
#include <Eigen/Dense>
#include "LQR_track/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();
};
LQR.cpp
#include <iostream>
#include "LQR_track/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;//反馈率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;
}4.ROS中自定义库的编译生成
可以参考一下这一篇：
最低0.47元/天 解锁文章
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
