机械臂六维力传感器重力补偿

1.力传感器数据分析

将六维力传感器三个力分量与力矩分量的零点值分别记为$(F_{x0},F_{y0},F_{z0})$，$(M_{x0},M_{y0},M_{z0})$，

传感器与末端工装重力为$G$,质心在六维力传感器坐标系中的坐标为$(x,y,z)$，

重力$G$在三个坐标轴方向上的分力与作用力矩分别为$(G_x,G_y,G_z)$，$(M_{gx},M_{gy},M_{gz})$。

根据力与力矩的关系可以得到（正方向由右手定则确定）：
$$\{\begin{array}{c}{M}_{gx}=G_z\times y-G_y\times z\\ M_{gy}=G_x\times z-G_z\times x\\ M_{gz}=G_y\times x-G_x\times y\end{array}$$
将六维力传感器直接测量得到的三个方向的力分量与力矩分量分别记为$(F_x,F_y,F_z)$，$(M_x,M_y,M_z)$，

假设标定的时候没有外部作用力在末端夹持器上，则力传感器所测得的力和力矩由负载重力及零点组成，则有
$$\{\begin{array}{c}{F}_x=G_x+F_{x0}\\ F_y=G_y+F_{y0}\\ F_z=G_z+F_{z0}\end{array}$$

$$\{\begin{array}{c}{M}_x=M_{gx}+M_{x0}\\ M_y=M_{gy}+M_{y0}\\ M_z=M_{gz}+M_{z0}\end{array}$$

联立得：
$$\{\begin{array}{c}{M}_x=(F_z-F_{z0})\times y-(F_y-F_{y0})\times z+M_{x0}\\ M_y=(F_x-F_{x0})\times z-(F_z-F_{z0})\times x+M_{y0}\\ M_z=(F_y-F_{y0})\times x-(F_x-F_{x0})\times y+M_{z0}\end{array}$$

$$\{\begin{array}{c}{M}_x=F_z\times y-F_y\times z+M_{x0}+F_{y0}\times z-F_{z0}\times y\\ M_y=F_x\times z-F_z\times x+M_{y0}+F_{z0}\times x-F_{x0}\times z\\ M_z=F_y\times x-F_x\times y+M_{z0}+F_{x0}\times y-F_{y0}\times x\end{array}$$

其中的$F_{x0},F_{y0},F_{z0},M_{x0},M_{y0},M_{z0},x,y,z$为定值常数（忽略力传感器数据波动，并且末端工装不进行更换）

2.最小二乘法求解参数

2.1力矩方程

令
$$\{\begin{array}{c}{k}_1=M_{x0}+F_{y0}\times z-F_{z0}\times y\\ k_2=M_{y0}+F_{z0}\times x-F_{x0}\times z\\ k_3=M_{z0}+F_{x0}\times y-F_{y0}\times x\end{array}$$

则：

控制机械臂末端夹持器的位姿，取N个不同的姿态(N $\ge$ 3，要求至少有三个姿态下的机械臂末端夹持器的指向向量不共面），得到N组六维力数据传感器数据，

$M=F\cdot A$ ，其中$A=[x,y,z,k_1,k_2,k_3{]}^{\mathrm{T}}$，则矩阵的最小二乘解为$A=(F^{\mathrm{T}}F{)}^{-1}\cdot F^{\mathrm{T}}\cdot M$，

由多组数据可以解出负载质心在六维力传感器坐标系中的坐标$(x,y,z)$以及常数$(k_1,k_2,k_3)$的值。

2.2力方程

在机器人的安装固定过程中，由于安装平台会存在一定的斜度，使得机器人的基坐标系与世界坐标系出现一定的夹角，同样也会使得传感器的测量值与实际值存在一定偏差，因此需要进行倾角的计算。

记世界坐标系为$O$，基坐标系为$B$，法兰坐标系为$W$，力传感器坐标系为$C$，则由基坐标系向世界坐标系的姿态变换矩阵为：

其中$U$是绕世界坐标系$x$轴的旋转角，$V$是绕基坐标系的$y$轴的旋转角

由末端法兰到基坐标系的姿态变换矩阵为：
$${}_W^{B}R=R_z(A)R_y(B)R_x(C)$$
$ABC$的值可以通过机器人示教器读出

因为力传感器安装到末端时，会存在$Z$轴上的一定偏角$\alpha$（可通过更换机器人参考坐标系手动将其重合，即偏角为0），由力传感器向法兰坐标系坐标系的姿态变换矩阵为：

则由世界坐标系到六维力传感器坐标系的姿态变换矩阵为

$${}_0^{C}R={}_W^{C}R\cdot {}_B^{W}R\cdot {}_O^{B}R={{}_C^{W}R}^{\mathrm{T}}\cdot {{}_W^{B}R}^{\mathrm{T}}\cdot {{}_B^{O}R}^{\mathrm{T}}$$

重力在世界坐标系$O$中的表示为${}^{O}G=[0,0,-g{]}^{\mathrm{T}}$,

通过坐标变换，将重力分解到力传感器坐标系$C$得到
$$\begin{gathered} {}^{C}G{=}_O^{C}R\cdot {}^{O}G={}_W^{C}R\cdot {}_B^{W}R\cdot {}_O^{B}R\cdot {}^{O}G={{}_C^{W}R}^{\mathrm{T}}\cdot {{}_W^{B}R}^{\mathrm{T}}\cdot {{}_B^{O}R}^{\mathrm{T}}\cdot {}^{O}G \\ ={{}_C^{W}R}^{\mathrm{T}}\cdot {{}_W^{B}R}^{\mathrm{T}}\cdot \left[\begin{array}{c}cosUsinV\ast g\\ -sinU\ast g\\ -cosUcosV\ast g\end{array}\right] \end{gathered}$$

则：
$$[F_x,F_y,F_z{]}^{\mathrm{T}}=[G_x,G_y,G_z{]}^{\mathrm{T}}+[F_{x0},F_{y0},F_{z0}{]}^{\mathrm{T}}{=}_O^{C}R\cdot {}^{O}G+[F_{x0},F_{y0},F_{z0}{]}^{\mathrm{T}}$$

令：

$$\{\begin{array}{c}{L}_x=cosUsinV\ast g\\ L_y=-sinU\ast g\\ L_z=-cosUcosV\ast g\end{array}$$

则可以表示为:

同理，取N个不同的姿态得到N组六维力传感器数据，

$f=R\cdot B$，其中$B=[L_x,L_y,L_z,F_{x0},F_{y0},F_{z0}{]}^{\mathrm{T}}$，则矩阵的最小二乘解为$B=(R^{\mathrm{T}}R{)}^{-1}\cdot R^{\mathrm{T}}\cdot f$，

由多组数据可以解出力传感器的零点值$(F_{x0},F_{y0},F_{z0})$，安装倾角$U,V$以及重力$g$的大小。
$$g=\sqrt{{L_x}^2+{L_y}^2+{L_z}^2}$$

$$U=arcsin(\frac{-L_y}{g})$$

$$V=arctan(\frac{-L_x}{L_z})$$

3.外部接触力计算

由此可以得出外部接触力为：
$$\{\begin{array}{c}{F}_{ex}=F_x-F_{x0}-G_x\\ F_{ey}=F_y-F_{y0}-G_y\\ F_{ez}=F_z-F_{z0}-G_z\end{array}$$

又：
$$\{\begin{array}{c}{M}_{x0}=k_1-F_{y0}\times z+F_{z0}\times y\\ M_{y0}=k_2-F_{z0}\times x+F_{x0}\times z\\ M_{z0}=k_3-F_{x0}\times y+F_{y0}\times x\end{array}$$

$${}^{C}G{=}_O^{C}R\cdot {}^{O}G=[G_x,G_y,G_z{]}^{\mathrm{T}}$$

$$\{\begin{array}{c}{M}_{gx}=G_z\times y-G_y\times z\\ M_{gy}=G_x\times z-G_z\times x\\ M_{gz}=G_y\times x-G_x\times y\end{array}$$

即:

得出外部接触力矩为：

$$\{\begin{array}{c}{M}_{ex}=M_x-M_{x0}-M_{gx}\\ M_{ey}=M_y-M_{y0}-M_{gy}\\ M_{ez}=M_z-M_{Z0}-M_{gz}\end{array}$$

[1]高培阳. 基于力传感器的工业机器人恒力磨抛系统研究[D].华中科技大学,2019.
[2]张立建,胡瑞钦,易旺民.基于六维力传感器的工业机器人末端负载受力感知研究[J].自动化学报,2017,43(03):439-447.DOI:10.16383/j.aas.2017.c150753.
[3]董浩. 轴孔装配工业机器人柔顺控制算法研究[D].河南理工大学,2018.

Made by 水木皆Ming

附代码（已给出3组数据用于测试算法）：

from numpy import *
import numpy as np
import math

'''
Made by 水木皆Ming
重力补偿计算
'''
class GravityCompensation:
    M = np.empty((0, 0))
    F = np.empty((0, 0))
    f = np.empty((0, 0))
    R = np.empty((0, 0))

    x = 0
    y = 0
    z = 0
    k1 = 0
    k2 = 0
    k3 = 0

    U = 0
    V = 0
    g = 0

    F_x0 = 0
    F_y0 = 0
    F_z0 = 0

    M_x0 = 0
    M_y0 = 0
    M_z0 = 0

    F_ex = 0
    F_ey = 0
    F_ez = 0

    M_ex = 0
    M_ey = 0
    M_ez = 0

    def Update_M(self, torque_data):
        M_x = torque_data[0]
        M_y = torque_data[1]
        M_z = torque_data[2]

        if (any(self.M)):
            M_1 = matrix([M_x, M_y, M_z]).transpose()
            self.M = vstack((self.M, M_1))
        else:
            self.M = matrix([M_x, M_y, M_z]).transpose()

    def Update_F(self, force_data):
        F_x = force_data[0]
        F_y = force_data[1]
        F_z = force_data[2]

        if (any(self.F)):
            F_1 = matrix([[0, F_z, -F_y, 1, 0, 0],
                          [-F_z, 0, F_x, 0, 1, 0],
                          [F_y, -F_x, 0, 0, 0, 1]])
            self.F = vstack((self.F, F_1))
        else:
            self.F = matrix([[0, F_z, -F_y, 1, 0, 0],
                             [-F_z, 0, F_x, 0, 1, 0],
                             [F_y, -F_x, 0, 0, 0, 1]])

    def Solve_A(self):
        A = dot(dot(linalg.inv(dot(self.F.transpose(), self.F)), self.F.transpose()), self.M)

        self.x = A[0, 0]
        self.y = A[1, 0]
        self.z = A[2, 0]
        self.k1 = A[3, 0]
        self.k2 = A[4, 0]
        self.k3 = A[5, 0]
        # print("A= \n" , A)
        print("x= ", self.x)
        print("y= ", self.y)
        print("z= ", self.z)
        print("k1= ", self.k1)
        print("k2= ", self.k2)
        print("k3= ", self.k3)

    def Update_f(self, force_data):
        F_x = force_data[0]
        F_y = force_data[1]
        F_z = force_data[2]

        if (any(self.f)):
            f_1 = matrix([F_x, F_y, F_z]).transpose()
            self.f = vstack((self.f, f_1))
        else:
            self.f = matrix([F_x, F_y, F_z]).transpose()

    def Update_R(self, euler_data):
        # 机械臂末端到基坐标的旋转矩阵
        R_array = self.eulerAngles2rotationMat(euler_data)

        alpha = (0) * 180 / np.pi

        # 力传感器到末端的旋转矩阵
        R_alpha = np.array([[math.cos(alpha), -math.sin(alpha), 0],
                            [math.sin(alpha), math.cos(alpha), 0],
                            [0, 0, 1]
                            ])

        R_array = np.dot(R_alpha, R_array.transpose())

        if (any(self.R)):
            R_1 = hstack((R_array, np.eye(3)))
            self.R = vstack((self.R, R_1))
        else:
            self.R = hstack((R_array, np.eye(3)))

    def Solve_B(self):
        B = dot(dot(linalg.inv(dot(self.R.transpose(), self.R)), self.R.transpose()), self.f)

        self.g = math.sqrt(B[0] * B[0] + B[1] * B[1] + B[2] * B[2])
        self.U = math.asin(-B[1] / self.g)
        self.V = math.atan(-B[0] / B[2])

        self.F_x0 = B[3, 0]
        self.F_y0 = B[4, 0]
        self.F_z0 = B[5, 0]

        # print("B= \n" , B)
        print("g= ", self.g / 9.81)
        print("U= ", self.U * 180 / math.pi)
        print("V= ", self.V * 180 / math.pi)
        print("F_x0= ", self.F_x0)
        print("F_y0= ", self.F_y0)
        print("F_z0= ", self.F_z0)

    def Solve_Force(self, force_data, euler_data):
        Force_input = matrix([force_data[0], force_data[1], force_data[2]]).transpose()

        my_f = matrix([cos(self.U)*sin(self.V)*self.g, -sin(self.U)*self.g, -cos(self.U)*cos(self.V)*self.g, self.F_x0, self.F_y0, self.F_z0]).transpose()

        R_array = self.eulerAngles2rotationMat(euler_data)
        R_array = R_array.transpose()
        R_1 = hstack((R_array, np.eye(3)))

        Force_ex = Force_input - dot(R_1, my_f)
        print('接触力：', Force_ex.T)

    def Solve_Torque(self, torque_data, euler_data):
        Torque_input = matrix([torque_data[0], torque_data[1], torque_data[2]]).transpose()
        M_x0 = self.k1 - self.F_y0 * self.z + self.F_z0 * self.y
        M_y0 = self.k2 - self.F_z0 * self.x + self.F_x0 * self.z
        M_z0 = self.k3 - self.F_x0 * self.y + self.F_y0 * self.x

        Torque_zero = matrix([M_x0, M_y0, M_z0]).transpose()

        Gravity_param = matrix([[0, -self.z, self.y],
                                [self.z, 0, -self.x],
                                [-self.y, self.x, 0]])

        Gravity_input = matrix([cos(self.U)*sin(self.V)*self.g, -sin(self.U)*self.g, -cos(self.U)*cos(self.V)*self.g]).transpose()

        R_array = self.eulerAngles2rotationMat(euler_data)
        R_array = R_array.transpose()

        Torque_ex = Torque_input - Torque_zero - dot(dot(Gravity_param, R_array), Gravity_input)

        print('接触力矩：', Torque_ex.T)

    def eulerAngles2rotationMat(self, theta):
        theta = [i * math.pi / 180.0 for i in theta]  # 角度转弧度

        R_x = np.array([[1, 0, 0],
                        [0, math.cos(theta[0]), -math.sin(theta[0])],
                        [0, math.sin(theta[0]), math.cos(theta[0])]
                        ])

        R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
                        [0, 1, 0],
                        [-math.sin(theta[1]), 0, math.cos(theta[1])]
                        ])

        R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
                        [math.sin(theta[2]), math.cos(theta[2]), 0],
                        [0, 0, 1]
                        ])

        # 第一个角为绕X轴旋转，第二个角为绕Y轴旋转，第三个角为绕Z轴旋转
        R = np.dot(R_x, np.dot(R_y, R_z))
        return R


def main():
    #
    force_data = [-6.349214527290314e-05, 0.0016341784503310919, -24.31537437438965]
    torque_data= [-0.25042885541915894, 0.32582423090934753, 2.255179606436286e-05]
    euler_data = [-80.50866918099089, 77.83705434751874, -9.294185889510375 + 12]

    force_data1 = [-7.469202995300293, 2.3709897994995117, -23.0179500579834]
    torque_data1= [-0.2169264256954193, 0.3719269931316376, 0.10870222747325897]
    euler_data1 = [-105.99038376663763, 60.89987226261212, -10.733422007074305 + 12]

    force_data2 = [-14.45930004119873, 0.995974063873291, -19.523677825927734]
    torque_data2= [-0.19262456893920898, 0.3845194876194, 0.1622740775346756]
    euler_data2 = [-114.24258417090118, 43.78913507089547, -19.384088817327235 + 12]


    compensation = GravityCompensation()

    compensation.Update_F(force_data)
    compensation.Update_F(force_data1)
    compensation.Update_F(force_data2)

    compensation.Update_M(torque_data)
    compensation.Update_M(torque_data1)
    compensation.Update_M(torque_data2)


    compensation.Solve_A()

    compensation.Update_f(force_data)
    compensation.Update_f(force_data1)
    compensation.Update_f(force_data2)

    compensation.Update_R(euler_data)
    compensation.Update_R(euler_data1)
    compensation.Update_R(euler_data2)

    compensation.Solve_B()


    compensation.Solve_Force(force_data, euler_data)
    compensation.Solve_Torque(torque_data, euler_data)
    compensation.Solve_Force(force_data1, euler_data1)
    compensation.Solve_Torque(torque_data1, euler_data1)
    compensation.Solve_Force(force_data2, euler_data2)
    compensation.Solve_Torque(torque_data2, euler_data2)



if __name__ == '__main__':
    main()

代码运行结果：

其中，xyz为仿真软件中设置的三维位置值，g为末端重物的质量，UV倾角为设置的偏角，F_x0等理想情况下为0，末端未接触，得出的接触力应为0。