通常在机械臂腕部与末端夹持器之间安装六维力传感器，用于机器人在装配过程中的力反馈控制。六维力传感器能够测量三维空间中任何坐标系下的三维力 和三维力矩。 在机械臂处于静态时，机械臂腕部六维力传感器测出的力与力矩数据由三部分组成:

1. 传感器自身系统误差 

2. 传感器与末端夹持器自身重力作用

3. 末端夹持器所受外部接触力

若要得到装配轴在装配过程中所受外部接触力，需要消除六维力传感器系统自身误差以及装配轴重力作用两方面的影响。因此，我们需要通过标定与辨识算法，剔除这些干扰因素的影响，以便更精确地获取与环境的接触力信息。辨识与标定方法可分为静态标定与动态标定。https://zhuanlan.zhihu.com/p/62958434

• 静态标定中，让机器人运动到几个姿态并停止，根据稳定时力矩传感器的数值与最小二乘法来标定负载参数及传感器自身参数。这种方式忽略了运动惯性的影响。

• 动态标定：即考虑负载运动速度等对力矩传感器的影响。这种处理方式与机器人的动力学参数辨识类似，让机器人数个轴连续地运动，采集力矩传感器的数值与关节角度，完成对负载及零漂的辨识。

可能由于六维传感器比较昂贵，虽然传感器标定的理论很多论文都能找到，但是基于ROS的标定程序似乎很少，而且对传感器有一定要求，GitHub上的force_torque_tool包需要用到imu传感器测得坐标系当中三维的重力加速度，记得实验室使用的ATI gamma系列F/T sensor 似乎没有imu模块，所以暂时放弃，后续如果需要比较高精度的效果时可能还会给机器人末端安装imu，到时候再回来研究这个程序。目前疫情在家，就先自己动手写一个静态标定的简单demo，记录一下。

理论部分：

[1] 高培阳. 基于力传感器的工业机器人恒力磨抛系统研究[D].华中科技大学,2019.

[2] 董浩.轴孔装配工业机器人柔顺控制算法研究[D].河南理工大学,2018.

六维力传感器坐标系中负载重力的作用示意图：

将六维力传感器三个力分量的零点值记为， 三个力矩分量的零点值记为 六维力传感器的坐标系 为空间直角坐标系，有三个坐标轴，传感器与末端夹持器重力为 ，传感器与末端夹持器质心centroid在六维力传感器坐标系中的坐标为 ，重力G 在 三个坐标轴方向的作用分力 分 别 ，重力G 对三个坐标轴的作用力矩分别为 。 根据力与力矩的关系可以得到：

将六维力传感器直接测量得到的三个方向的力分量记为， 三个方向的力矩分量记为,假设标定的时候没有外部作用力作用在末端夹持器上，则力传感器所测得的力和力矩有负载重力影响及零点组成，则有 

 联立得：

其中均为常数(这些值实际情况应该不是常数，毕竟力和力矩传感器数据波动还是很厉害的，这里静态标定就不考虑那么多了)，令

即：

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

则矩阵的最小二乘解为 ,根据多维数据，可以得到负载重心centriod在六维力传感器坐标系中的坐标以及常数的值。矩阵的最小二乘解可以参考这个：https://blog.csdn.net/Sunshine_in_Moon/article/details/45797343

参考文献中考虑到机器人安装过程中和基座之间有一定倾角，同样也会使传感器的测量值与实际值存在一定偏差，目前实验在仿真阶段，就不考虑安装倾角问题（如需考虑，请参考文献）。记世界坐标系为 ，安装的基坐标系，则由基坐标系(或世界坐标系，因为没有考虑倾角)到六维力传感器坐标系的姿态变换矩阵为，在ROS中可以通过 moveit! 编程接口获取。重力在世界坐标系 中的可以表示为

,通过坐标变换，得到重力在六维力传感器坐标系 中的向量为：。

则 ：

同理，取 N 个不同的姿态 得到 N 组六维力传感器数据， ，其中，矩阵式和最小二乘式如下：

则矩阵的最小二乘解为 , 根据多维数据，可以得到传感器的力零点值已经重力的大小,将已经求出的负载重心坐标及常数 ， 带入得：

由此可以得到传感器的零点、负载的重量以及负载重心的坐标已经全部得出。

 测试代码部分：