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

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

Info

Publication number
CN107234617A
CN107234617A CN201710558115.6A CN201710558115A CN107234617A CN 107234617 A CN107234617 A CN 107234617A CN 201710558115 A CN201710558115 A CN 201710558115A CN 107234617 A CN107234617 A CN 107234617A
Authority
CN
China
Prior art keywords
msub
mrow
node
mtd
mechanical arm
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.)
Granted
Application number
CN201710558115.6A
Other languages
English (en)
Other versions
CN107234617B (zh
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.)
Beijing University of Posts and Telecommunications
Original Assignee
Beijing 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 Beijing University of Posts and Telecommunications filed Critical Beijing University of Posts and Telecommunications
Priority to CN201710558115.6A priority Critical patent/CN107234617B/zh
Publication of CN107234617A publication Critical patent/CN107234617A/zh
Application granted granted Critical
Publication of CN107234617B publication Critical patent/CN107234617B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

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

Description

一种避障任务无关人工势场引导的避障路径规划方法
【技术领域】
本发明涉及自动化控制技术，尤其涉及一种避障任务无关人工势场引导的避障路径规划方法。
【背景技术】
路径规划算法的计算量取决于任务、环境的复杂性以及对规划路径质量的要求，一个好的路径规划算法应该兼顾对规划速度和路径质量；另一方面，从航天任务操作环境的特点出发，在完成避障规划任务的同时，如何降低机械臂其使用代价，也是避障规划方法需要解决的难点问题之一。
依据机器人对环境信息的把握程度可把避障路径规划划分为基于先验完全信息的全局路径规划和基于传感器信息的局部路径规划。一般来说，局部路径规划算法由传感器实时采集环境信息，了解环境地图信息，然后确定出所在地图的位置及其局部的障碍物分布情况，从而进行实时路径规划。该类方法由于获取的环境信息有限，所以容易陷入局部最小。全局路径规划算法可分为两类，一类是基于环境地图的路径规划算法，该类算法首先需要建立环境地图，然后进行无障碍路径搜索，但在高维空间建立环境地图十分困难，所以不适用于冗余度机械臂避障路径规划；另一类方法是基于随机采样的路径规划算法，该类算法可以通过在机械臂关节空间进行采样来避免建立显式的机械臂无障碍关节空间，适用于高维空间的路径规划，但该类算法多数没有对路径进行优化，或者优化效率低下。同时，对于全局路径规划而言，在环境发生改变时，很难快速做出反应，一般需要通过与局部避障路径规划算法来完成任务。
综上所述，对空间机械臂的避障任务进行算法改进是非常重要的，一方面为保证规划得出的路径质量，同时为使全局规划算法能够与局部算法有效结合，就需要使规划出的路径远离奇异位形，并且在规划过程中考虑机械臂的使用代价；另一方面从路径规划的效率出发，希望机械臂能以较高的效率完成规定的操作程序，降低路径规划的时间代价。
【发明内容】
有鉴于此，本发明实施例提出了一种避障任务无关人工势场引导的避障路径规划方法，以降低机械臂的使用代价，提升与局部避障路径规划算法结合的能力，提升避障路径规划的效率。
本发明实施例提出了一种避障任务无关人工势场引导的避障路径规划方法，包括：
考虑空间机械臂使用代价，选用机械臂能耗、末端速度、摩擦磨损作为优化目标，在此基础上提出一种引入速度极限的最小奇异值指标；
考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，提出一种考虑机械臂操作代价的梯度势场离线构建方法；
使用构建的梯度势场引导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，计算σjvj′ Tvj；其中vj为将雅克比矩阵奇异值分解后，J(q)＝U∑VT，矩阵V中对应σj的第j个列向量，vj′为vj方向上的一个向量，vj′的每一分量都小于关节角速度极限，且vj′的某一分量等于关节角速度极限；σjmvjm′ Tvjm为所有σjvj′ Tvj中的最小值，j＝n，n-1，..，t。
上述方法中，考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，提出一种考虑机械臂操作代价的梯度势场的离线构建方法，包括：考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，离线构建一种考虑机械臂操作代价的梯度势场，包括：由所述引入速度极限的雅各比矩阵最小奇异值，建立考虑最小奇异值及关节速度极限的势场表达式E1，为：
式中，α1和β1为任意正数，用于调节势场幅值与作用范围；K为任意正数，表示任务要求末端速度大小；K′为所述σjvj′ Tvj的最小值σjmvjm′ Tvjm；为机械臂所有关节中最小的速度极限；σr为机械臂雅克比矩阵最小奇异值；
考虑可操作度建立的势场表达式E2如下：
其中ω为机械臂可操作度，κ为条件数；α2和β2为任意正数，α2用于调整势函数的幅值，β2用于调整引力势场E2的作用范围；
由关节极限建立的势场为E3：.
其中，α为任意正数；n为机械臂自由度数；qi为第i个关节的关节角，qimin和qimax分别为第i个关节角的最小值和最大值；
将所述三个势场合并，所述合势场定义E(q)为：
E(q)＝E1+E2+E3
计算得出势场的梯度表达式：
通过上式的离散形式，能够求出n个梯度势场，其中关于q1的所述梯度势场表达式为：
至均可通过类似表达式求得。
上述方法中，使用构建的梯度势场引导RRT算法中的随机树进行拓展，规划得出空间机械臂的运行路径，包括：将所述离线建立的梯度势场融入RRT算法，在每一次拓展时使随机树有概率朝向梯度下降的方向拓展；
所述一种考虑使用代价优化的避障任务无关人工势场引导的RRT改进算法，算法中涉及符号表示如下：
qx和cx：分别表示某构型空间位置及其对应笛卡尔坐标系位置，下标x用于说明二者之间的对应关系；
T：为一系列节点node的集合；随机树T包含以下几个属性：
(1)根节点noderoot；
(2)构型节点集NODE：包含一系列节点node的集合，按其rank由高到低排列；
node：构成随机树的基本单元，每个节点都包含以下几个属性：
(1)node.q：节点node代表的构型位置；
(2)node.rank：节点node的rank值，rank值越大说明构型node.q处势场值较小且距离障碍物较远，rank的计算方法如下：
其中，d1为机械臂末端在当前构型下距目标位置的距离；d2为机械臂当前构型下距离障碍物的距离；dsafe为具体避障规划任务限定的机械臂距离障碍物的安全距离；
(3)node.fcount：用于记录随机树节点node拓展失败的次数；
(4)node.parent：构型node的父节点，随机树T的根节点noderoot的父节点为；
(5)node.child：构型node的子节点；
Qset：Qset用于记录进行过碰撞检测的构型位置及其距离障碍物的距离，用于估计新采样构型距障碍物的距离；
d3：随机树的最小拓展步长；
算法中使用的函数如下：
T.init(nodeinit)：初始化随机树T；
T.add(q1，node2)：向随机树T中加入以q1为构型的节点，node2为该节点的父节点；
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个关节的关节力矩；t0为任务开始时间；tf为任务终止时间。
机械臂雅各比矩阵的最小奇异值能够用来作为衡量所需关节速度上限的指标，即：
σr为雅各比矩阵最小奇异值，为机械臂末端速度，为机械臂关节角速度；上式表明，在任务所需末端速度不变的情况下，σr越小，则需求的越大；所以从降低能耗的角度出发，应当使雅各比矩阵最小奇异值尽可能大；另一方面，从与局部路径规划算法的结合方面出发，σr越大，雅各比矩阵越远离病态，当使用伪逆法求解机械臂关节角速度时，精度越高；反之，当机械臂处于奇异位型时，伪逆法求解得到的关节角速度趋于无穷，无法正常运作。
机械臂可操作度定义为：
其中，J(q)为机械臂在构型q处的雅各比矩阵；在末端速度一定的情况下，可操作度越大，则需求的关节角速度相对越小；与最小奇异值指标不同的是，可操作度衡量了机械臂各个操作方向上的灵敏度。
此外，当机械臂关节长时间处于关节极限位置时，会加剧关节的磨损。所以应当在任务完成的前提下，使机械臂关节远离极限位置。
综上所述，选取机械臂雅各比矩阵最小奇异值，机械臂可操作度，机械臂关节极限等指标作为路径规划时考虑的优化目标。通过在路径规划过程中选择相应的构型节点，完成对这些指标进行优化，一方面降低机械臂使用代价，另一方面使机械臂在避障路径规划过程中能与局部避障算法有效的结合。
将机械臂雅各比矩阵J(q)mxn进行奇异值分解，得到：
其中，∑＝UTJ(q)V＝diag(σ1(q)，σ2(q)，...，σr(q))，最小奇异值为：σr(q)
上式可以写做：
J(q)＝u1σ1v1 T+u2σ2v2 T+…+umσmvm T
则有可将视为将关节角速度向Rnx1空间的一组基v1，v2，v3，...，vn进行投影，上式可以理解为在Rmx1空间内将u1，u2，u3，...，um进行线性组合，若存在某奇异值σi＝0，则无论关节角速度如何变化，得出的机械臂末端速度在ui方向都不会存在速度分量；同理，若存在奇异值σi→0，要在ui方向产生速度分量，则需要
在机械臂在执行任务的过程中，有时希望机械臂末端在任意方向上的速度大小都能够达到K，以确保机械臂能够完成任务，此时仅对最小奇异值σr加以约束不能够保证满足该条件，应当同时考虑关节速度极限在v1，v2，v3，...，vn上的分量，如图2所示。
为便于说明，以一个二自由度机械臂为模型进行解释。如图2所示，为角速度极限范围内的一个关节角速度，v1和v2为JTJ的一组特征向量，在R2空间内为一组标准正交基，其对应的特征向量分别为σ1与σ2，其中σ1＞σ2，但所以不能确定与之间的具体大小关系，也就无法确定和之间大小关系。即仅确保最小奇异值方向的速度Kur对应的关节角速度满足角速度约束，并不一定保证其他方向的角速度满足约束。
在各关节角速度极限近似相等，某奇异值远小于其他奇异值σr＜＜σi i＝1，2，3...，r-1时。考虑极限情况，若最小奇异值对应的关节速度方向能取到关节速度极限的最大值，即且次最小奇异值方向对应的关节速度方向vr-1同时取到关节速度极限最小值，在时，不妨取其中M和N均为常数。Mvr在vr上的投影为Nvr-1在vr-1上的投影为n为机械臂自由度数。若要求由σrM＞K能导出σr-1N＞K，只需σr-1N＞σrM，即
若机械臂自由度n＝8，则即可。即对于一个8自由度机械臂，在情况下，只要满足：
则其他方向末端速度均能达到要求速度大小K。式中，σr为雅各比矩阵最小奇异值，vr为σr对应的特征向量；vr′为vr方向上的一个向量，vr′的每一分量都小于关节角速度极限，且vr′的某一分量取到关节角速度极限；K为任务要求的最低末端速度，需要留出一定余量。在各关节角速度极限近似相等，某几个奇异值远小于其他奇异值σj＜＜σi时，则对于较小的几个奇异值，分别计算σjvj′ Tvj，取其中的最小值σjmvjm′ Tvjm＞K，即保证即可。
步骤102，考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，提出一种考虑机械臂操作代价的梯度势场离线构建方法。
由上述讨论可建立考虑最小奇异值及关节速度极限的势场表达式E1，为：
式中，α1和β1为任意正数，用于调节势场幅值与作用范围；K为任意正数，表示任务要求末端速度大小；K′为步骤101中σjvj′ Tvj的最小值σjmvjm′ Tvjm；为机械臂所有关节中最小的速度极限；σr为机械臂雅克比矩阵最小奇异值；当远大于K时，说明σr足够大，此时E1→0；只有当接近K时，才考虑K-K′，；当K′远离K时，E1增大，产生斥力，使机械臂远离该构型位置。
谢碧云等人于2010年提出一种基于条件数约束的方向可操作度(DMCCN)，并以此为路径规划过程中的优化目标，对平面3R机械臂进行仿真实验。该指标当条件数较小时，指标主要受方向可操作度的影响；条件数不断增大时，方向可操作度对指标的影响逐渐削弱，条件数对指标的影响不断增大。DMCCN的定义为：
式中，为条件数，定义为雅各比矩阵J(q)最大奇异值与最小奇异值的比值；κmax为任务要求的最大条件数；u为沿末端运动速度方向的单位矢量；定义为机械臂的方向可操作度；α为一正数，用于调节条件数及方向可操作度的灵敏度。
可以将出现较大条件数的情况分为两种，σmin较小的情况在依最小奇异值建立势场时已经加以考虑；σmax较大对机械臂末端运动没有影响。并且，对于可操作度ω较小的情况在之前最小奇异值建立势场时也加以考虑，故此时只考虑可操作度较大的情况。所以在考虑机械臂末端各方向运动能力时，期望机械臂朝向条件数较小且可操作度较大的构型移动。由于势场与任务无关，与DMCCN不同，更多的考虑可操作度的各向同性，表达式如下：
考虑可操作度建立的势场表达式E2如下：
其中ω为机械臂可操作度，κ为条件数；α2和β2为任意正数，α2用于调整势函数的幅值，β2用于调整引力势场E2的作用范围；
冗余度空间机械臂在关节极限范围内，其末端执行器能够到达工作空间中的任意目标位置，机械臂关节极限可以表示为：
qimin≤qi≤qimax i＝1，2，..，n
其中，qimin为第i个关节的关节角下限，qimax为第i个关节的关节角上限，n为机械臂自由度数。
由以上分析可知，希望使机械臂在运行过程中尽量远离关节角上下限，且当机械臂远离关节限位时，对机械臂的运行无影响。
由关节极限建立的势场为E3：.
其中，α为任意正数；n为机械臂自由度数；qi为第i个关节的关节角，qimin和qimax分别为第i个关节角的最小值和最大值；由上式得知，在当前关节位置距关节角下限较近时，dimin值较大，dimax值较小，势场主要由式中的第一项决定；若当前关节位置距关节角上下限都较远时，dimin和dimax的值都较小，势场趋近于0。
将以上三个势场合并，共同作用于随机树的拓展，以实现对优化目标的优化。
将所述三个势场合并，所述合势场定义E(q)为：
E(q)＝E1+E2+E3
计算得出势场的梯度表达式：
通过上式的离散形式，能够求出n个梯度势场，其中关于q1的所述梯度势场表达式为：
至均可通过类似表达式求得。
步骤103，使用构建的梯度势场引导RRT算法中的随机树进行拓展，规划得出空间机械臂的无障碍运行路径。
算法中涉及符号表示如下：
qx和cx：分别表示某构型空间位置及其对应笛卡尔坐标系位置，下标x用于说明二者之间的对应关系；
T：为一系列节点node的集合；随机树T包含以下几个属性：
(1)根节点noderoot；
(2)构型节点集NODE：包含一系列节点node的集合，按其rank由高到低排列；
node：构成随机树的基本单元，每个节点都包含以下几个属性：
(1)node.q：节点node代表的构型位置；
(2)node.rank：节点node的rank值，rank值越大说明构型node.q处势场值较小且距离障碍物较远，rank的计算方法如下：
其中，d1为机械臂末端在当前构型下距目标位置的距离；d2为机械臂当前构型下距离障碍物的距离；dsafe为机械臂距离障碍物的安全距离；由上式可知，只有当机械臂距离障碍物距离小于安全距离时才对其进行考虑，并且dsafe随实际任务的不同可以进行调整；
(3)node.fcount：用于记录随机树节点node拓展失败的次数；
(4)node.parent：构型node的父节点，随机树T的根节点noderoot的父节点为；
(5)node.child：构型node的子节点：
Qset：Qset用于记录进行过碰撞检测的构型位置及其距离障碍物的距离，用于估计新采样构型距障碍物的距离；
d3：随机树的最小拓展步长；
算法中使用的函数如下：
T.init(nodeinit)：初始化随机树T；
T.add(q1，node2)：向随机树T中加入以q1为构型的节点，node2为该节点的父节点；
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次，从中选出势场值最小的构型qh；传统的随机性拓展只从随机树T中选取选择距离qh最近的节点进行拓展，在此基础上以一定概率选取rank值较高的节点，目的在于在目标优化的同时提升算法收敛的能力；将选出的节点作为noderand，以noderand.q→qh方向将随机树进行拓展，并且不考虑新添加节点距目标构型的距离，这样做能够充分保留算法的“随机性”；
Case2的伪代码如下所示：
此外，本算法还引入基于概率的碰撞检测策略与该算法进行结合；原策略通过记录每一个节点的构型的哈希值及碰撞状态，当得到足够的样本后，以KNN算法估计新构型的碰撞状态；
由以上分析可以综合得出改进的RRT算法的整体流程，如下所示：
核心函数伪代码及说明：
该函数中init(QSet)用于初始化构型节点及其距障碍物距离，用于之后的碰撞检测策略；该函数主要用于随机生成哈希表参数。
用于判断节点node的fcount是否高于阈值，若高于阈值，则从树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-1 ai-1 di θi
1 0 0 220 -π/2
2 -π/2 0 -110 -π/2
3 -π/2 0 245 0
4 π 135 90 0
5 0 130 -90 -π/2
6 -π/2 0 300 0
7 π/2 0 0 0
8 -π/2 0 85 0
机械臂关节角变化范围：
八自由度机械臂仿真模型如图6所示。
其中机械臂初始位姿：
目标位置为：cgoal＝[150 300 600]
由所述算法得出的无障碍路径示意图如图7所示。
由图中能够看出，该算法能够规划出一条从初始构型到目标位置的无障碍路径，并且机械臂在运行过程中与障碍物始终保持一定的安全距离，说明该算法的有效性。
所述算法的优化性能通过三个指标对优化性能进行比较：
Obj1表示产生末端速度最弱方向的能产生的最高速度的能力；该值越小，说明该方向产生末端速度的能力较强；式中Num为该路径上的节点个数，Obj1为各个节点上(K-K′)的平均；
Obj2表示机械臂各个节点上可操作度的平均；该值越大，说明机械臂在各个方向上的运动能力整体较好，产生同样的末端速度所需的关节角速度较小，在一定程度上能够降低机械臂的能耗；
Obj3表示机械臂各个节点上条件数的平均；该值同样用来衡量机械臂各方向上的运动能力，可以看作是对Obj2的补充。
表二为原算法规划路径得出的指标，如下所示：
表二
组别 第1组 第2组 第3组 第4组 第5组 第6组 第7组 第8组
Obj1 26.33 43.06 30.93 31.64 16.76 38.58 15.01 19.88
Obj2 4.45 3.13 4.03 3.89 4.24 3.88 4.32 5.18
Obj3 15.37 15.93 11.51 11.97 9.38 12.66 10.08 10.59
组别 第9组 第10组 第11组 第12组 第13组 第14组 第15组 平均值
Obj1 46.21 13.24 20.94 24.42 24.79 28.95 9.44 26.01
Obj2 3.13 5.65 4.23 4.07 4.40 4.12 5.76 4.30
Obj3 17.28 7.96 9.43 7.70 10.18 10.5 8.16 11.25
表三为所述算法规划路径得出优化后的指标，如下所示：
表三
组别 第1组 第2组 第3组 第4组 第5组 第6组 第7组 第8组
Obj1 16.07 24.33 -3.82 8.66 3.29 17.47 20.96 16.65
Obj2 5.15 5.08 5.79 5.80 6.01 5.51 4.71 4.96
Obj3 8.87 10.66 6.90 8.41 6.21 8.99 10.05 13.39
组别 第9组 第10组 第11组 第12组 第13组 第14组 第15组 平均值
Obj1 6.62 14.42 21.93 13.15 4.53 14.42 9.79 12.56
Obj2 5.94 4.58 4.56 4.98 5.54 5.00 4.90 5.23
Obj3 7.38 9.50 11.89 8.96 6.30 9.30 11.48 9.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(qidist)
qidist＝min(min_dist(qi，qsub)，min_dist(qi，qsup))
qsub与qsup分别为关节角的下极限与上极限，均为n维向量；min_dist(q，qsub)用来计算q与qsub每一个维度上的距离，并取其中的最小值；式中i为整个路径关节序列的组数，M则用于取qidist中的最小值；
表四为原算法规划路径得出的与关节极限的距离，如下所示：
表四
组别 第1组 第2组 第3组 第4组 第5组 第6组 第7组 第8组
M 0.6529 0.0964 0.0043 0.0533 0.8452 0.1746 0.0092 0.3971
组别 第9组 第10组 第11组 第12组 第13组 第14组 第15组 平均值
M 0.0691 0.7898 0.0129 0.0022 0.2210 0.0188 0.7660 0.2742
表五为所述算法规划路径得出的与关节极限的距离，如下所示：
表五
组别 第1组 第2组 第3组 第4组 第5组 第6组 第7组 第8组
M 0.7628 0.7369 0.5756 0.8880 0.7866 0.6550 1.2790 0.6994
组别 第9组 第10组 第11组 第12组 第13组 第14组 第15组 平均值
M 0.0816 0.2559 0.0699 0.0412 0.1265 0.3772 0.3410 0.5118
若取关节角距离极限3度为标准，换算成弧度值为0.052，则由所述算法规划得出的路径在此安全范围外的只有1组；由原方法得出的路径在范围外有5组；取关节角极限5度为标准，弧度值为0.087，则所述方法有3组在安全范围外，原方法有7组；并且，由所述方法规划得出的路径关节极限最小值为0.0412，为2～3度之间，原方法得出的最小值为0.0022，该值小于1度。由以上分析得出，所述方法在避关节极限方面同样具有明显的效果。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

