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

重力补偿原理

1.力传感器数据分析

将六维力传感器三个力分量与力矩分量的零点值分别记为机械臂六维力传感器重力补偿原理，，

传感器与末端工装重力为机械臂六维力传感器重力补偿原理 ,质心在六维力传感器坐标系中的坐标为，

重力机械臂六维力传感器重力补偿原理在三个坐标轴方向上的分力与作用力矩分别为，。

根据力与力矩的关系可以得到（正方向由右手定则确定）：
机械臂六维力传感器重力补偿原理
将六维力传感器直接测量得到的三个方向的力分量与力矩分量分别记为，，

假设标定的时候没有外部作用力在末端夹持器上，则力传感器所测得的力和力矩由负载重力及零点组成，则有
机械臂六维力传感器重力补偿原理

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

联立得：
机械臂六维力传感器重力补偿原理

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

其中的机械臂六维力传感器重力补偿原理为定值常数（忽略力传感器数据波动，并且末端工装不进行更换）

2.最小二乘法求解参数

2.1力矩方程

令
机械臂六维力传感器重力补偿原理
则：

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

机械臂六维力传感器重力补偿原理，其中，则矩阵的最小二乘解为，

由多组数据可以解出负载质心在六维力传感器坐标系中的坐标机械臂六维力传感器重力补偿原理以及常数的值。

2.2力方程

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

记世界坐标系为机械臂六维力传感器重力补偿原理，基坐标系为，法兰坐标系为，力传感器坐标系为，则由基坐标系向世界坐标系的姿态变换矩阵为：

其中是绕世界坐标系轴的旋转角，是绕基坐标系的轴的旋转角

由末端法兰到基坐标系的姿态变换矩阵为：
机械臂六维力传感器重力补偿原理
的值可以通过机器人示教器读出

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

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

重力在世界坐标系机械臂六维力传感器重力补偿原理中的表示为,

通过坐标变换，将重力分解到力传感器坐标系机械臂六维力传感器重力补偿原理得到

则：
机械臂六维力传感器重力补偿原理

机械臂六维力传感器重力补偿原理
令：

则可以表示为:

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

机械臂六维力传感器重力补偿原理，其中，则矩阵的最小二乘解为，

由多组数据可以解出力传感器的零点值机械臂六维力传感器重力补偿原理，安装倾角以及重力的大小。

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

3.外部接触力计算

由此可以得出外部接触力为：
机械臂六维力传感器重力补偿原理

机械臂六维力传感器重力补偿原理
又：

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

即:
机械臂六维力传感器重力补偿原理
得出外部接触力矩为：

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

代码如下：

from numpy import *
import numpy as np
'''
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('接触力：\n', Force_ex)

    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('接触力矩：\n', Torque_ex)

    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_Force(force_data1, euler_data1)
    # compensation.Solve_Force(force_data2, euler_data2)
    #
    # compensation.Solve_Torque(torque_data, euler_data)
    # compensation.Solve_Torque(torque_data1, euler_data1)
    # compensation.Solve_Torque(torque_data2, euler_data2)


if __name__ == '__main__':
    main()

Made by 水木皆Ming

文章出处登录后可见！

已经登录？立即刷新

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

重力补偿原理

1.力传感器数据分析

2.最小二乘法求解参数

2.1力矩方程

2.2力方程

3.外部接触力计算

相关推荐