纯追踪（Pure Pursuit）的几何直觉

核心思想

纯追踪是一类几何路径跟踪算法：不跟误差信号打交道，而是在前方路线上选一个前瞻点（lookahead point），把车”用线牵”过去。每一步只做一件事——算一条从当前位置出发、经过前瞻点的圆弧，然后按这条弧打方向。

设车与前瞻点的距离为 $L_d$（前瞻距离），车身朝向与车指向前瞻点连线的夹角为 $\alpha$。纯追踪要求圆弧同时切于车身当前朝向、并穿过前瞻点。由弦切角关系，这条弧的半径为：

$R = \frac{L_d}{2\sin\alpha}$

对应的曲率 $\kappa = 1/R$：

$\kappa = \frac{2\sin\alpha}{L_d}$

这就是纯追踪的全部控制律：曲率正比于前瞻点偏角的正弦。前瞻点偏得越厉害（$\sin\alpha$ 大），转得越狠；前瞻点越正（$\alpha \to 0$），方向越回直。

为什么”盯前方五米”而不是盯脚下

• 盯脚下（最近点）：误差小时修正信号也小，车会贴着路线抖动——每一步都在对噪声级的小误差做反应。
• 盯前方：前瞻点天然把”未来一段路线”的形状平均进控制量，输出是一条平滑弧线，相当于自带低通滤波。

它是个比例控制器

把 $\kappa = \frac{2}{L_d}\sin\alpha$ 换个读法：这是对横向偏差（用 $\sin\alpha$ 表达）的比例控制，增益是 $2/L_d$。$L_d$ 越小增益越大——响应快但易超调振荡；$L_d$ 越大增益越小——稳但迟钝，弯道跟不上。这个参数的全部权衡由此而来（前瞻距离怎么选，见 B07 对应的深读）。

容易看错的点

• 纯追踪不产生速度命令。它只管横向（方向），纵向（油门刹车）需要另一个控制器（本项目用 PD，见 B10 深读）。
• 圆弧假设隐含了自行车模型（bicycle model）：把左右两轮各等效成前后轴中点的一个轮。低速下足够精确，这也是它在仿真教练车上够用的原因。

参考

• Pure pursuit（维基百科）
• Coulter, Implementation of the Pure Pursuit Path Tracking Algorithm（CMU, 1992，原始论文）

最近点推进：为什么索引只进不退

代码

def _advance(self, x, y):
    # 推进最近点索引(单调,防回跳)
    while self.idx < len(self.wps) - 1:
        wx, wy, _ = self.wps[self.idx]
        nx, ny, _ = self.wps[self.idx + 1]
        # 下一个路点比当前路点更近? 就前进一格
        if ((x - nx) ** 2 + (y - ny) ** 2) < ((x - wx) ** 2 + (y - wy) ** 2):
            self.idx += 1
        else:
            break

（tools/t31_collect_paired.py 第 57–65 行，PurePursuitFollower._advance。）

单调性防的是什么

如果每帧全局搜索”离车最近的路点”，在掉头和 S 弯上会出事故：路线在这些地方自我折叠，几何上的最近点可能突然跳到车身后方的另一段路线上。索引一回跳，前瞻点跟着回跳，车就会原地画圈甚至倒着开。

while 循环只在”下一点比当前点更近”时前进一格，索引单调不减，从机制上消灭了回跳：折叠段上哪怕几何最近点确实在身后，控制器也只认”当前点及以后”的世界。

索引的第二个身份：进度条

因为索引单调且只沿路线前进，它同时免费充当了路线完成度指标——主循环里 idx >= len(wps) - 3 就判定本路线采完、销毁换线（t42_collect_full.py）。不需要另写一套”是否到终点”的判定。

均摊 O(1) 为什么成立

单帧最坏要连跳多格，但整个路线生命周期里索引从 0 单调爬到 $N-1$，总推进次数不超过 $N$。摊到每帧就是常数级——20Hz 控制循环里这条路每帧只花几次距离比较，比每帧全表 $O(N)$ 扫描最近点便宜且行为更正确（全局扫描反而会引入上面的回跳 bug）。这是一个”快”与”对”同时成立的罕见例子：单调性既是性能优化，也是正确性约束。

