[发明专利]一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法

说明书

一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法，属于诊疗机器人力控领域，为了解决现有的重力补偿方法对诊疗机器人重力补偿精度不足的问题。本发明实时记录机器人多个姿态下的姿态矩阵以及传感器的测量值，构造线性方程组，通过最小二乘法求解诊疗工具的参数向量；在诊疗工具处于初始位置时以及按照大地坐标系调整诊疗工具的姿态，使诊疗工具的重力只剩沿六维力与力矩传感器坐标的y轴方向时，分别记录传感器的测量值；在传感器坐标系下，利用降维解析法计算诊疗工具重心的位置；计算出诊疗工具的重力分量与力矩分量补偿值，实现对诊疗工具的重力补偿。有益效果为实现对多种诊疗工具在多姿态工作模式下的高精度重力补偿。

技术领域

本发明属于诊疗机器人力控领域。

背景技术

诊疗机器人在重力环境下结合六维力与力矩传感器通过人手接触控制柔顺运动时，进入控制闭环的力为诊疗机器人末端与人手之间的接触力，换言之，要实现人手力对机器人的控制，首先就要精确检测出人手力的大小和方向；实际上，安装于诊疗机器人末端的六维力传感器测量到的力，不仅包含机器人末端与人手间的接触力，还包含安装于力传感器上诊疗工具的重力；但是传感器无法自主区分人手力和诊疗工具的重力，诊疗工具的重力属于干扰力，因此有必要进行精确的重力补偿；如果不进行重力补偿，系统的控制精度必定大大受其影响；其次，根据诊疗机器人的实际需求，机器人末端需要在超声阵列探头、诊疗光纤和治疗光纤这三种诊疗工具中来回切换，而这三种诊疗工具的质量特性各异，重心也随自身姿态的变化而变化；现有的重力补偿方法无法满足诊疗机器人多模式、多姿态和高精度的工作需求，每次对末端工具进行更换或者重新安装后，都需要对其进行精密测量来测定其质量特性，操作繁琐，不适用于诊疗机器人的工作环境。

发明内容

本发明的目的是为了解决现有的重力补偿方法对诊疗机器人重力补偿精度不足的问题，提出了一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法。

本发明所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法通过以下步骤实现的：

步骤一、将六维力与力矩传感器安装在多关节诊疗机器人末端，选取多关节诊疗机器人的多个姿态，并实时记录关节诊疗机器人多个姿态下对应的姿态矩阵以及记录关节诊疗机器人多个姿态下六维力与力矩传感器的测量值，构造线性方程组，通过最小二乘法求解诊疗工具的参数向量；

步骤二、在诊疗工具处于初始位置时，记录六维力与力矩传感器的测量值；

步骤三、按照大地坐标系调整诊疗工具的姿态，使诊疗工具的重力只剩沿六维力与力矩传感器坐标的y轴方向，并记录此时六维力与力矩传感器的测量值；

步骤四、根据步骤二记录的测量值与步骤三记录的测量值，在传感器坐标系下，利用降维解析法计算诊疗工具重心的位置；

步骤五、将步骤四计算出的诊疗工具重心的位置投影到六维力与力矩传感器坐标系上，并结合步骤一求解出的诊疗工具参数向量，计算出诊疗工具的重力分量与力矩分量补偿值，实现对诊疗工具的重力补偿。

本发明的有益效果是：该重力补偿方法能够实时提供重力补偿，来抵消多关节诊疗机器人末端诊疗工具自身重力的影响，使得补偿后的多关节诊疗机器人在任何末端位姿下不受外力作用时，多关节诊疗机器人在基坐标系下沿各方向的力和力矩都趋于零，因此该重力补偿方法重力补偿算法精度较高。

附图说明

图1为为具体实施方式所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法流程图；

图2为具体实施方式一中多关节诊疗机器人的实物图；

图3为具体实施方式二中基于坐标系变换的重力计算示意图；

图4为具体实施方式七中对计算出的诊疗工具重力分量与力矩分量补偿值进行验证的流程图。

具体实施方式