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

Wai-Ngai

已于 2024-09-09 13:49:22 修改

阅读量1.1w

点赞数 43

分类专栏：自动驾驶控制算法文章标签：算法自动驾驶 c++ 人工智能

于 2023-08-02 14:53:27 首次发布

本文链接：https://blog.csdn.net/WaiNgai1999/article/details/132062188

版权

自动驾驶控制算法专栏收录该内容

2 篇文章 10 订阅

订阅专栏

目前主流的轨迹跟踪方法有两类：基于几何模型的跟踪方法和基于动力学模型的跟踪方法。

基于几何模型的跟踪方法：

Pure Pursuit
Stanley

基于动力学模型的跟踪方法:

0.文章目录

1. 算法思想

我觉得纯跟踪算法就是模拟人开车时候的想法的，人在开车的时候想要跟踪一条期望的轨迹，那么应该怎样思考呢？（假如速度恒定的话），很自然而然的会想到，首先确定一个距离本车有一定距离的一个点，然后通过控制方向盘来控制车辆到达那个点，然后再使用同样的方式到达下一个点，当所有的点都跟踪上之后，也就完成了轨迹跟踪。

所以，我们可以借鉴人类驾驶习惯，在参考轨迹上选取离车辆当前位置一定距离的点作为预瞄点，通过调整方向盘转角（前轮转角），控制车辆沿圆轨迹驶向预瞄点。

算法核心思想：基于当前车辆后轮中心位置，在参考路径path上向 $l_d$ （自定义）的距离匹配一个预瞄点C，假设车辆后轮中心点A可以按照一定的转弯半径𝑅 行驶抵达该预瞄点（也就是沿着 定圆运动），然后根据预瞄距离 $l_d$ ，转弯半径𝑅，车辆坐标系下预瞄点的朝向角𝛼 之间的几何关系来确定前轮转角 $\delta{{}}{}_{{f}}$ 。如下图所示：

Alt

图中，

大地坐标系	X 0 Y
车身坐标系	x 0 y
全局路径下的规划轨迹	path
前轮转角	$\delta{{}}{}_{{f}}$
航向角	$\varphi$
预瞄点与车身的夹角	$\alpha$
转弯半径	R
预瞄距离	$l_d$
轴距	L
预瞄点C	$\left({{x}}{}_{{r}},{{y}}{}_{{r}}\right)$
自车后轴中心A	$\left({{x}}{}_{{h}},{{y}}{}_{{h}}\right)$

因为 $\bigtriangleup AOC$ 是等腰三角形，在 $\bigtriangleup AOB$ 中， $AB\bot OA$ ，则 $\angle AOC$ ：
$\angle AOC=\:\pi-2\angle CAO\:=\:\pi-2\left(\frac{\pi}{2}-\alpha\right)=2\alpha$

为了使车辆后轮跟踪圆弧虚线轨迹到达C点，在 $\bigtriangleup AOC$ 中需要满足的正弦定理关系为：
$\frac{{{l}}{}_{{d}}}{\sin2\alpha}=\:\frac{R}{\sin\left(\frac{\pi}{2}-\alpha\right)}$

化简得到：
$R\:=\:\frac{{{l}}{}_{{d}}}{2\sin\alpha}$

再看为了达到这种关系，作为控制变量的前轮转角需要满足的关系。在阿克曼转向（ Ackermann turn） $\bigtriangleup AOB$ 中：
$tan{{\delta}}{}_{{f}}=\:\frac{L}{R}$

因此，联立上面两式，可以得到纯追踪算法的控制量 $\delta{{}}{}_{{f}}$ 的最终表达式：
${{\delta}}{}_{{f}}\left(t\right)=\:\arctan\left(\frac{2L\sin\left.\alpha\left(t\right.\right)}{{{l}}{}_{{d}}}\right)$

上式：
L：是车身轴距，为已知
$\alpha$ ：预瞄点与车身的夹角， $\alpha=\angle CAD-\angle BAD\:=\:arctan\frac{\left({{y}}{}_{{r}}-{{y}}{}_{{h}}\right)}{\left(x{}_{{r}}-{{x}}{}_{{h}}\right)}\:-\:\varphi$
$l_d$ ：预瞄距离， $l_d\:=kv+l_0\:$

上述，预瞄距离 $l_d$ ，由人为给定，通常认为和速度相关，这也符合人类开车的习惯：车速很快的时候，就需要尽可能的看得远些；反之，车速比较慢时，需要看得比较近些。一种最常用的方法就是把前视距离表示为车辆纵向速度的线性函数，即 $l_d\:=kv+l_0\:$ ，将 $l_d$ 带入上面的控制律，得到

${{\delta}}{}_{{f}}\left(t\right)=\:\arctan\left(\frac{2L\sin\left.\alpha\left(t\right.\right)}{{{kv+l}}{}_0}\right)$