容易看错的点

• 比较用平方距离，不开根号。比大小不需要真实距离，省一次 sqrt，数值结果完全等价。
• while 而不是 if：车辆瞬移式跳动（如 spawn 后的第一帧、物理引擎抖动）时一帧可能要连跳多格才能重新咬住路线。

前瞻点搜索与前瞻距离的权衡

搜索代码

# 前瞻点: 距 ego >= lookahead 的下一个 wp
tgt = self.wps[min(self.idx + 1, len(self.wps) - 1)]
dist = math.hypot(tgt[0] - x, tgt[1] - y)
j = self.idx + 1
while j < len(self.wps) - 1 and dist < self.lookahead:
    tgt = self.wps[j + 1]                    # 距离不够, 继续往后找
    dist = math.hypot(tgt[0] - x, tgt[1] - y)
    j += 1

（tools/t31_collect_paired.py 第 71–78 行。）

从最近点索引出发沿路线向后走，取第一个离车超过前瞻距离 $L_d$ 的路点作为前瞻点。搜索起点复用了最近点推进的结果，所以每帧只在局部窗口内扫几个点，同样是常数级开销。

前瞻距离是唯一的关键参数

纯追踪的曲率律 $\kappa = 2\sin\alpha / L_d$ 里，$L_d$ 扮演比例增益的倒数。两个方向都有代价：

$L_d$	行为	代价
太小	紧贴路线，横向误差小	对路点噪声敏感，方向盘高频抖动，轨迹不平滑
太大	平滑稳定	弯道切内角（corner cutting）：弧还没走到弯心就开始回正，轨迹内切

切内角不是小瑕疵：它直接决定教练车会不会刮上内弯的墙角。本项目的两组实测取值正是沿这个权衡调的——

• t31（采图阶段）：$L_d = 5$ m，目标速度 5 m/s，低速求贴线；
• t42（正式采数）：$L_d$ 放宽到 8 m，配合提速（纵向 kp 提到 1.5）让高速下转向更顺。

代价在数据质检时兑现：Town10 紧凑路口处前瞻点常越过弯心，轨迹内切刮蹭静态物，成为 73% 路线带重碰撞的几何根因（ep19p2 的 B14 深读展开）。

一个工程细节

转向公式里的分母用的是 max(self.lookahead, dist)（t31 第 84 行）：接近路线终点时搜不到足够远的点，真实距离 $dist < L_d$，此时仍按 $L_d$ 归一，避免分母变小导致末端方向盘剧烈放大。

世界系到车体系：旋转矩阵求 α

问题

前瞻点的坐标是世界系（world frame）的，而”偏右 45 度”是车体系（ego/body frame）的说法。求夹角 $\alpha$ 之前，必须先把”车指向前瞻点”的向量从世界系旋进车体系。

推导

设车在世界系的位置为 $(x, y)$，朝向角（偏航，yaw）为 $\psi$，前瞻点世界坐标减车位置得到世界系向量：

$(l_x, l_y) = (x_{tgt} - x,\; y_{tgt} - y)$

车体系 = 世界系绕 $z$ 轴反向旋转 $\psi$，即乘旋转矩阵 $R(-\psi) = R(\psi)^\top$：

$$\begin{pmatrix} l_x' \\ l_y' \end{pmatrix} = \begin{pmatrix} \cos\psi & \sin\psi \\ -\sin\psi & \cos\psi \end{pmatrix} \begin{pmatrix} l_x \\ l_y \end{pmatrix}$$

$l_x'$ 是向量的纵向分量（车头方向），$l_y'$ 是横向分量（车左方向）。夹角即：

$\alpha = \operatorname{atan2}(l_y',\; l_x')$

前瞻点正在前方时 $l_y' = 0$，$\alpha = 0$ 无需修正；偏右 45° 时（CARLA/UE 左手系 $y$ 向右）$\alpha = -45°$ 量级，符号随坐标系约定。

代码对照

