CN115167388A - 一种基于目标引导的rrt多机器人编队路径规划算法 - Google Patents

一种基于目标引导的rrt多机器人编队路径规划算法 Download PDF

Info

Publication number
CN115167388A
CN115167388A CN202210637095.2A CN202210637095A CN115167388A CN 115167388 A CN115167388 A CN 115167388A CN 202210637095 A CN202210637095 A CN 202210637095A CN 115167388 A CN115167388 A CN 115167388A
Authority
CN
China
Prior art keywords
node
target
search tree
point
new
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.)
Pending
Application number
CN202210637095.2A
Other languages
English (en)
Inventor
尹莉莉
银滔
邓浩男
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin University of Science and Technology
Original Assignee
Harbin University of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Harbin University of Science and Technology filed Critical Harbin University of Science and Technology
Priority to CN202210637095.2A priority Critical patent/CN115167388A/zh
Publication of CN115167388A publication Critical patent/CN115167388A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
    • G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公布了一种基于目标引导的RRT多机器人编队路径规划算法。路径规划问题在机器人研究领域十分重要，多机器人路径规划相比单机器人路径规划更加复杂，在规划时需要考虑多机器人避障与相互协作等问题，本发明针对全局路径规划算法中的快速搜索随机树(RRT)算法随机性大、路径曲折且效率低等问题，提出了一种改进的RRT算法。通过改进新节点的生成方式向目标方向扩展，解决节点生成过于随机导致的效率低问题；将直接向目标方向扩展的新节点保存为下次向目标方向扩展的最近节点，降低寻找最近节点的时间复杂度；删除直接向目标方向扩展失败的最近节点，避免无效节点的重复计算；最后对最终生成的路径进行优化处理，使得规划的路径尽可能接近最优路径。

Description

