一种基于改进RRT算法的机器人实时路径规划方法与流程

一种基于改进rrt算法的机器人实时路径规划方法
技术领域
1.本发明涉及路径规划技术领域，具体涉及一种基于改进rrt算法的机器人实时路径规划方法。


背景技术：

2.随着时代的发展，越来越多的机器人替代了人类完成一些工作，比如搬运东西、在餐厅送餐、在一些场所为人类提供服务等。但目前这些机器人的行走路线大部分是制定好的、唯一固定不变的，所以机器人有许多地方都不能到达且显得不灵活，并且当障碍物在机器人行走的路线上时，还容易造成碰撞事件。在障碍物流动的场景中，安全高效地引导机器人是一个至关重要但仍具有挑战性的问题。传统方法中将路径规划问题转化为最优化问题，模型复杂，计算量大，实时效率较低，且不能展示出机器人运动的灵活性。
3.快速搜索随机树算法(rapidly
‑
exploring random tree，rrt)是一种采用增量方式增长的随机采样算法，rrt算法可以解决机械臂、机器人或其他物体的路径规划问题。但对于实时变化的环境，传统的rrt算法难以完成实时路径规划，即使可以进行实时路径规划，其路径规划速度也较慢。


技术实现要素：

4.本发明解决的技术问题是：克服现有技术的不足，提供一种基于改进rrt算法的机器人实时路径规划方法，通过划分区域提高路径规划的速度，在一定范围内根据实时变化的障碍物实时改变随机树的节点及其连接方式，从而实现实时路径规划。
5.本发明解决技术的方案是：一种基于改进rrt算法的机器人实时路径规划方法，按如下步骤进行路径规划：
6.步骤1：根据机器人所处的环境，将整个环境进行分区，根据机器人的起始点与终止点，省略不需要进行路径规划的区域，确定需要路径规划的区域；
7.步骤2：针对当前需要路径规划的所有区域按照rrt算法进行静态路径规划，确定包含节点的随机树及初始运动路径；
8.步骤3：机器人按照运动路径开始运动，每运动到一个节点均以该节点为中心确定一定范围内是否由于障碍物的影响导致机器人不能通过，若不能通过，则发送指令控制机器人等待，延迟一定时间重新进行判断，直至存在能够通过的路径，转步骤4重新进行局部路径规划；若能通过，直接转步骤4；
9.步骤4：在步骤3确定的一定范围内，以机器人运动到的当前节点为局部路径起始点，根据步骤2的随机树以及障碍物的实时位置，实时规划局部路径起始点及其附近节点之间的连接关系，确定新的运动路径；
10.步骤5：判断是否抵达终点，若抵达终点则终止循环，否则，返回步骤3。
11.进一步的，步骤1中相邻的区域之间通过开关进行隔断或连接，其中隔断的两个区域之间通过开关表明二者之间存在障碍，不能进行路径规划，连接的两个区域之间能够进
行路径规划。
12.进一步的，所述步骤2中静态路径规划中不仅将节点作为选择父节点的集合，还将这些节点的祖先作为选择父节点的集合。
13.进一步的，所述步骤3中的一定范围为(6
‑
10)l，其中l为相邻节点之间的距离。
14.进一步的，所述步骤4中通过下述方式进行局部路径规划：
15.如果从当前节点记为a点沿初始运动路径上的后面的节点变为不可抵达的节点，则在随机树中去除这个节点，保持其他节点不变，确定父节点被去除的节点，将父节点被去除的节点分别记为bi点，i＝1、2
…
；
16.从随机树中分别匹配bi点的父节点，分别选取从a点至某一bi点累加欧式距离最短的父节点作为该bi点新的父节点，依据新的父节点更新各个节点之间的连接方式，完成局部路径规划。
17.进一步的，所述步骤4中局部路径规划完成之后且机器人运行尚未超过去除节点，若该去除节点处的障碍物离开，则在去除节点的父节点与子节点之间增加一个中点作为新的节点；生成新节点后按照累加欧式距离最小化的原则修剪随机树中步骤3中一定范围内节点的连接方式，重新进行局部路径规划。
18.进一步的，所述步骤4中，保证步骤3中一定范围内的节点总数与原来随机树中对应范围内节点总数相等。
19.进一步的，所述步骤5中，如果当前位置距离终点小于预设的距离，并且从当前位置直线运动到终点无障碍物，则终止循环。
20.进一步的，所述预设的距离为机器人的一个步长。
21.本发明方法适用于服务机器人、无人驾驶玩具车、自主导航无人机。
22.本发明与现有技术相比的有益效果是：
23.本发明提出一种区域可选式重生rrt算法，在具有移动的障碍物的环境中，对服务性机器人的行走路线进行实时规划。本发明在保留rrt算法的随机性的基础下，根据起始点与目标点进行区域选择，路径规划过程中考虑尽可能少的区域，只是针对于机器人快要接近的区域进行实时路径规划，可以减小许多不必要的路径规划，提高了计算效率。在一定范围内根据动态障碍物调节随机树中的节点之间的连接关系，并且在动态障碍物离开后根据中点原则，重生一些新的节点，即本发明实时规划了障碍物经过的节点及这些节点连接的父节点、子结点、父父节点、子子结点。在实时路径规划中分析了移动障碍物附近的树节点是否需要去除、如何去除、添加、重新连线，避免了全局规划，从而提高频繁变化环境下路径规划的效率。
附图说明
24.图1本发明的流程图；
25.图2机器人的运动环境模型图；
26.图3重生路径的过程示意图；
27.图4未划分区域下的随机树分布图；
28.图5关闭不需要的区域后的随机树分布图；
29.图6未划分区域对应的箱形图；
30.图7关闭不需要的区域对应的箱形图；
31.图8考虑动态障碍物之前规划的路径；
32.图9考虑动态障碍物之后规划的路径；
33.图10本发明区域可选式重生rrt算法下规划出的路径。
具体实施方式
34.下面结合实施例对本发明作进一步阐述。
35.通过划分区域提高路径规划的速度，在一定范围内根据实时变化的障碍物实时改变随机树的节点及其连接方式，从而实现实时路径规划，
36.下面结合图1所示的流程图，具体分析如何高效的规划出服务机器人的实时运动路径。
37.具体实施步骤如下：
38.步骤1：根据机器人所处的环境，将整个环境进行分区，根据机器人的起始点与终止点，省略不需要进行路径规划的区域，确定需要路径规划的区域。
39.本发明可适用于一些移动物体在一定环境中进行实时路径规划，应用环境需要具有范围限制，例如：超市、酒店、娱乐场所、学校、车站。在划分区域时可以将一些特殊的位置(例如：门、分隔物体、墙体的延长线、等距划分线)视为开关，从而便于划分区域。
40.在进行路径规划之前将服务机器人所在的运动环境进行分区，本文以图2所示的模型作为服务机器人的运动环境，将整体环境分为十个区域，每个区域之间通过开关(图中的十字符号即为开关)进行隔断或连接，例如区域1和区域2之间通过2个开关决定是否连通、区域1和区域3之间通过1个开关决定是否连通、区域2和区域3之间始终不连通。图中的黑色物体为障碍物。
41.如果接到机器人运动的任务，则根据运动任务制定起始点与终止点，并根据这两点设定好各个分区的开关点，可以减小许多不必要的路径规划，从而提高路径规划的效率。相邻的区域之间通过开关进行隔断或连接，其中连接的两个区域之间能够进行路径规划，而隔断的两个区域之间通过开关表明二者之间存在障碍，不能进行路径规划。
42.步骤2：针对当前需要路径规划的所有区域按照rrt算法进行静态路径规划，确定包含节点的随机树及初始运动路径，为后面的实时路径规划提供可刷新的初始路径。
43.选用quick
‑
rrt*算法进行静态路径规划，该算法相对于rrt*算法，不仅将球形范围内的节点作为选择父节点的集合，还将这些节点的祖先作为选择父节点的集合。
44.步骤3：机器人按照运动路径开始运动，每运动到一个节点均以该节点为中心确定一定范围内是否由于障碍物的影响导致机器人不能通过，若不能通过，则发送指令控制机器人等待，延迟一定时间重新进行判断，直至存在能够通过的路径，转步骤4重新进行局部路径规划。
45.各步骤中以节点为中心确定一定范围，该范围为(6
‑
10)l的圆，其中l为相邻节点之间的距离。
46.步骤4：在步骤3确定的一定范围内，以机器人运动到的当前节点为局部路径起始点，根据步骤2的随机树实时规划局部路径起始点及其附近节点之间的连接关系，确定新的运动路径。
47.如果从当前节点记为a点沿初始运动路径上的后面的节点变为不可抵达的节点，则在随机树中去除这个节点，保持其他节点不变，确定父节点被去除的节点记为b点。从随机树中确定b点的父节点，选取从a点至b点累加欧式距离最短的父节点作为新的路径规划节点，完成局部路径规划。
48.局部路径规划完成之后且机器人运行尚未超过去除节点，若该去除节点处的障碍物离开，则在去除节点的父节点与子节点之间增加一个中点作为新的节点；并根据累加欧式距离最小化的原则，重新进行局部路径规划。
49.下面以具体事例讲述步骤4。假设“静态”路径的局部图如图3(a)所示，且目前的服务机器人运动到了x
now
点，则需要实时规划距离较近的一些节点及附近节点之间的连接关系。图中的黑色圆与黑色实线分别为节点和连接方式，虚线圆与虚线分别为需要删除的节点和需要删除的连接方式，灰色实线为新生成的连接方式，填充物为障碍物。假设动态障碍物运动到了图3(b)所示的位置，则会使得x
11
点与x
12
点为不可抵达的节点，在随机树中去除这两个节点，保持其他节点不变，重新规划父节点被去除的节点，如图3(a)中的x
111
点、x
112
点、x
121
点与x
122
点的父节点发生了变化，把这四个节点变为了图3(b)中的x
21311
点、x
2131
点、x
213
点与x
212
点，此时的路径为可通过路径。在重新匹配了这些节点的父节点后，需要根据个节点之间的欧式距离更新个节点之间的连接方式，从而降低各个节点的累加欧式距离，例如图3(c)中x
213
点沿着两条路径到达x
2132
点的欧式距离满足下式：
50.|x
213
x
2131
| |x
2131
x
2132
|>|x
213
x
2132
|
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
51.根据累加欧式距离最小化的原则进行重新规划各个节点之间的连接方式。
52.假设动态障碍物运动到了图3(d)所示的位置，可以看出没有可通过的路径，此时服务机器人只能停在目前所在的x
now
点，并发送信号，过一段时间再重新规划局部路径，判断能否通过。图中没有被动态障碍物覆盖的节点依旧存储在随机树中，只不过有些节点无法连接至x
now
点。
53.假设动态障碍物运动到了图3(e)所示的位置，首先根据累加欧式距离最小化的原则，为没有父节点的节点重新选择父节点，例如图中x
111
点可以选择图中x1点作为新的父节点，但由于随机树中的节点总数减小，且x1点与x
111
点之间的距离过远，需要重新生成一个新的节点来减小相邻父节点之间的距离，选取这两个点的中点x
11
点作为新的节点。由于最初的随机树中x1点仅有两个子结点，所以这里也只重新生成两个新的子结点x
11
点与x
12
点，并按照累加欧式距离最小化的原则修剪了随机树中一些节点的连接方式。同理，在图3(f)中将x
14
点与x
211
点之间的连接断开，连接了x2点与x
211
点，并在中间生成了新的节点x
21
点。
54.本算法在这个步骤中有以下两个特点：1.原有路径被动态障碍物遮挡后，会根据累加欧式距离最小化的原则重新调整各个节点的连接关系，并生成新的路径；2.在动态障碍物运动过后，选取一些父子节点之间的中点，生成了新的节点，并保证节点总数与原来路径节点总数相等。
55.步骤5：判断是否抵达终点，即如果当前位置距离终点小于预设的距离，并且从当前位置直线运动到终点无障碍物，则认为抵达终点，终止循环，否则，返回步骤3。
56.下面通过仿真来验证本发明。
57.首先分析将区域划分，能否提高路径规划的效率。图4与图5中的十字点为机器人的起始点和目标点，图中的粗线段为规划的运动路径，如果不划分区域，仿真得到图4所示
的随机树分布图，从图中可以看出随机树在每个区域都有节点，随机树的分布范围较广。如果划分区域，关闭不需要的区域，只保留需要通过的区域，仿真得到图5所示的随机树分布图，从图中可以看出，在关闭一些区域后，避免了许多不需要的路径规划，树状图中的节点也都在最终路径附近。这里是仿真1次得到的随机树分布图，下面分析仿真多次后，是否划分区域的仿真结果。
58.若未划分区域，仿真100次得到如图6所示的箱形图，从图中可以看出未划分区域时，规划出一条可行路径的平均值为17.96s，最大值、上四分数、下四分数、最小值分别为46.93s、29.16s、10.73s、2.08s。若划分区域，仿真100次得到如图7所示的箱形图，从图中可以看出划分区域后，规划出一条可行路径的平均值为1.97s，最大值、上四分数、下四分数、最小值分别为3.05s、2.24s、1.72s、1.14s。从图及数据中可以看出划分区域后规划出一条可行路径的时间远远小于未划分区域时规划出一条可行路径的时间。
59.通过仿真发现，根据机器人的起始点与终止点，关闭一些不需要的区域可降低路径规划所需时间，大大提高规划效率，验证了本方法的高效性。
60.若采用传统的quick
‑
rrt*算法对实时变化的环境规划路径，当机器人运动到图中圆圈位置时，如果不考虑动态障碍物，则路径和之前规划的路径相同，如图8所示。如果考虑动态障碍物所在的位置，则规划出如图9所示的路径。从图中可以看出若障碍物位置不发生改变的话，则机器人沿着规划的路径运动则不会碰到障碍物，但图中的障碍物为动态障碍物，其位置会随着时间的变化而改变。因此本发明在传统的quick
‑
rrt*算法的基础上，提出了一种在一定范围内重生路径的新型rrt算法。
61.本发明提出的区域可选式重生rrt算法不考虑r半径区域之外的动态障碍物与静态障碍物，并且还不考虑其他区域中的障碍物。通过仿真得到如图10所示的路径，当机器人运动到图中上方的半径为r的圆的圆心位置时，由于动态障碍物未影响机器人的运动，所以规划的路径未变化。当机器人运动到图中下方的圆的圆心位置时，动态障碍物遮挡住了为机器人规划的初始路径，按照本发明讲述的算法原理，重新规划各个节点之间的连接方式，规划得到新的路径。
62.通过仿真发现区域可选式重生rrt算法通过划分区域并省略了不需要规划的区域，从而减小了规划所需时间。并且利用该算法进行实时路径规划可解决动态障碍物存在的问题。
63.本发明未详细说明部分属于本领域技术人员的公知常识。