CN114911233A - 基于多优化快速拓展随机树的足球机器人路径规划方法 - Google Patents

基于多优化快速拓展随机树的足球机器人路径规划方法 Download PDF

Info

Publication number
CN114911233A
CN114911233A CN202210472563.5A CN202210472563A CN114911233A CN 114911233 A CN114911233 A CN 114911233A CN 202210472563 A CN202210472563 A CN 202210472563A CN 114911233 A CN114911233 A CN 114911233A
Authority
CN
China
Prior art keywords
path
sampling
tree
point
points
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
CN202210472563.5A
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.)
Nanjing University of Posts and Telecommunications
Original Assignee
Nanjing University of Posts and Telecommunications
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 Nanjing University of Posts and Telecommunications filed Critical Nanjing University of Posts and Telecommunications
Priority to CN202210472563.5A priority Critical patent/CN114911233A/zh
Publication of CN114911233A publication Critical patent/CN114911233A/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, 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
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • 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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种基于多优化快速拓展随机树的足球机器人路径规划方法，该方法针对简化地图下的伪可行路径构建启发式采样集合，并以双向随机树同时生长代替交替生长以提高算法的收敛速度。并提出一种路径缓存,双向树缓存以及目标点引力的混合采样方案降低RRT生长的随机性。最后对搜索路径进行冗余点删除，转弯半径调整。并使用3次准均匀b样条曲线生成平滑路径。并采用Python，SimRobot进行仿真验证。本方法在障碍物密集且差异较大的环境中能够有效提高计算的收敛速度，提高了地图空间利用率的同时，也大幅度降低了算法运行时间。

Description

