preface
The last blog used pure C++ program to write the LQR simulation process, and the effect was OK. Then this blog was used to deploy this program to the ROS system of the experimental platform for simulation and experimental verification.
It is suggested to take a brief look at a program idea in the previous article: Visual studio C++:LQR trajectory tracking simulation
thinking
In fact, the idea is very simple, that is, path_planning to LQR_node can send the target points. It mainly depends on the ones in the red circle, and the others are used for visualization.
path_planning generation path is passed to LQR through topic /path_ In node, LQR_node for tracking.
Code deployment
1, Custom library functions (LQR, tool, trajectory)
Based on the previous blog, I have written customized related library functions, including LQR, tool, trajectory, and matplot. However, matplot can be used in ROS instead of rviz, so it is discarded.
1.Tool Optimization
In order to make the path generation process more organized, the library function is optimized. In the Tool, the path point parameters are only set to x, y, yaw, and the curvature K selection is calculated for the obtained path in the controller. Added a function to calculate curvature K.
Tool.h
#pragma once
#include <iostream>
#include <math.h>
#include <vector>
using namespace std;
#define pi acos(-1)
//Define path points
typedef struct waypoint {
int ID;
double x, y, yaw;//x,y
}waypoint;
//Define trolley status
typedef struct vehicleState {
double x, y, yaw, v, kesi;//x. Y, yaw, front wheel deflection angle kesi
}vehicleState;
//Define control quantity
typedef struct U {
double v;
double kesi;//Speed v, front wheel deflection angle kesi
}U;
double cal_K(vector<waypoint> waypoints, int index);//Calculate curvature K
Tool.cpp
#include<iostream>
#include<LQR_track/Tool.h>
double cal_K(vector<waypoint> waypoints, int index){
double res;
//Solving first-order derivative and second-order derivative by difference method
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
//Calculate the curvature: let the curve r(t) =(x(t),y(t)), then the curvature 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 optimization
The parameter interface that can be imported from the launch file is set to design the track starting point, length limit and other information, and the user-defined track custom is added_ Path, just operate slowly according to the notes in 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;//Trajectory calculation frequency
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 meters
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
//Calculate tangent direction and store
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 optimization
It doesn't seem to be optimized. It's the same as the original. It's still attached with code:
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;
//Equation of state variable: x = [x_e y_e yaw_e]^t
//Equation of state control input: 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;//Vehicle wheelbase
double T;//Sampling interval
double x_car, y_car, yaw_car, x_d, y_d, yaw_d;//Vehicle pose and target point pose
double v_d, kesi_d;//Desired speed and front wheel deflection
double Q3[3];//Q weight, three items
double R2[2];//R weight, two items
int temp = 0;
public:
void initial(double L_, double T_, vehicleState car, waypoint waypoint, U U_r, double* Q_, double* R_);//initialization
void param_struct();//Structural equation of state parameters
Matrix2x3 cal_Riccati();//Solution of Riccati equation
U cal_vel();//LQR controller calculation speed
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 matrix is: \n" < < Q < < endl;
R << R2[0], 0.0,
0.0, R2[1];
//Cout < < "R matrix is: \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 matrix is: \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 matrix is: \n" < < B_ d << endl;
X_e << x_car - x_d, y_car - y_d, yaw_car - yaw_d;
//Cout < < "x _e matrix is: \n" < < x_ e << endl;
}
Matrix2x3 LQR::cal_Riccati() {
int N = 150;//Iteration termination times
double err = 100;//Error value
double err_tolerance = 0.01;//Error convergence threshold
Matrix3x3 Qf = Q;
Matrix3x3 P = Qf;//Iteration initial value
//Cout < < "P initial matrix is \n" < < p < < endl;
Matrix3x3 Pn;//Calculated latest P matrix
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;//Iterative formula
//Cout < < "convergence error is" < < (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 < < iterations < < ITER_ num << endl;
break;
}
P = Pn;
}
//Cout < < "P matrix is \n" < < p < < endl;
//P = Q;
Matrix2x3 K = -(R + B_d.transpose() * P * B_d).inverse() * B_d.transpose() * P * A_d;//Feedback rate K
return K;
}
U LQR::cal_vel() {
U output;
param_struct();
Matrix2x3 K = cal_Riccati();
Matrix2x1 U = K * X_e;
//Cout < < "feedback gain K is: \n" < < K < < endl;
//Cout < < control input u is: \n < < U < < endl;
output.v = U[0] + v_d;
output.kesi = U[1] + kesi_d;
return output;
}
void LQR::test() //Controller effect 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. Compilation and generation of custom libraries in ROS
You can refer to this article: 5. Call of user-defined header file and source file for ROS learning , if you don't want to see it, you can follow my process.
First, refer to the previous blog: Specific operation steps of ROS C++ calling osqp eigen Library , complete: add the Eigen module of ROS.
Then write the following code under Build:
Attention!!!: The absolute path of header file and source file must be written as your own!!
###########
## Build ##
###########
include_directories(
include
${catkin_INCLUDE_DIRS}
${Eigen_INCLUDE_DIRS}
)
####The following absolute path must be written as your own!!!
add_library(LQR_LIBRARIES
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/include/LQR_track/Tool.h
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/include/LQR_track/trajectory.h
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/include/LQR_track/LQR.h
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/src/Tool.cpp
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/src/trajectory.cpp
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/src/LQR.cpp
)
target_link_libraries(LQR_LIBRARIES ${catkin_LIBRARIES})That's good. Compile it to generate LQR_ The name of the track feature pack is LQR_LIBRARIES custom library.
2, Call library functions to complete track generation (path_planning.cpp)
Write a path publisher based on ROS publisher mechanism, and the path data type is nav_msgs::Path. You can call the trajectory directly to obtain the path, and then convert the data type (from vector<waypoint> to nav_msgs::Path) to publish it.
At the same time, the process of importing parameters from the launch file is: launch → path_planning→trajectory.h
path_planning.cpp
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
#include <vector>
#include "LQR_track/trajectory.h"
#include "ros/init.h"
#include "ros/node_handle.h"
#include "ros/publisher.h"
#include "ros/rate.h"
#include "ros/time.h"
#include <math.h>
#include <tf/tf.h>
double x_start,y_start,limit_x,limit_y;
string trajectory_type;
nav_msgs::Path produce_path(){
trajectory* trajec = new trajectory(x_start,y_start,trajectory_type,limit_x,limit_y);//Instantiate the trajectory class;
vector<waypoint> waypoint_vec;//Define the vector class to receive the path generated by trajec and get several groups of [ID,x,y]
nav_msgs::Path waypoints;//Define nav_msgs::Path class is used to construct the Path message of ROS
//Get path
trajec->refer_path();
waypoint_vec = trajec->get_path();
//Construct Path message for ROS
waypoints.header.stamp = ros::Time::now();
waypoints.header.frame_id = "map";
for(int i =0;i<waypoint_vec.size();i++){
geometry_msgs::PoseStamped this_pose;
this_pose.header.seq = i;
//ROS_INFO("path_id is %d", this_pose.header.seq);
this_pose.header.frame_id = "map";
this_pose.pose.position.x = waypoint_vec[i].x;
this_pose.pose.position.y = waypoint_vec[i].y;
this_pose.pose.position.z = 0;
geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(waypoint_vec[i].yaw);
//ROS_INFO("the yaw is %f",waypoint_vec[i].yaw);
this_pose.pose.orientation.x = goal_quat.x;
this_pose.pose.orientation.y = goal_quat.y;
this_pose.pose.orientation.z = goal_quat.z;
this_pose.pose.orientation.w = goal_quat.w;
waypoints.poses.push_back(this_pose);
}
return waypoints;//Return Path message for ROS
}
int main(int argc,char **argv){
ros::init(argc, argv, "path_produce");
ros::NodeHandle n;
ros::NodeHandle n_prv("~");
n_prv.param<double>("x_start",x_start,0.0);
n_prv.param<double>("y_start",y_start,0.0);
n_prv.param<string>("trajectory_type",trajectory_type,"line");
n_prv.param<double>("limit_x",limit_x,10);
n_prv.param<double>("limit_y",limit_y,0.0);
ros::Publisher path_pub = n.advertise<nav_msgs::Path>("path",10);
ros::Rate loop_rate(1);
while(ros::ok()){
nav_msgs::Path path = produce_path();
ROS_INFO("the trajectory size is: %ld",path.poses.size());
path_pub.publish(path);
loop_rate.sleep();
}
}3, Call the library function to complete the track tracking of LQR controller
1. Simulation program (LQR_node.cpp)
This file defines two classes, one is the class that processes the subscribed path (Class Path), and the other is the class that is responsible for track tracking (Class LQR_node). The implementation steps of its tracking function are as follows:
Implementation steps of tracking function:
1. Get the path point, which is completed when you subscribe to the callback function addpointcallback in the ros::spinOnce() phase under the while(ros::ok()) function
2. Search for path points
3. Obtain the path point information and construct the expected control quantity
4. First make a deceleration judgment to determine the longitudinal expected speed v_r. And the current action of the robot: tracking or reached the goal
5. Construct the weight matrix from the parameter Q of the launch file_ set,R_ Set import
6. Use LQR controller to calculate the speed and front wheel angle, in which the finite amplitude process
7. Post topics, including the calculation of angular velocity
8. Calculate the position and posture of the trolley at the next moment with the kinematic model
8. Judge whether it is necessary to turn off the controller. If it reaches the end point, turn it off
9.loop_rate.sleep(), end a cycle and prepare to start the next tracking work
Put the code directly:
LQR_node.cpp:
#include <ros/ros.h>
#include <iostream>
#include "LQR_track/LQR.h"
#include <vector>
#include "ros/node_handle.h"
#include "ros/publisher.h"
#include "tf/LinearMath/Quaternion.h"
#include "tf/transform_broadcaster.h"
#include "visualization_msgs/Marker.h"
#include <nav_msgs/Path.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
using namespace std;
double freq,L,V_DESIRED;//Sampling frequency, vehicle wheelbase, desired speed
double v_max;//Maximum speed
bool limit_v_and_kesi;//Whether the amplitude is limited (for akaman steering vehicles, the amplitude is limited, and the omni-directional vehicle is OK)
double initial_x,initial_y,initial_yaw,initial_v,initial_kesi;//Initialize vehicle posture, speed and front wheel angle
std::vector<double> Q_set;
std::vector<double> R_set;
double slow_LEVE1_DISTANCE,slow_LEVE2_DISTANCE,slow_LEVE1_V,slow_LEVE2_V,goal_tolerance_DISTANCE;//Define secondary deceleration distance and speed
#define pi acos(-1)
#define T 1/freq / / sampling time
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;
return car;
}
class Path {
private:
vector<waypoint> path;
public:
//Add a new path point
void Add_new_point(waypoint& p)
{
path.push_back(p);
}
void Add_new_point(vector<waypoint>& p)
{
path = p;
}
//Number of path points
unsigned int Size()
{
return path.size();
}
//Get path points
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;
return p;
}
vector<waypoint> Get_waypoints(){
return path;
}
// Search the path points, compare the distance from the car to the starting point with the distance from the car to each point, and find the nearest target point index value
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;
}
}
//Before indexing to the end point, when (the distance Lf between the robot and the next target point) is less than (the distance L between the current target point and the next target point), index the next target point
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:
//car
vehicleState car;//Trolley status
U control;//Trolley control quantity [v,kesi]
double Q[3];
double R[2];
int lastIndex;//Last point index value
waypoint lastPoint;//Last point information
string action;//Current action of trolley: tracking or reach goal
//ROS
ros::Subscriber path_sub;//Subscription path, message type nav_msgs::Path
ros::Publisher vel_pub;//Publish speed information. The message type is geometry_msgs::Twist
ros::Publisher actual_state_pub;//Publish the actual position and posture of the car, and the message type is geometry_msgs::Pose2D
ros::Publisher visual_state_pub;//Publish the virtual track of the car to rviz, and the message type is visualization_msgs::Marker
geometry_msgs::Point visual_state_pose;
visualization_msgs::Marker visual_state_trajectory;
geometry_msgs::Pose2D actual_pose;
geometry_msgs::Twist vel_msg;
int temp;//Counting, used to judge whether the controller needs to be turned off when the end point is reached
public:
LQR* controller;
Path* path;
LQR_node(ros::NodeHandle& nh)//Add trajectory and initial position and posture of the car in initialization
{
controller = new LQR();
path = new Path();
//ROS:
path_sub = nh.subscribe("path",10,&LQR_node::addpointcallback,this);
vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel",10);
visual_state_pub = nh.advertise<visualization_msgs::Marker>("visualization_pose",10);
actual_state_pub = nh.advertise<geometry_msgs::Pose2D>("LQR_pose",10);
//robot state initialize:
car.x = initial_x;car.y = initial_y;car.yaw = initial_yaw;car.v = initial_v;car.kesi = initial_kesi;
action = "the car is tracking!!";
}
~LQR_node() {
delete(controller);
delete(path);
}
void addpointcallback(const nav_msgs::Path::ConstPtr& msg){
vector<waypoint> waypoints;
for(int i=0;i<msg->poses.size();i++){
waypoint waypoint;
//ROS_INFO("THE PATH[%d]'s ID is %d",i,msg->poses[i].header.seq);
waypoint.ID = msg->poses[i].header.seq;
waypoint.x = msg->poses[i].pose.position.x;
waypoint.y = msg->poses[i].pose.position.y;
//Get angle
double roll,pitch,yaw;
tf::Quaternion quat;
tf::quaternionMsgToTF(msg->poses[i].pose.orientation, quat);
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
waypoint.yaw = yaw;
waypoints.push_back(waypoint);
}
path->Add_new_point(waypoints);//Pass the vector array of path points to the path class
lastIndex = path->Size() - 1;
lastPoint = path->Get_waypoint(lastIndex);
}
double slow_judge(double distance) {
if (distance>=slow_LEVE2_DISTANCE&&distance <= slow_LEVE1_DISTANCE) {
return slow_LEVE1_V;
}
else if (distance>=goal_tolerance_DISTANCE&&distance < slow_LEVE2_DISTANCE) {
return slow_LEVE2_V;
}
else if (distance < goal_tolerance_DISTANCE) {
action = "the car has reached the goal!";
return 0.0;
}
else
{
return V_DESIRED;
}
}
//Controller process
void LQR_track() {
U U_r;
waypoint Point;
double K;
//ROS_INFO("path size is: %d",path->Size());
if(path->Size()!=0){
//Search path points
int target_index = path->Find_target_index(car);
//ROS_INFO("target index is : %d", target_index);
//Obtain path point information and construct expected control quantity
Point = path->Get_waypoint(target_index);//Get x,y
//ROS_INFO("waypoint information is x:%f,y:%f,yaw:%f", Point.x, Point.y, Point.yaw);
K = cal_K(path->Get_waypoints(),target_index);//Calculate curvature
//Deceleration judgment
double kesi = atan2(L * K, 1);
double v_distance = abs(sqrt(pow(car.x - lastPoint.x, 2) + pow(car.y - lastPoint.y, 2)));
//ROS_INFO("the distance is %f\n", v_distance);
U_r.v = slow_judge(v_distance);U_r.kesi = kesi;
ROS_INFO("%s",action.c_str());//Robot action
//ROS_INFO("the desired v is: %f,the desired kesi is: %f", U_r.v,U_r.kesi);
//Weight matrix
for(int q =0;q<Q_set.size();q++)Q[q] = Q_set[q];
for(int r =0;r<R_set.size();r++)R[r] = R_set[r];
if(Q[0]>=R[0]||Q[1]>=R[1])
ROS_WARN("Q >= R, the calculation result may be abnormal,please set R < Q ");
//Using LQR controller,
controller->initial(L, T, car, Point, U_r, Q, R);//Initialize controller
control = controller->cal_vel();//Calculation input [v, kesi]
if(U_r.v==0)control.v = 0;//Judge that if the expected speed is 0, the robot will stop
if(limit_v_and_kesi)control = v_and_kesi_limit(control);//Speed and front wheel angle limiting
ROS_INFO("the speed is: %f,the kesi is: %f", control.v, control.kesi);
//ROS_INFO("the car position is x: %f, y: %f", car.x, car.y);
//Topic release
PUB();
//Trolley pose status update
car = update_state(control, car);
//Controller shutdown judgment
shutdown_controller();
ROS_INFO("--------------------------------------");
}
}
//Control start stop function
void node_control() {
ros::Rate loop_rate(freq);
Marker_set();//Set Marker attribute
//Set tf coordinate conversion
tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
while (ros::ok()) {
transform.setOrigin(tf::Vector3(car.x, car.y, 0));
q.setRPY(0, 0, car.yaw);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "car"));
ros::spinOnce();
LQR_track();
loop_rate.sleep();
}
}
void PUB(){
visual_state_pose.x = car.x; visual_state_pose.y = car.y;
actual_pose.x = car.x; actual_pose.y = car.y; actual_pose.theta = car.yaw;
vel_msg.linear.x = control.v; vel_msg.angular.z = control.v*tan(control.kesi)/L;//Yaw rate is w = v*tan(kesi)/L
visual_state_trajectory.points.push_back(visual_state_pose);//visualization_msgs::Marker is a container, so now you need to push it inside_ Back structure, and then publish
visual_state_pub.publish(visual_state_trajectory);//Publish virtual tracks
vel_pub.publish(vel_msg);//Release speed
actual_state_pub.publish(actual_pose);//Release pose
}
void shutdown_controller(){
if(action == "the car has reached the goal!"){
temp+=1;
if(temp ==50){
ROS_WARN("shutdown the LQR controller!");
temp = 0;
ros::shutdown();
}
}
}
void Marker_set(){
//Set message type parameters
visual_state_trajectory.header.frame_id = "map";
visual_state_trajectory.header.stamp = ros::Time::now();
visual_state_trajectory.action = visualization_msgs::Marker::ADD;
visual_state_trajectory.ns = "LQR";
//Set point properties
visual_state_trajectory.id = 0;
visual_state_trajectory.type = visualization_msgs::Marker::POINTS;
visual_state_trajectory.scale.x = 0.02;
visual_state_trajectory.scale.y = 0.02;
visual_state_trajectory.color.r = 1.0;
visual_state_trajectory.color.a = 1.0;
}
U v_and_kesi_limit(U control_value){
if(control_value.v>=v_max)//Speed limiting
{
control_value.v = v_max;
ROS_WARN("The calculated value may be inaccurate ");
}
else if(control_value.v<=-v_max){
control_value.v = -v_max;
ROS_WARN("The calculated value may be inaccurate ");
}
if(control_value.kesi>=pi/2)//Front wheel angle limiting
{
control_value.kesi = pi/2;
ROS_WARN("The calculated value may be inaccurate ");
}
else if(control_value.kesi<=-pi/2){
control_value.kesi = -pi/2;
ROS_WARN("The calculated value may be inaccurate ");
}
return control_value;
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "LQR_node");
ros::NodeHandle n;
ros::NodeHandle n_prv("~");
n_prv.param<double>("freq",freq,20);
n_prv.param<double>("L",L,0.2);
n_prv.param<double>("V_DESIRED",V_DESIRED,0.5);
n_prv.param<double>("v_max",v_max,1.0);
n_prv.param<double>("initial_x",initial_x,0.0);
n_prv.param<double>("initial_y",initial_y,2.0);
n_prv.param<double>("initial_yaw",initial_yaw,0.0);
n_prv.param<double>("initial_v",initial_v,0.0);
n_prv.param<double>("initial_kesi",initial_kesi,0.1);
n_prv.param<double>("slow_LEVE1_DISTANCE",slow_LEVE1_DISTANCE,5.0);
n_prv.param<double>("slow_LEVE2_DISTANCE",slow_LEVE2_DISTANCE,2.0);
n_prv.param<double>("goal_tolerance_DISTANCE",goal_tolerance_DISTANCE,0.1);
n_prv.param<double>("slow_LEVE1_V",slow_LEVE1_V,0.35);
n_prv.param<double>("slow_LEVE2_V",slow_LEVE2_V,0.15);
n_prv.param<bool>("limit_v_and_kesi",limit_v_and_kesi,false);
n_prv.param("Q_set",Q_set,Q_set);
n_prv.param("R_set",R_set,R_set);
LQR_node* node = new LQR_node(n);
node->node_control();
return (0);
}
2. Experimental procedure (LQR_node_world.cpp)
This experimental program is very similar to the simulation program, but instead of using the kinematic model to calculate the position and posture of the car, it samples the position and posture of the car, so a subscription interface (odom_sub) is added
Implementation steps of tracking function:
1. Get the current position, posture and path point of the car, and complete it when you subscribe to the callback functions odomcallback and addpointcallback in the ros::spinOnce() stage under the while(ros::ok()) function
2. Search for path points
3. Obtain the path point information and construct the expected control quantity
4. First make a deceleration judgment to determine the longitudinal expected speed v_r. And the current action of the robot: tracking or reached the goal
5. Construct the weight matrix and import it from the parameters Q=[],R=[] of the launch file
6. Use LQR controller to calculate the speed and front wheel angle, in which the finite amplitude process
7. Post topics, including the calculation of angular velocity
8. Judge whether it is necessary to turn off the controller. If it reaches the end point, turn it off
9.loop_rate.sleep(), end a cycle and prepare to start the next tracking work
Then the tf that releases the map to the car in the program is also removed. After positioning, there will be tf in rviz, so it is no longer needed here.
LQR_node_world.cpp
#include <ros/ros.h>
#include <iostream>
#include "LQR_track/LQR.h"
#include <vector>
#include "ros/node_handle.h"
#include "ros/publisher.h"
#include "tf/LinearMath/Quaternion.h"
#include "tf/transform_broadcaster.h"
#include "visualization_msgs/Marker.h"
#include <nav_msgs/Path.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
using namespace std;
double freq,L,V_DESIRED;//Sampling frequency, vehicle wheelbase, desired speed
double v_max;//Maximum speed
bool limit_v_and_kesi;//Whether to limit the amplitude (for akaman steering vehicles, it is necessary to limit the amplitude, and the omni-directional vehicle is optional)
std::vector<double> Q_set;//Q matrix
std::vector<double> R_set;//R matrix
double slow_LEVE1_DISTANCE,slow_LEVE2_DISTANCE,slow_LEVE1_V,slow_LEVE2_V,goal_tolerance_DISTANCE;//Define secondary deceleration distance and speed
#define pi acos(-1)
#define T 1/freq / / sampling time
class Path {
private:
vector<waypoint> path;
public:
//Add a new path point
void Add_new_point(waypoint& p)
{
path.push_back(p);
}
void Add_new_point(vector<waypoint>& p)
{
path = p;
}
//Number of path points
unsigned int Size()
{
return path.size();
}
//Get path points
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;
return p;
}
vector<waypoint> Get_waypoints(){
return path;
}
// Search the path points, compare the distance from the car to the starting point with the distance from the car to each point, and find the nearest target point index value
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;
}
}
//Before indexing to the end point, when (the distance L_1between the robot and the next target point) is less than (the distance L_1between the current target point and the next target point) Index the next target point when
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:
//car
vehicleState car;//Trolley status
U control;//Trolley control quantity [v,kesi]
double Q[3];
double R[2];
int lastIndex;//Last point index value
waypoint lastPoint;//Last point information
string action;//Current action of trolley: tracking or reach goal
//ROS
ros::Subscriber path_sub;//Subscription path, message type nav_msgs::Path
ros::Subscriber odom_sub;//Subscribe to the current position and attitude information of the trolley, and the message type is nav_msgs::Odometry
ros::Publisher vel_pub;//Publish speed information. The message type is geometry_msgs::Twist
ros::Publisher actual_state_pub;//Publish the actual position and posture of the car, and the message type is geometry_msgs::Pose2D
ros::Publisher visual_state_pub;//Publish the virtual track of the car to rviz, and the message type is visualization_msgs::Marker
geometry_msgs::Point visual_state_pose;
visualization_msgs::Marker visual_state_trajectory;
geometry_msgs::Pose2D actual_pose;
geometry_msgs::Twist vel_msg;
geometry_msgs::Pose2D pose2d_robot;
int temp;//Counting, used to judge whether the controller needs to be turned off when the end point is reached
public:
LQR* controller;
Path* path;
LQR_node(ros::NodeHandle& nh)//Add trajectory and initial position and posture of the car in initialization
{
controller = new LQR();
path = new Path();
//ROS:
path_sub = nh.subscribe("path",10,&LQR_node::addpointcallback,this);
vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel",10);
visual_state_pub = nh.advertise<visualization_msgs::Marker>("visualization_pose",10);
actual_state_pub = nh.advertise<geometry_msgs::Pose2D>("LQR_pose",10);
odom_sub = nh.subscribe<nav_msgs::Odometry>("odom_tf", 1, &LQR_node::odomcallback, this);
//robot state initialize:
//car.x = car.y = car.yaw = car.v = car.kesi = 0;
action = "the car is tracking!!";
}
~LQR_node() {
delete(controller);
delete(path);
}
void odomcallback(const nav_msgs::Odometry::ConstPtr& odom_value)
{
pose2d_robot.x = odom_value->pose.pose.position.x;
pose2d_robot.y = odom_value->pose.pose.position.y;
pose2d_robot.theta = tf::getYaw(odom_value->pose.pose.orientation);
car.x = pose2d_robot.x;
car.y = pose2d_robot.y;
car.yaw = pose2d_robot.theta;
}
void addpointcallback(const nav_msgs::Path::ConstPtr& msg){
vector<waypoint> waypoints;
for(int i=0;i<msg->poses.size();i++){
waypoint waypoint;
//ROS_INFO("THE PATH[%d]'s ID is %d",i,msg->poses[i].header.seq);
waypoint.ID = msg->poses[i].header.seq;
waypoint.x = msg->poses[i].pose.position.x;
waypoint.y = msg->poses[i].pose.position.y;
//Get angle
double roll,pitch,yaw;
tf::Quaternion quat;
tf::quaternionMsgToTF(msg->poses[i].pose.orientation, quat);
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
waypoint.yaw = yaw;
waypoints.push_back(waypoint);
}
path->Add_new_point(waypoints);//Pass the vector array of path points to the path class
lastIndex = path->Size() - 1;
lastPoint = path->Get_waypoint(lastIndex);
}
double slow_judge(double distance) {
if (distance>=slow_LEVE2_DISTANCE&&distance <= slow_LEVE1_DISTANCE) {
return slow_LEVE1_V;
}
else if (distance>=goal_tolerance_DISTANCE&&distance < slow_LEVE2_DISTANCE) {
return slow_LEVE2_V;
}
else if (distance < goal_tolerance_DISTANCE) {
action = "the car has reached the goal!";
return 0.0;
}
else
{
return V_DESIRED;
}
}
//Controller process
/*Steps:
1.Obtain the current posture and path point of the car, and complete it when the callback functions odomcallback and addpointcallback are subscribed in the ros::spinOnce() stage under the while(ros::ok()) function
2.Search path points
3.Obtain path point information and construct expected control quantity
4.First make a deceleration judgment to determine the longitudinal expected speed v_r. And the current action of the robot: tracking or reached the goal
5.Construct the weight matrix and import it from the parameters Q=[],R=[] of the launch file
6.The LQR controller is used to calculate the speed and front wheel angle, in which the finite amplitude process
7.Post topics, including the calculation of angular velocity
8.Judge whether the controller needs to be shut down. If it reaches the end point, it will be shut down
9.loop_rate.sleep()，End one cycle and prepare for the next.
*/
void LQR_track() {
U U_r;
waypoint Point;
double K;
//ROS_INFO("path size is: %d",path->Size());
if(path->Size()!=0){
//Search path points
int target_index = path->Find_target_index(car);
//ROS_INFO("target index is : %d", target_index);
//Obtain path point information and construct expected control quantity
Point = path->Get_waypoint(target_index);//Get x,y
//ROS_INFO("waypoint information is x:%f,y:%f,yaw:%f", Point.x, Point.y, Point.yaw);
K = cal_K(path->Get_waypoints(),target_index);//Calculate curvature
//Deceleration judgment
double kesi = atan2(L * K, 1);
double v_distance = abs(sqrt(pow(car.x - lastPoint.x, 2) + pow(car.y - lastPoint.y, 2)));
//ROS_INFO("the distance is %f\n", v_distance);
U_r.v = slow_judge(v_distance);U_r.kesi = kesi;
ROS_INFO("%s",action.c_str());//Robot action
//ROS_INFO("the desired v is: %f,the desired kesi is: %f", U_r.v,U_r.kesi);
//Weight matrix
for(int q =0;q<Q_set.size();q++)Q[q] = Q_set[q];
for(int r =0;r<R_set.size();r++)R[r] = R_set[r];
if(Q[0]>=R[0]||Q[1]>=R[1])
ROS_WARN("Q >= R, the calculation result may be abnormal,please set R < Q ");
//Using LQR controller,
controller->initial(L, T, car, Point, U_r, Q, R);//Initialize controller
control = controller->cal_vel();//Calculation input [v, kesi]
if(U_r.v==0)control.v = 0;//Judge that if the expected speed is 0, the robot will stop
if(limit_v_and_kesi)control = v_and_kesi_limit(control);//Speed and front wheel angle limiting
ROS_INFO("the speed is: %f,the kesi is: %f", control.v, control.kesi);
//ROS_INFO("the car position is x: %f, y: %f", car.x, car.y);
//Topic release
PUB();
//Controller shutdown judgment
shutdown_controller();
ROS_INFO("--------------------------------------");
}
}
//Control start stop function
void node_control() {
ros::Rate loop_rate(freq);
Marker_set();//Set Marker attribute
while (ros::ok()) {
ros::spinOnce();
LQR_track();
loop_rate.sleep();
}
}
void PUB(){
visual_state_pose.x = car.x; visual_state_pose.y = car.y;
actual_pose.x = car.x; actual_pose.y = car.y; actual_pose.theta = car.yaw;
vel_msg.linear.x = control.v; vel_msg.angular.z = control.v*tan(control.kesi)/L;//Yaw rate is w = v*tan(kesi)/L
visual_state_trajectory.points.push_back(visual_state_pose);//visualization_msgs::Marker is a container, so now you need to push it inside_ Back structure, and then publish
visual_state_pub.publish(visual_state_trajectory);//Publish virtual tracks
vel_pub.publish(vel_msg);//Release speed
actual_state_pub.publish(actual_pose);//Release pose
}
void shutdown_controller(){
if(action == "the car has reached the goal!"){
temp+=1;
if(temp ==50){
ROS_WARN("shutdown the LQR controller!");
temp = 0;
ros::shutdown();
}
}
}
void Marker_set(){
//Set message type parameters
visual_state_trajectory.header.frame_id = "map";
visual_state_trajectory.header.stamp = ros::Time::now();
visual_state_trajectory.action = visualization_msgs::Marker::ADD;
visual_state_trajectory.ns = "LQR";
//Set point properties
visual_state_trajectory.id = 0;
visual_state_trajectory.type = visualization_msgs::Marker::POINTS;
visual_state_trajectory.scale.x = 0.02;
visual_state_trajectory.scale.y = 0.02;
visual_state_trajectory.color.r = 1.0;
visual_state_trajectory.color.a = 1.0;
}
U v_and_kesi_limit(U control_value){
if(control_value.v>=v_max)//Speed limiting
{
control_value.v = v_max;
ROS_WARN("The calculated value may be inaccurate ");
}
else if(control_value.v<=-v_max){
control_value.v = -v_max;
ROS_WARN("The calculated value may be inaccurate ");
}
if(control_value.kesi>=pi/2)//Front wheel angle limiting
{
control_value.kesi = pi/2;
ROS_WARN("The calculated value may be inaccurate ");
}
else if(control_value.kesi<=-pi/2){
control_value.kesi = -pi/2;
ROS_WARN("The calculated value may be inaccurate ");
}
return control_value;
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "LQR_node");
ros::NodeHandle n;
ros::NodeHandle n_prv("~");
n_prv.param<double>("freq",freq,20);
n_prv.param<double>("L",L,0.2);
n_prv.param<double>("V_DESIRED",V_DESIRED,0.5);
n_prv.param<double>("v_max",v_max,1.0);
n_prv.param<double>("slow_LEVE1_DISTANCE",slow_LEVE1_DISTANCE,5.0);
n_prv.param<double>("slow_LEVE2_DISTANCE",slow_LEVE2_DISTANCE,2.0);
n_prv.param<double>("goal_tolerance_DISTANCE",goal_tolerance_DISTANCE,0.1);
n_prv.param<double>("slow_LEVE1_V",slow_LEVE1_V,0.35);
n_prv.param<double>("slow_LEVE2_V",slow_LEVE2_V,0.15);
n_prv.param<bool>("limit_v_and_kesi",limit_v_and_kesi,false);
n_prv.param("Q_set",Q_set,Q_set);
n_prv.param("R_set",R_set,R_set);
LQR_node* node = new LQR_node(n);
node->node_control();
return (0);
}
3. Position and posture acquisition of small cars in the experiment (odom_listen_from_tf.cpp)
Everyone may have different positioning topics on the experimental platform. I'll simply write a posture acquisition program here, which is to monitor map to base_link or base_ It's good to output the coordinate transformation relationship of footprint. The output topic is odom_tf, the message type is nav_msgs::Odometry, this topic is LQR_ node_ The car posture subscribed in world.cpp.
odom_listen_from_tf.cpp
#include <ros/ros.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Odometry.h>
std::string base_frame;
int main(int argc, char** argv)
{
ros::init(argc, argv, "odom_listener");
ros::NodeHandle n;
ros::NodeHandle nh("~");
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener odomListener(tfBuffer);
nh.param<std::string>("base_frame", base_frame, "base_link");
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom_tf",10);
ros::Rate r(20);
while(ros::ok()){
geometry_msgs::TransformStamped transform;
try{
transform = tfBuffer.lookupTransform("map", base_frame, ros::Time(0));
}
catch (tf2::TransformException &ex){
ROS_ERROR("%s", ex.what());
ros::Duration(1.0).sleep();
continue;
}
tf2::Vector3 translation;
translation.setValue(transform.transform.translation.x,
transform.transform.translation.y,
transform.transform.translation.z);
tf2::Quaternion rotation;
rotation.setValue(transform.transform.rotation.x,
transform.transform.rotation.y,
transform.transform.rotation.z,
transform.transform.rotation.w);
tf2::Matrix3x3 m(rotation);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
/*ROS_INFO("\n=== Got Transform ===\n"
" Translation\n"
" x : %f\n y : %f\n z : %f\n"
" Quaternion\n"
" x : %f\n y : %f\n z : %f\n w : %f\n"
" RPY\n"
" R : %f\n P : %f\n Y : %f",
translation.x(), translation.y(), translation.z(),
rotation.x(), rotation.y(), rotation.z(), rotation.w(),
roll, pitch, yaw);*/
nav_msgs::Odometry odom;
odom.header.stamp = ros::Time::now();
odom.header.frame_id = "odom";
odom.pose.pose.position.x = translation.x();
odom.pose.pose.position.y = translation.y();
odom.pose.pose.position.z = translation.z();
odom.pose.pose.orientation.x = rotation.x();
odom.pose.pose.orientation.y = rotation.y();
odom.pose.pose.orientation.z = rotation.z();
odom.pose.pose.orientation.w = rotation.w();
odom.child_frame_id = base_frame;
odom.twist.twist.linear.x = 0;
odom.twist.twist.linear.y = 0;
odom.twist.twist.linear.z = 0;
odom_pub.publish(odom);
ros::spinOnce();
r.sleep();
}
return 0;
}
4, Compile all files
Don't forget on target_ link_ Add the previously generated library name LQR to libraries_ LIBRARIES:
###########
## Build ##
###########
include_directories(
include
${catkin_INCLUDE_DIRS}
${Eigen_INCLUDE_DIRS}
)
add_library(LQR_LIBRARIES
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/include/LQR_track/Tool.h
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/include/LQR_track/trajectory.h
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/include/LQR_track/LQR.h
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/src/Tool.cpp
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/src/trajectory.cpp
/home/dlrobot/dlrobot_robot/src/617_robot/lqr_track/src/LQR.cpp
)
target_link_libraries(LQR_LIBRARIES ${catkin_LIBRARIES})
link_directories(
${catkin_LIB_DIRS}
${Eigen_LIB_DIRS}
)
add_executable(LQR_node src/LQR_node.cpp)
add_dependencies(LQR_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(LQR_node ${catkin_LIBRARIES} LQR_LIBRARIES)
add_executable(path_planning src/path_planning.cpp)
add_dependencies(path_planning ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(path_planning ${catkin_LIBRARIES} LQR_LIBRARIES)
add_executable(lqr_odom_listen src/odom_listen_from_tf.cpp)
add_dependencies(lqr_odom_listen ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(lqr_odom_listen ${catkin_LIBRARIES} LQR_LIBRARIES)
add_executable(LQR_node_world src/LQR_node_world.cpp)
add_dependencies(LQR_node_world ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(LQR_node_world ${catkin_LIBRARIES} LQR_LIBRARIES)5, Configure rviz files
You can refer to the previous blog: Traditional model predictive control trajectory tracking - circular trajectory (function package has been updated)
6, Configure launch file
The chassis radar function and positioning function can be enabled without starting from the launch file of the function package, but there are three functions that must be enabled from the function package:
path_planning.launch
trajectory_ The type parameter is determined by the three track names written in trajectory.cpp. By default, three types are written, namely line, wave1, and wave2.
<launch>
<node pkg="lqr_track" type="path_planning" name="path_planning" output="screen">
<param name="x_start" value="0.0"/>
<param name="y_start" value="-0.25"/>
<param name="trajectory_type" value="line"/><!--choose the trajectory type default:line,wave1,wave2-->
<!--If you need to add a custom path, please see trajectory.h-->
<param name="limit_x" value="4.0"/>
<param name="limit_y" value="0.0"/>
</node>
<!--rviz-->
<!--node name="path_rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find lqr_track)/rviz/path_show.rviz">
</node-->
</launch>
LQR_track.launch
There are vehicle information, initialization posture, controller parameter configuration, and relevant explanations or precautions are also written in the notes
<launch>
<node pkg="lqr_track" type="LQR_node" name="LQR_node" output="screen">
<!--Basic vehicle information--><!--kesi is the Vehicle front wheel deflection angle-->
<param name="L" value="0.2"/><!--Vehicle wheelbase-->
<param name="V_DESIRED" value="0.5"/>
<param name="v_max" value="1.0"/>
<!--car initial state-->
<param name="initial_x" value="-0.127"/>
<param name="initial_y" value="-0.1474"/>
<param name="initial_yaw" value="0.0138"/>
<param name="initial_kesi" value="0.0"/>
<!--controller information-->
<param name="freq" value="20"/><!--control freq-->
<param name="slow_LEVE1_DISTANCE" value="4.0"/><!--First stage deceleration distance-->
<param name="slow_LEVE2_DISTANCE" value="1.0"/><!--Secondary deceleration distance-->
<param name="goal_tolerance_DISTANCE" value="0.1"/><!--Tracking stop distance-->
<param name="slow_LEVE1_V" value="0.5"/>
<param name="slow_LEVE2_V" value="0.15"/>
<param name="limit_v_and_kesi" value="true"/><!--If it is an akaman steering car, it must be limited. If it is an omni-directional car, it is optional-->
<rosparam param="Q_set">[1.0,1.0,1.0]</rosparam><!--State weight matrix Q = diag{Q1,Q2,Q3},please set Q<R-->
<rosparam param="R_set">[5.0,5.0]</rosparam><!--Control input weight matrix R = diag{R1,R2},please set Q<R-->
</node>
<!--rviz-->
<node name="lqr_track_rviz" pkg="rviz" type="rviz" required="true" args="-d $(find lqr_track)/rviz/track.rviz"/>
</launch>
LQR_track_world.launch
<launch>
<node pkg="lqr_track" type="LQR_node_world" name="LQR_node_world" output="screen">
<!--Basic vehicle information--><!--kesi is the Vehicle front wheel deflection angle-->
<param name="L" value="0.24"/><!--Vehicle wheelbase-->
<param name="V_DESIRED" value="0.5"/>
<param name="v_max" value="1.0"/>
<!--controller information-->
<param name="freq" value="20"/><!--control freq-->
<param name="slow_LEVE1_DISTANCE" value="4.0"/><!--First stage deceleration distance-->
<param name="slow_LEVE2_DISTANCE" value="1.0"/><!--Secondary deceleration distance-->
<param name="goal_tolerance_DISTANCE" value="0.2"/><!--Tracking stop distance-->
<param name="slow_LEVE1_V" value="0.5"/>
<param name="slow_LEVE2_V" value="0.15"/>
<param name="limit_v_and_kesi" value="true"/><!--If it is an akaman steering car, it must be limited. If it is an omni-directional car, it is optional-->
<rosparam param="Q_set">[1.0,1.0,1.0]</rosparam><!--State weight matrix Q = diag{Q1,Q2,Q3},please set Q<R-->
<rosparam param="R_set">[4.0,4.0]</rosparam><!--Control input weight matrix R = diag{R1,R2},please set Q<R-->
</node>
<node pkg="lqr_track" type="lqr_odom_listen" name="lqr_odom_listen" output="screen"/>
</launch>
simulation result
This project has been simulated for three times:
1, Linear simulation trajectory_type = line
LQR_line_simulation
2, Wave line 1 simulation trajectory_type = wave1
LQR_wave1_simulation
3, Wave line 2 simulation trajectory_type = wave2
LQR_wave2_simulation
experimental result
Experimental process:
1. Open the robot chassis and radar to ensure that the radar data can be received
You can configure launch/turn in the function package_ on_ base_ and_ Ladier.launch file to start the territory radar at the same time
2. Turn on the robot positioning function to ensure that there is a map to base_ tf conversion of link (or base_footprint),
If you have a cartographer, you can run the following command: roslaunch lqr_track carto_robot_localization.launch
3. Carry out robot positioning work and go near the starting point of the path. Don't put it one meter beyond the starting point of the path. The tracking effect will become worse, which is related to the controller parameters, such as the selection of Q and P matrices
4.roslaunch lqr_track LQR_track_world.launch
5.roslaunch lqr_track path_planning.launch
Among them, green is the desired trajectory, red is the actual trajectory of the tracking process, and the tracking error may be related to vehicle parameters (such as front and rear wheelbase) or controller parameters.
The specific experiments are as follows:
line_track_experiment
Complete program
For the complete procedure, see: Bitbucket
git clone to the workspace, just follow the README.md operation in the function package