Autoware(Pure pursuit代码学习)
Pure Pursuit学习（二）
1. 前言
pure pursuit只能用于一些简单的场景，如直线道路上的循迹；对于一些复杂的路径如U型/S型等曲线路径跟踪效果较差。
根据pure pursuit的原理可以知道，其跟踪效果很大程度上取决于前视距离 L d L_{d} Ld的选择，设置固定的前视距离和路径曲率肯定无法适应不同的路径，因此就需要对前视距离的计算方法进行研究改进。Pure Pursuit 纯追踪法理论推导、预瞄距离的确定见前篇文章：Pure Pursuit 纯追踪法
在Autoware中，绿色的球体即为计算的跟踪预瞄点；红色的点为规划好的路径点；白色的轨迹为轨迹跟踪算法计算出的车辆将要运行的轨迹。
2. Autoware算法框架
如图所示Autoware里Purepursuit的算法框架
见Cmake 4.1
总共有4个cpp文件：
pure_pursuit_core.cpp：Topic收发
pure_pursuit_node.cpp：main函数入口
pure_pursuit.cpp：规划路径点有效性的判定，曲率的计算
pure_pursuit_viz.cpp（暂且略过）：Rviz的显示
2.1 pure_pursuit_node.cpp
主函数，实例化对象并运行
// ROS Includes
#include 2.2 pure_pursuit_core.cpp
/*pure_pursuit_core.h*/
#ifndef PURE_PURSUIT_PURE_PURSUIT_CORE_H
#define PURE_PURSUIT_PURE_PURSUIT_CORE_H
// ROS includes
#include /*pure_pursuit_core.cpp*/
#include 2.3 pure_pursuit.cpp
/*pure_pursuit.h*/
#ifndef PURE_PURSUIT_PURE_PURSUIT_H
#define PURE_PURSUIT_PURE_PURSUIT_H
// ROS includes
#include /*pure_pursuit.cpp*/
#include 2.4 详解求曲率kappa
//计算下一路径点与汽车当前位置之间的圆弧曲率 这里利用的相对坐标和圆里面的直角三角形的相似来求得圆的半径，最后得到圆弧曲率
double PurePursuit::calcCurvature(const geometry_msgs::Point& target) const
{
double kappa;
const geometry_msgs::Point pt = calcRelativeCoordinate(target, current_pose_);
const double denominator = pt.x * pt.x + pt.y * pt.y;
const double numerator = 2.0 * pt.y;
if (denominator != 0.0)
{
kappa = numerator / denominator; //kappa = 2y/L^2
}
else
{
kappa = numerator > 0.0 ? KAPPA_MIN_ : -KAPPA_MIN_;
}
return kappa;
}
将全局坐标下的目标点转换成车辆坐标下
X O Y XOY XOY: 世界坐标系
X 1 O 1 Y 1 X_{1}O_{1}Y_{1} X1O1Y1 :车辆坐标系/ROS坐标系
X 1 X_{1} X1轴是车辆的运动方向, Y 1 Y_{1} Y1轴在车辆左方
A B = L , A C = y , B C = x , B O 2 = R AB=L,AC=y,BC=x,BO_{2}=R AB=L,AC=y,BC=x,BO2=R
x 2 + y 2 = L 2 x^2+y^2=L^2 x2+y2=L2
x 2 + ( R − y ) 2 = R 2 x^2+(R-y)^2=R^2 x2+(R−y)2=R2 => x 2 + y 2 = 2 R y x^2+y^2=2Ry x2+y2=2Ry
R = L 2 2 y \large R=\frac{L^2}{2y} R=2yL2 => k a p p a = 1 R = 2 y L 2 \large kappa=\frac{1}{R}=\frac{2y}{L^2} kappa=R1=L22y
X 1 O 1 Y 1 X_{1}O_{1}Y_{1} X1O1Y1以当前车辆为原点，通过函数计算出目标位置点相对于车辆当前位置的坐标P点。