一种避障任务无关人工势场引导的避障路径规划方法

摘要

本发明实施例提出了一种避障任务无关人工势场引导的避障路径规划方法，包括：考虑空间机械臂使用代价，选用机械臂能耗、末端速度、摩擦磨损作为优化目标，在此基础上提出一种引入速度极限的最小奇异值指标；考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，提出一种考虑机械臂操作代价的梯度势场离线构建方法；使用构建的梯度势场引导RRT算法中的随机树进行拓展，规划得出空间机械臂的无障碍运行路径。根据本发明实施例提供的技术方案，在执行冗余度空间机械臂避障路径规划任务的同时，依照上述方法可以提升避障规划算法的效率并降低机械臂的使用代价。

说明书

【技术领域】

本发明涉及自动化控制技术，尤其涉及一种避障任务无关人工势场引导的避障路径规划方法。

【背景技术】

路径规划算法的计算量取决于任务、环境的复杂性以及对规划路径质量的要求，一个好的路径规划算法应该兼顾对规划速度和路径质量；另一方面，从航天任务操作环境的特点出发，在完成避障规划任务的同时，如何降低机械臂其使用代价，也是避障规划方法需要解决的难点问题之一。

依据机器人对环境信息的把握程度可把避障路径规划划分为基于先验完全信息的全局路径规划和基于传感器信息的局部路径规划。一般来说，局部路径规划算法由传感器实时采集环境信息，了解环境地图信息，然后确定出所在地图的位置及其局部的障碍物分布情况，从而进行实时路径规划。该类方法由于获取的环境信息有限，所以容易陷入局部最小。全局路径规划算法可分为两类，一类是基于环境地图的路径规划算法，该类算法首先需要建立环境地图，然后进行无障碍路径搜索，但在高维空间建立环境地图十分困难，所以不适用于冗余度机械臂避障路径规划；另一类方法是基于随机采样的路径规划算法，该类算法可以通过在机械臂关节空间进行采样来避免建立显式的机械臂无障碍关节空间，适用于高维空间的路径规划，但该类算法多数没有对路径进行优化，或者优化效率低下。同时，对于全局路径规划而言，在环境发生改变时，很难快速做出反应，一般需要通过与局部避障路径规划算法来完成任务。

综上所述，对空间机械臂的避障任务进行算法改进是非常重要的，一方面为保证规划得出的路径质量，同时为使全局规划算法能够与局部算法有效结合，就需要使规划出的路径远离奇异位形，并且在规划过程中考虑机械臂的使用代价；另一方面从路径规划的效率出发，希望机械臂能以较高的效率完成规定的操作程序，降低路径规划的时间代价。

【发明内容】

有鉴于此，本发明实施例提出了一种避障任务无关人工势场引导的避障路径规划方法，以降低机械臂的使用代价，提升与局部避障路径规划算法结合的能力，提升避障路径规划的效率。

本发明实施例提出了一种避障任务无关人工势场引导的避障路径规划方法，包括：

考虑空间机械臂使用代价，选用机械臂能耗、末端速度、摩擦磨损作为优化目标，在此基础上提出一种引入速度极限的最小奇异值指标；

考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，提出一种考虑机械臂操作代价的梯度势场离线构建方法；

使用构建的梯度势场引导RRT算法中的随机树进行拓展，规划得出空间机械臂的无障碍运行路径。

上述方法中，考虑空间机械臂使用代价，选用机械臂能耗、末端速度、摩擦磨损作为优化目标，在此基础上提出一种引入速度极限的最小奇异值指标，包括：考虑空间机械臂使用代价，选用机械臂能耗、末端速度、摩擦磨损作为优化目标，在此基础上提出的引入速度极限的最小奇异值指标为：

