CN113580130B - 六轴机械臂避障控制方法、系统及计算机可读存储介质 - Google Patents

六轴机械臂避障控制方法、系统及计算机可读存储介质 Download PDF

Info

Publication number
CN113580130B
CN113580130B CN202110819346.4A CN202110819346A CN113580130B CN 113580130 B CN113580130 B CN 113580130B CN 202110819346 A CN202110819346 A CN 202110819346A CN 113580130 B CN113580130 B CN 113580130B
Authority
CN
China
Prior art keywords
mechanical arm
point
point cloud
obstacle
distance
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
CN202110819346.4A
Other languages
English (en)
Other versions
CN113580130A (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.)
Foshan Institute Of Intelligent Equipment Technology
Original Assignee
Foshan Institute Of Intelligent Equipment Technology
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 Foshan Institute Of Intelligent Equipment Technology filed Critical Foshan Institute Of Intelligent Equipment Technology
Priority to CN202110819346.4A priority Critical patent/CN113580130B/zh
Publication of CN113580130A publication Critical patent/CN113580130A/zh
Application granted granted Critical
Publication of CN113580130B publication Critical patent/CN113580130B/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
    • 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
    • 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/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及机器人控制技术领域，具体涉及一种六轴机械臂避障控制方法、系统及计算机可读存储介质，所述方法包括：确定机械臂上的关键点集；控制机械臂朝目标运行，实时获取机械臂所在工作空间的点云数据，根据所述点云数据确定障碍物点云；根据所述关键点集和障碍物点云实时确定机械臂和障碍物的最短距离，所述最短距离为最近关键点和最近障碍物点的距离；当所述最短距离小于设定的安全距离时，控制机械臂在朝目标运行的过程中实时避障；本发明通过准确而自主的距离计算提高了机械臂的避障效率。

Description

