CN113119100B - 冗余机械臂控制方法、装置、可读存储介质及设备 - Google Patents

Abstract

本申请公开了一种冗余机械臂控制方法、装置、可读存储介质及设备，所述方法包括：获取所述机械臂的末端所受到的外力以及每个关节所受到的外力力矩；根据每个关节对末端在每个运动维度上的影响程度以及所述末端受到的外力，计算每个关节的第一关节速度；根据连杆所受到的外力对每个关节产生的连杆力矩，确定各个关节对应末端当前位置的零空间速度，所述零空间速度为在连杆外力作用下维持末端位置不变时各个所述关节对应的速度；根据所述第一关节速度以及所述零空间速度计算总关节速度；根据所述总关节速度控制所述机械臂运动。由于在控制机械臂时，叠加了末端零空间，因此，机械臂受到外力时，不会影响末端上任务的执行。

Description

冗余机械臂控制方法、装置、可读存储介质及设备

技术领域

本申请涉及自动控制技术领域，具体而言，涉及一种冗余机械臂控制方法、装置、可读存储介质及设备。

背景技术

机械臂是指高精度，多输入多输出、高度非线性、强耦合的复杂系统。因其独特的操作灵活性，已在工业装配，安全防爆等领域得到广泛应用。机械臂是一个复杂系统，存在着参数摄动、外界干扰及未建模动态等不确定性。因而机械臂的建模模型也存在着不确定性，对于不同的任务，需要规划机械臂关节空间的运动轨迹，从而级联构成末端位姿。对于机械臂而言，其在运动过程中，可能会遇到障碍物等从而会使得机械臂受到外力。

目前，在对机械臂进行调整时，通常是直接根据机械臂的末端受力情况来计算末端的速度偏差，然后再根据末端速度的偏差来调整机械臂的关节。当有外力作用在机械臂上时，机械臂的本身的控制量并不会发生改变，机械臂将与障碍物发生碰撞，产生大的冲击力。若采用机械臂在末端受力情况下的调整方法，机械臂会在连杆所受外力作用下带动机械臂末端移动，从而导致机械臂末端的实际位置会与执行主任务的轨迹产生偏差。

发明内容

为了至少克服现有技术中的上述不足，本申请的目的之一在于提供一种冗余机械臂控制方法，应用于机械臂控制设备，所述机械臂控制设备与机械臂电性连接，所述方法包括：

获取所述机械臂的末端所受到的外力以及每个关节所受到的外力力矩；

根据每个关节对末端在每个运动维度上的影响程度以及所述末端受到的外力，计算每个关节的第一关节速度，所述第一关节速度为该关节对应所述末端所受外力的速度的分量；

根据连杆所受到的外力对每个关节产生的连杆力矩，确定各个关节对应末端当前位置的零空间速度，所述零空间速度为在连杆外力作用下维持末端位置不变时各个所述关节对应的速度；

根据所述第一关节速度以及所述零空间速度计算总关节速度；

根据所述总关节速度控制所述机械臂运动。

可选地，所述根据所述末端受到的外力以及每个关节对末端在每个运动维度上的影响程度计算每个关节的第一关节速度的步骤包括：

获取所述机械臂末端的笛卡尔参数，所述笛卡尔参数包括末端的每个维度的质量参数、每个维度的阻尼参数以及每个维度的刚度参数；

根据所述笛卡尔参数以及末端受到的外力计算末端的速度偏差，所述速度偏差为末端当前速度与受阻任务的期望速度之间的偏差；

根据末端的速度偏差计算末端的目标速度；

根据所述末端的目标速度计算每个关节的第一关节速度。

可选地，末端的加速度偏差

末端的速度偏差

末端的位移偏差

与末端所受外力的关系为：

所述根据所述笛卡尔参数以及末端受到的外力计算末端的速度偏差的计算公式为：

其中，F_ext为末端所受外力在各个运动维度上的分量的矩阵，M_e为末端每个维度的质量参数构成的矩阵，B_e为末端每个维度的阻尼参数构成的矩阵，K_e为末端每个维度的刚度参数构成的矩阵，s为拉普拉斯变换。

可选地，所述根据连杆所受到的外力对每个关节产生的连杆力矩，确定各个关节对应末端当前位置的零空间速度的步骤包括：