其中，σ_i为机械臂雅克比矩阵奇异值，i＝1，2，3，...，n，n为机械臂自由度数；当某几个奇异值σ_j远小于其他奇异值σ_k时，即σ_j＜＜σ_k，j＝n，n-1，..，t，k＝t-1，t-2，..，2，1，对所有σ_j，j＝n，n-1，..，t，计算σ_jv_j′^Tv_j；其中v_j为将雅克比矩阵奇异值分解后，J(q)＝U∑V^T，矩阵V中对应σ_j的第j个列向量，v_j′为v_j方向上的一个向量，v_j′的每一分量都小于关节角速度极限，且v_j′的某一分量等于关节角速度极限；σ_jmv_jm′^Tv_jm为所有σ_jv_j′^Tv_j中的最小值，j＝n，n-1，..，t。

上述方法中，考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，提出一种考虑机械臂操作代价的梯度势场的离线构建方法，包括：考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，离线构建一种考虑机械臂操作代价的梯度势场，包括：由所述引入速度极限的雅各比矩阵最小奇异值，建立考虑最小奇异值及关节速度极限的势场表达式E₁，为：

式中，α₁和β₁为任意正数，用于调节势场幅值与作用范围；K为任意正数，表示任务要求末端速度大小；K′为所述σ_jv_j′^Tv_j的最小值σ_jmv_jm′^Tv_jm；为机械臂所有关节中最小的速度极限；σ_r为机械臂雅克比矩阵最小奇异值；

考虑可操作度建立的势场表达式E₂如下：

其中ω为机械臂可操作度，κ为条件数；α₂和β₂为任意正数，α₂用于调整势函数的幅值，β₂用于调整引力势场E₂的作用范围；

由关节极限建立的势场为E₃：.

其中，α为任意正数；n为机械臂自由度数；q_i为第i个关节的关节角，q_imin和q_imax分别为第i个关节角的最小值和最大值；

将所述三个势场合并，所述合势场定义E(q)为：

E(q)＝E₁+E₂+E₃

计算得出势场的梯度表达式：

通过上式的离散形式，能够求出n个梯度势场，其中关于q₁的所述梯度势场表达式为：

至均可通过类似表达式求得。

上述方法中，使用构建的梯度势场引导RRT算法中的随机树进行拓展，规划得出空间机械臂的运行路径，包括：将所述离线建立的梯度势场融入RRT算法，在每一次拓展时使随机树有概率朝向梯度下降的方向拓展；

所述一种考虑使用代价优化的避障任务无关人工势场引导的RRT改进算法，算法中涉及符号表示如下：

q_x和c_x：分别表示某构型空间位置及其对应笛卡尔坐标系位置，下标x用于说明二者之间的对应关系；

T：为一系列节点node的集合；随机树T包含以下几个属性：

(1)根节点node_root；

(2)构型节点集NODE：包含一系列节点node的集合，按其rank由高到低排列；

node：构成随机树的基本单元，每个节点都包含以下几个属性：

(1)node.q：节点node代表的构型位置；

(2)node.rank：节点node的rank值，rank值越大说明构型node.q处势场值较小且距离障碍物较远，rank的计算方法如下：

其中，d₁为机械臂末端在当前构型下距目标位置的距离；d₂为机械臂当前构型下距离障碍物的距离；d_safe为具体避障规划任务限定的机械臂距离障碍物的安全距离；

(3)node.f_count：用于记录随机树节点node拓展失败的次数；

(4)node.parent：构型node的父节点，随机树T的根节点node_root的父节点为；

(5)node.child：构型node的子节点；

Q_set：Q_set用于记录进行过碰撞检测的构型位置及其距离障碍物的距离，用于估计新采样构型距障碍物的距离；

d₃：随机树的最小拓展步长；

算法中使用的函数如下：

T.init(node_init)：初始化随机树T；

T.add(q₁，node₂)：向随机树T中加入以q₁为构型的节点，node₂为该节点的父节点；

T.random_pick(n)：从随机树T中依rank值选出对应的构型节点node，n用于调整高rank值节点被选出的概率，n越大，该概率越高；

T.remove(node)：从随机树T中移除构型节点node，及其所有子节点；