yaw = math.radians(transform.rotation.yaw)
lx, ly = tgt[0] - x, tgt[1] - y
alpha = math.atan2(
    -math.sin(yaw) * lx + math.cos(yaw) * ly,   # 横向分量 l_y'
     math.cos(yaw) * lx + math.sin(yaw) * ly)   # 纵向分量 l_x'

（tools/t31_collect_paired.py 第 79–83 行。）两个参数正是旋转矩阵第二行、第一行与向量的点积——“这两行展开就是旋转矩阵乘向量”，字面意义成立。

容易看错的点

• 必须用 atan2 而不是 atan。atan(l_y'/l_x') 分不出象限：前瞻点在正后方与正前方算出同一个值，车会在倒库场景原地打转。atan2 吃两个独立参数，值域 $(-\pi, \pi]$，前后左右分得清。
• 旋转方向：把向量从世界系换到车体系用 $R(\psi)^\top$；如果把坐标轴转过去则是 $R(\psi)$。两者差一个转置，写反了 $\alpha$ 就差一个负号，效果是把左偏当右偏——这类 bug 不会报错，只会让车”镜像驾驶”。

从 α 到方向盘：阿克曼几何与归一化

一行公式

steer = math.atan2(2.0 * 2.7 * math.sin(alpha),
                   max(self.lookahead, dist)) / (math.pi / 2.0)

（tools/t31_collect_paired.py 第 84 行。）

$\delta = \arctan\frac{2\,L_w \sin\alpha}{L_d}, \qquad \text{steer} = \frac{\delta}{\pi/2}$

三个符号：$L_w = 2.7$ m 是轴距（wheelbase，前后轴间距）；$L_d$ 是前瞻距离；$\alpha$ 是前瞻点在车体系下的方位角（求法见 B08 深读）。

推导：两步几何

第一步是纯追踪的圆弧（B04 深读）：过当前位置、切于车头方向、穿过前瞻点的弧，半径

$R = \frac{L_d}{2\sin\alpha}$

第二步是阿克曼转向几何（Ackermann steering geometry）的自行车模型近似：前轮转角 $\delta$、轴距 $L_w$ 与转弯半径 $R$ 满足

$\tan\delta = \frac{L_w}{R}$

代入 $R$ 即得 $\delta = \arctan\!\big(2 L_w \sin\alpha / L_d\big)$。轴距出现在分子里是物理必然：同样的前轮转角下，轴距越长转弯半径越大（车越”钝”），所以要达到同一个 $R$，长轴距车需要更大的 $\delta$。

为什么除以 π/2

CARLA 的 VehicleControl.steer 是归一化量，取值 $[-1, 1]$，对应物理最大转角约 $\pm \pi/2$ 量级。算出的弧度制 $\delta$ 必须除以 $\pi/2$ 才能喂给 API——这是单位换算，不是控制律的一部分。漏掉这一步，任何非零偏差都会打出满舵。

容易看错的点

• 代码用 atan2(分子, 分母) 而不是 atan(分子/分母)：分母被 max(lookahead, dist) 保证为正，两者等价，但 atan2 写法对极端输入更稳。
• 转向角有上限保护：主循环里 ctl.steer 还会被 clamp 到 $[-1, 1]$，公式本身不负责限幅。
• 这”一行公式”就是整个项目横向控制的全部——没有 Stanley、没有 MPC。教练车的定位是”稳定地产出可模仿的演示”，不是刷操控极限，简单可解释优先。

纵向速度控制：PID、抗饱和与 ki=0

代码

err = self.target_speed - speed                       # 速度误差
self.i_err = max(-5.0, min(5.0, self.i_err + err * 0.05))  # 积分项, 限幅±5
d_err = (err - self.last_err) / 0.05                  # 微分项, dt=0.05s
self.last_err = err
acc = self.kp * err + self.ki * self.i_err + self.kd * d_err

（tools/t31_collect_paired.py 第 85–89 行。）

离散 PID 的三项

连续形式 $u(t) = k_p e + k_i \int e\,dt + k_d \frac{de}{dt}$ 在 20Hz 定步长（$\Delta t = 0.05$ s，仿真 fixed_delta_seconds）下离散化：

