PythonRobotics 建图（Mapping）模块全解析：栅格地图、NDT、点云采样与目标形状识别的 10 个算法实现指南

本文以 PythonRobotics 仓库的 Mapping 章节导读文档 为骨架，系统梳理机器人环境建图的完整技术栈：从占据栅格地图（Ray Casting / LIDAR 转栅格）、高斯栅格地图、NDT 正态分布变换地图，到点云采样、k-means 目标聚类、圆/矩形形状拟合、法向量估计与距离场（SDF/UDF）。读者将掌握每个算法的原理、可运行代码入口、核心参数与源码实现细节，可直接在仓库中复现运行。

Mapping：机器人环境感知的根基

按 mapping_main.rst 的定义，Mapping（建图）是机器人借助 LIDAR、相机等外部传感器理解自身周围环境的能力。机器人必须识别障碍物的位置与形状才能避障，因此在建图领域，栅格地图（grid mapping）与机器学习算法（machine learning） 是两类被广泛使用的方法。

PythonRobotics 的 Mapping 模块正是围绕这两条技术路线组织，包含 10 个可独立运行的子模块：

技术路线	子模块	解决的问题
栅格地图	gaussian_grid_map	用高斯分布描述每个栅格被占据的概率
栅格地图	ray_casting_grid_map	用 2D 光线投射生成占据栅格地图
栅格地图	lidar_to_grid_map	从 LIDAR 距离数据构造占据栅格地图（含完整教程）
栅格地图	ndt_map	用正态分布变换表示环境（NDT 地图）
点云处理	point_cloud_sampling	对点云降采样以减少计算量
机器学习/聚类	kmeans_clustering	2D 目标聚类
机器学习/形状识别	circle_fitting	圆形目标形状识别
机器学习/形状识别	rectangle_fitting	矩形目标（车辆）L 型拟合
机器学习/形状识别	normal_vector_estimation	点云法向量估计（含 RANSAC）
距离场	DistanceMap	计算无符号/有符号距离场（UDF/SDF）

下面按技术路线逐一深入，并结合仓库源码印证每个算法的实现细节。

栅格地图：占据概率与高斯建模

光线投射栅格地图（Ray Casting Grid Map）

2D 光线投射栅格建图是最直观的占据栅格地图生成方式。核心思路：从传感器位置沿激光束方向发射"射线"，射线穿过的栅格标记为自由（0.5 变为空），射线终点（击中障碍物）的栅格标记为占据（1.0）。

源码位于 ray_casting_grid_map.py，核心函数为：

generate_ray_casting_grid_map(ox, oy, xyreso, yawreso)

• ox, oy：障碍物（激光击中点）的 x/y 坐标列表；
• xyreso：栅格分辨率（main() 中默认 0.25 m）；
• yawreso：角度分辨率（默认 np.deg2rad(10.0)）。

该实现有一个值得注意的工程优化：预计算（pre-casting）。通过 pre_casting 函数，在建图前先把地图上所有栅格按"相对传感器的方位角"分桶存入 precastDB（记录每个栅格的坐标、距离、角度、索引）；在线建图时只需根据每个观测点的角度查表，一次性把该角度桶内距离更远的所有栅格置为 0.5（自由），再把观测点所在栅格置为 1.0（占据），从而避免逐栅格重复计算。这正是其高效的原因。

高斯栅格地图（Gaussian Grid Map）

高斯栅格地图不再用离散的 0/0.5/1 三值表示栅格，而是用高斯分布刻画每个栅格的占据概率。核心函数在 gaussian_grid_map.py：

generate_gaussian_grid_map(ox, oy, xyreso, std)

其中 std 为高斯分布的标准差参数，控制占据概率在障碍物附近的扩散范围；xyreso 为栅格分辨率。地图范围由 calc_grid_map_config 根据点云边界外扩 EXTEND_AREA = 10.0 计算，最终以热力图（heatmap）形式可视化，draw_heatmap 使用 plt.pcolor 绘制。

LIDAR 转栅格地图实战教程（最完整的代码范例）

这是 Mapping 章节中代码量最大、最接近真实传感器数据的子模块。lidar_to_grid_map 教程文档 完整演示了"从 CSV 距离数据到占据栅格地图"的流水线，其实现位于 lidar_to_grid_map.py。