T.closet(q)：从随机树T中选出距离构型q最近的节点；

T.judgeFcount(node)：判断节点node拓展失败次数是否超过阈值，如果超过，删除该节点；

random_sample(n)：在构型空间中随机选取n个构型q；

random_num()：产生[0，1]区间内的随机数；

lowest_potential(A)：从集合A中选出势场值最低的构型；

get_config(q，d)：给定当前构型位置，确定新节点的位置，方向随机；

get_alpha(q，d)：给定拓展方向，计算该方向上随机树拓展的最大步长；

collision_detection(q)：对处于构型q下的机械臂进行碰撞检测，返回距离障碍物的最近距离，若发生碰撞则返回0；

grad(q)：返回构型q处的势场梯度；

所述避障任务无关人工势场引导的RRT改进算法如下所示：

所述算法中，Case1的伪代码如下所示：

所述算法中，Case2的伪代码如下所示：

由上述算法能够为机械臂规划出一条无障碍路径；

所述运行路径即为由避障任务无关人工势场引导的RRT改进算法得出的路径；所述路径能够使空间机械臂在完成避障路径规划问题并保证路径规划效率的前提下降低机械臂使用代价，提升运行过程中与局部路径规划算法结合的能力。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。

图1是本发明实施例所提出的避障任务无关人工势场引导的避障路径规划方法的流程示意图；

图2是二自由度机械臂关节速度极限示意图；

图3是利用本发明实施例所提出的避障任务无关人工势场引导的避障路径规划算法的流程图；

图4是基于KNN的碰撞检测策略流程示意图；

图5是本发明实施例中八自由度空间机械臂DH坐标系示意图；

图6是本发明实施例中八自由度机械臂仿真模型；

图7是利用本发明实施例所提出的避障任务无关人工势场引导的避障路径规划算法规划得出的无障碍路径示意图。

【具体实施方式】

为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

避障任务无关人工势场引导的避障路径规划方法主要包括三个部分：考虑空间机械臂使用代价，选用机械臂能耗、末端速度、摩擦磨损作为优化目标，在此基础上提出一种引入速度极限的最小奇异值指标；考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，提出一种考虑机械臂操作代价的梯度势场离线构建方法；使用构建的梯度势场引导RRT算法中的随机树进行拓展，规划得出空间机械臂的无障碍运行路径。

本发明实施例给出一种避障任务无关人工势场引导的避障路径规划方法，请参考图1，其为本发明实施例所提出的避障任务无关人工势场引导的避障路径规划方法的流程示意图，如图1所示，该方法包括以下步骤：

步骤101，考虑空间机械臂使用代价，选用机械臂能耗、末端速度、摩擦磨损作为优化目标，在此基础上提出一种引入速度极限的最小奇异值指标。

具体的，在完成避障路径规划的前提下，应当尽量减小使用不当给机械臂带来的损耗，延缓其部件性能衰减，在这一过程中，机械臂的使用代价可以由轴承间隙、齿轮位置误差、轴承运动误差等影响因素来表征。这些影响因素能够通过等中间响应层因素间接反映。所以选用机械臂能耗、末端速度、摩擦磨损等间接因素作为优化目标。

机械臂能耗与关节角速度的关系可以由下式表示：

其中，为第i个关节的角速度；τ_i(t)为第i个关节的关节力矩；t₀为任务开始时间；t_f为任务终止时间。

机械臂雅各比矩阵的最小奇异值能够用来作为衡量所需关节速度上限的指标，即：

σ_r为雅各比矩阵最小奇异值，为机械臂末端速度，为机械臂关节角速度；上式表明，在任务所需末端速度不变的情况下，σ_r越小，则需求的越大；所以从降低能耗的角度出发，应当使雅各比矩阵最小奇异值尽可能大；另一方面，从与局部路径规划算法的结合方面出发，σ_r越大，雅各比矩阵越远离病态，当使用伪逆法求解机械臂关节角速度时，精度越高；反之，当机械臂处于奇异位型时，伪逆法求解得到的关节角速度趋于无穷，无法正常运作。

