机器人示教过程中六维力传感器安装误差计算

曹 彤，郜 夯，刘 达

（1.北京科技大学机械工程学院，北京 100089；2.北京航空航天大学机器人研究所，北京 100089）

1 引言

随着现代机器人的不断发展，各式传感器在机器人应用领域扮演着越来越重要的角色。在现在较为流行的协作机器人和传统的工业机器人［1］的工作中，六维力/矩传感器大放异彩，起到了不可替代的作用。它大大提高了机器人的感知能力，带来了良好的人机交互体验［2］。六维力/矩传感器通常被安装在机器人末端与所用工具之间，用以实时测量工具所受的外力。但是由于工具重力、传感器自身重力，以及传感器初值的存在，当未施加外力的时候传感器上会有所显示，造成示数的偏差。其中传感器的零位初值是一个受多方面因素影响的参数，末端工具和传感器之间的不同连接方式以及连接强度会对传感器的零位初值造成影响［3］，称之为安装误差。它的存在使得传感器在未加载时不能准确置零。安装误差无法在空载的时候通过相关方法计算或者补偿，故必须在负载安装完毕的条件下进行。因此，有必要对机器人示教过程中的六维力传感器的安装误差进行精确计算。

为了降低安装误差带来的影响，国内外学者对于传感器的初值的计算也进行过相关的推导与实验，并且取得了一定的实际效果。文献［4］提出了一个计算传感器初值的基本公式，对后人的影响很大；文献［5］提出了一种通过六个特殊位置的相关信息直接计算得到传感器零位初值的方法；文献［3］通过获取大量传感器数据后利用最小二乘法等来计算传感器初值以及机器人的安装倾角。还有很多学者对这个问题根据不同的实际情况，进行了深入的研究与实验，很有借鉴价值。但是从文献中可以发现，很多研究过程中没有考虑到这个负载安装引起的误差，或者将其归到出厂前厂商进行校准和补偿的范围。还有一些将传感器自身重力对示数的影响也忽略了，这对于机器人进行精密操作可能会有严重的影响。

针对很多研究将安装误差归为出厂前调试补偿而不进行相关计算等现状，提出了一种简单又有效的安装误差的计算方法来帮助传感器置零。利用变换矩阵在不同坐标系的变换以补偿工具重力以及传感器自身重力，结合传感器实时的数值计算得到传感器安装误差，以便于传感器的置零操作。

2 六维力传感器安装误差的推导与分析

对于一个六自由度的协作机器人来说，传感器安装在机器人末端与工具之间，用于获取末端工具受力情况。为了计算出工具重力、传感器自身重力影响以及传感器初值，需要以机器人本体、六维力/矩传感器、末端工具为研究对象，研究和推导传感器初值的计算方法。以Universal-Robots公司的UR5型机器人和ATI公司的FT21729型六维力传感器为研究对象，UR5型机器人是一个六自由度的串联协作机器人，共有六个转动关节，较为符合实验要求，如图1所示。FT21729型六维力传感器是一个具有高精度、高频率特点的传感器，如图2所示。

图1 UR5机器人Fig.1 UR5 Robot

图2 ATI六维力传感器Fig.2 ATI Net Force/Torque Sensor

2.1 机器人、六维力传感器以及末端工具的建系方式

根据D-H法对机器人建立坐标系［6］，如图3所示。机器人基座设为0系｛x0，y0，z0｝，机器人末端为6系｛x6，y6，z6｝。定义各杆的参数，列出机器人坐标系对应的D-H参数表，如表1所示。机器人末端的传感器和工具的连接顺序，如图4所示。针对这种连接方式，机器人末端的传感器系和工具系的建立，如图5所示。其中，传感器系记为S｛Sx，Sy，Sz｝，工具系记为T｛Tx，Ty，Tz｝。传感器系和工具系的坐标原点分别建立在传感器和工具的重心位置，由于传感器质心的轻微偏移以及末端工具的不同形状，它们的重心会分布在机器人末端6系的z轴延长线的周围。末端工具重力大小及重心位置的求取可参考文献［7-8］等，在此不予讨论。

图3 机器人D-H坐标系Fig.3 Robot D-H Coordinate System

表1 机器人D-H参数表Tab.1 Robot D-H Parameter Table

图4 传感器和末端工具的连接方式Fig.4 Connection Mode of Sensor and End Tool

图5 传感器系和工具系的建立Fig.5 Establishment of Sensor System and Tool System

前置坐标系的变换矩阵为：

