CN106166750B - 一种改进型d*机械臂动态避障路径规划方法 - Google Patents

Abstract

本发明实施例提供了一种改进型D^*机械臂避障路径规划方法，实现机械臂动态避障路径规划，包括：通过层次包围盒法将机械臂所有部件和环境中的障碍物均转化为长方体包络，并设计了相关算法实现机械臂与机械臂以及机械臂与环境之间的快速碰撞检测；根据空间机械臂特征，获得基于启发函数的路径搜索算法；将应用于七自由度机械臂D^*避障路径规划进行改进，完成七自由度机械臂动态避障路径规划。根据本发明实施例提供的技术方案，以实现七自由度机械臂基于改进型D^*动态避障路径规划的方法。

Description

一种改进型D*机械臂动态避障路径规划方法

【技术领域】

本发明涉及多自由度机械臂碰撞检测技术及动态避障路径规划技术，尤其涉及七自由度机械臂动态避障路径规划方法。

【背景技术】

在人类科学技术日新月异的发展潮流中，机器人技术以其广泛的应用范围、灵活的使用方式，以及在特种作业中的独特需求，成为了高新技术的必然发展趋势之一，也因此受到了各个国家越来越多的关注。近几年来，机器人技术在航空航天领域中的应用变得越来越重要。毫无疑问，机械臂在航空航天领域中起着至关重要的作用。机械臂工作性能的强弱、稳定性能的高低，直接影响着在轨操作的诸多方面。

避障路径规划是机械臂在运动过程中很重要的一个环节，其关系到机械臂在移动过程中的精确度和稳定性，以及响应时间。特别的，在较为复杂的环境当中，机械臂需要在确保自身安全的情况下规划出一条可行的工作路径。目前，关于机械臂避障路径规划的研究多不考虑关于实时避障路径规划问题以及环境中出现动态障碍物的情况，导致了相关方法无法普遍应用于较为复杂的场景当中。因此，研究如何实现多自由度机械臂实时避障路径规划以及考虑环境中出现动态障碍物的情况具有重要的现实意义。

【发明内容】

有鉴于此，本发明实施例提供了一种基于改进型D*机械臂动态避障路径规划方法，以实现七自由度机械臂的动态避障路径规划。

本发明实施例提供了一种方法，包括：

一种改进型D*机械臂动态避障路径规划方法，其特征在于，所述方法包括：

依据简单的长方体对机械臂及环境进行包络，基于长方体间的相交测试，完成机械臂之间以及机械臂与环境的快速碰撞检测。

依据空间机械臂特性，获得基于启发函数的路径搜索算法。

依据改进的D*算法进行机械臂动态避障路径规划。

上述方法中，所述快速碰撞检测算法至少包括：

依据简单的长方体包络，进行空间机械臂和环境的包络；

依据包络长方体特征，将其进行重叠式旋转变换，使其形成的包络区域近似为圆柱体或球体；

依据长方体间位姿关系，进行长方体之间的碰撞检测，完成快速碰撞检测过程。

上述方法中，所述启发式函数搜索路径规划算法包括：

依据空间机械臂特性，改进启发式搜索算法的启发函数；

根据空间机械臂特性引入搜索维度D、位置搜索步长Δl及姿态搜索步长Δθ。

搜索维度为在空间中机械臂末端搜索的方向，可以进行设定，位置搜索步长为机械臂末端的位置搜索长度，姿态搜索步长为机械臂末端的姿态搜索欧拉角长度，通过以机械臂末端位置和姿态为目标进行启发函数的改进，使其最后能够搜索到目标位置和姿态。

上述方法中，所述动态避障算法包括：

基于改进的D*动态搜索算法，D*算法开辟三个状态列表分别为OPEN列表、CLOSED列表和NEW列表，分别存储不同的路径代价：OPEN列表的集合为A，用于存储未经访问的节点路径代价；CLOSED列表的集合为B，用于存储已访问节点的路径代价；NEW列表的集合为C，用于存储待更新节点的路径代价。

由以上技术方案可以看出，本发明实施例具有以下有益效果：

本发明实施例的技术方案中，依据长方体相交测试思想，实现了机械臂与机械臂之间以及机械臂与环境之间的快速碰撞检测，并成功的将所有机械臂部件以及环境中的障碍物全部利用简单的长方体进行了包络。依据启发函数的路径搜索方法，对D*算法进行了改进，使其能够应用于七自由度机械臂的动态避障路径规划。本发明通过快速碰撞检测算法的实现，以及改进型D*避障路径规划算法实现，实现了七自由度机械臂动态避障路径规划方法，解决了机械臂无法完成动态路径规划的问题，很好的为机械臂操作人员提供了更灵活、更现实的操作方法。

【附图说明】

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

图1是本发明实施例所提供的改进型D*机械臂避障路径规划方法的流程示意图；

