技术摘要：
一种无需末端工具质量信息和力传感器安装信息的六维力传感器重力补偿方法及系统，包括：(1)将力传感器安装在六自由度机械臂末端，基于力传感器读数和机械臂正运动学，建立：末端工具质量测量方程、质心位置测量方程、力传感器安装信息测量方程；(2)选取机械臂多个构型  全部
背景技术：
机械臂在重力环境下进行柔顺接触操作时，进入控制闭环的力应该为机械臂末端 与环境的接触力。实际上，安装于机械臂腕部的六维力传感器测量到的力，不仅包含机械臂 末端与环境间的接触力，还包含安装于力传感器之后的工具的重力。工具重力不应进入控 制闭环，它属于干扰力。如果不进行重力补偿，系统进入柔顺控制模式后，机械臂会跟随重 力下掉，使得柔顺控制无法正常实施。因此，需要进行实时重力补偿来抵消末端工具自身重 力的影响，使得补偿后机械臂不受外力作用时，在任何末端位姿下，机械臂基座坐标系下各 方向力和力矩都趋于零。 目前常用的机械臂末端工具重力补偿方法均需要对机械臂安装信息、力传感器安 装信息、末端工具质量特性进行精密测量。此重力补偿方法存在以下缺点： (1)机械臂末端工具多种多样，质量特性各异，安装方式多变，每次对工具进行更 换或者重新安装后，都需要对末端工具进行精密测量来测定其质量特性，非常繁琐； (2)需要多种测量信息，如机械臂安装信息、力传感器安装信息、末端工具质量特 性等，测量过程复杂。
技术实现要素：
本发明的目的在于克服现有技术的不足，提供一种无需末端工具质量信息和力传 感器安装信息的六维力传感器重力补偿方法，解决了传统的机械臂末端工具重力补偿方法 依赖多种测量信息的问题。 本发明目的通过如下技术方案予以实现： 一种六维力传感器重力补偿方法，步骤如下： (1)将力传感器安装在六自由度机械臂末端，基于力传感器读数和机械臂正运动 学，建立一组测量方程，包括：末端工具质量测量方程、质心位置测量方程、力传感器安装信 息测量方程； (2)选取机械臂多个构型，得到l组对应的力传感器测量数据，进而得到l组末端工 具质量测量方程、质心位置测量方程、力传感器安装信息测量方程； (3)采用加权平均或最小二乘法，得到末端工具质量、质心位置、力传感器安装信 息； (4)建立包含末端工具质量、质心位置、力传感器安装矩阵的末端工具重力补偿模 型； (5)计算得到末端工具在机械臂不同构型下的重力补偿量，完成对末端工具的实 时重力补偿。 6 CN 111590564 A 说　明　书 2/10 页 进一步的，选定末端工具并以确定姿态固定于力传感器工具平面上后，工具重力G 和工具质心位置 即唯一确定，末端工具的重力为G＝mg，末端工具的质量为m，重 力加速度为g； 末端工具质量测量方程： 其中 ，对应于机械臂处于某构型的 力传感器的三维力分量的 读数为 表示工具重力G沿力传感器坐标系的各坐标轴分力。 3、根据权利要求2所述的一种六维力传感器重力补偿方法，其特征在于：末端工具 质心位置在力传感器坐标系中的表示为 对应于机械臂处 于某构型的力传感器的三维力矩分量的读数为 表示 工具重力G沿力传感器坐标系的各坐标轴分力矩； 根据工具质心位置Ls与力传感器测量得到的分力Fs、分力矩Ms之间的关系，得到工 具质心位置Ls的测量方程 将上式写为矩阵形式，有 进一步的，力传感器安装信息的测量方程： Fsxcosθ-Fsysinθ＝Γ1 其中，θ为力传感器安装角度， 且Γ1为三维矢量Γ的第 一个元素； 为矩阵0Rend的逆矩阵； 传感器坐标系六维力传感器相对于机械臂末端系的安装矩阵如下 7 CN 111590564 A 说　明　书 3/10 页 将六维力传感器测量得到的分力Fs转换到机械臂坐标系0中，有 进一步的，根据l组对应的力传感器测量数据，计算得到l组工具重力估计值，记为 Gi，i＝1,…,l， 其中， 为第i次测量得到的力传感器的三维力 分量的读数； 基于l组力传感器测量数据，得到l组工具质心位置Ls的测量方程 其中， 为第i次测量得到的力传感器的三维 力矩分量的读数； 根据l组对应的力传感器测量数据，得到l组力传感器安装角度θ的测量方程 Fsxicosθ-Fsyisinθ＝Γ1i 其中，Γ1i为三维矢量Γi的第一个元素， endR0i为根据第i次 测量对应的机械臂构型，基于机械臂正运动学计算得到的姿态变换矩阵； 若上式满足条件 则基于该次测量得到力传感器安装角度为 进一步的，采用加权平均法，得到末端工具质量： 利用力传感器进行多次测量，末端工具质量m由下式给出 8 CN 111590564 A 说　明　书 4/10 页 使不同末端位姿下力传感器测量次数l满足条件：l>>2，然后采用最小二乘法求解 末端工具质心位置Ls，有 其中， 为矩阵 的经典伪逆； 采用加权平均法，得到力传感器安装角度 进一步的，建立包含末端工具质量、质心位置、力传感器安装矩阵的末端工具重力 补偿模型，具体为： 将力传感器测量得到的力和力矩分量分别转换到机械臂坐标系0下，有 F0＝0RendFs M ＝00 RendMs 其中， 与 分别为力传感器的力和力矩分量在机械臂坐标系0中的 表示； 将末端工具质心位置矢量表示在机械臂坐标系0中，有 L 00＝ RendLs 其中， 为工具质心位置矢量在机械臂坐标系0中的表示； 在机械臂坐标系0中，重力对力传感器的作用力 为 在机械臂坐标系0中，重力对力传感器的作用力矩 为 从机械臂坐标系0下力传感器的测量值中，减去重力和重力矩的补偿量，即可得到 补偿后的六维力传感器测量值 由下式给出 进一步的，计算得到末端工具在机械臂不同构型下的重力补偿量，完成对末端工 具的实时重力补偿，具体为： 采用一阶低通滤波器对力传感器测量数据进行滤波，基于步骤(4)中得到的末端 工具重力补偿模型和经过滤波后的力传感器测量数据，进行末端工具的实时重力补偿。 进一步的，一阶低通数字滤波器由下式给出 9 CN 111590564 A 说　明　书 5/10 页 Yn＝qXn (1-q)Yn-1 其中，q为一阶低通数字滤波器的时间常数，Xn为第n次采样时的滤波器输入，Yn为 第n次采样时的滤波器输出，Yn-1为第n-1次采样时的滤波器输出。 进一步的，本发明还提出一种重力补偿系统，包括： 测量方程建立模块：将力传感器安装在六自由度机械臂末端，基于力传感器读数 和机械臂正运动学，建立一组测量方程，包括：末端工具质量测量方程、质心位置测量方程、 力传感器安装信息测量方程； 选取机械臂多个构型，得到l组对应的力传感器测量数据，进而得到l组末端工具 质量测量方程、质心位置测量方程、力传感器安装信息测量方程； 数据处理模块：采用加权平均或最小二乘法，得到末端工具质量、质心位置、力传 感器安装信息； 补偿模型建立模块：建立包含末端工具质量、质心位置、力传感器安装矩阵的末端 工具重力补偿模型； 重力补偿模块：计算得到末端工具在机械臂不同构型下的重力补偿量，完成对末 端工具的实时重力补偿。 本发明与现有技术相比具有如下优点： (1)本发明解决了传统的机械臂末端工具重力补偿方法依赖多种测量信息的问 题。本发明重力补偿方法需要的测量信息少，在无需末端工具质量/质心位置、力传感器安 装信息的情况下，仅需力传感器测量信息，即可实现对末端工具的重力补偿； (2)本发明重力补偿方法基于力传感器测量信息和机械臂正运动学对其力传感器 安装信息进行测量比较经济、简单； (3)本发明重力补偿方法适用于各种带有六维力传感器的机械臂力控制场景中。 附图说明 图1为本发明的方法流程示意图。