CN116339334A - 一种机器人最优路径规划调度系统及方法 - Google Patents

Abstract

本发明公开一种机器人最优路径规划调度系统及方法，其方法包括步骤：S1、通过摄像头采集当前待采集检测的目标节点和当前机械臂的状态节点的三维坐标参数；S2、通过快速扩展树算法遍历当前机械臂的状态节点到目标节点的所有路径；S3、计算所有路径中各个单一路径的时间数值；S4、将时间数值逐一输入冒泡程序中，获取最短时间数据值；S7、判断多个最短时间数据值中机械臂运转最短路径输出为机械臂所需运行最优路径；S8、机械臂接收当前机械臂的状态节点到目标节点的最优路径，并控制相应关节依次执行最优路径的运行参数。本发明实现单一机械臂动态避障路径规划无法实现多个机械臂工作对应的多个目标节点的路径规划的作用。

Description

一种机器人最优路径规划调度系统及方法

技术领域

本发明属于机器人技术领域，具体来说，涉及一种机器人最优路径规划调度系统及方法。

背景技术

生物样本检测过程中，对采集的样本溶液需要进行稀释后再逐一检测，该过程需要机械臂的配合，减少人工检测不安全性和失准的情况产生。机械壁通常以转动、伸缩和摆动到达指定坐标点位的整个运动过程。

在专利号为CN201811398581.3的中国发明专利中，公开了一种基于快速随机搜索树的七自由度冗余机械臂动态避障路径规划方法，包括以下步骤：步骤一：根据目标点的位置，利用冗余机械臂逆运动学的解析解法确定机械臂的最优目标状态；步骤二：以最优目标状态为目标节点，在关节空间构建搜索树，规划一条无碰路径；步骤三：从步骤二规划好的路径中选取前k个节点作为当前路径输入给机械臂，然后将第k个节点更新为根节点；步骤四：根据障碍物的位置变化，实时更新环境地图，并对搜索树重新布线；步骤五：若关节空间上的目标节点被障碍物遮挡，则重新计算当前环境下的最优目标状态作为新的目标节点。利用解析解求解机械臂逆运动学然后通过优化目标函数确定目标节点的方法，避免了冗余机械臂目标状态不确定性的问题；将规划过程分为离线规划和在线规划两个过程，解决了因随机搜索计算复杂度过高导致无法用于冗余机械臂实时避障的问题；在线规划阶段，利用搜索树的扩展和重新布线达到避开动态障碍物的目的，当原目标节点被动态障碍物遮挡导致目标不可达时，能够切换目标节点重新搜索一条新的路径。

现有专利的缺陷在于，上述方法实现了单一的机械臂动态避障路径规划，但对于多样本检测流程需要考虑避障路径规划的同时，流水线的多样本检测还需要考虑采集机械臂到达目标节点的运行时间成本。现有技术中，缺乏对多个机械臂和多目标节点的最优时间的路径规划调度方法。

发明内容

针对现有机械臂动态避障路径规划方案中缺乏对多个机械臂和多目标节点的最优时间的路径规划调度方法的问题，本发明提供了一种机器人最优路径规划调度系统及方法。

为实现上述技术目的，本发明采用的技术方案如下：

一种机器人最优路径规划调度系统，包括样本坐标采集模块、机械臂初始状态采集模块、限制屏障采集模块、路径遍历模块、路径动作次数计算模块、路径时间数值计算模块、判断最短时间的路径模块、最优路径存储模块和最优路径执行模块；

样本坐标采集模块，通过摄像头采集当前机械臂所需采集检测的目标节点位置；该动作的实现，可以通过接近传感器采集是否有待检测的存放样本坐标的试管或者其它器皿落入检测口，再通过摄像头采集待检测口内是否有试管或者其它器皿进一步验证，待检测口的坐标是初始化预设的，只要接近传感器判断出为多少号检测口，即可调用数据库中该检测口的坐标，即可获得所需采集检测的目标节点位置坐标参数；若为多个机械臂采集一一对应的多个所需采集检测的目标节点位置时，只需每个机械臂对应离当前机械臂的状态节点的三维坐标最近的待采集检测的目标节点即可。

