【自动驾驶】基于车辆几何模型的横向控制算法 | Pure Pursuit 纯跟踪算法详解与编程实现

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

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

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

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

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

---

---

引言

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

---

一、纯跟踪算法简介

目前主流方法有两类：

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

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

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

---

二、自行车模型概述

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

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

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

---

三、纯跟踪算法理论基础

3.1 算法基本思想

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

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

3.2 几何关系推导

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

  由于知道目标点和车身后轮方向，即目标点方向和当前航向角的夹角为 $\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 )}=2R$$这样得到 $(1)$ 式
$$\frac{1}{R}=\frac{2\sin (\alpha )}{l_d}\text{\hspace{1em}(1)}$$建立了转弯半径与目标点方向和当前航向角的夹角之间的关系。

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

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

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

---

四、纯跟踪算法实现步骤

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

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

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

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

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

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

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

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

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

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

---

五、纯跟踪算法优化

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

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

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

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

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

5.2 根据速度缩放前视距离

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

5.3 比例系数的影响及调整

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

$$\delta (t)={\tan }^{-1}(\frac{2L\sin (\alpha (t))}{l_d})={\tan }^{-1}(\frac{2L\sin (\alpha (t))}{k{v}_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 的纯跟踪算法实现。

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

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

• 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.4ld_lateral_error_ratio: 3.6ld_curvature_ratio: 120.0long_ld_lateral_error_threshold: 0.5min_lookahead_distance: 4.35max_lookahead_distance: 15.0converged_steer_rad: 0.1reverse_min_lookahead_distance: 7.0prediction_ds: 0.3prediction_distance_length: 21.0resampling_ds: 0.1curvature_calculation_distance: 4.0enable_path_smoothing: falsepath_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
};
}  // namespacenamespace 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 Parametersconst 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 Parametersparam_.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 Publisherspub_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 trajectorypub_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*vconst double curvature_ld = -abs(param_.ld_curvature_ratio * curvature);  // -k2*kappadouble 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 curvatureconst 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 curvaturesize_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 trajectoryreturn 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 trajectoryreturn 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 constantdouble 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 pointpredicted_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 calculationconst 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/Rconst 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 trajectoryif (trajectory_resampled_->points.size() < 3) {RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "received path size is < 3, ignored");return {};}// Calculate target point for velocity/acceleration// 计算规划轨迹中距离自车最近的点的indexconst 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 errorconst double lateral_error =motion_utils::calcLateralOffset(trajectory_resampled_->points, pose.position);// calculate the current curvatureconst double current_curvature = calcCurvature(*closest_idx_result);// Calculate lookahead distanceconst 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 datapure_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 dataif (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*vconst double curvature_ld = -abs(param_.ld_curvature_ratio * curvature);  // -k2*kappadouble 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());}// 找到规划的轨迹点中距离自车最近的点的indexauto 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());}// 找到预瞄点的indexint32_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 firstif (next_wp_idx == 0) {next_tgt_pos = curr_wps_ptr_->at(next_wp_idx).position;} else {// linear interpolationstd::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.00001const 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 intersectionif (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 vehicleconst 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 lastif (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 forwardif (gld == 0) {// if waypoint is not in front of ego, skipauto 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, skipauto 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; // 当前规划的轨迹点iconst geometry_msgs::msg::Point & curr_pose_point = curr_pose_ptr_->position;        // 当前自车位置// if there exists an effective waypointconst double ds = planning_utils::calcDistSquared2D(curr_motion_point, curr_pose_point); // 计算自车与规划轨迹点i的欧氏距离// 找到 规划轨迹 中到 自车当前位置 的距离大于预瞄距离的点的indexif (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)
{// rotationgeometry_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);// translationgeometry_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)
{// translationgeometry_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 纯跟踪算法详解与编程实现 的技术博客。 📚

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

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

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

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