CN114274129A - 基于视觉引导的机械臂运动规划与控制方法、系统及介质 - Google Patents

Abstract

本发明提供了一种基于视觉引导的机械臂运动规划与控制方法、系统及介质，涉及机器人技术领域，系统包括：机械臂模块、视觉模块及智能规划与控制模块；其中，智能规划与控制模块实时接收视觉模块与机械臂模块反馈的状态信息，经过运算处理后发送机械臂运动信息至机械臂模块；方法包括：步骤S1：系统通过人机交互接收到任务指令后开始工作；步骤S2：视觉模块与机械臂模块反馈当前时刻环境信息与机械臂状态，两者同时进行；步骤S3：判断机械臂当前位姿是否到达目标位姿，并规划出一条运动轨迹控制机械臂到达目标位姿。本发明能够保证达到目标位置的前提下，动态避开运动过程中的障碍物，提升机械臂运动过程中的安全性。

Description

基于视觉引导的机械臂运动规划与控制方法、系统及介质

技术领域

本发明涉及机器人技术领域，具体地，涉及一种基于视觉引导的机械臂运动规划与控制方法、系统及介质。

背景技术

机械臂是一种高精度，多输入多输出，高度非线性，强耦合的复杂系统，因其具备的自动化属性，使得机械臂在工业制造、医学治疗、半导体制造以及太空探索等领域都发挥举足轻重的功能。传统机械臂负载大，功能性能稳定，能够很好的满足单一重复的应用场景需求，如工厂上的上下料，机械臂仅需要不断重复固定路径运动即可。但是随着机械臂应用场景的复杂化，传统工业机械臂显然已经无法满足复杂场景下的使用需求，机械臂的智能化升级势在必行。

机械臂智能化升级一大方向就是机械臂+相机，通过赋予机械臂视觉功能以此来提升机械臂的智能性，但是目前主流的机械臂运动规划与控制方案中，视觉与机械臂的联系并不紧密，一般都是视觉模块检测并定位目标位置，然后将该目标位置发送给机械臂，机械臂再通过笛卡尔空间轨迹规划功能到达目标位置，但是在机械臂开始运动至到达目标位置之前，视觉模块并没有发挥作用，当机械臂运动过程中突然出现障碍物时，机械臂会与障碍物发生碰撞，造成非常严重的安全事故。因此，这种动态避障功能的缺失会严重影响到机械臂的智能化发展，进而影响到人机协作过程中的安全性，解决机械臂运动过程中的动态避障能力具有非常高的实用价值。

公开号为CN113119116A的发明专利，公开了一种机械臂运动规划方法、装置、可读存储介质及机械臂，包括：确定机械臂从预设的初始位姿运动至预设的目标位姿的虚拟最优运动规划路径；在所述机械臂的自由空间中进行随机采样，得到所述机械臂的第一随机位姿；根据所述虚拟最优运动规划路径对所述第一随机位姿进行迭代优化，得到优化后的第二随机位姿；将所述第二随机位姿作为预设的机械臂运动规划算法中的随机采样结果对所述机械臂进行运动规划。

发明内容

针对现有技术中的缺陷，本发明提供一种基于视觉引导的机械臂运动规划与控制方法、系统及介质。

根据本发明提供的一种基于视觉引导的机械臂运动规划与控制方法、系统及介质，所述方案如下：

第一方面，提供了一种基于视觉引导的机械臂运动规划与控制方法，所述方法包括：

步骤S1：系统通过人机交互接收到任务指令后开始工作；

步骤S2：视觉模块与机械臂模块反馈当前时刻环境信息与机械臂状态，两者同时进行；

步骤S3：判断机械臂当前位姿是否到达目标位姿，并规划出一条运动轨迹控制机械臂到达目标位姿。

优选地，所述步骤S3判断机械臂当前位姿是否到达目标位姿包括：

若机械臂当前位姿已经到达目标位姿，则工作流程结束，该任务执行完成；

若机械臂当前位姿并未到达目标位姿，则一直执行如下循环：智能规划与控制模块根据环境信息与机械臂状态规划出下一时刻机械臂位姿，并控制机械臂到达该位姿。

