纯跟踪(Pure Pursuit)路径跟踪算法研究（2）

纯跟踪(Pure Pursuit)路径跟踪算法研究（2）

下午进行了简单的公式推导，理论推导部分是没有问题的

下面的博客提供了在实车上用 GPS 实现纯跟踪控制的一些思路和注意点

Pure Pursuit（纯追踪算法）ROS实践

并不急于在实车上实现，第一步实现 CarSim 于 Simulink 的联合仿真

下面几篇博客主要是用Python模拟，读着稍微有点困难

无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪

无人驾驶运动控制----pure pursuit算法实践和理解

自主跟随学习笔记（一）使用纯跟踪（pure pursuit）算法跟随目标

下面的博客介绍了纯跟踪的原理以及在开源框架 Autoare 中的实现

自动驾驶Motion Planning—Pure Pursuit算法原理及实现

Pure Pursuit算法的基本思想是：参考人类驾驶的行为，通过计算车辆当前位置到预瞄点（goal point）的曲率，使车辆沿着经过预瞄点的圆弧行驶，从而实现轨迹跟踪(如图1)。因此，该算法的核心在于通过设计合理的预瞄距离，从而计算出轨迹跟踪的控制曲率

Pure Pursuit本质上是一个基于横向误差控制的比例控制器

预瞄点(goal point)的选择

如何从路径点中选择预瞄点并且保证预瞄点在行驶方向的前方而不是反方向，还不清楚

预瞄距离(Ld)的选择

Pure Pursuit的效果强依赖于预瞄距离的选择，可根据车速动态的调整预瞄距离，从而增强系统的控制鲁棒性

下面的代码是基于上面的 Python 代码，用 MATLAB 实现了下，但代码并不完整

自动驾驶—基于运动学模型约束的纯跟踪控制算法 Pure Pursiut

纯跟踪算法是一种基于运动学约束的算法，对该算法的学习可以更好地理解车辆运动学模型。根据上一文档的讲解，我们可以建立后轮速度、前轮偏角和车辆大地坐标系下的位置、航向角的关系，那么我们在忽略轮胎力学、执行机构特性等情况下，就能根据后轮速度和前轮偏角任意控制车辆的速度、位置。本文只讨论横向控制，假设后轮速度恒定，也就是只控制前轮偏角

首先也应当实现当车辆后轮速度恒定，即车辆纵向速度恒定下的纯跟踪控制

纯跟踪算法提出“预瞄距离”的概念，根据预瞄距离寻找目标轨迹中符合条件的目标路径点，判断逻辑就是寻找目标轨迹上哪个点和当前车辆位置（图中的当前后轮位置）的相对距离等于预瞄距离（可以以当前位置为圆心，预瞄距离为半径，画圆，寻找与目标轨迹的交点），则该点就是当前时刻的目标点（图中的目标后轮位置）。控制目标则是计算多大的前轮偏角，可以使当前后轮位置运动到目标后轮位置

恒定后轮速度，恒定前轮偏角，那么后轮的运动轨迹将是一个圆，即车辆绕着转动中心做圆周运动，如下图所示：

基于这个现象，可以认为在每个运行周期内（速度恒定，前轮偏角恒定），车辆绕转动中心做圆周运动。那么控制目标就可以简化为两点：①是怎样的圆周运动轨迹可以将后轮从当前位置运动至目标位置？②如何控制前轮偏角实现这个圆周运动？

下面的博客并没有给出具体的 Simulink 实现，但对预瞄控制的解释还是很生动的

Pure Pursuit(纯跟踪算法)与Simulink实现

如上图所示，A点是车辆后轴中心（也是图中坐标系的原点，图中这种坐标系我们一般称之为：车辆坐标系），B点是车辆要达到的目标点（预瞄点），L是AB之间的距离（预瞄距离），O点是假设存在的圆心使得车辆从位置A以转弯半径r可以达到位置B。圆心位置O的坐标与转弯半径r就是计算目标。此图颇有遗憾，因为AO线段看上去比BO线段长，但二者应该是相等的，都是转弯半径

纯跟踪控制算法就是一种**基于几何关系计算车辆转弯半径**的方法