将前视距离设置为速度的函数只是一种常见的方法，前视距离也有可能和其他因素有关，所以纯追踪调参的难点就在前视距离的选取上，很短的前视距离会造成车辆控制的不稳定甚至震荡，为了确保车辆稳定设置较长的前视距离又会出现车辆在大转角处转向不足的问题。

Alt
为了更深的理解纯追踪控制器的原理，定义横向误差为预瞄点到车辆的距离：
${{e}}{}_{{y}}={{l}}{}_{{d}}\:\sin\alpha$

联立上式和前轮转角控制公式，并考虑小角度的情况：
${{e}}{}_{{y}}=\frac{{{l}}_{{d}}^{{2}}}{2L}\tan{{\delta}}{}_{{f}}\thickapprox\frac{{{l}}_{{d}}^{{2}}}{2L}\bullet{{\delta}}{}_{{f}}$

由此，可以看出纯跟踪算法近似于一个P控制器， $P=\frac{2L}{{{l}}_{{d}}^{{2}}}$
跟踪效果由预瞄距离 $l_d$ 决定。

2. 算法优缺点

优点：

对外界的鲁棒性较好

缺点：

预瞄点位置靠前于车辆位置，其横向误差为零不能保证车辆位置横向误差为零；
纯跟踪算法可以看做是，以车辆后轴中心相对于预瞄点的横向位置误差比例控制器，而比例控制器决定了控制品质上限不高；
纯跟踪算法可以缩小车辆与期望轨迹的位置偏差，但是对角度偏差束手无策，因为数学原理上根本就没有考虑角度偏差；
要求轨迹多帧连续性好，因预瞄的特性无法对变化轨迹（尤其是预瞄距离内）进行响应；
要求轨迹性能稳定，因为标定系数是按照轨迹性能进行标定的，如果轨迹性能变化，可能导致车辆转弯内切等现象；

适用场景：低速场景(速度过高易产生转弯内切以及超调)

3. 纯跟踪算法优化

3.1 预瞄距离 $l_d$

预瞄距离和弯道曲率以及车速相关。我理解这就像人开车一样，车速越快，我们人眼眺望的距离就远；弯道越急，人眼眺望的距离更近。
预瞄距离 $l_d$ 的计算与当前车辆速度v，曲率kappa存在下面的函数关系：
$l_{d} =f(v,kappa)$

速度越快，预瞄距离越远，可以认为，预瞄距离与速度成正比。
如果前方跟踪曲率较大说明为弯道，需要减少预瞄距离满足弯道行驶的跟踪精度；曲率较小，可以增加预瞄距离保证行驶的平稳性。

3.2 预瞄点

4. 代码实现

在自动驾驶开源代码autoware中，详见autoware.universe/tree/main/control/pure_pursuit，纯跟踪算法Pure Pursuit的实现还是比较简单的。当然，其距离工程化应用还是有一定差距，但是对于算法的学习已经足够了，下面就来详细讲解一下Autoware的纯跟踪算法实现。

下面是整个纯跟踪算法的目录树，可以看到并不复杂。其中最重要的几个文件是：pure_pursuit.param.yaml、pure_pursuit_lateral_controller.cpp、pure_pursuit.cpp、planning_utils.cpp。看懂这几个文件，那么整个纯跟踪算法就差不多掌握了，这也是学习autoware纯跟踪代码的一个顺序。
Alt

4.1 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

4.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类成员函数众多，接下来详细介绍里面几个关键的函数。

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

LateralOutput PurePursuitLateralController::run(const InputData & input_data)

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

AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd()

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

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

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

const auto pure_pursuit_result = pure_pursuit_->run();

4.作为纯跟踪算法里面最关键的参数，没有之一！！预瞄距离的计算在这里实现：

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

4.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

这里有三点需要详细说明一下：

1. PurePursuit类主要的算法逻辑是 std::pair<bool, double> PurePursuit::run() 函数
通过该函数计算出纯跟踪算法最核心的控制量----曲率kappa，这里的曲率也就是1/R，是纯跟踪定圆的圆弧曲率。至于为啥不是前轮转角或者方向盘转角？因为在PurePursuitLateralController::generateCtrlCmdMsg()才会计算出转角。

另外本函数也是在PurePursuitLateralController类中通过PurePursuitLateralController::calcTargetCurvatur()函数调用的：

const auto pure_pursuit_result = pure_pursuit_->run();

run()函数主要包含几步：

首先检查数据是否准备就绪，如果不是，则返回一个包含false值和NaN（不是一个数字）的std::pair对象。
接下来，函数调用planning_utils::findClosestIdxWithDistAngThr()函数，找到规划轨迹中距离当前车辆位置最近的点的索引。如果未能找到最近的点，则返回一个包含false值和NaN的std::pair对象。
然后，函数调用findNextPointIdx()函数，找到预瞄点（目标点）的索引。如果未能找到预瞄点，则返回一个包含false值和NaN的std::pair对象。
接下来，函数根据预瞄点的索引获取其位置信息，并将其赋值给loc_next_tgt_变量。
最后，函数调用planning_utils::calcCurvature()函数，计算自车转过的定圆的曲率（1/R），并将其赋值给kappa变量。
最终，函数返回一个包含true值和计算得到的曲率值的std::pair对象。

