六维力传感器标定方法

相关背景

  随着机器人应用领域的扩展，出现了越来越多的接触式作业场景，如机器人打磨、易碎物品抓取、人形机器人精细化操作等。在这些应用中，需要对交互力/力矩进行较好控制，否则会导致任务失败的后果。此外，在交互力/力矩过大时可能会造成机器人系统及操作对象的损坏。
  为了对交互力/力矩进行较好控制，首先需要精确获取交互力/力矩。尽管目前存在基于关节电流、关节双编码器方式对末端交互力/力矩进行估计的方法，然而受到各种因素的影响误差较大，对力控算法的研发带来较大影响。目前较为常见的方式为在机械臂末端安装多维力传感器，并在此基础上对三个维度的力和绕三个轴的力矩进行测量。然而作为一种高度非线性的设备，力传感器的读数受各种因素的影响，如噪声、传感器零漂、自身重力等。为了进行精准力控，需要首先对无关数据进行剔除。
   尽管目前越来越多的商用协作臂推出了力控臂版本，并给出了基于示教器的标定接口，在使用中可自动剔除外力的影响，但仅能应对单台协作臂配备单个力传感器的场合。然而，随着应用的拓展，逐渐出现了需要多个力传感器的应用场合。在这种背景下，就需要自行对力传感器进行标定，同时对重力等因素影响进行剔除。此外，在特殊力控算法的研发中，需要对力/力矩信息进行特殊转换，而且需要和标定及外力剔除部分结合。在这种背景下，需要深入掌握力传感器离线标定方法。

标定策略

   力传感器标定可结合牛顿-欧拉法与优化算法进行，为了提高计算速度，本文转而以若干组特殊姿态下的力/力矩信息为基础，并基于均值加权方式获取力传感器核心参数。针对力传感器，主要受以下因素的影响:
   ★ 噪声因素;
   ★ 传感器零漂;
   ★ 负载重力;
   ★ 负载重心。
   针对噪声因素，可基于巴特沃斯低通滤波器进行处理，本文不再赘述，本文主要描述传感器零漂、负载重力、负载重心的确定方法。在此基础上，实际使用中结合力传感器读数、零漂值、负载重心、以及机械臂末端位姿，可实时获取真实接触力/力矩。

标定步骤

   力传感器标定背后涉及的理论包含机器人运动学、牛顿-欧拉法等，为了便于读者编程实现，本文省略推导过程，直接给出标定量计算公式。
   在标定前，首先假定部分变量：
   ◆ $G_T$: 负载重力;
   ◆ $\overrightarrow{p_1}={\left[\begin{array}{ccc}{p}_{xS}& p_{yS}& p_{zS}\end{array}\right]}^T$: 负载重心在力传感器中的位置;
   ◆ $\overrightarrow{f_0}={\left[\begin{array}{cccccc}{f}_{x0}& f_{y0}& f_{z0}& m_{x0}& m_{y0}& m_{z0}\end{array}\right]}^T$: 传感器零飘，6维分别为x方向力零飘、y方向力零飘、z方向力零飘、x轴力矩零漂、y轴力矩零漂、z轴力矩零漂。
  标定步骤如下所示：
  步骤一、控制力传感器x轴竖直向下，在不施加任何外力的前提下获取N组力读数与力矩读数, 如下所示：
$$\overrightarrow{f_1}={\left[\begin{array}{cccccc}{f}_{x{i}_1}& f_{y{i}_1}& f_{z{i}_1}& m_{x{i}_1}& m_{y{i}_1}& m_{z{i}_1}\end{array}\right]}^T(0\le i<N)$$
  步骤二、控制力传感器y轴竖直向下，在不施加任何外力的前提下获取N组力读数与力矩读数, 如下所示：
$$\overrightarrow{f_2}={\left[\begin{array}{cccccc}{f}_{x{i}_2}& f_{y{i}_2}& f_{z{i}_2}& m_{x{i}_2}& m_{y{i}_2}& m_{z{i}_2}\end{array}\right]}^T(0\le i<N)$$
  步骤三、控制力传感器z轴竖直向下，在不施加任何外力的前提下获取N组力读数与力矩读数, 如下所示：
$$\overrightarrow{f_3}={\left[\begin{array}{cccccc}{f}_{x{i}_3}& f_{y{i}_3}& f_{z{i}_3}& m_{x{i}_3}& m_{y{i}_3}& m_{z{i}_3}\end{array}\right]}^T(0\le i<N)$$
  步骤四、传感器零漂值基于下式计算：
$$\{\begin{array}{l}{f}_{x0}={\sum }_{i=0}^{N-1}(\frac{f_{x{i}_2}+f_{x{i}_3}}{2N})\\ f_{y0}={\sum }_{i=0}^{N-1}(\frac{f_{y{i}_1}+f_{y{i}_3}}{2N})\\ f_{z0}={\sum }_{i=0}^{N-1}(\frac{f_{z{i}_1}+f_{z{i}_2}}{2N})\\ m_{x0}={\sum }_{i=0}^{N-1}\frac{m_{x{i}_1}}{N}\\ m_{y0}={\sum }_{i=0}^{N-1}\frac{m_{y{i}_2}}{N}\\ m_{z0}={\sum }_{i=0}^{N-1}\frac{m_{z{i}_3}}{N}\end{array}$$
  步骤五、末端负载重力基于下式计算:
$$G_T=\frac{{\sum }_{i=0}^{N-1}(f_{z{i}_3}-f_{z0})}{N}$$
  步骤六、负载重心基于下式计算：
$$\{\begin{array}{l}{p}_{xS}=(\frac{{\sum }_{i=0}^{N-1}(\frac{(m_{z{i}_2}-m_{z0})-(m_{y{i}_3}-m_{y0})}{G_T})}{2N})\\ p_{yS}=(\frac{{\sum }_{i=0}^{N-1}(\frac{(m_{x{i}_3}-m_{x0})-(m_{z{i}_1}-m_{z0})}{G_T})}{2N})\\ p_{zS}=(\frac{{\sum }_{i=0}^{N-1}(\frac{(m_{y{i}_1}-m_{y0})-(m_{x{i}_2}-m_{x0})}{G_T})}{2N})\end{array}$$
  至此，通过标定方法获取了全部力传感器参数。

真实接触力计算

  假设力传感器读数滤波后读数为${\vec{f}}_{filter}$, 基坐标系在末端工具坐标系中表示的旋转部分为:
$$A=\left[\begin{array}{ccc}{n}_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\end{array}\right]$$
  由负载重心得到的反对称矩阵为:
$$B=\left[\begin{array}{ccc}0& -p_{zS}& p_{yS}\\ p_{zS}& 0& -p_{xS}\\ -p_{yS}& p_{xS}& 0\end{array}\right]$$
  则真实接触力和力矩由下式确定：
$${\vec{f}}_{filter}-\left[\begin{array}{cc}A& 0_{3\ast 3}\\ BA& A\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -G_T\\ 0\\ 0\\ 0\end{array}\right]-\left[\begin{array}{c}{f}_{x0}\\ f_{y0}\\ f_{z0}\\ m_{x0}\\ m_{y0}\\ m_{z0}\end{array}\right]$$
  至此，真实接触力/力矩在力传感器坐标系中的表示则可基于上式获取。

讨论

  力传感器标定及真实接触力/力矩确定过程对柔顺控制、精细化操作至关重要。在复杂力控算法的研发中，不仅需要前述计算过程，而且需要灵活构建利于控制算法的虚拟参考系，做到可靠转换与近似。