计算连杆上所受外力对每个关节产生的第一力矩；

根据每个所述关节的所述第一力矩计算对应的速度偏差，所述速度偏差为该关节的实际速度与期望速度之差；

根据每个关节的权重系数、每个关节的速度偏差计算关节的速度控制权重；

根据末端受到的外力以及每个关节对末端在每个运动维度上的影响程度构成的第一矩阵计算所述机械臂的零空间基向量，所述零空间基向量的每个元素代表一个关节的速度；

根据所述速度控制权重以及所述零空间基向量计算零空间速度。

可选地，所述计算连杆上所受外力对每个关节产生的第一力矩的计算公式为：

τ_ext＝τ_link+J^TF_ext

τ_link＝τ_ext-J^TF_ext

其中，τ_link为连杆所受外力对每个关节所产生的力矩构成的矩阵，τ_ext为关节上所受到的外力矩的总量，J^T为末端受到的外力以及每个关节对末端在每个运动维度上的影响程度所构成的矩阵的转置，F_ext为末端所受外力在各个运动维度上的分量构成的矩阵。

可选地，所述根据每个所述连杆力矩计算每个所述关节的速度偏差的计算公式为：

其中，

为关节的位移偏差构成的矩阵，

为关节的速度偏差构成的矩阵，

为关节的角速度偏差构成的矩阵，M_J为每个关节的质量参数构成的矩阵，B_J为每个关节的阻尼参数构成的矩阵，K_J为每个关节的刚度参数构成的矩阵，s为拉普拉斯变换。

可选地，所述机械臂中预先配置有优化的目标函数，目标函数为：

所述根据每个关节的权重系数、每个关节的速度偏差计算每个关节的速度控制权重的计算公式为：

其中，

其中，

为零空间速度，

为优化的目标函数值。

本申请的另一目的在于提供一种冗余机械臂控制装置，应用于机械臂控制设备，所述机械臂控制设备与机械臂电性连接，所述装置包括：

获取模块，用于获取所述机械臂的末端所受到的外力以及每个关节所受到的外力力矩；

第一计算模块，用于根据每个关节对末端在每个运动维度上的影响程度以及所述末端受到的外力，计算每个关节的第一关节速度，所述第一关节速度为该关节对应所述末端所受外力的速度的分量；

第二计算模块，用于根据连杆所受到的外力对每个关节产生的连杆力矩，确定各个关节对应末端当前位置的零空间速度，所述零空间速度为在连杆外力作用下维持末端位置不变时各个所述关节对应的速度；

第三计算模块，用于根据所述第一关节速度以及所述零空间速度计算总关节速度；

控制模块。用于根据所述总关节速度控制所述机械臂运动。

本申请的另一目的在于提供一种可读存储介质，所述可读存储介质存储有可执行程序，处理器在执行所述可执行程序时，实现如本申请任一项所述的方法。

本申请的另一目的在于提供一种冗余机械臂控制设备，包括存储器和处理器，所述存储器和所述处理器电性连接，所述存储器中存储有可执行程序，所述处理器在执行所述可执行程序时，实现如本申请任一项所述的方法。

相对于现有技术而言，本申请具有以下有益效果：

本申请实施例提供的冗余机械臂控制方法、装置、可读存储介质及设备，通过计算机械臂末端的速度偏差，并根据机械臂末端的速度偏差计算出机械臂各个关节的第一关节速度；同时，计算出机械臂的零空间基向量，并计算出各个机械臂在零空间下的零空间速度；最后将第一关节速度和零空间速度叠加起来作为各个关节的总关节速度来对关节进行控制，实现机械臂的柔顺响应，在机械臂受到外力影响时，不会对机械臂末端的运动情况产生影响，避免对机械臂上正在执行的任务产生影响。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1是本申请实施例提供的冗余机械臂控制设备的结构示意框图；

图2是本申请实施例提供的冗余机械臂控制方法的流程示意图一；

图3是本申请实施例提供的冗余机械臂控制方法的流程示意图二；

图4是本申请实施例提供的冗余机械臂控制方法的流程示意图三；

图5是本申请实施例提供的及冗余机械臂控制装置的结构示意图。

