一种带负载六自由度空间机械臂的路径搜索方法与流程

本发明涉及带负载六自由度空间机械臂的路径搜索算法，用于实现满足动态负载能力要求的机械臂路径搜索。

背景技术：

随着科技水平的不断进步，人类对生产生活的各个领域的探索与研究也越发深入，可以代替人类完成许多重要任务的机械臂也逐渐走入了人们的视野。尤其是在航空航天领域，一些复杂、繁重、危险的操作必须要使用空间机械臂才能够完成；而要想空间机械臂能够顺利完成任务，就必须对其进行合理的路径搜索。

机械臂的路径搜索方法有很多。对于带负载的机械臂而言，机械臂的运动轨迹在很大程度上受限于其末端携带负载的质量和惯量对其关节力矩的影响；一旦关节力矩超出机械臂所能承受的最大限度，就有可能导致机械臂关节受损；这不仅会影响空间任务的按时完成，还可能进一步造成更大的损失。因此，搜索一条满足机械臂动态负载能力要求的任务路径，对于机械臂安全可靠地完成负载操作任务具有十分重要的现实意义。

技术实现要素：

有鉴于此，本发明实施例提供了一种带负载六自由度空间机械臂的路径搜索方法，所提方法可为六自由度空间机械臂进行动态负载操作提供了保障。

上述带负载六自由度空间机械臂的路径搜索过程中，用到的方法至少包括：

依据所选择的合理搜索步长Δl，对空间机械臂动态负载能力工作空间M_c进行栅格化处理，并求解每个栅格的动态负载能力值。在空间机械臂的动态负载能力工作空间已知的前提下，首先，用长方体将该动态负载能力工作空间包络；其次，基于所要搜索路径的平滑程度和算法运算量，选定一个合适的搜索步长Δl；然后，基于该搜索步长将长方体栅格化成边长为Δl的小立方体的集合；最后，将每个小立方体内所包含的所有点的负载能力的平均值，作为该立方体的动态负载能力值，进而完成对动态负载能力工作空间的栅格化。

依据“改进后的A*算法”对空间机械臂进行路径搜索，以获得一条满足动态负载能力要求的可达路径。在完成了对“空间机械臂动态负载能力工作空间”栅格化的基础上，首先，基于估价函数f(p)＝g(p)+h(p)+C(S_k)，以起始点为当前节点，计算所要搜索路径的下一个节点(即为与当前节点相邻的估价函数值最小的节点)；获得下一个节点之后，再以该节点为当前节点，计算其所对应的下一个节点，以此类推，直到下一个节点为目标节点时，停止计算；依次连接所计算的节点，即可形成一条满足动态负载能力要求的“可达路径”。同时，将这一过程中计算的所有节点组成集合A，以用于对所得可达路径的进一步优化。

依据空间机械臂的工作特征，优化所得可达路径，完成带负载六自由度空间机械臂的路径搜索。在基于改进后的A*算法获得一条可达路径、以及其所对应的节点集合A之后，依据空间机械臂的工作特征(即机械臂在实际操作过程中其路径上的拐点不益太多，以避免其频繁启停)，对集合A中的节点进行“再筛选”，以获得较少的依然满足动态负载能力要求的节点，令这些节点组成集合B，依次连接集合B中的节点，可得到一条“优化了可达路径之后的”新的路径，该路径即为带负载六自由度空间机械臂的实际可行路径。

上述求解过程中，对空间机械臂动态负载能力工作空间进行栅格化的过程包括：

第一步：基于已有研究，获得空间机械臂的动态负载能力工作空间，即：首先，使机械臂末端依次处于由蒙特卡洛法所求的工作空间内的每一点；其次，考虑机械臂末端处于某一散点处的所有可能的运动状态；然后，选出在极端任务条件下，机械臂所有运动状态对应的动态负载能力最小值，作为机械臂在该点处的动态负载能力值；最后，组合工作空间内所有点处的动态负载能力值，即可获得机械臂的动态负载能力工作空间。基于此工作空间，可实现带负载空间机械臂的路径搜索。