• 比例项 $k_p e$：速度差多少补多少，响应主力；
• 积分项 $k_i \sum e\,\Delta t$：消除稳态误差（比如持续上坡导致的速度欠账）；
• 微分项 $k_d \frac{\Delta e}{\Delta t}$：误差变化率的阻尼，抑制超调。

err * 0.05 与 (err - last_err) / 0.05 里的 0.05 就是定步长 $\Delta t$——同步模式下它是不变量，硬编码安全；异步模式下这么写会引入随帧率漂移的增益错误。

抗饱和（anti-windup）

积分项被钳制在 $\pm 5$：长时间大误差（起步、避障让行后恢复）下积分会无限累积，等误差反号时控制器还背着巨额”存量”，造成严重的反向过冲——这叫积分饱和（integral windup）。钳制是最简单的抗饱和方案。本项目 $k_i = 0$，积分项实际不生效，限幅属于防御性保留。

为什么仿真里 PD 就够

积分项的价值是消除持续存在的未知扰动（风阻、坡度、载重）造成的稳态误差。仿真里执行器严格服从命令、无模型失配，PD 已经能把速度压到目标值附近；此时挂上积分反而引入相位滞后——误差消失后积分项还在输出，速度在目标值附近来回摆动。教练车求的是”演示平滑可模仿”，振荡的速度曲线会直接变成噪声标签（油门/刹车值就是训练标签），所以宁缺毋滥。

增益实测取值：t31 冒烟用 $(k_p, k_i, k_d) = (1.0, 0.0, 0.1)$；t42 正式采数为配合提速把 $k_p$ 提到 1.5（t42_collect_full.py 第 152–153 行）。

参考

• PID controller（维基百科）
• Integral windup（维基百科）

传感器四件套与”同口径”地基

挂载清单

col = world.spawn_actor(bpl.find("sensor.other.collision"), ...)  # 碰撞: 质检真值
cam = world.spawn_actor(bpl.find("sensor.camera.rgb"), ...)       # RGB 相机: 模型输入
gnss = world.spawn_actor(bpl.find("sensor.other.gnss"), ...)      # GNSS: 全局定位
imu = world.spawn_actor(bpl.find("sensor.other.imu"), ...)        # IMU: 罗盘+加速度

（tools/t42_collect_full.py 第 126–150 行。）

碰撞传感器不喂模型，它是质检的耳朵（impulse 与碰撞对象，ep19p2 用）；GNSS/IMU 提供 measurements 里的 gps/compass/accel 字段，与评测链对齐；相机是唯一的模型输入通道。

相机口径一步都不能差

cam_bp.set_attribute("image_size_x", "1024")
cam_bp.set_attribute("image_size_y", "512")
cam_bp.set_attribute("fov", "110")
cam_bp.set_attribute("exposure_compensation", "0.0")   # 锁死自动曝光
# 位姿: 车顶偏后 1.5m, 高 2.0m
carla.Transform(carla.Location(x=-1.5, y=0.0, z=2.0), carla.Rotation())

这套参数逐项复刻 SimLingo 的训练相机（config_simlingo_base.py，t31 第 34–36 行注释明确标注”勿改”）。原因是域适配实验的变量控制：

• 模型在官方数据上学到的视觉表征，绑定在这组内参（分辨率、fov）与外参（安装位姿）上；
• 采集、评测、训练、官方数据四方若有一方口径不同，模型看到的图像分布就偏移了，微调回收的将是”相机差”而不是”渲染差”——UE4 vs UE5 的对比结论会被污染；
• 锁曝光同理：自动曝光会随场景亮度动态改变图像统计量，引入一个与渲染器无关的额外变量。

fov 110° 的广视角让路侧信息（建筑、栅栏、路口）大量入画——这既是 UE5 高保真渲染的研究对象，也意味着教练车刮蹭静态物时”案发现场”都在画面里，脏数据的毒性有直接的视觉对应。

容易看错的点

