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

权利要求书

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

步骤1、对六自由度机械臂进行参数化建模，并建立机械臂的正逆运动学模型：

采用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；

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

其中，p_x,p_y,p_z表示平移变换在x，y，z三个方向的分量，n＝[n_x n_y n_z]，o＝[o_x o_y o_z]，a＝[a_x a_y a_z]，分别为绕x，y，z三个方向旋转矢量；

步骤2、建立机械臂及环境的OBB包围盒：利用soildwork建模软件建立机械臂及环境模型，并将机械臂及环境模型另存为STL格式文件，读取STL格式文件，从而得到模型中各个物体的三角形网格模型，采用OBB包围盒算法建立机械臂及环境障碍物的OBB包围盒；OBB包围盒方法流程为：

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

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

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

步骤3、设置A*算法的估价函数和搜索步长，采用基于分离轴的碰撞检测算法在搜索的每一步中判断机械臂是否与环境发生碰撞：

机械臂在关节空间中某个节点的构型矢量为q_i，其中包括六个旋转关节的角度变幅，即q_i＝(q_i1,q_i2,q_i3,q_i4,q_i5,q_i6)，距离估价函数为所有关节转动的角度之和，在估价函数中加入一项，即当前节点到目标节点的各关节位移变化量之和，估价函数如下：

在机械臂的作业场景中，障碍物区域设置0.1到0.2度之间较小的搜索步长；而在非障碍物区域设置1到1.5度之间较大的搜索步长；

采用基于分离轴相交测试的方法在搜索过程中进行碰撞检测，对于包围盒子A与B的碰撞检测算法流程为：

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

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

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

步骤4、根据关节转动角度之和最小且不发生碰撞的原则，在机械臂的六维关节空间中进行搜索，得到无碰撞的路径；

设置两个存储矩阵，分别为OPEN表和CLOSED表，其中OPEN存放所有搜索节点，CLOSED中存放每一步搜索到的最佳节点，并将它们均初始化为空，机械臂起始位姿的构型矢量为q_ori，目标位姿的构型矢量为q_fin，机械臂当前位姿的构型矢量为q_cur，当前位姿的相邻位姿构型矢量为q_{nei_i}；

将机械臂的初始位姿的构型矢量、它的父亲位姿的构型矢量，即它本身和与之对应的g、h、f合并为一个向量放入OPEN表的第一行，即：

OPEN(1,:)＝(q_ori,q_ori,g(q_ori),h(q_ori),f(q_ori))

OPEN(1,:)是一个1×15的矢量，存放到OPEN表的第一行，并将这个向量放入CLOSED表的第一行；

重复进行以下步骤，直到q_cur＝q_fin，完成搜索任务，若OPEN表变为空，则跳出循环，搜索失败；

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

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

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

4.4)对于每一个邻近的搜索节点执行如下操作：

a)对邻近的搜索节点进行碰撞检测，如果发生了碰撞，则忽略该节点；

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

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

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

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

当搜索成功后，从CLOSED表中的最后一行开始，通过节点索引函数依次查找当前节点的父亲节点，一直到起始节点，就完成了机械臂在关节空间的构型搜索任务。