一种机械臂零空间实时避障控制方法及系统

技术领域

本发明涉及自动控制技术领域，特别涉及一种基于虚拟阻抗和预设性能函数的机械臂零空间实时避障控制方法及系统。

背景技术

近年来，智能机器人产业迅速发展，已经开始被广泛地应用于服务、娱乐、医疗等领域。越来越多的任务需要人与机器人协作完成，因此人机协作的相关问题得到了科研人员的高度重视。

人机协作控制相比于传统机器人控制的一大难点就是既要借助机器人的功能特点来提高人机协作的工作效率，同时还要保证人与机器人交互过程的柔顺性、安全性和智能性。因此对人机协作过程中智能控制方法的研究尤为重要。

现有的人机协作控制方法中主要依靠于触觉信息（交互力/力矩）来实现人机协作任务。这种单一的控制方法只能保证机器人可以跟随操作者的意图一起将物体协作移动到指定位置，以及保持机器人身体的柔顺性，但是在协作过程中机器人并不具备主动躲避障碍物的能力，因此该种控制方法安全性欠缺。并且现有的用于实现位置控制的控制器的控制效果不够理想，进而导致了机器人在人机协作过程中效率低下。

发明内容

针对上述问题，本发明的目的在于提供一种基于虚拟阻抗和预设性能函数的机械臂零空间实时避障控制方法及系统，以解决现有人机协作搬运系统在协作过程中机器人不具备主动躲避障碍物的问题以及机器人在人机协作过程中效率低下的问题。

为解决上述技术问题，本发明的实施例提供如下方案：

一方面，提供了一种机械臂零空间实时避障控制方法，所述方法基于包括机械臂、双目立体摄像头、服务器的机械臂零空间实时避障控制系统，所述方法由感知任务、规划任务与控制任务组成，包括以下步骤：

采集障碍物信息，获取障碍物的三维坐标，完成所述感知任务；

基于人工势场法，使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间的排斥力，进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避障轨迹，完成所述规划任务；

基于预设性能函数根据需求对机械臂的瞬态性能进行约束，设计机械臂轨迹跟踪控制程序，得到机械臂跟踪避障轨迹的关节力矩，并实时控制机械臂按照预设性能跟踪避障轨迹，完成所述控制任务。

优选地，所述感知任务包括：得到障碍物表面与潜在碰撞点最近点的三维坐标，其表现形式如下：

其中，

潜在碰撞点的三维坐标通过机械臂前向运动学计算得到，机械臂上的潜在碰撞点和障碍物之间的距离表现形式如下：

至此，完成所述感知任务。

优选地，所述规划任务包括：根据障碍物与机械臂之间的相对距离，实时生成避障轨迹；所述规划任务由虚拟排斥力函数与虚拟阻抗控制算法组成；

基于人工势场法，规划任务中的虚拟排斥力函数

其中，冒险空间是包围机械臂的一层虚拟保护外壳，

所述虚拟排斥力函数的作用在于，在三维空间中，在物理意义上直观地作用于机械臂上。

优选地，所述虚拟阻抗控制算法的输入是虚拟力矩，输出是机械臂各个关节对应的运动轨迹，所述虚拟阻抗控制算法的表现形式如下：

其中

优选地，虚拟排斥力为三维任务空间中的力，虚拟排斥力函数

其中

虚拟排斥力矩结合虚拟阻抗控制算法，得到下述实时避障参考轨迹的加速度：

通过积分

优选地，所述机械臂为冗余机械臂，关节个数为7个，而工作空间的自由度为6个，这使得机械臂的自由度大于工作空间的自由度，机械臂存在的冗余自由度，使机械臂可以完成次级任务；

机械臂的主任务在机械臂的末端执行器上执行；上述次级任务，即为不妨碍末端执行器执行主任务时，机械臂的身体，包括肩关节、肘关节、腕关节执行的任务；上述冗余机械臂的主任务与次级任务之间组合运动轨迹的表现形式如下：

其中

优选地，所述零空间投影技术用于完成主任务与次级任务的优先级分层；所述主任务在机械臂零空间实时避障控制方法中，指末端执行器执行的任务，包括抓取任务、协作装配任务、协作搬运任务；所述次级任务为实时避障任务。

优选地，所述控制任务包括：基于反馈控制、前馈补偿、误差约束，计算出机械臂各个关节的控制力矩，以控制机械臂跟踪上述得到的期望位置和期望姿态，使机械臂在执行任务的同时实现实时避障；其控制力矩的控制器表现形式如下：

