MPC轨迹跟踪——基于ROS系统和全向车实验平台

前言

        之前写过一次MPC，但代码框架非常杂乱，所以做出了更新。内存大一点吧，我的虚拟机内存8G，跑过一次仿真，后面重启就打不开了。建议备份一个新的虚拟机来跑仿真。

思路

思路就是/path_palnning向/MPC_node节点发送目标点，然后调用MPC_optimizer优化器进行解算返回解算结果。

前期准备

一、安装python3-pip

sudo apt-get install python3-pip

可能的问题是找不到相关安装包，我是直接换源了，采用阿里云源，按照下面链接里面来换的：

ubuntu18.04换源

二、安装cvxpy库

优化器采用python3中的cvxpy库进行解算，所以要提前安装。

分步安装：python cvxpy包安装教程，其中不需要cvxopt库

懒得翻链接就直接按照如下顺序：

pip3 install numpy
pip3 install mkl
pip3 install scs
pip3 install ecos
pip3 install osqp
pip3 install cvxpy

三、安装python3-rospkg

在后面启动节点后可能会没有rospkg这个库

按照如下命令安装：

pip3 install rospkg

最好还是别用sudo apt-get install python3-rospkg安装，会把ros-melodic-desktop-full卸载。

四、配置功能包，可编译.msg和.srv

        因为后面要用到自定义消息类型和自定义服务数据类型，需要在CmakeLists.txt和package.xml中进行配置。

代码部署

一、自定义库函数

1.Tool工具库

        库函数中定义了结构体数组如路径点waypoint、小车状态vehicleState、控制量U，并定义了计算曲率函数cal_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<mpc_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里面的备注慢慢操作就好了。

trajecoty.h

#include <iostream>
#include <vector>
#include "mpc_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 "mpc_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.ROS中自定义库的编译生成

可以参考一下这一篇：5.ROS学习之自定义头文件和源文件的调用，如果不想看的话可以就按照我这个过程就好了。

在Build下面写上如下代码：

注意头文件和源文件的绝对路径一定要写成自己的！！

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
add_library(MPC_LIBRARIES
  /home/cute/catkin_ws/src/mpc_track/include/mpc_track/Tool.h
  /home/cute/catkin_ws/src/mpc_track/src/Tool.cpp
  /home/cute/catkin_ws/src/mpc_track/include/mpc_track/trajectory.h
  /home/cute/catkin_ws/src/mpc_track/src/trajectory.cpp
)

target_link_libraries(MPC_LIBRARIES ${catkin_LIBRARIES})

二、调用库函数完成轨迹生成工作(path_planning.cpp)

基于ROS的发布者机制去写一个路径发布程序，路径数据类型为nav_msgs::Path。直接调用trajectory就可以实现路径的获取，然后转换一下数据类型(从vector<waypoint>转换为nav_msgs::Path)就可以发布出去啦。

同时从launch文件传入参数流程是：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 "mpc_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);//实例化trajectory类；
    vector<waypoint> waypoint_vec;//定义vector类用于接收由trajec生成的路径,得到若干组[ID,x,y]
    nav_msgs::Path waypoints;//定义nav_msgs::Path类用于构造ROS的Path消息

    //获取路径
    trajec->refer_path();
    waypoint_vec = trajec->get_path();

    //构造适用于ROS的Path消息
    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;//返回适用于ROS的Path消息
}


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);
    int temp = 0;
    while(ros::ok()){
	temp++;
        nav_msgs::Path path = produce_path();
        ROS_INFO("the trajectory size is: %ld",path.poses.size());
        path_pub.publish(path);
	if(temp>=2){ros::shutdown();}
        loop_rate.sleep();
    }
}

三、自定义消息类型(control.msg)

        该消息类型由速度v、前轮转角ksi两个控制量组成，方便信息传输。

control.msg

float32  v  #speed
float32  kesi  #Front wheel angle

四、自定义服务数据(mpc.srv)

        服务数据中，从客户端传入参考局部路径(state_ref)、参