Pure Pursuit纯跟踪算法：算法原理、前视距离选择策略、代码实现与仿真

各位同学好，我是你们的老朋友。今天我们来聊聊路径跟踪里一个非常经典、也非常直观的算法——Pure Pursuit（纯跟踪）。

说实话，我刚入行那会儿，第一次看到这个算法的名字，还以为是什么高深莫测的“纯追踪”黑科技。后来一研究，发现它的核心思想其实特别朴素：就像你开车时，眼睛一直盯着前方路上的一个点，然后打方向盘让车头对准那个点开过去。嗯，就是这么回事。

1. 算法原理：几何法中的“老大哥”

Pure Pursuit 本质上是一种基于几何的路径跟踪方法。它不关心车辆复杂的动力学模型，而是把问题简化成一个纯几何问题。

它的核心逻辑是这样的：

1. 找到目标点：在预定义的参考路径上，找到一个距离当前车辆位置恰好为 \( L_d \)（前视距离）的点，这个点我们叫它“目标点”或“前视点”。
2. 计算圆弧：假设车辆从当前位置，沿着一条圆弧行驶到那个目标点。
3. 求解曲率：根据车辆当前位置、航向角以及目标点的位置，计算出这条圆弧的曲率 \( \kappa \)。
4. 输出控制量：将曲率 \( \kappa \) 转换成前轮转角 \( \delta \)，发给执行器。

说白了，就是“看多远，拐多大弯”。前视距离越长，转弯越平缓；前视距离越短，转弯越急促。

这里有个细节，我在项目中遇到过：公式里的 \( y_g \) 是目标点在车辆坐标系下的横向偏差。如果直接用全局坐标算，结果会完全不对。我当时就犯过这个错，调试了半天，发现车在画龙...

2. 前视距离选择策略：算法的“灵魂”

Pure Pursuit 好不好用，90% 取决于前视距离 \( L_d \) 怎么选。这玩意儿选不好，车要么像喝醉了酒（振荡），要么像没睡醒（切弯太慢）。

我个人习惯把前视距离的选择策略分为三类：

策略类型	核心思想	适用场景	我的经验
固定前视距离	\( L_d \) 为常数	低速、曲率变化不大的场景	最简单，但适应性差。我一般只在测试时用。
速度自适应	\( L_d = k \cdot v \)	中高速、道路较直	最常用。速度越快，看得越远。但弯道容易“飞出去”。
曲率自适应	\( L_d = f(\kappa_{path}) \)	复杂弯道、城市道路	效果最好，但调参麻烦。我曾经调了一个月才稳定下来。

3. 代码实现：C++与Python混合实战

好了，理论说完了，咱们上代码。我个人习惯用Python做原型验证，C++做工程落地。这里我给出一个核心的Pure Pursuit计算函数，用C++写，因为实际项目中跑得最多的就是它。

// pure_pursuit.cpp
#include <cmath>
#include <vector>

struct Point {
    double x;
    double y;
};

struct VehicleState {
    double x;       // 后轴中心x
    double y;       // 后轴中心y
    double yaw;     // 航向角 (rad)
    double velocity; // 当前速度 (m/s)
};

// 计算前轮转角
double calcPurePursuitSteer(const VehicleState& state,
                            const std::vector<Point>& path,
                            double lookahead_distance) {
    // 1. 找到前视目标点
    Point target_point;
    double min_dist = 1e9;
    bool found = false;

    for (const auto& pt : path) {
        double dx = pt.x - state.x;
        double dy = pt.y - state.y;
        double dist = std::sqrt(dx*dx + dy*dy);
        
        // 找距离刚好超过前视距离的点
        if (dist >= lookahead_distance) {
            target_point = pt;
            found = true;
            break;
        }
    }

    if (!found) {
        // 如果没找到（比如路径太短），就用最后一个点
        target_point = path.back();
    }

    // 2. 将目标点转换到车辆坐标系
    double dx = target_point.x - state.x;
    double dy = target_point.y - state.y;
    
    // 旋转到车辆坐标系
    double local_x = dx * std::cos(-state.yaw) - dy * std::sin(-state.yaw);
    double local_y = dx * std::sin(-state.yaw) + dy * std::cos(-state.yaw);

    // 3. 计算曲率 (注意：这里local_x理论上应该接近lookahead_distance)
    double curvature = 2.0 * local_y / (lookahead_distance * lookahead_distance);

    // 4. 计算前轮转角 (L为轴距)
    double L = 2.8; // 假设轴距2.8米
    double steer = std::atan(curvature * L);

    // 5. 限幅
    double max_steer = 0.6; // 约34度
    steer = std::max(-max_steer, std::min(max_steer, steer));

    return steer;
}

这段代码我用了很多年，核心逻辑没变过。你想想看，其实就三步：找点、坐标变换、算曲率。是不是很简单？

4. 仿真验证：用Python快速看效果

写完了C++代码，怎么验证？我一般先用Python搭个简单的仿真环境，把算法跑一遍，看看轨迹对不对。

下面是一个极简的仿真框架，你可以在Jupyter Notebook里直接跑：

# pure_pursuit_sim.py
import numpy as np
import matplotlib.pyplot as plt

def pure_pursuit_steer(state, path, ld):
    # 找目标点
    target = None
    for pt in path:
        dist = np.hypot(pt[0]-state[0], pt[1]-state[1])
        if dist >= ld:
            target = pt
            break
    if target is None:
        target = path[-1]
    
    # 坐标变换
    dx = target[0] - state[0]
    dy = target[1] - state[1]
    local_x = dx * np.cos(-state[2]) - dy * np.sin(-state[2])
    local_y = dx * np.sin(-state[2]) + dy * np.cos(-state[2])
    
    # 曲率 -> 转角
    curvature = 2.0 * local_y / (ld**2)
    steer = np.arctan(curvature * 2.8)
    return steer

# 仿真主循环
def simulate():
    # 定义路径（一个圆形）
    path = [(5*np.cos(t), 5*np.sin(t)) for t in np.linspace(0, 2*np.pi, 100)]
    
    # 初始状态 [x, y, yaw, v]
    state = [5.0, 0.0, np.pi/2, 2.0]  # 从圆的最右侧出发，朝上
    dt = 0.1
    ld = 3.0  # 前视距离
    
    traj = [state[:2]]
    for _ in range(500):
        steer = pure_pursuit_steer(state, path, ld)
        # 更新状态（简单运动学模型）
        state[0] += state[3] * np.cos(state[2]) * dt
        state[1] += state[3] * np.sin(state[2]) * dt
        state[2] += state[3] / 2.8 * np.tan(steer) * dt
        traj.append(state[:2])
    
    # 绘图
    traj = np.array(traj)
    path = np.array(path)
    plt.plot(path[:,0], path[:,1], 'b--', label='Reference')
    plt.plot(traj[:,0], traj[:,1], 'r-', label='Tracking')
    plt.axis('equal')
    plt.legend()
    plt.show()

if __name__ == '__main__':
    simulate()

这段代码跑起来，你会看到红色轨迹慢慢追上蓝色虚线。如果前视距离调得合适，轨迹会非常平滑。如果调得不好...嗯，你会看到车在画龙。

5. 知识体系总览

为了让你对整个Pure Pursuit的知识结构有个全局印象，我画了一张图：

这张图把Pure Pursuit的整个知识脉络串起来了。从核心原理出发，到三个关键步骤，再到前视距离的三种策略，最后落到工程实现上。你学习的时候，可以按这个结构来，先理解原理，再动手调参，最后写代码。

好了，关于Pure Pursuit纯跟踪算法，我就讲这么多。记住，这个算法虽然简单，但非常实用。很多量产车的L2级辅助驾驶，底层用的就是它。你把它吃透了，后面学更复杂的MPC、LQR都会轻松很多。