CN103279037B - Based on the Robot Force accompany movement control method of six-dimensional force/torque sensor - Google Patents

Abstract

Translated fromChinese

本发明公开了基于六维力/力矩传感器的机器人力跟随运动控制方法，在利用机器人作示教等目的时，这种方法能获得很好的效果。本发明主要涉及4个重要步骤，第一个是利用较为简单的方法对力/力矩传感器的零位值进行标定，第二个是利用特定方法对力/力矩传感器在不同姿态下，由于自身和安装在传感器上面工具的重力对力/力矩传感器零位值造成影响进行补偿，然后是设定一个力/力矩传感器在相应姿态下的稳定区间，最后一个是利用力/力矩传感器的力和力矩值对机器人进行力跟随运动控制。

The invention discloses a force-following motion control method for a robot based on a six-dimensional force/torque sensor. When the robot is used for teaching purposes, the method can achieve good results. The present invention mainly involves four important steps. The first is to use a relatively simple method to calibrate the zero value of the force/torque sensor, and the second is to use a specific method to calibrate the force/torque sensor under different attitudes. The gravity of the tool installed on the sensor compensates for the influence of the zero value of the force/torque sensor, and then sets a stable range of the force/torque sensor under the corresponding attitude, and the last one is to use the force and moment value of the force/torque sensor Force-following motion control of a robot.

Description

Translated fromChinese

基于六维力/力矩传感器的机器人力跟随运动控制方法Robot force-following motion control method based on six-dimensional force/torque sensor

技术领域technical field

本发明涉及机器人控制领域，尤其涉及基于六维力/力矩传感器的机器人力跟随运动控制方法。The invention relates to the field of robot control, in particular to a robot force-following motion control method based on a six-dimensional force/torque sensor.

背景技术Background technique

在工业领域中常会用到力/力矩传感器，常用的力/力矩传感器在使用时姿态是固定的，但是若力/力矩传感器运行在姿态会改变的场合，由于传感器及在其上面安装的工具受重力影响，传感器的零位置（传感器在该姿态下没有对其施加外力时传感器的输出值）会发生改变，因此对其补偿的效果直接影响到实际的性能。为了对这种改变进行补偿，需要用到特殊的方法。在六维力/力矩传感器的标定方面，本领域技术人员提出了一种对六维力/力矩传感器在某一姿态下的测量线性度进行标定的方法，但它们在应用方面比较复杂。Force/torque sensors are often used in the industrial field. The posture of the commonly used force/torque sensors is fixed when used. Due to the influence of gravity, the zero position of the sensor (the output value of the sensor when no external force is applied to the sensor in this attitude) will change, so the effect of its compensation directly affects the actual performance. In order to compensate for this change, special methods are required. Regarding the calibration of the six-dimensional force/torque sensor, those skilled in the art have proposed a method for calibrating the measurement linearity of the six-dimensional force/torque sensor at a certain attitude, but they are relatively complicated in application.

发明内容Contents of the invention

本发明的目的在于克服上述现有技术的缺点和不足，提供基于六维力/力矩传感器的机器人力跟随运动控制方法，通过力补偿实现对机器人的力跟随运动控制。The object of the present invention is to overcome the shortcomings and deficiencies of the above-mentioned prior art, provide a robot force-following motion control method based on a six-dimensional force/torque sensor, and realize the force-following motion control of the robot through force compensation.

本发明通过下述技术方案实现：The present invention realizes through following technical scheme:

基于六维力/力矩传感器的机器人力跟随运动控制方法，包括下述步骤：A force-following motion control method for a robot based on a six-dimensional force/torque sensor comprises the following steps:

步骤（1）：通过反馈力/力矩传感器当前姿态并利用以下公式对力/力矩传感器的力和力矩数值进行补偿：Step (1): Compensate the force and moment values of the force/torque sensor by feeding back the current attitude of the force/torque sensor and using the following formula:

$\left\{\begin{array}{c}{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>xc</span>xc</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>31</span>31</span>}}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>xz</span>xz</span>}}\mathrm{<span><span>G</span>G</span>}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>yc</span>yc</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>32</span>32</span>}}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>yz</span>yz</span>}}\mathrm{<span><span>G</span>G</span>}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>zc</span>zc</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>33</span>33</span>}}\mathrm{<span><span>G</span>G</span>}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>xc</span>xc</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>yc</span>yc</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>zc</span>zc</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}\end{array}\right.$

式中，r₃₁、r₃₂、r₃₃分别从力传感器当前姿态$T=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$中获得；In the formula, r₃₁ , r₃₂ , and r₃₃ are respectively from the current attitude of the force sensor$T=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{\mathrm{twenty\; one}}& r_{\mathrm{twenty\; two}}& r_{\mathrm{twenty\; three}}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$ obtained from

