lidar_design

Chapter 8: 点云处理基础算法

激光雷达输出的原始数据是三维空间中的散点集合，称为点云。本章将介绍点云处理的基础算法，包括坐标变换、滤波去噪和地面分割。这些算法是后续高级处理的基础，对于理解和应用激光雷达数据至关重要。通过本章学习，读者将掌握点云数据的基本操作方法，并能够实现实用的预处理流程。

8.1 坐标变换

激光雷达扫描得到的原始数据通常以球坐标形式表示，需要转换到笛卡尔坐标系才能进行后续处理。此外，在移动平台上还需要考虑运动补偿。

8.1.1 球坐标到笛卡尔坐标转换

激光雷达测量三个基本参数：

• 距离 r：激光发射到接收的时间差计算得出
• 水平角 θ（方位角）：激光扫描的水平旋转角度
• 垂直角 φ（俯仰角）：激光束相对于水平面的角度

坐标系约定： 不同厂商的激光雷达可能采用不同的坐标系定义，常见的有：

1. 右手坐标系（ROS标准）：X向前，Y向左，Z向上
2. Velodyne坐标系：X向前，Y向左，Z向上，φ向下为正
3. Hesai坐标系：X向右，Y向前，Z向上

转换公式（以ROS标准为例）：

x = r·cos(φ)·cos(θ)
y = r·cos(φ)·sin(θ)
z = r·sin(φ)

详细推导： 从球坐标系到笛卡尔坐标系的转换基于三角函数关系：

1. 先将点投影到XY平面：r_xy = r·cos(φ)
2. 在XY平面内分解：x = r_xy·cos(θ), y = r_xy·sin(θ)
3. Z分量直接计算：z = r·sin(φ)

计算实例1：某激光雷达测得一点的球坐标为 r=10m, θ=30°, φ=5°

x = 10 × cos(5°) × cos(30°) = 10 × 0.9962 × 0.8660 = 8.628 m
y = 10 × cos(5°) × sin(30°) = 10 × 0.9962 × 0.5000 = 4.981 m
z = 10 × sin(5°) = 10 × 0.0872 = 0.872 m

计算实例2：考虑16线激光雷达的多个扫描线 假设垂直视场角范围为-15°到+15°，16线均匀分布：

线号  垂直角φ    距离r=20m时的高度z
0     +15°       z = 20×sin(15°) = 5.18m
1     +13°       z = 20×sin(13°) = 4.50m
2     +11°       z = 20×sin(11°) = 3.82m
...
7     +1°        z = 20×sin(1°) = 0.35m
8     -1°        z = 20×sin(-1°) = -0.35m
...
15    -15°       z = 20×sin(-15°) = -5.18m

角度分辨率对坐标精度的影响： 设角度分辨率为Δθ = 0.1°，在不同距离r下，横向误差：

Δx_lateral = r × sin(Δθ/2) ≈ r × Δθ/2 (弧度)

距离r    角度误差Δθ    横向误差
10m      0.1°         1.7cm
50m      0.1°         8.7cm
100m     0.1°         17.5cm

8.1.2 坐标系定义

常见的激光雷达坐标系定义：

• 传感器坐标系：原点在激光雷达光学中心
  • X轴：指向前方
  • Y轴：指向左侧
  • Z轴：指向上方（右手坐标系）
• 车体坐标系（用于车载应用）：
  • 原点：车辆后轴中心在地面的投影
  • X轴：车辆前进方向
  • Y轴：车辆左侧
  • Z轴：垂直向上

8.1.3 坐标系转换矩阵

从传感器坐标系到车体坐标系的转换：

P_vehicle = R × P_sensor + T

其中：

• R 是3×3旋转矩阵
• T 是3×1平移向量

旋转矩阵的三种表示方法：

1. 欧拉角表示（ZYX顺序，即yaw-pitch-roll）：

   R = Rz(γ) × Ry(β) × Rx(α)
   

其中各基本旋转矩阵为：

Rx(α) = [1    0       0    ]
        [0    cos(α) -sin(α)]
        [0    sin(α)  cos(α)]

Ry(β) = [ cos(β)  0  sin(β)]
        [ 0       1  0     ]
        [-sin(β)  0  cos(β)]

