一种六维力传感器重力补偿方法及系统技术方案

一种无需末端工具质量信息和力传感器安装信息的六维力传感器重力补偿方法及系统，包括：(1)将力传感器安装在六自由度机械臂末端，基于力传感器读数和机械臂正运动学，建立：末端工具质量测量方程、质心位置测量方程、力传感器安装信息测量方程；(2)选取机械臂多个构型，得到l组对应的力传感器测量数据，进而得到l组末端工具质量测量方程、质心位置测量方程、力传感器安装信息测量方程；(3)采用加权平均或最小二乘法，得到末端工具质量、质心位置、力传感器安装信息；(4)建立包含末端工具质量、质心位置、力传感器安装矩阵的末端工具重力补偿模型；(5)计算得到末端工具在机械臂不同构型下的重力补偿量，完成对末端工具的实时重力补偿。

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

【技术保护点】
1.一种六维力传感器重力补偿方法，其特征在于步骤如下：/n(1)将力传感器安装在六自由度机械臂末端，基于力传感器读数和机械臂正运动学，建立一组测量方程，包括：末端工具质量测量方程、质心位置测量方程、力传感器安装信息测量方程；/n(2)选取机械臂多个构型，得到l组对应的力传感器测量数据，进而得到l组末端工具质量测量方程、质心位置测量方程、力传感器安装信息测量方程；/n(3)采用加权平均或最小二乘法，得到末端工具质量、质心位置、力传感器安装信息；/n(4)建立包含末端工具质量、质心位置、力传感器安装矩阵的末端工具重力补偿模型；/n(5)计算得到末端工具在机械臂不同构型下的重力补偿量，完成对末端工具的实时重力补偿。/n

【技术特征摘要】
1.一种六维力传感器重力补偿方法，其特征在于步骤如下：
(1)将力传感器安装在六自由度机械臂末端，基于力传感器读数和机械臂正运动学，建立一组测量方程，包括：末端工具质量测量方程、质心位置测量方程、力传感器安装信息测量方程；
(2)选取机械臂多个构型，得到l组对应的力传感器测量数据，进而得到l组末端工具质量测量方程、质心位置测量方程、力传感器安装信息测量方程；
(3)采用加权平均或最小二乘法，得到末端工具质量、质心位置、力传感器安装信息；
(4)建立包含末端工具质量、质心位置、力传感器安装矩阵的末端工具重力补偿模型；
(5)计算得到末端工具在机械臂不同构型下的重力补偿量，完成对末端工具的实时重力补偿。


2.根据权利要求1所述的一种六维力传感器重力补偿方法，其特征在于：选定末端工具并以确定姿态固定于力传感器工具平面上后，工具重力G和工具质心位置即唯一确定，末端工具的重力为G＝mg，末端工具的质量为m，重力加速度为g；
末端工具质量测量方程：



其中，对应于机械臂处于某构型的力传感器的三维力分量的读数为表示工具重力G沿力传感器坐标系的各坐标轴分力。


3.根据权利要求2所述的一种六维力传感器重力补偿方法，其特征在于：末端工具质心位置在力传感器坐标系中的表示为对应于机械臂处于某构型的力传感器的三维力矩分量的读数为表示工具重力G沿力传感器坐标系的各坐标轴分力矩；
根据工具质心位置Ls与力传感器测量得到的分力Fs、分力矩Ms之间的关系，得到工具质心位置Ls的测量方程



将上式写为矩阵形式，有





4.根据权利要求3所述的一种六维力传感器重力补偿方法，其特征在于：力传感器安装信息的测量方程：
Fsxcosθ-Fsysinθ＝Γ1
其中，θ为力传感器安装角度，且Γ1为三维矢量Γ的第一个元素；为矩阵0Rend的逆矩阵；
传感器坐标系六维力传感器相对于机械臂末端系的安装矩阵如下



将六维力传感器测量得到的分力Fs转换到机械臂坐标系0中，有





5.根据权利要求4所述的一种六维力传感器重力补偿方法，其特征在于：根据l组对应的力传感器测量数据，计算得到l组工具重力估计值，记为Gi，i＝1,…,l，



其中，为第i次测量得到的力传感器的三维力分量的读数；
基于l组力传感器测量数据，得到l组工具质心位置Ls的测量方程



其中，为第i次测量得到的力传感器的三维力矩分量的读数；
根据l组对应的力传感器测量数据，得到l组力传感器安装角度θ的测量方程



其中，Γ1i为三维矢量Γi的第一个元素，endR0i为根据第i次测量对应的机械臂构型，基于机械臂正运动学计算得到的姿态变换矩阵；
若上式满足条件则基于该次测量得到力传感器安装角度为


<...

【专利技术属性】
技术研发人员：徐拴锋，魏春岭，张海博，胡海东，王勇，胡勇，
申请(专利权)人：北京控制工程研究所，
类型：发明
国别省市：北京;11