[发明专利]基于伪距离函数的工业机器人路径规划方法和系统

权利要求书

1.一种基于伪距离函数的工业机器人路径规划方法，其特征在于，包括：

步骤1：用凸多面体对障碍物以及机械臂进行包络，将机械臂的几何外形以及环境中障碍物以凸多面体顶点的形式记录下来；

步骤2：输入起始点关节位形以及目标点关节位形；

步骤3：生成随机点或者以目标点作为采样点，选择已有路径点中与采样点最近的点作为父节点沿采样点方向生长一段距离产生新路径点；

步骤4：把父节点与新路径点之间的路径离散化，通过机械臂的运动学方程把机械臂关节空间的关节参数映射成机械臂工作空间的几何外形，通过GJK算法判断机械臂是否与环境障碍物发成碰撞，如果没有发生碰撞则执行步骤6；否则执行步骤5；

步骤5：如果采样点是随机点，则放弃该点重新返回到步骤3；如果发生了碰撞且采样点是目标点，则在碰撞点计算伪距离对机械臂各个关节的梯度，将碰撞点沿着梯度方向进行修正；如果没有发生碰撞，将该点加入到已有路径中；

步骤6：判断新路径点与目标点的距离是否小于设定阈值，如果小于则路径生成完毕，否则返回步骤3。

2.根据权利要求1所述的基于伪距离函数的工业机器人路径规划方法，其特征在于，所述步骤1包括：对障碍物和机械臂用如下记号进行表示：

机械臂：

障碍物：

其中，robot(θ)表示机器人机械臂在位形θ下的集合；obstacles表示环境障碍物的集合；A_i(θ)表示机械臂第i连杆在位形θ下顶点集的集合；B_j表示第j个环境障碍物顶点集的集合；i表示机械臂第i个连杆；I表示机器人连杆数的集合；j表示第j个环境障碍物；J表示环境障碍物的集合；k表示第k个顶点；n表示第n个顶点；N_i表示机械臂第i连杆的顶点集合；K_j表示第j个环境障碍物的顶点集合；表示机械臂第i连杆在位形θ下顶点集的凸包；表示第j个环境障碍物顶点集的凸包。

3.根据权利要求2所述的基于伪距离函数的工业机器人路径规划方法，其特征在于，所述步骤4包括：定义父节点为θ_parent，新节点为θ_new，在父节点与新节点之间生成一段路径，表达式为：

T(θ_parent,θ_new)＝{λθ_new+(1-λ)θ_parent|0≤λ≤1}

将该路径离散化，得到一系列位姿点θ_parent,θ₁,θ₂,...,θ_n,θ_new，其中||θ_i+1-θ_i||₂＝0.05，λ为系数，对位姿点使用机器人的运动学方程求得每个连杆的位置，确定机械臂每一个连杆的顶点集合A_i(θ)，使用GJK算法判断A_i(θ)与B_j发生碰撞，则将该位形记录为θ_collision并执行步骤5。