[发明专利]冗余机械臂控制方法、装置、可读存储介质及设备

权利要求书

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为拉普拉斯变换。