【自动驾驶】基于几何模型的跟踪方法——Stanley算法

本文介绍了自动驾驶中基于几何模型的路径跟踪算法：Stanley算法

m0_72429709

3077人浏览 · 2025-02-07 13:12:57

m0_72429709 · 2025-02-07 13:12:57 发布

一、Stanley算法原理

1.算法简介

Stanley算法是一种经典的路径跟踪算法，由斯坦福大学团队在2005年DARPA自动驾驶挑战赛中首次应用于无人车“Stanley”，并成功夺冠。该算法基于几何模型，通过横向误差和航向误差调整车辆转向角，实现高效稳定的路径跟踪。

2.核心思想

算法的核心是将参考点设置在前轮中心，其控制律结合了横向误差（Cross-Track Error）和航向误差（Heading Error），并引入车速反馈以提升稳定性，实现横向跟踪误差收敛到0。

3.算法原理及推导

综合三个控制要求，实现路径跟踪的转向控制：

（1）航向对齐控制

目标：为了消除相对路径的航向误差，使车辆当前航向角与期望航向角一致。

$\delta(t)=\theta_e(t)$

$\theta_e(t)$ ：当前航向角与期望航向角的误差。

假设车辆没有横向误差，即 $e=0$ 时，前轮只需转过 $\theta_e$ 的角度就可以完全追踪上参考轨迹，直接通过航向误差生成转向角，无比例增益（假设误差单位为弧度）。

（2）横向误差控制

目标：消除车辆与路径的横向偏差。

假设前轮中心为 $A$ ，取前轮中心最近的点为 $C$ ，沿速度方向的交点得到 $B$ 。这样可以看出由横向误差所产生的 $\angle A B C=\delta_e$ ，可得：

$\tan \delta_e=\frac{e}{d}$

其中， $d$ 是与速度有关的量，加入与速度相反的比例系数 $k$ ，则：

$d=\frac{v}{k}$

前轮转角误差就可写成：

$\tan \delta_e=\frac{k e}{v}$

$\delta(t)=\delta_e(t)=\tan ^{-1}\left(\frac{k e(t)}{v_f(t)}\right)$

$e(t)$ ：横向误差（车辆到路径的垂直距离）。
$v_f(t)$ ：车辆当前速度。 $k$ ：实验确定的增益系数。

特点：

比例调节：横向误差越大，转向角越大。
速度反比：高速时转向更平缓，避免失控。
限幅效果： $\tan ^{-1}$ 函数限制大误差时的转向幅度（趋近于±90°）。

（3）转向角机械限制

约束条件：
$\delta(t) \in\left[\delta_{\min }, \delta_{\max }\right]$
实际车辆转向系统存在物理限制（如±30°）。

（4）总转向角计算

综合航向误差与横向误差的转向角，并施加机械限制：

$\delta(t)=\theta_e(t)+\tan ^{-1}\left(\frac{k e(t)}{v_f(t)}\right), \delta(t) \in\left[\delta_{\min }, \delta_{\max }\right]$

二、误差对转向角的影响

1.大航向误差

当导航角误差非常大，且转向指令与导航角方向相反时：

可以看到车头朝向和参考轨迹的角度差为 $\theta_e$ ，这时前轮转角已经非常大，要让车辆转角纠正与路径的错位，就会有很大的航向误差。此时我们会让车辆保持 $\delta_{\max }$ 来修正误差

2.大横向误差

假设初始时航向误差为 0 ，由于横向误差 $e$ 很大，导致转向角很大：

$\tan ^{-1}\left(\frac{k e(t)}{v_f(t)}\right) \approx \frac{\pi}{2} \rightarrow \delta(t) \approx \theta_e(t)+\frac{\pi}{2}$

此时车辆不断地向参考轨迹靠近，即车辆会不断地转向参考路径，而这样会增加相反方向的航向误差 $\theta_e$ .

当转向角开始以最大的方向往参考轨迹转时，车就会变成下图这样，航向误差在增加，直至当航向误差超过 $-\frac{\pi}{2}$ 时，前轮转角会到达平衡状态，即航向误差和横向误差相互抵消。

此时前轮转角 $\delta(t) \approx 0$ 保持直行到参考路径，直到横向误差减小，这时才开始再次矫正与路径对齐，让车辆开始更紧密的追踪路径。

