一种动态环境下的机器人导航系统及方法

1.本方法属于机器人导航技术领域。具体涉及一种动态环境下的机器人导航系统及方法。


背景技术：

2.当今机器人导航领域越来越火，各种新的导航技术也是层出不穷。在整个过程中由于数据处理的时延特性，路径规划的速度远远快于导航速度。此领域诞生了许多优秀的方法包括rrt-connect、mt-rrt、risk-rrt等方法。这些新方法、新技术的出现大大推动了自动驾驶技术的进步。
3.文献cn202011145680.8公开了一种采用a
*
算法和rrt算法融合的方法，提高了路径规划的性能。但是其需要对地图数据完全已知情况下a
*
才可规划出一条最优路径。对地图完整数据依赖性较强，并且未考虑动态环境下的情况，无法实现动态环境下的导航。文献cn201911086247.9公开了一种改进动态rrt
*
的移动机器人运动规划方法，考虑了动态障碍物的影响，但是rrt
*
算法的收敛时间是一个比较突出的问题，复杂路况下，计算时间过长，实现规避障碍动态导航的效果不佳。


技术实现要素：

4.本发明需要解决的技术问题是提供一种动态环境下的机器人导航系统及方法，实现在复杂的动态环境下快速生成导航路线，有效规避动态障碍物，实现快速准确高质量的动态导航。
5.为解决上述技术问题，本发明所采用的技术方案是：
6.一种动态环境下的机器人导航系统，包括：地图处理模块、指导路径生成模块和导航模块，所述导航模块包括：启发式模块、障碍物检测模块和碰撞风险检测模块。
7.一种动态环境下的机器人导航方法，步骤如下：
8.步骤1：加载地图库中数据，获得地图的静态障碍物空间数据集c
sobj
和自由空间数据集c
free
；
9.步骤2：给定终点和插入辅助生成指导路径l的插入点，确定指导路径子集ln和需进行rrt探索的节点；
10.步骤3：根据规划算法生成完整的指导路径；
11.步骤4：指导路径生成后作为启发式引导路径完成导航路径l
nav
生成；
12.步骤5：导航过程中当遇到移动障碍物c
dobj
时，碰撞风险检测模块会规划可行性路线，并选择一条碰撞风险最小的路径作为导航路径进行避障操作；
13.步骤6：当移动障碍物消失，导航过程又在在启发式模块和障碍物检测模块以及指导路径中节点的启发域作用下快速向终点生长，当遇到移动障碍物时就返回执行步骤5，直到到达终点域，完成导航过程。
14.本发明技术方案的进一步改进在于：步骤1中加载地图库中数据进行二值化处理，
或者直接加载二值化后的地图数据，获得地图的静态障碍物空间数据集c
sobj
和自由空间数据集c
free
。
15.本发明技术方案的进一步改进在于：步骤2在地图中在给定终点后，起点和终点在地图中坐标都为已知，起点坐标x
init
(x
init
，y
init
),终点坐标x
end
(x
end
，y
end
)，在地图上起点和终点的连线上插入n个节点作为指导路径生成的辅助节点，使得指导路径l可以更快生成，插入节点的坐标计算公式：
[0016][0017][0018]
表示把线段分成n等份。判断各点(xn，yn)是否在自由空间内，即(xn，yn)∈c
free
，若在则保留，否则删除。
[0019]
本发明技术方案的进一步改进在于：步骤2在不考虑动态障碍物的前提下，从起点到终点依次连接剩余各点，然后判断每条线段的集合ln(n＝1，2，3
…
)是否在自由空间c
free
内，若则删除ln集合，若ln∈c
free
则保留此线段作为构建完整指导路径l的一个备用子集ln。
[0020]
本发明技术方案的进一步改进在于：步骤2连接失败的每个点向连接失败的方向进行随机树探索。
[0021]
本发明技术方案的进一步改进在于：步骤三当两个树的节点欧式距离小于一个时间步距离d
t
就进行连接合并成一个树继续进行随机树探索，循环执行此步骤，直到起点和终点在同一树中，即(x
init
∪x
end
)∈l；时间步距离d
t
是单位时间内随机树探索的距离，这里设d
t
＝1；当检测(x
init
∪x
end
)∈l，即表示一条完整的指导路径构建完成；指导路径l并不作为导航路径l
nav
使用，仅作为导航过程中的一条启发式路径。
[0022]
本发明技术方案的进一步改进在于：步骤4当指导路径构建完成后，则使用完整的指导路径数据作为启发式路径进行导航路径的构建，使用完整的指导路径数据作为启发式路径完成导航过程，指导路径节点处的圆域为启发域δ，启发域的的节点在导航路径生成过程中起到吸引的作用，当导航路径进入启发域即向指导路径的下一个节点生长。
[0023]
本发明技术方案的进一步改进在于：步骤5具体包括:在导航过程中启发式模块和障碍物检测模块实时开启，当遇到移动障碍物c
dobj
时才会启用基于高斯分布的碰撞风险检测模块。局部地图网格化处理，假设移动障碍物与静态障碍物不会重叠，根据静态障碍物与移动障碍物在网格地图中的位置和移动障碍物的运动预测，分别计算规划路径中机器人与静态障碍物和移动障碍物的碰撞风险，并选择一条碰撞风险最小的路径作为导航路径；当移动障碍物消失，导航过程又在在启发式模块和障碍物检测模块以及指导路径中节点的启发域δ作用下快速向终点生长，启发域半径δr为一个时间步距离d
t
的一半；当遇到移动障碍物时就执行上述步骤，直到到达终点域，终点域半径为一个时间步距离d
t
的一半。
[0024]
由于采用了上述技术方案，本发明取得的技术进步是：
[0025]
通过采用插入辅助点的方法以及mt-rrt的多树同时探索性和rrt-connect子树互
相连接的快速性进行指导路径的探索和生成，使得指导路径的生成速度得到提升。指导路径l的存在也为导航过程进行了加速。在导航过程中以指导路径为启发路径即启发式模块的引用，避免了直接使用指导路径，而不用考虑两点边值问题的求解问题。碰撞风险检测算法的加入，也提高了系统动态环境下的效率。
附图说明
[0026]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图；
[0027]
图1一种动态环境下的机器人导航系统图；
[0028]
图2一种动态环境下的机器人导航方法流程图；
[0029]
图3起点终点和插入点位置图；
[0030]
图4连接各点后各线段与静态障碍物关系图；
[0031]
图5连接失败各点进行rrt探索图；
[0032]
图6探索过程中，两棵树结合为一棵树并继续探索；
[0033]
图7指导路径生成完毕和移动障碍物图；
[0034]
图8指导路径、导航路径和移动障碍物相互影响图；
[0035]
图9指导路径对导航路径的启发式偏置图；
[0036]
图10指导路径、碰撞风险检测算法和移动障碍物不同状态共同影响下规划出的导航路径图。
具体实施方式
[0037]
下面结合实施例对本发明做进一步详细说明：
[0038]
如图1所示，为动态环境下的机器人导航系统组成包括地图处理模块、指导路径生成模块和导航模块。
[0039]
如图2所示，为动态环境下的机器人导航方法，步骤如下所示：
[0040]
步骤1：加载地图库中数据，获得地图的静态障碍物空间数据集c
sobj
和自由空间数据集c
free
；
[0041]
步骤2：给定终点和插入辅助生成指导路径l的插入点，确定指导路径子集ln和需进行rrt探索的节点；
[0042]
步骤3：根据规划算法生成完整的指导路径；
[0043]
步骤4：指导路径生成后作为启发式引导路径完成导航路径l
nav
生成；
[0044]
步骤5：导航过程中当遇到移动障碍物c
dobj
时，碰撞风险检测模块会规划可行性路线，并选择一条碰撞风险最小的路径作为导航路径进行避障操作；
[0045]
步骤6：当移动障碍物消失，导航过程又在在启发式模块和障碍物检测模块以及指导路径中节点的启发域作用下快速向终点生长，当遇到移动障碍物时就返回执行步骤5，直到到达终点域，完成导航过程。
[0046]
实施例一：
[0047]
如图3-10所示，为在动态环境下的机器人导航过程，通过地图处理模块加载地图库中数据，二值化处理，或者直接加载地图库中二值化后的地图数据。获得地图的静态障碍物空间数据集c
sobj
和自由空间数据集c
free
。图3中黑色大四边形边框为静态障碍物，空白区域为自由空间，
①
为起点，
②
为插入点，
③
为终点。在给定终点后，起点和终点在地图中坐标都为已知，起点坐标x
init
(x
init
，y
init
),终点坐标x
end
(x
end
，y
end
)。在地图上起点和终点的连线上插入n(图3中n＝2)个节点作为指导路径生成的辅助节点，使得指导路径l可以更快生成。插入节点的坐标计算公式：
[0048][0049][0050]
表示把线段分成n等份。判断路径l上各点(xn，yn)是否在自由空间内，即(xn，yn)∈c
free
，若在则保留，否则删除。
[0051]
在不考虑动态障碍物的前提下，从起点到终点依次连接剩余各点。然后判断每条线段的集合ln(n＝1，2，3
…
)是否在自由空间c
free
内，若则删除ln集合。如图4中的
④
所示，虚线与障碍物有交集，所以需要删除。若ln∈c
free
则保留此线段作为构建完整指导路径l的一个备用子集ln，如图4中的细实线。
[0052]
连接失败的每个点向连接失败的方向进行随机树探索如图5所示，当两个树的节点欧式距离小于一个时间步距离d
t
就进行连接合并成一个树继续进行随机树探索，如图5中
⑤
和图6中
⑥
所示。循环执行此步骤，直到起点和终点在同一树中即(x
init
∪x
end
)∈l。一个时间步距离d
t
是单位时间内随机树探索的距离，这里设d
t
＝1。当检测(x
init
∪x
end
)∈l即表示一条完整的指导路径构建完成，如图7中
⑦
所示。指导路径l并不作为导航路径l
nav
使用，仅作为导航过程中的一条启发式路径。
[0053]
当指导路径构建完成之前起点处的导航路径依靠起点处指导路径的子集l1作为启发式路径进行构建，当指导路径构建完成后，则使用完整的指导路径数据作为启发式路径完成导航过程。启发式过程如图9所示。细实线为指导路径，粗实线为导航路径。指导路径节点处的圆域为启发域δ，如图9中启发域δ在导航路径生成过程中提供启发式偏置。启发域δ的的节点在导航路径生成过程中起到吸引的作用，当导航路径进入启发域δ即向指导路径的下一个节点生长。
[0054]
在导航过程中启发式模块和障碍物检测模块实时开启，当遇到移动障碍物c
dobj
时才会启用基于高斯分布的碰撞风险检测模块。局部地图网格化处理，假设移动障碍物与静态障碍物不会重叠，根据静态障碍物与移动障碍物在网格地图中的位置和移动障碍物的运动预测，分别计算规划路径中机器人与静态障碍物和移动障碍物的碰撞风险，并选择一条碰撞风险最小的路径作为导航路径，如图10所示，细实线为指导路径，带箭头的三角形为移动障碍物c
dobj
如图7中
⑧
所示。如图10所示。细实线为指导路径，带箭头的三角形为移动障碍物c
dobj
如图7中
⑧
所示。如图10中所示粗实线为碰撞风险检测算法作用时系统
生成的可行性避障路径。当移动障碍物消失，导航过程又在在启发式模块和障碍物检测模块以及指导路径中节点的启发域δ作用下快速向终点生长，启发域半径δr为一个时间步距离d
t
的一半。当遇到移动障碍物时就执行上述步骤，直到到达终点域，终点域半径为一个时间步距离d
t
的一半。
[0055]
到达终点域后导航过程结束，如图8所示，细实线为指导路径，粗实线为导航路径。图8中
⑨
是导航过程中遇到移动障碍物后，系统在启发式模块、障碍物检测模块、碰撞风险检测模块共同作用下生成的可行性避障路径。
[0056]
以上所述的实施例仅仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。