纯跟踪算法核心计算部分是非常简洁的，不过实际应用时还有两块工作要做：1，从期望轨迹中找预瞄点B，方法是从期望轨迹上找距离A点等于预瞄距离L的点。2，A点与B点坐标可能都是基于某个全局坐标系，所以需要将B点坐标从全局坐标系转换至车辆坐标系上。这两部分计算是使用纯跟踪算法的准备工作

上面所说的两个问题确实是比较关键的问题

全局坐标系到车辆坐标系的变换或许可以参考下面的代码

自动驾驶Motion Planning—Pure Pursuit算法原理及实现

float32_t PurePursuit::compute_points_distance_squared(
  const TrajectoryPoint & point1,
  const TrajectoryPoint & point2)
{
  using autoware::common::types::float64_t;
  const float64_t diff_x = point1.pose.position.x - point2.pose.position.x;
  const float64_t diff_y = point1.pose.position.y - point2.pose.position.y;
  return static_cast<float32_t>((diff_x * diff_x) + (diff_y * diff_y));
}

std::pair<float32_t, float32_t> PurePursuit::compute_relative_xy_offset(
  const TrajectoryPoint & current,
  const TrajectoryPoint & target) const
{
  const auto diff_x = target.pose.position.x - current.pose.position.x;
  const auto diff_y = target.pose.position.y - current.pose.position.y;
  const auto yaw = ::motion::motion_common::to_angle(current.pose.orientation);
  const auto cos_pose = std::cos(yaw);
  const auto sin_pose = std::sin(yaw);
  const auto relative_x = static_cast<float32_t>((cos_pose * diff_x) + (sin_pose * diff_y));
  const auto relative_y = static_cast<float32_t>((-sin_pose * diff_x) + (cos_pose * diff_y));
  const std::pair<float32_t, float32_t> relative_xy(relative_x, relative_y);
  return relative_xy;
}

float32_t PurePursuit::compute_steering_rad(const TrajectoryPoint & current_point)
{
  // Compute the steering angle by arctan(curvature * wheel_distance)
  // link: https://www.ri.cmu.edu/pub_files/2009/2/
  //       Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf
  const float32_t denominator = compute_points_distance_squared(current_point, m_target_point);
  const float32_t numerator = compute_relative_xy_offset(current_point, m_target_point).second;
  constexpr float32_t epsilon = 0.0001F;
  // equivalent to (2 * y) / (distance * distance) = (2 * sin(th)) / distance
  const float32_t curvature = (denominator > epsilon) ? ((2.0F * numerator) / denominator) : 0.0F;
  const float32_t steering_angle_rad = atanf(curvature * m_config.get_distance_front_rear_wheel());
  return steering_angle_rad;
}

车辆当前位置和预瞄点间连线与车辆轴线(前进方向)夹角可以参考下面的公式

《基于纯追踪算法和樽海鞘优化算法的无人驾驶路径跟踪算法研究》

纯跟踪算法的轨迹跟随效果，与参数：预瞄距离关系很大。就像人开车时眼睛往前看多远，看的太近了，到了弯道才慌忙打方向盘；看的太远了，又容易导致最近一段的轨迹跟随效果不好

最后谈一下纯跟踪算法的硬伤：纯跟踪算法可以缩小车辆与期望轨迹的位置偏差，但是对角度偏差束手无策，因为数学原理上根本就没有考虑角度偏差

下面的博客用 MATLAB 跟踪了圆形轨迹，并且测试了大预瞄距离和小预瞄距离的情况

路径跟踪之Pure Pursuit控制算法

减小前视距离，仿真结果如下图所示，可以看出跟踪结果出现了一定程度的震荡，这是因为前视距离减小后，近似P控制器的比例系数增加，从而造成动态响应出现超调和震荡

同时讨论了朝向角 == 90度和朝向角 > 90度的情况

如果预瞄点如下图所示，那么大于90度，车辆后轮的运行轨迹应该是大圆，绕了一个大圈，是在向后跟踪，此时的前轮转角计算公式仍然不变

等于90度的情况，车辆后轮的运行轨迹应该是右上那个半圆

通过以上分析，不难发现，三种情况下的前轮转角计算公式均适用，同样“近似P控制器”这个结论在上述三种情况下也都成立

下面的方法简要分析了 Pure Pursuit、Stanley、MPC 这几种轨迹跟踪控制方法，可以作为后面继续研究的开展