图标：100-机械臂控制设备；110-机械臂控制装置；111-获取模块；112-第一计算模块；113-第二计算模块；114-第三计算模块；115-控制模块；120-存储器；130-处理器。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。

因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

在本申请的描述中，还需要说明的是，除非另有明确的规定和限定，术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本申请中的具体含义。

机械臂是由多个连杆连接而成的结构，每两个连杆之间为机械臂的关节，位于机械臂末端未连接其他结构的连杆的一端即机械臂的末端。机械臂在运动过程中，为了避免机械臂在触碰到障碍物时，按照受阻任务(第一任务)运动导致机械臂或者外物受到损伤，通常会在检测到机械臂受到外力时，对机械臂的运动状态进行调整，使机械臂在碰到障碍物后执行新的任务(第二任务)，例如，使机械臂的末端按照特定的轨迹远离障碍物等。

现有技术中，在检测到机械臂末端受到外力时，会直接通过调节末端的速度来使末端执行新的任务，但是这种方式中，一旦机械臂的连杆受到外力，就会导致机械臂的新的任务受到影响不能正常执行。

为了解决上述技术问题，本申请提供了一种用于对冗余机械臂进行控制的机械臂控制方案。以下结合具体的实施例来对该方案进行详细阐述。

请参照图1，图1是本申请实施例提供的机械臂控制设备100(冗余机械臂控制设备)的结构示意框图，所述机械臂控制设备100包括机械臂控制装置110，存储器120和处理器130，存储器120和处理器130相互之间直接或间接电性连接，用于实现数据交互。例如，这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。所述机械臂控制装置110包括至少一个可以软件或固件(firmware)的形式存储于所述存储器120中或固化在所述机械臂控制设备100的操作系统(Operating System，OS)中的软件功能模块。所述处理器130用于执行所述存储器120中存储的可执行模块，例如，所述机械臂控制装置110所包括的软件功能模块及计算机程序等。

在机械臂中，每个关节的运动情况对末端的运动情况所产生的影响并不相同。对于一个已知的机械臂，首先计算出机械臂中，每个关节转动的角度大小对末端位置所产生的影响大小的状态方程，在计算时，可以通过现有的方式来进行，例如，可以采用前向运动学来进行计算。对所述状态方程求偏导数，可以获得所述状态方式的雅克比矩阵J，以下简称为第一矩阵，其中，雅克比矩阵J为一个M行，N列的矩阵，M与末端的运动维度相等，N与关节的数量相等，雅克比矩阵中的第i行第j列(0<i<M，0<j<N)元素代表每个关节末端的第i个运动维度的运动情况受到第j个关节的影响程度。

在机械臂的末端，可以设置第一传感器用于采集末端所受到的外力，在机械臂的每个关节上设置第二传感器用于采集机械臂上连杆所受外力在每个关节上产生的力矩。其中第一传感器为力传感器，第二传感器为力矩传感器，当机械臂的末端的维度(运动维度)为6维时，第一传感器可以是六维力矩传感器。

在本方案中，第一传感器采集的值用F_ext表示F_ext是一个Mx1的矩阵，其中，第i个元素代表末端所受外力在第i个运动维度上的分量；第二传感器所采集到的力矩为τ_ext，τ_ext同样为一个N x1的矩阵，其中，第j个元素代表第j个关节上的力矩。在配置好第一传感器和第二传感器后，便可以从第一传感器获取第一传感器所采集的外力的结果，以及第二传感器所采集的对应关节的外力矩的结果。然后结合上述雅克比矩阵、第一传感器和每个第二传感器所采集的结果可以判断出机械臂整体的受力情况。具体如下：

如果第一传感器和第二传感器均有值，且τ_ext＝J^TF_ext≠0，则末端受力，其中J^T为雅克比矩阵J的转置。

若仅关节力矩传感器有值，即τ_ext≠0，F_ext＝0，则连杆受力。

若二者皆有值且大小不等，即τ_ext≠0，F_ext≠0，τ_ext≠J^TF_ext，则末端与连杆均有受力。若二者皆为零，即τ_ext＝0，F_ext＝0，则机械臂不受外力作用。

请参照图2，图2是本实施例提供的机械臂控制方法的流程示意图，所述机械臂控制方法包括步骤S110-步骤S150。以下结合步骤S110-步骤S150来对所述方法进行详细阐述。

