CN112902971B - 基于高斯采样和目标偏向引导的快速扩展随机树算法的机器人运动轨迹计算方法、电子设备及存储介质 - Google Patents

Abstract

基于高斯采样和目标偏向引导的快速扩展随机树算法、电子设备及存储介质，包括结合高斯采样和目标偏向引导的改进型快速扩展随机树算法及其装备设置，用于生成和优化机器人运动轨迹。本发明提出使用动态高斯采样器取代原有的随机采样，减少节点生成的随机性；本发明结合使用了目标偏向引导的扩展方式，使得树的扩展更具方向性；本发明使用了延后的碰撞检查，避免重复的碰撞检查过程所造成的冗余计算量和时间代价。

Description

基于高斯采样和目标偏向引导的快速扩展随机树算法的机器 人运动轨迹计算方法、电子设备及存储介质

技术领域

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

背景技术

运动规划是在约束条件下连接机器人初始构型和目标构型的运动状态序列，同时需避免障碍物空间的影响，重点是查找一组连续的轨迹。运动规划是自主机器人导航的核心内容之一，建立在获取准确的位置信息的基础上，并与控制输入接口相关，可以用来生成并优化运动轨迹，便于驱动机器人运动。解决运动规划问题的算法可分为基于数值优化计算、基于图搜索和基于采样等的方法。运动规划算法广泛应用于机器人学、结构生物学、自动驾驶和计算机动画等领域。

快速扩展随机树算法(Rapidly Exploring Random Tree，RRT)是一种基于采样的方法，是成功应用于机器人运动规划领域的经典算法之一，它能使机器人计算快速找到初始路径并且随着样本数增加路径不断优化。RRT*是对RRT的完善，保证算法最终收敛于最优性结果。RRT*算法具有概率完备性和渐进最优性，但通常需要比较长的时间代价来达到这一效果。

RRT*算法概要过程为：从初始位置开始生长树，在自由空间随机采样，按照指定步长在随机采样点方向矢量引导下添加节点和连线到图集合，迭代地扩展树枝结构，并在扩展过程中重选父节点和重连枝，当随机树中的子节点包含了目标点或进入了目标区域，再经过剪枝和平滑处理，由此得到从树节点组成的从初始点到目标点的一条轨迹。

RRT算法由Steven M.LaValle提出，该方法能有效搜索高维非凸环境，通过对工作空间采样，将搜索引向空白区域；RRT通过计算到障碍物区域的距离进行碰撞检查，避免对含障碍物的空间建模，算法优越性还体现在适用于非完整性约束和多自由度机器人系统中。缺点在于收敛结果通常并非最优且轨迹质量不高，路径探索易受困于狭窄空间。具体见文献[1]Lavalle,S.“Rapidly-Exploring Random Trees:A New Tool for PathPlanning.”The Annual Research Report,1998.记载。

RRT*是在原有RRT算法基础上增加了重选父节点和重布线的操作，使得求解过程最终趋于渐进最优的解，但这一操作同时带来了巨大的时间代价，使得收敛快速性受影响。具体见文献[2]Karaman S,Frazzoli E.Sampling-based Algorithms for OptimalMotion Planning[J].The International Journal of Robotics Research,2011,30(7):846-894.记载。

后人的研究改进多数在此基础上进行。

Ahmed Hussain Qureshi提出的P-RRT是基于势函数的RRT*，它将人工势场引入到RRT*算法，该算法融合了势函数，减少了相当一部分迭代次数，从而压缩了内存和时间代价，解决了RRT*内存消耗大和收敛速度慢的问题，在几乎所有的场景下P-RRT表现的都比RRT*更有效率。具体见文献[3]Qureshi AH,Ayaz Y.Potential functions basedsampling heuristic for optimal path planning[J].Autonomous Robots,2016,40(6):1079-1093.记载。

RRT*-Smart引入了智能采样和路径优化两种特性，在时间和路径代价上都提升了效率；但这种效率的提升依赖一种叫偏向比的启发式算法，该算法在收敛速度和空间探索之间进行权衡，这种启发式并不是自动的，在不同环境中需要程序员独立设置参数值。具体见文献[4]Islam F,Nasir J,Malik U,et al.RRT-Smart:Rapid convergenceimplementation of RRT towards optimal solution[C]//Mechatronics andAutomation(ICMA),2012International Conference on.IEEE,2012.记载。

