CN107953334A

CN107953334A - 一种基于a星算法的工业机械臂无碰撞路径规划方法

Info

Publication number: CN107953334A
Application number: CN201711424592.XA
Authority: CN
Inventors: 杨泽霖; 张亮; 罗红亮
Original assignee: Shenzhen He Zhongcheng Technology Co Ltd
Current assignee: Shenzhen He Zhongcheng Technology Co Ltd
Priority date: 2017-12-25
Filing date: 2017-12-25
Publication date: 2018-04-24

Abstract

本发明公开了一种基于A星算法的工业机械臂无碰撞路径规划方法，首先对六自由度机械臂进行参数化建模，并建立机械臂的正运动学模型。之后，构造机械臂及环境的OBB包围盒。随后，设置A星算法的估价函数和搜索步长，采用基于分离轴的碰撞检测算法在搜索的每一步中判断机械臂是否与环境发生碰撞。最后，根据关节转动角度之和最小且不发生碰撞原则在机械臂的六维关节空间中进行搜索，得到无碰撞的路径。本发明解决了针对六自由度机械臂，传统A星算法无法成功搜索的问题并提高了算法的搜索效率，为工业机械臂在已知障碍物分布情况下的路径规划提出了一种有效的方法。

Description

一种基于A星算法的工业机械臂无碰撞路径规划方法

技术领域

本发明涉及工业机器人路径规划技术领域，具体为一种基于A星算法的工业机械臂无碰撞路径规划方法。

背景技术

工业机器人作为“中国制造2025”的十大重点领域之一，工业机器人正逐渐被广泛应用到各种制造生产线上以提高生产线的智能化水平。路径规划是控制工业机器人按照人工预设的路径运动完成作业任务，决定了机器人的工作方式与工作效率。路径规划是工业机器人运动控制的基础，也是工业机器人重要的研究领域之一，路径规划技术是工业机器人自动化生产的基础，智能制造业所必不可缺的重要组成部分。

针对六自由度的工业机械臂，传统的无碰撞路径规划方法是基于C空间法(构型空间法)的路径规划，但是在求解C空间的障碍物边界时，需要将工作空间的障碍物区域转换到C空间，这需要大量的计算，并且精确的障碍域难以获得。

现有的基于A星算法的路径规划，很多是在工作空间(笛卡尔空间)进行路径搜索，这样规划出的最短路径并不是关节变化量最小的路径，所以需要在C空间进行路径规划。而针对C空间的路径规划，由于在六维空间的搜索问题的复杂性，现有的算法大多忽略了几个自由度，对搜索空间进行了一定的简化，这样得到的路径也是简化后的，并不能够保证路径的安全性。为了得到准确避障的路径，往往需要设置较小的搜索步长，搜索步长于避障准确度是相互矛盾的，如何在保证无碰撞的前提下提高搜索效率是需要解决的问题。

综上所述，针对六自由度机械臂在C空间的路径规划，传统的无碰撞路径规划的方法需要大量计算，并且精确的障碍域难以获得。传统的A星算法不适用于六维空间的搜索问题，并且搜索效率有待提高。

发明内容

本发明的目的在于提供一种基于A星算法的工业机械臂无碰撞路径规划方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：一种基于A星算法的工业机械臂无碰撞路径规划方法，包括以下步骤：

A、对六自由度机械臂进行参数化建模，并建立机械臂的正逆运动学模型，采用D-H法描述机械臂的位姿，建立机械臂的连杆坐标系，确定机械臂的D-H参数；根据D-H参数，连杆i和连杆i-1相对位置的齐次变换矩阵^i-1A_i为：

其中，C_i＝cosθ_i，S_i＝sinθ_i，其中i表示机械臂的连杆个数，i＝1,2,3,4,5,6；

将各关节的齐次变换矩阵相乘，得到了机械臂末端执行器的齐次变换矩阵为：

B、建立机械臂及环境的OBB包围盒：利用建模软件建立机械臂及环境的模型，并将机械臂的模型另存为STL格式文件，读取STL格式文件，采用OBB包围盒算法建立机械臂及环境障碍物的OBB包围盒；

C、设置A星算法的估价函数和搜索步长：采用基于分离轴的碰撞检测算法在搜索的每一步中判断机械臂是否与环境发生碰撞；机械臂在关节空间中某个节点的构型矢量为q_i，其中包括六个旋转关节的角度变幅，即q_i＝(q_i1,q_i2,q_i3,q_i4,q_i5,q_i6)；距离估价函数为所有关节转动的角度之和，即当前节点到目标节点的各关节位移变化量之和，估价函数为：