步骤S110，获取所述机械臂的末端所受到的外力以及每个关节所受到的外力力矩。

步骤S120，计算每个关节的第一关节速度。

根据每个关节对末端在每个运动维度上的影响程度以及所述末端受到的外力，计算每个关节的第一关节速度，所述第一关节速度为该关节对应所述末端所受外力的速度的分量。

步骤S130，确定各个关节对应末端当前位置的零空间速度。

具体地，根据连杆所受到的外力对每个关节产生的连杆力矩，确定各个关节对应末端当前位置的零空间速度，所述零空间速度为在连杆外力作用下维持末端位置不变时各个所述关节对应的速度。

步骤S140，根据所述第一关节速度以及所述零空间速度计算总关节速度。

步骤S150，根据所述总关节速度控制所述机械臂运动。

本实施例中，通过机械臂末端所受到的外力计算出每个关节对应该外力下控制机械臂的速度分量，同时根据末端零空间将连杆上的外力映射到零空间中，计算出各个关节在零空间中的零空间速度，然后将零空间速度与第一关节速度叠加后作为关节的控制量，从而实现对各个关节的控制，由于将连杆受到的力在末端零空间中进行了映射，因此，当机械臂的连杆受力时，机械臂的连杆和关节会在零空间中运动，从而能够避免机械臂末端受到影响，实现机械臂的柔顺响应。

请参照图3所示，可选地，本实施例中，所述根据所述末端受到的外力以及每个关节对末端在每个运动维度上的影响程度计算每个关节的第一关节速度的步骤包括子步骤S121-子步骤S124。

步骤S121，获取所述机械臂末端的笛卡尔参数，所述笛卡尔参数包括末端的每个维度的质量参数、每个维度的阻尼参数以及每个维度的刚度参数。

其中，质量参数越大机械臂受外力作用时的稳定性越高，阻尼矩阵越大机械臂碰撞时的能量耗散越快，刚度参数越大末端位置的精确度越高。因此，机械臂末端的笛卡尔参数时可以根据需要机械臂展现出的性能来确定。

步骤S122，根据所述笛卡尔参数以及末端受到的外力计算末端的速度偏差，所述速度偏差为末端当前速度与受阻任务的期望速度之间的偏差。该受阻任务即第一任务，期望速度是受阻任务正常执行情况下的在当前时刻的速度。

步骤S123，根据末端的速度偏差计算末端的目标速度。目标速度即目标遇到障碍后所执行的新的任务(第二任务)的速度。

步骤S124，根据所述末端的目标速度计算每个关节的第一关节速度。

本实施例用于根据末端受到的外力计算每个关节的第一关节速度。

根据Hogan提出的经典阻抗控制理论，在机械臂的实际位移为x，机械臂执行任务时的期望位移为x_d时，末端的加速度偏差、末端的速度偏差、末端的位移偏差与末端所受外力的关系为：

所述根据所述笛卡尔参数以及末端受到的外力计算末端的速度偏差的计算公式为：

其中，x是末端在每个维度的位移分量构成的矩阵，其第i行元素代末端在第i个维度的位移分量；

是末端在每个维度的速度分量构成的矩阵，其第i行元素代末端在第i个维度的速度分量；

是末端在每个维度的期望速度的分量构成的矩阵，其第i行元素代末端在第i个维度的期望速度的分量；

是末端在每个维度的加速度分量构成的矩阵，其第i行元素代末端在第i个维度的加速度分量；

是末端在每个维度的期望加速度的分量构成的矩阵，其第i行元素代末端在第i个维度的期望加速度的分量。

是末端在每个维度的位移偏差分量构成的矩阵，其第i行元素代末端在第i个维度的位移偏差分量。

是末端在每个维度的速度偏差的分量构成的矩阵，其第i行元素代末端在第i个维度的速度偏差的分量。