Rz(γ) = [cos(γ) -sin(γ)  0]
        [sin(γ)  cos(γ)  0]
        [0       0       1]

1. 四元数表示： 四元数 q = [w, x, y, z]，其中 w² + x² + y² + z² = 1

旋转矩阵：

R = [1-2(y²+z²)   2(xy-wz)    2(xz+wy) ]
    [2(xy+wz)     1-2(x²+z²)  2(yz-wx) ]
    [2(xz-wy)     2(yz+wx)    1-2(x²+y²)]

1. 轴角表示： 绕轴n = [nx, ny, nz]旋转角度θ：

   R = I + sin(θ)·K + (1-cos(θ))·K²
   

   其中K是n的反对称矩阵。

计算实例1：激光雷达安装在车顶，相对车体坐标系的安装参数为：

• 平移：T = [0, 0, 1.8]ᵀ m
• 旋转：roll=0°, pitch=2°, yaw=0°

对于传感器坐标系中的点 P_sensor = [5, 3, -0.5]ᵀ：

R = Rz(0°) × Ry(2°) × Rx(0°) = 
[0.9994  0      0.0349]
[0       1      0     ]
[-0.0349 0      0.9994]

P_vehicle = R × P_sensor + T = 
[0.9994  0      0.0349] [5]   [0]   [4.983]
[0       1      0     ] [3] + [0] = [3.000]
[-0.0349 0      0.9994] [-0.5] [1.8] [1.626]

计算实例2：考虑更复杂的安装角度 激光雷达安装参数：

• 平移：T = [0.3, -0.1, 1.5]ᵀ m
• 旋转：roll=1°, pitch=3°, yaw=2°

详细计算过程：

1. 计算各旋转矩阵：
Rx(1°) = [1      0        0     ]
         [0      0.9998  -0.0175]
         [0      0.0175   0.9998]

Ry(3°) = [ 0.9986  0      0.0523]
         [ 0       1      0     ]
         [-0.0523  0      0.9986]

Rz(2°) = [0.9994  -0.0349  0]
         [0.0349   0.9994  0]
         [0        0       1]

2. 组合旋转矩阵：
R = Rz × Ry × Rx = 
[0.9981  -0.0349   0.0519]
[0.0362   0.9990  -0.0261]
[-0.0509  0.0280   0.9983]

3. 对点P_sensor = [10, 5, -1]进行转换：
P_vehicle = R × P_sensor + T = 
[9.720]   [0.3]   [10.020]
[0.835] + [-0.1] = [0.735]
[-0.249]  [1.5]   [1.251]

坐标系转换的误差传播： 考虑标定误差对最终坐标的影响：

δP = ∂P/∂R × δR + ∂P/∂T × δT + ∂P/∂P_sensor × δP_sensor

主要误差源：
1. 角度标定误差：δα, δβ, δγ ≈ 0.1°
2. 位置标定误差：δT ≈ 1cm
3. 测量误差：δP_sensor ≈ 2cm

误差传播示例（r=50m）：
角度误差贡献：50m × sin(0.1°) ≈ 8.7cm
位置误差贡献：1cm（直接传递）
总误差：√(8.7² + 1²) ≈ 8.8cm

8.1.4 运动补偿

机械旋转式激光雷达完成一帧扫描需要100ms，期间载体可能发生位移和旋转，导致点云畸变。

点云畸变的表现：

1. 平移畸变：直线变成曲线
2. 旋转畸变：平面变成扭曲面
3. 尺度畸变：物体尺寸失真

运动模型：

1. 匀速运动模型（最常用）：
   • 线速度：v = [vx, vy, vz]ᵀ
   • 角速度：ω = [ωx, ωy, ωz]ᵀ
2. 匀加速运动模型：
   • 加速度：a = [ax, ay, az]ᵀ
   • 角加速度：α = [αx, αy, αz]ᵀ

对于时刻 t_i 采集的点，补偿公式：

P_comp(t_i) = R(t_i) × P_raw(t_i) + T(t_i)

其中：

匀速模型：
T(t_i) = v × t_i
R(t_i) = exp(ω^ × t_i)  （ω^为反对称矩阵）

匀加速模型：
T(t_i) = v × t_i + 0.5 × a × t_i²
R(t_i) = exp((ω + 0.5 × α × t_i)^ × t_i)

