Pure-Pursuit 跟踪双移线 Gazebo 仿真

主要参考学习下面的博客和开源项目

自动驾驶规划控制（Ａ*、pure pursuit、LQR算法，使用c++在ubuntu和ros环境下实现）

https://github.com/NeXTzhao/planning

Pure-Pursuit 的理论基础见今年六月份的笔记

对参考轨迹进行调整，采用双移线轨迹

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <iostream>
#include <string>
#include <vector>
#include <math.h>
#include <fstream>
using namespace std;// 双移线构造的参数
const float shape = 2.4;
const float dx1 = 25.0, dx2 = 21.95;
const float dy1 = 4.05, dy2 = 5.7;
const float Xs1 = 27.19, Xs2 = 56.46;
// 参考路径在 X 方向长度以及参考点的步长
const float length = 120.0;
const float step = 0.1;// 计算 Y 轴参考位置
inline float calculate_reference_y(const float ref_x)
{float z1 = shape / dx1 * (ref_x - Xs1) - shape / 2.0;float z2 = shape / dx2 * (ref_x - Xs2) - shape / 2.0;return dy1 / 2.0 * (1 + tanh(z1)) - dy2 / 2.0 * (1 + tanh(z2));
}int main(int argc, char *argv[])
{ros::init(argc, argv, "DoubleLane");ros::NodeHandle nh;ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("/double_lane", 1000, true);nav_msgs::Path reference_path;reference_path.header.frame_id = "world";reference_path.header.stamp = ros::Time::now();geometry_msgs::PoseStamped pose;pose.header.stamp = ros::Time::now();pose.header.frame_id = "world";int points_size = length / step;reference_path.poses.resize(points_size + 1);for (int i = 0; i <= points_size; ++i){float ref_x = i * step;float ref_y = calculate_reference_y(ref_x);// cout << ref_x << "\t" << ref_y << endl;pose.pose.position.x = ref_x;pose.pose.position.y = ref_y;pose.pose.position.z = 0.0;pose.pose.orientation.x = 0.0;pose.pose.orientation.y = 0.0;pose.pose.orientation.z = 0.0;pose.pose.orientation.w = 0.0;reference_path.poses[i] = pose;}ros::Rate loop(10);while (ros::ok()){path_pub.publish(reference_path);ros::spinOnce();loop.sleep();}return 0;
}

编程方面进行了一些简单的优化，轨迹跟踪的算法在 poseCallback 中实现，和博主有所区别

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>#include <algorithm>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>#include "cpprobotics_types.h"
#include "cubic_spline.h"
#include "geometry_msgs/PoseStamped.h"#define PREVIEW_DIS 1.5 // 预瞄距离#define Ld 1.868 // 轴距using namespace std;
using namespace cpprobotics;ros::Publisher purepersuit_;
ros::Publisher path_pub_;
nav_msgs::Path path;float carVelocity = 0;
float preview_dis = 0;
float k = 0.1;// 计算四元数转换到欧拉角
std::array<float, 3> calQuaternionToEuler(const float x, const float y,const float z, const float w)
{std::array<float, 3> calRPY = {(0, 0, 0)};// roll = atan2(2(wx+yz),1-2(x*x+y*y))calRPY[0] = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));// pitch = arcsin(2(wy-zx))calRPY[1] = asin(2 * (w * y - z * x));// yaw = atan2(2(wx+yz),1-2(y*y+z*z))calRPY[2] = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));return calRPY;
}cpprobotics::Vec_f r_x_;
cpprobotics::Vec_f r_y_;int pointNum = 0; // 保存路径点的个数
int targetIndex = pointNum - 1;// 计算发送给模型车的转角
void poseCallback(const geometry_msgs::PoseStamped &currentWaypoint)
{auto currentPositionX = currentWaypoint.pose.position.x;auto currentPositionY = currentWaypoint.pose.position.y;auto currentPositionZ = 0.0;auto currentQuaternionX = currentWaypoint.pose.orientation.x;auto currentQuaternionY = currentWaypoint.pose.orientation.y;auto currentQuaternionZ = currentWaypoint.pose.orientation.z;auto currentQuaternionW = currentWaypoint.pose.orientation.w;std::array<float, 3> calRPY =calQuaternionToEuler(currentQuaternionX, currentQuaternionY,currentQuaternionZ, currentQuaternionW);cout << currentPositionX << "\t" << currentPositionY << "\t" << calRPY[2] << endl;for (int i = pointNum - 1; i >= 0; --i){float distance = sqrt(pow((r_x_[i] - currentPositionX), 2) +pow((r_y_[i] - currentPositionY), 2));if (distance < preview_dis){targetIndex = i + 1;break;}}if (targetIndex >= pointNum){targetIndex = pointNum - 1;}float alpha =atan2(r_y_[targetIndex] - currentPositionY, r_x_[targetIndex] - currentPositionX) -calRPY[2];// 当前点和目标点的距离Idfloat dl = sqrt(pow(r_y_[targetIndex] - currentPositionY, 2) +pow(r_x_[targetIndex] - currentPositionX, 2));// 发布小车运动指令及运动轨迹if (targetIndex == pointNum - 1 && dl < 0.2) // 离终点很近时停止运动{geometry_msgs::Twist vel_msg;vel_msg.linear.x = 0;vel_msg.angular.z = 0;purepersuit_.publish(vel_msg);}else{float theta = atan(2 * Ld * sin(alpha) / dl);geometry_msgs::Twist vel_msg;vel_msg.linear.x = 3;vel_msg.angular.z = theta;purepersuit_.publish(vel_msg);// 发布小车运动轨迹geometry_msgs::PoseStamped this_pose_stamped;this_pose_stamped.pose.position.x = currentPositionX;this_pose_stamped.pose.position.y = currentPositionY;geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(theta);this_pose_stamped.pose.orientation.x = currentQuaternionX;this_pose_stamped.pose.orientation.y = currentQuaternionY;this_pose_stamped.pose.orientation.z = currentQuaternionZ;this_pose_stamped.pose.orientation.w = currentQuaternionW;this_pose_stamped.header.stamp = ros::Time::now();this_pose_stamped.header.frame_id = "world";path.poses.push_back(this_pose_stamped);}path_pub_.publish(path);
}void velocityCall(const geometry_msgs::TwistStamped &carWaypoint)
{carVelocity = carWaypoint.twist.linear.x;// 预瞄距离计算preview_dis = k * carVelocity + PREVIEW_DIS;
}void pointCallback(const nav_msgs::Path &msg)
{// 避免参考点重复赋值if (pointNum != 0){return;}// geometry_msgs/PoseStamped[] posespointNum = msg.poses.size();// 提前开辟内存r_x_.resize(pointNum);r_y_.resize(pointNum);for (int i = 0; i < pointNum; i++){r_x_[i] = msg.poses[i].pose.position.x;r_y_[i] = msg.poses[i].pose.position.y;}
}
int main(int argc, char **argv)
{// 创建节点ros::init(argc, argv, "pure_pursuit");// 创建节点句柄ros::NodeHandle n;// 创建Publisher，发送经过pure_pursuit计算后的转角及速度purepersuit_ = n.advertise<geometry_msgs::Twist>("/smart/cmd_vel", 20);path_pub_ = n.advertise<nav_msgs::Path>("/rvizpath", 100, true);// ros::Rate loop_rate(10);path.header.frame_id = "world";// 设置时间戳path.header.stamp = ros::Time::now();geometry_msgs::PoseStamped pose;pose.header.stamp = ros::Time::now();// 设置参考系pose.header.frame_id = "world";ros::Subscriber splinePath = n.subscribe("/double_lane", 20, pointCallback);ros::Subscriber carVel = n.subscribe("/smart/velocity", 20, velocityCall);ros::Subscriber carPose = n.subscribe("/smart/rear_pose", 20, poseCallback);ros::spin();return 0;
}

这里和 CarSim-Simulink 联合仿真的代码类似

function [sys,x0,str,ts] = MY_MPCController3(t,x,u,flag)
%   该函数是写的第3个S函数控制器(MATLAB版本：R2011a)
%   限定于车辆运动学模型，控制量为速度和前轮偏角，使用的QP为新版本的QP解法
%   [sys,x0,str,ts] = MY_MPCController3(t,x,u,flag)
%
% is an S-function implementing the MPC controller intended for use
% with Simulink. The argument md, which is the only user supplied
% argument, contains the data structures needed by the controller. The
% input to the S-function block is a vector signal consisting of the
% measured outputs and the reference values for the controlled
% outputs. The output of the S-function block is a vector signal
% consisting of the control variables and the estimated state vector,
% potentially including estimated disturbance states.switch flag,case 0[sys,x0,str,ts] = mdlInitializeSizes; % Initializationcase 2sys = mdlUpdates(t,x,u); % Update discrete statescase 3sys = mdlOutputs(t,x,u); % Calculate outputscase {1,4,9} % Unused flagssys = [];otherwiseerror(['unhandled flag = ',num2str(flag)]); % Error handling
end
% End of dsfunc.%==============================================================
% Initialization
%==============================================================function [sys,x0,str,ts] = mdlInitializeSizes
% Call simsizes for a sizes structure, fill it in, and convert it 
% to a sizes array.
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 4; % this parameter doesn't matter
sizes.NumOutputs     = 1;
sizes.NumInputs      = 5;
sizes.DirFeedthrough = 1; % Matrix D is non-empty.
sizes.NumSampleTimes = 1;
sys = simsizes(sizes); 
x0 =[0.00001;0.00001;0.00001;0.00001];   
global U; % store current ctrl vector:[vel_m, delta_m]
U=[0];global cx;
cx = 0:0.01:160;
global cy;
shape=2.4;%参数名称，用于参考轨迹生成
dx1=25;dx2=21.95;%没有任何实际意义，只是参数名称
dy1=4.05;dy2=5.7;%没有任何实际意义，只是参数名称
Xs1=27.19;Xs2=56.46;%参数名称
for i = 1:length(cx)                      %全局路径c(y)生成 路径初始化z1=shape/dx1*(cx(i)-Xs1)-shape/2;z2=shape/dx2*(cx(i)-Xs2)-shape/2;cy(i) = dy1/2*(1+tanh(z1))-dy2/2*(1+tanh(z2));
end% Initialize the discrete states.
str = [];             % Set str to an empty matrix.
ts  = [0.05 0];       % sample time: [period, offset]
%End of mdlInitializeSizes%==============================================================
% Update the discrete states
%==============================================================
function sys = mdlUpdates(t,x,u)sys = x;
%End of mdlUpdate.%==============================================================
% Calculate outputs
%==============================================================
function sys = mdlOutputs(t,x,u)global U; %store chi_tilde=[vel-vel_ref; delta - delta_ref]global cx;global cy;pi = 3.1415926;ticfprintf('Update start, t=%6.3f\n',t);x = u(1);y = u(2);yaw_angle =u(3)*pi/180;%CarSim输出的Yaw angle为角度，角度转换为弧度v = u(4) / 3.6;k = 0.1;                                  % look forward gain  前向预测距离所用增益Lfc = 3;                                  % 基础预瞄距离L = 2.7;                                % [m] wheel base of vehicleLd = k * v + Lfc;N =  length(cx);ind = N;for i = N : -1 : 1distance = sqrt((cx(i)-x)^2 + (cy(i)-y)^2);if distance < Ldind = i + 1;break;endendif ind > Nind = N; endtx = cx(ind);ty = cy(ind);Ld = sqrt((tx-x)^2 + (ty-y)^2);alpha = atan((ty-y)/(tx-x))-yaw_angle;       %该处定义向左转为alpha=beta-Fai，所以向右转就输出-alphadelta = atan(2*L * sin(alpha)/Ld);                %前轮转角U = delta;sys= U; % vel, steering, x, ytoc
% End of mdlOutputs.