是末端在每个维度的加速度偏差的分量构成的矩阵，其第i行元素代末端在第i个维度的加速度偏差的分量。F_ext为末端所受外力在各个运动维度上的分量的矩阵，M_e为末端每个维度的质量参数构成的矩阵，其中，M_e的行数和列数均与末端的运动维度相同，M_e中第i行第m列元素代表第i个维度受到第m个运动维度影响下的质量参数分量，0<m<M；B_e为末端每个维度的阻尼参数构成的矩阵，其中，B_e的行数和列数均与末端的运动维度相同，B_e中第i行第m列元素代表第i个维度受到第m个运动维度影响下的阻尼参数分量，K_e为末端每个维度的刚度参数构成的矩阵，K_e的行数和列数均与末端的运动维度相同，K_e中第i行第m列元素代表第i个维度受到第m个运动维度影响下的刚度参数分量；s为拉普拉斯变换。

在计算出末端的速度偏差之后，便可以根据末端的速度偏差计算执行第二任务时末端的目标速度，具体计算公式为：

接着，便可以根据所述末端的目标速度计算每个关节的第一关节速度。

具体地，可以根据第一矩阵来进行计算，本实施例中，机械臂为冗余机械臂时，第一矩阵为不可逆矩阵，因此，可以采用第一矩阵的伪逆J⁺来进行计算，其中，JJ⁺＝I，I为单位矩阵。第一关节速度

的计算公式为：

请参照图4所示，可选地，本实施例中，所述根据连杆所受到的外力对每个关节产生的连杆力矩，确定各个关节对应末端当前位置的零空间速度的步骤包括子步骤S131-子步骤S135。

步骤S131，计算连杆上所受外力对每个关节产生的第一力矩。

步骤S132，根据每个所述关节的所述第一力矩计算对应的速度偏差，所述速度偏差为该关节的实际速度与期望速度之差。

步骤S133，根据每个关节的权重系数、每个关节的速度偏差计算关节的速度控制权重。

步骤S134，计算机械臂的零空间基向量。

具体地，根据末端受到的外力以及每个关节对末端在每个运动维度上的影响程度构成的第一矩阵计算所述机械臂的零空间基向量，所述零空间基向量的每个元素代表一个关节的速度。

步骤S135，根据所述速度控制权重以及所述零空间基向量计算零空间速度。

本实施例用于将连杆所受外力的力矩对关节的作用转化到零空间中。

可选地，本实施例中，所述计算连杆上所受外力对每个关节产生的第一力矩的计算公式为：

τ_ext＝τ_link+J^TF_ext

τ_link＝τ_ext-J^TF_ext

其中，τ_link为连杆所受外力对每个关节所产生的力矩构成的矩阵，τ_ext为关节上所受到的外力矩的总量，J^T为末端受到的外力以及每个关节对末端在每个运动维度上的影响程度所构成的矩阵的转置，即第一矩阵的转置，F_ext为末端所受外力在各个运动维度上的分量构成的矩阵。

本实施例中，期望速度、期望加速度、期望位移都是末端在第一任务正常执行的情况下，在当前时刻对应的速度、速度、位移。

可选地，本实施例中，q为每个关节的当前角位移构成的Nx1的矩阵，第j个元素代表第j个关节的当前角位移；q_d为每个关节对应第一任务在当前时刻的期望角位移构成的Nx1的矩阵，第j个元素代表第j个关节的期望角位移。所述根据每个所述连杆力矩计算每个所述关节的角速度偏差的计算公式为：

其中，

为关节的角位移偏差构成的矩阵，第j个元素代表第j个关节的角位移偏差；

为关节的角速度偏差构成的矩阵，第j个元素代表第j个关节的角速度偏差；

为关节的角加速度偏差构成的矩阵，第j个元素代表第j个关节的角加速度偏差；M_J为每个关节的质量参数构成的矩阵，其构成元素的行数和列数均与关节的数量相同，M_J中第j行第n列元素代表第j个关节受到第n个关节影响下的质量参数分量；B_J为每个关节的阻尼参数构成的矩阵，其构成元素的行数和列数均与关节的数量相同，B_J中第j行第n列元素代表第j个关节受到第n个关节影响下的阻尼参数分量；K_J为每个关节的刚度参数构成的矩阵，其构成元素的行数和列数均与关节的数量相同，K_J中第j行第n列元素代表第j个关节受到第n个关节影响下的刚度参数分量；s为拉普拉斯变换，0<n<N。

期望角位移都是关节在第一任务正常执行的情况下，在当前时刻的角位移。