机械臂可操作度定义为：

其中，J(q)为机械臂在构型q处的雅各比矩阵；在末端速度一定的情况下，可操作度越大，则需求的关节角速度相对越小；与最小奇异值指标不同的是，可操作度衡量了机械臂各个操作方向上的灵敏度。

此外，当机械臂关节长时间处于关节极限位置时，会加剧关节的磨损。所以应当在任务完成的前提下，使机械臂关节远离极限位置。

综上所述，选取机械臂雅各比矩阵最小奇异值，机械臂可操作度，机械臂关节极限等指标作为路径规划时考虑的优化目标。通过在路径规划过程中选择相应的构型节点，完成对这些指标进行优化，一方面降低机械臂使用代价，另一方面使机械臂在避障路径规划过程中能与局部避障算法有效的结合。

将机械臂雅各比矩阵J(q)_mxn进行奇异值分解，得到：

其中，∑＝U^TJ(q)V＝diag(σ₁(q)，σ₂(q)，...，σ_r(q))，最小奇异值为：σ_r(q)

上式可以写做：

J(q)＝u₁σ₁v₁^T+u₂σ₂v₂^T+…+u_mσ_mv_m^T

则有可将视为将关节角速度向R^nx1空间的一组基v₁，v₂，v₃，...，v_n进行投影，上式可以理解为在R^mx1空间内将u₁，u₂，u₃，...，u_m进行线性组合，若存在某奇异值σ_i＝0，则无论关节角速度如何变化，得出的机械臂末端速度在u_i方向都不会存在速度分量；同理，若存在奇异值σ_i→0，要在u_i方向产生速度分量，则需要

在机械臂在执行任务的过程中，有时希望机械臂末端在任意方向上的速度大小都能够达到K，以确保机械臂能够完成任务，此时仅对最小奇异值σ_r加以约束不能够保证满足该条件，应当同时考虑关节速度极限在v₁，v₂，v₃，...，v_n上的分量，如图2所示。

为便于说明，以一个二自由度机械臂为模型进行解释。如图2所示，为角速度极限范围内的一个关节角速度，v₁和v₂为J^TJ的一组特征向量，在R₂空间内为一组标准正交基，其对应的特征向量分别为σ₁与σ₂，其中σ₁＞σ₂，但所以不能确定与之间的具体大小关系，也就无法确定和之间大小关系。即仅确保最小奇异值方向的速度Ku_r对应的关节角速度满足角速度约束，并不一定保证其他方向的角速度满足约束。

在各关节角速度极限近似相等，某奇异值远小于其他奇异值σ_r＜＜σ_i>且次最小奇异值方向对应的关节速度方向v_r-1同时取到关节速度极限最小值，在时，不妨取其中M和N均为常数。Mv_r在v_r上的投影为Nv_r-1在v_r-1上的投影为n为机械臂自由度数。若要求由σ_rM＞K能导出σ_r-1N＞K，只需σ_r-1N＞σ_rM，即

若机械臂自由度n＝8，则即可。即对于一个8自由度机械臂，在情况下，只要满足：

则其他方向末端速度均能达到要求速度大小K。式中，σ_r为雅各比矩阵最小奇异值，v_r为σ_r对应的特征向量；v_r′为v_r方向上的一个向量，v_r′的每一分量都小于关节角速度极限，且v_r′的某一分量取到关节角速度极限；K为任务要求的最低末端速度，需要留出一定余量。在各关节角速度极限近似相等，某几个奇异值远小于其他奇异值σ_j＜＜σ_i时，则对于较小的几个奇异值，分别计算σ_jv_j′^Tv_j，取其中的最小值σ_jmv_jm′^Tv_jm＞K，即保证即可。

步骤102，考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，提出一种考虑机械臂操作代价的梯度势场离线构建方法。

