地图结构 | 图解占据栅格地图原理(附Matlab建图实验)

目录

• 0 专栏介绍
• 1 栅格地图
• 2 占据栅格地图
• 3 仿真实现

0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 栅格地图

1.1 应用场景

栅格地图(grid map)是在机器人和自动化领域中广泛使用的一种地图表示方法。它将环境划分为规则的网格单元，并在每个单元中存储关于该区域的信息。每个单元可以表示空闲、障碍物、未知区域或其他地图属性。

栅格地图最主要的应用是服务于机器人导航中的路径规划和避障。机器人可以利用栅格地图中的障碍物信息来规划安全的路径，并避开可能的碰撞或危险区域。同时，栅格地图也是SLAM算法中常用的地图表示方式之一。通过与传感器数据融合，机器人能够同时进行自身位置估计和地图构建。

总之，栅格地图是一种简单且直观的地图表示方法，它可以提供对环境的可视化和语义信息，并为机器人的感知、规划和决策提供基础。然而，栅格地图也存在分辨率、存储消耗和精度等方面的限制，在实际应用中需要权衡和优化。

1.2 基本概念

栅格地图的基本概念总结如下

• 邻域模式
  栅格地图中常用的邻域模式有8邻域法、24邻域法和48邻域法，如下所示

  
  

• 栅格示数
  栅格地图中每个栅格都被赋予栅格示数，其指明了该栅格在全局环境中表达的语义。例如0表示无障碍的自由栅格，1表示障碍物

  
  

• 栅格坐标
  栅格地图可视为离散直角坐标系，其中可用有序二元组 ( i , j ) (i,j) (i,j)定位栅格

• 栅格序号
  栅格按照行列顺序依次进行的编号称为栅格序号，由于栅格序号是一维线性的，因此可以加速信息处理与运算

  
  

• 栅格粒度
  栅格对应物理世界的比例系数称为栅格粒度，栅格粒度越小，环境分辨率越大，对环境的刻画越具体、丰富。但相应地，存储地图所占的内存、处理地图耗费的时间越多

2 占据栅格地图

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

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)​

2.1 更新模型

构建地图的问题可形式化为：已知机器人激光传感器的观测序列 ，更新地图中栅格的后验概率 ，根据贝叶斯公式和马尔科夫链可得

P ( s ∣ z 1 : t ) = P ( z t ∣ s , z 1 : t − 1 ) P ( s ∣ z 1 : t − 1 ) P ( z t ∣ z 1 : t − 1 )    = P ( z t ∣ s ) ⏞ M a r k o v P ( s ∣ z 1 : t − 1 ) P ( z t ∣ z 1 : t − 1 ) = P ( s ∣ z t ) P ( z t ) ⏞ B a y e s P ( s ∣ z 1 : t − 1 ) P ( s ) P ( z t ∣ z 1 : t − 1 ) \begin{aligned}P\left( s|z_{1:t} \right) &=\frac{P\left( z_t|s,z_{1:t-1} \right) P\left( s|z_{1:t-1} \right)}{P\left( z_t|z_{1:t-1} \right)}\\\,\, & =\frac{{ \overset{\mathrm{Markov}}{\overbrace{P\left( z_t|s \right) }}}P\left( s|z_{1:t-1} \right)}{P\left( z_t|z_{1:t-1} \right)}\\&=\frac{{ \overset{\mathrm{Bayes}}{\overbrace{P\left( s|z_t \right) P\left( z_t \right) }}}P\left( s|z_{1:t-1} \right)}{{ P\left( s \right) }P\left( z_t|z_{1:t-1} \right)}\end{aligned} P(s∣z1:t​)​=P(zt​∣z1:t−1​)P(zt​∣s,z1:t−1​)P(s∣z1:t−1​)​=P(zt​∣z1:t−1​)P(zt​∣s) ​Markov​P(s∣z1:t−1​)​=P(s)P(zt​∣z1:t−1​)P(s∣zt​)P(zt​) ​Bayes​P(s∣z1:t−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 ( s = 0 ) = P ( s = 1 ) = 0.5 P\left( s=0 \right) =P\left( s=1 \right) =0.5 P(s=0)=P(s=1)=0.5，引入Logistic变换

L ( p ) = log ⁡ [ p / ( 1 − p ) ] L\left( p \right) =\log \left[ {{p}/{\left( 1-p \right)}} \right] L(p)=log[p/(1−p)]

则

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)​

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

2.2 截断策略

从更新模型可以看出， L ( s ∣ z 1 : t ) L\left( s|z_{1:t} \right) L(s∣z1:t​)是对历史观测序列的整合。换言之，若假设 ∣ l o o c c u ∣ = ∣ l o f r e e ∣ \left| \mathrm{looccu} \right|=\left| \mathrm{lofree} \right| ∣looccu∣=∣lofree∣，则当一个栅格被观察到 k k k次自由状态后，必须再被观察到至少 k k k次占据状态，才有可能被设置为占据栅格。这导致实际应用时，动态环境中的地图可能无法被快速更新。

为了建图的适应性，采用截断策略，定义概率上下限来限制改变栅格状态所需的更新次数，代价是概率在0-1区间内不再完备，靠近边界的概率丢失

L ( s ∣ z 1 : t ) = max ⁡ { min ⁡ { L ( s ∣ z t ) + L ( s ∣ z 1 : t − 1 ) , L max ⁡ } , L min ⁡ } L\left( s|z_{1:t} \right) =\max \left\{ \min \left\{ L\left( s|z_t \right) +L\left( s|z_{1:t-1} \right) , L_{\max} \right\} , L_{\min} \right\} L(s∣z1:t​)=max{min{L(s∣zt​)+L(s∣z1:t−1​),Lmax​},Lmin​}

3 仿真实现

3.1 算法流程

算法流程如下所示

其中关于Bresenham视线法原理，请参考路径规划 | 图解Theta*算法(附ROS C++/Python/Matlab仿真)

3.2 Matlab实现

核心代码如下所示

for i = 1:N 
    x = scan_pose(1, i);
    y = scan_pose(2, i);
    theta = scan_pose(3, i);
    robot_pos = [ceil(x * resolution) + origin(1), ceil(y * resolution) + origin(2)];

    % Find grids hit by the rays (in the gird map coordinate)
    rays = scan_ranges(:, i);
    x_occ = rays .* cos(scan_angles + theta) + x;
    y_occ = -rays .* sin(scan_angles + theta) + y;
    occ_pos = [ceil(x_occ * resolution) + origin(1), ceil(y_occ * resolution) + origin(2)];

    % Find occupied-measurement cells and free-measurement cells
    occ_id = sub2ind(size(grid_map), occ_pos(:, 2), occ_pos(:, 1));

    free = [];
    for j = 1:scans_num
        [ix_free, iy_free] = bresenham(robot_pos, occ_pos(j, :));  
        free = [free; iy_free, ix_free];
    end
    free_id = sub2ind(size(grid_map), free(:, 1), free(:, 2));

    % Update the log-odds
    grid_map(occ_id) = grid_map(occ_id) + lo_occ;
    grid_map(free_id) = grid_map(free_id) - lo_free;

    % Saturate the log-odd values
    grid_map(grid_map > lo_max) = lo_max;
    grid_map(grid_map < lo_min) = lo_min;
end

---

🔥 更多精彩专栏：

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