一种在复杂地图下改进VisualGraph的方法与流程

本发明涉及智能机器人运动规划领域内的一种路径规划算法，具体是一种针对地形复杂的地图下Visual Graph算法的改进，使其不仅可以应用在凸多边形障碍物的地图下，也可以运用在复杂的地图下。

背景技术：

当下能够自主导航避障的机器人系统研究异常火热，而且也已经有许多产品应用于实践。如餐馆的服务机器人，就拥有障碍物识别，路径规划的能力；又如家庭扫地机器人，也拥有在开阔空间内规划自身运动路径的能力。在公共运输方面，无人机快递投递的应用已经开展的如火如荼，在快递投递上也必须用到路径规划，以使无人机能够在避开障碍物的前提下以最短的路径到达目的地并完成投递。

路径规划即是指机器人决策如何从地图的某一点运动到另一点的能力。首先要求机器人能够获得当前环境的地图信息，并能定位当前自身的位置，随后才可以进行路径规划，定位和建图的算法目前最实用的就是SLAM算法。目前有许多路径规划算法，如RRT、PRM等。

Visual Graph也是常用的路径规划算法，但是其应用对地图有严格的限制，要求地图的障碍物要是凸多边形，并且各个障碍物的顶点坐标已知，随后连接各个障碍物的顶点，并以这些连线为载体，生成路径。但是在实际应用场景中，地形是十分复杂的，地图中的障碍物形状是不规则的，而且其顶点坐标也是未知的，因此要直接利用Visual Graph算法是不可行的。现行的解决该问题的方法为，首先对地图进行预处理，使用形态学方法，如分水岭算法、特征匹配算法等，可以提取地图中的特征点或者障碍物的边界，获得障碍物的大致形状，并以直线连接各个特征点，将障碍物转化为一个凸多边形，再应用Visual Graph算法。这种方法的计算量不小，涉及的额外算法多，效率很难估算，而且在其将障碍物转化到凸多边形时，会对障碍物的边缘进行一定程度的膨胀或腐蚀，可能造成地图中原本不是障碍物的区域成为障碍物，原本是障碍物的区域成为可通过，可能使得最终得到的路径非法。

技术实现要素：

本发明的技术解决问题：克服了原始Visual Graph对地图的硬性要求，提出一种在复杂地图下改进Visual Graph的方法，使其不再仅限于凸多边形障碍物的地图，在复杂的SLAM地图下也可以运行，并且无需对地图做预处理，节点的密度参数也很容易调整，以生成精确的路径。

本发明的技术解决方案：一种在复杂地图下改进Visual Graph的方法，其特征在于：能够使Visual Graph不仅仅应用于凸多边形障碍物的地图，对一般的SLAM地图也可以适用，其步骤如下：

步骤1：对地图进行转化，将灰度图或者SLAM所构建的地图二值化处理，得到转化后的地图，此过程称为地图二值化过程；

步骤2：针对转化后的地图，给定起点和终点，将起点和终点储存在一个顺序表中，并从起点和终点开始发射光线，开始建立光路图，此过程称为初始化过程；

步骤3：当光线碰触障碍物时，记录该碰撞点的坐标，并与顺序表中已有的节点求取距离，若距离小于一个阈值则舍弃该点，否则保留，该过程称为节点储存判定过程；

步骤4：从保留的节点开始，重新发射光线，并重复步骤3的过程，此过程称为重复光线发射过程；

步骤5：当所有光线碰撞障碍物都无法生成新的节点时，判断建图结束；

步骤6：在建好的拓扑地图上应用A*寻路算法，即可获得一条连接起点和终点，以光路为载体的路径，此过程称为寻路过程。

所述步骤1中，地图的二值化的方法为：

调用opencv库读入所给的地图文件，并将地图矩阵转化为一个二维整形数组，即表示二维平面空间，数组中的每一个元素代表地图上相应坐标的一个像素点，若该点为障碍物点，则将数组值设为1，若该点为可通过点，则将数组值设为0。

所述步骤2中，发射光线的方法为：

(1)对于某一点，从0度，即水平向右方向开始每隔一个固定的步长发射光线，直到360度为止；

(2)对于每个方向，光线逐像素向前推进，每推进一个像素，将会检测该点是否为可通过点，若可通过则继续推进，若不可通过则表示碰到障碍物，记录该点，进入步骤3的判断过程；

(3)若某一方向的光线距离过小，则表示该方向与障碍物十分贴近，是不合理的，因此若光线距离小于某一阈值，则该方向的光线将被舍弃。

所述步骤2中，储存节点的方法为：

设置两个顺序表，一个顺序表allnode储存所有的节点，即已经发射过光线与尚未发射光线的所有节点，另一个顺序表open储存尚未发射光线的节点。初始化时，将起点和终点都存入两个顺序表中。

所述步骤3中，节点储存的判定方法为：

当光线碰到障碍物时，将该碰撞点与allnode中的所有节点求取距离，若该距离小于某一阈值，则舍弃该点，否则将该点加入allnode和open表中。

所述步骤4中，重复光线发射的方法为：

取出open表头的一个元素，重复步骤2所示的光线发射过程。

所述步骤5中，建图结束的判断方法为：

