基于多层栅格地图的机器人过窄通道的方法及系统

技术领域

本发明涉及机器人技术领域，特别涉及一种基于多层栅格地图的机器人过窄通道的方法及系统。

背景技术

机器人导航过程中会遇到一些狭窄的区域。由于存在地图分辨率、传感器数据误差、定位误差等因素，会出现一些实际能通过而导航算法上却不能通过的窄通道。现有技术还无法解决这些问题。另外一些能通过的窄通道会因为地图的精度不够而规划不出全局路径导致导航失败，机器人无法到达窄通道前面，更无法通过。

发明内容

基于此，有必要提供一种提高过窄通道性能的基于多层栅格地图的机器人过窄通道的方法。

同时，提供一种提高过窄通道性能的基于多层栅格地图的机器人过窄通道的系统。

一种基于多层栅格地图的机器人过窄通道的方法，包括：

规划路径：规划全局路径；

窄通道判断：若得到全局路径，根据当前位置与目标点或预定目的地的路径判断是否穿过窄通道；

减小致命膨胀：若穿过窄通道，则控制机器人从当前位置运动到等候点1，走到等候点1后控制减小致命膨胀；

过窄通道：根据减小后的致命膨胀规划路径，从等候点1运动到等候点2穿过窄通道；

恢复致命膨胀：到达等候点2后，控制恢复致命膨胀；

到达目标地：控制机器人从等候点2运动到目标点或预定目的地。

在优选实施例中，所述规划路径步骤，根据全局路径画出窄通道区域，所述窄通道区域为盖住整个窄通道的矩形区域。

在优选实施例中，所述等候点1、等候点2根据窄通道区域进行设置，分别设置在所述窄通道区域的两端。

在优选实施例中，所述等候点1设置在所述窄通道区域的入口端，所述等候点2设置在所述窄通道区域的出口端，所述等候点1、等候点2设置在所述窄通道区域的中线上或左右距离该中线为分辨率的3单元大小以内。

在优选实施例中，所述减小致命膨胀步骤中，根据栅格地图的分辨率减小致命膨胀。

在优选实施例中，所述减小致命膨胀步骤中，到达等候点1后控制减小致命膨胀减小一单位或二单位分辨率距离大小。

在优选实施例中，所述减小致命膨胀步骤中，到达等候点1后控制减小致命膨胀减小0.05m。

在优选实施例中，所述规划路径步骤中，规划全局路径未得到全局路径则控制减小致命膨胀，然后规划全局路径，恢复致命膨胀，若得到全局路径，进入所述窄通道判断步骤；若未得到全局路径则退出。

在优选实施例中，所述窄通道判断步骤中，若判断不穿过窄通道则控制机器人从当前位置走到目标点或预定目的地；所述减小致命膨胀包括：关闭地图更新线程、删除地图更新线程、设置机器人半径、添加地图更新线程、启动地图更新线程。

一种基于多层栅格地图的机器人过窄通道的系统，包括：

规划路径模块：规划全局路径；

窄通道判断模块：若得到全局路径，根据当前位置与目标点或预定目的地的路径判断是否穿过窄通道；

减小致命膨胀模块：若需穿过窄通道，则控制机器人从当前位置A运动到等候点1，走到等候点1后控制减小致命膨胀；

过窄通道模块：根据减小后的致命膨胀规划路径，从等候点1运动到等候点2穿过窄通道；

恢复致命膨胀模块：到达等候点2后控制恢复致命膨胀；

到达目标地模块：控制机器人从等候点2运动到目标点或预定目的地。

上述基于多层栅格地图的机器人过窄通道的方法及系统，在过窄通道时，通过减小致命膨胀，重新规划过窄通道的路径，通过修改地图配置大大提高通过窄通道的概率，当到达等候点1后才开始减小致命膨胀，当到达等候点2后，恢复致命膨胀，不影响全局路径规划，不会增加机器人的碰撞危险性。

附图说明

图1为本发明一实施例的基于多层栅格地图的机器人过窄通道的方法的流程图；

图2为本发明一实施例的多层栅格地图示意图；

图3为本发明一实施例的基于多层栅格地图的机器人过窄通道的方法及系统的栅格的代价值与障碍物的距离关系；