DT-RRT采用了考虑了工作空间树和状态树两种不同的拓扑结构，由于两种树的结构不同，可以实现灵活的节点扩展。从而提高了节点扩展的成功率，节省了计算成本。首先，工作空间树探索目标环境而不考虑任何物理约束。然后，利用运动学和动力学约束，从工作空间树节点生成轨迹树。该方案还提供了重接线方案，以减少计算成本的方式保持子节点，保证渐近最优性。采用双树方案的DT-RRT方法显示了较高的成功率，因为它通过应用动力学条件减少了节点排斥的机会。此外，该方法与先进的近邻搜索方案兼容，如空间降维和k-d树。在两轮移动机器人高速导航的仿真结果中表明，该方法具有较好的计算时间。然而，由于此方案重连树方案只适用于已知的环境，因此对于动态和未知的场景，该方法需要进一步改进。具体见文献[5]Moon,CB,Chung.Kinodynamic Planner Dual-Tree RRT(DT-RRT)for Two-Wheeled Mobile Robots Using the Rapidly Exploring Random Tree[J].IEEE T IND ELECTRON,2015,2015,62(2)(-):1080-1090.记载。

原始的RRT*存在探索效率低下、最优性不足、不适用于狭窄空间及复杂环境等问题，采样点的选择为扩展过程引入了随机性，另外这种操作导致了低效的搜索和连枝，改进后的RRT*算法在一定程度上优化了搜索策略和生成的轨迹，但也存在计算量大、计算复杂度高、内存资源耗费严重，轨迹不够平滑，不适用于大规模搜索场景等问题。具体而言体现在如下几个方面:

搜索覆盖率低、内存消耗巨大：基于RRT*的算法需要大量的迭代和样本来避免局部最小值，从而增加内存需求。RRT*最初采用的是无法有效捕捉环境连通性的均匀采样策略。此外，树的扩展是在离最终解决方案很远的构型空间领域。因此，树中大量的节点不足以为最终的最优路径做出贡献，添加大量无效的节点和分支反而增加了树的密度。这种现象增加了计算时间，降低了收敛速度。算法收敛缓慢也与规划器使用的搜索空间探索标准和采样策略有关。

狭窄通道问题：传统的RRT*均匀随机采样选择狭窄通道中节点的概率明显偏低。解决狭窄通道问题的方法要么使用依赖于初始解决方案的启发式，要么使用理论假设。这些方法的优化和可靠性问题是目前仍需详细研究的课题。此外，在运动动力学约束的背景使机器人通过狭窄通道更加困难。

近邻搜索效率不高：每次迭代中，近邻搜索的计算复杂度也随着树的扩张而指数级增长。因此，这一问题也是整体效率和收敛性的瓶颈。部分算法通过固定树中的最大可能节点来保持效率，大多数研究人员采用的是使用更智能的搜索技术，如离散化空间、智能数据结构。然而，树中最小连接成本的替代技术或更快的搜索方法仍有欠缺。

后处理问题：基于RRT*的方法会产生次优路径，因此需要采用后处理技术对路径进行进一步优化。通常用于路径改良的两种后处理技术是剪枝和平滑。剪枝包括局部修剪和全局修剪两种。路径修剪通过去除冗余节点来减少路径长度。即使经过修剪，生成的线性分段轨迹对于无人机和车型机器人也是不可行的。所生成的轨迹曲率不连续使得控制器不稳定，超调现象严重。这些会造成机械老化、定位误差和能量浪费。

发明内容

本发明提供一种基于高斯采样和目标偏向引导的快速扩展随机树算法，针对RRT*迭代时间长、内存消耗大的问题，解决扩展过程中盲目性问题，加速收敛过程，提升机器人路径探索效率。同时本发明还提供一种可以实现该算法的电子设备、存储介质。

本发明采取的技术方案为：基于高斯采样和目标偏向引导的快速扩展随机树算法，包括以下步骤：

S1：将含障碍物信息的环境地图，机器人初始点构型x_init、目标构型x_goal输入规划系统；初始化各个参数，包括循环变量i、最大迭代次数N、局部最优代价函数c_best、目标区域检查半径R；

S2：判定循环变量i是否大于最大迭代次数N，若是，则进行S3；若否，则跳出循环至S8；

S3：在自由空间C_free内进行高斯采样，得到随机样本点x_rand，迭代次数递增1，迭代开始；

S4：以初始构型x_init为父节点开始生长树，选择距离树欧氏距离较近的样本点x_nearest；

S5:在随机样本点x_rand、x_nearest和目标构型x_goal共同影响下的方向矢量上，按照步长公式计算得到目标偏向的扩展新节点的位置；

