puresuit算法 C++实现(ROS RVIZ 环境下展示)
/*
*初次写代码时间:20211月18号
*编译环境:c++11
*系统环境:ubuntu16.04+ros-kinetic
*代码实现功能:puresuit 算法实现路径跟踪,现在只考虑位置点的变化,方位角和速度还没展示出来。
*/
#include<iostream>
#include<cstring>
#include<stdlib.h>
#include<vector>
#include<math.h>
#include<cmath>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
using namespace std;
double dt=0.1;//时间间隔
double K=0.1;//前视距离系数
double Lfc=2.0;//前视距离
double Kp=1.0;//速度P控制系数
double L=2.9;//车辆轴距,单位:M
//创建类 vechicleState
struct vechicleState
{
double x;
double y;
double yaw;
double v;
};
vechicleState update(vechicleState state,double a,double delta)//定义更新函数
{
state.x+=state.v*cos(state.yaw)*dt;
state.y+=state.v*sin(state.yaw)*dt;
state.yaw+=state.v/L*tan(delta)*dt;
state.v+=a*dt;
return state;
}
//转角控制器函数
double PControl(double target,double current)
{
double a;
a=Kp*(target-current);
return a;
}
int calc_target_index(vechicleState state,vector<double> &cx,vector<double> &cy)//计算最短路径。
{
double min=abs(sqrt(pow(state.x-cx[0],2)+pow(state.y-cy[0],2)));
int index=0;
for(int j=0;j<cx.size();j++)
{
double d=abs(sqrt(pow(state.x-cx[j],2)+pow(state.y-cy[j],2)));
if(d<min)
{
min=d;
index=j;
}
}
double L=0.0;
double Lf=K*state.v+2.0;
while(Lf>L && (index+1)<50)
{
double dx=cx[index+1]-cx[index];
double dy=cy[index+1]-cy[index];
L+=sqrt(pow(dx,2)+pow(dy,2));
index+=1;
}
return index;
}
//用struct Result返回两个参数
struct Result
{
double delta;
int ind;
};
Result pure_pursuit_control(vechicleState state,vector<double> &cx,vector<double> &cy,int pind)//纯路径控制
{
Result ret;
double tx;
double ty;
int ind=calc_target_index(state,cx,cy);//目标点的索引
if(pind>=ind)
{
ind=pind;
}
if(ind<cx.size())
{
tx=cx[ind];
ty=cy[ind];
}
else
{
tx=cx[cx.size()-1];//倒数第一个点
ty=cy[cx.size()-1];
ind=cx.size()-1;//最后一个点的索引
}
double pi=3.14;
double alpha=atan2(ty-state.y,tx-state.x)-state.yaw;
if (state.v<0)
{
alpha=pi-alpha;
}
//cout<<"alpha:"<<alpha<<endl;
double Lf=K*state.v+Lfc;
double delta=atan2(2.0*L*sin(alpha)/Lf,1.0);
//cout<<"delta:"<<delta<<endl;
ret.delta=delta;
ret.ind=ind;
return ret;//返回角度和最近点的索引。
}
int main(int argc,char **argv)
{
ros::init(argc,argv,"puresuit");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
visualization_msgs::Marker points;
points.header.frame_id="odom";
points.header.stamp =ros::Time::now();
ros::Rate loop_rate(10);
points.action =visualization_msgs::Marker::ADD;
points.ns="puresuit";
points.pose.orientation.w=1.0;
//设置点的属性
points.id = 0;
points.type = visualization_msgs::Marker::POINTS;
points.scale.x = 0.1;
points.scale.y = 0.1;
points.color.g = 1.0f;
points.color.a = 1.0;
vector<double> cx={};
vector<double> cy={};
for(int i=0;i<50;i++)
{
cx.push_back(i);
cy.push_back(sin(i/5.0)*i/2.0);
}
double target_speed=10.0/3.6; //速度
double T=100; //模拟最大时间
//初始化state
struct vechicleState state={0.0,-3.0,0.0,0.0};
double lastIndex=cx.size()-1;
double time =0.0;
int target_ind=calc_target_index(state,cx,cy);
Result res;
while(T>=time && lastIndex>target_ind)
{
double ai =PControl(2.777778,state.v);
//目标点的索引和航向角
res=pure_pursuit_control(state,cx,cy,target_ind);
double di=res.delta;
int target_ind=res.ind;
//更新车的位置
state =update(state,ai,di);
cout<<"ai:"<<ai<<" di:"<<di<<" target_ind:"<<target_ind<<" x:"<<state.x<<" v:"<<state.v<<endl;
time=time+dt;
geometry_msgs::Point p;
p.x = state.x;
p.y = state.y;
p.z = 0;
points.points.push_back(p);
pub.publish(points);
loop_rate.sleep();
}
}
本次的实现环境为ROS-kinectic,用Marker几何信息发布。
以下为实现图
问题:算法实现了大半部分,但到最后一个点,不知是用了ROS原因还是最后while 条件没用正确,最后一个点过后,轨迹线又会绕回来。
图中x变为48了,说明已经往回走了。