第二步：用一个“长、宽、高”分别平行于机械臂基座坐标系x,y,z坐标轴的长方体，包络机械臂的动态负载能力工作空间，以使该动态负载能力工作空间中的所有点都能够被该长方体所包含。

第三步：选择合理的搜索步长Δl，将所得长方体栅格化成边长为Δl的小立方体的集合。关于搜索步长Δl的选择，由于当Δl偏大时，基于平均法所计算的每个栅格的动态负载能力值与实际情况会有较大偏差，进而就会导致所搜索路径的可靠性降低；而当Δl偏小时，基于A*算法进行路径搜索时所需的运算量就会明显增大，且所搜索的路径拐点较多也会导致机械臂在实际操作过程中频繁启停；因此，在实际的路径搜索中，需根据实际情况来选择搜索步长；确定搜索步长之后，进一步即可将上述包络动态负载能力工作空间的长方体栅格化成边长为Δl的小正方体的集合。

第四步：计算栅格化后每个小立方体的动态负载能力值。用n来表示某一小立方体内所包含的工作空间点的个数，用来表示每个工作空间点的动态负载能力值，用这入小立方体内工作空间点的动态负载能力值平均值来表示该小立方体的负载能力，则有栅格化后的第k个小立方体所对应的动态负载能力值S_k可表示为：

完成所有小立方体所对应的动态负载能力值的求解之后，即为完成了机械臂动态负载能力工作空间的栅格化。

上述计算过程中，基于“改进后的A*算法”搜索出一条满足动态负载能力要求的可达路径的过程包括：

第一步：建立改进后的A*算法的估价函数，如下式所示：

f(p)＝g(p)+h(p)+C(S_k)

其中，g(p)代表当前节点到任意节点n的代价值；h(p)代表任意节点n到目标节点的代价值；C(S_k)是一个仅跟“每个小立方体的动态负载能力S_k”相关的值，若某一小立方体的动态负载能力S_k满足机械臂实际的动态负载能力要求，则其所对应的C(S_k)取：C(S_k)＝0，否则取：C(S_k)＝+∞；f(p)代表该节点的总代价值。

在估价函数f(p)的表达式中，函数g(p)和h(p)是通过多个不同的优化目标来共同决定的，如下式所示：

g(p)＝k₁×g₁(p)+k₂×g₂(p)+...

h(p)＝k₁×h₁(p)+k₂×h₂(p)+...

其中，g₁(p)、g₂(p)...是与“当前节点到任意节点n”相对应的优化函数，h₁(p)、h₂(p)...是与“任意节点n到目标节点”相对应的优化函数，优化目标可为关节行程、末端行程和能耗等；k₁,k₂,...是与各优化函数所对应的权值；而p则代表函数的决策变量，它是由不同的变量所决定的，是一组集合。

第二步：基于改进后的A*算法，在动态负载能力工作空间中搜索出一条满足动态负载能力要求的可达路径，具体的搜索步骤如下：

(1)将起始点作为当前节点，依次计算栅格化后与“当前节点所在栅格”相邻的所有小立方体的总代价值f(p)，并取其中总代价值最小的小立方体的“体中心”，作为当前节点的下一个节点；

(2)以(1)中所求的下一个节点为当前节点，依次计算栅格化后与其相邻的所有小立方体的总代价值f(p)，并取其中总代价值最小的小立方体的“体中心”为该节点的下一个节点；

(3)依此类推，直至求得下一个节点即为目标节点时，停止计算；然后依次连接所求得的节点，即可形成一条从起始点到目标点的可达路径，完成满足动态负载能力要求的机械臂路径搜索。

第三步：将上述基于改进后的A*算法所搜索的可达路径上的各个节点依次摘取出来，共同组成一个节点集合A，如下所示：