S6：在取得新节点x_new后，对其周围紧邻区域进行考察并进行结构重组，即重选父节点和重连边的操作；

S7：对新结点x_new进行碰撞检查，将新节点x_new加入到节点结合V，将新边e_new加入到边集合E，并计算此刻x_new到x_near和x_near沿树连边到初始构型x_init的路径代价c_new和 Cost(x_near)。

S8：通过函数GoalRegion判断新结点x_new是否到达目标构型附近区域，若是，则进入步骤S9；若否，则重新进入S4步骤的循环。

S9：连接树与目标构型，并对树的整体进行剪枝和平滑操作，最终得到机器人运动轨迹。

S3中，高斯采样的参数包含初始构型x_init，目标构型x_goal以及局部最小代价函数c_best。

S5中，步长公式为：

上述公式中的参数k₁、k₂为可调节参量，当距离障碍物空间较近时令k₁>k₂，增加随机性使扩展过程摆脱局部最小值，当x_near处于节点稀疏的自由空间时，令k₁<k₂，使节点选择趋向于目标构型。

S6中，结构重组过程为：

分别计算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。

S9中，利用局部路径有界曲率约束，去除相对位置中的波浪节点来修剪路径冗余节点和旁支，在此基础上利用夹具式B样条曲线预定边界参数限定路线曲率，以此来平滑路径。

与现有技术相比，此发明具有如下优点：

1)本发明提出使用动态高斯采样器取代原有的随机采样，减少节点生成的随机性；本发明结合使用了目标偏向引导的扩展方式，使得树的扩展更具方向性；本发明使用了延后的碰撞检查，避免重复的碰撞检查过程所造成的冗余计算量和时间代价。

2)本发明结合了高斯采样和目标偏向引导的各自优势，在充分探索工作空间的同时减少迭代计算量，提高路径探索的趋向性，避免无效的扩展过程和碰撞检查，提高路径探索效率和规划收敛速度。

3)本发明引入了适时调整的动态参量，对于步骤S5步长公式，当距离障碍物空间较近时令k₁>k₂，增加随机性使扩展过程摆脱局部最小值，当x_near处于节点稀疏的自由空间时令 k₁<k₂，使节点选择趋向于目标构型；这种设计在扩展速度和摆脱障碍物面之间取得平衡并保持灵活操作。

4)本发明采用了延迟的碰撞检查，即只在新节点和新边即将成功加入到扩展树前才进行检查，进一步减少因碰撞检查所耗费的时间代价和计算量。

附图说明

图1为本发明算法流程图。

图2为本发明算法节点扩展过程示意图。

图3为高斯采样示意图。

图4为候选父节点比较示意图。

图5为重选父节点操作示意图。

图6为重连边操作后效果图。

图7为RRT*算法运行结果图。

图8为本发明算法运行结果图。

图9为电子设备结构示意图。

图10为存储介质结构示意图。

图11为机器人运动轨迹图。

具体实施方式

本发明以伪代码的形式提供了算法过程及重要函数，如表一所示。

表一：本发明算法伪代码

其中，函数GaussianSample()用高斯函数返回随机采样点。函数GoalRegion()是一个布尔函数，如果新节点在目标区域内，则返回为真，目标区域是预定义的值。函数SampleUnitBall 返回采样点的单位球半径。首先由函数SampleUnitBall生成此次迭代中的样本点，此时，λ₁、λ₂、u₁、u₂是从始末构型连线得到的转置矩阵取得，其中，u₁、u₂是协方差矩阵相应特征值，λ₁、λ₂是协方差矩阵的主轴特征向量。λ₁、λ₂、x_ball、x_center作为Σ函数的输入，输出得到随机采样点，x_center是采样的中心点。对于参考点，使用上述步长公式，函数Σ(x_ball,(λ₁，λ₂),x_center)返回符合高斯分布特性的采样点。本发明通过使用碰撞检测算法来拒绝障碍物空间中的样本。Nearest()选择RRT已有图G中最接近x_rand的顶点x_nearest。Steer()函数通过从 x_near向x_rand的方向移动一个增量距离η来帮助选择一个新的配置x_new。这些都基于一项假设，即任何方向的扩展运动都是可能的。如果存在微分约束，则将输入应用于相应的控制系统，通过数值积分得到新的结构。最后，图G添加一个新的顶点x_new，并从x_near添加一条新的边连接到x_new。

本发明以伪代码的形式提供了高斯采样器的过程及重要函数，如表二所示。