优选地，所述规划机械臂下一时刻位姿并控制机械臂到达包括：

1)目标速度求解：

目标位姿X_goal由视觉模块通过三维点云匹配与图像分割技术获得，目标速度求解由如下分段函数实现：

上式中，V_goal表示智能规划与控制模块根据目标位姿生成的目标速度；d表示目标位姿与机械臂当前位姿的距离，δ表示目标位姿与机械臂当前位姿距离的阈值；

当距离不大于距离阈值时，目标速度为零，当距离大于阈值时，再次判断当前目标速度的值，当目标速度大于目标速度阈值V_{goal_max}时，目标速度等于该速度阈值，当目标速度不大于该速度阈值时，目标速度是一个与时间有关的线性函数，其中，a表示加速度，方向指向目标位姿，t表示时间。

优选地，所述规划机械臂下一时刻位姿并控制机械臂到达还包括：

2)位姿偏差补偿速度求解：

位姿偏差补偿速度的公式为：

上式中，V_err表示由位姿偏差引起的位姿偏差补偿速度；目标位姿X_goal由视觉模块通过三维点云匹配与图像分割技术获得，X_fdb表示机械臂当前状态实际位姿；

机械臂当前位姿由正向运动学根据机械臂模块反馈的关节位置计算得到；在计算位姿偏差补偿速度时考虑目标位姿与机械臂末端当前位姿的距离d与其阈值δ之间的关系，当距离过大时，位姿偏差补偿速度为零，当距离较近，即快接近目标位姿时，根据上述公式得到补偿速度。

优选地，所述规划机械臂下一时刻位姿并控制机械臂到达还包括：

3)避障补偿速度求解：

在机械臂末端周围设置障碍检测空间，在该空间内给每个障碍物设置势力场，即引力场与斥力场，在机械臂靠近障碍物时，避免机械臂与障碍物发生碰撞，最终在目标速度与位姿偏差补偿速度的作用下，机械臂末端到达目标位姿。

优选地，在考虑避障补偿速度基础上机械臂笛卡尔空间内的目标速度解算，方法如下：

a.将机械臂末端为圆心，d_o为半径的球形空间设置为障碍检测空间，在解算避障补偿速度时仅考虑障碍检测空间内的障碍网格；

b.设置障碍网格的势力场函数为U(q)＝U_att(q)+U_rep(q)；

其中，U_att(q)为引力场函数，U_rep(q)为势力场函数；

c.设置引力场函数具体表达式：

其中，K_att为引力增益，d(X_goal，X_fdb)为目标位姿与当前位姿的距离；

d.设置斥力场函数具体表达式：

其中，K_rep为斥力增益，d_m(q)为障碍物与机械臂末端的距离，d_o为障碍检测空间半径；

e.利用梯度下降法求得避障补偿速度：

其中，V_U表示由障碍物引起的补偿速度；

表示各障碍物与机械臂末端距离对机械臂各关节的微分。

优选地，所述步骤S3根据计算出的目标速度、位姿偏差补偿速度及避障补偿速度，再根据机械臂实际位姿与目标位姿，不断迭代求解，最终规划出一条安全的运动轨迹并控制机械臂到达目标位姿。

第二方面，提供了一种基于视觉引导的机械臂运动规划与控制系统，所述系统包括：

机械臂模块：包括机械臂本体和夹爪，保证机械臂完成抓取或转移物体在内的相关操作；

视觉模块：由深度相机构成，提供目标平面与立体的点云数据，并根据这些数据获取目标特征点；

智能规划与控制模块：根据视觉模块反馈的环境信息与机械臂模块反馈的机械臂状态，实时规划到达目标位置的运动轨迹；

其中，智能规划与控制模块实时接收视觉模块与机械臂模块反馈的状态信息，经过运算处理后发送机械臂运动信息至机械臂模块。

第三方面，提供了一种存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现所述中方法的步骤。

与现有技术相比，本发明具有如下的有益效果：

1、本发明提高了机械臂使用安全性，极大程度的保证的人机协作过程中人与机械臂各自的安全；