Step 1：读取 LIDAR 数据。 测量文件以 CSV 格式存放（每行：角度, 距离），用 file_read 解析：

def file_read(f):
    """Reading LIDAR laser beams (angles and corresponding distance data)"""
    measures = [line.split(",") for line in open(f)]
    angles = []
    distances = []
    for measure in measures:
        angles.append(float(measure[0]))
        distances.append(float(measure[1]))
    angles = np.array(angles)
    distances = np.array(distances)
    return angles, distances

Step 2：极坐标转笛卡尔坐标。 由角度与距离计算 x/y 坐标（注意激光雷达通常以正前方为 0°，此处 y 轴与视觉坐标方向相反，绘图时需翻转 y 轴以匹配栅格方向）：

ang, dist = file_read("lidar01.csv")
ox = np.sin(ang) * dist
oy = np.cos(ang) * dist

Step 3：Bresenham 画线标记占据栅格。 教程先展示了 bresenham(start, end) 的用法——在栅格地图上画一条从起点到终点的直线，将沿线栅格置 1。它对应 bresenham 实现，是激光射线模拟的基础工具：

import lidar_to_grid_map as lg
map1 = np.ones((50, 50)) * 0.5
line = lg.bresenham((2, 2), (40, 30))
for l in line:
    map1[l[0]][l[1]] = 1

Step 4：洪水填充（flood fill）标记自由空间。 对初始化值为 0.5（未知）的占据地图，从给定中心点出发，用队列（collections.deque）逐格向四邻域扩散，把值仍为 0.5 的未知栅格填为 0.0（自由），遇到障碍物（值为 1.0）与边界即停止。这就是 flood_fill 函数 的核心逻辑。

Step 5：生成最终占据栅格地图。 对真实数据调用 generate_ray_casting_grid_map，教程中的调用参数为：

xyreso = 0.02            # 栅格分辨率 [m]
yawreso = math.radians(3.1)  # 角度分辨率 [rad]
pmap, minx, maxx, miny, maxy, xyreso = lg.generate_ray_casting_grid_map(ox, oy, xyreso, False)

最终生成的地图为 150 x 100 的 numpy 数组。如教程所述，栅格值接近 1 表示占据（红色），接近 0 表示自由（绿色），接近 0.5 表示未知（未观测区域）——这正是占据栅格地图（Occupancy Grid Map，源自 Moravec & Elfes 1985 年提出的概率方法）的经典三态表示。

NDT 地图：用正态分布建模观测点

NDT（Normal Distribution Transform，正态分布变换）地图 是一种用正态分布对观测点建模的地图表示，其数学基础是多元正态分布：

• 正态分布由均值 μ 和协方差矩阵 Σ 两个参数决定：X ~ N(μ, Σ)；
• 2D 情形下 μ 是 2 维向量、Σ 是 2×2 矩阵；
• 其概率密度函数的矩阵形式为：X = 1/sqrt((2π)²|Σ|) · exp{ -1/2 (x-μ)ᵀ Σ⁻¹ (x-μ) }。

NDT 建图分两步：

1. 网格聚类：对新的观测点云，用基于栅格的聚类算法（grid based clustering）把点分到各个栅格单元中；
2. 拟合正态分布：对每个栅格单元内的点拟合一个正态分布（计算均值与协方差），每个 NDT 栅格用黑色椭圆可视化（椭圆方向与长轴由协方差矩阵的特征向量/特征值决定）。

源码在 ndt_map.py 中体现得十分清晰。NDTMap 类内部定义了嵌套类 NDTGrid（L21-L42），每个栅格单元记录：点数 n_points、均值 mean_x/mean_y、协方差矩阵 covariance、特征向量 eig_vec 与特征值 eig_values。构造时：

• 栅格分辨率由 resolution 参数指定，地图尺寸由点云边界外扩余量计算（L44-L59）；
• 通过 _create_grid_index_map 把每个观测点映射到所属栅格索引；
• _construct_grid_map 对点数 ≥ min_n_points（默认 3） 的栅格计算 np.cov(ox[inds], oy[inds]) 作为协方差，再经 np.linalg.eig 分解得到椭圆方向。

NDT 地图的优势在于用参数化分布压缩了栅格内点云的几何信息，为后续的扫描匹配、定位（如 NDT 配准）提供了紧凑且连续的地图表达。