三、算法误差分析

车辆速度的分量可以分解为：

沿参考路径方向的分量： $v_f(t) \cos \left(\delta_e\right)$
垂直于参考路径方向的分量： $v_f(t) \sin \left(\delta_e\right)$

因为 $e(t)$ 是横向误差，受车辆垂直于路径方向速度的影响，因此横向误差的变化率 $\dot{e}(t)$ )等于车辆在垂直方向上的速度分量：

$\dot{e}(t)=v_f(t) \sin \left(\delta_e\right)$

从图中可以看出，当车辆的横向误差增大时，车辆是朝着负方向偏离路径的（与定义的路径方向相反）。因此需要加一个负号来表明误差的变化方向：

$\dot{e}(t)=-v_f(t) \sin \left(\delta_e\right)$

在跟踪过程中。车辆转角一般很小，可以近似为：

$\delta_e \approx \tan ^{-1}\left(\frac{e(t)}{d(t)}\right)$

带入得：

$\begin{aligned} \dot{e}(t) & =-v_f(t) \sin \frac{e(t)}{\sqrt{e^2(t)+d^2(t)}} \\ & =-v_f(t) \sin \frac{e(t)}{\sqrt{e^2(t)+\frac{v_f^2(t)}{k^2}}} \\ & =\frac{-k e(t)}{\sqrt{1+\left(\frac{k e(t)}{v f}\right)^2}} \end{aligned}$

假设横向误差相对较小，则分母部分可近似为 1 ，可以得到微分方程：

$\dot{e}(t) \approx-k e(t)$

跟踪误差的解为自然指数衰减形式，自然指数衰减的方程就是稳定的，同时根据微分方程，还可以看出当 $k$ 值增加时，衰减更快，就可以加快误差纠正。

四、Stanley 算法优化

在实际运用中，Stanley 算法仅仅是基于几何模型的轨迹追踪控制器，因此并没有考虑到实际车辆很多不同的方面，比如噪声的测试、执行器的动力学，轮胎力效应等等，而所有的这些因素都可能在跟踪轨迹过程中导致不良的特性，但可以在对 Stanley 算法做一些调整，来避免不良的问题。

1. 低速情况下的噪声软化

在低速情况下，Stanley 算法面对很大噪声时，对速度的估计会表现得非常激进，由于此处速度这一项在反正切的分母下面：

$\delta(t)=\theta_e(t)+\tan ^{-1}\left(\frac{k e(t)}{v(t)}\right)$

在低速估计中，如果有一点误差 $\Delta v$ ，会导致整个转向命令被放大，从而导致转向角度出现剧烈摆动。这一点对车辆驾驶的舒适度影响非常大，举一个非常极端的例子，假如在高速行驶（速度约为 20 m / s ）时，若速度估计中存在噪声 $\Delta v=2 m / s$ ，当然这是非常夸大的噪声。在正常高速行驶下，此噪声只占 10%，其实没什么关系。

但如果把噪声放到泊车场景中，可能实际场景的行驶速度只有 $3-5 \mathrm{~m} / \mathrm{s}$ ，这时加了这么大的噪声，可能就占 50% 以上，对转向角造成很大影响。

当然这是非常极端的例子，在现实生活中很难出现这么大的噪声，但我们就是要夸大的讲问题。为了摆脱噪声问题来提高低速的行驶稳定性，常常会加入软化常数 $k_s$ :

$\delta(t)=\theta_e(t)+\tan ^{-1}\left(\frac{k e(t)}{k_s+v_f(t)}\right)$

这样就能够确保分母始终具有最小值， $k_s$ 可根据实际实验场景调整，这样就保证了在低速行驶的稳定性。

2 .高速情况下的航向阻尼

在高速情况下，当有转向命令时，需要转向角更缓慢的变化，以确保侧向力不会过大。而即使在 Stanley 算法里已经有速度的考量，引入了速度量 $v_f$ 但 Stanley 算法在高速时反应还是有些过于激进。因此常常会在车头朝向，即

$\delta=\theta_e+\delta_e$

在航向角误差上可以加入阻尼项，实质上就是在航向角误差的控制部分加了 $PD$ 控制器，这样就可以确保在高速时，转向指令可以更缓慢、更准确得确保侧向力不会过大。

