基于力传感器坐标系预标定的重力补偿算法

doi:10.12677/airr.2024.134088

期刊菜单

基于力传感器坐标系预标定的重力补偿算法
Gravity Compensation Algorithm Based on Initial Force Sensor Frame Calibration

DOI: 10.12677/airr.2024.134088, PDF, HTML, XML, 国家自然科学基金支持
作者: 孟偲^*, 李定哲：北京航空航天大学宇航学院，北京；陈殿生：北京航空航天大学机械工程与自动化学院，北京；唐洲平：华中科技大学同济医学院附属同济医院，湖北武汉；刘文杰：北京万特福医疗器械有限公司，北京
关键词: 重力补偿；力传感器；机器人；坐标系标定；Gravity Compensation； Force Sensors； Robot； Coordinate System Calibration

摘要: 重力补偿算法需要实时估计力传感器的初始值及工具重力对力传感器所施加的力和力矩，进而获得外界所施加的力和力矩。当估计工具重力对力传感器所施加的力和力矩时，需要获得力传感器坐标系和机器人末端坐标系的旋转变换矩阵。当前的重力补偿算法采用手动校准的方式，通过不断的拆卸力传感器去保证力传感器坐标系和机器人末端坐标系平行。该手动校准的方式操作复杂耗时较长，本文提出一种带有标定力传感器坐标系相对于机器人末端坐标系的重力补偿算法，从算法层面标定力传感器坐标系和机器人末端坐标系的旋转变换矩阵，实现重力补偿算法的自动化。通过实验验证该方法不需要手动校准，即可实现[0.237/N 0.395/N 0.928/N 0.032/N·m 0.018/N·m 0.002/N·m]的重力补偿误差。

Abstract: The gravity compensation algorithm requires real-time estimation of the initial values of the force sensor and the forces and torques applied by the tool’s gravity on the force sensor. This allows us to obtain the external forces and torques. When estimating the forces and torques applied by the tool’s gravity on the force sensor, the rotation transformation matrix between the force sensor frame and the robot’s end-effector frame is needed. The current gravity compensation algorithm uses manual calibration, involving repeated disassembly of the force sensor to ensure that the frame of the force sensor and the robot’s end-effector are parallel. This manual process is complex and time-consuming. This paper proposes a gravity compensation algorithm that calibrates the rotation transformation matrix between the force sensor and the robot’s end-effector frame through the algorithm, automating the gravity compensation process. Experimental results show that this method achieves a gravity compensation error of [0.237/N 0.395/N 0.928/N 0.032/N·m 0.018/N·m 0.002/N·m] without manual calibration.

文章引用：孟偲, 李定哲, 陈殿生, 唐洲平, 刘文杰. 基于力传感器坐标系预标定的重力补偿算法[J]. 人工智能与机器人研究, 2024, 13(4): 861-871. https://doi.org/10.12677/airr.2024.134088

1. 引言

在机器人进行工业装配[1]、工业加工[2]及医疗应用[3]开发时，力传感器扮演着重要的角色。受环境结构的复杂性影响，工作人员经常利用力传感器及机器人柔顺控制算法实现与机器人的交互，保证机器人工作的安全性。机器人柔顺控制算法需要实时获取工作人员所施加的力和力矩，然而，当力传感器连接工具时，力传感器的读数包括力传感器初始值、工具重力所施加的力和力矩及工作人员所施加的力和力矩，因此无法直接从力传感器读数获取工作人员所施加的力和力矩，利用重力补偿算法估计力传感器初始值及工具所施加的力和力矩，进而获取工作人员所施加的力和力矩成为必要的工作。

与文献[4]六维力传感器动态力补偿不同，本文认为机器人在低速运动时，工具重力不会带来惯性力，因此本文聚焦于六维力传感器的静态重力补偿。重力补偿本质问题是一个参数估计的问题，具体的参数包括：1) 机器人安装倾角；2) 力传感器坐标系相对机器人末端坐标系的旋转变换矩阵；3) 力传感器初始值；4) 工具的重力及重心位置。

针对机器人安装倾角的估计方法，文献[5]-[8]将安装倾角参数带入力传感器的物理约束方程中，利用最小二乘法对参数进行估计，由此可得到重力方向在机器人基坐标系下的表示。有两种针对力传感器坐标系相对于机器人末端坐标系的旋转变换矩阵的估计方法，文献[9]-[11]假设了力传感器坐标系和机器人末端坐标系平行，两个坐标系之间相差一个坐标原点的距离。文献[12]利用最小二乘法标定力传感器坐标系相对于机器人末端坐标系Z轴的旋转角度，但没有标定相对于X、Y轴的旋转角度。有三种针对力传感器初始值的估计方法。文献[13]-[15]通过变换机器人多组姿态，并保证工具重力对力传感器的施力方向在同组姿态中相反，将多组结果进行相加即可求得力传感器的初始值。文献[9]-[13] [16] [17]通过拆卸末端工具直接获得力传感器初始值，但该方法增加了操作的复杂度。文献[5]-[8]通过变换多组机器人末端的任意姿态，将初始值参数带入力传感器的物理约束方程中，利用最小二乘法对该参数进行估计。有三种针对工具重力及重心位置进行估计的方法，文献[5]-[8] [13]-[16]将待估计参数代入力传感器的物理约束方程中，利用最小二乘法对参数进行估计。文献[9]-[11] [17]通过测量称重和CAD模型直接得出重力和重心位置。文献[11]没有估计工具的重心位置，直接从清零后的力传感器读数中得到工具的重力参数值。