2、本发明提高了机械臂的部署效率，无需事先建立机械臂周围环境模型，能够根据视觉模块实时反馈的环境状态安全工作；

3、本发明提升了机械臂的适应性与智能性，降低了机械臂对环境的强依赖，在面对复杂的周边环境时，也能保证机械臂正常工作。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为系统组成框架图；

图2为工作流程图；

图3为整体运动规划与控制算法框架图；

图4为目标速度解算概念图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

本发明实施例提供了一种基于视觉引导的机械臂运动规划与控制系统，参照图1所示，该系统包括：机械臂模块、视觉模块及智能规划与控制模块。

机械臂模块：包括机械臂本体和夹爪，保证机械臂完成抓取或转移物体在内的相关操作。

视觉模块：由深度相机构成，提供目标平面与立体的点云数据，并根据这些数据获取目标特征点。

智能规划与控制模块：根据视觉模块反馈的环境信息与机械臂模块反馈的机械臂状态，实时规划到达目标位置的运动轨迹。

其中，智能规划与控制模块实时接收视觉模块与机械臂模块反馈的状态信息，经过运算处理后发送机械臂运动信息至机械臂模块，完成既定目标任务，三大模块之间的构成关系与智能机械臂系统组成框架如图1所示。

本发明中提出的动态规划与控制方法基于视觉模块的三维点云匹配技术与图像分割技术。

实际工作过程中，机械臂的工作任务可以被分解并抽象为机械臂从当前位置运动至目标位置，实际工作就是根据任务需求，不断的重复上述过程，但本发明涉及到的应用场景更加复杂，考虑到了动态避障的需求，因此本发明中智能机械臂系统抽象任务为：实时规划出一条无碰撞轨迹，并控制机械臂从当前位置运动至目标位置。

本发明还提供了一种基于视觉引导的机械臂运动规划与控制方法，根据上述分解的抽象任务，其工作流程如图2所示。

步骤S1：当系统通过人机交互接收到任务指令后开始工作。

步骤S2：视觉模块与机械臂模块反馈当前时刻环境信息与机械臂状态，两者同时进行。

步骤S3：判断机械臂当前位姿是否到达目标位姿，并规划出一条运动轨迹控制机械臂到达目标位姿。

具体地，若机械臂当前位姿已经到达目标位姿，则工作流程结束，该任务执行完成；

若机械臂当前位姿并未到达目标位姿，则一直执行如下循环：智能规划与控制模块根据环境信息与机械臂状态规划出下一时刻机械臂位姿，并控制机械臂到达该位姿。

在上述工作流程中，规划机械臂下一时刻位姿并控制机械臂到达是机械臂智能化与自适应化的关键，也是本发明重点，从控制理论考虑该问题：已知机械臂末端目标位姿和实际位姿，设计控制算法保证机械臂末端平稳、准确并快速的到达目标位姿，整体运动规划与控制算法框架图如图3所示。

从图3中可以看出，只要得到目标速度，位姿偏差补偿速度，避障补偿速度，再经过矩阵运算与积分，便能够得到机械臂各关节位置，因此智能规划与控制可以划分为三大方向分别求解计算。

1)目标速度求解：

目标位姿X_goal由视觉模块通过三维点云匹配与图像分割技术获得，目标速度求解由如下分段函数实现：

上式中，V_goal表示智能规划与控制模块根据目标位姿生成的目标速度；d表示目标位姿与机械臂当前位姿的距离，δ表示目标位姿与机械臂当前位姿距离的阈值。

当距离不大于距离阈值时，目标速度为零，当距离大于阈值时，再次判断当前目标速度的值，当目标速度大于目标速度阈值V_{goal_max}时，目标速度等于该速度阈值，当目标速度不大于该速度阈值时，目标速度是一个与时间有关的线性函数，其中，a表示加速度，方向指向目标位姿，t表示时间。

2)位姿偏差补偿速度求解：

位姿偏差补偿速度的公式为：

