基于伪距离函数的工业机器人路径规划方法和系统

技术领域

本发明涉及机器人技术领域，具体地，涉及一种基于伪距离函数的工业机器人路径规划方法和系统。

背景技术

机器人离线编程是机器人在实际生产加工前的预演规划过程，与传统的手动示教相比，离线编程具有规划精度高、操作环境安全、不占用机器人等优点；规划一条使机械臂从位姿A移动到位姿B的无碰撞的路径是离线编程中最常见的功能之一。该应用场景下最常见的算法是RRT(Rapidly-exploring random trees)算法。RRT是一种通用的方法，不管什么机器人类型、不管自由度是多少、不管约束有多复杂都能使用，且实现原理简单。但其缺点也是非常明显的，首先RRT找到的路径只能满足可达性，并不能对路径质量做出保证，相同条件下两次RRT生成的路径差别可能很大。其次，对于复杂的障碍物环境(例如路径需要穿过狭长通道)，在给定尝试次数下，RRT能找到可达路径的概率较低。

专利文献CN105528759A(申请号：CN201610080714.7)公开了一种基于动态规划的距离变换计算方法，所述方法包括：步骤1：输入二值图像创建数组，将目标像素的8个邻域划分为上邻域和下邻域，其中，上邻域和下邻域分别包括4个邻域；步骤2：在目标像素的上邻域内计算目标像素与背景像素的伪距离变换，获得第一伪距离变换结果；步骤3：在目标像素的下邻域内计算目标像素与背景像素的伪距离变换，获得第二伪距离变换结果；步骤4：比较所述第一伪距离变换结果和所述第二伪距离变换结果，获得二者的较小值，则二者的较小值作为最终距离变换计算的结果。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于伪距离函数的工业机器人路径规划方法和系统。

根据本发明提供的基于伪距离函数的工业机器人路径规划方法，包括：

步骤1：用凸多面体对障碍物以及机械臂进行包络，将机械臂的几何外形以及环境中障碍物以凸多面体顶点的形式记录下来；

步骤2：输入起始点关节位形以及目标点关节位形；

步骤3：生成随机点或者以目标点作为采样点，选择已有路径点中与采样点最近的点作为父节点沿采样点方向生长一段距离产生新路径点；

步骤4：把父节点与新路径点之间的路径离散化，通过机械臂的运动学方程把机械臂关节空间的关节参数映射成机械臂工作空间的几何外形，通过GJK算法判断机械臂是否与环境障碍物发成碰撞，如果没有发生碰撞则执行步骤6；否则执行步骤5；

步骤5：如果采样点是随机点，则放弃该点重新返回到步骤3；如果发生了碰撞且采样点是目标点，则在碰撞点计算伪距离对机械臂各个关节的梯度，将碰撞点沿着梯度方向进行修正；如果没有发生碰撞，将该点加入到已有路径中；

步骤6：判断新路径点与目标点的距离是否小于设定阈值，如果小于则路径生成完毕，否则返回步骤3。

优选的，所述步骤1包括：对障碍物和机械臂用如下记号进行表示：

机械臂：

障碍物：

其中，robot(θ)表示机器人机械臂在位形θ下的集合；obstacles表示环境障碍物的集合；A

优选的，所述步骤4包括：定义父节点为θ