基于多优化快速拓展随机树的足球机器人路径规划方法
技术领域
本发明属于路径规划技术领域，具体涉及一种基于多优化快速拓展随机树的足球机器人路径规划方法。
背景技术
在RoboCup标准平台组足球机器人比赛中,路径规划是各个角色机器人动作的前提,前锋球员需要基于路径规划的结果与敌方机器人争夺球权。后卫球员也需要根据规划的结果来阻止对手的进攻。一般来说，路径规划是指计算机能够自主计算出一条从当前位置到目标位置的无碰撞路径。在实时未知环境下的路径规划问题是足球机器人比赛中的关键。
路径规划算法可以划分为三大类,第一类是基于搜索的路径规划算法,以Dijkstra算法,A*算法为代表。搜索算法，往往需要对全局地图进行栅格化以保证在规划过程中避开障碍物,因此算法表现高度依赖栅格分辨率。并且在路径搜索的过程中需要拓展的结点繁多。算法性能较差，耗时较长，其优点在于路径规划具备最优性。第二类则是以蚁群算法，遗传算法为代表的智能路径规划算法。这一类算法对于初始状态较为依赖，计算量大。算法运行时间较长。难以满足足球场上实时性的要求，但不易陷入局部最优的困境。第三类算法是基于采样的路径规划算法,以快速拓展随机树算法(Rapidly ExploringRandom Tree,RRT)算法,PRM(probabilistic road maps,PRM)算法为代表,与传统的搜索算法不同，这类算法不依赖于对地图进行建模，具备计算量小，规划速度快，概率完备等特点。并且在高维空间中也可以进行良好的应用。但缺点是规划过程具有较强的随机性,寻找到的路径不确定且不具备最优性。
为了提高基本RRT的算法表现，国内外众多学者对RRT算法提出了一系列的改进方法。Karaman等学者提出了RRT*算法,该算法在保留基本RRT算法优势的基础上,还具备渐进最优性。在每次拓展新结点的同时，通过为新结点重选父结点以及基于新结点优化周边网络等手段。大幅度提高了随机树生长的质量。但很大程度上仍没有解决基本算法的强随机性问题。搜索效率低下。随后有学者提出将RRT*算法与人工势能场法相结合,降低扩充新结点的随机性。为了提高RRT算法的收敛性能，在2013年以Kuffner为代表学者又提出一种双向搜索的路径搜索算法(BI-RRT)。但仍不能避免RRT算法在自由空间中进行大量无效采样所带来的内存损耗与时间成本。因此Gammell和Srinivasa随后又提出了Informed-RRT*算法，在使用RRT*算法搜索到一条可行路径后，利用该路径构建启发式采样区域，进一步提高算法的收敛速度。
发明内容
本发明所要解决的技术问题是克服现有技术的不足，提供一种基于多优化快速拓展随机树的足球机器人路径规划方法，针对简化地图下的伪可行路径构建启发式采样集合，并以双向随机树同时生长代替交替生长以提高算法的收敛速度。双向树缓存以及目标点引力的混合采样方案降低RRT生长的随机性。最后对搜索路径进行冗余点删除，转弯半径调整。并使用3次准均匀b样条曲线生成平滑路径。
本发明提供一种基于多优化快速拓展随机树的足球机器人路径规划方法，包括如下步骤，
步骤S1.对栅格化的地图进行简化，对地图上所有障碍物占据栅格数进行分析，选取合适的栅格数作为阈值；低于该阈值的障碍物不参与引导路径的搜索；
步骤S2.通过启发式RRT*算法寻找一条初始可行路径，基于该条路径构建启发式采样集合，在集合中进行均匀采样，并考虑非简化地图；
步骤S3.采用线程技术，两颗随机树同时从起点与终点以同样的方式生长，并使用锁控制全局资源，当一棵树进入连接阶段时，另一棵树则暂停生长，直到本次连接成功或失败；
步骤S4.在采样过程中添加目标点位置，引导随机树朝着目标方向生长；
步骤S5.采用混合缓存方法，分别以一定概率选取现有可行路径上的点，对手树上的点以及目标点以替代随机采样以及启发式区域内的采样；
步骤S6.以路径规划起点开始，依次选取后一个点作为线段终点，检测两点之间连线是否发生碰撞，若未发生碰撞,则两点之间的其余结点均可删除，同时选取路径中的下一个点作为新起点继续该步骤；
步骤S7.计算基本路径中的所有转角，并根据Nao机器人的电机约束设置阈值，对基本路径进行优化，直到所有的转弯角均大于阈值。
作为本发明的进一步技术方案，步骤S1中，若存在一条可行路径fviable，则必然有
Figure BDA0003623472550000031
fviable(t)∈Xfree，且满足
Figure BDA0003623472550000032
当fviable不满足最优路径时，则
Figure BDA0003623472550000033
满足C(fviable)≥C(fstart→x)+C(fx→goal)；
所有满足上述的x集合为Xsample，已知可行路径fviable满足
C(fviable)≥||xstart-xgoal||2，同时若x∈Xfree，使得当前路径得到优化，则必然满足C(fviable)≥||xstart-x||2+||x-xgoal||2；
定义自由空间中满足上式的点集合为Xsample'，显然若x∈Xsample，则x∈Xsample'，Xsample'作为估计采样集；
在二维条件下，显然该集合中的点位于一个椭圆内部，该椭圆的两个焦点分别为路径规划的初始点xstart与目标点xgoal，焦距长为||xstart-xgoal||2记为Cmin,长轴长为C(fviable)，记为Cmax,即当前规划下最优可行路径长度，短轴长为
Figure BDA0003623472550000034
进一步的，步骤S1中，均匀采样的具体方法为，设xcircle＝[x1,x2]T位于以世界原点为圆心的单位圆内，即xcircle～U(Xcircle)；满足xcircle＝1，显然椭圆采样区域内点xellipse满足xellipse＝CLxcircle+xcenter；其中
Figure BDA0003623472550000035
表示采样集中心，令xellipse'＝Lxcircle＝[xe1,xe2]T，则
Figure BDA0003623472550000041
设矩阵S满足如下条件：
Figure BDA0003623472550000042
则
Figure BDA0003623472550000043
进一步LTSL＝I；
将矩阵S带入，则
Figure BDA0003623472550000044
二维旋转矩阵C满足
Figure BDA0003623472550000045
其中
Figure BDA0003623472550000046
为向量xgoal-xstart与坐标横轴所呈夹角，则
Figure BDA0003623472550000047
因此，在以原点为圆心的单位圆内部均匀采样，可将点映射至椭圆采样区域内部。
进一步有，步骤S2具体为，将简化地图搜索到的可行路径作为引导构建椭圆采样集合，若在指定迭代次数内未搜索到路径，则扩大采样区域进行采样，。记C(fguide)为引导路径长度,CMin＝||xstart-xgoal||2有C(fg'uide)＝C(fguide)+KaCMin，其中，Ka∈(0,1]为采样扩充因子，使用C(fg'uide)更新引导路径使采样区域增大，随着C(fguide)的更新，采样区域延伸至整个自由空间。
进一步的，步骤S3中，两颗随机树在连接时，采用贪婪连接法，从新结点出发，选择另一棵树中最近的结点，以步长e不断向目标方向生长，直到连接成功或遇见障碍物宣告连接失败并退出连接阶段继续同步生长；
两颗随机树在每次拓展新结点后则判断是否能够与另一棵树相连，两个线程各负责一棵树的生长，但只要有一棵树进入连接阶段，则另一棵树的线程在获取锁资源前都处于等待状态；
当连接成功时，以连接区域最近的两个点作为出发点；在各自的树上依次选取各自的上级结点，直到分别达到起点，并获取一条可行路径。
进一步的，步骤S4中，每次新增结点时在原先采样函数的基础上考虑额外的引力函数F2(n)，其中xnear为随机树中距离采样点最近的结点，xgoal为目标点，xnew为本次生长中拓展的新结点，新的随机树生长函数为F(n)＝F1(n)+F2(n)，由势能函数
Figure BDA0003623472550000051
得到目标点引导公式为
Figure BDA0003623472550000052
从而构造出目标引导函数为
Figure BDA0003623472550000053
其中e为随机树生长步长，Kp为引导系数；基本RRT算法中的生长公式为
Figure BDA0003623472550000054
新的结点计算公式为
Figure BDA0003623472550000055
进一步的，步骤S5中，假定三种缓存采样概率分别为ptrace,ptree以及pgoal,采样时首先生成[0,1]之间的随机数x,若x≤ptrace,则以路径缓存区中的作为采样点，若ptrace＜x≤ptrace+ptree,则选择另一棵随机树上的点作为采样点，若满足ptrace+ptree＜x≤ptrace+ptree+ptrace，则选择目标点作为新的采样点，否则进行正常的启发式采样过程。
进一步的，还包括步骤S8.采用Python，SimRobot进行仿真验证。
本发明的优点在于，首先对地图栅格化，以栅格简化地图中的可行路径构建启发式区域，提前了Informed-RRT*进行启发采样的时间，在障碍物密集且差异较大的环境中能够有效提高算法的收敛速度。第二，以双树同步生长替代单棵树生长，不仅提高了地图空间利用率，也大幅度降低了算法运行时间。第三，引入人工引力提高目的导向性，并采用混合缓存进一步降低了RRT算法在采样过程中的随机性。最后提出了包括冗余点快速删除，混合路径修剪对搜索路径长以及转弯半径进行优化并利用B样条曲线进行路径平滑，得到一条易于足球机器人跟踪的路径。
附图说明
图1为本发明的椭圆采样集合二维图；
图2为本发明的简化地图示意图；
图3为本发明的引导路径对比示意图；
图4为本发明的双向树Quick连接示意图；
图5为本发明的目标引导下随机树生长过程示意图；
图6为本发明的混合路径生长示意图；
图7为本发明的路径修剪方式1示意图；
图8为本发明的路径修剪方式2示意图；
图9为本发明的模拟地图下各算法对比示意图；
图10为本发明的路径代价随迭代次数变化曲线示意图；
图11为本发明的SimRobot仿真图。
具体实施方式
本实施例提供一种基于多优化快速拓展随机树的足球机器人路径规划方法，包括如下步骤，
步骤S1.对栅格化的地图进行简化，对地图上所有障碍物占据栅格数进行分析，选取合适的栅格数作为阈值；低于该阈值的障碍物不参与引导路径的搜索；
步骤S2.通过启发式RRT*算法寻找一条初始可行路径，基于该条路径构建启发式采样集合，在集合中进行均匀采样，并考虑非简化地图；
步骤S3.采用线程技术，两颗随机树同时从起点与终点以同样的方式生长，并使用锁控制全局资源，当一棵树进入连接阶段时，另一棵树则暂停生长，直到本次连接成功或失败；
步骤S4.在采样过程中添加目标点位置，引导随机树朝着目标方向生长；
步骤S5.采用混合缓存方法，分别以一定概率选取现有可行路径上的点，对手树上的点以及目标点以替代随机采样以及启发式区域内的采样；
步骤S6.以路径规划起点开始，依次选取后一个点作为线段终点，检测两点之间连线是否发生碰撞，若未发生碰撞,则两点之间的其余结点均可删除；
步骤S7.计算基本路径中的所有转角，并根据Nao机器人的电机约束设置阈值，对基本路径进行优化，直到所有的转弯角均大于阈值
步骤S8.采用Python，SimRobot进行仿真验证。
步骤S1中，若存在一条可行路径fviable，则必然有
Figure BDA0003623472550000071
fviable(t)∈Xfree，且满足
Figure BDA0003623472550000072
当fviable不满足最优路径时，则
Figure BDA0003623472550000073
满足C(fviable)≥C(fstart→x)+C(fx→goal)；
所有满足上述的x集合为Xsample，已知可行路径fviable满足
C(fviable)≥||xstart-xgoal||2，同时若x∈Xfree，使得当前路径得到优化，则必然满足C(fviable)≥||xstart-x||2+||x-xgoal||2；
定义自由空间中满足上式的点集合为Xsample'，显然若x∈Xsample，则x∈Xsample'，Xsample'作为估计采样集；
在二维条件下，如图1所示，显然该集合中的点位于一个椭圆内部，该椭圆的两个焦点分别为路径规划的初始点xstart与目标点xgoal，焦距长为||xstart-xgoal||2记为Cmin,长轴长为C(fviable)，记为Cmax,即当前规划下最优可行路径长度，短轴长为
Figure BDA0003623472550000074
由于搜索到的路径渐优，因此椭圆区域随路径规划不断缩小，算法收敛速度逐渐加快。
步骤S1中，均匀采样的具体方法为，设xcircle＝[x1,x2]T位于以世界原点为圆心的单位圆内，即xcircle～U(Xcircle)；满足xcircle＝1，显然椭圆采样区域内点xellipse满足xellipse＝CLxcircle+xcenter；其中
Figure BDA0003623472550000081
表示采样集中心，令xellipse'＝Lxcircle＝[xe1,xe2]T，则
Figure BDA0003623472550000082
设矩阵S满足如下条件：
Figure BDA0003623472550000083
则
Figure BDA0003623472550000084
进一步LTSL＝I；
将矩阵S带入，则
Figure BDA0003623472550000085
二维旋转矩阵C满足
Figure BDA0003623472550000086
其中
Figure BDA0003623472550000087
为向量xgoal-xstart与坐标横轴所呈夹角，则
Figure BDA0003623472550000088
因此，在以原点为圆心的单位圆内部均匀采样，可将点映射至椭圆采样区域内部。
步骤S2具体为，如图2所示，将简化地图搜索到的可行路径作为引导构建椭圆采样集合，若在指定迭代次数内未搜索到路径，则扩大采样区域进行采样，。记C(fguide)为引导路径长度,CMin＝||xstart-xgoal||2有C(fg'uide)＝C(fguide)+KaCMin，其中，Ka∈(0,1]为采样扩充因子，使用C(fg'uide)更新引导路径使采样区域增大，随着C(fguide)的更新，采样区域延伸至整个自由空间，初始引导路径的建立如图3所示。
步骤S3中，两颗随机树在连接时，采用贪婪连接法，从新结点出发，选择另一棵树中最近的结点，以步长e不断向目标方向生长，直到连接成功或遇见障碍物宣告连接失败并退出连接阶段继续同步生长，双树贪婪连接过程如图4所示；
两颗随机树在每次拓展新结点后则判断是否能够与另一棵树相连，两个线程各负责一棵树的生长，但只要有一棵树进入连接阶段，则另一棵树的线程在获取锁资源前都处于等待状态；
当连接成功时，以连接区域最近的两个点作为出发点；在各自的树上依次选取各自的上级结点，直到分别达到起点，并获取一条可行路径。随后利用该可行路径进行后续算法处理。
步骤S4中，如图5所示，每次新增结点时在原先采样函数的基础上考虑额外的引力函数F2(n)，其中xnear为随机树中距离采样点最近的结点，xgoal为目标点，xnew为本次生长中拓展的新结点，新的随机树生长函数为F(n)＝F1(n)+F2(n)，由势能函数
Figure BDA0003623472550000091
得到目标点引导公式为
Figure BDA0003623472550000092
从而构造出目标引导函数为
Figure BDA0003623472550000093
其中e为随机树生长步长，Kp为引导系数；基本RRT算法中的生长公式为
Figure BDA0003623472550000094
新的结点计算公式为
Figure BDA0003623472550000095
步骤S5中，假定三种缓存采样概率分别为ptrace,ptree以及pgoal,采样时首先生成[0,1]之间的随机数x,若x≤ptrace,则以路径缓存区中的作为采样点，若ptrace＜x≤ptrace+ptree,则选择另一棵随机树上的点作为采样点，若满足ptrace+ptree＜x≤ptrace+ptree+ptrace，则选择目标点作为新的采样点，否则进行正常的启发式采样过程，本实施例中三种概率均为0.1，采样混合缓存采样的生长如图6所示。
RRT系列算法的特点在于采样并线性拓展结点。因此算法搜索到的路径不平滑，可能存在许多冗余结点。同时在障碍物的约束下，搜索路径还可能存在转角过小的问题。这会加剧足球机器人移动时的电机磨损，在某些情况下，由于机器人的运动约束，RRT算法搜索到的路径机器人甚至无法跟随。
冗余结点删除步骤为首先以路径规划起点开始，依次选取后一个点作为线段终点，检测两点之间连线是否发生碰撞，若未发生碰撞则两点之间的其余结点均可删除。如图11a所示，线段(x1,x3)未触碰障碍物，因此原始路径中的结点x2被删除。若线段发生碰撞，则选取终止结点的上级结点作为起始结点继续上述过程。如图11a所示，线段(x1,x4)与障碍物发生碰撞，因此选择图11a中的x3代替x1作为起始结点,并继续算法步骤。因此图11a中x4被去除。
在删除冗余结点后，能够一定程度上缓和路径不平滑，以及可能出现的转弯角度过小等问题。但实验表明，单纯的去除冗余结点不作其它处理，无法完全解决RRT算法的路径粗糙问题。因此本文提出了两种修剪方法相结合的转角补偿策略。以进一步提高路径规划的可跟踪性。
首先计算基本路径中的所有转角，并根据Nao机器人的电机约束设置适当的阈值，对基本路径进行优化，直到所有的转弯角均大于阈值，则优化结束。优化算法中首先尝试使用结点替换的方式进行路径修剪，如图7所示。
在图7中，线段(x3,x4)与(x4,x5)所呈角度θ低于设定阈值需要修剪，因此在由边(x3,x4)，(x4,x5)，(x3,x5)所形成的三角形区域内随机采样，以采样到的新结点替代x4,并且新结点需满足θn高于阈值。且发生结点替代后的新路径无碰撞。如图7b所示。
考虑到若图7三角形区域内若存在较大的障碍物，则可能该修剪方案可能会因优化困难而无法停止迭代，因此当迭代次数超过阈值时，采用修剪方法2进行替代，如图8所示。
当障碍物的约束导致无法在三角区域内进行结点替换时，则考虑在由线段(x4,x6)形成的三角形区域内(图8b虚线区域)随机采样一个点作为新增结点，并满足θn1,θn2均大于设定的角度阈值以及无碰撞条件。
为了验证改进Informed-RRT*算法，本实施例在Intel(R)Core(IM)I7-10875H，Nvdia RTX2060平台上进行仿真,实验主要使用的模块为matplotlib3.3.3，numpy1.19.4。测试地图尺寸为60cm*60cm，起点坐标与终点坐标分别为[55,50]和[5,5]以圆形构建障碍物，模拟密集障碍物环境，并分别对RRT*算法，Informed-RRT*算法以及本文提出的SMBI-RRT*算法进行对比，环境地图以及各算法路径规划表现如图9所示。
由图9可以看出，RRT*算法的采样过程随机，空间中存在大量无效结点，目标导向型差，迭代效率不够优，而Informed-RRT*算法引入了基于可行路径的启发采样，因此采样过程在算法搜索中期有良好的收敛，避免了大量的无效采样，但仍受限于单棵随机树限制，算法迭代过程产生的可行路径仍然较少，因此有时也无法产生较好的路径，并且路径往往不够平滑，存在冗余结点以及转弯角度过小的问题。本文算法在相同迭代次数下产生的可行解更多，能够更好逼近最优解。
基于两张地图对三种算法各进行100组实验，算法迭代次数设置为1000次，并分别使用3个参数对算法进行对比:平均路径长度(lavg),启发式采样时间(ti)，平均搜索时间(t5％)。100组实验均值数据如表1所示。
算法 l<sub>avg</sub>/cm t<sub>i</sub>/s t<sub>5％</sub>/s
RRT* 69.4505 — 15.3703
Informed-RRT* 67.5810 0.7832 22.7445
SMBI-RRT* 67.5369 0.0551 8.7872
表1模拟地图下各算法仿真数据
从表1中可以看出，由于三种算法具备渐进最优性，因此当迭代次数较高的情形下，lavg接近，但由于Informed-RRT*算法采用启发均匀采样，算法计算规模扩大，因此t5％较RRT*提升了约50-60％，本文算法保留启发采样优势的同时，使得t5％较RRT*降低了50％。由于本文算法的启发采样初始基于简化后的地图，因此ti提前，所以在密集环境且障碍物体积差异较大的条件下，该算法较Informed-RRT*算法收敛的更为迅速。
由图10可知，受到单棵随机树生长的限制，RRT*以及Informed-RRT*算法在迭代次数较低时，由于拓展结点不足在较复杂环境下会导致搜索不到可行路径。本文算法采用双随机树同步拓展结点，大幅度提高了空间效率，因此可以在较低的迭代次数下搜索到路径。同时可以从图中看出本文算法进一步提高Informed-RRT*算法的优势，可以快速收敛到一个次优解。
本文采用BHuman团队开发的仿真比赛环境SimRobot来对本文算法进行测试，验证算法对于实时的足球比赛是否能够很好的满足要求。如图11所示。
从上图中可以看出RRT*在相同迭代次数下路径较长，而Informed-RRT*虽然搜索到的更优，但也存在路径曲折，转弯半径小的问题。而从图11中可以看到SMBI-RRT*算法搜索的路径较为平滑，易于足球机器人的跟随。
以上显示和描述了本发明的基本原理、主要特征和优点。本领域的技术人员应该了解，本发明不受上述具体实施例的限制，上述具体实施例和说明书中的描述只是为了进一步说明本发明的原理，在不脱离本发明精神范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护的范围由权利要求书及其等效物界定。

