CN108274465A - 基于优化a*的人工势场机械臂三维避障路径规划方法 - Google Patents

Abstract

本发明涉及一种基于优化A^*的人工势场机械臂三维避障路径规划方法包括以下步骤：建立三维避障环境及机械臂初始化；用A^*避免人工势场法陷入局部极小值的同时，通过最小二叉堆排序提高A^*搜索最小估计代价的效率，以此进行机械臂三维避障路径规划；对规划出的路径求运动学逆解，得到机械臂各关节角，由此规划一条避开障碍物的机械臂末端路径。利用最小二叉堆排序方法对A^*的人工势场法路径搜索算法中的OPEN表进行最小二叉堆排序，能够更加快速搜索出估计代价最小的节点，从而能快速寻找最小估计代价的避障路径，提高算法的效率。使用最小二乘法拟合对避障路径做平滑处理，使所得避障路径平滑，避免机械臂运动抖动。

Description

基于优化A*的人工势场机械臂三维避障路径规划方法

技术领域

本发明涉及一种机械臂避障路径规划技术，特别涉及一种基于二叉堆优化 A^*的人工势场机械臂三维避障路径规划方法。

背景技术

机械臂避障路径规划指的是，在给定环境与障碍条件，起始点位姿和目标点位姿的情况下，要规划一条从起始点至目标点的路径，使得机械臂能无碰撞的规划一条路径。

二维环境中有移动机器人的避障路径规划，三维环境中有机械臂的避障路径规划。传统的路径规划的方法有模拟退火算法、人工势场法等，较好的路径搜索算法有fallback算法、Floyd算法、Dijkstra算法、A^*算法等。

人工势场法的算法结构简单，运算速度快，但易于陷入局部极小值点，A^*是一种全局最优的规划算法，可以有效地避免人工势场法陷入局部极小值点。但是，在三维空间环境下搜索路径过程中，每次都从A^*的OPEN表中取出估计代价最小的节点用于下一次计算，存在运算速度慢的问题。

发明内容

本发明是针对三维环境中有机械臂的避障路径规划存在的问题，提出了一种基于优化A^*的人工势场机械臂三维避障路径规划方法，利用最小二叉堆排序方法对A^*的人工势场法搜索路径算法中的OPEN表进行最小二叉堆排序，能够更加快速搜索出估计代价最小的节点，从而能快速寻找最小估计代价的避障路径，提高算法的效率。

本发明的技术方案为：一种基于优化A^*的人工势场机械臂三维避障路径规划方法，

第一步：在三维空间环境中建立障碍物的模型，使用球体包络障碍物，并对机械臂末端执行器的起始点位置，目标点位置，每个关节的初始关节角均进行初始化；

第二步：使用最小二叉堆优化A^*的人工势场法来规划机械臂的三维避障路径，定义有序数组存储节点信息，即定义OPEN表用来存储已生成而待考察的节点， CLOSED表用来存储访问过的节点，使用最小二叉堆对OPEN表排序，寻找最小估计代价节点避障路径，

根据每一步判断节点的情况，把符合要求的节点存入OPEN表，每运行完该节点后，再把该节点从OPEN表存入CLOSED表，每个节点里面存储的是位置信息，并使用最小二叉堆对***新节点i+1的OPEN表进行排序，保证更新OPEN表的根节点对应的启发函数f(n)最小，f(n)＝g(n)+h(n)，f(n)表示机械臂末端从起始节点到目标节点的最优路径代价之和，g(n)表示从起始节点到当前节点n的实际代价，h(n)表示从当前节点n到目标节点的估计代价,机械臂末端每一走一步均需计算该启发函数f(n)的值，所得f(n)最小的节点就是机械臂末端下一步到达的位置；

第三步：判断第二步中规划出来的机械臂末端三维避障路径是否存在运动学逆解：求逆解是通过机械臂末端的坐标点位置信息确定机械臂各连杆关节角，确定机械臂是否存在可运行的避障路径，若存在逆运动学解，则执行第四步；若求逆运动学解失败，说明机械臂达到奇异限位或所求解的路径点超出机械臂末端执行器的工作空间，则返回第二步重新规划路径；