表二高斯采样器伪代码

其中，x_center为初始构型和目标构型之间的中心，x_ball用于存放高斯采样的参考点， SampleUnitBall函数返回包络采样点的球面半径；c_best为候选节点中到父节点最小的新节点，c_min是计算到初始构型处的总体代价。

基于高斯采样和目标偏向引导的快速扩展随机树算法，定义如下：设机器人工作空间

在二维空间中

在三维空间中

本发明讨论的运动在构型空间(Configuration Space)中进行，机器人的状态由各个构型来描述，其中：C为构型空间，C_free为自由空间，是机器人能达到的状态集合，C_obstacle是工作空间中障碍物或约束空间，于是C＝C_free∪ C_obstacle。树的结构通过图结合G(V，E)表示，其中：V为节点集合，E为边集合。

算法具体实施步骤如下：

S1：将含障碍物信息的环境地图，机器人初始点构型x_init、目标构型x_goal输入规划系统；初始化各个参数，包括循环变量i，最大迭代次数N，局部最优代价函数c_best，目标区域检查半径R；

障碍物信息在程序中使用公共成员函数定义，主要参数包含障碍物序号、维数、基元、大小等信息；环境地图具体指一个程序类，是维护规划实例所获取的环境表示形式，包含保持环境几何形状、机器人几何形状和状态参量。

规划系统是按照本发明方法制定的装置和程序，机器人装置实体运行封装好的一组计算机程序。

S2：判定循环变量i是否大于最大迭代次数N，若是，则进行S3；若否，则跳出循环至S8；

S3：在自由空间C_free内进行高斯采样，得到随机样本点x_rand，迭代次数递增1，迭代开始；

S3中，高斯采样的参数包含初始构型x_init，目标构型x_goal以及局部最小代价函数c_best。

S4：以初始构型x_init为父节点开始生长树，选择距离树欧氏距离较近的样本点x_nearest；

S5:在随机样本点x_rand、x_nearest和目标构型x_goal共同影响下的方向矢量上，按照步长公式计算得到目标偏向的扩展新节点的位置；

S5中，步长公式为：

上述公式中的参数k₁、k₂为可调节参量，当距离障碍物空间较近时令k₁>k₂，增加随机性使扩展过程摆脱局部最小值，当x_near处于节点稀疏的自由空间时，令k₁<k₂，使节点选择趋向于目标构型。

S6：在取得新节点x_new后，对其周围近邻区域进行考察并进行结构重组，即重选父节点和重连边的操作；

S6中，结构重组过程为：

分别计算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。

S7：对新结点x_new进行碰撞检查，将新节点x_new加入到节点结合V，将新边e_new加入到边集合E，计算全局路径代价c_new和局部边代价Cost(x_new)。

S8：通过函数GoalRegion判断新结点x_new是否到达目标构型附近区域，若是，则进入步骤S9；若否，则重新进入S4步骤的循环。

S9：连接树与目标构型，并对树的整体进行剪枝和平滑操作，最终得到机器人运动轨迹。对树的整体进行剪枝和平滑操作具体参照文献[6]Elbanhawi M,Simic M,Jazar RN. Continuous Path Smoothing for Car-Like Robots Using B-Spline Curves[J].Journal of Intelligent&Robotic Systems,2015,80(1Supplement):23-56.中的记载。

本发明沿用了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。

本发明沿用了RRT*算法重布线的方法。在重新选择x_new父节点后，需要按照新的父节点重新布线，前提条件是新线路的整体代价要比原线路小。从整体树的扩展来看，并不是每次重布线都使整体的路径代价减小，因此需要引导扩展过程，兼顾局部和全局的规划过程。重布线使得新节点扩展得到的随机树冗余线路更少，路径代价更小。图4和图6展示了RRT*重布线操作的过程。

本发明提供一种含障碍物情况的平面环境下运动规划效果的比较，程序实例都需一定迭代时间来探索。图7和图8展示了以x_init＝(10,10)初始位置到目标位置x_goal＝(90,70)的运动规划过程，含障碍环境地图的大小是100*100m²，起点到终点之间的距离是100m。为找到起点到终点之间的路径。如图7所示，算法搜索整个工作空间，需要更多的迭代找到最优路径。本发明提出的方法在更少的时间内找到了路径代价更小的途径，寻路过程因为高斯分布采样、目标偏向的属性以及延后碰撞检查聚焦于端点，趋势性明显，节省了初始搜索路径的时间。随着迭代的增加，该规划器将搜索区域限制在第一个可行路径附近以节约整体循环时间，并最终提供了一个更具成本效益的轨迹。图11为在仿真软件V-rep中基于此发明算法所作的机器人运动规划实例示意图，机器人沿着规划路径行进，期间遵从运动学和动力学约束，同时避开不同形状障碍物。

