Motion Planning回顾：Occupy grid map设置障碍物的原理

leida_花

已于 2024-01-13 10:55:42 修改

阅读量488

点赞数 9

分类专栏： Search-Planning 文章标签： c# linux 人工智能

于 2024-01-13 10:41:44 首次发布

本文链接：https://blog.csdn.net/weixin_52814031/article/details/135562332

版权

Search-Planning 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

0 引言

在进行motion planning运动规划学习过程中，对代码进行回顾记录。

1 引入

在栅格地图中，已知

Occupy Grid Map相关定义
地图分辨率	$resolution$
地图边界（m）	$-x_{max}$ - $x_{max}$ , $-y_{max}$ - $y_{max}$ , $-z_{max}$ - $z_{max}$
地图栅格索引边界	$x_{maxindex}$ , $y_{maxindex}$ , $z_{maxindex}$
存放地图数据一维数组	$data\left [ x_{maxindex}y_{maxindex}z_{maxindex}\right ]$

上述是部分Occupy Grid Map相关参数。

将地图栅格化表示可以根据如下公式进行：

$x_{maxindex}=\frac{2x_{max}}{resolution}$

$y_{maxindex}=\frac{2y_{max}}{resolution}$

$z_{maxindex}=\frac{2z_{max}}{resolution}$

对于地图中某点(x,y,z)及其栅格索引计算为：

$x_{index}=\frac{x+x_{max}}{resolution}$

$y_{index}=\frac{y+y_{max}}{resolution}$

$z_{index}=\frac{z+z_{max}}{resolution}$

而对于占据栅格地图而言，障碍物的状态一般为1，而free空间设定为0；所以对于某个坐标（x，y，z）是障碍物的话，应该设定该点的值为1。

由于我们将整个地图数据放置到一维数组data中，即需要对data进行索引进而设置上述坐标为障碍物。数组索引的地址data[address]中address求解公式如下：

$address=x_{index}y_{maxindex}z_{maxindex}+y_{index}z_{maxindex}+z_{index}$

具体代码如下：

void gridPathFinder::setObs(const double coord_x, const double coord_y, const double coord_z)
{   
    if( coord_x < gl_xl  || coord_y < gl_yl  || coord_z <  gl_zl || 
        coord_x >= gl_xu || coord_y >= gl_yu || coord_z >= gl_zu )
        return;

    int idx_x = static_cast<int>( (coord_x - gl_xl) * inv_resolution);
    int idx_y = static_cast<int>( (coord_y - gl_yl) * inv_resolution);
    int idx_z = static_cast<int>( (coord_z - gl_zl) * inv_resolution);      

    data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] = 1;
}

2 问题

如何理解上述障碍物设置的公式呢（代码部分也是一样）？

这里给出我个人的理解方式：

对于一个三维占据栅格地图， $x_{index}y_{maxindex}z_{maxindex}$ 部分表明y-z平面按照 $x_{index}$ 进行序号1所示索引，而索引到某x列的黑色平面部分栅格；然后 $y_{index}z_{maxindex}$ 部分表明继续在黑色栅格中z轴线按照 $y_{index}$ 进行序号2所示索引，索引蓝色列栅格；最后 $z_{index}$ 表明继续在蓝色栅格中按照序号3所示索引，索引到最后红色栅格，对该栅格进行障碍物设定。