一种用于带有力传感器初始值的力牵引重力补偿方法与流程

1.本发明涉及机器人力控技术领域，更具体的说是涉及用于带有力传感器初始值的力牵引重力补偿方法。


背景技术：

2.在利用力传感器对机器人进行应用开发时，力传感器需要实时获取外界所施加的力和力矩。当力传感器连接工具时，力传感器会受工具重力及初始值的影响，无法实时获取外界所施加力的数据。在下颌骨截骨机器人力随动及力学安全监控系统中，对力传感器初始值及工具重力参数进行估计成为必要的工作。
3.由于力传感器工作方式的原因，力传感器在刚通电工作时，力传感器会受到温度等环境影响，初始值往往不固定，且力传感器末端往往连接着法兰和工具，无法实时获得力传感器的初始值，对力传感器初始值进行估计，可以通过拆卸工具和法兰，对力传感器的数值进行采集，求解平均值作为力传感器的初始值，但此种方法需要重复拆卸工具和法兰，增加了工作的繁琐性。为解决此问题，本发明提出不拆卸工具和法兰，利用参数估计的方法，对力传感器初始值进行估计。并且补充了机器人末端坐标系和力传感器坐标系的配准部分，以便提高重力补偿精度。


技术实现要素：

4.有鉴于此，本发明提供了一种用于带有力传感器初始值的力牵引重力补偿方法，用于有效对工具重力进行参数估计，实现工具重力补偿。
5.为了实现上述目的，本发明采用如下技术方案：
6.一种用于带有力传感器初始值的力牵引重力补偿方法，包括：
7.获取末端执行工具不受外力且机器人处于不同姿态下的多组六维力传感器测量数据；
8.根据不同姿态下的所述多组六维力传感器测量数据为固定值建立方程组，并利用最小二乘法求解最优值，求得六维力传感器初始值参数；
9.根据所述六维力传感器初始值参数获取末端执行工具重力、末端执行工具质心在所述六维力传感器的坐标和六维力传感器力矩初始值；
10.求解所述六维力传感器坐标系相对于机器人基坐标系的位姿变换；
11.求解末端执行工具不受外力时所施加给所述六维力传感器的力和力矩。
12.优选的，根据不同姿态下的所述六维力传感器测量数据为固定值建立方程组，并利用最小二乘法求解最优值，求得六维力传感器初始值参数，具体包括：
13.从所述六维力传感器侧得的n组数据中选择d对不同的两组数据带入公式(1)中，a＝1,
2...d,d≥3；ai,aj≤n,ai≠aj,n≥4，其中2...d,d≥3；ai,aj≤n,ai≠aj,n≥4，其中表示第a对数据中第i、j组六维力传感器测得数据的x、y、z轴力的大小，表示第a对数据中第i、j组六维力传感器测得数据的x、y、z轴力矩的大小；
14.构建方程为：
[0015][0016]
其中，表示第k组六维力传感器测得数据的x、y、z轴力的大小，表示六维力传感器x、y、z轴力初始值的大小，g为工具重力的大小；
[0017]
则：
[0018][0019][0020][0021]
利用最小二乘法求解出六维力传感器xyz轴力的初始值和
[0022][0023]
优选的，根据所述六维力传感器初始值参数获取末端执行工具重力、末端执行工具质心在所述六维力传感器的坐标和六维力传感器力矩初始值，具体包括：
[0024]
将六维力传感器xyz轴力的初始值和带入公式(1)，并求平均值，得到末端执行工具重力g；
[0025]
利用公式(7)计算末端执行工具对六维力传感器xyz轴所施加的力
[0026][0027]
将末端执行工具对六维力传感器xyz轴所施加的力带入公式(8)：
[0028][0029]
其中，是第k组六维力传感器xyz测得的力矩大小，是第k组六维力传感器xyz测得的力矩大小，是六维力传感器xyz轴的初始力矩，p
x
、p
y
、p
z
是工具质心在六维力传感器坐标系下的x、y、z轴的坐标；
[0030]
得：
[0031][0032][0033]
利用最小二乘法求得末端执行工具质心在六维力传感器的xyz轴坐标p
x
、p
y
、p
z
，以及六维力传感器xyz轴分别对应的力矩初始值
[0034][0035]
优选的，求解所述六维力传感器坐标系相对于机器人基坐标系的位姿变换，具体包括：
[0036]
假设工具的重力方向一直竖直向下，且与机器人的基坐标系的z轴重合，方向相反。调整机器人的末端坐标系x轴与机器人基坐标系z轴重合，方向相反，获得六维力传感器xyz轴力和力矩测量数据为：调整机器人的末端坐标系y轴与机器人基坐标系z轴重合，方向相反，获得六维力传感器xyz轴力和力矩测量数据为：调整机器人的末端坐标系z轴与机器人基坐标系z轴重合，方向相反，获得六维力传感器xyz轴力和力矩测量数据为：
[0037]
设六维力学传感器坐标系为sensor，机器人末端坐标系为end，两者之间旋转矩阵为：
[0038][0039][0040][0041][0042]
其中，a
x
、o
x
、n
x
为机器人末端坐标系x轴在六维力传感器坐标系中的方向余弦，a
y
、o
y
、n
y
为机器人末端坐标系y轴在六维力传感器坐标系中的方向余弦，a
z
、o
z
、n
z
为机器人末端坐标系z轴在六维力传感器坐标系中的方向余弦。
[0043]
由此实现力传感器坐标系和机器人末端坐标系之间的姿态配准。
[0044]
优选的，求解外界所施加给所述六维力传感器的力和力矩，具体包括：
[0045]
末端执行工具重力g在机器人本体基坐标系中的矢量方向为：
[0046]
g
base
＝[0 0
ꢀ‑
1]
ꢀꢀꢀꢀꢀ
(17)
[0047]
末端执行工具重力g在六维力传感器坐标系的矢量方向为：
[0048]
其中：为力传感器坐标系在机器人本体基坐标系下的旋转矩阵；设则末端执行工具重力g对六维力传感器所施加的力为：
[0049][0050]
优选的，末端执行工具质心施加给六维力传感器xyz轴的力和力
矩为：
[0051][0052][0053]
经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种用于带有力传感器初始值的力牵引重力补偿方法，不拆卸力传感器末端的工具，就可以对力传感器初始值进行参数估计的方法。首先使机器人变换n种不同的姿态，获得力传感器测得的数据，根据不同姿态工具重力值为固定值构建方程组，并利用最小二乘法求解最优值，以此获得力传感器初始值参数，并根据重力分解公式求得工具重力对六维力传感器所施加的力，便可实时估计出所施加的力。将本发明应用到力传感器工具重力补偿，具有操作方便、应用普遍性等优点。
附图说明
[0054]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
[0055]
图1附图为一种应用于本发明重力补偿方法的结构示意图。
[0056]
图2附图为本发明重力补偿方法流程图。
[0057]
其中，1、机械臂；2、六维力传感器；3、末端执行工具。
具体实施方式
[0058]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0059]
本发明实施例公开了一种用于带有力传感器初始值的力牵引重力补偿方法，流程图如图2所示，包括：
[0060]
s1：获取末端执行工具不受外力且机器人处于4种不同姿态下的多组六维力传感器测量数据，此时六维力传感器所显示的数据，包括工具重力加力传感器初始值以及力矩初始值，研究目的是从多组力传感器测量数据和机器人位姿中估计出力传感器初始值和工具重力等；机器人处于4种不同姿态下的六维力传感器xyz轴力和力矩的测量数据具体数值
为：为：为：为：
[0061]
s2：根据不同姿态下的六维力传感器测量数据为固定值建立方程组，并利用最小二乘法求解最优值，求得六维力传感器初始值参数；具体的：
[0062]
从六维力传感器侧得的4组数据中选择6对不同的两组数据
[0063][0064]
ai,aj≤n,ai≠aj,令ai,aj取代k带入(1)，
[0065][0066]
其中：
[0067][0068][0069][0070][0071][0072][0073][0074][0075][0076][0077][0078][0079]
构建方程为：
[0080]
[0081]
则：
[0082][0083][0084][0085]
利用最小二乘法求解出六维力传感器xyz轴力的初始值和
[0086][0087]
求得：
[0088][0089]
s3：根据六维力传感器初始值参数获取末端执行工具重力、末端执行工具质心在六维力传感器的坐标和六维力传感器力矩初始值；具体的：
[0090]
将六维力传感器xyz轴力的初始值和带入公式(1)，并求平均值，得到末端执行工具重力g＝7.2171；
[0091]
利用公式(7)计算末端执行工具对六维力传感器所施加的力
[0092][0093]
将末端执行工具对六维力传感器xyz轴所施加的力带入公式(8)：
[0094][0095]
得：
[0096][0097][0098]
利用最小二乘法求得末端执行工具质心在六维力传感器的xyz轴坐标p
x
、p
y
、p
z
，以及六维力传感器xyz轴分别对应的力矩初始值
[0099][0100]
求得：
[0101]
s4：求解六维力传感器坐标系相对于机器人基坐标系的位姿变换；具体的：
[0102]
此处认为工具的重力方向一直竖直向下，且与机器人的基坐标系的z轴重合，方向相反，力学传感器坐标系z轴与机器人末端坐标系z轴方向相同。调整机器人的末端坐标系x轴与机器人基坐标系z轴重合，方向相反，获得力传感器的测量数据为：
[0103][0104]
则机器人末端坐标系通过绕z轴旋转与力学传感器坐标系重合；选择力学传感器坐标系为sensor，机器人末端坐标系为end，两者之间旋转矩阵为：
[0105][0106]
s5：求解外界所施加给六维力传感器的力和力矩，具体的：
[0107]
末端执行工具重力g在机器人本体基坐标系中的矢量方向为：
[0108]
g
base
＝[0 0
ꢀ‑
1]
ꢀꢀꢀꢀ
(17)
[0109]
末端执行工具重力g在六维力传感器坐标系的矢量方向为：
[0110][0111]
其中：为力传感器坐标系在机器人本体基坐标系下的旋转矩阵；设则末端执行工具重力g对六维力传感器所施加的力为：
[0112][0113]
末端执行工具质心施加给六维力传感器的力和力矩为：
[0114][0115][0116]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0117]
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。