在发明本实施例中，提供了一种电子设备，包括处理器、存储器、通信总线、网络接口。其中，处理器用于对执行计算机程序；存储器包括内部存储器和外部存储器，内部存储器用于为操作系统和程序执行提供临时性存储环境，外部存储器用于存储现场数据、缓存和记录工作日志；通信总线包括地址总线和数据总线，用于各组件之间发送控制命令和传输数据；可选地，可增补网络接口用于外部中断访问并连接通信。处理器和存储器在组成系统时采用改进型哈佛结构。该计算机程序在执行时可以实现本发明提出的基于高斯采样和目标偏向引导的机器人运动规划算法。

在发明本实施例中，提供了一种存储介质，依据该算法编写的计算机程序可以通过可选的方式存储于介质中。其中，存储介质可以选择不易丢失的存储介质及其组合集成，包括但不限于与非型闪存(NAND FLASH)、或非型闪存(NOR FLASH)、静态存储器(SRAM)、只读存储器(ROM)、可编程存储器(PROM)、电子可擦除可编程只读存储器(EEROM)、磁盘等。

Claims (5)

1.基于高斯采样和目标偏向引导的快速扩展随机树算法的机器人运动轨迹计算方法，其特征在于包括以下步骤：

S1：将含障碍物信息的环境地图，机器人初始点构型x_init、目标构型x_goal输入规划系统；初始化各个参数，包括循环变量i、最大迭代次数N、局部最优代价函数c_best、目标区域检查半径R；

S2：判定循环变量i是否大于最大迭代次数N，若是，则进行S3；若否，则跳出循环至S8；

S3：在自由空间C_free内进行高斯采样，得到随机样本点x_rand，迭代次数递增1，迭代开始；

S4：以初始构型x_init为父节点开始生长树，选择距离树欧氏距离较近的样本点x_nearest；

S5：在随机样本点x_rand、x_nearest和目标构型x_goal共同影响下的方向矢量上，按照步长公式计算得到目标偏向的扩展新节点的位置；

步长公式为：

上述公式中的参数k₁、k₂为可调节参量，当距离障碍物空间较近时令k₁>k₂，增加随机性使扩展过程摆脱局部最小值，当x_near处于节点稀疏的自由空间时，令k₁<k₂，使节点选择趋向于目标构型；

S6：在取得新节点x_new后，对其周围紧邻区域进行考察并进行结构重组，即重选父节点和重连边的操作；

结构重组过程为：分别计算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；

S7：对新结点x_new进行碰撞检查，若通过则将新节点x_new加入到节点集合V，将新边e_new加入到边集合E，并计算此刻x_new到x_near和x_near沿树连边到x_init的路径代价c_new和Cost(x_near)；

S8：通过函数GoalRegion判断新结点x_new是否到达目标构型近邻区域，若是，则进入步骤S9；若否，则重新进入S4步骤的循环；

S9：连接树与目标构型，并对树的整体进行剪枝和平滑操作，最终得到机器人运动轨迹。

2.根据权利要求1所述基于高斯采样和目标偏向引导的快速扩展随机树算法的机器人运动轨迹计算方法，其特征在于：S3中，高斯采样的参数包含初始构型x_init、目标构型x_goal以及局部最小代价函数c_best。

3.一种电子设备，其特征在于：包括处理器、存储器、通信总线、网络接口；其中，处理器用于执行如权利要求1或2所述的机器人运动轨迹计算方法；存储器包括内部存储器和外部存储器，内部存储器用于为操作系统和程序执行提供临时性存储环境，外部存储器用于存储现场数据、缓存和记录工作日志；通信总线包括地址总线和数据总线，用于各组件之间发送控制命令和传输数据。

4.根据权利要求3所述一种电子设备，其特征在于：该电子设备包括网络接口，用于外部中断访问并连接通信。

5.一种存储介质，其特征在于：包括与非型闪存NAND FLASH、或非型闪存NOR FLASH、静态存储器SRAM、只读存储器ROM、可编程存储器PROM、电子可擦除可编程只读存储器EEROM、磁盘中的一种或多种；

依据权利要求1或2所述的机器人运动轨迹计算方法，能够通过可选的方式存储于该存储介质中。

Images