机械臂初始状态采集模块，通过摄像头采集机械臂的状态节点三维坐标参数；该过程的实现，同样采用水平设置的接近传感器或位置传感器检测机械臂最前端的样本采集点，然后通过电感式传感器检测摄像头是否通电，进一步确保机械臂处于初始未通电收紧状态；最后通过摄像头采集的图像验证摄像头处于初始未动作状态，节点三维坐标参数统一为初始原点坐标参数。

限制屏障采集模块，用于采集当前机械臂的运行屏障节点的三维坐标参数，作为路径遍历的限制遍历节点三维坐标；若有两个机械臂，则两个机械臂的中轴平面即为两个机械臂的限制遍历节点三维平面；通过设置机械臂遍历过程中的限制遍历节点三维坐标，不仅避免了机械臂运行过程中相互碰撞的可能性，还降低了路径遍历的运算复杂度和数据量。

路径遍历模块，采用快速扩展树算法遍历当前机械臂前端所有可接触的节点坐标以及路径，并将所有可接触的节点坐标及路径传输至路径动作次数计算模块；

路径动作次数计算模块，对每条单一路径的机械臂前端所有可接触的节点坐标分解，得到机械臂完成整个路径时所有关节的动作(转动、伸缩和摆动)次数；

路径时间数值计算模块，通过分解得到的每个关节的动作次数乘以完成该动作机械臂所需的平均时间，再对所有动作的时间求和，得到单一路径的运转总时间数据值；

判断最短时间的路径模块，通过冒泡程序判断所有路径中用时最短的路径，作为机械臂运行最优路径；

最优路径存储模块，用于存储最优路径的机械臂前端所有可接触的节点坐标，每个关节的动作次数和动作的先后顺序；便于最优路径执行模块调用相应数据至机械臂执行；后续机械臂运行中为了防止程序错误产生，可以通过可接触的节点坐标验证每个关节的动作是否完整实现初始遍历出来的最优路径。通过图像识别算法识别机械臂在执行每个动作时，动作的起止时间节点的视频帧图像坐标是否与最优路径的机械臂前端所有可接触的节点坐标一至，若一致则说明该机械臂执行动作过程没有错误产生，若不一致，则发出机械臂执行动作有无命令。

最优路径执行模块，用于调用最优路径存储模块中机械臂每个关节的动作次数和动作的先后顺序，机械臂依次执行完成。

进一步地，所述最优路径存储模块还存储有机械臂名称、采集检测的目标节点位置、机械臂的状态节点三维坐标参数、机械臂前端所有可接触的节点坐标、关节的动作次数和单一路径的运转总时间数据值。通过机械臂名称和采集检测的目标节点位置可以快速定位机械臂所处位置；机械臂的状态节点三维坐标参数、机械臂前端所有可接触的节点坐标、关节的动作次数和单一路径的运转总时间数据值可用于相互验证机械臂最优路径规划是否合理。

进一步地，还包括机械臂运行系统，所述机械臂运行系统包括第一传送机构、第一接近传感器、第二传送机构、第二接近传感器、多个机械臂、生物样本检测口、数值转换器和终端控制器；

第一传送机构设置在机械臂正下方，第一传送机构上方设有生物样本检测口第一传送机构正下方设有第一接近传感器；

第二传送机构纵向设置在机械臂侧面，第二传送机构上设置第二接近传感器；

数值转换器接收最优路径执行模块数据，将其转换为终端控制器可识别的电信号，终端控制器通过控制线路连接多个机械臂的关节点，并通过电信号控制机械臂的关节点的运动。

进一步地，机械臂至少包括一个运动单元，运动单元逐一连接的一个转动臂、一个伸缩臂和一个摆动臂；转动臂控制整个机械臂可转动角度大小，伸缩臂连接在转动臂上；摆动臂连接在伸缩臂前端，可围绕连接点多角度摆动。可通过预设的转动臂可转动角度以及每次转动角度数值进一步缩减机械臂的遍历所需总的路径数量，同样的，伸缩臂伸缩长度和摆动臂可摆动角度也可以通过预设达到限制最优路径规划过程中遍历产生的数据冗余量。也起到一定的防止两个机械臂动作时相互碰撞和屏障的作用。

一种机器人最优路径规划调度方法，包括步骤：

S1、通过摄像头采集当前待采集检测的目标节点和当前机械臂的状态节点的三维坐标参数；

