第4章横向控制基础

4、横向控制基础：路径跟踪问题定义、预瞄距离概念、纯跟踪算法

各位同学，今天我们来聊聊横向控制里最基础、也最核心的几个概念。说实话，很多新手一上来就盯着PID参数调，结果车在路上画龙。我当年也干过这事儿，后来才明白——先把路径跟踪的问题定义清楚，比什么都重要。

4.1 路径跟踪问题定义

什么叫路径跟踪？说白了就是：让车沿着一条预先规划好的路径走。

你可能会想，这不就是导航吗？其实不一样。导航只告诉你“前方300米右转”，而路径跟踪要解决的是——每一毫秒，方向盘该打多少度。

我习惯把这个问题拆成三个要素：

参考路径：一系列离散的路径点，每个点包含位置(x, y)和朝向(θ)
车辆状态：当前的位置、航向角、速度、横摆角速度
控制目标：最小化车辆与参考路径之间的横向偏差和航向偏差

核心公式：路径跟踪的本质是一个误差调节问题。

横向误差 e_y = 车辆质心到参考路径的最短距离

航向误差 e_θ = 车辆航向角 - 参考路径切线方向角

嗯，这里要注意：误差的定义方式直接影响控制效果。我在项目里见过有人直接用GPS坐标差算误差，结果车在弯道里疯狂振荡。为什么？因为坐标差没有考虑路径的曲率变化。

4.2 预瞄距离概念

预瞄距离，这个词听起来挺玄乎。其实你想想看，人开车的时候会盯着前方多远？近处？远处？

对，这就是预瞄距离的本质——驾驶员会选择一个前方的目标点，然后朝着那个点开。

在自动驾驶里，预瞄距离 L_d 决定了：

控制系统的“视野”：L_d 越大，控制越平滑，但响应越慢
弯道跟踪能力：L_d 越小，过弯越激进，容易 overshoot

我记得有一次调试，预瞄距离设得太长，车在高速上直道跑得挺好，一进弯道就直接冲出去了。后来改成动态预瞄距离——速度越快，看得越远；曲率越大，看得越近。

我的经验公式：

L_d = k_v * v + L_min

其中 k_v 取 0.3~0.5，L_min 取 2~5 米。具体数值跟车辆轴距、最大转向角有关，需要实车标定。

你可能会问：预瞄距离和PID里的“微分项”是不是一回事？不是。微分项看的是误差变化率，而预瞄距离看的是未来的路径信息。一个是“事后补偿”，一个是“事前预判”。

4.3 纯跟踪算法

纯跟踪算法（Pure Pursuit）是我个人非常喜欢的一个算法。为什么？因为它简单、直观、好调。

算法的核心思想就一句话：计算从车辆当前位置到预瞄点的圆弧曲率，然后转换成方向盘转角。

具体步骤：

在参考路径上找到距离车辆最近的路径点
从该点沿路径向前移动预瞄距离 L_d，得到预瞄点
计算车辆后轴中心到预瞄点的连线与车辆纵轴的夹角 α
根据几何关系计算转向角 δ

代码实现其实非常简洁：

def pure_pursuit_control(vehicle_state, path_points, L_d):
    # 1. 找到最近点
    nearest_idx = find_nearest_point(vehicle_state, path_points)
    
    # 2. 找到预瞄点
    lookahead_point = get_lookahead_point(path_points, nearest_idx, L_d)
    
    # 3. 计算夹角
    dx = lookahead_point.x - vehicle_state.x
    dy = lookahead_point.y - vehicle_state.y
    alpha = atan2(dy, dx) - vehicle_state.yaw
    
    # 4. 计算转向角 (基于自行车模型)
    L = vehicle_state.wheelbase  # 轴距
    delta = atan2(2 * L * sin(alpha), L_d)
    
    return delta

我曾经踩过的坑：

纯跟踪算法在低速时表现很好，但高速时容易振荡。原因在于：预瞄距离固定时，高速下控制周期内的路径变化率太大。我的解决方案是：

加入速度前馈，让预瞄距离随速度线性增长
限制最大转向角变化率，防止方向盘猛打
在弯道入口处提前减小预瞄距离

4.4 三种方法的对比

为了让你有个直观的认识，我把三种常见横向控制方法做了个对比：

方法	优点	缺点	适用场景
纯跟踪	实现简单、参数少、低速稳定	高速振荡、对预瞄距离敏感	低速园区、泊车
Stanley	高速稳定、误差收敛快	对路径连续性要求高	高速公路
LQR/MPC	最优控制、可处理约束	计算量大、调参复杂	复杂工况、极限操控

我个人建议：先从纯跟踪入手。为什么？因为你能快速看到效果，建立信心。等把预瞄距离、路径跟踪这些概念吃透了，再上Stanley或者MPC也不迟。

一句话总结：

路径跟踪就是“看多远、打多少”。预瞄距离决定了你看多远，纯跟踪算法告诉你怎么打。两者配合好了，车就能稳稳地沿着路走。