写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
个人主页：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
==转载请注明出处，尊重原创，从我做起。==

👍 点赞、评论、收藏，三连走一波，让我们一起养成好习惯😜
==在这里，您将收获的不只是技术干货，还有思维的火花！==

📚 系列专栏：【运动控制】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！

🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

目录

  • 引言
  • 一、纯跟踪算法简介
  • 二、自行车模型概述
  • 三、纯跟踪算法理论基础
  • 3.1 算法基本思想
  • 3.2 几何关系推导
  • 四、纯跟踪算法实现步骤
  • 五、纯跟踪算法优化
  • 5.1 前视距离对性能的影响及优化策略
  • 5.2 根据速度缩放前视距离
  • 5.3 比例系数的影响及调整
  • 5.4 限制值范围
  • 5.5 高度鲁棒性
  • 六、纯跟综算法的难点与局限性
  • 6.1 最佳前视距离的选择
  • 6.2 路径追踪与稳定性的平衡
  • 6.3 高速下前视距离的调整与稳态误差
  • 七、编程实现
  • 7.1 pure_pursuit.param.yaml
  • 7.2 pure_pursuit_lateral_controller.cpp
  • 7.3 pure_pursuit.cpp
  • 7.4 planning_utils.cpp
  • 八、总结
  • 参考资料

引言

  在自动驾驶领域，Pure Pursuit纯跟踪算法作为一种高效的车道保持技术，备受关注。本文将带您深入解析Autoware开源项目中Pure Pursuit算法的实现细节，揭秘自动驾驶车辆如何精确追踪参考轨迹。通过讲解算法原理、实现步骤及优化策略，探索这一关键技术的精髓。

一、纯跟踪算法简介

目前主流方法有两类：

  • 基于几何的方法
  • 基于模型的方法

  本篇博客介绍基于几何方法的 Pure Pursuit 纯跟踪算法。

  Pure Pursuit 纯追踪算法是一种广泛使用的基于几何的追踪方法，建立在自行车模型和阿曼转向几何基础上，在低速状态下有比较好的效果。

二、自行车模型概述

  在讲纯追踪算法之前，先回顾一下自行车模型。

  自行车模型是将四轮简化成两轮的模型，假设车只在平面上行驶，最大好处是简化了前轮转向角和后轮轨迹曲率之间的关系，根据阿克曼转向几何关系，可以建立车辆前轮转向角和后轮轨迹曲率之间的关系。