步骤（2）：先以力传感器x轴朝上作为初始位置1，测得当前力传感器的数据为F_x1，F_y1，F_z1，M_x1，M_y1，M_z1；在初始位置1基础上绕z轴逆时针旋转90°得到位置2，测得当前力传感器的变量为F_x2，F_y2，F_z2，M_x2，M_y2，M_z2；在位置2基础上绕z轴逆时针旋转90°得到位置3，测得当前力传感器的变量为F_x3，F_y3，F_z3，M_x3，M_y3，M_z3；在位置3基础上绕z轴逆时针旋转90°得到位置4，测得当前力传感器的变量为F_x4，F_y4，F_z4，M_x4，M_y4，M_z4；在初始位置1基础上绕y轴逆时针旋转90°得到位置5，测得当前力传感器的变量为F_x5，F_y5，F_z5，M_x5，M_y5，M_z5；在初始位置1基础上绕y轴顺时针旋转90°得到位置6，测得当前力传感器的变量为F_x6，F_y6，F_z6，M_x6，M_y6，M_z6Step (2): First take the force sensor x-axis upward as the initial position 1, and measure the data of the current force sensor as F_x1 , F_y1 , F_z1 , M_x1 , M_y1 , M_z1 ; based on the initial position 1 Rotate 90° counterclockwise around the z-axis to obtain position 2, and measure the current force sensor variables as F_x2 , F_y2 , F_z2 , M_x2 , M_y2 , M_z2 ; rotate 90 counterclockwise around the z-axis on the basis of position 2 ° to obtain position 3, the measured variables of the current force sensor are F_x3 , F_y3 , F_z3 , M_x3 , M_y3 , M_z3 ; on the basis of position 3, rotate 90° counterclockwise around the z-axis to obtain position 4, and measure The variables of the current force sensor are F_x4 , F_y4 , F_z4 , M_x4 , M_y4 , M_z4 ; on the basis of the initial position 1, rotate 90° counterclockwise around the y-axis to obtain position 5, and the measured variable of the current force sensor is F_x5 , F_y5 , F_z5 , M_x5 , M_y5 , M_z5 ; rotate 90° clockwise around the y-axis on the basis of the initial position 1 to obtain position 6, and the measured variables of the current force sensor are F_x6 , F_y6 , F_z6 , M_x6 , M_y6 , M_z6

通过以下公式获得相应的初值：The corresponding initial value is obtained by the following formula:

$\left\{\begin{array}{c}{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\end{array}\right.$

$\left\{\begin{array}{c}{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>4</span>4</span>}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>4</span>4</span>}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>4</span>4</span>}\end{array}\right.$

算出初值后，计算x轴、y轴和z轴分别测得的重力值：After calculating the initial value, calculate the gravity values measured by the x-axis, y-axis and z-axis respectively:

$\left\{\begin{array}{c}{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>x</span>x</span>}}<span><span>=</span>=</span><span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>|</span>|</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}}<span><span>=</span>=</span><span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>|</span>|</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>z</span>z</span>}}<span><span>=</span>=</span><span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>|</span>|</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\end{array}\right.$

根据这个重力值计算出传感器各轴线性度之间的偏差关系Calculate the deviation relationship between the linearity of each axis of the sensor according to this gravity value

$\left\{\begin{array}{c}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>xz</span>xz</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>x</span>x</span>}}<span><span>/</span>/</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>z</span>z</span>}}\\ {\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>yz</span>yz</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}}<span><span>/</span>/</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>z</span>z</span>}}\end{array}\right.$

以G_z为重力基准，即G=G_zTake G_z as the gravity reference, that is, G=G_z

根据当前力/力矩传感器的姿态，利用步骤（1）中的公式算出力/力矩传感器当前的姿态的零位值F_xc、F_yc、F_zc、M_xc、M_yc、M_zc，通过以上公式，对力/力矩传感器的基本参数进行标定；According to the current attitude of the force/torque sensor, use the formula in step (1) to calculate the zero position value F_xc , F_yc , F_zc , M_xc , M_yc , M_zc of the current attitude of the force/torque sensor, through the above formula , to calibrate the basic parameters of the force/torque sensor;

步骤（3）：利用步骤（1）中的公式分别求出步骤（2）中所测得的6个位置的零位值，并与测量值进行比较，通过下面公式算出此时各个测量量的误差最大绝对值：Step (3): Use the formula in step (1) to calculate the zero value of the 6 positions measured in step (2), and compare with the measured value, and calculate the value of each measured quantity at this time by the following formula Maximum absolute value of error:

${\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>f</span>f</span>}}<span><span>=</span>=</span>\underset{\mathrm{<span><span>i</span>i</span>}<span><span>=</span>=</span>\mathrm{<span><span>1</span>1</span>}}{\overset{\mathrm{<span><span>6</span>6</span>}}{\mathrm{<span><span>max</span>max</span>}}}<span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>ni</span>ni</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>nc</span>nc</span>}}<span><span>|</span>|</span><span><span>,</span>,</span><span><span>(</span>(</span>\mathrm{<span><span>n</span>no</span>}<span><span>=</span>=</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>z</span>z</span>}<span><span>)</span>)</span>$

${\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>=</span>=</span>\underset{\mathrm{<span><span>i</span>i</span>}<span><span>=</span>=</span>\mathrm{<span><span>1</span>1</span>}}{\overset{\mathrm{<span><span>6</span>6</span>}}{\mathrm{<span><span>max</span>max</span>}}}<span><span>|</span>|</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>ni</span>ni</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>nc</span>nc</span>}}<span><span>|</span>|</span><span><span>,</span>,</span><span><span>(</span>(</span>\mathrm{<span><span>n</span>no</span>}<span><span>=</span>=</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>z</span>z</span>}<span><span>)</span>)</span>$

式中F_ni、M_ni表示传感器测量回来的量，F_nc、M_nc表示相应通道计算出来的补偿量；各通道误差的最大绝对值分别记为η_fx、η_fy、η_fz、η_mx、η_my、η_mzIn the formula, F_ni and M_ni represent the amount measured by the sensor, F_nc and M_nc represent the compensation amount calculated by the corresponding channel; the maximum absolute value of each channel error is respectively recorded as η_fx , η_fy , η_fz , η_mx , η_my , η_mz

取稳定系数ε_fx、ε_fy、ε_fz（均大于1，建议选1.5～2.0），令力/力矩传感器的当前零位稳定区间为：Take the stability coefficients ε_fx , ε_fy , ε_fz (all greater than 1, it is recommended to choose 1.5~2.0), so that the current zero stability range of the force/torque sensor is:

$\left\{\begin{array}{c}{\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>fx</span>fx</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>fx</span>fx</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>fx</span>fx</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>fy</span>fy</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>fy</span>fy</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>fy</span>fy</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>fz</span>fz</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>fz</span>fz</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>fz</span>fz</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>mx</span>mx</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>mx</span>mx</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>mx</span>mx</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>my</span>my</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>my</span>my</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>my</span>my</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>mz</span>mz</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>mz</span>mz</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>mz</span>mz</span>}}\end{array}\right.$

