Pure Pursuit控制器路径跟随-编程知识

Pure Pursuit控制器路径跟随

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

1 Pure Pursuit纯追踪法

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

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

$\Large\frac{L_{d}}{sin2\alpha } =\frac{R}{sin(\frac{\pi }{2} -\alpha ){\large }}$
=> $\large kappa =\frac{1}{R} =\frac{2sin\alpha}{Ld }$
=> ${\Large {\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.1415926int 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 COMPONENTSroscppstd_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.cppsrc/KinematicModel.cppsrc/main.cpp)
target_link_libraries(pp_controller ${PYTHON2.7_LIB})