上式中，V_err表示由位姿偏差引起的位姿偏差补偿速度；目标位姿X_goal由视觉模块通过三维点云匹配与图像分割技术获得，X_fdb表示机械臂当前状态实际位姿。

机械臂当前位姿可由正向运动学根据机械臂模块反馈的关节位置计算得到；为保证机械臂运动过程中的平顺与精确性，在计算位姿偏差补偿速度时考虑目标位姿与机械臂末端当前位姿的距离d与其阈值δ之间的关系，当距离过大时，位姿偏差补偿速度为零，当距离较近，即快接近目标位姿时，根据上述公式得到补偿速度。

3)避障补偿速度求解：

避障补偿速度的求解既取决于机械臂当前位姿，也取决于障碍物的位置，其计算原理为：视觉模块将应用场景中的障碍物以一定分辨率进行网格化处理，兼顾计算效率与避障稳定，如可以将分辨率设置为1cm³，仅考虑障碍物的最外层障碍网格。在机械臂末端周围设置障碍检测空间，在该空间内给每个障碍物设置势力场，即引力场与斥力场，在机械臂靠近障碍物时，障碍物对机械臂的斥力较大，避免机械臂与障碍物发生碰撞，最终在目标速度与位姿偏差补偿速度的作用下，机械臂末端到达目标位姿。在考虑避障补偿速度基础上机械臂笛卡尔空间内的目标速度解算概念图如图4所示。

具体方法如下：

a.将机械臂末端为圆心，d_o为半径的球形空间设置为障碍检测空间，在解算避障补偿速度时仅考虑障碍检测空间内的障碍网格；

b.设置障碍网格的势力场函数为U(q)＝U_att(q)+U_rep(q)；

其中，U_att(q)为引力场函数，U_rep(q)为势力场函数；

c.设置引力场函数具体表达式：

其中，K_att为引力增益，d(X_goal，X_fdb)为目标位姿与当前位姿的距离；

d.设置斥力场函数具体表达式：

其中，K_rep为斥力增益，d_m(q)为障碍物与机械臂末端的距离，d_o为障碍检测空间半径；

e.利用梯度下降法求得避障补偿速度：

其中，V_U表示由障碍物引起的补偿速度；

表示各障碍物与机械臂末端距离对机械臂各关节的微分。

根据计算出的目标速度、位姿偏差补偿速度及避障补偿速度，再根据机械臂实际位姿与目标位姿，不断迭代求解，最终规划出一条安全的运动轨迹并控制机械臂到达目标位姿。

本发明实施例提供了一种基于视觉引导的机械臂运动规划与控制方法、系统及介质，将机械臂与视觉看作一个整体，机械臂运动过程中视觉实时反馈机械臂周围环境状态，根据机械臂周围环境状态，机械臂实时调整自身构型，在保证达到目标位置的前提下，能够动态避开运动过程中的障碍物，提升机械臂运动过程中的安全性。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (9)

1.一种基于视觉引导的机械臂运动规划与控制方法，其特征在于，包括：

步骤S1：系统通过人机交互接收到任务指令后开始工作；

步骤S2：视觉模块与机械臂模块反馈当前时刻环境信息与机械臂状态，两者同时进行；

步骤S3：判断机械臂当前位姿是否到达目标位姿，并规划出一条运动轨迹控制机械臂到达目标位姿。

2.根据权利要求1所述的基于视觉引导的机械臂运动规划与控制方法，其特征在于，所述步骤S3判断机械臂当前位姿是否到达目标位姿包括：

若机械臂当前位姿已经到达目标位姿，则工作流程结束，该任务执行完成；

若机械臂当前位姿并未到达目标位姿，则一直执行如下循环：智能规划与控制模块根据环境信息与机械臂状态规划出下一时刻机械臂位姿，并控制机械臂到达该位姿。

3.根据权利要求2所述的基于视觉引导的机械臂运动规划与控制方法，其特征在于，所述规划机械臂下一时刻位姿并控制机械臂到达包括：

1)目标速度求解：

目标位姿X_goal由视觉模块通过三维点云匹配与图像分割技术获得，目标速度求解由如下分段函数实现：