图2是本发明实施例中长方体之间快速碰撞检测的原理图；

【具体实施方式】

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

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

本发明实施例给出一种改进型D*机械臂动态避障路径规划方法，请参考图1，其为本发明实施例所提供的改进型D*机械臂动态避障路径规划方法的流程示意图，如图1所示，该方法包括以下步骤：

步骤101，依据简单的长方体对机械臂及环境进行包络，基于长方体间的相交测试，完成机械臂之间以及机械臂与环境的快速碰撞检测。

具体的，在机械臂之间以及机械臂与环境之间的快速碰撞检测过程中，对机械臂和环境进行适当的建模和简化是十分必要的，二者几何模型的复杂程度直接关系到碰撞检测算法的精度和效率。考虑机械臂和环境形状的多样性，同时为了减少运算以保证碰撞检测的效率，一般采用包围盒法来包络机械臂自身和环境中的障碍物。包围盒法的基本思想是用体积略大而形状简单的几何体来近似地代替复杂的对象。其中，简单的几何元素如长方体、圆柱体、球体等被广泛采用。

为了进一步提升碰撞检测的运行速度，以及为机械臂动态避障路径规划的实现提供可能性，本发明采用简单的长方体对机械臂的所有关节以及环境中的所有障碍物进行包络。当机械臂所包含的部件或环境中的障碍物可近似为圆柱体或球体时，采用对包络长方体进行旋转的方法实现近似。

以长方体近似构成圆柱体为例，其具体过程为，以长方体中心为原点，到各表面中心为坐标轴建立空间直角坐标系，将包络的长方体绕坐标轴按照设置角度旋转(如绕坐标轴连续旋转45度)，以旋转过程中产生的多个长方体近似代替圆柱体。同理，绕三个坐标轴旋转可以近似代替球体。当旋转角度设置的越小时，还原长方体和球体的程度越高。

然后以长方体之间的相交测试来进行机械臂与自身以及机械臂与环境之间的碰撞检测，具体步骤如下：

利用如下公式进行长方体中心距离检测，若满足则返回碰撞，否则，继续；

其中，Dist(G_C1,G_C2)为表示长方体一G_C1与长方体二G_C2之间的距离，||O_cO_c'||表示长方体G_C1中心O_c与长方体G_C2中心O_c'之间距离，s₁、s₂、s₃、s₁'、s₂'、s₃'为起点为长方体中心，终点为三个互不相对面中心的方向向量，d_S为碰撞检测的安全裕度；

第二步：判断长方体1的顶点是否在长方体2内，此即点-长方体碰撞检测。若有1个顶点碰撞则返回碰撞，否则继续判断下一个顶点，若都不碰撞则进入第三步；

第三步：判断长方体1的边是否与长方体2碰撞，此即线段-长方体碰撞检测。若有1条边碰撞则返回碰撞，否则继续判断下一条边，若都不碰撞则返回不碰撞。

具体的碰撞检测过程采用二叉树的方式进行机械臂与机械臂之间以及机械臂与环境之间的碰撞检测，具体过程如下：

将机械臂的关节、连杆各部件按照相应的尺寸进行长方体包络；

将已知的环境信息进行长方体包络；

按照包络的长方体尺寸生成相应的AABB盒，以关节为单位将每个关节包含的所有长方体生成相应的AABB树，将所有的关节构成AABB树生成森林；

同理，构建环境包络的二叉树森林；

根据建立的森林进行，二叉树之间的遍历，如果二叉树底层的长方体相交测试返回碰撞，则两物体之间发生碰撞，否则，未发生碰撞。

步骤102，依据空间机械臂特性，获得基于启发函数的路径搜索算法。

具体方法为，整个过程采用分段拼接的方法进行路径搜索，当从起始点到目标点之间的路径不能直接到达时，通过启发式搜索方法，搜索若干中间点，将起始点与目标点和中间点拼接在一起，就可以形成完成的路径。当算法执行结束时，中间点个数随即确定。由于启发项的介入，在每个搜索步骤中，算法始终趋向资源配置最优的方向进行，保证整个任务代价最小。启发式搜索算法的启发函数，如下式所示：

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

其中，f(p)代表该节点的代价值，g(p)代表起点到任意节点n的最小代价值，h(p)代表到目标节点n的代价启发值。而g(p)、h(p)的代价值是通过多个不同优化目标共同决定的，如下式所示

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

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

其中k1,k2,...为相应的权值，g₁(p)、g₂(p)...和h₁(p)、h₂(p)...等为相应的优化函数，通过结合不同的优化目标得到该节点在当前状态下的适应性，比如，优化目标可以是关节行程，末端距离，消耗能量等指标。而p是函数的决策变量，它由不同的变量决定，是一组集合。