其中，

鲁棒项可将逼近域为半全局的神经网络扩展为作用域为全局的带有鲁棒项的神经网络，当神经网络输入越过神经网络逼近域时，鲁棒项起作用；其中

其中

通过误差转换得到

一方面，提供了一种机械臂零空间实时避障控制系统，所述系统包括机械臂、双目立体摄像头和服务器，所述机械臂、所述双目立体摄像头与所述服务器通过ROS通信机制连接，并由所述服务器进行控制；

所述机械臂为七关节冗余机械臂，用于完成布置的各项任务；

所述双目立体摄像头用于采集位于所述机械臂运动空间内的障碍物信息，获取障碍物的三维坐标，完成感知任务；

所述服务器用于基于人工势场法，使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间的排斥力，进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避障轨迹，完成规划任务；

所述服务器还用于基于预设性能函数根据需求对机械臂的瞬态性能进行约束，设计机械臂轨迹跟踪控制程序，得到机械臂跟踪避障轨迹的关节力矩，并实时控制机械臂按照预设性能跟踪避障轨迹，完成所述控制任务。

优选地，所述服务器用于完成所述感知任务、所述规划任务和所述控制任务中的计算；所述系统的工作流程如下：

双目立体摄像头实时获取机械臂当前运动空间内的障碍物信息；

服务器根据采集到的障碍物信息，基于人工势场法，使用虚拟排斥力计算函数生成当前障碍物在机械臂潜在碰撞点产生的虚拟排斥力；

同时，服务器根据计算得到的虚拟排斥力，基于预设的虚拟阻抗控制模型，利用虚拟阻抗控制算法生成每个关节的避障轨迹；

接着，服务器将机械臂末端执行任务和实时避障任务进行优先级分层，机械臂末端执行任务作为第一优先级，实时避障任务作为第二优先级，设计基于组合运动轨迹的全身控制框架，实现第二优先级任务执行不影响第一优先级任务，即在不影响第一优先级任务执行的情况下，调整机械臂与障碍物之间的距离以保证机械臂系统在执行任务过程中的安全性；

最后，服务器基于反馈控制网络、前馈补偿的径向基函数神经网络、带有瞬态约束的误差约束生成控制程序，计算出机械臂各个关节的控制力矩，以控制机械臂跟踪上述得到的期望位置和期望姿态，使机械臂在执行任务的同时实现实时避障。

本发明实施例提供的技术方案带来的有益效果至少包括：

1、本发明基于冗余机械臂，并搭配双目立体摄像头搭建的机械臂实时避障系统，使得机械臂在协作过程中具备了获取外界信息的能力，因此具备了完成更加复杂任务的硬件基础；

2、本发明将传统的阻抗控制与实时视觉信息相结合设计的虚拟阻抗控制方法结合了这两种控制方法的优势，实现了根据外界环境，实时修改期望轨迹的功能，赋予了机械臂实时避障能力与适应能力；

3、本发明的零空间虚拟阻抗控制方法相比传统的避障方法，利用零空间投影技术，将避障任务投影到机械臂的主任务的零空间中，可以实现不影响主任务进行的实时避障任务，使得机械臂可以更加高效和安全地完成任务；

4、本发明的实时避障控制方法使得机械臂在执行任务过程中，限制了跟踪误差的动态变化，使得跟踪误差的动态变化符合设定的要求，从而提升了机械臂在执行任务时的快速性和准确性。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种机械臂零空间实时避障控制系统的结构示意图；

图2是本发明实施例提供的一种机械臂零空间实时避障控制方法的原理示意图；

图3是本发明实施例中冒险空间的表现形式示意图；

图4是本发明实施例中设定边界示意图。

如图所示，为了能明确实现本发明的实施例的结构，在图中标注了特定的结构和器件，但这仅为示意需要，并非意图将本发明限定在该特定结构、器件和环境中，根据具体需要，本领域的普通技术人员可以将这些器件和环境进行调整或者修改，所进行的调整或者修改仍然包括在本发明的保护范围中。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的实施例提供了一种基于虚拟阻抗和预设性能函数的机械臂零空间实时避障控制方法，所述方法基于包括机械臂、双目立体摄像头、服务器的机械臂零空间实时避障控制系统，参考图1和图2所示，所述方法由感知任务、规划任务与控制任务组成，包括以下步骤：

采集障碍物信息，获取障碍物的三维坐标，完成所述感知任务；

基于人工势场法，使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间的排斥力，进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避障轨迹，完成所述规划任务；

基于预设性能函数根据需求对机械臂的瞬态性能进行约束，设计机械臂轨迹跟踪控制程序，得到机械臂跟踪避障轨迹的关节力矩，并实时控制机械臂按照预设性能跟踪避障轨迹，完成所述控制任务。

其中，机械臂用于执行各项具体操作，双目立体摄像头用于采集障碍物信息，服务器用于完成感知任务、规划任务与控制任务中的计算。

进一步地，所述感知任务包括：得到障碍物表面与潜在碰撞点最近点的三维坐标，其表现形式如下：

其中，

潜在碰撞点的三维坐标通过机械臂前向运动学计算得到，机械臂上的潜在碰撞点和障碍物之间的距离表现形式如下：

至此，完成所述感知任务。

进一步地，所述规划任务包括：根据障碍物与机械臂之间的相对距离，实时生成避障轨迹；所述规划任务由虚拟排斥力函数与虚拟阻抗控制算法组成；

基于人工势场法，规划任务中的虚拟排斥力函数

其中， 冒险空间是包围机械臂的一层虚拟保护外壳，

冒险空间的表现形式如图3所示，其中其中

所述虚拟排斥力函数的作用在于，在三维空间中，在物理意义上直观地作用于机械臂上。

所述虚拟阻抗控制算法的输入是虚拟力矩，输出是机械臂各个关节对应的运动轨迹，例如，当虚拟力矩是为了躲避障碍物生成的虚拟力矩，则对应生成的运动轨迹为避障运动轨迹；所述虚拟阻抗控制算法的表现形式如下：

其中

虚拟排斥力为三维任务空间中的力，虚拟排斥力函数

其中

虚拟排斥力矩结合虚拟阻抗控制算法，得到下述实时避障参考轨迹的加速度：

通过积分

本发明实施例中，所述机械臂为冗余机械臂，关节个数为7个，而工作空间的自由度为6个，这使得机械臂的自由度大于工作空间的自由度，机械臂存在的冗余自由度，使机械臂可以完成次级任务。

机械臂的主任务在机械臂的末端执行器上执行；上述次级任务，即为不妨碍末端执行器执行主任务时，机械臂的身体，包括肩关节、肘关节、腕关节执行的任务；上述冗余机械臂的主任务与次级任务之间组合运动轨迹的表现形式如下：

其中

其中，所述零空间投影技术用于完成主任务与次级任务的优先级分层；所述主任务在机械臂零空间实时避障控制方法中，指末端执行器执行的任务，包括抓取任务、协作装配任务、协作搬运任务；所述次级任务为实时避障任务。

进一步地，所述控制任务包括：基于反馈控制、前馈补偿、误差约束，计算出机械臂各个关节的控制力矩，以控制机械臂跟踪上述得到的期望位置和期望姿态，使机械臂在执行任务的同时实现实时避障。其控制力矩的控制器表现形式如下：

其中，

鲁棒项可将逼近域为半全局的神经网络扩展为作用域为全局的带有鲁棒项的神经网络，当神经网络输入越过神经网络逼近域时，鲁棒项起作用。

其中

其中，

其中，

其中，

其中

其中

通过误差转换得到

误差约束系数可以将上述协作机械臂的跟踪误差约束在设定的预设性能函数内，增强机械臂实时避障系统的快速性。

本发明方法通过获得视觉感知等外部信息获取设备实时传递来的外界动态环境中障碍物的信息，使用人工势场法计算出障碍物与机械臂潜在碰撞点之间的虚拟排斥力，将虚拟排斥力作为虚拟阻抗控制的输入，进而利用虚拟阻抗控制算法生成每个关节的实时运动轨迹。根据实时生成的轨迹，设计基于预设性能函数的轨迹跟踪控制器，得到每个关节的控制力矩，控制力矩驱动机械臂的每个关节按照生成的实时运动轨迹运动，并时刻保证误差瞬态需求，通过机械臂各关节的运动相互配合，最终实现任务空间的动态障碍物的实时躲避。

本发明提出的机械臂零空间实时避障控制方法的优势如下：一、可以局部修改期望轨迹——当没有障碍物时根据初始的期望轨迹运行，存在障碍物时，期望轨迹被局部修改，躲避障碍。二、可以自动恢复期望轨迹——当避障过程完成后，局部修改的期望轨迹将恢复原状态，继续执行原任务。三、可以节约时间地完成障碍物躲避任务——本发明利用机械臂零空间避障，可以实现避障任务与末端执行器任务并行，任务同时执行可节省时间。四、响应时间短——带有瞬态约束的控制器，可以使避障任务根据需求快速动作，从而提高避障的瞬态性能。

相应地，本发明的实施例还提供了一种机械臂零空间实时避障控制系统，包括：

所述系统包括机械臂、双目立体摄像头和服务器，所述机械臂、所述双目立体摄像头、所述服务器支持机器人操作系统（Robot Operation Sysem，ROS）。所述机械臂、所述双目立体摄像头与所述服务器通过ROS通信机制连接，并由所述服务器进行控制。

所述机械臂为七关节冗余机械臂，用于完成布置的各项任务，如协作搬运、协作装配、抓取指定物体等任务；

所述双目立体摄像头用于采集位于所述机械臂运动空间内的障碍物信息，获取障碍物的三维坐标，完成感知任务；

所述服务器用于基于人工势场法，使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间的排斥力，进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避障轨迹，完成规划任务；

所述服务器还用于基于预设性能函数根据需求对机械臂的瞬态性能进行约束，设计机械臂轨迹跟踪控制程序，得到机械臂跟踪避障轨迹的关节力矩，并实时控制机械臂按照预设性能跟踪避障轨迹，完成所述控制任务。

进一步地，所述服务器用于完成所述感知任务、所述规划任务和所述控制任务中的计算。

所述感知任务中，得到机械臂潜在碰撞点与障碍物表面间的最近距离。

所述规划任务中，根据感知层得到的障碍物与机械臂之间的最近距离，实时生成避障轨迹；此计算由虚拟排斥力函数与虚拟阻抗控制算法组成。

所述虚拟排斥力计算函数，在三维空间中，在物理意义上，直观地作用于机械臂上。规划任务中的另一个组成算法——虚拟阻抗控制算法，此算法的输入是虚拟力矩，输出是机械臂各个关节对应的运动轨迹，例如，当虚拟力矩是为了躲避障碍物生成的虚拟力矩，则对应生成的运动轨迹为避障运动轨迹。

所述服务器还用于完成所述规划任务中的分层计算，机械臂存在的冗余自由度，使机械臂可以完成次级任务。机械臂的主任务通常在机械臂的末端执行器上执行；上述次级任务，即为不妨碍末端执行器执行主任务时，机械臂的身体（肩关节、肘关节、腕关节）执行的任务；且上述主任务、次级任务，即可抽象为任务轨迹。

所述的零空间投影技术是指，完成主任务与次级任务的优先级分层。上述主任务在机械臂实时避障系统中，泛指末端执行器执行的任务，如抓取任务，协作装配，协作搬运等任务；上述次级任务为实时避障任务。

所述控制任务中，基于反馈控制、前馈补偿、基于预设性能函数的误差约束，计算出机械臂各个关节的控制力矩，使得机械臂的关节真实轨迹跟踪上述得到的期望位置和期望姿态，令所述冗余机械臂按要求执行任务的同时实现实时避障。

具体地，本实施例中的机械臂可以采用Kinova-Gen3机械臂，双目立体摄像头可以采用ZED2摄像头，服务器优选的配置为运行内存32G，显卡为GeForce GTX Titan X；利用固定支架，将双目立体摄像头ZED2安全、合理地固定在指定位置。

其中，服务器用于同时控制ZED2摄像头和 Kinova-Gen3机械臂；Kinova-Gen3机械臂用于利用其机械臂完成操作者指定任务；ZED2摄像头用于获取机械臂当前运动空间内的障碍物三维坐标信息；Kinova-Gen3机械臂使用Linux操作系统以及ROS控制，可以随时接收Kinova-Gen3机械臂的关节角度、角速度信息，并且可以实时对Kinova-Gen3机械臂发送控制消息。ZED2摄像头同样使用Linux操作系统以及ROS控制，可以实时获取机械臂当前运动空间内的障碍物位置信息。

所述系统的工作流程如下：

双目立体摄像头实时获取机械臂当前运动空间内的障碍物信息；

服务器根据采集到的障碍物信息，基于人工势场法，使用虚拟排斥力计算函数生成当前障碍物在机械臂潜在碰撞点产生的虚拟排斥力；其中，用于计算虚拟排斥力的函数是根据避障要求预先设定好的，并且可在机械臂潜在碰撞点上生成三维方向上的虚拟排斥力；

