(一) 路径规划算法---Astar与C++可视化在RVIZ的三维点云地图

Astar与C++可视化在RVIZ的三维点云地图

源代码下载链接: https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots
本文代码下载链接: https://gitee.com/tanggujie/astar

1.功能包介绍

本文主要对上述工作空间中的grid_path_search、rviz_plugins、waypoint_generator进行介绍，其中rviz_plugins和waypoint_generator为RVIZ插件，为了更加方便的在rviz中提供指定的三维坐标点，并不是本文的研究重点。下面对三维点云地图中Astar算法的实现的功能包grid_path_search进行介绍。

---

2.算法功能包的组成与介绍

2.1文件系统组成

其中文件说明如下

include：存放Astar算法的头文件

launch：Astar算法的启动文件

pcd：存放点云地图的文件夹

rviz：RVIZ中的配置文件

src：Astar算法的头文件以及Astar对外的ros接口文件

2.2 头文件说明

其中红色框为本文基于原作者自己修改的头文件代码，包含自己所理解的中文注释。绿色框为源代码所拥有的头文件(源代码还提供了JPS算法，大家有兴趣可以将它和Astar进行比较。)

2.3 源文件说明

其中红色框为本文基于原作者自己修改的源文件代码，包含中文注释。绿色框为源代码所拥有的源文件

Astar.cpp:Astar算法的本体函数

astar_demo.cppAstar算法对外的ros接口函数

complex_map.cpp：提供的固定点云地图文件，点云地图以pcd的方式进行存储。其中pcd文件是通过源代码中的random_complex_generator.cpp进行产生(注意：该random_complex_generator.cpp并不会自己产生点云文件，需要自己添加保存点云的代码，并且random_complex_generator.cpp每次会随机产生点云地图,调试代码并不方便)

查看保存好的点云，指令如下,其中fc为背景颜色，ax为坐标轴的大小

pcl_viewer map.pcd -fc 255,255,255 -ax 1

---

3.相关坐标系说明

3.1 坐标系

世界坐标系：明确一点世界坐标系的原点就是点云地图的原点，且点云地图是反应真实世界的，默认单位为m。

栅格坐标系：名称自定义的，嘻嘻。目的是为了方便Astar算法搜索周围邻居，而在三维点云中，各个点之间并不会像图片那样可以方便的查找自己周围的邻居，它们是无序排列的。因此采用将点云空间进行栅格化，如果其中一个栅格有三维点，则在该栅格中置1，否则置0。

两大坐标系如下图所示，其中$O_1{X}_1{Y}_1$为世界坐标系，$O_2{X}_2{Y}_2$为栅格坐标系，其中在程序代码中，$O_1$处在整个栅格的中心位置，$O_2$处在$O_1$的左上角位置。$O_{2}A$为整个点云栅格的宽，$O_{2}B$为整个点云栅格的长，$O_{2}C$为整个点云栅格的高。

3.2 点云数据存储

该程序代码中，点云栅格地图数据的存储也是一维数据，按照先Z后Y再X的方向进行存储。例如栅格地图中某点坐标为$(x,y,z)$（其中x,y,z均为整数），对应于一维点云数据为$data[x\times Length\times Hight+y\times Hight+x]$。

---

4.重要代码说明

1.程序通过先读取点云地图进行Astar地图初始化，在程序设计中将该点的世界坐标系与栅格坐标系绑定起来，方便后续路径查找。

for (int i=0;i<GLX_SIZE;i++)
    {
        GridNodeMap[i].resize(GLY_SIZE);
        for (int j=0;j<GLY_SIZE;j++)
        {
            GridNodeMap[i][j].resize(GLZ_SIZE);
            for (int k=0;k<GLZ_SIZE;k++)
            {
                Vector3i tmpIdx(i,j,k);     //栅格索引 
                Vector3d pos = gridIndex2coord(tmpIdx);  //该栅格对应的世界坐标系下的点云位置坐标
                GridNodeMap[i][j][k] = new GridNode(tmpIdx, pos);
            }
        }
    }