由于本实施例中已经获得了第一矩阵，因此，可计算第一矩阵的零空间基向量Z，这些零空间基向量的每个元素代表对应关节在不影响末端位移下的第二关节速度。对于关节的零空间速度而言，其计算方式为：

根据这些基向量的组合，即可在末端所执行任务的零空间中对关节进行控制。具体地，我们可以根据实际需求构造优化的目标函数，用于获取实际控制需求中最优的零空间速度。例如，可以将整个机器人重力势能最小，或者到某个点的距离最小作为优化的目标函数。

以下以控制需求为：所选取的零空间速度控制权重λ使零空间速度

与关节阻抗计算得到的关节角速度偏差

的差值最小来构造优化的目标函数为：

对已经构造好的优化的目标函数求导，然后将

代入求导后的公式，则可以获得以下计算公式：

其中，

为优化的目标函数值，λ为一个Hx1的矩阵，H为零空间基向量的个数，W为正定权重矩阵，其每个对角元素为一个关节的权重。

如此，便可以计算得到λ的表达式为：

进而获得零空间速度为：

得到零空间速度之后，就可以计算总关节速度，总关节速度的计算公式为：

请参照图5，本申请实施例还提供了一种110(冗余机械臂控制装置)，应用于机械臂控制设备，所述机械臂控制设备100与机械臂电性连接，所述装置包括获取模块111、第一计算模块112、第二计算模块113、第三计算模块114、控制模块115。所述机械臂控制装置110包括一个可以软件或固件的形式存储于所述存储器120中或固化在所述机械臂控制设备100的操作系统(operating system，OS)中的软件功能模块。

获取模块111，用于获取所述机械臂的末端所受到的外力以及每个关节所受到的外力力矩。

本实施例中的获取模块111用于执行步骤S110，关于所述获取模块111的具体描述可参照对所述步骤S110的描述。

第一计算模块112，用于根据每个关节对末端在每个运动维度上的影响程度以及所述末端受到的外力，计算每个关节的第一关节速度，所述第一关节速度为该关节对应所述末端所受外力的速度的分量。

本实施例中的第一计算模块112用于执行步骤S120，关于所述第一计算模块112的具体描述可参照对所述步骤S120的描述。

第二计算模块113，用于根据连杆所受到的外力对每个关节产生的连杆力矩，确定各个关节对应末端当前位置的零空间速度，所述零空间速度为在连杆外力作用下维持末端位置不变时各个所述关节对应的速度。

本实施例中的第二计算模块113用于执行步骤S130，关于所述第二计算模块113的具体描述可参照对所述步骤S130的描述。

第三计算模块114，用于根据所述第一关节速度以及所述零空间速度计算总关节速度。

本实施例中的第三计算模块114用于执行步骤S140，关于所述第三计算模块114的具体描述可参照对所述步骤S140的描述。

控制模块115，用于根据所述总关节速度控制所述机械臂运动。

本实施例中的控制模块115用于执行步骤S150，关于所述控制模块115的具体描述可参照对所述步骤S150的描述。

本申请实施例还提供一种可读存储介质，所述可读存储介质存储有可执行程序，处理器130在执行所述可执行程序时，实现如本实施例任一项所述的方法。

以上所述，仅为本申请的各种实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应所述以权利要求的保护范围为准。

Claims (10)

1.一种冗余机械臂控制方法，其特征在于，应用于机械臂控制设备，所述机械臂控制设备与机械臂电性连接，所述方法包括：

获取所述机械臂的末端所受到的外力以及每个关节所受到的外力力矩；

根据每个关节对末端在每个运动维度上的影响程度以及所述末端受到的外力，计算每个关节的第一关节速度，所述第一关节速度为该关节在所述末端所受外力作用下控制机械臂的速度分量；

根据连杆所受到的外力对每个关节产生的连杆力矩，确定各个关节对应末端当前位置的零空间速度，所述零空间速度为在连杆外力作用下维持末端位置不变时各个所述关节对应的速度；

根据所述第一关节速度以及所述零空间速度计算总关节速度；

根据所述总关节速度控制所述机械臂运动。

2.根据权利要求1所述的方法，其特征在于，所述根据所述末端受到的外力以及每个关节对末端在每个运动维度上的影响程度计算每个关节的第一关节速度的步骤包括：