横向自动控制方法：Purepursuit, Stanley, MPC对比

下面的博客给出了 Pure Pursuit 的仿真实现，可以参考学习下，实测可以跑

PurePursuit纯追踪算法MATLAB程序个人总结

Pure Pursuit纯跟踪算法Python/Matlab算法实现

%% 纯跟踪算法跟随效果与预瞄距离关系很大。就像人开车时眼睛往前看多远，看的太近了，到了弯道才慌忙打方向盘；看的太远了，又容易导致最近一段的轨迹跟随效果不好。

%% initialization
k = 0.1;                                  % look forward gain  前向预测距离所用增益
Kp = 1.0 ;                                % speed propotional gain
Lfc = 1;                                  % 基础预瞄距离
dt = 0.1;                                 % [s]
L = 2.9  ;                                % [m] wheel base of vehicle
cx = 0:0.1:50;                            %每0.1长度取一次预瞄点的x值

for i = 1:length(cx)                      %全局路径c(y)生成 路径初始化
     cy(i) = cos(cx(i)/5)*cx(i)/4;
end

i = 1;
target_speed = 10/3.6;                    %设定速度  如果实际自动驾驶，这个就是设定要到达的速度，
                                          %附加：目前这里没有规划，如果有规划，就在过弯的时候给出横向加速度，所以车身方向速度就会减小。
T = 500;
lastIndex = length(cx);                   %返回cx的向量长度   有501个预瞄点x值   有501个序号
x = 0; y = 4; yaw = 0; v = 0;             %车辆初始位置  
time = 0;
Ld = k * v + Lfc;                         %初始速度为0时的预瞄距离Ld=Lfc
 
 figure(1);
while T > time 
  target_index = calc_target_index(x,y,cx,cy);                 %输出距离车辆当前最近的点的位置
  ai = PIDcontrol(target_speed,v,Kp); %
  Ld = k * v + Lfc ;
  delta = pure_pursuit_control(x,y,yaw,v,cx,cy,target_index,k,Lfc,L,Ld); %前轮要转的角度
   
  [x,y,yaw,v] = update(x,y,yaw,v, ai, delta,dt,L); %更新车辆状态
  time = time + dt;                          %时间过完一周期
% pause(0.1)
  plot(cx,cy,'b',x,y,'r-*')                 %将预瞄点位置用蓝色表示，当前位置用红色表示
  drawnow
  hold on
  
end

function [x, y, yaw, v] = update(x, y, yaw, v, ai, delta,dt,L)   %更新车辆状态的函数
    x = x + v * cos(yaw) * dt;
    y = y + v * sin(yaw) * dt;
    yaw = yaw + v / L * tan(delta) * dt;
    v = v + ai * dt;
end

function [a] = PIDcontrol(target_v, current_v, Kp)              %计算加速度的函数
a = Kp * (target_v - current_v);
end

function [delta] = pure_pursuit_control(x,y,yaw,v,cx,cy,ind,k,Lfc,L,Ld)  %纯追踪控制器前轮转角方程的函数
    tx = cx(ind);                          %距离车辆当前最近的点的位置对应的cx值     ind是距离车辆当前最近的点的位置
    ty = cy(ind);                          %距离车辆当前最近的点的位置对应的cy值
    
    alpha = atan((ty-y)/(tx-x))-yaw;       %该处定义向左转为alpha=beta-Fai，所以向右转就输出-alpha
    
 Ld = k * v + Lfc ;                        % 预瞄距离Ld与车速成线性关系 
 delta = atan(2*L * sin(alpha)/Ld)  ;                %前轮转角
end

function [ind] = calc_target_index(x,y, cx,cy)      %计算找到距车辆当前位置最近点的序号  输出最近点的函数
N =  length(cx);                                     %N = 501
Distance = zeros(N,1);                               % 501行 1列的矩阵

for i = 1:N
Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);      %每一个预瞄点到初始位置的距离
end

[~, location]= min(Distance);                        %找出最近的距离对应的位置
ind = location;
ind = ind + 5
end

实际上 MATLAB 本身也提供了 Pure Pursuit 工具箱

MATLAB Pure Pursuit Controller

GitHub 上也有相关的基于 Gazebo 的仿真

https://github.com/NeXTzhao/planning