点云采样：降维提速的三种策略

点云数据量通常非常庞大，全部处理会带来计算耗时问题。点云采样文档 指出：点云采样通过只抽取具有代表性的点来稀疏化点云，在保证下游处理性能的前提下降低计算复杂度。PythonRobotics 实现了三种算法，全部位于 point_cloud_sampling.py：

算法	核心思路	特点	函数签名
Voxel 采样	用三维规则网格（voxel grid）划分空间，同一网格内的点取平均值替代	简单、均匀降密度	voxel_point_sampling(original_points, voxel_size)（L16）
最远点采样（Farthest Point Sampling）	按指定点数抽样，使所选点之间距离尽可能远	适合机器学习等需要固定点数的场景	farthest_point_sampling(orig_points, ...)（L41）
泊松盘采样（Poisson Disk Sampling）	按指定点数抽样，且任意两点距离大于某阈值	单点分布不如最远点采样均匀，但计算快、适合实时处理	poisson_disk_sampling(orig_points, n_points, ...)（L77）

三种算法的取舍非常典型：需要均匀覆盖与实时性选泊松盘，需要确定点数且空间分布极致的选最远点采样，需要快速降密度选 Voxel 网格平均。

目标聚类与形状识别：机器学习路线

mapping_main.rst 提到的"机器学习算法"在 Mapping 模块中具体落地为目标聚类与形状识别。

k-means 目标聚类

k-means 聚类文档 展示的是 2D 目标聚类，将传感器观测到的多个目标点云划分成若干簇。核心函数在 kmeans_clustering.py：

kmeans_clustering(rx, ry, nc)

其中 rx, ry 是观测点坐标，nc 是簇数量。函数返回每个簇的质心与点归属，随后由 Clusters 类管理各簇数据。它常与激光雷达目标检测配合，把属于不同物体的点云分开，是后续形状识别（圆/矩形拟合）的前置步骤。

圆形形状识别（Circle Fitting）

圆形拟合文档 解决"从测距传感器观测恢复圆形目标形状"的问题：蓝色圆是真实物体形状，红色十字是测距传感器观测点，红色圆是拟合出的估计形状。核心函数：

circle_fitting(x, y)

实现在 circle_fitting.py，输入为观测点坐标，输出拟合圆心与半径。它配套了 get_sample_points（按角度分辨率生成真实圆形采样点）与 ray_casting_filter（模拟测距传感器只保留最近命中点的过程），从而在仿真中验证拟合精度。

矩形形状识别（L-Shape Fitting）

矩形拟合文档 基于 CMU 的 "Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners" 论文，专门用于激光扫描下的车辆检测（车辆在激光视角下常呈现 L 型轮廓）。算法分两步：

Step 1：自适应距离分割（Adaptive Range Segmentation）。 计算每个测距点与其最近点的距离，若低于阈值则归为同一簇。关键设计是阈值随传感器距离自适应增大——因为测距传感器天然具有"距离越远、点云越稀疏"的分布特性。阈值公式：

r_th = R0 + Rd * r_origin

• r_th：分割阈值；
• R0, Rd：常数参数；
• r_origin：该测距点离传感器的距离。

Step 2：矩形搜索（Rectangle Search）。 对每个簇，以一定角度间隔旋转数据，用三种评估函数之一选出最优旋转角作为矩形姿态：

1. 矩形面积最小化准则（Area）：计算能包含所有点的最小矩形面积（由点集 x-y 方向最大最小值之差得出），使矩形尽可能紧凑包裹点集；
2. 贴近度准则（Closeness）：用矩形右侧上下顶点到各点的距离作为评估值，点越靠近矩形边缘评估值越小；
3. 方差准则（Variance）：用各点到矩形边缘（水平与垂直）距离的平方和评估，平方误差等价于方差计算，方差越小说明点越贴合矩形。

源码中这些准则对应 LShapeFitting 类的 Criteria 枚举（rectangle_fitting.py）与三个私有方法 _calc_area_criterion、_calc_closeness_criterion、_calc_variance_criterion。从 默认参数 可以看到可直接调优的关键配置：

参数	默认值	含义
criteria	Criteria.VARIANCE	矩形搜索使用的评估准则
min_dist_of_closeness_criteria	0.01 [m]	贴近度准则的最小距离下限
d_theta_deg_for_search	1.0 [deg]	矩形搜索的角度步长
R0	3.0 [m]	自适应分割常数参数
Rd	0.001	自适应分割距离系数