图4为本发明一实施例的简化窄通道的示意图；

图5为本发明一实施例的减小致命膨胀后的简化窄通道的示意图；

图6为本发明一实施例的窄通道路径的示意图；

图7为本发明一实施例的窄通道的示意图；

图8为本发明一实施例的减小致命膨胀后的窄通道的示意图。

实施方式

以下的实施例便于更好地理解本发明，但并不限定本发明。

如图1所示，本发明的一种基于多层栅格地图的机器人过窄通道的方法，包括：

步骤S101，规划路径：规划全局路径；

步骤S103，窄通道判断：若得到全局路径，根据当前位置A与目标点B或预定目的地的路径判断是否穿过窄通道；

步骤S105，减小致命膨胀：若需穿过窄通道，则控制机器人从当前位置A走到等候点1，走到等候点1后控制减小致命膨胀；

步骤S107，过窄通道：根据减小后的致命膨胀规划路径，从等候点1走到等候点2穿过窄通道；

步骤S109，恢复致命膨胀：到达等候点2后控制恢复致命膨胀；

步骤S111，到达目标地：控制机器人从等候点2走到目标点B或预定目的地。

当机器人走到目标点B后，B点成为下一个路段或行程段的起点即下一路径的当前位置。

进一步，本实施例的步骤S101，规划路径，根据全局路径规划出窄通道区域。窄通道区域为盖住某个窄通道的整个窄通道的矩形区域。

本实施例优选的，规划路径，可使用的是dijkstra算法。当然也可采用其他算法或方法进行规划路径。

进一步，本实施例的等候点1、等候点2根据窄通道区域进行设置，分别设置在窄通道区域的两端。

进一步，本实施例的等候点1设置在窄通道区域的入口端。等候点2设置在所述窄通道区域的出口端。等候点1、等候点2设置在窄通道区域的中线上或左右离该中线分辨率的3个单元大小以内。

本实施例中，若得到全局路径则存储路径点的集合不为空。得不到全局路径则存储路径点的集合为空。

进一步，本实施例的步骤S105，减小致命膨胀步骤中，根据栅格地图的分辨率减小致命膨胀。

进一步，本实施例的步骤S105，减小致命膨胀步骤中，到达等候点1后控制减小致命膨胀减小一单位或二单位分辨率距离大小。

优选的，本实施例的减小致命膨胀步骤中，到达等候点1后控制减小致命膨胀减小0.05m。

若用的栅格地图是0.05米的分辨率，只需减小一次即可。如果栅格地图的分辨率为0.025米则可以分两次减小。致命膨胀距离恢复时一步到位。

减小致命膨胀包括：关闭地图更新线程->删除地图更新线程->设置机器人半径->添加地图更新线程->启动地图更新线程。

本实施例的栅格地图为多层栅格地图，包括：静态层、障碍物层、膨胀层等。

如图2所示，栅格地图的示意图，栅格中的数字仅为说明，不为限定之用。本实施例的减小致命膨胀是在膨胀层实现的。设置膨胀层的是为了把机器人当作一个质点便于规划路径，将障碍物膨胀一定距离。栅格地图也可以叫代价地图，每个栅格都有一个代价值，范围为0-255，代价值越高是障碍物的概率越大。

如图3所示，栅格的代价值与障碍物的距离关系。衰减系数：scaling_factor =20.0 （可设置），致命膨胀（米）：inscribed_radius = 0.1（可设置），栅格与障碍物的距离（米）：distance。距离在致命膨胀范围内的代价值为253，不采用下面的公式计算。系数：factor = exp(-1.0 * scaling_factor * (distance - inscribed_radius))；栅格的代价值：cost = (取整)(252 * factor)。

如图4至图6所示，矩形框为设置好的窄通道区域。等候点1是机器人穿过窄通道前需要先到达的点。等候点2为窄通道对面的等候点。A到B只能穿过窄通道后到达。本实施例优选的将栅格地图的分辨率设置为0.05m。

