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

1.本发明属于路径规划技术领域，具体涉及一种基于多优化快速拓展随机树的足球机器人路径规划方法。


背景技术：

2.在robocup标准平台组足球机器人比赛中,路径规划是各个角色机器人动作的前提,前锋球员需要基于路径规划的结果与敌方机器人争夺球权。后卫球员也需要根据规划的结果来阻止对手的进攻。一般来说，路径规划是指计算机能够自主计算出一条从当前位置到目标位置的无碰撞路径。在实时未知环境下的路径规划问题是足球机器人比赛中的关键。
3.路径规划算法可以划分为三大类,第一类是基于搜索的路径规划算法,以dijkstra算法,a*算法为代表。搜索算法，往往需要对全局地图进行栅格化以保证在规划过程中避开障碍物,因此算法表现高度依赖栅格分辨率。并且在路径搜索的过程中需要拓展的结点繁多。算法性能较差，耗时较长，其优点在于路径规划具备最优性。第二类则是以蚁群算法，遗传算法为代表的智能路径规划算法。这一类算法对于初始状态较为依赖，计算量大。算法运行时间较长。难以满足足球场上实时性的要求，但不易陷入局部最优的困境。第三类算法是基于采样的路径规划算法,以快速拓展随机树算法(rapidly exploring random tree,rrt)算法,prm(probabilistic road maps,prm)算法为代表,与传统的搜索算法不同，这类算法不依赖于对地图进行建模，具备计算量小，规划速度快，概率完备等特点。并且在高维空间中也可以进行良好的应用。但缺点是规划过程具有较强的随机性,寻找到的路径不确定且不具备最优性。
4.为了提高基本rrt的算法表现，国内外众多学者对rrt算法提出了一系列的改进方法。karaman等学者提出了rrt*算法,该算法在保留基本rrt算法优势的基础上,还具备渐进最优性。在每次拓展新结点的同时，通过为新结点重选父结点以及基于新结点优化周边网络等手段。大幅度提高了随机树生长的质量。但很大程度上仍没有解决基本算法的强随机性问题。搜索效率低下。随后有学者提出将rrt*算法与人工势能场法相结合,降低扩充新结点的随机性。为了提高rrt算法的收敛性能，在2013年以kuffner为代表学者又提出一种双向搜索的路径搜索算法(bi-rrt)。但仍不能避免rrt算法在自由空间中进行大量无效采样所带来的内存损耗与时间成本。因此gammell和srinivasa随后又提出了informed-rrt*算法，在使用rrt*算法搜索到一条可行路径后，利用该路径构建启发式采样区域，进一步提高算法的收敛速度。


技术实现要素：