A＝{a₁,a₂,a₃......a_n}

其中，a_i表示基于改进后的A*算法所搜索的可达路径的中间节点，n表示中间节点的个数。

该节点集合A将被用于对所得“满足动态负载能力要求的可达路径”进行进一步优化。

上述求解过程中，基于空间机械臂的工作特征优化“所得可达路径”以完成带负载六自由度空间机械臂的路径搜索的过程包括：

第一步：设起始点为a₀，连接a₀与上述所得节点集合A中的第一个节点a₁，可得线段a₀a₁，判断a₀a₁与栅格化空间中的哪些立方体相交，并将与其相交的k₁个立方体所代表的负载能力值与机械臂所携带负载m进行比较，若满足任意S_i≥m(其中1≤i≤k₁)，则连接a₀与集合A中的第二个节点a₂，得线段a₀a₂，判断a₀a₂与栅格化空间中的哪些立方体相交，并将与其相交的k₂个立方体所代表的负载能力值与机械臂所携带负载m进行比较，若满足任意S_i≥m(其中1≤i≤k₂)，则连接a₀与集合A中的第三个节点a₃，得线段a₀a₃…以此类推，直至栅格化空间中与线段a₀a_i相交的所有立方体所对应的动态负载能力值{S₁,S₂,...,S_k}不满足任意S_i≥m(其中1≤i≤k)时，则将a_i的前一个节点a_i-1作为a₀的下一个节点，记为b₁。

第二步：再以a_i-1(b₁)为起始点，重复第一步，以求得其所对应的下一个节点b₂；以此类推，当求得下一个节点为目标点时，停止运算；将整个计算过程中所获得的所有节点组成一个新的集合B，如下所示：

B＝{b₁,b₂,b₃......b_m}

其中，b_i表示被优化以后的路径的中间节点，m表示中间节点的个数。上述求解过程中所得到的节点集合A和B，应满足m≤n,的关系。

第三步：依次连接起始点、集合B中的各节点和目标点，即可得到优化后的“拐点较少且满足动态负载能力要求的”路径，进而即可完成带负载六自由度空间机械臂的路径搜索。

【附图说明】

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

图1是本发明实施例中针对带负载六自由度空间机械臂的路径搜索方法的流程示意图；

图2是本发明实施例中对“基于启发式搜索算法所获得的路径”进行优化的原理图；

图3是本发明实施例中研究对象的建系图；

图4是本发明实施例中机械臂的动态负载能力工作空间图；

图5是本发明实施例中路径搜索的结果图，包含优化前和优化后共两条路径。

【具体实施例】

根据本发明所述的路径搜索方法，以如图3所示的六自由度空间机械臂为研究对象展开验证，机械臂动态负载能力工作空间如图4所示。

在图4所示的机械臂动态负载能力工作空间中，运用改进后的A*算法，搜索可达路径并进行优化，即可得到如图5所示的路径搜索结果图；图中沿“实线”从起点到中间点K、再经“虚线”到达终点的轨迹，为基于改进后的A*算法搜索出的原始路径；整条“实线”轨迹为优化后的路径。通过验证“优化后的路径”可知，该路径符合动态负载能力要求，且相比于原始轨迹而言，其所包含的拐点数要明显减少，这可以在很大程度上减少机械臂的启停次数，使得所搜索路径更具有实际可行性。

本发明实施例的技术方案具有以下有益效果：

本发明基于动态负载能力工作空间，利用改进后的启发式搜索算法，实现了带负载六自由度空间机械臂的路径搜索。本发明提出的轨迹搜索方法能够在动态负载能力工作空间中快速准确地搜索出满足六自由度空间机械臂任务负载能力需求的轨迹，并且搜索得到的任务轨迹负载能力具有绝对的安全裕度，另外该方法能够应用于类似的轨迹搜索任务中，如避障轨迹规划等。

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