• CARLA 相机回调给的是 BGRA 原始字节，代码里 reshape(h, w, 4)[:, :, :3] 取前三通道得 BGR，OpenCV 直接 imwrite 成 jpg，没有任何后处理——与官方数据”原图落盘”的口径一致。
• exposure_compensation 属性名在 UE4(0.9.15) 与 UE5(0.10) 两侧不同，t31 的做法是准备候选属性名列表逐个探测设置（EXPOSURE_ATTR_CANDIDATES），设不上就声明默认值——跨引擎兼容的实用模式。

参考

• CARLA Sensors 参考文档

为什么是 4fps：对齐、磁盘账与预测窗口

仿真帧率 ≠ 存储帧率

仿真跑在同步定步长 $\Delta t = 0.05$ s（20Hz），但每 5 个 tick 才落盘一帧：

ap.add_argument("--save-every", type=int, default=5)   # 4fps 官方口径
...
if tick_n % args.save_every == 0 and "img" in fh:      # 每 5 tick 存一帧

（tools/t42_collect_full.py 第 58、199 行。）

$20 / 5 = 4$ fps，即每 0.25 s 仿真时间存一条样本（一帧 jpg + 一个 measurements.json.gz）。

理由一：对齐官方数据

SimLingo 官方训练数据（carla_garage 血统）的存储率就是 4fps。模仿学习里时间粒度是数据语义的一部分：相邻样本的动作间隔、车速与位移的比例关系都锚定在 0.25 s 步长上。自采数据若用别的帧率，等于在输入分布里又掺了一个与渲染无关的变量。

理由二：磁盘账

存储帧率直接乘在数据体积上。T4.2 放飞总量 549,852 帧、110GB（CP4 报告实测口径）：

存储率	同驾驶时长体积	结论
20fps（全量落盘）	~550GB（110GB × 5）	超出单机数据预算
4fps	110GB	落在 WSL ext4 分区的预算内

单帧约 200KB（1024×512 jpg + 压缩 measurements），帧率翻 5 倍体积就翻 5 倍，没有压缩技巧能抹平这个线性关系。

理由三：预测窗口由此定义

SimLingo 训练配置 pred_len = 11——模型学习预测未来 11 个路点。在 4fps 下：

$11 \times 0.25\text{ s} = 2.75\text{ s}$

存储粒度直接定义了模型要学的未来有多长。帧率变了，同样的 pred_len 对应的物理时间窗就变了，与官方预训练的归纳偏置错位。

容易看错的点

• 丢掉的 16/20 帧不是信息浪费：控制仍在 20Hz 闭环运行（每 tick 都算动作并 apply），只是不全部落盘。教练车开得好不好取决于控制频率，数据集密度取决于存储频率，两者解耦。
• 相邻存储帧间隔 0.25 s、均速 3.8 m/s，即相邻样本间车已移动约 1 m——这对自回归式路点监督是合适的采样密度，再密则相邻帧冗余度急增。

三层保障：让 55 万帧采集连跑数天

第一层：nohup 后台脱离会话

nohup conda run -n mysim-ue5 env PYTHONPATH=tools python3 tools/t42_collect_full.py \
  > logs/t42-full.nohup 2>&1 &

nohup 让进程忽略 SIGHUP——SSH 会话断开时终端发给前台进程组的挂断信号。配合 & 脱离作业控制，采集进程的生存期与登录会话解耦。日志重定向到 nohup 文件，事后排查全靠它。

第二层：server 守护与子进程隔离探活

CARLA server 易崩（项目硬性规则：所有采数/评测脚本内置崩溃自动重启）。守护逻辑在 tools/server_watchdog.py，关键设计是探活必须子进程隔离：

• server 死后，WSL→Windows 的半开 TCP 连接上，carla RPC 的 C 层 recv 会永久挂死——set_timeout 和 SIGALRM 都打不断（信号 handler 不回解释器，AGENTS.md T3.0 实测坑）；
• 因此 server_alive 探活放在子进程里跑，用 subprocess 的 timeout 兜底：探活卡死 = 杀子进程判失联，主进程不被拖死；
• 判失联后自动拉起 server，采集循环外层重连 client 继续（t42 第 254–267 行的保活段）。

