[发明专利]一种基于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平面面积最小的方向，记录下在旋转后的坐标系下的包围盒的体积；