前言
之前写过一次MPC，但代码框架非常杂乱，所以做出了更新。内存大一点吧，我的虚拟机内存8G，跑过一次仿真，后面重启就打不开了。建议备份一个新的虚拟机来跑仿真。
思路
思路就是/path_palnning向/MPC_node节点发送目标点，然后调用MPC_optimizer优化器进行解算返回解算结果。
前期准备
一、安装python3-pip
sudo apt-get install python3-pip可能的问题是找不到相关安装包，我是直接换源了，采用阿里云源，按照下面链接里面来换的：
二、安装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);//计算曲率KTool.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)、参考控制量序列(control_ref)、移动机器人当前位姿(state)，给出的回应是最优控制量(opt_control)、预测轨迹(state_pre)、计算时间(solve_time)。
mpc.srv
geometry_msgs/Pose2D[] state_ref # state_reference
mpc_track/control[] control_ref # control_reference
geometry_msgs/Pose2D state #car_state
# request
---
# respond
mpc_track/control opt_control #control valve
geometry_msgs/Pose2D[] state_pre #state_predictive
float32 solve_time #optimize time五、优化器(服务端)构建(mpc_optimizer.py)
mpc_optimizer.py优化器步骤：
1.声明服务名称为"mpc_optimization"，并定义回调函数callback
2.在回调函数中解包，得到所需要的当前位姿、局部路径、参考控制量序列
3.将上述内容传入mpc_optimize_function中通过cvxpy库进行二次规划解算得到最优控制量、预测路径、计算时间
4.将最优控制量、预测路径、计算时间进行打包，传给mpc_node(在这里作为客户端)
5.rospy.spin()等待唤醒
mpc_optimizer.py
注意这里是用python3来编译！！！！
这里的优化算法参考了这篇博客：【自动驾驶】模型预测控制(MPC)实现轨迹跟踪，再结合自己对MPC的理解。
并且为了提高运行速度，用while函数替代了所有for循环，因为在python中采用for循环时，会把所有结果先列出来依次运行，而while函数跟C++的for循环一样是一次一次进行的。
#! /usr/bin/env python3
# -*- coding: UTF-8 -*-
import numpy as np
import cvxpy
import rospy
from mpc_track.srv import *
from geometry_msgs.msg import *
from mpc_track.msg import *
import math
import time
global prediction_horizon, freq, L, MAX_VEL_delta, MAX_DSTEER, MAX_ANGULAR_delta
def get_nparray_from_matrix(x):
return np.array(x).flatten()
def state_space(control_ref, ref_yaw):
v_d = control_ref[0]
ref_delta = control_ref[1]
A = np.matrix([
[1.0, 0.0, -v_d * dt * math.sin(ref_yaw)],
[0.0, 1.0, v_d * dt * math.cos(ref_yaw)],
[0.0, 0.0, 1.0]])
B = np.matrix([
[dt * math.cos(ref_yaw), 0],
[dt * math.sin(ref_yaw), 0],
[dt * math.tan(ref_delta), v_d * dt / (L * math.cos(ref_delta) * math.cos(ref_delta))]
])
return A, B
def mpc_optimize_function(car_state, state_reference, control_reference):
dsteer_limit = min(abs(math.atan(MAX_ANGULAR_delta*L/(control_reference[0, 0] + MAX_VEL_delta))), MAX_DSTEER)
# print(dsteer_limit)
x = cvxpy.Variable((3, prediction_horizon + 1))
u = cvxpy.Variable((2, prediction_horizon))
cost = 0 # 代价函数
constraints = [] # 约束条件
t = 0
while t < prediction_horizon:
A, B = state_space(control_reference[:, t], state_reference[2, t])
# U成本函数
cost += cvxpy.quad_form(u[:, t], R)
# X成本函数
cost += cvxpy.quad_form(x[:, t] - state_reference[:, t], Q)
# 状态方程约束
constraints += [x[:, t + 1] - state_reference[:, t] == A @ (x[:, t] - state_reference[:, t]) + B @ u[:, t]]
t += 1
# 状态量初值为当前小车位姿
constraints += [(x[:, 0]) == car_state]
constraints += [u[0, :] <= MAX_VEL_delta, -MAX_VEL_delta <= u[0, :]]
constraints += [u[1, :] <= dsteer_limit, -dsteer_limit <= u[1, :]]
prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
s_l = time.time()
prob.solve(verbose=False, warm_start=True, solver=cvxpy.SCS)
solve_time = time.time() - s_l
if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:
opt_x = get_nparray_from_matrix(x.value[0, :])
opt_y = get_nparray_from_matrix(x.value[1, :])
opt_yaw = get_nparray_from_matrix(x.value[2, :])
opt_v_dd = get_nparray_from_matrix(u.value[0, :])
opt_kesi_dd = get_nparray_from_matrix(u.value[1, :])
else:
rospy.logerr("Error: Cannot solve mpc..")
rospy.signal_shutdown("Error: Cannot solve mpc..")
opt_v_dd, opt_kesi_dd, opt_x, opt_y, opt_yaw = None, None, None, None, None,
opt_v = opt_v_dd[0] + control_reference[0, 0]
opt_kesi = opt_kesi_dd[0] + control_reference[1, 0]
return opt_v, opt_kesi, opt_x, opt_y, opt_yaw, solve_time
def callback(req):
# 数据预处理
state_reference = np.zeros((3, prediction_horizon))
control_reference = np.zeros((2, prediction_horizon))
car_state = np.zeros(3)
car_state[0] = req.state.x
car_state[1] = req.state.y
car_state[2] = req.state.theta
i = 0
while i < len(req.state_ref):
state_reference[0, i] = req.state_ref[i].x
state_reference[1, i] = req.state_ref[i].y
state_reference[2, i] = req.state_ref[i].theta
control_reference[0, i] = req.control_ref[i].v
control_reference[1, i] = req.control_ref[i].kesi
i += 1
# 向优化函数传参解算
v, kesi, opt_x, opt_y, opt_yaw, time = mpc_optimize_function(car_state, state_reference, control_reference)
# 数据打包
opt_control = control()
opt_control.v = v
opt_control.kesi = kesi
solve_time = time
state_predictive = []
j = 0
while j < prediction_horizon+1:
state_pre = Pose2D()
state_pre.x = opt_x[j]
state_pre.y = opt_y[j]
state_pre.theta = opt_yaw[j]
state_predictive.append(state_pre)
j += 1
return mpcResponse(opt_control, state_predictive, solve_time)
if __name__ == "__main__":
rospy.init_node("mpc_optimizer")
server = rospy.Service("mpc_optimization", mpc, callback)
prediction_horizon = rospy.get_param('~prediction_horizon')
freq = rospy.get_param('~freq')
L = rospy.get_param('~L')
MAX_VEL_delta = rospy.get_param('~MAX_VEL_delta')
MAX_ANGULAR_delta = rospy.get_param('~MAX_ANGULAR_delta')
MAX_DSTEER = rospy.get_param('~MAX_DSTEER')
MAX_DSTEER = np.deg2rad(MAX_DSTEER)
R_set = rospy.get_param('~R_set')
Q_set = rospy.get_param('~Q_set')
dt = 1/freq
Q = np.diag(Q_set) # state cost matrix
R = np.diag(R_set) # input cost matrix
rospy.loginfo("ready to optimize")
rospy.spin()
六、模型预测控制器节点(MPC_node.cpp)
跟踪功能实现步骤：
1.获取路径点，在while(ros::ok())函数下面ros::spinOnce()阶段订阅回调函数addpointcallback时完成
2.搜索路径点
3.获取局部路径state_reference信息，构造对应的期望控制量序列control_reference
在构造control_reference中有减速判断，确定了期望速度和机器人当前动作
4.将当前位姿、局部路径、对应期望控制量序列传入优化器中，采用server/client的形式调用优化服务(mpc_optimizer.py)
5.计算速度和前轮转角
6.发布话题，其中有角速度的计算
7.用运动学模型计算小车下一时刻位姿
8.判断是否需要关闭控制器，如果到达终点就关闭
9.loop_rate.sleep()，结束一次循环，准备开始下一次个跟踪工作
MPC_node.cpp
#include <ros/ros.h>
#include <iostream>
#include <vector>
#include <mpc_track/trajectory.h>
#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>
#include <mpc_track/mpc.h>
#include <mpc_track/control.h>
using namespace std;
double freq,L,V_DESIRED;//采样频率，车辆轴距，期望速度
int prediction_horizon;//预测时域
bool limit_v_and_kesi;//是否限幅(对于阿卡曼转向车辆需要限幅，全向车倒还好)
double initial_x,initial_y,initial_yaw,initial_v,initial_kesi;//初始化车辆位姿，速度和前轮转角
double slow_LEVE1_DISTANCE,slow_LEVE2_DISTANCE,slow_LEVE1_V,slow_LEVE2_V,goal_tolerance_DISTANCE;//定义二级减速距离和速度
#define pi acos(-1)
#define T 1/freq //采样时间
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:
//添加新的路径点
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;
return p;
}
vector<waypoint> Get_waypoints(){
return path;
}
// 搜索路径点, 将小车到起始点的距离与小车到每一个点的距离对比，找出最近的目标点索引值
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;
}
vector<waypoint> get_local_path(int index){
vector<waypoint> local_path;
int num = Size();
//ROS_INFO("%d", num);
for (int i = 0; i<prediction_horizon; i++){
if(index + i<num){
local_path.push_back(Get_waypoint(index + i));
}
else{
local_path.push_back(Get_waypoint(num-1));
}
/*ROS_INFO("local_path hs %d numbers\n"
"the ID is %d\nthe x is %f\n"
"the y is %f\nthe yaw is %f\n",
local_path.size(),local_path[i].ID,local_path[i].x,local_path[i].y,local_path[i].yaw);*/
}
return local_path;
}
};
class MPC_node {
private:
//car
vehicleState car;//小车状态
U control;//小车控制量[v,kesi]
int lastIndex;//最后一个点索引值
waypoint lastPoint;//最后一个点信息
string action;//小车目前动作：跟踪或跟踪完成(tracking or reach goal!)
//ROS
ros::Subscriber path_sub;//订阅路径，消息类型为nav_msgs::Path
ros::Publisher vel_pub;//发布速度信息，消息类型为geometry_msgs::Twist
ros::Publisher actual_state_pub;//发布小车实际位姿，消息类型为geometry_msgs::Pose2D
ros::Publisher visual_state_pub;//向rviz发布小车虚拟轨迹，消息类型为visualization_msgs::Marker
ros::Publisher predictive_state_pub;//向rviz发布小车预测轨迹，消息类型为visualization_msgs::Marker
ros::ServiceClient mpc_optimization; //建立优化客户端
geometry_msgs::Point visual_state_pose;
visualization_msgs::Marker visual_state_trajectory;
nav_msgs::Path predictive_state_path;
geometry_msgs::Pose2D actual_pose;
geometry_msgs::Twist vel_msg;
vector<geometry_msgs::Pose2D> state_prediction;
int temp;//计数，达到终点时，用于判断控制器是否需要关闭
public:
Path* path;
MPC_node(ros::NodeHandle& nh)//初始化中添加轨迹、小车初始位姿
{
path = new Path();
//ROS:
path_sub = nh.subscribe("path",10,&MPC_node::addpointcallback,this);
vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel",10);
visual_state_pub = nh.advertise<visualization_msgs::Marker>("visualization_pose",10);
predictive_state_pub = nh.advertise<nav_msgs::Path>("predictive_pose",10);
actual_state_pub = nh.advertise<geometry_msgs::Pose2D>("MPC_pose",10);
mpc_optimization = nh.serviceClient<mpc_track::mpc>("mpc_optimization");
//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!!";
}
~MPC_node() {
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;
//获取角度
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);//路径点vector数组传到path类中
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;
}
}
//参考状态获取函数
vector<geometry_msgs::Pose2D> get_state_reference(int index){
vector<geometry_msgs::Pose2D> state_reference;
vector<waypoint> local_path = path->get_local_path(index);
for (int i = 0; i<local_path.size();i++){
geometry_msgs::Pose2D point;
point.x = local_path[i].x;
point.y = local_path[i].y;
point.theta = local_path[i].yaw;
state_reference.push_back(point);
}
//ROS_INFO("state_reference length is %d",state_reference.size());
return state_reference;
}
//参考控制量获取函数
vector<mpc_track::control> get_control_reference(int index, double distance){
vector<mpc_track::control> control_reference;
vector<waypoint> local_path = path->get_local_path(index);
double v_judge = slow_judge(distance);
for (int i = 0; i<local_path.size();i++){
mpc_track::control uu;
uu.v = v_judge;
//接着计算对应的前轮转角参考量
double K = cal_K(path->Get_waypoints(),local_path[i].ID);//计算曲率
uu.kesi = atan2(L * K, 1);
control_reference.push_back(uu);
//ROS_INFO("the v_desired is %f\nthe kesi_desired is %f\n",uu.v, uu.kesi);
}
//ROS_INFO("control_reference length is %d",control_reference.size());
return control_reference;
}
//控制器流程
/*
跟踪功能实现步骤：
1.获取路径点，在while(ros::ok())函数下面ros::spinOnce()阶段订阅回调函数addpointcallback时完成
2.搜索路径点
3.获取局部路径state_reference信息，构造对应的期望控制量序列control_reference
在构造control_reference中有减速判断，确定了期望速度和机器人当前动作
4.将当前位姿、局部路径、对应期望控制量序列传入优化器中，采用server/client的形式调用优化服务(mpc_optimizer.py)
5.计算速度和前轮转角
6.发布话题，其中有角速度的计算
7.用运动学模型计算小车下一时刻位姿
8.判断是否需要关闭控制器，如果到达终点就关闭
9.loop_rate.sleep()，结束一次循环，准备开始下一次个跟踪工作
*/
void MPC_track() {
//ROS_INFO("path size is: %d",path->Size());
if(path->Size()!=0){
//搜索路径点
int target_index = path->Find_target_index(car);
//ROS_INFO("target index is : %d", target_index);
//获取路径点信息，构造期望控制量(分为参考状态和参考控制量)
//参考状态
vector<geometry_msgs::Pose2D> state_reference = get_state_reference(target_index);
//参考控制量
double v_distance = abs(sqrt(pow(car.x - lastPoint.x, 2) + pow(car.y - lastPoint.y, 2)));
ROS_INFO("the distance is %f", v_distance);
ROS_INFO("%s",action.c_str());//机器人动作
vector<mpc_track::control> control_reference = get_control_reference(target_index, v_distance);
//调用优化服务计算控制量
mpc_optimize(state_reference, control_reference);
if(slow_judge(v_distance)==0){control.v = 0;control.kesi = 0;}//判断，期望速度为0，则机器人停下
ROS_INFO("the speed is: %f,the kesi is: %f", control.v, control.kesi);
ROS_INFO("the angular speed is: %f", control.v*tan(control.kesi)/L);
//ROS_INFO("the car position is x: %f, y: %f", car.x, car.y);
Prediction_Path_set();//设置Path属性
//话题发布
PUB();
//小车位姿状态更新
car = update_state(control, car);
//控制器关闭判断
shutdown_controller();
ROS_INFO("--------------------------------------");
}
}
//调用优化服务
void mpc_optimize(vector<geometry_msgs::Pose2D> state_reference, vector<mpc_track::control> control_reference){
double solve_time;
mpc_track::mpc srv;
srv.request.state_ref = state_reference;
srv.request.control_ref = control_reference;
geometry_msgs::Pose2D position;
position.x = car.x; position.y = car.y; position.theta = car.yaw;
srv.request.state = position;
if(mpc_optimization.call(srv)){
control.v = srv.response.opt_control.v;
control.kesi = srv.response.opt_control.kesi;
state_prediction = srv.response.state_pre;
solve_time = srv.response.solve_time;
ROS_INFO("optimization solve time is %fs", solve_time);
}
else{
ROS_ERROR("can't connect to optimization!");
control.v = control.kesi = 0;
state_prediction.clear();
solve_time = 0;
}
}
//控制启停函数
void node_control() {
ros::Rate loop_rate(freq);
Marker_set();//设置Marker属性
//设置tf坐标转换
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();
MPC_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;//横摆角速度为w = v*tan(kesi)/L
visual_state_trajectory.points.push_back(visual_state_pose);//visualization_msgs::Marker为一个容器，所以现需要向里面push_back结构体，再发布
visual_state_pub.publish(visual_state_trajectory);//发布虚拟轨迹
predictive_state_pub.publish(predictive_state_path);//发布预测轨迹
vel_pub.publish(vel_msg);//发布速度
actual_state_pub.publish(actual_pose);//发布位姿
}
void shutdown_controller(){
if(action == "the car has reached the goal!"){
temp+=1;
if(temp >= 20){
ROS_WARN("shutdown the MPC controller!");
temp = 0;
ros::shutdown();
}
}
}
void Marker_set(){
//设置消息类型参数
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 = "MPC";
//设置点的属性
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;
}
void Prediction_Path_set(){
predictive_state_path.poses.clear();
predictive_state_path.header.seq = 0;
predictive_state_path.header.stamp = ros::Time::now();
predictive_state_path.header.frame_id = "map";
for(int i = 0;i<state_prediction.size();i++){
geometry_msgs::PoseStamped predictive_state_pose;
predictive_state_pose.header.seq = i + 1;
predictive_state_pose.header.stamp = ros::Time::now();
predictive_state_pose.header.frame_id = "map";
predictive_state_pose.pose.position.x = state_prediction[i].x;
predictive_state_pose.pose.position.y = state_prediction[i].y;
predictive_state_pose.pose.orientation = tf::createQuaternionMsgFromYaw(state_prediction[i].theta);
predictive_state_path.poses.push_back(predictive_state_pose);
}
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "MPC_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>("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<int>("prediction_horizon",prediction_horizon,13);
MPC_node* node = new MPC_node(n);
node->node_control();
return (0);
}
七、编译全部的文件
在target_link_libraries里面加上之前生成的库的名字MPC_LIBRIARIES:
###########
## 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})
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/mpc_track_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
add_executable(MPC_node src/MPC_node.cpp)
add_dependencies(MPC_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(MPC_node ${catkin_LIBRARIES} MPC_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} MPC_LIBRARIES)
#############
## Install ##
#############八、配置rviz文件
rviz中需要显示tf坐标变换、maker(用于显示小车实际轨迹)、path(用于显示参考路径)、path(用与现实预测轨迹)等。
注意话题接口，可以结合程序去看一下订阅了哪些话题。
九、配置launch文件
1.路径生成(path_planning.launch)
可以修改trajectory_type更改参考路径形状，默认的有line/wave1/wave2。
<launch>
<node pkg="mpc_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="wave2"/><!--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 mpc_track)/rviz/path_show.rviz">
</node-->
</launch>2.轨迹跟踪(MPC_track.launch)
launch中分为MPC控制器(客户端)和MPC解算优化器(服务端)的启动，以及rviz的启动，并配置了相关参数，各参数都有解释
<launch>
<arg name="L" value="0.2"/><!--Vehicle wheelbase-->
<arg name="freq" value="20"/><!--control freq-->
<arg name="prediction_horizon" value="25"/>
<!--controller node-->
<node pkg="mpc_track" type="MPC_node" name="MPC_node" output="screen">
<!--Basic vehicle information--><!--kesi is the Vehicle front wheel deflection angle-->
<param name="L" value="$(arg L)"/><!--Vehicle wheelbase-->
<param name="V_DESIRED" value="0.5"/><!--desired speed-->
<!--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_v" value="0.0"/>
<param name="initial_kesi" value="0.0"/>
<!--controller information-->
<param name="freq" value="$(arg freq)"/><!--control freq-->
<param name="slow_LEVE1_DISTANCE" value="1.5"/><!--First stage deceleration distance-->
<param name="slow_LEVE2_DISTANCE" value="0.75"/><!--Secondary deceleration distance-->
<param name="goal_tolerance_DISTANCE" value="0.05"/><!--Tracking stop distance-->
<param name="slow_LEVE1_V" value="0.3"/>
<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-->
<param name="prediction_horizon" value="$(arg prediction_horizon)"/><!--prediction horizon-->
</node>
<!--optimizer server-->
<node pkg="mpc_track" type="mpc_optimizer.py" name="MPC_optimizer" output="screen">
<!--Basic vehicle information-->
<param name="L" value="$(arg L)"/>
<!--controller information-->
<param name="freq" value="$(arg freq)"/><!--control freq-->
<param name="prediction_horizon" value="$(arg prediction_horizon)"/><!--prediction horizon-->
<param name="MAX_VEL_delta" value="0.2"/><!--max speed delta-->
<param name="MAX_ANGULAR_delta" value="1.5"/><!--max angular velocity-->
<param name="MAX_DSTEER" value="45"/><!--max front wheel deflection angle delta, Angle system-->
<rosparam param="Q_set">[100,100,10]</rosparam><!--State weight matrix Q = diag{Q1,Q2,Q3}-->
<rosparam param="R_set">[0.1,0.1]</rosparam><!--Control input weight matrix R = diag{R1,R2}-->
</node>
<!--rviz-->
<node name="mpc_track_rviz" pkg="rviz" type="rviz" required="true" args="-d $(find mpc_track)/rviz/track.rviz"/>
</launch>仿真结果
本项目一共进行了三次仿真：
rviz中绿色为参考路径，紫色(图中为蓝色)为预测轨迹，红色为实际轨迹。
一、直线仿真 trajectory_type = line
mpc_line
二、波浪线1仿真 trajectory_type = wave1
mpc_wave1
三、波浪线2仿真 trajectory_type = wave2
mpc_wave2
实验部分
本项目没有进行实验，因为实验平台上的库怎么装都装不上去，但是已经写好了相关的内容，主要是负责监听map->base_link(base_footprint)的节点odom_listen_from_tf.cpp，负责实验的MPC控制器节点MPC_node_world.cpp。
实验步骤：
1.打开机器人底盘、雷达，保证能接收到雷达数据
2.开启移动机器人定位功能，保证有map->base_link(或base_foorprint)的坐标转换。
4.进行移动机器人定位工作，并使其位于路径起始点一米以内
4.roslaunch mpc_track MPC_track_world.launch
5.roslaunch mpc_track path_planning.launch
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;
}MPC_node_world.cpp
#include <ros/ros.h>
#include <iostream>
#include <vector>
#include <mpc_track/trajectory.h>
#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 <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <mpc_track/mpc.h>
#include <mpc_track/control.h>
using namespace std;
double freq,L,V_DESIRED;//采样频率，车辆轴距，期望速度
int prediction_horizon;//预测时域
bool limit_v_and_kesi;//是否限幅(对于阿卡曼转向车辆需要限幅，全向车倒还好)
double slow_LEVE1_DISTANCE,slow_LEVE2_DISTANCE,slow_LEVE1_V,slow_LEVE2_V,goal_tolerance_DISTANCE;//定义二级减速距离和速度
#define pi acos(-1)
#define T 1/freq //采样时间
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;
return p;
}
vector<waypoint> Get_waypoints(){
return path;
}
// 搜索路径点, 将小车到起始点的距离与小车到每一个点的距离对比，找出最近的目标点索引值
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;
}
vector<waypoint> get_local_path(int index){
vector<waypoint> local_path;
int num = Size();
//ROS_INFO("%d", num);
for (int i = 0; i<prediction_horizon; i++){
if(index + i<num){
local_path.push_back(Get_waypoint(index + i));
}
else{
local_path.push_back(Get_waypoint(num-1));
}
/*ROS_INFO("local_path hs %d numbers\n"
"the ID is %d\nthe x is %f\n"
"the y is %f\nthe yaw is %f\n",
local_path.size(),local_path[i].ID,local_path[i].x,local_path[i].y,local_path[i].yaw);*/
}
return local_path;
}
};
class MPC_node {
private:
//car
vehicleState car;//小车状态
U control;//小车控制量[v,kesi]
int lastIndex;//最后一个点索引值
waypoint lastPoint;//最后一个点信息
string action;//小车目前动作：跟踪或跟踪完成(tracking or reach goal!)
//ROS
ros::Subscriber path_sub;//订阅路径，消息类型为nav_msgs::Path
ros::Subscriber odom_sub;//订阅小车当前位姿信息，消息类型为nav_msgs::Odometry
ros::Publisher vel_pub;//发布速度信息，消息类型为geometry_msgs::Twist
ros::Publisher visual_state_pub;//向rviz发布小车虚拟轨迹，消息类型为visualization_msgs::Marker
ros::Publisher predictive_state_pub;//向rviz发布小车预测轨迹，消息类型为visualization_msgs::Marker
ros::ServiceClient mpc_optimization; //建立优化客户端
geometry_msgs::Point visual_state_pose;
visualization_msgs::Marker visual_state_trajectory;
nav_msgs::Path predictive_state_path;
geometry_msgs::Pose2D actual_pose;
geometry_msgs::Twist vel_msg;
vector<geometry_msgs::Pose2D> state_prediction;
int temp;//计数，达到终点时，用于判断控制器是否需要关闭
public:
Path* path;
MPC_node(ros::NodeHandle& nh)//初始化中添加轨迹、小车初始位姿
{
path = new Path();
//ROS:
path_sub = nh.subscribe("path",10,&MPC_node::addpointcallback,this);
vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel",10);
visual_state_pub = nh.advertise<visualization_msgs::Marker>("visualization_pose",10);
predictive_state_pub = nh.advertise<nav_msgs::Path>("predictive_pose",10);
mpc_optimization = nh.serviceClient<mpc_track::mpc>("mpc_optimization");
odom_sub = nh.subscribe<nav_msgs::Odometry>("odom_tf", 1, &MPC_node::odomcallback, this);
action = "the car is tracking!!";
}
~MPC_node() {
delete(path);
}
void odomcallback(const nav_msgs::Odometry::ConstPtr& odom_value)
{
geometry_msgs::Pose2D pose2d_robot;
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;
//获取角度
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);//路径点vector数组传到path类中
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;
}
}
//参考状态获取函数
vector<geometry_msgs::Pose2D> get_state_reference(int index){
vector<geometry_msgs::Pose2D> state_reference;
vector<waypoint> local_path = path->get_local_path(index);
for (int i = 0; i<local_path.size();i++){
geometry_msgs::Pose2D point;
point.x = local_path[i].x;
point.y = local_path[i].y;
point.theta = local_path[i].yaw;
state_reference.push_back(point);
}
//ROS_INFO("state_reference length is %d",state_reference.size());
return state_reference;
}
//参考控制量获取函数
vector<mpc_track::control> get_control_reference(int index, double distance){
vector<mpc_track::control> control_reference;
vector<waypoint> local_path = path->get_local_path(index);
double v_judge = slow_judge(distance);
for (int i = 0; i<local_path.size();i++){
mpc_track::control uu;
uu.v = v_judge;
//接着计算对应的前轮转角参考量
double K = cal_K(path->Get_waypoints(),local_path[i].ID);//计算曲率
uu.kesi = atan2(L * K, 1);
control_reference.push_back(uu);
//ROS_INFO("the v_desired is %f\nthe kesi_desired is %f\n",uu.v, uu.kesi);
}
//ROS_INFO("control_reference length is %d",control_reference.size());
return control_reference;
}
//控制器流程
/*
跟踪功能实现步骤：
1.获取小车当前位姿和路径点，在while(ros::ok())函数下面ros::spinOnce()阶段订阅回调函数odomcallback和addpointcallback时完成
2.搜索路径点
3.获取局部路径state_reference信息，构造对应的期望控制量序列control_reference
在构造control_reference中有减速判断，确定了期望速度和机器人当前动作
4.将当前位姿、局部路径、对应期望控制量序列传入优化器中，采用server/client的形式调用优化服务(mpc_optimizer.py)
5.计算速度和前轮转角
6.发布话题，其中有角速度的计算
7.用运动学模型计算小车下一时刻位姿
8.判断是否需要关闭控制器，如果到达终点就关闭
9.loop_rate.sleep()，结束一次循环，准备开始下一次个跟踪工作
*/
void MPC_track() {
//ROS_INFO("path size is: %d",path->Size());
if(path->Size()!=0){
//搜索路径点
int target_index = path->Find_target_index(car);
//ROS_INFO("target index is : %d", target_index);
//获取路径点信息，构造期望控制量(分为参考状态和参考控制量)
//参考状态
vector<geometry_msgs::Pose2D> state_reference = get_state_reference(target_index);
//参考控制量
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);
ROS_INFO("%s",action.c_str());//机器人动作
vector<mpc_track::control> control_reference = get_control_reference(target_index, v_distance);
//调用优化服务计算控制量
mpc_optimize(state_reference, control_reference);
if(slow_judge(v_distance)==0){control.v = 0;control.kesi = 0;}//判断，期望速度为0，则机器人停下
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);
Prediction_Path_set();//设置Path属性
//话题发布
PUB();
//控制器关闭判断
shutdown_controller();
ROS_INFO("--------------------------------------");
}
}
//调用优化服务
void mpc_optimize(vector<geometry_msgs::Pose2D> state_reference, vector<mpc_track::control> control_reference){
double solve_time;
mpc_track::mpc srv;
srv.request.state_ref = state_reference;
srv.request.control_ref = control_reference;
geometry_msgs::Pose2D position;
position.x = car.x; position.y = car.y; position.theta = car.yaw;
srv.request.state = position;
if(mpc_optimization.call(srv)){
control.v = srv.response.opt_control.v;
control.kesi = srv.response.opt_control.kesi;
state_prediction = srv.response.state_pre;
solve_time = srv.response.solve_time;
ROS_INFO("optimization solve time is %fs", solve_time);
}
else{
ROS_ERROR("can't connect to optimization!");
control.v = control.kesi = 0;
state_prediction.clear();
solve_time = 0;
}
}
//控制启停函数
void node_control() {
ros::Rate loop_rate(freq);
Marker_set();//设置Marker属性
while (ros::ok()) {
ros::spinOnce();
MPC_track();
loop_rate.sleep();
}
}
void PUB(){
vel_msg.linear.x = control.v; vel_msg.angular.z = control.v*tan(control.kesi)/L;//横摆角速度为w = v*tan(kesi)/L
visual_state_pose.x = car.x; visual_state_pose.y = car.y;
visual_state_trajectory.points.push_back(visual_state_pose);//visualization_msgs::Marker为一个容器，所以现需要向里面push_back结构体，再发布
visual_state_pub.publish(visual_state_trajectory);//发布虚拟轨迹
predictive_state_pub.publish(predictive_state_path);//发布预测轨迹
vel_pub.publish(vel_msg);//发布速度
}
void shutdown_controller(){
if(action == "the car has reached the goal!"){
temp+=1;
if(temp >= 20){
ROS_WARN("shutdown the MPC controller!");
temp = 0;
ros::shutdown();
}
}
}
void Marker_set(){
//设置消息类型参数
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 = "MPC";
//设置点的属性
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;
}
void Prediction_Path_set(){
predictive_state_path.poses.clear();
predictive_state_path.header.seq = 0;
predictive_state_path.header.stamp = ros::Time::now();
predictive_state_path.header.frame_id = "map";
for(int i = 0;i<state_prediction.size();i++){
geometry_msgs::PoseStamped predictive_state_pose;
predictive_state_pose.header.seq = i + 1;
predictive_state_pose.header.stamp = ros::Time::now();
predictive_state_pose.header.frame_id = "map";
predictive_state_pose.pose.position.x = state_prediction[i].x;
predictive_state_pose.pose.position.y = state_prediction[i].y;
predictive_state_pose.pose.orientation = tf::createQuaternionMsgFromYaw(state_prediction[i].theta);
predictive_state_path.poses.push_back(predictive_state_pose);
}
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "MPC_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>("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<int>("prediction_horizon",prediction_horizon,13);
MPC_node* node = new MPC_node(n);
node->node_control();
return (0);
}
MPC_track_world.launch
<launch>
<!--node pkg="tf" type="static_transform_publisher" name="map_to_base_link" args="-0.127 -0.147 0 0 0 0.0138 map base_link 100" />
<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 1 map odom 100" /-->
<arg name="L" value="0.2"/>
<arg name="freq" value="20"/>
<arg name="prediction_horizon" value="25"/>
<!--controller node-->
<node pkg="mpc_track" type="MPC_node_world" name="MPC_node_world" output="screen">
<!--Basic vehicle information--><!--kesi is the Vehicle front wheel deflection angle-->
<param name="L" value="$(arg L)"/><!--Vehicle wheelbase-->
<param name="V_DESIRED" value="0.5"/>
<!--controller information-->
<param name="freq" value="$(arg freq)"/><!--control freq-->
<param name="slow_LEVE1_DISTANCE" value="1.5"/><!--First stage deceleration distance-->
<param name="slow_LEVE2_DISTANCE" value="0.75"/><!--Secondary deceleration distance-->
<param name="goal_tolerance_DISTANCE" value="0.1"/><!--Tracking stop distance-->
<param name="slow_LEVE1_V" value="0.3"/>
<param name="slow_LEVE2_V" value="0.15"/>
<param name="limit_v_and_kesi" value="false"/><!--If it is an akaman steering car, it must be limited. If it is an omni-directional car, it is optional-->
<param name="prediction_horizon" value="$(arg prediction_horizon)"/><!--prediction horizon-->
</node>
<!--optimizer server-->
<node pkg="mpc_track" type="mpc_optimizer.py" name="MPC_optimizer">
<!--Basic vehicle information-->
<param name="L" value="$(arg L)"/>
<!--controller information-->
<param name="freq" value="$(arg freq)"/><!--control freq-->
<param name="prediction_horizon" value="$(arg prediction_horizon)"/><!--prediction horizon-->
<param name="MAX_VEL_delta" value="0.2"/><!--max speed delta-->
<param name="MAX_ANGULAR_delta" value="1.5"/><!--max angular velocity-->
<param name="MAX_DSTEER" value="45"/><!--max front wheel deflection angle delta, Angle system-->
<rosparam param="Q_set">[100,100,10]</rosparam><!--State weight matrix Q = diag{Q1,Q2,Q3}-->
<rosparam param="R_set">[0.1,0.1]</rosparam><!--Control input weight matrix R = diag{R1,R2}-->
</node>
<!--listen the tf transform map->base_link(or base_foorprint)-->
<node pkg="mpc_track" type="mpc_odom_listen" name="mpc_odom_listen" output="screen">
<param name="base_frame" value="base_link"/>
</node>
<!--rviz-->
<node name="mpc_track_rviz" pkg="rviz" type="rviz" required="true" args="-d $(find mpc_track)/rviz/track_world.rviz"/>
</launch>补充说明
一、计算时间
MPC算法的预测时域越长，精度越高，但带来的是较高的计算负载，所以在采用python3的cvxpy库进行优化时，速度话题的发布频率只有3Hz。读者可以在该项目的框架基础上，自己写优化程序，可以用C++的osqp库和casadi库。
二、跟踪圆形轨迹
跟踪圆形轨迹的整体步骤其实跟上述差不多，自己写一个圆形路径或者半圆形路径，但是在跟踪过程中会产生角度方面的问题：
移动机器人的角度取值范围是：顺时针-0°~-180°，逆时针0°~180°，那么在比如顺时针跟踪时可能会产生由-180°到180°的航向角控制问题，这需要读者自己去研究一下控制器如何处理这个问题。
完整程序
完整程序请见：Bitbucket
git clone到工作空间下，按照功能包中的README.md操作就好了
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