Claims (8)

1.基于多优化快速拓展随机树的足球机器人路径规划方法，其特征在于，包括如下步骤，
步骤S1.对栅格化的地图进行简化，对地图上所有障碍物占据栅格数进行分析，选取合适的栅格数作为阈值；低于该阈值的障碍物不参与引导路径的搜索；
步骤S2.通过启发式RRT*算法寻找一条初始引导路径，基于该条路径构建启发式采样集合，在集合中进行均匀采样，并考虑非简化地图；
步骤S3.采用线程技术，两颗随机树同时从起点与终点以同样的方式生长，并使用锁控制全局资源，当一棵树进入连接阶段时，另一棵树则暂停生长，直到本次连接成功或失败；
步骤S4.在采样过程中添加目标点位置，引导随机树朝着目标方向生长；
步骤S5.采用混合缓存方法，分别以一定概率选取现有可行路径上的点，对手树上的点以及目标点以替代随机采样以及启发式区域内的采样；
步骤S6.以路径规划起点开始，依次选取后一个点作为线段终点，检测两点之间连线是否发生碰撞，若未发生碰撞,则两点之间的其余结点均可删除，同时选取路径中的下一个点作为新起点继续该步骤；
步骤S7.计算基本路径中的所有转角，并根据Nao机器人的电机约束设置阈值，对基本路径进行优化，直到所有的转弯角均大于阈值。
2.根据权利要求1所述的基于多优化快速拓展随机树的足球机器人路径规划方法，其特征在于，所述步骤S1中，若存在一条可行路径fviable，则必然有
Figure FDA0003623472540000011
fviable(t)∈Xfree，且满足
Figure FDA0003623472540000012
当fviable不满足最优路径时，则
Figure FDA0003623472540000013
满足C(fviable)≥C(fstart→x)+C(fx→goal)；
所有满足上述的x集合为Xsample，已知可行路径fviable满足C(fviable)≥||xstart-xgoal||2，同时若x∈Xfree，使得当前路径得到优化，则必然满足C(fviable)≥||xstart-x||2+||x-xgoal||2；
定义自由空间中满足上式的点集合为Xsample'，显然若x∈Xsample，则x∈Xsample'，Xsample'作为估计采样集；
在二维条件下，显然该集合中的点位于一个椭圆内部，该椭圆的两个焦点分别为路径规划的初始点xstart与目标点xgoal，焦距长为||xstart-xgoal||2记为Cmin,长轴长为C(fviable)，记为Cmax,即当前规划下最优可行路径长度，短轴长为
Figure FDA0003623472540000021
3.根据权利要求1所述的基于多优化快速拓展随机树的足球机器人路径规划方法，其特征在于，所述步骤S1中，均匀采样的具体方法为，设xcircle＝[x1,x2]T位于以世界原点为圆心的单位圆内，即xcircle～U(Xcircle)；满足xcircle＝1，显然椭圆采样区域内点xellipse满足xellipse＝CLxcircle+xcenter；其中
Figure FDA0003623472540000022
表示采样集中心，令xellipse'＝Lxcircle＝[xe1,xe2]T，则
Figure FDA0003623472540000023
设矩阵S满足如下条件：
Figure FDA0003623472540000024
则
Figure FDA0003623472540000025
进一步LTSL＝I；
将矩阵S带入，则
Figure FDA0003623472540000026
二维旋转矩阵C满足
Figure FDA0003623472540000027
其中
Figure FDA0003623472540000028
为向量xgoal-xstart与坐标横轴所呈夹角，则
Figure FDA0003623472540000029
因此，在以原点为圆心的单位圆内部均匀采样，可将点映射至椭圆采样区域内部。
4.根据权利要求1所述的基于多优化快速拓展随机树的足球机器人路径规划方法，其特征在于，所述步骤S2具体为，将简化地图搜索到的可行路径作为引导构建椭圆采样集合，若在指定迭代次数内未搜索到路径，则扩大采样区域进行采样，。记C(fguide)为引导路径长度,CMin＝||xstart-xgoal||2有C(f′guide)＝C(fguide)+KaCMin，其中，Ka∈(0,1]为采样扩充因子，使用C(f′guide)更新引导路径使采样区域增大，随着C(fguide)的更新，采样区域延伸至整个自由空间。
5.根据权利要求1所述的基于多优化快速拓展随机树的足球机器人路径规划方法，其特征在于，所述步骤S3中，两颗随机树在连接时，采用贪婪连接法，从新结点出发，选择另一棵树中最近的结点，以步长e不断向目标方向生长，直到连接成功或遇见障碍物宣告连接失败并退出连接阶段继续同步生长；
两颗随机树在每次拓展新结点后则判断是否能够与另一棵树相连，两个线程各负责一棵树的生长，但只要有一棵树进入连接阶段，则另一棵树的线程在获取锁资源前都处于等待状态；
当连接成功时，以连接区域最近的两个点作为出发点；在各自的树上依次选取各自的上级结点，直到分别达到起点，并获取一条可行路径。
6.根据权利要求1所述的基于多优化快速拓展随机树的足球机器人路径规划方法，其特征在于，所述步骤S4中，每次新增结点时在原先采样函数的基础上考虑额外的引力函数F2(n)，其中xnear为随机树中距离采样点最近的结点，xgoal为目标点，xnew为本次生长中拓展的新结点，新的随机树生长函数为F(n)＝F1(n)+F2(n)，由势能函数
Figure FDA0003623472540000031
得到目标点引导公式为
Figure FDA0003623472540000032
从而构造出目标引导函数为
Figure FDA0003623472540000033
其中e为随机树生长步长，Kp为引导系数；基本RRT算法中的生长公式为
Figure FDA0003623472540000034
新的结点计算公式为
Figure FDA0003623472540000035
7.根据权利要求1所述的基于多优化快速拓展随机树的足球机器人路径规划方法，其特征在于，所述步骤S5中，假定三种缓存采样概率分别为ptrace,ptree以及pgoal,采样时首先生成[0,1]之间的随机数x,若x≤ptrace,则以路径缓存区中的作为采样点，若ptrace＜x≤ptrace+ptree,则选择另一棵随机树上的点作为采样点，若满足ptrace+ptree＜x≤ptrace+ptree+ptrace，则选择目标点作为新的采样点，否则进行正常的启发式采样过程。
8.根据权利要求1所述的基于多优化快速拓展随机树的足球机器人路径规划方法，其特征在于，还包括步骤S8.采用Python，SimRobot进行仿真验证。
CN202210472563.5A 2022-04-29 2022-04-29 基于多优化快速拓展随机树的足球机器人路径规划方法 Pending CN114911233A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210472563.5A CN114911233A (zh) 2022-04-29 2022-04-29 基于多优化快速拓展随机树的足球机器人路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210472563.5A CN114911233A (zh) 2022-04-29 2022-04-29 基于多优化快速拓展随机树的足球机器人路径规划方法