启发式搜索算法种类多样，包含：局部择优搜索法、最好优先搜索法、A*算法等等。这些算法都使用了启发函数，但在具体的选取最佳搜索节点时的策略不同。任务规划功能模块为了避免局部择优搜索法只能得到局部最优解，求解的最佳节点只是在该阶段的最佳并不一定是全局的最佳这一问题，采用了A*算法作为搜索算法。A*算法在搜索时，没有舍弃节点(除非该节点是死节点)，在每一步的估价中，都把当前的节点和以前的节点的估价值比较，得到一个“最佳的节点”，这样可以有效地防止“最佳节点”的丢失。

A*算法从起点开始，把它(以机械臂初始状态的末端点为起始点A)作为待处理的位置点存入一个“开启列表”，也就是一个等待检查位置点的列表。之后，开始寻找起点周围可以到达的位置点，将它们放入“开启列表”，并设置它们的“父节点”为A。

根据空间机械臂特性引入搜索维度D、位置搜索步长Δl及姿态搜索步长Δθ，搜索维度为在空间中机械臂末端搜索的方向，可以进行设定，位置搜索步长为机械臂末端的位置搜索长度，姿态搜索步长为机械臂末端的姿态搜索欧拉角长度，通过以机械臂末端位置和姿态为目标进行启发函数的改进，然后采用A*算法进行启发式的路径搜索过程。

在A*算法中引入了名义父节点的概念，即以当节点为子节点搜索路径寻到的父节点并不是真实路径中的下一点，在路径搜索过程中，当前节点的父节点总会与之前的子节点进行直接的路径规划，若可以直接到达，那么当前节点的名义父节点就是之前的节点，这样就可以实现只以若干个中间点拼接就可以到达目标点，实现了关于整个机械臂的避障路径搜索过程，搜索可以到达目标的中间点，将所有中间点进行拼接，形成从起始点到目标点的路径，完成整个路径搜索过程。

步骤103，依据改进的D*算法进行机械臂动态避障路径规划。

具体来说，就是依据以上的启发式路径搜索过程，采用改进的D*算法，进行动态的避障路径规划。

D*算法利用上次规划的信息重新搜索路径，避免重复计算相同数据，提高了二次规划的效率。采用遍历搜索法，每次对当前位置的上、下、左、右4个方向或上、下、左、右、左上、左下、右上、右下8个方向进行搜索。用估价函数来估计任意位置到目标位置的代价，选择代价最少的方向作为路径下一个前进方向。

D*算法开辟三个状态列表即OPEN列表、CLOSED列表和NEW列表，分别存储不同的路径代价：OPEN列表的集合为A，用于存储未经访问的节点路径代价；CLOSED列表的集合为B，用于存储已访问节点的路径代价；NEW列表的集合为C，用于存储待更新节点的路径代价。

首先，采用A*算法过程，以原任务的目标节点为A*算法的起始节点，以原任务的起始节点为A*算法的目标节点，进行启发式路径搜索过程，即进行环境的预处理。

为了寻求最短路径的单调序列Y，可以设置K为序列的最小路径代价估计，从起始节点向目标节点沿着A*已经规划完成的路径搜索，如果下一节点环境无变化，则按照预先规划的路径继续向前，若环境变换则修改路径代价估计值K，进行新的路径搜索，如果搜索到A*算法未覆盖区域，则将该节点放入NEW列表中，继续搜索，如此循环，直至满足终止条件。

在环境之中设置按照一定轨迹移动的障碍物，机械臂首先进行环境的预处理，在检测到下一部环境无变化的情况下，机械臂按照原定路线移动，当检测到下一步的环境发生变换，机械臂基于改进的D*避障路径规划算法，进行下一步的动态寻路，找到一条无碰撞的路径。通过如此寻路过程，完成实时动态避障过程。因为简单包络的碰撞检测效率非常高，以及改进D*避障路径规划算法的优势，使得机械臂能够完成动态的避障寻路。

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

本发明实施例的技术方案中，依据长方体相交测试思想，实现了机械臂与机械臂之间以及机械臂与环境之间的快速碰撞检测，并成功的将所有机械臂部件以及环境中的障碍物全部利用简单的长方体进行了包络。依据启发函数的路径搜索方法，对D*算法进行了改进，使其能够应用于七自由度机械臂的动态避障路径规划。本发明通过快速碰撞检测算法的实现，以及改进型D*避障路径规划算法实现，实现了七自由度机械臂动态避障路径规划方法，解决了机械臂无法完成动态路径规划的问题，很好的为机械臂操作人员提供了更灵活、更现实的操作方法。

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

Claims (3)

1.一种改进型D*的机械臂动态避障路径规划方法：

依据长方体包络的相交测试思想，获得了快速并较为精确的碰撞检测算法；

依据空间机械臂特性，获得启发式函数搜索路径规划算法；

依据改进型D*动态搜索算法，获得机械臂动态避障路径规划算法：