反对称矩阵计算： 对于角速度ω = [ωx, ωy, ωz]ᵀ，反对称矩阵：

ω^ = [ 0   -ωz   ωy]
     [ ωz   0   -ωx]
     [-ωy   ωx   0 ]

罗德里格斯公式（计算旋转矩阵）：

R = I + sin(θ)/θ × ω^ + (1-cos(θ))/θ² × (ω^)²
其中 θ = ||ω|| × t_i

计算实例1：车辆以 v=20m/s 前进，角速度 ω=0.1rad/s（左转），激光雷达扫描周期100ms。

对于扫描中点（t=50ms）的补偿：

平移补偿：T = [20, 0, 0]ᵀ × 0.05 = [1.0, 0, 0]ᵀ m
旋转角度：θ = 0.1 × 0.05 = 0.005 rad ≈ 0.29°

旋转矩阵（使用小角度近似）：
R ≈ [1      -0.005   0    ]
    [0.005   1       0    ]
    [0       0       1    ]

对于原始点P_raw = [10, 5, 0]：
P_comp = R × P_raw + T = [10.975, 4.950, 0]

计算实例2：高速转弯场景 车速v=30m/s，转弯半径R=100m，扫描周期100ms：

角速度：ω = v/R = 30/100 = 0.3 rad/s
扫描期间转过角度：Δθ = 0.3 × 0.1 = 0.03 rad ≈ 1.72°

不同时刻的点补偿量：
t=0ms:    ΔX=0m,     ΔY=0m
t=25ms:   ΔX=0.75m,  ΔY=0.028m  
t=50ms:   ΔX=1.50m,  ΔY=0.112m
t=75ms:   ΔX=2.25m,  ΔY=0.253m
t=100ms:  ΔX=3.00m,  ΔY=0.450m

基于IMU的精确补偿： 利用高频IMU数据（如200Hz）进行更精确的运动估计：

1. IMU数据插值到激光点时间戳
2. 积分计算位姿变化：
   p(t) = p₀ + ∫v(τ)dτ
   q(t) = q₀ ⊗ ∫ω(τ)dτ  （四元数积分）
3. 应用补偿变换

补偿效果评估： 通过平面拟合残差评估补偿效果：

补偿前残差：σ_before = 15cm
补偿后残差：σ_after = 3cm
改善率：(15-3)/15 × 100% = 80%

8.1.5 时间戳对齐

不同激光雷达的时间戳定义不同：

• 帧首时间戳：第一个点的采集时间
• 帧尾时间戳：最后一个点的采集时间
• 帧中时间戳：扫描中间时刻

点的实际采集时间计算：

t_point = t_frame + (angle / 360°) × T_scan

其中：

• angle：点的方位角
• T_scan：完整扫描周期（如100ms）

8.1.6 多激光雷达坐标统一

大型系统可能使用多个激光雷达，需要将所有点云统一到同一坐标系。

标定方法：

1. 使用标定板或特征点
2. 最小化重叠区域的配准误差

标定优化目标函数：

E = Σᵢⱼ ||P_i - (R_j × P_j + T_j)||²

8.1.7 坐标精度分析

坐标转换引入的误差主要来源：

1. 角度测量误差：δθ, δφ
2. 距离测量误差：δr
3. 标定参数误差：δR, δT
4. 时间同步误差：δt

完整的误差传播模型：

对于球坐标到笛卡尔坐标的转换，雅可比矩阵为：

J = [∂x/∂r  ∂x/∂θ  ∂x/∂φ]
    [∂y/∂r  ∂y/∂θ  ∂y/∂φ]
    [∂z/∂r  ∂z/∂θ  ∂z/∂φ]

具体偏导数：

∂x/∂r = cos(φ)cos(θ)
∂x/∂θ = -r·cos(φ)sin(θ)  
∂x/∂φ = -r·sin(φ)cos(θ)

∂y/∂r = cos(φ)sin(θ)
∂y/∂θ = r·cos(φ)cos(θ)
∂y/∂φ = -r·sin(φ)sin(θ)

∂z/∂r = sin(φ)
∂z/∂θ = 0
∂z/∂φ = r·cos(φ)

协方差矩阵：

Σ_xyz = J × Σ_rθφ × J^T

数值示例1：r=50m, θ=45°, φ=0°, σ_r=2cm, σ_θ=σ_φ=0.1°