3. 引入前馈项

因为 Stanley 算法和 Pure Pursuit 算法都是根据几何模型推导的控制器，并没有考虑参考路径的曲率。所以可通过在航向上引入前馈项来改善曲线跟踪性能，一般前馈项只需要和路径曲率保持一致即可，即只需增加和路径曲率相同的转向角。

4. 综合优化

在实际应用中，横向控制可以转化为 $PD$ 控制器，加入软化常数 $k_s$ 再加上前馈控制项 $\delta_{f f}(\kappa)$

$\delta^{\prime}=P D\left[\theta_e\right]+\arctan \left(\frac{k e}{k_s+v_f}\right)+\delta_{f f}(\kappa)$

前馈控制项 $\delta_{f f}(\kappa)$ 要和实际路径曲率 $\kappa$ 直接关联，通过这些优化措施就可以使 Stanley 控制器成为更舒适、更准确的轨迹跟踪控制器。

五、与Pure Pursuit算法比较

	Pure Pursuit	Stanley
误差度量	预瞄点与车辆当前位置的几何关系	航向误差 + 横向位置误差
预瞄机制	显式使用预瞄点，依赖路径全局信息	隐式使用最近点，依赖局部路径信息
优点	高速稳定性好（预瞄距离动态调整）路径曲率适应性强（圆弧拟合）	低速精度高（直接纠偏）对横向误差响应快
缺点	预瞄距离敏感，需精细调参低速时可能跟踪延迟	高速时易震荡（需速度补偿）复杂曲率路径可能超调
适用场景	高速场景：如公路驾驶，预瞄机制可平滑转向动作大曲率路径：圆弧拟合能更好地适应连续弯道	低速场景：如停车场导航，快速纠正横向偏差精确跟踪：需要高精度路径跟踪的场景（如狭窄通道）

六、代码实现

笔者使用的软件为LGSVL Simulator ，其是 LG 的硅谷实验室基于 Unity 引擎研发的一款开源自动驾驶模拟器。可在ubuntu系统上与ROS进行联合仿真。

下面放一些控制的核心代码：

stanley_controller.cpp

/*
 * @Author: HaiChen
 * @LastEditTime: 2025-02-07 13:02:07
 * @Description: stanel算法核心模块
 */

#include "stanely_control/stanely_controller.h"
#include "stanely_control/pid_controller.h"

#include <algorithm>
#include <iomanip>
#include <utility>
#include <vector>

#include "Eigen/LU"
#include <math.h>

using namespace std;

namespace shenlan {
    namespace control {

        shenlan::control::PIDController e_theta_pid_controller(1.0, 0.0, 0.4); // PID控制器中的微分环节相当于阻尼，加在航向误差引起的前轮转角上

        double atan2_to_PI(const double atan2) 
        {
            return atan2 * M_PI / 180;
        }

        double PointDistanceSquare(const TrajectoryPoint &point, const double x, const double y) 
        {
            const double dx = point.x - x;
            const double dy = point.y - y;
            return dx * dx + dy * dy;
        }
        void Limit(double &value, const double min, const double max)
        {
            if(value < min)
            {
                value = min;
            }
            else if(value > max)
            {
                value = max;
            }

        }
        //周期性：角度具有周期性，θ 和 θ + 2π 实际上表示相同的方向。因此，将角度限制在 -π 到 π 之间可以使角度值标准化，便于后续的计算和处理。
        void NormalizeAngle(double &angle) 
        {
            while(angle > M_PI)
            {
                angle -= 2 * M_PI;
            }
            while(angle < -M_PI)
            {
                angle += 2 * M_PI;
            }
        }

        void StanleyController::LoadControlConf() 
        {
            k_y_ = 0.5;
        }

