基于高斯采样和目标偏向引导的快速扩展随机树算法、电子设备及存储介质的制作方法

1.本发明涉及机器人运动轨迹规划技术领域，具体涉及一种基于高斯采样和目标偏向引导的快速扩展随机树算法、电子设备及存储介质。


背景技术：

2.运动规划是在约束条件下连接机器人初始构型和目标构型的运动状态序列，同时需避免障碍物空间的影响，重点是查找一组连续的轨迹。运动规划是自主机器人导航的核心内容之一，建立在获取准确的位置信息的基础上，并与控制输入接口相关，可以用来生成并优化运动轨迹，便于驱动机器人运动。解决运动规划问题的算法可分为基于数值优化计算、基于图搜索和基于采样等的方法。运动规划算法广泛应用于机器人学、结构生物学、自动驾驶和计算机动画等领域。
3.快速扩展随机树算法(rapidly exploring random tree，rrt)是一种基于采样的方法，是成功应用于机器人运动规划领域的经典算法之一，它能使机器人计算快速找到初始路径并且随着样本数增加路径不断优化。rrt*是对rrt的完善，保证算法最终收敛于最优性结果。rrt*算法具有概率完备性和渐进最优性，但通常需要比较长的时间代价来达到这一效果。
4.rrt*算法概要过程为：从初始位置开始生长树，在自由空间随机采样，按照指定步长在随机采样点方向矢量引导下添加节点和连线到图集合，迭代地扩展树枝结构，并在扩展过程中重选父节点和重连枝，当随机树中的子节点包含了目标点或进入了目标区域，再经过剪枝和平滑处理，由此得到从树节点组成的从初始点到目标点的一条轨迹。
5.rrt算法由steven m.lavalle提出，该方法能有效搜索高维非凸环境，通过对工作空间采样，将搜索引向空白区域；rrt通过计算到障碍物区域的距离进行碰撞检查，避免对含障碍物的空间建模，算法优越性还体现在适用于非完整性约束和多自由度机器人系统中。缺点在于收敛结果通常并非最优且轨迹质量不高，路径探索易受困于狭窄空间。具体见文献[1]lavalle,s.“rapidly
‑
exploring random trees:a new tool for path planning.”theannual research report,1998.记载。
[0006]
rrt*是在原有rrt算法基础上增加了重选父节点和重布线的操作，使得求解过程最终趋于渐进最优的解，但这一操作同时带来了巨大的时间代价，使得收敛快速性受影响。具体见文献[2]karaman s,frazzoli e.sampling
‑
based algorithms for optimal motionplanning[j].the international journal of robotics research,2011,30(7):846
‑
894.记载。
[0007]
后人的研究改进多数在此基础上进行。
[0008]
ahmed hussain qureshi提出的p
‑
rrt是基于势函数的rrt*，它将人工势场引入到 rrt*算法，该算法融合了势函数，减少了相当一部分迭代次数，从而压缩了内存和时间代价，解决了rrt*内存消耗大和收敛速度慢的问题，在几乎所有的场景下p
‑
rrt表现的都比
rrt*更有效率。具体见文献[3]qureshi ah,ayaz y.potential functions based samplingheuristic for optimal path planning[j].autonomous robots,2016,40(6):1079
‑
1093.记载。
[0009]
rrt*
‑
smart引入了智能采样和路径优化两种特性，在时间和路径代价上都提升了效率；但这种效率的提升依赖一种叫偏向比的启发式算法，该算法在收敛速度和空间探索之间进行权衡，这种启发式并不是自动的，在不同环境中需要程序员独立设置参数值。具体见文献[4]islam f,nasir j,malik u,et al.rrt
‑
smart:rapid convergence implementation ofrrt towards optimal solution[c]//mechatronics and automation(icma),2012internationalconference on.ieee,2012.记载。
[0010]
dt
‑
rrt采用了考虑了工作空间树和状态树两种不同的拓扑结构，由于两种树的结构不同，可以实现灵活的节点扩展。从而提高了节点扩展的成功率，节省了计算成本。首先，工作空间树探索目标环境而不考虑任何物理约束。然后，利用运动学和动力学约束，从工作空间树节点生成轨迹树。该方案还提供了重接线方案，以减少计算成本的方式保持子节点，保证渐近最优性。采用双树方案的dt
‑
rrt方法显示了较高的成功率，因为它通过应用动力学条件减少了节点排斥的机会。此外，该方法与先进的近邻搜索方案兼容，如空间降维和k
‑
d树。在两轮移动机器人高速导航的仿真结果中表明，该方法具有较好的计算时间。然而，由于此方案重连树方案只适用于已知的环境，因此对于动态和未知的场景，该方法需要进一步改进。具体见文献[5]moon,cb,chung.kinodynamic planner dual
‑
treerrt(dt
‑
rrt)for two
‑
wheeled mobile robots using the rapidly exploring random tree[j]. ieee t ind electron,2015,2015,62(2)(
‑
):1080
‑
1090.记载。
[0011]
原始的rrt*存在探索效率低下、最优性不足、不适用于狭窄空间及复杂环境等问题，采样点的选择为扩展过程引入了随机性，另外这种操作导致了低效的搜索和连枝，改进后的rrt*算法在一定程度上优化了搜索策略和生成的轨迹，但也存在计算量大、计算复杂度高、内存资源耗费严重，轨迹不够平滑，不适用于大规模搜索场景等问题。具体而言体现在如下几个方面:
[0012]
搜索覆盖率低、内存消耗巨大：基于rrt*的算法需要大量的迭代和样本来避免局部最小值，从而增加内存需求。rrt*最初采用的是无法有效捕捉环境连通性的均匀采样策略。此外，树的扩展是在离最终解决方案很远的构型空间领域。因此，树中大量的节点不足以为最终的最优路径做出贡献，添加大量无效的节点和分支反而增加了树的密度。这种现象增加了计算时间，降低了收敛速度。算法收敛缓慢也与规划器使用的搜索空间探索标准和采样策略有关。
[0013]
狭窄通道问题：传统的rrt*均匀随机采样选择狭窄通道中节点的概率明显偏低。解决狭窄通道问题的方法要么使用依赖于初始解决方案的启发式，要么使用理论假设。这些方法的优化和可靠性问题是目前仍需详细研究的课题。此外，在运动动力学约束的背景使机器人通过狭窄通道更加困难。
[0014]
近邻搜索效率不高：每次迭代中，近邻搜索的计算复杂度也随着树的扩张而指数级增长。因此，这一问题也是整体效率和收敛性的瓶颈。部分算法通过固定树中的最大可能节点来保持效率，大多数研究人员采用的是使用更智能的搜索技术，如离散化空间、智能数据结构。然而，树中最小连接成本的替代技术或更快的搜索方法仍有欠缺。
[0015]
后处理问题：基于rrt*的方法会产生次优路径，因此需要采用后处理技术对路径进行进一步优化。通常用于路径改良的两种后处理技术是剪枝和平滑。剪枝包括局部修剪和全局修剪两种。路径修剪通过去除冗余节点来减少路径长度。即使经过修剪，生成的线性分段轨迹对于无人机和车型机器人也是不可行的。所生成的轨迹曲率不连续使得控制器不稳定，超调现象严重。这些会造成机械老化、定位误差和能量浪费。


