参考博客：
Pure Pursuit 纯追踪法
Autoware(Pure pursuit代码学习)

1 Pure Pursuit纯追踪法

适用场景：低速场景（速度过高会产生转弯内切以及超调）
简化前轮转向角和后轴将遵循的曲率之间的关系
(Gx,Gy)是下一个需要追踪的路点，位于已经规划好的全局路径上，现在需要控制车辆的后轴经过该路点。

• $L_d$: 车辆后轴中心(Cx,Cy)到目标路点(Gx,Gy)的距离，即预瞄距离
• $\alpha$: 目前车身姿态和目标路点的夹角
• $R$: 跟踪的曲率半径
• $\delta$: 前轮转角

$$\frac{L_d}{sin2\alpha }=\frac{R}{sin(\frac{\pi }{2}-\alpha )}$$
=>$$kappa=\frac{1}{R}=\frac{2sin\alpha }{Ld}$$
=>$$\delta (t)=arctan(\frac{2Lsin\alpha (t)}{Ld})$$

调节预瞄距离成为纯追踪算法的关键。

2 代码分析

pure_pursuit.h

#pragma once
#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>

using namespace std;
class PurePursuit {
public:
    double calTargetIndex(vector<double>robot_state, vector<vector<double>> ref_path, double l_d);

    double Pure_Pursuit_Control(vector<double> robot_state, vector<double> current_ref_point, double l_d, double psi, double L);
};

pure_pursuit.cpp

#include "Pure_pursuit.h"

// 计算邻近路点 (G_x, G_y)
// robot_state:当前机器人位置 ref_path:参考轨迹 l_d:前向距离
double PurePursuit::calTargetIndex(vector<double> robot_state, vector<vector<double>> ref_path, double l_d)
{
    vector<double> dists;
    for (vector<double> xy : ref_path)
    {
        double dist = sqrt(pow(xy[0] - robot_state[0], 2) + pow(xy[1] - robot_state[1], 2));
        dists.push_back(dist);
    }
    double min_index = min_element(dists.begin(), dists.end()) - dists.begin();
    double delta_l = sqrt(pow(ref_path[min_index][0] - robot_state[0], 2) + pow(ref_path[min_index][1] - robot_state[1], 2));

    while (l_d > delta_l && min_index < ref_path.size() - 1)
    {
        delta_l = sqrt(pow(ref_path[min_index + 1][0] - robot_state[0], 2) + pow(ref_path[min_index + 1][1] - robot_state[1], 2));
        min_index += 1;
    }
    return min_index;
}

// Pure Pursuit Control
// robot_state 当前机器人位置; current_ref_point 参考轨迹点 ; l_d 前向距离 ; psi 机器人航向角 ; L 轴距 ; return 转角控制量
double PurePursuit::Pure_Pursuit_Control(vector<double> robot_state, vector<double> current_ref_point, double l_d, double psi, double L)
{
    double alpha = atan2(current_ref_point[1] - robot_state[1], current_ref_point[0] - robot_state[0]) - psi;
    double delta = atan2(2 * L * sin(alpha), l_d);
    return delta;
}

main.cpp

#include "KinematicModel.h"
#include "matplotlibcpp.h"
#include "Pure_pursuit.h"
namespace plt = matplotlibcpp;

#define PI 3.1415926

int main()
{
    double x0 = 0.0, y0 = 2.3, psi = 0.5, v=2, L=2, dt=0.1;
    double lam = 0.01; // 前视距离
    double c = 2;
    vector<vector<double>> ref_path(1000, vector<double>(2));
    vector<double> ref_x, ref_y; // 保存参考数据用于画图
    // 生成参考轨迹
    for (int i = 0; i < 1000; i++) {
        ref_path[i][0] = 0.1 * i;
        ref_path[i][1] = 2 * sin(ref_path[i][0] / 3.0) + 2.5 * cos(ref_path[i][0] / 2.0);
        ref_x.push_back(ref_path[i][0]);
        ref_y.push_back(ref_path[i][1]);
    }
    KinematicModel model(x0, y0, psi, v, L, dt);

    vector<double> x_, y_;
    vector<double> robot_state(2);
    PurePursuit pp;
    for (int i = 0; i < 600; i++) {
        plt::clf();
        robot_state[0] = model.x;
        robot_state[1] = model.y;
        double l_d = lam * model.v + c;
        double min_ind = pp.calTargetIndex(robot_state, ref_path, l_d);
        double delta = pp.Pure_Pursuit_Control(robot_state, ref_path[min_ind], l_d, model.psi, L);
        model.updateState(0, delta);
        x_.push_back(model.x);
        y_.push_back(model.y);

        plt::plot(ref_x, ref_y, "b--");
        plt::plot(x_, y_, "r--");
        plt::grid(true);
        plt::ylim(-5, 5);
        plt::pause(0.01);
    }
    const char* filename = "./pure_pursuit.png";
    plt::save(filename);
    plt::show();
    return 0;
}

CMakeList.txt

cmake_minimum_required(VERSION 3.0.2)
project(Pure_pursuit)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
)

set(CMAKE_CXX_STANDARD 11)

file(GLOB_RECURSE PYTHON2.7_LIB "/usr/lib/python2.7/config-x86_64-linux-gnu/*.so")
set(PYTHON2.7_INLCUDE_DIRS "/usr/include/python2.7")

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES huatu
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
)

include_directories(include
        ${PYTHON2.7_INLCUDE_DIRS}
)

add_executable(pp_controller src/Pure_pursuit.cpp
                              src/KinematicModel.cpp
                              src/main.cpp)
target_link_libraries(pp_controller ${PYTHON2.7_LIB})