        // 计算需要的控制命令, 实现对应的stanley模型,并将获得的控制命令传递给汽车
        // 在LGSVL车辆控制消息中，前轮转角的命令是弧度单位
        // float32 acceleration_pct  # 0 to 1
        // float32 braking_pct  # 0 to 1
        // float32 target_wheel_angle  # radians，经过实验，给正值的时候右转，给负值的时候左转
        // float32 target_wheel_angular_rate  # radians / second
        // uint8 target_gear
        void StanleyController::ComputeControlCmd(const VehicleState &vehicle_state, const TrajectoryData &planning_published_trajectory, ControlCmd &cmd) 
        {
            // 参考路径点
            this->trajectory_points_ =planning_published_trajectory.trajectory_points
            // 主车的状态信息
            double car_x = vehicle_state.x;
            double car_y = vehicle_state.y;
            double car_theta = vehicle_state.heading;
            double car_v = vehicle_state.velocity;
            double e_y = 0.0;
            double e_theta = 0.0;
            double kappa = 0.0;
            this->ComputeLateralErrors(car_x,car_y,car_theta,e_y,e_theta,kappa);
            // atan2 返回的是弧度单位，在分子上添加一个软化项，在低速条件下改善控制器性能
            double steer_e = atan2(this->k_y_ * e_y , this->k_s + car_v);
            // 限制Cross track error 引起的前轮转角的取值区间
            NormalizeAngle(steer_e);
            // PID控制器中的微分环节相当于阻尼，加在航向误差引起的前轮转角上，抑制高速工况下的过大的前轮转角变化率
            double steer_theta = e_theta_pid_controller.Control(e_theta,0.01);
            // 限制航向误差引起的前轮转角的取值区间
            NormalizeAngle(steer_theta);
            // 在连续弯道中，引入一个前馈项来提高跟踪性能，前馈项和路径曲率相同就足够
            // LGSVL中的车辆右转的时候，参考路径的曲率为负值,方向盘转角为正值，这里要加个负号
            double feedforward = -kappa;
            //计算前轮转角
            double steer_total = steer_e + steer_theta + feedforward;
            // 限制前轮转角的取值区间
            NormalizeAngle(steer_total);
            // 限制前轮转角的取值区间
            Limit(steer_total,-atan2_to_PI(20),atan2_to_PI(20));
            // 限制前轮最大转角，这里定义前轮最大转角位于 [-20度～20度]
            // 给出控制信号      
            cmd.steer_target = steer_total;
        }

        // 计算需要的误差，包括横向误差，纵向误差，误差计算函数没有传入主车速度，因此返回的位置误差就是误差，不是根据误差计算得到的前轮转角
        void StanleyController::ComputeLateralErrors(const double x, const double y, const double theta, double &e_y, double &e_theta, double &kappa) 
        {
            // 得到距离最近的路径点的信息
            TrajectoryPoint nearestpoint = QueryNearestPointByPosition(x, y);
            kappa = nearestpoint.kappa;
            // 将位置误差转换为前轮转角的时候：需要将路径上距离车辆最近的点从世界坐标系变换到车辆坐标系下，根据路径点在车辆坐标系下的y坐标的正负决定前轮转角的方向
            e_y = sqrt((x - nearestpoint.x) * (x - nearestpoint.x) + (y - nearestpoint.y) * (y - nearestpoint.y));
            double y_in_car = (nearestpoint.y - y)*cos(theta) - (nearestpoint.x - x)*sin(theta);
            if(y_in_car > 0)
            {
                //说明目标点在车辆左侧，车辆应该左转以跟踪参考路径
                e_y = -e_y;
            }
            else if (y_in_car < 0)
            {
                e_y = e_y;
            }
            // 自动驾驶中一般方向盘转角是左负右正
            // 路径上距离车辆最近的点的参考航向角，大于车辆的当前航向角的话，车辆应左转以跟踪航向
            e_theta = -(theta_ref_ - theta);

        }

        // 返回参考路径上和车辆当前位置距离最近的点，返回的是点结构体
        TrajectoryPoint StanleyController::QueryNearestPointByPosition(const double x, const double y) 
        {
            double d_min = PointDistanceSquare(trajectory_points_.front(), x, y); // 得到当前位置距离参考路径的第一个点的距离
            size_t index_min = 0;

            for (size_t i = 1; i < trajectory_points_.size(); ++i) 
            {
                double d_temp = PointDistanceSquare(trajectory_points_[i], x, y);
                if (d_temp < d_min) 
                {
                    d_min = d_temp;
                    index_min = i;
                }
            }
            // cout << " index_min: " << index_min << endl;
            //cout << "tarjectory.heading: " << trajectory_points_[index_min].heading << endl;
            theta_ref_ = trajectory_points_[index_min].heading; // 获得距离车辆当前位置最近的路径点的航向角

            return trajectory_points_[index_min];
        }
    }  // namespace control
}  // namespace shenlan