将表1数据带入公式中，可以依次获得从机器人基系到机器人末端6系的位姿变换矩阵T01（T01表示从基系到1系的坐标转化，以此类推）、T12、T23、T34、T45、T56。按照次序将各个转换矩阵相乘可以得到0系到6系的坐标转换矩阵T06：

根据传感器重心相对于传感器末端的位置，可以得到6系到传感器系的转换矩阵：

式中：Sx—传感器重心相对于机器人末端6系原点x方向上的偏移量；Sy—传感器重心相对于机器人末端6系原点y方向上的偏移量；Sz—传感器重心相对于机器人末端6系原点z方向上的偏移量。

在计算工具坐标系T相对于传感器坐标系S的转换矩阵时，由于转换矩阵的第四列是工具重心相对于传感器重心的位置，故转换矩阵中的p列为：

式中：Tx—工具重心相对于传感器末端中心x方向上的偏移量；Ty—工具重心相对于传感器末端中心y方向上的偏移量；Tz—工具重心相对于传感器末端中心z方向上的偏移量。

则传感器坐标系S到工具坐标系T的转换矩阵TST为：

根据以上转换矩阵可以得到从机器人基系到工具重心的坐标转换矩阵T0T为：

至此得到所需全部坐标转换矩阵。

2.2 工具重力补偿的数值计算

工具重力在基系坐标系下的大小为G，方向竖直向下，而它的作用点在工具系的原点。所以想要得到工具重力在工具系下的大小，需要将其转换到工具系中。根据文献［9］，两个坐标系之间广义操作力的坐标变换关系为：

由于工具的重力只产生了力的作用效果，没有产生力矩，故基系上的工具重力可以表示为［10］：

将相关数据带入式（7），可以将基系上的工具重力转化到工具坐标系，得到工具坐标系上的工具重力(T fTn)T为：

这样就得到了工具的重力在工具坐标系上的数值。因传感器测得的数值是外力在传感器系上作用的体现，故当对工具重力进行补偿时，需要将这个重力转化到传感器系上进行操作。利用公式：

可以进一步得到工具的六维力在传感器系上的数值大小，即工具重力引起的传感器数值变化，这就是所需要补偿的工具重力值。

2.3 传感器自身重力补偿的数值计算

在查阅文献的过程中，发现在一些研究过程中将传感器自身重力引起的示数变化也归结到厂商进行补偿的范围，只进行了工具重力补偿后便直接使用六维力/矩传感器。六维传感器是由上平台和下平台以及其他的一些部件构成，工作的主要原理是通过外力作用使得上下平台之间产生微小位移，再将该微小位移通过相关算法转化为相应的力和力矩。在实际应用时会发现，仅进行工具重力补偿后的传感器示数同样会随着传感器位置的不同而出现明显的变化。经分析可知是由传感器自身的重力引起上下平台的位移而产生的示数变化，故需要对其进行补偿。

根据图4建立的机器人末端坐标系和传感器坐标系，可以推知从机器人基系到传感器坐标系的坐标转换矩阵T0S：

在基系中，传感器自身重力的大小为Gs，方向竖直向下。要想获得该重力在传感器系中的六维力的大小，同样需要使用两个坐标系之间的广义操作力坐标变换式（7），在这里具体表现为：

通过这个公式可以将传感器自身重力从基系转换到传感器系上，即传感器自身重力引起的传感器数值的变换，这就是需要进行补偿的传感器重力值。

2.4 由负载安装等因素引起的传感器安装误差的计算

前面两部分讲到由于重力引起的传感器示数的变化，它们需要进行实时补偿。传感器在制造过程中，由于原理、工艺等多方面的因素，会使得传感器在未安装负载时就有示数，在出厂前厂商会对这个初值进行校准和补偿。而安装误差是在安装传感器，以及在传感器后安装负载后引起的传感器示数的变化，这一值与连接配件的结构、安装的紧固方式、安装的紧固强度等因素有很大的关系。正是这样的特点决定了它无法在未安装传感器以及末端工具时得到，也无法通过相关的工具与技术直接测量，只能通过间接的计算来得到。安装误差可能对机器人进行的精密操作造成重大影响，不能忽略。

根据上述的分析，一个出厂并投入使用的六维力/矩传感器的示数主要由以下几部分组成：传感器的安装误差、工具重力、传感器自身重力、所受外力。工具重力和传感器自身重力可以通过计算来获得；所受外力无法直接获取大小和方向，但是可以根据控制变量法相关原理将其置零，以减少计算安装误差时的因素。故可以得到传感器示数的组成公式：

