手术机器人机械臂的力位混合控制方法与系统

本发明涉及机器人控制，尤其涉及手术机器人机械臂的力位混合控制方法与系统。

背景技术：

1、手术机器人技术中，机械臂需在有限操作空间内完成高精度接触操作任务。力位混合控制方法能够同时调控末端位置与接触力，是实现柔顺接触操作的核心控制策略。

2、现有手术机器人机械臂的力位混合控制方法，通常将力控制子空间与位置控制子空间通过选择矩阵解耦，分别在对应自由度上独立实施力控与位控。其中，力控通道以末端接触力信号为反馈依据调节接触力，位控通道以关节编码器经运动学解算的末端位姿为反馈依据跟踪期望轨迹；该框架在接触目标静止时可实现有效解耦。然而，手术操作中人体呼吸、心跳等生理活动导致组织周期性位移，同时引起末端接触力波动与相对位置偏离。由于力控与位控目标不同，两通道对机械臂的调节需求存在固有矛盾，此现象称为生理运动耦合扰动。

3、生理运动耦合扰动的存在，对手术机器人机械臂力位混合控制的稳定性与精度造成本质性负面影响。现有技术在处理该扰动时，通常将其视为随机噪声，并采用固定带宽低通滤波器加以抑制，从而滤除力控制通道反馈信号中的生理运动分量。然而，滤波处理并未消除生理运动对系统的物理影响：当组织因生理运动向机械臂末端方向位移时，末端与组织的相对位置被动变化，接触力随之发生物理波动。力控制通道因滤波而丧失对该频段内真实接触力变化的感知能力，无法主动调节以维持恒力；位控制通道基于关节编码器反馈，同样无法感知组织的周期性位移，无法主动调整末端位置以跟随组织运动。两个控制通道均缺乏对生理运动的主动感知能力，且现有方法未建立针对生理运动周期性规律的预测机制，导致机械臂末端既不能主动跟随组织位移，也不能主动调节接触力波动，末端位置随组织运动被动偏离期望轨迹，接触力产生持续性波动偏差。此外，滤波处理引入的相位滞后会进一步加剧力控通道对真实接触力变化的响应延迟，使系统在生理运动扰动下的整体控制性能持续下降，难以保障手术操作过程中机械臂末端接触力与位置的双重精度要求。

技术实现思路

1、为了克服现有技术的上述缺陷，本发明的实施例提供手术机器人机械臂的力位混合控制方法与系统，以解决上述背景技术中提出的问题。

2、为实现上述目的，本发明提供如下技术方案：手术机器人机械臂的力位混合控制方法，包括以下步骤：

3、s1、获取机械臂各关节力矩数据和末端力传感器数据的时间序列，对时间序列进行频域分析，提取人体生理运动的周期特征参数；

4、s2、基于所述周期特征参数，建立人体生理运动预测模型，利用生理运动预测模型，实时生成当前时刻的组织位移预测值与接触力波动预测值；

5、s3、基于所述组织位移预测值对位置控制通道的期望末端位姿进行前馈补偿，基于所述接触力波动预测值对力控制通道的期望接触力进行前馈补偿，分别生成补偿后的位置控制指令与力控制指令；

6、s4、基于选择矩阵对补偿后的位置控制指令与力控制指令进行解耦分配，将二者分别映射至对应的控制自由度方向，生成关节驱动指令；

7、s5、基于所述关节驱动指令驱动机械臂关节运动，实时更新各关节力矩数据和末端力传感器数据，利用更新后的数据在线修正周期特征参数，并返回s2循环执行。

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、计算人体生理运动预测模型输出的当前时刻生理运动预测值与末端力传感器实测值之间的差值，将所述差值作为当前时刻的预测误差；

33、基于预测误差，对周期特征参数中各频率成分的频率值、幅值和相位进行在线修正，得到修正后的周期特征参数；

34、将修正后的周期特征参数返回s2，更新人体生理运动预测模型，循环执行s2至s5。

35、为实现上述目的，本发明提供如下技术方案：手术机器人机械臂的力位混合控制系统，实施上述手术机器人机械臂的力位混合控制方法，包括：

36、参数提取模块：用于获取机械臂各关节力矩数据和末端力传感器数据的时间序列，对时间序列进行频域分析，提取人体生理运动的周期特征参数；

37、预测建模模块：基于所述周期特征参数，建立人体生理运动预测模型，利用生理运动预测模型，实时生成当前时刻的组织位移预测值与接触力波动预测值；

38、前馈补偿模块：基于所述组织位移预测值对位置控制通道的期望末端位姿进行前馈补偿，基于所述接触力波动预测值对力控制通道的期望接触力进行前馈补偿，分别生成补偿后的位置控制指令与力控制指令；

39、解耦分配模块：基于选择矩阵对补偿后的位置控制指令与力控制指令进行解耦分配，将二者分别映射至对应的控制自由度方向，生成关节驱动指令；

40、在线修正模块：基于所述关节驱动指令驱动机械臂关节运动，实时更新各关节力矩数据和末端力传感器数据，利用更新后的数据在线修正周期特征参数，并返回预测建模模块循环执行。

41、如上所述，本发明提供的手术机器人机械臂的力位混合控制方法与系统，至少具有以下有益效果：

42、本发明提供的手术机器人机械臂的力位混合控制方法与系统，通过在接触操作过程中同步获取各关节力矩数据和末端力传感器数据并进行频域分析，将人体生理运动从传统力位混合控制框架中不可感知的随机扰动转化为可量化的周期特征参数，进而建立生理运动预测模型实时输出组织位移预测值与接触力波动预测值，为力控制通道与位置控制通道的协同补偿提供统一的数据基础，从根本上改变了现有方法被动响应生理运动扰动的处理方式。

43、在前馈补偿环节，本发明引入组织运动速度预测值的方向判断机制，根据组织运动方向动态切换两通道的补偿逻辑，使位置控制通道与力控制通道在任意时刻均基于同一物理判断协同动作，消除了两通道因独立感知生理运动扰动而产生的指令冲突，保障了手术机器人机械臂在生理运动环境下力位混合控制的稳定性。此外，本发明在闭环控制回路中引入周期特征参数的在线修正机制，利用预测误差持续修正生理运动预测模型的频率值、幅值和相位，使预测模型能够跟踪生理运动特征的缓慢漂移，保证长时程手术操作过程中预测精度的持续有效性，进一步提升了力位混合控制的双通道控制精度。