stanley_controller.h

#pragma once
#include <math.h>

#include <fstream>
#include <iomanip>
#include <memory>
#include <string>

#include "Eigen/Core"
#include "common.h"


namespace shenlan 
{
    namespace control 
    {
        using Matrix = Eigen::MatrixXd;

        class StanleyController 
        {
            public:
                StanleyController(){};
                ~StanleyController(){};

                void LoadControlConf();
                void ComputeControlCmd(const VehicleState &vehicle_state, const TrajectoryData &planning_published_trajectory, ControlCmd &cmd);
                void ComputeLateralErrors(const double x, const double y, const double theta, double &e_y, double &e_theta, double &kappa);
                TrajectoryPoint QueryNearestPointByPosition(const double x, const double y);

            protected:
                std::vector<TrajectoryPoint> trajectory_points_;
                double k_y_ = 0.0;//速度项的比例系数
                double u_min_ = 0.0;
                double u_max_ = 100.0;
                double k_s = 1.0;//软化常数

                double theta_ref_;
                double theta_0_;
        };
    }  // namespace control
}  // namespace shenlan

lat_lon_coupled_controller.cpp

/*
 * @Author: HaiChen
 * @LastEditTime: 2025-02-07 13:07:56
 * @Description: PID纵向控制和Stanley横向控制节点
 */

#include "stanely_control/lat_lon_coupled_controller.h"

using namespace std;
using std::placeholders::_1;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("lat_lon_coupled_controller");

StanelyAndPIDController::StanelyAndPIDController() : Node("vehicle_control")
{
    first_record_ = false;
    V_set_ = 5.0; // m/s
    wheelbase_ = 2.852;
    car_length_ = 2.852;

    pid_controller_longitudinal = std::make_unique<shenlan::control::PIDController>(0.5, 0.0, 0.0);
    stanely_controller_lateral = std::make_unique<shenlan::control::StanleyController>();
    stanely_controller_lateral->LoadControlConf();

    vehicle_control_publisher = this->create_publisher<lgsvl_msgs::msg::VehicleControlData>("/vehicle_cmd", 1000);

    global_path_publisher_ = this->create_publisher<nav_msgs::msg::Path>("/global_reference_path", 2);

    history_path_visualization_publisher = this->create_publisher<nav_msgs::msg::Path>("/history_path", 2);

    localization_data_subscriber = this->create_subscription<nav_msgs::msg::Odometry>("/odom", 10, std::bind(&StanelyAndPIDController::OdomCallback, this, _1));

    tf_broadcaster_gps_vehicle = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    vehicle_control_iteration_timer = this->create_wall_timer(10ms, std::bind(&StanelyAndPIDController::VehicleControllerIterationCallback, this));
    global_path_publish_timer = this->create_wall_timer(500ms, std::bind(&StanelyAndPIDController::GlobalPathPublushCallback, this)); 

    std::ifstream infile;
    infile.open("src/stanley_control/data/cube_town_reference_line.txt");  //将文件流对象与文件连接起来
    assert(infile.is_open());  //若失败,则输出错误消息,并终止程序运行

    std::vector<std::pair<double, double>> xy_points;
    std::string s;
    std::string x;
    std::string y;
    while (getline(infile, s)) 
    {
        std::stringstream word(s);
        word >> x;
        word >> y;
        double pt_x = std::atof(x.c_str());
        double pt_y = std::atof(y.c_str());
        xy_points.push_back(std::make_pair(pt_x, pt_y));
    }
    infile.close();

    // 读取参考线文件并构建参考路径
    std::vector<double> headings;
    std::vector<double> accumulated_s;
    std::vector<double> kappas;
    std::vector<double> dkappas;
    std::unique_ptr<shenlan::control::ReferenceLine> reference_line = std::make_unique<shenlan::control::ReferenceLine>(xy_points);
    reference_line->ComputePathProfile(&headings, &accumulated_s, &kappas, &dkappas);

    for (size_t i = 0; i < headings.size(); i++) 
    {
        std::cout << "pt " << i << " heading: " << headings[i] << " acc_s: " << accumulated_s[i] << " kappa: " << kappas[i] << " dkappas: " << dkappas[i] << std::endl; // heading 是弧度单位

    }

    for (size_t i = 0; i < headings.size(); i++) 
    {
        TrajectoryPoint trajectory_pt;
        trajectory_pt.x = xy_points[i].first;
        trajectory_pt.y = xy_points[i].second;
        trajectory_pt.v = 2.0;
        trajectory_pt.a = 0.0;
        trajectory_pt.heading = headings[i];
        trajectory_pt.kappa = kappas[i];

        planning_published_trajectory.trajectory_points.push_back(trajectory_pt);

        this_pose_stamped.header.frame_id = "gps";             
        this_pose_stamped.header.stamp = this->get_clock()->now();
        this_pose_stamped.pose.position.x = xy_points[i].first;
        this_pose_stamped.pose.position.y = xy_points[i].second;
        this_pose_stamped.pose.position.z = 0;
        this_pose_stamped.pose.orientation.x = 0;
        this_pose_stamped.pose.orientation.y = 0;
        this_pose_stamped.pose.orientation.z = 0;
        this_pose_stamped.pose.orientation.w = 0; // 这里实际上是放的frenet坐标系的S
        
        global_path.poses.push_back(this_pose_stamped);
        global_path.header.frame_id = "gps";

    }

    goal_point = planning_published_trajectory.trajectory_points.back();

}
StanelyAndPIDController::~StanelyAndPIDController(){}

