METHOD FOR ESTIMATING A WRENCH

Abstract

A method for estimating a wrench acting on a reference point of a robot includes the steps of: a) measuring at least one component, but not all components, of the wrench; and b) estimating non-measured components of the wrench based on a dynamical model of the robot while taking into account the measured components.

Description

FIELD

The present invention relates to a method for estimating a wrench acting on a reference point of a robot, such as its tool center point, TCP.

BACKGROUND

In robotics, a wrench is a six-dimensional vector, of which three components describe a force acting on the reference point, and three more component define a torque which is applied to said same point.

If industrial robots are to operate in a force-controlled manner, they are typically equipped with a 6 degrees of freedom (DOF) force/torque (F/T) sensor that measures both contact forces in three translational directions and contact torques about all three axes. Based on this information, the contact forces and torques can actively be controlled in one or several directions, e.g. for assembly, grinding, or polishing. While 6 DOF F/T measurement provides high quality F/T information, the sensors that are required for the purpose are expensive.

As an alternative to measuring, contact forces and torques can also be estimated based on a dynamic model of the robot. Such estimation of forces and torques requires no force and torque sensors; the only input that is required are data that define angles of the various joints of the robot, and data on motor current or any other data from which motor torque can be calculated. Since these data are needed for controlling any movement of the robot, force-controlled or not, devices that provide them are generally present in a robot system and can be used for providing input to the dynamic model at no further cost.

The quality of the estimated forces and torques depends heavily on the precision with which the dynamic model of the robot allows to predict friction in the joints of the robot. This prediction can be difficult, since there are many parameters of the robot which have an influence on friction but which may not all be known, or which cannot be measured or predicted with the desired precision.

SUMMARY

In an embodiment, the present invention provides a method for estimating a wrench acting on a reference point of a robot, comprising the steps of: a) measuring at least one component, but not all components, of the wrench; and b) estimating non-measured components of the wrench based on a dynamical model of the robot while taking into account the measured components.

BRIEF DESCRIPTION OF THE DRAWINGS

The present invention will be described in even greater detail below based on the exemplary figures. The invention is not limited to the exemplary embodiments. Other features and advantages of various embodiments of the present invention will become apparent by reading the following detailed description with reference to the attached drawings which illustrate the following:

FIG. 1 is a schematic view of a robot system to which the invention is applicable.

DETAILED DESCRIPTION

In an embodiment, the present invention provides a method for estimating a wrench which can achieve a high precision at low cost.

In an embodiment, the present invention provides a method comprising the steps of

a) measuring at least one, but not all components of the wrench, typically using a built-in sensor of the robot, and

b) estimating non-measured components of the wrench based on a dynamical model of the robot while taking the measured components into account.

The cost of implementing the method can be kept low not only because the overall number of sensors can be reduced in comparison with the 6 DOF F/T measurement approach, but also because the component(s) of the wrench that are chosen to be measured can be the one (s) which offer the best ratio of cost to efficiency.

For example, measuring a force may be preferable over measuring a torque since a precise torque measurement may require a certain distance between a force-sensitive element and an axis relative to which the torque is measured, while a force measurement does not, so that a force sensor can fit into a smaller space.

From the viewpoint of cost and efficiency, no additional sensor can improve the precision of the wrench estimation more than the first sensor does. Therefore, in a preferred embodiment, the number of measured components is 1.

Further, it is preferred that the measured component is parallel to a surface normal of a surface of the robot which extends through the reference point. In that way, a contact force between the robot and an outside object touching the surface can be measured precisely, and there is no risk of the measurement being distorted by the object slipping along the surface. Alternatively, it can be parallel to a direction in which the reference point advances when processing a workpiece, so that during processing, pressure exercised on the workpiece can be measured precisely.

There are various approaches by which the measured component(s) can be taken account of

One of these is to estimate the non-measured components by a maximum-likelihood estimation, i.e. by finding those values of the non-measured components for which the likelihood of measuring the values of the measured components that actually were measured is highest.

Another is based on the principle of the Kalman filter. A Kalman filter can be set up having the coordinates and speeds of the robot in the various degrees of freedom as its state. The at least one measured component is input as a state measurement into said Kalman filter, so that it will be taken into account when a future state is predicted based on the measured state of the Kalman filter.

Still another approach comprises, as substeps of step b), steps of calculating a first estimate of the wrench based on the dynamical model alone, and of refining the first estimate using a maximum-aposteriori estimation.

Further features and advantages of the invention will become apparent from the following description of embodiments thereof, referring to the appended drawing.

The robot system of FIG. 1 comprises a robot arm 1 and an associated controller 2. The robot arm 1 comprises a base 3, an end effector 4, an arbitrary number of links 5i, i=1, 2, . . . and joints 6j, j=1, 2, . . . which connect the links 5i to each other, to the base 3 or to the end effector 4. For the sake of simplicity it will be assumed here that each joint has one degree of rotational freedom, i.e. is rotatable around a single axis 7. A given link 5i can be rotatable with respect to a neighbouring link 5 (i+1) or 5 (i−1) about two different axes; in that case—although the connection might in practice be formed by a single ball joint—it will be assumed here that the links are connected by two joints, each having one degree of rotational freedom.

Each such joint 6j comprises a motor to drive its rotation and a rotational encoder for feeding back the rotation angle of the joint 6j to controller 2.

The end effector 4 is a tool of any desired type, such as a gripper, a welding or soldering tool, a machining tool, a grinding tool etc., or, in the example of FIG. 1, a drill. The drill comprises a drilling head 8 and a socket 9 in which the drilling head 8 is removably mounted and is rotatably driven around an axis 7 and advances along axis 7 during drilling. The socket 9 accommodates a force sensor 10 which is designed to detect a force component along axis 7.

As is well known, external forces and torques at a tool center point (TCP) 11 such as the tip of drilling head 8 induce joint load torques according to

τ_ext=J^τ·f,  (1)

where J∈^6XnDoF the manipulator TCP Jacobian with respect to the base frame. We partition the wrench f∈₆ into f=f_m+f_nm, where f_m contains the components that can directly be measured by available low-dimensional sensors of the robot, i.e. by sensor 10 in the case of FIG. 1, and f_nm contains components which cannot be measured because the robot arm 1 lacks appropriate sensors. Assuming that the direction of axis 7 is the z direction, f_m and f_nm can be written

$\begin{array}{cc}{f}_w=\left[\begin{array}{c}0\\ 0\\ f_{m,z}\\ 0\\ 0\\ 0\end{array}\right],f_{\mathrm{nm}}=\left[\begin{array}{c}\text{?}\\ f_{\mathrm{nm},\text{?}}\\ f_{\mathrm{nm},\text{?}}\\ 0\\ {\tau }_{\mathrm{nm},\text{?}}\\ {\tau }_{\mathrm{nm},\text{?}}\\ {\tau }_{\mathrm{nm},\text{?}}\end{array}\right]\text{?}\text{indicates text missing or illegible when filed}& \left(2\right)\end{array}$

Replacing f by f_m+f_nm in eq. (1), we can write τ_ext=J^T(f_n+f_nm), and we define

$\begin{array}{cc}{\tau }_{\mathrm{ext}}^{\prime }={\tau }_{\mathrm{ext}}-J^T·f_x=J^T·f_{\mathrm{nm}}& \left(3\right)\end{array}$

Since f_nm has at least one component which is zero (corresponding to the measured component of f_m), eq. (3) can be rewritten

$\begin{array}{cc}{\tau }_{\mathrm{ext}}^{\prime }=J^T·f_{\mathrm{nm}}=J_{\mathrm{red}}^T·f_{\mathrm{nm},\mathrm{red}}& \left(4\right)\end{array}$

where f_nm,red∈^6−N_meas is a vector that contains all non-zero elements of f_nm (N_meas being the number of measured wrench components) and J^T_red is a reduced version of the Jacobian transpose in which all columns corresponding to the measured components of wrench have been removed. Note that with N_DoF being the number of manipulator degrees of freedom (DoF), J^T_red∈. Hence, J^T_red has more rows than columns. This redundancy allows to improve the quality of F/T estimation for the remaining (non-measured) components of wrench.

To this end, we assume that joint load estimates τ_ext are available. They can e.g. be obtained from