六轴机械臂避障控制方法、系统及计算机可读存储介质
技术领域
本发明涉及机器人控制技术领域，具体涉及一种六轴机械臂避障控制方法、系统及计算机可读存储介质。
背景技术
传统工业机械臂缺乏对工作环境的视觉监控能力，只能按照设定的程序进行作业，避碰能力低，当前协作机器人的使用也潜在着误伤人的潜在危险，因此要避免机器人在作业过程中对人、机械臂和周围物品等产生碰撞。
目前工业领域尚未形成一套成熟的机械臂避障系统，随着KinectV1、real sense等深度相机的问世，越来越多的机械臂避障方案被提出并在一定程度上得到论证，但都并不是完整。中国专利CN111168681A公开了一种面向人机安全交互的机械臂智能避障方法、系统该方法需要通过识别码对障碍物进行定位，不符合实际情境中人或障碍物出现的不确定性。中国专利CN110253570A提出了一种基于视觉的工业机械臂人机安全系统，通过深度相机监控并获得人体点云信息，对人体点云进行包围盒和包围球处理，并由此获得人与机械臂的最短距离，但是该方法计算量大，且包围盒的处理会造成最短距离计算的不准确，特别是对于非凸物体，所计算出来的距离带有较大误差。
发明内容
本发明目的在于提供一种六轴机械臂避障控制方法、系统及计算机可读存储介质，以解决现有技术中所存在的一个或多个技术问题，至少提供一种有益的选择或创造条件。
为了实现上述目的，本发明提供以下技术方案：
一种六轴机械臂避障控制方法，所述方法包括以下步骤：
确定机械臂上的关键点集；其中，所述关键点集包括多个关键点，以各个关键点为球心的球均能满足在机械臂处于任意位姿时均刚好覆盖整个机械臂；
控制机械臂朝目标运行，实时获取机械臂所在工作空间的点云数据，根据所述点云数据确定障碍物点云；
根据所述关键点集和障碍物点云实时确定机械臂和障碍物的最短距离，所述最短距离为最近关键点和最近障碍物点的距离；其中，所述最近关键点为所述关键点集中的关键点，所述最近障碍物点为障碍物点云中的障碍物点；
当所述最短距离小于设定的安全距离时，控制机械臂在朝目标运行的过程中实时避障。
进一步，所述控制机械臂朝目标运行，包括：
实时确定机械臂末端与目标点的距离和位置向量，根据机械臂末端与目标点的距离和位置向量确定吸引速度；其中，所述目标点为控制机械臂到达的目标位置；
将吸引速度定义为第一笛卡尔速度，根据第一笛卡尔速度机械臂末端以及机械臂按全局路径朝目标运行；所述全局路径根据对机械臂末端以及机械臂进行全局路径规划得到。
进一步，所述实时确定机械臂末端与目标点的距离和位置向量，根据机械臂末端与目标点的距离和位置向量确定吸引速度，包括：
根据以下公式确定所述机械臂末端与目标点的距离：
Figure BDA0003171298900000021
其中，(Pendx，Pendy，Pendz)为机械臂末端在机械臂的基座坐标系下的坐标，(Pgoalx，Pgoaly，Pgoalz)为目标点在机械臂的基座坐标系下的坐标，Deg为机械臂末端与目标点的距离；
确定机械臂末端和目标点在机械臂的基座坐标系下的位置向量，并根据以下公式确定吸引速度：
Figure BDA0003171298900000022
其中，X(Pend，Pgoal)为机械臂末端和目标点的位置向量，Vatt为吸引速度，katt1、katt2为控制因子，Da为设定的阈值。
进一步，所述实时获取机械臂所在工作空间的点云数据，根据所述点云数据确定障碍物点云，包括：
获取机械臂所在工作空间的点云数据，对所述点云数据进行融合处理，得到第一深度图；其中，所述第一深度图包括机械臂点云和障碍物点云；
将第一深度图中的机械臂点云进行过滤，得到第二深度图；
将第二深度图由相机坐标系转换为机械臂的基座坐标系，得到第一点云图；
滤除第一点云图中残留的机械臂点云，得到第二点云图。
进一步，所述滤除第一点云图中残留的机械臂点云，得到第二点云图，包括：
设置过滤球的半径以及点云数量阈值，所述点云数量阈值为过滤球的内部所包含的最少点数量；
采用设置好的过滤球对第一点云图进行半径滤波，若过滤球的内部所包含的点数量低于点云数量阈值，则将过滤球的内部所包含的点均剔除，以此去除第一点云图中残留的机械臂点云，使得第一点云图中只剩下障碍物点云；
根据第一点云图中剩余的障碍物点进行体素滤波处理，得到第二点云图。
进一步，所述根据所述关键点集和障碍物点云实时确定机械臂和障碍物的最短距离，包括：
依次从第二点云图中选择一个障碍物点，分别确定该障碍物点与各个关键点的欧氏距离，通过遍历第二点云图中的各个障碍物点，得到各个障碍物点与各个关键点的欧氏距离；
将计算得到的欧氏距离中的最小值作为最短距离，确定欧氏距离最短的障碍物点和关键点，分别作为最近关键点和最近障碍物点。
进一步，所述当所述最短距离小于设定的安全距离时，控制机械臂在朝目标运行的过程中实时避障，包括：
当确定机械臂和目标点之间存在障碍物时，实时确定机械臂和障碍物的最短距离；
若所述最短距离小于设定的安全距离，则将吸引速度降为零，并根据机械臂上的关键点和最近障碍物点确定排斥速度；
将排斥速度定义为第二笛卡尔速度，根据第二笛卡尔速度控制机械臂的最近关键点按局部路径朝目标运行；所述局部路径根据对最近关键点进行局部路径规划得到。
进一步，所述排斥速度的计算公式为：
Figure BDA0003171298900000031
Figure BDA0003171298900000032
其中，X(Pkey，Pmin)为机械臂上的关键点与最近障碍物点的位置向量，2-范数||X(Pkey，Pmin)||2为位置向量的长度，Ds为设定的安全距离值，krep为控制因子，α为形变因子，vrep为排斥速度的大小值，ρ为排斥速度影响范围的半径值，Vrep为排斥速度，Vendmax为机械臂的最大线速度。
一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述任一项所述的六轴机械臂避障控制方法的步骤。
一种六轴机械臂避障控制系统，所述系统包括：
至少一个处理器；
至少一个存储器，用于存储至少一个程序；
当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现上述任一项所述的六轴机械臂避障控制方法。
本发明的有益效果是：本发明公开一种六轴机械臂避障控制方法、系统及计算机可读存储介质，首先通过确定机械臂上的关键点集，可针对选用的机械臂结构进行设置，通用性高；在控制机械臂朝目标运行时，实时获取机械臂所在工作空间的点云数据，根据所述点云数据确定障碍物点云；根据所述关键点集和障碍物点云实时确定机械臂和障碍物的最短距离，不需要通过识别码对障碍物进行定位，符合实际情境中人或障碍物随机出现的特点；当所述最短距离小于设定的安全距离时，控制机械臂在朝目标运行的过程中实时避障。本发明通过准确而自主的距离计算提高了机械臂的避障效率。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例中六轴机械臂避障控制方法的流程示意图；
图2是本发明实施例中机械臂关键点的示意图；
图3是本发明实施例中三个深度相机采集点云数据的示意图；
图4是本发明实施例中手眼标定示意图；
图5是本发明实施例中最短距离的标记示意图；
图6是本发明实施例中障碍物位于机械臂和目标点之间的示意图。
具体实施方式
以下将结合实施例和附图对本申请的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本申请的目的、方案和效果。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。
参考图1，如图1所示为本申请实施例提供的一种六轴机械臂避障控制方法，所述方法包括以下步骤：
步骤S100、确定机械臂上的关键点集；其中，所述关键点集包括多个关键点，以各个关键点为球心的球均能满足在机械臂处于任意位姿时均刚好覆盖整个机械臂；
本发明提供的实施例中，根据机械臂的结构和DH模型，在ROS(Robot OperatingSys tem，机器人操作系统)仿真平台下，选择2～5个连杆坐标系原点位置作为关键点，以关键点为球心的球能在机械臂任何位姿下覆盖整个机械臂，且球的半径尽可能小。可以理解，不同型号的机械臂可根据其DH模型和结构特点进行关键点的设置，以UR5机械臂为例，关键点Pkey1、Pkey2、Pkey3如图2所示。
步骤S200、控制机械臂朝目标运行，实时获取机械臂所在工作空间的点云数据，根据所述点云数据确定障碍物点云；
步骤S300、根据所述关键点集和障碍物点云实时确定机械臂和障碍物的最短距离，所述最短距离为最近关键点和最近障碍物点的距离；其中，所述最近关键点为所述关键点集中的关键点，所述最近障碍物点为障碍物点云中的障碍物点；
步骤S400、当所述最短距离小于设定的安全距离时，控制机械臂在朝目标运行的过程中实时避障。
本发明提供的实施例中，通过深度相机对机械臂工作空间进行实时监控，反馈障碍物的距离值，并根据改进的人工势场法使机械臂做出相应的避障决策。
作为上述实施例的进一步改进，步骤S200中，所述控制机械臂朝目标运行，包括：
实时确定机械臂末端与目标点的距离和位置向量，根据机械臂末端与目标点的距离和位置向量确定吸引速度；其中，所述目标点为控制机械臂到达的目标位置；
将吸引速度定义为第一笛卡尔速度，根据第一笛卡尔速度机械臂末端以及机械臂按全局路径朝目标运行；所述全局路径根据对机械臂末端以及机械臂进行全局路径规划得到。
作为上述实施例的进一步改进，所述实时确定机械臂末端与目标点的距离和位置向量，根据机械臂末端与目标点的距离和位置向量确定吸引速度，包括：
根据以下公式确定所述机械臂末端与目标点的距离：
Figure BDA0003171298900000051
其中，(Pendx，Pendy，Pendz)为机械臂末端在机械臂的基座坐标系下的坐标，(Pgoalx，Pgoaly，Pgoalz)为目标点在机械臂的基座坐标系下的坐标，Deg为机械臂末端与目标点的距离；
确定机械臂末端和目标点在机械臂的基座坐标系下的位置向量，并根据以下公式确定吸引速度：
Figure BDA0003171298900000052
其中，X(Pend，Pgoal)为机械臂末端和目标点的位置向量，Vatt为吸引速度，katt1、katt2为控制因子，Da为设定的阈值，防止机械臂末端靠近目标点时吸引力过小。
需要说明的是，吸引速度与机械臂末端和目标点的位置向量X(Pend，Pgoal)的方向一致，设置的控制因子katt1、katt2需要能够保证机械臂末端的速度不超过机械臂的最大线速度Vendmax。
作为上述实施例的进一步改进，步骤S200中，所述实时获取机械臂所在工作空间的点云数据，根据所述点云数据确定障碍物点云，包括：
步骤S210、获取机械臂所在工作空间的点云数据，对所述点云数据进行融合处理，得到第一深度图；其中，所述第一深度图包括机械臂点云和障碍物点云；
参考图3，本发明提供的实施例中，机械臂所在工作空间的点云数据通过三个深度相机采集得到，三个深度相机采用眼在手外的方式分别设置于工作台架上，以分别对机械臂所在工作空间的左上方、正上方和右上方的环境信息进行采集，通过对三个深度相机所采集的点云数据进行融合处理，以减小视野盲区，得到第一深度图，所述第一深度图包括机械臂点云和障碍物点云。
步骤S220、将第一深度图中的机械臂点云进行过滤，得到第二深度图；
在一些实施例中，采用开源的realtime_urdf_filter机器人实时过滤包将第一深度图中的机械臂点云进行过滤，得到第二深度图；
步骤S230、将第二深度图由相机坐标系转换为机械臂的基座坐标系，得到第一点云图；
在一些实施例中，采用以下公式将第二深度图由相机坐标系转换为机械臂的基座坐标系：
z＝zdepth：
Figure BDA0003171298900000061
Figure BDA0003171298900000062
其中，zdepth为深度相机到障碍物的距离值。即深度值；cx、cy、fx、fy为深度相机的内参；(m，n)为深度相机的像素坐标系中的像素点在第二深度图的坐标值，(x，y)为第二深度图中的坐标值(m，n)对应的物理坐标系的坐标值，(x，y，z)为根据第二深度图中的坐标值(m.n)转换得到的相机坐标系的坐标。
如图4所示，图4示出了眼在手外的方式，需要求取的是，机械臂的基座坐标系到相机坐标系之间的转换关系，即TB→C，为2个变量不变量的其中一个，另一是标定板与机械臂末端之间的坐标转换关系不变，即TP→E。在不同的机械臂位姿下将TP→E进行展开，通过计算即可求得TB→C。
步骤S240、滤除第一点云图中残留的机械臂点云，得到第二点云图。
在一些实施例中，使用半径滤波滤除第一点云图中残留的机械臂点云，得到第二点云图；本发明在滤除环境背景与机械臂模型后，能够获得完整障碍物形状的稀疏点云图，对于人或其它物体等障碍物无特殊要求，也无需进行贴码标记或制作数据集等操作，适用性高；
作为上述实施例的进一步改进，所述步骤S240包括：
设置过滤球的半径以及点云数量阈值，所述点云数量阈值为过滤球的内部所包含的最少点数量；
采用设置好的过滤球对第一点云图进行半径滤波，若过滤球的内部所包含的点数量低于点云数量阈值，则将过滤球的内部所包含的点均剔除，以此去除第一点云图中残留的机械臂点云，使得第一点云图中只剩下障碍物点云；
根据第一点云图中剩余的障碍物点进行体素滤波处理，得到第二点云图。得到的第二点云图可以保留障碍物基本的点云形状，同时大大减小点云的数量级。
作为上述实施例的进一步改进，所述步骤S300包括：
依次从第二点云图中选择一个障碍物点，分别确定该障碍物点与各个关键点的欧氏距离，通过遍历第二点云图中的各个障碍物点，得到各个障碍物点与各个关键点的欧氏距离；
将计算得到的欧氏距离中的最小值作为最短距离，确定欧氏距离最短的障碍物点和关键点，分别作为最近关键点和最近障碍物点。
参考图5，在一些实施例中，最短距离值两端的点分别为最近关键点和最近障碍物点的距离。在仿真软件中用RVIz可视化界面的Marker工具进行标记，最短距离的连线如图5所示，此过程采用GPU并行计算，以提高实时性。可以理解，计算机械臂与障碍物的距离是碰撞检测和避障规划的基础，准确而自主的距离计算能提高避障效率。本发明不需要通过识别码对障碍物进行定位，符合实际情境中人或障碍物随机出现的特点。本发明通过多相机点云数据融合以及滤波处理，能够获得保留障碍物形状的稀疏点云图，提高距离计算的精度。
作为上述实施例的进一步改进，所述步骤S400包括：
当确定机械臂和目标点之间存在障碍物时，实时确定机械臂和障碍物的最短距离；
若所述最短距离小于设定的安全距离，则将吸引速度降为零，并根据机械臂上的关键点和最近障碍物点确定排斥速度；
将排斥速度定义为第二笛卡尔速度，根据第二笛卡尔速度控制机械臂的最近关键点按局部路径朝目标运行；所述局部路径根据对最近关键点进行局部路径规划得到。
作为上述实施例的进一步改进，所述排斥速度的计算公式为：
Figure BDA0003171298900000071
Figure BDA0003171298900000072
其中，X(Pkey，Pmin)为机械臂上的关键点与最近障碍物点的位置向量，2-范数||X(Pkey，Pmin)||2为位置向量的长度，Ds为设定的安全距离值，krep为控制因子，α为形变因子，vrep为排斥速度的大小值，ρ为排斥速度影响范围的半径值，Vrep为排斥速度(具有方向性)，Vendmax为机械臂的最大线速度。
本发明提供的实施例中，采用改进的人工势场法进行避障规划，速度场产生“吸引”和“排斥”作用，吸引速度根据目标点产生，用于机械臂末端以及机械臂的全局路径规划；当最短距离小于设定的安全距离时，排斥速度由最近障碍物点产生，用于最近关键点的局部路径规划。
如图6所示，当障碍物位于机械臂和目标点之间时，传统方案中吸引速度的产生会增加远离障碍物所需的时间，且有可能由于吸引速度过大撞上障碍物。为了避免这种情况，当最短距离小于安全距离时，将吸引速度降为零，根据最近障碍物点确定排斥速度，根据排斥速度控制最近关键点运行。同时也可以避免吸引速度和排斥速度共存产生局部极小值点。当机械臂与障碍物的最短距离大于安全距离后，吸引速度才产生作用。这种情况下吸引速度和排斥速度不会同时产生，有“互斥”作用，可避免碰撞以及局部极小值点的出现。
需要说明的是，在机器人学中，关节空间到笛卡尔空间的映射通常通过雅可比矩阵实现，上述的吸引速度和排斥速度是在笛卡尔空间上的规划，需要把它们映射为关节速度，以实现对机器人的控制；本实施例中，采用以下公式将第一笛卡尔速度和第二笛卡尔速度分别映射为关节的吸引速度和排斥速度，分别得到；
Figure BDA0003171298900000081
Figure BDA0003171298900000082
Figure BDA0003171298900000083
其中，
Figure BDA0003171298900000084
为机械臂末端的雅可比矩阵的伪逆，
Figure BDA0003171298900000085
为第i个关键点的雅可比矩阵的伪逆，n表示受到排斥速度的关键点数量，VattJ为机械臂关节的吸引速度，VrepJi为第i个关键点的排斥速度，Vrepi为第i个关键点的笛卡尔速度，VrepJ为机械臂关节排斥速度的矢量合成。
本发明通过深度相机的点云采集来获取机械臂与障碍物的最短距离和最近点，并通过改进的人工势场法实现机械臂末端和关节的动态避障。可自主识别障碍物，无需事先进行标记和神经网络训练；使用GPU加速处理实现距离并行计算，保证避障的实时性。该发明可实时检测机械臂工作空间的人或其它物品等障碍物，可提高无人条件下机械臂的避碰能力，将工业机械臂进一步推向智能化和无人化；当下协作机器人受到大力的发展和广泛应用，该发明可保证人与协作机器人共存作业环境下人的安全，提高人机协同作业能力。
与图1的方法相对应，本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有六轴机械臂避障控制程序，所述六轴机械臂避障控制程序被处理器执行时实现如上述任一实施例所述的六轴机械臂避障控制方法的步骤。
与图1的方法相对应，本发明实施例还提供一种六轴机械臂避障控制系统，所述系统包括：
至少一个处理器；
至少一个存储器，用于存储至少一个程序；
当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现上述任一实施例所述的六轴机械臂避障控制方法。
上述方法实施例中的内容均适用于本系统实施例中，本系统实施例所具体实现的功能与上述方法实施例相同，并且达到的有益效果与上述方法实施例所达到的有益效果也相同。
所述处理器可以是中央处理单元(Central-Processing-Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital-Signal-Processor，DSP)、专用集成电路(Application-Specific-Integrated-Circuit，ASIC)、现场可编程门阵列(Field-Programmable-Gate-Arr ay，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述六轴机械臂避障控制系统的控制中心，利用各种接口和线路连接整个六轴机械臂避障控制系统可运行装置的各个部分。
所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述六轴机械臂避障控制系统的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart-Media-Card，SMC)，安全数字(Secure-Digital，SD)卡，闪存卡(Flash-Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
尽管本申请的描述已经相当详尽且特别对几个所述实施例进行了描述，但其并非旨在局限于任何这些细节或实施例或任何特殊实施例，而是应当将其视作是通过参考所附权利要求，考虑到现有技术为这些权利要求提供广义的可能性解释，从而有效地涵盖本申请的预定范围。此外，上文以发明人可预见的实施例对本申请进行描述，其目的是为了提供有用的描述，而那些目前尚未预见的对本申请的非实质性改动仍可代表本申请的等效改动。

Claims (6)

1.一种六轴机械臂避障控制方法，其特征在于，所述方法包括以下步骤：
确定机械臂上的关键点集；其中，所述关键点集包括多个关键点，以各个关键点为球心的球均能满足在机械臂处于任意位姿时刚好覆盖整个机械臂；
控制机械臂朝目标运行，实时获取机械臂所在工作空间的点云数据，根据所述点云数据确定障碍物点云；
根据所述关键点集和障碍物点云实时确定机械臂和障碍物的最短距离，所述最短距离为最近关键点和最近障碍物点的距离；其中，所述最近关键点为所述关键点集中的关键点，所述最近障碍物点为障碍物点云中的障碍物点；
当所述最短距离小于设定的安全距离时，控制机械臂在朝目标运行的过程中实时避障；
其中，所述控制机械臂朝目标运行，包括：
实时确定机械臂末端与目标点的距离和位置向量，根据机械臂末端与目标点的距离和位置向量确定吸引速度；其中，所述目标点为控制机械臂到达的目标位置；
将吸引速度定义为第一笛卡尔速度，根据第一笛卡尔速度机械臂末端以及机械臂按全局路径朝目标运行；所述全局路径根据对机械臂末端以及机械臂进行全局路径规划得到；
所述实时确定机械臂末端与目标点的距离和位置向量，根据机械臂末端与目标点的距离和位置向量确定吸引速度，包括：
根据以下公式确定所述机械臂末端与目标点的距离：
Figure FDA0003739013120000011
其中，(Pendx,Pendy,Pendz)为机械臂末端在机械臂的基座坐标系下的坐标，(Pgoalx,Pgoaly,Pgoalz)为目标点在机械臂的基座坐标系下的坐标，Deg为机械臂末端与目标点的距离；
确定机械臂末端和目标点在机械臂的基座坐标系下的位置向量，并根据以下公式确定吸引速度：
Figure FDA0003739013120000012
其中，X(Pend,Pgoal)为机械臂末端和目标点的位置向量，Vatt为吸引速度，katt1、katt2为控制因子,Da为设定的阈值；
其中，所述当所述最短距离小于设定的安全距离时，控制机械臂在朝目标运行的过程中实时避障，包括：
当确定机械臂和目标点之间存在障碍物时，实时确定机械臂和障碍物的最短距离；
若所述最短距离小于设定的安全距离，则将吸引速度降为零，并根据机械臂上的关键点和最近障碍物点确定排斥速度；
将排斥速度定义为第二笛卡尔速度，根据第二笛卡尔速度控制机械臂的最近关键点按局部路径朝目标运行；所述局部路径根据对最近关键点进行局部路径规划得到；
所述排斥速度的计算公式为：
Figure FDA0003739013120000021
Figure FDA0003739013120000022
其中，X(Pkey,Pmin)为机械臂上的关键点与最近障碍物点的位置向量，2-范数||X(Pkey,Pmin)||2为位置向量的长度，Ds为设定的安全距离值，krep为控制因子，α为形变因子，vrep为排斥速度的大小值，ρ为排斥速度影响范围的半径值，Vrep为排斥速度，Vendmax为机械臂的最大线速度。
2.根据权利要求1所述的一种六轴机械臂避障控制方法，其特征在于，所述实时获取机械臂所在工作空间的点云数据，根据所述点云数据确定障碍物点云，包括：
获取机械臂所在工作空间的点云数据，对所述点云数据进行融合处理，得到第一深度图；其中，所述第一深度图包括机械臂点云和障碍物点云；
将第一深度图中的机械臂点云进行过滤，得到第二深度图；
将第二深度图由相机坐标系转换为机械臂的基座坐标系，得到第一点云图；
滤除第一点云图中残留的机械臂点云，得到第二点云图。
3.根据权利要求2所述的一种六轴机械臂避障控制方法，其特征在于，所述滤除第一点云图中残留的机械臂点云，得到第二点云图，包括：
设置过滤球的半径以及点云数量阈值，所述点云数量阈值为过滤球的内部所包含的最少点数量；
采用设置好的过滤球对第一点云图进行半径滤波，若过滤球的内部所包含的点数量低于点云数量阈值，则将过滤球的内部所包含的点均剔除，以此去除第一点云图中残留的机械臂点云，使得第一点云图中只剩下障碍物点云；
根据第一点云图中剩余的障碍物点进行体素滤波处理，得到第二点云图。
4.根据权利要求3所述的一种六轴机械臂避障控制方法，其特征在于，所述根据所述关键点集和障碍物点云实时确定机械臂和障碍物的最短距离，包括：
依次从第二点云图中选择一个障碍物点，分别确定该障碍物点与各个关键点的欧氏距离，通过遍历第二点云图中的各个障碍物点，得到各个障碍物点与各个关键点的欧氏距离；
将计算得到的欧氏距离中的最小值作为最短距离，确定欧氏距离最短的障碍物点和关键点，分别作为最近关键点和最近障碍物点。
5.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至4任一项所述的六轴机械臂避障控制方法的步骤。
6.一种六轴机械臂避障控制系统，其特征在于，所述系统包括：
至少一个处理器；
至少一个存储器，用于存储至少一个程序；
当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如权利要求1至4任一项所述的六轴机械臂避障控制方法。
CN202110819346.4A 2021-07-20 2021-07-20 六轴机械臂避障控制方法、系统及计算机可读存储介质 Active CN113580130B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110819346.4A CN113580130B (zh) 2021-07-20 2021-07-20 六轴机械臂避障控制方法、系统及计算机可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110819346.4A CN113580130B (zh) 2021-07-20 2021-07-20 六轴机械臂避障控制方法、系统及计算机可读存储介质

Publications (2)

Publication Number Publication Date
CN113580130A CN113580130A (zh) 2021-11-02
CN113580130B true CN113580130B (zh) 2022-08-30

Family

ID=78248481

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110819346.4A Active CN113580130B (zh) 2021-07-20 2021-07-20 六轴机械臂避障控制方法、系统及计算机可读存储介质

Country Status (1)

Country Link
CN (1) CN113580130B (zh)

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6069923B2 (ja) * 2012-07-20 2017-02-01 セイコーエプソン株式会社 ロボットシステム、ロボット、ロボット制御装置
CN103455034B (zh) * 2013-09-16 2016-05-25 苏州大学张家港工业技术研究院 一种基于最近距离向量场直方图的避障路径规划方法
CN104029203B (zh) * 2014-06-18 2017-07-18 大连大学 实现空间机械臂避障的路径规划方法
CN105598965B (zh) * 2015-11-26 2018-03-16 哈尔滨工业大学 基于立体视觉的机器人欠驱动手自主抓取方法
CN106569491A (zh) * 2016-10-31 2017-04-19 江苏华航威泰机器人科技有限公司 一种机器人避障轨迹规划方法
CN107351085B (zh) * 2017-08-21 2019-11-08 西北工业大学 一种基于多个控制点的空间机器人避撞方法
CN108326849B (zh) * 2018-01-04 2019-08-30 浙江大学 一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法
JP6911777B2 (ja) * 2018-01-23 2021-07-28 トヨタ自動車株式会社 動作軌道生成装置
CN109465835A (zh) * 2018-09-25 2019-03-15 华中科技大学 一种动态环境下双臂服务机器人作业的事前安全预测方法
CN109968358B (zh) * 2019-03-28 2021-04-09 北京工业大学 一种考虑运动平稳性的冗余机器人全关节避障轨迹优化方法
CN110253570B (zh) * 2019-05-27 2020-10-27 浙江工业大学 基于视觉的工业机械臂人机安全系统
CN110262478B (zh) * 2019-05-27 2022-04-19 浙江工业大学 基于改进人工势场法的人机安全避障路径规划方法
CN112651535A (zh) * 2019-10-10 2021-04-13 北京京东尚科信息技术有限公司 局部路径规划方法、装置、存储介质、电子设备及车辆
CN110764531B (zh) * 2019-11-12 2023-01-03 西北工业大学 基于激光雷达与人工势场法的无人机编队飞行避障方法
CN112975938B (zh) * 2019-12-12 2022-04-05 中国科学院沈阳自动化研究所 一种基于零空间的机械臂速度层轨迹规划方法
CN111267107B (zh) * 2020-03-23 2021-07-02 上海高仙自动化科技发展有限公司 控制方法、机器人、电子设备和可读存储介质
CN111958590B (zh) * 2020-07-20 2021-09-28 佛山科学技术学院 一种复杂三维环境中机械臂防碰撞方法及系统

Also Published As

Publication number Publication date
CN113580130A (zh) 2021-11-02

Similar Documents

Publication Publication Date Title
CN110253570B (zh) 基于视觉的工业机械臂人机安全系统
CN107914272B (zh) 一种七自由度机械臂组件抓取目标物体的方法
Song et al. CAD-based pose estimation design for random bin picking using a RGB-D camera
Davison Real-time simultaneous localisation and mapping with a single camera
CN109986560B (zh) 一种面向多目标种类的机械臂自适应抓取方法
CN110874100A (zh) 用于使用视觉稀疏地图进行自主导航的系统和方法
CN112489126B (zh) 车辆关键点信息检测方法、车辆控制方法及装置、车辆
CN111958590B (zh) 一种复杂三维环境中机械臂防碰撞方法及系统
CN111015656A (zh) 一种机器人主动避障的控制方法、装置及存储介质
CN113246143A (zh) 一种机械臂动态避障轨迹规划方法及装置
CN114102585A (zh) 一种物品抓取规划方法及系统
CN113885554B (zh) 一种无人机集群的分布式围捕控制方法及系统
CN114310892B (zh) 基于点云数据碰撞检测的物体抓取方法、装置和设备
CN112338922B (zh) 一种五轴机械臂抓取放置方法及相关装置
CN115509231A (zh) 一种机器人跟随避障方法、装置及存储介质
CN115338856A (zh) 用于控制机器人装置的方法
CN113580130B (zh) 六轴机械臂避障控制方法、系统及计算机可读存储介质
CN117325170A (zh) 基于深度视觉导引机械臂抓取硬盘架的方法
Shaw et al. Development of an AI-enabled AGV with robot manipulator
CN114353779B (zh) 一种采用点云投影快速更新机器人局部代价地图的方法
CN115902977A (zh) 基于视觉和gps的变电站机器人双重定位方法及系统
Pinzón-Arenas et al. Obstacle detection using faster R-CNN oriented to an autonomous feeding assistance system
Chowdhury et al. Comparison of neural network-based pose estimation approaches for mobile manipulation
CN111915727A (zh) 基于深度学习的agv新型slam系统
US20230330849A1 (en) Information processing device, information processing method, and storage medium

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