第四步：借助MATLAB的机器人工具箱Robotics Toolbox中的机器人逆解函数ikine()，对所规划出的机械臂末端三维避障路径求运动学逆解，得到8组不同的关节角，并挑选其中最优的一组解作为所求机械臂各个关节的关节角；

第五步：障碍物碰撞检测：使用基于包围球的障碍物检测方法，根据第四步计算出机械臂各关节角，及机械臂与障碍物球心的距离，检测机械臂是否与障碍物发生碰撞，若发生碰撞，返回第四步重新选择一组可行的次优解进行碰撞检测；若未发生碰撞，则第四步所求关节角为最优关节角，所规划的路径符合机械臂避障规划的要求，成功规划一条避障的机械臂末端路径，规划结束。

所述步骤第二步具体包括如下步骤：

S1：创建空的OPEN表和CLOSED表，定义步长stepL；

S2：将起始点START***OPEN表，计算启发函数f(n)＝g(n)+h(n)，f(n)表示机械臂末端从起始节点到目标节点的最优路径代价之和，g(n)表示从起始节点到当前节点n的实际代价，h(n)表示从当前节点n到目标节点的估计代价,

对于起始点START，其中实际代价g(n)＝0，估计代价

本文使用的估计代价函数h(n)为机械臂末端点位置(x_n、y_n、z_n)与目标点(x_d、y_d、z_d)之间的欧氏距离,接着把起始点STRAT***CLOSED表，记当前节点为i＝START；

S3：对当前节点i进行碰撞检测，若发生碰撞，规划失败，，停止规划；若未发生碰撞，则执行下一步S4；

S4：检测当前节点i对应的是否是目标节点，若不是目标节点，则执行步骤S5；若是目标节点，则执行步骤S9；

S5：使用人工势场法计算目标点对机械臂末端的斥力、引力、合力，再根据合力计算下一节点i+1的受力情况，其中当前节点i的实际代价g(n)为上一节点的实际代价加上步长stepL；

S6：判断当前节点i是否是极小值点，及当前节点i机械臂所受合力是否为零，若不是极小值点，则执行下一步S7；若是极小值点，则执行IV，

IV具体步骤包括：

SS1：对当前节点i进行上、下、左、右、前、后共6个方向同步长stepL扩展；

SS2：通过扩展得到后继节点next，计算后继节点NEXT的代价f(next)；

SS3：判断后继节点next是否在OPEN表中，若在，则执行SS4；若不在，则执行SS5；

SS4：把f(next)与当前节点i对应的最小根节点的f(n)进行比较，使用最小二叉堆排序更新OPEN表中的根节点或叫最小父节点f(n)；

SS5：判断后继节点next是否在CLOSED表中，若在，则执行SS6；若不在，则把后继节点next***OPEN表中；

SS6：判断OPEN表是否为空，若不为空，则执行SS7；若为空，则结束规划；

SS7：用最小二叉堆法对OPEN表进行排序，更新最小根节点f(n),把更新后的最小根节点作为避障规划的下一节点i+1，此时避障规划跳出了局部极小值点；

SS8：判断节点i+1处机械臂是否发生碰撞，若发生碰撞，则选取次最小节点的 f(n)作为下一节点i+1，并转到SS7重新判断；若不发生碰撞，则执行SS9；

SS9：把下一节点i+1***CLOSED表，并使用最小二叉堆法对该CLOSED表排序，赋值i＝i+1，即把下一节点i+1作为当前节点i，更新符合要求的下一节点i+1 为当前节点i；

SS10：判断更新后的节点i是否在障碍物影响的范围之内，若是，则转到IV重新进行扩展；若不是，则转到II重新判断更新后的当前节点i是否是目标节点； S7：把由人工势场法计算的下一节点i+1***OPEN表与CLOSED表，并使用最小二叉堆对***新节点i+1的OPEN表进行排序，保证更新OPEN表的根节点f(n) 最小，同理对CLOSED表使用最小二叉堆排序更新，再把当前节点i保存为父节点，保存使用最小二叉堆排序更新后的OPEN表和CLOSED表；