$\begin{array}{cc}{\hat{\tau }}_{\mathrm{ext}}={\tau }_{\mathrm{ext}}-G\left(q\right)-{\tau }_{\mathrm{fric}}-{\tilde{\tau }}_{\mathrm{fric}}& (5\end{array}$

where τ_mot are motor torques, G(q) are gravity induced torques, τ_ffw represents dynamic feedforward and τ_fric are friction torques obtained from a friction model. Furthermore, we assume

$\begin{array}{cc}{\hat{\tau }}_{\mathrm{ext}}={\tau }_{\mathrm{ext}}+e_{\text{?}},e_{\text{?}}\sim N\left(0,{\Sigma }_{\text{?}}\right)\text{?}\text{indicates text missing or illegible when filed}& \left(6\right)\end{array}$

i.e. the error in joint load torque estimates is normally distributed with zero mean. Assuming that motor torques and gravity compensation are known very accurately, the covariance matrix å can be obtained by determining the standard deviation in the friction model error for each joint (e.g. by gathering joint load torque estimates during a validation motion with no external load and a defined tool}.

Considering (4) with the joint level uncertainty e_j and an additional uncertainty N(0,Σ) in the low-dimensional F/T measurements from built-in sensors such as 10, we obtain

$\begin{array}{cc}{\hat{\tau }}_{\mathrm{ext}}=J_{\mathrm{fric}}^Tf_m+e_j-J^Te_{\mathrm{fric}}={\hat{\tau }}_{\mathrm{ext}}^{\prime }+e_j-J^Te_{\mathrm{fric}}=J^Tf_{\mathrm{nm},\mathrm{ext}}& \left(7\right)\end{array}$

With this prior information in joint space, the remaining components of wrench can be estimated by maximum likelihood estimation as

$\begin{array}{cc}{\hat{f}}_{\mathrm{nm},\mathrm{red}}={\left(J_{\mathrm{red}}·{\left({\Sigma }_j+J^T{\Sigma }_{\text{?}}J\right)}^{-1}J_{\mathrm{red}}^T\right)}^{-1}·J_{\mathrm{red}}·{\left({\Sigma }_j+J^T{\Sigma }_{\text{?}}J\right)}^{-1}{\hat{\tau }}_{\mathrm{ext}}\text{?}\text{indicates text missing or illegible when filed}& \left(8\right)\end{array}$

This expression can be simplified under certain conditions, e.g. if the uncertainty of the measured components is neglected, Σ=0, equation (8) reduces to

$\begin{array}{cc}{\hat{f}}_{\mathrm{nm},\mathrm{red}}={\left(J_{\mathrm{red}}·{\Sigma }_j^{-1}·J_{\mathrm{red}}^T\right)}^{-1}·J_{\mathrm{red}}·{\Sigma }_j^{-1}{\tilde{r}}_{\text{?}}\text{?}\text{indicates text missing or illegible when filed}& \left(9\right)\end{array}$

If no data on the covariance matrix Σ are available, it can be necessary to replace it by an identity matrix I_NDoF, implicitly assuming that the quality of the friction model is the same for all joints. Equation (9) then boils down to

$\begin{array}{cc}{\hat{f}}_{\mathrm{nm},\mathrm{red}}={\left(J_{\mathrm{red}}·J_{\mathrm{red}}^T\right)}^{-1}·J_{\mathrm{red}}{\hat{\tau }}_{\text{?}}\text{?}\text{indicates text missing or illegible when filed}& \left(10\right)\end{array}$

which corresponds to the standard pseudo-inverse of the Jacobian transpose.

Alternatively, qualitative information with respect to the quality of the friction model can be used. To do so, the diagonal elements corresponding to main joints can be set to larger values than the elements corresponding to wrist joints (only the ratio of the elements matters for estimation).

According to a second approach for combining measured and calculated wrench component, a first estimate of wrench is calculated without regard to measurements, using the expression

$\begin{array}{cc}\tilde{f}={\left(J·{\Sigma }_j^{-1}·J_{\mathrm{rel}}^{\gamma }\right)}^{-1}·J_{\mathrm{rel}}·{\Sigma }_j^{-1}\hat{r}\text{?}\text{?}\text{indicates text missing or illegible when filed}& \left(11\right)\end{array}$

In a next step, a second estimate of wrench is calculated. In this step, all forces and torques that can directly be measured are replaced by the corresponding measurements. For non-measured directions, the original estimates are kept. This is formalized as

$\begin{array}{cc}{\hat{f}}_i^{\prime }=\{\begin{array}{cc}{f}_{\text{?}}& \mathrm{if}\mathrm{component}i\mathrm{is}\mathrm{measured}\\ {\tilde{f}}_i& \mathrm{if}\mathrm{component}i\mathrm{is}\mathrm{not}\mathrm{measured}\end{array}\text{?}\text{indicates text missing or illegible when filed}& \left(12\right)\end{array}$

A covariance matrix in Cartesian space is constructed as, Σ=diag(σ,σ,σ,σ,σ,σ), its diagonal elements being defined as

$\begin{array}{cc}{\sigma }_{\text{?}}^2=\{\begin{array}{cc}{\sigma }_{\text{?}}^2& \mathrm{if}\mathrm{component}i\mathrm{is}\mathrm{measured}\\ {\left(\frac{f_{\text{?}}}{3}\right)}^2& \mathrm{if}\mathrm{component}i\mathrm{is}\mathrm{not}\mathrm{measured}\end{array}\text{?}\text{indicates text missing or illegible when filed}& \left(13\right)\end{array}$

wherein σ²_sens,i, is the variance of the sensor 10 and f_max,i is the maximum expected magnitude of the force or torque in direction i.

A refined estimate of the wrench is then obtained based on the first estimate of eq. (11) by a maximum a-posteriori estimation:

$\begin{array}{cc}\hat{f}={\left(J·{\Sigma }_j^{-1}·J^T+{\Sigma }_{\text{?}}^{-1}\right)}^{-1}·\left(J_{\mathrm{ext}}·{\Sigma }_j^{-1}{\hat{\tau }}_{\mathrm{ext}}+{\Sigma }_{\text{?}}^{-1}{\tilde{f}}^{\prime }\right)\text{?}\text{indicates text missing or illegible when filed}& \left(14\right)\end{array}$

According to a third approach, joint level load torque information τ_ext and the measurements gathered from a low-dimensional F/T sensor such as sensor 10 are combined into an augmented measurement vector y. This vector y has N_DoF⁺N_meas components, N_DoF being the number of degrees of freedom of the robot, and N_meas being the number of measured components of the wrench. Movement of the robot is modelled

$\begin{array}{cc}\mathrm{by}\left[\begin{array}{c}\stackrel{.}{p}\\ \stackrel{.}{f}\end{array}\right]=\left[\begin{array}{cc}0& -J^T\\ 0& 0\end{array}\right]\left[\begin{array}{c}p\\ f\end{array}\right]+\left[\begin{array}{c}I\\ 0\end{array}\right]\stackrel{\_}{\tau }& \left(15\right)\end{array}$

where p=M{dot over (q)} is the generalized momentum and=τ−G(q)−τ−. The vector y is related to

$\left[\begin{array}{c}p\\ f\end{array}\right]\hspace{1em}$

by

$\begin{array}{cc}y=\mathrm{Cx}=\left[\begin{array}{cc}I& 0\\ 0& C_{\text{?}}\end{array}\right]\left[\begin{array}{c}p\\ f\end{array}\right],\text{?}\text{indicates text missing or illegible when filed}& \left(16\right)\end{array}$

where C_sens is a vector specifying which components of wrench can directly be measured. For example, if force in z-direction can be measured, the vector is c_sens=[0 0 1 0 0 0].

A Kalman filter is implemented that has

$x=\left[\begin{array}{c}p\\ f\end{array}\right]$

as its state vector. As usual in the art, the Kalman filter predicts a future state vector x^k+1 based on the current one x^k using eq. 15. The prediction yields changes of both generalized momenta q and of wrench f. A discrepancies are calculated between the predicted and the measured values of the generalized momenta and of the at least one measured component f_m,i of the wrench. These discrepancies are used for updating the status vector, as is common in the art, but as the amount of data is higher than in case of conventional prediction of forces and torques based on a dynamic model of the robot alone, without having recourse to force or torque measurements, precision is improved.

According to a fourth approach, in analogy to τ_ext=J^Tf+e_j, e=N(0,Σ) we define an augmented Jacobian so that

$\begin{array}{cc}\left[\begin{array}{c}{\tilde{\tau }}_{\mathrm{ext}}\\ f_{\mathrm{sens}}\end{array}\right]=\left[\begin{array}{c}{J}^T\\ C_{\mathrm{sens}}\end{array}\right]f+\left[\begin{array}{c}{e}_f\\ e_s\end{array}\right]=J_{\mathrm{aug}}^Tf+e_{\mathrm{aug}},e_{\mathrm{aug}}\sim \left(0,{\Sigma }_{\mathrm{aug}}\right),{\Sigma }_{\mathrm{aug}}=\left[\begin{array}{cc}{\Sigma }_j& 0\\ 0& {\Sigma }_s\end{array}\right].& \left(17\right)\end{array}$

where e_s is the sensor measurement noise with covariance matrix Σ_s and the matrix c_sens describes the Cartesian directions (or linear combinations thereof) that are directly measured by a sensor such as 10. As in case of the first approach, a solution for eq. 17 is obtained by a maximum likelihood estimation, yielding

$\begin{array}{cc}\tilde{f}={\left(J_{\mathrm{aug}}·{\Sigma }_{\mathrm{aug}}^{-1}·J_{\mathrm{aug}}^T\right)}^{-1}·J_{\mathrm{aug}}·{\Sigma }_{\mathrm{aug}}^{-1}·\left[\begin{array}{c}{\tilde{\tau }}_{\mathrm{ext}}\\ f_{\mathrm{sens}}\end{array}\right].& \left(18\right)\end{array}$

Eq. 18 provides estimates for all components of the wrench {dot over (f)}, including those that are directly measured using sensor 10 or the like, so that by comparing the estimated values to the measured ones, reliability of the underlying model can be judged.

While the invention has been illustrated and described in detail in the drawings and foregoing description, such illustration and description are to be considered illustrative or exemplary and not restrictive. It will be understood that changes and modifications may be made by those of ordinary skill within the scope of the following claims. In particular, the present invention covers further embodiments with any combination of features from different embodiments described above and below. Additionally, statements made herein characterizing the invention refer to an embodiment of the invention and not necessarily all embodiments.

The terms used in the claims should be construed to have the broadest reasonable interpretation consistent with the foregoing description. For example, the use of the article “a” or “the” in introducing an element should not be interpreted as being exclusive of a plurality of elements. Likewise, the recitation of “or” should be interpreted as being inclusive, such that the recitation of “A or B” is not exclusive of “A and B,” unless it is clear from the context or the foregoing description that only one of A and B is intended. Further, the recitation of “at least one of A, B and C” should be interpreted as one or more of a group of elements consisting of A, B and C, and should not be interpreted as requiring at least one of each of the listed elements A, B and C, regardless of whether A, B and C are related as categories or otherwise. Moreover, the recitation of “A, B and/or C” or “at least one of A, B or C” should be interpreted as including any singular entity from the listed elements, e.g., A, any subset from the listed elements, e.g., A and B, or the entire list of elements A, B and C.

REFERENCE NUMERALS

• 1 robot arm
• 2 controller
• 3 base
• 4 end effector
• 5 link
• 6 joint
• 7 axis
• 8 drilling head
• 9 socket
• 10 sensor
• 11 TCP

Claims

1. A method for estimating a wrench acting on a reference point of a robot, comprising the steps of: a) measuring at least one component, but not all components, of the wrench; andb) estimating non-measured components of the wrench based on a dynamical model of the robot while taking into account the measured components.

2. The method of claim 1, wherein step b) comprises a maximum-likelihood estimation of the non-measured components.

3. The method of claim 1, wherein step b) comprises calculating a first estimate of the wrench based on the dynamical model alone, and refining the first estimate using a maximum-a-posteriori estimation.

4. The method of claim 1, wherein the wrench is predicted by a Kalman filter and the at least one measured component is input as a state measurement into the Kalman filter.

5. The method of claim 1, wherein among the measured components of the wrench there is at least one force component and/or among the non-measured components there is at least one torque component.

6. The method of claim 1, wherein a number of measured components is 1.

7. The method of claim 1, wherein the measured component is parallel to a surface normal of a surface of the robot which extends through the reference point or to a direction in which the reference point advances when processing a workpiece.