则力/力矩传感器的力端零位稳定区间分别为力矩端的零位稳定区间分别为[M_xc-δ_mx,M_xc+δ_mx]、[M_zc-δ_mz,M_zc+δ_mz]。Then the zero stability range of the force end of the force/torque sensor is The zero stable intervals at the torque end are [M_xc -δ_mx ,M_xc +δ_mx ], [M_zc -δ_mz ,M_zc +δ_mz ].

通过上述方法获得对力/力矩传感器当前姿态下的力和力矩数值稳定区间进行设置；Obtain and set the stable range of force and moment values under the current attitude of the force/torque sensor through the above method;

步骤（4）：根据步骤（1）、（2）和（3）设计得到力跟随的控制方法力端为：Step (4): According to the design of steps (1), (2) and (3), the force end of the force-following control method is:

${\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>f</span>f</span>}}<span><span>=</span>=</span>\left\{\begin{array}{cc}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>fc</span>fc</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ \mathrm{<span><span>0</span>0</span>}& {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span><</span><</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ {\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>fc</span>fc</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>></span>></span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\end{array}\right.$

式中F_m表示在某个力方向上施加力后的测量值，F_c表示在该方向上对力的补偿量，δ表示该方向上的稳定区间；In the formula, F_m represents the measured value after applying a force in a certain force direction, F_c represents the compensation amount for force in this direction, and δ represents the stable interval in this direction;

同理，力矩端的控制方法为：Similarly, the control method of the torque end is:

${\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>=</span>=</span>\left\{\begin{array}{cc}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>mc</span>mc</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ \mathrm{<span><span>0</span>0</span>}& {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span><</span><</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ {\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>mc</span>mc</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>></span>></span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\end{array}\right.$

式中M_m表示在某个力矩方向上施加力后的测量值，M_c表示在该方向上对力的补偿量，δ表示该方向上的稳定区间；In the formula, M_m represents the measured value after applying a force in a certain moment direction, M_c represents the compensation amount for force in this direction, and δ represents the stable range in this direction;

控制值将反馈到机器人上进行力跟随控制，并通过以下公式进行：The control value will be fed back to the robot for force-following control, and is performed by the following formula:

$\stackrel{<span><span>\&CenterDot;</span>\&CenterDot;</span>}{\mathrm{<span><span>q</span>q</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>J</span>J</span>}}^{<span><span>-</span>-</span>\mathrm{<span><span>1</span>1</span>}}\mathrm{<span><span>u</span>u</span>}$

其中J为机器人当前的雅可比矩阵，u为力和力矩的控制输入，表示为Where J is the current Jacobian matrix of the robot, u is the control input of force and torque, expressed as

$\mathrm{<span><span>u</span>u</span>}<span><span>=</span>=</span>\left[\begin{array}{c}{\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>f</span>f</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>m</span>m</span>}}\end{array}\right]<span><span>=</span>=</span>\left[\begin{array}{c}{\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>fx</span>fx</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>fy</span>fy</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>fz</span>fz</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>mx</span>mx</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>my</span>my</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>mz</span>mz</span>}}\end{array}\right]$

为机器人各关节的速度，算出各关节的速度后，利用该速度对机器人各关节进行控制，使其作力跟随运动，每当机器人姿态发生改变，都需要使用步骤（2）中的方法对传感器的零位值重新标定。 is the speed of each joint of the robot. After calculating the speed of each joint, use the speed to control each joint of the robot, so that its force follows the movement. Whenever the posture of the robot changes, it is necessary to use the method in step (2) to adjust the sensor The zero value of the recalibration.

本发明通过简单几个步骤能够得出对于力/力矩传感器来说精度较高的补偿效果；提供了由于各种测量误差及数据干扰所造成的零位变动的解决方法。The invention can obtain a high-precision compensation effect for the force/torque sensor through a few simple steps; it provides a solution to the zero position variation caused by various measurement errors and data interference.

附图说明Description of drawings

图1为本发明力控制函数表示意图。Fig. 1 is a schematic diagram of the force control function table of the present invention.

图2为本发明力矩控制函数表示意图。Fig. 2 is a schematic diagram of the torque control function table of the present invention.

图3为本发明力/力矩传感器所要测量的初始6个位置的状态（重力都朝下）变化框图，其中：Fig. 3 is a block diagram of changes in the states of the initial six positions (gravity is all facing down) to be measured by the force/torque sensor of the present invention, wherein:

表1（初始位置1）所示的位置为以力传感器的x轴朝上作为初始位置，测得当前力传感器的变量为F_x1，F_y1，F_z1，M_x1，M_y1，M_z1；The position shown in Table 1 (initial position 1) is that the x-axis of the force sensor is upward as the initial position, and the measured variables of the current force sensor are F_x1 , F_y1 , F_z1 , M_x1 , M_y1 , M_z1 ;

表2（位置2）所示的位置为表1所述位置绕z轴逆时针旋转90°所得到的位置，测得当前力传感器的变量为F_x2，F_y2，F_z2，M_x2，M_y2，M_z2；The position shown in Table 2 (position 2) is the position obtained by rotating the position described in Table 1 counterclockwise by 90° around the z-axis. The measured variables of the current force sensor are F_x2 , F_y2 , F_z2 , M_x2 , M_y2 , M_z2 ;

表3（位置3）所示的位置为表2所述位置绕z轴逆时针旋转90°所得到的位置，测得当前力传感器的变量为F_x3，F_y3，F_z3，M_x3，M_y3，M_z3；The position shown in Table 3 (position 3) is the position obtained by rotating the position described in Table 2 counterclockwise by 90° around the z-axis. The measured variables of the current force sensor are F_x3 , F_y3 , F_z3 , M_x3 , M_y3 , M_z3 ;

表4（位置4）所示的位置为表3所述位置绕z轴逆时针旋转90°所得到的位置，测得当前力传感器的变量为F_x4，F_y4，F_z4，M_x4，M_y4，M_z4；The position shown in Table 4 (position 4) is the position obtained by rotating the position described in Table 3 counterclockwise by 90° around the z-axis. The measured variables of the current force sensor are F_x4 , F_y4 , F_z4 , M_x4 , M_y4 , M_z4 ;

表5（位置5）所示的位置为表1所述位置绕y轴逆时针旋转90°所得到的位置，测得当前力传感器的变量为F_x5，F_y5，F_z5，M_x5，M_y5，M_z5；The position shown in Table 5 (position 5) is the position obtained by rotating the position described in Table 1 counterclockwise by 90° around the y-axis, and the measured variables of the current force sensor are F_x5 , F_y5 , F_z5 , M_x5 , M_y5 , M_z5 ;

表6（位置6）所示的位置为表1所述位置绕y轴顺时针旋转90°所得到的位置，测得当前力传感器的变量为F_x6，F_y6，F_z6，M_x6，M_y6，M_z6。The position shown in Table 6 (position 6) is the position obtained by rotating the position described in Table 1 clockwise by 90° around the y-axis. The measured variables of the current force sensor are F_x6 , F_y6 , F_z6 , M_x6 , M_y6 , M_z6 .

具体实施方式Detailed ways

下面结合具体实施例对本发明作进一步具体详细描述。The present invention will be described in further detail below in conjunction with specific embodiments.

实施例Example

如图1、图2、图3所示。本发明基于六维力/力矩传感器的机器人力跟随运动控制方法，其特征在于包括下述步骤：As shown in Figure 1, Figure 2, and Figure 3. The robot force-following motion control method based on the six-dimensional force/torque sensor of the present invention is characterized in that comprising the following steps:

步骤（1）：通过反馈力/力矩传感器当前姿态并利用以下公式对力/力矩传感器的力和力矩数值进行补偿：Step (1): Compensate the force and moment values of the force/torque sensor by feeding back the current attitude of the force/torque sensor and using the following formula:

$\left\{\begin{array}{c}{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>xc</span>xc</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>31</span>31</span>}}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>xz</span>xz</span>}}\mathrm{<span><span>G</span>G</span>}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>yc</span>yc</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>32</span>32</span>}}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>yz</span>yz</span>}}\mathrm{<span><span>G</span>G</span>}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>zc</span>zc</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>33</span>33</span>}}\mathrm{<span><span>G</span>G</span>}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>xc</span>xc</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>yz</span>yz</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>zc</span>zc</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}\end{array}\right.$