S8：将下一节点i+1当作当前节点i，转到步骤S3进行碰撞检测；

S9：当前节点i是目标节点，记目标节点为END，则说明机械臂寻找到避障路径，从END反向回寻至START，得到规划的避障路径；

S10：使用最小二乘法，对规划得到的避障路径进行拟合，使该路径更加平滑；

S11：规划结束。

本发明的有益效果在于：本发明基于优化A^*的人工势场机械臂三维避障路径规划方法，最小二叉堆排序提高了A^*的人工势场法搜索路径的寻找速度，使用最小二乘法拟合对避障路径做平滑处理，使所得避障路径平滑，避免机械臂运动抖动。

附图说明

图1为本发明人工势场机械臂三维避障路径规划方法主流程图；

图2为本发明方法中最小二叉堆优化A^*的人工势场法流程图；

图3为本发明方法中人工势场法陷入局部极小值点时用二叉堆A^*跳出局部极小值点的流程图；

图4为本发明方法最小二叉堆对A^*的OPEN表排序实施例的流程图；

图5为本发明方法删除操作后用最小二叉堆重新排序实施例图；

图6为本发明方法***操作后用最小二叉堆重新排序实施例图。

具体实施方式

二叉堆是完全或近似完全的二叉树，本方法使用最小二叉堆优化A^*的人工势场法，最小二叉堆的特点：父节点总是不大于子节点，使用最小二叉堆排序之后，该根节点的值最小。故而使用最小二叉堆对OPEN表排序，使得OPEN表的根节点为最小估计代价的节点，能加快寻找最小估计代价节点避障路径的速度。

本方法实施例，如图1所示人工势场法机械臂三维避障路径规划方法主流程步骤包括：

第一步：在三维空间环境中建立障碍物的模型，使用球体包络障碍物，并对机械臂末端执行器的起始点位置，目标点位置，每个关节的初始关节角均进行初始化；

第二步：使用最小二叉堆优化A^*的人工势场法来规划机械臂的三维避障路径；

第三步：判断第二步中规划出来的机械臂末端三维避障路径是否存在运动学逆解。求逆解是通过机械臂末端的坐标点位置信息确定机械臂各连杆关节角等，确定机械臂是否存在可运行的避障路径。若存在逆运动学解，则执行第四步；若求逆运动学解失败，说明机械臂达到奇异限位，或所求解的路径点超出机械臂末端执行器的工作空间，则返回第二步重新规划路径；

第四步：借助MATLAB的机器人工具箱Robotics Toolbox中的机器人逆解函数ikine()，对所规划出的机械臂末端三维避障路径求运动学逆解，得到8组不同的关节角，并挑选其中最优的一组解作为所求机械臂各个关节的关节角；

第五步：障碍物碰撞检测。使用基于包围球的障碍物检测方法，根据第四步计算出机械臂各关节角，及机械臂与障碍物球心的距离，检测机械臂是否与障碍物发生碰撞，若发生碰撞，返回第四步重新选择一组可行的次优解(求运动学逆解的时候，会自动求出最优解，次优解，第三优解，…，等等，只不过每次都是优选挑选最优的那组作为所求解，最优那组不符合避障运动规划的时候，才会挑选次优解)，若未发生碰撞，则第四步所求关节角为最优关节角，所规划的路径符合机械臂避障规划的要求，规划结束。

对图1第二步的具体过程如图2和3所示，最小二叉堆优化A^*的人工势场法和人工势场法陷入局部极小值点时用二叉堆A^*跳出局部极小值点的流程图，为本方法实施例使用A^*算法避免人工势场法陷入局部极小值点的同时，使用最小二叉堆对OPEN表排序，能加快寻找最小估计代价节点避障路径的速度，包括：

S1：创建空的OPEN表和CLOSED表，定义步长stepL；

S2：将起始点START***OPEN表，计算启发函数f(n)＝g(n)+h(n)，f(n) 表示机械臂末端从起始节点到目标节点的最优路径代价之和，g(n)表示从起始节点到当前节点n的实际代价，h(n)表示从当前节点n到目标节点的估计代价, 对于起始点START，其中实际代价g(n)＝0，估计代价

