一种机械臂的力反馈方法及计算设备与流程

本技术涉及遥操机械臂，具体涉及一种机械臂的力反馈方法及计算设备。

背景技术：

1、在具身智能的二次开发过程中，需要采集大量真机数据来训练具身模型，数据采集经常采用主从臂遥操作方式实现。传统的遥操作系统大多仅将主臂的位移或关节角度等比例转换为从臂的运动指令，不具备将从臂末端与环境交互产生的力回传给操作者的能力。这种力觉缺失使得操作者无法感知从臂与外界的接触情况，也难以精确控制末端执行器的施力大小，在精密装配或狭小空间作业时容易引发碰撞事故或导致任务失败。

2、针对力反馈问题，相关技术中，一类方案基于同构映射，要求主臂和从臂关节构型完全一致，仅适用于相同品牌、相同构型的机械臂，无法适配异构系统，且末端工具更换后需重新校准映射参数。另一类方案基于关节扭矩与末端力双转换，需要分别为主臂和从臂各构建一套完整的动力学模型，建模工作量大、参数标定复杂，实时性难以保证；末端工装更换导致从臂惯量和重心改变时，还需对从臂动力学模型重新构建。

3、综上所述，相关技术或受限于主从机械臂同构要求，或需要构建主臂和从臂的完整动力学模型与运动学模型，构建流程繁复，且末端工装更换后需对应重建模型，更换末端工装的过程复杂。

技术实现思路

1、本技术实施例提供一种机械臂的力反馈方法及计算设备，能够减少力反馈过程的复杂度，提高遥操作力反馈的实时性，还能够降低主从臂构型不同场景下的建模适配难度。

2、为达到上述目的，本技术采用如下技术方案：

3、第一方面，提供一种机械臂的力反馈方法，应用于控制机械臂的控制装置，机械臂包括主臂、从臂和末端工装，从臂与主臂通信连接，从臂的工作端与末端工装连接，末端工装用于与环境或目标对象进行交互，从臂设置有检测组件，检测组件被配置为检测环境反馈给末端工装的力反馈参数；力反馈方法包括：获取检测组件检测到的力反馈参数、从臂的当前位姿状态和标定参数；基于力反馈参数、当前位姿状态和标定参数，确定末端工装与环境的交互力和交互力矩；根据主臂的主臂动力学模型，基于主臂的输入信号、交互力和交互力矩，确定主臂的目标输出力矩；目标输出力矩用于控制主臂输出与力反馈参数相应的力反馈；按照目标输出力矩控制主臂工作，以通过主臂进行力反馈。

4、基于本方案，通过检测组件检测力反馈参数，再对力反馈参数进行修正得到末端工装与环境真实的交互力和交互力矩，再根据主臂动力学模型、主臂的输入信号以及交互力和交互力矩确定主臂的目标输出力矩，使操作者能够通过主臂感知末端工装与环境之间的接触作用。这样，无需构建从臂的动力学模型，在主臂自身结构基本不变的条件下，更换从臂的末端工装也不必重建其动力学模型，降低对从臂动力学建模的依赖和计算复杂度，有利于提高遥操作力反馈的实时性。进一步地，本方案不限制主从机械臂的构型，既支持同构机械臂也兼容异构机械臂，提高了应用结构的灵活程度与适应性。不限制主从机械臂的构型，既支持同构机械臂也兼容异构机械臂。此外，检测组件直接测量末端力而非通过关节力矩反推，避免了齿轮摩擦、电机电流估算噪声等关节传动链误差对力感知精度的影响。

5、在一种可能的实现方式中，力反馈参数包括原始力分量和原始力矩分量，标定参数包括检测组件的零点力分量和零点力矩分量，基于力反馈参数、当前位姿状态和标定参数，确定末端工装与环境的交互力和交互力矩，包括：根据从臂的当前位姿状态与标定参数，分别计算当前位姿下末端工装的重力分量和重力矩分量；将力反馈参数中的原始力分量减去标定参数中的零点力分量与重力分量，得到交互力；将力反馈参数中的原始力矩分量减去零点力矩分量与重力矩分量，得到交互力矩。

6、基于本方案，从原始力或力矩中依次剔除检测组件零偏和工装自重的影响，得到的交互力和交互力矩反映环境对工装的实际作用力，整个过程无需依赖从臂动力学模型，计算开销低、实时性好。并且，交互力直接取自检测组件原始输出经补偿得到，而非通过关节力矩反推，从根本上消除了齿轮摩擦和电机电流估算噪声等传动链误差对力感知精度的影响。