式中，r₃₁、r₃₂、r₃₃分别从力传感器当前姿态$T=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$中获得；In the formula, r₃₁ , r₃₂ , and r₃₃ are respectively from the current attitude of the force sensor$T=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{\mathrm{twenty\; one}}& r_{\mathrm{twenty\; two}}& r_{\mathrm{twenty\; three}}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$ obtained from

步骤（2）：先以力传感器x轴朝上作为初始位置1，测得当前力传感器的数据为F_x1，F_y1，F_z1，M_x1，M_y1，M_z1；在初始位置1基础上绕z轴逆时针旋转90°得到位置2，测得当前力传感器的变量为F_x2，F_y2，F_z2，M_x2，M_y2，M_z2；在位置2基础上绕z轴逆时针旋转90°得到位置3，测得当前力传感器的变量为F_x3，F_y3，F_z3，M_x3，M_y3，M_z3；在位置3基础上绕z轴逆时针旋转90°得到位置4，测得当前力传感器的变量为F_x4，F_y4，F_z4，M_x4，M_y4，M_z4；在初始位置1基础上绕y轴逆时针旋转90°得到位置5，测得当前力传感器的变量为F_x5，F_y5，F_z5，M_x5，M_y5，M_z5；在初始位置1基础上绕y轴顺时针旋转90°得到位置6，测得当前力传感器的变量为F_x6，F_y6，F_z6，M_x6，M_y6，M_z6Step (2): First take the force sensor x-axis upward as the initial position 1, and measure the data of the current force sensor as F_x1 , F_y1 , F_z1 , M_x1 , M_y1 , M_z1 ; based on the initial position 1 Rotate 90° counterclockwise around the z-axis to obtain position 2, and measure the current force sensor variables as F_x2 , F_y2 , F_z2 , M_x2 , M_y2 , M_z2 ; rotate 90 counterclockwise around the z-axis on the basis of position 2 ° to obtain position 3, the measured variables of the current force sensor are F_x3 , F_y3 , F_z3 , M_x3 , M_y3 , M_z3 ; on the basis of position 3, rotate 90° counterclockwise around the z-axis to obtain position 4, and measure The variables of the current force sensor are F_x4 , F_y4 , F_z4 , M_x4 , M_y4 , M_z4 ; on the basis of the initial position 1, rotate 90° counterclockwise around the y-axis to obtain position 5, and the measured variable of the current force sensor is F_x5 , F_y5 , F_z5 , M_x5 , M_y5 , M_z5 ; rotate 90° clockwise around the y-axis on the basis of the initial position 1 to obtain position 6, and the measured variables of the current force sensor are F_x6 , F_y6 , F_z6 , M_x6 , M_y6 , M_z6

通过以下公式获得相应的初值：The corresponding initial value is obtained by the following formula:

$\left\{\begin{array}{c}{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\end{array}\right.$

$\left\{\begin{array}{c}{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>4</span>4</span>}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>4</span>4</span>}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>4</span>4</span>}\end{array}\right.$

算出初值后，计算x轴、y轴和z轴分别测得的重力值：After calculating the initial value, calculate the gravity values measured by the x-axis, y-axis and z-axis respectively:

$\left\{\begin{array}{c}{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>x</span>x</span>}}<span><span>=</span>=</span><span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>|</span>|</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}}<span><span>=</span>=</span><span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>|</span>|</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>z</span>z</span>}}<span><span>=</span>=</span><span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>|</span>|</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\end{array}\right.$