1. 计算偏导数：
∂x/∂r = cos(0°)cos(45°) = 0.707
∂x/∂θ = -50×cos(0°)sin(45°) = -35.36m
∂x/∂φ = -50×sin(0°)cos(45°) = 0

2. 误差传播：
σ²_x = (0.707×0.02)² + (-35.36×0.00175)² + 0²
     = 0.0002 + 0.0038 = 0.004
σ_x = 0.064m = 6.4cm

3. 各分量贡献：
距离误差贡献：0.014m (22%)
角度误差贡献：0.062m (78%)

数值示例2：考虑不同距离下的误差

距离r   σ_r    σ_θ     x方向总误差   角度误差占比
10m     2cm    0.1°    2.3cm         34%
30m     2cm    0.1°    5.5cm         73%
50m     2cm    0.1°    8.8cm         82%
100m    3cm    0.1°    17.6cm        89%
200m    5cm    0.1°    35.2cm        93%

标定误差的影响： 考虑激光雷达安装角度误差δα=0.1°，在不同距离的影响：

横向误差：Δ_lateral = r × sin(δα)

距离r    横向误差
10m      1.7cm
50m      8.7cm
100m     17.5cm

运动补偿误差： 速度估计误差δv=0.1m/s，角速度误差δω=0.01rad/s：

扫描周期T=100ms时：
位置误差：δP = δv × T = 0.1 × 0.1 = 0.01m = 1cm
角度误差：δθ = δω × T = 0.01 × 0.1 = 0.001rad ≈ 0.057°

综合误差模型：

总误差 = √(测量误差² + 标定误差² + 运动误差² + 时间误差²)

典型值（r=50m，v=20m/s）：
- 测量误差：6.4cm
- 标定误差：8.7cm  
- 运动误差：2.0cm
- 时间误差：1.0cm
总误差：√(6.4² + 8.7² + 2.0² + 1.0²) ≈ 11.0cm

误差优化策略：

1. 近距离：优化距离测量精度
2. 远距离：提高角度分辨率
3. 高速运动：增加IMU辅助
4. 多传感器：互相校验减少系统误差

8.2 滤波算法

原始点云数据包含各种噪声：测量噪声、多路径反射、大气散射等。滤波是点云预处理的关键步骤，用于去除噪声点和离群点，同时保留有用的几何特征。

8.2.1 统计滤波（Statistical Outlier Removal）

统计滤波基于点云的局部密度分布，去除明显偏离邻域统计特性的离群点。

算法原理：

1. 对每个点，计算其到k个最近邻点的平均距离
2. 计算所有点的平均距离的均值μ和标准差σ

3. 去除满足条件的点：

   d_i - μ

   > α·σ

参数选择：

• k：邻域点数，典型值10-50
• α：标准差倍数，典型值1.0-3.0

计算实例： 设某点P的10个最近邻距离为：[0.05, 0.06, 0.05, 0.07, 0.06, 0.08, 0.05, 0.06, 0.07, 0.55]m

该点的平均距离：d_p = 0.13m
全局统计：μ = 0.06m, σ = 0.02m
判断：|0.13 - 0.06| = 0.07 > 2×0.02 = 0.04
结论：该点为离群点，应被滤除

8.2.2 半径滤波（Radius Outlier Removal）

半径滤波通过统计固定半径内的邻域点数来判断离群点。

算法步骤：

1. 设定搜索半径r和最小点数阈值n_min
2. 统计每个点半径r内的邻域点数n
3. 若n < n_min，则标记为离群点

参数影响分析：

• r过小：可能误删稀疏区域的有效点
• r过大：无法去除小尺度噪声
• 建议：r = 2-5倍点云平均间距

密度自适应改进：

r_adaptive = r_base × √(d_avg / d_local)

其中d_avg是全局平均点间距，d_local是局部平均点间距。

8.2.3 体素滤波（Voxel Grid Filter）

体素滤波将空间划分为规则的立方体网格，每个体素内的所有点用一个代表点替换。

基本算法：

1. 计算点云的包围盒
2. 按体素大小划分网格
3. 每个体素内计算质心作为代表点

体素索引计算：