2. 预瞄点的计算有两步：

通过int32_t PurePursuit::findNextPointIdx(int32_t search_start_idx) 函数，计算规划轨迹中与自车距离大于预瞄距离的点的index，C点；
由于规划的轨迹是离散点，上面得到的index点有可能并不是真正的预瞄点，所以通过std::pair<bool, geometry_msgs::msg::Point> PurePursuit::lerpNextTarget(int32_t next_wp_idx)函数，插值计算真正预瞄点（CD之间那个紫色点）的坐标（大地坐标系）。

插值函数的计算原理，可以通过下图来理解：
假设预瞄距离 $l_d$ ，红色离散点C、D、E是规划轨迹中的三个点，其中C点是函数findNextPointIdx计算的该index对应的点，D点是离散轨迹中index-1那个点，E是index-2那个点。

我们的目标是计算预瞄点的坐标，首先通过向量计算得到A点到投影点H的长度 AH ，从而计算得到H点的坐标，接着判断横向误差与预瞄距离的关系。通常情况下横向误差都是小于预瞄距离的，所以可以通过H点的坐标和预瞄距离 $l_d$ 计算预瞄点的坐标。具体见代码实现，结合下图更容易理解：
Alt

3. 横向误差大于预瞄距离，记录debug信息，并且直接返回计算失败

在上面lerpNextTarget()函数中，有一步是得到H点后判断横向误差与预瞄距离的大小。

如果横向误差超过了预瞄距离，说明车辆已经偏离了路径太远，此时继续追踪下一个目标点可能会导致车辆无法及时回到路径上，或者产生过大的偏差。为了确保车辆能够稳定地跟踪路径，代码选择返回错误，以便采取相应的纠正措施，比如调整车辆的速度或转向角度。

这样做的目的是为了防止车辆失控并保证行驶安全。纯跟踪算法是一种基本的路径跟踪算法，其设计初衷是帮助车辆稳定地沿着预定的路径行驶，而不是超出车辆的控制范围。因此，在横向误差大于预瞄距离时，返回错误可以帮助避免潜在的危险情况。

横向误差大于预瞄距离可能发生在以下情况下：

车辆偏离了路径：当车辆偏离了预定的路径时，车辆当前位置与路径之间的横向距离就会增大。如果车辆偏离路径太远，横向误差可能会超过预瞄距离。
预瞄距离过小：如果设置的预瞄距离比路径曲率或速度要求所需要的更小，那么即使车辆没有明显偏离路径，横向误差也可能超过预瞄距离。
路径弯曲或变化较大：当路径发生急转弯或变化较大时，车辆可能无法及时调整自身的位置，导致横向误差超过预瞄距离。
控制系统响应滞后：如果车辆的控制系统响应滞后，即使车辆已经偏离路径，控制系统可能无法及时纠正，从而导致横向误差超过预瞄距离。

需要注意的是，确切的横向误差和预瞄距离的关系取决于具体的算法实现和应用场景。不同的算法和车辆控制系统可能会有不同的判断条件和阈值来确定横向误差是否大于预瞄距离。因此，在具体应用中，需要根据实际情况进行参数调整和验证。

4.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

planning_utils.cpp是一个十分重要的文件，里面包含纯跟踪算法关键逻辑的实现，包括计算欧式距离、点到线段的距离、坐标转换等。下面详细介绍里面的一个函数double calcRadius()，这是计算曲率的关键，至于其他的函数，详见代码，原理都比较简单。

Alt
首先通过坐标变换，可以将预瞄点C的坐标从大地坐标X0Y转换到车身坐标系x0y下，得的C点的坐标为（a,b）;
然后通过圆内三角形相似原理，计算圆的半径R：
$a^{2} +b^{2} =l_{d} ^{2}$ $R-a)^{2} +b^{2} =R ^{2}$
联立上述2式，化简得到：
$l_{d} ^{2} =2R\times a$
由此得到转弯半径
$R=\frac{l_{d} ^{2}}{2a}$

具体见代码实现：

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

5. 总结

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

Wai-Ngai

关注

43
点赞
踩
209

收藏

觉得还不错? 一键收藏
13
评论
自动驾驶控制算法——纯跟踪算法（Pure Pursuit）

把预瞄距离变大，那么纯追踪控制器就会表现得更加“平滑”，更加平滑的结果就是在某些急剧的转角处会存在转向不足的情况。把预瞄距离变小，那么车辆控制会不稳定甚至震荡，为了确保车辆稳定而设置较长的前视距离又会出现车辆在大转角处转向不足的问题。
复制链接

扫一扫