其中，快速碰撞检测算法至少包括：

依据简单的长方体包络，进行空间机械臂和环境的包络；

依据包络长方体特征，将其进行重叠式旋转变换，使其形成的包络区域近似为圆柱体或球体；

依据长方体间位姿关系，进行长方体之间的碰撞检测，完成快速碰撞检测过程；

长方体之间的碰撞检测包括：

通过旋转将长方体近似等价为圆柱体或球体的过程为，以长方体中心为原点，到各表面中心为坐标轴建立空间直角坐标系，以长方体构建圆柱体过程为例，将包络的长方体绕坐标轴按照设置角度旋转，以旋转过程中产生的多个长方体所构成的叠加图形近似代替圆柱体，同理，绕三个坐标轴旋转近似代替球体；

简单长方体的碰撞检测过程，利用如下公式进行长方体中心距离检测，若满足则返回碰撞，否则，继续；

其中，Dist(G_C1,G_C2)为表示长方体一G_C1与长方体二G_C2之间的距离，||O_cO_c'||表示长方体G_C1中心O_c与长方体G_C2中心O_c'之间距离，s₁、s₂、s₃、s₁'、s₂'、s₃'为起点为长方体中心坐标，终点为三个互不相对面中心的方向向量，d_S为碰撞检测的安全裕度；

第二步：判断长方体一的顶点是否在长方体二内，此即点-长方体碰撞检测，若有1个顶点碰撞则返回碰撞，否则继续判断下一个顶点，若都不碰撞则进入第三步；

第三步：判断长方体一的边是否与长方体二碰撞，此即线段-长方体碰撞检测，若有1条边碰撞则返回碰撞，否则继续判断下一条边，若都不碰撞则返回不碰撞。

2.根据权利要求1所述的方法，其特征在于，所述启发式函数搜索路径规划算法包括：

依据空间机械臂特性，改进启发式搜索算法的启发函数，如下式所述：

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

其中，f(p)代表该节点的代价值，g(p)代表起点到任意节点n的最小代价值，h(p)代表到目标节点n的代价启发值，而g(p)、h(p)的代价值是通过多个不同优化目标共同决定的，如下式所述：

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

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

其中k₁,k₂,...为相应的权值，g₁(p)、g₂(p)...和h₁(p)、h₂(p)...为相应的优化函数，通过结合不同的优化目标得到该节点在当前状态下的适应性，优化目标是关节行程、末端距离或消耗能量，是空间机械臂工作过程中需要考虑的指标，而p是函数的决策变量，它由不同的变量决定，是一组集合；

根据空间机械臂特性引入搜索维度D、位置搜索步长Δl及姿态搜索步长Δθ，搜索维度为在空间中机械臂末端搜索的方向，能够进行设定位置搜索步长为机械臂末端的位置搜索长度，以及设定姿态搜索步长为机械臂末端的姿态搜索欧拉角长度，通过以机械臂末端位置和姿态为目标进行启发函数的改进，使其最后能够搜索到目标位置和姿态。

3.根据权利要求1所述的方法，其特征在于，所述机械臂动态避障路径规划算法包括：

基于改进的D*动态搜索算法，D*算法开辟三个状态列表分别为OPEN列表，CLOSED列表和NEW列表，OPEN列表是一个保存等待检查的位置点的列表，CLOSED列表是一个保存不需要再次检查的位置点的列表；集合A用来存储OPEN列表中的位置点的路径代价，集合B用来存储CLOSED列表中的位置点的路径代价，集合C用来存储NEW列表中待更新节点的路径代价；

首先，采用A*算法过程，以原任务的目标节点为A*算法的起始节点，以原任务的起始节点为A*算法的目标节点，进行启发式路径搜索过程，即进行环境的预处理，A*算法从起点开始，以机械臂初始状态的末端点为起点A，把它作为待处理的位置点存入OPEN列表；

之后，开始寻找起点周围能够到达的位置点，将它们放入OPEN列表中，并设置它们的“父节点”为A；之后，从OPEN列表中删除起点A，并将起点A加入CLOSED列表；

为了寻求最短路径的单调序列Y，需要设置K为序列的最小路径代价估计，从起始节点向目标节点沿着A*已经规划完成的路径搜索，如果下一节点环境无变化，则按照预先规划的路径继续向前，若环境变换则修改路径代价估计值K，进行新的路径搜索，如果搜索到A*算法未覆盖区域，则将该节点放入NEW列表中，继续搜索，如此循环，直至满足终止条件；

采用改进的D*搜索算法，将带有启发函数的路径搜索算法引入到机械臂路径规划过程中，使机械臂在路径规划过程中能够根据启发函数选择出满足约束的最优路径，通过A*算法对环境进行预处理，使得寻路过程大大简化，能够适应具有动态障碍物的环境，实现动态的避障路径规划。