T4.2 放飞中途真实兑现过一次：actor destroy 触发 abort 使整个采集进程退出，守护发现进程消失后自动重启，断点续采接着跑（M4 复盘时间线 09-03~04）。

第三层：断点续采，路线目录是原子单位

rdir = os.path.join(args.out, f"route_{rc.route_id:05d}_s{seed}")
if os.path.isdir(os.path.join(rdir, "measurements")) and \
   len(os.listdir(os.path.join(rdir, "measurements"))) > 50:
    continue  # 断点续采: 已有路线跳过

（t42 第 92–95 行。）

恢复粒度是整条路线目录，不是单帧：一条路线要么完整重采、要么整条跳过。原子单位选路线而不是帧，是因为路线级真值（results.json.gz 的碰撞/完成度打分）只在路线收官时一次性写盘——帧级续采会产生”有测量无真值”的半成品目录，下游过滤语义就乱了。代价是崩溃当条路线白跑，几十到两百帧的损失，可接受。

容易看错的点

• 三层各自防不同的故障：nohup 防会话断，守护防 server/进程崩，续采防重启后重复劳动。少任何一层，数天级任务都要人肉盯。
• “人只负责每天看一眼日志尾巴”是这套设计的验收标准——无人值守能力是用前面所有层的自动化换来的。

两道安全带：扇形避障与怠速缓冲

前向避障：一个扇形测试

ht = hero.get_transform()
hyaw = math.radians(ht.rotation.yaw)
fx, fy = math.cos(hyaw), math.sin(hyaw)          # 车头单位向量
for bv in bg.actors:                             # 只遍历背景车!
    bt = bv.get_transform()
    dx, dy = bt.location.x - ht.location.x, bt.location.y - ht.location.y
    fwd = dx * fx + dy * fy                       # 纵向投影
    lat = abs(dx * -fy + dy * fx)                 # 横向投影(取绝对值)
    if 2.0 < fwd < 7.0 and lat < 2.5:             # 扇形判据
        tgt_spd = min(tgt_spd, max(0.0, fwd - 4.0))
        break

（tools/t42_collect_full.py 第 176–190 行。）

把每个背景车的相对位置投影到车体系（与 B08 深读同一个旋转矩阵），落在”前方 2–7 m、横向 ±2.5 m”扇形内就让行：目标速度压到 fwd - 4，即离得越近开得越慢，4 m 处停车让行。投影用点积/叉积各一次，每帧 6 辆背景车，开销可忽略。

起步缓冲：防真值污染，不是防碰撞本身

if tick_n * args.dt < 2.0:
    tgt_spd = 0.0        # spawn 后前 2s 怠速

（t42 第 191–193 行。）

动机是数据质量而非安全：spawn 瞬间车身可能与静态物穿模/紧贴，一动就蹭；而碰撞传感器在如实记录一切，这种”非驾驶行为导致”的蹭碰会混进路线级碰撞真值，把无辜路线打上污染标记。怠速 2 秒让世界物理稳定下来，再起步。

致命盲区：名单里只有背景车

避障循环遍历的是 bg.actors——背景车列表，且只有背景车。墙、栅栏、灯杆等静态物根本不在感知名单里，PurePursuit 又只管贴线，“贴线”与”撞墙”之间没有任何仲裁。这个偏科设计在采数时无感（让行逻辑对动态交通确实有效），直到质检才发现：99.6% 的路线有碰撞记录、73% 带重碰撞，碰撞对象里静态物占绝对多数（qc.json 与 CP4 报告实测）。

这是一类典型的测试对象错位：避障逻辑通过了它设计范围内的所有场景（动态让行），但它的缺陷在范围之外——而不在范围内的东西恰恰是测试时最容易忘记列进检查单的。

容易看错的点

• break 只处理第一辆命中的背景车：让行取最保守的速度即可，多车叠加不改变决策。
• 扇形判据 fwd > 2.0 的下界防自触：紧贴车尾/并行的车不触发让行，否则教练车会被旁边车道的车”吓停”。