Claims (4)

1.一种避障任务无关人工势场引导的避障路径规划方法，其特征在于，所述方法步骤包括：
(1)考虑空间机械臂使用代价，选用机械臂能耗、末端速度、摩擦磨损作为优化目标，在此基础上提出一种引入速度极限的最小奇异值指标；
(2)考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，提出一种考虑机械臂操作代价的梯度势场离线构建方法；
(3)使用构建的梯度势场引导RRT算法中的随机树进行拓展，规划得出空间机械臂的无障碍运行路径。
2.根据权利要求1所述的方法，其特征在于，考虑空间机械臂使用代价，选用机械臂能耗、末端速度、摩擦磨损作为优化目标，在此基础上提出一种引入速度极限的最小奇异值指标，包括：考虑空间机械臂使用代价，选用机械臂能耗、末端速度、摩擦磨损作为优化目标，在此基础上提出的引入速度极限的最小奇异值指标为：
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mi>max</mi> </mtd> <mtd> <mrow> <msub> <mi>&amp;sigma;</mi> <mrow> <mi>j</mi> <mi>m</mi> </mrow> </msub> <msup> <msub> <mi>v</mi> <mrow> <msup> <mi>jm</mi> <mo>&amp;prime;</mo> </msup> </mrow> </msub> <mi>T</mi> </msup> <msub> <mi>v</mi> <mrow> <mi>j</mi> <mi>m</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>s</mi> <mo>.</mo> <mi>t</mi> <mo>.</mo> </mrow> </mtd> <mtd> <mrow></mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;sigma;</mi> <mi>j</mi> </msub> <mo>&lt;</mo> <msqrt> <mi>n</mi> </msqrt> <msub> <mi>&amp;sigma;</mi> <mi>k</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mi>j</mi> <mo>=</mo> <mi>n</mi> <mo>,</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mn>..</mn> <mo>,</mo> <mi>t</mi> <mi> </mi> <mi>k</mi> <mo>=</mo> <mi>t</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>t</mi> <mo>-</mo> <mn>2</mn> <mo>,</mo> <mn>..</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mn>1</mn> </mrow> </mtd> </mtr> </mtable> </mfenced>
其中，σ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，计算σjvj' Tvj；其中vj为将雅克比矩阵奇异值分解后，J(q)＝UΣVT，矩阵V中对应σj的第j个列向量，vj'为vj方向上的一个向量，vj'的每一分量都小于关节角速度极限，且vj'的某一分量等于关节角速度极限；σjmvjm' Tvjm为所有σjvj' Tvj中的最小值，j＝n,n-1,..,t。
3.根据权利要求2所述的方法，其特征在于，考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，提出一种考虑机械臂操作代价的梯度势场离线构建方法，具体说明如下：考虑所述引入速度极限的雅各比矩阵最小奇异值、机械臂可操作度、条件数、关节极限，离线构建一种考虑机械臂操作代价的梯度势场，包括：由所述引入速度极限的雅各比矩阵最小奇异值，建立考虑最小奇异值及关节速度极限的势场表达式E1，为：
<mrow> <msub> <mi>E</mi> <mn>1</mn> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>1</mn> <mo>+</mo> <msub> <mi>&amp;alpha;</mi> <mn>1</mn> </msub> <msup> <mi>e</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;sigma;</mi> <mi>r</mi> </msub> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>m</mi> <mi>a</mi> <mi>x</mi> </mrow> </msub> <mo>-</mo> <mi>K</mi> <mo>)</mo> </mrow> </msup> </mrow> </mfrac> <msup> <mi>e</mi> <mrow> <msub> <mi>&amp;beta;</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>K</mi> <mo>-</mo> <msup> <mi>K</mi> <mo>&amp;prime;</mo> </msup> <mo>)</mo> </mrow> </mrow> </msup> </mrow>
式中，α1和β1为任意正数，用于调节势场幅值与作用范围；K为任意正数，表示任务要求末端速度大小；K'为σjvj' Tvj的最小值σjmvjm' Tvjm；为机械臂所有关节中最小的速度极限；σr为机械臂雅克比矩阵最小奇异值；
考虑可操作度建立的势场表达式E2如下：
<mrow> <msub> <mi>E</mi> <mn>2</mn> </msub> <mo>=</mo> <mo>-</mo> <msub> <mi>&amp;alpha;</mi> <mn>2</mn> </msub> <msup> <mi>&amp;omega;e</mi> <mrow> <mo>-</mo> <mfrac> <msup> <mrow> <mo>(</mo> <mi>&amp;kappa;</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mn>2</mn> </msup> <msub> <mi>&amp;beta;</mi> <mn>2</mn> </msub> </mfrac> </mrow> </msup> <mo>,</mo> <mrow> <mo>(</mo> <mi>&amp;kappa;</mi> <mo>&gt;</mo> <mn>1</mn> <mo>,</mo> <mi>&amp;omega;</mi> <mo>&gt;</mo> <mn>0</mn> <mo>)</mo> </mrow> </mrow>
其中ω为机械臂可操作度，κ为条件数；α2和β2为任意正数，α2用于调整势函数的幅值，β2用于调整引力势场E2的作用范围；
由关节极限建立的势场为E3：.
<mrow> <msub> <mi>E</mi> <mn>3</mn> </msub> <mo>=</mo> <msub> <mi>&amp;alpha;</mi> <mn>3</mn> </msub> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mrow> <mo>(</mo> <mfrac> <msub> <mi>d</mi> <mrow> <mi>i</mi> <mi>min</mi> </mrow> </msub> <mrow> <mn>1</mn> <mo>+</mo> <msup> <mi>e</mi> <mrow> <mo>-</mo> <msub> <mi>d</mi> <mrow> <mi>i</mi> <mi>min</mi> </mrow> </msub> </mrow> </msup> </mrow> </mfrac> <mo>+</mo> <mfrac> <msub> <mi>d</mi> <mrow> <mi>i</mi> <mi>m</mi> <mi>a</mi> <mi>x</mi> </mrow> </msub> <mrow> <mn>1</mn> <mo>+</mo> <msup> <mi>e</mi> <mrow> <mo>-</mo> <msub> <mi>d</mi> <mrow> <mi>i</mi> <mi>m</mi> <mi>a</mi> <mi>x</mi> </mrow> </msub> </mrow> </msup> </mrow> </mfrac> <mo>)</mo> </mrow> </mrow>
其中，α为任意正数；n为机械臂自由度数；qi为第i个关节的关节角，qimin和qimax分别为第i个关节角的最小值和最大值；
将所述三个势场合并，所述合势场定义E(q)为：
E(q)＝E1+E2+E3
计算得出势场的梯度表达式：
<mrow> <mfrac> <mrow> <mo>&amp;part;</mo> <mi>E</mi> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> </mrow> </mfrac> <mo>=</mo> <munder> <mi>lim</mi> <mrow> <msub> <mi>&amp;Delta;q</mi> <mn>1</mn> </msub> <mo>&amp;RightArrow;</mo> <mn>0</mn> </mrow> </munder> <mfrac> <mrow> <mi>E</mi> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>&amp;Delta;q</mi> <mn>1</mn> </msub> <mo>,</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msub> <mi>q</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mi>E</mi> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>,</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msub> <mi>q</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>&amp;Delta;q</mi> <mn>1</mn> </msub> </mrow> </mfrac> </mrow>
通过上式的离散形式，能够求出n个梯度势场，其中关于q1的所述梯度势场表达式为：
<mrow> <mfrac> <mrow> <mo>&amp;part;</mo> <mi>E</mi> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> </mrow> </mfrac> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>E</mi> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mn>1</mn> </msub> <mo>+</mo> <mn>1</mn> <mo>,</mo> <msub> <mi>N</mi> <mn>2</mn> </msub> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msub> <mi>N</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mi>E</mi> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mn>1</mn> </msub> <mo>,</mo> <msub> <mi>N</mi> <mn>2</mn> </msub> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msub> <mi>N</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>F</mi> <mi>o</mi> <mi>r</mi> <mi> </mi> <mi>a</mi> <mi>l</mi> <mi>l</mi> <mi> </mi> <msub> <mi>N</mi> <mn>2</mn> </msub> <mo>,</mo> <mn>..</mn> <mo>,</mo> <msub> <mi>N</mi> <mi>n</mi> </msub> <mo>,</mo> <mi>f</mi> <mi>o</mi> <mi>r</mi> <mi> </mi> <msub> <mi>N</mi> <mn>1</mn> </msub> <mo>=</mo> <mn>1</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>E</mi> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mn>1</mn> </msub> <mo>,</mo> <msub> <mi>N</mi> <mn>2</mn> </msub> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msub> <mi>N</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mi>E</mi> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>1</mn> <mo>,</mo> <msub> <mi>N</mi> <mn>2</mn> </msub> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msub> <mi>N</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>F</mi> <mi>o</mi> <mi>r</mi> <mi> </mi> <mi>a</mi> <mi>l</mi> <mi>l</mi> <mi> </mi> <msub> <mi>N</mi> <mn>2</mn> </msub> <mo>,</mo> <mn>..</mn> <mo>,</mo> <msub> <mi>N</mi> <mi>n</mi> </msub> <mo>,</mo> <mi>f</mi> <mi>o</mi> <mi>r</mi> <mi> </mi> <msub> <mi>N</mi> <mn>1</mn> </msub> <mo>=</mo> <msub> <mi>N</mi> <mrow> <mn>1</mn> <mi>m</mi> <mi>a</mi> <mi>x</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mrow> <mo>(</mo> <mi>E</mi> <mo>(</mo> <mrow> <msub> <mi>N</mi> <mn>1</mn> </msub> <mo>+</mo> <mn>1</mn> <mo>,</mo> <msub> <mi>N</mi> <mn>2</mn> </msub> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msub> <mi>N</mi> <mi>n</mi> </msub> </mrow> <mo>)</mo> <mo>-</mo> <mi>E</mi> <mo>(</mo> <mrow> <msub> <mi>N</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>1</mn> <mo>,</mo> <msub> <mi>N</mi> <mn>2</mn> </msub> <mo>,</mo> <mo>...</mo> <mo>,</mo> <msub> <mi>N</mi> <mi>n</mi> </msub> </mrow> <mo>)</mo> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>O</mi> <mi>t</mi> <mi>h</mi> <mi>e</mi> <mi>r</mi> <mi>w</mi> <mi>i</mi> <mi>s</mi> <mi>e</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
至均可通过类似表达式求得。
4.根据权利要求1所述的方法，其特征在于，使用构建的梯度势场引导RRT算法中的随机树进行拓展，规划得出空间机械臂的运行路径，包括：将所述离线建立的梯度势场融入RRT算法，在每一次拓展时使随机树有概率朝向梯度下降的方向拓展；
所述一种考虑使用代价优化的避障任务无关人工势场引导的RRT改进算法，算法中涉及符号表示如下：
qx和cx：分别表示某构型空间位置及其对应笛卡尔坐标系位置，下标x用于说明二者之间的对应关系；
T：为一系列节点node的集合；随机树T包含以下几个属性：
(1)根节点noderoot；
(2)构型节点集NODE：包含一系列节点node的集合，按其rank由高到低排列；
node：构成随机树的基本单元，每个节点都包含以下几个属性：
(1)node.q：节点node代表的构型位置；
(2)node.rank：节点node的rank值，rank值越大说明构型node.q处势场值较小且距离障碍物较远，rank的计算方法如下：
<mrow> <mi>n</mi> <mi>o</mi> <mi>d</mi> <mi>e</mi> <mo>.</mo> <mi>r</mi> <mi>a</mi> <mi>n</mi> <mi>k</mi> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mfrac> <msub> <mi>d</mi> <mn>2</mn> </msub> <msub> <mi>d</mi> <mn>1</mn> </msub> </mfrac> </mtd> <mtd> <mrow> <msub> <mi>d</mi> <mn>2</mn> </msub> <mo>&lt;</mo> <msub> <mi>d</mi> <mrow> <mi>s</mi> <mi>a</mi> <mi>f</mi> <mi>e</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mfrac> <msub> <mi>d</mi> <mrow> <mi>s</mi> <mi>a</mi> <mi>f</mi> <mi>e</mi> </mrow> </msub> <msub> <mi>d</mi> <mn>1</mn> </msub> </mfrac> </mtd> <mtd> <mrow> <msub> <mi>d</mi> <mn>2</mn> </msub> <mo>&amp;GreaterEqual;</mo> <msub> <mi>d</mi> <mrow> <mi>s</mi> <mi>a</mi> <mi>f</mi> <mi>e</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> 2
其中，d1为机械臂末端在当前构型下距目标位置的距离；d2为机械臂当前构型下距离障碍物的距离；dsafe为具体避障规划任务限定的机械臂距离障碍物的安全距离；
(3)node.fcount：用于记录随机树节点node拓展失败的次数；
(4)node.parent：构型node的父节点，随机树T的根节点noderoot的父节点为
(5)node.child：构型node的子节点；
Qset：Qset用于记录进行过碰撞检测的构型位置及其距离障碍物的距离，用于估计新采样构型距障碍物的距离；
d3：随机树的最小拓展步长；
算法中使用的函数如下：
T.init(nodeinit)：初始化随机树T；
T.add(q1,node2)：向随机树T中加入以q1为构型的节点，node2为该节点的父节点；
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改进算法得出的路径；所述路径能够使空间机械臂在完成避障路径规划问题并保证路径规划效率的前提下降低机械臂使用代价，提升运行过程中与局部路径规划算法结合的能力。
CN201710558115.6A 2017-07-10 2017-07-10 一种避障任务无关人工势场引导的避障路径规划方法 Active CN107234617B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710558115.6A CN107234617B (zh) 2017-07-10 2017-07-10 一种避障任务无关人工势场引导的避障路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710558115.6A CN107234617B (zh) 2017-07-10 2017-07-10 一种避障任务无关人工势场引导的避障路径规划方法

Publications (2)

Publication Number Publication Date
CN107234617A true CN107234617A (zh) 2017-10-10
CN107234617B CN107234617B (zh) 2020-04-28

Family

ID=59991016

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710558115.6A Active CN107234617B (zh) 2017-07-10 2017-07-10 一种避障任务无关人工势场引导的避障路径规划方法

Country Status (1)

Country Link
CN (1) CN107234617B (zh)

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107943053A (zh) * 2017-12-15 2018-04-20 陕西理工大学 一种移动机器人的路径规划方法
CN108326849A (zh) * 2018-01-04 2018-07-27 浙江大学 一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法
CN108555904A (zh) * 2018-01-10 2018-09-21 苏州工业职业技术学院 一种表面改性机器人操作性能的优化方法
CN108579085A (zh) * 2018-03-12 2018-09-28 腾讯科技（深圳）有限公司 障碍物碰撞的处理方法和装置、存储介质及电子装置
CN108608427A (zh) * 2018-06-05 2018-10-02 中科新松有限公司 机器人力控牵引过程中的避奇异方法及装置
CN108621165A (zh) * 2018-05-28 2018-10-09 兰州理工大学 障碍环境下的工业机器人动力学性能最优轨迹规划方法
CN109877836A (zh) * 2019-03-13 2019-06-14 浙江大华技术股份有限公司 路径规划方法、装置、机械臂控制器和可读存储介质
CN109955250A (zh) * 2019-01-21 2019-07-02 中国船舶重工集团公司第七一六研究所 应用于工业机器人的追踪与实时避障反应规划算法
EP3572193A1 (en) * 2018-05-23 2019-11-27 General Electric Company System and method for controlling a robotic arm
CN110977974A (zh) * 2019-12-11 2020-04-10 遨博（北京）智能科技有限公司 一种机器人规避奇异位型的导纳控制方法、装置及系统
CN112045664A (zh) * 2019-06-06 2020-12-08 南京理工大学 一种基于ros系统的通用机械臂控制器
CN112327856A (zh) * 2020-11-13 2021-02-05 云南电网有限责任公司保山供电局 一种基于改进A-star算法的机器人路径规划方法
CN112828891A (zh) * 2021-01-20 2021-05-25 深圳辰视智能科技有限公司 机器人路径规划有效性的检测方法、检测装置及计算机可读存储介质
CN113263496A (zh) * 2021-04-01 2021-08-17 北京无线电测量研究所 一种六自由度机械臂优化路径的方法和计算机设备
CN113352319A (zh) * 2021-05-08 2021-09-07 上海工程技术大学 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法
CN113601512A (zh) * 2021-08-23 2021-11-05 太原理工大学 一种机械臂奇异点的通用规避方法与系统
CN113733094A (zh) * 2021-09-14 2021-12-03 北京邮电大学 一种高欠驱动空间机械臂可控程度表征方法
CN113985875A (zh) * 2021-10-27 2022-01-28 福州大学 基于碰撞预测模型的人工势场无人车动态路径规划方法
CN115648220A (zh) * 2022-11-15 2023-01-31 华侨大学 基于最小代价下降的机械臂关节空间避障路径规划方法
WO2023024278A1 (zh) * 2021-08-24 2023-03-02 深圳市优必选科技股份有限公司 机器人关节位姿优化方法、机器人控制方法和机器人

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103009389A (zh) * 2012-11-30 2013-04-03 北京控制工程研究所 一种冗余空间机械臂在轨抓捕的轨迹规划方法
CN106426164A (zh) * 2016-09-27 2017-02-22 华南理工大学 一种冗余度双机械臂的多指标协调运动规划方法
EP3171133A1 (en) * 2015-11-19 2017-05-24 Sikorsky Aircraft Corporation Kinematic motion planning with regional planning constraints
CN106695802A (zh) * 2017-03-19 2017-05-24 北京工业大学 基于多自由度机械臂的改进式rrt*的避障运动规划方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103009389A (zh) * 2012-11-30 2013-04-03 北京控制工程研究所 一种冗余空间机械臂在轨抓捕的轨迹规划方法
EP3171133A1 (en) * 2015-11-19 2017-05-24 Sikorsky Aircraft Corporation Kinematic motion planning with regional planning constraints
CN106426164A (zh) * 2016-09-27 2017-02-22 华南理工大学 一种冗余度双机械臂的多指标协调运动规划方法
CN106695802A (zh) * 2017-03-19 2017-05-24 北京工业大学 基于多自由度机械臂的改进式rrt*的避障运动规划方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
XIN GAO ET AL.: "Research on the operational reliability system control methodology and its application for aerospace mechanism", 《INTERNATIONAL JOURNAL OF ADVANCED ROBOTIC SYSTEMS》 *

Cited By (32)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107943053A (zh) * 2017-12-15 2018-04-20 陕西理工大学 一种移动机器人的路径规划方法
CN108326849B (zh) * 2018-01-04 2019-08-30 浙江大学 一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法
CN108326849A (zh) * 2018-01-04 2018-07-27 浙江大学 一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法
CN108555904B (zh) * 2018-01-10 2020-12-01 苏州工业职业技术学院 一种表面改性机器人操作性能的优化方法
CN108555904A (zh) * 2018-01-10 2018-09-21 苏州工业职业技术学院 一种表面改性机器人操作性能的优化方法
CN108579085A (zh) * 2018-03-12 2018-09-28 腾讯科技（深圳）有限公司 障碍物碰撞的处理方法和装置、存储介质及电子装置
CN108579085B (zh) * 2018-03-12 2020-05-12 腾讯科技（深圳）有限公司 障碍物碰撞的处理方法和装置、存储介质及电子装置
EP3572193A1 (en) * 2018-05-23 2019-11-27 General Electric Company System and method for controlling a robotic arm
US11084169B2 (en) 2018-05-23 2021-08-10 General Electric Company System and method for controlling a robotic arm
CN108621165A (zh) * 2018-05-28 2018-10-09 兰州理工大学 障碍环境下的工业机器人动力学性能最优轨迹规划方法
CN108621165B (zh) * 2018-05-28 2021-06-15 兰州理工大学 障碍环境下的工业机器人动力学性能最优轨迹规划方法
CN108608427A (zh) * 2018-06-05 2018-10-02 中科新松有限公司 机器人力控牵引过程中的避奇异方法及装置
CN109955250A (zh) * 2019-01-21 2019-07-02 中国船舶重工集团公司第七一六研究所 应用于工业机器人的追踪与实时避障反应规划算法
CN109877836A (zh) * 2019-03-13 2019-06-14 浙江大华技术股份有限公司 路径规划方法、装置、机械臂控制器和可读存储介质
CN109877836B (zh) * 2019-03-13 2021-06-08 浙江大华技术股份有限公司 路径规划方法、装置、机械臂控制器和可读存储介质
CN112045664A (zh) * 2019-06-06 2020-12-08 南京理工大学 一种基于ros系统的通用机械臂控制器
CN110977974B (zh) * 2019-12-11 2021-06-01 遨博（北京）智能科技有限公司 一种机器人规避奇异位型的导纳控制方法、装置及系统
CN110977974A (zh) * 2019-12-11 2020-04-10 遨博（北京）智能科技有限公司 一种机器人规避奇异位型的导纳控制方法、装置及系统
CN112327856A (zh) * 2020-11-13 2021-02-05 云南电网有限责任公司保山供电局 一种基于改进A-star算法的机器人路径规划方法
CN112327856B (zh) * 2020-11-13 2022-12-06 云南电网有限责任公司保山供电局 一种基于改进A-star算法的机器人路径规划方法
CN112828891A (zh) * 2021-01-20 2021-05-25 深圳辰视智能科技有限公司 机器人路径规划有效性的检测方法、检测装置及计算机可读存储介质
CN113263496A (zh) * 2021-04-01 2021-08-17 北京无线电测量研究所 一种六自由度机械臂优化路径的方法和计算机设备
CN113352319A (zh) * 2021-05-08 2021-09-07 上海工程技术大学 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法
CN113352319B (zh) * 2021-05-08 2022-10-21 上海工程技术大学 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法
CN113601512B (zh) * 2021-08-23 2022-12-02 太原理工大学 一种机械臂奇异点的通用规避方法与系统
CN113601512A (zh) * 2021-08-23 2021-11-05 太原理工大学 一种机械臂奇异点的通用规避方法与系统
WO2023024278A1 (zh) * 2021-08-24 2023-03-02 深圳市优必选科技股份有限公司 机器人关节位姿优化方法、机器人控制方法和机器人
CN113733094A (zh) * 2021-09-14 2021-12-03 北京邮电大学 一种高欠驱动空间机械臂可控程度表征方法
CN113733094B (zh) * 2021-09-14 2022-10-25 北京邮电大学 一种高欠驱动空间机械臂可控程度表征方法
CN113985875A (zh) * 2021-10-27 2022-01-28 福州大学 基于碰撞预测模型的人工势场无人车动态路径规划方法
CN113985875B (zh) * 2021-10-27 2024-02-27 福州大学 基于碰撞预测模型的人工势场无人车动态路径规划方法
CN115648220A (zh) * 2022-11-15 2023-01-31 华侨大学 基于最小代价下降的机械臂关节空间避障路径规划方法

Also Published As

Publication number Publication date
CN107234617B (zh) 2020-04-28

Similar Documents

Publication Publication Date Title
CN107234617A (zh) 一种避障任务无关人工势场引导的避障路径规划方法
Mainprice et al. Human-robot collaborative manipulation planning using early prediction of human motion
US20190361452A1 (en) Method and system for controlling a vehicle
CN108803321A (zh) 基于深度强化学习的自主水下航行器轨迹跟踪控制方法
CN108444489A (zh) 一种改进rrt算法的路径规划方法
Esposito et al. Adaptive RRTs for validating hybrid robotic control systems
Wen et al. A multi-robot path-planning algorithm for autonomous navigation using meta-reinforcement learning based on transfer learning
CN106055522A (zh) 冗余空间机械臂最小基座姿态扰动的轨迹规划方法
CN101770235A (zh) 路径计划装置、路径计划方法和计算机程序
CN114397896B (zh) 一种改进粒子群算法的动态路径规划方法
Ding et al. Hierarchical reinforcement learning framework towards multi-agent navigation
CN113821045B (zh) 一种腿足机器人强化学习动作生成系统
CN107685343A (zh) 一种机械臂运动学参数标定构型优化方法
Zarubin et al. Hierarchical Motion Planning in Topological Representations.
Okuyama et al. Minimum-time trajectory planning for a differential drive mobile robot considering non-slipping constraints
Branicky et al. Nonlinear and hybrid control via RRTs
Sharma et al. Model based path planning using Q-Learning
Imtiaz et al. Comparison of Two Reinforcement Learning Algorithms for Robotic Pick and Place with Non-Visual Sensing
Gayle et al. Efficient motion planning of highly articulated chains using physics-based sampling
Li et al. Human-aware robot navigation via reinforcement learning with hindsight experience replay and curriculum learning
Son et al. Bio-insect and artificial robot interaction using cooperative reinforcement learning
Xu et al. Reinforcement learning with construction robots: A preliminary review of research areas, challenges and opportunities
Qin et al. A path planning algorithm based on deep reinforcement learning for mobile robots in unknown environment
Chang et al. An implementation of reinforcement learning in assembly path planning based on 3D point clouds
CN117606490B (zh) 一种水下自主航行器协同搜索路径规划方法

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
GR01 Patent grant
GR01 Patent grant