一种应用VisualGraph算法提取道路中线的方法与流程

本发明涉及智能机器人运动规划领域内的一种提取道路中线的方法，具体是一种应用visualgraph算法思路来提取道路的中线，最终输出道路中线的坐标点以及坐标点之间的连接关系。

背景技术：

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

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

一种优秀的路径规划算法，必须保证机器人运行过程中能够有效地避开障碍物，快速准确地到达目的地。而目前许多的路径规划算法都有一种行为特点，那就是“贴边行走”。如经典的规划算法a*，它所生成的路径往往是紧贴障碍物的边缘的，这样的话，一旦机器人的传感器出现了一些问题，精度不够高的话，机器人很可能就会与障碍物发生碰撞导致不可预料的后果。为了使机器人获得的路径能够更加安全可靠，更加有效地避开障碍物，一般需要机器人能够运动在道路的中线，这样，即使机器人出现一些小的故障，偏离了原来的轨道，机器人也能够不碰撞障碍物，行走安全合法的道路。

现有的道路中线提取办法有voronoi图构造法，此方法是以数学图形voronoi图为理论基础，通过构造广义voronoi图来进行道路中线的提取。但一般意义上的voronoi图并不能表现出道路中线的信息，必须构造广义的voronoi图，而广义voronoi图的构造比较困难，一般采用扩展障碍物边界的方法，直到障碍物边界在某处相遇，则记该处为道路中线。这就首先需要障碍物边界提取技术的支持，扩展障碍物的算法复杂度也相对较大，导致该方法不易实现且计算量过大。

visualgraph是常用的路径规划算法，在1979年首次提出，其主要应用了光线沿直线传播这一思想，利用模拟光线发射，与障碍物相撞得到碰撞点，再以碰撞点为新的发射节点重新发射光线，重复此过程以建立一张以光路为载体的拓扑图。

技术实现要素：

本发明的技术解决问题：克服现有技术的不足，提供一种应用visualgraph算法思路来提取道路中线的方法，最终可以输出道路中线的节点坐标以及节点之间的连接关系，并在地图中绘出道路的中线，以供给机器人一条安全合适的路径进行行走。

本发明的技术解决方案：应用visualgraph算法提取道路中线的方法，其特点在于：应用visualgraph算法思路来提取道路中线，最终输出道路中线的坐标点以及其连接关系，其步骤如下：

步骤1：对地图进行转化，将灰度图或者slam所构建的地图二值化处理，得到转化后的地图，存储为一个二维数组。此过程称为地图二值化过程；

步骤2：针对转化后的二维数组，在可通过的区域开始遍历所有像素点，并从每个可通行的像素点发射光线，每次发射两条射线，起点为所选点，两条射线成180°反向发射，此过程称为光线发射过程；

步骤3：当两条射线碰触障碍物时，记录该碰撞点的坐标，并计算碰撞点到光线发射起点的距离，若两条射线碰撞点到起点的距离相等，则该点即为道路中点，该过程称为判定过程；

步骤4：若两条射线的碰撞点到起点的距离不相等，则将射线顺时针旋转30°再次以发射射线，重复步骤3的判定过程，该过程称为重复发射光线过程；

步骤5：若某像素点发射射线已转过180°而判定过程总不成立，则该点即不是道路中点，开始对下一个像素点进行步骤2、3、4的过程；

步骤6：当所有可通过的像素点均遍历完成后，则道路中线的坐标点即都已生成，道路中线经过的点提取过程结束。

步骤7：为将道路中点连接行成道路中线，即对每一个上述过程获得的点，将其与邻近像素为1的所有点建立连接，即可获得道路中点之间的连接关系，即得道路中线，该过程称为连接过程。

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

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

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

(1)对于某一点，从0°和180°，即水平方向开始向两边发射射线，重复发射射线时，即将射线顺时针旋转30°，直到180°为止；

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

所述步骤2中，遍历像素点的方法为：

从地图的左上角开始直到右下角结束，水平扫描遍历，若某点为不可通过，则直接跳过；若某点为可通过，则进行光线发射过程；

所述步骤3中，判定过程的方法为：

当遍历过程中从某一像素点发出的两向射线都碰到障碍物时，记录两个碰撞点，分别计算其与发射点的距离，若两距离相等，则保留该像素点作为道路中点，结束该点的光线发射过程，否则按照步骤4，将光线顺时针旋转一个角度再次发射光线。若光线已经旋转过180°，则该点不是道路中点，继续判断下一个像素点。

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