D、根据关节转动角度之和最小且不发生碰撞的原则，在机械臂的六维关节空间中进行搜索，得到无碰撞的路径；其中OPEN存放所有搜索节点，CLOSED中存放每一步搜索到的最佳节点，并将它们均初始化为空；机械臂起始位姿的构型矢量为q_ori，目标位姿的构型矢量为q_fin，机械臂当前位姿的构型矢量为q_cur，当前位姿的相邻位姿构型矢量为q_{nei_i}。

优选的，所述步骤B中，OBB包围盒算法流程如下：

1)通过三角面片信息计算每个三角形平面的法向矢量，并在每个三角面片上建立一个坐标系，原点为三角形任意顶点，三角形平面为x-y平面，z轴方向为三角形平面的法向矢量；

2)遍历每一个三角面片的平面法向矢量，绕该法向矢量旋转该三角面片的坐标系，寻找该物体的三角形网格模型投影到该三角面片的x-y平面面积最小的方向，记录下在旋转后的坐标系下的包围盒的体积；

3)将最小体积对应的坐标系的方向作为包围盒的三个方向向量，将物体的三角形网格中所有顶点投影到三个方向向量中，求出每个方向的最大值和最小值，得到OBB包围盒的八个顶点。

优选的，所述步骤C中，对于包围盒子A与B的碰撞检测算法流程如下：

a、计算两个包围潜在的15条分离轴，对每一条分离轴进行相交测试；

b、对每一条分离轴，将八个顶点做投影，得到两个包围盒投影的最大值和最小值分别为maxA、minA、maxB和minB,若maxA<mimB或maxB<minA，则在此分离轴下，两个包围盒分离，否则相交；

c、若对所有的分离轴均满足maxA<mimB或maxB<minA，则两个包围盒分离，否则两个包围盒相交。

优选的，所述步骤D中，A星算法的流程如下：

a、OPEN表中找到f(q_i)值最小的那一行向量，将该行向量作为当前搜索节点，并将该节点向量放入CLOSED表中，同时从OPEN表中删除该节点向量；

b、判断当前搜索节点是否为目标节点，如果是，那么搜索成功；否则继续下一步；

c、对于机械臂而言，每一个关节有三种变化方式，分别是保持不变、增加一定步长，减少一定步长，这样在一个六维空间中，当前搜索节点就有3⁶-1＝728个邻近的搜索节点，设第i个邻近节点为q_{nei_i}；

d、对于每一个邻近的节点执行如下操作：

(1)、对该搜索节点进行碰撞检测，如果发生了碰撞，则忽略该节点；

(2)、判断该邻近节点是否超出了该关节的最大角度或者最小角度，如果超限，则转至下一个邻近节点；

(3)、若邻近节点既没有发生碰撞也没有超限，则往下执行；

(4)、判断该节点矢量是否在OPEN表中，如果在OPEN表中，则通过节点索引函数查找该节点在OPEN表中的位置，然后判断当前q_{nei_i}节点中的g_{nei_i}与OPEN表中已经存在的g_{old_i}的大小关系，如果g_{nei_i}＜g_{old_i}，则就将当前的节点、当前节点的父亲节点以及新的g、h、f更新至OPEN表中原来旧节点的位置；

(5)、若该节点矢量不在OPEN表中，则直接把该节点矢量、当前节点矢量、该节点矢量的g、h、f值放入到OPEN表新的一行中，同时将当前节点、当前节点的父亲节点、当前节点的g、h、f值放入到CLOSED表新的一行中。

与现有技术相比，本发明的有益效果是：本发明无需求解障碍域，通过碰撞检测规避障碍，具有计算简单、方便，能够实现较准确的避障；对传统的A星算法的估价函数进行了改进，克服了六维空间搜索问题的复杂性，能够实现六自由度的机械臂在C空间的无碰撞路径搜索；此外，针对障碍物分布情况，将传统的A星算法的搜索步长设置为变步长，能够在一定程度上提高搜索的效率。

附图说明

图1为本发明流程图；

图2为本发明机械臂及环境障碍物的包围盒示意图；

图3为本发明机械臂的初始位姿示意图；