0 d≤δ

上式中，V_goal表示智能规划与控制模块根据目标位姿生成的目标速度；d表示目标位姿与机械臂当前位姿的距离，δ表示目标位姿与机械臂当前位姿距离的阈值；

当距离不大于距离阈值时，目标速度为零，当距离大于阈值时，再次判断当前目标速度的值，当目标速度大于目标速度阈值V_{goal_max}时，目标速度等于该速度阈值，当目标速度不大于该速度阈值时，目标速度是一个与时间有关的线性函数，其中，a表示加速度，方向指向目标位姿，t表示时间。

4.根据权利要求3所述的基于视觉引导的机械臂运动规划与控制方法，其特征在于，所述规划机械臂下一时刻位姿并控制机械臂到达还包括：

2)位姿偏差补偿速度求解：

位姿偏差补偿速度的公式为：

上式中，V_err表示由位姿偏差引起的位姿偏差补偿速度；目标位姿X_goal由视觉模块通过三维点云匹配与图像分割技术获得，X_fdb表示机械臂当前状态实际位姿；

机械臂当前位姿由正向运动学根据机械臂模块反馈的关节位置计算得到；在计算位姿偏差补偿速度时考虑目标位姿与机械臂末端当前位姿的距离d与其阈值δ之间的关系，当距离过大时，位姿偏差补偿速度为零，当距离较近，即快接近目标位姿时，根据上述公式得到补偿速度。

5.根据权利要求4所述的基于视觉引导的机械臂运动规划与控制方法，其特征在于，所述规划机械臂下一时刻位姿并控制机械臂到达还包括：

3)避障补偿速度求解：

在机械臂末端周围设置障碍检测空间，在该空间内给每个障碍物设置势力场，即引力场与斥力场，在机械臂靠近障碍物时，避免机械臂与障碍物发生碰撞，最终在目标速度与位姿偏差补偿速度的作用下，机械臂末端到达目标位姿。

6.根据权利要求5所述的基于视觉引导的机械臂运动规划与控制方法，其特征在于，在考虑避障补偿速度基础上机械臂笛卡尔空间内的目标速度解算，方法如下：

a.将机械臂末端为圆心，d_o为半径的球形空间设置为障碍检测空间，在解算避障补偿速度时仅考虑障碍检测空间内的障碍网格；

b.设置障碍网格的势力场函数为U(q)＝U_att(q)+U_rep(q)；

其中，U_att(q)为引力场函数，U_rep(q)为势力场函数；

c.设置引力场函数具体表达式：

其中，K_att为引力增益，d(X_goal，X_fdb)为目标位姿与当前位姿的距离；

d.设置斥力场函数具体表达式：

其中，K_rep为斥力增益，d_m(q)为障碍物与机械臂末端的距离，d_o为障碍检测空间半径；

e.利用梯度下降法求得避障补偿速度：

其中，V_U表示由障碍物引起的补偿速度；

表示各障碍物与机械臂末端距离对机械臂各关节的微分。

7.根据权利要求6所述的基于视觉引导的机械臂运动规划与控制方法，其特征在于，所述步骤S3根据计算出的目标速度、位姿偏差补偿速度及避障补偿速度，再根据机械臂实际位姿与目标位姿，不断迭代求解，最终规划出一条安全的运动轨迹并控制机械臂到达目标位姿。

8.一种基于视觉引导的机械臂运动规划与控制系统，其特征在于，基于权利要求1-7中任意一项所述的基于视觉引导的机械臂运动规划与控制方法，包括：

机械臂模块：包括机械臂本体和夹爪，保证机械臂完成抓取或转移物体在内的相关操作；

视觉模块：由深度相机构成，提供目标平面与立体的点云数据，并根据这些数据获取目标特征点；

智能规划与控制模块：根据视觉模块反馈的环境信息与机械臂模块反馈的机械臂状态，实时规划到达目标位置的运动轨迹；

其中，智能规划与控制模块实时接收视觉模块与机械臂模块反馈的状态信息，经过运算处理后发送机械臂运动信息至机械臂模块。

9.一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。

Images