式中：Ds—传感器所显示的示数（DisplɑySensor）。

DTG()

θ—工具重力引起的传感器示数（DisplɑyToolsG）。

DSG()θ—传感器自身重力引起的传感器示数（DisplɑySensorG）。

DI—传感器安装误差（DisplɑyInstɑllɑtion）。

DO(t)—所受外力（DisplɑyOutside）。

根据该公式可知，如想得到传感器的安装误差DI，则需要在同一时刻获得其余的几个量进行相关的计算。在实际使用过程中，利用机器人的位置信息根据2.2节和2.3节计算得到工具重力和传感器自身重力的补偿数值，并将所受外力置零使得DO=0。由示数的组成公式可以推知传感器安装误差DI为：

通过这样的方法计算得到的传感器安装误差DI是考虑了负载安装等因素后得到的一个综合性的计算结果。这个误差在每次传感器及工具安装完成后是一个定值，故只需要计算一次，便可在后续传感器使用过程中作为固定补偿值来使用。所以工具所受外力的值DO(t)为：

通过式（17）可以看到，通过对工具重力、传感器重力的补偿以及传感器安装误差的计算，可以准确得到机器人末端工具所受到的外力和力矩。将这个力信息及时反馈到控制系统，可以进一步实现机器人的力觉拖动示教控制功能。

3 传感器安装误差计算实验论证

为了实际检测上述方法的实际使用效果，利用现有的UR5机器人和六维力/矩传感器进行实验。实验平台，如图6所示。实验过程设计为：将机器人末端工具指向不同的方向进行传感器的置零操作，把传感器经补偿后的示数作为效果判断的标准，即经过补偿后得到的传感器示数越接近零效果越好。为了在有限的实验次数中尽可能多的测试末端工具不同方向的效果，将实验划分为五组，分别为末端工具指向右、向上、向后、向下、向左，运动方向，如图7所示。与图6所示机器人对应。在给定的五组大致方向范围内，将工具随机拖动二十次到不同的位置并进行传感器的置零操作，记录传感器未加载时的示数。最后将得到的数据进行分析，计算各组示数的平均值以及标准差。

图6 实验平台：UR5机器人和六维力/矩传感器Fig.6 Experimental Platform：UR5 Robot and Sensor

图7 实验分组的方向Fig.7 Direction of Experimental Grouping

为了进行有效的对比，将仅进行工具重力补偿的方法和其他文献中的一种比较流行的方法同时进行数据计算，并采集计算结果。将这里方法记为方法一，仅进行工具重力补偿的方法记为方法二，其他文献里的方法记为方法三，实验结果如下所示。

当工具的方向设为向右时，得到二十组数据平均值，如表2所示。从表2中可以看到，方法三在这个方向上误差较大，并没有起到置零的作用，而方法一得到的合力误差接近于零值，相较方法二降低了3.6N。力矩方面，方法三比方法二降低0.33Nm，方法一较方法二降低了0.42Nm，因此方法一的误差比方法三的误差降低21%，具有比较明显的效果。

表2 工具向右数据Tab.2 Data of the Tool in the Right Direction

将该方向的分力和分力矩，如图8所示。可以看到，方法二具有较大的波动性；方法三在个别方向上的补偿效果较好，但整体不稳定；方法一补偿后各个方向的分力和力矩均比较稳定。

图8 补偿后的均值（工具向右）Fig.8 Mean Value（right）

该方向的标准差，如图9所示。可以看到在这个方向上方法二对力和力矩有所补偿，但稳定性不够理想。方法三对力矩的控制有明显的效果，但是在该方向上对力的补偿稳定性不够好。只有方法一对力和力矩的补偿均较为稳定。当工具的方向设为向上时，得到的几何平均值，如表3所示。方法三还是没能在这个方向上起到补偿作用，而方法一的合力比方法二降低将近4N。力矩方面，方法三比方法二减低了0.39Nm，方法一较方法二降低了0.44Nm，因此方法一的误差比方法三的误差降低将近11%。此方向上的分力和分力矩图，如图10所示。方法二在力矩的控制上较差，方向间的差异较大；方法三力矩控制的较为理想，但力有个别方向不受控制，稳定性较差。只有方法一较理想，能基本维持在零值附近。该方向的标准差，如图11所示。可以看到方法二力和力矩的控制效果均有上升空间；方法三的力矩控制稳定性较好，但力的控制没能达到使用要求。方法一对于力和力矩的补偿的稳定性较高。

