Gravity and Inertial Compensation of Force/Torque Sensors
Force and torque measurements from a robotic F/T sensor are compensated for the effects of gravity, and optionally additionally for the effects of robot motion. The weight of an attached tool W tool , and a vector {right arrow over (r)} CG from the F/T sensor body CF origin to a center of gravity of the tool are obtained, such as from user input or by parameter identification. During a robotic operation, a rotation matrix R International CF Body CF from the F/T sensor body CF to an inertial reference frame is obtained, such as from an internal inertial measurement unit (IMU), or from forward kinematics data from the robot. The force and torque measurements resolved by the F/T sensor from transducer outputs are compensated for gravity based on the W tool and {right arrow over (r)} CG , and the instantaneous value of R International CF Body CF . For inertial compensation, the additional information is obtained, including: the mass m of the attached tool; the angular velocity {right arrow over (ω)} of the F/T sensor body CF; the angular acceleration {dot over (ω)} of the F/T sensor body CF; the linear acceleration {right arrow over (a)} of the F/T sensor body CF; and inertia tensor I defined in the F/T sensor body CF which contains all moments and products of inertia. The force and torque measurements resolved by the F/T sensor from transducer outputs are compensated for inertial effects based on m, {right arrow over (ω)}, {right arrow over (ω)}, {right arrow over (r)} CG , {right arrow over (α)}, and I.
1 . A robotic force/torque (F/T) sensor including transducers configured to generate signals in response to forces or torques applied to the sensor, comprising:
measurement circuitry configured to resolve force and torque measurements from the transducer signals, the force and torque measurements referenced to a body Coordinate Frame (CF) of the F/T sensor; and
compensation circuitry configured to
obtain the weight W tool , of an attached tool;
obtain a vector {right arrow over (r)} CG from the F/T sensor body CF origin to a center of gravity of the tool;
obtain a rotation matrix R Inertial CF Body CF from the F/T sensor body CF to an inertial reference frame; and
compensate the force and torque measurements for gravitational effects of the attached tool based on R Inertial CF Body CF , W tool , and {right arrow over (r)} CG .
2 . The F/T sensor of claim 1 wherein the compensation circuitry comprises:
an inertial measurement unit integrated with the F/T sensor and calibrated to be in the F/T sensor body CF; and
processing circuitry coupled to the inertial measurement unit in data transfer relationship, and configured to perform compensation calculations.
3 . The F/T sensor of claim 1 wherein the compensation circuitry comprises circuitry external to the F/T sensor, the external circuitry configured to:
receive forward kinematics data from a robot to which the F/T sensor is attached;
receive transducer signals from the F/T sensor; and
output gravity-compensated force and torque measurements.
4 . The F/T sensor of claim 1 wherein the compensation circuitry is configured to perform gravity compensation of force and torque measurements by continuously calculating
{right arrow over (F)} contact ={right arrow over (F)} measured −{right arrow over (F)} gravity
where each {right arrow over (F)} is a spatial vector,
F
→
"\[Rule]"
=
(
F
x
F
y
F
z
T
x
T
y
T
z
)
,
and where F is Force and T is Torque and the
subscript identifies an axis; and
where {right arrow over (F)} contact are the gravity-compensated F/T measurements;
{right arrow over (F)} measured are force and torque measurements resolved from the F/T sensor transducers; and
{right arrow over (F)} gravity are compensation terms accounting for the effects of gravity due to the attached tool.
5 . The F/T sensor of claim 4 where
F
→
"\[Rule]"
gravity
=
(
R
Inertial
CF
Body
CF
W
→
"\[Rule]"
tool
S
(
r
→
"\[Rule]"
CG
)
R
Inertial
CF
Body
CF
W
→
"\[Rule]"
tool
)
{right arrow over (W)} tool is the weight vector of the attached tool in the inertial reference frame; and
S(•) denotes a skew-symmetric matrix operation.
6 . The F/T sensor of claim 1 wherein the compensation circuitry is further configured to:
obtain a mass m of the attached tool;
obtain an angular velocity {right arrow over (ω)} of the F/T sensor body CF;
obtain an angular acceleration {dot over (ω)} of the F/T sensor body CF;
obtain a linear acceleration {right arrow over (α)} of the F/T sensor body CF;
obtain the inertia tensor I defined in the F/T sensor body CF which contains all moments and products of inertia; and
further compensate the force and torque measurements for inertial effects of movement of the robot based on m, {right arrow over (ω)}, {dot over (ω)}, {right arrow over (r)} CG , {right arrow over (α)}, and I.
7 . The F/T sensor of claim 6 wherein the compensation circuitry is configured to perform gravity and inertial compensation of force and torque measurements by continuously calculating
{right arrow over (F)} contact ={right arrow over (F)} measured −{right arrow over (F)} gravity −{right arrow over (F)} inertial .
8 . The F/T sensor of claim 7 wherein
6
F
→
"\[Rule]"
inertial
=
(
F
→
"\[Rule]"
inertial
T
→
"\[Rule]"
inertial
)
=
(
m
a
→
"\[Rule]"
+
m
(
ω
.
×
r
→
"\[Rule]"
CG
)
+
m
(
ω
→
"\[Rule]"
×
(
ω
→
"\[Rule]"
×
r
→
"\[Rule]"
CG
)
)
I
ω
.
+
ω
→
"\[Rule]"
×
(
I
ω
→
"\[Rule]"
)
+
m
(
ω
.
×
r
→
"\[Rule]"
CG
)
+
m
(
r
→
"\[Rule]"
CG
×
a
→
"\[Rule]"
)
)
.
9 . The F/T sensor of claim 7 wherein the gravitational and inertial compensation are performed separately for the F/T sensor the attached tool, according to:
{right arrow over (F)} contact ={right arrow over (F)} measured −{right arrow over (F)} gravity-tool −{right arrow over (F)} inertial-tool −{right arrow over (F)} gravity-F/T −F inerthal-F/T ,
where {right arrow over (F)} graviy-tool and {right arrow over (F)} inertial-tool are the respective gravitational and inertial compensations for the tool, and
{right arrow over (F)} graviy-tool and {right arrow over (F)} inertial-tool are the respective gravitational and inertial compensations for the F/T sensor.
10 . A method of compensating force and torque measurements resolved from transducer output signals of a robotic Force/Torque (F/T) sensor by measurement circuitry for gravitational effects due to the weight of an attached tool, the force and torque measurements referenced to a body Coordinate Frame (CF) of the F/T sensor, wherein the F/T sensor includes compensation circuitry, the method comprising:
obtaining the weight W tool of a tool attached to the F/T sensor;
obtaining a vector {right arrow over (r)} CG from the F/T sensor body CF origin to a center of gravity of the tool;
obtaining a rotation matrix R Inertial CF Body CF from the F/T sensor body CF to an inertial reference frame; and
compensating the force and torque measurements for gravitational effects of the attached tool based on R Inertial CF Body CF , W tool , and {right arrow over (r)} CG .
11 . The method of claim 10 wherein obtaining W tool and {right arrow over (r)} CG occur in a parametric identification procedure, and obtaining R Inertial CF Body CF and compensating the force and torque measurements occur during a robotic operation at a high frequency relative to change in orientation and movement of the robotic tool.
12 . The method of claim 10 wherein the compensation circuitry is external to the F/T sensor and wherein obtaining W tool and {right arrow over (r)} CG comprises determining them based on forward kinematics data from the robot.
13 . The method of claim 10 wherein the compensation circuitry comprises an inertial measurement unit (IMU) in the F/T sensor that is aligned with the F/T sensor body CF and wherein obtaining W tool and {right arrow over (r)} CG comprises determining them based on data from the IMU.
14 . The method of claim 10 wherein compensating the force and torque measurements for gravitational effects of the attached tool based on R Inertial CF Body CF , W tool , and {right arrow over (r)} CG comprises continuously calculating
{right arrow over (F)} contact ={right arrow over (F)} measured −{right arrow over (F)} gravity where
each F is a spatial vector,
F
→
"\[Rule]"
=
(
F
x
F
y
F
z
T
x
T
y
T
z
)
;
{right arrow over (F)} contact are the gravity-compensated F/T measurements;
{right arrow over (F)} measured are force and torque measurements resolved from the F/T sensor transducers;
F
→
"\[Rule]"
gravity
=
(
R
Inertial
CF
Body
CF
W
→
"\[Rule]"
tool
S
(
r
→
"\[Rule]"
CG
)
R
Inertial
CF
Body
CF
W
→
"\[Rule]"
tool
)
are compensation terms accounting for the effects of gravity due to the weight of the attached tool; and where
S (•) denotes a skew-symmetric matrix operation.
15 . The method of claim 10 wherein obtaining W tool comprises:
positioning the tool in a number j of different orientations, j=1, 2, . . . , n, and for each such orientation:
resolving the force terms of
F
→
"\[Rule]"
measured
j
=
(
F
x
j
F
y
j
F
z
j
)
from F/T sensor transducer outputs;
obtaining a unit gravity vector
g
→
"\[Rule]"
measured
j
=
(
g
x
j
g
y
j
g
z
j
)
decomposing a unit vector
directed downwardly in the inertial reference frame to its components along axes of the F/T sensor body CF; and
determining {right arrow over (W)} tool by data fitting the j matrices of {right arrow over (F)} measured and {right arrow over (g)} measured according to
{right arrow over (F)} measured j ,=W tool {right arrow over (g)} measured j .
16 . The method of claim 15 wherein determining {right arrow over (W)} tool by data fitting the j matrices of {right arrow over (F)} measured and {right arrow over (g)} measured comprises computing a least squares solution according to
W
→
"\[Rule]"
tool
=
(
(
G
T
G
)
-
1
(
G
T
F
measured
)
)
where
F
measured
=
(
F
→
"\[Rule]"
T
measured
1
F
→
"\[Rule]"
T
measured
2
⋮
F
→
"\[Rule]"
T
measured
j
)
and
G
=
(
g
→
"\[Rule]"
T
measured
1
g
→
"\[Rule]"
T
measured
2
⋮
g
→
"\[Rule]"
T
measured
j
)
.
17 . The method of claim 15 wherein obtaining {right arrow over (r)} CG comprises:
for each of the j tool orientations, resolving the torque terms of
T
→
"\[Rule]"
measured
j
=
(
T
x
j
T
y
j
T
z
j
)
from F/T sensor transducer outputs; and
determining {right arrow over (r)} CG by data fitting the j matrices of {right arrow over (T)} measured and g measured according to
{right arrow over (T)} measured ={right arrow over (r)}CG ×( W tool {right arrow over (g)} measured ,).
18 . The method of claim 17 wherein determining {right arrow over (r)} CG by data fitting the j matrices of {right arrow over (T)} measured and {right arrow over (g)} measured comprises computing a least squares solution according to
r
→
"\[Rule]"
CG
T
=
1
W
tool
(
(
A
T
A
)
-
1
(
A
T
T
measured
)
)
where
T
measured
=
(
T
→
"\[Rule]"
T
measured
1
T
→
"\[Rule]"
T
measured
2
⋮
T
→
"\[Rule]"
T
measured
j
)
and
A
=
(
A
1
A
2
⋮
A
j
)
where
each
A
j
=
-
S
(
g
→
"\[Rule]"
measured
j
)
=
(
0
g
zj
-
g
yj
-
g
zj
0
g
xj
g
yj
-
g
xj
0
)
.
19 . The method of claim 10 further comprising compensating the force and torque measurements resolved from transducer output signals for inertial effects due to movement of the robot, the method further comprising:
obtaining the mass m of the tool;
obtaining the angular velocity {right arrow over (ω)} of the F/T sensor body CF;
obtaining the angular acceleration {dot over (ω)} of the F/T sensor body CF;
obtaining the linear acceleration {right arrow over (α)} of the F/T sensor body CF;
obtaining the inertia tensor I defined in the F/T sensor body CF, I comprising a matrix containing all moments and products of inertia; and
compensating the force and torque measurements for inertial effects due to movement of the robot based on m, {right arrow over (ω)}, {dot over (ω)}, {right arrow over (r)} CG , {right arrow over (α)}, and I.
20 . The method of claim 19 wherein the compensation circuitry is external to the F/T sensor and wherein obtaining {right arrow over (ω)}, {dot over (ω)}, rc G , and {right arrow over (α)} comprises determining them based on forward kinematics data from the robot.
21 . The method of claim 19 wherein the compensation circuitry comprises an inertial measurement unit (IMU) in the F/T sensor that is aligned with the F/T sensor body CF and wherein obtaining {right arrow over (ω)}, {dot over (ω)}, rc G , and {right arrow over (α)} comprises determining them based on data from the IMU.
22 . The method of claim 19 wherein compensating the force and torque measurements for inertial effects due to movement of the robot based on m, {right arrow over (ω)}, {dot over (ω)}, {right arrow over (r)} CG , {right arrow over (α)}, and I comprises continuously calculating
F
→
"\[Rule]"
contact
=
F
→
"\[Rule]"
measured
-
F
→
"\[Rule]"
gravity
-
F
→
"\[Rule]"
inertial
where
6
F
→
"\[Rule]"
inertial
=
(
F
→
"\[Rule]"
inertial
T
→
"\[Rule]"
inertial
)
=
(
m
a
→
"\[Rule]"
+
m
(
ω
.
×
r
→
"\[Rule]"
CG
)
+
m
(
ω
→
"\[Rule]"
×
(
ω
→
"\[Rule]"
×
r
→
"\[Rule]"
CG
)
)
I
ω
.
+
ω
→
"\[Rule]"
×
(
I
ω
→
"\[Rule]"
)
+
m
(
ω
.
×
r
→
"\[Rule]"
CG
)
+
m
(
r
→
"\[Rule]"
CG
×
a
→
"\[Rule]"
)
)
23 . The method of claim 22 further comprising relating torque measurements to I according to
{right arrow over (T)} T inertial_rot {right arrow over (= {right arrow over (T)} T )} Measured −{right arrow over (T)} T Grav −{right arrow over (T)} T inertial_lin
where {right arrow over (T)} T Measured is the torque measured by the 6-axis F/T sensor 12 ;
{right arrow over (T)} T Grav is the torque due to gravity acting through the center of mass;
{right arrow over (T)} T inertial_lin is the torque created by a linear acceleration acting through the center of mass; and thus the rotational inertial compensation term {right arrow over (T)} T inertial_rot is {right arrow over (T)} T inertial_rot +I{dot over (ω)}+{right arrow over (ω)}+I{right arrow over (ω)}
24 . The method of claim 22 further comprising expressing the rotational inertial compensation term {right arrow over (T)} T inertial_rot as
{right arrow over (T)} T inertial_rot =Y,θ
where θ is the parameter vector of inertial terms defined as
θ
=
(
I
xx
I
yy
I
zz
I
xy
I
xz
I
yz
)
and
Y
i
=
[
α
x
-
ω
y
ω
z
ω
y
ω
y
(
α
y
-
ω
x
ω
z
)
(
α
z
-
ω
x
ω
z
)
(
ω
y
2
-
ω
z
2
)
ω
x
ω
z
α
y
-
ω
x
ω
z
(
α
x
+
ω
y
ω
z
)
(
ω
z
2
-
ω
x
2
)
(
α
z
-
ω
x
ω
y
)
-
ω
x
ω
y
ω
x
ω
y
α
z
(
ω
x
2
-
ω
y
2
)
(
α
x
-
ω
y
ω
z
)
(
α
y
+
ω
x
ω
z
)
]
,
solving the rotational inertial compensation term formulation {right arrow over (T)} T inertial_rot =Y i θ using a least squares formulation
θ
=
[
Y
T
Y
]
-
1
⌈
Y
T
T
inertial
_
rot
⌉
where
Y
=
(
Y
1
Y
2
⋮
Y
n
)
and
T
inertial
_
rot
=
(
T
→
"\[Rule]"
T
inertial
_
rot
1
T
→
"\[Rule]"
T
inertial
_
rot
2
⋮
T
→
"\[Rule]"
T
inertial
_
rot
n
)
.
25 . The method of claim 14 wherein the gravitational and inertial compensation are performed separately for the F/T sensor the attached tool, according to:
{right arrow over (F)} contact ={right arrow over (F)} measured −{right arrow over (F)} gravity-tool −{right arrow over (F)} inertial-tool −{right arrow over (F)} gravity-F/T −{right arrow over (F)} inertial-F/T)
where {right arrow over (F)} graviy-tool and {right arrow over (F)} inertial-tool are the respective gravitational and inertial compensations for the tool, and
{right arrow over (F)} gravity-tool and {right arrow over (F)} inertial-tool are the respective gravitational and inertial compensations for the F/T sensor.
26 . A robotic force/torque (F/T) sensor including transducers configured to generate signals in response to forces or torques applied to the sensor, comprising:
measurement circuitry configured to resolve force and torque measurements from the transducer signals, the force and torque measurements referenced to a body
Coordinate Frame (CF) of the F/T sensor; and
compensation circuitry configured to
obtain a mass m of the attached tool;
obtain an angular velocity {right arrow over (ω)} of the F/T sensor body CF;
obtain an angular acceleration {dot over (ω)} of the F/T sensor body CF;
obtain a linear acceleration {right arrow over (α)} of the F/T sensor body CF;
obtain the inertia tensor I defined in the F/T sensor body CF which contains all moments and products of inertia; and
compensate the force and torque measurements for inertial effects of movement of the robot based on m, {right arrow over (ω)}, {dot over (ω)}, {right arrow over (r)} CG , {right arrow over (α)}, and I.
27 . The F/T sensor of claim 26 wherein the compensation circuitry is configured to perform inertial compensation of force and torque measurements by continuously calculating
F
→
"\[Rule]"
contact
=
F
→
"\[Rule]"
measured
-
F
→
"\[Rule]"
inertial
,
wherein
6
F
→
"\[Rule]"
inertial
=
(
F
→
"\[Rule]"
inertial
T
→
"\[Rule]"
inertial
)
=
(
m
a
→
"\[Rule]"
+
m
(
ω
.
×
r
→
"\[Rule]"
CG
)
+
m
(
ω
→
"\[Rule]"
×
(
ω
→
"\[Rule]"
×
r
→
"\[Rule]"
CG
)
)
I
ω
.
+
ω
→
"\[Rule]"
×
(
I
ω
→
"\[Rule]"
)
+
m
(
ω
.
×
r
→
"\[Rule]"
CG
)
+
m
(
r
→
"\[Rule]"
CG
×
a
→
"\[Rule]"
)
)
.
28 . The F/T sensor of claim 27 wherein the compensation circuitry is further configured to:
obtain the weight W tool of an attached tool;
obtain a vector {right arrow over (r)} CG from the F/T sensor body CF origin to a center of gravity of the tool;
obtain a rotation matrix R Inertial CF Body CF from the F/T sensor body CF to an inertial reference frame; and
compensate the force and torque measurements for gravitational effects of the attached tool based on R Inertial CF Body CF , W tool , and {right arrow over (r)} CG .
29 . The F/T sensor of claim 28 wherein the compensation circuitry is configured to perform gravity and inertial compensation of force and torque measurements by continuously calculating
F
→
"\[Rule]"
contact
=
F
→
"\[Rule]"
measured
-
F
→
"\[Rule]"
gravity
-
F
→
"\[Rule]"
inertial
,
wherein
F
→
"\[Rule]"
gravity
=
(
R
Inertial
CF
Body
CF
W
→
"\[Rule]"
tool
S
(
r
→
"\[Rule]"
CG
)
R
Inertial
CF
Body
CF
W
→
"\[Rule]"
tool
)
;
and S(•) denotes a skew-symmetric matrix operation.