0 专栏介绍

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

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

---

1 栅格地图

1.1 应用场景

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

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

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

1.2 基本概念

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

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

  
  

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

  
  

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

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

  
  

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

2 占据栅格地图

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

$$\mathrm{odd}\left(s\right)=\frac{P\left(s=1\right)}{P\left(s=0\right)}$$

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 ) P(s|z1:t)=P(zt|s,z1:t−1)P(s|z1:t−1)P(zt|z1:t−1)=Markov⏞P(zt|s)P(s|z1:t−1)P(zt|z1:t−1)=Bayes⏞P(s|zt)P(zt)P(s|z1:t−1)P(s)P(zt|z1:t−1)

$$egin{aligned}Pleft( s|z_{1:t} ight) &=frac{Pleft( z_t|s,z_{1:t-1} ight) Pleft( s|z_{1:t-1} ight)}{Pleft( z_t|z_{1:t-1} ight)}\,, & =frac{ { overset{mathrm{Markov}}{overbrace{Pleft( z_t|s ight) }}}Pleft( s|z_{1:t-1} ight)}{Pleft( z_t|z_{1:t-1} ight)}\&=frac{ { overset{mathrm{Bayes}}{overbrace{Pleft( s|z_t ight) Pleft( z_t ight) }}}Pleft( s|z_{1:t-1} ight)}{ { Pleft( s ight) }Pleft( z_t|z_{1:t-1} ight)}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​)​​

计算后验概率优势比

$$\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\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\left(s|z_{1:t}\right)=L\left(s|z_t\right)+L\left(s|z_{1:t-1}\right)$$

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

{ l o o c c u : L ( s ∣ z t = 1 ) l o f r e e : L ( s ∣ z t = 0 ) {looccu:L(s|zt=1)lofree:L(s|zt=0)

$$egin{cases} mathrm{looccu}: Lleft( s|z_t=1 ight)\ mathrm{lofree}: Lleft( s|z_t=0 ight)\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$

2.2 截断策略

从更新模型可以看出，$L\left(s|z_{1:t}\right)$是对历史观测序列的整合。换言之，若假设$\left|\mathrm{looccu}\right|=\left|\mathrm{lofree}\right|$，则当一个栅格被观察到$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 ⁡ } Lleft( s|z_{1:t} ight) =max left{ min left{ Lleft( s|z_t ight) +Lleft( s|z_{1:t-1} ight) , L_{max} ight} , L_{min} ight} 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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

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

---

? 更多精彩专栏：

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

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