2、几何模型推导：阿克曼转向几何、自行车模型、纯跟踪算法的几何关系推导
好，咱们直接进入正题。
纯跟踪算法，说白了就是让车去追一个前方的目标点。但车怎么转、转多少，这背后全是几何关系。我当年刚接触这个算法时，觉得公式很简单，不就是个圆弧嘛。结果一上车，车就是走不直。后来才发现，我对底层的几何模型理解太浅了。
这一节，咱们就把三个核心模型掰开揉碎讲清楚：阿克曼转向几何、自行车模型、以及纯跟踪算法的几何推导。你想想看，这三个东西其实是层层递进的。
2.1 阿克曼转向几何：为什么四个轮子不能随便转？
先问一个问题：汽车转弯时，四个轮子的转角一样吗？
答案是不一样。如果四个轮子转同样的角度，内侧轮和外侧轮会「打架」，轮胎会严重磨损，车也转不顺畅。这就是阿克曼转向几何要解决的问题。
核心思想：转弯时，所有车轮的轴线延长线应交于同一个圆心点。
我举个例子。你想象一辆车在绕圈，内侧轮走的路径半径小，外侧轮走的半径大。为了让它们都不打滑，内侧轮的转角必须比外侧轮大。这个几何关系，就是阿克曼原理。
具体公式长这样：
cot(δ_o) - cot(δ_i) = L / W其中：
- δ_o：外侧轮转角
- δ_i：内侧轮转角
- L：轴距（前后轮距离）
- W：轮距（左右轮距离）
嗯，这里要注意。实际项目中，我们很少直接用这个公式去控制每个轮子。因为大多数控制算法都基于简化模型——也就是接下来要讲的自行车模型。
个人经验：我在做实车标定时，发现阿克曼率如果偏差超过5%，轮胎在低速大转角时就会发出「吱吱」的异响。所以，如果你在做仿真，可以忽略这个细节；但上实车前，一定要检查阿克曼几何的标定。
2.2 自行车模型：把四轮车简化成两轮
为什么要用自行车模型？说白了，四个轮子太复杂了。控制算法需要实时计算，每毫秒都要算一次转角，用阿克曼几何去算四个轮子，计算量太大。
自行车模型的核心假设是：把左右两个前轮合并成一个虚拟前轮，左右两个后轮合并成一个虚拟后轮。这样，四轮车就变成了一个两轮自行车。
这个模型有三个关键参数：
- L：轴距（前后虚拟轮的距离）
- δ：前轮转角（虚拟前轮的转角）
- R：转弯半径
它们之间的关系很简单：
R = L / tan(δ)你想想看，这个公式意味着什么？意味着只要知道轴距和转角，就能算出车会转多大的弯。反过来，知道想要的转弯半径，也能算出需要的转角。
避坑指南：我曾经在高速场景下吃过亏。自行车模型在低速时（比如泊车、园区低速巡航）非常准，误差在2%以内。但车速超过60km/h时，轮胎侧偏效应开始显现，自行车模型的误差会急剧增大。所以，高速场景下，建议引入动力学模型进行补偿。
2.3 纯跟踪算法的几何推导
好，重头戏来了。纯跟踪算法，本质上就是利用自行车模型，让车沿着一条圆弧去追一个目标点。
先看一张图，我手绘了一个SVG，帮你理解这个几何关系：
这张图里，最关键的是三个量：
- 前视距离 Ld：从后轴中心到目标点的直线距离
- 转弯半径 R：车辆需要走的圆弧半径
- 角度 α：车辆朝向与目标点方向之间的夹角
根据正弦定理，我们可以推导出核心公式：
R = Ld / (2 * sin(α))然后结合自行车模型的公式：
δ = arctan(L / R)把 R 代入，得到最终的控制律：
δ = arctan(2 * L * sin(α) / Ld)这个公式，就是纯跟踪算法的灵魂。你只要输入目标点位置和当前车辆姿态，就能算出前轮转角。
关键理解：α 越大，说明目标点偏离车头方向越远，需要的转角就越大。Ld 越大，说明目标点越远，转角就越小，车走得越「稳」。
2.4 前视距离的玄机
公式里有个参数 Ld，也就是前视距离。这个值怎么选？
我个人的习惯是：Ld 不能是固定值。车速快的时候，Ld 要大一些，让车看得远，转向平缓；车速慢的时候，Ld 要小一些，让车能灵活转弯。
常用的经验公式：
Ld = k * v + Ld_min其中：
- v：当前车速
- k：比例系数（一般取 0.3~0.8）
- Ld_min：最小前视距离（防止车速为0时 Ld=0）
| 场景 | 车速范围 | 推荐 Ld 范围 | 说明 |
|---|---|---|---|
| 泊车 | 0~5 km/h | 1.0~2.0 m | 需要大转角，小 Ld |
| 园区低速 | 5~20 km/h | 2.0~5.0 m | 适中 Ld，兼顾灵活与稳定 |
| 城市道路 | 20~60 km/h | 5.0~15.0 m | 偏大 Ld，保证平顺性 |
| 高速巡航 | 60~120 km/h | 15.0~30.0 m | 大 Ld，避免频繁转向 |
实战技巧：我建议你在调试时，先固定 Ld 跑一圈，观察轨迹的震荡情况。如果车左右摇摆，说明 Ld 太小；如果车切弯太慢，说明 Ld 太大。然后根据车速动态调整 k 值，直到轨迹平滑为止。
2.5 从几何到代码：一个简单的实现
理论讲完了，咱们看看代码怎么写。下面是一个纯跟踪算法的核心函数：
def pure_pursuit_control(vehicle_state, target_point, L, k, Ld_min):
"""
纯跟踪算法核心计算
:param vehicle_state: 车辆状态 (x, y, yaw)
:param target_point: 目标点 (gx, gy)
:param L: 轴距
:param k: 前视距离比例系数
:param Ld_min: 最小前视距离
:return: 前轮转角 delta
"""
# 1. 计算前视距离
v = vehicle_state.v # 当前车速
Ld = k * v + Ld_min
# 2. 计算目标点相对于车辆的位置
dx = target_point.gx - vehicle_state.x
dy = target_point.gy - vehicle_state.y
# 3. 转换到车辆坐标系
alpha = atan2(dy, dx) - vehicle_state.yaw
# 4. 计算转弯半径
R = Ld / (2 * sin(alpha))
# 5. 计算前轮转角
delta = atan2(2 * L * sin(alpha), Ld)
return delta这段代码看起来简单，但有几个坑要注意：
- alpha 的范围：atan2 返回的是 -π 到 π，但纯跟踪算法要求 alpha 在 [-π/2, π/2] 之间，否则车会倒着走。我建议做一次角度归一化。
- Ld 不能太小：如果 Ld 小于 0.5 米，转弯半径会非常小，导致转角饱和，车会原地打转。
- 目标点选择：目标点必须是路径上距离车辆前视距离最近的点，而不是任意点。
血的教训：我曾经在项目中，因为忘记对 alpha 做归一化处理，导致车辆在掉头时直接反向行驶，差点撞上护栏。从那以后，我每次写纯跟踪算法，第一件事就是检查角度处理逻辑。
2.6 小结：三个模型的关系
最后，我用一句话总结这三个模型的关系：
- 阿克曼几何告诉你「真实的车怎么转」
- 自行车模型告诉你「简化的车怎么转」
- 纯跟踪算法告诉你「车应该转多少才能追上目标点」
说白了，纯跟踪算法就是站在自行车模型的肩膀上，用几何关系算出了一个控制律。你理解了这三个模型的递进关系，后面调参的时候，心里就有底了。
嗯，这一节的内容就到这儿。公式虽然简单，但每个符号背后都有实际意义。下一节咱们会讲怎么把这些公式变成可调参的代码，以及如何应对各种路况。