当某一节点发射的光线与障碍物的碰撞点与已有点的距离均小于阈值时，则该节点不会生成新的节点，此后再从open表中取出一个点进行光线发射，此时open表的元素个数将会减少，当open表为空时，即建图过程结束。

本发明与现有技术相比的优点在于：

(1)本发明沿用原始算法的光线直线传播思路，但突破了原始算法只能应用于凸多边形障碍物地图的局限。在复杂地图中，以光线和障碍物表面的碰撞点作为节点，即类似于凸多边形的顶点，并从该节点发射新的光线来生成更多的节点，遍布整张地图的障碍物表面，并以光路为载体形成路径，对地图本身没有任何要求；

(2)本发明无需对地图进行预处理，计算量极小。与现有的改进Visual Graph的方法相比，现有技术需要额外设计算法来对地图进行预处理，以生成一张凸多边形地图来适应原始的Visual Graph算法，增加了非常巨大的计算量。而本发明对地图没有任何要求，即无需另外设计算法对地图进行预处理来生成凸多边形障碍物地图，直接对原地图进行操作，省去大量计算量，提高了运行效率；

(3)本发明的各项阈值参数都十分容易调整，与现有的技术方法相比，现有技术需要对障碍物表面进行膨胀或腐蚀，使障碍物的形状变得规则均匀，以此适应原始的Visual Graph算法，所得的路径常常出现穿越障碍物的非法情况。而本发明通过调整阈值参数可以得到一条更精确、合法的路径，无需对障碍物的边缘腐蚀或膨胀，不会导致地图中的障碍物点被腐蚀为可通过点，可通过点被膨胀为障碍物点，所得的路径精确而且合法。

附图说明

图1为原始Visual Graph算法效果；

图2为本发明方法的流程图；

图3为本发明效果图。

具体实施方式

本发明一般应用于slam算法建立的地图中，即是实际地形，较为复杂。

如图2所示，本发明具体实施步骤详细说明如下：

步骤1：用slam算法或其他建图算法获取当前所处环境的地图信息，以.pgm格式的图片传入，调用opencv库函数将其二值化，得到opencv的矩阵变量Mat，再将其转化成一个二维数组，其值为0或1,0表示可以通过，1表示存在障碍物，二值化过程完成；

步骤2：给定起点和终点，若已经由SLAM等定位算法定位机器人所在的位置，则起点为机器人当前所在点，终点则是目标点。首先将起点和终点置入一个名为open的顺序表中，作为待发射光线的节点，同时将它们存入一张名为allnode的顺序表，allnode是用来储存所有节点的顺序表。

步骤3：当open表非空时，从open表中取出一个点开始以起始发射角0度，即水平向右发射光线，在光线还没有碰到障碍物时，光线逐像素向前推进，每推进一个像素点，都对下一个像素点进行检测，若可通过则继续推进，若不可通过则表示碰到障碍物，此时光线停止推进，并记录该碰撞点坐标；

步骤4：对该碰撞点进行检测，若该碰撞点与光线发出点的距离小于某一阈值，则表示该方向与障碍物十分贴近，该方向的光线是不合理的，应当舍弃。若光线合理，则求取碰撞点与allnode表中所有元素的欧式距离，当其距离小于某一阈值时，则该点舍弃，否则将该点作为新的预备发射节点，同时存入open表和allnode表的表尾，并将该节点与光线发射节点建立连接，连接的具体实现为两个节点的结构体中各自包含一个指针，令两个对象所包含的指针互指，即实现节点连接，亦即拓扑图中的边；

步骤5：给发射角度一个步进角度，重复光线发射的过程，并对光线与障碍物的碰撞点进行步骤4的检测。若角度到达360度则该点的光线发射结束，重复步骤3，重新从open表中取出一个点，重复进行光线发射；

步骤6：当某一光线360度光线发射结束后均无法生成新的节点，则从这个时候开始，open表的内容将会减少，当open表中所有的节点都无法创建新的节点时，open表将为空，此时表明建图完成，已获得一张拓扑地图，其顶点为光线发射点，其边为节点与节点之间的光线连接；

步骤7：在所得的拓扑地图上应用A*寻路算法，A*的启发式代价设置为节点之间光线的距离，从起点开始，依照A*的启发式规则，到终点为止，即可获得连接起点和终点的一条以光路为载体的路径。

如图1，是未改进前的Visual Graph路径规划算法，其应用的地图只能是如图所示的，障碍物均为凸多边形的地图，而且每个障碍物的顶点都需要已知，将起点和终点并障碍物的顶点互相之间建立连接，则可以以连接为载体构成一条从起点到终点的连通路径。路径的优点在于直线度好，拐弯次数少，因而机器人运行时的速度损失小，但由于其只能运用在简单的凸多边形障碍物地图上，故很难应用在实际问题中。

如图3，是本发明改进以后的Visual Graph方法效果，图中的圆点表示光线发射节点，细线表示光路，而粗实线表示最终获得的路径。本发明继承了原有方法的优点，即拐弯次数较少，机器人的速度损失小，并且本发明克服了原有算法致命的缺点，使其可以应用在复杂地图中，而且无需对地图进行预处理操作，使其在实际问题中的应用前景大大的提高。