图9 力和力矩的标准差（工具向右）Fig.9 Standard Deviation（right）

表3 工具向上数据Tab.3 Data of the Tool in the up Direction

图10 补偿后的均值（工具向上）Fig.10 Mean Value（up）

图11 力和力矩的标准差（工具向上）Fig.11 Standard Deviation（up）

将工具的方向设为向后，经过二十组的实验计算后得到几何平均值，如表4所示。在力的几何平均值上，方法三较方法二降低6.2N，方法一比方法二降低了9.4N，综上方法一比方法三误差降低了63%，精度得到较高提升。力矩方面，相较于方法二，方法三降低了0.23Nm，方法一降低了0.35Nm，方法一比方法三的误差降低32%。在这个方向上，方法一同样具有良好的效果。该方向上的力与力矩的分量均值，如图12所示。在该方向上，方法二有较大的波动，没能达到补偿的效果；方法三优于方法二，但效果远远不及方法一。方法一能保持各个方向稳定的补偿效果。而力和力矩的标准差，如图13所示。方法三在该方向上可以对力和力矩起到一定的控制作用，但稳定性不如方法一，后者在力和力矩的控制上都有较为满意的效果。当把工具的方向设为向下，方法一不出所料的展示出良好的置零效果，如表5所示。方法二在这个方向上几乎没有起到补偿的作用，方法三较方法二减少了1.22N，而方法一几乎补偿了所有的误差量，比方法二减少了11.55N，和方法三相比误差降低了89%。力矩方面，相较于方法二，方法三减少了0.41Nm，方法一减少了0.63Nm，误差降低了34%。

表4 工具向后数据Tab.4 Data of the Tool in the Backward Direction

图12 补偿后的均值（工具向后）Fig.12 Mean Value（backward）

图13 力和力矩的标准差（工具向后）Fig.13 Standard Deviation（backward）

表5 工具向下数据Tab.5 Data of the Tool in the Down Direction

查看力和力矩的分量均值，如图14所示。可以看到方法二有较大的波纹，几乎失控；方法三稳定性提升很多；方法一依然保持较高的补偿水准。向下方向的标准差，如图15所示。从中可以看出在该方向上的方法三力的稳定性有待提高，方法一在力和力矩两个方面依然能保持较高的稳定性。工具方向向左的几何平均值，如表6所示。在力方面，方法三比方法二减少了2.96N，而方法一比方法二减少了8.64N，误差减少65%。而在力矩方面，方法三降低了0.42Nm，方法一降低了0.47Nm，误差减少了15.5%。该方向的力和力矩的分量均值，如图16所示。可以看到，方法二的结果波动极大，各个方向都有较大误差；方法三曲线较为平缓，但是远远不及方法一在各个方向上的结果稳定。

图16 补偿后的均值（工具向左）Fig.16 Mean Value（left）

表6 工具向左数据Tab.6 Data of the Tool in the Left Direction

图14 补偿后的均值（工具向下）Fig.14 Mean Value（down）

图15 力和力矩的标准差（工具向下）Fig.15 Standard Deviation（down）

该方向上的力与力矩标准差，如图17所示。从中可以看到，与前几个方向的情况相似，方法一继续保持着较高的稳定性，方法二的稳定性还是具有较大的波动，而方法三介于二者之间。

图17 力和力矩的标准差（工具向左）Fig.17 Standard Deviation（left）

根据五组数据的均值和标准差比较，方法二在只进行工具重力的补偿后，不受外力时传感器还存在着明显的示数，直接使用可能会带来较大的偏差；方法三在一定程度上可以对传感器的安装误差进行补偿，且补偿力矩的效果优于补偿力，但补偿程度受工具位置的影响较大。而方法一可以有效地帮助传感器置零，在计算传感器安装误差时效果显著。与现在广泛使用的一些方法相比，可在力的置零过程中平均减少将近70%的安装误差，力矩的置零过程中平均减少22%的安装误差。且方法一稳定性较高，不受工具方向影响。

4 结论

通过对机器人、六维力/矩传感器以及其末端工具相关数据的一系列计算，可以推导得到一种有效计算由负载安装引起的传感器安装误差的方法。在进行实验后，得到了多组随机位置的传感器的置零结果，绘制成表格与图像进行比较。通过比较不同方法间的传感器的置零效果，可以看出在不需要其他设备介入的前提下，这里提出的方法具有简便、效果显著、稳定性高的优点。实验证实了该方法的正确性与可行性，可以帮助六维力/矩传感器置零，在实际使用中获得更为准确的所受外力数值。