double StanelyAndPIDController::PointDistance(const TrajectoryPoint& point, const double x, const double y)
{
    const double dx = point.x - x;
    const double dy = point.y - y;
    return sqrt(dx * dx + dy * dy);
}

void StanelyAndPIDController::OdomCallback(nav_msgs::msg::Odometry::SharedPtr msg)
{
    if (!first_record_)
    {
        first_record_ = true;
    }

    RCLCPP_INFO(LOGGER, "I hear localization");

    vehicle_state_.vx = msg->twist.twist.linear.x;
    vehicle_state_.vy = msg->twist.twist.linear.y; 
    
    tf2::Quaternion quat_tf;
    tf2::convert(msg->pose.pose.orientation, quat_tf);
    tf2::Matrix3x3(quat_tf).getRPY(vehicle_state_.roll, vehicle_state_.pitch, vehicle_state_.yaw);

    vehicle_state_.heading = vehicle_state_.yaw;

    // 将位置转移到前车轮的中心点
    vehicle_state_.x = msg->pose.pose.position.x + std::cos(vehicle_state_.heading) * 0.5 * car_length_;
    vehicle_state_.y = msg->pose.pose.position.y + std::sin(vehicle_state_.heading) * 0.5 * wheelbase_;

    vehicle_state_.velocity = std::sqrt(msg->twist.twist.linear.x * msg->twist.twist.linear.x + msg->twist.twist.linear.y * msg->twist.twist.linear.y);
    vehicle_state_.angular_velocity = std::sqrt(msg->twist.twist.angular.x * msg->twist.twist.angular.x + msg->twist.twist.angular.y * msg->twist.twist.angular.y);
    vehicle_state_.acceleration = 0.0;

    /* 将收到的定位信息发布出来,在rviz里显示历史轨迹 */
    history_path.header.stamp = this->get_clock()->now();
    history_path.header.frame_id = "gps";

    history_path_points.header.stamp = this->get_clock()->now();
    history_path_points.header.frame_id = "gps";
    history_path_points.pose.position.x = vehicle_state_.x;
    history_path_points.pose.position.y = vehicle_state_.y;
    history_path_points.pose.position.z = 0;
    history_path_points.pose.orientation = msg->pose.pose.orientation;
    history_path.poses.push_back(history_path_points);

    if (history_path.poses.size() > 2000)
    {
        vector<geometry_msgs::msg::PoseStamped>::iterator k = history_path.poses.begin();
        history_path.poses.erase(k);
    }

    history_path_visualization_publisher->publish(history_path);

    // 将世界坐标系和车辆坐标系的位置关系广播出来
    geometry_msgs::msg::TransformStamped transformStamped;
    transformStamped.header.stamp = this->get_clock()->now();
    transformStamped.header.frame_id = "gps";
    transformStamped.child_frame_id = "vehicle_odometry";
    transformStamped.transform.translation.x = msg->pose.pose.position.x;
    transformStamped.transform.translation.y = msg->pose.pose.position.y;
    transformStamped.transform.translation.z = msg->pose.pose.position.z;

    transformStamped.transform.rotation.x = quat_tf.x();
    transformStamped.transform.rotation.y = quat_tf.y();
    transformStamped.transform.rotation.z = quat_tf.z();
    transformStamped.transform.rotation.w = quat_tf.w();

    tf_broadcaster_gps_vehicle->sendTransform(transformStamped);
}

