[发明专利]一种机械臂避障路径规划方法、设备及存储设备

权利要求书

1.一种机械臂避障路径规划方法，其特征在于：包括以下步骤：

S101：通过机械臂中的外部传感器，检测作业目标及障碍物的位姿数据，以所述机械臂末端的初始位置作为避障路径规划的起始节点，以作业目标的位置作为所述避障路径规划的目标节点；

S102：采用C空间法对所述机械臂的作业空间进行三维建模,得到C空间的网格化模型；

所述C空间用来表示物体自由度的参数空间，所述C空间分为自由空间和障碍物空间，所述自由空间为所述机械臂及其他固定物体互不干涉的C空间，所述C空间中的障碍物被称为C障碍物，所述障碍物空间为所述C障碍物所处的C空间，为避免所述机械臂与所述C障碍物发生碰撞，需对所述障碍物空间进行扩展，所述障碍物空间的扩展过程为：将所述机械臂外形简化为圆柱形，取所述机械臂大臂的半径为d，对所述障碍物空间向外以所述C障碍物为圆心，d为半径做各个方向上的扩展，凡是与扩展范围内相交的立方体都纳入到所述障碍物空间；使用网格法将所述C空间用若干个相等的立方体表示，每一个所述立方体都对应有一个单独的索引值，所述障碍物空间内的所述立方体对应的索引值为index(X,Y,Z)，所述自由空间内的所述立方体对应的索引值为index(x,y,z)，所述索引值index(x,y,z)对应于所述自由空间内的所述立方体的8个顶点中的任一顶点，所述顶点被称为索引点，所述自由空间中的所述索引点的选择必须同一方向；索引值index(m，n，l)中的(m，n，l)分别表示从原点0到所述索引点处在x、y和z方向上所述立方体的个数，取所述立方体的边长为length，则所述索引点处的坐标为：其中，或由所述自由空间和障碍物空间中各个索引点的坐标得到C空间的网格化模型；

S103：根据所述网格化模型，采用改进的A*算法在网格化的C空间内进行避障路径搜索，得到所述机械臂末端自起始节点到目标节点的避障路径；将网格宽度作为路径搜索过程中的基本步长，网格中的任意两个节点的连接线段可构成C空间中的一条路径，判断所述路径安全的两个条件为：所述路径两端点必须处于自由空间和所述路径不能与所述C障碍物相交或相切；

建立改进的A*算法的估价函数如公式(1)所示：

f(n)＝g(n)+h(n) (1)

其中，f(n)为当前节点的估价函数，g(n)为起始节点到当前节点的实际代价，表示搜索广度的优先趋势，h(n)为启发函数，表示从当前节点到目标节点的最佳路径的估计代价，体现了搜索中的启发信息；

估计代价h(i)取当前节点与目标节点在C空间中的距离，如公式(2)所示：

其中，h(i)为当前节点与目标节点在C空间中的距离，(x_i,y_i,z_i)为当前节点的坐标，(x_d,y_d,z_d)为目标节点的坐标；

采用改进的A*算法搜索避障路径的过程为：

S201：将起始节点的坐标S(x,y,z)添加到open列表；

S202：检测所述open列表是否为空；若是，则到步骤S203；若否，则到步骤S204；

S203：结束路径搜索，搜索不到避障路径；

S204：选择所述open列表中未计算过的一个节点，并计算所有节点的f(n)值；

S205：判断所述节点的f(n)值是否最小；若是，则到步骤S206；若否，则到步骤S207；f(n)值最小的节点被称为当前节点；

S206：将路径点代入逆运动学方程，得到逆解，采用时间—能量最优法选择一组逆解，所述逆解即为所述机械臂末端到达目标节点的关节角；其中，采用动态步长的方式，通过估价函数f(n)选出所述路径点；采用D-H法将关节角代入正运动学方程，得到关节线段方程，然后到步骤S208；

S207：将所述节点从所述open列表中删除，加入到closed列表中，回到步骤S202；

S208：求解所述关节线段方程与采用长方体包络法得到的障碍物包络的位置关系，进行碰撞检测；

S209：判断所述关节线段方程与所述障碍物包络是否碰撞；若是，则到步骤S210；若否，则到步骤S211；

采用长方体包络法，包络L个障碍物的长方体分别为{O₁，O₂，…，O_L}，通过检测所述机械臂的各关节所连接的线段与{O₁，O₂，…，O_L}各个面是否有交点，把碰撞问题转化为空间直线段与四边形之间的位置关系的判断，位置关系的判断方法是：已知空间线段上2个端点p₁(x₁，y₁，z₁)和p₂(x₂，y₂，z₂)、空间四边形的法向量M以及四个顶点的坐标，由空间解析几何知识可知，线段与平面位置关系可以用空间线段方向向量与空间四边形法向量之间的关系来判断，其中空间线段方向向量T＝[x₂-x₁ y₂-y₁ z₂-z₁]；若M·T＝0，则空间线段与平面平行，没有交点；若M·T≠0，则空间线段与平面有交点，设所述交点为p₀(x₀，y₀，z₀)；判断所述交点是否为碰撞点；若是，则满足两个条件：(1)所述交点p₀(x₀，y₀，z₀)在空间线段上和所述交点在空间四边形内；(2)所述障碍物包络成的长方体的前面、后面平行于z轴，左面、右面平行于y轴，上面、下面平行于x轴，以平行于x轴的上平面为例，上平面的4个顶点为：s₁(x₃，y₃，z₃)，s₂(x₄，y₄，z₄)，s₃(x₅，y₅，z₅)，s₄(x₆，y₆，z₆)，若有：min(z₁，z₂)≤z₀≤max(z₁，z₂)，min(x₃，x₄)≤x₀≤max(x₃，x₄)，min(y₃，y₄)≤y₀≤max(y₃，y₄)，则所述交点p₀(x₀，y_o，z₀)是碰撞点；若否，则所述交点p₀(x₀，y₀，z₀)不是碰撞点；

S210：判断是否还有未检测的逆解；若是，则到步骤S212；若否，则回到步骤S202；

S211：将当前节点加入所述open列表，然后到步骤S213；

S212：将当前关节角舍弃，在剩下的逆解中选择一组解，回到步骤S206；

S213：设置当前节点为父节点，扩展后继节点，将生成的后继节点加入所述open列表；

S214：建立后继节点返回到当前节点的指针；

S215：根据所述指针，判断所述机械臂末端是否找到目标节点；若是，则到步骤S216；若否，则回到步骤S202；

S216：根据起始节点、路径点和目标节点，并经过碰撞检测，最终找到所述避障路径，并保存所述避障路径；

S104：采用三次均匀B样条曲线方法对所述避障路径进行平滑处理，将平滑处理过的所述避障路径作为最终的路径规划结果。