puresuit演算法 C++實作

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了，說明已經往回走了，