Gravity and Inertial Compensation of Force/Torque Sensors

Abstract

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.sub.tool, and a vector {right arrow over (r)}.sub.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.sub.International CF.sup.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.sub.tool and {right arrow over (r)}.sub.CG, and the instantaneous value of R.sub.International CF.sup.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)}.sub.CG, {right arrow over (α)}, and I.

Claims

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.sub.tool, of an attached tool; obtain a vector {right arrow over (r)}.sub.CG from the F/T sensor body CF origin to a center of gravity of the tool; obtain a rotation matrix R.sub.Inertial CF.sup.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.sub.Inertial CF.sup.Body CF, W.sub.tool, and {right arrow over (r)}.sub.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)}.sub.contact={right arrow over (F)}.sub.measured−{right arrow over (F)}.sub.gravity where each {right arrow over (F)} is a spatial vector, $\stackrel{.fwdarw.}{F}=\left(\begin{array}{c}{F}_x\\ F_y\\ F_z\\ T_x\\ T_y\\ T_z\end{array}\right),$ and where F is Force and T is Torque and the subscript identifies an axis; and where {right arrow over (F)}.sub.contact are the gravity-compensated F/T measurements; {right arrow over (F)}.sub.measured are force and torque measurements resolved from the F/T sensor transducers; and {right arrow over (F)}.sub.gravity are compensation terms accounting for the effects of gravity due to the attached tool.

5. The F/T sensor of claim 4 where ${\stackrel{.fwdarw.}{F}}_{\mathrm{gravity}}=\left(\begin{array}{cc}{R}_{\mathrm{Inertial}\mathrm{CF}}^{\mathrm{Body}\mathrm{CF}}& {\stackrel{.fwdarw.}{W}}_{\mathrm{tool}}\\ S\left({\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)R_{\mathrm{Inertial}\mathrm{CF}}^{\mathrm{Body}\mathrm{CF}}& {\stackrel{.fwdarw.}{W}}_{\mathrm{tool}}\end{array}\right)$ {right arrow over (W)}.sub.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)}.sub.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)}.sub.contact={right arrow over (F)}.sub.measured−{right arrow over (F)}.sub.gravity−{right arrow over (F)}.sub.inertial.