一种基于目标引导的RRT多机器人编队路径规划算法
技术领域
本发明设计了一种基于目标引导的RRT多机器人编队路径规划算法，属于机器人学的一个重要研究领域。
背景技术
多机器人编队问题是指多机器人在执行某项任务的过程中保持预先规定的队形不变，避障问题可以看作多机器人从起点运动到终点的过程中规划出一条安全且高效的路径。多机器人协调控制的基础是共识问题，需要解决领导者-追随者一致性问题。相比于单机器人，多机器人能完成更复杂的任务，通过多个机器人之间相互合作，可大大提高完成任务的效率。路径规划的过程中，路径长度、规划时间等因素决定了所规划路径的合理性和有效性，同时还需考虑运动动力学约束问题。
目前，全局路径规划方法主要有Dijkstra、A*、D*、栅格法、快速扩展随机树法等，局部路径规划方法主要有人工势场法、遗传算法、神经网络算法等。多机器人编队保持与路径规划是对单机器人路径规划的扩展，在多机器人编队路径规划的过程中，由于机器人之间的协同避障、位置约束等问题导致规划任务更加困难。
发明内容：
本发明的目的是提出一种基于目标引导的RRT多机器人编队路径规划算法，为了解决单机器人执行任务效率低下的问题。
一种基于目标引导的RRT多机器人编队路径规划算法，所述的预测方法包括以下步骤：
步骤一、设置多机器人初始时刻的位置信息以及各种参数信息；
步骤二、分别从起点和终点处生成一棵搜索树；
步骤三、根据leader新节点的位置计算出跟随者节点的位置并判断是否合适；
步骤四、判断新节点的位置是否到达目的地，是则结束规划并对搜索树进行回溯找出合适的路径；
步骤五、对找到的路径进行优化处理；
进一步的，所述步骤一中，具体的，设置仿真环境大小为500×500，环境中黑色部分为障碍物区域，白色部分为可通行区域，多机器人之间的编队队形采用三角形。
进一步的，在步骤一中，设置出发点的坐标为(20,20)，目标点的坐标为(470,470)，节点扩展步长p为20，距离阈值
Figure BDA0003682528810000011
为15，follower与leader的距离为10，follower与队形方向夹角为45°。
进一步的，在步骤二中，将起点加入搜索树T1作为其第一个节点，然后向终点方向寻找合适的节点加入搜索树T1，将终点加入搜索树T2作为其第一个节点，然后向起点方向寻找合适的节点加入搜索树T2。
对于搜索树T1，尝试将目标点作为随机点进行扩展，通过式(1)(2)得到最近节点Rnearest和新节点Rnew，对于搜索树T2，尝试将出发点作为随机点进行扩展，通过式(1)(2)得到最近节点Rnearest和新节点Rnew。
Rnearest＝min{D(Rrand,T)} (1)
Figure BDA0003682528810000012
进一步的，在步骤三中包括以下步骤。
步骤三一、根据leader新节点的位置，通过式(3)(4)(5)(6)分别计算向目标点方向扩展和向出发点方向扩展的follower的位置信息，利用式(7)(8)(9)检测新节点是否合适，如果新节点合适则将新节点加入搜索树T和目标搜索树GT中，将下次扩展的最近节点设置为此次扩展产生的新节点，继续使用目标点作为随机点进行搜索树的生长。
[X'f1,Y'f1]＝[XL2-lcos(α-β)，YL2-lsin(α-β)] (3)
[X'f2,Y'f2]＝[XL2-lcos(α+β)，YL2-lsin(α+β)] (4)
[X'f1,Y'f1]＝[XL1-lcos(α-β),YL1-lsin(α-β)] (5)
[X'f2,Y'f2]＝[XL1-lcos(α+β),YL1-lsin(α+β)] (6)
Figure BDA0003682528810000021
Figure BDA0003682528810000022
Figure BDA0003682528810000023
步骤三二、不合适则放弃此次生长且在目标搜索树GT中删除不合适的最近节点，记录删除的节点数目。然后尝试通过在状态空间中随机采样的方式获取随机点进行树的扩展，使用步骤二中获取新节点的方式得到新节点，对leader新节点和follower新节点进行碰撞检测，符合条件则继续执行步骤二进行路径规划。
进一步的，所属步骤四中包括以下步骤：
判断新节点是否与另一棵搜索树中的某个节点一致或距离小于初始设定的距离阈值
Figure BDA0003682528810000024
若是则规划结束。路径规划结束后对搜索树T1和T2进行回溯，找到一条从出发点到达目标点且无碰撞的有效路径。
进一步的，所属步骤五中包括以下步骤：
路径优化是对初始规划的路径进行重构使其更加平滑，采用二分查找方式从搜索树T中寻找合适的节点实现。
本发明的主要优点是：本发明针对全局路径规划算法中的快速搜索随机树(RRT)算法随机性大、路径曲折且效率低等问题，提出了一种改进的基于目标引导的双向RRT算法。通过改进新节点的生成方式向目标方向扩展，解决节点生成过于随机导致的效率低问题；将直接向目标方向扩展的新节点保存为下次向目标方向扩展的最近节点，降低寻找最近节点的时间复杂度；删除直接向目标方向扩展失败的最近节点，避免无效节点的重复计算；最后对最终生成的路径进行优化处理，使得规划的路径尽可能接近最优路径。
附图说明
图1为本发明中根据领导者位置计算跟随者位置信息示意图。
图2为多机器人编队保持过程。
图3为采用二分查找方式从搜索树T中寻找合适的节点对路径进行优化的过程。
图4为本发明中多机器人保持编队队形从出发点无碰撞到达目标点的过程。
具体实施方式
下面将结合本发明实施例中的附图对本发明实施例中的技术方案啊进行清楚、完整地描述，显然所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例，基于本发明中的实施例中，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1-图4所示，本实施方式所述的一种基于目标引导的RRT多机器人编队路径规划算法的一实施例，包括以下步骤。
步骤一、设置多机器人初始时刻的位置信息以及各种参数信息；
设置仿真环境大小为500×500，环境中黑色部分为障碍物区域，白色部分为可通行区域，多机器人之间的编队队形采用三角形；
设置出发点的坐标为(20,20)，目标点的坐标为(470,470)，节点扩展步长ρ为20，距离阈值
Figure BDA0003682528810000031
为15，follower与leader的距离l为10，follower与队形方向夹角β为45°；
步骤二、分别从起点和终点处生成一棵搜索树；
将起点加入搜索树T1作为其第一个节点，然后向终点方向寻找合适的节点加入搜索树T1，将终点加入搜索树T2作为其第一个节点，然后向起点方向寻找合适的节点加入搜索树T2；
对于搜索树T1，尝试将目标点作为随机点进行扩展，通过式(1)(2)得到最近节点Rnearest和新节点Rnew，对于搜索树T2，尝试将出发点作为随机点进行扩展，通过式(1)(2)得到最近节点Rnearest和新节点Rnew；
Rnearest＝min{D(Rrand,T)} (1)
Figure BDA0003682528810000032
步骤三、根据leader新节点的位置计算出跟随者节点的位置并判断是否合适，follower与leader位置变化过程如图1所示，follower与leader队形保持过程如图2所示；
根据leader新节点的位置，分别计算向目标点方向扩展和向出发点方向扩展的follower的位置信息，检测新节点是否合适，如果新节点合适则将新节点加入搜索树T和目标搜索树GT中，将下次扩展的最近节点设置为此次扩展产生的新节点，继续使用目标点作为随机点进行搜索树的生长；
不合适则放弃此次生长且在目标搜索树GT中删除不合适的最近节点，记录删除的节点数目。然后尝试通过在状态空间中随机采样的方式获取随机点进行树的扩展，使用步骤二中获取新节点的方式得到新节点，对leader新节点和follower新节点进行碰撞检测，符合条件则继续执行步骤二进行路径规划。
步骤四、判断新节点的位置是否到达目的地，是则结束规划并对搜索树进行回溯找出合适的路径。
判断新节点是否与另一棵搜索树中的某个节点一致或距离小于初始设定的距离阈值
Figure BDA0003682528810000033
若是则规划结束。路径规划结束后对搜索树T1和T2进行回溯，找到一条从出发点到达目标点且无碰撞的有效路径。
步骤五、对找到的路径进行优化处理。
图3中S为机器人出发点，E为机器人目标点，x1～x39为路径规划生成的节点，黑色圆圈为障碍物，连接S与E的黑色虚线是初步的路径规划结果，绿色实线为优化后的路径。
首先将x1记为初始节点，在x1与x39节点之间寻找合适的下一节点，计算得到初始点x1到目标点x39的中间节点x20，然后判断x1与x20节点之间是否与障碍物存在冲突，如果不存在冲突则取x20与x39的中间节点x30，否则取x1与x20的中间节点x11，判断x1与x11的连线是否与障碍物存在冲突，不存在冲突则取x11与x20的中间节点x16，如图3所示得到x16为合适的下一节点，然后继续以x16为出发点重复上述过程寻找合适的下一节点，当出发点与目标点一致时路径优化完成。
经过上述步骤后，图4中显示了多机器人保持编队队形从出发点无碰撞到达目标点的过程。

Claims (6)

1.一种基于目标引导的RRT多机器人编队路径规划算法，主要包括以下步骤：
步骤一、设置多机器人初始时刻的位置信息以及各种参数信息；
步骤二、分别从起点和终点处生成一棵搜索树；
步骤三、根据leader新节点的位置计算出跟随者节点的位置并判断是否合适；
步骤四、判断新节点的位置是否到达目的地，是则结束规划并对搜索树进行回溯找出合适的路径；
步骤五、对找到的路径进行优化处理。
2.根据权利要求1所述的一种基于目标引导的RRT多机器人编队路径规划算法，所述步骤一中包含以下步骤：
步骤一一、设置仿真环境大小为500×500，环境中黑色部分为障碍物区域，白色部分为可通行区域，多机器人之间的编队队形采用三角形。
步骤一二、设置出发点的坐标为(20,20)，目标点的坐标为(470,470)，节点扩展步长ρ为20，距离阈值
Figure RE-FDA0003758484360000014
为15，follower与leader的距离l为10，follower与队形方向夹角β为45°。
3.根据权利要求1所述的一种基于目标引导的RRT多机器人编队路径规划算法，所述步骤二中包含以下步骤：
步骤二一、将起点加入搜索树T1作为其第一个节点，然后向终点方向寻找合适的节点加入搜索树T1，将终点加入搜索树T2作为其第一个节点，然后向起点方向寻找合适的节点加入搜索树T2。
步骤二二、对于搜索树T1，尝试将目标点作为随机点进行扩展，通过式(1)(2)得到最近节点Rnearest和新节点Rnew，对于搜索树T2，尝试将出发点作为随机点进行扩展，通过式(1)(2)得到最近节点Rnearest和新节点Rnew。
Rnearest＝min{D(Rrand,T)} (1)
Figure RE-FDA0003758484360000011
4.根据权利要求1所述的一种基于目标引导的RRT多机器人编队路径规划算法，所述步骤三中包含以下步骤：
步骤三一、根据leader新节点的位置，通过式(3)(4)(5)(6)分别计算向目标点方向扩展和向出发点方向扩展的follower的位置信息，利用式(7)(8)(9)检测新节点是否合适，如果新节点合适则将新节点加入搜索树T和目标搜索树GT中，将下次扩展的最近节点设置为此次扩展产生的新节点，继续使用目标点作为随机点进行搜索树的生长。
[X'f1,Y'f1]＝[XL2-lcos(α-β)，YL2-lsin(α-β)] (3)
[X'f2,Y'f2]＝[XL2-lcos(α+β)，YL2-lsin(α+β)] (4)
[X'f1,Y'f1]＝[XL1-lcos(α-β),YL1-lsin(α-β)] (5)
[X'f2,Y'f2]＝[XL1-lcos(α+β),YL1-lsin(α+β)] (6)
Figure RE-FDA0003758484360000012
Figure RE-FDA0003758484360000013
Figure RE-FDA0003758484360000021
步骤三二、不合适则放弃此次生长且在目标搜索树GT中删除不合适的最近节点，记录删除的节点数目；然后尝试通过在状态空间中随机采样的方式获取随机点进行树的扩展，使用步骤二中获取新节点的方式得到新节点，对leader新节点和follower新节点进行碰撞检测，符合条件则继续执行步骤二进行路径规划。
5.根据权利要求1所述的一种基于目标引导的RRT多机器人编队路径规划算法，所述步骤四中包含以下步骤：
判断新节点是否与另一棵搜索树中的某个节点一致或距离小于初始设定的距离阈值
Figure RE-FDA0003758484360000022
若是则规划结束；路径规划结束后对搜索树T1和T2进行回溯，找到一条从出发点到达目标点且无碰撞的有效路径。
6.根据权利要求1所述的一种基于目标引导的RRT多机器人编队路径规划算法，所述步骤五中包含以下步骤：
路径优化采用二分查找方式从搜索树T中寻找合适的节点实现，假设x1～x39为路径规划生成的节点，首先将x1记为初始节点，在x1与x39节点之间寻找合适的下一节点，计算得到初始点x1到目标点x39的中间节点x20，然后判断x1与x20节点之间是否与障碍物存在冲突，如果不存在冲突则取x20与x39的中间节点x30，否则取x1与x20的中间节点x11，判断x1与x11的连线是否与障碍物存在冲突，不存在冲突则取x11与x20的中间节点x16，得到x16为合适的下一节点，然后继续以x16为出发点重复上述过程寻找合适的下一节点，当出发点与目标点一致时路径优化完成。
CN202210637095.2A 2022-06-07 2022-06-07 一种基于目标引导的rrt多机器人编队路径规划算法 Pending CN115167388A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210637095.2A CN115167388A (zh) 2022-06-07 2022-06-07 一种基于目标引导的rrt多机器人编队路径规划算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210637095.2A CN115167388A (zh) 2022-06-07 2022-06-07 一种基于目标引导的rrt多机器人编队路径规划算法

