很多做无人驾驶控制的朋友第一次接触横向控制算法基本都是从纯跟踪Pure Pursuit和Stanley这两个名字开始的。它们在无人驾驶车辆控制里的地位有点像练字时的“横竖撇捺”——看似简单却是后面所有复杂控制方案的地基。我在做无人驾驶车辆控制项目时最早在仿真里跑通的也是这两套算法后来在低速园区车、工程机械无人驾驶场景里也做过实际部署对它们的脾气摸得比较透。这篇就把Pure Pursuit和Stanley从原理推导、代码实现到工程调参讲清楚顺便把两个算法怎么选、踩过哪些坑一并整理出来。文章面向的是刚接触车辆控制、准备在仿真环境或实车平台上做路径跟踪的开发者。你不需要很强的自动控制理论背景只要懂一点三角函数和车辆运动学就能把算法跑起来。搞懂这两个算法之后再去碰MPC、LQR这类更“重”的控制器理解成本会低很多。1. 为什么需要横向控制先把坐标系和问题定义说清楚1.1 车辆控制问题的拆分无人驾驶的车辆控制模块通常拆成纵向控制和横向控制两条线。纵向控制管油门刹车解决“跑多快”的问题横向控制管方向盘转角解决“往哪走”的问题。Pure Pursuit和Stanley属于横向控制里的几何类方法它们不依赖复杂的车辆动力学模型直接利用车辆运动学关系根据当前车辆姿态和前方参考路径计算出一个前轮转角指令。在讨论具体算法之前先把问题抽象一下车辆通常用一个自行车模型来近似也就是把左右前轮合并成一个前轮左右后轮合并成一个后轮前后轴距记为L。状态量一般用后轴中心坐标x, y、航向角yaw来描述。控制量是前轮转角delta。参考路径则是一系列带坐标的离散点。横向控制的任务就变成已知车辆当前状态和参考路径求出一个合适的前轮转角delta让车辆能沿着参考路径行驶。这里有个很重要的认知几何类算法在低速场景下表现得特别好因为它忽略了轮胎侧偏、离心力这些动力学因素。速度越低运动学假设越成立。这也是为什么园区的无人配送车、工程机械的自动作业路径跟踪很多都直接拿Pure Pursuit或Stanley做底层控制。速度一旦超过某个阈值比如60km/h以上几何算法的精度就会明显下降需要引入动力学模型来做前馈补偿。1.2 横向控制的评价指标评价一个横向控制算法好不好不能只看“跟得紧不紧”我一般会看四个维度精度、稳定性、鲁棒性和实时性。精度指横向偏差也就是车辆实际位置到参考路径的垂直距离通常用RMS横向误差来量化。稳定性指车辆行驶过程中有没有左右振荡方向盘的修正是否平滑。鲁棒性指碰到路面不平、GPS信号抖动、参考路径不光滑时算法还能不能稳住。实时性指单次计算耗时Pure Pursuit和Stanley都只是几条三角函数公式计算量几乎可以忽略在MCU级别的控制器上也能跑到几百赫兹。这四个维度里稳定性和精度往往是矛盾的。很多新手调参时只盯着横向误差把增益调得很大结果误差是小了但车辆开始蛇形走位方向盘来回抽动实际体验非常差。真正用得好的调参是在误差和稳定性之间找一个平衡点。2. 纯跟踪Pure Pursuit二轮车模型里的“追点”艺术2.1 算法几何推导Pure Pursuit的核心思想非常直观通俗点说就是车辆始终盯着前方路径上的一个目标点然后根据这个目标点计算出一条圆弧让车辆沿着圆弧走到那个点。具体做法是以车辆当前后轴中心为圆心以“前视距离ld”为半径在参考路径上找一个目标点然后计算车辆当前航向与车辆到目标点连线方向的夹角alpha再用一个圆弧去拟合从当前位置到目标点的轨迹圆弧的曲率就是我们要的控制量。推导过程不复杂。假设车辆前后轴距为L前轮转角为delta则在低速条件下车辆转弯半径R与前轮转角的关系为R L / tan(delta)根据图中几何关系文字描述后轴中心、前视目标点、车辆航向之间构成三角形可以推出圆弧曲率kappa与夹角alpha的关系kappa 2 * sin(alpha) / ld联合两式得到纯跟踪的控制律delta arctan(2 * L * sin(alpha) / ld)这就是Pure Pursuit的全部核心公式。看起来简单但里面藏着几个关键点。第一前视距离ld是算法里唯一的“玄学”参数它直接决定了控制的激进程度第二alpha的计算要正确处理角度归一化否则车辆在跨过±π边界时会出现转角跳变第三ld如果取得比转弯半径还大目标点可能选不到实际工程里通常要加一个最大前视距离的限制。2.2 代码实现与调参细节Pure Pursuit的实现代码很简洁核心函数一般长这样def pure_pursuit_steer(vehicle_state, ref_path, ld): # 找到前视距离内的目标点 target_idx find_target_index(vehicle_state, ref_path, ld) target_point ref_path[target_idx] # 计算车辆到目标点的向量与车辆航向的夹角 dx target_point.x - vehicle_state.x dy target_point.y - vehicle_state.y alpha normalize_angle(math.atan2(dy, dx) - vehicle_state.yaw) # 纯跟踪转角公式 delta math.atan2(2.0 * vehicle_state.wheelbase * math.sin(alpha), ld) return delta这里最容易被忽视的是find_target_index这个函数。最土的办法是每帧遍历所有路径点找距离等于ld的点路径点一多效率就低。我在工程里一般会维护一个“当前最近点索引”每次从它附近往后搜索这样既保证连续性又减少了计算量。另一个细节是如果路径点稀疏或车辆横向偏差较大前视圆可能跟路径没有交点这时要按投影点加/减ld的方式估算目标点位置。调参方面前视距离ld基本决定了一切。ld太短车辆会剧烈振荡像喝醉了酒一样左右晃ld太长车辆会严重切弯走出一条“切西瓜”的轨迹。我在低速园区车上的经验是ld取车速的0.5到1倍再加一个基础值基础值不小于2米。举个例子车速2m/s、轴距2.5m的车辆ld起步可以设成2.5米左右再根据实际振荡情况微调。还有一种常见做法是把ld做成随速度变化的函数即ld kv * v ld0。kv一般取0.5到1.5ld0取2到5米。这个思路的本质是速度越高需要越长的预瞄距离来提前转向避免车辆在高速下反应过激。需要注意的是ld随速度变化时如果kv设得太大车辆在低速时又容易因为ld过小而产生振荡所以kv的标定是一个在低速和高速之间反复权衡的过程。3. Stanley算法前轴中心的航向修正3.1 算法公式推导Stanley算法是斯坦福大学在2005年DARPA无人车挑战赛中提出来的它跟Pure Pursuit的“看远点”思路完全不同。Pure Pursuit看的是前方一段距离的目标Stanley看的是当前车辆正前方的路径偏差通过同时修正航向误差和横向误差来逼近期望路径。Stanley的控制律可以写成delta psi arctan(k * e / v)其中psi是车辆航向与参考路径上最近点切线的夹角航向误差e是前轴中心到最近路径点的横向偏差v是车速k是增益系数。这个公式的物理含义很清晰第一项psi负责把车头摆正让车辆航向跟路径方向一致第二项arctan(k * e / v)负责消除横向偏差当车辆偏离路径时给它一个朝向路径的转向角。两项相加就是最终的前轮转角。为什么第二项要用反正切函数而不是直接用k*e因为反正切函数有天然的饱和特性。当横向偏差很大时这项输出也只会接近±90度而不会无限增大避免了转角指令过大导致执行机构饱和。同时分母上的v意味着车速越低横向偏差修正的权重越大这符合低速下需要更激进纠偏的直觉。不过这里有个明显的隐患v接近0时arctan(k * e / v)会趋向±90度也就是说车辆静止时只要有一点横向偏差Stanley就会直接给出极限转角。这在低速工程机械场景下很成问题实测中必须给v加一个下限或者将公式改造成带速度反馈的形式否则停车起步阶段方向盘会猛打一下非常吓人。3.2 代码实现与参数意义Stanley的实现比Pure Pursuit稍微复杂一点核心在于求最近路径点和航向误差。伪代码如下def stanley_steer(vehicle_state, ref_path, k): # 找最近路径点及其切线方向 nearest_idx, nearest_point find_nearest_point(vehicle_state, ref_path) path_yaw ref_path.yaw[nearest_idx] # 航向误差 psi normalize_angle(path_yaw - vehicle_state.yaw) # 横向误差前轴中心到最近路径点的垂直距离 e compute_lateral_error(vehicle_state, nearest_point, path_yaw) # 速度下限保护 v max(vehicle_state.speed, 0.5) delta psi math.atan2(k * e, v) return delta这里最关键的参数是增益k。k越大横向偏差的修正越猛烈收敛越快但过大也会引发振荡。我调试时的经验是k先设成1.0然后每次增加0.5观察横向误差收敛曲线。如果出现等幅振荡或者方向盘高频抖动就回退到上一个值。Stanley算法还有个细节容易被忽略就是横向误差e的计算方向。在前轴中心到最近路径点的向量与路径切向量之间做叉积就能得出带符号的横向偏差正负代表车辆在路径的哪一侧。这个符号直接决定转角的方向一旦搞反车辆会往错误的方向修正直接冲出道路。值得注意的是Stanley的航向误差项psi并没有增益系数所以在高速场景下它的纠错能力完全依赖横向偏差项。如果路径曲率变化快、车辆速度又高Stanley容易出现“修不过来的”情况表现为路径内侧偏航过大。实际工程里我会对psi项再加上一个可调的权重系数相当于给了算法更大的自由度去平衡两个误差项的权重。4. 实际部署中的对比不光看精度还要看可调性4.1 两个算法的核心差异对比搞懂了两个算法的原理之后最难的问题来了项目里到底该选谁这个没有标准答案取决于车辆平台、运行速度、路径类型和执行机构特性。我把它们的差异整理成一个表格方便你对照选择。对比维度Pure PursuitStanley控制基准点后轴中心前轴中心预瞄机制有前视距离ld无只看最近点航向误差修正间接通过目标点夹角直接psi项横向误差修正间接通过圆弧拟合直接atan项低速表现良好更优收敛更快高速稳定性好适当增大ld中等易偏航过弯切弯程度明显ld越大越切轻微对执行器延迟敏感度低天然带预瞄高调参难度只需调ld简洁需调k还需处理速度下限从这个表能看出来两个算法其实代表了两种不同的思想Pure Pursuit用“预瞄”换稳定性Stanley用“直接纠偏”换精度。这也是为什么很多实车项目会选择在低速环境下用Stanley、在高速环境下用Pure Pursuit的原因。但也不是绝对的关键还是看你的路径曲率变化和执行机构的响应速度。4.2 不同场景下的算法选型建议如果你做的是园区无人配送车速度一般在1到3m/s路径弯道多但曲率不大我建议优先试Stanley。因为它收敛快、稳态误差小车辆能贴得很紧尤其在狭窄通道里Stanley的优势非常明显。我做过一个封闭园区的无人车项目用Pure Pursuit在弯道处总是切内线车身会蹭到路沿换成Stanley并把k调到1.8之后最大横向误差从30厘米压到了10厘米以内。如果你做的是干线物流或者高速场景的无人驾驶Pure Pursuit通常更合适。高速下动态响应慢需要较长的预瞄距离来缓冲纯跟踪的“看远点再走”正好符合这个需求。此前有一个矿区宽体自卸车的项目速度在20到30km/h我用的就是Pure Pursuitld取30米左右车辆在尘土飞扬、GPS偶尔丢星的恶劣条件下依然能保持车道内的稳定行驶而Stanley在同样的速度下会出现高频修正方向盘机构磨损明显。工程机械无人驾驶这两年热度很高挖掘机、推土机、压路机的自动作业大量用到横向控制。这类平台有个共同特点液压转向执行器响应慢延时可能到几百毫秒。在这种执行器下我会更倾向于用Pure Pursuit或者带预瞄机制的改进版Stanley。我之前在一个无人压路机项目里试过原始Stanley转向迟滞导致车辆在路径修正时产生持续振荡后来把Stanley的误差项加了一个低通滤波、再串联一个前馈预瞄才算把问题压住。5. 仿真环境搭建与验证流程5.1 车辆运动学模型与仿真框架在把算法搬到实车之前我强烈建议你先在仿真环境里把逻辑跑通。不需要太重型的仿真器用Python加matplotlib就能完成一套基础的横向控制验证。核心是搭一个自行车运动学模型递推公式如下def update_state(state, delta, v, dt, L): state.x v * math.cos(state.yaw) * dt state.y v * math.sin(state.yaw) * dt state.yaw v / L * math.tan(delta) * dt return state这个模型里delta是前轮转角v是速度L是轴距。它忽略了轮胎滑移、侧倾、空气阻力等因素对低速控制算法验证来说精度完全够用。仿真时每隔一个控制周期生成一条参考路径可以是直线、圆形、双移线然后调用Pure Pursuit或Stanley算出转角再更新车辆状态记录每一步的横向误差和转角曲线。仿真的意义不只是把代码跑通更关键的是给你一个安全环境去理解参数变化的影响。我见过很多新手直接跳到实车调参一次失误就可能导致车辆撞到路沿或转向机构损坏。在仿真里大胆地把ld从1米调到30米看看轨迹怎么从“大幅摆动”变成“缓慢切弯”这种直观感受对后续实车标定帮助很大。5.2 测试流程和评价标准我的仿真测试流程一般分四步。第一步直线跟踪测试看算法能不能把初始横向偏差收敛到0这个阶段主要查符号问题和角度归一化问题。第二步圆形路径跟踪测试固定速度、固定曲率按压带测量稳态误差同时记录转角是否平滑。第三步双移线测试模拟换道工况看路径曲率突变时车辆是否跟得住有没有超调。第四步速度扫描测试把同一路径在0.5m/s、1m/s、2m/s、3m/s下分别跑一遍画出横向误差随速度变化的曲线用来标定ld或k的速率相关项。评价标准我习惯看三个数RMS横向误差、最大横向误差、方向盘转角变化率。RMS横向误差反映整体贴合程度最大横向误差反映是否有危险偏差方向盘转角变化率反映执行机构压力。一个合格的控制算法在低速仿真下RMS横向误差应该在5厘米以内最大误差不超过15厘米转角曲线没有明显毛刺。6. 实战中的坑与调参经验6.1 前视距离选择的经验法则关于Pure Pursuit的前视距离网上说法很多但真正在工程里管用的还是那套“速度分段标定”的思路。我在低速无人车上常用的初始值是这样ld 0.8 * v 3.0其中v的单位是m/sld的单位是米。先按这个跑然后根据横向误差曲线微调系数。如果车辆出现蛇形振荡说明ld太小按0.2米的步长逐步加大ld。如果车辆在弯道明显切内线说明ld太大按0.5米的步长逐步减小。这里有个容易犯的错误只看横向误差曲线调整却忽略了参考路径本身的曲率。路径曲率大的地方需要的ld反而应该更短因为过长的预瞄会让目标点落到弯道内侧导致转向不足越来越严重。更精细的做法是让ld根据当前路径曲率动态调整曲率越大ld越小。6.2 常见问题速查表最后把我在实际部署中踩过的一些典型问题整理成表格方便你排查时对照。现象可能原因排查与解决方法车辆左右振荡、蛇形走位Pure Pursuit前视距离过短或Stanley的k过大逐步增大ld或减小k每次微调幅度不超过20%弯道切内线严重前视距离ld过大减小ld或改为随曲率动态调整的ld直线上横向误差收敛慢航向误差项权重不足在Stanley中增加psi项的权重系数或适当增大k车速接近0时转角突变Stanley公式中e/v在低速下过大给v加下限值或者对输出转角做速率限幅方向盘高频抖动控制频率与执行器响应不匹配或路径点噪声大对转角指令做低通滤波对参考路径点做平滑处理高速下明显偏航几何算法在高动态场景下力不从心加入动力学前馈或考虑换用LQR/MPC控制器GPS跳变导致横向误差突变定位噪声进入控制环增加定位信号滤波或对横向误差做限幅处理工程机械液压转向延迟导致振荡执行器响应太慢增加预瞄距离、串联低通滤波器或在Stanley上做延迟补偿我个人调参时的一个小习惯是先在仿真里把参数范围摸出来再上实车按“每次只改一个参数、改完跑三遍重复验证”的节奏来。宁可多花时间也不要一次动多个参数不然出了问题根本定位不到是哪个参数引起的。横向控制调试这件事七分靠耐心三分靠经验没有任何一个参数组合能一劳永逸地适配所有场景这也是为什么实车标定在无人驾驶项目里永远是最耗时的一环。