Pure-Pursuit 跟踪双移线 Gazebo 仿真
主要参考学习下面的博客和开源项目
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 ¤tWaypoint)
{
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];
// 当前点和目标点的距离Id
float 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[] poses
pointNum = 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; % Initialization
case 2
sys = mdlUpdates(t,x,u); % Update discrete states
case 3
sys = mdlOutputs(t,x,u); % Calculate outputs
case {1,4,9} % Unused flags
sys = [];
otherwise
error(['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;
tic
fprintf('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 vehicle
Ld = k * v + Lfc;
N = length(cx);
ind = N;
for i = N : -1 : 1
distance = sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
if distance < Ld
ind = i + 1;
break;
end
end
if ind > N
ind = N;
end
tx = cx(ind);
ty = cy(ind);
Ld = sqrt((tx-x)^2 + (ty-y)^2);
alpha = atan((ty-y)/(tx-x))-yaw_angle; %该处定义向左转为alpha=beta-Fai，所以向右转就输出-alpha
delta = atan(2*L * sin(alpha)/Ld); %前轮转角
U = delta;
sys= U; % vel, steering, x, y
toc
% End of mdlOutputs.
注意处理接近终点的情况，不加限制的话容易出现绕着终点转圈的现象
限制后整体的跟踪效果尚可，但在终点处仍旧会出现异常的偏航，仍有较大的优化空间
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