同时，服务器根据计算得到的虚拟排斥力，基于预设的虚拟阻抗控制模型，利用虚拟阻抗控制算法生成每个关节的避障轨迹；其中，由虚拟阻抗控制生成的每个关节的运动轨迹，在共同运动下，得到潜在碰撞点处的避障轨迹，实现潜在碰撞点的避障任务；调整机械臂与障碍物之间的距离以保证机械臂系统的安全性；

接着，服务器将机械臂末端执行任务和实时避障任务进行优先级分层，机械臂末端执行任务作为第一优先级，实时避障任务作为第二优先级，设计基于组合运动轨迹的全身控制框架，实现第二优先级任务执行不影响第一优先级任务，即在不影响第一优先级任务执行的情况下，调整机械臂与障碍物之间的距离以保证机械臂系统在执行任务过程中的安全性；

最后，服务器基于反馈控制网络、前馈补偿的径向基函数神经网络、带有瞬态约束的误差约束生成控制程序，计算出机械臂各个关节的控制力矩，以控制机械臂跟踪上述得到的期望位置和期望姿态，使机械臂在执行任务的同时实现实时避障，提高协作机器人系统的安全性与高效性。

本实施例中系统的具体执行算法可参考方法实施例，其实现原理和技术效果类似，此处不再赘述。

本实施例基于冗余机械臂，并搭配双目立体摄像头搭建的机械臂实时避障系统提高了机械臂在协作过程中获取外界信息的能力，使得机械臂具备了完成更加复杂协作任务的硬件基础；通过结合人工势场法与虚拟阻抗控制这两种方法，解决了给定的机械臂协作任务下的避障问题，提高了机械臂的对动态环境的适应能力；通过全身控制算法，解决了机械臂多任务执行时的优先级分配问题，提高了机械臂的高效性与灵活性。

需要说明的是，在说明书中提到“一个实施例”、“实施例”、“示例性实施例”、“一些实施例”等指示所述的实施例可以包括特定特征、结构或特性，但未必每个实施例都包括该特定特征、结构或特性。另外，在结合实施例描述特定特征、结构或特性时，结合其它实施例（无论是否明确描述）实现这种特征、结构或特性应在相关领域技术人员的知识范围内。

通常，可以至少部分从上下文中的使用来理解术语。例如，至少部分取决于上下文，本文中使用的术语“一个或多个”可以用于描述单数意义的任何特征、结构或特性，或者可以用于描述复数意义的特征、结构或特性的组合。另外，术语“基于”可以被理解为不一定旨在传达一组排他性的因素，而是可以替代地，至少部分地取决于上下文，允许存在不一定明确描述的其他因素。

如本文使用的，术语“标称/标称地”是指在生产或制造过程的设计阶段期间设置的针对部件或过程操作的特性或参数的期望或目标值，以及高于和/或低于期望值的值的范围。值的范围可能是由于制造过程或容限中的轻微变化导致的。如本文使用的，术语“大约”指示可以基于与主题半导体器件相关联的特定技术节点而变化的给定量的值。基于特定技术节点，术语“大约”可以指示给定量的值，其例如在值的5%-15%（例如，值的±5%、±10%或±15%）内变化。

可以理解的是，本公开中的“在……上”、“在……之上”和“在……上方”的含义应当以最宽方式被解读，以使得“在……上”不仅表示“直接在”某物“上”而且还包括在某物“上”且其间有居间特征或层的含义，并且“在……之上”或“在……上方”不仅表示“在”某物“之上”或“上方”的含义，而且还可以包括其“在”某物“之上”或“上方”且其间没有居间特征或层的含义。

此外，诸如“在…之下”、“在…下方”、“下部”、“在…之上”、“上部”等空间相关术语在本文中为了描述方便可以用于描述一个元件或特征与另一个或多个元件或特征的关系，如在附图中示出的。空间相关术语旨在涵盖除了在附图所描绘的取向之外的在设备使用或操作中的不同取向。设备可以以另外的方式被定向，并且本文中使用的空间相关描述词可以类似地被相应解释。

本发明涵盖任何在本发明的精髓和范围上做的替代、修改、等效方法以及方案。为了使公众对本发明有彻底的了解，在以下本发明优选实施例中详细说明了具体的细节，而对本领域技术人员来说没有这些细节的描述也可以完全理解本发明。另外，为了避免对本发明的实质造成不必要的混淆，并没有详细说明众所周知的方法、过程、流程、元件和电路等。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于计算机可读取存储介质中，如：ROM/RAM、磁碟、光盘等。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。