本文使用的估计代价函数h(n)为机械臂末端点位置(x_n、y_n、z_n)与目标点(x_d、y_d、z_d)之间的欧氏距离,接着把起始点STRAT***CLOSED表，记当前节点为i＝START；

S3：对当前节点i进行碰撞检测，若发生碰撞，规划失败，停止规划；若未发生碰撞，则执行下一步S4；

S4：检测当前节点i对应的是否是目标节点，若不是目标节点，则执行步骤S5，若是目标节点，则执行步骤S9；

S5：使用人工势场法计算目标点对机械臂末端的斥力、引力、合力等(机械臂末端每运动到下一个节点，都需要计算机械臂末端所受引力、斥力、合力等，通过合力自动引导机械臂末端到达下一个节点位置)，再根据合力计算下一节点i+1(在人工市场的引导下，机械臂朝着目标点运动，每次运动一个步长 stepL，由合力的方向加上步长就知道下一个节点)的受力情况，其中当前节点i 的实际代价g(n)为上一节点的实际代价加上步长stepL；

S6：判断当前节点i是否是极小值点，及当前节点i机械臂所受合力是否为零，若不是极小值点，则执行下一步S7，若是极小值点，则执行IV；

S7：把由人工势场法计算的下一节点i+1***OPEN表与CLOSED表(OPEN表用来存储已生成而待考察的节点，CLOSED表用来存储访问过的节点。起初定义的OPEN表和CLOSED表都是空的，会根据每一步判断节点的情况，把符合要求的节点存入OPEN表，每运行完该节点后，再把该节点从OPEN表存入CLOSED表。每个节点里面存储的是位置信息)，并使用最小二叉堆对***新节点i+1的OPEN 表进行排序，保证更新OPEN表的根节点(根节点为最小二叉堆最顶上的一个节点)f(n)最小，同理对CLOSED表使用最小二叉堆排序更新，再把当前节点i保存为父节点(父节点为当前所描述节点的上一个节点)，保存使用最小二叉堆排序更新后的OPEN表和CLOSED表；

S8：将下一节点i+1当作当前节点i，转到S3进行碰撞检测；

S9：当前节点i是目标节点，记目标节点为END，则说明机械臂寻找到避障路径，从END反向回寻至START，得到规划的避障路径；

S10：使用最小二乘法，对规划得到的避障路径进行拟合，使该路径更加平滑；

S11：规划结束。

前面步骤S6中，若为极小值点，则执行IV，IV具体步骤包括：

SS1：对当前节点i进行上、下、左、右、前、后共6个方向同步长stepL 扩展；

SS2：通过扩展得到后继节点next，计算后继节点NEXT的代价f(next)；

SS3：判断后继节点next是否在OPEN表中，若在，则执行SS4，若不在，则执行SS5；

SS4：把f(next)与当前节点i对应的最小根节点的f(n)进行比较，使用最小二叉堆排序更新OPEN表中的根节点或叫最小父节点f(n)；

SS5：判断后继节点next是否在CLOSED表中，若在，则执行SS6，若不在，则把后继节点next***OPEN表中；

SS6：判断OPEN表是否为空，若不为空，则执行SS7，若为空，则结束规划；

SS7：用最小二叉堆法对OPEN表进行排序，更新最小根节点f(n),把更新后的最小根节点作为避障规划的下一节点i+1，此时避障规划跳出了局部极小值点；

SS8：判断节点i+1处机械臂是否发生碰撞，若发生碰撞，则选取“次最小节点”的f(n)作为下一节点i+1，并转到SS7重新判断，若不发生碰撞，则执行SS9；

SS9：把下一节点i+1***CLOSED表，并使用最小二叉堆法对该CLOSED表排序，赋值i＝i+1，即把下一节点i+1作为当前节点i，更新符合要求的下一节点i+1为当前节点i；

SS10：判断更新后的节点i是否在障碍物影响的范围之内，若是，则转到IV重新进行扩展，若不是，则转到II重新判断更新后的当前节点i是否是目标节点。

