3. 道路边界提取与表示：高精地图中的边界数据、激光雷达点云边界提取、边界曲线的多项式拟合
各位同学，咱们今天聊聊道路边界。这玩意儿在自动驾驶里有多重要？我打个比方——没有边界约束的轨迹规划，就像在悬崖边上开车却没有护栏。你想想看，多吓人。
道路边界，说白了就是告诉车「你能走多远」的那条线。它可能是马路牙子，可能是护栏，也可能是车道线。不管是什么，我们得先把它从传感器数据里捞出来，然后用数学的方式表达清楚。今天我就把这三步拆开来讲。
3.1 高精地图中的边界数据
先说说最简单的情况——高精地图已经给你画好了边界。嗯，这里要注意，高精地图里的边界数据，通常不是一条连续的曲线，而是一堆离散的点，我们叫它 Shape 点序列。
我个人习惯把高精地图的边界数据分成两类：
- 硬边界：物理存在的障碍物，比如路缘石、护栏、防撞墙。这些是绝对不能压的。
- 软边界：交通规则定义的边界，比如车道线。紧急情况下可以压，但尽量别压。
高精地图里怎么存这些边界？我拿 OpenDRIVE 格式举个例子：
<!-- OpenDRIVE 道路边界示例 -->
<road name="MainRoad" length="500.0">
<lanes>
<laneSection s="0.0">
<left>
<lane id="-1" type="border">
<width sOffset="0.0" a="3.5" b="0.0" c="0.0" d="0.0"/>
<roadMark sOffset="0.0" type="solid" color="white"/>
</lane>
</left>
</laneSection>
</lanes>
</road>你看，这里用 type="border" 明确标注了边界车道。但说实话，我在实际项目中发现，很多高精地图的边界标注并不完整。有一次我们拿到某城市的高精地图，发现高架桥的匝道口边界数据是断的——嗯，这就是典型的「地图老了，路修了」的情况。
3.2 激光雷达点云边界提取
高精地图靠不住的时候怎么办？那就得靠激光雷达实时扫了。激光雷达点云提取边界，说白了就是找「地面突然塌下去」或者「突然凸起来」的地方。
我常用的方法分三步走：
- 地面分割：先把地面点云和非地面点云分开。常用的有 RANSAC 平面拟合，或者基于高程的网格滤波。
- 高程突变检测：在分割后的点云里，找相邻点之间高程变化超过阈值的位置。比如路缘石，通常有 10-15cm 的高度差。
- 聚类与跟踪：把检测到的边界点聚类成线，然后用跟踪算法保持连续性。
这里我贴一段当年在项目里用过的核心代码，帮你理解：
def extract_curb_from_lidar(points, z_threshold=0.12):
"""
从激光雷达点云中提取路缘石边界点
points: Nx3 的点云数组 (x, y, z)
z_threshold: 高程突变阈值，单位米
"""
# 1. 地面分割（简化版：用高度直通滤波）
ground_mask = (points[:, 2] > -0.5) & (points[:, 2] < 0.3)
ground_points = points[ground_mask]
# 2. 构建局部网格，计算每个网格内的最大高程差
grid_size = 0.2 # 20cm 网格
x_min, y_min = ground_points[:, :2].min(axis=0)
x_max, y_max = ground_points[:, :2].max(axis=0)
# 3. 高程突变检测
curb_points = []
for i in range(int((x_max - x_min) / grid_size)):
for j in range(int((y_max - y_min) / grid_size)):
mask_x = (ground_points[:, 0] >= x_min + i*grid_size) & \
(ground_points[:, 0] < x_min + (i+1)*grid_size)
mask_y = (ground_points[:, 1] >= y_min + j*grid_size) & \
(ground_points[:, 1] < y_min + (j+1)*grid_size)
cell_points = ground_points[mask_x & mask_y]
if len(cell_points) > 5:
z_range = cell_points[:, 2].max() - cell_points[:, 2].min()
if z_range > z_threshold:
# 记录该网格的边界候选点
curb_points.append(cell_points.mean(axis=0))
return np.array(curb_points)3.3 边界曲线的多项式拟合
好，现在边界点有了。但你不能直接把一堆离散点扔给规划模块——它要的是连续、平滑的曲线。这就轮到多项式拟合上场了。
为什么用多项式？说白了，多项式简单、好算、导数容易求。规划模块需要知道边界在任意位置的曲率，多项式一阶导、二阶导直接手算就行。
我常用的两种方式：
| 拟合方式 | 适用场景 | 优点 | 缺点 |
|---|---|---|---|
| 3次多项式 | 直道、缓弯 | 计算快，不易过拟合 | 无法表达S弯 |
| 5次多项式 | 复杂弯道、匝道 | 曲率连续，表达能力强 | 参数多，容易震荡 |
| 分段多项式 | 长距离、多弯道路段 | 兼顾精度和稳定性 | 需要处理衔接点 |
我个人最常用的是分段3次多项式。为什么？因为5次多项式在长距离拟合时，两端容易翘起来——我管它叫「甩尾效应」。你想想看，本来是一条平直的路，拟合出来的边界曲线在末端突然拐了个弯，规划模块一看，以为前面有障碍物，一脚急刹...这谁受得了？
来看一个最小二乘拟合的代码示例：
import numpy as np
def fit_boundary_poly(x, y, degree=3):
"""
用多项式拟合边界曲线
x, y: 边界点坐标（假设沿道路方向为x轴）
degree: 多项式次数，默认3次
"""
# 构造范德蒙矩阵
A = np.vander(x, degree + 1, increasing=True)
# 最小二乘求解
coeffs, residuals, rank, s = np.linalg.lstsq(A, y, rcond=None)
# 构造多项式函数
poly_func = np.polynomial.Polynomial(coeffs)
return poly_func
# 使用示例
x_points = np.array([0, 10, 20, 30, 40, 50])
y_points = np.array([3.0, 3.1, 3.2, 3.15, 3.05, 3.0])
boundary = fit_boundary_poly(x_points, y_points, degree=3)
# 在任意位置求边界值和曲率
x_query = 25.0
y_boundary = boundary(x_query) # 边界位置
dy = boundary.deriv(1)(x_query) # 一阶导（方向）
ddy = boundary.deriv(2)(x_query) # 二阶导（曲率相关）嗯，说到坐标变换，我记得有一次在高速公路上测试，发现拟合出来的边界曲线在弯道处误差特别大。排查了半天，发现是原始点云的坐标系没有对齐——激光雷达的安装角度偏了0.5度。你看，有时候问题不在算法本身，而在数据预处理。
最后总结一下我的建议：
- 高精地图边界数据，拿来当先验，别当真理
- 激光雷达提取边界，别忘了加反射强度辅助判断
- 多项式拟合，优先选分段3次，稳定压倒一切
边界搞定了，下一步就是怎么用这些边界来约束轨迹。不过那是后面章节的事了，咱们先把今天的内容消化掉。