Publications (1)

Publication Number Publication Date
CN115167388A true CN115167388A (zh) 2022-10-11

Family

ID=83484384

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210637095.2A Pending CN115167388A (zh) 2022-06-07 2022-06-07 一种基于目标引导的rrt多机器人编队路径规划算法

Country Status (1)

Country Link
CN (1) CN115167388A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115903814A (zh) * 2022-11-22 2023-04-04 哈尔滨工业大学（深圳） 基于凸多边形树的多机器人最优编队路径规划

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115903814A (zh) * 2022-11-22 2023-04-04 哈尔滨工业大学（深圳） 基于凸多边形树的多机器人最优编队路径规划
CN115903814B (zh) * 2022-11-22 2023-08-18 哈尔滨工业大学（深圳） 基于凸多边形树的多机器人最优编队路径规划方法

Similar Documents

Publication Publication Date Title
CN109115226B (zh) 基于跳点搜索的多机器人冲突避免的路径规划方法
CN110231824B (zh) 基于直线偏离度方法的智能体路径规划方法
CN113219998B (zh) 一种基于改进双向informed-RRT*的车辆路径规划方法
CN109945873B (zh) 一种用于室内移动机器人运动控制的混合路径规划方法
WO2023016101A1 (zh) 一种基于启发式偏置采样的室内环境机器人探索方法
CN113103236B (zh) 一种快速渐进最优的机械臂避障路径规划方法
CN109579854B (zh) 基于快速扩展随机树的无人车避障方法
CN110989352B (zh) 一种基于蒙特卡洛树搜索算法的群体机器人协同搜索方法
CN112229419B (zh) 一种动态路径规划导航方法及系统
CN110006429A (zh) 一种基于深度优化的无人船航迹规划方法
CN112987799B (zh) 一种基于改进rrt算法的无人机路径规划方法
CN110609547A (zh) 一种基于可视图引导的移动机器人规划方法
CN110320919B (zh) 一种未知地理环境中的巡回机器人路径优化方法
CN112327876B (zh) 一种基于终距指数的机器人路径规划方法
CN112947594B (zh) 一种面向无人机的航迹规划方法
CN115167388A (zh) 一种基于目标引导的rrt多机器人编队路径规划算法
Przybylski et al. D* extra lite: A dynamic A* with search–tree cutting and frontier–gap repairing
CN111552300A (zh) 一种基于实例分割和路径规划的农作物采摘系统
CN115167398A (zh) 一种基于改进a星算法的无人船路径规划方法
CN110705803B (zh) 基于三角形内心引导rrt算法的路径规划方法
CN115373384A (zh) 一种基于改进rrt的车辆动态路径规划方法及系统
CN113721608A (zh) 机器人局部路径规划方法、系统和可读存储介质
CN113721622A (zh) 一种机器人路径规划方法
CN117032231A (zh) 一种基于改进rrt的多智能体路径规划方法
CN116852367A (zh) 一种基于改进RRTstar的六轴机械臂避障路径规划方法

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