由上述讨论可建立考虑最小奇异值及关节速度极限的势场表达式E₁，为：

式中，α₁和β₁为任意正数，用于调节势场幅值与作用范围；K为任意正数，表示任务要求末端速度大小；K′为步骤101中σ_jv_j′^Tv_j的最小值σ_jmv_jm′^Tv_jm；为机械臂所有关节中最小的速度极限；σ_r为机械臂雅克比矩阵最小奇异值；当远大于K时，说明σ_r足够大，此时E₁→0；只有当接近K时，才考虑K-K′，；当K′远离K时，E₁增大，产生斥力，使机械臂远离该构型位置。

谢碧云等人于2010年提出一种基于条件数约束的方向可操作度(DMCCN)，并以此为路径规划过程中的优化目标，对平面3R机械臂进行仿真实验。该指标当条件数较小时，指标主要受方向可操作度的影响；条件数不断增大时，方向可操作度对指标的影响逐渐削弱，条件数对指标的影响不断增大。DMCCN的定义为：

式中，为条件数，定义为雅各比矩阵J(q)最大奇异值与最小奇异值的比值；κ_max为任务要求的最大条件数；u为沿末端运动速度方向的单位矢量；定义为机械臂的方向可操作度；α为一正数，用于调节条件数及方向可操作度的灵敏度。

可以将出现较大条件数的情况分为两种，σ_min较小的情况在依最小奇异值建立势场时已经加以考虑；σ_max较大对机械臂末端运动没有影响。并且，对于可操作度ω较小的情况在之前最小奇异值建立势场时也加以考虑，故此时只考虑可操作度较大的情况。所以在考虑机械臂末端各方向运动能力时，期望机械臂朝向条件数较小且可操作度较大的构型移动。由于势场与任务无关，与DMCCN不同，更多的考虑可操作度的各向同性，表达式如下：

考虑可操作度建立的势场表达式E₂如下：

其中ω为机械臂可操作度，κ为条件数；α₂和β₂为任意正数，α₂用于调整势函数的幅值，β₂用于调整引力势场E₂的作用范围；

冗余度空间机械臂在关节极限范围内，其末端执行器能够到达工作空间中的任意目标位置，机械臂关节极限可以表示为：

q_imin≤q_i≤q_imax>

其中，q_imin为第i个关节的关节角下限，q_imax为第i个关节的关节角上限，n为机械臂自由度数。

由以上分析可知，希望使机械臂在运行过程中尽量远离关节角上下限，且当机械臂远离关节限位时，对机械臂的运行无影响。

由关节极限建立的势场为E₃：.

其中，α为任意正数；n为机械臂自由度数；q_i为第i个关节的关节角，q_imin和q_imax分别为第i个关节角的最小值和最大值；由上式得知，在当前关节位置距关节角下限较近时，d_imin值较大，d_imax值较小，势场主要由式中的第一项决定；若当前关节位置距关节角上下限都较远时，d_imin和d_imax的值都较小，势场趋近于0。

将以上三个势场合并，共同作用于随机树的拓展，以实现对优化目标的优化。

将所述三个势场合并，所述合势场定义E(q)为：

E(q)＝E₁+E₂+E₃

计算得出势场的梯度表达式：

通过上式的离散形式，能够求出n个梯度势场，其中关于q₁的所述梯度势场表达式为：

至均可通过类似表达式求得。

步骤103，使用构建的梯度势场引导RRT算法中的随机树进行拓展，规划得出空间机械臂的无障碍运行路径。

算法中涉及符号表示如下：

q_x和c_x：分别表示某构型空间位置及其对应笛卡尔坐标系位置，下标x用于说明二者之间的对应关系；

T：为一系列节点node的集合；随机树T包含以下几个属性：

(1)根节点node_root；

(2)构型节点集NODE：包含一系列节点node的集合，按其rank由高到低排列；

node：构成随机树的基本单元，每个节点都包含以下几个属性：

