做机器人路径跟踪这几年，我最大的感受是：真正考验算法的不是直道，而是圆弧。很多项目跑直线漂亮得很，一到转弯就露馅，要么切内弯蹭墙，要么外抛压线，车身姿态像喝醉了酒。智能车跟随圆弧路径这个主题，看着基础，其实把运动学建模、几何运算、反馈控制、速度规划全部串到了一根链条上：先理解底盘怎么动，再把圆弧换算成几何量，最后设计控制器把偏差压下去。这篇文章从原理讲到参数整定，把我调试过程中反复验证过的那套方法完整写出来。适合刚接触路径跟踪、被"转弯跑不圆"困扰的同学参考，也适合想系统理解纯跟踪、曲率前馈这些概念的同行拿来对照。
1. 圆弧路径跟踪到底在解决什么问题
1.1 直线模型不够用，圆弧才是常态
很多人第一个跟蹤demo是走直线，拿一个比例P对着横向误差打方向盘就够了。原因很简单：直线的切线方向不变，航向误差近似常数，纯比例反馈就能收敛。但把同一套逻辑搬到弯道里问题立刻暴露——弯道上轨迹的切线方向时刻在变，航向误差不是一个静止的量，靠"错了再补"的反馈思路会在弯道里形成持续的滞后侧偏，车身总比理想圆弧慢半拍。
这个问题用曲率语言描述更清楚：直线是曲率为0的特例，圆弧是曲率不为0的一般情况。更关键的是，任意一条光滑路径在足够小的局部范围内都可以近似成一段圆弧，这就是曲率圆的思想。所以把圆弧跟明白了，曲线路径本质上就是无数小段圆弧的拼接。
开车走高速公路弯道时，熟练司机的视线从不落在车头正前方，而是提前瞄向弯道内沿的某个出弯点，手提前打方向，人眼做的就是"前视+预判"。机器算法要向人这个行为学习，而不是等压线了再猛打一把。
1.2 底盘运动学是一切计算的基础
做控制之前必须先搞清楚底盘是哪一种，因为输出指令完全不同。最常见的两种是差速底盘和阿克曼底盘。
差速底盘：两个驱动轮加一个从动轮，左右轮线速度分别记为vL和vR，轮间距记为b，那么底盘的线速度和角速度是：
- 线速度：v = (vL + vR) / 2
- 角速度：ω = (vR - vL) / b
一个控制周期dt内，车辆位置更新可以写成：
- x += v·cos(θ)·dt
- y += v·sin(θ)·dt
- θ += ω·dt
这一组式子几乎是所有路径跟踪推导的地基。
阿克曼底盘：前轮转向角δ，轴距L，低速工况下曲率κ约等于δ/L，也就是说转向角与曲率近似线性。这类底盘输出的是(v, δ)而非(v, ω)。
我在自己常用的验证平台上跑的是差速底盘，轮距0.23m，控制周期20ms，后面例子都按这个配置算。实际调试中很多人忽略轮距测量误差，结果航迹推算和真值越差越大，圆弧跟随做得再好也白搭，这段一定要提前校准。
1.3 圆弧跟随的终极目标
把"跟随一条圆弧"翻译成数学语言：让车辆的位置偏差e_y（到圆弧弧线的垂距）接近0，航向偏差e_ψ（车头方向与圆弧切线方向的夹角）接近0。
由运动学关系可知，理想圆弧运动中，线速度和角速度必须满足：
ω = v·κ
其中κ = 1/R是圆弧曲率。这个等式是整个圆弧跟随最核心的一条公式，它告诉我们：圆弧跟踪不是拼命纠偏，而是一边按曲率给出稳态控制量，一边只修正小偏差。脑子里有了这个结构，后面看到PID和前馈的组合就不会乱。
2. 圆弧路径的几何怎么算
2.1 路径数据从哪来，怎么拟合出圆弧
实际项目里，圆弧通常不是直接给定的，全局路径往往是A*、RRT这类规划器生成的折线，经过平滑后变成一列密集点，需要从点列里识别出圆弧段并算出半径。最常用的方法是三点定圆。
过三个点(x₁,y₁)、(x₂,y₂)、(x₃,y₃)的圆，圆心(x₀,y₀)到三点距离相等，联立两条中垂线方程就能解出来。代码很短，示意如下：
import numpy as np def circle_from_three_points(p1, p2, p3): p1, p2, p3 = map(np.asarray, (p1, p2, p3)) a = p2 - p1 b = p3 - p1 # 中垂线交点求解 A = np.array([2*a, 2*b]) rhs = np.array([np.dot(a, p1+p2), np.dot(b, p1+p3)]) center = np.linalg.solve(A, rhs) radius = np.linalg.norm(center - p1) return center, radius一个常见坑：如果三个点选得太近或者近乎共线，圆心会飘，半径可能计算出几十米的虚假值。我一般要求三个点的间距不小于3到5个控制周期内车辆行进的距离，点比较密的情况下用最小二乘圆拟合会更稳。
2.2 关键几何参数：半径、弧长、起始角、弓形高
拿到圆心和半径之后，圆弧段还需要用起点角、终点角来确定到底跟随哪一段。
| 参数 | 符号 | 计算方法 | 物理意义 |
|---|---|---|---|
| 圆心坐标 | (x₀, y₀) | 三点定圆/最小二乘 | 圆弧所在圆的中心 |
| 半径 | R | 圆心到任一点的距离 | 弧线弯曲程度 |
| 起点角 | φ_start | atan2(y₁-y₀, x₁-x₀) | 起点相对圆心的极角 |
| 终点角 | φ_end | atan2(y₃-y₀, x₃-x₀) | 终点相对圆心的极角 |
| 转过角度 | Δθ | 规范化到(-π, π] | 弧段张角，核心目标量 |
| 弧长 | s | R·Δθ | 路径里程，用于前视插值 |
| 弦长 | d | R·√(2-2cosΔθ) | 直线距离 |
| 弓形高 | h | R - R·cos(Δθ/2) | 弧线到弦的最大距离 |
这里我特别注意用弧长s做路径索引而不是弦长。控制计算里要沿路径前进找前视点，用弧长参数化后点列均匀，不会因为原始采样疏密不均导致前视距离抖动。车走起来之后，前视距离的变化直接反映在方向指令的抖动上，这是新手最容易忽略的一步。
2.3 用曲率统一表达，路径瞬间简单
曲率κ=1/R，单位是1/m。直线可以看成κ=0的圆弧，半径无限大。这个统一的直接好处是：算法层不需要区分"直线模式"和"圆弧模式"，同一套曲率前馈通吃。
任意曲线在某点附近都可以用密切圆近似，它的切向、法向和半径都继承自该点的局部几何。于是曲线跟踪可以拆成一连串瞬时圆弧来处理，这就是样条插值和贝塞尔曲线规划的理论基础。
再做一步预处理：如果原始点列疏密不均，前视点在推进时会忽远忽近，纯跟踪输出跟着抖。我用等弧长重采样，间隔取0.05m左右，让车走起来手感稳定很多。
3. 控制算法选型与推导
3.1 主流的跟踪思路对比
| 算法 | 核心思路 | 参数 | 圆弧场景评价 |
|---|---|---|---|
| PID反馈 | 对横向/航向偏差做线性反馈 | 2到3个增益 | 简单，但纯反馈有稳态滞后 |
| 纯跟踪 | 几何构造相切圆 | 前视距离ld | 圆弧表现很好，参数少 |
| Stanley | 前轮位置+航向误差几何控制 | 增益k | 直道收敛快，弯道前视不敏感 |
| MPC | 滚动优化，带约束 | 多，需要调权重 | 效果好，算力高，复杂度大 |
对智能车圆弧跟随这个场景，我的结论很明确：纯跟踪加曲率前馈是性价比最高的组合，它只需要一个前视距离参数，调试成本最低，圆弧上精度完全够用，而且能在低算力MCU上跑到50Hz以上。
3.2 纯跟踪：把问题变成"求一个相切圆"
纯跟踪的几何推导非常漂亮。以车辆后轴中心P为基点，沿前进方向前方ld处找路径上的目标点G。所谓纯跟踪，本质是构造一条经过P和G的圆弧，并且圆弧在P点的切线方向与车辆当前航向重合。
设车辆航向与PG连线的夹角为α（目标点在车头左侧为正，右侧为负），弦PG长度为ld。由弦切角定理，弦所对的圆心角是弦切角的2倍，所以半径R满足：
R = ld / (2·sinα)
转换曲率就是：
κ = 2·sinα / ld
遇到α=0时曲率命令为0，车走直线；ld必须大于0，低速下要设下限。这个公式就是纯跟踪的全部秘密。
生活里骑自行车的时候，你眼睛盯着前方某个点，车把自然就会画出一条弧线把自己送过去，你盯得越远，弧线越平缓；盯得越近，车把越灵活。纯跟踪的ld就是这个"目光的距离"。
代码实现就是把公式套进去：
def pure_pursuit(target_point, ld, heading): alpha = normalize_angle(atan2(target_point.y, target_point.x) - heading) curvature = 2.0 * sin(alpha) / ld return curvature # 左转为正如果前视点超出路径末端，按末端点处理并降速，避免目标点丢失导致指令跳变。
3.3 PID加前馈：偏差怎么压下去
纯跟踪输出的是期望曲率，但实际底盘还需要一个跟横向偏差挂钩的修正量，因为前视点是一个"远处目标"，短距离内误差仍然可能存在，不能指望它完全自纠。
工程上我常用的控制结构是：
控制量 = Kp_e·e_y + Kd·e_y' + Kp_ψ·e_ψ + 曲率前馈κ_ff
- e_y：车辆到目标圆弧的垂距，右偏为正
- e_y'：垂距的微分
- e_ψ：车头方向与圆弧切线方向的夹角差，要处理角度环绕
- κ_ff：目标圆弧的命令曲率
前馈部分，阿克曼底盘直接换算成转向角δ_ff = atan(L·κ) ≈ L·κ；差速底盘直接把角速度指令写成ω_cmd = v·κ。
为什么一定需要前馈？拿PID做纯反馈时，弯道处要等误差积出来才开始补方向，稳态时车身与理想圆弧之间永远差一个滞后偏距。曲率前馈直接注入控制量后，车体带着"预转向"进入弯道，横向偏差才能真的逼近0。类比骑摩托过弯，重心和方向把必须先于弯道调好，而不是看到差距才猛打方向。
4. 参数计算与实际部署
4.1 前视距离怎么定，公式和试凑
前视距离是纯跟踪唯一的参数，直接影响过弯性格。
ld太小，车盯得近，方向修正频繁，容易蛇形。ld太大，车反应慢，短圆弧直接"切弦"抄近路，表现为外抛压线。我用的经验公式是：
ld = k·v² + ld₀
k在0.1到0.5之间，ld₀在0.5m到1.5m之间。平方项的意义在于：速度高时制动距离和控制器延迟对应的"远见距离"也要更大，所以它是随速度二次增长的。
算例：v=1.2m/s，k=0.3，ld₀=0.8m，那么ld=0.3×1.44+0.8≈1.23m。
前视还要和路径最小曲率半径做一次匹配，ld最好不要超过0.5倍的R_min，否则在最小弯里前视点可能跨过弧段中心甚至跑到弧外去。
| 车速 | 前视距离 | 效果 |
|---|---|---|
| 0.5m/s | 0.75m | 偏保守，过弯稳但慢 |
| 1.0m/s | 1.0m | 适中，推荐起步值 |
| 1.5m/s | 1.4m | 偏激进，适合熟练调参 |
| 2.0m/s | 1.8m | 高速大弯专用 |
4.2 速度规划如何配合曲率
圆弧路径上，车速直接决定跟不跟得住。允许最大速度受横向加速度限制：
v_max = √(a_lat_max·R)
a_lat_max一般取1.5到3.0 m/s²，要看轮子摩擦力和整车稳定性。算例：R=2.0m，限制取2.5m/s²，那么v_max=√5≈2.24m/s。
如果车载惯性测量单元显示横向加速度长期超过设定值，说明速度规划没有匹配圆弧半径，磨损和侧翻风险都会上来。我自己的流程是：先按路径曲率生成速度曲线，弯前减速、弯中匀速、出弯加速，再做跟踪，而不是先满速测算法。弯前减速采用梯形加速度曲线，提前一个减速距离开始处理，这个距离一般按v²/(2a_dec)计算，再留0.3s反应余量。
4.3 现场调参流程复现
下面是我在验证平台上跑的一段真实调参记录，目标圆弧半径R=2.0m，基准速度1.0m/s起步。
第一步：关闭前馈，令D=0，P从0.3开始观察e_y收敛曲线。P=0.3时收敛慢，加到0.8时车依然稳定，但入弯初期有超调。
第二步：加D=0.1，超调被明显压下去，但弯中横向偏差稳定在0.12m左右，这就是纯反馈的稳态滞后，说明反馈再加大也可能震荡。
第三步：叠加曲率前馈。系数从0.5开始逐步加到1.0，稳态偏差从0.12m降到0.04m，此时横摆角速度曲线变得平滑，车体画弧的姿态明显顺了。
第四步：速度提到1.5m/s，重新按公式把前视提高到1.16m，微调Kp到1.0、D到0.15，全程最大横向偏差可以控制在0.05m以内。
| 阶段 | P | D | 前馈系数 | 速度 | 最大横向偏差 |
|---|---|---|---|---|---|
| 纯反馈起步 | 0.3 | 0 | 0 | 1.0m/s | 明显超调 |
| 加微分 | 0.8 | 0.1 | 0 | 1.0m/s | 0.12m |
| 加前馈 | 0.8 | 0.1 | 1.0 | 1.0m/s | 0.04m |
| 提高速度 | 1.0 | 0.15 | 1.0 | 1.5m/s | 0.05m |
调参过程有几个经验：先低速把反馈调稳再加前馈，前馈系数从0.5往上试探而不是一步到位；速度变了前视必须跟着变；每次只改一个参数，记录目标曲率和实际曲率的偏差曲线再决定下一步。
5. 常见问题与排查技巧实录
5.1 症状对照表
| 症状 | 常见原因 | 排查顺序 | 解决措施 |
|---|---|---|---|
| 直道蛇形 | D过大放大了噪声、前视过小、控制周期过长 | 先看e_y日志是否有毛刺，再增ld，最后降D | 滑动平均滤波，ld调大 |
| 弯道外抛 | 入弯速度过高、前视距离太大 | 先查速度曲线是否提前减速，再调小ld | 按v_max=√(a·R)限速 |
| 弯道切内 | 前视距离过小、前馈系数过大 | 按公式重算ld，再看前馈 | ld调大，前馈系数降到0.7开始试 |
| 跟踪发散 | 曲率符号反了、角度未归一化、控制周期和底盘不匹配 | 先查符号约定，再看角度处理 | 统一坐标系后重测 |
5.2 几个反复踩坑的案例
案例一：前视距离固定0.6m，在R=1.5m的弯道里车一直切内。算一下0.6/1.5=0.4，前视点跨过了弧段中心位置，稳态α偏大，纯跟踪给出的命令曲率比目标曲率大，车自然就往里切。换成ld=k·v²+ld₀动态前视后恢复正常。
案例二：D项增益过高，弯道里超调确实没了，但方向指令持续振荡。抓数据发现是横向误差的微分把角速度噪声放大了，控制周期20ms下噪声在每周期都有几个单位的跳变。解决方法是先对e_y做滑动平均滤波，窗口取3个点，再把D从0.2降到0.1。
案例三：前馈符号错误。车在圆弧上反而持续往外偏，排除了速度、前视、增益，最后发现是我读的曲率符号约定和底盘正方向定义不一致。花了整整两天才定位，从那以后我把符号约定写成注释贴在代码文件头，再也不靠脑子记。
5.3 一个实用的离线复盘小工具
最推荐的问题定位方法不是盯着车看轨迹，而是把日志里目标曲率、实际曲率和横向偏差画在同一张图上。用Python的绘图库，横轴时间，纵轴曲率，目标曲率是一条恒定线，实际曲率在哪段没跟上、超调了多少一目了然。
读图的经验：实际曲率出现周期性波动，多半是前视或增益不合适；实际曲率整体低于目标线，说明车辆在整体外抛；过弯段实际曲率延后才跟上，说明前馈不足或底盘响应慢。这个小工具我一直在用，比肉眼判断车体轨迹快得多。
做圆弧跟随这几年，我自己的体会是算法原理几天就能讲完，真正值钱的是"几何量→曲率前馈→偏差反馈→速度规划"这条链路意识，缺一环都会在弯道上现形。最后再分享一个土办法：调参时在车头车尾贴上两张不同颜色的标尺，用俯拍图像观察轨迹外缘和内缘是不是严格落在目标圆弧两侧，能快速验证左右轮速分配和转向响应是否对称，这招我到现在还在用。
单调栈解法)
