在哪里自己建设网站,跨网浏览器,网络总体方案设计,.net网站开发的例子Pure-Pursuit 跟踪双移线 Gazebo 仿真
主要参考学习下面的博客和开源项目 自动驾驶规划控制#xff08;#xff21;*、pure pursuit、LQR算法#xff0c;使用c在ubuntu和ros环境下实现#xff09; https://github.com/NeXTzhao/planning Pure-Pursuit 的理论基础见今年六月…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.advertisenav_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::arrayfloat, 3 calQuaternionToEuler(const float x, const float y,const float z, const float w)
{std::arrayfloat, 3 calRPY {(0, 0, 0)};// roll atan2(2(wxyz),1-2(x*xy*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(wxyz),1-2(y*yz*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::arrayfloat, 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.advertisegeometry_msgs::Twist(/smart/cmd_vel, 20);path_pub_ n.advertisenav_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 doesnt 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;
shape2.4;%参数名称用于参考轨迹生成
dx125;dx221.95;%没有任何实际意义只是参数名称
dy14.05;dy25.7;%没有任何实际意义只是参数名称
Xs127.19;Xs256.46;%参数名称
for i 1:length(cx) %全局路径c(y)生成 路径初始化z1shape/dx1*(cx(i)-Xs1)-shape/2;z2shape/dx2*(cx(i)-Xs2)-shape/2;cy(i) dy1/2*(1tanh(z1))-dy2/2*(1tanh(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; %该处定义向左转为alphabeta-Fai所以向右转就输出-alphadelta atan(2*L * sin(alpha)/Ld); %前轮转角U delta;sys U; % vel, steering, x, ytoc
% End of mdlOutputs.注意处理接近终点的情况不加限制的话容易出现绕着终点转圈的现象 限制后整体的跟踪效果尚可但在终点处仍旧会出现异常的偏航仍有较大的优化空间