(1)node.q：节点node代表的构型位置；

(2)node.rank：节点node的rank值，rank值越大说明构型node.q处势场值较小且距离障碍物较远，rank的计算方法如下：

其中，d₁为机械臂末端在当前构型下距目标位置的距离；d₂为机械臂当前构型下距离障碍物的距离；d_safe为机械臂距离障碍物的安全距离；由上式可知，只有当机械臂距离障碍物距离小于安全距离时才对其进行考虑，并且d_safe随实际任务的不同可以进行调整；

(3)node.f_count：用于记录随机树节点node拓展失败的次数；

(4)node.parent：构型node的父节点，随机树T的根节点node_root的父节点为；

(5)node.child：构型node的子节点：

Q_set：Q_set用于记录进行过碰撞检测的构型位置及其距离障碍物的距离，用于估计新采样构型距障碍物的距离；

d₃：随机树的最小拓展步长；

算法中使用的函数如下：

T.init(node_init)：初始化随机树T；

T.add(q₁，node₂)：向随机树T中加入以q₁为构型的节点，node₂为该节点的父节点；

T.random_pick(n)：从随机树T中依rank值选出对应的构型节点node，n用于调整高rank值节点被选出的概率，n越大，该概率越高；

T.remove(node)：从随机树T中移除构型节点node，及其所有子节点；

T.closet(q)：从随机树T中选出距离构型q最近的节点；

T.judgeFcount(node)：判断节点node拓展失败次数是否超过阈值，如果超过，删除该节点；

random_sample(n)：在构型空间中随机选取n个构型q；

random_num()：产生[0，1]区间内的随机数；

lowest_potential(A)：从集合A中选出势场值最低的构型；

get_config(q，d)：给定当前构型位置，确定新节点的位置，方向随机；

get_alpha(q，d)：给定拓展方向，计算该方向上随机树拓展的最大步长；

collision_detection(q)：对处于构型q下的机械臂进行碰撞检测，返回距离障碍物的最近距离，若发生碰撞则返回0；

grad(q)：返回构型q处的势场梯度；

所述算法通过在路径规划的过程中引入势场，使机械臂的某些目标达到优化，提高与其他局部规划算法结合的能力，同时，借鉴一种基于KNN算法的碰撞检测思想，以提升路径规划中碰撞检测的效率；所述算法的说明可以从随机性和确定性两个方面进行展开。算法整体流程如图3所示。

在确定性拓展方面，其目的在于能够使机械臂朝向目标位置生长，将确定性拓展的情况记为Case1；在Case1中，首先，每次从随机树T中随机挑选一个节点，拥有较高rank值的节点被选出的概率较高，某节点若拥有较高的rank值则表明该节点距目标位置可能较近，同时机械臂在该节点处有较好的运动性能并且距离障碍物较远；其次，对于选出的节点，使用随机拓展的方式或沿势场梯度的方向进行拓展，其中沿势场梯度下降只在机械臂距离目标位置较远时才使用，这是因为在实际使用中势场间隔较大，所以当机械臂靠近目标位置时，依旧采用梯度下降方向作为引导，可能会使机械臂在目标位置附近反复震荡；最后，对拓展之后的节点判断其距离笛卡尔目标的距离，以确保每一次生长都能够靠近目标位置；

Case1的伪代码如下所示：

在随机拓展方面，目的在于使路径规划算法达到概率完备，能够使机械臂绕开障碍物，将随机拓展的情况记为Case2：在Case2中，首先在构型空间中随机采样n次，从中选出势场值最小的构型q_h；传统的随机性拓展只从随机树T中选取选择距离q_h最近的节点进行拓展，在此基础上以一定概率选取rank值较高的节点，目的在于在目标优化的同时提升算法收敛的能力；将选出的节点作为node_rand，以node_rand.q→q_h方向将随机树进行拓展，并且不考虑新添加节点距目标构型的距离，这样做能够充分保留算法的“随机性”；