T(θ

将该路径离散化，得到一系列位姿点θ

优选的，所述步骤5包括：

伪距离是描述两凸集之间在一度规函数下的距离，定义

使用GJK算法判断凸集A与凸集B是否相交，将A、B和Q分别由各自的顶点集的凸包表示，即

优选的，将d

如果

d

d

其中，β

根据本发明提供的基于伪距离函数的工业机器人路径规划系统，包括：

模块M1：用凸多面体对障碍物以及机械臂进行包络，将机械臂的几何外形以及环境中障碍物以凸多面体顶点的形式记录下来；

模块M2：输入起始点关节位形以及目标点关节位形；

模块M3：生成随机点或者以目标点作为采样点，选择已有路径点中与采样点最近的点作为父节点沿采样点方向生长一段距离产生新路径点；

模块M4：把父节点与新路径点之间的路径离散化，通过机械臂的运动学方程把机械臂关节空间的关节参数映射成机械臂工作空间的几何外形，通过GJK算法判断机械臂是否与环境障碍物发成碰撞，如果没有发生碰撞则执行模块M6；否则执行模块M5；

模块M5：如果采样点是随机点，则放弃该点重新返回到模块M3；如果发生了碰撞且采样点是目标点，则在碰撞点计算伪距离对机械臂各个关节的梯度，将碰撞点沿着梯度方向进行修正；如果没有发生碰撞，将该点加入到已有路径中；

模块M6：判断新路径点与目标点的距离是否小于设定阈值，如果小于则路径生成完毕，否则返回模块M3。

优选的，所述模块M1包括：对障碍物和机械臂用如下记号进行表示：

机械臂：

障碍物：

其中，robot(θ)表示机器人机械臂在位形θ下的集合；obstacles表示环境障碍物的集合；A

优选的，所述模块M4包括：定义父节点为θ

T(θ

将该路径离散化，得到一系列位姿点θ

优选的，所述模块M5包括：

伪距离是描述两凸集之间在一度规函数下的距离，定义

使用GJK算法判断凸集A与凸集B是否相交，将A、B和Q分别由各自的顶点集的凸包表示，即

优选的，将d

如果

d

d

其中，β

与现有技术相比，本发明具有如下的有益效果：

(1)本发明通过在RRT技术的基础上引入基于伪距离的避障技术，在进行路径规划时实现了更快的收敛速度以及更好的路径质量；

(2)本发明算法与传统RRT相比生成的路径更优，鲁棒性更好，在障碍物较多或存在狭窄通道的环境下优势更加明显；

(3)本发明方法可用于在离线编程环境，给定机械臂起始、终止位姿以及环境障碍物下，在关节空间快速生成一系列位姿点，继而生成一条无碰撞路径。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明方法流程图；

图2为在复杂障碍物环境下平面三连杆机械臂的路径规划效果图；

图3为在复杂障碍物环境下工业机械臂UR10的路径规划效果图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例：

本发明的目的是提供一种适用于工业机械臂的快速、稳定的无碰撞路径生成方法，包括如下步骤：

步骤1：用凸多面体对障碍物以及机械臂进行包络；

步骤2：输入起始点关节位形θ

步骤3：以一定概率生成随机点或者以目标点作为采样点θ

步骤4：把父节点与新路径点之间的路径离散化，通过机械臂的运动学方程把机械臂关节空间的关节参数映射成机械臂工作空间的几何外形；使用GJK算法判断机械臂是否与环境障碍物发成了碰撞；如果没有发生碰撞则进入步骤6；否则进入步骤5；

步骤5：如果采样点是随机点，则放弃该点重新返回到步骤3；如果发生了碰撞且采样点是目标点，则在碰撞点计算伪距离对机械臂各个关节的梯度，将碰撞点沿着梯度方向进行修正；如果没有发生碰撞，将该点加入到已有路径中；

步骤6：判断新路径点与目标点的距离是否小于设定阈值，如果小于则路径生成完毕，否则返回步骤3。

对于步骤1中所述的环境障碍物以及机械臂我们用如下记号进行表示：

机械臂：

障碍物：

其中，

robot(θ)表示机器人机械臂在位形θ下的集合；obstacles表示环境障碍物的集合；A

对于步骤4，定义父节点为θ

T(θ

将该路径离散化，得到一系列位姿点θ

对于步骤5中所述伪距离是描述两凸集之间在一度规函数下的距离，定义

可以使用GJK算法判断凸集A与凸集B是否相交。

将A、B和Q分别由各自的顶点集的凸包表示，即

d

如果

d

d

其中，由伪距离函数的Lipschitz连续性可知d

其中，

类似的，当

针对步骤4中发生碰撞的姿态θ

其中，d

按下式计算

上式可以理解为

其中，s＞0代表步长，这里取值为

如图1是本实施例的具体流程，结合图2所示实施例做出阐述。

步骤1：首先，用多边形对机械臂以及环境障碍物进行包络；

障碍物：

机械臂连杆：

其中，a

步骤2：设置机械臂关节位形的起始点和终点：

θ

步骤3：初始化一个数组结构RRTree，在该结构中每一个元素由一个路径点θ

在已有路径点中寻找θ

在父节点与新节点之间生成一段路径：

T(θ

将该路径离散化，得到一系列位姿点θ

如果θ

在步骤6中判断||θ

图3为本算法在六轴机械臂UR10上的应用，设置了一个起始点，一个终点以及一个中间点，规划出的无碰路径如图3所示。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。