[发明专利]一种用于带有力传感器初始值的力牵引重力补偿方法

权利要求书

1.一种用于带有力传感器初始值的力牵引重力补偿方法，其特征在于，包括：

获取末端执行工具不受外力且机器人处于不同姿态下的多组六维力传感器测量数据；

根据不同姿态下的所述六维力传感器测量数据为固定值建立方程组，并利用最小二乘法求解最优值，求得六维力传感器初始值参数；

根据所述六维力传感器初始值参数获取末端执行工具重力、末端执行工具质心在所述六维力传感器的坐标和六维力传感器力矩初始值；

求解所述六维力传感器坐标系相对于机器人基坐标系的位姿变换；具体包括：

调整机器人的末端坐标系x轴与机器人基坐标系z轴重合，方向相反，获得六维力传感器xyz轴力和力矩测量数据为：调整机器人的末端坐标系y轴与机器人基坐标系z轴重合，方向相反，获得六维力传感器xyz轴力和力矩测量数据为：调整机器人的末端坐标系z轴与机器人基坐标系z轴重合，方向相反，获得六维力传感器xyz轴力和力矩测量数据为：

设六维力学传感器坐标系为sensor，机器人末端坐标系为end，两者之间旋转矩阵为：

其中，a_x、o_x、n_x为机器人末端坐标系x轴在六维力传感器坐标系中的方向余弦，a_y、o_y、n_y为机器人末端坐标系y轴在六维力传感器坐标系中的方向余弦，a_z、o_z、n_z为机器人末端坐标系z轴在六维力传感器坐标系中的方向余弦；G为末端执行工具重力；

求解末端执行工具不受外力时所施加给所述六维力传感器的力和力矩，具体包括：

末端执行工具重力G在机器人本体基坐标系中的矢量方向为：

G_base＝[0 0 -1] (17)

末端执行工具重力G在六维力传感器坐标系的矢量方向为：

其中：为力传感器坐标系在机器人本体基坐标系下的旋转矩阵；设则末端执行工具重力G对六维力传感器所施加的力为：

末端执行工具质心施加给六维力传感器xyz轴力和力矩为：

表示六维力传感器x、y、z轴力初始值的大小，表示末端执行工具对六维力传感器xyz轴分别施加的力，是第k组六维力传感器xyz测得的力矩大小。

2.根据权利要求1所述的一种用于带有力传感器初始值的力牵引重力补偿方法，其特征在于，根据不同姿态下的所述六维力传感器测量数据为固定值建立方程组，并利用最小二乘法求解最优值，求得六维力传感器初始值参数，具体包括：

从所述六维力传感器测得的n组数据中选择d对不同的两组数据带入公式(1)中，a＝1,2...d,d≥3；ai,aj≤n,ai≠aj,n≥4，其中表示第a对数据中第i、j组六维力传感器测得数据的x、y、z轴力的大小，表示第a对数据中第i、j组六维力传感器测得数据的x、y、z轴力矩的大小；

其中，表示第k组六维力传感器测得数据的x、y、z轴力的大小，表示六维力传感器x、y、z轴力初始值的大小，G为工具重力的大小；

构建方程为：

则：

利用最小二乘法求解出六维力传感器xyz轴力的初始值和

3.根据权利要求2所述的一种用于带有力传感器初始值的力牵引重力补偿方法，其特征在于，根据所述六维力传感器初始值参数获取末端执行工具重力、末端执行工具质心在所述六维力传感器的坐标和六维力传感器力矩初始值，具体包括：

将六维力传感器xyz轴力的初始值和带入公式(1)，并求平均值，得到末端执行工具重力G；

利用公式(7)计算末端执行工具对六维力传感器xyz轴分别施加的力

将末端执行工具对六维力传感器xyz轴所施加的力带入公式(8)：

其中，是第k组六维力传感器xyz测得的力矩大小，是六维力传感器xyz轴的初始力矩，p_x、p_y、p_z是工具质心在六维力传感器坐标系下的x、y、z轴的坐标；

得：

利用最小二乘法求得末端执行工具质心在六维力传感器的xyz轴坐标p_x、p_y、p_z，以及六维力传感器xyz轴分别对应的力矩初始值