一种基于PD控制器的轨迹跟踪算法

hust_smartcar

一.引言

​        目前在智能车竞赛中，比较常用的循迹方法是基于中线误差，利用PID控制跟踪循迹。这种方法虽然简单有效，但这种算法拟合出的轨迹不够光滑，缺乏对轨迹曲率的预测能力，难以应对非线性、高动态、复杂情景。针对这种算法存在的问题，我们想到利用纯跟随算法找到预瞄点，利用PD控制器进行反馈控制循迹的方法。

二.算法原理介绍

​        纯跟随算法（Pure Pursuit）是一种基于几何原理的轨迹跟踪算法，核心思想是通过让车辆始终追踪路径上的一个 “预瞄点”，实现对参考轨迹的平滑跟随。其原理和内容可以简单概括为以下几点：

        1. 核心思路
        想象人走路时会看向前方一点并朝着它移动，纯跟随算法类似：在预设的参考轨迹（由一系列离散点组成）上，根据车辆当前位置选择一个 “预瞄点”（位于车辆前方固定距离处），然后控制车辆转向，使车辆沿圆弧轨迹驶向这个预          瞄点，最终自然跟随整个参考路径。
       2. **关键要素**
   - **参考轨迹**：由多个坐标点（x,y）组成的目标路径。
   - **预瞄点**：从参考轨迹上选取的、距离车辆当前位置约为 “预瞄距离” 的点，是车辆短期跟踪的目标。
   - **预瞄距离**：车辆到预瞄点的距离，是核心参数。
   - **车辆模型**：简化为 “自行车模型”，通过几何关系计算转向角。
       3. **转向角计算**
   基于几何推导，根据车辆当前位置、航向角、预瞄点位置和轴距，计算所需转向角：
   - 先确定车辆与预瞄点的相对位置关系；
   - 利用圆弧运动特性，推导转向角公式。
       4. **特点**
   - 优点：原理简单、计算量小、实现容易，能够使得跟踪轨迹变得更加平滑，优越性尤其表现在圆环路径上。

三.关键代码介绍

     1.找到预瞄点

```C++
bool findLookaheadTarget(const geometry_msgs:oseStamped& current_pose, geometry_msgs:oseStamped& target)
    {
        double current_yaw = tf2::getYaw(current_pose.pose.orientation);
        double min_dist = std::numeric_limits<double>::max();
        bool target_found = false;
        double v = cmd.linear.x;  // 获取当前线速度

        for (const auto& pose : path_.poses)
        {
            // 计算目标点相对于当前点的dx和dy
            double dx = pose.pose.position.x - current_pose.pose.position.x;
            double dy = pose.pose.position.y - current_pose.pose.position.y;
            double dist = std::sqrt(dx * dx + dy * dy);

            // 计算路径点相对于当前朝向的角度
            double target_angle = std::atan2(dy, dx);
            double angle_diff = std::atan2(std::sin(target_angle - current_yaw), std::cos(target_angle - current_yaw));


            // 获取路径点本身的朝向
            double pose_yaw = tf2::getYaw(pose.pose.orientation);
            double yaw_diff = std::atan2(std::sin(pose_yaw - current_yaw), std::cos(pose_yaw - current_yaw));
            double yaw_factor = std::cos(0.5 * yaw_diff);
            yaw_factor = std::max(0.2, yaw_factor);
            lookahead_distance_ = std::max(min_lookahead_, k_lookahead_*v*yaw_factor);

            // 只考虑前方的点（角度差小于90度）
            if (std::fabs(angle_diff) <  M_PI/2 && std::fabs(yaw_diff) < 2*M_PI/3)  // 180度以内
            {
                // 寻找距离最近的点
                if (dist >= lookahead_distance_ && dist < min_dist)
                {
                    min_dist = dist;
                    target = pose;
                    target_found = true;
                }
            }
        }

        if(!target_found)
        {
            ROS_WARN("No valid target point found within lookahead distance.");
        }
        return target_found;
    }

```

​        这部分代码的核心功能是找到小车行进过程中的预瞄点，让小车跟随这个点进行运动。首先要先获取小车本身的参数，例如当前的线速度和角速度，根据当前小车的定位得到目前小车与轨迹的相对位置关系，包括当前的朝向与轨迹的误差、当前位置与预瞄点的距离。通过小车的朝向和预瞄点的误差确定转向角度，并由此确定预瞄距离。

       2.基于PD控制器进行运动控制

```C++
geometry_msgs::Twist computeControlCommand(const geometry_msgs:oseStamped& current_pose,const geometry_msgs:oseStamped& target)
    {
        geometry_msgs::Twist cmd;

        double dx = target.pose.position.x - current_pose.pose.position.x;
        double dy = target.pose.position.y - current_pose.pose.position.y;

        double yaw = tf2::getYaw(current_pose.pose.orientation);

        double target_angle = std::atan2(dy, dx);
        double angle_error = target_angle - yaw;

        angle_error = std::atan2(std::sin(angle_error), std::cos(angle_error));

        ros::Time current_time = ros::Time::now();
        double dt = (current_time - prev_time_).toSec();
        if (dt <= 0.0) dt = 0.01;  // 防止除零错误

        double raw_derivative = (angle_error - prev_angle_error_) / dt;
        filtered_derivative_ = filter_alpha_ * filtered_derivative_ + (1.0 - filter_alpha_) * raw_derivative;   

        double pd_output = kp_ * angle_error + kd_ *  filtered_derivative_;

        // 限幅
        pd_output = std::max(-max_angular_speed_, std::min(pd_output, max_angular_speed_));

        prev_angle_error_ = angle_error;
        prev_time_ = current_time;

        cmd.linear.x = std::max(max_linear_speed_ * std::cos(angle_error),min_angular_speed_);
        cmd.angular.z = pd_output;
        last_servo = pd_output;
        return cmd;
    }
```

​        在小车的运动控制上，我们选择了位置式PD控制器。当我们确定预瞄点后，即可确定当前小车与预瞄点之间的角度误差，并将该角度误差作为PD控制器的输入。为了防止小车对噪声和突变信号的相应过于敏感，我们对微分项做了限幅处理。这一部分是经典的PID控制部分，许多细节部分同学们可以根据调车情况进行修改，例如加入积分项消除稳态误差、增加积分抗饱和部分、使用动态PD参数等。

四. 结语

​        最后，希望这篇文章能够帮助大家调好自己的小车，欢迎大家和我们进行交流，相互学习，共同进步！