获取所述机械臂末端的笛卡尔参数，所述笛卡尔参数包括末端的每个维度的质量参数、每个维度的阻尼参数以及每个维度的刚度参数；

根据所述笛卡尔参数以及末端受到的外力计算末端的速度偏差，所述速度偏差为末端当前速度与受阻任务的期望速度之间的偏差；

根据末端的速度偏差计算末端的目标速度；

根据所述末端的目标速度计算每个关节的第一关节速度。

3.根据权利要求2所述的方法，其特征在于，末端的加速度偏差

末端的速度偏差

末端的位移偏差

与末端所受外力的关系为：

所述根据所述笛卡尔参数以及末端受到的外力计算末端的速度偏差的计算公式为：

其中，F_ext为末端所受外力在各个运动维度上的分量的矩阵，M_e为末端每个维度的质量参数构成的矩阵，B_e为末端每个维度的阻尼参数构成的矩阵，K_e为末端每个维度的刚度参数构成的矩阵，s为拉普拉斯变换。

4.根据权利要求1所述的方法，其特征在于，所述根据连杆所受到的外力对每个关节产生的连杆力矩，确定各个关节对应末端当前位置的零空间速度的步骤包括：

计算连杆上所受外力对每个关节产生的第一力矩；

根据每个所述关节的所述第一力矩计算对应的速度偏差，所述速度偏差为该关节的实际速度与期望速度之差；

根据每个关节的权重系数、每个关节的速度偏差计算关节的速度控制权重；

根据末端受到的外力以及每个关节对末端在每个运动维度上的影响程度构成的第一矩阵计算所述机械臂的零空间基向量，所述零空间基向量的每个元素代表一个关节的速度；

根据所述速度控制权重以及所述零空间基向量计算零空间速度。

5.根据权利要求4所述的方法，其特征在于，所述计算连杆上所受外力对每个关节产生的第一力矩的计算公式为：

τ_ext＝τ_link+J^TF_ext

τ_link＝τ_ext-J^TF_ext

其中，τ_link为连杆所受外力对每个关节所产生的力矩构成的矩阵，τ_ext为关节上所受到的外力矩的总量，J^T为末端受到的外力以及每个关节对末端在每个运动维度上的影响程度所构成的矩阵的转置，F_ext为末端所受外力在各个运动维度上的分量构成的矩阵。

6.根据权利要求5所述的方法，其特征在于，所述根据每个所述连杆力矩计算每个所述关节的速度偏差的计算公式为：

其中，

为关节的位移偏差构成的矩阵，

为关节的速度偏差构成的矩阵，

为关节的角速度偏差构成的矩阵，M_J为每个关节的质量参数构成的矩阵，B_J为每个关节的阻尼参数构成的矩阵，K_J为每个关节的刚度参数构成的矩阵，s为拉普拉斯变换。

7.根据权利要求6所述的方法，其特征在于，所述机械臂中预先配置有优化的目标函数，目标函数为：

所述根据每个关节的权重系数、每个关节的速度偏差计算每个关节的速度控制权重的计算公式为：

其中，

为零空间速度，

为优化的目标函数值。

8.一种冗余机械臂控制装置，其特征在于，应用于机械臂控制设备，所述机械臂控制设备与机械臂电性连接，所述装置包括：

获取模块，用于获取所述机械臂的末端所受到的外力以及每个关节所受到的外力力矩；

第一计算模块，用于根据每个关节对末端在每个运动维度上的影响程度以及所述末端受到的外力，计算每个关节的第一关节速度，所述第一关节速度为该关节在所述末端所受外力作用下控制机械臂的速度分量；

第二计算模块，用于根据连杆所受到的外力对每个关节产生的连杆力矩，确定各个关节对应末端当前位置的零空间速度，所述零空间速度为在连杆外力作用下维持末端位置不变时各个所述关节对应的速度；

第三计算模块，用于根据所述第一关节速度以及所述零空间速度计算总关节速度；

控制模块，用于根据所述总关节速度控制所述机械臂运动。

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

10.一种冗余机械臂控制设备，其特征在于，包括存储器和处理器，所述存储器和所述处理器电性连接，所述存储器中存储有可执行程序，所述处理器在执行所述可执行程序时，实现如权利要求1-7任一项所述的方法。

Images