[发明专利]机械臂的运动路径的确定方法和装置

权利要求书

1.一种机械臂的运动路径的确定方法，其特征在于，包括：

从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点；

根据障碍物模型、所述抓取部与障碍物安全阈值，对所述当前节点的邻居节点中的无效节点进行过滤，所述无效节点包括碰撞点和超出所述机械臂运动范围的节点；

计算过滤后的邻居节点的代价估计，从所述过滤后的邻居节点中确定代价估计最小的邻居节点为所述当前节点的下一节点；

当所述下一节点与目标点之间的距离小于预设收敛阈值时，根据所述起始点、目标点以及所述起始点和所述目标点之间所确定的所有所述下一节点确定所述机械臂的抓取部的运动路径；

其中，所述机械臂包括四个轴，所述四个轴中至少包括用于控制所述抓取部高度的轴，从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点之前，还包括：

从所述障碍物模型包括的障碍物中删除无效障碍物，所述无效障碍物为高度小于所述起始点的用于控制所述抓取部高度的轴的位置参数和所述目标点的用于控制所述抓取部高度的轴的位置参数中的较大值的障碍物；

删除所述起始点的用于控制所述抓取部高度的轴的位置参数和所述目标点的用于控制所述抓取部高度的轴的位置参数；

根据所述起始点、目标点以及所述起始点和所述目标点之间所确定所有所述下一节点确定所述机械臂的抓取部的运动路径之后，还包括：

为所述运动路径中的各节点的位置参数增加用于控制所述抓取部高度的轴的位置参数，其中，增加的用于控制所述抓取部高度的轴的位置参数为所述起始点的用于控制所述抓取部高度的轴的位置参数和所述目标点的用于控制所述抓取部高度的轴的位置参数中的较大值。

2.根据权利要求1所述的方法，其特征在于，所述计算过滤后的邻居节点的代价估计，包括：

当没有存储所述邻居节点的信息时，根据以下公式计算所述邻居节点的代价估计：

f(θ')＝g(θ')+h(θ')；

g(θ')＝g(θ)+dis(θ,θ')；

h(θ')＝dis(θ',θ_g)；

其中，f(θ')表示从所述起始点经由所述邻居节点θ'到所述目标点θ_g的代价估计，g(θ')表示从所述起始点到所述邻居节点θ'的实际代价，g(θ)表示从所述起始点到所述当前节点θ的实际代价，dis(θ,θ')表示从所述当前节点θ到所述邻居节点θ'的距离，h(θ')表示从所述邻居节点θ'到所述目标点θ_g的最佳路径的代价估计。

3.根据权利要求1所述的方法，其特征在于，所述计算过滤后的邻居节点的代价估计，包括：

当存储了所述邻居节点的信息时，根据预设的更新条件判断是否更新所述邻居节点的信息，其中，所述预设的更新条件为：

g(θ)+dis(θ,θ')≥g(θ')；

其中，g(θ')表示从所述起始点到所述邻居节点θ'的实际代价，g(θ)表示从所述起始点到所述当前节点θ的实际代价，dis(θ,θ')表示所述当前节点到所述邻居节点θ'的距离，g(θ')和g(θ)为存储的值；

当所述预设的更新条件成立时，确定不更新所述邻居节点θ'的信息；

当所述预设的更新条件不成立时，确定更新所述邻居节点θ'的信息，并根据以下公式更新所述邻居节点θ'的f(θ')和g(θ')值：

f(θ')＝g(θ')+h(θ')；

g(θ')＝g(θ)+dis(θ,θ')；

其中，f(θ')表示从所述起始点经由所述邻居节点θ'到所述目标点的代价估计；

h(θ')表示从所述邻居节点θ'到所述目标点θ_g的最佳路径的代价估计。

4.根据权利要求1所述的方法，其特征在于，从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点之前，还包括：

根据所述机械臂的运动范围，以及所述抓取部与障碍物的距离，判断所述起始点和所述目标点的位置是否合理。