![在这里插入图片描述](https://i-blog.csdnimg.cn/direct/6b0f6ea7051c4b06896434d12ce8c7e2.png#pic_center =400x)

  假设前轮的转角为 $\delta$，后轮滑过的半径为 $R$，有如下关系：
$$\tan \delta = \frac { L } { R }$$其中，$\delta$ 是前轮转向角， $L$ 是轴距，$R$ 是在给定转向角后，后轮所遵循的圆半径。此公式能在低速场景下对车辆的运动做简单估计。

三、纯跟踪算法理论基础

3.1 算法基本思想

  纯跟踪算法最基本的思想就是参考人类驾驶员的行为，以车的后轮轴心为基点，通过控制前轮转向，使得车沿着一条经过预瞄点的圆弧行驶。即 Pure Pursuit 算法以后轮轴心为切点，纵向车身为切线，通过控制前轮转角，使得车辆沿着一条通过目标点的圆弧行驶。

![在这里插入图片描述](https://i-blog.csdnimg.cn/direct/f5515014e15a456b8f7f402f81fe32c9.png#pic_center =400x)

  由这样的定义可以看出，首先要选择预瞄点， 预瞄点是距离后轮中心 $l_d$长度的路径点，$l_d$ 为前视距离。

3.2 几何关系推导

  当路径是这样时，连接目标点和后轮中心，设目标点是 $G$，转向中心是 $O$，后轮的中心是$B$，可以构建出三角形 $\Delta OBG$：

![在这里插入图片描述](https://i-blog.csdnimg.cn/direct/749e3de1ae2f40a5a99639ff9353780a.png#pic_center =400x)
  由于知道目标点和车身后轮方向，即目标点方向和当前航向角的夹角为 $\alpha$。而实际是切线，所以可得到 $\angle OBG=\frac{\pi}{2}-\alpha$ ，因为$\Delta OBG$ 是等腰三角形。所以两个底角相等，都等于 $\frac{\pi}{2}-\alpha$，由于是三角形，所以顶点角度就是 $2 \alpha$ 。

  下面来看一下如何根据几何关系来找到前轮转角 $\delta$ 和目标点的关系。

  看一下三角形 $\Delta OBG$，刚才讲过 $\overline{BG}=l_d$，所以根据三角形正弦定理，可得:
$$ \frac { l _ { d } } { \sin ( 2 \alpha ) } = \frac { R } { \sin ( \frac { \pi } { 2 } - \alpha ) }$$由三角函数就可以把这两项展开，得到：
$$ \frac { l _ { d } } { 2 \sin ( \alpha ) \cos ( \alpha ) } = \frac { R } { \cos ( \alpha ) }$$这时发现有两个 $\cos \alpha$，相互抵消，得到：
$$ \frac { l _ { d } } { \sin ( \alpha ) } = 2 R$$这样得到 $(1)$ 式
$$ \frac { 1 } { R } = \frac { 2 \sin ( \alpha ) } { l _ { d } }\tag{1}$$建立了转弯半径与目标点方向和当前航向角的夹角之间的关系。

根据阿克曼转向几何可得：
$$ \tan \delta = \frac { L } { R } \Rightarrow \delta = \tan ^ { - 1 } ( \frac { L } { R } )\tag{2}$$根据 $(1)$ 式 $(2)$ 式可得：
$$ \delta ( t ) = \tan ^ { - 1 } ( \frac { 2 L \sin ( \alpha ( t ) ) } { l _ { d } } )\tag{3}$$

这时看下面的三角形 $\Delta BGA$：

![在这里插入图片描述](https://i-blog.csdnimg.cn/direct/19f69d88205546f4993850c4e6207908.png#pic_center =400x)

可得
$$ \sin ( \alpha ) = \frac { e _ { l_d } } { l _ { d } } $$设 $AG =e_{l_d}$ 为横向误差，把 $\sin\alpha$ 带到 $(3)$ 式可得：
$$\delta ( t ) = \tan ^ { - 1 } ( \frac { 2 L } { l _ { d } ^ { 2 } } e _ { l _ { d } } ( t ) )\tag{4}$$  这时可以看到， Pure Pursuit 算法的本质就是对前视横向误差 $e_{l_d}$ 以$\frac { 2 L } { l _ { d } ^ { 2 } }$ 为比例系数的比例控制器。这样就建立起了转向角 $\delta$ 和前视距离 $l_d$，还有车辆轴距 $L$ 以及目标点方向和当前航向角夹角 $\alpha$ 之间的关系。

四、纯跟踪算法实现步骤

下面介绍 Pure Pursuit 算法的实现步骤。

  $Step\ 1$：确定车辆自身位置，可以通过定位模块直接拿到车辆的实时位置。

  $Step\ 2$：找出在规划路径中距离车辆最近的点。

  $Step\ 3$：找到预瞄点。首先要定义预瞄距离 $l_d$，然后以车的后轮中心为圆心， $l_d$ 为半径画圆弧，在规划路径上找到距离车辆后轮中心 $l_d$ 距离的交点，交点在不同路径中可能不只一个。

  比如在做非常激进的变道场景中， 路径是这样：

![在这里插入图片描述](https://i-blog.csdnimg.cn/direct/28b547995d344635a4324b222c04f6e2.png#pic_center =200x)

  假如车在这样的位置，如果前视距离为 $l_d$，可能出现两个交点，在这种情况下需要先找离车最近的路径点，选择比较靠近车辆的目标点为最终的预瞄点。

  $Step\ 4$：把预瞄点转化到车身坐标系下，方便计算。

  $Step\ 5$：用刚才推出来 Pure Pursuit 算法计算公式算出到达目标点所需的转向角，并操纵车辆的转向运动。

  $Step\ 6$：根据单位时间内车辆的运动，更新车辆的实时状态

这就是 Pure Pursuit 算法的基本步骤。

五、纯跟踪算法优化

  从前面的推导中，可以看到影响 Pure Pursuit 算法性能最重要的量就是 $l_d$：

$$ \delta ( t ) = \tan ^ { - 1 } ( \frac { 2 L \sin ( \alpha ( t ) ) } { l _d } ) $$  直接影响车的转向角大小，从而影响车辆对轨迹的追踪能力。纯跟踪算法优化主要是对前视距离 $l_d$ 的选择。

5.1 前视距离对性能的影响及优化策略

  较短的前视距离能提供更精确的跟踪，而较长的距离则能提供更平滑的跟踪。

  通常选择小一点的 $l_d$，追踪会更加准确，预瞄点更靠近车辆本身，所以更好追踪。但这样就需要不断地更换目标点，导致系统的稳定性也会随之降低。而如果大一点的目标点，可以跟踪更长时间，表现更稳定，但也牺牲掉追踪的准确性。

5.2 根据速度缩放前视距离

  还有一种就是比较常用的前视距离表达方式，就是把 $l_d$ 和速度相关联，随速度增加而增大前视距离：
$$l_d=kv_x$$  随速度增加，$l_d$ 也会越大，这样就会提高 Pure Pursuit 算法适应不同速度的跟踪能力。

5.3 比例系数的影响及调整

  $k$ 值过小会导致不稳定，而 $k$ 值过大会导致跟踪效果不佳。

$$ \delta ( t ) = \tan ^ { - 1 } ( \frac { 2 L \sin ( \alpha ( t ) ) } { l _d } ) = \tan ^ { - 1 } ( \frac { 2 L \sin ( \alpha ( t ) ) } { kv _ { x } ( t ) } )$$  因为 $k$ 在前轮转角表达式的分母上，所以如果 $k$ 太小，会造成不稳定；如果 $k$ 太大，会牺牲掉追踪的准确度，导致跟踪效果不佳。

5.4 限制值范围

  可设置最大最小的值范围，使得来确保追踪的准确性和车辆的稳定性。

5.5 高度鲁棒性

  由于 Pure Pursuit 算法本身对路径的曲率没有直接的关系，即可以看出来Pure Pursuit 算法公式里完全没有和参考路径的关系，导致算法对路径的连续性要求并不高，所以 可以很好的处理不连续路径的轨迹追踪问题。

六、纯跟综算法的难点与局限性

  Pure Pursuit 纯跟踪算法是简单实用的控制器，但也正是由于它的简单性，给方法本身带来了难点和局限性。

6.1 最佳前视距离的选择

  将前视距离 $l_d$ 和速度相关联是常用方法，但车辆追踪轨迹完全依靠预瞄点，而不和路径本身的曲率相关联。

  由于算法本身的原因，在曲线路径中，当车接近目标点时，哪怕是以很慢的速度，但是距离只要在预瞄点范围之内，就会马上更新到下个预瞄点，这样就车还没到达预瞄点，在靠近预描点时，车就更换了预瞄点，导致车辆对曲线路径无法做到 $100$% 的追踪效果，很难保证非常小的跟踪误差。当然在直线的情况下没有问题，因为当在跟踪直线时，一般经过几次预瞄点的更新就可以很好得跟踪直线。

  所以可以在在选择前视距离 $l_d$ 时，把路径的曲率信息也考虑进来，更进一步把横向误差也考虑进来，这样会进一步提高 Pure Pursuit 算法的鲁棒性。

6.2 路径追踪与稳定性的平衡

  在特定路径上，不要过度追求完美的追踪效果。因为在曲线路径上，小车永远都是在看前方的轨迹，除非轨迹是一条直线，否则轨迹误差是永远存在。

  改变前视距离只是改变车辆行驶的曲率半径，可以补偿因车辆转向不足而增加的（与运动学自行车模型相比）半径。

  如果为了确保稳定性而进行调整，由于前视距离较长而在路径上走弯路，就会大大降低追踪的准确度。

6.3 高速下前视距离的调整与稳态误差

  在高速情况下，前视距离 $l_d$ 增加，误差也会变大，即使不考虑速度因素，在速度逐渐增加时，也会增加更新前视距离的频率，导致还没等到小车真正跟踪到目标点，就已经跳到第二个目标点了，这样也会导致曲线的稳态误差变大。

七、编程实现

   在自动驾驶开源代码 autoware 中，详见：

  autoware.universe/tree/main/control/pure_pursuit

  纯跟踪算法 Pure Pursuit 的实现比较简单，距离工程化应用还有一定差距，但对于算法的学习已经足够了，下面详细讲解一下 Autoware 的纯跟踪算法实现。

  下面是整个纯跟踪算法的目录树，可以看到并不复杂。

![在这里插入图片描述](https://i-blog.csdnimg.cn/direct/125f6da9981647d4bc800bd5165a35e3.png#pic_center =400x)

其中最重要的几个文件是：

  • pure_pursuit.param.yaml
  • pure_pursuit_lateral_controller.cpp
  • pure_pursuit.cpp
  • planning_utils.cpp

  看懂这几个文件，整个纯跟踪算法就差不多掌握了，这也是学习 autoware 纯跟踪代码的顺序。

7.1 pure_pursuit.param.yaml

  pure_pursuit.param.yaml里都是纯跟踪算法用到的一些参数，包括用于计算预瞄距离 $l_d$ 的各个系数和最小最大预瞄距离。

  唯一点需要说明的是，这里给出计算预瞄距离的系数太过单一，实际工程化应该通过查表得到各个系数，具体代码如下：

/**:
  ros__parameters:
    ld_velocity_ratio: 2.4
    ld_lateral_error_ratio: 3.6
    ld_curvature_ratio: 120.0
    long_ld_lateral_error_threshold: 0.5
    min_lookahead_distance: 4.35
    max_lookahead_distance: 15.0
    converged_steer_rad: 0.1
    reverse_min_lookahead_distance: 7.0
    prediction_ds: 0.3
    prediction_distance_length: 21.0
    resampling_ds: 0.1
    curvature_calculation_distance: 4.0
    enable_path_smoothing: false
    path_filter_moving_ave_num: 25

7.2 pure_pursuit_lateral_controller.cpp

#include "pure_pursuit/pure_pursuit_lateral_controller.hpp"

#include "pure_pursuit/pure_pursuit_viz.hpp"
#include "pure_pursuit/util/planning_utils.hpp"
#include "pure_pursuit/util/tf_utils.hpp"

#include <vehicle_info_util/vehicle_info_util.hpp>

#include <algorithm>
#include <memory>
#include <utility>

namespace
{
enum TYPE {
  VEL_LD = 0,
  CURVATURE_LD = 1,
  LATERAL_ERROR_LD = 2,
  TOTAL_LD = 3,
  CURVATURE = 4,
  LATERAL_ERROR = 5,
  VELOCITY = 6,
  SIZE  // this is the number of enum elements
};
}  // namespace

namespace pure_pursuit
{
PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node)
: clock_(node.get_clock()),
  logger_(node.get_logger().get_child("lateral_controller")),
  tf_buffer_(clock_),
  tf_listener_(tf_buffer_)
{
  pure_pursuit_ = std::make_unique<PurePursuit>();

  // Vehicle Parameters
  const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
  param_.wheel_base = vehicle_info.wheel_base_m;
  param_.max_steering_angle = vehicle_info.max_steer_angle_rad;

  // Algorithm Parameters
  param_.ld_velocity_ratio = node.declare_parameter<double>("ld_velocity_ratio");
  param_.ld_lateral_error_ratio = node.declare_parameter<double>("ld_lateral_error_ratio");
  param_.ld_curvature_ratio = node.declare_parameter<double>("ld_curvature_ratio");
  param_.long_ld_lateral_error_threshold = node.declare_parameter<double>("long_ld_lateral_error_threshold");
  param_.min_lookahead_distance = node.declare_parameter<double>("min_lookahead_distance");
  param_.max_lookahead_distance = node.declare_parameter<double>("max_lookahead_distance");
  param_.reverse_min_lookahead_distance = node.declare_parameter<double>("reverse_min_lookahead_distance");
  param_.converged_steer_rad_ = node.declare_parameter<double>("converged_steer_rad");
  param_.prediction_ds = node.declare_parameter<double>("prediction_ds");
  param_.prediction_distance_length = node.declare_parameter<double>("prediction_distance_length");
  param_.resampling_ds = node.declare_parameter<double>("resampling_ds");
  param_.curvature_calculation_distance = node.declare_parameter<double>("curvature_calculation_distance");
  param_.enable_path_smoothing = node.declare_parameter<bool>("enable_path_smoothing");
  param_.path_filter_moving_ave_num = node.declare_parameter<int64_t>("path_filter_moving_ave_num");

  // Debug Publishers
  pub_debug_marker_ =
    node.create_publisher<visualization_msgs::msg::MarkerArray>("~/debug/markers", 0);
  pub_debug_values_ = node.create_publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
    "~/debug/ld_outputs", rclcpp::QoS{1});

  // Publish predicted trajectory
  pub_predicted_trajectory_ = node.create_publisher<autoware_auto_planning_msgs::msg::Trajectory>(
    "~/output/predicted_trajectory", 1);
}

double PurePursuitLateralController::calcLookaheadDistance(
  const double lateral_error, const double curvature, const double velocity, const double min_ld,
  const bool is_control_cmd)
{
  const double vel_ld = abs(param_.ld_velocity_ratio * velocity);           //  k1*v
  const double curvature_ld = -abs(param_.ld_curvature_ratio * curvature);  // -k2*kappa
  double lateral_error_ld = 0.0;

  if (abs(lateral_error) >= param_.long_ld_lateral_error_threshold) {
    // If lateral error is higher than threshold, we should make ld larger to prevent entering the
    // road with high heading error.
    // 如果横向误差高于阈值，我们应该使ld更大，以防止进入具有高航向误差的道路。
    lateral_error_ld = abs(param_.ld_lateral_error_ratio * lateral_error);  // k3*Err_y
  }

  // 总的预瞄距离，限幅
  const double total_ld =
    std::clamp(vel_ld + curvature_ld + lateral_error_ld, min_ld, param_.max_lookahead_distance);

  auto pubDebugValues = [&]() {
    tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{};
    debug_msg.data.resize(TYPE::SIZE);
    debug_msg.data.at(TYPE::VEL_LD) = static_cast<float>(vel_ld);
    debug_msg.data.at(TYPE::CURVATURE_LD) = static_cast<float>(curvature_ld);
    debug_msg.data.at(TYPE::LATERAL_ERROR_LD) = static_cast<float>(lateral_error_ld);
    debug_msg.data.at(TYPE::TOTAL_LD) = static_cast<float>(total_ld);
    debug_msg.data.at(TYPE::VELOCITY) = static_cast<float>(velocity);
    debug_msg.data.at(TYPE::CURVATURE) = static_cast<float>(curvature);
    debug_msg.data.at(TYPE::LATERAL_ERROR) = static_cast<float>(lateral_error);
    debug_msg.stamp = clock_->now();
    pub_debug_values_->publish(debug_msg);
  };

  if (is_control_cmd) {
    pubDebugValues();
  }

  return total_ld;
}

TrajectoryPoint PurePursuitLateralController::calcNextPose(
  const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const
{
  geometry_msgs::msg::Transform transform;
  transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0);
  transform.rotation =
    planning_utils::getQuaternionFromYaw(((tan(cmd.steering_tire_angle) * ds) / param_.wheel_base));
  TrajectoryPoint output_p;

  tf2::Transform tf_pose;
  tf2::Transform tf_offset;
  tf2::fromMsg(transform, tf_offset);
  tf2::fromMsg(point.pose, tf_pose);
  tf2::toMsg(tf_pose * tf_offset, output_p.pose);
  return output_p;
}

void PurePursuitLateralController::setResampledTrajectory()
{
  // Interpolate with constant interval distance.
  std::vector<double> out_arclength;

  // 将原始轨迹转换为轨迹点数组
  const auto input_tp_array = motion_utils::convertToTrajectoryPointArray(trajectory_);
  const auto traj_length = motion_utils::calcArcLength(input_tp_array);
  
  // 以恒定的间隔距离进行插值
  for (double s = 0; s < traj_length; s += param_.resampling_ds) {
    out_arclength.push_back(s);
  }
  trajectory_resampled_ = std::make_shared<autoware_auto_planning_msgs::msg::Trajectory>(
                          motion_utils::resampleTrajectory(
                          motion_utils::convertToTrajectory(input_tp_array), out_arclength));
  
  // 将重新采样的轨迹的最后一个点替换为原始轨迹的最后一个点，以保持轨迹的连续性。
  trajectory_resampled_->points.back() = trajectory_.points.back();
  // 将重新采样的轨迹的头部信息设置为与原始轨迹相同。
  trajectory_resampled_->header = trajectory_.header;
  output_tp_array_ = motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_);
}

double PurePursuitLateralController::calcCurvature(const size_t closest_idx)
{
  // Calculate current curvature
  const size_t idx_dist = static_cast<size_t>(
    std::max(static_cast<int>((param_.curvature_calculation_distance) / param_.resampling_ds), 1));

  // Find the points in trajectory to calculate curvature
  size_t next_idx = trajectory_resampled_->points.size() - 1;
  size_t prev_idx = 0;

  // 根据最近点的索引进行判断，确定计算曲率时的起点和终点的索引
  if (static_cast<size_t>(closest_idx) >= idx_dist) {
    prev_idx = closest_idx - idx_dist;
  } else {
    // return zero curvature when backward distance is not long enough in the trajectory
    return 0.0;
  }

  // 判断是否存在足够的轨迹点进行曲率计算，确定计算曲率时的起点和终点的索引
  if (trajectory_resampled_->points.size() - 1 >= closest_idx + idx_dist) {
    next_idx = closest_idx + idx_dist;
  } else {
    // return zero curvature when forward distance is not long enough in the trajectory
    return 0.0;
  }
  // TODO(k.sugahara): shift the center point of the curvature calculation to allow sufficient
  // distance, because if sufficient distance cannot be obtained in front or behind, the curvature
  // will be zero in the current implementation.

  // Calculate curvature assuming the trajectory points interval is constant
  double current_curvature = 0.0;

  try {
    // 根据给定的三个轨迹点，计算曲率
    current_curvature = tier4_autoware_utils::calcCurvature(
      tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(prev_idx)),
      tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(closest_idx)),
      tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(next_idx)));
  } catch (std::exception const & e) {
    // ...code that handles the error...
    RCLCPP_WARN(rclcpp::get_logger("pure_pursuit"), "%s", e.what());
    current_curvature = 0.0;
  }
  return current_curvature;
}

void PurePursuitLateralController::averageFilterTrajectory(
  autoware_auto_planning_msgs::msg::Trajectory & u)
{
  if (static_cast<int>(u.points.size()) <= 2 * param_.path_filter_moving_ave_num) {
    RCLCPP_ERROR(logger_, "Cannot smooth path! Trajectory size is too low!");
    return;
  }

  autoware_auto_planning_msgs::msg::Trajectory filtered_trajectory(u);

  for (int64_t i = 0; i < static_cast<int64_t>(u.points.size()); ++i) {
    TrajectoryPoint tmp{};
    int64_t num_tmp = param_.path_filter_moving_ave_num;
    int64_t count = 0;
    double yaw = 0.0;
    if (i - num_tmp < 0) {
      num_tmp = i;
    }
    if (i + num_tmp > static_cast<int64_t>(u.points.size()) - 1) {
      num_tmp = static_cast<int64_t>(u.points.size()) - i - 1;
    }
    for (int64_t j = -num_tmp; j <= num_tmp; ++j) {
      const auto & p = u.points.at(static_cast<size_t>(i + j));

      tmp.pose.position.x += p.pose.position.x;
      tmp.pose.position.y += p.pose.position.y;
      tmp.pose.position.z += p.pose.position.z;
      tmp.longitudinal_velocity_mps += p.longitudinal_velocity_mps;
      tmp.acceleration_mps2 += p.acceleration_mps2;
      tmp.front_wheel_angle_rad += p.front_wheel_angle_rad;
      tmp.heading_rate_rps += p.heading_rate_rps;
      yaw += tf2::getYaw(p.pose.orientation);
      tmp.lateral_velocity_mps += p.lateral_velocity_mps;
      tmp.rear_wheel_angle_rad += p.rear_wheel_angle_rad;
      ++count;
    }
    auto & p = filtered_trajectory.points.at(static_cast<size_t>(i));

    p.pose.position.x = tmp.pose.position.x / count;
    p.pose.position.y = tmp.pose.position.y / count;
    p.pose.position.z = tmp.pose.position.z / count;
    p.longitudinal_velocity_mps = tmp.longitudinal_velocity_mps / count;
    p.acceleration_mps2 = tmp.acceleration_mps2 / count;
    p.front_wheel_angle_rad = tmp.front_wheel_angle_rad / count;
    p.heading_rate_rps = tmp.heading_rate_rps / count;
    p.lateral_velocity_mps = tmp.lateral_velocity_mps / count;
    p.rear_wheel_angle_rad = tmp.rear_wheel_angle_rad / count;
    p.pose.orientation = pure_pursuit::planning_utils::getQuaternionFromYaw(yaw / count);
  }
  trajectory_resampled_ = std::make_shared<Trajectory>(filtered_trajectory);
}

boost::optional<Trajectory> PurePursuitLateralController::generatePredictedTrajectory()
{
  const auto closest_idx_result =
    motion_utils::findNearestIndex(output_tp_array_, current_odometry_.pose.pose, 3.0, M_PI_4);

  if (!closest_idx_result) {
    return boost::none;
  }

  const double remaining_distance = planning_utils::calcArcLengthFromWayPoint(
    *trajectory_resampled_, *closest_idx_result, trajectory_resampled_->points.size() - 1);

  const auto num_of_iteration = std::max(
    static_cast<int>(std::ceil(
      std::min(remaining_distance, param_.prediction_distance_length) / param_.prediction_ds)),
    1);
  Trajectory predicted_trajectory;

  // Iterative prediction:
  for (int i = 0; i < num_of_iteration; i++) {
    if (i == 0) {
      // For first point, use the odometry for velocity, and use the current_pose for prediction.

      TrajectoryPoint p;
      p.pose = current_odometry_.pose.pose;
      p.longitudinal_velocity_mps = current_odometry_.twist.twist.linear.x;
      predicted_trajectory.points.push_back(p);

      const auto pp_output = calcTargetCurvature(true, predicted_trajectory.points.at(i).pose);
      AckermannLateralCommand tmp_msg;

      if (pp_output) {
        tmp_msg = generateCtrlCmdMsg(pp_output->curvature);
        predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity;
      } else {
        RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction");
        tmp_msg = generateCtrlCmdMsg(0.0);
      }
      TrajectoryPoint p2;
      p2 = calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg);
      predicted_trajectory.points.push_back(p2);

    } else {
      const auto pp_output = calcTargetCurvature(false, predicted_trajectory.points.at(i).pose);
      AckermannLateralCommand tmp_msg;

      if (pp_output) {
        tmp_msg = generateCtrlCmdMsg(pp_output->curvature);
        predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity;
      } else {
        RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction");
        tmp_msg = generateCtrlCmdMsg(0.0);
      }
      predicted_trajectory.points.push_back(
        calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg));
    }
  }

  // for last point
  predicted_trajectory.points.back().longitudinal_velocity_mps = 0.0;
  predicted_trajectory.header.frame_id = trajectory_resampled_->header.frame_id;
  predicted_trajectory.header.stamp = trajectory_resampled_->header.stamp;

  return predicted_trajectory;
}

bool PurePursuitLateralController::isReady([[maybe_unused]] const InputData & input_data)
{
  return true;
}

LateralOutput PurePursuitLateralController::run(const InputData & input_data)
{
  current_pose_ = input_data.current_odometry.pose.pose;
  trajectory_ = input_data.current_trajectory;
  current_odometry_ = input_data.current_odometry;
  current_steering_ = input_data.current_steering;

  // 通过固定间隔的方式重新采样原始轨迹，生成一条新的重新采样的轨迹
  setResampledTrajectory();
  if (param_.enable_path_smoothing) {
    averageFilterTrajectory(*trajectory_resampled_);
  }

  // 计算纯跟踪控制转角
  const auto cmd_msg = generateOutputControlCmd();

  LateralOutput output;
  output.control_cmd = cmd_msg;
  // 计算的控制转角与当前方向盘转角之差的绝对值 < 0.1rad  
  output.sync_data.is_steer_converged = calcIsSteerConverged(cmd_msg);

  // calculate predicted trajectory with iterative calculation
  const auto predicted_trajectory = generatePredictedTrajectory();
  if (!predicted_trajectory) {
    RCLCPP_ERROR(logger_, "Failed to generate predicted trajectory.");
  } else {
    pub_predicted_trajectory_->publish(*predicted_trajectory);
  }

  return output;
}

bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCommand & cmd)
{
  // 计算的控制转角与当前方向盘转角之差的绝对值 < 0.1rad  
  return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) <
                  static_cast<float>(param_.converged_steer_rad_);
}

AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd()
{
  // Generate the control command

  // 调用纯跟踪算法，计算跟踪曲率半径
  const auto pp_output = calcTargetCurvature(true, current_odometry_.pose.pose);

  AckermannLateralCommand output_cmd;
  if (pp_output) {
    // 计算方向盘转角，注意这里传入的是纯跟踪定圆的曲率，而不是规划轨迹的曲率
    output_cmd = generateCtrlCmdMsg(pp_output->curvature);
    prev_cmd_ = boost::optional<AckermannLateralCommand>(output_cmd);
    publishDebugMarker();
  } else {
    RCLCPP_WARN_THROTTLE(
      logger_, *clock_, 5000, "failed to solve pure_pursuit for control command calculation");
    if (prev_cmd_) {
      output_cmd = *prev_cmd_;
    } else {
      output_cmd = generateCtrlCmdMsg(0.0);
    }
  }
  return output_cmd;
}

AckermannLateralCommand PurePursuitLateralController::generateCtrlCmdMsg(
  const double target_curvature)
{
  // 根据阿克曼转向几何，计算方向盘转角  tanδ = L/R
  const double tmp_steering =
    planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature);

  AckermannLateralCommand cmd;
  cmd.stamp = clock_->now();

  // 限制方向盘转角范围，转换成float数据类型
  cmd.steering_tire_angle = static_cast<float>(
    std::min(std::max(tmp_steering, -param_.max_steering_angle), param_.max_steering_angle));

  // pub_ctrl_cmd_->publish(cmd);
  return cmd;
}

void PurePursuitLateralController::publishDebugMarker() const
{
  visualization_msgs::msg::MarkerArray marker_array;

  marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target));
  marker_array.markers.push_back(
    createTrajectoryCircleMarker(debug_data_.next_target, current_odometry_.pose.pose));
}

boost::optional<PpOutput> PurePursuitLateralController::calcTargetCurvature(
  bool is_control_output, geometry_msgs::msg::Pose pose)
{
  // Ignore invalid trajectory
  if (trajectory_resampled_->points.size() < 3) {
    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "received path size is < 3, ignored");
    return {};
  }

  // Calculate target point for velocity/acceleration

  // 计算规划轨迹中距离自车最近的点的index
  const auto closest_idx_result =
    motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4);
  if (!closest_idx_result) {
    RCLCPP_ERROR(logger_, "cannot find closest waypoint");
    return {};
  }

  const double target_vel =
    trajectory_resampled_->points.at(*closest_idx_result).longitudinal_velocity_mps;

  // calculate the lateral error

  const double lateral_error =
    motion_utils::calcLateralOffset(trajectory_resampled_->points, pose.position);

  // calculate the current curvature

  const double current_curvature = calcCurvature(*closest_idx_result);

  // Calculate lookahead distance

  const bool is_reverse = (target_vel < 0);
  const double min_lookahead_distance =
    is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance;
  double lookahead_distance = min_lookahead_distance;
  if (is_control_output) {
    // 根据横向误差、轨迹曲率、车速计算预瞄距离
    lookahead_distance = calcLookaheadDistance(
      lateral_error, current_curvature, current_odometry_.twist.twist.linear.x,
      min_lookahead_distance, is_control_output);
  } else {
    lookahead_distance = calcLookaheadDistance(
      lateral_error, current_curvature, target_vel, min_lookahead_distance, is_control_output);
  }

  // Set PurePursuit data
  pure_pursuit_->setCurrentPose(pose);
  pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_resampled_));
  pure_pursuit_->setLookaheadDistance(lookahead_distance);

  // Run PurePursuit
  // 计算纯跟踪算法的曲率，也就是半径
  const auto pure_pursuit_result = pure_pursuit_->run();
  if (!pure_pursuit_result.first) {
    return {};
  }

  const auto kappa = pure_pursuit_result.second;

  // Set debug data
  if (is_control_output) {
    debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget();
  }
  PpOutput output{};
  output.curvature = kappa;
  if (!is_control_output) {
    output.velocity = current_odometry_.twist.twist.linear.x;
  } else {
    output.velocity = target_vel;
  }

  return output;
}
}  // namespace pure_pursuit

  PurePursuitLateralController类成员函数众多，接下来详细介绍里面几个关键的函数。

LateralOutput PurePursuitLateralController::run(const InputData & input_data)

  此函数是入口函数，是计算纯跟踪前轮转角的外层函数，主要通过调用generateOutputControlCmd()来计算前轮转角。

AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd()

  该函数首先调用纯跟踪算法calcTargetCurvature()，计算跟踪的曲率半径；然后，调用generateCtrlCmdMsg()通过阿克曼转向几何计算得到前轮转角，也就是$\tan \delta = L/R$

boost::optional<PpOutput> PurePursuitLateralController::calcTargetCurvature(
  bool is_control_output, geometry_msgs::msg::Pose pose)

  该函数首先调用calcLookaheadDistance()计算预瞄距离

  然后，通过指针调用纯跟踪类的run()函数计算得到，车辆纯跟踪定圆的曲率半径。

const auto pure_pursuit_result = pure_pursuit_->run();

  作为纯跟踪算法里面最关键的参数，预瞄距离的计算在这里实现：

  ld = k1 * v - k2 * kappa + k3 * Err_y

  这样的计算本身没有毛病，正如前面所讲，这里各个系数的给的工程上一般通过查表得到。

double PurePursuitLateralController::calcLookaheadDistance(
  const double lateral_error, const double curvature, const double velocity, const double min_ld,
  const bool is_control_cmd)
{
  const double vel_ld = abs(param_.ld_velocity_ratio * velocity);           //  k1*v
  const double curvature_ld = -abs(param_.ld_curvature_ratio * curvature);  // -k2*kappa
  double lateral_error_ld = 0.0;

  if (abs(lateral_error) >= param_.long_ld_lateral_error_threshold) {
    // If lateral error is higher than threshold, we should make ld larger to prevent entering the
    // road with high heading error.
    // 如果横向误差高于阈值，我们应该使ld更大，以防止进入具有高航向误差的道路。
    lateral_error_ld = abs(param_.ld_lateral_error_ratio * lateral_error);  // k3*Err_y
  }

  // 总的预瞄距离，限幅
  const double total_ld =
    std::clamp(vel_ld + curvature_ld + lateral_error_ld, min_ld, param_.max_lookahead_distance);
    
  return total_ld;
}

7.3 pure_pursuit.cpp

#include "pure_pursuit/pure_pursuit.hpp"

#include "pure_pursuit/util/planning_utils.hpp"

#include <limits>
#include <memory>
#include <utility>
#include <vector>

namespace pure_pursuit
{
bool PurePursuit::isDataReady()
{
  if (!curr_wps_ptr_) {
    return false;
  }
  if (!curr_pose_ptr_) {
    return false;
  }
  return true;
}

std::pair<bool, double> PurePursuit::run()
{
  if (!isDataReady()) {
    return std::make_pair(false, std::numeric_limits<double>::quiet_NaN());
  }

  // 找到规划的轨迹点中距离自车最近的点的index
  auto closest_pair = planning_utils::findClosestIdxWithDistAngThr(
    *curr_wps_ptr_, *curr_pose_ptr_, closest_thr_dist_, closest_thr_ang_);

  // 规划轨迹中未能找过最近的点
  if (!closest_pair.first) {
    RCLCPP_WARN(
      logger, "cannot find, curr_bool: %d, closest_idx: %d", closest_pair.first,
      closest_pair.second);
    return std::make_pair(false, std::numeric_limits<double>::quiet_NaN());
  }

  // 找到预瞄点的index
  int32_t next_wp_idx = findNextPointIdx(closest_pair.second);

  // 没有找到预瞄点
  if (next_wp_idx == -1) {
    RCLCPP_WARN(logger, "lost next waypoint");
    return std::make_pair(false, std::numeric_limits<double>::quiet_NaN());
  }

  loc_next_wp_ = curr_wps_ptr_->at(next_wp_idx).position;

  geometry_msgs::msg::Point next_tgt_pos;
  // if next waypoint is first
  if (next_wp_idx == 0) {
    next_tgt_pos = curr_wps_ptr_->at(next_wp_idx).position;
  } else {
    // linear interpolation
    std::pair<bool, geometry_msgs::msg::Point> lerp_pair = lerpNextTarget(next_wp_idx);

    // 插值没有找到预瞄点
    if (!lerp_pair.first) {
      RCLCPP_WARN(logger, "lost target! ");
      return std::make_pair(false, std::numeric_limits<double>::quiet_NaN());
    }

    next_tgt_pos = lerp_pair.second;
  }
  loc_next_tgt_ = next_tgt_pos;

  // 计算曲率1/R，也就是纯跟踪算法中自车转过的定圆
  double kappa = planning_utils::calcCurvature(next_tgt_pos, *curr_pose_ptr_);

  return std::make_pair(true, kappa);
}

// linear interpolation of next target
std::pair<bool, geometry_msgs::msg::Point> PurePursuit::lerpNextTarget(int32_t next_wp_idx)
{
  constexpr double ERROR2 = 1e-5;  // 0.00001
  const geometry_msgs::msg::Point & vec_end = curr_wps_ptr_->at(next_wp_idx).position;
  const geometry_msgs::msg::Point & vec_start = curr_wps_ptr_->at(next_wp_idx - 1).position;
  const geometry_msgs::msg::Pose & curr_pose = *curr_pose_ptr_;

  Eigen::Vector3d vec_a((vec_end.x - vec_start.x), 
                        (vec_end.y - vec_start.y), 
                        (vec_end.z - vec_start.z));

  // 计算三维向量的范数（即向量的长度），记录debug信息
  if (vec_a.norm() < ERROR2) {
    RCLCPP_ERROR(logger, "waypoint interval is almost 0");
    return std::make_pair(false, geometry_msgs::msg::Point());
  }

  // 根据给定的线段和点的坐标，计算点到线的距离
  const double lateral_error =
    planning_utils::calcLateralError2D(vec_start, vec_end, curr_pose.position);

  // 横向误差大于预瞄距离，记录debug信息
  if (fabs(lateral_error) > lookahead_distance_) {
    RCLCPP_ERROR(logger, "lateral error is larger than lookahead distance");
    RCLCPP_ERROR(
      logger, "lateral error: %lf, lookahead distance: %lf", lateral_error, lookahead_distance_);
    return std::make_pair(false, geometry_msgs::msg::Point());
  }

  /* calculate the position of the foot of a perpendicular line */
  // 根据横向距离计算垂直于路径线的垂足位置，并返回该位置作为插值结果。
  Eigen::Vector2d uva2d(vec_a.x(), vec_a.y());
  uva2d.normalize();  // 归一化，只保留方向信息

  Eigen::Rotation2Dd rot =(lateral_error > 0) ? Eigen::Rotation2Dd(-M_PI / 2.0) : Eigen::Rotation2Dd(M_PI / 2.0);
  Eigen::Vector2d uva2d_rot = rot * uva2d;

  geometry_msgs::msg::Point h;
  h.x = curr_pose.position.x + fabs(lateral_error) * uva2d_rot.x();
  h.y = curr_pose.position.y + fabs(lateral_error) * uva2d_rot.y();
  h.z = curr_pose.position.z;

  // if there is a intersection
  if (fabs(fabs(lateral_error) - lookahead_distance_) < ERROR2) {
    // 横向误差与预瞄距离之差的绝对值小于ERROR2，则表示垂足点就是目标点
    return std::make_pair(true, h);
  } else {
    // if there are two intersection, get intersection in front of vehicle
    const double s = sqrt(pow(lookahead_distance_, 2) - pow(lateral_error, 2));

    geometry_msgs::msg::Point res;
    res.x = h.x + s * uva2d.x();
    res.y = h.y + s * uva2d.y();
    res.z = curr_pose.position.z;
    
    return std::make_pair(true, res);
  }
}

// 找到预瞄点的index
int32_t PurePursuit::findNextPointIdx(int32_t search_start_idx)
{
  // if waypoints are not given, do nothing.
  if (curr_wps_ptr_->empty() || search_start_idx == -1) {
    return -1;
  }

  // look for the next waypoint.
  for (int32_t i = search_start_idx; i < (int32_t)curr_wps_ptr_->size(); i++) {
    // if search waypoint is the last
    if (i == ((int32_t)curr_wps_ptr_->size() - 1)) {
      return i;
    }

    const auto gld = planning_utils::getLaneDirection(*curr_wps_ptr_, 0.05);
    // if waypoint direction is forward
    if (gld == 0) {
      // if waypoint is not in front of ego, skip
      auto ret = planning_utils::transformToRelativeCoordinate2D(
        curr_wps_ptr_->at(i).position, *curr_pose_ptr_);
      if (ret.x < 0) {
        continue;
      }
    } else if (gld == 1) {
      // waypoint direction is backward
      // if waypoint is in front of ego, skip
      auto ret = planning_utils::transformToRelativeCoordinate2D(
        curr_wps_ptr_->at(i).position, *curr_pose_ptr_);
      if (ret.x > 0) {
        continue;
      }
    } else {
      return -1;
    }

    const geometry_msgs::msg::Point & curr_motion_point = curr_wps_ptr_->at(i).position; // 当前规划的轨迹点i
    const geometry_msgs::msg::Point & curr_pose_point = curr_pose_ptr_->position;        // 当前自车位置
    // if there exists an effective waypoint
    const double ds = planning_utils::calcDistSquared2D(curr_motion_point, curr_pose_point); // 计算自车与规划轨迹点i的欧氏距离
    // 找到 规划轨迹 中到 自车当前位置 的距离大于预瞄距离的点的index
    if (ds > std::pow(lookahead_distance_, 2)) {
      return i;
    }
  }

  // if this program reaches here , it means we lost the waypoint!
  return -1;
}

void PurePursuit::setCurrentPose(const geometry_msgs::msg::Pose & msg)
{
  curr_pose_ptr_ = std::make_shared<geometry_msgs::msg::Pose>();
  *curr_pose_ptr_ = msg;
}

void PurePursuit::setWaypoints(const std::vector<geometry_msgs::msg::Pose> & msg)
{
  curr_wps_ptr_ = std::make_shared<std::vector<geometry_msgs::msg::Pose>>();
  *curr_wps_ptr_ = msg;
}

}  // namespace pure_pursuit

7.4 planning_utils.cpp

#include "pure_pursuit/util/planning_utils.hpp"

#include <limits>
#include <utility>
#include <vector>

namespace pure_pursuit
{
namespace planning_utils
{
double calcArcLengthFromWayPoint(
  const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx,
  const size_t dst_idx)
{
  double length = 0;
  for (size_t i = src_idx; i < dst_idx; ++i) {
    const double dx_wp =
      input_path.points.at(i + 1).pose.position.x - input_path.points.at(i).pose.position.x;
    const double dy_wp =
      input_path.points.at(i + 1).pose.position.y - input_path.points.at(i).pose.position.y;
    length += std::hypot(dx_wp, dy_wp);
  }
  return length;
}

double calcCurvature(
  const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose)
{
  constexpr double KAPPA_MAX = 1e9;
  const double radius = calcRadius(target, current_pose);

  if (fabs(radius) > 0) {
    return 1 / radius;
  } else {
    return KAPPA_MAX;
  }
}

double calcDistance2D(const geometry_msgs::msg::Point & p, const geometry_msgs::msg::Point & q)
{
  const double dx = p.x - q.x;
  const double dy = p.y - q.y;
  return sqrt(dx * dx + dy * dy);
}

double calcDistSquared2D(const geometry_msgs::msg::Point & p, const geometry_msgs::msg::Point & q)
{
  const double dx = p.x - q.x;
  const double dy = p.y - q.y;
  return dx * dx + dy * dy;
}

/* a_vec = line_e - line_s, b_vec = point - line_s
 * a_vec x b_vec = |a_vec| * |b_vec| * sin(theta)
 *               = |a_vec| * lateral_error ( because, lateral_error = |b_vec| * sin(theta) )
 *
 * lateral_error = a_vec x b_vec / |a_vec|
 *        = (a_x * b_y - a_y * b_x) / |a_vec| */
double calcLateralError2D(
  const geometry_msgs::msg::Point & line_s, const geometry_msgs::msg::Point & line_e,
  const geometry_msgs::msg::Point & point)
{
  // 根据给定的线段和点的坐标，计算点到线的距离
  tf2::Vector3 a_vec((line_e.x - line_s.x), (line_e.y - line_s.y), 0.0);
  tf2::Vector3 b_vec((point.x - line_s.x), (point.y - line_s.y), 0.0);

  double lat_err = (a_vec.length() > 0) ? a_vec.cross(b_vec).z() / a_vec.length() : 0.0;
  return lat_err;
}

double calcRadius(
  //计算下一路径点与汽车当前位置之间的圆弧曲率 这里利用的相对坐标和圆里面的直角三角形的相似来求得圆的半径，最后得到圆弧曲率
  const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose)
{
  constexpr double RADIUS_MAX = 1e9;
  const double denominator = 2 * transformToRelativeCoordinate2D(target, current_pose).y;
  const double numerator = calcDistSquared2D(target, current_pose.position);

  if (fabs(denominator) > 0) {
    return numerator / denominator;  // R = L^2 / 2y
  } else {
    return RADIUS_MAX;
  }
}

double convertCurvatureToSteeringAngle(double wheel_base, double kappa)
{
  return atan(wheel_base * kappa);
}

std::vector<geometry_msgs::msg::Pose> extractPoses(
  const autoware_auto_planning_msgs::msg::Trajectory & trajectory)
{
  std::vector<geometry_msgs::msg::Pose> poses;

  for (const auto & p : trajectory.points) {
    poses.push_back(p.pose);
  }

  return poses;
}

// get closest point index from current pose
// 找到规划的轨迹点中距离自车最近的点的index
std::pair<bool, int32_t> findClosestIdxWithDistAngThr(
  const std::vector<geometry_msgs::msg::Pose> & poses,
  const geometry_msgs::msg::Pose & current_pose, double th_dist, double th_yaw)
{
  double dist_squared_min = std::numeric_limits<double>::max();
  int32_t idx_min = -1;

  for (size_t i = 0; i < poses.size(); ++i) {
    // 计算轨迹点到自车的欧氏距离
    const double ds = calcDistSquared2D(poses.at(i).position, current_pose.position);
    if (ds > th_dist * th_dist) {
      // 距离不得超过阈值
      continue;
    }

    const double yaw_pose = tf2::getYaw(current_pose.orientation);
    const double yaw_ps = tf2::getYaw(poses.at(i).orientation);

    // 航向角误差统一到[-pi pi]
    const double yaw_diff = normalizeEulerAngle(yaw_pose - yaw_ps);
    if (fabs(yaw_diff) > th_yaw) {
      // 航向角误差不得超过阈值
      continue;
    }

    if (ds < dist_squared_min) {
      dist_squared_min = ds;
      idx_min = i;
    }
  }

  return (idx_min >= 0) ? std::make_pair(true, idx_min) : std::make_pair(false, idx_min);
}

int8_t getLaneDirection(const std::vector<geometry_msgs::msg::Pose> & poses, double th_dist)
{
  if (poses.size() < 2) {
    RCLCPP_ERROR(rclcpp::get_logger(PLANNING_UTILS_LOGGER), "size of waypoints is smaller than 2");
    return 2;
  }

  for (uint32_t i = 0; i < poses.size(); i++) {
    geometry_msgs::msg::Pose prev;
    geometry_msgs::msg::Pose next;

    if (i == (poses.size() - 1)) {
      prev = poses.at(i - 1);
      next = poses.at(i);
    } else {
      prev = poses.at(i);
      next = poses.at(i + 1);
    }

    if (planning_utils::calcDistSquared2D(prev.position, next.position) > th_dist * th_dist) {
      // 转换成相对坐标，判断x大于0，则路径方向为正
      const auto rel_p = transformToRelativeCoordinate2D(next.position, prev);
      return (rel_p.x > 0.0) ? 0 : 1;
    }
  }

  RCLCPP_ERROR(rclcpp::get_logger(PLANNING_UTILS_LOGGER), "lane is something wrong");
  return 2;
}

bool isDirectionForward(
  const geometry_msgs::msg::Pose & prev, const geometry_msgs::msg::Pose & next)
{
  return (transformToRelativeCoordinate2D(next.position, prev).x > 0.0) ? true : false;
}

bool isDirectionForward(
  const geometry_msgs::msg::Pose & prev, const geometry_msgs::msg::Point & next)
{
  return transformToRelativeCoordinate2D(next, prev).x > 0.0;
}

template <>
bool isInPolygon(
  const std::vector<geometry_msgs::msg::Point> & polygon, const geometry_msgs::msg::Point & point)
{
  std::vector<tf2::Vector3> polygon_conv;
  for (const auto & el : polygon) {
    polygon_conv.emplace_back(el.x, el.y, el.z);
  }

  tf2::Vector3 point_conv = tf2::Vector3(point.x, point.y, point.z);

  return isInPolygon<tf2::Vector3>(polygon_conv, point_conv);
}

double kmph2mps(const double velocity_kmph)
{
  return (velocity_kmph * 1000) / (60 * 60);
}

double normalizeEulerAngle(const double euler)
{
  double res = euler;
  while (res > M_PI) {
    res -= (2 * M_PI);
  }
  while (res < -M_PI) {
    res += 2 * M_PI;
  }

  return res;
}

// ref: http://www.mech.tohoku-gakuin.ac.jp/rde/contents/course/robotics/coordtrans.html
// (pu, pv): relative, (px, py): absolute, (ox, oy): origin
// (px, py) = rot * (pu, pv) + (ox, oy)
geometry_msgs::msg::Point transformToAbsoluteCoordinate2D(
  const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin)
{
  // rotation
  geometry_msgs::msg::Point rot_p;
  double yaw = tf2::getYaw(origin.orientation);
  rot_p.x = (cos(yaw) * point.x) + ((-1) * sin(yaw) * point.y);
  rot_p.y = (sin(yaw) * point.x) + (cos(yaw) * point.y);

  // translation
  geometry_msgs::msg::Point res;
  res.x = rot_p.x + origin.position.x;
  res.y = rot_p.y + origin.position.y;
  res.z = origin.position.z;

  return res;
}

// ref: http://www.mech.tohoku-gakuin.ac.jp/rde/contents/course/robotics/coordtrans.html
// (pu, pv): relative, (px, py): absolute, (ox, oy): origin
// (pu, pv) = rot^-1 * {(px, py) - (ox, oy)}
geometry_msgs::msg::Point transformToRelativeCoordinate2D(
  const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & origin)
{
  // translation
  geometry_msgs::msg::Point trans_p;
  trans_p.x = point.x - origin.position.x;
  trans_p.y = point.y - origin.position.y;

  // rotation (use inverse matrix of rotation)
  double yaw = tf2::getYaw(origin.orientation);

  geometry_msgs::msg::Point res;
  res.x = (cos(yaw) * trans_p.x) + (sin(yaw) * trans_p.y);
  res.y = ((-1) * sin(yaw) * trans_p.x) + (cos(yaw) * trans_p.y);
  res.z = origin.position.z;

  return res;
}

geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double _yaw)
{
  tf2::Quaternion q;
  q.setRPY(0, 0, _yaw);
  return tf2::toMsg(q);
}

}  // namespace planning_utils
}  // namespace pure_pursuit

八、总结

  纯跟踪算法是假设车辆做定圆运动，其本质是 $P$ 控制，这也就意味着算法的上限不会很高。但通过一系列优化方法，在实际工程化应用中还是可以达到很好的效果。

参考资料

  1、自动驾驶规划控制

  2、自动驾驶控制算法——纯跟踪算法（Pure Pursuit）

后记：

🌟 感谢您耐心阅读这篇关于 Pure Pursuit 纯跟踪算法详解与编程实现 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡

🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀

# 人工智能 # 自动驾驶
Logo
松山湖开发者村综合服务平台

助力广东及东莞地区开发者，代码托管、在线学习与竞赛、技术交流与分享、资源共享、职业发展，成为松山湖开发者首选的工作与学习平台

更多推荐

cover

地图讲故事之GIS遇到历史-中国十大古都篇

avatar 松山湖开发者村综合服务平台
cover

[C高手编程] Socket编程、套接字选项与多路复用：深入探索网络通信

avatar 松山湖开发者村综合服务平台
cover

探索 Spring Boot 中 Elasticsearch 的实战应用

avatar 松山湖开发者村综合服务平台