根据这个重力值计算出传感器各轴线性度之间的偏差关系Calculate the deviation relationship between the linearity of each axis of the sensor according to this gravity value

$\left\{\begin{array}{c}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>xz</span>xz</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>x</span>x</span>}}<span><span>/</span>/</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>z</span>z</span>}}\\ {\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>yz</span>yz</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}}<span><span>/</span>/</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>z</span>z</span>}}\end{array}\right.$

以G_z为重力基准，即G=G_zTake G_z as the gravity reference, that is, G=G_z

根据当前力/力矩传感器的姿态，利用步骤（1）中的公式算出力/力矩传感器当前的姿态的零位值F_xc、F_yc、F_zc、M_xc、M_yc、M_zc，通过以上公式，对力/力矩传感器的基本参数进行标定；According to the current attitude of the force/torque sensor, use the formula in step (1) to calculate the zero position value F_xc , F_yc , F_zc , M_xc , M_yc , M_zc of the current attitude of the force/torque sensor, through the above formula , to calibrate the basic parameters of the force/torque sensor;

步骤（3）：利用步骤（1）中的公式分别求出步骤（2）中所测得的6个位置的零位值，并与测量值进行比较，通过下面公式算出此时各个测量量的误差最大绝对值：Step (3): Use the formula in step (1) to calculate the zero value of the 6 positions measured in step (2), and compare with the measured value, and calculate the value of each measured quantity at this time by the following formula Maximum absolute value of error:

${\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>f</span>f</span>}}<span><span>=</span>=</span>\underset{\mathrm{<span><span>i</span>i</span>}<span><span>=</span>=</span>\mathrm{<span><span>1</span>1</span>}}{\overset{\mathrm{<span><span>6</span>6</span>}}{\mathrm{<span><span>max</span>max</span>}}}<span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>ni</span>ni</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>nc</span>nc</span>}}<span><span>|</span>|</span><span><span>,</span>,</span><span><span>(</span>(</span>\mathrm{<span><span>n</span>no</span>}<span><span>=</span>=</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>z</span>z</span>}<span><span>)</span>)</span>$

${\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>=</span>=</span>\underset{\mathrm{<span><span>i</span>i</span>}<span><span>=</span>=</span>\mathrm{<span><span>1</span>1</span>}}{\overset{\mathrm{<span><span>6</span>6</span>}}{\mathrm{<span><span>max</span>max</span>}}}<span><span>|</span>|</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>ni</span>ni</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>nc</span>nc</span>}}<span><span>|</span>|</span><span><span>,</span>,</span><span><span>(</span>(</span>\mathrm{<span><span>n</span>no</span>}<span><span>=</span>=</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>z</span>z</span>}<span><span>)</span>)</span>$

式中F_ni、M_ni表示传感器测量回来的量，F_nc、M_nc表示相应通道计算出来的补偿量；各通道误差的最大绝对值分别记为η_fx、η_fy、η_fz、η_mx、η_my、η_mzIn the formula, F_ni and M_ni represent the amount measured by the sensor, F_nc and M_nc represent the compensation amount calculated by the corresponding channel; the maximum absolute value of each channel error is respectively recorded as η_fx , η_fy , η_fz , η_mx , η_my , η_mz

取稳定系数ε_fx、ε_fy、ε_fz（均大于1，建议选1.5～2.0），令力/力矩传感器的当前零位稳定区间为：Take the stability coefficients ε_fx , ε_fy , ε_fz (all greater than 1, it is recommended to choose 1.5~2.0), so that the current zero stability range of the force/torque sensor is:

$\left\{\begin{array}{c}{\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>fx</span>fx</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>fx</span>fx</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>fx</span>fx</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>fy</span>fy</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>fy</span>fy</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>fy</span>fy</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>fz</span>fz</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>fz</span>fz</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>fz</span>fz</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>mx</span>mx</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>mx</span>mx</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>mx</span>mx</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>my</span>my</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>my</span>my</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>my</span>my</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>mz</span>mz</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>mz</span>mz</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>mz</span>mz</span>}}\end{array}\right.$

则力/力矩传感器的力端零位稳定区间分别为力矩端的零位稳定区间分别为[M_xc-δ_mx,M_xc+δ_mx]、[M_zc-δ_mz,M_zc+δ_mz]。Then the zero stability range of the force end of the force/torque sensor is The zero stable intervals at the torque end are [M_xc -δ_mx ,M_xc +δ_mx ], [M_zc -δ_mz ,M_zc +δ_mz ].

通过上述方法获得对力/力矩传感器当前姿态下的力和力矩数值稳定区间进行设置；Obtain and set the stable range of force and moment values under the current attitude of the force/torque sensor through the above method;

步骤（4）：根据步骤（1）、（2）和（3）设计得到力跟随的控制方法力端为：Step (4): According to the design of steps (1), (2) and (3), the force end of the force-following control method is:

