障碍感知 | 基于3D激光雷达的三维膨胀栅格地图构建(附ROS C++仿真)_激光点云边三维栅格地图

目录

• 1 2D激光雷达 vs 3D激光雷达
• 2 三维占据栅格地图
• 3 三维栅格地图膨胀策略
• 4 三维栅格地图动态更新
• 5 ROS算法仿真

1 2D激光雷达 vs 3D激光雷达

在机器人环境感知领域，传统2D激光雷达通过单层扫描平面提供高效的二维轮廓信息，在室内导航、仓库AGV等结构化环境中表现出色。其优势在于数据量小（通常每秒数千个点）、处理简单，且成本相对低廉。然而，这种\"切片式\"感知存在根本性局限：当遇到多层货架、悬空障碍物或复杂地形时，单平面扫描会丢失关键的三维信息。更严重的是在物体遮挡场景中——例如机器人前方的低矮障碍物被较高物体部分遮挡时，2D雷达只能返回最近物体的距离数据，无法区分垂直方向上的空间关系，导致\"盲区效应\"。

3D激光雷达的出现彻底改变了环境感知的维度。通过多线旋转扫描（如16线、32线、64线）或固态相控阵技术，它能在垂直方向形成密集的点云覆盖，典型设备如Velodyne VLP-16每秒可产生30万个空间点。这种三维感知能力带来三大突破性优势：首先，在遮挡场景中能精确区分不同高度的物体，例如同时检测到低处的路缘石和高处的交通标志；其次，可构建真实的三维占用地图，使机器人理解坡道、楼梯等复杂地形；最重要的是能识别物体的立体结构，为语义理解提供几何基础。

2 三维占据栅格地图

直接应用点云数据存在实时性差、噪声敏感等问题，特别是在动态环境中进行路径规划时，机器人需要一种既能表达三维结构又能支持高效碰撞检测的地图表示方法。类比于二维栅格地图，通过3D激光雷达可以构建三维栅格地图，不仅可以保留三维点云的空间精度，还大幅提升了导航决策的鲁棒性。在无人机室内飞行、自动驾驶复杂场景、以及服务机器人的多楼层应用中，三维栅格地图的应用都非常广泛。

在工程上，通常使用占据法构建占据栅格地图(Occupancy Grid Map)。考虑到构建栅格地图使用的激光雷达存在噪声，即在相同条件下对障碍物的相对距离探测可能有误差，因此引入概率定义栅格状态：对于地图中的栅格$s$，$P\left(s=1\right)$表示栅格被占据的概率；$P\left(s=0\right)=1-P\left(s=1\right)$表示栅格自由的概率。实际应用时，使用一种更紧凑的表达

o d d( s ) = P ( s = 1 ) P ( s = 0 ) \\mathrm{odd}\\left( s \\right) =\\frac{P\\left( s=1 \\right)}{P\\left( s=0 \\right)} odd(s)=P(s=0)P(s=1)​

计算后验概率优势比

P ( s = 1 ∣ z 1 : t ) P ( s = 0 ∣ z 1 : t ) = P ( s = 1 ∣ z t ) P ( s = 0 ∣ z t ) ⋅ P ( s = 1 ∣ z 1 : t − 1 ) P ( s = 0 ∣ z 1 : t − 1 ) ⋅ P ( s = 0 ) P ( s = 1 ) \\frac{P\\left( s=1|z_{1:t} \\right)}{P\\left( s=0|z_{1:t} \\right)}=\\frac{P\\left( s=1|z_t \\right)}{P\\left( s=0|z_t \\right)}\\cdot \\frac{P\\left( s=1|z_{1:t-1} \\right)}{P\\left( s=0|z_{1:t-1} \\right)}\\cdot \\frac{P\\left( s=0 \\right)}{P\\left( s=1 \\right)} P(s=0∣z1:t​)P(s=1∣z1:t​)​=P(s=0∣zt​)P(s=1∣zt​)​⋅P(s=0∣z1:t−1​)P(s=1∣z1:t−1​)​⋅P(s=1)P(s=0)​

一般令先验概率$P\left(s=0\right)=P\left(s=1\right)=0.5$，引入Logistic变换

$$L\left(p\right)=\log \left[p/\left(1-p\right)\right]$$

则