S2、通过快速扩展树算法遍历当前机械臂的状态节点到目标节点的所有路径；

S3、根据所有路径中单一路径各个动作的转动、伸缩和摆动次数乘以预设的各个动作的运作时间，计算所有路径中各个单一路径的时间数值；

S4、将时间数值逐一输入冒泡程序中，获取最短时间数据值；

S5、判断最短时间数据值的个数，若为1，则进入步骤S6，若大于1，则进入步骤S7；

S6、输出该最短时间数据值对应的机械臂所需运行最优路径；

S7、判断多个最短时间数据值中机械臂运转最短路径输出为机械臂所需运行最优路径；

S8、机械臂接收当前机械臂的状态节点到目标节点的最优路径，并控制相应关节依次执行最优路径的运行参数。

进一步地，所述步骤2的详细步骤包括：

S201、首先初始化变量，如起始点q_init、目标点q_goal、路径Path、树T和障碍点坐标(即为两两机械臂之间均分平面和该平分面映射的空间其它平面，如机械臂对应的竖直方向上的四个平面)；

S202、随机采样生成节点q_new并进行碰撞检测，若发生碰撞则进入下一次迭代，重新采样，否则进行下一步；

S203、判断生成的q_new节点与q_goal是否满足提前给定的约束，若满足，则转S207，否则转S204；

S204、根据生成的q_new情况，调用自适应步长策略，调整步长因子；

S205、根据扩展点选择策略，扩展新节点，然后进行碰撞检测，如果无碰撞，则生成新节点q_new然后转S203判断，否则转到S206；

S206、此时判断生成树是否陷入了局部极小值，如果陷入局部极小值，则调用局部逃脱算法生成q_new然后转S203判断，若没陷入极小值转S204；

S207、此时一个满足系统约束的随机扩展树已经生成，调用Dijkstra算法优化路径，若新路径代价较小，则更新路径；

S208、返回生成的RRT树，结束。

RRT算法在笛卡尔空间中规划出一条机械臂末端的有效路径，但是对于机械臂末端在走这条有效路径时，其他关节连杆的位姿同样需要考虑。

进一步地，通过将机械臂各关节的转动或摆动角度、伸缩量，以及可转动或摆动的角度数值定义一个权重系数，以此为依据来计算路径代价：

其中，ω_i指每个关节的权重，Δθ_i指关节变化量；通过将步骤S208的所有路径解带入上式，得到最优目标位姿的关节摆动角度。i＝1～6，指的是伸缩臂和转动臂之间摆动角度有6个关节点，即机械臂由6个运动单元组成。

本发明相比现有技术，具有如下有益效果：

通过对多个机械臂和多个目标节点设置统一路径规划调度系统，实现单一机械臂动态避障路径规划无法实现多个机械臂工作对应的多个目标节点的路径规划。同时通过时间最短算法，加速机械臂的工作效率，以达到选取最优工作效率的动作路径，再选取最短路径。达到多个机械臂工作效率的提升，适应大数据量的生物样本检测过程。

附图说明

图1为本发明实施例一种机器人最优路径规划调度系统的结构框图；

图2为本发明实施例一种机器人最优路径规划调度方法的流程图；

图3为本发明实施例步骤S2的详细流程图；

图4为本发明实施例机械臂运行系统的结构框图。

图中标记说明：。

具体实施方式

为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。

如图1所示，本实施例提供了一种机器人最优路径规划调度系统，包括样本坐标采集模块、机械臂初始状态采集模块、限制屏障采集模块、路径遍历模块、路径动作次数计算模块、路径时间数值计算模块、判断最短时间的路径模块、最优路径存储模块和最优路径执行模块；

样本坐标采集模块，通过摄像头采集当前机械臂所需采集检测的目标节点位置；该动作的实现，可以通过接近传感器采集是否有待检测的存放样本坐标的试管或者其它器皿落入检测口，再通过摄像头采集待检测口内是否有试管或者其它器皿进一步验证，待检测口的坐标是初始化预设的，只要接近传感器判断出为多少号检测口，即可调用数据库中该检测口的坐标，即可获得所需采集检测的目标节点位置坐标参数；若为多个机械臂采集一一对应的多个所需采集检测的目标节点位置时，只需每个机械臂对应离当前机械臂的状态节点的三维坐标最近的待采集检测的目标节点即可。