Case2的伪代码如下所示：

此外，本算法还引入基于概率的碰撞检测策略与该算法进行结合；原策略通过记录每一个节点的构型的哈希值及碰撞状态，当得到足够的样本后，以KNN算法估计新构型的碰撞状态；

由以上分析可以综合得出改进的RRT算法的整体流程，如下所示：

核心函数伪代码及说明：

该函数中init(Q_Set)用于初始化构型节点及其距障碍物距离，用于之后的碰撞检测策略；该函数主要用于随机生成哈希表参数。

用于判断节点node的f_count是否高于阈值，若高于阈值，则从树T中移除节点node，且返回1；否则返回0；

该函数用于从随机树中选出一个节点用于拓展。首先从随机树中选出n-1个节点以及T.node[1]，共n个节点，再从这n个节点中选出一个节点；这样能够保证距离目标最近的节点每次都有较大概率被选出同时保留随机性；在实际使用中，可以从随机树中选出rank值较高的2、3个节点，再任意选择其他节点，以此提高在靠近目标位置附近的解的随机性。

传统碰撞检测策略在用于RRT，PRM等随机采样路径规划算法时，每产生一个新的节点，都需要进行一次碰撞检测，以确保新加入的节点没有发生碰撞；Pan等人在2016年提出一种基于概率的碰撞检测策略，通过记录之前查询点的构型位置及碰撞状态，来估计新构型位置的碰撞状态，通过这一策略，能够减少执行碰撞检测算法的次数，以提升路径规划算法的效率。该策略的整体流程如图4所示。

首先，通过LSH-based KNN算法找出新构型位置附近的已查询的其他构型节点；接着，由“距离条件”判断这些节点是否距离新构型足够近，如果足够近则通过这些节点估计新构型的碰撞状态，否则执行碰撞检测；对于计算得出的碰撞概率，由“模糊条件”判断是否足够确定新构型的碰撞结果，若估计出的新构型碰撞或者不碰撞的可能性较为接近，则说明该构型可能处于临界碰撞状态，需要执行碰撞检测；最后将得到的估计结果记录、保存。

所述算法主要利用碰撞检测算法计算随机树拓展的步长；所以，对该碰撞检测策略做了如下改动：

(1)首先，在记录节点时，使用距障碍物距离代替碰撞状态，以此估计新构型x距离障碍物的距离；

(2)其次，在记录距离时，只有当执行碰撞检测时才进行记录，对于估计的距离值不进行记录，这是为了保证估计结果不会漂移；

(3)最后，通过对原算法的分析可知，模糊条件主要是为了在临界碰撞位置保证机械臂的安全性，但该条件需要计算概率，在本算法中并不适用，所以为了保证机械臂的安全性，只有当估计出的距离大于某一阈值时才使用估计结果，即当机械臂距障碍物较近时，执行碰撞检测，以保证机械臂与障碍物不发生碰撞。

用于具体实施例中，以一个八自由度的模块化机械臂为具体研究对象进行了仿真及实物实验研究，其中本发明实施例中八自由度空间机械臂DH坐标系示意图如图5所示。

表一为空间机械臂DH参数表，如下所示：

表一

连杆iα_i-1a_i-1d_iθ_i100220-π/22-π/20-110-π/23-π/2024504π13590050130-90-π/26-π/2030007π/20008-π/20850

机械臂关节角变化范围：

八自由度机械臂仿真模型如图6所示。

其中机械臂初始位姿：