如图4和图7所示，窄通道内的可行区域非常狭窄或完全堵死。致命膨胀的距离是将机器人当作一个质点参与运算，将障碍物周围一定范围内设置为不允许机器人进入。导航过程中，致命膨胀的值可以根据需要重新设置，致命膨胀稍微减小后大部分实际环境中机器人通过窄通道不会发生碰撞。如图5和图8所示，本实施例中，优选的将减小致命膨胀时将致命膨胀减小0.05米。为了保证通过性和防止发生碰撞，致命膨胀的大小不能无限制增大或缩小，致命膨胀的缩小大小与实际使用中传感器的数据误差、地图分辨率、定位误差有关系。本实施例中，致命膨胀减小的数值为栅格地图分辨率的单元格大小。

进一步，本实施例的步骤S101，规划路径步骤中，规划全局路径未得到全局路径则控制减小致命膨胀，然后规划全局路径，恢复致命膨胀，若得到全局路径，进入所述窄通道判断步骤；若未得到全局路径则退出。

进一步，本实施例的步骤S103窄通道判断步骤中，若判断不需穿过窄通道则控制机器人从当前位置A走到目标点B或预定目的地。

本发明一实施例的基于多层栅格地图的机器人过窄通道的系统，包括：

规划路径模块：规划全局路径；

窄通道判断模块：若得到全局路径，根据当前位置A与目标点B或预定目的地的路径判断是否穿过窄通道；

减小致命膨胀模块：若需穿过窄通道，则控制机器人从当前位置A走到等候点1，走到等候点1后控制减小致命膨胀；

过窄通道模块：根据减小后的致命膨胀规划路径，从等候点1走到等候点2穿过窄通道；

恢复致命膨胀模块：到达等候点2后控制恢复致命膨胀；

到达目标地模块：控制机器人从等候点2走到目标点B或预定目的地。

进一步，本实施例的规划路径模块，根据全局路径规划出窄通道区域。窄通道区域为盖住某个窄通道的整个窄通道的矩形区域。

本实施例优选的，规划路径，可使用的是dijkstra算法。当然也可采用其他算法或方法进行规划路径。

进一步，本实施例的等候点1、等候点2根据窄通道区域进行设置，分别设置在窄通道区域的两端。

进一步，本实施例的等候点1设置在窄通道区域的入口端。等候点2设置在所述窄通道区域的出口端。等候点1、等候点2设置在窄通道区域的中线上或左右离该中线分辨率的3个单元大小以内。

本实施例中，若得到全局路径则存储路径点的集合不为空。得不到全局路径则存储路径点的集合为空。

进一步，本实施例的减小致命膨胀模块中，根据栅格地图的分辨率减小致命膨胀。

进一步，本实施例的减小致命膨胀模块中，到达等候点1后控制减小致命膨胀减小一单位或二单位分辨率距离大小。

优选的，本实施例的减小致命膨胀模块中，到达等候点1后控制减小致命膨胀减小0.05m。

进一步，本实施例的规划路径模块中，规划全局路径未得到全局路径则控制减小致命膨胀，然后规划全局路径，恢复致命膨胀，若得到全局路径，进入所述窄通道判断步骤；若未得到全局路径则退出。

进一步，本实施例的窄通道判断模块中，若判断不需穿过窄通道则控制机器人从当前位置A走到目标点B或预定目的地。

本实施例中机器人在A点当前位置，机器人要从A点去B点，进行全局规划，从A点到等候点1，然后穿过窄通道，到达等候点2，最后到达B点。本实施例中规划路径步骤，规划全局路径，若规划不出全局路径则减小致命膨胀再规划一次，若减小致命膨胀后还是规划不出全局路径则说明目标点不可达，退出。窄通道判断步骤判断全局路径是否穿过窄通道，若不穿过窄通道则直接控制机器人运动到B点。若穿过窄通道，控制机器人从A点运动到等候点1，到达等候点1减小致命膨胀，规划路径，控制机器人运动到等候点2，到达等候点2恢复致命膨胀，控制机器人运动到B点。

现有技术没有对地图进行修改，在地图上对于窄通道的宽度刚好在临界宽度时，机器人很可能判断不能通过，但现实中很多场景时可以通过的。本发明通过修改地图配置后大大提高了通过窄通道的概率，与在路径规划和速度控制上优化结合一起使用效果更好。

以上述依据本申请的理想实施例为启示，通过上述的说明内容，相关工作人员完全可以在不偏离本项申请技术思想的范围内，进行多样的变更以及修改。本项申请的技术性范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。