Pure Pursuit控制器路径跟随

2024-02-15 14:44

本文主要是介绍Pure Pursuit控制器路径跟随,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

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

1 Pure Pursuit纯追踪法

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

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

L d s i n 2 α = R s i n ( π 2 − α ) \Large\frac{L_{d}}{sin2\alpha } =\frac{R}{sin(\frac{\pi }{2} -\alpha ){\large }} sin2αLd=sin(2πα)R
=> k a p p a = 1 R = 2 s i n α L d \large kappa =\frac{1}{R} =\frac{2sin\alpha}{Ld } kappa=R1=Ld2sinα
=> δ ( t ) = a r c t a n ( 2 L s i n α ( t ) L d ) {\Large {\delta (t)=arctan(\frac{2Lsin\alpha (t)}{Ld} )} } δ(t)=arctan(Ld2Lsinα(t))

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

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})

这篇关于Pure Pursuit控制器路径跟随的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/711688

相关文章

Linux修改pip和conda缓存路径的几种方法

《Linux修改pip和conda缓存路径的几种方法》在Python生态中,pip和conda是两种常见的软件包管理工具,它们在安装、更新和卸载软件包时都会使用缓存来提高效率,适当地修改它们的缓存路径... 目录一、pip 和 conda 的缓存机制1. pip 的缓存机制默认缓存路径2. conda 的缓

Windows系统下如何查找JDK的安装路径

《Windows系统下如何查找JDK的安装路径》:本文主要介绍Windows系统下如何查找JDK的安装路径,文中介绍了三种方法,分别是通过命令行检查、使用verbose选项查找jre目录、以及查看... 目录一、确认是否安装了JDK二、查找路径三、另外一种方式如果很久之前安装了JDK,或者在别人的电脑上,想

Python中Windows和macOS文件路径格式不一致的解决方法

《Python中Windows和macOS文件路径格式不一致的解决方法》在Python中,Windows和macOS的文件路径字符串格式不一致主要体现在路径分隔符上,这种差异可能导致跨平台代码在处理文... 目录方法 1:使用 os.path 模块方法 2:使用 pathlib 模块(推荐)方法 3:统一使

一文教你解决Python不支持中文路径的问题

《一文教你解决Python不支持中文路径的问题》Python是一种广泛使用的高级编程语言,然而在处理包含中文字符的文件路径时,Python有时会表现出一些不友好的行为,下面小编就来为大家介绍一下具体的... 目录问题背景解决方案1. 设置正确的文件编码2. 使用pathlib模块3. 转换路径为Unicod

MySQL9.0默认路径安装下重置root密码

《MySQL9.0默认路径安装下重置root密码》本文主要介绍了MySQL9.0默认路径安装下重置root密码,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们... 目录问题描述环境描述解决方法正常模式下修改密码报错原因问题描述mysqlChina编程采用默认安装路径,

python获取当前文件和目录路径的方法详解

《python获取当前文件和目录路径的方法详解》:本文主要介绍Python中获取当前文件路径和目录的方法,包括使用__file__关键字、os.path.abspath、os.path.realp... 目录1、获取当前文件路径2、获取当前文件所在目录3、os.path.abspath和os.path.re

hdu2544(单源最短路径)

模板题: //题意:求1到n的最短路径,模板题#include<iostream>#include<algorithm>#include<cstring>#include<stack>#include<queue>#include<set>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#i

poj 1734 (floyd求最小环并打印路径)

题意: 求图中的一个最小环,并打印路径。 解析: ans 保存最小环长度。 一直wa,最后终于找到原因,inf开太大爆掉了。。。 虽然0x3f3f3f3f用memset好用,但是还是有局限性。 代码: #include <iostream>#include <cstdio>#include <cstdlib>#include <algorithm>#incl

【408DS算法题】039进阶-判断图中路径是否存在

Index 题目分析实现总结 题目 对于给定的图G,设计函数实现判断G中是否含有从start结点到stop结点的路径。 分析实现 对于图的路径的存在性判断,有两种做法:(本文的实现均基于邻接矩阵存储方式的图) 1.图的BFS BFS的思路相对比较直观——从起始结点出发进行层次遍历,遍历过程中遇到结点i就表示存在路径start->i,故只需判断每个结点i是否就是stop

Android Environment 获取的路径问题

1. 以获取 /System 路径为例 /*** Return root of the "system" partition holding the core Android OS.* Always present and mounted read-only.*/public static @NonNull File getRootDirectory() {return DIR_ANDR