L ( s ∣ z 1 : t ) = L ( s ∣ z t ) + L ( s ∣ z 1 : t − 1 ) L\\left( s|z_{1:t} \\right) =L\\left( s|z_t \\right) +L\\left( s|z_{1:t-1} \\right) L(s∣z1:t​)=L(s∣zt​)+L(s∣z1:t−1​)

称为栅格状态的更新模型。更新模型中与新测量值 z t z_t zt​有关的项是 L ( s ∣ z t ) L\\left( s|z_t \\right) L(s∣zt​)，由于激光雷达的测量值只有两种情况，因此定义

{ l o o c c u : L ( s ∣ z t = 1 ) l o f r e e : L ( s ∣ z t = 0 ) \\begin{cases} \\mathrm{looccu}: L\\left( s|z_t=1 \\right)\\\\ \\mathrm{lofree}: L\\left( s|z_t=0 \\right)\\\\\\end{cases} {looccu:L(s∣zt​=1)lofree:L(s∣zt​=0)​

必须指出，$\mathrm{looccu}$与$\mathrm{lofree}$表达了在获得感知数据的情况下栅格真实状态的概率，这是与传感器性能有关的常数。传感器性能越好，测量结果越接近真实值，$\mathrm{looccu}$越大$\mathrm{lofree}$越小。一般地，可以设定$\mathrm{looccu}=0.9$、$\mathrm{lofree}=-0.7$

3 三维栅格地图膨胀策略

3D激光雷达获取的点云数据本质上是连续三维空间中的离散采样点集 P= p 1 , p 2 , . . . , p n \\mathcal{P} = {\\mathbf{p}_1, \\mathbf{p}_2, ..., \\mathbf{p}_n} P=p1​,p2​,...,pn​，其中 p i =( x i , y i , z i )∈ R 3 \\mathbf{p}_i = (x_i, y_i, z_i) \\in \\mathbb{R}^3 pi​=(xi​,yi​,zi​)∈R3。构建三维栅格地图的首要任务是将这些连续坐标映射到离散的体素空间：

Φ : R 3 → Z 3 , Φ ( p ) = ( ⌊ x δ ⌋ , ⌊ y δ ⌋ , ⌊ z δ ⌋ ) = v i j k \\Phi:\\mathbb{R}^{3}\\rightarrow\\mathbb{Z}^{3},\\quad\\Phi(\\mathbf{p})=\\left(\\left\\lfloor\\frac{x}{\\delta}\\right\\rfloor,\\left\\lfloor\\frac{y}{\\delta}\\right\\rfloor,\\left\\lfloor\\frac{z}{\\delta}\\right\\rfloor\\right)=v_{ijk} Φ:R3→Z3,Φ(p)=(⌊δx​⌋,⌊δy​⌋,⌊δz​⌋)=vijk​

其中 $\delta$ 是体素分辨率。这个过程称为体素化，它将每个点分配到唯一的体素单元 v i j k v_{ijk} vijk​。

三维膨胀的核心是在障碍物周围创建安全缓冲层，其数学本质是构建障碍物邻域的闵可夫斯基和：设原始障碍物体素集合为 O⊂ Z 3 \\mathcal{O} \\subset \\mathbb{Z}^3 O⊂Z3，膨胀半径 $r$（以体素为单位），则膨胀区域定义为：

E = ⋃ o ∈ O B i ( o ) \\mathcal{E}=\\bigcup_{o\\in\\mathcal{O}} B_{i}(o) E=o∈O⋃​Bi​(o)

其中 B r (o) \\mathcal{B}_r(\\mathbf{o}) Br​(o) 是以障碍物 $\mathbf{o}$ 为中心的超立方邻域：

B r ( o ) = { v ∈ Z 3 ∣   v − o   ∞ ⩽ r } \\mathcal{B}_{r}(\\mathbf{o})=\\left\\{\\mathbf{v}\\in\\mathbb{Z}^{3}\\mid\\ \\mathbf{v}-\\mathbf{o}\\ _{\\infty}\\leqslant r\\right\\} Br​(o)={v∈Z3∣ v−o ∞​⩽r}

这里 $|\cdot |\infty$ 是切比雪夫距离，满足 ∣v∣∞=max⁡(∣ v x ∣,∣ v y ∣,∣ v z ∣) |\\mathbf{v}|\\infty = \\max(|v_x|, |v_y|, |v_z|) ∣v∣∞=max(∣vx​∣,∣vy​∣,∣vz​∣)。该定义确保膨胀区域在三个坐标轴方向均匀扩展，形成轴对齐的立方体缓冲区。当多个障碍物相邻时，其膨胀区域需要融合：

B r( o 1 ) ∪ B r( o 2 ) = B r( o 1 ) ∣ ∣ o 1 − o 2 ∣ ∣   ∞ < 2 r \\mathcal{B}_{r}\\left(o_{1}\\right)\\cup\\mathcal{B}_{r}\\left(o_{2}\\right)=\\mathcal{B}_{r}\\left(o_{1}\\right) ||o_{1}-o_{2}||\\ _{\\infty}<2 r Br​(o1​)∪Br​(o2​)=Br​(o1​)∣∣o1​−o2​∣∣ ∞​<2r

膨胀层可以设置和障碍物点云不同的代价，具体而言，膨胀代价场 C: Z 3 →[0,1] \\mathcal{C}: \\mathbb{Z}^3 \\to [0,1] C:Z3→[0,1] 根据与最近障碍物的 L ∞ L_\\infty L∞​ 距离线性衰减：

C ( v ) = max ⁡ ( 0 , 1 − d ∞ ( v , Q ) r ) \\mathcal{C}(\\mathbf{v}) = \\max\\left(0, 1 - \\frac{d_\\infty(\\mathbf{v}, \\mathcal{Q})}{r}\\right) C(v)=max(0,1−rd∞​(v,Q)​)

其中 d ∞ (v,O)= min ⁡o ∈ O ∣v−o∣∞ d_\\infty(\\mathbf{v}, \\mathcal{O}) = \\min_{\\mathbf{o} \\in \\mathcal{O}} |\\mathbf{v} - \\mathbf{o}|\\infty d∞​(v,O)=mino∈O​∣v−o∣∞。该函数在障碍物表面取最大值1，在膨胀边界 $d\infty =r$ 处降为0，形成连续的梯度场。

4 三维栅格地图动态更新

在碰撞检测 | 图解视线生成Bresenham算法(附ROS C++/Python/Matlab实现)中，我们介绍过Bresenham视线生成算法是一种高效的算法，用于在二维网格上绘制直线。它是由Jack Bresenham在1962年提出的，广泛应用于计算机图形学和游戏开发中。该算法的主要优点是只使用整数运算，因此速度较快且易于实现。下面是该算法的动图案例

利用类似的思想，我们可以知道：激光束击中的障碍物点不仅提供了该点的占据信息，其路径上的所有空间都应标记为自由空间。这种\"逆向推理\"使得地图能区分未知区域、自由区域和占据区域。首先，将Bresenham算法拓展到三维空间，给定传感器原点 o∈ R 3 \\mathbf{o} \\in \\mathbb{R}^3 o∈R3 和测量点 $\mathbf{p}$，激光束路径可表示为参数化直线：

$$\mathcal{L}(t)=\mathfrak{o}+t(\mathfrak{p}-\mathfrak{o}),t\in [0,1]$$

沿途的体素设置为自由空间，下面是三维版本Bresenham算法的迭代过程

while (true){ if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) { output.push_back(Eigen::Vector3d(x, y, z)); if ((Eigen::Vector3d(x, y, z) - start).squaredNorm() > max_dist) return; } if (x == end_x && y == end_y && z == end_z) break; // t_max stores the t-value at which we cross a cube boundary along the // axis. Therefore, choosing the least t_max chooses the closest cube boundary. if (t_max_x < t_max_y) { if (t_max_x < t_max_z) { // Update which cube we are now in. x += step_x; // Adjust tMaxX to the next X-oriented boundary crossing. t_max_x += t_delta_x; } else { z += step_z; t_max_z += t_delta_z; } } else { if (t_max_y < t_max_z) { y += step_y; t_max_y += t_delta_y; } else { z += step_z; t_max_z += t_delta_z; } }}

5 ROS算法仿真

设置感知参数

grid_map: resolution: 0.15 map_x_size: 30.0 map_y_size: 30.0 map_z_size: 3.0 local_update_range_x: 4.0 local_update_range_y: 4.0 local_update_range_z: 1.5 max_ray_range: 5.5 inflation_radius: 0.15

启动感知算法功能包

完整工程代码请联系下方博主名片获取

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