${\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>f</span>f</span>}}<span><span>=</span>=</span>\left\{\begin{array}{cc}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>fc</span>fc</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ \mathrm{<span><span>0</span>0</span>}& {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span><</span><</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ {\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>fc</span>fc</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>></span>></span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\end{array}\right.$

式中F_m表示在某个力方向上施加力后的测量值，F_c表示在该方向上对力的补偿量，δ表示该方向上的稳定区间；In the formula, F_m represents the measured value after applying a force in a certain force direction, F_c represents the compensation amount for force in this direction, and δ represents the stable interval in this direction;

同理，力矩端的控制方法为：Similarly, the control method of the torque end is:

${\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>=</span>=</span>\left\{\begin{array}{cc}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>mc</span>mc</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ \mathrm{<span><span>0</span>0</span>}& {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span><</span><</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ {\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>mc</span>mc</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>></span>></span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\end{array}\right.$

式中M_m表示在某个力矩方向上施加力后的测量值，M_c表示在该方向上对力的补偿量，δ表示该方向上的稳定区间；In the formula, M_m represents the measured value after applying a force in a certain moment direction, M_c represents the compensation amount for force in this direction, and δ represents the stable range in this direction;

控制值将反馈到机器人上进行力跟随控制，并通过以下公式进行：The control value will be fed back to the robot for force-following control, and is performed by the following formula:

$\stackrel{<span><span>\&CenterDot;</span>\&CenterDot;</span>}{\mathrm{<span><span>q</span>q</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>J</span>J</span>}}^{<span><span>-</span>-</span>\mathrm{<span><span>1</span>1</span>}}\mathrm{<span><span>u</span>u</span>}$

其中J为机器人当前的雅可比矩阵，u为力和力矩的控制输入，表示为Where J is the current Jacobian matrix of the robot, u is the control input of force and moment, expressed as

$\mathrm{<span><span>u</span>u</span>}<span><span>=</span>=</span>\left[\begin{array}{c}{\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>f</span>f</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>m</span>m</span>}}\end{array}\right]<span><span>=</span>=</span>\left[\begin{array}{c}{\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>fx</span>fx</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>fy</span>fy</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>fz</span>fz</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>mx</span>mx</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>my</span>my</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>mz</span>mz</span>}}\end{array}\right]$

为机器人各关节的速度，算出各关节的速度后，利用该速度对机器人各关节进行控制，使其作力跟随运动，每当机器人姿态发生改变，都需要使用步骤（2）中的方法对传感器的零位值重新标定。 is the speed of each joint of the robot. After calculating the speed of each joint, use the speed to control each joint of the robot, so that its force follows the movement. Whenever the posture of the robot changes, it is necessary to use the method in step (2) to adjust the sensor The zero value of the recalibration.

如上所述，便可较好地实现本发明。As described above, the present invention can be preferably carried out.

本发明的实施方式并不受上述实施例的限制，其他任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。The implementation of the present invention is not limited by the above examples, and any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be equivalent replacement methods, and are included in within the protection scope of the present invention.

Claims (1)

Translated fromChinese

1.基于六维力/力矩传感器的机器人力跟随运动控制方法，其特征在于包括下述步骤：1. The robot force following motion control method based on six-dimensional force/moment sensor, is characterized in that comprising the following steps:(1)通过反馈力/力矩传感器当前姿态并利用以下公式对力/力矩传感器的力和力矩数值进行补偿：(1) Compensate the force and moment values of the force/torque sensor by feeding back the current attitude of the force/torque sensor and using the following formula:$\left\{\begin{array}{c}{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>31</span>31</span>}}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>z</span>z</span>}}\mathrm{<span><span>G</span>G</span>}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>32</span>32</span>}}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>z</span>z</span>}}\mathrm{<span><span>G</span>G</span>}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>33</span>33</span>}}\mathrm{<span><span>G</span>G</span>}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}\end{array}\right.$式中，F_x0，F_y0，F_z0，M_x0，M_y0，M_z0分别为力/力矩传感器不受重力影响时所在的零位点；k_xz为同一作用力在力/力矩传感器X轴与Z轴上线性度的偏差，k_yz为同一作用力在力/力矩传感器Y轴与Z轴上线性度的偏差；G为力/力矩传感器的重量；r₃₁、r₃₂、r₃₃分别从力传感器当前姿态$T=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$中获得；In the formula, F_x0 , F_y0 , F_z0 , M_x0 , M_y0 , M_z0 are the zero point where the force/torque sensor is not affected by gravity; k_xz is the same force on the X-axis of the force/torque sensor The deviation from the linearity on the Z axis, k_yz is the deviation of the linearity of the same force on the Y axis and the Z axis of the force/torque sensor; G is the weight of the force/torque sensor; r₃₁ , r₃₂ , and r₃₃ are respectively from Current attitude of the force sensor$T=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{\mathrm{twenty\; one}}& r_{\mathrm{twenty\; two}}& r_{\mathrm{twenty\; three}}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$ obtained from(2)先以力传感器x轴朝上作为初始位置1，测得当前力传感器的数据为F_x1，F_y1，F_z1，M_x1，M_y1，M_z1；在初始位置1基础上绕z轴逆时针旋转90°得到位置2，测得当前力传感器的变量为F_x2，F_y2，F_z2，M_x2，M_y2，M_z2；在初始位置2基础上绕z轴逆时针旋转90°得到位置3，测得当前力传感器的变量为F_x3，F_y3，F_z3，M_x3，M_y3，M_z3；在初始位置3基础上绕z轴逆时针旋转90°得到位置4，测得当前力传感器的变量为F_x4，F_y4，F_z4，M_x4，M_y4，M_z4；在初始位置1基础上绕y轴逆时针旋转90°得到位置5，测得当前力传感器的变量为F_x5，F_y5，F_z5，M_x5，M_y5，M_z5；在初始位置1基础上绕y轴顺时针旋转90°得到位置6，测得当前力传感器的变量为F_x6，F_y6，F_z6，M_x6，M_y6，M_z6(2) First take the force sensor x-axis upward as the initial position 1, and measure the data of the current force sensor as F_x1 , F_y1 , F_z1 , M_x1 , M_y1 , M_z1 ; on the basis of the initial position 1, circle z Rotate the shaft counterclockwise by 90° to obtain position 2, and the measured variables of the current force sensor are F_x2 , F_y2 , F_z2 , M_x2 , M_y2 , M_z2 ; rotate 90° counterclockwise around the z-axis on the basis of the initial position 2 Get position 3, measure the current variable of the force sensor as F_x3 , F_y3 , F_z3 , M_x3 , M_y3 , M_z3 ; rotate 90° counterclockwise around the z-axis on the basis of the initial position 3 to get position 4, and measure The variables of the current force sensor are F_x4 , F_y4 , F_z4 , M_x4 , M_y4 , M_z4 ; on the basis of the initial position 1, rotate 90° counterclockwise around the y-axis to obtain position 5, and the measured variable of the current force sensor is F_x5 , F_y5 , F_z5 , M_x5 , M_y5 , M_z5 ; rotate 90° clockwise around the y-axis on the basis of the initial position 1 to obtain position 6, and the measured variables of the current force sensor are F_x6 , F_y6 , F_z6 , M_x6 , M_y6 , M_z6通过以下公式获得相应的初值：The corresponding initial value is obtained by the following formula:$\left\{\begin{array}{c}{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\end{array}\right.$$\left\{\begin{array}{c}{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>4</span>4</span>}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>4</span>4</span>}\\ {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>0</span>0</span>}}<span><span>=</span>=</span><span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>)</span>)</span><span><span>/</span>/</span>\mathrm{<span><span>4</span>4</span>}\end{array}\right.$算出初值后，计算x轴、y轴和z轴分别测得的重力值：After calculating the initial value, calculate the gravity values measured by the x-axis, y-axis and z-axis respectively:$\left\{\begin{array}{c}{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>x</span>x</span>}}<span><span>=</span>=</span><span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>3</span>3</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>1</span>1</span>}}<span><span>|</span>|</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}}<span><span>=</span>=</span><span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>2</span>2</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>4</span>4</span>}}<span><span>|</span>|</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\\ {\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>z</span>z</span>}}<span><span>=</span>=</span><span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>6</span>6</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>z</span>z</span>}\mathrm{<span><span>5</span>5</span>}}<span><span>|</span>|</span><span><span>/</span>/</span>\mathrm{<span><span>2</span>2</span>}\end{array}\right.$根据这个重力值计算出传感器各轴线性度之间的偏差关系Calculate the deviation relationship between the linearity of each axis of the sensor according to this gravity value$\left\{\begin{array}{c}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>x</span>x</span>}\mathrm{<span><span>z</span>z</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>x</span>x</span>}}<span><span>/</span>/</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>z</span>z</span>}}\\ {\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}\mathrm{<span><span>z</span>z</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>y</span>the\; y</span>}}<span><span>/</span>/</span>{\mathrm{<span><span>G</span>G</span>}}_{\mathrm{<span><span>z</span>z</span>}}\end{array}\right.$以G_z为重力基准，即G＝G_zTake G_z as the gravity reference, that is, G=G_z根据当前力/力矩传感器的姿态，利用步骤(1)中的公式算出力/力矩传感器当前的姿态的零位值F_xc、F_yc、F_zc、M_xc、M_yc、M_zc，通过以上公式，对力/力矩传感器的基本参数进行标定；According to the attitude of the current force/torque sensor, use the formula in step (1) to calculate the zero position value F_xc , F_yc , F_zc , M_xc , M_yc , M_zc of the current attitude of the force/torque sensor, through the above formula , to calibrate the basic parameters of the force/torque sensor;(3)利用步骤(1)中的公式分别求出步骤(2)中所测得的6个位置的零位值，并与测量值进行比较，通过下面公式算出此时各个测量量的误差最大绝对值：(3) Use the formula in step (1) to obtain the zero values of the six positions measured in step (2), and compare them with the measured values, and use the following formula to calculate the maximum error of each measurement at this time Absolute value:${\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>f</span>f</span>}}<span><span>=</span>=</span>\underset{\mathrm{<span><span>i</span>i</span>}<span><span>=</span>=</span>\mathrm{<span><span>1</span>1</span>}}{\overset{\mathrm{<span><span>6</span>6</span>}}{\mathrm{<span><span>max</span>max</span>}}}<span><span>|</span>|</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>n</span>no</span>}\mathrm{<span><span>i</span>i</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>n</span>no</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>|</span>|</span><span><span>,</span>,</span><span><span>(</span>(</span>\mathrm{<span><span>n</span>no</span>}<span><span>=</span>=</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>z</span>z</span>}<span><span>)</span>)</span>$${\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>=</span>=</span>\underset{\mathrm{<span><span>i</span>i</span>}<span><span>=</span>=</span>\mathrm{<span><span>1</span>1</span>}}{\overset{\mathrm{<span><span>6</span>6</span>}}{\mathrm{<span><span>max</span>max</span>}}}<span><span>|</span>|</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>n</span>no</span>}\mathrm{<span><span>i</span>i</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>n</span>no</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>|</span>|</span><span><span>,</span>,</span><span><span>(</span>(</span>\mathrm{<span><span>n</span>no</span>}<span><span>=</span>=</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>z</span>z</span>}<span><span>)</span>)</span>$式中F_ni、M_ni表示传感器测量的量，F_nc、M_nc表示相应通道计算出来的补偿量；各通道误差的最大绝对值分别记为η_fx、η_fy、η_fz、η_mx、η_my、η_mzIn the formula, F_ni and M_ni represent the amount measured by the sensor, F_nc and M_nc represent the compensation amount calculated by the corresponding channel; the maximum absolute value of each channel error is recorded as η_fx , η_fy , η_fz , η_mx , η_my , η_mz定义稳定系数ε_fx、ε_fy、ε_fz、ε_mx、ε_my、ε_mz，均大于1，以保证各通道误差区间大于各通道误差的最大绝对值，然后定义力/力矩传感器各通道当前零位稳定区间大小为δ_fx、δ_fy、δ_fz、δ_mx、δ_my、δ_mz，求得力/力矩传感器的当前零位稳定区间为：Define the stability coefficients ε_fx , ε_fy , ε_fz , ε_mx , ε_my , ε_mz , all greater than 1, to ensure that the error interval of each channel is greater than the maximum absolute value of each channel error, and then define the current zero of each channel of the force/torque sensor The size of the position stability interval is δ_fx , δ_fy , δ_fz , δ_mx , δ_my , δ_mz , and the current zero position stability interval of the force/torque sensor is obtained as:$\left\{\begin{array}{c}{\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>x</span>x</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>x</span>x</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>x</span>x</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>y</span>the\; y</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>y</span>the\; y</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>y</span>the\; y</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>z</span>z</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>z</span>z</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>z</span>z</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>x</span>x</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>x</span>x</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>x</span>x</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>y</span>the\; y</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>y</span>the\; y</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>y</span>the\; y</span>}}\\ {\mathrm{<span><span>\&delta;</span>\&delta;</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>z</span>z</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&eta;</span>\&eta;</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>z</span>z</span>}}{\mathrm{<span><span>\&epsiv;</span>\&epsiv;</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>z</span>z</span>}}\end{array}\right.$则力/力矩传感器的力端零位稳定区间分别为力矩端的零位稳定区间分别为[M_xc-δ_mx,M_xc+δ_mx]、[M_zc-δ_mz,M_zc+δ_mz]Then the zero stability range of the force end of the force/torque sensor is The zero stable intervals at the torque end are [M_xc -δ_mx ,M_xc +δ_mx ], [M_zc -δ_mz ,M_zc +δ_mz ]通过上述方法获得对力/力矩传感器当前姿态下的力和力矩数值稳定区间进行设置；Obtain and set the stable range of force and moment values under the current attitude of the force/torque sensor through the above method;(4)根据步骤(1)、(2)和(3)设计得到力跟随的控制方法力端为：(4) According to the steps (1), (2) and (3), the force end of the force following control method is:${\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>f</span>f</span>}}<span><span>=</span>=</span>\left\{\begin{array}{cc}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ \mathrm{<span><span>0</span>0</span>}& {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span><</span><</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ {\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>></span>></span>{\mathrm{<span><span>F</span>f</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\end{array}\right.$式中，u_f为力/力矩传感器受力时某个力方向上力的输出值，k_fc为力/力矩传感器上对应力方向测量值与实际力值之间的比例，F_m表示在某个力方向上施加力后的测量值，F_c表示在该方向上对力的补偿量，δ表示该方向上的稳定区间；In the formula, u_f is the output value of the force in a certain force direction when the force/torque sensor is stressed, k_fc is the ratio between the measured value of the force/torque sensor in the direction of the force and the actual force value, and F_m represents the force in a certain direction The measured value after applying force on a force direction,_Fc represents the compensation amount to force in this direction, and δ represents the stable interval in this direction;同理，力矩端的控制方法为：Similarly, the control method of the torque end is:${\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>=</span>=</span>\left\{\begin{array}{cc}{\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ \mathrm{<span><span>0</span>0</span>}& {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span><</span><</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span><</span><</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\\ {\mathrm{<span><span>k</span>k</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>c</span>c</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>-</span>-</span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>-</span>-</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}<span><span>)</span>)</span>& {\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>m</span>m</span>}}<span><span>></span>></span>{\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>c</span>c</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&delta;</span>\&delta;</span>}\end{array}\right.$式中，u_m为力/力矩传感器受力时某个力矩方向上力矩的输出值，k_mc为力/力矩传感器上对应力矩方向测量值与实际力矩值之间的比例，M_m表示在某个力矩方向上施加力后的测量值，M_c表示在该方向上对力的补偿量，δ表示该方向上的稳定区间；In the formula, u_m is the output value of torque in a certain torque direction when the force/torque sensor is stressed, k_mc is the ratio between the measured value of the corresponding force torque direction on the force/torque sensor and the actual torque value, and M_m represents The measured value after exerting force on a torque direction, M_c represents the compensation amount to force in this direction, and δ represents the stable interval in this direction;控制值将反馈到机器人上进行力跟随控制，并通过以下公式进行：The control value will be fed back to the robot for force-following control, and is performed by the following formula:$\stackrel{<span><span>\&CenterDot;</span>\&CenterDot;</span>}{\mathrm{<span><span>q</span>q</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>J</span>J</span>}}^{<span><span>-</span>-</span>\mathrm{<span><span>1</span>1</span>}}\mathrm{<span><span>u</span>u</span>}$其中J为机器人当前的雅可比矩阵，u为力和力矩的控制输入，表示为Where J is the current Jacobian matrix of the robot, u is the control input of force and torque, expressed as$\mathrm{<span><span>u</span>u</span>}<span><span>=</span>=</span>\left[\begin{array}{c}{\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>f</span>f</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>m</span>m</span>}}\end{array}\right]<span><span>=</span>=</span>\left[\begin{array}{c}{\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>x</span>x</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>y</span>the\; y</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>f</span>f</span>}\mathrm{<span><span>z</span>z</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>x</span>x</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>y</span>the\; y</span>}}\\ {\mathrm{<span><span>u</span>u</span>}}_{\mathrm{<span><span>m</span>m</span>}\mathrm{<span><span>z</span>z</span>}}\end{array}\right]$其中u_f表示力控制输入，由u_fx、u_fy、u_fz组成；u_m表示力矩控制输入，由u_mx、u_my、u_mz组成；为机器人各关节的速度，算出各关节的速度后，利用该速度对机器人各关节进行控制，使其作力跟随运动，每当机器人姿态发生改变，都需要使用步骤(2)中的方法对传感器的零位值重新标定。Among them, u_f represents force control input, composed of u_fx , u_fy , u_fz ; u_m represents torque control input, composed of u_mx , u_my , u_mz ; is the speed of each joint of the robot. After calculating the speed of each joint, use the speed to control each joint of the robot so that its force follows the movement. Whenever the posture of the robot changes, it is necessary to use the method in step (2) to adjust the sensor The zero value of the recalibration.