机械臂初始状态采集模块，通过摄像头采集机械臂的状态节点三维坐标参数；该过程的实现，同样采用水平设置的接近传感器或位置传感器检测机械臂最前端的样本采集点，然后通过电感式传感器检测摄像头是否通电，进一步确保机械臂处于初始未通电收紧状态；最后通过摄像头采集的图像验证摄像头处于初始未动作状态，节点三维坐标参数统一为初始原点坐标参数。

限制屏障采集模块，用于采集当前机械臂的运行屏障节点的三维坐标参数，作为路径遍历的限制遍历节点三维坐标；若有两个机械臂，则两个机械臂的中轴平面即为两个机械臂的限制遍历节点三维平面；通过设置机械臂遍历过程中的限制遍历节点三维坐标，不仅避免了机械臂运行过程中相互碰撞的可能性，还降低了路径遍历的运算复杂度和数据量。

路径遍历模块，采用快速扩展树算法遍历当前机械臂前端所有可接触的节点坐标以及路径，并将所有可接触的节点坐标及路径传输至路径动作次数计算模块；

路径动作次数计算模块，对每条单一路径的机械臂前端所有可接触的节点坐标分解，得到机械臂完成整个路径时所有关节的动作(转动、伸缩和摆动)次数；

路径时间数值计算模块，通过分解得到的每个关节的动作次数乘以完成该动作机械臂所需的平均时间，再对所有动作的时间求和，得到单一路径的运转总时间数据值；

判断最短时间的路径模块，通过冒泡程序判断所有路径中用时最短的路径，作为机械臂运行最优路径；

最优路径存储模块，用于存储最优路径的机械臂前端所有可接触的节点坐标，每个关节的动作次数和动作的先后顺序；便于最优路径执行模块调用相应数据至机械臂执行；后续机械臂运行中为了防止程序错误产生，可以通过可接触的节点坐标验证每个关节的动作是否完整实现初始遍历出来的最优路径。通过图像识别算法识别机械臂在执行每个动作时，动作的起止时间节点的视频帧图像坐标是否与最优路径的机械臂前端所有可接触的节点坐标一至，若一致则说明该机械臂执行动作过程没有错误产生，若不一致，则发出机械臂执行动作有无命令。

最优路径执行模块，用于调用最优路径存储模块中机械臂每个关节的动作次数和动作的先后顺序，机械臂依次执行完成。

所述最优路径存储模块还存储有机械臂名称、采集检测的目标节点位置、机械臂的状态节点三维坐标参数、机械臂前端所有可接触的节点坐标、关节的动作次数和单一路径的运转总时间数据值。通过机械臂名称和采集检测的目标节点位置可以快速定位机械臂所处位置；机械臂的状态节点三维坐标参数、机械臂前端所有可接触的节点坐标、关节的动作次数和单一路径的运转总时间数据值可用于相互验证机械臂最优路径规划是否合理。

如图4所示，还包括机械臂运行系统，所述机械臂运行系统包括第一传送机构、第一接近传感器、第二传送机构、第二接近传感器、多个机械臂、生物样本检测口、数值转换器和终端控制器；

第一传送机构设置在机械臂正下方，第一传送机构上方设有生物样本检测口第一传送机构正下方设有第一接近传感器；

第二传送机构纵向设置在机械臂侧面，第二传送机构上设置第二接近传感器；

数值转换器接收最优路径执行模块数据，将其转换为终端控制器可识别的电信号，终端控制器通过控制线路连接多个机械臂的关节点，并通过电信号控制机械臂的关节点的运动。

机械臂至少包括一个运动单元，运动单元逐一连接的一个转动臂、一个伸缩臂和一个摆动臂；转动臂控制整个机械臂可转动角度大小，伸缩臂连接在转动臂上；摆动臂连接在伸缩臂前端，可围绕连接点多角度摆动。可通过预设的转动臂可转动角度以及每次转动角度数值进一步缩减机械臂的遍历所需总的路径数量，同样的，伸缩臂伸缩长度和摆动臂可摆动角度也可以通过预设达到限制最优路径规划过程中遍历产生的数据冗余量。也起到一定的防止两个机械臂动作时相互碰撞和屏障的作用。