目标位置为：c_goal＝[150>

由所述算法得出的无障碍路径示意图如图7所示。

由图中能够看出，该算法能够规划出一条从初始构型到目标位置的无障碍路径，并且机械臂在运行过程中与障碍物始终保持一定的安全距离，说明该算法的有效性。

所述算法的优化性能通过三个指标对优化性能进行比较：

Obj1表示产生末端速度最弱方向的能产生的最高速度的能力；该值越小，说明该方向产生末端速度的能力较强；式中Num为该路径上的节点个数，Obj1为各个节点上(K-K′)的平均；

Obj2表示机械臂各个节点上可操作度的平均；该值越大，说明机械臂在各个方向上的运动能力整体较好，产生同样的末端速度所需的关节角速度较小，在一定程度上能够降低机械臂的能耗；

Obj3表示机械臂各个节点上条件数的平均；该值同样用来衡量机械臂各方向上的运动能力，可以看作是对Obj2的补充。

表二为原算法规划路径得出的指标，如下所示：

表二

组别第1组第2组第3组第4组第5组第6组第7组第8组Obj126.3343.0630.9331.6416.7638.5815.0119.88Obj24.453.134.033.894.243.884.325.18Obj315.3715.9311.5111.979.3812.6610.0810.59组别第9组第10组第11组第12组第13组第14组第15组平均值Obj146.2113.2420.9424.4224.7928.959.4426.01Obj23.135.654.234.074.404.125.764.30Obj317.287.969.437.7010.1810.58.1611.25

表三为所述算法规划路径得出优化后的指标，如下所示：

表三

组别第1组第2组第3组第4组第5组第6组第7组第8组Obj116.0724.33-3.828.663.2917.4720.9616.65Obj25.155.085.795.806.015.514.714.96Obj38.8710.666.908.416.218.9910.0513.39组别第9组第10组第11组第12组第13组第14组第15组平均值Obj16.6214.4221.9313.154.5314.429.7912.56Obj25.944.584.564.985.545.004.905.23Obj37.389.5011.898.966.309.3011.489.22

由实验结果看出，原算法与所述算法在路径运动性能优化方面，具有较明显的优化效果。由原算法得出的Obj1、Obj2、Obj3的平均值分别为26.01，4.30，11.25由所述算法，经过在同一实验条件下进行15次实验，得出的Obj1、Obj2、Obj3平均值分别为12.56、5.23和9.22；由Obj1、Obj2、Obj3的特点可知，所述算法对以上三项指标都进行了一定的优化，Obj1减小了13.45，说明机械臂以该路径运行时，有更高的能力产生较大的末端速度；Obj2上升了0.93，Obj3下降了2.03说明机械臂使用所述路径规划算法，能使各方向的运动能力更加均衡，并使各方向整体运动性能得到提升。

在与关节角极限的距离方面，使用的指标为：

M＝min(q_idist)

q_idist＝min(min_dist(q_i，q_sub)，min_dist(q_i，q_sup))

q_sub与q_sup分别为关节角的下极限与上极限，均为n维向量；min_dist(q，q_sub)用来计算q与q_sub每一个维度上的距离，并取其中的最小值；式中i为整个路径关节序列的组数，M则用于取q_idist中的最小值；

表四为原算法规划路径得出的与关节极限的距离，如下所示：

表四

组别第1组第2组第3组第4组第5组第6组第7组第8组M0.65290.09640.00430.05330.84520.17460.00920.3971组别第9组第10组第11组第12组第13组第14组第15组平均值M0.06910.78980.01290.00220.22100.01880.76600.2742

表五为所述算法规划路径得出的与关节极限的距离，如下所示：

表五

组别第1组第2组第3组第4组第5组第6组第7组第8组M0.76280.73690.57560.88800.78660.65501.27900.6994组别第9组第10组第11组第12组第13组第14组第15组平均值M0.08160.25590.06990.04120.12650.37720.34100.5118

若取关节角距离极限3度为标准，换算成弧度值为0.052，则由所述算法规划得出的路径在此安全范围外的只有1组；由原方法得出的路径在范围外有5组；取关节角极限5度为标准，弧度值为0.087，则所述方法有3组在安全范围外，原方法有7组；并且，由所述方法规划得出的路径关节极限最小值为0.0412，为2～3度之间，原方法得出的最小值为0.0022，该值小于1度。由以上分析得出，所述方法在避关节极限方面同样具有明显的效果。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。