CN111650941B - 基于RRT-Connect改进的医用移动机器人路径规划方法 - Google Patents
基于RRT-Connect改进的医用移动机器人路径规划方法 Download PDFInfo
- Publication number
- CN111650941B CN111650941B CN202010525927.2A CN202010525927A CN111650941B CN 111650941 B CN111650941 B CN 111650941B CN 202010525927 A CN202010525927 A CN 202010525927A CN 111650941 B CN111650941 B CN 111650941B
- Authority
- CN
- China
- Prior art keywords
- random tree
- node
- random
- starting point
- tree
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000005070 sampling Methods 0.000 claims abstract description 61
- 230000006872 improvement Effects 0.000 claims abstract description 14
- 230000006870 function Effects 0.000 claims description 41
- 238000004590 computer program Methods 0.000 claims description 14
- 238000003860 storage Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000002194 synthesizing effect Effects 0.000 claims 1
- 238000004422 calculation algorithm Methods 0.000 abstract description 23
- 238000012952 Resampling Methods 0.000 abstract description 4
- 230000003044 adaptive effect Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 10
- 230000008569 process Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000002457 bidirectional effect Effects 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000002922 simulated annealing Methods 0.000 description 1
- 238000004659 sterilization and disinfection Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/0088—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Business, Economics & Management (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Game Theory and Decision Science (AREA)
- Medical Informatics (AREA)
- Feedback Control In General (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种基于RRT‑Connect改进的医用移动机器人路径规划方法、***、介质和设备，该方法根据距离代价函数判断起始点与目标点的距离，在起始点与目标点之间，通过中节点采样函数同时采样出距离相等的第二起始点和第二目标点，使得改进后的算法能同时从起始点、目标点、第二起始点、第二目标点生长出六棵随机树，每棵随机树同时向着各自的目标方向进行快速拓展。同时，在该算法中加入自适应避障重采样原则，当中节点采样函数的采样点与障碍物发生碰撞或在障碍物内部时，则放弃该节点的采样，然后根据中节点的采样位置，在该节点与相邻节点的中心位置进行重采样。解决了医用移动机器人在复杂的环境中，其路径规划时间成本开销大，很难在短时间内寻找到目标点的问题。
Description
技术领域
本发明属于机器人路径规划领域，特别涉及一种基于RRT-Connect改进的医用移动机器人路径规划方法、装置、介质及设备。
背景技术
随着现代科技的不断进步，人们的生活水平的不断提高，医用移动机器人在医疗领域得到了深入而广泛的研究与应用。医用移动机器人是指用于医院、诊所的医疗或辅助医疗的移动机器人。医用移动机器人工作的环境比较复杂，人口比较密集，其路径规划问题一直备受关注。如何保证医用移动机器人在医疗领域的复杂环境下路径规划的时间代价函数、距离代价函数最优。同时，在已知医疗环境地图或者未知医疗环境地图中，需要解决路径规划过程中存在的障碍物因素，医用移动机器人才能完成从起始位置到目标位置的路径规划，实现安全、准确到达目标点。
现如今，路径规划算法在很多领域都得到了应用，针对不同的环境地图，人们提出了不同的路径规划算法。比较常见的路径规划算法有Dijkstra算法、Astar算法、模拟退火算法、人工势场算法等。对于复杂的医疗环境地图中，传统的路径规划算法虽然能成功地规划出一条从起始位置到目标位置的路径，但其付出的代价是巨大的，也难以达到理想的效果。同时，在解决高维空间和复杂约束的路径规划问题时，它们都难以求解出最优解，也影响算法的实时性和高效性。
RRT-Connect是一种双树结构的快速搜索随机树的算法，2000年由LaValle教授和日本东京大学的Kuffner教授联合提出。它是基于RRT(Rapidly-Exploring Random Tree)算法的基础上进行优化，具有良好的快速搜索特点，能够有效的提高搜索速度、减少搜索时间的代价函数。同时，该算法具有双向扩展的特性，能从初始位置和目标位置同时生长两棵快速扩展随机树来搜索状态空间，直到两棵树连接在一起为止。双树结构的RRT-Connect算法虽然能极大地提高搜索速度、减少搜索时间，但是对于复杂的医疗环境，其路径规划时间成本开销很大，很难在短时间内寻找到目标点。
发明内容
本发明提供了一种基于RRT-Connect改进的医用移动机器人路径规划方法、装置、介质及设备，其目的在于，其目的在于通过中节点采样函数同时采样出距离相等的第二起始点和第二目标点，然后在起始点、目标点、第二起始点、第二目标点生长出六棵随机树向前拓展，直到所有相邻随机树连接在一起后，从所有随机树中选出起始点到目标点的最优路径，完成路径规划。
本发明提供的技术方案如下：
一种基于RRT-Connect改进的医用移动机器人路径规划方法，包括如下步骤：
步骤1：在已知环境地图中，设置移动机器人路径规划的原始起始点和原始目标点；
步骤2：依据距离代价函数，获取起始点和目标点之间的距离，并基于中节点采样函数，在已知环境地图中进行等距采样，获得第二起始点和第二目标点；
步骤3：将原始起始点和原始目标点分别作为第一随机树和第二随机树的根节点，将第二起始点作为第三随机树、第四随机树的起始点，将第二目标点作为第五随机树、第六随机树的起始点，第一随机树至第六随机树均通过随机采样函数在已知环境地图中进行节点初始拓展；
其中第一随机树与第三随机树为彼此的拓展目标，第四随机树与第五随机树为彼此的拓展目标，第六随机树与第二随机树为彼此的拓展目标；即向彼此拓展的随机树为相邻随机树；
步骤4：对第一随机树至第六随机树均按步骤5-步骤6进行重新拓展，获得规划路径；
步骤5：对于每棵随机树，从环境地图中随机选择一个采样节点Qrand，并计算同一随机树中其余节点与所选节点之间的距离，找出与所选节点的最近节点Qnear；
步骤6：从随机树的最近节点Qnear与采样节点Qrand之间，按照节点步长距离D，获取离最近节点最近的新节点Qnew，并判断所获取的新节点、新节点与最近节点之间的路径是否与障碍物发生碰撞，若发生，则从随机树中删除该节点，并返回步骤5，否则，将新节点加入到随机树的路径中，直到当前随机树的新节点与相邻随机树的所有节点之间的距离均小于节点步长距离D，完成随机树的路径规划，进入步骤7；
第一随机树与第三随机树、第四随机树与第五随机树、第六随机树与第二随机树为相邻随机树；
步骤7：从重新拓展后的六棵随机树中的路径点，选择一条从原始起始点到原始目标点之间的最优路径。
它根据距离代价函数判断起始点与目标点的距离，在起始点与目标点之间，通过中节点采样函数同时采样出距离相等的第二起始点和第二目标点，使得改进后的能同时从起始点、目标点、第二起始点、第二目标点生长出六棵随机树，每棵随机树同时向着各自的目标方向进行快速拓展。同时,在该中加入自适应避障重采样原则，当中节点采样函数的采样点与障碍物发生碰撞或在障碍物内部时，则放弃该节点的采样，然后根据中节点的采样位置，在该节点与相邻节点的中心位置进行重采样。
进一步地，所述中节点采样函数按以下公式计算：
其中，Qinit为原始起始点、Qgoal为原始目标点、Qinit2为第二起始点、Qgoal2为第二目标点。
进一步地，所述步骤6中节点之间的距离为欧式距离。
进一步地，以六棵随机树中对应的最短路径作为从原始起始点到原始目标点之间的最优路径。
另一方面，一种基于RRT-Connect改进的医用移动机器人路径规划***，包括：
初始化单元：用于在已知环境地图中，设置移动机器人路径规划的原始起始点和原始目标点；
第二起始点与目标点设置单元：用于依据距离代价函数，获取起始点和目标点之间的距离，并基于中节点采样函数模块，在已知环境地图中进行等距采样，获得第二起始点和第二目标点；
随机树初始化单元：用于将原始起始点和原始目标点分别作为第一随机树和第二随机树的根节点，将第二起始点作为第三随机树、第四随机树的起始点，将第二目标点作为第五随机树、第六随机树的起始点，第一随机树至第六随机树均通过随机采样函数在已知环境地图中进行节点初始拓展；
随机树拓展单元：对第一随机树至第六随机树均按以下模块进行重新拓展，获得规划路径；
其中第一随机树与第三随机树为彼此的拓展目标，第四随机树与第五随机树为彼此的拓展目标，第六随机树与第二随机树为彼此的拓展目标；即向彼此拓展的随机树为相邻随机树；
随机节点采样模块：对于每棵随机树，从环境地图中随机选择一个采样节点Qrand，并计算同一随机树中其余节点与所选节点之间的距离，找出与所选节点的最近节点Qnear；
新节点获取模块：从随机树的最近节点Qnear与采样节点Qrand之间，按照节点步长距离D，获取离最近节点最近的新节点Qnew，并判断所获取的新节点、新节点与最近节点之间的路径是否与障碍物发生碰撞，若发生，则从随机树中删除该节点，并重新调用随机节点采样模块，否则，将新节点加入到随机树的路径中，直到当前随机树的新节点与相邻随机树的所有节点之间的距离均小于节点步长距离D，完成随机树的路径规划，进入最优路径获取单元；
第一随机树与第三随机树、第四随机树与第五随机树、第六随机树与第二随机树为相邻随机树；
最优路径获取单元：从重新拓展后的六棵随机树中，选择一条从原始起始点到原始目标点之间的最优路径。
进一步地，所述中节点采样函数模块按以下公式计算：
其中，Qinit为原始起始点、Qgoal为原始目标点、Qinit2为第二起始点、Qgoal2为第二目标点。
一方面，一种计算机存储介质，包括计算机程序，所述程序被处理器执行时实现所述的一种基于RRT-Connect改进的医用移动机器人路径规划方法。
再一方面，一种基于RRT-Connect改进的医用移动机器人路径规划设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时，以使所述基于RRT-Connect改进的医用移动机器人路径规划设备实现所述的一种基于RRT-Connect改进的医用移动机器人路径规划方法。
有益效果
与当前的技术相比，本发明主要有以下的几处优势：
(1)本发明采用了中节点采样函数生成了六树结构的快速拓展随机树，可以使得算法的路径规划时间成本大大减少，提高了路径规划的搜索效率；
(2)与双树结构的RRT-Connect算法相比，改进后的算法迭代次数及路径规划时间均能取得很好的效果，具有优越的路径规划性能。同时，该算法能够在很短的时间内，快速、高效地规划出一条路径平滑地通过复杂环境的空间。
(3)除此之外，本发明也可以运用于港口搬运、物流运输、无人自动驾驶、航空航天、军事救援、防疫消毒等领域的路径规划。
附图说明
图1为本发明实例所述方法的流程示意框图；
图2为本发明实例中所述复杂环境的实验地图；
图3为本发明实例中所述六棵随机树的起始点和目标点的设置示意图；
图4为本发明实例中随机树拓展示意图；
图5为运用本发明实例所述方法进行路径规划的结果示意图。
具体实施方式
下面将结合附图和实施例对本发明做进一步的说明。
准备工作：如图2所示，环境地图的范围为500×900，设定移动机器人的起始点为Qinit＝(270，81)、目标点为Qgoal＝(270,819)。图中黑色的方块为障碍物，白色区域为可行区域。图中起始点位于地图中的左侧；目标点位于地图的右侧。当规划好一条可靠的最优路径时，移动机器人将从起始点出发，沿着规划好的路径，安全无碰撞到达目标点。
请参阅图1所示，本发明提供了一种基于RRT-Connect改进的医用移动机器人路径规划方法，包括如下步骤：
步骤一、在已知环境地图中设置移动机器人路径规划的起始点Qinit和目标点Qgoal，同时获取环境地图中障碍物的位置信息；
步骤二、对随机树在生长过程中的节点步长距离设置为D，同时设置算法的最大迭代次数；
具体的，随机树拓展的节点步长距离D＝5，算法的最大迭代次数设置为50000。
步骤三、根据距离代价函数判断起始点Qinit和目标点Qgoal的距离，在起始点Qinit与目标点Qgoal之间，通过中节点采样函数同时采样出距离相等的第二起始点Qinit2和第二目标点Qgoal2，使得改进后的算法能同时从起始点Qinit、目标点Qgoal、第二起始点Qinit2、第二目标点Qgoal2生长出六棵随机树。第二起始点Qinit2和第二目标点Qgoal2的计算公式如下所示：
具体的，所述的通过中节点采样函数同时采样出距离相等的第二起始点Qinit2和第二目标点Qgoal2，其表示为起始点Qinit到第二起始点Qinit2的距离，第二起始点Qinit2到第二目标点Qgoal2的距离，第二目标点Qgoal2到目标点Qgoal的距离都相等。
步骤四、将起始点Qinit作为随机树T1的根节点，目标点Qgoal作为随机树T2的根节点，随机树T1、T2、通过随机采样函数进行拓展，如图3、4所示；
步骤五、将第二起始点Qinit2作为随机树T3、T4的根节点，第二目标点Qgoal2作为随机树T5、T6的根节点、随机树T3、T4、T5、T6也通过随机采样函数进行拓展，如图3、4所示；
随机树T1的拓展目标为随机树T3，随机树T3的拓展目标为随机树T1，拓展角度为180度，当它们相接在一起，则停止随机树T1和随机树T3的拓展。随机树T4的拓展目标为随机树T5，随机树T5的拓展目标为随机树T4，拓展角度为180度，当它们相接在一起，则停止随机树T4和随机树T5的拓展。随机树T2的拓展目标为随机树T6，随机树T6的拓展目标为随机树T2，拓展角度为180度，当它们相接在一起，则停止随机树T2和随机树T6的拓展。
步骤六、在环境地图中，随机树T1通过随机采样函数选择一个随机采样点Qrand，随机树T1中的所有节点Ki(i＝1,2,…)与随机采样点Qrand通过欧式距离函数进行比较，找到距离随机采样点Qrand中的最近节点Qnear，最近节点Qnear的计算公式如下所示；
步骤七、在随机树T1的最近节点Qnear与随机采样点Qrand之间，以步长距离D获得新节点Qnew；
步骤八、判断新节点Qnew是否与环境地图的障碍物发生碰撞，如果发生碰撞，则丢弃该新节点Qnew，转至步骤六、七后，重新获取新节点Qnew，如果没有发生碰撞，则转至步骤九。
步骤九、判断新节点Qnew与最近节点Qnear连接的路径是否与障碍物发生碰撞，如果发生碰撞，则丢弃该新节点Qnew，转至步骤六、七、八后，重新获取新节点Qnew，如果没有发生碰撞，则把新节点Qnew加入到随机树的路径中。
步骤十、将随机树T2、T3、T4、T5、T6替换步骤六、七中的随机树T1，重复步骤六、七、八、九，随机树T1、T2、T3、T4、T5、T6在拓展的过程中，需要实时计算当前随机树的新节点Qnew与相邻随机树所有节点的欧式距离，当欧式距离小于步长D时，则表示该随机树与相邻随机树相连接，同理，当所有随机树T1、T2、T3、T4、T5、T6的距离小于步长D时，表示所有相邻随机树相连接，路径规划完成；
请参阅图5，步骤十一、综合随机树T1、T2、T3、T4、T5、T6规划完成的所有路径点，得到从起始点Qinit到目标点Qgoal的最优路径S1(Qinit)，S2，…，Si(Qinit2)，…，Sj(Qgoal2)，…，Sn(Qgoal)。
基于上述方法，本发明实施例还提供一种基于RRT-Connect改进的医用移动机器人路径规划***，包括：
初始化单元：用于在已知环境地图中，设置移动机器人路径规划的原始起始点和原始目标点；
第二起始点与目标点设置单元：用于依据距离代价函数，获取起始点和目标点之间的距离，并基于中节点采样函数模块，在已知环境地图中进行等距采样，获得第二起始点和第二目标点；
随机树初始化单元：用于将原始起始点和原始目标点分别作为第一随机树和第二随机树的根节点，将第二起始点作为第三随机树、第四随机树的起始点，将第二目标点作为第五随机树、第六随机树的起始点，第一随机树至第六随机树均通过随机采样函数在已知环境地图中进行节点初始拓展；
随机树拓展单元：对第一随机树至第六随机树均按以下模块进行重新拓展，获得规划路径；
随机节点采样模块：对于每棵随机树，从环境地图中随机选择一个采样节点Qrand，并计算同一随机树中其余节点与所选节点之间的距离，找出与所选节点的最近节点Qnear；
新节点获取模块：从随机树的最近节点Qnear与采样节点Qrand之间，按照节点步长距离D，获取离最近节点最近的新节点Qnew，并判断所获取的新节点、新节点与最近节点之间的路径是否与障碍物发生碰撞，若发生，则从随机树中删除该节点，并重新调用随机节点采样模块，否则，将新节点加入到随机树的路径中，直到当前随机树的新节点与相邻随机树的所有节点之间的距离均小于节点步长距离D，完成随机树的路径规划，进入最优路径获取单元；
第一随机树与第三随机树、第四随机树与第五随机树、第六随机树与第二随机树为相邻随机树；
最优路径获取单元：从重新拓展后的六棵随机树中，选择一条从原始起始点到原始目标点之间的最优路径。
所述中节点采样函数模块按以下公式计算：
其中，Qinit为原始起始点、Qgoal为原始目标点、Qinit2为第二起始点、Qgoal2为第二目标点。
应当理解，本发明各个实施例中的功能单元模块可以集中在一个处理单元中，也可以是各个单元模块单独物理存在，也可以是两个或两个以上的单元模块集成在一个单元模块中，可以采用硬件或软件的形式来实现。
本发明实施例还提供一种计算机存储介质，包括计算机程序，所述程序被处理器执行时实现所述的一种基于RRT-Connect改进的医用移动机器人路径规划方法，其有益效果参见方法部分的有益效果，在此不再赘述。
本发明实施例还提供一种基于RRT-Connect改进的医用移动机器人路径规划设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时，以使所述基于RRT-Connect改进的医用移动机器人路径规划设备实现所述的一种基于RRT-Connect改进的医用移动机器人路径规划方法。
本领域内的技术人员应明白，本申请的实施例可提供为方法、***、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详尽的说明，所属领域的普通技术人员应当理解，上述实施例仅仅是对本发明的示意性实现方式的解释，实施例中的细节并不构成对本发明范围的限制，在不背离本发明的精神和范围的情况下，任何基于本发明技术方案的等效变换、简单替换等显而易见的改变，均落在本发明保护范围之内。
Claims (6)
1.一种基于RRT-Connect改进的医用移动机器人路径规划方法，其特征在于，包括如下步骤：
步骤1：在已知环境地图中，设置移动机器人路径规划的原始起始点和原始目标点；
步骤2：依据距离代价函数，获取起始点和目标点之间的距离，并基于中节点采样函数，在已知环境地图中进行等距采样，获得第二起始点和第二目标点；
步骤3：将原始起始点和原始目标点分别作为第一随机树和第二随机树的根节点，将第二起始点作为第三随机树、第四随机树的起始点，将第二目标点作为第五随机树、第六随机树的起始点，第一随机树至第六随机树均通过随机采样函数在已知环境地图中进行节点初始拓展；
其中第一随机树与第三随机树为彼此的拓展目标，第四随机树与第五随机树为彼此的拓展目标，第六随机树与第二随机树为彼此的拓展目标；
步骤4：对第一随机树至第六随机树均按步骤5-步骤6进行重新拓展，获得规划路径；
步骤5：对于每棵随机树，从环境地图中随机选择一个采样节点Qrand，并计算同一随机树中其余节点与所选节点之间的距离，找出与所选节点的最近节点Qnear；
步骤6：从随机树的最近节点Qnear与采样节点Qrand之间，按照节点步长距离D，获取离最近节点最近的新节点Qnew，并判断所获取的新节点、新节点与最近节点之间的路径是否与障碍物发生碰撞，若发生，则从随机树中删除该节点，并返回步骤5，否则，将新节点加入到随机树的路径中，直到当前随机树的新节点与相邻随机树的所有节点之间的距离均小于节点步长距离D，完成随机树的路径规划，进入步骤7；
步骤7：从重新拓展后的六棵随机树中的路径点，选择一条从原始起始点到原始目标点之间的最优路径；
所述中节点采样函数按以下公式计算：
其中，Qinit为原始起始点、Qgoal为原始目标点、Qinit2为第二起始点、Qgoal2为第二目标点；
最近节点Qnear的计算公式如下所示；
其中，Qrand表示随机树T1通过随机采样函数选择一个随机采样点，Ki表示随机树T1中的节点，n表示随机树T1中节点的数量。
2.根据权利要求1所述的方法，其特征在于，所述步骤6中节点之间的距离为欧式距离。
3.根据权利要求1所述的方法，其特征在于，以六棵随机树中对应的最短路径作为从原始起始点到原始目标点之间的最优路径。
4.一种基于RRT-Connect改进的医用移动机器人路径规划***，其特征在于，包括：
初始化单元：用于在已知环境地图中，设置移动机器人路径规划的原始起始点和原始目标点；
第二起始点与目标点设置单元：用于依据距离代价函数，获取起始点和目标点之间的距离，并基于中节点采样函数模块，在已知环境地图中进行等距采样，获得第二起始点和第二目标点；
随机树初始化单元：用于将原始起始点和原始目标点分别作为第一随机树和第二随机树的根节点，将第二起始点作为第三随机树、第四随机树的起始点，将第二目标点作为第五随机树、第六随机树的起始点，第一随机树至第六随机树均通过随机采样函数在已知环境地图中进行节点初始拓展；
其中第一随机树与第三随机树为彼此的拓展目标，第四随机树与第五随机树为彼此的拓展目标，第六随机树与第二随机树为彼此的拓展目标；
随机树拓展单元：对第一随机树至第六随机树均按以下模块进行重新拓展，获得规划路径；
随机节点采样模块：对于每棵随机树，从环境地图中随机选择一个采样节点Qrand，并计算同一随机树中其余节点与所选节点之间的距离，找出与所选节点的最近节点Qnear；
新节点获取模块：从随机树的最近节点Qnear与采样节点Qrand之间，按照节点步长距离D，获取离最近节点最近的新节点Qnew，并判断所获取的新节点、新节点与最近节点之间的路径是否与障碍物发生碰撞，若发生，则从随机树中删除该节点，并重新调用随机节点采样模块，否则，将新节点加入到随机树的路径中，直到当前随机树的新节点与相邻随机树的所有节点之间的距离均小于节点步长距离D，完成随机树的路径规划，进入最优路径获取单元；
最优路径获取单元：从重新拓展后的六棵随机树中，综合随机树一至六规划好的路径点，得到从原始起始点到原始目标点之间的最优路径；
所述中节点采样函数模块按以下公式计算：
其中，Qinit为原始起始点、Qgoal为原始目标点、Qinit2为第二起始点、Qgoal2为第二目标点；
最近节点Qnear的计算公式如下所示；
其中，Qrand表示随机树T1通过随机采样函数选择一个随机采样点，Ki表示随机树T1中的节点，n表示随机树T1中节点的数量。
5.一种计算机存储介质，包括计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1-3任一项所述的一种基于RRT-Connect改进的医用移动机器人路径规划方法。
6.一种基于RRT-Connect改进的医用移动机器人路径规划设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时，以使所述基于RRT-Connect改进的医用移动机器人路径规划设备实现如权利要求1-3任一项所述的一种基于RRT-Connect改进的医用移动机器人路径规划方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010525927.2A CN111650941B (zh) | 2020-06-10 | 2020-06-10 | 基于RRT-Connect改进的医用移动机器人路径规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010525927.2A CN111650941B (zh) | 2020-06-10 | 2020-06-10 | 基于RRT-Connect改进的医用移动机器人路径规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111650941A CN111650941A (zh) | 2020-09-11 |
| CN111650941B true CN111650941B (zh) | 2023-05-02 |
Family
ID=72347556
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010525927.2A Active CN111650941B (zh) | 2020-06-10 | 2020-06-10 | 基于RRT-Connect改进的医用移动机器人路径规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111650941B (zh) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113804196B (zh) * | 2020-09-17 | 2024-04-12 | 北京京东乾石科技有限公司 | 无人车路径规划方法及相关设备 |
| CN112393739B (zh) * | 2020-10-29 | 2022-07-29 | 国网安徽省电力有限公司检修分公司 | 大面积环境中的改进型快速搜索随机树路径规划方法 |
| CN112711256B (zh) * | 2020-12-23 | 2022-05-20 | 杭州电子科技大学 | 一种移动机器人的目标搜索观测点自动生成方法 |
| CN113064426B (zh) * | 2021-03-17 | 2022-03-15 | 安徽工程大学 | 一种改进双向快速搜索随机树算法的智能车路径规划方法 |
| CN113204238B (zh) * | 2021-04-22 | 2023-09-12 | 武汉理工大学 | 一种移动机器人的路径规划方法、设备及存储介质 |
| CN113341984A (zh) * | 2021-06-15 | 2021-09-03 | 桂林电子科技大学 | 基于改进rrt算法的机器人路径规划方法和装置 |
| CN113671969B (zh) * | 2021-08-25 | 2024-04-05 | 齐鲁工业大学 | 二维静态环境下的路径规划方法、***、介质及存储设备 |
| CN114153210B (zh) * | 2021-12-01 | 2024-03-19 | 苏州盈科电子有限公司 | 机器人的移动控制方法及*** |
| CN114700937B (zh) * | 2022-01-13 | 2024-02-13 | 深圳市越疆科技有限公司 | 机械臂及其运动路径规划方法、控制***、介质及机器人 |
| CN115031739A (zh) * | 2022-08-12 | 2022-09-09 | 中国科学院自动化研究所 | 连续体机器人路径规划方法、装置、电子设备及存储介质 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101667030B1 (ko) * | 2009-08-10 | 2016-10-17 | 삼성전자 주식회사 | 로봇의 경로 계획 장치 및 그 방법 |
| KR101667029B1 (ko) * | 2009-08-10 | 2016-10-17 | 삼성전자 주식회사 | 로봇의 경로 계획방법 및 장치 |
| KR102165437B1 (ko) * | 2014-05-02 | 2020-10-14 | 한화디펜스 주식회사 | 이동 로봇의 경로 계획 장치 |
| CN108681787B (zh) * | 2018-04-28 | 2021-11-16 | 南京航空航天大学 | 基于改进双向快速扩展随机树算法的无人机路径优化方法 |
| CN110497403A (zh) * | 2019-08-05 | 2019-11-26 | 上海大学 | 一种改进双向rrt算法的机械臂运动规划方法 |
| CN111141304B (zh) * | 2019-12-30 | 2021-11-02 | 福州大学 | 一种基于同心圆采样引导rrt算法的路径规划方法 |
-
2020
- 2020-06-10 CN CN202010525927.2A patent/CN111650941B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN111650941A (zh) | 2020-09-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111650941B (zh) | 基于RRT-Connect改进的医用移动机器人路径规划方法 | |
| CN110228069B (zh) | 一种机械臂在线避障运动规划方法 | |
| CN113219998B (zh) | 一种基于改进双向informed-RRT*的车辆路径规划方法 | |
| CN111546347B (zh) | 一种适用于动态环境下的机械臂路径规划方法 | |
| CN108444489A (zh) | 一种改进rrt算法的路径规划方法 | |
| US8892253B2 (en) | Swarm robot and sweeping method using swarm robot | |
| CN113296496B (zh) | 基于多采样点的引力自适应步长双向rrt路径规划方法 | |
| US20080306628A1 (en) | Multi-Modal Push Planner for Humanoid Robots | |
| CN111761582B (zh) | 一种基于随机采样的移动机械臂避障规划方法 | |
| CN112987799B (zh) | 一种基于改进rrt算法的无人机路径规划方法 | |
| CN112549016A (zh) | 一种机械臂运动规划方法 | |
| CN111610786A (zh) | 基于改进rrt算法的移动机器人路径规划方法 | |
| CN113359746A (zh) | 基于改进双向RRT和Dijkstra融合算法的路径规划方法和装置 | |
| Raja et al. | Path planning for mobile robots in dynamic environments using particle swarm optimization | |
| CN112828889A (zh) | 一种六轴协作机械臂路径规划方法及*** | |
| CN110705803B (zh) | 基于三角形内心引导rrt算法的路径规划方法 | |
| Dirik et al. | Rrt-dijkstra: An improved path planning algorithm for mobile robots | |
| Wang et al. | Learning of long-horizon sparse-reward robotic manipulator tasks with base controllers | |
| Lai et al. | Path planning and obstacle avoidance approaches for robot arm | |
| Mahadev et al. | Collecting a swarm in a grid environment using shared, global inputs | |
| Zang et al. | Path planning based on Bi-RRT algorithm for redundant manipulator | |
| Chi et al. | Risk-Informed-RRT*: A sampling-based human-friendly motion planning algorithm for mobile service robots in indoor environments | |
| Nasir | Adaptive rapidly-exploring-random-tree-star (RRT*)-smart: algorithm characteristics and behavior analysis in complex environments | |
| Jiang et al. | Learning humanoid reaching tasks in dynamic environments | |
| CN116476058A (zh) | 基于改进rrt算法的远程穿刺机器人路径规划方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |