一种机械臂避障方法、装置、电子设备及存储介质

技术领域

本申请涉及机器人控制技术领域，具体而言，涉及一种机械臂避障方法、装置、电子设备及存储介质。

背景技术

对于需要机械臂和人协同工作的应用场景，为避免机械臂对人员造成伤害，一般会在机械臂上设置相机作为视觉传感器来感知周围的环境，利用视觉识别人员和机械臂的位置，控制机械臂停止运动或避障来保障人员安全。

然而，视觉传感器对光照比较敏感，在光照条件不好的情况下，难以有效感知环境，另外，视觉传感器容易受到物体遮挡，从而产生视觉盲区，也会存在安全隐患。另外，机械臂在进行避障时，容易导致机械臂末端的姿态发生改变，从而导致一些不可控问题，危害人员的安全；例如，当机械臂用于搬运装有液体的开口罐子时，如果末端姿态改变，会导致液体洒出跌落在人员身上；当机械臂的末端通过吸盘吸取工件时，如果末端姿态改变，可能导致吸力的方向与重力方向垂直的情况，从而导致工件掉落砸伤人员。

发明内容

本申请的目的在于提供一种机械臂避障方法、装置、电子设备及存储介质，可提高避障过程的安全性。

第一方面，本申请提供了一种机械臂避障方法，用于控制机械臂进行避障，所述机械臂的表面设置有多个接近传感器，包括步骤：

A1.获取由各所述接近传感器测得的障碍物的位置；

A2.根据所述障碍物的位置计算各所述接近传感器受到的虚拟力；

A3.根据各所述接近传感器受到的虚拟力，计算所述机械臂受到的总虚拟力；

A4.根据所述总虚拟力，结合所述机械臂在笛卡尔空间的阻抗控制模型和动力学模型，计算所述机械臂的关节力矩；

A5.根据所述关节力矩控制所述机械臂的各关节运动，实现避障。

由于接近传感器作为环境的感知传感器没有视觉传感器的对光照敏感、容易被遮挡等缺陷，且具有响应快、灵敏度高的优点，因此采用接近传感器作为环境的感知传感器时，机械臂在靠近物体但尚未接触的情况下可及时做出响应，有利于提高避障过程的安全性；此外，通过结合虚拟力、阻抗控制模型和动力学模型实现机械臂在笛卡尔空间的避障，可实现在不改变机械臂末端姿态的情况下进行有效避障，从而避免由于机械臂末端的姿态改变而导致不可控问题的情况，进一步提高避障过程的安全性。

优选地，步骤A2包括：

根据各所述接近传感器测得的所述障碍物的位置，计算所述障碍物与各所述接近传感器之间的距离；

根据以下公式计算各所述接近传感器受到的所述虚拟力的模：

；

其中，

以从各所述接近传感器指向所述障碍物的方向向量作为对应的所述虚拟力的方向向量，根据所述方向向量和所述模计算所述虚拟力在对应的传感器坐标系下的三个坐标分量。

该虚拟力是一种虚拟的斥力，接近传感器越靠近障碍物，则斥力越大，从而使用后续计算得到的关节力矩对机械臂进行控制时，能够使机械臂以更大的加速度远离障碍物运动，有效避免机械臂与障碍物碰撞。

优选地，步骤A3包括：

A301.通过坐标变换把所述虚拟力换算为机械臂基坐标系下的标准虚拟力；

A302.根据所述标准虚拟力计算所述机械臂受到的总虚拟力。

优选地，步骤A301包括：

根据以下公式把所述虚拟力换算为机械臂基坐标系下的标准虚拟力：

；

其中，

优选地，步骤A302包括：

根据以下公式计算所述机械臂受到的总虚拟力：

；

其中，

优选地，所述阻抗控制模型为：

；/>

其中，

优选地，所述动力学模型为：

；

其中，

使用上述的阻抗控制模型和动力学模型计算机械臂的关节力矩，可实现笛卡尔空间的动态避障，从而在不改变机械臂末端姿态的情况下进行避障。

第二方面，本申请提供了一种机械臂避障装置，用于控制机械臂进行避障，所述机械臂的表面设置有多个接近传感器，包括：

第一获取模块，用于获取由各所述接近传感器测得的障碍物的位置；

第一计算模块，用于根据所述障碍物的位置计算各所述接近传感器受到的虚拟力；

第二计算模块，用于根据各所述接近传感器受到的虚拟力，计算所述机械臂受到的总虚拟力；