整体流程由 fitting(ox, oy) 驱动：先对所有点做自适应分割得到簇集合，再对每个簇执行矩形搜索，最终返回拟合矩形与簇内点索引。

法向量估计（含 RANSAC 鲁棒估计）

法向量估计文档 讲了两种方法：

方法一：三角面法向量计算。 给定三维空间中三个点 p1, p2, p3，三角形法向量为两条边向量的叉积归一化：

v1 = p2 - p1
v2 = p3 - p1
n = (v1 × v2) / |v1 × v2|

对应 calc_normal_vector(p1, p2, p3)，是平面法向量的解析解。

方法二：RANSAC 平面法向量估计。 当从 N 个带噪 3D 点估计平面时，直接最小二乘拟合对点云噪声敏感。RANSAC（随机采样一致性）是对含离群点数据集的鲁棒估计方法，流程为：

1. 从点云中随机选取 3 个点；
2. 计算这 3 点构成的平面法向量；
3. 计算该平面到所有点云的距离；
4. 距离小于阈值的点判为内点（inlier）；
5. 重复上述步骤，直到内点比例大于阈值。

对应 ransac_normal_vector_estimation(points_3d, inlier_radio_th=0.7, ...)，其中 inlier_radio_th 默认 0.7，即内点占比达到 70% 即收敛。源码还提供 sample_3d_points_from_a_plane 与 distance_to_plane 用于仿真验证：前者按给定法向量生成带噪平面点，后者计算点到平面的距离供内点判定。

距离地图：SDF 与 UDF 距离场

距离地图文档 实现的是面向路径规划的距离场算法，输入为表示障碍物的布尔栅格，输出两类距离场：

• UDF（无符号距离场）：每个点到最近障碍物的距离；
• SDF（有符号距离场）：障碍物外部点取正值，内部点取负值（含符号的距离）。

实现在 distance_map.py：

• compute_udf(obstacles) 与 compute_sdf(obstacles)：基于 Felzenszwalb 与 Huttenlocher 的《Distance Transforms of Sampled Functions》论文方法，其中 dt(d) 是经典的一维距离变换核心例程；
• 另提供 compute_udf_scipy 与 compute_sdf_scipy 作为基于 scipy 的对照实现。

距离场是诸多路径规划与导航算法（如势场法、代价地图）的重要基础数据结构，SDF 的符号信息尤其便于判断点相对障碍物的内外关系。

如何运行与验证

所有 Mapping 子模块均提供 main() 入口（如 ray_casting_grid_map.py#L113、ndt_map.py#L114、lidar_to_grid_map.py#L207），直接以 Python 运行对应文件即可在本地生成仿真数据与可视化结果，例如：

python Mapping/ray_casting_grid_map/ray_casting_grid_map.py
python Mapping/gaussian_grid_map/gaussian_grid_map.py
python Mapping/lidar_to_grid_map/lidar_to_grid_map.py
python Mapping/ndt_map/ndt_map.py

仓库在 tests/ 目录提供了与每个模块一一对应的自动化测试，是验证算法正确性与理解调用方式的最佳入口，例如：

• test_ray_casting_grid_map.py、test_gaussian_grid_map.py：校验栅格地图生成结果；
• test_kmeans_clustering.py、test_circle_fitting.py、test_rectangle_fitting.py、test_normal_vector_estimation.py：校验聚类与形状识别的输出；
• test_point_cloud_sampling.py：校验三种采样算法的点数量与分布约束；
• test_distance_map.py：校验 SDF/UDF 距离场数值。

各模块所需的第三方依赖（numpy、matplotlib、scipy 等）见 requirements/requirements.txt，完整环境配置可参考 requirements/environment.yml。

小结

PythonRobotics 的 Mapping 模块以"栅格地图 + 机器学习"为主线，覆盖了从原始传感器数据到可导航地图的完整链路：LIDAR 距离数据 → 占据栅格地图 / 高斯栅格地图 / NDT 地图，以及面向感知的 点云采样 → 聚类 → 圆/矩形/平面形状识别 → 距离场。每个算法都有可直接运行的源码、配套文档与自动化测试，是学习机器人建图与感知算法的高质量参考实现。