本方法实施例如附图4，使用最小二叉堆对A^*的OPEN表排序，该排序方法具体包括：

STEP1：把该OPEN表用最小二叉堆排序方法进行排序，得到根节点为最小值的最小二叉堆列表，并在对排序后的表进行***或删除操作后能重新排序，保持根节点最小；

STEP2：若向该最小二叉堆列表中***新节点时，执行STEP4，若向该最小二叉堆列表中删除最小值节点，即删除根节点时执行STEP3；

STEP3：向该最小二叉堆列表执行删除节点操作，本方法删除操作后用最小二叉堆重新排序实施例如附图5，其步骤包括：

STEP31：删除根节点f(n),即删除该二叉堆列表的最小估计代价值的节点，使该处为空位；

STEP32：接着把该二叉堆列表末端的节点放入删除根节点空位处，构成一个新的堆列表；

STEP33：对该新的堆列表使用最小二叉堆排序的方法进行排序操作，朝着使父节点不比子节点小的方向排序，当该新节点大于子节点时，把该新节点与子节点交换位置，保证跟更新后的父节点永远不比子节点小；

STEP34：当所有的父节点均不比子节点小时，则对该堆列表的小二叉堆排序完成，所得更新的根节点f(n)，为删除新节点重新排序后代表最小估计代价值的节点；

STEP4：向该最小二叉堆列表执行***新节点操作，本方法***操作后用最小二叉堆重新排序实施例如附图6，其步骤包括：

STEP41：把新节点***在该最小二叉堆列表的末端，得到***新节点的堆列表；

STEP42：接着把***新节点的堆列表，使用最小二叉堆排序的方法进行排序操作，朝着使父节点不比子节点小的方向排序，当该新节点小于父节点时把该节点与父节点交换位置；

STEP43：当所有的父节点均不比子节点小时，则对该堆列表的小二叉堆排序完成，所得更新的根节点f(n)，为***新节点重新排序后代表最小估计代价值的节点。

Claims (2)

1.一种基于优化A^*的人工势场机械臂三维避障路径规划方法，其特征在于，

第一步：在三维空间环境中建立障碍物的模型，使用球体包络障碍物，并对机械臂末端执行器的起始点位置，目标点位置，每个关节的初始关节角均进行初始化；

第二步：使用最小二叉堆优化A^*的人工势场法来规划机械臂的三维避障路径，定义有序数组存储节点信息，即定义OPEN表用来存储已生成而待考察的节点，CLOSED表用来存储访问过的节点，使用最小二叉堆对OPEN表排序，寻找最小估计代价节点避障路径，

根据每一步判断节点的情况，把符合要求的节点存入OPEN表，每运行完该节点后，再把该节点从OPEN表存入CLOSED表，每个节点里面存储的是位置信息，并使用最小二叉堆对***新节点i+1的OPEN表进行排序，保证更新OPEN表的根节点对应的启发函数f(n)最小，f(n)＝g(n)+h(n)，f(n)表示机械臂末端从起始节点到目标节点的最优路径代价之和，g(n)表示从起始节点到当前节点n的实际代价，h(n)表示从当前节点n到目标节点的估计代价,机械臂末端每一走一步均需计算该启发函数f(n)的值，所得f(n)最小的节点就是机械臂末端下一步到达的位置；

第三步：判断第二步中规划出来的机械臂末端三维避障路径是否存在运动学逆解：求逆解是通过机械臂末端的坐标点位置信息确定机械臂各连杆关节角，确定机械臂是否存在可运行的避障路径，若存在逆运动学解，则执行第四步；若求逆运动学解失败，说明机械臂达到奇异限位或所求解的路径点超出机械臂末端执行器的工作空间，则返回第二步重新规划路径；

第四步：借助MATLAB的机器人工具箱Robotics Toolbox中的机器人逆解函数ikine()，对所规划出的机械臂末端三维避障路径求运动学逆解，得到8组不同的关节角，并挑选其中最优的一组解作为所求机械臂各个关节的关节角；

