1. 先把底盘搞清楚差速轮与舵轮到底差在哪聊纯跟踪算法之前我觉得有必要先把底盘的运动学模型掰开揉碎讲清楚。因为很多做AGV控制的朋友一开始就把注意力全放在算法本身上结果代码写完了装到车上跑起来才发现——轮子布置方式不一样控制量的下发逻辑完全是两码事。差速轮底盘最常见的就是两轮差速左右两个驱动轮独立控制转速通过两侧的速度差实现转向。前轮或者后轮加一个万向轮做支撑纯属“陪跑”——不参与驱动也不参与转向。这种结构的运动学模型非常简单给定左轮速度v_L和右轮速度v_R底盘线速度v (v_L v_R) / 2角速度ω (v_R - v_L) / L其中L是轮距。也就是说你要让车转到什么方向本质上就是在调节左右轮的转速差。舵轮底盘就不一样了。舵轮是一个集驱动和转向于一体的轮组模块电机驱动轮子旋转提供前进动力另一个电机控制轮组绕垂直轴旋转改变方向。每个舵轮都有两个自由度驱动速度v_d和转向角度θ_s。这种结构最典型的应用就是重载AGV比如工厂里拉几百公斤物料的潜伏牵引式AGV或者港口码头用的巨型运输车。舵轮的优点是能实现全向运动车体可以不改变朝向就横着走、斜着走、原地转圈这在狭窄空间里简直是救命功能缺点嘛就是贵、重、控制复杂。我在实际项目中见过不少半路出家的工程师拿着差速轮的控制代码直接往舵轮车上套结果车走起来歪歪扭扭转弯的时候轮子疯狂抖动。问题根源就在于差速轮的控制量直接就是左右轮速差而舵轮必须先做一个运动学逆解——把车体的目标线速度和角速度分解到每个舵轮上得到“这个轮子该转多大角度、该滚多快”。这里还要提一个很多初学者容易忽略的点舵轮的转向角度是有限制的。你以为舵轮可以360度无限旋转不实际机械结构往往限制了转向范围常见的是±90度或者±135度。所以做运动学解算的时候必须把目标转向角约束到这个范围以内否则机械限位会直接卡死轻则报警停机重则损坏设备。这个坑我在后面“常见问题”章节会详细展开。关于热词里有人提到“三条AGV基本A算法”——这里顺带澄清一下A算法属于全局路径规划层解决的是“从地图上的A点到B点走哪条路”的问题而纯跟踪算法属于局部路径跟踪层解决的是“已经有一条路径了车怎么沿着它走”的问题。两层是不同的模块不能混为一谈。很多项目里路径规划用A*路径跟踪用纯跟踪两者配合工作时底层还需要一个运动学控制器把期望速度转成轮速指令。2. 纯跟踪算法的核心思路与几何推导纯跟踪算法Pure Pursuit这个名字起得很形象——就像你走路时盯着前方路上的某个点然后不断调整自己的方向朝那个点追过去。这个“点”就是前视点而你和前视点之间的连线形成的夹角就是控制车辆转向的依据。具体几何关系是这样的我们先在路径上取一个离车辆当前位置一定距离的点作为目标点这个距离叫前视距离L_d。然后以车辆后轴中心为原点建立坐标系找到目标点相对车辆坐标系的横向偏移量e_y。根据圆弧几何关系可以推导出车辆到达目标点所需的转弯半径RR (L_d²) / (2 × e_y)有了转弯半径再结合车辆当前的线速度v就能算出需要的角速度ω v / R。这就是纯跟踪算法最核心的输出。差速轮底盘拿到ω之后直接反解成左右轮速舵轮底盘拿到v和ω之后做运动学分解到每个舵轮上。这个公式推导过程其实很简单用初中级别的几何知识就能搞定——连接车辆当前位置和目标点车辆下一时刻要走的圆弧路径使得车辆方向变化量与目标点相对车辆方向的偏差成比例。本质上纯跟踪是一种比例控制器横向偏差越大车辆转弯越急。那么问题来了为什么用圆弧来拟合路径而不是直线因为车辆的非完整约束决定了它不能像全向轮那样横移。差速轮和舵轮虽然自由度不同但它们的运动轨迹天然是圆弧——差速轮的瞬时运动是一个以瞬时旋转中心ICR为圆心的圆弧舵轮在给定转向角下走的也是圆弧。纯跟踪算法假设车辆未来会走一段圆弧这正好符合底盘的运动学特性所以跟踪效果天然平滑。我在实际调车中特别喜欢纯跟踪算法的一个原因是它对路径形式不挑剔。你给它一段离散的点序列或者一段多项式曲线它都能工作。它不需要知道你整条路径的全局信息只需要知道“下一个目标点在哪儿”这是一种典型的局部控制策略。这种特性在AGV的部署中特别友好——无论是地磁导航、激光SLAM导航还是二维码导航上层给你一段路径点底层只需要跟踪就行。不过也正是因为它只看局部所以存在一个明显的缺陷如果前视点跳过了路径上的某个急转弯车辆会直接“抄近道”切弯切得很厉害。这在后面的调参章节里我会专门讲怎么处理。3. 路径跟踪实战差速轮和舵轮的代码实现理论讲再多不如直接上代码。下面我给出一套我在项目中实际用过的纯跟踪实现分别针对差速轮和舵轮底盘。代码用Python写ROS环境下可以直接把速度指令发布到/cmd_vel话题上非ROS环境下把输出值直接下发给运动控制板也行。3.1 差速轮底盘的纯跟踪实现差速轮的核心就是把算出来的ω和v分解为左右轮速。先用一个函数计算当前车辆状态下的目标角速度再做轮速分配。import math import numpy as np class DifferentialPurePursuit: def __init__(self, wheel_base0.5, max_steer0.8): self.wheel_base wheel_base # 轮距单位米 self.max_steer max_steer # 最大角速度限制单位rad/s def find_target_point(self, path, pose, lookahead_distance): 在路径上寻找距离当前位置约lookahead_distance的目标点 best_idx 0 best_dist float(inf) for i, point in enumerate(path): dist math.hypot(point[0] - pose[0], point[1] - pose[1]) if abs(dist - lookahead_distance) best_dist: best_dist abs(dist - lookahead_distance) best_idx i # 如果前方路径点距离都不够前视距离取最后一个点 if best_idx len(path) - 1: return None return path[best_idx], best_idx def calculate_omega(self, target_point, pose, velocity): 根据目标点计算角速度 # 将目标点转换到车辆坐标系下 dx target_point[0] - pose[0] dy target_point[1] - pose[1] # 车辆坐标系下的横向偏移 theta pose[2] local_x dx * math.cos(theta) dy * math.sin(theta) local_y -dx * math.sin(theta) dy * math.cos(theta) # 避免除零 if abs(local_x) 1e-6: return 0.0 # 计算转弯半径和角速度 radius (local_x**2 local_y**2) / (2 * local_y) omega velocity / radius # 限制角速度范围 return max(-self.max_steer, min(self.max_steer, omega)) def compute_wheel_speeds(self, velocity, omega): 将线速度和角速度分解为左右轮速 v_left velocity - omega * self.wheel_base / 2 v_right velocity omega * self.wheel_base / 2 return v_left, v_right这段代码有几个细节值得说明。第一找目标点的时候我没有用“距离前视距离最近的点”这个简单粗暴的策略而是取了“与期望前视距离差的绝对值最小”的点这样做的好处是前视点不会在后视区域反复横跳。第二计算local_y的时候横坐标为负表示目标点在车辆后方此时转弯半径的符号会自动反转车辆就能“倒车入位”——这个特性在做AGV倒退对接时特别有用。3.2 舵轮底盘的纯跟踪实现舵轮的实现比差速轮多了一个环节运动学解算。假设底盘有两个舵轮前后布置前舵轮负责转向和驱动后舵轮也可以转向和驱动两轮协同实现全向移动。这里给出一个简化版本先算车体的v和ω再解算到每个舵轮上。class SteerWheelPurePursuit: def __init__(self, wheel_base0.6, wheel_track0.8, max_steer_anglemath.pi/2): self.wheel_base wheel_base # 前后舵轮间距 self.wheel_track wheel_track # 左右舵轮间距 self.max_steer max_steer_angle # 最大转向角默认±90度 def calculate_velocity(self, target_point, pose, lookahead_distance): 核心纯跟踪计算目标线速度和角速度 dx target_point[0] - pose[0] dy target_point[1] - pose[1] theta pose[2] # 转到车辆坐标系 local_x dx * math.cos(theta) dy * math.sin(theta) local_y -dx * math.sin(theta) dy * math.cos(theta) # 纯跟踪公式 curvature 2 * local_y / (local_x**2 local_y**2) velocity 0.3 # 固定巡航速度实际项目中可根据曲率调节 omega velocity * curvature return velocity, omega def inverse_kinematics(self, v, omega): 将车体速度分解到前后两个舵轮上 # 前舵轮位置相对车体中心的坐标 front_x self.wheel_base / 2 # 舵轮在该位置的线速度 v_front_x v - omega * front_x # x方向分量 v_front_y omega * self.wheel_track / 2 # y方向分量 # 前舵轮的转向角和驱动速度 front_angle math.atan2(v_front_y, v_front_x) front_speed math.hypot(v_front_x, v_front_y) # 后舵轮同理但位置在车体中心后方 rear_x -self.wheel_base / 2 v_rear_x v - omega * rear_x v_rear_y -omega * self.wheel_track / 2 rear_angle math.atan2(v_rear_y, v_rear_x) rear_speed math.hypot(v_rear_x, v_rear_y) # 转向角约束 front_angle self.constrain_angle(front_angle) rear_angle self.constrain_angle(rear_angle) return front_speed, front_angle, rear_speed, rear_angle def constrain_angle(self, angle): 将角度约束到±max_steer范围内 angle (angle math.pi) % (2 * math.pi) - math.pi return max(-self.max_steer, min(self.max_steer, angle))这段代码里的逆运动学解算是舵轮控制的核心。注意我用了前后舵轮分别计算的方式而不是常见的“前轮转向、后轮驱动”模式。在实际的重载AGV上前后舵轮协同转向才能实现小半径转弯和原地旋转。如果你的项目只是单舵轮加两个从动轮那解算逻辑会更简单——直接把车体v和ω给到唯一的舵轮上就行。3.3 前视距离选择的一个实用方法前视距离L_d怎么定很多教材会告诉你取一个固定值比如1米或者2米。但真实场景中车速变化、路径曲率变化都会影响跟踪效果。我用过一个自适应前视距离的公式效果不错L_d k × v L_min其中v是当前车速k是一个时间常数通常取0.3到1.0L_min是保证算法稳定性的最小前视距离。这个公式的物理含义是车速越快看得越远给控制系统留出足够的反应时间。我习惯把这个公式写进参数配置文件里在调试时先固定v来调L_min再固定L_min去调k两个参数分开调不容易打架。4. 如何把路径跟踪调顺前视距离与速度的配合经验我在好几个AGV项目里调过纯跟踪说实话这个算法本身不难难的是参数调节。很多人卡在一个地方车走起来左右摇摆看起来就像喝醉了一样。我遇到这种情况第一反应就是去看前视距离是不是太小了。前视距离过小相当于你一直盯着脚底下走路稍微一点横向偏差就会导致大角度修正反映到车辆上就是剧烈的左右摆动严重的时候甚至会引发振荡——车头甩过来甩过去根本停不下来。反过来前视距离过大相当于你走路一直盯着几十米外的地方近处的弯道你根本看不见转弯的时候就会严重“切弯”甚至直接冲出路径。我见过最夸张的一次一台AGV在一个直角转弯处前视点直接越过弯道顶点车辆愣是开出路径一米多才转过来差点撞上货架。那么前视距离取多大合适我总结了一个经验公式在差速轮底盘上屡试不爽L_d v × t_response L_offset其中t_response是系统的响应时间可以理解为“从发出转向指令到车辆实际开始转向”的延迟一般取0.3到0.8秒L_offset是一个基础偏移量通常等于车体宽度。举个例子车速0.5m/s响应时间0.5秒车宽0.6米那L_d就是0.85米左右。我自己的调参流程是这样的先让车走直线确认基础运动学没问题然后放一个半径较大的圆弧路径比如半径3米把前视距离从大到小试观察转弯的平滑度和偏差量找到临界值之后再放一个S弯检验前视距离过大时的“切弯”问题最后如果是舵轮底盘还要重点测原地旋转和横移时的表现。另外一个容易被忽略的因素是速度。纯跟踪算法的控制频率通常在10到50Hz之间控制频率越低就需要越大的前视距离来弥补“感知盲区”。我有一次把控制频率从20Hz降到了10Hz同一组参数下车辆明显开始抖动把前视距离从0.8米调到1.2米之后才恢复稳定。后来我在代码里加了一个自适应逻辑根据控制周期动态微调前视距离低频控制时自动加大前视距离高频时减小。还有一个经验是关于路径点密度的。如果上游发过来的路径点非常稀疏比如两个点之间相隔2米而你的前视距离只有0.8米那么前视点会在两个路径点之间来回跳导致车辆转向指令剧烈波动。这种情况下有两种解决办法一是在路径跟踪模块里做一次插值把稀疏点加密成0.1米间隔的密集点序列二是直接加大前视距离让前视点覆盖至少两个路径点的跨度。我强烈建议做插值因为前视距离调大虽然能缓解抖动但会牺牲弯道跟踪精度。5. 舵轮解算里的几个硬骨头轮速分配与转向约束舵轮的运动学解算比差速轮复杂得多尤其是多舵轮系统。我在实际项目中调试过双舵轮和四舵轮底盘这里把最常见的坑和解决思路梳理一遍。5.1 双舵轮底盘的轮速分配双舵轮底盘通常是一前一后布置或者一左一右布置。前后布置时车体的运动学约束是两个舵轮的瞬时旋转中心必须重合否则车轮会“打架”——两个轮子互相较劲轻则轮胎磨损重则驱动电机过流报警。以我用的双舵轮底盘为例前后各一个舵轮给定目标线速度v和角速度ω时每个舵轮的速度分量可以看成舵轮所在位置的车体速度加上车体旋转产生的牵连速度。具体公式我在上面的代码里已经写了核心就是v_steer v_body ω × r其中r是舵轮到车体旋转中心的向量。这里要注意一个符号问题车体旋转方向和舵轮位置不同牵连速度的方向也不同。前舵轮在车体中心前方转弯时它的牵连速度方向是向弯道内侧的后舵轮在车体中心后方牵连速度方向是向弯道外侧的。如果符号搞反了两个舵轮的转向角就会一个朝左一个朝右车直接卡死。5.2 转向角约束与最短路径换向舵轮转向角约束是实际部署中最容易踩的坑。理论上舵轮可以转任意角度但机械结构往往限制了转向范围。我遇到过一台设备舵轮机械限位±95度但控制程序里没有做角度约束结果有一次路径任务要求车辆原地旋转180度程序给舵轮下发了一个120度的转向指令舵轮直接撞上了机械限位块听到“咔嚓”一声整个轮组都报废了。正确的做法是解算出的目标转向角如果超出约束范围不能简单粗暴地截断而是要做一个“换向”处理——把驱动速度取反转向角旋转180度。举个例子舵轮当前转向角是0度朝前目标转向角是135度超出了±95度的约束。此时正确的指令应该是转向角改为135 - 180 -45度驱动速度取反。这样舵轮转到-45度驱动轮反转实际行驶方向等效于135度。这个换向逻辑在低速场景下没什么问题但在高速场景下要小心速度取反的瞬间驱动电机会经历一个减速再反向的过程如果直接把速度从正的最大值跳到负的最大值机械冲击会非常大可能损坏减速机。我在代码里做了一层斜坡限制让驱动速度经过一个滤波器缓慢变化这样换向过程就平顺多了。5.3 四舵轮底盘的冗余控制有些重载AGV用四个舵轮每个都能独立转向和驱动。四舵轮系统的好处是承载能力强、灵活性高但运动学解算的复杂度也上去了。四个舵轮不是简单地把双舵轮的结果复制到两侧就行而是要保证四个轮子的瞬时旋转中心严格重合否则任何一个轮子都会产生滑移加剧轮胎磨损。四舵轮解算的一种常见做法是把四个舵轮分为两组每组两个对角布置的舵轮按双舵轮的方式解算然后取两组解算结果的平均值作为最终指令。这样做的好处是对角舵轮的误差可以互相抵消一部分提高系统的鲁棒性。我在实际项目里验证过这种方式在正常工况下误差可以控制在±1度以内。6. 纯跟踪的进阶玩法应对曲率突变与动态目标基础版的纯跟踪算法在路径比较平滑的情况下表现良好但工厂环境里路径往往没那么好走——直角弯、S弯、U型弯比比皆是。这时候基础版算法就开始力不从心了。曲率突变是纯跟踪算法最大的敌人。路径从直线突然进入小半径弯道前视点如果在弯道入口之前车辆还能正常转向但如果前视点直接跳到了弯道中段车辆就会以一个大圆弧“切”进弯道轨迹完全偏离路径。一种有效的方法是“动态前视距离”——根据路径前方区域的曲率自动调整前视距离的大小。曲率大的地方前视距离小一点曲率小的地方前视距离大一点。这个逻辑很像你在高速上开车进入匝道前会提前收油、把视线拉近而不是盯着远方的路标。动态前视距离的实现思路是这样的在路径上取前视距离范围内的所有路径点计算这些点的曲率平均值或者最大值然后根据曲率值动态调整前视距离的权重。我用的一个简化公式是L_adjusted L_base × (1 α × (C_max - C_threshold))其中C_max是前视范围内路径的最大曲率C_threshold是触发调节的曲率阈值α是调节系数。曲率越大的地方L_adjusted越小车辆转向响应越灵敏。另一个进阶玩法是前馈控制。纯跟踪本质上是反馈控制它只能根据当前偏差进行修正存在一定的滞后。为了减小滞后可以引入一个前馈项根据车辆当前位置到目标点的航向差直接在角速度输出上叠加一个前馈值。这个前馈值不需要精确只要方向对、量级大致正确就能明显减小过弯时的横向偏差。我测试过一组对比数据纯反馈跟踪时直角转弯的最大横向偏差约15厘米加了前馈之后最大横向偏差降到了5厘米以内。对于一条60厘米宽的通道来说15厘米的偏差意味着车辆几乎贴着边缘走稍有颠簸就可能蹭到而5厘米的偏差就从容多了。最后说说动态目标跟踪。有些AGV应用场景需要跟踪移动目标比如让AGV跟随一个移动的料车。这种情况下前视点不再是从固定路径上取而是直接以目标点作为跟踪对象。纯跟踪算法天然适合这种场景——你只需要不断更新目标点位置算法会自动微调方向去接近它。我在一个物料搬运项目里做过跟随功能AGV最高能以1.5m/s的速度跟随人工推行的料车横向误差控制在±10厘米以内稳定性完全可以接受。7. 现场调试速查表常见问题、排查方法与效果对比项目做完之后现场调试才是真正考验人的地方。下面我把这些年遇到最多的问题、排查思路和最终的解决效果整理成了一张表方便大家在现场快速定位问题。7.1 常见问题速查现象可能原因排查方法解决效果车辆左右摇摆前视距离过小逐步增大前视距离观察摇摆幅度是否收敛前视距离增到合适值后30秒内车辆趋于稳定转弯严重切弯前视距离过大减小前视距离或在弯道处启用动态前视最大横向偏差从20cm降到5cm直线行驶时车辆画龙控制频率过低提高纯跟踪算法发布频率调整前视距离20Hz以上时画龙现象消失舵轮转向抖动驱动速度和转向速度不匹配在运动学解算中增加速度斜坡限制抖动消失电机声音明显安静舵轮换向时冲击大速度指令跳变增加速度滤波换向前先减速至零冲击感大幅降低机械寿命提升前方障碍物急停后重新启动跑偏前视点跳变急停后重新初始化前视点为最近路径点重新启动后对准误差小于3cm差速轮底盘原地旋转时路径偏移轮距参数设置错误实测轮距与代码参数比对修正后旋转中心偏差小于1cm舵轮式AGV全向移动时姿态漂移舵轮零点漂移增加零点校准程序直线100米累计角度误差从2度降到0.2度7.2 我印象最深的一个排查案例有一个项目AGV在车间里跑着跑着突然往左偏每次偏的程度还不一样。我一开始以为是纯跟踪参数问题调了半天前视距离和速度没有改善。后来抓数据一看发现车辆在同一个位置附近总会往左偏而且是周期性出现的。排查了很久才发现罪魁祸首根本不是控制算法而是地面。车间地面上有一条焊缝高度差大概5毫米AGV每次经过焊缝时轮子会发生微小的侧向滑移导致车辆实际轨迹偏移了几厘米。纯跟踪算法根本不知道地面上有焊缝它只看到了“目标点在右边但是轮子打滑了”这个现象于是继续加大转向指令等焊缝过去了车已经明显偏了。这个案例让我意识到一个很重要的经验纯跟踪只是运动控制链路里的一个环节它依赖的是底层的速度闭环能不能精确执行指令。如果轮胎打滑、电机丢步、地面不平等因素导致轮速不准再好的路径跟踪算法也是巧妇难为无米之炊。所以在部署纯跟踪之前一定要先把底层运动学执行精度校好。我后来在项目中加了一个“轮速标定”环节让AGV直线行驶10米测量实际位移并修正轮径参数把速度闭环的误差控制在1%以内再上路径跟踪算法效果立马就不一样了。7.3 几个补充的排查技巧第一做纯跟踪调参时一定不要先在真实车辆上试。先用仿真环境跑一遍——哪怕是一个最简单的Python仿真输入一段圆弧路径看算法输出是否平滑。我经常先用一个纯数学仿真把参数初步调好再上真车验证节省了大量现场调试时间。第二调参时从大到小不要从小往大调。先把前视距离设得很大让车先“走起来”不抖动然后逐步减小前视距离直到出现轻微震荡回退20%作为最终参数。这个方法比“从一个很小值开始慢慢增大”效率高得多因为前视距离过小的时候车不走直线你根本无法判断其他参数的影响。第三如果条件允许给AGV加一个IMU惯性测量单元做航向角反馈。纯跟踪算法输出的角速度指令经过底盘执行之后实际航向角会有一个滞后和偏差IMU可以把这个偏差反馈回来实现航向闭环大幅提升跟踪精度。我在一个精度要求特别高的项目里加了IMU闭环之后横向跟踪误差从±3厘米缩小到了±1厘米以内效果非常明显。最后分享一个踩坑经验控制频率不要盲目调高。我以前总觉得控制频率越高越好从20Hz调到50Hz结果车辆不仅没有更稳反而开始出现高频抖动。后来才明白底盘电机驱动器本身也有响应带宽如果你的指令频率超过电机驱动器的响应能力指令就会被“截断”或者“滞后”相当于在控制环路上引入了一个额外的相位延迟反而降低了系统的稳定性。对于大多数AGV应用纯跟踪算法的控制频率设在20到30Hz之间就够了前提是前视距离匹配车速。写在最后搞了这么多年AGV运动控制最大的体会是纯跟踪算法不是银弹但它是一个特别“实惠”的路径跟踪方案——原理简单、计算量小、对路径形式不挑剔在成熟的算法框架里已经是非常可靠的存在。真正的功夫不在算法公式本身而在于你怎么把它和底盘的运动学特性、机械约束、现场环境匹配起来。把轮距校准好、把前视距离调好、把转向限位处理好一台AGV跑得又稳又准往往就是这些看似琐碎的细节堆出来的。如果你正准备踏进AGV运动控制这个领域我建议你从差速轮底盘开始学起先把纯跟踪在差速轮上跑通、调顺然后再挑战舵轮解算。差速轮理解了运动学和前视距离的关系舵轮那些转向角解算、速度分配自然就水到渠成。这个学习曲线是最平滑的也是我比较推荐的路径。