第三计算模块，用于根据所述总虚拟力，结合所述机械臂在笛卡尔空间的阻抗控制模型和动力学模型，计算所述机械臂的关节力矩；

避障模块，用于根据所述关节力矩控制所述机械臂的各关节运动，实现避障。

由于接近传感器作为环境的感知传感器没有视觉传感器的对光照敏感、容易被遮挡等缺陷，且具有响应快、灵敏度高的优点，因此采用接近传感器作为环境的感知传感器时，机械臂在靠近物体但尚未接触的情况下可及时做出响应，有利于提高避障过程的安全性；此外，通过结合虚拟力、阻抗控制模型和动力学模型实现机械臂在笛卡尔空间的避障，可实现在不改变机械臂末端姿态的情况下进行有效避障，从而避免由于机械臂末端的姿态改变而导致不可控问题的情况，进一步提高避障过程的安全性。

第三方面，本申请提供了一种电子设备，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如前文所述机械臂避障方法中的步骤。

第四方面，本申请提供了一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时运行如前文所述机械臂避障方法中的步骤。

有益效果：

本申请提供的机械臂避障方法、装置、电子设备及存储介质，由于接近传感器作为环境的感知传感器没有视觉传感器的对光照敏感、容易被遮挡等缺陷，且具有响应快、灵敏度高的优点，因此采用接近传感器作为环境的感知传感器时，机械臂在靠近物体但尚未接触的情况下可及时做出响应，有利于提高避障过程的安全性；此外，通过结合虚拟力、阻抗控制模型和动力学模型实现机械臂在笛卡尔空间的避障，可实现在不改变机械臂末端姿态的情况下进行有效避障，从而避免由于机械臂末端的姿态改变而导致不可控问题的情况，进一步提高避障过程的安全性。

附图说明

图1为本申请实施例提供的机械臂避障方法的流程图。

图2为本申请实施例提供的机械臂避障装置的结构示意图。

图3为本申请实施例提供的电子设备的结构示意图。

图4为示例性的接近传感器的分布图。

标号说明：1、第一获取模块；2、第一计算模块；3、第二计算模块；4、第三计算模块；5、避障模块；301、处理器；302、存储器；303、通信总线。

具体实施方式

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

请参照图1，图1是本申请一些实施例中的一种机械臂避障方法，用于控制机械臂进行避障，机械臂的表面设置有多个接近传感器，包括步骤：

A1.获取由各接近传感器测得的障碍物的位置；

A2.根据障碍物的位置计算各接近传感器受到的虚拟力；

A3.根据各接近传感器受到的虚拟力，计算机械臂受到的总虚拟力；

A4.根据总虚拟力，结合机械臂在笛卡尔空间的阻抗控制模型和动力学模型，计算机械臂的关节力矩；

A5.根据关节力矩控制机械臂的各关节运动，实现避障。

由于接近传感器作为环境的感知传感器没有视觉传感器的对光照敏感、容易被遮挡等缺陷，且具有响应快、灵敏度高的优点，因此采用接近传感器作为环境的感知传感器时，机械臂在靠近物体但尚未接触的情况下可及时做出响应，有利于提高避障过程的安全性；此外，通过结合虚拟力、阻抗控制模型和动力学模型实现机械臂在笛卡尔空间的避障，可实现在不改变机械臂末端姿态的情况下进行有效避障，从而避免由于机械臂末端的姿态改变而导致不可控问题的情况，进一步提高避障过程的安全性。

此外，在计算得到机械臂的关节力矩后，直接按计算结果控制电机的力矩环，比传统的基于位置环的避障方法相比，响应速度更快，动态性能更好。

其中，可在机械臂的最末端的一个或两个关节的臂杆上设置多个接近传感器，例如，机械臂为6轴机械臂，从根部到末端，各关节依次为1号关节、2号关节、3号关节、4号关节、5号关节、6号关节，则可在6号关节的臂杆上设置多个接近传感器，也可在5号关节的臂杆和6号关节的臂杆上均设置多个接近传感器。优选地，多个接近传感器在臂杆的外表面绕臂杆的周向均匀排布，以感知各个方向上的障碍物的位置。其中，个接近传感器的数量根据接近传感器的角度检测范围设置，需要保证同一臂杆上的所有接近传感器的总检测角度覆盖360°范围。

例如图4所示的机械臂，其最末端的两个关节的臂杆上均设置多个接近传感器，且两个臂杆上的多个接近传感器均沿周向均匀排布。

此处，接近传感器测得的障碍物的位置为三维位置数据，包括在该接近传感器的传感器坐标系下的三个坐标值。需要说明的是，接近传感器的实际角度检测范围较小，当接近传感器的角度检测范围内存在多个障碍物时，接近传感器只会返回最近的障碍物的距离，因此，每个接近传感器测得的障碍物的位置只有一个。当接近传感器的角度检测范围内没有障碍物或障碍物的距离超出该接近传感器的最大感应距离时，接近传感器检测不到障碍物，此时可把该接近传感器测得的障碍物的位置设置为预设位置（可根据实际需要设置），该预设位置与该接近传感器的距离超过该接近传感器的最大感应距离。

在一些优选实施方式中，步骤A2包括：

A201.根据各接近传感器测得的障碍物的位置，计算障碍物与各接近传感器之间的距离；

A202.根据以下公式计算各接近传感器受到的虚拟力的模（即虚拟力的大小）：

（1）；

其中，

A203.以从各接近传感器指向障碍物的方向向量作为对应的虚拟力的方向向量，根据该方向向量和模计算虚拟力在对应的传感器坐标系下的三个坐标分量。

该虚拟力是一种虚拟的斥力，接近传感器越靠近障碍物，则斥力越大，从而使用后续计算得到的关节力矩对机械臂进行控制时，能够使机械臂以更大的加速度远离障碍物运动，有效避免机械臂与障碍物碰撞。

通过上述公式（1）计算虚拟力的模，当接近传感器越靠近障碍物，则虚拟力的模越大，从而可使机械臂越靠近障碍物就用越大的加速度远离障碍物，大大地提高了避障可靠性，而且，虚拟力的模存在上限值

在步骤A201中，可根据以下公式计算障碍物与各接近传感器之间的距离：

；

其中，

在步骤A203中，从各接近传感器指向障碍物的方向向量为：

；

其中，

；

；

；

；

其中，

具体地，步骤A3包括：

A301.通过坐标变换把虚拟力换算为机械臂基坐标系下的标准虚拟力；

A302.根据标准虚拟力计算机械臂受到的总虚拟力。

由于前文计算得到的虚拟力是基于相应的接近传感器的传感器坐标系下的位置坐标进行计算的，为得到机械臂受到的总虚拟力，需要先把这些虚拟力转换为统一坐标系下的标准虚拟力，一般地，在控制机械臂运动时，是基于机械臂基坐标系进行控制的，因此，把这些虚拟力转换为机械臂基坐标系下的标准虚拟力。

其中，步骤A301包括：

根据以下公式把虚拟力换算为机械臂基坐标系下的标准虚拟力：

；

其中，

其中，步骤A302包括：

根据以下公式计算机械臂受到的总虚拟力：

（2）；

其中，

根据上述公式（2）计算总虚拟力，当所有标准虚拟力的合力的模小于预设的虚拟力阈值时，总虚拟力等于所有标准虚拟力的合力，否则，使总虚拟力的大小等于该虚拟力阈值，并使总虚拟力方向与该合力的方向相同；从而，一方面保证机械臂避障方向正确，另一方面避免由于计算得到的总虚拟力过大导致最后计算得到的关节力矩超过机械臂关节能够提供的力矩，保证最后计算得到的关节力矩是可达到的。

步骤A4中计算机械臂的关节力矩，实际上是需要机械臂关节提供的力矩。

在本实施例中，阻抗控制模型为笛卡尔空间的阻抗控制模型，具体地，该阻抗控制模型为：

（3）；

其中，

在本实施例中，动力学模型为笛卡尔空间的动力学模型，具体地，该动力学模型为：

（4）；

其中，

结合公式（3）和（4），可以得到：

（5）；

从而根据上述公式（5）可计算得到机械臂的关节力矩。

使用上述的阻抗控制模型和动力学模型计算机械臂的关节力矩，可实现笛卡尔空间的动态避障，从而在不改变机械臂末端姿态的情况下进行避障。

在步骤A5中，把计算得到的关节力矩

由上可知，该机械臂避障方法，通过获取由各接近传感器测得的障碍物的位置；根据障碍物的位置计算各接近传感器受到的虚拟力；根据各接近传感器受到的虚拟力，计算机械臂受到的总虚拟力；根据总虚拟力，结合机械臂在笛卡尔空间的阻抗控制模型和动力学模型，计算机械臂的关节力矩；根据关节力矩控制机械臂的各关节运动，实现避障；从而提高避障过程的安全性。

参考图2，本申请提供了一种机械臂避障装置，用于控制机械臂进行避障，机械臂的表面设置有多个接近传感器，包括：

第一获取模块1，用于获取由各接近传感器测得的障碍物的位置；

第一计算模块2，用于根据障碍物的位置计算各接近传感器受到的虚拟力；

第二计算模块3，用于根据各接近传感器受到的虚拟力，计算机械臂受到的总虚拟力；

第三计算模块4，用于根据总虚拟力，结合机械臂在笛卡尔空间的阻抗控制模型和动力学模型，计算机械臂的关节力矩；

避障模块5，用于根据关节力矩控制机械臂的各关节运动，实现避障。

由于接近传感器作为环境的感知传感器没有视觉传感器的对光照敏感、容易被遮挡等缺陷，且具有响应快、灵敏度高的优点，因此采用接近传感器作为环境的感知传感器时，机械臂在靠近物体但尚未接触的情况下可及时做出响应，有利于提高避障过程的安全性；此外，通过结合虚拟力、阻抗控制模型和动力学模型实现机械臂在笛卡尔空间的避障，可实现在不改变机械臂末端姿态的情况下进行有效避障，从而避免由于机械臂末端的姿态改变而导致不可控问题的情况，进一步提高避障过程的安全性。

此外，在计算得到机械臂的关节力矩后，直接按计算结果控制电机的力矩环，比传统的基于位置环的避障方法相比，响应速度更快，动态性能更好。

其中，可在机械臂的最末端的一个或两个关节的臂杆上设置多个接近传感器，例如，机械臂为6轴机械臂，从根部到末端，各关节依次为1号关节、2号关节、3号关节、4号关节、5号关节、6号关节，则可在6号关节的臂杆上设置多个接近传感器，也可在5号关节的臂杆和6号关节的臂杆上均设置多个接近传感器。优选地，多个接近传感器在臂杆的外表面绕臂杆的周向均匀排布，以感知各个方向上的障碍物的位置。其中，个接近传感器的数量根据接近传感器的角度检测范围设置，需要保证同一臂杆上的所有接近传感器的总检测角度覆盖360°范围。

例如图4所示的机械臂，其最末端的两个关节的臂杆上均设置多个接近传感器，且两个臂杆上的多个传感器均沿周向均匀排布。

此处，接近传感器测得的障碍物的位置为三维位置数据，包括在该接近传感器的传感器坐标系下的三个坐标值。需要说明的是，接近传感器的实际角度检测范围较小，当接近传感器的角度检测范围内存在多个障碍物时，接近传感器只会返回最近的障碍物的距离，因此，每个接近传感器测得的障碍物的位置只有一个。当接近传感器的角度检测范围内没有障碍物或障碍物的距离超出该接近传感器的最大感应距离时，接近传感器检测不到障碍物，此时可把该接近传感器测得的障碍物的位置设置为预设位置（可根据实际需要设置），该预设位置与该接近传感器的距离超过该接近传感器的最大感应距离。

在一些优选实施方式中，第一计算模块2在根据障碍物的位置计算各接近传感器受到的虚拟力的时候，执行：

根据各接近传感器测得的障碍物的位置，计算障碍物与各接近传感器之间的距离；

根据以下公式计算各接近传感器受到的虚拟力的模（即虚拟力的大小）：

（1）；

其中，

以从各接近传感器指向障碍物的方向向量作为对应的虚拟力的方向向量，根据该方向向量和模计算虚拟力在对应的传感器坐标系下的三个坐标分量。

该虚拟力是一种虚拟的斥力，接近传感器越靠近障碍物，则斥力越大，从而使用后续计算得到的关节力矩对机械臂进行控制时，能够使机械臂以更大的加速度远离障碍物运动，有效避免机械臂与障碍物碰撞。

通过上述公式（1）计算虚拟力的模，当接近传感器越靠近障碍物，则虚拟力的模越大，从而可使机械臂越靠近障碍物就用越大的加速度远离障碍物，大大地提高了避障可靠性，而且，虚拟力的模存在上限值

第一计算模块2在根据各接近传感器测得的障碍物的位置，计算障碍物与各接近传感器之间的距离的时候，可根据以下公式计算障碍物与各接近传感器之间的距离：

；

其中，

其中，从各接近传感器指向障碍物的方向向量为：

；

其中，

；

；

；

；

其中，

具体地，第二计算模块3在根据各接近传感器受到的虚拟力，计算机械臂受到的总虚拟力的时候，执行：

通过坐标变换把虚拟力换算为机械臂基坐标系下的标准虚拟力；

根据标准虚拟力计算机械臂受到的总虚拟力。

由于前文计算得到的虚拟力是基于相应的接近传感器的传感器坐标系下的位置坐标进行计算的，为得到机械臂受到的总虚拟力，需要先把这些虚拟力转换为统一坐标系下的标准虚拟力，一般地，在控制机械臂运动时，是基于机械臂基坐标系进行控制的，因此，把这些虚拟力转换为机械臂基坐标系下的标准虚拟力。

其中，第二计算模块3在通过坐标变换把虚拟力换算为机械臂基坐标系下的标准虚拟力的时候，执行：

根据以下公式把虚拟力换算为机械臂基坐标系下的标准虚拟力：

；

其中，

其中，第二计算模块3在根据标准虚拟力计算机械臂受到的总虚拟力的时候，执行：

根据以下公式计算机械臂受到的总虚拟力：

（2）；

其中，

根据上述公式（2）计算总虚拟力，当所有标准虚拟力的合力的模小于预设的虚拟力阈值时，总虚拟力等于所有标准虚拟力的合力，否则，使总虚拟力的大小等于该虚拟力阈值，并使总虚拟力方向与该合力的方向相同；从而，一方面保证机械臂避障方向正确，另一方面避免由于计算得到的总虚拟力过大导致最后计算得到的关节力矩超过机械臂关节能够提供的力矩，保证最后计算得到的关节力矩是可达到的。

第三计算模块4计算得到的机械臂的关节力矩，实际上是需要机械臂关节提供的力矩。

在本实施例中，阻抗控制模型为笛卡尔空间的阻抗控制模型，具体地，该阻抗控制模型为：

（3）；

其中，

在本实施例中，动力学模型为笛卡尔空间的动力学模型，具体地，该动力学模型为：

（4）；

其中，

结合公式（3）和（4），可以得到：

（5）；

从而根据上述公式（5）可计算得到机械臂的关节力矩。

使用上述的阻抗控制模型和动力学模型计算机械臂的关节力矩，可实现笛卡尔空间的动态避障，从而在不改变机械臂末端姿态的情况下进行避障。

避障模块5在根据关节力矩控制机械臂的各关节运动的时候，把计算得到的关节力矩

由上可知，该机械臂避障装置，通过获取由各接近传感器测得的障碍物的位置；根据障碍物的位置计算各接近传感器受到的虚拟力；根据各接近传感器受到的虚拟力，计算机械臂受到的总虚拟力；根据总虚拟力，结合机械臂在笛卡尔空间的阻抗控制模型和动力学模型，计算机械臂的关节力矩；根据关节力矩控制机械臂的各关节运动，实现避障；从而提高避障过程的安全性。

请参照图3，图3为本申请实施例提供的一种电子设备的结构示意图，本申请提供一种电子设备，包括：处理器301和存储器302，处理器301和存储器302通过通信总线303和/或其他形式的连接机构（未标出）互连并相互通讯，存储器302存储有处理器301可执行的计算机程序，当电子设备运行时，处理器301执行该计算机程序，以执行上述实施例的任一可选的实现方式中的机械臂避障方法，以实现以下功能：获取由各接近传感器测得的障碍物的位置；根据障碍物的位置计算各接近传感器受到的虚拟力；根据各接近传感器受到的虚拟力，计算机械臂受到的总虚拟力；根据总虚拟力，结合机械臂在笛卡尔空间的阻抗控制模型和动力学模型，计算机械臂的关节力矩；根据关节力矩控制机械臂的各关节运动，实现避障。

本申请实施例提供一种存储介质，其上存储有计算机程序，计算机程序被处理器执行时，执行上述实施例的任一可选的实现方式中的机械臂避障方法，以实现以下功能：获取由各接近传感器测得的障碍物的位置；根据障碍物的位置计算各接近传感器受到的虚拟力；根据各接近传感器受到的虚拟力，计算机械臂受到的总虚拟力；根据总虚拟力，结合机械臂在笛卡尔空间的阻抗控制模型和动力学模型，计算机械臂的关节力矩；根据关节力矩控制机械臂的各关节运动，实现避障。其中，存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（Static Random Access Memory,简称SRAM），电可擦除可编程只读存储器（Electrically Erasable Programmable Read-Only Memory, 简称EEPROM），可擦除可编程只读存储器（Erasable Programmable ReadOnly Memory, 简称EPROM），可编程只读存储器（Programmable Red-Only Memory, 简称PROM），只读存储器（Read-Only Memory, 简称ROM），磁存储器，快闪存储器，磁盘或光盘。

在本申请所提供的实施例中，应该理解到，所揭露装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

另外，作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，既可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

再者，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。

以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。