第五步：障碍物碰撞检测：使用基于包围球的障碍物检测方法，根据第四步计算出机械臂各关节角，及机械臂与障碍物球心的距离，检测机械臂是否与障碍物发生碰撞，若发生碰撞，返回第四步重新选择一组可行的次优解进行碰撞检测；若未发生碰撞，则第四步所求关节角为最优关节角，所规划的路径符合机械臂避障规划的要求，成功规划一条避障的机械臂末端路径，规划结束。

2.根据权利要求1所述基于优化A^*的人工势场机械臂三维避障路径规划方法，其特征在于，所述步骤第二步具体包括如下步骤：

S1：创建空的OPEN表和CLOSED表，定义步长stepL；

S2：将起始点START***OPEN表，计算启发函数f(n)＝g(n)+h(n)，f(n)表示机械臂末端从起始节点到目标节点的最优路径代价之和，g(n)表示从起始节点到当前节点n的实际代价，h(n)表示从当前节点n到目标节点的估计代价,

对于起始点START，其中实际代价g(n)＝0，估计代价本文使用的估计代价函数h(n)为机械臂末端点位置(x_n、y_n、z_n)与目标点(x_d、y_d、z_d)之间的欧氏距离,接着把起始点STRAT***CLOSED表，记当前节点为i＝START；

S3：对当前节点i进行碰撞检测，若发生碰撞，规划失败，，停止规划；若未发生碰撞，则执行下一步S4；

S4：检测当前节点i对应的是否是目标节点，若不是目标节点，则执行步骤S5；若是目标节点，则执行步骤S9；

S5：使用人工势场法计算目标点对机械臂末端的斥力、引力、合力，再根据合力计算下一节点i+1的受力情况，其中当前节点i的实际代价g(n)为上一节点的实际代价加上步长stepL；

S6：判断当前节点i是否是极小值点，及当前节点i机械臂所受合力是否为零，若不是极小值点，则执行下一步S7；若是极小值点，则执行IV，

IV具体步骤包括：

SS1：对当前节点i进行上、下、左、右、前、后共6个方向同步长stepL扩展；

SS2：通过扩展得到后继节点next，计算后继节点NEXT的代价f(next)；

SS3：判断后继节点next是否在OPEN表中，若在，则执行SS4；若不在，则执行SS5；

SS4：把f(next)与当前节点i对应的最小根节点的f(n)进行比较，使用最小二叉堆排序更新OPEN表中的根节点或叫最小父节点f(n)；

SS5：判断后继节点next是否在CLOSED表中，若在，则执行SS6；若不在，则把后继节点next***OPEN表中；

SS6：判断OPEN表是否为空，若不为空，则执行SS7；若为空，则结束规划；

SS7：用最小二叉堆法对OPEN表进行排序，更新最小根节点f(n),把更新后的最小根节点作为避障规划的下一节点i+1，此时避障规划跳出了局部极小值点；

SS8：判断节点i+1处机械臂是否发生碰撞，若发生碰撞，则选取次最小节点的f(n)作为下一节点i+1，并转到SS7重新判断；若不发生碰撞，则执行SS9；

SS9：把下一节点i+1***CLOSED表，并使用最小二叉堆法对该CLOSED表排序，赋值i＝i+1，即把下一节点i+1作为当前节点i，更新符合要求的下一节点i+1为当前节点i；

SS10：判断更新后的节点i是否在障碍物影响的范围之内，若是，则转到IV重新进行扩展；若不是，则转到II重新判断更新后的当前节点i是否是目标节点；

S7：把由人工势场法计算的下一节点i+1***OPEN表与CLOSED表，并使用最小二叉堆对***新节点i+1的OPEN表进行排序，保证更新OPEN表的根节点f(n)最小，同理对CLOSED表使用最小二叉堆排序更新，再把当前节点i保存为父节点，保存使用最小二叉堆排序更新后的OPEN表和CLOSED表；

S8：将下一节点i+1当作当前节点i，转到步骤S3进行碰撞检测；

S9：当前节点i是目标节点，记目标节点为END，则说明机械臂寻找到避障路径，从END反向回寻至START，得到规划的避障路径；

S10：使用最小二乘法，对规划得到的避障路径进行拟合，使该路径更加平滑；

S11：规划结束。