栅格转世界坐标系函数gridIndex2coord()
$$\begin{gathered} W_x=M_x\times resolution+origin\_x \\ {W}_y=M_y\times resolution+origin\_y \\ {W}_z=M_z\times resolution+origin\_z \end{gathered}$$

Vector3d AstarPathFinder::gridIndex2coord(const Vector3i & index) 
{
    Vector3d pt;

    pt(0) = ((double)index(0) + 0.5) * resolution + gl_xl;
    pt(1) = ((double)index(1) + 0.5) * resolution + gl_yl;
    pt(2) = ((double)index(2) + 0.5) * resolution + gl_zl;

    return pt;
}

世界转栅格坐标系函数coord2gridIndex()

其中$min(max(X,a),b)$表示
$$f(X)=\{\begin{array}{lc}X& \text{X}\in [a,b]\\ a& \text{X}<a\\ b& \text{X}>b\end{array}$$

Vector3i AstarPathFinder::coord2gridIndex(const Vector3d & pt) 
{
    Vector3i idx;
    /*
        0<=(pt(0) - gl_xl) * inv_resolution)<=GLX_SIZE - 1
        0<=(pt(1) - gl_yl) * inv_resolution)<=GLY_SIZE - 1
        0<=(pt(2) - gl_zl) * inv_resolution)<=GLZ_SIZE - 1
    */
    idx <<  min( max( int( (pt(0) - gl_xl) * inv_resolution), 0), GLX_SIZE - 1),
            min( max( int( (pt(1) - gl_yl) * inv_resolution), 0), GLY_SIZE - 1),
            min( max( int( (pt(2) - gl_zl) * inv_resolution), 0), GLZ_SIZE - 1);                  
  
    return idx;
}

2.Astar算法的障碍物判断为isOccupied()函数，三维栅格点云数据以一维数组data进行存储，数组大小为$Width\times Length\times Hight$。具体栅格坐标系下某点坐标转化到一维数组中的位置，见3.2节点云数据的存储。

inline bool AstarPathFinder::isOccupied(const int & idx_x, const int & idx_y, const int & idx_z) const 
{
    return  (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE && 
            (data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] == 1));
}

3.其他均是一些Astar算法的本体函数，与一些接口函数的调用，本文就不赘述了。具体可参考代码中的注释。

3.1 Astar算法中的启发式函数$h$的计算是对真实距离的估计，因此$h$的计算基于世界坐标系。

3.2 Astar邻居的索引是在栅格坐标系中的，因为三维点云数据无序，需要进行栅格化。

3.3 Astar路径的生成是通过rviz中的Marker功能实现的，而Marker所需的坐标点是基于世界坐标系的，因此，Astar路径的生成也是基于世界坐标系的。

---

5.代码运行情况

启动

roslaunch grid_path_search my_demo.launch 

稍等片刻，就会显示rviz信息，如下图所示。可以改变AllMap下的Spheres进行点云数据不同风格的显示。点击3D Nav Goal(或者点击键盘中的"g"键) 进行目标点的设置，首先通过鼠标左键进行XY平面位置的确定，此时按住鼠标左键不要松，在点击鼠标右键进行上下拖动，进行Z位置的确定，同时松开鼠标左键和鼠标右键，即可显示Astar的三维路径。

注意：

1. 在该算法演示中，并没有设置Astar的起点，若想改变Astar的起点位置，需要在my_demo.launch中

<arg name="start_x" default=" 0.0"/>
<arg name="start_y" default=" 0.0"/>
<arg name="start_z" default=" 1.0"/>

进行起始点$X、Y、Z$的设置。其实也可以利用3D Nav Goal进行起始点的设置，不过为了偷懒，并没有写，感兴趣的可以尝试一下。

2. 该显示函数提供了更新功能，即不需要关闭程序，在此指定一个新的坐标点，算法会重新生成一条新的Astar路径点。

此外，非常感谢源代码对我的帮助，从中学到了很多有用的知识。