i = floor((x - x_min) / voxel_size)
j = floor((y - y_min) / voxel_size)
k = floor((z - z_min) / voxel_size)
voxel_index = i + j×n_x + k×n_x×n_y

计算复杂度：O(n)，其中n为点数

降采样率计算： 原始点数N，体素滤波后点数M 降采样率 = M/N

实例：100万点的点云，体素大小0.1m，滤波后约10万点，降采样率10%。

8.2.4 双边滤波（Bilateral Filter）

双边滤波在平滑噪声的同时保持边缘特征，考虑空间距离和属性相似度。

滤波公式：

p'_i = Σⱼ w_ij × p_j / Σⱼ w_ij

权重函数：

w_ij = exp(-||p_i - p_j||² / (2σ_s²)) × exp(-|I_i - I_j|² / (2σ_r²))

其中：

• σ_s：空间域标准差（控制平滑范围）
• σ_r：值域标准差（控制边缘保持）
• I：点的属性（如反射强度、法向量）

参数设置建议：

• σ_s = 2-5倍平均点间距
• σ_r = 0.1-0.3（归一化强度）

8.2.5 移动最小二乘（MLS）滤波

MLS通过局部曲面拟合实现平滑和上采样。

算法步骤：

1. 对每个点，拟合局部二次曲面
2. 将点投影到拟合曲面上

局部曲面方程：

f(x,y) = a₀ + a₁x + a₂y + a₃x² + a₄xy + a₅y²

最小二乘求解：

min Σᵢ wᵢ(zᵢ - f(xᵢ,yᵢ))²

权重函数：wᵢ = exp(-d²ᵢ/h²)，h为带宽参数。

8.2.6 条件滤波（Conditional Filter）

根据点的属性（坐标、强度、法向量等）进行条件筛选。

常用条件：

1. 距离范围：r_min < r < r_max
2. 高度范围：z_min < z < z_max
3. 强度阈值：I > I_threshold
4. 入射角限制：θ_incident < θ_max

组合条件示例： 保留车辆前方120°、距离5-80m、高度-2m到3m的点：

条件1: -60° < θ < 60°
条件2: 5m < r < 80m
条件3: -2m < z < 3m
满足所有条件的点保留

8.2.7 时序滤波

利用多帧数据的时间相关性进行滤波。

滑动平均：

p'(t) = Σᵢ₌₀ⁿ⁻¹ αᵢ × p(t-i)

卡尔曼滤波（用于动态点跟踪）： 预测：x̂(k|k-1) = F × x̂(k-1|k-1) 更新：x̂(k|k) = x̂(k|k-1) + K × (z(k) - H × x̂(k|k-1))

8.2.8 滤波算法性能比较

算法	时间复杂度	保边缘能力	适用场景
统计滤波	O(n·k·log k)	差	离群点去除
半径滤波	O(n²)	差	稀疏噪声
体素滤波	O(n)	中	快速降采样
双边滤波	O(n·k)	优	保特征平滑
MLS滤波	O(n·k²)	优	曲面重建

8.2.9 滤波参数自适应

根据点云特性自动调整滤波参数：

密度估计：

density = n_points / volume
k_adaptive = max(10, min(50, density × 0.001))

噪声水平估计： 通过局部平面拟合的残差估计噪声：

noise_level = median(residuals)
filter_threshold = 3 × noise_level

8.3 地面分割

地面分割是自动驾驶和机器人导航中的基础任务，将点云分为地面点和非地面点。准确的地面分割对后续的障碍物检测、可通行区域分析至关重要。

8.3.1 基于高度的简单分割

最简单的方法是设置高度阈值，但仅适用于平坦地面。

基本方法：

if z < z_threshold:
    点标记为地面
else:
    点标记为非地面

局限性：

• 无法处理坡道
• 对传感器倾斜敏感
• 需要精确标定

8.3.2 RANSAC地面拟合

RANSAC（Random Sample Consensus）通过随机采样拟合地面平面模型。

平面模型：ax + by + cz + d = 0

算法步骤：

1. 随机选择3个点，计算平面参数
2. 计算所有点到平面的距离
3. 统计内点数（距离小于阈值）
4. 重复N次，选择内点最多的模型

迭代次数计算：

N = log(1-p) / log(1-w³)

其中：

• p：期望成功概率（如0.99）
• w：内点比例估计