技术实现要素：

[0016]
本发明提供一种基于高斯采样和目标偏向引导的快速扩展随机树算法，针对rrt*迭代时间长、内存消耗大的问题，解决扩展过程中盲目性问题，加速收敛过程，提升机器人路径探索效率。同时本发明还提供一种可以实现该算法的电子设备、存储介质。
[0017]
本发明采取的技术方案为：基于高斯采样和目标偏向引导的快速扩展随机树算法，包括以下步骤：
[0018]
s1：将含障碍物信息的环境地图，机器人初始点构型x
init
、目标构型x
goal
输入规划系统；初始化各个参数，包括循环变量i、最大迭代次数n、局部最优代价函数c
best
、目标区域检查半径r；
[0019]
s2：判定循环变量i是否大于最大迭代次数n，若是，则进行s3；若否，则跳出循环至s8；
[0020]
s3：在自由空间c
free
内进行高斯采样，得到随机样本点x
rand
，迭代次数递增1，迭代开始；
[0021]
s4：以初始构型x
init
为父节点开始生长树，选择距离树欧氏距离较近的样本点x
nearest
；
[0022]
s5:在随机样本点x
rand
、x
nearest
和目标构型x
goal
共同影响下的方向矢量上，按照步长公式计算得到目标偏向的扩展新节点的位置；
[0023]
s6：在取得新节点x
new
后，对其周围紧邻区域进行考察并进行结构重组，即重选父节点和重连边的操作；
[0024]
s7：对新结点x
new
进行碰撞检查，将新节点x
new
加入到节点结合v，将新边e
new
加入到边集合e，并计算此刻x
new
到x
near
和x
near
沿树连边到初始构型x
init
的路径代价c
new
和 cost(x
near
)。
[0025]
s8：通过函数goalregion判断新结点x
new
是否到达目标构型附近区域，若是，则进入步骤s9；若否，则重新进入s4步骤的循环。
[0026]
s9：连接树与目标构型，并对树的整体进行剪枝和平滑操作，最终得到机器人运动轨迹。
[0027]
s3中，高斯采样的参数包含初始构型x
init
，目标构型x
goal
以及局部最小代价函数c
best
。
[0028]
s5中，步长公式为：
[0029][0030]
上述公式中的参数k1、k2为可调节参量，当距离障碍物空间较近时令k1>k2，增加随机性使扩展过程摆脱局部最小值，当x
near
处于节点稀疏的自由空间时，令k1<k2，使节点选择
趋向于目标构型。
[0031]
s6中，结构重组过程为：
[0032]
分别计算x
new
和x
near
沿树连边到初始构型x
init
的路径代价c
new
和cost(x
near
)，若cost(x
near
) +c.line(x
near
,x
new
)<c(x
new
)，则调整参数按照s5重新选择x
new
及其父节点x
parent
；否则，新节点x
new
赋给局部最优节点x
min
；得到到局部区域内最优节点x
min
及对应的最小代价c
min
，连接x
min
和x
parent
作为新边加入树结构；最后返回x
min
和c
min
。
[0033]
s9中，利用局部路径有界曲率约束，去除相对位置中的波浪节点来修剪路径冗余节点和旁支，在此基础上利用夹具式b样条曲线预定边界参数限定路线曲率，以此来平滑路径。
[0034]
与现有技术相比，此发明具有如下优点：
[0035]
1)本发明提出使用动态高斯采样器取代原有的随机采样，减少节点生成的随机性；本发明结合使用了目标偏向引导的扩展方式，使得树的扩展更具方向性；本发明使用了延后的碰撞检查，避免重复的碰撞检查过程所造成的冗余计算量和时间代价。
[0036]
2)本发明结合了高斯采样和目标偏向引导的各自优势，在充分探索工作空间的同时减少迭代计算量，提高路径探索的趋向性，避免无效的扩展过程和碰撞检查，提高路径探索效率和规划收敛速度。
[0037]
3)本发明引入了适时调整的动态参量，对于步骤s5步长公式，当距离障碍物空间较近时令k1>k2，增加随机性使扩展过程摆脱局部最小值，当x
near
处于节点稀疏的自由空间时令 k1<k2，使节点选择趋向于目标构型；这种设计在扩展速度和摆脱障碍物面之间取得平衡并保持灵活操作。
[0038]
4)本发明采用了延迟的碰撞检查，即只在新节点和新边即将成功加入到扩展树前才进行检查，进一步减少因碰撞检查所耗费的时间代价和计算量。
附图说明
[0039]
图1为本发明算法流程图。
[0040]
图2为本发明算法节点扩展过程示意图。
[0041]
图3为高斯采样示意图。
[0042]
图4为候选父节点比较示意图。
[0043]
图5为重选父节点操作示意图。
[0044]
图6为重连边操作后效果图。
[0045]
图7为rrt*算法运行结果图。
[0046]
图8为本发明算法运行结果图。
[0047]
图9为电子设备结构示意图。
[0048]
图10为存储介质结构示意图。
[0049]
图11为机器人运动轨迹图。
具体实施方式
[0050]
本发明以伪代码的形式提供了算法过程及重要函数，如表一所示。
[0051]
表一：本发明算法伪代码
[0052][0053][0054][0055]
其中，函数gaussiansample()用高斯函数返回随机采样点。函数goalregion()是
一个布尔函数，如果新节点在目标区域内，则返回为真，目标区域是预定义的值。函数sampleunitball 返回采样点的单位球半径。首先由函数sampleunitball生成此次迭代中的样本点，此时，λ1、λ2、u1、u2是从始末构型连线得到的转置矩阵取得，其中，u1、u2是协方差矩阵相应特征值，λ1、λ2是协方差矩阵的主轴特征向量。λ1、λ2、x
ball
、x
center
作为σ函数的输入，输出得到随机采样点，x
center
是采样的中心点。对于参考点，使用上述步长公式，函数σ(x
ball
,(λ1，λ2),x
center
)返回符合高斯分布特性的采样点。本发明通过使用碰撞检测算法来拒绝障碍物空间中的样本。nearest()选择rrt已有图g中最接近x
rand
的顶点x
nearest
。steer()函数通过从 x
near
向x
rand
的方向移动一个增量距离η来帮助选择一个新的配置x
new
。这些都基于一项假设，即任何方向的扩展运动都是可能的。如果存在微分约束，则将输入应用于相应的控制系统，通过数值积分得到新的结构。最后，图g添加一个新的顶点x
new
，并从x
near
添加一条新的边连接到x
new
。
[0056]
本发明以伪代码的形式提供了高斯采样器的过程及重要函数，如表二所示。
[0057]
表二高斯采样器伪代码
[0058][0059]
其中，x
center
为初始构型和目标构型之间的中心，x
ball
用于存放高斯采样的参考点， sampleunitball函数返回包络采样点的球面半径；c
best
为候选节点中到父节点最小的新节点，c
min
是计算到初始构型处的总体代价。
[0060]
基于高斯采样和目标偏向引导的快速扩展随机树算法，定义如下：设机器人工作空间在二维空间中在三维空间中本发明讨论的运动在构型空间(configurationspace)中进行，机器人的状态由各个构型来描述，其中：c为构型空间，c
free
为自由空间，是机器人能达到的状态集合，c
obstacle
是工作空间中障碍物或约束空间，于是c＝c
free
∪ c
obstacle
。树的结构通过图结合g(v，e)表示，其中：v为节点集合，e为边集合。
[0061]
算法具体实施步骤如下：
[0062]
s1：将含障碍物信息的环境地图，机器人初始点构型x
init
、目标构型x
goal
输入规划系统；初始化各个参数，包括循环变量i，最大迭代次数n，局部最优代价函数c
best
，目标区域检查半径r；
[0063]
障碍物信息在程序中使用公共成员函数定义，主要参数包含障碍物序号、维数、基
元、大小等信息；环境地图具体指一个程序类，是维护规划实例所获取的环境表示形式，包含保持环境几何形状、机器人几何形状和状态参量。
[0064]
规划系统是按照本发明方法制定的装置和程序，机器人装置实体运行封装好的一组计算机程序。
[0065]
s2：判定循环变量i是否大于最大迭代次数n，若是，则进行s3；若否，则跳出循环至s8；
[0066]
s3：在自由空间c
free
内进行高斯采样，得到随机样本点x
rand
，迭代次数递增1，迭代开始；
[0067]
s3中，高斯采样的参数包含初始构型x
init
，目标构型x
goal
以及局部最小代价函数c
best
。
[0068]
s4：以初始构型x
init
为父节点开始生长树，选择距离树欧氏距离较近的样本点x
nearest
；
[0069]
s5:在随机样本点x
rand
、x
nearest
和目标构型x
goal
共同影响下的方向矢量上，按照步长公式计算得到目标偏向的扩展新节点的位置；
[0070]
s5中，步长公式为：
[0071][0072]
上述公式中的参数k1、k2为可调节参量，当距离障碍物空间较近时令k1>k2，增加随机性使扩展过程摆脱局部最小值，当x
near
处于节点稀疏的自由空间时，令k1<k2，使节点选择趋向于目标构型。
[0073]
s6：在取得新节点x
new
后，对其周围近邻区域进行考察并进行结构重组，即重选父节点和重连边的操作；
[0074]
s6中，结构重组过程为：
[0075]
分别计算x
new
和x
near
沿树连边到初始构型x
init
的路径代价c
new
和cost(x
near
)，若cost(x
near
) +c.line(x
near
,x
new
)<c(x
new
)，则调整参数按照s5重新选择x
new
及其父节点x
parent
；否则，新节点x
new
赋给局部最优节点x
min
；得到到局部区域内最优节点x
min
及对应的最小代价c
min
，连接x
min
和x
parent
作为新边加入树结构；最后返回x
min
和c
min
。
[0076]
s7：对新结点x
new
进行碰撞检查，将新节点x
new
加入到节点结合v，将新边e
new
加入到边集合e，计算全局路径代价c
new
和局部边代价cost(x
new
)。
[0077]
s8：通过函数goalregion判断新结点x
new
是否到达目标构型附近区域，若是，则进入步骤s9；若否，则重新进入s4步骤的循环。
[0078]
s9：连接树与目标构型，并对树的整体进行剪枝和平滑操作，最终得到机器人运动轨迹。对树的整体进行剪枝和平滑操作具体参照文献[6]elbanhawi m,simic m,jazar r n. continuous path smoothing for car
‑
like robots using b
‑
spline curves[j].journal ofintelligent&robotic systems,2015,80(1supplement):23
‑
56.中的记载。
[0079]
本发明沿用了rrt*算法重选父节点的方法。重选父节点过程是在新节点x
new
附近按照定义的r(i)半径寻找近邻节点，范围内的节点都可作为父节点的候选。按顺序计算每个节点到起点的路径代价和x
new
到每个近邻的路径代价。图4和图5说明了rrt*算法重选父节点的过程，其中节点编号表示节点顺序，从初始节点x
init
开始，线上数值表示路径代价，
通常为欧式距离。在查找父节点的过程中，以新节点x
new
为圆心，r(i)为半径做球面，在球面范围内查找x
new
的近邻，即图中序号为4、5、6、8的节点。0
‑1‑4‑
9这条线路的代价为11，在近邻路径中最短，所以重选父节点为节点4。
[0080]
本发明沿用了rrt*算法重布线的方法。在重新选择x
new
父节点后，需要按照新的父节点重新布线，前提条件是新线路的整体代价要比原线路小。从整体树的扩展来看，并不是每次重布线都使整体的路径代价减小，因此需要引导扩展过程，兼顾局部和全局的规划过程。重布线使得新节点扩展得到的随机树冗余线路更少，路径代价更小。图4和图6展示了rrt*重布线操作的过程。
[0081]
本发明提供一种含障碍物情况的平面环境下运动规划效果的比较，程序实例都需一定迭代时间来探索。图7和图8展示了以x
init
＝(10,10)初始位置到目标位置x
goal
＝(90,70)的运动规划过程，含障碍环境地图的大小是100*100m2，起点到终点之间的距离是100m。为找到起点到终点之间的路径。如图7所示，算法搜索整个工作空间，需要更多的迭代找到最优路径。本发明提出的方法在更少的时间内找到了路径代价更小的途径，寻路过程因为高斯分布采样、目标偏向的属性以及延后碰撞检查聚焦于端点，趋势性明显，节省了初始搜索路径的时间。随着迭代的增加，该规划器将搜索区域限制在第一个可行路径附近以节约整体循环时间，并最终提供了一个更具成本效益的轨迹。图11为在仿真软件v
‑
rep中基于此发明算法所作的机器人运动规划实例示意图，机器人沿着规划路径行进，期间遵从运动学和动力学约束，同时避开不同形状障碍物。
[0082]
在发明本实施例中，提供了一种电子设备，包括处理器、存储器、通信总线、网络接口。其中，处理器用于对执行计算机程序；存储器包括内部存储器和外部存储器，内部存储器用于为操作系统和程序执行提供临时性存储环境，外部存储器用于存储现场数据、缓存和记录工作日志；通信总线包括地址总线和数据总线，用于各组件之间发送控制命令和传输数据；可选地，可增补网络接口用于外部中断访问并连接通信。处理器和存储器在组成系统时采用改进型哈佛结构。该计算机程序在执行时可以实现本发明提出的基于高斯采样和目标偏向引导的机器人运动规划算法。
[0083]
在发明本实施例中，提供了一种存储介质，依据该算法编写的计算机程序可以通过可选的方式存储于介质中。其中，存储介质可以选择不易丢失的存储介质及其组合集成，包括但不限于与非型闪存(nand flash)、或非型闪存(nor flash)、静态存储器(sram)、只读存储器(rom)、可编程存储器(prom)、电子可擦除可编程只读存储器(eerom)、磁盘等。