7、在一种可能的实现方式中，该方法还包括：在更换从臂的工作端安装的末端工装的情况下，对机械臂进行标定。

8、基于本方案，在更换末端工装的情况下，只需重新进行标定，无需重新构建从臂的动力学模型，减少了换工装所需的时间。

9、在一种可能的实现方式中，在更换从臂的工作端安装的末端工装的情况下，还包括：响应于末端工装更换完成，控制从臂运动至至少三组线性无关姿态，获取每个线性无关姿态下检测组件输出的检测数据，以及从臂的工作端的姿态矩阵；其中，检测数据用于表征末端工装在对应线性无关姿态下作用于检测组件的力学载荷；基于各线性无关姿态下的检测数据和姿态矩阵，确定标定参数；其中，标定参数包括检测组件的零点力分量、零点力矩分量、所述末端工装的质量以及末端工装的质心。

10、基于本方案，末端工装更换后使从臂运动若干组不同姿态、采集检测组件数据并求解，即可获得新的标定参数，无需重建任何动力学模型或调整控制算法参数，实现了末端工装的即插即用，同一套系统可兼容多种末端工具，提高了机械臂的灵活性与作业适应性。

11、在一种可能的实现方式中，主臂包括多个关节和多个连杆，连杆用于连接关节，根据主臂的主臂动力学模型，基于主臂的输入信号、交互力和交互力矩，确定主臂的目标输出力矩，包括：根据主臂动力学模型，基于主臂的输入信号，计算位姿跟踪力矩；位姿跟踪力矩用于使主臂跟踪目标位姿运动；根据主臂动力学模型，基于主臂的当前关节角度和连杆的参数，计算重力补偿力矩；重力补偿力矩用于抵消主臂自身重力对力反馈的干扰；根据主臂动力学模型，基于交互力、交互力矩，计算力反馈映射力矩；力反馈映射力矩用于将环境作用力反馈至主臂；将位姿跟踪力矩、重力补偿力矩、力反馈映射力矩进行代数叠加，得到主臂各关节的目标输出力矩。

12、基于本方案，主臂总输出力矩被分解为位姿跟踪力矩、重力补偿力矩和力反馈映射力矩三个独立计算的组成部分，三者分别用于响应操作者的运动意图、抵消主臂自重和传递从臂端环境力，各部分可独立设计和调试，架构清晰、便于工程实现。

13、在一种可能的实现方式中，根据主臂动力学模型，基于主臂的输入信号，计算位姿跟踪力矩，包括：根据主臂的输入信号，确定主臂末端的目标位姿；计算主臂末端的当前位姿与目标位姿之间的位姿误差和速度误差；基于主臂动力学模型、位姿误差和速度误差，计算得到位姿跟踪力矩。

14、基于本方案，位姿跟踪力矩用于响应操作者对主臂的运动输入，使主臂末端能够跟踪目标位姿，从而保证操作者输入动作与主臂反馈动作之间的一致性。

15、在一种可能的实现方式中，主臂包括多个连杆，连杆的参数包括各连杆的连杆质量和主臂的质心位置，根据主臂动力学模型，基于主臂的当前关节角度和主臂的连杆参数，计算重力补偿力矩，包括：根据主臂的当前关节角度、各连杆质量、质心位置和重力加速度，结合主臂动力学模型，确定各连杆的重力载荷并映射至关节空间，得到重力补偿力矩。

16、基于本方案，重力补偿力矩用于抵消主臂自身重力对操作者力感知的影响，使反馈至操作者的力主要反映末端工装与环境之间的交互。

17、在一种可能的实现方式中，根据主臂动力学模型，基于交互力、交互力矩，计算力反馈映射力矩，包括：将交互力和交互力矩，通过主臂动力学模型中的雅可比矩阵转置映射至主臂关节空间，得到力反馈映射力矩；力反馈映射力矩用于将环境作用力反馈至主臂。

18、基于本方案，力反馈映射力矩用于将末端工装与环境之间的交互力和交互力矩映射至主臂关节空间，使操作者能够通过主臂感知从臂端的环境作用力。

19、在一种可能的实现方式中，响应于主臂的输入端输入信号，获取主臂末端的第一初始位姿、从臂的工作端的第二初始位姿，以及主臂末端的第一移动量；主臂的输入端为操作者施加运动输入的部位；第一移动量为主臂响应于输入端输入信号时，主臂末端相对第一初始位姿的偏移量；根据主臂的主臂运动学模型，基于第一初始位姿、第二初始位姿与第一移动量，确定从臂的工作端的目标位姿；根据从臂的从臂运动学模型对目标位姿进行逆运动学求解，确定从臂的目标关节位置参数；按照目标关节位置参数控制从臂工作，从臂工作用于使末端工装与环境进行交互，以使检测组件检测力反馈参数。

