[发明专利]一种运动追踪方法、装置、机械臂及计算机可读存储介质

权利要求书

1.一种运动追踪方法，应用于机械臂，其特征在于，所述机械臂的各个关节设置有致动器；所述致动器设置有编码器和力矩传感器；所述编码器用于输出与所述致动器对应的关节的关节角度，所述力矩传感器用于输出与所述致动器对应的关节的关节力矩；所述运动追踪方法包括：

获取机械臂末端与追踪对象之间的目标相对位姿数据，以及所述机械臂末端与追踪对象之间的实时相对位姿数据；

通过比较所述目标相对位姿数据与所述实时相对位姿数据之间的差距确定机械臂末端在所述追踪对象的位姿发生变化后需要到达的目标位置；

获取各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩，并根据所述各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩计算机械臂各个关节的参考关节角度；

根据所述机械臂各个关节的参考关节角度以及所述机械臂的DH表，计算得到所述机械臂末端的参考位置；

根据所述机械臂末端的目标位置、所述机械臂末端的参考位置以及所述机械臂各个关节的参考关节角度计算得到机械臂各个关节的目标力矩增益；

获取所述机械臂各个关节的重力补偿值，并分别将所述重力补偿值与关节对应的目标力矩增益的和作为关节对应的致动器的力矩输入，以使所述机械臂能够在各个关节致动器的驱动下跟随追踪对象的移动而移动。

2.如权利要求1所述的运动追踪方法，其特征在于，所述根据所述各个致动器的编码器输出的关节角度和力矩传感器输出的关节力矩计算机械臂各个关节的参考关节角度，包括：计算各个关节的关节力矩除以关节对应的力矩传感器的刚度系数得到的商，并将所述关节力矩除以关节对应的力矩传感器的刚度系数得到的商与关节对应的编码器输出的关节角度相加得到关节对应的参考关节角度。

3.如权利要求1所述的运动追踪方法，其特征在于，所述根据所述各个关节的参考关节角度以及所述机械臂的DH表，计算得到所述机械臂末端的参考位置，包括：将各个关节的参考关节角度代入根据DH表得到的各个关节的转换矩阵，并将各个转换矩阵的相乘得到机械臂末端的参考位置。

4.如权利要求1所述的运动追踪方法，其特征在于，所述根据所述机械臂末端的目标位置、所述机械臂末端的参考位置以及所述机械臂各个关节的参考关节角度计算得到机械臂各个关节的目标力矩增益，包括：

获取所述机械臂的比例系数、所述机械臂的雅可比矩阵以及所述机械臂各个关节的阻尼系数；

计算所述机械臂末端的目标位置与所述机械臂末端的参考位置的差值，并计算所述差值与所述机械臂的比例系数相乘得到的第一乘积；

对所述机械臂各个关节的参考关节角度进行微分得到各个关节的关节速度，并计算所述各个关节的关节速度与所述机械臂的雅可比矩阵相乘得到的各个关节的参考末端速度与对应的关节的阻尼系数相乘得到的第二乘积；

将所述第一乘积减去所述第二乘积之后乘以所述机械臂的雅可比矩阵的转置得到所述机械臂各个关节的目标力矩增益。

5.如权利要求1所述的运动追踪方法，其特征在于，所述获取所述机械臂各个关节的重力补偿值包括：

获取所述机械臂的基座、各个连杆以及各个关节的质量和质心分布；

根据所述机械臂的基座、各个连杆以及各个关节的质量和质心分布计算所述机械臂各个基座、各个连杆以及各个关节的重力势能；

计算所述机械臂各个基座、各个连杆以及各个关节的重力势能与对应的关节的参考关节角度的偏微分乘以对应的参考关节角度得到的所述机械臂各个关节的重力补偿值。