/**
 * @description: 车辆控制核心函数
 * @return {*}
 */
void StanelyAndPIDController::VehicleControllerIterationCallback()
{
    if(this->first_record_)
    {
        if (this->PointDistance(this->goal_point, this->vehicle_state_.x, this->vehicle_state_.y) < 0.5)
        {
            V_set_ = 0;
        }
        stanely_controller_lateral->ComputeControlCmd(this->vehicle_state_, this->planning_published_trajectory, cmd);

        double ego_speed = std::sqrt(this->vehicle_state_.vx * this->vehicle_state_.vx + this->vehicle_state_.vy * this->vehicle_state_.vy);
        double v_err = V_set_ - ego_speed;  // 速度误差
        cout << "v_err: " << v_err << endl;
        double acceleration_cmd = pid_controller_longitudinal->Control(v_err, 0.01);
        control_cmd.header.stamp = this->get_clock()->now();

        control_cmd.acceleration_pct = acceleration_cmd;
        control_cmd.target_gear = lgsvl_msgs::msg::VehicleControlData::GEAR_DRIVE;
        control_cmd.target_wheel_angle = cmd.steer_target;

        vehicle_control_publisher->publish(control_cmd);

    }
    
}
void StanelyAndPIDController::GlobalPathPublushCallback()
{   
    global_path.header.stamp = this->get_clock()->now();
    global_path_publisher_->publish(global_path);
} 

int main(int argc, char **argv)
{
    RCLCPP_INFO(LOGGER, "Initializa Node~");
    rclcpp::init(argc, argv);

    auto n = std::make_shared<StanelyAndPIDController>();

    rclcpp::spin(n);

    rclcpp::shutdown();

    return 0;

}

common.h

/*
 * @Author: HaiChen
 * @LastEditTime: 2025-02-07 13:09:03
 * @Description: 车辆及路径参数
 */

#ifndef COMMON_H
#define COMMON_H
#include <fstream>
#include <iostream>
#include <string>

#include <lgsvl_msgs/msg/vehicle_control_data.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <std_msgs/msg/header.hpp>

#include <vector>
#include "map.h"
#include "reference_line.h"
#include "rclcpp/rclcpp.hpp"
// #include <ros/ros.h>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/transform_datatypes.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/convert.h>
// #include "tf/tf.h"

struct VehicleState 
{
    double x;
    double y;
    double heading;   // 车辆朝向
    double kappa;     // 曲率(切线斜率)
    double velocity;    // 速度
    double angular_velocity;  // 角速度
    double acceleration;    // 加速度

    // 规划起点
    double planning_init_x; 
    double planning_init_y;

    double roll;  
    double pitch;
    double yaw;

    double target_curv;  // 期望点的曲率

    double vx;
    double vy;

    double v;

    // added
    double start_point_x;
    double start_point_y;

    double relative_x = 0;
    double relative_y = 0;

    double relative_distance = 0;

    double start_heading;
};

struct TrajectoryPoint 
{
    double x;
    double y;
    double heading;
    double kappa;
    double v;
    double a;
};

struct TrajectoryData 
{
    std::vector<TrajectoryPoint> trajectory_points;
};

struct LateralControlError 
{
    double lateral_error;
    double heading_error;
    double lateral_error_rate;
    double heading_error_rate;
};


struct ControlCmd 
{
    double steer_target;
    double acc;
};

typedef std::shared_ptr<LateralControlError> LateralControlErrorPtr;

#endif

由于水平有限以上内容仅是自己学习理解过程，各位读者朋友发现问题之处还请帮忙提出，不胜感激。

如果觉得文章对你有帮助的话请点个赞吧，有问题还请指正，共同交流，共同进步！