离中心线多远

离中心线多远

车就在车道里。下面的图都是从天上往下看的：灰色虚线不是路边，是车道正中间那条线（CARLA 里的红点连起来，每 0.5 米一颗）。 我们要量车的中心点 P 偏离这条线多少：新尺子横着量到中心线上正好在车旁边的那个点 F（叫垂足）；老尺子斜着量到最近那颗红点。

① 从天上往下看一段车道（真实比例）

P = 车的中心点 车道边线 车道中心线（红点连起来） 车（长约 4.7 米、宽约 1.9 米），往右开 车道宽约 3.5 米 蓝框放大，就是下面这张图（约 1.4 × 0.85 米）

② 放大蓝框：两颗红点 A、B 之间这一小段（整张图都在车道里面）

一小格 0.1 米 d = B − A v = P − A F

拖动圆点 P（车的中心点）或红点 B；选中后也可以用方向键挪，每次 0.01 米。

老尺子 · 到最近钉子0.39
新尺子 · 垂直量0.30车到垂足 F
老尺子多报0.09米
共 7 步 · 看的是结果
  1. 路的方向 d = B − A =
    dx, dy = bx - ax, by - ay
  2. 路长² =
    seg_len2 = dx * dx + dy * dy
  3. 车的中心点相对 A：v = P − A =
    (px - ax), (py - ay)
  4. 把 v 拆成两段：沿着路走了多远（前后），和偏到旁边多远（左右）。先算沿路那段：
    点积 =
    沿路 = 点积 ÷ 路长 =
    t = 沿路 ÷ 路长 =
    一共除了两次路长，所以代码里直接除以路长²
    t = ((px - ax) * dx + (py - ay) * dy) / seg_len2
  5. t 要在 0 到 1 之间（0 = 在 A，1 = 在 B）：
    t = max(0.0, min(1.0, t))
  6. 垂足 F（中心线上正好在车旁边的点）F = A + t × d =
    fx, fy = ax + t * dx, ay + t * dy
  7. 新尺子 = 车到垂足 =
    return math.hypot(px - fx, py - fy)

③ 车道会拐弯：中心线是很多小段，每一段都量一次，取最小

还是从天上往下看。一个 point_to_segment_m 只认一小段（两颗红点之间），到段的尽头线就断了，所以车开过 B 时它只能斜着量到 B。B 后面归下一段管。拖动 P 试试。

红点每 0.5 米一颗 · 车道往左拐

    下一步要敲的（先看懂图，不用现在敲）

    def lateral_error_m(path, x_m, y_m):
    """车到中心线（一串红点连成的折线）的最短距离：每段量一次，取最小。"""
    segs = zip(path, path[1:])     # 相邻两颗红点配成一段
    return min(point_to_segment_m(x_m, y_m, ax, ay, bx, by)
               for (ax, ay), (bx, by) in segs)

    车沿着路开：两把尺子的读数

    P 老尺子 新尺子
    新算法算的是车到墙的垂直距离，老算法算的是类似三角形第三条边的距离。你的结论 · 2026-10-18