[发明专利]一种基于A星算法的工业机械臂无碰撞路径规划方法

摘要

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

主权项

一种基于A星算法的工业机械臂无碰撞路径规划方法，其特征在于：包括以下步骤：A、对六自由度机械臂进行参数化建模，并建立机械臂的正逆运动学模型，采用D‑H法描述机械臂的位姿，建立机械臂的连杆坐标系，确定机械臂的D‑H参数；根据D‑H参数，连杆i和连杆i‑1相对位置的齐次变换矩阵i‑1Ai为：A01=C10-S10S10C100-10d10001A12=C2-S20a2C2S2C20a2S200100001A23=C30-S3a3C3S30C3a3C30-1000001]]>A45=C50-S50S50C500-1000001A45=C50-S50S50C500-1000001A56=C6-S600S6C600001d60001]]>其中，Ci＝cosθi，Si＝sinθi，其中i表示机械臂的连杆个数，i＝1,2,3,4,5,6；将各关节的齐次变换矩阵相乘，得到了机械臂末端执行器的齐次变换矩阵为：B、建立机械臂及环境的OBB包围盒：利用建模软件建立机械臂及环境的模型，并将机械臂的模型另存为STL格式文件，读取STL格式文件，采用OBB包围盒算法建立机械臂及环境障碍物的OBB包围盒；C、设置A星算法的估价函数和搜索步长：采用基于分离轴的碰撞检测算法在搜索的每一步中判断机械臂是否与环境发生碰撞；机械臂在关节空间中某个节点的构型矢量为qi，其中包括六个旋转关节的角度变幅，即qi＝(qi1,qi2,qi3,qi4,qi5,qi6)；距离估价函数为所有关节转动的角度之和，即当前节点到目标节点的各关节位移变化量之和，估价函数为：D、根据关节转动角度之和最小且不发生碰撞的原则，在机械臂的六维关节空间中进行搜索，得到无碰撞的路径；其中OPEN存放所有搜索节点，CLOSED中存放每一步搜索到的最佳节点，并将它们均初始化为空；机械臂起始位姿的构型矢量为qori，目标位姿的构型矢量为qfin，机械臂当前位姿的构型矢量为qcur，当前位姿的相邻位姿构型矢量为qnei_i。