计算实例： 假设地面点占比w=0.5，期望成功率p=0.99：

N = log(0.01) / log(1-0.5³) = log(0.01) / log(0.875) = 35次

距离阈值自适应：

threshold = base_threshold × (1 + k × distance)

考虑测距误差随距离增长。

8.3.3 高程图方法（2.5D Grid Map）

将3D点云投影到2D网格，每个格子记录最低点高度。

网格划分：

grid_x = floor((x - x_min) / resolution)
grid_y = floor((y - y_min) / resolution)

地面高度估计：

1. 每个格子内选择最低点作为地面候选
2. 相邻格子间进行平滑
3. 高度差异大于阈值的标记为非地面

坡度计算：

slope_x = (h[i+1,j] - h[i-1,j]) / (2 × resolution)
slope_y = (h[i,j+1] - h[i,j-1]) / (2 × resolution)
slope = √(slope_x² + slope_y²)

优势：

• 计算效率高：O(n)
• 易于实现
• 适合实时处理

8.3.4 扇形分割法（Ray Ground Filter）

利用激光雷达的扫描特性，将点云按方位角划分为扇形区域。

算法原理：

1. 将360°划分为若干扇区（如720个，每个0.5°）
2. 每个扇区内按距离排序
3. 从近到远判断地面点

地面判断条件： 相邻两点的坡度：

slope = (z₂ - z₁) / (r₂ - r₁)
if slope < slope_threshold:
    标记为地面点

多线激光雷达改进： 考虑垂直相邻的扫描线：

slope_radial = (z₂ - z₁) / (r₂ - r₁)
slope_tangential = (z_upper - z_lower) / Δh_laser

8.3.5 基于法向量的分割

计算每个点的局部法向量，根据法向量与垂直方向的夹角判断。

法向量计算（PCA方法）：

1. 选择k个最近邻点
2. 计算协方差矩阵C
3. 最小特征值对应的特征向量为法向量

地面判断：

angle = arccos(|n · [0,0,1]|)
if angle < angle_threshold:
    标记为地面点

计算实例： 法向量n=[0.05, 0.03, 0.998]，与z轴夹角：

angle = arccos(0.998) = 3.6°
若阈值为10°，则为地面点

8.3.6 种子点生长法

从确定的地面种子点开始，逐步生长扩展地面区域。

种子点选择：

1. 选择传感器下方一定范围内的最低点
2. 使用RANSAC拟合初始地面

生长条件：

|z_neighbor - z_seed| < Δz_threshold
且
distance_to_plane < d_threshold

实现优化：

• 使用优先队列管理待处理点
• 采用八叉树加速邻域搜索

8.3.7 基于图的分割（Graph-based）

将点云构建为图结构，使用图切割算法分割。

图构建：

• 节点：每个点
• 边：相邻点之间的连接
• 权重：高度差或法向量差异

能量函数：

E = Σᵢ D(i) + λ Σᵢⱼ V(i,j)

其中：

• D(i)：数据项（点i属于地面的代价）
• V(i,j)：平滑项（相邻点标签不同的代价）

8.3.8 深度学习方法

使用神经网络直接从点云学习地面特征。

网络输入特征：

• 点坐标(x,y,z)
• 反射强度
• 局部几何特征
• 相对高度

常用网络结构：

• PointNet：直接处理点云
• RangeNet：处理距离图像
• PolarNet：极坐标表示

8.3.9 多传感器融合地面分割

结合多个传感器信息提高分割准确性。

IMU辅助： 利用IMU提供的倾斜角补偿点云：

z_corrected = z - x×sin(pitch) - y×sin(roll)

相机辅助：

• 利用语义分割识别道路区域
• 投影到点云约束地面范围

8.3.10 地面分割评价指标

精确率（Precision）：

Precision = TP / (TP + FP)

召回率（Recall）：

Recall = TP / (TP + FN)

F1分数：

F1 = 2 × Precision × Recall / (Precision + Recall)

实时性指标：

• 处理时间 < 50ms（20Hz）
• 适应不同点云规模

8.3.11 典型参数配置

场景	RANSAC阈值	坡度阈值	高程图分辨率
城市道路	0.15m	10°	0.2m
越野地形	0.3m	30°	0.5m
室内环境	0.05m	5°	0.1m
高速公路	0.1m	7°	0.3m