[发明专利]一种主控制臂运动控制方法

权利要求书

1.一种主控制臂运动控制方法，其特征在于，包括：

根据主控制臂各关节的当前速度和当前位置得到各关节的第一期望速度；

根据成本函数以及所述主控制臂各关节的当前位置得到各关节的第二期望速度，所述成本函数为所述主控制臂的一个或多个关节的姿态变化量的函数；

根据所述第一期望速度和第二期望速度计算得到各关节的第三期望速度，根据各关节的第三期望速度得到各关节的期望位置；

根据各关节的第三期望速度和期望位置控制所述主控制臂的各关节进行运动。

2.根据权利要求1所述的主控制臂运动控制方法，其特征在于，根据各关节的第三期望速度和期望位置控制所述主控制臂的各关节进行运动，包括：

通过逆动力学方法计算各关节的重力前馈；

根据各关节的第三期望速度、期望位置、当前速度、当前位置以及重力前馈计算目标力矩；

根据所述目标力矩控制各关节进行运动。

3.根据权利要求1所述的主控制臂运动控制方法，其特征在于，所述成本函数的计算方式为：

其中，i为所述各关节的关节序号，C_i(q)表示关节i的成本函数，q_i(t)表示关节i的当前位置，q_imax表示所述关节i的关节最大限位，q_imin表示所述关节i的关节最小限位。

4.根据权利要求1所述的主控制臂运动控制方法，其特征在于，根据成本函数以及所述主控制臂各关节的当前位置得到各关节的第二期望速度，包括：

获取所述主控制臂各关节的当前位置，根据所述当前位置计算雅克比矩阵；

根据所述雅克比矩阵及其伪逆矩阵，计算零空间映射矩阵；

构建成本函数，计算得到所述成本函数的负梯度，根据所述负梯度和零空间映射矩阵得到各关节的第二期望速度，其中，所述成本函数为所述主控制臂的一个或多个关节的姿态变化量的函数。

5.根据权利要求4所述的主控制臂运动控制方法，其特征在于，构建成本函数，计算得到所述成本函数的负梯度，根据所述负梯度和零空间映射矩阵得到各关节的第二期望速度，包括：

构建成本函数，计算得到所述成本函数的负梯度，根据所述负梯度、零空间映射矩阵以及步长得到各关节的第二期望速度。

6.根据权利要求5所述的主控制臂运动控制方法，其特征在于，所述步长根据操作者的操作速度实时计算得到，计算公式包括：其中，x1为所述步长，w为所述主控制臂对应的末端角速度，w₀为预设角速度，x₀为所述主控制臂对应的末端角速度为w₀时的步长。

7.根据权利要求1所述的主控制臂运动控制方法，其特征在于，根据所述第一期望速度和第二期望速度计算得到各关节的第三期望速度，包括：

对第一期望速度和第二期望速度进行加权计算得到各关节的第三期望速度，计算公式包括：

dq_d＝k1*dq1+k2*dq2

其中，dq_d为所述第三期望速度，k1为所述第一期望速度的权重系数，k2为所述第二期望速度的权重系数。

8.根据权利要求7所述的主控制臂运动控制方法，其特征在于，k1、k2均为常数。

9.根据权利要求7所述的主控制臂运动控制方法，其特征在于，k2为常数，k1根据操作者的操作速度实时计算得到，计算公式包括：k1＝k₀*(w/w₀)，其中，w为所述主控制臂对应的末端角速度，w₀为预设角速度，k₀为k1在所述主控制臂对应的末端角速度为w₀时的取值。

10.根据权利要求1所述的主控制臂运动控制方法，其特征在于，根据主控制臂各关节的当前速度和当前位置得到各关节的第一期望速度，包括：

获取所述主控制臂各关节的当前位置和当前速度，根据所述当前位置计算雅克比矩阵及其伪逆矩阵；

根据各关节的当前速度和雅克比矩阵，计算各关节对应的末端角速度；

根据各关节的末端角速度和雅克比伪逆矩阵，计算得到各关节的第一期望速度。