第一次实车跑路径跟踪的时候我站在跑道边上看着规划模块吐出一条非常平滑的曲线然后车沿着它歪歪扭扭画了一通龙最后斜着停在路边。那一瞬间我意识到一件事规划层能画出一条路跟控制层能让车辆真正压着这条路走中间隔着非常多的细节。无人驾驶的车辆控制模块里路径跟踪要做的就是这件事——把参考路径和车辆当前状态换算成前轮转角、油门和刹车这些执行指令。而纯跟踪Pure Pursuit算法和Stanley算法是最经典也最常用的两种几何路径跟踪解法计算量小、参数直觉、容易调通特别适合作为入门的第一个横向控制算法。这篇文章我把这两个算法从头到尾拆开讲清楚包括运动学模型、公式推导逻辑、代码实现、调参经验以及落到工程机械这一类载体上要怎么改。内容面向正在做无人驾驶控制、想转行做控制算法、或者在做机器人路径跟踪的工程师也适合刚入门的同学按顺序读下来。1. 规划层把路径画好了控制层是怎么把车压上去的先把路径跟踪在整个无人驾驶系统里的位置捋清楚。一个典型的无人驾驶软件栈会分成感知、定位、规划、控制四块。规划层负责输出一条从起点到终点的参考路径通常是一串带有坐标的离散点有时还带上每个点期望的速度、曲率、时间戳等信息。控制层拿到这条路径之后要做的事是让车尽量沿着这条路径走同时保证乘坐舒适、执行平稳、不出安全事故。控制层内部一般又分成横向控制和纵向控制两条线。横向控制管方向盘也就是“往左打多少、往右打多少”纵向控制管速度也就是“油门踩多少、刹车踩多少”。这两个方向并非完全独立比如转弯时往往会主动减速所以实际系统中还会有横纵向协同的策略。但底层的核心算法可以分开设计这也是行业里很常见的做法。纯跟踪和Stanley都属于横向控制里的几何类方法。它们跟另一大类基于车辆动力学模型的方法不一样最大的特点是不需要知道车辆的质量、轮胎侧偏刚度、路面附着系数这些动力学参数只依赖运动学关系就能算出前轮转角。这意味着代码实现非常轻量调试门槛低而且在中低速场景下精度已经相当够用。所以即使在如今MPC、LQR这类方法满天飞的时代Pure Pursuit和Stanley依然是很多量产项目、行业竞赛、工程机械自动驾驶项目的首选起步方案。用个不太严谨但很好懂的类比MPC这类方法像是“深思熟虑型司机”它要考虑接下来几秒钟的变化、车辆动态响应、轮胎极限然后给出一个综合最优的打轮方案而Pure Pursuit和Stanley像是“眼睛盯着目标点打方向”的直觉型司机虽然不做长远规划但反应快、动作直接在大多数常规工况下已经能开得很稳。这也是为什么它们被归为“几何类方法”因为核心就是几何关系不涉及复杂受力分析。对刚接触控制模块的人来说我建议先把这两个几何算法吃透。原因很简单它们能把“路径跟踪到底在解决什么数学问题”这件事展示得非常直白你会清楚地看到横向误差怎么定义、航向误差怎么定义、误差怎么通过反馈转换成控制量。把这些基础打牢后面再上手MPC、LQR理解起来会顺畅得多。2. 单车模型与阿克曼转向几何所有几何跟踪算法的共同地基想真正看懂Pure Pursuit和Stanley先要理解它们底层的车辆运动学模型。这里最常用的就是自行车模型Bicycle Model也叫单车模型。它把四轮车辆等效成两个轮子前轮负责转向后轮负责驱动整车看成一根连杆。虽然这是个简化但在分析路径跟踪控制问题时非常有效因为低速下车辆的运动更多由几何约束决定而不是轮胎受力。简化过程是这样的把左右前轮合并成一个位于车辆中心线上的虚拟前轮左右后轮合并成一个虚拟后轮前后轮之间的距离取真实的轴距L。这样得到的模型只有三个关键量车辆速度v、前轮转角δ、航向角θ。车辆在平面上的运动可以写成x_dot v * cos(θ) y_dot v * sin(θ) θ_dot (v / L) * tan(δ)前两个式子表示车辆位置的变化速度第三个式子最关键它说车辆的航向变化率也就是横摆角速度主要由前轮转角决定转角越大车头转得越快轴距越长同样的转角下车头转得越慢。从这个式子还能推出一个常用关系车辆转弯半径R。在稳态转向时车会绕着某个圆心做圆周运动几何关系是R L / tan(δ)也就是说前轮转角δ直接决定车辆转弯半径。反过来如果我想让车辆沿某条曲率为κ的路径运动也就是转弯半径为R1/κ那么对应的前轮转角应该是δ arctan(L / R) arctan(L * κ)这个公式特别重要因为Pure Pursuit和Stanley本质上都是在做一件事估计当前车辆应该走的圆弧曲率然后通过这个公式反算出前轮转角。理解了这一点你再看后面那些“看起来有点神奇”的推导就不会觉得是凭空冒出来的。再说说阿克曼转向几何。真实车辆转弯时内外侧前轮的转角其实不一样内侧轮转角更大这样才能保证所有轮子绕同一个瞬时圆心转动、不打滑。自行车模型天然忽略了这个问题直接用中心线上的虚拟前轮转角。在控制算法里我们算出来的一般就是这个虚拟转角真正下发给转向执行器的时候再通过阿克曼几何换算成左右轮各自的转角。这个换算通常是底盘厂商或者线控转向系统内部完成的控制算法不必过多操心但你心里得有数避免在标定转向机构时把虚拟转角和真实轮角混为一谈。在路径跟踪里误差定义是整个问题的关键。两个算法里经常提的误差有两类一类是横向误差lateral error指的是车辆某个参考点到参考路径的垂直距离另一类是航向误差heading error指的是车辆航向和参考路径方向之间的角度差。Pure Pursuit主要是通过预瞄点隐式地考虑误差Stanley则是把横向误差和航向误差显式地写进了同一个控制律里。这两种思路的差异直接影响两个算法在弯道、在高速、在噪声环境下的表现。3. Pure Pursuit的核心预瞄点、圆弧与那个经典的atan2Pure Pursuit的算法思想特别直观就是模仿人开车时盯着前方某个目标点的行为。你开车的时候并不会死盯着车轮底下那一点点路面而是会看前方一定距离处的某个点然后根据车头方向和目标点的关系打方向。Pure Pursuit把这个行为抽象成三步在参考路径上找一个距离车辆前方Ld处的目标点然后假设车辆沿着一段圆弧行驶可以到达这个目标点最后根据这段圆弧的曲率计算出前轮转角。这里Ld叫预瞄距离是整个算法里最核心的参数。它决定了你“看多远”也决定了路径跟踪的激进程度和稳定性。3.1 几何直觉从“看路开车”到圆弧拟合解释算法之前先画个几何草图在脑子里车辆当前位置在P点车头方向朝前前方某个距离Ld处有个目标点G。从P到G画一条连线这条线就是车辆要走的圆弧的弦。车头方向线和PG连线之间的夹角记为α。如果α不为零说明目标点不在正前方车辆需要转弯才能到达转弯半径和α直接相关。这里的几何关系有个经典的推导圆弧的弦长与圆弧半径之间满足 Ld 2R * sin(α)。这个式子怎么来的圆心在车辆位置到目标点圆弧的圆心处弦长对应的圆心角是2α因为弦切角定理切线与弦的夹角α等于圆心角的一半。有了这个关系转弯半径就是R Ld / (2 * sin(α))然后结合上一章的公式δ arctan(L / R)把R代进去就得到Pure Pursuit的前轮转角计算式δ arctan(2 * L * sin(α) / Ld)这个公式是Pure Pursuit算法最经典的形式。换个角度理解它其实是“根据目标点偏移量估算圆弧曲率”的过程。目标点越偏向一侧或者预瞄距离越短算出来的曲率越大转角也越大。3.2 公式推到这里就够了前轮转角怎么来的工程实现上有一个比上面更稳的写法。为了避免角度象限问题和正负号混淆我习惯直接调用atan2把分子分母都传进去delta atan2(2 * L * sin(alpha), Ld)这样写的好处是能正确区分角度所在的象限不会出现本该左转却算成右转的情况。alpha本身也要注意做归一化处理把它限定在[-π, π]范围内避免因为角度跨越±π的边界产生跳变。很多刚写代码的同学在这里栽过跟头航向角从179度变到-179度时如果不做归一化alpha会突然从2度跳成-358度转向指令瞬间打满车就会猛甩一下。如果参考路径点比较密还可以做一步近似简化。把sin(α)展开假设α比较小那么sin(α) ≈ 横向偏移量 / Ld于是公式可以改写成delta ≈ atan2(2 * L * e_l, Ld²)其中e_l是目标点相对于车辆航向方向的横向偏移量。这种形式在分析算法稳定性时很好用它更直白地展示了Pure Pursuit相当于一个“带预瞄的比例控制器”横向偏差越大转角越大预瞄距离越长转角被稀释得越厉害、越柔和。实际代码里我一般还是用原始的sin(α)版本因为它在大角度工况下更准确。3.3 预瞄距离才是灵魂Ld的标定策略预瞄距离Ld怎么定是Pure Pursuit调参的重头戏。定得太短算法会变得非常敏感参考路径上任何一个小噪声都会放大成明显的转向抖动车会沿着路径“画蛇”定得太长算法对弯道的响应变慢车会大范围切弯内切严重曲率大的弯道甚至可能直接转不过去。最常用的做法是把Ld设成与车速成线性关系Ld k * v Ld0其中k是预瞄时间系数单位是秒表示“我相当于在盯着前方多少秒后的轨迹”Ld0是基础预瞄距离保证极低速时依然有一个最小向前看的范围。根据我的实测经验k在0.3到1.0之间是一个比较常见的范围。低速园区场景车速2到3m/sLd取3到5米就比较合适高速干线场景车速15m/s以上Ld可能得到10到20米才稳得住。具体数值跟控制频率、路径曲率分布、转向执行器响应速度都有关系没有一个万能值。我的建议是先在仿真里扫参数把不同k值下的横向误差曲线画出来看然后上实车微调。调的时候注意一个规律如果车辆走直线时会左右小幅摆动大概率是预瞄太短如果进弯明显走到路径内侧大概率是预瞄太长。用这个规律去反推比瞎试参数要快很多。3.4 一个可运行的Pure Pursuit参考实现下面给一段简化的Python实现。假设车辆状态用一个长度为3的数组表示[x, y, yaw]参考路径是N×2的numpy数组控制周期内调用一次get_steering函数就能得到前轮转角。import math import numpy as np def find_target_point(path, x, y, Ld): for i in range(len(path)): dist math.hypot(path[i][0] - x, path[i][1] - y) if dist Ld: return i, path[i] return len(path) - 1, path[-1] def pure_pursuit_steer(vehicle_state, path, Ld, L): x, y, yaw vehicle_state idx, target find_target_point(path, x, y, Ld) tx, ty target alpha math.atan2(ty - y, tx - x) - yaw # 归一化到 [-pi, pi] alpha math.atan2(math.sin(alpha), math.cos(alpha)) delta math.atan2(2.0 * L * math.sin(alpha), Ld) return delta, idx这段代码里find_target_point的写法是最朴素的顺序查找路径点数多的时候可以加个缓存从上一次目标点索引往后继续找减少每帧的遍历量。另外要提醒一句Ld在公式里出现的位置决定了它是标量还是个动态值。如果Ld随速度变化那么每一帧都要重算一遍不能只算一次然后在整个控制过程中保持不变否则速度一变算法表现会突然劣化。4. Stanley算法放下预瞄把横向误差和航向误差掰进同一个公式Stanley算法是2005年斯坦福大学在DARPA无人车挑战赛中使用的横向控制算法以那辆参赛车的名字命名。Pure Pursuit是“向前看”的思路Stanley则是“盯住最近点”的思路它找出参考路径上离车辆前轴中心最近的那个点然后根据车辆航向与该点切线方向的夹角、以及车辆到该点的横向距离直接合成一个前轮转角。4.1 Stanley在DARPA里的来历与核心思路当年DARPA挑战赛要穿越一段很长很复杂的沙漠越野路线车辆速度不低路径曲率变化也大。纯跟踪在这种路况下表现不够稳定因为预瞄机制天然有一定的滞后性和切弯趋势。Stanley算法的思路更直接我不去看前方多远我就看车轮底下最近的那个路径点让车头的朝向收敛到与路径方向一致同时让横向距离快速衰减到零。它的控制律长这样δ θ_e arctan(k * e / v)符号含义需要注意θ_e是航向误差等于车辆当前航向减去参考路径最近点处的切线方向e是前轴中心到参考路径最近点的横向距离通常约定车辆位于参考路径右侧时为正或者为负取决于代码实现里怎么定义这个符号问题非常容易出bugv是当前车速k是一个需要标定的增益系数。结构上这个式子分两块θ_e负责纠正方向偏差相当于前馈加方向反馈后面那个arctan项负责消除横向偏差相当于横向修正量。横向误差e越大修正转角越大车速v越快同样的横向误差下修正转角被稀释防止高速时猛打方向引起失控。这个速度项的设计逻辑很朴素同样偏了半米在低速时可以立刻把轮子打过去纠正在高速时必须小角度慢慢地掰回来。4.2 θ_e和e怎么算——注意前轴还是后轴Stanley里横向误差用的是前轴中心位置这是很多人忽略的一个细节。Pure Pursuit的推导通常基于后轴中心因为后轴的运动更符合圆弧约束但Stanley用的是前轴因为方向盘转角直接控制前轮用前轴的横向误差做反馈更直接收敛速度也更快。如果代码里拿到的是后轴坐标很多定位模块给的是后轴中心需要先根据航向角和轴距换算一下前轴位置再算误差。求e的具体做法是遍历参考路径点找到离前轴中心最近的点然后计算前轴中心到那个点的距离再根据叉积判断车辆在路径的哪一侧给距离加上正负号。符号方向必须和θ_e的符号约定保持一致否则两个误差项会互相抵消车会一直跑偏或者画龙。方向误差θ_e的计算也有讲究。最近点的切线方向要用它跟相邻点构成的向量来求不能用全局坐标系直接算航向角差因为路径点顺序可能不连续。一般这么做找到最近点索引i之后用i1点的坐标减去i点的坐标然后atan2得到切线方向θ_pathθ_e就等于当前车辆航向减去θ_path再做归一化。4.3 增益k与速度项的真实作用很多人在Stanley调参时把k当成一个“越大越好”的增益结果发现k大了车照样来回甩。原因在于k跟速度耦合在一起。从控制律的形式来看横向修正量等于arctan(k * e / v)当v变大的时候同样的k作用相当于被缩小了。这是个合理的物理直觉速度越快同一段时间内车辆前进的距离越远横向误差本身的衰减也更快所以不需要那么大的修正角度。k的实际标定范围通常在1到5之间但这严重依赖车速单位和坐标单位。如果速度单位是m/s、误差单位是米k1的时候e1米、v5m/s对应的修正角大约是11度k3时对应约31度。从这些数值能大概感受到k量级的意义。我的建议是先在仿真里用固定速度测试把k从0.5往大调看横向误差收敛速度和转向平稳性找到那个“既不迟钝又不振荡”的值再放到变速场景里验证。Stanley还有一个容易踩的坑是参考路径噪声。因为它用的是最近点路径上任何一个小毛刺都会直接影响最近点的位置和切线方向所以路径平滑度对Stanley的影响比Pure Pursuit大得多。实测中如果路径点来自感知或者实时构建的地图点位抖动比较明显建议先对路径做平滑比如三次样条插值或者滑动平均否则方向盘会一直在那里高频微抖。4.4 参考实现与修改建议下面是一个Stanley的简化实现输入车辆前轴中心坐标、航向角、路径点集输出前轮转角。import math import numpy as np def nearest_point(path, x, y): dist np.hypot(path[:, 0] - x, path[:, 1] - y) idx np.argmin(dist) return idx, path[idx] def stanley_steer(front_x, front_y, yaw, path, L, k, v): idx, nearest nearest_point(path, front_x, front_y) # 计算最近点切线方向 if idx len(path) - 1: dx path[idx, 0] - path[idx - 1, 0] dy path[idx, 1] - path[idx - 1, 1] else: dx path[idx 1, 0] - path[idx, 0] dy path[idx 1, 1] - path[idx, 1] theta_path math.atan2(dy, dx) # 航向误差 theta_e yaw - theta_path theta_e math.atan2(math.sin(theta_e), math.cos(theta_e)) # 横向误差用叉积判断左右侧 vec_t np.array([dx, dy]) vec_p np.array([nearest[0] - front_x, nearest[1] - front_y]) cross vec_t[0] * vec_p[1] - vec_t[1] * vec_p[0] e math.hypot(nearest[0] - front_x, nearest[1] - front_y) if cross 0: e -e # 前轮转角使用前轴到后轴的轴距L只是为了把前轴坐标换出来 # 实际控制量本身与L无关因为最近点是基于前轴算的。 delta theta_e math.atan2(k * e, v) return delta, idx读者可能注意到这个实现里带了轴距L参数但控制律里没直接用到。这是因为调用方如果给的是后轴位置要先用L换算前轴位置再传进来如果直接给前轴位置L就用不到。这个细节代码里没有强制统一各项目写起来习惯不一样但语义要清楚。从我的实测经验看Stanley在低速、路径平滑、没有太多大弯的场景下跟踪精度通常优于Pure Pursuit横向误差收敛到厘米级不是问题。但速度一旦上来它对误差的快速修正反而容易变成激发车辆横摆的源头。所以很多项目会做速度切换低速用Stanley高速切回Pure Pursuit。5. 实车对比哪种场景下哪个算法更靠谱很多初学者会问这两个算法哪个更好答案永远是“看场景”。我整理了两种算法在关键维度上的表现对比下面这个表格是我在园区低速、矿区中低速、以及干线高速三类场景实测后的主观总结不是理论推导结论参考时可以结合自己的场景情况。对比维度Pure PursuitStanley参考点方式前方固定距离的预瞄点前轴中心最近路径点横/航向误差收敛较慢但有平滑效果较快误差直接反馈弯道跟随特性明显内切曲率大时延迟贴合度好内切不明显对路径噪声敏感性低预瞄有平均效应高最近点易受毛刺影响高速稳定性拉长预瞄后表现较好容易引发横向振荡调参最容易犯的错Ld太小导致画蛇k太大导致画龙最适合的场景中高速、路线平滑、弯道不太急低速、高精度、路径质量好从表格能看出来这俩算法不是替代关系而是互补关系。园区巡逻车、景区接驳车这些场景速度通常不超过5m/s路径也比较规整我倾向于用Stanley因为横向误差收敛快停车、进站、靠边这些操作做起来更精准。矿区或者干线场景速度高、弯道分布不确定我倾向于用Pure Pursuit把预瞄距离随着速度拉上去车会走得更稳更顺。还有一个工程上常见的做法二者切换使用。低速段用Stanley做精准入位中高速段用Pure Pursuit做稳定跟线。切换点最需要注意的是连续性处理不能直接一个if语句瞬间切算法否则切换那一刻计算出的转角如果差异很大转向执行器会被猛掰一下。实际项目里我见过有人用线性加权过渡在切换速度附近的一个速度窗口内把两个算法输出的转角按权重混合起来。这个方法简单有效但要注意权重变化不能太快而且两个算法的坐标系定义必须完全一致否则混合的物理意义就不对。在实际项目选型时还要考虑到一个很现实的因素参考路径是怎么来的。如果路径是全局规划一次性生成的高质量平滑路径Stanley这样的精细算法更容易发挥精度如果路径是实时感知拼接出来的、隔一阵就更新一次、带噪声和跳变那Pure Pursuit的预瞄机制反而更像一个天然的滤波器能把路径的不完美过滤掉一部分。这点往往比算法本身的精度更能决定实车表现。6. 从乘用车到工程机械载体换了算法怎么改工程机械无人驾驶是近两年很热的方向但很多人拿着乘用车的控制算法直接往装载机、压路机、矿卡上搬结果发现完全不是那么回事。这里最大的区别不在算法本身而在底盘结构和工作场景。工程机械大多不是阿克曼转向而是铰接式转向前车体、后车体通过一个垂直铰接轴连接轮胎本身不转向整车靠液压缸改变前后车体的相对夹角来完成转弯。6.1 铰接底盘的运动学差异铰接式车辆的运动学模型跟自行车模型长得像但转向输入从“前轮转角δ”变成了“铰接角γ”。它的运动学方程可以写成类似的形式θ_dot (v / L) * sin(γ)这里L还是前后车体等效轴距γ是前车体相对后车体的铰接角。注意乘用车模型里是tan(δ)铰接车这里是sin(γ)差异来自几何结构的不同。如果用Pure Pursuit的思路去控制铰接车最直接的做法是把“前轮转角”替换成“铰接角”但公式里的L要换成等效轴距。具体怎么等效取决于参考点取在前车体还是后车体的哪个位置这部分在实车标定时需要根据机械结构仔细推。实际项目里铰接车还有个麻烦点γ一般不能像方向盘那样快速大幅度动作液压执行器的响应速度比乘用车的电动转向慢不少而且铰接角本身有机械限位大部分车型可能只有±35度到±40度。这意味着同样的预瞄距离下铰接车能转的弯半径下限更高预瞄也不能设得太短否则会出现“明明算出来要打35度但机械上只能打到32度差值一直存在”的稳态误差。6.2 低速高精度作业与高速转运的不同侧重工程机械的工作场景非常极端作业时速度低到0.1到0.5m/s但对位置精度要求却很高比如压路机要保证相邻两遍碾压带搭接误差在几厘米以内推土机找平作业要控制铲刀高度和高程。这时候Stanley的快速收敛特性就比Pure Pursuit有优势因为预瞄算法在极低速下需要很小的Ld才能保持精度而Ld太小会让转向动作变得很敏感反而不好调。矿卡、宽体自卸车这类时速能到30到40km/h的大型矿用车辆则更像乘用车的高速场景Pure Pursuit的稳定性和柔和转向特性更有价值。不过矿卡还有一个乘用车没有的变量空载和满载的整车质量差异特别大惯性变化剧烈转向响应的延迟和超调表现完全不同。这种情况下固定增益的Pure Pursuit或者Stanley往往不够用需要根据载荷估算值对预瞄距离或者增益做调度满载时把预瞄拉长一点、收敛增益放得慢一点空载时收回来。工程机械上另一个和乘用车不一样的痛点是传感信息质量。矿山上粉尘大、振动强GPS/RTK的信号遮挡和多路径效应比城市道路严重得多组合导航输出的航向角经常会有一个小幅高频跳动。这对Stanley这种直接使用航向误差的算法影响特别大哪怕横向误差很小航向一跳方向盘就会跟着抖。所以工程机械上用Stanley一定要先给航向信号做滤波或者用一定时间窗口内的平均航向来算误差。而Pure Pursuit使用预瞄点的方式天然对航向噪声不那么敏感鲁棒性会好一些这也是为什么不少矿卡项目最终选用Pure Pursuit而不是Stanley。7. 实车调试中反复踩过的几个坑最后这部分是纯实操经验。下面这几条都是我在项目里真实遇到、并且花了不少时间才排查清楚的问题写出来给大家提个醒。7.1 坐标系、消息时序和控制频率这些“地基”路径跟踪看起来只是算个转角但它依赖的上游数据非常多任何一个环节出问题算法再漂亮也白搭。第一个是坐标系统一。定位模块输出的坐标要跟规划模块的路径坐标在同一个坐标系下通常都是UTM或者ENU这类全局坐标系但实际项目中经常出现GPS用的坐标系、规划模块用的坐标系、控制模块期望的坐标系不一致的情况。尤其是不同厂家提供的定位设备可能默认坐标基准都不同联调时一定要先做坐标一致性校验否则车会一直朝一个方向偏看起来像算法有问题其实是坐标系差着一个平移量。第二个是消息频率和控制频率的匹配。控制模块跑多快的循环频率直接决定了算法参数的取值范围。如果控制频率只有10Hz那么预瞄距离的计算误差会比较大转向指令的执行也会有量化感如果频率能到50Hz以上同样的算法参数会表现得平滑很多。我一般建议控制频率至少20Hz起步50Hz比较理想。此外还要关注消息延迟从定位数据采集到控制模块拿到数据中间经过的通信、滤波、同步环节如果引入几百毫秒延迟那么路径跟踪用的车辆状态就已经“过期”了。这个问题在低速时还不太明显高速时会导致车辆进入弯道后转向总是慢半拍严重的会冲出参考路径。解决思路是给状态打时间戳在控制算法里做位置补偿按延迟时间用车辆运动模型外推一小段当前位姿。7.2 符号约定为什么我的车总往反方向打转向横向误差的方向定义问题是新手最容易踩的坑而且踩了还不容易发现。Pure Pursuit里alpha的符号如果反了车会向路径的反方向一直转很快就冲出路径Stanley里如果叉积判断左右侧的符号约定和航向误差的符号不一致两个误差项就会互相拉扯车会一边走一边轻微画龙最终仍然偏离路径。这种问题的排查方式其实很快先在一个笔直路径上做仿真给车一个初始横向偏移看看转角方向是否让车朝路径修正。如果车越走越偏把横向误差项取个反号再试。真正麻烦的是符号在某个工况下是对的、另一种工况下又变成错的比如倒车场景。Pure Pursuit前进时目标点在车前方倒车时目标点应该在车后方alpha的定义也要跟着翻转。Stanley原始版本设计上就不适合倒车因为它的最近点方向箭头只在前进方向成立。解决倒车跟踪问题时要么把路径反转一下、把模型里航向角加个π再套用原算法要么用其他专门支持倒车的控制律。项目里支持倒车入库这类功能时建议单独设计流程不要在同一个控制律里硬加倒车标志位。7.3 转向执行器的物理限制饱和、速率限制与液压延迟很多控制算法在仿真里调得好好的一上实车就出问题原因往往不是算法本身而是没考虑执行器物理限制。阿克曼车辆的机械转向角有限位常见乘用车前轮转角范围是±30度到±40度货车或者工程机械可能更小。算法算出来的转角如果超过限位必须做饱和处理否则执行器指令会在限位处被硬截断出现转向不足、车辆无法按期望半径转弯的现象。比饱和更隐蔽的是转向速率限制也就是方向盘从当前角度转到目标角度需要时间。如果控制频率很高而执行器的最大转向速度跟不上算法输出的转角变化过于剧烈执行器就会出现跟踪滞后表现为弯道中车辆转向不足、出弯后过度修正。解决方式是给转角输出加一阶低通滤波或者速率限制器把指令变化速率限制在执行器能力范围内。我习惯把滤波截止频率设在2到5Hz左右然后根据实车方向盘响应的表现做调整方向盘高频抖就把截止频率调低进弯转向不够及时就适当调高。液压转向系统比电动转向系统还要多一个延迟问题。液压缸的建立压力需要时间从给出铰接角指令到实际铰接角到位往往有几百毫秒的大延迟。这种情况下单纯调算法的增益很难兼顾稳定性和跟线精度我见过比较有效的做法是串联一个滞后补偿器或者用史密斯预估器的思路把当前实际铰接角反馈进来做修正而不是直接假设“我发了指令转角就立刻到了那个角度”。7.4 调参规律先低速后高速先直线后弯道最后给一个调参顺序的建议。很多人一上来就想在完整路径上把参数调到完美结果横纵向误差混在一起根本看不清是横向算法的问题还是纵向速度配合的问题。我的习惯是先在非常简单的场景里做“变量隔离”让问题暴露得更纯粹。第一步在直线路径上跑低速测试比如1到2m/s。这时候只调基础增益和基础预瞄目标是让车能稳定跟线、不画龙、不跑偏。如果直线都稳不住那问题大概率不在弯道算法上而在符号、滤波或者执行器响应上。第二步保持低速把路径加上一圈半径合适的圆或者大弯道观察车进弯、弯道中、出弯三个阶段的跟踪误差。Pure Pursuit重点看内切量是否过大Stanley重点看出弯后是否有多余振荡。第三步提升速度按速度重新标定Ld或者k观察高速段的稳定性。最后把车辆加速度、减速策略加进来验证横纵向协同情况下算法是否依然稳定。整个调参过程一定要做好数据记录和分析。把参考路径、车辆实际轨迹、横向误差、前轮转角指令画在同一张图上对比着看。很多时候实车问题一眼就能从这些曲线里找到答案而不是靠猜。我见过不少工程师把时间花在反复改参数上结果问题其实出在参考路径更新频率太低或者定位延迟太大这种时候调参数是浪费时间。关于这两个算法我自己实践下来最大的体会是它们虽然“老”但远没有过时。在算力受限、传感器噪声大、执行器响应慢的工程环境里简单可靠的几何算法往往比复杂精细的优化算法更抗造。关键是吃透它们背后的几何直觉和每个参数的真实物理含义之后不管做什么样的载体都能快速上手改造。希望这篇内容对你调试自己的路径跟踪系统有所帮助。
