智能车辆路径跟踪控制：纯跟踪控制与Stanley控制算法，其他线相关算法。 主要是MATLAB程序

智能车辆路径跟踪控制：纯跟踪控制与Stanley控制算法，其他线相关算法。 主要是MATLAB程序，可以根据需要的路径进行跟踪

最近在倒腾无人车路径跟踪算法，发现纯跟踪（Pure Pursuit）和Stanley这俩经典方法真是让人又爱又恨。今儿咱们直接上代码实战，手把手看看这俩货到底怎么玩转轨迹跟踪。

先搞个测试路径热热身。假设咱们要让小车跑个半径10米的圆：

theta = 0:0.1:2*pi; refPath = [10*cos(theta'); 10*sin(theta')]; % 圆形参考路径 scatter(refPath(:,1), refPath(:,2), 10, 'filled');

纯跟踪的核心在于预瞄点选择。看看这个魔性的lookahead distance参数怎么影响跟踪效果：

function delta = pure_pursuit(currentPos, lookahead_dist, refPath) % 找距离最近的路径点 [~, idx] = min(vecnorm(refPath - currentPos, 2, 2)); % 前看L距离的路径点 target_idx = idx; while target_idx <= length(refPath) && norm(refPath(target_idx,:)-currentPos) < lookahead_dist target_idx = target_idx + 1; } % 计算转向角（核心公式） alpha = atan2(refPath(target_idx,2)-currentPos(2), refPath(target_idx,1)-currentPos(1)) - currentYaw; delta = atan(2 * wheelbase * sin(alpha) / lookahead_dist); end

这里有个坑：预瞄距离得随速度动态调整。低速时设0.5米能完美跟弯道，但速度一快就容易画龙——这时候得用速度系数动态放大，比如L = 0.3*v + 0.5。

智能车辆路径跟踪控制：纯跟踪控制与Stanley控制算法，其他线相关算法。 主要是MATLAB程序，可以根据需要的路径进行跟踪

接下来是Stanley算法，这货对航向误差特别敏感。核心代码长这样：

function delta = stanley_control(currentPos, currentYaw, v, refPath) % 找最近点 [nearest_point, idx] = min(vecnorm(refPath - currentPos, 2, 2)); path_yaw = atan2(refPath(idx+1,2)-refPath(idx,2), refPath(idx+1,1)-refPath(idx,1)); % 横向误差计算 front_axle = currentPos + wheelbase/2 * [cos(currentYaw), sin(currentYaw)]; cross_track_error = sign(dot([front_axle(2)-refPath(idx,2)], [-sin(path_yaw), cos(path_yaw)])) * nearest_point; % 转向控制（注意这个k参数） heading_error = path_yaw - currentYaw; delta = heading_error + atan(k * cross_track_error / (v + 0.1)); % 防除零 end

重点在k参数的调整——我试过当车速超过10m/s时，k值超过0.3就会让方向盘抽风。建议用PID参数整定法来找最佳值。

实测对比发现：纯跟踪在8字路径的交叉点容易走外道，这时候Stanley的航向修正就派上用场了。但Stanley在急弯时如果车速过高，横向误差项会突然暴涨，需要加个tanh函数限幅。

最后给个实时调试建议：在回调函数里动态绘制预瞄点和误差向量，比看数据直观十倍。比如这样实时显示预瞄点：

set(preview_point, 'XData', refPath(target_idx,1), 'YData', refPath(target_idx,2)); drawnow

这俩算法其实都是调参小能手，参数对了啥路况都能跑。下次试试把两者结合——用Stanley修正航向，用纯跟踪控制预瞄距离，说不定有惊喜。代码扔GitHub了，需要自取。