如图2所示，一种机器人最优路径规划调度方法，包括步骤：

S1、通过摄像头采集当前待采集检测的目标节点和当前机械臂的状态节点的三维坐标参数；

S2、通过快速扩展树算法遍历当前机械臂的状态节点到目标节点的所有路径；

S3、根据所有路径中单一路径各个动作的转动、伸缩和摆动次数乘以预设的各个动作的运作时间，计算所有路径中各个单一路径的时间数值；

S4、将时间数值逐一输入冒泡程序中，获取最短时间数据值；

S5、判断最短时间数据值的个数，若为1，则进入步骤S6，若大于1，则进入步骤S7；

S6、输出该最短时间数据值对应的机械臂所需运行最优路径；

S7、判断多个最短时间数据值中机械臂运转最短路径输出为机械臂所需运行最优路径；

S8、机械臂接收当前机械臂的状态节点到目标节点的最优路径，并控制相应关节依次执行最优路径的运行参数。

如图3所示，所述步骤2的详细步骤包括：

S201、首先初始化变量，如起始点q_init、目标点q_goal、路径Path、树T；

S202、随机采样生成节点q_new并进行碰撞检测，若发生碰撞则进入下一次迭代，重新采样，否则进行下一步；

S203、判断生成的q_new节点与q_goal是否满足提前给定的约束，若满足，则转S207，否则转S204；

S204、根据生成的q_new情况，调用自适应步长策略，调整步长因子；

S205、根据扩展点选择策略，扩展新节点，然后进行碰撞检测，如果无碰撞，则生成新节点q_new然后转S203判断，否则转到S206；

S206、此时判断生成树是否陷入了局部极小值，如果陷入局部极小值，则调用局部逃脱算法生成q_new然后转S203判断，若没陷入极小值转S204；

S207、此时一个满足系统约束的随机扩展树已经生成，调用Dijkstra算法优化路径，若新路径代价较小，则更新路径；

S208、返回生成的RRT树，结束。

RRT算法在笛卡尔空间中规划出一条机械臂末端的有效路径，但是对于机械臂末端在走这条有效路径时，其他关节连杆的位姿同样需要考虑。通过将机械臂各关节的转动或摆动角度、伸缩量，以及可转动或摆动的角度数值定义一个权重系数，以此为依据来计算路径代价：

其中，ω_i指每个关节的权重，Δθ_i指关节变化量；通过将步骤S208的所有路径解带入上式，得到最优目标位姿的关节摆动角度。i＝1～6，指的是伸缩臂和转动臂之间摆动角度有6个关节点，即机械臂由6个运动单元组成。

本发明相比现有技术，具有如下有益效果：

通过对多个机械臂和多个目标节点设置统一路径规划调度系统，实现单一机械臂动态避障路径规划无法实现多个机械臂工作对应的多个目标节点的路径规划。同时通过时间最短算法，加速机械臂的工作效率，以达到选取最优工作效率的动作路径，再选取最短路径。达到多个机械臂工作效率的提升，适应大数据量的生物样本检测过程。

以上对本申请提供的一种机器人最优路径规划调度系统及方法进行了详细介绍。具体实施例的说明只是用于帮助理解本申请的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本申请原理的前提下，还可以对本申请进行若干改进和修饰，这些改进和修饰也落入本申请权利要求的保护范围内。

Claims (6)

1.一种机器人最优路径规划调度系统，其特征在于，包括样本坐标采集模块、机械臂初始状态采集模块、限制屏障采集模块、路径遍历模块、路径动作次数计算模块、路径时间数值计算模块、判断最短时间的路径模块、最优路径存储模块和最优路径执行模块；

样本坐标采集模块，通过摄像头采集当前机械臂所需采集检测的目标节点位置；

机械臂初始状态采集模块，通过摄像头采集机械臂的状态节点三维坐标参数；

限制屏障采集模块，用于采集当前机械臂的运行屏障节点的三维坐标参数，作为路径遍历的限制遍历节点三维坐标；

路径遍历模块，采用快速扩展树算法遍历当前机械臂前端所有可接触的节点坐标以及路径，并将所有可接触的节点坐标及路径传输至路径动作次数计算模块；

路径动作次数计算模块，对每条单一路径的机械臂前端所有可接触的节点坐标分解，得到机械臂完成整个路径时所有关节的动作次数；