图4为本发明的机械臂的目标位姿示意图；

图5为本发明的路径规划的结果示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-5，本发明提供一种技术方案：一种基于A星算法的工业机械臂无碰撞路径规划方法，包括以下步骤：

A、对六自由度机械臂进行参数化建模，并建立机械臂的正逆运动学模型，采用D-H法描述机械臂的位姿，建立机械臂的连杆坐标系，确定机械臂的D-H参数；其中，机械臂的D-H参数如下表所示：

根据D-H参数，连杆i和连杆i-1相对位置的齐次变换矩阵^i-1A_i为：

本发明中，步骤B中，OBB包围盒算法流程如下：

本发明中，步骤C中，对于包围盒子A与B的碰撞检测算法流程如下：

此外，本发明中，步骤D中，A星算法的流程如下：

d、对于每一个邻近的节点执行如下操作：

(3)、若邻近节点既没有发生碰撞也没有超限，则往下执行；

图5为机械臂的路径规划的结果。固定步长和变步长的搜索效率对比如下表所示：

综上所述，本发明无需求解障碍域，通过碰撞检测规避障碍，具有计算简单、方便，能够实现较准确的避障；对传统的A星算法的估价函数进行了改进，克服了六维空间搜索问题的复杂性，能够实现六自由度的机械臂在C空间的无碰撞路径搜索；此外，针对障碍物分布情况，将传统的A星算法的搜索步长设置为变步长，能够在一定程度上提高搜索的效率。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims

1.一种基于A星算法的工业机械臂无碰撞路径规划方法，其特征在于：包括以下步骤：

<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mi>g</mi> <mo>(</mo> <msub> <mi>q</mi> <mi>i</mi> </msub> <mo>)</mo> <mo>=</mo> <mstyle> <munderover> <mo>&Sigma;</mo> <mrow> <mi>n</mi> <mo>=</mo> <mn>1</mn> </mrow> <mn>6</mn> </munderover> </mstyle> <mo>|</mo> <msub> <mi>q</mi> <mi>i</mi> </msub> <mo>(</mo> <mi>n</mi> <mo>)</mo> <mo>-</mo> <msub> <mi>q</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>(</mo> <mi>n</mi> <mo>)</mo> <mo>|</mo> <mo>+</mo> <mi>g</mi> <mo>(</mo> <msub> <mi>q</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mtd> </mtr> <mtr> <mtd> <mi>h</mi> <mo>(</mo> <msub> <mi>q</mi> <mi>i</mi> </msub> <mo>)</mo> <mo>=</mo> <mstyle> <munderover> <mo>&Sigma;</mo> <mrow> <mi>n</mi> <mo>=</mo> <mn>1</mn> </mrow> <mn>6</mn> </munderover> </mstyle> <mo>|</mo> <msub> <mi>q</mi> <mi>i</mi> </msub> <mo>(</mo> <mi>n</mi> <mo>)</mo> <mo>-</mo> <msub> <mi>q</mi> <mrow> <mi>f</mi> <mi>i</mi> <mi>n</mi> </mrow> </msub> <mo>(</mo> <mi>n</mi> <mo>)</mo> <mo>|</mo> </mtd> </mtr> <mtr> <mtd> <mi>f</mi> <mo>(</mo> <msub> <mi>q</mi> <mi>i</mi> </msub> <mo>)</mo> <mo>=</mo> <mi>g</mi> <mo>(</mo> <msub> <mi>q</mi> <mi>i</mi> </msub> <mo>)</mo> <mo>+</mo> <mn>2</mn> <mi>h</mi> <mo>(</mo> <msub> <mi>q</mi> <mi>i</mi> </msub> <mo>)</mo> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>

2.根据权利要求1所述的一种基于A星算法的工业机械臂无碰撞路径规划方法，其特征在于：所述步骤B中，OBB包围盒算法流程如下：

3.根据权利要求1所述的一种基于A星算法的工业机械臂无碰撞路径规划方法，其特征在于：所述步骤C中，对于包围盒子A与B的碰撞检测算法流程如下：

4.根据权利要求1所述的一种基于A星算法的工业机械臂无碰撞路径规划方法，其特征在于：所述步骤D中，A星算法的流程如下：

d、对于每一个邻近的节点执行如下操作：

(3)、若邻近节点既没有发生碰撞也没有超限，则往下执行；