综上所述，针对机器人安装倾角参数主要采用最小二乘法估计，但一般机器人底座和地面平行，该参数估计可以省略。针对力传感器坐标系相对于机器人末端坐标系的旋转变换矩阵的估计，大多文献都认为两个坐标系平行，或只针对力传感器坐标系相对于机器人末端坐标系Z轴的旋转角度进行估计，但上述文献都不能精准地标定出两个坐标系之间的旋转变换矩阵。当力传感器坐标系没有安装示意时，很难判断力传感器坐标系与机器人末端坐标系平行的安装位置，需要反复拆卸旋转调整力传感器的安装姿态。同时，受器件加工精度的影响，力传感器坐标系Z轴往往和机械臂末端坐标系的Z轴不平行，导致重力补偿的效果变差。当将重力补偿应用到机械臂力控模型时，如果不能获得力传感器坐标系和机械臂末端坐标系的旋转变换矩阵，重力补偿效果变差，不能保证力控模型的有效性，因此需要提出一种对力传感器坐标系和机械臂末端坐标系的旋转矩阵精准估计方法。针对力传感器初始值的估计方法，通过拆卸工具进而获得初始值参数的方法无疑增加了操作的复杂度。通过变换机械臂多组姿态获得力传感器初始值的方法需要保证工具重力对力传感器的施力方向相反，也增加了操作的复杂度。针对工具重力及重心位置进行估计的方法，通常会受加工和安装精度的影响，无法直接从CAD中直接获得参数。因此本文选择采用参数估计的方法对力传感器初始值，工具重力及重心位置进行估计。

本文提出一种带有力传感器初始值的力牵引重力补偿方法，与已有方法不同，为解决力传感器因安装不精确，无法保证力传感器坐标系和机械臂末端坐标系平行，进而需要反复拆卸调整安装力传感器的问题，本文通过测试并保证机器人底座和地面基本平行的前提下，变换机器人多组任意姿态，利用最小二乘法估计力传感器的初始值、工具的重力及工具的重心位置，变换机器人多组特定姿态精准标定力传感器坐标系相对机器人末端坐标系的旋转变换矩阵，从而解决力传感器因安装不精确，需要反复拆卸调整安装的问题。基于以上步骤，可以实现重力补偿自动化，进而实时获得工作人员所施加的力和力矩。

2. 算法原理

2.1. 重力补偿原理

如图1所示，为重力补偿原理示意图， ${B}$ 表示机器人基坐标系， ${E}$ 表示机器人末端坐标系， ${F_{S}}$ 表示六维力传感器坐标系，工具重心在 $p^{gravity}$ 处，且工具对六维力传感器 $X$ 、 $Y$ 、 $Z$ 轴所施加力及力矩为 $F_{T x}$ 、 $F_{T y}$ 、 $F_{T z}$ 、 $M_{T x}$ 、 $M_{T y}$ 、 $M_{T z}$ 。

设施加给力传感器的外力和外力矩为 $F_{W x}$ 、 $F_{W y}$ 、 $F_{W z}$ 、 $M_{W x}$ 、 $M_{W y}$ 、 $M_{W z}$ ，本文讨论的是带有力传感器初始值的重力补偿算法，虽然力传感器可以在空载情况下将初始值重新标零，但该方法存在两个弊端：第一个弊端为需要在每次进行重力补偿实验中拆卸工具，步骤繁琐且耗时较长；第二个弊端为所安装工具会对力传感器产生挤压，对力传感器初始值造成影响，造成力传感器空载标零失效。在本文的算法中力传感器初始值是需要待估计的参数，因此设力传感器的初始值为 $F_{I n i t x}$ 、 $F_{I n i t y}$ 、 $F_{I n i t z}$ 、 $M_{I n i t x}$ 、 $M_{I n i t y}$ 、 $M_{I n i t z}$ ，力传感器的读数为 $F_{S x}$ 、 $F_{S y}$ 、 $F_{S z}$ 、 $M_{S x}$ 、 $M_{S y}$ 、 $M_{S z}$ ，工具重心 $p^{gravity}$ 在力传感器坐标系 $X$ 、 $Y$ 、 $Z$ 轴的坐标为 $p_{x}$ 、 $p_{y}$ 、 $p_{z}$ 。重力补偿的目标为离线标定力传感器的初始值，实时估计工具对六维力传感器所施加的力和力矩，进而通过力传感器的读数去实时估计出施加给力传感器的外力和外力矩。下文详细叙述重力补偿具体的算法流程。