Publications (1)

Publication Number Publication Date
CN114911233A true CN114911233A (zh) 2022-08-16

Family

ID=82764381

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210472563.5A Pending CN114911233A (zh) 2022-04-29 2022-04-29 基于多优化快速拓展随机树的足球机器人路径规划方法

Country Status (1)

Country Link
CN (1) CN114911233A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116784975A (zh) * 2023-04-26 2023-09-22 安徽医科大学 基于改进Bi-RRT算法的柔性穿刺针路径规划方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116784975A (zh) * 2023-04-26 2023-09-22 安徽医科大学 基于改进Bi-RRT算法的柔性穿刺针路径规划方法

Similar Documents

Publication Publication Date Title
CN113219998B (zh) 一种基于改进双向informed-RRT*的车辆路径规划方法
CN109116841B (zh) 一种基于蚁群算法的路径规划平滑优化方法
CN112393728B (zh) 一种基于a*算法和rrt*算法的移动机器人路径规划方法
CN111678523B (zh) 一种基于star算法优化的快速bi_rrt避障轨迹规划方法
CN109343345B (zh) 基于qpso算法的机械臂多项式插值轨迹规划方法
CN107883961A (zh) 一种基于Smooth‑RRT算法的水下机器人路径优化方法
CN114161416B (zh) 基于势函数的机器人路径规划方法
CN112650256A (zh) 一种基于改进双向rrt机器人路径规划方法
CN110347151A (zh) 一种融合Bezier优化遗传算法的机器人路径规划方法
CN107169557A (zh) 一种对布谷鸟优化算法进行改进的方法
CN113858210B (zh) 基于混合算法的机械臂路径规划方法
CN112947480B (zh) 一种移动机器人路径规划方法、存储介质及系统
CN113296520A (zh) 融合a*与改进灰狼算法的巡检机器人路径规划方法
CN112327876A (zh) 一种基于终距指数的机器人路径规划方法
CN114911233A (zh) 基于多优化快速拓展随机树的足球机器人路径规划方法
CN113917925A (zh) 一种基于改进遗传算法的移动机器人路径规划方法
CN116734877A (zh) 基于改进a*算法与动态窗口法的机器人动态避障方法
CN113204238B (zh) 一种移动机器人的路径规划方法、设备及存储介质
Zhang et al. Improve RRT algorithm for path planning in complex environments
CN114995413A (zh) 基于改进遗传算法的智能小车路径规划方法
CN115167388A (zh) 一种基于目标引导的rrt多机器人编队路径规划算法
CN113741484A (zh) 一种基于概率模型的路径规划方法、系统及介质
Zheng et al. Obstacle avoidance and path planning for ugvs using improved rrt algorithm
Zhang et al. Robot path planning based on shuffled frog leaping algorithm combined with genetic algorithm
Xiangde et al. Global dynamic path planning algorithm based on harmony search algorithm and artificial potential field method

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