路径时间数值计算模块，通过分解得到的每个关节的动作次数乘以完成该动作机械臂所需的平均时间，再对所有动作的时间求和，得到单一路径的运转总时间数据值；

判断最短时间的路径模块，通过冒泡程序判断所有路径中用时最短的路径，作为机械臂运行最优路径；

最优路径存储模块，用于存储最优路径的机械臂前端所有可接触的节点坐标，每个关节的动作次数和动作的先后顺序；

最优路径执行模块，用于调用最优路径存储模块中机械臂每个关节的动作次数和动作的先后顺序，机械臂依次执行完成。

2.根据权利要求1所述的一种机器人最优路径规划调度系统，其特征在于，所述最优路径存储模块还存储有机械臂名称、采集检测的目标节点位置、机械臂的状态节点三维坐标参数、机械臂前端所有可接触的节点坐标、关节的动作次数和单一路径的运转总时间数据值。

3.根据权利要求2所述的一种机器人最优路径规划调度系统，其特征在于，还包括机械臂运行系统，所述机械臂运行系统包括第一传送机构、第一接近传感器、第二传送机构、第二接近传感器、多个机械臂、生物样本检测口、数值转换器和终端控制器；

第一传送机构设置在机械臂正下方，第一传送机构上方设有生物样本检测口第一传送机构正下方设有第一接近传感器；

第二传送机构纵向设置在机械臂侧面，第二传送机构上设置第二接近传感器；

数值转换器接收最优路径执行模块数据，将其转换为终端控制器可识别的电信号，终端控制器通过控制线路连接多个机械臂的关节点，并通过电信号控制机械臂的关节点的运动。

4.根据权利要求3所述的一种机器人最优路径规划调度系统，其特征在于，机械臂至少包括一个运动单元，运动单元逐一连接的一个转动臂、一个伸缩臂和一个摆动臂；转动臂控制整个机械臂可转动角度大小，伸缩臂连接在转动臂上；摆动臂连接在伸缩臂前端，可围绕连接点多角度摆动。

5.一种机器人最优路径规划调度方法，其特征在于，包括步骤：

S1、通过摄像头采集当前待采集检测的目标节点和当前机械臂的状态节点的三维坐标参数；

S2、通过快速扩展树算法遍历当前机械臂的状态节点到目标节点的所有路径；

S3、根据所有路径中单一路径各个动作的转动、伸缩和摆动次数乘以预设的各个动作的运作时间，计算所有路径中各个单一路径的时间数值；

S4、将时间数值逐一输入冒泡程序中，获取最短时间数据值；

S5、判断最短时间数据值的个数，若为1，则进入步骤S6，若大于1，则进入步骤S7；

S6、输出该最短时间数据值对应的机械臂所需运行最优路径；

S7、判断多个最短时间数据值中机械臂运转最短路径输出为机械臂所需运行最优路径；

S8、机械臂接收当前机械臂的状态节点到目标节点的最优路径，并控制相应关节依次执行最优路径的运行参数。

6.根据权利要求1所述的一种机器人最优路径规划调度方法，其特征在于，所述步骤2的详细步骤包括：

S201、首先初始化变量，如起始点q_init、目标点q_goal、路径Path、树T和障碍点坐标(即为两两机械臂之间均分平面和该平分面映射的空间其它平面，如机械臂对应的竖直方向上的四个平面)；

S202、随机采样生成节点q_new并进行碰撞检测，若发生碰撞则进入下一次迭代，重新采样，否则进行下一步；

S203、判断生成的q_new节点与q_goal是否满足提前给定的约束，若满足，则转S207，否则转S204；

S204、根据生成的q_new情况，调用自适应步长策略，调整步长因子；

S205、根据扩展点选择策略，扩展新节点，然后进行碰撞检测，如果无碰撞，则生成新节点q_new然后转S203判断，否则转到S206；

S206、此时判断生成树是否陷入了局部极小值，如果陷入局部极小值，则调用局部逃脱算法生成q_new然后转S203判断，若没陷入极小值转S204；

S207、此时一个满足系统约束的随机扩展树已经生成，调用Dijkstra算法优化路径，若新路径代价较小，则更新路径；

S208、返回生成的RRT树，结束。

Images