本文假定机器人基座水平，则工具重力的方向和机器人基坐标系的 $Z$ 轴平行。由此工具重力在机器人基坐标系中的单位矢量方向为：

Figure 1. Schematic diagram of gravity compensation principle

图1. 重力补偿原理示意图

$G_{B} = [\begin{matrix} 0 \\ 0 \\ - 1 \end{matrix}]$ (1)

工具重力在六维力学传感器坐标系的矢量方向为：

$G_{F_{s}} = {({}_{E}^{B}R {}_{F_{s}}^{E}R)}^{- 1} \cdot G_{B} = [\begin{matrix} n_{x} \\ n_{y} \\ n_{z} \end{matrix}]$ (2)

式中： ${}_{E}^{B}R$ 为机器人末端坐标系在机器人基坐标系下的旋转矩阵， ${}_{F_{s}}^{E}R$ 为力传感器坐标系在机器人末端坐标系下的旋转矩阵。

则工具重力与力传感器坐标系 $X$ 、 $Y$ 、 $Z$ 轴的夹角为：

${\begin{cases} α = acos (\frac{n_{x}}{\sqrt{n_{x}^{2} + n_{y}^{2} + n_{z}^{2}}}) \\ β = acos (\frac{n_{y}}{\sqrt{n_{x}^{2} + n_{y}^{2} + n_{z}^{2}}}) \\ γ = acos (\frac{n_{z}}{\sqrt{n_{x}^{2} + n_{y}^{2} + n_{z}^{2}}}) \end{cases}$ (3)

则工具重力对六维力传感器所施加的力为：

${\begin{cases} F_{T x} = G \cos (α) \\ F_{T y} = G \cos (β) \\ F_{T z} = G \cos (γ) \end{cases}$ (4)

由此可以得到估计出施加给力传感器的外力和外力矩的公式为：

${\begin{cases} F_{W x} = F_{S x} - F_{T x} - F_{I n i t x} \\ F_{W y} = F_{S y} - F_{T y} - F_{I n i t y} \\ F_{W z} = F_{S z} - F_{T z} - F_{I n i t z} \\ M_{W x} = M_{S x} - F_{T z} p_{y} + F_{T y} p_{z} - M_{I n i t x} \\ M_{W y} = M_{S y} - F_{T x} p_{z} + F_{T z} p_{x} - M_{I n i t y} \\ M_{W z} = M_{S z} - F_{T y} p_{x} + F_{T x} p_{y} - M_{I n i t z} \end{cases}$ (5)

由式(5)可知，实时估计施加给力传感器的外力和外力矩需要获得多个未知参数，其中包括 ${}_{F_{s}}^{E}R$ 、 $F_{I n i t x}$ 、 $F_{I n i t y}$ 、 $F_{I n i t z}$ 、 $M_{I n i t x}$ 、 $M_{I n i t y}$ 、 $M_{I n i t z}$ 、 $p_{x}$ 、 $p_{y}$ 、 $p_{z}$ 、 $G$ 。下文1.2节介绍基于最小二乘法对 $F_{I n i t x}$ 、 $F_{I n i t y}$ 、 $F_{I n i t z}$ 、 $M_{I n i t x}$ 、 $M_{I n i t y}$ 、 $M_{I n i t z}$ 、 $p_{x}$ 、 $p_{y}$ 、 $p_{z}$ 、 $G$ 参数估计方法。1.3节介绍变换机器人多组特定姿态精准标定 ${}_{F_{s}}^{E}R$ 的方法。

2.2. 基于最小二乘法的参数估计

力传感器安装工具且不受外力情况下，机器人末端坐标系变换 $k = 1, \dots, n^{gravity} (n^{gravity} \geq 4)$ 种不同的姿态，获得力传感器测得的数据为 $[\begin{matrix} \begin{matrix} F_{S x}^{k} & F_{S y}^{k} & F_{S z}^{k} \end{matrix} & \begin{matrix} M_{S x}^{k} & M_{S y}^{k} & M_{S z}^{k} \end{matrix} \end{matrix}]$ ，且此时工具重力对力传感器的力及力矩为 $[\begin{matrix} \begin{matrix} F_{T x}^{k} & F_{T y}^{k} & F_{T z}^{k} \end{matrix} & \begin{matrix} M_{T x}^{k} & M_{T y}^{k} & M_{T z}^{k} \end{matrix} \end{matrix}]$ ，所施加给力学传感器的力及力矩为 $[\begin{matrix} \begin{matrix} F_{W x}^{k} & F_{W y}^{k} & F_{W z}^{k} \end{matrix} & \begin{matrix} M_{W x}^{k} & M_{W y}^{k} & M_{W z}^{k} \end{matrix} \end{matrix}]$ ，由此可得：

${\begin{cases} F_{S x}^{k} = F_{I n i t x} + F_{T x}^{k} \\ F_{S y}^{k} = F_{I n i t y} + F_{T y}^{k} \\ F_{S z}^{k} = F_{I n i t z} + F_{T z}^{k} \end{cases}$ (6)

${\begin{cases} M_{S x}^{k} = M_{I n i t x} + F_{T z}^{k} p_{y} - F_{T y}^{k} p_{z} \\ M_{S y}^{k} = M_{I n i t y} + F_{T x}^{k} p_{z} - F_{T z}^{k} p_{x} \\ M_{S z}^{k} = M_{I n i t z} + F_{T y}^{k} p_{x} - F_{T x}^{k} p_{y} \end{cases}$ (7)

从力传感器测得的 $n^{gravity}$ 组数据中选择 $d (d \geq 3)$ 对不同且不线性相关数据为 $[F_{S x}^{a i} F_{S y}^{a i} F_{S z}^{a i} M_{S x}^{a i} M_{S y}^{a i} M_{S z}^{a i}]$ $[F_{S x}^{a j} F_{S y}^{a j} F_{S z}^{a j} M_{S x}^{a j} M_{S y}^{a j} M_{S z}^{a j}]$ $(a = 1, 2, \dots, d; a i, a j \leq n^{gravity}, a i \neq a j)$ ，代入式(6)中得：

$\begin{array}{l} F_{S x}^{a i^{2}} + F_{S y}^{a i^{2}} + F_{S z}^{a i^{2}} - (F_{S x}^{a j^{2}} + F_{S y}^{a j^{2}} + F_{S z}^{a j^{2}}) \\ = 2 (F_{S x}^{a i} - F_{S x}^{a j}) F_{I n i t x} + 2 (F_{S y}^{a i} - F_{S y}^{a j}) F_{I n i t y} + 2 (F_{S z}^{a i} - F_{S z}^{a j}) F_{I n i t z} \end{array}$ (8)

则：

$B = A [\begin{matrix} F_{I n i t x} \\ F_{I n i t y} \\ F_{I n i t z} \end{matrix}]$ (9)

式中：

$B = [\begin{matrix} F_{S x}^{1 i^{2}} + F_{S y}^{1 i^{2}} + F_{S z}^{1 i^{2}} - (F_{S x}^{1 j^{2}} + F_{S y}^{1 j^{2}} + F_{S z}^{1 j^{2}}) \\ F_{S x}^{2 i^{2}} + F_{S y}^{2 i^{2}} + F_{S z}^{2 i^{2}} - (F_{S x}^{2 j^{2}} + F_{S y}^{2 j^{2}} + F_{S z}^{2 j^{2}}) \\ F_{S x}^{3 i^{2}} + F_{S y}^{3 i^{2}} + F_{S z}^{3 i^{2}} - (F_{S x}^{3 j^{2}} + F_{S y}^{3 j^{2}} + F_{S z}^{3 j^{2}}) \\ ⋮ \\ F_{S x}^{d i^{2}} + F_{S y}^{d i^{2}} + F_{S z}^{d i^{2}} - (F_{S x}^{d j^{2}} + F_{S y}^{d j^{2}} + F_{S z}^{d j^{2}}) \end{matrix}]$

$A = [\begin{matrix} 2 (F_{S x}^{1 i} - F_{S x}^{1 j}) & 2 (F_{S y}^{1 i} - F_{S y}^{1 j}) & 2 (F_{S z}^{1 i} - F_{S z}^{1 j}) \\ 2 (F_{S x}^{2 i} - F_{S x}^{2 j}) & 2 (F_{S y}^{2 i} - F_{S y}^{2 j}) & 2 (F_{S z}^{2 i} - F_{S z}^{2 j}) \\ 2 (F_{S x}^{3 i} - F_{S x}^{3 j}) & 2 (F_{S y}^{3 i} - F_{S y}^{3 j}) & 2 (F_{S z}^{3 i} - F_{S z}^{3 j}) \\ ⋮ & ⋮ & ⋮ \\ 2 (F_{S x}^{d i} - F_{S x}^{d j}) & 2 (F_{S y}^{d i} - F_{S y}^{d j}) & 2 (F_{S z}^{d i} - F_{S z}^{d j}) \end{matrix}]$

利用最小二乘法求解出初始值 $F_{I n i t x}^{k}$ 、 $F_{I n i t y}^{k}$ 、 $F_{I n i t z}^{k}$ 。将 $F_{I n i t x}^{k}$ 、 $F_{I n i t y}^{k}$ 、 $F_{I n i t z}^{k}$ 代入式(6)中可以得出 $n^{gravity}$ 组 $G$ 的结果，将 $n^{gravity}$ 组结果求平均值，即可求得工具重力 $G$ 的值。

将 $F_{I n i t x}^{k}$ 、 $F_{I n i t y}^{k}$ 、 $F_{I n i t z}^{k}$ 带入式(6)中可得到 $F_{T x}^{k}$ 、 $F_{T y}^{k}$ 、 $F_{T z}^{k} (k = 1, \dots, n^{gravity})$ ，将此值代入式(7)中，可得：

$B B = A A [\begin{matrix} \begin{matrix} M_{I n i t x} \\ M_{I n i t y} \end{matrix} \\ \begin{matrix} M_{I n i t z} \\ p_{x} \end{matrix} \\ \begin{matrix} p_{y} \\ p_{z} \end{matrix} \end{matrix}]$ (10)

式中：

$B B = [\begin{matrix} M_{S x}^{1} \\ M_{S y}^{1} \\ M_{S z}^{1} \\ ⋮ \\ M_{S x}^{n^{gravity}} \\ M_{S y}^{n^{gravity}} \\ M_{S z}^{n^{gravity}} \end{matrix}], A A = [\begin{matrix} 1 & 0 & 0 & 0 & F_{T z}^{1} & - F_{T y}^{1} \\ 0 & 1 & 0 & - F_{T z}^{1} & 0 & F_{T x}^{1} \\ 0 & 0 & 1 & F_{T y}^{1} & - F_{T x}^{1} & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ 1 & 0 & 0 & 0 & F_{T z}^{n^{gravity}} & - F_{T y}^{n^{gravity}} \\ 0 & 1 & 0 & - F_{T z}^{n^{gravity}} & 0 & F_{T x}^{n^{gravity}} \\ 0 & 0 & 1 & F_{T y}^{n^{gravity}} & - F_{T x}^{n^{gravity}} & 0 \end{matrix}]$

利用最小二乘法求得工具质心在六维力学传感器的坐标 $p_{x}$ 、 $p_{y}$ 、 $p_{z}$ ，力传感器力矩初始值 $M_{I n i t x}$ 、 $M_{I n i t y}$ 、 $M_{I n i t z}$ 。

2.3. 力传感器坐标系标定

本文假定工具重力的方向和机器人基坐标系的 $Z$ 轴平行，则重力方向与机器人基坐标系的 $Z$ 轴重合，方向相反。

调整机器人位姿，保证机器人末端坐标系 $X$ 轴与机器人基坐标系 $Z$ 轴重合，方向相反，获得工具对力传感器所施加的力和力矩为： $[\begin{matrix} \begin{matrix} F_{T x}^{m x} & F_{T y}^{m x} & F_{T z}^{m x} \end{matrix} & \begin{matrix} M_{T x}^{m x} & M_{T y}^{m x} & M_{T z}^{m x} \end{matrix} \end{matrix}]$ ，调整机器人位姿，保证机器人末端坐标系 $Y$ 轴与机器人基坐标系 $Z$ 轴重合，方向相反，获得工具对力传感器所施加的力和力矩为： $[\begin{matrix} \begin{matrix} F_{T x}^{m y} & F_{T y}^{m y} & F_{T z}^{m y} \end{matrix} & \begin{matrix} M_{T x}^{m y} & M_{T y}^{m y} & M_{T z}^{m y} \end{matrix} \end{matrix}]$ ，该调整机器人位姿过程可以通过编写笛卡尔空间规划代码自动实现。设力学传感器坐标系和机器人末端坐标系两者之间初始估计的变换矩阵为：

${}_{E}^{F_{s}}{R^{'}} = [\begin{matrix} {a^{'}}_{x} & {a^{'}}_{y} & {a^{'}}_{z} \\ {o^{'}}_{x} & {o^{'}}_{y} & {o^{'}}_{z} \\ {n^{'}}_{x} & {n^{'}}_{y} & {n^{'}}_{z} \end{matrix}]$ (11)

式中： ${a^{'}}_{x}, {o^{'}}_{x}, {n^{'}}_{x}$ 为机器人末端坐标系 $X$ 轴在力学传感器坐标系中的方向余弦， ${a^{'}}_{y}, {o^{'}}_{y}, {n^{'}}_{y}$ 为机器人末端坐标系 $Y$ 轴在力学传感器坐标系中的方向余弦， ${a^{'}}_{z}, {o^{'}}_{z}, {n^{'}}_{z}$ 为机器人末端坐标系 $Z$ 轴在力学传感器坐标系中的方向余弦。

则由采集的力传感器信息可得到：

${\begin{cases} {a^{'}}_{x} = \frac{F_{T x}^{m x}}{\sqrt{F_{T x}^{m x^{2}} + F_{T y}^{m x^{2}} + F_{T z}^{m x^{2}}}} \\ {o^{'}}_{x} = \frac{F_{T y}^{m x}}{\sqrt{F_{T x}^{m x^{2}} + F_{T y}^{m x^{2}} + F_{T z}^{m x^{2}}}} \\ {n^{'}}_{x} = \frac{F_{T z}^{m x}}{\sqrt{F_{T x}^{m x^{2}} + F_{T y}^{m x^{2}} + F_{T z}^{m x^{2}}}} \end{cases}$

${\begin{cases} {a^{'}}_{y} = \frac{F_{T x}^{m y}}{\sqrt{F_{T x}^{m y^{2}} + F_{T y}^{m y^{2}} + F_{T z}^{m y^{2}}}} \\ {o^{'}}_{y} = \frac{F_{T y}^{m y}}{\sqrt{F_{T x}^{m y^{2}} + F_{T y}^{m y^{2}} + F_{T z}^{m y^{2}}}} \\ {n^{'}}_{y} = \frac{F_{T z}^{m y}}{\sqrt{F_{T x}^{m y^{2}} + F_{T y}^{m y^{2}} + F_{T z}^{m y^{2}}}} \end{cases}$ (12)

为保证最终结果的正交性，这里需要对初始结果进行叉乘的处理。设力学传感器坐标系和机器人末端坐标系两者之间最终结果的旋转矩阵为：

${}_{E}^{F_{s}}R = [\begin{matrix} a_{x} & a_{y} & a_{z} \\ o_{x} & o_{y} & o_{z} \\ n_{x} & n_{y} & n_{z} \end{matrix}]$ (13)

式中： $a_{x}, o_{x}, n_{x}$ 为机器人末端坐标系 $X$ 轴在力学传感器坐标系中的方向余弦， $a_{y}, o_{y}, n_{y}$ 为机器人末端坐标系 $Y$ 轴在力学传感器坐标系中的方向余弦， $a_{z}, o_{z}, n_{z}$ 为机器人末端坐标系 $Z$ 轴在力学传感器坐标系中的方向余弦。

则力学传感器坐标系和机器人末端坐标系两者之间最终结果的旋转矩阵为：

$[\begin{matrix} a_{x} \\ o_{x} \\ n_{x} \end{matrix}] = [\begin{matrix} {a^{'}}_{x} \\ {o^{'}}_{x} \\ {n^{'}}_{x} \end{matrix}], [\begin{matrix} a_{z} \\ o_{z} \\ n_{z} \end{matrix}] = [\begin{matrix} {a^{'}}_{x} \\ {o^{'}}_{x} \\ {n^{'}}_{x} \end{matrix}] \times [\begin{matrix} {a^{'}}_{y} \\ {o^{'}}_{y} \\ {n^{'}}_{y} \end{matrix}], [\begin{matrix} a_{y} \\ o_{y} \\ n_{y} \end{matrix}] = [\begin{matrix} a_{z} \\ o_{z} \\ n_{z} \end{matrix}] \times [\begin{matrix} a_{x} \\ o_{x} \\ n_{x} \end{matrix}]$ (14)

3. 实验验证

3.1. 实验系统

如图2所示，为真实实验示意图，本文所采用的机械臂为AUBO-I5六轴机械臂，额定负载为5 kg，重复定位精度为±0.02 mm，具有防碰撞停止功能，并提供了对应的SDK控制包。本文所采用的力传感器为ATI-Gamma六维力学传感器， $X$ 、 $Y$ 轴的力量程为±130 N， $Z$ 轴的力量程为±400 N，力矩量程为±10 N∙m，最小力分辨率为0.0063 N，最小力矩分辨率为0.0005 N∙m，在机械臂末端利用连接件固连施力工具。

Figure 2. Schematic diagram of real experiment

图2. 真实实验示意图

3.2. 实验结果

为验证算法的精度，将力传感器安装到机械臂末端，随机生成N (本文实验N = 10)组机械臂六个关节角度，并将所生成的六个关节角的角度记录到表1中。由于此时没有施加外力和外力矩，因此经过重力补偿后所测得的施加的力和力矩的真值应为[0 0 0 0 0 0]。这里将N个机械臂末端处于不同位姿时所测得的施加的力和力矩求平均值，并将该值定义为残差D。

$Δ = \frac{\sum_{i = 1}^{N} [\begin{matrix} \begin{matrix} | F_{W x}^{i} | & | F_{W y}^{i} | & | F_{W z}^{i} | \end{matrix} & \begin{matrix} | M_{W x}^{i} | & | M_{W y}^{i} | & | M_{W z}^{i} | \end{matrix} \end{matrix}]}{N}$ (15)

该残差越接近于0，说明重力补偿的效果越好。表2显示了未进行重力补偿时的残差，表3显示了有重力补偿且无力传感器坐标系标定时残差，表4显示了有重力补偿且有力传感器坐标系标定时残差。

从表2、表3及表4中可以看出，在重力补偿中添加力传感器坐标系标定的步骤后，重力补偿的残差最小，本文重力补偿算法的残差为[0.237/N 0.395/N 0.928/N 0.032/N∙m 0.018/N∙m 0.002/N∙m]，对比于原算法的重力补偿残差[5.011/N 3.171/N 0.3/N 0.214/N∙m 0.339/N∙m 0.010/N∙m]，该算法具有一定的优越性，因此本文的重力补偿算法具有有效性。

图3表示3种方法的重力补偿残差示意图， $Sqrt ({| F_{W x} |}^{2} + {| F_{W y} |}^{2} + {| F_{W z} |}^{2})$ 表示力维度上的重力补偿后残差， $Max (| M_{W x} |, | M_{W y} |, | M_{W z} |)$ 表示力矩维度上的重力补偿后残差，从曲线变化图可以看出本文的重力补偿

Table 1. Ten groups of robot joint angles

表1. 十组不同的机器人关节角

次数	J1˚	J2˚	J3˚	J4˚	J5˚	J6˚
1	25	15	30	43	−17	2
2	−33	−15	37	34	42	28
3	−10	−19	18	2	48	−11
4	−37	23	30	−38	−22	−43
5	13	22	37	−29	−48	−19
6	40	−26	49	47	4	1
7	46	20	−32	29	31	21
8	−22	20	−21	20	−7	−48
9	48	−19	−28	11	−33	−24
10	9	16	34	37	−5	−44

Table 2. Residual error without gravity compensation

表2. 无重力补偿时残差

次数	$\| F_{W x} \|$ (N)	$\| F_{W y} \|$ (N)	$\| F_{W z} \|$ (N)	$\| M_{W x} \|$ (N∙m)	$\| M_{W y} \|$ (N∙m)	$\| M_{W z} \|$ (N∙m)
1	1.722	9.868	0.343	0.648	0.043	0.020
2	9.231	3.492	0.103	0.215	0.554	0.004
3	8.286	5.907	1.234	0.387	0.494	0.003
4	1.335	8.897	2.821	0.590	0.035	0.013
5	5.234	7.093	3.296	0.464	0.414	0.016
6	0.741	8.381	5.858	0.559	0.132	0.017
7	3.002	9.792	0.593	0.655	0.126	0.023
8	5.695	5.501	5.493	0.380	0.284	0.025
9	8.164	5.645	0.154	0.372	0.481	0.005
10	6.065	7.854	0.821	0.524	0.344	0.014
error	4.948	7.243	2.072	0.480	0.291	0.140

Table 3. Residual error after gravity compensation without force sensor frame calibration

表3. 力传感器坐标系无标定时的重力补偿后残差

次数	$\| F_{W x} \|$ (N)	$\| F_{W y} \|$ (N)	$\| F_{W z} \|$ (N)	$\| M_{W x} \|$ (N∙m)	$\| M_{W y} \|$ (N∙m)	$\| M_{W z} \|$ (N∙m)
1	6.051	1.628	0.421	0.119	0.399	0.003
2	4.221	5.133	0.508	0.340	0.284	0.019
3	5.421	3.455	0.006	0.218	0.366	0.014
4	5.558	1.508	0.424	0.117	0.376	0.001
5	2.902	5.732	0.244	0.396	0.209	0.020
6	4.771	2.839	0.514	0.212	0.323	0.008
7	6.494	0.945	0.251	0.085	0.429	0.005
8	2.666	4.790	0.218	0.307	0.192	0.009
9	5.508	3.916	0.176	0.246	0.370	0.013
10	6.522	1.760	0.241	0.098	0.440	0.005
error	5.011	3.171	0.300	0.214	0.339	0.010

Table 4. Residual error after gravity compensation with force sensor frame calibration

表4. 力传感器坐标系标定后的重力补偿后残差

次数	$\| F_{W x} \|$ (N)	$\| F_{W y} \|$ (N)	$\| F_{W z} \|$ (N)	$\| M_{W x} \|$ (N∙m)	$\| M_{W y} \|$ (N∙m)	$\| M_{W z} \|$ (N∙m)
1	0.580	0.488	1.341	0.042	0.049	0.002
2	0.058	0.256	0.675	0.021	0.003	0.000
3	0.022	0.732			0.003	0.001
4	0.484	0.465	1.263	0.047	0.032	0.002
5	0.028	0.391	1.035	0.014	0.014	0.001
6	0.240	0.535	1.385	0.015	0.016	0.001
7	0.310	0.435	1.149	0.051	0.030	0.005
8	0.550	0.368	0.498	0.010	0.026	0.003
9	0.076	0.086	0.606	0.022	0.005	0.002
10	0.019	0.190	0.930	0.033	0.002	0.003
error	0.237	0.395	0.928	0.032	0.018	0.002

Figure 3. Residual errors under different gravity compensation

图3. 不同重力补偿情况下的残差示意图

算法残差最小且平稳性最好。

4. 结论

1) 目前重力补偿算法普遍假设力传感器坐标系和机器人末端坐标系平行，而受加工件的影响，工作人员需要不断设计加工件并拆卸调整安装力传感器，从而保证力传感器坐标系和机器人末端坐标系平行。本文提出一种具有标定力传感器坐标系相对于机器人末端坐标系的带有力传感器初始值的重力补偿算法，从算法层面解决了力传感器坐标系和机器人末端坐标系不平行的问题，降低了重力补偿操作的复杂度。

2) 给出了所提出方法的理论分析，说明了重力补偿所需要估计的参数量，介绍了利用最小二乘法估计力传感器初始值、工具重力及重心坐标及利用多个特定机器人位姿精准标定力传感器坐标系相对于机器人末端坐标系的方法，确保了模型的有效性。

3) 实验结果表明，在力传感器坐标系和机器人末端坐标系不平行的情况下，本文的算法与原有的重力补偿算法相比，降低了重力补偿残差，提高了重力补偿算法的场景实用度。

后续工作将深入研究力传感器坐标系标定的精确度及基于动态力的重力补偿算法，提高机器人运动过程中的重力补偿效果，进而提高机器人力控算法的稳定性。

基金项目

国家自然科学基金共融机器人重大研究计划重点项目(No. 92148206)与北京市自然科学基金(7202103)。

NOTES

^*通讯作者Email: Tsai@buaa.edu.cn

参考文献

[1]	张思涵. 采用力控的外骨骼人机交互及机械臂柔顺装配研究[D]: [硕士学位论文]. 杭州: 浙江大学, 2021.
[2]	徐冰洁. 基于末端六维力传感器的操作臂力/位混合控制及应用[D]: [硕士学位论文]. 济南: 山东大学, 2021.
[3]	李玉希, 黄浚燊, 刘婷, 等. 脊柱手术机器人在脊柱外科手术中的应用进展[J]. 机器人外科学杂志, 2021, 2(2): 143-150.
[4]	王志军, 刘璐, 李占贤, 等. 基于六维力传感器的机器人动态力补偿研究[J]. 机械设计, 2020, 37(11): 72-77.
[5]	张立建, 胡瑞钦, 易旺民. 基于六维力传感器的工业机器人末端负载受力感知研究[J]. 自动化学报, 2017, 43(3): 439-447.
[6]	朱文斌, 李俊, 杨进兴, 等. 基于重力补偿与滤波模型的工业机械臂打磨技术的研究[J]. 工业控制计算机, 2021, 34(8): 67-69+135.
[7]	张健, 刘韦, 宋丽. 基于力传感器重力补偿的机器人主动柔顺插孔控制方法[J]. 制造业自动化, 2022, 44(3): 197-200.
[8]	黄玲涛, 王彬, 倪水, 等. 基于力传感器重力补偿的机器人柔顺控制研究[J]. 农业机械学报, 2020, 51(3): 386-393.
[9]	Du, H., Sun, Y., Feng, D. and Xu, J. (2015) Automatic Robotic Polishing on Titanium Alloy Parts with Compliant Force/Position Control. Proceedings of the Institution of Mechanical Engineers, Part B: Journal of Engineering Manufacture, 229, 1180-1192. https://doi.org/10.1177/0954405414567518
[10]	张光辉, 王耀南. 末端F/T传感器的重力环境下大范围柔顺控制方法[J]. 智能系统学报, 2015, 10(5): 675-683.
[11]	高强, 田凤杰, 杨林, 等. 机器人自动研抛系统平台搭建及重力补偿研究[J]. 工具技术, 2015, 49(8): 47-50.
[12]	盛国栋, 曹其新, 潘铁文, 等. 主从式机器人系统中力反馈的实现[J]. 中国机械工程, 2015(9): 1157-1160+1166.
[13]	Vougioukas, S. (2014) Bias Estimation and Gravity Compensation for Force-Torque Sensors. https://www.researchgate.net/publication/253989945_Bias_Estimation_and_Gravity_Compensation_For_Force-Torque_Sensors
[14]	林君健. 基于力传感器的工业机器人主动柔顺装配系统研究[D]: [硕士学位论文]. 广州: 华南理工大学, 2013.
[15]	蔡明君. 基于力融合控制的遥操作机器人同构式手控器研究[D]: [硕士学位论文]. 长春: 吉林大学, 2015.
[16]	刘文波. 基于力控制方法的工业机器人磨削研究[D]: [硕士学位论文]. 广州: 华南理工大学, 2014.
[17]	Massa, D., Callegari, M. and Cristalli, C. (2015) Manual Guidance for Industrial Robot Programming. Industrial Robot: An International Journal, 42, 457-465. https://doi.org/10.1108/ir-11-2014-0413

为你推荐

友情链接