将两条射线都顺时针旋转30°，它们之间仍成180°反向发射，直到旋转过180°为止。

所述步骤7中，中点连接的方法为：

将经上述过程所获得的中点都存储为一个顺序表，对顺序表中的每一个元素，都与表中其他元素进行比较，若与之距离为1像素，则建立两点之间的连接；当某一点与所有点比较完成之后，则从顺序表中删去该点。将连接关系和中点存储为另一张顺序表，以供最后输出。

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

(1)本发明的思路是全新的，应用了路径规划算法visualgraph算法的运行思路，采用光线沿直线传播的基本思路，既简单容易理解，并且十分有效。在道路的中线上，它到道路两边的距离是相等的，采用光沿直线传播的策略，用光线来丈量到道路两边的距离，若光线的长度相等，则表示该点到道路两边的距离相等，则是道路中线上的点；

(2)本发明采用多方向发射光线的策略，因为在许多情况下，机器人运行的道路边线并非规则的直线，可能存在多处的波折和不平整，此时可能丢失原本是道路中线的点，也可能出现原本不是道路中线的点。在这里采用多方向发射光线的策略，将两条夹角180°的射线以一定步长不断旋转，直到旋转180°为止，如此多次判定该点的特征，可以更准确的得出道路中线的坐标点；

(3)本发明的连接方法是采用邻近像素点连接的方法，这样，在道路中点产生的时候，可能生成一些冗余的、不是道路中线的点，这些点将会被孤立起来，无法连接入道路中线中去，这样也就可以剔除冗余点，使产生的道路中线合理、正确。

附图说明

图1为直道地图提取效果；

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

图3为slam复杂地图的提取效果。

具体实施方式

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

步骤1：用slam算法或其他建图算法获取所要生成道路中线的地图，以.pgm格式的图片传入，调用opencv库函数imread将其二值化，得到opencv的矩阵变量mat，再将其转化成一个二维数组，数组的每一个元素代表一个像素点，其值为0或1,0表示可以通过，1表示存在障碍物，二值化过程完成；

步骤2：从地图左上角开始遍历整张地图。若某一个像素点是不可通过点，则直接跳过，若某一像素点为可通过点，则从该点开始发射光线。从该点出发，首先水平方向发射两条互相成180°的射线，射线逐像素前进，每前进一个像素就进行一次检测，检测当前点是否为不可通过点，若是，则停止光线的推进；

步骤3：在步骤2中，发射的两条射线到达不可通过点之后，存储碰撞点的坐标，并计算碰撞点和光线发射点的距离，若两个碰撞点到发射点的距离相等，则该点即为道路中线上的点，则该点的光线发射过程立即结束，将该点储存为道路中线点，并进行下一个像素点的光线发射过程；

步骤4：在步骤3中，若两条射线到达不可通过点之后，求得的到光线发射点的距离不相等，则将两条射线的发射方向顺时针旋转一个步长，(此处是30°，可以调整步长来实现精度的调整)，再次重复步骤2的光线发射过程和步骤3的判定过程。；

步骤5：若射线的已经旋转过180°都无法满足步骤3中的判定条件的话，则该点即不是道路中线上的点，应予以舍弃，跳过该点进行下一个像素点的光线发射过程和判定过程；

步骤6：当地图遍历完成，右下角的像素点都已经经过了光线发射和判定过程，则产生道路中线的过程结束，获得了一个顺序表，其中储存的道路中线点的坐标；

步骤7：处理道路中线坐标顺序表，每次取出顺序表头的一个元素，将其和顺序表中所有的元素进行比较，若与某一元素的距离在一个像素，则建立这两点之间的连接，当该元素与顺序表中所有的元素都已经进行比较，则从顺序表中删去该元素，将连接关系和点坐标储存在一个结构体中，并将结构体置入一个新的顺序表中，以供最后输出。对中线坐标顺序表中的每一个元素都重复上述操作，最终完成中线的连接。

如图1，是本发明在直道地图中的效果。图中黑色部分表示不可通过区域，白色部分表示可以通过区域，即道路。现要求提取白色区域道路部分的中线。可以看出本发明在这种横平竖直的道路中提取道路中线的效果还是非常好的。

如图3所示，是本发明在复杂的slam地图上应用的效果，图中深色部分表示不可通过区域，白色部分表示可以通过区域。在slam生成的地图中，可以看出有许多噪点，这会相当大程度影响道路中线的提取，可以看出本发明在这种情况下也能够比较正确地提取出道路的中线，在一般的机器人应用中已经可以完成需求。