8. The F/T sensor of claim 7 wherein ${ }^6{\stackrel{.fwdarw.}{F}}_{\mathrm{inertial}}=\left(\begin{array}{c}{\stackrel{.fwdarw.}{F}}_{\mathrm{inertial}}\\ {\stackrel{.fwdarw.}{T}}_{\mathrm{inertial}}\end{array}\right)=\left(\begin{array}{c}m\stackrel{.fwdarw.}{a}+m\left(\stackrel{.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)+m\left(\stackrel{.fwdarw.}{\omega }\times \left(\stackrel{.fwdarw.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)\right)\\ I\stackrel{.}{\omega }+\stackrel{.fwdarw.}{\omega }\times \left(I\stackrel{.fwdarw.}{\omega }\right)+m\left(\stackrel{.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)+m\left({\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\times \stackrel{.fwdarw.}{a}\right)\end{array}\right).$

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)}.sub.contact={right arrow over (F)}.sub.measured−{right arrow over (F)}.sub.gravity-tool−{right arrow over (F)}.sub.inertial-tool−{right arrow over (F)}.sub.gravity-F/T−F.sub.inerthal-F/T, where {right arrow over (F)}.sub.graviy-tool and {right arrow over (F)}.sub.inertial-tool are the respective gravitational and inertial compensations for the tool, and {right arrow over (F)}.sub.graviy-tool and {right arrow over (F)}.sub.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.sub.tool of a tool attached to the F/T sensor; obtaining a vector {right arrow over (r)}.sub.CG from the F/T sensor body CF origin to a center of gravity of the tool; obtaining a rotation matrix R.sub.Inertial CF.sup.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.sub.Inertial CF.sup.Body CF, W.sub.tool, and {right arrow over (r)}.sub.CG.

11. The method of claim 10 wherein obtaining W.sub.tool and {right arrow over (r)}.sub.CG occur in a parametric identification procedure, and obtaining R.sub.Inertial CF.sup.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.sub.tool and {right arrow over (r)}.sub.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.sub.tool and {right arrow over (r)}.sub.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.sub.Inertial CF.sup.Body CF, W.sub.tool, and {right arrow over (r)}.sub.CG comprises continuously calculating
{right arrow over (F)}.sub.contact={right arrow over (F)}.sub.measured−{right arrow over (F)}.sub.gravity where each F is a spatial vector, $\stackrel{.fwdarw.}{F}=\left(\begin{array}{c}{F}_x\\ F_y\\ F_z\\ T_x\\ T_y\\ T_z\end{array}\right);$ {right arrow over (F)}.sub.contact are the gravity-compensated F/T measurements; {right arrow over (F)}.sub.measured are force and torque measurements resolved from the F/T sensor transducers; ${\stackrel{.fwdarw.}{F}}_{\mathrm{gravity}}=\left(\begin{array}{cc}{R}_{\mathrm{Inertial}\mathrm{CF}}^{\mathrm{Body}\mathrm{CF}}& {\stackrel{.fwdarw.}{W}}_{\mathrm{tool}}\\ S\left({\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)R_{\mathrm{Inertial}\mathrm{CF}}^{\mathrm{Body}\mathrm{CF}}& {\stackrel{.fwdarw.}{W}}_{\mathrm{tool}}\end{array}\right)$ 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.sub.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 ${\stackrel{.fwdarw.}{F}}_{{\mathrm{measured}}_j}=\left(\begin{array}{c}{F}_{x_j}\\ F_{y_j}\\ F_{z_j}\end{array}\right)$ from F/T sensor transducer outputs; obtaining a unit gravity vector ${\stackrel{.fwdarw.}{g}}_{{\mathrm{measured}}_j}=\left(\begin{array}{c}{g}_{x_j}\\ g_{y_j}\\ g_{z_j}\end{array}\right)$ 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)}.sub.tool by data fitting the j matrices of {right arrow over (F)}.sub.measured and {right arrow over (g)}.sub.measured according to
{right arrow over (F)}.sub.measured.sub.j,=W.sub.tool{right arrow over (g)}.sub.measured.sub.j.

16. The method of claim 15 wherein determining {right arrow over (W)}.sub.tool by data fitting the j matrices of {right arrow over (F)}.sub.measured and {right arrow over (g)}.sub.measured comprises computing a least squares solution according to ${\stackrel{.fwdarw.}{W}}_{\mathrm{tool}}=\left({\left(G^{T}G\right)}^{-1}\left(G^T{F}_{\mathrm{measured}}\right)\right)$ $\mathrm{where}{F}_{\mathrm{measured}}=\left(\begin{array}{c}{\stackrel{.fwdarw.T}{F}}_{{\mathrm{measured}}_1}\\ {\stackrel{.fwdarw.T}{F}}_{{\mathrm{measured}}_2}\\ .Math.\\ {\stackrel{.fwdarw.T}{F}}_{{\mathrm{measured}}_j}\end{array}\right)\mathrm{and}G=\left(\begin{array}{c}{\stackrel{.fwdarw.T}{g}}_{{\mathrm{measured}}_1}\\ {\stackrel{.fwdarw.T}{g}}_{{\mathrm{measured}}_2}\\ .Math.\\ {\stackrel{.fwdarw.T}{g}}_{{\mathrm{measured}}_j}\end{array}\right).$

17. The method of claim 15 wherein obtaining {right arrow over (r)}.sub.CG comprises: for each of the j tool orientations, resolving the torque terms of ${\stackrel{.fwdarw.}{T}}_{{\mathrm{measured}}_j}=\left(\begin{array}{c}{T}_{x_j}\\ T_{y_j}\\ T_{z_j}\end{array}\right)$ from F/T sensor transducer outputs; and determining {right arrow over (r)}.sub.CG by data fitting the j matrices of {right arrow over (T)}.sub.measured and g.sub.measured according to
{right arrow over (T)}.sub.measured={right arrow over (r)}CG×(W.sub.tool{right arrow over (g)}.sub.measured,).

18. The method of claim 17 wherein determining {right arrow over (r)}.sub.CG by data fitting the j matrices of {right arrow over (T)}.sub.measured and {right arrow over (g)}.sub.measured comprises computing a least squares solution according to ${{\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}}^T=\frac{1}{W_{\mathrm{tool}}}\left({\left(A^{T}A\right)}^{-1}\left(A^T{T}_{\mathrm{measured}}\right)\right)\mathrm{where}$ $T_{\mathrm{measured}}=\left(\begin{array}{c}{\stackrel{.fwdarw.T}{T}}_{{\mathrm{measured}}_1}\\ {\stackrel{.fwdarw.T}{T}}_{{\mathrm{measured}}_2}\\ .Math.\\ {\stackrel{.fwdarw.T}{T}}_{{\mathrm{measured}}_j}\end{array}\right)\mathrm{and}A=\left(\begin{array}{c}{A}_1\\ A_2\\ .Math.\\ A_j\end{array}\right)\mathrm{where}\mathrm{each}$ $A_j=-S\left({\stackrel{.fwdarw.}{g}}_{{\mathrm{measured}}_j}\right)=\left(\begin{array}{ccc}0& g_{\mathrm{zj}}& -g_{\mathrm{yj}}\\ -g_{\mathrm{zj}}& 0& g_{\mathrm{xj}}\\ g_{\mathrm{yj}}& -g_{\mathrm{xj}}& 0\end{array}\right).$

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)}.sub.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.sub.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.sub.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)}.sub.CG, {right arrow over (α)}, and I comprises continuously calculating ${\stackrel{.fwdarw.}{F}}_{\mathrm{contact}}={\stackrel{.fwdarw.}{F}}_{\mathrm{measured}}-{\stackrel{.fwdarw.}{F}}_{\mathrm{gravity}}-{\stackrel{.fwdarw.}{F}}_{\mathrm{inertial}}\mathrm{where}$ ${ }^6{\stackrel{.fwdarw.}{F}}_{\mathrm{inertial}}=\left(\begin{array}{c}{\stackrel{.fwdarw.}{F}}_{\mathrm{inertial}}\\ {\stackrel{.fwdarw.}{T}}_{\mathrm{inertial}}\end{array}\right)=\left(\begin{array}{c}m\stackrel{.fwdarw.}{a}+m\left(\stackrel{.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)+m\left(\stackrel{.fwdarw.}{\omega }\times \left(\stackrel{.fwdarw.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)\right)\\ I\stackrel{.}{\omega }+\stackrel{.fwdarw.}{\omega }\times \left(I\stackrel{.fwdarw.}{\omega }\right)+m\left(\stackrel{.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)+m\left({\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\times \stackrel{.fwdarw.}{a}\right)\end{array}\right)$

23. The method of claim 22 further comprising relating torque measurements to I according to
{right arrow over (T)}.sup.T.sub.inertial_rot{right arrow over (={right arrow over (T)}.sup.T)}.sub.Measured−{right arrow over (T)}.sup.T.sub.Grav−{right arrow over (T)}.sup.T.sub.inertial_lin where {right arrow over (T)}.sup.T.sub.Measured is the torque measured by the 6-axis F/T sensor 12; {right arrow over (T)}.sup.T.sub.Grav is the torque due to gravity acting through the center of mass; {right arrow over (T)}.sup.T.sub.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)}.sup.T.sub.inertial_rot is {right arrow over (T)}.sup.T.sub.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)}.sup.T.sub.inertial_rot as
{right arrow over (T)}.sup.T.sub.inertial_rot=Y,θ where θ is the parameter vector of inertial terms defined as $\theta =\left(\begin{array}{c}{I}_{\mathrm{xx}}\\ I_{\mathrm{yy}}\\ I_{\mathrm{zz}}\\ I_{\mathrm{xy}}\\ I_{\mathrm{xz}}\\ I_{\mathrm{yz}}\end{array}\right)$ $\mathrm{and}$ $Y_i=\left[\begin{array}{cccccc}{\alpha }_x& -{\omega }_y{\omega }_z& {\omega }_y{\omega }_y& \left({\alpha }_y-{\omega }_x{\omega }_z\right)& \left({\alpha }_z-{\omega }_x{\omega }_z\right)& \left({\omega }_y^2-{\omega }_z^2\right)\\ {\omega }_x{\omega }_z& {\alpha }_y& -{\omega }_x{\omega }_z& \left({\alpha }_x+{\omega }_y{\omega }_z\right)& \left({\omega }_z^2-{\omega }_x^2\right)& \left({\alpha }_z-{\omega }_x{\omega }_y\right)\\ -{\omega }_x{\omega }_y& {\omega }_x{\omega }_y& {\alpha }_z& \left({\omega }_x^2-{\omega }_y^2\right)& \left({\alpha }_x-{\omega }_y{\omega }_z\right)& \left({\alpha }_y+{\omega }_x{\omega }_z\right)\end{array}\right],$ solving the rotational inertial compensation term formulation {right arrow over (T)}.sup.T.sub.inertial_rot=Y.sub.iθ using a least squares formulation $\theta ={\left[Y^{T}Y\right]}^{-1}.Math.Y^T{T}_{\mathrm{inertial}\_\mathrm{rot}}.Math.$ $\mathrm{where}$ $Y=\left(\begin{array}{c}{Y}_1\\ Y_2\\ .Math.\\ Y_n\end{array}\right)$ $\mathrm{and}$ $T_{\mathrm{inertial}\_\mathrm{rot}}=\left(\begin{array}{c}{\stackrel{.fwdarw.T}{T}}_{{\mathrm{inertial}\_\mathrm{rot}}_1}\\ {\stackrel{.fwdarw.T}{T}}_{{\mathrm{inertial}\_\mathrm{rot}}_2}\\ .Math.\\ {\stackrel{.fwdarw.T}{T}}_{{\mathrm{inertial}\_\mathrm{rot}}_n}\end{array}\right).$

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)}.sub.contact={right arrow over (F)}.sub.measured−{right arrow over (F)}.sub.gravity-tool−{right arrow over (F)}.sub.inertial-tool−{right arrow over (F)}.sub.gravity-F/T−{right arrow over (F)}.sub.inertial-F/T) where {right arrow over (F)}.sub.graviy-tool and {right arrow over (F)}.sub.inertial-tool are the respective gravitational and inertial compensations for the tool, and {right arrow over (F)}.sub.gravity-tool and {right arrow over (F)}.sub.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)}.sub.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 ${\stackrel{.fwdarw.}{F}}_{\mathrm{contact}}={\stackrel{.fwdarw.}{F}}_{\mathrm{measured}}-{\stackrel{.fwdarw.}{F}}_{\mathrm{inertial}},$ $\mathrm{wherein}$ ${ }^6{\stackrel{.fwdarw.}{F}}_{\mathrm{inertial}}=\left(\begin{array}{c}{\stackrel{.fwdarw.}{F}}_{\mathrm{inertial}}\\ {\stackrel{.fwdarw.}{T}}_{\mathrm{inertial}}\end{array}\right)=\left(\begin{array}{c}m\stackrel{.fwdarw.}{a}+m\left(\stackrel{.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)+m\left(\stackrel{.fwdarw.}{\omega }\times \left(\stackrel{.fwdarw.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)\right)\\ I\stackrel{.}{\omega }+\stackrel{.fwdarw.}{\omega }\times \left(I\stackrel{.fwdarw.}{\omega }\right)+m\left(\stackrel{.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)+m\left({\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\times \stackrel{.fwdarw.}{a}\right)\end{array}\right).$

28. The F/T sensor of claim 27 wherein the compensation circuitry is further configured to: obtain the weight W.sub.tool of an attached tool; obtain a vector {right arrow over (r)}.sub.CG from the F/T sensor body CF origin to a center of gravity of the tool; obtain a rotation matrix R.sub.Inertial CF.sup.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.sub.Inertial CF.sup.Body CF, W.sub.tool, and {right arrow over (r)}.sub.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 ${\stackrel{.fwdarw.}{F}}_{\mathrm{contact}}={\stackrel{.fwdarw.}{F}}_{\mathrm{measured}}-{\stackrel{.fwdarw.}{F}}_{\mathrm{gravity}}-{\stackrel{.fwdarw.}{F}}_{\mathrm{inertial}},$ $\mathrm{wherein}$ ${\stackrel{.fwdarw.}{F}}_{\mathrm{gravity}}=\left(\begin{array}{cc}{R}_{\mathrm{Inertial}\mathrm{CF}}^{\mathrm{Body}\mathrm{CF}}& {\stackrel{.fwdarw.}{W}}_{\mathrm{tool}}\\ S\left({\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)R_{\mathrm{Inertial}\mathrm{CF}}^{\mathrm{Body}\mathrm{CF}}& {\stackrel{.fwdarw.}{W}}_{\mathrm{tool}}\end{array}\right);$ and S(•) denotes a skew-symmetric matrix operation.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0013] The present invention will now be described more fully hereinafter with reference to the accompanying drawings, in which embodiments of the invention are shown. However, this invention should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art. Like numbers refer to like elements throughout.

[0014] FIG. 1 is a diagram of a robot arm with a F/T sensor and tool attached.

[0015] FIG. 2 is a block diagram of an F/T sensor and tool attached, with centers of gravity and a body coordinate frame identified.

[0016] FIG. 3 is a perspective section exploded diagram view of an F/T sensor with an IMU.

[0017] FIG. 4 is a diagram of a system including an external electronics module using robotic forward data to obtain parameters for compensation.

[0018] FIG. 5 is a flow diagram of a method of compensating force and torque measurements resolved from transducer output signals of a F/T sensor for gravitational effects due to the weight of an attached tool.

[0019] FIG. 6 is a graph of raw and compensated F/T data.

DETAILED DESCRIPTION

[0020] For simplicity and illustrative purposes, the present invention is described by referring mainly to an exemplary embodiment thereof. In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present invention. However, it will be readily apparent to one of ordinary skill in the art that the present invention may be practiced without limitation to these specific details. In this description, well known methods and structures have not been described in detail so as not to unnecessarily obscure the present invention.

[0021] FIG. 1 depicts part of an arm of a robot 10 with a Force/Torque (F/T) sensor 12 attached. As used herein, the term robot 10 refers to both the physical robot arm, and the robot control system that controls its operation. Attached to the other side of the F/T sensor 12 is an end effector tool 20. The tool 20 may, for example, be a grinder or polishing tool that contacts and applies a predetermined force/torque vector (known in the robotic arts as a “wrench”) to a workpiece (not shown), such as for shaping the workpiece or finishing its surface(s). In general, the workpiece may have a complex shape, and the robot 10 may move the tool 20 over the workpiece surface(s) by translating and rotating along various degrees of freedom, to place the tool 20 in various orientations and spatial positions, so that it can operate on the workpiece. As the tool 20 assumes different orientations, the F/T sensor experiences differing components of the weight of the tool 20 along the axes of the sensor 12. Additionally, the robot 10 movement induces inertial forces and/or torques on the F/T sensor 12. These effects are indistinguishable, at the transducer level, from the contact forces and torques between the tool 20 and workpiece, which the F/T sensor 12 is supposed to measure. Unless they can be compensated, gravitational and inertial effects can make force control robotic operations difficult or impossible.

[0022] Dynamic Compensation

[0023] According to embodiments of the present invention, one or both of gravitational and inertial effects are dynamically compensated by an F/T sensor 12—either stand-alone, with integrated inertial measurement unit (IMU), or in conjunction with an external compensation unit, which receives data from both the F/T sensor 12 and the robot 10. The measurement compensation is performed by adding correcting factors to the F/T sensor 12 measurement outputs, where the correcting factors are based on a model of how the F/T measurement is affected by various environmental factors. Note that this disclosure does not consider temperature compensation. As mentioned above, there are known techniques to compensate for temperature drift, and temperature effects are independent of gravitational and inertial effects. The basic compensation equation is given by equation (1).

{right arrow over (F)}.sub.measured={right arrow over (F)}.sub.contact+{right arrow over (F)}.sub.gravity+{right arrow over (F)}.sub.inertial+{right arrow over (F)}.sub.noise  (1)

where {right arrow over (F)}.sub.measured is the output of the F/T sensor;

[0024] {right arrow over (F)}.sub.contact is the contact force (e.g., between a robotic tool 20 and a workpiece);

[0025] {right arrow over (F)}.sub.gravity is the force applied by tooling 20 (and sensor 12) sprung-mass weight due to gravity;

[0026] {right arrow over (F)}.sub.inertial is the force arising from linear and/or angular acceleration, or angular velocity, of the robot arm 10; and

[0027] {right arrow over (F)}.sub.noise is noise from electrical and/or mechanical sources.

[0028] Throughout this disclose, for convenience the matrix F is used to refer to both force and torque components. Where an F or T notation refers to a matrix of only force or torque terms, respectively, this will be explicitly stated, or it will be apparent to those of skill in the art from the context. Furthermore, this disclosure does not follow the strict Plücker basis, but places linear terms above rotational terms. Accordingly, each “F” term in equation (1) is a spatial vector containing the information as depicted in equation (2):

[00001]$\begin{array}{cc}{\stackrel{.fwdarw.}{F}}_{\mathrm{description}}=\left(\begin{array}{c}{F}_{x_{\mathrm{description}}}\\ F_{y_{\mathrm{description}}}\\ F_{z_{\mathrm{description}}}\\ T_{x_{\mathrm{description}}}\\ T_{y_{\mathrm{description}}}\\ T_{z_{\mathrm{description}}}\end{array}\right)& \left(2\right)\end{array}$

[0029] In force control robotic operations, the contact force is the desired measured quantity. Accordingly, equation (1) is rewritten to yield this value. Furthermore, assume low-pass filtering is effective to eliminate the {right arrow over (F)}.sub.noise term.

{right arrow over (F)}.sub.contact={right arrow over (F)}.sub.measured−{right arrow over (F)}.sub.gravity−{right arrow over (F)}.sub.inertial  (3)

[0030] Equation (3) represents full compensation—referred to as Active Dynamic Compensation, as it is continuously applied—including both gravitational and inertial compensation components. According to one embodiment, the F/T sensor 12 may be equipped or configured to perform any of three levels of compensation. First, the F/T sensor 12 may perform no compensation, also known in the art as “static compensation.” This is the default setting of an F/T sensor 12. Static compensation is characterized by equation (4):

{right arrow over (F)}.sub.contact={right arrow over (F)}.sub.measured  (4)

[0031] Second, the F/T sensor 12 may perform only gravitational compensation. This is referred to as Active Gravitational Compensation because it occurs continuously, and it is characterized by equation (5):

{right arrow over (F)}.sub.contact−{right arrow over (F)}.sub.measured−{right arrow over (F)}.sub.gravity  (5)

[0032] Finally, as discussed above, Active Dynamic Compensation includes both gravitational and inertial compensation, and is characterized by equation (3).

[0033] Gravity Compensation

[0034] FIG. 1 depicts the robotic tool 20 in one particular orientation. As used herein, the “orientation” of the tool 20 refers to its location and attitude in 3-dimensional space, in an inertial reference frame (also referred to as the gravitational reference frame). In the inertial reference frame, one axis is always vertical (e.g., z), and the other two are always horizontal. The weight of the tool 20 is modeled as a vector from the center of gravity (CG) of the tool, pointing towards the center of the Earth. In the inertial reference frame, the weight vector of the tool 20 is always parallel to the vertical axis.

[0035] The F/T sensor 12 defines an F/T sensor body Coordinate Frame (CF)—that is, mutually orthogonal x, y, and z axes with an origin in the F/T sensor 12. The F/T sensor body CF translates and rotates through space as the robot arm moves, yet remains fixed as viewed by the F/T sensor 12. The F/T sensor 12 reports forces and torques referenced to its body CF.

[0036] The magnitude of the tool 20 weight vector, and a rotation matrix relating the inertial reference frame to the F/T sensor body CF, are the only quantities needed to compensate F/T sensor 12 force measurements for the effect of the tool 20 weight. Calculating gravitational torque compensation terms requires the additional datum of the positional offset between the tool 20 center of gravity (CG) and the F/T sensor body CF origin. The complete gravitational compensation matrix is defined in equation (6):

[00002]$\begin{array}{cc}{\stackrel{.fwdarw.}{F}}_{\mathrm{gravity}}=\left(\begin{array}{cc}{R}_{\mathrm{Inertial}\mathrm{CF}}^{\mathrm{Body}\mathrm{CF}}& {\stackrel{.fwdarw.}{W}}_{\mathrm{tool}}\\ S\left({\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)R_{\mathrm{Inertial}\mathrm{CF}}^{\mathrm{Body}\mathrm{CF}}& {\stackrel{.fwdarw.}{W}}_{\mathrm{tool}}\end{array}\right)& \left(6\right)\end{array}$

where R.sub.International CF.sup.Body CF is the rotation matrix from the F/T sensor body CF to the inertial reference frame. This may be obtained from an output of an onboard inertial measurement unit (IMU) (e.g., the quaternion orientation output), or alternatively from forward kinematics of the robot 10. This value is updated every time the IMU or forward kinematics is sampled, which during use occurs at a high frequency relative to the rate of change of the tool 20 orientation (e.g., up to 400 Hz).
{right arrow over (W)}.sub.tool is the weight vector of the tool 20 in the inertial reference frame. When used without vector notation, the variable {right arrow over (W)}.sub.tool is the scalar weight of the tool 20. Accordingly, {right arrow over (W)}.sub.tool=[0 0 W.sub.tool] where z is the vertical axis in the inertial reference frame. This value may be entered by a user, or may be obtained by parameter identification.
{right arrow over (r)}.sub.CG is a vector from the F/T sensor body CF origin to the tool 20 CG. S({right arrow over (r)}.sub.CG) denotes the skew-symmetric matrix of {right arrow over (r)}.sub.CG. The {right arrow over (r)}.sub.CG value may be entered by a user, or may be obtained by parameter identification.

[0037] Parameter identification refers to a data-driven method of obtaining important physical properties of a system from available measurements. Parameter identification is also known as system identification in robotics and control system literature. Measurement of {right arrow over (W)}.sub.tool and {right arrow over (r)}.sub.CG by parameter identification is described below.

[0038] Many F/T sensors 12 exhibit muted sensitivity and/or accuracy in one or more of the six F/T measurements, due to the geometry of their construction, the distribution and orientation of strain gages, and the like. Accordingly, to obtain the most accurate measurements of {right arrow over (W)}.sub.tool, and {right arrow over (r)}.sub.CG (and hence obtain the most accurate gravitational compensation), the quantities are best measured numerous times, with the F/T sensor 12 in a different spatial orientation each time. The resulting data sets are then fitted, such as by using a least squares regression, to obtain the final {right arrow over (W)}.sub.tool, and {right arrow over (r)}.sub.CG values. As in all least squares regressions, the greatest accuracy is obtained if the samples are well spaced throughout their dynamic ranges. While it is possible to obtain usable gravitational compensation terms from only one tool 20 orientation and measurement, accuracy improves (e.g., noise is averaged out) as the number of discrete orientations sampled increases, up to about six. More than six tool 20 orientation measurements increase computational complexity, but add little improvement in accuracy.

[0039] Gravity Compensation of Force Terms

[0040] The force terms are gravity-compensated by knowing only the weight of the tool, which is referenced to the F/T sensor body CF to obtain the gravity compensation terms, for each axis. The resolved force measurements, which are referenced to the F/T sensor body CF, are then compensated for the weight of the tool 20 by subtracting the gravity compensation terms.

[0041] To determine the weight of the tool 20 by parameter identification, the robot 10 is commanded to place the tool 20 in j different spatial orientations (j=1, 2, . . . , n). The forces experienced at each of these orientations, from the weight of the attached tool 20, are resolved from F/T sensor transducers. Additionally, a unit gravity vector is obtained for each orientation. The unit gravity vector decomposes a weight vector of magnitude one—which is always directed downward along the vertical axis in the inertial reference frame—to its components along the three axes of the F/T sensor body CF at that orientation. The gravity unit vector for the

j.sup.th orientation is denoted

[00003]${\stackrel{.fwdarw.}{g}}_{{\mathrm{measured}}_j}=\left(\begin{array}{c}{g}_{x_j}\\ g_{y_j}\\ g_{z_j}\end{array}\right).$

The resolved force measurements are then described by equation (7) (where the F here denotes only force terms; no torque terms):

{right arrow over (F)}.sub.measured,={right arrow over (W)}.sub.tool{right arrow over (g)}.sub.measured,  (7)

[0042] When the j sample sets are stacked together, a least squares matrix equation is solved to minimize the square residual. It is known that if A is an m×n matrix and b is a vector in R.sup.m, the least-squares solution of Ax=b are the solutions to the matrix equation A.sup.TAx=A.sup.Tb. As applied to equation (7), this yields equation (8):

W.sub.tool=((G.sup.TG).sup.−1(G.sup.TF.sub.measured))  (8)

where F.sub.measured is the stacked gravity matrix vector

[00004]$F_{\mathrm{measured}}=\left(\begin{array}{c}{\stackrel{.fwdarw.T}{F}}_{{\mathrm{measured}}_1}\\ {\stackrel{.fwdarw.T}{F}}_{{\mathrm{measured}}_2}\\ .Math.\\ {\stackrel{.fwdarw.T}{F}}_{{\mathrm{measured}}_j}\end{array}\right)$

and
G is the stacked gravity matrix vector

[00005]$G=\left(\begin{array}{c}{\stackrel{.fwdarw.T}{g}}_{{\mathrm{measured}}_1}\\ {\stackrel{.fwdarw.T}{g}}_{{\mathrm{measured}}_2}\\ .Math.\\ {\stackrel{.fwdarw.T}{g}}_{{\mathrm{measured}}_j}\end{array}\right).$

[0043] Note that the weight of an attached tool 20 may not be a static quantity. For example, a grinder or sander may lose abrasive as it operates. Furthermore, a robot may change tools 14 over the course of an overall force control operation (e.g., change from a grinding tool 20 to a polishing tool 20, where both contact forces are fed back to control robot 10 positioning). As another example, a robotic tool 20 may comprise a gripper that picks up an item, moves it, and places it. If force control is required throughout, different values of W.sub.tool would apply while the tool 20 holds the item, than after it places it. In one embodiment, a plurality of values of tool weight may be entered or derived through the above-described parameter identification procedure. These values may be stored in memory and recalled for use in equation (6) as required.

[0044] Gravity Compensation of Torque Terms

[0045] The vector {right arrow over (r)}.sub.CG from the F/T sensor body CF origin to the tool CG can only be measured from the torque values. The torque equation (9) follows from Newtonian physics:

{right arrow over (T)}.sub.measured,={right arrow over (r)}.sub.CGx(W.sub.tool{right arrow over (g)}.sub.measured)  (9)

[0046] This is rearranged into the Ax=b form for least squares regression using cross product identities and the skew symmetric matrix:

{right arrow over (T)}.sub.measured,=W.sub.tool({right arrow over (g)}.sub.measured,{right arrow over (r)}.sub.CGx)  (10)

[0047] Let A.sub.j=−S(g.sub.measured) where j is the measurement/orientation index and A.sub.j is:

[00006]$\begin{array}{cc}{A}_j=\left(\begin{array}{ccc}0& g_{\mathrm{zj}}& -g_{\mathrm{yj}}\\ -g_{\mathrm{zj}}& 0& g_{\mathrm{xj}}\\ g_{\mathrm{yj}}& -g_{\mathrm{xj}}& 0\end{array}\right)& \left(11\right)\end{array}$

[0048] The least squares formulation is then:

{right arrow over (T)}.sub.measured,={right arrow over (W)}.sub.toolA.sub.j{right arrow over (r)}.sub.CG  (12)

[0049] The vector from the F/T sensor body CF origin to the tool 20 CG is solved using j samples and fitting the data according to equation 13:

[00007]$\begin{array}{cc}{{\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}}^T=\frac{1}{W_{\mathrm{tool}}}\left({\left(A^{T}A\right)}^{-1}\left(A^T{T}_{\mathrm{measured}}\right)\right)& \left(13\right)\end{array}$

where

[00008]$T_{\mathrm{measured}}=\left(\begin{array}{c}{\stackrel{.fwdarw.T}{T}}_{{\mathrm{measured}}_1}\\ {\stackrel{.fwdarw.T}{T}}_{{\mathrm{measured}}_2}\\ .Math.\\ {\stackrel{.fwdarw.T}{T}}_{{\mathrm{measured}}_j}\end{array}\right)$

is the stacked matrix of torque measurements; and

[00009]$A=\left(\begin{array}{c}{A}_1\\ A_2\\ .Math.\\ A_j\end{array}\right).$

[0050] To eliminate noise and obtain a better estimate of the tool CG, in one embodiment, a plurality of values of the vector from the F/T sensor CF origin to the tool CG may be entered or derived through the above-described parameter identification procedure. These values may be stored in memory and recalled for use in equation (6) as required.

[0051] Inertial Compensation

[0052] The inertial compensation term is considerably more complex than the gravitational compensation case, as it includes inertial, Coriolis, and centripetal coupling terms. The inverse-dynamical model of the tool 20 with respect to F/T sensor body CF can be found by application of Newton-Euler mechanics. Assuming a rigid-body model of the tool 20, the following model is derived using the following classical Newton Euler assumptions: [0053] By Newton's 3.sup.rd Law, the reaction wrench measured by the F/T sensor 12 will be equal and opposite to that experienced by the tool 20. [0054] The total force on the system is equal to the time rate of change of the linear momentum. [0055] The total torque on the system is equal to the time rate of change of the angular momentum.

[0056] The total force and torque on the F/T sensor body CF, excluding gravitational terms, are given by equations (14) and (15), respectively:

{right arrow over (F)}.sub.inertail=m{right arrow over (α)}+m({dot over (ω)}×{right arrow over (r)}.sub.CG)+m({right arrow over (ω)}×({right arrow over (ω)}×{right arrow over (r)}.sub.CG))  (14)

{right arrow over (T)}.sub.inertial=I{dot over (ω)}+{right arrow over (ω)}×(I{right arrow over (ω)})+m({dot over (ω)}×{right arrow over (r)}.sub.CG)+m({right arrow over (r)}.sub.CG×{right arrow over (α)})  (15)

where m is the mass of the tool 20. This term can be obtained from the gravitational compensation parameter identification, by converting weight to mass

[00010]$\left(m=\frac{W_{\mathrm{tool}}}{9.8N/\mathrm{kg}}\right).$

{dot over (ω)} is the angular velocity of the F/T sensor body CF. This may be obtained from data output by the onboard IMU, or alternatively from forward kinematics of the robot 10.
{dot over (ω)} is the angular acceleration of the F/T sensor body CF. This may be differentiated from IMU data, or alternatively from forward kinematics of the robot 10.
{right arrow over (r)}.sub.CG is the vector from the F/T sensor body CF origin to the tool 20 CG. This value may be entered by a user, or may be obtained by parameter identification as described above for gravitational compensation.
{right arrow over (a)} is the linear acceleration of the F/T sensor body CF. An onboard IMU outputs an acceleration value, which comprises both gravitational acceleration and linear acceleration. The linear acceleration may be extracted by estimating the gravitational acceleration from absolute orientation data that is transformed into an estimated gravity vector. With this estimated gravity vector, the linear acceleration is computed as {right arrow over (α)}={right arrow over (α)}.sub.IMU−{right arrow over (g)}.sub.estimate. Alternatively, {right arrow over (α)} may be obtained from robot forward kinematics.
I is the inertia tensor defined in the F/T sensor body CF. An inertia tensor is a matrix containing all moments and products of inertia. Intuitively, an inertia tensor provides the transformation between angular velocity and angular momentum. This value may be entered by a user or may be obtained by parameter identification.

[0057] The force and torque vectors of equations (14) and (15) are obtained from measurements performed at j different tool 20 orientations, j=1, 2, . . . , n (although six is a reasonable upper limit for n, this is not a limitation of the present invention). Similar to the gravitational compensation case, these are then stacked into a spatial vector to create the final inertial compensation terms, according to equation (16) (where the superscript 6 refers to the dimension of the spatial vector when used around traditional coordinate vectors):

[00011]$\begin{array}{cc}{ }^6{\stackrel{.fwdarw.}{F}}_{\mathrm{inertial}}=\left(\begin{array}{c}{\stackrel{.fwdarw.}{F}}_{\mathrm{inertial}}\\ {\stackrel{.fwdarw.}{T}}_{\mathrm{inertial}}\end{array}\right)=\left(\begin{array}{c}m\stackrel{.fwdarw.}{a}+m\left(\stackrel{.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)+m\left(\stackrel{.fwdarw.}{\omega }\times \left(\stackrel{.fwdarw.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)\right)\\ I\stackrel{.}{\omega }+\stackrel{.fwdarw.}{\omega }\times \left(I\stackrel{.fwdarw.}{\omega }\right)+m\left(\stackrel{.}{\omega }\times {\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\right)+m\left({\stackrel{.fwdarw.}{r}}_{\mathrm{CG}}\times \stackrel{.fwdarw.}{a}\right)\end{array}\right)& \left(16\right)\end{array}$

[0058] Identification of Inertial Compensation Terms

[0059] The inertia tensor I is a required term, that is difficult to quantify without solid models containing exact materials. Accordingly, in one embodiment a parameter estimation method is used to obtain the six distinct parameters of the inertia tensor.

[0060] In order to create a relation between torque measurements and the inertia tensor, the rotational inertia terms are isolated on one side.

{right arrow over (T)}.sup.T.sub.inertial_rot={right arrow over (T)}.sup.T.sub.Measured−{right arrow over (T)}.sup.T.sub.Grav−{right arrow over (T)}.sup.T.sub.inertial_lin  (17)

[0061] where {right arrow over (T)}.sup.T.sub.Measured is the torque measured by the 6-axis F/T sensor 12;

[0062] {right arrow over (T)}.sup.T.sub.Grav is the torque due to gravity acting through the center of mass (bottom of Eq. 6);

[0063] and

[0064] T.sub.inertial_lin is the torque created by a linear acceleration acting through the center of mass (the m ({right arrow over (r)}.sub.CG×{right arrow over (α)}) term in the bottom of Eq. 16). This leaves the rotational inertia terms to be:

{right arrow over (T)}.sup.T.sub.inertial_rot=I{dot over (ω)}+{right arrow over (ω)}×I{right arrow over (ω)}  (18)

Knowing

[0065] [00012]$\begin{array}{cc}I=\left[\begin{array}{ccc}{I}_{\mathrm{xx}}& I_{\mathrm{xy}}& I_{\mathrm{xz}}\\ I_{\mathrm{yz}}& I_{\mathrm{yy}}& I_{\mathrm{yz}}\\ I_{\mathrm{zx}}& I_{\mathrm{zy}}& I_{\mathrm{zz}}\end{array}\right]& \left(19\right)\end{array}$

Let

[0066]
I.sub.xy=I.sub.yx,I.sub.xz=I.sub.zx, and I.sub.zy=I.sub.yz  (20)

such that there are now only six unique inertial parameters. If the angular velocity and angular acceleration are given by

{right arrow over (ω)}=(ω.sub.x,ω.sub.y,ω.sub.z)  (21)

and

{dot over (ω)}={right arrow over (α)}=(α.sub.x,α.sub.y,α.sub.z)  (22)

Multiplying out equation 18 allows for a purely matrix form to be found.

{right arrow over (T)}.sup.T.sub.inertial_rot.sub.i=Y.sub.iθ  (23)

where θ is the parameter vector of inertial terms defined as

[00013]$\begin{array}{cc}\theta =\left(\begin{array}{c}{I}_{\mathrm{xx}}\\ I_{\mathrm{yy}}\\ I_{\mathrm{zz}}\\ I_{\mathrm{xy}}\\ I_{\mathrm{xz}}\\ I_{\mathrm{yz}}\end{array}\right)& \left(24\right)\end{array}$$\mathrm{and}$$\begin{array}{cc}{Y}_i=\left[\begin{array}{cccccc}{\alpha }_x& -{\omega }_y{\omega }_z& {\omega }_y{\omega }_y& \left({\alpha }_y-{\omega }_x{\omega }_z\right)& \left({\alpha }_z-{\omega }_x{\omega }_z\right)& \left({\omega }_y^2-{\omega }_z^2\right)\\ {\omega }_x{\omega }_z& {\alpha }_y& -{\omega }_x{\omega }_z& \left({\alpha }_x+{\omega }_y{\omega }_z\right)& \left({\omega }_z^2-{\omega }_x^2\right)& \left({\alpha }_z-{\omega }_x{\omega }_y\right)\\ -{\omega }_x{\omega }_y& {\omega }_x{\omega }_y& {\alpha }_z& \left({\omega }_x^2-{\omega }_y^2\right)& \left({\alpha }_x-{\omega }_y{\omega }_z\right)& \left({\alpha }_y+{\omega }_x{\omega }_z\right)\end{array}\right]& \left(25\right)\end{array}$

And the subscript i refers to the sample at instant i.

[0067] Equation 23 can be solved in the least squared sense for θ, as

[00014]$\begin{array}{cc}\theta ={\left[Y^{T}Y\right]}^{-1}.Math.Y^T{T}_{\mathrm{inertial}\_\mathrm{rot}}.Math.& \left(26\right)\end{array}$$\mathrm{where}$$\begin{array}{cc}Y=\left(\begin{array}{c}{Y}_1\\ Y_2\\ .Math.\\ Y_n\end{array}\right)& \left(27\right)\end{array}$$\mathrm{and}$$\begin{array}{cc}{T}_{\mathrm{inertial}\_\mathrm{rot}}=\left(\begin{array}{c}{\stackrel{.fwdarw.T}{T}}_{{\mathrm{inertial}\_\mathrm{rot}}_1}\\ {\stackrel{.fwdarw.T}{T}}_{{\mathrm{inertial}\_\mathrm{rot}}_2}\\ .Math.\\ {\stackrel{.fwdarw.}{T}}_{{\mathrm{inertial}\_\mathrm{rot}}_n}\end{array}\right)& \left(28\right)\end{array}$

[0068] Each of the samples in equations 27 and 28 are generated from a complex trajectory of the robot, which involves a simultaneous rotation of the tooling mass in all three rotational axes. Each time instant generates a data point that can be used in the least squares estimation.

[0069] From an accuracy perspective, creating angular motions that excite a good range of the F/T sensor's torque rating will provide a better estimate of the inertia tensor; however the angular motions of the tooling could create stresses within the tooling that were not anticipated in the design of the tool. For this reason, the parameter estimation data gathering motion preferably does not exceed the speed of the specific use application.

[0070] The terms of the inertia tensor I, tool weight {right arrow over (W)}.sub.tool, and tool CG may not be static. For example these quantities may change as the tool 20 mass changes. In one embodiment, a plurality of values of the I, {right arrow over (W)}.sub.tool, and CG may be entered, or derived through the above-described parameter identification procedure. These values may be stored in memory and recalled for use in equation (6) as required.

[0071] Dual Mass Distribution

[0072] In one embodiment, the sprung mass of the F/T sensor 12 and the mass of the tool are accounted for separately. This facilitates gravitational and inertial compensation of a variety of tools to which the F/T sensor 12 may be coupled. FIG. 2 depicts an F/T sensor 12 attached to a robot arm (not shown) and a tool 20. FIG. 2 depicts the F/T sensor 12 center of gravity (CG), its body coordinate frame (CF), and the tool 20 center of gravity (CG).

[0073] In this embodiment, Eq. 3 is rewritten as

{right arrow over (F)}.sub.contact={right arrow over (F)}.sub.measured−{right arrow over (F)}.sub.gravity-tool−{right arrow over (F)}.sub.inertial-tool−{right arrow over (F)}.sub.gravity-F/T−{right arrow over (F)}.sub.inertial-F/T  (29)

where the subscripts identify the gravitational and inertial forces and torques derived separately for each of the tool 20 and the F/T sensor 12. These are computed separately, e.g., using the techniques described herein.

[0074] Orientation, Velocity, and Acceleration Sensing

[0075] Gravitational compensation relies on sensing the instantaneous orientation of the tool 20, and inertial compensation relies on sensing the instantaneous linear and angular velocity of the F/T sensor body CF. These values may be directly sensed by an integrated IMU, or obtained from forward kinematics of the robot 10.

[0076] Inertial Measurement Unit

[0077] FIG. 3 depicts an F/T sensor 12 with an IMU integrated therein, such as on a circuit board. The IMU may comprise a 9-axis sensor unit integrating an accelerometer, gyroscope, magnetic sensor, and microcontroller. The IMU is positioned at the F/T sensor body CF origin, and is calibrated to align its axes with the F/T sensor body CF (for clarity, FIG. 3 is an exploded parts diagram, depicting the as IMU as “hovering” over the CF; in an assembled F/T sensor 12 the IMU is mounted to the MAP 14 at the CF origin). The IMU then provides absolute orientation and angular velocity data at a high frequency (e.g., hundreds of Hz). A suitable IMU is the BN0080/85 9-axis SIP IMU available from CEVA Technologies, Inc. of Rockville Md., https://www.ceva-dsp.com/

[0078] The F/T sensor 12 includes measurement circuitry configured to resolve force and torque measurements from the transducer signals, as well known in the robotic arts. The measurement circuitry may, for example, include processing circuitry configured by appropriate software to calculate force and torque measurements from transducer signals according to known methods, such as those described in U.S. Pat. No. 10,067,019, assigned to the assignee of the present disclosure and incorporated herein by reference in its entirety, or the above-incorporated U.S. Pat. No. 10,422,707. The measurement circuitry may additionally perform thermal compensation of force and torque measurements, as described in the above-incorporated international patent publication WO 2018/200668. The processing circuitry may be integrated into the sensor 12 body, or may be external, such as by being connected by one or more wired or wireless communication links.

[0079] According to embodiments of the present invention, the F/T sensor 12 additionally includes compensation circuitry configured to perform gravitational and/or inertial compensation of force and torque measurements. The compensation circuitry includes the IMU described above, and processing circuitry integrated with the IMU, or connected to the IMU in data transfer relationship. The processing circuitry is configured, such as by the provision of appropriate software, to receive or retrieve data from the IMU and other sources, and perform the calculations described herein to effect gravitational and/or inertial compensation of force and torque measurements.

[0080] In some embodiments, the measurement circuitry and compensation circuitry may comprise the same processing circuitry, with the configuration for resolving resolve force and torque measurements from the transducer signals, or performing gravitational and/or inertial compensation of the force and torque measurements, being by the provision of appropriate software. The processing circuitry may comprise any computational hardware known in the art, such as a dedicated state machine implemented in hardware; programmable logic together with appropriate firmware; one or more stored-program processors or Digital Signal Processors (DSP) together with appropriate software; or any combination of the above. The processing circuitry may include peripheral circuitry, such as memory, co-processors, data and/or communication interfaces, man/machine interfaces, and the like, as known in the art.

[0081] Forward Kinematics Data Processing

[0082] In some embodiments, due to size and/or cost constraints, it is not practical to integrate an IMU into each F/T sensor 12. In these embodiments, orientation and velocity data are obtained from forward kinematics of the robot. Forward kinematics refers to the use of the kinematic equations of a robot to compute the position, velocity, or other dynamic characteristics of the tool 20 from instantaneous values of the robot parameters, such as various joint positions. FIG. 4 depicts a block diagram of a system comprising the robot 10 and F/T sensor 12, and an external electronics module 22. The electronics module 22 receives forward kinematics data from the robot 10 (i.e., the robotic control system), and transducer signals from the F/T sensor 12. One or more processors and appropriate software within the electronics module 22 resolve the transducer signals into F/T measurements and calculate and apply gravitational and optionally inertial compensation terms. The compensated F/T data, which represents only {right arrow over (F)}.sub.contact, is fed back to the robot 10 for use in force control operations. A suitable external electronics module 22 is available from ATI Industrial Automation, Inc. of Apex, NC http://www.ati-ia.com/

[0083] Methods

[0084] FIG. 5 depicts the steps in a method 100 of compensating force and torque measurements of a Force/Torque (F/T) sensor 12 attached to a robot 10 for gravitational effects due to the weight of an attached tool 20. The force and torque measurements are resolved from transducer output signals by measurement circuitry of the sensor 12. The force and torque measurements are referenced to a body Coordinate Frame (CF) of the F/T sensor 12. The F/T sensor 12 also includes compensation circuitry that performs the method 100. The compensation circuitry may be internal to the F/T sensor 12, such as an inertial measurement unit (IMU), or may be external electronics receiving forward kinematics data from the robot 10.

[0085] As indicated by the dashed line, the method 100 can be considered to comprise two separate phases, or modes. In a first phase, parameter identification, the weight W.sub.tool, of a tool attached to the F/T sensor 12 is obtained (block 102). Also in the first phase, a vector {right arrow over (r)}.sub.CG from the F/T sensor body CF origin to a center of gravity of the tool 20 is also obtained (block 104). The data W.sub.tool, and {right arrow over (r)}.sub.CG may be input by a user, or may be obtained in a parametric identification process. In the latter case, the robot 10 positions the tool 20 in one or more different orientations. The forces and torques resolved from transducers, and a unit gravity vector, are obtained at each orientation. The data W.sub.tool, and {right arrow over (r)}.sub.CG are derived by fitting these data measured from one or more orientations, such as by formulating and solving a least squares regression model. A plurality of values for W.sub.tool, and {right arrow over (r)}.sub.CG may be calculated and stored to account for changes in tool 20 weight or configuration during robotic operations.

[0086] In the second phase of the method 100, robotic operation (which may comprise a force control operation), a rotation matrix R.sub.Inertial CF.sup.Body CF from the F/T sensor body CF to an inertial reference frame is obtained (block 106) and the force and torque measurements for gravitational effects of the attached tool are compensated based on R.sub.Inertial CF.sup.Body CF, W.sub.tool, and {right arrow over (r)}.sub.CG (block 108). R.sub.Inertial CF.sup.Body CF may be obtained from the internal IMU, or the external electronics. During the second phase, an updated value of R.sub.Inertial CF.sup.Body CF is obtained, and updated gravity compensation terms are calculated and applied to the sensor 12 force and torque measurements.

[0087] The F/T sensor 12 transducers are analog devices that continuously output signals in response to mechanical stresses. The rate at which those signals are resolved into forces and torques is set by the user and may vary according to the needs of a particular robotic operation. In one embodiment force and torque measurements have a maximum update rate of about 8 kHz. The frequency of positioning data depends on the IMU or forward kinematics processing. Although some IMU devices may output data at up to 1 kHz, very high frequency outputs contain significant noise. In one embodiment, positioning data is obtained at about 400 kHz, and the most recent positioning data is used for gravity and/or inertial compensation when each force measurement is taken. Accordingly, the compensation of measured force and torque terms for gravitational and inertial effects occurs at a high frequency relative to changes in orientation of the robotic tool 20. The values for W.sub.tool and {right arrow over (r)}.sub.CG may be selected from among a plurality of stored values, in dependence on the tool selected, its estimated mass loss rate, whether it is carrying a known extra mass, and the like.

[0088] In a further refinement of the method 100 (not shown), the force and torque measurements are additionally compensated for inertial effects due to movement of the robot 10. The mass m of the tool is obtained in the first phase of the method 100. In the second phase, during robotic operation, the following properties of the F/T sensor body CF are additionally obtained: its angular velocity {right arrow over (ω)}; its angular acceleration {dot over (ω)}; and its linear acceleration {right arrow over (α)}. The terms of an inertia tensor I defined in the F/T sensor body CF are also updated. The inertia tensor I comprises a matrix containing all moments and products of inertia. In the second phase, inertial compensation terms are calculated and also applied to the sensor 12 force and torque measurements, at the same frequency as gravity compensation terms. In robotic operations where the effective mass or size of the tool may change, appropriate values for relevant parameters, such as I, {right arrow over (W)}.sub.tool, CG, or others may be selected from among a plurality of stored values.

[0089] Results

[0090] FIG. 6 depicts the dramatic improvement to force measurements that gravity compensation can achieve. The robotic operation depicted was sanding the outer surface of a pipe, with a desired contact force F.sub.contact of 5 lbs. Both the raw and gravity-compensated force components Fx and Fy are graphed (both raw and compensated Fz curves were close to zero; these are omitted from the graph for clarity). In both cases, the gravity-compensated data are swamped by the magnitude of fluctuations in the raw data—in the case of Fx, by nearly 4x! This is an example where a force control robotic operation may not even be possible using uncompensated force measurements. By compensating the Fx and Fy measurements resolved from transducer output signals to account for effects due to the weight of the tool, useful contact force information can be extracted from the data. Note that the compensated data in FIG. 6 include only gravitational compensation; applying inertial compensation would additionally remove the effects of robotic movement from the measured data.

[0091] Embodiments of the present invention present numerous advantages over the prior art. By obtaining orientation data from an IMU or forward kinematics, embodiments of the present invention provide on-going, high-speed compensation of measured forces and torques for gravitational and inertial effects. By using parameter identification methods, quantities such as tool weight and a vector from the F/T sensor CF origin to the tool center of gravity may be derived. As FIG. 6 shows, the apparatus and methods described and claimed herein dramatically improve raw force and torque measurement from a robotic F/T sensor.

[0092] Generally, all terms used herein are to be interpreted according to their ordinary meaning in the relevant technical field, unless a different meaning is clearly given and/or is implied from the context in which it is used. All references to a/an/the element, apparatus, component, means, step, etc. are to be interpreted openly as referring to at least one instance of the element, apparatus, component, means, step, etc., unless explicitly stated otherwise. The steps of any methods disclosed herein do not have to be performed in the exact order disclosed, unless a step is explicitly described as following or preceding another step and/or where it is implicit that a step must follow or precede another step. Any feature of any of the embodiments disclosed herein may be applied to any other embodiment, wherever appropriate. Likewise, any advantage of any of the embodiments may apply to any other embodiments, and vice versa. Other objectives, features and advantages of the enclosed embodiments will be apparent from the description.

[0093] The term unit may have conventional meaning in the field of electronics, electrical devices and/or electronic devices and may include, for example, electrical and/or electronic circuitry, devices, modules, processors, memories, logic solid state and/or discrete devices, computer programs or instructions for carrying out respective tasks, procedures, computations, outputs, and/or displaying functions, and so on, as such as those that are described herein. As used herein, the term “configured to” means set up, organized, adapted, or arranged to operate in a particular way; the term is synonymous with “designed to.”

[0094] The present invention may, of course, be carried out in other ways than those specifically set forth herein without departing from essential characteristics of the invention. The present embodiments are to be considered in all respects as illustrative and not restrictive, and all changes coming within the meaning and equivalency range of the appended claims are intended to be embraced therein.