5.本发明所要解决的技术问题是克服现有技术的不足，提供一种基于多优化快速拓展随机树的足球机器人路径规划方法，针对简化地图下的伪可行路径构建启发式采样集合，并以双向随机树同时生长代替交替生长以提高算法的收敛速度。双向树缓存以及目标
点引力的混合采样方案降低rrt生长的随机性。最后对搜索路径进行冗余点删除，转弯半径调整。并使用3次准均匀b样条曲线生成平滑路径。
6.本发明提供一种基于多优化快速拓展随机树的足球机器人路径规划方法，包括如下步骤，
7.步骤s1.对栅格化的地图进行简化，对地图上所有障碍物占据栅格数进行分析，选取合适的栅格数作为阈值；低于该阈值的障碍物不参与引导路径的搜索；
8.步骤s2.通过启发式rrt*算法寻找一条初始可行路径，基于该条路径构建启发式采样集合，在集合中进行均匀采样，并考虑非简化地图；
9.步骤s3.采用线程技术，两颗随机树同时从起点与终点以同样的方式生长，并使用锁控制全局资源，当一棵树进入连接阶段时，另一棵树则暂停生长，直到本次连接成功或失败；
10.步骤s4.在采样过程中添加目标点位置，引导随机树朝着目标方向生长；
11.步骤s5.采用混合缓存方法，分别以一定概率选取现有可行路径上的点，对手树上的点以及目标点以替代随机采样以及启发式区域内的采样；
12.步骤s6.以路径规划起点开始，依次选取后一个点作为线段终点，检测两点之间连线是否发生碰撞，若未发生碰撞,则两点之间的其余结点均可删除，同时选取路径中的下一个点作为新起点继续该步骤；
13.步骤s7.计算基本路径中的所有转角，并根据nao机器人的电机约束设置阈值，对基本路径进行优化，直到所有的转弯角均大于阈值。
14.作为本发明的进一步技术方案，步骤s1中，若存在一条可行路径f
viable
，则必然有f
viable
(t)∈x
free
，且满足
15.当f
viable
不满足最优路径时，则满足c(f
viable
)≥c(f
start
→
x
) c(f
x
→
goal
)；
16.所有满足上述的x集合为x
sample
，已知可行路径f
viable
满足
17.c(f
viable
)≥||x
start-x
goal
||2，同时若x∈x
free
，使得当前路径得到优化，则必然满足c(f
viable
)≥||x
start-x||2 ||x-x
goal
||2；
18.定义自由空间中满足上式的点集合为x
sample'
，显然若x∈x
sample
，则x∈x
sample'
，x
sample'
作为估计采样集；
19.在二维条件下，显然该集合中的点位于一个椭圆内部，该椭圆的两个焦点分别为路径规划的初始点x
start
与目标点x
goal
，焦距长为||x
start-x
goal
||2记为c
min
,长轴长为c(f
viable
)，记为c
max
,即当前规划下最优可行路径长度，短轴长为
20.进一步的，步骤s1中，均匀采样的具体方法为，设x
circle
＝[x1,x2]
t
位于以世界原点为圆心的单位圆内，即x
circle
～u(x
circle
)；满足x
circle
＝1，显然椭圆采样区域内点x
ellipse
满足x
ellipse
＝clx
circle
x
center
；其中表示采样集中心，令x
ellipse'
＝lx
circle
＝[x
e1
,x
e2
]
t
，则
[0021]
设矩阵s满足如下条件：
[0022][0023]
则进一步l
t
sl＝i；
[0024]
将矩阵s带入，则
[0025]
二维旋转矩阵c满足
[0026]
其中为向量x
goal-x
start
与坐标横轴所呈夹角，则
[0027][0028]
因此，在以原点为圆心的单位圆内部均匀采样，可将点映射至椭圆采样区域内部。
[0029]
进一步有，步骤s2具体为，将简化地图搜索到的可行路径作为引导构建椭圆采样集合，若在指定迭代次数内未搜索到路径，则扩大采样区域进行采样，。记c(f
guide
)为引导路径长度,c
min
＝||x
start-x
goal
||2有c(fg'
uide
)＝c(f
guide
) kac
min
，其中，ka∈(0,1]为采样扩充因子，使用c(fg'
uide
)更新引导路径使采样区域增大，随着c(f
guide
)的更新，采样区域延伸至整个自由空间。
[0030]
进一步的，步骤s3中，两颗随机树在连接时，采用贪婪连接法，从新结点出发，选择另一棵树中最近的结点，以步长e不断向目标方向生长，直到连接成功或遇见障碍物宣告连接失败并退出连接阶段继续同步生长；
[0031]
两颗随机树在每次拓展新结点后则判断是否能够与另一棵树相连，两个线程各负责一棵树的生长，但只要有一棵树进入连接阶段，则另一棵树的线程在获取锁资源前都处于等待状态；
[0032]
当连接成功时，以连接区域最近的两个点作为出发点；在各自的树上依次选取各自的上级结点，直到分别达到起点，并获取一条可行路径。
[0033]
进一步的，步骤s4中，每次新增结点时在原先采样函数的基础上考虑额外的引力函数f2(n)，其中x
near
为随机树中距离采样点最近的结点，x
goal
为目标点，x
new
为本次生长中拓展的新结点，新的随机树生长函数为f(n)＝f1(n) f2(n)，由势能函数得到目标点引导公式为从而构造出目标
引导函数为其中e为随机树生长步长，k
p
为引导系数；基本rrt算法中的生长公式为新的结点计算公式为
[0034][0035]
进一步的，步骤s5中，假定三种缓存采样概率分别为p
trace
,p
tree
以及p
goal
,采样时首先生成[0,1]之间的随机数x,若x≤p
trace
,则以路径缓存区中的作为采样点，若p
trace
＜x≤p
trace
p
tree
,则选择另一棵随机树上的点作为采样点，若满足p
trace
p
tree
＜x≤p
trace
p
tree
p
trace
，则选择目标点作为新的采样点，否则进行正常的启发式采样过程。
[0036]
进一步的，还包括步骤s8.采用python，simrobot进行仿真验证。
[0037]
本发明的优点在于，首先对地图栅格化，以栅格简化地图中的可行路径构建启发式区域，提前了informed-rrt*进行启发采样的时间，在障碍物密集且差异较大的环境中能够有效提高算法的收敛速度。第二，以双树同步生长替代单棵树生长，不仅提高了地图空间利用率，也大幅度降低了算法运行时间。第三，引入人工引力提高目的导向性，并采用混合缓存进一步降低了rrt算法在采样过程中的随机性。最后提出了包括冗余点快速删除，混合路径修剪对搜索路径长以及转弯半径进行优化并利用b样条曲线进行路径平滑，得到一条易于足球机器人跟踪的路径。
附图说明
[0038]
图1为本发明的椭圆采样集合二维图；
[0039]
图2为本发明的简化地图示意图；
[0040]
图3为本发明的引导路径对比示意图；
[0041]
图4为本发明的双向树quick连接示意图；
[0042]
图5为本发明的目标引导下随机树生长过程示意图；
[0043]
图6为本发明的混合路径生长示意图；
[0044]
图7为本发明的路径修剪方式1示意图；
[0045]
图8为本发明的路径修剪方式2示意图；
[0046]
图9为本发明的模拟地图下各算法对比示意图；
[0047]
图10为本发明的路径代价随迭代次数变化曲线示意图；
[0048]
图11为本发明的simrobot仿真图。
具体实施方式
[0049]
本实施例提供一种基于多优化快速拓展随机树的足球机器人路径规划方法，包括如下步骤，
[0050]
步骤s1.对栅格化的地图进行简化，对地图上所有障碍物占据栅格数进行分析，选取合适的栅格数作为阈值；低于该阈值的障碍物不参与引导路径的搜索；
[0051]
步骤s2.通过启发式rrt*算法寻找一条初始可行路径，基于该条路径构建启发式采样集合，在集合中进行均匀采样，并考虑非简化地图；
[0052]
步骤s3.采用线程技术，两颗随机树同时从起点与终点以同样的方式生长，并使用锁控制全局资源，当一棵树进入连接阶段时，另一棵树则暂停生长，直到本次连接成功或失败；
[0053]
步骤s4.在采样过程中添加目标点位置，引导随机树朝着目标方向生长；
[0054]
步骤s5.采用混合缓存方法，分别以一定概率选取现有可行路径上的点，对手树上的点以及目标点以替代随机采样以及启发式区域内的采样；
[0055]
步骤s6.以路径规划起点开始，依次选取后一个点作为线段终点，检测两点之间连线是否发生碰撞，若未发生碰撞,则两点之间的其余结点均可删除；
[0056]
步骤s7.计算基本路径中的所有转角，并根据nao机器人的电机约束设置阈值，对基本路径进行优化，直到所有的转弯角均大于阈值
[0057]
步骤s8.采用python，simrobot进行仿真验证。
[0058]
步骤s1中，若存在一条可行路径f
viable
，则必然有f
viable
(t)∈x
free
，且满足
[0059]
当f
viable
不满足最优路径时，则满足c(f
viable
)≥c(f
start
→
x
) c(f
x
→
goal
)；
[0060]
所有满足上述的x集合为x
sample
，已知可行路径f
viable
满足
[0061]
c(f
viable
)≥||x
start-x
goal
||2，同时若x∈x
free
，使得当前路径得到优化，则必然满足c(f
viable
)≥||x
start-x||2 ||x-x
goal
||2；
[0062]
定义自由空间中满足上式的点集合为x
sample'
，显然若x∈x
sample
，则x∈x
sample'
，x
sample'
作为估计采样集；
[0063]
在二维条件下，如图1所示，显然该集合中的点位于一个椭圆内部，该椭圆的两个焦点分别为路径规划的初始点x
start
与目标点x
goal
，焦距长为||x
start-x
goal
||2记为c
min
,长轴长为c(f
viable
)，记为c
max
,即当前规划下最优可行路径长度，短轴长为由于搜索到的路径渐优，因此椭圆区域随路径规划不断缩小，算法收敛速度逐渐加快。
[0064]
步骤s1中，均匀采样的具体方法为，设x
circle
＝[x1,x2]
t
位于以世界原点为圆心的单位圆内，即x
circle
～u(x
circle
)；满足x
circle
＝1，显然椭圆采样区域内点x
ellipse
满足x
ellipse
＝clx
circle
x
center
；其中表示采样集中心，令x
ellipse'
＝lx
circle
＝[x
e1
,x
e2
]
t
，则
[0065]
设矩阵s满足如下条件：
[0066][0067]
则进一步l
t
sl＝i；
[0068]
将矩阵s带入，则
[0069]
二维旋转矩阵c满足
[0070]
其中为向量x
goal-x
start
与坐标横轴所呈夹角，则
[0071][0072]
因此，在以原点为圆心的单位圆内部均匀采样，可将点映射至椭圆采样区域内部。
[0073]
步骤s2具体为，如图2所示，将简化地图搜索到的可行路径作为引导构建椭圆采样集合，若在指定迭代次数内未搜索到路径，则扩大采样区域进行采样，。记c(f
guide
)为引导路径长度,c
min
＝||x
start-x
goal
||2有c(fg'
uide
)＝c(f
guide
) kac
min
，其中，ka∈(0,1]为采样扩充因子，使用c(fg'
uide
)更新引导路径使采样区域增大，随着c(f
guide
)的更新，采样区域延伸至整个自由空间，初始引导路径的建立如图3所示。
[0074]
步骤s3中，两颗随机树在连接时，采用贪婪连接法，从新结点出发，选择另一棵树中最近的结点，以步长e不断向目标方向生长，直到连接成功或遇见障碍物宣告连接失败并退出连接阶段继续同步生长，双树贪婪连接过程如图4所示；
[0075]
两颗随机树在每次拓展新结点后则判断是否能够与另一棵树相连，两个线程各负责一棵树的生长，但只要有一棵树进入连接阶段，则另一棵树的线程在获取锁资源前都处于等待状态；
[0076]
当连接成功时，以连接区域最近的两个点作为出发点；在各自的树上依次选取各自的上级结点，直到分别达到起点，并获取一条可行路径。随后利用该可行路径进行后续算法处理。
[0077]
步骤s4中，如图5所示，每次新增结点时在原先采样函数的基础上考虑额外的引力函数f2(n)，其中x
near
为随机树中距离采样点最近的结点，x
goal
为目标点，x
new
为本次生长中拓展的新结点，新的随机树生长函数为f(n)＝f1(n) f2(n)，由势能函数得到目标点引导公式为从而构造出目标
引导函数为其中e为随机树生长步长，k
p
为引导系数；基本rrt算法中的生长公式为新的结点计算公式为
[0078]
步骤s5中，假定三种缓存采样概率分别为p
trace
,p
tree
以及p
goal
,采样时首先生成[0,1]之间的随机数x,若x≤p
trace
,则以路径缓存区中的作为采样点，若p
trace
＜x≤p
trace
p
tree
,则选择另一棵随机树上的点作为采样点，若满足p
trace
p
tree
＜x≤p
trace
p
tree
p
trace
，则选择目标点作为新的采样点，否则进行正常的启发式采样过程，本实施例中三种概率均为0.1，采样混合缓存采样的生长如图6所示。
[0079]
rrt系列算法的特点在于采样并线性拓展结点。因此算法搜索到的路径不平滑，可能存在许多冗余结点。同时在障碍物的约束下，搜索路径还可能存在转角过小的问题。这会加剧足球机器人移动时的电机磨损，在某些情况下，由于机器人的运动约束，rrt算法搜索到的路径机器人甚至无法跟随。
[0080]
冗余结点删除步骤为首先以路径规划起点开始，依次选取后一个点作为线段终点，检测两点之间连线是否发生碰撞，若未发生碰撞则两点之间的其余结点均可删除。如图11a所示，线段(x1,x3)未触碰障碍物，因此原始路径中的结点x2被删除。若线段发生碰撞，则选取终止结点的上级结点作为起始结点继续上述过程。如图11a所示，线段(x1,x4)与障碍物发生碰撞，因此选择图11a中的x3代替x1作为起始结点,并继续算法步骤。因此图11a中x4被去除。
[0081]
在删除冗余结点后，能够一定程度上缓和路径不平滑，以及可能出现的转弯角度过小等问题。但实验表明，单纯的去除冗余结点不作其它处理，无法完全解决rrt算法的路径粗糙问题。因此本文提出了两种修剪方法相结合的转角补偿策略。以进一步提高路径规划的可跟踪性。
[0082]
首先计算基本路径中的所有转角，并根据nao机器人的电机约束设置适当的阈值，对基本路径进行优化，直到所有的转弯角均大于阈值，则优化结束。优化算法中首先尝试使用结点替换的方式进行路径修剪，如图7所示。
[0083]
在图7中，线段(x3,x4)与(x4,x5)所呈角度θ低于设定阈值需要修剪，因此在由边(x3,x4)，(x4,x5)，(x3,x5)所形成的三角形区域内随机采样，以采样到的新结点替代x4,并且新结点需满足θn高于阈值。且发生结点替代后的新路径无碰撞。如图7b所示。
[0084]
考虑到若图7三角形区域内若存在较大的障碍物，则可能该修剪方案可能会因优化困难而无法停止迭代，因此当迭代次数超过阈值时，采用修剪方法2进行替代，如图8所示。
[0085]
当障碍物的约束导致无法在三角区域内进行结点替换时，则考虑在由线段(x4,x6)形成的三角形区域内(图8b虚线区域)随机采样一个点作为新增结点，并满足θ
n1
,θ
n2
均大于设定的角度阈值以及无碰撞条件。
[0086]
为了验证改进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所示。
[0087]
由图9可以看出，rrt*算法的采样过程随机，空间中存在大量无效结点，目标导向型差，迭代效率不够优，而informed-rrt*算法引入了基于可行路径的启发采样，因此采样过程在算法搜索中期有良好的收敛，避免了大量的无效采样，但仍受限于单棵随机树限制，算法迭代过程产生的可行路径仍然较少，因此有时也无法产生较好的路径，并且路径往往不够平滑，存在冗余结点以及转弯角度过小的问题。本文算法在相同迭代次数下产生的可行解更多，能够更好逼近最优解。
[0088]
基于两张地图对三种算法各进行100组实验，算法迭代次数设置为1000次，并分别使用3个参数对算法进行对比:平均路径长度(l
avg
),启发式采样时间(ti)，平均搜索时间(t
5％
)。100组实验均值数据如表1所示。
[0089]
算法l
avg
/cmti/st
5％
/srrt*69.4505—15.3703informed-rrt*67.58100.783222.7445smbi-rrt*67.53690.05518.7872
[0090]
表1模拟地图下各算法仿真数据
[0091]
从表1中可以看出，由于三种算法具备渐进最优性，因此当迭代次数较高的情形下，l
avg
接近，但由于informed-rrt*算法采用启发均匀采样，算法计算规模扩大，因此t
5％
较rrt*提升了约50-60％，本文算法保留启发采样优势的同时，使得t
5％
较rrt*降低了50％。由于本文算法的启发采样初始基于简化后的地图，因此ti提前，所以在密集环境且障碍物体积差异较大的条件下，该算法较informed-rrt*算法收敛的更为迅速。
[0092]
由图10可知，受到单棵随机树生长的限制，rrt*以及informed-rrt*算法在迭代次数较低时，由于拓展结点不足在较复杂环境下会导致搜索不到可行路径。本文算法采用双随机树同步拓展结点，大幅度提高了空间效率，因此可以在较低的迭代次数下搜索到路径。同时可以从图中看出本文算法进一步提高informed-rrt*算法的优势，可以快速收敛到一个次优解。
[0093]
本文采用bhuman团队开发的仿真比赛环境simrobot来对本文算法进行测试，验证算法对于实时的足球比赛是否能够很好的满足要求。如图11所示。
[0094]
从上图中可以看出rrt*在相同迭代次数下路径较长，而informed-rrt*虽然搜索到的更优，但也存在路径曲折，转弯半径小的问题。而从图11中可以看到smbi-rrt*算法搜索的路径较为平滑，易于足球机器人的跟随。
[0095]
以上显示和描述了本发明的基本原理、主要特征和优点。本领域的技术人员应该了解，本发明不受上述具体实施例的限制，上述具体实施例和说明书中的描述只是为了进一步说明本发明的原理，在不脱离本发明精神范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护的范围由权利要求书及其等效物界定。