CN108839019B - 机械臂的运动路径的确定方法和装置 - Google Patents

机械臂的运动路径的确定方法和装置 Download PDF

Info

Publication number
CN108839019B
CN108839019B CN201810664809.2A CN201810664809A CN108839019B CN 108839019 B CN108839019 B CN 108839019B CN 201810664809 A CN201810664809 A CN 201810664809A CN 108839019 B CN108839019 B CN 108839019B
Authority
CN
China
Prior art keywords
node
target point
point
starting point
neighbor node
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810664809.2A
Other languages
English (en)
Other versions
CN108839019A (zh
Inventor
杨开红
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Original Assignee
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangzhou Shiyuan Electronics Thecnology Co Ltd filed Critical Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority to CN201810664809.2A priority Critical patent/CN108839019B/zh
Publication of CN108839019A publication Critical patent/CN108839019A/zh
Application granted granted Critical
Publication of CN108839019B publication Critical patent/CN108839019B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明提供一种机械臂的运动路径的确定方法和装置，控制装置从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点，根据障碍物模型、抓取部与障碍物安全阈值，对当前节点的邻居节点中的无效节点进行过滤，计算过滤后的邻居节点的代价估计，从过滤后的邻居节点中确定代价估计最小的邻居节点为当前节点的下一节点，当下一节点与目标点之间的距离小于预设收敛阈值时，根据起始点、目标点以及起始点和目标点之间所确定所有下一节点确定机械臂的抓取部的运动路径，所述方法能够找到最优的运动路径。

Description

机械臂的运动路径的确定方法和装置
技术领域
本发明涉及智能装备技术领域，尤其涉及一种机械臂的运动路径的确定方法和装置。
背景技术
机械臂作为一种自动化机械装置，广泛应用在工业制造、医学治疗、娱乐服务、军事、半导体制造以及太空探索等领域。机械臂的形态多种多样，但它们都有一个共同的特点，就是能够接受指令，精确地定位到三维空间上的某一点进行作业。
机械臂由一个位置点移动到另一个位置点所经过的路径称为机械臂的运动路径，机械臂在工作之前需要规划机械臂的运动路径，机械臂的运动路径规划是指：在已知机械臂的模型和周围障碍物模型的情况下，给定起始点和目标点，找到一条合适的路径连接起始点和目标点，并且在这条路径的任何一个中间点上机械臂必须是无碰撞的，碰撞包括机械臂和障碍物的碰撞以及自身轴之间的碰撞。现有的路径规划采用人工势场法，人工势场法的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。
应用人工势场法规划出来的路径一般是比较平滑并且安全的，但是这种方法容易陷入局部极值点。虽然可以通过增加路标点作为临时的目标点逃离局部极值点，但在一些情况下这种技巧仍旧很难奏效，使得路径规划失败。
发明内容
本发明提供一种机械臂的运动路径的确定方法和装置，能够找到最优的运动路径。
本发明第一方面提供一种机械臂的运动路径的确定方法，包括：
从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点；
根据障碍物模型、所述抓取部与障碍物安全阈值，对所述当前节点的邻居节点中的无效节点进行过滤，所述无效节点包括碰撞点和超出所述机械臂运动范围的节点；
计算过滤后的邻居节点的代价估计，从所述过滤后的邻居节点中确定代价估计最小的邻居节点为所述当前节点的下一节点；
当所述下一节点与所述目标点之间的距离小于预设收敛阈值时，根据所述起始点、目标点以及所述起始点和所述目标点之间所确定所有所述下一节点确定所述机械臂的抓取部的运动路径。
可选的，所述机械臂包括四个轴，所述四个轴中至少包括用于控制所述抓取部高度的轴，从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点之前，还包括：
从所述障碍物模型包括的障碍物中删除无效障碍物，所述无效障碍物为高度小于所述起始点和所述目标点的用于控制所述抓取部高度的轴的位置参数中的较大值的障碍物；
删除所述起始点和所述目标点的用于控制所述抓取部高度的轴的位置参数。
可选的，所述计算过滤后的邻居节点的代价估计，包括：
当没有存储所述邻居节点的信息时，根据以下公式计算所述邻居节点的代价估计：
f(θ')＝g(θ')+h(θ')；
g(θ')＝g(θ)+dis(θ,θ')；
h(θ')＝dis(θ',θg)；
其中，f(θ')表示从所述起始点经由所述邻居节点θ'到所述目标点θg的代价估计，g(θ')表示从所述起始点到所述邻居节点θ'的实际代价，g(θ)表示从所述起始点到所述当前节点θ的实际代价，dis(θ,θ')表示从所述当前节点θ到所述邻居节点θ'的距离，h(θ')表示从所述邻居节点θ'到所述目标点θg的最佳路径的代价估计。
可选的，所述计算过滤后的邻居节点的代价估计，包括：
当存储了所述邻居节点的信息时，根据预设的更新条件判断是否更新所述邻居节点的信息，其中，所述预设的更新条件为：
g(θ)+dis(θ,θ')≥g(θ')；
其中，g(θ')表示从所述起始点到所述邻居节点θ'的实际代价，g(θ)表示从所述起始点到所述当前节点θ的实际代价，dis(θ,θ')表示所述当前节点到所述邻居节点θ'的距离，g(θ')和g(θ)为存储的值；
当所述预设的更新条件成立时，确定不更新所述邻居节点θ'的信息；
当所述预设的更新条件不成立时，确定更新所述邻居节点θ'的信息，并根据以下公式更新所述邻居节点θ'的f(θ')和g(θ')值：
f(θ')＝g(θ')+h(θ')；
g(θ')＝g(θ)+dis(θ,θ')；
其中，f(θ')表示从所述起始点经由所述邻居节点θ'到所述目标点的代价估计。
可选的，从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点之前，还包括：
根据所述机械臂的运动范围，以及所述抓取部与障碍物的距离，判断所述起始点和所述目标点的位置是否合理。
可选的，所述根据所述机械臂的运动范围，以及所述抓取部与障碍物的距离，判断所述起始点和所述目标点的位置是否合理，包括：
判断所述起始点和所述目标点是否满足预设条件，所述预设条件包括：所述起始点和所述目标点位于所述机械臂的运动范围内，所述抓取部位于所述起始点时所述抓取部与所述障碍物模型中的所有障碍物的距离均大于所述抓取部与障碍物安全阈值，以及抓取部位于所述目标点时所述抓取部与所述障碍物模型中的所有障碍物的距离均大于所述抓取部与障碍物安全阈值；
当所述起始点和所述目标点满足所述预设条件时，判断所述起始点和所述目标点的距离是否大于或等于所述预设收敛阈值；
当所述起始点和所述目标点的距离大于或等于所述预设收敛阈值时，确定所述起始点和所述目标点的位置合理；
当所述起始点和所述目标点的距离小于所述预设收敛阈值时，返回所述起始点和所述目标点的位置不合理；
当所述起始点和所述目标点不满足所述预设条件时，返回所述起始点和所述目标点的位置不合理。
可选的，根据所述起始点、目标点以及所述起始点和所述目标点之间所确定所有所述下一节点确定所述机械臂的抓取部的运动路径之后，还包括：
为所述动路径中的各节点的位置参数增加用于控制所述抓取部高度的轴的位置参数，其中，增加的用于控制所述抓取部高度的轴的位置参数为所述起始点和所述目标点的用于控制所述抓取部高度的轴的位置参数中的较大值。
本发明第二方面提供一种机械臂的运动路径的确定装置，包括用于执行本发明第一方面提供的方法的功能模块。
本发明第三方面提供一种机械臂的运动路径的确定装置，包括处理器、存储器和收发器，所述存储器用于存储指令，所述收发器用于和其他设备通信，所述处理器用于执行所述存储器中存储的指令，以使所述业务服务器执行如本发明第一方面所述的方法。
本发明第四方面提供一种计算机可读存储介质，所述计算机可读存储介质存储有指令，当所述指令被执行时，使得计算机执行如本发明第一方面所述的方法。
本发明提供的机械臂的运动路径的确定方法和装置，控制装置从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点，根据障碍物模型、抓取部与障碍物安全阈值，对当前节点的邻居节点中的无效节点进行过滤，计算过滤后的邻居节点的代价估计，从过滤后的邻居节点中确定代价估计最小的邻居节点为当前节点的下一节点，当下一节点与目标点之间的距离小于预设收敛阈值时，根据起始点、目标点以及起始点和目标点之间所确定所有下一节点确定机械臂的抓取部的运动路径，所述方法能够找到最优的运动路径。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。
图1为本发明实施例一提供的机械臂的运动路径的确定方法的流程图；
图2为四轴机械臂运动空间示意图；
图3为本发明实施例二提供的机械臂的运动路径的确定方法的流程图；
图4为本发明实施例三提供的机械臂的运动路径的确定方法的流程图；
图5为本发明实施例四提供的机械臂的运动路径的确定方法的流程图；
图6为本发明实施例五提供的机械臂的运动路径的确定装置的结构示意图；
图7为本发明实施例六提供的机械臂的运动路径的确定装置的结构示意图；
图8为本发明实施例七提供的机械臂的运动路径的确定装置的结构示意图。
通过上述附图，已示出本发明明确的实施例，后文中将有更详细的描述。这些附图和文字描述并不是为了通过任何方式限制本发明构思的范围，而是通过参考特定实施例为本领域技术人员说明本发明的概念。
具体实施方式
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。
图1为本发明实施例一提供的机械臂的运动路径的确定方法的流程图，本实施例的方法应用在四轴机械臂中，由机械臂的控制装置执行，如图1 所示，本实施例的方法包括以下步骤：
步骤S101、从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点。
本实施例中，机械臂包括四个轴，示例性的，第一轴和第二轴用于控制机械臂的抓取部的水平运动，第三轴用于控制抓取部的垂直运动，第四轴用于控制抓取部的水平旋转运动，抓取部也称为手抓。在步骤S101之前，需要获取机械臂模型和障碍物模型。
其中，机械臂模型包括：机械臂的轴的个数、每个轴的长度、每个轴的运动范围等。四轴机械臂在任何姿态下，四个轴都不可能相撞，也即两个轴之间的距离恒为正，所以只需要计算机械臂末端的抓取部与障碍物的距离，抓取部与障碍物的距离即为机械臂与障碍物的距离。
机械臂有一个基座，在某些位姿，手抓可能与基座产生碰撞，所以将基座建模为障碍物。本实施例中，障碍物模型包括：障碍物的几何形状、尺寸大小以及坐标等。图2为四轴机械臂运动空间示意图，如图2所示，机械臂运动空间中存在J、K、L、M、N以及机械臂的底座O等障碍物，起始点为 A，目标点为B。
在建立障碍物模型时，可以通过以下方式获取障碍物的坐标：建立一个三维坐标系，该三维坐标系的原点为机械臂底座的中心，将水平面上两个相互垂直的方向设为X轴和Y轴，将竖直方向设为Z轴。在三维坐标系下，障碍物的分布情况可以用坐标(x,y,z)表示，不同障碍物的坐标不同。可选的，为了方便管理障碍物，可以对障碍物进行编号。
每个轴都有一个预设步长，不同轴的预设步长可能不同，根据当前节点和预设步长，可以得到当前节点的邻居节点，当前节点的邻居节点为多个，本实施例中，起始点为第一个当前节点。
步骤S102、根据障碍物模型、抓取部与障碍物安全阈值，对当前节点的邻居节点中的无效节点进行过滤。
其中，抓取部与障碍物安全阈值可以预先配置好，该无效节点包括碰撞点和超出机械臂运动范围的邻居节点，碰撞点是指与障碍物发生碰撞的邻居节点。控制装置可以通过以下方式判断一个邻居节点是否为无效节点：先判断邻居节点是否超出了机械臂的运动范围，机械臂的运动范围为机械臂能够移动到的最远距离，如果邻居节点超出了机械臂的运动范围，机械臂无论如何移动也不可能达到该邻居节点，因此，确定该邻居节点为无效节点。如果邻居节点没有超出机械臂的运动范围，则进一步判断该邻居节点与障碍物模型中的所有障碍物是否都不会发生碰撞，如果该邻居节点与障碍物模型中的所有障碍物都不会发生碰撞，则确定该邻居节点为一个有效节点，继续进行后续步骤，如果该邻居节点与障碍物模型中的任意一个障碍物发生了碰撞，则确定该邻居节点为碰撞点，碰撞点为无效点。
在判断邻居节点与障碍物模型中的障碍物是否会发生碰撞时，需要该邻居节点与每个障碍物的距离，然后判断该邻居节点与障碍物的距离是否小于抓取部与障碍物安全阈值，如果该邻居节点与障碍物的距离小于抓取部与障碍物安全阈值，说明抓取部位于该邻居节点时不会与障碍物发生碰撞，该邻居节点为有效节点，如果该邻居节点与障碍物的距离大于或等于抓取部与障碍物安全阈值，说明抓取部位于该邻居节点时会与障碍物发生碰撞，该邻居节点为碰撞点。
其中，邻居点与障碍物的距离是指抓取部位于该邻居点时抓取部与障碍物的距离，在计算抓取部与障碍物的距离时，需要已知机械臂的正运动学模型，抓取部模型以及障碍物的模型。其中，正运动学模型包括机械臂的轴的长度和连接关系。抓取部模型包括抓取部的大小、形状以及抓取部在机械臂上的固定位置等。通过正运动学模型计算出抓取部在空间中的分布，再通过投影方法或其他方法计算得到抓取部与每个障碍物的距离。抓取部与障碍物的距离可正可负，如果抓取部与障碍物不相撞，则抓取部与障碍物的距离为正，如果抓取部与障碍物相撞，则抓取部与障碍物的距离为负，并且其绝对值的大小表征了抓取部与障碍物碰撞的程度。
步骤S103、计算过滤后的邻居节点的代价估计，从过滤后的邻居节点中确定代价估计最小的邻居节点为当前节点的下一节点。
本实施例中，可以在确定当前节点的邻居节点之后，对当前节点的所有邻居节点中的无效节点进行过滤，然后分别计算每个过滤后的邻居节点的代价估计。也可以在计算每个邻居节点的代价估计之前，该对该邻居节点进行过滤。
本实施例中，可以采用A*算法(A-Star)算法确定机械臂的运动路径，A* 算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。A*算法采用如下公式计算节点的代价估计：
f(θ)＝g(θ)+h(θ)；
其中，f(θ)表示从起始点经由节点θ到目标点的代价估计；g(θ)表示在从起始点到节点θ的实际代价；h(θ)表示从节点θ到目标点的最佳路径的代价估计，h(θ)称为的θ启发式函数。
可选的，
Figure BDA0001707372430000071
其中，dis(θ,θg)表示从节点θ到目标点的带权值的欧式距离，当然，h(θ)的计算方式不限于欧式距离，还可以采用曼哈顿距离等。
具体的，当没有存储邻居节点的信息时，根据以下公式计算邻居节点的代价估计：
f(θ')＝g(θ')+h(θ')；
g(θ')＝g(θ)+dis(θ,θ')；
h(θ')＝dis(θ',θg)；
其中，f(θ')表示邻居节点的代价估计，代价估计表示从起始点经由邻居节点θ'到目标点θg的代价估计，g(θ')表示从起始点到邻居节点θ'的实际代价， g(θ)表示从起始点到当前节点θ的实际代价，dis(θ,θ')表示从当前节点θ到邻居节点θ'的距离，h(θ')表示从邻居节点θ'到目标点θg的最佳路径的代价估计。 g(θ)为已知量，dis(θ,θ')可以根据当前节点θ和邻居节点θ'的位置参数计算得到，h(θ')根据邻居节点θ'和目标点θg的位置参数计算得到。
当当前节点为起始节点时，g(θs)为零，f(θs)＝h(θs)＝dis(θs,θg)，其中，f(θs) 表示从起始点θs到目标点θg的代价估计，h(θs)表示从起始点θs到目标点θg的最佳路径的代价估计，dis(θs,θg)表示起始点θs到目标点θg之间的距离。
当存储了邻居节点的信息时，根据预设的更新条件判断是否更新邻居节点的信息，该邻居节点的信息包括该邻居节点的位置参数以及f(θ')和g(θ')。
其中，该预设的更新条件为：g(θ)+dis(θ,θ')≥g(θ')；
其中，g(θ')表示从起始点到邻居节点θ'的实际代价，g(θ)表示从起始点到前节点θ的实际代价，dis(θ,θ')表示当前节点到邻居节点的距离，g(θ')和g(θ) 为存储的值；
当预设的更新条件成立时，确定不更新邻居节点的信息，即存储的邻居节点θ'的f(θ')和g(θ')不变，其中，f(θ')表示从起始点经由邻居节点θ'到目标点的代价估计，g(θ')表示从起始点到邻居节点θ'的实际代价；
当预设的更新条件不成立时，确定更新所述邻居节点θ'的信息，并根据以下公式更新邻居节点θ'的f(θ')和g(θ')值：
f(θ')＝g(θ')+h(θ')；
g(θ')＝g(θ)+dis(θ,θ')。
其中，当该预设的更新条件成立时，说明从到前节点θ到该邻居节点θ'使得路径的代价估计增大了或者不变，那么该路径不是最优路径，当该预设的更新条件不成立，说明从到前节点θ到该邻居节点θ'使得路径的代价估计减小了，即有更优的路径出现了，因此，更新该邻居节点θ'的f(θ')和g(θ')值。
当采用A*算法时，控制装置可以判断邻居节点是否在OPEN列表中，如果邻居节点在OPEN列表中，则说明存储了邻居节点的信息，如果邻居节点不在OPEN列表中，则说明没有存储邻居节点的信息，OPEN列表用于存储未访问过的节点的信息。
步骤S104、当下一节点与目标点之间的距离小于预设收敛阈值时，根据起始点、目标点以及起始点和目标点之间所确定所有下一节点确定机械臂的抓取部的运动路径。
如果下一节点与目标点之间的距离不小于预设收敛阈值，则将当前节点的下一节点更新为当前节点，返回执行步骤S101-S103直至下一节点与目标点之间的距离小于预设收敛阈值。
控制装置根据起始点和目标点之间所确定所有下一节点的顺序，得到运动路径，由于每次确定当前节点的下一节点时，都选取代价估计最小的下一节点，从而能够保证确定的运动路径的代价估计最小，该运动路径为最优路径。
本实施例中，控制装置从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点，根据障碍物模型、抓取部与障碍物安全阈值，对当前节点的邻居节点中的无效节点进行过滤，计算过滤后的邻居节点的代价估计，从过滤后的邻居节点中确定代价估计最小的邻居节点为当前节点的下一节点，当下一节点与目标点之间的距离小于预设收敛阈值时，根据起始点、目标点以及起始点和目标点之间所确定所有下一节点确定机械臂的抓取部的运动路径，所述方法能够找到最优路径。
图3为本发明实施例二提供的机械臂的运动路径的确定方法的流程图，本实施例在实施例一的基础上，在运动路径规划之前，对每个节点的位置参数进行降维处理，以减少算法的执行时间，如图3所示，本实施例提供的方法包括以下步骤：
步骤S201、判断起始点和目标点的位置是否合理。
本实施例中，根据机械臂的运动范围，以及抓取部与障碍物的距离，判断起始点和目标点的位置是否合理，当起始点和目标点的位置合理时，执行步骤S202，当起始点和目标点的位置不合理时，执行步骤S209。其中，步骤S201为可选步骤，如果用户一开始就输入合理的起始点和目标点则不需要判断起始点和目标点的位置是否合理。
示例性的，可以通过以下方式判断起始点和目标点的位置是否合理：判断起始点和目标点是否满足预设条件，该预设条件包括：起始点和目标点位于机械臂的运动范围内，抓取部位于起始点时抓取部与障碍物模型中的所有障碍物的距离均大于抓取部与障碍物安全阈值，以及抓取部位于目标点时抓取部与障碍物模型中的所有障碍物的距离均大于抓取部与障碍物安全阈值；
当起始点和目标点满足预设条件时，判断起始点和目标点的距离是否大于或等于预设收敛阈值，当起始点和目标点的距离大于或等于预设收敛阈值时，确定起始点和所述目标点的位置合理，当起始点和目标点的距离小于预设收敛阈值时，返回起始点和目标点的位置不合理。当起始点和目标点不满足预设条件时，返回起始点和目标点的位置不合理。
其中，在判断起始点和目标点是否满足预设条件时，先判断起始点或目标点是否超出机械臂的运动范围，如果起始点或目标点超出机械臂的运动范围，则返回起始点和目标点的位置不合理。如果起始点和目标点没有超出机械臂的运动范围，则判断抓取部位于起始点时，抓取部与障碍物模型中的所有障碍物的距离是否均大于抓取部与障碍物安全阈值。当抓取部与障碍物模型中的所有障碍物的距离均大于抓取部与障碍物安全阈值时，判断抓取部位于目标点时，抓取部与障碍物模型中的所有障碍物的距离是否均大于抓取部与障碍物安全阈值，当抓取部与障碍物模型中的所有障碍物的距离均大于抓取部与障碍物安全阈值时，确定起始点和目标点满足预设条件。如果抓取部位于起始点时，抓取部与障碍物模型中的任一障碍物的距离小于或等于抓取部与障碍物安全阈值，则确定起始点和目标点不满足预设条件，也不需要进行一步判断抓取部位于目标点时，抓取部与障碍物模型中的所有障碍物的距离是否均大于抓取部与障碍物安全阈值。
步骤S202、从障碍物模型包括的障碍物中删除无效障碍物。
该无效障碍物为高度小于起始点和目标点的用于控制抓取部高度的轴的位置参数中的较大值的障碍物。在四轴机械臂中用于控制抓取部高度的轴仅改变手抓的高度，不改变手抓的位置和姿态，所以在运动路径的确定过程中，可以将其剥离出去，仅仅考虑其他三轴。因此，本实施例中在运动路径的确定过程中，当前节点以及当前节点的邻居节点的位置参数中不包括用于控制抓取部高度的轴的位置参数。
例如，某个节点的位置参数为θ＝(θ1,θ2,θ3,θ4)，其中，θ1表示该节点上第一轴的参数，θ2表示该节点上第二轴的参数，θ3表示该节点上第三轴的参数，θ4表示该节点上第四轴的参数，其中，第三轴为用于控制抓取部的高度，通过降维得到该节点的位置参数为θ＝(θ1,θ2,θ4)
步骤S203、删除起始点和目标点中用于控制抓取部高度的轴的位置参数。
控制装置还需要保存起始点和目标点的用于控制抓取部高度的轴的位置参数，以便于运动路径确定完成后的处理过程中需要。
步骤S204、从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点。
步骤S205、根据障碍物模型、抓取部与障碍物安全阈值，对当前节点的邻居节点中的无效节点进行过滤。
无效节点包括碰撞点和超出机械臂运动范围的节点。
步骤S206、计算过滤后的邻居节点的代价估计，从过滤后的邻居节点中确定代价估计最小的邻居节点为当前节点的下一节点。
步骤S207、当下一节点与目标点之间的距离小于预设收敛阈值时，根据起始点、目标点以及起始点和目标点之间所确定所有下一节点确定机械臂的抓取部的运动路径。
步骤S204-S207的具体实现方式参照实施例一的相关描述，这里不再赘述。不同点是，本实施例中在确定运动路径的过程中，使用的是节点的位置参数不是四维位置参数，而是降维后的三维位置参数，从而降低了域的运动路径规划过程中的计算复杂度，减少了计算时间。
步骤S208、为动路径中的各节点的位置参数中增加用于控制抓取部高度的轴的位置参数。
步骤S209、返回路径规划失败。
其中，增加的用于控制抓取部高度的轴的位置参数为起始点和目标点的用于控制抓取部高度的轴的位置参数中的较大值，起始点和目标点的用于控制抓取部高度的轴的位置参数是在绛维过程中保存的。
本实施例中，在运动路径规划之前，从障碍物集合中删除无效障碍物，无效障碍物为高度小于起始点和目标点的用于控制抓取部高度的轴的位置参数中的较大值的障碍物，以及删除起始点和目标点中用于控制抓取部高度的轴的位置参数，后续在路径规划过程中使用绛维后的位置参数，降低了计算复杂度，减少了计算时间。
图4为本发明实施例三提供的机械臂的运动路径的确定方法的流程图，本实施例在实施例一和实施例二的基础上，以A*算法为例进行说明，如图 4所示，本实施例提供的方法包括以下步骤：
步骤S301、判断起始点和目标点的位置是否合理。
本实施例中，可以根据机械臂的运动范围，以及抓取部与障碍物的距离，判断起始点和目标点的位置是否合理，当起始点和目标点的位置合理时，执行步骤S302，当起始点和目标点的位置不合理时，执行步骤S311。
步骤S302、从障碍物模型包括的障碍物中删除无效障碍物。
该无效障碍物为高度小于起始点和目标点的用于控制抓取部高度的轴的位置参数中的较大值的障碍物。
步骤S303、删除起始点和目标点中用于控制抓取部高度的轴的位置参数。
控制装置还需要保存起始点和目标点的用于控制抓取部高度的轴的位置参数，以便于运动路径确定完成后的处理过程中需要。
步骤S301-S303的具体实现方式参照实施例二中步骤S201-S203的具体实现方式，这里不再赘述。
步骤S304、将起始点的信息加入OPEN列表。
OPEN列表用于保存未访问的节点的信息，其中，起始点的信息包括起始点的位置参数和起始点的代价估计f(θ)，其中，起始点的代价估计 f(θs)＝h(θs)＝dis(θs,θg)，其中，f(θs)表示从起始点θs到目标点θg的代价估计， h(θs)表示从起始点θs到目标点θg的最佳路径的代价估计，dis(θs,θg)表示起始点θs到目标点θg之间的距离。
步骤S305、从OPEN列表中取出代价估计最小的节点作为当前节点，并将当前节点的信息加入CLOSED列表。
CLOSED列表用于保存已经访问过的节点的信息，已经访问过的节点的信息包括节点的位置参数、节点的代价估计f(θ)、节点的父子关系等。初始计算时OPEN列表中只有初始节点的信息，因此，初始计算时当前节点为起始点。
步骤S306、判断当前节点与目标点的距离是否小于预设收敛阈值。
当当前节点与目标点的距离大于或等于预设收敛阈值时，执行步骤S307，当当前节点与目标点的距离小于预设收敛阈值时，执行步骤S309。
步骤S307、根据存储的已经访问过的节点的父子关系确定机械臂的抓取部的运动路径。
确定运动路径的方式是从终止搜索处的节点开始回溯父节点，假设此处的节点是θn，节点θn的父节点是θn-1，节点θn-1的父节点是θn-2，依此类推，节点θ1的父节点为起始点θs，那么提取到的路径就是{θs,θ1,…,θn,θg}。
步骤S308、为动路径中的各节点的位置参数增加用于控制抓取部高度的轴的位置参数。
步骤S307之后执行步骤S308，步骤S307中得到的运动路径中每个节点的位置参数为三维位置参数，因此，需要为动路径中的各节点的位置参数增加用于控制抓取部高度的轴的位置参数，使得每个节点的位置参数变为四维位置参数，其中，增加的用于控制抓取部高度的轴的位置参数为起始点和目标点的用于控制抓取部高度的轴的位置参数中的较大值。
可选的，如果运动路径上的第一个节点与起始点不同(用于控制抓取部高度的轴的位置参数可能不一致，其他轴的位置参数必定一致)，则直接在第一个节点的前面加入起始点即可。如果运动路径的最后一个节点与目标点不同，则直接在最后一个节点的后面加入目标点即可。
步骤S309、根据预设步长，确定当前节点的邻居节点。
其中，每个机械臂的每个轴都设置了一个预设步长，预设步长可以在步骤S301之前，由用户输入，当前节点的邻居节点为多个。
步骤S310、对每个邻居节点进行运动检验，得到邻居节点的父子关系。
步骤S311、返回路径规划失败。
图5为本发明实施例四提供的机械臂的运动路径的确定方法的流程图，本实施例在实施例三的基础上，对步骤S310中的运动检验过程进行详细说明，如图5所示，本实施例提供的方法包括以下步骤：
步骤S3101、判断邻居节点是否在CLOSED列表中。
如果邻居节点在CLOSED列表中，说明邻居节点已经作为当前节点处理过，则执行步骤S3109，如果邻居节点不在CLOSED列表中，则执行步骤S3102。
步骤S3102、判断邻居节点的位置是否合理。
可以通过如下方式判断邻居节点的位置是否合理：先判断邻居节点是否超出了机械臂的运动范围，如果邻居节点超出了机械臂的运动范围，则确定邻居节点的位置不合理，如果邻居节点没有超出机械臂的运动范围，则判断抓取部位于邻居节点时，抓取部与障碍物模型中包括的障碍物的距离是否均大于抓取部与障碍物安全阈值，如果抓取部与障碍物模型中包括的障碍物的距离均大于抓取部与障碍物安全阈值，则确定邻居节点的位置合理，如果抓取部与障碍物模型中包括的任一障碍物的距离小于或等于抓取部与障碍物安全阈值，则确定邻居节点的位置不合理。如果邻居节点的位置合理，则执行步骤S3103，如果邻居节点的位置不合理，则执行步骤S3109。
步骤S3103、计算当前节点和邻居节点的距离。
当前节点和邻居节点的距离可以是欧式距离，还可以是曼哈顿距离等。
步骤S3104、判断邻居节点是否在OPEN列表中。
如果邻居节点不在OPEN列表中，则执行步骤S3105，如果邻居节点在 OPEN列表中，则执行步骤S3106。在运动路径的确定过程中，两个节点的邻居节点可能相同，如果两个节点的邻居节点相同，则在后访问的节点邻居节点可能已经被加入OPEN列表中了。
步骤S3105、计算邻居节点的代价估计，并将邻居节点的信息加入OPEN 列表中。
可以根据以下公式计算邻居节点的代价估计：
f(θ')＝g(θ')+h(θ')；
g(θ')＝g(θ)+dis(θ,θ')；
h(θ')＝dis(θ',θg)；
其中，f(θ')表示邻居节点的代价估计，代价估计表示从起始点经由所述邻居节点θ'到目标点θg的代价估计，g(θ')表示从起始点到邻居节点θ'的实际代价，g(θ)表示从起始点到当前节点θ的实际代价，dis(θ,θ')表示从当前节点θ到邻居节点θ'的距离，h(θ')表示从邻居节点θ'到目标点θg的最佳路径的代价估计。
将邻居节点的信息加入OPEN列表中表示已经访问过该邻居节点，邻居节点的信息包括邻居节点的位置参数、邻居节点的代价估计、从起始点到邻居节点的实际代价等。
步骤S3105之后执行步骤S3108。
步骤S3106、判断是否需要更新邻居节点的信息。
示例性的，根据预设的更新条件判断是否更新所述邻居节点的信息，该预设的更新条件为：
g(θ)+dis(θ,θ')≥g(θ')；
其中，g(θ')表示从起始点到邻居节点θ'的实际代价，g(θ)表示从起始点到当前节点θ的实际代价，dis(θ,θ')表示当前节点到邻居节点θ'的距离，g(θ')和 g(θ)为上一次访问邻居节点θ'时保存值，g(θ')和g(θ)可以从OPEN列表和 CLOSED列表中获取。
当预设的更新条件成立时，确定不更新邻居节点θ'的信息，即邻居节点θ' 的f(θ')和g(θ')不变，执行步骤S3109。当预设的更新条件不成立时，确定更新邻居节点θ'的信息，则执行步骤S3107。
步骤S3107、更新邻居节点的信息。
可以根据以下公式更新邻居节点θ'的f(θ')和g(θ')值：
f(θ')＝g(θ')+h(θ')；
g(θ')＝g(θ)+dis(θ,θ')；
其中，f(θ')表示从起始点经由邻居节点θ'到目标点的代价估计，g(θ')表示从起始点到邻居节点θ'的实际代价。
步骤S3107之后执行步骤S3108。
步骤S3108、将当前节点设置为邻居节点的父节点。
设置父节点的目的是为了后续提取运动路径时方便回溯。
步骤S3109、结束。
本实施例中，对当前节点的每个节点都执行步骤S3101至S3109的方法。通过上述方式确定的运动路径可能为多条，从中选取一条代价估计最小的运动路径。
图6为本发明实施例五提供的机械臂的运动路径的确定装置的结构示意图，如图6所示，本实施例提供的装置包括：
确定模块11，用于从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点；
过滤模块12，根据障碍物模型、所述抓取部与障碍物安全阈值，对所述当前节点的邻居节点中的无效节点进行过滤，所述无效节点包括碰撞点和超出所述机械臂运动范围的节点；
代价估计模块13，用于计算过滤后的邻居节点的代价估计，从所述过滤后的邻居节点中确定代价估计最小的邻居节点为所述当前节点的下一节点；
路径提取模块14，用于当所述下一节点与所述目标点之间的距离小于预设收敛阈值时，根据所述起始点、目标点以及所述起始点和所述目标点之间所确定所有所述下一节点确定所述机械臂的抓取部的运动路径。
可选的，所述代价估计模块13，具体用于：
当没有存储所述邻居节点的信息时，根据以下公式计算所述邻居节点的代价估计：
f(θ')＝g(θ')+h(θ')；
g(θ')＝g(θ)+dis(θ,θ')；
h(θ')＝dis(θ',θg)；
其中，f(θ')表示从所述起始点经由所述邻居节点θ'到所述目标点θg的代价估计，g(θ')表示从所述起始点到所述邻居节点θ'的实际代价，g(θ)表示从所述起始点到所述当前节点θ的实际代价，dis(θ,θ')表示从所述当前节点θ到所述邻居节点θ'的距离，h(θ')表示从所述邻居节点θ'到所述目标点θg的最佳路径的代价估计。
或者，所述代价估计模块13，具体用于：
当存储了所述邻居节点的信息时，根据预设的更新条件判断是否更新所述邻居节点的信息，其中，所述预设的更新条件为：
g(θ)+dis(θ,θ')≥g(θ')；
其中，g(θ')表示从所述起始点到所述邻居节点θ'的实际代价，g(θ)表示从所述起始点到所述当前节点θ的实际代价，dis(θ,θ')表示所述当前节点到所述邻居节点θ'的距离，g(θ')和g(θ)为存储的值；
当所述预设的更新条件成立时，确定不更新所述邻居节点θ'的信息；
当所述预设的更新条件不成立时，确定更新所述邻居节点θ'的信息，并根据以下公式更新所述邻居节点θ'的f(θ')和g(θ')值：
f(θ')＝g(θ')+h(θ')；
g(θ')＝g(θ)+dis(θ,θ')；
其中，f(θ')表示从所述起始点经由所述邻居节点θ'到所述目标点的代价估计。
可选的，所述过滤模块12，还用于：
根据所述机械臂的运动范围，以及所述抓取部与障碍物的距离，判断所述起始点和所述目标点的位置是否合理。
可选的，所述过滤模块12具体用于：
判断所述起始点和所述目标点是否满足预设条件，所述预设条件包括：所述起始点和所述目标点位于所述机械臂的运动范围内，所述抓取部位于所述起始点时所述抓取部与所述障碍物模型中的所有障碍物的距离均大于所述抓取部与障碍物安全阈值，以及抓取部位于所述目标点时所述抓取部与所述障碍物模型中的所有障碍物的距离均大于所述抓取部与障碍物安全阈值；
当所述起始点和所述目标点满足所述预设条件时，判断所述起始点和所述目标点的距离是否大于或等于所述预设收敛阈值；
当所述起始点和所述目标点的距离大于或等于所述预设收敛阈值时，确定所述起始点和所述目标点的位置合理；
当所述起始点和所述目标点的距离小于所述预设收敛阈值时，返回所述起始点和所述目标点的位置不合理；
当所述起始点和所述目标点不满足所述预设条件时，返回所述起始点和所述目标点的位置不合理。
图7为本发明实施例六提供的机械臂的运动路径的确定装置的结构示意图，如图7所示，本实施例提供的装置在图6所示装置的基础上，还包括：删除模块15和增加模块16。
所述机械臂包括四个轴，所述四个轴中至少包括用于控制所述抓取部高度的轴，删除模块15，用于从所述障碍物模型包括的障碍物中删除无效障碍物，所述无效障碍物为高度小于所述起始点和所述目标点的用于控制所述抓取部高度的轴的位置参数中的较大值的障碍物；
所述删除模块，还用于删除所述起始点和所述目标点的用于控制所述抓取部高度的轴的位置参数。
相应的，增加模块16，用于为所述动路径中的各节点的位置参数增加用于控制所述抓取部高度的轴的位置参数，其中，增加的用于控制所述抓取部高度的轴的位置参数为所述起始点和所述目标点的用于控制所述抓取部高度的轴的位置参数中的较大值。
本实施例提供的机械臂的运动路径的确定装置可用于执行实施例一至实施例四所述的方法，具体实现方式和技术效果类似，这里不再赘述。
图8为本发明实施例七提供的机械臂的运动路径的确定装置的结构示意图，如图8所示，本实施例提供的装置包括处理器21、存储器22和收发器23，所述存储器22用于存储指令，所述收发器23用于和其他设备通信，所述处理器21用于执行所述存储器22中存储的指令，以使所述装置执行如实施例一至实施例四所述的方法。
本发明实施例八提供一种计算机可读存储介质，所述计算机可读存储介质存储有指令，当所述指令被执行时，使得计算机执行如实施例一至实施例四所述的方法。
本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本公开的其它实施方案。本发明旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本公开的真正范围和精神由下面的权利要求书指出。

Claims (12)

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所述的方法，其特征在于，从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点之前，还包括：
根据所述机械臂的运动范围，以及所述抓取部与障碍物的距离，判断所述起始点和所述目标点的位置是否合理。
5.根据权利要求4所述的方法，其特征在于，所述根据所述机械臂的运动范围，以及所述抓取部与障碍物的距离，判断所述起始点和所述目标点的位置是否合理，包括：
判断所述起始点和所述目标点是否满足预设条件，所述预设条件包括：所述起始点和所述目标点位于所述机械臂的运动范围内，所述抓取部位于所述起始点时所述抓取部与所述障碍物模型中的所有障碍物的距离均大于所述抓取部与障碍物安全阈值，以及抓取部位于所述目标点时所述抓取部与所述障碍物模型中的所有障碍物的距离均大于所述抓取部与障碍物安全阈值；
当所述起始点和所述目标点满足所述预设条件时，判断所述起始点和所述目标点的距离是否大于或等于所述预设收敛阈值；
当所述起始点和所述目标点的距离大于或等于所述预设收敛阈值时，确定所述起始点和所述目标点的位置合理；
当所述起始点和所述目标点的距离小于所述预设收敛阈值时，返回所述起始点和所述目标点的位置不合理；
当所述起始点和所述目标点不满足所述预设条件时，返回所述起始点和所述目标点的位置不合理。
6.一种机械臂的运动路径的确定装置，其特征在于，包括：
确定模块，用于从机械臂的抓取部的起始点开始以预设步长，确定当前节点的邻居节点；
过滤模块，根据障碍物模型、所述抓取部与障碍物安全阈值，对所述当前节点的邻居节点中的无效节点进行过滤，所述无效节点包括碰撞点和超出所述机械臂运动范围的节点；
代价估计模块，用于计算过滤后的邻居节点的代价估计，从所述过滤后的邻居节点中确定代价估计最小的邻居节点为所述当前节点的下一节点；
路径提取模块，用于当所述下一节点与目标点之间的距离小于预设收敛阈值时，根据所述起始点、目标点以及所述起始点和所述目标点之间所确定的所有所述下一节点确定所述机械臂的抓取部的运动路径；其中，所述机械臂包括四个轴，所述四个轴中至少包括用于控制所述抓取部高度的轴，还包括：
删除模块，用于从所述障碍物模型包括的障碍物中删除无效障碍物，所述无效障碍物为高度小于所述起始点的用于控制所述抓取部高度的轴的位置参数和所述目标点的用于控制所述抓取部高度的轴的位置参数中的较大值的障碍物；
所述删除模块，还用于删除所述起始点的用于控制所述抓取部高度的轴的位置参数和所述目标点的用于控制所述抓取部高度的轴的位置参数；
还包括：
增加模块，用于为所述运动路径中的各节点的位置参数增加用于控制所述抓取部高度的轴的位置参数，其中，增加的用于控制所述抓取部高度的轴的位置参数为所述起始点的用于控制所述抓取部高度的轴的位置参数和所述目标点的用于控制所述抓取部高度的轴的位置参数中的较大值。
7.根据权利要求6所述的装置，其特征在于，所述代价估计模块，具体用于：
当没有存储所述邻居节点的信息时，根据以下公式计算所述邻居节点的代价估计：
f(θ')＝g(θ')+h(θ')；
g(θ')＝g(θ)+dis(θ,θ')；
h(θ')＝dis(θ',θg)；
其中，f(θ')表示从所述起始点经由所述邻居节点θ'到所述目标点θg的代价估计，g(θ')表示从所述起始点到所述邻居节点θ'的实际代价，g(θ)表示从所述起始点到所述当前节点θ的实际代价，dis(θ,θ')表示从所述当前节点θ到所述邻居节点θ'的距离，h(θ')表示从所述邻居节点θ'到所述目标点θg的最佳路径的代价估计。
8.根据权利要求6所述的装置，其特征在于，所述代价估计模块，具体用于：
当存储了所述邻居节点的信息时，根据预设的更新条件判断是否更新所述邻居节点的信息，其中，所述预设的更新条件为：
g(θ)+dis(θ,θ')≥g(θ')；
其中，g(θ')表示从所述起始点到所述邻居节点θ'的实际代价，g(θ)表示从所述起始点到所述当前节点θ的实际代价，dis(θ,θ')表示所述当前节点到所述邻居节点θ'的距离，g(θ')和g(θ)为存储的值；
当所述预设的更新条件成立时，确定不更新所述邻居节点θ'的信息；
当所述预设的更新条件不成立时，确定更新所述邻居节点θ'的信息，并根据以下公式更新所述邻居节点θ'的f(θ')和g(θ')值：
f(θ')＝g(θ')+h(θ')；
g(θ')＝g(θ)+dis(θ,θ')；
其中，f(θ')表示从所述起始点经由所述邻居节点θ'到所述目标点的代价估计；
h(θ')表示从所述邻居节点θ'到所述目标点θg的最佳路径的代价估计。
9.根据权利要求6所述的装置，其特征在于，所述过滤模块，还用于：
根据所述机械臂的运动范围，以及所述抓取部与障碍物的距离，判断所述起始点和所述目标点的位置是否合理。
10.根据权利要求9所述的装置，其特征在于，所述过滤模块具体用于：
判断所述起始点和所述目标点是否满足预设条件，所述预设条件包括：所述起始点和所述目标点位于所述机械臂的运动范围内，所述抓取部位于所述起始点时所述抓取部与所述障碍物模型中的所有障碍物的距离均大于所述抓取部与障碍物安全阈值，以及抓取部位于所述目标点时所述抓取部与所述障碍物模型中的所有障碍物的距离均大于所述抓取部与障碍物安全阈值；
当所述起始点和所述目标点满足所述预设条件时，判断所述起始点和所述目标点的距离是否大于或等于所述预设收敛阈值；
当所述起始点和所述目标点的距离大于或等于所述预设收敛阈值时，确定所述起始点和所述目标点的位置合理；
当所述起始点和所述目标点的距离小于所述预设收敛阈值时，返回所述起始点和所述目标点的位置不合理；
当所述起始点和所述目标点不满足所述预设条件时，返回所述起始点和所述目标点的位置不合理。
11.一种机械臂的运动路径的确定装置，其特征在于，包括处理器、存储器和收发器，所述存储器用于存储指令，所述收发器用于和其他设备通信，所述处理器用于执行所述存储器中存储的指令，以使所述机械臂的运动路径的确定装置执行如权利要求1-5任一项所述的方法。
12.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有指令，当所述指令被执行时，使得计算机执行如权利要求1-5任一项所述的方法。
CN201810664809.2A 2018-06-25 2018-06-25 机械臂的运动路径的确定方法和装置 Active CN108839019B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810664809.2A CN108839019B (zh) 2018-06-25 2018-06-25 机械臂的运动路径的确定方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810664809.2A CN108839019B (zh) 2018-06-25 2018-06-25 机械臂的运动路径的确定方法和装置

Publications (2)

Publication Number Publication Date
CN108839019A CN108839019A (zh) 2018-11-20
CN108839019B true CN108839019B (zh) 2020-09-01

Family

ID=64201807

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810664809.2A Active CN108839019B (zh) 2018-06-25 2018-06-25 机械臂的运动路径的确定方法和装置

Country Status (1)

Country Link
CN (1) CN108839019B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109664317B (zh) * 2019-01-24 2022-06-14 深圳勇艺达机器人有限公司 机器人的物体抓取系统及方法
CN109857110A (zh) * 2019-02-13 2019-06-07 广州视源电子科技股份有限公司 运动规划方法、装置、设备及计算机可读存储介质
CN113589809B (zh) * 2021-07-26 2024-04-05 江苏徐工工程机械研究院有限公司 可避障的挖掘机工作装置作业轨迹规划方法及装置

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0155084B1 (en) * 1984-02-16 1988-07-06 Kabushiki Kaisha Toshiba Device for measuring the shape of a three-dimensional object
CN102446122A (zh) * 2011-12-21 2012-05-09 上海电机学院 一种基于包围盒树的碰撞检测方法
CN103676944A (zh) * 2013-12-11 2014-03-26 北京理工大学 基于Dubins路径和稀疏A*搜索的无人机航迹规划方法
CN104155974A (zh) * 2013-07-29 2014-11-19 深圳信息职业技术学院 一种用于机器人快速避碰的路径规划方法及设备
CN107953334A (zh) * 2017-12-25 2018-04-24 深圳禾思众成科技有限公司 一种基于a星算法的工业机械臂无碰撞路径规划方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0155084B1 (en) * 1984-02-16 1988-07-06 Kabushiki Kaisha Toshiba Device for measuring the shape of a three-dimensional object
CN102446122A (zh) * 2011-12-21 2012-05-09 上海电机学院 一种基于包围盒树的碰撞检测方法
CN104155974A (zh) * 2013-07-29 2014-11-19 深圳信息职业技术学院 一种用于机器人快速避碰的路径规划方法及设备
CN103676944A (zh) * 2013-12-11 2014-03-26 北京理工大学 基于Dubins路径和稀疏A*搜索的无人机航迹规划方法
CN107953334A (zh) * 2017-12-25 2018-04-24 深圳禾思众成科技有限公司 一种基于a星算法的工业机械臂无碰撞路径规划方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
曲面工件超声检测的逆运动学及路径规划研究;马华栋;《中国优秀硕士学位论文全文数据库 信息科技辑》;20110715(第07期);第2-52页 *

Also Published As

Publication number Publication date
CN108839019A (zh) 2018-11-20

Similar Documents

Publication Publication Date Title
CN108839019B (zh) 机械臂的运动路径的确定方法和装置
CN110221614B (zh) 一种基于快速探索随机树的多机器人地图探索方法
US10723024B2 (en) Specialized robot motion planning hardware and methods of making and using same
WO2016103297A1 (ja) アーム型のロボットの障害物自動回避方法及び制御装置
CN110986953B (zh) 路径规划方法、机器人及计算机可读存储介质
CN112809682B (zh) 机械臂避障路径规划方法、系统及存储介质
CN110940341B (zh) 路径规划方法、机器人及计算机可读存储介质
US20210370510A1 (en) Robot path planning method with static and dynamic collision avoidance in an uncertain environment
Dalibard et al. Whole-body task planning for a humanoid robot: a way to integrate collision avoidance
EP4157589A1 (en) A robot path planning method with static and dynamic collision avoidance in an uncertain environment
CN113246143A (zh) 一种机械臂动态避障轨迹规划方法及装置
CN108803592B (zh) 用于避免机器人碰撞的方法、装置和系统
CN113253744B (zh) 多机器人协同轨迹规划方法、装置、电子设备和存储介质
CN111766783A (zh) 一种面向集群系统的有限时间内收敛的编队合围跟踪方法
CN111716352B (zh) 一种配电网带电作业机械臂导航避障方法及系统
CN113211447A (zh) 一种基于双向rrt*算法的机械臂实时感知规划方法及系统
CN114326810B (zh) 一种无人机在复杂动态环境下的避障方法
CN114485621A (zh) 导航方法、装置及计算机可读存储介质
Lee et al. Visualization of deep reinforcement autonomous aerial mobility learning simulations
CN116700298B (zh) 路径规划方法、系统、设备及存储介质
CN113524168A (zh) 基于拆卸的组装规划
CN114740862A (zh) 一种用于自动采血机器人的路径规划方法及系统
CN114029940A (zh) 机械臂的运动路径规划方法、装置、设备、介质及机械臂
CN114310904A (zh) 一种适用于机械臂关节空间路径规划的新型双向rrt*方法
Quin et al. Exploring in 3D with a climbing robot: Selecting the next best base position on arbitrarily-oriented surfaces

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant