开始写技术细节之前先说个现象网上很多智能车教程都在讲“循迹”可一遇到圆弧路径就含糊带过要么只贴一段纯追踪代码要么直接让你调参数。真正自己把车放上去跑一圈各种画龙、切弯、出弯冲出去的问题全冒出来。这篇文章我就围绕“智能车跟随圆弧路径原理”这个主题从几何建模、运动学推导讲到工程调参尽量把每一处为什么讲透。不管是正在做电赛、智能车竞赛还是在搞ROS小车、AGV底盘的同学应该都能从中找到能直接落地的方案。1. 为什么智能车要单独研究圆弧路径1.1 圆弧不是一种特殊路径而是路径规划的基础积木很多人有个误区觉得直线路径最简单圆弧路径是“进阶内容”。实际上刚好相反在真实场景里纯粹走直线的机会反而少。路口转弯、绕障、停车场环形通道、倒车入库这些动作拆开看全是圆弧段直线只是半径无穷大的圆弧特例。全局路径规划输出的一般是一堆离散路径点这些点连接起来是折线直接让车沿着折线跑到拐点处就得急打方向速度稍微快一点就冲出路径。所以工程上都会在拐点处做圆弧过渡或者用“直线-圆弧-直线”的路径结构来连接无碰撞路线。也就是说圆弧路径的跟踪能力决定了一台车能不能把规划层的意图真正执行出来。从动力学角度看圆弧还有个特别好用的性质曲率恒定。曲率恒定意味着转向角基本不变、角速度基本不变车辆处于稳态转向状态这对控制设计非常友好。相比之下样条曲线和自由曲线虽然更灵活但曲率在变化控制难度直接上一个台阶。所以几乎所有智能车项目都是先把圆弧段跑稳再去碰更复杂的路径类型。1.2 底盘结构不同圆弧的“表达方式”完全不同开始写代码之前一定要先搞清楚底盘模型。同一个纯追踪公式在差速车和阿克曼车上的落地方案差距很大。差速底盘也就是两轮差速通过左右轮速差实现转向能转的圆弧半径下限很低甚至可以原地旋转R0。控制量是左右轮速圆弧参数和轮速的关系非常直接线速度 v 等于左右轮速平均值角速度 ω 等于左右轮速差除以轮距。要跟半径 R 的圆弧给定目标线速度 v直接算出期望角速度 ω v / R再反解左右轮速就行。阿克曼底盘或者简化的自行车模型则受转向几何限制前轮转角有物理上限所以存在最小转弯半径。控制量是前轮转角 δ与圆弧半径的关系是 R L / tan(δ)其中 L 是轴距。同一段圆弧差速车只需要给轮速差阿克曼车要考虑转角限位、转向执行机构的延迟两者调参思路完全不同。麦克纳姆轮这类全向底盘虽然能平移和旋转解耦表面上看起来“想走什么轨迹都容易”但平滑圆弧依然有价值全向底盘如果直接横移拼出圆弧轮子磨损大、能耗高而且轨迹依然要经过圆弧拟合。所以不管哪种底盘掌握圆弧跟踪的核心原理都绕不开。2. 圆弧路径的几何与运动学基础2.1 用曲率思考而不是只用半径圆弧最直观的描述是圆心和半径R但做控制时我更建议把“期望路径”转换成“期望曲率”。曲率 κ 1 / R单位是 1/m直线就是 κ0半径1米的弯是 κ1半径0.5米的急弯是 κ2。为什么曲率视角更好用因为车辆运动学里角速度 ω 和线速度 v、曲率 κ 之间有一个非常简洁的关系ω v·κ。差速底盘的左右轮速差本质上就是输出角速度阿克曼底盘的前轮转角本质上也是通过几何关系对应一个曲率。所以“跟踪一段圆弧”这件事可以统一表述成“让车辆的瞬时曲率去逼近目标曲率”后续接纯追踪、前馈控制或MPC都是在这个框架上做文章。计算横向误差时圆弧的圆心坐标和半径就派上用场了车辆位置到圆心的距离减去半径就是径向误差再结合车辆在圆弧的哪一侧确定符号。这个“符号约定”看着简单实际写代码时非常容易出错我后面会专门强调。还有个工程细节圆弧是有方向的。逆时针和顺时针的圆弧圆心角增量一正一负参考点选取、横向误差符号、转向方向全都不一样。代码里一定要带上方向标志位比如 direction1 表示逆时针direction-1 表示顺时针不要靠半径正负去猜。2.2 自行车模型下的转向几何推导自行车模型是路径跟踪里最常用的等效模型把一台车的两个前轮等效成一个前轮两个后轮等效成一个后轮轴距为 L前轮转角为 δ。在低速、无侧滑的假设下车辆转弯半径满足R L / tan(δ)所以给定目标圆弧半径 R期望前轮转角为δ arctan(L / R) arctan(L·κ)这里用真实数据感受一下假设轴距 L 0.3m目标半径 R 1m代入得 δ arctan(0.3) ≈ 16.7°。如果半径变成 0.5mδ arctan(0.6) ≈ 31°。轴距越长同样半径需要的转角越大。阿克曼底盘的左右轮实际转角并不一样外侧轮转角小于内侧轮这是为了保证四个轮子的转向中心交于同一点。但自行车模型的等效单轮转角已经足够用于控制只要把 δ 当作“等效前轮转角”再根据阿克曼比例分配到左右轮即可。很多小车为了省事不做阿克曼几何修正低速下影响不大速度快了就会有轮胎侧偏导致的轨迹偏移这个要用实测数据去标定补偿。差速底盘的推导同样简单设左轮速 v_l、右轮速 v_r、轮距 d则v (v_l v_r) / 2 ω (v_r - v_l) / d要跟踪半径 R 的圆弧且保持线速度 v期望角速度 ω_d v / R反解左右轮速v_r v ω_d·d / 2 v_l v - ω_d·d / 2再举个数v 1m/sR 1m轮距 d 0.2m则 ω_d 1 rad/s左右轮速差为 0.1m/s即左轮0.95 m/s、右轮1.05 m/s。这个量级在电机控制里接近死区吗很多小车轮速控制精度不够就会出现“想走圆弧但实际半径飘忽不定”的问题后面排查章节会提到。2.3 圆弧跟踪的误差定义横向误差和航向误差闭环控制必须先定义误差。圆弧跟踪通常看两个量横向误差 e车辆位置到目标圆弧的最短距离数学上等于 |distance(P, C) - R|符号由车辆相对圆弧的位置决定。航向误差 Δθ车辆当前航向与目标圆弧在当前投影点处的切线方向之差。我习惯的符号约定是以期望行驶方向为参考车辆偏向圆弧外侧时横向误差为正偏向内侧为负。这个约定跟车辆控制的方向直接相关e 0 说明离圆心太远需要增加转向角来拉回来e 0 说明切得太狠需要减小转向角。符号不统一是新手写圆弧跟踪最容易犯的错误。我曾经见过一个项目横向误差的符号差一个负号结果车在圆弧上走了一条“外八”轨迹像是喝醉了在转圈。排查半天最后发现是对误差方向的定义跟转向角的符号不一致。所以建议在代码里把误差定义写清楚并给横向误差和转向角各画一张方向示意图避免后续调试时绕晕。3. 圆弧路径跟踪控制纯追踪方法实战3.1 纯追踪算法在圆弧上的几何直觉纯追踪算法的核心很朴素人走路时不会一直盯着脚下而是看着前方某个目标点然后不断调整方向朝那个点走。车也一样拿当前车辆位置为圆心以“前视距离”L_d为半径画一个圆和目标圆弧求交点这个交点就是“视线目标点”。车辆只做一件事——转向让车头朝这个目标点。这个策略为什么能在圆弧上工作因为只要目标点始终落在目标圆弧上车辆就会不断修正自己的方向去贴合圆弧。前视距离越长修正越柔和轨迹越平滑但响应越慢前视距离越短修正越猛烈但容易振荡。圆弧跟踪比直线跟踪更难调参就在于 L_d 还要跟圆弧半径 R 匹配R 很小的时候L_d 太大目标点直接“跨过”圆弧控制方向完全和圆弧切线脱节。3.2 圆弧上的参考点怎么找这是纯追踪在圆弧上最需要靠代码细节的地方。目标点是“以车辆位置为圆心、L_d为半径的圆”与“目标圆弧”的交点理论上要解一个二次方程得到最多两个交点再根据行驶方向选一个。工程上我喜欢用更简单的“极角推进法”先把车辆当前位置投影到目标圆弧上得到投影点对应的圆心角 φ_proj然后沿期望方向增加一个角度步长 Δφ使得对应的弧长接近 L_dΔφ direction · L_d / R新的参考点就是target_x C.x R·cos(φ_proj Δφ) target_y C.y R·sin(φ_proj Δφ)这个近似的代价是当 L_d 相对 R 较大时参考点会稍微落在圆弧外侧解决办法是给 Δφ 设一个上限。我一般限制最大步进角在 0.5~0.6 弧度约30度以内防止大前视时参考点跑到圆弧另一侧去。如果追求更准可以把参考点再次投影回圆弧上迭代两三次精度就足够工程用了。需要注意的是车辆当前位置往往不在目标圆弧上先按极角投影会引入一点误差。对绝大多数低速智能车来说这个误差可以忽略但如果你做的是高精度跟踪比如误差要求小于5cm建议用“过车辆位置且与目标圆弧相切的近似圆弧”来选参考点或者直接用二次方程求精确交点。3.3 从参考点到控制量的完整计算拿到参考点坐标后计算参考点在车辆坐标系下的方位角 α也就是参考点相对车辆的方向与车辆当前航向之差然后套纯追踪的核心公式期望曲率 κ_d 2·sin(α) / L_d这个公式的来源并不神秘它相当于让车辆以参考点为“目标”实时计算出为了追到这个点车辆应该产生的曲率。从期望曲率换算到控制量就取决于底盘差速底盘ω_d v·κ_d v_r v ω_d·d / 2 v_l v - ω_d·d / 2阿克曼/自行车模型δ_d arctan(κ_d·L)我给出一个可参考的差速底盘Python实现这段代码我实测过直接套用就能跑import math def normalize_angle(a): while a math.pi: a - 2.0 * math.pi while a -math.pi: a 2.0 * math.pi return a def diff_wheel_speed(v, omega, wheel_track): 由线速度、角速度反解左右轮速 v_r v omega * wheel_track / 2.0 v_l v - omega * wheel_track / 2.0 return v_l, v_r def arc_pure_pursuit( x, y, yaw, center, radius, lookahead, direction, v, wheel_track ): center: 圆心坐标 (cx, cy) direction: 1 表示逆时针-1 表示顺时针 # 1. 车辆位置相对圆心的极角 phi math.atan2(y - center[1], x - center[0]) # 2. 沿期望方向推进角度步长弧长约等于 lookahead d_phi direction * (lookahead / max(radius, 1e-6)) max_d_phi 0.6 # 上限约 34 度 if abs(d_phi) max_d_phi: d_phi direction * max_d_phi target_phi phi d_phi # 3. 目标参考点 tx center[0] radius * math.cos(target_phi) ty center[1] radius * math.sin(target_phi) # 4. 参考点相对车辆的方位角 alpha math.atan2(ty - y, tx - x) - yaw alpha normalize_angle(alpha) # 5. 纯追踪期望曲率 curvature 2.0 * math.sin(alpha) / max(lookahead, 1e-6) # 6. 差速底盘转为角速度和左右轮速 omega v * curvature v_l, v_r diff_wheel_speed(v, omega, wheel_track) return tx, ty, omega, v_l, v_r阿克曼底盘的版本只需要把第6步换成steering math.atan(curvature * wheel_base)然后记得对转向角做饱和处理别超过舵机物理限位。这段代码的核心是把“路径跟踪”转换成了“每帧计算一个期望曲率再转换到执行机构”思路清晰排查问题也方便。实际部署时建议加上低通滤波处理航向角并把最终控制量的变化速率限制一下避免执行器忽快忽慢。3.4 前视距离怎么调从振荡到画龙的完整流程前视距离 L_d 是纯追踪里最重要的参数我踩过很多次坑这里给出最直接的经验公式和调参顺序。经验公式上L_d 建议与速度成正比再加一个下限L_d k_v · v L_min低速差速小车v 在 0.3~0.5m/s一般取 L_d 0.3~0.6m阿克曼车跑到 1~2m/s取 L_d 0.8~1.5m 比较稳。k_v 通常取 0.3~0.5L_min 取 0.2~0.4m。圆弧半径对 L_d 的约束是硬条件当 L_d 超过约 0.4R 时参考点会明显脱离圆弧切线整车轨迹开始向内切。半径1m的圆弧L_d 最好不要超过0.4~0.5m。这也是为什么很多实车项目里直线段跑得挺好一进弯就切内侧因为前视距离没按半径压缩。调参步骤我建议按这个顺序走先用仿真固定速度把 L_d 从0.2m开始往上扫找一个既不高频振荡、又能跟上圆弧的值。实测时如果看到车身来回鼓动画龙先加大 L_d同时降低速度。如果看到轨迹切内线把 L_d 调小或增加前馈控制。最后处理高速场景L_d 随速度增长的那一段要通过实车数据多标定几次不能只靠感觉。3.5 只靠纯追踪不够前馈加反馈的工程组合纯追踪说到底是个误差反馈算法它天生有滞后。直线路径上滞后不明显但进入大曲率圆弧时纯追踪往往要先把横向误差“攒”起来才会给足转角表现出来就是入弯反应慢、出弯过度修正。工程上更稳的做法是“前馈反馈”既然已经知道目标圆弧的半径 R期望转角本来就能直接算出来好像不需要反馈了——但实际有执行器误差、轮胎侧偏、里程计噪声所以前馈保证“稳”反馈保证“准”。阿克曼底盘的控制量可以写成δ arctan(L / R) Kp·e其中第一项是来自圆弧半径的前馈转角第二项用横向误差 e 修正偏差。差速底盘类似ω v / R Kp·e注意这里的符号依然要跟误差定义严格统一。我自己实测下来加了这个前馈项之后进入圆弧的瞬间转向角就不是“从0慢慢追”的状态而是一上来就接近正确值横向误差直接小了一个量级。Kp 别取太大低速车一般 1.0~3.0 之间起步观察振荡再微调。如果系统有静态误差可以加积分项但务必加抗积分饱和否则出弯时积分量会让车走歪。这种“已知几何结构就前馈偏差部分反馈”的思路其实也适合扩展到更复杂的路径只要能求出当前点的目标曲率前馈项就有了反馈项负责把误差压下去。4. 实际工程中的参数标定与问题排查4.1 常见问题速查表圆弧跟踪的常见问题其实高度集中我把它们整理成一张速查表调试时按图索骥会快很多现象可能原因排查方法解决建议车身来回画龙、高频振荡前视距离过短增大L_d观察振荡频率是否下降用 L_dk_v·vL_min并对航向做低通滤波轨迹切向圆弧内侧前视距离相对半径太长回放数据检查参考点是否远离圆弧切线限制 L_d ≤ 0.4R或加入前馈曲率入弯反应慢、冲出圆弧起点控制滞后或前馈缺失对比期望曲率与实际曲率计算控制量时加入 arctan(L/R) 前馈项实际轨迹半径比目标大转向执行机构饱和或舵机增益不足检查转向角是否持续顶到限位标定舵机角度-实际转角曲线做饱和处理车辆抖动但不前进α符号判断错误参考点选到了反向交点打印alpha、参考点坐标统一方向标志位补角度归一化函数里程计航向噪声太大陀螺仪原始数据未滤波观察 yaw 曲线毛刺加低通滤波或滑动窗口滤波这张表看上去短但每一条背后都有一个“折腾半天最后发现是小问题”的故事。比如切弯问题新手第一反应往往是调转向增益实际上多数情况是前视距离没按半径压缩。再比如车辆抖动但不前进代码逻辑看起来完全正确最后发现是 atan2 返回值没有归一化到 [-π, π]α 跳变一下控制量就从正向打到反向车当然在原地哆嗦。4.2 我踩过的坑前视距离与执行器延迟的耦调前视距离不是孤立参数它和转向执行机构的延迟是强耦合的。舵机、电机都有响应时间纯追踪给出的期望转角是阶梯变化的如果舵机追不上这个阶梯实际转角一直滞后于期望转角整个系统就会进入持续振荡。有一次测试差速小车v0.6m/sR1.5m前视距离固定0.5m车始终在圆弧外侧画大圈。我一开始以为是横向误差系数不够把Kp从1.5调到3.0结果更糟转弯过猛开始画内圈。后来录下舵机指令和实际转角发现舵机响应延迟大约80ms而纯追踪每个控制周期20ms都在更新目标舵机永远追不上指令。解决办法有两个一是控制周期中加入预测用车辆当前速度把未来位置算出来再去选参考点二是对期望转角做斜坡限制让舵机可以追得上。第二个方案见效最快把本来80ms的“舵机追指令”问题变成“控制量按最大角速度走”系统立刻稳定下来。还有一次是在铺满环氧地坪的场地上测圆弧发现同样的参数今天能跑、明天不能跑原因是轮胎磨损后有效半径变大、侧偏特性变了。这种情况不要硬调控制参数先给轮胎充气或者替换轮胎再重新标定。电机齿轮间隙、履带打滑这些机械因素也会体现为“轨迹半径偏大”排查顺序永远是先机械后算法。4.3 调试数据回放四曲线法定位问题圆弧跟踪调试时我强烈建议把四路数据录下来画在同一个时间轴上目标圆弧、车辆实际轨迹、当前参考点、控制量输出。不要只看车辆轨迹因为轨迹只能告诉你“不对”看不出“哪里不对”。举个例子如果目标圆弧是条完美圆车辆轨迹也是条圆但半径偏大看四曲线图就会发现参考点始终在车辆前方偏外侧控制量一直在往内侧打可就是打不够。这说明要么执行器响应慢要么转角增益小。如果车辆轨迹在进入圆弧的瞬间出现一波外扩看曲线会发现参考点在入弯时跳到了很远的地方这往往是投影点算法在车辆离圆弧较远时失效了。数据回放还有一个隐藏好处它能把“控制周期抖动”这种偶发问题暴露出来。有些问题真车上只出现零点几秒肉眼根本看不出来但回放曲线里会出现明显的毛刺这种毛刺多半是航向角更新率不匹配或者里程计时间戳抖动导致的定位起来非常快。4.4 可复制的参数模板与标定步骤最后给一个我在差速小车上实测过的参数模板作为起步参考不同底盘一定要重新标定不能照抄参数推荐值说明目标线速度 v0.8 m/s圆弧上速度建议比直线低20%左右轴距 / 轮距差速0.18m / 0.16m视底盘而定前视距离L_d 0.4 0.25·v低速起步用最高不超过0.6m目标圆弧半径1.2m测试标准弯前馈角速度v / R差速底盘直接加反馈增益 Kp1.8调大看振荡调小看切弯控制周期50ms与电机驱动频率匹配标定步骤建议这样先让车沿着半径2.0m的大圆弧跑保证起步稳定。逐步减小目标半径直到车出现明显的指向内侧趋势记录这个半径和对应的L_d。把 L_d 改成 0.3R、0.4R、0.5R 三组分别在同一个圆弧上测试选出横向误差最小的一组。加前馈项再微调Kp通常一次就能稳定。这套流程走下来你对这台车的“性格”会有很直观的认识舵机快不快、轮胎抓得住不抓得住、里程计噪声大不大全都会暴露出来。最后分享一点个人体会圆弧跟踪做到后面真正决定成败的往往不是算法本身有多高级而是“前视距离怎么跟半径配合”“执行器延迟怎么补”“符号约定怎么统一”这三件事。纯追踪、前馈控制、甚至MPC都是工具把这三个工程问题处理好了哪怕你用的只是几十行纯追踪代码一样能跑出漂亮轨迹反之算法再花哨符号差了就满盘皆输。