20、基于本方案，主臂末端的偏移量经坐标变换转换到从臂基坐标系下，再叠加至从臂初始位姿，整个位姿映射在笛卡尔空间内完成，操作者无需手动规划从臂轨迹即可通过主臂直观控制从臂的工作端运动；从臂工作端带动末端工装与环境交互后，检测组件即可检测力反馈参数，从而为主臂力反馈提供基础。

21、在一种可能的实现方式中，机械臂的力反馈方法还包括：获取从臂的当前关节位置和当前关节速度；基于当前关节位置，结合预先构建的从臂的从臂运动学模型，计算确定从臂雅可比矩阵的条件数；在条件数大于条件数阈值的情况下，在当前关节速度的基础上沿条件数梯度方向施加速度修正分量，得到目标关节速度；条件数梯度方向是以从臂当前关节位置为起点，条件数随关节角度变化率最大的方向；控制从臂按照目标关节速度运动，以避开奇异位形。

22、基于本方案，通过实时监控从臂雅可比矩阵的条件数，在条件数超出安全阈值时主动对关节速度施加修正，使从臂自主规避奇异位形，无需操作者干预，提升了系统的运动安全性和连续作业能力。

23、在一种可能的实现方式中，基于当前关节位置，结合预先构建的从臂运动学模型，计算确定从臂雅可比矩阵的条件数，包括：基于当前关节位置，结合从臂运动学模型，计算确定从臂的当前位姿；基于从臂的当前位姿，计算确定对应的从臂雅可比矩阵；对雅可比矩阵做奇异值分解，得到最大奇异值与最小奇异值；计算最大奇异值与最小奇异值之间的比值，得到条件数。

24、基于本方案，通过从臂正运动学计算雅可比矩阵并经奇异值分解获取奇异值并求比值，得到条件数，可连续、定量地反映从臂接近奇异位形的程度，为是否触发规避提供判断依据；基于条件数梯度和避奇异增益系数计算速度修正量并叠加至当前关节速度，使得修正幅度随条件数超限程度自适应匹配，实现平滑、连续的自主规避。

25、在一种可能的实现方式中，在条件数大于条件数阈值的情况下，沿条件数梯度方向施加速度修正分量，得到目标关节速度，包括：在条件数大于条件数阈值的情况下，基于条件数，计算得到条件数梯度方向；根据条件数梯度方向和预设的调整系数，计算得到速度修正分量；利用速度修正分量对当前关节速度进行修正，得到目标关节速度。

26、基于本方案，基于条件数梯度方向和预设的调整系数计算速度修正分量，并叠加至当前关节速度得到目标关节速度，使得修正幅度随条件数超限程度自适应匹配——条件数偏高时修正量相应增大、接近奇异极限时自动减小，实现平滑、连续的自主规避，无需操作者干预。

27、第二方面，提供一种机械臂的力反馈系统，包括：机械臂和控制装置；机械臂包括主臂、从臂和末端工装，从臂与主臂通信连接，从臂的工作端与末端工装连接，末端工装用于与环境进行交互，从臂设置有检测组件，检测组件被配置为检测环境反馈给末端工装的力反馈参数；主臂、从臂和检测组件均与控制装置通信连接，控制装置用于执行第一方面任一项所述的机械臂的力反馈方法。

28、第三方面，提供一种机械臂的力反馈装置。该装置包括用于执行第一方面或第一方面任一种可能实现方式中所述方法的各个模块。

29、第四方面，提供一种计算设备。该计算设备包括存储器和处理器，存储器和处理器耦合，存储器用于存储计算机程序代码，计算机程序代码包括计算机指令，当处理器执行计算机指令时，使计算设备执行第一方面或第一方面任一种可能实现方式中所述的机械臂的力反馈方法。

30、第五方面，提供一种计算机可读存储介质。该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现第一方面或第一方面任一种可能实现方式中所述的机械臂的力反馈方法。

31、第六方面，提供一种计算机程序产品。该计算机程序产品包含计算机指令，该计算机指令被处理器执行时实现第一方面或第一方面任一种可能实现方式中所述的机械臂的力反馈方法。

32、可以理解的是，上述第二方面至第六方面的有益效果可以参见上述第一方面中的相关描述，在此不再赘述。